基于改進(jìn)Sigma點的無跡卡爾曼濾波水下目標(biāo)跟蹤算法

徐正興，諸云，吳祎楠

（南京理工大學(xué)自動化學(xué)院，南京 210094）

1 引 言

海洋擁有廣袤的資源，在經(jīng)濟(jì)、軍事等領(lǐng)域占有重要地位，發(fā)展海洋成為世界發(fā)展不可或缺的一部分。因此，海洋資源的開發(fā)利用以及水下軍事技術(shù)的發(fā)展受到了越來越廣泛的關(guān)注［1］。水下的各種作業(yè)工具及其運(yùn)動載體，是人類了解海洋、發(fā)展海洋的重要工具，其中水下航行器是探索水下世界的主要工具［2］。由于海洋中航行器的種類復(fù)雜、數(shù)量龐大，因此陣列所接收的信號也較為復(fù)雜，這些信號通常由多種水下航行器的輻射噪聲、水上環(huán)境噪聲和目標(biāo)信號組成。同時，在復(fù)雜海洋環(huán)境（如水團(tuán)、湍流、生物群體等）的作用下，一些混響、多途效應(yīng)會使陣列接收信號變得更加復(fù)雜多變［3］，面對這樣復(fù)雜的信號，微弱的目標(biāo)信號掩埋在其中，若不對干擾信號進(jìn)行抑制和消除，很難實現(xiàn)對弱目標(biāo)的探測。目前較為廣泛使用的水下目標(biāo)跟蹤算法有卡爾曼濾波算法以及擴(kuò)展的卡爾曼濾波算法［4］，卡爾曼濾波算法開創(chuàng)了對噪聲優(yōu)化的先河［5］。在此基礎(chǔ)上提出的擴(kuò)展卡爾曼濾波算法則是較好地解決了非線性低階系統(tǒng)的優(yōu)化問題［6］，邁出從線性到非線性的跨越性一步。但是卡爾曼濾波使用的前提條件是已知噪聲的協(xié)方差，過程噪聲和測量噪聲近似為白噪聲，并且兩者獨(dú)立互不相干，因此對于非白噪聲的干擾無法進(jìn)行有效的抑制。而擴(kuò)展的卡爾曼濾波算法只適用于低階非線性系統(tǒng)，因此，為了對高階的非線性系統(tǒng)的噪聲干擾進(jìn)行抑制，Julier等［7-9］基于近似概率分布比近似非線性函數(shù)或非線性變換要相對簡單的思想，提出了采用無跡變換（Unscented Transformation，UT）結(jié)合卡爾曼濾波的無跡卡爾曼濾波算法（Unscented Kalman Filter，UKF）。文獻(xiàn)［10］提出的算法實現(xiàn)了對運(yùn)動噪聲協(xié)方差矩陣的精確估計，具有較高的魯棒性。文獻(xiàn)［11］通過井下無線網(wǎng)絡(luò)、慣性定位終端采集相關(guān)信息數(shù)據(jù)估算目標(biāo)點最終的位置坐標(biāo)。文獻(xiàn)［12］有效提高了目標(biāo)位置和速度的誤差估計、可靠性及穩(wěn)定性。文獻(xiàn)［13］提出的算法具有更好的精度和穩(wěn)定性。雖然以上算法在精度方面有所提升，但是提升的效果并不是非常顯著，往往只有10%～20%的提升。因此，在此基礎(chǔ)上本文所提出的基于改進(jìn)Sigma點的無跡卡爾曼濾波水下目標(biāo)跟蹤算法（Improved Unscented Kalman Filter，IUKF）相對于無跡卡爾曼濾波等算法來說有著更為廣泛的應(yīng)用空間，在噪聲的抑制、估計位置與實際位置、估計距離與實際距離的比對方面效果更好。本文的主要貢獻(xiàn)：（1）本算法可以應(yīng)用到水下航行器及聲吶系統(tǒng)，具有一定的工程應(yīng)用價值；（2）進(jìn)行了二次無跡變換，使得基于Sigma點的更新值以及測量值更加的準(zhǔn)確，從而大幅降低估計值以及預(yù)測值與真值之間的誤差；（3）根據(jù)水下噪聲干擾的特點，除了高階線性以外，對于白噪聲、低階非線性等同樣有著一定的抑制效果。經(jīng)仿真實驗驗證，本方法在噪聲抑制方面相比于其他算法可以取得較好的結(jié)果。

2 無跡卡爾曼濾波算法介紹

無跡變換［14］是在估計點附近確定采樣，用這些采樣點表示的高斯密度近似狀態(tài)的概率密度函數(shù)。無跡變換實現(xiàn)方法為：在原狀態(tài)分布中按某一規(guī)則選取一些采樣點，使這些采樣點的均值和協(xié)方差等于原狀態(tài)分布的均值和協(xié)方差，將這些點代入非線性函數(shù)匯總，得到相應(yīng)的非線性函數(shù)值點集，通過這些點集求取變換后的均值和協(xié)方差。這樣得到的非線性變換后的均值和協(xié)方差精度最少具有2階精度。對于高斯分布，可以達(dá)到3階精度。其采樣點的選擇是基于先驗均值和先驗協(xié)方差矩陣的平方根的相關(guān)列實現(xiàn)的。

定義增廣狀態(tài)向量為

式中，令XX為水下航行器狀態(tài)向量對應(yīng)的Sigma點向量，XW為在航行器航行過程中所產(chǎn)生的噪聲向量對應(yīng)的 Sigma點向量，XV為觀測噪聲向量對應(yīng)的Sigma點向量，按照增廣狀態(tài)向量的定義方式［15］，定義增廣Sigma點向量為

UKF 算法的實現(xiàn)步驟如下：

①設(shè)定水下航行器的增廣狀態(tài)向量賦初值，給定協(xié)方差矩陣Pα0。

②記k時刻增廣狀態(tài)向量為協(xié)方差矩陣為Pαk，Sigma 點向量為

式中，α為可以進(jìn)行設(shè)置的濾波器參數(shù)即比例縮放因子，κ為Sigma采樣點間距離的比例因子，N為增廣狀態(tài)向量維數(shù)。

式（4）～（5）中，為Sigma點集Xi的均值對應(yīng)的權(quán)值，為Sigma點集Xi的方差對應(yīng)的權(quán)值。

③對時間更新進(jìn)行計算，計算狀態(tài)向量所對應(yīng)的Sigma點的預(yù)測值，然后計算水下航行器先驗狀態(tài)估計值和航行器的先驗協(xié)方差矩陣

式（6）～（7）中，無跡卡爾曼濾波算法的W(m)i為計算均值時對應(yīng)第i個Sigma點的權(quán)值，W(c)i為計算協(xié)方差時對應(yīng)第i個Sigma點的權(quán)值。

④對量測進(jìn)行更新，以代替水下航行器狀態(tài)變量，求取Sigma點的測量更新值，接著求取航行器的測量更新值和協(xié)方差矩陣PZk、PXkZk。

⑤求取濾波器增益矩陣，計算水下航行器的后驗狀態(tài)估計和狀態(tài)協(xié)方差矩陣，

⑤完成后，返回②，計算下一時刻的水下航行器的狀態(tài)估計值。無跡卡爾曼濾波算法的一些參數(shù)的設(shè)置如下。比例因子κ≥0 可以保證水下航行器協(xié)方差矩陣的正半定性，通常與增廣狀態(tài)向量的變量個數(shù)有關(guān)，為方便仿真，由于本實驗的某個時刻的增廣狀態(tài)向量為單變量，故本文的比例因子κ=0。縮放因子α控制Sigma點的分布范圍，0<α<1，通常為一個很小的正數(shù)，在本實驗仿真中，令α=0.001。本文假設(shè)測量噪聲滿足高斯分布，參數(shù)的優(yōu)化值反應(yīng)狀態(tài)歷史信息的高階狀態(tài)特性，對高斯分布而言，參數(shù)的優(yōu)化取值為2的時候最優(yōu)，令λ=α2(N+κ)-N。

3 基于改進(jìn)Sigma點的無跡卡爾曼濾波算法的介紹

3.1 系統(tǒng)的狀態(tài)方程

假設(shè)水下航行器做勻速運(yùn)動，水下航行器的初始位置為（X0，Y0），假設(shè)K時刻的位置為（Xk，Yk），假設(shè)初始速度為（Vx，0，Vy，0），K時刻的速度為（Vx，k，Vy，k），則X（k）=［Xk，Vx，Yk，Vy］，其中狀態(tài)變量的關(guān)系為

則目標(biāo)系統(tǒng)的運(yùn)動狀態(tài)方程為

式中，A為水下航行器狀態(tài)轉(zhuǎn)移矩陣，W（k）為系統(tǒng)狀態(tài)噪聲，G為過程噪聲驅(qū)動矩陣，T為采樣時間。

3.2 系統(tǒng)量測方程

假設(shè)岸基顯控設(shè)備在水下任意位置，以岸基顯控設(shè)備為原點，對水下航行器M進(jìn)行跟蹤，可以得到水下航行器M與岸基顯控設(shè)備之間的X軸方向距離以及Y軸方向距離，如圖1所示。

圖1 量測模型Fig.1 Measurement model

由該水下航行器與岸基顯控設(shè)備之間所得的X軸方向距離與Y軸距離可以得出在直角坐標(biāo)系中，系統(tǒng)的量測方程為［15］

3.3 基于改進(jìn)Sigma點的無跡卡爾曼濾波算法

本文提出的IUKF首先在原算法的第②步與第③步之間對Sigma點進(jìn)行了再一次估計

由于水下航行器除了本身不可避免地會產(chǎn)生各種各樣的噪聲之外，還會受到周圍環(huán)境的干擾噪聲，因此將產(chǎn)生的噪聲考慮進(jìn)去后，對水下航行器的先驗狀態(tài)估計值和先驗協(xié)方差矩陣的計算進(jìn)行了一些改進(jìn)使其可以較好地對噪聲達(dá)到抑制效果。

式（19）～（20）中，G為過程噪聲驅(qū)動矩陣，Q為輸入白噪聲w（k）的方差矩陣。接著進(jìn)行二次UT變換，對水下航行器的先驗狀態(tài)估計值和先驗協(xié)方差矩陣進(jìn)行無跡變換

式中，參數(shù)λ=α2(N+κ)-N。這樣進(jìn)行UT變換所使用的則不是初始所給定的水下航行器的增廣狀態(tài)向量賦初值以及給定協(xié)方差矩陣Pα0，而是將所給定的水下航行器的增廣狀態(tài)向量賦初值以及給定協(xié)方差矩陣Pα0經(jīng)過一次UT變換所得到的水下航行器的先驗狀態(tài)估計值和先驗協(xié)方差矩陣，與初始的增廣狀態(tài)向量以及給定的協(xié)方差矩陣相比，經(jīng)過了一次UT變換后得到的增廣狀態(tài)向量、協(xié)方差矩陣精度更高，再作為再次給定的增廣狀態(tài)向量以及給定協(xié)方差矩陣，這樣再次進(jìn)行UT變換的精度更高，與UKF算法相比，降低了估計值以及預(yù)測值與真值位置與距離之間的誤差。IUKF的流程圖如圖2所示。

圖2 IUKF的水下目標(biāo)跟蹤算法流程圖Fig.2 Flow chart of IUKF underwater target tracking algorithm

4 仿真分析

4.1 仿真背景

以某水下航行器及其聲吶系統(tǒng)［16］為研究對象，分別使用UKF以及IUKF對水下航行器的移動進(jìn)行位置估計、距離估計。為避免偶然性，在平面隨機(jī)模擬仿真選取一點為岸基顯控設(shè)備，設(shè)定其坐標(biāo)為（200，300），水下航行器初始位置為（-200，300），假設(shè)其在勻速運(yùn)動，其在x軸上的運(yùn)動速度為20 m/s，在y軸上的運(yùn)動速度為4 m/s。假定隨機(jī)噪聲均滿足高斯分布，模擬的白噪聲的的方差矩陣為［0.0001 0；0 0.0001］，過程驅(qū)動噪聲為［0.5 0；1 0；0 0. 5；0 1］，觀測白噪聲的方差矩陣為［0.001 5 0.05 0.001］；在觀測過程中產(chǎn)生的觀測噪聲方差為5。為減少實驗誤差，將10次實驗結(jié)果進(jìn)行取平均值后再進(jìn)行繪圖，其實驗結(jié)果如4.2節(jié)仿真結(jié)果所示。

4.2 仿真結(jié)果

4.2.1 UKF與IUKF下估計距離與實際距離對比仿真

圖3～4是用UKF與IUKF對水下航行器與岸基顯控設(shè)備距離的估計對比曲線。從圖3可以看出，隨著時間的增加，UKF對距離的估計與實際距離之間的偏差越來越大，而從圖4可以看出，IUKF隨著時間的增加，其估計距離與實際距離的曲線仍然貼合。IUKF相對于傳統(tǒng)UKF對水下航行器與岸基顯控設(shè)備實際距離的估計是更貼近實際距離的。圖5～6是用UKF與IUKF對水下航行器與岸基顯控設(shè)備局部估計距離與局部實際距離誤差的估計對比曲線。隨著時間的增加，使用UKF對水下航行器與岸基顯控設(shè)備實際距離進(jìn)行估測的誤差隨著時間的增大而變大，最終近似達(dá)到了18 m的誤差，而使用IUKF對水下航行器與岸基顯控設(shè)備進(jìn)行估測的誤差曲線一直在-3到3的誤差帶波動。

圖3 UKF下估計距離與真值對比曲線Fig.3 Comparison curve of estimated distance and true value under UKF

圖4 IUKF下估計距離與真值對比曲線Fig.4 Comparison curve of estimated distance and true value under IUKF

圖5 UKF下局部估計距離與真值誤差曲線Fig.5 Error curve of local estimated distance and true value under UKF

圖6 IUKF下局部估計距離與真值誤差曲線Fig.6 Error curve of local estimated distance and truth value under IUKF

4.2.2 UKF與IUKF下估計位置與實際位置對比仿真

圖7～8是使用UKF與IUKF對水下航行器與岸基顯控設(shè)備實際位置的估計對比曲線。隨著時間增加，水下航行器與岸基顯控設(shè)備估計位置與實際位置的誤差變得越來越大，幾米乃至幾十米的誤差在整體曲線圖上很難顯示出來，對于水下航行器與岸基顯控設(shè)備實際位置的估計，UKF與IUKF的效果似乎相差不大。為了保證數(shù)據(jù)的嚴(yán)謹(jǐn)性，對前兩條曲線的后面一段拿出來進(jìn)行了量化研究。圖9～10是用UKF與IUKF對水下航行器與岸基顯控設(shè)備局部實際位置的估計對比曲線。當(dāng)放大后，IUKF對水下航行器與岸基顯控設(shè)備實際位置的估計與實際位置在曲線上較為貼合，而水下航行器與岸基顯控設(shè)備的實際距離通過無跡卡爾曼濾波在放大后在曲線圖上則有了略微的出入。接著計算出水下航行器與岸基顯控設(shè)備局部估計距離與局部實際距離的誤差。

圖7 UKF下估計位置與真值對比曲線Fig.7 Comparison curve of estimated position and true value under UKF

圖8 IUKF下估計位置與真值對比曲線Fig.8 Comparison curve of estimated position and true value under IUKF

圖9 UKF下局部估計位置與真值對比曲線Fig.9 Comparison curve of local estimated position and true value under UKF

圖10 IUKF下局部估計位置與真值對比曲線Fig.10 Comparison curve of local estimated position and true value under IUKF

4.2.3 UKF與IUKF對估計位置與實際位置偏差仿真

圖11～12描述的是水下航行器與岸基顯控設(shè)備估計距離、觀測距離以及估計位置與實際位置的偏差。從曲線圖中可以看出，使用UKF對水下航行器與岸基顯控設(shè)備的觀測距離與相應(yīng)實際距離之間的誤差在0的上下波動，并且誤差峰值較多且相對較高，而使用IUKF對水下航行器與岸基顯控設(shè)備的觀測距離與相應(yīng)實際距離之間的誤差在0的上下波動，但是波動的峰值較低。使用UKF對水下航行器與岸基顯控設(shè)備的估計位置與實際值之間的誤差曲線屬于隨著時間呈上升狀態(tài)，最終水下航行器與岸基顯控設(shè)備之間的位置誤差達(dá)到了18 m，而使用IUKF對水下航行器與岸基顯控設(shè)備的估計位置與實際值之間的誤差則不超過4 m，與傳統(tǒng)的UKF相比誤差降低了77.7%。并且隨著時間的增加，IUKF對水下航行器與岸基顯控設(shè)備的估計位置與實際之間的誤差也一直處于10 m之內(nèi)。

圖11 UKF下估計、觀測值與真值誤差曲線Fig.11 Error curves of estimated,observed and true values under UKF

圖12 IUKF下估計、觀測值與真值誤差曲線Fig.12 Error curves of estimated,observed and true values under IUKF

假設(shè)UKF估計位置和真值偏差為M，IUKF估計位置和真值偏差為N。UKF估計距離和實際距離誤差為m，基于改進(jìn)Sigma點的UKF估計距離和實際距離誤差為n，則

如表1所示，使用傳統(tǒng)的UKF對水下航行器與岸基顯控設(shè)備的估計位置和真值之間的偏差在120 s內(nèi)達(dá)到了13.8 m，對水下航行器與岸基顯控設(shè)備的估計距離和實際距離之間的偏差峰值達(dá)到了17 m，而使用IUKF對水下航行器與岸基顯控設(shè)備的估計位置和真值之間的偏差在120 s內(nèi)僅僅達(dá)到了7.9 m，與使用UKF對水下航行器與岸基顯控設(shè)備的位置進(jìn)行估測相比精度提升了42%以上，與此同時，水下航行器與岸基顯控設(shè)備的估計距離和實際距離之間的偏差峰值也僅為2.7 m，與使用UKF對水下航行器與岸基顯控設(shè)備的距離估測相比精度提升了86%。

表1 UKF與IUKF估計位置、估計距離與真值誤差表Table 1 UKF and IUKF estimated position,estimated distance and truth error table

5 結(jié) 論

本文以位置以及速度作為狀態(tài)變量，以某水下航行器及其聲吶系統(tǒng)為研究對象，分別使用無跡卡爾曼濾波算法以及IUKF對水下航行器與岸基顯控設(shè)備進(jìn)行了位置估計、距離估計［17］。主要對水下航行器與岸基顯控設(shè)備的估計位置與實際位置進(jìn)行了對比分析，對水下航行器與岸基顯控設(shè)備的估計距離與實際距離進(jìn)行了誤差對比分析，對水下航行器與岸基顯控設(shè)備的估計位置和真值偏差曲線進(jìn)行了對比分析。還分別對比了UKF以及IUKF的濾波效果，仿真結(jié)果在肯定了UKF的基礎(chǔ)上也同樣證明了IUKF的濾波的實用性，尤其是在對水下航行器與岸基顯控設(shè)備的估計距離與實際距離的誤差降低以及對估計位置和真值偏差降低方面取得了顯著的優(yōu)化效果，并且IUKF通過二次UT變換還使得精度更高，水下航行器與岸基顯控設(shè)備的估計距離與實際距離的誤差相對UKF降低了86%，估計位置和真值的偏差相對UKF降低了53%，這在圖10和圖12中有著很好的體現(xiàn)。

海洋擁有廣袤的資源，在經(jīng)濟(jì)、軍事等領(lǐng)域占有重要地位，發(fā)展海洋成為世界發(fā)展不可或缺的一部分。在國家海洋強(qiáng)國戰(zhàn)略的引導(dǎo)下，海洋裝備的研究與發(fā)展已成為研究熱點與重點，其中水下航行器更是探索水下世界的主要工具。在提出IUKF并成功實現(xiàn)水下航行器與岸基顯控設(shè)備的估計距離與實際距離的誤差以及估計位置和真值偏差的降低后，接下來將會在此基礎(chǔ)上引入更為復(fù)雜的強(qiáng)干擾，實現(xiàn)水下航行器與動態(tài)目標(biāo)的估計距離與實際距離的誤差以及估計位置與真值偏差的降低。