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

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

0 引言

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

本文對基于軌跡優(yōu)化的動(dòng)態(tài)重規(guī)劃方法進(jìn)行研究，采用利于重規(guī)劃的高階B樣條曲線，結(jié)合提出的3項(xiàng)飛行約束方程，動(dòng)態(tài)重規(guī)劃路徑；采用基于時(shí)間的再分配策略，實(shí)現(xiàn)無人機(jī)動(dòng)態(tài)避障，解決動(dòng)態(tài)重規(guī)劃路徑算法的實(shí)時(shí)性問題。

1 算法流程

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

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

然而，部分動(dòng)態(tài)規(guī)劃算法會將時(shí)間這一維度抽離[10]，用離散的時(shí)間進(jìn)行軌跡優(yōu)化。這樣的方式并不適用于無人機(jī)，因?yàn)樗鼘?dòng)態(tài)約束更加敏感。為此， Oleynikova等[11]提出了一種用于無人機(jī)規(guī)劃的連續(xù)時(shí)間多項(xiàng)式軌跡優(yōu)化方法。本文借鑒這一思想設(shè)計(jì)時(shí)間再分配策略。

文中的路徑規(guī)劃算法采用B樣條曲線進(jìn)行預(yù)規(guī)劃路徑，根據(jù)飛行約束方程，結(jié)合ESDF地圖提供的梯度信息，實(shí)現(xiàn)路徑搜索。在飛行的過程中，結(jié)合時(shí)間再分配策略，進(jìn)行無人機(jī)飛行軌跡的動(dòng)態(tài)規(guī)劃，實(shí)現(xiàn)動(dòng)態(tài)避障。算法流程如圖1所示。

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

2 算法實(shí)現(xiàn)

2.1 ESDF地圖

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

ESDF源于TSDF(Truncated Signed Distance Functions)，TSDF對距離信息進(jìn)行了截?cái)啵催h(yuǎn)離障礙物曲面一定距離后，視為無限大。這樣的處理方式導(dǎo)致路徑信息在穿過無限大的區(qū)域時(shí)，無法獲取梯度信息。當(dāng)控制點(diǎn)在障礙物里面時(shí)，無法被排斥出來。ESDF不對距離信息進(jìn)行截?cái)?，可以很方便地對?dāng)前位置到障礙物表面的距離和梯度信息進(jìn)行查詢。采用開源工具FIESTA的原理[13]，可以快速生成并動(dòng)態(tài)更新ESDF地圖。其流程如圖2所示。

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

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

第二步，使用光線追蹤法將點(diǎn)云疊加到占有的柵格地圖中，然后將所有占用狀態(tài)發(fā)生改變的體素分別添加到insertQueue和deleteQueue兩個(gè)隊(duì)列中。

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

2.2 B樣條曲線

傳統(tǒng)尋路算法如A*算法等，在生成路徑時(shí)，部分路徑段存在尖銳拐點(diǎn)。為了保障無人機(jī)飛行的安全性，采用B樣條曲線進(jìn)行路徑優(yōu)化可獲得平滑路徑[14]。B樣條曲線的核心在于節(jié)點(diǎn)的劃分，在無人機(jī)的路徑規(guī)劃中，B樣條曲線的節(jié)點(diǎn)就是一個(gè)個(gè)的控制點(diǎn)Q。控制點(diǎn)作為決策變量，影響一段曲線的形狀。B樣條曲線的數(shù)學(xué)表達(dá)式如下：

(1)

式中，Ni,k(x)是k次B樣條基函數(shù)，又稱調(diào)和函數(shù)，是一個(gè)遞歸函數(shù)。其遞歸表達(dá)式如下：

(2)

(3)

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

(4)

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

圖3 控制點(diǎn)與錨定點(diǎn)Fig.3 Control points and anchor points

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

dij=(Qi-Pij)·vij。

(5)

2.3 飛行約束方程

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

(6)

參考文獻(xiàn)[15]，將控制點(diǎn)的優(yōu)化問題抽象為：

(7)

式中，Js表示平滑懲罰項(xiàng)；Jc表示碰撞懲罰項(xiàng)；Jd表示可行性懲罰項(xiàng)；γ表示各懲罰項(xiàng)的權(quán)值。

2.3.1 平滑懲罰項(xiàng)

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

(8)

式中，N表示控制點(diǎn)的個(gè)數(shù)。

2.3.2 碰撞懲罰項(xiàng)

碰撞懲罰項(xiàng)用于將控制點(diǎn)通過場的作用推出障礙物。通過定義一個(gè)安全通道Sf，將控制點(diǎn)中dij

(9)

由于每一個(gè)控制點(diǎn)單獨(dú)計(jì)算碰撞懲罰項(xiàng)，曲線上整體的碰撞懲罰項(xiàng)的計(jì)算如下：

(10)

2.3.3 可行性懲罰項(xiàng)

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

(11)

式中，w為相應(yīng)的權(quán)值；函數(shù)F()是控制點(diǎn)的二次連續(xù)可微度量函數(shù)：

(12)

f(cr)的計(jì)算為分段函數(shù)：

(13)

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

2.4 時(shí)間再分配策略

目標(biāo)函數(shù)J在飛行過程中自適應(yīng)地根據(jù)新發(fā)現(xiàn)的障礙物進(jìn)行改變，這個(gè)過程就是動(dòng)態(tài)重規(guī)劃。動(dòng)態(tài)重規(guī)劃中，時(shí)間間隔是一個(gè)關(guān)鍵變量，但在優(yōu)化之前分配一個(gè)精確的時(shí)間間隔是不合理的，因?yàn)樵诔跏蓟瘯r(shí)不知道關(guān)于最終軌跡的信息。因此，額外的時(shí)間重新分配策略對于確保動(dòng)態(tài)可行性至關(guān)重要。文獻(xiàn)[16]將軌跡參數(shù)化為非均勻B樣條曲線，并在某些線段超過導(dǎo)數(shù)極限時(shí)，迭代地延長屬于一個(gè)時(shí)間間隔的控制點(diǎn)子集。

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

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

(14)

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

Δt′=reΔt。

(15)

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

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

(16)

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

(17)

用新的Jf代替原目標(biāo)函數(shù)中的Jc，實(shí)現(xiàn)了動(dòng)態(tài)規(guī)劃過程中的時(shí)間再分配，對原目標(biāo)函數(shù)的實(shí)時(shí)性進(jìn)行優(yōu)化。

3 算法實(shí)驗(yàn)

在Ubuntu20.04環(huán)境下的ROS系統(tǒng)中，使用Gazebo進(jìn)行環(huán)境仿真，并在Rviz平臺上展示路徑規(guī)劃過程。實(shí)驗(yàn)對比了在不同環(huán)境下不同路徑規(guī)劃算法的性能，并針對算法中的個(gè)別參數(shù)，進(jìn)行對比實(shí)驗(yàn)。通過實(shí)驗(yàn)，分析算法的可行性和實(shí)用性。

3.1 在簡單環(huán)境下的實(shí)驗(yàn)對比

在簡單的環(huán)境中，設(shè)置了數(shù)量較少的障礙物，地圖大小為20 m×20 m×5 m，設(shè)置統(tǒng)一的起點(diǎn)和終點(diǎn)，設(shè)置無人機(jī)的最大飛行速度為3 m/s，采用不同的算法進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)飛行軌跡如圖5所示。由圖5可以看出，動(dòng)態(tài)規(guī)劃的路徑與傳統(tǒng)的A*算法在路徑規(guī)劃上有很大區(qū)別。相較于A*算法，采用了B樣條曲線的方法，規(guī)劃的路徑更為平滑，能更好地適應(yīng)無人機(jī)的動(dòng)力控制。

圖5 簡單環(huán)境飛行軌跡Fig.5 Flight path in simple environment

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

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

為了更直觀地比較飛行過程中無人機(jī)狀態(tài)的區(qū)別，記錄了采用不同算法時(shí)，無人機(jī)飛行過程中的速度，如圖6所示。可以看出，采用A*算法時(shí)，無人機(jī)的速度波動(dòng)較大，F(xiàn)ast-planner和本文算法能更好地控制無人機(jī)勻速飛行。

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

3.2 在復(fù)雜環(huán)境下的實(shí)驗(yàn)對比

將地圖擴(kuò)大，變?yōu)?0 m×50 m×5 m的大地圖，且在地圖中隨機(jī)生成200個(gè)障礙物，其中存在少量速度為0.5 m/s做巡回運(yùn)動(dòng)的動(dòng)態(tài)障礙物。對采用動(dòng)態(tài)規(guī)劃思想的2種算法進(jìn)行對比，飛行軌跡如圖7所示。

圖7 復(fù)雜環(huán)境飛行軌跡Fig.7 Flight path in complex environment

無人機(jī)在規(guī)避動(dòng)態(tài)障礙物時(shí)的路徑改變?nèi)鐖D8所示，圖中綠色方塊為移動(dòng)中的障礙物，圖片中的曲線變化展示了無人機(jī)在遇到動(dòng)態(tài)障礙物時(shí)，根據(jù)約束方程進(jìn)行避障的過程。

圖8 動(dòng)態(tài)避障軌跡變化Fig.8 Trajectory change for dynamic obstacle avoidance

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

表2 動(dòng)態(tài)規(guī)劃算法對比Tab.2 Comparison of dynamic planning algorithms

針對控制點(diǎn)的個(gè)數(shù)對飛行質(zhì)量的影響，在復(fù)雜地圖中隨機(jī)選取起點(diǎn)和終點(diǎn)位置，就控制點(diǎn)數(shù)量，各進(jìn)行50次實(shí)驗(yàn)，記錄實(shí)驗(yàn)的成功率和平均規(guī)劃時(shí)間，如表3所示。針對ESDF地圖分辨率對算法的影響，做同類型對比實(shí)驗(yàn)，改變地圖分辨率，以相同的起點(diǎn)和終點(diǎn)進(jìn)行飛行，記錄飛行時(shí)間和規(guī)劃時(shí)間，實(shí)驗(yàn)結(jié)果如表4所示。

表3 控制點(diǎn)數(shù)量對比Tab.3 Comparison of number of control points

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

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

4 結(jié)束語

本文提出的B樣條曲線結(jié)合時(shí)間再分配的無人機(jī)動(dòng)態(tài)避障算法，可以做到根據(jù)無人機(jī)飛行過程中動(dòng)態(tài)生成的ESDF地圖，迅速規(guī)劃一條平滑、安全的路徑。該路徑中各控制點(diǎn)均符合無人機(jī)的動(dòng)力學(xué)模型，有利于無人機(jī)的飛行控制。

基于Gazebo和Rviz的模擬仿真環(huán)境的實(shí)驗(yàn)，驗(yàn)證了算法的有效性。在面對外界干擾如動(dòng)態(tài)障礙物時(shí)，算法依然保證了無人機(jī)選擇合理的路徑。算法具有良好的實(shí)時(shí)性，擁有廣泛的應(yīng)用前景。

本文提出的方法，依賴于ESDF地圖，在地圖的構(gòu)建中消耗了大量計(jì)算時(shí)間，如何提高計(jì)算速度，減少對ESDF地圖的依賴，是后續(xù)研究的方向。