坐标系与坐标变换：无人车的“世界观”
做无人车算法这些年，我最大的感触就是——坐标系这东西，看着简单，但坑是真多。你想想看，一辆车在路上跑，它怎么知道自己在哪？怎么知道障碍物在哪？怎么决定往哪拐？
说白了，这一切都离不开坐标系。今天我们就来聊聊无人车的“世界观”，也就是全局坐标系、局部坐标系、车辆坐标系，以及它们之间怎么互相转换。
1. 三种坐标系，三种视角
我刚开始做路径跟踪时，总搞混坐标系。后来我总结了一句话：全局坐标系是地图，局部坐标系是眼睛，车辆坐标系是身体。
1.1 全局坐标系（World Coordinate System）
全局坐标系是固定不变的。它通常以地球上的某个点为原点，比如经纬度原点，或者高精地图的某个参考点。X轴指向东，Y轴指向北，Z轴指向天。
这个坐标系用来描述：
- 道路的绝对位置
- 障碍物的绝对坐标
- 车辆的绝对位姿
我在项目中遇到过一个问题：GPS信号飘移导致全局坐标不准，结果车辆以为自己已经在车道线上了，其实还差半米。嗯，这就是全局坐标系的局限性——它依赖外部传感器。
1.2 车辆坐标系（Vehicle Coordinate System）
车辆坐标系是跟着车走的。原点通常在车辆后轴中心，或者质心。X轴指向车头前方，Y轴指向左侧，Z轴指向上方。
这个坐标系用来描述：
- 车辆自身的速度、加速度
- 车辆自身的横摆角速度
- 车辆自身的姿态（俯仰、侧倾）
我建议在做车辆动力学建模时，一定要用车辆坐标系。因为牛顿第二定律在车辆坐标系下写起来最顺手。
1.3 局部坐标系（Local Coordinate System）
局部坐标系是个“中间人”。它通常以车辆当前位置为原点，但坐标轴方向与全局坐标系平行。说白了，就是把全局坐标系平移到了车的位置。
这个坐标系用来描述：
- 传感器检测到的障碍物相对位置
- 路径跟踪时的横向误差、纵向误差
- 局部路径规划
你想想看，如果直接用全局坐标做路径跟踪，计算量会很大。但用局部坐标，误差一目了然。
2. 坐标变换矩阵：数学的魔法
三种坐标系之间怎么转换？靠的就是坐标变换矩阵。我记得第一次手推这个矩阵时，推了整整一页纸，结果发现有个符号写反了……
2.1 二维旋转矩阵
假设车辆在全局坐标系中有一个航向角 θ（绕Z轴旋转），那么从车辆坐标系到全局坐标系的旋转矩阵是：
R = | cosθ -sinθ |
| sinθ cosθ |反过来，从全局坐标系到车辆坐标系，就是转置一下：
R_inv = | cosθ sinθ |
| -sinθ cosθ |这里有个小技巧：旋转矩阵的逆等于它的转置。这个性质在代码里非常有用，不用真的去求逆矩阵。
2.2 平移变换
光旋转还不够，还得平移。假设车辆在全局坐标系中的位置是 (x_v, y_v)，那么从车辆坐标系到全局坐标系的完整变换是：
X_global = R * X_vehicle + T其中 T = [x_v, y_v]^T。
3. 齐次坐标：让变换更优雅
你发现没有？上面的变换里，旋转是矩阵乘法，平移是向量加法。两个操作混在一起，代码写起来很别扭。
齐次坐标就是来解决这个问题的。它给二维坐标加一个维度，变成 (x, y, 1)。这样，旋转和平移就可以合并成一个 3x3 的矩阵：
T = | cosθ -sinθ x_v |
| sinθ cosθ y_v |
| 0 0 1 |那么从车辆坐标系到全局坐标系，就变成了一个矩阵乘法：
X_global_homo = T * X_vehicle_homo是不是清爽多了？
4. 实战：坐标系变换在路径跟踪中的应用
光说不练假把式。我们来看一个实际场景：路径跟踪。
假设我们有一条全局路径，由一系列全局坐标点组成。车辆当前在某个位置，我们要计算车辆到路径的横向误差。
步骤是这样的：
- 找到路径上离车辆最近的点（匹配点）
- 计算匹配点在车辆坐标系下的坐标
- 横向误差就是匹配点在车辆坐标系下的Y坐标
代码实现大概是这样的：
// 假设已知：
// vehicle_pose: 车辆在全局坐标系下的位姿 (x, y, theta)
// path_point: 路径上匹配点的全局坐标 (x_p, y_p)
// 1. 计算相对位置
double dx = path_point.x - vehicle_pose.x;
double dy = path_point.y - vehicle_pose.y;
// 2. 旋转到车辆坐标系
double local_x = cos(theta) * dx + sin(theta) * dy;
double local_y = -sin(theta) * dx + cos(theta) * dy;
// 3. 横向误差就是 local_y
double lateral_error = local_y;你看，代码就这么几行。但我在项目中踩过一个坑：航向角 theta 的定义。有的传感器输出的是弧度，有的是角度；有的定义是北偏东，有的是东偏北。一定要统一！
5. 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
6. 避坑指南
最后，我把自己这些年踩过的坑总结一下，希望能帮你少走弯路：
- 航向角定义要统一：我建议统一使用“从X轴正方向逆时针旋转”为正方向。所有传感器数据进来，先做一次转换。
- 注意浮点精度：坐标变换涉及大量三角函数运算。我习惯用 double 而不是 float，尤其是在累积变换时。
- 测试要覆盖边界：比如航向角为0度、90度、180度时，变换矩阵是否正常？我曾经在航向角为90度时，sin和cos写反了，结果车辆横着走。
- 齐次坐标的最后一维：变换后记得归一化，把最后一维变回1。虽然大多数情况下它本来就是1，但养成习惯总没错。
好了，坐标系与坐标变换就讲到这里。这些东西看似基础，但真的贯穿了整个无人车算法。路径跟踪、避障、传感器融合……哪哪都离不开它。希望你能把这些概念吃透，后面学起来会轻松很多。
公众号:蓝海资料掘金营，微信deep3321