面向應用的RGB-D機器人道路坡度融合估計方法

摘要：為提高機器人在移動路徑中對道路坡度的估計精度，提出一種面向應用的RGB-D（red green blue-depth）機器人融合型道路坡度估計方法。首先，引入隨機采樣一致性算法完成點云處理；其次，采用改進型平面擬合方法完成法向量估計；最后，采用余弦聚類及累加平均方法實現高精度道路坡度估計。實驗結果表明，該算法在數據集下相較最小二乘法與稀疏子空間法，估計誤差分別降低1.21%、2.13%，在實際環境下較最小二乘法平均誤差降低1.43°，這證明了所提方法的可行性和準確性。

關鍵詞：RGB-D； 機器人； 法向量估計； 坡度估計

中圖分類號：TP242文獻標志碼：A

文章編號：1001-3695（2022）10-033-3095-06

doi：10.19734/j.issn.1001-3695.2022.03.0131

Fusion slope estimation algorithm for RGB-D moving robot

Ling Chenfei， Dang Shuwen， Chen Li

（School of Air Transport， Shanghai University of Engineering Science， Shanghai 201600， China）

Abstract：In order to improve the estimation accuracy of the road slope during the movement of the robot， this paper proposed a fusion slope estimation algorithm for RGB-D moving robot. Firstly， the method used random sampling consistency algorithm to complete the point cloud processing. Secondly， the normal vector estimation followed an improve plane fitting method. Finally， it used the cosine clustering and cumulative average method to accurately complete the road slope estimation. Experimental results show that compared with the least squares method and the sparse subspace method under the data set， the estimation error of the algorithm is reduced by 1.21% and 2.13% respectively， in the actual environment， the average error is reduced by 1.43° compared with the least squares method， which verifies the feasibility and effectiveness of the proposed algorithm.

Key words：RGB-D; robot; normal vector estimation; slope estimation

移動機器人在復雜地形環境下、尤其是坡面道路條件下的運動過程中，準確并穩定地完成道路坡度估計成為關鍵性問題，目前基于激光［1，2］和視覺［3，4］傳感器的環境信息探測是移動機器人完成地圖構建［5］、路徑規劃［6］的主要手段。激光傳感器因其測量精度高這一優勢常被應用于智能交通、智能防疫、甚至外星探測等領域，而提高道路坡度估計精度及穩定性成為亟待解決的問題。文獻［7～10］采用傳統最小二乘法直接擬合區域化地形塊的均值平面，擬合出平面的傾角即為該區域的坡度，該方法計算簡便，設計精簡，但易受異常值影響，估計誤差較大；李海波等人［11］對傳統最小二乘法進行優化，采用稀疏系數劃分子空間，再對子空間進行平面擬合，最后在子空間采用隨機搜索獲取最優平面，進而完成坡度估計，該方法可有效減少異常值對坡度估計的影響，但需要根據平面情況預先設定搜索閾值，同時最優平面的隨機搜索可能導致計算結果陷入局部最優，導致坡度估計結果有一定隨機性;白莎莎等人［12］在中線擬合部分使用自適應閾值擬合算法，通過點云數量信息和坐標信息自動計算閾值，豐富了滑雪場雪道坡度計算的方法。

以上坡度估計方法采用激光雷達方案，激光測量發展迅速、測量范圍廣［13，14］，但與視覺相機相比，激光雷達所獲取的三維環境信息較稀疏，丟失了很多三維特征；且多數視覺相機的體積小、重量輕，對機器人的承重影響較小［15］。目前，視覺相機主要應用于工件缺陷檢測［16］、物體分揀［17］、視覺定位［18］等，但應用于移動機器人的坡度估計方案較少。從實際應用來說，相較激光傳感器，基于視覺傳感器的移動機器人在坡度道面環境下的應用更為合適，但目前面向視覺移動機器人的道路坡度估計方法普遍存在僅停留在仿真無法面向實際應用，估計誤差較大的問題。

本文提出一種面向實際應用的視覺RGB-D機器人道路坡度融合型估計方法。該方法系統構建了包含點云濾波、平面分割、法向量提取、余弦聚類和累加平均濾波幾個環節的融合型算法流程，并就數據集和真實環境下進行了道路坡度估計實驗。經與傳統方法比較分析，結果表明所提方法在降低估計誤差、提高坡度估計精度具備顯著優勢。

1算法流程

本文算法流程如圖1所示，其中：

a）對得到的點云信息，首先采用統計濾波方法，濾除視覺傳感器采集所得點云表面的離群點，之后采用隨機采樣一致性算法進行平面分割，分別提取斜面和水平面。為提高點云處理效率，設定平面點云的數據判斷閾值為10 000，當數據點超閾值時，采用直通濾波方法降低數據點數目，并引入隨機最小二乘方法擬合點云平面提取法向量。

b）對提取得到的法向量，首先采用余弦聚類，對異常向量進行聚類剔除，最后通過累加擬合方法以得到更為精確的坡度。

2三維點云擬合平面的法向量獲取

2.1濾波處理

使用視覺相機進行點云獲取時，相機深度數據易受外界光線的干擾，故采集到的圖像存在異常噪聲，因此首先引入統計濾波方法對點云內每一個點的鄰域進行統計分析，估計它到所有臨近點的平均距離。假設某個點云為

D={pi∈R3}i=1，2，…，t（1）

其中：t表示樣本中點云中點的總個數；pi表示點云D中無序點，只取每個無序點的x、y、z三維坐標。計算點pt的距離閾值davg。

davg=1m∑mj=1d（pt，pj）+α×dstd（2）

其中：m為點pt的鄰近點個數；d（pt，pj）為點pt與其鄰近點pj的歐氏距離，其計算公式為d（pt，pj）=‖pt－pj‖2；α為閾值系數；dstd為點pt與其鄰近的m個點之間的距離標準差。若某點與點pt的距離大于davg，則判定為離群點，將其剔除出點集［19，20］。經多組實驗對比分析，統計濾波的參數設置為m=20，α=2時，可更有效地濾除離群點［21］。

2.2平面分割

假定形成道面夾角的兩個平面中，墻面為斜面，地面為水平面。引入隨機采樣一致算法以完成兩個平臺的點云提取，提取算法流程如下：

從待處理點云中隨機選取三個不共線的點，得出由這三個點確定的平面的方程為

Ax+By+Cz=E（3）

其中：A、B、C、E為所確定平面方程的參數，其他點（a，b，c）到平面的距離di為

di=|A×a+B×b+C×c－E|A2+B2+C2（4）

其中：di平均距離以及di的標準偏差σ為

=1n∑ni=1di（5）

σ=∑ni=1（di－）2n－1（6）

如圖2所示，取距離閾值t=2σ，當digt;t時，此點被認為是外點，予以刪除；反之，則為內點，予以保留，并統計局內點的總數［22］；找出內點數量最多的平面點集，并返回相應內點的序號，將其記為一個平面區域。從場景點云中剔除已經得到的內點，重復上述步驟，直到場景中剩余的點數小于預先設定的閾值，或者最后一次得到的內點數量小于閾值［23，24］。

為便于后續計算，對于分割后的各平面點云中數據點進行判斷，當數據點超過10 000時，使用直通濾波來濾除部分點云。

2.3法向量提取

對于分割得到的兩平面，需要先提取其平面法向量，再進行夾角估計，點云法向量的提取過程分為法向量估計及法向量定向。

2.3.1法向量估計

根據隨機選取的鄰域內點pi，并設置擬合鄰域值k，可得到一個擬合平面，平面的法線即為該點的法向量，點的鄰域為

Euclid Math OneNAp={pi（xi，yi，zi）}i=1，2，…，n（7）

點pi的三維空間坐標為（xi，yi，zi），可得擬合平面N的一般形式是

Fx+Gy+Hz=I（8）

其中：F、G、H、I為所得擬合平面Euclid Math OneNAp的平面方程參數。

F2+G2+H2=1（9）

使用隨機最小二乘進行擬合，即求解以下最小值

minF，G，H，I∑ni=1（Fxi+Gyi+Hzi+I）2（10）

可得以下線性方程組

MFGH=x2-2xy-×z-×xy-×y2-2yz-×xz-×z-×z2-2FGH=0（11）

Mi為3×3的協方差矩陣，對其進行協方差分析

Mi=1kpi1-pi1pik-pikTpi1-pi1pik-pik（12）

Mi×v（j）i=λ（j）i×v（j）ii={1，2，3}（13）

其中：pi是鄰域的重心；λ（j）i和v（j）i分別表示特征值及特征向量，若特征值滿足λ（1）i≤λ（2）i≤λ（3）i，則v（1）i即點pi的法向量ni。

2.3.2法向量定向

考慮到估計得到的法向量ni方向具有不確定性，并沒有確定法向量的矢量方向，故采用視線方向進行統一以保證法向量方向的一致性，已知視點為Vp，對所有法向量定向，使它們朝向視點方向，需滿足

ni·（Vp－Pi）gt;0（14）

3道路坡度估計

稀疏子空間的平面優化方法可有效減少異常值對坡度估計的影響，但其最優平面采用隨機搜索獲取，極有可能得到局部最優解，增加平面迭代次數等問題。為了減少隨機取值帶來的影響，本章在余弦K-means聚類中引入余弦距離，解決聚類初始點隨機取值問題。表1列出了本章中用到的重要公式變量的定義。

3.1余弦聚類

在單個平面提取所得的法向量中，任意取兩個向量a，b在歐氏空間中表示為a=（a1，a2，…，an），b=（b1，b2，…，bn），a與b的歐氏距離dist（a，b）為

dist（a，b）=∑ni=1（ai－bi）2（15）

a與b的余弦夾角定義為余弦相似度，在法向量定向章節已保證向量朝向一致，故此時cos（a，b）取值分布在［0，1］。

cos（a，b）=∑nk=1akbk∑nk=1a2k∑nk=1b2k=∑nk=1akbk‖a‖·‖b‖（16）

余弦相似度是利用向量夾角的余弦值來刻畫相似性，注重維度間相對層面的差異［25］，繼續對上述公式推導。首先對歐氏空間任意向量a=（a1，a2，…，an），b=（b1，b2，…，bn）進行標準化：

a～=a1‖a‖，a2‖a‖，…，an‖a‖（17）

b～=a1‖b‖，a2‖b‖，…，an‖b‖（18）

將標準化向量a～、b～帶入式（2）得a～與b～的余弦相似度

cos（a～，b～）=∑ni=1a～ib～i=∑ni=1ai‖a‖bi‖b‖=∑ni=1aibi/‖a‖·‖b‖=cos（a，b）（19）

即向量a，b在標準化前后的余弦相似度保持不變，此時向量長度為1，式（16）可簡化為∑nk=1akbk，計算效率得到有效提高。

聚類初始點的選取是基于歐氏距離的K-means算法研究的熱點問題［26］。其核心是在第一次迭代選點時對類別進行預測，使選點更接近于理想值，以減少聚類過程的迭代次數和計算開銷。初始點的選取遵循以下原則：距離大的樣本分到同一類的可能性小；相反，距離小的樣本分到同一類的可能性大［27］。因此，聚類的初始迭代需要選取K個相互之間距離較遠的樣本以取得較好的效果。如何在余弦聚類中對空間向量進行第一輪選點成為接下來需要解決的問題。如果采用余弦相似度小作為初始選點的方式，向量a1、a2作為初始的兩點，距離兩點盡可能遠的a3是極難找到的［28］。因為在歐氏距離中，其第三點a3選取采用分別到a1、a2的距離和或積的最大值，然而和或積在余弦中是沒有意義的，無法表示相距較遠，因此引入余弦距離distc（a，b）

distc（a，b）=1－cos（a，b）（20）

由于cos（a，b）取值分布在［0，1］，故distc（a，b）取值分布在0～1，且余弦距離與向量在空間上夾角大小呈正相關，即兩向量在空間上夾角越小，余弦距離數值越小，兩向量越容易分為一類，故余弦距離可作為衡量距離的指標。

取歐氏空間中任意兩個標準化向量c=（c1，c2，…，cn），d=（d1，d2，…，dn）繼續推導歐氏距離dist（c，d），余弦相似度cos（c，d），余弦距離distc（c，d）三者之間的關系。將標準化向量‖c‖2=∑nk=1c2k=1，‖d‖2=∑nk=1d2k=1，結合式（16）得

cos（c，d）=∑nk=1ckdk∑nk=1c2k∑nk=1d2k=∑nk=1ckdk‖c‖·‖d‖=∑nk=1ckdk（21）

由式（15）得

dist2（c，d）=∑ni=1（ci－di）2=∑ni=1（c2i－2cidi+d2i）=2－2∑ni=1cidi（22）

結合式（21），得關系式cos（c，d）=1－12dist2（c，d），可知余弦相似度與歐氏距離具有直接函數關系，兩者間存在緊密聯系。將式（20）代入得到歐氏距離dist（c，d），余弦相似度cos（c，d），余弦距離distc（c，d）三者的關系如下：

cos（c，d）=1－12dist2（c，d）（23）

distc（c，d）=1－cos（c，d）（24）

distc（c，d）=12dist2（c，d）（25）

余弦距離的引入彌補了余弦K-means聚類缺少距離度量的缺點。同時作為紐帶將基于歐氏距離的K-means算法遷移到本文余弦聚類算法之中，通過式（25）可知余弦距離在刻畫距離的方法上與歐氏距離一致，并且與歐氏距離呈正相關，因此余弦距離可表達與歐氏距離一樣的距離意義；同時余弦距離完全由余弦相似度決定，聚類效果與余弦相似度一致。所以將樣本點之間歐氏距離遠，作為聚類初始點的思想應用于余弦K-means聚類，依次尋找余弦距離相對遠的K個點。該方法幫助余弦K-means聚類有條件的選取初始聚類中心，以減少初始聚類中心隨機取值的不利影響。設計聚類流程如圖3所示。

3.2累加擬合

經余弦聚類后的斜面和水平面法向量夾角rθ可定義為［29］

rθ=arctan‖ni×nj‖ni×nj（26）

其中：ni表示斜坡面任一點pi的法線；nj表示地面任一點pj的法線。

采用隨機取值方法的坡度估計結果常具有較大的不確定性，故采用累加擬合的方法完成坡度估計，夾角值取rθ，其公式為

rθ=1n∑nθ=1rθ（27）

4實驗與分析

4.1實驗平臺

本文實驗所用移動機器人結構如圖4所示，其底盤采用兩主動輪、兩驅動輪的四輪結構，主動輪控制方向調整，驅動輪提供行進動力，從而完成移動機器人的運動過程。機器人配備了基于二代Kinect的RGB-D視覺相機，用以感知環境信息，并搭載Core i7主機來完成數據處理和運動控制。

4.2與常用方法比較

為驗證本文融合型算法的優越性，將其與基于傳統最小二乘法的坡度估計方法，以及基于稀疏子空間法的坡度估算方法進行對比。在華盛頓大學公開數據集RGB-scenes-v2中選取了數據集中角度為90°的墻體與地面（模擬道路坡度為90°）用于仿真實驗分析，數據集點云圖如圖5所示。

本文所述融合型算法實驗步驟如下：

對濾波處理后的數據集進行平面分割，圖6為數據集分割后得到的斜面（藍色）和水平面（紅色）（見電子版）。

對向量提取的擬合鄰域值k取值進行討論：當擬合鄰域范圍的k足夠大時，得到的法向量效果更好，噪聲小，但隨之而來的是成倍增長的運算時間和計算壓力。為了保證運算的實時性和準確性，對數據集點云不同鄰域范圍的k值進行實驗驗證：對于坡度為90°的兩平面，分別取k為25，35，50，60，100，1 000，2 000進行實驗。圖7為不同k值時得到的坡道夾角情況，隨著k值增大，夾角變化趨于穩定，測量值更接近于真實值。

實驗結果表明，當klt;100時，所得角度與真實值誤差在1°以上，且具有較多異常值；當kgt;100時，所得角度其誤差變化穩定在0.5°左右，綜合考慮選擇k=100作為鄰域值。根據擬合鄰域值k分別提取斜面和水平面法向量，如圖8、9所示。

對單個平面的向量進行余弦聚類后采用累加擬合，累加擬合次數在與隨機取值對比后，選擇設定為100次。部分結果如表2所示，全部數值對比情況如圖10所示，可知累加擬合方法在取值100次后趨于穩定，而隨機取值方法計算結果波動仍較大，再次證明引入累加擬合后，極大保證了融合型算法的估計精度。

最終本文算法在數據集RGB-scenes-v2中90°墻面坡度下，估計結果為90.55°，測量相對誤差為0.55°，與其余兩種算法的估計結果對比見表3。圖11、12分別給出了基于傳統最小二乘法提取的斜面及水平面法向量；基于稀疏子空間聚類法結果引用文獻［11］數據。由對比結果可知，在數據集中相較最小二乘法及稀疏子空間算法，本文融合型算法估計精度更高，相對誤差分別降低了1.21%和2.13%。

此外，如圖13在實際環境中，搭建坡度分別為16°、47°、60°的斜坡平面，對比最小二乘法與本文方法在實際應用場景下不同坡度的具體表現。測量結果見表4，相較最小二乘算法，本文方法坡度估計準確率仍保持在較高水準，在所給的三種坡度下平均誤差為0.86°，較最小二乘算法的平均誤差降低1.43°。

4.3實際環境下的坡道角度識別

為進一步驗證融合型算法在室外環境的有效性及可行性，選取搭載ROS系統的RGB-D移動機器人在戶外坡道環境下進行半物理仿真實驗，實驗環境如圖14所示。

未處理的坡道點云如圖15所示，其點云邊緣存在可見的離群點，離群點對于法向量的提取是十分不利的，需要對其進行濾波處理。

本文使用統計濾波方法對原點云進行處理，濾波參數設置為m=20，α=2。濾波后點云如圖16所示，被過濾的點云離群點如圖17所示，可見點云離群點已被有效濾除。

經濾波處理后的點云，其數據點較多，需要在分割的同時進行降采樣處理，以滿足點云設定要求。分割后的點云如圖18所示，其中水平面呈紅色，坡道呈藍色，綠框標注部分為該坡道一處臺階凸起，該凸起對于法向量的生成存在干擾。經平面分割，水平面（紅色）和坡道（藍色）均殘存部分凸起，且該坡道點云點數為12 405，遠超過設定閾值，故使用直通濾波法濾除綠框所示的凸起道面點云，直至所含點云個數低于10 000（見電子版）。

平面分割后進行法向量提取，此時法向量提取閾值k設定為100，斜面和水平面法向量提取結果如圖19、20所示。其中共提取斜面法向量4 329組，水平面法向量4 830組。通過余弦聚類對異常值進行聚類剔除，之后采用累加擬合，擬合次數設置為100，最終得到該斜坡角度為12.5°。

5結束語

本文提出一種面向實際應用的視覺RGB-D機器人道路坡度估計融合型算法。針對坡度估計算法普遍局限于仿真，在真實環境中估計精度不高的問題，設計并實現了融合統計濾波、平面分割、法向量提取、余弦聚類和累加平均等多種方法的可用于真實環境的坡道估計方法，并與傳統方法比較分析。結果表明所提融合型算法在降低估計誤差、提高坡度估計精度上有顯著優勢。在數據集下，相較傳統最小二乘法與稀疏子空間算法，基于新融合算法的坡道估計誤差分別降低1.21%、2.13%；在真實環境下，本文方法較最小二乘方法平均誤差降低1.43°。為其后續移動機器人在真實環境中的路徑規劃和移動避障提供了精確環境信息保障。

參考文獻：

[1]Hess W， Kohler D， Rapp H， et al. Real-time loop closure in 2D LiDAR SLAM［C］//Proc of IEEE International Conference on Robotics and Automation.Piscataway，NJ：IEEE Press，2016：1271-1278.

［2]龐帆，危雙豐，師現杰，等.激光雷達慣導耦合的里程計與建圖方法［J］.計算機應用研究，2021，38（7）：2188-2193.（Pang Fan， Wei Shuangfeng， Shi Xianjie， et al. Odometry and mapping method for coupling LiDAR and IMU［J］.Application Research of Computers，2021，38（7）：2188-2193.）

［3]Mur A R， Tardos J D. ORB-SLAM2： an open-source SLAM system for monocular， stereo， and RGB-D cameras［J］.IEEE Trans on Robotics，2017，33（5）：1255-1262.

［4]Campos C， Elvira R， Rodríguez J J G， et al. ORB-SLAM3： an accurate open-source library for visual， visual-inertial and multi-map SLAM［J］.IEEE Trans on Robotics，2021，37（6）：1874-1890.

［5]潘林豪，田福慶，應文健，等.基于直接法的視覺同時定位與地圖構建技術綜述［J］.計算機應用研究，2019，36（4）：961-966.（Pan Linhao， Tian Fuqing， Ying Wenjian， et al. Survey on direct-method visual simultaneous localization and mapping［J］.Application Research of Computers，2019，36（4）：961-966.）

［6]王梓強，胡曉光，李曉筱，等.移動機器人全局路徑規劃算法綜［J］.計算機科學，2021，48（10）：19-29.（Wang Ziqiang， Hu Xiao-guang， Li Xiaoxiao， et al. Overview of global path planning algorithms for mobile robots［J］.Computer Science，2021，48（10）：19-29.）

［7]梁棟，王鵬基，劉良棟.一種基于LiDAR的精確月球軟著陸目標點選定方法［J］.空間控制技術與應用，2009，35（6）：24-29.（Liang Dong， Wang Pengji， Liu Liangdong. A LiDAR-based autonomous landing site selection method for pinpoint lunar soft landing［J］.Ae-rospace Control and Application，2009，35（6）：24-29.）

［8]張澤旭，王衛東，崔平遠，等.一種行星軟著陸地形風險評估方法［J］.哈爾濱工業大學學報，2011，43（5）：25-29.（Zhang Zexu， Wang Weidong， Cui Pingyuan， et al. An algorithm of terrain hazard assessment for planetary soft landing［J］.Journal of Harbin Institute of Technology，2011，43（5）：25-29.）

［9]王亞林，劉鵬，吳輝陽，等.碎石堆構造小行星表面地形分析與仿真驗證［J］.深空探測學報，2019，6（5）：481-487.（Wang Yalin， Liu Peng， Wu Huiyang， et al. Terrain analysis and simulation verification on rubble-pile-constructed asteroid surfaces［J］.Journal of Deep Space Exploration，2019，6（5）：481-487.）

［10]Li Boyuan， Zhang Jiawei， Du Haiping， et al. Two-layer structure based adaptive estimation for vehicle mass and road slope under longitudinal motion［J］.Measurement，2017，95：439-455.

［11]李海波，曹云峰，丁萌，等.基于三維點云聚類的坡度估計方法［J］.計量學報，2018，39（3）：304-309.（Li Haibo， Cao Yunfeng， Ding Meng， et al. A method of slope estimation based on clustering of three-dimensional point cloud［J］.Acta Metrologica Sinica，2018，39（3）：304-309.）

［12]白莎莎，張海洋，許世東，等.基于機載激光雷達點云的雪道坡度提取算法［J］.應用光學，2021，42（3）：481-487.（Bai Shasha， Zhang Haiyang， Xu Shidong， et al. Slope extraction algorithm of ski tracks based on airborne LiDAR point cloud［J］.Journal of Applied Optics，2021，42（3）：481-487.）

［13]Debeunne C， Vivet D. A review of visual-LiDAR fusion based simultaneous localization and mapping［J］.Sensors，2020，20（7）：2068-2088.

［14]周治國，曹江微，邸順帆.3D激光雷達SLAM算法綜述［J］.儀器儀表學報，2021，42（9）：13-27.（Zhou Zhiguo， Cao Jiangwei， Di Shunfan. Overview of 3D LiDAR SLAM algorithms［J］.Chinese Journal of Scientific Instrument，2021，42（9）：13-27.）

［15]李尚璁.基于深度視覺信息的四足機器人野外可通過性分析［D］.濟南：山東大學，2020.（Li Shangcong. Off-road environmental travers ability analyzing for quadruped robots based on depth sensing information［D］.Jinan：Shandong University，2020.）

［16]呂寧，肖劍，高健，等.基于改進MRF的沖壓件輪廓缺陷圖像分割算法［J］.鍛壓技術，2022，47（4）：101-109.（Lyu Ning， Xiao Jian， Gao Jian， et al. Image segmentation algorithm on contour defects for stamping part based on improved MRF［J］.Forging amp; Stamping Technology，2022，47（4）：101-109.）

［17]Han Song， Liu Xiaoping， Han Xing， et al. Visual sorting of express parcels based on multi-task deep learning［J］.Sensors，2020，20（23）：6785-6803.

［18]Young J K， Seon I K， Yeol D Y， et al. Visual positioning system based on 6D object pose estimation using mobile Web［J］.Electro-nics，2022，11（6）：865-879.

［19]彭育輝，鄭瑋鴻，張劍鋒.基于深度學習的道路障礙物檢測方法［J］.計算機應用，2020，40（8）：2428-2433.（Peng Yuhui， Zheng Weihong， Zhang Jianfeng. Deep learning-based on-road obstacle detection method［J］.Journal of Computer Applications，2020，40（8）：2428-2433.）

［20]Charles R Q， Hao Su， Mo Kaichun， et al. PointNet： deep learning on point sets for 3D classification and segmentation［C］//Proc of IEEE Conference on Computer Vision and Pattern Recognition.Pisca-taway，NJ：IEEE Press，2017：77-85.

［21]王新杰，張瑩，張東波，等.自主移動機器人路徑規劃中的點云噪聲處理［J］.智能系統學報，2021，16（4）：699-706.（Wang Xinjie， Zhang Ying， Zhang Dongbo， et al. Point cloud noise processing in path planning of autonomous mobile robot［J］.CAAI Trans on Intel-ligent Systems，2021，16（4）：699-706.）

［22]吳慶華，朱思斯，周陽，等.基于隨機采樣一致性的規則棒料位姿識別［J］.傳感器與微系統，2019，38（9）：137-140.（Wu Qinghua， Zhu Sisi， Zhou Yang， et al. Regular sticks pose position and orientation recognition based on RANSAC［J］.Transducer and Microsystem Technologies，2019，38（9）：137-140.）

［23]魯榮榮，朱楓，吳清瀟，等.一種板型物體混疊場景的快速分割算法［J］.光學學報，2019，39（4）：151-160.（Lu Rongrong， Zhu Feng， Wu Qingxiao， et al. A fast segmenting method for scenes with stacked plate-shaped objects［J］.Acta Optica Sinica，2019，39（4）：151-160.）

［24]Schnabel R， Wahl R， Klein R. Efficient RANSAC for point-cloud shape detection［J］.Computer Graphics Forum，2007，26（2）：214-226.

［25]Jain A K. Data clustering： 50 years beyond K-means［J］.Pattern Recognition Letters，2010，31（8）：651-666.

［26]章永來，周耀鑒.聚類算法綜述［J］.計算機應用，2019，39（7）：1869-1882.（Zhang Yonglai， Zhou Yaojian. Review of clustering algorithms［J］.Journal of Computer Applications，2019，39（7）：1869-1882.）

［27]翟東海，魚江，高飛，等.最大距離法選取初始簇中心的K-means文本聚類算法的研究［J］.計算機應用研究，2014，31（3）：713-715.（Zhai Donghai， Yu Jiang， Gao Fei， et al. K-means text clustering algorithm based on initial cluster centers selection according to maximum distance［J］.Application Research of Computers，2014，31（3）：713-715.）

［28]王彬宇，劉文芬，胡學先，等.基于余弦距離選取初始簇中心的文本聚類研究［J］.計算機工程與應用，2018，54（10）：11-18.（Wang Binyu， Liu Wenfen， Hu Xuexian， et al. Research on text clustering for selecting initial cluster center based on cosine distance［J］.Computer Engineering and Applications，2018，54（10）：11-18.）

［29]王飛，劉如飛，任紅偉，等.利用道路目標特征的多期車載激光點云配準［J］.測繪科學技術學報，2020，37（5）：496-502.（Wang Fei， Liu Rufei， Ren Hongwei， et al. Multi-stage vehicle-mounted laser point cloud registration using road target features［J］.Journal of Geomatics Science and Technology，2020，37（5）：496-502.）

收稿日期：2022-03-10；

修回日期：2022-05-17

基金項目：國家自然科學基金資助項目（52175103）

作者簡介：凌晨飛（1996-），男，浙江湖州人，碩士研究生，主要研究方向為視覺SLAM；黨淑雯（1979-），女（通信作者），內蒙古烏蘭察布人，副教授，碩導，博士，主要研究方向為SLAM、多傳感器數據融合（swdang@126.com）；陳麗（1975-），女，黑龍江牡丹江人，教授，碩導，博士，主要研究方向為飛行器和機器人動力學與控制.