郭永春，劉文博，羅作煌

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

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

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

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

1.1 單幀激光點(diǎn)云的濾波

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

圖1 單幀激光點(diǎn)云掃描效果

1.1.1 激光點(diǎn)云分線

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

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

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

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

1.1.3 單幀濾波處理

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

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

1.2.1 局部點(diǎn)云拼接與滑窗更新機(jī)制

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

將局部感知范圍內(nèi)各幀激光點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到當(dāng)前幀的實(shí)施方式為，設(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)點(diǎn)Xi在當(dāng)前幀中的坐標(biāo)P。

1.2.2 局部點(diǎn)云的路面分割

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

2 障礙物檢測

2.1 單幀激光點(diǎn)云的障礙物檢測

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

2.2 局部點(diǎn)云的障礙物檢測

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

3 方法步驟

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

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

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

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

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

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

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

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

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

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

4 實(shí)例驗(yàn)證

本文選用禾賽32線激光點(diǎn)云設(shè)備，掃描角度-16°～+16°，搭載在室外作業(yè)機(jī)器上上進(jìn)行算法的測試和驗(yà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ù)實(shí)際應(yīng)用的需求設(shè)計了若干測試用例，測試案例涵蓋了寬路、窄路、拐彎、盡頭、陡坎、臺階、障礙物、植被等場景。本次測試的軟硬件情況如下，測試工控機(jī)處理器i7-9700、內(nèi)存16 G，采用Ubuntu18.04 操作系統(tǒng)、ROS-melodic 機(jī)器人系統(tǒng)框架進(jìn)行算法適配。測試效果如圖2 所示，經(jīng)過統(tǒng)計，可行駛區(qū)域整體的識別正確率大于95%，單線程測試的單幀算法時間消耗在70 ms左右。

5 總結(jié)與展望

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

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