陳東隅

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

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

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

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

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

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

1.1 基本原理

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

(1)

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

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

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

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

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

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

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

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

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

(2)

(3)

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

(4)

(5)

1.3 模型的線性化及濾波過程

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

(6)

(7)

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

(8)

(9)

(10)

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

(11)

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

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

(12)

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

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

(13)

式中,Pk為從艇運動狀態(tài)的協(xié)方差陣,Qk為系統(tǒng)過程噪聲協(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為量測誤差矩陣,Hk+1為量測方程的雅可比矩陣。

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

(15)

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

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

(16)

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

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

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

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

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

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

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

2 仿真結(jié)果分析

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

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

表1 仿真實驗參數(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)境因素的影響,三艘艇航行的實際軌跡如圖2所示。

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

圖2中,黑色線段表示主艇實際航行軌跡;紅色線段表示從艇A的航行軌跡;藍色線段表示從艇B的航行軌跡。

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

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

這里采用以上改進的協(xié)同導(dǎo)航算法,對于一艘主艇多艘從艇的協(xié)同導(dǎo)航情況,任意選取兩艘從艇之間的測距信息進行融合解算,仿真結(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 改進后從艇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實際的航行軌跡;紅線分別表示航推軌跡;藍線表示傳統(tǒng)的協(xié)同導(dǎo)航軌跡;綠線表示改進后的主從式協(xié)同導(dǎo)航解算的軌跡。圖5b)和圖6b)中,黑線表示從艇A、B的航推誤差;紅線表示傳統(tǒng)的主從式協(xié)同導(dǎo)航算法解算誤差;藍線表示改進后的主從式協(xié)同導(dǎo)航算法解算的誤差。對比可以發(fā)現(xiàn),主從式協(xié)同導(dǎo)航過程中,融合解算了從艇之間的測距信息,相比較傳統(tǒng)的主從式協(xié)同導(dǎo)航方法,從艇A、B位置誤差的發(fā)散速度得到了有效抑制,如表2所示。

3 結(jié)束語

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

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