水下目標(biāo)跟蹤粒子濾波算法性能分析*

張程振，丁元明*，楊 陽(yáng)

（1.大連大學(xué)信息工程學(xué)院，遼寧 大連 116622；2.大連大學(xué)通信與網(wǎng)絡(luò)重點(diǎn)實(shí)驗(yàn)室，遼寧 大連 116622）

0 引言

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

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

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

1 數(shù)學(xué)建模

1.1 目標(biāo)運(yùn)動(dòng)分析

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

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

基于當(dāng)前時(shí)間狀態(tài)向量計(jì)算下一時(shí)間的相對(duì)狀態(tài)向量，如式（2）所示：

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

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

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

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

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

式中，

1.2 粒子濾波算法

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

PF 算法流程如下：

1.3 擴(kuò)展粒子濾波算法

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

EPF 算法流程如下：

1.4 無(wú)跡粒子濾波算法

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

UPF 算法流程如下：

2）For=1：

重要性采樣階段：

For=1：

用UKF 算法計(jì)算均值和方差。

計(jì)算Sigma 點(diǎn)集合。

對(duì)2+1 個(gè)Sigma 點(diǎn)集做一步預(yù)測(cè)：

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

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

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

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

融入最新的觀測(cè)，并更新。

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

計(jì)算卡爾曼增益矩陣：

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

計(jì)算采樣更新粒子。

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

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

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

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

討論EPF和UPF 的理論關(guān)系，其實(shí)就是討論EKF和UKF 的關(guān)系。UKF 的交叉協(xié)方差矩陣由Sigma 點(diǎn)近似，如式（33）所示。可以在狀態(tài)空間模型是線性的假設(shè)下導(dǎo)出以下方程：

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

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

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

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

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

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

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

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

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

1.6 高斯噪聲和散粒噪聲

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

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

2.1 模型初始化

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

2.2 仿真結(jié)果

2.2.1 高斯噪聲實(shí)驗(yàn)

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

圖1 高斯噪聲實(shí)驗(yàn)下的目標(biāo)跟蹤曲線圖

圖2 高斯噪聲實(shí)驗(yàn)下的跟蹤誤差曲線圖

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

圖3 高斯噪聲背景下的RMSE

圖4 高斯噪聲運(yùn)行時(shí)間曲線

2.2.2 散粒噪聲實(shí)驗(yàn)

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

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

圖5 散粒噪聲實(shí)驗(yàn)下的目標(biāo)跟蹤曲線圖

圖6 散粒噪聲實(shí)驗(yàn)下的跟蹤誤差曲線圖

圖7 散粒噪聲背景下的RMSE

圖8 散粒噪聲運(yùn)行時(shí)間曲線

2.2.3 不同過(guò)程噪聲實(shí)驗(yàn)

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

表1 PF、EPF、UPF 誤差表

3 結(jié)論

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

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