改進(jìn)APF與Bezier相結(jié)合的多無人機(jī)協(xié)同避碰航路規(guī)劃

楊 勇, 丁 勇, 黃鑫城

(南京航空航天大學(xué)自動化學(xué)院，南京 211106)

0 引言

多無人機(jī)協(xié)同目標(biāo)追蹤與避障是指多架無人機(jī)協(xié)同合作對目標(biāo)進(jìn)行追蹤并在追蹤過程中規(guī)避障礙物，是無人機(jī)協(xié)同合作領(lǐng)域的研究熱點之一，在軍用和民用領(lǐng)域均具有重要的應(yīng)用價值[1]。針對多無人機(jī)目標(biāo)追蹤與防碰撞問題，國內(nèi)外學(xué)者做了大量的研究。文獻(xiàn)[2]將支持向量機(jī)與人工勢場法結(jié)合，采用球面對稱函數(shù)構(gòu)建無人機(jī)導(dǎo)航模型，使得多架無人機(jī)無碰撞飛行；文獻(xiàn)[3]基于認(rèn)知博弈制導(dǎo)算法，建立無人機(jī)與入侵機(jī)的運動學(xué)模型，將無人機(jī)機(jī)間防碰問題轉(zhuǎn)化為雙方博弈問題，實現(xiàn)多無人機(jī)自主防碰撞?？柭鼮V波預(yù)測算法也是解決多無人機(jī)防碰撞問題的一種有效方法。文獻(xiàn)[4]基于流體擾動計算與交互多模型相結(jié)合，完成多無人機(jī)三維環(huán)境下的無碰撞航路規(guī)劃；文獻(xiàn)[5]采用自適應(yīng)卡爾曼算法預(yù)測僚機(jī)的狀態(tài)進(jìn)而控制僚機(jī)機(jī)動，當(dāng)無人機(jī)編隊過程中機(jī)間距離過近時，僚機(jī)便迅速做出規(guī)避動作，實現(xiàn)無人機(jī)編隊互不碰撞。人工勢場法是一種虛擬力法，由于算法簡單，運算速度快，能夠保證路徑安全等特點，在路徑規(guī)劃中得到應(yīng)用，但該算法也存在規(guī)劃路徑振蕩等問題[6]。針對這些問題不少學(xué)者進(jìn)行了研究，文獻(xiàn)[7]模擬橡皮條的拉緊法去除機(jī)器人行駛路徑的冗余節(jié)點，并用對稱多項式處理得到平滑路徑；文獻(xiàn)[8]在圓錐曲線引力場函數(shù)的基礎(chǔ)上增加一個指數(shù)項到引力場函數(shù)中，消除了奇異值點,避免了抖動現(xiàn)象。

總的來看，無人機(jī)碰撞規(guī)避問題大多研究多無人機(jī)在追蹤目標(biāo)過程中對環(huán)境障礙物的規(guī)避，從而實現(xiàn)無人機(jī)的目標(biāo)追蹤路徑規(guī)劃，而對于多無人機(jī)之間的機(jī)間防碰，大多單純采用組網(wǎng)通信方式，很少采用路徑規(guī)劃算法達(dá)到多無人機(jī)之間的防碰目的；此外，基于人工勢場法航路規(guī)劃中的路徑振蕩問題多針對地面機(jī)器人展開，但由于地面機(jī)器人與無人機(jī)的運動特性有很大不同，如何避免無人機(jī)航路規(guī)劃過程中的航路振蕩是迫切需要解決的問題。

針對上述問題的不足，本文提出一種改進(jìn)APF與Bezier相結(jié)合的多無人機(jī)協(xié)同避碰航路規(guī)劃方法。該方法通過在人工勢場法中增加無人機(jī)自身斥力場進(jìn)行多無人機(jī)協(xié)同目標(biāo)追蹤航路規(guī)劃，并結(jié)合分段Bezier曲線進(jìn)行在線航路平滑優(yōu)化，為解決多無人機(jī)機(jī)間防碰，消除航路規(guī)劃過程中易出現(xiàn)的路徑振蕩現(xiàn)象，提供了一個有效的解決途徑。

1 人工勢場法簡介

人工勢場法是由KHATIB于1986年提出的一種虛擬力法，其基本思想是:通過構(gòu)造一種抽象的勢力場，無人機(jī)在追蹤目標(biāo)過程中受到該勢力場的影響，目標(biāo)對無人機(jī)產(chǎn)生“引力”，障礙物對無人機(jī)產(chǎn)生“斥力”，引力與斥力疊加求出合力控制無人機(jī)的運動[9]。圖1為無人機(jī)的引力與斥力受力示意圖。

圖1 引力與斥力示意圖Fig.1 Diagram of attraction and repulsion force

1.1 勢函數(shù)

設(shè)q為無人機(jī)處于二維環(huán)境空間中的一點，則該無人機(jī)在該點的引力勢函數(shù)和斥力勢函數(shù)分別定義為

(1)

(2)

式中:k和m分別為引力增益和斥力增益；ρa(bǔ)(q)和ρr(q)分別為q點到目標(biāo)a和障礙物r的距離；ρ0是障礙物的最大影響半徑。

1.2 斥力函數(shù)與引力函數(shù)

無人機(jī)處在引力勢場和斥力勢場內(nèi)，所受的合力F表示為

F=Fatt+Frep

(3)

式中:Fatt為目標(biāo)對無人機(jī)的引力;Frep為障礙物對無人機(jī)的斥力。F決定了無人機(jī)的運動方向。

定義障礙物對無人機(jī)的斥力和目標(biāo)點對無人機(jī)的引力分別為對應(yīng)斥力場和引力場的負(fù)梯度[10]，則q點受到目標(biāo)點的引力Fatt(q)和障礙物的斥力Frep(q)分別為該點引力勢函數(shù)和斥力勢函數(shù)的負(fù)梯度，即

Fatt(q)=-grad(Uatt(q))=-kρa(bǔ)(q)

(4)

>。

(5)

多個障礙物共同影響無人機(jī)運動時，根據(jù)力的疊加性理論可得出無人機(jī)所受合力F為

(6)

式中：N為障礙物的個數(shù);Frep i為第i個障礙物對無人機(jī)的斥力。

2 改進(jìn)APF的多無人機(jī)協(xié)同避碰航路規(guī)劃

本文提出一種改進(jìn)APF方法解決多無人機(jī)協(xié)同目標(biāo)追蹤過程中機(jī)間防碰撞問題。首先通過建立無人機(jī)的引力與斥力受力模型，保證每架無人機(jī)在追蹤目標(biāo)的過程中均可有效規(guī)避障礙物，在此基礎(chǔ)上，將每架無人機(jī)視為移動障礙物，自身產(chǎn)生斥力勢場來避免與其他無人機(jī)之間的相互碰撞。

2.1 模型假設(shè)

對于多無人機(jī)協(xié)同目標(biāo)追蹤問題，實際情況下需要考慮的因素過多將不宜于理論分析，為了重點分析問題，本文做出了一些合理的假設(shè)：

1) 忽略無人機(jī)的種類、機(jī)型、性能等因素，將無人機(jī)視為質(zhì)點；

2) 本文所指的障礙物分為靜止障礙物和移動障礙物，其中靜止障礙物一般有敵方雷達(dá)探測區(qū)域、火炮打擊區(qū)域、建筑物或山脈，但其形狀都視為一定大小的圓，且這些障礙物的位置認(rèn)為是已知的；

3) 移動障礙物可以由無人機(jī)機(jī)載的視覺傳感器和避障傳感器快速精確地感知；

4) 無人機(jī)機(jī)載的是高性能飛行控制器和視覺處理器，具備快速計算能力,可進(jìn)行較復(fù)雜計算。

2.2 單架無人機(jī)的人工勢場

在多無人機(jī)協(xié)同目標(biāo)追蹤與避障過程中，每架無人機(jī)都會受到目標(biāo)對它的引力作用和障礙物對它的斥力作用。

定義1設(shè)q點為第j架無人機(jī)在空間中的一點，則對于第j架無人機(jī)而言，目標(biāo)a對它的引力和障礙物r對它的斥力分別為

(7)

(8)

在多無人機(jī)協(xié)同目標(biāo)追蹤中，通常存在多個障礙物，因此無人機(jī)受到的斥力是多個障礙物對它產(chǎn)生的斥力的疊加。第j架無人機(jī)所受到的合力Fj為

(9)

2.3 機(jī)間防碰人工勢場

圖2是無人機(jī)自身斥力場示意圖。

圖2 無人機(jī)自身斥力場示意圖Fig.2 UAV’s own repulsive field

在多無人機(jī)協(xié)同目標(biāo)追蹤的過程中，為了防止多無人機(jī)機(jī)間碰撞，造成機(jī)體毀壞導(dǎo)致任務(wù)失敗，本文將無人機(jī)視為移動障礙，對其建立一個斥力勢場，使得每架無人機(jī)對其他無人機(jī)產(chǎn)生一定的斥力作用，這樣無人機(jī)之間就能時刻保持在安全距離范圍之外，避免相互碰撞。

定義2設(shè)q為空間中的一點，則第j架無人機(jī)產(chǎn)生的斥力勢場函數(shù)為

(10)

相應(yīng)的，在q點的無人機(jī)受到第j架無人機(jī)的斥力是式(10)斥力勢函數(shù)的負(fù)梯度，即

(11)

2.4 APF算法規(guī)避局部極小的措施

在無人機(jī)利用人工勢場法對目標(biāo)進(jìn)行追蹤過程中，當(dāng)無人機(jī)尚未到達(dá)目標(biāo)，而此時其所受目標(biāo)引力和障礙物斥力的總合力為零時，無人機(jī)陷入局部最優(yōu)狀態(tài)，本文提出如下逃離局部最優(yōu)的方法。

當(dāng)無人機(jī)所受引力和斥力的合力為零，但引力不為零時，引力和合斥力大小相等、方向相反，可以在無人機(jī)和目標(biāo)之間假想存在一個虛擬障礙物，其對無人機(jī)產(chǎn)生的斥力就是此時所有實際障礙物對該無人機(jī)的合斥力，如圖3所示。虛擬障礙物的具體位置由式(5)反推出ρr(q)來確定，該虛擬障礙物斥力范圍仍是以ρ0為半徑的圓，此時無人機(jī)逃離局部最優(yōu)的飛行方向就是沿著圓的切線方向，左右切線均可，逃離局部最優(yōu)后再按照原來的算法飛向目標(biāo)。

圖3 無人機(jī)逃離局部最優(yōu)的方向示意圖Fig.3 The direction of UAV escaping the local optimal

3 Bezier曲線的路徑平滑

用改進(jìn)人工勢場法進(jìn)行的無人機(jī)航路規(guī)劃存在航路折線多、轉(zhuǎn)折角度大等問題，尤其是無人機(jī)在障礙物的周圍容易出現(xiàn)路徑振蕩，會造成無人機(jī)大角度機(jī)動動作頻繁，使無人機(jī)執(zhí)行任務(wù)效率低，追蹤目標(biāo)風(fēng)險大。本文提出分段Bezier曲線對改進(jìn)人工勢場法規(guī)劃出的航路進(jìn)行平滑優(yōu)化，有效地解決了人工勢場法路徑規(guī)劃中的路徑振蕩問題，使得平滑后的航路適合無人機(jī)的飛行。

3.1 Bezier曲線的原理

Bezier曲線是一種參數(shù)多項式曲線，n階Bezier曲線定義為[11]

(12)

式中：Pi(0≤i≤n)為曲線的第i個控制點，順次連接從P0到Pn的折線被稱為Bezier曲線的控制多邊形；Bi,n(u)為n次Bernstein多項式，它的表達(dá)式為

(13)

3.2 分段Bezier曲線的連接

由于控制點數(shù)的增加會引起B(yǎng)ezier曲線次數(shù)的提高，而高次多項式又會帶來計算上的困難和算法的魯棒性差[12],所以本文采用分段Bezier曲線進(jìn)行航路規(guī)劃平滑優(yōu)化，然后將各段路徑相連接，并在接合處保持一定的連續(xù)和光滑性。

假設(shè)兩條Bezier曲線P(t)和Q(t)的控制點分別為Pi(i=0,1,…,n)和Qj(j=0,1,…,m)，如圖4所示,若把兩條曲線連接起來，則該曲線在連接處應(yīng)具備以下特點[13]：

1) 連續(xù)的充要條件是Pn=Q0；

2) 平滑的充要條件是Pn-1,Pn(Q0),Q1三點共線。

圖4 兩段Bezier曲線連接圖Fig.4 Connection of two fragments of Bezier curve

3.3 分段Bezier曲線解決路徑振蕩

采用上述改進(jìn)人工勢場法對無人機(jī)進(jìn)行航路動態(tài)規(guī)劃會出現(xiàn)路徑振蕩的現(xiàn)象，這里提出分段Bezier曲線的方法來解決。具體思路是：假設(shè)在t0,t1,t2,t3時刻利用人工勢場法計算出的飛行航點分別是P0,P1,P2,P3;V1,V2,V3分別是線段P0P1,P1P2,P2P3的中點，則由控制點V1,P1,V2和V2,P2,V3生成的Bezier曲線在連接點V2處滿足連續(xù)和平滑性條件。若當(dāng)前時刻是t2，此時無人機(jī)已按Bezier曲線由V1飛到V2，利用人工勢場法計算出經(jīng)過t3-t2后的航點P3，沿著V2,P2,V3生成的Bezier曲線飛行即為下一個時間段的航行路線。這樣使得原本振蕩的路徑得到平滑，解決了當(dāng)無人機(jī)靠近障礙物飛行時出現(xiàn)的路徑振蕩問題。示意圖如圖5所示。

圖5 分段Bezier曲線的航路規(guī)劃Fig.5 The route planning of a sectional Bezier curve

4 仿真驗證與結(jié)果分析

本章對所提算法進(jìn)行仿真驗證與分析，主要分為兩個步驟：

1) 采用本文改進(jìn)的人工勢場法進(jìn)行多無人機(jī)協(xié)同目標(biāo)追蹤的航路規(guī)劃，并與未增加無人機(jī)斥力場進(jìn)行的人工勢場法航路規(guī)劃進(jìn)行對比；

2) 采用分段Bezier曲線對改進(jìn)勢場法動態(tài)規(guī)劃的航路進(jìn)行在線平滑優(yōu)化。

4.1 仿真環(huán)境與參數(shù)設(shè)置

仿真實驗在Windows10操作系統(tǒng)中Matlab2012軟件下進(jìn)行，微機(jī)的處理器是Intel Core i5-3210M，主頻2.5 GHz，內(nèi)存為4 GiB。

在仿真中有3架無人機(jī)UAV1，UAV2，UAV3，起始位置分別為 (50 km,0 km)，(8 km,0 km)和(10 km,20 km)，目標(biāo)按一定軌跡運動。共有15個障礙物按編號1～15分布在120 km×120 km的目標(biāo)追蹤區(qū)域內(nèi)，其中,編號12為突發(fā)的移動障礙物。具體仿真參數(shù)如表1所示。

表1 仿真參數(shù)設(shè)置

4.2 仿真步驟及算法流程

無人機(jī)在tk時刻機(jī)載傳感器感知周圍環(huán)境，獲取障礙物、目標(biāo)和其他無人機(jī)的位置，然后規(guī)劃出無人機(jī)下一個航點，飛行一段時間Δt，在tk+1=tk+Δt時刻到達(dá)下一個航點。仿真最終目的是使從不同起點出發(fā)的3架無人機(jī)能避開所有障礙物且機(jī)間互不碰撞并最終全部到達(dá)目標(biāo)位置，即追蹤到目標(biāo)。

圖6為仿真框圖。

圖6 仿真框圖Fig.6 Block diagram of simulation

仿真圖分為3個部分，首先構(gòu)建每架無人機(jī)的受力模型，對3架無人機(jī)使用未增加無人機(jī)斥力場的人工勢場法，進(jìn)行多無人機(jī)目標(biāo)追蹤的航路動態(tài)規(guī)劃，各無人機(jī)單獨規(guī)劃互不干擾；其次，在增加無人機(jī)斥力場后對3架無人機(jī)進(jìn)行協(xié)同目標(biāo)追蹤的航路動態(tài)規(guī)劃，無人機(jī)之間因斥力存在互不碰撞;最后，在相同環(huán)境下，使用分段Bezier曲線在改進(jìn)人工勢場法動態(tài)規(guī)劃過程中進(jìn)行在線航路平滑優(yōu)化。圖7為改進(jìn)人工勢場法與分段Bezier曲線結(jié)合進(jìn)行多無人機(jī)協(xié)同目標(biāo)追蹤航路規(guī)劃的仿真流程圖。

圖7 仿真流程圖Fig.7 Flow chart of simulation

4.3 仿真結(jié)果及分析

4.3.1 改進(jìn)人工勢場法目標(biāo)追蹤航路規(guī)劃

未增加無人機(jī)斥力場的目標(biāo)追蹤航路規(guī)劃仿真結(jié)果如圖8所示,15個障礙物按照1～15進(jìn)行編號，障礙物的內(nèi)圓表示障礙物的大小，無人機(jī)絕對不能與它碰撞。障礙物的外圓表示障礙物斥力場的影響范圍，此范圍是半徑為10 km的圓。三角形代表無人機(jī)的起始位置。從3架無人機(jī)目標(biāo)追蹤的航線中可以看出，所有無人機(jī)均避開障礙物，最終追蹤到目標(biāo)。但是，無人機(jī)UAV1和UAV2的航線在障礙物5和障礙物6之間產(chǎn)生了重疊，仿真參數(shù)中設(shè)置UAV1,UAV2的規(guī)劃步長分別為1 km和1.3 km，這2架無人機(jī)將同時到達(dá)航路重疊區(qū)，在此區(qū)域發(fā)生碰撞，致使多無人機(jī)協(xié)同目標(biāo)追蹤任務(wù)失敗。

圖8 未增加無人機(jī)斥力場的航路規(guī)劃Fig.8 The route planning without UAV’s repulsive field

為了解決這個問題，采用所提出的改進(jìn)人工勢場法，增加無人機(jī)自身斥力場，使得每架無人機(jī)對其余無人機(jī)均產(chǎn)生斥力作用，即無人機(jī)之間互斥，互斥的影響范圍為半徑5 km的圓，仿真結(jié)果見圖9所示。

圖9 改進(jìn)人工勢場法的航路規(guī)劃Fig.9 The route planning with UAV’s repulsive field

從圖9中可明顯看出，UAV1和UAV2的航線在障礙物5和障礙物6之間產(chǎn)生了航路分離，并未出現(xiàn)圖8中的航線重疊現(xiàn)象，這是無人機(jī)之間相互排斥的作用導(dǎo)致的，解決了多無人機(jī)協(xié)同目標(biāo)追蹤過程中的機(jī)間防碰問題。但可以明顯看出，當(dāng)無人機(jī)在障礙物附近時，航路出現(xiàn)了嚴(yán)重的振蕩現(xiàn)象，這在實際情況中無論從飛機(jī)的機(jī)動性能、能源消耗還是從實時追蹤方面考慮，都是絕對不允許出現(xiàn)的。

4.3.2 路徑振蕩平滑優(yōu)化

采用改進(jìn)人工勢場法結(jié)合分段Bezier曲線進(jìn)行航路在線平滑優(yōu)化，仿真結(jié)果如圖10所示，可以看出平滑優(yōu)化的效果顯著，消除了路徑振蕩現(xiàn)象。

圖10 結(jié)合分段Bezier曲線的航路平滑優(yōu)化Fig.10 Path smoothing combining the sectional Bezier curve

5 結(jié)束語

本文提出了一種改進(jìn)人工勢場法的多無人機(jī)協(xié)同避碰航路規(guī)劃方法，根據(jù)無人機(jī)協(xié)同目標(biāo)追蹤的飛行特點，給出了無人機(jī)自身斥力場和斥力的合理定義，使多無人機(jī)在協(xié)同目標(biāo)追蹤過程中互不碰撞。當(dāng)人工勢場法陷入局部極小時，通過構(gòu)造虛擬障礙物的方法，使得無人機(jī)逃離局部最優(yōu)；提出分段Bezier曲線對規(guī)劃路徑進(jìn)行在線實時平滑優(yōu)化，在兩個分段Bezier曲線連接處保證了曲線的平滑性，防止了無人機(jī)飛行做大角度轉(zhuǎn)彎，平滑后的航路不僅不再出現(xiàn)航路振蕩現(xiàn)象，而且整條航線變得更加平滑順暢。