鄭 滋，楊圣慧，鄭永軍，劉星星，陳 建，蘇道畢力格

·農(nóng)業(yè)航空工程·

多旋翼無人機避障航跡規(guī)劃算法

鄭 滋，楊圣慧，鄭永軍※，劉星星，陳 建，蘇道畢力格

（中國農(nóng)業(yè)大學工學院，北京 100083）

多旋翼無人機的自主避障能力是安全作業(yè)的重要保證。該研究針對多旋翼無人機自主避障問題，提出了一種改進的雙向RRT快速隨機樹航跡規(guī)劃算法，結合最小化位移四階導數(shù)的動力學優(yōu)化方法，生成更符合多旋翼無人機動力學性能的避障航跡，解決避障過程中重復搜索、航跡曲率波動性大等問題，實現(xiàn)平穩(wěn)避障；提出了以隨機采樣算法規(guī)劃難度（用時）為核心的場景復雜度評價方法，在不同復雜度場景下進行了仿真試驗。結果表明：與改進前相比，避障航跡再規(guī)劃迭代次數(shù)減少23.69%；有效避障航跡規(guī)劃用時不高于0.33 s、平均避障速度不低于1.16 m/s、避障航跡延長率最多達20.82%。所提出的避障航跡規(guī)劃方法，提升了避障航跡的規(guī)劃效率與效果，可為多旋翼無人機自主作業(yè)過程中的避障航跡規(guī)劃提供參考。

無人機；優(yōu)化；航跡規(guī)劃；避障；路徑配置；RRT-Connect；動力學優(yōu)化

0 引 言

多旋翼無人機作業(yè)效率高、操作簡便、通過性強[1-4]，在植保作業(yè)、林業(yè)巡檢等領域得到了廣泛應用。但在實際作業(yè)過程中，無人機會面臨種類繁多、特征復雜的障礙，及時感知周圍環(huán)境，實現(xiàn)快速、平穩(wěn)、低消耗的避障[5-6]，可極大降低作業(yè)風險。

然而，現(xiàn)有多旋翼無人機采用的避障方法智能化程度不足，在發(fā)現(xiàn)障礙后需懸停等待操作人員介入，如植保領域的大疆MG-1P[7]、高科新農(nóng)M23-E[8]等，當無人機較遠時，操作人員不易把握行列特征[9]，不能保證采取有效的規(guī)避措施。因此，基于實時感知到的障礙信息自主規(guī)劃形成避障路徑，是無人機無須在人工干預下，特別是在操作人員無法觀測到環(huán)境狀況或自動作業(yè)過程中，完成后繼任務的一種有效手段。

目前，已有大量相關研究形成了基于環(huán)境感知數(shù)據(jù)的避障路徑規(guī)劃算法，例如A*[10-12]、粒子群優(yōu)化[13]、PRM[14]、人工勢場法[15]、幾何強化學習[16]、演化算法[17]等，但是上述算法未考慮動力學約束，規(guī)劃的路徑較為曲折。在農(nóng)機領域，更多集中在地面農(nóng)機的自主作業(yè)方面，包括基于激光雷達的溫室機器人環(huán)境建圖與導航[18]、油菜播種機的自主導航[19]、丘陵山區(qū)田間轉運設備的導航[20]、葡萄園植保裝備的導航[21]等，采用的算法也是PID、A*、支持向量機等，較少涉及多旋翼無人機的路徑規(guī)劃與避障算法研究。

RRT快速隨機樹算法[22]作為基于采樣的路徑規(guī)劃方法，其概率完備性確保了只要有解則必能求解，但由于較強的隨機性無法保證尋徑效率，因此最終尋徑結果欠優(yōu)。RRT算法的一些的變體，例如RRT*[23]、RRT-Connect[24]等，雖降低了解的隨機性，但也未考慮動力學約束問題。有學者將動力學特性與RRT算法進行融合，例如Webb等提出的運動動力學優(yōu)化RRT*[25]、Ghosh等提出的KB-RRT[26]。這些變體修改了RRT算法的延拓方式，確保了任意單步航跡的動力學優(yōu)化，但仍然不能獲得整條避障航跡上的最優(yōu)動力學優(yōu)化路徑。

綜上，本文提出針對RRT-Connect路徑規(guī)劃算法的多種優(yōu)化改進，生成更符合多旋翼無人機動力學約束的航跡，實現(xiàn)最小代價的避障飛行，以期為多旋翼無人機自主作業(yè)與避障提供方案參考。

1 多旋翼無人機避障系統(tǒng)

1.1 坐標系定義

多旋翼無人機及相關部件的位姿使用本地東北天（Local East-North-Universe, ENU）坐標系表示，機體右手坐標系以表示，傳感器右手坐標系以表示。其中，ENU坐標系的坐標軸為[x,y,z]。x軸指向地理正東方向，y軸指向地理正北方向，z軸指向重力反方向，與世界坐標系（WGS-84）的相對關系基于多旋翼無人機的GNSS定位數(shù)據(jù)確定；機體右手坐標系的坐標軸為[x,y,z]，x垂直機頭方向水平向右，y指向機頭方向，z垂直向上，原點位于無人機質(zhì)心；傳感器坐標系的坐標軸[x,y,z]，x指向傳感器右側，y指向傳感器正向，z垂直向上，原點位于傳感器接收器位置。圖1為各坐標系的示意圖。

注：Ω為東北天坐標系原點；OB為機體右手坐標系原點；OE為傳感器坐標系原點；g為重力加速度；[xΩ,yΩ,zΩ]為東北天坐標系；[xB,yB,zB]為機體右手坐標系；[xΕ,yE,zE]為傳感器坐標系；F1、F2、F3、F4分別為螺旋槳1、螺旋槳2、螺旋槳3、螺旋槳4產(chǎn)生的升力，N；n1、n2、n3、n4分別為螺旋槳1、螺旋槳2、螺旋槳3、螺旋槳4的轉速，r·min-1。

1.2 系統(tǒng)硬件

本研究基于5kg載荷的Carto F4構建多旋翼無人機系統(tǒng)。機架上安裝Rplidar S1激光雷達和PIXHAWK飛行控制器，分別用于檢測和控制多旋翼無人機的飛行；根據(jù)飛行控制器芯片處理能力加裝了NVIDIA TX2高性能計算模塊，用于點云數(shù)據(jù)與避障邏輯處理。選用的Rplidar S1為二維單線激光雷達，測量分辨率為3 cm（實測近距離可到1 cm），以一個固定的小仰角布置，掃描某二維平面，用于檢測30 m范圍內(nèi)的障礙物。該仰角大小與飛行時產(chǎn)生的前傾角相等，保證飛行時激光雷達檢測面水平。同時，在飛行過程中，激光雷達僅掃描無人機所在的飛行面，獲取的障礙信息即為飛行路徑上的障礙。表1為Rplidar S1激光雷達、PIXHAWK飛行控制器、NVIDIA TX2的主要參數(shù)。

表1 Rplidar S1激光雷達、PIXHAWK飛行控制器、NVIDIA TX2主要參數(shù)

圖2為系統(tǒng)的功能結構及各主要功能模塊間的數(shù)據(jù)流關系。NVIDIA TX2的軟件基于ROS創(chuàng)建，采用分布式軟件結構解除耦合關系；PIXHAWK的軟件由飛行固件PX4提供，通過Mavlink協(xié)議與PX4提供的API接口進行數(shù)據(jù)交換。

注：ψ′為多旋翼無人機偏航角速度，rad·s-1；r為多旋翼無人機位置向量，m；r′為多旋翼無人機速度向量，m。

1.3 多旋翼無人機控制方法

根據(jù)多旋翼無人機的微分平坦特性[27]，若控制位置及偏航角，即可保證唯一姿態(tài)解。限于機載電腦與飛信控制器間的通信速率，將控制量轉化為ENU坐標系下的速度指令并發(fā)送給飛行控制器，實現(xiàn)對航跡的跟蹤控制。此外，設計了純跟隨策略的PID位姿控制器，實現(xiàn)航跡的平穩(wěn)跟蹤，實時計算理想位姿。

位置誤差控制為基本限幅PID反饋控制算法，輸入量為理想位置r、理想偏航角，輸出速度控制向量r′、偏航角速度控制向量c′。使用比例因子K乘以實際位姿與理想位姿之間的差值表示修正力度；使用積分因子K乘以誤差和消除系統(tǒng)靜差；使用K乘以誤差變化量增加響應速度。PID的離散位置形式如公式（1）。

式中為當前離散時刻，無量綱；為位置誤差向量，rad；e為偏航角誤差，rad；r′為輸出速度控制向量，m/s；c′為偏航角速度控制向量，rad/s。

為使理想姿態(tài)平穩(wěn)變化，采用純跟隨策略設計理想位姿r、的更新方式：在所有航點中首先確定距離當前位置歐氏距離最近的航點，并由此航點向終點方向遍歷，選擇第一個滿足條件的航點()，()與當前位置()的歐氏距離大于給定閾值，然后將此航點作為接下來飛行的目標航點。航點更新策略用公式（2）表示：

2 航跡規(guī)劃

若僅依賴實時傳感器數(shù)據(jù)，無人機避障時會面臨適應場景較為單一、難以脫離局部最優(yōu)、避障航跡不光滑的問題。其中，局部最優(yōu)可導致無人機在特定位置震蕩，避障航跡不光滑可導致多旋翼無人機較大的飛行能量開銷，最終降低植保作業(yè)效率。因此，在建立詳細的環(huán)境感知數(shù)據(jù)后，需進行航跡規(guī)劃。

多旋翼無人機的航跡規(guī)劃一般分為2個部分：首先基于環(huán)境地圖規(guī)劃基本避障路徑，然后將飛行路徑與各種速度、加速度約束共同考慮優(yōu)化為航跡。

2.1 概率柵格地圖構建

概率柵格地圖通過二值貝葉斯概率對物體在空間的占據(jù)狀態(tài)進行估計，目前已廣泛應用于智能導航領域[28]。為簡化計算和規(guī)劃復雜度，首先將三維地圖降維成二維柵格地圖。通過八叉樹地圖軟件OctoMap[29]，對于如圖3a所展示的仿真場景進行重建，得到三維概率柵格地圖（圖3b），再以多旋翼無人機所在的飛行平面對地圖進行截取得到圖3c概率柵格地圖。

2.2 基于改進的RRT-Connect路徑規(guī)劃

基于構建的概率柵格地圖，需建立起點與終點間的聯(lián)系。為解決起點與終點之間互尋能力較弱的問題，本文采用雙向快速擴展隨機樹（RRT-Connect）算法，并對該算法優(yōu)化改進，縮短空間探索時間，增強算法對復雜環(huán)境的適應性。

2.2.1 質(zhì)心偏向采樣改進

RRT-Connect節(jié)點生長過程由2部分決定，一是起點樹與終點樹互尋的引導環(huán)節(jié)，二是隨機采樣。對于無人機而言，障礙分布是隨機的，因此隨機采樣目的性不強。故本研究采用質(zhì)心偏向采樣，使2棵樹的節(jié)點生長互相牽引、互相引導。質(zhì)心偏向采樣通過增量計算獲得樹的質(zhì)心位置（如公式（3））。

式中為樹索引，為1時代表起點樹，為2時代表終點樹；為質(zhì)心位置，m；為樹的節(jié)點；下標為水平坐標，m；下標為垂直坐標，m；為節(jié)點索引；為隨機樹的節(jié)點數(shù)量，m。

基于雙向RRT算法，根據(jù)當前的RRT樹，將起點或者終點作為采樣點。若該采樣點無法通過碰撞檢測及重復點檢測，則進行隨機采樣。若隨機采樣添加節(jié)點成功，則下一次采樣繼續(xù)使用目標引導采樣，否則繼續(xù)隨機采樣。

2.2.2 在線滾動優(yōu)化改進

由于障礙物柵格地圖是隨著無人機飛行過程逐漸建立的，當新障礙出現(xiàn)時，原有路徑和隨機樹可能被截斷。若重新規(guī)劃，則會拋棄對原空間的感知，重復不必要的探索，并可能得到與現(xiàn)有路徑差異極大的新路徑，導致控制器震蕩。因此，本文采用在線滾動優(yōu)化法，基于上一規(guī)劃得到的隨機樹，隨地圖更新調(diào)整隨機樹結構，繼續(xù)采樣生長具體步驟為：首先找出與新的規(guī)劃起點和終點距離最近的隨機樹，若與樹上最近點的距離低于閾值，則將規(guī)劃的起點設置為該最近點，并把該點變更為樹的根節(jié)點；若高于閾值，則嘗試與最近樹建立連接關系，若失敗，則以這一點重新建立樹。如此獲得2顆樹后，先剪去2顆樹上與障礙物發(fā)生干涉的所有“枝條”，然后對隨機樹進行延伸生長，完成在線滾動優(yōu)化。

2.3 規(guī)劃路徑的全航跡動力學優(yōu)化

建立飛行規(guī)劃路徑后，為減小路徑中的跳變點和異常干涉點，本文采用基于Minimum Snap的動力學優(yōu)化方法對全航跡進行精簡優(yōu)化，并設計高低膨脹概率柵格地圖算法，防止航跡與障礙物發(fā)生干涉。

全航跡動力學優(yōu)化共分為3步：獲取最簡避障路徑，建立代價方程，求解最小化代價函數(shù)的解獲取航跡參數(shù)。

在飛行過程中，無人機作業(yè)飛行的穩(wěn)定性與航跡曲率有關。由于載荷與加速度的約束值呈反比，因此可調(diào)整速度與加速度的約束值，形成最佳避障路徑。對于航拍、測繪、遙感用無人機而言，其載荷固定，因此航跡曲率變化極??；對于植保無人機而言，雖然藥液質(zhì)量會發(fā)生變化，但是由無人機微分平坦性可知[27]，各坐標系確定后，軌跡方程的解唯一，與藥液質(zhì)量的變化無關，不影響速度與加速度約束初值，故不影響無人機實現(xiàn)避障功能。

2.3.1 路徑簡化預處理

2.3.2 代價方程

路徑簡化處理后，需建立代價方程。多旋翼無人機的作業(yè)飛行航跡可以通過若干個多項式函數(shù)表示。航點集合{()(0000)(1111),,(x,y,z,ψ)}包含由空間坐標與偏航角從起始0到當前組成的航點。連接中兩兩相鄰航點的航線為。{,,,}的4個平坦輸出分量相互獨立，可獨立求解。設為平坦輸出分量的一般化形式，則有一般化航點集合P={|0,1,…,α}，連接任意相鄰一般化航跡的航點可以用階多項式方程表示為

由此，一般化航跡函數(shù)可表示為

以一般化航跡的階導數(shù)的平方和積分構建代價函數(shù)()：

令=(1,2,…,n)，代入公式（6）得到：

由于()對于任意的非負，則矩陣為半正定黑塞矩陣（當優(yōu)化深度為0時，為正定黑塞矩陣），通過進一步構建約束方程，得到如下形式的二次規(guī)劃問題：

式中為約束矩陣；為約束下限向量；為約束上限向量。

2.3.3 一般化航跡的約束方程

按類型，約束可分為規(guī)劃性約束、連續(xù)性約束和動力學約束[31]。規(guī)劃性約束用于保證一般化航跡經(jīng)過指定的一般化航點；連續(xù)性約束用于保證相鄰段一般化航跡的連續(xù)性；動力學約束用于保證一般化航跡符合一般化動力學要求。表2為一般化航跡所需約束類型及約束個數(shù)。

表2 一般化航跡的約束類型

1）規(guī)劃性約束

一般化航點位置約束用于保證航跡函數(shù)S()經(jīng)過一般化航點集合P，其形式為

式中s為各航跡曲線的參數(shù)向量；i(=0,1,…,)為歸一化的時間量；P(=0,1,…,)為航跡點位置，m。

2）連續(xù)性約束

由于兩航線之間采用不同的多項式函數(shù)表示，需保證兩函數(shù)交點的連續(xù)性，至少要求相鄰航線的零階、一階、二階導數(shù)在交點處相等。

相鄰航跡交點一般化位置連續(xù)性約束表達形式為

相鄰航跡接點一般化速度連續(xù)性約束表達形式為

相鄰航跡接點一般化加速度連續(xù)性約束表達式為

3）動力學約束

動力學約束需對一般化速度S()′、一般化加速度S()(2)的極值做出限制。

最大速度約束數(shù)學表達形式為

式中u為最大一般化速度約束上限，m/s；l為最大一般化速度約束下限，m/s。

最大加速度約束數(shù)學表達形式為

式中u為最大一般化加速度約束上限，m/s2；l為最大一般化加速度約束下限，m/s2。

2.3.4 基于高低膨脹的概率柵格地圖處理

由于一般化位置約束僅在約束處生效，在特定的動力學約束條件下，偏離量可能無法接受，導致全航跡動力學優(yōu)化后的航跡侵入障礙區(qū)域（如圖4a所示）。因此，采用如圖4b所示的高低膨脹柵格概率地圖方法解決該問題。

首先通過高膨脹量獲取障礙物柵格地圖，使用前述系列方法獲得優(yōu)化后航跡；然后使用低膨脹量的障礙物柵格地圖進行干涉檢測：若檢測通過，則按該航跡飛行；若多次嘗試失敗，則將該航跡降級為直線連接航跡，在下一次障礙地圖更新時再嘗試擬合為多項式航跡。

圖4 概率柵格地圖膨脹前后無人機規(guī)劃路徑對比

3 算法驗證

為更準確地量化分析規(guī)劃場景的復雜程度，本文采用以隨機采樣算法為核心的場景復雜度評價方法，將隨機采樣算法應用于不同規(guī)劃場景，對比分析規(guī)劃用時，評估場景復雜度；以隨機采樣規(guī)劃為對照，對比不同算法在同一場景條件的規(guī)劃用時，評估算法的規(guī)劃能力。

3.1 改進RRT-Connect算法試驗

為驗證改進RRT-Connect算法的有效性，在Matlab環(huán)境下實現(xiàn)改進的雙向RRT算法。構建隨機設計的6種不同復雜度場景（圖5），分別執(zhí)行1 000次尋徑測試。由于算法的規(guī)劃維度為二維，因此解的空間維度設為2。雙向RRT規(guī)劃結果分別與RRT算法、RRT*算法、RRT-Connect的條件隨機采樣算法和啟發(fā)式采樣改進型算法的規(guī)劃結果分別進行對比。條件隨機采樣與質(zhì)心偏向采樣類似，為相互采樣中點的改進。各RRT算法計算步長設為0.4 m、閾值設為0.4 m，最大失敗迭代次數(shù)=10 000，終點閾值t=0.8 m，地圖高膨脹量=2 m，低膨脹量=1 m，啟發(fā)改進的RRT算法啟發(fā)參數(shù)=0.5。

圖5 障礙物地圖

3.1.1 改進RRT-Connect算法優(yōu)化結果與分析

1）質(zhì)心偏向采樣優(yōu)化效果分析

圖6為改進RRT-Connect算法計算成功率與迭代次數(shù)關系圖。其中，成功率是指在一定迭代次數(shù)下，成功完成路徑規(guī)劃的次數(shù)與總路徑規(guī)劃次數(shù)的比值。圖6可知：相比于隨機采樣，基于質(zhì)心偏向采樣的RRT-Connect在迭代次數(shù)較低時成功率更高，如單障礙物、三障礙物、多障礙物、隨機障礙物場景，但在某些場景中的成功率會低于其他RRT算法。與其他RRT算法相比，應用質(zhì)心偏向采樣的改進RRT-Connect算法保持了能快速在復雜場景規(guī)劃的優(yōu)勢，僅需約6 000步即可到100%成功率；而在簡單場景（如圖6a）中，隨機采樣更迅速。當成功率為100%時，質(zhì)心偏向采樣和隨機采樣的總迭代次數(shù)相近，而條件隨機采樣相對于RRT-Connect算法效果較差。因此，質(zhì)心偏向采樣可在多數(shù)場景中更快地引導起點樹與終點樹的互尋，但在被遮擋的情況下算法成功率沒有提升。

圖7為依據(jù)迭代次數(shù)和路徑代價測試數(shù)據(jù)繪制的數(shù)據(jù)點云圖。其中，路徑代價在本研究中為路徑長度。由數(shù)據(jù)點云分布可知，包括RRT*在內(nèi)的所有算法迭代次數(shù)和路徑代價不具有回歸關系，但數(shù)據(jù)點的整體分布呈現(xiàn)出明顯的差異。由圖7中黑色的誤差區(qū)間長短可知，質(zhì)心偏向采樣改進的RRT-Connect所需要的平均迭代次數(shù)在雙障礙物、三障礙物、四障礙物、多障礙物與隨機障礙物場景中最少，路徑代價更低。

注：α為啟發(fā)改進RRT算法的啟發(fā)參數(shù)

圖7 不同障礙物地圖的規(guī)劃路徑代價-迭代次數(shù)數(shù)據(jù)點云圖

2）在線滾動優(yōu)化結果分析

首先采用RRT算法對避障航跡進行規(guī)劃，然后更新測試場景，對比在新場景下的在線滾動優(yōu)化和完全重新規(guī)劃的規(guī)劃時間、規(guī)劃迭代次數(shù)。每個測試場景均包含一個即將出現(xiàn)的障礙物。每組場景進行1 000次測試。

圖8為不同場景下的迭代次數(shù)與迭代時間的數(shù)據(jù)點分布圖。不同的測試場景中，在線滾動優(yōu)化改進后的RRT-Connect算法可以顯著降低迭代次數(shù)和規(guī)劃時間。由于快速隨機樹算法是基于概率的，因此會出現(xiàn)少許與改進后RRT-Connect算法規(guī)劃時間相近的情況。在這些情況中，改進后RRT-Connect算法并沒有顯著的優(yōu)化效果；由誤差棒可知，優(yōu)化并不能顯著降低性能指標的主要分布范圍。迭代次數(shù)平均降低了35.98%，單障礙物場景迭代次數(shù)降低了39.58%，雙障礙物場景優(yōu)化效果最低，迭代次數(shù)降低了23.69%，場景3迭代次數(shù)優(yōu)化效果最好，降低了44.67%。

圖8 在線滾動優(yōu)化改進前后迭代次數(shù)和規(guī)劃時間散點圖

3.1.2 全航跡動力學優(yōu)化結果分析

采用OSQP二次規(guī)劃問題求解器作為QP（二次規(guī)劃）問題的求解器，對多個RRT避障路徑結果截彎取直得到避障路徑關鍵節(jié)點，然后進行全航跡動力學優(yōu)化，表3為相關動力學優(yōu)化參數(shù)設置。

表3 全航跡動力學優(yōu)化參數(shù)

注：狀態(tài)向量組成為p,p,v,v,a,a。其中,,分別代表位置（m）、速度（m·s-1）、加速度（m·s--2）。下標、代表在坐標系下的方向。下同。

Note: The state vectorconsists ofp,p,v,v,aanda, where,andrepresent position(m), velocity(m·s-1) and acceleration(m·s--2), respectively. The subscriptsandare the orientation in thecoordinate. The same below.

圖9為優(yōu)化得到的航跡，可知航向角在0°至45°至?30°變化過程中連續(xù)，無跳躍間斷點或振蕩間斷點，證明避障航跡平滑。圖10分別是圖9中速度、加速度、偏航角速度′和偏航角加速度″變化曲線。4個變量均為矢量值，在圖中以正負區(qū)分方向。由圖9可知，速度、加速度的正方向為向右（方向）和向前（方向）；角速度、角加速度正方向為無人機向左飛行時產(chǎn)生的角速度方向。

由圖10可知，各個參數(shù)平滑性較好，僅在23～30 s，x方向的速度快速變化3 m/s、加速度快速變化約0.4 m/s2，這是由于多旋翼無人機此時處在避障航跡的拐點上，飛行方向發(fā)生了較大的變化。、、′、″對應的最大值符合表3中給出的參數(shù)絕對值約束。上述結果顯示，全航跡動力學優(yōu)化算法可行，可以生成符合動力學要求的航跡。

圖9 避障規(guī)劃路徑的動力學優(yōu)化結果

圖10 避障規(guī)劃路徑的速度v、加速度a、角速度ψ′與角加速度ψ″變化

3.2 不同場景的仿真試驗與結果分析

3.2.1 仿真試驗設計

為證明算法在不同復雜度場景的通用性，基于Gazebo和PX4的SITL設計了多旋翼無人機避障仿真系統(tǒng)，Gazebo用于仿真避障場景，提供仿真激光雷達數(shù)據(jù)；PX4的SITL用于模擬多旋翼無人機飛行控制器，與本文的多旋翼無人機避障控制軟件交互，與Gazebo進行仿真姿態(tài)與物理反饋信息的通信；Mavros用于將Mavlink協(xié)議轉換為ROS（Robot Operating System）話題、服務，承擔通信中間件。各功能組件間數(shù)據(jù)流關系如圖11所示。

圖11 仿真系統(tǒng)結構與數(shù)據(jù)流關系圖

在Gazebo中編輯URDF（United Robot Description File，統(tǒng)一機器人描述文件）腳本，搭建不同復雜度的障礙場景，包括單障礙物場景（圖12a）、多障礙物場景（圖12b）和迷宮障礙物場景（圖12c），檢驗方法的適應性和通用性。圖12d為仿真試驗場景。

采用松樹模型組成飛行地圖，網(wǎng)格棋盤用于標記起點和終點位置。其中，終點設置在一個完全被遮擋住且靠近障礙的區(qū)域。

試驗設定起點與終點間直線距離45 m；自然風干擾設置為0；保留GPS定位干擾和隨機游走干擾，重力加速度設置為9.8 m/s2；根據(jù)Rplidar S1激光雷達參數(shù)，設置激光雷達水平掃描范圍?0.873～1.047 rad（為減少數(shù)據(jù)量做了掃描范圍的縮減），水平掃描點云數(shù)量200，傳感器刷新率25 Hz，最小測量距離0.3 m，最大測量距離40 m，傳感器測量噪聲為0.04 m。

圖12 仿真場景和仿真多旋翼無人機

表4為仿真試驗參數(shù)，啟動避障控制系統(tǒng)軟件，控制多旋翼無人機自動起飛開始避障任務，到達目的地后自動降落后關閉程序，完成測試過程。

表4 避障控制關鍵參數(shù)

注：各參數(shù)通過調(diào)試階段的整定得出，綜合了多旋翼無人機避障域的安全尺寸要求、Minimum Snap動力學優(yōu)化的高低膨脹法要求和航線規(guī)劃成功率的要求。

Note: The parameters were obtained by adjusting the parameters during debugging, which integrated the requirements of safety sizes of obstacle avoidance area of multi-rotor UAVs, high-and-low expansion of minimum snap dynamics optimization, and the success rate of trajectory planning.

3.2.2 仿真試驗結果分析

由圖13可知，相對于單障礙物場景，多障礙物場景與迷宮障礙物場景的第2次RRT規(guī)劃用時明顯上升，說明第2次迭代時場景細節(jié)重建更加完整，因此場景更復雜；隨后，由于在線滾動優(yōu)化，可以利用上一次迭代修剪過的RRT樹進行規(guī)劃，而且更加接近終點，地圖復雜度下降，規(guī)劃用時隨之開始下降。采用動力學優(yōu)化算法用時呈現(xiàn)略微下降趨勢。整體上，該算法對多場景具有良好的適應性和通用性，任意場景的總規(guī)劃用時不高于0.33 s。

圖13 不同復雜度場景的規(guī)劃用時

從試驗結果（圖14和圖15）看，在所設計的各個場景中，多旋翼無人機均能成功完成避障到達終點。在單障礙物場景中，僅需一次規(guī)劃即可達到最終目標，總飛行時間39.10 s，飛行距離51.24 m，實際航跡長度較起止最短距離延長2.48%，平均飛行速度1.31 m/s；在多障礙物場景中，經(jīng)歷4次規(guī)劃達到最終目標，總飛行時間43.55 s，飛行距離50.61 m，實際航跡長度較起止點最短距離延長1.22%，平均飛行速度1.16 m/s；對于迷宮場景，共計滾動優(yōu)化航跡2次。雖然2次規(guī)劃所依賴的障礙地圖差異較大，但避障航跡的規(guī)劃結果仍保持穩(wěn)定。降維得到的二維障礙物柵格概率地圖也保留了障礙核心信息，總飛行時間43.77 s，飛行距離54.37 m，實際航跡長度較起止點最短距離延長20.82%，平均飛行速度1.24 m/s。

由圖16可知，速度曲線沒有明顯拐點，加速度曲線基本平滑，證明無人機飛行過程中航跡沒有明顯的突變或異常，本文算法可以規(guī)劃生成符合無人機動力學的平滑航跡。

圖14 三維重建及避障航跡圖

圖15 避障航跡滾動優(yōu)化過程仿真結果

圖16 不同場景下速度與加速度的變化

4 結 論

本文基于環(huán)境感知信息重建障礙柵格概率地圖，提出了基于質(zhì)心偏向采樣的雙向RRT航線在線滾動優(yōu)化方法；利用Minimum Snap法和高低膨脹柵格概率地圖法，對規(guī)劃的航跡進行了全航跡動力學優(yōu)化，結論如下：

1）提出的以隨機采樣算法規(guī)劃難度（用時）為核心的場景復雜度評價方法，以隨機采樣算法在各場景下完成路徑規(guī)劃的用時為依據(jù)，量化了不同場景的規(guī)劃難度差異，更準確、更泛化地評估場景的復雜程度，并在同一場景條件下對比分析了不同算法間的差異；

2）經(jīng)不同場景仿真測試可知，改進的雙向RRT算法較改進前避障航跡再規(guī)劃迭代次數(shù)減少23.69%，有效避障航跡規(guī)劃用時不高于0.33 s、平均避障航跡速度不低于1.16 m/s、避障航跡延長率不超過20.82%。所提出的避障航跡規(guī)劃方法能夠滿足不同復雜度場景避障路徑的實時規(guī)劃要求，可以有效提升路徑規(guī)劃效率和平穩(wěn)性，為無人機在農(nóng)田果園植保、林間巡檢等實際作業(yè)環(huán)境的避障提供了一種可行的技術方案。

[1]鄭永軍，陳炳太，呂昊暾，等. 中國果園植保機械化技術與裝備研究進展[J]. 農(nóng)業(yè)工程學報，2020，36(20)：110-124.

Zheng Yongjun, Chen Bingtai, Lyu Haotun, et al. Research progress of orchard plant protection mechanization technologyand equipment in China[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2020, 36(20): 110-124. (in Chinese with English abstract)

[2]鄭永軍，江世界，陳炳太，等. 丘陵山區(qū)果園機械化技術與裝備研究進展[J]. 農(nóng)業(yè)機械學報，2020，51(11)：1-20.

Zheng Yongjun, Jiang Shijie, Chen Bingtai, et al. Review on technology and equipment of mechanization in hilly orchard[J]. Transactions of the Chinese Society for Agricultural Machinery, 2020, 51(11): 1-20. (in Chinese with English abstract)

[3]蘭玉彬，王林琳，張亞莉. 農(nóng)用無人機避障技術的應用現(xiàn)狀及展望[J]. 農(nóng)業(yè)工程學報，2018，34(9)：104-113.

Lan Yubin, Wang Linlin, Zhang Yali. Application and prospect on obstacle avoidance technology for agricultural UAV[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(9): 104-113. (in Chinese with English abstract)

[4]周志艷，臧英，羅錫文，等. 中國農(nóng)業(yè)航空植保產(chǎn)業(yè)技術創(chuàng)新發(fā)展戰(zhàn)略[J]. 農(nóng)業(yè)工程學報，2013，29(24)：1-10.

Zhou Zhiyan, Zang Ying, Luo Xiwen, et al. Technology innovation development strategy on agricultural aviation industryfor plant protection in China[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2013, 29(24): 1-10. (in Chinese with English abstract)

[5]Tripicchio P, Satler M, Dabisias G, et al. Towards smart farming and sustainable agriculture with drones[C]//2015 International Conference on Intelligent Environments, Prague: 2015.

[6]Hese S, Behrendt F. Multiseasonal tree crown structure mapping with point clouds from OTS quadrocopter systems[J]. The International Archives of Photogrammetry, Remote Sensing and Spatial Information Sciences, 2017, XLII-2/W6: 141-143.

[7]大疆創(chuàng)新. MG-1P農(nóng)業(yè)無人機用戶手冊[M/OL]. [2018-11-08]. https://www.dji.com/cn/downloads/products/mg-1p.

[8]高科新農(nóng). 全自主飛行20L單旋翼電動無人機[ER/OL]. https://www.81uav.cn/product/5568.html.

[9]彭孝東，張鐵民，李繼宇，等. 基于目視遙控的無人機直線飛行與航線作業(yè)試驗[J]. 農(nóng)業(yè)機械學報，2014，45(11)：258-263.

Peng Xiaodong, Zhang Tiemin, Li Jiyu, et al. Experiment of straight and airline flight operation for farmlandbased on UAV in visual remote mode[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(11): 258-263. (in Chinese with English abstract)

[10]Liu Chenguang, Mao Qingzhou, Chu Xiumin, et al. An improved A-Star algorithm considering water current, traffic separation and berthing for vessel path planning[J]. Apply Science[J]. 2019, 9(6): 1057.

[11]Michael G H Bell. Hyperstar: A multi-path A-star algorithm for risk averse vehicle navigation[J]. Transportation Research Part B: Methodological, 2009, 43(1): 97-107.

[12]Su Heng, Yun Hongquan, He Jingwen, et al. Multi-aircraft path planning method based on cooperative search A-star algorithm[C]//2019 IEEE International Conference on Unmanned Systems (ICUS), Beijing: 2019.

[13]Yong Ma, Hu Mengqi, Yan Xinping. Multi-objective path planning for unmanned surface vehicle with currents effects[J]. ISA Transactions, 2018, 75: 137-156.

[14]Sanchez-Ante G, Latombe J. A Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking[M]//Springer Tracts in Advanced Robotics. Berlin: Springer, 2002:403-417.

[15]張遜遜，許宏科，朱旭. 低空低速植保無人直升機避障控制系統(tǒng)設計[J]. 農(nóng)業(yè)工程學報，2016，32(2)：43-50.

Zhang Xunxun, Xu Hongke, Zhu Xu. Design of obstacle avoidance control system for low altitude and low speed eppo unmanned helicopter[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(2): 43-50. (in Chinese with English abstract)

[16]Zhang B, Mao Z, Liu W, et al. Geometric reinforcement learning for path planning of UAVs[J]. Journal of Intelligent & Robotic Systems, 2015, 77(2): 391-409.

[17]Niu Xi, Yuan Xiaodong, Zhou Yongwei, et al. UAV track planning based on evolution algorithm in embedded system[J]. Microprocessors and Microsystems, 2020, 75: 103068.

[18]侯加林，蒲文洋，李天華，等. 雙激光雷達溫室運輸機器人導航系統(tǒng)研制[J]. 農(nóng)業(yè)工程學報，2020，36(14)：80-88.

Hou Jialin, Pu Wenyang, Li Tianhua, et al. Development of dual-lidar navigation system for greenhouse transportation robot[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2020, 36(14): 80-88. (in Chinese with English abstract)

[19]丁幼春，何志博，夏中州，等. 小型履帶式油菜播種機導航免疫PID控制器設計[J]. 農(nóng)業(yè)工程學報，2019，35(7)：12-20.

Ding Youchun, He Zhibo, Xia Zhongzhou, et al. Design of navigation immune controller of small crawler-type rape seeder[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(7): 12-20. (in Chinese with English abstract)

[20]李云伍，徐俊杰，王銘楓，等. 丘陵山區(qū)田間道路自主行駛轉運車及其視覺導航系統(tǒng)研制[J]. 農(nóng)業(yè)工程學報，2019，35(1)：52-61.

Li Yunwu, Xu Junjie, Wang Mingfeng, et al. Development of autonomous driving transfer trolley on field roads and its visual navigation system for hilly areas[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(1): 52-61. (in Chinese with English abstract)

[21]艾長勝，林洪川，武德林，等. 葡萄園植保機器人路徑規(guī)劃算法[J]. 農(nóng)業(yè)工程學報，2018，34(13)：77-85.

Ai Changsheng, Lin Hongchuan, Wu Delin, et al. Path planning algorithm for plant protection robots in vineyard[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2018, 34(13): 77-85. (in Chinese with English abstract)

[22]Steven M LaValle. Rapidly-Exploring Random Trees: A new tool for path planning[R]. Iowa: Computer Science Dept, 1999.

[23]Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846-894.

[24]Kuffner J J, LaValle S M. RRT-connect: An efficient approach to single-query path planning[C]//Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation, San Francisco: IEEE, 2000.

[25]Webb D J, Van Den B J. Kinodynamic RRT*: Asymptotically optimal motion planning for robots with linear dynamics[C]//2013 IEEE International Conference on Robotics and Automation, 2013.

[26]Ghosh Dibyendu, Nandakumar Ganeshram, Narayanan Karthik, et al. Kinematic constraints based Bi-directional RRT (KB-RRT) with parameterized trajectories for robot path planning in cluttered environment[C]//2019 International Conference on Robotics and Automation (ICRA). Montreal, QC, Canada, 2019.

[27]黃敏. 基于微分平坦理論的四旋翼無人機軌跡規(guī)劃的研究[D]. 沈陽：東北大學，2013.

Huang Min. Trajectory Planning Research of Quadrotor UAV Based on Differential Flatness Theory[D]. Shenyang: Northeastern University, 2013. (in Chinese with English abstract)

[28]Thrun S, Burgard W, Fox D. Probabilistic Robotics[M]. Cambridge: The MIT Press, 2005.

[29]Armin Hornung, Kai M. Wurm, Maren Bennewitz, et al. OctoMap: An efficient probabilistic 3D mapping framework based on octrees[J]. Autonomous Robots, 2013, 34(3): 189-206.

[30]Praveen V, Pillai A. Modeling and simulation of quadcopter using PID controller[J]. International Journal of Computer Technology and Applications, 2016, 9(15): 7151-7158.

[31]Kevin M Lynch, Frank C Park. 現(xiàn)代機器人學：機構、規(guī)劃與控制[M]. 北京：機械工業(yè)出版社，2020.

Obstacle avoidance path planning algorithm for multi-rotor UAVs

Zheng Zi, Yang Shenghui, Zheng Yongjun※, Liu Xingxing, Chen Jian, Su Daobilige

(100083)

Unmanned Aerial Vehicles (UAVs) have been commonly used for the plant protection in modern agriculture. Autonomous operation is a heated issue of UAVs development, while obstacle avoidance is one of essential abilities. If obstacles are not effectively avoided during automatic operation, the security of UAVs will be inevitably at risk. This study proposed a novel method of collision-free trajectory planning for multi-rotor UAVs, which used modified dynamic optimisation, thereby to deal with autonomous obstacle detection and avoidance. A quad-rotor UAV, Carto F4, equipped with the workload of 5 kg, was selected as the flight platform. A LIDAR, Rplidar S1, and a PIXHAWK flight controller were used on the UAV. Meanwhile, a high-speed computing module, NVIDIA TX2, was used for complex computation. The specific method of trajectory planning consisted of three procedures during optimisation. First, a probability grid map was established to serve as the environment map, using the binary Bayesian probability. Then, an optimal bi-directional Rapidly-exploring Random Trees (RRT) was developed to search a complete and low-cost path for the UAV. Specifically, a systematic optimisation included the application of both centroid bias sampling and online-rolling optimization. The centroid bias sampling was used for the mutual guidance in node growing, while the online-rolling optimization was used for the avoidance of repeated growing of nodes. A more efficient path was established according to the two steps. Third, a dynamic optimisation of full trajectories was applied, where the dynamic optimization of minimizing the fourth derivative of displacement was utilised to make the path to be a trajectory that was more in line with dynamic performance, thereby to achieve stable avoidance of obstacles. A minimum snap was employed during optimisation, where three types of constraints were added, containing planning constraints, continuity constraints, and dynamic constraints. Meanwhile, a probability grid map with high and low expansions was developed to ensure that the full trajectory did not interfere with obstacle areas. In-depth simulation test results illustrated that the re-planning duration of obstacle avoidance could be reduced by up to 23.69%, compared with the non-improvements, indicating that the dynamic optimisation made the trajectory more feasible and smoother. Moreover, the duration of planning of effective trajectories for obstacle avoidance was less than 0.33s, and the average speed of trajectory tracking of obstacle avoidance was not lower than 1.16 m/s. In addition, the extension rate of trajectories for obstacle avoidance was up to 20.82%, indicating that the efficiency and effectiveness of trajectory planning were improved. The proposed method of obstacle-free trajectory planning for multi-rotor UAVs can provide a sound of theoretical scheme and technical reference for the autonomous operation and obstacle avoidance of multi-rotor UAVs.

UAV; optimisation; path planning; obstacle avoidance; path configuration; RRT-Connect; dynamics optimisation

鄭滋，楊圣慧，鄭永軍，等. 多旋翼無人機避障航跡規(guī)劃算法[J]. 農(nóng)業(yè)工程學報，2020，36(23)：59-69.doi：10.11975/j.issn.1002-6819.2020.23.007 http://www.tcsae.org

Zheng Zi, Yang Shenghui, Zheng Yongjun, et al. Obstacle avoidance path planning algorithm for multi-rotor UAVs[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2020, 36(23): 59-69. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2020.23.007 http://www.tcsae.org

2020-07-21

2020-11-18

國家重點研發(fā)計劃（2016YFD0200702，2018YFD0700603）

鄭滋，主要從事計算機測控技術、智能檢測與避障技術研究。Email：493956170@qq.com

鄭永軍，博士，教授，主要從事智能農(nóng)業(yè)裝備技術與研發(fā)。Email：zyj@cau.edu.cn

10.11975/j.issn.1002-6819.2020.23.007

S251; TP273

A

1002-6819(2020)-23-0059-11