2、坐标系与坐标变换：世界坐标系、车辆坐标系、传感器坐标系、坐标变换矩阵、四元数与欧拉角、工程中的坐标系管理
说实话，坐标系这个话题，我见过太多人栽跟头了。
刚入行那会儿，我觉得坐标系不就是个数学概念嘛，有啥好讲的？直到有一次，我调试的感知模块死活跟规划对不上——明明前方有个障碍物，规划路径却直接穿过去了。查了三天，最后发现是激光雷达坐标系和车辆坐标系差了90度没转过来。嗯，从那以后，我再也不敢小看坐标系了。
2.1 世界坐标系：我们到底在哪儿？
世界坐标系，说白了就是给整个物理世界定一个“绝对位置”。
在自动驾驶里，我们通常用UTM坐标系（通用横轴墨卡托投影）或者自定义的局部坐标系。我个人习惯用UTM，因为它能直接跟GPS数据对接，省去很多麻烦。
关键点：世界坐标系是静止的，所有动态物体（车、人、障碍物）的位置都相对于它来描述。
举个例子：
- 车辆当前位置： (x=452301.2, y=4421897.3, z=0.0) 米
- 前方障碍物： (x=452310.5, y=4421905.8, z=0.0) 米
你看，这些坐标都是相对于世界原点的绝对位置。
2.2 车辆坐标系：车自己的小世界
车辆坐标系是跟着车走的。一般定义：
- X轴：车头方向
- Y轴：车身左侧
- Z轴：车顶方向（右手系）
为什么需要这个坐标系？因为传感器都装在车上，它们看到的数据天然就是相对于车的。比如激光雷达说“前方5米有障碍物”，这个“前方”就是车辆坐标系下的前方。
我的经验：车辆坐标系的原点通常选在后轴中心。为什么？因为车辆的动力学模型（自行车模型）就是基于后轴中心建立的。你选别的位置，后面做运动预测时还得再转一次，麻烦。
2.3 传感器坐标系：每个传感器都有自己的视角
车上装了那么多传感器——激光雷达、摄像头、毫米波雷达、IMU……每个都有自己的坐标系。
比如：
- 激光雷达：原点在雷达中心，X轴指向雷达正前方
- 摄像头：原点在光心，Z轴指向镜头前方（注意，不同厂家定义可能不同）
- IMU：原点在IMU芯片中心，通常跟车辆坐标系对齐
我记得有一次，供应商给的激光雷达标定文件里，坐标系定义跟我们默认的差了180度。结果融合出来的点云全反了。从那以后，我拿到新传感器第一件事就是确认坐标系定义。
避坑指南：不同传感器厂商的坐标系定义可能不一样。比如有的摄像头把Y轴朝下，有的朝上。拿到标定文件后，一定要先画个图验证一下，别直接往代码里塞。
2.4 坐标变换矩阵：怎么从一个坐标系跳到另一个？
坐标变换，说白了就是“旋转+平移”。
数学上，我们用4x4的齐次变换矩阵来表示：
| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0 0 0 1 |其中：
- R：3x3旋转矩阵
- t：3x1平移向量
举个例子，把激光雷达坐标系下的点变换到车辆坐标系：
// 伪代码
Point3D point_in_vehicle = T_lidar_to_vehicle * point_in_lidar;这里T_lidar_to_vehicle就是激光雷达到车辆坐标系的变换矩阵。
重要：变换矩阵是可逆的。从车辆到激光雷达，就是求逆矩阵。但注意，旋转矩阵的逆等于它的转置（因为旋转矩阵是正交矩阵），平移部分要重新计算。
2.5 四元数与欧拉角：旋转的两种表达方式
说到旋转，就绕不开欧拉角和四元数。
欧拉角：用三个角度（roll, pitch, yaw）表示旋转。直观，但有个大问题——万向锁。
什么叫万向锁？就是当pitch达到±90度时，roll和yaw会变得无法区分。我在做坡道场景时遇到过这个问题，车辆俯仰角一大，欧拉角直接崩了。
四元数：用四个数（w, x, y, z）表示旋转。没有万向锁，插值平滑，计算效率高。
我个人强烈建议：内部计算全部用四元数，只在可视化或人机交互时转成欧拉角。
四元数转欧拉角的公式（以ZYX顺序为例）：
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z))小技巧：用Eigen库或者ROS的tf库，这些转换都有现成函数。别自己手写，容易出bug。我见过有人手写四元数乘法写反了顺序，结果旋转方向全反了。
2.6 工程中的坐标系管理：别让坐标系成为你的噩梦
在实际工程中，坐标系管理是个大问题。车上有十几个传感器，每个都要标定到车辆坐标系。如果管理不好，数据流就乱了。
我推荐的做法：
- 统一命名规范：比如
lidar_front,camera_left,imu_center - 用树状结构管理：所有坐标系都挂在车辆坐标系下，形成一棵树
- 缓存变换矩阵：标定完成后，把所有变换矩阵存成静态数据，运行时直接查表
- 加断言检查：每次变换后，检查结果是否在合理范围内（比如距离不能突变）
举个例子，坐标系树可能长这样：
world
└── vehicle
├── lidar_front
├── lidar_rear
├── camera_front
├── camera_left
├── camera_right
├── radar_front
└── imu每个节点都存着到父节点的变换矩阵。要转换数据时，沿着树往上走到公共祖先，再往下走到目标节点。
我曾经踩过的坑：有一次，两个传感器都标定到了车辆坐标系，但一个用的是后轴中心，另一个用的是车辆质心。结果差了0.5米，融合出来的障碍物位置全偏了。所以，标定前一定要确认基准点一致。
2.7 实战：一个完整的坐标变换流程
假设我们要把激光雷达检测到的障碍物位置，转换到世界坐标系下：
- 激光雷达检测到障碍物：
p_lidar = (x, y, z) - 用标定矩阵转到车辆坐标系：
p_vehicle = T_lidar_to_vehicle * p_lidar - 用车辆定位信息（GPS+IMU）得到车辆在世界坐标系下的位姿：
T_world_to_vehicle - 转到世界坐标系：
p_world = T_world_to_vehicle * p_vehicle
代码实现（伪代码）：
// 1. 激光雷达检测
Point3D p_lidar = lidar.detect();
// 2. 转到车辆坐标系
Point3D p_vehicle = T_lidar_to_vehicle * p_lidar;
// 3. 获取车辆位姿（来自定位模块）
Pose vehicle_pose = localization.getPose();
// 4. 转到世界坐标系
Matrix4 T_world_to_vehicle = vehicle_pose.toMatrix();
Point3D p_world = T_world_to_vehicle * p_vehicle;
// 5. 输出结果
std::cout << "障碍物在世界坐标系下的位置: " << p_world << std::endl;你看，整个过程就是一连串的矩阵乘法。只要标定数据准确，代码逻辑清晰，就不会出错。
总结一下：
- 世界坐标系是绝对参考
- 车辆坐标系是动态参考
- 传感器坐标系是原始数据来源
- 变换矩阵是连接它们的桥梁
- 四元数比欧拉角更可靠
- 工程管理比数学本身更重要
嗯，坐标系这块就讲这么多。下一章我们聊聊路径规划的核心算法，到时候你会看到，坐标系搞不清楚，后面的规划算法再牛也白搭。