精华内容
下载资源
问答
  • D star算法

    2018-05-06 16:39:04
    d*算法综述,D*算法是A*算法的一个加强版,在动态路径规划中,实现对路径的最优化处理
  • 寻找路径的方法,对这种方法感兴趣的可以参考一下
  • D-Star算法

    千次阅读 2019-10-10 09:27:27
    D-Star算法名称Dynamic A Star,是一种适应于动态环境的启发式的路径搜索算法,适合周围环境位置或者周围环境处于动态变化的场景 【适用范围】 动态环境 【算法流程】 由于是动态环境,算法主要是两个阶段, 第...

    在这里插入图片描述
    【由于专栏后几篇限制vip观看,我又把完整算法在公众号“武汉AI算法研习”进行了发布,可以查看全专栏文章。】

    D-Star算法名称Dynamic A Star,是一种适应于动态环境的启发式的路径搜索算法,适合周围环境位置或者周围环境处于动态变化的场景

    【适用范围】

    动态环境

    【算法流程】

    由于是动态环境,算法主要是两个阶段,

    第一阶段是从目标点往起点进行探索,得到搜索区域节点距离终点最短路径的信息(父节点信息)

    第二阶段是机器人动态环境交互中,遇见障碍物时对路径做动态修改,机器人从起点开始往终点行走过程中遇见环境发生变化时的动作过程。

    【算法举例】

    同A-Star算法类似,D-star通过一个维护一个优先队列(OpenList)来对场景中的路径节点进行搜索,所不同的是,D-Star不是由起始点开始搜索,而是以目标点为起始,通过将目标点置于Open list中来开始搜索,直到机器人当前位置节点由队列中出队为止(当然如果中间某节点状态有动态改变,需要重新寻路,所以才是一个动态寻路算法)。

    【环境初始化】如下假如在一个20*20的方格网中,起点位置(1,2),终点位置(17,11),中间障碍物位置(4, 3), (4, 4), (4, 5), (4, 6), (5, 3), (6, 3), (7, 3)。

    在这里插入图片描述

    【第一步:采用Dijstra算法从目标节点向起始点搜索】(具体参考Dijkstra算法)最后通过从开始节点回溯到终点即可得到最短路径

    在这里插入图片描述

    【第二步:机器人沿最短路径移动过程中,如果下一状态发生变化(堵塞等)】假设机器人沿着起点开始往终点进行移动,移动过程中,重新设置障碍物(9, 3), (9, 4), (9, 5), (9, 6), (9, 7), (9, 8),此时机器人抵达(8,3)位置。

    在这里插入图片描述

    发现障碍物后,由于此时的(8,3)点已经不再Open list队列中,首先需要对(8,3)这个点的部分信息进行(modify_cost)更新:

    1、由于障碍物存在则其当前到终点的估计开销可以设置为无穷大(因为障碍物存在,估计开销h设置无穷大),原来k值依旧不变

    2、同时将次(8,3)节点加入Open list队列,此时Open list队列中的点一览如下用o表示

    在这里插入图片描述

    接下来进行process_state操作,首先获取Open list列表中的最小k的节点,以及其k值。(K(X):Key Function,该值是优先队列Openlist中的排序依据,K值最小的State位于队列头,对于处于OpenList上的State X,K(X)表示从X被置于Openlist后,X到G的最小代价H(X),可以简单理解为K(X)将位于Openlist的State X划分为两种不同的状态,一种状态为Raise(如果K(X)<H(X)),用来传递路径开销的增加(例如某两点之间开销的增加,会导致与之相关的节点到目标的路径开销随之增加);另一种状态为 Lower(如果K(X)=H(X)),用来传递路径开销的减少(例如某两点之间开销的减少,或者某一新的节点被加入到Openlist中,可能导致与之相关的节点到目标的路径开销随之减少)

    则由上图可以知道肯定获取的最小k值节点是(8,3)点,而且其K(X)<H(X),则意味着预估距离开销是增加的,然后遍历(8,3)周围的节点(此处假设其周围的节点h值是真实的),通过遍历(8,3)的周围节点选取

    1、距离终点更近的点( 明显(8,4))(代码计算如下)

    for y in self.map.get_neighbers(x): #遍历周边的点
         if y.h 新点到终点距离预估值<= k_old 小于(8,3)到终点距离的值 and x.h > y.h + x.cost(y)(8,2)到(8,3)距离:
              x.parent = y
              x.h = y.h + x.cost(y)
    

    则更新(8,3)的父节点为(8,4),新的规划路径为(8,3)=>(8,4)=>(9,5),新的路径显示如下

    在这里插入图片描述

    【第三步:沿当前路径继续到(8,4),然后(9,5)发现障碍物】继续上面第二步的处理方法规划新的路线,往下移动一步,直到没有障碍物抵达终点为止,最终的路线为如下显示。

    在这里插入图片描述

    【总结】

    D-Star算法相较于A-Star静态算法实现了比静态算法实时刷新更好的效率,一方面这效率来源于从终点往起点的搜索,这于Dijkstra算法非常类似,另一方面来源遇见障碍物时对周围节点进行遍历寻求下一个可行的节点,每次遇见障碍物不断进行对周围节点的遍历搜索,这动态过程是非常依赖于第一次从终点往起点进行搜索得到的信息。

    D_star算法能够在障碍物发生变化时,仍能找到一条路径,但不一定是一条最短的路径。

    展开全文
  • D star路径搜索算法

    万次阅读 2018-09-25 16:23:48
    D Star 寻路算法一、简介二、算法介绍2.1 符号表示2.2 算法描述三、算法总结 一、简介 “D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known ...

    一、简介

    “D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境存在动态变化的场景。

    二、算法介绍

    同A*算法类似,D-star通过一个维护一个优先队列(OpenList)来对场景中的路径节点进行搜索,所不同的是,D*不是由起始点开始搜索,而是以目标点为起始,通过将目标点置于Openlist中来开始搜索,直到机器人当前位置节点由队列中出队为止(当然如果中间某节点状态有动态改变,需要重新寻路,所以才是一个动态寻路算法)。

    2.1 符号表示

    本部分主要介绍一下论文中用到的一些符号及其含义。

    论文中将地图中的路径点用State表示,每一个State包含如下信息:

    Backpointer: 指向前一个state的指针,指向的state为当前状态的父辈,当前state称为指针指向state的后代,目标state无Backpointer。(路径搜索完毕后,通过机器人所在的state,通过backpointer即可一步步地移动到目标Goal state,GoalState以后用 G表示),b(X)=Y表示X的父辈为Y。

    Tag:表示当前state的状态,有 New、Open、Closed三种状态,New表示该State从未被置于Openlist中,Open表示该State正位于OpenList中,Closed表示已不再位于Openlist中。

    H(X):代价函数估计,表示当前State到G的开销估计。

    K(X):Key Function,该值是优先队列Openlist中的排序依据,K值最小的State位于队列头 ,对于处于OpenList上的State X,K(X)表示从X被置于Openlist后,X到G的最小代价H(X),可以简单理解为。K(X)将位于Openlist的State X划分为两种不同的状态,一种状态为Raise(如果K(X)<H(X)),用来传递路径开销的增加(例如某两点之间开销的增加,会导致与之相关的节点到目标的路径开销随之增加);另一种状态为 Lower(如果K(X)<H(X)),用来传递路径开销的减少(例如某两点之间开销的减少,或者某一新的节点被加入到Openlist中,可能导致与之相关的节点到目标的路径开销随之减少)。

    kmin:表示所有位于Openlist上的state的最小K值。

    C(X,Y) :表示X与Y之间的路径开销。

    Openlist 是依据K值由小到大进行排序的优先队列。

    2.2 算法描述

    搜索的关键是state的传递过程,即由G向机器人所在位置进行搜索的过程,这种传递过程是通过不断地从当前OpenList中取出K值最小的State来实现的,每当一个State由Openlist中移出时,它会将开销传递给它的邻居state,这些邻居state会被置于Openlist中,持续进行该循环,直到机器人所在State的状态为 Closed ,或者Openlist为空(表示不存在到G的路径)。

    算法最主要的是两个函数, Process-StateModify-Cost ,前者用于计算到目标G的最优路径,后者用于改变两个state之间的开销C(X,Y)并将受影响的state置于Openlist中。

    算法的主要流程,在初始时,所有state的t(Tag)被设置为 New ,H(G)被设置为0,G被放置于OpenList,然后Process-State函数被不断执行,直到机器人所处state X由openlist中出队,然后可以通过机器人的当前state按backpointer指向目标G。当移动过程中发现新探测到的障碍时,Modify-Cost函数立刻被调用,来更正C(°)中的路径开销并将受影响的state重新置于openlist中。令Y表示robot发现障碍时所在的state,通过不断调用Process-State直到kmin≥H(Y),这时表示路径开销的更改已经传播到了Y,此时,新的路径构建完成。

    在这里插入图片描述

    上图中L1-L3表示拥有最低K值的X由openlist中移出,如果X为Lower,那么它的路径代价为最优的。

    在L8-L13,X的所有邻接state都被检测是否其路径代价可以更低,状态为New的邻接state被赋予初始路径开销值,并且开销的变动被传播给每一个backpointer指向X的邻接state Y(不管这个新的开销比原开销大或者小),也就是说只要你指向了X,那么X的路径开销变动时,你的路径代价必须随之改变。这里可能存在由于X路径开销变动过大,Y可以通过非X的其他state到达目标且路径开销更小的情况,这点在L8-13中并没有处理,而是放在后续针对Y的process-state函数中,在对Y进行处理时,会将其backpointer指向周围路径开销最小的state。如果X的邻接State状态为New时,应将其邻接state的backpointer指向X。所有路径开销有所变动的state都被置于Openlist中进行处理,从而将变动传播给邻接的state。
    上述讨论的时X为Lower状态,接下来讨论X为Raise状态。

    如果X为Raise,它的路径开销H可能不是最优的,在L4-L7中,通过其邻居state中已经处于最优开销(即h(Y)≤kold)的节点来优化X的路径开销,如果存在更短的路径,则将X的backpointer指向其neighbor。在L15-L18中,开销变动传播到状态为New的邻居state。如果X可以使一个backpointer并不指向X的邻居state的路径开销最小,即Y通过X到目标G的距离更短,但是此时Y的backpointer并不指向X,针对这种情况,可以将X重新置于Openlist中进而优化Y。在L23-25中,如果X可以通过一个状态为closed的并不是最理想的邻居stateY来减小路径开销,那么将Y重新置于Openlist中。

    在这里插入图片描述
    在modify-cost中,更新C(X,Y)并将X重新置于Openlist中,当X通过process-state进行传播时,会对Y进行开销计算,h(Y)=h(X)+c(X,Y)

    三、算法总结


    相比A-star算法,D-star的主要特点就是由目标位置开始向起始位置进行路径搜索,当物体由起始位置向目标位置运行过程中,发现路径中存在新的障碍时,对于目标位置到新障碍之间的范围内的路径节点,新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播,能最小程度的减少计算开销。 路径搜索的过程我个人感觉其实和Dijkstra算法比较像,A-star算法中f(n)=g(n)+h(n),h(n)在D-star中并没有体现,路径的搜索并没有A-star所具有的方向感,即朝着目标搜索的感觉,这种搜索更多的是一种由目标位置向四周发散搜索,直到把起始位置纳入搜索范围为止,更像是Dijkstra算法。

    水平有限,对原文中的许多地方理解的还不透彻,描述能力也有限,本文更多的像是对论文第二部分进行了简要的翻译。不懂的地方建议去原文中查询。

    在网上找到了一个用java写的D-star的源码,分享给大家,帮助大家理解。
    链接:https://pan.baidu.com/s/1G1UtCe_1H8UhNe4hRdFl6A 密码:yta2

    展开全文
  • D star lite 算法的论文网址:http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf 根本思想是从后往前计算,每当客观环境发生变化时就可以节省未变环境的计算量: ...pf = DStar(x_start=0, y_

    D star lite 算法的论文网址:http://idm-lab.org/bib/abstracts/papers/aaai02b.pdf

    在这里插入图片描述
    根本思想是从后往前计算,每当客观环境发生变化时就可以节省未变环境的计算量:
    在这里插入图片描述
    Github复现代码:https://github.com/avgaydashenko/d_star
    clone这份代码后,运行main.py:

    from d_star import DStar
    
    # 设置起点(0,1)与终点(3,1)
    pf = DStar(x_start=0, y_start=1, x_goal=3, y_goal=1)
    
    # (2,1)不允许通行
    pf.update_cell(2, 1, -1)
    
    # (2,1)准许通行
    # pf.update_cell(2, 1, 0)
    
    # 重新规划路径
    pf.replan()
    # 得到路径规划的结果
    path = pf.get_path()
    
    展开全文
  • 基于实时数据的DSTAR寻路算法,徐宝华,张成文,地图的寻路算法主要包括两大类,基于静态数据的寻路算法和基于实时数据的寻路算法。本文对基于实时数据(路况拥堵因子)的寻路算
  • 但是您不需要他们在自己的程序中使用Dstar类。 $ tar -xzf dstar.tgz $ cd dstar $ make $ ./dstar 指令 [q / Q]-退出 [r / R]-重新计划 [a / A]-切换自动重新计划 [c / C]-清除(重新启动) 鼠标左键-使单元格不...
  • D*算法又称为动态A*算法,在未知环境或有动态障碍物出现时,采用A*算法需要丢弃初始规划完成的open表和close表,重新进行规划。造成规划时间的增加,D*算法的核心思想是先用dijkstra或A*从目标点向初始点进行反向...
  • D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境...

    一、简介

    “D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境存在动态变化的场景。

    1 算法介绍
    同A算法类似,D-star通过一个维护一个优先队列(OpenList)来对场景中的路径节点进行搜索,所不同的是,D不是由起始点开始搜索,而是以目标点为起始,通过将目标点置于Openlist中来开始搜索,直到机器人当前位置节点由队列中出队为止(当然如果中间某节点状态有动态改变,需要重新寻路,所以才是一个动态寻路算法)。

    2 .1 符号表示
    本部分主要介绍一下论文中用到的一些符号及其含义。
    论文中将地图中的路径点用State表示,每一个State包含如下信息:

    Backpointer: 指向前一个state的指针,指向的state为当前状态的父辈,当前state称为指针指向state的后代,目标state无Backpointer。(路径搜索完毕后,通过机器人所在的state,通过backpointer即可一步步地移动到目标Goal state,GoalState以后用 G表示),b(X)=Y表示X的父辈为Y。

    Tag:表示当前state的状态,有 New、Open、Closed三种状态,New表示该State从未被置于Openlist中,Open表示该State正位于OpenList中,Closed表示已不再位于Openlist中。

    H(X):代价函数估计,表示当前State到G的开销估计。

    K(X):Key Function,该值是优先队列Openlist中的排序依据,K值最小的State位于队列头 ,对于处于OpenList上的State X,K(X)表示从X被置于Openlist后,X到G的最小代价H(X),可以简单理解为。K(X)将位于Openlist的State X划分为两种不同的状态,一种状态为Raise(如果K(X)<H(X)),用来传递路径开销的增加(例如某两点之间开销的增加,会导致与之相关的节点到目标的路径开销随之增加);另一种状态为 Lower(如果K(X)<H(X)),用来传递路径开销的减少(例如某两点之间开销的减少,或者某一新的节点被加入到Openlist中,可能导致与之相关的节点到目标的路径开销随之减少)。
    kmin:表示所有位于Openlist上的state的最小K值。
    C(X,Y) :表示X与Y之间的路径开销。
    Openlist 是依据K值由小到大进行排序的优先队列。

    1.2 算法描述
    搜索的关键是state的传递过程,即由G向机器人所在位置进行搜索的过程,这种传递过程是通过不断地从当前OpenList中取出K值最小的State来实现的,每当一个State由Openlist中移出时,它会将开销传递给它的邻居state,这些邻居state会被置于Openlist中,持续进行该循环,直到机器人所在State的状态为 Closed ,或者Openlist为空(表示不存在到G的路径)。

    算法最主要的是两个函数, Process-State 和 Modify-Cost ,前者用于计算到目标G的最优路径,后者用于改变两个state之间的开销C(X,Y)并将受影响的state置于Openlist中。

    算法的主要流程,在初始时,所有state的t(Tag)被设置为 New ,H(G)被设置为0,G被放置于OpenList,然后Process-State函数被不断执行,直到机器人所处state X由openlist中出队,然后可以通过机器人的当前state按backpointer指向目标G。当移动过程中发现新探测到的障碍时,Modify-Cost函数立刻被调用,来更正C(°)中的路径开销并将受影响的state重新置于openlist中。令Y表示robot发现障碍时所在的state,通过不断调用Process-State直到kmin≥H(Y),这时表示路径开销的更改已经传播到了Y,此时,新的路径构建完成。
    在这里插入图片描述
    上图中L1-L3表示拥有最低K值的X由openlist中移出,如果X为Lower,那么它的路径代价为最优的。

    在L8-L13,X的所有邻接state都被检测是否其路径代价可以更低,状态为New的邻接state被赋予初始路径开销值,并且开销的变动被传播给每一个backpointer指向X的邻接state Y(不管这个新的开销比原开销大或者小),也就是说只要你指向了X,那么X的路径开销变动时,你的路径代价必须随之改变。这里可能存在由于X路径开销变动过大,Y可以通过非X的其他state到达目标且路径开销更小的情况,这点在L8-13中并没有处理,而是放在后续针对Y的process-state函数中,在对Y进行处理时,会将其backpointer指向周围路径开销最小的state。如果X的邻接State状态为New时,应将其邻接state的backpointer指向X。所有路径开销有所变动的state都被置于Openlist中进行处理,从而将变动传播给邻接的state。
    上述讨论的时X为Lower状态,接下来讨论X为Raise状态。

    如果X为Raise,它的路径开销H可能不是最优的,在L4-L7中,通过其邻居state中已经处于最优开销(即h(Y)≤kold)的节点来优化X的路径开销,如果存在更短的路径,则将X的backpointer指向其neighbor。在L15-L18中,开销变动传播到状态为New的邻居state。如果X可以使一个backpointer并不指向X的邻居state的路径开销最小,即Y通过X到目标G的距离更短,但是此时Y的backpointer并不指向X,针对这种情况,可以将X重新置于Openlist中进而优化Y。在L23-25中,如果X可以通过一个状态为closed的并不是最理想的邻居stateY来减小路径开销,那么将Y重新置于Openlist中。
    在这里插入图片描述
    在modify-cost中,更新C(X,Y)并将X重新置于Openlist中,当X通过process-state进行传播时,会对Y进行开销计算,h(Y)=h(X)+c(X,Y)。

    2 算法总结
    相比A-star算法,D-star的主要特点就是由目标位置开始向起始位置进行路径搜索,当物体由起始位置向目标位置运行过程中,发现路径中存在新的障碍时,对于目标位置到新障碍之间的范围内的路径节点,新的障碍是不会影响到其到目标的路径的。新障碍只会影响的是物体所在位置到障碍之间范围的节点的路径。在这时通过将新的障碍周围的节点加入到Openlist中进行处理然后向物体所在位置进行传播,能最小程度的减少计算开销。 路径搜索的过程我个人感觉其实和Dijkstra算法比较像,A-star算法中f(n)=g(n)+h(n),h(n)在D-star中并没有体现,路径的搜索并没有A-star所具有的方向感,即朝着目标搜索的感觉,这种搜索更多的是一种由目标位置向四周发散搜索,直到把起始位置纳入搜索范围为止,更像是Dijkstra算法。

    二、源代码

    function varargout = A_GUI(varargin)
    % A_GUI MATLAB code for A_GUI.fig
    %      A_GUI, by itself, creates a new A_GUI or raises the existing
    %      singleton*.
    %
    %      H = A_GUI returns the handle to a new A_GUI or the handle to
    %      the existing singleton*.
    %
    %      A_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
    %      function named CALLBACK in A_GUI.M with the given input arguments.
    %
    %      A_GUI('Property','Value',...) creates a new A_GUI or raises the
    %      existing singleton*.  Starting from the left, property value pairs are
    %      applied to the GUI before A_GUI_OpeningFcn gets called.  An
    %      unrecognized property name or invalid value makes property application
    %      stop.  All inputs are passed to A_GUI_OpeningFcn via varargin.
    %
    %      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
    %      instance to run (singleton)".
    %
    % See also: GUIDE, GUIDATA, GUIHANDLES
    
    % Edit the above text to modify the response to help A_GUI
    
    % Last Modified by GUIDE v2.5 21-Oct-2018 17:10:48
    
    % Begin initialization code - DO NOT EDIT
    gui_Singleton = 1;
    gui_State = struct('gui_Name',       mfilename, ...
                       'gui_Singleton',  gui_Singleton, ...
                       'gui_OpeningFcn', @A_GUI_OpeningFcn, ...
                       'gui_OutputFcn',  @A_GUI_OutputFcn, ...
                       'gui_LayoutFcn',  [] , ...
                       'gui_Callback',   []);
    if nargin && ischar(varargin{1})
        gui_State.gui_Callback = str2func(varargin{1});
    end
    
    if nargout
        [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
    else
        gui_mainfcn(gui_State, varargin{:});
    end
    % End initialization code - DO NOT EDIT
    
    
    % --- Executes just before A_GUI is made visible.
    function A_GUI_OpeningFcn(hObject, eventdata, handles, varargin)
    % This function has no output args, see OutputFcn.
    % hObject    handle to figure
    % eventdata  reserved - to be defined in a future version of MATLAB
    % handles    structure with handles and user data (see GUIDATA)
    % varargin   command line arguments to A_GUI (see VARARGIN)
    
    % Choose default command line output for A_GUI
    handles.output = hObject;
    
    % Update handles structure
    guidata(hObject, handles);
    
    % UIWAIT makes A_GUI wait for user response (see UIRESUME)
    % uiwait(handles.figure1);
    
    
    % --- Outputs from this function are returned to the command line.
    function varargout = A_GUI_OutputFcn(hObject, eventdata, handles) 
    % varargout  cell array for returning output args (see VARARGOUT);
    % hObject    handle to figure
    % eventdata  reserved - to be defined in a future version of MATLAB
    % handles    structure with handles and user data (see GUIDATA)
    
    % Get default command line output from handles structure
    varargout{1} = handles.output;
    
    
    % --- Executes on button press in pushbutton1.
    function pushbutton1_Callback(hObject, eventdata, handles)
    % hObject    handle to pushbutton1 (see GCBO)
    % eventdata  reserved - to be defined in a future version of MATLAB
    % handles    structure with handles and user data (see GUIDATA)
    
    % set up color map for display 生成彩色地图 
    global cmap;
    global map;
    global n_r;
    global n_c;
    global state;
    
    cmap = [1 1 1; ...% 1 -白色-无障碍
            0 0 0; ...% 2 -黑色-有障碍
            0 0.8 0; ...% 3 -绿色-已搜索
            0 0.4 0; ...% 4 -粉色-正在搜索
            0 1 1; ...% 5 -浅蓝色-起始点
            1 1 0; ...% 6 -黄色-目标点
            0 0 1];   % 7 -蓝色-最终路径
    colormap(cmap); 
    %生成随机地图
    map = zeros(n_r,n_c);
    randmap = rand(n_r,n_c);
    for i = 2:(sub2ind(size(randmap),n_r,n_c)-1)
        if (randmap(i) >= 0.75)
            map(i) = 2;
        end
    end
    
    map(1, 1) = 5; % start_coords 起点坐标
    map(n_r, n_c) = 6; % dest_coords 终点坐标
    image(1.5,1.5,map); 
    grid on; 
    axis image; 
    set(handles.text5,'string','随机地图生成完毕');
    
    
    % --- Executes on button press in pushbutton2.
    function pushbutton2_Callback(hObject, eventdata, handles)
    % hObject    handle to pushbutton2 (see GCBO)
    % eventdata  reserved - to be defined in a future version of MATLAB
    % handles    structure with handles and user data (see GUIDATA)
    
    %搜索最佳路径
    global n_r;
    global n_c;
    global cmap;
    global map;
    global state;
    
    nrows = n_r; 
    ncols = n_c; 
    start_node = sub2ind(size(map), 1, 1); 
    %sub2ind()函数将矩阵中的某个元素的线性序号计算出来
    %线性索引号例子:2*2矩阵[1 3;中,1是第一个,5是第二个
    %                       5 7]3是第三个,7是第四个
    %(matlab是列优先,不是我们通常习惯的行优先)
    dest_node = sub2ind(size(map), n_r, n_c); 
    % Initialize distance array 初始化距离数组
    distanceFromStart = Inf(nrows,ncols); 
    distanceFromStart(start_node) = 0 ;
    % For each grid cell this array holds the index of its parent 对于每个网格单元,该数组都保存其父单元的索引
    parent = zeros(nrows,ncols); 
     % Main Loop 
    while true 
      % Draw current map 
      map(start_node) = 5; 
      map(dest_node) = 6; 
      image(1.5, 1.5, map); 
      grid on; %网格
      axis image; %显示坐标
      drawnow; %刷新屏幕
      % Find the node with the minimum distance 找到距离最短的节点
      [min_dist, current] = min(distanceFromStart(:));
      if ((current == dest_node) || isinf(min_dist)) %TF = isinf(A)  返回一个和A尺寸一样的数组, 如果A中某个元素是inf  (无穷), 则对应TF中元素是1, 否则TF中对应元素是0break; 
      end; 
      %搜索中心的索引坐标:current,
      %搜索中心与起始点的路程:min_dist
      % 这两个值后面会用。
     
      map(current) = 3; 
      distanceFromStart(current) = Inf; 
      [i, j] = ind2sub(size(distanceFromStart), current); %索引号变为坐标
      neighbor = [i-1,j; 
                  i+1,j; 
                  i,j+1; 
                  i,j-1]; 
        outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows)+(neighbor(:,2)<1) + (neighbor(:,2)>ncols); 
        locate = find(outRangetest>0);  %返回outRangetest中大于0的元素的相对应的线性索引值。
        neighbor(locate,:)=[]; 
        neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2));
    for i=1:length(neighborIndex) 
     if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) 
         map(neighborIndex(i)) = 4; 
       if (distanceFromStart(neighborIndex(i))>= min_dist + 1 )     
           distanceFromStart(neighborIndex(i)) = min_dist+1;
             parent(neighborIndex(i)) = current;   
            % pause(0.02); 
       end 
      end 
     end 
     end
    

    三、运行结果

    在这里插入图片描述

    四、备注

    版本:2014a

    展开全文
  • 本文在研究了静态路径规划中用到的一些算法后,如A*算法,继而分析动态路径规划的一些思想,在此基础上分析D*Lite算法可以改进的地方,并给出优化后的算法程序。利用10×10、50×50、100×100三种规模的模拟路网做...
  • d_star D *寻路算法的基于文本的可视化。 该应用程序仅使用D *寻路算法,并在世界的每一步都打印出一个网格。 警告词:请勿运行类似map5的地图。 我对D *算法的实现不能很好地处理map5之类的地图。 当程序用完堆...
  • D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境...
  • D*算法”的名称源自 Dynamic A Star,最初由Anthony Stentz于“Optimal and Efficient Path Planning for Partially-Known Environments”中介绍。它是一种启发式的路径搜索算法,适合面对周围环境未知或者周围环境...
  • D*算法(Dynamic A Star)

    2020-09-12 05:09:41
    D*算法(Dynamic A Star) 符号及函数说明 Openlist是一个可以用来做广度优先搜索的队列 节点state的标识tag分为三类:没有加入过open表的(new)、在open表的(open)、曾经在open表但现在已经被移走的(closed)。 ...
  • A*/D*(寻路算法

    2021-02-27 07:48:52
    总共3个压缩包,包含2个Astar算法的压缩包,一个Dstar算法的压缩包,亲测有效。

空空如也

空空如也

1 2 3 4 5 ... 8
收藏数 142
精华内容 56
关键字:

dstar算法