一種基于量測偽距的EKF移動長基線AUV協同導航方法

劉明雍, 黃 博, 蔡 挺

?

一種基于量測偽距的EKF移動長基線AUV協同導航方法

劉明雍, 黃 博, 蔡 挺

(西北工業大學航海學院, 陜西西安, 710072)

在移動長基線(MLBL)定位中, 由于水聲信號在水中傳播速度較慢, 以及被定位AUV的自身運動, 導致AUV與不同浮標的距離是在不同時間不同位置測量產生的, 所以應用移動長基線會產生一定的時間延遲定位誤差。本文針對該定位誤差, 提出了一種基于量測偽距的EKF移動長基線時間延遲算法, 該算法利用擴展Kalman濾波, 通過對AUV的位置狀態的前推, 重構了系統的量測方程,實現了量測方程同系統的量測量在時間上的一致性, 有效地解決了時間延遲產生的誤差。理論分析與仿真結果均表明, 該方法可以顯著提高導航定位精度。

自主式水下航行體; 移動長基線; 定位誤差; 擴展Kalman濾波

0 引言

導航定位是自主式水下航行器(autonomous underwater vehicle, AUV)發展的關鍵和制約性問題。它不僅受到AUV自身體積和導航設備的限制, 而且水下環境的特殊性也給AUV導航定位帶來諸多的制約瓶頸, 例如水下未知洋流、水聲通信受限等。目前水下導航定位方法主要有慣性導航和水聲導航。慣性導航短航程時定位精度高, 但長航程會出現累積誤差, 導致定位精度降低; 水聲導航精度較高, 主要有長基線, 短基線和超短基線方法, 但水聲導航方法都需要在載體上安裝多個水聽器, 在水底布放基陣, 技術操作復雜。

為了降低工程實施難度, 同時進一步提高AUV作業方式和區域的靈活度, 文獻[1]首次提出“移動長基線(moving long baseline, MLBL)”的概念, 即通過改進傳統長基線技術, 利用無人水面艦船或浮標充當MLBL定位基陣, 通過全球衛星定位系統GPS或其他方式實時獲得定位基陣的自身位置, 然后通過基陣之間的探測和定位方程解算, 確定航行器的位置坐標。它與傳統長基線系統最大的不同是, 應答器不是固定布設在海底, 而是浮于海面且移動的。文獻[2]~文獻[5]基于MLBL的概念, 利用最小二乘法算法, 進行了協同定位的試驗, 驗證了MLBL的有效性。文獻[6]針對MLBL定位模型, 設計了擴展卡爾曼濾波(extended Kalman felter, EKF)協同導航算法, 并與幾何解方程算法進行了對比, 提高了定位精度。文獻[6]同時指出, 在實際應用中, 由于AUV處于運動狀態, 同時水聲信號在水中傳播速度較慢, 由此會產生時間延遲, 在此期間AUV會移動一段距離, 導致AUV定位產生誤差。誤差大小與海面浮標和被定位AUV之間的距離、被定位AUV的航速有關。距離越遠, 航速越大, 則誤差越大, 是目前MLBL有待解決的問題之一。

本文詳細分析了時間延遲產生的誤差, 并提出了一種基于測量偽距的EKF的MLBL時間延遲算法, 可以有效地解決時間延遲產生的誤差。

1 MLBL定位原理及模型

圖1中, 水面移動浮標布放在海平面上, 至少要3個移動浮標應答器才能得到目標的3D坐標。實際應用中, 一般需要布放4個以上的移動浮標應答器, 以提高定位精度。

系統的定位算法來自于長基線定位算法, 主要有球面相交和雙曲面相交法。與長基線基陣固定在海底不同的是, MLBL基陣漂浮在海面上, 通過攜帶的GPS實時更新自身位置信息。

圖1 移動長基線定位原理

MLBL導航定位系統在進行導航定位解算時, 一般按如下步驟進行。

第1步, 按照預定的時間周期, 被定位AUV發送Ping詢問信號, 海面上的浮標接收到詢問信號后, 通過水聲通信裝置以不同的頻率廣播自身的位置信息。

第2步, 被定位AUV接收應答信號后, 通過所用時間可計算出與移動浮標的距離:(為水聲在水中傳播速度), 從而確定相對位置的幾何關系。

第3步, 根據被定位AUV與應答器的相對位置幾何關系, 以及接收到的應答器的位置信息, 依照一定的定位算法, 更新自身位置信息, 這就是MLBL導航定位的原理。

通常使用球面相交最小二乘定位算法更新自身的位置信息。因為被定位AUV深度可以通過壓力傳感器測得, 所以通過3個移動浮標便可測得AUV位置。但為了提高定位精度, 通常都布放4個移動浮標, 采用最小二乘法解算。設AUV位置為(,,), 則定位方程為

兩兩相減, 消去二次項,,, 同時深度可通過壓力傳感器測得

(2)

寫成矩陣形式

其中

(4)

運用最小二乘法, 可得

可求得AUV位置坐標。

2 時間延遲誤差分析

在應用MLBL的過程中, 被定位AUV處于運動狀態下, 且由于水聲信號在水中傳播速度較慢(1 500 m/s), 因此會產生時間延遲問題。如圖2所示, 被定位AUV發送Ping詢問信號后到收到浮標的應答信號需要一定的時間。同時由于AUV與每個浮標距離不同, AUV收到每個浮標的應答信號的時間也不相同, 加上AUV的自身運動, 所以AUV是在不同位置不同時間收到應答信號的, 所求R也并非是AUV完全收到應答信號時與各浮標的距離。

圖2 時間延遲誤差示意圖

對于AUV和浮標1, AUV從時刻發出Ping到收到應答信號所用時為, 其間AUV航行位移為, 由于時間很短,可由航位推算求得, 代入方程(1)中, 得

上式簡化為

可以看出, 誤差大小與被定位AUV和浮標之間的距離及AUV的航速有關, 距離越遠, 航速越大, 則誤差越大。

3 基于偽距的EKF移動長基線時間延遲算法

3.1 系統方程

AUV運動方程

(11)

代入式(9), 運動方程可簡寫為

其中

(13)

根據式(12), 由EKF算法得一步狀態預測

預測協方差

(15)

(17)

3.2 重構量測方程

由于水聲傳播速度較慢及AUV的自身運動, 若由方程(1)作為量測方程, 據上文知, 測量值是AUV在不同時間不同位置與各浮標的距離, 這將造成量測方程和觀測量的不匹配, 求得的解是AUV發出Ping時刻的位置, 將產生時間延遲造成的誤差, 所以需重構測量方程。AUV在時刻的位置狀態可由時刻位置向前推求得

同理可得, AUV在時刻與其他偽浮標的偽距方程, 以作為量測量, 可得EKF的量測方程

(21)

量測方程的雅可比矩陣為

依據EKF公式得

(23)

(25)

(26)

卡爾曼增益為

狀態更新為

(28)

4 仿真分析

為了驗證算法的有效性, 進行仿真分析研究。在仿真試驗中, 海面上布放4個浮標, 每個相距5 000 m, 組成正方形基陣。AUV直線航行, 速度為5 m/s, 航向為45°, 深度為-50 m。AUV依據式(1)的運動學模型, 每隔0.2 s采用傳統的航位推算更正自身位置狀態, 其中速度傳感器的測量噪聲取為的零均值高斯白噪聲, 航向角的測量噪聲取為的零均值高斯白噪聲, 且速度與航向角的噪聲獨立且不相關。AUV同時每隔10 s向外發送一次Ping即詢問信號, 完全收到各浮標的應答信號后進行定位解算。

本文分別對航位推算, 球面相交最小二乘, 基于測量偽距的EKF算法這3種方法進行對比仿真。由圖3可以看出, 被定位AUV單純運用航位推算算法的位置曲線出現了明顯的偏離, 球面相交最小二乘法和基于偽距的EKF算法位置曲線與理想的比較吻合。

圖3 AUV的協同導航軌跡

由圖4可得, 當AUV只依賴內部傳感器進行推算導航時, 其定位誤差不斷增大; 當利用長基線球面相交最小二乘法后, 導航精度得到顯著提高, 定位誤差在50 m內。

圖4 AUV的定位誤差曲線

此外, 誤差的大小和AUV與4個浮標的幾何位置有關, 當AUV航行到600 s時, 位于4個海面移動浮標的中心位置同每個浮標的距離最近, 此時誤差最小; 基于偽距的EKF方法誤差最小, 大約保持在6m之內, 誤差明顯小于前2種。仿真結果表明, 改進后的MLBL算法能有效減少由于時間延遲產生的誤差。

5 結束語

本文研究了基于海面浮標的AUV的MLBL導航定位方法。重點分析了由于水聲傳播速度慢和AUV自身運動造成的時間延遲誤差。并針對時間延遲誤差提出了新的解決方法。主要是利用被定位AUV在一個Ping采樣周期內, 航位推算相對精度較高的特點, 通過對位置狀態的前推, 重構系統的量測方程, 從而同量測值在時間上匹配, 修正了時間延遲造成的誤差。文章最后通過數學仿真, 給出了誤差曲線, 驗證了算法的有效性。

[1] Vaganay J, Leonard J, Curcio J. Experimental Validation of the Moving Long Base-Line Navigation Concept, in Autonomous Underwater Vehicles[C]//2004 IEEE/OES, 2004: 59-69.

[2] Curcio J, Leonard J, Vaganay J, et al. Experiments in Moving Baseline Navigation Using Autonomous Surface Craft[C]// 2005 MTS/IEEE International Conference on Oceans. Washnigon, PC, USA: IEEE, 2005:730-735.

[3] Bahr A, Leonard J, Fallon M. Cooperative Localization for Autonomous Underwater Vehicles[J]. The International Journal of Robotics Research, 2009, 28(6): 714-728.

[4] Bahr A．Cooperative Localization for Autonomous Underwater Vehicles[D]. Massachusetts Institute of Technology and Woods Hole Oceanographic Institution, 2009.

[5] Baccou P, Jouvencel B, Creuze V, et al. Cooperative Positioning and Navigation for Multiple AUV operations[C]//Oceans'01 MTS/IEEE Conference and Exhibition, Honolulu, 2001: 1816-1821．

[6] 張立川, 徐德民. 基于移動長基線的多AUV協同導航[J].機器人, 2009, 40(4): 794-799.Zhang Li-chuan, Xu De-min. Cooperative Navigation of Multiple AUVs Using Moving Long Baseline[J]. Robot, 2009, 40(4): 794-799.

[7] 田坦. 水下定位與導航技術[M]. 北京: 國防工業出版社, 2007.

(責任編輯:楊力軍)

A Cooperative Navigation Method of EKF Moving Long Baseline for AUV Based on Pseudo-range Measurements

LIU Ming-yong, HUANG Bo, CAI Ting

(College of Marine Engineering, Northwestern Polytechnical University, Xi′an 710072, China)

Moving long baseline(MLBL) localization of an autonomous undersea vehicle(AUV) will produce a certain amount of time-delay positioning error due to the slow acoustic signal propagation velocity in water and the movement of AUV itself, which cause differences in time and position in measuring the distances between an AUV and different buoys. In this paper, an extended Kalman filtering(EKF) MLBL time-delay algorithm based on pseudo-range measurement is proposed. In this algorithm, the EKF is adopted to reconstruct the measurement equation of the system via pushing-forward the AUV's position states. So the measurement equation matches with the system measurement in time, and the error from time delay is eliminated. Theoretical analysis and simulation results show that the algorithm can significantly improve the accuracy of navigation and localization.

autonomous undersea vehicle(AUV); moving long baseline; positioning error; extended Kalman filter(EKF)

TJ630.33; TP391

A

1673-1948(2012)06-0432-05

2012-07-30;

2012-09-03.

國家自然科學基金(50979093).

劉明雍(1971-), 男, 博士生導師, 主要研究方向為水下導航、慣性技術及非線性控制.