📚 课程目录第3章外参标定基础

3、外参标定基础：外参（旋转矩阵、平移向量）的数学表示、欧拉角与四元数、齐次变换矩阵

激光雷达与车体、相机或 IMU 之间的空间关系，决定了点云能否正确映射到统一坐标系中。外参标定的本质，就是求解传感器坐标系之间的刚体变换关系。这种变换在数学上由旋转矩阵和平移向量共同描述，而工程实现中又常借助欧拉角、四元数和齐次变换矩阵来简化计算与存储。理解这些数学表示之间的内在联系与适用边界，是后续进行标定操作、误差分析和在线校准的前提。

3.1 旋转矩阵：刚体旋转的完整描述

旋转矩阵是描述三维空间中刚体旋转最基础、最严谨的数学工具。它是一个 3×3 的正交矩阵，满足 RᵀR = I 且 det(R) = +1。这意味着旋转矩阵的每一列（或每一行）都是单位向量，且两两正交。在激光雷达外参标定中，旋转矩阵直接表达了传感器坐标系相对于目标坐标系的姿态关系。

旋转矩阵的物理意义可以从列向量理解：R 的三列分别表示传感器坐标系的 X、Y、Z 轴在目标坐标系中的方向余弦。例如，若激光雷达坐标系相对于车体坐标系绕 Z 轴旋转了 θ 角，则对应的旋转矩阵为：

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

旋转矩阵的优点是唯一性——任意姿态对应唯一的旋转矩阵，不存在多解或歧义。同时，矩阵乘法可以直接实现旋转的复合运算。但它的缺点是参数冗余：9 个元素中只有 3 个独立自由度，且必须满足正交约束，这在优化求解时增加了约束处理的复杂度。

工程经验：在标定求解过程中，如果直接对旋转矩阵的 9 个元素进行优化，结果往往不满足正交性。实际工程中通常采用“先求旋转向量或四元数，再转换为旋转矩阵”的策略，或者使用流形优化（如李群李代数方法）来保证旋转矩阵的约束始终成立。

3.2 欧拉角与四元数：两种实用的旋转参数化方式

欧拉角将旋转分解为绕三个坐标轴的依次旋转，通常定义为 roll（绕 X 轴）、pitch（绕 Y 轴）、yaw（绕 Z 轴）。在车辆应用中，yaw 对应航向角，pitch 对应俯仰角，roll 对应侧倾角，这种直观的物理含义使得欧拉角在调试和故障排查时非常方便。例如，当标定结果显示 yaw 偏差为 0.5° 时，工程师可以立即判断是激光雷达安装时水平旋转未对齐。

然而欧拉角存在两个显著问题：旋转顺序依赖和万向锁。不同的旋转顺序（如 ZYX、ZYZ、XYZ）会产生不同的结果，必须明确约定。万向锁发生在中间旋转角为 ±90° 时，此时两个旋转轴重合，丢失一个自由度，导致姿态解算不稳定。因此，欧拉角适合用于人机交互和结果展示，但不适合作为内部计算和插值的核心表示。

四元数由 4 个实数组成 q = (w, x, y, z)，其中 w 为实部，(x, y, z) 为虚部，且满足 w² + x² + y² + z² = 1。四元数可以看作是一个标量加上一个三维向量，其乘法运算可以高效地实现旋转复合。与欧拉角相比，四元数没有万向锁问题，且插值（如球面线性插值 SLERP）平滑自然，非常适合用于标定过程中的姿态平滑和滤波。

表示方式	参数数量	约束条件	优点	缺点	适用场景
旋转矩阵	9	正交 + 行列式=1	唯一、无歧义、复合运算直接	参数冗余、约束处理复杂	最终结果存储、坐标变换计算
欧拉角	3	无（但需约定旋转顺序）	直观、物理意义明确	万向锁、顺序依赖	调试显示、人工读数
四元数	4	单位模长	无万向锁、插值平滑、紧凑	不够直观、需归一化	内部计算、滤波、优化求解

3.3 齐次变换矩阵：统一旋转与平移的数学工具

在实际标定中，旋转和平移总是同时存在的。齐次变换矩阵将 3×3 旋转矩阵和 3×1 平移向量组合为 4×4 矩阵，使得坐标变换可以统一表示为矩阵乘法：

T = [R  t;
     0  1]

其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。对于点 p = (x, y, z, 1)ᵀ，变换后的坐标为 p' = T·p。这种表示的核心优势在于：链式变换可以通过矩阵乘法直接复合。例如，激光雷达到车体的变换 T_lidar_to_vehicle，车体到世界的变换 T_vehicle_to_world，则激光雷达到世界的变换为：

T_lidar_to_world = T_vehicle_to_world × T_lidar_to_vehicle

齐次变换矩阵的逆矩阵也具有良好的形式：

T⁻¹ = [Rᵀ  -Rᵀ·t;
        0    1]

这避免了直接对 4×4 矩阵求逆的数值计算，提高了计算效率和数值稳定性。在标定过程中，我们经常需要将点从传感器坐标系变换到目标坐标系，再反向变换回来验证误差，齐次变换矩阵的逆运算简洁性在此发挥了重要作用。

  核心原理总结：旋转矩阵是旋转的“标准答案”，四元数是旋转的“高效计算格式”，欧拉角是旋转的“人类可读格式”，齐次变换矩阵则是将旋转和平移打包的“工程封装”。在标定流程中，通常使用四元数进行优化求解，用旋转矩阵进行坐标变换计算，用欧拉角进行结果展示和人工校验，最终以齐次变换矩阵的形式保存和传递外参。

3.4 案例拆解：从欧拉角到齐次变换矩阵的完整转换

某量产车型在装配激光雷达后，工程师通过倾角仪测得雷达相对于车体坐标系的安装姿态为：yaw = 2.5°，pitch = -1.2°，roll = 0.8°（旋转顺序为 ZYX，即先绕 Z 轴转 yaw，再绕 Y 轴转 pitch，最后绕 X 轴转 roll）。同时，激光雷达的光学中心在车体坐标系中的位置为 (0.15m, 0.0m, 1.2m)。

初始现象：直接使用这些测量值进行点云投影时，发现地面点云在车辆前方 30m 处出现约 0.4m 的横向偏移，且路面点云有轻微倾斜。

分析判断过程：横向偏移主要来源于 yaw 角的误差，而路面倾斜则与 pitch 和 roll 的偏差相关。倾角仪测量存在约 0.1°~0.3° 的安装面误差，且测量时雷达未通电，内部陀螺仪未参与校准，因此需要将测量值作为初值，通过标定算法进一步优化。

实际操作步骤：

将欧拉角 (roll=0.8°, pitch=-1.2°, yaw=2.5°) 按 ZYX 顺序转换为旋转矩阵 R。
将平移向量 t = (0.15, 0.0, 1.2) 与 R 组合为齐次变换矩阵 T。
将 T 作为初始外参，采集 20 帧包含地面和垂直墙面的点云数据。
使用地面平面拟合和墙面特征匹配进行优化，得到优化后的四元数 q = (0.9992, 0.0070, -0.0105, 0.0218) 和平移向量 t' = (0.152, -0.003, 1.198)。
将四元数转换回欧拉角，得到优化后的 yaw = 2.49°，pitch = -1.21°，roll = 0.79°，与测量值偏差在 0.05° 以内。

结果对比：优化后的外参将 30m 处的横向偏移从 0.4m 降低到 0.05m，路面点云倾斜角从 0.8° 降低到 0.1° 以内，满足 L2+ 级辅助驾驶的点云拼接精度要求。

风险与代价：倾角仪测量值虽然接近真值，但无法感知雷达内部 PCB 安装偏差和透镜光轴偏移，因此不能直接作为最终外参。若跳过优化步骤直接使用测量值，在高速场景下可能导致目标检测框抖动和测距偏差。

常见误区提醒：欧拉角转换时务必确认旋转顺序和旋转参考系（内旋 vs 外旋）。同一组欧拉角在不同旋转顺序下对应的旋转矩阵完全不同。在标定软件中，默认的旋转顺序必须与算法实现保持一致，否则会出现“标定结果正确但应用时点云错位”的隐蔽问题。

3.5 常见误区与总结复盘

在外参的数学表示环节，工程师常犯的错误集中在以下几个方面：

旋转矩阵非正交化：直接对 R 的 9 个元素做平均或滤波，导致结果不再是旋转矩阵。正确做法是使用四元数或旋转向量进行平均，再转换回旋转矩阵。
四元数未归一化：四元数在连续乘法后模长会漂移，必须定期归一化，否则会导致旋转尺度错误。
平移向量与旋转中心混淆：平移向量表示的是坐标系原点的位移，而非传感器物理中心的位置。当旋转中心与原点不重合时，需要先进行坐标平移再旋转。
齐次变换矩阵的维度错误：在代码实现中，点坐标必须补 1 维为齐次坐标 (x, y, z, 1)，否则矩阵乘法结果错误。

总结而言，外参的数学表示是标定工作的“语言”。旋转矩阵提供了严谨的数学基础，四元数提供了高效的计算手段，欧拉角提供了直观的调试视角，齐次变换矩阵则将这些统一为工程可用的标准格式。在实际项目中，建议以齐次变换矩阵作为外参的存储格式，以四元数作为优化变量，以欧拉角作为人工校验的辅助工具，从而在保证精度的同时兼顾开发效率。