多無人船編隊控制及其位置估計算法研究

鄧志良，王振凎，沈薇，劉云平

（南京信息工程大學(xué) 自動化學(xué)院，南京 210044）

目前，多無人船編隊控制被廣泛應(yīng)用于水質(zhì)監(jiān)測、氣象監(jiān)測、水產(chǎn)養(yǎng)殖以及軍事等領(lǐng)域[1-3]。與單無人船水面作業(yè)相比，多無人船編隊通過在航行過程中形成動態(tài)網(wǎng)絡(luò)，多艘無人船之間進(jìn)行信息交互，可有效提高作業(yè)效率和系統(tǒng)的容錯性，增強系統(tǒng)防御突擊能力，保證任務(wù)的完成度。

已知的編隊控制方法有領(lǐng)航-跟隨法[4]、基于行為法[5]、虛擬結(jié)構(gòu)法[6]、人工勢場法[7]和基于圖論法[8]等。領(lǐng)航-跟隨法因其結(jié)構(gòu)簡單，理論成熟且易于實現(xiàn)，并有著良好的拓展性，被廣泛應(yīng)用。Wei[9]提出虛擬領(lǐng)航者的概念并將此應(yīng)用到領(lǐng)航跟隨編隊中，利用虛擬領(lǐng)航者消除真實領(lǐng)航者出現(xiàn)故障的隱患，提高了系統(tǒng)的穩(wěn)定性；Wu 等[10]基于領(lǐng)航法，提出通過調(diào)整偏差角減少避障時間，同時縮短編隊需要調(diào)整的距離；Wang 等[11]將領(lǐng)航-跟隨應(yīng)用于欠驅(qū)動水下航行器編隊控制中，提出一種結(jié)合反步法、自適應(yīng)神經(jīng)網(wǎng)絡(luò)和動態(tài)水面控制技術(shù)的滑模編隊控制策略。本文主要針對編隊算法的實際應(yīng)用，選擇領(lǐng)航跟隨法。

編隊導(dǎo)航是編隊控制實際應(yīng)用的核心技術(shù)之一，主要由GPS 獲取的經(jīng)緯度信息確定位置信息。GPS 所測得的數(shù)據(jù)精度相對較低，且在有遮擋的環(huán)境下通常會出現(xiàn)數(shù)據(jù)漂移的現(xiàn)象，無人船中使用的GPS 測量誤差大約在2～ 5 m，定位精度低會導(dǎo)致編隊隊形的不穩(wěn)定。Kwang-Kyo 等[12]研究了基于方向?qū)?zhǔn)和位置估計的單雙積分器在二維平面上的編隊控制，并提出一種基于方向?qū)?zhǔn)和位置估計的編隊控制策略；Tsuyoshi 等[13]融合距離傳感器和無線傳感器的信息，并結(jié)合了擴展卡爾曼濾波器實現(xiàn)機器人編隊控制中的位置估計；王念曾等[14]融合GNSS、UWB 和INS 設(shè)計了基于無人機之間距離信息的相對導(dǎo)航方案，并建立系統(tǒng)狀態(tài)方程及觀測方程，引入無跡卡爾曼濾波器對相對狀態(tài)進(jìn)行估計；Yang 等[15]開發(fā)了一種位置估計器，并基于估計的相對位置提出針對跟隨者的控制律，通過仿真驗證了系統(tǒng)的穩(wěn)定性及有效性。然而，以上研究并未考慮定位精度對無人船編隊隊形控制的影響，且具體的精度要求尚未有明確規(guī)定。

本文針對多無人船編隊隊形控制問題，利用領(lǐng)航-跟隨法建立多無人船編隊控制模型，并設(shè)計多無人船編隊控制器，保證多無人船編隊結(jié)構(gòu)的穩(wěn)定性。同時，針對實際應(yīng)用研究中定位精度低引起的編隊隊形不穩(wěn)定的問題，提出了基于擴展卡爾曼濾波技術(shù)的位置估計算法，通過融合GPS 和IMU 數(shù)據(jù)，進(jìn)一步提高定位精度獲得更為精準(zhǔn)的位置信息，并通過仿真驗證方法的有效性及穩(wěn)定性。

1 問題描述

1.1 無人船系統(tǒng)模型

無人船的運動是在三維空間的復(fù)合運動，由沿3 個坐標(biāo)軸的直線運動和繞著3 個坐標(biāo)軸的旋轉(zhuǎn)運動組成，即沿x軸 的縱蕩和繞x軸 旋轉(zhuǎn)的橫搖；沿y軸的橫蕩和繞y軸旋轉(zhuǎn)的縱搖；沿z軸的垂蕩和繞z軸旋轉(zhuǎn)的艏搖。本文所研究的無人船主要在河道水面航行，可忽略垂蕩、縱搖及橫搖的影響，因此無人船模型被簡化為三自由度的非線性模型，其運動模型見圖1。

圖1 單船運動模型

根據(jù)文獻(xiàn)[16]建立無人船數(shù)學(xué)模型：

本文中的整個編隊行動基于領(lǐng)航者的運動軌跡，通過指定其中一艘無人船為領(lǐng)航者，形成領(lǐng)航跟隨結(jié)構(gòu)，如圖2 所示。領(lǐng)航船與跟隨船的位置分別為(xl,yl)和(xf,yf)，L為兩船之間的相對距離，Lx、Ly分別為兩船之間的橫縱距離，φl、φf分別為領(lǐng)航船和跟隨船的航向。通過控制跟隨船到領(lǐng)航船之間的期望距離Ld和實際距離L，實現(xiàn)船之間的相對距離保持不變，從而實現(xiàn)多無人船的編隊。

圖2 無人船領(lǐng)航-跟隨編隊模型

由圖2 可得領(lǐng)航船與跟隨船之間相對距離為：

對兩邊求導(dǎo)得到兩艘無人船的相對運動方程：

式中：Ld、θd分別為領(lǐng)航船與跟隨船之間的期望距離和期望相對角度。x、y方向上的距離誤差分別為：

兩邊求導(dǎo)得：

基于領(lǐng)航-跟隨法的無人船編隊控制模型為：

式中：τu f、τr f分別為跟隨船的前進(jìn)推力和艏搖力矩。

1.2 編隊控制目標(biāo)

2）通過改變控制輸入τu f和 τr f，使uf→ufd，rf→，即：

1.3 卡爾曼濾波器

卡爾曼濾波是控制領(lǐng)域中一個重要的研究方向，它可以從一組測量值中得到所需的信息，濾過數(shù)值中包含噪聲和偏差等問題。線性方程可以由最小二乘遞推得出，被用于線性系統(tǒng)預(yù)測。

設(shè)一個線性方程表示的離散控制系統(tǒng)為

系統(tǒng)測量值為

式中：X(k)為無人船k時刻的狀態(tài)；Z(k)為觀測值；A為狀態(tài)轉(zhuǎn)移矩陣；H為觀測矩陣；W(k-1)為過程噪聲；V(k)為測量噪聲。

由上一時刻無人船的狀態(tài)預(yù)測當(dāng)前k時刻無人船的狀態(tài)，即過程方程為

由狀態(tài)轉(zhuǎn)移矩陣A和 過程噪聲矩陣Q，更新預(yù)測狀態(tài)X(k|k-1)的協(xié)方差矩陣P，即

由觀測矩陣H、測量噪聲矩陣R和P(k|k-1)計算增益Kg，即

根據(jù)增益Kg、預(yù)測狀態(tài)X(k|k-1)和觀測矩陣H，計算k時刻的最優(yōu)狀態(tài)估計值X(k|k)，即

由增益Kg、觀測矩陣H和P(k|k-1),計算出k時刻的最優(yōu)值P(k|k)，即

2 控制器設(shè)計

2.1 編隊控制器設(shè)計

由式（8）和式（9）的編隊目標(biāo)，根據(jù)反步法設(shè)計相應(yīng)的控制器，首先引入兩個坐標(biāo)轉(zhuǎn)換z1和z2，即：

兩邊求導(dǎo)可得：

則式（7）轉(zhuǎn)化為：

假設(shè)uf、vf為(z1,z2)系統(tǒng)的輸入量，和分別為uf和vf的虛擬量，定義 Lyapunov 函數(shù)V1=z12/2+z22/2，兩邊求導(dǎo)可得

假設(shè)選擇

將式（21）代入式（20）可得

由此可知，(z1,z2)漸近穩(wěn)定。

引入速度誤差變量ue=uf-ufα，兩邊求導(dǎo)可得

假設(shè)選擇τu f

代入式（24）可得

所以，(ue,eφ)是漸近穩(wěn)定的。

定義速度誤差

兩邊求導(dǎo)可得

選擇 τrf為

式中k4為正常數(shù)。將式（30）代入式（29），得-k4ue2≤0。

通過給出式（25）和式（30）的控制輸入 τuf和 τr f，可使無人船之間保持期望的距離和相對角度，從而可按照期望隊形航行，實現(xiàn)編隊。

2.2 無人船位置估計設(shè)計

由1.1 節(jié)的數(shù)學(xué)模型可知，根據(jù)多無人船系統(tǒng)中的每艘無人船的位置信息可實現(xiàn)多無人船編隊。本文采用擴展卡爾曼濾波算法，將GPS 所測得的數(shù)據(jù)與IMU 所測得的數(shù)據(jù)進(jìn)行融合，獲得較為精準(zhǔn)的位置信息，提高了定位的精度，保證了編隊隊形的穩(wěn)定性。

選擇的狀態(tài)量為

式中：px、py為無人船的二維坐標(biāo)；vx、vy為航行線速度；ax、ay為航行加速度。各參數(shù)都在北東地坐標(biāo)系下。

建立過程方程，將所使用的方法進(jìn)行線性化，使其適用于無人船系統(tǒng)。其次，再根據(jù)式（12）和式（16）對其進(jìn)行預(yù)測和更新。當(dāng)前時刻k的位置信息，泰勒展開式為

速度信息的泰勒展開式為

假設(shè)短時間內(nèi)，忽略加速度，則

結(jié)合式（13）～ 式（15），針對式（10）的離散系統(tǒng)設(shè)計的狀態(tài)轉(zhuǎn)移矩陣為

選擇的測量值向量為

式中：pmx、pmy為二維測量值，由GPS 所測得的差值計算得來；vmx、vmy為由GPS 測得的速度；amx、amy是由IMU 的加速度計獲得的。

式（11）中的觀測矩陣H為

結(jié)合式（10）、式（11）、式（31）、式（35）～ 式（37）可得出位置估計的系統(tǒng)方程，即：

由式（12）和式（16）得無人船位置的估計值，即

3 仿真結(jié)果

對設(shè)計的控制器進(jìn)行仿真實驗，實驗中的多無人船系統(tǒng)由3 艘無人船組成，各項參數(shù)[17]如下：

在兩種不同初值的情況下，采用不同的隊形約束和相同的速度約束來驗證編隊算法的可行性。領(lǐng)航無人船的航行軌跡記為xL，兩艘跟隨無人船的航行軌跡分別記為xF1和xF2。使跟隨無人船在船體坐標(biāo)系下與領(lǐng)航無人船的相對位置(lx,ly)=即相對期望距離保持為2 m，領(lǐng)航無人船相對其跟隨無人船的方位分別為 π/6和-π/6。設(shè)置領(lǐng)航無人船從（1,1）起航，偏航角為 π/6，以速度（2,0）勻速直線航行，在航行過程中，航向保持不變，兩艘無人船的起始位置分別為（1,-1）和（-1,3），初始偏航角分別為π/12和0，速度為（1,0）。

3 艘無人船仿真的偏航角變化如圖3 所示，兩艘跟隨無人船的偏航角收斂至 π/6，與領(lǐng)航無人船偏航角一致。領(lǐng)航無人船相對兩艘跟隨無人船的偏航角收斂至 π/6和-π/6，達(dá)到設(shè)定的偏航角。

期望相對位置的仿真結(jié)果如圖4 所示，跟隨無人船在靠近期望距離過程中，通過調(diào)整航速和航向，逐漸達(dá)到期望距離，雖有一定波動，但在可接受范圍內(nèi)，達(dá)到編隊要求。

圖4 期望相對位置仿真

結(jié)合圖3 和圖4 的仿真結(jié)果，可見本文設(shè)計的編隊方法可有效控制兩艘無人船形成穩(wěn)定的l-φ結(jié)構(gòu)的編隊隊形。如圖5 仿真結(jié)果所示，領(lǐng)航無人船以2 m/s 的速度勻速航行，跟隨無人船在保持隊形的同時，速度逐漸收斂至2 m/s，并保持該速度繼續(xù)航行，保證了系統(tǒng)編隊速度的一致性。

圖5 跟隨無人船速度仿真

多無人船編隊系統(tǒng)仿真航行軌跡如圖6 所示。開始三艘無人船并未形成期望距離2 m 的編隊隊形結(jié)構(gòu)，之后通過編隊控制器的不斷調(diào)整，開始保持隊形，逐漸滿足編隊隊形要求。

圖6 多無人船編隊隊形仿真

從無人船的偏航角變化、期望距離、速度變化以及3 艘無人船航行軌跡這幾個方面進(jìn)行分析，從仿真結(jié)果來看，誤差均在接受范圍內(nèi)，可見該編隊方法是可行的。

依舊使用3 艘無人船進(jìn)行編隊，一艘為領(lǐng)航者，其余兩艘為跟隨者，從不同位置按照約束進(jìn)行編隊。最初設(shè)定的期望距離為2 m，角度為 π/4，航行一段時間后，改變領(lǐng)航無人船的航向，將編隊隊形中設(shè)定的期望距離改為5 m，角度改為 π/6，航行一段時間之后再恢復(fù)成初始設(shè)定，仿真結(jié)果如圖7 所示。當(dāng)隊形發(fā)生變化之后，跟隨無人船迅速調(diào)整自身姿態(tài)，形成新的隊形，滿足隊形控制的要求。

圖7 變換隊形仿真

為驗證位置估計算法的可行性，對GPS 和IMU的數(shù)據(jù)融合進(jìn)行仿真實驗。為貼近實際無人船的航行，仿真數(shù)據(jù)中加入了加速度計和陀螺儀的漂移等誤差信息，并分析無人船的位置信息。設(shè)定重力加速度為9.8 m/s,初始經(jīng)緯度信息為（N 32.02,E 118.14）。引入誤差信息如表1 所示。

表1 誤差參數(shù)

圖8 為單獨使用GPS 測得的位置誤差，GPS 的測量誤差不隨時間積累，但誤差范圍偏大，最大達(dá)到了3 m。在多無人船編隊實際應(yīng)用中，難以滿足實際需求。

圖8 GPS 位置誤差

圖9 為將GPS 和IMU 數(shù)據(jù)融合之后再經(jīng)過擴展卡爾曼濾波的位置誤差。短時間內(nèi)誤差較小，隨著時間的增加，誤差存在一定波動。擴展卡爾曼濾波對誤差進(jìn)行了補償，所以誤差范圍較小，緯度誤差在0.6 m左右；經(jīng)度誤差在0.4 m 左右。與GPS 單獨工作相比，定位精度明顯提高，證明了濾波算法的有效性。為驗證多無人船編隊的隊形穩(wěn)定性，對編隊過程中的數(shù)據(jù)進(jìn)行處理，生成如圖10 所示的航行軌跡。從圖中可以看出，編隊軌跡較為平滑，雖有一定的波動，但在可接受范圍之內(nèi)。結(jié)果表明，該方法可有效完成編隊控制，保證隊形結(jié)構(gòu)的穩(wěn)定性。

圖9 位置估計后的位置誤差

圖10 引入EKF 后的編隊航行軌跡

對引入擴展卡爾曼濾波后的多無人船編隊航行數(shù)據(jù)進(jìn)行分析。跟隨無人船速度仿真結(jié)果和偏航角偏差仿真結(jié)果見圖11。跟隨無人船速度逐漸趨近于領(lǐng)航無人船的速度，僅伴隨少許波動，跟隨無人船偏航角偏差逐漸趨近于0，僅在0 附近上下波動。編隊過程中跟隨無人船縱向和橫向的位置數(shù)據(jù)分析見圖12。跟隨無人船的橫向、縱向距離逐漸趨近于期望距離，存在一定波動，但波動都相對穩(wěn)定，誤差大約為0.6 m，一定程度的提高了定位精度。

圖11 跟隨無人船航速及偏航角偏差

圖12 位置數(shù)據(jù)分析

多無人船系統(tǒng)變換隊形階段位置數(shù)據(jù)如圖13所示。圖13a）中，期望距離從5 m 變?yōu)? m 時，跟隨無人船可較快地進(jìn)行調(diào)整以完成隊形的重構(gòu)及保持。圖13b）中，可在變換隊形后迅速重構(gòu)隊形。

圖13 變換隊形階段位置曲線

4 結(jié)論

結(jié)合擴展卡爾曼濾波算法和領(lǐng)航-跟隨法，提出了一種可有效解決編隊隊形不穩(wěn)定問題的編隊控制策略。通過融合GPS 和IMU 兩部分?jǐn)?shù)據(jù)，獲得了更為精準(zhǔn)的位置信息。對多無人船編隊及隊形變換進(jìn)行了MATLAB 仿真實驗，驗證了編隊控制算法和位置估計算法的有效性和穩(wěn)定性。