基于UKF的環境自適應UWB/DR室內定位方法①

周 琳,李曉明,江先志

(浙江理工大學 機械與自動控制學院,杭州 310018)

隨著無線定位技術的不斷發展,基于位置服務的研究越來越受到了人們的關注[1].目前,全球定位系統(Global Positioning System,GPS)已被廣泛應用于室外定位[2],但由于墻體等建筑物對衛星信號遮擋,其在室內環境下定位效果較差.因此,室內定位技術不斷涌現,比如Zigbee、UWB、WIFI、RFID 等定位技術[3].其中UWB 是一種穿透力強、抗干擾能力強的無線定位技術,比WIFI、ZigBee、RFID 等室內無線定位技術有著更高的定位精度,在視距(los of sight,LOS)情況下可達厘米級別定位精度且不存在誤差累積,然而在復雜的室內環境中,UWB 信號在傳播過程中不可避免地產生多徑效應,從而導致明顯的NLOS 定位誤差[4–8].而DR 是一種具有自主定位導航能力并且短時定位高精度的技術,但其定位結果將會隨著時間的推移發生飄移,即存在定位誤差累積的問題[9].

針對上述問題,本文提出了一種基于UKF 的環境自適應UWB/DR 室內定位方法,將UWB 定位和DR定位的優勢聯合,用DR 定位降低UWB 定位出現的NLOS 誤差,用UWB 定位更新DR 定位的當前位置,減少DR 定位的累積誤差.該方法通過建立自適應UKF 濾波模型,將UWB 定位信息和DR 定位信息進行融合.在此過程中,利用新息和高斯分布的 3σ原則來對UWB 定位結果進行非視距檢測,再通過新息的實時估計協方差和理論協方差來構建環境適應系數,進而用此系數動態修正UWB 定位的觀測噪聲,使得觀測噪聲自適應真實環境,更大程度地降低NLOS 誤差對融合定位結果的影響.為了驗證本文定位方法的有效性和穩定性,設計差速移動機器人在多遮擋室內環境中進行折線運動實驗并實時采集和處理數據.實驗效果表明,該方法能夠有效減小UWB 定位的NLOS誤差和DR 定位的累積誤差,并且由于環境適應系數的創新引入,比UKF 定位方法和PF 定位方法具有更高的定位精度和更強的抗NLOS 誤差性能.

1 DR 定位

DR 定位是一種精度較高的自主式定位方法,尤其適用于短時短距定位.其核心思想可概述為根據當前時刻的位置、速度和航向推算下一時刻的位置.航位推算的實現原理如圖1所示.

已知當前時刻的初始位置 (xk,yk)、航向角θk和速度vk,移動機器人在下一時刻移動到(xk+1,yk+1)時,航向角的變化為 ωk,根據當前的速度vk乘以間隔時間tk+1?tk得到的航行距離lk,從而tk+1時刻的位置和航向角可以表示為:

圖1 航位推算原理圖

采用的數據采集移動平臺如圖2所示.該移動平臺采用差速驅動,左右兩個為直流電機驅動的主動輪,前后兩個為用于支撐的萬向輪.本文通過安裝在電機軸端的光電編碼器輸出來進行航位推算.

設車輪的直徑為D,兩車輪間距為L,左右兩個驅動輪的轉速分別為ωl,ωr,根據差速移動機器人的運動學關系[10]可知速度和航向變化的計算表達式為:

圖2 數據采集移動平臺圖

因此k時刻根據的位置和航向推得的下一時刻的位置和航向可表達為:

其中,ωRk,ωLk分 別表示左右驅動輪ti時刻的轉速,由光電編碼器測得.

2 基于TOA 的UWB 定位

到達時間方法(Time Of Arrive,TOA)的核心在于精確地測量出信號從發射機到達接收機的飛行時間.已知信號的傳播速度即可精確地計算出發送機和接收機的之間的距離,但是發射機和接收機采用不同的時鐘源,即兩者存在影響飛行時間測量值的時鐘差,又因為信號傳播速度非?？?TOA 所測距離準確度難以保證.因此采用DS 測距(Double-Sided two-way ranging)消除時鐘差影響,其原理圖如圖3所示.

圖3 DS 測距原理圖

圖3中,Tc1,Tc2分別表示設備1 和設備2 發出信號到收到回應信號所耗費的時間,Tr1,Tr2則分別表示設備1 和設備2 處理收到的信號所耗費的時間,c為光速,從而可得待測節點到四周各個基站的距離為:

該方法等效于分別用設備發射機和接收機的時鐘計算一次來回路程花費的時間,再四均等分總時間求取測量時間.這可以有效減少不同設備之間的時鐘差導致的飛行時間誤差,極大提高了TOA 的精度.

本文所采用的UWB 定位系統的原理如圖4所示,移動站安裝在數據采集移動平臺上,定位系統的基準錨點為四周的4 個定位基站,基站基于TOA 方法實時獲取其到移動站的距離.

令UWB 移動站(Moving Station,MS)和基站(Base Station,BS)的位置分別為(x,y)和(xi,yi),MS 和BS 的距離為di,因此可得方程組為:

圖4 UWB 定位系統原理圖

用該方程組的最后一個方程減去前n–1 個方程即可得方程組如下:

然而在實際情況下,通過測量所得d1,d2···di都是存在誤差的,因此使用最小二乘法去估計待測節點的位置,則可以得到式(6)的解為:

3 基于UKF 的環境自適應UWB/DR 定位

3.1 選用UKF 的合理性分析

線性系統疊加原理表明線性系統的多個輸入信號的總響應等于各個信號單獨作用產生響應的代數和.由式(3)可知,DR 定位涉及三角函數運算,因此本文提出的室內定位模型是非線性的,不符合疊加原理.一般采用擴展卡爾曼濾波(Extended Kalman Filter,EKF)或無跡卡爾曼濾波處理非線性模型的濾波問題.其中EKF 通過將非線性函數進行Taylor 級數展開,并保留其一階近似項實現線性化,難免引入線性化誤差,而UKF 利用無跡變換(Unscented Transform,UT)處理均值和協方差的非線性傳遞問題,直接基于非線性模型進行解算,免除線性化誤差,精確度更高[11–14].因此本文選用UKF 方法來進行融合濾波.

3.2 UKF 預測方程和觀測方程確定

根據DR 定位建立系統的狀態預測方程如下:

式中,X=[x,y,θ]T為運動狀態,(x,y)為機器人當前位置,θ為當前航向,ωRk?1,ωLk?1為電機編碼器測得的電機轉速,作為運動狀態的控制變量,Qk為三維的過程噪聲.根據式(5)可得f(Xk?1,ωRk?1,ωLk?1)的表達式為:

根據UWB 的TOA 定位方法可以獲得位置坐標,根據電子羅盤可測得航向,因此以位置和航向作為觀測量從而建立測量方程如下:

式中,Rk為三維得到觀測噪聲,H為觀測矩陣,其表達式為:

3.3 UKF 方法

無跡卡爾曼濾波的大體思路是結合Kalman 線性濾波框架和UT 變換來處理均值和協方差的非線性傳遞問題,提高非線性問題的濾波效果.UKF 計算步驟如下:

(1)初始化

計算系統狀態和方差的初始值,在導航開始階段多次采集UWB 定位坐標和電子羅盤航向來計算系統狀態和方差的初始值.

(2)Sigma 點和對應權值計算

其中,λ=α2(n+κ)?n,n為系統狀態X的維數,α 和κ 確定采樣點在均值附近的分布,通常 α取一個較小的正值,κ=3?n,P(k|k)i為協方差矩陣的第i列.β 是非負的權系數,如果x服從高斯分布,β 最優值為2.

(3)狀態預測

(4)量測預測

(5)狀態更新

3.4 環境適應系數構建

由于室內環境復雜,往往存在著諸多障礙,采用UWB 進行室內定位時,其信號傳播過程中將會發生反射和折射,從而導致NLOS 誤差,此時LOS 狀態下確認的UWB 定位結果的觀測噪聲已無法正確反饋.而利用穩定噪聲統計特性的UKF 濾波不能得知這種變化,因而大大影響其濾波的精度和穩定性.為了優化濾波的效果,構建環境適應系數來對觀測噪聲實時修正,使得觀測噪聲能根據環境變化自適應.

新息定義為:

理論上,最優濾波的殘差服從均值為零的高斯分布,其協方差為:

但當BS 和MS 出現NLOS 環境的時候,殘差將會發生顯著的變化.因此可以根據高斯分布的 3σ原則,利用新息的理論協方差的均方根構建校驗信息來對UWB 得到的定位結果進行非視距檢測.

結合開窗估計法和極大似然估計法可得到新息的實時估計協方差為[15,16]:

則利用實時估計協方差和理論協方差構建環境適應系數為:

式中,RLOS為UWB 在LOS 情況下定位結果的協方差,則UWB 定位的觀測噪聲在每一次進行無跡卡爾曼濾波的時候都進行動態修正為:

4 實驗分析

實驗環境如圖5所示,在場地內共放置了4 個UWB 基站,坐標分別為A(0,0),B(840,0),C(0,840),D(840,840),單位為cm.數據采集移動平臺在移動過程中的可以實時采集航向和車速信息,并通過ESP8266不斷向外傳輸,UWB 基站A 通過串口不斷向外發送移動站和各個基站的距離.通過自主開發的C#軟件同頻率實時記錄以上數據.

圖5 實驗環境圖

為了更好驗證所提出方法的定位效果,設計途經NLOS 誤差路段的折線路徑實驗,即采集平臺在空間阻擋的區域中按計劃路徑勻速運行時的信息.由于NLOS 誤差的存在,觀測噪聲模型必然呈現非高斯特性.至此可知,室內定位模型是非線性且非高斯的.而粒子濾波是一種適于處理此類模型的成熟濾波方法.因此本文分別通過UKF、PF 和環境自適應UKF 方法處理數據,并比對和分析定位效果得出實驗結果.

實驗結果如圖6～圖9所示,分別分析基于最小二乘法得到的測量值、UKF 方法、PF 方法以及本文方法的運動軌跡復現、定位誤差、定位累計誤差以及定位誤差累積概率分布.

圖6對比了不同方法的運動軌跡復現結果,可以看出,在LOS 情況下測量值也有著較高的定位精度,此時基于非視距檢測構建的環境適應系數為I,即環境自適應UKF 和UKF 具有相同濾波效果;當存在NLOS測距異常值時,測量值的定位精度明顯降低,UKF、PF 和環境自適應UKF 方法均表現出較強的抗NLOS性能.

圖6 運動軌跡復現

圖7和圖8中50～60 s 可以看出,當出現NLOS 誤差的時候,測量值的定位誤差明顯增大,UKF 方法和PF 方法一定程度抑制了誤差,本文方法能夠顯著提高NLOS 情況下的定位精度,使之與LOS 情況下的定位誤差變化與累計誤差增長趨勢相近.

圖7 定位誤差

圖8 定位累積誤差

圖9所示為不同方法的誤差累積概率分布,進一步驗證了在有遮擋的路徑下,本文方法相比于其它方法具有更優的定位性能.

表1是NLOS 誤差量化表,從中可以看出在NLOS情況下,本文方法的最大誤差值、平均誤差值相較其他方法最低,NLOS 降低率最高.此外用Matlab 統計得,UKF、PF、本文方法的運算時間分別是0.0117 s,0.6094 s,0.0125 s.因此PF 方法定位效果略優于UKF方法,但運算效率過低;環境自適應UKF 通過消耗低成本運算效率換得顯著優于UKF 方法的定位效果,進一步表明在NLOS 情況下,本文方法相比于其它方法具有更好的性能.

圖9 定位誤差累積概率分布

表1 NLOS 誤差量化表

總之,實驗數據和結果表明,本文方法在LOS 情況下定位效果與UKF 方法、PF 方法相近;在NLOS情況下,由于環境適應系數的動態修正,比UKF 方法和PF 方法具有更高的定位性能.

5 結論

針對復雜室內環境下UWB 信號傳播的非視距NLOS 誤差問題,本文提出基于UKF 的環境自適應UWB/DR 室內定位方法.該方法從UWB 定位環境的誤差產生原因出發,通過建立環境自適應UKF 濾波模型,在UWB/DR 定位融合過程中對UWB 定位信息進行非視距檢測并且引入環境適應系數動態修正UWB定位觀測噪聲.實驗效果表明,該方法具有較高的抗NLOS 誤差性能,定位精度優于PF 方法和UKF 方法,是一種具有較強實用性且高精度的室內定位方法.