基于EKF的AUV協(xié)同定位方法仿真與驗(yàn)證＊

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

1 引言

20世紀(jì)90年代日本科學(xué)家Kurazume Ryo等在研究多機(jī)器人定位時(shí)首次提出了協(xié)同定位的概念［1］。這一提出引起了導(dǎo)航領(lǐng)域的學(xué)者的極大興趣，隨著定位和導(dǎo)航技術(shù)在各個(gè)應(yīng)用領(lǐng)域中發(fā)揮的作用逐步加大，基于位置的服務(wù)的軍用價(jià)值和商業(yè)價(jià)值也逐步擴(kuò)大［2］。自20世紀(jì)開(kāi)始各國(guó)學(xué)者開(kāi)始進(jìn)行協(xié)同導(dǎo)航相關(guān)研究，在衛(wèi)星導(dǎo)航、機(jī)器人以及水下航行器等多個(gè)領(lǐng)域有了一定成果。美國(guó)Minnesota大學(xué)以Stergios I．Roumeliotis為首的研究小組開(kāi)展了一系列機(jī)器人協(xié)同定位基礎(chǔ)理論研究，如可觀測(cè)性分析、誤差分析、估計(jì)一致性分析、隊(duì)形優(yōu)化研究等［3］。MIT 的John J．Leonard研究團(tuán)隊(duì)對(duì)水下航行器的協(xié)同定位進(jìn)行了基于水面皮艇和不基于水面皮艇的算法研究。

歐盟最近資助某科研團(tuán)體進(jìn)行研究的名為GREX 的項(xiàng)目也在對(duì)協(xié)同定位進(jìn)行研究，他們的主要目的是通過(guò)某種軟件來(lái)協(xié)調(diào)多MAUV 的行動(dòng)。將水下機(jī)器人看作一個(gè)中轉(zhuǎn)站，將來(lái)自主艇的信息通過(guò)網(wǎng)絡(luò)傳遞給其他機(jī)器人，其通信距離可達(dá)到英里級(jí)別［4］。

美國(guó)L-3通信公司下屬的Interstate Electronics Corp研究了地球同步衛(wèi)星協(xié)同導(dǎo)航的分散式算法，同時(shí)L-3通信公司資助悉尼大學(xué)自主系統(tǒng)研究中心以機(jī)器人協(xié)同導(dǎo)航為背景，研究了相關(guān)的協(xié)同算法［5］。

MIT 的“CRADE”［6］系統(tǒng)采用通信導(dǎo)航輔助的多無(wú)人水下航行器（UUV）協(xié)同定位方法［7］。這個(gè)多UUV 系統(tǒng)包括一些專門用于通信和導(dǎo)航的裝備有高精度慣導(dǎo)系統(tǒng)和多普勒測(cè)速儀的輔助航行器。它們?cè)谙到y(tǒng)之中用于與其他低精度的航行器的通信與定位。

國(guó)內(nèi)外學(xué)者嘗試采用單個(gè)領(lǐng)航艇來(lái)實(shí)現(xiàn)協(xié)同定位，即所謂的單領(lǐng)航艇協(xié)同定位。如美國(guó)的Bahr A 和Leonard J等提出了一種基于Kullback-Leiber的單領(lǐng)航艇協(xié)同導(dǎo)航算法，并做了大量的水上試驗(yàn)；Baccou P等為了解決單領(lǐng)航艇協(xié)同導(dǎo)航求解不充分的問(wèn)題提出了通過(guò)領(lǐng)航艇、跟隨艇之間的機(jī)動(dòng)來(lái)提高協(xié)同導(dǎo)航的定位精度；國(guó)內(nèi)西北工業(yè)大學(xué)協(xié)同定位研究小組，則采用相鄰時(shí)刻的運(yùn)動(dòng)矢徑作為量測(cè)量來(lái)實(shí)現(xiàn)單領(lǐng)航艇協(xié)同定位［8～13］。

國(guó)內(nèi)學(xué)者王玲對(duì)多機(jī)器人群在位置的環(huán)境中基于相對(duì)測(cè)量和自身的運(yùn)動(dòng)進(jìn)行協(xié)同、合作定位的技術(shù)。她對(duì)多機(jī)器人協(xié)同定位的系統(tǒng)模型進(jìn)行了深入研究，并且由此借助EKF 算法來(lái)融合運(yùn)動(dòng)信息和相對(duì)觀測(cè)信息，給出了定位機(jī)器人群中每個(gè)機(jī)器人的濾波方程［14～16］。

隨著協(xié)同作戰(zhàn)應(yīng)用需求的提升，協(xié)同導(dǎo)航算法研究逐步成為熱門。本文主要研究利用擴(kuò)展卡爾曼濾波算法來(lái)提升協(xié)同定位的精度，這無(wú)疑有很大理論價(jià)值和實(shí)際意義。對(duì)無(wú)人艇應(yīng)用的拓展和協(xié)同導(dǎo)航算法研究的深入有著極大的促進(jìn)作用。

2 協(xié)同定位方法

多AUV 協(xié)同導(dǎo)航正是基于地面移動(dòng)機(jī)器人的思想，通過(guò)定位信息傳遞與相對(duì)距離測(cè)量實(shí)現(xiàn)協(xié)同導(dǎo)航。但是，由于水面環(huán)境與陸地完全不同，不能采用與陸地機(jī)器人相同的通訊手段。多艇協(xié)同定位時(shí)，艇間是通過(guò)水聲通信裝置發(fā)送彼此的位置信息并同時(shí)測(cè)量通信雙方的相對(duì)距離，通過(guò)信息融合對(duì)自身定位進(jìn)行修正，提升AUV 的定位精度。

多艇協(xié)同定位技術(shù)已經(jīng)成為當(dāng)前多艇導(dǎo)航定位領(lǐng)域的重點(diǎn)研究課題。從位置估計(jì)的融合算法來(lái)分，主要可以分為基于幾何學(xué)的協(xié)同定位和基于概率學(xué)的協(xié)同定位。我們主要采取基于概率學(xué)的濾波定位算法，本設(shè)計(jì)針對(duì)多水面AUV 編隊(duì)的定位問(wèn)題，應(yīng)用擴(kuò)展卡爾曼濾波算法提高AUV 定位的精度，達(dá)到預(yù)期效果。

在對(duì)多艇編隊(duì)進(jìn)行協(xié)同定位研究時(shí)，有兩種協(xié)同定位方式得到學(xué)者的關(guān)注。

1）并行式。系統(tǒng)中每個(gè)艇的功能和結(jié)構(gòu)相同，使用各自的導(dǎo)航系統(tǒng)進(jìn)行導(dǎo)航定位，通過(guò)水聲通信，獲得“伙伴”艇的位置信息。

2）主從式。即有一個(gè)攜帶有高精度捷聯(lián)慣性導(dǎo)航系統(tǒng)和多普勒測(cè)速儀等設(shè)備的AUV 擔(dān)任領(lǐng)航艇，而在其他艇上裝備成本低、精度低的慣導(dǎo)系統(tǒng)。根據(jù)領(lǐng)航艇提供的高精度全局位置信息，結(jié)合自身所測(cè)得的與領(lǐng)航艇的相對(duì)距離和方位信息，對(duì)自身推位結(jié)果進(jìn)行修正，從而提高導(dǎo)航精度。

本設(shè)計(jì)中主要研究采用主從式單個(gè)領(lǐng)航艇來(lái)實(shí)現(xiàn)協(xié)同定位，協(xié)同定位原理如圖1所示。

圖1 協(xié)同定位原理圖

跟隨艇向領(lǐng)航艇發(fā)送測(cè)距請(qǐng)求信號(hào)，領(lǐng)航艇一旦接受到測(cè)距請(qǐng)求信號(hào)，即可通過(guò)水聲傳播發(fā)出與到達(dá)時(shí)間差計(jì)算出相對(duì)距離，緊接著領(lǐng)航艇通過(guò)水聲Modem 以廣播的形式向跟隨艇發(fā)送領(lǐng)航艇的位置信息以及相互之間的距離信息，然后跟隨艇就可以通過(guò)一定的濾波算法來(lái)校正跟隨艇的位置信息，達(dá)到提高導(dǎo)航精度的目的。此外領(lǐng)航艇可以通過(guò)進(jìn)行一定的路徑規(guī)劃來(lái)提高協(xié)同導(dǎo)航系統(tǒng)的可觀測(cè)性，進(jìn)而提高單領(lǐng)航艇協(xié)同導(dǎo)航的定位精度。

3 多AUV 系統(tǒng)協(xié)同定位算法

本文采取基于單領(lǐng)航艇的主從式協(xié)同導(dǎo)航定位。領(lǐng)航的主AUV 裝備高精度慣性導(dǎo)航設(shè)備、多普勒計(jì)程儀、GPS接收機(jī)、水聲通信設(shè)備，其導(dǎo)航設(shè)備以慣性導(dǎo)航為主，初始位置通過(guò)GPS獲得，以多普勒速度儀測(cè)量的絕對(duì)速度作為慣導(dǎo)外部輸入，進(jìn)一步提高了精度，跟隨艇裝備低精度導(dǎo)航設(shè)備。主從艇在執(zhí)行任務(wù)前均通過(guò)GPS進(jìn)行時(shí)間校正，以保證時(shí)間同步。在協(xié)同定位的過(guò)程之中，主艇按照預(yù)先設(shè)定的時(shí)間間隔向外發(fā)送固定頻率的聲信號(hào)脈沖，同時(shí)廣播領(lǐng)航艇的自身位置信息，從艇接收到聲信號(hào)脈沖及主艇的位置信息之后，由聲信號(hào)脈沖結(jié)算出相對(duì)距離，再根據(jù)主艇的位置來(lái)完成協(xié)同定位。

單領(lǐng)航艇的定位方程可以表示為

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

定位方程可以簡(jiǎn)寫為

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

系統(tǒng)過(guò)程噪聲方差陣：

圖2 當(dāng)領(lǐng)航艇協(xié)同定位示意圖

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

在單領(lǐng)航艇協(xié)同定位中，由于只有一個(gè)領(lǐng)航艇，因此可使用的信息也變少，即量測(cè)量為領(lǐng)航艇位置信息、以及距離標(biāo)量信息，即：

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

由于式（4）組成的系統(tǒng)模型為非線性系統(tǒng)，因此需采用非線性濾波方法對(duì)進(jìn)行狀態(tài)估計(jì)，而擴(kuò)展卡爾曼濾波算法由于操作方便，逐漸成為工程實(shí)踐中應(yīng)用最廣泛的一類非線性濾波算法，這里我們采用擴(kuò)展卡爾曼濾波來(lái)實(shí)現(xiàn)協(xié)同定位算法的設(shè)計(jì)。

1）根據(jù)運(yùn)動(dòng)學(xué)方程，擴(kuò)展卡爾曼濾波算法系統(tǒng)狀態(tài)一步預(yù)測(cè)可表示為

2）協(xié)同定位系統(tǒng)一步預(yù)測(cè)協(xié)方差矩陣為

其中Fk為狀態(tài)轉(zhuǎn)移矩陣；Gk為過(guò)程噪聲分布矩陣；分別為

3）k＋1時(shí)刻的系統(tǒng)量測(cè)值預(yù)測(cè)：

4）k＋1時(shí)刻的系統(tǒng)新息及新息協(xié)方差為：

5）濾波增益陣為

6）系統(tǒng)狀態(tài)估計(jì)及估計(jì)均方誤差協(xié)方差陣可表示為

其中：

4 仿真分析

為檢驗(yàn)算法的可靠性與實(shí)用性，進(jìn)行仿真分析研究。在仿真中領(lǐng)航艇的起點(diǎn)坐標(biāo)是（0，0），單位為m，初始航向角為30°，航速為10節(jié)。航行半小時(shí)之后航向角變?yōu)?0°，速度保持不變。跟隨艇的起點(diǎn)坐標(biāo)是（10，10），單位為m，初始航向角為0°，角速度為0．05°／s，航速為8 節(jié)。仿真時(shí)長(zhǎng)為1h。速度信息有多普勒速度傳感器測(cè)量，?。剑?．5）2的零均值高斯白噪聲，航向角信息由陀螺經(jīng)測(cè)量，?。剑?．3）2得零均值高斯白噪聲，無(wú)人艇的航行速度和航向角的噪聲相互獨(dú)立且不相關(guān)。量測(cè)方程中xk，yk，rk為無(wú)人艇經(jīng)緯度和領(lǐng)航艇與跟隨艇之間距離，相互獨(dú)立且不相關(guān)，取仿真之中引入的量測(cè)噪聲為

仿真結(jié)果如圖3～圖8所示。

圖3 真實(shí)軌跡示意圖

圖4 X 軸定位誤差比較圖

圖5 Y 軸定位誤差比較圖

圖6 定位誤差比較圖

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

圖8 跟隨艇航跡比較圖

5 結(jié)語(yǔ)

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

當(dāng)領(lǐng)航艇、跟隨艇按照?qǐng)D3所示的軌跡運(yùn)動(dòng)時(shí)，由于兩艇運(yùn)動(dòng)方式差別較大，所以可以認(rèn)為在相鄰測(cè)距的方向都不同，系統(tǒng)可觀測(cè)性強(qiáng)，協(xié)同定位精度高。

通過(guò)拐彎仿真可以得到：?jiǎn)晤I(lǐng)航艇協(xié)同定位精度與領(lǐng)航艇、跟隨艇之間的相對(duì)幾何位置關(guān)系有很大的關(guān)系，如何進(jìn)行路徑規(guī)劃使得領(lǐng)航艇、跟隨艇始終處于協(xié)同導(dǎo)航的最佳位置是單領(lǐng)航艇協(xié)同定位下一步研究的重點(diǎn)方向。

［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］張立川，劉明雍，徐德民，等．基于水聲傳播延遲的主從式多無(wú)人水下航行器協(xié)同導(dǎo)航定位研究［J］．兵工學(xué)報(bào)，2009（12）：1674-1678．

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

［10］張立川，徐德民，劉明雍．無(wú)人水下航行器網(wǎng)絡(luò)控制器設(shè)計(jì)［J］．船舶工程，2010（5）：30-32．

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

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

［13］張立川，徐德民，劉明雍．基于雙水聽(tīng)器的多自主水下航行器協(xié)同導(dǎo)航方法［J］．系統(tǒng)工程與電子技術(shù)，2011（7）：1603-1606．

［14］王玲，劉云輝，萬(wàn)建偉．基于相對(duì)方位的多機(jī)器人合作定位算法［J］．傳感技術(shù)學(xué)報(bào)，2007，40（4）：794-799．

［15］王玲．未知環(huán)境中基于相對(duì)測(cè)量的多機(jī)器人合作定位研究［D］．長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2006．

［16］王玲，劉云輝，萬(wàn)建偉，等．基于相對(duì)方位的多機(jī)器人合作定位算法［J］．傳感技術(shù)學(xué)報(bào)，2007，40（4）：794-799．