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

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

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

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

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

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

1 問(wèn)題描述

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

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

圖1 單船運(yùn)動(dòng)模型

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

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

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

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

對(duì)兩邊求導(dǎo)得到兩艘無(wú)人船的相對(duì)運(yùn)動(dòng)方程：

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

兩邊求導(dǎo)得：

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

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

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

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

1.3 卡爾曼濾波器

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

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

系統(tǒng)測(cè)量值為

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

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

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

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

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

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

2 控制器設(shè)計(jì)

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

由式（8）和式（9）的編隊(duì)目標(biāo)，根據(jù)反步法設(shè)計(jì)相應(yīng)的控制器，首先引入兩個(gè)坐標(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。

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

2.2 無(wú)人船位置估計(jì)設(shè)計(jì)

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

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

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

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

速度信息的泰勒展開(kāi)式為

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

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

選擇的測(cè)量值向量為

式中：pmx、pmy為二維測(cè)量值，由GPS 所測(cè)得的差值計(jì)算得來(lái)；vmx、vmy為由GPS 測(cè)得的速度；amx、amy是由IMU 的加速度計(jì)獲得的。

式（11）中的觀測(cè)矩陣H為

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

由式（12）和式（16）得無(wú)人船位置的估計(jì)值，即

3 仿真結(jié)果

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

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

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

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

圖4 期望相對(duì)位置仿真

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

圖5 跟隨無(wú)人船速度仿真

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

圖6 多無(wú)人船編隊(duì)隊(duì)形仿真

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

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

圖7 變換隊(duì)形仿真

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

表1 誤差參數(shù)

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

圖8 GPS 位置誤差

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

圖9 位置估計(jì)后的位置誤差

圖10 引入EKF 后的編隊(duì)航行軌跡

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

圖11 跟隨無(wú)人船航速及偏航角偏差

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

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

圖13 變換隊(duì)形階段位置曲線(xiàn)

4 結(jié)論

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