坐标系与变换：世界坐标系、机器人坐标系、坐标变换

做机器人定位，说白了就是回答三个问题：我在哪？我要去哪？我怎么去？ 而这三个问题的答案，都绕不开坐标系。

我记得刚入行那会儿，带我的老工程师丢给我一句话：「搞不清坐标系，你的机器人就是个无头苍蝇。」当时我不以为然，直到第一次把里程计数据搞反，机器人原地转圈撞了墙……嗯，从那以后我再也不敢轻视坐标系了。

2.1 世界坐标系与机器人坐标系

先说说最基础的两个坐标系。

世界坐标系，也叫全局坐标系。你可以把它想象成一张地图的绝对参考系。比如我常跟团队说：「你把世界坐标系当作房间的墙角，原点定在左下角，X轴指向东，Y轴指向北。」一旦定下来，整个任务规划里的所有目标点、障碍物位置，都基于这个坐标系描述。

机器人坐标系，也叫局部坐标系或本体坐标系。原点在机器人中心，X轴通常指向机器人正前方，Y轴指向左侧。这个坐标系是「跟着机器人跑的」——机器人转个身，坐标系也跟着转。

为什么要区分这两个？

你想想看，传感器装在机器人身上，它测量到的数据（比如激光雷达扫到的障碍物距离）天然是在机器人坐标系下的。但你要规划路径，得把这些数据转换到世界坐标系里，才能知道障碍物在地图上的真实位置。这就是坐标变换要做的事。

2.2 坐标变换：平移与旋转

坐标变换就两件事：平移和旋转。

平移好理解。机器人从点A走到点B，位置变了，方向没变。用数学表达就是：新坐标 = 旧坐标 + 平移向量。我在项目中遇到过最典型的坑——平移向量的单位搞混了。传感器给的是毫米，地图用的是米，一乘一除差了1000倍，机器人直接「瞬移」到地图外面去了。

旋转稍微绕一点。机器人原地转个90度，它坐标系里的点，在世界坐标系里看，坐标值全变了。旋转用矩阵表示：

// 二维旋转矩阵（绕Z轴转θ角）
R(θ) = | cosθ  -sinθ |
       | sinθ   cosθ  |

// 三维旋转矩阵（绕Z轴）
Rz(θ) = | cosθ  -sinθ  0 |
        | sinθ   cosθ  0 |
        | 0      0     1 |

我个人习惯把旋转矩阵当作「坐标转换器」——你给它一个机器人坐标系下的点，它吐出来的是世界坐标系下的坐标。反过来，用转置矩阵就能从世界坐标转回机器人坐标。

2.3 齐次坐标：把平移和旋转统一起来

平移用加法，旋转用乘法。每次变换都要分开算，烦不烦？

齐次坐标就是来解决这个问题的。它给每个坐标多加一个维度——对于二维点(x, y)，齐次坐标是(x, y, 1)；三维点(x, y, z)变成(x, y, z, 1)。

这样一来，平移和旋转就能合并成一个4x4的变换矩阵：

// 齐次变换矩阵
T = | R   t |
    | 0   1 |

// 其中 R 是3x3旋转矩阵，t 是3x1平移向量
// 应用变换：p' = T * p

你想想看，原来要两步操作，现在一步矩阵乘法搞定。而且多个变换可以连乘——先转机器人坐标系，再转世界坐标系，直接乘出一个复合矩阵。这在任务规划里太常用了，尤其是做多传感器融合的时候。

2.4 欧拉角与四元数

说到旋转表示，就绕不开欧拉角和四元数。这两个东西，我建议你当成「工具」来理解，别被数学吓到。

欧拉角：用三个角度表示旋转——绕X轴转（横滚roll）、绕Y轴转（俯仰pitch）、绕Z轴转（偏航yaw）。直观，好理解。我调试时经常用欧拉角，因为一看数值就知道机器人歪了多少。

但欧拉角有个致命问题——万向锁。当pitch接近±90度时，roll和yaw会耦合，丢失一个自由度。我在做无人机任务规划时就踩过这个坑，飞控在俯冲时突然乱转，就是因为欧拉角算法在万向锁附近炸了。

四元数：用四个数表示旋转——一个实部加三个虚部。不直观，但稳定，没有万向锁，插值平滑。四元数长这样：

q = w + xi + yj + zk
// 其中 w^2 + x^2 + y^2 + z^2 = 1

// 用四元数旋转一个向量 v：
v' = q * v * q^(-1)

我个人习惯：内部计算用四元数，人机交互用欧拉角。代码里所有旋转运算都用四元数，只在显示给用户看或手动调试时转成欧拉角。

2.5 知识体系总览

下面这张图把本章的核心逻辑串起来了。我建议你多看几遍，搞清楚每个概念之间的依赖关系。

嗯，坐标系与变换这块，说白了就是定位的「语言」。你描述位置、规划路径、融合传感器数据，全得靠这套语言。搞懂了它，后面的定位算法学起来会顺畅很多。