利用UWB校正LiDAR SLAM的室內(nèi)組合定位方法*

王昭東，秦文虎

(東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

0 引 言

定位與導(dǎo)航技術(shù)是室內(nèi)自主移動機器人的關(guān)鍵技術(shù)之一。在室外良好的情況下，以全球定位系統(tǒng)(global positioning system,GPS)、北斗系統(tǒng)(BDS)為代表的全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS)能夠提供良好的定位效果[1]。但在室內(nèi)環(huán)境下，由于建筑物對信號的遮擋，GNSS的信號會嚴(yán)重衰減，無法滿足室內(nèi)高精度定位的需要。在這種情況下，室內(nèi)定位技術(shù)朝多方向的發(fā)展。按照使用的傳感器分類，常見的室內(nèi)定位技術(shù)有WiFi[2]、射頻識別(radio frequency identification,RFID)、超寬帶(ultra-wideband,UWB)、慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)、藍牙(Bluetooth)、激光雷達(LiDAR)、工業(yè)相機等[3]。

近年來，學(xué)者們趨向于采用多傳感器融合的方法來解決室內(nèi)定位的問題。文獻[4]提出利用粒子濾波器融合加速度計、指南針和慣性測量單元(inertial measurement unit,IMU)的數(shù)據(jù)實現(xiàn)室內(nèi)定位的方法，在不考慮實時性的情況下具有比較高的精度；文獻[5]提出了一種融合了智能手機中方向傳感器、環(huán)境中的WiFi和藍牙的多源信息融合的定位算法。文獻[6]提出的將雙目立體視覺和IMU數(shù)據(jù)緊耦合的組合定位導(dǎo)航算法，具有較好的精度，魯棒性較好。文獻[7]提出了一種將里程計、IMU和視覺相結(jié)合的輪式機器人定位的輔助導(dǎo)航系統(tǒng)，實驗證明該系統(tǒng)具有較高的定位精度。

相比于其他技術(shù)，LiDAR具有分辨率高、抗干擾能力強的優(yōu)點，而SLAM技術(shù)是解決LiDAR定位的熱門技術(shù)[8]。但LiDAR即時定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)要求工作的環(huán)境有豐富的特征。在特征缺乏的環(huán)境下，LiDAR SLAM算法會發(fā)生特征匹配錯誤的問題，造成誤差的累積，這將導(dǎo)致定位結(jié)果出現(xiàn)較大誤差。UWB具有低功耗、高時間分辨率和保密性好的優(yōu)點[9,10]，并且具有較大的覆蓋范圍。但UWB容易受到多徑效應(yīng)和非視距誤差的影響，導(dǎo)致定位的結(jié)果發(fā)散。

本文提出了一種利用UWB觀測值來校正LiDAR SLAM預(yù)測結(jié)果的組合定位方法。首先，通過里程計預(yù)測模型得到下一時刻的位姿信息；之后，利用粒子濾波的方法融合雷達觀測值對局部定位結(jié)果進行修正，并通過坐標(biāo)變換將局部定位結(jié)果轉(zhuǎn)換到全局坐標(biāo)系中；最后，利用UWB觀測值對全局坐標(biāo)系中的定位結(jié)果進行修正。該組合方法的優(yōu)勢在于，既有效地通過UWB來消除LiDAR SLAM中的累積誤差，又抑制了定位結(jié)果較為分散的現(xiàn)象。

1 UWB與LiDAR SLAM定位原理

1.1 UWB定位原理

基于測距信息進行定位是UWB定位的常用方法。雙向測距(two-way ranging,TWR)算法[11]是一種典型的測距方法，這種測距方法不需要信標(biāo)之間保持時間同步。

圖1 DS-TWR算法原理

1.2 LiDAR SLAM定位原理

常用的LiDAR SLAM算法中，目前Gmapping算法[12]融合了里程計信息，同時，使用了Rao-Blackwellized 粒子濾波(Rao-Blackwellized particle filtering,RBPF)算法，在構(gòu)建小場景地圖時所需的計算量較小、精度較高，且相對于Hector算法[13]來說魯棒性更好；Cartographer算法[14]由于需要大量的粒子且有回環(huán)檢測的部分，需要較大的計算量，適用于構(gòu)建大場景并需要部署在算力較高的設(shè)備上。綜合考慮，本文選用Gmapping算法作為LiDAR SLAM的基本算法。

Gmapping算法中采用的RBPF算法將SLAM問題中同時定位與建圖的問題拆分成了2個問題：一個問題是已知預(yù)測值和觀測值求位姿；另一個問題是已知機器人位姿進行地圖構(gòu)建，如式(1)所示

p(x1︰t,m|z1︰t,u1︰t)=p(m|x1︰t,z1︰t)p(x1︰t|z1︰t,u1︰t)

(1)

式中x1︰t為1到t時刻的位姿，m為地圖，z1︰t為LiDAR的觀測值，u0︰t為里程計得到的輸入值。本文關(guān)心的問題為兩個問題中求位姿的問題。通過貝葉斯準(zhǔn)則可以對p(x1︰t|z1︰t,u0︰t)進行分解得到

p(x1︰t|z1︰t,u1︰t)=ηp(zt|xt)p(xt|xt-1,ut)p(x1︰t-1|u1︰t-1,

z1︰t-1)

(2)

通過推導(dǎo)，求位姿的過程可以分解成3個過程：p(x1︰t-1|u1︰t-1,z1︰t-1)代表獲取上一時刻的位置估計；p(xt|xt-1,ut)代表在上時刻位姿的基礎(chǔ)上，通過里程計預(yù)測模型求得當(dāng)前位置的估計值；ηp(zt|xt)代表通過LiDAR觀測模型，對各個預(yù)測的粒子進行權(quán)重歸一化，得到當(dāng)前時刻的位姿。

2 UWB/LiDAR融合定位算法

融合算法的流程如圖2。

在語言學(xué)大師索緒爾的眼中，符號是由 “能指”（signifier）與“所指”（signified）構(gòu)成的二元實體。[8]商標(biāo)犯罪侵犯的是社會主義市場經(jīng)濟秩序。細言之，以商業(yè)利用的方式而假冒他人的注冊商標(biāo)應(yīng)用在自己的商品或者服務(wù)上，造成消費者對相關(guān)商品或者服務(wù)的混淆，從而無法清晰分辨各個商標(biāo)所施指的商品或服務(wù)，致使商標(biāo)指示市場的混亂。

圖2 融合算法流程

首先利用上一時刻的粒子群信息來初始化位姿信息xt-1，粒子權(quán)重wt-1和地圖mt-1，如式(3)

(3)

之后利用里程計預(yù)測模型得到位置的估計值

(4)

在LiDAR觀測模型的基礎(chǔ)上通過極大似然估計求得局部極值

(5)

如果這里沒有找到局部極值，使用提議分布，更新粒子的位姿狀態(tài)，并使用觀測模型對位姿的權(quán)重進行更新，并從初始化階段重新開始

(6)

(7)

如果找到了局部極值，在局部極值附近取k個位姿

(8)

假設(shè)k個位姿分布服從高斯分布，對k個位姿xj∈{x1,…,xk}求均值和歸一化參數(shù)

(9)

(10)

對均值進行歸一化處理

(11)

計算k個位姿的方差

(12)

方差歸一化處理

(13)

這樣可以用正態(tài)分布的形式來表示新的位姿

(14)

更新該位姿粒子的權(quán)重

(15)

引入UWB的觀測值，對每個粒子的均值和方差進行修正，設(shè)UWB在t時刻的位置信息為

(16)

(17)

根據(jù)更新后的粒子權(quán)重、位置信息加權(quán)求和，可以得到修正后的當(dāng)前位置的信息。最后通過離子權(quán)重和各粒子的均值加權(quán)平均得到當(dāng)前位置的定位結(jié)果。

3 實驗與結(jié)果分析

3.1 實驗機器人的搭建

搭建機器人平臺，如圖3所示。

圖3 實驗小車的左視圖和俯視圖

本文實驗選取的UWB模塊為研創(chuàng)物聯(lián)基于Decawave1000的UWB Mini 3s Plus模塊，選用的LiDAR模塊為思嵐科技的RPLiDAR A1模塊。UWB傳感器的參數(shù)：發(fā)射功率為110 kW/6.8 M，覆蓋半徑為300 m(110 kW)，測距誤差小于10 cm，定位誤差小于15 cm。LiDAR參數(shù)：測量半徑為12 m，測量角度為360°，測量頻率為8 000次/s，掃描頻率為2～10 Hz，角度誤差為1°。

安裝時要將服務(wù)端的UWB信標(biāo)的天線置于LiDAR的正上方，便于兩者坐標(biāo)系的統(tǒng)一。小車的ROS系統(tǒng)部署在一塊樹莓派4B上。為了緩解樹莓派的運算壓力，UWB的測距信息首先被一塊STMF103C8接收，將得到的坐標(biāo)解算結(jié)果通過串口發(fā)送給樹莓派。同時為了便于監(jiān)控系統(tǒng)的運行狀態(tài)，安裝一個無線串口，將小車位姿數(shù)據(jù)實時發(fā)送給電腦。

3.2 實驗場景布置

實驗場地布置在一個11.7 m×8.5 m的房間中進行，在房間中央布置場地作為試驗場所。場地中布置4個UWB基站，并在地面貼設(shè)地標(biāo)作為參考標(biāo)識。場景的布置效果如圖4所示。

圖4 實驗場地布置效果

UWB基站與地標(biāo)的位置關(guān)系如圖5所示。

圖5 UWB與地標(biāo)位置關(guān)系

圖5中A0，A1，A2，A3為布置在場地中的4個UWB基站，線框代表地標(biāo)線貼設(shè)的位置。各距離測量值通過激光測距儀得到，誤差為±2 mm。實驗時操控小車從A點開始按照A→B→C→D→A的順序沿著地標(biāo)線運動，比較通過傳感器定位得到的結(jié)果和地標(biāo)參考位置之間的差距。

3.3 實驗結(jié)果與分析

分別讓機器人在UWB單獨定位，LiDAR SLAM單獨定位和組合定位的條件下，沿著地標(biāo)線運行1周，得到的結(jié)果如圖6所示。

圖6 定位實驗結(jié)果

圖6(a)中散點構(gòu)成的軌跡為UWB單獨定位得到的結(jié)果分布情況，虛線框代表由地標(biāo)線標(biāo)識的參考軌跡。由圖可見，在UWB單獨定位的情況下，小車的定位軌跡分布在參考軌跡周圍，定位結(jié)果比較分散。圖6(b)中散點代表單獨LiDAR SLAM得到的定位結(jié)果分布的情況，虛線框代表地標(biāo)參考軌跡?？梢?，在LiDAR單獨定位的情況下，小車的軌跡比較平滑，但由于實驗環(huán)境中缺少特征，隨著時間的推進，累積誤差逐漸變大。同時可以觀察到由于定位點缺失導(dǎo)致的軌跡中斷現(xiàn)象。圖6(c)中散點代表UWB/LiDAR融合方法的定位結(jié)果分布情況。由圖可見，在組合定位下，小車的軌跡較為平滑，并消除了累積誤差，具有比較好的定位效果。

統(tǒng)計小車在A，B，C，D四點坐標(biāo)的均方根誤差如表1。

表1 均方根誤差統(tǒng)計 m

4 結(jié) 論

本文提出的一種利用UWB觀測值來校正LiDAR SLAM的組合定位方法，在LiDAR SLAM的基礎(chǔ)上通過高斯濾波的方法融合UWB的觀測值，并通過實驗對單獨使用UWB和LiDAR SLAM兩種定位方式進行了比較。實驗結(jié)果表明：本文提出的組合定位方法能夠有效地消除LiDAR SLAM算法的累積誤差，同時改善了單獨用UWB進行定位時結(jié)果分散的現(xiàn)象。因此，相對于單一傳感器，本文提出的組合定位系統(tǒng)更適用于室內(nèi)高精度定位。