路径规划与建图：从洪水填充到 SLAM
你能在已知栅格里规划路线，在新障碍出现时重新规划，并讲清楚机器人是如何建出它所依据的那张地图的。
反应式导航能让一台机器人在房间里穿行，却始终不知道自己在哪里。这点很快就用完了：一个纯反应式机器人没法被告知去厨房，而且它会在一个马蹄形的房间里愉快地永远绕圈。规划就是答案，而它需要一样反应式控制永远不会有的东西——一张地图。
这条路线从地图免费的地方开始。在迷宫里，洪水填充从目标向外把距离铺开，机器人只要沿着梯度往下走，一发现原本以为是空的墙就立刻重新规划。然后世界变得杂乱起来：A* 在一张占据栅格上做规划，格子有代价，障碍需要膨胀，而启发式函数决定了你的规划器是快，还是仅仅正确。
最后一个节点是最诚实的一个。上面每一个规划器都假定你已经有地图，而真实机器人必须一边在会骗它的轮子上移动，一边把地图建出来。SLAM 就是建图与定位拒绝分开时发生的事，而回环闭合是阻止地图随里程计漂移而扭曲的关键。IMU 在这里同样凭这个理由挣得了一席之地：它直接测量旋转，这是打滑的轮子根本做不到的。
开始之前
你需要熟悉二维数组和基本的算法思维—— 队列，以及按某种顺序访问格子这个概念。电机控制 路线值得先做，因为这里每一个规划器都假定 机器人能执行被指令的运动，并大致报告自己走了多远。
你将会做到什么
- 用从目标出发的距离填充一张栅格，然后顺着梯度走到目标。
- 用一个你能证明绝不高估的启发式函数运行 A*，并解释这个性质为什么 让结果成为最短路径。
- 通过射线投射把测距读数变成占据栅格，自由格与占据格分别更新。
- 说明为什么单靠里程计无法在长时间运动中产出可用的地图，以及 SLAM 补上了什么。
大家卡在哪里
**高估的启发式函数。**只有当启发式函数从不声称某个距离大于真实剩余代价时， A* 才保证给出最短路径。在 4 连通栅格上使用欧氏距离，或者为了加快搜索而 把启发式函数整体放大，都会悄悄地返回并非最短的路径。
**为点式机器人做规划。**路径是在栅格里算出来的，但机器人是有宽度的。 一条擦着拐角走的路线在几何上合法，在物理上却是碰撞。规划之前先把障碍 按机器人的半径膨胀，问题就消失了。
**把未知当成自由。**一个没有任何东西观察过的格子不是空的——它是未被测量的。 把它当自由空间规划通过，机器人就会自信地开进那些从未被看到的东西里。未知 是第三种状态，而且必须始终保持为第三种状态。
**对角移动只算一格。**在 8 连通栅格上，对角一步覆盖的是 √2 个格子，不是一个。 按一格收费会产出偏爱楼梯的路径，以及干脆就是错的距离。
**假定机器人就在里程计所说的位置。**这里每一个规划器都作用在一个位姿上。轮 式里程计的漂移没有上限，因此一个完美的计划是从一个越来越虚构的位置执行的。 这个差距正是本路线最后一个节点的全部动机。
你需要什么
| 什么 | 这条路线为什么需要它 | 说明 |
|---|---|---|
| 熟悉二维数组和队列 | 这里每个算法都是在栅格上的一层循环 | 不需要高等数学；一个队列加一个嵌套循环就是全部工具 |
| 一台能执行指令动作的机器人 | 机器人开不出这个计划，计划就毫无价值 | 电机控制路线能带你走到这一步 |
| 陀螺仪或 IMU | 轮子测不出自己打滑穿过的那些旋转 | MPU-6050；建图节点依赖航向 |
| 测距传感器，最好能扫描 | 格子只有被测量才会变得已知 | 一个用舵机扫的声呐就足以建出一张粗糙的栅格 |
| 一个能可视化栅格的地方 | 盯着机器人来调试规划器几乎不可能 | 把 ASCII 打到串口，或者用本路线上的实验台 |
两个实验台完全在浏览器里运行，所以不带任何硬件也能完成洪水填充和覆盖 这两个节点。
为什么按这个顺序
**迷宫排在最前，因为地图是免费的。**在微鼠迷宫里，世界在构造上就是一张栅格 ——格子尺寸已知，墙就在格子边界上，机器人的位置永远是一个格子索引。这一次性 去掉了所有感知问题，只留下规划——而那正是你来这里要学的东西。在已知栅格上 做洪水填充，是“算出一张距离场，然后往下走”最干净的入门方式。
**A* 排在第二，因为它把迷宫的便利一条一条拿掉。**格子现在有代价，而不只是墙。 栅格更大了，所以把整张栅格展开一遍是浪费，启发式函数也就值得有了。机器人 有宽度，所以障碍需要膨胀。而格子还可能是未知的，这是一个迷宫从来不需要的、 真正全新的概念。
占据栅格排在第三，因为地图不再白送到你手上。 射线投射把一次测距读数变成关于许多格子的证据——射线上的一切更可能是自由的， 端点更可能是被占据的——而把这两者分开更新就是整个技术本身。
**IMU 刻意放在这里。**它出现在航向误差刚开始写进地图的那个节点上， 因为陀螺仪直接测量旋转，而打滑的轮子根本做不到。
SLAM 排在最后，因为它是承认前面那些节点都在假定一件假事。 上面每一个规划器都作用在一个位姿上，而每一个位姿都来自漂移没有上限的里程计。 SLAM 就是当你不再假装建图和定位可以分开解决时发生的事。
覆盖为这条路线收尾，因为它是最明显需要一张地图、 又最明显不可能用反应式做法完成的问题。“我到过所有地方了吗？“这个问题只有记忆才能回答。
检查点：如何判断一个阶段已经落地
| 阶段 | 检查内容 | 通过是什么样 |
|---|---|---|
| 洪水填充 | 打印一张小迷宫的距离场 | 目标为 0，数值向外递增，墙产生了你预期的绕行 |
| 重新规划 | 揭示一堵机器人原本以为是空的墙 | 距离场更新，机器人的下一步变了——而且不需要重新探索 |
| A* 正确性 | 在一张你手算得出答案的栅格上运行它 | 每次都和手算的最短路径等长 |
| 可采纳性 | 故意把启发式函数放大 2 倍 | 搜索变快，路径变长——现在你亲眼见过这笔交易 |
| 对角代价 | 在 8 连通栅格上穿过空旷区域规划 | 路径是一条笔直的对角线而不是楼梯；对角线代价是 √2 |
| 障碍膨胀 | 关闭膨胀绕过拐角规划一次，再打开一次 | 关闭时路径擦着拐角；打开时留出机器人半径的间隙 |
| 占据栅格 | 用传感器扫过一道门并渲染栅格 | 射线上出现自由空间，端点出现占据格，墙后仍是未知 |
| 未知的处理 | 让规划器尝试路由到未建图区域 | 它要么拒绝，要么有意识地明确承诺——两者都可以，猜测不行 |
| 漂移 | 走一个闭环，比较地图的起点和终点 | 地图不闭合——那道缺口正是回环闭合要修的 |
| 覆盖 | 对同样的时长，分别跑一次规划扫荡和一次随机行走 | 规划扫荡的覆盖率高得多，而且两个百分比你都说得出来 |
这条路线到哪里为止
这条路线是在一个静态、二维、尺寸已知的世界里做规划。这几个词每一个 都是值得点明的限制。
**静态。**这里没有任何机制处理有人从计划中横穿而过。动态障碍需要在全局 规划器下面再跑一个局部规划器，它的重规划速度要远快于整张栅格上的 A*。
**二维。**一张由自由格和占据格组成的栅格就是一张平面图。它无法表达那种 能从底下钻过去、却不能直接撞上去的桌子，所以正经的机器人会使用三维表示 或者分层的代价地图。
**基于栅格。**RRT 和 RRT* 这类采样规划器能处理高维空间——比如机械臂的 关节空间——而那里栅格完全无能为力。它们用最短路径的保证换来了“能干活” 这件事本身。
本路线也不涵盖粒子滤波或蒙特卡洛定位，那是一台机器人在一张别人给它的 地图里寻找自己、而不是自己建图时所用的方法。那是更简单、也常见得多的问题， 而且值得在伸手去抓完整 SLAM 之前先知道它的存在。
学习路线
路线
按顺序完成各个节点——完成一个就会解锁下一个。你的进度会保存在这台设备上。
已完成 0 / 9
常见问题
常见问题解答
在机器人路径规划中，洪水填充和 A* 有什么区别？
洪水填充计算的是从目标到栅格中每一个格子的距离，这样机器人无论身在何处，都可以顺着下坡走过去。A* 则从单一起点向单一目标搜索，只展开启发式函数认为有希望的那些格子。在微型迷宫鼠的迷宫里，洪水填充是正确的选择，因为机器人不断地发现新墙，每走几个格子就需要从新的位置重新规划——每个格子的距离都已经算好，重新规划就不花任何代价。而在一张很大的栅格上，如果你只从一个已知起点规划一次，展开整张地图就是浪费，这时 A* 才是正确的选择。
为什么我的 A* 路径会擦过障碍物的拐角？
因为你是按一个点来规划的，而机器人是有宽度的。这条路径在栅格上几何合法，在物理上却是一次碰撞，因为规划器根本不知道机器人占据的不止一个格子。标准的解决办法是在规划之前把每个障碍按机器人的半径膨胀——把被占据的格子向外扩展——这样规划器找到的任何路径，对真实机器人来说都自动是畅通的。这比在搜索过程中检查间隙要简单可靠得多。
什么样的 A* 启发式函数是可采纳的？为什么这很重要？
可采纳的启发式函数永远不会高估到达目标的真实剩余代价。正是这一个性质，保证了 A* 返回的是最短路径——只要在任何地方高估，搜索就可能认定一条本该被否决的路线。破坏它的常见方式有两种：在 4 连通栅格上使用欧氏距离，而那里的真实代价是曼哈顿距离，总是至少一样大；以及为了让搜索更快，故意把启发式函数放大。后者是一种真实存在的技术，但它是拿最短路径的保证去交换的，而你应该清楚自己在做这笔交换。
业余机器人需要 SLAM 吗？
只有当机器人必须在一个没人给它地图的空间里长时间运行、并且要知道自己在哪里时才需要。如果你能提供地图，单做定位要简单得多。如果空间小到里程计漂移始终可以容忍——一个迷宫、一个房间、几分钟的行驶——你同样不需要它。只有当这两个条件同时不成立时，SLAM 的复杂度才值得付出，而这是一道真实的门槛，而不是一枚勋章：很多真正能干的机器人从来都不需要它。
为什么机器人一边开，我的占据栅格地图就一边弯曲？
因为里程计漂移被写进了地图。每一次扫描都被放在机器人自认为所处的位姿上，所以一个已经慢慢转偏的航向估计，会把一条笔直的走廊画成地图里的一条曲线。建图本身没有任何问题——它只是忠实地记录了一个虚构的位姿。这正是回环闭合要解决的问题：识别出机器人回到了以前去过的地方，并修正整条轨迹，让两次观测彼此吻合。
为什么不能把未知格当作自由空间？
因为一个没有被任何东西观察过的格子是未被测量的，而不是空的，这个区别决定了机器人会不会开进它从未看见过的东西里。把未知当作自由，规划器就会心安理得地让路线穿过一片从没有任何传感器覆盖过的区域——而那里往往恰恰就是家具所在的位置，因为被遮挡的空间和被占据的空间高度相关。要把未知作为与自由、占据并列的第三种独立状态保留下来，并有意识地决定某个规划器是否允许让路线穿过它。