自主移動機器人路徑規劃中的點云噪聲處理

王新杰，張瑩,2，張東波,2，王玉，楊知橋

（1.湘潭大學 自動化與電子信息學院，湖南 湘潭 411105;2.湖南大學 機器人視覺感知與控制技術國家工程實驗室，湖南 長沙 410082）

機器人在自主導航過程中，對當前環境的點云地圖進行運算處理，根據分析結果實施路徑規劃。但點云地圖會由于傳感器誤差、光照、陰影等外在因素，導致存在大量的冗余信息，需要通過濾波消除不利影響[1-2]。

點云的噪聲分為小振幅噪聲和離群點噪聲[3]。小振幅噪聲體現為物體表面隨機分布的點，為了減少小振幅噪聲點，Alexa 等[4]使用移動最小二乘(moving least squares,MLS)算法生成平滑表面；Chen 等[5]定義隱式移動最小二乘(implicit moving least squares,IMLS)方法構建插值或逼近隱式曲面；Zhang 等[6]提出加權最小二乘法和層次聚類算法以加快MLS 方法；但是這些算法對大數量的離群點和平滑表面敏感。而離群點噪聲表現為遠離真實物體表面的點或點簇[7]。離群點去噪算法分為基于統計的方法和基于幾何的方法。基于統計的方法將噪聲數據擬合為標準概率分布，根據噪聲點云與正常點云的概率分布不同進行去噪[8]。基于幾何的方法根據點到相鄰點的距離或局部密度中心的距離與閾值的比較，判斷是否是噪聲點[9]。

基于圖像的ORB-SLAM2[10]在ORB-SLAM[11]的基礎上增加了雙目和RGB-D 模式，可以直接獲取圖像的深度信息，但算法只能生成稀疏的特征點云地圖，不能準確表達環境的結構和紋理信息。同時基于圖像的三維重建技術比基于激光傳感器生成的點云數據具有更高幅值和更多的噪聲[12]，需要進一步研究。本文在ORB-SLAM2 的基礎上，研究機器人路徑規劃中的點云噪聲數據處理，并將處理后的結果用于機器人越障或避障的決策。

1 基于ORB-SLAM2 的點云拼接

ORB-SLAM2 利用RGB 圖像信息和對應的深度信息進行點云拼接，獲取環境特征的地圖形式。定義序列X={x1,x2,···,xn} 來表示三維空間點的集合，其中x1=[x y z]T表示相機在三維空間中的坐標。則三維空間中的坐標點映射到二維圖像平面的像素坐標 [u v]T用相機成像模型來描述：

式中：K是相機的內參數矩陣；R和t分別為相機的旋轉矩陣和平移向量；d是像素值的深度。

相機坐標[xy z]T可以根據其在三維空間的坐標到二維像素坐標的反推計算：

其中，cx、cy和fx、fy分別為相機內參中的光心和焦距，是相機的固有參數。

式(2)將當前幀的RGB 圖像、深度圖像，關鍵幀對應的相機位姿信息變換到世界坐標，創建當前幀的點云地圖。本文采用迭代最近點(iterated closest points,ICP)[13-14]算法將不同坐標系下的點云對準到同一世界坐標系，實現局部點云地圖的拼接。算法如圖1 所示。

圖1 基于ICP 的三維點云拼接Fig.1 Three-dimensional point cloud registration based on ICP

ICP 首先將待拼接的2 片點云或其中1 片點云進行采樣，然后根據點到點、點到投影、點到面這3 種方法確定對應點集，一般采用最小二乘法迭代求解2 片點云間的最優坐標變換，完成點云的拼接。

2 路徑規劃中的點云數據處理

2.1 點云模型中的濾波方法

機器人路徑規劃一般用直通濾波分割點云的范圍，刪除指定場景外的點。深度相機獲取的點云數據量過大，需要經過降采樣處理以減少點的數量，提高處理效率，路徑規劃算法常用體素濾波進行數據壓縮，節省存儲空間。由于設備精度誤差和環境物體間的視野相互遮擋，深度相機輸出的原始點云數據存在大量離群噪聲點，路徑規劃算法常用統計濾波去除離群點噪聲，保留物體表面上的點。

統計濾波首先通過K個最近鄰域搜索功能計算點到其鄰域的平均距離。計算如式(3)所示。

式中：μ 代表估計的平均距離；σ 代表標準差；α 為標準差倍數閾值。如果某點的計算值 μ±α·σ 不小于點云中所有點與鄰域的距離l，則被認為是異常點，可將其剔除。故離群點的去除效果由閾值K和 α 決定。

2.2 障礙物點云分割及其坡度計算

機器人越障需把斜坡面和地平面分割出來，計算兩個平面之間的夾角，如圖2 所示。經典的點云分割算法分為基于聚類的分割算法和基于模型的分割算法[15]，基于模型的分割算法采用隨機采樣一致性(random sampling consensus,RANSAC)原理，刪除范圍以外的點，得到指定范圍內的點所組成的子集，從而擬合線或面。

圖2 障礙物平面分割Fig.2 Obstacle plane segmentation

當兩個點云平面即斜坡面和地平面被分割以后，如果以兩個方程分別表示斜坡面：A1x+B1y+C1z+D1=0和地平面：A2x+B2y+C2z+D2=0，則兩個平面的夾角 θ 用式(4)求取：

2.3 去噪方法的性能評估

為了評估統計濾波方法在不同設置下的準確性，定義假陽性率(FPR)和假陰性率(FNR)來表示兩類不同的誤差[16]，FPR 為被誤檢為噪聲點的正常點個數在總正常點云數中的比例，FNR 為被誤檢為正常點的噪聲點個數在總噪聲點云數中的比例。

3 實驗結果與分析

3.1 數據集下的異常點剔除分析及參數選擇

斯坦福大學的Table Scene 數據集如圖3 所示，數據量共460 400 個點，將帶有噪聲的點云(4 719點)標為紅色區域，統計濾波算法在不同參數下的異常點刪除對比如圖4 所示。不同參數的濾波結果如表1 所示。

圖3 Table Scene 點云數據集Fig.3 Table Scene point cloud dataset

圖4 不同參數下統計濾波的異常點刪除效果Fig.4 Outlier removal effect of statistical filtering under different parameter

表1 統計濾波在不同參數下的濾波結果Table 1 Results of statistical filtering under different parameters

續表 1

從圖4 可以看出，當K=20，α分別為0 和0.5時，濾波刪除了物體表面上的點；當K=20，α=1時，濾波刪除了桌面后方邊緣處桌面內的點，出現了孔洞；當K=20，α=3 時，在桌面與地面之間出現了沒有被刪除的離群點；當α=2，K分別為20、50、100 時，濾波效果基本相當。根據表1 的處理時間，選取K=20，α=2 作為統計濾波的計算參數。

3.2 實際環境下的點云處理

3.2.1 機器人在路徑規劃中的點云處理

為了驗證濾波算法是否可行，選取室外環境中的兩處斜坡作為研究對象，如圖5 所示。利用Kinect-2.0 相機采集RGB 圖像和深度圖像，通過ORB-SLAM2 算法求取關鍵幀，從而進行點云拼接，構建三維點云地圖，Turtlebot 機器人如圖6 所示，障礙物判斷流程如圖7 所示。

圖5 室外環境示意圖Fig.5 Outdoor environment diagram

圖6 Turtlebot 機器人Fig.6 Turtlebot robot

圖7 機器人路徑規劃中的點云處理示意Fig.7 Point cloud processing in robot path planning

機器人以0.2 m/s 的速度運行，如果斜坡角度小于20°，機器人繼續執行先前規劃的路徑；如果坡度大于20°，那么機器人會停下來重新規劃新的路徑。兩處斜坡如圖8 所示。由Kinect-2.0 所拍攝的畫面及通過ORB-SLAM2 所構建的局部環境點云如圖9 所示，可發現點云拼接的局部地圖包括斜坡、藍色卷閘門以及大量離散分布的噪聲點。

圖8 室外越障環境Fig.8 Outdoor obstacle crossing environment

圖9 兩處斜坡的點云地圖Fig.9 Point cloud maps of two slopes

3.2.2 直通濾波

在機器人左右兩側和前方進行直通濾波之后，得到的點云地圖如圖10 所示，可以看到濾波較好地去除了無關信息，保留了機器人視角范圍內的有效信息。

圖10 直通濾波后的點云地圖Fig.10 Point cloud map after pass through filtering

3.2.3 體素濾波

直通濾波處理后，點云仍然存在許多視野重疊點，為提高算法速度，體素濾波降采樣處理后的點云如圖11 所示，點云變得稀疏但是仍保持著原來的形狀特征，計算數據量對比如表2 所示。

圖11 體素降采樣后的點云地圖Fig.11 Point cloud map after voxel down sampling

表2 體素濾波前后的點云地圖數據對比Table 2 Comparison of point cloud map data before and after voxel filtering

從表2 可得，經過體素降采樣后，第1 處斜坡環境地圖中的點云數減少98.47%，存儲空間減少了98.46%；第2 處斜坡環境地圖中的點云數減少90.18%，存儲空間減少了89.95%。

3.2.4 統計濾波

從圖11 可以看出，經過直通濾波和體素濾波處理之后的兩處斜坡點云，仍然存在遠離點云平面的異常點，尤其是第2 處斜坡出現的噪聲點要比第1 處斜坡更多(紅色框標出部分)，可能的原因是機器人在第2 處斜坡處的相機獲得的信息更復雜，深度圖像范圍越大，噪聲越多，嚴重影響平面分割的準確度，需要進行異常點的刪除。采用3.1 節得到的K=20，α=2 作為統計濾波輸入參數，濾波效果如圖12 所示，基本保留了第1 處斜坡和第2 處斜坡表面的內點，去除了離群的噪聲外點。

圖12 統計濾波剔除離群點后的點云地圖Fig.12 Point cloud map with outliers removed by statistical filtering

3.2.5 點云分割及其平面夾角計算

經過直通、體素和統計濾波處理以后，算法將圖12 障礙物點云分割為兩個平面，其效果如圖13 所示。

圖13 分割后的斜坡面和地平面點云Fig.13 Point cloud of slope and ground plane after segmentation

分割出的斜坡面和地平面的方程系數如表3所示，將每一個平面方程的前3 個系數A1、B1、C1和A2、B2、C2代入式(4)可求得第1 處斜坡坡度大小約為32°，第2 處斜坡坡度大小約為10°。

表3 分割出的各平面方程系數值Table 3 Coefficients of each plane equation separated out

3.2.6 機器人路徑規劃過程

機器人檢測到前方有斜坡，算法經過式(4)計算后判斷是否可以通過。由于第一個斜坡夾角為32°，Turtlebot 機器人無法通過，只能左轉運動到位姿D重新規劃路徑，第2 處斜坡夾角約10°，機器人可直行，經過位姿E、F、G到達坡上終點，整個過程如圖14 所示，機器人運動時線速度和角速度變化如圖15 所示。

圖14 機器人路徑規劃線路Fig.14 Robot path planning

圖15 驅動輪線速度(V)和角速度(W)變化曲線Fig.15 Variation curve of linear velocity (V) and angular velocity (W) of driving wheel

從圖15 可以看出，機器人在位姿A處檢測到第1 處斜坡，停止前進對坡度進行計算，計算出來的夾角32°大于機器人越障的閾值20°，故無法通過，機器人原地旋轉變為位姿B；在經過位姿C、位姿D的過程中，機器人同時有線速度和角速度的變化，在位姿E處檢測到第2 處斜坡，同樣停止前進對坡度進行計算，計算出來的夾角10°小于系統設定的閾值，機器人繼續前進，經過F點和G點到達設定的終點。

4 結束語

本文對戶外的機器人運動環境建立了三維點云地圖，通過對點云地圖進行直通濾波篩選出目標數據，再通過體素濾波降采樣，然后使用統計濾波對目標點云中的噪聲點進行刪除，最后通過分割后的兩個平面夾角，判斷機器人是否可以通行。下一步計劃在前端視覺里程計中，根據圖像的灰度信息來計算相機運動，減少對環境紋理的依賴，實現更魯棒的三維點云地圖處理。