Motion Planning学习笔记一：配置空间、图、图搜索、图遍历

        学习高飞博士的路径规划课程所总结的学习笔记。

目录

1、配置空间（Configuration Space, C-space）

2、图（Graphs）

3、图搜索（Graph Search Basis）

3.1、总体框架

3.2、两种基本的图遍历算法

3.3、启发式搜索（Heuristic search）

3.4、移动的代价

---

1、配置空间（Configuration Space, C-space）

        首先解释一下机器人的工作空间这个概念，机器人的工作空间是指机器人所能达到的空间点的集合，在此空间中，机器人是有大小和形状的，因此很难进行运动规划。

        配置空间是学习路径规划的基础，绝大多数路径规划算法都是基于配置空间提出的。如下图，配置空间中，机器人将变成一个质点，而机器人本身的大小和形状将会与障碍物做一个结合，以便进行碰撞检测。

图1-1. 圆形机器人的工作空间与配置空间

图1-2 方形机器人的工作空间与配置空间

       图1-1和图1-2是两种简单的机器人在配置空间中的表示。需要注意的是，因为机器人在工作空间中可能存在多个自由度，这时候将其转换到配置空间就会变得很复杂。因此，实际上经常采用近似的表示方法，即将机器人看作一个球体，然后再到配置空间中以球体半径为长度膨胀所有障碍物，如图1-3。

图1-3. 无人机的配置空间

2、图（Graphs）

        图是有节点和边的表达方式，节点与节点之间的关系：

        1）无向：机器人可以在任意连接的节点之间移动。

图2-1. 无向示意图

        2）有向：节点与节点只能根据箭头方向移动，例如图中机器人可以从节点S移动到节点A，但不能从节点A移动到节点S。

图2-2. 有向示意图

        3）权重：机器人在节点间移动的代价，代价可以是机器人移动的距离，也可以是机器人移动需要消耗的能量、时间等，可以根据问题自定义。

图2-3. 权重示意图

       对于任何一个路径规划的问题，必须先人为构造一个图。以下为两种常用地图：图2-4（a）为栅格地图，栅格地图每个节点之间天然形成了连接，栅格地图是基于搜索的路径规划方法用的地图；图2-4（b）为基于采样生成的地图，本身是只有障碍物而不具备任何节点，需要基于采样生成节点然后连接生成地图，是基于采样的路径规划方法用的地图。

   

（a）                                                                   (b)

图2-4. 两种常用地图。(a)栅格地图，(b)基于采样生成的地图

3、图搜索（Graph Search Basis）

3.1、总体框架

1. 建立一个容器，这个容器用以包含所有要访问的节点；
2. 容器由初始节点进行初始化；
3. 循环过程：
   1. 根据预先设定好的目的或指标弹出一个节点（访问一个节点）；
   2. 拓展：获取所有该节点的相邻节点；
   3. 将这些相邻节点储存到容器中。
4. 结束循环。

     两个问题：

        1、什么时候结束循环？

        两种可能：a）当容器是空的时候会结束循环；b）得到目标路径。

        2、如果图是循环的那该怎么办？（两个节点互通）

        为了防止死循环，可以新建一个容器，这个容器会包含所有已经被访问过的节点，这些节点不会被再次访问。

3.2、两种基本的图遍历算法

1）深度优先搜索（Depth First Search，DFS）：后进先出（LIFO）

图3-1. 深度优先搜索示意图

特点：每次访问都会优先访问容器中（如图3-1中的栈）最深（后）的节点。通俗地讲，深度优先搜索会一条路径搜索到底（深），没有找到目标则回溯再搜索其他路径。

图3-2. 搜索示意图

图3-3. 深度优先搜索流程

        举例：假设S为起始节点，G为目标节点。图3-2、图3-3中，容器首先由初始节点S初始化，随后弹出S进行拓展有A、V，按照某种规则将它们依次放入容器中（假设先入A），则容器中有V、A；弹出V，进一步拓展有C、F，按照某种规则依次放入容器中（假设先入C），则容器中现有F、C、A；下一步弹出F，无拓展则回溯；之后弹出C，拓展可得Z，放入容器中；弹出Z，Z拓展有G，G为目标节点，则循环结束，返回该路径SVCZG。事实上，可以看出SADG才是最短路径，因此，深度优先搜索找到的路径可能不是最优解。

       动图展示：

2）广度优先搜索（Breadth First Search，BFS）：先进先出（FIFO）

图3-4. 广度优先搜索示意图

特点：每次访问都会优先访问容器中（如图3-4中的队列）最先进入的节点。通俗地讲，广度优先搜索会如图3-5中的树状图一层一层地搜索完，直到找到目标。

图3-5. 搜索示意图

图3-6. 广度优先搜索流程

        举例：图3-5、图3-6中，可以看出，与深度优先搜索从容器顶部弹出节点不同，广度优先搜索是从容器底部弹出节点的。容器首先由初始节点S初始化，弹出S进行拓展有A、V，按照某种规则将它们依次放入容器中（假设A先入容器），则容器中有V、A；弹出A，拓展有C、D，按照某种规则依次放入容器中（假设C先放入），则此时容器中有D、C、V；下一步弹出V拓展可得C、F，按照某种规则依次放入容器中（假设C先放入），此时容器中有F、C、D、C；弹出节点C，……。如此循环直至搜索到目标G这一层则结束循环返回路径。按图3-5来，则会在ZGZ这一层结束循环，返回路径为SADG，该路径为最短路径。广度优先搜索会找到全局最优路径。

       动图展示：

        以上可以看出，深度优先搜索和广度优先搜索都是遵循3.1中的总体框架进行的。

3）广度优先搜索VS.深度优先搜索：使用哪一种？

        上述动画展示可以看出，按照两种搜索方式的特点，广度优先搜索更有利于找到最短路径，因此将广度优先搜索作为图搜索的基础。

3.3、启发式搜索（Heuristic search）

       前面提到的深度优先搜索和广度优先搜索是按照FIFO或者LIFO的方式将节点弹出容器。启发式搜索（贪心算法）与这两种不同的是，它弹出节点的方式是靠自定义的规则弹出的，这种规则称为启发（Heuristic）。启发的本质是猜测目标节点与当前节点有多近。注意这里说的是猜测，因为搜索过程中无法知道目标节点与当前节点的距离。如图3-7，一般合理的猜测有：1）欧式距离（Euclidean Distance），即两个节点间的距离（忽略障碍物）；2）曼哈顿距离（Manhattan Distance），即两个点在标准坐标系上的绝对轴距总和。启发会引导节点到正确的方向，但必须容易计算，否则反而降低搜索效率。

图3-7. 欧式距离与曼哈顿距离的示意图

       动图展示（无障碍物）：

       

 Greedy Best-First Search                    Breadth First Search

       动图展示（有障碍物）：

      

 Greedy Best-First Search                    Breadth First Search

       总结：启发式搜索（贪心算法）在无障碍物的情况下可以快速地得到最优全局路径，具有很强的目的性，会优先拓展离终点更近的节点；相对应地，广度优先搜索算法是层层递进地找最优全局路径。然而，实际中是有很多障碍物的，在这种情况下，广度优先搜索算法虽然花了更多的时间找到路径，但所找到的路径是最优的；启发式算法快速地找到了路径，但所找到的路径确是次优的（局部最优）。这是因为启发式算法在估计当前点到终点的距离时是忽略障碍物的。

3.4、移动的代价

图3-8. 深度优先搜索/广度优先搜索地图

        事实上，深度优先搜索与广度优先搜索使用的地图节点间的边是有权重的，且所有边的权重是相同的，如图3-8。在这种地图上，深度优先搜索与广度优先搜索才能成功实施。然而，对于一个机器人搜索问题，地图上节点间的边权重通常都是不同的。对于这种情况，则需要其他算法来进行搜索，例如：Dijkstra和A*等，在下篇笔记中会分析。

 此处给出两个路径规划算法可视化的网址，以便理解：

1、PathFinding.js

2、Pathfinding Visualizer

注：以上部分图片截取自高飞博士课件，高飞博士的教学视频（Motion Planning for Mobile Robots）可在深蓝学院中找到。

（转载请标明源地址）