基于可达性地图与逆可达性地图的果实采摘机器人系统设计研究报告
本研究由韩国汉阳大学机器人学院的Jae-Woong Han、Jae-Hoon Cho和Yong-Tae Kim（通讯作者）合作完成。相关研究成果以论文“Design of Fruit Harvesting Robot System Based on a Reachability and Inverse Reachability Map”为题，于2025年12月4日发表于开源学术期刊 Agriengineering (2025, Volume 7, Issue 12, Article 417)。
学术背景与研究目标 本研究属于农业机器人学与智能装备领域的交叉研究，重点关注自动化果实采摘这一关键课题。其研究背景源于全球范围内，特别是发达国家和地区面临的严峻农业劳动力短缺与老龄化问题。论文引用了联合国及韩国统计厅的数据，指出农村人口锐减、农业劳动力老龄化加剧，这严重威胁着农业系统的长期可持续性。为了应对这一挑战，将物联网、人工智能和机器人技术应用于农业的“智慧农业”成为必然趋势。其中，果实采摘作为劳动密集型作业，自动化需求尤为迫切。
然而，现有的采摘机器人研究在实际果园集成和用户友好性方面仍存在局限。一个核心挑战在于，操作者（或机器人自身）往往难以直观、量化地理解机械臂的工作空间及其运动学约束。传统方法依赖于用户经验或假设机械臂可达空间为简单的球形区域，而忽略了关节限制、连杆几何和奇异位形等复杂约束，这可能导致操作效率低下甚至机械损伤。此外，对于超出当前机械臂直接可达范围的果实，如何自主、高效地移动机器人基座以重新定位，也是实现完全自主采摘的关键问题。
为此，本研究旨在开发一个集成的自主果实采摘机器人系统。其核心创新在于系统性地利用可达性地图 (Reachability Map, RM) 与逆可达性地图 (Inverse Reachability Map, IRM) 来提升采摘效率。RM用于对机械臂工作空间进行量化评估，预先筛选出高可采性果实并优先采摘；IRM则用于为不可直接到达的果实计算最优的机器人基座位姿，引导移动平台重新定位以实现采摘。最终目标是通过结合视觉感知、运动规划和基于地图的优化决策，在复杂的果园环境中实现高效、自主的果实采摘作业。
详细研究流程与方法 整个研究流程可概括为以下几个核心环节：系统构建与仿真环境搭建、视觉感知与果实三维定位、可达性/逆可达性地图的构建与计算、基于地图的采摘决策与路径规划、以及ROS 2框架下的系统集成与实验验证。
第一环节：系统构建与仿真环境。 研究在虚拟果园环境中进行，使用ROS 2 Humble和Gazebo 11仿真平台。机器人系统是一个移动操作臂，由六自由度机械臂Indy7和移动底盘Husky组成，并配备了Intel RealSense D435i RGB-D相机、IMU、激光雷达和轮式编码器等传感器（均通过Gazebo插件实现）。这为算法开发和性能评估提供了一个可控且逼真的测试平台。
第二环节：视觉感知与果实三维定位。 这是机器人“看到”并确定果实空间位置的关键步骤。 1. 相机标定： 采用经典的Zhang氏方法对RGB-D相机进行标定，获取内参矩阵和畸变系数，平均重投影误差低于0.35像素，确保了从二维图像到三维世界坐标转换的亚毫米级精度。 2. 果实检测： 采用基于卷积神经网络的目标检测算法YOLOv5进行实时果实识别。YOLOv5在CSPDarknet53主干网络和PANet颈部结构的支持下，能够有效处理不同大小和光照条件下的果实。其损失函数结合了CIoU边界框回归损失、目标性损失和分类损失。 3. 三维坐标计算： 将YOLOv5检测到的果实二维边界框中心点与RGB-D相机提供的深度信息融合，通过逆投影模型将像素坐标转换到相机坐标系下的三维坐标。 4. 手眼标定与坐标转换： 由于相机以“眼在手”配置安装在机械臂末端，研究采用了Tsai和Lenz的算法进行手眼标定，求解出相机坐标系到末端执行器坐标系的变换矩阵。结合机械臂的正运动学（通过ROS 2 MoveIt 2计算），最终将果实的三维位置从相机坐标系转换到机器人基座坐标系，为后续的运动规划提供准确的输入。
第三环节：可达性地图与逆可达性地图的构建。 这是本研究的核心方法论创新。 1. 工作空间体素化： 首先将机械臂的理论工作空间离散化为细小的立方体单元（体素），分辨率为0.08米，以平衡计算精度与效率。 2. 姿态生成与可达性评估： 在每个体素中心，使用斐波那契螺旋采样法均匀生成50个不同的末端执行器朝向。对于每个生成的目标位姿，尝试求解逆运动学 (IK)。可达性指数 (Reachability Index, RI) 定义为该体素内成功求得IK解的姿态数量占总采样姿态数量的比例，量化了到达该空间点的概率。 3. 操作性指标计算： 除了可达性，研究还引入了衡量机械臂运动灵活性的指标。操作度各向同性 (Manipulability Isotropy, MI) 通过计算雅可比矩阵的奇异值比率得到，其值越接近1，表明机械臂在该位姿下各个方向的运动能力越均衡（即远离奇异位形）。 4. 构建复合可达性地图： 本研究没有仅依赖单一指标，而是提出了一个复合评估框架。除了RI和MI，还引入了一个可采性指数 (Harvestability Index, HI) ，表征在给定姿态下成功采摘的概率（在仿真中基于预设规则评估）。最终，将这三个指数按权重（HI: 0.5， MI: 0.3， RI: 0.2）综合，形成每个体素的复合可达性得分 (Composite Reachability Score, CRS) 。整个工作空间共生成约98万个姿态，其中约19.5万个具有有效IK解，最终生成的颜色编码RM（蓝色表示高可达性/可操作性）直观展示了机械臂的性能分布。 5. 构建逆可达性地图： IRM是RM的逆向表达。对于RM中每一个可达的末端执行器位姿，计算其相对于机器人基座的变换矩阵，然后对该变换矩阵求逆。这个逆变换描述的是：为了能让末端执行器达到该特定位姿，机器人基座可以（或必须）处于的位置和姿态。将所有这样的基座位姿集合起来，就构成了IRM。它本质上是一个庞大的数据库，记录了“末端目标位姿”与“可实现该目标的可行基座位姿”之间的对应关系。
第四环节：基于地图的采摘决策、基座优化与路径规划。 1. 可达果实的采摘规划： 系统首先获取所有检测到的果实的3D位置，并计算其相对于相机的距离进行排序（先近后远）。对于每个目标果实，会根据其位置计算机械臂末端的偏航角，并施加一个5厘米的接近偏移量，形成最终的采摘位姿。然后，使用KDL求解IK，并采用RRT-Connect算法进行无碰撞路径规划，选择关节空间位移最小的轨迹执行。 2. 不可达果实的基座优化： 对于RM判定为不可直接采摘的果实，系统启动IRM优化流程。首先，以该果实的采摘位姿为输入，查询IRM，得到一系列能使机械臂到达该位姿的候选基座位姿点云。接着，对这些候选位姿施加一系列物理约束滤波：基座高度（Z轴）变化限制在±0.05米内以保证稳定性；基座滚转和俯仰角接近0度（移动平台限制）；偏航角限制在±45度内以保持面向果实；机械臂基座与果实间沿X轴的最小距离设为0.4米以防止超载。 3. 最优基座位姿评分与选择： 对过滤后的候选基座位姿，通过逆变换验证其有效性，并与预先计算的RM进行匹配，获取该基座位姿下对应末端工作点的复合可达性得分。同时，计算一个距离得分（鼓励选择移动距离较短的位姿）。最终通过加权公式（距离得分权重0.3，复合可达性得分权重0.7）计算综合得分，选择得分最高的基座位姿作为最优目标。 4. 多果实聚类优化： 当有多个不可达果实时，为避免为每个果实单独移动机器人造成的低效，系统对它们的候选基座位姿进行聚类分析。使用K-means++算法将不同果实的候选基座位置分组，然后通过Delaunay三角剖分和密度分析识别出“重叠区域”——即一个基座位姿可以覆盖多个果实的区域。对于识别出的重叠区域（限制最多覆盖4个果实），同样通过上述评分机制选择一个最优的公共基座位姿，并使用主成分分析法从该区域所有姿态的旋转四元数中计算出一个代表性的基座朝向。这使得机器人通过一次移动就能高效采摘多个果实。
第五环节：系统集成与实验验证。 整个系统在ROS 2框架下实现模块化集成。核心功能如水果检测、导航、采摘规划等被设计为独立的ROS 2节点，通过服务 (Service) 和动作 (Action) 机制进行通信，支持异步任务执行和实时状态反馈。系统工作流为：自主导航到目标果树 -> 视觉检测与定位 -> 基于RM评估并采摘可达果实 -> 对剩余果实进行IRM基座优化与聚类 -> 移动机器人至新基座继续采摘 -> 循环直至完成。
主要研究结果 研究通过对比实验，系统地评估了所提出方法的性能。
1. 单一指标与基础系统的性能对比： 实验首先对比了仅使用基础系统（仅判断是否在理论工作空间内）、仅使用RI构建的RM、以及仅使用MI构建的RM的采摘性能。基础系统成功率为66%，但错误尝试率（尝试采摘但失败）高达24%。仅使用RI时，通过调整得分百分位阈值可以在成功率与错误率间权衡，例如在50%百分位时，成功率为50%，但错误率降至0%，表明RI能有效排除难以到达的果实，但可能过于保守。仅使用MI时，整体性能较差，在50%百分位时成功率仅18%，说明单纯考虑运动灵活性不足以指导有效的采摘决策。
2. 复合指数RM的性能评估： 为了克服单一指标的局限性，研究测试了多种权重组合的复合指数（HI:RI:MI）。实验结果表明，权重比为 5:3:2（即强调可采性，同时兼顾可达性与操作性）且在40%百分位阈值下，系统取得了最佳平衡：成功采摘次数为29/50（成功率58%），错误尝试率为0%，召回率 (Recall) 为87.9%，精确率 (Precision) 达100%，F1分数为93.5%。这证明复合指数能够更准确地识别出既“可到达”又“易操作且易采摘”的果实位置。
3. 结合IRM的完整系统性能： 最终，将基于5:3:2复合RM的采摘系统与IRM驱动的基座重定位功能结合，在包含6棵果树的模拟场景中进行端到端测试。实验结果如下： * 基础系统： 成功采摘33个果实（成功率66%），总耗时596.06秒。 * 仅使用复合RM的系统： 成功采摘29个果实（成功率58%），总耗时413.65秒。成功率降低是因为跳过了低得分果实，但效率更高。 * 复合RM + IRM的完整系统： 成功采摘45个果实（成功率90%），总耗时647.95秒。
结果显示，完整系统虽然总时间因包含移动时间而略有增加，但成功率得到了显著提升（从58%提升至90%） 。这证实了IRM在扩展机械臂有效工作范围、处理初始位置不可达果实方面的关键作用。系统首先高效采摘RM筛选出的高可采性果实，然后通过IRM计算最优移动策略去采摘剩余果实，从而实现了整体采摘覆盖率的极大提高。
研究结论与价值 本研究成功设计并验证了一个基于可达性地图与逆可达性地图的自主果实采摘机器人系统。主要结论如下： 1. 提出的复合可达性评估框架（综合HI, RI, MI）能够对机械臂的采摘可行性进行定量、系统的分析，有效区分高可采性与低可采性区域，指导机器人优先处理易采摘目标，减少无效尝试，提高作业效率。 2. 逆可达性地图为解决“果实超出当前机械臂范围”这一经典难题提供了有效方案。通过预计算的IRM数据库，系统能够快速为不可达果实计算出最优的机器人基座位姿，引导移动平台进行重新定位，从而显著扩展了单次部署的有效采摘范围。 3. 结合空间聚类算法，系统能够对多个不可达果实的采摘任务进行协同规划，将位置相近的果实分组，用一次移动完成多个果实的采摘，进一步优化了移动路径，减少了总体作业时间。 4. 在ROS 2框架下实现的模块化、分布式系统架构，证明了该方法在仿真环境中的可行性与有效性，为向实际果园环境部署奠定了技术基础。
该研究的科学价值在于，它将机器人运动学分析中的可达性、操作性概念与具体的农业采摘任务指标（可采性）深度融合，形成了一套面向任务的、量化的机器人工作空间分析与规划方法。其应用价值在于为解决农业自动化中的采摘覆盖率与效率问题提供了一个系统性的解决方案框架，特别是在非结构化、大范围的果园环境中，展现了通过感知-决策-执行闭环实现完全自主作业的潜力。
研究亮点 1. 方法论创新： 提出了融合“可采性-可达性-操作性”的复合指数来构建面向任务的可达性地图，超越了传统仅基于运动学可达性或操作度的单一评估方法。 2. 系统级解决方案： 不仅关注单点采摘，更通过逆可达性地图和基座位姿优化，解决了移动采摘机器人全局任务规划的核心问题，实现了从“静态采摘”到“动态覆盖”的跨越。 3. 工程实现完整： 研究包含从视觉感知、坐标转换、地图构建、运动规划到系统集成的完整技术链条，并在ROS 2/Gazebo仿真环境中进行了端到端的验证，流程清晰，结果可复现。 4. 效率优化策略： 引入了基于K-means和密度分析的多果实聚类方法，对移动采摘任务进行打包优化，体现了对整体作业效率的深度思考。
其他有价值内容 论文还对未来的研究方向进行了展望，包括：将系统应用于真实果园环境，应对果实随风晃动、复杂遮挡和光照变化等挑战；集成视觉伺服技术以补偿采摘过程中的微小果实运动；提升果实检测模型对外观相似或部分损伤果实的鲁棒性；以及将该框架推广至更多种类的水果采摘任务中。这些展望指出了从仿真验证走向实际应用所需攻克的关键技术难点。