在本课程中，我们将考虑机器人如何决定如何到达目标的问题。这个问题通常被称为运动规划，它以不同的方式来描述不同的情况。您将学习一些解决这个问题的最常用方法，包括基于图的方法、基于采样的方法和人工势场法。
写在前面的
这是之前Coursera上宾夕法尼亚大学开的Robotics专项课程的第二个部分：Computational Motion Planning，也就是路径规划部分。讲的都是目前最基础的最经典的路径规划算法，适合初学者学习。
之前的第一部分：Aerial Robotics课程已经学完，感兴趣的可以去看一下：
Robotics: Aerial Robotics（空中机器人）笔记（一）： 初识四旋翼无人机
在这一章我们会讲基于图搜索的方法，其中包括Grassfire、Dijkstra和A*算法。
图结构
我们先从最简单的栅格图开始看起，在这里机器人被限制仅能在网格单元上向着东南西北方向相邻的格子进行移动，其中黑色的部分是障碍物。
现在我们把每一个未被障碍物占据的网格想象成一个节点， 并且如图所示在相邻节点之间用线段连接，会得到一个被称为图(graph)的结构，图是由一系列的节点和边组成，其中边将各节点连接了起来。
比如下图对应的是城市之间的收费公路，我们感兴趣的点也许是在不同城市之间找到最少花费的路径规划，其中每个节点（城市）之间边的属性代表城市之间公路的通行需要支付的费用。其中边的属性， 其将与代价或距离最小化问题相关。
Grassfire 算法
我们的目的是提出一个算法，可用来将机器人从起始位置引导到目标（终点）位置，同时能最小化执行的总步数。
针对这个特定问题，我们将在网格上对路径进行规划， 事实证明我们可以应用 一个非常简单的被称为Grassfire的算法来解决该路径规划问题。
首先在目标点上标记距离为0，下一步，我们将找到所有距离 目的地仅 1 步之遥的所有节点， 并将他们标记上一个数字 1。 再下一步，所有的 2 步之遥的 节点都标记上一个 2。 所有的节点有 3 步之遥的， 都标记上一个 3，以此类推。 直到抵达起始节点：
对于网格中的每一个单元， 相应被标注上的距离值代表的是这个点抵达目的地节点的最小步数注意到这些数字由目的地节点向外发散，就像火势蔓延一样，也是名字的由来。
如果找不到起始点到目标点的路径也能识别这种情况并返回结果：
其伪代码的形式如下，具体操作这个算法的方法之一， 是维护一个节点列表。 每次对这个算法进行迭代， 都把列表的第一个节点取出， 再将其所有还未访问过的相邻点标记上当前点标记数值加 1 的值。 然后把这个节点加入到这个列表的末尾：
更直观的图如下，其实这本质上是一个广度优先（BFS）算法，初始化的时候每个节点的距离标记为Ifinity，然后把目标节点标记为0，然后放到空列表（这个列表本质上是一个队列，遵循先进先出原则）里，然后开始进入循环。在循环里，弹出列表中的第一个节点，然后找与该节点中相邻的其它节点，如果这个节点的距离不是为Ifinity，将其标记成弹出节点的距离+1，然后放进列表中。直到列表为空。
其时间复杂度为，其中V代表节点的数量：
随着地图维度的增加，其节点会指数性地增加：
总体来说，如果从起始点到目的地的路径是存在的，Grassfire算法是可以找到一个边数量最少的路径规划方案。但是Grassfire算法只适用于各节点之前边没有权重的情况。
Dijkstra 算法
还有一类问题是在所有边有权重并且都是正值的路径规划问题，这一类问题最基础的解决方案就是Dijkstra 算法。
让我们考虑一个从某村庄到另一村庄的问题。用图来对这个问题进行建模，各个节点来代表不同的村庄， 而各边代表村庄之间的道路。不同边分别对应不同的数字， 这些数字代表道路的距离。 我们的目标是找到图上从节点 A 到节点 E 的最短路径：
刚开始的时候，我们用距离值 0 对起始节点进行标记。 红色代表这个节点已经被访问或标记过了。 随后，对于节点 A 相邻的各节点进行标记， 标记的时候我们会计算它们的距离值，并且记录下它们上一个节点，方便找到目标点后追踪回去。在这个例子中，相邻的就是节点 B、D 和 F，其上一个节点就是 A 。蓝色代表访问过的节点的相邻节点。
现在 A 访问完后我们发现节点 B 相应的距离标注最小，所以在下一轮迭代时我们会访问节点 B，通过访问 B 节点的所有相邻节点，将它们上一个节点记录成 B 并标注成蓝色，其中节点 C 路径长度为 8， 而这是现阶段我们所知道的 C 的最短路径，之后进入下一轮迭代。
访问完节点 B 后， 我们将会访问节点 D。我们会发现一条途经节点 D 抵达节点 C 的更短的路径， 于是我们可以对节点 C 的距离进行更新为 5，并把 G 的上一个节点修改成 D 以及标注成蓝色。
下一轮访问节点 F ，因为节点 F 是比较先进入列表里的。它会发现一条途经节点 F 抵达节点 G 的更短的路径，于是我们可以对节点 G 的距离进行更新为 9，其上一个节点更新为 F。最后访问节点 C，我们就会发现目标点 E，到此路径搜索结束，我们找到了一条从 A 到 E 的最优路径：A->D->C->E。
其伪代码的形式与Grassfire 算法相差不大，只是节点的距离值的更新由于引入了边的权重所以不再是加一而是边的权重（节点间的距离）。并且节点引入了父节点的属性方便追溯。
下图是更形象地描述了 Dijkstra 算法，其中最重要的是放置节点的列表采用了优先队列，也就是说，列表中存储节点的时候列表会自动按距离值从小到大排序这些节点，使其在下一轮迭代时能选择当前距离值最小的节点，其优先队列如何实现其实网上有很多种方法，不同编程语言也有不同的实现手段，这里不再赘述。
A*算法
Grassfire 算法 和 Dijkstra 算法实际上都能找到存在的最短路径， 而当路径不存在时，它们识别并报告。 但是这类算法都是一种无目的的，它们搜索过程中都会从起始节点向外围均匀发散，直至找到目标点，如下图所示：
事实上我们已经有了地图，并且知道了目标点的位置，那么我们何不利用起来，让其更快地搜索到目标点呢？A* 算法就是按照这种思想设计的一种算法，A* 算法其实是被称为最佳优先算法的一个例子。
最佳优先算法通过使用一种启发式逼近代价函数来划分不同的选择， 然后按照这种顺序来检视这些选项。
在路径规划问题上，因为我们实质上很清楚目的地的位置在哪， 于是我们可以知道大概哪个方向对于探索前往目标节点路径会更有成效。 A* 算法引入了启发式距离的概念， 即给定节点与目标节点之间的距离估计。 我们可以使用这些信息对路径检索进程进行辅助， 以使传播方向向着我们认为前往目标更快的方向上传播。
其实就是引入了一个启发式函数 H(n)，用来估计节点 n 和目标 g 之间的距离。常用的启发式函数有：
欧氏距离 ：
曼哈顿距离：
以上是二维平面下的距离计算公式。
其伪代码的形式如下，其中与 Dijkstra 算法的区别我已经用红色方框标注了出来，最明显的区别就是列表是以 f 值从小到大排序的，f 值就是距离值加上到启发式函数（目标点的估计距离值相加）。这样做的话我们就会优先探索离目标点近的节点，从而更快找到路径。
下一章链接：
Robotics: Computational Motion Planning（路径规划）笔记（二）:配置空间（Configuration Space）