多源性能約束的無人機可靠動態航路規劃算法

孫淑光，朱曉文

（1.中國民航大學 電子信息與自動化學院，天津 300300；2.中國民航大學 交通科學與工程學院，天津 300300）

隨著無人機在航空物流運輸、應急救援、森林防護以及農漁業作業等領域的廣泛應用，低空空域交通流量快速增加，無人機的航路規劃及運行安全日益成為人們關注的重點，有效的機載端動態航路規劃是實現無人機自主運行安全的關鍵[1-4]。十四五期間，中國民航將以安全為要，推動無人機基于任務的自主航路規劃能力和自主智能化復雜任務完成能力的實現。提高空中交通自適應、自診斷、自決策、重規劃能力，實現交通管理全過程的數字化與自動化[5-9]。

現有的無人機航路規劃方法中，大都是將無人機看作一個已知精確位置的質點進行航路規劃，不考慮無人機機載導航系統的定位誤差，對于運行于復雜低空空域，如城市建筑峽谷、復雜山區等區域的無人機來說，存在極大的安全隱患[10-12]?，F有的無人機導航系統大多采用普通精度慣性測量單元/全球定位系統（Inertial Measurement Unit/Global Positioning System,IMU/GPS）導航模塊來確定無人機的位置、速度和姿態信息。受無人機周圍建筑物或地形的遮擋，機載導航系統極易處于衛星信號拒止的環境，接收的衛星信號數量大大減少，幾何精度因子急劇增大，出現較大的定位誤差及位置漂移[13-15]，無人機與空域障礙物之間的實際安全容限急劇下降，會導致所規劃的航路不可行，或者即使航路可行，但與周圍障礙物存在極大的碰撞風險，影響規劃航路的可行性和安全性。

為提高規劃航路的可行性和安全性，本文提出一種敏感無人機機載導航系統實際性能及無人機機動性能的動態航路規劃算法，將無人機航路規劃由基于等效質點的規劃轉變為基于多源性能約束的敏感區域路徑規劃，確保所規劃航路的安全性和可行性。

1 無人機航路規劃系統架構及約束條件分析

1.1 系統架構

根據無人機的任務需求確定航路的起始點和目的地，利用機載地形數據庫信息建立三維地形網格，將機載導航系統的實際導航性能和無人機機動性能作為航路規劃的約束條件，根據機載導航系統提供的無人機實時位置信息動態更新航路規劃的起始點，實現機載端的動態航路規劃。系統整體架構如圖1 所示。

圖1 無人機動態航路規劃系統架構Fig.1 UAV dynamic flight path planning architecture

1.2 航路規劃約束條件

1.2.1 機載導航系統導航性能約束

利用機載導航系統實時評估自身實際導航性能，計算導航系統定位誤差包括，也即導航系統位置不確定度（Estimated Position Uncertainty,EPU），并將其與無人機機動性能一起作為航路規劃的搜索代價函數來約束航路規劃，EPU為機載導航系統的總系統誤差。

在充分考慮機載導航系統定位誤差及無人機機動性能的基礎上進行航路規劃，可以有效提高所規劃航路的安全性。航路規劃中，計算當前節點與臨近障礙物之間的距離，與EPU 相比較確定安全節點。當節點與障礙物之間距離的最小值大于EPU 時，該節點被定義為安全節點。

1.2.2 無人機機動性能約束

無人機自身的機動性能是制約航路規劃的另一關鍵要素，直接關系到所規劃航路的可行性，需要在航路規劃的過程中將其作為重要的判定依據。影響規劃航路可行性的無人機機動性能包括無人機的最大轉彎角度和飛行高度。

1）最大轉彎角約束

實際飛行中，受無人機空氣動力學特性及飛行控制系統能力所限，無人機尤其是固定翼無人機，規劃航路的轉彎角度最大不能超過無人機可能實現的最大允許轉彎角。

航路規劃中，將無人機最大轉彎角作為約束，可以減少搜索子節點數量，縮短航路規劃時間。如圖2所示，根據無人機規劃航路前序節點與當前節點的位置關系計算航跡角，作為無人機當前位置的航跡。在忽略側風影響的情況下，可近似認為是無人機的當前航向。以此航跡作為基準向量，篩選無人機最大機動轉彎角覆蓋范圍內的節點，作為后續航路搜索節點。圖2 中，虛線為無人機當前航跡，黑色節點為不滿足無人機最大轉彎角限制的節點，灰色節點為滿足最大轉彎角限制節點。

圖2 基于導航系統性能的可行航路Fig.2 Feasible trajectories based on navigation system performance

圖3 最大轉彎角約束Fig.3 Maximum turning angle constraint

航路規劃中，計算航路的最大轉彎角并與無人機最大允許轉彎角相比較，以確保無人機能夠實現一定距離容限內的安全轉彎。

式中：θrp為規劃航路的最大轉彎角，θuav-max為無人機飛行時的最大允許轉彎角。

2）最大飛行高度約束

在不考慮空域限制的情況下，飛行高度越高，無人機受地形障礙物的影響越小，航路規劃越經濟簡單。但無人機受到自身高度升限的局限，在不同的飛行任務需求下，需要綜合評估無人機高度升限、續航時間與規劃航路之間的制約關系，在確保規劃航路可行的前提下實現經濟性。規劃航路高度要限定在無人機的高度升限之下：

式中：Hrp為規劃航路最大飛行高度，Huav-max為無人機高度升限。將所有高度高于Huav-max的位置節點定義為障礙物，低于Huav-max的位置節點則根據無人機高度與地形高度之間的差值來確定各節點的屬性（障礙、非障礙）。

2 改進A＊算法的無人機航路規劃

常用的路徑規劃算法有：傳統的模擬退火算法、人工勢場法；包括A*算法、Dijkstra 算法的啟發式搜索算法；以及蟻群算法、粒子群算法等智能仿生學算法[16-20]。相比較而言，A*算法以簡單、有效、直觀等優點在路徑規劃中得到廣泛應用，并可以通過稀疏改進、雙向搜索等方式進行算法改進，提高優化效果及運行效率。

A*算法最早由斯坦福大學的Peter Hart 等人于1968 年提出，是一種典型的圖遍歷啟發式搜索算法。A*算法利用啟發函數描述當前節點距離目標點的距離，在全局范圍逐次搜索臨近節點，計算代價函數，通過代價函數最小選定最優航路。傳統A*算法通過搜索遍歷實現最優航路規劃，對環境反應迅速，搜索路徑直接，并可以通過調節啟發函數有效控制算法的速度和精確度，相比其他算法，所規劃航路不僅滿足航路規劃的需求，且具有很好的穩定性，易于無人機實現遵循規劃航路的飛行。本文對傳統A*算法進行改進，將無人機機動性能作為約束條件，減小搜索空間，縮短算法運行時間；通過啟發函數加權計算，綜合考慮導航系統EPU、機動影響和航路長度權重因子，提供高效的無人機航路規劃。

傳統A*算法的代價函數如下：

式中：n表示當前節點，g(n)為從起點到當前節點的實際代價函數，h(n)為從當前節點到目標點的估計代價函數（啟發函數），f(n) 為所規劃航路的總代價函數。f(n) 最小時，所規劃的航路最優。

假設當前節點的位置坐標為(xn,yn,zn)，目標點的位置坐標為(xg,yg,zg)，則：

其中，啟發函數采用曼哈頓距離計算方式。

2.1 加權啟發函數

A*算法中，可以通過加權系數調節實際代價函數g(n)和啟發函數h(n)對航路規劃的影響，啟發函數h(n)的權重越大，算法收斂速度越快，反之則收斂速度變慢。

為確保所規劃航路的安全性和可行性，需要對A*算法進行改進，綜合考慮機載導航系統定位誤差和無人機機動性能對航路規劃的影響，利用機載導航系統EPU、無人機機動轉彎角和曼哈頓距離加權計算啟發函數h(n)。

式(4)中，Dw為導航系統EPU 代價，Dz為無人機機動轉彎角代價，Ds為航程代價，w1,w2,w3分別為三種代價的權重系數，w1+w2+w3=1，可以根據任務需要調節權重，實現不同代價需求的規劃航路。其中w2根據無人機的特性可以為0～1/3，對航向可以任意改變的旋翼無人機，權重系數為0，而對于航向改變受限的固定翼無人機，其權重可以根據受限程度改變，受限程度越高，權重系數越大，直至與EPU 和航程代價的權重相同。

2.1.1 導航系統EPU 代價函數 Dw

捷聯慣性導航系統的定位精度與傳感器精度及工作時長有關。隨著飛行時間的增長，慣性導航系統的定位誤差呈累加狀態，極大地限制了慣性導航系統獨立使用的時長。隨著卡爾曼濾波技術的改進，GPS/IMU 組合導航的情況下，慣導系統的定位精度得到較大改善，但在GPS 衛星信號拒止，需要慣導系統獨立工作的情況下，其定位誤差隨時間的積累相對還是比較大的。典型的機載慣導系統的誤差變化變化如下式所示。

式(5)中：t為IMU 的工作時間，單位為小時。

GPS 接收模塊的導航定位誤差主要與接收機的用戶等效測距誤差和所接收衛星的空間幾何布局（Dilution of Precision,DOP）有關。

GPS/IMU 組合導航系統的實際導航性能用EPU來描述，它以機載導航系統計算的無人機位置為圓心，根據GPS/IMU 組合導航所用的濾波算法，計算95%概率下的系統定位誤差，即為機載導航系統EPU。假設GPS/IMU 采用松組合形式，機載導航系統EPU 的計算公式如下：

式(6)中：HFOM 是GPS 接收模塊根據DOP 及測距誤差計算的GPS 模塊2σ（95%）定位誤差；δPINS是慣性導航計算誤差，與IMU 傳感器的誤差特性有關；Δlatency是組合導航算法處理的延遲時間，GS 為無人機地速。

以當前節點為核心，以圖4 所示的方式順序搜尋當前節點周圍八個方向的最近障礙物節點，并分別計算二者之間的距離。假設當前節點坐標為(xi,yi)，第j個障礙物節點的坐標為(xnj,ynj)，則二者之間的距離為計算如式(7)所示：

圖4 當前節點與臨近障礙物之間的距離Fig.4 Distance between present position and adjacent obstacle

式(7)中，Se為當前節點到臨近障礙物網格的最小距離，當Se≤EPU時，該節點不可用，只有當Se＞EPU時，才屬于安全節點，代價函數是Se的倒數，Se越大，則規劃航路的安全性越高，在啟發函數中的影響力就越大。

2.1.2 無人機機動轉彎角代價Dz

無人機的航路規劃首先應滿足無人機的機動性能需求，只有滿足性能需求的航路才是可實施航路。假設無人機當前節點坐標為(xi,yi,zi)，前序節點坐標為(xi-1,yi-1,zi-1)，后序節點坐標為(xi+1,yi+1,zi+1)，θi為規劃航路轉彎角大小。則θi為：

當θi＞θuav-max時，后續節點導致的無人機機動轉彎角大于無人機的最大允許轉彎角，該節點為無效節點，只有當θi≤θuav-max時，該節點才可作為航路規劃的下一節點，且機動轉彎角越小，在啟發函數中的影響力就越大。因此，無人機激動轉彎角的代價函數為：

航程代價Ds為當前節點到目的地的距離代價。假設無人機當前節點坐標為(xi,yi,zi)，目的地節點坐標為(xd,yd,zd)，則航程代價為：

2.2 無人機動態航路規劃

2.2.1 航路規劃網格的構建

本文航路規劃所用地形數據庫為global mapper提供的數字高程模型（Digital Elevation Model,DEM）數據，分辨率為5×5×5 m3，航路規劃使用該數據作為網格數據。

為提高航路規劃的搜索效率，通過高度分層來降低搜索維度，縮短搜索時間。起降階段按照5 m 高度分層進行航路規劃，巡航階段則根據機載導航系統提供的無人機高度信息，取整到其下方的5 m 高度層網格，動態調整DEM 高程數據地形障礙物的網格圖。由于無人機起降階段短，航向相對穩定，對整體規劃航程的影響小，因此對改進算法的性能分析主要集中在特定高度的巡航階段。

2.2.2 航路規劃流程

如圖5 所示，無人機起飛前首先按照默認EPU 進行航路規劃，默認EPU 為GPS 接收機常規誤差，所規劃航路作為初始航路。

飛行過程中，根據機載導航系統提供的無人機位置、高度信息和EPU 決定系統是否啟動新一輪航路規劃，邏輯判定的依據是無人機高度偏離和水平偏離。

當無人機導航系統提示無人機的當前高度與前期所規劃航路的高度層之間的偏差大于1/2 網格高時，調用當前高度臨近的低高度層網格信息，啟動新的航路規劃。

利用導航系統提供的水平位置信息和EPU 計算無人機的水平偏離，當水平偏離大于1/2 網格寬度時，啟用新的航路規劃。其中無人機的水平偏離為航道偏離和導航系統EPU 之和。

其中L為無人機的水平偏離，L1為航道偏離。如果兩個偏離都未超出閾值，則保持前期規劃航路不變。

新的航路規劃中，導航系統提供的無人機實時位置作為航路規劃的新起始點，終點不變。導航系統EPU用于計算當前時刻導航系統EPU 代價函數。改進后的A*算法流程圖如圖5 所示。

對于確定飛行任務的無人機來說，只有在出現特殊情況時，如強側風、雷雨區躲避、動目標的機動沖突解脫等原因，才可能會出現較大的航路偏離，需要重新規劃航路，或無人機任務臨時改變時需要重新規劃航路。常規飛機密度的空域中，所規劃航路通常會得到較好的遵循。利用高度偏離和水平偏離邏輯判定來啟動航路重新規劃功能可以有效避免規劃的無效重復，節省計算資源。同時，初始航路規劃使用默認EPU進行規劃，可以確保所規劃的初始航路具有較大的安全裕度，為后期的航路更新提供足夠的規劃時間。極端情況下，只要規劃時間小于EPU 與無人機最大飛行速度的比值，就可以確保飛機的飛行安全性。

根據現有典型無人機飛行速度來看，商業用途的固定翼無人機最高航速一般在 28 m/s～55.5 m/s（100～200 km/h）之間，旋翼無人機的最高航速一般在14 m/s～28 m/s（50～100 km/h）之間?？紤]導航系統定位不確定度（EPU）約為20 m～30 m，可以滿足航路安全的實時性要求。另外，隨著無人機的飛行，距離目的地的距離不斷縮短，航路規劃所用時間也將大大縮短。

3 實驗對比與結果分析

為驗證基于機載導航系統實際導航性能的改進A*算法對無人機航路規劃算法有效性和安全性的提升，利用Matlab2020a 軟件對相關算法進行了仿真驗證。模擬無人機在不同地形環境和不同導航性能的航路規劃結果，并對所規劃航路的安全性和規劃時間進行了驗證分析。

3.1 無約束下的航路規劃分析

在不考慮無人機機動性能及導航系統EPU 的情況下，選定規劃航路的起始點位置的經緯度為[-38.128554931 °,-7.477058516 °]，目的地經緯度為[-38.133504426 °,-7.503455822 °]。設定無人機分別以370 m，400 m，420 m 三個不同的巡航高度飛行，最大轉彎角度為360 °，所規劃航路如圖6 所示。

可以看出，在不考慮無人機機動性能和導航系統EPU 的情況下，無人機航路規劃主要受地形數據庫中障礙物分布的影響，巡航高度越高，空域中障礙物的數量越少，分布越稀疏，所規劃的航路長度越短，優化效果明顯。

3.2 轉彎角約束下的航路規劃分析

不考慮導航系統EPU，考慮無人機的機動轉彎性能，對航路規劃效果及所規劃航路的轉彎角度進行分析。假設無人機起始點經緯度為[-38.128554931 °,-7.477058516 °]，目的地經緯度為[-38.129615537 °,-7.482715081 °]，運行于400 m 巡航高度。在無人機最大允許轉彎角度分別為80 °和180 °，以及復雜場景最大允許轉彎角度分別為30°和120°的情況下，進行無人機航路規劃的對比分析，如圖7 所示。

圖7 不同場景/不同轉彎角約束下的規劃航路Fig.7 Planned trajectories under different maximum turning angle constraints

可以看出，在不同轉彎角機動的約束下，無人機的規劃航路出現了明顯的差異。改進后的A*算法在80 °和180 °轉彎角約束下所規劃航路的轉彎角度變化如圖8 中所示。相較于有轉彎角約束規劃，無轉彎角約束的規劃航路中，無人機的航線轉彎角變化幅度較大，航路可行性及穩定性不及有約束航路規劃，對無人機的控制及飛行安全易造成較大影響。

圖8 不同轉彎角約束下的無人機轉彎角Fig.8 UAV turning angle under different maximum turning angle constraints

表1 給出了傳統A*算法和改進A*算法航路規劃的用時，以及不同轉彎角約束下規劃航路的角度變化率?？梢钥闯觯焊倪M后的A*算法航路規劃用時大大縮短，規劃時間效率提高一倍。相較于傳統A*算法，最大允許轉彎角較大的無人機規劃航線的角度變化率也有一定程度的降低，但對于最大允許轉彎角較小的無人機來說，由于某些特定的環境無法滿足其轉彎角度要求，所規劃的航路有可能會更曲折，角度變化較頻繁，但其整個航程的航路轉彎角度都保持在較小值，符合其轉彎角約束要求。

表1 有/無轉彎角約束的仿真結果對比Tab.1 Simulation results comparison of with/without maximum turning angle constraints

3.3 導航系統EPU 約束下的航路規劃分析

低成本無人機機載導航系統的誤差較大，且極易受無人機復雜地形運行環境的影響。導航系統EPU 直接影響所規劃航路的可行性和無人機運行的安全性。本文對不同導航系統EPU 影響下的航路規劃進行了仿真，并利用無人機與障礙物之間的距離安全裕度分析所規劃航路的安全性。

將無人機由質點轉換為覆蓋EPU 的空間結構，形成無人機包絡，進行EPU 分別為15 m 和5 m 情況下的航路規劃，飛行高度為390 m，最大轉彎角度為360 °，規劃結果如圖9 所示。

圖9 不同EPU 約束下的規劃航路Fig.9 Planned trajectories under different EPU constraints

仿真結果顯示，受機載導航系統位置不確定度的影響，相同性能的無人機在相同的運行環境下，所規劃的航路有較大的差異，主要是處于不同導航系統EPU 下的航路安全需求，這種差異在低空復雜空域中表現得尤其明顯。

以370 m 巡航高度為例，圖10 和圖11 給出了考慮導航系統EPU 和不考慮EPU 情況下的規劃航路以及所規劃航路與障礙物之間的距離?？梢钥闯?，不考慮導航系統EPU 的規劃航路存在極大的安全風險。而基于無人機機載導航系統性能的改進A*算法考慮了安全裕度，所規劃航路可以確保無人機到障礙物的距離都大于導航系統EPU，避免了無人機與障礙物的碰撞風險，增加了航路的安全性。

圖10 EPU 對規劃航路的影響Fig.10 Planned trajectories influenced by EPU

圖11 考慮和不考慮EPU 規劃航路的安全裕度Fig.11 Safety margins for trajectories with and without consideration of navigation system EPU

3.4 機動性能及EPU 多源約束下的航路規劃

綜合考慮無人機機動性能和導航系統性能的約束，進行多源敏感航路規劃的仿真及效果分析，對比傳統算法和改進后算法的航路規劃及相關效能數據，確認改進后算法的有效性。設無人機起始點經緯度為[-38.1268897803 °,-7.478922489236 °]，目的地經緯度為[38.1303897803 °,-7.4978558225 °]，巡航高度360 m，最大允許轉彎角60 °，導航系統EPU 為5 m。仿真結果如圖12、圖13 和表2 所示。

表2 算法改進前后數據對比Tab.2 Data comparison of different algorithm

圖12 傳統算法與改進算法所規劃航路的對比Fig.12 Planned trajectories comparison of traditional A*algorithm and improved A* algorithm

圖13 算法改進前后無人機航路轉彎角對比Fig.13 UAV flight path turning angle comparison of different algorithm

表2 中數據顯示，基于無人機機動性能和機載導航系統性能的改進A*算法所規劃的航路，無論是航路距離障礙物的安全裕度方面，還是對無人機轉彎機動的影響方面都有極大的改善，提高了所規劃航路的安全性及可行性。

4 結論

為提升無人機航路規劃的安全性，提出基于無人機機動性能和機載導航系統性能約束的改進A*航路規劃算法，構建了融合無人機最大允許轉彎角、導航系統EPU 和最短路徑的加權啟發函數，實現航路規劃的多約束適應性，并通過機動約束條件減少搜索節點，提高航路規劃速度。

利用數字高程模型模擬無人機運行環境，仿真不同最大允許轉彎角和導航系統EPU 約束條件下的航路規劃，將改進算法的規劃航路與傳統算法航路規劃結果進行對比，實驗結果表明，改進算法對所規劃航路的安全性、可行性起到了良好的效果。由于大多數無人機體積小、重量輕，易受環境影響，后期需綜合考慮環境強風對無人機飛行的擾動，以及導航衛星信號拒止環境下導航系統EPU 突然增大的情況對無人機運行安全性的影響，進一步提高航路規劃的魯棒性。