基于YOLOv5与IBVS的eye-in-hand视觉伺服抓取实战

简介：本资源面向机器人视觉伺服方向的开发者与研究生，提供一套基于YOLOv5目标识别、MoveIt动作规划与Gazebo仿真的eye-in-hand视觉伺服完整工程，用于解决机械臂在仿真环境中对目标进行实时识别、定位与抓取动作规划的问题，适合具备ROS基础、希望打通视觉与运动规划链路的中高级学习者。压缩包共105个文件，约8.02MB，包含20个launch启动文件、19个xml与14个yaml参数配置、13个stl模型、8个xacro机器人描述、6个py脚本及rviz、srv、srdf等，覆盖仿真环境搭建、机械臂建模、视觉节点与规划接口的完整目录结构。目前已有88人学习下载。读者可据此复现从相机图像输入到MoveIt路径生成的全流程，理解视觉反馈与动作规划的衔接方式，并参考其中的参数配置与节点组织，快速搭建自己的视觉伺服实验平台。

1. 从一条抓取指令到机械臂闭环：eye-in-hand 视觉伺服到底在解决什么

机械臂抓取这件事，很多人第一次做都会卡在同一个地方：相机看到目标了，坐标也解算出来了，可机械臂伸过去就是差那么几厘米。原因不复杂——相机固定在机架上（eye-to-hand）时，标定误差、机械臂运动学误差、目标位姿估计误差会层层叠加，最后全砸在末端执行器上。而 eye-in-hand 把相机装到机械臂末端，让相机跟着手一起动，用图像误差直接驱动机器人运动，这就是视觉伺服（Visual Servoing）的核心思路。再往下分，基于位置的视觉伺服（PBVS）要先估计三维位姿再规划，基于图像的视觉伺服（IBVS）直接拿图像特征误差算速度指令，后者对相机标定和深度误差的鲁棒性更好，也是这套方案选 Image-Based 的原因。

这套组合——YOLOv5 做目标识别、MoveIt 做动作规划、Gazebo 做仿真、eye-in-hand 做视觉伺服——本质上是把「感知-规划-控制」三段串成闭环。YOLOv5 负责在图像里框出目标，IBVS 负责把框的中心偏差转成末端速度，MoveIt 负责在接近阶段做无碰撞的关节空间规划，Gazebo 负责在没有真机的情况下把整条链路跑通。适合谁？适合手上有 ROS 环境、想验证视觉伺服算法但不想一上来就烧电机的研究生和机器人方向工程师。下面按「先搭仿真、再通感知、再接伺服、最后调参」的顺序拆开讲。

2. 在 Gazebo 里搭出 eye-in-hand 的仿真底盘

2.1 为什么选 Gazebo 而不是直接上真机

真机调试视觉伺服的成本极高：一次碰撞可能烧掉驱动器，标定误差和机械间隙还会让算法问题被硬件噪声掩盖。Gazebo 的价值在于它把物理引擎、传感器插件和 ROS 接口都封装好了，你可以反复重置场景、改相机内参、加噪声，而不用担心硬件损坏。常见做法是用 Panda 或 UR5 这类有成熟 URDF 的机械臂模型，在末端 link 上挂一个camera关节，Gazebo 会自动通过libgazebo_ros_camera.so插件发布/camera/image_raw和/camera/camera_info。注意 Gazebo 版本和 ROS 版本的对应关系：Ubuntu 22.04 上一般是 ROS 2 Humble 配 Gazebo Fortress 或 Classic 11，如果用的是 VMware 虚拟机，屏幕闪烁多半是 OpenGL 渲染后端问题，在~/.gazebo/gui.ini里加[rendering]段指定ogre或改用软件渲染能缓解，但这属于环境问题，不影响算法链路。

2.2 给机械臂挂上 eye-in-hand 相机的 URDF 片段

下面这段是往 Panda 末端加相机的典型写法，关键是joint的parent要指向panda_link8或panda_hand，origin的 xyz 决定相机光心相对末端的位置。

<!-- 在 panda_hand 之后追加，eye-in-hand 相机 --> <joint name="camera_joint" type="fixed"> <parent link="panda_hand"/> <child link="camera_link"/> <!-- 相机装在手掌前方 5cm，光轴朝下 --> <origin xyz="0.05 0 0.05" rpy="0 1.5708 0"/> </joint> <link name="camera_link"> <visual> <geometry><box size="0.02 0.05 0.05"/></geometry> </visual> </link> <gazebo reference="camera_link"> <sensor type="camera" name="eye_in_hand_cam"> <update_rate>30</update_rate> <camera name="head"> <horizontal_fov>1.047</horizontal_fov> <image><width>640</width><height>480</height><format>R8G8B8</format></image> <clip><near>0.02</near><far>10</far></clip> </camera> <plugin name="camera_controller" filename="libgazebo_ros_camera.so"> <ros><namespace>/camera</namespace></ros> <cameraName>eye_in_hand</cameraName> <imageTopicName>image_raw</imageTopicName> <cameraInfoTopicName>camera_info</cameraInfoTopicName> <frameName>camera_link</frameName> </plugin> </sensor> </gazebo>

逻辑说明：origin的rpy="0 1.5708 0"是把相机光轴从默认的 z 轴转到 x 轴方向，具体数值取决于你希望相机看向哪里，装反了图像会上下颠倒。horizontal_fov设 1.047 弧度约等于 60 度，和常见 USB 相机接近。update_rate30Hz 是视觉伺服的底线，低于 15Hz 时 IBVS 的速度指令会明显滞后。参数怎么改：如果目标物体很小，把width/height提到 1280x720，但要注意 Gazebo 渲染开销会翻倍；clip near不要小于 0.01，否则深度图会出现大量无效值。

2.3 用 launch 文件把仿真、MoveIt 和相机一起拉起来

启动顺序有讲究：先起 Gazebo 加载模型，再起robot_state_publisher和move_group，最后起相机话题的转发。常见做法是写一个顶层 launch，用include把panda_gazebo.launch和moveit_planning_execution.launch串起来。

# 终端 1：启动 Gazebo + Panda + 相机 roslaunch panda_sim eye_in_hand_gazebo.launch # 终端 2：启动 MoveIt roslaunch panda_moveit_config moveit_planning_execution.launch sim:=true # 终端 3：确认相机话题有数据 rostopic hz /camera/eye_in_hand/image_raw

如果rostopic hz显示频率为 0，先检查gazebo_ros_camera插件是否加载成功，用gzserver --verbose看有没有Failed to load plugin的报错。MoveIt 这边要确认sim:=true，否则它会去找真实控制器。这一步跑通后，/camera/eye_in_hand/image_raw应该能看到机械臂末端视角的画面，这是后面所有工作的前提。

3. YOLOv5 识别结果怎么喂给视觉伺服

3.1 从检测框到图像特征点：别直接把 bbox 中心当特征

YOLOv5 输出的是x1 y1 x2 y2 conf cls，很多人第一反应是取框中心(cx, cy)作为 IBVS 的特征点。这在目标形状规则、背景干净时能用，但一旦目标旋转或部分遮挡，框中心会跳变，速度指令跟着抖。更稳的做法是取框内特定角点或质心，比如抓取圆柱体时用框的底部中心，抓取方块时用四个角点的均值。YOLOv5 本身不输出关键点，所以要么在检测框内做颜色阈值分割求质心，要么换用 YOLOv5-pose 版本。我一般会在检测框内做一次 HSV 掩膜，取最大连通域的质心，这样即使框有轻微偏移，特征点也稳定。

3.2 把 YOLOv5 封装成 ROS 节点并发布特征点

下面是一个最小可用的 ROS 节点，订阅图像，跑 YOLOv5 推理，发布特征点。

#!/usr/bin/env python3 import rospy import cv2 import torch import numpy as np from sensor_msgs.msg import Image from geometry_msgs.msg import PointStamped from cv_bridge import CvBridge class YoloFeatureNode: def __init__(self): # 加载本地训练好的 yolov5 模型，注意用 conda 环境里的 torch self.model = torch.hub.load('ultralytics/yolov5', 'custom', path='weights/best.pt', force_reload=False) self.model.conf = 0.5 # 置信度阈值，低于 0.5 的框直接丢 self.model.iou = 0.45 # NMS 阈值，目标密集时调到 0.5 以上 self.bridge = CvBridge() self.pub = rospy.Publisher('/target/feature_point', PointStamped, queue_size=1) rospy.Subscriber('/camera/eye_in_hand/image_raw', Image, self.callback) def callback(self, msg): frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') results = self.model(frame) df = results.pandas().xyxy[0] if len(df) == 0: return # 取置信度最高的框 best = df.loc[df['confidence'].idxmax()] x1, y1, x2, y2 = int(best.xmin), int(best.ymin), int(best.xmax), int(best.ymax) roi = frame[y1:y2, x1:x2] # HSV 掩膜求质心，比直接用框中心稳 hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) mask = cv2.inRange(hsv, (0, 80, 80), (10, 255, 255)) M = cv2.moments(mask) if M['m00'] == 0: return cx = x1 + M['m10'] / M['m00'] cy = y1 + M['m01'] / M['m00'] pt = PointStamped() pt.header = msg.header pt.point.x, pt.point.y, pt.point.z = cx, cy, 0 self.pub.publish(pt) if __name__ == '__main__': rospy.init_node('yolo_feature') YoloFeatureNode() rospy.spin()

逻辑说明：torch.hub.load走的是本地缓存，第一次会下载仓库，之后离线可用。conf=0.5是经验值，太低会引入误检导致特征点乱跳，太高会漏检。HSV 范围(0,80,80)-(10,255,255)针对红色目标，换目标颜色要重调。发布PointStamped而不是自定义消息，是为了让后面的 IBVS 节点能直接用tf做坐标变换。参数怎么改：如果目标在图像里很小，把conf降到 0.3 并开augment=True；如果推理频率跟不上 30Hz，把输入尺寸从 640 降到 416，精度掉一点但速度能翻倍。

3.3 用 camera_info 把像素坐标转成归一化偏差

IBVS 的误差通常定义在归一化图像平面：e = [(u - u*) / fx, (v - v*) / fy]，其中u*, v*是期望位置（一般取图像中心），fx, fy从/camera/eye_in_hand/camera_info里读。这一步不做，速度指令的量纲就是错的，机械臂会猛冲。

from sensor_msgs.msg import CameraInfo cam_info = rospy.wait_for_message('/camera/eye_in_hand/camera_info', CameraInfo) fx, fy = cam_info.K[0], cam_info.K[4] cx_img, cy_img = cam_info.K[2], cam_info.K[5] # 期望特征点在图像中心 u_star, v_star = cx_img, cy_img # 当前特征点来自 YOLO 节点 u, v = pt.point.x, pt.point.y ex = (u - u_star) / fx ey = (v - v_star) / fy

K矩阵是 3x3 行优先，K[0]=fx，K[4]=fy，K[2]=cx，K[5]=cy。如果camera_info里的K全是 0，说明 Gazebo 相机插件没配好，回去检查cameraInfoTopicName。归一化后的ex, ey一般在 -0.5 到 0.5 之间，超过这个范围说明目标已经跑出视野，这时候不该继续发速度，而应该让 MoveIt 重新规划。

4. MoveIt 规划与 IBVS 速度指令的接力逻辑

4.1 接近阶段用 MoveIt，精调阶段切 IBVS

整套流程分两段：远距离时目标在图像里很小，YOLO 检测不稳定，这时候用 MoveIt 做关节空间规划，把末端送到目标上方一个预设的观察位姿；当目标在图像里占据足够像素（比如框面积超过 5000 像素）后，切换到 IBVS，用图像误差直接发/joint_group_vel_controller/command或笛卡尔速度指令。切换阈值不能拍脑袋，我一般用框面积和检测置信度双条件：面积 > 5000 且 conf > 0.7 才切，否则容易在目标刚出现时就切过去，导致速度指令震荡。

4.2 IBVS 控制律与交互矩阵的简化实现

标准 IBVS 控制律是v = -λ * L^+ * e，其中L是交互矩阵，λ是增益。对于单个点特征，L是 2x6 矩阵，简化实现可以只控制 x、y 平移和 z 旋转，把其余自由度锁死，这样交互矩阵退化成常数矩阵，调试难度大幅降低。

import numpy as np lam = 0.5 # 增益，太大震荡，太小收敛慢 # 简化交互矩阵：只控 x,y 平移和绕 z 旋转 L = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0]]) e = np.array([ex, ey]) v = -lam * np.linalg.pinv(L) @ e # v 是 6 维速度，只取前两维给笛卡尔速度控制器 cmd = np.zeros(6) cmd[0], cmd[1] = v[0], v[1]

逻辑说明：lam=0.5是保守值，实际调试从 0.1 开始往上加，直到出现轻微超调再回调。pinv是伪逆，因为L不是方阵。只控 x、y 意味着机械臂末端只在图像平面内平移，深度方向靠 MoveIt 的接近位姿保证。参数怎么改：如果目标在图像里移动很快，把lam提到 1.0 并加低通滤波；如果机械臂抖动明显，把lam降到 0.2 并检查相机帧率是否稳定。

4.3 用 tf 把相机坐标系速度转到基座坐标系

IBVS 算出的速度是在相机坐标系下的，必须转到机械臂基座坐标系才能发给控制器。用tf2_ros查camera_link到panda_link0的变换，把速度向量旋转过去。

import tf2_ros from tf2_geometry_msgs import do_transform_vector3 tf_buffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tf_buffer) trans = tf_buffer.lookup_transform('panda_link0', 'camera_link', rospy.Time(0)) # 把 cmd 前三维当向量旋转 vec = Vector3Stamped() vec.vector.x, vec.vector.y, vec.vector.z = cmd[0], cmd[1], 0 vec_trans = do_transform_vector3(vec, trans) cmd_base = [vec_trans.vector.x, vec_trans.vector.y, vec_trans.vector.z, 0, 0, 0]

lookup_transform的rospy.Time(0)表示取最新可用变换，不要用rospy.Time.now()，否则会因为时间戳不同步抛异常。如果报LookupException，检查robot_state_publisher是否在跑，以及 URDF 里camera_link是否在 tf 树里。这一步是 eye-in-hand 和 eye-to-hand 的关键区别：eye-to-hand 的相机坐标系固定，变换是常数；eye-in-hand 的相机随末端动，变换每帧都在变，所以必须实时查 tf。

5. 避坑与排查：这套链路最容易翻车的五个地方

5.1 现象：YOLO 检测框在仿真里正常，一切到 IBVS 就抖

原因：Gazebo 相机默认没有加噪声，图像过于干净，YOLO 输出的框边界像素级跳动，归一化后误差虽然小但高频。解决：在相机插件里加<noise>高斯噪声，或者在 IBVS 误差上做一阶低通滤波e_filt = 0.8 * e_filt + 0.2 * e，同时把lam降到 0.3 以下。

5.2 现象：MoveIt 规划成功但机械臂不动

原因：move_group发的轨迹发到了/panda_arm_controller/follow_joint_trajectory，但 Gazebo 里的控制器监听的是/panda_arm_controller/command，话题对不上。解决：检查ros_control的配置文件，确认joint_trajectory_controller的action_ns和 MoveIt 的moveit_controller_manager配置一致。常见做法是直接用panda_moveit_config自带的ros_controllers.yaml，不要自己改话题名。

5.3 现象：camera_info 的 K 矩阵全零

原因：Gazebo 相机插件里cameraInfoTopicName写错，或者image_width/height和camera_info里的分辨率不匹配。解决：用rostopic echo /camera/eye_in_hand/camera_info确认K有值，如果全零，把插件里的cameraName和frameName改成和 URDF 里camera_link一致，重启 Gazebo。

5.4 现象：IBVS 收敛到目标附近但始终有稳态误差

原因：交互矩阵用了简化版，忽略了深度 Z 的影响，当目标距离变化时L不再是常数。解决：要么在误差里除以深度估计值（从点云或已知目标尺寸反推），要么在接近目标后切换到 PBVS 做最后几厘米的精定位。我一般会在误差小于 5 像素时直接停发速度，用 MoveIt 做一次微小的笛卡尔直线运动收尾。

5.5 现象：仿真里跑通，换真机后 YOLO 帧率掉到 5Hz

原因：真机相机是 USB 2.0 接口，带宽不够，或者 YOLOv5 跑在 CPU 上。解决：把 YOLOv5 转 TensorRT 或 ONNX Runtime，输入尺寸降到 320，用半精度推理。如果还不行，把检测和伺服解耦：YOLO 每 5 帧跑一次，中间帧用光流跟踪特征点，这样伺服环能保持 30Hz。

6. 把增益调稳的一个笨办法：从 0.1 开始扫频

调 IBVS 增益这件事，玄学成分不少，但有个笨办法很管用：固定目标不动，把lam从 0.1 开始，每次加 0.1，记录末端从初始位置到收敛的时间，以及超调量。我一般会画一张表，横轴是lam，纵轴是收敛时间和超调，找那个收敛时间已经明显下降但超调还没起来的点。下面是我在 Panda 仿真里扫出来的一组参考值，目标距离 0.5m，相机 30Hz。

从表里看，0.3 到 0.5 之间是甜点区。但这不是万能值，目标距离变远时lam要适当加大，因为图像误差变小了；目标距离变近时要减小，否则容易冲过头。我的习惯是写一个自适应增益：lam = 0.3 + 0.2 * (1 - exp(-dist))，dist是目标在图像里的归一化距离，这样近距离时增益自动降下来。

验证收敛还有个技巧：不要只看最终误差，把ex, ey随时间的变化录成 rosbag，用rqt_plot画出来。如果曲线是单调下降的，说明增益合适；如果有明显振荡，先降增益再加低通；如果下降很慢但无振荡，说明增益太小或者交互矩阵不准。另外，Gazebo 的实时因子（RTF）如果低于 0.8，仿真时间比真实时间慢，这时候调出来的增益搬到真机上会偏大，因为真机的控制周期更短。我一般会在gazebo启动参数里加-u强制实时更新，并关掉不必要的渲染来保 RTF。

最后说个血泪教训：别在 IBVS 还没调稳的时候就去加抓取动作。抓取涉及夹爪闭合和力控，一旦伺服震荡，夹爪可能撞到目标把物体推飞，仿真里重置一下就行，真机上就是一次碰撞。我现在的习惯是先把伺服环单独跑通，用rqt_plot确认误差收敛到 5 像素以内并保持 2 秒，再接抓取。这套链路从搭仿真到调稳，我前后花了大概两周，其中一半时间耗在 tf 时间戳和相机内参上。如果你也在做类似的东西，建议先把camera_info和 tf 树确认无误，再动 YOLO 和 IBVS，能省掉很多后悔药。希望帮到你。

本文还有配套的精品资源，点击获取