IMMKF與Chan-Taylor算法的協(xié)同定位

王欣悅,余慧敏,胡露寧

(湖南師范大學(xué) 信息科學(xué)與工程學(xué)院,湖南 長(zhǎng)沙 410000)

超寬帶(Ultra Wide-Band,UWB)定位技術(shù)是一種區(qū)別于傳統(tǒng)通信方式的新通信技術(shù),其主要采用的測(cè)距方法包括接收信號(hào)強(qiáng)度(Received Signal Strength Indication,RSSI)、信號(hào)到達(dá)時(shí)間(Time of Arrival,TOA)、信號(hào)到達(dá)時(shí)間差(TDOA)以及信號(hào)到達(dá)角度(Angle of Arrival,AOA)[1]。TOA和TDOA的定位精度較AOA高,故室內(nèi)定位主要采用TOA和TDOA作為測(cè)距方法,根據(jù)飛行信號(hào)的相關(guān)時(shí)間信息解算目標(biāo)的位置信息?；赥DOA的測(cè)距多采用多節(jié)點(diǎn)TDOA法,再根據(jù)測(cè)量的TDOA值建立雙曲線方程求解交點(diǎn),由數(shù)學(xué)原理可知該點(diǎn)為目標(biāo)節(jié)點(diǎn)位置坐標(biāo)[2]。求解雙曲線的主要算法有最小二乘法、三邊定位算法、Chan算法、Taylor算法以及Fang算法[3],每種算法對(duì)TDOA測(cè)量值的誤差特性要求均不同,計(jì)算的復(fù)雜性也各不同,不同算法在不同TDOA噪聲特性下能達(dá)到定位估計(jì)的最優(yōu)解或次優(yōu)解。上述幾種求解算法面臨一個(gè)共同問(wèn)題,即在信道環(huán)境較差情況下,定位精度較低,較難滿足定位精度的要求,故多數(shù)研究選擇使用融合算法對(duì)坐標(biāo)進(jìn)行解算。

TDOA測(cè)距定位方法在理論上能夠達(dá)到相對(duì)理想的定位精度,但仍會(huì)因?yàn)橛布O(shè)備與環(huán)境因素引起噪聲,其主要包括非視距誤差和多徑效應(yīng)引起的信號(hào)誤差[4]。由于卡爾曼濾波算法具有良好的濾波效果,研究人員選擇使用卡爾曼濾波來(lái)消除誤差,提高定位精度及性能。早期基于卡爾曼濾波的研究主要集中在直接處理測(cè)量值。文獻(xiàn)[5]提出先使用卡爾曼濾波處理TOA測(cè)量值,消除誤差,再進(jìn)行TOA定位,理論和實(shí)驗(yàn)均表明該方法的定位結(jié)果優(yōu)于單純的TDOA測(cè)距方法。文獻(xiàn)[6]提出了基于擴(kuò)展卡爾曼濾波器的混合TDOA/TOA室內(nèi)定位方法,判斷TDOA和TOA測(cè)量值有無(wú)NLOS(Non-Line-of-Sight)誤差,有則使用卡爾曼濾波,并將處理后的結(jié)果與優(yōu)選AOA值一起代入擴(kuò)展卡爾曼濾波器中,最終算出目標(biāo)位置坐標(biāo)。文獻(xiàn)[7]首次提出卡爾曼濾波算法和Chan算法相結(jié)合的算法,將各種卡爾曼濾波算法與Chan算法結(jié)合再進(jìn)行目標(biāo)位置解算,實(shí)驗(yàn)結(jié)果表明結(jié)合卡爾曼濾波算法后定位精度更高。文獻(xiàn)[8]提出了Chan-Kalman定位算法,用Chan算法的位置估計(jì)值作為Kalman濾波的初始值,有效提高了室內(nèi)定位的精度和穩(wěn)定性。文獻(xiàn)[9]將IMM算法與卡爾曼濾波算法結(jié)合,利用TWS雷達(dá)數(shù)據(jù)跟蹤機(jī)動(dòng)目標(biāo),顯著提升了跟蹤目標(biāo)的定位精度。基于以上研究,本文提出一種結(jié)合交互式多模型IMM算法的Chan-Taylor-IMMKF室內(nèi)定位算法,將Chan-Taylor算法解算的坐標(biāo)估計(jì)值代入卡爾曼濾波器的觀測(cè)矩陣來(lái)作為卡爾曼濾波器的觀測(cè)值。同時(shí),使用IMM算法建立3個(gè)模型,根據(jù)濾波器的估計(jì)值匹配合適的模型進(jìn)行正規(guī)濾波,再將各個(gè)模型的定位結(jié)果進(jìn)行加權(quán)融合,以此達(dá)到提高定位精度的效果。

1 UWB室內(nèi)定位技術(shù)

超寬帶(UWB)是一種無(wú)線通訊技術(shù),使用持續(xù)時(shí)間較短、占空比較低的窄脈沖信號(hào),故在目前所知的室內(nèi)定位技術(shù)中其定位精度最高。超寬帶無(wú)線通信原理如圖1所示[10],在發(fā)送端脈沖發(fā)生器產(chǎn)生信號(hào)脈沖,通過(guò)調(diào)制器實(shí)現(xiàn)信息傳輸;在接收端,被接收到的信號(hào)經(jīng)過(guò)低噪聲放大器放大,然后通過(guò)匹配濾波器和相關(guān)接收機(jī)處理,被門(mén)限電路恢復(fù)信號(hào)攜帶的信息。

UWB定位主要分為測(cè)距與解算兩個(gè)過(guò)程。本文采用TDOA定位算法測(cè)距,其根據(jù)標(biāo)簽到各個(gè)定位基站的到達(dá)時(shí)間差解算標(biāo)簽位置[11],測(cè)距主要實(shí)現(xiàn)方式是多節(jié)點(diǎn)TDOA法,其原理如圖2所示。

圖2 多節(jié)點(diǎn)TDOA法Figure 2. Ulti-node TDOA method

多節(jié)點(diǎn)TDOA法的基本原理是雙曲線的數(shù)學(xué)性質(zhì),即雙曲線的點(diǎn)到兩焦點(diǎn)的距離之差為一個(gè)定值。如圖2所示,包括以信標(biāo)節(jié)點(diǎn)1、信標(biāo)節(jié)點(diǎn)2為焦點(diǎn)的一組雙曲線方程及以信標(biāo)節(jié)點(diǎn)2、信標(biāo)節(jié)點(diǎn)3為焦點(diǎn)的一組雙曲線方程,聯(lián)立兩組雙曲線方程,解算方程得出交點(diǎn)即為目標(biāo)節(jié)點(diǎn)坐標(biāo)。數(shù)學(xué)描述為:目標(biāo)節(jié)點(diǎn)所發(fā)射的信號(hào)到達(dá)3個(gè)信標(biāo)節(jié)點(diǎn)的時(shí)延分別為t1、t2和t3,則可算出目標(biāo)節(jié)點(diǎn)和每個(gè)信標(biāo)節(jié)點(diǎn)的距離分別為

r1=c×t1
r2=c×t2
r3=c×t3

(1)

其中,c是電磁波在空氣中的傳輸速度,為3×108m·s-1。進(jìn)一步便可求得目標(biāo)節(jié)點(diǎn)與信標(biāo)節(jié)點(diǎn)之間的距離差,為

(2)

信標(biāo)節(jié)點(diǎn)的坐標(biāo)已知,直接將第i個(gè)節(jié)點(diǎn)的坐標(biāo)表示為(xi,yi),將目標(biāo)節(jié)點(diǎn)的坐標(biāo)設(shè)為(x,y),由數(shù)學(xué)原理可建立雙曲線方程

(3)

將式(2)代入式(3)可解出這對(duì)雙曲線的交點(diǎn),即為目標(biāo)節(jié)點(diǎn)坐標(biāo)。上述雙曲線求解算法主要為Chan算法和Taylor算法[12]。求解目標(biāo)節(jié)點(diǎn)位置坐標(biāo)之后,為提高精確度再使用濾波算法濾除噪聲。

2 Chan-Taylor-IMMKF室內(nèi)定位算法

2.1 Chan-Taylor加權(quán)算法

Chan算法是非遞歸雙曲線方程組的一種解法[13],當(dāng)噪聲服從高斯分布時(shí)其具有定位精度大、誤差小的特點(diǎn),而且通過(guò)信標(biāo)節(jié)點(diǎn)的數(shù)量可以提高定位精度。Taylor算法是一種迭代算法[14],首先在泰勒級(jí)數(shù)展開(kāi)的基礎(chǔ)上使用初始迭代值進(jìn)行加權(quán)最小二乘法估計(jì),再解出位置估計(jì)誤差的局部最小二乘解,更新目標(biāo)節(jié)點(diǎn)的位置估計(jì)。

Chan算法和Taylor算法對(duì)誤差處理方式不同,計(jì)算復(fù)雜程度也不同,故將兩種算法的優(yōu)點(diǎn)結(jié)合提出Chan-Taylor加權(quán)算法。Chan-Taylor加權(quán)算法避免了Taylor算法由于初始值與真實(shí)值相差較大時(shí)分散的問(wèn)題,也減小了NLOS誤差定位精度的影響。算法具體實(shí)現(xiàn)方法為:先使用Chan算法解算TDOA測(cè)量值,將該位置估計(jì)值作為T(mén)aylor級(jí)數(shù)展開(kāi)算法的初始值進(jìn)行迭代,對(duì)位置進(jìn)行再次估計(jì)。為了減少級(jí)數(shù)發(fā)散問(wèn)題,將Chan算法解算的位置估計(jì)值和Taylor算法的位置估計(jì)值均進(jìn)行加權(quán)計(jì)算,最終得出位置估計(jì)值。具體加權(quán)過(guò)程[15]為:

1)計(jì)算解算結(jié)果與測(cè)量值之間的殘差

(4)

式中,N為信標(biāo)節(jié)點(diǎn)數(shù)量;X為位置估計(jì)值;Xi為第i個(gè)信標(biāo)節(jié)點(diǎn)坐標(biāo);

2)為了提高定位精度,將殘差歸一化

(5)

式中,第k種算法的加權(quán)系數(shù)為Rk。

3)加權(quán)計(jì)算得最終位置估計(jì)結(jié)果,如式(6)所示。

(6)

2.2 傳統(tǒng)卡爾曼濾波算法

卡爾曼濾波算法是一種最優(yōu)化自回歸數(shù)據(jù)處理算法,能夠估計(jì)平穩(wěn)的一維隨機(jī)過(guò)程,也能夠估計(jì)非平穩(wěn)的、多維隨機(jī)過(guò)程,且卡爾曼濾波算法遞推,易于在計(jì)算機(jī)上實(shí)現(xiàn)[16]?？柭鼮V波算法通過(guò)正態(tài)分布進(jìn)行遞推處理,對(duì)最優(yōu)值的估計(jì)分為兩個(gè)階段[17]:1)利用上一時(shí)刻的位置狀態(tài)對(duì)當(dāng)前時(shí)刻的位置進(jìn)行預(yù)測(cè)估計(jì);2)使用當(dāng)前時(shí)刻帶有噪聲的觀測(cè)值對(duì)估計(jì)值進(jìn)行修正。其具體實(shí)現(xiàn)方程如下:

預(yù)測(cè)過(guò)程為

(7)

(8)

(9)

(10)

更新過(guò)程分為3步,分別如式(11)～式(13)所示

(11)

(12)

Pk|k=(I-KkHk)Pk|k-1

(13)

2.3 Chan-Taylor-IMMKF定位算法

Chan-Taylor加權(quán)算法已達(dá)到相對(duì)準(zhǔn)確的定位精度,但仍存在環(huán)境誤差,采用卡爾曼濾波可以從概率統(tǒng)計(jì)角度估計(jì)并消除誤差,提高系統(tǒng)的定位性能?？柭鼮V波器是線性濾波器,基本原理是根據(jù)上一時(shí)刻系統(tǒng)狀態(tài)的預(yù)測(cè)狀態(tài)推測(cè)下一時(shí)刻系統(tǒng)的真實(shí)狀態(tài),故卡爾曼濾波器需要滿足線性條件。但當(dāng)定位對(duì)象忽然改變運(yùn)動(dòng)狀態(tài)時(shí),卡爾曼濾波可能會(huì)“滯后”影響定位結(jié)果。本文提出Chan-Taylor-IMMKF定位算法,使用自適應(yīng)算法——交互式多模型算法IMM來(lái)提高定位精度。IMM算法主使用多個(gè)卡爾曼濾波器并行處理數(shù)據(jù),每個(gè)濾波器對(duì)應(yīng)不同的狀態(tài)空間模型,最后通過(guò)有效加權(quán)融合進(jìn)行系統(tǒng)狀態(tài)估計(jì),克服了單模型估計(jì)誤差較大的問(wèn)題,提高了定位性能。Chan-Taylor-IMMKF的完整算法流程如圖5所示。

在圖3中,算法主要由兩部分組成,第一部分是Chan-Taylor加權(quán)算法解算目標(biāo)節(jié)點(diǎn)位置估計(jì),第二部分是IMMKF算法濾波。Chan-Taylor加權(quán)算法先通過(guò)Chan算法解算TDOA測(cè)量值,再將結(jié)果作為T(mén)aylor算法的初始迭代值繼續(xù)計(jì)算,最后將兩種算法的結(jié)果進(jìn)行殘差加權(quán)得出位置估計(jì)值。IMMKF算法使用多個(gè)對(duì)應(yīng)不同狀態(tài)空間模型的卡爾曼濾波器并行處理,并將Chan-Taylor加權(quán)算法得到的目標(biāo)節(jié)點(diǎn)位置估計(jì)值作為IMMKF算法的估計(jì)狀態(tài)[18]。IMM算法的主要思想是假設(shè)一個(gè)模型相對(duì)于其他模型在此刻有效,通過(guò)混合前一時(shí)刻所有濾波器狀態(tài)估計(jì)值,獲得與此特定模型相互匹配的濾波器初始條件,然后再對(duì)模型庫(kù)中的每個(gè)模型進(jìn)行常規(guī)濾波,并加權(quán)融合每個(gè)濾波器的輸出結(jié)果,得到最終狀態(tài)估計(jì)。具體實(shí)現(xiàn)步驟為:

圖3 Chan-Taylor-IMMKF算法流程Figure 3. Flow of Chan-Taylor-IMMkF algorithm

步驟1計(jì)算模型混合概率。假設(shè)目標(biāo)有r種運(yùn)動(dòng)狀態(tài),對(duì)應(yīng)r個(gè)運(yùn)動(dòng)模型,第j個(gè)模型表示的目標(biāo)狀態(tài)方程為

Xj(k+1)=Φj(k)Xj+Gj(k)Wj(k)

(14)

觀測(cè)方程為

Z(k)=H(k)X(k)+V(k)

(15)

式中,Wj(k)是過(guò)程噪聲,是均值為零、協(xié)方差矩陣為Qj的獨(dú)立多元正態(tài)分布。元素pij是目標(biāo)由第i個(gè)運(yùn)動(dòng)模型轉(zhuǎn)移到第j個(gè)運(yùn)動(dòng)模型的概率,通過(guò)馬爾可夫狀態(tài)轉(zhuǎn)移矩陣確定轉(zhuǎn)移矩陣,具體為

(16)

模型i的預(yù)測(cè)概率(歸一化常數(shù))為

(17)

模型i到模型j的混合概率如式(18)所示。

(18)

(19)

混合協(xié)方差矩陣為

(20)

步驟4模型概率更新。采用似然函數(shù)更新uj(k)

(21)

Λj(k)似然函數(shù)為

(22)

步驟5輸出交互?；谀Ｐ透怕?將每個(gè)濾波器濾波的估計(jì)狀態(tài)進(jìn)行加權(quán)合并,得到總估計(jì)狀態(tài)和協(xié)方差矩陣,輸出最終結(jié)果即為目標(biāo)節(jié)點(diǎn)的估計(jì)坐標(biāo)。

計(jì)算總估計(jì)狀態(tài)為

(23)

協(xié)方差矩陣為

(24)

對(duì)所有濾波器的結(jié)果進(jìn)行加權(quán)平均,最終計(jì)算的值即為濾波器的輸出,其中權(quán)重為模型概率。

3 計(jì)算結(jié)果分析

使用MATLAB軟件對(duì)本文相關(guān)算法進(jìn)行仿真,全面分析算法性能。

3.1 Chan-Taylor加權(quán)算法仿真

對(duì)Chan-Taylor加權(quán)算法進(jìn)行仿真處理,查看定位效果。仿真場(chǎng)地為500 m×500 m,信標(biāo)節(jié)點(diǎn)的布置遵從蜂窩網(wǎng)絡(luò)原則,具體設(shè)為(0,0)、(500,0)、(500,500)、(0,500),如圖4所示,目標(biāo)節(jié)點(diǎn)的位置坐標(biāo)是(50,100)。不考慮NLOS誤差,僅考慮TDOA測(cè)量值誤差,設(shè)定測(cè)量值誤差為均值為0且標(biāo)準(zhǔn)差分別0.2、0.4、0.6、0.8、1.0的高斯噪聲。

圖4 目標(biāo)與信標(biāo)節(jié)點(diǎn)分布Figure 4. Distribution of target and beacon nodes

圖5給出不同算法的平均定位誤差標(biāo)準(zhǔn)差與噪聲誤差之間的關(guān)系。Chan-Taylor加權(quán)算法的定位誤差比單純Chan算法小,并且接近以真實(shí)值為初始迭代值的Taylor算法。雖然隨著差的增大其定位誤差依然增加,但整體定位效果有所提升。

圖5 3種算法誤差標(biāo)準(zhǔn)差Figure 5. Error standard deviation of three algorithms

3.2 Chan-Taylor-IMMKF定位算法仿真

3.2.1 單種運(yùn)動(dòng)狀態(tài)仿真

使用卡爾曼濾波算法進(jìn)一步提升定位效果,主要仿真查看動(dòng)態(tài)性能。仿真場(chǎng)地為5 000 cm×5 000 cm,信標(biāo)節(jié)點(diǎn)具體設(shè)為(0,0)、(5 000,0)、(5 000,5 000)、(0,5 000),目標(biāo)為動(dòng)態(tài),目標(biāo)節(jié)點(diǎn)的初始節(jié)點(diǎn)坐標(biāo)為(50,100)。運(yùn)動(dòng)狀態(tài)為勻速運(yùn)動(dòng),設(shè)x方向的速度均為2 cm·s-1,y方向的速度為20 cm·s-1,在目標(biāo)運(yùn)動(dòng)過(guò)程中加入均值0的高斯過(guò)程噪聲。TDOA測(cè)量值誤差設(shè)定仍為均值為0,標(biāo)準(zhǔn)差分別0.2、0.4、0.6、0.8、1.0的高斯噪聲,每次仿真次數(shù)為120次。使用卡爾曼濾波算法處理Chan-Taylor加權(quán)算法的估計(jì)值,觀察結(jié)果。

圖6為是否使用卡爾曼濾波算法的位置估計(jì)曲線圖,可看出融合卡爾曼濾波算法之后的軌跡更接近真實(shí)軌跡,而Chan-Taylor加權(quán)算法的軌跡波動(dòng)較大,表明Kalman濾波器可以有效減小誤差來(lái)提高精度。圖7為兩種算法的定位誤差對(duì)比,可看出Chan-Taylor加權(quán)算法最大誤差值達(dá)到了42 cm,而卡爾曼濾波算法的誤差值在15 cm以內(nèi),定位效果有所改善。

圖6 目標(biāo)節(jié)點(diǎn)軌跡對(duì)比Figure 6. Trajectory comparison of target nodes

圖7 目標(biāo)節(jié)點(diǎn)定位誤差對(duì)比Figure 7. Comparison of location errors of target nodes

3.2.2 多種運(yùn)動(dòng)狀態(tài)仿真

圖8為目標(biāo)節(jié)點(diǎn)運(yùn)動(dòng)軌跡對(duì)比,可看出在0～400 s內(nèi),卡爾曼濾波的濾波軌跡接近真實(shí)軌跡。在400 s處目標(biāo)節(jié)點(diǎn)快轉(zhuǎn)彎改變運(yùn)動(dòng)狀態(tài),卡爾曼濾波軌跡偏離真實(shí)軌跡。目標(biāo)節(jié)點(diǎn)回歸一種運(yùn)動(dòng)狀態(tài)后,卡爾曼濾波軌跡逐漸回歸真實(shí)軌跡。與卡爾曼濾波軌跡相比,Chan-Taylor-IMMKF算法的濾波軌跡在0～600 s時(shí)間內(nèi)全程靠近真實(shí)軌跡,在600 s處出現(xiàn)短暫偏離也可及時(shí)修正,表明Chan-Taylor-IMMKF算法可以較好地處理誤差。由兩種算法的濾波軌跡對(duì)比可看出,Chan-Taylor-IMMKF算法能夠較好地避免因?yàn)槟繕?biāo)忽然改變運(yùn)動(dòng)狀態(tài)卡爾曼濾波算法滯后不能及時(shí)跟蹤的問(wèn)題,其濾波軌跡更接近真實(shí)軌跡。

圖8 目標(biāo)節(jié)點(diǎn)軌跡對(duì)比Figure 8. Trajectory comparison of target nodes

圖9為傳統(tǒng)卡爾曼算法與Chan-Taylor-IMMKF算法對(duì)x、y方向獨(dú)立觀測(cè)的誤差對(duì)比。由誤差曲線可看出在0～400 s內(nèi),卡爾曼濾波與IMMKF濾波的效果均可達(dá)到定位需求,在400 s處卡爾曼濾波算法定位誤差開(kāi)始大幅增加,到運(yùn)動(dòng)停止未恢復(fù); Chan-Taylor-IMMKF算法在600 s處x、y方向均出現(xiàn)較大誤差,但其在較短時(shí)間內(nèi)恢復(fù)濾波效果,整體滿足定位需求。

圖9 濾波誤差的均值曲線 Figure 9. Mean curve of filtering error

圖10為Chan-Taylor算法未濾波與Chan-Taylor-IMMKF算法濾波的誤差標(biāo)準(zhǔn)差對(duì)比,可以看出在600 s處誤差較大,但可及時(shí)修正并快速恢復(fù)濾波效果,整體上濾波處理后的誤差小于不濾波處理的誤差。表1為誤差結(jié)果具體數(shù)值對(duì)比,使用融合交互式多模型的卡爾曼濾波算法后的誤差均值為之前的一半,誤差均值控制在15 cm以內(nèi),說(shuō)明濾波效果顯著。雖然本文所提方法的效果小于勻速運(yùn)動(dòng)時(shí)卡爾曼濾波算法,但多種運(yùn)動(dòng)狀態(tài)時(shí)該誤差符合定位需求,相比于單獨(dú)卡爾曼濾波算法,IMMKF濾波軌跡更接近于真實(shí)軌跡。

表1 定位誤差對(duì)比

圖10 目標(biāo)節(jié)點(diǎn)濾波誤差對(duì)比 Figure 10. Comparison of filtering errors of target nodes

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

本文分析了基于TDOA定位模型解算目標(biāo)節(jié)點(diǎn)位置估計(jì)的Chan算法和Taylor算法,針對(duì)Chan算法由于NLOS精度降低與Taylor算法發(fā)散問(wèn)題提出了加權(quán)Chan-Taylor算法,將Chan算法的估計(jì)值作為T(mén)aylor算法的初始迭代值,將兩個(gè)算法的估計(jì)值進(jìn)行殘差加權(quán)得到融合估計(jì)值,提高了定位精度,降低了NLOS誤差,使誤差標(biāo)準(zhǔn)差接近于以真實(shí)值迭代的Taylor算法。針對(duì)傳統(tǒng)卡爾曼濾波器由于目標(biāo)節(jié)點(diǎn)忽然改變運(yùn)動(dòng)狀態(tài)產(chǎn)生“滯后”問(wèn)題,提出了Chan-Taylor-IMMKF算法,將殘差加權(quán)Chan-Taylor算法的估計(jì)值作為IMMKF的測(cè)量值。實(shí)驗(yàn)結(jié)果表明,該算法可以較好地避免目標(biāo)改變運(yùn)動(dòng)狀態(tài)不能及時(shí)跟蹤的問(wèn)題,將誤差均值控制在15 cm左右,提高了定位性能。該方法具有普適性,可廣泛用于室內(nèi)定位與機(jī)動(dòng)目標(biāo)跟蹤。在后續(xù)研究中,可對(duì)其繼續(xù)進(jìn)行優(yōu)化,提高精度,并將其應(yīng)用于多目標(biāo)跟蹤定位。