无极限网站模板,wordpress评论加头像,深圳建设培训中心网站,江苏网站建设培训上文讲解了D*算法#xff0c;D*算法为在动态环境下进行路径规划的场景提出了可行的解决方案#xff0c;本文将继续介绍另外一种动态规划路径的方法——Lifelong Planning A*#xff08;LPA*#xff09;算法。
该算法可以看作是A*的增量版本#xff0c;是一种在固定起始点…
上文讲解了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)0g(A3)∞此时rhs(A3) g(A3)局部不一致此刻将其插入到优先级队列U中。如上图所示每一个节点处包含k[k1,k2]后续将根据此从U中弹出元素。
3.拓展节点循环1 此时弹出A3点 此时到流程中的步骤2(代码09-16)rhs(A3)0g(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之间的距离) 011、rhs(B3)g(A3)d(A3、B3之间的距离) 011此时由于初始化时候g(A2)g(B3)inf1此时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行此时rhsE1、F0、E1 inf故此时的图变为了#4的状态灰色的为上次搜索得到结果后在U序列中的节点。此时U{A0(8,3)、E3(7,4)}。此时弹出E3循环执行代码步骤2根据上述流程即可得到一条新的路径 大家看完案例讲解后再看一遍上述的1、2部分相信会有新的收获
二、LPA*算法代码
这是笔者按照课程打出来的代码已经上传到了本人Github需要的自取Adamaser/Path-Planning (github.com)
