基于IAPF算法的無人艇動態(tài)編隊自主路徑規(guī)劃*

劉夢佳 馮 輝 徐海祥

(武漢理工大學(xué)交通學(xué)院1) 武漢 430063) (高性能船舶技術(shù)教育部重點實驗室2) 武漢 430063)

0 引 言

多無人艇編隊(unmanned surface vehicle formation,USV Formation)系統(tǒng)已成為無人艇領(lǐng)域的研究熱點.相較于單艇作業(yè)，無人艇編隊具有信息處理能力強、負載能力高、覆蓋范圍大等優(yōu)勢，在應(yīng)急救援、水質(zhì)采樣、氣象監(jiān)測、信息偵查等領(lǐng)域有廣闊的應(yīng)用前景[1].國內(nèi)外對編隊控制進行廣泛的研究，編隊模型控制的主要方法有領(lǐng)航者-跟隨者(leader-follower,L-F)法、基于行為(behavior-based)法、虛擬領(lǐng)航(virtual leader)法和基于圖論法等[2]，每種方法都有著優(yōu)缺點，因此，隊形控制目前還沒有形成統(tǒng)一有效的算法.

L-F法因其簡單性和良好的擴展性成為無人艇編隊控制領(lǐng)域的研究熱點[3-4]，編隊路徑規(guī)劃見圖1.

圖1 編隊路徑規(guī)劃示意圖

對領(lǐng)航艇進行路徑規(guī)劃，根據(jù)領(lǐng)航艇的位置和艏向信息計算出跟隨艇的位置信息，從而完成整體編隊系統(tǒng)的路徑規(guī)劃.Liu等[5-6]采用快速行進法，采用L-F編隊控制，解決了USV編隊路徑規(guī)劃問題.張瑞雷[7]提出一種多機器人動態(tài)協(xié)調(diào)控制算法．建立跟隨機器人對虛擬機器人軌跡跟蹤系統(tǒng)模型.Ali等[8]采用勢場法對L-F編隊系統(tǒng)進行了機器人路徑規(guī)劃實驗，驗證了勢場法的可行性.吳垠[9]采用L-F法對多智能體編隊控制問題進行了研究，設(shè)計了一種能夠?qū)崿F(xiàn)其穩(wěn)定編隊的迭代學(xué)習(xí)控制算法.

人工勢場法(artificial potential field,APF)將障礙物和目標假設(shè)為相互作用的靜電荷進行建模，從而在無人艇周圍形成一個潛在的勢場.人工勢場法的計算簡單和實時性好，但主要缺點是尤其是障礙物的坐標未知情況下，APF算法可能存極小值即在局部存在死鎖的位置，阻礙無人艇到達目標.為了克服這些局限性，Adeli等[10]將工作空間離散化為矩形網(wǎng)格，Zhang[11]針對凸障礙或凹障礙環(huán)境提出了一種改進的隨墻方法，葉錢波[12]利用松弛變量將約束優(yōu)化問題轉(zhuǎn)化為無約束優(yōu)化問題.

為適用于無人艇的動態(tài)運動，本文在對已有的方法進行深入分析的基礎(chǔ)上，對傳統(tǒng)的人工勢場法改進得到IAPF(Improved APF)算法，并且在國內(nèi)外研究的基礎(chǔ)上，本文針對無人艇編隊通過狹窄水域的問題，提出自適應(yīng)編隊控制策略，自適應(yīng)調(diào)整隊形.

1 無人艇編隊模型

1.1 單艇運動模型

對于無人艇的路徑規(guī)劃問題，只研究三個自由度的平面運動，即縱蕩、橫蕩和首搖運動.單艇三自由度運動模型見圖2.

圖2 單艇三自由度運動模型ψ-首向角；u-船舶縱向線速度；v-橫向運動線速度；r-角速度；V-無人艇運動速度；G-重心.

1.2 編隊運動模型

采用L-F結(jié)構(gòu)作為編隊運動控制模型，無人艇編隊運動模型示意圖見圖3.采用L-F編隊結(jié)構(gòu)進行計算時需要參考至少三條艇之間的相對距離和相對方位信息.假設(shè)以三條艇的編隊控制為例.三條艇之間的關(guān)系為，一條領(lǐng)航艇和兩條跟隨艇，這種方法只需要對兩條跟隨艇到領(lǐng)航艇之間的期望和實際距離信息ld和l進行控制，保持艇體間相對距離不變就可以達到控制編隊隊形的目的.

圖3 編隊運動模型L1，L2-跟隨艇USV1和跟隨艇USV2與領(lǐng)航艇USVL之間的相對距離；ψ1，ψ2-兩跟隨艇的首向角

只要將領(lǐng)航艇與跟隨艇之間的實際相對距離L與期望相對距離LR之差ΔL、實際相對角度φ與期望角度φR之差Δφ保持在允許的誤差范圍內(nèi)即可達到隊形控制的目的，滿足約束條件為

ΔL=‖L-LR‖≤δL

(1)

Δφ=‖φ-φR‖≤δφ

(2)

標準的L-F編隊控制結(jié)構(gòu)的隊形穩(wěn)定性高，但不利于動態(tài)調(diào)整編隊中跟隨艇的位置，所以本文提出虛擬跟隨者的編隊控制結(jié)構(gòu)，具體是設(shè)置一艘虛擬跟隨艇USVV，與另一艘跟隨艇USV2保持相同的期望相對距離和期望角度，跟隨艇USV1的位置和角度可以根據(jù)虛擬跟隨艇的位置推算出.虛擬跟隨者的L-F具體示意圖見圖4.

圖4 虛擬跟隨者-編隊運動模型

由圖4可知，無人艇USV1，USV2是USVL的跟隨無人艇，USVV是無人艇USV1需要跟隨的虛擬無人艇，與USVL保持期望距離LV和期望角度φV，根據(jù)USVL的位置和首向角即可得到USVV的位置和首向角.

為達到所需的編隊控制結(jié)構(gòu)，假設(shè)φ1=0°，各跟隨艇的位置計算為

φV=φ2

(3)

VV=VL

(4)

xV=xL-LVsin (ψ+φV)

(5)

yV=yL+LVcos (ψ+φV)

(6)

x2=xL-L2sin (ψ-φ2)

(7)

y2=yL-L2cos (ψ-φ2)

(8)

x1=xV-L1sinψ

(9)

y1=yV-L1cosψ

(10)

2 自適應(yīng)編隊控制策略

2.1 自適應(yīng)調(diào)整跟隨角

在無人艇編隊進行自主避障時，需保證跟隨艇USV1和USV2避免碰撞到障礙物，對兩艇跟隨角進行自適應(yīng)調(diào)整策略，當兩跟隨艇分別距左、右障礙物的距離小于安全距離時，根據(jù)其可通航水域?qū)挾茸赃m應(yīng)調(diào)整兩艇跟隨角以避開障礙物，具體計算為

(11)

(12)

式中：φ1為跟隨艇USV1跟隨角；φ2為跟隨艇USV2跟隨角；B1為跟隨艇USV1距左側(cè)障礙物的距離；B2為跟隨艇USV2距右側(cè)障礙物的距離.

2.2 自適應(yīng)隊形控制

在自適應(yīng)調(diào)整跟隨角的基礎(chǔ)上，針對通航水域較窄的情況提出自適應(yīng)隊形控制，具體策略為將無人艇編隊看作一個整體，在寬闊水域，按照前文提出的虛擬跟隨艇L-F編隊控制策略進行航行，在狹窄水域，根據(jù)水域?qū)挾茸赃m應(yīng)調(diào)整編隊整體寬度，即自適應(yīng)調(diào)整φ，為

(13)

(14)

式中：B為可通航水域?qū)挾?

當可通航水域?qū)挾戎荒芡ㄟ^一艘艇時，即φV=φ2=0°，領(lǐng)航艇與跟隨艇保持一字隊形通過狹窄水域，見圖5.

圖5 自適應(yīng)調(diào)整編隊結(jié)構(gòu)示意圖

3 IAPF算法

3.1 基本原理

APF基本思想是在環(huán)境中的無人艇，對目標位置構(gòu)造引力勢場和靜態(tài)障礙物周圍構(gòu)造斥力勢場.引力吸引無人艇朝目標點方向運動，斥力阻止無人艇向障礙物方向運動，在這兩個勢場力共同作用下使無人艇向目標運動.

定義S空間內(nèi)，無人艇位置r(x1,y1)，目標位置g(x2,y2)，無人艇到靜態(tài)障礙物的最近位姿點oi(x3,y3)，環(huán)境中目標對無人艇產(chǎn)生的引力場Uatt的吸引力Fatt，靜態(tài)障礙物對無人艇產(chǎn)生的斥力場Urep的斥力Frep見圖6.引力場和目標吸引力為

Uatt(ρ(r,g))=ξ‖ρ(r,g)‖m

(15)

Fatt(ρ(r,g))=-Uatt(ρ(r,g))=

mξ‖r(x1,y1)-g(x2,y2)‖m-1ρ(r,g)

(16)

式中：m值為勢場函數(shù)曲線的形狀；ξ為目標勢場系數(shù)；ρ(r,g)為無人艇與目標物距離函數(shù)，ρ(r,g)=‖r(x1,y1)-g(x2,y2)‖；Uatt(ρ(r,g))為Uatt在無人艇位置r(x1,y1)處勢場變化率最大的方向；ρ(r,g)為無人艇到目標的單位矢量.

圖6 二維環(huán)境S中無人艇的引力和斥力

靜態(tài)障礙物對無人艇產(chǎn)生的斥力場及相對應(yīng)的斥力為

(17)

Frep(ρ1(r,o))=-Ureps(ρi(r,o))=

(18)

式中：ρ0為障礙物的斥力影響距離，只有在ρ0的范圍之內(nèi)無人艇才受到障礙物的排斥作用；ρi(r,o)為無人艇與第i個靜態(tài)障礙物的最小距離，ρi(r,o)=‖r(x1,y1)-oi(x3,y3)‖；ρi(r,o)是由oi(x3,y3)指向r(x1,y1)的單位向量.

傳統(tǒng)的人工勢場法勢場參數(shù)取值不易選取，且生成的路徑容易產(chǎn)生局部極小值，局部震蕩.同時未考慮到動態(tài)障礙物的情況.

3.2 改進方法

3.2.1改進引力場

改進后的引力場函數(shù)包含了無人艇與目標物的相對位置、相對速度和相對加速度產(chǎn)生的勢場，改進后的引力場計算公式為

Uatt(q,v,a)=αq(ρ(r,g))m+

αv‖v‖n+αa‖a‖p

(19)

式中：αq，αv，αa和m，n，q為比例系數(shù)，不同的值表示在引力函數(shù)中無人艇和目標點相對位置、速度和加速度信息的權(quán)重.由引力場可推得相應(yīng)的吸引力大小為

Fatt(q,v,a)=-Uatt(q,v,a)=

(20)

Fatt(q)=-?Uatt(q,v,a)/?q=

mαq(ρ(r,g))m-1ρ(r,g)

(21)

Fatt(v)=-?Uatt(q,v,a)/?v=nαv‖v‖n-1v

(22)

Fatt(a)=-?Uatt(q,v,a)/?a=pαa‖a‖p-1a

(23)

勢場引力可表達為

Fatt(q,v,a)=Fattq(q)+Fattv(v)+Fatta(a)

(24)

圖7 無人艇所受勢場引力合力示意圖

3.2.2改進斥力場

(25)

(26)

算法流程見圖8.

4 仿真結(jié)果分析

采用的仿真平臺是MATLAB，仿真是基于二值化圖像且不考慮風、浪、流作用下的無人艇編隊路徑規(guī)劃，仿真圖像像素大小為500×500，為便于分析仿真結(jié)果，表1～2給出了船舶參數(shù)和算法參數(shù).

表1 船舶參數(shù)

表2 算法參數(shù)

為便于對比分析仿真結(jié)果，本文首先對傳統(tǒng)的人工勢場法與改進后的人工勢場法進行算法的性能對比；然后對比應(yīng)用于無人艇編隊的效果，對比虛擬跟隨者L-F與標準的L-F效果；最后驗證提出的自適應(yīng)編隊控制策略，證明其可行性.

4.1 算法性能對比仿真結(jié)果

為對比IAPF算法與標準的APF算法性能，將標準的L-F無人艇編隊應(yīng)用于提出的IAPF算法進行避障仿真，仿真對比結(jié)果見圖9.APF和IAPF算法采用相同的參數(shù)：引力參數(shù)2，斥力參數(shù)30.由圖9仿真結(jié)果可知，改進后的算法IAPF明顯優(yōu)于標準APF算法.

圖9 APF與IAPF算法對比

4.2 改進的控制策略仿真結(jié)果

為驗證本文提出的虛擬跟隨者L-F編隊控制算法的動態(tài)調(diào)整性能，與標準L-F控制結(jié)構(gòu)進行對比仿真，采用提出的IAPF算法，表3給出了編隊仿真參數(shù)，仿真結(jié)果見圖10.由圖10b)可知，增加了虛擬跟隨艇后，跟隨艇1與跟隨艇2之間的縱向距離增大，保證了兩艇之間的安全，這種控制結(jié)構(gòu)有利于實現(xiàn)本文提出的自適應(yīng)編隊控制策略.

圖10 編隊效果對比

L1/mL2/mLV/mφ1φ2φV404040π/6π/60

4.3 自適應(yīng)編隊控制策略仿真結(jié)果

為保證無人艇編隊整體避開靜態(tài)障礙物，并且保證跟隨艇之間的安全，對本文提出的自適應(yīng)編隊控制策略進行仿真驗證，跟隨艇USV1與虛擬跟隨艇USV virtual之間的跟隨角設(shè)為15°，結(jié)果見圖11～12.

圖12 跟隨角對比結(jié)果

由圖11a)可知，跟隨艇1與跟隨艇2均在避障過程中撞到障礙物上，未能安全航行.由圖11b)可知，采用自適應(yīng)調(diào)整跟隨角策略，實現(xiàn)在障礙物附近自動調(diào)整角度以避開障礙物，保證跟隨艇安全航行.

圖13為采用自適應(yīng)調(diào)整編隊寬度的策略以通過限制水域的仿真結(jié)果，由圖13可知，采用自適應(yīng)編隊寬度控制策略后，無人艇編隊可以實現(xiàn)自適應(yīng)調(diào)整跟隨角大小，保證安全通過限制水域.跟隨艇的跟隨角變化情況見圖14，在通過限制水域時，跟隨角明顯根據(jù)通航水域?qū)挾冗M行調(diào)整.

圖13 自適應(yīng)調(diào)整編隊寬度仿真對比結(jié)果

圖14 跟隨角對比結(jié)果

5 結(jié) 束 語

文中采用虛擬跟隨艇建立Leader-Follower無人艇編隊模型，提出自適應(yīng)編隊控制策略，可根據(jù)通航水域?qū)挾群碗x障礙物的距離進行自適應(yīng)調(diào)整跟隨角，實現(xiàn)不同環(huán)境條件下的隊形調(diào)整，保證無人艇編隊的整體安全性.針對編隊路徑規(guī)劃提出IAPF算法，引入實際船舶在水域中的速度、加速度參量，改進了傳統(tǒng)勢場的勢場力.仿真結(jié)果表明，在狹窄水域，無人艇自適應(yīng)編隊控制策略可以實現(xiàn)動態(tài)編隊，實現(xiàn)跟隨艇避開障礙物以及通過狹窄水域，所提出的編隊模型也適用于動態(tài)調(diào)整編隊結(jié)構(gòu).