一種改進(jìn)的動力學(xué)約束人工勢場法

韓知玖, 吳文江,李孝偉, 張 丹,李春欣

(1.上海大學(xué)上海市應(yīng)用數(shù)學(xué)和力學(xué)研究所,上海200072;2.中國艦船研究設(shè)計(jì)中心,武漢430064;3.上海大學(xué)機(jī)電工程與自動化學(xué)院,上海200444;4.上海大學(xué)計(jì)算機(jī)工程與科學(xué)學(xué)院,上海200444)

隨著無人戰(zhàn)斗機(jī)在現(xiàn)代戰(zhàn)爭中扮演越來越重要的角色,路徑規(guī)劃技術(shù)受到了國內(nèi)外學(xué)者越來越廣泛的關(guān)注.路徑規(guī)劃要在地形、威脅、任務(wù)執(zhí)行的時(shí)間和協(xié)同等約束條件下,實(shí)現(xiàn)在任務(wù)空間的最優(yōu)路徑,保證無人機(jī)(unmanned aerial vehicle,UAV)順利到達(dá)目標(biāo)[1].

常用搜索最短路徑的方法是A*(A-Star)算法和稀疏A*算法,二者均基于柵格化的規(guī)劃空間模型,通過啟發(fā)式信息估算下一步前進(jìn)的最優(yōu)節(jié)點(diǎn)位置,尋找最優(yōu)路線[2].但是,這類方法過于依賴環(huán)境信息,并行性差,計(jì)算量大.基于柵格模型,還有各類智能優(yōu)化算法:Nikolos等[3-4]采用結(jié)合了差分進(jìn)化的遺傳算法規(guī)劃無人機(jī)路徑,求解速度較快且可以獲得多個(gè)可行解;Foo等[5]通過粒子群算法求解多無人機(jī)的協(xié)同三維路徑規(guī)劃問題,效率比遺傳算法更高;Roberge等[6]對比分析了遺傳算法與粒子群算法,并將二者的優(yōu)點(diǎn)結(jié)合后應(yīng)用到多無人機(jī)的協(xié)同路徑規(guī)劃中,獲得了一定的可行解.另外,蟻群算法[7]、人工免疫算法[8]和神經(jīng)網(wǎng)絡(luò)[9]等智能優(yōu)化算法,由于具有良好的并行能力,廣泛應(yīng)用于求解路徑規(guī)劃這類計(jì)算量較大的問題.

概率圖法是將要規(guī)劃的空間根據(jù)約束條件簡化成路網(wǎng)圖后進(jìn)行圖搜索的一種路徑規(guī)劃方法.常用的概率圖算法中,應(yīng)用于無人機(jī)路徑規(guī)劃較多的是Voronoi圖法[10],該方法將規(guī)劃平面的障礙物作為特征元素按照最鄰近原則劃分后,由連接兩鄰點(diǎn)直線的垂直平分線組成的連續(xù)多邊形的網(wǎng)格圖.Voronoi圖具有計(jì)算量小、構(gòu)造時(shí)間快和路徑較安全等優(yōu)點(diǎn)[11],但是由于難以在三維空間中選取特征元素,幾乎無法應(yīng)用于三維的路徑規(guī)劃.

除了上述基于柵格化模型和路網(wǎng)圖模型的規(guī)劃方法之外,1986年Khatib[12]提出了一種比較獨(dú)特的路徑規(guī)劃方法——人工勢場法.人工勢場法既不是將規(guī)劃空間劃分為柵格,也不是將規(guī)劃空間簡化成路網(wǎng)圖,而是將整個(gè)規(guī)劃空間視為一種虛擬的勢場,將無人機(jī)在戰(zhàn)場環(huán)境中的運(yùn)動抽象為虛擬勢場中的運(yùn)動,即假設(shè)目標(biāo)點(diǎn)對無人機(jī)產(chǎn)生“引力”,障礙物對無人機(jī)產(chǎn)生“斥力”,最后通過合力引導(dǎo)無人機(jī)向勢能下降最快的方向前進(jìn).采用人工勢場法規(guī)劃的路徑比其他基于柵格模型和路網(wǎng)圖模型計(jì)算的路徑更加平滑,計(jì)算速度更快,更加適合無人機(jī)的路徑規(guī)劃.但是,這種方法有一個(gè)致命缺陷,即容易陷入引力和斥力相等的局部極值點(diǎn),使得算法停滯,無法順利求解出路徑[13].

本工作針對傳統(tǒng)的人工勢場(artificial potential field,APF)路徑規(guī)劃算法存在局部極小值問題,通過引入虛擬力進(jìn)行改進(jìn),使該算法在考慮控制對象動力運(yùn)動約束條件下能夠快速跳出局部極值.

1 傳統(tǒng)人工勢場法

1.1 算法模型

人工勢場法的實(shí)質(zhì)是對無人機(jī)所在區(qū)域人為地定義勢場,該勢場為地圖中的障礙物斥力場和目標(biāo)點(diǎn)引力場的疊加[12].假設(shè)無人機(jī)的位置為q=(x,y,z),則目標(biāo)點(diǎn)與無人機(jī)的引力場函數(shù)為

式中,ξ為引力勢場增益系數(shù),qg為目標(biāo)點(diǎn)的位置.

因此,由式(1)可知引力為

定義第i個(gè)障礙物對無人機(jī)的斥力勢場為

式中,η為斥力勢場增益系數(shù),qoi表示第i個(gè)障礙物的位置,doi表示第i個(gè)障礙物的影響半徑.

n個(gè)障礙物形成的總的斥力勢場為

因此,無人機(jī)受到的總斥力可為

總勢場為引力勢場和斥力勢場的疊加,即有

因此,無人機(jī)受到的作用力為

由式(7)可以推斷無人機(jī)的下一步運(yùn)動軌跡.

1.2 局部極值問題

采用人工勢場法規(guī)劃的路徑較為平滑安全,但在某些特殊點(diǎn)處,引力和斥力剛好大小相等、方向相反,此時(shí)無人機(jī)容易陷入局部震蕩的狀況[14].如圖1所示,當(dāng)障礙物在無人機(jī)和目標(biāo)點(diǎn)之間時(shí),人工勢場中會形成一個(gè)合力為0或接近0的局部極小值點(diǎn),無人機(jī)將在此停止不動或者在附近來回震蕩,無法避開障礙物.此時(shí)算法停滯,路徑規(guī)劃失敗.

圖1 局部極小值點(diǎn)受力示意圖Fig.1 Diagram of resultant force at local minima point

考慮到局部極值導(dǎo)致算法停滯的根本原因是受力平衡,本工作提出了一種改進(jìn)的人工勢場法,即當(dāng)算法陷入停滯時(shí),在人工勢場中引入一個(gè)作用在無人機(jī)上的虛擬力Fs,其方向與斥力垂直,并偏向目標(biāo)一側(cè),如圖2所示.

圖2 作用在無人機(jī)上的虛擬力Fig.2 Swerving force on UAV

虛擬力的大小為

式中,f為虛擬力的增益系數(shù).為了使無人機(jī)能夠順利地從凹陷的障礙物中退出來,增益系數(shù)需要大于1,因此在本工作中取為2.

虛擬力的方向可以表示為

式中,Fr(q)是所有在影響范圍內(nèi)的障礙物的總斥力.當(dāng)引力與斥力在同一條直線上時(shí),即Fs(q)Fa(q)=0時(shí),在垂直于斥力的兩個(gè)方向中隨機(jī)選擇一個(gè)作為虛擬力的方向.

為了驗(yàn)證改進(jìn)的人工勢場法可以解決局部極值問題,并能夠求解出兩點(diǎn)之間的可行路徑,本工作在2.2 GHz四核處理器、8 GB內(nèi)存、操作系統(tǒng)為Windows10的電腦上通過Matlab R2013a模擬仿真了多種地形情況.人工勢場法中的基本參數(shù)如表1所示,其中Do為引力閾值,l為無人機(jī)步長.

表1 人工勢場法的參數(shù)設(shè)置Table 1 Parameter values of APF algorithm

本工作中路徑的起點(diǎn)設(shè)為(10,50),終點(diǎn)設(shè)為(90,50),通過障礙圓模擬地形中的障礙,其圓心分別為(70,50),(50,35),(50,65),障礙圓半徑為5,如圖3(a)所示.從圖3(a)中可以看出,在由障礙圓組成的地形約束中,傳統(tǒng)的人工勢場法陷入局部極值困境,無法得到可行路徑,而改進(jìn)后的人工勢場法能夠成功跳出局部極值,并成功求解得到從障礙物之間穿過的較短路徑.

將多個(gè)障礙圓疊在一起用來模擬更加復(fù)雜的地形.傳統(tǒng)人工勢場法和改進(jìn)人工勢場法的仿真結(jié)果如圖3(b)和(c)所示.可以看出:傳統(tǒng)的人工勢場法在兩種情況下均遇到了局部極值問題,沒有能夠求解出路徑;而改進(jìn)的人工勢場法均成功解決了局部極值問題,順利完成了兩種地形下的路徑規(guī)劃.

圖3 傳統(tǒng)人工勢場法與改進(jìn)人工勢場法的路徑規(guī)劃Fig.3 Path planning using traditional artificial potential field method and improved artificial potential field method

通過以上算例可以驗(yàn)證:在人工勢場中引進(jìn)虛擬力的方法能夠順利找到兩點(diǎn)之間的路徑,使無人機(jī)成功逃離局部極小值點(diǎn),繞開障礙物,抵達(dá)目標(biāo)點(diǎn);在面對復(fù)雜的戰(zhàn)場情況時(shí),改進(jìn)的人工勢場算法也有較好的表現(xiàn).

2 動力約束條件分析

考慮到路徑規(guī)劃的結(jié)果將用于引導(dǎo)無人機(jī)的實(shí)際飛行,而實(shí)際上固定翼無人戰(zhàn)斗機(jī)的運(yùn)動方式,與小型的旋翼無人機(jī)或者無人艇、無人駕駛車輛等不同,具有特殊性.因此,本工作針對無人機(jī)動力學(xué)性能進(jìn)行了簡單的分析.

在實(shí)際飛行過程中,隨著燃料的不斷消耗,無人機(jī)的質(zhì)量并非保持不變.另外,無人機(jī)的結(jié)構(gòu)也存在彈性形變.當(dāng)戰(zhàn)場的尺度較大時(shí),整個(gè)戰(zhàn)場的重力加速度也可能存在變化,甚至地球的自轉(zhuǎn)對無人機(jī)的飛行也會有一定影響.對于路徑規(guī)劃問題,將這些因素全部考慮并建立精確的無人機(jī)動力學(xué)模型是沒有必要的,因此,本工作忽略一些次要條件,將無人機(jī)近似為質(zhì)量不變的剛體模型,并且不考慮重力加速度的變化,同時(shí)假設(shè)無人機(jī)在轉(zhuǎn)彎時(shí)做協(xié)調(diào)轉(zhuǎn)彎運(yùn)動,即轉(zhuǎn)彎過程中無人機(jī)偏航角θ持續(xù)改變而側(cè)滑角(無人機(jī)速度矢量和縱向?qū)ΨQ平面的夾角)始終保持為0.

無人機(jī)協(xié)調(diào)轉(zhuǎn)彎時(shí)的受力分析如圖4所示.由圖4(a)可知,無人機(jī)豎直方向上的力只有升力的豎直分力和重力.因此,假設(shè)滾轉(zhuǎn)角為φ,若保持無人機(jī)的高度不變,則有

如圖4(b)所示,無人機(jī)的向心力由升力的水平分量提供,即

圖4 無人機(jī)協(xié)調(diào)轉(zhuǎn)彎時(shí)的受力分析Fig.4 Coordinated turning force of UAV

轉(zhuǎn)彎過程中無人機(jī)的過載可以表示為

轉(zhuǎn)彎半徑可表示為

3 改進(jìn)動力約束人工勢場法

改進(jìn)的人工勢場法雖然能成功地求解兩點(diǎn)之間的路徑,但是路徑在障礙物附近存在比較尖銳的拐角,實(shí)際飛行過程中無人機(jī)無法按照該路徑飛行,因此需要對人工勢場法進(jìn)行優(yōu)化.

根據(jù)無人機(jī)轉(zhuǎn)彎時(shí)的受力分析可以看出,無人機(jī)由于其性能的限制,飛行時(shí)轉(zhuǎn)彎半徑不能無限小,因此與之相對應(yīng)的路徑也需要限制曲線的曲率半徑.圖5為相鄰路徑點(diǎn)的角度差.若限制路徑的最小曲率半徑(即無人機(jī)的最小轉(zhuǎn)彎半徑)為rmin,則相鄰的3個(gè)路徑點(diǎn)之間的最大角度差為

當(dāng)?θi>?θmax時(shí),令?θi=?θmax,以此來限制路徑的曲率半徑,使其更加符合無人機(jī)的力學(xué)性能.

圖5 相鄰路徑點(diǎn)的角度差Fig.5 Angle difference between adjacent path points

為了能夠通過仿真算例看出此限制方法的具體效果,假設(shè)無人機(jī)的最小轉(zhuǎn)彎半徑為5(與障礙物半徑一致),即角度差限制為0.04.將優(yōu)化后的路徑與圖3中未優(yōu)化的路徑進(jìn)行對比,結(jié)果如圖6所示.從圖6中可以看出,增加了角度差限制的路徑在彎曲時(shí)比沒有增加限制的路徑更加圓滑,更加符合無人機(jī)的實(shí)際性能.雖然優(yōu)化后的路徑距離障礙物更近,犧牲了一定的安全性,但此時(shí)尚在安全范圍之內(nèi),并且人工勢場法的優(yōu)點(diǎn)之一就是可以通過調(diào)節(jié)障礙物的斥力影響范圍來隨意調(diào)節(jié)路徑點(diǎn)與障礙物之間的最小距離.

為了進(jìn)一步驗(yàn)證此方法的可行性,通過障礙圓模擬更加復(fù)雜和接近實(shí)際的地形,并通過增加角度差限制后的人工勢場法求解路徑.由于設(shè)置的障礙物中,兩個(gè)障礙物之間的最小距離約為12,為了使路徑能順利地從障礙物之間穿過,障礙物的影響半徑do調(diào)至6.仿真結(jié)果如圖7所示.由圖7可以看出,在通過障礙圓模擬的復(fù)雜地形下,增加了角度差限制的改進(jìn)人工勢場法仍然具有較好的效果,能夠順利求解出兩點(diǎn)之間的較短路徑,并且同時(shí)兼顧了圓滑和安全性.這表明此種改進(jìn)人工勢場法完全可以用于固定翼無人戰(zhàn)斗機(jī)的路徑規(guī)劃.

4 結(jié)束語

本工作對傳統(tǒng)人工勢場法的不足進(jìn)行了分析,并重點(diǎn)研究了局部極小值的問題,通過在人工勢場中引進(jìn)虛擬力的方法,使算法能夠成功求解出各種復(fù)雜障礙下的最優(yōu)路徑.另外,對無人機(jī)的動力學(xué)性能進(jìn)行分析,在求解過程中添加角度差限制,通過調(diào)節(jié)斥力影響半徑,使路徑能夠兼顧距離的長短、曲線的平滑和安全性.仿真實(shí)驗(yàn)驗(yàn)證了改進(jìn)的動力約束人工勢場法的有效性,可用于固定翼無人戰(zhàn)斗機(jī)的路徑規(guī)劃.

圖6 具有限制和沒有限制的人工勢場法路徑規(guī)劃對比Fig.6 Simulations of path planning using APF method with and without constraint

圖7 真實(shí)地形下具有限制的人工勢場法路徑規(guī)劃仿真Fig.7 Simulation of path planning using constrained APF method under real terrain