坐标系与状态表示：全局坐标系、局部坐标系、机器人位姿表示（x, y, theta）

好，咱们进入第二个题目。坐标系这东西，听起来有点抽象，但说白了，它就是机器人的「世界观」。

我刚开始做机器人规划时，犯过一个低级错误——把局部坐标当成全局坐标用。结果机器人原地转了三圈，然后一头撞向墙壁。嗯，从那以后，我对坐标系的理解就深刻多了。

1. 全局坐标系：机器人的「世界地图」

全局坐标系，也叫世界坐标系。你可以把它想象成一张固定的地图。不管机器人走到哪，这个坐标系的原点和方向都不变。

我个人习惯用 X轴指向东，Y轴指向北。当然，这不是硬性规定。有些项目里，X轴指向正前方，Y轴指向左侧，也完全没问题。关键是——整个团队要统一。

我在项目中遇到过一个问题：两个传感器各自定义了自己的「全局坐标系」，结果数据融合时全乱了。后来我们强制规定：所有传感器数据必须先转换到同一个全局坐标系下。这个教训，值好几万块钱。

2. 局部坐标系：机器人的「第一人称视角」

局部坐标系是跟着机器人走的。机器人往左转，局部坐标系也跟着往左转。说白了，就是「以我为中心」看世界。

你想想看，机器人身上的激光雷达、摄像头，它们采集的数据都是相对于机器人自身的。如果不用局部坐标系，每次都要换算到全局坐标，计算量会爆炸。

为什么局部坐标系这么重要？因为控制指令都是基于局部坐标发出的。比如「向前走1米」，这个「向前」就是局部坐标系里的X轴方向。

3. 机器人位姿表示：(x, y, theta)

好了，现在我们要回答一个核心问题：如何描述机器人「在哪」以及「朝哪看」？

答案就是三个量：x, y, theta。

符号	含义	单位
x	机器人在全局坐标系中的X坐标	米 (m)
y	机器人在全局坐标系中的Y坐标	米 (m)
theta	机器人的朝向角（与X轴正方向的夹角）	弧度 (rad) 或度 (°)

这里有个坑，我必须要说：theta 的取值范围。有些库用 [-π, π]，有些用 [0, 2π)。我曾经因为没统一这个范围，导致角度跳变，机器人原地打转。嗯，后来我写了个函数专门做角度归一化。

4. 坐标系转换：从局部到全局

实际应用中，我们经常需要在全局坐标和局部坐标之间来回切换。比如：

• 激光雷达检测到前方2米处有障碍物（局部坐标）
• 需要把这个障碍物位置转换到全局坐标，更新地图
• 规划出一条全局路径后，再转换回局部坐标，生成控制指令

转换公式其实很简单，但容易写错。我直接给你代码：

// 局部坐标 (lx, ly) 转全局坐标 (gx, gy)
// 机器人位姿 (x, y, theta)
gx = x + lx * cos(theta) - ly * sin(theta);
gy = y + lx * sin(theta) + ly * cos(theta);

// 全局坐标 (gx, gy) 转局部坐标 (lx, ly)
dx = gx - x;
dy = gy - y;
lx = dx * cos(theta) + dy * sin(theta);
ly = -dx * sin(theta) + dy * cos(theta);

你看，其实就是个旋转加平移。我建议你把这段代码封装成函数，每次调用就行，别手算——我手算错过好几次。

5. 知识体系结构图

下面这张图，是我自己总结的坐标系与状态表示的知识结构。你看一眼，心里就有谱了。

6. 实际项目中的经验

最后，分享几个我在实际项目中踩过的坑：

• 角度单位要统一：有的传感器输出度，有的输出弧度。我建议全部转成弧度，因为数学库里的三角函数都认弧度。
• 坐标系方向要明确：是右手系还是左手系？Z轴朝上还是朝下？这些细节决定了你的转换公式对不对。
• 别忘了初始化：机器人启动时，位姿 (x, y, theta) 通常设为 (0, 0, 0)。但如果你用的是GPS，那初始值就是经纬度转换后的坐标。

好了，坐标系和状态表示就讲到这里。记住一句话：坐标系是机器人的世界观，搞错了世界观，后面全白搭。