3. 相机模型与标定
在机器人 6D 位姿估计与抓取任务中，相机是把三维世界映射为二维图像的第一道“传感器关口”。如果相机模型不准确，后续的点云配准、位姿求解和抓取规划都会建立在错误几何假设上。本节围绕针孔成像、畸变建模、内外参标定与手眼标定四条主线，建立从像素到机器人基座坐标系的完整坐标链路。
一、针孔相机模型：从三维点到像素
针孔模型把相机近似为一个理想小孔成像系统。设相机坐标系下一点 P_c = (X_c, Y_c, Z_c)，其投影到归一化平面后为 (X_c/Z_c, Y_c/Z_c)，再经内参矩阵 K 映射到像素坐标：
u fx 0 cx X_c/Z_c
v = 0 fy cy · Y_c/Z_c
1 0 0 1 1
K = [[fx, 0, cx],
[0, fy, cy],
[0, 0, 1]]其中 fx、fy 是以像素为单位的焦距，cx、cy 是主点。内参描述“相机自身如何成像”，与相机在空间中的位置无关。若考虑世界坐标系，则需引入外参 R、t，把世界点变换到相机坐标系：P_c = R·P_w + t。因此完整投影链为：世界点 → 外参 → 相机坐标 → 内参 → 像素。
二、畸变模型：径向与切向修正
真实镜头并非理想针孔，广角镜头尤其明显。畸变主要分为径向畸变和切向畸变。径向畸变由镜头曲率引起，表现为直线变弯；切向畸变由镜头与成像平面不平行引起。常用 Brown-Conrady 模型：
x_dist = x(1 + k1·r² + k2·r⁴ + k3·r⁶) + 2·p1·x·y + p2·(r² + 2x²)
y_dist = y(1 + k1·r² + k2·r⁴ + k3·r⁶) + p1·(r² + 2y²) + 2·p2·x·y
r² = x² + y²其中 k1、k2、k3 为径向系数，p1、p2 为切向系数。去畸变时通常先对像素坐标反归一化，再迭代求解无畸变坐标。若忽略畸变，边缘区域的角点误差可达数像素，直接导致 PnP 位姿解算偏移。
三、内参外参标定：张正友标定法流程
最常用的内参标定方法是张正友平面标定法。它使用已知棋盘格或圆点阵列，从多张不同姿态图像中提取角点，利用平面单应性约束求解内参和畸变系数。典型流程如下：
- 打印标定板，固定于刚性平面，确保平整无翘曲。
- 从不同角度、不同距离拍摄 15～30 张图像，覆盖画面中心和四角。
- 提取角点亚像素坐标，剔除模糊、过曝或角点缺失的图像。
- 求解单应矩阵，线性估计内参初值，再用最大似然优化。
- 联合优化内参、畸变系数和外参，输出重投影误差。
外参标定则用于确定相机相对于某个已知坐标系（如标定板、机器人基座或末端）的位姿。若标定板固定在机器人基座坐标系中，可通过多组“基座-标定板”与“相机-标定板”关系求解相机到基座的外参。
| 参数类型 | 符号 | 物理含义 | 典型标定手段 |
|---|---|---|---|
| 内参 | fx, fy, cx, cy | 焦距与主点，像素单位 | 棋盘格多视角标定 |
| 畸变 | k1,k2,k3,p1,p2 | 径向与切向畸变系数 | 与内参联合优化 |
| 外参 | R, t | 相机相对世界/基座位姿 | 已知靶标或手眼标定 |
| 手眼 | X | 相机与机器人末端/基座关系 | AX=XB 求解 |
四、手眼标定：AX=XB 的原理与实战
手眼标定解决“相机看到的物体，如何转换到机器人坐标系”的问题。根据相机安装方式分为 eye-in-hand（相机固定在末端）和 eye-to-hand（相机固定在外部）。以 eye-in-hand 为例，控制机器人运动到多个位姿，每个位姿下拍摄固定标定板，构造方程：
A_i · X = X · B_i
A_i：相邻两次末端位姿之间的相对运动
B_i：相邻两次相机观测标定板的相对运动
X ：待求的相机到末端（或基座）的齐次变换求解时先由旋转部分解算 R_X，再代入平移部分求 t_X。实战中建议至少采集 10～15 组位姿，且旋转轴不要过于单一，否则方程退化。标定完成后，用未参与求解的验证位姿检查重投影误差和抓取点误差。
五、典型误区与复盘清单
- 标定板不平：打印纸贴在弯曲表面，角点共面假设被破坏，内参出现系统性偏差。
- 视角单一：所有图像都在同一距离和角度拍摄，焦距与主点耦合，标定结果不可靠。
- 忽略畸变：只标内参不标畸变，边缘抓取时像素误差被放大为厘米级偏移。
- 手眼旋转不足：机器人只做平移或绕单轴旋转，AX=XB 退化，解不稳定。
- 时间不同步：相机曝光与机器人位姿记录不同步，运动中标定产生拖影误差。
掌握相机模型与标定，本质上是建立一条可信的坐标变换链。针孔模型给出理想投影，畸变模型修正真实镜头，内外参标定确定几何关系，手眼标定打通相机与机器人。只有这条链路闭合且误差可控，后续的 6D 位姿估计与抓取才有稳定的几何基础。