Dijkstra算法、A*算法

最新推荐文章于 2024-01-03 09:57:54 发布

Jason.Li_0012

最新推荐文章于 2024-01-03 09:57:54 发布

阅读量1.9k

点赞数 2

分类专栏：路径规划算法文章标签：算法

本文链接：https://blog.csdn.net/weixin_45929038/article/details/122971956

版权

路径规划算法专栏收录该内容

3 篇文章 5 订阅

订阅专栏

Dijkstra算法（迪杰斯特拉算法）

代价函数g(n)

首先回顾BFS的弹出策略：先进先出，也即采用队列方式弹出节点。然而无论BFS还是DFS算法，都未考虑图中权重问题，也即从一个节点到下一个节点的代价值 $C o s t$ 。

为解决此问题，Dijkstra算法定义了一个函数 $g (n)$ ，用于计算从起点到当前节点的累计代价总值，用于存储节点的队列将更加 $g (n)$ 进行排序。

对于节点 $n$ 的邻居节点 $m$ ，首先判断是否被探索过（也即是否在已探索的队列中），若未探索过，则记其代价 $g (m)$ 为起点到节点 $n$ 的代价值 $g (n)$ 加上从节点 $n$ 到 $m$ 的代价值 $Cost_{nm}$ ；若探索过，则比较 $g (m)$ 同起到到节点 $n$ 加上节点 $n$ 到 $m$ 的代价值哪个小，并用小值更新 $g (m)$

在这里插入图片描述

同时，在Dijkstra算法中应保证所有被探索过的节点，从起点节点到该节点的代价 $g$ 为最小代价。

算法流程

首先应创建一个优先级队列（Priority Queue）能够对所有元素从小到大进行排列，该队列容器用于存储被访问的节点，并将自动弹出最小代价的节点。

优先级队列（Priority Queue）用起始节点 $x_s$ 初始化，并对图中所有节点的代价进行初始化：起点的代价值 $g(x_s)=0$ ，而其余图上节点尚未被探索，故而初始化为无穷大（一个极大值）： $g(n)=\infin$
$\begin{aligned} &Loop\\ &\qquad if\:queue\:is\:empty:\\ &\qquad\qquad return\:false;\\ &\qquad\qquad break;\\ &\qquad Remove\:the\:node\:n\:with\:the\:lowest\:g(n)\:from\:the\:priority\:queue\\ &\qquad Mark\:node\:n\:as\:expanded\\ &\qquad if\:the\:node\:n\:is\:the\:goal\:state:\\ &\qquad\qquad return\:true;\\ &\qquad\qquad break; \\ &\qquad For\:all\:unexpanded\:neighbours\:m\:of\:node\:n:\\ &\qquad\qquad if\:g(m)==infinite: \\ &\qquad\qquad\qquad g(m)=g(n)+Cost_{nm}\\ &\qquad\qquad\qquad Push\:node\:m\:into\:the\:queue\\ &\qquad\qquad if\:g(m)>g(n)+Cost_{nm} \\ &\qquad\qquad\qquad g(m)=g(n)+Cost_{nm}\\ &\qquad end\\ &End\:Loop \\ \end{aligned}$

举例说明

如下图所示，起点节点为 $S$ 其代价值为 $g (s) = 0$ ，目标节点为 $G$ ：

在这里插入图片描述

首先，用起始节点 $S$ 初始化优先级队列，弹出起点后对三个邻居节点 $d 、 e 、 p$ 计算其代价值：
$g(d)=0+3=3\\ g(e)=0+9=9\\ g(p)=0+1=1\\ g(p)<g(d)<g(e)$
对代价值排序后，弹出最小代价值的节点 $p$ 并得到其邻居节点 $q$ 及其代价值：
$g(q)=0+1+15=16\\ g(d)<g(e)<g(q)$
将节点 $q$ 置入优先级队列并弹出最小代价值的节点 $d$ ，得到其邻居节点 $b 、 c 、 e$ ，计算代价值：
$g(b)=0+3+1=4\\ g(c)=0+3+8=11\\ g(e)=0+3+2=5<9\to更新g(e)=5\\ g(b)<g(e)<g(c)<g(q)$
节点 $e$ 代价值被更新并再次排序，弹出节点 $b$ ……知道得到抵达目标点的最小代价结束循环。

算法效果

Dijkstra算法是完备的和最优的，也即当问题有解时，算法一定能够得到解且该解为最优解。

然而，算法由于未知目标节点的方向，故而向四周进行穷举探索，当权重为1时，退化等同于BFS。

$A^*$ 算法（A星算法）

代价函数 $f (n)$

由于Dijkstra算法未知目标节点方位，而导致四处穷举探索，将耗费额外的计算。此处考虑贪心算法的特性，采用heuristic能够知道当前节点同目标节点间的距离，从而进一步知道探索的方向。由此，在Dijkstra算法的基础上增加一个heuristic即可避免额外的计算。

在 $A^*$ 算法中，仍然沿用 $g (n)$ 记录由起点到当前节点 $n$ 路径上的最小代价值；用 $h (n)$ 记录当前节点 $n$ 同目标节点 $G$ 之间的估计代价（采用欧氏距离或曼哈顿距离计算）；

则定义代价函数 $f (n) = g (n) + h (n)$ ，优先级队列将根据 $f (n)$ 的大小进行弹出元素。

算法流程

$A^*$ 算法同Dijkstra算法的区别在于队列排序的依据由 $g (n)$ 改为 $f (n)$ ：

同样创建一个优先级队列并用起点节点 $x_s$ 初始化，对代价进行初始化 $g(s)=0,g(n)=\infin$ ，同时所有节点的 $h (n)$ 被预先定义：
$\begin{aligned} &Loop\\ &\qquad if\:queue\:is\:empty:\\ &\qquad\qquad return\:false;\\ &\qquad\qquad break;\\ &\qquad Remove\:the\:node\:n\:with\:the\:lowest\:f(n)=g(n)+h(n)\:from\:the\:priority\:queue\\ &\qquad Mark\:node\:n\:as\:expanded\\ &\qquad if\:the\:node\:n\:is\:the\:goal\:state:\\ &\qquad\qquad return\:true;\\ &\qquad\qquad break; \\ &\qquad For\:all\:unexpanded\:neighbours\:m\:of\:node\:n:\\ &\qquad\qquad if\:g(m)==infinite: \\ &\qquad\qquad\qquad g(m)=g(n)+Cost_{nm}\\ &\qquad\qquad\qquad Push\:node\:m\:into\:the\:queue\\ &\qquad\qquad if\:g(m)>g(n)+Cost_{nm} \\ &\qquad\qquad\qquad g(m)=g(n)+Cost_{nm}\\ &\qquad end\\ &End\:Loop \\ \end{aligned}$

举例说明

示例说明如下：

在这里插入图片描述

首先，用起点节点 $s$ 初始化容器，其代价 $f (s) = 6$ ，弹出起点节点并得到其邻居节点 $a$ ，计算其代价：
$f (a) = g (a) + h (a) = (0 + 1) + 5 = 6$
随后，弹出节点 $a$ 得到其邻居节点 $b 、 d 、 e$ ，计算其代价：
$f(b)=g(b)+h(b)=(0+1+1)+6=8\\ f(d)=g(d)+h(d)=(0+1+3)+2=6\\ f(e)=g(e)+h(e)=(0+1+5)+1=7\\ f(d)<f(e)<f(b)$
对代价值进行排序，弹出节点 $d$ 并得到其邻居节点 $G$ ，计算代价值：
$f(G)=g(G)+h(G)=(0+1+3+2)+0=6\\ f(G)<f(e)<f(b)$
弹出目标节点 $G$ ，抵达目标点，循环结束。

启发函数 $h (n)$

$A^*$ 算法的heuristic必须满足条件：任意节点的 $h (n)$ 值必须小于等于从当前节点抵达目标点的真实代价值 $h^*(n)$ ：
$h(n)\leq h^*(n)$

在这里插入图片描述

假设使用欧氏距离计算heuristic，也即不考虑障碍物环境，估计当前节点到目标节点的举例，可知，当机器人可以斜对角运动（如麦轮底盘），则若环境无障碍物存在： $h(n)=h^*(n)$ ；若环境有障碍物存在 $h(n)<h^*(n)$ ，欧氏距离始终保持条件成立。

假设使用曼哈顿距离计算heuristic，当机器人无法进行斜对角运动，可保证： $h(n)\leq h^*(n)$ ；当机器人能够机械能斜对角运动，则必然存在 $h(n)> h^*(n)$ 的现象发生。故而曼哈顿距离应看条件而定。

假设使用无穷范数（向量中最大元素的绝对值，也即横坐标之差和纵坐标之差中较大一项）或0（退化为Dijkstra算法），同样可以满足条件。

算法对比

在这里插入图片描述

左图进行对比，若权值为1，Dijkstra算法穷举四周，形成一圈圈的同心圆知道找到目标；而 $A^*$ 算法通过heuristic贪心于目标方向。

右图进行对比，同样在迷宫地图中， $A^*$ 更快抵达目标点。

Weighted $A^$ 算法（权重 $A^$ 算法）

已知对于 $A^*$ 算法，其heuristic必须满足： $h(n)\leq h^*(n)$ ，从而使得搜索得到的路径为最优路径。

Weighted $A^*$ 算法则使 $h(n)>h^*(n)$ ，从而更快的得到次优路径：
$f=g+\varepsilon h,\varepsilon>1$
权重 $\varepsilon$ 越大，算法越偏向于贪心算法。它通过用最优性换取规划速度，使得比之 $A^*$ 算法更快得到规划路径。

如下图为算法对比：

在这里插入图片描述

左侧为贪心算法，中间为权重 $A^*$ 算法，右侧为 $A^*$ 算法，可在如下网址进行测试：

http://qiao.github.io/PathFinding.js/visual/

工程建议

图结构

二维平面中栅格地图绘制图Graph结构时，可使用八联通结构进行：

在这里插入图片描述

优先级队列

在C++中可以使用如下数据类型构建优先级队列：

std::priority_queue
std::make_heap
std::multimap

对角Heuristic

经过前述分析，可知使用欧式距离、 $L\infin$ 范数、0构建heuristic在任何情况下满足 $A^*$ 条件，然而此类h(n)函数效率不高，如下图：

在这里插入图片描述

使用欧氏距离进行规划时，两侧将额外探索大量无用的节点，这将导致大量的浪费算力。使用对角Heuristic可解决上述问题。

在这里插入图片描述

假设现在有起点（上述图片中红色点）到终点（上述图片中黑色点），对于最短路径（上述黄色连线），可使用如下方式计算：
$dx=\begin{vmatrix}node.x-goal.x\end{vmatrix}\\ dy=\begin{vmatrix}node.y-goal.y\end{vmatrix}\\ h(node)=(dx+dy)+(\sqrt2-2)\cdot\min(dx,dy)$
称采用这种方式计算得到的heuristic为对角Heuristic（Diagonal Heuristic），同欧式距离进行对比如下：

在这里插入图片描述

左侧为对角Heuristic，将避免大量的无用探索存在。由于Path具有对称性质，故而左右两图的Path形状不同，但具有相同的效果（长度）。

Tie Breaker

由于Path具有对称性，故而在进行排序时，将存在大量Path有相同的代价值 $f$ ，此类Path具有相同的效果，从而导致同时扩展两条，致使产生大量无用节点被探索：

在这里插入图片描述

常用方式有两种，可大幅降低被探索的节点。

方式一：使相同的f变不同

为解决此类问题，可考虑将两个 $f$ 相同的节点，使其边不同：
$h=(1.0+p)\times h\\ p<\frac{行走一步最小的代价值}{最大路径的期望代价值}$
参数 $p$ 的分母为一个极大值，例如在一张大地图中进行规划，其代价值必然不会超过某一个极大值，如10000；参数 $p$ 的分子为机器人行走的最小代价，如1。

也即将每个节点的 $h$ 值扩大一个极小倍数，从而导致存在不同，其效果如下：

在这里插入图片描述

方式二：增加倾向性

当两条不同的Path具有相同的 $f$ 值时，可对其增加倾向性：使得机器人倾向于距离起点、终点连线的偏移量较小的方向进行探索：
$dx_1=\begin{vmatrix}node.x-goal.x\end{vmatrix}\\ dy_1=\begin{vmatrix}node.y-goal.y\end{vmatrix}\\\\ dx_s=\begin{vmatrix}start.x-goal.x\end{vmatrix}\\ dy_s=\begin{vmatrix}start.y-goal.y\end{vmatrix}\\\\ cross=\begin{vmatrix}dx_1\cdot dy_2-dx_2\cdot dy_1\end{vmatrix}\\ h=h+cross\times0.001$
式中， $c r o s s$ 为机器人探索的节点同起点、终点连线的偏移量。其效果如下：
在这里插入图片描述