简介基于ROS的手眼标定实现项目面向机器人视觉、机械臂控制领域的开发者和研究人员旨在解决相机与机械臂之间相对位姿的精确标定问题适用于视觉伺服、抓取定位等典型场景。压缩包共78个文件以19个Python脚本、11个launch启动文件、C源文件及头文件为主体辅以txt配置说明、CSV数据、日志文件等整体大小仅5.25MB目录结构清晰便于按模块学习和调用。目前已有460人学习下载。除完整源码外资源还配套流程教程从实验环境搭建、标定图像采集、特征提取与参数求解到结果验证与误差分析均有详细讲解并针对Tsai-Lenz与张正友等标定方法给出ROS实现思路。源码关键部分附有注释便于理解与二次开发既适合ROS初学者快速上手也能为已有经验的研究者提供可直接使用的标定参考。1. 基于ROS的手眼标定为什么这个包值得亲手跑一遍机械臂视觉抓取的第一步往往不是训练模型而是让机械臂搞清楚“相机看到的东西在机械臂坐标系里到底在哪”。手眼标定解决的就是这个坐标转换问题它求出一个4x4的齐次变换矩阵把相机坐标系下的坐标换算到机械臂基座或工具坐标系。基于ROS的手眼标定实现已经是相当成熟的套路采集标定板位姿、记录TF、用OpenCV求解、发布成TF做验证一整套流程半天就能跑通。这篇笔记面向想往视觉抓取方向走、但又不想只调黑匣子的开发者我会把原理、命令、参数和血泪踩坑一次说清楚。2. 手眼标定先搞清楚两件事eye-in-hand与eye-to-hand的数学差异和选型手眼标定最容易被新手忽略的是你手里的机械臂和相机组合究竟是“眼在手上”还是“眼在手外”。这两者的数学模型不一样待求矩阵的含义也不一样选错了后面所有数据都是白采集。2.1 eye-in-hand眼在手上相机随机械臂移动标定的是相机到工具末端的变换眼在手上是目前视觉抓取最常用的构型。相机固定在机械臂末端法兰上标定板摆在机械臂工作空间内的固定位置机械臂带着相机从不同角度去看这块板。每次移动机械臂我们可以记录两个位姿一是相机观察到的标定板相对相机的位姿二是机械臂末端相对基座的位姿。连续两组数据之间做差得到两个变换增量机械臂末端从位姿A移动到位姿A的增量以及相机看到的标定板从位姿B到B的增量。由于相机和末端都是刚性地固定在一起这两个增量之间满足AX XB的关系其中X就是相机坐标系到机械臂末端坐标系的变换矩阵。为什么用增量而不直接用绝对位姿因为绝对位姿里混着相机安装位置、机械臂零位误差等未知量而增量只关心相对运动误差能在一定程度上相互抵消。AX XB这个方程其实就是旋转矩阵约束加上平移向量约束的联立方程组需要至少三组非平行的运动才能有唯一解实际中我们往往采15到30组让姿态差异尽量大。这个矩阵X一旦求出来之后相机看到任何一个目标点我们都能通过X把它换算到末端坐标系下再结合TF树里末端到基座的变换最终得到目标在机械臂基座坐标系下的坐标直接交给规划器去抓取。眼在手上的优点是相机跟着末端走能够近距离观察目标精度高缺点是工作范围受机械臂臂展限制而且标定误差会随机械臂姿态变化被放大。它特别适合桌面抓取、装配引导这一类场景。2.2 eye-to-hand眼在手外相机固定标定的是相机到机械臂基座的变换眼在手外的构型是相机固定在天花板或者支架上机械臂在相机视野里运动。此时待求矩阵变成相机坐标系到机械臂基座坐标系的变换。方程形式上也可以写成AX XB但A和B的含义对调了A是机械臂末端两次运动的增量B是相机看到的机械臂末端或附着在末端上的标定板两次观测的增量。由于相机固定标定板通常绑在机械臂末端跟着末端一起动。眼在手外的好处是整个工作空间都在视野内适合对全局进行观察和避障缺点是相机离目标往往较远定位精度可能不如眼在手上。选型上如果你的目标物体尺寸小、机械臂工作范围不大优先考虑眼在手上如果目标是多工位切换或者机械臂移动范围很大眼在手外更省心。还有一点眼在手外的标定结果不随机械臂姿态变化使用起来更稳定但安装时需要保证相机支架本身不发生松动否则标定矩阵要重新做。2.3 旋转与平移分离为什么标定结果是4x4齐次矩阵而不是3x3旋转矩阵很多入门教程直接给一个四元数结果就完事但实际标定出来的是一个4x4齐次矩阵它由3x3旋转矩阵R和3x1平移向量t组合而成。旋转矩阵负责把方向拨正平移向量负责把坐标系原点挪到位。3x3矩阵只能表达旋转无法表达两个坐标系原点的偏移而手眼标定的本质就是同时求旋转和平移所以必须是齐次矩阵。理解这个结构对后续验证很重要。一个合法的手眼矩阵它的3x3子矩阵必须是正交矩阵行列式严格等于1平移向量量级应该和相机到末端的实际距离吻合。你可以把标定板、相机、机械臂末端的物理尺寸画出来粗略估算一下平移向量是否合理。如果R的行列式偏离1超过0.01或者t向量比实际物理距离大了一个数量级那一定不是最优解而是数据采集或坐标系定义出了问题。把旋转和平移分开看定位问题会快很多。构型相机位置待求矩阵适用场景eye-in-hand固定在机械臂末端相机到末端camera to tool桌面抓取、近距离装配eye-to-hand固定在外部支架相机到基座camera to base全局视野、多工位切换3. 在ROS里跑通手眼标定的最小闭环采集、记录、求解一条龙原理讲完接下来就是动手。这一章我按最常见的eye-in-hand流程来写标定板固定相机随机械臂动。整套流程分为四步准备环境、采集数据、离线求解、验证结果。每一步的命令和代码都会给出并说明关键参数怎么改。3.1 准备相机内参、标定板、TF树与消息流开始之前先确认三样东西是齐的。第一相机驱动要正常发布图像话题和camera_info话题camera_info里必须带有相机内参矩阵K和畸变系数D如果没有可以先通过ROS的camera_calibration功能包或离线OpenCV棋盘格标定把内参标出来再写进相机驱动。第二标定板用ArUco比棋盘格更方便因为单张图片就能得到唯一的位姿而且鲁棒性更好。我一般买边长5到8厘米的ArUco板打印后一定要用卡尺量实际尺寸不要相信打印设置里的尺寸。第三机械臂的TF树里要有从base_link到tool0的完整变换在RViz里能看到才算正常。消息流连接起来是这样相机发布/camera/color/image_raw和/camera/color/camera_info机械臂驱动发布TF。采集脚本订阅图像话题检测ArUco标定板得到标定板在相机坐标系下的位姿同时通过TF监听器拿到机械臂末端在基座坐标系下的位姿。两条数据配对保存构成一组手眼标定样本。这里有一个容易混淆的点标定板位姿是“标定板坐标系在相机坐标系下”的表示末端位姿是“末端坐标系在基座坐标系下”的表示两者的参考系不同所以不能直接比较必须等求解阶段通过AXXB把它们关联起来。3.2 采集端用ArUco检测得到标定板在相机坐标系的位姿同时记录机械臂末端位姿下面是采集脚本的核心代码。这个脚本会实时显示图像检测到ArUco ID为0的标定板后绘制坐标轴按s键保存一组数据按q键退出并保存全部样本。注意要先标定好相机内参否则estimatePoseSingleMarkers会直接报错或者结果偏移。#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import numpy as np import cv2 import tf2_ros import tf.transformations as tf from cv_bridge import CvBridge from sensor_msgs.msg import Image, CameraInfo MARKER_SIZE 0.05 # ArUco 实际边长单位米必须用卡尺量 ARUCO_DICT cv2.aruco.Dictionary_get(cv2.aruco.DICT_ARUCO_ORIGINAL) DET_PARAMS cv2.aruco.DetectorParameters_create() class HandEyeSampler: def __init__(self): self.bridge CvBridge() self.tf_buffer tf2_ros.Buffer() self.tf_listener tf2_ros.TransformListener(self.tf_buffer) self.K None self.D None self.samples [] self.latest_T_marker_cam None rospy.Subscriber(/camera/color/camera_info, CameraInfo, self.info_cb) rospy.Subscriber(/camera/color/image_raw, Image, self.image_cb) def info_cb(self, msg): if self.K is None: self.K np.array(msg.K).reshape(3, 3) self.D np.array(msg.D) def image_cb(self, msg): if self.K is None: return img self.bridge.imgmsg_to_cv2(msg, bgr8) corners, ids, _ cv2.aruco.detectMarkers(img, ARUCO_DICT, parametersDET_PARAMS) if ids is None or 0 not in ids.flatten(): return i list(ids.flatten()).index(0) rvec, tvec, _ cv2.aruco.estimatePoseSingleMarkers( corners[i], MARKER_SIZE, self.K, self.D) R, _ cv2.Rodrigues(rvec[0]) T_marker_cam np.eye(4) # 标定板坐标系在相机坐标系下的位姿 T_marker_cam[:3, :3] R T_marker_cam[:3, 3] tvec[0].ravel() self.latest_T_marker_cam T_marker_cam cv2.drawFrameAxes(img, self.K, self.D, rvec[0], tvec[0], MARKER_SIZE) cv2.imshow(calib, img) cv2.waitKey(1) def grab(self): if self.latest_T_marker_cam is None: rospy.logwarn(没有检测到标定板不保存) return False try: trans self.tf_buffer.lookup_transform( base_link, tool0, rospy.Time(0)) except Exception as e: rospy.logwarn(TF获取失败: %s, e) return False q trans.transform.rotation T_tool_base tf.quaternion_matrix([q.x, q.y, q.z, q.w]) T_tool_base[:3, 3] [ trans.transform.translation.x, trans.transform.translation.y, trans.transform.translation.z ] self.samples.append((self.latest_T_marker_cam.copy(), T_tool_base)) rospy.loginfo(第 %d 组已保存, len(self.samples)) return True def main(): rospy.init_node(handeye_sampler) sampler HandEyeSampler() rate rospy.Rate(10) while not rospy.is_shutdown(): key cv2.waitKey(10) 0xFF if key ord(s): sampler.grab() elif key ord(q): break rate.sleep() if len(sampler.samples) 3: T_cam_marker np.array([s[0] for s in sampler.samples]) T_tool_base np.array([s[1] for s in sampler.samples]) np.savez(handeye_samples.npz, T_cam_markerT_cam_marker, T_tool_baseT_tool_base) rospy.loginfo(已保存 %d 组数据到 handeye_samples.npz, len(sampler.samples)) if __name__ __main__: main()这段代码里MARKER_SIZE是最关键的参数它直接决定平移向量的尺度。如果你把5厘米的板写成5米求出来的t向量会大一百倍整个结果毫无意义。lookup_transform(base_link, tool0, ...)返回的是tool0坐标系在base_link坐标系下的位姿也就是我们在AXXB中需要的gripper2base变换。如果你用的是UR机械臂可能还需要确认TCP坐标系到底是tool0还是tool0_controller两者差了一个工具中心点的偏移实际标定前最好在RViz里点开TF树看清楚。保存格式是npz里面存了两个4x4矩阵数组第一维是样本序号。3.3 求解端用OpenCV calibrateHandEye计算X矩阵采集完数据进入离线求解。OpenCV内置了calibrateHandEye函数支持TSAI、PARK、HORAUD等多种算法。我们只需要把采集到的两组矩阵拆成旋转和平移按函数要求的顺序传入即可。#!/usr/bin/env python3 # -*- coding: utf-8 -*- import numpy as np import cv2 data np.load(handeye_samples.npz) T_cam_marker data[T_cam_marker] # 标定板在相机系下 T_tool_base data[T_tool_base] # 末端在基座系下 # calibrateHandEye 要求输入的是 R_gripper2base / t_gripper2base # 以及 R_target2cam / t_target2cam R_gripper2base T_tool_base[:, :3, :3] t_gripper2base T_tool_base[:, :3, 3] R_target2cam T_cam_marker[:, :3, :3] t_target2cam T_cam_marker[:, :3, 3] # TSAI 是经典方法数据质量好时推荐PARK 对噪声更稳 R_cam2gripper, t_cam2gripper cv2.calibrateHandEye( R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, methodcv2.CALIB_HAND_EYE_TSAI ) X np.eye(4) X[:3, :3] R_cam2gripper X[:3, 3] t_cam2gripper.ravel() np.savetxt(handeye_X.txt, X, fmt%.6f) print(相机到机械臂末端变换矩阵 X:) print(X)这段代码的输出X含义是“相机坐标系在机械臂末端坐标系下”的位姿。也就是说如果你在TF里把camera_link挂在tool0下面父子关系应该是tool0为父、camera_link为子他们的变换矩阵就是X。使用calibrateHandEye有一个细节函数内部会做归一化和去均值因此对数据量并不苛刻但输入数据如果全是同一个姿态变化很小的样本算法会返回不可信的结果。经验上看TSAI方法在16到25组数据时表现稳定数据量超过30组后提升有限反而可能引入更多手眼运动带来的机械振动误差。matrix求解后记得检查旋转子矩阵的行列式是否为1。可以在代码里加一行验证# 验证旋转矩阵合法性 R X[:3, :3] print(det(R) , np.linalg.det(R))如果行列式在0.999到1.001之外直接说明数据有问题不要继续往下用。3.4 验证端把标定结果发布成TF用重投影误差和位姿误差打分求解出X之后先不要急着接抓取。最直接的验证方法是把X发布成静态TF然后在一个固定标定板场景下移动机械臂看看“标定板在基座坐标系下的位置”是否稳定。因为标定板是固定不动的理论上不管机械臂怎么动用X和当前机械臂末端的TF把相机观测到的标定板位姿换算到基座系下结果应该是一个几乎不动的点。#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import numpy as np import tf2_ros import tf.transformations as tf X np.loadtxt(handeye_X.txt).reshape(4, 4) rospy.init_node(publish_handeye_tf) br tf2_ros.StaticTransformBroadcaster() msg geometry_msgs.msg.TransformStamped() msg.header.stamp rospy.Time.now() msg.header.frame_id tool0 # 父坐标系机械臂末端 msg.child_frame_id camera_link # 子坐标系相机 quat tf.quaternion_from_matrix(X) msg.transform.translation.x X[0, 3] msg.transform.translation.y X[1, 3] msg.transform.translation.z X[2, 3] msg.transform.rotation.x quat[0] msg.transform.rotation.y quat[1] msg.transform.rotation.z quat[2] msg.transform.rotation.w quat[3] br.sendTransform([msg]) rospy.spin()运行这个发布节点后在RViz里把TF树展开你会看到base_link - tool0 - camera_link这个链。如果相机图像话题和标定板检测能对上你还可以对当前检测到的标定板位姿做一个定量验证把标定板在相机坐标系下的四个角点通过X变换到tool0再通过TF变换到base_link和实际物理位置对比偏差。更简单的做法是用3.2的采集脚本再采几组新数据离线计算标定板在基座系下的坐标波动标准差小于5毫米基本就可以用了大于10毫米需要回头查原因。4. 手眼标定的四个必调参数数据量、标定板尺寸、采样策略、内参精度手眼标定表面上是求解一个矩阵实际操作中真正影响结果的是几个参数。这一章把四个最容易踩的参数位拆开讲每个都对应你在终端里能看到、能改的数值。4.1 数据量为什么12组数据是玄学20组以上才稳定网上很多教程写“至少采12组”这句话被很多人当成铁律。实际上12组只是理论上能求的最小数量真实数据里每一组都带噪声12组往往让求解结果落在局部敏感区间。我自己的习惯是至少采20组保证姿态覆盖充分。数据量不是越多越好超过40组后每次移动机械臂都会引入微小的振动而且时间越长越容易混入标定板被碰移位的风险。合理区间是20到30组。判断数据量是否够不需要看绝对值可以做一个简单实验用采集到的前12组数据求一次X再用全部数据求一次X对比两个结果的平移向量差。如果差超过5毫米说明数据量不足或姿态多样性不够需要补采。这个方法比盲目堆数据量更靠谱。4.2 标定板尺寸与角点间距毫米单位的细微差别如何影响t向量ArUco板的MARKER_SIZE参数是标定板物理边长单位米。这个参数同时影响estimatePoseSingleMarkers的尺度恢复如果实际边长为49.5毫米你却在代码里写0.05那么求出来的相机到标定板距离会偏大约1%最终手眼矩阵的平移向量也会跟着偏1%。比如实际距离是50厘米偏差就有5毫米这在抓取精度要求高的场景下已经很明显。我一般用电子卡尺量标定板的黑色外边框多量几次取平均值。不要量内部白色方块因为不同型号的ArUco字典白色区域占比不一样只有最外层的黑色边框尺寸才是算法使用的真实尺度。另外打印纸的受潮、弯曲也会导致尺寸变化建议把标定板贴在硬质平板上不要用软纸。4.3 采样策略不要把机械臂拧成一个姿势连拍要让末端姿态差异尽量大采样策略直接决定AXXB方程是否有解。原理上说X被旋转约束唯一确定的前提是多组姿态变化之间不共线。实操中最常见的错误是让机械臂在同一个位置附近小幅平移姿态几乎不变这样采集到的数据在旋转维度上高度相关求解结果会病态。正确做法是让机械臂末端绕X、Y、Z轴分别做30度以上的姿态变化同时平移范围覆盖相机视野的不同区域。一个有效的经验是相邻两组的末端姿态差异至少要让标定板在图像中的角度变化超过15度。你可以想象让相机从一个“俯视”的视角逐渐变成一个“斜视”视角这样才能给求解器足够的约束。如果机械臂运动学精度差这个姿态差异要求还要更高。4.4 相机内参精度先标内参再标外参否则全盘翻车手眼标定建立在相机内参准确的前提上。ArUco位姿估计需要K矩阵和畸变系数D如果内参标定时误差超过1个像素标定板位姿会出现系统性偏差这个偏差会原封不动地传递到手眼矩阵里。更隐蔽的是畸变系数不准时图像边缘的标定板位姿误差明显大于中心区域即使你后期重新采样也无法消除。所以我的流程永远是先用棋盘格离线标定相机内参检查重投影误差小于0.5像素再开始手眼标定。不要把ROS机器人驱动里写的默认内参当真很多模拟器与真实相机驱动出来的camera_info只是占位符。内参不准时手眼标定结果看起来可能很漂亮但一旦做抓取实验就会原形毕露属于典型的“标定一时爽验证火葬场”。5. 手眼标定避坑指南3个真实翻车现场与排查清单这一章把我实际遇到过的、以及在多个现场帮别人解决的问题整理成三条。每一条都按现象、原因、解决的顺序写你在复现时如果遇到类似症状可以直接照着定位。5.1 坑1标定板在图像边缘算出来的X平移向量偏得离谱现象求解出的X旋转矩阵看起来正常但平移向量数值比物理距离大好几倍或者验证时投影误差达到几厘米。原因标定板靠近图像边缘时相机镜头畸变最严重ArUco角点检测出现亚像素偏差畸变校正后的位姿误差被放大。另一方面图像边缘的标定板往往太小或成像角度过于倾斜导致位姿估计退化。解决采集时把标定板控制在图像中心60%的区域并且让标定板在图像中占据至少10%的面积。如果必须使用边缘数据先确认畸变系数标定可靠再决定是否保留该样本。5.2 坑2机械臂末端位姿用了错误坐标系结果R矩阵一直震荡现象每次运行求解脚本得到的X都不一致旋转矩阵的行列式明显不等于1甚至出现NaN。原因TF树上存在多个与末端相关的坐标系例如tool0、tool0_controller、flange等选了错误的父坐标系。这些坐标系的原点差异只有几毫米但旋转方向可能相差很大导致同一组数据算出的X完全不同。解决在RViz里显示TF树找到真正的运动学链末端坐标系。用示教器手动移动机械臂观察base_link - tool0的变换是否随运动持续更新。确认后把采集脚本里的坐标系字符串固定下来不要随意改。5.3 坑3时间戳对不齐A和B序列错位求解器直接给NaN现象采集时偶尔提示TF lookup失败求解时某些数据点产生极端异常值甚至数组维度不匹配。原因图像回调数据和TF数据来自不同话题传感器时间戳相差过大。图像需要经过传输和编码解码到达回调时已经滞后而TF查询用的是当前时间两者配对到同一物理运动状态时存在时间偏差。解决图像话题开启transport_hints使用tcpNoDelay降低延迟TF查询时不要用rospy.Time(0)而是用图像消息的header.stamp这样能保证配对的是同一时刻的位姿。另外对每一组样本计算图像时间戳和TF时间戳的差值超过20毫秒的样本直接剔除。5.4 排错清单从TF树到矩阵行列式的五步检查如果你已经按流程走了一遍但结果不理想按下面顺序检查基本能定位80%的问题。对每项有一个典型阈值供参考。检查项方法合理范围TF树坐标系RViz查看末端与基座关系父子关系正确无跳变旋转矩阵行列式numpy计算det(R)0.999 到 1.001平移向量量级对比物理安装距离误差不超过20%重投影误差固定标定板多姿态验证标准差小于5毫米交叉一致性前12组与全部数据各求一次X平移差小于5毫米这个清单的最后一项往往最能说明问题。如果你发现前12组数据和全部数据求出来的X差异很大而其他检查项都正常那大概率是采样姿态多样性不够不是算法问题。不要急着换求解方法先回去补几组大姿态变化的数据往往效果立竿见影。6. 标定完别急着抓取验证重投影误差与动态精度的两个技巧求出手眼矩阵X只代表数学上收敛了不代表真实场景里能用。我每次标定完都会先做一个固定标定板稳定性验证把标定板固定在桌上机械臂带着相机移动六个不同姿态每一组都用当前X把检测到的标定板位姿变换到基座坐标系下计算这六个三维点的离散程度。这个离散度直接反映了手眼矩阵在动态运动中的精度我的习惯是标准差小于3毫米才敢进入下一步。第二个技巧是交叉验证。把采集到的样本随机去掉五组用剩余数据求一次X再用这五组数据做重投影记录误差重复做十次取平均误差作为X的最终评分。如果某一次验证误差特别大说明该组数据可能是手抖、标定板移位或时间戳错乱产生的离群值直接删除并重新标定。我一般会保留误差最小的那次标定结果而不是每次都重新采集数据。进阶一点可以用在线标定思路在机械臂实际工作时每抓取成功一次就把当前观测到的目标点反投影到图像上计算像素偏差持续累加。如果偏差逐渐变大说明手眼矩阵可能因为碰撞或相机松动已经失效。这个技巧虽然简单却能帮你提前发现“标定突然不准”的问题。我自己的习惯是每次开机先跑一次固定标定板验证耗时不到两分钟换来的却是后面抓取实验的稳定复现非常划算。手眼标定这个方向真正难的不是解法而是对坐标系、参数和数据质量的敬畏。把这一套流程跑通之后你会发现视觉抓取的后续调试都顺畅了很多。希望这份实战笔记能帮你少走一些弯路顺利跑出自己的手眼标定结果。本文还有配套的精品资源点击获取