帶有NLOS誤差補(bǔ)償?shù)腢WB/INS室內(nèi)融合定位算法研究

唐新明，王鵬，張茂帥

（青島大學(xué)自動(dòng)化學(xué)院，山東青島 266071）

近年來，隨著智能設(shè)備和技術(shù)的普及，人們對各種基于位置服務(wù)(Location Based Services，LBS)的需求日益增加[1]，如自動(dòng)駕駛、應(yīng)急救援、人員管理、智慧工廠、無人超市等。由于建筑物遮擋，全球衛(wèi)星導(dǎo)航系統(tǒng)無法在室內(nèi)環(huán)境中提供可靠的位置服務(wù)[2]，推動(dòng)了Wi-Fi 定位、藍(lán)牙定位、ZigBee 定位以及UWB 定位等[3]室內(nèi)定位技術(shù)的發(fā)展。其中，UWB 技術(shù)具有時(shí)間分辨率高、穿透力強(qiáng)、抗多徑性能好等優(yōu)勢，成為了當(dāng)前室內(nèi)定位領(lǐng)域的一種主要技術(shù)[4]。然而室內(nèi)環(huán)境復(fù)雜多變，UWB 信號(hào)易受人員、物體或墻壁等因素的遮擋造成非視距(Non-Line-of-Sight,NLOS)傳播，導(dǎo)致UWB 定位技術(shù)出現(xiàn)精度低和穩(wěn)定性差等問題[5]。因此，在復(fù)雜環(huán)境下僅僅依靠單一類型的傳感器很難實(shí)現(xiàn)高精度的定位。

INS 是一種獨(dú)立的導(dǎo)航系統(tǒng)，通過慣性測量單元(Inertial Measurement Unit,IMU)來估計(jì)位置。IMU 不依賴外部信息，抗干擾能力強(qiáng)，但長期工作會(huì)產(chǎn)生較大累積誤差[6]。因此，將UWB 和IMU 組合起來進(jìn)行優(yōu)勢互補(bǔ)的融合定位算法被廣泛研究。文獻(xiàn)[7]提出了一種基于四元數(shù)卡爾曼濾波器的UWB/IMU 室內(nèi)融合導(dǎo)航方法，避免了復(fù)雜的線性化，降低了系統(tǒng)的復(fù)雜度。文獻(xiàn)[8]通過機(jī)器學(xué)習(xí)識(shí)別UWB 測距信息中的非視距誤差，然后融合IMU 信息緩解NLOS 對定位精度的影響，缺點(diǎn)是需要保留大量離線數(shù)據(jù)。文獻(xiàn)[9]提出了一種基于迭代卡爾曼濾波的融合定位算法，該算法利用三角不等式原理檢測非視距測量值，在無連續(xù)非視距誤差時(shí)具有較好的定位精度。然而,在實(shí)際室內(nèi)環(huán)境中，經(jīng)常出現(xiàn)多個(gè)UWB 設(shè)備同時(shí)并長時(shí)間處于非視距環(huán)境的情況，導(dǎo)致UWB 定位系統(tǒng)失效，無法校準(zhǔn)IMU 的累積誤差，進(jìn)而影響融合定位系統(tǒng)的定位精度和穩(wěn)定性。

針對以上現(xiàn)狀，該文基于無跡卡爾曼濾波(Unscented Kalman Filter,UKF)提出了一種帶有非視距誤差補(bǔ)償?shù)腢WB/INS 室內(nèi)融合定位算法(UKF with NLOS Error Compensation,EC-UKF)。該算法不需要環(huán)境先驗(yàn)信息，僅使用UWB 測距信息通過殘差檢驗(yàn)法[10]即可識(shí)別非視距誤差；同時(shí)通過構(gòu)造“近似補(bǔ)償誤差”實(shí)現(xiàn)對非視距誤差的高精度補(bǔ)償，從而保證UWB 定位系統(tǒng)的穩(wěn)定運(yùn)行。實(shí)驗(yàn)數(shù)據(jù)表明，所提算法在具有連續(xù)非視距誤差的強(qiáng)NLOS 環(huán)境下依舊能保持較高的定位精度和穩(wěn)定性。

1 系統(tǒng)模型

1.1 基于INS的運(yùn)動(dòng)模型

INS 通過IMU 來估計(jì)位置，其定位原理如圖1所示。

圖1 IMU定位原理示意

該文采用“東-北-天”坐標(biāo)系作為導(dǎo)航坐標(biāo)系，載體坐標(biāo)系采用“右-前-上”坐標(biāo)系，利用位置、速度、四元數(shù)[11]來描述組合定位系統(tǒng)的運(yùn)動(dòng)狀態(tài)。因此，組合定位系統(tǒng)的運(yùn)動(dòng)模型為：

其中，pn為導(dǎo)航坐標(biāo)系中的三維位置向量；vn為導(dǎo)航坐標(biāo)系中的三維速度向量；qbn為方向四元數(shù)；a?b為載體坐標(biāo)系下加速計(jì)測量值；ω?b為載體坐標(biāo)系下陀螺儀測量值；ba和bg分別為加速度計(jì)和陀螺儀偏差向量；na和ng分別為加速度計(jì)和陀螺儀的測量噪聲；δa和δg分別為加速度計(jì)和陀螺儀偏差的隨機(jī)游走噪聲；g為當(dāng)?shù)刂亓铀俣龋?為四元數(shù)乘積運(yùn)算符[12]；為從機(jī)體坐標(biāo)系到導(dǎo)航坐標(biāo)系的旋轉(zhuǎn)矩陣[13]。

1.2 觀測模型

基于UWB 的室內(nèi)行人定位系統(tǒng)由四個(gè)位置已知的UWB 基站、一個(gè)待定位UWB 標(biāo)簽、一個(gè)無線數(shù)據(jù)傳輸通道和一個(gè)上位機(jī)組成，如圖2 所示。

圖2 超寬帶定位系統(tǒng)

為減少基站和標(biāo)簽之間時(shí)鐘不同步以及頻率漂移而產(chǎn)生的測距誤差，采用基于到達(dá)時(shí)間(Time of Arrival，TOA)的雙邊雙向測 距(Double-Sided Two-Way Ranging，DS-TWR)方法[14]測量標(biāo)簽與基站之間的距離。

基于圖2 所示的UWB 定位系統(tǒng)，利用多邊定位算法可以得到組合定位系統(tǒng)的觀測模型為：

其中，D1,k、D2,k、D3,k和D4,k分別為時(shí)刻標(biāo)簽到四個(gè)基站的距離；(px,k,py,k,pz,k)為k時(shí)刻標(biāo)簽的位置；nuwb,1、nuwb,2、nuwb,3和nuwb,4分別為四個(gè)基站的測量噪聲；(xi,yi,zi)為基站坐標(biāo)，i=1,2,3,4。

2 基于EC-UKF的融合算法設(shè)計(jì)

2.1 NLOS誤差識(shí)別與補(bǔ)償

在室內(nèi)行人導(dǎo)航系統(tǒng)中，相鄰時(shí)間的UWB 測距值總是相似的。然而NLOS 會(huì)造成正傳播延遲，導(dǎo)致NLOS 環(huán)境下的測距誤差遠(yuǎn)大于LOS 環(huán)境。通過設(shè)定決策閾值[15]，利用殘差檢驗(yàn)法識(shí)別NLOS 誤差，可以在沒有先驗(yàn)信息的情況下區(qū)分出LOS 和NLOS 數(shù)據(jù)。具體過程如下：

令ΔDj,k=Dj,k-Dj,k-1,j=1,2,3,4為k時(shí)刻第j個(gè)UWB 基站的測距殘差。

則NLOS 識(shí)別方法為:

其中，TH 為決策閾值，大小由標(biāo)簽的移動(dòng)速度和LOS 環(huán)境下UWB 測距的標(biāo)準(zhǔn)方差決定。通常選擇TH=σ+Δs，其中，Δs為標(biāo)簽在測距時(shí)的位移。

為了提高定位系統(tǒng)的定位精度和魯棒性，當(dāng)?shù)趉時(shí)刻的測量值為NLOS 數(shù)據(jù)時(shí)，使用第k-1 時(shí)刻的測量值對其進(jìn)行補(bǔ)償。

在考慮NLOS 誤差補(bǔ)償時(shí)，UWB在k時(shí)刻的測量數(shù)據(jù)可以表示為：

設(shè)Δk為補(bǔ)償NLOS 數(shù)據(jù)后估計(jì)結(jié)果的誤差，其計(jì)算公式如下：

其中，hk(Xk)=[h1,k,h2,k,h3,k,h4,k]T表示第k時(shí)刻UWB 標(biāo)簽和各基站之間的歐氏距離。

對于室內(nèi)行人的實(shí)時(shí)導(dǎo)航與定位，在正常情況下，系統(tǒng)狀態(tài)的變化和誤差Δk通常都是可以忽略的。然而，在NLOS 環(huán)境下誤差Δk將持續(xù)增加，這將影響補(bǔ)償NLOS 數(shù)據(jù)的準(zhǔn)確性。因此，計(jì)算補(bǔ)償NLOS數(shù)據(jù)后估計(jì)結(jié)果的誤差Δk是有必要的。令Xk-1等于k-1 時(shí)刻的后驗(yàn)狀態(tài)估計(jì)值，并且令Xk等于k時(shí)刻的估計(jì)值。的值通常是已知的，但是?k的值未知且無法計(jì)算。因此，考慮使用-1的一步預(yù)測值?k來近似?k，則近似補(bǔ)償誤差Δk計(jì)算如下：

因此，考慮近似補(bǔ)償誤差Δk的NLOS 修正數(shù)據(jù)可以表示為：

2.2 EC-UKF融合定位算法

卡爾曼濾波器最早是處理線性濾波問題的一個(gè)最小方差估計(jì)器。然而實(shí)際生活中許多系統(tǒng)都是非線性系統(tǒng)，由此發(fā)展出了擴(kuò)展卡爾曼濾波、無跡卡爾曼濾波等可以處理非線性濾波問題的卡爾曼濾波器。其中，UKF 通過無跡變換(Unscented Transformation,UT)[16]近似非線性系統(tǒng)狀態(tài)的后驗(yàn)概率密度分布，避免了計(jì)算雅可比矩陣的需要[17]，具有相對較低的計(jì)算開銷和較高的估計(jì)精度。

該文設(shè)計(jì)了基于EC-UKF 的UWB 和IMU 融合定位算法。在濾波更新階段，當(dāng)UWB 系統(tǒng)出現(xiàn)非視距測量時(shí)，通過上述補(bǔ)償算法對NLOS 誤差進(jìn)行補(bǔ)償，以此提高融合定位系統(tǒng)的定位精度和穩(wěn)定性，算法流程如圖3 所示。

圖3 EC-UKF算法流程圖

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

3.1 實(shí)驗(yàn)環(huán)境及實(shí)驗(yàn)裝置

為驗(yàn)證所提算法的有效性，在山東高速西海岸中心B1 層車庫進(jìn)行了實(shí)地實(shí)驗(yàn)。實(shí)驗(yàn)場景如圖4所示。

圖4 實(shí)驗(yàn)環(huán)境

其中，四個(gè)UWB 基站分布在矩形的四個(gè)頂角，分別固定在高2 m 的三角支架上；實(shí)驗(yàn)區(qū)域中間為兩根混凝土承重柱，用來構(gòu)造NLOS 環(huán)境；預(yù)設(shè)運(yùn)動(dòng)軌跡如圖4(b)虛線所示，是一個(gè)長12.9 m、寬4.7 m 的矩形軌跡。

實(shí)驗(yàn)采用Decawave 的UWB 收發(fā)器DW1000 以及意法半導(dǎo)體的六軸慣性測量單元LSM6DSL。實(shí)驗(yàn)中UWB和IMU的采樣頻率分別為20 Hz和100 Hz。

3.2 實(shí)驗(yàn)結(jié)果及分析

圖5 是所提出的EC-UKF 融合定位算法與單獨(dú)使用UWB 或IMU 定位方法的定位結(jié)果對比。

圖5 EC-UKF算法與單一傳感器算法的定位結(jié)果對比

從圖5 可以看出，EC-UKF 融合定位結(jié)果明顯優(yōu)于單一傳感器的定位結(jié)果。在NLOS 環(huán)境下UWB 定位軌跡波動(dòng)較大，且在遮擋嚴(yán)重處出現(xiàn)較大的“毛刺”；受累積誤差的影響，IMU 定位軌跡在實(shí)驗(yàn)開始不久便出現(xiàn)較大偏移，無法進(jìn)行正常定位。

圖6 是EC-UKF、UWB、IMU 三種定位算法的定位誤差累積分布函數(shù)。

圖6 EC-UKF、UWB、IMU三種定位算法的誤差累積分布函數(shù)

由圖6 可知，與單一傳感器定位方法相比，ECUKF 算法的最大誤差和均方根誤差更小，定位精度相較于UWB 定位提高了84.15%，相較于IMU 定位提高了96.16%，具體數(shù)值如表1 所示。

表1 三種定位算法的定位誤差

圖7 是EC-UKF 融合算法、EC-EKF 融合算法與傳統(tǒng)UKF 融合算法的定位結(jié)果對比。

圖7 EC-UKF、EC-EKF與UKF算法的定位結(jié)果對比

從圖7 可以看出，EC-UKF 和EC-EKF 算法定位性能相近，但是所提出的EC-UKF 算法定位軌跡更加平滑，精度更高；傳統(tǒng)UKF 融合算法在弱NLOS 環(huán)境下性能與EC-UKF 相近，而在強(qiáng)NLOS 環(huán)境下定位軌跡出現(xiàn)較大偏差，并且之后即使UWB 通信恢復(fù)正常，但I(xiàn)MU 累積誤差沒有得到校正，導(dǎo)致定位軌跡偏離真實(shí)軌跡。

圖8 是EC-UKF、EC-EKF、UKF 三種定位算法的誤差累積分布函數(shù)。

圖8 EC-UKF、EC-EKF、UKF三種定位算法的誤差累積分布函數(shù)

由圖8 可知，EC-UKF 與EC-EKF 性能相近，但是EC-UKF 的最大誤差與均方根誤差更小，定位精度相較于EC-EKF 提高了9.58%，相較于傳統(tǒng)UKF提高了94.71%，具體數(shù)值如表2 所示。

表2 三種定位算法的定位誤差

4 結(jié)論

針對復(fù)雜室內(nèi)環(huán)境中，UWB 定位精度易受非視距影響導(dǎo)致誤差增大以及INS 定位誤差隨運(yùn)行時(shí)間迅速累積的問題，提出了一種基于EC-UKF 的UWB/INS 室內(nèi)融合定位算法。該算法沒有忽略由障礙物遮擋引起的非視距誤差，而是利用INS 預(yù)測信息計(jì)算出“近似補(bǔ)償誤差”，然后結(jié)合UWB 測距信息估計(jì)和補(bǔ)償非視距誤差，從而緩解非視距環(huán)境對組合系統(tǒng)定位精度的影響。實(shí)驗(yàn)結(jié)果表明，在具有連續(xù)非視距的強(qiáng)NLOS 環(huán)境下，EC-UKF 算法依舊能保持較好的定位精度和穩(wěn)定性。