上文讲解了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)