徐正興，諸云，吳祎楠

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

1 引 言

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

2 無(wú)跡卡爾曼濾波算法介紹

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖1 量測(cè)模型Fig.1 Measurement model

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

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

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

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

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

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

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

4 仿真分析

4.1 仿真背景

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

4.2 仿真結(jié)果

4.2.1 UKF與IUKF下估計(jì)距離與實(shí)際距離對(duì)比仿真

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

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

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

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

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

4.2.2 UKF與IUKF下估計(jì)位置與實(shí)際位置對(duì)比仿真

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

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

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

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

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

4.2.3 UKF與IUKF對(duì)估計(jì)位置與實(shí)際位置偏差仿真

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

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

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

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

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

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

5 結(jié) 論

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

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