趙思琦，胡曉桐，郭 琴，曾萃霞，鄧小飛

(吉首大學(xué)信息科學(xué)與工程學(xué)院，湖南 吉首 416000)

根據(jù)運(yùn)動(dòng)觀(guān)測(cè)器獲得測(cè)量距離并對(duì)目標(biāo)進(jìn)行跟蹤，是目標(biāo)運(yùn)動(dòng)分析領(lǐng)域中的一個(gè)經(jīng)典問(wèn)題，在很多應(yīng)用場(chǎng)合都可以獲得較精確的距離信息.因此，研究?jī)H利用距離信息進(jìn)行目標(biāo)跟蹤具有十分重要的意義[1].對(duì)于非線(xiàn)性系統(tǒng)，Bucy等[2]在1971年提出了擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF).其基本思想是采用 Taylor 變化對(duì)非線(xiàn)性方程進(jìn)行展開(kāi)，截?cái)喔唠A項(xiàng)對(duì)非線(xiàn)性系統(tǒng)進(jìn)行一階線(xiàn)性化，以一階線(xiàn)性化方程作為系統(tǒng)狀態(tài)方程和量測(cè)方程，然后在標(biāo)準(zhǔn)卡爾曼濾波框架下進(jìn)行濾波處理.EKF算法具備計(jì)算量小、易實(shí)現(xiàn)的優(yōu)點(diǎn)，但在處理強(qiáng)非線(xiàn)性系統(tǒng)和非高斯白噪聲環(huán)境時(shí)效果較差，且需要計(jì)算狀態(tài)方程和量測(cè)方程的雅可比(Jacobian)矩陣.此外， EKF 線(xiàn)性化過(guò)程中只截取了展開(kāi)式的一階項(xiàng)，故算法線(xiàn)性化過(guò)程中存在較大的近似誤差[3].基于此，Julier等[4]在2000年提出了無(wú)跡卡爾曼濾波(Unscented Kalman Filter,UKF).UKF 采用無(wú)跡變換(Unscented Transform, UT)對(duì)非線(xiàn)性系統(tǒng)狀態(tài)空間變量進(jìn)行高斯密度(概率密度)近似，通過(guò)確定性采樣獲取一定數(shù)量的采樣點(diǎn)來(lái)處理非線(xiàn)性系統(tǒng)狀態(tài)方程和量測(cè)方程的均值，以及協(xié)方差在卡爾曼濾波框架下的非線(xiàn)性傳遞.UKF不需要計(jì)算雅可比矩陣或海森(Hessians)矩陣，拓寬了算法的使用范圍，并通過(guò)保留高階項(xiàng)，提高了算法的計(jì)算精度[5].相比于EKF，UKF沒(méi)有忽略高階項(xiàng)，因此對(duì)于非線(xiàn)性分布的統(tǒng)計(jì)量有較高的計(jì)算精度，有效地克服了EKF估計(jì)精度低和穩(wěn)定性差的缺點(diǎn)[6].筆者擬基于觀(guān)測(cè)距離，分別采用EKF和UKF進(jìn)行目標(biāo)跟蹤，并通過(guò)Matlab仿真工具對(duì)2種方法進(jìn)行比較.

1 擴(kuò)展卡爾曼濾波原理

1.1 局部線(xiàn)性化

離散非線(xiàn)性系統(tǒng)動(dòng)態(tài)方程可表示為

X(k+1)=f(k,X(k))+G(k)W(k)，

(1)

Z(k)=h(k,X(k))+V(k).

(2)

當(dāng)過(guò)程噪聲W(k)和觀(guān)測(cè)噪聲V(k)恒為0時(shí)，非線(xiàn)性系統(tǒng)(1)和(2)的真實(shí)解稱(chēng)為“真軌跡”或“真狀態(tài)”.

為了使數(shù)學(xué)計(jì)算更加方便，假設(shè)輸入的控制量為0，過(guò)程噪聲是高斯白噪聲且均值為0，噪聲驅(qū)動(dòng)矩陣G(k)是已知的，觀(guān)測(cè)噪聲V(k)是加性均值為0的高斯白噪聲，再假設(shè)過(guò)程噪聲和觀(guān)測(cè)噪聲序列二者之間相互獨(dú)立.

(3)

令

則由(3)式可得狀態(tài)方程

X(k+1)=Φ(k+1|k)X(k)+G(k)W(k)+Φ(k)，

(4)

初始值X(0)=E(X(0)).

(5)

令

則由(5)式可得觀(guān)測(cè)方程

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

(6)

1.2 線(xiàn)性卡爾曼濾波

在得到線(xiàn)性化的狀態(tài)方程(4)和觀(guān)測(cè)方程(6)之后，可得擴(kuò)展Kalman濾波遞推關(guān)系：

P(k|k+1)=Φ(k|k+1)P(k|k)ΦT(k|k+1)+Q(k+1)，

(7)

P(k+1)=(I-K(k+1)H(k+1))P(k+1|k).

(8)

其中濾波的初值和濾波的誤差方差矩陣的初值分別為

X(0)=E(X(0)),P(0)=var(X(0)).

(9)

(10)

1.3 EKF設(shè)計(jì)步驟

現(xiàn)選用一個(gè)非線(xiàn)性系統(tǒng)來(lái)詳細(xì)說(shuō)明EKF設(shè)計(jì)步驟.設(shè)系統(tǒng)狀態(tài)方程為

(11)

觀(guān)測(cè)方程為

(12)

狀態(tài)方程和觀(guān)測(cè)方程都為非線(xiàn)性系統(tǒng).EKF算法實(shí)現(xiàn)步驟如下：

(ⅰ)初始化初始狀態(tài)X(0)，Z(0)和協(xié)方差矩陣P0；

(ⅵ)協(xié)方差矩陣P(k|k-1)=Φ(k)P(k-1|k-1)ΦT(k)+Q，

(ⅶ)由(7)式可得EKF濾波增益K(k)=P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R)；

(ⅷ)目標(biāo)狀態(tài)更新為X(k)=X(k|k-1)+K(Z(k)-Z(k|k-1))；

(ⅸ)由(8)式可得協(xié)方差更新為P(k)=(In-K(k)H(k))P(k|k-1).

以上步驟為EKF的一個(gè)計(jì)算周期，如此循環(huán)，就是各個(gè)時(shí)刻對(duì)非線(xiàn)性系統(tǒng)的處理過(guò)程.

2 無(wú)跡卡爾曼濾波原理

2.1 UT算法

UT算法是UKF中的核心步驟，是一種對(duì)隨機(jī)變量在經(jīng)過(guò)非線(xiàn)性變換之后進(jìn)行概率統(tǒng)計(jì)的方法.通過(guò)UT算法得到2n+1個(gè)Sigma采樣點(diǎn)X和權(quán)值ω，再來(lái)計(jì)算y的統(tǒng)計(jì)特征：

(1)計(jì)算2n+1個(gè)Sigma采樣點(diǎn)：

(13)

(2)計(jì)算Sigma采樣點(diǎn)相應(yīng)的權(quán)值：

(14)

其中：m為均值；c為協(xié)方差；i為采樣點(diǎn)的位數(shù)；λ為縮放比例參數(shù)，可用其降低預(yù)測(cè)誤差，λ=α2(n+k)-n；α為控制采樣點(diǎn)分布狀態(tài)的值.

在UT算法中，最重要的是確定Sigma采樣點(diǎn)的方法，也就是如何確定使用Sigma采樣點(diǎn)的個(gè)數(shù)、位置和相應(yīng)權(quán)值.

2.2 UKF設(shè)計(jì)步驟

對(duì)于不同時(shí)刻k，X為一個(gè)隨機(jī)變量，Z為一個(gè)觀(guān)測(cè)變量，分別添加高斯白噪聲W(k)和V(k)，構(gòu)成一個(gè)非線(xiàn)性系統(tǒng)：

(15)

其中f為非線(xiàn)性函數(shù)的狀態(tài)方程，h為非線(xiàn)性函數(shù)的觀(guān)測(cè)方程.設(shè)W(k)的協(xié)方差矩陣為Q，V(k)的協(xié)方差矩陣為R，則UKF算法實(shí)現(xiàn)步驟如下：

(ⅰ)根據(jù)UT預(yù)測(cè)方程(13)和(14)，獲得一組Sigma采樣點(diǎn)及其權(quán)值，得到如下的X(i)(k|k)：

(ⅱ)對(duì)2n+1個(gè)Sigma采樣點(diǎn)進(jìn)行預(yù)測(cè)：

X(i)(k+1|k)=f(k,X(i)(k|k))i=1,2,…,2n+1.

(16)

(ⅲ)將(16)式中預(yù)測(cè)到的Sigma采樣點(diǎn)代入到觀(guān)測(cè)方程中，得到預(yù)測(cè)觀(guān)測(cè)量

Z(i)(k+1|k)=h(X(i)(k+1|k))i=1,2,…,2n+1.

(17)

(ⅳ)由(17)式得到的預(yù)測(cè)觀(guān)測(cè)量，通過(guò)加權(quán)求和得到預(yù)測(cè)系統(tǒng)結(jié)果的均值及協(xié)方差：

(18)

(19)

(ⅵ)根據(jù)卡爾曼增益值獲得準(zhǔn)確的狀態(tài)向量和協(xié)方差，對(duì)系統(tǒng)結(jié)果的狀態(tài)向量和協(xié)方差進(jìn)行更新：

P(k+1|k+1)=P(k+1|k)-K(k+1)PZkZkKT(k+1).

由上述步驟可以看出，UKF在處理非線(xiàn)性系統(tǒng)問(wèn)題時(shí)，可以直接在預(yù)測(cè)點(diǎn)附近進(jìn)行UT，得到Sigma采樣點(diǎn)，將采樣點(diǎn)的均值和協(xié)方差與原統(tǒng)計(jì)特性作比較，之后直接進(jìn)行非線(xiàn)性映射，可得到近似的概率密度函數(shù).

3 基于觀(guān)測(cè)距離的EKF和UKF目標(biāo)跟蹤理論及仿真結(jié)果

3.1目標(biāo)跟蹤算法數(shù)學(xué)建模

假定目標(biāo)做勻速直線(xiàn)運(yùn)動(dòng)，運(yùn)動(dòng)速度為v，在k時(shí)刻，設(shè)目標(biāo)的位置為s(k)，經(jīng)過(guò)采樣時(shí)間T之后，目標(biāo)的位置為s(k+1)=s(k)+vT.顯然，在直角坐標(biāo)系中目標(biāo)在x和y方向都有相應(yīng)的分量，運(yùn)動(dòng)系統(tǒng)的狀態(tài)量包括x方向的位置、y方向的位置、x方向的速度分量和y方向的速度分量.在目標(biāo)運(yùn)動(dòng)過(guò)程中，可表示為

X(k+1)=ΦX(k)+G(k)W(k)，

(20)

其中

通過(guò)觀(guān)測(cè)站或者傳感器對(duì)目標(biāo)進(jìn)行探測(cè)(如雷達(dá)探測(cè)).設(shè)雷達(dá)站的位置為(x0,y0)，目標(biāo)k時(shí)刻的位置為(x(k),y(k))，又設(shè)觀(guān)測(cè)量Z(k)為目標(biāo)與雷達(dá)之間的距離，則觀(guān)測(cè)方程為

(21)

其中V(k)為雷達(dá)自身的測(cè)量誤差.由此得到勻速運(yùn)動(dòng)的系統(tǒng)模型，其中狀態(tài)方程(20)為線(xiàn)性的，觀(guān)測(cè)方程(21)為非線(xiàn)性的.

3.2 EKF目標(biāo)跟蹤

根據(jù)局部線(xiàn)性化方法將非線(xiàn)性方程(21)線(xiàn)性化，并由(10)式得到相應(yīng)的雅可比矩陣

(22)

3.3 UKF目標(biāo)跟蹤

根據(jù)3.1節(jié)的數(shù)學(xué)模型，設(shè)采樣時(shí)間間隔T=1 s，運(yùn)行總時(shí)間N=60 s，則過(guò)程驅(qū)動(dòng)矩陣Φ和噪聲驅(qū)動(dòng)矩陣G(k)為常數(shù)矩陣：

W(k)的協(xié)方差矩陣Q=σw*diag(1,1)，其中σw為可調(diào)參數(shù)，σw?1.V(k)的協(xié)方差矩陣R為已知量.設(shè)置UT中的相關(guān)系數(shù)，α=0.01，K=0，β=2，維數(shù)n=9.

(23)

3.4 仿真結(jié)果

根據(jù)(20)—(23)式求解，運(yùn)行各時(shí)刻點(diǎn)，分別得到EKF和UKF的跟蹤軌跡和誤差曲線(xiàn)，結(jié)果如圖1和圖2所示.

EKF和UKF對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤軌跡如圖1所示.從圖1可以看出，EKF對(duì)真實(shí)軌跡的仿真效果沒(méi)有UKF好，當(dāng)橫坐標(biāo)增大到一定程度時(shí)，EKF與真實(shí)軌跡有較大誤差.這是由泰勒展開(kāi)式保留一階近似項(xiàng)、省略二階(及以上)項(xiàng)導(dǎo)致的線(xiàn)性化誤差所引起的，因?yàn)閁KF使用UT，所以在此情況下UKF的仿真結(jié)果優(yōu)于EKF.

EKF和UKF的跟蹤誤差如圖2所示.從圖2可以看出，EKF的跟蹤誤差與UKF的跟蹤誤差的差距很明顯，EKF的變化劇烈，而UKF的較好，且UKF均方差變化小，收斂性能優(yōu)于EKF.這說(shuō)明相較于EKF，UKF具有良好的跟蹤效果.

圖1 EKF和UKF對(duì)運(yùn)動(dòng)目標(biāo)的跟蹤軌跡Fig.1 Track of Moving Targets by EKF and UKF

圖2 EKF和UKF的跟蹤誤差Fig.2 EKF and UKF Tracking Errors

4 結(jié)語(yǔ)

由EKF和UKF對(duì)目標(biāo)跟蹤的原理可知，EKF是將非線(xiàn)性函數(shù)方程通過(guò)求其偏導(dǎo)數(shù)高階導(dǎo)數(shù)和雅可比矩陣作線(xiàn)性化處理，而得到一個(gè)近似的線(xiàn)性化函數(shù)方程，再進(jìn)行目標(biāo)跟蹤；UKF是直接對(duì)非線(xiàn)性函數(shù)方程通過(guò)高斯分布而得到概率密度的近似值，再利用UT得到Sigma采樣點(diǎn)，與之前估計(jì)的狀態(tài)方程進(jìn)行非線(xiàn)性映射，從而得到準(zhǔn)確的目標(biāo)跟蹤方程.所以，UKF的準(zhǔn)確度和收斂性比EKF的好.UKF算法雖然能夠有效提高非線(xiàn)性系統(tǒng)目標(biāo)跟蹤的精度，但目前只是在高斯噪聲的環(huán)境下進(jìn)行處理，若系統(tǒng)是在非高斯的環(huán)境下，則可能出現(xiàn)較大誤差，這就需要對(duì)算法進(jìn)行不斷的改進(jìn).