3、坐标系与变换基础：世界坐标系、车辆坐标系、传感器坐标系、刚体变换、欧拉角、四元数、旋转矩阵与齐次坐标
多传感器标定的本质，是求解不同坐标系之间的空间变换关系。无论是激光雷达与相机的联合标定，还是毫米波雷达与惯导系统的外参标定，最终都要落到“将某个传感器坐标系下的点，精确映射到另一个坐标系（或统一的世界坐标系）下”这一数学问题上。因此，理解坐标系定义与刚体变换的数学表达，是进入标定工程实践的第一道门槛。本章将系统梳理从世界坐标系到传感器坐标系的层级关系，并深入对比旋转矩阵、欧拉角、四元数三种旋转表达方式的工程适用场景，最终通过齐次坐标将旋转与平移统一为矩阵运算，为后续章节的外参标定算法打下坚实的数学基础。
上图展示了智能驾驶系统中三个核心坐标系之间的层级关系。世界坐标系（W系）是全局参考基准，通常定义为车辆启动时的初始位置或高精地图的UTM坐标；车辆坐标系（V系）固连于车体，原点常设于后轴中心或车辆质心，x轴指向车辆前进方向，y轴指向驾驶员左侧，z轴垂直向上；传感器坐标系（S系）则固连于各个传感器，如激光雷达的光心、相机的光心或毫米波雷达的天线阵列中心。标定过程就是求解相邻坐标系之间的变换矩阵T，最终将传感器感知到的障碍物、车道线等目标统一映射到世界坐标系下，供规划与控制模块使用。
3.1 核心原理：坐标系定义与刚体变换的数学本质
刚体变换描述的是物体在三维空间中只发生旋转和平移、不发生形变的运动。在三维空间中，一个刚体具有6个自由度（Degree of Freedom, DoF）：3个旋转自由度（绕x、y、z轴）和3个平移自由度（沿x、y、z轴方向）。数学上，刚体变换可以表示为：
P' = R · P + t其中，P为原始坐标系下的三维点坐标（x, y, z），R为3×3旋转矩阵，t为3×1平移向量，P'为变换后坐标系下的坐标。为了将旋转和平移统一为线性运算，引入齐次坐标（Homogeneous Coordinates），将三维点扩展为四维向量（x, y, z, 1），变换矩阵扩展为4×4矩阵：
| P' | | R11 R12 R13 tx | | P |
| | = | R21 R22 R23 ty | · | |
| | | R31 R32 R33 tz | | |
| 1 | | 0 0 0 1 | | 1 |在智能驾驶标定中，旋转矩阵R必须满足正交性约束（R·Rᵀ = I，det(R) = 1），即旋转矩阵的逆等于其转置。这一性质使得旋转矩阵在坐标变换计算中非常高效，但9个元素之间存在6个约束条件，导致其不适合直接作为优化变量。因此，工程中通常使用四元数或欧拉角作为旋转的“参数化形式”，而旋转矩阵则作为“计算形式”。
3.2 判断方法：三种旋转表达方式的选型与转换
旋转矩阵、欧拉角、四元数各有优劣，工程选型需根据具体场景判断。下表从存储、计算、插值、奇异性等维度进行对比：
| 对比维度 | 旋转矩阵 R | 欧拉角（RPY） | 四元数 q |
|---|---|---|---|
| 参数数量 | 9个（冗余） | 3个（紧凑） | 4个（紧凑） |
| 物理直观性 | 差 | 好（俯仰/横滚/偏航） | 中 |
| 奇异性问题 | 无 | 有（万向锁） | 无 |
| 插值平滑性 | 差（需正交化） | 差（角度跳变） | 好（球面线性插值） |
| 计算效率 | 高（矩阵乘法） | 中（需三角函数） | 中（需归一化） |
| 工程应用场景 | 坐标变换计算 | 人机交互、姿态显示 | 优化求解、滤波融合 |
在标定工程中，判断选型的核心依据是：如果需要进行迭代优化或状态估计（如卡尔曼滤波），优先选择四元数；如果仅做一次性的坐标变换计算，旋转矩阵最直接；如果需要向工程师或客户展示姿态，欧拉角最直观。 此外，三者之间可以相互转换。例如，从四元数 q = (w, x, y, z) 转换为旋转矩阵的公式为：
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²)|从旋转矩阵提取欧拉角时，需要注意万向锁问题。当 pitch 角接近 ±90° 时，roll 和 yaw 的旋转轴重合，导致自由度丢失。因此，在标定外参时，应避免使用欧拉角作为优化变量，而应使用四元数。
3.3 案例拆解：激光雷达-相机联合标定中的坐标系变换
以典型的激光雷达与相机联合标定为例，目标是将激光雷达点云投影到相机图像上，实现像素级融合。整个流程涉及三个坐标系：激光雷达坐标系（L系）、相机坐标系（C系）、图像像素坐标系（I系）。标定需要求解两个变换：外参 T_CL（从激光雷达到相机）和内参 K（相机焦距、主点等）。
具体背景与约束： 某量产车型搭载了一颗前向128线激光雷达和一颗8MP前视摄像头，两者安装位置相距约30cm，存在明显的旋转和平移偏差。标定场地为室内标定间，墙面布置了棋盘格标定板。初始外参由CAD设计值提供，但实际安装公差导致投影误差达到15像素以上，无法满足感知融合需求（要求小于3像素）。
关键输入数据与参数：
- 激光雷达点云：每帧约12万点，包含x、y、z、intensity信息。
- 相机图像：分辨率3840×2160，内参 K = [fx=2800, fy=2800, cx=1920, cy=1080]（已标定）。
- 初始外参：平移向量 t = [0.3m, -0.05m, 0.1m]，旋转欧拉角 = [0.5°, -0.2°, 0.8°]（roll, pitch, yaw）。
分析判断过程： 首先，将激光雷达点云通过初始外参变换到相机坐标系：P_C = R_CL · P_L + t_CL。然后，通过相机内参投影到像素坐标：u = fx · X_C/Z_C + cx，v = fy · Y_C/Z_C + cy。计算投影点与图像中棋盘格角点的重投影误差，发现误差主要来源于旋转分量（尤其是yaw角偏差约2°）。由于欧拉角在优化中容易陷入局部最优，改用四元数作为优化变量，使用Levenberg-Marquardt算法最小化重投影误差。
实际操作步骤：
- 采集20帧包含棋盘格的点云与图像同步数据。
- 在图像中检测棋盘格角点，获取2D像素坐标。
- 在点云中提取棋盘格平面，计算角点对应的3D坐标（在L系下）。
- 构建优化问题：min Σ || u_measured - u_projected(q, t) ||²。
- 使用四元数参数化旋转，迭代优化，收敛后得到最优外参。
结果对比： 优化后，重投影误差从15.2像素降至1.8像素，满足感知融合要求。同时，将激光雷达点云投影到图像上，验证了障碍物边缘与图像轮廓的贴合度，目视检查无错位。
复盘经验： 本次标定中，初始值的选择对收敛结果影响显著。若初始外参误差过大（如平移偏差超过0.5m），优化容易陷入局部极小值。因此，工程上通常先用粗略的靶标（如标定板中心）进行粗标定，再使用精细的角点特征进行精标定，形成“粗-精”两级标定流程。
3.4 常见误区与总结复盘
在坐标系与变换基础的学习和工程应用中，以下误区值得特别注意：
- 误区一：混淆旋转矩阵的转置与逆。 旋转矩阵的正交性意味着 R⁻¹ = Rᵀ，但仅适用于纯旋转矩阵。若变换矩阵包含平移，则逆矩阵为 T⁻¹ = [Rᵀ, -Rᵀ·t; 0, 1]，不能简单转置。
- 误区二：欧拉角旋转顺序不统一。 不同车型或算法库可能采用不同的旋转顺序（如ZYX、ZXY、YXZ），导致同一组欧拉角对应不同的旋转矩阵。在跨团队协作时，必须明确约定旋转顺序，否则标定结果无法复用。
- 误区三：四元数未归一化。 四元数在优化迭代中可能因数值误差导致模长偏离1，此时旋转矩阵不再正交，坐标变换会产生形变。每次迭代后应强制归一化。
- 误区四：忽略传感器坐标系的原点定义。 例如，激光雷达坐标系原点可能在扫描中心，也可能在底部安装面；相机坐标系原点在光心，但光心位置与镜头安装面存在偏差。标定前必须确认各传感器数据手册中的坐标系定义。