3. 相机模型与标定：针孔相机模型、内参矩阵、畸变模型、张正友标定法
做视觉伺服，第一件事是什么？
不是调控制参数，也不是写代码。而是搞清楚你的眼睛——也就是相机——到底是怎么看世界的。
我见过不少新手，上来就调PID，结果图像坐标抖得跟筛子似的，还以为是控制器没调好。其实呢？是相机标定没做准。说白了，你连相机看到的点和真实世界的点之间是什么关系都没搞明白，后面的一切都是空中楼阁。
核心观点：相机模型是连接像素坐标与世界坐标的数学桥梁。标定就是给这座桥打地基。地基不稳，视觉伺服就是瞎忙活。
3.1 针孔相机模型——最朴素的成像原理
先说说最简单的模型：针孔相机。
你想想看，一个小黑盒子，前面戳个针眼大小的孔，光线穿过这个孔，在后面的感光面上成像。这就是针孔模型的原型。
数学上怎么描述？其实就是相似三角形。
假设世界坐标系中有一个点 P(X, Y, Z)，它在相机坐标系下的坐标为 (Xc, Yc, Zc)。这个点通过针孔投影到成像平面上，得到物理坐标 (x, y)：
x = f * (Xc / Zc)
y = f * (Yc / Zc)其中 f 是焦距。就这么简单。
但注意，这里有个坑。我刚开始做项目时，用的是一台工业相机，厂家给的焦距参数是 8mm。我直接套公式，结果算出来的坐标全偏了。后来才发现，厂家标的是镜头焦距，而针孔模型里的 f 是等效焦距，单位是像素。这两个东西差了十万八千里。
我的习惯：拿到新相机，第一件事就是重新标定。别信厂家给的参数，尤其是那些廉价工业相机，出厂标定数据经常不准。
3.2 内参矩阵——从物理坐标到像素坐标
刚才我们得到了成像平面上的物理坐标 (x, y)。但相机最终输出的是像素坐标 (u, v)。这中间还需要一个转换。
这个转换由内参矩阵 K 来完成：
K = [fx 0 cx
0 fy cy
0 0 1]其中：
- fx, fy：分别是 x 和 y 方向上的等效焦距（单位：像素）
- cx, cy：主点坐标，也就是光轴与成像平面的交点
完整的投影公式就是：
s * [u] = K * [R | t] * [X]
[v] [Y]
[1] [Z]
[1]这里 R 和 t 是外参，描述相机在世界坐标系中的位姿。s 是尺度因子。
为什么要强调内参矩阵？因为视觉伺服里，我们经常需要把图像上的像素误差转换成实际空间中的位姿误差。如果内参不准，这个转换就是错的。
我记得有一次调试一个抓取任务，机械臂总是差那么几毫米抓不到工件。折腾了两天，最后发现是 cx 偏了 3 个像素。重新标定后，问题立刻解决。嗯，这种教训一次就够了。
3.3 畸变模型——现实总是不完美的
针孔模型是理想情况。现实中的镜头，尤其是广角镜头，会有畸变。
畸变主要分两种：
- 径向畸变：光线通过镜头边缘时弯曲得更厉害。表现为图像边缘的直线变弯。
- 切向畸变：镜头和成像平面不平行导致的。表现为图像像被拉伸了一样。
数学上，我们用多项式来建模畸变：
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]其中 r^2 = x^2 + y^2，k1, k2, k3 是径向畸变系数，p1, p2 是切向畸变系数。
避坑指南：我曾经在一个项目中，用了畸变很大的鱼眼镜头，但只标定了 k1 和 k2，结果边缘区域的误差大到离谱。后来加上 k3 才勉强能用。对于畸变严重的镜头，建议至少标定到 k3。
另外，标定畸变时，棋盘格要尽量覆盖整个视野。我见过有人只拍了中间一小块区域，标定出来的畸变参数在边缘完全失效。你想想看，畸变模型是靠边缘数据来拟合的，你边缘都没数据，怎么可能拟合得准？
3.4 张正友标定法——工业界的标准答案
好了，前面讲了模型和参数。那怎么得到这些参数呢？
张正友标定法是目前最主流的方法。它的核心思想是：用一张已知尺寸的棋盘格，从不同角度拍多张照片，然后通过数学优化来求解内参和畸变参数。
具体步骤是这样的：
- 准备棋盘格：打印一张棋盘格，测量每个格子的实际尺寸（比如 30mm x 30mm）。
- 拍摄多张图片：从不同角度、不同距离拍摄棋盘格。我一般拍 15-20 张。角度要多样，有正面的、倾斜的、旋转的。
- 提取角点：用 OpenCV 的 findChessboardCorners 函数提取棋盘格的内角点。
- 求解内参：利用角点的像素坐标和世界坐标，通过最小二乘法求解内参矩阵。
- 优化畸变参数：用非线性优化方法（比如 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)
objp *= 30.0 # 每个格子 30mm
# 存储所有图片的角点
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)我的经验：标定时，图片数量不是越多越好。关键是角度要丰富。我试过拍 30 张几乎相同角度的图片，结果还不如 10 张角度多样的好。另外，棋盘格不要太小，否则角点提取精度会下降。
3.5 标定结果怎么用？
标定完成后，我们得到了内参矩阵 K 和畸变系数 dist。在视觉伺服中，这些参数有两个主要用途：
- 去畸变：用 cv2.undistort() 对实时图像进行去畸变处理，得到符合针孔模型的理想图像。
- 坐标转换：利用内参矩阵，将像素坐标转换为归一化相机坐标，再结合深度信息得到三维坐标。
举个例子，在视觉伺服中，我们通常用图像特征（比如目标中心的像素坐标）作为反馈。但控制律需要的是实际空间中的误差。这时候就需要用内参矩阵把像素误差转换成角度误差或位置误差。
我习惯在系统启动时先做一次去畸变，然后在整个伺服过程中使用去畸变后的图像。这样后续的所有计算都基于针孔模型，简单又可靠。
一句话总结：相机标定是视觉伺服的基础。针孔模型告诉你理想情况，畸变模型告诉你现实情况，张正友标定法帮你把两者统一起来。别嫌麻烦，这一步做扎实了，后面的控制调试会省心很多。
这张图把整个知识体系串起来了。从世界坐标开始，经过针孔模型、内参矩阵，再到畸变模型和标定方法，最后输出可用的图像和坐标。每一步都环环相扣。
好了，相机模型和标定就讲到这里。记住，这是视觉伺服的地基。地基打牢了，后面的控制参数整定才能有的放矢。