月面環境三維激光SLAM技術

2021-03-27 12:40:06尚天祥王景川董凌峰陳衛東

航空學報 2021年1期

尚天祥，王景川,*，董凌峰，陳衛東

1. 上海交通大學電子信息與電氣工程學院，上海 200240 2. 系統控制與信息處理教育部重點實驗室，上海 200240

月球探索機器人[1]在無定位系統輔助的未知環境中執行任務，需要精確獲取機器人的實時位姿。高可靠的定位性能為未知環境探索、局部導航避障提供了重要的依據。因此，研究在復雜月球表面環境中精確、快速得獲取機器人的位姿尤為重要。

在無定位系統輔助的環境中進行定位主要依靠多傳感融合，利用同步建圖與定位(Simultaneous Localization And Mapping，SLAM)的策略進行實時定位[2]，月球探索機器人一般配備有深度相機，二維、三維激光雷達，里程計和慣性傳感器等。地面環境中的激光或視覺同步建圖與定位的算法如文獻[3-5]都在自動駕駛上得到了很好的應用效果。文獻[6]提出的基于多層ICP(Iterative Clostest Point)的匹配算法在室外巡檢機器人SLAM中取得了不錯的應用效果。

但是針對月球探索機器人來說，無輔助定位系統，表面隕坑[7]的存在影響移動機器人的正常行走，顛簸情況導致機器人的位置和姿態變化劇烈，組合導航系統可能失效。月球表面特征不顯著、機器人的機載計算資源有限等問題都對同步建圖與定位方法提出了挑戰。同時，月球表面環境的光照變化極為劇烈，對視覺同步建圖與定位來說是一個巨大的挑戰，文獻[8]提出了一個針對月面環境的視覺同步建圖與定位算法。基于視覺的同步建圖與定位很難全天候進行工作，但是激光雷達不受光照條件限制，且探測距離遠、精度高[9]。文獻[10-11]的研究基于擴展卡爾曼濾波的同步建圖與定位，通過三維激光進行自然信標的跟蹤，在模擬火星環境驗證了有效性，但是月球表面難以捕捉穩定的特征信標，算法在計算實時性和精度上有待提高。文獻[12]提出第一個完整的基于激光的同步建圖與定位算法，在模擬月球的環境中驗證了有效性，但是由于沒有融合慣性傳感器等其他元件，系統的精度還有待提升。文獻[13]利用激光的強度信息和提取的地標提升了月球環境里程計的精度，但是目前仍依賴于人工反復測量的地標，在實際應用中存在一定的困難。

針對月球表面坑洼路面導致的機器人位姿劇烈變化的問題，通過擴展卡爾曼濾波[14](Extend-ed Kalman Filter，EKF)對輪式里程計和慣性傳感器進行融合，實現位姿的粗略估計；月面環境地形起伏程度不一，基于三維激光的點云觀測會存在部分特征不顯著的冗余信息，本文提出一種顯著特征點云提取算法，基于深度圖和高度灰度圖進行灰度熵計算和點云分割，對點云特征進行解析計算，提取特征顯著的點云。針對位姿估計不確定情況下的位姿優化問題，提出了基于曲面定位能力估計的增量式優化方法，通過對組合導航位姿優化，實現有限計算資源下的高精度實時定位。最后，在物理仿真月面環境中開展測試，驗證算法的有效性。

1 月面三維激光SLAM算法架構

算法整體架構如圖1所示。在顯著特征點云提取部分，根據點云映射的深度圖、高度圖進行點云分割，通過主成分分析解析點云塊的特征值，基于特征值定義平面度和線性度，根據線性度和平面度篩選提取數據，從而獲得顯著特征點云。

同時，基于里程計和慣性傳感器數據擴展卡爾曼濾波融合，在獲得粗略位姿估計基礎上，融合激光觀測計算位姿估計的不確定性，將位姿估計的不確定性指標作為優化參數之一，利用增量式的優化算法實現高精度定位。

2 顯著特征點云提取

月球表面環境特征不顯著，僅存在起伏地形、凹陷以及大小不一的石塊，缺乏平整地面、樹木、建筑等地面環境特征。月表起伏程度不一致的地面和大小不一的石塊包含著不同的特征信息量，基于三維激光進行觀測，在點云中獲取特征顯著的數據，有利于進行數據融合實現高精度的定位。

基于月面環境特征均在地面附近的特點，選擇三維激光傾斜向下安裝。首先對點云數據進行旋轉變化，使其平行于機器人本體質心。根據激光雷達的掃描模型，計算激光點的水平角度α和垂直角度ω，對點云數據進行深度矩陣映射。得到一個16×2 000維的矩陣，映射方法如式(1)所示：

(1)

式中：vbottom為最下面一層掃描線的垂直角度；vres為垂直角分辨率；hres為水平角分辨率；r、c分別為映射后的行坐標和列坐標。

得到深度矩陣后，在文獻[15]的點云分割算法基礎上，設定分割閾值S，得到若干點云塊。由于月面地形變化大，平地和坑洼地形處的特征顯著程度不一致。為了選擇顯著特征點云，將每個點云塊轉化為高度灰度圖，通過式(2)計算圖像的一元灰度熵H判斷點云特征的顯著程度。gk表示某個灰度在圖像中出現的概率，其中k=0,1,…,255表示灰度值，H越大表明特征越顯著。

(2)

(3)

對式(3)協方差矩陣C進行特征值分解，得到特征向量e0、e1、e2對應的3個特征值為λ0、λ1、λ2，特別地，λ0≥λ1≥λ2且被歸一化。由主成分分析[16](Principal Component Analysis，PCA)可知，對于場景中物體的3種特征類型具有不同的特征值表示，若有λ0≈λ1≈λ2則表示散亂特征；若有λ0?λ1≈λ2則表示線性特征；若有λ0≈λ1?λ2則表示平面特征。定義線性度Lλ與平面度Plλ，其中Xv、Yv、Zv分別為X、Y、Z方向單位向量：

(4)

對每個點云塊計算如下3個值：

(5)

式中:f1表示平面特征且決定對(tx,θ)的貢獻;f2表示平面特征且決定對(ty,θ)的貢獻;f3表示線性特征且決定對(tx,ty)的貢獻。選取f1、f2、f3大的點云塊數據，提取了有利于環境中有利于定位(tz,ψ,φ)方向與(tx,ty,θ) 方向的點云數據。

由于點云數據是輸入到匹配算法中用于求解連續兩幀數據間的變換矩陣，針對月球車的三維姿態估計問題，機器人六自由度位姿為p=[x,y,z,φ,ψ,θ]，變換矩陣為[tx,ty,tz,tφ,tψ,tθ]，其中φ,ψ,θ分別為機器人的俯仰角、滾轉角和偏航角。

圖2所示的月面環境場景觀測中，初始三維點云觀測數據如圖2(b)所示，一些較小的石塊與凹坑，由于其三維點云觀測特征不明顯被濾除，從而得到顯著特征提取后的點云數據，如圖2(c)所示。

圖2 實際場景與特征提取前后點云觀測Fig.2 Actual scene and pointcloud before and after feature extraction

3 基于曲面定位能力估計的增量式優化

基于SLAM方法對月面移動機器人進行位姿估計時，由于某些位置的顯著特征點云觀測少，易導致不準確的點云數據關聯甚至錯誤匹配。通過計算當前位姿估計的不確定性，自適應地調整點云匹配的優化參數，能夠有效減少位姿估計的誤差和漂移，提升位姿估計的準確性。

在本文中，將Fisher信息矩陣的逆矩陣作為協方差。Fisher信息矩陣定義為預期測量值和激光傳感器掃描的表面坡度的函數[17]。Liu[17]和Wang[18]等給出了基于二維概率網格的Fisher信息矩陣的推導。定義一幀激光觀測數據包含一組離散的激光雷達點信息。假設激光點i的預期長度為riE，其中i=1,2,…,M，M為雷達掃描線總數，則離散機器人可定位性矩陣可以定義為

(6)

(7)

最終可以給出月面環境下的位姿不確定性估計如式(8)所示：

(8)

根據概率模型，將SLAM問題轉換為基于最大后驗概率估計的最小二乘問題，最終基于Kaess等[19]提出的增量優化算法求解該最小二乘問題。

針對優化過程中的點云數據關聯問題，很多算法被提出來解決此類問題，例如迭代最近點匹配[20]、正態分布變換[21]、柵格相關性[22]。在本文中，將數據關聯分為兩類：時間相鄰和空間相鄰。對于時間相鄰的觀測，本文利用正態分布變換(Normal Distributions Transform，NDT)算法將時間相鄰幀觀測數據關聯起來，從而可以獲得當前機器人姿態的粗略估計。針對月球探索機器人來說，由于觀測顯著特征的匱乏會導致匹配陷入局部最優，幀間匹配無法獲得準確的姿態估計，使地圖缺乏一致性，從而導致機器人定位的精度顯著降低。因此選擇ICP算法將當前幀與全局地圖匹配，以進一步消除誤差。

(9)

4 實驗環境與結果分析

4.1 仿真實驗環境

為驗證本文算法，在Gazebo中搭建仿真月球車，并搭建了A、B兩個仿真環境模擬月面環境中存在的隕坑、石塊和山脈等地形，如圖3所示。

仿真月球車(如圖4所示)配備有三維激光(Velodyne VLP-16)、里程計、慣性傳感器(ADIS16495)，三維激光幀發布頻率為10 Hz，慣性傳感器幀發布頻率200 Hz、偏置穩定度0.8 (°)/h。三維激光最遠探測距離100 m，分辨率0.001 m，高斯噪聲方差為0.01。

程序運行環境為Ubuntu16.04+ROS(Kinetic版本)，CPU為i3-3110M，運行內存為4 G。

4.2 實驗數據與分析

為了驗證算法的性能，以及在不同環境下的有效性，選擇分別在場景A、B進行實驗。實驗中算法參數如表1所示。

為了體現算法的性能，選擇LOAM[23](Lidar Odometry And Mapping)算法進行對比實驗。對機器人的平移旋轉誤差進行分析，場景A實驗的定位軌跡與對比如圖5所示，位置誤差如圖6所示，角度誤差如圖7所示。本文算法與LOAM算法誤差分析如表2所示。在圖5(c)圓圈所示區域，機器人處于隕坑向上邊緣，姿態傾斜，可觀測信息量下降，導致定位誤差在此區域增大。