规划算法中的图搜索算法简单小结

[TOC]
本文对规划算法中的图搜索算法做简单小结.内容包括DFS,BFS,GBFS,Dijkstra,A*,D*,Hybrid A*.

非启发式搜索

深度优先搜索(DFS,deep first)

深度越大的节点会被优先扩展, 使用栈(Stack)数据结构来实现上述特性. 是一种以时间换空间的方法, 不能保证一定得到最优解.

广度优先搜索(BFS, breadth first)

BFS 采用队列(Queue)作为 openlist 的数据结构. 从起点开始, 首先遍历起点周围邻近的点, 然后再遍历已经遍历过的点邻近的点, 逐步的向外扩散, 直到找到终点. 这种算法就像洪水(Flood fill)一样向外扩张. 同时需要记录每个节点的父节点, 反推所有最优节点的父节点即得到路径. 能够保证搜索到的路径是最优的(具有完备性).

启发式搜索(Heuristic Algorithm)

非启发式搜索未考虑目标位置等因素, 这就使搜索过程变得漫无目的, 导致效率低下. 启发式搜索用来解决此问题. 启发式搜索一般使用的是优先队列(Priority Queue)作为 openlist 的数据结构. 优先队列使用代价函数 $f(n)$ ​来作为优先级判断的标准 $f(n)$​ 越小, 优先级越高, 反之优先级越低.

代价函数里最常用的计算方式为距离值计算. 常用的距离值计算方法如下:

两点 $n_i=(x_i,y_i)$,$n_j=(x_j,y_j)$, 假设为方形格子, 边长为 $D$.

1. 曼哈顿距离(常用于图形中只允许朝上下左右四个方向移动的场景)
   $$
   h(n)=D \times [abs(x_i-x_j)+abs(y_i-y_j)]
   $$
2. 对角距离(常用于图形中允许朝八个方向移动的场景)
   如果格子是斜接的,从格子的对角穿过计算距离
   $$
   h(n)=D \times [abs(x_i-x_j)+abs(y_i-y_j)] + (\sqrt{2}D-2D) \times min(abs(x_i-x_j),abs(y_i-y_j))
   $$
3. 欧式距离(常用于图形中允许朝任何方向移动的场景)
   $$
   h(n)=D \times \sqrt {(x_i-x_j)^2+(y_i-y_j)^2}
   $$

同时考虑到不同位置之间的移动特性不同, 例如由地形的特点影响, 上坡的移动代价比下坡的移动代价要高. 可以对不同位置间的距离加权.

PS. 这里涉及到度量空间的内容. 写的非常粗糙, 需要深入了解的可以参考度量空间的教材.

贪婪最佳优先算法(Greedy Best First Search, GBFS)

GBFS 使用启发评估函数 $h(n)$ (当前节点到终点的代价)来作为代价函数：

$$
f(n)=h(n)
$$

但是在实际的地图中, 常常会有很多障碍物, 它就很容易陷入局部最优的陷阱. 下图的地图中有一个专门设置的局部最优陷阱, 很显然 GBFS 虽然搜索速度够快, 但是找不到最优路径.

将其应用到复杂二维地图路径规划中, 效果如下：

Dijkstra 算法

汉语发音: 狄克斯特拉. 英文发音:/ˈdaɪkstrə/.由计算机科学家 Edsger W. Dijkstra 在 1956 年提出的.

代价函数由 $g(n)$ 决定, 其为从起点到当前点的移动代价.

$$
f(n)=g(n)
$$

Dijkstra 算法能够得到最优路径, 与BFS一样波状前进的方式, 导致速度较慢.

A*算法

将 GBFS 算法与 Dijkstra 算法的代价函数计算方式融合即可得到A*算法. 由 Stanford 研究院的 Peter Hart, Nils Nilsson 以及 Bertram Raphael 于 1968 年发表. 论文:A Formal Basis for the Heuristic Determination of Minimum Cost Paths

$$
f(n)=g(n)+h(n)
$$

特点

• 在极端情况下,当 启发函数 $h(n)$ 始终为0, 则将由 $g(n)$ 决定节点的优先级, 此时算法就退化成了 Dijkstra 算法.
• 如果 $h(n)$ 始终小于等于节点 $n$ 到终点的代价, 则 A* 算法保证一定能够找到最短路径.但是当 $h(n)$ 的值越小, 算法将遍历越多的节点, 也就导致算法越慢.
• 如果 $h(n)$ 完全等于节点 $n$ 到终点的代价, 则 A* 算法将找到最佳路径, 并且速度很快. 可惜的是, 并非所有场景下都能做到这一点. 因为在没有达到终点之前, 我们很难确切算出距离终点还有多远.
• 如果 $h(n)$ 的值比节点 $n$ 到终点的代价要大, 则 A* 算法不能保证找到最短路径, 不过此时会很快. 这就是 anytime A* 算法类的主题思想.
• 在另外一个极端情况下, 如果 $h(n)$ 相较于 $g(n)$ 大很多, 则此时只有 $h(n)$ 产生效果, 这也就变成了 GBFS.

算法过程

$$ \begin{aligned} &\text{1. Mark s "open" and calculate} \quad \hat{f}(s).\\ &2. \text{Select the open node n whose value of} \quad \hat{f} \quad \text{is smallest.} \\ &\quad \text{ Resolve ties arbitrarily, but always in favor of any node} \quad n \in T .\\ &3. \text{If} \quad n \in T \text{, mark n "closed" and terminate the algorithm.}\\ &4. \text{Otherwise, mark n closed and apply the successor operator} \quad \Gamma \quad \text{to n .}\\ &\quad \text{Calculate} \quad \hat{f} \quad \text{for each successor of n and mark as open each successor not already marked closed.} \\ &\quad \text{Remark as open any closed node} \quad n_{i} \quad \text{ which is a successor of n and for which} \quad \hat{f}\left(n_{i}\right) \quad \text{ is smaller now than it was} \\ &\quad \text{ when} \quad n_{i} \quad \text{was marked closed.} \\ &\quad \text{Go to Step 2 .} \end{aligned} $$

时间复杂度

取决于启发函数的选取, 最坏: $O(b^d)$, $b$ 为深度, depth of the solution (the shortest path). $d$ 为广度 branching factor (the average number of successors per state). 当然如果无解的话, 复杂度为无穷.

启发函数的衡量可以使用 effective branching factor b* 概念. 所有搜索 expand 的 node 数目为 $N$, 一般满足如下式子: $$ N+1=1+b^{*}+\left(b^{*}\right)^{2}+\cdots+\left(b^{*}\right)^{d} $$ 最优的情况下 $b^{*}=1$.

空间复杂度

case-by-case, 但维护图以及 openlist 的空间一般必不可少.
Iterative deepening A*, memory bounded A*, and SMA* 对空间复杂度有改进.

D*(Dynamic A)

由 Anthony Stentz 在 1994 年发表, 论文:Optimal and Efficient Path Planning for Partially-Known Environments. 从名字可以看出是 A* 的改良.

D* 不是由起始点开始搜索, 而是以目标点为起始, 通过将目标点置于 Openlist 中来开始搜索, 直到机器人当前位置节点由队列中出队为止. 如此做的原因是当物体由起始位置向目标位置运行过程中, 发现路径中存在新的障碍时, 对于目标位置到新障碍之间的范围内的路径节点, 新的障碍是不会影响到其到目标的路径的. 新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径. 在这时通过将新的障碍周围的节点加入到 Openlist 中进行处理然后向物体所在位置进行传播, 能最小程度的减少计算开销.

因此听起来与 Dijkstra 算法很像, 只不过搜索的起点变成了目标点.

伪代码如下:

$$ \begin{array}{l} \textbf { Function: PROCESS-STATE } \\ \text { L1 } X=M I N-S T A T E() \\ \text { L2 if } X=N U L L \text { then return }-1 \\ \text { L3 } k_{o l d}=G E T-K M I N() ; D E L E T E(X) \\ \text { L4 if } k_{o l d} < h(X) \text { then } \\ \text { L5 } \quad \text { for each neighbor } Y \text { of } X: \\ \text { L6 } \quad \text { if } h(Y) \leq k_{o l d} \text { and } h(X) > h(Y)+c(Y, X) \text { then } \\ \text { L7 } \quad b(X)=Y ; h(X)=h(Y)+c(Y, X) \\ \text { L8 if } k_{o l d}=h(X) \text { then } \\ \text { L9 } \quad \text { for each neighbor } Y \text { of } X: \\ \text { L10 } \quad \quad \text { if } t(Y)=N E W \text { or } \\ \text { L11 } \quad \quad \quad (b(Y)=X \text { and } h(Y) \neq h(X)+c(X, Y)) \text { or } \\ \text { L12 } \quad \quad \quad (b(Y) \neq X \text { and } h(Y) > h(X)+c(X, Y)) \text { then } \\ \text { L13 } \quad \quad \quad b(Y)=X ; \text { INSERT } Y, h(X)+c(X, Y)) \\ \text { L14 else } \\ \text { L15 } \quad \text { for each neighbor } Y \text { of } X: \\ \text { L16 } \quad \quad \text { if } t(Y)=N E W \text { or } \\ \text { L17 } \quad \quad \quad (b(Y)=X \text { and } h(Y) \neq h(X)+c(X, Y)) \text { then } \\ \text { L18 } \quad \quad \quad b(Y)=X ; I N S E R T(Y, h(X)+c(X, Y)) \\ \text { L19 } \quad \quad \text { else } \\ \text { L20 } \quad \quad \quad \text { if } b(Y) \neq X \text { and } h(Y)>h(X)+c(X, Y) \text { then } \\ \text { L21 }\quad \quad \quad \quad I N S E R T(X, h(X)) \\ \text { L22 } \quad \quad \quad \text { else } \\ \text { L23 } \quad \quad \quad \quad \text { if } b(Y) \neq X \text { and } h(X)>h(Y)+c(Y, X) \text { and } \\ \text { L24 } \quad \quad \quad \quad \quad t(Y)=C L O S E D \text { and } h(Y)>k_{o l d} \text { then } \\ \text { L25 } \quad \quad \quad \quad \quad I N S E R T(Y, h(Y)) \\ \text { L26 } \text { return } G E T-K M I N(~)\\ \textbf {Function: MODIFY-COST (X, Y, cval)} \\ \text {L1} \quad c(X, Y)= cval\\ \text {L2 if} \quad t(X)=\text {C L O S E D then} \operatorname{INSERT}(X, h(X)) \\ \text {L3 return G E T-K M I N( )} \end{array} $$

伪代码解读:

符号	含义	符号	含义
$X$	state/node/格子	$b(X)$	$X$的父nodes(backpointers),更靠近目标点的node
$NEW$	该State从未被置于 Openlist 中	$OPEN$	该State正位于OpenList中
$CLOSE$	该State不再位于 OpenList 中	$h(G,X)$	代价函数估计,表示当前State到G的开销估计,G已定时可以省略.
$k(G,X)$	Key Function,Openlist中的排序依据,值最小的位于队列头	$RAISE$	若 $k(X)<h(X)$ ,state 的tag
$LOWER$	若$k(X)=h(X)$,state 的tag	$K_{min}$	所有位于Openlist上的state的最小K值
$c(X,Y)$	从 state $X$ 到 $Y$ 的转移成本	$t(X)$	得到 state 的 tag 为 Open/Close/New
$INSERT(X,h_{new})$	对Open/Close/New中的 state 进行 h值的更新

$INSERT(X,h_{new})$ 的详细过程:
对于 $t ( X ) = N E W$, $k(X) = h_{new} $.
对于 $t(X) = O P E N$,$ k(X) = min(k(X), h_{new}) $,其中$k(X) = min(h(X), h_{new}) $.
对于 $ t ( X ) = CLOSED$,$h(X) = h_{new}$ 且使 $t(X) = O P E N$.

这里与 A* 搜索过程不同的最大一点是 $RASIE$ 与 $LOWER$ 的状态分类.

当一个 state 处于 $RASIE$ 状态时说明当前的路径不再是最优的了,可能是因为障碍物的影响, 需要在周围找到一个$h(Y)+c(Y,X)$ 最小的 state $Y$作为 当前 state $X$的父 state.(伪代码里的$L5-L7$).

当一个 state 处于 $LOWER$ 状态时说明较上次产生了新的到终点的 path 或者途中障碍物更新导致较低的 cost 产生,从而导致搜索更优的路径的可能性. 这种时候对周围所有的 state $Y$ 更新其h值$h(Y) = h(X)+c(X,Y)$ 并标记其父 state 为 $X$.(伪代码里的$L8-L13$).

既不属于 $RASIE$ 也不属于 $LOWER$ 的场景,如果$X$ 周围 state $Y$是 $NEW$ 或者是 $X$ 的子节点并且 $h(Y) \ne h(X) + c(X, Y) $, 按照 $LOWER$ 状态同样处理.如果不是 $X$ 的子节点并且$h(Y) > h(X) + c(X, Y) $说明该 state $Y$ 与 $X$ 没关系, 更新 $X$, $INSERT(X, h(X)$. 最后如果不是 $X$ 的子节点并且$h(Y) > h(X) + c(X, Y) $并且$t(Y) = CLOSED, h(Y) > k_{old}$,需要把 $Y$ 从 CLOSE 中移除,更新 hvalue 并放入 OPEN 中.

Hybrid A*

对 A* 的改良,由斯坦福 2010 年首次提出并在 DARPA 中应用.
论文:https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf.
改进点在于考虑了车辆动力学对 state 之间的移动加上了车辆运动的约束.
一图胜千言:

左侧图为普通A*的搜索,考虑移动方向为八方向, 并且移动点固定为格子中央.
中间图为 Field D* 以及 Theta*, 不固定格子内的移动点, 但是仍为直线连接.
右侧图为 Hybrid A*, 在中间图的基础上考虑狭窄空间汽车只能走圆弧(稳态下)的动力学特性进行了采样并保证连续性.

伪代码(出处论文)如下：

$$ \begin{array}{l} \textbf { 1: function ROUNDSTATE }(x) \\ \text { 2: } \quad x \cdot P o s_{X}=\max \left\{m \in \mathbb{Z} \mid m \leq x \cdot \operatorname{Pos}_{X}\right\} \\ \text { 3: } \quad x \cdot P o s_{Y}=\max \left\{m \in \mathbb{Z} \mid m \leq x \cdot \operatorname{Pos}_{Y}\right\} \\ \text { 4: } \quad x \cdot A n g_{\theta}=\max \left\{m \in \mathbb{Z} \mid m \leq x \cdot A n g_{\theta}\right\} \\ \text { 5: } \quad \text { return } x \\ \text { 6: end function } \\ \textbf { 7: function EXISTS }\left(x_{\text {succ }}, \mathcal{L}\right) \\ \text { 8: if }\left\{x \in \mathcal{L} \mid \text { roundState }(x)=\text { roundState }\left(x_{\text {succ }}\right)\right\} \neq \emptyset \text { then } \\ \text { 9: } \quad \text { return true } \\ \text { 10: } \text { else } \\ \text { 11: } \quad \text { return false } \\ \text { 12: }\text { end if } \\ \text { 13: end function }\\ \textbf { Require: } x_{s} \cap x_{g} \in X \\ \text { 14: } O=\emptyset \\ \text { 15: } C=\emptyset \\ \text { 16: Pred }\left(x_{s}\right) \leftarrow \text { null } \\ \text { 17: O.push }\left(x_{s}\right) \\ \text { 18: while } O \neq \emptyset \text { do } \\ \text { 19: } \quad x \leftarrow \text { O.popMin }() \\ \text { 20: } \quad \text { C.push }(x) \\ \text { 21: } \quad \text { if } \text { roundState }(x)=\text { roundState }\left(x_{g}\right) \text { then } \\ \text { 22: }\quad \quad \text { return } x \\ \text { 23: } \quad \text { else } \\ \text { 24: }\quad \quad \text { for } u \in U(x) \text { do } \\ \text { 25: }\quad \quad \quad x_{\text {succ }} \leftarrow f(x, u) \\ \text { 26: }\quad \quad \quad \text { if } \neg \text { exists }\left(x_{\text {succ }}, C\right) \text { then } \\ \text { 27: }\quad \quad \quad \quad g \leftarrow g(x)+l(x, u) \\ \text { 28: }\quad \quad \quad \quad \text { if } \neg \text { exists }\left(x_{\text {succ }}, O\right) \text { or } g < g \left(x_ {succ } \right) \text { then } \\ \text { 29: }\quad \quad \quad \quad \quad \quad \text { Pred }\left(x_{\text {succ }}\right) \leftarrow x \\ \text { 30: }\quad \quad \quad \quad \quad \quad g\left(x_{\text {succ }}\right) \leftarrow g \\ \text { 31: }\quad \quad \quad \quad \quad \quad h\left(x_{\text {succ }}\right) \leftarrow H e u r i s t i c\left(x_{s u c c}, x_{g}\right) \\ \text { 32: }\quad \quad \quad \quad \quad \quad \quad \text { if } \neg \text { exists }\left(x_{\text {succ }}, O\right) \text { then } \\ \text { 33: }\quad \quad \quad \quad \quad \quad \quad \quad O . p u s h\left(x_{\text {succ }}\right) \\ \text { 34: }\quad \quad \quad \quad \quad \quad \quad \text { else } \\ \text { 35: }\quad \quad \quad \quad \quad \quad \quad \quad \text { O.decreaseKey }\left(x_{\text {succ }}\right) \\ \text { 36: }\quad \quad \quad \quad \quad \quad \quad \text { end if } \\ \text { 37: }\quad \quad \quad \quad \quad \quad \text { end if } \\ \text { 38: }\quad \quad \quad \quad \text { end if } \\ \text { 39: } \quad \quad \text { end for } \\ \text { 40: } \quad \text { end if } \\ \text { 41: end while } \\ \text { 42: return null } \end{array} $$

具体的执行过程可以参考我之前对 Apollo 解读的博文.

A* 的其他变种

A* 还有很多改良的变种.
常见的如下列:

• Anytime A* (anytime,提高 real-time 表现)
• Block A*
• Field D*
• Fringe
• Fringe Saving A*(Incremental)
• Generalized Adaptive A*
• Reduced A*
• Iterative deepening A* (减小内存依赖,memory bounded)
• Jump point search
• Lifelong Planning A*(Incremental)
• New Bidirectional A*
• Simplified Memory bounded A* (减小内存依赖,memory bounded)
• Theta*
  我在另外一篇博文会介绍 anytime A* 以及 incremental A*的改良变种.
  其他变种后续在实际需要时研读后续加入整理.

参考链接

https://zhuanlan.zhihu.com/p/346666812
https://zhuanlan.zhihu.com/p/54510444
https://blog.csdn.net/a380331382/article/details/82841071
https://zhuanlan.zhihu.com/p/161660932
https://en.wikipedia.org/wiki/A*_search_algorithm
http://theory.stanford.edu/~amitp/GameProgramming/Variations.html