一種改進(jìn)的主從式協(xié)同導(dǎo)航技術(shù)研究

陳東隅

(江蘇自動(dòng)化研究所,江蘇 連云港 222061)

利用水面無(wú)人艇編隊(duì)參與區(qū)域搜救、反潛等特殊任務(wù)時(shí),需要多艘無(wú)人艇之間相互協(xié)同作業(yè)才能完成任務(wù)。但在復(fù)雜的戰(zhàn)場(chǎng)環(huán)境下,各種針對(duì)衛(wèi)星導(dǎo)航系統(tǒng)的干擾和拒止手段層出不窮,各無(wú)人艇僅依靠自身搭載的慣性組件,其導(dǎo)航定位精度很難滿足協(xié)同作業(yè)的發(fā)展需求,因此,如何讓無(wú)人艇編隊(duì)在衛(wèi)星導(dǎo)航信號(hào)拒止環(huán)境下長(zhǎng)時(shí)間保持良好的導(dǎo)航定位精度,是遂行相關(guān)任務(wù)的關(guān)鍵[1-2]。

近年來(lái),國(guó)內(nèi)外研究人員對(duì)于陸上機(jī)器人、水下自主潛航器和無(wú)人機(jī)的協(xié)同導(dǎo)航定位技術(shù)研究較為深入[3-5]。美國(guó)明尼蘇達(dá)大學(xué)的S.I.Roumeliotis等人對(duì)機(jī)器人協(xié)同導(dǎo)航進(jìn)行了研究,提出了一種新穎的分散式協(xié)同導(dǎo)航算法,使用基于卡爾曼濾波的算法來(lái)實(shí)現(xiàn)協(xié)同定位[6];2016年,歐洲多家科研單位在歐盟的資金支持下開展了名為MORPH的研究項(xiàng)目,該項(xiàng)目對(duì)多個(gè)AUV進(jìn)行協(xié)調(diào)作業(yè)時(shí)的系統(tǒng)結(jié)構(gòu)和通信問(wèn)題進(jìn)行了研究,并完成了基于多AUV系統(tǒng)的協(xié)調(diào)控制和協(xié)同定位等多項(xiàng)技術(shù)的試驗(yàn)驗(yàn)證[7]。

目前,對(duì)于水面無(wú)人艇編隊(duì)的協(xié)同導(dǎo)航研究尚處于起步階段,其中,主從式協(xié)同導(dǎo)航技術(shù)是該領(lǐng)域的重要研究方向[8],其分為單主艇和多主艇兩種技術(shù)途徑。無(wú)人艇可通過(guò)自身搭載的光電、雷達(dá)、水聲或無(wú)線電測(cè)距等設(shè)備獲取相對(duì)其他無(wú)人艇的距離信息和方位信息等,再利用無(wú)線通信交換各自的運(yùn)動(dòng)狀態(tài)信息進(jìn)行協(xié)同導(dǎo)航定位,從而提高自身的導(dǎo)航定位精度[9]。

本文以一艘搭載高精度組合導(dǎo)航設(shè)備或能夠正常獲取衛(wèi)星導(dǎo)航信號(hào)的無(wú)人艇作為主艇,其他搭載廉價(jià)的低精度慣導(dǎo)組件的無(wú)人艇為從艇,組成了單主多從的無(wú)人艇編隊(duì)。其中,主艇的高精度定位信息在無(wú)人艇編隊(duì)中可以實(shí)時(shí)共享。在傳統(tǒng)的單主多從協(xié)同導(dǎo)航系統(tǒng)中,各從艇僅與主艇建立關(guān)系,獨(dú)立解算自身的位置信息。本文將任意兩艘從艇與主艇組成了改進(jìn)的主從式協(xié)同導(dǎo)航系統(tǒng),通過(guò)測(cè)量并共享三者間的距離改正從艇的位置,最終提高編隊(duì)中從艇的導(dǎo)航定位精度。該方法避免了在每艘無(wú)人艇上都安裝價(jià)格昂貴的高精度導(dǎo)航設(shè)備,減小了無(wú)人艇編隊(duì)的成本,且相比傳統(tǒng)方法可進(jìn)一步抑制誤差累積速度,提升導(dǎo)航定位精度。

1 改進(jìn)的主從式協(xié)同導(dǎo)航算法

1.1 基本原理

無(wú)人艇編隊(duì)在衛(wèi)星導(dǎo)航信號(hào)拒止環(huán)境下進(jìn)行航位推算時(shí),由于受慣導(dǎo)組件誤差特性影響,導(dǎo)航定位時(shí)會(huì)隨著時(shí)間推移產(chǎn)生較大的累積誤差。本文中的無(wú)人艇編隊(duì)包含1艘主艇和2艘從艇,從艇均處于衛(wèi)星導(dǎo)航信號(hào)拒止區(qū)域。無(wú)人艇編隊(duì)的運(yùn)動(dòng)過(guò)程可看作二維平面運(yùn)動(dòng),可忽略高程因素,分別將主艇Μ和從艇Α、Β的位置記為PM=(xM,yM)、PA=(xA,yA)和PB=(xB,yB)。本文認(rèn)為主艇的位置是包含高斯噪聲的準(zhǔn)確位置。而從艇在衛(wèi)星導(dǎo)航信號(hào)拒止環(huán)境下,僅依靠自身搭載的低精度慣性導(dǎo)航組件進(jìn)行航跡推算,則從艇A和B的航跡推算模型可表示為

(1)

傳統(tǒng)的主從式協(xié)同導(dǎo)航方法中,從艇通過(guò)無(wú)線網(wǎng)絡(luò)實(shí)時(shí)獲取主艇的精確位置,再利用測(cè)距設(shè)備獲取與主艇之間的相對(duì)距離,最終通過(guò)與本艇運(yùn)動(dòng)狀態(tài)信息的融合濾波計(jì)算出該時(shí)刻的位置,提高了從艇導(dǎo)航定位精度。

改進(jìn)的主從式協(xié)同導(dǎo)航算法在此基礎(chǔ)上,引入從艇之間的測(cè)距信息作為一種誤差約束條件,使得基于EKF的主從式協(xié)同導(dǎo)航技術(shù)在一主多從的應(yīng)用場(chǎng)景中,導(dǎo)航定位精度得到進(jìn)一步提高。

改進(jìn)的主從式協(xié)同導(dǎo)航定位原理如圖1所示。

圖1 改進(jìn)的主從式協(xié)同導(dǎo)航原理Fig.1 Principle of the improved leader-follower cooperative navigation

主艇在k時(shí)刻的運(yùn)動(dòng)狀態(tài)(包括位置、速度、加速度、航向角等)以及與從艇之間的相對(duì)距離可實(shí)時(shí)廣播至各從艇,將主艇與從艇、從艇與從艇間測(cè)距信息以及各艇的運(yùn)動(dòng)狀態(tài)進(jìn)行融合解算,即構(gòu)建相應(yīng)的運(yùn)動(dòng)方程和量測(cè)方程進(jìn)行擴(kuò)展卡爾曼濾波,可有效抑制從艇的航跡誤差隨時(shí)間增加的速率,提升從艇的導(dǎo)航定位精度[10]。改進(jìn)的算法中增加了從艇間的測(cè)距信息,且該信息較為精準(zhǔn),因此,在融合計(jì)算時(shí),可以將兩艘從艇的位置誤差約束在一定范圍以內(nèi)。

圖1中虛線圓圈表示從艇航跡推算誤差,斜紋圓圈表示采用傳統(tǒng)主從式協(xié)同導(dǎo)航后的航跡誤差,深灰色圓圈表示增加了從艇間距離觀測(cè)量后進(jìn)行濾波計(jì)算的誤差?？梢园l(fā)現(xiàn),隨著已知條件的增加,從艇的導(dǎo)航定位精度改善較為明顯。

1.2 數(shù)學(xué)模型

本文針對(duì)1艘主艇和2艘從艇組成的協(xié)同導(dǎo)航系統(tǒng)建立離散非線性系統(tǒng)狀態(tài)方程如下:

Xk+1=f(Xk,μk,υk)=Xk+Γ(μk+υk)

(2)

(3)

在此應(yīng)用場(chǎng)景下,主艇和從艇以及從艇和從艇之間的距離觀測(cè)量組成的量測(cè)方程Zk可表示為

(4)

(5)

1.3 模型的線性化及濾波過(guò)程

由式(2)和式(4)可知,系統(tǒng)的狀態(tài)方程和量測(cè)方程均包含非線性項(xiàng),卡爾曼濾波(KF)一般只針對(duì)線性系統(tǒng),因此,本文采用擴(kuò)展卡爾曼濾波(EKF)對(duì)以上系統(tǒng)進(jìn)行濾波[11]。根據(jù)EKF算法,首先分別圍繞狀態(tài)向量Xk和控制向量μk將系統(tǒng)的狀態(tài)方程非線性部分進(jìn)行泰勒級(jí)數(shù)展開:

(6)

(7)

將狀態(tài)方程的泰勒級(jí)數(shù)展開式略去二階以上的高階項(xiàng),則f(Xk,μk,υk)關(guān)于狀態(tài)向量Xk和控制向量μk的雅可比矩陣(Jacobian)分別為:

(8)

(9)

(10)

將量測(cè)方程的泰勒級(jí)數(shù)展開式略去二階以上的高階項(xiàng),則量測(cè)方程關(guān)于狀態(tài)向量Xk的雅可比矩陣H可表示為

(11)

對(duì)系統(tǒng)線性化后的運(yùn)動(dòng)模型應(yīng)用卡爾曼濾波基本方程可得到擴(kuò)展卡爾曼濾波遞推方程,其具體步驟如下:

1)系統(tǒng)狀態(tài)一步預(yù)測(cè)

(12)

2)系統(tǒng)一步預(yù)測(cè)的協(xié)方差矩陣

Pk+1,k=FkPk(Fk)T+UkQk(Uk)T

(13)

式中,Pk為從艇運(yùn)動(dòng)狀態(tài)的協(xié)方差陣,Qk為系統(tǒng)過(guò)程噪聲協(xié)方差陣,Fk和Uk分別為系統(tǒng)狀態(tài)方程的雅可比矩陣。

3)濾波增益

Kk+1=Pk+1,k(Hk+1)T(Hk+1Pk+1,k(HK+1)T+Rk+1)-1

(14)

式中,Rk+1為量測(cè)誤差矩陣,Hk+1為量測(cè)方程的雅可比矩陣。

4)系統(tǒng)狀態(tài)估計(jì)

(15)

5)系統(tǒng)狀態(tài)估計(jì)誤差的協(xié)方差陣更新

Pk+1=Pk+1,k-Kk+1Hk+1Pk+1,k

(16)

以上就是擴(kuò)展卡爾曼濾波器的一個(gè)計(jì)算周期,各個(gè)時(shí)刻EKF對(duì)主從式協(xié)同導(dǎo)航系統(tǒng)的處理就是這個(gè)周期不斷重復(fù)的過(guò)程。

1.4 系統(tǒng)誤差源分析

改進(jìn)的主從式協(xié)同導(dǎo)航系統(tǒng)的誤差主要來(lái)源于以下幾個(gè)方面:

1)主艇和從艇本身的運(yùn)動(dòng)狀態(tài)誤差,即主從艇本身的航行位置、速度、加速度和航向角等產(chǎn)生的誤差。其中,主艇的位置誤差包含艇載的高精度導(dǎo)航定位設(shè)備誤差(可優(yōu)于米級(jí))以及無(wú)人艇受航行控制系統(tǒng)和水文條件等因素影響產(chǎn)生的航行控制誤差[12]。

2)主從艇之間的測(cè)距誤差,即主艇與從艇間的測(cè)距以及從艇與從艇間的測(cè)距誤差。目前,艇間距離測(cè)量可采用激光測(cè)距、水聲測(cè)距或者雷達(dá)測(cè)距等手段完成,但隨著艇間距離逐漸增大,測(cè)距誤差也逐步增大[13]。

3)信息傳輸時(shí)延誤差。由于各觀測(cè)量均通過(guò)無(wú)線網(wǎng)絡(luò)傳輸并進(jìn)行計(jì)算,在傳輸過(guò)程中存在一定的網(wǎng)絡(luò)時(shí)延,解算的定位結(jié)果與實(shí)際位置存在誤差。本文假設(shè)所有數(shù)據(jù)都是實(shí)時(shí)獲取和處理的,暫不討論信息傳輸時(shí)延產(chǎn)生的誤差。

4)數(shù)學(xué)模型誤差。由于系統(tǒng)狀態(tài)方程和量測(cè)方程在線性化過(guò)程中省略了高階項(xiàng),從而產(chǎn)生了計(jì)算誤差。本文僅討論主從艇以固定航向進(jìn)行的勻速直線運(yùn)動(dòng),相應(yīng)的運(yùn)動(dòng)方程不屬于高度非線性系統(tǒng),因此,在線性化過(guò)程中,略去的高階項(xiàng)產(chǎn)生的系統(tǒng)誤差可以忽略。

2 仿真結(jié)果分析

為驗(yàn)證改進(jìn)的主從式協(xié)同導(dǎo)航算法相比于傳統(tǒng)方法對(duì)從艇導(dǎo)航定位誤差的抑制效果,本節(jié)進(jìn)行了仿真計(jì)算。

1艘主艇和2艘從艇在編隊(duì)航行時(shí),進(jìn)入衛(wèi)星導(dǎo)航信號(hào)拒止區(qū)域,主艇和從艇相互之間保持一定的距離,且主從艇之間具備無(wú)線網(wǎng)絡(luò)通信及相互觀測(cè)的條件。設(shè)定主艇和從艇的仿真條件如表1所示。

表1 仿真實(shí)驗(yàn)參數(shù)Tab. 1 Parameters of the simulation experiment

主艇和從艇A、B按照表1中設(shè)定的參數(shù)航行約2 000 s,其中,主艇航速約10 kn,從艇A航速約6 kn,從艇B航速約14 kn,不考慮水流、風(fēng)速等環(huán)境因素的影響,三艘艇航行的實(shí)際軌跡如圖2所示。

圖2 主艇和從艇A、B真實(shí)航行軌跡圖Fig.2 Real route of leader boat and follower boats

圖2中,黑色線段表示主艇實(shí)際航行軌跡;紅色線段表示從艇A的航行軌跡;藍(lán)色線段表示從艇B的航行軌跡。

現(xiàn)分別對(duì)從艇A和從艇B采用傳統(tǒng)的主從式協(xié)同導(dǎo)航方法解算各自的航行軌跡,如圖3和圖4所示。

圖3a)和圖4a)中,黑線分別表示從艇A、B的實(shí)際航行軌跡;紅線分別表示航跡推算軌跡;藍(lán)線分別表示采用傳統(tǒng)的主從式協(xié)同導(dǎo)航算法,從艇A、B的航行軌跡。圖3b)和圖4b)中,黑色線段表示從艇A、B的航跡推算誤差,紅色線段表示協(xié)同導(dǎo)航后的航跡誤差?？梢钥闯?傳統(tǒng)的主從式協(xié)同導(dǎo)航技術(shù)可在一定程度上抑制從艇的導(dǎo)航定位誤差。

這里采用以上改進(jìn)的協(xié)同導(dǎo)航算法,對(duì)于一艘主艇多艘從艇的協(xié)同導(dǎo)航情況,任意選取兩艘從艇之間的測(cè)距信息進(jìn)行融合解算,仿真結(jié)果如圖5和圖6所示。

圖3 從艇A的協(xié)同導(dǎo)航定位結(jié)果Fig.3 The result of cooperative navigation for follower A

圖4 從艇B的協(xié)同導(dǎo)航定位結(jié)果Fig.4 The result of cooperative navigation for follower B

圖5 改進(jìn)后從艇A的協(xié)同導(dǎo)航定位結(jié)果Fig.5 The result of improved cooperative navigation for follower A

圖6 融合解算后,從艇B的協(xié)同導(dǎo)航定位結(jié)果Fig.6 The result of improved cooperative navigation for follower B

圖5a)和圖6a)中,黑線分別表示從艇A、B實(shí)際的航行軌跡;紅線分別表示航推軌跡;藍(lán)線表示傳統(tǒng)的協(xié)同導(dǎo)航軌跡;綠線表示改進(jìn)后的主從式協(xié)同導(dǎo)航解算的軌跡。圖5b)和圖6b)中,黑線表示從艇A、B的航推誤差;紅線表示傳統(tǒng)的主從式協(xié)同導(dǎo)航算法解算誤差;藍(lán)線表示改進(jìn)后的主從式協(xié)同導(dǎo)航算法解算的誤差。對(duì)比可以發(fā)現(xiàn),主從式協(xié)同導(dǎo)航過(guò)程中,融合解算了從艇之間的測(cè)距信息,相比較傳統(tǒng)的主從式協(xié)同導(dǎo)航方法,從艇A、B位置誤差的發(fā)散速度得到了有效抑制,如表2所示。

3 結(jié)束語(yǔ)

本文針對(duì)無(wú)人艇編隊(duì)的協(xié)同導(dǎo)航問(wèn)題,基于傳統(tǒng)的主從式協(xié)同導(dǎo)航算法,提出了一種增加從艇間距離觀測(cè)信息的改進(jìn)算法,并通過(guò)了仿真試驗(yàn)驗(yàn)證。由仿真結(jié)果可知,在一艘主艇兩艘從艇的情況下,將從艇間的測(cè)距信息作為量測(cè)值進(jìn)行融合解算比傳統(tǒng)單主艇與單從艇的協(xié)同導(dǎo)航定位結(jié)果對(duì)誤差的抑制更有效。

表2 傳統(tǒng)算法與改進(jìn)后算法的誤差對(duì)比表