祝瑞輝，蔚保國，李 爽

（1.衛(wèi)星導(dǎo)航系統(tǒng)與裝備技術(shù)國家重點實驗室，河北 石家莊 050081;2.中國電子科技集團公司第五十四研究所，河北 石家莊 050081）

0 引言

基于慣性測量單元（Inertial Measurement Unit, IMU）和里程計的航位推算系統(tǒng)（Dead Reckoning System, DRS）具有在3D空間中自主導(dǎo)航、連續(xù)、易于隱藏等良好特性[1]。但是，陀螺儀固有的漂移誤差和加速度計存在的噪聲會導(dǎo)致角速度和加速度積分產(chǎn)生累積誤差，該誤差隨運行時間而不斷累積，沒有邊界，這使得DRS在沒有其他導(dǎo)航手段輔助的情況下不可靠[2]。外部無線室內(nèi)定位方法被廣泛研究以實現(xiàn)絕對定位，包括偽衛(wèi)星[3][4]、射頻識別（RFID）[5]、近距離通信（NFC）[6]、Wi-Fi[7]、藍牙[8]、超寬帶（UWB）[9]等。所有這些都難以滿足UGV對室內(nèi)定位精度，連續(xù)性和魯棒性的要求。盡管基于外部無線定位設(shè)備和慣性傳感器研究了多源信息融合技術(shù)，包括將Wi-Fi與慣性傳感器融合[10][11]、Wi-Fi與藍牙組合[12]以及Magnetic/Wi-Fi/PDR三者融合[13]，仍然無法解決上述問題。

視覺和激光雷達（Light Detection and Ranging, LiDAR）是一種常用的室內(nèi)定位和構(gòu)圖技術(shù)，迭代最近點（Iterative Closest Point，ICP）算法[14]和迭代最近線（Iterative Closest Line，ICL）算法[15]被廣泛用于掃描匹配。如果單純使用激光雷達進行點云的掃描匹配，需要激光雷達有很高的掃描頻率，較高的掃描頻率會加重計算機CPU的數(shù)據(jù)處理負擔(dān)。遺傳算法（GA）也是一種原始數(shù)據(jù)匹配技術(shù)，它利用強大的搜索功能在大范圍內(nèi)尋找位姿變化[16]。文獻[17]提出了一種由GA和ICP算法組成的混合掃描匹配算法，其中GA用于搜索全局最大值，而ICP算法用于優(yōu)化從GA導(dǎo)出的初始姿態(tài)估計值。然而GA太過復(fù)雜，大大加重了服務(wù)器的運行負擔(dān)。當(dāng)DRS推導(dǎo)的初始姿勢估計值與全局估計值相差甚遠時，僅使用ICP算法可能會使姿勢估計值陷入局部最小值或最大值。本文提出了一種新的2D掃描匹配算法，該算法首先使用遍歷搜索估計算法（Ergodic search estimation algorithm, ESSA）來尋找一個大略位姿初始估計值，然后使用數(shù)值優(yōu)化方法對該ESSA估計的位姿進行優(yōu)化。由谷歌工程師開發(fā)的數(shù)值優(yōu)化算法[18]已被有效地證明可以解決具有邊界約束的非線性最小二乘問題和實際工程應(yīng)用中的一般無約束優(yōu)化問題，已被制圖師使用。本文提出的掃描匹配方法的靈感來自hectorslam[19]。但是，我們將適應(yīng)度函數(shù)引入到ESSA中，以在較大范圍內(nèi)搜索良好的初始估計值，而hectorslam在第一階段使用多分辨率網(wǎng)格圖，從不同角度解決了相同的問題。

1 一種新的混合掃描匹配方法

本文提出一種新的混合二維掃面匹配算法，其中遍歷搜索估計值算法用于粗略獲得姿態(tài)變化估計值，然后將粗略姿態(tài)估計值作為數(shù)據(jù)優(yōu)化算法的初始值，經(jīng)數(shù)值優(yōu)化后，進一步獲得更準確的姿態(tài)估計值。

1.1 遍歷搜索估計算法

該算法的關(guān)鍵是選擇合適的適應(yīng)度函數(shù)來計算每一個候選位置估計準確程度的大小。本文將激光擊中占有網(wǎng)格概率之和作為適應(yīng)度函數(shù)來求解位置估計，作為數(shù)據(jù)優(yōu)化算法的初始姿態(tài)值。算法步驟如下所示。

（1）初始時刻位置。令(x(0),y(0))表示初始位置，t=0表示迭代計數(shù)器，T表示最大迭代次數(shù)。在初始位置±0.5 m內(nèi)以0.1 m為分辨率均勻選取M個位置作為初代候選者P(0)。

（2）計算候選者適應(yīng)度值。由于適應(yīng)度值的大小決定了它成為最終位置估計值，因此，適應(yīng)度函數(shù)的選擇直接影響遺傳算法的性能。假設(shè)A是一個候選者。令φAi(k)和ρAi(k)分別表示第i個天線的載波相位和偽距觀測量，其中k是采樣時刻。候選者A的位置表示為PA，第i個天線的位置表示為psati。候選者A的平滑偽距表示為。令RAi(k)為候選者A的預(yù)測偽距。載波相位平滑偽距與預(yù)測偽距之差為eAi(k)；FA(k)為候選者A的適應(yīng)度值；n是偽衛(wèi)星天線個數(shù)。

算法1：候選者A適應(yīng)度計算。1. 輸入：k采樣時刻的載波相位量測φAi(k)。

2. 輸出：候選者A的適應(yīng)度值3. for i=1 : n 4. =φAi(k)×c/fc 5. RAi(k)=||PA-psati||6. eAi(k)=- RAi(k)7. end for 8. FA(k)=

適應(yīng)度值越小，越接近真實位置值。

（3）終止判定。如果t=T，擁有最小適應(yīng)度值的候選者被選為最優(yōu)估計。

1.2 數(shù)值優(yōu)化算法

眾所周知，非線性最小二乘問題的成本函數(shù)是誤差項的總和（稱為“殘差”）。由遍歷搜索估計算法估算的UGV姿態(tài)變化表示為，令Tm=[xm,ym,Am]表示量測轉(zhuǎn)換。求解由遍歷搜索估計方法計算出的初始位姿估計的旋轉(zhuǎn)和平移成本，該成本隨著解與初始位置估計距離的增加而增加。和從掃面幀到映射幀的量測變換Tm之間的加權(quán)平移和旋轉(zhuǎn)誤差分別計算為[20]：

式中，wt和wr分別為平移和旋轉(zhuǎn)的權(quán)重。

計算將點云描述的占用空間插入地圖的代價，該代價隨著可用地圖空間將由占用空間代替的數(shù)量增加而增加。

函數(shù)Msmooth是映射在圖中的概率值的平滑形式，是一個雙三次插值函數(shù)。由于數(shù)值優(yōu)化算法是局部優(yōu)化，因此它的精度要高于遍歷搜索估計方法，但需要較好的初始估算。獲得的掃描點可寫為H={hk}k=1,2,…,K,hk∈R2。

2 濾波設(shè)計

LiDAR和DRS的緊密耦合僅比松散耦合稍高，但代價是算法更加復(fù)雜。本文我們主要想證明所提出系統(tǒng)可以顯著降低DRS估計值的累積誤差。因此，在提出的系統(tǒng)中，LiDAR與DRS采用松耦合，以降低算法實現(xiàn)的復(fù)雜性。將IMU與里程表集成來構(gòu)建DRS，以獲取UGV姿態(tài)預(yù)測估計。一方面，該預(yù)測估計值用作新的混合2D掃描匹配算法運行的初始值，另一方面，它通過UKF與從LiDAR測量中得出的姿態(tài)變化估計值進行融合。本文所提系統(tǒng)的框圖如圖1所示。

圖1 濾波設(shè)計

3 試驗分析

真實的實驗已在測試大樓中使用自行開發(fā)的UGV進行，以評估所提出算法的性能。UGV配備了IMU XSENS MTI-10-2A5G4-DK（荷蘭），激光掃描儀RPLIDAR A2（SLAMTEC，中國），正交編碼器和微型處理器。微處理器用于運行車輛的導(dǎo)航程序，運動控制器模塊和電池位于車輛底部，無人車平臺由無線控制器控制，IMU、車輪編碼器和激光掃描儀的采樣頻率分別為100 Hz、20 Hz和10 Hz。

圖2中的長虛線是UGV行駛的預(yù)設(shè)參考軌跡，約為190 m。在無線控制器的控制下，車輛從建筑物的東南角開始沿著走廊行駛，然后沿順時針方向移動一圈后返回起點，在沿著走廊的運動中，無人駕駛車進入教室。圖2中的點虛線軌跡是所提出系統(tǒng)的性能。從圖2中我們還可以看到，由于手動控制的錯誤，UGV不能精確地按照當(dāng)前的參考軌跡運動。例如，當(dāng)UGV轉(zhuǎn)彎時，實際的運動軌跡是圓弧，而當(dāng)前的參考軌跡是直角。為了說明所提出算法的有效性，我們忽略了人工運算的錯誤，這仍然是可行的。

圖2 LiDAR/DRS松組合導(dǎo)航軌跡

為了更直觀地證明所提出系統(tǒng)的性能，我們選擇了5個點作為參考位置（路標），如圖2所示。當(dāng)UGV在測試大樓中行駛時，它停在參考點處以收集至少15 s的位置信息，然后計算位置誤差以比較DRS和LiDAR輔助系統(tǒng)的精度。結(jié)果如表1所示。在起點處，DRS輸出的位置誤差為0.45 m，基于hectorslam方法的導(dǎo)航系統(tǒng)為0.14 m，而LiDAR輔助導(dǎo)航系統(tǒng)的位置誤差為0.13 m。當(dāng)UGV移動時，DRS的位置誤差繼續(xù)累積。在終點航路點，DRS的位置誤差達到6.30 m，基于hectorslam方法的導(dǎo)航系統(tǒng)達到1.02 m，而由LiDAR輔助的系統(tǒng)為0.48 m，消除了92.4%的誤差。

表1 位置誤差

姿態(tài)角變化曲線如圖3所示，從中可以看到，在UGV的整個運動過程中，方位角隨UGV的旋轉(zhuǎn)而變化。由于UGV一直在水平面上運動，俯仰角和橫滾角幾乎為零。

圖3 姿態(tài)角

4 結(jié)束語

在本文中，我們介紹了用于在室內(nèi)環(huán)境中行駛的基于LiDAR/IMU松組合導(dǎo)航系統(tǒng)的UGV平臺。DRS由IMU和里程計來構(gòu)建。此外，提出了一種新的由遍歷搜算估計算法和數(shù)值優(yōu)化算法構(gòu)成的混合二維掃描匹配算法，該算法比基于hectorslam的導(dǎo)航系統(tǒng)更有效。在辦公樓的真實實驗結(jié)果表明，本文提出的系統(tǒng)可以有效減少慣性傳感器裝置漂移引起的累積誤差，提高應(yīng)用性能，實現(xiàn)了室內(nèi)辦公樓整個軌跡的亞米導(dǎo)航精度。