引入強(qiáng)跟蹤濾波器的IMM算法在導(dǎo)航定位解算中的應(yīng)用

劉宇璽,吳 鵬,劉文祥,王飛雪

(國防科學(xué)技術(shù)大學(xué)電子科學(xué)與工程學(xué)院衛(wèi)星導(dǎo)航定位技術(shù)工程研究中心,湖南 長沙 410073)

0 引 言

目前,全球四大衛(wèi)星導(dǎo)航系統(tǒng)逐步發(fā)展完善,已有的北斗系統(tǒng)(BDS)、GPS和GLONASS三個全球?qū)Ш较到y(tǒng)已經(jīng)能夠提供導(dǎo)航定位服務(wù)[1]。但是,隨著導(dǎo)航終端產(chǎn)品應(yīng)用范圍越來越廣,應(yīng)用場景日趨復(fù)雜多樣化,即便是比較成熟的GPS導(dǎo)航系統(tǒng)也會出現(xiàn)定位效果較差的時候。例如,對于導(dǎo)航終端大范圍應(yīng)用的城市機(jī)動載體,經(jīng)常工作在多動態(tài)環(huán)境下,而且對導(dǎo)航定位系統(tǒng)的精度和可靠性要求也較高,運(yùn)用單一運(yùn)動模型已經(jīng)不能有效的估計(jì)載體的運(yùn)動狀態(tài)。近年來,學(xué)者們開始嘗試?yán)枚鄠€模型進(jìn)行估計(jì),最早出現(xiàn)的是變維濾波算法,用低階模型估計(jì)非機(jī)動狀態(tài),用高階模型估計(jì)機(jī)動狀態(tài),模型之間屬于硬切換,這種方法往往帶有機(jī)動檢測與切換的滯后性[2]。交互式多模型[4](IMM)則是一種基于軟切換的目標(biāo)跟蹤方法。交互式多模型方法根據(jù)載體不同的運(yùn)動狀態(tài)應(yīng)用不同的運(yùn)動模型,各個模型濾波器之間通過估計(jì)狀態(tài)組合實(shí)現(xiàn)交互,而模型之間基于一個馬爾科夫鏈進(jìn)行切換。最后不同模型濾波器相互融合得到最終估計(jì)結(jié)果。

本文利用“CV”模型和“當(dāng)前”模型作為載體狀態(tài)估計(jì)的基礎(chǔ)模型。兩種模型分別可以有效應(yīng)對載體低動態(tài)運(yùn)動和高動態(tài)運(yùn)動,考慮到EKF濾波自身魯棒性較差,容易受到參數(shù)設(shè)置的影響,本文將強(qiáng)跟蹤濾波器引入交互式多模型算法中。針對EKF的缺陷,文獻(xiàn)[5]中提出的基于強(qiáng)跟蹤濾波器(STF)的“當(dāng)前”統(tǒng)計(jì)模型自適應(yīng)算法具有更好的機(jī)動跟蹤性能。強(qiáng)跟蹤濾波器由于漸消因子的引入,與標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波器(EKF)相比有較強(qiáng)的魯棒性,極強(qiáng)的跟蹤能力,適中的計(jì)算復(fù)雜性。由于漸消因子的作用,強(qiáng)跟蹤濾波器保持了不同時刻濾波殘差序列處處正交。當(dāng)存在模型不確定性時,通過在線調(diào)整增益可以使得殘差保持高斯白噪聲的性質(zhì)。

本文主要論述了交互式多模型的基本原理,將強(qiáng)跟蹤濾波器引入交互式多模型并應(yīng)用于導(dǎo)航定位解算中,可以應(yīng)對城市機(jī)動載體復(fù)雜多變的運(yùn)動狀態(tài)。仿真結(jié)果證明：相比于單模型估計(jì)和標(biāo)準(zhǔn)的IMM算法,本文提出的引入強(qiáng)跟蹤濾波器的交互式多模型算法能夠提高定位精度。

1 引入強(qiáng)跟蹤濾波器的交互式多模型原理

交互式多模型算法主要包括五個步驟:

1)輸入交互

uk-1/k-1(j/i),

(1)

1)T)]uj/i(k-1/k-1)，

(2)

其中:

(3)

(4)

2)模型估計(jì)

兩個濾波器的似然函數(shù)為

Λi(k)=P[Z(k)/Mi(k),Zk-1].

(5)

由初始混合條件和協(xié)方差可以得到

(6)

其中:

(7)

3)模型概率更新

(8)

(9)

4)融合輸出

(10)

(11)

以上就是標(biāo)準(zhǔn)IMM算法的基本原理,標(biāo)準(zhǔn)IMM算法通常采用的都是擴(kuò)展卡爾曼濾波器。這里類似于單個濾波器的卡爾曼濾波過程,不同的是需要計(jì)算兩個模型?？紤]到EKF跟蹤強(qiáng)機(jī)動目標(biāo)時魯棒性較差的缺陷,本文在標(biāo)準(zhǔn)IMM算法的基礎(chǔ)上引入強(qiáng)跟蹤濾波器。

強(qiáng)跟蹤濾波器具體過程為

(12)

(13)

(14)

(15)

Pk=[Ι-KkHk]Pk|k-1.

(16)

由上式可以看出,相比于EKF濾波器,強(qiáng)跟蹤濾波器在擴(kuò)展卡爾曼濾波器的估計(jì)誤差協(xié)方差陣中引入次優(yōu)漸消矩陣,對不同數(shù)據(jù)通道進(jìn)行漸消,使不同時刻的殘差序列處處正交,使得濾波器能夠較好的保持對強(qiáng)機(jī)動目標(biāo)實(shí)際狀態(tài)的跟蹤。漸消因子的具體計(jì)算過程為

(17)

N(k)=Z0(k)-βR(k)-

H(k)Q(k-1)HT(k),

(18)

M(k)=H(k)φ(k-1)P(k)φT(k-1)HT(k),

(19)

(20)

其中:ρ為遺忘因子,通常取值在[0,1]之間，本文中取值為0.9;β為弱化因子,根據(jù)經(jīng)驗(yàn)取值,在本文中取值為1;而V(1)為初始?xì)埐钪?。從上述公式可以看?當(dāng)載體運(yùn)動于一般機(jī)動時,預(yù)測值和估計(jì)值相差不大,此時漸消因子較小,強(qiáng)跟蹤濾波器就退化為標(biāo)準(zhǔn)EKF濾波器;當(dāng)運(yùn)動狀態(tài)發(fā)生突變時,濾波殘差增大導(dǎo)致漸消因子增大,濾波增益得到自適應(yīng)調(diào)節(jié),迫使殘差近似正交,從而提高狀態(tài)變化時的跟蹤性能。

2 濾波模型

本文所使用的IMM算法包含兩個基礎(chǔ)運(yùn)動模型,“CV”模型和“當(dāng)前”模型。

對于載體靜止和低動態(tài)時,通常采用恒定速度模型,即“CV”模型,“CV”模型一般都進(jìn)行勻速直線運(yùn)動,因此可將運(yùn)動載體的轉(zhuǎn)彎、大氣湍流而引起的加速度看作是勻速直線運(yùn)動中的攝動,在運(yùn)動模型中,將這種攝動作為隨機(jī)噪聲輸入。以一維直線運(yùn)動為例,CV模型的離散時間系統(tǒng)狀態(tài)方程為

(21)

對于載體高動態(tài)時,通常采用“當(dāng)前”模型匹配。由于在實(shí)際情況中載體的運(yùn)動并不完全屬于等速或等加速范圍內(nèi),對此,Zhou和Kumar提出了一種“當(dāng)前”統(tǒng)計(jì)模型[6],這種模型認(rèn)為當(dāng)目標(biāo)以某一加速度機(jī)動時,下一時刻的加速度值是有限的,并且只能在當(dāng)前加速度的某一鄰域范圍內(nèi)。運(yùn)動載體的加速度可表示為

(21)

故系統(tǒng)狀態(tài)方程可以表示為

(22)

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

圖1和圖2是X軸和Y軸位置誤差圖,圖3和圖4是載體在機(jī)動階段的局部仿真結(jié)果圖。0～40 s,載體處于勻速運(yùn)動狀態(tài);40～50 s,載體以加速度ax=-6 m/s2,ay=-4 m/s2做轉(zhuǎn)彎運(yùn)動;50～145 s,載體做勻速運(yùn)動;145～160 s,載體再次做轉(zhuǎn)彎運(yùn)動,ax=4.5 m/s2,ay=3.7 m/s2;之后50 s內(nèi)載體做勻速運(yùn)動;在210～230 s和265～285 s載體再次進(jìn)行兩次加速轉(zhuǎn)彎運(yùn)動;之后載體一直保持勻速運(yùn)動。由圖示定位結(jié)果可以看出,本文提出的引入強(qiáng)跟蹤濾波器的交互式多模型算法相比于CV模型在載體機(jī)動階段定位誤差較小,同時克服了“當(dāng)前”模型在載體處于勻速運(yùn)動等低動態(tài)下跟蹤精度下降的缺點(diǎn),強(qiáng)跟蹤濾波器的引入使得載體對機(jī)動變化有了更好的魯棒性。表1示出了利用不同方法得到的定位結(jié)果的位置誤差方差值,在本組試驗(yàn)數(shù)據(jù)中,改進(jìn)的交互式多模型算法相比于“當(dāng)前”模型濾波和標(biāo)準(zhǔn)交互式多模型算法,分別可以將徑向誤差方差從19.11和16.78降低到13.24.

圖1 X軸誤差

圖2 Y軸誤差

圖3 機(jī)動局部放大圖1

圖4 機(jī)動局部放大圖2

表1方差統(tǒng)計(jì)表

模 型X誤差方差Y誤差方差徑向-誤差方差 CV模型48.0128.5755.87 CS模型12.1814.7219.11 標(biāo)準(zhǔn)IMM10.6812.9416.78 改進(jìn)IMM算法7.3411.2113.24

圖5 模型概率圖

圖5示出了改進(jìn)算法在處理過程中兩種基礎(chǔ)模型概率的變化曲線,從圖中可以看出,根據(jù)算法原理,隨著運(yùn)動過程中載體機(jī)動狀態(tài)的不斷變化,不同模型匹配概率也隨之自適應(yīng)變化;圖6示出了仿真實(shí)驗(yàn)中載體運(yùn)動軌跡圖,為了更加貼近載體實(shí)際運(yùn)動狀態(tài),全過程融合了勻速、加速和轉(zhuǎn)彎運(yùn)動三種運(yùn)動狀態(tài)。

圖6 載體運(yùn)動軌跡圖2

4 結(jié)束語

本文將機(jī)動目標(biāo)跟蹤領(lǐng)域應(yīng)用廣泛的交互式多模型算法引入到導(dǎo)航定位中,并結(jié)合Kalman濾波闡述了改進(jìn)的IMM算法流程。利用信號源仿真機(jī)動載體運(yùn)動軌跡對引進(jìn)強(qiáng)跟蹤濾波器的交互式多模型的定位性能進(jìn)行分析,通過實(shí)驗(yàn)結(jié)果可以得到以下結(jié)論:

1)相比于單模型濾波,改進(jìn)的交互式多模型可以在不增加較多復(fù)雜度情況下應(yīng)對不同的動態(tài)場景,有效綜合了兩種基礎(chǔ)模型的優(yōu)點(diǎn)。

2)強(qiáng)跟蹤濾波器的引入可以使得濾波器在載體處于高動態(tài)和動態(tài)轉(zhuǎn)換階段有著更好的魯棒性。

3)交互式多模型算法中模型轉(zhuǎn)移概率的獲取是決定濾波精度的關(guān)鍵因素,如何更好的自適應(yīng)載體運(yùn)動狀態(tài)實(shí)時更新模型轉(zhuǎn)移概率是下一步研究的重點(diǎn)。

[1]KAPLAN E D. GPS原理與應(yīng)用 [M]. 2 版. 寇艷紅譯,北京:電子工業(yè)出版社,2006.

[2]NDJENG A,GRUYER D,GLASER S.A Improvement of the proprioceptive-sensors based EKF and IMM localization[C]//The International IEEE Conference on Intelligent Transportation Systems,2008:900-905.

[3]劉貴喜,高恩克,范春宇.改進(jìn)的交互式多模型粒子濾波跟蹤算法[J].電子信息學(xué)報,2007,29(12):1009-5896.

[4]LI Rongx, JILKOV V P. Survey of maneuvering target tracking part v: Multiple-model methods[J].IEEE Transactions on Aerospace and Electronic Systems,2005,41(4):1254-1280.

[5]范小軍,劉 峰,秦 勇,等.基于STF的“當(dāng)前”統(tǒng)計(jì)模型及自適應(yīng)跟蹤算法[J].電子學(xué)報,2006,34(6):981-984.

[6]JWO Dahjing, TSENG Chienhao.GPS navigation processing using the IMM-based EKF.[C]//3rdInternational Conference on Sensing Technology,2008.

[7]羅笑冰,王宏強(qiáng),黎 湘.模型轉(zhuǎn)移概率自適應(yīng)的交互式多模型跟蹤算法[J].電子信息學(xué)報，2005,10(10):1593-1595.

[8]ZHOU Hongren, KUMAR K S P. A current statistical model and adaptive algorithm for estimating maneuvering targets. AIAA.[J].Journal of Guidance, Control and Dynamics, 1984, 7(5):596-602.

[9]劉建書,李人厚,劉云龍.基于“當(dāng)前”統(tǒng)計(jì)模型的交互式多模型算法[J].系統(tǒng)工程與電子技術(shù),2008，30(7):1001-506.

[10]徐 敏,金以慧,楊瑞娟.基于強(qiáng)跟蹤濾波器的多目標(biāo)跟蹤方法[J].傳感器技術(shù),2002,21(3):1000-9787.