第三节 坐标系与变换：全局坐标系与局部坐标系

做路径规划，绕不开坐标系。说白了，你得知道车在哪，要去哪，周围的东西又在哪。

我刚开始做自动驾驶项目时，犯过一个低级错误——把全局坐标和局部坐标搞混了。结果规划出来的路径，车直接往墙上怼。嗯，那次教训挺深刻的。

3.1 全局坐标系与局部坐标系

先搞清楚这两个概念。

全局坐标系，也叫世界坐标系。它是固定不动的。比如我们常用的UTM坐标系，或者自己定义的一个原点。所有静态地图信息，比如车道线、红绿灯、障碍物，都在这个坐标系下描述。

局部坐标系，也叫车身坐标系。它跟着车走。原点通常放在车辆后轴中心，x轴指向车头前方，y轴指向左侧。你想想看，传感器（激光雷达、摄像头）采集的数据，一开始都是在局部坐标系下的。

为什么要区分？

因为路径规划需要在全局坐标系下做决策，但控制指令必须在局部坐标系下执行。这两者之间，需要一座桥——坐标变换。

3.2 坐标变换矩阵

坐标变换，说白了就是旋转加平移。

假设车辆在全局坐标系下的位姿是 (x, y, yaw)。其中 yaw 是航向角，也就是车头朝向与全局x轴的夹角。

那么，从局部坐标系到全局坐标系的变换矩阵长这样：

R = | cos(yaw)  -sin(yaw)  x |
    | sin(yaw)   cos(yaw)  y |
    |    0          0       1 |

这是一个3x3的齐次变换矩阵。为什么用齐次坐标？因为可以把旋转和平移写在一起，方便计算。

举个例子：

激光雷达检测到前方5米处有个障碍物。在局部坐标系下，它的坐标是 (5, 0)。那么它在全局坐标系下的位置就是：

| X_global |   | cos(yaw)  -sin(yaw)  x |   | 5 |
| Y_global | = | sin(yaw)   cos(yaw)  y | * | 0 |
|    1     |   |    0          0       1 |   | 1 |

算出来：

X_global = x + 5 * cos(yaw)
Y_global = y + 5 * sin(yaw)

反过来，从全局到局部，就是求逆矩阵。因为旋转矩阵是正交矩阵，逆矩阵等于转置矩阵：

R_inv = | cos(yaw)   sin(yaw)  -x*cos(yaw) - y*sin(yaw) |
        | -sin(yaw)  cos(yaw)   x*sin(yaw) - y*cos(yaw) |
        |    0          0                   1            |

3.3 车辆位姿表示 (x, y, yaw)

车辆的位姿，就是它在全局坐标系下的位置和朝向。

为什么用 (x, y, yaw) 而不是 (x, y, theta)？

其实一回事。yaw 就是偏航角，绕z轴旋转的角度。在平面运动中，车辆只有3个自由度：前后平移、左右平移、旋转。没有俯仰和侧倾（假设路面平坦）。

我遇到过一个问题：

有一次，我们用 GPS 获取车辆位姿，发现 yaw 值在 -π 到 π 之间跳变。比如从 179° 转到 -179°，明明只转了2°，但数值上跳了358°。这会导致路径规划算法以为车辆原地掉头了。

def normalize_angle(angle):
    while angle > np.pi:
        angle -= 2 * np.pi
    while angle < -np.pi:
        angle += 2 * np.pi
    return angle

另外，位姿的更新通常来自组合导航（IMU+GPS）或里程计。更新频率一般在 100Hz 左右。路径规划模块需要拿到最新的位姿，才能做滚动优化。

3.4 实际项目中的应用

我记得在一个园区物流项目中，车辆需要在狭窄通道里掉头。全局路径规划给出了一个掉头轨迹，但局部坐标系下的控制点计算错了——我把局部坐标当成了全局坐标直接用。

结果车在掉头时，方向盘打反了方向。还好速度慢，没有撞到墙。

从那以后，我养成了一个习惯：

• 每次做坐标变换，先打印出变换矩阵，肉眼检查一遍
• 在代码里明确标注「当前变量是在哪个坐标系下」
• 写单元测试，验证变换前后的一致性

你想想看，如果坐标变换错了，后面的路径规划、控制执行全是错的。这个错误会一路传导下去，最后车就不知道跑哪去了。

3.5 小结

这一节的内容，说白了就是三件事：

1. 搞清楚全局和局部坐标系，别搞混
2. 会用齐次变换矩阵做坐标转换
3. 处理好 yaw 角度的归一化

嗯，这些基础打牢了，后面讲路径生成算法时，你才不会晕头转向。