坐标系基础：全局坐标系与局部坐标系、齐次坐标、坐标变换

说到坐标系，我刚开始做机器人时，觉得这东西太简单了——不就是画个十字轴嘛。直到第一次调试实车，发现明明代码里算得好好的，车就是不走直线。查了三天，最后发现是坐标系搞混了。嗯，从那以后，我再也不敢小看坐标系了。

说白了，全局坐标系就是“世界”的参考系。你站在路边看一辆车，车的位置、朝向，都是相对于地面说的。这个地面，就是全局坐标系。

局部坐标系呢？是跟着物体走的。比如车自己的坐标系——车头方向是X轴，左侧是Y轴，向上是Z轴。车转弯时，这个坐标系也跟着转。

我习惯把全局坐标系叫“地图坐标系”，局部坐标系叫“车身坐标系”。在自动驾驶里，这两个坐标系天天打架。

  核心区别一句话：全局坐标系固定不动，局部坐标系跟着物体动。

举个例子。你坐在车里，手机导航告诉你“前方200米右转”。这个200米是全局坐标系下的距离。但你的方向盘怎么打？得先把全局坐标转成车身坐标，才能知道“右转”对应的是打多少度方向。

你想想看，二维平面上的点用(x, y)表示，三维空间用(x, y, z)表示。那旋转用矩阵乘法，平移用向量加法。旋转和平移混在一起用，代码写起来特别别扭。

齐次坐标就是来解决这个问题的。它给坐标多加一个维度：二维变三维(x, y, w)，三维变四维(x, y, z, w)。

我项目中遇到过最典型的场景：传感器融合。激光雷达、摄像头、IMU，每个传感器都有自己的坐标系。要把它们统一到一个坐标系下，没有齐次坐标，代码能写到崩溃。

我的习惯：所有坐标运算都用齐次坐标。最后输出时再转回普通坐标。这样代码统一，不容易出错。

齐次坐标的转换规则很简单：

// 普通坐标转齐次坐标
(x, y) → (x, y, 1)

// 齐次坐标转普通坐标
(x, y, w) → (x/w, y/w)

为什么要除以w？因为w=0时表示无穷远点（方向向量），w=1时表示普通点。这个设计很巧妙，你细品。

坐标变换就两件事：旋转和平移。旋转用矩阵乘法，平移用向量加法。用齐次坐标后，两者统一成矩阵乘法。

二维旋转矩阵长这样：

R(θ) = [cosθ  -sinθ]
       [sinθ   cosθ ]

三维旋转就复杂多了。绕X轴、Y轴、Z轴分别旋转，矩阵形式不同。我建议你直接记住一个原则：旋转矩阵是正交矩阵，逆矩阵等于转置矩阵。这个性质在代码里特别有用。

我曾经踩过的坑：旋转矩阵的乘法顺序不可交换。先绕X轴转再绕Y轴转，和先绕Y轴转再绕X轴转，结果完全不同。做姿态解算时，顺序搞反了，车会往反方向跑。

平移就是加一个偏移量。在齐次坐标下，平移矩阵是：

T = [1  0  0  tx]
    [0  1  0  ty]
    [0  0  1  tz]
    [0  0  0  1 ]

tx, ty, tz就是三个方向上的平移量。

旋转和平移合在一起，就是4x4的齐次变换矩阵：

M = [R  t]
    [0  1]

其中R是3x3旋转矩阵，t是3x1平移向量。这个矩阵可以同时完成旋转和平移。

  实战经验：在代码里，我习惯用4x4矩阵表示所有变换。不管是传感器标定、路径规划还是控制指令，统一用这个格式。调试时打印矩阵，一眼就能看出问题。

下面这张图，是我自己总结的坐标系知识框架。你看一眼，心里就有数了。

我在项目里总结了一套坐标系管理方法，分享给你：

一个小技巧：调试时在关键位置打印变换矩阵的行列式。旋转矩阵的行列式应该是1，如果不是，说明矩阵有问题。

我曾经在坐标系上栽过的跟头，列出来给你参考：

嗯，坐标系基础就讲到这里。这些内容看着简单，但真到项目里，90%的bug都出在这里。你写代码时多留个心眼，能省不少调试时间。