网站付费推广有哪些,wordpress space,wordpress now,如何修改网站模版#xff01;#xff01;#xff01;注意#xff01;#xff01;#xff01; 看本篇之前#xff0c;一定要先看笔者上一篇的LPA*讲解#xff0c;笔者统一了符号看起来过渡会更加好理解#xff01; 到目前为止#xff0c;我们学习了广度优先搜索Dijkstra算法、能够计…注意 看本篇之前一定要先看笔者上一篇的LPA*讲解笔者统一了符号看起来过渡会更加好理解 到目前为止我们学习了广度优先搜索Dijkstra算法、能够计算任意两个路径点距离的Floyd算法、基于采样实现快速计算的RRT算法、能够利用启发项加速探索过程的A*算法、适用于动态环境反向搜索的D*算法、增量式动态搜索的A*——LPA*算法每种算法都有其特点大家根据需求选择就好。 无独有偶上述6种路径搜索算法仅适用于处理起点与终点固定情况下的路径搜索问题本文将介绍搜索算法篇的最后一种算法——D*lite算法该算法基于D*反向搜索的思想、LPA*增量式搜索的思想是一种变起点反向增量式动态路径搜索算法接下来便进入正题吧老规矩分为算法介绍与案例讲解。PS搜索算法部分总结完后将开启路径优化的篇章将介绍各种拟合轨迹的曲线并且给出能够直接采用的C代码需要的朋友可以关注后续) 一、D*lite算法流程 1.与LPA*算法的联系与区别 其算法原理与LPA*算法类似没有看的朋友先看我上一篇LPA*算法的讲解再来看因为里面定义的符号含义需要理解不然看不懂的其改进了LAP*中的代价项k增加了机器人当前实际移动距离km机器人位置移动可以看作是起点位置移动了其定义如下 如上图所示S黄色为当前遍历节点G红色为终点位置绿色为机器人所在的位置可以看作是路径搜索的起点如左图所示g(s)为终点距离所记录的最小代价值rhs(s)为搜索过程中所更新计算的当时的终点距离代价值对角移动距离为1.4直接连接距离为1通过计算即可得到对应的值。当机器人移动到新的s_star时原位置点标记为s_last此时h变为1km更新为1左右图k1值保持不变计算结果如上图2所示。 上述就是k值更新过程中与LPA*算法的区别此外流程上相差并不大大致框架相同稍后会进行讲解。但由于k值计算方式的不同会产生新的问题机器人每移动一步km实时发生变化此时对于每一个节点都需要更新其k1值。当当前优先队列U中节点较多的时候每运动一步都需要更新这会增加计算负担。 本算法提出了一种解决方案当机器人起点位置发生改变后与其减去原优先队列U中每一个节点的h值不如直接将新插入队列的节点加上km值如上两个图所示h减少的数值正好等于km所增加的数值这样就不会影响U节点中的弹出顺序并且节省计算资源。 2.算法代码框图 符号含义如下所示 g(s)之间记录的起点距离代价的最小值rhs(s)基于父节点g所预测的最小值设s为s的父节点此时有rhs(s)g(s)d(ss)d(ss)表示s与父节点s‘的连接代价由于D*lite为反向搜索的算法所以父节点指的是离终点近的路径点。K代价值与LPA*不同其K包含k1、k2计算如下所示 S地形图中路径点集合U优先队列每次弹出k值最小的节点s 其函数包含初始化部分Initialize、计算路径部分ComputeShortestPath、更新节点部分UpdateVertex、计算代价部分CalculateKey直接看估计会蒙圈下面将通过一个例子讲解该算法的更新过程 3.D*lite算法案例讲解 1.地图介绍 搜索地图如上图所示其中B1为起点E3为目标点在地图预处理时根据设定的启发函数如曼哈顿距离、欧氏距离、切比雪夫距离等计算出每一个点的启发值h规定直线距离、对角移动距离均为1。 2.初始化起点可以看作是第1次循环 初始化代码如Initialize函数(02-06行)所示首先将优先队列U初始化为空初始化机器人运动距离km0即机器人当前没有发生移动起点没有发生运动。初始化所有节点集合S中的所有节点终点最小距离g(s)与实时计算终点距离rhs(s)为无穷大。将起点rhs置为0并且将其插入到优先队列中U中启动搜索。 此时U{E3}此时g(E3)∞、rhs(E3)0、h(E3)由上一步计算为3此时k(E3)[030,0][3,0]接下来执行代码中的步骤2——ComputeShortestPath代码10-20行进图while循环当且仅当搜索起点达到局部一致且弹出的k值大等于起点的k值时才跳出循环 首先从U中弹出k值最小的节点此时U中只有E3此时CalculateKey(E3)[3,0]kold(E3)跳到代码15行进行判断得到g(E3)∞ rhs(E3)0满足条件执行16-17行的代码此时更新g(E3)rhs(E3)0E3达到局部平衡标记为0。此外遍历所有周围节点中的父节点此处规定距离起点近的为父节点即D2、E2、D3。 3.拓展节点第2次循环 遍历D2、E2、D3分别带入UpdateVertex函数中代码07-09行根据其子节点E3更新rhs值如rhs(D2)1(D2与E3的连接代价)g(E3)101同样可以计算的到rhs(E2)1、rhs(D3)1判断其是否为局部一致08行代码由于初始化g值均为∞D2、E2、D3均为局部过一致状态此时计算其k值并将其加入到优先级队列U中这步结束后U{D2[3,1],E2[4,1],D3[3,1]}。 4.拓展节点不断循环得到起点没有变化时的最终路径 不断循环上面的步骤跟LPA*算法几乎一样只是搜索方向上是反过来的LPA*从起点开始而D*lite从终点开始还是不难理解的 得到如上图所示的结果此时满足循环跳出的条件得到最终路径为B1-C1-D2-E3。跳出步骤2主函数中的ComputeShortestPath部分执行结束代码23行 5.起点变换时候的处理 如左图所示此时机器人位置由B1变为了C1D2变为了障碍物执行24-35行的代码。 当起点与终点不为同一节点时候进入该循环首先更新所有节点与当前新起点的启发值计算新的km值km(new)kmh(s_start,s_last)011代码第30行此时检查受影响的节点扩散其变化造成的影响代码32-34行详细执行更新代价步骤如下所示 如图6-1所示D2变为障碍物rhs(D2)inf由于C1与D2直接相连代码第32行C1的rhs值受到影响变rhs(C1)d(D1与C1之间的距离)g(D1)1infinf。同理找出C1的子节点并更新其rhs值rhs(D1)rhs(E1)inf这便是更新障碍物的传递过程代码第33行。 此时C1检查周围节点查找能够将rhs(C1)值变为最小的节点作为父节点代码第34行跳到步骤4此时将B1作为父节点能够使得rhs(C1)314与g(C1)2不相等为局部欠一致将其重新插入到队列U中代码09行 同理将D1、E1也加入到队列U中。 更新完所有受到影响的节点后再次调用路径搜索函数代码第35行重复2-4的步骤即可得到新的路径为C1-D1-E2-E3。 该算法的奇妙之处在于在起点更新后更新了h值能够启发新一轮的搜索大家可以这样理解一旦节点的起点发生变化基于起点求解得到的每一个节点的h值肯定会发生改变这工作量是巨大的这是LPA*算法不能用于变起点搜索的主要原因。为了减少计算量我们无需在原有的节点上减去h值而是维护一个由于起点位置更改而受影响km值km值可以理解为我们原本需要更新在原来每一个节点上h值的差值原本我们需要在每一个节点h中加或者减km此时我们只需要在新更新的节点上减或者加上这个差额维持其相对的大小顺序妙哉 二、D*lite算法MATLAB代码 这是笔者按照古月的课程打出来的代码已经上传到了本人Github需要的自取 Adamaser/Path-Planning (github.com)