📚 课程目录第3章标定坐标系与刚体变换基础

3. 标定坐标系与刚体变换基础

露天矿无人矿卡的感知标定，本质上是在回答一个问题：同一个物理点，在不同坐标系里分别怎么表示，如何精确互换。激光雷达扫到一块挡墙、相机拍到一块标识板、毫米波雷达回波给出一个目标，这些观测只有统一到同一参考框架下，才能用于融合、避障和装载点对准。本节围绕坐标系定义与刚体变换的数学表达展开，为后续外参标定、手眼标定和时间同步打下基础。

一、三个基础坐标系的定义

工程上通常约定三个层次分明的坐标系，并统一采用右手系：

世界坐标系（W）：固定于矿区场景，原点可设在卸载站基准点或矿区测量控制点上，Z 轴竖直向上，X/Y 轴按矿区主运输方向布置。它不随车辆运动，是全局地图、调度定位和高精地图的统一基准。
车体坐标系（B）：固连于车体，原点一般取后轴中心地面投影点或车辆几何中心，X 轴指向车头前进方向，Y 轴指向车身左侧，Z 轴向上。它描述“车自己怎么看周围”，是感知结果与车辆控制之间的桥梁。
传感器坐标系（S）：每个传感器独立定义，原点在传感器光心或扫描中心。激光雷达常取 X 向前、Y 向左、Z 向上；相机按光学惯例取 Z 向前、X 向右、Y 向下。不同厂商约定不同，标定前必须先核对手册，否则会出现“符号对、方向错”的隐蔽错误。

三者关系可概括为：传感器观测 → 经外参变换到车体系 → 经定位位姿变换到世界系。标定要确定的，正是这些变换中的旋转与平移量。

二、旋转的三种表达与选用

旋转矩阵 R 是 3×3 正交矩阵，满足 RᵀR=I、det(R)=1。它无奇异、可直接作用于向量、便于连乘复合，是计算与存储的首选。缺点是 9 个元素只有 3 个自由度，冗余且需正交化维护。

欧拉角 用三个角描述姿态，最直观，便于人工检查和界面输入。以车体系常用的 Z-Y-X 顺序（先绕 Z 偏航 yaw，再绕 Y 俯仰 pitch，最后绕 X 横滚 roll）为例，旋转矩阵为 R = Rz(ψ)·Ry(θ)·Rx(φ)。必须强调：欧拉角与旋转顺序强绑定，同一组角度换顺序结果完全不同；且 pitch 接近 ±90° 时存在万向节锁，不适合作为优化变量。

四元数 q = (w, x, y, z) 用 4 个参数表示旋转，无奇异、插值平滑（SLERP）、复合只需四元数乘法，是标定优化和位姿传输的主流形式。单位四元数满足 w²+x²+y²+z²=1。由轴角 (n, α) 构造：q = (cos(α/2), n·sin(α/2))。

实践中常见做法是：界面输入与人工核对用欧拉角，内部计算与优化用四元数或旋转矩阵，输出标定文件时同时给出两者并做一致性校验。

三、齐次变换与标定参数表达

把旋转和平移合并为 4×4 齐次变换矩阵，可将“旋转+平移”的复合写成一次矩阵乘法：

T = [ R  t ]      T_BS 表示传感器系到车体系的变换
    [ 0  1 ]      T_WB 表示车体系到世界系的变换（由定位给出）

点变换：P_B = T_BS · P_S ，  P_W = T_WB · P_B
链式复合：T_WS = T_WB · T_BS

注意方向约定：T_BS 的物理含义是“把 S 系下的坐标变到 B 系”，其平移 t 是 S 系原点在 B 系中的位置，旋转 R 是 S 系坐标轴在 B 系中的姿态。逆变换为 T⁻¹ = [Rᵀ, −Rᵀt; 0, 1]，无需重新求逆矩阵。标定文件中通常以 (x, y, z, roll, pitch, yaw) 六参数存储外参，加载时先转成 T 再参与计算，避免在公式里混用角度。

一致性检查：任取传感器系下一点 P_S，分别用“T_BS 直接变换”和“先转四元数再转旋转矩阵”两条路径计算 P_B，两者差值应在 1e-6 量级；若差异明显，多半是欧拉角顺序或四元数归一化出了问题。

四、一个可复现的数值示例

设某激光雷达相对车体系安装为：沿 X 前移 1.20 m、Y 左移 0.35 m、Z 高 1.85 m，俯仰向下 5°，其余角为 0。则 t = (1.20, 0.35, 1.85)，roll=0、pitch=−5°、yaw=0。按 Z-Y-X 顺序，R = Ry(−5°)，即

R = [ cos(-5°)  0  sin(-5°) ]   [ 0.9962  0  -0.0872 ]
    [    0      1      0     ] = [   0     1     0    ]
    [-sin(-5°)  0  cos(-5°) ]   [ 0.0872  0   0.9962 ]

若雷达在自身系下测得一点 P_S = (10, 0, 0)（正前方 10 m），则 P_B = R·P_S + t = (9.962+1.20, 0.35, 0.872+1.85) ≈ (11.16, 0.35, 2.72)。可以看到，5° 的俯仰让 10 m 处的点抬高了约 0.87 m，这正是安装角必须精确标定的原因——小角度误差在远距离会被放大成显著的高度偏差，直接影响障碍物高度判断与卸载对位。

五、标定参数如何落地表达

外参标定文件建议至少包含：传感器名称、父坐标系（通常为 B）、平移三参数（单位 m）、旋转表达（欧拉角或四元数，注明顺序与单位）、标定时间、残差指标。加载时统一转成 T 矩阵参与计算，并做一次往返一致性校验。对于多传感器系统，应保证所有传感器共享同一 B 系定义，否则融合结果会出现系统性偏移。

常见错误：把“传感器到车体”的变换与“车体到传感器”的变换混用。两者互为逆，平移量并非简单取负，而是 −Rᵀt。标定前先写清变换方向，并在代码注释和配置文件中显式标注，可避免大量返工。

掌握上述定义与变换规则后，后续的相机内参标定、激光雷达与相机外参联合标定、以及多传感器时空对齐，都可以归结为“求一个使重投影误差或点云配准误差最小的 T”。坐标系定义清晰、旋转表达统一、变换方向明确，是标定结果可复现、可维护的前提。