多源性能約束的無人機(jī)可靠動(dòng)態(tài)航路規(guī)劃算法

孫淑光，朱曉文

（1.中國(guó)民航大學(xué) 電子信息與自動(dòng)化學(xué)院，天津 300300；2.中國(guó)民航大學(xué) 交通科學(xué)與工程學(xué)院，天津 300300）

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

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

為提高規(guī)劃航路的可行性和安全性，本文提出一種敏感無人機(jī)機(jī)載導(dǎo)航系統(tǒng)實(shí)際性能及無人機(jī)機(jī)動(dòng)性能的動(dòng)態(tài)航路規(guī)劃算法，將無人機(jī)航路規(guī)劃由基于等效質(zhì)點(diǎn)的規(guī)劃轉(zhuǎn)變?yōu)榛诙嘣葱阅芗s束的敏感區(qū)域路徑規(guī)劃，確保所規(guī)劃航路的安全性和可行性。

1 無人機(jī)航路規(guī)劃系統(tǒng)架構(gòu)及約束條件分析

1.1 系統(tǒng)架構(gòu)

根據(jù)無人機(jī)的任務(wù)需求確定航路的起始點(diǎn)和目的地，利用機(jī)載地形數(shù)據(jù)庫(kù)信息建立三維地形網(wǎng)格，將機(jī)載導(dǎo)航系統(tǒng)的實(shí)際導(dǎo)航性能和無人機(jī)機(jī)動(dòng)性能作為航路規(guī)劃的約束條件，根據(jù)機(jī)載導(dǎo)航系統(tǒng)提供的無人機(jī)實(shí)時(shí)位置信息動(dòng)態(tài)更新航路規(guī)劃的起始點(diǎn)，實(shí)現(xiàn)機(jī)載端的動(dòng)態(tài)航路規(guī)劃。系統(tǒng)整體架構(gòu)如圖1 所示。

圖1 無人機(jī)動(dòng)態(tài)航路規(guī)劃系統(tǒng)架構(gòu)Fig.1 UAV dynamic flight path planning architecture

1.2 航路規(guī)劃約束條件

1.2.1 機(jī)載導(dǎo)航系統(tǒng)導(dǎo)航性能約束

利用機(jī)載導(dǎo)航系統(tǒng)實(shí)時(shí)評(píng)估自身實(shí)際導(dǎo)航性能，計(jì)算導(dǎo)航系統(tǒng)定位誤差包括，也即導(dǎo)航系統(tǒng)位置不確定度（Estimated Position Uncertainty,EPU），并將其與無人機(jī)機(jī)動(dòng)性能一起作為航路規(guī)劃的搜索代價(jià)函數(shù)來約束航路規(guī)劃，EPU為機(jī)載導(dǎo)航系統(tǒng)的總系統(tǒng)誤差。

在充分考慮機(jī)載導(dǎo)航系統(tǒng)定位誤差及無人機(jī)機(jī)動(dòng)性能的基礎(chǔ)上進(jìn)行航路規(guī)劃，可以有效提高所規(guī)劃航路的安全性。航路規(guī)劃中，計(jì)算當(dāng)前節(jié)點(diǎn)與臨近障礙物之間的距離，與EPU 相比較確定安全節(jié)點(diǎn)。當(dāng)節(jié)點(diǎn)與障礙物之間距離的最小值大于EPU 時(shí)，該節(jié)點(diǎn)被定義為安全節(jié)點(diǎn)。

1.2.2 無人機(jī)機(jī)動(dòng)性能約束

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

1）最大轉(zhuǎn)彎角約束

實(shí)際飛行中，受無人機(jī)空氣動(dòng)力學(xué)特性及飛行控制系統(tǒng)能力所限，無人機(jī)尤其是固定翼無人機(jī)，規(guī)劃航路的轉(zhuǎn)彎角度最大不能超過無人機(jī)可能實(shí)現(xiàn)的最大允許轉(zhuǎn)彎角。

航路規(guī)劃中，將無人機(jī)最大轉(zhuǎn)彎角作為約束，可以減少搜索子節(jié)點(diǎn)數(shù)量，縮短航路規(guī)劃時(shí)間。如圖2所示，根據(jù)無人機(jī)規(guī)劃航路前序節(jié)點(diǎn)與當(dāng)前節(jié)點(diǎn)的位置關(guān)系計(jì)算航跡角，作為無人機(jī)當(dāng)前位置的航跡。在忽略側(cè)風(fēng)影響的情況下，可近似認(rèn)為是無人機(jī)的當(dāng)前航向。以此航跡作為基準(zhǔn)向量，篩選無人機(jī)最大機(jī)動(dòng)轉(zhuǎn)彎角覆蓋范圍內(nèi)的節(jié)點(diǎn)，作為后續(xù)航路搜索節(jié)點(diǎn)。圖2 中，虛線為無人機(jī)當(dāng)前航跡，黑色節(jié)點(diǎn)為不滿足無人機(jī)最大轉(zhuǎn)彎角限制的節(jié)點(diǎn)，灰色節(jié)點(diǎn)為滿足最大轉(zhuǎn)彎角限制節(jié)點(diǎn)。

圖2 基于導(dǎo)航系統(tǒng)性能的可行航路Fig.2 Feasible trajectories based on navigation system performance

圖3 最大轉(zhuǎn)彎角約束Fig.3 Maximum turning angle constraint

航路規(guī)劃中，計(jì)算航路的最大轉(zhuǎn)彎角并與無人機(jī)最大允許轉(zhuǎn)彎角相比較，以確保無人機(jī)能夠?qū)崿F(xiàn)一定距離容限內(nèi)的安全轉(zhuǎn)彎。

式中：θrp為規(guī)劃航路的最大轉(zhuǎn)彎角，θuav-max為無人機(jī)飛行時(shí)的最大允許轉(zhuǎn)彎角。

2）最大飛行高度約束

在不考慮空域限制的情況下，飛行高度越高，無人機(jī)受地形障礙物的影響越小，航路規(guī)劃越經(jīng)濟(jì)簡(jiǎn)單。但無人機(jī)受到自身高度升限的局限，在不同的飛行任務(wù)需求下，需要綜合評(píng)估無人機(jī)高度升限、續(xù)航時(shí)間與規(guī)劃航路之間的制約關(guān)系，在確保規(guī)劃航路可行的前提下實(shí)現(xiàn)經(jīng)濟(jì)性。規(guī)劃航路高度要限定在無人機(jī)的高度升限之下：

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

2 改進(jìn)A＊算法的無人機(jī)航路規(guī)劃

常用的路徑規(guī)劃算法有：傳統(tǒng)的模擬退火算法、人工勢(shì)場(chǎng)法；包括A*算法、Dijkstra 算法的啟發(fā)式搜索算法；以及蟻群算法、粒子群算法等智能仿生學(xué)算法[16-20]。相比較而言，A*算法以簡(jiǎn)單、有效、直觀等優(yōu)點(diǎn)在路徑規(guī)劃中得到廣泛應(yīng)用，并可以通過稀疏改進(jìn)、雙向搜索等方式進(jìn)行算法改進(jìn)，提高優(yōu)化效果及運(yùn)行效率。

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

傳統(tǒng)A*算法的代價(jià)函數(shù)如下：

式中：n表示當(dāng)前節(jié)點(diǎn)，g(n)為從起點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際代價(jià)函數(shù)，h(n)為從當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)的估計(jì)代價(jià)函數(shù)（啟發(fā)函數(shù)），f(n) 為所規(guī)劃航路的總代價(jià)函數(shù)。f(n) 最小時(shí)，所規(guī)劃的航路最優(yōu)。

假設(shè)當(dāng)前節(jié)點(diǎn)的位置坐標(biāo)為(xn,yn,zn)，目標(biāo)點(diǎn)的位置坐標(biāo)為(xg,yg,zg)，則：

其中，啟發(fā)函數(shù)采用曼哈頓距離計(jì)算方式。

2.1 加權(quán)啟發(fā)函數(shù)

A*算法中，可以通過加權(quán)系數(shù)調(diào)節(jié)實(shí)際代價(jià)函數(shù)g(n)和啟發(fā)函數(shù)h(n)對(duì)航路規(guī)劃的影響，啟發(fā)函數(shù)h(n)的權(quán)重越大，算法收斂速度越快，反之則收斂速度變慢。

為確保所規(guī)劃航路的安全性和可行性，需要對(duì)A*算法進(jìn)行改進(jìn)，綜合考慮機(jī)載導(dǎo)航系統(tǒng)定位誤差和無人機(jī)機(jī)動(dòng)性能對(duì)航路規(guī)劃的影響，利用機(jī)載導(dǎo)航系統(tǒng)EPU、無人機(jī)機(jī)動(dòng)轉(zhuǎn)彎角和曼哈頓距離加權(quán)計(jì)算啟發(fā)函數(shù)h(n)。

式(4)中，Dw為導(dǎo)航系統(tǒng)EPU 代價(jià)，Dz為無人機(jī)機(jī)動(dòng)轉(zhuǎn)彎角代價(jià)，Ds為航程代價(jià)，w1,w2,w3分別為三種代價(jià)的權(quán)重系數(shù)，w1+w2+w3=1，可以根據(jù)任務(wù)需要調(diào)節(jié)權(quán)重，實(shí)現(xiàn)不同代價(jià)需求的規(guī)劃航路。其中w2根據(jù)無人機(jī)的特性可以為0～1/3，對(duì)航向可以任意改變的旋翼無人機(jī)，權(quán)重系數(shù)為0，而對(duì)于航向改變受限的固定翼無人機(jī)，其權(quán)重可以根據(jù)受限程度改變，受限程度越高，權(quán)重系數(shù)越大，直至與EPU 和航程代價(jià)的權(quán)重相同。

2.1.1 導(dǎo)航系統(tǒng)EPU 代價(jià)函數(shù) Dw

捷聯(lián)慣性導(dǎo)航系統(tǒng)的定位精度與傳感器精度及工作時(shí)長(zhǎng)有關(guān)。隨著飛行時(shí)間的增長(zhǎng)，慣性導(dǎo)航系統(tǒng)的定位誤差呈累加狀態(tài)，極大地限制了慣性導(dǎo)航系統(tǒng)獨(dú)立使用的時(shí)長(zhǎng)。隨著卡爾曼濾波技術(shù)的改進(jìn)，GPS/IMU 組合導(dǎo)航的情況下，慣導(dǎo)系統(tǒng)的定位精度得到較大改善，但在GPS 衛(wèi)星信號(hào)拒止，需要慣導(dǎo)系統(tǒng)獨(dú)立工作的情況下，其定位誤差隨時(shí)間的積累相對(duì)還是比較大的。典型的機(jī)載慣導(dǎo)系統(tǒng)的誤差變化變化如下式所示。

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

GPS 接收模塊的導(dǎo)航定位誤差主要與接收機(jī)的用戶等效測(cè)距誤差和所接收衛(wèi)星的空間幾何布局（Dilution of Precision,DOP）有關(guān)。

GPS/IMU 組合導(dǎo)航系統(tǒng)的實(shí)際導(dǎo)航性能用EPU來描述，它以機(jī)載導(dǎo)航系統(tǒng)計(jì)算的無人機(jī)位置為圓心，根據(jù)GPS/IMU 組合導(dǎo)航所用的濾波算法，計(jì)算95%概率下的系統(tǒng)定位誤差，即為機(jī)載導(dǎo)航系統(tǒng)EPU。假設(shè)GPS/IMU 采用松組合形式，機(jī)載導(dǎo)航系統(tǒng)EPU 的計(jì)算公式如下：

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

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

圖4 當(dāng)前節(jié)點(diǎn)與臨近障礙物之間的距離Fig.4 Distance between present position and adjacent obstacle

式(7)中，Se為當(dāng)前節(jié)點(diǎn)到臨近障礙物網(wǎng)格的最小距離，當(dāng)Se≤EPU時(shí)，該節(jié)點(diǎn)不可用，只有當(dāng)Se＞EPU時(shí)，才屬于安全節(jié)點(diǎn)，代價(jià)函數(shù)是Se的倒數(shù)，Se越大，則規(guī)劃航路的安全性越高，在啟發(fā)函數(shù)中的影響力就越大。

2.1.2 無人機(jī)機(jī)動(dòng)轉(zhuǎn)彎角代價(jià)Dz

無人機(jī)的航路規(guī)劃首先應(yīng)滿足無人機(jī)的機(jī)動(dòng)性能需求，只有滿足性能需求的航路才是可實(shí)施航路。假設(shè)無人機(jī)當(dāng)前節(jié)點(diǎn)坐標(biāo)為(xi,yi,zi)，前序節(jié)點(diǎn)坐標(biāo)為(xi-1,yi-1,zi-1)，后序節(jié)點(diǎn)坐標(biāo)為(xi+1,yi+1,zi+1)，θi為規(guī)劃航路轉(zhuǎn)彎角大小。則θi為：

當(dāng)θi＞θuav-max時(shí)，后續(xù)節(jié)點(diǎn)導(dǎo)致的無人機(jī)機(jī)動(dòng)轉(zhuǎn)彎角大于無人機(jī)的最大允許轉(zhuǎn)彎角，該節(jié)點(diǎn)為無效節(jié)點(diǎn)，只有當(dāng)θi≤θuav-max時(shí)，該節(jié)點(diǎn)才可作為航路規(guī)劃的下一節(jié)點(diǎn)，且機(jī)動(dòng)轉(zhuǎn)彎角越小，在啟發(fā)函數(shù)中的影響力就越大。因此，無人機(jī)激動(dòng)轉(zhuǎn)彎角的代價(jià)函數(shù)為：

航程代價(jià)Ds為當(dāng)前節(jié)點(diǎn)到目的地的距離代價(jià)。假設(shè)無人機(jī)當(dāng)前節(jié)點(diǎn)坐標(biāo)為(xi,yi,zi)，目的地節(jié)點(diǎn)坐標(biāo)為(xd,yd,zd)，則航程代價(jià)為：

2.2 無人機(jī)動(dòng)態(tài)航路規(guī)劃

2.2.1 航路規(guī)劃網(wǎng)格的構(gòu)建

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

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

2.2.2 航路規(guī)劃流程

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

飛行過程中，根據(jù)機(jī)載導(dǎo)航系統(tǒng)提供的無人機(jī)位置、高度信息和EPU 決定系統(tǒng)是否啟動(dòng)新一輪航路規(guī)劃，邏輯判定的依據(jù)是無人機(jī)高度偏離和水平偏離。

當(dāng)無人機(jī)導(dǎo)航系統(tǒng)提示無人機(jī)的當(dāng)前高度與前期所規(guī)劃航路的高度層之間的偏差大于1/2 網(wǎng)格高時(shí)，調(diào)用當(dāng)前高度臨近的低高度層網(wǎng)格信息，啟動(dòng)新的航路規(guī)劃。

利用導(dǎo)航系統(tǒng)提供的水平位置信息和EPU 計(jì)算無人機(jī)的水平偏離，當(dāng)水平偏離大于1/2 網(wǎng)格寬度時(shí)，啟用新的航路規(guī)劃。其中無人機(jī)的水平偏離為航道偏離和導(dǎo)航系統(tǒng)EPU 之和。

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

新的航路規(guī)劃中，導(dǎo)航系統(tǒng)提供的無人機(jī)實(shí)時(shí)位置作為航路規(guī)劃的新起始點(diǎn)，終點(diǎn)不變。導(dǎo)航系統(tǒng)EPU用于計(jì)算當(dāng)前時(shí)刻導(dǎo)航系統(tǒng)EPU 代價(jià)函數(shù)。改進(jìn)后的A*算法流程圖如圖5 所示。

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

根據(jù)現(xiàn)有典型無人機(jī)飛行速度來看，商業(yè)用途的固定翼無人機(jī)最高航速一般在 28 m/s～55.5 m/s（100～200 km/h）之間，旋翼無人機(jī)的最高航速一般在14 m/s～28 m/s（50～100 km/h）之間。考慮導(dǎo)航系統(tǒng)定位不確定度（EPU）約為20 m～30 m，可以滿足航路安全的實(shí)時(shí)性要求。另外，隨著無人機(jī)的飛行，距離目的地的距離不斷縮短，航路規(guī)劃所用時(shí)間也將大大縮短。

3 實(shí)驗(yàn)對(duì)比與結(jié)果分析

為驗(yàn)證基于機(jī)載導(dǎo)航系統(tǒng)實(shí)際導(dǎo)航性能的改進(jìn)A*算法對(duì)無人機(jī)航路規(guī)劃算法有效性和安全性的提升，利用Matlab2020a 軟件對(duì)相關(guān)算法進(jìn)行了仿真驗(yàn)證。模擬無人機(jī)在不同地形環(huán)境和不同導(dǎo)航性能的航路規(guī)劃結(jié)果，并對(duì)所規(guī)劃航路的安全性和規(guī)劃時(shí)間進(jìn)行了驗(yàn)證分析。

3.1 無約束下的航路規(guī)劃分析

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

可以看出，在不考慮無人機(jī)機(jī)動(dòng)性能和導(dǎo)航系統(tǒng)EPU 的情況下，無人機(jī)航路規(guī)劃主要受地形數(shù)據(jù)庫(kù)中障礙物分布的影響，巡航高度越高，空域中障礙物的數(shù)量越少，分布越稀疏，所規(guī)劃的航路長(zhǎng)度越短，優(yōu)化效果明顯。

3.2 轉(zhuǎn)彎角約束下的航路規(guī)劃分析

不考慮導(dǎo)航系統(tǒng)EPU，考慮無人機(jī)的機(jī)動(dòng)轉(zhuǎn)彎性能，對(duì)航路規(guī)劃效果及所規(guī)劃航路的轉(zhuǎn)彎角度進(jìn)行分析。假設(shè)無人機(jī)起始點(diǎn)經(jīng)緯度為[-38.128554931 °,-7.477058516 °]，目的地經(jīng)緯度為[-38.129615537 °,-7.482715081 °]，運(yùn)行于400 m 巡航高度。在無人機(jī)最大允許轉(zhuǎn)彎角度分別為80 °和180 °，以及復(fù)雜場(chǎng)景最大允許轉(zhuǎn)彎角度分別為30°和120°的情況下，進(jìn)行無人機(jī)航路規(guī)劃的對(duì)比分析，如圖7 所示。

圖7 不同場(chǎng)景/不同轉(zhuǎn)彎角約束下的規(guī)劃航路Fig.7 Planned trajectories under different maximum turning angle constraints

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

圖8 不同轉(zhuǎn)彎角約束下的無人機(jī)轉(zhuǎn)彎角Fig.8 UAV turning angle under different maximum turning angle constraints

表1 給出了傳統(tǒng)A*算法和改進(jìn)A*算法航路規(guī)劃的用時(shí)，以及不同轉(zhuǎn)彎角約束下規(guī)劃航路的角度變化率。可以看出：改進(jìn)后的A*算法航路規(guī)劃用時(shí)大大縮短，規(guī)劃時(shí)間效率提高一倍。相較于傳統(tǒng)A*算法，最大允許轉(zhuǎn)彎角較大的無人機(jī)規(guī)劃航線的角度變化率也有一定程度的降低，但對(duì)于最大允許轉(zhuǎn)彎角較小的無人機(jī)來說，由于某些特定的環(huán)境無法滿足其轉(zhuǎn)彎角度要求，所規(guī)劃的航路有可能會(huì)更曲折，角度變化較頻繁，但其整個(gè)航程的航路轉(zhuǎn)彎角度都保持在較小值，符合其轉(zhuǎn)彎角約束要求。

表1 有/無轉(zhuǎn)彎角約束的仿真結(jié)果對(duì)比Tab.1 Simulation results comparison of with/without maximum turning angle constraints

3.3 導(dǎo)航系統(tǒng)EPU 約束下的航路規(guī)劃分析

低成本無人機(jī)機(jī)載導(dǎo)航系統(tǒng)的誤差較大，且極易受無人機(jī)復(fù)雜地形運(yùn)行環(huán)境的影響。導(dǎo)航系統(tǒng)EPU 直接影響所規(guī)劃航路的可行性和無人機(jī)運(yùn)行的安全性。本文對(duì)不同導(dǎo)航系統(tǒng)EPU 影響下的航路規(guī)劃進(jìn)行了仿真，并利用無人機(jī)與障礙物之間的距離安全裕度分析所規(guī)劃航路的安全性。

將無人機(jī)由質(zhì)點(diǎn)轉(zhuǎn)換為覆蓋EPU 的空間結(jié)構(gòu)，形成無人機(jī)包絡(luò)，進(jìn)行EPU 分別為15 m 和5 m 情況下的航路規(guī)劃，飛行高度為390 m，最大轉(zhuǎn)彎角度為360 °，規(guī)劃結(jié)果如圖9 所示。

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

仿真結(jié)果顯示，受機(jī)載導(dǎo)航系統(tǒng)位置不確定度的影響，相同性能的無人機(jī)在相同的運(yùn)行環(huán)境下，所規(guī)劃的航路有較大的差異，主要是處于不同導(dǎo)航系統(tǒng)EPU 下的航路安全需求，這種差異在低空復(fù)雜空域中表現(xiàn)得尤其明顯。

以370 m 巡航高度為例，圖10 和圖11 給出了考慮導(dǎo)航系統(tǒng)EPU 和不考慮EPU 情況下的規(guī)劃航路以及所規(guī)劃航路與障礙物之間的距離。可以看出，不考慮導(dǎo)航系統(tǒng)EPU 的規(guī)劃航路存在極大的安全風(fēng)險(xiǎn)。而基于無人機(jī)機(jī)載導(dǎo)航系統(tǒng)性能的改進(jìn)A*算法考慮了安全裕度，所規(guī)劃航路可以確保無人機(jī)到障礙物的距離都大于導(dǎo)航系統(tǒng)EPU，避免了無人機(jī)與障礙物的碰撞風(fēng)險(xiǎn)，增加了航路的安全性。

圖10 EPU 對(duì)規(guī)劃航路的影響Fig.10 Planned trajectories influenced by EPU

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

3.4 機(jī)動(dòng)性能及EPU 多源約束下的航路規(guī)劃

綜合考慮無人機(jī)機(jī)動(dòng)性能和導(dǎo)航系統(tǒng)性能的約束，進(jìn)行多源敏感航路規(guī)劃的仿真及效果分析，對(duì)比傳統(tǒng)算法和改進(jìn)后算法的航路規(guī)劃及相關(guān)效能數(shù)據(jù)，確認(rèn)改進(jìn)后算法的有效性。設(shè)無人機(jī)起始點(diǎn)經(jīng)緯度為[-38.1268897803 °,-7.478922489236 °]，目的地經(jīng)緯度為[38.1303897803 °,-7.4978558225 °]，巡航高度360 m，最大允許轉(zhuǎn)彎角60 °，導(dǎo)航系統(tǒng)EPU 為5 m。仿真結(jié)果如圖12、圖13 和表2 所示。

表2 算法改進(jìn)前后數(shù)據(jù)對(duì)比Tab.2 Data comparison of different algorithm

圖12 傳統(tǒng)算法與改進(jìn)算法所規(guī)劃航路的對(duì)比Fig.12 Planned trajectories comparison of traditional A*algorithm and improved A* algorithm

圖13 算法改進(jìn)前后無人機(jī)航路轉(zhuǎn)彎角對(duì)比Fig.13 UAV flight path turning angle comparison of different algorithm

表2 中數(shù)據(jù)顯示，基于無人機(jī)機(jī)動(dòng)性能和機(jī)載導(dǎo)航系統(tǒng)性能的改進(jìn)A*算法所規(guī)劃的航路，無論是航路距離障礙物的安全裕度方面，還是對(duì)無人機(jī)轉(zhuǎn)彎?rùn)C(jī)動(dòng)的影響方面都有極大的改善，提高了所規(guī)劃航路的安全性及可行性。

4 結(jié)論

為提升無人機(jī)航路規(guī)劃的安全性，提出基于無人機(jī)機(jī)動(dòng)性能和機(jī)載導(dǎo)航系統(tǒng)性能約束的改進(jìn)A*航路規(guī)劃算法，構(gòu)建了融合無人機(jī)最大允許轉(zhuǎn)彎角、導(dǎo)航系統(tǒng)EPU 和最短路徑的加權(quán)啟發(fā)函數(shù)，實(shí)現(xiàn)航路規(guī)劃的多約束適應(yīng)性，并通過機(jī)動(dòng)約束條件減少搜索節(jié)點(diǎn)，提高航路規(guī)劃速度。

利用數(shù)字高程模型模擬無人機(jī)運(yùn)行環(huán)境，仿真不同最大允許轉(zhuǎn)彎角和導(dǎo)航系統(tǒng)EPU 約束條件下的航路規(guī)劃，將改進(jìn)算法的規(guī)劃航路與傳統(tǒng)算法航路規(guī)劃結(jié)果進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法對(duì)所規(guī)劃航路的安全性、可行性起到了良好的效果。由于大多數(shù)無人機(jī)體積小、重量輕，易受環(huán)境影響，后期需綜合考慮環(huán)境強(qiáng)風(fēng)對(duì)無人機(jī)飛行的擾動(dòng)，以及導(dǎo)航衛(wèi)星信號(hào)拒止環(huán)境下導(dǎo)航系統(tǒng)EPU 突然增大的情況對(duì)無人機(jī)運(yùn)行安全性的影響，進(jìn)一步提高航路規(guī)劃的魯棒性。