雖然目前針對(duì)GPS-denied環(huán)境下的定位方法很多，但實(shí)現(xiàn)穩(wěn)健的幾何退化環(huán)境下的定位，如隧道，仍然是一個(gè)具有挑戰(zhàn)性的問(wèn)題。將IMU、LiDAR和UWB相結(jié)合的傳感器融合方法。實(shí)驗(yàn)結(jié)果表明，該方法可以實(shí)現(xiàn)長(zhǎng)直隧道的魯棒定位。

將UWB和IMU-LiDAR系統(tǒng)結(jié)合起來(lái)進(jìn)行精 確定位。

1. LIDAR定位能力

LIDAR通過(guò)長(zhǎng)直隧道時(shí)，因?yàn)楦魈幍臏y(cè)量值都相同故無(wú)法確定其位置。如圖1，通過(guò)類比管道內(nèi)滑塊來(lái)解釋退化?；瑝K是通過(guò)連接激光雷達(dá)掃描的端點(diǎn)獲得的。

2. UWB定位能力

UWB測(cè)量固定的錨點(diǎn)和移動(dòng)的目標(biāo)之間的距離。

3.LIDAR和UWB的概率融合

融合基于Error State Kalman Filter (ESKF)，與EKF類似，只是在誤差狀態(tài)下對(duì)系統(tǒng)運(yùn)動(dòng)學(xué)建模，對(duì)系統(tǒng)線性化有益。其中，積分IMU以預(yù)測(cè)機(jī)器人狀態(tài)，LiDAR匹配先驗(yàn)地圖以恢復(fù)6D位姿之后進(jìn)行更新預(yù)測(cè)，UWB測(cè)量3D位置之后也對(duì)位置更新。LiDAR和UWB的更新率取決于數(shù)據(jù)頻率。

 隧道內(nèi)定位能力

在隧道沿線20個(gè)均勻采樣點(diǎn)對(duì)LiDAR定位能力評(píng)估。X軸定義為沿隧道方向，Z軸定義為重力方向。在15m范圍內(nèi)均勻采樣4000個(gè)點(diǎn)（大致為旋轉(zhuǎn)180°激光點(diǎn)數(shù)量）以模擬每個(gè)地方的測(cè)量。由于距離遠(yuǎn)的點(diǎn)有近90度的反射角，使其有效范圍減小，導(dǎo)致測(cè)量不可靠。在每一點(diǎn)通過(guò)擬合附近20點(diǎn)估計(jì)平面。利用式2-6可以估計(jì)其定位能力。如圖6可以看出，X軸方向更差一些，因?yàn)槭茄厮淼婪较蛲嘶瘒?yán)重。由于隧道有一個(gè)弧頂和幾乎相同的高度和寬度，雷達(dá)測(cè)量的roll角不能很好約束，但好在融合IMU后約束較好。

如圖7，特征空間的X軸與體坐標(biāo)系平行，因?yàn)槠涫峭嘶较颉?/span>Y、Z方向沒(méi)有顯著差異。

關(guān)于UWB，將力矩陣投影到特征空間評(píng)估定位能力。相較于LiDAR，UWB在X軸方向定位較好，這為融合它們提供了好的約束。然而其在錨點(diǎn)附近X方向定位能力下降，這是奇異點(diǎn)?？梢栽黾渝^點(diǎn)數(shù)量解決。