坐标系基础：世界坐标系、车身坐标系、传感器坐标系、坐标变换原理
大家好，我是你们的老朋友。今天咱们来聊聊坐标系。
说实话，我刚入行那会儿，觉得坐标系这东西太简单了，不就是三个轴嘛。结果第一次做实车测试，传感器数据怎么都对不上，折腾了一整天。后来才发现，是坐标系搞混了。嗯，从那以后，我再也不敢小看坐标系了。
坐标系，说白了就是给空间里的每个点一个「身份证」。在自动驾驶里，我们至少需要三套身份证系统：世界坐标系、车身坐标系、传感器坐标系。它们之间怎么换算？这就是坐标变换要干的事。
1. 世界坐标系：全局的「锚点」
世界坐标系，也叫全局坐标系。你可以把它想象成整个地球的经纬度系统。不管车跑到哪儿，这个坐标系是固定不动的。
我习惯用 W系 来称呼它。通常，我们会把原点选在某个方便的位置，比如停车场入口、十字路口的中心。X轴指向东，Y轴指向北，Z轴指向天。这就是经典的 ENU 坐标系（东-北-天）。
关键点：世界坐标系是绝对的、不变的。高精地图里的所有元素——车道线、红绿灯、路沿——都在这个坐标系下定义。
举个例子。你开车到天安门广场，你的 GPS 给出的经纬度坐标，经过转换后，就变成了世界坐标系下的一个点 (x, y, z)。这个点，就是你的车在这个世界里的「绝对位置」。
2. 车身坐标系：以车为本
车身坐标系，也叫车辆坐标系，我管它叫 V系。它的原点在车辆的后轴中心（有些车厂用质心，但我个人习惯用后轴中心，因为做运动学模型时方便）。
怎么定义方向？
- X轴：指向车辆正前方
- Y轴：指向车辆左侧（注意，是左侧！很多新手会搞反）
- Z轴：指向车顶
车身坐标系是跟着车动的。车转弯，坐标系也跟着转。车加速，坐标系也跟着加速。说白了，它是以「车自己」为参考系。
我的小技巧：在做路径规划时，我经常把目标点从世界坐标系转换到车身坐标系。这样，控制模块就能直接知道「目标在我前方几米、左侧几米」，不用再考虑车头朝向的问题。
3. 传感器坐标系：感知的「眼睛」
每个传感器都有自己的坐标系，我统称为 S系。
比如：
- 激光雷达：原点在激光雷达的旋转中心。X轴指向雷达正前方，Y轴指向左侧，Z轴指向上方。
- 摄像头：原点在摄像头的光心。Z轴指向镜头正前方（也就是拍摄方向），X轴指向右侧，Y轴指向下方。嗯，图像坐标系和相机坐标系还不一样，这个我们后面会细说。
- 毫米波雷达：原点在雷达天线中心。X轴指向雷达正前方。
为什么要有传感器坐标系？因为每个传感器看到的世界，都是从它自己的「视角」出发的。激光雷达看到的是「我前方3米有个障碍物」，摄像头看到的是「我画面中心偏右100像素有个红点」。
我曾经踩过的坑：有一次，我把激光雷达的坐标系定义成了「X轴向右，Y轴向前」。结果融合模块怎么都对不齐。查了两天，才发现是坐标系定义和团队其他人不一致。所以，坐标系定义一定要写在文档里，并且团队所有人都要遵守。
4. 坐标变换：让数据「对齐」
好了，现在我们有三个坐标系。问题来了：激光雷达检测到「前方2米有个行人」，这个信息怎么告诉规划模块？
答案就是坐标变换。
坐标变换，说白了就是「把一个坐标系下的点，换算到另一个坐标系下」。它由两部分组成：旋转 和 平移。
4.1 旋转矩阵
旋转矩阵 R 是一个 3x3 的矩阵。它描述了坐标系之间的角度关系。比如，车身坐标系相对于世界坐标系旋转了多少度。
公式很简单：
P_world = R * P_vehicle + T其中：
- P_world：点在世界坐标系下的坐标
- P_vehicle：点在车身坐标系下的坐标
- R：旋转矩阵（3x3）
- T：平移向量（3x1）
反过来，从世界坐标系到车身坐标系：
P_vehicle = R^(-1) * (P_world - T)注意，旋转矩阵的逆等于它的转置（因为旋转矩阵是正交矩阵）。这个性质很好用，计算量小很多。
4.2 齐次坐标
每次都要写旋转加平移，太麻烦了。所以工程上我们常用齐次坐标。把 3D 点扩展成 4D：
[x, y, z, 1]^T这样，旋转和平移就可以合并成一个 4x4 的变换矩阵 T：
| R T |
| 0 1 |变换就变成了简单的矩阵乘法：
P_world_homogeneous = T * P_vehicle_homogeneous你看，是不是清爽多了？
3. 变换链
实际项目中，我们很少直接做「传感器→世界」的变换。通常是走一条链：
传感器坐标系 → 车身坐标系 → 世界坐标系每个环节都有一个变换矩阵。比如：
- T_sensor_to_vehicle：传感器安装在车上的位置和角度（这个需要标定）
- T_vehicle_to_world：车当前的位置和朝向（这个来自定位模块）
那么，从传感器到世界的总变换就是：
T_sensor_to_world = T_vehicle_to_world * T_sensor_to_vehicle我的习惯：我会把所有变换矩阵都写成「从A到B」的形式，并且用变量名明确标注。比如 T_v2w 表示从车身到世界。这样代码可读性高，不容易出错。
5. 知识体系总览
下面这张图，是我自己画的坐标系知识体系。你可以把它当作一个「地图」，随时回来看看。
6. 实战：一个完整的变换例子
假设激光雷达检测到一个点，在雷达坐标系下是 (1, 0, 0.5) 米。雷达安装在车顶，相对于后轴中心的位置是 (0, 0, 1.5) 米，且雷达朝前安装（没有旋转）。
那么，这个点在车身坐标系下是多少？
P_vehicle = P_lidar + T_lidar_to_vehicle
= (1, 0, 0.5) + (0, 0, 1.5)
= (1, 0, 2.0) 米现在，车的位置是 (100, 200, 0)，朝向是正北（即车头指向Y轴正方向）。那么，这个点在世界坐标系下是多少？
注意，这里有一个旋转。车身坐标系相对于世界坐标系旋转了90度（因为车头指向Y轴）。旋转矩阵是：
R = | 0 -1 0 |
| 1 0 0 |
| 0 0 1 |所以：
P_world = R * P_vehicle + T_vehicle_to_world
= | 0 -1 0 | | 1 | | 100 |
| 1 0 0 | * | 0 | + | 200 |
| 0 0 1 | | 2 | | 0 |
= | 0 | | 100 |
| 1 | + | 200 |
| 2 | | 0 |
= (100, 201, 2) 米你看，激光雷达看到的点，最终在世界坐标系下是 (100, 201, 2)。这个点，就可以和高精地图里的障碍物做匹配了。
总结一下：坐标系是自动驾驶的「语言」。世界坐标系是全局的锚点，车身坐标系是车的视角，传感器坐标系是感知的起点。坐标变换就是把这些语言翻译成统一的「普通话」。搞懂了这些，你就能让传感器数据、地图数据、规划数据在同一个空间里对话。
好了，今天就聊到这儿。坐标系这东西，多动手算几次就熟了。下次见！