石桂欣 鄢社鋒 劉 宇

(1 中國(guó)科學(xué)院聲學(xué)研究所 北京 100190)

(2 中國(guó)科學(xué)院大學(xué) 北京 100049)

0 引言

水下目標(biāo)跟蹤在保障海洋安全、維護(hù)海洋權(quán)益和開(kāi)發(fā)海洋資源中具有重要意義，高精度的水下目標(biāo)跟蹤算法一直是水聲信號(hào)處理領(lǐng)域的研究熱點(diǎn)和難點(diǎn)問(wèn)題[1?3]。當(dāng)狀態(tài)轉(zhuǎn)移模型和觀測(cè)模型已知時(shí)，采用濾波(比如卡爾曼濾波)算法對(duì)目標(biāo)的狀態(tài)進(jìn)行估計(jì)是最常用且有效的方法?？柭鼮V波是線性高斯系統(tǒng)的最優(yōu)濾波器，但是實(shí)際的跟蹤系統(tǒng)往往含有非線性結(jié)構(gòu)。擴(kuò)展卡爾曼濾波(Extended Kalman filter,EKF)借助Jacobian 矩陣，將非線性系統(tǒng)進(jìn)行一階泰勒近似，把非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題。但是EKF 只適用于系統(tǒng)非線性程度較弱的情況，且需要事先求解狀態(tài)方程和觀測(cè)方程的Jacobian 矩陣，給實(shí)際應(yīng)用帶來(lái)不便。基于無(wú)跡變換(Unscented transform,UT)，Julier 等[4?5]提出的無(wú)跡卡爾曼濾波(Unscented Kalman filter,UKF)突破了卡爾曼濾波的限制，適用于強(qiáng)非線性目標(biāo)跟蹤系統(tǒng)。雖然UKF 需要調(diào)整參數(shù)以避免濾波發(fā)散，但是由于其高精度的估計(jì)能力，在過(guò)去二十多年里得到了廣泛的應(yīng)用[6?7]。文獻(xiàn)[1,8–11]研究了基于UKF的水下純方位被動(dòng)目標(biāo)跟蹤算法。文獻(xiàn)[12]將自適應(yīng)UKF 算法應(yīng)用于深海條件下航行器的長(zhǎng)基線定位和跟蹤。文獻(xiàn)[13]則進(jìn)一步考慮了水下聲速不確定的移動(dòng)長(zhǎng)基線(Moving long baseline,MLBL)定位跟蹤場(chǎng)景，并使用UKF 算法來(lái)估計(jì)目標(biāo)的狀態(tài)。最近，Arasaratnam 等[14]基于容積準(zhǔn)則(Cubature rules)提出了一種新的濾波算法——容積卡爾曼濾波(Cubature Kalman filter,CKF)。CKF參數(shù)簡(jiǎn)單，運(yùn)算量稍小于UKF，也受到廣泛關(guān)注，并逐漸被應(yīng)用于諸多場(chǎng)景[15?16]。文獻(xiàn)[17]提出了一種基于方差平方根的CKF 算法，用于魚雷的跟蹤。文獻(xiàn)[18]將CKF 算法應(yīng)用于水下多UUV的協(xié)同定位場(chǎng)景中。文獻(xiàn)[19]研究了基于CKF 算法的水下航行器動(dòng)基座對(duì)準(zhǔn)技術(shù)，以提高初始對(duì)準(zhǔn)的精度。為了降低水下長(zhǎng)基線定位系統(tǒng)中航跡推算(Dead reckoning,DR)系統(tǒng)的累積誤差，文獻(xiàn)[20]基于Sage-Husa 最大后驗(yàn)概率準(zhǔn)則(Sage-Husa maximum a posterior)提出了一種自適應(yīng)平方根CKF算法，在過(guò)程噪聲時(shí)變條件下比標(biāo)準(zhǔn)CKF算法估計(jì)精度更高。UKF 和CKF已經(jīng)成為水下目標(biāo)跟蹤中常用的算法。為了更好應(yīng)用UKF 和CKF 這兩種水下目標(biāo)跟蹤中常用的濾波算法，本文結(jié)合實(shí)際應(yīng)用環(huán)境，對(duì)UKF 和CKF 進(jìn)行了針對(duì)性研究。結(jié)合使用距離聯(lián)合方位信息對(duì)目標(biāo)進(jìn)行跟蹤的實(shí)際情況，改進(jìn)提出了適用于測(cè)距誤差有偏的跟蹤算法，推導(dǎo)出兩種算法在運(yùn)動(dòng)方程線性、觀測(cè)方程非線性條件下的簡(jiǎn)化形式并分析了算法的運(yùn)算復(fù)雜度和目標(biāo)軌跡跟蹤性能。

1 狀態(tài)-空間模型

假設(shè)系統(tǒng)在時(shí)刻k的狀態(tài)為Xk ∈Rnx，Zk ∈Rnz為對(duì)應(yīng)狀態(tài)的觀測(cè)信號(hào)，nx為狀態(tài)變量的維數(shù)，nz為量測(cè)向量的維數(shù)。采用如下?tīng)顟B(tài)空間模型描述離散化動(dòng)態(tài)系統(tǒng)：

其中，f(·)為狀態(tài)函數(shù)，h(·)為量測(cè)函數(shù)；wk?1和vk分別為過(guò)程噪聲和量測(cè)噪聲。本文假設(shè)wk?1和vk都是服從高斯分布的零均值噪聲，其方差分別為Qk?1和Rk。

2 測(cè)距誤差有偏情況下的改進(jìn)濾波算法及其簡(jiǎn)化形式

2.1 改進(jìn)濾波算法的狀態(tài)方程和測(cè)量方程

眾多文獻(xiàn)已經(jīng)給出了常用UKF算法和CKF算法的主要步驟[4,14]，本文不再贅述。將其應(yīng)用于目標(biāo)跟蹤系統(tǒng)時(shí)，需要先建立狀態(tài)向量和運(yùn)動(dòng)模型，式(3)給出了一種常用的狀態(tài)向量：

其中，[Bx,By]是觀測(cè)節(jié)點(diǎn)的坐標(biāo)，rk和θk分別是觀測(cè)節(jié)點(diǎn)對(duì)目標(biāo)的測(cè)距和測(cè)向信息，vk是噪聲向量。常規(guī)方法是得到式(4)中的觀測(cè)信息Zk后，直接使用UKF或CKF算法進(jìn)行跟蹤。

但是，常規(guī)方法未考慮距離信息往往是有偏的情況，這會(huì)引起額外的跟蹤誤差。為了提升目標(biāo)跟蹤精度，定義εk為偏差系數(shù)，同時(shí)，將εk也看作狀態(tài)變量之一，構(gòu)建一個(gè)新的狀態(tài)向量，記為Yk，

記?為采樣間隔，則狀態(tài)轉(zhuǎn)移矩陣為

狀態(tài)轉(zhuǎn)移方程為

其中，wk?1是噪聲向量。觀測(cè)信息服從以下模型：

其中，Hk=[0,0,0,0,0,0,1]，rk和θk定義同式(4)，εk是一個(gè)接近于0 的正或負(fù)的未知數(shù)，可能是常數(shù)，也可能隨時(shí)間變化。使用UKF 或者CKF 算法進(jìn)行濾波后，即可得到當(dāng)前時(shí)刻狀態(tài)的估計(jì)值根據(jù)上文分析易得收斂速度很慢。為了使算法更快收斂，每次濾波操作后，對(duì)k的值進(jìn)行如下修正：

式(9)中，ζ為修正系數(shù)，Zk(1)為觀測(cè)向量中的第一項(xiàng)(距離信息)，k為利用估計(jì)出的距離信息。為區(qū)別于標(biāo)準(zhǔn)的UKF 和CKF 算法(使用Xk為狀態(tài)向量)，將本節(jié)改進(jìn)提出的算法分別記為IUKF(Improved Unscented Kalman filter)算法和ICKF(Improved Cubature Kalman filter)算法(使用Yk為狀態(tài)向量)。

2.2 改進(jìn)算法在UKF 框架下的簡(jiǎn)化及復(fù)雜度分析

實(shí)際場(chǎng)景中，經(jīng)常出現(xiàn)目標(biāo)運(yùn)動(dòng)模型為線性方程、觀測(cè)模型為非線性方程的情況。對(duì)于非機(jī)動(dòng)目標(biāo)，跟蹤系統(tǒng)常采用的目標(biāo)運(yùn)動(dòng)模型有勻速運(yùn)動(dòng)模型(Constant velocity,CV)、勻加速運(yùn)動(dòng)模型(Constant acceleration,CA)等；對(duì)于機(jī)動(dòng)目標(biāo)，常用的有Singer 模型、當(dāng)前統(tǒng)計(jì)模型、半馬爾可夫模型等[21]。這些運(yùn)動(dòng)模型均可以寫成線性方程的形式。針對(duì)這些場(chǎng)景，本文進(jìn)一步對(duì)改進(jìn)算法進(jìn)行簡(jiǎn)化分析和推導(dǎo)?？紤]將式(1)重寫為式(7)所示的線性運(yùn)動(dòng)模型，狀態(tài)轉(zhuǎn)移矩陣Φ是一個(gè)nx(nx是狀態(tài)向量的維數(shù))維的方陣，具體取值由運(yùn)動(dòng)模型決定。根據(jù)式(7)表示的狀態(tài)方程，可以將UKF算法中的一步預(yù)測(cè)做相應(yīng)的化簡(jiǎn)。記,i=0,1,2,··· ,2nx為濾波算法的采樣點(diǎn)，可得

易得

然后可得狀態(tài)預(yù)測(cè)協(xié)方差矩陣為

量測(cè)更新的步驟保持不變，即可得到簡(jiǎn)化的UKF 算法，記為SUKF(Simplified Unscented Kalman filter)算法。2.1 節(jié)所提的改進(jìn)算法顯然也可以使用該簡(jiǎn)化步驟，記為IS-UKF(Improved simplified Unscented Kalman filter)算法?；?jiǎn)后的步驟采用矩陣-向量形式執(zhí)行時(shí)間更新過(guò)程，只需要執(zhí)行式(12)和式(15)兩步，即可得到狀態(tài)向量的一步預(yù)測(cè)值及對(duì)應(yīng)的協(xié)方差矩陣。因此，在時(shí)間更新過(guò)程中，不必求2nx+1個(gè)Sigma 點(diǎn)，也就不再需要對(duì)k?1,k?1進(jìn)行Cholesky分解的操作。SUKF的時(shí)間更新步驟中，含有約次乘法和加法操作。而標(biāo)準(zhǔn)UKF 算法大約需要次乘法和加法操作，以及一次nx維的Cholesky 分解(約含有次乘法和加法操作)。

顯然，ηS-UKF是關(guān)于nx/nz的單調(diào)遞減函數(shù)。根據(jù)前文分析可知，如果不考慮量測(cè)函數(shù)的運(yùn)算量，當(dāng)nz的取值接近nx時(shí)，ηS-UKF25%；當(dāng)nz的取值很小(接近1)時(shí)，ηS-UKF50%。

2.3 改進(jìn)算法在CKF 框架下的簡(jiǎn)化及復(fù)雜度分析

類似地，易得相同條件下CKF 算法的簡(jiǎn)化形式。

類似地，狀態(tài)向量的一步預(yù)測(cè)結(jié)果為

可得

因此狀態(tài)預(yù)測(cè)協(xié)方差矩陣為

量測(cè)更新的過(guò)程不變，即可得到簡(jiǎn)化后的SCKF(Simplified Cubature Kalman filter)算法。SCKF 的時(shí)間更新過(guò)程只需執(zhí)行式(18)和式(20)兩步。相應(yīng)地，2.1 節(jié)所提的改進(jìn)算法顯然也可以使用該簡(jiǎn)化步驟，并記為IS-CKF(Improved simplified Cubature Kalman filter)算法。顯然，SCKF 和SUKF 的預(yù)測(cè)更新過(guò)程是一樣的，且與卡爾曼濾波預(yù)測(cè)更新過(guò)程是相同的。這并不難理解，當(dāng)系統(tǒng)的狀態(tài)方程為線性方程時(shí)，非線性濾波算法中的時(shí)間更新過(guò)程自動(dòng)退化成卡爾曼濾波中的時(shí)間更新過(guò)程。卡爾曼濾波是高斯線性系統(tǒng)的最優(yōu)濾波器，說(shuō)明本文化簡(jiǎn)后的結(jié)果是該條件下最優(yōu)的簡(jiǎn)化算法。

由于CKF 和UKF 的算法結(jié)構(gòu)非常相似，對(duì)于同樣的狀態(tài)變量(Xk或者Yk)，運(yùn)算量也很接近，因此SCKF 和SUKF 的運(yùn)算量也很接近。SCKF 降低運(yùn)算復(fù)雜度的分析與前文中對(duì)SUKF 的分析是幾乎一樣的，在此不再贅述。類似地，記OCKF、OS-CKF分別為標(biāo)準(zhǔn)CKF、SCKF 的復(fù)雜度，定義ηS-CKF為SCKF相對(duì)于標(biāo)準(zhǔn)CKF運(yùn)算效率的提升比例，即

顯然，ηS-CKF也是關(guān)于nx/nz的單調(diào)遞減函數(shù)。由于UKF 和CKF 的運(yùn)算量比較接近，SUKF 和SCKF的運(yùn)算量也很接近。因此ηS-CKF和ηS-UKF的取值范圍幾乎是一樣的。

下面以IS-CKF 算法為例，給出本文所提算法的主要步驟，如表1 所示。IS-UKF 與IS-CKF 算法結(jié)構(gòu)相同，只是對(duì)量測(cè)更新部分做了相應(yīng)更改。

表1 IS-CKF 算法步驟Table 1 Improved simplified CKF algorithm

3 仿真實(shí)驗(yàn)與結(jié)果分析

首先使用仿真實(shí)驗(yàn)驗(yàn)證本文所提改進(jìn)算法的性能。使用MATLAB軟件進(jìn)行蒙特-卡洛仿真實(shí)驗(yàn)來(lái)統(tǒng)計(jì)上述幾種算法的平均運(yùn)行時(shí)間和均方根誤差(Root mean square error,RMSE)，并進(jìn)行比較分析。

本實(shí)驗(yàn)中認(rèn)為節(jié)點(diǎn)和目標(biāo)均處于同一水平面上，且深度信息均已知，只考慮x(東西)和y(南北)兩個(gè)方向的坐標(biāo)。如圖1 所示，假設(shè)只有一個(gè)觀測(cè)節(jié)點(diǎn)(坐標(biāo)[Bx,By]= [?100 m,700 m])對(duì)目標(biāo)進(jìn)行測(cè)距和測(cè)向。目標(biāo)從原點(diǎn)處開(kāi)始先做CV 運(yùn)動(dòng)，速度為vx=5 m/s，vy=3 m/s；5 s 后加速度為ax=?0.1 m/s2，ay=0.1 m/s2并持續(xù)到55 s；55～105 s 期間的加速度為ax=0.1 m/s2，ay=?0.1 m/s2；105～110 s 期間做CV 運(yùn)動(dòng)，速度為上一階段結(jié)束時(shí)(105 s)的目標(biāo)速度。實(shí)驗(yàn)中采用CA 模型對(duì)目標(biāo)的運(yùn)動(dòng)狀態(tài)進(jìn)行建模，本文所提改進(jìn)的UKF 和CKF 算法(IS-UKF、IS-CKF和Improved UKF、Improved CKF)的狀態(tài)向量如式(5)所示，狀態(tài)轉(zhuǎn)移矩陣如式(6)所示，觀測(cè)方程為式(8)；常規(guī)UKF 和CKF 算法(SUKF、SCKF 和標(biāo)準(zhǔn)UKF、標(biāo)準(zhǔn)CKF)的狀態(tài)向量如式(3)所示，狀態(tài)轉(zhuǎn)移矩陣如式(22)所示，觀測(cè)方程為式(4)。

常規(guī)算法初始狀態(tài)向量為X0=[5,3,5,3,0,0]T，過(guò)程噪聲方差為Q=diag[1,1,0.1,0.1,0.01,0.01]，初始協(xié)方差矩陣為P0,0=diag[1,1,0.1,0.1,0.01,0.01]。UKF 算法中的參數(shù)為α=1，κ=2，β=0，λ=2，以下同。改進(jìn)算法初始狀態(tài)向量為Y0=[5,3,5,3,0,0,0]T，過(guò)程噪聲方差為Q=diag[1,1,0.1,0.1,0.01,0.01,0.0001]，初始協(xié)方差矩陣為P0,0=diag[1,1,0.1,0.1,0.01,0.01,0.0001]。給定真實(shí)的εk=0.005，量測(cè)噪聲方差為R=diag[25 m2,5×10?6rad2]。修正系數(shù)為ζ=0.8。

圖1 目標(biāo)軌跡Fig.1 Real trajectory of the target

圖2 給出了某一次實(shí)驗(yàn)中SCKF 和IS-CKF 兩種算法估計(jì)的軌跡。由于引入了偏差系數(shù)εk對(duì)濾波結(jié)果進(jìn)行修正，IS-CKF 的濾波精度得以提升。SUKF 和IS-UKF 的軌跡對(duì)比結(jié)果與圖2 類似，因此未再給出。表2和表3給出了這幾種算法進(jìn)行500次蒙特-卡洛實(shí)驗(yàn)后的均方根誤差、平均運(yùn)行時(shí)間對(duì)比。由于簡(jiǎn)化前后的算法精度基本不變，表3 只給出了IS-UKF、IS-CKF、SUKF、SCKF 四種算法的精度對(duì)比結(jié)果。該場(chǎng)景下，相比常規(guī)算法，改進(jìn)算法的估計(jì)精度有一定的提升，稍高于常規(guī)算法，雖然狀態(tài)向量維數(shù)多了一維，但I(xiàn)S-UKF和IS-CKF算法的運(yùn)行時(shí)間分別和相應(yīng)的常規(guī)算法(標(biāo)準(zhǔn)UKF和標(biāo)準(zhǔn)CKF)相當(dāng)。

表2 幾種算法的運(yùn)行時(shí)間對(duì)比(仿真)Table 2 Comparations of running time among improved filters and conventional filters (Simulation)

表3 幾種算法的估計(jì)精度對(duì)比(仿真)Table 3 Comparations of RMSEs among improved filters and conventional filters(Simulation)

圖2 目標(biāo)軌跡的估計(jì)結(jié)果對(duì)比Fig.2 Estimated trajectories by SCKF and ISCKF,respectively

4 千島湖實(shí)驗(yàn)與結(jié)果分析

為了進(jìn)一步驗(yàn)證算法的效果，取2018年3月的一次主動(dòng)探測(cè)湖上實(shí)驗(yàn)數(shù)據(jù)進(jìn)行改進(jìn)算法的性能驗(yàn)證分析。目標(biāo)船的真實(shí)軌跡由差分GPS 給出，觀測(cè)節(jié)點(diǎn)的真實(shí)坐標(biāo)由GPS 給出，且目標(biāo)和觀測(cè)節(jié)點(diǎn)的深度已知，均為5 m。因此以下實(shí)驗(yàn)研究仍然只考慮目標(biāo)的二維平面運(yùn)動(dòng)狀態(tài)信息。本次實(shí)驗(yàn)的觀測(cè)節(jié)點(diǎn)坐標(biāo)為[?500 m,?394 m]。水聲測(cè)距信號(hào)間隔2 s 發(fā)送和采集一次，節(jié)點(diǎn)共采集到740 組距離觀測(cè)信息(方差約為25 m2)，方位信息由GPS 數(shù)據(jù)加高斯噪聲得到(噪聲均值為零、方差為4×10?6rad2)。該跟蹤系統(tǒng)狀態(tài)空間模型與第3 節(jié)仿真實(shí)驗(yàn)中相同。常規(guī)算法和改進(jìn)算法的初始狀態(tài)向量分別為X0=[105,?25,1,?1,0,0]T和Y0=[105,?25,1,?1,0,0,0]T，其他參數(shù)與第3 節(jié)相同。圖3 給出了目標(biāo)的軌跡和節(jié)點(diǎn)的坐標(biāo)。實(shí)驗(yàn)中并未測(cè)量目標(biāo)的速度和加速度信息，因此本節(jié)只對(duì)比位置坐標(biāo)的精度，表4 和表5 分別給出了500次蒙特- 卡洛實(shí)驗(yàn)后上述各個(gè)算法的平均運(yùn)行時(shí)間和對(duì)應(yīng)的RMSE。圖4 給出了某一次實(shí)驗(yàn)中SCKF和IS-CKF兩種算法估計(jì)的軌跡，圖5給出該次實(shí)驗(yàn)中IS-CKF 算法估計(jì)的?εk變化曲線。在該實(shí)驗(yàn)場(chǎng)景下，改進(jìn)算法的位置估計(jì)精度明顯優(yōu)于常規(guī)算法。另外，本文所提出的簡(jiǎn)化算法分別與標(biāo)準(zhǔn)UKF、標(biāo)準(zhǔn)CKF 算法的精度相當(dāng)，而平均運(yùn)行時(shí)間則顯著減少了。

圖3 目標(biāo)軌跡(湖上試驗(yàn))Fig.3 Real trajectory of the target (Lake trial)

圖4 目標(biāo)軌跡的估計(jì)結(jié)果對(duì)比(湖上試驗(yàn))Fig.4 Estimated trajectories by SCKF and ISCKF,respectively (Lake trial)

表4 幾種算法的運(yùn)行時(shí)間對(duì)比Table 4 Comparations of running time among improved filters and conventional filters

表5 幾種算法的估計(jì)精度對(duì)比Table 5 Comparations of RMSEs among improved filters and conventional filters

圖5 k 變化曲線Fig.5 The curve of k against time

5 結(jié)論

結(jié)合距離與方位聯(lián)合進(jìn)行水下目標(biāo)跟蹤的實(shí)際應(yīng)用特點(diǎn)，本文考慮了測(cè)距誤差有偏的情況，基于標(biāo)準(zhǔn)UKF 和CKF 算法提出了相應(yīng)的改進(jìn)算法，并給出了線性狀態(tài)方程條件下的簡(jiǎn)化形式。仿真實(shí)驗(yàn)和湖試實(shí)驗(yàn)數(shù)據(jù)的仿真處理結(jié)果表明，在目標(biāo)的方位信息比較可靠、誤差較小的條件下，改進(jìn)算法估計(jì)精度更高，而運(yùn)算量與常規(guī)算法相當(dāng)，可應(yīng)用于水下目標(biāo)跟蹤。實(shí)際應(yīng)用中，水下環(huán)境復(fù)雜多變，在將來(lái)的工作中，將關(guān)注復(fù)雜模型下的改進(jìn)算法及其在復(fù)雜多變的跟蹤場(chǎng)景中的應(yīng)用。