具身智能：零基础入门睿尔曼机械臂（六）——手眼标定代码库详解，从原理到实践

前言

手眼标定是机器人视觉领域的关键技术，它解决了机械臂与相机之间的坐标转换问题，为精准抓取、视觉伺服等应用奠定基础。上一篇博客中我们讲解了手眼标定的概念以及原理，本文将详细解析睿尔曼官方提供的完整的手眼标定代码库，包括其结构设计、核心功能及使用方法，帮助读者快速掌握手眼标定的实现流程。

文章目录

前言
项目概述
项目结构
核心原理
- [1. 眼在手上模式（Eye-in-Hand）](#1. 眼在手上模式（Eye-in-Hand）)
- [2. 眼在手外模式（Eye-to-Hand）](#2. 眼在手外模式（Eye-to-Hand）)
核心代码解析
- 一、核心计算模块深度解析
- - [1. 眼在手上标定（compute_in_hand.py）](#1. 眼在手上标定（compute_in_hand.py）)
  - - [1.1 初始化与配置加载](#1.1 初始化与配置加载)
    - [1.2 标定板角点检测与相机外参计算](#1.2 标定板角点检测与相机外参计算)
    - [1.3 相机标定与位姿转换](#1.3 相机标定与位姿转换)
    - [1.4 手眼标定核心计算](#1.4 手眼标定核心计算)
  - [2. 眼在手外标定（compute_to_hand.py）](#2. 眼在手外标定（compute_to_hand.py）)
  - - [2.1 核心差异点](#2.1 核心差异点)
    - [2.2 标定目标差异](#2.2 标定目标差异)
- 二、辅助模块解析
- - [1. 数据采集模块（collect_data.py）](#1. 数据采集模块（collect_data.py）)
  - [2. 配置文件（config.yaml）](#2. 配置文件（config.yaml）)
  - [3. 日志模块（libs/log_setting.py）](#3. 日志模块（libs/log_setting.py）)
  - [4. 位姿处理工具（save_poses.py / save_poses2.py）](#4. 位姿处理工具（save_poses.py / save_poses2.py）)
- 三、代码串联与整体流程
- - [1. 数据流向图](#1. 数据流向图)
总结

项目概述

该代码库专注于实现两种常见的手眼标定模式：

眼在手上（Eye-in-Hand）：相机固定在机械臂末端，随机械臂一起运动
眼在手外（Eye-to-Hand）：相机固定在机械臂外部，位置保持不变

通过采集机械臂位姿和标定板图像，计算出相机与机械臂之间的坐标变换矩阵，最终实现目标从相机坐标系到机械臂基坐标系的转换。

项目结构

复制代码

hand_eye_calibration/
├── README.md               # 项目说明文档
├── collect_data.py         # 数据采集程序
├── compute_in_hand.py      # 眼在手上标定计算
├── compute_to_hand.py      # 眼在手外标定计算
├── config.yaml             # 配置文件
├── requirements.txt        # 依赖包列表
├── save_poses.py           # 位姿数据处理工具
├── save_poses2.py          # 位姿数据处理工具
├── picture/                # 图片资源
└── libs/                   # 辅助工具库
    ├── auxiliary.py        # 辅助函数
    └── log_setting.py      # 日志配置

核心原理

手眼标定的本质是求解坐标变换矩阵，两种模式分别对应不同的数学模型，上一章已经详细讲过其中的数学原理，这边我们简单复习一下：

1. 眼在手上模式（Eye-in-Hand）

需要求解相机相对于机械臂末端的变换矩阵 H C A M E E H^{EE}_{CAM} HCAMEE，核心方程为：
A 2 − 1 ⋅ A 1 ⋅ X = X ⋅ B 2 ⋅ B 1 − 1 A_2^{-1} \cdot A_1 \cdot X = X \cdot B_2 \cdot B_1^{-1} A2−1⋅A1⋅X=X⋅B2⋅B1−1

其中：

A A A 表示机械臂末端在基坐标系下的位姿变换
B B B 表示标定板在相机坐标系下的位姿变换
X X X 即为待求的相机与末端的变换矩阵

2. 眼在手外模式（Eye-to-Hand）

需要求解相机相对于机械臂基坐标系的变换矩阵 H C A M R O B H^{ROB}_{CAM} HCAMROB，核心方程同样遵循 A X = X B AX=XB AX=XB 形式，只是矩阵定义不同。

核心代码解析

一、核心计算模块深度解析

1. 眼在手上标定（compute_in_hand.py）

该模块是眼在手上模式的核心计算单元，负责将采集到的图像和位姿数据转换为相机与机械臂末端的坐标变换关系。

1.1 初始化与配置加载

python 复制代码

current_path = os.path.join(os.path.dirname(os.path.abspath(__file__)),"eye_hand_data")
images_path = os.path.join("eye_hand_data",find_latest_data_folder(current_path))
file_path = os.path.join(images_path,"poses.txt")

with open("config.yaml", 'r', encoding='utf-8') as file:
    data = yaml.safe_load(file)
XX = data.get("checkerboard_args").get("XX")  # 标定板长度方向角点数
YY = data.get("checkerboard_args").get("YY")  # 标定板宽度方向角点数
L = data.get("checkerboard_args").get("L")    # 角点间距（米）

路径处理 ：通过find_latest_data_folder自动定位最新采集的数据文件夹，确保使用最新标定数据
配置加载 ：从config.yaml读取标定板参数，支持不同规格标定板的灵活适配

1.2 标定板角点检测与相机外参计算

python 复制代码

# 标定板3D坐标定义（世界坐标系）
objp = np.zeros((XX * YY, 3), np.float32)
objp[:, :2] = np.mgrid[0:XX, 0:YY].T.reshape(-1, 2)
objp = L * objp  # 转换为实际物理尺寸

# 角点检测
for i in range(1, len(images_num) + 1):
    img = cv2.imread(image_file)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (XX, YY), None)
    if ret:
        obj_points.append(objp)
        # 亚像素优化：提高角点检测精度到亚像素级别
        corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
        img_points.append(corners2)

世界坐标系定义：将标定板视为世界坐标系，Z轴为0，X/Y轴由角点网格定义
亚像素优化 ：通过cv2.cornerSubPix将角点坐标从像素级优化到亚像素级（精度达0.1像素以下），为后续标定提供更高精度的输入

1.3 相机标定与位姿转换

python 复制代码

# 相机标定：获取标定板在相机坐标系下的位姿
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    obj_points, img_points, size, None, None)

# 机械臂位姿处理：将poses.txt转换为齐次变换矩阵
poses_main(file_path)
tool_pose = np.loadtxt("RobotToolPose.csv", delimiter=',')

# 提取旋转矩阵和平移向量
R_tool = []
t_tool = []
for i in range(int(N)):
    R_tool.append(tool_pose[0:3,4*i:4*i+3])  # 旋转矩阵部分
    t_tool.append(tool_pose[0:3,4*i+3])      # 平移向量部分

相机外参计算 ：cv2.calibrateCamera返回的rvecs和tvecs分别是标定板在相机坐标系下的旋转向量和平移向量
机械臂位姿转换 ：poses_main函数将机械臂末端的位姿（通常是X,Y,Z,_rx,ry,rz）转换为齐次变换矩阵，存储于RobotToolPose.csv

1.4 手眼标定核心计算

python 复制代码

# 使用Tsai算法求解手眼变换
R, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI)

参数解析 ：
- R_tool/t_tool：机械臂末端在基坐标系下的旋转矩阵/平移向量序列
- rvecs/tvecs：标定板在相机坐标系下的旋转向量/平移向量序列
- cv2.CALIB_HAND_EYE_TSAI：采用Tsai-Lenz算法（计算速度快，精度高）
输出：R为相机相对于机械臂末端的旋转矩阵，t为对应的平移向量

2. 眼在手外标定（compute_to_hand.py）

该模块与compute_in_hand.py结构高度相似，但存在关键差异：

2.1 核心差异点

python 复制代码

# 机械臂位姿处理函数不同
from save_poses2 import poses2_main
...
poses2_main(file_path)  # 替代compute_in_hand.py中的poses_main

位姿转换逻辑 ：poses2_main与poses_main的区别在于机械臂位姿的变换方向不同：
- 眼在手上：需要机械臂末端相对于基坐标系的变换（ A A A矩阵）
- 眼在手外：需要基坐标系相对于机械臂末端的变换（ A − 1 A^{-1} A−1矩阵）

2.2 标定目标差异

python 复制代码

# 眼在手外模式下，calibrateHandEye输出的是相机相对于基坐标系的变换
R, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI)

眼在手上： X = H C A M E E X = H^{EE}_{CAM} X=HCAMEE（相机→末端）
眼在手外： X = H C A M R O B X = H^{ROB}_{CAM} X=HCAMROB（相机→基坐标系）

二、辅助模块解析

1. 数据采集模块（collect_data.py）

该模块实现图像与位姿的同步采集，确保每一组数据的时间一致性：

python 复制代码

# 机械臂位姿获取
socket_command = '{"command": "get_current_arm_state"}'
state, pose = send_cmd(client, socket_command)

# 数据保存
with open(filename, 'a+') as f:
    pose_ = [str(i) for i in pose]
    new_line = f'{",".join(pose_)}\n'
    f.write(new_line)
cv2.imwrite(image_path, cv_img)

同步机制：通过按键触发（'s'键），确保同一时刻采集的图像和位姿被关联存储
数据格式 ：
- 位姿：poses.txt中每行存储一组机械臂位姿（X,Y,Z,rx,ry,rz）
- 图像：按序号命名（1.jpg, 2.jpg...），与位姿文件行号一一对应

2. 配置文件（config.yaml）

yaml 复制代码

checkerboard_args:
  XX: 9        # 长度方向角点数
  YY: 6        # 宽度方向角点数
  L: 0.02      # 角点间距（米）

标定板参数：需根据实际使用的标定板修改，直接影响世界坐标系到图像坐标系的转换精度
扩展性：可通过添加其他参数（如相机内参路径、日志级别等）增强配置灵活性

3. 日志模块（libs/log_setting.py）

python 复制代码

class CommonLog(object):
    def __console(self, level, message):
        # 文件日志（按天轮转）
        fh = TimedRotatingFileHandler(
            self.logname, when='MIDNIGHT', interval=1, encoding='utf-8')
        # 控制台日志
        ch = logging.StreamHandler()
        
        self.logger.addHandler(fh)
        self.logger.addHandler(ch)
        # 记录日志后移除处理器，避免重复输出
        self.logger.removeHandler(ch)
        self.logger.removeHandler(fh)

双重输出：同时向控制台和文件输出日志，方便调试和记录
日志轮转：按天分割日志文件，避免单文件过大，便于追溯历史标定过程

4. 位姿处理工具（save_poses.py / save_poses2.py）

虽然未提供完整代码，但根据上下文可推断其功能：

功能：将机械臂位姿（欧拉角）转换为齐次变换矩阵

核心公式 ：

python 复制代码

# 欧拉角转旋转矩阵（以ZYX顺序为例）
R = Rz(rz) * Ry(ry) * Rx(rx)
# 构造齐次矩阵
H = [[R[0][0], R[0][1], R[0][2], x],
     [R[1][0], R[1][1], R[1][2], y],
     [R[2][0], R[2][1], R[2][2], z],
     [0,       0,       0,       1]]

差异点 ：save_poses2.py会计算变换矩阵的逆，以适应眼在手外的数学模型

三、代码串联与整体流程

1. 数据流向图

复制代码

采集阶段：
机械臂 → [get_current_arm_state] → 位姿数据 → 写入poses.txt
   ↑
   同步触发（'s'键）
   ↓
相机 → [RealSense SDK] → 图像帧 → 保存为.jpg文件

计算阶段：
poses.txt → [save_poses.py/2] → 机械臂变换矩阵(R_tool, t_tool)
   ↓
.jpg文件 → [角点检测] → 图像点集 → [calibrateCamera] → 标定板位姿(rvecs, tvecs)
   ↓
[calibrateHandEye] → 手眼变换矩阵(R, t) → 输出旋转矩阵/平移向量/四元数

总结

本代码库提供了一套完整的手眼标定解决方案，通过模块化设计实现了数据采集、标定计算等核心功能。无论是眼在手上还是眼在手外模式，都能通过简单的操作流程完成标定。实际应用中，需注意机械臂运动姿态的多样性和标定板的稳定性，以获得更高的标定精度。

通过本文的解析，相信读者已经对该代码库有了全面的了解，可以根据实际需求进行二次开发或直接应用于机器人视觉项目中。