4、坐标系与变换：世界坐标系、车辆坐标系、传感器坐标系、欧拉角与四元数
各位同学，今天我们来聊聊定位里最绕不开的一个话题——坐标系与变换。
说实话，我刚开始做自动驾驶定位时，被坐标系搞晕过好几次。明明传感器数据都对，但融合出来的位置就是飘。后来才发现，原来是某个坐标系变换矩阵写反了。嗯，这种坑，踩过一次就记住了。
4.1 为什么需要这么多坐标系？
你想想看，一辆车上装了GPS、IMU、激光雷达、摄像头。每个传感器都有自己的“视角”。GPS告诉你在地球上的经纬度，IMU告诉你车本身的加速度和角速度，激光雷达告诉你周围障碍物相对于雷达的距离。
这些数据如果不统一到一个坐标系下，根本没法用。所以我们需要一套坐标系体系，把大家“对齐”。
核心原则：所有传感器数据最终都要变换到同一个参考系下，才能进行融合和定位计算。
4.2 世界坐标系
世界坐标系，说白了就是“大地基准”。在自动驾驶里，我们通常用WGS-84（经纬度）或者UTM（通用横轴墨卡托投影）坐标系。
我个人习惯用UTM坐标系做局部定位，因为它单位是米，计算起来直观。WGS-84适合做全局定位，但经纬度算距离太麻烦，还得考虑地球曲率。
世界坐标系的特点：
- 固定不动，不随车辆移动
- 通常以地球质心或某个参考点为原点
- Z轴指向天顶（东北天坐标系）或指向地心
我在项目中遇到过一个问题：高精地图用的是WGS-84，但定位算法用的是UTM。每次都要做投影变换，如果投影参数设置错了，定位误差能差出几十米。后来我写了个自动化校验脚本，每次启动时先对比地图原点坐标，确保投影参数一致。
4.3 车辆坐标系
车辆坐标系是固定在车上的。ISO标准定义：X轴指向车头前方，Y轴指向驾驶员左侧（也就是车辆左侧），Z轴垂直向上。
为什么这么定义？因为这样符合人的直觉。你开车时，前方是X正方向，左边是Y正方向，头顶是Z正方向。
车辆坐标系的作用：
- 描述车辆自身的运动状态（速度、加速度、角速度）
- 作为传感器安装位置的基准参考
- 控制模块需要知道车辆当前朝向
小技巧：车辆坐标系的原点通常选在后轴中心。为什么？因为车辆转弯时，后轴中心基本不产生侧偏，定位更稳定。这是我做实车测试时发现的规律。
4.4 传感器坐标系
每个传感器都有自己的坐标系。比如激光雷达，通常以雷达自身为中心，X轴指向雷达前方，Y轴指向左侧，Z轴向上。
摄像头坐标系有点特殊——Z轴指向镜头前方，X轴指向右侧，Y轴指向下方。嗯，这个跟图像坐标系有关，像素坐标的u轴对应X，v轴对应Y。
常见的传感器坐标系：
| 传感器 | 坐标系定义 | 原点位置 |
|---|---|---|
| GPS/RTK | WGS-84 或 UTM | 天线相位中心 |
| IMU | 右手系，X前Y左Z上 | IMU芯片中心 |
| 激光雷达 | 右手系，X前Y左Z上 | 激光发射中心 |
| 摄像头 | 右手系，Z前X右Y下 | 光心 |
我曾经犯过一个低级错误：把激光雷达的坐标系当成了车辆坐标系直接使用。结果发现点云数据总是偏了30厘米。后来一查，原来是激光雷达安装在车顶右侧，需要先做一次平移变换。
4.5 坐标系变换：从传感器到车辆到世界
坐标系变换说白了就是“搬家”。把传感器测到的点，搬到车辆坐标系下，再搬到世界坐标系下。
变换公式很简单：
P_world = R * P_sensor + T其中R是旋转矩阵，T是平移向量。
但实际做起来，麻烦在于旋转矩阵的表示方式。这里就引出了欧拉角和四元数。
4.6 欧拉角
欧拉角用三个角度来描述旋转：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。
- Roll（横滚）：绕X轴旋转，就像飞机侧身
- Pitch（俯仰）：绕Y轴旋转，就像飞机抬头低头
- Yaw（偏航）：绕Z轴旋转，就像飞机转向
欧拉角直观，但有个致命问题——万向锁。当Pitch达到±90度时，Roll和Yaw会变得无法区分。这在自动驾驶里很危险，比如车辆在陡坡上时，欧拉角会突然跳变。
避坑指南：我曾经在实车测试时，车辆经过一个30度的坡，欧拉角表示的姿态突然跳了180度。吓得我赶紧把代码改成了四元数。所以，如果你做的是全姿态定位，别用欧拉角。
4.7 四元数
四元数是个数学工具，用四个数表示旋转：q = w + xi + yj + zk。其中w是实部，x、y、z是虚部。
为什么用四元数？三个原因：
- 无万向锁：任何姿态都能平滑表示
- 插值方便：可以做球面线性插值（SLERP），姿态过渡平滑
- 计算高效：比旋转矩阵少用内存，乘法也快
四元数转旋转矩阵的公式：
R = [
[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]
]看着复杂，但代码里直接调库就行。我个人习惯用Eigen库，四元数运算一行搞定。
实用建议：在代码里，我建议统一用四元数做内部计算，只在可视化或调试时转成欧拉角。这样既避免了万向锁，又方便人理解。
4.8 坐标系变换的工程实践
在实际项目中，坐标系变换通常做成一个独立的模块。我一般会维护一个“变换树”（Transform Tree），记录所有坐标系之间的相对关系。
比如：
- 车辆坐标系 → 世界坐标系：由定位模块提供
- 激光雷达坐标系 → 车辆坐标系：由标定参数提供（固定值）
- 摄像头坐标系 → 车辆坐标系：由标定参数提供（固定值）
每次收到传感器数据，先查变换树，找到对应的变换矩阵，然后统一变换到世界坐标系下。
这样做的好处是：如果某个传感器的安装位置变了，只需要更新标定参数，不用改代码逻辑。
4.9 本章知识体系
下面这张图展示了坐标系与变换的核心逻辑：
从图中可以看出，所有传感器数据先通过标定参数变换到车辆坐标系，再通过定位结果变换到世界坐标系。这个链条上的每一个环节，都离不开坐标系变换。
好了，坐标系与变换就讲到这里。记住一句话：定位的本质，就是不断求解和更新坐标系之间的变换关系。把这个想明白了，后面的内容就好理解了。
本章要点回顾：
- 世界坐标系是固定基准，车辆坐标系随车运动，传感器坐标系各有定义
- 坐标系变换用旋转矩阵+平移向量，但旋转矩阵的表示方式很关键
- 欧拉角直观但有万向锁，四元数才是工程首选
- 维护变换树，统一管理所有坐标系关系
可可精品资料库