简介:一套基于ROS的手眼标定程序包,适用于机械臂与视觉结合方向的毕业设计、课程设计及ROS入门进阶者,主要解决机械臂末端与相机之间坐标变换矩阵的标定问题。程序支持JAKA、AUBO等常见机械臂,并提供基础标定模块,将多组机械臂位姿与Marker位姿输入后即可计算输出,已在ROS kinetic/melodic平台测试通过。压缩包共158个文件,约10.44MB,以38个Python计算脚本、22个launch启动文件、CSV数据文件及CPP源文件、XML配置和MD说明文档为主,目录清晰,便于直接编译和二次开发。除完整源码外,还附带详细程序使用说明,涵盖编译运行、数据配置、不同算法结果及标准差/方差解读,对搭建可复现的手眼标定实验流程很有参考价值。目前已有413人学习/下载,适合机器人、人工智能等专业学生及工程技术人员作为项目参考。

1. 手眼标定:坐标对不齐,机械臂的视觉抓取就是一场盲赌

机械臂加装相机做定位抓取,最常见的翻车现场就是:相机明明看到工件在某个坐标,机械臂走过去却差几毫米,严重时直接撞上夹具。别急着调视觉算法,九成问题出在手眼标定——相机坐标系和机械臂坐标系之间那一次变换没求对。这份资源是“基于 ROS 的手眼标定程序包源码+详细程序使用说明”,核心就是帮你完成眼在手上(eye-in-hand)或眼在手外(eye-to-hand)标定,从环境配置、参数写入、样本采集到结果验证一条龙。它适合三类人:正在做机械臂视觉项目的一线工程师、被毕业设计或课程设计卡在标定环节的本科生、以及想搞懂 AX=XB 数学原理再落地的 ROS 学习者。

2. 手眼标定在解什么:AX=XB 同构方程与两种模型的边界

2.1 四个坐标系一条链:手眼标定到底在求哪个变换

先啰嗦一句:干这行最忌讳一上来就动手采样本,得先知道在解什么。ROS 机械臂视觉场景里,坐标关系通常涉及四个坐标系:基座 base_link、机械臂末端 tool0(有的机器人叫 ee_link 或 flange)、相机光心坐标系 camera_color_optical_frame、标定板坐标系 marker_frame。手眼标定要解的,是那个固定不变的变换 X,它要么是相机相对末端的位姿,要么是相机相对基座的位姿,完全取决于相机装在哪。

以最常见的眼在手上为例:相机跟着末端走,标定板固定在工作台上不动。对任意一个采样位姿,下面的链式关系都成立:标定板相对基座的位姿 = 末端相对基座的位姿 × 相机相对末端的位姿(待求 X)× 标定板相对相机的位姿(视觉测得)。采样两次,把两个等式左边的项消掉(标定板没动过),右边就整理成 A × X = X × B 。这里 A 是两次采样之间末端相对基座的相对运动,B 是两次采样之间相机观察到的标定板相对运动,X 就是我们要的结果。这就是手眼标定的最小本质:从一个同构方程组里解出那个唯一的刚体变换。

实际落地时 A 和 B 不是现成的,得从 TF 和时间戳里凑。下面这段是我常用的小函数,先把 ROS 的 Pose 消息转成 4×4 齐次矩阵,方便后面求解。

import numpy as np
from geometry_msgs.msg import Pose

def pose_to_matrix(pose: Pose) -> np.ndarray:
    # 输入:ROS Pose,携带 position 和 orientation(四元数)
    # 输出:4x4 齐次变换矩阵,用于叠加 A、B 矩阵计算
    x, y, z = pose.position.x, pose.position.y, pose.position.z
    qx, qy, qz, qw = (pose.orientation.x, pose.orientation.y,
                      pose.orientation.z, pose.orientation.w)

    # 四元数转旋转矩阵,ROS 里四元数顺序是 x,y,z,w,qw 在最后
    R = np.array([
        [1 - 2*(qy**2 + qz**2), 2*(qx*qy - qz*qw), 2*(qx*qz + qy*qw)],
        [2*(qx*qy + qz*qw), 1 - 2*(qx**2 + qz**2), 2*(qy*qz - qx*qw)],
        [2*(qx*qz - qy*qw), 2*(qy*qz + qx*qw), 1 - 2*(qx**2 + qy**2)]
    ])
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = [x, y, z]
    return T

参数说明:pose 来自哪里?眼在手上时取 robot_state_publisher 发布的末端 tf,也就是 tool0 在 base_link 下的 Pose;眼在手外时取相机观测标定板得到的 Pose。四元数转 R 这段网上版本很多,最容易写错的是 qw 位置,ROS 里四元数排列是 x、y、z、w,别按习惯想成 w 在第一位。

拿到 Pose 转换函数之后,A 和 B 的构造顺序是关键,尤其是矩阵乘法左右顺序:

# 眼在手上:A = inv(T_base_ee1) @ T_base_ee2
A = np.linalg.inv(pose_to_matrix(pose_ee1)) @ pose_to_matrix(pose_ee2)
# B = T_cam_marker2 @ inv(T_cam_marker1),注意不是反过来
B = pose_to_matrix(marker_cam2) @ np.linalg.inv(pose_to_matrix(marker_cam1))

逻辑说明:A 表示末端从位姿 1 到位姿 2 的相对运动,要先把位姿 1 的矩阵求逆,再左乘位姿 2;B 表示标定板在相机视野里的相对运动,先取位姿 2 的观测矩阵,再右乘位姿 1 的逆。顺序一旦反了,解出来的 X 会带上多余的旋转,后面怎么验都对不上。

2.2 眼在手上与眼在手外:模型选错,代码白跑

这里有个很常见的翻车点:把标定板放错位置。很多第一次做手眼标定的人,拿到程序包就默认把标定板放桌上、相机装末端,结果标出来误差大得离谱。其实两种模型的采样方式完全不同,先看对比表:

对比项 眼在手上 eye-in-hand 眼在手外 eye-to-hand
相机安装位置 固定在机械臂末端手爪上方 固定在工作台或支架上
标定板位置 固定在工作台上不动 固定在机械臂末端
待求变换 相机相对末端 tool0 相机相对基座 base_link
求解 X T_ee_cam T_base_cam
代码参数 eye_on_hand: true eye_on_hand: false
适用场景 分拣、移动抓取 固定工位质检、扫码

怎么选?我一般这样判断:如果相机要跟着机械臂到处跑、视野随末端位置变化,比如传送带分拣,用眼在手上;如果相机焊死在工位上看同一个区域,机械臂只是把手伸进去操作,用眼在手外。选错模型的结果不是误差大一点点,而是整个 X 的量纲和方向都对不上,后面所有坐标换算全废。

眼在手上的方程是 T_base_marker = T_base_ee × X × T_cam_marker ,消掉不变项得到 AX=XB;眼在手外的推导类似,但 A 的构造顺序会反过来,所以不能拿同一套 A/B 构建代码直接用,必须改顺序。这也是为什么源码包里会把 eye_on_hand 单独做成参数,而不是写死在脚本里。

2.3 Tsai-Lenz 与 OpenCV calibrateHandEye:算法选型与样本量的边界

理论有了,具体用什么算法求解?OpenCV 的 calibrateHandEye 函数直接支持 Tsai、Park、Horaud、Andreff、Daniilidis 五种求解器,其中最常用的是 Tsai-Lenz。Tsai 法数学上最少只要 3 组样本就能解,但那是理论下限,工程上 3 组解出来的精度根本没法看,结果常常随采样顺序剧烈变化。

我一般采 15 到 20 组样本,覆盖多个高度、多个朝向,且要求旋转轴交叉。为什么要交叉?AX=XB 本质是解线性方程组,如果所有样本姿态都近似同一个旋转,方程会退化,数值上解就在多个方向之间来回跳。这跟相机内参标定要转动棋盘格是一个道理,样本多样性决定了方程的条件数。

算法 特点 工程建议
Tsai-Lenz 速度快、实现简单、对噪声敏感 默认先用它
Park 对旋转噪声更鲁棒 样本质量一般时换它
Daniilidis 联合解算、精度高但计算慢 精度优先、样本多时用

最省事的做法是:先 Tsai 跑一遍,如果结果抖动,别急着换算法,先检查第 4 章里的采样坑;样本质量确认没问题了,再换 Park 验一遍结果是否一致。

3. 把 ROS 手眼标定源码包跑起来:环境准备、参数写入与一次完整标定

3.1 装环境与读源码包:先看目录再动手

先解决环境问题。装 ROS 时最容易卡在换源这一步,Ubuntu 20.04 上装 ROS Noetic、Ubuntu 22.04 上装 ROS Humble,如果官方的源速度跑不动,常见做法是用鱼香 ROS 一键安装脚本,它会自动处理镜像切换和依赖安装。我在 20.04 和 22.04 上都跑过,机械臂手眼标定这类项目我一般选 Ubuntu 20.04 配 ROS Noetic,因为很多机械臂驱动、相机驱动的老接口还在 Noetic 上,Humble 也能跑,但有些第三方包要自己重新编译。

源码包解压后,先放到 catkin 工作空间的 src 目录下编译,比如 ~/catkin_ws/src 。这类 ROS 标定工程的目录布局一般比较统一,解压后先花五分钟把目录结构看完,再动手改参数:

# 解压后源码包常见的目录布局
hand_eye_calib/
├── CMakeLists.txt
├── package.xml
├── launch/
│   ├── calibration.launch      # 主标定流程
│   └── publish_result.launch   # 发布静态 tf
├── config/
│   ├── camera.yaml             # 相机内参
│   └── hand_eye_params.yaml    # 手眼参数
├── scripts/
│   ├── sample.py               # 采集样本
│   ├── compute.py              # 求解
│   └── verify.py               # 验证
└── doc/
    └── 程序使用说明.pdf

逻辑说明:解压后别急着编译,先把 doc 里的程序使用说明快速翻一遍,重点看 launch 目录下每个 launch 文件的参数名和默认值;再看 scripts 目录里 sample 和 compute 两个脚本——一个是采集,一个是求解,verify 是最后验证用的。源码包这种组织方式的好处是:采集和求解分离,你可以在采样阶段反复删掉不合适的样本,而不用重新求解整个流程。

编译命令也很简单,在 catkin_ws 根目录执行:

cd ~/catkin_ws
catkin_make
source devel/setup.bash

参数说明:catkin_make 是 ROS1 的经典编译命令,如果用的是 ROS2 Humble,对应的编译命令是 colcon build,但手眼标定源码包大多基于 ROS1 的 roscpp/rospy 接口,所以优先用 Noetic 环境跑。

3.2 相机内参标定与配置文件参数表

手眼标定依赖准确的内参。如果相机内参标定没做或结果不对,后续手眼方程解出来就是乱的。准备一块棋盘格标定板,常见规格是 10×7 的方格、内角点 9×6,方格边长 25mm 或 30mm。标定方法有两种:一是用 OpenCV 的 calibrateCamera 写脚本,二是用 ROS 的 camera_calibration 包实时采集。我一般用 camera_calibration,因为它会直接产出 camera_info 话题,省去手动填 yaml 的麻烦。

内参标定完成后,把结果写进 config/camera.yaml:

# camera.yaml 示例:内参参数含义
image_width: 640
image_height: 480
camera_matrix:
  rows: 3
  cols: 3
  data: [618.2, 0.0, 321.5,  0.0, 619.8, 248.3,  0.0, 0.0, 1.0]
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.28, 0.09, -0.001, 0.002, -0.02]

参数说明:camera_matrix 是内参矩阵,data 按行优先填入 fx、0、cx、0、fy、cy、0、0、1;fx 和 fy 是焦距,单位是像素,cx、cy 是主点坐标。distortion_coefficients 是 5 参数畸变模型,依次是 k1、k2、p1、p2、k3。注意如果你的相机畸变比较大,比如鱼眼镜头,5 参数模型可能不够,得用 fisheye 标定,但那属于另一个话题。

然后是手眼参数文件 hand_eye_params.yaml,这是整个流程里最需要仔细核对的地方:

eye_on_hand: true              # true=相机在末端;false=相机固定在外
robot_base_frame: base_link     # 基座坐标系名,必须与 URDF 一致
robot_effector_frame: tool0     # 末端坐标系名,注意大小写
tracking_base_frame: base_link  # 眼在手外时用 base,眼在手上时留空
tracking_marker_frame: aruco_marker_frame  # 标定板坐标系名
marker_size: 0.033              # marker 边长,单位米
camera_topic: /camera/color/image_raw      # 图像话题
calibration_type: aruco         # 标定板类型:aruco 或 chessboard

参数说明:eye_on_hand 是模型开关,填错等于让整个程序按错误模型跑;robot_base_frame 和 robot_effector_frame 必须和你机械臂 URDF 里的名字完全一致,大小写都区分;tracking_base_frame 在眼在手上时通常留空或填 base_link,因为标定板相对基座的关系不被用到;marker_size 是 aruco 码的边长,单位是米,这个值必须用卡尺实测,不能只用打印设置里的理论值。

3.3 一次完整标定的操作流程与时间戳对齐

环境配置好了,参数写好了,下面是一次完整标定的操作流程。这个流程我在不同机械臂上跑过很多遍,顺序基本固定,新手照做就行:

# 第一步:启动相机驱动,确认图像话题有输出
rostopic echo /camera/color/image_raw --once

# 第二步:查看 TF 树,确认 base_link 到 tool0 的链路完整
rosrun rqt_tf_tree rqt_tf_tree

# 第三步:启动标定主流程
roslaunch hand_eye_calib calibration.launch

命令说明:rostopic echo 是确认相机话题有数据,如果这里就 echo 不出来,后面标定程序肯定拿不到图像;rqt_tf_tree 是图形化检查 TF 关系,装好 ROS 后自带,重点看 base_link 到 tool0 是否有实时 tf 发布,以及相机光学坐标系是否挂载在正确位置。

标定程序启动后,会弹出 rviz 窗口,里面能看到相机图像和标定板 marker 的识别结果。接下来按这个顺序操作:

  1. 用示教器或 MoveIt 把机械臂移到第一个采样位姿,让标定板完整出现在相机视野里。
  2. 等机械臂完全停稳至少 500ms,在 rviz 里点击 Take Sample 采集一组数据。
  3. 移动机械臂到新位姿,要求旋转角度明显变化,不要只是平移。重复 15 到 20 次。
  4. 所有样本采完后,点击 Compute 按钮求解。
  5. 读取输出的平移向量和四元数,检查数值是否合理。

采集过程中最容易忽略的是时间戳对齐。我一般会在 sample.py 里强制使用 tf2 的 waitForTransform,确保取到的末端位姿和图像采样时刻相差不超过 20ms。如果机械臂还在运动中就触发采集,或者 ROS 时间不同步,A 矩阵和 B 矩阵对不上,标定结果就会随机跳动。

Compute 结束后的典型输出长这样:

Translation:  [0.031, -0.202, 0.418]
Rotation(quaternion): [0.021, 0.789, 0.023, 0.613]

参数说明:平移向量是相机相对末端的 xyz,单位是米。这个数值的量级要和机械臂实际尺寸匹配——比如六轴机械臂把相机装在法兰下方 10cm 左右,Z 方向大概就是 0.1 量级,如果你算出来平移是 [1.23, -0.41, 25.7] ,25 米的 Z 值必然有问题,直接跳到第 4 章查坐标系配置。

提示:正解一行数据之前,先把 rqt_tf_tree 截图保存。后面验证阶段如果发现坐标对不上,这张 TF 截图是最快的定位工具。

4. 手眼标定避坑:五个让精度翻车的常见问题与排查

下面这五条全部来自实际项目中踩过的坑,不分先后,每一条都按现象、原因、解决三个步骤写清楚。标定结果不对时,先按这个清单逐项排除,比自己瞎调参数高效得多。

4.1 平移向量数值离谱:先查坐标系名和 TF 树

现象:Compute 输出 Translation 里的 Z 值是 25 米,或者某个分量是负数,明显超出机械臂工作空间。

原因:robot_effector_frame 写错了。比如 UR 机械臂末端实际叫 tool0,你填成 ee_link,TF 取不到数据,程序可能拿到默认 Pose 或上一次的缓存值。另一种常见情况是相机 topic 给错,拿到的图像和末端位姿不是同一时刻。还有一种,eye_on_hand 参数与实际的相机安装方式相反,待求 X 变成了另一对坐标系之间的变换,数值自然对不上。

解决:先跑 rqt_tf_tree 检查 base_link 到 tool0 到 camera 到 marker 的链路是否贯通;确认 frame 名大小写完全一致;采集时确认机械臂没有在运动中被触发采样。

4.2 重投影误差很小但实际抓取偏 5mm:标定板尺寸与 TCP 是疑点

现象:标定报告里 reprojection error 只有 0.3px,看起来精度很好,但点接触实测偏差 5 到 8mm。

原因:aruco 的 marker_size 填错了。marker_size 是边长,单位是米,打印误差 0.5mm 在 500mm 工作距离下就会放大成 2 到 3mm 的位置偏差。另一个高发点是机械臂 TCP 与 URDF 模型不一致,比如换过夹爪或快换盘后没有重新标定工具坐标。

解决:用游标卡尺实测 marker 边长,改掉配置里的理论值重新标定;顺便做一次机械臂 TCP 验证——用示教器把工具尖点对准桌面固定点,对比 TF 里实际的工具坐标,确认 TCP 误差在 1mm 以内。

4.3 标定板检测断断续续:光线与材质问题

现象:rviz 里 aruco marker 识别时好时坏,点击 Take Sample 频繁报检测失败。

原因:marker 打印在光面铜版纸上,反光严重;或者环境灯光直射造成过曝;还有一种情况是 marker 在画面中占比太小,角点提取不稳定。

解决:改用哑光纸打印并塑封,不要用光面纸;调整相机曝光时间,降低亮度,侧面打光避免直射反光;让 marker 在相机画面里占 20% 以上。如果 aruco 识别一直不稳,可以切到棋盘格角点检测,棋盘格对光照的敏感度明显更低。

4.4 两次标定结果相差很大:采样姿态太“挤”

现象:同一套环境,连着标两次,旋转四元数看着不一样,平移差几毫米到一厘米。

原因:采样姿态太接近。所有样本几乎都是末端微移、旋转极少,A 矩阵数值上趋近奇异,AX=XB 退化成欠定方程,解在多个方向之间来回跳。

解决:每个采样点之间要有大角度旋转,覆盖不同朝向,横滚、俯仰、偏航分步转动;不要只在一个平面里扫。样本量控制在 15 到 20 组,采样路径在规划时就要刻意让姿态分散。

4.5 标定板被动过:眼在手上的隐形破坏者

现象:标定过程全程顺利,误差报告也好看,但验证时换个角度偏,同一个角度又准。

原因:眼在手上模型的前提是标定板固定在世界系不动,方程左边的 T_marker_base 不随时间改变。如果标定过程中标定板被碰到,或者桌面振动移位,所有样本的消元前提都失效了,但程序本身不会报错。

解决:标定前用胶带把标定板固定在工作台上,贴一张提醒条;Compute 前在 rviz 里查看 marker_frame 相对 base_link 的 TF 是否始终恒定,如果看到漂移,删掉漂移后的那几组样本重新计算。

5. 标定完先别收工:一个点接触测试把精度打到明面上

5.1 把标定结果发布成静态 TF

标定输出拿到后,第一步不是写进业务代码,而是先把结果固化成静态 tf。把 Compute 输出的平移向量和四元数填进下面的 launch 片段:

<launch>
  <node pkg="tf2_ros" type="static_transform_publisher"
        name="hand_eye_result"
        args="0.031 -0.202 0.418 0.021 0.789 0.023 0.613 \
              camera_color_optical_frame tool0" />
</launch>

参数说明:static_transform_publisher 的前三个参数是平移 xyz,后四个是四元数 xyzw,最后两个分别是父坐标系和子坐标系。注意这里顺序是相机坐标系在父、末端坐标系在子,还是反过来,取决于你求解的 X 的方向,发布后在 rqt_tf_tree 里确认链路正确即可。

5.2 点接触测试:让机械臂真的“碰”一下

发布静态 tf 后,在标定板上选一个特征角点,比如棋盘格左上角的内角点,然后用 rviz 的 Publish Point 工具点击该点,读它在 base_link 下的坐标。接着用示教器把机械臂末端装上针尖,移动到该坐标,看针尖是否真的落在角点上。

偏差小于 2mm 说明标定可用;大于 5mm 说明第 4 章列的五类问题至少有一项没排清。这个测试比看 reprojection error 可靠得多,因为它验证的是整个坐标链路的最终结果,而不是某个环节的中间误差。

从那以后,我每次标完手眼,不管项目多赶,都要先跑一遍点接触测试再关标定窗口,顺手把配置文件、输出结果和验证截图一起存档,方便后面排查问题。这套习惯帮我避免了不少返工,希望帮到你。

本文还有配套的精品资源,点击获取 menu-r.4af5f7ec.gif

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐