融合A＊算法與動態(tài)窗口法的無人機路徑規(guī)劃研究

袁 塘，李廷元

（中國民用航空飛行學(xué)院計算機學(xué)院，廣漢 618307）

0 引言

“十四五”時期，交通運輸部將無人機列為未來發(fā)展的重要領(lǐng)域，未來將會加速無人機相關(guān)技術(shù)研發(fā)和應(yīng)用。這幾年無人機技術(shù)的發(fā)展越來越成熟，無人機越來越多地融入到服務(wù)和救援任務(wù)等領(lǐng)域，提高了工作效率。作為無人機的一項關(guān)鍵技術(shù)，路徑規(guī)劃旨在為無人機尋找最短或最優(yōu)路徑，以便利用最少的能量和資源，更加精準、高效地完成更多的任務(wù)。

無人機路徑規(guī)劃的目標是生成連接起始狀態(tài)和目標狀態(tài)同時滿足所需約束的飛行路徑，路徑的優(yōu)化、路徑的完整性、搜索效率和實現(xiàn)魯棒性是無人機路徑規(guī)劃的關(guān)鍵障礙。目前常用的搜索算法主要有基于啟發(fā)式的路徑搜索算法、基于采樣的搜索算法、勢場法、生物啟發(fā)式算法以及單一算法的不斷改進和多種算法的融合。A*算法是當(dāng)前路徑規(guī)劃算法中使用的最多的算法之一，于1968年提出。該算法具有靈活性強、計算速度快、路徑搜索效率高等優(yōu)點，廣泛地用于無人機領(lǐng)域。李曉輝等提出的改良A*算法解決了在有多邊形禁飛區(qū)情況下的路徑規(guī)劃問題。姜秋月等提出改進的基于柵格法的聯(lián)合雙向搜尋策略的A*算法，提高了路徑規(guī)劃過程中節(jié)點的搜尋效率，減少了節(jié)點搜索個數(shù)，降低了運行時間。Chen J等提出了向量叉積優(yōu)化的A*算法，搜索范圍變得更小，提高了航跡點的搜索效率。上述研究基本上都對A*算法做了一部分的改進，尋路效率都有所提高，但無法避免空間中突如其來的障礙物，未解決路徑不平滑的問題，本文將針對這兩方面的缺陷做一定的研究。

計算從起飛點到達目的地的安全路徑，是無人機能夠完成任務(wù)所必須要做的工作，正確選擇路徑點則是無人機路徑規(guī)劃中的重要環(huán)節(jié)。因此，設(shè)計合適的算法對于無人機的路徑規(guī)劃顯然非常重要。把A*算法與動態(tài)窗口算法融合在一起，在確保無人機飛行的全局路徑最優(yōu)的基礎(chǔ)上實現(xiàn)實時動態(tài)規(guī)劃路徑，融合算法的好處既提高了算法的效率和路徑的平滑性，同時也讓算法擁有了良好的局部規(guī)劃的效果。

1 無人機的運動建模

建立無人機的動力學(xué)模型，是路徑規(guī)劃研究的前提基礎(chǔ)部分。無人機的位置變化控制轉(zhuǎn)化成速度控制，避障問題轉(zhuǎn)變成空間中的運動約束問題，這樣可以根據(jù)運動約束條件選擇局部最優(yōu)的路徑。動態(tài)窗口算法在規(guī)劃路徑時先確定無人機的速度的范圍，將無人機模型帶入該速度范圍，進行軌跡的模擬，使用評價函數(shù)對模擬出來的軌跡打分的方式，分數(shù)高的軌跡則為最終的最優(yōu)路徑。

1.1 無人機的運動模型

圖1 無人機運動模型坐標系

在時刻無人機的線速度表示為，角速度表示為，無人機的速度方向與整體坐標系中軸的夾角為。經(jīng)過Δ時刻，無人機的位移增量為：

由此可得+1時刻的無人機在整體坐標系中的坐標位置為：

+1時刻無人機速度與整體坐標系軸正向的夾角為：

1.2 速度采樣

在假定了無人機的軌跡運動模型之后，通過無人機的速度推算出對應(yīng)的軌跡，要求采樣很多速度，在無人機的速度(,)空間中，有非常多組速度，需要根據(jù)無人機自身和障礙物限制將采樣速度保持在一定的區(qū)間內(nèi)，即在一個動態(tài)窗口內(nèi)。

（1）受自身飛行的最大速度、最小速度的限制范圍：

式中（max）和（min）分別代表無人機所能達到的最大線速度和最小線速度，（max）和（min）分別代表無人機能達到的最大角速度和最小角速度。

（2）無人機受最大加速度、最大減速度的限制范圍：

全面預(yù)算管理通過預(yù)算編制把高校醫(yī)院預(yù)算目標具體化和量化，全部分解落實到各部門、各科室、各環(huán)節(jié)中去，建立責(zé)任中心和責(zé)任追究機制，將各個崗位、各個職工的權(quán)、責(zé)、利進行有機結(jié)合，激發(fā)全體員工發(fā)揮主觀能動性，調(diào)動全員參與管理的積極性，有利于提高工作效率和管理水平。

式中（max）和（min）分別代表時刻無人機的最大和最小線加速度，（min）和（max）分別代表時刻無人機的最大和最小角加速度。

（3）周邊障礙物的限制，確保無人機飛行安全，不能讓無人機與障礙物相撞，無人機需要和障礙物保持一定的間隔，在無人機可用的最大減速度情況下，無人機的速度就必須保持在一個可控制的范圍內(nèi)：

公式當(dāng)中的（，）表示無人機到障礙物的最小間隔。

1.3 評價函數(shù)

在上述所得到的采樣速度組中，有一部分的軌跡是可用于無人機飛行的，所以可以采用綜合評價目標函數(shù)的形式，為每一條可行的軌跡信息進行綜合評價打分，通過評價函數(shù)選擇下一時刻中滿足約束的最優(yōu)速度，最優(yōu)速度用最大（，）表示，評價指標包括方位角、安全距離和速度，評價函數(shù)為：

式中、、為權(quán)值參數(shù)；（，ω）評價的是無人機在當(dāng)前設(shè)定的采樣速度下，到達模擬軌跡終點時無人機的朝向和目的地方向的角度差，角度越小，評價得分越高；（，ω）表示無人機在當(dāng)前行進的軌跡上，與周圍的障礙物之間的距離大小，距離越大，評價得分越高；無人機的線速度和角速度用（，ω）表示，速度越大，評價得分越高。

1.4 歸一化處理

為防止評價指標中某一項占比過高，在優(yōu)化之前還需要的步驟是對評價函數(shù)的各項做歸一化處理：

公式當(dāng)中的為動態(tài)窗口內(nèi)的軌跡，為當(dāng)前需要進行評價所對應(yīng)的軌跡。

2 融合算法路徑規(guī)劃

2.1 A*算法

作為一種采用啟發(fā)式搜索方法的A*算法，搜索出來的路徑必須是在靜態(tài)狀態(tài)空間中的，A*算法搜索過程相對其他算法比較直觀清晰，它是屬于遍歷的確定性搜索方式，所以是解決地圖環(huán)境預(yù)知的情況下全局路徑搜索問題十分經(jīng)典的一種算法。A*算法通過下列的公式（14）來計算每個節(jié)點所對應(yīng)的優(yōu)先級。

公式里面的（）是節(jié)點的估價相關(guān)函數(shù)，選擇下一個節(jié)點遍歷時，需要選取一個綜合利用優(yōu)先級程度最高的節(jié)點；（）是在狀態(tài)空間中重要節(jié)點距離起點的代價；（）表示的是估計成本,是A*算法的啟發(fā)函數(shù)。對于網(wǎng)格形式的地圖，曼哈頓距離、對角距離和歐幾里得距離是啟發(fā)函數(shù)的可以采用的計算距離。本文采用對角距離作為A*算法的啟發(fā)函數(shù)。

2.2 動態(tài)窗口算法

動態(tài)窗口方法（dynamic window approach，DWA）是應(yīng)用比較廣泛的一種算法，主要用于局部路徑規(guī)劃。DWA通過選擇無人機的速度，不斷向前搜索使快速到達目標位置，對于空間中存在的對無人機有威脅的障礙物也能夠及時的躲避。將無人機的動態(tài)特性加入到該方法中，能夠找到無人機飛行過程可使用的最佳速度，并且能夠把搜索的空間縮小到在動態(tài)限制下能采用的速度，為無人機飛行提供保障。

DWA算法原理是在速度空間內(nèi)采樣無人機的線速度和角速度，并按照無人機的運動學(xué)模型預(yù)測后續(xù)飛行的軌跡，對存在多條預(yù)測的軌跡評分，最終得到平滑、安全、具有穩(wěn)定性的最優(yōu)局部路徑。DWA算法的流程如圖2所示。

圖2 DWA算法流程

2.3 融合算法

A*算法可以得到全局路徑規(guī)劃中的最優(yōu)路徑，在靜態(tài)工作環(huán)境中能很好地完成全局路徑規(guī)劃能力。DWA算法路徑規(guī)劃缺少全局指引，只有目的地一個方向指引，在障礙物較多的環(huán)境中容易使無人機的路徑規(guī)劃陷入局部最優(yōu)，導(dǎo)致路徑規(guī)劃失敗。

DWA的優(yōu)勢在于局部的路徑規(guī)劃，檢測窗口滾動前進，有非常不錯的避障效果，獲取局部的最優(yōu)路徑。本文通過采用全局與局部的相結(jié)合的方法，將A*與動態(tài)窗口的優(yōu)勢融合在一起，確保給無人機規(guī)劃的路徑最優(yōu)。融合算法流程如圖3所示。

圖3 融合算法流程

3 仿真實驗分析

3.1 仿真環(huán)境描述

確保融合算法的可靠性和有效性，使用Matlab R2016a仿真工具為實驗平臺進行仿真實驗。動態(tài)窗口法的相關(guān)參數(shù)設(shè)置為：起始的坐標點為（0，0），目標點所在位置為（18，18），初始方位角為π/2，初始線速度為0 m/s，初始角速度0 rad/s，最大線速度1 m/s，最大角速度設(shè)置為20 rad/s，最大線加速度設(shè)置為為0.2 m/s，最大角加速度設(shè)置為50 rad/s，線速度的分辨率為0.01 m/s，角速度的分辨率為1 rad/s，時間分辨率為0.1 s，預(yù)測周期間隔為3 s。

3.2 仿真結(jié)果分析

從實驗結(jié)果可以看出，A*算法能夠在地圖上找到較短的路徑，但是路徑不平滑，轉(zhuǎn)彎方式是折線轉(zhuǎn)彎，在實際應(yīng)用中容易導(dǎo)致無人機在障礙物的拐角處與障礙物發(fā)生碰撞。動態(tài)窗口算法能確保無人機有效的避開障礙物，而且所規(guī)劃出來的路徑與周圍的障礙能有一個最小的間隔，不會發(fā)生碰撞的情況，但該算法運行時間較長、所規(guī)劃路徑不是最短路徑。對比DWA算法與融合算法的線速度圖，可以看出融合算法能讓無人機的速度保持好的穩(wěn)定性。融合算法將兩者有效的結(jié)合，彌補了各自的缺點，在確保全局最優(yōu)路徑的狀態(tài)下，能夠有效地避開障礙物，并保持路徑的平滑性，確保無人機在工作飛行過程能夠一直保持良好的姿態(tài)。

圖4 A＊算法路徑規(guī)劃

圖5 DWA算法路徑規(guī)劃

圖6 A＊與DWA融合算法路徑規(guī)劃

圖7 DWA算法線速度

圖8 融合算法線速度

表1 算法性能指標對比

4 結(jié)語

本文把A*算法和動態(tài)窗口算法融合在一起的無人機路徑規(guī)劃的方法，將二者的優(yōu)勢聚集在一起，在確保全局最優(yōu)可行路徑的前提下，進行局部避障，保證了路徑的平滑穩(wěn)定性。仿真實驗結(jié)果表明，該融合算法切實有效，能夠滿足無人機的運動約束，可以合理地規(guī)劃出優(yōu)良路徑，使無人機安全到達目標位置，表明了該融合算法在無人機路徑規(guī)劃中具有可行性。