室內UWB/LiDAR組合定位算法

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

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

0 引言

如今基于位置的服務(location based service，LBS)已逐漸成為人們日常生活和工作中必不可少的服務需求[1]。在室外良好的環境下，利用全球衛星導航系統(global navigation satellite system，GNSS)與慣性導航系統(inertial navigation system，INS)構成的組合定位系統可以提供厘米級的定位精度[2]；但在室內環境下，GNSS信號高度衰減，已無法滿足高精度定位需求。

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

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

1 UWB和LiDAR SLAM定位原理

1.1 UWB定位原理

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

測距模型[9]可表示為

(1)

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

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

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

(2)

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

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

(3)

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

(4)

1.2 LiDAR SLAM定位原理

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

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

圖1 EKF-SLAM算法流程

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

2 UWB/LiDAR組合定位算法

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

2.1 狀態方程

UWB/LiDAR組合的狀態方程為

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 實驗與結果分析

3.1 實驗說明

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

表1 UWB和LiDAR精度指標

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

圖2 實驗布局

3.2 實驗與分析

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

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

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

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

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

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

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

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

表2 UWB、LiDAR和UWB/LiDAR組合誤差統計 m

4 結束語

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