胡陽修，賀 亮，趙長春，陳 揚(yáng)，錢洲元

(1.上海航天控制技術(shù)研究所·上?！?01109；2.上海市空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室·上?！?01109)

0 引 言

采用集群的方式可以拓展單體無人機(jī)的能力，以高效、合作、協(xié)同地完成任務(wù)。在無人機(jī)之間實(shí)現(xiàn)相互的功能備份，可以避免由單一無人機(jī)失效而造成的任務(wù)失敗[1-2]。目前，集群無人機(jī)協(xié)同編隊(duì)控制方法主要通過位置與速度控制保持相對(duì)隊(duì)形[3]。常用的方法有：基于領(lǐng)航-跟隨、基于行為、基于虛擬結(jié)構(gòu)等。基于領(lǐng)航-跟隨的方法有著結(jié)構(gòu)簡單、對(duì)通信容量需求少的優(yōu)點(diǎn)，在工程實(shí)踐中的應(yīng)用較多，國內(nèi)外學(xué)者在此基礎(chǔ)上進(jìn)行了大量的改進(jìn)研究[4]。

領(lǐng)航無人機(jī)通常需要完成全局路徑規(guī)劃，A*算法主要采用了路徑搜索算法[5]。文獻(xiàn)[6]通過改進(jìn)A*算法開閉表中的元素存儲(chǔ)方式，提升了路徑規(guī)劃效率，但并沒有提升路徑規(guī)劃的效果。文獻(xiàn)[7]將A*算法與Dubins曲線進(jìn)行了結(jié)合，在起止段采用Dubins曲線，中間采用A*算法，滿足了長度最短與安全性的要求。但是，安全性改進(jìn)只在起點(diǎn)和終點(diǎn)有效，沒有考慮全過程的障礙威脅。

跟隨無人機(jī)通常根據(jù)領(lǐng)航無人機(jī)的距離與方位信息完成編隊(duì)跟隨。通過將路徑信息進(jìn)行參數(shù)化，利用數(shù)據(jù)鏈在集群中同步，可以大大提升路徑跟蹤效果[8]。文獻(xiàn)[9]建立了一種基于路徑參數(shù)協(xié)同的多移動(dòng)機(jī)器人編隊(duì)控制模型，通過建立統(tǒng)一的路徑參數(shù)，實(shí)現(xiàn)了對(duì)編隊(duì)路徑的協(xié)同，但其所選擇的路徑參數(shù)對(duì)于不規(guī)則曲線而言不具備較好的普適性。

集群無人機(jī)之間的隊(duì)形協(xié)同控制通常采用一致性原理。通過選取無人機(jī)的狀態(tài)信息，設(shè)計(jì)控制率保持一致[10]。避障是集群系統(tǒng)必須考慮的問題，文獻(xiàn)[11]針對(duì)無人機(jī)編隊(duì)的機(jī)間防碰撞需求，基于人工勢(shì)場改進(jìn)防碰撞控制方法，構(gòu)建了受兩者相對(duì)運(yùn)動(dòng)速度影響的輔助斥力勢(shì)，并給出了機(jī)間防碰撞、障礙物規(guī)避的總速度場，但沒有考慮動(dòng)態(tài)障礙下的避障優(yōu)化問題。

綜上所述，針對(duì)上述研究中存在的問題，本文在領(lǐng)航-跟隨法的基礎(chǔ)上通過對(duì)各障礙物引入威脅系數(shù)而改進(jìn)A*算法，在A*最短路徑的基礎(chǔ)上考慮不同障礙的安全威脅，提高集群的生存率。在利用改進(jìn)A*算法完成領(lǐng)航無人機(jī)全局路徑規(guī)劃后，采用Hermite多項(xiàng)式完成了全局路徑的參數(shù)化表示。Hermite多項(xiàng)式在路徑點(diǎn)多階連續(xù)的特性，可以降低對(duì)無人機(jī)動(dòng)力系統(tǒng)的沖擊。最后，通過帶有動(dòng)態(tài)避障功能的編隊(duì)協(xié)同控制器，實(shí)現(xiàn)無人機(jī)編隊(duì)隊(duì)形的協(xié)同控制。

1 問題描述

1.1 無人機(jī)運(yùn)動(dòng)模型

無人機(jī)主要包括多旋翼與固定翼兩類，兩者的運(yùn)動(dòng)學(xué)模型并不完全一致。為了使異構(gòu)無人機(jī)也能進(jìn)行組合并完成協(xié)同編隊(duì)任務(wù)，需將無人機(jī)統(tǒng)一視作二階積分系統(tǒng)[12]

(1)

式中，xi,vi∈Rm為無人機(jī)的狀態(tài)量，ui為控制輸入量，i=1,2,…,k為積分系統(tǒng)數(shù)量?？紤]水平面的運(yùn)動(dòng)，假設(shè)無人機(jī)作勻速運(yùn)動(dòng)，其運(yùn)動(dòng)學(xué)模型與運(yùn)動(dòng)約束為

(2)

式中，(x,y)為無人機(jī)在NED北東地地理坐標(biāo)系下的位置，BODY為機(jī)體坐標(biāo)系，v、a和λ分別為無人機(jī)速度、加速度和速度角，vmax、amax分別為最大速度和最大加速度。各物理量的示意圖如圖1所示。

圖1 無人機(jī)運(yùn)動(dòng)模型示意圖Fig.1 UAV motion model diagram

1.2 通信拓?fù)浣Y(jié)構(gòu)

使用圖論中的有向圖來描述各無人機(jī)之間的通信拓?fù)浣Y(jié)構(gòu)。領(lǐng)航無人機(jī)負(fù)責(zé)全局路徑的規(guī)劃，將各跟隨無人機(jī)的路徑參數(shù)進(jìn)行全局統(tǒng)一分配。其與跟隨無人機(jī)之間為雙向通信，各跟隨無人機(jī)之間通過互聯(lián)進(jìn)行狀態(tài)信息的同步。無人機(jī)的通信拓?fù)浣Y(jié)構(gòu)如圖2所示。

圖2 無人機(jī)通信拓?fù)浣Y(jié)構(gòu)Fig.2 Communication topology between UAV

根據(jù)圖論知識(shí)[13]，上述通信拓?fù)浣Y(jié)構(gòu)可用有向圖G=(V,E,A)表示。其由頂點(diǎn)集合V={v1,v2,…,vn}、邊集合E和鄰接矩陣A=[aij]∈Rn×n組成。若任意?i,j=1,2,…,n,i≠j，頂點(diǎn)i可直接到達(dá)頂點(diǎn)j，則該邊存在于邊集合E中，可記為(vi,vj)∈E。此時(shí)，鄰接矩陣A中的元素aij=1。否則aij=0，且記作aii=0，即有

(3)

2 全局航跡路徑規(guī)劃

2.1 改進(jìn)A*算法

為了最小化任務(wù)時(shí)間與能量消耗，對(duì)無人機(jī)進(jìn)行預(yù)先的全局航跡路徑規(guī)劃是必要的。航跡路徑規(guī)劃算法的目標(biāo)是基于預(yù)先給定的地圖生成一條最小代價(jià)的路徑。考慮計(jì)算的時(shí)間與空間代價(jià)，利用啟發(fā)式搜索的A*算法可以獲得理論上的時(shí)間最優(yōu)。其代價(jià)函數(shù)如下

f(n)=g(n)+h(n)

(4)

式中，n代表當(dāng)前搜索過程中的節(jié)點(diǎn)，g(n)為沿著規(guī)劃路徑從起點(diǎn)到節(jié)點(diǎn)n的耗費(fèi)值，h(n)為從節(jié)點(diǎn)n到目標(biāo)點(diǎn)的耗費(fèi)估算。g(n)由已產(chǎn)生的路徑確定，h(n)的選取會(huì)直接影響路徑的規(guī)劃。通常使用曼哈頓距離作為h(n)的估計(jì)，如公式(6)所示

(5)

式中，x、y和xd、yd分別是當(dāng)前節(jié)點(diǎn)坐標(biāo)與目標(biāo)節(jié)點(diǎn)坐標(biāo)。由于曼哈頓距離只考慮了當(dāng)前節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的預(yù)測(cè)距離，搜索目標(biāo)為最短路徑，考慮各障礙對(duì)集群任務(wù)的威脅不同，需引入障礙威脅系數(shù)。這里選用了新的預(yù)測(cè)函數(shù)

h′(n)=

(6)

式中，ma為權(quán)重系數(shù)，wi(x,y)為第i個(gè)障礙在(x,y)處的威脅系數(shù)，ri_max是在路徑搜索過程中無人機(jī)與第i個(gè)障礙中心距離的預(yù)設(shè)最大值，ri是無人機(jī)與第i個(gè)障礙的中心距離，k為障礙個(gè)數(shù)。

路徑點(diǎn)搜索的流程如下：

(1)路徑初始化

將搜索區(qū)域網(wǎng)格化，將起點(diǎn)A周圍可通過的節(jié)點(diǎn)加入到open列表，將A加入到close列表。其中，open列表存儲(chǔ)了待搜索的節(jié)點(diǎn)，close列表存儲(chǔ)了最小代價(jià)點(diǎn)以及不再需要搜索的節(jié)點(diǎn)。

(2)搜索節(jié)點(diǎn)

從A點(diǎn)開始，向周圍8個(gè)方向?qū)Υ嬖谟趏pen列表中的節(jié)點(diǎn)進(jìn)行搜索。依據(jù)上述代價(jià)函數(shù)計(jì)算各節(jié)點(diǎn)的代價(jià)值，選取其中的最小代價(jià)值節(jié)點(diǎn)，將其從open列表中移除，并將其加入到close列表。設(shè)置當(dāng)前節(jié)點(diǎn)為最小代價(jià)點(diǎn)的父節(jié)點(diǎn)，然后從該子節(jié)點(diǎn)開始進(jìn)行搜索。重新添加子節(jié)點(diǎn)的周圍節(jié)點(diǎn)并將其添加到open列表，如果新的待加入節(jié)點(diǎn)已經(jīng)在子節(jié)點(diǎn)的open列表中，則計(jì)算新的代價(jià)值。若代價(jià)更小則更新該值，并更新父節(jié)點(diǎn)為當(dāng)前節(jié)點(diǎn)，否則保持。

(3)終止判斷

循環(huán)上述步驟(2)直至目標(biāo)節(jié)點(diǎn)被加入到close列表時(shí)關(guān)閉循環(huán)。在close列表中，從目標(biāo)節(jié)點(diǎn)開始，將各節(jié)點(diǎn)的父節(jié)點(diǎn)根據(jù)順序連接起來直至起點(diǎn)，即為規(guī)劃的路徑。若開啟列表為空后還未找到目標(biāo)節(jié)點(diǎn)，則路徑搜索失敗，如圖3所示。

圖3 A*算法搜索流程圖Fig.3 A* algorithm search flow chart

引入威脅系數(shù)后，搜索的方向?qū)⒆優(yōu)槁窂酱鷥r(jià)與威脅代價(jià)和最小的方向，兩者的優(yōu)先級(jí)可以根據(jù)實(shí)際任務(wù)通過權(quán)重系數(shù)進(jìn)行調(diào)整。改進(jìn)前后的A*路徑搜索對(duì)比如圖4所示。3個(gè)藍(lán)色網(wǎng)格為障礙，綠色和紅色網(wǎng)格分別為起點(diǎn)和終點(diǎn)。設(shè)每個(gè)網(wǎng)格的長度為10，對(duì)角線長度為14，則在ma=0.5、wi(x,y)=1、ri_max=70時(shí)，改進(jìn)前和改進(jìn)后的搜索網(wǎng)格數(shù)分別為34和52。兩條路徑的長度均為68，與障礙中心的累計(jì)距離分別為120和128。改進(jìn)之后，A*搜索路徑的次數(shù)雖然增大了，但路徑長度均為68，且降低了路徑平均威脅。

2.2 離散航跡路徑優(yōu)化與編隊(duì)隊(duì)形表示

由上述步驟生成的路徑為全局曲線路徑。考慮降低數(shù)據(jù)鏈的通信需求，本文采用離散路徑結(jié)合曲線擬合的方式對(duì)該路徑進(jìn)行參數(shù)化表示。領(lǐng)航無人機(jī)的規(guī)劃路徑Ω是網(wǎng)格化后的離散路徑點(diǎn)的集合，從當(dāng)前位置沿路徑Ω在到達(dá)目標(biāo)點(diǎn)之前截取一段長度為L的路徑，采用Hermite多項(xiàng)式對(duì)路徑Ω進(jìn)行參數(shù)化表示[14]。

(a)傳統(tǒng)A*算法

路徑Ω的參數(shù)化信息包含m+1個(gè)路徑點(diǎn)Pi=(xi,yi)，以及路徑點(diǎn)上對(duì)應(yīng)的導(dǎo)數(shù)qi，i=0,2,…,m。選取擬合曲線在離散路徑點(diǎn)處的切線與離散路徑點(diǎn)前后折線的角平分線垂直，則起始點(diǎn)與路徑折線的斜率相同，可由此計(jì)算各路徑點(diǎn)的導(dǎo)數(shù)qi

(7)

(8)

選取Hermite多項(xiàng)式插值基函數(shù)αi(xk)、βi(xk)，并滿足

(9)

利用插值基函數(shù)表示Hermite多項(xiàng)式H2m+1(x)=H(x)

(10)

(11)

式中，k1,k2，k3,k4為引入的臨時(shí)中間變量。根據(jù)式(8)～式(11)，可得到該路徑的分段3次Hermite多項(xiàng)式

(12)

式中，x∈[xi,xi+1]，(i=0,1,…,m-1)。由式(12)可得到利用Hermite多項(xiàng)式進(jìn)行參數(shù)化后的領(lǐng)航無人機(jī)路徑，但該路徑有可能重新進(jìn)入障礙物影響范圍，因此需要重新優(yōu)化路徑?？刹捎萌缦碌木植恐匦乱?guī)劃方案：在發(fā)生碰撞的路徑的下一路徑點(diǎn)B、新增點(diǎn)B的前后兩條原始路徑點(diǎn)的連線中點(diǎn)D和E重新生成新的曲線。重復(fù)該步驟，直至曲線滿足運(yùn)動(dòng)學(xué)約束與避碰要求。優(yōu)化后的示意圖如圖5所示。

圖5 重新規(guī)劃路徑優(yōu)化示意圖Fig.5 Re-planning path optimization diagram

考慮跟隨無人機(jī)的編隊(duì)路徑Ωi，由領(lǐng)航無人機(jī)為編隊(duì)中的所有跟隨無人機(jī)分配隊(duì)形位置。為了直觀描述編隊(duì)隊(duì)形，首先以領(lǐng)航無人機(jī)的位置為原點(diǎn)，建立Serret-Frenet(SF)坐標(biāo)系[15]。其中，速度切線方向?yàn)閤sf軸，法線方向?yàn)閥sf軸。在SF坐標(biāo)系下，無人機(jī)i到無人機(jī)j的期望編隊(duì)坐標(biāo)向量為dij=(xoff_ij,yoff_ij,zoff_ij)，i,j=0,1,2,…,n，n為跟隨無人機(jī)數(shù)量，領(lǐng)航無人機(jī)的編號(hào)為0。第i架跟隨無人機(jī)的路徑Ωi生成參考領(lǐng)航無人機(jī)，在將相對(duì)于領(lǐng)航無人機(jī)的編隊(duì)坐標(biāo)向量d0i旋轉(zhuǎn)到NED北東地地理坐標(biāo)系下后，將其添加到領(lǐng)航無人機(jī)的離散路徑點(diǎn)上，再用Hermite多項(xiàng)式進(jìn)行表示

Ωi=Ω+R(-λ)d0i(i=1,2,…,n)

(13)

式中，λ為1.1節(jié)所述的速度角，R(-λ)為繞NED北東地地理坐標(biāo)系z(mì)軸轉(zhuǎn)動(dòng)-λ角度的方向余弦矩陣。無人機(jī)在SF坐標(biāo)系下的隊(duì)形示意圖如圖6所示。

圖6 Serret-Frenet 坐標(biāo)系下的編隊(duì)隊(duì)形示意圖Fig.6 Swarm formation diagram under Serret-Frenet coordinate

3 編隊(duì)協(xié)同控制器設(shè)計(jì)

3.1 路徑協(xié)同控制器設(shè)計(jì)

無人機(jī)編隊(duì)任務(wù)多種多樣，常需要在編隊(duì)飛行過程中進(jìn)行隊(duì)形變換以躲避障礙物、切換任務(wù)等，或者在到達(dá)目標(biāo)點(diǎn)上空后進(jìn)行盤旋、環(huán)繞，以對(duì)目標(biāo)進(jìn)行偵察等。隨著隊(duì)形的切換以及時(shí)間的推移，如果不加修正，則無法保證無人機(jī)按照預(yù)期的隊(duì)形飛行。因此，需根據(jù)一致性原理設(shè)計(jì)路徑協(xié)同控制器，使編隊(duì)隊(duì)形滿足

(14)

式中，Di為第i架無人機(jī)在SF坐標(biāo)系下的實(shí)際位置。定義無人機(jī)i編隊(duì)隊(duì)形誤差ζei為

(15)

根據(jù)上述1.2節(jié)的圖論知識(shí)，可將上式改寫為矩陣形式

(16)

無人機(jī)的運(yùn)動(dòng)模型采用了上述1.1節(jié)的二階積分模型。這里，將二階積分模型具體細(xì)化為加速度與速度。為使編隊(duì)誤差趨近于零，可將路徑協(xié)同控制器分為兩部分：單機(jī)基于路徑跟隨完成對(duì)上述2.2節(jié)規(guī)劃的路徑的跟蹤，以及多機(jī)路徑協(xié)調(diào)。其中，在路徑跟蹤中采用上述1.1節(jié)的二階加速度作為控制量，利用L1算法實(shí)現(xiàn)路徑的穩(wěn)定跟蹤[16]，而路徑協(xié)調(diào)則采用了速度控制量。

由L1算法實(shí)現(xiàn)路徑跟蹤而生成的橫向加速度控制量acmd為

(17)

式中，L1為從無人機(jī)當(dāng)前位置到期望路徑上參考點(diǎn)的向量，其長度大小為一個(gè)預(yù)設(shè)的定值。η是L1向量到無人機(jī)預(yù)設(shè)期望飛行速度向量v的夾角?？紤]到無人機(jī)的速度限制，設(shè)計(jì)在采用L1算法跟蹤路徑時(shí)限制路徑跟蹤的分速度Vd為

(18)

式中，vmin、vmax和Δv分別是無人機(jī)最小、最大飛行速度和路徑協(xié)調(diào)控制器的可調(diào)速度，可設(shè)計(jì)如下的路徑協(xié)調(diào)控制器

Vr=Vd-Δvf(ζe)f(x)=2/(1+e-x)-1
f(x)∈(-1,1)，x∈R

(19)

3.2 動(dòng)態(tài)障礙避障控制器設(shè)計(jì)

考慮跟隨無人機(jī)與無人機(jī)以及無人機(jī)與外部之間的動(dòng)態(tài)障礙避障，可采用結(jié)合路徑預(yù)測(cè)的人工勢(shì)場法進(jìn)行避障。

人工勢(shì)場法的目的是構(gòu)造包含斥力與引力的合勢(shì)能函數(shù)，使得無人機(jī)在勢(shì)力引導(dǎo)下改變飛行路徑[17]。其中，引力可引導(dǎo)無人機(jī)前往目標(biāo)點(diǎn)，而斥力可使無人機(jī)避開障礙物。3.1節(jié)已經(jīng)完成了對(duì)目標(biāo)編隊(duì)的引導(dǎo)，這里只需考慮斥力的避障。

在構(gòu)造斥力場函數(shù)時(shí)，為保證無人機(jī)與無人機(jī)或無人機(jī)與其他障礙之間在距離較近時(shí)能夠快速分離，以無人機(jī)之間的避障為例，無人機(jī)i對(duì)無人機(jī)j的斥力可采用如下的斥力場函數(shù)

(20)

式中，b、c是斥力調(diào)節(jié)常數(shù)，Rmin和Rmax分別表示無人機(jī)間最小的安全距離和斥力最大作用距離。距離小于安全距離時(shí)，無人機(jī)將發(fā)生碰撞；距離超過Rmax后，斥力將不起作用。由式(20)可得無人機(jī)i對(duì)無人機(jī)j的斥力為

(21)

無人機(jī)i所受的合斥力為

(22)

由式(22)可得斥力矩陣為F=[F0,F1,…,Fn]T。考慮在動(dòng)態(tài)障礙情況下的避障軌跡優(yōu)化。無人機(jī)在進(jìn)入碰撞半徑后，動(dòng)態(tài)的障礙運(yùn)動(dòng)軌跡與無人機(jī)避障后的合成運(yùn)動(dòng)軌跡可能仍然存在碰撞的可能，或產(chǎn)生的避障控制量大于無人機(jī)運(yùn)動(dòng)學(xué)限制，因此動(dòng)態(tài)障礙下的軌跡優(yōu)化過程如下。

(a)情形1

(23)

式中,U為障礙對(duì)無人機(jī)的斥力，Vb為障礙運(yùn)動(dòng)速度，c1、c2分別為表征兩者速度方向的中間矢量。當(dāng)滿足上述條件時(shí)，新的斥力Un為

(24)

式中，Vt為障礙圓心O指向目標(biāo)的單位速度向量。R(90)和R(-90)分別表示繞上述NED地理坐標(biāo)系z(mì)軸轉(zhuǎn)動(dòng)正負(fù)90度方向的方向余弦矩陣。將式(21)的各分斥力按照式(23)和式(24)處理后，再由式(22)可得加入動(dòng)態(tài)避障的新合斥力矩陣Fn。

綜上，將上述3.1節(jié)所述的路徑協(xié)同控制器結(jié)合動(dòng)態(tài)避障控制器后，可得到最終避開障礙情況下編隊(duì)協(xié)同控制器的速度量輸出Vout

(25)

式中，fm為無人機(jī)的質(zhì)量系數(shù)。

4 仿真驗(yàn)證

通過三架無人機(jī)的典型的協(xié)同編隊(duì)隊(duì)形保持與避障，對(duì)上述方法進(jìn)行了驗(yàn)證，并給出了仿真結(jié)果。

首先，期望的編隊(duì)路徑由第2節(jié)所述的路徑規(guī)劃方法生成。將200m×200m的區(qū)域劃分為80m×80m的網(wǎng)格，并隨機(jī)設(shè)置5個(gè)固定障礙，生成各障礙的威脅代價(jià)色度圖。傳統(tǒng)A*最短路徑與改進(jìn)A*路徑規(guī)劃方法的對(duì)比如圖8所示。由圖8可知，傳統(tǒng)A*方法在沒有障礙阻擋的前提下，選擇了前往終點(diǎn)的最短直線路徑，而本文的改進(jìn)A*方法在生成的威脅代價(jià)地圖基礎(chǔ)上，考慮了威脅的代價(jià)，進(jìn)而降低了規(guī)劃路徑上的平均威脅。仿真結(jié)果對(duì)比如表1所示。

圖8 障礙威脅環(huán)境下的編隊(duì)路徑規(guī)劃方法對(duì)比Fig.8 Comparison of formation path planning methods under obstacle threat environment

表1 路徑規(guī)劃方法仿真對(duì)比Tab.1 Comparison of path planning methods simulation

其次，為無人機(jī)設(shè)定初始仿真條件。無人機(jī)1、2、3的初始位置分別為(10，0)、(20，20)、(0，60)，單位為m，對(duì)應(yīng)的初始速度分別為(5，1)、(2，3)、(4，2)，單位為m/s。其中，2號(hào)為領(lǐng)航無人機(jī)，1、3號(hào)為跟隨無人機(jī)。靜態(tài)障礙的起始坐標(biāo)為(113m，53m)，移動(dòng)速度為4m/s，移動(dòng)航向角為45°，障礙影響半徑為40m，最小安全半徑為25m。無人機(jī)的期望速度為5m/s，最大速度為10m/s，最大加速度為5m/s2。跟隨無人機(jī)按照編隊(duì)間隔y向各20m的隊(duì)形，參照領(lǐng)航無人機(jī)路徑飛行。速度量標(biāo)準(zhǔn)差為0.1m/s，位置量標(biāo)準(zhǔn)差為0.2m。設(shè)置采樣周期1ms，仿真時(shí)間50s。

在考慮動(dòng)態(tài)障礙的前提下，采用本文的改進(jìn)領(lǐng)航-跟隨方法的編隊(duì)飛行軌跡與編隊(duì)誤差如圖9所示。由圖9可以看出，各無人機(jī)均能在避開動(dòng)態(tài)障礙的情況下按照規(guī)劃的全局路徑飛行。在飛行過程中，無人機(jī)從初始位置起逼近隊(duì)形，編隊(duì)誤差減小并趨近于零；在遇到障礙時(shí)，通過改變速度調(diào)整隊(duì)形避障，編隊(duì)誤差增大；穿過障礙后，編隊(duì)誤差再次減小，各無人機(jī)開始穩(wěn)定跟蹤期望路徑并保持了隊(duì)形。

圖10是傳統(tǒng)領(lǐng)航-跟隨法與采用路徑跟隨的改進(jìn)領(lǐng)航-跟隨法的編隊(duì)誤差對(duì)比圖。圖10(a)、圖10(b)是兩架跟隨無人機(jī)的編隊(duì)誤差，圖10(c)是領(lǐng)航無人機(jī)的編隊(duì)誤差。由圖10可以看出，領(lǐng)航無人機(jī)的編隊(duì)誤差區(qū)別不大，但跟隨無人機(jī)在采用了基于路徑跟隨的改進(jìn)領(lǐng)航-跟隨協(xié)同編隊(duì)方法后，通過路徑參數(shù)的預(yù)先同步與路徑跟隨協(xié)同，編隊(duì)誤差低于傳統(tǒng)的領(lǐng)航-跟隨方法。因此，采用本文的基于路徑跟隨的改進(jìn)領(lǐng)航-跟隨方法，可以實(shí)現(xiàn)更高的集群無人機(jī)編隊(duì)精度。

(a)編隊(duì)飛行隊(duì)形

(a)無人機(jī)1的編隊(duì)隊(duì)形誤差

5 結(jié) 論

本文研究了領(lǐng)航-跟隨法無人機(jī)的編隊(duì)路徑最優(yōu)規(guī)劃，以及動(dòng)態(tài)障礙環(huán)境下路徑跟隨與協(xié)同編隊(duì)隊(duì)形保持問題。通過改進(jìn)A*算法與Hermite多項(xiàng)式，分別完成了全局安全路徑最優(yōu)規(guī)劃與編隊(duì)路徑參數(shù)化表示，并利用一致性原理與改進(jìn)的人工勢(shì)場法實(shí)現(xiàn)了動(dòng)態(tài)障礙環(huán)境下的編隊(duì)協(xié)同控制。仿真結(jié)果表明，與傳統(tǒng)的領(lǐng)航-跟隨方法相比，本方法可以獲得更優(yōu)的全局路徑，以及更高的協(xié)同編隊(duì)精度。此外，本方法采用了路徑參數(shù)協(xié)調(diào)同步，可以降低對(duì)通信系統(tǒng)的要求，選取的Hermite多項(xiàng)式路徑參數(shù)可以獲得光滑的二階動(dòng)力學(xué)控制量，使編隊(duì)的飛行過程更為平穩(wěn)，因此具有一定的工程應(yīng)用價(jià)值。