基于A*搜索算法的改良算法介绍

[TOC]

项目实践中使用 Hybrid A*算法(关于图搜索算法的介绍博文)搜索粗轨迹的时候, 由于搜索过程太耗费资源加上很多时候不需要每时每刻进行搜索. 因此需要决定什么时候才会 replan, 提升效率. 本篇文章为调查的总结. 该系列算法都是对 A* 的改进, 都是出自 Likhachebv Maxim 教授或者 Sven Koenig 教授的研究成果.

如果对 A* 搜索改进算法感谢的同学也可以关注这两位大牛的主页, 里面有很多教程以及研究结果, 演示软件等.

Likhachebv Maxim 教授的主页 :http://www.cs.cmu.edu/~maxim/

Sven Koenig 教授的主页: http://idm-lab.org/project-a.html

Likhachebv Maxim 教授研究室还研发出了开源的 SBPL C++ 算法库. 支持在ROS中使用, 不仅包含执行代码,还 包括可视化与 debug 的功能.

SBPL 的代码库, ROS wiki. 支持的搜索算法: ARA*, Anytime D*, ANA*, R* , Multi-Heuristic A*.

回到本文内容, 共介绍了 anytime 类: ARA* (Anytime Repairing A*), Incremental类: LPA* (Lifelong Planning A*),FSA*(Fringe-Saving A*),D* Lite.

ARA* (Anytime Repairing A*)

介绍一种对 A* 算法的衍生算法 ARA* 算法.

简介

首先 ARA* 是一种 anytime 算法, anytime 算法是一种即便算法被中断也可以产生次优的可行解的算法, 持续地运行此算法可以导致较优甚至最优的解.

对于 A* 算法 anytime 的思想可以自然地应用. 搜索是一种很耗资源的算法, 尤其是对于 real-time 类的应用, 对于很复杂的图, 如果等到 A* 搜索到最优解可能被控对象都已经失败了(例如撞毁). 这时候我们不需要时刻得到最优的解, 先有一个可行解(次优解)更重要. 衍生出来的算法即为 Anytime A*算法.

Anytime A* 的发展

A* 算法的特点为通过启发式的评价函数找到最优解, 表达式为: $f(n) = g(n) + h(n)$, 其中 $h(n)$ 为启发成本函数.

通过在 $h(n)$ 增加权重 $w$ 得到 $f_w(n) = g(n) + w × h(n), w > 1$, 被称为 weighted A* ,这样可以跳过一些 node expanding 尽快地获得解.

通过迭代地减小 $w$, 可以得到 inflated ATA* 算法, 特点是可以在资源允许的情况下使 $w=1$ ​​最终变为 A* 算法得到最优解.

但是 ATA* 也有每次迭代重复搜索计算的问题, 为了解决这个问题产生了本节的主角: ARA*, Anytime Repairing A*算法.

下面的内容整理自 Likhachebv Maxim 的论文《ARA: Anytime A with Provable Bounds on Sub-Optimality》.

术语与前置概念

标记	意义	标记	意义
$s$	搜索node	$s_{goal}$	终点node
$s_{start}$	起点node	$f(s)$	评价函数
$g(s)$	起点路径成本函数	$h(s)$	启发成本函数
$c(s,s’)$	从$s$到其相邻$s’$的点的代价	$\epsilon$	权重
$s_p$	$s$的前一个node	OPEN list	open node待探索列表
CLOSE list	close node已探索列表	INCONS list	除 OPEN 以外的 local inconsistency node 集合

local inconsistency: 这个概念是 ARA*能够实现避免重复搜索计算的关键概念.

假设在一次搜索得到 $s \rightarrow s’$ 的搜索顺序, 并且在这次搜索中 $s$ 是 $s’$ 的 best predecessor, 即

$$
g\left(s^{\prime}\right)=\min _{s_p^{\prime} \in \operatorname{pred}\left(s^{\prime}\right)}\left(g\left(s_p^{\prime}\right)+c\left(s_p^{\prime}, s^{\prime}\right)\right)=g(s)+c\left(s, s^{\prime}\right)
$$

但是在接下来的一次搜索中 $g(s)$ 变化了, 小于上一时刻的 $g(s)$, 即出现了下面的不一致 inconsistency, 原本 $s$ 与 $s’$ 的 $g()$ 函数值 g-value 无法满足等式关系.

$$
g\left(s^{\prime}\right) > \min _{s_p^{\prime} \in \operatorname{pred}\left(s^{\prime}\right)}\left(g\left(s_p^{\prime}\right)+c\left(s_p^{\prime}, s^{\prime}\right)\right)
$$

为了满足等式关系, 需要重新计算 $s’$ 的 g-value , 然后这就导致了 $s’$ 的下一个node $s’’$ 之间也产生了 inconsistency , 导致 $s’’$ 也需要重新计算 g-value .于是这样传导(propagate)下去完成了一次新的搜索过程. 那什么情况下这个 inconsistency 才会发生? 换句话说如果找到 inconsistency 的规律我们就可以对满足 consistency 的 node 不进行重新搜索节省资源. 答案很明显正常情况下 OPEN list consists of exactly all locally inconsistent states !

weighted A*算法

伪代码如下:

$$ \begin{aligned} &01 \quad g\left(s_{s t a r t}\right)=0 ; \text { OPEN }=\emptyset ; \\ &02 \quad \text { insert } s_{s t a r t} \text { into } O P E N \text { with } f\left(s_{s t a r t}\right)=\epsilon * h\left(s_{s t a r t}\right) \text {; } \\ &03 \quad \text { while }\left(s_{g o a l}\right. \text { is not expanded) } \\ &04 \qquad \text { remove } s \text { with the smallest } f \text {-value from } O P E N ; \\ &05 \qquad \text { for each successor } s^{\prime} \text { of } s \\ &06 \quad \qquad \text { if } s^{\prime} \text { was not visited before then } \\ &07 \qquad \qquad f\left(s^{\prime}\right)=g\left(s^{\prime}\right)=\infty ; \\ &08 \quad \qquad \text { if } g\left(s^{\prime}\right)>g(s)+c\left(s, s^{\prime}\right) \\ &09 \qquad \qquad g\left(s^{\prime}\right)=g(s)+c\left(s, s^{\prime}\right) ; \\ &10 \qquad \qquad f\left(s^{\prime}\right)=g\left(s^{\prime}\right)+\epsilon * h\left(s^{\prime}\right) ; \\ &11 \qquad \qquad \text { insert } s^{\prime} \text { into } O P E N \text { with } f\left(s^{\prime}\right) ; \end{aligned} $$

如果再套一个循环每次减小 $\epsilon$, 则成为 ATA* 算法. 然后 $03-11$ 行会重复.

ARA* 的具体过程

主体程序

伪代码如下

$$ \begin{aligned} &\text { procedure Main }() \\ &\text { 01' } g\left(s_{g o a l}\right)=\infty ; g\left(s_{s t a r t}\right)=0 ; \\ &\text { 02' } O P E N=C L O S E D=I N C O N S=\emptyset ; \\ &\text { 03' insert } s_{s t a r t} \text { into OPEN with fvalue }\left(s_{s t a r t}\right) ; \\ &\text { 04' ImprovePath }() ; \\ &\text { 05' } \epsilon^{\prime}=\min \left(\epsilon, g\left(s_{g o a l}\right) / \min _{s \in O P E N \cup I N C O N S}(\mathrm{~g}(s)+\mathrm{h}(s))\right) ; \\ &\text { 06' publish current } \epsilon^{\prime} \text {-suboptimal solution; } \\ &\text { 07' while } \epsilon^{\prime}>1 \\ &\text { 08' } \quad \text { decrease } \epsilon ; \\ &\text { 09' } \quad \text { Move states from INCONS into OPEN; } \\ &\text { 10' } \quad \text { Update the priorities for all } s \in O P E N \text { according to fvalue }(s) ; \\ &\text { 11' } \quad C L O S E D=\emptyset ; \\ &\text { 12' } \quad \text { ImprovePath }() ; \\ &\text { 13' } \quad \epsilon^{\prime}=\min \left(\epsilon, g\left(s_{g o a l}\right) / \min _{s \in O P E N \cup I N C O N S}(\mathrm{~g}(s)+\mathrm{h}(s))\right) ; \\ &\text { 14' } \quad \text { publish current } \epsilon^{\prime} \text {-suboptimal solution; } \end{aligned} $$

• $01’-06’$为 anytime 算法的首次搜索环节, 也就是说被终止之前必须执行完得到的效果最差的解.
• $07’-14’$ 为迭代缩小 $\epsilon$ 的过程.

这里有三个地方与 weighted A* 算法不同:

1. ImprovePath函数: 这个函数通过 local inconsistency 的概念省略重复的搜索计算.
2. INCONS集合: 稍后讲解.
3. $\epsilon$ 更新的方式: 一是判断迭代结束与否的 $\epsilon^{\prime}$, 二是减小 $\epsilon$ 的过程 ${ decrease }\quad \epsilon$.

ImprovePath 省略重复搜索

伪代码如下:

$$ \begin{array}{l} \textbf { procedure fvalue }(s) \\ 01 \text { return } g(s)+\epsilon * h(s) \\ \textbf { procedure ImprovePath }() \\ \left.02 \text { while(fvalue }\left(s_{g o a l}\right)>\min _{s \in O P E N}(\text { fvalue }(s))\right) \\ 03 \quad \text { remove } s \text { with the smallest fvalue }(s) \text { from } O P E N ; \\ 04 \quad C L O S E D=C L O S E D \cup\{s\} ; \\ 05 \quad \text { for each successor } s^{\prime} \text { of } s \\ 06 \quad \text { if } s^{\prime} \text { was not visited before then } \\ 07 \qquad g\left(s^{\prime}\right)=\infty ; \\ 08 \quad \text { if } g\left(s^{\prime}\right)>g(s)+c\left(s, s^{\prime}\right) \\ 09 \qquad g\left(s^{\prime}\right)=g(s)+c\left(s, s^{\prime}\right) \\ 10 \quad \quad \text { if } s^{\prime} \notin C L O S E D \\ 11 \quad \qquad \text { insert } s^{\prime} \text { into } O P E N \text { with fvalue }\left(s^{\prime}\right) \\ 12 \quad \quad \text { else } \\ 13 \quad \qquad \text { insert } s^{\prime} \text { into I N C O N S ; } \end{array} $$

• 限制每个 node expand 次数不大于1.
  A*搜索可以保证每个 node 只 expand 一次. 但在 ATA* 中无法保证,由于多次迭代. 显式地限制 expand 次数在第$10$行, 只有不在 CLOSED 集合中的 node 才会计算 fvalue 并放入 OPEN 集合中.
• INCONS 集合
  上面的限制会将一部分属于 locally inconsistent node 不再会放入 OPEN 中,这样导致 OPEN 集合无法保证涵盖所有 locally inconsistent node 的属性. 解决办法是增加 INCONS 集合,把剩余的 locally inconsistent node 放入其中. 对应的代码为$13$行.这部分其实这样理解,每次迭代我们可以得到 2 类集合, OPEN 与 CLOSE, 下一次迭代如果按照常规的 A* 搜索的话, OPEN 里的一部分 node 其实不需要在探索一遍,但是仍然会被探索一遍,导致浪费,这个导致会探索的属性叫做 locally inconsistency .为了效率,可以把 OPEN 限定为每次迭代需要重新 expand 的 node, 而之前已经确定需要 expand 的 node 标记一下放入 INCONS 集合中即可.通过 INCONS 的设置,我们实现了对之前搜索结果的复用.
• 搜索终止条件
  上面的设置会带来另一个问题 goal node 不会被放入 OPEN 集合中,搜索不会停止. 通过检查 goal node 的 fvalue 是否在 OPEN 中最小来确定搜索终止条件. 在$02$行.
• 计算 fvalue 的计算节省
  我们可以看到每次迭代中, INCONS 中的 node 的 fvalue 都是不会被重新计算的(理论上$\epsilon$变化了, fvalue 也变化了),只会计算放入 OPEN 里的新的 node 的 fvalue. INCONS 中的 node 的 fvalue 会在下一次迭代中被重新计算放入 OPEN 中, 见主程序的$09’-10’$行.

$\epsilon$ 的减小方式

主程序的$05'$行 $\epsilon^{\prime}=\min \left(\epsilon, g\left(s_{g o a l}\right) / \min _{s \in O P E N \cup I N C O N S}(\mathrm{~g}(s)+\mathrm{h}(s))\right) $ 表明了迭代结束的条件. $\min _{s \in O P E N \cup I N C O N S}(\mathrm{~g}(s)+\mathrm{h}(s))$ 为普通A\*的最优条件, `/`左侧的$g\left(s_{g o a l}\right)$为迭代后得到的$S_{goal}$.很明显只要两者的比值等于即代表迭代到最优解了可以停止搜索.

这里有个疑问, 为什么不直接拿 $\epsilon’$ 作为减小迭代 $\epsilon$ 的方式. 论文中给出的解释为直接用的话会导致减小 step 过大,会导致下一次的 search 有太多的 expansion. 因此可以根据自己的需求确定减小方式,如主程序中的$08’$行.

图解

左侧为 ATA* 的结果图示. 右侧为ARA*的结果.每次搜索 expand 的格子被涂成灰色. 格子中央加上星号符的为每次迭代最终的 locally inconsistent 格子.

首先可以对比左侧的第三个与第一个图, $\epsilon$ 大于 1 时, 比 A* 搜索探索的格子数目明显变少, 同样地得到的搜索结果也不是最优的.

然后可以对比左右侧的第二张图, 右侧 ARA*只 expand 了一个格子, 而左侧的 ATA* 为15个格子.

LPA* (Lifelong Planning A*)

受强化学习类算法中的 Incremental 的思路的启发, 能否在前后两次 A*搜索的过程中利用上一次的搜索过程记录,提高效率. 是为此算法的 motivation.

应用场景

1. 整体图不变包括起点终点位置.
2. 前后 2 次的障碍物变化情况可以很便利地得到.

看到这 2 个限制条件估计大家会跟我一样觉得很难应用到自动驾驶中吧. 自动驾驶图每时每刻都在变化, 除了作为传感器位置的自车在图中位置固定以外. 并且如果能够遍历一遍把障碍物信息更新出来的话, 不如直接再直接搜一遍来的有效.
但作为一种思路还是值得研读一下.

论文地址1
论文地址2

算法简介

其实原理很简单, 如果图中的障碍物信息变化了, 我们希望只重新搜索因此导致的所有格子重新搜索, 而不是从头开始重新搜索. 假设我们能够得到每次图中障碍物变化导致的所有 $c(s,s’)$ 变化, 即格子到格子之间转移 edge cost 变化的所有信息(例如从 $s’$ 从障碍物变为可通行, $c(s,s’)=\infty \rightarrow 1 $). 那如何接着往下进行搜索呢?
有 2 点需要考虑:
一是 local inconsistent 的概念. 在 ARA* 里介绍过, 这里不再阐述. 具体到这里我们需要知道那些因为 edge cost 变化的格子及其后续所有格子的都需要重新更新状态以及搜索. 而其余的 consisitent 的格子就不用再更新搜索了, 节省资源.
二是新的优先队列 $U$ 的构造(也就是接下来进行搜索的顺序), 这里的优先队列的排序按照双重规则进行, 也就是 key 分为2层: $k(s)=[k_1(s);k_2(s)]=[\min (g(s), r h s(s))+h(s) ; \min (g(s), r h s(s))]$. 第一层为与A*一样的$f(n)$, 第二层为$g(n)$. 当 fvalue 一致的情况下用 gvalue 排序.
这里的 $rhs(s)$ 被定义如下:

$$ r h s(s)=\left\{\begin{array}{ll} 0 & \text { if } s=s_{s t a r t} \\ \min _{s^{\prime} \in \operatorname{Pred}(s)}\left(g\left(s^{\prime}\right)+c\left(s^{\prime}, s\right)\right) & \text { otherwise } \end{array}\right. $$

当一个格子的 g(s)=rhs(s)$ 的时候, 类似于 Bellman Equation 我们得到的是最优解(稳定状态), 表示此格子经过前向搜索的结果已经稳定了.

然后与 A* 一样只要按照优先队列的顺序设置好终止条件进行搜索(更新)即可.

具体过程

伪代码如下:

$$ \begin{array}{l} \textbf { procedure CalculateKey }(s) \\ \{01\} \text { return }[\min (g(s), r h s(s))+h(s) ; \min (g(s), r h s(s))] ; \\ \textbf { procedure Initialize }() \\ \{02\} U:=\emptyset ; \\ \{03\} \text { for all } s \in S \quad rhs(s)=g(s)=\infty ; \\ \{04\} r h s\left(s_{s t a r t}\right)=0 ; \\ \{05\} \text { U.Insert }\left(s_{s t a r t},\left[h\left(s_{s t a r t}\right) ; 0\right]\right) ; \\ \textbf { procedure UpdateVertex }(u) \\ \{06\} \text { if }\left(u \neq s_{s t a r t}\right) r h s(u)=\min _{s^{\prime}\in P r e d(u)} \left(g\left(s^{\prime}\right)+c\left(s^{\prime}, u\right)\right) ; \\ \{07\} \text { if }(u \in U) \text { U.Remove }(u) ; \\ \{08\} \text { if }(g(u) \neq r h s(u)) \text { U.Insert }(u, \text { CalculateKey }(u)) ; \\ \textbf { procedure ComputeShortestPath }() \\ \left.\{09\} \text { while (U.TopKey() }<\text { CalculateKey }\left(s_{g o a l}\right) \quad \text { OR } \quad r h s\left(s_{g o a l}\right) \neq g\left(s_{g o a l}\right)\right) \\ \{10\} \quad u=\text { U.Pop }() ; \\ \{11\} \quad \text { if }(g(u)>r h s(u)) \\ \{12\} \qquad g(u)=r h s(u) ; \\ \{13\} \quad \quad \text { for all } s \in S u c c(u) \text { UpdateVertex }(s) ; \\ \{14\} \quad \text { else } \\ \{15\} \quad \quad g(u)=\infty ; \\ \{16\} \quad \quad \text { for all } s \in S u c c(u) \cup\{u\} \text { UpdateVertex }(s) ; \\ \textbf { procedure Main }() \\ \{17\} \text { Initialize }() ; \\ \{18\} \text { forever } \\ \{19\} \quad \text { ComputeShortestPath }() ; \\ \{20\} \quad \text { Wait for changes in edge costs; } \\ \{21\} \quad \text { for all directed edges }(u, v) \text { with changed edge costs } \\ \{22\} \qquad \text { Update the edge cost } c(u, v) ; \\ \{23\} \qquad \text { UpdateVertex }(v) ; \end{array} $$

主程序里, Initialize 函数进行初始化, 把 $U$ 重置为空, 把起点信息放入 $U$ 中, 把所有格子的 gvalue 设置为 $\infty$.

然后执行 ComputeShortestPath 进行普通 A* 类似的搜索,与 A* 不同的是对于 $g(s)$ 与 $rhs(s)$ 加了一层判断.

这里论文中引入了两个概念: overconsistent $g(s)>rhs(s)$, underconsistent $g(s)<rhs(s)$ .下面分别对 2 种情况进行图解.

• overconsistent
  如下图, 2 与 4 的可通过性被交换了, 从1->3只能绕道 4 进行. 假设从其他格子到 1, 以及 1 右侧的格子的 gvalue 都是 $g_0$, 从 $s_{goal}$ 到 3 的 hvalue 是 $h_0$. 图更新后的4的$rhs(4)=g_0+1$(这里假设只能四向移动,黄色),然而$g(4)=g_0+2$(红色),即$g(4)>rhs(4)$也就是说从1->4的路径不一定可取了(不一定是因为还有hvalue),因此4的gvalue被更新到$rhs(s)$(第$12$行)后把这一优化信息传递给4的所有子格子(第$13$行).对更新好的优先队列再重复进行搜索即可.
  

• underconsistent
  这次右下角5的障碍物消失了,因此可以通过下方5->4,而不是1->2->3->4路径绕道.由于5之前是不可通行的,gvalue 是 $\infty$, 因此$rhs(4)=g_0+6$,更新后$g(4)=g_0+1$,即 $g(4)<rhs(4)$ ,然后将$g(4) = \infty$ 将格子4改成了 overconsistent (如果是$s_{goal}$的话是 consistent ),更新4及其子格子序列(第$15-16$行)可转换成 overconsistent 再次循环.
  

只要图会发生变化,获取其变化 changes in edge costs 再重复整个过程即可.

改进

论文中还对算法流程进行了一些执行上的改进.由于LPA*本身在自动驾驶规划上实用性低,改进就不展开了.只把伪代码放在这里.

FSA*(Fringe-Saving A*)

Fringe-Saving A*后面简称FSA*,是在 LPA*的基础上 Sven Koenig 教授研究室的 Xiaoxun Sun 又进一步改进此 incremental 算法.改进点主要在于下面点:

1. 不需要固定起点与终点信息,只要后帧图能把需要重复使用的前帧图的部分包含进去即可(最好是坐标不会改变,改变的话需要加上坐标系转换).
2. 输入的信息虽然同为 traversability changes, LPA*要求 cost,而FSA*仅需要变化的格子坐标即可.
3. 通过存储的形式记录上次搜索的 tree ,标记哪些可以 reuse 以及哪些需要重新 expand.我理解的是拿空间换时间的意思,因为要记录的信息比较多,格子搜索的顺序(包括多次迭代的)/父子关系链/fvalue值等.
4. 当封闭空间较小(障碍物较小)时,FSA*比普通A*,LSA*节省较多运算时间.但是问题也产生了,对代码的要求也随之水涨船高.维护起来的成本应该不低.

论文地址.

关键概念与步骤

• sequence number $ExpandedId(s)$: 在一次A*搜索过程中第$ExpandedId(s)$个 expand 的. 其中起点: $ExpandedId(s)=1$, 障碍物处的为 $ExpandedId(s)=\infty$.如果从$s_i$开始,$s_{i+1}$第一个变化并变成 blocked 的话,$m = ExpandedId(s_{i+1})$ 那两者之间的 node 都可以不用重新搜索. 如果$s_{i+1}$第一个变化并变成 unblocked 的话,$m = ExpandedId(s_{i+1}) < 1+min_{s_{i+2} \in Succ(s_{i+1})} ExpandedId(s_{i+2})$ 两者之间的 node $ExpandedId$ 小于上式的都不需要重新搜索. 这些不用重新搜索被称作 reusable cells.

• $BlockId(i)$:第$i$次搜索即第$i$次$Iteration$的值$ExpandedId(s)$的集合,第$i$次迭代的$s$处的值表示为$BlockId(ExpandedIteration(s))$.这么做的原因是 sequence number 可能不会与格子的坐标一一对应,也就是说例如,第1次搜索 $s_i$被 expanded, 第2次搜索, $s_i$被标记为 non reusable ,那么它在第1次里的 sequence number 可能会被周围其他格子代替,到了第3次搜索,如何确定$s_i$的问题就由$BlockId(i)$组成了二维数组/队列解决.因为记录的是 sequence number 而不是每个 node 的坐标,才会产生这个问题.

• anchor与fringe:把相邻的 non-reusable 的格子包在一起成为 anchor(从 goal 向前回溯). 然后沿着一个方向绕 usable 格子一圈把 unblocked 的格子都确定为 fringe. 所有的 fringe 组成 OPEN list 进入本轮的A*测试.

• Restoration of the gvalues and Parent Pointers:有2种情况需要更新 gvalues and Parent Pointers.

1. 当格子从从 blocked 变为 unblocked.
2. 当 OPEN list 中的格子的父格子变成 not reusable.
   更新的方法是找到待更新格子相邻的 reusable 格子,在 reusable 格子 gvalue 的基础上加1即可.

• Sorting the new OPEN list: 如果使用二叉树存储 OPEN set,需要注意与其一个接一个地把元素插入到空的二叉树中,不如直接把 set 一步转化成二叉树.

伪代码

看到下面这么复杂的伪代码,实用感觉基本上只存在于论文中.

$$ \begin{array}{l} \textbf { procedure Initialize }() \\ \{01\} \text { BlockId }(0):=0 ; \\ \{02\} \text { Forall cells } s \\ \{03\} \quad \text { Generatediteration }(s):=0 ; \\ \{04\} \quad \text { ExpandedIteration }(s):=0 ; \\ \{05\} \quad \text { ExpandedId }(s):=0 ; \\ \{06\} m:=0 ; \\ \{07\} \text { OPEN }:=\emptyset ; \\ \{08\} g\left(s_{\text {Start }}\right):=0 ; \\ \{09\} \text { OPEN.Insert }\left(s_{\text {start }}\right) ; \\ \{10\} \text { GeneratedIteration }\left(s_{\text {start }}\right):=1 ; \\ \{11\} \text { Iteration }:=1 ; \\ \textbf { function ComputeShortestPath }() \\ \{12\} \text { BlockId (Iteration) }:=\infty ; \\ \{13\} \text { While }(\text { OPEN } \neq \emptyset) \\ \{14\} \quad s:=\text { OPEN.Pop }() ; \\ \{15\} \quad \text { ExpandedIteration }(s):=\text { Iteration; } \\ \{16\} \quad \text { ExpandedId }(s):=m ; \\ \{17\} \quad m:=m+1 ; \\ \{18\} \quad \text { If }\left(s=s_{\text {goal }}\right) \\ \{19\} \quad \quad \text { Return True; } \\ \{20\} \quad \text { Else } \\ \{21\} \quad \qquad \text { Forall } s^{\prime} \in \operatorname{Succ}(s) \\ \{22\} \qquad \qquad \text { If }\left(\text { Generatediteration }(s) \neq \text { Iteration Or } g(s)+1 < g\left(\mathrm{~s}^{\prime}\right)\right) \\ \{23\} \quad \qquad \qquad g\left(\mathrm{~s}^{\prime}\right):=g(s)+1 ; \\ \{24\} \quad \qquad \qquad\text { GeneratedIteration }\left(s^{\prime}\right):=\text { Iteration; } \\ \{25\} \quad \qquad \qquad \text { Parent }\left(s^{\prime}\right):=s ; \\ \{26\} \quad \qquad \qquad \text { OPEN.Insert }\left(s^{\prime}\right) ;\\ \{27\} \text { Return False; } \\ \textbf { function CellReusable }(s) \\ \{28\} \text { If }(\text { ExpandedId }(s)<\text { BlockId }(\text { ExpandedIteration }(s))) \\ \{29\} \quad \text { Return True; } \\ \{30\} \text { Return False; } \\ \textbf { procedure UpdateMazeTraversability }() \\ \{31\} \text { TmpBlockId }:=\infty ; \\ \{32\} \text { For all cells } s \text { whose traversability has changed } \\ \{33\} \qquad \text { If }(s \text { is blocked }) \\ \{34\} \qquad \quad \text { If (CellReusable }(s)) \\ \{35\} \qquad \qquad \text { If }(\text { ExpandedId }(s)<\text { TmpBlockId }) \\ \{36\} \quad \qquad \qquad \text { TmpBlockId }:=\text { ExpandedId }(s) ; \\ \{37\} \qquad \text { Else } \\ \{38\} \quad \qquad \text { Parent }(s):=N U L L ; \\ \{39\} \quad \qquad \text { For all } s^{\prime} \in \text { Succ }(s) \\ \{40\} \quad \qquad \qquad \text { If }\left(\text { CellReusable }\left(s^{\prime}\right)\right) \\ \{41\} \quad \qquad \qquad \qquad \text { If }\left(\text { ExpandedId }\left(s^{\prime}\right)+1<\text { TmpBlockId }\right) \\ \{42\} \qquad \qquad \qquad \qquad \text { TmpBlockId }:=\text { ExpandedId }\left(s^{\prime}\right)+1 ; \\ \{43\} \text { Forall } i=1 \ldots \text { Iteration } \\ \{44\} \quad \text { If }(\text { TmpBlockId }<\text { BlockId }(i)) \\ \{45\} \qquad \qquad \text { BlockId }(i):=\text { TmpBlockId } ; \\ \{46\} m:= \text { BlockId }(\text { Iteration }) ; \\ \end{array} $$ $$ \begin{array}{l} \textbf { procedure RetrieveFringe }() \\ \{47\} \text { OPEN }:=\emptyset ; \\ \{48\} \text { Iteration }:=\text { Iteration }+1 ; \\ \{49\} s:=s_{\text {goal }} ; \\ \{50\} \text { While (Not CellReusable }(\text { Parent }(s))) \\ \{51\} \quad s:=\text { Parent }(s) ; \\ \{52\} \quad \text { If }\left(s=s_{\text {start }}\right) \\ \{53\} \quad \quad \text { Exit; } / \text { there is no path } * / \\ \{54\} \text { Move } s \text { around the area that contains exactly the cells } s^{\prime} \text { with CellReusable }\left(s^{\prime}\right) \\ \{55\} \quad \text { Forall } s^{\prime} \in \text { Succ }(s) \text { with CellReusable }\left(s^{\prime}\right) \\ \{56\} \quad \quad \text { GeneratedIteration }\left(s^{\prime}\right):=\text { Iteration; } \\ \{57\} \quad \text { If }(s \text { is unblocked }) \\ \{58\} \quad \quad \text { If }(\text { Parent }(s)=N U L L \text { Or (Not CellReusable }(\text { Parent }(s)))) \\ \{59\} \quad \quad \quad \text { Forall } s^{\prime} \in \operatorname{Succ}(s) \\ \{60\} \quad \quad \quad \quad \text { If }\left(\text { CellReusable }\left(s^{\prime}\right)\right) \\ \{61\} \quad \quad \quad \quad \quad \text { Parent }(s):=s^{\prime} ; \\ \{62\} \quad \quad \quad \quad \quad g(s):=g\left(s^{\prime}\right)+1 ; \\ \{63\} \quad \quad \quad \quad \quad \text { break; } \\ \{64\} \quad \quad \text { GeneratedIteration }(s):=\text { Iteration; } \\ \{65\} \quad \quad \text { OPEN.Insert }(s) ; \\ \{66\} \text { Until the initial cell is about to be left in the same direction again; }\\ \textbf { procedure Main }() \\ \{67\} \text { Initialize }() ; \\ \{68\} \text { Repeat } \\ \{69\} \quad \text { If (Not ComputeShortestPath }()) \\ \{70\} \quad \quad \text { Exit; /* there is no path } * / \\ \{71\} \quad \text { Repeat } \\ \{72\} \quad \quad \text { Identify the path using the parent pointers and use it; } \\ \{73\} \quad \quad \text { Wait for traversability changes; } \\ \{74\} \quad \quad \text { UpdateMazeTraversability }() ; \\ \left.\{75\} \quad \text { Until (BlockId (Iteration) } \leq \text { ExpandedId }\left(s_{g o a l}\right)\right) ; \\ \{76\} \quad \text { RetrieveFringe }() ; \\ \{77\} \text { Until False; } \end{array} $$

图解

下图为搜索问题以及一次A*搜索后的每个格子的状态,搜索的 sequence number 已被标注在右下角.

C5格子(11)变成 blocked 如下:

通过父子链找到 anchor cells:

找到 fringe,包括删除边界以及 blocked 部分:

D* Lite

D* Lite也是改自LPA*的 Incremental 算法.

与LPA*的改进点在于:

• 适用于终点不变,整张图不变的场景.更具体一些为,固定图中机器人从起点移动到终点的过程,也就是说起点一直在变化.
• 难点在于起点的时刻改变.

由于也基本上无法应用在自动驾驶的规划领域,这里就不详细研读了.

论文地址.

伪代码如下:

写在最后, 还有很多变种没有调研过, 后面需要再调研.