呂 旭，胡柏青，李開(kāi)龍，戴永彬

（1.海軍工程大學(xué)電氣工程學(xué)院，武漢 430033；2.遼寧工業(yè)大學(xué)電氣工程學(xué)院，遼寧 錦州 121001）

0 引言

狀態(tài)估計(jì)理論被廣泛應(yīng)用于目標(biāo)跟蹤系統(tǒng)及組合導(dǎo)航系統(tǒng)等。當(dāng)系統(tǒng)為線性時(shí)，Kalman 濾波器是最優(yōu)的解決方案［1-2］。然而，在許多實(shí)際狀態(tài)估計(jì)問(wèn)題中，遇到的系統(tǒng)模型都是非線性的。要精確求解非線性狀態(tài)估計(jì)問(wèn)題一般是十分困難的，其難點(diǎn)主要在于精確建立概率分布的非線性傳遞關(guān)系［3-4］。為此，學(xué)者們提出了一系列以高斯濾波為理論框架、基于模型線性化近似和數(shù)值積分近似的非線性狀態(tài)估計(jì)方法。在非線性濾波算法中，擴(kuò)展Kalman濾波算法（Extended Kalman filter，EKF）［5］最為常見(jiàn)。EKF 利用泰勒級(jí)數(shù)和多元雅克比矩陣將非線性的系統(tǒng)模型線性化近似，實(shí)現(xiàn)了Kalman 濾波理論在非線性系統(tǒng)模型中的應(yīng)用。進(jìn)入到21 世紀(jì)初，數(shù)值積分近似方法開(kāi)始在非線性濾波中使用，使得非線性Kalman 濾波理論得到了飛速發(fā)展，這其中有基于確定性采樣的Gauss-Hermite 數(shù)值積分的高斯-厄米特Kalman 濾波（Gauss-Hermite filter，GHKF）［6］、無(wú)味Kalman 濾波（Unscented Kalman filter，UKF）［7］以及容積Kalman 濾波（Cubature Kalman filter，CKF）等［8-9］。類似于確定性采樣的方法，基于Monte-Carlo實(shí)驗(yàn)的隨機(jī)性采樣的粒子濾波（Particle filter，PF）也隨之誕生［10-11］，應(yīng)該說(shuō)PF 已經(jīng)脫離了Kalman 濾波理論體系。

國(guó)內(nèi)外學(xué)者針對(duì)非線性系統(tǒng)中狀態(tài)估計(jì)問(wèn)題，紛紛進(jìn)行比較與分析。文獻(xiàn)［12］中王碩等學(xué)者針對(duì)低維和高維模型對(duì)典型非線性濾波算法進(jìn)行對(duì)比分析，在數(shù)值穩(wěn)定和濾波精度方面CKF 濾波算法最優(yōu)。文獻(xiàn)［13］中Cheng Yang 等研究了不同反饋方式下的EKF 與UKF 之間關(guān)系，同時(shí)說(shuō)明在解決非線性方程方面，UKF 在精度和一致性方面比EKF 表現(xiàn)出更好的性能。文獻(xiàn)［14］中Kulikova Maria 等提出在剛性連續(xù)離散隨機(jī)系統(tǒng)中，EKF 在狀態(tài)估計(jì)精度上相比CKF 和UKF 性能更好。文獻(xiàn)［15］中胡洪濤等指出PF 在處理非高斯噪聲時(shí)，濾波性能遠(yuǎn)優(yōu)于EKF。目前為止，在非線性狀態(tài)估計(jì)領(lǐng)域之中，并不存在一種“最好”的算法。對(duì)于以上非線性濾波算法的研究取得了諸多成果，但對(duì)于上述5 種濾波算法同時(shí)在一維和多維狀態(tài)下非線性目標(biāo)跟蹤中的精度、計(jì)算量和適用差異性等研究分析還十分缺乏。本文正是基于此，通過(guò)強(qiáng)非線性模型、雷達(dá)機(jī)動(dòng)目標(biāo)跟蹤模型仿真實(shí)驗(yàn)和跑車試驗(yàn)，探究了5 種非線性目標(biāo)跟蹤算法的優(yōu)缺點(diǎn)，為非線性目標(biāo)跟蹤濾波算法的選取提供了有益的參考。

1 非線性Kalman 濾波算法

在本章中，介紹了EKF、GHKF、UKF 和CKF 卡爾曼濾波器所有技術(shù)細(xì)節(jié)，并討論了它們?cè)跍y(cè)量數(shù)據(jù)時(shí)的性能。非線性Kalman 濾波理論體系框圖如圖1 所示。

圖1 非線性Kalman 濾波理論體系結(jié)構(gòu)框圖

1.1 擴(kuò)展Kalman 濾波

在實(shí)際工程應(yīng)用中，離散非線性系統(tǒng)對(duì)于噪聲，一般采用加性噪聲來(lái)表征，加性噪聲形式的系統(tǒng)運(yùn)動(dòng)模型如式（1）所示。

1.2 高斯-厄米特Kalman 濾波

GHKF 是一種針對(duì)非線性系統(tǒng)運(yùn)動(dòng)模型的多項(xiàng)式積分近似濾波算法，無(wú)論是求解先驗(yàn)概率密度，還是求解后驗(yàn)概率密度，可以將高斯濾波的積分形式寫(xiě)成具有普遍代表性的一般形式，如式（10）所示：

考慮如式（1）的加性噪聲非線性系統(tǒng)運(yùn)動(dòng)模型，得到m 階GHKF 的計(jì)算流程如下

1）時(shí)間更新

2）量測(cè)更新

Step 8：濾波量測(cè)方差、協(xié)方差及時(shí)刻估計(jì)值和濾波方差值

1.3 無(wú)味Kalman 濾波

UKF 是基于Unscented Transformation（UT）變換，它是一種使用確定性Sigma 點(diǎn)的統(tǒng)計(jì)線性化方法。所謂UT 變換就是將如下積分形式近似式（9）所示。

對(duì)應(yīng)的權(quán)重wi為

式中，k 是一個(gè)微調(diào)標(biāo)量參數(shù)，k 的大小與樣本矩大小有關(guān)，控制著Sigma 點(diǎn)到均值點(diǎn)的距離。并且權(quán)重wi需要滿足歸一化條件：

考慮如式（1）的加性噪聲非線性系統(tǒng)運(yùn)動(dòng)模型，得到UKF 基本計(jì)算流程如下：

1）時(shí)間更新

Step 3：計(jì)算預(yù)測(cè)值和濾波方差

2）量測(cè)更新

1.4 容積Kalman 濾波

容積Kalman 算法利用一組權(quán)值相等的容積點(diǎn)計(jì)算狀態(tài)變量的均值和協(xié)方差，經(jīng)非線性變換后得到非線性最優(yōu)逼近性能、濾波精度和數(shù)值穩(wěn)定性?？紤]如式（1）的加性噪聲非線性系統(tǒng)運(yùn)動(dòng)模型，得到UKF 基本計(jì)算流程如下：

1）時(shí)間更新

Step 3：計(jì)算預(yù)測(cè)值和濾波方差

2 粒子濾波算法

粒子濾波是一種基于Monte Carlo 的貝葉斯估計(jì)方法，采用帶權(quán)值的隨機(jī)粒子來(lái)逼近后驗(yàn)概率密度函數(shù)，因此，它就獨(dú)立于系統(tǒng)的模型，即可以解決非高斯情況下的非線性估計(jì)問(wèn)題。針對(duì)模型（1），粒子濾波的基本步驟如下所示。

Step 4：輸出xk的近似后驗(yàn)概率密度

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

3.1 UNGM 模型模特卡洛仿真

UNGM（Univariate Nonstationary Growth Model，UNGM）模型屬于強(qiáng)非線性模型，如下所示。

濾波計(jì)算時(shí)間采用100 次Monte-Carlo 仿真實(shí)驗(yàn)的平均值來(lái)衡量。5 種濾波算法的估計(jì)精度比較如下頁(yè)圖2 所示，計(jì)算時(shí)間比較如圖3 所示。

在圖2 中，PF 整體的濾波精度最好，UKF、CKF和2 階GHKF 次之，而EKF 相對(duì)較差。然而，EKF的缺點(diǎn)是十分明顯的，主要有：1）當(dāng)系統(tǒng)運(yùn)動(dòng)模型非線性特征極為明顯，采用雅克比矩陣將很可能無(wú)法逼近，從而導(dǎo)致濾波降階與發(fā)散。2）為了達(dá)到更高濾波精度，高階EKF 可以通過(guò)提高泰勒展開(kāi)階次來(lái)實(shí)現(xiàn)，但高階EKF 不僅需要計(jì)算高階的多元雅克比矩陣，濾波流程也會(huì)更加復(fù)雜，從而增大計(jì)算負(fù)擔(dān)。在一維非線性系統(tǒng)中，UKF、CKF 和GHKF 相比之下濾波精度相近，UKF 濾波精度相對(duì)最好，CKF次之，GHKF 稍差。

圖2 濾波估計(jì)誤差比較

圖3 運(yùn)算時(shí)間比較

表1 均方誤差與計(jì)算時(shí)間

由表1 可知，EKF、2 階GHKF、UKF、CKF 和PF 5 種非線性濾波算法的Monte-Carlo 仿真MSE 均值分別為530.474 8、207.407 8、72.797 5、117.149 0 和22.207 7。然而，從5 種算法計(jì)算時(shí)間來(lái)看，平均計(jì)算時(shí)間分別為0.141 5 s、0.876 9 s、0.235 7 s、0.223 7 s和2.820 2 s，顯然，EKF 在計(jì)算時(shí)間上具有明顯的優(yōu)勢(shì)，而PF 和GHKF 的計(jì)算時(shí)間要明顯長(zhǎng)于EKF、UKF 和CKF，說(shuō)明了PF 和GHKF 的計(jì)算精度是通過(guò)巨大的計(jì)算負(fù)擔(dān)所獲得的，而UKF 雖然比CKF計(jì)算時(shí)間要長(zhǎng)，但是考慮到其具有明顯優(yōu)勢(shì)的估計(jì)精度和濾波穩(wěn)定性，說(shuō)明UKF 仍是濾波性能相當(dāng)均衡的非線性濾波算法，總結(jié)本仿真實(shí)驗(yàn)數(shù)據(jù)結(jié)果如表1 所示。

3.2 雷達(dá)目標(biāo)跟蹤性能的比較分析

機(jī)動(dòng)目標(biāo)（轉(zhuǎn)彎率w 未知）跟蹤模型是五維非線性模型，在雷達(dá)探測(cè)中應(yīng)用廣泛，其離散后的狀態(tài)和觀測(cè)方程如式（55）和式（56）所示。

圖4 目標(biāo)跟蹤軌跡對(duì)比

圖5 位置誤差估計(jì)對(duì)比

圖6 速度誤差估計(jì)對(duì)比

由圖中發(fā)現(xiàn)，在估計(jì)精度上，PF 整體的濾波精度仍為最好，UKF、CKF 和2 階GHKF 次之，而EKF相對(duì)較差，與一維非線性系統(tǒng)不同的是CKF 濾波效果稍優(yōu)于UKF。容易看出對(duì)于維數(shù)大于3 的非線性濾波問(wèn)題，UKF 中心點(diǎn)處sigma 點(diǎn)的權(quán)重為負(fù)值，而這很容易導(dǎo)致傳遞的方差非正定，進(jìn)而導(dǎo)致濾波算法的數(shù)值不穩(wěn)定。但是，從第2 節(jié)可以看出，UKF 中每一個(gè)采樣點(diǎn)與中心點(diǎn)的距離是與成比例的，而k=3-n，因此，維數(shù)增加對(duì)非局部采樣的貢獻(xiàn)被抵消了。而對(duì)于CKF 而言，由于沒(méi)有可調(diào)參數(shù)對(duì)狀態(tài)擴(kuò)張的抵消，其每一個(gè)采樣點(diǎn)與中心點(diǎn)的距離是與成比例的，因此，隨著維數(shù)的增加會(huì)產(chǎn)生非局部采樣問(wèn)題。

表3 給出了5 種濾波方法對(duì)狀態(tài)x 和vx（水平方向）的均方誤差及計(jì)算時(shí)間的比較。可知，PF 的選取粒子數(shù)越多，精度越好，但是計(jì)算量巨大。雖然UKF、CKF 和GHKF 的估計(jì)精度和穩(wěn)定性是基本相當(dāng)?shù)?，但GHKF 的濾波效果也是通過(guò)高計(jì)算量所獲得的，GHKF 的計(jì)算時(shí)間是UKF 的4.8 倍，CKF 的5.4 倍，EKF 的8.6 倍，而且在本次實(shí)驗(yàn)中，即使增加GHKF 的多項(xiàng)式逼近的階次，對(duì)于估計(jì)精度的貢獻(xiàn)也不明顯，但卻進(jìn)一步增大計(jì)算量。因此，從本次實(shí)驗(yàn)來(lái)看，UKF 是在3 種非線性Kalman 濾波算法中性價(jià)比最高的濾波算法。

3.3 跑車實(shí)驗(yàn)對(duì)比

為了驗(yàn)證本文提及算法的有效性，跑車試驗(yàn)平臺(tái)主要通過(guò)雷達(dá)（RADAR）和激光雷達(dá)（LIDAR）來(lái)測(cè)量目標(biāo)車輛，對(duì)激光和雷達(dá)兩種跟蹤系統(tǒng)進(jìn)行數(shù)據(jù)融合，提高跟蹤器的性能。恒定轉(zhuǎn)率模型（Constant Turn Rate，CTR）原理和相關(guān)技術(shù)參數(shù)可參考文獻(xiàn)［16-17］，整個(gè)跑車實(shí)驗(yàn)持續(xù)時(shí)間約120 s，實(shí)驗(yàn)過(guò)程中全程能夠較好地接收雷達(dá)信號(hào)。行車路線如圖7 所示，具體跟蹤誤差曲線如圖8 所示。

表2 濾波精度及耗時(shí)

圖7 車輛運(yùn)動(dòng)軌跡

圖8 位置誤差估計(jì)對(duì)比

由實(shí)驗(yàn)結(jié)果得知，在非線性機(jī)動(dòng)目標(biāo)位置估計(jì)中，圖8 通過(guò)跟蹤誤差曲線，可以更加清晰地顯示粒子濾波相比于Kalman 框架下非線性濾波算法的顯著優(yōu)勢(shì)。而基于Kalman 框架下非線性濾波算法位置估計(jì)精度相近，相比之下，UKF 和CKF 估計(jì)精度較優(yōu)，GHKF 和EKF 次之。從而充分驗(yàn)證了非線性濾波對(duì)雷達(dá)目標(biāo)跟蹤理論。

4 結(jié)論

本文對(duì)非線性目標(biāo)跟蹤濾波算法進(jìn)行了分析和比較研究，重點(diǎn)對(duì)EKF、GHKF、UKF、CKF 和PF 5 種非線性濾波算法的演變與關(guān)鍵點(diǎn)作了分析與探討。通過(guò)仿真實(shí)驗(yàn)和實(shí)際運(yùn)行，比較了5 種典型非線性目標(biāo)跟蹤算法在仿真模型中的估計(jì)效果。實(shí)驗(yàn)表明，PF 相比其他4 種非線性Kalman 濾波算法估計(jì)精度高計(jì)算量大，根據(jù)其本身特性可以用于任何復(fù)雜環(huán)境；EKF 在計(jì)算時(shí)間上具有明顯的優(yōu)勢(shì)，濾波精度低；UKF、CKF 和GHKF 三者濾波性能相近，通過(guò)分析比較UKF 濾波性能和計(jì)算時(shí)間，證明其具有較高的濾波性價(jià)比和廣闊的應(yīng)用空間。而隨著計(jì)算機(jī)計(jì)算速度的不斷提高，PF 將是最優(yōu)的選擇。