3. 相机模型与标定：针孔相机模型、内参矩阵、畸变模型、张正友标定法
做视觉伺服，第一件事是什么？
不是写控制律，也不是调PID。而是搞清楚你的眼睛——也就是相机——到底是怎么看世界的。
我见过太多工程师，上来就怼算法，结果伺服精度死活上不去。最后查来查去，发现是相机标定差了0.5个像素。嗯，0.5个像素在图像里看着不大，但在机械臂末端，可能就是几毫米的偏差。对于精密装配来说，这已经足够让工件报废了。
所以这一章，咱们把相机模型和标定这件事彻底聊透。
核心观点：相机标定的精度，直接决定了视觉伺服系统的控制上限。标定做不好，后面所有的控制算法都是在沙子上盖楼。
3.1 针孔相机模型——最朴素的成像原理
说白了，针孔相机模型就是初中物理的小孔成像。光线从物体表面反射，穿过一个小孔，投射到后面的感光平面上。
但这里有个关键点：实际相机不是真的用针孔，而是用透镜。透镜能收集更多光线，让成像更亮更清晰。但透镜也带来了麻烦——畸变。这个我们后面再说。
针孔模型的数学表达其实很简单。我习惯把它拆成三步：
- 世界坐标系到相机坐标系——刚体变换，旋转加平移
- 相机坐标系到图像坐标系——透视投影，用相似三角形
- 图像坐标系到像素坐标系——离散化，加上主点偏移
用公式写出来就是：
s * [u, v, 1]^T = K * [R | t] * [X, Y, Z, 1]^T其中K就是内参矩阵，[R|t]是外参。s是个尺度因子，因为从3D到2D丢失了深度信息。
我的经验：刚开始做标定时，我总搞不清s到底怎么来的。后来想明白了——它就是Zc，物体在相机坐标系下的深度。你标定板上每个角点的Zc都不一样，所以s是个变量。这个理解对了，后面解方程才不会乱。
3.2 内参矩阵——相机的“身份证”
内参矩阵K长这样：
K = [fx, 0, cx;
0, fy, cy;
0, 0, 1]fx和fy是焦距在像素单位下的表达。cx和cy是主点坐标，也就是光轴和成像平面的交点。
为什么fx和fy可能不一样？
因为像素不一定是正方形的。我遇到过一些工业相机，像素是矩形的，fx和fy能差好几个像素。如果你用fx=fy的假设去标定，后面做手眼标定的时候，误差会一路放大。
| 参数 | 物理含义 | 典型值范围 |
|---|---|---|
| fx | 焦距在x方向的像素当量 | 500~3000 |
| fy | 焦距在y方向的像素当量 | 500~3000 |
| cx | 主点x坐标 | 图像宽度的一半附近 |
| cy | 主点y坐标 | 图像高度的一半附近 |
注意：千万别以为cx、cy就是图像中心。实际生产中，由于装配误差，主点偏移几十个像素是常有的事。我曾经碰到过一台相机，主点偏了将近100个像素，用默认值去算，投影误差直接炸了。
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其中k1、k2、k3是径向畸变系数，p1、p2是切向畸变系数。r是点到主点的距离。
避坑指南：我曾经只用k1和k2去标定一个工业镜头，结果边缘区域的误差始终降不下来。后来加了k3，误差直接砍半。对于高精度视觉伺服，我建议至少用3个径向畸变参数。别省这点计算量，伺服精度更重要。
3.4 张正友标定法——最经典的标定方案
张正友老师在1998年提出的这个方法，到现在还是工业界的主流。为什么？因为它只需要一个棋盘格，拍几张照片，就能同时算出内参、外参和畸变系数。
核心思路是这样的：
- 打印一张棋盘格，贴在平面上
- 从不同角度拍10~20张照片
- 对每张照片，提取角点坐标
- 利用棋盘格的物理尺寸已知，建立方程
- 先忽略畸变，用线性方法求初始解
- 再用非线性优化（比如Levenberg-Marquardt）精调所有参数
我个人的习惯是拍15张左右。太少的话，参数解不稳定。太多的话，边际效益递减，还浪费时间。
关键技巧：拍照时，棋盘格要覆盖视野的各个区域——四个角、中心、边缘都要有。如果所有照片的棋盘格都在画面中央，那边缘的畸变参数根本标不准。我见过有人拍了20张，全在中间，标定结果一塌糊涂。
下面是我用Python实现的一个标定流程骨架：
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 = [] # 3D点
imgpoints = [] # 2D点
# 遍历图像
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)注意：cv2.findChessboardCorners这个函数对图像质量有要求。光照不均匀、棋盘格反光、或者图像模糊，都会导致角点检测失败。我建议拍照时用漫射光源，避免直射光。另外，棋盘格一定要平整，贴在弯曲的表面上标定，结果全是错的。
3.5 标定结果的验证
标定完了，怎么知道结果好不好？
我一般看两个指标：
- 重投影误差：把3D角点投影回图像，和检测到的2D角点比，算平均误差。小于0.5像素算合格，小于0.2像素算优秀。
- 畸变校正效果：拿一张有直线的场景图，校正后看直线是不是真的变直了。
如果重投影误差大于1个像素，我建议重新标定。别凑合，凑合的结果会在视觉伺服里给你颜色看。
我的经验：有一次标定结果重投影误差0.3像素，我觉得不错了。结果在伺服实验里，机械臂定位总是有0.5mm的偏差。后来发现是棋盘格打印精度不够，格子间距实际是24.8mm，我用了25mm。差这0.2mm，在标定里就变成了系统误差。所以，棋盘格的物理尺寸一定要精确测量，别信打印机的标称值。
3.6 本章知识体系
下面这张图，把相机模型和标定的核心逻辑串起来了。你可以把它当作一个快速索引：
这张图把整个标定流程串起来了。从针孔模型出发，理解内参矩阵和畸变模型，然后用张正友法求解，最后用重投影误差验证。每一步都环环相扣。
好了，相机模型和标定就聊到这儿。记住一句话：标定是视觉伺服的基石。这块功夫下足了，后面的控制才能稳得住。