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

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

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

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

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

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

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

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

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

1.1 可行運(yùn)動(dòng)點(diǎn)的生成

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

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

無(wú)人機(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 離散控制輸入得到下一刻無(wú)人機(jī)的可行運(yùn)動(dòng)點(diǎ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)邊值問(wèn)題計(jì)算啟發(fā)式開銷

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

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

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

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

三階多項(xiàng)式軌跡方程為

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

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

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

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

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

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

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

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

2.1 控制點(diǎn)的求解

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

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

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

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

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

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

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

其中，每個(gè)點(diǎn)的c(x)代價(jià)函數(shù)定義如下

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

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

2.3 B樣條軌跡的生成

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

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

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

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

3.1 試驗(yàn)設(shè)置與試驗(yàn)平臺(tái)

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

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

圖3 無(wú)人機(jī)平臺(tái)Fig.3 UAV platform

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

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

3.3 無(wú)人機(jī)運(yùn)動(dòng)的路徑比較

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

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

圖4 軌跡對(duì)比Fig.4 Trajectory comparison

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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