基于擴(kuò)展Kalman濾波的單領(lǐng)航者自主水下航行器協(xié)同導(dǎo)航判別式訓(xùn)練方法研究

楊少凡 余華兵 陳新華 孫長(zhǎng)瑜

?

基于擴(kuò)展Kalman濾波的單領(lǐng)航者自主水下航行器協(xié)同導(dǎo)航判別式訓(xùn)練方法研究

楊少凡①②余華兵*①陳新華①孫長(zhǎng)瑜①

①(中國(guó)科學(xué)院聲學(xué)研究所 北京 100190)②(中國(guó)科學(xué)院大學(xué) 北京 100049)

單領(lǐng)航者自主水下航行器(AUV)協(xié)同導(dǎo)航算法中，系統(tǒng)模型是非線性的，擴(kuò)展Kalman濾波(EKF)是針對(duì)非線性系統(tǒng)的很有影響力的濾波算法，但是，EKF算法的性能嚴(yán)格依賴于一系列模型參數(shù)，而這些參數(shù)往往需要花費(fèi)很大的代價(jià)來(lái)捕獲，并且常需要人工調(diào)整。該文應(yīng)用一種能自動(dòng)學(xué)習(xí)Kalman濾波噪聲協(xié)方差參數(shù)的方法，通過仿真分析，證明了該學(xué)習(xí)算法可以完全自主并且高效、準(zhǔn)確地輸出Kalman濾波噪聲參數(shù)，進(jìn)一步提高了單領(lǐng)航者AUV協(xié)同導(dǎo)航系統(tǒng)的導(dǎo)航精度。

自主水下航行器；協(xié)同導(dǎo)航；擴(kuò)展Kalman濾波；自動(dòng)學(xué)習(xí)噪聲參數(shù)

1 引言

隨著人工智能理論、傳感器和微計(jì)算機(jī)等技術(shù)的不斷成熟和發(fā)展，具有感知思維和動(dòng)作能力的自主水下航行器(AUV)在軍事和民用領(lǐng)域得到了廣泛的應(yīng)用，如海洋勘測(cè)、水下情報(bào)搜集、遠(yuǎn)程水下攻擊等。就當(dāng)前的技術(shù)而言，水下導(dǎo)航問題仍然是AUV所面臨的主要挑戰(zhàn)之一。目前在水下環(huán)境中，AUV主要導(dǎo)航方法有航位推算與慣性導(dǎo)航方法、水聲導(dǎo)航方法、地球物理導(dǎo)航方法、仿生學(xué)導(dǎo)航方法和組合導(dǎo)航方法等[1,2]。單一導(dǎo)航系統(tǒng)往往很難滿足現(xiàn)代水下的導(dǎo)航要求，組合導(dǎo)航針對(duì)每種單一導(dǎo)航系統(tǒng)都有各自的獨(dú)特性能和局限性，把幾種單一系統(tǒng)組合起來(lái)，利用多種信息源，構(gòu)成一種有多余度和導(dǎo)航精確度更高的多功能系統(tǒng)[3]。新的數(shù)據(jù)處理方法，特別是Kalman濾波(KF)[4]方法的應(yīng)用是實(shí)現(xiàn)組合導(dǎo)航的關(guān)鍵。

本文研究有關(guān)EKF的一個(gè)基本問題：優(yōu)化系統(tǒng)模型參數(shù)來(lái)提高從AUV狀態(tài)估計(jì)精度[5]。實(shí)際應(yīng)用中即使?fàn)顟B(tài)預(yù)測(cè)方程和觀測(cè)方程可以精確建模，準(zhǔn)確估計(jì)噪聲協(xié)方差往往非常地困難。噪聲往往由若干不同的因素產(chǎn)生：(1)附加噪聲的測(cè)量數(shù)據(jù)；(2)未被EKF建模的隱含環(huán)境狀態(tài)信息；(3)時(shí)間離散化引入的附加噪聲；(4)EKF算法本身的近似，如常用Taylor公式進(jìn)行線性近似。所有這些因素都會(huì)在狀態(tài)估計(jì)和觀測(cè)數(shù)據(jù)時(shí)引入干擾，在EKF中，它們通常被看作噪聲。

本文研究了3個(gè)學(xué)習(xí)算法，均應(yīng)用從AUV的GPS定位信息來(lái)學(xué)習(xí)EKF的協(xié)方差參數(shù)?；舅枷牒苤庇^：訓(xùn)練EKF來(lái)最大化它的預(yù)測(cè)精度，預(yù)測(cè)精度通過訓(xùn)練中獲取的參考數(shù)據(jù)來(lái)評(píng)估。

2 基于擴(kuò)展Kalman濾波的單領(lǐng)航者AUV協(xié)同導(dǎo)航算法

圖1 AUV的仿真真實(shí)路徑

擴(kuò)展Kalman濾波過程可以表示為

預(yù)測(cè)過程：

更新過程：

3 自動(dòng)學(xué)習(xí)EKF濾波參數(shù)

本節(jié)討論自動(dòng)捕獲Kalman濾波的噪聲參數(shù)的學(xué)習(xí)算法。雖然接下來(lái)要討論的算法也可以應(yīng)用到更廣泛的方面，但為了研究簡(jiǎn)單，我們只專注于學(xué)習(xí)噪聲參數(shù)和。除已有研究成果中所需要的測(cè)量數(shù)據(jù)外，此算法還需要一個(gè)能夠精確測(cè)量從AUV狀態(tài)的全部變量或部分變量的測(cè)量設(shè)備。換言之，在EKF學(xué)習(xí)階段，我們可以獲得額外的變量，每個(gè)變量由映射得到

3.1最大化聯(lián)合似然算法

可以觀察到，該算法實(shí)際上并沒有真正地訓(xùn)練EKF，而是簡(jiǎn)單地訓(xùn)練濾波器元素。因此，這暗示了我們假想獨(dú)立訓(xùn)練濾波器元素和整體訓(xùn)練EKF濾波器的效果相同。

3.2最小化殘余預(yù)測(cè)誤差算法

最大化聯(lián)合似然函數(shù)算法僅當(dāng)可以測(cè)量完整的狀態(tài)向量時(shí)才可以應(yīng)用。但是，往往將完整狀態(tài)向量映射到低維的狀態(tài)向量，比如單領(lǐng)航者協(xié)同導(dǎo)航系統(tǒng)中，完整的狀態(tài)向量還包含航向角，這項(xiàng)不能直接由GPS數(shù)據(jù)獲得。此外，最大化條件似然函數(shù)的算法實(shí)際上并沒有真正運(yùn)行EKF濾波器，特別地，若噪聲項(xiàng)是相關(guān)的，這將影響算法性能。

求解這個(gè)優(yōu)化問題比最大化聯(lián)合似然要困難得多，因?yàn)檎`差函數(shù)不是簡(jiǎn)單的和的函數(shù)，而是通過狀態(tài)估計(jì)均值這個(gè)中間項(xiàng)來(lái)調(diào)節(jié)，并且是和的復(fù)雜函數(shù)。因此，這個(gè)學(xué)習(xí)算法真正評(píng)估了EKF的性能，而不是它的獨(dú)立元素。

3.3 最大化預(yù)測(cè)似然算法

最大化lg似然函數(shù)變?yōu)?/p>

本文算法與最小化殘余預(yù)測(cè)誤差算法的不同之處是需要額外提供給EKF協(xié)方差矩陣參數(shù)，以此來(lái)反映狀態(tài)估計(jì)量的穩(wěn)定性。

4 數(shù)值仿真分析

為驗(yàn)證自動(dòng)學(xué)習(xí)EKF濾波參數(shù)3個(gè)算法的有效性，進(jìn)行了仿真研究。如圖2所示，領(lǐng)航者AUV由坐標(biāo)原點(diǎn)出發(fā)沿直線航行，跟隨者AUV的航程分為兩段，首先沿斜線航行一段距離，然后與領(lǐng)航者保持平行航行。在兩段航程中跟隨者AUV的航向角分別為和。主、從AUV的航速均為2.5 m/s，系統(tǒng)的狀態(tài)更新時(shí)間。仿真中假設(shè)所有誤差均為零均值高斯噪聲。從AUV的速度信息通過測(cè)量自身推進(jìn)器的轉(zhuǎn)速獲得，誤差為。主、從AUV的航向角信息由陀螺儀測(cè)量，,。AUV間的相對(duì)距離通過水聲通信測(cè)量，相對(duì)方位通過聲波相位差量測(cè)，設(shè)聲速為定值，取和。GPS量測(cè)噪聲分別為和。

下面將分別研究初始系統(tǒng)可以獲得精確的噪聲參數(shù)和獲得的噪聲參數(shù)存在較大誤差時(shí)3個(gè)學(xué)習(xí)算法對(duì)EKF協(xié)同導(dǎo)航性能的影響。仿真中選擇400個(gè)測(cè)試點(diǎn)進(jìn)行導(dǎo)航性能分析，并采用RMS誤差進(jìn)行度量。

表1初始獲得精確的噪聲協(xié)方差參數(shù)時(shí)各算法定位誤差

協(xié)同導(dǎo)航算法RMS誤差(m) EKF13.2 最大聯(lián)合似然15.8 最小殘余預(yù)測(cè)誤差12.5 最大預(yù)測(cè)誤差12.6

4.1初始時(shí)刻系統(tǒng)可獲得精確的噪聲協(xié)方差參數(shù)時(shí)算法性能分析

仿真結(jié)果表明，當(dāng)初始給定協(xié)方差參數(shù)準(zhǔn)確時(shí)，EKF濾波算法的RMS誤差大約為13.2 m；最大聯(lián)合似然算法的RMS誤差約為15.8 m，誤差比EKF算法大，這是由于該算法并沒有真正訓(xùn)練EKF濾波器，而是訓(xùn)練獨(dú)立的濾波器元素，并假設(shè)這兩者等價(jià)，而實(shí)際的協(xié)同導(dǎo)航系統(tǒng)不完全服從該假設(shè)(比如存在相關(guān)噪聲)，因此帶來(lái)較大誤差；最小殘余預(yù)測(cè)誤差算法與最大預(yù)測(cè)誤差算法的RMS誤差要小于EKF算法，這是由于通過訓(xùn)練EKF濾波器獲取了更優(yōu)的噪聲協(xié)方差參數(shù)，即提取了EKF內(nèi)部的關(guān)聯(lián)信息，從而提高了導(dǎo)航精度。

圖2 AUV的仿真運(yùn)動(dòng)路徑?????圖3 從AUV的協(xié)同導(dǎo)航軌跡?????圖4 EKF算法定位誤差

圖5 最大化聯(lián)合似然算法定位誤差???圖6 最小殘余預(yù)測(cè)誤差算法定位誤差???圖7 最大化預(yù)測(cè)似然算法定位誤差

4.2 初始時(shí)刻系統(tǒng)獲得的噪聲協(xié)方差參數(shù)與實(shí)際參數(shù)存在較大偏差時(shí)算法性能分析

實(shí)際情況下，初始噪聲協(xié)方差參數(shù)很難獲得準(zhǔn)確值，往往存在較大誤差。假設(shè)實(shí)際的噪聲協(xié)方差參數(shù)分別為

仿真結(jié)果表明，當(dāng)初始時(shí)刻不能獲取準(zhǔn)確的噪聲協(xié)方差參數(shù)時(shí)，EKF濾波算法的RMS誤差大約為20.0 m。3個(gè)學(xué)習(xí)算法的RMS誤差分別為15.6 m, 13.0 m與13.2 m，可見學(xué)習(xí)算法大大提高了單領(lǐng)航者AUV協(xié)同導(dǎo)航系統(tǒng)的性能。

圖8 從AUV的協(xié)同導(dǎo)航軌跡??????圖9 EKF算法定位誤差??????圖10 最大化聯(lián)合似然算法定位誤差

圖11 最小殘余預(yù)測(cè)誤差算法定位誤差????????圖12 最大化預(yù)測(cè)似然算法定位誤差

表2初始誤差協(xié)方差參數(shù)設(shè)為大誤差值時(shí)算法定位誤差

協(xié)同導(dǎo)航算法RMS誤差(m) EKF20.0 最大聯(lián)合似然15.6 最小殘余預(yù)測(cè)誤差13.0 最大預(yù)測(cè)誤差13.2

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

本文應(yīng)用一系列算法來(lái)訓(xùn)練EKF的噪聲參數(shù)。所有的算法有相同的思想：通過調(diào)整EKF的噪聲協(xié)方差參數(shù)來(lái)最大化系統(tǒng)狀態(tài)預(yù)測(cè)精度。仿真結(jié)果表明，訓(xùn)練算法與常規(guī)EKF算法相比有兩個(gè)優(yōu)點(diǎn)。第一，它完全自動(dòng)搜索噪聲協(xié)方差參數(shù)，解決了人工調(diào)節(jié)費(fèi)時(shí)費(fèi)力的缺點(diǎn)。第二，通過訓(xùn)練學(xué)習(xí)獲得的噪聲協(xié)方差參數(shù)比初始的噪聲參數(shù)更優(yōu)，從而提高了協(xié)同導(dǎo)航系統(tǒng)精度。

[1] Paull L, Saeedi S, Seto M,. AUV navigation and localization: A review[J].,2014, 39(4): 131-149.

[2] Liu Ming-yong, Li Hong, and Liu Kun. Geomagnetic navigation of AUV without a priori magnetic map[C]. MTS/ IEEE OCEANS, Taipei, 2014: 1-5.

[3] 劉明雍. 水下航行器協(xié)同導(dǎo)航技術(shù)[M]. 北京: 國(guó)防工業(yè)出版社, 2014: 1-117.

Liu Ming-yong. Cooperative Navigation Technology for Underwater Vehicles[M]. Beijing: National Defence Industry Press, 2014: 1-117.

[4] Kalman R. A new approach to linear filtering and prediction problems[J].,1960, 82(Series D): 35-45.

[5] Abbeel P, Coates A, Montemerlo M,. Discriminative training of Kalman filters[C]. Robotic: Science and Systems, Cambridge, 2005: 289-296.

[6] Zhang Li-chuan, Xu De-min, Liu Ming-yong,. Cooperative navigation and localization for multiple UUVs [J]., 2009, 8(3): 216-221.

[7] Fallon M, Papadopoulos G, Lenorad J,. Cooperative AUV navigation using a single maneuvering surface craft[J]., 2011, 29(12): 1461-1474.

[8] 李聞白, 劉明雍, 李虎雄，等. 基于單領(lǐng)航者相對(duì)位置測(cè)量的多AUV協(xié)同導(dǎo)航系統(tǒng)定位性能分析[J]. 自動(dòng)化學(xué)報(bào), 2011, 37(6): 724-736.

Li Wen-bai, Liu Ming-yong, Li Hu-xiong,.. Localization performance analysis of cooperative navigation system for multiple AUVs based on relative position measurements with a single leader[J]., 2011, 37(6): 724-736.

[9] Liu Ming-yong, Li Wen-bai, Mu Bing-xian,. Cooperative navigation for multiple AUVs based on relative range measurements with a single leader[C]. IEEE International Conference on Intelligent Computing and Intelligent Systems, Xiamen, 2010: 762-766.

[10] Liu Jian, Xu De-min, Zhang Fu-bin,. Research on cooperative navigation for multiple UUVs[C]. International Symposium on Instrumentation & Measurement, Sensor Network and Automation, Sanya, 2012: 248-251.

[11] Maki T, Matsuda T, Sakamaki T,. Navigation method for underwater vehicles based on mutual acoustical positioning with a single seafloor station[J].,2013, 38(1): 167-176.

[12] Teo K, Goh B, and Chai O. Fuzzy docking guidance using augmented navigation system on an AUV[J].,2014, 40(2): 1-13.

[13] Roumeliotis S and Bekey G. Distributed multirobot localization[J]., 2002, 18(5): 781-795.

[14] Roweis S and Ghahramani Z. A unifying review of linear Gaussian models[J].,1999, 11(2): 305-345.

Discriminative Training of Kalman Filters Based Cooperative Navigation for Multiple Autonomous Underwater Vehicles with a Single Leader

Yang Shao-fan①②Yu Hua-bing①Chen Xin-hua①Sun Chang-yu①

①(,,100190,)②(,100049,)

In the cooperative navigation algorithm for multiple Autonomous Underwater Vehicles (AUVs)with a single leader, the model of the systemis nonlinear. The Extended Kalman Filter (EKF), which is directed against the nonlinear system, is one of the most influential techniques. However, the performance of EKF critically depends on a large number of modeling parameters which can be very difficult to obtain, and are often set by manual tweaking and at a great cost. In this paper, a method for automatically learning the noise covariance of a Kalman filter is applied, and the simulation result shows that this algorithm fully automatically and quickly outputs the noise covariance, which improves the navigation accuracy of the cooperative navigation system.

Autonomous Underwater Vehicle (AUV); Cooperative navigation; Extended Kalman Filter (EKF); Automatically learning the noise parameters

TB568

A

1009-5896(2015)11-2756-06

10.11999/JEIT150036

2015-01-08；改回日期：2015-06-02；

2015-07-06

余華兵 yuhuabing@mail.ioa.ac.cn

國(guó)家自然科學(xué)基金(61372180)

The National Natural Science Foundation of China (61372180)

楊少凡： 男，1991年生，博士生，研究方向?yàn)槎郃UV協(xié)同導(dǎo)航、定位、水聲陣列信號(hào)處理.

余華兵： 男，1975年生，研究員，研究方向?yàn)樗曣嚵行盘?hào)處理.

陳新華： 男，1978年生，副研究員，研究方向?yàn)樗曅盘?hào)處理.

孫長(zhǎng)瑜： 男，1954年生，研究員，研究方向?yàn)樗曣嚵行盘?hào)處理.