面向苹果采摘机器人的渐进式混合自动切换视觉伺服（HAVS）方法：提升精度、鲁棒性与采收效率

这篇研究针对苹果采摘机器人中视觉伺服（Visual Servoing, VS）控制面临的末端执行器位姿精度与鲁棒性平衡难题，提出了一种渐进式混合自动切换视觉伺服（Hybrid Automatic Switching Visual Servoing, HAVS）方法。该方法整合了基于位置的视觉伺服（Position-Based Visual Servoing, PBVS）和基于图像的视觉伺服（Image-Based Visual Servoing, IBVS），通过设计的基于深度的切换函数实现优势互补，并引入了带模糊增益调度的自适应比例-微分（Proportional-Derivative, PD）控制器在线更新控制增益，有效降低了目标丢失与控制奇异性的风险。硬件实验验证了该方法在室内外采摘场景下的高成功率与低损伤率，为农业采摘机器人的视觉伺服控制提供了高效参考方案。

　　

1. 引言

苹果是全球消费最广泛的水果之一，与健康益处相关，如降低癌症和心血管疾病风险。随着果园种植面积扩大，劳动力成本上升和恶劣天气下的采收工作成为主要制约。人工智能与自动控制的进步加速了机器人苹果采摘机作为人工采摘替代方案的采用。近年来，机械化苹果采摘取得了实质性进展，其中视觉系统为采摘机器人的环境感知和目标定位提供了关键信息。然而，在复杂的非结构化果园环境中，如何快速精确地定位果实并引导机械臂可靠采收，仍然是视觉伺服控制面临的关键挑战。

视觉伺服的概念最早由 Hill 等人提出，与常规机器人控制不同，它形成了一个闭环系统，使用视觉特征作为实时运动控制的反馈。Hutchinson 等人进一步将其形式化，分为基于位置的视觉伺服（PBVS）和基于图像的视觉伺服（IBVS）。PBVS 将图像测量转换为三维目标位姿进行控制，强调精确的定位和方向控制，但对外部校准和目标建模误差敏感，且运动中易丢失目标。IBVS 直接在二维图像平面上定义和控制误差，通常对校准误差更鲁棒，并能保持目标在视野内，但可能导致复杂的三维轨迹并出现图像奇异性。为克服PBVS和IBVS各自的局限性，研究者们提出了混合视觉伺服方法，但现有方法的稳定性、可靠性以及在真实采收环境中的有效性仍需进一步验证。

为此，本研究提出了一种针对非结构化农业环境量身定制的渐进式混合自动切换视觉伺服（HAVS）方法，其主要贡献包括：（1）开发了一种HAVS方法，在粗对准阶段采用混合IBVS-PBVS控制以实现快速接近目标并确保目标保留在视野内，当目标深度低于最优阈值时，则切换至PBVS进行精对准。（2）提出了一种带模糊增益调度的自适应PD控制器，在线更新控制增益以提高响应速度和动态稳定性。（3）构建了一个苹果采摘机器人系统，并通过室内模拟和田间采摘实验验证了其整体性能。

2. 苹果采摘机器人系统设计

2.1. 苹果采摘机器人的硬件和软件设计

该苹果采摘机器人系统硬件包括一台uFactory xArm6六自由度（6-DoF）协作机械臂、一个Intel RealSense D435i深度相机、一个电动三指柔性夹爪、一个控制终端和一个移动平台。末端执行器采用电动三指柔性夹爪，支持顺应性抓取和失速保护，有助于减少采摘过程中的苹果损伤。为提高目标定位精度，系统采用了手眼（eye-in-hand）配置，将深度相机安装在末端执行器上随机械臂移动。

软件栈构建于Ubuntu 22.04和机器人操作系统（ROS）之上，使用C++开发，并利用LibTorch 1.13.0（PyTorch C++ API）进行GPU加速的苹果检测与定位推理。图像处理使用深度相机SDK和OpenCV 4.8，运动规划和视觉伺服控制则通过xArm SDK实现。

2.2. 苹果采摘机器人的工作流程

该机器人工作流程包括目标检测、基于视觉的伺服控制和采摘动作三个阶段。首先，RGB-D相机捕获目标的同步彩色和深度图像，预处理后由目标检测网络识别目标果实。然后计算目标中心在基坐标系（3D）和图像平面（2D）中的位置。采用带模糊增益调度的自适应PD控制器，PBVS驱动机械臂朝向目标苹果，IBVS调节末端执行器姿态以保持目标在相机光轴上。当目标深度低于预设阈值时，控制器切换至PBVS进行最终精确定位。达到期望位姿后，夹爪闭合，机械臂缩回并释放果实以完成采摘操作。

3. HAVS控制方法

所提出的渐进式混合自动切换视觉伺服（HAVS）方法采用手眼视觉配置，支持目标苹果的实时检测、三维定位和机械臂的视觉伺服。软件采用多线程架构，视觉、控制和显示线程并发运行，以提高系统稳定性并满足实时性约束。

3.1. 目标检测

考虑到果园环境高度非结构化、果实密集且遮挡严重，本研究选择了YOLOv5进行目标检测。相较于YOLOv8等无锚框检测器，YOLOv5的基于锚框方法提供了更鲁棒的几何先验，能更有效地分离密集簇中的相邻果实。考虑到实时性要求，选择了最小的YOLOv5s模型。使用MinneApple数据集进行训练和评估，并采用在COCO数据集上获得的预训练权重进行迁移学习。训练采用标准YOLOv5数据增强技术，在配备NVIDIA GeForce RTX 4060 GPU的笔记本电脑上进行。典型检测结果显示了模型在果园场景中的有效性。

3.2. 基本模块与控制律设计

3.2.1. IBVS控制模块

IBVS在二维图像平面上定义控制误差。本研究使用检测到的苹果中心作为图像特征，将图像中心设为期望特征位置，IBVS旨在将苹果中心与图像中心对齐。图像分辨率为640 × 480，图像中心位于像素(320, 240)。IBVS误差定义为苹果中心与图像中心之间的像素偏移：e_IBVS= [(u-u_c), (v-v_c)]^T。指定期望深度d₀（150 mm）表示苹果接近夹爪，深度误差定义为期望深度与测量深度之差：e_d= d - d₀。比例控制将像素误差和深度误差映射到末端执行器姿态的修正命令。经过经验调参，比例增益K_pα和K_pβ分别设为-0.1和0.1：Δα = K_pα·(u-u_c), Δβ = K_pβ·(v-v_c)。其中，Δα和Δβ表示末端执行器滚转角和俯仰角的控制增量。对于IBVS基线，利用深度误差计算的控制命令驱动机械臂朝向目标：Δz = K_p2e_d。

3.2.2. PBVS控制模块

PBVS将图像观测和深度测量映射到三维空间中的目标位置，并在此空间域中进行控制，目标是通过协调夹爪工作点与目标苹果中心之间的位置误差，驱动末端执行器运动。这需要协调相机、机械臂和目标之间的坐标变换。通过YOLOv5s从RGB图像估计目标苹果中心像素坐标(u, v)，检测结果与深度图对齐，并读取(u, v)处的深度值。使用移动平均滤波器平滑深度值以抑制光照变化和传感器噪声引起的波动。深度图测量的是苹果表面到相机的距离，并非苹果几何中心的深度。将苹果近似为球体，其等效图像半径r从YOLOv5s边界框估计得到，通过将r加到测量深度d上来校正中心深度。利用深度相机内参焦距(f_x, f_y)和主点坐标(c_x, c_y)，通过针孔相机模型将(u, v)和校正后的深度反投影，得到相机坐标系中的三维目标中心：(X_c, Y_c, Z_c)^T，计算公式为：X_c= (u - c_x)·d / f_x, Y_c= (v - c_y)·d / f_y, Z_c= d + r。通过手眼标定，将目标坐标从相机坐标系转换到机械臂基坐标系。PBVS控制误差定义为机械臂末端期望位置与当前位置在基坐标系下的差值。采用比例控制律生成末端执行器的线速度和角速度命令，以最小化该位置误差。

3.2.3. 混合自动切换机制与自适应控制

所提出的HAVS方法的核心是其混合自动切换机制。在初始粗对准阶段，系统同时运行IBVS和PBVS。IBVS负责调整末端执行器的姿态（滚转角α和俯仰角β），确保目标苹果始终位于图像中心附近，从而将其保持在视野内。PBVS则负责驱动末端执行器沿着相机光轴方向（Z轴）接近目标。这种并行的混合控制策略结合了IBVS在保持目标可视性方面的鲁棒性和PBVS在深度方向上的直接控制能力。切换函数基于实时测量的目标深度d设计。设定一个最优深度阈值d_th。当d > d_th时，控制器保持混合IBVS-PBVS模式进行粗对准。一旦检测到d ≤ d_th，表明末端执行器已足够接近目标，控制器将自动切换至纯PBVS模式，利用其高精度的三维位姿控制能力进行最终的精确定位和对准，以完成抓取。

为了进一步提升系统的动态性能和稳定性，研究提出了一种带模糊增益调度的自适应PD控制器。该控制器根据当前图像误差e_IBVS和深度误差e_d的大小，通过模糊逻辑规则在线动态调整比例增益K_p和微分增益K_d。当误差较大时，增大增益以提高响应速度；当误差较小时，减小增益以抑制超调和振荡，从而在快速性和稳定性之间取得平衡。

4. 实验与结果分析

为评估所提HAVS方法的性能，研究进行了室内模拟实验和户外田间实验，并与纯PBVS、纯IBVS方法进行了对比。

在室内模拟实验中，设置了包含悬挂苹果的模拟果树环境。实验指标包括采摘成功率、平均采摘时间和目标丢失率。结果表明，HAVS方法的采摘成功率达到96%，分别比纯PBVS和纯IBVS高出4和10个百分点。平均采摘时间为12.5秒，比纯PBVS和纯IBVS分别缩短0.3秒和1.1秒。同时，HAVS方法有效降低了目标丢失和控制奇异性的发生。

在户外田间实验中，在真实果园环境下对机器人系统进行了测试。实验面临自然光照变化、枝叶遮挡和背景复杂等挑战。HAVS方法取得了87.5%的采摘成功率，平均采摘时间为13.2秒，果实损伤率为4.2%。这些结果证明了该方法在非结构化真实环境中的有效性和鲁棒性。与室内结果相比，性能略有下降，这主要归因于户外更复杂的光照条件和环境干扰，但HAVS方法依然显著优于传统的单一伺服方法。

5. 结论

本研究针对苹果采摘机器人，提出并验证了一种渐进式混合自动切换视觉伺服（HAVS）方法。该方法创造性地在粗对准阶段融合IBVS和PBVS，通过基于深度的自动切换机制，在保证目标不丢失的前提下，实现了快速接近与精确定位的无缝衔接。引入的带模糊增益调度的自适应PD控制器进一步增强了系统的动态响应和稳定性。构建的完整机器人系统及详实的实验表明，HAVS方法在提升采摘成功率、缩短作业时间、降低果实损伤方面具有显著优势，为解决非结构化农业环境中采摘机器人的视觉伺服控制难题提供了切实可行的解决方案，具有重要的理论参考和工程应用价值。