多旋翼无人机避障航迹规划算法

郑 滋，杨圣慧，郑永军，刘星星，陈 建，苏道毕力格

·农业航空工程·

多旋翼无人机避障航迹规划算法

郑 滋，杨圣慧，郑永军※，刘星星，陈 建，苏道毕力格

（中国农业大学工学院，北京 100083）

多旋翼无人机的自主避障能力是安全作业的重要保证。该研究针对多旋翼无人机自主避障问题，提出了一种改进的双向RRT快速随机树航迹规划算法，结合最小化位移四阶导数的动力学优化方法，生成更符合多旋翼无人机动力学性能的避障航迹，解决避障过程中重复搜索、航迹曲率波动性大等问题，实现平稳避障；提出了以随机采样算法规划难度（用时）为核心的场景复杂度评价方法，在不同复杂度场景下进行了仿真试验。结果表明：与改进前相比，避障航迹再规划迭代次数减少23.69%；有效避障航迹规划用时不高于0.33 s、平均避障速度不低于1.16 m/s、避障航迹延长率最多达20.82%。所提出的避障航迹规划方法，提升了避障航迹的规划效率与效果，可为多旋翼无人机自主作业过程中的避障航迹规划提供参考。

无人机；优化；航迹规划；避障；路径配置；RRT-Connect；动力学优化

0 引 言

多旋翼无人机作业效率高、操作简便、通过性强[1-4]，在植保作业、林业巡检等领域得到了广泛应用。但在实际作业过程中，无人机会面临种类繁多、特征复杂的障碍，及时感知周围环境，实现快速、平稳、低消耗的避障[5-6]，可极大降低作业风险。

然而，现有多旋翼无人机采用的避障方法智能化程度不足，在发现障碍后需悬停等待操作人员介入，如植保领域的大疆MG-1P[7]、高科新农M23-E[8]等，当无人机较远时，操作人员不易把握行列特征[9]，不能保证采取有效的规避措施。因此，基于实时感知到的障碍信息自主规划形成避障路径，是无人机无须在人工干预下，特别是在操作人员无法观测到环境状况或自动作业过程中，完成后继任务的一种有效手段。

目前，已有大量相关研究形成了基于环境感知数据的避障路径规划算法，例如A*[10-12]、粒子群优化[13]、PRM[14]、人工势场法[15]、几何强化学习[16]、演化算法[17]等，但是上述算法未考虑动力学约束，规划的路径较为曲折。在农机领域，更多集中在地面农机的自主作业方面，包括基于激光雷达的温室机器人环境建图与导航[18]、油菜播种机的自主导航[19]、丘陵山区田间转运设备的导航[20]、葡萄园植保装备的导航[21]等，采用的算法也是PID、A*、支持向量机等，较少涉及多旋翼无人机的路径规划与避障算法研究。

RRT快速随机树算法[22]作为基于采样的路径规划方法，其概率完备性确保了只要有解则必能求解，但由于较强的随机性无法保证寻径效率，因此最终寻径结果欠优。RRT算法的一些的变体，例如RRT*[23]、RRT-Connect[24]等，虽降低了解的随机性，但也未考虑动力学约束问题。有学者将动力学特性与RRT算法进行融合，例如Webb等提出的运动动力学优化RRT*[25]、Ghosh等提出的KB-RRT[26]。这些变体修改了RRT算法的延拓方式，确保了任意单步航迹的动力学优化，但仍然不能获得整条避障航迹上的最优动力学优化路径。

综上，本文提出针对RRT-Connect路径规划算法的多种优化改进，生成更符合多旋翼无人机动力学约束的航迹，实现最小代价的避障飞行，以期为多旋翼无人机自主作业与避障提供方案参考。

1 多旋翼无人机避障系统

1.1 坐标系定义

多旋翼无人机及相关部件的位姿使用本地东北天（Local East-North-Universe, ENU）坐标系表示，机体右手坐标系以表示，传感器右手坐标系以表示。其中，ENU坐标系的坐标轴为[x,y,z]。x轴指向地理正东方向，y轴指向地理正北方向，z轴指向重力反方向，与世界坐标系（WGS-84）的相对关系基于多旋翼无人机的GNSS定位数据确定；机体右手坐标系的坐标轴为[x,y,z]，x垂直机头方向水平向右，y指向机头方向，z垂直向上，原点位于无人机质心；传感器坐标系的坐标轴[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产生的升力，N；n1、n2、n3、n4分别为螺旋桨1、螺旋桨2、螺旋桨3、螺旋桨4的转速，r·min-1。

1.2 系统硬件

本研究基于5kg载荷的Carto F4构建多旋翼无人机系统。机架上安装Rplidar S1激光雷达和PIXHAWK飞行控制器，分别用于检测和控制多旋翼无人机的飞行；根据飞行控制器芯片处理能力加装了NVIDIA TX2高性能计算模块，用于点云数据与避障逻辑处理。选用的Rplidar S1为二维单线激光雷达，测量分辨率为3 cm（实测近距离可到1 cm），以一个固定的小仰角布置，扫描某二维平面，用于检测30 m范围内的障碍物。该仰角大小与飞行时产生的前倾角相等，保证飞行时激光雷达检测面水平。同时，在飞行过程中，激光雷达仅扫描无人机所在的飞行面，获取的障碍信息即为飞行路径上的障碍。表1为Rplidar S1激光雷达、PIXHAWK飞行控制器、NVIDIA TX2的主要参数。

表1 Rplidar S1激光雷达、PIXHAWK飞行控制器、NVIDIA TX2主要参数

图2为系统的功能结构及各主要功能模块间的数据流关系。NVIDIA TX2的软件基于ROS创建，采用分布式软件结构解除耦合关系；PIXHAWK的软件由飞行固件PX4提供，通过Mavlink协议与PX4提供的API接口进行数据交换。

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

1.3 多旋翼无人机控制方法

根据多旋翼无人机的微分平坦特性[27]，若控制位置及偏航角，即可保证唯一姿态解。限于机载电脑与飞信控制器间的通信速率，将控制量转化为ENU坐标系下的速度指令并发送给飞行控制器，实现对航迹的跟踪控制。此外，设计了纯跟随策略的PID位姿控制器，实现航迹的平稳跟踪，实时计算理想位姿。

位置误差控制为基本限幅PID反馈控制算法，输入量为理想位置r、理想偏航角，输出速度控制向量r′、偏航角速度控制向量c′。使用比例因子K乘以实际位姿与理想位姿之间的差值表示修正力度；使用积分因子K乘以误差和消除系统静差；使用K乘以误差变化量增加响应速度。PID的离散位置形式如公式（1）。

式中为当前离散时刻，无量纲；为位置误差向量，rad；e为偏航角误差，rad；r′为输出速度控制向量，m/s；c′为偏航角速度控制向量，rad/s。

为使理想姿态平稳变化，采用纯跟随策略设计理想位姿r、的更新方式：在所有航点中首先确定距离当前位置欧氏距离最近的航点，并由此航点向终点方向遍历，选择第一个满足条件的航点()，()与当前位置()的欧氏距离大于给定阈值，然后将此航点作为接下来飞行的目标航点。航点更新策略用公式（2）表示：

2 航迹规划

若仅依赖实时传感器数据，无人机避障时会面临适应场景较为单一、难以脱离局部最优、避障航迹不光滑的问题。其中，局部最优可导致无人机在特定位置震荡，避障航迹不光滑可导致多旋翼无人机较大的飞行能量开销，最终降低植保作业效率。因此，在建立详细的环境感知数据后，需进行航迹规划。

多旋翼无人机的航迹规划一般分为2个部分：首先基于环境地图规划基本避障路径，然后将飞行路径与各种速度、加速度约束共同考虑优化为航迹。

2.1 概率栅格地图构建

概率栅格地图通过二值贝叶斯概率对物体在空间的占据状态进行估计，目前已广泛应用于智能导航领域[28]。为简化计算和规划复杂度，首先将三维地图降维成二维栅格地图。通过八叉树地图软件OctoMap[29]，对于如图3a所展示的仿真场景进行重建，得到三维概率栅格地图（图3b），再以多旋翼无人机所在的飞行平面对地图进行截取得到图3c概率栅格地图。

2.2 基于改进的RRT-Connect路径规划

基于构建的概率栅格地图，需建立起点与终点间的联系。为解决起点与终点之间互寻能力较弱的问题，本文采用双向快速扩展随机树（RRT-Connect）算法，并对该算法优化改进，缩短空间探索时间，增强算法对复杂环境的适应性。

2.2.1 质心偏向采样改进

RRT-Connect节点生长过程由2部分决定，一是起点树与终点树互寻的引导环节，二是随机采样。对于无人机而言，障碍分布是随机的，因此随机采样目的性不强。故本研究采用质心偏向采样，使2棵树的节点生长互相牵引、互相引导。质心偏向采样通过增量计算获得树的质心位置（如公式（3））。

式中为树索引，为1时代表起点树，为2时代表终点树；为质心位置，m；为树的节点；下标为水平坐标，m；下标为垂直坐标，m；为节点索引；为随机树的节点数量，m。

基于双向RRT算法，根据当前的RRT树，将起点或者终点作为采样点。若该采样点无法通过碰撞检测及重复点检测，则进行随机采样。若随机采样添加节点成功，则下一次采样继续使用目标引导采样，否则继续随机采样。

2.2.2 在线滚动优化改进

由于障碍物栅格地图是随着无人机飞行过程逐渐建立的，当新障碍出现时，原有路径和随机树可能被截断。若重新规划，则会抛弃对原空间的感知，重复不必要的探索，并可能得到与现有路径差异极大的新路径，导致控制器震荡。因此，本文采用在线滚动优化法，基于上一规划得到的随机树，随地图更新调整随机树结构，继续采样生长具体步骤为：首先找出与新的规划起点和终点距离最近的随机树，若与树上最近点的距离低于阈值，则将规划的起点设置为该最近点，并把该点变更为树的根节点；若高于阈值，则尝试与最近树建立连接关系，若失败，则以这一点重新建立树。如此获得2颗树后，先剪去2颗树上与障碍物发生干涉的所有“枝条”，然后对随机树进行延伸生长，完成在线滚动优化。

2.3 规划路径的全航迹动力学优化

建立飞行规划路径后，为减小路径中的跳变点和异常干涉点，本文采用基于Minimum Snap的动力学优化方法对全航迹进行精简优化，并设计高低膨胀概率栅格地图算法，防止航迹与障碍物发生干涉。

全航迹动力学优化共分为3步：获取最简避障路径，建立代价方程，求解最小化代价函数的解获取航迹参数。

在飞行过程中，无人机作业飞行的稳定性与航迹曲率有关。由于载荷与加速度的约束值呈反比，因此可调整速度与加速度的约束值，形成最佳避障路径。对于航拍、测绘、遥感用无人机而言，其载荷固定，因此航迹曲率变化极小；对于植保无人机而言，虽然药液质量会发生变化，但是由无人机微分平坦性可知[27]，各坐标系确定后，轨迹方程的解唯一，与药液质量的变化无关，不影响速度与加速度约束初值，故不影响无人机实现避障功能。

2.3.1 路径简化预处理

2.3.2 代价方程

路径简化处理后，需建立代价方程。多旋翼无人机的作业飞行航迹可以通过若干个多项式函数表示。航点集合{()(0000)(1111),,(x,y,z,ψ)}包含由空间坐标与偏航角从起始0到当前组成的航点。连接中两两相邻航点的航线为。{,,,}的4个平坦输出分量相互独立，可独立求解。设为平坦输出分量的一般化形式，则有一般化航点集合P={|0,1,…,α}，连接任意相邻一般化航迹的航点可以用阶多项式方程表示为

由此，一般化航迹函数可表示为

以一般化航迹的阶导数的平方和积分构建代价函数()：

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

由于()对于任意的非负，则矩阵为半正定黑塞矩阵（当优化深度为0时，为正定黑塞矩阵），通过进一步构建约束方程，得到如下形式的二次规划问题：

式中为约束矩阵；为约束下限向量；为约束上限向量。

2.3.3 一般化航迹的约束方程

按类型，约束可分为规划性约束、连续性约束和动力学约束[31]。规划性约束用于保证一般化航迹经过指定的一般化航点；连续性约束用于保证相邻段一般化航迹的连续性；动力学约束用于保证一般化航迹符合一般化动力学要求。表2为一般化航迹所需约束类型及约束个数。

表2 一般化航迹的约束类型

1）规划性约束

一般化航点位置约束用于保证航迹函数S()经过一般化航点集合P，其形式为

式中s为各航迹曲线的参数向量；i(=0,1,…,)为归一化的时间量；P(=0,1,…,)为航迹点位置，m。

2）连续性约束

由于两航线之间采用不同的多项式函数表示，需保证两函数交点的连续性，至少要求相邻航线的零阶、一阶、二阶导数在交点处相等。

相邻航迹交点一般化位置连续性约束表达形式为

相邻航迹接点一般化速度连续性约束表达形式为

相邻航迹接点一般化加速度连续性约束表达式为

3）动力学约束

动力学约束需对一般化速度S()′、一般化加速度S()(2)的极值做出限制。

最大速度约束数学表达形式为

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

最大加速度约束数学表达形式为

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

2.3.4 基于高低膨胀的概率栅格地图处理

由于一般化位置约束仅在约束处生效，在特定的动力学约束条件下，偏离量可能无法接受，导致全航迹动力学优化后的航迹侵入障碍区域（如图4a所示）。因此，采用如图4b所示的高低膨胀栅格概率地图方法解决该问题。

首先通过高膨胀量获取障碍物栅格地图，使用前述系列方法获得优化后航迹；然后使用低膨胀量的障碍物栅格地图进行干涉检测：若检测通过，则按该航迹飞行；若多次尝试失败，则将该航迹降级为直线连接航迹，在下一次障碍地图更新时再尝试拟合为多项式航迹。

图4 概率栅格地图膨胀前后无人机规划路径对比

3 算法验证

为更准确地量化分析规划场景的复杂程度，本文采用以随机采样算法为核心的场景复杂度评价方法，将随机采样算法应用于不同规划场景，对比分析规划用时，评估场景复杂度；以随机采样规划为对照，对比不同算法在同一场景条件的规划用时，评估算法的规划能力。

3.1 改进RRT-Connect算法试验

为验证改进RRT-Connect算法的有效性，在Matlab环境下实现改进的双向RRT算法。构建随机设计的6种不同复杂度场景（图5），分别执行1 000次寻径测试。由于算法的规划维度为二维，因此解的空间维度设为2。双向RRT规划结果分别与RRT算法、RRT*算法、RRT-Connect的条件随机采样算法和启发式采样改进型算法的规划结果分别进行对比。条件随机采样与质心偏向采样类似，为相互采样中点的改进。各RRT算法计算步长设为0.4 m、阈值设为0.4 m，最大失败迭代次数=10 000，终点阈值t=0.8 m，地图高膨胀量=2 m，低膨胀量=1 m，启发改进的RRT算法启发参数=0.5。

图5 障碍物地图

3.1.1 改进RRT-Connect算法优化结果与分析

1）质心偏向采样优化效果分析

图6为改进RRT-Connect算法计算成功率与迭代次数关系图。其中，成功率是指在一定迭代次数下，成功完成路径规划的次数与总路径规划次数的比值。图6可知：相比于随机采样，基于质心偏向采样的RRT-Connect在迭代次数较低时成功率更高，如单障碍物、三障碍物、多障碍物、随机障碍物场景，但在某些场景中的成功率会低于其他RRT算法。与其他RRT算法相比，应用质心偏向采样的改进RRT-Connect算法保持了能快速在复杂场景规划的优势，仅需约6 000步即可到100%成功率；而在简单场景（如图6a）中，随机采样更迅速。当成功率为100%时，质心偏向采样和随机采样的总迭代次数相近，而条件随机采样相对于RRT-Connect算法效果较差。因此，质心偏向采样可在多数场景中更快地引导起点树与终点树的互寻，但在被遮挡的情况下算法成功率没有提升。

图7为依据迭代次数和路径代价测试数据绘制的数据点云图。其中，路径代价在本研究中为路径长度。由数据点云分布可知，包括RRT*在内的所有算法迭代次数和路径代价不具有回归关系，但数据点的整体分布呈现出明显的差异。由图7中黑色的误差区间长短可知，质心偏向采样改进的RRT-Connect所需要的平均迭代次数在双障碍物、三障碍物、四障碍物、多障碍物与随机障碍物场景中最少，路径代价更低。

注：α为启发改进RRT算法的启发参数

图7 不同障碍物地图的规划路径代价-迭代次数数据点云图

2）在线滚动优化结果分析

首先采用RRT算法对避障航迹进行规划，然后更新测试场景，对比在新场景下的在线滚动优化和完全重新规划的规划时间、规划迭代次数。每个测试场景均包含一个即将出现的障碍物。每组场景进行1 000次测试。

图8为不同场景下的迭代次数与迭代时间的数据点分布图。不同的测试场景中，在线滚动优化改进后的RRT-Connect算法可以显著降低迭代次数和规划时间。由于快速随机树算法是基于概率的，因此会出现少许与改进后RRT-Connect算法规划时间相近的情况。在这些情况中，改进后RRT-Connect算法并没有显著的优化效果；由误差棒可知，优化并不能显著降低性能指标的主要分布范围。迭代次数平均降低了35.98%，单障碍物场景迭代次数降低了39.58%，双障碍物场景优化效果最低，迭代次数降低了23.69%，场景3迭代次数优化效果最好，降低了44.67%。

图8 在线滚动优化改进前后迭代次数和规划时间散点图

3.1.2 全航迹动力学优化结果分析

采用OSQP二次规划问题求解器作为QP（二次规划）问题的求解器，对多个RRT避障路径结果截弯取直得到避障路径关键节点，然后进行全航迹动力学优化，表3为相关动力学优化参数设置。

表3 全航迹动力学优化参数

注：状态向量组成为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为优化得到的航迹，可知航向角在0°至45°至−30°变化过程中连续，无跳跃间断点或振荡间断点，证明避障航迹平滑。图10分别是图9中速度、加速度、偏航角速度′和偏航角加速度″变化曲线。4个变量均为矢量值，在图中以正负区分方向。由图9可知，速度、加速度的正方向为向右（方向）和向前（方向）；角速度、角加速度正方向为无人机向左飞行时产生的角速度方向。

由图10可知，各个参数平滑性较好，仅在23～30 s，x方向的速度快速变化3 m/s、加速度快速变化约0.4 m/s2，这是由于多旋翼无人机此时处在避障航迹的拐点上，飞行方向发生了较大的变化。、、′、″对应的最大值符合表3中给出的参数绝对值约束。上述结果显示，全航迹动力学优化算法可行，可以生成符合动力学要求的航迹。

图9 避障规划路径的动力学优化结果

图10 避障规划路径的速度v、加速度a、角速度ψ′与角加速度ψ″变化

3.2 不同场景的仿真试验与结果分析

3.2.1 仿真试验设计

为证明算法在不同复杂度场景的通用性，基于Gazebo和PX4的SITL设计了多旋翼无人机避障仿真系统，Gazebo用于仿真避障场景，提供仿真激光雷达数据；PX4的SITL用于模拟多旋翼无人机飞行控制器，与本文的多旋翼无人机避障控制软件交互，与Gazebo进行仿真姿态与物理反馈信息的通信；Mavros用于将Mavlink协议转换为ROS（Robot Operating System）话题、服务，承担通信中间件。各功能组件间数据流关系如图11所示。

图11 仿真系统结构与数据流关系图

在Gazebo中编辑URDF（United Robot Description File，统一机器人描述文件）脚本，搭建不同复杂度的障碍场景，包括单障碍物场景（图12a）、多障碍物场景（图12b）和迷宫障碍物场景（图12c），检验方法的适应性和通用性。图12d为仿真试验场景。

采用松树模型组成飞行地图，网格棋盘用于标记起点和终点位置。其中，终点设置在一个完全被遮挡住且靠近障碍的区域。

试验设定起点与终点间直线距离45 m；自然风干扰设置为0；保留GPS定位干扰和随机游走干扰，重力加速度设置为9.8 m/s2；根据Rplidar S1激光雷达参数，设置激光雷达水平扫描范围−0.873～1.047 rad（为减少数据量做了扫描范围的缩减），水平扫描点云数量200，传感器刷新率25 Hz，最小测量距离0.3 m，最大测量距离40 m，传感器测量噪声为0.04 m。

图12 仿真场景和仿真多旋翼无人机

表4为仿真试验参数，启动避障控制系统软件，控制多旋翼无人机自动起飞开始避障任务，到达目的地后自动降落后关闭程序，完成测试过程。

表4 避障控制关键参数

注：各参数通过调试阶段的整定得出，综合了多旋翼无人机避障域的安全尺寸要求、Minimum Snap动力学优化的高低膨胀法要求和航线规划成功率的要求。

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规划用时明显上升，说明第2次迭代时场景细节重建更加完整，因此场景更复杂；随后，由于在线滚动优化，可以利用上一次迭代修剪过的RRT树进行规划，而且更加接近终点，地图复杂度下降，规划用时随之开始下降。采用动力学优化算法用时呈现略微下降趋势。整体上，该算法对多场景具有良好的适应性和通用性，任意场景的总规划用时不高于0.33 s。

图13 不同复杂度场景的规划用时

从试验结果（图14和图15）看，在所设计的各个场景中，多旋翼无人机均能成功完成避障到达终点。在单障碍物场景中，仅需一次规划即可达到最终目标，总飞行时间39.10 s，飞行距离51.24 m，实际航迹长度较起止最短距离延长2.48%，平均飞行速度1.31 m/s；在多障碍物场景中，经历4次规划达到最终目标，总飞行时间43.55 s，飞行距离50.61 m，实际航迹长度较起止点最短距离延长1.22%，平均飞行速度1.16 m/s；对于迷宫场景，共计滚动优化航迹2次。虽然2次规划所依赖的障碍地图差异较大，但避障航迹的规划结果仍保持稳定。降维得到的二维障碍物栅格概率地图也保留了障碍核心信息，总飞行时间43.77 s，飞行距离54.37 m，实际航迹长度较起止点最短距离延长20.82%，平均飞行速度1.24 m/s。

由图16可知，速度曲线没有明显拐点，加速度曲线基本平滑，证明无人机飞行过程中航迹没有明显的突变或异常，本文算法可以规划生成符合无人机动力学的平滑航迹。

图14 三维重建及避障航迹图

图15 避障航迹滚动优化过程仿真结果

图16 不同场景下速度与加速度的变化

4 结 论

本文基于环境感知信息重建障碍栅格概率地图，提出了基于质心偏向采样的双向RRT航线在线滚动优化方法；利用Minimum Snap法和高低膨胀栅格概率地图法，对规划的航迹进行了全航迹动力学优化，结论如下：

1）提出的以随机采样算法规划难度（用时）为核心的场景复杂度评价方法，以随机采样算法在各场景下完成路径规划的用时为依据，量化了不同场景的规划难度差异，更准确、更泛化地评估场景的复杂程度，并在同一场景条件下对比分析了不同算法间的差异；

2）经不同场景仿真测试可知，改进的双向RRT算法较改进前避障航迹再规划迭代次数减少23.69%，有效避障航迹规划用时不高于0.33 s、平均避障航迹速度不低于1.16 m/s、避障航迹延长率不超过20.82%。所提出的避障航迹规划方法能够满足不同复杂度场景避障路径的实时规划要求，可以有效提升路径规划效率和平稳性，为无人机在农田果园植保、林间巡检等实际作业环境的避障提供了一种可行的技术方案。

[1]郑永军，陈炳太，吕昊暾，等. 中国果园植保机械化技术与装备研究进展[J]. 农业工程学报，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]郑永军，江世界，陈炳太，等. 丘陵山区果园机械化技术与装备研究进展[J]. 农业机械学报，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]兰玉彬，王林琳，张亚莉. 农用无人机避障技术的应用现状及展望[J]. 农业工程学报，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]周志艳，臧英，罗锡文，等. 中国农业航空植保产业技术创新发展战略[J]. 农业工程学报，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]大疆创新. MG-1P农业无人机用户手册[M/OL]. [2018-11-08]. https://www.dji.com/cn/downloads/products/mg-1p.

[8]高科新农. 全自主飞行20L单旋翼电动无人机[ER/OL]. https://www.81uav.cn/product/5568.html.

[9]彭孝东，张铁民，李继宇，等. 基于目视遥控的无人机直线飞行与航线作业试验[J]. 农业机械学报，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]. 农业工程学报，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]侯加林，蒲文洋，李天华，等. 双激光雷达温室运输机器人导航系统研制[J]. 农业工程学报，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]. 农业工程学报，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]李云伍，徐俊杰，王铭枫，等. 丘陵山区田间道路自主行驶转运车及其视觉导航系统研制[J]. 农业工程学报，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]. 农业工程学报，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]黄敏. 基于微分平坦理论的四旋翼无人机轨迹规划的研究[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. 现代机器人学：机构、规划与控制[M]. 北京：机械工业出版社，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]. 农业工程学报，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

国家重点研发计划（2016YFD0200702，2018YFD0700603）

郑滋，主要从事计算机测控技术、智能检测与避障技术研究。Email：493956170@qq.com

郑永军，博士，教授，主要从事智能农业装备技术与研发。Email：zyj@cau.edu.cn

10.11975/j.issn.1002-6819.2020.23.007

S251; TP273

A

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