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

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

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

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

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

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

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

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

Hybrid A*算法來自自動駕駛汽車的應(yīng)用，在柵格地圖中搜尋到一個符合汽車運(yùn)動的、安全的二維路徑，搜索過程與傳統(tǒng)的A*算法相似，創(chuàng)建OPEN 列表和CLOSE 列表，在OPEN 列表中找到規(guī)劃代價最小的父節(jié)點，擴(kuò)展父節(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)用于三維的四旋翼無人機(jī)路徑規(guī)劃搜索，結(jié)合四旋翼無人機(jī)動力學(xué)和運(yùn)動學(xué)，在3D的柵格地圖中生成前端路徑。

1.1 可行運(yùn)動點的生成

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

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

無人機(jī)的狀態(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 離散控制輸入得到下一刻無人機(jī)的可行運(yùn)動點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*算法中，直接使用歐式距離，且只考慮柵格地圖中障礙物的信息，而忽略了無人機(jī)的運(yùn)動學(xué)和動力學(xué)特性，本文所采用的方法是通過計算最優(yōu)邊值問題，來計算啟發(fā)式的開銷。啟發(fā)式開銷模型可表示為

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

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

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

三階多項式軌跡方程為

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

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

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

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

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

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

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

在柵格障礙物地圖中，前端生成的改進(jìn)Hybrid A*路徑往往會很靠近障礙物，這樣對無人機(jī)的運(yùn)動飛行是不合理的，因此需要對前端生成的路徑進(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ù)，限制著無人機(jī)的速度和加速度，提高無人機(jī)的魯棒性和安全性；λ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ī)多次進(jìn)行戶外測試來試驗。所提出的相關(guān)路徑規(guī)劃和軌跡優(yōu)化方法，均在TX2板載計算機(jī)、Ubuntu18.04 系統(tǒng)、ROS 平臺上使用C++代碼試驗。調(diào)用非線性優(yōu)化求解器NLopt 對控制點進(jìn)行優(yōu)化處理，在真實無人機(jī)上驗證算法之前，已在gazebo 進(jìn)行了若干次試驗，確保算法可行的情況下，才進(jìn)行真實無人機(jī)試驗。

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

圖3 無人機(jī)平臺Fig.3 UAV platform

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

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

3.3 無人機(jī)運(yùn)動的路徑比較

將不同的算法生成的路徑與本文提出的優(yōu)化算法進(jìn)行對比。無人機(jī)運(yùn)行最大速度設(shè)置為0.5m/s，最大加速度設(shè)置為0.5m/s2，飛行高度為2m。對比不同算法生成軌跡的弦長和弧長的距離，來判斷無人機(jī)飛行路徑的平滑度，見表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

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

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

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

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

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

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

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

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

4 結(jié)論

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