Dijkstra算法(迪杰斯特拉算法)
代价函数g(n)
首先回顾BFS的弹出策略:先进先出,也即采用队列方式弹出节点。然而无论BFS还是DFS算法,都未考虑图中权重问题,也即从一个节点到下一个节点的代价值 C o s t Cost Cost。
为解决此问题,Dijkstra算法定义了一个函数 g ( n ) g(n) g(n),用于计算从起点到当前节点的累计代价总值,用于存储节点的队列将更加 g ( n ) g(n) g(n)进行排序。
对于节点 n n n的邻居节点 m m m,首先判断是否被探索过(也即是否在已探索的队列中),若未探索过,则记其代价 g ( m ) g(m) g(m)为起点到节点 n n n的代价值 g ( n ) g(n) g(n)加上从节点 n n n到 m m m的代价值 C o s t n m Cost_{nm} Costnm;若探索过,则比较 g ( m ) g(m) g(m)同起到到节点 n n n加上节点 n n n到 m m m的代价值哪个小,并用小值更新 g ( m ) g(m) g(m)
同时,在Dijkstra算法中应保证所有被探索过的节点,从起点节点到该节点的代价 g g g为最小代价。
算法流程
首先应创建一个优先级队列(Priority Queue)能够对所有元素从小到大进行排列,该队列容器用于存储被访问的节点,并将自动弹出最小代价的节点。
优先级队列(Priority Queue)用起始节点
x
s
x_s
xs初始化,并对图中所有节点的代价进行初始化:起点的代价值
g
(
x
s
)
=
0
g(x_s)=0
g(xs)=0,而其余图上节点尚未被探索,故而初始化为无穷大(一个极大值):
g
(
n
)
=
∞
g(n)=\infin
g(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
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)+CostnmPushnodemintothequeueifg(m)>g(n)+Costnmg(m)=g(n)+CostnmendEndLoop
举例说明
如下图所示,起点节点为 S S S其代价值为 g ( s ) = 0 g(s)=0 g(s)=0,目标节点为 G G G:
首先,用起始节点
S
S
S初始化优先级队列,弹出起点后对三个邻居节点
d
、
e
、
p
d、e、p
d、e、p计算其代价值:
g
(
d
)
=
0
+
3
=
3
g
(
e
)
=
0
+
9
=
9
g
(
p
)
=
0
+
1
=
1
g
(
p
)
<
g
(
d
)
<
g
(
e
)
g(d)=0+3=3\\ g(e)=0+9=9\\ g(p)=0+1=1\\ g(p)<g(d)<g(e)
g(d)=0+3=3g(e)=0+9=9g(p)=0+1=1g(p)<g(d)<g(e)
对代价值排序后,弹出最小代价值的节点
p
p
p并得到其邻居节点
q
q
q及其代价值:
g
(
q
)
=
0
+
1
+
15
=
16
g
(
d
)
<
g
(
e
)
<
g
(
q
)
g(q)=0+1+15=16\\ g(d)<g(e)<g(q)
g(q)=0+1+15=16g(d)<g(e)<g(q)
将节点
q
q
q置入优先级队列并弹出最小代价值的节点
d
d
d,得到其邻居节点
b
、
c
、
e
b、c、e
b、c、e,计算代价值:
g
(
b
)
=
0
+
3
+
1
=
4
g
(
c
)
=
0
+
3
+
8
=
11
g
(
e
)
=
0
+
3
+
2
=
5
<
9
→
更
新
g
(
e
)
=
5
g
(
b
)
<
g
(
e
)
<
g
(
c
)
<
g
(
q
)
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)
g(b)=0+3+1=4g(c)=0+3+8=11g(e)=0+3+2=5<9→更新g(e)=5g(b)<g(e)<g(c)<g(q)
节点
e
e
e代价值被更新并再次排序,弹出节点
b
b
b……知道得到抵达目标点的最小代价结束循环。
算法效果
Dijkstra算法是完备的和最优的,也即当问题有解时,算法一定能够得到解且该解为最优解。
然而,算法由于未知目标节点的方向,故而向四周进行穷举探索,当权重为1时,退化等同于BFS。
A ∗ A^* A∗算法(A星算法)
代价函数 f ( n ) f(n) f(n)
由于Dijkstra算法未知目标节点方位,而导致四处穷举探索,将耗费额外的计算。此处考虑贪心算法的特性,采用heuristic能够知道当前节点同目标节点间的距离,从而进一步知道探索的方向。由此,在Dijkstra算法的基础上增加一个heuristic即可避免额外的计算。
在 A ∗ A^* A∗算法中,仍然沿用 g ( n ) g(n) g(n)记录由起点到当前节点 n n n路径上的最小代价值;用 h ( n ) h(n) h(n)记录当前节点 n n n同目标节点 G G G之间的估计代价(采用欧氏距离或曼哈顿距离计算);
则定义代价函数 f ( n ) = g ( n ) + h ( n ) f(n)=g(n)+h(n) f(n)=g(n)+h(n),优先级队列将根据 f ( n ) f(n) f(n)的大小进行弹出元素。
算法流程
A ∗ A^* A∗算法同Dijkstra算法的区别在于队列排序的依据由 g ( n ) g(n) g(n)改为 f ( n ) f(n) f(n):
同样创建一个优先级队列并用起点节点
x
s
x_s
xs初始化,对代价进行初始化
g
(
s
)
=
0
,
g
(
n
)
=
∞
g(s)=0,g(n)=\infin
g(s)=0,g(n)=∞,同时所有节点的
h
(
n
)
h(n)
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)+CostnmPushnodemintothequeueifg(m)>g(n)+Costnmg(m)=g(n)+CostnmendEndLoop
举例说明
示例说明如下:
首先,用起点节点
s
s
s初始化容器,其代价
f
(
s
)
=
6
f(s)=6
f(s)=6,弹出起点节点并得到其邻居节点
a
a
a,计算其代价:
f
(
a
)
=
g
(
a
)
+
h
(
a
)
=
(
0
+
1
)
+
5
=
6
f(a)=g(a)+h(a)=(0+1)+5=6
f(a)=g(a)+h(a)=(0+1)+5=6
随后,弹出节点
a
a
a得到其邻居节点
b
、
d
、
e
b、d、e
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
)
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)
f(b)=g(b)+h(b)=(0+1+1)+6=8f(d)=g(d)+h(d)=(0+1+3)+2=6f(e)=g(e)+h(e)=(0+1+5)+1=7f(d)<f(e)<f(b)
对代价值进行排序,弹出节点
d
d
d并得到其邻居节点
G
G
G,计算代价值:
f
(
G
)
=
g
(
G
)
+
h
(
G
)
=
(
0
+
1
+
3
+
2
)
+
0
=
6
f
(
G
)
<
f
(
e
)
<
f
(
b
)
f(G)=g(G)+h(G)=(0+1+3+2)+0=6\\ f(G)<f(e)<f(b)
f(G)=g(G)+h(G)=(0+1+3+2)+0=6f(G)<f(e)<f(b)
弹出目标节点
G
G
G,抵达目标点,循环结束。
启发函数 h ( n ) h(n) h(n)
A
∗
A^*
A∗算法的heuristic必须满足条件:任意节点的
h
(
n
)
h(n)
h(n)值必须小于等于从当前节点抵达目标点的真实代价值
h
∗
(
n
)
h^*(n)
h∗(n):
h
(
n
)
≤
h
∗
(
n
)
h(n)\leq h^*(n)
h(n)≤h∗(n)
假设使用欧氏距离计算heuristic,也即不考虑障碍物环境,估计当前节点到目标节点的举例,可知,当机器人可以斜对角运动(如麦轮底盘),则若环境无障碍物存在: h ( n ) = h ∗ ( n ) h(n)=h^*(n) h(n)=h∗(n);若环境有障碍物存在 h ( n ) < h ∗ ( n ) h(n)<h^*(n) h(n)<h∗(n),欧氏距离始终保持条件成立。
假设使用曼哈顿距离计算heuristic,当机器人无法进行斜对角运动,可保证: h ( n ) ≤ h ∗ ( n ) h(n)\leq h^*(n) h(n)≤h∗(n);当机器人能够机械能斜对角运动,则必然存在 h ( n ) > h ∗ ( n ) h(n)> h^*(n) h(n)>h∗(n)的现象发生。故而曼哈顿距离应看条件而定。
假设使用无穷范数(向量中最大元素的绝对值,也即横坐标之差和纵坐标之差中较大一项)或0(退化为Dijkstra算法),同样可以满足条件。
算法对比
左图进行对比,若权值为1,Dijkstra算法穷举四周,形成一圈圈的同心圆知道找到目标;而 A ∗ A^* A∗算法通过heuristic贪心于目标方向。
右图进行对比,同样在迷宫地图中, A ∗ A^* A∗更快抵达目标点。
Weighted A ∗ A^* A∗算法(权重 A ∗ A^* A∗算法)
已知对于 A ∗ A^* A∗算法,其heuristic必须满足: h ( n ) ≤ h ∗ ( n ) h(n)\leq h^*(n) h(n)≤h∗(n),从而使得搜索得到的路径为最优路径。
Weighted
A
∗
A^*
A∗算法则使
h
(
n
)
>
h
∗
(
n
)
h(n)>h^*(n)
h(n)>h∗(n),从而更快的得到次优路径:
f
=
g
+
ε
h
,
ε
>
1
f=g+\varepsilon h,\varepsilon>1
f=g+εh,ε>1
权重
ε
\varepsilon
ε越大,算法越偏向于贪心算法。它通过用最优性换取规划速度,使得比之
A
∗
A^*
A∗算法更快得到规划路径。
如下图为算法对比:
左侧为贪心算法,中间为权重 A ∗ A^* A∗算法,右侧为 A ∗ A^* A∗算法,可在如下网址进行测试:
http://qiao.github.io/PathFinding.js/visual/
工程建议
图结构
二维平面中栅格地图绘制图Graph结构时,可使用八联通结构进行:
优先级队列
在C++中可以使用如下数据类型构建优先级队列:
std::priority_queue
std::make_heap
std::multimap
对角Heuristic
经过前述分析,可知使用欧式距离、 L ∞ L\infin L∞范数、0构建heuristic在任何情况下满足 A ∗ A^* A∗条件,然而此类h(n)函数效率不高,如下图:
使用欧氏距离进行规划时,两侧将额外探索大量无用的节点,这将导致大量的浪费算力。使用对角Heuristic可解决上述问题。
假设现在有起点(上述图片中红色点)到终点(上述图片中黑色点),对于最短路径(上述黄色连线),可使用如下方式计算:
d
x
=
∣
n
o
d
e
.
x
−
g
o
a
l
.
x
∣
d
y
=
∣
n
o
d
e
.
y
−
g
o
a
l
.
y
∣
h
(
n
o
d
e
)
=
(
d
x
+
d
y
)
+
(
2
−
2
)
⋅
min
(
d
x
,
d
y
)
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)
dx=∣∣node.x−goal.x∣∣dy=∣∣node.y−goal.y∣∣h(node)=(dx+dy)+(2−2)⋅min(dx,dy)
称采用这种方式计算得到的heuristic为对角Heuristic(Diagonal Heuristic),同欧式距离进行对比如下:
左侧为对角Heuristic,将避免大量的无用探索存在。由于Path具有对称性质,故而左右两图的Path形状不同,但具有相同的效果(长度)。
Tie Breaker
由于Path具有对称性,故而在进行排序时,将存在大量Path有相同的代价值 f f f,此类Path具有相同的效果,从而导致同时扩展两条,致使产生大量无用节点被探索:
常用方式有两种,可大幅降低被探索的节点。
方式一:使相同的f变不同
为解决此类问题,可考虑将两个
f
f
f相同的节点,使其边不同:
h
=
(
1.0
+
p
)
×
h
p
<
行
走
一
步
最
小
的
代
价
值
最
大
路
径
的
期
望
代
价
值
h=(1.0+p)\times h\\ p<\frac{行走一步最小的代价值}{最大路径的期望代价值}
h=(1.0+p)×hp<最大路径的期望代价值行走一步最小的代价值
参数
p
p
p的分母为一个极大值,例如在一张大地图中进行规划,其代价值必然不会超过某一个极大值,如10000;参数
p
p
p的分子为机器人行走的最小代价,如1。
也即将每个节点的 h h h值扩大一个极小倍数,从而导致存在不同,其效果如下:
方式二:增加倾向性
当两条不同的Path具有相同的
f
f
f值时,可对其增加倾向性:使得机器人倾向于距离起点、终点连线的偏移量较小的方向进行探索:
d
x
1
=
∣
n
o
d
e
.
x
−
g
o
a
l
.
x
∣
d
y
1
=
∣
n
o
d
e
.
y
−
g
o
a
l
.
y
∣
d
x
s
=
∣
s
t
a
r
t
.
x
−
g
o
a
l
.
x
∣
d
y
s
=
∣
s
t
a
r
t
.
y
−
g
o
a
l
.
y
∣
c
r
o
s
s
=
∣
d
x
1
⋅
d
y
2
−
d
x
2
⋅
d
y
1
∣
h
=
h
+
c
r
o
s
s
×
0.001
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
dx1=∣∣node.x−goal.x∣∣dy1=∣∣node.y−goal.y∣∣dxs=∣∣start.x−goal.x∣∣dys=∣∣start.y−goal.y∣∣cross=∣∣dx1⋅dy2−dx2⋅dy1∣∣h=h+cross×0.001
式中,
c
r
o
s
s
cross
cross为机器人探索的节点同起点、终点连线的偏移量。其效果如下:
问题存在
上述方式在无障碍物下将存在较好的效果,然而在障碍物环境下,不同的Tie Breaker将导致不同的生成效果。
如上图,右侧生成的轨迹虽然最优,但实际不符合机器人进行移动的特点,应对其进行调整。