2. 相机模型与标定

相机标定，说白了就是搞清楚「相机到底怎么看世界的」。我刚开始做机器人抓取时，总觉得标定是件麻烦事，随便买个标定板拍几张就完事了。结果呢？抓取误差直接偏了 5 厘米，工件死活抓不准。嗯，从那以后我再也不敢糊弄标定了。

这一章，我们就把相机模型和标定这件事彻底讲透。从针孔模型到畸变校正，从张正友标定到手眼标定，每一步我都会结合项目里的实际坑点来讲。

2.1 针孔相机模型

针孔模型是相机成像的基础。你想想看，光线通过一个小孔投射到成像平面上，这就是最朴素的成像原理。

数学上，我们用四个坐标系来描述这个过程：

• 世界坐标系（World）：物体在真实空间中的位置，单位 m
• 相机坐标系（Camera）：以相机光心为原点，Z 轴指向镜头前方
• 图像坐标系（Image）：成像平面上的物理坐标，单位 mm
• 像素坐标系（Pixel）：图像上的整数像素坐标

从世界坐标到像素坐标，本质上是三次变换的串联：

P_pixel = K * [R | t] * P_world

其中 K 是内参矩阵，[R | t] 是外参矩阵。内参决定了相机本身的属性，外参决定了相机在世界中的位姿。

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

2.2 畸变模型

现实中的镜头不是完美的针孔。透镜的制造误差和装配误差会引入畸变。最常见的两种是：

• 径向畸变：光线通过透镜边缘时弯曲程度不同，产生「桶形」或「枕形」畸变
• 切向畸变：透镜与成像平面不平行，产生「倾斜」效果

畸变校正的数学模型是这样的：

x_distorted = x * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2)
y_distorted = y * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y^2) + 2*p2*x*y

其中 k1, k2, k3 是径向畸变系数，p1, p2 是切向畸变系数。r 是像素点到光心的距离。

2.3 张正友标定法

张正友标定法是目前最主流的相机标定方法。它的核心思路是：用一张棋盘格，从不同角度拍 10-20 张照片，然后通过数学优化求解内参和畸变参数。

我个人习惯的标定流程是这样的：

1. 准备标定板：打印一张 7×9 或 9×11 的棋盘格，贴在平整的硬板上
2. 采集图像：从不同角度拍摄 15-20 张照片，覆盖视野的各个区域
3. 角点检测：用 OpenCV 的 findChessboardCorners 提取角点
4. 求解内参：基于角点坐标和已知的棋盘格尺寸，用最小二乘法求解
5. 优化畸变：用非线性优化（如 Levenberg-Marquardt）进一步精化所有参数

下面是一个完整的标定代码示例：

import cv2
import numpy as np
import glob

# 棋盘格尺寸（内角点数量）
pattern_size = (9, 6)
square_size = 25.0  # 单位 mm

# 准备世界坐标系中的点
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)
objp *= square_size

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

images = glob.glob('calib_images/*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    
    if ret:
        objpoints.append(objp)
        # 亚像素精度优化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        imgpoints.append(corners2)

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

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

2.4 手眼标定

相机标定完成后，我们知道了「相机怎么看世界」。但机器人抓取还需要知道「相机和机械臂之间的相对位姿」。这就是手眼标定要解决的问题。

根据相机安装位置的不同，分为两种模式：

• Eye-in-Hand：相机固定在机械臂末端，随机械臂移动
• Eye-to-Hand：相机固定在工作空间上方，不随机械臂移动

两种模式的核心方程不同：

其中 A 是机械臂末端位姿的变化量，B 是标定板在相机坐标系中的位姿变化量，X 就是我们要找的手眼变换矩阵。

下面是一个 Eye-in-Hand 标定的核心代码片段：

import cv2
import numpy as np

def hand_eye_calibration(R_gripper2base, t_gripper2base,
                         R_target2cam, t_target2cam):
    """
    Eye-in-Hand 手眼标定
    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
    )
    return R_cam2gripper, t_cam2gripper

2.5 本章知识体系

下面这张图总结了相机模型与标定的完整知识结构：

从图中可以看出，相机模型是基础，畸变校正是预处理，张正友标定法解决内参和外参，而手眼标定则把相机和机器人系统连接起来。每一步都环环相扣，缺一不可。