网站开发常用的谷歌插件,有什么网站是专门做电商详情页,卖灯杆的做网站好,金华做公司网站所需先验知识#xff08;没有先验知识可能会有大碍#xff0c;了解的话会对D*的理解有帮助#xff09;#xff1a;A*算法/ Dijkstra算法 何为D*算法
Dijkstra算法是无启发的寻找图中两节点的最短连接路径的算法#xff0c;A*算法则是在Dijkstra算法的基础上加入了启发函数…所需先验知识没有先验知识可能会有大碍了解的话会对D*的理解有帮助A*算法/ Dijkstra算法 何为D*算法
Dijkstra算法是无启发的寻找图中两节点的最短连接路径的算法A*算法则是在Dijkstra算法的基础上加入了启发函数h(x)以引导Dijkstra算法搜索过程中的搜索方向让无必要搜索尽可能的少从而提升找到最优解速度。这两者都可应用于机器人的离线路径规划问题即已知环境地图已知起点终点要求寻找一条路径使机器人能从起点运动到终点。 路径规划问题红色起点蓝色终点黑色障碍物浅蓝色规划出来的路 但是上述两个算法在实际应用中会出现问题机器人所拥有的地图不一定是最新的地图或者说机器人拥有的地图上明明是可以行走的地方但是实际运行时却可能不能走因为有可能出现有人突然在地上放了个东西或者桌子被挪动了或者单纯的有一个行人在机人运行时路过或挡在机器人的面前。 机器人走了2步突然发现自己地图上不存在的障碍物 碰到这样的问题比如机器人沿着预定路径走到A点时发现在预先规划的路径上下一个应该走的点被障碍物挡住了这种情况时最简单的想法就是让机器人停下来然后重新更新这个障碍物信息并重新运行一次Dijkstra算法 / A*算法这样就能重新找到一条路。
但是这样子做会带来一个问题重复计算。假如如下图所示新的障碍物仅仅只是挡住了一点点机器人完全可以小绕一下绕开这个障碍物然后后面的路径仍然按照之前规划的走。可是重复运行Dijkstra算法 / A*算法时却把后面完全一样的路径重新又计算了一遍 标只需要小绕一下后面的路径跟原来一模一样本文中认为地图8邻接而非4邻接 D*算法的存在就是为了解决这个重复计算的问题在最开始求出目标路径后把搜索过程的所有信息保存下来等后面碰到了先验未知的障碍物时就可以利用一开始保存下来的信息快速的规划出新的路径。
顺便一提因为D*算法有上述的特性所以D*算法可以使用在“无先验地图信息/先验地图信息不多的环境中的导航”的问题因为只需要在最开始假装整个地图没有任何障碍起点到终点的路径就是一条直线然后再在在线运行时不断使用D*算法重新规划即可。
下面将开始讲述D*算法的流程。为此先提及一下算法应用的语境
机器人已知一个地图map在某个起点S要前往终点E。但是机器人走着走着突然在中途发现了地图上没有标注的障碍物比如走着走着突然用激光雷达发现前面1米处有一些没有标注在地图上的障碍物而这些障碍物有的挡住了机器人原本规划好的路有的没挡住。
为了简化我们认为机器人在栅格地图上运动每次运动一个格子8邻接并且机器人为一个点体积仅占据一个格子大小。 D*算法流程
先说明一下需要用到的类开始运行前需要对地图中每个节点创建一个state类对象以便在搜索中使用。 # 伪代码 class state: # 存储了每个地图格子所需的信息下面会说到这个类用在哪。以下为类成员变量 x # 横坐标 y # 纵坐标 t new # 记录了当前点是否被搜索过可为“new”,open, close,分别代表这个格子没被搜索过这个格子在open表里这个格子在close表里。关于什么是open表什么是close表建议去看A*算法能很快理解。初始化为new parent None # 父指针指向上一个state沿着某个点的父指针一路搜索就能找到从这个点到目标点end的最短路径 h # 当前代价值D*算法核心 k # 历史最小代价值D*算法核心意义是所有更新过的h之中最小的h值 d*算法主代码 function Dstar(map, start, end): # map:m*n的地图记录了障碍物。map[i][j]是一个state类对象 # start起点,state类对象 # end: 终点,state类对象 open_list [ ] # 新建一个open list用于引导搜索 insert_to_openlist(end0 open_list) # 将终点放入open_list中 # 第一次搜索基于离线先验地图找到从起点到终点的最短路径注意从终点往起点找 loop until start.t “close” process_state() # D*算法核心函数之一 end loop # 让机器人一步一步沿着路径走有可能在走的过程中会发现新障碍物 temp_p start while (p ! end) do if ( unknown obstacle found) then # 发现新障碍物, 执行重规划 for new_obstacle in new_obstacles: # 对每个新障碍物调用modify_cost函数 modify_cost( new_obstacle ) #D*算法核心函数之一 end for do k_min process_state() while not ( k_min temp_p.h or open_list.isempty() ) continue end if temp_p temp_p.parent end while 上述伪代码中核心函数为2个modify_cost 和 process_state。我翻阅了csdn几个关于process_state的解释发现都有挺大的错误会让整个算法在某些情况下陷入死循环比如D*规划算法及python实现_mhrobot的博客-CSDN博客。而且就连原论文的伪代码都有点问题可能原论文Optimal and Effificient Path Planning for Partially-Known Environments Anthony Stentz有解释但是我没仔细看.....但是如果仅按其伪代码来实现process_state函数的话是会有问题的。此文章最主要的目的就是说明这个问题并解决解决方法来源于wikipedia的D*算法页面。 下面是modify_cost的伪代码 function modify_cost( new_obstacle ) set_cost(any point to new_obstacle ) 10000000000 # 让“从任何点到障碍点的代价”和“从障碍点到任何点的代价” 均设置为无穷大程序中使用超级大的数代替考虑8邻接 if new_obstacle.t close then insert(new_obstacle, new_obstacle.h ) # 放到open表中insert也是d*算法中的重要函数之一 end if 下面是 Process_state函数的伪代码注意标红那条 function process_state( ) x get_min_k_state(oepn_list) # 拿出openlist中获取k值最小那个state这点目的跟A*是一样的都是利用k值引导搜索顺序但注意这个k值相当于A*算法中的f值fgh, g为实际代价函数值h为估计代价启发函数值而且在D*中不使用h这个启发函数值, 仅使用实际代价值引导搜索所以其实硬要说D*更像dijkstra都是使用实际代价引导搜索而不用启发函数缩减搜索范围D*这点对于后面发现新障碍物进行重新规划来说是必要的。 if x Null then return -1 k_old get_min_k(oepn_list) # 找到openlist中最小的k值其实就是上面那个x的k值 open_list.delete(x) # 将x从open list中移除, 放入close表 x.t close # 相当于放入close表只不过这里不显式地维护一个close表 # 以下为核心代码 # 第一组判断 if k_old x.h then # 满足这个条件说明x的h值被修改过认为x处于raise状态 for each_neighbor Y of X: #考虑8邻接neighbor if y.hk_old and x.h y.h cost(y,x) then x.parent y x.h y.h cost(x,y) end if end for end if # 第二组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for else: # 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then y.parent x insert(y, x.h cost(x,y)) else: if (y.parent ! x and y.h x.h cost(x,y)) then x.k x.h # 注意这行没有这行会出现特定情况死循环。在查阅大量资料后在wikipedia的d*算法页面中找到解决办法就是这行代码。网上大部分资料包括d*原始论文里都是没这句的不知道为啥 insert(x, x.h) else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then insert(y,y.h) end if end if end if end for end if return get_min_k(oepn_list) 上面使用到的insert函数 function insert(state, new_h) if state.t new: # 将未探索过的点加入到open表时h设置为传进来的 new_hstate.k h_new # 因为未曾被探索过k又是历史最小的h所以k就是new_helif state.t open:state.k min(state.k, h_new) # 保持k为历史最小的helif state.t close:state.k min(state.k, h_new) # 保持k为历史最小的hstate.h h_new # h意为当前点到终点的代价state.t open # 插入到open表里所以状态也要维护成openopen_list.add(state) # 插入到open表 以上便是完整的d*算法流程。 D*流程详解
现在我们从“d*算法主代码”开始详解d*函数
第一次搜索
首先可以看到在最开始我们需要创建一个open list然后将终点end加入到open list之后不断调用process_state直到起始点start被加入到close表。open表中的东西要按照k值大小排序每次调用process_state需要取出k值最小的一个来扩展搜索注意open list中的东西要按照 k 值大小从小到大排序而不是h值。
第一次搜索时对应主代码中的 # 第一次搜索基于离线先验地图找到从起点到终点的最短路径注意从终点往起点找 loop until start.t “close” process_state() # D*算法核心函数之一 end loop 这部分。
其实在首次搜索时我们可以发现每个点第一次被加入到open表中时都是从new状态加入即每次调用insert函数时state.t 最开始都是new所以其h值与k值其实是相等的在搜索过程中也即每一步调用process_state时k_old 也一直等于x.hk_old相当于x.k因此每次第一组判断都不会被执行第二组判断也总是只执行k_old x.h这部分 # 第二组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for else: ........ 而这部分中对x的8邻接点y的判断只会有两种情况①y.t new ②y.parent ! x and y.h x.h cost(x,y)。而第三种情况③y.parent x and y.h !x.h cost(x,y) 在这时是不会出现的因为h的意思是当前点到终点的代价parent的意思是最短路径上当前点到终点的路径的上一个点那么只要某个点的parent是x那么这个点的h一定是x的h值加上从x到这个点的代价。第三种情况出现必定是因为y.h、x.h或者cost(x,y)被非正常流程中人为修改过这在第一次搜索时是不会出现的而是在重规划时出现这点下面一个部分我们再谈。
也就是说在初次搜索时其实整个process_state代码相当于如下因为其他部分此时不会被调用 function process_state****(初次搜索时等价于如下函数)( ) x get_min_k_state(oepn_list) if x Null then return -1 k_old get_min_k(oepn_list) open_list.delete(x) x.t close # 第二组判断 for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for return get_min_k(oepn_list) 可以发现其实这就是dijkstra算法。
实际上d*算法在初步规划时完全退化成了dijkstra算法也就是说如果沿着规划出来的路径运动过程中不发生障碍物变化的话其实d*效率是要比a*低的因为d*并没有启发式地往目标去搜索搜索空间实际上比a*大很多。
但是d*这么做主要是为了提高在重规划时的效率因为dijkstra算法其实实现的是“找到某个点到空间内所有点的最短路径”注意我们是从终点开始往起点搜索的所以找到的就是全部点到终点的最短路径其实也不是全部点因为找到起点之后就停止搜索了没搜完的都还留在open表中也就是最短路程大于从起点到终点距离的点都没被搜。在运行过程中碰到障碍物需要重规划时我们就不再需要从当前点重新搜索到终点而是找出一条路径让我们绕开障碍物到达附近没有障碍物的空闲点即可而到达这个空闲点之后的路径不需要重新搜索因为根据初次搜索这个空闲点到目的地的最短路径我们已经有了。 重规划
接下来我们继续看主代码第一次搜索完之后的部分该程序默认第一次搜索能找到解第一次无解的话就没有重规划了所以不考虑奥 # 让机器人一步一步沿着路径走有可能在走的过程中会发现新障碍物 temp_p start while (p ! end) do if ( unknown obstacle found) then # 发现新障碍物 for new_obstacle in new_obstacles: # 对每个新障碍物调用modify_cost函数 modify_cost( new_obstacle ) #D*算法核心函数之一 end for do k_min process_state() while not ( k_min temp_p.h or open_list.isempty() ) continue end if temp_p temp_p.parent end while 这里让temp_p start然后每一步都让temp_p temp_p.partent是为了模拟机器人沿着路径一步步走的情形。当走到中途通过传感器发现了新的障碍物时首先将发现的新障碍物信息加入到已知地图中即执行 for new_obstacle in new_obstacles: # 对每个新障碍物调用modify_cost函数 modify_cost( new_obstacle ) #D*算法核心函数之一 end for 对每个新发现的障碍物点调用 modify_cost注意d*无法处理某个点本来是障碍物现在发现它不再是障碍物的情况不考虑它只考虑新增障碍物 function modify_cost( new_obstacle ) set_cost(any point to new_obstacle ) 10000000000 if new_obstacle.t close then insert(new_obstacle, new_obstacle.h ) # 放到open表中 end if return 首先将障碍物到周围所有点的移动代价设为无穷大。然后如果
新发现的障碍物点状态是“new”那就不用管因为等一下执行process_state时new点被第一次加入open表中时这个无穷大的代价就会体现在其周围的点的h值上。实际上如果一个点在重规划时还在new中的话证明这个点第一次搜索并没有利用到它的信息所以它在这次搜索过程中并不算“新成为障碍物”因为第一次搜索时根本都不知道它是不是障碍物如果这个点的状态是“open”也即这个点在open表中那么也不用管因为在open表中等下运行process_state时会被展开然后其障碍信息会通过cost(x,y)这一项传递给周围的点。障碍物点本身的h值不是无穷并没有关系因为虽然这样做的话障碍物点本身能找到通往终点的路径但是障碍物点本身并不会被机器人占领所以不用担心。如果这个点的状态是close那就将其原封不动也即不用修改其h值地移入open表待等下执行process_state时将其障碍信息通过cost传递给附近的点即可
在将新变成障碍物的点放入open表中后new 的点也总归会被加入open表如果没被加入那证明不需要它就开始重复执行process_state, 直到找到解或者无解即这段代码 do k_min process_state() while not ( k_min temp_p.h or open_list.isempty() ) 其中判断结束条件的 open_list.isempty() 如果被满足即open表为空意味着无解这点和A*及dijkstra是一样的。当然实际操作时一般不用open表为空作为条件而是用open表中最小的k值大于等于前面设置的超大的值时就退出或者在搜索过程中对于那些k值超过前面设置的超大值时直接不将其加入open表。
判断条件的 k_min temp_p.h 如果被满足则代表找到解找到从当前点到目标点的新最短路径了。简单来说 k_min process_state()这句 process_state()每次都是处理k值最小的点当open表中最小的k值k_min比当前所在的点的h值还要大的时候说明当前机器人所在的这个点temp_p已经被搜索过了。
在解释上面标绿的部分之前需要提一下h跟k究竟具体代表什么。
h值
在重规划时h表示搜到当前这一步的时候当前这个点到目标点的最小代价但是它不一定是最终找到最短路径后的最小代价因为很可能最短路径上的点还没被搜索到呢等会搜索到比现在这一步还短的路径时当前点h的值自然会被修改成沿着那条更短路径到终点的代价。
k值
对于初次搜索路径时k值的意义很明显就是跟h值是等价的两者并无区分具体意义参考dijkstra和A*算法的实际总代价值。
对于重规划时k值意义较为复杂为了理解它的作用我们必须看回代码。因为重规划实际上也是不断调用process_state的过程所以我们去找process_state中所有有可能修改k值的地方注意下面代码标红处 function process_state( ) x get_min_k_state(oepn_list) if x Null then return -1 k_old get_min_k(oepn_list) open_list.delete(x) x.t close # 以下为核心代码 # 第一组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.hk_old and x.h y.h cost(y,x) then x.parent y x.h y.h cost(x,y) # 仅仅只会修改h不修改k end if end for end if # 第二组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) # 调用了insert可能会涉及k值变化 end if end for else: # 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then y.parent x insert(y, x.h cost(x,y)) # 调用了insert可能会涉及k值变化 else: if (y.parent ! x and y.h x.h cost(x,y)) then x.k x.h # 直接修改了k insert(x, x.h) # 调用了insert可能会涉及k值变化 else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then insert(y,y.h) # 调用了insert可能会涉及k值变化 end if end if end if end for end if return get_min_k(oepn_list) 可以看到稍微涉及到k值变化的也就上面几个标红的地方其中4个都是insert函数。那么我们再来看insert函数 function insert(state, new_h) if state.t new: # 将未探索过的点加入到open表时h设置为传进来的 new_h state.k h_new # 因为未曾被探索过k又是历史最小的h所以k就是new_h elif state.t open: state.k min(state.k, h_new) # 保持k为历史最小的h elif state.t close: state.k min(state.k, h_new) # 保持k为历史最小的h state.h h_new # h意为当前点到终点的代价 state.t open # 插入到open表里所以状态也要维护成open open_list.add(state) # 插入到open表 在insert函数里一个state的k值仅仅在传进来的参数new_h比k值更小的时候k值才会被替换成新h值这也合理毕竟原论文中k的意义本身是“历史h值中最小的h值”嘛。
让我们再回到重规划时的process_state这个语境中。想一件事我有一个地图并且我一开始已经知道起点到终点的最短路径R也即第一次搜索得到的结果。可以肯定的是对于这个最短路径R上的任何一个点来说从这个点出发到目的地的最短路径肯定是前面说的这个最短路径R的从这个点开始的剩余部分。反证因为如果有更短的路那最短路径R就不是从起点到终点的最短路径了呀我完全可以走到这个点之后走那个更短的路径这样肯定更短这会出现矛盾。
现在我在原来地图的基础上额外增加新的障碍物很可能①障碍物挡在了我原来的最短路径R上。这种情况下我可能需要绕一条更长的路才能到达目标也可能存在一条跟当前路径一样长的路我可以走那条路但不论怎么说新的最短路径长度一定大于等于原本的最短路径R绝对不可能比最短路径R还短没理由障碍物多了反而我走的路更短吧。也可能是②障碍物没挡住我那我的最短路径一定不变。总结来说发现新障碍物之后的最短路径一定比原本没有新障碍物时候的最短路径长或者是一样长。
那么根据上面2段文字我们知道加入新障碍物后某个点到目标点的最短路径长度也即代价一定是只能增不能减的。这说明什么说明在重规划这个时候执行insert函数时传进来的参数new_h一定不可能比原本的k值小因为在第一次搜索结束时h跟k是相等的都是最小代价加入新障碍物后搜索过程中new_h “新代价”一定要比第一次搜索结束时的h“代价”要大那也就是肯定比k都大。那也就是说在重规划时insert函数中k值绝对不可能被更新。
说了3大段那么多就是想说明一个事情重规划时insert函数并不改变k值。那么返回到上面5处标红代码我们可以看到k值会被改变仅仅只有一个情况。那就是第二组判断中的某个条件下直接修改k值那个地方下面标红 function process_state( ) x get_min_k_state(oepn_list) if x Null then return -1 k_old get_min_k(oepn_list) open_list.delete(x) x.t close # 以下为核心代码 # 第一组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.hk_old and x.h y.h cost(y,x) then x.parent y x.h y.h cost(x,y) # 仅仅只会修改h不修改k end if end for end if # 第二组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) # 重规划过程中不修改k end if end for else: # 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then y.parent x insert(y, x.h cost(x,y)) # 重规划过程中不修改k else: if (y.parent ! x and y.h x.h cost(x,y)) then x.k x.h # 唯一一个修改k的地方 insert(x, x.h) # 重规划过程中不修改k else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then insert(y,y.h) # 重规划过程中不修改k end if end if end if end for end if return get_min_k(oepn_list) 关于这个标红的地方的k值究竟会怎么修改结论是k值一定会被变大变得比更新前的k值更大。原因在上面也说过了在重规划过程中所有点的代价一定比原来的大也即这里出现的所有h值肯定都比第一次搜索结束后的k值大那x.kx.h这句x.h肯定是比x.k大的。
那么什么时候一个点的k值会被修改呢先告诉你结论当我们找到了从这个点到目标点的考虑了新障碍物之后的新的最短路径newR后这个点的k值会标修改成更大并且这个修改后的k值代表了沿着新的最短路径newR这个点到目标点的代价 / 实际距离。注意如果在新的最短路径newR上某个点的代价即到目标的距离没有变化那其k值也不会被改变。至于原因我们将在后面详解process_state中各判断条件时讨论。
综上所述某个点的k值在重规划时意义如下
如果某个点的k值在重规划时被修改那么其修改后的k值就是考虑了新的障碍物之后这个点到目标点的最短路径长度代价。这里需要注意被修改k值的点不一定就在机器人接下来要走的路径上可能是周围一大片因为加入了新障碍物导致变化的点因为重规划可能不止运行一次下一次运行重规划时地图是基于此次重规划之上的所以需要这一次运行完重规划后地图状态是维持在dijkstra算法运行完毕即地图上每个点都知道自己到目标点的最短路径的状态。如果某个点的k值没被修改那么其k值就有两种情况①一是没有太大意义硬要说的话意义就仅仅是“运行重规划前即考虑新障碍物前该点到目标点的最短路径代价值”②二是不管考虑新障碍物与否因为这个点到目标点的最短路径长度不变所以其k值没变。
k的意义如上2种情况但是其实2情况的①出现的点的数量正常来说是很少的也就是说我们可以这样理解k值的意义基本上就是重规划后某个点到目标点的最短距离。
那么可能有人会想到即使2的①情况出现的很少但是它还是会出现那这难道不会影响后续又又又发现新障碍物时的重规划么答案是不会。原因是因为这部分点其实真的非常少说实话虽然我不太确定但是是有可能完全不出现这些点的而前面也说过k值真正影响的是搜索的顺序这部分点k值过小仅仅会影响他们在下一次重规划时优先考察优先在他们处调用process_state。 并且在调用process_state时就会正常被处理根据情况决定是否修正其k值相当于使其回到1.的状态或者2.②的状态。如果看到这里不太能理解那么建议看完下一部分之后再倒回来看看这个地方。
回到前面讨论的结束判断条件k_min temp_p.h代表的意义:
从d*算法主函数中可以看到重规划的过程实际上是不断调用process_state()直到满足终止条件为止。每一次调用process_state()都是在从open listopen表中抽出一个k值最小的state然后处理它并将其移出open表然后放入close表或者重新放回open表。也就是说不断调用process_state的过程中open list中最小的k值k_min的总体趋势是在不断增大oepn list每次都取出一个拥有最小k值的state并按照条件将新的节点加入open表这些新节点固然很可能有更小的k使open list中的k_min变小但是总体趋势上k_min是在不断变大的。
而对于open表来说最小的k值k_min表示所有需要搜索的点即所有在open表中的点到目标点的距离都大于等于k_min这个值。从上面我们知道temp_p.h代表的意义是当前点即机器人发现新障碍物时机器人所在的位置的到目标点的代价不一定是实际可行的路径中最小的但是只要这个值不是无穷大那证明已经找到从当前点到目标的一条路。那么k_min temp_p.h这个条件的意义就是“现在还需要继续搜索的点到目标的距离已经大于等于现在已经找到的当前机器人所在点到目标点的距离”那就是说再搜索下去即便找到新的路其代价值也比现在已知的这条路的代价要大所以不如现在这条路。因此可以说达到这个条件就可以结束重规划过程了因为已经找到最优解了。 详解process_state
为了真正理解d*算法必须理解其核心函数process_state。该函数在第一次搜索时意义在上面已经说明就是完全退化成dijkstgra 下面对该函数在重规划过程中的行为进行分析。先再次将这个函数完整地放在这里方便查看 function process_state( ) x get_min_k_state(oepn_list) if x Null then return -1 k_old get_min_k(oepn_list) open_list.delete(x) x.t close # 以下为核心代码 # 第一组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.hk_old and x.h y.h cost(y,x) then x.parent y x.h y.h cost(x,y) end if end for end if # 第二组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for else: # 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then y.parent x insert(y, x.h cost(x,y)) else: if (y.parent ! x and y.h x.h cost(x,y)) then x.k x.h insert(x, x.h) else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then insert(y,y.h) end if end if end if end for end if return get_min_k(oepn_list) 首先先看第一段 x get_min_k_state(oepn_list) # 从open list中取出k值最小的state if x Null then return -1 # 若没取到东西证明open_list为空也就是无解返回-1 k_old get_min_k(oepn_list) # 用一个变量k_old来存这个最小的k值 open_list.delete(x) # 将x移出open表并放入close表close表不显式地维护 x.t close # 因为x被放入close表修改其状态为close 这段表明每一次process_state都是取k值最小的一个进行搜索展开并且被搜索展开的state会被放入close表这跟a*、dijkstra的本质是一样的都是根据一个值引导搜索的先后在这里这个值是k值。
关于k_old你没有看错k_old就是等于取出来的x的k值get_min_k_state 和 get_min_k两个函数中min k都指的是同一个值。为啥这么写呢因为原论文伪代码就是这样写的那就照抄吧虽然感觉有点怪怪的。 障碍信息初步传递
接下来我们先讨论第二组判断。因为重规划过程中首先是对新障碍物进行modify_cost操作 for new_obstacle in new_obstacles: # 对每个新障碍物调用modify_cost函数 modify_cost( new_obstacle ) #D*算法核心函数之一 end for function modify_cost( new_obstacle ) set_cost(any point to new_obstacle ) 10000000000 if new_obstacle.t close then insert(new_obstacle, new_obstacle.h ) # 放到open表中 end if return 为了便于理解假设情况如下机器人处于下图的情况 灰色已经走过的路 红色机器人当前位置紫色新发现障碍物蓝色目标位置标题 这一步之后所有障碍物点即紫色点将被原封不动加入open表即不改变其k值h值直接放入open表。因为在加入这些障碍物点之前即第一次搜索之后open表中最小的k值肯定是大于等于起点的k值的dijkstra算法特性所以这些新加入的障碍物点k值肯定比open表中其他的点的k值都小那么在调用process_state时一定就是先被搜索。
而当process_state展开这些点时因为没有改过其k值h值所以第一组判断是不会进入的而是直接进入第二组判断的前半k_old x.h即会执行下面这段 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for 对于每个障碍物点周围的的点y如果
y是new也即没加入过open表的点那么因为y是第一次被搜素将y的父节点设置为这个障碍物点然后将其加入到open表中。注意因为障碍物点到周围所有点的代价都被设置为无穷大所以insert这句实际上相当于insert(y,∞, 而y是第一次加入open表那么其k值也会被设置为无穷。 在上图举例的情况里只有整个图左上角那个紫色的点的周围的点有可能是new而红色点附近的紫色点的8邻接点肯定不是new。如果是因为new而在这一步被把k设置成无穷大那么这部分点是不会被后续搜索的。但是不用担心因为出现这种情况的点是不会影响重新规划的最短路径的。y不是new一般来说大部分情况下y都会是close除非障碍物出现在上次搜索范围的边缘才会出现有open的情况可以想象一下是怎么回事并且y的父节点是这个新的障碍物点y.parent x and y.h !x.h cost(x,y) 注意如果y的父节点是这个新障碍物点那y.h !x.h cost(x,y)一定满足因为本来y.h 是等于x.h cost(x,y)的但是x变成了障碍物之后cost(x,y)变成了无穷大右边就不等于左边了。不用管y.h和x.h都是无穷大的情况这种清情况表示根本没解不会被process_state扩展到的满足这个条件的y都会被放入open表但其h值要被设置为无穷大因为相当于insert(y,∞y不是new并且y的父节点不是x而且y当前路径的代价比“y下一步走到x再走x的最短路径” 的代价还小即y.h x.h cost(x,y)不可能出现这种情况因为cost(x,y)是无穷大不用考虑。所以实际上这时只有上面2种情况
而其他不满足条件的点此时也就是y的父节点不是x的点那么这部分点是不会受到新障碍物影响的其到终点的最短路径不会变化所以不用加入open表重新考察。
对于上面因满足条件被加入open表的点发生了什么变化呢那就是这些点的h值都变成了无穷大成为了raise状态 这个“障碍物点周围的点h值变为无穷”的过程就是障碍物信息被传递的过程这个过程还会继续不断传递下去但是这次是通过另外的方式我们马上就来讨论 障碍信息消除与接力传递
在我们将障碍物信息初步传递给周围之后如果第一次搜索得到的最短路径并没有被新发现的障碍物挡住的话程序到这里就结束了这也很好理解我原本的路都没被挡那我干嘛要找别的路就按照原来的肯定就是最短的。但是如果挡住了这些障碍信息就需要被处理。本来我们讨论的目的也是讨论如果出问题了会怎么样嘛躲不开的
对于上一步被加入了open表的点我们知道他们的h值被设置成了无穷他们处于raise状态。那么在process_state处理到它们时这一次会首先从“第一组判断”开始处理因为满足了条件 # 第一组判断 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.hk_old and x.h y.h cost(y,x) then x.parent y x.h y.h cost(x,y) end if end for end if 这一次x是指在上一步中被加入open表的“障碍物周围的点”其h值x.h无穷大而k_old是第一次搜索时的值是一个有限值代表不考虑障碍物时到目标点的距离。对于x扩展其8邻接点并考察每一个点y如果同时满足 y的h值比x的k值小y.hk_old首先只有x周围的不是新发现障碍物的点的h值才会不是无穷而这些点因为还没被重规划过程处理过其h值仍然等于其k值。满足y.hk_old 也就意味着在第一次规划时x周围的比x更靠近终点的点这句话需要详细捋一捋因为在k_old就是x第一次规划时的ky.h又等于y.k也都是第一次规划时的值那在第一次规划这个语境下就是y的到终点的代价小于x的代价。用图来理解的话就大概是这个情况 X标记为点x; 假设终点蓝色在x的右下方的话那么粉色为满足1条件的点 x当前代价即无穷大比x先走y再按y的最短路径走的代价更大x.h y.h cost(y,x) 因为x当前代价x.h是无穷大所以这里的意思就是只要满足1的几个点里有不是障碍物的点那x就从那边走代价肯定更低。
满足上面2个条件也就意味着这个点虽然前往终点的路被新障碍物挡住了但是我只要小小的拐一下就可以绕开这个障碍物那么满足这样条件的点都会被执行 x.parent y x.h y.h cost(x,y) 也即直接从y那边绕也不用再往后搜索了现在在这步就可以立刻得出到终点最短的路径。成功被这样处理的raise状态的点h值会因为利用了第一次搜索遗留下来的信息直接下降这被称为“初步降低代价”同时这样相当于障碍信息被直接消除了。 至于需要大绕的点比如说需要往后倒退两步才能绕开障碍物的情况比如下图 因为往回倒退这个过程会让代价相对于上一次搜索来说增大那就设计其他栅格就需要后续考察了所以不能在这里直接处理。需要到第二组判断里处理 第一组判断结束后需要进入第二组判断如果在第一组判断中成功修改了x的h值那么是有可能让x直接退出raise状态称为low状态的也即让x的h值直接变回第一次搜索之后的k值那么这里在第二组判断中就会直接按照dijkstra的思路继续处理x及其周围的点了也即会执行这部分 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for 因为是dijkstra所以这部分中的 y.parent x and y.h !x.h cost(x,y)这个条件是不会触发的原因可以思考一下。提示一下x的h值变成了第一次搜索之后的k值也即k_old。
而如果x没有直接退出raise状态或者x的h值并没有在第一组判断中修改仍然是无穷那么在这第二组判断中就会执行后半部分并将这个“走另一条路所以路径边长了”的信息或者障碍信息也即h为正无穷这个信息传递给周围的邻格 else# 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then y.parent x insert(y, x.h cost(x,y)) else if (y.parent ! x and y.h x.h cost(x,y)) then x.k x.h insert(x, x.h) else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then insert(y,y.h) end if end if end for # 为了方便看我又改了改else if 的逻辑对比一下前面可以知道实际没有变化哦 在这部分也是遍历x的8邻接点y对y有以下情况 y.t new or (y.parent x and y.h !x.h cost(x,y) ) y是new表示第一次没搜索过yy.parent x and y.h !x.h cost(x,y)表示虽然y父节点指向x但是y的代价不是x的代价加上x到y的代价这代表y的代价不正确了它受到了x的变化带来的影响。这两种情况直接让y的父节点指向x, 再insert(y, y下一步走x的话的路径代价)放到open表里等会搜索即可。此时如果x的h是无穷那么相当于x的障碍信息被传递给了周围如果x.h是有限值但是比k_old大那相当于将“x现在要走一条跟之前不一样长的路”这个信息传递给了周围 y.parent ! x and y.h x.h cost(x,y) y的父节点不是x但是y的当前路径长度y.h比“y先走到x再根据x的最短路径走到终点”的路径长度 还要长那说明对y来说有更短的路而这个更短的路要经过x这说明x现在已知的路值得肯定因为会满足y.h x.h cost(x,y)这个条件时x.h一定不是正无穷好像不好说非常极端的情况下x.h∞ y.h∞一个值cost(x,y)有限值但比前面那个“一个值”小的时候因为程序中无穷大是用一个非常大的树数代替的所以有可能出现这种情况但是太极端了暂时没想到怎么构造这种情况一般应该是不会出现的先别考虑这个情况出现那也就是说从x点已经找到新的到达终点的路了那么现在就让x的k提升为x的h这时x的k值就在新障碍物存在的语境下具有了其应该具有的“最低代价”的意义。*******这也是程序中唯一一个能提升k值的地方也正因为有这句话在“发现的新障碍物导致最短路径会变长”这个情况下k值也即实际代价才能正确被提高为更大的值。很多网上的其他资料里伪代码里都没有这句而且d*原论文里也没有这句我认为那是错误的没有这句话的话新障碍物挡住路导致新路径需要变得更长的情况是根本没办法处理的即便第一次重规划成功后续很可能又发现新的障碍物那第二次重规划时的k值已经完全不再具有在第一次重规划时k值所具有的的意义了但是对于一个迭代运行的算法每一次运行时同样的变量应该有同样的意义才对。****** (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old 我们先来讨论y.hk_old这个条件我们回到第一组判断里的if中发现那里有一个“y.hk_old ”这么样的条件很明显在这里y.hk_old就是其互补条件也就是说我们首先要挑出在第一组判断中因为“y.hk_old ”被筛掉的点考察这些点那基本上用图来理解就是这样 X标记为点x; 假设终点蓝色在x的右下方的话那么粉色为满足y.hk_old的点 在这部分点中我们再考虑剩下的条件。那么我们再考察y.t close 这个条件。注意到满足那 么一大串条件y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old的话需要 执行的是insert(y,y.h) 那如果y.t是“open”的话执行insert又不修改h值的话相当于什么都没 干; 如果y.t是“new”的话就不会到这里了在第二组判断的一开始的“if y.t new or xxx那里就执行别的事情去了。 那么这里剩下需要讨论的条件是y.parent!x and x.hy.hcost(y,x): 这个条件指的是邻接点y有 不经过x的前往目标点的路径并且x按照当前路径走所需代价比先走到y再沿着y的路径走的 代价还要大注意跟2.一样隐含了y.h不是无穷大也即y到目标点是有解的这么一个条件。出 现这种情况则将y放入到open表里待重新考察。可是既然x先走y再沿y的路走比按照自己的路 走更快为什么不是修改x的父节点让其指向y即让x.parent y, x.h y.hcost(y,x)呢因 为其实如果把y加入到open表中下一次process_state展开这个y的时候根据y的k和h值的 情况还是会再次考察这里的“y.parent!x and x.hy.hcost(y,x)”这个条件的相当于展开y这个 点时考察的条件 y.hx.hcost(x,y)。 然后我们再来考察上面没说明的最后一个条件即第一组判断中的 if k_old x.h then for each_neighbor Y of X: #考虑8邻接neighbor if y.t new or (y.parent x and y.h !x.h cost(x,y) ) or (y.parent ! x and y.h x.h cost(x,y)) then y.parent x insert(y, x.h cost(x,y)) end if end for 上述标红的条件代表的意义。
先从结论说起上述条件会被触发必须满足前提k_old x.h 但是对于那些在重规划中x.h从头到尾一直等于k_old的点来说这里是不会被触发的因为对于这样的点来说相当于新发现的障碍物并不影响其到目标点的路径代价所以其h值才一直不变化。只有那些受到新发现障碍物影响导致x.h先上升了然后经过后续搜索修改了x.k的值使得h值与k值又相等了通过x.k x.h这一行的那些点才有可能会触发这一段。这代表着x虽然已经找到了到达目标点的新的最短路径并且x的k值已经经过修改恢复了正确的意义但是这个恢复并没有传递到yy的父节点是x的话其代价值h应该就等于x.hcost(x,y)才对但现在不等所以需要将其修改正确。之所以要把y再次加入open表是因为必须把y修改正确之后的信息继续传递给后续的栅格。简单来说这里的目的就是传递修正代价的信息 process_state函数总结
综上我们可以得到process_state函数中达到每个判断条件时所接受的处理的意义 function process_state( ) x get_min_k_state(oepn_list) if x Null then return -1 k_old get_min_k(oepn_list) open_list.delete(x) x.t close # 第一组判断 if k_old x.h then for each_neighbor Y of X: if y.hk_old and x.h y.h cost(y,x) then # 尝试初步降低代价仅在重规划时被使用 x.parent y x.h y.h cost(x,y) end if end for end if # 第二组判断 if k_old x.h then for each_neighbor Y of X: if y.t new or # ①dijkstra正常搜素流程 (y.parent x and y.h !x.h cost(x,y) ) or # ②传递修正代价的信息 (y.parent ! x and y.h x.h cost(x,y)) then # ③dijkstra正常搜素流程 # ②条件仅在重规划时会被使用①③在全过程使用 y.parent x insert(y, x.h cost(x,y)) end if end for else: # 不满足k_old x.h 那就是k_old x.h for each_neighbor Y of X: if y.t new or (y.parent x and y.h !x.h cost(x,y) ) then # 传递路径代价变化信息传递的可能是障碍信息也可能是“正确路径产生变化”的信息 y.parent x insert(y, x.h cost(x,y)) else: if (y.parent ! x and y.h x.h cost(x,y)) then # x点已经找到新的到达终点的路了修正k值使之恢复“最小代价”的含义 x.k x.h insert(x, x.h) else: if (y.parent!x and x.hy.hcost(y,x) and y.t close and y.hk_old then # 该点待重新考察重新放入open表 insert(y,y.h) end if end if end if end for end if return get_min_k(oepn_list) 以上便是关于d*算法尤其是process_state的详解。 Python 实践代码
以下为实践代码使用时请修改main函数中 NEW_OBS_STEP 38 # 在走了多少步之后发现新障碍map_png ./my_map.png # 初始地图png图片obs_png ./my_map_newobs.png # 新增障碍物png图片要跟初始地图相同大小黑色认为是新障碍物 上述3个变量。附带可用的地图图片如下 my_map.png my_map_newobs.png 好像只要是图片就会被CSDN压建议自己用画图软件画2个PNG文件命名为my_map.png跟my_map_newobs.png用黑白分别表示障碍物与空闲区域其中my_map表示最初的地图my_map_newobs表示走到一半发现的新障碍物要求两个图片像素长宽一致推荐都是100*100像素。 将2个图片放到与以下.py文件同目录并在同目录下运行以下.py文件即可
但是上面2个图有csdn加的水印可能自己画一张好
#!/usr/bin python
# coding:utf-8
import math
import cv2
from sys import maxsize
NEW_OBS_STEP 38 # 在走了多少步之后发现新障碍class State(object):def __init__(self, x, y):self.x xself.y yself.parent Noneself.state .self.t newself.h 0self.k 0def cost(self, state):if self.state # or state.state # or self.state % or state.state %:return maxsizereturn math.sqrt(pow(self.x - state.x, 2) pow(self.y - state.y, 2))def set_state(self, state):# .普通格子 新路径 # 障碍 % 新障碍 第一次搜索的路径 S 起点 E 终点if state not in [., , #, %, , S, E]:returnself.state stateclass Map(object):def __init__(self, row, col):self.row rowself.col colself.map self.init_map()def init_map(self):map_list []for i in range(self.row):temp []for j in range(self.col):temp.append(State(i, j))map_list.append(temp)return map_listdef get_neighbors(self, state):state_list []for i in [-1, 0, 1]:for j in [-1, 0, 1]:if i 0 and j 0:continueif state.x i 0 or state.x i self.row:continueif state.y j 0 or state.y j self.col:continuestate_list.append(self.map[state.x i][state.y j])return state_listdef set_obstacle(self, point_list):for i, j in point_list:if i 0 or i self.row or j 0 or j self.col:continueself.map[i][j].set_state(#)class DStar(object):def __init__(self, maps):self.map mapsself.open_list set()def process_state(self):x self.min_state()if x is None:return -1old_k self.min_k_value()self.remove(x)print(In process_state:(, x.x, , , x.y, , , x.k, ))# raise状态的点包含两种情况①有障碍信息不知道怎么去终点的点 ②找到了到终点的路径但是这条路径比最初没障碍的路径长还要考察一下if old_k x.h:for y in self.map.get_neighbors(x):if old_k y.h and x.h y.h x.cost(y):x.parent yx.h y.h x.cost(y)# low状态的点if old_k x.h: # 注意这个开头的if不可以是elif不然算法就不对了。某网上资源是有错误的for y in self.map.get_neighbors(x):if ((y.t new) or(y.parent x and y.h ! x.h x.cost(y)) or(y.parent ! x and y.h x.h x.cost(y))):y.parent xself.insert_node(y, x.h x.cost(y))else: # raise状态的点for y in self.map.get_neighbors(x):if (y.t new) or (y.parent x and y.h ! x.h x.cost(y)):y.parent xself.insert_node(y, x.h x.cost(y))else:if y.parent ! x and y.h x.h x.cost(y):x.k x.hself.insert_node(x, x.h)else:if y.parent ! x and x.h y.h x.cost(y) and y.t close and y.h old_k:self.insert_node(y, y.h)return self.min_k_value()def min_state(self):if not self.open_list:print(Open_list is NULL)return Noneresult min(self.open_list, keylambda x: x.k) # 获取openlist中k值最小对应的节点if result.k maxsize/2:return Nonereturn resultdef min_k_value(self):if not self.open_list:return -1result min([x.k for x in self.open_list]) # 获取openlist表中值最小的kif result maxsize/2:return -1return resultdef insert_node(self, state, h_new):if state.t new:state.k h_newelif state.t open:state.k min(state.k, h_new)elif state.t close:state.k min(state.k, h_new)state.h h_newstate.t openself.open_list.add(state)def remove(self, state):if state.t open:state.t closeself.open_list.remove(state)def modify_cost(self, state):if state.t close:self.insert_node(state, state.h)def run(self, start, end, obs_pic_path):self.insert_node(end, 0)while True:temp_min_k self.process_state()if start.t close or temp_min_k -1:breakstart.set_state(S)s startwhile s ! end:s s.parentif s is None:print(No route!)returns.set_state()s.set_state(E)# 添加噪声障碍点# rand_obs set()# while len(rand_obs) 1000:# rand_obs.add((int(random.random()*self.map.row), int(random.random()*self.map.row)))# 根据图片添加障碍点rand_obs set()# new_obs_pic cv2.imread(./my_map_newobs.png)new_obs_pic cv2.imread(obs_pic_path)for i in range(new_obs_pic.shape[0]):for j in range(new_obs_pic.shape[0]):if new_obs_pic[i][j][0] 0 and new_obs_pic[i][j][1] 0 and new_obs_pic[i][j][2] 0:rand_obs.add((i,j))temp_step 0 # 当前机器人走了多少步(一步走一格)temp_s startis_noresult False # 新障碍物是不是导致了无解while temp_s ! end:if temp_step NEW_OBS_STEP:# 观察到新障碍for i, j in rand_obs:if self.map.map[i][j].state #:continueelse:self.map.map[i][j].set_state(%) # 新增障碍物self.modify_cost(self.map.map[i][j])k_min self.min_k_value()while not k_min -1:k_min self.process_state()if k_min temp_s.h or k_min -1:# 条件之所以是x_c.h是因为如果从x_c找到了到达目的地的路那么自身的h就会下降当搜索到k_min比这个h小时# 证明需要超过“从当前点到终点的最短路径”的代价的路径已经找完了再搜也只会找到更长的路径所以没必要找了其他# 解都不如这个优。如果一直找不到解那么x_c.h会是一直是无穷那么就是说整个openlist都会被搜索完导致其变为# 空。所以就是无解is_noresult (k_min -1)breakif is_noresult:breakif temp_step NEW_OBS_STEP:draw_s temp_swhile not draw_s end:draw_s.set_state()draw_s draw_s.parenttemp_s temp_s.parenttemp_step 1temp_s.set_state(E)if __name__ __main__:NEW_OBS_STEP 38 # 在走了多少步之后发现新障碍map_png ./my_map.png # 初始地图png图片obs_png ./my_map_newobs.png # 新增障碍物png图片要跟初始地图相同大小黑色认为是新障碍物my_map cv2.imread(map_png)_x_range_max my_map.shape[0]_y_range_max my_map.shape[1]mh Map(_x_range_max,_y_range_max)for i in range(_x_range_max):for j in range(_y_range_max):if my_map[i][j][0] 0 and my_map[i][j][1] 0 and my_map[i][j][2] 0:# png图片里黑色的点认为是障碍物mh.set_obstacle([[i,j]])start mh.map[0][0] # 起点为0,0,左上角end mh.map[_x_range_max-1][_y_range_max-1] # 终点为max_x-1,max_y-1右下角dstar DStar(mh)dstar.run(start, end, obs_png )for i in range(_x_range_max):for j in range(_y_range_max):if dstar.map.map[i][j].state[0] : # 红色格子为发现新障碍物重规划后的路径my_map[i][j][0] 0my_map[i][j][1] 0my_map[i][j][2] 255elif dstar.map.map[i][j].state[0] : # 蓝色格子为第一次搜索的路径my_map[i][j][0] 255my_map[i][j][1] 0my_map[i][j][2] 0elif dstar.map.map[i][j].state[0] %: # 紫色为新发现的障碍物my_map[i][j][0] 255my_map[i][j][1] 0my_map[i][j][2] 255elif dstar.map.map[i][j].state[0] E: # 终点my_map[i][j][0] 128my_map[i][j][1] 0my_map[i][j][2] 128cv2.imshow(xx, my_map)cv2.waitKey()cv2.imwrite(./my_map_result.png, my_map) 上图为运行程序后输出的结果。
其中左上角为起点右下角为终点。蓝色路径为第一次规划的路径紫色为走到一半发现的新障碍物红色路径为发现新障碍物后重新规划的路径。修改程序中NEW_OBS_STEP变量可以修改走到第几步发现新的障碍物
