(一)路径规划算法---Graph Search Basis介绍

路径规划算法—Graph Search Basis

1.配置空间–Configuration Space

机器人由工作空间转化为配置空间，相当于一个点(上图的红点)，障碍物在原始的基础上外围添加一层膨胀层（膨胀层根据机器人的外形而定，例如机器人是圆形，膨胀层为半径），这样机器人就相当于一个质点，不需要考虑机器人半径或者长宽。

---

2.图搜索路径规划算法通用模板

• 维护一个容器，通常叫做****开放列表，该容器装载将来要去访问的节点

• 容器一开始是空的，刚开始装入的节点是起始节点$X_s$

• 循环

  • Remove：在容器中根据事先给定的一个指标弹出一个节点
  • Expandsion: 发现它可能存在的邻居节点
  • Push： 将邻居重新装入该容器

• 结束循环

那么当这个容器为空或者将终点加入到这个容器中，结束循环；其次还需要创建另外一个容器（关闭列表）来维护弹出的节点，防止节点回环，险入死循环。

---

3.常用的图搜索算法

3.1非启发式搜索算法

3.1.1 DFS（Deapth First Search）深度度优先搜索

DFS维护的容器是一个栈，栈的特点为先进后出(FILO “First In Last Out”),过程如下

---

3.1.2 BFS（Breadth First Search） 广度优先搜索

3.1.2.1 BFS介绍

BFS维护的容器是一个对列，对列的特点为先进先出(FIFO “First In First Out”),过程如下

3.1.2.2 BFS动态演示

动画1

动画2

BFS与DFS动画

---

3.1.3 Dijkstra

3.1.3.1Dijkstra算法过程

Dijkstra算法的策略为以最小损失的累积成本g(n)扩展/访问节点

伪代码如图

其中容器为priority queue，它表示优先对列，就是一个节点在插入容器的时候，容器会根据某个值自动排序，在C++中通常有std::multimap,大顶堆，小顶堆等实现方式。指标为g（n），通常用两点间的欧式距离或者曼哈顿距离。

运行过程如下

注意：当上图中的边的权重为1时，Dijstra=BFS

3.1.3.2Dijkstra动画

3.1.3.3Dijkstra与其他算法对比动画

Dijkstra与贪心算法

无障碍物

有障碍物

Dijkstra与BFS

---

3.2 启发式搜索算法

其实启发式搜索算法讲白了就是有更强的目的性，寻路过程中更倾向与终点靠拢

3.2.1 Greedy Best First Search 贪心算法

因为BFS和DFS算法根据“先在”或“最后在”的边界进行选择一个节点，其中贪婪最优根据某种规则选择最佳节点，方法称为启发式搜索。启发式表示你离目标点的距离。 启发式搜索或者贪心算法有很强的目的性，对接下来要扩展的节点用一个函数来估计起点到终点的距离 。

视频一：无障碍物

视频二：有障碍物

---

3.2.2 Astar算法

3.2.2.1 Astar算法过程

通俗的来说Astar=Dijkstra+Greedy Best First，那么Astar的容器依然为优先队列，控制指标为**$f(n)=g(n)+h(n)$,** 其中$g(n)$表示从开始状态到节点$n$的累计成本,$h(n)$ 表示从节点n到目标状态的估计最小代价 ，即目标代价。
伪代码和Dijksta差不多，如下

Astar运行过程如下

注意这里的$b$的$g(n)$表示的是累积成本，即$g(b)=g(a)+C_{ab}$。这里可能和网上不太一样

3.2.2.2 Astar的最佳性

其中S表示起始点,G表示最终点。其中$f(G)=g(G)+h(G)=5+0=5$,而$f(A)=g(A)+h(A)=1+6=7$,因此在Astar寻路过程中，会优先选择$S\to G$的路，但是在整个寻路过程，最佳的路线为$S\to A\to G$,因为$h$只是估计的成本值。这就导致了AStar寻路的最佳性，$h$的选择。

在上述问题中只要满足
$$h(n)\le h^{\ast }(n)$$
其中$h(n)$表示估计的成本，$h^{\ast }(n)$表示真实的成本。因此启发式距离($h(n)$)的设计如下

对于下图

Euclidean Distance: 估计的成本$h(n)$表示对角线距离，橘黄色表示；真实的成本$h^{\ast }(n)$用紫色表示，不管机器人满不满足对角线运动，欧式距离体现Astar的最佳性
Manhattan Distance:估计的成本$h(n)$用青色表示，当机器人满足对角线运动，那么曼哈顿距离算出来的$h(n)=2$,而真实的成本$h^{\ast }(n)=\sqrt{2}$,此时曼哈顿距离就不是最佳的；当机器人只允许上、下、左、右运动时，此时的曼哈顿距离就满足Astar算法的最佳性了。
注意： 当估计的成本$h(n)=0$时，此时Astar就退化成了Dijksta算法，此时Astar算法一定是最优的，因为$0=h(n)\le h^{\ast }(n)$。

3.2.2.3 变种的Astar算法

变种的Astar算法成本函数满足$f=a\times g+b\times h$ ,那么根据$a,b$的取值，可以得到不同的“变种Astar”

1. $a=0,b=1$: Astar=Greedy Best First，动图如下
   

2. $a=1,b=1$:Astar，动图如下
   

3. $a=1,b=0$:Astar=Dijksta,动图如下

4. $a=1,b=\epsilon >1$:Astar变成权重Astar，英文名为(Weighted Astar)，动图如下

3.2.2.4 Astar的一些说明

在上述仿真过程中，

Dijkstra类似圆形搜索，算法一定是最优的的，但速度不够快，搜索效率低；

Greedy搜索效率高，但并不是最优的；

Astar类似椭圆形搜索，有更明确的目的性，是结合了Dijkstra与贪心算法。但是在一些场合中更加注重搜索效率，对路径是否最优并不感兴趣（路径最优是最好的），此时就出现了带有Weighted Astar

Dijkstra

Astar

附：上述动图均来自 https://www.redblobgames.com 和 http://qiao.github.io/PathFinding.js/visual/