一種基于Taylor和Kalman的室內(nèi)協(xié)同定位方法*

王瑞榮,鄭書萬(wàn),陳浩龍,薛 楚

(1.杭州電子科技大學(xué)生命信息與儀器工程學(xué)院,杭州 310018;2.杭州電子科技大學(xué)信息與控制研究所,杭州 310018)

?

一種基于Taylor和Kalman的室內(nèi)協(xié)同定位方法*

王瑞榮1*,鄭書萬(wàn)2,陳浩龍1,薛 楚1

(1.杭州電子科技大學(xué)生命信息與儀器工程學(xué)院,杭州 310018;2.杭州電子科技大學(xué)信息與控制研究所,杭州 310018)

結(jié)合Chan算法、Taylor算法及Kalman算法三種TDOA算法的優(yōu)點(diǎn),提出一種能應(yīng)用于室內(nèi)實(shí)時(shí)定位的協(xié)同方法。首先基于Chan與Taylor的協(xié)同定位方法估算位置信息,并通過對(duì)估計(jì)結(jié)果的殘差設(shè)置閾值來(lái)鑒別NLOS,從而拋棄受到NLOS污染嚴(yán)重的測(cè)量數(shù)據(jù)。其次,再對(duì)符合條件的測(cè)量數(shù)據(jù),利用Kalman方法計(jì)算定位結(jié)果,與Taylor方法的定位結(jié)果通過設(shè)置判別條件進(jìn)行比較,以此進(jìn)一步抑制NLOS干擾。對(duì)符合判別條件的定位結(jié)果,進(jìn)行殘差加權(quán)及移動(dòng)平均加權(quán)處理,從而完成最終定位結(jié)果的更新。最后,利用室內(nèi)實(shí)時(shí)定位實(shí)驗(yàn),證明該方法能有效過濾受到NLOS污染嚴(yán)重的測(cè)距數(shù)據(jù),提高定位精度,并且具有良好的穩(wěn)定性。

室內(nèi)定位;協(xié)同方法;Taylor算法;Kalman算法;殘差加權(quán)

隨著物聯(lián)網(wǎng)和智慧城市建設(shè)的興起和發(fā)展,數(shù)據(jù)和多媒體業(yè)務(wù)快速增加,帶動(dòng)了室內(nèi)定位應(yīng)用服務(wù)需求日益增加。但是,相比室外定位環(huán)境,室內(nèi)環(huán)境空間封閉狹小,障礙物多,無(wú)線信號(hào)不僅存在多徑傳播效應(yīng),且易受到空間隨機(jī)無(wú)線電波信號(hào)干擾。室內(nèi)環(huán)境的復(fù)雜多變,要求定位系統(tǒng)具有良好的抗干擾性、適應(yīng)性。目前,常見的室內(nèi)定位方法有基于測(cè)距和非測(cè)距兩種方法。相對(duì)而言,基于測(cè)距的方法具有更高的定位精度,且更具有應(yīng)用價(jià)值。常見基于測(cè)距的定位算法有時(shí)間到達(dá)法TOA(Time of Arrive)、時(shí)間到達(dá)差法TDOA(Time Difference of Arrival)、信號(hào)強(qiáng)度法RSSI(Received Signal Strength Indication)、到達(dá)角度法AOA(Angle of Arrival)等[1]。這些方法又有各自的特性,RSSI測(cè)量值在實(shí)際應(yīng)用中規(guī)律性較差,利用RSSI信息難以得到較高的定位精度;AOA測(cè)量雖然精度較高,但傳感節(jié)點(diǎn)成本高,能耗大;TOA和TDOA測(cè)量方法簡(jiǎn)單,傳感器節(jié)點(diǎn)成本適宜,且能得到較高定位精度,使得這兩種方法廣泛應(yīng)用。TDOA算法是對(duì)TOA算法的改進(jìn),其計(jì)算過程更加簡(jiǎn)單,精度也有所提高,只需基站之間同步[1-2]。因此,文中研究一種抑制NLOS(Non Line of Sight)的室內(nèi)TDOA定位方法。

典型的TDOA定位算法有Fang算法[3]、Chan算法[4]、Taylor展開算法[5]、Kalman算法[6]等。Chan算法計(jì)算過程中,采用兩步加權(quán)最小二乘估計(jì)定位結(jié)果,具有運(yùn)算快特點(diǎn)。在信道環(huán)境較差情況下,雖然其定位精度可能迅速下降,但估計(jì)結(jié)果仍能反映與TDOA測(cè)量值之間的大致特征[7-8]。Taylor和Kalman算法都能獲得較為準(zhǔn)確的定位估計(jì)值[9-11]。但是,Taylor算法需要設(shè)置合適的初始值,否則可能導(dǎo)致定位算法不收斂。Chan和Taylor的協(xié)同方法[12],將Chan方法的定位結(jié)果作為Taylor方法的初始位置結(jié)果,可保證其收斂性。Kalman算法對(duì)歷史定位數(shù)據(jù)具有記憶性,若歷史數(shù)據(jù)出現(xiàn)較大偏差,將對(duì)系統(tǒng)定位結(jié)果持續(xù)產(chǎn)生負(fù)面影響。因此需要對(duì)受到NLOS干擾嚴(yán)重的測(cè)量數(shù)據(jù)進(jìn)行識(shí)別處理,從而提高Kalman算法的定位精度。文中利用定位結(jié)果殘差鑒別受到NLOS污染嚴(yán)重的測(cè)距數(shù)據(jù),并對(duì)其進(jìn)行拋棄處理。

同時(shí),適用于室內(nèi)環(huán)境定位的無(wú)線技術(shù)有超寬帶技術(shù)(UWB)、線性調(diào)頻擴(kuò)頻技術(shù)(CSS)、Zigbee技術(shù)、Wifi技術(shù)等。其中,基于CSS技術(shù)的定位設(shè)備,利用對(duì)稱雙路雙邊測(cè)距方式(SDS-TWR)測(cè)量節(jié)點(diǎn)間距離,具有發(fā)射功率低、測(cè)距精度高、通信穩(wěn)定性好、抗干擾能力強(qiáng)等特點(diǎn),適用于室內(nèi)定位[13-15]。文中采用基于CSS技術(shù)的定位設(shè)備對(duì)定位算法進(jìn)行實(shí)驗(yàn)分析。

1 TDOA定位算法描述

在二維空間內(nèi),TDOA雙曲線數(shù)學(xué)模型如下:

(1)

其中,(xi,yi),i=1,2,3….,n為基站節(jié)點(diǎn)坐標(biāo),(x,y)為目標(biāo)節(jié)點(diǎn)坐標(biāo),ri1是第i(i≤2)個(gè)基站與第1個(gè)基站到目標(biāo)節(jié)點(diǎn)的距離差,ni1是相對(duì)應(yīng)的TDOA測(cè)距誤差。

1.1 Chan算法

Chan算法[4]是一種利用求解雙曲線方法的非遞歸算法。求解過程中,對(duì)式(1)進(jìn)行線性化處理,兩次利用最小二乘法(WLS)加權(quán)估計(jì)結(jié)果。第一次估計(jì)忽略目標(biāo)節(jié)點(diǎn)的坐標(biāo)(x,y)與基準(zhǔn)基站間的距離者之間的相關(guān)性,作為獨(dú)立變量求解;第二次估算則利用三者之間的約束關(guān)系,得到更加準(zhǔn)確的目標(biāo)節(jié)點(diǎn)標(biāo)值。對(duì)于Chan算法的估計(jì)結(jié)果,當(dāng)系統(tǒng)噪聲服從高斯分布時(shí),誤差較小;否則,精度將迅速下降。

1.2 Taylor算法

Taylor算法[5]是一種需要目標(biāo)節(jié)點(diǎn)初始估計(jì)位置的遞歸算法。求解過程中,估計(jì)坐標(biāo)(x,y)表示為:

式中,(x0,y0)初始估計(jì)位置,Δx和Δy是估計(jì)誤差。

對(duì)式(1)在初始點(diǎn)(x0,y0)進(jìn)行Taylor級(jí)數(shù)展開,忽略二階以上分量,建立求解TDOA測(cè)量誤差矢量的線性方程。然后,根據(jù)局部線性最小二乘解法估計(jì)坐標(biāo)誤差。以此,迭代更新目標(biāo)節(jié)點(diǎn)的估計(jì)位置直到坐標(biāo)誤差滿足預(yù)先設(shè)置的門限。但是,如果初始位置的設(shè)置不恰當(dāng),可能導(dǎo)致算法運(yùn)算結(jié)果發(fā)散,且對(duì)不收斂的情況無(wú)法進(jìn)行事先判斷。

1.3 Kalman算法

Kalman算法[6]是利用擴(kuò)展Kalman濾波(EKF)兩次遞歸估算求解的算法。基于式(1),建立線性的狀態(tài)方程和非線性的觀測(cè)方程分別為:

式中,Xk=[xk,yk,vxk,vyk]T是tk時(shí)刻狀態(tài)向量,(xk,yk)是目標(biāo)節(jié)點(diǎn)坐標(biāo)信息,(vxk,vyk)是目標(biāo)節(jié)點(diǎn)x方向和y方向的分速度,Φ是狀態(tài)轉(zhuǎn)移矩陣,h(Xk)是非線性量測(cè)函數(shù),Yk=[r21,r31,…,rn1]T是量測(cè)數(shù)據(jù)向量,Wk-1、Vk是輸入噪聲和觀測(cè)噪聲。

非線性的觀測(cè)方程,需利用Taylor級(jí)數(shù)展開得到雅可比矩陣,用非線性函數(shù)的局部線性化特性來(lái)近似描述其非線性。然后利用EKF算法方程估計(jì)定位結(jié)果。Kalman算法利用歷史定位數(shù)據(jù)進(jìn)行估算,具有較好的動(dòng)態(tài)定位性能。但若歷史定位數(shù)據(jù)出現(xiàn)較大偏差,將持續(xù)影響定位結(jié)果的估算。

2 基于Taylor和Kalman的協(xié)同定位方法

然后,對(duì)仍然符合條件的測(cè)距數(shù)據(jù),用Kalman方法計(jì)算得到位置結(jié)果(x3,y3),并與(x2,y2)進(jìn)行比較,進(jìn)一步過濾測(cè)量數(shù)據(jù)。對(duì)于Taylor方法和Kalman方法,在測(cè)量值誤差不大情況下,定位結(jié)果應(yīng)相近。以此,設(shè)置閾值δ3、δ4,建立如下兩種不同判斷不等式:

|x3-x2|+|y3-y2|<δ3

(2)

(3)

(4)

閾值δ1、δ2、δ3和δ4的選取主要根據(jù)定位設(shè)備的測(cè)距精度,及在室內(nèi)環(huán)境下的歷史定位信息。根據(jù)定位設(shè)備定位精度,選取δ1、δ2(δ1>δ2),確保誤差非常大的測(cè)距數(shù)據(jù)能有效過濾,減少計(jì)算量。然后利用歷史測(cè)量數(shù)據(jù),用Chan和Taylor的協(xié)同方法計(jì)算定位結(jié)果,舍去定位誤差大的定位結(jié)果及相對(duì)應(yīng)的測(cè)量數(shù)據(jù),再用Kalman方法計(jì)算位置結(jié)果。統(tǒng)計(jì)Taylor方法定位結(jié)果的殘差平方和均值和與Kalman方法定位結(jié)果的差值,確定合適的δ3和δ4,確保能使歷史數(shù)據(jù)基本滿足式(2)和式(3)的判斷不等式。δ3和δ4選取應(yīng)適宜,過大則降低對(duì)NLOS抑制的能力,過小則會(huì)對(duì)測(cè)量數(shù)據(jù)誤判,去除有效的測(cè)距數(shù)據(jù)。同時(shí),根據(jù)實(shí)時(shí)定位信息,微調(diào)閾值。

3 實(shí)驗(yàn)設(shè)計(jì)和數(shù)據(jù)分析

3.1 實(shí)驗(yàn)設(shè)計(jì)

定位測(cè)試實(shí)驗(yàn)使用Nanotron公司生產(chǎn)的nanoLOC Development Kit 3.0設(shè)備,在杭州電子科技大學(xué)第二教學(xué)樓南504實(shí)驗(yàn)室進(jìn)行。該實(shí)驗(yàn)室長(zhǎng)8.3 m,寬8.5 m。同時(shí),室內(nèi)環(huán)境較為復(fù)雜,擺放有多臺(tái)辦公桌、計(jì)算機(jī)及其它儀表儀器,人員走動(dòng)頻繁。在實(shí)驗(yàn)室中取5.65 m×5.4 m的區(qū)域并布置四個(gè)基站節(jié)點(diǎn):Anchor1(0.00,0.00),Anchor2(5.65,00.00),Anchor3(5.65,5.40),Anchor4(0.00,5.40),然后在該區(qū)域中放置一個(gè)目標(biāo)節(jié)點(diǎn)。

設(shè)備使用手冊(cè)介紹,在LOS情況下,定位設(shè)備理論上的測(cè)距精度能達(dá)到2 m。但經(jīng)過室內(nèi)測(cè)距實(shí)驗(yàn),表明測(cè)距誤差大于2 m,在3～4 m左右,但是都是誤差都是正值且都大于2 m。所以,估計(jì)Chan方法定位殘差誤差在4 m左右,Chan和Taylor的協(xié)同方法定位殘差在0.64 m左右。最終根據(jù)歷史定位信息,選取δ1=4.0 m、δ2=0.64 m、δ3=0.14 m、δ4=0.5 m。

實(shí)驗(yàn)過程中,先將目標(biāo)節(jié)點(diǎn)放置固定位置,然后對(duì)定位設(shè)備的TOA測(cè)量結(jié)果實(shí)時(shí)采集并保存。同時(shí),對(duì)TOA值進(jìn)行重構(gòu)得到TDOA值,利用文中提出的定位算法實(shí)時(shí)估算目標(biāo)節(jié)點(diǎn)位置并保存,同時(shí)圖形化顯示,如圖1所示。

圖1 目標(biāo)節(jié)點(diǎn)位置信息實(shí)時(shí)顯示

為了檢驗(yàn)文中提出的算法是否能有效抑制NLOS干擾,將重新采用最小二乘(LS)TOA方法,Chan方法、Taylor方法及Kalman方法估算定位結(jié)果,并與其進(jìn)行比較分析。同時(shí),挑選兩組原始定位數(shù)據(jù),通過Chan_Taylor協(xié)同方法、Kalman方法、定位結(jié)果殘差設(shè)置閾值的Chan_Taylor協(xié)同方法(設(shè)置的閾值分別為δ1和δ2)以及文中方法分別進(jìn)行計(jì)算定位結(jié)果誤差,并進(jìn)行比較分析。

3.2 數(shù)據(jù)分析

對(duì)目標(biāo)節(jié)點(diǎn)進(jìn)行多次定位實(shí)驗(yàn)后,比較采用LS TOA方法、Chan方法、Taylor方法、Kalman方法及文中方法計(jì)算得到的定位結(jié)果均值,如表1所示。比較Taylor方法、Kalman方法及文中方法定位結(jié)果的均方根誤差(RESE),如表2所示。

表1 不同算法目標(biāo)節(jié)點(diǎn)估計(jì)坐標(biāo)值比較

表2 不同算法估計(jì)結(jié)果的RMSE比較

從表中數(shù)據(jù)看出,在室內(nèi)環(huán)境下,采用LS TOA方法和Chan方法得到的定位結(jié)果較差,具有較大定位誤差,所以這兩種方法不適合室內(nèi)定位;Taylor方法和Kalman方法的定位精度較高,結(jié)果接近真實(shí)值,并且兩者定位結(jié)果相接近;文中結(jié)合基于Chan算法、Taylor方法和Kalman方法的協(xié)同定位方法,結(jié)合兩者算法優(yōu)點(diǎn),具有更高的定位精度。進(jìn)一步比較三者算法的RMSE表明,文中定位方法穩(wěn)定性也更好。

取目標(biāo)節(jié)點(diǎn)放置在(3.39,5.40)和(2.26,1.80)位置的原始測(cè)量數(shù)據(jù),然后分別利用Chan_Taylor協(xié)同方法、Kalman方法、定位結(jié)果殘差設(shè)置門限的Chan_Taylor協(xié)同方法及文中方法估算定位結(jié)果并計(jì)算定位誤差。圖2和圖3是目標(biāo)節(jié)點(diǎn)在(2.26,1.80)位置的定位誤差比較,圖4和圖5是目標(biāo)節(jié)點(diǎn)在(2.39,5.40)位置的定位誤差比較。

圖2 比較Chan_Taylor和Klaman在坐標(biāo)(2.26,1.8)定位誤差

圖3 比較Chan_Taylor和Kalman在坐標(biāo)(3.39,5.40)定位誤差

圖4 比較文中方法和殘差設(shè)置閾值的Taylor方法在坐標(biāo)(2.26,1.80)定位誤差

圖5 比較文中方法和殘差設(shè)置閾值的Taylor方法在坐標(biāo)(3.39,5.40)定位誤差

利用Chan_Taylor協(xié)同方法和Kalman方法估算定位結(jié)果,計(jì)算定位誤差,如圖2與圖4所示。圖中表明,定位結(jié)果受到NLOS的影響明顯,誤差波動(dòng)幅度較大。

而圖3和圖5表明,對(duì)Chan與Taylor協(xié)同方法的定位結(jié)果殘差設(shè)置閾值,雖具有過濾受到NLOS污染的測(cè)量數(shù)據(jù),來(lái)改善定位結(jié)果誤差的能力,但仍有誤差較大的定位結(jié)果,需進(jìn)一步過濾原始測(cè)距數(shù)據(jù),來(lái)抑制NLOS干擾;同時(shí),文中提出的方法更加有效的過濾受到NLOS污染的測(cè)量數(shù)據(jù),使定位結(jié)果未出現(xiàn)較大誤差,基本保持在一定范圍內(nèi),波動(dòng)幅度小,NLOS得到抑制。

從實(shí)驗(yàn)結(jié)果看出,文中提出的定位方法能有效抑制NLOS干擾,使定位誤差減小,對(duì)于室內(nèi)定位具有一定的實(shí)用性。

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

文中結(jié)合Chan算法、Taylor算法及Kalman算法三者算法特點(diǎn),提出一種室內(nèi)定位協(xié)同方法。實(shí)驗(yàn)結(jié)果表明,文中提出的室內(nèi)定位方法能有效抑制室內(nèi)環(huán)境下的NLOS干擾,具有較高的定位精度,具有良好的穩(wěn)定性,在室內(nèi)定位具有一定的實(shí)用價(jià)值。改進(jìn)現(xiàn)有的算法,對(duì)受NLOS污染嚴(yán)重的測(cè)量數(shù)據(jù),也可以進(jìn)行消極加權(quán)和統(tǒng)計(jì)校正處理,從而抑制NLOS;對(duì)測(cè)量數(shù)據(jù)進(jìn)行預(yù)處理,減少NLOS影響,及對(duì)定位結(jié)果進(jìn)行數(shù)據(jù)濾波處理,提高定位精度等,都是文中接下來(lái)的研究工作。

[1] 彭宇,王丹. 無(wú)線傳感器網(wǎng)絡(luò)定位技術(shù)綜述[J]. 電子測(cè)量與儀器學(xué)報(bào),2011,25(5):389-399.

[2]張麗. 超寬帶室內(nèi)精確定位技術(shù)研究[D]. 青島:中國(guó)海洋大學(xué),2012:21-40.

[3]Fang B T. Simple Solutions for Hyperbolic and Related Position Fixes[J]. IEEE Trans on Aerospace and Electronic Systems,1990,26(5):748-753.

[4]Chan Y T,Ho K C. A Simple and Efficient Estimator for Hyperbolic Location[J]. Signal Processing,IEEE Transactions,1994,42(8):1905-1915.

[5]Foy W H. Position-Location Solutions by Taylor Series Estimation[J]. IEEE Trans Aerosp Electron Syst,1976,12(2):187-194.

[6]Najar M,Vidal J. Kalman Tracking Based on TDOA for UMTS Mobile Location[C]//Proc IEEE Int Symp Personal,Indoor and Mobile Radio Communication(PIMRC). San Diego,CA,2001:B45-B49.

[7]桂忠,董利達(dá),蘭守珍. 一種在非視距環(huán)境中的移動(dòng)節(jié)點(diǎn)定位方法[J]. 浙江大學(xué)學(xué)報(bào)(理學(xué)版),2009,36(1):52-56.

[8]Ho K C. Bias Reduction for an Explicit Solution of Source Localization Using TDOA[J]. IEEE Transactions on Signal Processing,2012,60(5):2101-2114.

[9]Kovavisaruch L,Ho K C. Modified Taylor-Series Method for Source and Receiver Localization Using TDOA Measurements with Erroneous Receiver Positions[C]//Proc of IEEE International Symposium on Circuits and Systems(ISCAS). IEEE Press,2005:2295-2298.

[10]Rullan-Lara J,Salazar S,Lozano R. UAV Real-Time Location Using a Wireless Sensor Network[C]//Proc of IEEE Positioning Navigation and Communication(WPNC). Dresden,German:IEEE Press,2011:18-23.

[11]Chiang ChengTse,Tseng PoHsuan,Feng KaiTen. Hybrid Unified Kalman Tracking Algorithms for Heterogeneous Wireless Location Systems[J]. IEEE Transactions on Vehicular Technology,2012,61(2):702-715.

[12]劉林,鄧平,范平志. 基于Chan氏算法和Taylor級(jí)數(shù)展開法的協(xié)同定位方法[J]. 電子與信息學(xué)報(bào),2004,26(1):41-46.

[13]楊清玉,于寧,王霄,等. 無(wú)線傳感器網(wǎng)絡(luò)線性調(diào)頻擴(kuò)頻測(cè)距方法研究[J]. 傳感技術(shù)學(xué)報(bào),2010,23(12):1761-1765.

[14]Nanaotron Technologies GmbH. Nanotron Chirp Based Wireless Networks,White Paper,V1.04[EB/OL]. http://nanotron.com/EN/pdf/WP_CSS.pdf.

[15]段翠翠,王瑞榮,王建中,等. 無(wú)線傳感器網(wǎng)絡(luò)的高危生產(chǎn)區(qū)人員定位系統(tǒng)[J]. 傳感技術(shù)學(xué)報(bào),2011,25(11):1599-1602.

王瑞榮(1973-),男,浙江義烏人,博士,副教授,主要研究方向?yàn)橹悄軆x器;

鄭書萬(wàn)(1991-),男,浙江平陽(yáng)人,碩士在讀,主要研究方向?yàn)闊o(wú)線傳感器網(wǎng)絡(luò)。

AnCooperativeLocalizationMethodBasedonTaylorandKalmanAlgorithms*

WANGRuirong1*,ZHENGShuwan2,CHENHaolong1,XUEChu1

(1.College of Life Information Science & Instrument Engineering,Hangzhou Dianzi University,Hangzhou 310018,China;2.Institute of Information and Control,Hangzhou Dianzi University,Hangzhou 310018,China)

A cooperative method for indoor real-time localization based on three TDOA algorithms is presented. These algorithms are Chan algorithm,Taylor serials expansion algorithm,and Extended Kalman filter algorithm. Firstly,estimation result is calculated by a cooperative method based on Chan and Taylor and threshold value of its residuals is set to identity NLOS and discard the ranging data that is disturbed severely by NLOS. Then,Kalman method is used for the matching data to get estimation position. The location result of Kalman is compared with the result of Taylor through setting some condition to further restrain NLOS error. Next,the final estimation result is obtained,by using residual weighting algorithm and moving weighted average method to the meet results. Finally,the experimental results show that this method can restrain NLOS error efficiently and improve the precision of location.

indoor localization;cooperative method;Taylor algorithm;Kalman algorithm;residual weighting

項(xiàng)目來(lái)源:國(guó)家自然科學(xué)基金項(xiàng)目(61374005);浙江省重大科技專項(xiàng)項(xiàng)目(2011R09019-01)

2014-06-23修改日期:2014-09-15

10.3969/j.issn.1004-1699.2014.11.021

TP393

:A

:1004-1699(2014)11-1557-05