閆保芳, 毛慶洲

(武漢大學(xué) 測繪遙感信息工程國家重點(diǎn)實(shí)驗室,湖北 武漢 430079)

一種基于卡爾曼濾波的超寬帶室內(nèi)定位算法

閆保芳, 毛慶洲

(武漢大學(xué)測繪遙感信息工程國家重點(diǎn)實(shí)驗室,湖北武漢430079)

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

超寬帶; 室內(nèi)定位; 標(biāo)定; 卡爾曼濾波; 擴(kuò)展卡爾曼濾波

0 引 言

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

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

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

1 UWB傳感器測距原理與標(biāo)定模型

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

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

(1)

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

1.1 LOS環(huán)境

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

(2)

式中v0,i(d0,i)為與真實(shí)值相關(guān)的絕對誤差函數(shù),假設(shè)v0,i為一次函數(shù)

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

(3)

則

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

(4)

1.2 NLOS環(huán)境

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

(5)

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

2 測距值KF

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

(6)

由式(6)構(gòu)造離散卡爾曼濾波的狀態(tài)方程為

xk=Axk-1+wk-1

(7)

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

zk=Hxk+vk

(8)

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

(9)

(10)

(11)

(12)

(13)

創(chuàng)新生態(tài)系統(tǒng)核心企業(yè)主導(dǎo)能力測度研究—基于扎根理論的探索…………………………邊偉軍，密淑泉，羅公利，王玉英（1，24）

(14)

3 EKF跟蹤定位

TOA定位原理如圖1所示,三個錨節(jié)點(diǎn)位于同一水平面上,以P101為原點(diǎn)O,P101—P102為X軸,P101—P103為Y軸,垂直O(jiān)XY方向為Z軸,建立空間直角坐標(biāo)系。

圖1 TOA定位原理

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

(15)

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

假設(shè)P100運(yùn)動狀態(tài)模型為

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

(16)

非線性測量模型為

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

(17)

式中Xk為MN坐標(biāo)和相應(yīng)坐標(biāo)軸運(yùn)動速度向量,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為預(yù)測噪聲向量Wk的協(xié)方差矩陣;Zk為MN距AN觀測距離向量,Zk=(r1(k),r2(k),r3(k))T;Rk為觀測噪聲向量Vk的協(xié)方差矩陣;T為采樣時間間隔。則擴(kuò)展卡爾曼濾波方程為

(18)

(19)

(20)

(21)

(22)

4 實(shí)驗與結(jié)果分析

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

4.1 UWB傳感器測距標(biāo)定實(shí)驗

在室內(nèi)LOS環(huán)境中,從1～22m分別每隔1m靜態(tài)測量2000次距離,取2000次測量均值作為該距離下的測量值,用最小二乘擬合測距誤差與真實(shí)距離值間的函數(shù)關(guān)系,LOS環(huán)境標(biāo)定結(jié)果如圖2所示。

圖2 LOS環(huán)境標(biāo)定

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

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

圖3 SNR與測距關(guān)系

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

4.2 測距卡爾曼濾波

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

圖4 卡爾曼濾波測距處理

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

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

4.3 EKF定位實(shí)驗

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

圖5 EKF定位

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

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

5 結(jié)束語

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

[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的室內(nèi)定位技術(shù)評述[J].傳感器與微系統(tǒng),2009,28(2):1-7.

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

[4] 張令文,楊 剛.超寬帶室內(nèi)定位關(guān)鍵技術(shù)[J].數(shù)據(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] 歐陽昱.基于超寬帶的室內(nèi)定位算法研究[D].南京:南京郵電大學(xué),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定位算法在三維空間定位中的應(yīng)用[J].計算機(jī)仿真,2009,26(1):323-326.

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

[11] 毛永毅,李明遠(yuǎn),張寶軍.一種 NLOS 環(huán)境下的 TOA/AOA 定位算法[J].電子與信息學(xué)報,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].長春:吉林大學(xué),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-),女,碩士研究生,研究方向為室內(nèi)導(dǎo)航、3S集成。