基于Huber M-CKF的UUV目標(biāo)跟蹤算法

王 斌, 溫 泉, 范世東

基于Huber M-CKF的UUV目標(biāo)跟蹤算法

王 斌1, 溫 泉2, 范世東1

(1. 武漢理工大學(xué)能源與動(dòng)力工程學(xué)院, 湖北 武漢, 430063; 2. 長(zhǎng)江航道規(guī)劃設(shè)計(jì)研究院, 湖北 武漢, 430010)

針對(duì)無(wú)人水下航行器(UUV)目標(biāo)跟蹤精度不高的問(wèn)題, 文中將一種魯棒性較強(qiáng)的M極大似然估計(jì)代價(jià)函數(shù)引入Huber-容積卡爾曼濾波(H-CKF)并應(yīng)用于UUV的目標(biāo)跟蹤定位算法中, 通過(guò)改變歸一化新息協(xié)方差的方法對(duì)CKF矩陣進(jìn)行線性化求解。建立了UUV運(yùn)動(dòng)模型及觀測(cè)模型, 在不同的非高斯噪聲干擾下與轉(zhuǎn)換測(cè)量卡爾曼濾波、CKF和擴(kuò)展卡爾曼濾波3種濾波算法進(jìn)行對(duì)比試驗(yàn), 驗(yàn)證了HM-CKF的濾波精度和穩(wěn)定性優(yōu)于傳統(tǒng)算法。

無(wú)人水下航行器; 卡爾曼濾波; M極大似然估計(jì)代價(jià)函數(shù)

0 前言

無(wú)人水下航行器(unmanned undersea vehicle, UUV)技術(shù)在軍事領(lǐng)域、漁業(yè)以及海底探測(cè)等方面正在扮演著越來(lái)越重要的角色[1]。目前UUV的目標(biāo)跟蹤主要通過(guò)檢測(cè)目標(biāo)的有效輻射(例如聲輻射和電磁輻射), 以獲取目標(biāo)相對(duì)于UUV的連續(xù)方位角[2]。但是在水下惡劣情況下被動(dòng)聲吶的測(cè)量精度很低, 且海洋情況(海水溫度、密度和鹽度變化等)復(fù)雜, 噪聲統(tǒng)計(jì)特性(平均值和協(xié)方差)未知, 估計(jì)的準(zhǔn)確性未知。同時(shí), 非線性的測(cè)量量存在于UUV的目標(biāo)跟蹤中, 給濾波算法帶來(lái)了極大的困難, 因此精算濾波算法對(duì)于UUV的目標(biāo)跟蹤來(lái)說(shuō)具有極大的工程意義[3-5]。

目前UUV的目標(biāo)跟蹤方法主要有粒子濾波(particle filter, PF)、擴(kuò)展卡爾曼濾波(extended Ka- lman filter, EKF)、無(wú)跡卡爾曼濾波(unscented Kal- man filter, UKF)及容積卡爾曼濾波(cubature Kal- man ?lter, CKF)等。PF依據(jù)蒙特卡洛仿真思想, 通過(guò)隨機(jī)產(chǎn)生大量粒子來(lái)近似后驗(yàn)概率密度。因此粒子濾波計(jì)算量相當(dāng)大, 對(duì)于實(shí)時(shí)性的要求很難滿足, 且有粒子退化和貧化等問(wèn)題[6]。邊信黔等[7]將EKF算法應(yīng)用到UUV的航跡推算上, 取得較好效果, 但EKF對(duì)高非線性系統(tǒng)的預(yù)測(cè)準(zhǔn)確性較差, 而且需要計(jì)算雅可比矩陣, 導(dǎo)致計(jì)算系統(tǒng)復(fù)雜, 應(yīng)用性不強(qiáng)[8]。許多學(xué)者也提出了基于UKF的狀態(tài)估計(jì)算法, 該算法的性能優(yōu)于EKF, 且無(wú)需計(jì)算雅克比矩陣, 但UKF使用權(quán)值為負(fù)的Sigma點(diǎn)來(lái)更新矩陣, 在高維系統(tǒng)中準(zhǔn)確度低, 計(jì)算復(fù)雜在工程中難以實(shí)現(xiàn)[9]。Arasaratnam等[10]提出了基于容積變換的CKF方法, 其具有編號(hào)穩(wěn)定, 計(jì)算復(fù)雜度低, 操作簡(jiǎn)單等優(yōu)點(diǎn), 但是也不能很好地解決非高斯白噪聲問(wèn)題。

為此, 文中利用具有魯棒性的M極大似然估計(jì)函數(shù)[11]來(lái)改造Huber-CKF(H-CKF), 先通過(guò)重建CKF的測(cè)量信息, 對(duì)非線性測(cè)量信息進(jìn)行濾波, 再利用高階容積準(zhǔn)則對(duì)該濾波框架進(jìn)行近似計(jì)算, 進(jìn)而得到量測(cè)噪聲下的Huber M-CKF(HM-CKF)算法。這樣就無(wú)須使用傳統(tǒng)的算法來(lái)避免噪聲對(duì)信息的干擾, 從而實(shí)現(xiàn)了濾波算法的魯棒性。在不同的非高斯噪聲干擾下與CKF、EKF、轉(zhuǎn)換測(cè)量卡爾曼濾波(converted measurement Kalman fil- ter, CMKF)進(jìn)行了仿真對(duì)比試驗(yàn), 驗(yàn)證了HM- CKF的濾波精度和穩(wěn)定性更優(yōu)于傳統(tǒng)算法。

1 UUV運(yùn)動(dòng)模型建立

1.1 運(yùn)動(dòng)方程

為了模擬UUV在水下的運(yùn)動(dòng), 首先需要對(duì)目標(biāo)的運(yùn)動(dòng)規(guī)律進(jìn)行建模, 假設(shè)目標(biāo)在三維空間內(nèi)以勻速直線運(yùn)動(dòng), 則在離散時(shí)間系統(tǒng)下+1時(shí)刻目標(biāo)運(yùn)動(dòng)方程可以表示為

在實(shí)際運(yùn)行過(guò)程中, 目標(biāo)不可避免地會(huì)受到環(huán)境因素的影響, 速度會(huì)發(fā)生小范圍的波動(dòng), 不可能做嚴(yán)格的勻速直線運(yùn)動(dòng), 這些影響因素可以視為過(guò)程噪聲來(lái)進(jìn)行建模, 所以加入過(guò)程噪聲將運(yùn)動(dòng)方程修正為

目標(biāo)速度可表示為

將運(yùn)動(dòng)方程用矩陣的形式表示為

1.2 量測(cè)方程

應(yīng)用泰勒展開式, 并忽略高階項(xiàng), 不難得到

式中,=[3,3], 為測(cè)量轉(zhuǎn)換矩陣。

在理想狀態(tài)沒(méi)有系統(tǒng)偏差的條件下

在有系統(tǒng)偏差條件下

以上數(shù)學(xué)模型可計(jì)算出模擬聲吶收集的信號(hào)數(shù)據(jù)。

2 算法描述

2.1 EKF

EKF原理和經(jīng)典卡爾曼濾波的原理基本一致, 都是以線性最小方差估計(jì)理論為基本出發(fā)點(diǎn), 通過(guò)遞推算法, 從已有的測(cè)量信號(hào)中估算選取有用信號(hào), 并利用前一時(shí)刻的估計(jì)值和當(dāng)下時(shí)間的測(cè)量值來(lái)改變對(duì)變量的估算, 得到當(dāng)下時(shí)刻的狀態(tài)值。與經(jīng)典卡爾曼濾波不同的是, EKF算法把非線性函數(shù)在最優(yōu)估計(jì)點(diǎn)附近進(jìn)行了1階或2階泰勒展開, 有選擇地丟棄了高階項(xiàng), 從而把非線性模型簡(jiǎn)單線性化[7]。非線性系統(tǒng)可由狀態(tài)方程式(11)和量測(cè)方程式(12)表示。

2.2 CKF

構(gòu)造條件性高斯模型如下

CKF算法流程表述如下[4]。

CKF時(shí)間更新為

CKF量測(cè)更新為

2.3 HM-CKF

令

則有

構(gòu)造Huber M估計(jì)的代價(jià)函數(shù)為[14]

最后對(duì)方差陣進(jìn)行修正, 修正后的方差陣為

需要指出的是, 上式的推導(dǎo)沒(méi)有改變CKF解的形式, 也沒(méi)有對(duì)其進(jìn)行線性化近似, 從而新算法保留了CKF非求導(dǎo)、精度高、穩(wěn)定性好的優(yōu)點(diǎn), 同時(shí)也能有效抵抗異常噪聲的干擾。

3 仿真結(jié)果與分析

初始方差為

定義位置均方根誤差

仿真設(shè)定UUV模型為深?！昂＊{”號(hào)UUV, 攜帶高度計(jì)、/傾角傳感器和前視聲吶[15], 其主要性能指標(biāo)如表1所示。

表1 仿真模型參數(shù)

3.1 軌跡圖對(duì)比

式(1)~式(5)模擬產(chǎn)生UUV的真實(shí)軌跡, 所得數(shù)據(jù)稱之為真值, 由于此模型中所假設(shè)UUV在方向上速度為0, 在-平面上做勻速直線運(yùn)動(dòng); 但是實(shí)際應(yīng)用中的聲吶無(wú)法直接得到真實(shí)值, 只能得到測(cè)量值, 也就是文中數(shù)學(xué)模型中添加的噪聲測(cè)量值, 由于噪聲誤差的存在, 聲吶得到的數(shù)據(jù)可能與真實(shí)值差別很大, 其中真實(shí)軌跡和測(cè)量軌跡如圖1所示, 圖中, 橫、縱坐標(biāo)分別為大地坐標(biāo)系的和方向。

圖1 無(wú)人水下航行器仿真航跡

3.2 誤差對(duì)比

1) 線性系統(tǒng)

文中在、、這3個(gè)方向進(jìn)行了仿真試驗(yàn), 得到線性系統(tǒng)下誤差對(duì)比如圖2所示。

圖2 各方向?yàn)V波前后誤差比較

由圖2可知, 濾波前的誤差值(黑色曲線)在0值上下波動(dòng)很大, 但經(jīng)過(guò)濾波后的誤差值(紅色曲線)在15 s過(guò)后非常穩(wěn)定, 保持在0值附近, 即在線性系統(tǒng)內(nèi)經(jīng)過(guò)卡爾曼濾波后誤差顯著減小。

2) 非線性系統(tǒng)

在高非線性系統(tǒng)中, 文中為突顯各種濾波算法的優(yōu)劣, 在3種典型的聲吶工況下將4種濾波方法進(jìn)行對(duì)比, 每組進(jìn)行50次蒙特卡洛仿真試驗(yàn)。狀態(tài)方程過(guò)程噪聲協(xié)方差和轉(zhuǎn)換測(cè)量噪聲的標(biāo)準(zhǔn)差取值如表2所示[4]。

表2 3種工況下狀態(tài)方程過(guò)程噪聲協(xié)方差和標(biāo)準(zhǔn)差

圖3~圖8分別給出了UUV直航情況下各濾波算法pos對(duì)比圖。

由上圖可知, 在高非線性系統(tǒng)內(nèi)CMKF已經(jīng)發(fā)散, 位置誤差值偏大, 在工程應(yīng)用上表現(xiàn)不佳。相比較于CMKF, EKF和CKF收斂速度明顯加快, 數(shù)值也比較穩(wěn)定, 在低噪聲條件下平均位置誤差值穩(wěn)定在50 m內(nèi), 在高噪聲條件下平均位置誤差值穩(wěn)定在1 000 m以內(nèi), 工程上表現(xiàn)良好; 但EKF和CKF還是存在很大不足, 2種濾波算法在高噪聲條件下的平均位置誤差超過(guò)500 m, 無(wú)法再減小, 性能不如HM-CKF。

圖3 工況1下各濾波算法位置均方根誤差(RMSEpos)對(duì)比曲線

圖4 工況1下容積卡爾曼濾波和擴(kuò)展卡爾曼濾波的RMSEpos放大曲線

圖5 工況2下各濾波算法RMSEpos對(duì)比曲線

圖6 工況2下CKF和EKF的RMSEpos放大曲線

圖7 工況3下各濾波算法RMSEpos對(duì)比曲線

圖8 工況3下CKF和EKF的RMSEpos放大曲線

從收斂速度方面分析, HM-CKF從20 s左右已經(jīng)開始收斂, EKF和CKF則從30 s左右才開始收斂, 前者收斂速度明顯更快; 從平均位置誤差值方面分析, HM-CKF最后收斂的誤差值明顯小于EKF和CKF的收斂誤差值。

通過(guò)前面理論分析和仿真案列, 可得到如下結(jié)論:

1) 卡爾曼濾波在處理線性系統(tǒng)誤差方面性能優(yōu)異, 但在非線性系統(tǒng)內(nèi)性能不佳;

2) 在低噪聲狀況下, CKF無(wú)論在收斂速度或是位置誤差值方面都優(yōu)于EKF, 但在高噪聲條件下二者性能差別不明顯;

3) HM-CKF在水下目標(biāo)的信號(hào)收集處理方面表現(xiàn)良好, 極大地減小了位置誤差, 且比CKF和EKF收斂速度更快, 位置誤差數(shù)值更小, 同時(shí)HM-CKF的數(shù)值穩(wěn)定性和跟蹤精度更高。

4 結(jié)束語(yǔ)

文中基于魯棒性較強(qiáng)的Huber M極大似然估計(jì)代價(jià)函數(shù), 針對(duì)H-CKF算法在復(fù)雜的非高斯噪聲環(huán)境下估計(jì)精度不高的問(wèn)題, 進(jìn)行了求解優(yōu)化和仿真驗(yàn)證。仿真結(jié)果顯示, 在含復(fù)雜非高斯噪聲的環(huán)境下, HM-CKF較CMKF、CKF和EKF,的數(shù)據(jù)曲線明顯收斂更快、更穩(wěn)定, 誤差更小, 在非線性系統(tǒng)中其適用性更強(qiáng)。但在實(shí)際工程中, 有色噪聲干擾往往比較大, HM-CKF雖比傳統(tǒng)濾波算法表現(xiàn)得更為穩(wěn)定, 但絕對(duì)精度卻并不太高, 接下來(lái)的研究將著重優(yōu)化HM-CKF算法的精度。

[1] 李永龍, 王皓冉, 張華. 水下機(jī)器人在水利水電工程檢測(cè)中的應(yīng)用現(xiàn)狀及發(fā)展趨勢(shì)[J]. 中國(guó)水利水電科學(xué)研究院學(xué)報(bào), 2018, 16(6): 586-590.Li Yong-long, Wang Hao-ran, Zhang Hua.Application St- atus and Development Trend of Underwater Robot in Wat- er and Hydropower Engineering Detection[J]. Journal of China Institute of Water Resources and Hydropower Research, 2018, 16(6): 586-590.

[2] 龐碩, 糾海峰. 智能水下機(jī)器人研究進(jìn)展[J]. 科技導(dǎo)報(bào), 2015, 33(23): 66-71.Pang Shuo, Jiu Hai-feng. Current Statue of Autonomous Underwater Vehicles Research and Development[J]. Science & Technology Review, 2015, 33(23): 66-71.

[3] 張鐵棟, 萬(wàn)磊, 王博, 等. 基于改進(jìn)粒子濾波算法的水下目標(biāo)跟蹤[J]. 上海交通大學(xué)學(xué)報(bào), 2012, 46(6): 943- 948.Zhang Tie-dong, Wan Lei, Wang Bo, et al. Underwater Object Tracking Based on Improved Particle Filter[J]. Jo- urnal of Shanghai Jiaotong University, 2012, 46(6): 943- 948.

[4] Xu J, Xu M, Zhou X. The Bearing Only Target Tracking of UUV Based on Cubature Kalman Filter with Noise Estimator[C]//2017 36th Chinese Control Conference(CCC). Dalian, China: IEEE, 2017: 5288-5293.

[5] 王艷艷, 劉開周, 封錫盛. 基于強(qiáng)跟蹤平方根容積卡爾曼濾波的純方位目標(biāo)運(yùn)動(dòng)分析方法[J]. 計(jì)算機(jī)測(cè)量與控制, 2016, 24(11): 136-140.Wang Yan-yan, Liu Kai-zhou, Feng Xi-sheng. Bearings Only Target Motion Analysis Based on Strong Tracking Square-Root Cubature Kalman Filter[J]. Computer Measurement & Control, 2016, 24(11): 136-140.

[6] Kalman R E. A New Approach to Linear Filtering and Prediction Problems[J]. Journal of Basic Engineering, 1960, 82(1): 35-45.

[7] 邊信黔, 周佳加, 嚴(yán)浙平, 等. 基于EKF的無(wú)人潛航器航位推算算法[J]. 華中科技大學(xué)學(xué)報(bào): 自然科學(xué)版, 2011, 39(3): 100-104.Bian Xin-chan, Zhou Jia-jia, Yan Zhe-ping, et al. Dead Reckoning Method for Autonomous Underwater Vehicles Based on EKF[J]. Journal of Huazhong University of Scien- ce and Technology(Natural Science Edition), 2011, 39(3): 100-104.

[8] Leong P H, Arulampalam S, Lamahewa T A, et al. A Gaussian-sum Based Cubature Kalman Filter for Beari- ngs-only Tracking[J]. IEEE Transactions on Aerospace and Electronic Systems, 2013, 49(2): 1161-1176.

[9] Li P, Zhang T, Bo M. Unscented Kalman Filter for Visual Curve Tracking[J]. Image & Vision Computing, 2004, 22(2): 157-164.

[10] Arasaratnam I, Haykin S. Cubature Kalman Filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[11] 鄧先瑞, 聶雪媛, 劉國(guó)平. WSNs下移動(dòng)機(jī)器人HuberM- CKF離散濾波定位[J]. 計(jì)算機(jī)應(yīng)用研究, 2016, 33(6): 1839-1842.Deng Xian-rui, Nie Xue-yuan, Liu Guo-ping. Discrete HuberM-CKF Filter Based Mobile Robot Positioning in WSNs[J]. Application Research of Computers, 2016, 33 (6): 1839-1842.

[12] Donoho D L, Montanari A. Variance Breakdown of Huber (M)-estimators:/→∈(1,∞)[J]. Annals of Statistics, 2015. arXiv:1503.02106vl.

[13] Li W, Swetits J J. The Linear L1 Estimator and the Huber m-estimator[J]. SIAM Journal on Optimization, 1998, 8 (2): 457-475.

[14] 劉星成, 袁東升, 梁平元, 等. 基于代價(jià)函數(shù)的WSN能R效路由協(xié)議性能分析[J]. 通信學(xué)報(bào), 2011, 32(6): 132- 140.Liu Xing-cheng, Yuan Dong-sheng, Liang Ping-yuan, et al. Performance Analysis of Cost Function Based energy- efficient Routing Protocol in WSN[J]. Journal on Communications, 2011, 32(6): 132-140.

[15] 李曉蘭. 我國(guó)引進(jìn)深海水下機(jī)器人“海獅”號(hào)[J]. 海洋石油, 2009, 29(4): 103.

A Target Tracking Algorithm for UUV Based on Huber M-CKF

WANG Bin1, WEN Quan2, FAN Shi-dong1

(1. School of Energy and Power Engineering, Wuhan University of Technology, Wuhan 430063, China; 2. Changjiang Sea-route Planning Design Research Institute, Wuhan 430010, China)

To improve the target tracking accuracy for unmanned undersea vehicle(UUV), a robust M maximum likelihood estimation cost function is introduced into Huber-cubature Kalman filter(H-CKF) for UUV’s target tracking, and the CKF matrix is linearized by changing the normalized innovation covariance. UUV motion model and observation model are established to compare the Huber M-cubature Kalman filter(HM-CKF) with the converted measurement Kalman filter, the cubature Kalman filter and the extended Kalman filter(EKF) under different non-Gaussian noise interferences, and the results show higher filtering precision and stability of the HM-CKF than the traditonal algorithm in complicated undersea acoustic environment.

unmanned undersea vehicle(UUV); Kalman filter; M maximum likelihood estimation cost function

相關(guān)文章導(dǎo)航

1. 王維偉, 孫慧嵩, 顏宏璐. 一種移動(dòng)FH-FSK通信多普勒估計(jì)方法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(6).

2. 易鎮(zhèn)輝, 俞建成, 毛華斌, 等. 一種水下滑翔機(jī)鹽度數(shù)據(jù)的噪聲處理方法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(5).

3. 桑宏強(qiáng), 于佩元, 孫秀軍. 基于航向補(bǔ)償?shù)乃禄铏C(jī)路徑跟蹤控制方法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(5).

4. 阮衛(wèi), 張秦南, 迪玉茹. 一種慣性測(cè)量組合陀螺常值漂移外場(chǎng)標(biāo)定方法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(4).

5. 熊露, 諶劍, 畢曉文, 等. 基于迭代觀測(cè)更新濾波的單站無(wú)源定位算法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(4).

6. 黃玉龍, 張勇剛, 趙玉新. 自主水下航行器導(dǎo)航方法綜述[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(3).

7. 張思宇, 何心怡, 張馳, 等. 水下多目標(biāo)跟蹤技術(shù)現(xiàn)狀與展望[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2018, 26(6).

8. 孫寶全, 顏冰, 姜潤(rùn)翔, 等. 船舶靜電場(chǎng)在船舶跟蹤定位中的應(yīng)用[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2018, 26(1).

9. 趙俊波, 葛錫云, 馮雪磊, 等. 水下SINS/DVL組合導(dǎo)航技術(shù)綜述[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2018, 26(1).

10. 趙振軼, 李亞安, 陳曉, 等. 基于雙觀測(cè)站的水下機(jī)動(dòng)目標(biāo)被動(dòng)跟蹤[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2018, 26(1).

U674.76; TN953

A

2096-3920(2020)01-0039-07

10.11993/j.issn.2096-3920.2020.01.006

王斌, 溫泉, 范世東. 基于Huber M-CKF的UUV目標(biāo)跟蹤算法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2020, 28(1): 39-45.

2019-03-19;

2019-07-24.

中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資助項(xiàng)目(195205013).

王 斌(1995-), 男, 在讀碩士, 主要研究方向?yàn)槎嘣葱畔⑷诤霞夹g(shù).

(責(zé)任編輯: 楊力軍)