張程振，丁元明*，楊 陽

（1.大連大學信息工程學院，遼寧 大連 116622；2.大連大學通信與網(wǎng)絡重點實驗室，遼寧 大連 116622）

0 引言

AUV 在港口安全防御、雷區(qū)探測、水下目標打擊等有著重要的應用，其中熱點之一就是利用AUV 進行水下目標跟蹤。經(jīng)典的卡爾曼濾波器（Kalman filter）在目標跟蹤領域得到了廣泛的應用，但在實際應用中需要解決系統(tǒng)非線性與非高斯環(huán)境問題，為此出現(xiàn)了經(jīng)典的改進算法，如擴展卡爾曼濾波算法（extended Kalman filter，EKF）、無跡卡爾曼濾波（ynscented Kalman filter，UKF）等。解決非線性濾波問題的方法還有序列蒙特卡羅方法（sequential monte Carlo methods，SMC），即粒子濾波算法（particle filter，PF）。其正式建立應歸功于Gordon 等人將重采樣技術(shù)（resampling）引入蒙特卡羅采樣過程，有效解決了樣本權(quán)值退化問題。

在現(xiàn)代信號處理、人工智能、目標跟蹤等領域中，粒子濾波算法的應用不斷擴展。Isard和Blake 首先提出Condensation 算法利用粒子濾波解決目標跟蹤問題，在該算法中，使用先驗概率密度作為重要性采樣密度函數(shù)，從中抽取粒子，使用得到的樣本集合近似表示后驗概率密度，但只是適用于簡單場景下的目標跟蹤。Wang 等人針對純方位目標跟蹤問題，提出了融合當前時刻觀測信息的卡爾曼粒子濾波算法，其跟蹤效果與傳統(tǒng)卡爾曼濾波算法相比具有明顯的優(yōu)勢，但是沒有考慮目標距離信息。Liu 等人提出一種基于自適應馬爾科夫鏈的粒子濾波跟蹤算法，能夠以較少的迭代次數(shù)使粒子濾波收斂于后驗分布，在實時環(huán)境中跟蹤機動目標獲得較高的準確率，但只是二維狀態(tài)下的目標跟蹤。劉暢等針對粒子濾波中傳統(tǒng)重采樣存在的濾波性能不穩(wěn)定、有效粒子數(shù)波動劇烈、樣貧的缺點，提出了一種基于Chopthin 重采樣粒子濾波的目標跟蹤算法，其能夠改善傳統(tǒng)重采樣存在的樣貧，但是沒有考慮水下噪聲的情況。劉紅慶等提出一種基于高斯Monte Carlo 粒子濾波的機動目標跟蹤算法，提高了機動目標跟蹤精度和跟蹤效率，但只考慮了高斯白噪聲。

選擇適用的算法對目標跟蹤具有決定性的作用，但是大多數(shù)目標跟蹤研究很少考慮噪聲類型對算法的影響，以及算法跟蹤的時效性問題。PF、EPF和UPF 算法是適用于非高斯噪聲條件下的非線性系統(tǒng)濾波，本文研究了EPF和UPF 的關(guān)系，建立了水下三維目標跟蹤模型，分析比較了3 種濾波算法在高斯噪聲和散粒噪聲下目標跟蹤的能力。

1 數(shù)學建模

1.1 目標運動分析

考慮到AUV 最初處于（r，r，r）位置，并且假設目標運動是近似直線運動，使速度保持恒定值。目標在時刻的狀態(tài)向量表示為：

其中，r（），v（）表示目標在軸方向的位置和速度，軸和軸同理。

基于當前時間狀態(tài)向量計算下一時間的相對狀態(tài)向量，如式（2）所示：

式中，Φ 是按照公式計算的狀態(tài)轉(zhuǎn)移矩陣：

（）是過程噪聲，Γ 是過程噪聲驅(qū)動矩陣：

過程噪聲的協(xié)方差矩陣按公式計算：

式中，表示過程噪聲中的方差，是一個可以調(diào)節(jié)的參數(shù)，且＜＜1。

AUV 觀測方程式僅有距離，且觀測方程表示為：

式中，

1.2 粒子濾波算法

粒子濾波（PF）的基本思路是選取重要性概率密度并從中隨機抽樣，取得帶有權(quán)值的樣本后，在狀態(tài)觀測的基礎上調(diào)節(jié)粒子的位置和權(quán)重的大小，再使用這些樣本近似逼近狀態(tài)后驗分布，最后把樣本進行加權(quán)求和作為狀態(tài)的估計值。

PF 算法流程如下：

1.3 擴展粒子濾波算法

擴展粒子濾波（EPF）是一種局部線性化的方法，它通過一階Taylor 展開式實現(xiàn)，是一種遞歸的最小均方誤差MMSE 估計方法，要求系統(tǒng)是近似高斯后驗分布模型。EPF 算法在采樣階段，可以利用EKF 算法為每個粒子計算均值和協(xié)方差，然后利用該均值和方差“指導”采樣。

EPF 算法流程如下：

1.4 無跡粒子濾波算法

無跡粒子濾波UPF 與EKF 不同的是，它并不是對非線性方程和在估計點處做線性化逼近，而是利用無跡變換（UT）在估計點附近確定采樣，用這些樣本點表示的高斯密度近似狀態(tài)的概率密度函數(shù)。

UPF 算法流程如下：

2）For=1：

重要性采樣階段：

For=1：

用UKF 算法計算均值和方差。

計算Sigma 點集合。

對2+1 個Sigma 點集做一步預測：

計算系統(tǒng)狀態(tài)量的一步預測和協(xié)方差矩陣：

根據(jù)一步預測值，再次使用無跡變換，產(chǎn)生新的Sigma 點集：

由上一步預測的Sigma 點集代入系統(tǒng)觀測函數(shù)式（6）中，得到預測的觀測量，=1，2，…，2+1。

由上一步得到的Sigma 點集的觀測預測值，通過加權(quán)求和得到系統(tǒng)預測的均值及協(xié)方差：

融入最新的觀測，并更新。

互相關(guān)協(xié)方差為：

計算卡爾曼增益矩陣：

計算系統(tǒng)的狀態(tài)更新和協(xié)方差更新：

計算采樣更新粒子。

For=1：，為每個粒子重新計算權(quán)重。

For=1：，歸一化權(quán)重。

3）選擇階段（重采樣）：

1.5 擴展粒子濾波與無跡粒子濾波的關(guān)系

討論EPF和UPF 的理論關(guān)系，其實就是討論EKF和UKF 的關(guān)系。UKF 的交叉協(xié)方差矩陣由Sigma 點近似，如式（33）所示?？梢栽跔顟B(tài)空間模型是線性的假設下導出以下方程：

因此，EKF和UKF 具有相同的卡爾曼增益意義，即，

P為噪聲的協(xié)方差矩陣。從導數(shù)和狀態(tài)估計器中，可以發(fā)現(xiàn)非線性模型狀態(tài)估計中UKF和EKF之間的一些理論關(guān)系：

1）UKF和EKF 都在卡爾曼濾波框架內(nèi)運行。估計狀態(tài)的準確性取決于預測狀態(tài)X、預測測量值Z以及卡爾曼增益矩陣。

2）在大多數(shù)非線性狀態(tài)下，UKF 比EKF 具有更高的預測精度。UKF 的狀態(tài)均值和協(xié)方差矩陣是通過非線性函數(shù)傳播的加權(quán)Sigma 點得到。傳播過程UT 變換避免了線性化誤差的引入，從而降低了濾波器的性能。

3）預測構(gòu)成新息向量，影響最終估計的準確性。UKF 的預測測量值Z的精度比EKF 的精度高。UKF 先通過系統(tǒng)動力學模型，然后通過觀測模型，用Sigma 點逼近預測測量值。第1 次傳播接收系統(tǒng)預測狀態(tài)，如2）中提到的。第2 次傳播，即變換的Sigma 點通過非線性觀測模型傳播，得到預測的測量值。然而，擴展卡爾曼濾波是通過線性化模型、系統(tǒng)模型和觀測模型來獲取預測值，在預測中存在線性化帶來的誤差。

4）估計狀態(tài)X的可靠性依賴于卡爾曼增益矩陣K，EKF 使用預測協(xié)方差矩陣P和P以及線性化的設計矩陣H來計算卡爾曼增益。上述3 項均存在線性化誤差。然而，UKF 通過加權(quán)Sigma 點獲得交叉協(xié)方差矩陣P和預測測量協(xié)方差矩陣P。這些點可以用來準確地計算二階的均值和協(xié)方差。

5）與EKF 相比，UKF 估計過程不需要非線性方程的雅可比矩陣。

6）UKF 通過與系統(tǒng)狀態(tài)向量維數(shù)相關(guān)的非線性函數(shù)迭代傳播Sigma 點，而EKF 只需要計算一次雅可比矩陣。

7）UKF 計算狀態(tài)協(xié)方差矩陣的平方根來采樣Sigma 點，目的是匹配當前的統(tǒng)計特性。Cholesky 分解是實現(xiàn)正定矩陣平方根的一種有效方法。然而，分解需要數(shù)值的穩(wěn)定性，因為UKF 的迭代并不總是保證產(chǎn)生正定矩陣。

1.6 高斯噪聲和散粒噪聲

高斯白噪聲是一種簡單的隨機過程，是由一系列不相關(guān)的隨機變量組成的理想化隨機過程。散粒噪聲（SN）通常是由于信號本身和統(tǒng)計交互過程中的統(tǒng)計波動造成的，是有色噪聲的一種。在SN 的情況下，噪聲幅度突然增大或減小。高斯噪聲樣本的幅度分布在-1～1 之間，噪聲幅度均值為零；SN 樣本是振幅大于1，為幅度較高的噪聲，并且噪聲幅度的平均值也不為零。

2 目標跟蹤仿真結(jié)果分析

2.1 模型初始化

設定樣本的采樣間隔是1 s，模擬時間為60 s，粒子數(shù)目=50，初始化EPF、UPF 中協(xié)方差矩陣P（6），同時設置UT 變換中的相關(guān)系數(shù)=1、=0、=2，維數(shù)=1。針對兩種觀測噪聲，分別用3 種算法做50 次蒙特卡羅仿真，得出某一次實驗下的跟蹤圖像、位置誤差和3 種算法的RMSE 曲線圖。

2.2 仿真結(jié)果

2.2.1 高斯噪聲實驗

圖1 給出了在某一次實驗中，PF、EPF和UPF算法的跟蹤曲線，可以看出EPF 的跟蹤效果最好，PF 次之，EPF 最差且有局部震蕩。下頁圖2 是該次實驗對應每一時刻的位置偏差，EPF 算法有隨時間增長的局勢。

圖1 高斯噪聲實驗下的目標跟蹤曲線圖

圖2 高斯噪聲實驗下的跟蹤誤差曲線圖

圖3 是50 次仿真實驗，3 種算法的RMSE 值的分布。對于相同的場景，UPF和EPF 相比，EPF 的跟蹤效果是不穩(wěn)定的，且RMSE 值整體偏大。PF 算法和EPF 算法整體的RMSE 值都保持在較低的水平。圖4 給出了3 種算法的運行時間，可以看到PF 算法運行時間最少，UPF和EPF 算法時間相當。

圖3 高斯噪聲背景下的RMSE

圖4 高斯噪聲運行時間曲線

2.2.2 散粒噪聲實驗

SN 的實現(xiàn)可以在MATLAB 中使用“randn”來設計，其中高斯噪聲與一些隨機的高幅噪聲相加，設計的SN 如下所示：

圖5～圖8 給出了PF、EPF和UPF 算法在散粒噪聲下的仿真結(jié)果?？梢钥闯?，3 種算法在前期保持較低的誤差估計，隨著時間推移，UPF和PF 算法的誤差有上升趨勢，EPF 的跟蹤誤差大，且不穩(wěn)定，RMSE 值整體偏大。PF和UPF 算法都能較好地實現(xiàn)目標跟蹤，且RMSE 值保持在較低的水平。算法的處理時間和高斯情況下基本一致。

圖5 散粒噪聲實驗下的目標跟蹤曲線圖

圖6 散粒噪聲實驗下的跟蹤誤差曲線圖

圖7 散粒噪聲背景下的RMSE

圖8 散粒噪聲運行時間曲線

2.2.3 不同過程噪聲實驗

給出PF、EPF、UPF 的誤差如表1 所示。從表1中可以看出，當系統(tǒng)過程噪聲的值較小時，UPF算法的性能是較優(yōu)越的，然而隨之值的增大，UPF 算法的表現(xiàn)低于EPF。3 種算法在應對比較大的過程噪聲時的表現(xiàn)，PF 是最差，EPF 最優(yōu)。

表1 PF、EPF、UPF 誤差表

3 結(jié)論

本文比較分析了PF、UPF和EPF 算法在高斯噪聲和散粒噪聲情況下AUV 目標跟蹤的性能。用不同的噪聲條件驗證了3 種濾波器的跟蹤精度。在高斯噪聲中，UPF和PF 獲得了精確的估計，EPF 誤差較大；在散粒噪聲中，EPF 估計不穩(wěn)定，PF和UPF的精度都是可以接受的，保持在較低的水平。

當AUV 系統(tǒng)測量噪聲不變時，只改變目標過程噪聲的值，隨著的增大，目標的運動軌跡將不再是直線，而是彎曲的軌跡，EPF 的跟蹤性能要明顯優(yōu)于PF，且比UPF 優(yōu)。UPF 在兩種噪聲條件下表現(xiàn)出了均衡的性能。在實際中，如果目標的狀態(tài)噪聲較大，可以考慮使用EPF 算法，如果對時效性有需求，且目標狀態(tài)噪聲較小時，可以使用PF 算法，如果沒有時效性需求，UPF 算法較優(yōu)越。