基于改進(jìn)IMM運(yùn)動(dòng)模型的室內(nèi)定位算法*

賀軍義， 郭 凱， 張 敏

(1.河南理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，河南 焦作 454002； 2.河南理工大學(xué) 教務(wù)處,河南 焦作 454002)

0 引 言

由于GPS信號(hào)在室內(nèi)復(fù)雜環(huán)境中受到遮擋而迅速衰減，導(dǎo)致該技術(shù)無法有效地應(yīng)用到室內(nèi)定位領(lǐng)域[1]。近年來，國(guó)內(nèi)外學(xué)者對(duì)室內(nèi)定位技術(shù)展開了大量的研究。室內(nèi)定位方法種類繁多，如基于到達(dá)角度(angle of arrival,AOA)、基于接收信號(hào)強(qiáng)度(receive signal strength,RSS)、基于到達(dá)時(shí)間差(time difference of arrival,TDOA)。由于TDOA算法具有成本低、效率高等特點(diǎn)故成為室內(nèi)定位最常用的算法[2]。

對(duì)定位數(shù)據(jù)處理是機(jī)動(dòng)目標(biāo)跟蹤中的關(guān)鍵環(huán)節(jié)，要想實(shí)現(xiàn)對(duì)目標(biāo)穩(wěn)定精確的跟蹤首先要準(zhǔn)確地預(yù)估目標(biāo)的運(yùn)動(dòng)模式[3]。經(jīng)過幾十年的發(fā)展，傳統(tǒng)卡爾曼濾波算法已經(jīng)衍生出多種版本。單模型(single model,SM)濾波器在線性高斯模型條件下得到較好的跟蹤效果，但很難精確地適應(yīng)整個(gè)運(yùn)動(dòng)過程，容易造成濾波器的發(fā)散[4]?；谠搯栴}有關(guān)學(xué)者后來提出了交互式多模型(interactive multi-model,IMM)算法，該算法使用多個(gè)機(jī)動(dòng)或非機(jī)動(dòng)模型來描述工作過程中可能的狀態(tài)，最后通過有效加權(quán)融合進(jìn)行系統(tǒng)狀態(tài)估計(jì)，較好地解決了SM估計(jì)誤差較大的問題。但固定結(jié)構(gòu)多模型(fixed structure multiple model,FSMM)算法的模型集是固定的，只有增加模型數(shù)量才能提高跟蹤精度；變結(jié)構(gòu)多模型(variable structure multiple model, VSMM)通過切換不同模型組使得模型集動(dòng)態(tài)變化[5]。無論FSMM還是VSMM，提高跟蹤精度必定會(huì)增加運(yùn)算量，降低跟蹤的實(shí)時(shí)性[6]。因此，SM算法和IMM算法都存在特定缺陷。

針對(duì)傳統(tǒng)IMM算法融合模型集過多導(dǎo)致效率降低的問題，本文提出了一種基于改進(jìn)IMM運(yùn)動(dòng)模型的融合Singer的交互式多模型(Singer-insetted interactive multiple model,SIMM)跟蹤算法。該算法只需融合非機(jī)動(dòng)模型和基于加速度為時(shí)間相關(guān)的Singer模型，SIMM算法不僅能夠根據(jù)目標(biāo)的實(shí)際運(yùn)動(dòng)狀態(tài)實(shí)時(shí)地調(diào)整模型概率，選擇與目標(biāo)運(yùn)動(dòng)相匹配的模型，還能根據(jù)目標(biāo)機(jī)動(dòng)強(qiáng)弱來調(diào)整Singer模型的參數(shù)。各模型之間的轉(zhuǎn)移由Markov轉(zhuǎn)移矩陣確定。本文將在高機(jī)動(dòng)運(yùn)動(dòng)情景下通過對(duì)比IMM算法和SIMM算法來驗(yàn)證SIMM算法的性能。

1 TDOA定位模型

TDOA又稱作雙曲線定位方案，其定位原理如圖1所示[7]。基于雙曲線的數(shù)學(xué)特性，以兩個(gè)定位基站為雙曲線焦點(diǎn)，基站位置坐標(biāo)已知，以標(biāo)簽到兩基站的距離差為實(shí)軸畫一對(duì)雙曲線。同理，再次選取其他基站也可與主基站畫出雙曲線，移動(dòng)標(biāo)簽的坐標(biāo)為兩曲線相交的位置。TDOA量測(cè)需要定位系統(tǒng)通過待測(cè)節(jié)點(diǎn)向各定位基站發(fā)送無線電脈沖，計(jì)算出待測(cè)節(jié)點(diǎn)到達(dá)從基站與主基站的時(shí)間差，進(jìn)而得到距離差進(jìn)行定位。

圖1 TDOA定位原理

將移動(dòng)標(biāo)簽坐標(biāo)設(shè)為(x,y)，運(yùn)動(dòng)環(huán)境內(nèi)裝N(N>3)個(gè)定位基站，坐標(biāo)為Pi(xi,yi)，i= 1,2,…,N。主基站為1號(hào)基站，從基站與主基站進(jìn)行數(shù)學(xué)計(jì)算得出TDOA值[8]

di,1=cti,1=di-d1,i= 1,2,…,N

(1)

式中di為第i號(hào)基站到移動(dòng)標(biāo)簽距離值；ti,1為移動(dòng)標(biāo)簽到i號(hào)基站和1號(hào)基站的時(shí)間差值，將時(shí)間差值與光速c相乘得到其距離差值di,1。另根據(jù)二維空間的歐氏距離公式有

(2)

(3)

式中xi,1=xi-x1為基站i與基站1橫坐標(biāo)的差值；yi,1=yi-y1為基站i與基站1縱坐標(biāo)的差值。顯然式(3)是關(guān)于未知數(shù)x,y和d1的線性方程。

2 SIMM算法工作原理

獲取到TDOA量測(cè)定位結(jié)果之后，需要對(duì)運(yùn)動(dòng)標(biāo)簽進(jìn)行跟蹤。由于目標(biāo)運(yùn)動(dòng)的不確定性，無法通過單一的運(yùn)動(dòng)模型描述其實(shí)際的運(yùn)動(dòng)狀態(tài)。本文將建立運(yùn)動(dòng)環(huán)境信息與運(yùn)動(dòng)模式預(yù)測(cè)、模型集之間的關(guān)系，并應(yīng)用于IMM算法。

2.1 運(yùn)動(dòng)模型設(shè)計(jì)

如何對(duì)加速度進(jìn)行準(zhǔn)確可靠地描述是解決機(jī)動(dòng)目標(biāo)建模問題的關(guān)鍵。本文將Singer模型融合到IMM模型集當(dāng)中，較好地解決了預(yù)估目標(biāo)運(yùn)動(dòng)狀態(tài)不準(zhǔn)確的問題。在Singer模型中，假設(shè)機(jī)動(dòng)目標(biāo)的加速度a(t)服從一階時(shí)間相關(guān)過程，a(t)的時(shí)間相關(guān)函數(shù)為Ra(τ)，可以表述為指數(shù)衰減形式[9]如式(4)所示

(4)

(5)

式中Amax為機(jī)動(dòng)目標(biāo)加速度的極大值；Pmax為機(jī)動(dòng)目標(biāo)加速度為極大值時(shí)發(fā)生概率；P0為目標(biāo)非機(jī)動(dòng)時(shí)的發(fā)生概率。應(yīng)用維納—柯爾莫哥洛夫白化法對(duì)加速度a(t)的時(shí)間相關(guān)函數(shù)Ra(τ)進(jìn)行白化后，機(jī)動(dòng)目標(biāo)的加速度a(t)可以用一階時(shí)間相關(guān)的白噪聲模型表示，如式(6)所示[11]

X(k+1)=F(k)X(k)+V(k)

(6)

不難看出，Singer模型本質(zhì)上是一種先驗(yàn)思想。

2.2 跟蹤算法設(shè)計(jì)

3 SIMM算法實(shí)驗(yàn)與分析

3.1 實(shí)驗(yàn)設(shè)置

在室內(nèi)環(huán)境中，定位目標(biāo)的實(shí)際運(yùn)動(dòng)狀態(tài)通常包含多種，其中包括勻速運(yùn)動(dòng)、勻加速運(yùn)動(dòng)、變加速運(yùn)動(dòng)等。本文對(duì)此要求在室內(nèi)環(huán)境設(shè)計(jì)了路徑實(shí)驗(yàn)對(duì)比檢驗(yàn)傳統(tǒng)IMM與SIMM的跟蹤效果，預(yù)設(shè)路徑及基站分布如圖2所示。

圖2 實(shí)驗(yàn)環(huán)境及路線設(shè)計(jì)

實(shí)驗(yàn)區(qū)域設(shè)置在25 m×12 m的室內(nèi)環(huán)境下，本實(shí)驗(yàn)使用4基站和1標(biāo)簽采集定位數(shù)據(jù)，4個(gè)基站分別分布在實(shí)驗(yàn)區(qū)域的4個(gè)頂點(diǎn)，并將(0,0)處基站設(shè)為主基站。首先，運(yùn)動(dòng)標(biāo)簽按預(yù)定路線從點(diǎn)A(20,120)處沿y軸負(fù)方向運(yùn)動(dòng)8.5 m；緊接標(biāo)簽從點(diǎn)B(20,35)處開始做變加速運(yùn)動(dòng)；最后，標(biāo)簽從點(diǎn)C(190,37)處沿x軸正向做勻速直線運(yùn)動(dòng)。

3.2 運(yùn)動(dòng)模型選擇

傳統(tǒng)IMM算法同樣融合3個(gè)運(yùn)動(dòng)模型：模型一為非機(jī)動(dòng)模型，過程噪聲方差為0；模型二選取機(jī)動(dòng)模型，系統(tǒng)擾動(dòng)噪聲方差為Q=0.001I2×2；模型三選取機(jī)動(dòng)模型，系統(tǒng)擾動(dòng)噪聲方差為Q=0.014 4I2×2，各算法運(yùn)動(dòng)模型組合設(shè)計(jì)如表1所示。為控制實(shí)驗(yàn)變量的唯一，兩種算法控制模型轉(zhuǎn)換的Markov鏈轉(zhuǎn)移概率矩陣都為

設(shè)定各模型在采樣開始時(shí)刻的概率分別為μ1=0.8,μ2=0.1,μ3=0.1。

表1 模型組合設(shè)計(jì)

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

實(shí)驗(yàn)測(cè)定了運(yùn)動(dòng)標(biāo)簽在實(shí)驗(yàn)場(chǎng)地中的運(yùn)動(dòng)軌跡，將SIMM算法與傳統(tǒng)IMM算法進(jìn)行比較并分析了實(shí)驗(yàn)結(jié)果，實(shí)驗(yàn)結(jié)果如圖3～圖5所示。本文采用位置的誤差均值和誤差標(biāo)準(zhǔn)差來評(píng)估算法性能。

定義濾波誤差均值

定義濾波誤差標(biāo)準(zhǔn)差

圖3展示了標(biāo)簽運(yùn)動(dòng)的真實(shí)軌跡和觀測(cè)軌跡還有兩種算法的濾波軌跡。結(jié)果表明，在沿y軸運(yùn)動(dòng)初期，兩種濾波算法對(duì)于目標(biāo)誤差的消除作用是非常明顯的，其軌跡相比于觀測(cè)值更加平滑，而且兩種算法的濾波效果并無明顯差別。但在坐標(biāo)(30,20)，(110,80)，(180,30)dm處濾波值相對(duì)于真實(shí)值都產(chǎn)生了“滯后”現(xiàn)象，甚至短時(shí)間內(nèi)造成誤差大于觀測(cè)值。這是由于標(biāo)簽出現(xiàn)急轉(zhuǎn)彎、急停等高機(jī)動(dòng)運(yùn)動(dòng)時(shí)算法融合的運(yùn)動(dòng)模型無法立即適應(yīng)目標(biāo)的運(yùn)動(dòng)狀態(tài)，使得干擾信號(hào)統(tǒng)計(jì)學(xué)特性發(fā)生了改變。由圖3可以看出，在濾波出現(xiàn)“滯后”現(xiàn)象后傳統(tǒng)IMM算法會(huì)逐漸恢復(fù)其功能，但短時(shí)間內(nèi)會(huì)造成極大誤差；SIMM算法雖然在轉(zhuǎn)彎時(shí)也出現(xiàn)了“滯后”現(xiàn)象，但相比較傳統(tǒng)IMM算法，其造成的誤差更小，并且收斂速度明顯快于傳統(tǒng)IMM算法。

圖3 不同算法濾波軌跡

圖4展示了不同坐標(biāo)軸下兩種濾波算法的估計(jì)誤差均值?？梢钥闯鰺o論是x軸還是y軸SIMM算法的濾波精度都明顯高于傳統(tǒng)IMM算法，當(dāng)目標(biāo)轉(zhuǎn)彎出現(xiàn)濾波發(fā)散時(shí)前者的收斂速度也更快，而且在標(biāo)簽做加速運(yùn)動(dòng)時(shí)SIMM的濾波效果更為顯著。

圖4 不同算法估計(jì)誤差均值

圖5給出了不同算法的估計(jì)誤差標(biāo)準(zhǔn)差，該指標(biāo)反映了兩種算法濾波結(jié)果的平滑程度。由該圖可以得出，當(dāng)采用SIMM算法時(shí)，相比于傳統(tǒng)IMM算法，前者的跟蹤效果更加穩(wěn)定，濾波軌跡更加平穩(wěn)。

圖5 不同算法估計(jì)誤差標(biāo)準(zhǔn)差

表2數(shù)據(jù)是對(duì)所有采樣點(diǎn)的濾波結(jié)果求誤差均值和誤差標(biāo)準(zhǔn)差，分析表中數(shù)據(jù)可以得出無論是濾波精度還是濾波穩(wěn)定性SIMM算法都有一定的優(yōu)勢(shì)。

綜上所述相比于傳統(tǒng)IMM算法，顯然文中提出的SIMM算法濾波效果更優(yōu)。

表2 兩種算法對(duì)移動(dòng)標(biāo)簽定位精度比較

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

對(duì)于機(jī)動(dòng)目標(biāo)而言，目標(biāo)運(yùn)動(dòng)狀態(tài)的快速轉(zhuǎn)變會(huì)導(dǎo)致濾波算法運(yùn)動(dòng)模型匹配失效，短時(shí)間內(nèi)會(huì)急劇增加濾波誤差。為提高跟蹤精度，一方面研究了如何減小初始定位誤差，本文利用TDOA算法精度較高和計(jì)算復(fù)雜度較低的特點(diǎn)，有效降低了定位時(shí)產(chǎn)生的誤差；另一方面研究了如何優(yōu)化IMM算法的適應(yīng)性和精確度，在傳統(tǒng)IMM算法的基礎(chǔ)上，結(jié)合Singer模型的自適應(yīng)性特點(diǎn)，提出了SIMM算法，進(jìn)一步提高了對(duì)機(jī)動(dòng)目標(biāo)的跟蹤性能。實(shí)驗(yàn)表明，本文提出的算法能夠有效降低目標(biāo)在強(qiáng)機(jī)動(dòng)運(yùn)動(dòng)狀態(tài)下定位的誤差。但算法仍然具有局限性，由于TDOA算法的特性，當(dāng)實(shí)驗(yàn)在非視距環(huán)境下定位誤差較視距環(huán)境下會(huì)變大，該問題將在下一步工作中解決。