基于YOLOv5与MoveIt的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人学习下载。读者可从中获得YOLOv5与MoveIt联调的工程组织方式、相机与机械臂末端标定的实现思路、Gazebo中传感器数据模拟与动作规划验证的完整流程以及多模块协同的目录结构参考便于快速复现并在此基础上扩展自己的视觉伺服方案。1. 从一块 GPU 到一条会看的机械臂这套 eye-in-hand 视觉伺服到底能跑出什么很多人第一次接触视觉伺服都是被“eye-in-hand”这个词唬住的——相机装在机械臂末端一边动一边看看起来像给机器人装了一双会跟着手走眼睛。但真到自己搭的时候问题立刻变成YOLOv5 检测出来的框怎么变成机械臂能执行的速度指令MoveIt 规划出来的轨迹和 Gazebo 里的仿真为什么对不上这套基于 YOLOv5 目标识别、MoveIt 动作规划、Gazebo 仿真的 eye-in-hand 视觉伺服方案解决的正是从“看到目标”到“手伸过去”之间那条最容易断的链路。它适合已经跑通过 YOLOv5 推理、装过 ROS 环境、想让机械臂在仿真里先验证视觉闭环的从业者。你不需要真机但需要理解相机坐标系、末端执行器坐标系和图像像素之间的换算关系否则后面调参时会一直卡在“框在动但手不动”的玄学状态。2. 拆开这套视觉伺服从像素坐标到关节轨迹的完整链路2.1 为什么选基于图像的视觉伺服而不是基于位置的方案视觉伺服分两大流派基于位置PBVS和基于图像IBVS。这套方案选的是 IBVS核心原因在于它绕开了对目标三维位姿的显式估计。PBVS 需要你先用深度相机或点云把目标在相机坐标系下的三维位置算出来再送到 MoveIt 做规划中间任何一步标定误差都会直接放大成末端定位偏差。IBVS 则直接把图像特征误差映射成相机速度误差定义在像素层面对相机标定误差的敏感度低得多。但 IBVS 也有代价。它需要你定义图像特征常见做法是取 YOLOv5 检测框的中心点作为特征点再结合框的宽高变化估计深度趋势。这套方案里YOLOv5 负责提供稳定的二维框MoveIt 负责把相机速度指令转成机械臂关节轨迹Gazebo 负责在物理仿真里验证这套闭环能不能收敛。选 YOLOv5 而不是更重的检测器是因为视觉伺服的伺服频率通常要求 10Hz 以上YOLOv5s 在普通 GPU 上能跑到 30FPS 以上留足了控制余量。2.2 环境搭建ROS、Gazebo 与 YOLOv5 的版本咬合这套方案对版本比较敏感尤其是 ROS 和 Gazebo 的搭配。常见做法是在 Ubuntu 20.04 上装 ROS NoeticGazebo 用自带的 Gazebo 11。如果你在 Ubuntu 22.04 上装 ROS 2 HumbleGazebo 会换成 Ignition Fortress很多老教程里的gazebo_ros插件包名和参数都会变这是第一个容易翻车的地方。YOLOv5 这边建议单独建 conda 环境不要和 ROS 的 Python 环境混在一起。ROS Noetic 默认用 Python 3.8YOLOv5 的依赖里 torch 版本对 Python 小版本有要求混装很容易出现cv_bridge导入失败或者 numpy 版本冲突。我一般会这样做# 创建独立的 YOLOv5 环境Python 版本对齐 ROS Noetic conda create -n yolov5_ibvs python3.8 -y conda activate yolov5_ibvs # 安装 PyTorch注意 CUDA 版本要和驱动匹配 pip install torch1.13.1cu117 torchvision0.14.1cu117 --extra-index-url https://download.pytorch.org/whl/cu117 # 克隆 YOLOv5 源码建议锁定到 v7.0 分支接口稳定 git clone -b v7.0 https://github.com/ultralytics/yolov5.git cd yolov5 pip install -r requirements.txt这里锁 v7.0 是因为 v6 之前的版本输出格式和 v7 有差异而 v8 之后 API 改动较大很多视觉伺服示例代码还是按 v7 的results.pandas().xyxy[0]写的。装完之后用python detect.py --source data/images/bus.jpg跑一张图确认检测框能正常输出再往下走。ROS 这边需要额外装cv_bridge和vision_msgs如果你要用 MoveIt 的 Python 接口还要装moveit_commandersudo apt install ros-noetic-cv-bridge ros-noetic-vision-msgs ros-noetic-moveit-commanderGazebo 仿真环境里机械臂模型建议直接用 MoveIt 配置助手生成的 URDF再补上相机插件。相机插件用libgazebo_ros_camera.so挂在末端 link 上这样 Gazebo 发布的/camera/image_raw就是 eye-in-hand 视角。注意相机插件的update_rate不要设太高30Hz 足够设到 100Hz 反而会让 Gazebo 实时因子掉下来仿真变慢。2.3 YOLOv5 检测结果到图像特征的转换YOLOv5 输出的是框的左上角和右下角像素坐标IBVS 需要的是特征点及其变化率。这套方案里特征点取框中心(u, v)另外取框宽w和框高h作为辅助特征。为什么要加宽高因为单靠中心点当相机正对目标时中心点误差接近零但深度方向可能还没到位机械臂会提前停下。加上宽高变化相当于给了一个深度收敛的间接约束。转换代码不复杂但有几个参数要留意import numpy as np def yolo_to_features(detection, img_shape): detection: [x1, y1, x2, y2, conf, cls] img_shape: (height, width) 返回归一化特征 [u, v, w, h] x1, y1, x2, y2 detection[:4] img_h, img_w img_shape[:2] # 中心点归一化到 [-1, 1]这是 IBVS 常用的归一化方式 u (x1 x2) / 2.0 / img_w * 2 - 1 v (y1 y2) / 2.0 / img_h * 2 - 1 # 宽高归一化避免不同分辨率下增益不一致 w (x2 - x1) / img_w h (y2 - y1) / img_h return np.array([u, v, w, h])归一化到[-1, 1]而不是[0, 1]是因为交互矩阵interaction matrix的推导默认图像坐标以光心为原点。如果你直接用像素坐标交互矩阵里的焦距参数会变得很大增益调起来非常别扭。宽高用[0, 1]归一化就够了它们只参与深度方向的误差计算。检测框的置信度阈值建议设 0.5 以上低于这个值的框中心抖动会很明显伺服过程中机械臂会跟着抖。如果目标类别多还要加一个类别过滤只保留你要抓的那个类。YOLOv5 的--classes参数可以在推理时直接过滤比后处理再筛更省算力。2.4 MoveIt 接收速度指令并生成关节轨迹IBVS 控制器输出的是相机速度[vx, vy, vz, wx, wy, wz]但 MoveIt 的move_group接口通常接收的是末端位姿目标或者关节空间目标。这里有两种常见做法一种是把相机速度积分成位姿增量每隔几个控制周期发一次set_pose_target另一种是用moveit_servo包直接把笛卡尔速度指令转成关节速度。这套方案用的是第一种因为实现简单和 Gazebo 的仿真步长容易对齐。控制周期取 0.1s每 5 个周期发一次位姿目标相当于 2Hz 的 MoveIt 规划频率。听起来慢但 IBVS 的收敛主要靠速度积分MoveIt 只负责把积分后的位姿变成可行的关节轨迹2Hz 足够。import rospy from geometry_msgs.msg import PoseStamped from moveit_commander import MoveGroupCommander class IBVSController: def __init__(self, group_namepanda_arm): self.group MoveGroupCommander(group_name) self.group.set_max_velocity_scaling_factor(0.3) # 仿真里慢一点方便观察 self.group.set_planning_time(0.5) self.pose_target PoseStamped() self.pose_target.header.frame_id panda_link0 def step(self, cam_velocity, dt0.1): # 把相机速度积分成末端位姿增量 # 注意这里假设相机坐标系和末端坐标系对齐实际要乘手眼矩阵 self.pose_target.pose.position.x cam_velocity[0] * dt self.pose_target.pose.position.y cam_velocity[1] * dt self.pose_target.pose.position.z cam_velocity[2] * dt # 姿态增量用四元数乘法这里简化成欧拉角累加 # 实际项目里建议用 tf.transformations.quaternion_multiply self.group.set_pose_target(self.pose_target) plan self.group.plan() if plan[0]: self.group.execute(plan[1], waitFalse)set_max_velocity_scaling_factor(0.3)是血泪经验。仿真里如果按 1.0 跑机械臂动作太快相机图像模糊YOLOv5 检测框会跳伺服直接发散。降到 0.3 之后图像稳定框也稳收敛虽然慢一点但可靠。set_planning_time(0.5)是限制规划时间避免 MoveIt 在复杂场景里卡太久影响控制周期。手眼矩阵这块要特别注意。eye-in-hand 构型下相机固定在末端相机坐标系到末端坐标系的变换是常数但需要你提前标定或者从 URDF 里读。如果 URDF 里相机 link 是直接挂在末端 link 下的那这个变换就是固定的用tf2_ros查一次存下来就行。如果 URDF 里没写那就得用棋盘格标定标定误差会直接体现在伺服精度上。3. Gazebo 仿真里的闭环调试从能跑到跑得稳3.1 仿真步长、控制周期与图像延迟的对齐Gazebo 默认的物理步长是 1ms但相机插件发布图像的频率通常设 30HzYOLOv5 推理又要花 20-30ms整个感知链路下来从图像采集到速度指令输出延迟大概在 50-80ms。如果控制周期设 0.1s这个延迟还在可接受范围如果设 0.05s延迟占比就太大了系统容易振荡。我一般会在 Gazebo 的 world 文件里把max_step_size设成 0.001real_time_update_rate设成 1000保证物理仿真精度。相机插件的update_rate设 30YOLOv5 这边用 TensorRT 或者 ONNX Runtime 加速把推理时间压到 15ms 以内。控制节点用rospy.Rate(10)跑 10Hz和 MoveIt 的规划频率对齐。调试的时候先别开 IBVS 闭环手动发几个位姿目标看 Gazebo 里机械臂动得对不对相机图像跟不跟手。这一步过了再开 YOLOv5 检测看框稳不稳。最后才把速度指令接进去从小增益开始调。3.2 交互矩阵与增益调参为什么你的机械臂在目标附近来回晃IBVS 的核心是交互矩阵它描述图像特征变化率和相机速度之间的关系。对于归一化中心点特征交互矩阵的形式是L [[-1/Z, 0, u/Z, uv, -(1u^2), v], [0, -1/Z, v/Z, 1v^2, -uv, -u]]其中Z是目标在相机坐标系下的深度。问题来了单目相机没有Z你得估。常见做法是用框的宽高反推假设目标物理尺寸已知Z f * W_real / w_pixel。这个估计不准交互矩阵就不准增益调起来就会遇到“目标附近来回晃”的情况。我的做法是把增益分成两部分平移增益lambda_v和旋转增益lambda_w。平移增益从 0.1 开始试旋转增益从 0.05 开始。如果机械臂在目标附近振荡先把旋转增益砍半因为旋转对图像特征的影响是非线性的增益大了很容易过冲。如果收敛太慢再慢慢加平移增益但不要超过 0.5。还有一个坑是特征点的选择。如果你只用中心点当目标在图像边缘时交互矩阵的条件数会变差速度指令会突然变大。加宽高特征之后相当于增加了约束条件数会好一些。但宽高特征的增益要设得比中心点小因为它们对深度估计误差更敏感。3.3 用 rqt 和 Gazebo 可视化工具定位收敛问题调试 IBVS 不能靠猜得看数据。我一般会开三个窗口Gazebo 看机械臂实际运动rqt_image_view看相机图像和 YOLOv5 框rqt_plot画特征误差曲线。特征误差定义成当前特征和目标特征的差目标特征就是图像中心(0, 0)加上期望的框宽高。如果误差曲线单调下降但很慢说明增益太小加增益。如果误差曲线振荡说明增益太大或者延迟太大先降增益再检查图像时间戳和控制周期是否对齐。如果误差曲线下降一段后突然跳变多半是 YOLOv5 丢帧了检查置信度阈值和光照条件。Gazebo 这边可以把相机的视场角调小一点比如从 60 度调到 45 度这样目标在图像里占比更大YOLOv5 检测更稳。但视场角太小又容易在接近目标时丢失目标需要根据工作距离权衡。我一般会在仿真里试 50 度左右真机上再根据实际镜头调。4. 避坑与排查这套视觉伺服最容易翻车的五个地方4.1 现象YOLOv5 框在动但机械臂不动原因速度指令没发出去或者 MoveIt 的set_pose_target被规划失败吞掉了。常见的是plan[0]返回 False但代码里没打印日志看起来就像没反应。解决在plan()后面加rospy.loginfo(plan success: %s, plan[0])如果一直是 False检查目标位姿是否超出工作空间或者set_pose_target的参考坐标系是不是设成了panda_link0而不是world。另外确认move_group的allow_replanning设成了 True否则每次规划都是从头算容易失败。4.2 现象Gazebo 里机械臂动作很慢实时因子掉到 0.3原因相机插件更新率太高或者 YOLOv5 推理占满了 CPUGazebo 抢不到计算资源。如果你在虚拟机里跑显卡加速没开这个问题会更明显。解决把相机update_rate从 100 降到 30YOLOv5 用 GPU 推理而不是 CPUGazebo 的real_time_update_rate不要超过 1000。如果是 VMware确认 3D 加速已开启否则 Gazebo 渲染会拖慢整个仿真。实在不行把 Gazebo 的 GUI 关掉用gzserver跑无头模式能省不少资源。4.3 现象伺服收敛到目标附近后机械臂开始高频抖动原因增益太大或者图像延迟导致相位滞后。IBVS 在目标附近时交互矩阵的Z估计误差最大速度指令容易过冲。解决在误差小于某个阈值时把增益降下来或者直接切换到位置控制用 MoveIt 做最后的精定位。我一般会在特征误差范数小于 0.05 时把平移增益乘 0.3旋转增益乘 0.1相当于加了一个软着陆。另外检查控制周期和图像时间戳如果图像延迟超过控制周期的 1.5 倍抖动很难避免只能降增益。4.4 现象YOLOv5 检测框偶尔跳到错误目标上原因置信度阈值太低或者场景里有相似类别的干扰物。视觉伺服对框的连续性要求很高跳一次框速度指令就会突变机械臂会猛动一下。解决把置信度阈值提到 0.6 以上并在后处理里加一个简单的跟踪器比如用 IOU 匹配上一帧的框只保留 IOU 大于 0.3 的检测。如果目标类别多用--classes参数只推理需要的类。另外可以在图像上做 ROI 裁剪只保留工作空间对应的图像区域减少干扰。4.5 现象MoveIt 规划成功但 Gazebo 里机械臂穿模或抖动原因URDF 里的碰撞模型和 Gazebo 的物理模型不一致或者关节控制器参数没调好。MoveIt 用的是规划场景里的碰撞体Gazebo 用的是实际碰撞网格两者不一致时MoveIt 认为可行的轨迹在 Gazebo 里会撞。解决确认 URDF 里collision和visual的几何体一致尤其是相机 link 的碰撞体不要设得太大。Gazebo 的关节控制器用ros_control的joint_trajectory_controllerPID 参数从默认值开始调如果抖动先把 P 增益降 20%。另外检查 Gazebo 的self_collide是否设成了 Trueeye-in-hand 构型下相机和末端容易自碰撞设 True 会让规划更保守但更安全。5. 进阶技巧把 IBVS 收敛速度再提一档的实用手段5.1 用自适应增益替代固定增益固定增益在远距离时收敛慢在近距离时容易振荡。一个实用的改进是按特征误差的范数调增益误差大时用大增益误差小时用小增益。具体做法是lambda lambda_max * tanh(k * ||e||)lambda_max取 0.5k取 5。这样远距离时增益接近 0.5近距离时自动降到 0.1 以下收敛速度和稳定性都能兼顾。def adaptive_gain(error_norm, lambda_max0.5, k5.0): 误差范数越大增益越接近 lambda_max误差小时增益自动衰减 return lambda_max * np.tanh(k * error_norm)这个改动很小但在 Gazebo 里试的时候收敛时间能从 8 秒降到 5 秒左右而且目标附近不再抖。注意lambda_max不要超过 0.6否则远距离时速度指令会超出 MoveIt 的规划能力反而导致规划失败。5.2 用 YOLOv5 的框宽高做深度粗估避免交互矩阵奇异单目 IBVS 最怕Z估计不准。除了用框宽高反推还可以在仿真里给目标加一个 ArUco 码用aruco_ros包做辅助深度估计。但 ArUco 码在真实场景里不一定方便贴所以更通用的做法是在伺服初期用框宽高估Z当框宽高超过某个阈值时认为已经接近目标把Z固定成估计值不再更新。这样交互矩阵在近距离时不会因为Z估计跳变而奇异。具体阈值根据目标物理尺寸和相机焦距算。比如目标宽 0.1m相机焦距 500 像素期望框宽 100 像素那Z 500 * 0.1 / 100 0.5m。当框宽超过 100 像素时Z就固定在 0.5m 附近不再跟着框宽变。这个技巧在 Gazebo 里验证过能明显减少近距离时的速度指令突变。5.3 验证收敛的量化指标别只看机械臂停没停要量化。我一般会记录三个指标特征误差范数、末端速度范数、YOLOv5 框的 IOU 稳定性。特征误差范数降到 0.02 以下、末端速度范数降到 0.01 以下、连续 10 帧 IOU 大于 0.8才算收敛。这三个条件同时满足基本可以认为伺服到位了。在 Gazebo 里跑的时候我会用rosbag record把/camera/image_raw、/yolov5/detections、/feature_error、/joint_states都录下来跑完之后用 Python 画曲线。如果特征误差曲线有超调说明增益还是大了如果末端速度曲线在收敛后还有尖峰说明 YOLOv5 框有跳变得回去查检测稳定性。从那以后我每次调 IBVS都强制先跑一遍开环验证手动发速度指令看图像特征变化是否符合交互矩阵的预测。这一步过了再关闭环。希望帮到你。本文还有配套的精品资源点击获取