坐标系与位姿表示：世界坐标系、机器人坐标系、位姿（x, y, θ）表示法、齐次变换矩阵

各位同学，咱们今天聊点硬核的。坐标系和位姿，说白了就是回答三个问题：我在哪？我要去哪？我怎么描述我去的过程？

我刚开始做AGV那会儿，觉得坐标系这东西太简单了，不就是画个十字轴嘛。结果第一次联调，机器人原地转圈，死活找不到目标点。查了半天，原来是坐标系搞反了。嗯，从那以后，我再也不敢小看这章内容了。

一、世界坐标系 vs 机器人坐标系

先搞清楚两个坐标系的关系。你想想看，AGV在车间里跑，它得知道自己在地图上的位置，也得知道自己车身上的传感器在什么方位。

• 世界坐标系（World Frame）：固定在地面上的参考系。通常以地图左下角为原点，X轴指向东，Y轴指向北。这个坐标系是全局的，所有物体都相对于它定位。
• 机器人坐标系（Robot Frame）：固定在AGV车体上的参考系。原点在机器人中心，X轴指向车头方向，Y轴指向左侧。这个坐标系是跟着机器人动的。

我在项目中遇到过一个问题：激光雷达扫描到的障碍物坐标，是在机器人坐标系下的。但路径规划需要世界坐标系下的坐标。如果不做转换，机器人就会撞墙。所以，坐标系变换是避不开的坎。

二、位姿（x, y, θ）表示法

位姿，就是位置+姿态。位置好理解，就是坐标点（x, y）。姿态呢？就是机器人朝哪个方向，用角度θ表示。

举个例子：
机器人位于世界坐标系中的 (2.5, 3.0) 处，车头朝向东北方向，角度为 45°。那么它的位姿就是：

pose = (x=2.5, y=3.0, θ=45°)

这里要注意几个细节：

• θ 通常以弧度为单位，但调试时我喜欢用角度，直观。代码里再转弧度。
• θ 的范围一般是 [-π, π] 或 [0, 2π)，看你的习惯。
• θ=0 表示车头朝向 X 轴正方向。

为什么用三个量就能描述一个平面机器人的状态？因为AGV在平面上运动，只有两个平移自由度和一个旋转自由度。说白了，它不能像无人机那样上下翻飞。

三、齐次变换矩阵

好了，现在问题来了：已知机器人在世界坐标系下的位姿，怎么把机器人坐标系下的一个点转换到世界坐标系下？

答案就是齐次变换矩阵。它是一个 3×3 的矩阵（对于二维平面），长这样：

| cosθ  -sinθ  x |
| sinθ   cosθ  y |
|  0      0    1 |

这个矩阵把旋转和平移打包在一起了。你想想看，如果没有齐次变换，你得先旋转再平移，两步操作。有了它，一步到位。

具体怎么用？假设机器人坐标系下有一个点 P_robot = (px, py)，想求它在世界坐标系下的坐标 P_world：

P_world = T * P_robot

其中 T 是齐次变换矩阵，P_robot 要写成齐次坐标形式 (px, py, 1)

展开计算：

P_world.x = cosθ * px - sinθ * py + x
P_world.y = sinθ * px + cosθ * py + y

是不是很简洁？

四、逆变换与复合变换

有正变换就有逆变换。已知世界坐标系下的点，想求它在机器人坐标系下的坐标，就用逆矩阵：

T_inv = | cosθ   sinθ  -x*cosθ - y*sinθ |
        | -sinθ  cosθ   x*sinθ - y*cosθ |
        |  0      0           1         |

复合变换更常用。比如AGV先移动到A点，再移动到B点，那么从起始点到B点的总变换，就是两个变换矩阵的乘积。

T_total = T_A * T_B

注意顺序：先发生的变换在右边，后发生的在左边。这个顺序搞反了，机器人就会跑到莫名其妙的地方去。

五、知识体系结构图

下面我用一张SVG图来总结本章的核心逻辑，帮你理清思路：

这张图把本章的核心逻辑串起来了：世界坐标系和机器人坐标系之间，通过位姿（x, y, θ）建立联系，而齐次变换矩阵就是实现这个联系的数学工具。

六、实战中的注意事项

场景	常见错误	我的建议
传感器数据转换	忘记将激光雷达坐标从机器人系转到世界系	写一个工具函数，输入位姿和点，输出转换后的点
多机器人协同	各机器人使用不同的世界坐标系原点	统一使用地图原点，并在启动时校准
路径跟踪	θ 角度跳变导致控制量突变	对 θ 做 unwrap 处理，避免 -π 到 π 的跳变

好了，坐标系和位姿这部分就讲到这里。记住一句话：没有坐标系，机器人就是瞎子；没有位姿，机器人就是路痴。 下一章我们会把这些知识用到运动学模型里，到时候你就知道今天学的有多重要了。

---