坐标系统与变换：世界坐标系、车辆坐标系、传感器坐标系、坐标变换矩阵、欧拉角与四元数
各位同学，今天我们来聊聊自动驾驶里最基础、也最容易出问题的一个模块——坐标系统与变换。
说实话，我见过太多新手工程师，算法写得飞起，结果车一上路就偏了。查来查去，最后发现是坐标系搞反了。嗯，这种坑我自己也踩过，所以今天咱们把这部分彻底讲透。
1. 为什么需要这么多坐标系？
你想想看，一辆自动驾驶车上，有GPS、有激光雷达、有摄像头、有IMU。每个传感器都有自己的“视角”。
- GPS告诉你：你在北纬多少度、东经多少度——这是世界坐标系。
- IMU告诉你：车头朝哪个方向、车身倾斜了多少——这是车辆坐标系。
- 激光雷达告诉你：前方3米处有个障碍物——这是传感器坐标系。
说白了，我们做规划控制，就是要把这些不同“语言”的数据，统一翻译成一种“语言”。不然的话，GPS说“前面有坑”，激光雷达说“右边有墙”，车就不知道该往哪走了。
2. 三大核心坐标系
2.1 世界坐标系（World Coordinate System）
世界坐标系是绝对的、固定的。通常我们使用UTM坐标系（通用横轴墨卡托投影），或者直接使用经纬度+海拔。
我个人习惯用UTM，因为它是平面直角坐标系，计算距离和角度非常方便。经纬度虽然直观，但涉及到球面几何，计算起来比较麻烦。
关键点：世界坐标系的原点通常是固定的，比如某个基准点。所有的高精地图、全局路径规划，都在这个坐标系下完成。
2.2 车辆坐标系（Vehicle Coordinate System）
车辆坐标系是跟着车走的。通常定义：
- X轴：车头方向（前进方向）
- Y轴：车身左侧方向
- Z轴：垂直向上（符合右手定则）
我记得有一次，团队里新来的同事把Y轴方向搞反了，结果车辆在变道时总是往反方向打方向盘。还好是在仿真环境里测试，不然真上路就出大事了。
注意：不同车厂对车辆坐标系的定义可能略有不同。有的把Y轴定义为右侧方向。拿到数据后，第一件事就是确认坐标系定义。
2.3 传感器坐标系（Sensor Coordinate System）
每个传感器都有自己的坐标系。比如：
- 摄像头：通常以光心为原点，Z轴指向拍摄方向，X轴向右，Y轴向下。
- 激光雷达：通常以旋转中心为原点，X轴指向正前方，Y轴指向左侧。
- IMU：通常以安装位置为原点，坐标轴与车辆坐标系对齐。
这里有个坑：传感器的安装位置和朝向，必须精确标定。我曾经遇到过一个项目，激光雷达装歪了0.5度，结果在100米外的障碍物位置偏差了将近1米。你说这能忍吗？
3. 坐标变换矩阵
坐标变换，说白了就是用一个矩阵，把一个坐标系下的点，映射到另一个坐标系下。
假设我们有一个点P，在坐标系A下的坐标为(x_a, y_a, z_a)，想把它转换到坐标系B下。那么：
P_b = R * P_a + T其中：
- R：旋转矩阵（3x3），描述两个坐标系之间的旋转关系。
- T：平移向量（3x1），描述两个坐标系原点之间的位移。
在实际代码中，我们通常使用齐次坐标，把旋转和平移合并成一个4x4的变换矩阵：
| R T |
| 0 1 |这样，一次矩阵乘法就能完成变换，非常高效。
我的习惯：在代码里，我通常用Eigen库或者Sophus库来处理坐标变换。千万别自己手写矩阵求逆，容易出错。用现成的库，既安全又高效。
4. 欧拉角与四元数
旋转的表达方式有很多种，最常用的是欧拉角和四元数。
4.1 欧拉角
欧拉角用三个角度来描述旋转：
- 偏航角（Yaw）：绕Z轴旋转，也就是车头的朝向。
- 俯仰角（Pitch）：绕Y轴旋转，也就是车头上仰或下俯。
- 横滚角（Roll）：绕X轴旋转，也就是车身侧倾。
欧拉角很直观，但有一个致命问题——万向锁（Gimbal Lock）。当俯仰角接近±90度时，偏航和横滚会失去一个自由度，导致旋转无法唯一表示。
避坑指南：我曾经在IMU数据融合时，直接用欧拉角做插值，结果车辆在坡道上出现了诡异的抖动。后来换成四元数，问题立刻解决了。所以，千万不要在插值和滤波中使用欧拉角。
4.2 四元数
四元数用四个数来表示旋转：q = (w, x, y, z)。它没有万向锁问题，而且插值平滑，是自动驾驶领域的首选。
四元数的好处：
- 没有奇点，可以表示任意旋转。
- 插值（球面线性插值，SLERP）非常平滑。
- 组合旋转只需要一次乘法，计算效率高。
举个例子，把欧拉角转成四元数：
// 假设欧拉角为 (yaw, pitch, roll)
// 转换为四元数
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
double w = cr * cp * cy + sr * sp * sy;
double x = sr * cp * cy - cr * sp * sy;
double y = cr * sp * cy + sr * cp * sy;
double z = cr * cp * sy - sr * sp * cy;嗯，这段代码我闭着眼睛都能写出来。因为我在项目里用过不下几百次了。
5. 实战中的坐标系管理
在实际项目中，我建议你建立一个统一的坐标系管理类。比如：
class CoordinateManager {
public:
// 世界坐标系 -> 车辆坐标系
Eigen::Matrix4d worldToVehicle(const Eigen::Matrix4d& pose);
// 车辆坐标系 -> 传感器坐标系
Eigen::Matrix4d vehicleToSensor(const std::string& sensor_name);
// 直接转换：世界坐标系 -> 传感器坐标系
Eigen::Matrix4d worldToSensor(const std::string& sensor_name);
private:
std::map<std::string, Eigen::Matrix4d> sensor_extrinsics_; // 传感器外参
Eigen::Matrix4d vehicle_pose_; // 当前车辆位姿（世界坐标系下）
};这样做的好处是，所有坐标变换都集中管理，不会出现“这个点是在哪个坐标系下”的混乱。
我的建议：在代码里，给每个坐标点加上一个“坐标系标签”。比如用枚举类型：
enum class CoordinateFrame {
WORLD,
VEHICLE,
LIDAR,
CAMERA
};
struct Point3D {
double x, y, z;
CoordinateFrame frame;
};这样，即使代码写得再快，也不会搞混坐标系。
6. 总结
坐标系统与变换，是自动驾驶规划控制的基石。说白了，你所有的算法——路径规划、行为决策、运动控制——都建立在正确的坐标变换之上。
我个人觉得，这部分内容虽然基础，但再怎么强调也不为过。因为一旦出错，后面的所有工作都是白费。
最后送大家一句话：坐标系搞不对，代码写得再漂亮也是零。
好了，今天就讲到这里。下一章我们聊聊路径规划中的曲线生成，到时候见。