閆保芳, 毛慶洲

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

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

閆保芳, 毛慶洲

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

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

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

0 引 言

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

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

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

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

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

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

(1)

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

1.1 LOS環(huán)境

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

(2)

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

(5)

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

2 測(cè)距值KF

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

(6)

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

xk=Axk-1+wk-1

(7)

離散卡爾曼濾波的觀測(cè)方程為

zk=Hxk+vk

(8)

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

(9)

(10)

(11)

(12)

(13)

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

(14)

3 EKF跟蹤定位

TOA定位原理如圖1所示,三個(gè)錨節(jié)點(diǎn)位于同一水平面上,以P101為原點(diǎn)O,P101—P102為X軸,P101—P103為Y軸,垂直O(jiān)XY方向?yàn)閆軸,建立空間直角坐標(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)和觀測(cè)距離ri之間的關(guān)系是非線性的,即測(cè)量方程非線性,需要將預(yù)測(cè)和狀態(tài)估計(jì)線性化后再應(yīng)用于KF。因此,為了提高M(jìn)N動(dòng)態(tài)定位精度,本文采用EKF來(lái)估計(jì)MN坐標(biāo)。

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

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

(16)

非線性測(cè)量模型為

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

(17)

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

(18)

(19)

(20)

(21)

(22)

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

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

4.1 UWB傳感器測(cè)距標(biāo)定實(shí)驗(yàn)

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

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

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

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

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

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

4.2 測(cè)距卡爾曼濾波

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

圖4 卡爾曼濾波測(cè)距處理

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

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

4.3 EKF定位實(shí)驗(yàn)

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

圖5 EKF定位

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

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

5 結(jié)束語(yǔ)

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

[3] 劉小康,張 翔,方 爽,等.基于ZigBee無(wú)線傳感器網(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] 歐陽(yáng)昱.基于超寬帶的室內(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] 章堅(jiān)武,唐 兵,秦 峰.Chan定位算法在三維空間定位中的應(yīng)用[J].計(jì)算機(jī)仿真,2009,26(1):323-326.

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

[11] 毛永毅,李明遠(yuǎn),張寶軍.一種 NLOS 環(huán)境下的 TOA/AOA 定位算法[J].電子與信息學(xué)報(bào),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] 孟宏偉.超寬帶無(wú)線定位方法研究[D].長(zhǎng)春:吉林大學(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-),女,碩士研究生,研究方向?yàn)槭覂?nèi)導(dǎo)航、3S集成。