2、车辆坐标系与位姿表示：世界坐标系、车身坐标系、欧拉角与四元数
各位同学，今天我们来聊聊自动驾驶里最基础、也最容易踩坑的一个话题——坐标系与位姿表示。
说实话，我刚开始做自动驾驶那会儿，觉得坐标系嘛，不就是个xyz嘛，有什么好讲的？结果第一次做实车测试，定位数据一跑，车直接往反方向开。嗯，从那以后，我再也不敢小看坐标系了。
2.1 世界坐标系：车在哪？
世界坐标系，说白了就是给车一个“绝对位置”。你想想看，如果我说“车在路口左转”，这个描述太模糊了。我需要一个精确的数字来描述——比如东经116.4度，北纬39.9度，海拔50米。
在自动驾驶里，世界坐标系通常用以下几种：
- UTM坐标系：把地球投影成平面，单位是米。我最喜欢用这个，因为计算距离直接减坐标就行。
- WGS-84坐标系：就是GPS用的经纬度。做全局路径规划时常用。
- 局部笛卡尔坐标系：以某个点为原点，比如起点或者地图原点。
重要提醒：世界坐标系的原点选择会影响所有后续计算。我建议在项目初期就定好，不要中途改，否则你会被坐标转换搞疯。
2.2 车身坐标系：车头朝哪？
世界坐标系告诉我们车在哪，但车本身也有自己的“视角”。这就是车身坐标系。
车身坐标系通常这样定义：
- x轴：指向车头前方
- y轴：指向车身左侧（注意！有些标准指向右侧，一定要确认清楚）
- z轴：指向车顶上方
我记得有一次，供应商给的传感器数据，y轴方向跟我们定义的反了。结果融合出来的障碍物位置全在车右边，实际上在左边。排查了整整两天才找到原因。
我的习惯：在代码里用枚举类型定义坐标系方向，不要用数字1和-1表示正反。这样别人看代码时一目了然。
2.3 欧拉角：旋转的“三个角度”
现在我们知道车在哪（世界坐标），也知道车头朝哪（车身坐标）。但车是歪的还是正的？这就需要描述姿态了。
欧拉角是最直观的方式——用三个角度描述旋转：
- 横摆角（Yaw）：绕z轴旋转，就是车头左右转
- 俯仰角（Pitch）：绕y轴旋转，就是车头上仰下俯
- 侧倾角（Roll）：绕x轴旋转，就是车身左右倾斜
为什么会用三个角度？因为任何旋转都可以分解成这三个方向的组合。你想想看，飞机在空中翻滚，不就是这三个角度的变化吗？
避坑指南：欧拉角有个致命问题——万向锁。当俯仰角接近±90度时，横摆角和侧倾角会变得无法区分。我曾经在模拟器里遇到这个问题，车辆翻滚时姿态直接崩了。所以，如果你要做全姿态控制，别用欧拉角。
2.4 四元数：没有死角的旋转表示
欧拉角有万向锁，那怎么办？四元数就是答案。
四元数用四个数表示旋转：q = w + xi + yj + zk。听起来有点抽象，但你可以理解为：
- w：旋转的“量”
- x, y, z：旋转轴的“方向”
四元数的好处：
- 没有万向锁
- 插值平滑（做轨迹规划时特别有用）
- 计算效率高（乘法比矩阵快）
代码示例：四元数转旋转矩阵
// C++ 示例：四元数转旋转矩阵
void quaternionToRotationMatrix(double q[4], double R[3][3]) {
double w = q[0], x = q[1], y = q[2], z = q[3];
R[0][0] = 1 - 2*y*y - 2*z*z;
R[0][1] = 2*x*y - 2*w*z;
R[0][2] = 2*x*z + 2*w*y;
R[1][0] = 2*x*y + 2*w*z;
R[1][1] = 1 - 2*x*x - 2*z*z;
R[1][2] = 2*y*z - 2*w*x;
R[2][0] = 2*x*z - 2*w*y;
R[2][1] = 2*y*z + 2*w*x;
R[2][2] = 1 - 2*x*x - 2*y*y;
}实用技巧：调试时可以把四元数转成欧拉角打印出来，方便人眼理解。但内部计算一定要用四元数。我习惯在代码里同时维护两套表示，调试用欧拉角，计算用四元数。
2.5 坐标系转换：把一切统一起来
实际项目中，你会遇到各种坐标系：
- GPS给的是WGS-84经纬度
- IMU给的是车身坐标系下的加速度
- 激光雷达给的是传感器坐标系下的点云
- 相机给的是像素坐标系下的图像
这些数据必须统一到同一个坐标系下，才能做融合和决策。
转换流程一般是：
- 传感器数据 → 传感器坐标系
- 传感器坐标系 → 车身坐标系（通过外参标定）
- 车身坐标系 → 世界坐标系（通过定位结果）
我曾经接手过一个项目，前同事把传感器到车身的转换矩阵写反了。结果车看到的前方障碍物，实际在后方。嗯，那次测试差点撞墙。
核心原则：所有数据最终都要转换到世界坐标系下做决策。不要在车身坐标系下做路径规划，因为车在动，坐标系也在动，你会算晕的。
2.6 总结与建议
好了，我们来捋一捋：
| 表示方法 | 优点 | 缺点 | 推荐场景 |
|---|---|---|---|
| 世界坐标系 | 绝对位置，全局统一 | 数值可能很大 | 路径规划、地图构建 |
| 车身坐标系 | 直观，传感器数据天然在此 | 随车移动，不适合长期规划 | 障碍物检测、控制指令 |
| 欧拉角 | 直观，三个角度好理解 | 万向锁，插值不平滑 | 调试、显示、小角度姿态 |
| 四元数 | 无死角，计算稳定 | 不直观，难调试 | 姿态控制、轨迹插值、滤波 |
我个人建议：
- 新手先用欧拉角理解概念，但正式项目一定要用四元数
- 写一个坐标系转换的工具类，把所有转换封装好，不要到处写转换代码
- 每次加新传感器，第一件事就是验证坐标系转换是否正确
下一章我们会讲车辆运动学模型，到时候这些坐标系知识会反复用到。嗯，先把基础打牢，后面才不慌。