坐标系与空间建模：全局坐标系与局部坐标系、栅格地图、拓扑地图、代价地图的构建
好，咱们进入第二个大话题——坐标系与空间建模。
说实话，做路径规划这么多年，我踩过最大的坑，往往不是算法本身，而是坐标系搞混了。你想想看，机器人明明看到障碍物在左边，结果一转弯撞上去了——十有八九是坐标系没对齐。所以这一节，咱们把坐标系和地图模型彻底聊透。
一、全局坐标系与局部坐标系
先问个问题：你站在房间里，说“杯子在我右手边”。这个“右手边”是相对于谁？
对，是相对于你自己的身体。这就是局部坐标系。而如果说“杯子在房间的东北角”，那就是全局坐标系了。
全局坐标系，也叫世界坐标系。它是固定不变的，通常以机器人的起点、或者地图的某个角落作为原点。所有物体的位置，都用这个坐标系下的坐标来描述。
局部坐标系，是跟着机器人走的。机器人转个身，它的局部坐标系也跟着转。传感器采集的数据，比如激光雷达扫到的点，默认都是在局部坐标系下的。
关键点：路径规划必须在全局坐标系下做，但传感器数据是局部坐标系下的。所以你必须做坐标变换。
我记得有一次做AGV小车，激光雷达装歪了2度，结果全局地图里的障碍物位置全偏了。排查了整整两天才发现是安装偏差。从那以后，我每次做系统集成，第一件事就是标定传感器安装角度。
二、坐标变换：从局部到全局
怎么把局部坐标变成全局坐标？说白了就是两步：旋转 + 平移。
假设机器人在全局坐标系下的位置是 (x_r, y_r)，朝向角是 θ。传感器测到一个障碍物在局部坐标系下的坐标是 (x_l, y_l)。那么它在全局坐标系下的坐标 (x_g, y_g) 就是：
x_g = x_r + x_l * cos(θ) - y_l * sin(θ)
y_g = y_r + x_l * sin(θ) + y_l * cos(θ)嗯，这就是一个标准的二维旋转平移变换。实际工程中，我们一般用齐次坐标矩阵来算，方便叠加多个变换。
我的小技巧：调试时，把局部坐标系下的点和全局坐标系下的点同时可视化出来。如果两个点对不上，八成是变换矩阵写错了。我曾经用这个方法半小时就定位了一个困扰团队两天的bug。
三、栅格地图
栅格地图，是最直观、最常用的一种地图表示方式。说白了，就是把环境切成一个个小格子，每个格子要么是空地，要么是障碍物，要么是未知区域。
每个格子的大小，就是地图的分辨率。分辨率越高，地图越精细，但计算量也越大。我一般建议：室内场景用5cm分辨率，室外场景用10-20cm分辨率。太细了没必要，反而拖慢规划速度。
| 分辨率 | 适用场景 | 优缺点 |
|---|---|---|
| 1-2cm | 精密装配、医疗机器人 | 精度极高，但计算量大，存储开销大 |
| 5cm | 室内服务机器人 | 平衡了精度和效率，我常用 |
| 10-20cm | 室外巡检、仓储AGV | 速度快，但可能漏掉小障碍物 |
| 50cm以上 | 大范围室外导航 | 只能用于粗略路径规划 |
栅格地图的构建，通常用激光雷达或深度相机。每个格子有一个概率值，表示它是障碍物的可能性。多次扫描后，概率会收敛。这就是所谓的占据栅格地图。
注意：栅格地图的更新要注意“动态障碍物”。比如一个人走过，他留下的点迹如果不及时清除，地图上就会多出一块永久障碍物。我见过一个项目，就是因为这个原因，机器人每次走到同一个地方就绕路，看起来像在“躲鬼”。
四、拓扑地图
栅格地图虽然直观，但有个问题——太大了。一个100m×100m的仓库，5cm分辨率，就是400万个格子。路径规划算法在上面跑，慢得让人抓狂。
这时候，拓扑地图就派上用场了。
拓扑地图，说白了就是只保留关键节点和它们之间的连接关系。比如走廊的拐角、房间的门口、电梯口，这些是节点。走廊本身是连接线。机器人只需要知道“从A点走到B点，经过C走廊”，不需要知道走廊里每一厘米的细节。
我做过一个项目，用栅格地图做全局规划，每次重规划要3秒。换成拓扑地图后，降到0.1秒。效果立竿见影。
拓扑地图的构建方法：
- 手动标注：人肉标出关键点，适合固定场景
- 自动提取：用Voronoi图或可视图算法，从栅格地图中自动提取拓扑结构
- 学习法：让机器人自己探索，记录关键位置
我的建议：实际工程中，我通常先用栅格地图做局部规划（精细避障），用拓扑地图做全局规划（快速寻路）。两者结合，效果最好。
五、代价地图
代价地图，是路径规划里最“聪明”的一层。它不是在描述“哪里有障碍物”，而是在描述“哪里走起来更划算”。
每个格子都有一个代价值。代价越高，路径规划算法就越不愿意走那里。代价值由多个因素叠加而成：
- 障碍物代价：离障碍物越近，代价越高。这是最基本的。
- 膨胀代价：把障碍物周围一圈也标成高代价，防止机器人擦着墙走。
- 地形代价：比如草地比水泥地难走，代价高一些。
- 时间代价：某些区域在特定时间段拥堵，代价临时升高。
- 风险代价：比如靠近楼梯口、悬崖边，代价高。
代价地图的构建，本质上是一个多层叠加的过程。我习惯用分层架构：
# 伪代码示例：代价地图叠加
costmap = base_costmap # 基础地图（障碍物）
costmap += inflation_layer # 膨胀层
costmap += static_layer # 静态障碍物（墙、柱子）
costmap += obstacle_layer # 动态障碍物（人、其他机器人）
costmap += inflation_layer # 再次膨胀，确保安全距离嗯，这里要注意顺序。我一般先加静态层，再加动态层，最后统一膨胀。如果顺序反了，动态障碍物的膨胀效果会被静态层覆盖掉。
核心思想：代价地图不是“真实世界”的映射，而是“规划偏好”的映射。你可以通过调整各层的权重，让机器人走不同的路线。比如想让它走大路，就把小路的代价调高。
六、三种地图的协同工作
实际系统中，这三种地图不是互斥的，而是协同工作的。我一般这样用：
- 拓扑地图做全局路径规划，快速找到从A到B的大致路线
- 代价地图做局部路径规划，在拓扑地图给出的路径上，精细调整避障
- 栅格地图作为底层数据源，实时更新障碍物信息，反馈给代价地图
举个例子：机器人要从仓库门口走到货架C3。拓扑地图告诉它“先走主通道，在第三个路口左转”。代价地图负责在行走过程中，避开突然出现的人或箱子。栅格地图则持续扫描环境，把新出现的障碍物更新到代价地图里。
这三层配合好了，机器人才能既快又安全地完成任务。
避坑指南：我曾经在一个项目里，只用了栅格地图做全局规划，结果每次重规划都要卡顿2-3秒。后来加上拓扑地图做全局层，栅格地图只做局部更新，流畅度直接翻倍。记住：全局规划要快，局部规划要准。
好了，坐标系与空间建模这块，咱们就聊到这儿。下一节，咱们进入路径规划算法的核心——搜索与优化。到时候我会分享一些实际调参的踩坑经历，保证让你少走弯路。