基于改進動態窗口法的無人機避障航跡規劃

戴凱龍，賈子彥，潘玲佼

（江蘇理工學院 電氣信息工程學院，江蘇 常州 213000）

0 引 言

避障與航跡規劃一直是無人機系統的研究重點，這就對如何控制無人機進行自主導航避障提出了要求。大多數的航跡規劃算法中，忽略了無人機的狀態和運動學約束。如A*算法、蟻群算法、人工勢場法等，規劃出的航跡轉折較大，無法直接使用，往往需要進行軌跡優化等操作，其代價隨搜索空間的維度和復雜度的增大而明顯增加。在航跡規劃時，應當充分考慮無人機自身的狀態和性能變化，使仿真更貼近于實際，增強實際控制的魯棒性。

動態 窗 口 法（Dynamic Windows Approach，DWA）是一種直接在控制指令空間內搜索最佳控制指令的自主避障算法。根據智能體的運動學特性，得到一系列離散的速度控制組合，并模擬在各控制輸入下的軌跡，最后通過最大化目標函數來選擇下一刻的最優控制輸入。

針對動態窗口法存在易陷入局部最優的問題，很多研究人員考慮將全局路徑規劃算法與局部路徑規劃算法相結合。首先通過全局規劃算法尋找出一條最優的路徑，然后提取路徑中的關鍵節點，最后以局部路徑規劃算法逐一跟蹤這些關鍵節點來不斷朝目標點逼近。但是，該方法存在一些局限性：一是很多情況下的探索任務需要實時進行，無法取得完整的環境信息；二是路徑的好壞與全局規劃出的關鍵節點有很大關系，在受到動態障礙物的影響偏離航線后，不能保證不會陷入局部最優中。

基于以上分析，本文針對飛行環境中存在諸多障礙物的情況，搭建了無人機的運動模型，采用動態窗口法來預測避障軌跡，并選擇最優的速度作為控制輸入。同時針對局部最優問題，給出了一種簡單的失效環境判別方法，并通過設置虛擬目標點的方法實現了規避。

1 無人機運動學模型

對無人機進行分析前，需要對模型進行一定的簡化，只考慮無人機的質心運動學方程。當無人機存在全向運動時，則在二維笛卡爾坐標系下的運動學方程為：

其中，x，y，θ、分別為無人機當前的位置和偏轉角度；v、v為無人機在機體坐標系下，軸和軸兩個方向的飛行速度；為偏轉速度；為時間間隔；x，y，θ為無人機下一時刻的位置和偏轉角度。

2 動態窗口法

根據無人機速度和位置的對應關系，將無人機的位置控制轉換為對無人機的速度控制，將避障問題轉換成速度空間帶約束的優化問題。

2.1 無人機速度空間

同時，無人機在水平飛行存在最大飛行速度和最大偏轉速度以及最小偏轉速度的限制，速度空間里的速度還應滿足式（3）的條件：

2.2 軌跡預測

在預測軌跡前，需要按照一定的速度分辨率和偏轉速度分辨率，將速度空間離散成有限的速度組合。在預測軌跡時，保持控制輸入不變，即加速度和偏轉加速度為0，然后根據式（1）對無人機在每一組速度下飛行一段時間后的軌跡進行預測，同時將其中會與障礙物發生碰撞的軌跡剔除。預測軌跡如圖1所示。

圖1預測軌跡仿真Fig.1 Simulation of predicted trajectory

2.3 目標函數

為了使無人機能夠更好、更安全地到達目標點，通過目標函數對預測軌跡進行評價，其形式如下：

其中，航向得分項（v，v，）度量該條預測軌跡速度下軌跡末端的飛行方向與目標方向之間的偏離程度，其公式為180（為軌跡末端的飛行方向與目標方向之間的角度差）；距離得分項（v，v，）表示該條軌跡到最近障礙物的距離，用來剔除會發生碰撞的軌跡以保證飛行安全，當無障礙物時，將函數值設置為一個常數；速度得分項（v，v，）用于從能夠安全避障的軌跡中選擇出飛行速度較快的軌跡，其值為合速度的模；，，分別為3個得分項的權重。

在計算完所有軌跡的得分后，需要對每條軌跡的各個得分項進行歸一化（σ）處理，然后再相加。歸一化的準則是每一項除以每一項的總和。以航向得分項為例，其計算公式如式（5）：

其中，為待評價的當前軌跡，為所有預測軌跡的采樣數量。

最后，選擇使目標函數值最大的速度，作為下一時刻的控制輸入。重復上述過程，直至到達目標點。

3 算法失效環境規避

當目標點出現在“U”型障礙物后面時，容易陷入局部最優，使算法失效。但是，在這種情況發生時很容易被檢測到。

3.1 算法失效環境判斷

假設無人機可獲得周圍中等或較低精度的障礙物距離和方位，選取前進方向上一定角度扇形探測區域，作為算法失效環境的判斷依據，如圖2所示。當障礙物與無人機之間的距離小于閾值R時，分析扇形區域中角度內探測到的距離信息。若角度中沒有可通行區域，且距離信息整體呈現“短長短”的形式，則判斷前方為“U”型失效環境，需要進行規避，否則判斷為一般障礙物，不做處理。

圖2 失效環境判斷Fig.2 Judgment of failure environment

3.2 基于虛擬逃逸目標點的障礙規避

由于原目標點無法幫助無人機規避失效環境，因此需要人為設置一個虛擬的目標點進行逃逸。另外，這個虛擬目標點應使無人機可達且安全。在一些特殊情況下，可能會檢測出沒有安全的區域。此時可以對探測信息進行二次處理，從無人機就近的安全區域中尋找逃逸目標點。

將傳感器的扇形探測區域按照一定的角度（）均等分割，虛擬目標點從沒有障礙物的分塊區域中選擇，應滿足式（6）的要求：

其中，為扇形分塊區域與最近障礙物之間的夾角；為扇形分塊區域與目標點之間的夾角；，分別為相應系數；為歸一化處理。

通過得分選擇一個比較安全，且使無人機偏離目標點越小的分塊區域的中點位置，作為逃逸時的虛擬目標點。同時增加（v，v，）得分項，使無人機朝虛擬目標點逃逸，其形式如式（7）：

其中，φ為預測軌跡末端的前進方向與到虛擬目標點方向之間的夾角；為失效環境標志量，當判定前方障礙物為“U”型失效環境時，為0，否則為1。

為避免航向得分項在逃逸期間影響無人機，對航向得分項進行修改，其形式如式（8）：

當沒有探測到失效環境時，將目標點作為虛擬目標點，同時對目標函數進行改進。

3.3 算法流程

本文算法的具體實現步驟如下：

獲得無人機當前的飛行速度和方向，并根據運動學特性，分別計算無人機下一刻所能達到的軸、軸方向的速度，以及偏轉速度的范圍；

根據速度范圍生成速度空間，并在速度空間中，對各個速度進行離散采樣，得到一系列速度（v，v，）；

根據無人機的運動學方程，對采樣得到的速度進行模擬，進而得到對應速度下的預測軌跡；

根據傳感器信息，判斷前方飛行區域是否存在“U”型失效環境，若存在則從探測區域中尋找虛擬目標點進行逃逸；否則將目標點作為虛擬目標點進行正常飛行；

采用目標函數對預測軌跡進行評價，然后選擇目標函數值最大的軌跡對應下的速度，作為下一時刻的控制輸入；

執行最優速度，基于當前位置判斷是否到達目標點，若已到達則結束運行；否則返回第一步，進入下一循環。

4 仿真驗證及結果分析

4.1 基本參數

為了驗證本文算法的有效性，對算法進行仿真實驗。仿真環境為MATLAB R2021b，算法涉及無人機自身參數與算法參數。根據實際情況和常規硬件配置，本文選取參數見表1。

表1 算法相關參數Tab.1 Relevant parameters of the algorithm

仿真時將無人機當作質點處理（實際無人機具有一定的體積），因此設置了無人機與障礙物的碰撞距離閾值（仿真閾值為0.5 m），同時當無人機到達目標點半徑0.2 m范圍內，便視為到達目標點。為了避免逃逸目標點不可達的情況，無人機在逃逸目標點附近1.5 m范圍內視為到達。

4.2 避障實驗

建立如圖3所示的24×24的環境地圖，為了避免環境的偶然性，分別設計了兩條路線。分別為起點（4，9）到目標點（13，21），以及起點（3，3）到目標點（22，17），黑色方格為障礙物。由仿真結果可見，當目標點出現在“U”型障礙物后面時，改進前的算法面對“U”型障礙物時直接陷入了局部最優，而改進后的算法通過虛擬目標點，實現了逃逸，并最終到達目標點。

圖3 算法仿真圖Fig.3 Simulation diagram of algorithm

4.3 動態環境實驗

為了驗證本文算法在動態環境中的性能，仿真結果如圖4所示。在環境中加入一些動態障礙物（圖中的紅色圓）后，無人機飛行時不但能夠躲避環境中的動態障礙物，同時還能夠避免因躲避障礙物而陷入局部最優的情況。

圖4 動態環境實驗Fig.4 Experiment in dynamic environment

5 結束語

本文考慮了無人機帶有運動約束情況的避障問題，使用動態窗口法來尋找最優的避障路徑，同時提出了一種依靠局部環境信息來規避“U”型失效環境的方法。仿真結果表明：本文算法能夠使無人機在避障規劃時避免陷入局部最優的情況，實現了在動態復雜環境中的自主飛行。