坐标系与位姿表示：全局坐标系与局部坐标系
做路径跟踪，第一件事不是调参数，而是搞清楚你的机器人在哪、它要去哪。说白了，就是坐标系和位姿的问题。我见过不少新手，上来就写控制代码，结果机器人原地打转——后来发现是坐标系搞反了。嗯，咱们先把这地基打牢。
全局坐标系与局部坐标系
先问个问题：你站在路边看一辆车，和坐在车里看路，感觉一样吗？当然不一样。这就是全局坐标系和局部坐标系的区别。
- 全局坐标系（世界坐标系）：固定在地面上的参考系。比如地图上的经纬度，或者车间里的某个墙角。所有物体的位置，都可以用这个坐标系来描述。
- 局部坐标系（机器人坐标系）：固定在机器人身上的参考系。通常以机器人的中心为原点，正前方为X轴正方向。机器人感知到的障碍物、目标点，都是相对于它自己的。
我个人习惯把全局坐标系叫“上帝视角”，局部坐标系叫“第一人称视角”。做路径跟踪时，你需要在两个视角之间来回切换。
核心要点：全局坐标系用于规划路径，局部坐标系用于执行控制。两者之间的桥梁，就是坐标变换。
机器人位姿 (x, y, θ) 表示
机器人在二维平面上的状态，用三个量就够了：
- x, y：机器人在全局坐标系中的位置坐标
- θ：机器人的朝向角，通常以全局X轴正方向为0°，逆时针为正
举个例子：位姿 (2.0, 1.5, 30°) 表示机器人在全局坐标 (2.0, 1.5) 处，车头朝东北方向偏了30度。
我的经验：θ 的取值范围一定要统一。我习惯用 [-π, π] 而不是 [0, 2π)，因为做角度差计算时不容易出错。曾经有个项目，就是因为角度归一化没做好，导致机器人原地转圈——嗯，那晚加班到凌晨三点。
齐次变换矩阵
坐标变换怎么算？用齐次变换矩阵。它把旋转和平移打包成一个 3×3 的矩阵，方便计算。
从局部坐标系到全局坐标系的变换矩阵长这样：
T = | cosθ -sinθ x |
| sinθ cosθ y |
| 0 0 1 |假设机器人在局部坐标系中看到一个点 (x_local, y_local)，想求它在全局坐标系中的坐标？直接乘：
| x_global | | cosθ -sinθ x | | x_local |
| y_global | = | sinθ cosθ y | * | y_local |
| 1 | | 0 0 1 | | 1 |反过来，从全局到局部，求逆矩阵就行：
T_inv = | cosθ sinθ -x*cosθ - y*sinθ |
| -sinθ cosθ x*sinθ - y*cosθ |
| 0 0 1 |避坑指南：我曾经在项目中直接用矩阵求逆函数算 T_inv，结果因为浮点数精度问题，算出来的结果有微小误差。对于旋转矩阵，直接用上面的公式算逆，又快又准。
坐标变换基础
实际项目中，坐标变换就三种场景：
- 传感器数据转全局：激光雷达测到的障碍物是局部坐标，要转到全局坐标系才能建图
- 全局路径转局部：规划器给出的路径点是全局坐标，要转到局部坐标系才能跟踪
- 多机器人协同：每个机器人有自己的局部坐标系，需要统一到全局坐标系才能协作
举个例子，假设机器人位姿是 (1.0, 2.0, 45°)，它在局部坐标系中看到一个障碍物在 (0.5, 0.0)。那么障碍物的全局坐标是多少？
θ = 45° = π/4
cosθ ≈ 0.707, sinθ ≈ 0.707
x_global = 0.707 * 0.5 - 0.707 * 0.0 + 1.0 = 1.354
y_global = 0.707 * 0.5 + 0.707 * 0.0 + 2.0 = 2.354你看，算起来其实不复杂。但要注意，实际代码里角度要用弧度制，别写成度数了——我见过有人直接传45进去，结果机器人跑偏了还找不到原因。
一句话总结：坐标系是视角，位姿是状态，变换矩阵是工具。这三样东西搞明白了，路径跟踪的底层逻辑就通了。
知识体系结构图
下面这张图，把本章的核心逻辑串起来了：
我的建议：刚开始学的时候，别急着背公式。先拿张纸，画个坐标系，标几个点，手动算一遍变换。我当年就是这么干的——算错了几次之后，印象特别深。
好了，坐标系和位姿这部分就聊到这。记住：搞清楚了“机器人在哪、它朝哪看、目标在哪”，剩下的控制算法才有意义。不然你调得再好，也是南辕北辙。