基于改進(jìn)Hybrid A*的旋翼無人機路徑規(guī)劃算法

彭錦城，彭俠夫，張霄力，陳錦文

廈門大學(xué)，福建 廈門 361102

近年來，四旋翼無人機由于具有機動性強、靈活方便、應(yīng)變能力強、價格便宜等特點，逐漸成為識別、檢測、監(jiān)控、偵察等任務(wù)的理想飛行載體[1]。目標(biāo)識別是監(jiān)控和偵察等后續(xù)任務(wù)的基礎(chǔ)，具有重要的研究意義。但是，現(xiàn)有的路徑規(guī)劃算法容易造成無人機目標(biāo)丟失和檢測精度低等問題。

在無人機快速轉(zhuǎn)動或翻轉(zhuǎn)等情況下，采集的圖像數(shù)據(jù)模糊，從而目標(biāo)識別YOLO v3 算法會出現(xiàn)準(zhǔn)確度降低、目標(biāo)丟失、檢測出錯等問題。因此，在無人機導(dǎo)航飛行過程中，一條平滑、魯棒與安全的路徑，對提高四旋翼無人機的目標(biāo)識別精度具有重要的研究意義[13]。

現(xiàn)今，無人機的路徑規(guī)劃算法可以分為基于優(yōu)化的方法和基于搜索的方法，其中，基于優(yōu)化的方法依賴于平滑和碰撞的約束函數(shù)，軌跡本身可以表示為多項式[2-3]?；谒阉鞯穆窂揭?guī)劃算法在柵格地圖上搜索一條非光滑的路徑，這條路徑可以是參考文獻(xiàn)[4]和文獻(xiàn)[5]中提到的柵格路徑，也可以是基于采樣路徑的快速搜索隨機樹法（RRT）或概率路標(biāo)法（PRM）[6-7]。

然而，無論是基于搜索還是基于優(yōu)化的方法所規(guī)劃出的路徑，都是非光滑的，降低了目標(biāo)檢測算法的準(zhǔn)確率。參考文獻(xiàn)[8]～文獻(xiàn)[10]結(jié)合前端的路徑搜索算法與后端的軌跡優(yōu)化，能夠生成一條符合無人機動力學(xué)和運動學(xué)的運動軌跡。但是這些算法卻過于追求無人機的飛行速度，導(dǎo)致無人機在目標(biāo)檢測過程中，出現(xiàn)準(zhǔn)確率不高和目標(biāo)丟失等問題。

為了提高目標(biāo)識別YOLO v3算法的準(zhǔn)確度，本文采用改進(jìn)二維的Hybrid A*算法，在前端生成三維的無人機飛行路徑，然后在后端對生成的路徑進(jìn)行平滑和碰撞優(yōu)化，并通過B樣條優(yōu)化算法生成一條平滑、魯棒與安全的飛行軌跡。改進(jìn)無人機飛行軌跡的平滑性和魯棒性，從而提高了無人機目標(biāo)識別算法的準(zhǔn)確率，本文方法的創(chuàng)新性主要體現(xiàn)在：（1）改進(jìn)自動駕駛汽車Hybrid A*二維的路徑規(guī)劃算法，并應(yīng)用于四旋翼無人機三維的路徑規(guī)劃；（2）在改進(jìn)Hybrid A*算法的基礎(chǔ)上，對前端生成的路徑，采集相應(yīng)的控制點，使用n次導(dǎo)數(shù)來作為約束函數(shù)軌跡的積分項，進(jìn)行平滑處理；（3）改進(jìn)四旋翼無人機飛行軌跡，提高無人機快速運行時的魯棒性，從而提高目標(biāo)檢測的準(zhǔn)確率的方法，并與現(xiàn)有方法進(jìn)行性能對比。本文提出的路徑規(guī)劃改進(jìn)算法提高了無人機在快速飛行時的魯棒性，從而提高無人機目標(biāo)識別、檢測和偵察時的準(zhǔn)確度，對無人機偵察等應(yīng)用具有重要的研究意義。

1 基于改進(jìn)Hybrid A＊算法的前端路徑規(guī)劃

Hybrid A*算法來自自動駕駛汽車的應(yīng)用，在柵格地圖中搜尋到一個符合汽車運動的、安全的二維路徑，搜索過程與傳統(tǒng)的A*算法相似，創(chuàng)建OPEN 列表和CLOSE 列表，在OPEN 列表中找到規(guī)劃代價最小的父節(jié)點，擴展父節(jié)點周圍的節(jié)點，同時記錄不在CLOSE列表中子節(jié)點的距離起點開銷G和啟發(fā)式開銷H，當(dāng)子節(jié)點的總開銷比父節(jié)點小，更新父節(jié)點，直到找到目標(biāo)點或者OPEN列表為空時，結(jié)束循環(huán)。本節(jié)在Hybrid A*的基礎(chǔ)上，改進(jìn)二維Hybrid A*算法，使其應(yīng)用于三維的四旋翼無人機路徑規(guī)劃搜索，結(jié)合四旋翼無人機動力學(xué)和運動學(xué)，在3D的柵格地圖中生成前端路徑。

1.1 可行運動點的生成

無人機在運動的情況下，需要考慮運動學(xué)的因素，而傳統(tǒng)A*算法只有節(jié)點的擴散，搜索的路徑不符合無人機飛行，且在自動駕駛汽車中，離散控制輸入是在二維平面的，在坐標(biāo)系中，控制二維平面只有xy軸，而無人機的控制是在三維平面的，需要考慮z軸的控制。因此，首先將無人機的運動分解為三個二維平面[8]，再分別進(jìn)行處理，得到下一次無人機的運動點

式中，px(t),py(t),pz(t)分別表示為x，y，z三個平面上的點。

無人機的狀態(tài)方程可以表示為s=[x,y,z,x?,y?,z?]Τ，系統(tǒng)的輸入為u=[x?,y?,z?]Τ，系統(tǒng)模型表示為s?=A?s+B?u。

狀態(tài)轉(zhuǎn)移方程為

圖1 離散控制輸入得到下一刻無人機的可行運動點Fig.1 The discrete control input is used to obtain the feasible movement point of the UAV at the next moment

1.2 構(gòu)建最優(yōu)邊值問題計算啟發(fā)式開銷

在傳統(tǒng)的A*算法中，直接使用歐式距離，且只考慮柵格地圖中障礙物的信息，而忽略了無人機的運動學(xué)和動力學(xué)特性，本文所采用的方法是通過計算最優(yōu)邊值問題，來計算啟發(fā)式的開銷。啟發(fā)式開銷模型可表示為

式中，u(t)為控制輸入。構(gòu)建最優(yōu)邊值問題模型

式中，Jk為所求的開銷；sk為無人機狀態(tài)；jk為無人機的控制輸入；s?為系統(tǒng)方程。根據(jù)龐特里亞金極小值原理，引入?yún)f(xié)態(tài)λ，建立哈密頓方程為

可求得最優(yōu)的控制輸入為

三階多項式軌跡方程為

式中，pμc,vμc是無人機當(dāng)前位置和速度；αμ,βμ為未知變量，其解析式為

式中，x1=pμg-pμc-vμcT，x2=vμg-vμc，pμg,vμg為無人機目標(biāo)位置和速度，根據(jù)所求的最優(yōu)控制輸入u?(t)，代入式（10）

求得啟發(fā)式開銷為

1.3 節(jié)點的擴展及剪枝

在可行運動點生成的基礎(chǔ)上，比較不同節(jié)點的開銷f=g+h，其中g(shù)為實際開銷，h為啟發(fā)式開銷，選擇開銷最小的節(jié)點進(jìn)行擴展，并把開銷大的路徑剪枝，在滿足動力學(xué)和運動學(xué)的基礎(chǔ)上，完成路徑的搜索。在離散控制的情況下，目標(biāo)原點的狀態(tài)很難準(zhǔn)確達(dá)到，因此，如果滿足安全性和動態(tài)可行性要求，且在目標(biāo)點一定范圍內(nèi)，則可以提前終止搜尋，以提高搜尋的效率。節(jié)點的擴展如圖2所示。

圖2 節(jié)點擴展搜尋圖Fig.2 Node expansion search diagram

2 基于B樣條算法的后端軌跡優(yōu)化

在柵格障礙物地圖中，前端生成的改進(jìn)Hybrid A*路徑往往會很靠近障礙物，這樣對無人機的運動飛行是不合理的，因此需要對前端生成的路徑進(jìn)行優(yōu)化，利用ESDF地圖提供的距離信息和梯度信息，進(jìn)行優(yōu)化，生成平滑、魯棒、安全的飛行軌跡。

2.1 控制點的求解

對前端生成的次優(yōu)路徑進(jìn)行等時間T采樣，得到K段路徑以及每段軌跡的首末位置pk、速度vk和加速度ak，每段軌跡利用最小二乘近似求解超定方程，得到控制點

式中，ps,pe為首尾端點位置；vs,ve為首尾端點速度；p0,p1,p2,p3為控制點位置。

2.2 控制點的優(yōu)化

對次優(yōu)路徑的采樣取點，求得的控制點也是次優(yōu)的，需要對它進(jìn)行非線性優(yōu)化處理，從而得到平滑、魯棒、安全的軌跡，總約束函數(shù)模型的建立如下

式中，fs,fc是路徑平滑度和碰撞安全性的代價函數(shù)；fv,fa是速度和加速度的代價函數(shù)，限制著無人機的速度和加速度，提高無人機的魯棒性和安全性；λ1,λ2,λ3為三者的權(quán)重。

平滑代價函數(shù)fs：在文中，平滑函數(shù)可以表述為使用i次導(dǎo)數(shù)代價來作為代價函數(shù)軌跡的積分項（加速度、加加速度等）[12]，其定義如下

碰撞代價函數(shù)fc：軌跡點在距離障礙物的τ范圍內(nèi)，函數(shù)表示如下

其中，每個點的c(x)代價函數(shù)定義如下

d(x)表示與障礙物最近的距離。速度和加速度的代價函數(shù)fv,fa，將不可行速度和加速度控制點進(jìn)行約束

其中，μ∈{x,y,z}，最大允許速度為vmax，且Fv(vμ)定義如下

2.3 B樣條軌跡的生成

本文軌跡優(yōu)化的方式采用B 樣條方法[11]，在生成控制點以及對控制點進(jìn)行優(yōu)化處理后，得到平滑的軌跡。K-1次B樣條的公式計算為

式中，S(t)表示優(yōu)化后的軌跡；Ni,k(t)是基函數(shù)表中第i個k次B 樣條的基函數(shù)；Qi為第i個控制點。基函數(shù)是使用Deboor算法生成的，表達(dá)式如下

式中，ui表示節(jié)點，Ni,k(u)表示基本函數(shù)表中的基函數(shù)。

3 試驗結(jié)果與分析

3.1 試驗設(shè)置與試驗平臺

使用DP1000四旋翼無人機多次進(jìn)行戶外測試來試驗。所提出的相關(guān)路徑規(guī)劃和軌跡優(yōu)化方法，均在TX2板載計算機、Ubuntu18.04 系統(tǒng)、ROS 平臺上使用C++代碼試驗。調(diào)用非線性優(yōu)化求解器NLopt 對控制點進(jìn)行優(yōu)化處理，在真實無人機上驗證算法之前，已在gazebo 進(jìn)行了若干次試驗，確保算法可行的情況下，才進(jìn)行真實無人機試驗。

在未知的環(huán)境中，使用DP1000 四旋翼無人機平臺，進(jìn)行目標(biāo)識別和無人機自主避障飛行，如圖3所示，PX4飛控板，配備英特爾實感深度攝像頭D455，生成密集點云圖，用于柵格障礙物地圖的繪制，本文使用擴展卡爾曼濾波（EKF）將GPS 和IMU 的數(shù)據(jù)進(jìn)行融合，從而獲得位姿信息。路徑生成、軌跡優(yōu)化、狀態(tài)估計和ESDF地圖構(gòu)建均在Nvidia TX2（配備8GB RAM、8GB SSD 和256GB 外部存儲器）板載計算機上進(jìn)行，通過ROS節(jié)點，將圖像信息以話題/camera/color/image_raw 的形式，傳輸?shù)降孛娑薒enovo Legion Y7000P 筆記本電腦進(jìn)行目標(biāo)識別、檢測和偵察。DP1000無人機如圖3所示。

圖3 無人機平臺Fig.3 UAV platform

3.2 試驗結(jié)果分析與比較

對于本文提出的改進(jìn)Hybrid A*算法結(jié)合后端優(yōu)化的方法，主要進(jìn)行兩個方面的測試：（1）不同算法路徑的比較，測試參數(shù)為：路徑的平滑程度和飛行過程中的最大加速度和平均加速度。（2）無人機在快速飛行過程中，對比不同算法的目標(biāo)識別的準(zhǔn)確率和檢測數(shù)目。隨機放置2.5m和3.0m的避障桿，讓無人機自主穿梭，并進(jìn)行目標(biāo)識別和偵察。在快速飛行時，四旋翼無人機能夠成功地生成平滑、魯棒、安全的路徑避開障礙物，且提高了目標(biāo)識別和檢測的準(zhǔn)確率。

3.3 無人機運動的路徑比較

將不同的算法生成的路徑與本文提出的優(yōu)化算法進(jìn)行對比。無人機運行最大速度設(shè)置為0.5m/s，最大加速度設(shè)置為0.5m/s2，飛行高度為2m。對比不同算法生成軌跡的弦長和弧長的距離，來判斷無人機飛行路徑的平滑度，見表1。將Fast planner[8]規(guī)劃的軌跡與本文算法規(guī)劃的軌跡在圖4中進(jìn)行對比，路徑的平滑度增加了10%。

表1 不同算法在無障礙物時生成的路徑平滑程度Table 1 Path smoothness degree generated by different algorithms without obstacles

圖4 軌跡對比Fig.4 Trajectory comparison

不同算法下，通過無人機快速飛行最大加速度和平均加速度來比較無人機飛行的平穩(wěn)性，試驗結(jié)果見表2。

表2 無人機在不同算法下飛行的魯棒性Table 2 Robust performance of UAV flying under different algorithms

真實障礙物環(huán)境飛行如圖5 所示，圖5（a）表示無人機飛行的真實環(huán)境，圖5（b）表示無人機局部的飛行軌跡，經(jīng)本文算法優(yōu)化后，繞障軌跡平滑，由圖5可知，經(jīng)優(yōu)化后，無人機避障飛行軌跡平滑。無人機在自主導(dǎo)航飛行中進(jìn)行目標(biāo)識別，如圖6所示。

圖5 真實環(huán)境中無人機自主導(dǎo)航Fig.5 Autonomous navigation of UAV in real environment

圖6 無人機目標(biāo)識別結(jié)果Fig.6 UAV target recognition results

3.4 目標(biāo)識別和檢測準(zhǔn)確率

無人機高機動飛行往往導(dǎo)致采集的圖像數(shù)據(jù)模糊，對后續(xù)的目標(biāo)識別造成影響[14]。將本文改進(jìn)的算法、A*算法和Fast planner 算法進(jìn)行對比。無人機沿不同算法生成的軌跡快速飛行，并對特定的物體進(jìn)行識別和檢測，現(xiàn)場布置了一個人和9輛車，對比在不同算法高機動飛行下，無人機檢測出的數(shù)目以及各個物體的準(zhǔn)確率，見表3。不同算法的對比，如圖7和圖8所示。

圖7 無人機在快速飛行時的目標(biāo)識別Fig.7 Target recognition of UAV in fast flight

圖8 經(jīng)本文優(yōu)化算法后的目標(biāo)檢測結(jié)果Fig.8 Target recognition diagram of UAV when the optimized algorithm flies fast

表3 不同路徑規(guī)劃算法對目標(biāo)識別的影響Table 3 Compares the effects of different path planning on fast flight on target recognition

經(jīng)過真實四旋翼無人機實際測試可知，改進(jìn)的路徑平滑優(yōu)化算法，提高了無人機飛行的平穩(wěn)性，四旋翼無人機在快速飛行中準(zhǔn)確率不高、目標(biāo)丟失等問題，得到了很好的改善。

4 結(jié)論

本文針對無人機飛行過程中的目標(biāo)識別YOLO v3 算法準(zhǔn)確率不高等問題，通過改進(jìn)無人機前端路徑搜索算法和后端非線性平滑優(yōu)化方法，計算得到一條平滑、魯棒與安全的飛行路徑，從而提高目標(biāo)識別YOLO v3算法在無人機上的準(zhǔn)確率。改進(jìn)Hybrid A*前端三維路徑搜索的啟發(fā)式函數(shù)和后端非線性優(yōu)化的平滑函數(shù)，解決了無人機在自主導(dǎo)航和自主避障飛行中快速旋轉(zhuǎn)或翻轉(zhuǎn)等問題。本文的算法具有以下幾個優(yōu)點：（1）無人機快速飛行時魯棒性和路徑的平滑性得到提高；（2）無人機飛行過程中傳統(tǒng)目標(biāo)識別算法的檢測精度更高。試驗結(jié)果表明，本文改進(jìn)Hybrid A*前端啟發(fā)式函數(shù)和后端平滑優(yōu)化函數(shù)，能夠提高無人機飛行的穩(wěn)定性和路徑的平滑性，對提高目標(biāo)識別的準(zhǔn)確率有很好的效果。