改進人工勢場的無人機避障機動策略

袁佳樂， 劉 蓉， 王 闖

(南京航空航天大學(xué)，a.自動化學(xué)院; b.無人機研究院; c.中小型無人機先進技術(shù)工信部重點實驗室,南京 210000)

0 引言

無人機(Unmanned Aerial Vehicle,UAV)具有輕便靈活、機動性強、隱蔽性好等特點，被廣泛地應(yīng)用于民用和軍事領(lǐng)域[1]。隨著無人機的不斷發(fā)展,其任務(wù)環(huán)境越來越復(fù)雜,對于安全的要求也越來越高,因而無人機的自主避障技術(shù)也逐漸成為關(guān)鍵技術(shù)之一[2-3]。

現(xiàn)有的無人機避障方法主要分為以下兩類:其一，基于航路規(guī)劃的避障方法，此方法的思想主要是將避障問題轉(zhuǎn)化為航路規(guī)劃問題,如遺傳算法[4-5]、人工勢場法[6-7]、A*算法[8-9]；其二， 基于幾何關(guān)系的避障方法，這類方法的主要思想是根據(jù)無人機與障礙物之間的相對距離、速度、加速度、角度等關(guān)鍵信息計算得到規(guī)避航路。如文獻[10]采用橢球體對障礙物建模,根據(jù)橢球體方程與無人機速度矢量計算得到障礙物規(guī)避航點；文獻[11]通過實時檢測無人機與障礙物位置關(guān)系獲取障礙物的特征點,得到安全飛行邊界,無人機沿邊界飛行規(guī)避障礙物?，F(xiàn)有的這兩類方法都有其獨特的優(yōu)點,通過分析出現(xiàn)這兩類方法并不互斥,本文將其結(jié)合起來,以實現(xiàn)優(yōu)勢互補。

為保證任務(wù)執(zhí)行成功率,當(dāng)無人機在飛行過程中遭遇突發(fā)障礙威脅時,需要快速高效地避開障礙物,并盡可能快地回歸到原始期望狀態(tài)。常規(guī)的規(guī)劃式避障算法是設(shè)置避障起始點與避障目標(biāo)點,然后在規(guī)劃空間內(nèi)尋找一條從起始點到目標(biāo)點的最優(yōu)航路,無人機跟蹤規(guī)劃出的避障航路進而完成避障。但設(shè)置起始點與目標(biāo)點本身就相當(dāng)于對避障路徑施加了一種限制,起始點和目標(biāo)點的位置將直接影響避障路徑和總體任務(wù)執(zhí)行的質(zhì)量。

本文設(shè)計的避障機動策略則拋棄起始點和目標(biāo)點,充分利用幾何關(guān)系，引入避障機動角系數(shù)與采用幾何計算得出的最佳避障速度一致項共同改進障礙規(guī)避過程；同時提出虛擬子機的概念,代替?zhèn)鹘y(tǒng)目標(biāo)點,通過虛擬子機實時吸引使無人機盡快回歸原始狀態(tài),對后續(xù)任務(wù)影響較小。

1 避障機動判定條件

無人機在飛行過程中,自身攜帶的傳感器實時檢測周圍環(huán)境,若無人機檢測到突發(fā)障礙物就執(zhí)行避障機動,無疑會增加許多非必要的避障機動行為?；诖?首先給出無人機是否執(zhí)行避障機動的判決條件。

對障礙物模型進行膨化處理，將無人機的半徑加至障礙物上，這樣在避障過程中無人機相對于障礙物可以視為一個質(zhì)點。根據(jù)無人機機動能力等因素，按照經(jīng)驗定義一個避障距離Davoid,如圖1所示。

圖1 無人機避障機動判決示意圖Fig.1 Schematic diagram of UAV obstacle avoidance maneuver judgment

當(dāng)無人機與障礙物的距離大于Davoid時,表明無人機仍有足夠的時間和距離來規(guī)避此障礙物,因此,可以認(rèn)為此障礙物暫時對無人機沒有威脅,不考慮避障;當(dāng)無人機與障礙物的距離小于Davoid時,表明無人機若存在碰撞風(fēng)險,則應(yīng)立即采取避障機動,否則可能會因為機動性能限制導(dǎo)致無人機撞上障礙物。在后一種情況下,進一步利用碰撞錐檢測方法判斷無人機是否存在碰撞風(fēng)險[12]。

如圖1所示,無人機飛行到當(dāng)前位置,其速度為vi,與障礙物的距離為Dio,當(dāng)前速度方向與障礙物的相對角度為η,經(jīng)膨化后障礙物的半徑為ri+ro(ri為無人機的半徑,ro為障礙物的半徑)。考慮臨界情況,當(dāng)無人機剛好與障礙物之間不發(fā)生碰撞時,即無人機飛行航線上與障礙物質(zhì)心的最短距離正好等于障礙物的半徑,這時無人機速度方向與障礙物的相對角度即是無人機對于該障礙物的臨界碰撞角。因此,臨界碰撞角算式為

(1)

當(dāng)無人機速度方向與障礙物的相對角度的絕對值小于臨界碰撞角時,則代表無人機存在碰撞風(fēng)險,反之,不存在碰撞風(fēng)險。

通過以上分析,無人機執(zhí)行避障機動的判決條件為

|η|<ζDio≤Davoid。

(2)

若上述算式成立,則無人機執(zhí)行避障機動;若不成立,則無人機不執(zhí)行避障機動。

2 避障機動控制策略設(shè)計

人工勢場法(APF)簡單、實用且具有較高的實時性,普遍應(yīng)用于避障問題。人工勢場法就是在障礙物周圍設(shè)定虛擬斥力勢場,在目標(biāo)點周圍設(shè)定虛擬引力勢場,當(dāng)無人機處于這兩種勢場的影響范圍之內(nèi)時,引力勢場對無人機產(chǎn)生引力,斥力勢場對無人機產(chǎn)生斥力,無人機在這兩種虛擬力的作用下遠離障礙物并飛向目標(biāo)[13]。

鑒于人工勢場法實時性高、易于實施的特點,本文基于人工勢場法的思想,對其進行改進,設(shè)計無人機避障機動策略。

2.1 幾何改進障礙規(guī)避策略設(shè)計

基于人工勢場的避障方法，障礙物的斥力場是其成功規(guī)避障礙的主要控制量。本文假設(shè)檢測到的突發(fā)障礙物都為球形,對于不規(guī)則的障礙物也用其外接球表示。傳統(tǒng)的斥力勢場函數(shù)為[13]

(3)

式中：C>0,為斥力場系數(shù);ρ為無人機到障礙物表面的距離;ρ0為障礙物o所形成的斥力勢場的影響范圍。則p點處無人機受障礙物的斥力為斥力場的負(fù)梯度,可以表示為

(4)

2.1.1 基于角度改進的時變斥力勢場

由式(4)可知,傳統(tǒng)的人工勢場法中距離是決定勢場力的唯一因素。只要無人機進入障礙物斥力場的影響范圍,障礙物就會對無人機產(chǎn)生斥力的作用,使其遠離障礙物,且隨著距離不斷靠近，無人機受到的斥力也越來越大。

文獻[12]研究了這樣一種情況：當(dāng)無人機前進路徑與障礙物不相交時,即使無人機處在障礙物的斥力勢場范圍內(nèi),也不受斥力影響,避免了多余的避障行為。但這僅是針對不發(fā)生碰撞情景的改進,當(dāng)存在碰撞風(fēng)險時,分析可得,無人機飛行方向與障礙物的相對角度也對無人機的避障有著至關(guān)重要的作用。顯然,無人機越朝著障礙物的中心點飛,其碰撞風(fēng)險越大,需要的避障機動也越大；越偏離障礙物的中心點飛,其碰撞風(fēng)險越小,需要的避障機動也越小。當(dāng)需要避障機動較小但又十分靠近障礙物時,依據(jù)傳統(tǒng)的人工勢場法,必然會產(chǎn)生較大的障礙規(guī)避控制量,就會產(chǎn)生不必要的大機動,進而影響系統(tǒng)穩(wěn)定性以及避障航路的質(zhì)量。因此,可利用角度關(guān)系來改進斥力場,實時調(diào)整障礙物對無人機的斥力,在有效規(guī)避障礙物的同時,又避免無人機做出非必要的避障行為。改進后的斥力可表示為

(5)

2.1.2 基于碰撞錐速度一致的避障速度場

當(dāng)無人機速度方向、障礙物中心以及期望航路方向共線時會引發(fā)無人機來回反復(fù)抖動,這也是人工勢場的弊端之一,大多數(shù)的改進方法是添加輔助目標(biāo)點,但如前文所述,對于局部避障機動而言,添加目標(biāo)點的行為本身就對避障機動行為施加了一種限制,其規(guī)劃出的避障機動策略是被限制后的避障機動策略。

本文從碰撞錐的碰撞預(yù)測方法中得到啟發(fā),考慮加入速度一致力來改進無人機的避障作用力。由碰撞錐方法可知,當(dāng)無人機的飛行速度方向正好位于臨界碰撞角方向時(如圖1中pA,pB方向),無人機剛好能避開障礙物,那么,當(dāng)無人機存在碰撞風(fēng)險時,若能使無人機的飛行速度方向趨于臨界碰撞角方向并保持一致,就可以脫離碰撞危險,且脫離碰撞風(fēng)險的路程也將是最短的。

通過以上分析,根據(jù)速度一致規(guī)則，基于速度的障礙規(guī)避控制力可表示為

(6)

式中：Fv為無人機的速度避障力;kv>0，為速度避障力系數(shù);v為當(dāng)前時刻無人機的速度;vζ為當(dāng)前時刻基于臨界碰撞角的無人機期望避障飛行速度,規(guī)定向右為正,向左為負(fù)。當(dāng)無人機偏障礙物左側(cè)飛行(η<0°)時,取左側(cè)的臨界碰撞角方向；當(dāng)無人機偏障礙物右側(cè)飛行(η>0°)時,取右側(cè)的臨界碰撞角方向；當(dāng)無人機正對障礙物飛行(η=0°)時,本文規(guī)定取右側(cè)的臨界碰撞角方向。

考慮到在避障過程中可能會受通信時延、干擾等不確定因素影響,緊貼著障礙物表面避障,可能會導(dǎo)致避障失敗,因此，添加附加避障距離δ,則臨界避障角變?yōu)?/p>

(7)

式中:ζexp為期望的臨界避障角;δ為考慮通信時延、擾動等不確定因素影響的附加避障距離。

由此,結(jié)合無人機與障礙物相對飛行的偏離方向,無人機的速度避障力可進一步表示為

對這樣的數(shù)字陳小華表示，58到家今天還活著，是因為沒有刻意把一些概念、標(biāo)簽往自己身上去靠，在中國創(chuàng)業(yè)光靠理念不行，必須要務(wù)實、靈活，58到家今天的業(yè)務(wù)，是市場淘汰后活下來的，不是靠規(guī)劃出來的。

(8)

式中：vθpo-ζexp為當(dāng)前時刻無人機正對障礙物中心或偏向右側(cè)飛行情況下的期望避障飛行速度;vθpo+ζexp為當(dāng)前時刻無人機偏向障礙物左側(cè)飛行情況下的期望避障飛行速度;θpo為當(dāng)前時刻無人機與障礙物的相對方位角;ζexp為當(dāng)前時刻無人機相對障礙物的期望臨界避障角。

綜上,障礙規(guī)避力可由基于角度改進人工勢場的距離斥力項和基于避障錐的速度一致項組成，即

Frep=Fd+Fv=

(9)

2.2 狀態(tài)回歸控制策略設(shè)計

一個完整的避障必然包含障礙規(guī)避和狀態(tài)回歸,無人機在上節(jié)設(shè)計的障礙規(guī)避控制力的作用下,引導(dǎo)無人機脫離碰撞風(fēng)險,在無碰撞風(fēng)險的情況下,無人機需要考慮如何快速地回歸到原始期望狀態(tài)繼續(xù)執(zhí)行任務(wù)。本文利用虛擬子機代表無人機無突發(fā)威脅情況下的實時期望位置,無人機在解除碰撞危險后,障礙規(guī)避控制力消失,此時，虛擬子機的實時吸引力起主導(dǎo)作用,使其能盡快回歸到原始期望狀態(tài)。創(chuàng)建虛擬子機來吸引無人機回歸原始航路,不人為增設(shè)目標(biāo)點,而是以無人機原本的期望位置作為實時目標(biāo)點,可以使無人機更快地回歸到原始的狀態(tài),對后續(xù)既定的任務(wù)產(chǎn)生更小的影響。

為使無人機在脫離碰撞風(fēng)險后盡快與虛擬子機保持一致,設(shè)計狀態(tài)回歸控制力為

(10)

顯然,一旦無人機脫離碰撞危險后,虛擬子機就會吸引無人機回歸原始航路,若此時虛擬子機仍在障礙物內(nèi)部的位置,那么此吸引力會使得無人機再次陷入碰撞危險,然后再避障,如此循環(huán)將使無人機產(chǎn)生較大的抖動,甚至導(dǎo)致避障失敗。

為使無人機避障路徑更加平滑,降低避障失敗的概率,本文利用模型預(yù)測調(diào)整狀態(tài)回歸力。假設(shè)t時刻無人機已解除碰撞風(fēng)險,則預(yù)測t～t+1時刻內(nèi)無人機受期望狀態(tài)的一致力影響回歸原始航跡,無人機的加速度為

(11)

假設(shè)一個步長內(nèi)無人機做勻速直線運動,那么t～t+1時刻段內(nèi)無人機的期望速度為

(12)

(13)

式中：比例系數(shù)K>1；ReLU函數(shù)為非線性激活單元f(x)=max(0,x)；ηt～t+1為t～t+1時段內(nèi)無人機速度與障礙物之間的相對角度,即優(yōu)化對象。

圖2 模型預(yù)測求解示意圖Fig.2 Schematic diagram of model prediction process

根據(jù)優(yōu)化后的ηt～t+1可得t～t+1時段內(nèi)無人機的期望飛行速度,再由速度反解出期望的狀態(tài)回歸力。

最終,經(jīng)過模型預(yù)測改進后的無人機狀態(tài)回歸力可表示為

(14)

式中，v0為上一時刻的無人機速度。

無人機在障礙規(guī)避力和狀態(tài)回歸力的共同作用下對突發(fā)障礙物進行規(guī)避機動,整個避障過程的機動策略可由合力表示，即

F=Fatt+Frep。

(15)

3 仿真結(jié)果與分析

以四旋翼六自由度模型作為控制對象驗證本文提出的新型無人機避障機動策略。本文所提的算法針對突發(fā)障礙物,即局部避障,故在仿真驗證時不考慮全局航路,且已知的障礙物(威脅源)不在仿真環(huán)境中列出。

假設(shè)某四旋翼無人機遭遇突發(fā)障礙時在某段預(yù)設(shè)的航路上飛行,飛行速度為1 m/s,俯仰角為0°,偏航角為90°。設(shè)置避障條件如下:避障距離Davoid=10 m;考慮干擾、通信時延等影響,增設(shè)的附加避障距離δ=1 m。設(shè)置障礙規(guī)避項中距離斥力變化系數(shù)c=0.01,速度一致系數(shù)kv=8。設(shè)置α=3,β=6。

3.1 傳統(tǒng)人工勢場法避障效果

傳統(tǒng)人工勢場法的避障俯視效果如圖3所示,可以看出傳統(tǒng)方法存在兩個問題：

圖3 傳統(tǒng)人工勢場法避障效果Fig.3 Result of UAV obstacle avoidance of traditional APF algorithm

1) 設(shè)置目標(biāo)點的行為對避障效果有很大的影響,目標(biāo)點過近會造成目標(biāo)不可達,設(shè)置過遠又會使得無人機避障時間過長,避障路徑浪費,影響后續(xù)既定任務(wù)的執(zhí)行;

2) 只要與障礙物的距離較近，在無人機已完成碰撞危險解脫,或處在碰撞危險較小的情況下,障礙規(guī)避控制策略仍以較大的控制量使無人機遠離障礙物,造成了無人機的過分避障,增加了額外避障路徑。

3.2 改進的避障效果

針對傳統(tǒng)方法存在的問題,首先利用避障機動角度改進障礙物斥力場后的避障俯視效果如圖4所示,顯然相較于傳統(tǒng)人工勢場法,其避障路徑更短,具有一定的改進意義。但在無人機脫離碰撞危險后,避障機動角不存在,導(dǎo)致斥力消失,無人機僅受目標(biāo)點引力作用,容易再次陷入碰撞危險,因此會出現(xiàn)抖動現(xiàn)象。

圖4 基于角度改進斥力場后避障效果Fig.4 Result of UAV obstacle avoidance of improved repulsion field by angle

其次,由虛擬子機取代傳統(tǒng)目標(biāo)點,避障俯視圖效果如圖5所示,在避障初期由于虛擬子機與無人機的位置十分接近,引力幾乎為零,無人機僅受斥力場的作用,會出現(xiàn)回退行為,且因為存在抖動,虛擬子機與無人機在避障過程中距離越來越遠,導(dǎo)致無人機需要較長的時間才能回歸到原始狀態(tài)。

圖5 虛擬子機取代目標(biāo)點后的避障效果Fig.5 Result of UAV obstacle avoidance after the virtual submachine replaced the target point

上述問題表明了進一步改進的必要,隨后加入速度控制項和模型預(yù)測做進一步改進,得到本文最終的避障機動策略,仿真結(jié)果如圖6所示。

圖6 無人機避障機動效果Fig.6 Result of UAV obstacle avoidance strategy

從圖6(a)，6(b)可知,無人機在虛線圓(避障安全距離)外時,即使存在碰撞風(fēng)險,無人機也不采取任何避障機動,繼續(xù)按照預(yù)設(shè)的全局航路飛行;當(dāng)無人機到達避障距離后,依然存在碰撞風(fēng)險,此時無人機做出避障機動,在改進后的人工勢場驅(qū)動下,無人機進行碰撞危險解脫,同時由于虛擬子機實時回歸吸引和模型預(yù)測控制的作用,危險解脫后無人機緊貼障礙物表面飛行,消除了抖動現(xiàn)象,避障航路較為平滑,并在第一時間就回歸到了原始航路,恢復(fù)到了原始期望狀態(tài),對后續(xù)既定任務(wù)影響較小。

圖6(c)給出了無人機在本文所提的避障機動策略下的飛行高度情況,顯然無人機在整個過程中飛行高度始終保持不變,這也符合多數(shù)飛行任務(wù)需要無人機維持一定高度的要求。

因此可知,在這樣避障機動方式作用下,產(chǎn)生的避障航路較短,避障時間較短,且對預(yù)設(shè)的后期任務(wù)影響較小。

將本文避障機動策略應(yīng)用于其余典型情形,避障俯視仿真效果如圖7所示。

圖7 其他典型情形下無人機避障機動效果Fig.7 Result of UAV obstacle avoidance strategy in other typical situations

圖7(a)為無人機飛行方向正對著障礙物中心的情形。此時,若按照傳統(tǒng)的勢場斥力驅(qū)動避障,則因為共線問題,無人機必然會陷入不斷的往復(fù)飛行局面,但本文機動策略中,在避障速度一致項作用下,即使在共線情況下,無人機也能較快、較好地完成避障。

圖7(b)表示雖在無人機預(yù)設(shè)全局航路前方存在突發(fā)障礙物,但預(yù)設(shè)航路與障礙物不相交情形。從圖7(b)可以看出,雖在無人機障礙規(guī)避范圍內(nèi)存在突發(fā)障礙物,但在本文的避障機動策略下無人機沒有采取任何的避障機動行為,可知本文設(shè)計的避障機動策略亦能有效避免無必要的機動行為,間接提高了飛行穩(wěn)定性和任務(wù)成功率。

3.3 不同復(fù)雜任務(wù)環(huán)境下的避障效果

一個良好的避障策略應(yīng)能適用于不同的任務(wù)場景,本節(jié)內(nèi)容將驗證本文所提無人機避障機動策略的通用性。常規(guī)針對復(fù)雜任務(wù)的算法大多將復(fù)雜任務(wù)看作整體來考慮,導(dǎo)致算法的復(fù)用性很差。本文根據(jù)化繁為簡的思想將復(fù)雜問題拆分,設(shè)計出通用的無人機避障機動策略,此策略可作為基礎(chǔ)模塊與整個任務(wù)系統(tǒng)的其他模塊配合,通過對不同任務(wù)場景的不同決策,利用本文避障機動策略可實現(xiàn)不同任務(wù)場景下的避障功能。避障俯視仿真效果如圖8所示。

圖8 不同復(fù)雜任務(wù)情形下無人機避障機動效果Fig.8 Result of UAV obstacle avoidance strategy in different complex mission situations

圖8(a)為無人機在多突發(fā)障礙物情形下的避障效果。仿真結(jié)果表明,利用整個任務(wù)系統(tǒng)的決策單元將本文避障機動策略應(yīng)用于多突發(fā)障礙物的環(huán)境時,無人機能夠較快、較好地完成避障任務(wù)。

圖8(b)則是將本文的避障機動策略應(yīng)用于無人機編隊的協(xié)同避障任務(wù)。仿真結(jié)果表明，本文避障機動策略亦能作為基礎(chǔ)避障策略配合決策單元共同完成無人機編隊的協(xié)同避障。

綜上，本文避障機動策略可以作為通用避障模塊與其他模塊相互配合完成各種不同的復(fù)雜任務(wù),有較高的通用性。

4 結(jié)束語

本文針對無人機在執(zhí)行任務(wù)過程中遭遇突發(fā)障礙的規(guī)避問題展開研究,并考慮算法的通用性,提出了一種新型通用無人機避障機動策略。與傳統(tǒng)依靠起始點和目標(biāo)點的避障方式不同,本文提出的避障策略始終以期望狀態(tài)為目標(biāo),并改進了傳統(tǒng)人工勢場,使無人機在有效避障的基礎(chǔ)上,能夠較快地回歸到原始期望狀態(tài),對后續(xù)任務(wù)的執(zhí)行影響較小。仿真證明了所提避障機動策略的有效性和通用性，后續(xù)工作可將其作為基礎(chǔ)的障礙規(guī)避策略即一個功能模塊與其他算法結(jié)合來完成更復(fù)雜的任務(wù)，如編隊避障等。