基于改進領(lǐng)航者法的多AUV隊形控制方法研究

李 良，李一平，張岳星，曾俊寶，徐高朋

（1.中國科學(xué)院沈陽自動化研究所 機器人學(xué)國家重點實驗室，遼寧 沈陽 110016；2.中國科學(xué)院機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169；3.遼寧省水下機器人重點實驗室，遼寧 沈陽 110169；4.中國科學(xué)院大學(xué)，北京 100049）

0 引言

自主水下機器人（AUV）是當(dāng)前人類探索海洋的有效工具之一，可用于繪制海底地圖、研究水生動植物生活模式、觀測海洋現(xiàn)象、安裝海底管道和國防應(yīng)用等[1]。隨著水下任務(wù)的日益復(fù)雜，需要通過多AUV協(xié)作的方式來完成單個AUV無法完成的水下任務(wù)。在一些任務(wù)中，多AUV還需要按照固定的隊形編隊運動[2]。

近年來許多學(xué)者對多 AUV編隊控制進行了研究，比較成熟的編隊控制方法有以下幾種：基于虛擬結(jié)構(gòu)法[3]、基于行為法[4]、基于跟隨領(lǐng)航者法[5-7]、基于人工勢場法[8-9]、基于路徑跟隨法[10-12]和基于信息一致性法[13-14]等。文獻[15]對多 AUV編隊方法進行了綜述，其中跟隨領(lǐng)航者法最易于理解，工程應(yīng)用最為廣泛。文獻[16]在跟隨領(lǐng)航者法的基礎(chǔ)上，基于跟隨者的狀態(tài)反饋制定領(lǐng)航者的運動策略，解決了跟隨者出現(xiàn)異常時隊形可能保持的問題。文獻[6]使用文獻[16]中提出的方法，在國內(nèi)較早的基于水聲通信對編隊控制方法展開外場試驗驗證，同時分析了實際應(yīng)用中水聲通信存在的問題和難點，并對此提出了寶貴的解決意見。隨后也有研究人員提出了多種編隊控制方法，但大多處于理論分析和仿真實驗驗證階段，沒有在實際中進行應(yīng)用。

本文在水聲通信的基礎(chǔ)上，對傳統(tǒng)的跟隨領(lǐng)航者法進行改進，提出了一種基于改進跟隨領(lǐng)航者法的編隊控制方法。通過仿真實驗驗證該方法的可行性并確定關(guān)鍵控制參數(shù)，然后在湖上對該方法進行試驗驗證，對編隊控制結(jié)果進行定量描述和詳細(xì)分析，證明該編隊控制方法的實用性和有效性。

1 編隊控制方法

傳統(tǒng)的跟隨領(lǐng)航者法是指多 AUV在三維水下環(huán)境編隊運動時，領(lǐng)航者按照給定的航行路線和速度運動，在此期間通過水聲通信周期性廣播自身位置和速度信息，跟隨者根據(jù)接收到的領(lǐng)航者信息自主調(diào)整航行路線和速度，與領(lǐng)航者保持期望的隊形。

本文研究3臺AUV的編隊控制，將其劃分為1臺領(lǐng)航者和2臺跟隨者。在跟隨領(lǐng)航者法的基礎(chǔ)上進行改進，跟隨者通過跟蹤虛擬目標(biāo)點實現(xiàn)編隊，將對跟隨者的航向和速度控制簡化成單一的速度控制，并采用多級調(diào)控方法對跟隨者的速度進行精準(zhǔn)調(diào)控。建立一套隊形評價標(biāo)準(zhǔn)，對編隊控制的快速性、準(zhǔn)確性和穩(wěn)定性進行定量評價。

1.1 改進的跟隨領(lǐng)航者法

對傳統(tǒng)的跟隨領(lǐng)航者法改進，改進方法中AUV之間通信數(shù)據(jù)量變小，更適應(yīng)通信受限情況下的水聲環(huán)境。同時在通信效果不穩(wěn)定時，AUV仍能按照預(yù)設(shè)軌跡運動，抗擾動能力強，改進方法分為以下2步。

第1步，簡化控制變量。給定AUV的運動深度和航向，AUV在不同深度定深運動，提高水聲通信成功率。AUV沿預(yù)設(shè)航線運動，將對跟隨者的航向和速度控制簡化為只對速度控制。

第2步，精確調(diào)控速度。跟隨者基于與領(lǐng)航者之間的距離誤差反饋，調(diào)整速度以形成和保持隊形。根據(jù)距離誤差的大小，設(shè)置多級調(diào)控模式，提高隊形形成的快速性和隊形保持的穩(wěn)定性。

某一時刻領(lǐng)航者和跟隨者的位置關(guān)系如圖1所示，其中OA(xa,ya)代表領(lǐng)航者的真實位置和發(fā)送給跟隨者的位置，OB(xB,yB)和OC(xC,yC)代表2個跟隨者的位置，O1和O2為跟隨者的虛擬目標(biāo)點，ΔOAO1O2為期望的編隊隊形。對其中一臺跟隨者B的速度控制方法進行數(shù)學(xué)說明。將整個平面劃分成區(qū)域Ⅰ和Ⅱ2部分，分別表示跟隨者在領(lǐng)航者的后方和前方2種情況。

圖1 領(lǐng)航者和跟隨者位置關(guān)系Fig.1 Position relation between leader and followers

式中：1α為射線與aγ之間的夾角，根據(jù)1α的大小，確定跟隨者所處的區(qū)域。

式中：λ代表跟隨者所處區(qū)域，λ=1表示處于區(qū)域Ⅰ，λ=2表示處于區(qū)域Ⅱ。令

式中：L為跟隨者與領(lǐng)航者之間的期望距離；e為跟隨者與領(lǐng)航者之間的距離誤差。

根據(jù)公式（2）–（3），提出 4種跟隨者 B相對與虛擬目標(biāo)點O1的位置狀態(tài)：正常（N），激進（A），落后（L）和脫離（B）。根據(jù)位置狀態(tài)，制定跟隨者速度控制策略，如表1所示。

表1 跟隨者速度控制策略Tab.1 Speed control strategy of the followers

表中R≥0，表示允許的距離誤差；vb和va為跟隨者和領(lǐng)航者的速度；k1和k2為比例系數(shù)且k2＞k1＞ 0；vmin和vmax表示設(shè)定的AUV最小和最大航行速度。跟隨者在正常狀態(tài)下，緩慢調(diào)整速度，維持隊形。在激進或者落后狀態(tài)下，快速調(diào)整速度，形成隊形。在脫離狀態(tài)下，需要以最小速度航行等待領(lǐng)航者，由脫離狀態(tài)變成激進狀態(tài)后快速形成隊形。

1.2 隊形評價指標(biāo)

為定量評價編隊控制效果，從隊形形成的快速性、準(zhǔn)確性和隊形保持的穩(wěn)定性3個方面定義隊形評價指標(biāo)。

定義1：隊形誤差。

式中：ei為其中 2臺 AUV的距離誤差；Di為 2臺AUV之間的歐氏距離；Li為2臺AUV的期望距離；E為隊形誤差，其大小可以代表編隊控制的精度。

定義2：隊形形成時間。

式中：t'為首次形成隊形的時間，其大小可以描述編隊控制的快速性。

定義3：隊形保持時間。

設(shè)AUV誤差采樣周期為Terror，誤差采樣點集合為E={E1,…En}，若?Ej，j=1,…n，?ei，都有ei≤R，i=1,2,3，則稱在Ej采樣點時刻保持了隊形。

式中：Nhold為Ej的總數(shù)；Ttotal為隊形保持時間，其大小可以反映編隊控制的穩(wěn)定性。

2 仿真驗證

對編隊控制方法進行仿真驗證，同時確定控制方法中參數(shù)k1和k2取值。在MATLAB上進行仿真實驗，3臺AUV分別從坐標(biāo)點(0,15)、(0,0)和(0,-15)（領(lǐng)航者位于中間）沿X軸方向運動約500 m，領(lǐng)航者的期望速度va=1.2 m/s，通信周期T=5s，AUV之間期望距離L1=L2=L3=30 m，整體為正三角形隊形。通信成功率C=80%，其中C的定義為

式中：Nr為跟隨者接收到領(lǐng)航者信息的總包數(shù)；Ns代表領(lǐng)航者發(fā)送信息的總包數(shù)；C越大，代表通信效果越好。

利用遺傳算法對參數(shù)k1和k2進行優(yōu)化，過程如下。

Step1：編碼。給定k1和k2的取值范圍均為（0，1]，采用二進制編碼方式，k1和k2分別對應(yīng) 10條染色體的前5位和后5位。

Step2：初始化種群。隨機產(chǎn)生4個10位二進制個體。

Step3：解碼。對種群個體解碼，得到4組k1和k2的值。

計算每個個體適應(yīng)度值。

Step5：選擇。采用最佳保留選擇算子，即將適應(yīng)度最高的個體完整的復(fù)制到下一代群體中，其余個體按照輪盤賭選擇方法執(zhí)行選擇操作。

Step6：交叉。采用隨機配對方法，對每組個體進行單點交叉，交叉概率Pc=0.5。

Step7：變異。采用均勻變異方法，變異概率Pm=0.005。

Step8：進化終止條件。迭代次數(shù)N=50時停止，否則執(zhí)行Step3。

優(yōu)化后的結(jié)果如圖2所示，隨著迭代次數(shù)的增加，最優(yōu)適應(yīng)度越來越大。將最后一步迭代得到的最優(yōu)個體解碼得到參數(shù)k1=0.04，k2=0.18。當(dāng)取上述參數(shù)時，3臺AUV的運動軌跡如圖3所示，編隊效果很好。

圖2 最優(yōu)適應(yīng)度值的迭代曲線Fig.2 Iteration curves of the optimal fitness value

圖3 仿真運動軌跡Fig.3 Trajectory of AUVs in the simulation test

3 外場試驗

為驗證本文編隊控制方法的實用性，使用 3臺“探索100”AUV[17]進行了水下編隊試驗，如圖4所示。編隊試驗中期望隊形為正三角形，即L1=L2=L3。領(lǐng)航者保持在隊伍的前方，跟隨者在后方兩側(cè)保持跟隨，試驗方案如下：3臺AUV定深航行，預(yù)定航線為3條間距約15 m的由西向東平行直線，航行距離約500 m。試驗中，取R=5 m，vmin=0.5 m/s，vmax=1.9 m/s，Terror=1s。一共進行了多次編隊試驗，每次試驗結(jié)果差別不大，由于本文篇幅有限，任取其中一次實驗數(shù)據(jù)具體分析，如圖5–9。

圖4 試驗中的AUVFig.4 AUVs in the field test

圖5 航行軌跡Fig.5 Trajectory of AUVs

圖5顯示了多AUV的航行軌跡，可以看出正三角形隊形逐漸形成并趨于穩(wěn)定。圖6為多AUV航行速度變化，可以看出跟隨者1和跟隨者2不斷調(diào)整自身速度以保持期望隊形。圖7為多AUV航向信息，航向角基本與預(yù)設(shè)值相同，證明改進跟隨領(lǐng)航者法中簡化控制變量是簡單有效的。圖8為深度變化圖，在垂直面上3臺AUV分別在不同深度定深運動，且深度控制穩(wěn)定，有利于減小對水平面上航向和速度控制的擾動。圖9為隊形誤差圖，可以看出三角形每條邊的誤差和總誤差不斷收斂，隊形形成速度快，控制精度高，期望隊形不斷趨于穩(wěn)定。

圖6 航行速度信息Fig.6 Speed of AUVs

圖7 航向角信息Fig.7 Heading of AUVs

圖8 深度信息Fig.8 Depth of AUVs

圖9 隊形誤差Fig.9 Formation errors

4 結(jié)束語

在水聲通信的基礎(chǔ)上，對跟隨領(lǐng)航者編隊控制方法進行改進，在MATLAB上對該方法進行仿真驗證并確定關(guān)鍵控制參數(shù)，最后在湖上進行外場試驗驗證。試驗結(jié)果表明：該方法在隊形形成的快速性、維持隊形的穩(wěn)定性和隊形控制的精確性等方面表現(xiàn)得很好。

AUV導(dǎo)航精度對編隊效果影響很大，本文中采用電子羅盤+多普勒計程儀的低成本組合導(dǎo)航方法，短距離內(nèi)導(dǎo)航誤差較小。未來將對AUV導(dǎo)航方式進行升級，擬采用慣導(dǎo)+多普勒計程儀的高精度導(dǎo)航方法，提高編隊精度。

本文試驗環(huán)境中水聲通信效果較好，下一步計劃在水聲通信效果較差甚至無法通信的環(huán)境中，對本文提出的方法進行改進，實現(xiàn)編隊控制，最大限度的解決通信問題對編隊控制的影響。