基于EKF的AUV協同定位方法仿真與驗證＊

（海軍駐武漢701所軍代表室 武漢 430060）

1 引言

20世紀90年代日本科學家Kurazume Ryo等在研究多機器人定位時首次提出了協同定位的概念［1］。這一提出引起了導航領域的學者的極大興趣，隨著定位和導航技術在各個應用領域中發揮的作用逐步加大，基于位置的服務的軍用價值和商業價值也逐步擴大［2］。自20世紀開始各國學者開始進行協同導航相關研究，在衛星導航、機器人以及水下航行器等多個領域有了一定成果。美國Minnesota大學以Stergios I．Roumeliotis為首的研究小組開展了一系列機器人協同定位基礎理論研究，如可觀測性分析、誤差分析、估計一致性分析、隊形優化研究等［3］。MIT 的John J．Leonard研究團隊對水下航行器的協同定位進行了基于水面皮艇和不基于水面皮艇的算法研究。

歐盟最近資助某科研團體進行研究的名為GREX 的項目也在對協同定位進行研究，他們的主要目的是通過某種軟件來協調多MAUV 的行動。將水下機器人看作一個中轉站，將來自主艇的信息通過網絡傳遞給其他機器人，其通信距離可達到英里級別［4］。

美國L-3通信公司下屬的Interstate Electronics Corp研究了地球同步衛星協同導航的分散式算法，同時L-3通信公司資助悉尼大學自主系統研究中心以機器人協同導航為背景，研究了相關的協同算法［5］。

MIT 的“CRADE”［6］系統采用通信導航輔助的多無人水下航行器（UUV）協同定位方法［7］。這個多UUV 系統包括一些專門用于通信和導航的裝備有高精度慣導系統和多普勒測速儀的輔助航行器。它們在系統之中用于與其他低精度的航行器的通信與定位。

國內外學者嘗試采用單個領航艇來實現協同定位，即所謂的單領航艇協同定位。如美國的Bahr A 和Leonard J等提出了一種基于Kullback-Leiber的單領航艇協同導航算法，并做了大量的水上試驗；Baccou P等為了解決單領航艇協同導航求解不充分的問題提出了通過領航艇、跟隨艇之間的機動來提高協同導航的定位精度；國內西北工業大學協同定位研究小組，則采用相鄰時刻的運動矢徑作為量測量來實現單領航艇協同定位［8～13］。

國內學者王玲對多機器人群在位置的環境中基于相對測量和自身的運動進行協同、合作定位的技術。她對多機器人協同定位的系統模型進行了深入研究，并且由此借助EKF 算法來融合運動信息和相對觀測信息，給出了定位機器人群中每個機器人的濾波方程［14～16］。

隨著協同作戰應用需求的提升，協同導航算法研究逐步成為熱門。本文主要研究利用擴展卡爾曼濾波算法來提升協同定位的精度，這無疑有很大理論價值和實際意義。對無人艇應用的拓展和協同導航算法研究的深入有著極大的促進作用。

2 協同定位方法

多AUV 協同導航正是基于地面移動機器人的思想，通過定位信息傳遞與相對距離測量實現協同導航。但是，由于水面環境與陸地完全不同，不能采用與陸地機器人相同的通訊手段。多艇協同定位時，艇間是通過水聲通信裝置發送彼此的位置信息并同時測量通信雙方的相對距離，通過信息融合對自身定位進行修正，提升AUV 的定位精度。

多艇協同定位技術已經成為當前多艇導航定位領域的重點研究課題。從位置估計的融合算法來分，主要可以分為基于幾何學的協同定位和基于概率學的協同定位。我們主要采取基于概率學的濾波定位算法，本設計針對多水面AUV 編隊的定位問題，應用擴展卡爾曼濾波算法提高AUV 定位的精度，達到預期效果。

在對多艇編隊進行協同定位研究時，有兩種協同定位方式得到學者的關注。

1）并行式。系統中每個艇的功能和結構相同，使用各自的導航系統進行導航定位，通過水聲通信，獲得“伙伴”艇的位置信息。

2）主從式。即有一個攜帶有高精度捷聯慣性導航系統和多普勒測速儀等設備的AUV 擔任領航艇，而在其他艇上裝備成本低、精度低的慣導系統。根據領航艇提供的高精度全局位置信息，結合自身所測得的與領航艇的相對距離和方位信息，對自身推位結果進行修正，從而提高導航精度。

本設計中主要研究采用主從式單個領航艇來實現協同定位，協同定位原理如圖1所示。

圖1 協同定位原理圖

跟隨艇向領航艇發送測距請求信號，領航艇一旦接受到測距請求信號，即可通過水聲傳播發出與到達時間差計算出相對距離，緊接著領航艇通過水聲Modem 以廣播的形式向跟隨艇發送領航艇的位置信息以及相互之間的距離信息，然后跟隨艇就可以通過一定的濾波算法來校正跟隨艇的位置信息，達到提高導航精度的目的。此外領航艇可以通過進行一定的路徑規劃來提高協同導航系統的可觀測性，進而提高單領航艇協同導航的定位精度。

3 多AUV 系統協同定位算法

本文采取基于單領航艇的主從式協同導航定位。領航的主AUV 裝備高精度慣性導航設備、多普勒計程儀、GPS接收機、水聲通信設備，其導航設備以慣性導航為主，初始位置通過GPS獲得，以多普勒速度儀測量的絕對速度作為慣導外部輸入，進一步提高了精度，跟隨艇裝備低精度導航設備。主從艇在執行任務前均通過GPS進行時間校正，以保證時間同步。在協同定位的過程之中，主艇按照預先設定的時間間隔向外發送固定頻率的聲信號脈沖，同時廣播領航艇的自身位置信息，從艇接收到聲信號脈沖及主艇的位置信息之后，由聲信號脈沖結算出相對距離，再根據主艇的位置來完成協同定位。

單領航艇的定位方程可以表示為

其中，xk，yk無人艇航推定位信息，xk表示k時刻無人艇的經度信息，yk表示k時刻無人艇的緯度信息。Δt為采樣周期，一般取為1s。vk為無人艇的前向合成速度，φk表示無人艇航向角，ωk表示k時刻無人艇航行的角速度，均受高斯白噪聲干擾。

定位方程可以簡寫為

xk＋1＝f（xk，uk，ωk）＝Xk＋Γ（uk＋ωk），式中Xk＝（xk，yk，φk）T表示AUV 在k時刻的狀態，Γ（uk＋ωk）為非線性項，uk＝（vk，φk）T，ωk為高斯白噪聲。

系統過程噪聲方差陣：

圖2 當領航艇協同定位示意圖

單領航艇協同定位演示圖如圖2所示，跟隨艇自身導航設備較低的精度會造成其定位誤差隨著時間的增長而增加，因此需領航艇周期性的提供自身精確的位置信息以及它們之間的距離信息，使跟隨艇利用這些信息來抑制誤差的不斷積累。

在單領航艇協同定位中，由于只有一個領航艇，因此可使用的信息也變少，即量測量為領航艇位置信息、以及距離標量信息，即：

其中（xak，yak）表示k時刻領航艇的位置信息，（xbk，ybk）表示的是k時刻跟隨艇的位置信息。領航艇、跟隨艇之間的水平距離的量測噪聲vk為零均值的高斯白噪聲，其方差矩陣可以表示為

由于式（4）組成的系統模型為非線性系統，因此需采用非線性濾波方法對進行狀態估計，而擴展卡爾曼濾波算法由于操作方便，逐漸成為工程實踐中應用最廣泛的一類非線性濾波算法，這里我們采用擴展卡爾曼濾波來實現協同定位算法的設計。

1）根據運動學方程，擴展卡爾曼濾波算法系統狀態一步預測可表示為

2）協同定位系統一步預測協方差矩陣為

其中Fk為狀態轉移矩陣；Gk為過程噪聲分布矩陣；分別為

3）k＋1時刻的系統量測值預測：

4）k＋1時刻的系統新息及新息協方差為：

5）濾波增益陣為

6）系統狀態估計及估計均方誤差協方差陣可表示為

其中：

4 仿真分析

為檢驗算法的可靠性與實用性，進行仿真分析研究。在仿真中領航艇的起點坐標是（0，0），單位為m，初始航向角為30°，航速為10節。航行半小時之后航向角變為90°，速度保持不變。跟隨艇的起點坐標是（10，10），單位為m，初始航向角為0°，角速度為0．05°／s，航速為8 節。仿真時長為1h。速度信息有多普勒速度傳感器測量，取＝（0．5）2的零均值高斯白噪聲，航向角信息由陀螺經測量，取＝（0．3）2得零均值高斯白噪聲，無人艇的航行速度和航向角的噪聲相互獨立且不相關。量測方程中xk，yk，rk為無人艇經緯度和領航艇與跟隨艇之間距離，相互獨立且不相關，取仿真之中引入的量測噪聲為

仿真結果如圖3～圖8所示。

圖3 真實軌跡示意圖

圖4 X 軸定位誤差比較圖

圖5 Y 軸定位誤差比較圖

圖6 定位誤差比較圖

圖7 定位誤差比較圖（局部放大）

圖8 跟隨艇航跡比較圖

5 結語

圖4～圖7表示從艇運動過程中航位推算得到的位置和EKF算法得到的位置信息的誤差。由圖中我們可以看到，EKF算法較航位推算在X軸和Y軸方向都有較小的誤差。并且可以看到當主艇改變其航向角時EKF算法的誤差有一定的波動，這表明主艇航行的軌跡突變對從艇定位精度也有較大的影響，這為以后實際應用中，在設定主從航行軌跡時做出啟示，即盡量保證主從艇的航向不會有較大的改變。圖8中點線表示航位推算的軌跡，虛線表示協同導航求得的軌跡，實線表示真實軌跡，由圖8可以看出，采用自身的捷聯慣導系統和多普勒測速儀組合定位求得的位置曲線明顯偏離真實軌跡，而采用本文所提出的協同定位算法求得的軌跡與真實軌跡較吻合。進一步由圖8可以得到，當跟隨艇只采用自身攜帶的低精度導航傳感器進行組合定位時，其整體的定位誤差也是不斷增大的。從而證明此算法確實能一定程度提高系統定位精度。

當領航艇、跟隨艇按照圖3所示的軌跡運動時，由于兩艇運動方式差別較大，所以可以認為在相鄰測距的方向都不同，系統可觀測性強，協同定位精度高。

通過拐彎仿真可以得到：單領航艇協同定位精度與領航艇、跟隨艇之間的相對幾何位置關系有很大的關系，如何進行路徑規劃使得領航艇、跟隨艇始終處于協同導航的最佳位置是單領航艇協同定位下一步研究的重點方向。

［1］Kurazume R，Nagata S，Hirose S．Cooperative positioning with multiple robots［C］／／Proceedings of the IEEE International Conference on Robotics and Automation，Los Alamitos，CA，1994：1250-1257．

［2］Win M Z，et al．Network Localization and Navigation via Cooperation［J］．IEEE Communication Magazine，2011，49（5）：56-62．

［3］Hidaka Y S，Mourikis A I，Roumeliotis S I．Optimal Formations for Cooperative Localization of Mobile Robots［C］／／Proceedings of the IEEE International Conference on Robotics and Automation（ICRA），2005：4126-4131．

［4］Engel R，Kalwa J．Coordinated Navigation of Multiple Underwater Vehicles［C］／／17th International Offshore and Polar Engineering Conference（ISOPE），2007：1066-1072．

［5］Nicosia J．Decentralized Cooperative Navigation for Spacecraft［C］／／2007 IEEE Aerospace Conference，2007：1-6．

［6］Willcox S，Goldberg D，Vaganay J，et al．Multi-vehicle cooperativenavigation and autonomy with the bluefin CADRE system［C］／／Proceedings of International Federation of Automatic Control Conference．Oxford：Pergamon Press，2006：1-6．

［7］Bahr A．Cooperative localization for autonomous underwater vehicles［D］．Cambridge：Massachusetts Institute of Technology，2009．

［8］張立川，劉明雍，徐德民，等．基于水聲傳播延遲的主從式多無人水下航行器協同導航定位研究［J］．兵工學報，2009（12）：1674-1678．

［9］張立川，徐德民，劉明雍，等．多無人水下航行器協同導航與定位研究（英文）［J］．Journal of Marine Science and Application，2009（3）：216-221．

［10］張立川，徐德民，劉明雍．無人水下航行器網絡控制器設計［J］．船舶工程，2010（5）：30-32．

［11］姚堯，徐德民，張立川，等．通信延遲下的多UUV 協同定位—基于航跡預測的實時更新算法［J］．機器人，2011（2）：161-168．

［12］李聞白，劉明雍，李虎雄，等．基于單領航者相對位置測量的多AUV 協同導航系統定位性能分析［J］．自動化學報，2011（6）：724-736．

［13］張立川，徐德民，劉明雍．基于雙水聽器的多自主水下航行器協同導航方法［J］．系統工程與電子技術，2011（7）：1603-1606．

［14］王玲，劉云輝，萬建偉．基于相對方位的多機器人合作定位算法［J］．傳感技術學報，2007，40（4）：794-799．

［15］王玲．未知環境中基于相對測量的多機器人合作定位研究［D］．長沙：國防科學技術大學，2006．

［16］王玲，劉云輝，萬建偉，等．基于相對方位的多機器人合作定位算法［J］．傳感技術學報，2007，40（4）：794-799．