基于卡爾曼濾波和粒子濾波融合的UWB室內(nèi)定位算法

程雪聰， 劉福才， 黃茹楠

(1.燕山大學(xué) 智能控制系統(tǒng)與智能裝備教育部工程研究中心,河北 秦皇島 066004；2.燕山大學(xué) 工業(yè)計(jì)算機(jī)控制工程河北省重點(diǎn)實(shí)驗(yàn)室,河北 秦皇島 066004)

1 引 言

隨著無線通信技術(shù)的發(fā)展，室內(nèi)定位技術(shù)得到了廣泛的應(yīng)用[1～5]，如倉儲機(jī)器人的定位、機(jī)場中人員的定位等；然而在建筑物中，全球?qū)Ш蕉ㄎ幌到y(tǒng)的衛(wèi)星信號在穿透建筑的過程中快速衰減，導(dǎo)致其定位精度降低，甚至無法實(shí)現(xiàn)定位，而超寬帶(ultra-wideband，UWB)定位技術(shù)憑借精度高、成本低成為了近年來被廣泛應(yīng)用的室內(nèi)定位技術(shù)。

為了得到位置信息，通常使用到達(dá)時(shí)間差(time different of arrival，TDOA)的方法，該方法降低了室內(nèi)定位系統(tǒng)對于系統(tǒng)時(shí)鐘的要求，同時(shí)提高了定位精度[6,7]?；趨f(xié)同定位的改進(jìn)算法，是將雙邊兩路測距(double-sided two-way ranging，DS-TWR)和TDOA結(jié)合，基于最小二乘和Taylor級數(shù)展開進(jìn)一步實(shí)現(xiàn)室內(nèi)條件下的精準(zhǔn)定位[8～11]。在LOS(line-of-sight)環(huán)境中UWB的定位精度可以達(dá)到厘米級別；但在某些特殊環(huán)境下，例如倉儲機(jī)器人所在的倉庫，UWB信號會被人、貨物或其他障礙物阻擋，因此產(chǎn)生NLOS (non-line-of-sight)誤差，最終會導(dǎo)致UWB的定位精度下降[12]。在室內(nèi)傳統(tǒng)定位算法基礎(chǔ)上，利用小波去噪消除NLOS誤差，從而改進(jìn)到達(dá)時(shí)間差定位算法[12～14]。利用卡爾曼濾波算法分別對UWB系統(tǒng)的測距信息和定位結(jié)果進(jìn)行濾波處理，有效減小了測距誤差，提高了定位的準(zhǔn)確性[15]。

基于卡爾曼濾波的UWB室內(nèi)三維實(shí)時(shí)動態(tài)定位算法，提出了一種不同觀測條件下的自適應(yīng)動態(tài)定位方法[16]，進(jìn)一步提高了定位精度。采用互補(bǔ)卡爾曼濾波對UWB和慣性測量單元(inertial measurement unit,IMU)進(jìn)行數(shù)據(jù)融合的定位方法，該算法減小了IMU和UWB的數(shù)據(jù)噪聲，從而提高了定位精度[17]。通過粒子濾波的方式融合IMU和UWB的數(shù)據(jù)，避免了定位軌跡突變現(xiàn)象的出現(xiàn),并且提高了最終的平均定位精度[18]。采用基于2次粒子濾波的定位算法可以有效抑制NLOS誤差的影響[19]。擴(kuò)展卡爾曼濾波和粒子濾波融合的定位算法，在NLOS環(huán)境中可以減小錯誤數(shù)據(jù)對定位結(jié)果的影響[20]。實(shí)驗(yàn)結(jié)果表明，約束魯棒迭代擴(kuò)展卡爾曼濾波算法在NLOS環(huán)境中平均定位精度可以達(dá)到0.21 m左右[21]。采用機(jī)器學(xué)習(xí)技術(shù)來區(qū)分在LOS環(huán)境下和在NLOS環(huán)境下的UWB測量值，可以有效提高定位精度[22]。

本文使用3基站和1標(biāo)簽的UWB定位系統(tǒng)，提出卡爾曼濾波(Kalman filters and particle filters,KPF)和粒子濾波融合的算法,分別給出了在LOS環(huán)境下和在NLOS環(huán)境下基于KPF融合的定位算法,并與傳統(tǒng)卡爾曼濾波、傳統(tǒng)粒子濾波進(jìn)行了比較。

2 UWB測距數(shù)據(jù)分析

為了分析LOS和NLOS環(huán)境下的UWB原始數(shù)據(jù)，在鋪有0.5 m×0.5 m的瓷磚上進(jìn)行了相關(guān)實(shí)驗(yàn)，可以明確實(shí)驗(yàn)中真實(shí)的運(yùn)動軌跡。在實(shí)驗(yàn)過程中，通過遮擋標(biāo)簽T的天線來模擬NLOS環(huán)境。UWB定位系統(tǒng)中基站和標(biāo)簽中使用的核心芯片為DecaWave的DW1000芯片。

2.1 LOS環(huán)境下的UWB數(shù)據(jù)誤差分析

如圖1所示，記錄了標(biāo)簽T與基站Ai之間的距離，i=0,1,2，并在表1中記錄相關(guān)誤差，實(shí)驗(yàn)結(jié)果表明，在靜態(tài)的LOS環(huán)境中，UWB定位系統(tǒng)的測距能力較為穩(wěn)定，測距誤差均保持在厘米級。

圖1 LOS環(huán)境下的測距結(jié)果Fig.1 Ranging results in LOS environment

表1 LOS環(huán)境下基站與標(biāo)簽的距離誤差Tab.1 Distance error between base station and tag in LOS environment mm

2.2 NLOS環(huán)境下的UWB數(shù)據(jù)誤差分析

如圖2所示，在保持標(biāo)簽T不動的情況下，通過遮擋標(biāo)簽T的天線會使UWB的測距信息產(chǎn)生較大誤差，如表2所示，在NLOS環(huán)境下產(chǎn)生的誤差最高可達(dá)692 mm，這將嚴(yán)重影響UWB的定位精度。

圖2 NLOS環(huán)境下的測距結(jié)果Fig.2 Ranging results in NLOS environment

表2 NLOS環(huán)境下基站與標(biāo)簽的距離誤差Tab.2 Distance error between base station and tag in NLOS environment mm

2.3 NLOS數(shù)據(jù)處理

對于在NLOS環(huán)境中獲取的數(shù)據(jù)，本文采用改進(jìn)卡爾曼濾波(improved Kalman filters,IKF)算法來消除NLOS誤差。如圖3所示，經(jīng)過過濾的UWB數(shù)據(jù)變得相對更加平滑。

圖3 NLOS環(huán)境下的測距濾波結(jié)果Fig.3 Ranging filtering results in NLOS environment

具體算法為：系統(tǒng)的狀態(tài)方程和觀測方程分別為式(1)和式(2)，與傳統(tǒng)卡爾曼濾波(Kalman filters，KF)相比，該算法求出了觀測值與預(yù)測值的差en，并設(shè)定一個(gè)閾值h，通過比較|en|和h的大小來確定狀態(tài)更新的值。

xn=Fnxn-1+vn

(1)

zn=Cnxn+wn

(2)

Step1：預(yù)測

狀態(tài)預(yù)測：

xn|n-1=Fn|n-1xn-1+Q

(3)

誤差協(xié)方差預(yù)測：

Pn|n-1=Fn|n-1Pn-1Fn|n-1+Q

(4)

觀測值與預(yù)測值的差：

en=zn-Cxn|n-1

(5)

Step2：更新

卡爾曼增益：

An=CnPn|n-1CT+R

(6)

(7)

誤差協(xié)方差更新：

Pn=(I-KnC)Pn|n-1

(8)

狀態(tài)更新：

xn=xn|n-1+Knen

(9)

式中：vn為滿足均值為0，協(xié)方差矩陣為Q的過程噪聲；wn為滿足均值為0，協(xié)方差矩陣為R的觀測噪聲；P為誤差協(xié)方差；K為卡爾曼增益。

3 卡爾曼濾波和粒子濾波融合算法

現(xiàn)有的濾波處理算法有卡爾曼濾波、擴(kuò)展卡爾曼濾波、粒子濾波等，這些算法在狀態(tài)空間模型下給出了未知參數(shù)估計(jì)的有效解決方案。其中粒子濾波適用于非線性、非高斯分布，而且應(yīng)用靈活、易于實(shí)施；其主要思想是通過蒙特卡洛采樣模擬的方法，首先應(yīng)用序貫重要性采樣算法尋找一組在狀態(tài)空間傳播的隨機(jī)樣本近似地表示概率密度函數(shù)，然后用樣本的均值代替積分運(yùn)算，隨著觀測值的更新，不斷調(diào)整粒子的權(quán)重和位置，并修正更新先驗(yàn)分布，最后得到目標(biāo)函數(shù)的后驗(yàn)概率分布。該算法主要包括初始化、重要性采樣、重采樣3步。具體算法為：

Step1：初始化

Step2：重要性采樣

對于i=1，2，…，N，采用高斯概率密度函數(shù)計(jì)算粒子的權(quán)值并歸一化權(quán)值：

(10)

(11)

Step3：重采樣

(12)

(13)

傳統(tǒng)粒子濾波僅采用運(yùn)動學(xué)模型而不利用觀測信息進(jìn)行隨機(jī)采樣，只有在定義粒子權(quán)重時(shí)才會用到觀測信息，所以標(biāo)準(zhǔn)粒子濾波的粒子精度低，易出現(xiàn)粒子退化的現(xiàn)象。

基于上述分析，在重要性采樣的過程中，應(yīng)使采樣的粒子盡可能地滿足真實(shí)狀態(tài)的概率分布，以提高粒子濾波的性能?；诖?，本文提出了基于KPF的融合的算法，其基本思想和流程如圖4所示。該算法通過卡爾曼濾波進(jìn)行狀態(tài)估計(jì)和誤差協(xié)方差的更新，通過估計(jì)的狀態(tài)和誤差協(xié)方差進(jìn)行粒子采樣，使采樣粒子盡可能地與真實(shí)狀態(tài)一致。

圖4 KPF算法流程圖Fig.4 KPF algorithm flow chart

4 實(shí)驗(yàn)分析

實(shí)驗(yàn)設(shè)計(jì)了2條路線，路線1為L型折線，路線2為矩形，每次實(shí)驗(yàn)分別在每條路線上走2個(gè)來回。算法的定位測試共設(shè)計(jì)了2種方案，一種是在LOS環(huán)境下，另一種是在NLOS環(huán)境下，通過計(jì)算定位誤差來評估算法的性能和可靠性。

4.1 LOS環(huán)境下的定位路線分析

圖5展示了基于各種算法的定位路徑。基于UWB原始數(shù)據(jù)的定位結(jié)果用綠色路徑表示，由于UWB數(shù)據(jù)中存在大量的噪聲，基于該算法的路線1定位結(jié)果與實(shí)際路線的平均誤差為32.9 mm，最大誤差為147.7 mm，；路線2定位結(jié)果與實(shí)際路線的平均誤差為30.5 mm，最大誤差為175.9 mm?；赑F算法的定位結(jié)果中用黃色路徑表示，該算法可以根據(jù)UWB數(shù)據(jù)來校正當(dāng)前的定位結(jié)果，使用該算法，路線1和路線2的平均誤差分別為31.5 mm和29.4 mm?；贙PF的融合定位算法的定位結(jié)果用藍(lán)色路徑表示，可以在實(shí)驗(yàn)中看到，該算法在路線1中的平均誤差為22.1 mm，在路線2中的平均誤差為25.8 mm，相對于使用UWB原始數(shù)據(jù)進(jìn)行定位，定位精度平均提高了20.6%。

圖5 LOS環(huán)境下各算法定位路徑Fig.5 Algorithm positioning path under LOS environment

4.2 NLOS環(huán)境下的定位路線分析

圖6顯示了受到NLOS環(huán)境干擾時(shí)基站與標(biāo)簽之間的距離數(shù)據(jù)，在圖中可以明顯看到，當(dāng)定位基站和定位標(biāo)簽處于NLOS環(huán)境中則數(shù)據(jù)會產(chǎn)生NLOS誤差，此時(shí)UWB定位系統(tǒng)中得到的距離數(shù)據(jù)會產(chǎn)生突變，進(jìn)而導(dǎo)致定位產(chǎn)生偏差。如圖6所示，含有NLOS誤差的距離數(shù)據(jù)在通過IKF濾波后，基本可以去除NLOS環(huán)境導(dǎo)致的誤差，使距離數(shù)據(jù)更加平滑、穩(wěn)定。

圖6 NLOS環(huán)境下2條路線濾波后的UWB測距信息Fig.6 Filtered UWB ranging information of two routesin NLOS environment

圖7展示了在NLOS環(huán)境下基于各種算法的定位路徑，其中綠色表示UWB數(shù)據(jù)的定位路徑，雖然經(jīng)過IKF濾波已經(jīng)消除了大部分NLOS誤差，但最大定位誤差依然達(dá)到了196.2 mm；黃色表示經(jīng)過PF濾波的定位路徑，該算法可以適當(dāng)矯正誤差，但矯正能力十分有限，平均定位誤差為34.9 mm；KPF算法的定位路徑用藍(lán)色表示，該算法可以充分利用KF中的狀態(tài)更新量和誤差協(xié)方差，該方法的定位結(jié)果相對較平滑，平均誤差為31.1 mm。

圖7 NLOS環(huán)境下2條路線各算法定位路徑Fig.7 Positioning paths of each algorithm in two routesunder NLOS environment

5 結(jié) 論

本文提出了一種卡爾曼濾波和粒子濾波融合的定位算法，該算法通過KF更新的狀態(tài)和誤差協(xié)方差進(jìn)行粒子采樣，克服了傳統(tǒng)粒子濾波采樣粒子和真實(shí)狀態(tài)不相符的缺點(diǎn)，大幅減少了粒子退化的現(xiàn)象，充分提高了UWB定位系統(tǒng)的定位精度。由實(shí)驗(yàn)可以看出該算法在LOS環(huán)境和NLOS環(huán)境下均有較好的定位精度。后續(xù)將針對更復(fù)雜的應(yīng)用需求，繼續(xù)研究基于IMU/UWB的定位系統(tǒng)，通過多傳感器的數(shù)據(jù)融合進(jìn)一步提高定位系統(tǒng)的精度和魯棒性。