室內(nèi)UWB/LiDAR組合定位算法

陳志鍵，徐愛功，隋 心，郝雨時，郭 哲

(遼寧工程技術(shù)大學(xué) 測繪與地理科學(xué)學(xué)院，遼寧 阜新 123000)

0 引言

如今基于位置的服務(wù)(location based service，LBS)已逐漸成為人們?nèi)粘Ｉ詈凸ぷ髦斜夭豢缮俚姆?wù)需求[1]。在室外良好的環(huán)境下，利用全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system，GNSS)與慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)構(gòu)成的組合定位系統(tǒng)可以提供厘米級的定位精度[2]；但在室內(nèi)環(huán)境下，GNSS信號高度衰減，已無法滿足高精度定位需求。

常見的室內(nèi)定位技術(shù)有無線保真(wireless fidelity，WiFi)、射頻識別(radio frequency identification，RFID)、超寬帶(ultra-wideband，UWB)技術(shù)[3]以及INS、激光探測與測量簡稱為激光雷達(light detection and ranging，LiDAR)和工業(yè)相機等。相比于其他技術(shù)，UWB具有低功耗、強穿透力、高時間分辨率和高傳輸數(shù)據(jù)速率的優(yōu)勢，而且可以提供較大的覆蓋范圍。但是UWB容易受到非視距(non line of sight，NLOS)與多路徑效應(yīng)等因素的影響，造成較大的定位誤差，降低了室內(nèi)環(huán)境下UWB定位的精度。針對這一問題，文獻[4]提出將NLOS傳播引起的偏差視為隨機變量，其概率密度函數(shù)能被發(fā)現(xiàn)并估計，提出的算法不需要訓(xùn)練，只需要假設(shè)最大可能偏差；文獻[5]提出利用卡爾曼濾波平滑獲得定位數(shù)據(jù)的方法。LiDAR具有分辨率高、獲取的信息量豐富、抗干擾能力強，能夠同時準(zhǔn)確量測距離和方位角等優(yōu)勢。但是LiDAR對工作的環(huán)境特征要求較高，在無特征環(huán)境下，室內(nèi)即時定位與地圖構(gòu)建(simultaneous localization and mapping，SLAM)算法產(chǎn)生誤差累積，這將導(dǎo)致定位結(jié)果發(fā)散，出現(xiàn)較大定位誤差，甚至無法定位。

針對室內(nèi)定位中UWB受非視距影響明顯以及LiDAR SLAM算法誤差累積的問題，本文建立了UWB/LiDAR組合定位算法，將UWB測距信息、LiDAR SLAM的位移增量和角度觀測值作為量測值，利用擴展卡爾曼濾波(extended Kalman filter，EKF)算法進行參數(shù)解算[6]。該組合優(yōu)勢在于：UWB/LiDAR組合定位系統(tǒng)能有效抑制SLAM算法的誤差累積；當(dāng)UWB出現(xiàn)NLOS影響時，UWB/LiDAR組合定位系統(tǒng)仍能提供良好的定位結(jié)果。

1 UWB和LiDAR SLAM定位原理

1.1 UWB定位原理

基于往返時間(round trip time，RTT)測距是通過測量UWB脈沖信號從流動站發(fā)出到達基準(zhǔn)站之后返回流動站的總時間，間接確定二者間的距離。因為傳輸過程是由輸入信號觸發(fā)，所以RTT測距不需要基站之間以及基站和流動站保持時間同步[7-8]，但是RTT測距必須準(zhǔn)確測量或者估計標(biāo)準(zhǔn)時間偏差。

測距模型[9]可表示為

(1)

式中：tround和pr分別為UWB流動站信號發(fā)出的時間以及此時刻的位置；ttrip和pb分別為UWB流動站信號接收的時間以及此時刻的位置；tD為UWB站間的標(biāo)準(zhǔn)時間偏差；c為光速；‖·‖2為矩陣的2范數(shù)；eNLOS為由非視距誤差造成的延遲。

因此，可以推算得到UWB流動站與基準(zhǔn)站之間的距離為

d=‖pr-pb‖2=rRTT-rD-rNLOS

(2)

在完全視距(line of sight，LOS)的環(huán)境下，通過大量的測距信息平均值與測距信息真實值差值算出rD，最后擬合確定出標(biāo)準(zhǔn)時間偏差的誤差模型，具體方法見參考文獻[10]。

UWB平面定位是利用3個及3個以上置于固定位置的UWB基站和1個流動站進行定位[11]，可得UWB的量測方程為

(3)

式中：dk,i為k時刻第i個基站和流動站間的量測距離，i=1,2,…,n；n為UWB基站的個數(shù)；vk,i為觀測噪聲序列；rk,i為k時刻第i個基站和流動站間的真實距離，且有

(4)

1.2 LiDAR SLAM定位原理

SLAM算法是目前解決LiDAR室內(nèi)定位的常用的方法，算法的主要原理是通過k時刻LiDAR獲得的數(shù)據(jù)，解算得到LiDAR的位姿信息(δdk,αk)，其中δdk為k時刻LiDAR相對于k-1時刻的位移增量，αk為k時刻LiDAR相對于k-1時刻的旋轉(zhuǎn)角度。常用的SLAM算法有：EKF-SLAM、無跡卡爾曼濾波(unscented Kalman filter，UKF)SLAM、FAST-SLAM[12]以及粒子濾波(particl filter，PF)SLAM[13]等。本文采用的是EKF-SLAM算法，主要包括位姿預(yù)測、位姿觀測、位姿更新和狀態(tài)擴充4個階段[14]，算法流程圖如圖1所示。

4)LiDAR在行進過程中，會不斷探測到新的特征點，此時需要擴充狀態(tài)估計，加入新的特征信息。

圖1 EKF-SLAM算法流程

本文對LiDAR獲取的原始觀測數(shù)據(jù)，采用EKF-SLAM算法進行數(shù)據(jù)處理。EKF-SLAM算法主要通過相對定位的方式獲取位姿信息，因為每一時刻獲取的位姿信息均存在誤差，所以該方法會隨距離和時間的增大，產(chǎn)生誤差累積，造成定位精度發(fā)散[15]。

2 UWB/LiDAR組合定位算法

由于單獨使用UWB或LiDAR SLAM進行定位均存在一些難以解決的問題，因此本文對2類傳感器進行組合，形成優(yōu)勢互補。組合的主體流程為：首先利用計算機對UWB和LiDAR添加時間標(biāo)簽，進行時間同步；將LiDAR與UWB固定在實驗平臺上，使2類傳感器中心在同一鉛垂線，達到空間同步。然后將UWB測距觀測值、LiDAR SLAM的位移增量和角度觀測值作為量測值，建立了UWB/LiDAR組合定位模型。最后利用EKF進行參數(shù)解算。

2.1 狀態(tài)方程

UWB/LiDAR組合的狀態(tài)方程為

Xk=AXk-1+wk-1

(5)

(6)

2.2 量測方程

LiDAR的量測方程為

(7)

式中：vk,δd、vk,α為觀測噪聲序列；δrk為k時刻LiDAR相對于k-1時刻位置變換的真實距離，且有

(8)

由式(3)、式(4)、式(7)及式(8)可得UWB/LiDAR組合的量測方程為

(9)

(10)

(11)

對式(9)～式(11)進行線性化，可得

(12)

(13)

(14)

因此可得UWB/LiDAR組合定位模型的觀測模型為

Yk=HkXk+vk

(15)

式中：

為觀測向量；

為量測矩陣。

2.3 EKF算法

由文獻[16]可得EKF算法的遞推公式為

(16)

(17)

(18)

(19)

Pk=[I-KkHk]Pk/k-1

(20)

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

3.1 實驗說明

為了驗證UWB/LiDAR組合定位算法，本文在遼寧工程技術(shù)大學(xué)測繪學(xué)院進行了一次實測實驗。在本次實驗中，分別以Time Domain公司的PulsON410模塊和RoboPeak團隊的RPLIDAR模塊作為UWB和LiDAR硬件，表1給出了UWB和LiDAR的精度指標(biāo)。

表1 UWB和LiDAR精度指標(biāo)

首先將UWB流動站和LiDAR安裝在實驗平臺上，分別通過USB接口將數(shù)據(jù)傳送到電腦，實時顯示并保存。然后將4個UWB基站分布在房間四周，接著利用Leica TS09全站儀分別測量真實軌跡的點A、B、C、D坐標(biāo)，UWB基準(zhǔn)站J1、J2、J3、J4坐標(biāo)以及UWB移動站和LiDAR的起始點L1、L2坐標(biāo)，并將全站儀測量坐標(biāo)作為參考真值。實驗開始時，小車從起始點A出發(fā)，按照預(yù)先設(shè)計好的長方形軌跡，逆時針推動行進，軌跡總長度24 m，行進的過程小車共經(jīng)過3個直角彎，最后回到起始點，如圖2所示。

圖2 實驗布局

3.2 實驗與分析

圖3給出了UWB原始數(shù)據(jù)解算的軌跡結(jié)果，以及通過全站儀測量出的參考軌跡。如圖所示，在第AB段，由于行進路上有障礙物遮擋，導(dǎo)致RTT測距信息出現(xiàn)異常，引起NLOS誤差，定位誤差最大達到1.13 m，水平精度為0.16 m；在后三段視距環(huán)境下，觀測環(huán)境較好，UWB解算軌跡接近參考真值，其中水平精度為0.06 m。

圖3 UWB解算軌跡與參考軌跡比較

圖4給出了LiDAR通過EKF-SLAM算法解算的軌跡結(jié)果，以及通過全站儀測量出的參考軌跡。如圖所示，在AB、BC、CD段LiDAR解算軌跡接近參考真值，定位結(jié)果較好，其中水平精度為0.06 m；但是在DA段，由于誤差累積，LiDAR定位結(jié)果發(fā)散，導(dǎo)致出現(xiàn)較大定位誤差，達到0.63 m，且無法閉合。

圖4 LiDAR解算軌跡與參考軌跡比較

圖5給出了通過UWB/LiDAR組合定位算法解算的軌跡結(jié)果，以及通過全站儀測量出的參考軌跡。如圖所示，AB段，在UWB出現(xiàn)NLOS誤差的情況下，UWB/LiDAR組合系統(tǒng)解算的軌跡依舊良好，最大定位誤差達到0.19 m，水平精度為0.08 m；BC、CD段，水平精度為0.05 m；DA

圖5 UWB/LiDAR解算軌跡與參考軌跡比較

段，在LiDAR發(fā)散，定位結(jié)果漂移的情況下，UWB/LiDAR組合系統(tǒng)解算的軌跡依舊良好，最大定位誤差達到0.09 m，水平精度為0.07 m。

表2給出了UWB、LiDAR以及UWB/LiDAR組合系統(tǒng)解算的誤差統(tǒng)計。經(jīng)分析可知，在觀測環(huán)境較好的情況下，UWB、LiDAR以及UWB/LiDAR組合系統(tǒng)都能達到厘米級的定位精度，其中UWB/LiDAR組合系統(tǒng)定位精度略高；在UWB出現(xiàn)NLOS誤差或LiDAR定位結(jié)果發(fā)散的情況下，單獨UWB或LiDAR已經(jīng)無法保證厘米級定位，UWB/LiDAR組合系統(tǒng)仍能提供厘米級的定位精度，滿足室內(nèi)定位的需要。

表2 UWB、LiDAR和UWB/LiDAR組合誤差統(tǒng)計 m

4 結(jié)束語

針對室內(nèi)定位中UWB受非視距影響明顯以及SLAM算法誤差累積的問題，本文提出了UWB/LiDAR組合定位算法，將UWB測距信息、LiDAR SLAM位移增量和角度觀測值作為量測值，推導(dǎo)出UWB/LiDAR組合定位系統(tǒng)的狀態(tài)方程和量測方程，利用EKF進行參數(shù)解算，最后進行實測實驗。實驗結(jié)果表明：UWB/LiDAR組合定位系統(tǒng)能有效抑制SLAM算法誤差累積造成的定位結(jié)果發(fā)散；當(dāng)UWB出現(xiàn)NLOS影響時，UWB/LiDAR組合定位系統(tǒng)仍能提供良好的定位結(jié)果。因此，相對于單一傳感器，UWB/LiDAR組合定位系統(tǒng)更適用于室內(nèi)高精度定位。