聯(lián)合約束級聯(lián)交互式多模型濾波器及其在機動目標(biāo)跟蹤中的應(yīng)用

夏小虎 劉 明

?

聯(lián)合約束級聯(lián)交互式多模型濾波器及其在機動目標(biāo)跟蹤中的應(yīng)用

夏小虎*①②劉 明②

①(合肥學(xué)院機械工程系 合肥 230601);②(中國科學(xué)院合肥智能機械研究所 合肥 230031)

該文提出一種新型聯(lián)合約束的級聯(lián)交互式多模型卡爾曼濾波器，該濾波器由兩個濾波器前后兩級串聯(lián)而成；第1級為標(biāo)準(zhǔn)交互式多模型濾波器；第2級為聯(lián)合約束濾波器。聯(lián)合約束濾波器的約束條件對第1級濾波器中的多模型集合中各子模型均有效。聯(lián)合約束濾波器采用平滑約束卡爾曼濾波算法對第1級濾波結(jié)果進一步優(yōu)化。以機動目標(biāo)實時跟蹤為實際工程應(yīng)用背景，數(shù)值仿真和飛行實驗結(jié)果證明新的聯(lián)合約束性級聯(lián)交互式多模型濾波器比標(biāo)準(zhǔn)交互式多模型濾波器具有更小的估計誤差和方差，所增計算量合理可行。該文為交互式多模型濾波器和機動目標(biāo)跟蹤兩個方向的進一步改進提供了有益借鑒。

機動目標(biāo)跟蹤；交互式多模型；卡爾曼濾波；狀態(tài)約束方程

1 引言

機動目標(biāo)跟蹤在軍事與民用諸多領(lǐng)域有著廣泛應(yīng)用，一直是研究的關(guān)注點。目前機動目標(biāo)跟蹤的主流算法是交互式多模型(Interactive Multi- Model, IMM)濾波算法。針對該算法如何進一步提高濾波精度的問題，許多學(xué)者提出了各種改進算法，如變結(jié)構(gòu)算法(Variable Structure interactive Multi-Model, VSMM)[9]和自適應(yīng)算法(Adaptive interactive Multi-Model, AMM)[10]，自適應(yīng)網(wǎng)格模糊算法(Adaptive Grid Fuzzy Multi-Model, AGFMM)[11]等。深入分析不難發(fā)現(xiàn)，上述諸多改進算法的共同點是對多模型集合進行各種動態(tài)調(diào)整，實時改變多模型集合，以適應(yīng)信號的實時變化，從而達到提高濾波精度的目的。使用這類算法跟蹤復(fù)雜快速信號，模型集合數(shù)必然將增加，計算量從而增加較大。同時，系統(tǒng)復(fù)雜度增加，穩(wěn)定性能和魯棒性能下降。本文受文獻[12]啟發(fā)，從另一個角度提出對交互式多模型濾波算法的改進。改進思想是從控制對象的狀態(tài)方程出發(fā)，進一步發(fā)掘關(guān)于控制對象各狀態(tài)之間相互關(guān)系的有用信息，利用該信息建立非線性約束方程，形成聯(lián)合約束濾波器；對原有交互式濾波結(jié)果再進行濾波，從而提高濾波精度；這一思想不妨稱為約束濾波。約束濾波的思想來源于文獻[12]，但該文獻只討論了單模型情況。本文將約束濾波思想推廣應(yīng)用到交互式多模型中，提出一種新型的聯(lián)合約束級聯(lián)交互式多模型濾波器(Unified Constrained Cascade Interactive Multi- Model, UCC-IMM)。該濾波器采用前后兩級串聯(lián)結(jié)構(gòu)形式，第1級為標(biāo)準(zhǔn)交互式多模型濾波器。再針對多模型集合中各子模型，建立統(tǒng)一的狀態(tài)約束方程?；谠摷s束方程，結(jié)合平滑約束卡爾曼濾波算法SCKF(Smoothly Constraint Kalman Filter)，形成聯(lián)合約束濾波器，做為第2級濾波器，對第1級濾波結(jié)果做進一步優(yōu)化。為驗證聯(lián)合約束級聯(lián)交互式多模型濾波器UCC-IMM的改進效果，針對機動目標(biāo)實時跟蹤這一工程實踐問題，進行數(shù)值仿真和硬件實驗，并與采用標(biāo)準(zhǔn)交互式多模型濾波器IMM進行了對比，結(jié)果表明聯(lián)合約束級聯(lián)交互式多模型濾波算法UCC-IMM比標(biāo)準(zhǔn)交互式多模型濾波器IMM具有更小的估計誤差和估計方差，而增加的計算量合理。本文安排如下：第1節(jié)引入研究的理論背景。第2節(jié)以機動目標(biāo)跟蹤問題為例建立約束等式；對約束等式的處理方法在第3節(jié)介紹。第4節(jié)是數(shù)值仿真和飛行實驗結(jié)果分析。最后總結(jié)全文。

2 聯(lián)合約束濾波算法推導(dǎo)

首先分析獨立濾波組和聯(lián)合濾波的利弊，確定采用聯(lián)合約束濾波器方案。再針對機動目標(biāo)實時跟蹤這一工程實踐問題，推導(dǎo)聯(lián)合約束濾波算法。

2.1 聯(lián)合約束在多模型濾波器中的位置

交互式濾波包含一組濾波器. 對每一個模型分別進行濾波。在輸入端和輸出端進行數(shù)據(jù)交互。那么，新增的非線性約束濾波多模型濾波器的位置應(yīng)該如何設(shè)置呢？可以設(shè)想到兩種方案，如圖1和圖2所示。

在圖1中，在標(biāo)準(zhǔn)IMM的內(nèi)部，分別針對多個子模型分別建立約束條件，建立平滑約束濾波器組分別對原濾波器組輸出結(jié)果再濾波，不妨稱為獨立約束濾波組。顯然，獨立約束濾波仍然沒有擺脫模型集的框架。如果跟蹤復(fù)雜快速信號，模型集增加，濾波器組擴大，獨立約束濾波組也同樣擴大，計算量大大增加，如此難免又陷入各類變結(jié)構(gòu)多模型算法所面臨的同樣的困境，無法體現(xiàn)改進的創(chuàng)新點和優(yōu)勢。所以，圖1方案不可行。

圖2中，點虛線中為標(biāo)準(zhǔn)IMM，聯(lián)合約束濾波器UCF與IMM串聯(lián)形成聯(lián)合約束級聯(lián)交互式多模型濾波器UCC-IMM(點劃線范圍內(nèi))。UCF一次性處理IMM輸出交互之后的數(shù)據(jù)，無論模型集合數(shù)量如何增減變化，均不能對聯(lián)合約束濾波器UCF計算量產(chǎn)生影響，從而避免了圖1方案面臨的問題。

這一方案的關(guān)鍵之處在于UCF遵循的約束方程滿足模型集中的所有子模型。即所有子模型“聯(lián)合”采用同一個約束濾波器，不再分別獨立濾波。這也是本文的創(chuàng)新點所在。

被控對象的狀態(tài)變化快速而復(fù)雜，使用一個數(shù)學(xué)模型不能準(zhǔn)確描述，采用多個子模型構(gòu)成模型集合描述被控對象不同時刻的狀態(tài)則更加準(zhǔn)確。以飛行器等機動目標(biāo)跟蹤為例，飛行狀態(tài)最多可以用3個子模型來描述：協(xié)調(diào)轉(zhuǎn)彎模型、等速模型和加速模型。在可以清楚闡述本文論點的前提下，為簡化分析，下面以協(xié)調(diào)轉(zhuǎn)彎和等速兩個模型描述的機動目標(biāo)跟蹤問題為例，建立聯(lián)合約束方程。

圖1 各獨立約束濾波器嵌入在IMM中 圖2 聯(lián)合約束濾波器與IMM級聯(lián)

2.2 協(xié)調(diào)轉(zhuǎn)彎模型的約束等式

文獻[13]提出了機動目標(biāo)線性離散動態(tài)模型：

(1)

顯然，相對其他轉(zhuǎn)彎模型，這一模型處理噪聲項的手法比較特別。取轉(zhuǎn)彎率受到加性高斯白噪聲影響。而其余變量為的函數(shù)，就不再考慮噪聲干擾。

將等式矩陣(1)的第5行改寫為

對隨機變量取均值：

(3)

(5)

(7)

將此結(jié)果再代入式(6)，并考慮式(5)，可得

(9)

對假設(shè)遞歸一步：

化簡可得：

(11)

綜合式(6)-式(11)，歸納法證明了式(9)假設(shè)成立。

這樣就獲得了對協(xié)調(diào)轉(zhuǎn)彎模型中狀態(tài)變量的約束方程。不失一般性，若初始速度狀態(tài)任意，可推得

(13)

即機動目標(biāo)在轉(zhuǎn)彎運動中兩個速度矢量的平方和為常數(shù)；該常數(shù)為轉(zhuǎn)彎運動的速度初始值。這個約束對并不是對任意實時狀態(tài)均有效，只是對平均值成立，不妨稱為軟約束(Soft Constraints)[12,13]。

2.3 等速模型的約束等式

文獻[13]中提出了機動目標(biāo)在等速飛行時的線性離散動態(tài)模型：

這一模型對噪聲的處理手法是在兩個速度變量中引入噪聲。而兩個位置變量為速度的函數(shù)，就不再考慮噪聲干擾。

考慮到隨機變量在均值上下波動，表達式直接可寫成

式(15)作為機動目標(biāo)在等速運動時的狀態(tài)約束方程。顯然，這也是一個軟約束(soft constraints)[12,13]。

至此，綜合式(13)和式(15)，機動目標(biāo)在協(xié)調(diào)轉(zhuǎn)彎運動和等速運動兩個狀態(tài)的聯(lián)合狀態(tài)約束方程已建立；從上述分析也可以注意到，針對隨機系統(tǒng)，對系統(tǒng)變量取均值，更多的是建立軟約束。

3 非線性約束等式的處理

式(13)和式(15)均為非線性方程。對于非線性約束，文獻[12]列舉了多種處理方法。如估計投影法(estimate projection)[15]、系統(tǒng)投影(system projection)[16]、分布概率截斷(pdf truncation)[17,18]、平滑約束卡爾曼濾波SCKF(Smoothly Constraint Kalman Filter)[19]，移動平均估計MHE(Moving Horizon Estimation)[20]、投影無味卡爾曼濾波(Projected UKF)[21]等。針對文獻[12]文中模型，MHE的均方誤差最小，但計算量很大。另一方面，實踐表明，對于非線性度較高的系統(tǒng)，估計投影等計算量較小的方法可能失效。

綜合均方誤差和計算量兩個因素，本文選用平滑約束卡爾曼濾波SCKF方法來處理非線性約束。該算法借鑒迭代卡爾曼濾波(iterated Kalman Filter)的思想，將約束等式線性化，并作為量測結(jié)果代入類似標(biāo)準(zhǔn)卡爾曼濾波更新公式，并多次迭代。具體計算流程見文獻[12]和文獻[19]，此處不再詳細展開。

4 數(shù)值仿真與硬件實驗

采用交互式卡爾曼濾波實現(xiàn)對機動目標(biāo)狀態(tài)估計。針對非線性模型，采用擴展卡爾曼濾波EKF (Extended Kalman Filter)[4,5]。

在此基礎(chǔ)上，增加平滑約束卡爾曼濾波SCKF方法來處理非線性等式約束式式(13)和式(15)[19]，具體步驟見第3節(jié)。其中：約束方程的雅克比矩陣采用差分形式；調(diào)節(jié)參數(shù)取。

為驗證聯(lián)合約束級聯(lián)交互式多模型濾波器(UCC-IMM)對標(biāo)準(zhǔn)交互式多模型濾波器(IMM)改進效果，分別進行了數(shù)值計算仿真和實驗。現(xiàn)在分別對數(shù)值計算仿真和實驗的結(jié)果和分析論述如下。

4.1 機動目標(biāo)飛行模態(tài)[13]

設(shè)機動目標(biāo)從原點出發(fā)，和方向初速度分別為1 m/s和2 m/s，即初始狀態(tài)。在20 s內(nèi)進行飛行實驗仿真：具體飛行模態(tài)如下：第1~4 s內(nèi)執(zhí)行等速飛行模態(tài)。第4~9 s內(nèi)執(zhí)行轉(zhuǎn)彎飛行模態(tài)，轉(zhuǎn)彎率=1。第9~11 s內(nèi)執(zhí)行等速飛行模態(tài)。第11~16 s內(nèi)執(zhí)行轉(zhuǎn)彎飛行模態(tài)，轉(zhuǎn)彎率。第16~20 s內(nèi)執(zhí)行等速飛行模態(tài)。采樣時間0.1 s，獲得200個采樣點[13]。

4.2 飛行模型參數(shù)設(shè)置[13]

4.3 數(shù)值仿真結(jié)果與分析

圖3~圖5為單次飛行仿真結(jié)果。圖中橫軸為200個采樣點，相鄰時間間隔0.1 s?？v軸為各點誤差值。

圖3 飛行軌跡與量測值 圖4 的估計誤差對比 圖5 的估計方差對比

圖3中光滑綠色細線為實際飛行軌跡，藍色粗線為無約束的標(biāo)準(zhǔn)交互式多模型濾波器(IMM)濾波值，紅色細線為聯(lián)合約束級聯(lián)交互式多模型濾波器(UCC-IMM)的濾波值。圖4是UCC-IMM和IMM對兩個速度變量的估計誤差對比。圖5是UCC- IMM和IMM對轉(zhuǎn)彎率的估計方差對比。

從圖3~圖5直觀上看，UCC-IMM的估計誤差和估計方差兩個濾波性能指標(biāo)在整體上比IMM有明顯改進。但也不排除在局部點表現(xiàn)較差。下面從對200點取平均值來看UCC-IMM和IMM的濾波性能對比。

設(shè)：實際飛行狀態(tài)向量為；無約束估計狀態(tài)向量為；有約束估計狀態(tài)向量為；無約束估計誤差向量，估計方差矩陣為；有約束估計誤差向量，估計方差矩陣為。

(1)設(shè)從UCC-IMM到IMM估計誤差的平均下降率：

考慮到UCC-IMM和IMM的估計誤差均有正負；對誤差均取絕對值處理；這不影響對比分析效果。

(2)設(shè)從UCC-IMM到IMM估計方差的平均下降率：

表1從UCC-IMM到IMM估計誤差和方差的平均下降率(單次仿真)

變量 (%)11.92 4.5821.927.663.23 (%)39. 6829.4169. 2154. 3419.30

將上述數(shù)值仿真實驗隨機重復(fù)多次，統(tǒng)計結(jié)果表明上述結(jié)論仍成立，如表2所示。

表2從UCC-IMM到IMM估計誤差和方差的平均下降率(100次仿真)

變量 (%)8.629.4815.3611.322.88 (%)39.0027.4369.1547.2717.87

數(shù)值仿真實驗結(jié)果顯示聯(lián)合約束級聯(lián)交互式多模型濾波在估計誤差和方差兩個方面有明顯改進效果。

另外，數(shù)值仿真實驗也表明，聯(lián)合約束級聯(lián)交互式多模型濾波計算量有所增加。分析原因如下：在原有無約束交互式卡爾曼濾波器中，每一個循環(huán)過程包括輸入交互、卡爾曼濾波、模型概率刷新和聯(lián)合輸出4步。其中，卡爾曼濾波又包括預(yù)測和更新兩步。采用平滑約束卡爾曼濾波SCKF方法處理約束等式的計算量和更新這一步的計算量相當(dāng)?？傮w上計算量增加約30%。增加量合理可接受。

4.4 飛行實驗結(jié)果與分析

采用微小型四旋翼無人機進行飛行實驗，考察聯(lián)合約束級聯(lián)交互式多模型濾波器性能的實際改進效果。由飛行姿態(tài)控制器和姿態(tài)獲取模塊組成內(nèi)環(huán)控制，由飛行速度和位置控制器和速度信息獲取模塊組成外環(huán)控制。分別采用聯(lián)合約束級聯(lián)UCC- IMM和標(biāo)準(zhǔn)IMM對速度信息獲取模塊輸出進行濾波處理。在自動飛行模式下采用GPS模塊定位導(dǎo)航平面2維信號。采用聲吶模塊測量飛行高度?？紤]到聲吶信號較好，但測量距離有限，只在小范圍內(nèi)進行低空飛行實驗。

圖6-圖7是正常巡航飛行模態(tài)下，UCC-IMM和IMM對軸的速度和位置兩個變量的濾波效果對比。圖中橫軸為100個離散數(shù)據(jù)點，相鄰時間間隔0.1 s?？v軸為估計誤差值。參考信號選取為GPS信號。飛行實驗表明UCC-IMM估計誤差整體上優(yōu)于IMM，和數(shù)值仿真的結(jié)論一致。

嚴格來講，GPS信號本身也受到元件噪聲影響，采用單次實驗數(shù)據(jù)做為比較依據(jù)并不嚴謹，因此進一步大量重復(fù)實驗，在統(tǒng)計意義下考察正常飛行模態(tài)時的聯(lián)合約束級聯(lián)IMM濾波器的表現(xiàn)。仍然采用和兩個指標(biāo)對位置變量和，速度變量和，以及轉(zhuǎn)彎率等5個指標(biāo)進行定量統(tǒng)計分析。重復(fù)自動飛行和手動飛行的實驗多次，對飛行實驗結(jié)果的統(tǒng)計結(jié)果見表3。統(tǒng)計表明，采用聯(lián)合非線性約束條件對交互式多模型濾波器性能的改進效果明顯。

同時，對比表2和表3，可以發(fā)現(xiàn)，在實際飛行實驗的情況下的改進效果比數(shù)值仿真計算效果好。因為實際飛行中存在傳感器噪聲誤差，聯(lián)合非線性約束方程在原理上對各變量進行了修正，克服了噪聲誤差的干擾。

表3 UCC-IMM到IMM估計誤差和方差的平均下降率(100次飛行實驗數(shù)據(jù))

5 結(jié)束語

本文提出了一種新型聯(lián)合約束的級聯(lián)交互式多模型卡爾曼濾波器。聯(lián)合濾波器的關(guān)鍵點是建立適用于多模型集合中各子模型的聯(lián)合約束條件。第一，本文建立了針對機動目標(biāo)跟蹤中的等速模型和協(xié)調(diào)轉(zhuǎn)彎模型的聯(lián)合約束方程，證明了這一思路的可行性。后續(xù)研究可以考慮建立包括等加速模型在內(nèi)聯(lián)合約束條件，進一步拓寬聯(lián)合濾波器在機動目標(biāo)跟蹤中的應(yīng)用。第二，本文建立的聯(lián)合約束方程是一種平均意義下的軟約束；數(shù)值仿真和硬件實驗表明這一軟約束條件能夠改進濾波效果，滿足實際工程要求。最后，為進一步提高濾波精度，能否建立和如何建立更好的約束條件，是下一步的研究重點方向之一。

圖6 的估計誤差對比 圖7 的估計誤差對比

[1] YANG Jinlong, JI Hongling, and FAN Zhenhua. Probability hypothesis density filter based on strong tracking MIE for multiple maneuvering targets tracking[J].,, 2013, 11(2): 306-316.

[2] LI Bo. Multiple-model Rao-Blackwellized particle CPHD filter for multi-target tracking[J]., 2015, 79(3): 2133-2143.

[3] LIU Meiqin, ZHANG Di, and ZHANG Senlin. Bearing-only target tracking using cubature rauch-tung-striebel smoother [C]. 34th Chinese Control Conference, Hangzhou, China, 2015: 4734-4738.

[4] EVERS C, MOORE A H, NAYLOR P A,. Bearing- only acoustic tracking of moving speakers for robot audition [C]. IEEE International Conference on Digital Signal Processing, Singapore, 2015: 1206-1210.

[5] BECKER S, MUNCH D, KIERITZ H,. Detecting abandoned objects using interacting multiple models[J]., 2015, 96520. doi: 10.1117/12.2195224.

[6] SABORDO M G and ABOUTANIOS E. Enhanced performance for the interacting multiple model estimator with integrated multiple filters[J]., 2015, 94600: 345-349. doi: 10.1117/12.2176180.

[7] MABROUK M B, GRIVEL E, MAGNANT C,. Compensating power amplifier distortion in cognitive radio systems with adaptive interacting multiple model[C]. 23rd European Signal Processing Conference (EUSIPCO), Nice, France, 2015: 1212-1216.

[8] HUANG H, YANG R, NG G W,. Helicopter tracking and classification with multiple interacting multiple model estimator with out-of-sequence acoustic and EO measurements[C]. 19th International Conference on Information Fusion (FUSION), Heidelberg, Germany, 2016: 1132-1139.

[9] KIM T H and MOON K R. Variable-structured interacting multiple model algorithm for the ballistic coefficient estimation of a re-entry ballistic target[J].,,, 2013, 11(6): 1204-1213.

[10] ZHU Zhengwei. Ship-borne radar maneuvering target tracking based on the variable structure adaptive grid interacting multiple model[J]., 2013, 14(9): 733-742.

[11] ZHANG Yuan, GUO Chen, HU Hai,. An algorithm of the adaptive grid and fuzzy interacting multiple models[J]., 2014, 13(3): 340-345.

[12] DAN S and TIEN L C. Kalman filtering with state equality constraints[J]., 2010, 4(8): 1303-1318.

[13] HARTIKAINEN J, SOLIN A, and SARKKA S. Optimal filtering with kalman filters and smoothers a manual for the MATLAB toolbox EKF/UKF[R]. 2011.

[14] 盛驟, 等. 概率論與數(shù)理統(tǒng)計[M]. 北京: 高等教育出版社, 1989: 73-75.

SHENG Zhou,. Probability and Statistics[M]. Beijing: High, Education Press, 1989: 73-75.

[15] CHIA T. Parameter identification and state estimation of constrained systems[D]. [Ph.D. dissertation], Case Western Reserve University, 1985, 17-25.

[16] KO S and BITMEAD R. State estimation for linear systems with state equality constraints[J]., 2007, 43(8): 1363-1368.

[17] SHIMADA N, SHIRAI Y, KUNO Y,. Hand gesture estimation and model refinement using monocular camera- ambiguity limitation by inequality constraints[C]. IEEE International Conference on Automatic Face Gesture Recognition, Japan, 1998: 268-273.

[18] SIMON D and SIMON D L. Constrained Kalman filtering via density function truncation for turbofan engine health estimation[J]., 2010, 41(2): 159-171.

[19] DE GEETER J, VAN BRUSSEL H, and DE SCHUTTER J. A smoothly constrained Kalman filter[J].&, 1997, 19(10): 1171-1177.

[20] GOODWIN G, SERON M, and DE DONA J. Constrained control and estimation[M]. Berlin: Springer-Verlag, 2005: 251-262.

[21] JULIER S and UHLMANN J. Unscented filtering and nonlinear estimation[J]., 2004, 92(3): 401-422.

夏小虎： 男，1975年生，副教授，研究方向為信息獲取與控制.

劉 明： 男，1955年生，研究員，研究方向為飛行器自動控制.

Unified Constrained Cascade Interactive Multi-model Filter and Its Application in Tracking of Manoeuvring Target

XIA Xiaohu①②LIU Ming②

①(,,230601,);②(,,230031,)

A novel unified cascade constrained interactive multi-model Kalman filter is put forward. The filter is composed of two cascade connected filters, a standard interactive-multiple-model and a unified constrained filter. The latter is effective for everyone in model set of controlled plant and refines the estimation of the former using smoothly constraint Kalman algorithm. Numerical simulation and flying experiments are made for maneuvering target tracking and lower estimated error and covariance are achieved by the unified cascade constrained interactive multi-model Kalman filter compared with conventional interactive multi-model filter. The added computation cost is reasonable and acceptable. The paper is valuable reference for maneuvering target tracking and interactive multi-model filter.

Maneuvering target tracking; Interactive multi-model; Kalman filter; State constraints equation

TN953

A

1009-5896(2017)01-0117-07

10.11999/JEIT160384

2016-04-20；改回日期：2016-12-06；

2016-12-14

夏小虎 xiaxh@hfuu.edu.cn

國家自然科學(xué)基金(61340016)，安徽省自然科學(xué)基金(1408085MF134)，安徽省高校優(yōu)秀青年骨干人才國內(nèi)外訪學(xué)研修重點項目(gxfxZD2016224)

The National Natural Science Fundation of China (61340016), Anhui Province Natural Science Foundation (1408085MF134), Anhui Province Youth Leading Talents and Visiting Scholar Key Scheme (gxfxZD2016224)