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

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

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

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

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

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

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

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

0 引 言

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

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

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

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

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

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

1.1 坐標(biāo)系定義

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

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

1.2 系統(tǒng)硬件

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

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

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

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

1.3 多旋翼無人機(jī)控制方法

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

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

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

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

2 航跡規(guī)劃

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

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

2.1 概率柵格地圖構(gòu)建

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

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

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

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

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

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

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

2.2.2 在線滾動(dòng)優(yōu)化改進(jìn)

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

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

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

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

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

2.3.1 路徑簡化預(yù)處理

2.3.2 代價(jià)方程

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

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

以一般化航跡的階導(dǎo)數(shù)的平方和積分構(gòu)建代價(jià)函數(shù)()：

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

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

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

2.3.3 一般化航跡的約束方程

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

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

1）規(guī)劃性約束

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

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

2）連續(xù)性約束

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

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

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

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

3）動(dòng)力學(xué)約束

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

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

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

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

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

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

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

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

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

3 算法驗(yàn)證

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

3.1 改進(jìn)RRT-Connect算法試驗(yàn)

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

圖5 障礙物地圖

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

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

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

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

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

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

2）在線滾動(dòng)優(yōu)化結(jié)果分析

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

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

圖8 在線滾動(dòng)優(yōu)化改進(jìn)前后迭代次數(shù)和規(guī)劃時(shí)間散點(diǎn)圖

3.1.2 全航跡動(dòng)力學(xué)優(yōu)化結(jié)果分析

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

表3 全航跡動(dòng)力學(xué)優(yōu)化參數(shù)

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

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ù)，無跳躍間斷點(diǎn)或振蕩間斷點(diǎn)，證明避障航跡平滑。圖10分別是圖9中速度、加速度、偏航角速度′和偏航角加速度″變化曲線。4個(gè)變量均為矢量值，在圖中以正負(fù)區(qū)分方向。由圖9可知，速度、加速度的正方向?yàn)橄蛴遥ǚ较颍┖拖蚯埃ǚ较颍?；角速度、角加速度正方向?yàn)闊o人機(jī)向左飛行時(shí)產(chǎn)生的角速度方向。

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

圖9 避障規(guī)劃路徑的動(dòng)力學(xué)優(yōu)化結(jié)果

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

3.2 不同場景的仿真試驗(yàn)與結(jié)果分析

3.2.1 仿真試驗(yàn)設(shè)計(jì)

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

圖11 仿真系統(tǒng)結(jié)構(gòu)與數(shù)據(jù)流關(guān)系圖

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

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

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

圖12 仿真場景和仿真多旋翼無人機(jī)

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

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

注：各參數(shù)通過調(diào)試階段的整定得出，綜合了多旋翼無人機(jī)避障域的安全尺寸要求、Minimum Snap動(dòng)力學(xué)優(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 仿真試驗(yàn)結(jié)果分析

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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

[1]鄭永軍，陳炳太，呂昊暾，等. 中國果園植保機(jī)械化技術(shù)與裝備研究進(jìn)展[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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ī)械化技術(shù)與裝備研究進(jìn)展[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，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)用無人機(jī)避障技術(shù)的應(yīng)用現(xiàn)狀及展望[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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è)技術(shù)創(chuàng)新發(fā)展戰(zhàn)略[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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è)無人機(jī)用戶手冊[M/OL]. [2018-11-08]. https://www.dji.com/cn/downloads/products/mg-1p.

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

[9]彭孝東，張鐵民，李繼宇，等. 基于目視遙控的無人機(jī)直線飛行與航線作業(yè)試驗(yàn)[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，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]張遜遜，許宏科，朱旭. 低空低速植保無人直升機(jī)避障控制系統(tǒng)設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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]侯加林，蒲文洋，李天華，等. 雙激光雷達(dá)溫室運(yùn)輸機(jī)器人導(dǎo)航系統(tǒng)研制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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]丁幼春，何志博，夏中州，等. 小型履帶式油菜播種機(jī)導(dǎo)航免疫PID控制器設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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ū)田間道路自主行駛轉(zhuǎn)運(yùn)車及其視覺導(dǎo)航系統(tǒng)研制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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]艾長勝，林洪川，武德林，等. 葡萄園植保機(jī)器人路徑規(guī)劃算法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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]黃敏. 基于微分平坦理論的四旋翼無人機(jī)軌跡規(guī)劃的研究[D]. 沈陽：東北大學(xué)，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)代機(jī)器人學(xué)：機(jī)構(gòu)、規(guī)劃與控制[M]. 北京：機(jī)械工業(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

鄭滋，楊圣慧，鄭永軍，等. 多旋翼無人機(jī)避障航跡規(guī)劃算法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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

國家重點(diǎn)研發(fā)計(jì)劃（2016YFD0200702，2018YFD0700603）

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

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

10.11975/j.issn.1002-6819.2020.23.007

S251; TP273

A

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