SLAM路径规划算法常用地图介绍

在SLAM路径规划过程中，涉及不同地图，它们的作用和特点截然不同，绝大多数成熟的、用于实时导航的路径规划算法都基于某种形式的栅格图（通常是2D占据栅格地图或其衍生的代价地图），原因在于路径规划算法的需求：

• 快速查询：算法需要每秒数十次地查询“这个位置能不能走？”。栅格图像一个二维数组，可以通过坐标直接索引，速度极快。
• 离散化空间：A、D等搜索算法需要在离散的节点或网格上运行。栅格图天然地将连续环境离散化，完美适配这些算法。
• 概率表示：代价地图中的每个栅格可以有0-255的值，不仅能表示“能/不能”，还能表示“好不好走”，这为优化平滑、安全的路径提供了基础。

1. 静态先验地图 (Static Map)

• 来源：由SLAM建图过程产生并保存
• 格式：占据栅格地图 (OccupancyGrid) 是绝对标准。
• 特点：包含整个环境的布局，且只包含建图时的静态障碍物（墙壁、固定家具等），虽然每个栅格有0-100的值，但在规划中通常被二值化为“可行”和“不可行”区域。
• 用途：是全局代价地图和局部代价地图的基础静态层，为全局规划提供全局信息，为局部规划提供背景环境。

2. 代价地图 (Costmap) - 规划真正使用的“地图”

这是理解规划的关键，规划算法（A*, DWA, TEB）并非直接使用原始的静态先验地图，而是使用一个叫做“代价地图”的中间抽象层，一个代价地图通常由多层融合而成。

2.1 全局代价地图：

• 范围大：通常覆盖整个静态地图的范围。
• 更新频率低：主要依赖静态层，只在有已知长期障碍物（如新添了一个文件柜）时才需要更新。
• 分辨率：可以与静态地图相同。
• 用途：专供全局路径规划器使用，用于在全局范围内搜索路径。

2.2 局部代价地图：

• 范围小：通常是一个以机器人为中心的、2-5米边长的正方形窗口，只关心机器人 实时周围的情况。
• 更新频率极高：每收到一次激光扫描数据都会更新，以融入最新的动态障碍物信息。
• 分辨率：通常比全局代价地图更高，以进行精细的避障操作。
• 用途：专供局部路径规划器使用，用于实时避障和局部轨迹优化。

2.3 代价地图的图层 ：

代价地图的强大之处在于其分层结构，这些层被叠加融合成最终的成本值。

• 静态层：底层，直接来源于静态先验地图，是所有计算的基础。
• 障碍物层：核心层，根据实时传感器（激光雷达）数据，添加动态障碍物（如行人、临时放置的椅子）和未被映射到静态层中的障碍物，是实现实时避障的根本。
• 膨胀层：极其重要，围绕所有障碍物（静态和动态）产生一个“成本场”，距离障碍物越近，成本值越高；距离越远，成本值逐渐衰减为0，让规划器不仅知道“哪里不能走”，还知道“哪里不好走”，驱使规划器生成的路径会自然地与障碍物保持安全距离，而不是紧贴着障碍物走，可以通过调节膨胀半径和成本函数来控制机器人的“冒险”程度。

3. 地图数据流

下图清晰地展示了从建图到规划过程中，不同地图的生成、融合与使用流程：

4. 其他地图应用场景

4.1 点云地图

点云地图保留了环境最原始的3D几何信息，可以精准的3D避障与可行性检查，在规划器生成一条初步的路径后，可以将机器人的3D模型（或一个包围盒）沿着路径“摆放”，并实时检查其与点云地图是否发生碰撞，这是纯2D规划无法做到的。

• 3D避障：仓储机器人需要判断一个通道的高度是否能让它通过，无人机需要感知树枝和电线，带机械臂的机器人需要判断操作空间等
• 可视化、调试与测量：用于查看建图效果、测量环境的实际尺寸（门宽、货架高度）、进行虚拟漫游等是环境最真实的数字孪生。
• 3D匹配与定位：在自动驾驶中，通过将当前激光扫描与高精点云地图进行匹配，可以实现厘米级定位，比2D定位更鲁棒，但计算量也大得多。
• 地图后处理：从点云地图中可以提取出平面、曲面、生成网格或三角化表面，用于创建更高级的地图。

4.2 体素地图

体素地图是点云的一种规整化表示，除了也有上述点云的部分用途外，和栅格图一样，可以通过索引快速查询某个体素是否被占据，这对于需要快速进行3D碰撞检测的应用至关重要它特别擅长：

• 高效的3D空间查询与碰撞检测：无人机在复杂3D环境中的实时导航，机械臂运动规划，需要判断机械臂连杆是否会与环境碰撞。
• 距离场计算：计算每个体素到最近障碍物的距离，生成一个3D距离场。这对于3D路径规划非常有用，可以让路径不仅避障，还能最大化地与障碍物保持安全距离，是3D版的“膨胀层”概念。
• 多传感器融合：不同的传感器（如不同角度的激光雷达、深度相机）的数据可以更容易地融合到统一的体素网格中。

4.3 矢量地图

矢量地图完全跳出了几何表示的范畴，进入了语义地图的领域，它的用途是“理解”环境，而不是“感知”环境。矢量地图包含了车道线、路口、交通标志、红绿灯等语义信息，规划器可以利用这些信息做出决策：“我需要在前方路口右转，所以现在必须提前变道到最右侧车道”，提供的是规则和约束，而几何地图提供的是物理约束。

• 高阶任务规划与决策：自动驾驶汽车不是简单地从A点无碰撞地移动到B点，它需要遵守交通规则。
• 人机交互与可视化：给用户展示地图时，清晰的矢量道路图比杂乱的激光点云或栅格图直观得多，可以轻松地说“去A房间”，而不是“去地图坐标(12.3, 45.6)”。
• 长期语义定位：通过识别和理解环境中的语义元素（“我正停在第一个红绿灯下面”），来实现更鲁棒、对人类有意义的定位。