基于改進人工勢場法的無人機避讓航跡規(guī)劃

楊 萌，王 玥

（北京理工大學機電學院，北京100081）

0 引言

近年來，隨著無人機技術(shù)的不斷發(fā)展，無人機被逐漸應(yīng)用于航空噴灑、航空物探、航空攝影、電力巡線、石油作業(yè)等通航作業(yè)領(lǐng)域。為保障飛行安全，通常將無人機限制在特定空域內(nèi)，與有人機隔離運行。在有限的空域資源下，隔離運行方式將難以滿足無人機、有人機日益增長的應(yīng)用需求，無人機進入低空空域融合運行是我國通用航空事業(yè)發(fā)展的必然趨勢［1］。

在低空空域內(nèi)運行的中小型無人機具有機動能力強、受天氣等因素影響大、活動空間范圍和類型廣泛等特點，融合運行的安全問題嚴峻。其中，如何避免無人機間或無人機與有人機間的空中碰撞沖突，是實現(xiàn)融合運行的一個核心技術(shù)。美國NASA的Ames研究中心將飛行沖突解脫分為3個階段：1）戰(zhàn)略階段，在20min前實現(xiàn)沖突解脫；2）戰(zhàn)術(shù)階段，在2min前實現(xiàn)沖突解脫；3）空中防撞系統(tǒng)（Traffic Alert and Collision Avoidance Sys?tem，TCAS）階段，即在上一階段解脫失敗的情況下，飛行器直接進行垂直機動［2］。對于低空運行的安全保障而言，地面通信、導航和監(jiān)視系統(tǒng)只能有效覆蓋終端區(qū)等管制空域，大部分低空飛行活動并不能獲得管制服務(wù)，且中小型無人機由于成本及體積限制，通常并不搭載TCAS系統(tǒng)。因此，在低空空域內(nèi)運行的中小型無人機必須具備戰(zhàn)術(shù)解脫能力。

1 感知與規(guī)避系統(tǒng)

各國進行的無人機空域安全研究主要可分為陸基感知與規(guī)避、空中感知與規(guī)避2種。

陸基感知與規(guī)避系統(tǒng)利用陸基系統(tǒng)使飛行器在規(guī)定的空域內(nèi)保持一定距離。陸基系統(tǒng)包括雷達、顯示設(shè)備、通信設(shè)備和軟件。陸基感知與規(guī)避的方案將逐漸削減現(xiàn)有自動化系統(tǒng)的限制，幫助無人機實現(xiàn)在國家空域系統(tǒng)內(nèi)的自由飛行，可以很好地適用于大中型無人機系統(tǒng)［3］。

空中感知與規(guī)避系統(tǒng)注重發(fā)展能夠同時進行自動分離與避免碰撞的機載設(shè)備，保障一定范圍內(nèi)的飛行安全。該性能的目的在于使有人機飛行員和無人機操作員能夠在任何空域中通過安全高效的方式避免沖突，避免與其他飛行器發(fā)生碰撞。無人機必須在目標到達危險前進行避讓機動，最終恢復到原飛行軌跡并與入侵機重新保持安全間隔［4］，整個沖突的避讓流程如圖1所示。

本文擬針對感知與規(guī)避實際背景，對避讓航跡規(guī)劃模塊進行算法設(shè)計與模型優(yōu)化，并通過仿真驗證其可靠性與優(yōu)化效率。

圖1 無人機感知與規(guī)避流程Fig.1 Process of sense and avoidance of UAV

2 避讓航跡規(guī)劃算法設(shè)計

目前，無人機在航跡規(guī)劃中使用較多的智能優(yōu)化算法包括A?算法、遺傳算法、蟻群算法、粒子群算法和人工勢場法等。A?算法適合進行全局最優(yōu)航跡規(guī)劃，理論上可以保證全局最優(yōu)解，但搜索空間的增大、內(nèi)部所存儲的數(shù)據(jù)的增加、計算的增大、搜索時間的變長，導致其實時性無法獲得保證［5］。遺傳算法和粒子群算法具有較強的魯棒性，能自適應(yīng)地調(diào)整搜索方向，不受搜索空間條件的約束，但其局部搜索能力低，在進化后期收斂速度慢且容易陷入局部最優(yōu)［6?7］。蟻群算法在對路徑進行搜索時是隨機的，由于信息素揮發(fā)的原因，其可能陷入局部最優(yōu)情況［8］。人工勢場法具有結(jié)構(gòu)簡單、計算量小、實時性好等優(yōu)點，但如果目標距離障礙物太近或斥力與引力相等，則容易陷入局部極小值和局部震蕩［9］。

考慮到感知與規(guī)避系統(tǒng)對高實時性的要求，放棄A?算法、遺傳算法、蟻群算法等搜索算法，采用實時性更強的人工勢場法進行規(guī)避航跡的計算。

文獻［10］將人工勢場法和蟻群算法進行了結(jié)合，通過人工勢場法構(gòu)建了環(huán)境與目標之間的啟發(fā)因子，并將其作為初始信息素帶入蟻群算法，幫助蟻群算法快速收斂航跡。文獻［11］通過搜索未知環(huán)境中的目標點，設(shè)計了一種無死鎖人工勢場法，有效解決了局部極小值問題。文獻［12］設(shè)計了一種新的勢能函數(shù)，避免了計算過程中繁瑣的逆運動學計算，大大降低了算法的復雜程度。文獻［13］在傳統(tǒng)人工勢場法中加入了調(diào)節(jié)因子，同時采用遍歷搜索法解決了局部極小值問題。文獻［14］引入了斥力偏轉(zhuǎn)模型，引導機器人在路徑規(guī)劃時避開局部極小點，并同時通過引入斥力增益系數(shù)函數(shù)，優(yōu)化了航向改變過大的問題。文獻［15］提出了一種虛擬目標法，解決了局部極小值的問題。文獻［16］提出了二次引力勢場法，通過傳統(tǒng)人工勢場法計算出了一條預(yù)規(guī)劃航跡，而后以此路徑所提供的航跡點作為基礎(chǔ)引力點，再次利用人工勢場法規(guī)劃航跡。

針對前文所述的人工勢場法容易陷入局部極小值的問題，國內(nèi)外諸多學者已進行了多項研究，并提出了多種改進方法，本文不再贅述。以下將結(jié)合感知與規(guī)避問題的實際背景，給出人工勢場法局部震蕩問題的解決方案，并對威脅區(qū)模型進行優(yōu)化。

2.1 人工勢場法的基本原理

人工勢場法的基本思想是將飛行器在規(guī)劃空間中的運動看作一種在虛擬力場中的受力運動，障礙物或威脅區(qū)域?qū)ζ洚a(chǎn)生排斥力，目標點對其產(chǎn)生吸引力，飛行器在合力的作用下朝著目標點運動［17］。人工勢場法航跡規(guī)劃的二維示意圖如圖2所示。

圖2 人工勢場法航跡規(guī)劃的二維示意圖Fig.2 Two-dimensional diagram of artificial potential field method

引力勢場函數(shù)與斥力勢場函數(shù)可分別表示為

其中，X=（x，y，z）為飛行器的位置向量，α和β分別為引力與斥力增益系數(shù)，X、Xg、Xo分別表示當前點、 目標點、 障礙物的空間位置，ρ(X，Xg)、ρ(X，Xo)分別為當前點到目標點和障礙物的空間距離，ρ0為障礙物的斥力作用范圍。

對于二元及以上函數(shù)而言，某一點方向?qū)?shù)的方向可以有無數(shù)個。所有方向?qū)?shù)中存在一個最大值，其方向為梯度的方向，方向?qū)?shù)最大值的大小為梯度的模。

對引力勢場函數(shù)和斥力勢場函數(shù)求負梯度，可得相應(yīng)引力函數(shù)與斥力函數(shù)

飛行器所受合力為

2.2 局部震蕩及其改進

在利用人工勢場法進行航跡規(guī)劃時，其規(guī)劃航跡有可能出現(xiàn)不必要的震蕩現(xiàn)象。主要原因是在某一航點處引力大于斥力，而在下一航點處斥力大于引力，兩種情況多次循環(huán)導致航跡來回震蕩。如圖3所示，由式（3）和式（4）可知，距離目標點d越遠的點所受引力越大，距離威脅點z越遠的點所受斥力越小。在規(guī)劃平面內(nèi)，必然有1條斥力與引力大小相等（方向不一定相反）的平衡線L。當前點mi所受到的引力大于斥力，則其必然向上前進1個步長l，得到航點mi+1。此時，點mi+1可能越過平衡線L，使其受到的引力小于斥力。向遠離威脅點方向規(guī)劃，得到航點mi+2。點mi+2將再次越過平衡線，以此類推，形成局部震蕩。

圖3 局部震蕩示意圖Fig.3 Diagram of local oscillation

針對感知與規(guī)避實際背景，可在航跡規(guī)劃時加入無人機最大轉(zhuǎn)彎角約束。由于無人機自身動力學性能的影響，無人機飛行方向的改變受到最大轉(zhuǎn)彎角（或最小轉(zhuǎn)彎半徑）的約束。設(shè)無人機最大轉(zhuǎn)彎角為θmax，最小轉(zhuǎn)彎半徑為Rmin，無人機飛行的轉(zhuǎn)彎角為α，轉(zhuǎn)彎半徑為r，要求規(guī)劃出的航跡中的每一點的曲率半徑都必須大于Rmin，則約束表達式為

其中，li為步長，最小轉(zhuǎn)彎半徑可由下式得出

其中，vmin為無人機的最小飛行速度，Nymax為無人機最大法向牽引力。

對于加入最大轉(zhuǎn)彎角約束的人工勢場法，當規(guī)劃到圖3所示的斥力與引力的平衡線附近的點mi+1時，受最大轉(zhuǎn)彎角θmax的約束，其下一點不會做出類似圖中點mi+2的大機動，可有效減緩或消除局部震蕩影響，其改進程度受最大轉(zhuǎn)彎角θmax大小的影響。

未進行局部震蕩改進的仿真結(jié)果如圖4所示。其中，起始點為A（0，0，0），目標點為B（100，100，100），威脅點為C（50，50，50），規(guī)劃步長l=1，目標點引力增益系數(shù)α=0.1，威脅點斥力增益系數(shù)β=500，威脅區(qū)勢場范圍P=35。加入最大轉(zhuǎn)彎角約束的仿真結(jié)果如圖5所示。其中，起始點為A′（0，0，0），目標點為B′（100，100，100），障礙點為C′（50，50，50），規(guī)劃步長l=1，目標點引力增益系數(shù)α=0.1，威脅點斥力增益系數(shù)β=500，威脅區(qū)勢場范圍P=35，最大轉(zhuǎn)彎角θmax=45°。

圖4 原算法的仿真結(jié)果Fig.4 Simulation results of original algorithm

圖5 改進后的算法仿真結(jié)果Fig.5 Simulation results of improved algorithm

2.3 威脅區(qū)模型優(yōu)化

避讓航跡規(guī)劃過程中的威脅區(qū)域來源于航跡預(yù)測模塊得出的預(yù)測航跡。結(jié)合一般飛行器機動性能，得到威脅區(qū)的結(jié)構(gòu)如圖6所示，類似于椎體。

圖6 威脅區(qū)的結(jié)構(gòu)示意圖Fig.6 Diagram of threat area structure

在感知與規(guī)避實際背景下，在利用人工勢場法進行航跡規(guī)劃的過程中，其威脅區(qū)勢場模型若采用傳統(tǒng)的以威脅區(qū)域中心為圓心構(gòu)建球體勢場的方法，容易出現(xiàn)因規(guī)劃空間過小而導致的規(guī)劃航跡與實際最優(yōu)解偏差過大的問題，進而造成空域和能量資源的浪費。

為此，本文采用將威脅區(qū)域結(jié)構(gòu)模型進行分解的方法構(gòu)建威脅區(qū)勢場模型。將威脅區(qū)域Z非均勻離散為多個威脅點｛z0，z1，…，zmax｝，則越靠近航跡預(yù)測點的區(qū)域威脅點分布越密集。傳感器最終探測點在XY平面的投影為z0（xz0，yz0），Kalman濾波預(yù)測Δt時刻后航點在XY平面的投影為zk（xzk，yzk）。飛行器從最終探測點開始，以最大偏航角θ′max向Y軸正向飛行Δt時刻后在XY平面的投影為zm（xzm，yzm），則有

最大偏航點zm和最終探測點z0的連線與X軸的夾角為θm0，航跡預(yù)測點zk和最大偏航點zm的連線與X軸的夾角為θkm，則有

二維威脅點集z1的分布為

其中，p為密集因子，p越大則航跡預(yù)測點zk附近的威脅點越密集。

改進后的威脅點分布如圖7所示。威脅區(qū)結(jié)構(gòu)類似于椎體結(jié)構(gòu)，圖中的各條線構(gòu)成了整體威脅區(qū)結(jié)構(gòu)的模型輪廓，椎體底面各線匯集于航跡預(yù)測模塊給出的預(yù)測點。作為入侵飛行器的高概率出現(xiàn)點，其周圍威脅點分布密度較高。在遠離預(yù)測點的區(qū)域，威脅點分布密度越來越低，在錐頂處達到最低。

每個威脅點zi單獨形成斥力勢場Urepi與斥力Frepi，飛行器所受斥力合力為所有斥力的疊加，則有

圖7 威脅點分布仿真圖Fig.7 Simulation of threat points distribution

2.4 無人機性能約束模型

在利用人工勢場法進行避讓航跡規(guī)劃的過程中，所規(guī)劃航跡必須滿足無人機自身性能的約束。除上述最大轉(zhuǎn)彎角約束之外，還有最大航程約束、飛行高度約束和最大爬升/俯沖角約束，具體模型如下。

（1）最大航程約束

由于攜帶的動力能源有限，無人機面臨著最大飛行距離的約束，即最大航程約束，這要求避讓航跡長度lavoid與其余段航跡長度lbefore的和必須小于無人機能夠飛行的最大航程值lmax。 設(shè)每2個航點之間的航跡段長度為li，航跡總長度的示意圖如圖8所示。

圖8 航跡總長度示意圖Fig.8 Schematic diagram of total track length

則無人機的航跡總長度ltotal應(yīng)滿足

（2）飛行高度約束

我國通用航空管理局對各類無人機的飛行活動制定了飛行高度限制，設(shè)無人機飛行高度的約束區(qū)間為［Hmin，Hmax］，則避讓航跡中各航點的高度h需滿足

（3）最大爬升/俯沖角約束

最大爬升/俯沖角是指無人機在三維空間中改變其飛行高度時可機動達到的最大角度，角度過大可能會引起失速。設(shè)最大爬升/俯沖角為γmax，無人機實際爬升/俯沖角為φ，當前航跡節(jié)點的坐標 為 (xi，yi，zi)，下 一 航 跡 節(jié) 點 的 坐 標 為(xi+1，yi+1，zi+1)，則約束滿足下式

3 仿真驗證

根據(jù)上述威脅區(qū)模型的優(yōu)化方法，對感知與規(guī)避過程中的避讓航跡規(guī)劃進行仿真驗證，并將其與改進之前的算法進行對比。其中，起始點、目標點、威脅點、引力增益系數(shù)、斥力增益系數(shù)固定不變，僅對威脅區(qū)勢場范圍進行修改，并比較兩種算法的仿真結(jié)果。

假設(shè)我方無人機于t時刻發(fā)現(xiàn)入侵飛行器，以此時我方無人機的位置作為原點建立直角坐標系。 威脅區(qū)域中心點的坐標為（xobs，yobs，zobs），威脅區(qū)勢場范圍為P1，以此構(gòu)造威脅區(qū)模型，進行基于傳統(tǒng)人工勢場的避讓航跡規(guī)劃。入侵飛行器航跡預(yù)測點的坐標為（xforecast，yforecast，zforecast），威脅點zi的自坐標為（xobsi，yobsi，zobsi），威脅點的勢場范圍為P1，以此構(gòu)造改進威脅區(qū)模型，并進行改進后的避讓航跡規(guī)劃。

為清晰觀察傳統(tǒng)算法的規(guī)劃結(jié)果是否成功繞開了威脅區(qū)域，需在傳統(tǒng)算法避讓航跡規(guī)劃的仿真結(jié)果中加入威脅點，以顯示威脅區(qū)域，且不將其作為障礙點代入計算。傳統(tǒng)避讓航跡規(guī)劃仿真參數(shù)的設(shè)置為：規(guī)劃步長l=1，目標點引力增益系數(shù)α=0.1，威脅點斥力增益系數(shù)β=500，威脅區(qū)域中心（79，50，70）。當威脅區(qū)勢場范圍P1=15時，得出的仿真結(jié)果如圖9、圖10所示。航跡從威脅區(qū)穿過，未成功規(guī)避威脅區(qū)域。

保持其他條件不變，擴大威脅區(qū)勢場范圍，使規(guī)劃航點提前受斥力作用。當威脅區(qū)勢場范圍P1=30時，得出的仿真結(jié)果如圖11、圖12所示。航跡從威脅區(qū)穿過，未成功規(guī)避威脅區(qū)域。

圖9 傳統(tǒng)算法的仿真結(jié)果（P1＝15）Fig.9 Simulation results of original algorithm （P1＝15）

圖10 傳統(tǒng)算法的仿真結(jié)果（圖9曲線右轉(zhuǎn)一定角度）Fig.10 Simulation results of original algorithm（curve in Fig.9 turns right at a certain angle）

圖11 傳統(tǒng)算法的仿真結(jié)果（P1＝30）Fig.11 Simulation results of original algorithm （P1＝30）

圖12 傳統(tǒng)算法的仿真結(jié)果（圖11曲線右轉(zhuǎn)一定角度）Fig.12 Simulation results of original algorithm（curve in Fig.11 turns right at a certain angle）

保持其他條件不變，再次擴大威脅區(qū)勢場范圍，使規(guī)劃航點提前受斥力作用。當威脅區(qū)勢場范圍P1=40時，得出的仿真結(jié)果如圖13、圖14所示。航跡未從威脅區(qū)穿過，成功規(guī)避威脅區(qū)域，此時總航點數(shù)為263。

改進后的避讓航跡規(guī)劃仿真參數(shù)的設(shè)置為：規(guī)劃步長l=1，威脅區(qū)勢場范圍P2=15，目標點引力增益系數(shù)α=0.1，威脅點斥力增益系數(shù)β=500。 得出的仿真結(jié)果如圖15、圖16所示，成功規(guī)避威脅區(qū)域，總航點數(shù)為219個。

圖13 傳統(tǒng)算法的仿真結(jié)果（P1＝40）Fig.13 Simulation results of original algorithm （P1＝40）

圖14 傳統(tǒng)算法的仿真結(jié)果（圖13曲線右轉(zhuǎn)一定角度）Fig.14 Simulation results of original algorithm（curve in Fig.13 turns right at a certain angle）

圖15 改進算法的仿真結(jié)果Fig.15 Simulation results of improved algorithm

圖16 改進算法的仿真結(jié)果（圖15曲線右轉(zhuǎn)一定角度）Fig.16 Simulation results of improved algorithm（curve in Fig.15 turns right at a certain angle）

4 結(jié)論

在相同規(guī)劃步長的條件下，避讓航跡總航點數(shù)可直接反映航跡總長度，影響無人機動力能源的消耗。由上文可知，在感知與規(guī)避實際背景下，本文提出的改進人工勢場法成功避讓航跡的總航點數(shù)較傳統(tǒng)人工勢場法減少了16.7%。在節(jié)約了動力能源的同時，無人機的避讓飛行耗時也有所減少，提高了任務(wù)執(zhí)行效率。

雖然基于人工勢場法的航跡規(guī)劃實時性高，但本文采用的威脅區(qū)模型優(yōu)化的改進人工勢場方法擴大了威脅勢場的數(shù)量，增加了計算量，其實時性也會成為未來實物實驗中的隱患，日后需在算法效率方面繼續(xù)改進。