尚天祥，王景川,*，董凌峰，陳衛(wèi)東

1. 上海交通大學(xué) 電子信息與電氣工程學(xué)院，上海 200240 2. 系統(tǒng)控制與信息處理教育部重點(diǎn)實(shí)驗(yàn)室，上海 200240

月球探索機(jī)器人[1]在無(wú)定位系統(tǒng)輔助的未知環(huán)境中執(zhí)行任務(wù)，需要精確獲取機(jī)器人的實(shí)時(shí)位姿。高可靠的定位性能為未知環(huán)境探索、局部導(dǎo)航避障提供了重要的依據(jù)。因此，研究在復(fù)雜月球表面環(huán)境中精確、快速得獲取機(jī)器人的位姿尤為重要。

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

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

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

1 月面三維激光SLAM算法架構(gòu)

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

同時(shí)，基于里程計(jì)和慣性傳感器數(shù)據(jù)擴(kuò)展卡爾曼濾波融合，在獲得粗略位姿估計(jì)基礎(chǔ)上，融合激光觀測(cè)計(jì)算位姿估計(jì)的不確定性，將位姿估計(jì)的不確定性指標(biāo)作為優(yōu)化參數(shù)之一，利用增量式的優(yōu)化算法實(shí)現(xiàn)高精度定位。

2 顯著特征點(diǎn)云提取

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

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

(1)

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

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

(2)

(3)

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

(4)

對(duì)每個(gè)點(diǎn)云塊計(jì)算如下3個(gè)值：

(5)

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

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

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

圖2 實(shí)際場(chǎng)景與特征提取前后點(diǎn)云觀測(cè)Fig.2 Actual scene and pointcloud before and after feature extraction

3 基于曲面定位能力估計(jì)的增量式優(yōu)化

基于SLAM方法對(duì)月面移動(dòng)機(jī)器人進(jìn)行位姿估計(jì)時(shí)，由于某些位置的顯著特征點(diǎn)云觀測(cè)少，易導(dǎo)致不準(zhǔn)確的點(diǎn)云數(shù)據(jù)關(guān)聯(lián)甚至錯(cuò)誤匹配。通過(guò)計(jì)算當(dāng)前位姿估計(jì)的不確定性，自適應(yīng)地調(diào)整點(diǎn)云匹配的優(yōu)化參數(shù)，能夠有效減少位姿估計(jì)的誤差和漂移，提升位姿估計(jì)的準(zhǔn)確性。

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

(6)

(7)

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

(8)

根據(jù)概率模型，將SLAM問(wèn)題轉(zhuǎn)換為基于最大后驗(yàn)概率估計(jì)的最小二乘問(wèn)題，最終基于Kaess等[19]提出的增量?jī)?yōu)化算法求解該最小二乘問(wèn)題。

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

(9)

4 實(shí)驗(yàn)環(huán)境與結(jié)果分析

4.1 仿真實(shí)驗(yàn)環(huán)境

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

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

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

4.2 實(shí)驗(yàn)數(shù)據(jù)與分析

為了驗(yàn)證算法的性能，以及在不同環(huán)境下的有效性，選擇分別在場(chǎng)景A、B進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)中算法參數(shù)如表1所示。

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

表1 算法參數(shù)設(shè)定Table 1 Setting of algorithm parameters

其次，選擇場(chǎng)景B進(jìn)行實(shí)驗(yàn)，算法定位軌跡與真值如圖8所示。對(duì)機(jī)器人的平移旋轉(zhuǎn)誤差進(jìn)行分析，位置誤差如圖9所示，角度誤差如圖10所示。

本文算法與LOAM算法誤差如表3所示。在圖8(b)圓圈區(qū)域內(nèi)，觀測(cè)顯著減少且機(jī)器人角速度較大，定位精度有一定下降。

在場(chǎng)景A和場(chǎng)景B分別進(jìn)行了算法的仿真驗(yàn)證，對(duì)算法的實(shí)時(shí)性分析如表4所示，由于場(chǎng)景B中特征較為豐富，特征顯著點(diǎn)云提取部分耗時(shí)相應(yīng)增多，導(dǎo)致實(shí)時(shí)性會(huì)有一定程度下降。本文算法相較LOAM算法實(shí)時(shí)性更高，在有限的計(jì)算資源條件下，滿足了實(shí)時(shí)性的要求，并且實(shí)現(xiàn)了較好的定位精度。

表2 場(chǎng)景A定位誤差Table 2 Localization errors of scene A m

圖9 位置誤差(場(chǎng)景B)Fig.9 Position errors (scene B)

表3 場(chǎng)景B定位誤差Table 3 Localization errors of scene Bm

表4 算法實(shí)時(shí)性分析Table 4 Real-time performance analysis Hz

圖10 姿態(tài)角誤差(場(chǎng)景B)Fig.10 Angle errors (scene B)

5 結(jié) 論

本文針對(duì)月球環(huán)境下的移動(dòng)機(jī)器人定位問(wèn)題，研究了一種基于三維激光的SLAM算法，本文的主要貢獻(xiàn)在于：

1) 針對(duì)月面環(huán)境特征不顯著的特點(diǎn)，提出了顯著特征點(diǎn)云提取算法，有效從點(diǎn)云中提取特征顯著的數(shù)據(jù)。

2) 基于Fisher信息矩陣計(jì)算曲面定位能力指標(biāo)，基于曲面定位能力實(shí)現(xiàn)對(duì)位姿不確定性的估計(jì)，通過(guò)增量式優(yōu)化提升了位姿估計(jì)的準(zhǔn)確性。

本文提出的算法通過(guò)仿真實(shí)驗(yàn)，充分驗(yàn)證了算法的有效性，表明本文算法有更高的定位精度和實(shí)時(shí)性。