主從式無人水面艇協同定位濾波方法與實驗驗證

徐博,肖永平,高偉,劉亞龍,楊建

(1.哈爾濱工程大學自動化學院,黑龍江哈爾濱 150001;2.陜西航天導航設備有限公司,陜西寶雞 721006)

主從式無人水面艇協同定位濾波方法與實驗驗證

徐博1,肖永平2,高偉1,劉亞龍1,楊建1

(1.哈爾濱工程大學自動化學院,黑龍江哈爾濱 150001;2.陜西航天導航設備有限公司,陜西寶雞 721006)

多無人水面艇(USV)協同導航定位技術是解決復雜作業環境下導航問題的重要途徑。針對單主艇的主從式多USV的協同導航定位問題,建立了協同定位的狀態空間模型,然后將非線性模型繞濾波值展開,得到非線性系統的線性化模型;利用擴展卡爾曼濾波算法進行信息融合;針對雙艇間的距離、角度與協同定位精度的關系展開研究,對協同導航濾波算法使用李代數法進行了可觀測性分析。研究表明,控制相鄰兩時刻主從艇之間距離和角度變化量是提高協同定位精度的有效措施。通過仿真和實驗驗證,為下一步深入研究和路徑規劃奠定了基礎。

控制科學與技術;無人水面艇;測距;協同定位;擴展卡爾曼濾波

0 引言

無人水面艇(USV)也叫水面無人艇,實際上是一種水面無人平臺,主要用于執行危險或復雜環境下以及不適于載人艇執行的復雜任務,例如資源勘探和水文地理監測等。同時,近些年來USV在軍事上也廣為應用,在無人艇上配備先進的控制設備、傳感器系統、通信和武器系統之后,可以執行偵察、搜索、排雷和搜救等任務,在反潛作戰、反特種作戰和打擊海盜等領域也有著巨大的作用。

在現代海洋資源開發和軍事任務中,常常面臨著復雜的情況,需要多艘無人艇協同作業才能夠完成,而導航是實現協作的基礎,因此研究多無人艇的協同定位導航有著巨大的理論價值和實際作用。協同單元相互合作不僅可以節省整體工作時間,還能夠避免編隊中全部單元均裝備高精度儀器所造成的費用過高問題。通過USV間的相互合作可以建立一個穩定的指揮系統;另一方面,未來戰爭單兵作戰的機會將大大減小,取而代之的是多系統的協同指揮。

隨著協同作戰應用需求的提升,協同導航算法研究逐步成為熱門。20世紀90年代日本科學家Kurazume等在研究多機器人定位時首次提出了協同定位的概念[1]。這一理論的提出引起了導航領域學者的極大興趣,隨著定位和導航技術在各個應用領域中發揮的作用逐步加大,基于位置服務的軍用價值和商業價值也逐步擴大[2]。隨著各國學者在協同導航相關方面的進一步研究,使得協同導航在衛星導航、機器人以及水下航行器等多個領域都取得了一定成果。其中,美國Minnesota大學以Roumeliotis為首的研究小組開展了一系列機器人協同定位基礎理論研究,如可觀測性分析、誤差分析、估計一致性分析、隊形優化研究等[3]。美國麻省理工學院的Leonard研究團隊對水下航行器的協同定位進行了基于水面皮艇和不基于水面皮艇的算法研究[4]。

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

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

美國麻省理工學院的“CRADE”系統采用通信導航輔助的多無人水下航行器(UUV)協同定位方法[7]。這個多UUV系統包括一些裝備有高精度慣導系統和多普勒測速儀、專用于通信和導航的輔助航行器,幫助其他低精度航行器進行通信與定位。

美國的Curcio等提出了一種基于Kullback-Leiber的單主艇協同導航算法,并做了大量的水上試驗[8];Baccou等為了解決單主艇協同導航求解不充分的問題提出了通過主艇、從艇之間的機動來提高協同導航的定位精度[9];Singh等提出了單程測距技術,解決了雙程測距中時間延遲而造成的測距精度下降的問題,且可以任意擴展艇的數量[10]。

Bahr[11]和國內西北工業大學協同定位研究小組[12-17],則采用相鄰時刻的運動矢徑作為觀測量來實現單主艇協同定位。國內學者王玲等對多機器人群在未知環境中進行協同、合作定位的技術進行了深入研究,基于相對測量和自身的運動,建立了系統模型,并且由此借助擴展卡爾曼濾波(EKF)算法來融合運動信息和相對觀測信息,給出了定位機器人群中每個機器人的濾波方程[18-19]。

綜上所述,國內外學者針對機器人和水下潛器進行了相關的研究工作,但是對于水面艇協同導航的研究尚處于起步階段。根據主艇數目的不同,主從式無人艇的協同定位研究分為基于雙主艇和基于單主艇兩大類?；陔p主艇的協同方案定位精度相對較高,但是所需信息量大,且對雙艇信息的同步性要求很高,相比來說基于單主艇的系統需要的融合信息更少,系統計算量較小,具有更強的應用價值。本文研究利用EKF算法來提升USV協同定位的精度,并分析了其可觀測性問題,具有較強的理論價值和實際意義。

1 協同定位方案

多USV協同定位基于地面移動機器人的思想,通過定位信息傳遞與相對距離測量實現協同定位。多艇協同定位時,艇間是通過水聲通信裝置發送彼此的位置信息并同時測量通信雙方的相對距離,通過信息融合對自身定位進行修正,提升無人水面艇的定位精度。多水面艇協同定位采取基于概率學的濾波定位算法,針對多USV編隊的定位問題,應用EKF算法提高USV定位的精度,達到預期效果。多USV的協同定位方式主要為:并行式(系統中每個USV的功能和結構完全相同)和主從式(系統中有一個攜帶有高精度捷聯慣性導航系統和多普勒測速儀等設備的艇擔任主艇,而在其他艇上裝備成本低、精度低的慣導系統)。

協同定位原理如圖1,從艇向主艇發送測距請求信號,主艇接收到測距請求信號后通過水聲傳播發出與到達的時間差來計算出相對距離以及通過水聲調制解調器以廣播的形式向從艇發送主艇的位置信息以及相互之間的距離信息,然后從艇就可以通過一定的濾波算法來校正從艇的位置信息,達到提高從艇導航精度的目的。

圖1 協同定位原理圖Fig.1 Principle of cooperative localization

2 多USV系統協同定位濾波算法

2.1 多USV運動學模型建立

由于從艇自身導航設備精度較低,隨著時間的增長,其定位誤差不斷積累,因此采用單個高精度的主艇提供精確的位置信息和測距信息來抑制誤差的不斷積累。

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

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

定位方程可以簡寫為

式中,Xk=(xk,yk,φk)T,表示USV在k時刻的狀態; Γ(uk+ωk)為非線性項,uk=(vk,wk)T,均受高斯白噪聲影響。

在單主艇協同定位中,由于只有一個主艇,因此可使用的信息也變少,即量測量為:主艇位置信息以及距離標量信息。因此,量測量Zk可以表示為

式中:(xMk,yMk)表示k時刻主艇的位置信息;(xSk, ySk)表示的是k時刻從艇的位置信息;rk為k時刻的距離信息;h[]表示非線性函數;主艇與從艇之間水平距離的量測噪聲為零均值的高斯白噪聲,其方差矩陣分別為

2.2 模型的線性化與濾波步驟

由運動學方程可知此系統為非線性系統,常規的卡爾曼濾波(KF)遞推方程只適合于線性系統,因此就需要利用適用于非線性系統的EKF方程來進行處理。本文采用圍繞濾波值將f[]和h[]展成泰勒級數并保留一次以下項的方法得到非線性系統的線性化模型。將f[]圍繞展開,泰勒級數略去二階以上項,得到

1

3 可觀測性分析

所謂量測系統的能觀測性,是指系統的初始狀態可由其輸出的量測值來確定的一種性能。對于如下一個線性離散時變系統:

系統是否可觀測的實質就是系統狀態Xk能否根據一組觀測值Z=[Z1,Z2,…,Zk]T來確定。根據(18)式,可以將量測和狀態的對應關系寫成如下矩陣形式[20]:

由(21)式可以看出,要想根據觀測量Z1,Z2,…,Zk最終確定狀態Xk,其充分必要條件是矩陣Wo(0,k)是非奇異矩陣,Wo(0,k)被定義為系統的能觀測性格拉姆判別矩陣。對于單主艇協同導航離散時變系統,系統可觀測判別矩陣的等價形式可以表示為

式中:Γ(k-N+1,k)為離散時變系統的可觀測秩判別矩陣;n為系統狀態向量維數。

由2.1節(3)式可以看出,量測量Z主要取決于第三維觀測信息rk,即距離rk將主從艇聯系起來,對子艇的導航誤差信息的估計起決定性作用。對于此一維觀測信息Z,需要至少進行N=2次觀測才可能完全估計出系統的二維位置狀態。根據連續兩次觀測數據得到系統的可觀測矩陣為

根據系統可觀測性秩判別條件,系統可觀測當且僅當系統可觀測矩陣滿秩,即系統的可觀測矩陣行列式不為0,

也就是說,當且僅當相鄰時刻主從USV距離向量方向相同時系統不可觀測,在其他條件下均可觀測。為了進一步分析系統可觀測度大小與系統狀態的量化對應關系,采用了基于譜條件數的可觀測性分析理論,對系統的可觀測性做進一步的定量分析[10-11]。

條件數反映了線性方程組AX=b(b∈Cn)的解對b的誤差或不確定度敏感性的度量,其中A為方程組的系數矩陣。如果A的條件數越小,則b的微小改變引起解X的改變越小,因此數值穩定性也越好。

當N=2時,cond(A)2為2范數(譜范數)的條件數,簡稱為譜條件數[12]。矩陣的條件數越小,矩陣越趨于良態,數值穩定性越好;反之,矩陣越病態,數值穩定性越差。因此可以通過觀測矩陣條件數的大小來確定系統的可觀測性強弱。由于恒有cond(A)≤1,因此將可觀測矩陣譜條件數的倒數定義為系統的可觀測度,表示為式中:dk為k時刻主從USV二維平面直線距離。令θk為k時刻主從USV距離向量方位角,則觀測矩陣可以進一步表示為

從(31)式可以看出可觀測度大小主要由相鄰時刻主從USV距離比γ以及相鄰時刻主從USV距離向量方位變化Γθ來決定,因此當γ確定時就可以得到可觀測度S與Γθ的關系,同理當Γθ確定時就可以得到可觀測度S與γ的關系,如圖2和圖3所示。

圖2 S與Γθ之間的關系Fig.2 The relation between S and Γθ

從上述理論推導和仿真圖可以得到:

1)從圖2可以看出,無論γ如何取值,系統可觀測度S隨著Γθ的變化先增大,后減小,并且在Γθ=90°處取得極大值,并且曲線以90°為軸,左右兩邊基本對稱。

圖3 S與γ之間的關系Fig.3 The relation between S and γ

2)從圖3中可以看出,無論Γθ如何取值,系統可觀測度S隨著γ的變化先增大,后減小,并且在γ=1處取得極大值,在其左右兩側曲線呈現出強烈的不對稱性和非線性。0＜γ＜1時,越接近0,可觀測度下降越快。大于1時,可觀測度的變化相對要慢,而且當其大于一定數值的時候,所引起的可觀測度的變化變得非常緩慢。

3)系統可觀測度S在Γθ=90°、γ=1處取得極大值,當濾波周期取為1 s的情況下,即主從艇間以角速度為90°/s做勻速圓周運動時,系統可觀測性最好。

4 仿真分析

為檢驗算法的可靠性和實用性,本文進行仿真分析,設置如下:

1)主艇起始坐標(10 m,10 m),初始航向角為0°,航速為8 kn,角速度為0.05°/s.從艇起始坐標(0 m,0 m),初始航向角為30°,航速為10 kn,航行30 min之后航向角變為90°,速度保持不變。狀態更新周期Γt=10 s,仿真時長為180 s.

圖4～圖7給出了從艇運動過程中航位推算(DR)得到的定位誤差和基于EKF算法得到的協同定位誤差。由圖中可以看到:

圖4 真實軌跡示意圖Fig.4 Real Tracks

圖5 定位誤差比較圖Fig.5 Locating errors

圖6 從艇航跡比較圖Fig.6 Tracks of slave USV

1)圖6中虛線表示航位推算的軌跡,空心圓線表示協同導航求得的軌跡,實線表示真實軌跡。由圖6可知,當從艇只采用自身的捷聯慣導系統和多普勒測速儀航位推算時,求得的位置明顯偏離真實軌跡,其整體的定位誤差不斷增大。

圖7 從艇航跡比較局部放大圖Fig.7 Tracks of slave USV(enlarged)

2)EKF算法較航位推算在x軸和y軸方向都有明顯的提高。當主艇、從艇按照圖4所示的軌跡運動時,由于兩艇運動方式差別較大,所以可以認為在相鄰測距的方向不同,系統可觀測性強,協同定位精度高。

3)當主艇改變其航向角時EKF算法的誤差有一定的波動,這表明主艇航行的軌跡突變對從艇定位精度也有較大的影響(因為當USV改變其航向角時,用于航位推算的運動學模型與USV的真實動力學模型之間存在失配,再加上從USV配備的設備精度較低,導致它的定位誤差快速增長),這為以后實際應用中如何設定主從航行軌跡作出啟示,即盡量保證主從艇的航向不發生突變。

圖8從可觀測性角度對仿真結果進行了分析。當主艇、從艇按照圖4所示的軌跡運動時,兩艇較大的運動方式差別使得系統可觀測性較強,反映在EKF算法上使得到的協同定位誤差較小。100 s處主艇航行的軌跡突變,系統可觀測性也大幅度降低,帶來較大的濾波誤差,使協同定位精度變差。隨后無較大軌跡突變,系統可觀測性逐漸恢復,協同定位誤差也逐漸變小。

圖8 系統可觀測性仿真圖Fig.8 Simulation diagram of system's observability

為了更清晰地描述EKF算法的定位效果,表1給出了t時刻EKF算法和航位推算算得的從艇位置誤差信息。從表中可以更加直觀地看出,EKF算法比DR算法所得到的位置誤差更小。

表1 t時刻從艇位置誤差Tab.1 Location errors of slave USV

5 水上實驗驗證

為了驗證本算法的有效性,在無錫太湖以水面小艇為載體進行了水上實驗。針對于不同機動方式進行了測試,進一步驗證了上述理論和仿真的正確性。

主艇上配備GPS、水聲通信設備(驗證本算法不需要使用主艇上的多普勒計程儀),從艇上配備MEMS微機械慣性導航系統、DVL計程儀以及水聲通信設備,兩艇之間通過水聲設備完成測距與通信。所用設備及其數量如表2所示,實驗設備現場如圖9所示。

表2 實驗設備Tab.2 Experimental equipment

上述設備中,水聲測距誤差為1 m,GPS定位精度為2 m,DVL計程儀測速誤差1%,MEMS慣性系統輸出精度5°/h.

實驗步驟如圖10所示。

步驟1,主艇的計算機時鐘調為同步,精確調到秒級。

步驟2,從艇通過MEMS輸出的航向角和GPS導航系統輸出的速度進行航跡推位。

圖9 實驗設備現場Fig.9 Site of experimental equipment

步驟3,主艇向從艇發出自身的位置信息。

步驟4,從艇通過自身的GPS位置信息和收到的主艇的GPS位置信息計算出和主艇的距離,作為觀測量,使用EKF算法計算航跡的估值,并和GPS輸出的航跡進行比較。

圖10 實驗步驟示意圖Fig.10 Experimental Procedure

實驗共進行2 000 s.在實驗中,主艇起點為31°25′16″N,120°8′14″E,從艇起點為31°25′19″N, 120°8′35″E.兩艇同時起航,做近似平行的并排前進運動。1 400 s后,從艇停止運動,終點為31°26′1″N, 120°8′13″E,同時主艇繼續圍繞從艇做半徑為170 m左右的圓周運動。2 000 s后主艇停止,主艇終點為31°25′55″N,120°8′11″E.以這兩組典型運動狀態數據為樣本,針對協同導航算法進行了驗證,兩組運動軌跡(圓周運動和并排前進)如圖11所示。

所對應的航位推算誤差和協同導航濾波算法誤差曲線如圖12所示。

圖11 主從艇運動軌跡圖(圓周運動和并排前進)Fig.11 Tracks of USVs

綜合圖11、圖12可以看出,雙艇運動的相對位置關系對協同導航的定位精度影響很大。前1 400 s對應于兩艇做并排前進運動,協同定位誤差小于組合推位誤差,但是濾波的效果不是很明顯。1 400～2000 s,從艇靜止不動,主艇圍繞從艇做半徑為170 m左右的圓周運動,這種運動狀態下的協同定位誤差迅速收斂至10 m以內,協同定位效果明顯優于并排前行的精度。

圖12 從艇協同定位誤差與組合推位誤差比對圖Fig.12 The comparison of position errors of the two methods

圖13 協同導航系統可觀測度Fig.13 Observability of cooperative navigation system

從可觀測性的角度對實驗結果進行分析。由圖13可以看出,1 400 s之前主從艇做近似平行的并排前進運動,主從艇間相對位置變化幅度較小,因此可觀測度較小,導致定位誤差相對較大。與此相比, 1 400 s以后的圓周運動帶來主從艇間較大的相對位置變化,使系統可觀測度增加,因此定位誤差大幅度減小。由此可見,主從艇不同的運動軌跡會導致可觀性度的變化,進而影響協同定位精度。在協同導航過程中,需要合理設計主從艇的運動軌跡。

6 結論

本文針對多USV編隊的協同定位問題給出了較實用的算法,分析了可觀測性問題,并通過了仿真與實驗驗證。主要結論如下:

1)給出了無人艇協同導航的背景和意義;給出了算法的基本方程并介紹了無人艇協同導航建模的基礎理論,之后建立系統定位模型。

2)提出EKF算法并使用Matlab進行仿真,通過設定一定的運動軌跡來實現主艇、從艇同時達到較高的定位精度。通過仿真曲線比較,本算法能夠有效地提升從艇定位精度,相比航位推算所產生的誤差,EKF算法能夠顯著地減小定位誤差。

3)從可觀測性角度進行了分析。研究結果表明單主艇協同定位精度與主艇、從艇之間的相對幾何位置關系有很大的關系,下一步研究的重點方向是如何進行路徑規劃使得主艇、從艇始終處于協同導航的最佳位置。

本算法雖然通過驗證,可以有效提高定位精度,但可以預見的是,在實際過程中還有許多需要考慮的問題,例如噪聲問題的不確定性等。這些問題都需要在實際操作過程中去探索。本文為進一步研究多水面艇的協同導航定位問題提供了可行性思路和方法。

References)

[1] Kurazume R,Nagata S,Hirose S.Cooperative positioning with multiple robots[C]∥International Conference on Robotics and Automation.Los Alamitos,California:IEEE,1994:1250-1257.

[2] Win M Z,Conti A,Mazuelas S,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]∥IEEE International Conference on Robotics and Automation(ICRA).Barcelona,Spain:IEEE,2005:4126-4131.

[4] Bahr A,Walter M,Leonard J.Consistent cooperative localization[C]∥IEEE International Conference on Robotics and Automation.Kobe, Japan:IEEE,2009.

[5] Engel R,Kalwa J.Coordinated navigation of multiple underwater vehicles[C]∥17th International Offshore and Polar Engineering Conference.Bremen,Germany:IEEE,2007:1066-1072.

[6] Nicosia J.Decentralized cooperative navigation for spacecraft[C]∥2007 IEEE Aerospace Conference.Big Sky,MT:IEEE,2007: 1-6.

[7] 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,UK:Pergamon Press,2006:1-6.

[8] Curcio J,Leonard J,Vaganay J,et al.Experiments in moving baseline navigation using autonomous surface craft[C]∥Proceedings of MTS/IEEE OCEANS.Washington,DC:IEEE,2005: 730-735.

[9] Baccou P,Jouvencel B,Creuze V,et al.Cooperative positioning and navigation for multiple AUV operations[C]∥MTS/IEEE Oceans 2001 Conference Proceedings.Honolulu,HI:IEEE, 2001:1816-1821.

[10] Singh S,Grund M,Bingham B,et al.Underwater acoustic navi-gation with the WHOI micro-modem[C]∥Proceedings of the IEEE/MTS OCEANS Conference and Exhibition.Boston,MA: IEEE,2006:1-4.

[11] Bahr A.Cooperative localization for autonomous underwater vehicles[R].Cambridge,MA,US:Massachusetts Institute of Technology,2009.

[12] 張立川,劉明雍,徐德民,等.基于水聲傳播延遲的主從式多無人水下航行器協同導航定位研究[J].兵工學報,2009, 30(12):1674-1678.

ZHANG Li-chuan,LIU Ming-yong,XU De-min,et al.Cooperative localization for multi-UVS based on time-of-flight of acoustic signal[J].Acta Armamentarii,2009,30(12):1674-1678.(in Chinese)

[13] Zhang L C,Xu D M,Liu M Y,et al.Cooperative navigation and location for multiple UUVs[J].Journal of Marine Science and Application,2009,8(3):216-221.

[14] 張立川,徐德民,劉明雍.無人水下航行器網絡控制器設計[J].船舶工程,2010,32(5):30-32.

ZHANG Li-chuan,XU De-min,LIU Ming-yong.Design of networked controller for unmanned underwater vehicle[J].Ship Engineering,2010,32(5):30-32.(in Chinese)

[15] 姚堯,徐德民,張立川,等.通信延遲下的多UUV協同定位——基于航跡預測的實時更新算法[J].機器人,2011, 33(1):161-168.

YAO Yao,XU De-min,ZHANG Li-chuan,et al.Cooperative localization of multiple UUVs with communication delays-a realtime update method based on path predication[J].Robot,2011, 33(1):161-168.(in Chinese)

[16] 李聞白,劉明雍,李虎雄,等.基于單領航者相對位置測量的多AUV協同導航系統定位性能分析[J].自動化學報,2011, 37(6):724-736.

LI Wen-bai,LIU Ming-yong,LI Hu-xiong,et al.Localization performance analysis of cooperative navigation system for multiple AUVS based on relative position measurements with a single leader[J].Acta Automatic Sinica,2011,37(6):724-736.(in Chinese)

[17] 張立川,徐德民,劉明雍.基于雙水聽器的多自主水下航行器協同導航方法[J].系統工程與電子技術,2011,33(7): 1603-1606.

ZHANG Li-chuan,XU De-min,LIU Ming-yong.Cooperative navigation for multiple autonomous underwater vehicle based on two hydrophones[J].Systems Engineering and Electronics,2011, 33(7):1603-1606.(in Chinese)

[18] 王玲,劉云輝,萬建偉,等.基于相對方位的多機器人合作定位算法[J].傳感技術學報,2007,20(4):794-799.

WANG Ling,LIU Yun-hui,WAN Jian-wei,et al.Multi-robot cooperative localization based on relative bearing[J].Chinese Journal of Sensors and Actuators,2007,20(4):794-799.(in Chinese)

[19] 王玲.未知環境中基于相對觀測量的多機器人合作定位研究[D].長沙:國防科學技術大學,2006.

WANG Ling.Research on multi-robot cooperative localization based on relative observation in unknown environment[D]. Changsha:National University of Defense Technology,2006.(in Chinese)

[20] 高偉,劉亞龍,徐博,等.基于雙主交替領航的多AUV協同導航方法[J].哈爾濱工程大學學報,2014,35(3):1-6.

GAO Wei,LIU Ya-long,XU Bo,et al.AUVs cooperative navigation based on two leaders alternately[J].Journal of Harbin Engineering University,2014,35(3):1-6.(in Chinese)

A Cooperative Navigation Approach and Its Verification of USVs with Leader-fellower Structure

XU Bo1,XIAO Yong-ping2,GAO Wei1,LIU Ya-long1,YANG Jian1
(1.Automation Academy,Harbin Engineering University,Harbin 150001,Heilongjiang,China; 2.Shaanxi Space Navigation Equipment Co.Ltd,Baoji 721006,Shaanxi,China)

The cooperative navigation and localization technology of multi-USVs is an important way to solve the navigation problem in complex environment.For the cooperative navigation and localization of multi-USVs with leader-follower structure,a state space model of cooperative localization is built,and then a linear model is obtained by expanding the nonlinear model around filter value.An extended Kalman filter is designed to fuse the sensor information to locate the slave USV in real time.At last,the relationship among USV distances,angle and accuracy is researched,and the observability analysis of cooperative navigation filter algorithm is done with Lie algebra.The results show that the navigation accuracy is improved effectively by controlling the variations of distances and angles of leader and slave USVs between two adjacent moments,which is verified by simulation and experiment.

control science and technology;unmanned surface vessel;ranging;cooperative localization; extended Kalman filter

U666

A

1000-1093(2014)11-1836-10

10.3969/j.issn.1000-1093.2014.11.015

2013-12-13

國家博士后基金項目(2012M510083);國家自然科學基金項目(61203225);黑龍江省自然科學基金項目(QC2014C069)

徐博(1982—),男,講師。E-mail:xubocarter@sina.com;

肖永平(1989—),男,工程師。E-mail:1160481487@qq.com