📚 课程目录第3章标定数学基础

3、标定数学基础：坐标系变换、刚体变换、旋转矩阵、四元数、欧拉角、齐次坐标与矩阵运算，以及标定中常用的优化方法（最小二乘、LM算法）

模组标定的本质，是通过数学方法建立“像素坐标”与“物理空间坐标”之间的映射关系。无论是摄像头模组的内参标定、多传感器外参标定，还是产线机械手抓取定位，都离不开刚体变换与数值优化这两大数学支柱。本章将系统梳理坐标系变换的数学语言，从旋转矩阵、欧拉角到四元数，再到齐次坐标下的矩阵运算，最后深入标定中应用最广泛的最小二乘与Levenberg-Marquardt（LM）优化算法。理解这些内容，是搭建标定产线、开发标定工具、分析标定误差的前提。

上图展示了本章的知识架构：刚体变换是核心，旋转矩阵、欧拉角、四元数是三种等价的旋转描述方式；齐次坐标将旋转与平移统一为4×4矩阵，便于级联运算；而最小二乘与LM算法则是求解标定方程、优化重投影误差的数学工具。下面逐一展开。

3.1 坐标系变换与刚体变换

3.1.1 核心原理：从世界坐标系到相机坐标系

在模组标定中，我们至少涉及三个坐标系：世界坐标系（描述物体在空间中的位置）、相机坐标系（以光心为原点，Z轴指向镜头前方）、图像像素坐标系（以图像左上角为原点）。标定的第一步，就是确定世界坐标到相机坐标的刚体变换关系。

刚体变换只包含旋转和平移，不改变物体的形状和大小。其数学表达式为：

P_cam = R * P_world + t

其中，R是3×3旋转矩阵，t是3×1平移向量。R和t合称为相机的外参（Extrinsic Parameters），它们描述了相机在世界坐标系中的位姿。

3.1.2 判断方法：如何选择旋转表示方式

表示方式	优点	缺点	适用场景
旋转矩阵 R	数学性质好，可直接用于矩阵运算	9个参数，有正交约束，冗余	坐标变换计算、多坐标系级联
欧拉角 (α,β,γ)	直观，3个参数，易理解	万向锁问题，插值不自然	人机交互、参数显示
四元数 q	无万向锁，4个参数，插值平滑	不够直观，需转换	姿态解算、优化迭代、滤波

经验提醒：在标定工具内部，建议统一使用四元数或旋转矩阵进行运算；仅在用户界面显示时转换为欧拉角。避免在迭代优化过程中使用欧拉角，否则容易因万向锁导致雅可比矩阵奇异。

3.1.3 案例拆解：手眼标定中的刚体变换

在产线机械臂抓取模组的场景中，需要标定相机坐标系与机械臂基坐标系的关系。设相机固定在机械臂末端（Eye-in-Hand），则存在以下变换链：

Base_T_End * End_T_Cam = Base_T_Obj * Obj_T_Cam

其中，Base_T_End由机械臂控制器读出，End_T_Cam是待求的手眼矩阵，Base_T_Obj是目标物体在基坐标系下的位姿。通过移动机械臂到多个位置，采集标定板图像，可以构建AX = XB方程，利用最小二乘或分离求解法得到X。

关键输入数据：至少3组（实际建议8-12组）机械臂位姿（平移+旋转）和对应的标定板外参。旋转部分用四元数存储，避免欧拉角歧义。

3.1.4 常见误区与复盘

误区1：认为旋转矩阵的9个参数相互独立。实际上，R必须满足正交性（R^T R = I）且行列式为+1。在优化时若直接更新9个参数，会导致矩阵退化。
误区2：忽略平移向量的单位。世界坐标通常用毫米（mm），而相机坐标可能用米（m），需统一单位。
复盘：在搭建标定工具时，应封装一个RigidTransform类，内部存储四元数+平移向量，提供toMatrix()、compose()、inverse()等方法，避免底层数学错误。

3.2 旋转矩阵、欧拉角与四元数

3.2.1 核心原理：三种表示的数学定义与转换

旋转矩阵是SO(3)群上的元素，表示绕坐标轴的旋转组合。例如绕Z轴旋转θ的矩阵为：

Rz(θ) = [cosθ, -sinθ, 0;
         sinθ,  cosθ, 0;
         0,      0,    1]

欧拉角通常按Z-Y-X顺序定义（yaw-pitch-roll），旋转矩阵为R = Rz(yaw) * Ry(pitch) * Rx(roll)。但注意，旋转顺序不同，结果不同。

四元数定义为q = w + xi + yj + zk，满足||q||=1。旋转矩阵可由四元数转换得到：

R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
     [2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
     [2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]

3.2.2 操作流程：四元数到欧拉角的转换

从四元数提取旋转矩阵R。
计算pitch = asin(-R[2][0])（注意值域限制）。
计算yaw = atan2(R[1][0], R[0][0])。
计算roll = atan2(R[2][1], R[2][2])。
若pitch接近±90°，出现万向锁，此时yaw和roll无法唯一确定，需特殊处理。

风险说明：在标定结果输出时，若使用欧拉角，务必注明旋转顺序（如Z-Y-X）和角度单位（度/弧度）。否则，下游工艺人员可能误解标定结果，导致装配偏差。

3.2.3 实战场景：多相机外参标定中的旋转插值

在产线上，有时需要将多个相机的点云对齐到同一坐标系。假设相机A和相机B之间的相对旋转为R_AB，但标定结果存在噪声。若直接对R_AB的9个元素做平均，得到的矩阵不再是旋转矩阵。正确做法是：将R_AB转换为四元数，对四元数做归一化平均，再转回旋转矩阵。这能保证结果的合法性。

3.2.4 常见误区与总结

误区：四元数乘法顺序与旋转矩阵乘法顺序混淆。四元数q1 * q2表示先应用q2再应用q1，与矩阵R1 * R2一致。
总结：在标定工具中，建议所有旋转存储为四元数，仅在显示和日志输出时转换为欧拉角。这样既保证计算稳定性，又便于人工阅读。

3.3 齐次坐标与矩阵运算

3.3.1 核心原理：将旋转和平移统一为线性变换

为了将多次刚体变换级联，我们引入齐次坐标。将三维点P = (x, y, z)扩展为P_h = (x, y, z, 1)，则刚体变换可写为：

T = [[R(3×3), t(3×1)],
     [0(1×3), 1]]

这样，P_cam = T * P_world，且多个变换可以直接相乘：T_total = T1 * T2 * T3。这在标定链路中极为重要，例如：像素坐标 → 相机坐标 → 机械臂基坐标 → 世界坐标。

3.3.2 操作流程：矩阵运算的工程实现

使用Eigen、OpenCV或自定义矩阵库，确保矩阵乘法、求逆、转置的数值稳定性。
求逆时，利用R^T = R^{-1}的性质，T^{-1} = [[R^T, -R^T * t], [0, 1]]，避免通用求逆的数值误差。
级联变换时，注意左乘和右乘的顺序。例如，T_base_to_cam * T_cam_to_obj表示先变换到相机系，再变换到物体系。

3.3.3 案例拆解：内参标定中的投影矩阵

针孔相机模型下，世界点P_world到像素坐标p的投影为：

p = K * [R|t] * P_world

其中K是内参矩阵（焦距fx, fy，主点cx, cy）。在标定时，我们通常将[R|t]和K分开优化。但若使用齐次坐标，可以合并为3×4投影矩阵P = K * [R|t]，简化计算。不过，直接优化P会丢失R的正交约束，因此实际标定工具仍分开优化。

3.3.4 常见误区与复盘

误区：在C++中直接使用二维数组存储矩阵，导致索引错误。建议使用Eigen的Matrix4d类型。
复盘：在标定工具开发中，应编写单元测试验证矩阵求逆和级联的正确性，例如验证T * T^{-1} = I。

3.4 标定中的优化方法：最小二乘与LM算法

3.4.1 核心原理：从线性最小二乘到非线性优化

标定问题通常转化为最小化重投影误差：

min Σ || p_i - K * [R|t] * P_i ||²

当待求参数（内参、外参、畸变系数）与观测值呈线性关系时，可用线性最小二乘直接求解，例如通过SVD分解求超定方程Ax = b的最小二乘解。但实际标定中，相机模型包含畸变（径向、切向），导致误差函数非线性，必须使用非线性优化。

LM算法是高斯牛顿法和梯度下降法的结合。其迭代公式为：

(J^T J + λI) Δx = -J^T r

其中，J是雅可比矩阵，r是残差向量，λ是阻尼因子。当λ较大时，接近梯度下降，收敛慢但稳定；当λ较小时，接近高斯牛顿，收敛快但可能发散。LM算法通过自适应调整λ，兼顾收敛速度与稳定性。

3.4.2 判断方法：何时用线性最小二乘，何时用LM

问题特征	推荐方法	原因
方程线性，无畸变	SVD / 正规方程	一次求解，无需初值
有畸变，或参数耦合	LM算法	处理非线性，需良好初值
参数过多，观测不足	LM + 正则化	防止过拟合

3.4.3 实战场景：使用LM算法标定相机内参

假设使用棋盘格标定板，采集20张图像。初始估计内参（fx, fy, cx, cy）和畸变系数（k1, k2, p1, p2），以及每张图像的外参。LM算法的输入为：

残差 r_i = 检测到的角点 - 重投影的角点
雅可比 J = ∂r / ∂(内参, 外参, 畸变)

实际操作步骤：

使用张正友标定法获得初始内参和外参。
固定外参，优化内参和畸变（或全部联合优化）。
设置LM参数：初始λ=0.001，最大迭代50次，收敛阈值1e-8。
迭代后检查重投影误差，若大于0.5像素，则剔除误差最大的图像，重新优化。

结果对比：线性方法（仅估计内参，忽略畸变）的重投影误差约2.3像素；LM联合优化后降至0.3像素。代价是计算时间增加约40ms/张，但产线标定允许离线处理，可接受。

复盘经验：LM算法对初值敏感。若初值偏差过大，容易陷入局部极小。建议先用线性方法或张正友法获得初值，再使用LM精化。此外，在产线标定工具中，应加入异常值剔除机制（如RANSAC），避免个别错误角点破坏优化结果。

3.4.4 常见误区与总结

误区1：认为LM算法一定比梯度下降快。实际上，若问题高度非线性且初值差，LM可能因λ调整不当而震荡。
误区2：忽略雅可比矩阵的解析推导，使用数值差分。数值差分在参数尺度差异大时（如焦距约1000，畸变约0.1）会引入较大误差。建议使用Ceres或g2o等库的自动求导功能。
总结：标定优化的核心是“残差定义”和“参数化”。残差应使用像素坐标差（单位像素），参数化应使用四元数+平移向量，避免欧拉角。