一種基于卡爾曼濾波的超寬帶室內定位算法

閆保芳, 毛慶洲

(武漢大學 測繪遙感信息工程國家重點實驗室,湖北 武漢 430079)

一種基于卡爾曼濾波的超寬帶室內定位算法

閆保芳, 毛慶洲

(武漢大學測繪遙感信息工程國家重點實驗室,湖北武漢430079)

為了減小室內環境中障礙物對超寬帶(UWB)傳感器測距結果的影響,提出了一種基于卡爾曼濾波(KF)的超寬帶室內定位算法。利用超寬帶接收信號的信噪比區分視距和非視距環境,給出了超寬帶傳感器測距性能最小二乘標定模型,減小測距系統誤差;判斷相鄰測距差分是否在閾值范圍內,否則用卡爾曼濾波先驗估計替代后驗估計處理測距結果,由此減弱多徑效應和非視距誤差對測距的影響;用擴展卡爾曼濾波器(EKF)實現室內定位。實驗結果表明:算法在復雜室內環境中可達到亞米級的動態實時定位精度。

超寬帶; 室內定位; 標定; 卡爾曼濾波; 擴展卡爾曼濾波

0 引 言

目前,常用的超寬帶[1～4](ultra-wideband,UWB)技術室內定位方法有到達角(angle of arrival，AOA),到達時間(time of arrival，TOA),接收信號強度(receive signal strength，RSS),到達時間差(time difference of arrival，TDOA),一般采用其中的一種或幾種混合作為定位方法[5]。Irahhauten Z等人[6]提出了基于信號達到角度和時間(TOA/AOA)的室內定位方法,算法只需要一個錨節點(anchor node,AN),非視距(non-line of sight,NLOS)定位估計精度較高,而視距(line of sight,LOS)定位估計精度較傳統TOA定位方法低。Wang T等人[7]增加參考節點(refe-rence node,RN),計算移動節點 (mobile node,MN)和RN到AN的TDOA的定位方法,獲得較高的室內定位精度,但是需要的節點數目較多,且定位算法復雜。Suski W等人[8]建立了測量噪聲地圖,在粒子濾波框架下將UWB室內定位導航精度提高了30%,但建立測量噪聲地圖需要測大量數據且環境普適性不強。

上述定位方法關于測量值的方程組通常情況下均為非線性的,為了求出最優解,提出了具有不同精度和復雜度的定位算法。常見的有:Chan定位算法[9],在LOS環境中定位精度較高,但NLOS環境下需要靠增加AN改善定位精度;Taylor級數展開法[10],計算速度較Chan算法快,NLOS環境下定位精度也有所改善,但是Taylor算法初值精度低時會發散;徑向基函數(radial basis function,RBF)神經網絡定位算法[11]與前兩者相比,在NLOS環境下遠距離的定位精度較高,而一般室內測量距離較短,因此并不適用。

相較而言,卡爾曼濾波(Kalman filtering,KF)利用某一時刻測量值和上一時刻估計值來預測當前值,具有良好的動態跟蹤定位的效果,遞歸計算復雜度低[12]。本文采用帶有觀測量差分閾值判斷后驗估計的KF以減小測距過程中環境、電氣等引入的噪聲,最后利用擴展卡爾曼濾波(extended Kalman filtering,EKF)實現動態跟蹤定位。

1 UWB傳感器測距原理與標定模型

本文選用P410超寬帶傳感器,利用超寬帶信號在2只傳感器之間的雙向飛行時間(two-way time of flight,TWTOF)進行測距[13]。

UWB傳感器TWTOF測距原理如圖1所示。傳感器A在τtxA時刻發送測距請求信號,傳感器B在τtxB時刻接收到信號后,在τtxB時刻發送測距響應信號,傳感器A在τtxA時刻接收并解析響應信號數據包,計算信號在兩個節點之間的飛行時間再乘以光速c,即可得兩節點間距AB。TWTOF測距方程為

(1)

LOS和NLOS環境中影響UWB測距精度的因素不同,需分別標定。本文選擇一次函數作為傳感器標定模型[14,15]。

1.1 LOS環境

受電氣轉換時間、天線和天線連接器電氣延遲等因素的影響,測距響應時延τdelay存在誤差。首先需要標定UWB傳感器的LOS測距性能。假設傳感器P0與Pi,i=1,2,3之間距離的測量值0,i和真值d0,i關系如下

(2)

式中v0,i(d0,i)為與真實值相關的絕對誤差函數,假設v0,i為一次函數

v0,i(d)=a0,id+b0,i

(3)

則

d0,i=(0,i-b0,i)/(1+a0,i)

(4)

1.2 NLOS環境

NLOS條件下,信號直接路徑上存在障礙物,一方面影響信號的檢測精度,另一方面影響信號穿過障礙物時的速度,使圖1所示的τrxB和τrxA數值偏大。障礙物對信號的衰減作用使NLOS環境接收信號強度較LOS環境弱很多,可以將接收信號的信噪比作為測距條件的判斷依據。對于相同材質的障礙物,NLOS誤差可以視為常數,在LOS標定后的基礎上式(4)可以改寫為

(5)

式中SNR為接收信號信噪比;TSNR為測距條件判定閾值;vNLOS為非視距誤差常數,需要實地測量確定。

2 測距值KF

首先將標定后的傳感器數據進行卡爾曼濾波處理,獲得精度較高的連續測距值,方可進行定位。短時間內,MN與AN的距離rk可視為均勻變化

(6)

由式(6)構造離散卡爾曼濾波的狀態方程為

xk=Axk-1+wk-1

(7)

離散卡爾曼濾波的觀測方程為

zk=Hxk+vk

(8)

式中zk為第k個采樣點距離測量值;wk和vk分別為過程噪聲和觀測噪聲;H=[1,0]。由文獻[16]可得

(9)

(10)

(11)

(12)

(13)

創新生態系統核心企業主導能力測度研究—基于扎根理論的探索…………………………邊偉軍，密淑泉，羅公利，王玉英（1，24）

(14)

3 EKF跟蹤定位

TOA定位原理如圖1所示,三個錨節點位于同一水平面上,以P101為原點O,P101—P102為X軸,P101—P103為Y軸,垂直OXY方向為Z軸,建立空間直角坐標系。

圖1 TOA定位原理

設MNP100坐標為(x,y,z),ANP101,P102,P103坐標為(xi,yi,zi),i=1,2,3,TOA定位原理如圖1所示,則MN和AN之間的距離為

(15)

式(15)所示的MN坐標(x,y,z)和觀測距離ri之間的關系是非線性的,即測量方程非線性,需要將預測和狀態估計線性化后再應用于KF。因此,為了提高MN動態定位精度,本文采用EKF來估計MN坐標。

假設P100運動狀態模型為

Xk=fk-1(Xk-1)+Wk-1,Wk～N(0,Qk)

(16)

非線性測量模型為

Zk=hk(Xk)+Vk,Vk～N(0,Rk)

(17)

式中Xk為MN坐標和相應坐標軸運動速度向量,Xk=[x(k),y(k),z(k),vz(k),vy(k),vz(k)]T,vi(k)=(i(k)-i(k-1))/T,i=x,y,z;Qk為預測噪聲向量Wk的協方差矩陣;Zk為MN距AN觀測距離向量,Zk=(r1(k),r2(k),r3(k))T;Rk為觀測噪聲向量Vk的協方差矩陣;T為采樣時間間隔。則擴展卡爾曼濾波方程為

(18)

(19)

(20)

(21)

(22)

4 實驗與結果分析

實驗流程如下:1)用最小二乘法標定傳感器;2)采用帶有觀測量差分閾值判斷后驗估計的KF提高測距精度,減小隨機誤差對測距的影響;并估計含粗大誤差的采樣點,減弱粗大誤差影響;3)通過非線性EKF估計MN位置

4.1 UWB傳感器測距標定實驗

在室內LOS環境中,從1～22m分別每隔1m靜態測量2000次距離,取2000次測量均值作為該距離下的測量值,用最小二乘擬合測距誤差與真實距離值間的函數關系,LOS環境標定結果如圖2所示。

圖2 LOS環境標定

從圖2可以看出:真實距離小于3m時,測距絕對誤差較大。由于距離太近,UWB信號在空中飛行時間過短,使圖1中傳感器A接收的響應信號被自己發射的測距請求信號干擾,標定UWB傳感器測距值可以減小這種干擾引入的誤差。

在室內LOS和NLOS條件下從1～16m分別每隔1m靜態測距1000次,取1000次測量均值,用一次函數最小二乘擬合信噪比(signal to noise ratio,SNR)與測距的關系,如圖3所示。其中,NLOS環境中2只傳感器間隔厚度約為250mm的空心磚墻。

圖3 SNR與測距關系

由圖3知:傳感器測量值相同時,LOS環境中的SNR較NLOS環境大,且隨著距離的增加,二者之間的差值逐漸增大。本文取LOS與NLOS環境中,SNR和傳感器測量值擬合曲線的角平分線作為測距環境的決策閾值曲線。獲得傳感器測距值后,根據決策閾值曲線求出此測距值對應的環境決策SNR閾值,即式(5)中的TSNR,對傳感器進行標定。

4.2 測距卡爾曼濾波

為了提高測距精度,減小多徑效應與NLOS的影響,用KF處理測距,結果如圖4所示。

圖4 卡爾曼濾波測距處理

LOS環境中以多徑效應測距誤差為主,UWB信號具有良好抗多徑效應的能力,從圖5(a)可以看出整體測距誤差較小,動態實時定位過程中,差分閾值判斷先驗估計代替后驗估計的KF可以很好估計個別含有粗差的測距值,削減突刺平滑測距采樣數據,KF處理后曲線較原始測量曲線平滑,且覆蓋了原來的測距曲線,具有良好跟蹤效果。

MN在NLOS環境運動過程中,與AN之間環境變化導致接收到的UWB信號強度和飛行時間劇烈變化,如圖5(b)所示,測距采樣曲線有很多毛刺平滑度低。KF處理后,測距采樣波形整體較平滑,有效減弱了NLOS誤差的影響。

4.3 EKF定位實驗

采用EKF能減弱測距誤差對定位的影響,動態定位效果如圖5所示。

圖5 EKF定位

圖5(a)為圖4(a)對應的定位軌跡,在56m×7.8m的室內LOS環境中進行實驗,MN實際運動軌跡為一個長方框。圖5(b)為在26m×7m的室內LOS環境中實驗結果,MN實際運動軌跡為回字形。圖5(c)為圖4(b)對應的定位軌跡,在室內NLOS環境中沿著厚度約為250mm的空心紅磚墻繞行。

從圖5可以看出:EKF后的軌跡較三邊測量定位軌跡平滑,受測距誤差影響較小,且更貼近實際運動軌跡。UWB在室內LOS環境中定位精度較高,尺寸較大的室內平均動態定位精度達到1m,小尺寸室內最大動態定位誤差在0.2m以內,室內NLOS環境中動態定位精度在1m左右。

5 結束語

本文通過分析實際測量誤差值,采用一次函數作為傳感器測距的誤差校正模型,減小了系統誤差的影響,并利用KF,減弱了多徑效應和非視距誤差。這兩步數據處理有效地提高了測距精度,最后采用EKF提高了定位精度。本文僅用3個錨節點,達到了1m的綜合室內動態定位精度,適用于機場、火車站和會展等室內公共場所的導航定位。由于本文僅使用UWB傳感器,缺少多余觀測量,因此,對于誤差較大的數據,KF估計效果不理想。后續工作中可以添加加速度計、陀螺儀等多傳感器組合導航,進一步提高室內定位精度。

[1] Zhang J,Orlik P V,Sahinoglu Z,et al.UWB systems for wireless sensor networks[C]∥Proceedings of the IEEE,2009:313-331.

[2] 王小輝,汪云甲,張 偉.基于RFID的室內定位技術評述[J].傳感器與微系統,2009,28(2):1-7.

[3] 劉小康,張 翔,方 爽,等.基于ZigBee無線傳感器網絡的一種室內定位新方法[J].傳感器與微系統,2013,32(11):29-36.

[4] 張令文,楊 剛.超寬帶室內定位關鍵技術[J].數據采集與處理,2013(6):706-713.

[5] Irahhauten Z,Nikookar H,Klepper M.A joint ToA /DoA technique for2D /3D UWB localization in indoor multipath environment[C]∥Internatimal Conference on Communications (ICC),Ottawa,ON,2012.

[6] 歐陽昱.基于超寬帶的室內定位算法研究[D].南京:南京郵電大學,2013.

[7] Wang T,Chen X,Ge N,et al.Error analysis and experimental study on indoor UWB TDOA localization with reference tag[C]∥APCC,Denpasar,2013.

[8] Suski W,Banerjee S,Hoover A.Using a map of measurement noise to improve UWB indoor position tracking[J].IEEE Tran-sactions on Instrumentation and Measurement,2013,62(8):2228-2236.

[9] 章堅武,唐 兵,秦 峰.Chan定位算法在三維空間定位中的應用[J].計算機仿真,2009,26(1):323-326.

[10] 蔣文美,王 玫.一種基于TOA的UWB直接Taylor復合定位算法[J].桂林電子工業學院學報,2006,26(1):1-5.

[11] 毛永毅,李明遠,張寶軍.一種 NLOS 環境下的 TOA/AOA 定位算法[J].電子與信息學報,2009,31(1):37-40.

[12] Zhang Y,Zhang R,Miao W,et al.An indoor positioning algorithm for mobile objects based on track smoothing[C]∥2014IEEE7th International Conference on Advanced Infocomm Technology (ICAIT),IEEE,2014:252-259.

[13] 孟宏偉.超寬帶無線定位方法研究[D].長春:吉林大學,2007.

[14] Monica S,Ferrari G.An experimental model for UWB distance measurements and its application to localization problems[C]∥2014 IEEE International Conference on Ultra-WideBand (ICUWB),IEEE,2014:297-302.

[15] Zoltan K,Charles T,Dorota G,et al.Performance analysis of UWB technology for indoor positioning[C]∥International Technical Meeting of The Institute of Navigation,San Diego,California,2014.

[16] Mohinder S Grewal,Angus P Andrews.Kalman filtering:Theory and practice using Matlab[M].2nd ed.New York:A Wiley-Interscience Publication,2001:170-184.

AnUWBindoorpositioningalgorithmbasedonKalmanfiltering

YAN Bao-fang, MAO Qing-zhou

(StateKeyLaboratoryofInformationEngineeringinSurvey,MappingandRemoteSensing,WuhanUniversity,Wuhan430079,China)

In order to eliminate the effects of indoor obstacles lead to ultra-wideband(UWB) ranging result,present an UWB indoor localization algorithm based on Kalman filtering.By using signal noise ratio of UWB to distinguish line of sight from non-line of sight environment,give least squares calibration models for ultra-wideband sensor ranging performance to reduce the distance measuring system error.To weaken multipath effects and effect of non-line of sight error on ranging,use Kalman filtering priori estimation to replace posterior estimation to process ranging result.Extended Kalman filter is used to achieve indoor positioning.Experimental results prove that,the algorithm has sub-meter high dynamic realtime positioning precision in complex indoor environments.

ultra wide band(UWB); indoor positioning; calibration; Kalman filtering(KF); extended Kalman filtering(EKF)

10.13873/J.1000—9787(2017)10—0137—04

2016—11—02

TN 929

A

1000—9787(2017)10—0137—04

閆保芳(1992-),女,碩士研究生,研究方向為室內導航、3S集成。