第2章相机成像与标定基础

2. 相机成像与标定基础

说到机器人抓取，第一步就是让机器人「看见」物体。怎么看见？靠相机。但相机看到的图像和真实世界之间，存在一个复杂的映射关系。搞懂这个映射，就是本章的核心。

我个人习惯把相机成像比作「用数学给世界拍一张照片」。你想想看，三维空间里的一个点，怎么变成二维像素的？这里面有四个关键环节：针孔模型、内外参数、畸变校正、标定方法。咱们一个一个来拆解。

2.1 针孔相机模型

针孔模型是最基础的成像模型。说白了，就是一个小孔让光线通过，在后面的感光面上成像。虽然简单，但它是所有复杂模型的基础。

数学上，一个三维点 P(X, Y, Z) 映射到图像平面上的点 p(x, y) 的关系是：

x = f * X / Z
y = f * Y / Z

其中 f 是焦距。这里有个关键点：Z 是深度，不是到相机的直线距离。我在项目中遇到过不少新手把 Z 理解成距离，结果坐标换算全错了。

核心思想：针孔模型是线性模型，但真实相机存在畸变，所以后面需要校正。

2.2 相机内参与外参

相机参数分两类：内参和外参。内参是相机「自己的事」，外参是相机「在哪儿、朝哪儿看」。

内参矩阵

内参矩阵 K 长这样：

K = [fx, 0, cx;
     0, fy, cy;
     0,  0,  1]

fx, fy 是焦距（单位像素），cx, cy 是主点坐标（图像中心）。嗯，这里要注意：fx 和 fy 通常不相等，因为像素不是正方形。

我曾经调试一个抓取系统，发现抓取点总是偏右。查了半天，原来是内参标定不准，cx 偏了 3 个像素。在近距离抓取时，3 个像素能导致几毫米的误差，足以让夹爪抓空。

外参矩阵

外参描述相机在世界坐标系中的位置和朝向。它由旋转矩阵 R 和平移向量 t 组成：

[R | t]

外参的作用，就是把世界坐标系的点转换到相机坐标系。你想想看，机器人要抓一个杯子，杯子在世界坐标系里有个坐标，但相机看到的是相机坐标系下的坐标。没有外参，这两个坐标系就对应不上。

我的经验：标定外参时，尽量让标定板覆盖整个视野。只标定中心区域，边缘误差会很大。

2.3 畸变模型

真实相机不是完美的针孔。镜头有畸变，主要是径向畸变和切向畸变。

径向畸变

就是「桶形」或「枕形」畸变。越靠近图像边缘，畸变越明显。数学模型是：

x_corrected = x * (1 + k1*r^2 + k2*r^4 + k3*r^6)
y_corrected = y * (1 + k1*r^2 + k2*r^4 + k3*r^6)

其中 r 是像素到主点的距离，k1, k2, k3 是畸变系数。

切向畸变

这是镜头和感光面不平行导致的。公式是：

x_corrected = x + [2*p1*x*y + p2*(r^2 + 2*x^2)]
y_corrected = y + [p1*(r^2 + 2*y^2) + 2*p2*x*y]

p1, p2 是切向畸变系数。

避坑指南：我曾经用一张标定板只拍了 5 张图就去算畸变系数，结果校正后图像反而更扭曲了。后来我总结：至少拍 15-20 张不同角度的图，畸变系数才稳定。

2.4 张正友标定法

张正友标定法是目前最主流的相机标定方法。它只需要一张棋盘格标定板，拍几张不同角度的照片，就能算出内参和畸变系数。

核心步骤：

检测角点：在每张图中找到棋盘格的角点
计算单应性矩阵：每张图都有一个从世界坐标到像素坐标的单应性矩阵 H
求解内参：利用多个 H 矩阵的约束，解出内参
求解外参：内参已知后，每张图的外参就迎刃而解
优化：用非线性最小二乘法（比如 Levenberg-Marquardt）优化所有参数

代码实现（OpenCV）：

import cv2
import numpy as np

# 准备棋盘格角点的世界坐标
pattern_size = (9, 6)  # 内角点数量
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1,2)

# 存储所有图像的世界坐标和像素坐标
objpoints = []  # 世界坐标
imgpoints = []  # 像素坐标

# 对每张图像检测角点
for fname in image_files:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        imgpoints.append(corners)

# 标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None
)

print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

实用建议：标定板要平整，不要用手捏着边缘。我习惯把标定板贴在硬纸板上，这样拍出来的角点更准。

2.5 手眼标定

手眼标定解决的是「相机和机器人手臂之间的坐标关系」。分两种：

Eye-in-Hand：相机装在机器人末端，跟着手臂动
Eye-to-Hand：相机固定在天花板或支架上，看手臂工作

Eye-in-Hand

相机装在手上，好处是视野灵活，可以凑近看细节。但每次手臂移动，相机坐标系就变了。需要求解的是：相机坐标系到末端坐标系的变换矩阵。

数学上，这是一个 AX = XB 问题。A 是两次拍摄之间机器人末端的位姿变化，B 是两次拍摄之间标定板的位姿变化，X 就是我们要的手眼矩阵。

Eye-to-Hand

相机固定，视野稳定，但可能被手臂遮挡。需要求解的是：相机坐标系到机器人基坐标系的变换矩阵。

这也是 AX = XB 问题，但 A 和 B 的含义不同。A 是两次拍摄之间标定板的位姿变化（在相机坐标系下），B 是两次拍摄之间机器人末端的位姿变化（在基坐标系下）。

我的选择：在抓取小零件时，我倾向用 Eye-in-Hand，因为可以靠近观察。抓取大件时，用 Eye-to-Hand 更稳，视野不会被手臂遮挡。

标定流程

在机器人末端装一个标定板（或用手眼标定专用的靶标）
控制机器人移动到多个不同位姿
在每个位姿记录：机器人关节角度（可算出末端位姿）和相机拍摄的标定板图像
用 OpenCV 的 calibrateHandEye() 求解

import cv2

# 假设已经得到了多组数据
# R_gripper2base: 末端到基座的旋转矩阵列表
# t_gripper2base: 末端到基座的平移向量列表
# R_target2cam: 标定板到相机的旋转矩阵列表
# t_target2cam: 标定板到相机的平移向量列表

R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
    R_gripper2base, t_gripper2base,
    R_target2cam, t_target2cam,
    method=cv2.CALIB_HAND_EYE_TSAI
)

print("手眼矩阵旋转部分:\n", R_cam2gripper)
print("手眼矩阵平移部分:\n", t_cam2gripper)

避坑指南：手眼标定最怕「退化运动」。就是机器人只在一个平面内移动，或者只旋转不移动。这样解出来的 X 不唯一。我建议：让机器人做 6 自由度的运动，平移和旋转都要有，至少 15 个位姿。

知识体系总览

下面这张图总结了本章的核心逻辑：

从这张图可以看出，整个知识体系是层层递进的。先理解成像模型，再掌握参数含义，然后学会标定方法，最后应用到机器人抓取中。每一步都离不开前面的基础。

我个人觉得，标定是机器人视觉里最「枯燥但最重要」的一环。很多项目后期出问题，追根溯源都是标定没做好。嗯，花时间把标定搞扎实，后面会省很多事。

📚 课程目录

2. 相机成像与标定基础

2.1 针孔相机模型

2.2 相机内参与外参

内参矩阵

外参矩阵

2.3 畸变模型

径向畸变

切向畸变

2.4 张正友标定法

2.5 手眼标定

Eye-in-Hand

Eye-to-Hand

标定流程

知识体系总览