程雪聰， 劉福才， 黃茹楠

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

1 引 言

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

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

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

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

2 UWB測距數據分析

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

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

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

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

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

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

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

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

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

2.3 NLOS數據處理

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

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

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

xn=Fnxn-1+vn

(1)

zn=Cnxn+wn

(2)

Step1：預測

狀態(tài)預測：

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

(3)

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

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

(4)

觀測值與預測值的差：

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 卡爾曼濾波和粒子濾波融合算法

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

Step1：初始化

Step2：重要性采樣

對于i=1，2，…，N，采用高斯概率密度函數計算粒子的權值并歸一化權值：

(10)

(11)

Step3：重采樣

(12)

(13)

傳統(tǒng)粒子濾波僅采用運動學模型而不利用觀測信息進行隨機采樣，只有在定義粒子權重時才會用到觀測信息，所以標準粒子濾波的粒子精度低，易出現粒子退化的現象。

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

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

4 實驗分析

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

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

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

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

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

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

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

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

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

5 結 論

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