韓 堯, 李少華

(電子科技大學(xué)航空航天學(xué)院, 四川 成都 611731)

0 引 言

至今為止,無(wú)人機(jī)已有約80多年的發(fā)展歷史,由于其隱蔽性好、造價(jià)相對(duì)低廉且不懼傷亡等優(yōu)點(diǎn),受到了各國(guó)軍事單位的重視[1]。在現(xiàn)代戰(zhàn)場(chǎng)上已出現(xiàn)了許多基于無(wú)人機(jī)的智能化武器,利用這些武器也能實(shí)現(xiàn)遠(yuǎn)距離攻擊,在未來(lái)軍事斗爭(zhēng)中無(wú)人機(jī)將扮演著十分重要的角色。除此之外,民用無(wú)人機(jī)發(fā)展迅猛且應(yīng)用十分廣泛,可用于農(nóng)業(yè)、電力、測(cè)繪等多個(gè)行業(yè)[2]。

航跡規(guī)劃是無(wú)人機(jī)任務(wù)規(guī)劃的關(guān)鍵問(wèn)題之一[3],其根據(jù)環(huán)境數(shù)據(jù)、無(wú)人機(jī)機(jī)動(dòng)約束和航程代價(jià)等信息,規(guī)劃出一條從起始位置到目標(biāo)位置的可行航跡,同時(shí)要求飛行代價(jià)盡可能小。在軍用和民用領(lǐng)域,高質(zhì)量的規(guī)劃航跡是保證無(wú)人機(jī)安全完成任務(wù)的重要手段[4-5]。目前,路徑規(guī)劃算法主要可以分為傳統(tǒng)經(jīng)典算法和現(xiàn)代智能算法[6]。其中,傳統(tǒng)經(jīng)典算法主要有人工勢(shì)場(chǎng)法[7-10]、A*算法[11-12]、模擬退火算法[13]等;現(xiàn)代智能算法主要有蟻群優(yōu)化算法[14]、粒子群優(yōu)化算法[15-16]、遺傳算法[17-18]等。

為解決全局路徑規(guī)劃問(wèn)題,Khatib于1986年提出人工勢(shì)場(chǎng)法[19],該算法簡(jiǎn)明且涉及信息較少,具有規(guī)劃速度快、實(shí)時(shí)性較好等優(yōu)點(diǎn)，但也存在比較明顯的問(wèn)題。首先,當(dāng)障礙物的斥力和目標(biāo)點(diǎn)的引力在同一直線(xiàn)上且方向相反,無(wú)人機(jī)容易陷入局部極小點(diǎn),從而出現(xiàn)震蕩或終止飛行[20-21]。其次,未引入無(wú)人機(jī)物理約束,無(wú)人機(jī)以固定步長(zhǎng)飛行且無(wú)任何飛行角限制[22],因此規(guī)劃出的軌跡并不符合動(dòng)力學(xué)模型,無(wú)人機(jī)無(wú)法按照規(guī)劃軌跡飛行。

國(guó)內(nèi)外學(xué)者對(duì)于傳統(tǒng)路徑規(guī)劃的局部極小點(diǎn)問(wèn)題進(jìn)行了比較深入的研究。文獻(xiàn)[23]在遇到局部極小點(diǎn)時(shí)通過(guò)虛設(shè)障礙物來(lái)改變勢(shì)場(chǎng)合力,進(jìn)而使受控對(duì)象離開(kāi)局部極小點(diǎn);文獻(xiàn)[24]利用模糊人工勢(shì)場(chǎng)法對(duì)勢(shì)場(chǎng)參數(shù)做出模糊決策,但該決策并未考慮位置信息的變化,因此仍出現(xiàn)了劇烈震蕩現(xiàn)象;文獻(xiàn)[25]引入了最優(yōu)控制理論,將無(wú)人機(jī)路徑規(guī)劃問(wèn)題重構(gòu)為約束優(yōu)化問(wèn)題并最終將其改造為最優(yōu)控制問(wèn)題,該方法的不足在于仍然存在部分陡峭的轉(zhuǎn)彎角;文獻(xiàn)[26]將人工勢(shì)場(chǎng)法和模擬退火法相結(jié)合,在無(wú)人機(jī)陷入威脅區(qū)時(shí),引入懲罰勢(shì)函數(shù)代替?zhèn)鹘y(tǒng)斥力勢(shì)函數(shù),并采用模擬退火法來(lái)搜索路徑,有效避免了局部最小點(diǎn)和震蕩點(diǎn),但引入太多信息導(dǎo)致計(jì)算量增加。文獻(xiàn)[27]為了克服動(dòng)態(tài)環(huán)境路徑規(guī)劃的震蕩問(wèn)題,使用快速探索的隨機(jī)樹(shù)(rapid-exploration random tree,RRT)來(lái)提供隨機(jī)干擾,以減少振蕩并在更短的時(shí)間內(nèi)達(dá)到目標(biāo)。根據(jù)仿真結(jié)果,所提出的算法發(fā)現(xiàn)與人工勢(shì)場(chǎng)法方法相比,找到更優(yōu)路徑的時(shí)間成本減少了約27%,并且發(fā)生碰撞的情況有了大幅下降。

本文首先在傳統(tǒng)人工勢(shì)場(chǎng)(traditional artificial potential field, TAPF)算法的基礎(chǔ)上,引入角度與速度參量及相應(yīng)的參量調(diào)節(jié)算法,解決了規(guī)劃軌跡不可行的問(wèn)題,同時(shí)由于加入角度限制,也避免了震蕩情況。其次，提出一種改進(jìn)人工勢(shì)場(chǎng)(improved artificial potential field, IAPF)算法,施加輔助避障力,提前改變航向角,使無(wú)人機(jī)避開(kāi)局部極小點(diǎn)的同時(shí)進(jìn)一步平滑路徑。最后，通過(guò)Matlab仿真驗(yàn)證算法可行性,并與其他算法對(duì)比，證明本算法的有效性。

1 人工勢(shì)場(chǎng)法

TAPF算法假設(shè)無(wú)人機(jī)在虛擬力場(chǎng)下運(yùn)動(dòng);力場(chǎng)包括引力場(chǎng)和斥力場(chǎng),引力場(chǎng)吸引無(wú)人機(jī)飛向目標(biāo)點(diǎn),斥力場(chǎng)使無(wú)人機(jī)避開(kāi)障礙物,引力與斥力的合力引導(dǎo)無(wú)人機(jī)從起點(diǎn)飛向終點(diǎn)的同時(shí)避開(kāi)航跡中的所有障礙物[28]。

1.1 引力場(chǎng)

常用的引力勢(shì)場(chǎng)函數(shù)如下:

(1)

式中:Ka是引力勢(shì)場(chǎng)尺度因子;d(p,pgoal)為無(wú)人機(jī)當(dāng)前位置與目標(biāo)點(diǎn)之間的距離,假定無(wú)人機(jī)在特定高度飛行,令p=(x,y)為當(dāng)前位置坐標(biāo),pgoal=(xgoal,ygoal)為目標(biāo)點(diǎn)坐標(biāo)。引力為引力勢(shì)場(chǎng)的負(fù)梯度,可表示為

(2)

引力方向θatt是從無(wú)人機(jī)當(dāng)前位置指向目標(biāo)點(diǎn),如圖1所示。

圖1 TAPF算法方法的合力

1.2 斥力場(chǎng)

Khatib提出的常規(guī)斥力場(chǎng)函數(shù)如下:

(3)

式中:i表示第i個(gè)障礙物附近的斥力勢(shì)場(chǎng);Kr是斥力尺度因子;do是障礙物的斥力有效范圍半徑;d(p,poi)是無(wú)人機(jī)與第i個(gè)障礙物之間的距離,poi=(xoi,yoi)為當(dāng)前障礙物坐標(biāo)。為了解決目標(biāo)不可達(dá)問(wèn)題[29],常用的斥力場(chǎng)函數(shù)如下:

(4)

該斥力場(chǎng)函數(shù)基于常規(guī)的斥力場(chǎng)函數(shù)進(jìn)行了改進(jìn),引入了因子dn(p,pgoal),在目標(biāo)和障礙物距離較近的情況,可以避免斥力太大而繞開(kāi)目標(biāo)的情況,本文將采用改進(jìn)的斥力場(chǎng)函數(shù)。同理,斥力函數(shù)如下:

(5)

(6)

(7)

當(dāng)計(jì)算出地圖中所有障礙物的斥力后,最終的斥力合力即為所有斥力矢量之和。

(8)

式中:n為地圖中障礙物數(shù)量。如圖1所示,無(wú)人機(jī)在整個(gè)勢(shì)場(chǎng)中受到障礙物斥力和目標(biāo)引力組成的合力是Fres。

2 IAPF算法

為了便于實(shí)際航跡規(guī)劃,將地圖信息中的所有障礙物作簡(jiǎn)化處理,將其統(tǒng)一建模為圓形,圓的半徑ro根據(jù)實(shí)際障礙物大小而定。要求建模后的圓形障礙物必須將實(shí)際障礙物覆蓋,為了航跡規(guī)劃的安全性,也可以將圓形障礙物的半徑適當(dāng)增大,同時(shí)保證障礙物的斥力有效范圍do必須大于ro。除此之外,假定無(wú)人機(jī)在特定高度飛行。

2.1 無(wú)人機(jī)物理約束

為解決TAPF算法規(guī)劃軌跡不符合實(shí)際飛行軌跡的問(wèn)題,引入無(wú)人機(jī)物理約束。

首先引入無(wú)人機(jī)最大轉(zhuǎn)向角Δθmax,定義理論轉(zhuǎn)向角Δθtheory如下:

(9)

(10)

其次,在TAPF算法中,無(wú)人機(jī)是按固定速度勻速飛行,為結(jié)合無(wú)人機(jī)實(shí)際飛行情況,引入速度參量。直線(xiàn)飛行或轉(zhuǎn)向角很小時(shí),無(wú)人機(jī)可以全力加速至最大速度;在正常轉(zhuǎn)向時(shí),可以調(diào)整至常規(guī)轉(zhuǎn)向速度;當(dāng)轉(zhuǎn)向角很大時(shí),無(wú)人機(jī)需要將速度減至最小飛行速度[30]。在此基礎(chǔ)上提出一種自適應(yīng)速度調(diào)節(jié)算法,以實(shí)現(xiàn)飛行速度的最優(yōu)控制。

如圖2所示,引入兩個(gè)角度閾值θ1和θ2(θ1≤θ2<Δθmax),當(dāng)無(wú)人機(jī)實(shí)際轉(zhuǎn)向角|Δθprac|小于θ1時(shí),無(wú)人機(jī)可以全力加速至最大速度Vmax;當(dāng)|Δθprac|大于θ2時(shí),無(wú)人機(jī)需要減速至最小速度;當(dāng)|Δθprac|介于θ1和θ2之間時(shí),速度進(jìn)行線(xiàn)性調(diào)節(jié)。

圖2 角度閾值

V=

(11)

2.2 輔助避障力

(12)

引入障礙物差值角Δθobs來(lái)判定探測(cè)到障礙物時(shí)轉(zhuǎn)向的方向,判定原則是更小的轉(zhuǎn)向角代價(jià)。令無(wú)人機(jī)指向障礙物圓心的角度為θobs,定義障礙物差值角Δθobs:

(13)

圖3 輔助避障力

(14)

(15)

(16)

(17)

3 航跡規(guī)劃策略

基于IAPF算法的航跡規(guī)劃流程如圖4所示,其基本步驟如下。

圖4 航跡規(guī)劃策略

步驟 1初始化算法參數(shù)。根據(jù)讀取的地圖信息設(shè)置起始位置pstart、目標(biāo)位置pgoal、障礙物坐標(biāo)Xobs等基本信息;除此之外,根據(jù)需求設(shè)定角度限制算法的最大轉(zhuǎn)向角Δθmax和速度調(diào)節(jié)算法的角度閾值θ1和θ2。

步驟 2計(jì)算當(dāng)前位置p處所受的目標(biāo)點(diǎn)引力Fatt以及Xobs中所有障礙物的斥力Frep,并將二者組合得到合力Fres,得到理論轉(zhuǎn)向角Δθtheory。

步驟 5計(jì)算引入輔助避障力Faux后的最終合力F以及新的理論轉(zhuǎn)向角Δθtheory。

步驟 7計(jì)算無(wú)人機(jī)下一時(shí)刻的飛行位置,如果已到達(dá)終點(diǎn)pgoal則結(jié)束,否則返回步驟2。

4 仿真對(duì)比分析

以上述算法分析為理論基礎(chǔ),本文在處理器為IntelCore i3-7100T,主頻為3.40 GHz,內(nèi)存為8 GHz的計(jì)算機(jī)上通過(guò)Matlab進(jìn)行仿真研究,驗(yàn)證了算法的可行性,并比較了TAPF算法、文獻(xiàn)[31]的A-APF算法與引入物理約束調(diào)節(jié)算法和探測(cè)避障算法的IAPF算法的差異。

將無(wú)人機(jī)視為一個(gè)質(zhì)點(diǎn),黑色實(shí)心圓代表區(qū)域中的障礙物,加號(hào)“+”代表起始點(diǎn),倒三角“▽”代表目標(biāo)點(diǎn)。主要參數(shù)設(shè)置如表1所示。由于TAPF算法通常假定無(wú)人機(jī)勻速飛行,故設(shè)TAPF算法中無(wú)人機(jī)飛行速度為常規(guī)速度Vc,IAPF算法針對(duì)TAPF算法的問(wèn)題所作改進(jìn)如圖5和圖6所示。圖5中由于障礙物恰好位于無(wú)人機(jī)與目標(biāo)點(diǎn)的連線(xiàn)上,所以障礙物產(chǎn)生的斥力與目標(biāo)點(diǎn)引力反向。TAPF算法中無(wú)人機(jī)在靠近障礙物附近陷入局部極小點(diǎn)產(chǎn)生震蕩,無(wú)法到達(dá)目標(biāo)點(diǎn);IAPF算法在探測(cè)距離ddet內(nèi)發(fā)現(xiàn)障礙物時(shí),就會(huì)受到輔助避障力的作用開(kāi)始轉(zhuǎn)向以提前避開(kāi)障礙物,所以無(wú)人機(jī)可以避開(kāi)震蕩點(diǎn)。同時(shí),可以發(fā)現(xiàn)雖然A-APF算法和IAPF算法均能解決局部極小點(diǎn)問(wèn)題,但I(xiàn)APF算法的軌跡更為平滑可行。

表1 主要參數(shù)

圖5 局部極小點(diǎn)

圖6 改進(jìn)算法與傳統(tǒng)算法的對(duì)比

在圖6(a)中可以觀(guān)察到,由于TAPF算法是無(wú)人機(jī)進(jìn)入斥力勢(shì)場(chǎng)后再受到斥力作用開(kāi)始避開(kāi)障礙物,同時(shí)斥力隨距離變化較大,所以無(wú)人機(jī)飛行軌跡不夠平滑;此外,由圖6(b)可知,該軌跡上無(wú)人機(jī)最大轉(zhuǎn)向角為-303.585 4°,航跡總長(zhǎng)度為55.2,飛行時(shí)間為69 s,顯然無(wú)人機(jī)的最大轉(zhuǎn)向角不符合實(shí)際飛行狀態(tài)。而在IAPF算法中,無(wú)人機(jī)受到的輔助避障力根據(jù)飛行情況動(dòng)態(tài)調(diào)整,使無(wú)人機(jī)與斥力勢(shì)場(chǎng)相切飛過(guò),避免無(wú)人機(jī)受到斥力勢(shì)場(chǎng)影響而導(dǎo)致無(wú)人機(jī)軌跡擺動(dòng)變大,并且在繞過(guò)障礙物后,輔助避障力消失,無(wú)人機(jī)繼續(xù)朝著目標(biāo)點(diǎn)飛行,在此軌跡上無(wú)人機(jī)的最大轉(zhuǎn)向角為18.482 4°,航跡總長(zhǎng)度為49.2,飛行時(shí)間為50 s,除了使最大轉(zhuǎn)向角符合無(wú)人機(jī)實(shí)際飛行狀態(tài)之外,在航跡長(zhǎng)度和飛行時(shí)間上都有一定的改善。

為了進(jìn)一步對(duì)IAPF算法的效果進(jìn)行分析,將建模區(qū)域提升到80×80,在區(qū)域內(nèi)隨機(jī)產(chǎn)生多個(gè)障礙物,將起點(diǎn)設(shè)置在(75,75),終點(diǎn)設(shè)置在(2,2),進(jìn)行多次仿真實(shí)驗(yàn)并記錄仿真數(shù)據(jù),相同地圖情況下3種算法的對(duì)比仿真結(jié)果以圖7為例。

圖7 多障礙物情況

從圖7可看出,TAPF算法和A-APF算法的最大轉(zhuǎn)向角均超過(guò)300°,IAPF算法的最大轉(zhuǎn)向角為23.23°,IAPF算法在多障礙物的情況下能夠成功避開(kāi)障礙物且規(guī)劃出的軌跡仍較為平滑。此外,TAPF算法、A-APF算法和IAPF算法的軌跡長(zhǎng)度分別為120.8、111.6和107.2,IAPF算法航程代價(jià)最小。TAPF算法、A-APF算法和IAPF算法的飛行時(shí)間分別為151 s、124 s、117 s。由于TAPF算法和A-APF算法未引入速度參量,故只能設(shè)定為常規(guī)飛行速度勻速飛行,但實(shí)際飛行時(shí),在直線(xiàn)段內(nèi)無(wú)人機(jī)是可以按照最大飛行速度進(jìn)行飛行,IAPF算法考慮了該點(diǎn),故其時(shí)間代價(jià)更小。

多次實(shí)驗(yàn)的仿真數(shù)據(jù)如表2所示。

表2 隨機(jī)多障礙物地圖下算法仿真結(jié)果比較

由于引入了角度限制,可以發(fā)現(xiàn)IAPF算法的最大轉(zhuǎn)向角都限制在了30°,而其他算法的最大轉(zhuǎn)向角都遠(yuǎn)大于IAPF算法;在少數(shù)情況下,IAPF算法的飛行時(shí)間大于A-APF算法,但差距不大。除仿真實(shí)驗(yàn)中TAPF算法的目標(biāo)不可達(dá)情況,IAPF算法相對(duì)于TAPF算法規(guī)劃航跡長(zhǎng)度減少9.63%,飛行時(shí)間減少19.1%,相對(duì)于A-APAF算法規(guī)劃軌跡長(zhǎng)度減少2.52%,飛行時(shí)間減少1.86%。

5 結(jié) 論

TAPF算法進(jìn)行航跡規(guī)劃存在航跡不符合實(shí)際飛行軌跡和局部極小值的問(wèn)題,本文引入角度限制和速度自適應(yīng)調(diào)節(jié)方案,一定程度上平滑了軌跡,減輕了TAPF算法規(guī)劃航跡的擺動(dòng)問(wèn)題;提出輔助避障力來(lái)實(shí)現(xiàn)提前避障,使無(wú)人機(jī)可以在還未到斥力勢(shì)場(chǎng)范圍內(nèi)時(shí)便開(kāi)始轉(zhuǎn)向,以避開(kāi)局部極小點(diǎn)的同時(shí)進(jìn)一步平滑軌跡。Matlab仿真實(shí)驗(yàn)結(jié)果表明,IAPF算法成功地避開(kāi)了局部極小點(diǎn)和震蕩點(diǎn),并且避開(kāi)了TAPF算法中的急轉(zhuǎn)彎處,使無(wú)人機(jī)以很小的轉(zhuǎn)向角代價(jià)避開(kāi)障礙物;在多次相同地圖信息下進(jìn)行仿真,IAPF算法相比于TAPF算法規(guī)劃航跡長(zhǎng)度減少9.63%,飛行時(shí)間減少19.1%,相比于A-APF也有一定的改進(jìn)作用,在最大轉(zhuǎn)向角上尤為突出。