基于魯棒無跡卡爾曼濾波的無線室內(nèi)定位算法

高端陽，李 安，傅 軍，周 鑫

（1. 海軍工程大學(xué) 導(dǎo)航工程系，武漢 430033；2. 海軍工程大學(xué)，武漢 430033；3. 92571部隊，三亞 572000）

基于位置服務(wù)（Location-Based Services,LBS）的快速發(fā)展,一方面給人們的日常生活帶來了極大便利，同時也促進了萬物萬聯(lián)、人工智能、應(yīng)急反恐等應(yīng)用領(lǐng)域的快速發(fā)展。正是由于LBS可帶來諸多好處，不管是人們的出行還是各行各業(yè)都對 LBS提出了更高的要求。根據(jù)美國環(huán)境保護局以及諾基亞的統(tǒng)計報告，人們有70%至90%的時間是在室內(nèi)活動，70%的移動電話使用和80%的數(shù)據(jù)連接使用均發(fā)生于室內(nèi)，因此人們迫切的需要將基于位置服務(wù)拓展到室內(nèi)。由于室內(nèi)環(huán)境復(fù)雜多變，無線電信號易受遮擋，產(chǎn)生非視距（Non light-of-sight,NLOS）傳播和多路徑效應(yīng)，近年來，室內(nèi)定位已成為廣大學(xué)者研究的熱點[1-2]。

超寬帶（Ultra Wideband,UWB）是一種無載波通信技術(shù)，是當(dāng)前短距離定位領(lǐng)域的研究熱點之一[3-5]。在室內(nèi)定位中，UWB可達厘米級定位精度，并具有較好的抗多路徑性能以及一定的穿透能力[6-7]。為減小NLOS及多路徑的影響，提高定位精度，文獻[8]使用相關(guān)向量機技術(shù)來識別和減輕NLOS影響，提出了一種基于變分貝葉斯推理的分布式協(xié)作算法，相比于常用的基于到達時間（Time of Arrival,TOA）定位方法，該方法雖然可實現(xiàn)較高的定位精度，但是復(fù)雜度要高得多。文獻[9]對 UWB基于往返時間（Round Trip Time,RTT）測量誤差特性進行研究，分析RTT測量可能存在非高斯噪聲的影響，并驗證了采用粒子濾波算法處理的優(yōu)越性，但是存在計算量大的問題。文獻[10]提出了采用基于UWB測距信息和時延定位模型的擴展有限脈沖響應(yīng)估計器實現(xiàn)目標(biāo)位置估計，但是其精度取決于平均水平的設(shè)定。文獻[11]提出了一種擴展卡爾曼濾波器（Extended Kalman Filter,EKF）超寬帶室內(nèi)定位方法，在復(fù)雜室內(nèi)環(huán)境下定位精度可達亞米級。但是量測方程具有較強的非線性，EKF線性化誤差較大，存在濾波發(fā)散的問題，同時EKF濾波算法雅克比矩陣求取復(fù)雜，計算量較大，不利于實際使用。

為了進一步提高UWB系統(tǒng)定位精度，本文提出了一種魯棒無跡卡爾曼濾波定位算法。該方法根據(jù) UWB系統(tǒng)定位誤差特性以及結(jié)合室內(nèi)環(huán)境實際情況，引入代價函數(shù)，修正觀測方差，建立魯棒機制，降低UKF算法對觀測噪聲特性分布的要求，增強 UWB系統(tǒng)的環(huán)境適應(yīng)能力。設(shè)計相關(guān)實驗，采用實測數(shù)據(jù)對所提出的算法進行驗證，結(jié)果表明，提出的魯棒UKF性能均優(yōu)于UKF、EKF和最小二乘算法，UWB系統(tǒng)定位精度提升明顯。

1 UWB定位實現(xiàn)

常用的無線定位算法主要有四種，分別是基于TOA、基于到達時間差（Difference of Time Arrival,TDOA）、基于到達角度（Angel of Arrival,AOA）和基于接收信號強度（Received Signal Strength Indication,RSSI）。UWB 是一種無載波通信技術(shù)，利用納秒的非正弦波窄脈沖傳輸數(shù)據(jù)，可測量精準(zhǔn)的信號傳輸時間，即可精確測量基站到標(biāo)簽的距離，因此UWB通常采用基于TOA的定位算法。

以平面定位為例，假設(shè)各個基站的坐標(biāo)分別為BSi(xi,yi)，定位標(biāo)簽坐標(biāo)設(shè)為Tag(x,y)，基站BSi到標(biāo)簽Tag的測量距離為ri，分別以基站BSi為圓心，測量距離ri為半徑，作圓位置線，由于測距誤差的存在，定位模型如圖1所示。

圖1 基于TOA的幾何定位模型Fig.1 Geometric positioning model based on TOA

基站的個數(shù)通常不小于3個，由幾何定位模型可得：

該方程為非線性方程組，常采用最小二乘法求解（Least Square,LS），即可求得標(biāo)簽Tag的位置。

2 標(biāo)準(zhǔn)UKF算法

在EKF算法的核心是對非線性的狀態(tài)方程和觀測方程進行泰勒展開，通過取其一階項實現(xiàn)方程的線性化。若方程的非線性較強，這樣就會造成較大的線性化誤差，最終導(dǎo)致濾波器的性能下降甚至濾波發(fā)散。UKF算法舍棄了EKF算法的線性化過程，采用無跡變換（Unscented Transform,UT）巧妙的避免了線性化所帶來的誤差，同時減少了算法的復(fù)雜度，克服了EKF算法的估計精度低、穩(wěn)定性差的缺陷，組合導(dǎo)航中被廣泛應(yīng)用[12]。

假設(shè)UWB基站個數(shù)為m，以UWB測距信息作為觀測量Zk=[r1r2rm]T，狀態(tài)量為目標(biāo)的位置和速度Xk=[xyzvxvyvz]T。對于不同時刻k，系統(tǒng)噪聲為高斯白噪聲wk～N(0,Qk)，量測噪聲為高斯白噪聲vk～N(0,Rk)，構(gòu)成UWB定位的非線性系統(tǒng)可以由式(2)表示：

UKF傳播流程首先求解誤差協(xié)方差矩陣的均方根Sk+-1，采用Cholesky分解來求解：

由式(4)計算sigma點：

傳播過后的狀態(tài)估計及其誤差協(xié)方差為

UKF的觀測更新流程由式(8)產(chǎn)生新的sigma點：

sigma點和平均觀測新息可由式(9)解算：

則觀測新息的協(xié)方差為：

最后，UKF卡爾曼增益、狀態(tài)向量更新和誤差協(xié)方差更新為

當(dāng)系統(tǒng)噪聲和量測噪聲均為高斯白噪聲時，通過上述標(biāo)準(zhǔn) UKF定位算法可實現(xiàn)目標(biāo)位置和速度的最優(yōu)估計。

3 基于觀測方差修正的魯棒UKF

無論是 EKF還是 UKF，都是基于最小l2范數(shù)原則?；谧钚2范數(shù)的估計相比較于其他估計方法有著無法比擬的優(yōu)勢，但是基于最小l2范數(shù)的估計不具有魯棒性，要求噪聲的統(tǒng)計特性為高斯分布，且當(dāng)假設(shè)條件和現(xiàn)實參數(shù)不一致時，狀態(tài)估計精度下降。1964年Huber提出了廣義極大似然估計，即M估計，同時并給出了解決一類在高斯分布附近存在對稱干擾問題的魯棒處理方法，即 Huber方法，該方法中，Huber用一種基于l1/l2的混合代價函數(shù)（Huber 代價函數(shù)）取代基于l2的代價函數(shù)以解決干擾高斯分布的問題。后續(xù)學(xué)者將Huber代價函數(shù)引入到卡爾曼濾波中以提高其魯棒性。結(jié)合文獻[13]，本文采用一種新的魯棒UKF（RUKF）算法實現(xiàn)室內(nèi)無線定位系統(tǒng)的位置估計。

基于式(2)非線性空間模型，構(gòu)建非線性回歸模型如下：

其中，δXk|k-1為狀態(tài)真值與預(yù)測值之差。用εk表示式(14)右側(cè)第二部分，則：

對式(14)兩邊分別右乘Sk-1，則：

其中，

根據(jù)廣義極大似然理論，式(16)中的回歸問題可最小化如下代價函數(shù)求解：

其中ek=yk-g(Xk)。令ψ(ek,i)=φ(ek,i) /ek,i，則：

其中，sgn(·)表示符號函數(shù)。令Ψk=diag[ψ(ek,i)]。對式(15)中的方差進行修正：

將修正的噪聲方差替換標(biāo)準(zhǔn) UKF中的噪聲方差即為基于觀測修正的魯棒UKF。

4 實驗驗證

為了充分驗證所提方法的合理性，采用一種常用的UWB定位系統(tǒng)進行實驗，輸出頻率為50 Hz，最大定位距離可達35 m。實驗場景如圖2所示。

圖2 實驗場景Fig.2 Experimental scene

4.1 測距誤差分布檢驗

目標(biāo)靜止情況下采集 1 min測距數(shù)據(jù)，首先對UWB系統(tǒng)目標(biāo)測距誤差分布特性進行檢驗，檢驗結(jié)果如圖3～6所示。

圖3 基站1測距誤差分布Fig.3 Ranging error distribution test of base station 1

圖4 基站2測距誤差分布Fig.4 Ranging error distribution test of base station 2

圖5 基站3測距誤差分布Fig.5 Ranging error distribution test of base station 3

圖6 基站4測距誤差分布Fig.6 Ranging error distribution test of base station 4

觀察圖3～6以及根據(jù) MATLAB單樣本分布的Kolmogorov-Smirnov檢驗結(jié)果，如果測試在15%顯著水平上拒絕零假設(shè)，標(biāo)簽到4個基站的測距誤差均為非正態(tài)分布。因此可知，標(biāo)簽的測距誤差均為非高斯噪聲。

4.2 靜態(tài)實驗

為驗證本文提出的魯棒UKF定位算法的合理性，在定位區(qū)域內(nèi)任選9個參考點，依次將定位標(biāo)簽放置于參考點位置1 min，采集測距數(shù)據(jù)，并分別采用LS、EKF、UKF、和RUKF不同的定位算法進行解算，計算每個參考點的均方根誤差（Root Mean Square Error,RMSE），計算結(jié)果分別如圖7和表1所示。

圖7 不同定位算法定位精度對比Fig.7 Comparison on static positioning accuracies of different location algorithms

表1 不同定位算法定位精度對比/mTab.1 Comparison on positioning accuracies of different location algorithms/m

從圖7及表1可知，采用LS定位算法，各參考點RMSE最大值為0.60 m，最小值為0.26 m，均值為0.39 m；采用EKF定位算法，各參考點RMSE最大值為0.55 m，最小值為0.33 m，均值為0.41 m；采用UKF定位算法，各參考點RMSE最大值為0.50 m，最小值為0.29 m，均值為0.37 m；而采用本文提出的RUKF定位算法，各參考點RMSE最大值為0.45 m，最小值為0.24 m，均值為0.33 m。

4.3 動態(tài)實驗

為進一步驗證本文提出的魯棒UKF定位算法的動態(tài)性能，采集運動軌跡為矩形的一組數(shù)據(jù)，分別采用 LS、EKF、UKF和RUKF算法進行處理，處理結(jié)果如圖8～10所示。

觀察圖8～10可知，RUKF相比較于LS、EKF和UKF具有更強的魯棒性，尤其是當(dāng)觀測值出現(xiàn)較大誤差時，RUKF所表現(xiàn)出的優(yōu)越性更加明顯。因此，RUKF在動態(tài)定位性能上均優(yōu)于其它3種算法。

圖8 不同定位算法動態(tài)定位對比Fig.8 Dynamic positioning comparison of different location algorithms

圖9 x向誤差比較Fig.9 Comparison of x axis errors

圖10 y向誤差比較Fig.10 Comparison of y axis errors

綜合圖7～10和表1，將EKF或UKF與LS對比可知，EKF和UKF相比較于LS定位算法并沒有絕對的優(yōu)越性，在某些參考點位置 LS定位算法的定位精度高于 EKF和UKF；將UKF與EKF對比可知，由于EKF線性化誤差的存在，UKF定位算法精度優(yōu)于EKF，但是觀測方程的非線性程度不高，UKF定位精度提升并不是十分的明顯；將RUKF與其他定位算法對比可知，由于引入了魯棒機制，RUKF對觀測噪聲分布特性要求降低，能夠?qū)τ^測數(shù)據(jù)更好的進行濾波處理，定位精度有了明顯的改善，尤其是當(dāng)其他算法定位誤差較大時，采用RUKF定位算法精度提升更加明顯，因此RUKF定位算法相比較于其他算法具有更強的魯棒性，環(huán)境適應(yīng)能力更強。

5 結(jié) 論

本文對 UWB室內(nèi)定位算法進行了研究，提出了一種魯棒UKF定位算法，通過引入代價函數(shù)，修正觀測方差，建立魯棒機制，以提高UWB在實測環(huán)境下的定位精度。實驗結(jié)果表明：EKF和UKF算法在某些參考點定位性能優(yōu)于LS算法，但也存在某些參考點LS算法高于EKF和UKF算法定位精度的現(xiàn)象，因此采用標(biāo)準(zhǔn)的EKF或UKF定位算法不具有普適性；本文提出的魯棒UKF算法，不再要求噪聲的統(tǒng)計特性為高斯分布，改進了標(biāo)準(zhǔn)UKF在實測環(huán)境下存在的問題，能夠?qū)崟r自適應(yīng)的對觀測噪聲進行修正，定位精度均優(yōu)于LS、EKF和UKF定位算法，平均定位精度可達0.33 m，與LS定位算法相比，定位精度提高了15%。

本文提出的一種新的自適應(yīng)UKF算法，考慮了UWB實測環(huán)境下誤差特性，實現(xiàn)簡單，計算量小，具有較強的實用性，是一種高精度的室內(nèi)定位方法。