多約束的無人機動態路徑規劃算法研究

羅銀輝，李榮枝，潘正宵，王星怡

(中國民用航空飛行學院 計算機學院，四川 廣漢 618370)

0 引言

多旋翼無人機具有體積小、機動性高和速度快的特點，在航拍、搜救和巡檢工作中具有廣泛應用。得益于智能移動設備的發展，斯坦福大學通過在無人機上搭載微機電系統(Micro-Electro-Mechanical System，MEMS)傳感器加嵌入式機載電腦的方式[1]，首次實現了無人機的自主飛行，此后，無人機的自主飛行研究迅速發展。無人機在自主飛行過程中，最重要的功能是路徑規劃與避障，即實現導航。與機器人相似，導航的目的在于找到從起點到終點具有避障能力的最優或次優路徑[2]。現實中的環境是復雜多變的，無人機在飛行的過程中除了要面對靜態的障礙物如樹木、燈柱等，還需要面對動態的障礙物如飛鳥和其他無人機等，如何使無人機在動態環境中實現靈活的自主避障，是研究的重要方向。

本文對基于軌跡優化的動態重規劃方法進行研究，采用利于重規劃的高階B樣條曲線，結合提出的3項飛行約束方程，動態重規劃路徑；采用基于時間的再分配策略，實現無人機動態避障，解決動態重規劃路徑算法的實時性問題。

1 算法流程

無人機要實現自主飛行，需要通過感知系統估計出自身的狀態信息，以及周圍的環境地圖[3]。傳統的做法是將障礙物的點云圖收集并轉換為在三維地圖中的占用柵格地圖，再通過路徑搜索算法計算無人機的可通行路徑[4]。但這類地圖在路徑規劃算法的應用過程中效率較低，為了更高效地獲取環境信息，采用包含了梯度信息的歐式有符號距離函數(Euclidean Signed Distance Functions，ESDF)地圖。基于梯度的運動規劃是當前主流的無人機局部路徑規劃方法，該方法將問題表述為無約束線性規劃。Ratliff等[5]率先將ESDF地圖引入機器人的路徑規劃，實現了ESDF地圖使用上的突破。沿著這一思路，后續研究中，許多框架直接利用梯度信息進行優化配置空間中的軌跡[6]，有非常好的效果。

在路徑搜索這一問題上，A*算法是一種經典的路徑搜索算法，在使用A*算法進行路徑規劃時，雖然能找到最短路徑，但A*算法不考慮無人機的動力學可行性，在實際飛行過程中，無人機可能會面臨危險[7]，且在面對動態環境時，A*算法并不適用[8]。文獻[9]采用高階B樣條曲線進行路徑規劃，該方式的優點在于預規劃的路徑更為平滑，且有助于動態重規劃。為了實現動態重規劃，文獻[5-6]就平滑、碰撞和可行性創建懲罰項，以此設計約束方程，對軌跡進行動態規劃。

然而，部分動態規劃算法會將時間這一維度抽離[10]，用離散的時間進行軌跡優化。這樣的方式并不適用于無人機，因為它對動態約束更加敏感。為此， Oleynikova等[11]提出了一種用于無人機規劃的連續時間多項式軌跡優化方法。本文借鑒這一思想設計時間再分配策略。

文中的路徑規劃算法采用B樣條曲線進行預規劃路徑，根據飛行約束方程，結合ESDF地圖提供的梯度信息，實現路徑搜索。在飛行的過程中，結合時間再分配策略，進行無人機飛行軌跡的動態規劃，實現動態避障。算法流程如圖1所示。

圖1 算法流程Fig.1 Flow chart of algorithm

2 算法實現

2.1 ESDF地圖

進入21世紀，ESDF被用于從傳感器中過濾嘈雜的數據，并構建對象[12]，而Ratliff等[5]帶來了新的思路，將ESDF用于機器人的運動規劃。ESDF在運用初期，不適用于增量構建，而在四旋翼無人機飛行過程中，需要不斷對場進行動態更新。為此，Han等[13]提出增量ESDF生成方式，實現并驗證了ESDF的動態更新。

ESDF源于TSDF(Truncated Signed Distance Functions)，TSDF對距離信息進行了截斷，即遠離障礙物曲面一定距離后，視為無限大。這樣的處理方式導致路徑信息在穿過無限大的區域時，無法獲取梯度信息。當控制點在障礙物里面時，無法被排斥出來。ESDF不對距離信息進行截斷，可以很方便地對當前位置到障礙物表面的距離和梯度信息進行查詢。采用開源工具FIESTA的原理[13]，可以快速生成并動態更新ESDF地圖。其流程如圖2所示。

圖2 FIESTA流程Fig.2 Flow chart of FIESTA

第一步，由傳感器獲取周圍環境的位置信息和深度信息。

第二步，使用光線追蹤法將點云疊加到占有的柵格地圖中，然后將所有占用狀態發生改變的體素分別添加到insertQueue和deleteQueue兩個隊列中。

第三步，初始化ESDF，將前2個隊列合并到updateQueue隊列中，并使用基于廣度優先搜索算法的ESDF更新算法更新所有可能更改的體素。

2.2 B樣條曲線

傳統尋路算法如A*算法等，在生成路徑時，部分路徑段存在尖銳拐點。為了保障無人機飛行的安全性，采用B樣條曲線進行路徑優化可獲得平滑路徑[14]。B樣條曲線的核心在于節點的劃分，在無人機的路徑規劃中，B樣條曲線的節點就是一個個的控制點Q。控制點作為決策變量，影響一段曲線的形狀。B樣條曲線的數學表達式如下：

(1)

式中，Ni,k(x)是k次B樣條基函數，又稱調和函數，是一個遞歸函數。其遞歸表達式如下：

(2)

(3)

Dk=(x+k-i-y)k。

(4)

路徑規劃過程中，先生成初始控制點，并通過迭代的方式檢測路徑信息，對于迭代中檢測到的每個碰撞段，生成無碰撞路徑Г。之后，每個控制點Qi將在障礙物表面上分配一個錨定點Pij，并具有相應的排斥方向向量vij，如圖3所示。

圖3 控制點與錨定點Fig.3 Control points and anchor points

圖中，i為控制點的個數，j為錨定點的個數，每一個錨定點和它的方向向量組合成一個(P，v)對，一個(P，v)對只對應一個控制點Qi。一個控制點到第j個錨定點的距離，即控制點到障礙物表面的距離可定義為dij，計算如下：

dij=(Qi-Pij)·vij。

(5)

2.3 飛行約束方程

為了將環境信息融入路徑規劃中，需要構造一個目標函數，即飛行約束方程，使軌跡可以遠離障礙物。為了提高軌跡評估效率，算法中使用的B樣條曲線，每一個控制點的時間間隔(Δt)是相同的。約束方程參照Faster-planner中對于四旋翼無人機局部路徑規劃的框架[13]，每個控制點的速度Vi，加速度Ai和加加速度Ji表示如下：

(6)

參考文獻[15]，將控制點的優化問題抽象為：

(7)

式中，Js表示平滑懲罰項；Jc表示碰撞懲罰項；Jd表示可行性懲罰項；γ表示各懲罰項的權值。

2.3.1 平滑懲罰項

平滑懲罰項用于控制無人機的加速度，保障無人機在飛行過程中不會出現急加速。受益于B樣條曲線的凸包性，最小化控制點的加速度及加加速度，就可以最小化曲線的高階導數，使整條曲線更加平滑，因此，平滑懲罰項的可表示為：

(8)

式中，N表示控制點的個數。

2.3.2 碰撞懲罰項

碰撞懲罰項用于將控制點通過場的作用推出障礙物。通過定義一個安全通道Sf，將控制點中dij

(9)

由于每一個控制點單獨計算碰撞懲罰項，曲線上整體的碰撞懲罰項的計算如下：

(10)

2.3.3 可行性懲罰項

為確保軌跡的可行性，需要在曲線的每個維度上限制其高階導數，即確保Vi，Ai和Ji都是可計算且有意義的。由于B樣條曲線的凸包性，約束每一個控制點的導數，即可約束整條曲線。則可行性懲罰項Jd可表示如下：

(11)

式中，w為相應的權值；函數F()是控制點的二次連續可微度量函數：

(12)

f(cr)的計算為分段函數：

(13)

式中，cr∈C∈{Vi,Ai,Ji}；a1，b1，c1，a2，b2，c2用來滿足函數二階連續性；cm為導數限制；cj為二次和三次函數的交界；λ<1-ε(ε?1)為彈性系數，使得最終的結果滿足約束。

2.4 時間再分配策略

目標函數J在飛行過程中自適應地根據新發現的障礙物進行改變，這個過程就是動態重規劃。動態重規劃中，時間間隔是一個關鍵變量，但在優化之前分配一個精確的時間間隔是不合理的，因為在初始化時不知道關于最終軌跡的信息。因此，額外的時間重新分配策略對于確保動態可行性至關重要。文獻[16]將軌跡參數化為非均勻B樣條曲線，并在某些線段超過導數極限時，迭代地延長屬于一個時間間隔的控制點子集。

然而，時間間隔會影響多個控制點，且在開始狀態附近調整時間間隔時，會導致前一段軌跡的高階不連續性。使用各向異性曲線擬合方法[17]，根據約束方程計算生成安全軌跡Φs。通過合理的時間重新分配，重新生成均勻的B樣條軌跡Φf，使Φf自由優化其控制點，以滿足高階導數約束，同時保持與Φs幾乎相同的形狀。

需要比較控制點在改變后的變化情況，計算超過限制的比例re，即相比于Φs，Φf需要多分配多少時間。re的計算如下：

(14)

式中，i∈{1,2,…,N-1}；j∈{1,2,…,N-2}；k∈{1,2,…,N-3}；r∈{x,y,z}；Vi，Aj，Jk分別與Δt的一次，二次，三次成反比。計算出re后，就可以計算出屬于Φf的新的時間間隔Δt′：

Δt′=reΔt。

(15)

控制點通過橢球度量(Spheroidal Metric)進行移動，該方法借助橢圓的性質來使同一球體表面的位移產生相同的懲罰，保障控制點移動過后，生成的Φf依然符合飛行約束。為了實現控制點的移動，需要建立模型Jf來表示從Φf到Φs的各項異性位移的積分。將Φf和Φs分別細化為αT′和αT(T′和T分別為軌跡Φf和Φs的時長，α∈[0,1])，由于初始曲線Φs已經符合飛行約束方程，實現了無碰撞，因此，對于需要生成的新曲線，使用帶有低權重的軸向位移da來放寬光滑調整限制，用高權重的徑向位移dr來防止碰撞。時間再分配示意如圖4所示。

圖4 時間再分配示意Fig.4 Schematic diagram of time reallocation

(16)

由橢圓體的長半軸a和短半軸b計算軸向位移da和徑向位移dr的積分，獲得Jf：

(17)

用新的Jf代替原目標函數中的Jc，實現了動態規劃過程中的時間再分配，對原目標函數的實時性進行優化。

3 算法實驗

在Ubuntu20.04環境下的ROS系統中，使用Gazebo進行環境仿真，并在Rviz平臺上展示路徑規劃過程。實驗對比了在不同環境下不同路徑規劃算法的性能，并針對算法中的個別參數，進行對比實驗。通過實驗，分析算法的可行性和實用性。

3.1 在簡單環境下的實驗對比

在簡單的環境中，設置了數量較少的障礙物，地圖大小為20 m×20 m×5 m，設置統一的起點和終點，設置無人機的最大飛行速度為3 m/s，采用不同的算法進行實驗。實驗飛行軌跡如圖5所示。由圖5可以看出，動態規劃的路徑與傳統的A*算法在路徑規劃上有很大區別。相較于A*算法，采用了B樣條曲線的方法，規劃的路徑更為平滑，能更好地適應無人機的動力控制。

圖5 簡單環境飛行軌跡Fig.5 Flight path in simple environment

實驗記錄了采用不同算法時無人機的飛行軌跡長度、規劃時間和飛行時間，如表1所示。由表1可以看出，傳統的A*算法[18]在路徑規劃的計算速度上有很大的優勢，且能找到最短飛行路徑。本文算法性能上與Fast-planner[13]相當，但相比于Fast-planner，可以找到更短一些的路徑。

表1 算法對比Tab.1 Comparison of algorithm

為了更直觀地比較飛行過程中無人機狀態的區別，記錄了采用不同算法時，無人機飛行過程中的速度，如圖6所示。可以看出，采用A*算法時，無人機的速度波動較大，Fast-planner和本文算法能更好地控制無人機勻速飛行。

圖6 無人機速度變化對比 Fig.6 Comparison of UAV speed changes

3.2 在復雜環境下的實驗對比

將地圖擴大，變為50 m×50 m×5 m的大地圖，且在地圖中隨機生成200個障礙物，其中存在少量速度為0.5 m/s做巡回運動的動態障礙物。對采用動態規劃思想的2種算法進行對比，飛行軌跡如圖7所示。

圖7 復雜環境飛行軌跡Fig.7 Flight path in complex environment

無人機在規避動態障礙物時的路徑改變如圖8所示，圖中綠色方塊為移動中的障礙物，圖片中的曲線變化展示了無人機在遇到動態障礙物時，根據約束方程進行避障的過程。

圖8 動態避障軌跡變化Fig.8 Trajectory change for dynamic obstacle avoidance

與前面的實驗相同，記錄飛行的路徑長度與時間等信息，動態規劃算法對比如表2所示。由表2可以看出，本文算法在復雜環境下仍能快速找到合適的路徑，且尋路能力相較于Fast-planner有所提升。在面對動態障礙物時，也能及時改變飛行軌跡，實現動態避障。

表2 動態規劃算法對比Tab.2 Comparison of dynamic planning algorithms

針對控制點的個數對飛行質量的影響，在復雜地圖中隨機選取起點和終點位置，就控制點數量，各進行50次實驗，記錄實驗的成功率和平均規劃時間，如表3所示。針對ESDF地圖分辨率對算法的影響，做同類型對比實驗，改變地圖分辨率，以相同的起點和終點進行飛行，記錄飛行時間和規劃時間，實驗結果如表4所示。

表3 控制點數量對比Tab.3 Comparison of number of control points

表4 地圖分辨率對比Tab.4 Comparison of map resolution

由表3可以看出，控制點的數量會影響飛行的成功率，控制點越多，說明提前規劃的路線越長，越能提前找到合適的路徑，但規劃所需的時間也隨之增長。地圖分辨率對于算法的影響在于規劃時間上的區別。由表4可以看出，高分辨率的情況下，需要更長的時間進行規劃，但能找到更貼近障礙物安全面的路徑，減少路徑上的消耗。

4 結束語

本文提出的B樣條曲線結合時間再分配的無人機動態避障算法，可以做到根據無人機飛行過程中動態生成的ESDF地圖，迅速規劃一條平滑、安全的路徑。該路徑中各控制點均符合無人機的動力學模型，有利于無人機的飛行控制。

基于Gazebo和Rviz的模擬仿真環境的實驗，驗證了算法的有效性。在面對外界干擾如動態障礙物時，算法依然保證了無人機選擇合理的路徑。算法具有良好的實時性，擁有廣泛的應用前景。

本文提出的方法，依賴于ESDF地圖，在地圖的構建中消耗了大量計算時間，如何提高計算速度，減少對ESDF地圖的依賴，是后續研究的方向。