3、坐标系统与变换：世界坐标系、车体坐标系、传感器坐标系、外参标定、坐标变换与刚体运动基础
在无人驾驶室外SLAM与路径规划系统中，坐标系统是描述车辆位置、环境感知和运动控制的数学基石。无论是激光雷达扫描到的点云、相机拍摄的图像，还是惯性测量单元（IMU）输出的加速度与角速度，所有传感器数据都必须被映射到统一的参考框架中，才能进行有效的融合、建图与规划。本章将系统梳理世界坐标系、车体坐标系与传感器坐标系之间的定义与关系，深入讲解外参标定的原理与操作流程，并夯实刚体运动与坐标变换的数学基础。理解这些内容，是后续实现高精度定位、实时建图与安全路径规划的前提。
上图清晰地展示了三种坐标系之间的层级关系与变换链路。世界坐标系为全局参考，车体坐标系描述车辆当前位姿，而传感器坐标系则承载着最原始的观测数据。外参标定建立了传感器到车体的刚性变换，而SLAM算法则负责估计车体在世界坐标系中的运动轨迹。下面我们将逐一深入这些核心概念。
3.1 三种核心坐标系的定义与作用
3.1.1 世界坐标系（World Frame）
核心原理：世界坐标系是一个固定不动的全局参考框架，用于描述环境中所有物体（包括车辆自身）的位置。在室外无人驾驶场景中，通常采用UTM（通用横墨卡托）坐标系或高斯-克吕格投影坐标系，以米为单位，X轴指向东，Y轴指向北，Z轴指向天顶（ENU）。该坐标系的原点可以设定在区域内的任意固定点，但一旦确定，在整个建图与定位过程中不再改变。
作用：它是全局地图的构建基准，也是路径规划中起点、终点和途经点的定义空间。所有局部感知数据最终都要转换到世界坐标系下进行累积与融合，形成一致的环境模型。
3.1.2 车体坐标系（Body Frame）
核心原理：车体坐标系与车辆本体固连，随车辆运动而运动。其原点通常定义在车辆后轴中心（对于阿克曼转向模型）或车辆质心（对于动力学控制）。按照ROS REP-103标准，X轴指向车辆前方，Y轴指向车辆左侧，Z轴垂直向上，构成右手坐标系。该坐标系是车辆运动控制、IMU测量和底盘状态反馈的直接参考系。
判断方法：在实车调试中，确认车体坐标系方向的最简单方法是：车辆水平停放，前进方向即为X正方向，左侧为Y正方向，上方为Z正方向。所有传感器外参标定结果都是相对于这个车体坐标系定义的。
3.1.3 传感器坐标系（Sensor Frame）
核心原理：每个传感器都有自己独立的坐标系，用于描述其原始测量数据。例如：
- 激光雷达（LiDAR）：原点位于激光发射中心，X轴向前，Y轴向左，Z轴向上（或根据具体型号定义）。点云数据中的每个点坐标都是相对于此坐标系。
- 相机（Camera）：原点位于相机光心，Z轴指向相机前方（沿光轴），X轴向右，Y轴向下（图像坐标系）。
- IMU：原点位于MEMS芯片中心，三轴方向与芯片封装上的标记一致，输出加速度和角速度。
传感器坐标系的定义直接影响外参标定的参数形式和点云/图像的投影计算。
3.2 外参标定：建立传感器与车体的刚性连接
3.2.1 标定的数学本质
外参标定是求解传感器坐标系（S系）到车体坐标系（B系）的刚体变换矩阵 T_B_S 的过程。该矩阵包含一个3×3的旋转矩阵 R 和一个3×1的平移向量 t，齐次形式为：
T_B_S = [ R t ]
[ 0 1 ]其中R描述了传感器相对于车体的姿态（俯仰、横滚、偏航角），t描述了传感器安装位置相对于车体原点的三维偏移量。
3.2.2 标定操作流程（以LiDAR-车体外参为例）
- 准备标定场地：选择空旷平坦区域，放置3-5个已知世界坐标的标定靶标（如反光柱、黑白棋盘格或特定形状的标定板）。
- 数据采集：车辆静止，采集至少10秒的LiDAR点云数据，确保靶标在点云中清晰可见。
- 特征提取：从点云中手动或自动提取靶标的中心点坐标（在S系下）。
- 对应点匹配：将靶标中心点的S系坐标与其在车体坐标系下的已知坐标（通过卷尺或全站仪测量）进行配对。
- 求解变换：使用SVD（奇异值分解）或优化算法（如Levenberg-Marquardt）求解最优的R和t，使得配对点之间的均方误差最小。
- 验证：将标定后的点云投影到车体坐标系，检查靶标位置是否与实测值吻合，误差应小于5cm。
| 标定方法 | 所需设备 | 精度范围 | 适用场景 |
|---|---|---|---|
| 靶标法（手动） | 标定板、全站仪 | ±2-5 cm | 量产车下线标定 |
| 自然特征法 | 无（利用环境特征） | ±5-10 cm | 在线自标定 |
| 手眼标定法 | 机械臂或车辆运动 | ±1-3 cm | 研发阶段高精度标定 |
3.3 坐标变换与刚体运动基础
3.3.1 刚体变换的数学表达
刚体运动是指物体在运动过程中保持其内部任意两点之间的距离不变。在三维空间中，刚体变换由旋转和平移组成。旋转矩阵R是正交矩阵（R^T R = I，det(R)=1），具有三个自由度，常用欧拉角（roll, pitch, yaw）、旋转向量或四元数表示。平移向量t具有三个自由度，因此外参标定总共需要求解6个参数。
坐标变换公式：若已知某点在S系下的坐标 p_s，则其在B系下的坐标 p_b 为：
p_b = R * p_s + t若需要转换到世界坐标系W系，则还需乘上车辆位姿 T_W_B：
p_w = T_W_B * p_b = T_W_B * T_B_S * p_s3.3.2 变换的链式法则与逆变换
在SLAM系统中，经常需要处理多个坐标系之间的连续变换。例如，将LiDAR点云转换到世界坐标系，需要经过 S→B→W 两级变换。链式法则为：
T_W_S = T_W_B * T_B_S逆变换用于将世界坐标下的点映射回传感器坐标系，其计算方式为：
T_S_W = inv(T_W_S) = [ R^T -R^T * t ]
[ 0 1 ]3.4 实战案例：多传感器融合前的坐标统一
案例背景：某无人驾驶项目同时搭载64线LiDAR、前视相机和IMU。LiDAR用于建图，相机用于目标识别，IMU用于运动估计。初始阶段，算法工程师发现目标检测框在图像上的位置与LiDAR点云投影位置存在约0.5米的系统性偏差，导致融合后的目标位置抖动严重。
分析过程：首先检查时间同步，确认各传感器时间戳对齐。然后检查外参标定文件，发现LiDAR的外参是半年前在上一辆测试车上标定的，而当前车辆重新安装了LiDAR支架，安装位置发生了约10cm的位移和2°的俯仰角变化。
操作步骤：
- 重新进行LiDAR-车体外参标定，使用靶标法，采集20秒静态点云数据。
- 提取3个靶标中心点，与实测车体坐标进行SVD求解。
- 更新外参文件中的R和t值。
- 将LiDAR点云投影到相机图像上，验证投影误差从0.5米降至0.08米。
- 重新运行融合算法，目标位置抖动幅度减小80%。
结果与复盘：通过重新标定，系统融合精度显著提升。复盘发现，项目缺乏外参变更管理流程，任何涉及传感器拆装的维护操作后，必须强制重新标定。此外，建议在车辆上设计固定安装定位销，减少重复安装带来的位置不确定性。
3.5 常见误区与总结复盘
- 误区一：混淆坐标系方向定义。不同传感器厂商的坐标系定义可能不同（如LiDAR的Z轴向上或向下），标定前必须仔细阅读手册，统一转换到ROS标准。
- 误区二：忽略时间同步。坐标变换只解决空间对齐问题，若传感器数据时间戳不一致，车辆运动会导致动态误差。必须同时进行时间同步（硬件同步或插值算法）。
- 误区三：外参标定一次到位。车辆运行过程中的振动、温度变化可能导致外参漂移，建议定期（如每季度）或异常时重新标定。
- 误区四：使用错误的旋转顺序。欧拉角旋转顺序（如ZYX、ZXY）不同，对应的旋转矩阵也不同。在代码中必须明确指定旋转顺序，否则会产生不可预知的姿态误差。