基于图搜索的基础知识

配置空间

机器人规划的配置空间概念：一个空间包含所有机器人自由度的机器人配置，描述为C-space

机器人配置：表示对机器人上面所以点的位置的描述

机器人自由度：规划的时候用最少的坐标数量去表示机器人配置，例如无人机规划，在微分平坦中进行规划则是xyzyaw四个变量，所以对于无人机轨迹规划来说有四个自由度。

机器人配置空间：一个空间包含所有机器人自由度的机器人配置，描述为C-space

任何可能的机器人的位姿在C-space中描述为一个点

配置空间的意义：

在工作空间中进行规划,机器人有不同的形状和大小，比如有圆形的或方形的碰撞检测需要知道机器人的外形，然后再做检测，这样是费时费力的。

在配置空间中做规划

机器人在C-space中表示一个点，例如位置就是一个点属于R3一个位姿属于SO(3)

在配置空间中，机器人表示成了一个点，那么在配置空间中，障碍物也需要特殊的处理，把工作空间中的障碍物变成配置空间中的障碍物，被称为 配置空间障碍物或者C-obstacle这个工作是在运动规划前完成的，一次完成的工作。

障碍物安照机器人尺寸进行膨胀，上面机器人被设置成了一个点，只要点在障碍物外面，就不会发生碰撞

C-space是C-obstacle 和C-free组成的

经过配置空间的处理，路径规划变成了在C-free 中找到起点和终点的路径寻找。

在工作空间中，机器人有尺寸有形状，对于运动规划会带来困难，在配置空间中，机器人用一个点来描述，方便做运动规划

经过上述配置空间的操作，碰撞检测就进行了简化，这就是配置空间的意义。

机器人被看做是一个球体，半径为r

对障碍物安照半径r进行膨胀，蓝色就是膨胀后的障碍物，然后就可以进行路径规划和生成。

图和图搜索算法的基本概念

图的基础概念

图是有节点和边的一种表达方式

各节点由边连起来

边可以是有向的，也可以无向的

边也可以有权重，如果没有特殊说明，可以认为权重是一样的。

下面则是有权重的

图搜索基本概念

对于任何一个图搜索算法，首先要构造一个图

上图是抽象概念里的图

对于实际场景，我们需要人为构造一个图，以下是两种简单的例子

栅格地图的路径规划，里面的节点和相邻的节点是具有连接关系的，所以本身就是一个图了

基于采样的，没有天然的节点关系，需要人为构造一个图在里面，例如上面就是通过算法构造的有节点和边组成的图

图搜索算法

搜索总是从起始状态Xs开始,到Xg结束

对于搜索节点，可以构建一个搜索树

右边和左边是等价的，只是写成了树状的结构，这样看彼此关系更加清晰点

从起点搜索到终点后，回溯整个搜索过程，就可以得到希望的搜索路径

对于实际机器人来说，构建整个空间的搜索树，代价很大，所以需要尽可能快，但是不失搜索路径的算法。

图搜索算法框架

所有的图搜索算法都是按照下面的框架进行的:

1、维护一个容器，装载将来有可能访问的一个节点

2、容器初始化为空，放入的第一个节点就是起始状态Xs

3、循环：根据预先定义的一个指标或者目的，从容器中弹出一个节点 ，称之为访问一个节点，获取弹出节点所有的邻居节点—扩展，将这些邻居节点装入容器

4、结束循环：访问到了结束状态，或者自定义的一个指标，结束循环

有两个下面的问题需要注意：

什么时候结束循环？

一种可能就是容器空了，这代表没有了我们将来要访问的节点了，遍历完了所有节点

搜索到了结束节点

如果这个图本身是有回环的呢？

为了在图搜索中避免形成回环，永远走不出去，需要再维护一个新的容器，该容器装载着已经被访问过的节点，被访问过的节点不能再次被访问

图搜索优化的方向就是：

按照什么规则去访问节点，按照什么规则弹出节点，使我们尽可能快的找到终止节点。

图遍历算法：

广度优先搜索

深度优先搜索

广度优先搜索遵循先进先出的原则，维护的是一个队列

弹出元素，总是从队列的头弹出的

深度优先搜索遵循的是后进先出的原则，维护的是一个堆栈

弹出从最上面弹出，最后进入容器的，最先被弹出来

广度优先搜索（BFS）

特点：每次弹出层级最浅的一个节点，维护的是一个队列的结构

所以搜索过程是一层一层进行的

最直观的理解就是一层一层的进行

BFS步骤的拆分：

这样的一个图结构，BFS的步骤是下面这样的

初始化容器是空的，首先放入开始节点S

弹出（访问）S节点，容器就再次变为空的

对S进行扩展，从图上上可以看出，S可以扩展的是dep

安照定义的规则，将扩展的节点以规则顺序放入容器中

与DFS区别就是，从下面弹出节点，先弹出p节点

然后再进行循环

最终找到终止节点，结束循环，完成图搜索。

深度优先搜索（DFS）

特点：每次弹出的节点是最深层级的一个节点，维护的是一个堆栈

直观的理解就是没次把一个分支走到底

DFS步骤的拆分：

注意—维护的是一个栈的结构

这样的一个图结构，DFS的步骤是下面这样的

初始化容器是空的，首先放入开始节点S

弹出（访问）S节点，容器就再次变为空的

对S进行扩展，从图上上可以看出，S可以扩展的是dep

安照定义的规则，将扩展的节点以规则顺序放入容器中

然后深度优先搜索是弹出容器中的，后入的节点（层级最深的），即d节点

然后再扩展d节点，将扩展节点放入容器，再弹出该弹出的节点，再扩展，再放入的循环操作

最终找到终止节点，结束循环，完成图搜索。

BFS vs DFS

从最终的遍历结果图中，可以看到两者的区别

BFS是从开始节点一层一层的去向外扩展，直到扩展到了目标节点，然后回溯去找到最短路径。

DFS是从开始节点一条路走到头，去找到目标节点。

由结果来看，BFS 找到的路径是最短的

所以对应移动机器人，在做路径规划找最短路径时，做好的方法就是BFS。

审核编辑：彭静