路径规划算法—Graph Search Basis
文章目录
1.配置空间–Configuration Space
机器人由工作空间转化为配置空间,相当于一个点(上图的红点),障碍物在原始的基础上外围添加一层膨胀层(膨胀层根据机器人的外形而定,例如机器人是圆形,膨胀层为半径),这样机器人就相当于一个质点,不需要考虑机器人半径或者长宽。
2.图搜索路径规划算法通用模板
-
维护一个容器,通常叫做****开放列表,该容器装载将来要去访问的节点
-
容器一开始是空的,刚开始装入的节点是起始节点 X s X_s Xs
-
循环
- 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
)
f(n)=g(n)+h(n)
f(n)=g(n)+h(n),** 其中
g
(
n
)
g(n)
g(n)表示从开始状态到节点
n
n
n的累计成本,
h
(
n
)
h(n)
h(n) 表示从节点n到目标状态的估计最小代价 ,即目标代价。
伪代码和Dijksta差不多,如下
Astar运行过程如下
注意这里的 b b b的 g ( n ) g(n) g(n)表示的是累积成本,即 g ( b ) = g ( a ) + C a b g(b)=g(a)+C_{ab} g(b)=g(a)+Cab。这里可能和网上不太一样
3.2.2.2 Astar的最佳性
其中S表示起始点,G表示最终点。其中 f ( G ) = g ( G ) + h ( G ) = 5 + 0 = 5 f(G)=g(G)+h(G)=5+0=5 f(G)=g(G)+h(G)=5+0=5,而 f ( A ) = g ( A ) + h ( A ) = 1 + 6 = 7 f(A)=g(A)+h(A)=1+6=7 f(A)=g(A)+h(A)=1+6=7,因此在Astar寻路过程中,会优先选择 S → G S \to G S→G的路,但是在整个寻路过程,最佳的路线为 S → A → G S \to A \to G S→A→G,因为 h h h只是估计的成本值。这就导致了AStar寻路的最佳性, h h h的选择。
在上述问题中只要满足
h
(
n
)
≤
h
∗
(
n
)
h(n) \le h^{*}(n)
h(n)≤h∗(n)
其中
h
(
n
)
h(n)
h(n)表示估计的成本,
h
∗
(
n
)
h^{*}(n)
h∗(n)表示真实的成本。因此启发式距离(
h
(
n
)
h(n)
h(n))的设计如下
对于下图
Euclidean Distance: 估计的成本
h
(
n
)
h(n)
h(n)表示对角线距离,橘黄色表示;真实的成本
h
∗
(
n
)
h^{*}(n)
h∗(n)用紫色表示,不管机器人满不满足对角线运动,欧式距离体现Astar的最佳性
Manhattan Distance:估计的成本
h
(
n
)
h(n)
h(n)用青色表示,当机器人满足对角线运动,那么曼哈顿距离算出来的
h
(
n
)
=
2
h(n)=2
h(n)=2,而真实的成本
h
∗
(
n
)
=
2
h^*(n)=\sqrt 2
h∗(n)=2,此时曼哈顿距离就不是最佳的;当机器人只允许上、下、左、右运动时,此时的曼哈顿距离就满足Astar算法的最佳性了。
注意: 当估计的成本
h
(
n
)
=
0
h(n)=0
h(n)=0时,此时Astar就退化成了Dijksta算法,此时Astar算法一定是最优的,因为
0
=
h
(
n
)
≤
h
∗
(
n
)
0=h(n) \le h^*(n)
0=h(n)≤h∗(n)。
3.2.2.3 变种的Astar算法
变种的Astar算法成本函数满足 f = a × g + b × h f=a \times g+b \times h f=a×g+b×h ,那么根据 a , b a,b a,b的取值,可以得到不同的“变种Astar”
-
a = 0 , b = 1 a=0,b=1 a=0,b=1: Astar=Greedy Best First,动图如下
-
a = 1 , b = 1 a=1,b=1 a=1,b=1:Astar,动图如下
-
a = 1 , b = 0 a=1,b=0 a=1,b=0:Astar=Dijksta,动图如下
- a = 1 , b = ε > 1 a=1,b= \varepsilon>1 a=1,b=ε>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/