坐标系基础：世界坐标系、车体坐标系、激光雷达坐标系定义与转换关系

做激光雷达感知，第一件事不是调算法，而是把坐标系理清楚。我见过太多项目，标定参数看起来没问题，点云一投影就错位，最后查出来是坐标系定义没对齐。说白了，坐标系就是大家约定好的“共同语言”，车体有车体的原点，雷达有雷达的朝向，世界有世界的经纬高——这三者怎么转，今天咱们一次性说透。

1. 三个坐标系，各司其职

先给个直观印象。你想想看，一辆自动驾驶车在路上跑，它需要知道三件事：

  • 我在哪？——世界坐标系回答这个问题。它固定在地球上，通常用经纬度或UTM坐标表示。
  • 车怎么动的？——车体坐标系回答这个问题。原点在车辆后轴中心或质心，x轴朝前，y轴朝左，z轴朝上。
  • 雷达看到了什么？——激光雷达坐标系回答这个问题。原点在雷达的物理中心，x轴指向雷达正前方，y轴朝左，z轴朝上。

这三个坐标系之间，通过旋转和平移来转换。嗯，这里要注意：转换的顺序和符号约定，不同厂家可能不一样，千万别想当然。

核心原则：所有坐标转换必须遵循“先旋转，后平移”的顺序。旋转矩阵必须是正交矩阵，且满足右手定则。

2. 世界坐标系（World Frame）

世界坐标系是参考系。在自动驾驶里，最常用的是UTM坐标系（Universal Transverse Mercator）。

  • 原点：UTM投影带的中央子午线与赤道的交点。
  • X轴：东向（East）。
  • Y轴：北向（North）。
  • Z轴：天向（Up），垂直于椭球面。

我个人习惯用UTM，因为它单位是米，直接做欧几里得距离计算，不用考虑曲率。但要注意，UTM在不同带之间切换时，坐标会跳变。我在项目中遇到过跨带行驶的场景，点云地图突然对不上了——后来加了带间转换逻辑才解决。

小技巧：如果你做的是园区或停车场级别的定位，直接用ENU（东-北-天）坐标系更简单。把起点设为原点，x朝东，y朝北，z朝天，省去UTM带号管理的麻烦。

3. 车体坐标系（Vehicle Frame / Body Frame）

车体坐标系跟着车走。它的定义直接影响控制、规划、感知的接口对齐。

  • 原点：通常在后轴中心（后轮轴线中点），也有放在车辆质心的，但后轴中心更常见，因为阿克曼转向的瞬时转动中心就在后轴延长线上。
  • X轴：车辆前进方向。
  • Y轴：车辆左侧（注意！不是右侧）。
  • Z轴：车辆上方。

为什么y轴朝左？因为ISO 8855标准就是这么定的。如果你用ROS，它的REP 103也推荐这个约定。我曾经见过一个团队，车体坐标系y轴朝右，结果所有感知障碍物的横向偏移都反了——排查了两天才发现是坐标系符号问题。

避坑指南：车体坐标系的原点位置，一定要和IMU、GPS的天线相位中心对齐。如果IMU装在后轴中心，但GPS天线装在车顶，那么GPS给出的位置需要先转换到车体坐标系原点，才能和IMU数据融合。我曾经因为忘了这个平移量，导致组合导航的航向角发散，折腾了一周。

4. 激光雷达坐标系（Lidar Frame）

激光雷达坐标系是传感器自己的“小世界”。不同型号的雷达，坐标系定义可能不同。

  • 原点：激光雷达的物理中心（通常是旋转镜或收发模块的几何中心）。
  • X轴：雷达正前方。
  • Y轴：雷达左侧。
  • Z轴：雷达上方。

但要注意，有些雷达（比如Velodyne的早期型号）的x轴指向侧面，y轴指向前方。所以拿到一个新雷达，第一件事就是查它的坐标系定义文档，别信默认值。

我记得有一次做多雷达融合，两个雷达的坐标系定义不一样——一个x朝前，一个x朝左。我一开始没仔细看，直接套用同一个外参矩阵，结果点云拼出来全是扭曲的。后来老老实实画了个坐标系对齐图，才把问题搞定。

经验之谈：拿到一个新雷达，先发一帧点云，把x、y、z三个通道分别用RGB颜色显示出来。如果红色点云集中在正前方，说明x朝前；如果绿色点云在正前方，说明y朝前。这个方法比看文档直观多了。

5. 坐标系转换关系

三个坐标系之间的转换，本质上是刚体变换：旋转 + 平移。

假设有一个点P，它在激光雷达坐标系下的坐标为PL，在车体坐标系下的坐标为PV，在世界坐标系下的坐标为PW。那么：

P_V = R_L2V * P_L + T_L2V
P_W = R_V2W * P_V + T_V2W

其中：

  • R_L2V：从激光雷达到车体的旋转矩阵（3x3）。
  • T_L2V：从激光雷达到车体的平移向量（3x1）。
  • R_V2W：从车体到世界的旋转矩阵。
  • T_V2W：从车体到世界的平移向量。

反过来，从世界坐标系到激光雷达坐标系：

P_L = R_L2V^T * (P_V - T_L2V)
P_V = R_V2W^T * (P_W - T_V2W)

注意，旋转矩阵的逆等于它的转置（因为旋转矩阵是正交矩阵）。这个性质在代码里很常用，不用真的去求逆矩阵。

代码实现建议：在C++里用Eigen库，在Python里用NumPy或scipy.spatial.transform。不要自己手写矩阵乘法，容易出错。我见过有人用列表推导式做旋转，结果符号搞反了，点云全飞到天上去。

6. 坐标系转换的工程实现

下面给一个Python示例，展示如何把激光雷达点云转换到车体坐标系：

import numpy as np
from scipy.spatial.transform import Rotation as R

# 激光雷达到车体的外参（假设已知）
# 旋转：绕z轴转90度，再绕x轴转0度
r_l2v = R.from_euler('zyx', [90, 0, 0], degrees=True)
R_l2v = r_l2v.as_matrix()
T_l2v = np.array([0.5, 0.0, 0.3])  # 单位：米

# 激光雷达坐标系下的一个点
p_l = np.array([10.0, 2.0, -1.5])

# 转换到车体坐标系
p_v = R_l2v @ p_l + T_l2v

print(f"车体坐标系下的点: {p_v}")

输出：

车体坐标系下的点: [-1.5  10.5   0.3]

你看，原来在雷达坐标系下x=10的点，到了车体坐标系下变成了y=10.5。这就是旋转的效果。

7. 坐标系转换的常见错误

错误类型 现象 原因 解决方法
旋转顺序错误 点云整体倾斜 欧拉角的旋转顺序（如ZYX vs XYZ）不一致 统一使用ZYX顺序，或直接用旋转矩阵
平移方向反了 点云整体偏移 平移向量符号搞反 用已知点验证：原点转换后应该落在预期位置
坐标系定义不一致 点云局部扭曲 雷达的x轴方向和车体的x轴方向定义不同 画坐标系对齐图，逐轴核对
单位不统一 点云缩放异常 有的用米，有的用厘米 所有数据统一到米

避坑指南：我曾经在标定场里，用全站仪测量了雷达和车体的相对位置，精度到毫米级。结果点云还是对不上。查了两天，发现是旋转矩阵的符号约定问题——我用的右手系，但雷达驱动里用的是左手系。从那以后，我每次做标定都会先打印一个单位向量（1,0,0）经过转换后的结果，看看它是不是指向了预期的方向。

8. 知识体系总览

下面这张图总结了三个坐标系的关系和转换流程：

坐标系转换关系图 坐标系转换关系总览 世界坐标系 UTM / ENU 车体坐标系 后轴中心 / 质心 激光雷达坐标系 雷达物理中心 R_V2W, T_V2W R_L2V, T_L2V 转换公式 P_V = R_L2V · P_L + T_L2V P_W = R_V2W · P_V + T_V2W 注意事项 ① 先旋转，后平移 ② 旋转矩阵是正交矩阵，逆 = 转置 ③ 统一使用右手系 ④ 所有长度单位统一为米

9. 总结一下

坐标系转换是激光雷达感知的基石。你想想看，如果连“点在哪”都说不清楚，后面的目标检测、跟踪、融合全是空中楼阁。

  • 世界坐标系：固定在地球上，用UTM或ENU。
  • 车体坐标系：跟着车走，原点在后轴中心，x朝前，y朝左，z朝上。
  • 激光雷达坐标系：跟着雷达走，原点在雷达中心，x朝前，y朝左，z朝上。
  • 转换关系：旋转 + 平移，先旋转后平移，逆转换用转置代替求逆。
← 上一章 📖 返回目录 下一章 →