Dijkstra算法、A*算法

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

代价函数g(n)

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

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

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

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

算法流程

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

优先级队列（Priority Queue）用起始节点$x_s$初始化，并对图中所有节点的代价进行初始化：起点的代价值$g(x_s)=0$，而其余图上节点尚未被探索，故而初始化为无穷大（一个极大值）：$g(n)=\infty$
L o o p i f   q u e u e   i s   e m p t y : r e t u r n   f a l s e ; b r e a k ; R e m o v e   t h e   n o d e   n   w i t h   t h e   l o w e s t   g ( n )   f r o m   t h e   p r i o r i t y   q u e u e M a r k   n o d e   n   a s   e x p a n d e d i f   t h e   n o d e   n   i s   t h e   g o a l   s t a t e : r e t u r n   t r u e ; b r e a k ; F o r   a l l   u n e x p a n d e d   n e i g h b o u r s   m   o f   n o d e   n : i f   g ( m ) = = i n f i n i t e : g ( m ) = g ( n ) + C o s t n m P u s h   n o d e   m   i n t o   t h e   q u e u e i f   g ( m ) > g ( n ) + C o s t n m g ( m ) = g ( n ) + C o s t n m e n d E n d   L o o p \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} ​Loopifqueueisempty:returnfalse;break;Removethenodenwiththelowestg(n)fromthepriorityqueueMarknodenasexpandedifthenodenisthegoalstate:returntrue;break;Forallunexpandedneighboursmofnoden:ifg(m)==infinite:g(m)=g(n)+Costnm​Pushnodemintothequeueifg(m)>g(n)+Costnm​g(m)=g(n)+Costnm​endEndLoop​

举例说明

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

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

算法效果

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

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

$A^{\ast }$算法（A星算法）

代价函数$f(n)$

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

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

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

算法流程

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

同样创建一个优先级队列并用起点节点$x_s$初始化，对代价进行初始化$g(s)=0,g(n)=\infty$，同时所有节点的$h(n)$被预先定义：
L o o p i f   q u e u e   i s   e m p t y : r e t u r n   f a l s e ; b r e a k ; R e m o v e   t h e   n o d e   n   w i t h   t h e   l o w e s t   f ( n ) = g ( n ) + h ( n )   f r o m   t h e   p r i o r i t y   q u e u e M a r k   n o d e   n   a s   e x p a n d e d i f   t h e   n o d e   n   i s   t h e   g o a l   s t a t e : r e t u r n   t r u e ; b r e a k ; F o r   a l l   u n e x p a n d e d   n e i g h b o u r s   m   o f   n o d e   n : i f   g ( m ) = = i n f i n i t e : g ( m ) = g ( n ) + C o s t n m P u s h   n o d e   m   i n t o   t h e   q u e u e i f   g ( m ) > g ( n ) + C o s t n m g ( m ) = g ( n ) + C o s t n m e n d E n d   L o o p \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} ​Loopifqueueisempty:returnfalse;break;Removethenodenwiththelowestf(n)=g(n)+h(n)fromthepriorityqueueMarknodenasexpandedifthenodenisthegoalstate:returntrue;break;Forallunexpandedneighboursmofnoden:ifg(m)==infinite:g(m)=g(n)+Costnm​Pushnodemintothequeueifg(m)>g(n)+Costnm​g(m)=g(n)+Costnm​endEndLoop​

举例说明

示例说明如下：

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

启发函数$h(n)$

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

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

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

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

算法对比

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

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

Weighted $A^{\ast }$算法（权重$A^{\ast }$算法）

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

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

如下图为算法对比：

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

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

工程建议

图结构

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

优先级队列

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

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

对角Heuristic

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

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

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

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

Tie Breaker

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

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

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

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

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

方式二：增加倾向性

当两条不同的Path具有相同的$f$值时，可对其增加倾向性：使得机器人倾向于距离起点、终点连线的偏移量较小的方向进行探索：
$$\begin{gathered} d{x}_1=\left|\begin{array}{c}node.x-goal.x\end{array}\right| \\ d{y}_1=\left|\begin{array}{c}node.y-goal.y\end{array}\right| \\ d{x}_s=\left|\begin{array}{c}start.x-goal.x\end{array}\right| \\ d{y}_s=\left|\begin{array}{c}start.y-goal.y\end{array}\right| \\ cross=\left|\begin{array}{c}d{x}_1\cdot d{y}_2-d{x}_2\cdot d{y}_1\end{array}\right| \\ h=h+cross\times 0.001 \end{gathered}$$
式中，$cross$为机器人探索的节点同起点、终点连线的偏移量。其效果如下：

问题存在

上述方式在无障碍物下将存在较好的效果，然而在障碍物环境下，不同的Tie Breaker将导致不同的生成效果。

如上图，右侧生成的轨迹虽然最优，但实际不符合机器人进行移动的特点，应对其进行调整。