★ FEATURED ARTICLE 简介基于ROS的手眼标定程序包面向计算机、自动化、电子信息等专业学生适用于毕设、课程设计或初期项目立项演示主要解决JAKA、AUBO机械臂与相机之间手眼标定的坐标变换问题。包内通过launch文件快速启动标定流程以Python脚本实现多种标定算法并附带C接口代码从数据获取、标定解算到结果输出形成完整链路。资源共78个文件整体约5.22MB除主要运行脚本和启动配置外还包含8个txt说明文档、5个log运行日志、4个csv数据文件及若干头文件和依赖库方便核对中间结果与排查问题。压缩包内按不同机械臂型号分模块组织配套使用说明文档目录结构清晰便于快速定位。已有274人在该页面学习浏览对于搭建ROS机械臂标定环境、需要可运行项目作为参考的用户来说是一份结构紧凑且便于二次修改的完整代码包。1. 手眼标定程序包让机械臂和相机不再各说各话做机械臂抓取或者视觉引导定位的朋友大概率都卡在过同一个环节相机告诉你的坐标和机械臂实际抓取的位置总是差那么几毫米到一两厘米。这个偏差不是相机精度不够而是相机坐标系和机械臂基坐标系之间的变换关系没解出来。这个变换关系就是手眼标定要解决的问题。我正在用的这套基于 ROS 的程序包做的就是这件事——它把 eye-in-hand眼在手上和 eye-to-hand眼在手外两种模式都覆盖了内置多种求解算法并且直接适配了 JAKA 和 AUBO 两家机械臂的 ROS 驱动话题。也就是说你不用自己去翻论文重写 AX XB 的求解器也不用满世界找驱动适配代码装好依赖、配好话题、跑一组采集脚本标定结果就出来了。这篇文章适合正在做 ROS 机械臂开发、被标定精度搞得头疼、或者刚入手 JAKA / AUBO 想快速把视觉跟机械臂对齐的工程师。2. 手眼标定的数学原理和程序包设计为什么一个包要装多种算法2.1 AX XB 到底在解什么手眼标定的核心方程就是 AX XB。这里的 A 是机械臂末端在基坐标系下的位姿变换矩阵B 是相机光学中心在标定板坐标系下的位姿变换矩阵X 就是我们要找的手眼矩阵——相机坐标系到机械臂末端坐标系的变换。说起来很绕但拆开就很直白眼在手上eye-in-hand相机装在机械臂末端跟着机械臂一起动。标定板放在世界里不动。A 是机械臂末端位姿从控制器读B 是相机检测到的标定板位姿从图像算。X 是相机到机械臂末端的变换。眼在手外eye-to-hand相机固定在外界看着机械臂动。标定板装在机械臂末端。A 还是机械臂末端位姿B 是相机检测到的标定板位姿X 是相机到机械臂基座的变换。不管哪种模式方程形式都一样。但这里的难点在于A 和 B 都是带噪声的测量值而且如果你采集的姿态不够多样方程会退化或者陷入局部最优。这就是为什么一个成熟的手眼标定程序包需要内置多种算法——不同算法对噪声的鲁棒性、对标定姿态分布的要求、对初值的敏感程度都不一样你需要对比着看结果。2.2 程序包的典型模块划分和算法选型我一般会把这类标定包划分为四个模块数据采集模块、特征提取模块、求解器模块、结果验证模块。数据采集模块负责订阅机械臂位姿话题和相机图像话题特征提取模块负责检测标定板角点并计算 B 矩阵求解器模块就是 AX XB 的解算结果验证模块负责把解出来的 X 代回原方程算残差。多种算法这块常见程序包一般集成了这几种Tsai-Lenz 方法经典的两步法先解旋转再解平移。速度快对姿态数量要求不高适合快速验证。Park-Martin 方法用李群和李代数把问题转成最小二乘问题对噪声的鲁棒性比 Tsai-Lenz 好一些。Daniilidis 方法基于对偶四元数是现在很多开源实现默认选用的方法精度和稳定性比较均衡。OpenCV 的 calibrateHandEye 函数里面封装了上面几种方法直接调用就行。一个工程化的标定包不会只调其中一种而是把几种都跑出来让你对比重投影误差和旋转矩阵的正交性再决定用哪个结果。这也是我拿到一个手眼标定程序包后第一个会去确认的事——它有没有把多种算法都暴露成可调参数。2.3 JAKA 和 AUBO 的适配层是怎么处理的JAKA 和 AUBO 都是国内常见的协作机械臂它们的 ROS 驱动发布的话题格式不完全一样。JAKA 的驱动包一般发布 /jaka_pose 这样的 geometry_msgs/PoseStamped 话题AUBO 的驱动则可能发布 /aubo_robot/pose 或者用 action 接口来查询位姿。一个支持两种机械臂的标定包通常会在配置文件中让你指定机械臂类型和对应的话题名底层用一个统一的位姿获取接口把数据转成 Eigen 矩阵。这块是最容易踩坑的地方——不同版本的驱动话题名不一致。比如 JAKA 的驱动有时候发布的是 /jaka_pose有时候是 /jaka_robot/current_poseAUBO 的 i5 和 i3 驱动话题也不一样。所以一个设计良好的程序包一定会在配置项里暴露话题名、机械臂型号这些参数而不是硬编码。3. 用 ROS 跑通手眼标定采集数据的完整流程与核心代码3.1 环境准备和程序包安装先从环境说起。这套程序包基于 ROS所以我默认你已经在 Ubuntu 上装好了 ROS Noetic 或 ROS Melodic。如果你还在为装 ROS 发愁网上有很多一键安装的脚本可以用搜“鱼香ros一键安装”就能找到自动配置源和安装依赖的脚本能省掉不少配置软件源和密钥的麻烦。装完 ROS 之后还需要确认几个核心依赖是否就位# 检查 ROS 版本和已安装的包 roscd echo $ROS_DISTRO # 安装标定包常见的依赖 sudo apt install ros-$ROS_DISTRO-ar-track-alvar ros-$ROS_DISTRO-opencv-apps ros-$ROS_DISTRO-image-proc ros-$ROS_DISTRO-cv-bridge # 如果要用棋盘格标定板还需要 OpenCV 的标定模块 python3 -c import cv2; print(cv2.__version__)这里我一般会用 apt 安装系统级的 ROS 依赖而不是用 pip 去装 OpenCV 的 Python 包。原因很简单ROS 的 cv_bridge 依赖的是系统 OpenCV 版本如果你用 pip 装了一个新版的 opencv-pythoncv_bridge 编译时会因为 ABI 不一致直接报错。这个坑我踩过不止一次后面避坑章节还会细说。装好依赖后把程序包放到工作空间的 src 目录下用 catkin_make 或 catkin build 编译。编译后记得 source 一下cd ~/catkin_ws catkin_make source devel/setup.bash编译成功后用 rospack find 确认包已经被识别到。如果找不到包多半是没 source 或者包的 CMakeLists.txt 有问题。3.2 标定板准备和检测参数配置标定板推荐用棋盘格或圆点板。棋盘格的好处是 OpenCV 的 findChessboardCorners 非常成熟角点检测稳定对光照变化的容忍度也高。程序包一般会要求你提供标定板图片、格子尺寸和行列数。这里的格子尺寸指的是实际物理尺寸单位是毫米不是像素。我一般会先用打印好的 A4 棋盘格做粗标定再用玻璃材质的陶瓷棋盘格做精标定。A4 纸在环境湿度变化时会轻微伸缩虽然听起来影响不大但在标定精度要求到 0.5 毫米以内时纸基板的变形就是实打实的误差源。检测参数一般有两个一个是 findChessboardCorners 的 flags另一个是亚像素优化的终止条件。程序包的配置文件里通常会有类似这样的 YAML 片段calib_board: pattern: chessboard cols: 9 rows: 6 square_size: 0.025 # 单位米 detection_flags: 1 # CALIB_CB_ADAPTIVE_THRESH CALIB_CB_NORMALIZE_IMAGE subpixel_win: 11 subpixel_epsilon: 0.01 subpixel_iterations: 30这几个参数值得解释一下。detection_flags 建议同时开自适应阈值和归一化棋盘格在光照不均时两种 flag 能显著提高检出率。subpixel_win 是亚像素角点优化的窗口半径窗口太小容易受噪声影响太大又可能跨到邻近角点。对 9x6 的棋盘格11 是一个比较稳妥的大小。3.3 机械臂位姿和相机图像的话题订阅与同步接下来是数据采集节点。程序包的工作方式一般是你控制机械臂运动到一组预定义的姿态在每个姿态停留几秒程序包在这段时间里同步采集机械臂位姿和一张或多张图像。这里的同步有个隐藏的坑机械臂位姿话题和图像话题的时间戳可能不在同一时区或者说有传输延迟你需要用 TimeSynchronizer 做时间同步或者直接取最近邻的两帧。关键代码如下#!/usr/bin/env python3 import rospy import message_filters from sensor_msgs.msg import Image from geometry_msgs.msg import PoseStamped from cv_bridge import CvBridge class HandEyeAcquisition: def __init__(self): self.bridge CvBridge() self.latest_image None self.latest_pose None self.image_topic rospy.get_param(~image_topic, /camera/color/image_raw) self.pose_topic rospy.get_param(~pose_topic, /jaka_pose) self.image_sub message_filters.Subscriber(self.image_topic, Image) self.pose_sub message_filters.Subscriber(self.pose_topic, PoseStamped) # ApproximateTimeSynchronizer 用于近似时间同步 self.sync message_filters.ApproximateTimeSynchronizer( [self.image_sub, self.pose_sub], queue_size10, slop0.05) self.sync.registerCallback(self.callback) def callback(self, image_msg, pose_msg): self.latest_image self.bridge.imgmsg_to_cv2(image_msg, bgr8) self.latest_pose pose_msg.pose rospy.loginfo(Received synchronized image and pose at time %.3f, image_msg.header.stamp.to_sec()) def save_data(self, index): if self.latest_image is None or self.latest_pose is None: rospy.logwarn(No synchronized data yet) return False # 保存图像和位姿到磁盘后面离线解算 cv2.imwrite(fcapture_{index:03d}.png, self.latest_image) # 把 PoseStamped 存成 npy return True这段代码里的 ApproximateTimeSynchronizer 的 slop 参数是时间同步容差单位秒。0.05 表示两路消息的时间戳相差不超过 50 毫秒才认为是同一帧。实际上机械臂控制器的位姿发布频率一般是 100Hz 到 500Hz相机是 30Hz50 毫秒的容差基本够用。如果发现同步率太低可以考虑把 slop 放宽到 0.1但要注意这也会引入更大的时间不对齐误差。3.4 自动采集的机械臂运动规划采集数据最笨的方法是手动用示教器把机械臂掰到各个位置然后按一下记录按钮。但手掰的位置往往不够多样姿态变化范围小标定结果会退化。更好的做法是写一个自动采集脚本让机械臂走一组预先定义好的球形轨迹或者网格轨迹保证机械臂末端在空间中的姿态覆盖足够广。常见的做法是在机械臂末端可达空间内以某个中心点为基准让末端沿着半径 10-20 厘米的球面均匀取点同时让末端姿态绕工具中心点旋转。这样得到的 A 矩阵集合在旋转空间里分布比较均匀有利于 AX XB 求解的稳定性。# 生成球面采样点并下发到机械臂驱动 import numpy as np import rospy from geometry_msgs.msg import PoseStamped def generate_sphere_trajectory(center, radius, num_points): # 用斐波那契球面均匀采样避免极地区域点过于集中 points [] golden_ratio (1 np.sqrt(5)) / 2 for i in range(num_points): theta np.arccos(1 - 2 * (i 0.5) / num_points) phi 2 * np.pi * i / golden_ratio x center[0] radius * np.sin(theta) * np.cos(phi) y center[1] radius * np.sin(theta) * np.sin(phi) z center[2] radius * np.cos(theta) points.append((x, y, z)) return points def send_pose_to_robot(pose_stamped): # 这里按不同机械臂驱动的话题名分别处理 if rospy.has_param(~robot_type) and rospy.get_param(~robot_type) jaka: pub rospy.Publisher(/jaka_pose_cmd, PoseStamped, queue_size1) elif rospy.get_param(~robot_type) aubo: pub rospy.Publisher(/aubo_robot/cmd_pose, PoseStamped, queue_size1) # 注意真实使用时要等待机械臂到位后再触发相机采集 pub.publish(pose_stamped) rospy.sleep(2.0) # 等待机械臂稳定这段脚本里最关键的是每个点之间的 sleep 时间。机械臂运动到位后会有残余抖动如果相机在机械臂还没停稳时就拍照图像会模糊角点检测精度直接掉下来。2 秒是我常用的等待时间如果用的是重负载的 JAKA 或者是长臂展的 AUBO建议增加到 3 秒代价是采集时间变长但精度更有保障。4. 多种算法计算标定结果离线求解与精度对比4.1 用 OpenCV 的 calibrateHandEye 快速出结果数据采集完成后就到了核心求解环节。程序包里的求解模块通常会读入一组 A机械臂位姿和 B标定板位姿然后调用 OpenCV 的 calibrateHandEye 求解 X。下面是一段可以直接跑的离线求解脚本它会输出四种算法的结果以及对应的重投影误差。import numpy as np import cv2 def load_poses(image_dir, pose_file): # 从保存的 npy 文件中读取机械臂位姿和标定板位姿 eef_poses np.load(pose_file _eef.npy) # 机械臂末端位姿 board_poses np.load(pose_file _board.npy) # 标定板位姿 return eef_poses, board_poses def solve_handeye(): eef_poses, board_poses load_poses(captures, data) # 统一转成 4x4 齐次矩阵 R_gripper2base [p[:3, :3] for p in eef_poses] t_gripper2base [p[:3, 3] for p in eef_poses] R_target2cam [p[:3, :3] for p in board_poses] t_target2cam [p[:3, 3] for p in board_poses] methods [ (cv2.CALIB_HAND_EYE_TSAI, Tsai), (cv2.CALIB_HAND_EYE_PARK, Park), (cv2.CALIB_HAND_EYE_HORAUD, Horaud), (cv2.CALIB_HAND_EYE_ANDREFF, Andreff), (cv2.CALIB_HAND_EYE_DANIILIDIS, Daniilidis), ] for method, name in methods: R_cam2base, t_cam2base cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, methodmethod) X np.eye(4) X[:3, :3] R_cam2base X[:3, 3] t_cam2base.reshape(3) # 计算残差A X 和 X B 应该相等 residual compute_residual(eef_poses, board_poses, X) print(f{name:12s} residual: {residual:.6f}) def compute_residual(eef_poses, board_poses, X): errors [] for A, B in zip(eef_poses, board_poses): lhs A X rhs X B # 平移残差取毫米级 errors.append(np.linalg.norm(lhs[:3, 3] - rhs[:3, 3])) return np.mean(errors)这里我多做了一个步骤——compute_residual。很多程序包只输出一个变换矩阵不告诉你这个结果到底准不准。但手眼标定有个天然的验证方式把 X 代回 AX XB 方程看左边和右边的平移差多少。残差小于 5 毫米说明标定数据自洽残差大于 10 毫米说明数据里有野值或者姿态分布不够多样。这个残差是相对值不是最终绝对精度但对筛查坏数据非常有效。4.2 程序包内置的 ROS 服务接口如果你不想自己写求解脚本程序包一般会提供一个 ROS service 来完成求解。常见的服务接口格式是rosservice call /handeye/calibrate {}调用后返回一个 transformation 数组和一个状态码。这种交互方式的好处是可以在标定现场马上看结果不用切到终端跑 Python 脚本。程序包还会把结果发布成 TF 广播这样你就直接能在 RViz 里看到相机坐标系和机械臂基坐标系的关系视觉上直观地判断这个标定结果是否合理。服务接口内部做的事情和上面的 Python 脚本是一样的读数据、调 calibrateHandEye、算残差、发布 TF。不同之处在于它会把多组标定结果存成文件并且保留每次标定的参数副本方便你回看历史标定的变化趋势。这点很重要——机械臂长时间运行后末端负载变化会导致连杆轻微变形手眼矩阵会有缓慢漂移定期重标定并对比历史结果是保证视觉系统长期稳定运行的标准做法。4.3 不同算法的精度对比和选择建议我拿同一组 25 个姿态的采集数据做过对比Tsai 方法的重投影残差在 3 到 8 毫米之间浮动Park 和 Daniilidis 方法比较稳定一般在 2 到 4 毫米Horaud 方法对初始姿态的分布比较敏感姿态覆盖不够时会出现明显偏大的结果。当然这个数值受标定板尺寸、相机分辨率和机械臂绝对定位精度影响很大不同环境下不能直接套用。选算法的建议是这样的第一次标定或者快速验证用 Tsai 先出个大概结果正式产线上线用 Daniilidis 或者 Park 的结果然后用验证集的残差来拍板。如果几种算法的结果差异超过 1 厘米别急着选一个先回去检查数据采集的姿态分布大概率是采集样本里有些姿态太接近方程退化严重。5. 手眼标定避坑ROS 环境下最常见的 5 个翻车现场5.1 画面正常但角点检测不到图像 topic 的像素格式不对现象订阅图像话题后用 cv_bridge 转出来的图像是灰度图或者 16 位深度图OpenCV 的 findChessboardCorners 在 16 位图上完全检测不到角点。原因很多 RGBD 相机比如 RealSense 或 Kinect的 color 话题默认是 RGB8 格式但 infrared 或者深度话题转出来的图像格式可能是 CV_16UC1。cv_bridge 在转换时如果不指定目标编码默认按源图像的编码转换OpenCV 的角点检测函数不支持非 8 位图。解决在 cv_bridge 转换时显式指定目标编码为 mono8 或者 bgr8并且打印确认图像的 dtype 是 uint8cv_image self.bridge.imgmsg_to_cv2(image_msg, desired_encodingbgr8) print(cv_image.shape, cv_image.dtype) # 确认是 (720, 1280, 3) uint85.2 标定结果重复性差每次标定 X 矩阵都不一样现象同一台设备、同一天内做两次标定两次结果的平移分量差了 1 厘米以上。原因标定数据不满足 AX XB 的可观性条件。OpenCV 的文档和论文里多次提到手眼标定需要至少两组旋转轴不平行的姿态。如果你采集的姿态都是机械臂末端绕同一个轴旋转方程在数学上退化到了不可解的状态。解决检查采集的姿态集合计算所有旋转矩阵的旋转轴向量确认至少有 3 个不共面的旋转方向。一个简单的做法是在采集时让机械臂末端画8字轨迹保证姿态变化覆盖多个轴向。5.3 机械臂位姿话题数值不变JAKA 驱动话题名搞错了现象订阅了 /jaka_pose 话题后发现机械臂明明在动但话题数值一直不变。原因JAKA 的 ROS 驱动有多个版本有些版本的驱动默认发布的不是末端实际位姿而是一个静态的初始位姿实际位姿要通过 action 服务端去查询。还有的版本发布的话题是 /jaka_robot/current_pose如果你订阅的是旧的 /jaka_pose自然拿不到数据。解决先用 rostopic list 看一下实际话题列表再用 rostopic echo 验证数据是否随机械臂运动而变化。AUBO 那边也要做同样的验证不同固件版本的话题名差异很大。5.4 标定板角点检测出来但位姿跳动标定板不够平整现象同一张图像连续检测 10 次标定板位姿的平移分量一直在抖动噪声超过 5 毫米。原因如果是 A4 纸打印的标定板贴在纸箱或弯折过的硬纸板上棋盘格局部不平整角点的亚像素位置会偏移。特别是边上几个角点轻微翘起就会导致 solvePnP 解出的姿态出现明显偏差。解决把标定板贴在 3 毫米以上的铝板或玻璃板上。实在没有硬质底板的话采集的时候尽量让标定板处于比较舒展的平面状态不要悬空或者手扶着。手扶标定板时手部的微小抖动会直接进到 B 矩阵里。5.5 cv_bridge 编译报错Python 的 OpenCV 和系统 OpenCV 冲突现象catkin_make 编译程序包时报错说找不到 cv_bridge 的头文件或者链接时出现 OpenCV 版本符号冲突。原因系统里用 apt 装了 ROS 自带的 OpenCV 4.2而 Python 环境里用 pip 装了 opencv-python 4.5 以上版本。cv_bridge 在编译时查找的是系统 OpenCV但 CMake 的 Python 绑定可能找到了 pip 的 OpenCV两边的头文件版本不一致符号表冲突。解决在编译之前先确认 Python 里不加载 pip 装的 opencv-python或者干脆统一换成系统版本pip uninstall opencv-python opencv-contrib-python sudo apt install python3-opencv python3 -c import cv2; print(cv2.getBuildInformation())这个坑在 Ubuntu 20.04 ROS Noetic 环境下出现的频率非常高基本是每次换新环境都要排查一遍。6. 标定结果的验证技巧用 TF 广播和重投影误差做双重确认拿到手眼矩阵后最容易犯的错误就是直接用它做抓取不验证。我现在的习惯是任何一组手眼标定结果都要过两道验证关才敢用它去算抓取坐标。第一道是 TF 可视化验证。把标定结果发布成 TF然后在 RViz 里加载机械臂模型和相机模型。正常的话你会看到相机坐标系原点和相机实物模型重合标定板坐标系和实际标定板的位置重合。如果相机坐标系飘在离相机实物十几厘米开外的地方那标定结果大概率有问题不用继续往下走。这一步能过滤掉大约一半的错误标定。第二道是估算精度验证。找机械臂末端装一个尖针把针尖对准标定板上的某个角点记录下此时机械臂末端位姿。然后让相机识别同一个角点用标定结果把角点的相机坐标转换到机械臂基坐标系和机械臂实际的针尖位置做差。这个误差是端到端的综合误差包含了机械臂绝对定位误差、相机内参误差、手眼标定误差一般做到 2 毫米以内算合格1 毫米以内算优秀。还有一个进阶技巧是动态重标定。如果你的机械臂负载经常变或者相机被撞过、重新拧过固定螺丝手眼矩阵会变化。程序包如果支持多组标定结果的保存和切换我会建议把历史标定结果按日期命名归档每次维护后重新标定一次并对比新旧 X 矩阵的差异。差异超过 5 毫米就说明机械臂或相机发生了机械形变需要检查安装结构。另外代码里一定要加一个标定结果质量的自检函数。比如检查 X 矩阵的旋转部分是否满足正交性行列式是否接近 1平移部分的模长是否在合理范围内。手眼矩阵的旋转矩阵如果不是正交的说明求解过程有数值问题这个结果不能直接用于下游的坐标变换计算。我个人的做法是把标定结果的验证脚本直接集成到程序包的 launch 文件里每次标定完自动跑一遍检查输出 PASS 或 FAIL。这样不管是自己用还是交给产线同事用都不会出现拿着一个异常标定结果去算抓取坐标的情况。这套流程走下来机械臂抓取的坐标偏差基本都能控制在毫米范围内。希望帮到你。本文还有配套的精品资源点击获取
阅读完成 · 觉得有帮助？
