一種新的基于LiDAR/IMU組合的室內(nèi)無人車導(dǎo)航系統(tǒng)

祝瑞輝，蔚保國，李 爽

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

0 引言

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

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

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

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

1.1 遍歷搜索估計(jì)算法

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

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

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

算法1：候選者A適應(yīng)度計(jì)算。1. 輸入：k采樣時(shí)刻的載波相位量測φ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)度值越小，越接近真實(shí)位置值。

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

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

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

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

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

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

2 濾波設(shè)計(jì)

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

圖1 濾波設(shè)計(jì)

3 試驗(yàn)分析

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

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

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

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

表1 位置誤差

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

圖3 姿態(tài)角

4 結(jié)束語

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