半結(jié)構(gòu)化環(huán)境稀疏點云可通行區(qū)域檢測方法

洪 洋，袁 夏，高 飛，成 誠，楊 歡，陳耀忠，陸建峰

1.南京理工大學(xué)，南京210094

2.海裝上海局駐南京地區(qū)第四軍事代表室，南京210008

3.北方信息控制研究院集團有限公司，南京211100

隨著人工智能的發(fā)展，機器人市場迎來了爆發(fā)式增長，服務(wù)機器人由室內(nèi)走向社區(qū)，由高速公路走向城市道路，由結(jié)構(gòu)化環(huán)境走向半結(jié)構(gòu)化環(huán)境。在半結(jié)構(gòu)化環(huán)境中，基于機器人本地計算資源的實時可通行區(qū)域檢測是一個熱門研究領(lǐng)域。半結(jié)構(gòu)化環(huán)境中，可通行區(qū)域檢測算法一般可分為以下四類。

（1）基于網(wǎng)格的方法。早期有學(xué)者直接利用網(wǎng)格高度信息檢測可通行區(qū)域的算法[1]。隨后有學(xué)者把網(wǎng)格方法與其他算法結(jié)合來檢測地面，例如基于條件隨機場的算法[2]和基于時空條件隨機場的算法[3]。為了提高算法實時性，李廣敬[4]以及李炯[5]等提出基于柵格的快速檢測方法。這類方法使用網(wǎng)格，將非結(jié)構(gòu)化點云轉(zhuǎn)為結(jié)構(gòu)化數(shù)據(jù)，但是點云數(shù)據(jù)的局部信息被嚴重壓縮，且當障礙物較低或較小時，網(wǎng)格內(nèi)數(shù)據(jù)都將被標記為地面，因此該類算法很難檢測到高度較低的障礙物，且要求半結(jié)構(gòu)化環(huán)境中點云數(shù)據(jù)密度較大。

（2）基于模型的方法。這類算法主要是基于RANSAC提取環(huán)境中平面特征，例如Awwad[6]和魏英姿[7]擬合地平面算法。Chen等提出了基于高斯的實時地面分割算法[8]。為了解決環(huán)境中上下坡問題，Patiphon[9]、Zermas[10]以及Bo等[11]提出了基于RANSAC多區(qū)域擬合算法。這類算法被廣泛應(yīng)用于半結(jié)構(gòu)化和結(jié)構(gòu)化環(huán)境中，但是RANSAC中用來區(qū)分局內(nèi)、局外點的閾值不容易設(shè)定，較低障礙物和地面很難區(qū)分；同時RANSAC 算法從一個數(shù)據(jù)集中只能估計一個模型，現(xiàn)實中的地面并不是一個完美平面或幾個平面簡單拼接的結(jié)果，因此這類算法依舊難以適應(yīng)非平坦地面。

（3）基于點位置分布特征的方法。Himmelsbach 提出了基于射線的地面分割方法[12]。Vo 提出區(qū)域增長分割算法[13]。段建民[14]以及蘇志遠[15]利用環(huán)境特征檢測城市半結(jié)構(gòu)化道路。這類算法雖然可以很精準地檢測到高度較低的障礙物，但是不適應(yīng)顛簸環(huán)境，顛簸或斜坡環(huán)境改變了點云數(shù)據(jù)的空間分布特征，例如密度、距離、高度等，因此該類算法會帶來較多的誤檢測問題。

（4）近年來基于深度學(xué)習(xí)的可通行區(qū)域檢測方法越來越受到關(guān)注。例如Velas 使用卷積神經(jīng)網(wǎng)絡(luò)分割地面[16]。但深度學(xué)習(xí)模型對計算資源要求較高，且需要標注大量數(shù)據(jù)集，在實驗中基于公開數(shù)據(jù)集訓(xùn)練的模型對真實環(huán)境的泛化能力仍有待提升?；谏疃葘W(xué)習(xí)技術(shù)的算法模型目前側(cè)重于模型研究，以及在云計算環(huán)境下進行部署，多數(shù)模型難以完全依靠移動機器人自身的計算資源在本地實時運行。

針對半結(jié)構(gòu)化環(huán)境，本文提出一種基于稀疏三維點云的快速可通行區(qū)域檢測算法，從兩個方面處理當前算法所面臨的問題。（1）本文算法避免直接使用點云高度特征，轉(zhuǎn)而使用點云直線特征檢測結(jié)構(gòu)化障礙物，因此上下坡、顛簸等環(huán)境不影響本文算法。（2）通過寬廣的角度約束，提取環(huán)境中非結(jié)構(gòu)化障礙物，降低了較低障礙物和地面之間相互誤檢測。

本文工作主要貢獻：（1）針對半結(jié)構(gòu)化環(huán)境，算法從兩個方向出發(fā)，區(qū)別處理結(jié)構(gòu)化障礙物和非結(jié)構(gòu)化障礙物，使得算法可以適應(yīng)較為復(fù)雜的半結(jié)構(gòu)化環(huán)境。（2）一般性算法難以檢測路緣石、臺階等較低障礙物，本文算法充分利用環(huán)境中的結(jié)構(gòu)化信息，能穩(wěn)定檢測路緣石、臺階等較低障礙物。（3）算法避免了直接使用點云的高度特征，可以適應(yīng)上下坡、顛簸等情況，同時算法且具備檢測負障礙的能力。（4）算法不僅標記出環(huán)境中障礙物，而且給出了可通行區(qū)域，同時可以在小型移動機器人平臺上依靠本地計算資源實時運行。

1 算法框架

本文算法步驟框圖如圖1所示：首先將無序點云轉(zhuǎn)有序點云，從中快速提取近似直線特征；然后分類直線特征以及提取有效直線特征；結(jié)合直線特征檢測結(jié)構(gòu)化障礙物和非結(jié)構(gòu)化障礙物；最后標記出環(huán)境中的可通行區(qū)域，并構(gòu)建成環(huán)境柵格地圖。

圖1 算法框圖Fig.1 Algorithm block diagram

2 直線特征提取

2.1 實驗數(shù)據(jù)

本文實驗數(shù)據(jù)來自于Velodyne-16 激光雷達，該雷達位置相對地面高度38 cm，每幀點云數(shù)據(jù)由16條環(huán)形掃描線組成，具有360°水平視場，水平角度分辨率約0.18°，±15°的垂直視場，垂直角度分辨率2°，機器人實驗平臺如圖2（a）所示。本文參考坐標系為雷達坐標系，即X軸向前、Y軸向左、Z軸向上。本文處理的對象為點云中的1～8 條掃描線，且投影到XY平面，如圖2（b）所示。

圖2 實驗數(shù)據(jù)說明Fig.2 Experimental platform description

點云預(yù)處理方法可以借鑒LeGO-LOAM[17]和LOAM算法[18]，包括點云運動畸變校正、無序點云轉(zhuǎn)有序點云。校正點云的運動畸變，將會使本文算法得到更好的實驗效果。

2.2 直線特征提取

由于環(huán)境中障礙物的尺寸不同，因此點云中結(jié)構(gòu)化障礙物對應(yīng)的直線段長度也大小不一。在初始階段將待提取直線特征最小長度設(shè)置在較小范圍，盡可能多地提取直線段，有利于降低漏檢情況。本文將直線段最小長度設(shè)置為25 cm。點云中存在的直線特征并不是絕對的直線，主要表現(xiàn)在以下三種情況：（1）激光雷達處在水平面上，點云由環(huán)形掃描線組成，當掃面線半徑很大時，可以將連續(xù)的數(shù)個點近似看作直線段；（2）當雷達相對地面高度較低時，環(huán)形掃描線在平緩起伏的地面也近似直線段；（3）激光雷達測距有一定的誤差，同一直線段上的點不會絕對對齊。

通過以上分析發(fā)現(xiàn)，在點云數(shù)據(jù)中精準地提取所有直線段是比較困難的，因此，本文算法使用模糊線段來提取直線特征。模糊線段（blurred segment）定義[19]：離散直線L(a,b,u,w)是一個整數(shù)離散點集(x,y)的集合，該點集滿足u≤ax-by

圖3 直線特征提取算法框圖Fig.3 Block diagram of line feature extraction

存儲滿足以上約束條件的近似直線段，包括直線段的索引、端點、斜率、截距。雷達是順時針方向掃描的，直線段起始點位置都是在終止點位置的逆時針方向，因此直線段的斜率用直線與Y軸正方向的夾角來表示，范圍是0°～360°。這里的直線截距是直線與Y軸交點坐標，當直線和Y軸近似平行時，直線截距會出現(xiàn)極大或者極小值的情況，因此需要設(shè)置直線的最大截距和最小截距，分別為Bmax、Bmin。當直線和Y軸平行時，規(guī)定直線的截距等于Bmax。提取直線特征的方法原理簡單，提取直線特征結(jié)果如圖4中白色直線段所示。

圖4 直線特征提取結(jié)果Fig.4 Extract lines from point clouds

3 直線特征分類和篩選

3.1 直線特征聚類

結(jié)構(gòu)化障礙物都有一定的長度以及高度，因此，同一個結(jié)構(gòu)化障礙物表面上的直線段特征會有多條，這一類直線段都有相似的屬性，即斜率和截距。因此可以借助聚類方法，將上一章中提取的近似直線段分類，使同一障礙物對應(yīng)的直線段屬于同一個類別，本文使用的是k均值聚類算法（K-means clustering algorithm）。算法框圖如圖5所示。

圖5 聚類算法框圖Fig.5 Block diagram of clustering algorithm

（1）對直線斜率進行聚類，聚類數(shù)目和聚類中心分別為式（2）和（3），聚類后可得到I個簇，但是角度是循環(huán)數(shù)據(jù)，例如0°和360°屬于同一個聚類，因此，需要將聚類中心在0°和360°附近的簇合并成一個簇，聚類結(jié)果表示為CA。

斜率的聚類數(shù)目：

斜率的初始聚類中心ci：

其中，N為直線段總數(shù)目，I為整數(shù)，直線段總數(shù)目是類別5倍。即將360°分成I份，ci是第i份的初始聚類中心。

（2）對直線的截距進行聚類，需要在CA中每個簇內(nèi)都進行一次對截距的聚類。設(shè)Ci中所有直線的最大截距（單位：m）為Bt_max,最小截距為Bt_min,Ck∈CAB，聚類數(shù)目和聚類中心分別為式（4）和（5），將Ci聚類后，可以得到K個簇，將聚類中心在Bmax和Bmin附近的簇合并成一個簇。最終聚類結(jié)果表示為CAB。

Ci中截距的聚類數(shù)目：

Ci中截距的初始聚類中心：

其中，K是整數(shù)。即在簇Ci中，將直線間的最大截距差分成K份，ck是第k份的初始聚類中心。

3.2 直線特征篩選

在第2章中提取的近似直線段，除了包含障礙物表面上真實的直線段以外，也包含非障礙物表面上無效的直線段，例如半徑較大掃描線上的圓弧。這些無效直線段也是聚類的對象，因此需要剔除這些無效直線段。分為以下兩種情況。

（1）非障礙物表面上的近似直線段的斜率和截距是沒有規(guī)律的，因此這些無效直線段所屬類別中的樣本數(shù)目也是極少的。如圖6（a）所示，無效直線段所構(gòu)成的類別分布如圖中C1、C2所示，有效直線段所構(gòu)成的類別分布如圖中C3、C4所示，C1,C2,C3,C4∈CAB。因此，當聚類Ck中直線樣本數(shù)只有1時，那么Ck包含的直線段都是無效的，Ck∈CAB。

（2）如圖6（b）所示，有些無效直線段所構(gòu)成的類別分布也會如圖中C1、C2所示，C1,C2∈CAB。這些無效直線段由于環(huán)境原因，恰好被聚集在同一個聚類中。本文2.2 節(jié)提取的直線特征保留了方向信息，因此本文的直線段特征是有向線段，線段的起始端點都在終止端點的逆時針方向。如圖6（b）中所示，類別C1包含兩條有向直線段AB、CD，連接AB起始點A 和CD終止點D點，得到新向量AD，同理可以得到新向量CB，從而可以計算出AD和CB之間的夾角α。

如圖6（b）所示，C1中夾角α是一個比較大的角度，同理，計算得到C2中夾角α也是一個比較大的角度。而C3中的夾角α值卻接近于0。即同一類別中包含兩條以上有向直線段，用兩條有向直線段重新組織成兩個向量，當兩向量間夾角α>α0時，表明該這兩條直線特征無效。直線特征篩選結(jié)果如圖6（c）、（d）所示。

圖6 直線特征篩選Fig.6 Lines selection

4 可通行區(qū)域檢測

4.1 聚類再分析

在第3章中，使斜率和截距近似的直線特征屬于同一個類別，并且認為該類別中的直線特征都屬于同一個障礙物。但是當兩個結(jié)構(gòu)化障礙物表面近似平行時，這兩個障礙物對應(yīng)的直線特征斜率和截距也是近似的，那么兩個障礙物對應(yīng)的直線特征將會被聚到一個類別中，例如圖7（a）所示，在類別Ck中，Ck∈CAB，如果存在空間位置相鄰的兩條直線段CD、EF,CD位于掃描線u上，EF位于掃描線m上，且有| |u-m >1，此時直線特征CD、EF顯然不屬于同一個障礙物。

圖7 聚類再分析Fig.7 Cluster reanalysis

當算法檢測出道路的兩個邊緣時，也可以根據(jù)兩個邊緣計算出當前環(huán)境下的道路寬度。當?shù)缆穼挾瓤捎?，且點D、E 間的長度大于道路寬度時，Ck也會被重新分成兩個新聚類。本節(jié)最終的目的是使每一個類別Ck中都只對應(yīng)一個或者零個障礙物，以方便在第4章中使用它們。

4.2 結(jié)構(gòu)化障礙物檢測

實際中，一個障礙物表面結(jié)構(gòu)一般是連續(xù)的，但由于點云數(shù)據(jù)的稀疏性，障礙物并沒有被點云完整地覆蓋，因此算法檢測到的障礙物是斷斷續(xù)續(xù)、不完整的。一般算法檢測可通行區(qū)域時，往往忽略了障礙物之間的相關(guān)性，算法中并沒有體現(xiàn)出結(jié)構(gòu)化特征。如圖8（a）所示，Ck中包含三條直線特征，Ck∈CAB，三條直線特征對應(yīng)著同一障礙物，這就意味著從A 點到F 點，都是障礙物實體，但是由于點云數(shù)據(jù)比較稀疏，使得B 點到C點、D點到E點間都是可以通行的。

為了體現(xiàn)障礙物更完整的結(jié)構(gòu)化信息，可以依次連接Ck中各條直線特征，Ck∈CAB,得到的結(jié)果如圖8（b）所示。即使在交叉路口等環(huán)境下，也可以直接連接每個聚類中的直線特征，因為4.1 節(jié)中，使每一個類別Ck中都只對應(yīng)一個或者零個障礙物。實驗結(jié)果如圖8（c）、（d）所示。

圖8 結(jié)構(gòu)化障礙物檢測Fig.8 Structured obstacle detection

4.3 非結(jié)構(gòu)化障礙物檢測

如圖9（a）所示，當算法檢測到結(jié)構(gòu)化障礙物AB、CD時，顯然，直線AB、CD是不可通行的，由于視角遮擋，區(qū)域S1、S2的可通行性是未知的。從應(yīng)用層次的角度的來分析，區(qū)域S1和S2不屬于可通行區(qū)域。

為了快速檢測區(qū)域S1、S2，本文使用快速排斥實驗和跨立實驗。如圖9（b）所示，直線特征AB是一障礙物的邊界信息，點云數(shù)據(jù)中任意一點pi，連接原點和pi得到向量Opi。當AB和Opi相交時，即可判定點pi屬于不可通行區(qū)域。實驗結(jié)果如圖9（c）、（d）所示。

圖9 不可通行區(qū)域檢測Fig.9 Detection of inaccessible areas

當圖9（a）中白色區(qū)域存在非結(jié)構(gòu)化障礙物時，這種情況并沒有得到有效的處理。本節(jié)通過地面最大傾斜約束條件，來檢測非結(jié)構(gòu)化障礙物。根據(jù)《城市道路設(shè)計規(guī)范》《汽車庫建筑設(shè)計規(guī)范》，以及《城市居住區(qū)規(guī)劃設(shè)計規(guī)范》等規(guī)定，在半結(jié)構(gòu)化環(huán)境中，地面的傾斜度一般不大于15°，考慮顛簸等情況，本文把地面最大傾斜度設(shè)置為σ=20°。

首先將點云數(shù)據(jù)由掃描線形式，重新組織成射線形式，如圖10（a）所示，相鄰射線之間的夾角為0.18°，共有2 000條射線，理論上每條射線包含16個節(jié)點，每個節(jié)點代表點云數(shù)據(jù)中的一個點。但是當一束激光反射點非常遠或者沒有反射時，該節(jié)點缺失。

本文把點云射線和障礙物相交的點，稱之為截至點。當射線遇到截至點時，射線會被截斷，即射線不包含區(qū)域S1、S2的點。既減少了算法的計算量，又降低了障礙物檢測對本節(jié)算法的依賴程度。

然后計算每一條射線上相鄰點之間的夾角，如圖10（b）所示，點pi和點pi+1是同一射線上相鄰點，坐標分別為(xi,yi,zi)、(xi+1,yi+1,zi+1)，兩點之間夾角θ：

圖10 非結(jié)構(gòu)化障礙物檢測原理Fig.10 Principle of unstructured obstacle detection

當夾角θ>σ時，將pi+1標記為障礙點，當點pi+1缺失時，依次用點pi+j來替代pi+1,j=2,3,4,j<5。實驗結(jié)果如圖11所示。

圖11 非結(jié)構(gòu)化障礙物檢測Fig.11 Unstructured obstacle detection

4.4 可通行區(qū)域

將4.2節(jié)、4.3節(jié)實驗結(jié)合，疊加兩部分檢測結(jié)果，剔除重復(fù)的障礙點，便可得到最終的實驗結(jié)果。如圖12（a）所示，其中彩色代表原始點云，白色點代表障礙物。按照4.3節(jié)實驗方法，將圖12（a）中不可通行區(qū)域填充后并表示為柵格地圖，效果如圖12（b）所示。

圖12 可通行區(qū)域Fig.12 Results of traversable area

5 實驗與分析

在實驗結(jié)果分析中，本文算法將與文獻[9]算法、LeGO-LOAM 算法[17]地面分割部分進行對比。文獻[9]算法通過分析點云幾何結(jié)構(gòu)和RANSAC多平面擬合的方法來檢測地面，該算法適用于斜坡環(huán)境中的三維點云地面分割方法。文獻[17]算法將點云數(shù)據(jù)投影成二維圖像，基于圖像的方法完成地面分割。本文算法中的實驗參數(shù)具體設(shè)置如下：n=5,α0=8°,σ=20°,Bmax=36 m,Bmin=-36 m。

5.1 算法效率

算法的實時性，是評估算法實用價值的重要標準之一。本文算法在ROS環(huán)境中，使用C++實現(xiàn)，可以在Intel NUC 迷你電腦上運行并實時檢測可通行區(qū)域，系統(tǒng)的具體參數(shù)如表1 所示。系統(tǒng)實時性要求算法完全依靠移動機器人自身的計算資源在本地實時運行，因為激光雷達數(shù)據(jù)輸出頻率為10 Hz，因此算法運行時間必須小于0.1 s。

表1 系統(tǒng)信息Table 1 System information

本文結(jié)合了7 100 幀點云數(shù)據(jù)，在平坦道路、彎道、斜坡等多種環(huán)境下，測試了上述三個算法運行時長，如圖13 所示。本文算法平均運行時間是0.014 s，最大時長為0.041 s，最小運行時長為0.006 s，本文算法完全可以達到實時性要求。

圖13 算法運行時間Fig.13 Run time of algorithms

5.2 負障礙環(huán)境測試

如圖14（a）所示，該場景下包含負障礙物，藍色框中是沒有井蓋的地下管道入口，井口的尺寸約為0.6 m×0.4 m。如圖14（b）、（d）所示，當激光雷達到負障礙的距離約2 m時（本文實驗平臺尺寸為0.5 m×0.5 m），文獻[9]算法和本文算法可以穩(wěn)定地標記出負障礙物，同時，本文算法可以準確地標記出路緣石等障礙物。文獻[17]算法并不具有檢測負障礙物的能力，且和激光雷達到負障礙的距離無關(guān)。實驗結(jié)果說明，本文算法具有檢測負障礙物的能力。

圖14 負障礙場景實驗結(jié)果Fig.14 Results of negative obstacle scene

5.3 平坦環(huán)境測試

平坦實驗場景如圖15，在（a）、（i）、（m）中，檢測較矮的障礙物方面，本文算法實驗結(jié)果都要遠好于文獻[9]算法和文獻[17]算法。在直道試驗場景圖15（a）中，本文算法可以準確提供道路的邊緣信息，在轉(zhuǎn)彎實驗場景圖15（i）中，本文算法更準確標記出了道路的輪廓，標記效果要遠好于文獻[9]算法、文獻[17]算法。場景圖15（m）表明環(huán)境中的車輛、行人等因素對本文算法影響較小。同時，在圖15（d）、（l）、（p）中，說明了本文算法可以準確地補充障礙表面信息，更好地體現(xiàn)了障礙物結(jié)構(gòu)化信息，更能體現(xiàn)可通行區(qū)域的概念。在車庫實驗場景圖15（e）中，環(huán)境中的障礙物高度都比較高，三個算法的實驗結(jié)果近似，沒有太大的差異。

圖15 平坦場景實驗結(jié)果Fig.15 Results of flat scenes

5.4 斜坡環(huán)境測試

斜坡在環(huán)境中主要表現(xiàn)為上下坡道，移動機器人在上坡過程中，主要有上坡前、上坡后兩個環(huán)節(jié)。移動機器人在下坡過程中也有下坡前、下坡后兩個環(huán)節(jié)。這4個環(huán)節(jié)分別對應(yīng)場景圖16（a）、（e）、（i）、（m）。雖然文獻[9]算法針對斜坡環(huán)境使用了基于RANSAC多平面擬合方法，但是該算法在上坡過程中，依舊將較遠距離的斜坡地面標記成障礙物，如圖16（b）、（f）所示，當然文獻[9]算法對所有幀的測試結(jié)果并不都是如此，但是算法誤檢測的概率也將近30%，因為文獻[9]算法中并不能保證它劃分的多個區(qū)域正確。文獻[9]算法和文獻[17]算法實驗結(jié)果近似，但是文獻[9]算法在障礙物和非障礙物交界處的實驗結(jié)果一般，如圖16（n）所示，文獻[9]算法將部分地面檢測成障礙物。而本文算法是結(jié)合直線特征來完成的，在半結(jié)構(gòu)化環(huán)境中，本文算法的適應(yīng)性要比文獻[9]算法和文獻[17]算法更好。

圖16 斜坡場景實驗結(jié)果Fig.16 Results of slope scenes

6 結(jié)束語

本文結(jié)合半結(jié)構(gòu)化環(huán)境特征，提出了一種實時可通行區(qū)域檢測算法。該算法將可通行區(qū)域檢測問題分解成了兩個任務(wù)，即結(jié)構(gòu)化障礙物和非結(jié)構(gòu)化障礙物檢測，避免了直接利用點云的高度信息，從而能更好地適應(yīng)斜坡、顛簸、負障礙等環(huán)境。該算法可以在中小型AGV上實時運行，適用于低密度點云，在后續(xù)的局部路徑規(guī)劃和目標分類中，具有一定實用價值。在接下來的研究中，將利用多幀數(shù)據(jù)間的時間關(guān)聯(lián)性進一步提高算法的魯棒性。