陳志鍵，徐愛(ài)功，隋 心，郝雨時(shí)，郭 哲

(遼寧工程技術(shù)大學(xué) 測(cè)繪與地理科學(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)可以提供厘米級(jí)的定位精度[2]；但在室內(nèi)環(huán)境下，GNSS信號(hào)高度衰減，已無(wú)法滿(mǎn)足高精度定位需求。

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

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

1 UWB和LiDAR SLAM定位原理

1.1 UWB定位原理

基于往返時(shí)間(round trip time，RTT)測(cè)距是通過(guò)測(cè)量UWB脈沖信號(hào)從流動(dòng)站發(fā)出到達(dá)基準(zhǔn)站之后返回流動(dòng)站的總時(shí)間，間接確定二者間的距離。因?yàn)閭鬏斶^(guò)程是由輸入信號(hào)觸發(fā)，所以RTT測(cè)距不需要基站之間以及基站和流動(dòng)站保持時(shí)間同步[7-8]，但是RTT測(cè)距必須準(zhǔn)確測(cè)量或者估計(jì)標(biāo)準(zhǔn)時(shí)間偏差。

測(cè)距模型[9]可表示為

(1)

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

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

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

(2)

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

UWB平面定位是利用3個(gè)及3個(gè)以上置于固定位置的UWB基站和1個(gè)流動(dòng)站進(jìn)行定位[11]，可得UWB的量測(cè)方程為

(3)

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

(4)

1.2 LiDAR SLAM定位原理

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

4)LiDAR在行進(jìn)過(guò)程中，會(huì)不斷探測(cè)到新的特征點(diǎn)，此時(shí)需要擴(kuò)充狀態(tài)估計(jì)，加入新的特征信息。

圖1 EKF-SLAM算法流程

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

2 UWB/LiDAR組合定位算法

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

2.1 狀態(tài)方程

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

Xk=AXk-1+wk-1

(5)

(6)

2.2 量測(cè)方程

LiDAR的量測(cè)方程為

(7)

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

(8)

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

(9)

(10)

(11)

對(duì)式(9)～式(11)進(jìn)行線性化，可得

(12)

(13)

(14)

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

Yk=HkXk+vk

(15)

式中：

為觀測(cè)向量；

為量測(cè)矩陣。

2.3 EKF算法

由文獻(xiàn)[16]可得EKF算法的遞推公式為

(16)

(17)

(18)

(19)

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

(20)

3 實(shí)驗(yàn)與結(jié)果分析

3.1 實(shí)驗(yàn)說(shuō)明

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

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

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

圖2 實(shí)驗(yàn)布局

3.2 實(shí)驗(yàn)與分析

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

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

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

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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