基于 ArUco 码的机器人手眼标定

基于 ArUco 码的机器人手眼标定

一、引言

在机器人视觉应用中，手眼标定是最基础也是最关键的步骤之一。它建立了相机坐标系与机器人坐标系之间的变换关系，使得机器人能够根据视觉信息精确地抓取、放置和操作物体。

本文将从数学原理、操作流程、代码实现三个层面，详细介绍基于 ArUco 码的手眼标定方法。

二、什么是手眼标定

2.1 基本概念

手眼标定解决的问题是：确定相机坐标系与机器人坐标系之间的固定变换关系。

根据相机的安装位置，手眼标定分为两种模式：

模式	安装方式	特点	应用场景
Eye-in-Hand	相机安装在机械臂末端	相机随机械臂移动	视觉伺服、移动抓取
Eye-to-Hand	相机固定在外部支架	相机固定，观察整个工作空间	视觉定位、质量检测

2.2 为什么要用 ArUco 码

ArUco 码是开源增强现实库中定义的基准标记，具有以下优势：

• 单目即可定位：仅需一张图像即可计算完整6自由度位姿
• 鲁棒性高：有纠错能力，抗遮挡
• 易于制作：打印即可使用
• 开源支持好：OpenCV 原生支持

三、数学原理

3.1 坐标系定义

在进行手眼标定之前，首先要明确几个坐标系：

坐标系	符号	说明
机器人基座坐标系	{Base}	机器人的世界坐标系原点
机器人末端坐标系	{End}	机械臂末端法兰盘中心
相机坐标系	{Camera}	相机光心为原点
ArUco码坐标系	{Marker}	标记中心为原点，Z轴垂直于平面

3.2 变换矩阵

任意两个坐标系之间的变换可以用 4×4 齐次变换矩阵表示：

T = [R    t]
    [0    1]

其中：

• R：3×3 旋转矩阵，表示两个坐标系的相对朝向
• t：3×1 平移向量，表示原点的相对位置

3.3 Eye-in-Hand 的数学推导

变换链

在 Eye-in-Hand 模式下，从 ArUco 码到机器人基座的变换链为：

Marker → Camera → End → Base

用齐次变换矩阵表示：

T_base_marker = T_base_end × T_end_camera × T_camera_marker

其中：

• T_base_end：机械臂给出的末端位姿（已知）
• T_camera_marker：相机观测到的 ArUco 位姿（已知）
• T_end_camera：相机相对于末端的固定变换（待求解）
• T_base_marker：ArUco 码在基座中的位置（未知但固定）

消去未知量

对于两次不同的采集（i 和 j），ArUco 码位置不变：

T_base_end_i × T_end_camera × T_camera_marker_i = T_base_end_j × T_end_camera × T_camera_marker_j

两边同时左乘 T_base_end_j^{-1}，右乘 T_camera_marker_i^{-1}：

(T_base_end_j^{-1} × T_base_end_i) × T_end_camera = T_end_camera × (T_camera_marker_j × T_camera_marker_i^{-1})

定义：

• A = T_base_end_j^{-1} × T_base_end_i（机械臂末端的相对运动）
• B = T_camera_marker_j × T_camera_marker_i^{-1}（相机观测到的相对运动）
• X = T_end_camera（待求解）

得到经典的 AX = XB 方程。

AX = XB 的几何意义

这个方程的含义是：机械臂末端的运动（A）经过相机到末端的变换（X）后，应该等于相机观测到的运动（B）。

换句话说，无论你从哪个角度观测，相机相对于末端的固定关系是一致的。

3.4 Eye-to-Hand 的数学推导

3.4.1 坐标系定义与变换链

在 Eye-to-Hand 模式下：

• 相机固定在外部支架上，不随机械臂移动
• ArUco 码固定在机械臂末端，随机械臂一起运动

需要求解的变换是：相机坐标系到机器人基座坐标系的固定变换 ，记为 X = T_base_camera。

定义以下坐标系：

• {Base}：机器人基座坐标系
• {End}：机械臂末端坐标系（ArUco 码安装位置）
• {Camera}：相机坐标系（固定）
• {Marker}：ArUco 码坐标系

变换链推导

从 Marker 到 Base 有两条等价路径：

路径一（通过机械臂末端）：

Marker → End → Base

用齐次变换矩阵表示：

T_base_marker = T_base_end × T_end_marker      (1)

路径二（通过相机）：

Marker → Camera → Base

用齐次变换矩阵表示：

T_base_marker = T_base_camera × T_camera_marker (2)

其中：

• T_base_end：机械臂给出的末端位姿（已知）
• T_camera_marker：相机观测到的 ArUco 位姿（已知）
• T_end_marker：ArUco 码在末端上的固定变换（通常设计为单位矩阵，即 ArUco 码坐标系与末端坐标系重合）
• T_base_camera：待求解的相机到基座的固定变换（未知）

由于两条路径都到达同一个 T_base_marker，因此：

T_base_end × T_end_marker = T_base_camera × T_camera_marker   (3)

为简化推导，假设 ArUco 码直接固定在末端上，即 T_end_marker = I（单位矩阵）。若存在固定偏移，可在标定后补偿。于是：

T_base_end = T_base_camera × T_camera_marker   (4)

3.4.2 构造相对运动方程

在公式 (4) 中，对于第 i 次采集：

T_base_end_i = X × T_camera_marker_i   (5)

其中 X = T_base_camera。

对于第 j 次采集：

T_base_end_j = X × T_camera_marker_j   (6)

由于 X 是常数，我们可以通过这两次采集构造相对运动方程。将 (5) 和 (6) 组合，消去 X。

由 (5) 得：

T_camera_marker_i = X^{-1} × T_base_end_i   (7)

由 (6) 得：

T_camera_marker_j = X^{-1} × T_base_end_j   (8)

现在考虑相机观测到的相对运动。从第 i 次到第 j 次，相机观测到的 ArUco 码变换为：

B = T_camera_marker_j × T_camera_marker_i^{-1}   (9)

将 (7) 和 (8) 代入 (9)：

B = (X^{-1} × T_base_end_j) × (X^{-1} × T_base_end_i)^{-1}
  = X^{-1} × T_base_end_j × T_base_end_i^{-1} × X   (10)

整理 (10)：

X × B = T_base_end_j × T_base_end_i^{-1} × X   (11)

令：

• A = T_base_end_j × T_base_end_i^{-1}（机械臂末端的相对运动）
• B = T_camera_marker_j × T_camera_marker_i^{-1}（相机观测到的 ArUco 码相对运动）
• X = T_base_camera（待求解的相机到基座的变换）

则方程 (11) 成为：

X × B = A × X   (12)

或等价地：

A × X = X × B   (13)

3.4.3 AX = XB 的物理意义

方程 A × X = X × B 的含义是：

• A：机械臂末端从位姿 i 到位姿 j 的相对运动（在基座坐标系下描述）
• B：相机观测到的 ArUco 码从位姿 i 到位姿 j 的相对运动（在相机坐标系下描述）
• X：相机坐标系到基座坐标系的固定变换

这个方程表示：机械臂末端的运动（A）经过相机到基座的变换（X）后，应该等于相机观测到的运动（B）经过同样的变换后再回转到基座坐标系下的表达。

换句话说，无论机械臂如何运动，相机观测到的 ArUco 码运动与机械臂末端运动之间存在固定的转换关系，这个转换关系就是 X。

3.4.4 两种模式的对比

模式	方程	A	B	X
Eye-in-Hand	A×X = X×B	T_base_end_j^{-1} × T_base_end_i	T_cam_marker_j × T_cam_marker_i^{-1}	T_end_camera
Eye-to-Hand	A×X = X×B	T_base_end_j × T_base_end_i^{-1}	T_cam_marker_j × T_cam_marker_i^{-1}	T_base_camera

关键发现：

1. 两种模式的 B 定义完全相同
2. 两种模式的 A 互为逆矩阵：Eye-to-Hand 的 A = (Eye-in-Hand 的 A)^{-1}
3. 两种模式的 X 物理含义不同，但数学形式相同
4. OpenCV 的 calibrateHandEye 函数通过参数顺序来区分这两种模式

OpenCV 中的使用

# Eye-in-Hand: 机械臂末端位姿作为第一组参数
R, t = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,  # 机械臂末端位姿 (Base → End)
    R_target2cam, t_target2cam,      # 相机观测位姿 (Marker → Camera)
    method=cv2.CALIB_HAND_EYE_TSAI
)

# Eye-to-Hand: 交换参数顺序
R, t = cv2.calibrateHandEye(
    R_target2cam, t_target2cam,      # 相机观测位姿 (Marker → Camera)
    R_gripper2base, t_gripper2base,  # 机械臂末端位姿 (Base → End)
    method=cv2.CALIB_HAND_EYE_TSAI
)

3.4.5 数学推导总结

Eye-to-Hand 的数学推导最终归结为求解 A × X = X × B，其中：

• A = T_base_end_j × T_base_end_i^{-1}：机械臂末端的相对运动（从 i 到 j）
• B = T_camera_marker_j × T_camera_marker_i^{-1}：相机观测到的 ArUco 码相对运动（从 i 到 j）
• X = T_base_camera：待求解的相机到基座的固定变换

通过采集多组不同位姿下的数据，构造多个 (A_i, B_i) 对，就可以用 Tsai、Park 等算法求解出最优的 X，完成手眼标定。

3.5 两种模式的数学统一

模式	已知	未知	方程
Eye-in-Hand	T_base_end, T_camera_marker	T_end_camera	A×X = X×B
Eye-to-Hand	T_base_end, T_camera_marker	T_base_camera	A×X = X×B

关键发现：两种模式的数学形式完全相同！区别仅在于：

• 采集数据时，ArUco 码的固定位置不同
• 标定结果的物理含义不同

四、操作流程

4.1 准备工作

硬件准备

项目	说明
ArUco 码	打印 DICT_5X5_50 编号0的标记，测量实际边长（如5cm）
固定装置	Eye-in-Hand：固定在桌面/墙面；Eye-to-Hand：固定在机械臂末端
相机	已标定内参和畸变系数
机械臂	能够提供末端位姿（通常是 4×4 齐次矩阵）

软件准备

import numpy as np
import cv2
import json
from pathlib import Path

4.2 数据采集步骤

Eye-in-Hand 模式

1. 将 ArUco 码固定在桌面或墙面 ，确保标定过程中纹丝不动
2. 移动机械臂到第一个位置，使 ArUco 码完整出现在相机视野中
3. 记录当前的机械臂末端位姿 T_base_end
4. 拍摄图像，检测 ArUco 码，计算 T_camera_marker
5. 保存这对数据
6. 移动机械臂到不同的位置和姿态，重复步骤2-5
7. 至少采集 10-20 组数据，确保位姿多样化

Eye-to-Hand 模式

1. 将 ArUco 码固定在机械臂末端
2. 将相机固定在外部支架 ，标定过程中相机不动
3. 移动机械臂，使 ArUco 码出现在相机视野中
4. 记录机械臂末端位姿和相机观测到的 ArUco 位姿
5. 移动机械臂到不同位置，重复采集
6. 至少采集 10-20 组数据

数据采集要点

• 位姿多样化：位置要覆盖工作空间的不同区域，姿态要有较大变化
• 避免共面：不要所有点都在同一平面或直线上
• 确保检测：ArUco 码必须完整、清晰、光照充足
• 精度要求：机械臂静止时记录数据

4.3 数据格式

每组数据包含两个 4×4 齐次矩阵：

# robot_pose: Base → End
robot_pose = np.array([
    [R11, R12, R13, tx],
    [R21, R22, R23, ty],
    [R31, R32, R33, tz],
    [0,   0,   0,   1]
])

# camera_pose: Marker → Camera
camera_pose = np.array([
    [R11, R12, R13, tx],
    [R21, R22, R23, ty],
    [R31, R32, R33, tz],
    [0,   0,   0,   1]
])

五、OpenCV 实现

5.1 定义参数

# ArUco 码参数
MARKER_LENGTH = 0.05  # 实际边长（米）
ARUCO_DICT = cv2.aruco.DICT_5X5_50
ARUCO_ID = 0  # 使用的标记编号

5.2 ArUco 码检测与位姿估计

def detect_aruco_pose(image, camera_matrix, dist_coeffs):
    """
    检测 ArUco 码并返回其在相机坐标系中的位姿
    
    Args:
        image: 输入图像 (BGR)
        camera_matrix: 相机内参矩阵 (3x3)
        dist_coeffs: 畸变系数 (1x5)
    
    Returns:
        rvec: 旋转向量 (3,)
        tvec: 平移向量 (3,)
        image: 可视化图像
    """
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    
    # 创建 ArUco 检测器
    aruco_dict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
    parameters = cv2.aruco.DetectorParameters()
    detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
    
    # 检测 ArUco 码
    corners, ids, _ = detector.detectMarkers(gray)
    
    if ids is None or ARUCO_ID not in ids:
        return None, None, image
    
    # 获取目标 ArUco 码的索引
    idx = list(ids.flatten()).index(ARUCO_ID)
    marker_corners = corners[idx][0]  # (4, 2)
    
    # 定义 ArUco 码的 3D 角点（原点在中心）
    marker_points = np.array([
        [-MARKER_LENGTH/2, -MARKER_LENGTH/2, 0],
        [ MARKER_LENGTH/2, -MARKER_LENGTH/2, 0],
        [ MARKER_LENGTH/2,  MARKER_LENGTH/2, 0],
        [-MARKER_LENGTH/2,  MARKER_LENGTH/2, 0]
    ], dtype=np.float32)
    
    # 求解 PnP 问题
    success, rvec, tvec = cv2.solvePnP(
        marker_points,
        marker_corners,
        camera_matrix,
        dist_coeffs
    )
    
    if not success:
        return None, None, image
    
    # 确保返回 1D 数组
    rvec = rvec.flatten()
    tvec = tvec.flatten()
    
    # 可视化
    cv2.aruco.drawDetectedMarkers(image, corners, ids)
    cv2.drawFrameAxes(image, camera_matrix, dist_coeffs, rvec, tvec, 0.05)
    
    return rvec, tvec, image

5.3 构建齐次变换矩阵

def build_transform_matrix(rvec, tvec):
    """
    从旋转向量和平移向量构建 4x4 齐次变换矩阵
    
    Args:
        rvec: 旋转向量 (3,)
        tvec: 平移向量 (3,)
    
    Returns:
        T: 4x4 齐次变换矩阵
    """
    T = np.eye(4, dtype=np.float64)
    R, _ = cv2.Rodrigues(rvec)
    T[:3, :3] = R
    T[:3, 3] = tvec.flatten()
    return T

5.4 数据采集与存储

class HandEyeCalibrationDataCollector:
    def __init__(self, camera_matrix, dist_coeffs):
        self.camera_matrix = camera_matrix
        self.dist_coeffs = dist_coeffs
        self.robot_poses = []   # Base → End
        self.camera_poses = []  # Marker → Camera
        
    def collect(self, robot_pose, image):
        """
        采集一组标定数据
        
        Args:
            robot_pose: 机械臂末端位姿 (4x4 齐次矩阵)
            image: 当前图像
        """
        # 检测 ArUco 码
        rvec, tvec, vis_image = detect_aruco_pose(
            image, self.camera_matrix, self.dist_coeffs
        )
        
        if rvec is None:
            print("未检测到 ArUco 码")
            return False
        
        # 构建相机观测矩阵
        T_camera_marker = build_transform_matrix(rvec, tvec)
        
        # 保存数据
        self.robot_poses.append(robot_pose.copy())
        self.camera_poses.append(T_camera_marker.copy())
        
        print(f"已采集 {len(self.robot_poses)} 组数据")
        return True
    
    def save_data(self, filepath):
        """保存采集的数据"""
        data = {
            'robot_poses': [p.tolist() for p in self.robot_poses],
            'camera_poses': [p.tolist() for p in self.camera_poses]
        }
        with open(filepath, 'w') as f:
            json.dump(data, f, indent=2)
    
    def load_data(self, filepath):
        """加载已保存的数据"""
        with open(filepath, 'r') as f:
            data = json.load(f)
        self.robot_poses = [np.array(p) for p in data['robot_poses']]
        self.camera_poses = [np.array(p) for p in data['camera_poses']]

5.5 手眼标定计算

def calibrate_hand_eye(robot_poses, camera_poses):
    """
    手眼标定主函数
    
    Args:
        robot_poses: 机械臂末端位姿列表 (Base → End)
        camera_poses: 相机观测位姿列表 (Marker → Camera)
    
    Returns:
        R: 旋转矩阵 (3x3)
        t: 平移向量 (3,)
        success: 是否成功
    """
    # 计算相对运动
    A_list = []  # 机械臂末端相对运动
    B_list = []  # 相机观测相对运动
    
    for i in range(len(robot_poses) - 1):
        # 机械臂末端相对运动：A = T_i⁻¹ × T_j
        T_i = robot_poses[i]
        T_j = robot_poses[i+1]
        A_rel = np.linalg.inv(T_i) @ T_j
        R_A = A_rel[:3, :3]
        t_A = A_rel[:3, 3]
        
        # 相机观测相对运动：B = T_j × T_i⁻¹
        # 注意：这里 T 表示 Marker → Camera
        T_i = camera_poses[i]
        T_j = camera_poses[i+1]
        B_rel = T_j @ np.linalg.inv(T_i)
        R_B = B_rel[:3, :3]
        t_B = B_rel[:3, 3]
        
        A_list.append(np.hstack((R_A, t_A.reshape(3, 1))))
        B_list.append(np.hstack((R_B, t_B.reshape(3, 1))))
    
    A = np.array(A_list)  # (n, 3, 4)
    B = np.array(B_list)  # (n, 3, 4)
    
    # 执行手眼标定
    try:
        R, t = cv2.calibrateHandEye(
            A[:, :3, :3],   # 机械臂相对运动的旋转
            A[:, :3, 3],    # 机械臂相对运动的平移
            B[:, :3, :3],   # 相机观测相对运动的旋转
            B[:, :3, 3],    # 相机观测相对运动的平移
            method=cv2.CALIB_HAND_EYE_TSAI
        )
        return R, t, True
    except Exception as e:
        print(f"标定失败: {e}")
        return None, None, False

5.6 标定结果验证

def verify_calibration(robot_poses, camera_poses, R, t, mode='eye_in_hand'):
    """
    验证标定结果
    
    Args:
        robot_poses: 机械臂末端位姿列表
        camera_poses: 相机观测位姿列表
        R, t: 标定结果
        mode: 'eye_in_hand' 或 'eye_to_hand'
    
    Returns:
        误差统计
    """
    X = np.eye(4)
    X[:3, :3] = R
    X[:3, 3] = t.flatten()
    
    if mode == 'eye_in_hand':
        # 验证：T_base_marker 应该恒定
        values = []
        for i in range(len(robot_poses)):
            T_base_end = robot_poses[i]
            T_cam_marker = camera_poses[i]
            T_base_marker = T_base_end @ X @ T_cam_marker
            values.append(T_base_marker[:3, 3])
        
        # 计算标准差
        values = np.array(values)
        std = np.std(values, axis=0)
        print(f"Base→Marker 平移标准差: {std}")
        return std
    
    elif mode == 'eye_to_hand':
        # 验证：T_base_camera 应该恒定
        values = []
        for i in range(len(robot_poses)):
            T_base_end = robot_poses[i]
            T_cam_marker = camera_poses[i]
            T_base_camera = T_base_end @ np.linalg.inv(T_cam_marker) @ X
            values.append(T_base_camera[:3, 3])
        
        values = np.array(values)
        std = np.std(values, axis=0)
        print(f"Base→Camera 平移标准差: {std}")
        return std

5.7 完整示例

def main():
    # 1. 加载相机参数（需要提前标定）
    camera_matrix = np.array([
        [fx, 0, cx],
        [0, fy, cy],
        [0, 0, 1]
    ])
    dist_coeffs = np.zeros(5)  # 替换为实际畸变系数
    
    # 2. 创建数据采集器
    collector = HandEyeCalibrationDataCollector(camera_matrix, dist_coeffs)
    
    # 3. 采集数据（这里需要连接实际硬件）
    # for i in range(20):
    #     robot_pose = get_robot_pose()  # 从机械臂获取
    #     image = get_camera_image()      # 从相机获取
    #     collector.collect(robot_pose, image)
    
    # 或者加载已保存的数据
    collector.load_data('calibration_data.json')
    
    print(f"已加载 {len(collector.robot_poses)} 组数据")
    
    # 4. 执行手眼标定
    R, t, success = calibrate_hand_eye(
        collector.robot_poses,
        collector.camera_poses
    )
    
    if success:
        print("标定成功！")
        print(f"旋转矩阵 R:\n{R}")
        print(f"平移向量 t: {t.flatten()}")
        
        # 5. 验证结果
        std = verify_calibration(
            collector.robot_poses,
            collector.camera_poses,
            R, t,
            mode='eye_in_hand'  # 根据实际模式选择
        )
        
        # 6. 保存结果
        result = {
            'R': R.tolist(),
            't': t.flatten().tolist()
        }
        with open('calibration_result.json', 'w') as f:
            json.dump(result, f, indent=2)
        
        print("结果已保存到 calibration_result.json")
    else:
        print("标定失败")

if __name__ == '__main__':
    main()

六、常见问题与解决方案

6.1 ArUco 码检测失败

可能原因：

• 光照不足或过曝
• ArUco 码尺寸太小
• 字典不匹配

解决方案：

• 调整光照
• 增大 ArUco 码尺寸
• 确认字典类型一致

6.2 标定结果为单位矩阵

可能原因：

• 未计算相对运动，直接传入绝对位姿
• 数据质量差，缺乏多样性

解决方案：

• 确保使用相对运动计算 A 和 B
• 增加位姿变化，避免共面

6.3 验证时误差大

可能原因：

• 数据采集时 ArUco 码或相机移动
• 机械臂位姿精度低
• 相机内参标定不准确

解决方案：

• 确保标定过程中固定物纹丝不动
• 使用高精度机械臂
• 重新标定相机内参

七、总结

基于 ArUco 码的手眼标定是一种成熟、可靠的机器人视觉标定方法。本文从数学原理、操作流程到代码实现，系统地介绍了完整的标定流程。

关键要点：

1. Eye-in-Hand 和 Eye-to-Hand 的数学形式相同，区别在于物理设置
2. 必须计算相对运动，不能直接使用绝对位姿
3. 至少采集 10-20 组多样化数据，确保标定精度
4. 验证是必要的，通过残差分析判断标定质量