眼在手上内参标定

ZPC8210

101人浏览 · 2026-06-04 14:51:48

ZPC8210 · 2026-06-04 14:51:48 发布

眼在手上（相机固定在机械臂末端）手眼标定代码逐行详细解释

这是眼在手上（Eye-in-Hand）的手眼标定核心代码，作用是：计算相机坐标系相对于机械臂末端（工具端）的固定旋转矩阵和平移向量，这是机械臂 + 视觉实现精准抓取的核心步骤。

我会分模块、逐函数、逐关键行完整解释，让你完全理解每一步的作用。

一、文件头与导入库

python

运行

# coding=utf-8
"""
眼在手内 用采集到的图片信息和机械臂末端位姿信息计算相机坐标系相对于机械臂末端的旋转矩阵和平移向量
"""

import os.path
import cv2
import numpy as np
import csv
np.set_printoptions(precision=8,suppress=True)

解释

# coding=utf-8：指定文件编码为 UTF-8，支持中文注释。
文档字符串：说明代码功能 ——眼在手上标定，求相机→机械臂末端的位姿。
导入库：
- os.path：判断图片 / 文件是否存在
- cv2：OpenCV，视觉标定、角点检测核心库
- numpy：矩阵运算（旋转矩阵、齐次变换）
- csv：保存位姿矩阵到 CSV 文件
np.set_printoptions：设置 numpy 输出8 位小数、不使用科学计数法，方便查看结果。

二、欧拉角 → 旋转矩阵核心函数

python

运行

def euler_angles_to_rotation_matrix(rx, ry, rz):
    # 计算旋转矩阵
    Rx = np.array([[1, 0, 0],
                   [0, np.cos(rx), -np.sin(rx)],
                   [0, np.sin(rx), np.cos(rx)]])
    Ry = np.array([[np.cos(ry), 0, np.sin(ry)],
                   [0, 1, 0],
                   [-np.sin(ry), 0, np.cos(ry)]])
    Rz = np.array([[np.cos(rz), -np.sin(rz), 0],
                   [np.sin(rz), np.cos(rz), 0],
                   [0, 0, 1]])
    R = Rz@Ry@Rx  # 先绕 z轴旋转 再绕y轴旋转 最后绕x轴旋转
    return R

核心作用

把机械臂给出的欧拉角（rx,ry,rz） 转换成旋转矩阵 R（3×3）。

关键解释

Rx/Ry/Rz：分别是绕 X、Y、Z 轴的基础旋转矩阵。
R = Rz@Ry@Rx：旋转顺序 Z→Y→X（固定坐标系），这是绝大多数机械臂的欧拉角定义规则。
输出：3×3 旋转矩阵，描述姿态。

三、位姿 → 齐次变换矩阵

python

运行

def pose_to_homogeneous_matrix(pose):
    x, y, z, rx, ry, rz = pose
    R = euler_angles_to_rotation_matrix(rx, ry, rz)
    t = np.array([x, y, z]).reshape(3, 1)
    H = np.eye(4)
    H[:3, :3] = R
    H[:3, 3] = t[:, 0]
    return H

核心作用

把机械臂末端 6 维位姿（x,y,z,rx,ry,rz） 转换成4×4 齐次变换矩阵 H。

齐次矩阵格式：H=[R0t001]

解释

解包位姿：x,y,z = 平移；rx,ry,rz = 旋转
调用上一个函数得到旋转矩阵 R
构造平移向量 t
生成 4×4 单位矩阵，填入 R 和 t，得到完整位姿矩阵

四、矩阵保存 CSV 工具函数

python

运行

def save_matrices_to_csv(matrices, file_name):
    rows, cols = matrices[0].shape
    num_matrices = len(matrices)
    combined_matrix = np.zeros((rows, cols * num_matrices))
    for i, matrix in enumerate(matrices):
        combined_matrix[:, i * cols: (i + 1) * cols] = matrix
    with open(file_name, 'w', newline='') as csvfile:
        csv_writer = csv.writer(csvfile)
        for row in combined_matrix:
            csv_writer.writerow(row)

作用

把多张4×4 齐次矩阵拼接成一个大矩阵，保存到 CSV 文件，方便后续读取。

五、机械臂位姿文本 → CSV 矩阵

python

运行

def poses_save_csv(filepath):
    with open(f'{filepath}', "r",encoding="utf-8") as f:
        lines = f.readlines()
    lines = [float(i)  for line in lines for i in line.split(',')]
    matrices = []
    for i in range(0,len(lines),6):
        matrices.append(pose_to_homogeneous_matrix(lines[i:i+6]))
    save_matrices_to_csv(matrices, 'robotToolPose.csv')

核心流程

读取poses.txt：每行是6 个数字（x,y,z,rx,ry,rz）
把所有数字转成浮点数
每 6 个数字一组，转换成4×4 齐次矩阵
保存所有矩阵到robotToolPose.csv

用途

给后面的手眼标定提供机械臂末端位姿数据。

六、主标定函数：compute_T（核心！）

这是整个代码的灵魂，完成视觉标定 + 手眼标定。

1. 初始化参数

python

运行

def compute_T(images_path,corner_point_long,corner_point_short,corner_point_size):
    print("标定板参数")
    criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)

criteria：亚像素角点优化的停止条件（迭代 30 次 / 误差 0.001）。

2. 生成标定板 3D 世界坐标

python

运行

objp = np.zeros((corner_point_long * corner_point_short, 3), np.float32)
objp[:, :2] = np.mgrid[0:corner_point_long, 0:corner_point_short].T.reshape(-1, 2)    
objp = corner_point_size*objp

建立标定板坐标系：Z=0，X/Y 按棋盘格排布
单位：米（真实物理尺寸）

3. 遍历图片，检测角点

python

运行

obj_points = []     # 存储3D点
img_points = []     # 存储2D点
for i in range(0, 50):
    image = f"{images_path}/{i}.png"
    if os.path.exists(image):
        img = cv2.imread(image)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, (w,h), None)
        if ret:
            obj_points.append(objp)
            corners2 = cv2.cornerSubPix(gray, corners, (5,5), (-1,-1), criteria)
            cv2.drawChessboardCorners(img, ...)
            cv2.imshow(...)
            img_points.append(corners2)

关键步骤

读取每张标定板图片
转灰度图
检测棋盘格角点（2D 像素坐标）
亚像素优化：提高精度
保存：
- obj_points：标定板 3D 真实坐标
- img_points：图片 2D 像素坐标

4. 相机内参标定

python

运行

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(...)
print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

得到相机内参（焦距、主点）
得到畸变系数（去畸变用）
得到每张图片：标定板→相机的位姿（rvecs 旋转向量、tvecs 平移向量）

5. 读取机械臂末端位姿

python

运行

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])

从 CSV 读取所有机械臂末端→基坐标系的位姿
拆分出旋转矩阵R_tool和平移向量t_tool

6. 旋转向量 → 旋转矩阵

python

运行

rotation_matrices = []
for rvec in rvecs:
    rotation_matrix, _ = cv2.Rodrigues(rvec)
    rotation_matrices.append(rotation_matrix)

OpenCV 标定输出是旋转向量，必须转成3×3 旋转矩阵才能用于手眼标定。

七、手眼标定（核心算法）

python

运行

# TSAI方法
R_cam2gripper_tsai, t_cam2gripper_tsai = cv2.calibrateHandEye(
    R_tool, t_tool, 
    rotation_matrices, tvecs,
    method=cv2.CALIB_HAND_EYE_TSAI
)

# PARK方法
# HORAUD方法

最重要的输入含义（眼在手上）

python

运行

cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,  # 机械臂末端 → 基坐标系（机械臂给出）
    R_target2cam, t_target2cam,      # 标定板 → 相机（视觉算出）
    method=...
)

输出（最终结果！）

R_cam2gripper：相机坐标系 → 机械臂末端 的旋转矩阵
t_cam2gripper：相机坐标系 → 机械臂末端 的平移向量

这就是你最终需要的手眼标定结果！

八、主程序入口

python

运行

if __name__ == '__main__':
    images_path = "img"        # 标定板图片路径
    file_path = "poses.txt"    # 机械臂位姿
    corner_point_long=11      # 棋盘格长边角点
    corner_point_short=8
    corner_point_size=0.025   # 方格尺寸m

    poses_save_csv(file_path)  # 位姿转CSV
    R, t = compute_T(...)     # 执行标定
    print(R)
    print(t)

执行流程

设置标定板参数、文件路径
把机械臂位姿文本转成矩阵 CSV
执行视觉 + 手眼标定
打印相机→末端的旋转矩阵 + 平移向量

整体工作原理（一句话总结）

相机固定在机械臂末端，移动机械臂拍摄多张标定板
视觉算出：标定板 → 相机 的位姿
机械臂给出：末端 → 基坐标系 的位姿
用calibrateHandEye求解：相机 → 末端 的固定位姿
得到结果后，机械臂就能通过相机知道物体的真实位置

关键名词速查

表格

名词	含义
眼在手上	相机固定在机械臂末端
齐次矩阵	4×4 矩阵，同时表示旋转 + 平移
内参矩阵	相机自身参数（焦距、主点）
手眼标定	求相机和机械臂末端的固定位姿关系
R	旋转矩阵（3×3）
t	平移向量（3×1）