3. 任务规划概述

做机器人避障规划，第一步不是写算法，而是选地图。地图选错了，后面再牛的规划器也白搭。我这些年踩过的坑，有一半都跟地图表示有关。今天咱们就把三种主流地图掰开揉碎聊清楚。

3.1 三种地图的核心思想

先给个直观印象。你想想看，如果让你描述你所在的房间，你会怎么说？

• 栅格地图：把空间切成小格子，每个格子标“有障碍”或“无障碍”。就像像素画。
• 拓扑地图：只记关键地点和它们之间的连接关系。像地铁线路图。
• 特征地图：用几何特征（点、线、面）来描述环境。像建筑图纸。

这三种思路，对应着完全不同的数学工具和工程实践。我个人习惯在项目初期就把地图类型定死，因为后面改起来太痛苦了。

3.2 栅格地图（Occupancy Grid Map）

这是最直观、最常用的地图形式。说白了就是把环境离散化成固定大小的格子，每个格子存一个概率值，表示被占据的可能性。

3.2.1 工作原理

假设你有一个 10m×10m 的房间，栅格分辨率设为 0.1m。那就会生成 100×100 = 10,000 个格子。每个格子初始值为 0.5（未知），激光扫到空闲区域就降为 0.2，扫到障碍物就升为 0.8。

// 伪代码：栅格更新逻辑
void updateGrid(GridCell &cell, SensorReading reading) {
    if (reading == FREE) {
        cell.probability *= 0.6;  // 降低占据概率
    } else if (reading == OCCUPIED) {
        cell.probability = 1 - (1 - cell.probability) * 0.4;  // 升高
    }
    // 概率值钳制在 [0.1, 0.9] 之间，避免极端值
}

3.2.2 优缺点

维度	优点	缺点
精度	分辨率可调，理论上无限精细	分辨率越高，内存爆炸
实现	算法简单，A*、Dijkstra 直接跑	大场景下计算量指数增长
融合	多传感器数据容易融合（贝叶斯更新）	动态障碍物处理困难
存储	结构规整，适合 GPU 加速	稀疏环境浪费大量内存

3.3 拓扑地图（Topological Map）

拓扑地图不关心“这个地方长什么样”，只关心“从 A 能不能到 B”。它把环境抽象成图结构：节点是关键位置（路口、房间、地标），边是连通关系。

3.3.1 构建方式

我常用的构建方法有两种：

1. 手动标注：人肉标出关键节点和边。适合结构化环境（办公楼、仓库）。
2. 自动提取：从栅格地图或特征地图中，用算法（如 Voronoi 图）提取拓扑结构。适合未知环境。

// 拓扑地图的简单数据结构
struct TopoNode {
    int id;
    double x, y;          // 物理坐标（可选）
    vector<int> neighbors; // 邻居节点 ID
    double cost;           // 经过该节点的代价
};

struct TopoEdge {
    int from, to;
    double length;
    bool is_one_way;
};

3.3.2 优缺点

维度	优点	缺点
效率	规划速度极快，适合大场景	丢失细节，无法做精确避障
存储	内存占用极小	构建过程依赖其他地图
鲁棒性	对传感器噪声不敏感	环境变化后需要重建拓扑
扩展性	容易做分层规划	闭环检测困难

3.4 特征地图（Feature Map）

特征地图用几何基元来描述环境：点（角点、圆点）、线（墙边、柱子）、面（地面、墙面）。它比栅格地图更紧凑，比拓扑地图更精确。

3.4.1 常见特征类型

• 点特征：SIFT、SURF、ORB 等视觉特征点，或者激光反射强度极值点。
• 线特征：直线段（室内墙线）、圆弧（圆形柱子）。
• 面特征：平面（地面、墙面）、曲面（不规则地形）。

// 线特征示例
struct LineFeature {
    double rho;      // 极坐标距离
    double theta;    // 极坐标角度
    double length;   // 线段长度
    double quality;  // 拟合质量（0~1）
};

// 提取直线：用 RANSAC 或 Hough 变换
vector<LineFeature> extractLines(const PointCloud &cloud) {
    // RANSAC 迭代拟合直线
    // 返回置信度高于阈值的线段
}

3.4.2 优缺点

维度	优点	缺点
精度	亚格子级精度，适合定位	特征提取不稳定
存储	非常紧凑，适合长期建图	复杂环境特征数量爆炸
匹配	容易做回环检测和重定位	动态物体干扰严重
规划	可以直接用几何方法做路径规划	需要额外做碰撞检测

3.5 三种地图的对比与选择策略

直接上对比表，这是我做项目时贴在工位上的参考：

对比维度	栅格地图	拓扑地图	特征地图
空间精度	高（取决于分辨率）	低（只有拓扑关系）	中高（取决于特征密度）
规划速度	慢（O(n²) 级别）	极快（O(V+E)）	中（需要几何计算）
内存占用	高（O(宽×高)）	低（O(V+E)）	中（O(特征数)）
传感器需求	低（激光/声纳即可）	中（需要建图基础）	高（需要高质量传感器）
动态环境	差（需要频繁更新）	好（拓扑结构稳定）	中（特征可能消失）
典型场景	室内扫地机、仓储 AGV	园区物流、多楼层导航	自动驾驶、AR/VR 定位

3.5.1 我的选择原则

做了这么多年，我总结出三条铁律：

1. 场景小于 100m²，用栅格地图。 简单粗暴，A* 直接跑，别折腾。
2. 场景大于 1000m²，用拓扑地图做粗规划 + 栅格地图做局部规划。 这是工业界最稳的方案。
3. 需要长期运行和重定位，用特征地图。 比如自动驾驶，车跑了 100 公里还能认出同一个路口。

3.6 知识体系总览

下面这张图把三种地图的关系和适用场景串起来了。你仔细看，会发现它们不是互斥的，而是互补的。

3.7 工程建议

最后给几条实在的建议：

• 别一上来就搞混合地图。 先跑通一种，再慢慢加。我见过太多团队死在“架构完美但跑不动”上。
• 栅格地图的分辨率 设成机器人直径的 1/3 到 1/5 就够了。再细就是浪费。
• 拓扑地图的节点 不要超过 200 个，否则图搜索的优势就没了。
• 特征地图的提取参数 一定要根据传感器标定结果来调。别用默认参数。