AGEP-RRT*-Connect：面向路径质量的收获机械臂运动规划方法

《Smart Agricultural Technology》：AGEP-RRT*-Connect: A Path Quality Oriented Planning Method for Harvesting Manipulator Motion

　　

提出了一种AGEP-RRT^*-Connect（自适应步长、目标偏向、椭球与剪枝RRT^*-Connect）算法，以解决传统RRT^*-Connect在非结构化收获环境中进行收获机械臂路径规划时的盲目采样和路径冗余问题。该算法基于RRT^*-Connect框架，集成了四种互补策略，即目标偏向采样、椭球空间约束、自适应步长和贪婪剪枝，在采样、树扩展、重连和后处理阶段形成系统化的集成规划方案。研究人员在MATLAB中构建了低、中、高三种障碍物密度的收获场景，进行了120次蒙特卡洛试验，将AGEP-RRT^*-Connect与RRT^*、Informed RRT^*、RRT^*-Connect和AEB-RRT^*进行比较。在所有三种密度下，AGEP-RRT^*-Connect均实现了最短平均路径长度，分别比RRT^*-Connect的路径长度减少了4.50%、7.85%和11.09%。AGEP-RRT^*-Connect所需的规划时间比传统RRT^*-Connect更长，但在所有模拟场景中其平均规划时间均低于0.11秒，这对于静态收获路径规划是可接受的。此外，研究人员在Ubuntu ROS2平台上实现了物理系统，并进行了30次受控实验室规划试验。所提出方法获得的平均路径长度为344.90 ± 4.2 mm，相对于RRT^*-Connect和AEB-RRT^*分别减少了11.02%和2.77%。总体而言，AGEP-RRT^*-Connect在保持可接受计算可行性的同时，提高了路径质量和规划稳定性，并为集成视觉感知、基于ROS2的执行和伺服反馈的智能收获机械臂提供了路径规划组件。

**论文解读：AGEP-RRT^*-Connect：面向路径质量的收获机械臂运动规划方法**

**研究背景与问题**
果蔬采摘是农业生产中劳动强度最大、耗时最长的环节之一，目前仍主要依赖人工操作。手动采摘具有高体力消耗、低操作效率的特点，且主观性强易损伤采摘目标，降低整体生产效益。因此，收获机械臂作为替代人工的关键设备，需支持大规模种植。在真实收获环境中，障碍物分布不规则，自由空间狭窄，预建模难以准确表征环境；同时，收获机械臂受关节运动限制和约束工作空间影响，路径规划需满足自动避碰、短路径长度、快速响应、兼容狭窄通道和关节限位等多重约束。现有路径规划方法中，图搜索法和人工势场法在高维空间中节点数快速增长，依赖精细离散化和精确先验建模，不适用于不规则障碍物场景；智能优化算法虽具备全局搜索能力，但在高维设置下需要大量迭代，计算成本高且对参数敏感。相比之下，基于采样的方法（如RRT及最优变体RRT^*）具有概率完备性，可扩展至高维空间，成为机械臂路径规划的重要技术。RRT能快速生成可行路径，但路径质量和节点利用率有限；RRT^*通过父节点重选和重连实现渐近最优性，但计算开销大，在高维空间或狭窄通道中收敛速度显著降低。针对RRT系列算法，已有研究通过启发式采样、自适应步长和局部平滑策略提升采样效率、收敛速度和路径质量。RRT^*-Connect结合双向树扩展机制与渐近最优性，在自由度高的机械臂场景中平衡了搜索效率与路径质量。Informed RRT^*在获得首个可行解后，将后续采样限制在当前最优解成本定义的椭球子集内，提高有效样本比例并加速收敛。然而，在密集障碍物温室环境中，邻域半径和扩展步长等参数需仔细调整以平衡优化性能与实时性。现有研究引入了动态目标偏向、混合双向搜索、自适应扩展和局部重连等技术，以增强全局引导、提高狭窄通道可行性并降低路径成本。AEB-RRT^*作为自适应扩展双向RRT^*算法，根据碰撞检测失败调整采样概率和扩展模式，提升搜索鲁棒性和路径质量。在农业收获场景中，RRT及其变体已被用于茶果园、修剪采摘等任务，但现有研究尚未充分评估不同策略在RRT^*-Connect框架下的组合效应，尤其是针对不同障碍物密度条件下的收获机械臂运动规划。

**研究内容与结论**
本研究基于RRT^*-Connect的双向树扩展和渐近优化，系统集成了目标偏向采样、椭球空间约束、自适应步长和贪婪剪枝四种策略，并评估其组合效应用于收获机械臂运动规划。该集成方法旨在适应非结构化收获场景，确保无碰撞运动、减少路径冗余，在保持计算可行性的同时提高路径质量。通过仿真研究和受控物理实验，验证了所提出算法在不同障碍物密度和实验室收获条件下的有效性和适用性。该工作为智能收获机械臂提供了路径规划组件，将基于采样的规划与视觉感知输入、基于ROS2的执行和伺服反馈相连接。论文发表在《Smart Agricultural Technology》。

**关键技术与方法**
（不超过250字）研究人员采用的主要关键技术方法包括：1）基于RRT^*-Connect框架的双向树扩展和渐近最优性；2）目标偏向采样策略，通过设定偏向概率P_goal，在全局探索与快速收敛间平衡；3）椭球空间约束，参考Informed RRT^*框架，将采样和重连限制在以起点和终点为焦点的椭球内，缩小搜索空间；4）自适应步长机制，基于距离截断调整扩展步长，提高探索效率同时保持碰撞安全；5）贪婪剪枝后处理，在可行路径上逐节点检查视线可行性，移除冗余节点。仿真实验在MATLAB中进行，构建了低、中、高三种障碍物密度的三维地图，并进行了120次蒙特卡洛试验。物理实验在Ubuntu ROS2平台上进行，使用RoArm-M2-S四自由度机械臂，通过立体相机获取目标点深度信息，并利用SGBM立体匹配算法生成视差图。实验样本为樱桃番茄簇，构建了简化番茄植株模型和泡沫聚乙烯块障碍物，固定起点和终点，进行30次重复规划试验。

**研究结果**
（保留每个小标题，简要说明通过什么研究得出什么结论）

**3.2.2. 三种密度障碍物地图中的测试**
在MATLAB中对三种密度地图进行120次重复试验，对比RRT^*、Informed RRT^*、RRT^*-Connect、AEB-RRT^*和AGEP-RRT^*-Connect。结果显示，AGEP-RRT^*-Connect在所有密度下均获得最短平均路径长度，分别比RRT^*-Connect减少4.50%、7.85%和11.09%，且路径长度随障碍物密度增加而增长最少，表明对密度敏感度低、路径质量稳定。其平均规划时间约0.10秒，虽高于RRT^*-Connect但远低于RRT^*和Informed RRT^*，在静态收获规划中可接受。节点数方面，AGEP-RRT^*-Connect节点数约为RRT^*的20%左右，且随密度增加而减少，表明冗余搜索有效抑制。

**3.2.4. 消融研究**
在密集场景下进行消融实验，基线为RRT^*-Connect（B0）。单独添加贪婪剪枝（B1）使路径长度从226.01 mm降至214.40 mm；添加目标偏向采样（A1）和自适应步长（A2）后路径长度分别为224.16 mm和222.42 mm，改善有限；引入椭球约束（A3）后路径长度显著降至206.66 mm，方差减小，但规划时间升至0.0781 s；完整方法A4（AGEP-RRT^*-Connect）进一步加入贪婪剪枝，路径长度降至200.95 mm，规划时间0.0991 s。结果表明椭球约束对路径质量贡献最大，贪婪剪枝进一步压缩冗余，组合后实现更短、更稳定的路径。

**3.3. ROS2平台仿真实验**
在Ubuntu ROS2平台下，利用MoveIt2和OMPL规划库，在Rviz2中构建三维仿真环境，设置起点（200 mm, -150 mm, 150 mm）和终点（200 mm, 300 mm, 150 mm），放置一个立方体和四个圆柱形障碍物。30次重复试验均成功生成无碰撞路径，验证了所提出算法在ROS2框架中的集成可行性。

**3.4. 物理实验与结果分析**
在实验室条件下，使用RoArm-M2-S机械臂，以樱桃番茄簇为收获目标，构建固定静态障碍物场景（泡沫聚乙烯块和番茄植株模型）。通过立体相机获取视差图，手动选择目标果实中心（重复10次分析，三维空间平均误差1.36 mm）。固定起点（0, -50, 0）和终点（160, -60, 270），30次重复试验。AGEP-RRT^*-Connect平均路径长度344.90 ± 4.2 mm，比RRT^*-Connect减少11.02%，比AEB-RRT^*减少2.77%，且标准差更小，表明路径质量和稳定性提升。规划时间0.0648 s，节点数268.5，均低于RRT^*和Informed RRT^*。执行过程中无碰撞，验证了基本操作可行性。

**讨论与结论**
讨论部分指出，AGEP-RRT^*-Connect在三种障碍物密度下均获得最短路径，且路径长度减少比例随密度增加而增大，表明在密集地图中优势更显著。自适应步长和贪婪剪枝分别减少碰撞和冗余节点，椭球约束贡献最大。计算开销虽高于RRT^*-Connect，但路径质量改善显著，且规划时间保持在0.10秒左右，可接受。物理实验验证了算法与立体视觉、ROS2执行和伺服反馈的集成可行性。研究结论（翻译原文）：结合三维仿真和受控静态物理实验的结果，AGEP-RRT^*-Connect能够在具有狭窄通道和密集障碍物的收获场景中稳定生成高质量、可执行且无碰撞的机械臂轨迹。该算法可嵌入收获机械臂的路径规划模块，适用于有限工作空间的温室栽培模式，如密植种植，其中减少冗余机械臂运动和提高路径稳定性至关重要。从智能农业系统开发的角度，本研究提供了规划模块的受控验证，该模块将基于采样的路径规划、立体视觉输入、基于ROS2的执行和伺服反馈相连接。当前的物理实验在实验室条件下进行，在农场规模部署之前需要在温室生产环境中进一步验证。未来工作将研究轻量级变体，以减少椭球约束和贪婪剪枝引入的额外计算，例如根据障碍物密度自适应激活椭球约束，以及通过重用碰撞检测结果加速剪枝阶段。