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

楊 萌，王 玥

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

0 引言

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

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

1 感知與規避系統

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

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

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

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

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

2 避讓航跡規劃算法設計

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

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

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

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

2.1 人工勢場法的基本原理

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

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

引力勢場函數與斥力勢場函數可分別表示為

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

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

對引力勢場函數和斥力勢場函數求負梯度，可得相應引力函數與斥力函數

飛行器所受合力為

2.2 局部震蕩及其改進

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

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

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

其中，li為步長，最小轉彎半徑可由下式得出

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

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

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

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

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

2.3 威脅區模型優化

避讓航跡規劃過程中的威脅區域來源于航跡預測模塊得出的預測航跡。結合一般飛行器機動性能，得到威脅區的結構如圖6所示，類似于椎體。

圖6 威脅區的結構示意圖Fig.6 Diagram of threat area structure

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

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

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

二維威脅點集z1的分布為

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

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

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

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

2.4 無人機性能約束模型

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

（1）最大航程約束

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

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

則無人機的航跡總長度ltotal應滿足

（2）飛行高度約束

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

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

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

3 仿真驗證

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

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

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

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

圖9 傳統算法的仿真結果（P1＝15）Fig.9 Simulation results of original algorithm （P1＝15）

圖10 傳統算法的仿真結果（圖9曲線右轉一定角度）Fig.10 Simulation results of original algorithm（curve in Fig.9 turns right at a certain angle）

圖11 傳統算法的仿真結果（P1＝30）Fig.11 Simulation results of original algorithm （P1＝30）

圖12 傳統算法的仿真結果（圖11曲線右轉一定角度）Fig.12 Simulation results of original algorithm（curve in Fig.11 turns right at a certain angle）

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

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

圖13 傳統算法的仿真結果（P1＝40）Fig.13 Simulation results of original algorithm （P1＝40）

圖14 傳統算法的仿真結果（圖13曲線右轉一定角度）Fig.14 Simulation results of original algorithm（curve in Fig.13 turns right at a certain angle）

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

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

4 結論

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

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