基于迭代無跡卡爾曼濾波的水下組合導(dǎo)航

付鳳婷， 褚振忠， 朱大奇

（上海海事大學(xué)物流工程學(xué)院，上海智能海事搜救與水下機(jī)器人工程技術(shù)研究中心，上海 201306）

高精度導(dǎo)航系統(tǒng)是水下機(jī)器人的關(guān)鍵技術(shù)之一[1]。目前，水下機(jī)器人常用的導(dǎo)航定位傳感器包括超短基線定位系統(tǒng)（USBL）、多普勒測(cè)速儀（DVL）、電子羅盤（EC）、深度計(jì)、慣性導(dǎo)航系統(tǒng)等[2-5]。為了提高水下機(jī)器人導(dǎo)航定位的精度，需要對(duì)多傳感器的輸出數(shù)據(jù)進(jìn)行融合處理以實(shí)現(xiàn)多傳感器的組合導(dǎo)航。

卡爾曼濾波是一種常用的數(shù)據(jù)融合算法，該算法的濾波和融合精度直接影響著運(yùn)動(dòng)系統(tǒng)定位信息的精度和實(shí)時(shí)性[6]。宋振華等[7]提出了一種以捷聯(lián)慣導(dǎo)、電子羅盤和深度計(jì)為觀測(cè)量，通過聯(lián)邦卡爾曼濾波器組合實(shí)現(xiàn)自主導(dǎo)航的方法。擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）的基本思想是利用泰勒展開直接線性化非線性方程組，其優(yōu)點(diǎn)是不需要額外的計(jì)算，計(jì)算結(jié)果與基本卡爾曼濾波的計(jì)算結(jié)果相似。Hong 等[8]提出了基于EKF 的MEMS/GPS組合導(dǎo)航系統(tǒng)。穆華等[9]提出了地磁/慣性組合導(dǎo)航的方法，通過點(diǎn)群濾波和擴(kuò)展卡爾曼濾波相結(jié)合的兩級(jí)濾波，實(shí)現(xiàn)了大觀測(cè)噪聲地磁測(cè)量的有效融合。然而，水下機(jī)器人運(yùn)動(dòng)具有較強(qiáng)的非線性，而擴(kuò)展卡爾曼濾波算法的基本思想是利用泰勒展開，將非線性方程直接線性化，線性化后的模型與實(shí)際系統(tǒng)存在很大差別。無跡卡爾曼濾波（Unscented Kalman Filter，UKF）利用無跡變換（Unscented Transform，UT）處理單步預(yù)測(cè)方程的均值和協(xié)方差的非線性傳遞，其非線性分布統(tǒng)計(jì)量的計(jì)算精度高于EKF[10-11]。Liu等[12]提出了一種基于UKF 和支持向量機(jī)的混合導(dǎo)航方法。史興波等[13]提出了基于UKF 算法的水下機(jī)器人組合導(dǎo)航系統(tǒng)，但是該導(dǎo)航系統(tǒng)的精度依賴于GPS 信號(hào)。由于GPS 在水下無法工作，僅僅使用多普勒測(cè)速儀、電子羅盤和陀螺儀會(huì)存在較大的累積誤差。

本文研究水下機(jī)器人的水下組合導(dǎo)航定位問題，提出了一種超短基線定位系統(tǒng)、多普勒測(cè)速儀與電子羅盤相結(jié)合的組合導(dǎo)航系統(tǒng)，通過迭代無跡卡爾曼濾波（Iterative Unscented Kalman Filter, IUKF）算法，實(shí)現(xiàn)水下機(jī)器人的動(dòng)態(tài)定位。通過水下機(jī)器人的水池實(shí)驗(yàn)與仿真分析驗(yàn)證了算法的有效性。

1 組合導(dǎo)航系統(tǒng)

本文搭建的水下組合導(dǎo)航系統(tǒng)采用了3 種類型的傳感器：超短基線定位系統(tǒng)、多普勒測(cè)速儀和電子羅盤。其中，超短基線定位系統(tǒng)用于測(cè)量水下機(jī)器人相對(duì)于母船的位置；多普勒測(cè)速儀從3 個(gè)方向捕捉水下機(jī)器人載體坐標(biāo)系下的速度；電子羅盤用于獲取水下機(jī)器人的艏向角、縱傾角和橫搖角?；赨KF 的水下組合導(dǎo)航系統(tǒng)流程圖如圖1 所示。具體實(shí)現(xiàn)過程如下：

（1）對(duì)于由多普勒測(cè)速儀測(cè)得的水下機(jī)器人3 個(gè)方向的速度信息，利用UKF 進(jìn)行濾波處理。

（2）將處理后的速度信息與電子羅盤獲得的角度信息相結(jié)合，將機(jī)器人的運(yùn)動(dòng)狀態(tài)從載體坐標(biāo)系轉(zhuǎn)換到大地坐標(biāo)系，計(jì)算出機(jī)器人在大地坐標(biāo)系下的位移信息。

（3）對(duì)于超短基線定位系統(tǒng)實(shí)時(shí)獲取的相對(duì)母船的位置坐標(biāo)信息，通過UKF 進(jìn)行濾波處理。

（4）將處理后的機(jī)器人位移信息和位置坐標(biāo)信息作為輸入，使用UKF 進(jìn)行數(shù)據(jù)融合，輸出定位信息。

（5）為提高定位精度，對(duì)輸出定位信息進(jìn)行3 次迭代，以輸出更高精度的定位信息。

2 UKF 數(shù)據(jù)融合

UKF 算法在處理狀態(tài)方程時(shí)，首先進(jìn)行UT 變換，然后使用變換后的狀態(tài)變量進(jìn)行濾波估計(jì)，以減少估計(jì)誤差。

UKF 算法通過確定性采樣得到一組Sigma 點(diǎn)，從而獲得更多的觀測(cè)假設(shè)，對(duì)系統(tǒng)狀態(tài)的均值和協(xié)方差的估計(jì)更為準(zhǔn)確，同時(shí)由于該算法采用了非線性的狀態(tài)方程或觀測(cè)方程，避免了線性化誤差[14]。首先使用UKF 算法對(duì)多普勒傳感器獲取的速度信息和超短基線獲取的位置坐標(biāo)信息進(jìn)行濾波，以提高初始數(shù)據(jù)的精度。然后在對(duì)多普勒測(cè)速儀、電子羅盤和超短基線定位系統(tǒng)獲取的數(shù)據(jù)信息進(jìn)行融合時(shí)，對(duì)圖1 中的位移估計(jì)和通過UKF 的位置坐標(biāo)分別進(jìn)行確定性采樣。以圖1 位移估計(jì)中得到的大地坐標(biāo)系下的X 方向位移信息和超短基線定位系統(tǒng)得到的X 方向位置信息為例，UKF 數(shù)據(jù)融合算法的具體步驟如下：

圖1基于UKF 的水下組合導(dǎo)航系統(tǒng)流程圖Fig.1Flow chart of underwater navigation system based on UKF

②計(jì)算輸出的一步提前預(yù)測(cè)，即觀測(cè)預(yù)測(cè)均值：

③濾波更新以及數(shù)據(jù)融合， zk為位移估計(jì)中大地坐標(biāo)系下的X 方向位移信息， wk為超短基線定位系統(tǒng)得到的X 方向位置信息。即

3 IUKF 算法

IUKF 是在原有的UKF 基礎(chǔ)上對(duì)量測(cè)更新的狀態(tài)估計(jì)值采用多步迭代，從而減少由于泰勒展開帶來的截?cái)嗾`差。本文應(yīng)用IUKF 算法減少誤差，提高水下機(jī)器人在水下定位的精度。將已經(jīng)通過UKF 輸出的位置信息作為新的輸入信號(hào)再次進(jìn)行濾波，得到一個(gè)新的輸出信號(hào)。

在任意時(shí)刻計(jì)算迭代估計(jì)值的步驟如下：

（3）迭代過程中，當(dāng)兩步狀態(tài)差值滿足式(6)：

迭代終止，否則返回步驟（2）。其中 ε 是預(yù)先給定的迭代閾值，i 代表迭代次數(shù)[15]。

4 數(shù)據(jù)分析

為驗(yàn)證水下組合導(dǎo)航算法的有效性，對(duì)水下機(jī)器人在水池環(huán)境下的數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)分析，并將分析結(jié)果與機(jī)器人在湖泊和池塘環(huán)境下的數(shù)據(jù)分析結(jié)果進(jìn)行對(duì)比，以確定最佳迭代次數(shù)。如圖2 所示，水下機(jī)器人自帶電子羅盤，并在底部加裝了多普勒測(cè)速儀。

4.1 IUKF 與UKF 對(duì)比

引入數(shù)據(jù)迭代不僅提高了UKF 的收斂速度，而且收斂過程穩(wěn)定，不會(huì)影響UKF 的收斂性。為驗(yàn)證IUKF 的有效性，基于多普勒測(cè)速儀測(cè)得的載體坐標(biāo)系下的X 方向速度數(shù)據(jù)，分別利用UKF 和IUKF 進(jìn)行處理，比較濾波效果和誤差。

圖2機(jī)器人水池測(cè)試Fig.2Robot is testing in the pool

圖3UKF 濾波結(jié)果（a）和誤差（b）Fig.3Results (a) and error (b) of UKF filtering

對(duì)UKF 濾波后的數(shù)據(jù)進(jìn)行第1 次迭代，并將圖3(a)中濾波處理后的結(jié)果作為第1 次IUKF 濾波的輸入。圖4、圖5 分別示出了水池環(huán)境下不同迭代次數(shù)的多普勒測(cè)速儀測(cè)得的載體坐標(biāo)系下X 方向速度數(shù)據(jù)及經(jīng)UKF 濾波后的效果對(duì)比。

為了更直觀地觀察迭代次數(shù)對(duì)濾波效果的影響，計(jì)算每組數(shù)據(jù)經(jīng)過UKF 濾波后的誤差絕對(duì)值均值。不同迭代次數(shù)下誤差絕對(duì)值均值如表1 所示。

由表1 以及圖4、圖5 可知，對(duì)由傳感器獲取的信息進(jìn)行3 次迭代后，誤差絕對(duì)值均值基本保持不變，可以達(dá)到預(yù)期效果。

4.2 不同場(chǎng)景對(duì)迭代次數(shù)的影響

圖4水池環(huán)境下 UKF 濾波效果圖Fig.4Effect diagrams of UKF filtering under the swimming pool environment

圖5水池環(huán)境下誤差比對(duì)Fig.5Error comparison under the swimming pool environment

表1不同迭代次數(shù)下的誤差絕對(duì)值均值Table1Mean value of absolute error under different iterations

為驗(yàn)證3 次迭代在其他環(huán)境應(yīng)用時(shí)的有效性，在上海海事大學(xué)校內(nèi)的池塘和湖泊進(jìn)行實(shí)驗(yàn)并采集了運(yùn)動(dòng)數(shù)據(jù)。圖6、圖7 分別示出了池塘環(huán)境不同迭代次數(shù)下的多普勒測(cè)速儀測(cè)得的載體坐標(biāo)系下X 方向速度數(shù)據(jù)以及經(jīng)UKF 濾波后的效果對(duì)比。圖8、圖9 分別示出了湖泊環(huán)境不同迭代次數(shù)下的多普勒測(cè)速儀測(cè)得的載體坐標(biāo)系下X 方向速度數(shù)據(jù)以及經(jīng)UKF 濾波后的效果對(duì)比。不同環(huán)境、不同迭代次數(shù)下的誤差絕對(duì)值均值如表2 所示。

由圖6～圖9 以及表2 可以看出，在水池和湖泊環(huán)境下采集的數(shù)據(jù)經(jīng)過3 次迭代可以達(dá)到預(yù)期效果，而在池塘環(huán)境下僅需要兩次就可以達(dá)到預(yù)期效果。迭代次數(shù)的不同在一定程度上受環(huán)境影響，但影響不是很大，因此在水池環(huán)境下選擇對(duì)輸出的定位信息進(jìn)行的迭代次數(shù)為3 次。

4.3 IUKF 數(shù)據(jù)融合

圖6池塘環(huán)境下 UKF 濾波效果圖Fig.6Effect diagrams of UKF filtering under the pond environment

圖7池塘環(huán)境下誤差比對(duì)Fig.7Error comparison under the pond environment

圖8湖泊環(huán)境下UKF 濾波效果圖Fig.8Effect diagrams of UKF filtering under the lake environment

圖9湖泊環(huán)境下誤差比對(duì)Fig.9Error comparison under the lake environment

表2不同環(huán)境不同迭代次數(shù)下的誤差絕對(duì)值均值Table2Mean value of absolute error under different iteration times in different environments

圖10 示出了使用超短基線定位模擬數(shù)據(jù)作出的機(jī)器人軌跡與機(jī)器人運(yùn)動(dòng)的原始軌跡，超短基線定位模擬數(shù)據(jù)是在實(shí)際定位信息數(shù)據(jù)的基礎(chǔ)上添加噪聲產(chǎn)生的。圖11 示出了根據(jù)航位推算數(shù)據(jù)作出的機(jī)器人軌跡與機(jī)器人運(yùn)動(dòng)的原始軌跡，兩者軌跡出現(xiàn)較大偏差是因?yàn)楹轿煌扑愦嬖诶鄯e誤差所致，圖12示出了使用IUKF 算法對(duì)超短基線定位、電子羅盤和多普勒測(cè)速儀的數(shù)據(jù)進(jìn)行處理后的軌跡，可見該軌跡與機(jī)器人運(yùn)動(dòng)的原始軌跡基本一致。比較圖10～圖12可以看出，僅基于電子羅盤與多普勒測(cè)速儀的航位推算方法得到的結(jié)果在后期具有較大的累積誤差，加入超短基線定位系統(tǒng)的定位數(shù)據(jù)后，對(duì)位置信息進(jìn)行了有效的修正，經(jīng)過IUKF 融合以后，取得了較好的融合效果。水池實(shí)驗(yàn)及仿真分析表明，本文提出的水下組合導(dǎo)航算法是可行、有效的。

圖10模擬的超短基線路徑Fig.10Simulate path of USBL

圖11航位推算路徑Fig.11Path of dead reckoning

圖12IUKF 融合路徑Fig.12Path of IUKF

5 結(jié) 論

本文提出了一種基于IUKF 的水下多傳感器組合導(dǎo)航融合算法，在對(duì)多傳感器反饋的水下機(jī)器人運(yùn)動(dòng)信息進(jìn)行UKF 濾波處理后進(jìn)行多次迭代。通過比較每次迭代后產(chǎn)生的均值誤差，確定最合適的迭代次數(shù)為3 次。通過對(duì)超短基線定位數(shù)據(jù)、僅基于電子羅盤與多普勒測(cè)速儀的航位推算數(shù)據(jù)和IUKF數(shù)據(jù)融合定位數(shù)據(jù)進(jìn)行對(duì)比，驗(yàn)證了基于IUKF 的水下組合導(dǎo)航系統(tǒng)的有效性。