简介本资源面向机器人视觉伺服方向的开发者与研究生提供一套基于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_handorigin的 xyz 决定相机光心相对末端的位置。!-- 在 panda_hand 之后追加eye-in-hand 相机 -- joint namecamera_joint typefixed parent linkpanda_hand/ child linkcamera_link/ !-- 相机装在手掌前方 5cm光轴朝下 -- origin xyz0.05 0 0.05 rpy0 1.5708 0/ /joint link namecamera_link visual geometrybox size0.02 0.05 0.05//geometry /visual /link gazebo referencecamera_link sensor typecamera nameeye_in_hand_cam update_rate30/update_rate camera namehead horizontal_fov1.047/horizontal_fov imagewidth640/widthheight480/heightformatR8G8B8/format/image clipnear0.02/nearfar10/far/clip /camera plugin namecamera_controller filenamelibgazebo_ros_camera.so rosnamespace/camera/namespace/ros cameraNameeye_in_hand/cameraName imageTopicNameimage_raw/imageTopicName cameraInfoTopicNamecamera_info/cameraInfoTopicName frameNamecamera_link/frameName /plugin /sensor /gazebo逻辑说明origin的rpy0 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, pathweights/best.pt, force_reloadFalse) 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_size1) 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走的是本地缓存第一次会下载仓库之后离线可用。conf0.5是经验值太低会引入误检导致特征点乱跳太高会漏检。HSV 范围(0,80,80)-(10,255,255)针对红色目标换目标颜色要重调。发布PointStamped而不是自定义消息是为了让后面的 IBVS 节点能直接用tf做坐标变换。参数怎么改如果目标在图像里很小把conf降到 0.3 并开augmentTrue如果推理频率跟不上 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) / fyK矩阵是 3x3 行优先K[0]fxK[4]fyK[2]cxK[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]逻辑说明lam0.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。lam收敛时间(s)超调(像素)是否震荡0.14.23否0.31.88否0.51.122轻微0.70.945是1.0不收敛-是从表里看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能省掉很多后悔药。希望帮到你。本文还有配套的精品资源点击获取 基于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_handorigin的 xyz 决定相机光心相对末端的位置。!-- 在 panda_hand 之后追加eye-in-hand 相机 -- joint namecamera_joint typefixed parent linkpanda_hand/ child linkcamera_link/ !-- 相机装在手掌前方 5cm光轴朝下 -- origin xyz0.05 0 0.05 rpy0 1.5708 0/ /joint link namecamera_link visual geometrybox size0.02 0.05 0.05//geometry /visual /link gazebo referencecamera_link sensor typecamera nameeye_in_hand_cam update_rate30/update_rate camera namehead horizontal_fov1.047/horizontal_fov imagewidth640/widthheight480/heightformatR8G8B8/format/image clipnear0.02/nearfar10/far/clip /camera plugin namecamera_controller filenamelibgazebo_ros_camera.so rosnamespace/camera/namespace/ros cameraNameeye_in_hand/cameraName imageTopicNameimage_raw/imageTopicName cameraInfoTopicNamecamera_info/cameraInfoTopicName frameNamecamera_link/frameName /plugin /sensor /gazebo逻辑说明origin的rpy0 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, pathweights/best.pt, force_reloadFalse) 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_size1) 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走的是本地缓存第一次会下载仓库之后离线可用。conf0.5是经验值太低会引入误检导致特征点乱跳太高会漏检。HSV 范围(0,80,80)-(10,255,255)针对红色目标换目标颜色要重调。发布PointStamped而不是自定义消息是为了让后面的 IBVS 节点能直接用tf做坐标变换。参数怎么改如果目标在图像里很小把conf降到 0.3 并开augmentTrue如果推理频率跟不上 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) / fyK矩阵是 3x3 行优先K[0]fxK[4]fyK[2]cxK[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]逻辑说明lam0.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。lam收敛时间(s)超调(像素)是否震荡0.14.23否0.31.88否0.51.122轻微0.70.945是1.0不收敛-是从表里看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能省掉很多后悔药。希望帮到你。本文还有配套的精品资源点击获取