路径规划——搜索算法详解（六）：LPA*算法详解与Matlab代码

上文讲解了D*算法，D*算法为在动态环境下进行路径规划的场景提出了可行的解决方案，本文将继续介绍另外一种动态规划路径的方法——Lifelong Planning A*（LPA*）算法。

该算法可以看作是A*的增量版本，是一种在固定起始点与目标点、动态环境下实时搜索路径的算法。笔者看了很多篇讲解、原文后发现许多文章描述得云里雾里，本文可以看作是笔者对于古月学院LPA*算法的学习笔记，希望本文的算法讲解与MATLAB仿真能够给大家带来理解上的帮助！

一、LPA*算法流程讲解：

1. 基本概念：

• 符号表示：
  • g(s)：之间记录的起点距离代价的最小值；
  • rhs(s)：基于父节点g所预测的最小值，设s'为s的父节点，此时有rhs(s)=g(s')+d(ss')，d(ss')表示s与父节点s‘的连接代价
  • 对于每一个节点，其记录着一个代价很熟k(n)，其决定了S中的弹出顺序，类比于A*算法中的Openlist弹出的规则，其包含两个值，根据k1优先排序，当k1相同时根据k2排序，其计算方式如下：                                      

    

  • S：地形图中路径点集合
  • U：优先队列，每次弹出k值最小的节点s；
• 与D*算法中采用h(X)、k(X)表示每个节点实时更新的代价、最小更新代价类似，LPA*算法采用rhs(s)、g(s)两个值表示每一个搜索点的本次计算中起始点到当前节点的代价值、目前所有次计算中从起始点到当前节点的最小代价值，注意D*是从终点开始搜索的，而LPA*算法是从起点开始搜索，所以衡量指标rhs(s)、g(s)均表示到起点的距离，这要注意不要跟D*混淆。根据rhs(s)、g(s)的大小关系定义节点的状态：
  • rhs(s)=g(s)：局部一致状态，表示当前节点并未造成影响，类比于D*算法中的Lower态；
  • rhs(s)<g(s)：局部过一致状态，即当前代价小于记录的历史代价，表明当前节点可以寻找到更加适合的父节点以减小到起点的代价值，更新代价：g(s)=rhs(s)；此时该节点的后续子节点必定受到影响，此时需要遍历后续子节点，对于每一个后续子节点更新其rhs，检查其子节点是否能恢复到历时最小代价g，当不可恢复时将其插入搜索队列，使节点恢复到局部一致；
  • rhs(s)>g(s)：局部欠一致状态，即当前代价大于记录的历史代价，此时表示该节点受到障碍物影响，遍历后续节点更新代价。一般由于父节点突变情况所致，需要将此节点g设置为∞，将其状态改为局部过一致，对于局部过一至的点在下一次循环中更新其g(s)达到局部一致状态。

2. 算法流程解释：

参考论文中的伪代码与符号解释，如下所示：

直接看代码流程会很混乱，待会会有案例讲解，有以下绩点关键需要注意，老规矩留个印象就好，看完例子再回来会有收获的：

1. 搜索过程不断重复事情是：搜索节点达到局部一致；

2. 搜索完成的指标如下（两者之一）如果目标的代价为无穷大，那么从开始到目标没有有限的代价路径。否则，最短路径可以通过向后移动来确定。

        (1)搜索队列U中最小k值大于目标点k值；

        (2)目标节点达到局部一致且不为inf；

3. 当节点s发生变化时，需要检查其所有子节点，更新子节点的rhs值，将局部一致的节点从搜索列表中删除，将受影响导致局部不一致的节点加入到搜索队列U中，并及时更新局部不一致节点的k值，不断循环该过程直到达到2中的搜索指标。

4. 增量式搜索是基于初次完成A*搜索的结果上进行的，需要进行初次搜索。

5. 上述流程中的函数含义如下：

3. 案例讲解（案例出于古月学院）：

（1）.初始化：

在增量式搜索开始前，求出路径点集合S中每一个自由栅格的起点的实际最小代价g*、终点启发函数h，笔者感觉此处启发函数用的是切比雪夫距离：

（2）.起点搜索处理

首先将所有节点的距离起点代价rhs、记录最小的起点代价g初始化为inf，由于起点本身距离为0，所以此时rhs(A3)=0，g(A3)=∞，此时rhs(A3)！= g(A3)，局部不一致，此刻将其插入到优先级队列U中。如上图所示，每一个节点处包含k=[k1,k2]，后续将根据此从U中弹出元素。

（3）.拓展节点（循环1）：

此时弹出A3点， 此时到流程中的步骤2(代码09-16)，rhs(A3)=0，g(A3)=∞，此时rhs(A3)<g(A3)，令g(A3)=rhs(A3)=0（代码12），此时A3点局部一致，g(A3)=rhs(A3)=0，故确定为代价0。

同时遍历A3在第一次搜索时候的子节点（看（1）中相连接且以A3作为父节点的A2、B3），此时将A2、B3分别带入UpdateVertex函数（代码06-08）中此时根据节点A3更新rhs(A2)=g(A3)+d(A2、A3之间的距离) = 0+1=1、rhs(B3)=g(A3)+d(A3、B3之间的距离) = 0+1=1，此时由于初始化时候g(A2)=g(B3)=inf>1，此时A2、B3局部过一致，将其加入到队列U中，此时U中元素为U={A2(6,1),B3(5,1)}。

（4）.弹出节点（循环2）：

跟上述流程相同，弹出k最小的节点，此时U中最小的为B3，首先令其g(B3)=rhs(B3)=1，达到局部一致，此时记为1，将流程（1）中对应的子节点（C3）带入代码（代码06-08）中，步骤与上次循环一样，应该很好理解。

（5）.不断循环直到结束：

不断循环计算，更新节点状态，直到满足跳出循环的条件，按照序号排序即可得到一条最佳轨迹。

（6）.节点状态改变处理步骤：

如上图所示，当D1节点突变为障碍物时，此时rhs(D1)=inf，此时节点D1变为欠一致状态，此时检查其所有与之相连接的节点（代码21-23行），此时如上图#1-#4所示，其子节点（子节点定义为在路径中排列在该节点后的所有路径节点E1、F0，而F1父节点为E1，也会受到影响）由于其状态变化也变为了欠一致状态（代码14-16行），此时rhs（E1、F0、E1）= inf，故此时的图变为了#4的状态，灰色的为上次搜索得到结果后在U序列中的节点。此时U={A0(8,3)、E3(7,4)}。此时弹出E3，循环执行代码步骤2，根据上述流程即可得到一条新的路径

大家看完案例讲解后再看一遍上述的1、2部分，相信会有新的收获！

二、LPA*算法代码：

这是笔者按照课程打出来的代码，已经上传到了本人Github，需要的自取：
Adamaser/Path-Planning (github.com)