基于改进RRT*算法的菠萝采收机导航路径规划
2022-03-10刘天湖程一丰
刘天湖，张 迪，郑 琰，程一丰，裘 健，齐 龙
基于改进RRT*算法的菠萝采收机导航路径规划
刘天湖，张 迪，郑 琰，程一丰，裘 健，齐 龙
（华南农业大学工程学院，广州 510642）
为了提高菠萝收获的机械化和自动化水平，该研究采用改进RRT*（Rapidly-exploring Random Trees Star）算法进行菠萝采收机全局作业路径规划。首先引入自启发式思想约束采样点的生成，借鉴人工势场引入方向权重对新节点拓展方向进行约束，同时计算合适的权重取值范围，采用双向拓展加快迭代速度，然后利用贪心算法修剪路径冗余节点，并利用Cantmull-Rom插值函数对路径进行平滑处理。根据农田道路情况创建多障碍物、迷宫和狭窄通道3种仿真环境，分别对RRT*算法、双向RRT*算法和改进后RRT*算法的性能进行测试。试验结果表明：3种环境下，本文算法的平均收敛时间是RRT*算法的18%，是双向RRT*算法的46.12%，平均规划速度是RRT*算法的5.7倍，是双向RRT*算法的2.3倍左右，平均拓展节点数量比 RRT* 算法减少87.23%，比双向 RRT* 算法减少 52.52%，平均路径长度比 RRT* 算法减少 3.81%，比双向 RRT* 算法减少 6.08%。田间试验结果表明：本文算法的规划时间仅为RRT*算法的14.12%，为双向RRT*的20.34%，迭代次数比RRT*算法减少80.89%，比双向RRT*减少69.70%。另外，RRT*和双向RRT*算法规划路径上大于60°的转角分别是本文算法的1.56和2.06倍，大于100°的转角分别是本文算法的1.55和2.18倍，本文算法规划的路径更平滑。研究结果可为菠萝采收机导航提供参考。
雷达；算法；菠萝收获；路径规划；快速搜索随机树
0 引 言
中国是世界十大菠萝生产国之一，产量和需求量逐年上升。现阶段，国内主要依靠人工采收菠萝，但农业劳动力老龄化问题日益严重[1]。无人驾驶、自动导航是菠萝采收机未来发展的主要方向。
在农机导航路径研究方面，贾闯等[2]研究了果园单轨运输机超声波避障系统，通过在果园预先铺设轨道实现运输车的自主移动，但是运输车只能在轨道上运行，缺乏灵活性。为了提高导航的自主性，有学者使用路径规划来完成导航。导航路径规划根据对环境感知信息的完整性可分为静态全局路径规划和局部动态路径规划[3]。全局路径规划需要先获取环境的完整信息，直接使用路径规划算法规划出一条全局路径。局部路径规划则在未知全部环境信息的情况下，使用传感器先对周围环境进行感知，再通过优化算法规划出一条合理的路径。部分学者基于机器视觉或激光雷达进行了农业机器人局部路径规划的相关研究。例如，彭顺正等[4]提出矮化密植枣园收获作业视觉导航路径提取方法，通过最小二乘法（Least Squares Method, LSM）原理拟合左右两侧边缘，提取边缘线上各行的几何中心点生成枣园导航基准线。张振乾等[5]提出一种基于双目视觉的香蕉园巡检路径提取方法。局部路径规划有效保障了规划路径的安全性与实时性，但只使用局部动态路径规划不能保证得到最优解，容易陷入局部最优导致导航效率低下[6]，所以需要先使用全局路径规划找到一条合理的路径。李云伍等[7]对A*算法估价函数进行改进，将路口节点处的道路曲率及道路起伏信息引入代价函数进行全局路径规划，但是A*算法在大地图内规划速度较低[8]。为此，本文引入快速扩展随机树（Rapid-exploration Random Trees, RRT）算法进行全局路径规划。RRT算法不需要对系统进行建模，且对搜索空间的覆盖率高、搜索范围广，适用于大规模环境的路径规划[9]。但是RRT算法不具备渐进最优性，且搜索过程中计算代价较大[10]。在考虑代价路径的基础上，文献[11]提出了渐进最优快速拓展随机树(Rapid-exploration Random Trees Star, RRT*)算法，通过选择最优父亲节点和相邻节点回溯重构保证算法达到全局最优解。大多数农业作业发生在以时间和空间快速变化为特征的非结构化环境中[12]。农田道路状况复杂，存在道路狭窄和多障碍物等情况，RRT* 算法仍然存在规划效率低、算法搜索到的目标点与实际位置存在偏差等问题。故本文提出一种改进RRT*算法，重点解决大地图内规划效率较低和位置偏差问题。根据道路环境创建仿真地图，对比RRT*算法、双向RRT*算法和改进RRT*算法在不同静态地图下的性能。
1 导航路径规划方法
1.1 技术框架
农机作业路径规划需要在满足实际作业约束条件前提下，以最小化调度成本建立调度模型。由于作业环境中障碍物较多，作业时单一路径规划算法无法满足导航要求，故需要将多种方法进行整合，在全局路径规划的前提下进行局部路径调整以保证全局最优。当农机在导航过程中与周围环境发生冲突时进行局部规划，冲突解决后继续按全局规划路径行进。本文导航路径规划的技术框架如图1所示。
图1 导航路径规划技术框架
具体步骤如下：
1）根据初始环境信息构建整体静态地图，路径信息反馈到农机的决策系统，决策系统将路径信息传输到环境感知系统，根据规划任务为路径规划系统提供路径信息，规划全局路径；
2）使用路径规划算法规划出一条从起点到目标点的最优或接近最优的全局路径；
3）根据农机上装载的传感器对周围环境进行实时感知，获取动态的障碍物信息；
4）将动态环境信息反馈给农机的执行装置生成新的决策方案实时调整全局路径信息。在运动过程中对农机进行定位，包括农机的位置、行为状态等信息引导农机沿着规划的路径安全运动到目标点。
1.2 算法介绍
RRT*算法是一种单向搜索算法，每次搜索路径时从起始点出发开始向目标点搜索，当快速搜索随机树搜索到目标点后，规划出一条可行路径。RRT*算法在拓展时随机产生采样点，这导致快速搜索随机树拓展的方向具有不确定性，容易与目标位置产生偏离从而导致拓展时间过长且耗费大量内存。改进后的算法融入自启发式思想[13]、人工势场[14]和双向拓展思想[15]对采样点进行约束，以提高采样点的目标导向性，同时对新节点的生成方式优化，防止新节点在拓展过程中过度远离目标点。
基于改进RRT*算法的全局路径规划流程如图2所示。首先，根据实际农田的路径信息建立全局静态地图；然后利用改进RRT*算法规划出一条从起始点到目标点的路径，并且修剪路径上的冗余节点；最后经过插值函数的路径曲线平滑处理生成一条连续无障碍的全局作业路径。
图2 使用RRT*算法生成全局路径的流程
1.3 构建全局地图
地图信息是获取导航路径规划的关键。本文使用无人机、遥感卫星捕获农田图像，然后进行图像处理（包括颜色增强、灰度化、二值化分割和高斯滤波等操作）[16]从农田图像中分割出静态地图。或使用装载在无人驾驶拖拉机上的环境感知工具如激光雷达、深度相机等创建全局地图。
1.4 算法改进
1.4.1 目标约束采样
采样点位置影响即将生成的新节点位置，为了提高采样点的目标导向性，本文借鉴自启发式思想设置目标偏置概率p（p的取值范围为0～1，本文中p=0.2），在采样中按照均匀概率随机产生一个概率值。当p>p时，在自由空间内随机产生一个采样点，否则把目标点作为采样点，目标约束采样公式为
式中X为采样点，X为目标点，Sample()为随机采样函数。
1.4.2 节点偏置拓展
RRT*算法完全由采样点控制新节点的拓展，生成采样点的随机性导致新节点在拓展时具有盲目性，从而使搜索时间过长。为了克服这一问题，本文对节点拓展方式进行优化，引入目标点使新节点在拓展时被目标点和采样点约束。同时借鉴人工势场算法中的引力场思想，分别在目标点和采样点方向分配不同的权重，使快速搜索随机树每一次都向更接近目标点的方向拓展，加快搜索速率。拓展过程如图3所示。
注：Xstart为起始点；Xrand为采样点；Xnear为快速搜索随机树上距离采样点最近的节点；Xgoal为目标点；Xnew为拓展的新节点；wg和wk为方向权重；δ为拓展步长。
新节点在拓展时分别在采样点和目标点方向分配权重w和w。新节点方向矢量和坐标如式（2）所示。
权重w影响节点的拓展方向，合适的w值可以约束快速搜索随机树的拓展方向，并防止拓展的新节点过度远离目标点或者接近障碍物，所以需要对w的取值范围进行约束。将最近点和目标点连接线上的障碍物当作质点，以任意质点为圆心设置半径为的膨胀区，且膨胀区半径不小于菠萝采收机的半径。图4为w范围求解示意图。
对于随机采样点X，如果采样点的位置在切线方向和目标点方向之间的区域，向采样点方向拓展已经靠近目标点的方向，为了减少计算量，此时w=1，只需讨论采样点落在区域外的情况。节点拓展时，如果最近点到障碍物质点距离与膨胀区半径的差值大于拓展步长，代表新节点拓展的方向偏向目标点方向，<。否则拓展方向偏向随机点方向，>。计算权重时比较cos和cos的值，cos和cos表示如下：
式中(,)为最近点到障碍物的距离，和的取值范围为0～π。
>时<，cos>cos；<时>，cos
1.最近点X向膨胀区作的切线 2.新节点拓展的方向矢量 3.障碍物 4.膨胀区
1.Tangent of the nearest pointXto the expansion zone 2.Direction vector of the new node expansion 3.Obstacle 4.Expansion zone
注：X为障碍物点；为膨胀区半径，mm；为X与X之间距离与的差值，mm；为新节点拓展的方向矢量；为切线方向矢量和目标点方向矢量夹角，（°）；为新节点拓展方向矢量和目标点方向矢量之间夹角，（°）。
Note:Xis the obstacle point;is the radius of the expansion zone, mm;is the length of the distance betweenXand Xminus the length of，mm; The vectoris the direction vector of the new node expansion;is the angle between the tangent direction vector and the target point direction vector, (°);is the angle between the new node expansion direction vector and the target point direction vector, (°).
图4 方向权重w求解原理图
Fig.4 Solution schematic for direction weightw
1.4.3 双向拓展
为了提高收敛速度，本文改进RRT*算法着重于提高两棵快速搜索随机树的连接速度。搜索过程中，从起始点出发向目标点方向拓展快速搜索随机树，记作Tree1。从目标点向Tree1方向拓展快速搜索随机树，记作Tree2。
为加快双向搜索速度，本文提出一种动态步长优化方法：首先，判断Tree1新节点X1与Tree2上距离X1最近的点之间是否存在障碍物，如果不存在则将Tree1上的新节点X1作为Tree2的新节点，此时采用大步长step_max=(X2,X1)。该步骤的执行优先级为I级，如图5a所示。如果存在障碍物，如图5b所示，则设置阈值1、2，当两棵树的距离D大于1时，则两棵树还未进入连接状态，步长由Tree2上最近节点X与障碍物之间的距离d决定。当d大于2时，认定菠萝采收机处在安全区域内，此时采用中步长，中步长step_mid=。当最近节点与障碍物距离小于阈值2时，采用小步长step_min。小步长小于固定步长的长度，当固定步长的取值较小时也可等于固定步长。该步骤执行优先级为II级。步长计算公式为
式中=0表示X1与X2之间无障碍物，=1表示存在障碍物。
注：X1为Tree1上距离采样点最近的节点；X2为Tree2上距离采样点最近的节点；X2为Tree2的采样点；X1为Tree1的新节点；X2为Tree2的新节点。
Note:X1is the node closest to the sampling point on Tree1;X2is the node closest to the sampling point on Tree2;X2is the sampling point of Tree2;X1is the new node of Tree1;X2is the new node of Tree2.
图5 双向拓展过程示意图
Fig.5 Schematic diagram of the bidirectional expansion process
1.5 路径优化
1.5.1 路径剪枝
两棵快速搜索随机树连接后形成一条从起始点到目标点的可行路径，该路径是一条由多个离散节点组成的折线段，该路径上还存在许多非必要的节点。这些节点会增加路径长度，导致菠萝采收机需要花费更多的时间进行转向。因此，对路径上多余的节点进行修剪，寻找出一条无碰撞最短路径。
本文根据贪心算法[17]提出拉直法去除路径上的多余节点，修剪过程如图6所示。首先将起点作为拉直路径的第一个点，记为0，依次遍历路径上的节点，直到遇到节点5。当连接0与5之间的线段与障碍物产生碰撞时，停止遍历。拉直0与4间折线段的同时删除所有不在这条线段上的节点；将4作为拉直路径上的第二个点继续拉直操作。重复拉直操作直到遍历到目标点11。
注：P0～P11为控制节点；灰色区域为障碍物；实线段为原始路径；虚线段为修剪后的路径。
1.5.2 Cantmull-Rom样条插值
修剪冗余节点后的路径存在较大的转折角度，可能导致菠萝采收机在转弯时耗费更多时间，因此对路径进行平滑处理。常见的路径平滑方法有贝塞尔函数法、样条插值法等，本文使用Cantmull-Rom样条差值函数[18]对路径进行平滑处理。
Cantmull-Rom样条插值函数至少受4个控制点影响，分别记为P2、P1、P、P1，其中P处的切线记为(P1‒P1)，会影响曲线的扭曲程度。Cantmull-Rom插值函数可表示为
图7为Cantmull-Rom函数的插值原理，据此可求解式（6）中的0、1、2、3。
注：P2、P1、P、P+1为控制节点；实线()为平滑操作后的曲线；虚线为曲线()在节点P-1和P处的切线。
Note:P2,P1,P,P+1are the control nodes; the solid curve() is the curve after smooth operation; dashed lines are the tangents of the curve() at nodesP1andPrespectively.
图7 Cantmull-Rom函数插值原理
Fig.7 The principle of interpolation of Cantmull-Rom function
()取值范围为0～1，由图7可知，点P1即为()在=0的值，点P即为()在=1的值，将0和1代入式（6）有：
由式（7）计算可得0、1、2、3，由此得到Cantmull-Rom插值函数的转换关系为
Cantmull-Rom插值函数进行路径平滑处理时存在一个缺点：平滑后的曲线不经过第一控制点和最后一个控制点。但在路径规划中必须保证路径曲线经过起始点和目标点，所以在实际应用时需要分别在起始点和目标点附近取一个邻近点，以保证平滑后的曲线经过所有的控制节点。
2 仿真与试验
为了验证本文改进算法的性能，参照魏武等[19-20]研究，并结合农田路径实际情况，搭建多障碍物环境、迷宫环境和狭窄通道环境进行仿真试验。仿真试验系统为Ubuntu18.04，仿真软件为ROS melodic，Gazebo9和RViz，采用学习平台Turtlebot3，硬件配置为NVIDIA GEFORCE RTX 2060Ti 4GB显存，搭载Intel®Core™i5 -10400U CPU，计算机运行内存为8 GB。
试验前在Gazebo中建立三维仿真地图，控制Turtlebot3在三维地图中运动。激光雷达在扫描三维环境的同时使用ROS平台的gmapping功能包在RViz中构建对应的二维静态地图，3种环境在RViz中的二维静态地图如图8所示。
图8 仿真环境
将本文路径规划算法作为插件添加到ROS中，使用move base功能包调用该插件，试验从内存占用率、路径规划时间和路径长度3个方面对比分析RRT*算法、双向RRT*算法和本文改进RRT*算法性能，其中内存占用采用节点数量表征，节点数量越少占用内存越少。仿真中设定Turtlebot3的半径为200 mm，对障碍物进行膨胀处理，膨胀区为250 mm，试验时将机器人作为质点。每个环境分别进行30次试验。各环境仿真结果如图9和表1所示。
注：紫色线条为障碍物；蓝色部分为膨胀区；红色曲线为最终规划出的路径；绿色线条是快速搜索随机树的分支。
表1 3种算法在不同环境中的试验数据统计
2.1 多障碍物环境仿真结果分析
静态环境地图大小为==5 000 mm，起始点为[600，500]，终点为[4 600，4 500]，拓展步长100 mm，图9a为RRT*算法、双向RRT*算法和本文算法在多障碍物静态环境地图中的仿真结果。本文算法和双向RRT*算法的节点数量明显少于RRT*算法，本文算法在多障碍物环境中能找到一条相对平缓的路径。
由表1可知，本文改进RRT*算法的收敛时间仅为RRT*算法的14.54%，是双向RRT*算法的55.71%。本文算法的规划速度是RRT*算法的6.9倍，是双向RRT*算法的1.8倍左右，节点数量比RRT*算法少91.91%，比双向RRT*算法少40.29%，规划的路径长度比RRT*算法减少1.56%，比双向RRT*算法减少 6.39%。
2.2 迷宫环境仿真结果分析
静态环境地图大小为==5 000 mm，起始点为[300，300]，终点为[4 500，4 500]，拓展步长为100 mm，图9b为 RRT*算法、双向RRT*算法和本文算法在迷宫静态环境地图中的仿真结果。
RRT*算法规划的路径中搜索到的目标点与实际目标点之间存在明显的偏差。本文算法的扩展节点数远少于其他2种算法。迷宫环境存在长路径和短路径，如图9b中第1和第3张图中的红色曲线所示，在30次试验中改进算法有86.67%的概率找到短路径，而RRT*算法有46.67%的概率找到短路径，双向RRT*算法只有26.67%的概率找到短路径。这在一定程度上反映了本文算法在迷宫环境下性能的优异性，能有更有效地减少规划路径的长度。
由表1可知，本文改进RRT*算法的收敛时间仅为RRT*算法的18.17%，是双向RRT*算法的47.88%，规划速度是RRT*算法的5.5倍，是双向RRT*算法的2.1倍左右，节点数量比RRT*算法少84.88%，比双向RRT*算法少48.13%，路径长度比RRT*算法减少7.48%，比双向RRT*算法减少10%。
2.3 狭窄通道环境仿真结果分析
静态环境地图大小为Í=5 000 mmÍ5 250 mm，起始点为[1 000，500]，终点为[1 000，4 500]，拓展步长为100 mm，图9c为RRT*算法、双向RRT*算法和本文算法在狭窄通道静态环境地图中的仿真结果。RRT*算法规划的路径中搜索到的目标点与实际目标点存在明显偏差。本文算法扩展的节点数少于另外2种算法。在狭窄通道环境中，改进RRT*算法的可行路径比其他2种算法更接近障碍物，拐点更少。
由表1可知，本文改进RRT*算法的收敛时间仅为RRT*算法的21.28%，是双向RRT*算法的34.78%，规划速度是RRT*算法的4.7倍，是双向RRT*算法的2.9倍左右。节点数量比RRT*算法少84.89%，比双向RRT*算法少69.14%，路径长度比RRT*算法减少2.39%，比双向RRT*算法减少1.84%。
仿真试验结果表明，本文改进算法在3种环境中的平均收敛时间是RRT*算法的18%，双向RRT*算法的46.12%，平均规划速度是RRT*算法的5.7倍，双向RRT*算法的2.3倍左右，平均拓展的节点数量比 RRT* 算法少87.23%，比双向 RRT* 算法少52.52%，平均路径长度比 RRT* 算法减少3.81%，比双向 RRT* 算法减少6.08%。改进算法在减少拓展节点数量、降低内存占用方面效果明显，且在复杂环境下更容易找到最短路径。大部分情况下的路径规划时间比RRT*算法和双向RRT*算法短，证明了算法的优越性，路径也更加平滑。
2.4 使用Gazebo软件仿真菠萝采收机的行走
使用建模软件 Blender（Blender Foundation，Amsterdam，Netherlands）构建高度900 mm、最大直径800 mm的菠萝植株模型，如图10a所示。将模型导入Gazebo仿真软件，搭建U型菠萝地场景，如图10b所示。图10c为改进算法平滑后的最终路径。
图10 仿真地图及改进算法的规划路径
2.5 田间试验
为了验证本文改进算法路径规划效果，分别在菠萝行间、果树行间进行路径规划试验和菠萝采收机导航试验。使用思岚A2M12激光雷达采集环境信息，激光雷达测量半径为12 m，采样频率为16 k，扫描频率为10 Hz。菠萝植株高分布比较分散，最大值为930 mm，大部分集中在 650～850 mm之间，枝叶密集部位大部分集中在250～450 mm之间[21]。测量的果树平均高度为2.98 m，枝叶密集部位大部分集中在1.50 m以上。
2.5.1 菠萝行间路径规划试验
试验时间为2022年8月18日，地点为华南农业大学。菠萝行间实际情况如图11a所示。激光雷达扫描到的环境信息如图11b所示。由于菠萝种植间距较小且枝叶密集，所以雷达扫描到的特征点足够多，不需要膨化处理。图11c为RRT*算法、双向RRT*算法和改进算法在菠萝行间的路径规划结果，本文算法规划出的路径更平滑且转折点较少，RRT*算法和双向RRT*算法规划出的路径则存在较多的转折点。
图11 菠萝行间路径规划试验结果
将规划时间、迭代次数、转角大于60°的个数和转角大于100°的个数作为评价指标，10次路径规划试验的平均值如表2所示。本文算法的规划时间为RRT*算法的14.12%，为双向RRT*的20.34%；迭代次数比RRT*算法减少了80.90 %，比双向RRT*减少了69.70%。RRT*和双向RRT*算法规划路径上大于60°的转角分别是本文算法的1.56和2.06倍，大于100°的转角分别是本文算法的1.55和2.18倍，本文算法规划出的路径比RRT*算法和双向RRT*算法更平滑且转折点较少。
表2 菠萝行间路径规划试验统计数据
2.5.2果树行间路径规划试验
为了进一步验证本文算法的适用性，开展果数行间路径规划试验。试验时间为2022年8月21日，地点为华南农业大学。果树行间的实际情况如图12a所示。激光雷达扫描的环境信息如图12b所示，部分果树由于树冠较小导致雷达扫描到的特征点较少，为了提高路径规划的成功率在RViz中对特征点进行膨化处理，膨胀半径设置为10 cm。图12c为RRT*算法、双向RRT*算法和改进RRT*算法试验结果。本文算法规划路径更加平滑且转弯较少。
图12 果树行间路径规划试验结果
将规划时间、迭代次数、转角大于60°的个数和转角大于100°的个数作为评价指标，10次路径规划试验的平均值如表3所示。本文算法的规划时间为RRT*算法的25.26%，双向RRT*的36.16%，迭代次数比RRT*算法减少了65.14%，比双向RRT*减少38.04%。RRT*和双向RRT*算法规划路径中大于60°的转角分别是本文算法的2.70和3.26倍，大于100°的转角分别是本文算法的2.68和3.05倍。本文算法在果树间路径规划的路径减少了急转拐角，规划路径质量更高。
2.5.3 导航试验
菠萝行间试验时间为2022年10月15日，地点为华南农业大学，试验设备为在履带式高床作业机上开发的无人驾驶菠萝采收机，如图13a所示，主要包括采收装置、履带式高床作业机和无人驾驶控制装置。导航系统使用Nvidia Jetson Nano开发板，基于python和c++在ROS 平台进行上位机开发，搭载思岚系列激光雷达。
表3 果树行间路径规划试验统计数据
图13 菠萝采收机导航试验
设置雷达与地面的距离为300 mm（雷达与地面距离可调），试验场景如图13b所示。试验使用RViz软件实现环境信息可视化，验证改进算法的可行性。采收机以0.2、0.4、0.6 m/s的行驶速度分别进行5次试验，测量目标点和采收机中心点的导航偏差，结果如表4所示。经过15次试验，菠萝采收机均可沿着规划地路径运行到目标点，随着运动速度的提升，位置偏差和航向偏差有增加的趋势。平均位置偏差由5.75 cm增加到8.96 cm，平均航向偏差由7.78°增加到12.57°。由于菠萝畦间沟宽大于50 cm, 履带宽度为13 cm，平均位置偏差和航线偏差增加后仍然能满足菠萝采收机行走要求。
表4 导航精度偏差
3 结 论
1）综合考虑路径代价、路径平滑和碰撞检测等因素，将路径规划分为全局路径规划与局部动态避障，分析了菠萝采收机导航路径规划问题。针对全局路径规划提出了基于改进RRT*算法的路径规划方案。改进算法改善了RRT*算法的盲目性、收敛性差和不稳定的问题；对路径的冗余节点进行修剪，并利用Cantmull-Rom插值对路径做平滑处理。
2）建立了多障碍物、迷宫和狭窄通道仿真地图在RViz软件中通过仿真试验对算法进行验证，本文算法的平均规划速度是RRT*算法的5.7倍，是双向RRT*算法的2.3倍左右，平均路径长度比RRT*算法减少3.81%，比双向RRT*算法减少6.08%。对全局路径进行路径剪枝后，有效降低了转弯次数。利用Cantmull-Rom函数线进行路径平滑后，路径转折点处的尖峰得到优化。
3）田间试验结果表明，菠萝采收机可沿着规划地路径运行到目标点，但随着运动速度由0.2 m/s增加到0.6 m/s，平均位置偏差由5.75 cm增加到8.96 cm，航向偏差由7.78°增加到12.57°。
[1] Wu L L, Li G C, Zhou X S. Change of factor endowments and China agricultural growth path selection[J]. China Population, Resources and Environment, 2015, 25(8): 144-152.
[2] 贾闯，李加念，洪添胜，等. 山地果园单轨运输机超声波避障系统的设计与试验[J]. 农业工程学报，2015，31(增刊2)：69-74.
Jia Chuang, Li Jianian, Hong Tiansheng, et al. Design and test of ultrasonic obstacle avoidance system for mountain orchard monorail conveyor[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(Supp.2): 69-74. (in Chinese with English abstract)
[3] 孙玉山，冉祥瑞，张国成，等. 智能水下机器人路径规划研究现状与展望[J]. 哈尔滨工程大学学报，2020，41(8)：1111-1116.
Sun Yushan, Ran Xiangrui, Zhang Guocheng, et al. Research status and prospect of path planning for autonomous underwater vehicles[J]. Journal of Harbin Engineering University, 2020, 41(8): 1111-1116. (in Chinese with English abstract)
[4] 彭顺正，坎杂，李景彬. 矮化密植枣园收获作业视觉导航路径提取[J]. 农业工程学报，2017，33(9)：45-52.
Peng Shunzheng, Kan Za, Li Jingbin. Extraction of visual navigation directrix for harvesting operation in short-stalked and close-planting jujube orchard[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(9): 45-52. (in Chinese with English abstract)
[5] 张振乾，李世超，李晨阳，等. 基于双目视觉的香蕉园巡检机器人导航路径提取方法[J]. 农业工程学报，2021，37(21)：9-15.
Zhang Zhenqian, Li Shichao, Li Chenyang, et al. Navigation path detection method for a banana orchard inspection robot based on binocular vision[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 9-15. (in Chinese with English abstract)
[6] 霍凤财，迟金，黄梓健，等. 移动机器人路径规划算法综述[J]. 吉林大学学报（信息科学版），2018，36(6)：639-647.
Huo Fengcai, Chi Jin, Huang Zijian, et al. Review of path planning for mobile robots[J]. Journal of Jilin University (Information Science Edition), 2018, 36(6): 639-647. (in Chinese with English abstract)
[7] 李云伍，徐俊杰,，王铭枫，等. 丘陵山区田间道路自主行驶转运车及其视觉导航系统研制[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)
[8] 蔡雨岑，杜鹏桢. 基于平衡鲸鱼优化算法的无人车路径规划[J]. 控制与决策，2021，36(11)：2647-2655.
Cai Yucen, Du Pengzhen. Path planning of unmanned ground vehicle based on balanced whale optimization algorithm[J]. Control and Decision, 2021, 36(11): 2647-2655. (in Chinese with English abstract)
[9] LaValle S M. Rapidly-exploring random trees: A new tool for path planning[R]. Ames, USA: Iowa State University. Computer Science Dept. 1998.
[10] Nasir J, Islam F, Malik U, et al. RRT*-SMART: A rapid convergence implementation of RRT[J]. International Journal of Advanced Robotic Systems, 2013, 10(7): 1651-1656.
[11] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846-894.
[12] 孟庆宽，杨晓霞，张漫，等. 基于语义分割的非结构化田间道路场景识别[J]. 农业工程学报，2021，37(22)：152-160.
Meng Qingkuan, Yang Xiaoxia, Zhang Man, et al. Recognition of unstructured field road scene based on semantic segmentation model[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(22): 152-160. (in Chinese with English abstract)
[13] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.
[14] 张建英，赵志萍，刘暾. 基于人工势场法的机器人路径规划[J]. 哈尔滨工业大学学报，2006，38(8)：1306-1309.
Zhang Jianying, Zhao Zhiping, Liu Tun. A path planning method for mobile robot based on artificial potential field[J]. Journal of Harbin Institute of Technology, 2006, 38(8): 1306-1309.
[15] 杜爽，尚伟伟，刘坤，等. 基于双向RRT算法的仿人机器人抓取操作[J]. 中国科学技术大学学报，2016，46(1)：12-20.
Du Shuang, Shang Weiwei, Liu Kun, et al. Bidirectional RRT algorithm based grasping manipulation of humanoid robots[J]. Journal of University of Science and Technology of China, 2016, 46(1): 12-20. (in Chinese with English abstract)
[16] 刘永波，雷波，胡亮，等. 机器视觉在HSV颜色空间下稻瘟病病程分级判定研究[J]. 农学学报，2020，10(10)：83-90.
Liu Yongbo, Lei Bo, Hu Liang, et al. The grading determination of rice blast: HSV color space method based on machine vision[J]. Journal of Agriculture, 2020, 10(10): 83-90. (in Chinese with English abstract)
[17] 梁亚杰，杨丽丽，徐媛媛，等. 不确定场景下无人农机多机动态路径规划方法[J]. 农业工程学报，2021，37(21)：1-8.
Liang Yajie, Yang Lili, Xu Yuanyuan, et al. Dynamic path planning method for multiple unmanned agricultural machines in uncertain scenarios[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 1-8. (in Chinese with English abstract)
[18] Song X, Fan X, Cao Z, et al. A tc-rrt-based path planning algorithm for the nonholonomic mobile robots[C]//2017 36th Chinese Control Conference (CCC). DaLian: IEEE, 2017: 6638-6643.
[19] 魏武，韩进，李艳杰，等. 基于双树Quick-RRT算法的移动机器人路径规划[J]. 华南理工大学学报（自然科学版），2021，49(7): 51-58.
Wei Wu, Han Jin, Li Yanjie, et al. Path planning of mobile robots based on dual-tree Quick RRT* algorithm[J]. Journal of South China University of Technology (Natural Science Edition), 2021, 49(7): 51-58. (in Chinese with English abstract)
[20] 马小陆，梅宏. 基于改进势场蚁群算法的移动机器人全局路径规划[J]. 机械工程学报，2021，57(1)：19-27.
Ma Xiaolu, Mei Hong. Mobile robot global path planning based on improved ant colony system algorithm with potential field[J]. Journal of Mechanical Engineering, 2021, 57(1): 19-27. (in Chinese with English abstract)
[21] 刘天湖，刘伟，曾霆俊，等. 多柔性指滚筒菠萝采收机构工作原理及设计[J]. 农业工程学报，2022，38(8)：21-26.
Liu Tianhu, Liu Wei, Zeng Tingjun, et al. Working principle and design of the multi-flexible fingered roller pineapple harvesting mechanism[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(8): 21-26. (in Chinese with English abstract)
Navigation path planning of the pineapple harvester based on improved RRT*algorithm
Liu Tianhu, Zhang Di, Zheng Yan, Cheng Yifeng, Qiu Jian, Qi Long
(,,510642,)
All pineapples are harvested manually at present in China. But manual harvesting cannot fully meet the large-scale production against the ever-increasing greying workforce. Fortunately, automatic navigation can be expected to develop the pineapple harvester. In this study, a path planning algorithm was proposed as the navigation scheme to improve the mechanization and automation level of pineapple harvesting. An improved RRT* algorithm was also used for global path planning. Firstly, the self-heuristic idea was used to constrain the generation range of sampling points. Then, the bias probabilitypwas introduced to generate the random sampling points. Specifically, the sampling points were randomly generated with the probabilityin the space, when>p. Otherwise, the target point was used as the sampling point, in order to decrease the blindness of sampling point generation. Thirdly, the gravitational field of the artificial potential field, and the concept of direction weight were introduced in the new node expansion. The weightswandwwere assigned to the directions of the sampling and target point, respectively, where the direction was constrained in the expansion of the new node. Fourthly, the bidirectional expansion was used to speed up the iteration speed in the double-tree expansion. Finally, the greedy algorithm was applied to prune the redundant nodes of the path. The Cantmull-Rom interpolation function was also used to smooth the path corners. Three environments (including multiple obstacles, mazes, and narrow passages) were created to simulate the path planning process, in order to evaluate the performance among the improved navigation path planning, RRT* and bidirectional RRT* algorithm. Planning time, node number, and path length were selected as the indicators. Each algorithm experimented with 30 times in every single environment. The average, maximum, minimum, and standard deviation were calculated for the simulation data of the three indicators, respectively. The simulation results showed that the average planning time of the path planning algorithm of this paper in the three environments was 18%, and 46.12% higher than that of the RRT* and bidirectional RRT* algorithms, respectively, while the average programming speed was 5.7, and 2.3 times as rapid as that of the RRT*, and bidirectional RRT* algorithm, respectively. Furthermore, the average node number was 87.23% and 52.52% less than that of the RRT* and bidirectional RRT* algorithms, respectively. The average path length was 3.81% and 6.08% less than the RRT* and bidirectional RRT* algorithms, respectively. The field test showed that the planning time was only 14.12% and 20.34% of the RRT* and bidirectional RRT*, respectively. The iteration number was 80.89% and 69.70% less than that of the RRT* and bidirectional RRT*, respectively. In addition, the rotation angles larger than 60° on the path planned by RRT* and bidirectional RRT* algorithms were 1.56 and 2.06 times as much as that of the improved, respectively, and the rotation angles larger than 100° on the path were 1.55 and 2.18 times. The improved RRT* algorithm can fully meet the path navigation requirements of agricultural machinery in the field. The pineapple harvester can run along the planned path to the target point as moving with the speed of 0.2, 0.4, and 0.6 m/s, but the position and heading deviation increase with the moving speed. This finding can provide a sound reference for the navigation development in agricultural machines.
radar; algorithms; pineapple harvester; path planning; rapidly-exploring random tree
10.11975/j.issn.1002-6819.2022.23.003
S224.24
A
1002-6819(2022)-23-0020-09
刘天湖，张迪，郑琰，等. 基于改进RRT*算法的菠萝采收机导航路径规划[J]. 农业工程学报，2022，38(23)：20-28.doi：10.11975/j.issn.1002-6819.2022.23.003 http://www.tcsae.org
Liu Tianhu, Zhang Di, Zheng Yan, et al. Navigation path planning of the pineapple harvester based on improved RRT* algorithm[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(23): 20-28. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2022.23.003 http://www.tcsae.org
2022-06-24
2022-11-14
国家自然科学基金资助项目（52175229）；广东省农业科技创新十大主攻方向“揭榜挂帅”项目（2022SDZG03）
刘天湖，博士，副教授，研究方向为水果机械化采收，采摘机器人。Email：liuparalake@scau.edu.cn
