郭永春，劉文博，羅作煌

（1. 深圳億嘉和科技研發(fā)有限公司，廣東 深圳 518055）

目前，對于可行駛區(qū)域感知技術(shù)的研究，已經(jīng)有部分學(xué)者進(jìn)行了相關(guān)的研究，并取得了一定的成果[1-5]。主要表現(xiàn)為，基于純激光點云的道路提取的研究[6]，基于純視覺方案的研究[7-10]，基于濾波聚類分割方法的研究[11-16]，基于激光和攝像頭融合方法的研究[17-18]。然而，在實際的落地場景中，很難有一種方法能夠兼顧實際的應(yīng)用需求，該項課題仍然需要進(jìn)行深入的研究和探索。本文以激光雷達(dá)點云作為數(shù)據(jù)源，提出了一種實時的激光點云路面分割方法和一套實時的路面障礙物檢測方法，綜合解決了可行駛區(qū)域生成的兩個技術(shù)點，并在實際非結(jié)構(gòu)化道路場景的應(yīng)用中取得了良好的效果。

1 非結(jié)構(gòu)化路面檢測方法

實時的路面檢測方法，需要兼顧精度和效率的雙重要求。本文提出一種單幀分線激光點云的濾波方法、基于滑窗法的多幀融合分割方法解決實時路面的檢測問題。

1.1 單幀激光點云的濾波

單幀激光點云是指激光雷達(dá)設(shè)備按照其運行頻率，單次完成一周掃描所獲取的點云數(shù)據(jù)。實時接收激光雷達(dá)設(shè)備發(fā)送的激光點云數(shù)據(jù)，對所述的單幀激光點云數(shù)據(jù)進(jìn)行濾波處理，是指按照設(shè)定局部感知區(qū)域，對單幀激光點云有效的掃描區(qū)域（在局部感知區(qū)域內(nèi)）進(jìn)行裁剪，然后對所述單幀激光點云按激光掃描角度進(jìn)行激光點云分線生成多線激光點云數(shù)據(jù)，接著對每條分線的激光點云進(jìn)行切段處理，將每條分線的激光點云切割成一系列分段點云，最后對每個分段點云按路面反射強度和幾何一致性準(zhǔn)則進(jìn)行過濾。圖1 所示為單幀激光點云的掃描效果圖。

圖1 單幀激光點云掃描效果

1.1.1 激光點云分線

激光點云分線的具體實施方式為，對每個激光點(x,y,z)按照公式（1）計算它的垂直角掃描角度θ；根據(jù)激光雷達(dá)傳感器提供的最大掃描角θmax和各掃描線之間的夾角ω等參數(shù)，按照公式（2）計算當(dāng)前點的掃描線標(biāo)識nScan，將掃描線標(biāo)識一致的點按順序分到同一組，完成單幀激光的分線任務(wù)。

1.1.2 幾何一致性準(zhǔn)則

幾何一致性準(zhǔn)則，是指在一個微小分段內(nèi)的單線點云所滿足的幾何特征，包括點云掃描半徑一致；點云中相鄰點之間的空間距離一致；點云近似滿足直線分布。

按照公式（3）中所述的幾何一致性準(zhǔn)則，在一個分線激光的微分片段內(nèi)，其最大半徑rmax與最小半徑rmin之差小于設(shè)定閾值δr；每兩個相鄰激光點距離di和dj距離之差小于設(shè)定閾值δd，該微分片段的方向包圍盒的短邊與長邊的比值小于設(shè)定閾值δs；其中obb_width 為方向包圍盒的寬度；obb_height 為方向包圍盒的高度；min(obb_width,obb_height)為取方向包圍盒的短邊；max(obb_width,obb_height)為取方向包圍盒的長邊。

1.1.3 單幀濾波處理

對所述分線激光點云按路面反射強度和幾何一致性準(zhǔn)則進(jìn)行過濾，包括激光路面反射強度值的初始化，是指根據(jù)機器人行駛方向計算方向向量，按照最近距離原則選該方向上距離較近的部分點進(jìn)行統(tǒng)計，計算強度初始值；單線激光的切段處理，是指按照設(shè)定閾值的切割長度將同一條掃描線上的點云劃分為若干小段，本文實驗中所設(shè)閾值長度為20 cm；計算分段點云強度值I和初始化路面強度值I0的差值；計算分段點云的幾何一致性，幾何一致性的計算方法按照1.1.2 中所述的幾何一致性準(zhǔn)則進(jìn)行，包含最大和最小半徑之差滿足半徑閾值、每兩個相鄰點之間距離差滿足距離閾值、分段點云的直線特性（方向包圍盒）滿足長寬比例閾值；過濾不滿足強度一致性(公示4)和幾何一致性（公式3）的點云，保留滿足閾值的點云。這里閾值包含強度閾值δI，半徑閾值δr，距離閾值δd，直線閾值δs，可以根據(jù)需求調(diào)整數(shù)據(jù)，本文實驗所設(shè)閾值分別設(shè)為2.0 cm，3 cm,2 cm，0.15 cm。

1.2 基于滑窗法的多幀激光的路面分割

1.2.1 局部點云拼接與滑窗更新機制

對單幀點云過濾后的結(jié)果緩存至局部感知系統(tǒng)的滑窗中，該滑窗中存儲當(dāng)前局部感知范圍內(nèi)的各幀經(jīng)過單幀激光濾波后的點云和位姿信息；裁剪超出范圍的各幀點云和位姿信息。如果遇到停止采集，或者幀間數(shù)據(jù)過于密集的情況，需按照設(shè)置的移動距離（本文實驗設(shè)置為5 cm）對幀間點云進(jìn)行采樣；如果當(dāng)前幀與上一幀的移動距離小于設(shè)定閾值，則用當(dāng)前幀數(shù)據(jù)替換滑窗中上一幀的數(shù)據(jù)，反之，則將當(dāng)前幀數(shù)據(jù)追加至滑窗的末尾；如果累計幀數(shù)超過滑窗設(shè)置的最大數(shù)量，則刪除滑窗頭部位置時間最早的幀數(shù)據(jù)，以此類推獲取滿足條件的各幀激光數(shù)據(jù)。

將局部感知范圍內(nèi)各幀激光點云數(shù)據(jù)轉(zhuǎn)換到當(dāng)前幀的實施方式為，設(shè)當(dāng)前幀的位置和姿態(tài)分別為T0和R0，其余各幀的位置和姿態(tài)分別為Ti和Ri(i=1,2,…,n)，按照公式5 和公式6 計算各幀相對于當(dāng)前幀的位姿信息R和T，根據(jù)公式7計算各幀坐標(biāo)點Xi在當(dāng)前幀中的坐標(biāo)P。

1.2.2 局部點云的路面分割

對局部緩存的激光點云數(shù)據(jù)進(jìn)行點云路面分割，具體流程是首先將局部緩存的各幀激光數(shù)據(jù)統(tǒng)一轉(zhuǎn)換至當(dāng)前幀坐標(biāo)系，生成局部密集激光點云數(shù)據(jù)，對密集點云按設(shè)定分辨率進(jìn)行網(wǎng)格組織和劃分，本文實驗設(shè)定網(wǎng)格分辨率為5 cm×5 cm，按照局部坐標(biāo)范圍計算柵格網(wǎng)的長度和寬度，將每個網(wǎng)格初始化為0；設(shè)置機器人本體附近的網(wǎng)格為初始種子點網(wǎng)格，種子點網(wǎng)格標(biāo)記為1；計算初始種子點網(wǎng)格的生長參數(shù)，所述生長參數(shù)是指網(wǎng)格內(nèi)點云所擬合的平面的法向量、網(wǎng)格內(nèi)點到網(wǎng)格面片的距離和、網(wǎng)格內(nèi)點云的最大最小高程；按照網(wǎng)格參數(shù)相似度準(zhǔn)則進(jìn)行區(qū)域生長，具體的實施方式為計算當(dāng)前種子點8領(lǐng)域中未被標(biāo)識的網(wǎng)格參數(shù)，比較種子點網(wǎng)格和領(lǐng)域網(wǎng)格參數(shù)的相似性，將滿足要求的領(lǐng)域網(wǎng)格加入種子點列表并標(biāo)識為1，從種子點列表刪除當(dāng)前種子點，然后從種子點列表重新取出一點進(jìn)行上述操作，直至種子點列表為空，統(tǒng)計所有被標(biāo)識的網(wǎng)格（標(biāo)記為1 的網(wǎng)格），這些網(wǎng)格所組成的最大連通域構(gòu)成有效分割區(qū)域，對標(biāo)記出來的有效區(qū)域生成規(guī)則化的三維格網(wǎng)點云。

2 障礙物檢測

2.1 單幀激光點云的障礙物檢測

基于單幀激光點云的障礙物檢測，首先按照1.1節(jié)中所述的方法進(jìn)行單幀激光點云的分線和切段，對每個切段內(nèi)按照ransac（隨機采樣一致性）的方法統(tǒng)計坐標(biāo)值的離群點，并將這些離群點按歐氏距離進(jìn)行聚類，對點云數(shù)量和聚類范圍滿足要求（本文實驗設(shè)置的點云數(shù)量滿足10，聚類尺寸大于10 cm）的結(jié)果進(jìn)行記錄，計算它們的中心點和切段方向上的長度，然后按照檢測出來的位置和聚類尺寸，將目標(biāo)障礙物從1.2 節(jié)中所提取的感知區(qū)域內(nèi)剔除。該方法適用于較小尺寸的障礙物。

2.2 局部點云的障礙物檢測

局部點云的障礙物檢測，是指按照1.2.1中的滑窗機制將原始未濾波的各幀點云進(jìn)行緩存和拼接，按照1.2.2中所分割的地面點云基于ransac平面擬合方法構(gòu)造局部地平面，按照設(shè)置的閾值對高于局部地平面的點云進(jìn)行聚類，形成障礙物區(qū)域的簇標(biāo)記，然后按照聚類尺寸對1.2.2 中對應(yīng)位置的可行駛區(qū)域進(jìn)行剔除。該方法適用于較大尺寸的障礙物。

3 方法步驟

基于本文所述的非結(jié)構(gòu)化路面檢測方法和障礙物檢測方法，設(shè)計實施局部感知系統(tǒng)的具體實施步驟為：

1）數(shù)據(jù)接收模塊，實時接收激光雷達(dá)設(shè)備采集到的每一幀激光點云數(shù)據(jù)并輸入系統(tǒng)。

2）單幀激光點云濾波，按照本文1.1所述原理對單幀激光點云進(jìn)行處理。

3）滑窗機制的更新和拼接，按照本文1.2.1 所述原理進(jìn)行局部點云的更新和緩存。

4）局部點云的路面分割，按照本文1.2.2 所述原理進(jìn)行局部點云的路面分割和提取。

5）單幀激光點云的障礙物檢測，基于步驟1獲取的數(shù)據(jù)，按照本文2.1所述方法進(jìn)行處理。

1）原始激光點云的滑窗更新和拼接，按照本文1.2.1所述原理對原始激光點云進(jìn)行更新和緩存。

2）多幀激光點云的障礙物檢測，按照本文2.2所述原理進(jìn)行障礙物檢測。

3）障礙物區(qū)域過濾，對步驟4中所得的局部路面除步驟5和步驟7所檢測到的障礙物區(qū)域。

4）輸出局部可行駛區(qū)域，向機器人操作系統(tǒng)發(fā)送處理后的局部可行駛區(qū)域。

4 實例驗證

本文選用禾賽32線激光點云設(shè)備，掃描角度-16°～+16°，搭載在室外作業(yè)機器上上進(jìn)行算法的測試和驗證。本次測試的評測方法是按照識別幀數(shù)進(jìn)行統(tǒng)計，采用人工來判別識別結(jié)果是否正確，并對識別結(jié)果正確的幀數(shù)與總樣本的幀數(shù)進(jìn)行比較，計算單個數(shù)據(jù)集的識別正確率。

本文選取了3個典型的園區(qū)數(shù)據(jù)進(jìn)行統(tǒng)計（圖3），為方便區(qū)分，3個園區(qū)的數(shù)據(jù)集分別以園區(qū)A、園區(qū)B和園區(qū)C進(jìn)行標(biāo)記。測試所設(shè)定的最大局部感知的范圍是以當(dāng)前位置作為中心，寬度和長度為4 m×8 m 的區(qū)域。園區(qū)A 地形以坡道為主，涵蓋了寬路、窄路、陡坎、臺階等地形，測試使用3 122幀數(shù)據(jù)進(jìn)行統(tǒng)計。園區(qū)B 地形整體比較平坦，涵蓋了寬路、窄路、拐彎、障礙物等場景，測試使用1 639幀數(shù)據(jù)進(jìn)行統(tǒng)計。園區(qū)C所用數(shù)據(jù)為某寫字樓的辦公區(qū)樓下，包含自行車道、人行廣場、樹木、植被和陡坎區(qū)域等，測試使用546幀數(shù)據(jù)進(jìn)行統(tǒng)計。具體測試結(jié)果如表1所示。

圖3 局部可行駛區(qū)域效果

表1 局部可行駛區(qū)域感知結(jié)果統(tǒng)計

本文根據(jù)實際應(yīng)用的需求設(shè)計了若干測試用例，測試案例涵蓋了寬路、窄路、拐彎、盡頭、陡坎、臺階、障礙物、植被等場景。本次測試的軟硬件情況如下，測試工控機處理器i7-9700、內(nèi)存16 G，采用Ubuntu18.04 操作系統(tǒng)、ROS-melodic 機器人系統(tǒng)框架進(jìn)行算法適配。測試效果如圖2 所示，經(jīng)過統(tǒng)計，可行駛區(qū)域整體的識別正確率大于95%，單線程測試的單幀算法時間消耗在70 ms左右。

5 總結(jié)與展望

本文通過研究單幀激光點云的非地面點濾波技術(shù)、基于滑窗機制的多幀激光點云路面分割技術(shù)、單幀激光點云的障礙物檢測技術(shù)和局部點云的障礙物檢測技術(shù)，提出了一套完整的局部可行駛區(qū)域感知方法和技術(shù)。經(jīng)過驗證，該方法識別的準(zhǔn)確性較高、結(jié)果可靠，能夠滿足目前機器人作業(yè)環(huán)境的實際應(yīng)用需求。

本文研究采用的傳感器是激光雷達(dá)設(shè)備，未對融合相機開展工作，后期研究考慮融合彩色相機或深度相機，進(jìn)行圖像和激光點云的深度融合，以及激光點云和深度相機點云的多模態(tài)點云融合，進(jìn)一步增強可行駛區(qū)域檢測的可靠性和精細(xì)程度，從而滿足機器人感知的更高標(biāo)準(zhǔn)的要求。