坐标系与空间变换：从全局到局部

各位同学，今天我们来聊聊避障规划里最基础、也最容易翻车的一个话题——坐标系与空间变换。

说实话，我刚开始做机器人规划那会儿，觉得坐标系不就是个数学概念嘛，有什么好讲的？直到有一次，我在一个仓储AGV项目里，因为搞混了全局坐标系和局部坐标系，导致机器人直接撞上了货架……嗯，从那以后，我再也不敢小看这个“基础”了。

全局坐标系与局部坐标系

先说说这两个概念。全局坐标系，也叫世界坐标系，是固定不变的。你可以把它想象成整个场景的“绝对参考系”。比如一个仓库，我们以某个墙角为原点，东西方向为X轴，南北方向为Y轴，高度方向为Z轴——这就是全局坐标系。

局部坐标系呢，是跟着机器人走的。机器人每动一下，它的局部坐标系就跟着变。我习惯把局部坐标系想象成“机器人自己的视角”。比如机器人说“我前方1米有障碍物”，这个“前方”就是基于它的局部坐标系说的。

为什么会需要两个坐标系？你想想看，传感器（比如激光雷达）采集的数据，都是基于机器人自身的局部坐标系。但规划路径时，我们得知道机器人在整个地图里的位置。所以，必须把局部数据转换到全局坐标系下。

刚体变换与齐次坐标

刚体变换，说白了就是“不改变物体形状的移动和旋转”。机器人从A点走到B点，它的形状没变，只是位置和朝向变了——这就是刚体变换。

刚体变换包括两部分：旋转和平移。旋转用旋转矩阵R表示，平移用平移向量t表示。那么一个点从局部坐标系变换到全局坐标系，公式就是：

P_global = R * P_local + t

这个公式看起来简单，但实际用起来有个麻烦——旋转和平移是分开的，写代码时得分开处理。我个人习惯用齐次坐标来统一处理。

齐次坐标是什么？就是在三维坐标(x, y, z)后面加一个1，变成(x, y, z, 1)。这样，旋转和平移就可以合并成一个4x4的变换矩阵：

| R   t |
| 0   1 |

用齐次坐标，上面的变换就变成了：

P_global_h = T * P_local_h

其中T就是那个4x4的变换矩阵。这样做的好处是，多个变换可以连乘，比如先旋转再平移再旋转，直接乘起来就行，不用分开算。

欧拉角与四元数

说到旋转，就绕不开欧拉角和四元数。欧拉角大家可能比较熟悉，就是绕X、Y、Z三个轴分别旋转一个角度。比如“先绕Z轴转30度，再绕Y轴转45度”，这就是欧拉角的描述方式。

但欧拉角有个致命问题——万向锁。当某个轴旋转到90度时，另外两个轴会变得“重合”，导致自由度丢失。我在做无人机避障时遇到过这个问题，无人机在俯仰角接近90度时，航向和横滚的控制突然乱掉了，差点炸机。

所以，在实际的机器人规划中，我强烈推荐使用四元数。四元数用四个数(q0, q1, q2, q3)来表示旋转，没有万向锁问题，而且插值平滑，非常适合做轨迹规划。

四元数和欧拉角的转换公式如下：

// 欧拉角转四元数
q0 = cos(roll/2)*cos(pitch/2)*cos(yaw/2) + sin(roll/2)*sin(pitch/2)*sin(yaw/2)
q1 = sin(roll/2)*cos(pitch/2)*cos(yaw/2) - cos(roll/2)*sin(pitch/2)*sin(yaw/2)
q2 = cos(roll/2)*sin(pitch/2)*cos(yaw/2) + sin(roll/2)*cos(pitch/2)*sin(yaw/2)
q3 = cos(roll/2)*cos(pitch/2)*sin(yaw/2) - sin(roll/2)*sin(pitch/2)*cos(yaw/2)

坐标变换在避障中的应用

好了，理论讲完了，我们来看看实际怎么用。在避障场景中，坐标变换主要用在以下几个环节：

1. 传感器数据转换：激光雷达或深度相机采集的点云，是在传感器自身的局部坐标系下。需要转换到机器人坐标系，再转换到全局坐标系。
2. 障碍物位置映射：检测到的障碍物位置，需要从局部坐标系映射到全局坐标系，才能更新全局地图。
3. 路径点生成：规划出的路径点是在全局坐标系下的，但控制机器人运动时，需要转换到局部坐标系，告诉机器人“往左转还是往右转”。
4. 多传感器融合：如果机器人有多个传感器（比如两个激光雷达），它们的坐标系不同，必须统一到一个坐标系下才能融合数据。

下面我画了一张图，展示整个坐标变换的流程：

从这张图可以看出，坐标变换不是单向的，而是一个闭环。传感器数据从局部到全局，路径规划结果从全局回到局部，这样才能驱动机器人运动。

最后，给大家一个实用的建议：在写代码时，把坐标变换封装成一个独立的模块，不要散落在各个功能模块里。这样不仅便于调试，也方便后续扩展。比如你要加一个新的传感器，只需要在坐标变换模块里加一个转换函数就行，不用改其他地方。

好了，这一章的内容就到这里。坐标系和空间变换是避障规划的基石，看似简单，但细节很多。希望大家在实际项目中多动手、多踩坑，才能真正掌握。