精华内容
下载资源
问答
  • 自动驾驶路径规划算法学习-A Star算法及matlab实现 参考 https://blog.csdn.net/ck_Leo_Libra/article/details/105244795?spm=1001.2014.3001.5506 ... 1. A Star算法原理 A star算法几乎与Dijkstra实现方法一致,...

    自动驾驶路径规划算法学习-A Star算法及matlab实现

    参考

    https://blog.csdn.net/ck_Leo_Libra/article/details/105244795?spm=1001.2014.3001.5506

    https://blog.csdn.net/weixin_39199083/article/details/117001590?spm=1001.2014.3001.5501

    1. A Star算法原理

    A star算法几乎与Dijkstra实现方法一致,不同得是A star是启发式得搜索,相比Dijkstra计算效率更高。

    此处A Star原理参见我得另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现 ,甚至连代码得实现也几乎相同,仅仅是代价函数得计算有所不同。

    Dijkstra算法是广度优先算法,扩展了很多很远得节点,搜索有些盲目,如下图:

    在这里插入图片描述在这里插入图片描述

                          Dijkstra算法搜索效果                                            A Star算法搜索效果

    A Star算法对代价函数加入了启发项:

    其他算法流程几乎与Dijkstra算法一致,仅代价函数计算不同

    在这里插入图片描述                在这里插入图片描述

                      Dijkstra算法流程                                                               A Star算法流程

    2. 计算示例

    同Dijkstra算法一样,我们这里给出一个计算示例方便理解A Star的工作原理

    同样要求s到t的最短路径,相比Dijkstra这里还要求MAP给出每个节点的坐标信息。

    同Dijkstra算法,也需要构建两个点集open, close

    代价函数F=G(起点到当前节点花费代价)+h(当前节点到目标点代价)

    这里启发项的h采用欧氏距离,也可采用曼哈顿或者其他距离。

    Step1:

    将起始节点加入open表,其cost全为0,其父节点为s

    Step2:

    s为open表里cost最小点,将其加入close表,并扩展s节点子节点a,b,c,既不在close中又不在open中,分别计算其total cost f,并将a,b,c加入open表

    f(a)=G(a)+h(a)=G(s)+G(sa)+h(a)=0+5+((4-1)^2+(2-2)^2)^0.5=0+5+3=8

    f(b)=G(b)+h(b)=G(s)+G(sb)+h(b)=0+7+((4-2)^2+(2-2)^2)^0.5=0+7+2=9

    f(c)=G(c)+h(c)=G(s)+G(sc)+h(c)=0+2+((4-1)^2+(2-0)^2)^0.5=0+2+3.6=5.6

    Step3:

    找到open表中cost f 最小的是c, 将c加入close表中,并扩展c子节点e,e不在open中,又不在close中,将e加入open表,计算e 的cost f

    f(e)=G(e)+h(e)=G(c)+G(ce)+h(e)=2+8+((4-3)^2+(2-3)^2)^0.5=10+1.4=11.4

    ...........

    除了代价函数的计算与Dijkstra算法不同,其他完全相同,这里只列出3步作为参考,不再赘述,重复下去,直到目标点出现在Open中,就找到了最优路径。

    3.Matlab代码实现

    要实现A star算法,这里需要11个m函数,只要在Dijkstra代码的基础上稍加修改即可,Dijkstra算法代码见我另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现 

    相比Dijkstra多加入一个节点启发值计算函数h.m

    main.m略加修改

    AStar.m代替Dijkstra.m,其实就是修改了里面的代价函数计算方法而已。

    剩下8个m函数 CreateMap.m, FindList.m, GetPath.m, GetObstacle.m, isopen.m,  isObstacle.m , MotionModel.m , plot_map.m 与Dijkstra代码一模一样,见我另一篇博客 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现 

    这里仅贴出与Dijkstra算法不同的3个m函数:AStar.m, main.m, h.m

    3.1 AStar.m

    这是A Star算法实现的主体程序,参照算法流程

    在这里插入图片描述

    function path=AStar(obstacle,map)
    % 该程序为A*算法
    
    % 用于存储路径
    path = [];
    %OpenList
    open = [];
    %CloseList
    close = []; 
    % findFlag用于判断While循环是否结束
    findFlag=false;%目标标志
    
    %===================1.将起始点放在Openlist中======================
    %open变量每一行  [节点坐标,代价值F=G+H,代价值G,父节点坐标]
    open =[map.start(1), map.start(2) , 0+h(map.start,map.goal) , 0 , map.start(1) , map.start(2)];
    
    %更新状态--下一步的八个点
    next = MotionModel();
    
    %=======================2.重复以下过程==============================
    while ~findFlag
    
        %--------------------首先判断是否达到目标点,或无路径-----
        if isempty(open(:,1))
            disp('No path to goal!!');
            return;
        end
        %------------------判断目标点是否出现在open列表中
        [isopenFlag,Id]=isopen(map.goal,open);
        if isopenFlag
            disp('Find Goal!!');
            close = [open(Id,:);close]
            findFlag=true;
            break;
        end
        %------------------a.按照Openlist中的第三列(代价函数F)进行排序,
        %--------------------查找F值最小的节点
        [Y,I] = sort(open(:,3)); % 对OpenList中第三列排序
        open=open(I,:);%open中第一行节点是F值最小的
        
        %------------------b.将F值最小的节点(即open中第一行节点),放到close
        %--------------------第一行(close是不断积压的),作为当前节点
        close = [open(1,:);close];
        current = open(1,:);
        open(1,:)=[];% 因为已经从open中移除了,所以第一列需要为空
        
        %--------------------c.对当前节点周围的4个相邻节点,算法的主体:------------------------
        for in=1:length(next(:,1))
            % 获得相邻节点的坐标,代价值F先等于0,代价值G先等于0  ,后面两个值是
            % 其父节点的坐标值,暂定为零(因为暂时还无法判断其父节点坐标是多少)
            m = [current(1,1)+next(in,1) , current(1,2)+next(in,2) , 0 , 0 , 0 ,0]; 
            m(4) = current(1,4) + next(in,3); % m(4)  相邻节点G值
            m(3) = m(4) + h(m(1:2),map.goal);% m(3)  相邻节点F值
            
            %>>如果它不可达,忽略它,处理下一个相邻节点  (注意,obstacle这个数
            %  组中是包括边界的)
            if isObstacle(m,obstacle)
                continue;
            end
            %flag == 1:相邻节点  在Closelist中  targetInd = close中行号
            %flag == 2:相邻节点不在Openlist中   targetInd = []
            %flag == 3:相邻节点  在Openlist中   targetInd = open中行号
            [flag,targetInd] = FindList(m,open,close);
            
            %>>如果它在Closelist中,忽略此相邻节点
            if flag==1
                continue;
            %>>如果它不在Openlist中,加入Openlist,并把当前节点设置为它的父节点
            elseif flag==2
                m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                open = [open;m];%将此相邻节点加放openlist中
            %>>剩下的情况就是它在Openlist中,检查由当前节点到相邻节点是否更好,
            %  如果更好则将当前节点设置为其父节点,并更新F,G值;否则不操作
            else
                %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数F值)
                if m(3) < open(targetInd,3)
                    %更好,则将此相邻节点的父节点设置为当前节点,否则不作处理
                    m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                    open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新
                end
            end
        end
        plot_map(map,obstacle,open,close);
    end
    %追溯路径
    path=GetPath(close,map.start);
    end
    

    3.2 h.m

    这里采用的是曼哈顿距离再乘以10,如下

    h(当前节点)=abs(当前节点x坐标-目标节点x坐标)+abs(当前节点y坐标-目标节点y坐标)

    function hcost = h( m,goal )
    
    %计算启发函数代价值 ,这里采用曼哈顿算法
    hcost =10* abs(  m(1)-goal(1)  )+10*abs(  m(2)-goal(2)  );
    
    end

    3.3 main.m

    其实main.m与Dijkstra算法的主程序也是几乎一样,除了调用AStar算法命令不同,以及提示程序开始打印的文字不同外,其他的都相同

    % 该文件为以map.mat为地图文件,point.mat为起止位置文件,
    % 进行A*算法路径规划的主程序
    clc
    clear all
    close all;
    disp('A Star Path Planing start!!')
    load map.mat                    % 加载地图
    load point.mat                  % 加载起止位置点
    [map.XMAX,map.YMAX] = size(MAP); %%代表我们要画一个地图的长和宽
    map.start = node(1:2);  %起始点 注意必须在地图范围内
    map.goal = node(3:4);  %目标点 注意必须在地图范围内
    obstacle = GetObstacle(map,MAP);% 获取边界数据和障碍物坐标
    clear MAP node                  % 后续程序不再使用这两个变量
    %obstacle = [obstacle;4,1; 4,2; 4,3; 4,4; 3,4 ;2,4;];%全封死的情况,是没有路的
    
    % 画出地图和起止点
    figure(1)
    if length(obstacle)>=1
        plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
        % plot(obstacle(:,1),obstacle(:,2),'om');hold on;
    end
    pause(1);
    h=msgbox('Please confirm the map information and click the buttion "confirm".');
    uiwait(h,20);% 5s后关闭消息框
    if ishandle(h) == 1
        delete(h);
    end
    close 1
    figure(1)
    axis([1 map.XMAX+1 1 map.YMAX+1])
    set(gca,'YTick',0:1:map.YMAX);
    set(gca,'XTick',0:1:map.XMAX);
    grid on;hold on;
    % 绘制边界和障碍点
    plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
    % 绘制起始点
    plot(map.start(1)+.5,map.start(2)+.5,'bo');hold on;
    % 绘制终止点
    plot(map.goal(1)+.5,map.goal(2)+.5,'gd');hold on;
    text(map.goal(1)+1,map.goal(2)+.5,'Target');
    % plot(map.start(1),map.start(2),'*r');hold on;
    % plot(map.goal(1),map.goal(2),'*b');hold on;
    
    % 采用A*算法进行路径规划
    path = AStar(obstacle,map)% A*算法
    
    %画出路径
    %
    if length(path)>=1
        plot(path(:,1)+0.5,path(:,2)+0.5,'-m','LineWidth',5);hold on;
    end
    %}
    
    grid on;

    4.运行结果

    可以看视频的运行结果,与Dijkstra算法不同,AStar算法很高效,几乎是直接奔着目标路径去的,只扩展很少的子节点。 

    运行视频:https://www.bilibili.com/video/BV1i5411u75w

    完整代码:https://download.csdn.net/download/weixin_39199083/18886562?spm=1001.2014.3001.5501

    展开全文
  • 写这个系列的第一篇文章是自动驾驶路径规划算法学习(2)—A*算法 这篇文章的起源是看了古月居的文章运动规划入门 | 白话Dijkstra,从原理到Matlab实现,由于其中的编程风格与之前A*算法的风格不同,所以将其重写。 ...

    写这个系列的第一篇文章是自动驾驶路径规划算法学习(2)—A*算法
    这篇文章的起源是看了古月居的文章运动规划入门 | 白话Dijkstra,从原理到Matlab实现,由于其中的编程风格与之前A*算法的风格不同,所以将其重写。
    理论部分不再赘述,可查看古月居的文章。

    一、问题

    给定材料(CreateMAP.m)中包含的MATLAB代码,可以生成50X50的地图,x表示障碍物,起点和终点均已给定,用Dijkstra算法实现路径规划。

    二、说明

    古月居在文章中也给出了代码实现,可查看其文章。
    之前采用了A *算法的Matlab实现来进行代码实现,个人认为其实现结构较完整,所以本文采用该结构进行Dijkstra算法的实现。
    以下介绍各模块功能。

    三、main.m

    主程序文件,包括调用创建的地图,起止点,调用函数Dijkstra算法获取路径,绘制程序运行过程图像。

    % 该文件为以map.mat为地图文件,point.mat为起止位置文件,
    % 进行Dijkstra算法路径规划的主程序
    clc
    clear all
    close all;
    disp('Dijkstra Path Planing start!!')
    load map.mat                    % 加载地图
    load point.mat                  % 加载起止位置点
    [map.XMAX,map.YMAX] = size(MAP); %%代表我们要画一个地图的长和宽
    map.start = node(1:2);  %起始点 注意必须在地图范围内
    map.goal = node(3:4);  %目标点 注意必须在地图范围内
    obstacle = GetObstacle(map,MAP);% 获取边界数据和障碍物坐标
    clear MAP node                  % 后续程序不再使用这两个变量
    %obstacle = [obstacle;4,1; 4,2; 4,3; 4,4; 3,4 ;2,4;];%全封死的情况,是没有路的
    
    % 画出地图和起止点
    figure(1)
    if length(obstacle)>=1
        plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
        % plot(obstacle(:,1),obstacle(:,2),'om');hold on;
    end
    pause(1);
    h=msgbox('Please confirm the map information and click the buttion "confirm".');
    uiwait(h,20);% 5s后关闭消息框
    if ishandle(h) == 1
        delete(h);
    end
    close 1
    figure(1)
    axis([1 map.XMAX+1 1 map.YMAX+1])
    set(gca,'YTick',0:1:map.YMAX);
    set(gca,'XTick',0:1:map.XMAX);
    grid on;hold on;
    % 绘制边界和障碍点
    plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
    % 绘制起始点
    plot(map.start(1)+.5,map.start(2)+.5,'bo');hold on;
    % 绘制终止点
    plot(map.goal(1)+.5,map.goal(2)+.5,'gd');hold on;
    text(map.goal(1)+1,map.goal(2)+.5,'Target');
    % plot(map.start(1),map.start(2),'*r');hold on;
    % plot(map.goal(1),map.goal(2),'*b');hold on;
    
    % 采用Dijkstra算法进行路径规划
    path = Dijkstra(obstacle,map)% A*算法
    
    %画出路径
    %
    if length(path)>=1
        plot(path(:,1)+0.5,path(:,2)+0.5,'-m','LineWidth',5);hold on;
    end
    %}
    grid on;
    

    四、GetObstacle.m

    该文件用于生成地图的障碍点和边界点。

    function obstacle=GetObstacle(map,MAP)
    %获得地图的边界和障碍点的坐标
        % 生成边界的坐标,此处XMAX表示MAP的行数,YMAX表示MAP的列数
        boundary=[];
        for i1=0:(map.YMAX+1)
            boundary=[boundary;[0 i1]];
        end
        for i2=0:(map.XMAX+1)
            boundary=[boundary;[i2 0]];
        end
        for i3=0:(map.YMAX+1)
            boundary=[boundary;[map.XMAX+1 i3]];
        end
        for i4=0:(map.XMAX+1)
            boundary=[boundary;[i4 map.YMAX+1]];
        end
        obstacle = boundary;
        % 生成障碍点的坐标
        for i=1:map.XMAX
            for j=1:map.YMAX
                if MAP(i,j) == -1
                    obstacle=[obstacle;[i j]];
                end
            end
        end
    end
    

    五、CreateMAP.m

    该文件包括参数初始化、设置障碍点、选择起止位置点;最后将地图数据存为map.mat,起止位置点存为point.mat。

    clc;
    clear all;
    figure;
    
    % 参数初始化
    MAX_X=50;% 代表我们要画一个地图的长
    MAX_Y=50;% 代表我们要画一个地图的宽
    p_obstacle = 0.3;% 障碍率
    
    % 设置障碍点
    obstacle = ones(MAX_X,MAX_Y)*p_obstacle;
    % 将MAP矩阵中障碍点置为-1,非障碍点置为9998
    MAP = 9999*((rand(MAX_X,MAX_Y))>obstacle)-1;    % -1值代表障碍物
    j=0;
    x_val = 1;
    y_val = 1;
    axis([1 MAX_X+1 1 MAX_Y+1])
    set(gca,'YTick',0:1:MAX_Y);
    set(gca,'XTick',0:1:MAX_X);
    grid on;
    hold on;
    % 绘制出地图上的障碍物
    for i=1:MAX_X
        for j=1:MAX_Y
            if MAP(i,j) == -1
                plot(i+.5,j+.5,'rx');
            end
        end
    end
    %%地图上选择起始位置
    pause(1);
    h=msgbox('Please Select the Vehicle initial position using the Left Mouse button');
    uiwait(h,5);% 5s后关闭消息框
    if ishandle(h) == 1
        delete(h);
    end
    xlabel('Please Select the Vehicle initial position ','Color','black');
    but=0;
    while (but ~= 1) %Repeat until the Left button is not clicked
        [xval,yval,but]=ginput(1);
        xval=floor(xval);
        yval=floor(yval);
    end
    xStart=xval;%Starting Position
    yStart=yval;%Starting Position
    MAP(xval,yval) = 0;
    plot(xval+.5,yval+.5,'bo');
    %%地图上选择目标点
    pause(1);
    h=msgbox('Please Select the Target using the Left Mouse button in the space');
    uiwait(h,5);
    if ishandle(h) == 1
        delete(h);
    end
    xlabel('Please Select the Target using the Left Mouse button','Color','black');
    but = 0;
    while (but ~= 1) %Repeat until the Left button is not clicked
        [xval,yval,but]=ginput(1);
    end
    xval = floor(xval);
    yval = floor(yval);
    xTarget = xval;
    yTarget = yval;
    MAP(xval,yval) = 9998;
    plot(xval+.5,yval+.5,'gd');
    text(xval+1,yval+.5,'Target');
    node = [xStart,yStart,xTarget,yTarget];
    save map MAP;
    save point node;
    close(figure(1));
    

    六、Dijkstra.m

    根据Dijkstra算法的理论:
    在变量open中存放起始点以及所需考虑的路径点集合,其中每一行包括节点坐标、代价值G,父节点坐标;
    在变量close中存放每个循环中最优路径点集合,其数据格式与open相同。
    算法流程图如下图
    在这里插入图片描述

    function path=Dijkstra(obstacle,map)
    % 该程序为A*算法
    
    % 用于存储路径
    path = [];
    %OpenList
    open = [];
    %CloseList
    close = []; 
    % findFlag用于判断While循环是否结束
    findFlag=false;%目标标志
    
    %===================1.将起始点放在Openlist中======================
    %open变量每一行  [节点坐标,代价值G,父节点坐标]
    open =[map.start(1), map.start(2) ,0 , map.start(1) , map.start(2)];
    
    %更新状态--下一步的相邻点
    next = MotionModel();
    
    %=======================2.重复以下过程==============================
    while ~findFlag
    
        %--------------------首先判断是否达到目标点,或无路径-----
        if isempty(open(:,1))
            disp('No path to goal!!');
            return;
        end
        %------------------判断目标点是否出现在open列表中
        [isopenFlag,Id]=isopen(map.goal,open);
        if isopenFlag
            disp('Find Goal!!');
            close = [open(Id,:);close]
            findFlag=true;
            break;
        end
        %------------------a.按照Openlist中的第三列(代价函数F)进行排序,
        %--------------------查找F值最小的节点
        [Y,I] = sort(open(:,3)); % 对OpenList中第三列排序
        open=open(I,:);%open中第一行节点是F值最小的
        
        %------------------b.将F值最小的节点(即open中第一行节点),放到close
        %--------------------第一行(close是不断积压的),作为当前节点
        close = [open(1,:);close];
        current = open(1,:);
        open(1,:)=[];% 因为已经从open中移除了,所以第一列需要为空
        
        %--------------------c.对当前节点周围的相邻节点,算法的主体:------------------------
        for in=1:length(next(:,1))
            % 获得相邻节点的坐标,代价值F先等于0,代价值G先等于0  ,后面两个值是
            % 其父节点的坐标值,暂定为零(因为暂时还无法判断其父节点坐标是多少)
            m = [current(1,1)+next(in,1) , current(1,2)+next(in,2) , 0 , 0 ,0]; 
            m(3) = current(1,3) + next(in,3); % m(4)  相邻节点G值
            
            %>>如果它不可达,忽略它,处理下一个相邻节点  (注意,obstacle这个数
            %  组中是包括边界的)
            if isObstacle(m,obstacle)
                continue;
            end
            %flag == 1:相邻节点  在Closelist中  targetInd = close中行号
            %flag == 2:相邻节点不在Openlist中   targetInd = []
            %flag == 3:相邻节点  在Openlist中   targetInd = open中行号
            [flag,targetInd] = FindList(m,open,close);
            
            %>>如果它在Closelist中,忽略此相邻节点
            if flag==1
                continue;
            %>>如果它不在Openlist中,加入Openlist,并把当前节点设置为它的父节点
            elseif flag==2
                m(4:5)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                open = [open;m];%将此相邻节点加放openlist中
            %>>剩下的情况就是它在Openlist中,检查由当前节点到相邻节点是否更好,
            %  如果更好则将当前节点设置为其父节点,并更新G值;否则不操作
            else
                %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数G值)
                if m(3) < open(targetInd,3)
                    %更好,则将此相邻节点的父节点设置为当前节点,否则不作处理
                    m(4:5)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                    open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新
                end
            end
        end
        plot_map(map,obstacle,open,close);
    end
    %追溯路径
    path=GetPath(close,map.start);
    end
    

    七、MotionModel.m

    生成当前节点的相邻节点。

    function next = MotionModel()
    %当前节点  周围的八个相邻节点  与  当前节点的坐标差值(前两列)
    %当前节点  周围的八个相邻节点  与  当前节点的距离值(最后一列)
    next = [-1,1,14;...
        0,1,10;...
        1,1,14;...
        -1,0,10;...
        1,0,10;...
        -1,-1,14;...
        0,-1,10;...
        1,-1,14];
    end
    

    八、Isopen.m、isObstacle.m

    判断节点是否在open中,判断节点是否为障碍点。

    function [isopenFlag,Id] = isopen( node,open )
    
    %判断节点是否在open列表中,在open中,isopenFlag = 1,不在open中,isopenFlag = 0 .并反回索引号
    
    isopenFlag = 0;
    Id = 0;
    
    %如果open列表为空,则不在open列表中
    if  isempty(open)
        isopenFlag = 0;
    
    else %open列表不为空时
        for i = 1:length( open(:,1) )
           if isequal(  node(1:2) , open(i,1:2)  )  %在Openlist中
                isopenFlag = 1;
                Id = i;
                return;
           end 
        end
    end
    
    end
    
    function flag = isObstacle( m,obstacle )
    
    %判断节点m是否为障碍点,如果是就返为1,不是就返回0
    for io=1:length(obstacle(:,1))
        if isequal(obstacle(io,:),m(1:2))
            flag=true;
            return;
        end
    end
    flag=false;
    end
    

    九、FindList.m

    函数功能:
    如果相邻节点(m存储其信息)已经在Closelist中,则flag = 1,targetInd = 其所在close的行数,用来定位;
    如果相邻节点(m存储其信息)不在Openlist 中,则flag = 2 targetInd = [];
    如果相邻节点(m存储其信息) 已经在Openlist 中,则flag = 3 targetInd = 其所在open的行数,用来定位。

    function [flag,targetInd]=FindList(m,open,close)
    %{
    函数功能:
    如果相邻节点(m存储其信息)  已经在Closelist中,则flag = 1  targetInd = 其所在close的行数,用来定位
    如果相邻节点(m存储其信息)    不在Openlist 中,则flag = 2  targetInd = []
    如果相邻节点(m存储其信息)  已经在Openlist 中,则flag = 3  targetInd = 其所在open的行数,用来定位
    %}
    
    %如果openlist为空,则一定不在openlist中
    if  isempty(open)
        flag = 2;
        targetInd = [];
        
    else  %open不为空时,需要检查是否在openlist中
        %遍历openlist,检查是否在openlist中
        for io = 1:length(open(:,1))
            if isequal(  m(1:2) , open(io,1:2)  )  %在Openlist中
                flag = 3;
                targetInd = io;
                return;
            else  %不在Openlist中
                flag = 2;
                targetInd = [];
            end
        end
    end
    
    %如果能到这一步,说明:  一定不在Openlist中    那么需要判断是否在closelist中
    
    %遍历Closelist(注意closelist不可能为空)
    for ic = 1:length(close(:,1))
        if isequal(  m(1:2) , close(ic,1:2)  )  %在Closelist中
            flag = 1;
            targetInd = ic;
            return;%在Closelist中直接return
        end
    end
    end
    

    十、plot_map.m

    绘制运行过程图像。

    function  plot_map( map,obstacle,open,close )
    
    % %画出障碍点、起始点、终点
    %绘制网格
    % for i = 1:map.XMAX+3
    %    line([-0.5,map.XMAX+1.5],[i-1.5,i-1.5]);
    % end
    % 
    % for j = 1:map.YMAX+3
    %    line([j-1.5,j-1.5],[-0.5,map.YMAX+1.5]);
    % end
    pause(0.1);
    title('黑色为障碍点和边界点,红色为close节点,绿色为open节点,连线为path');
    %绘制节点
    plot(close(:,1)+0.5,close(:,2)+0.5,'sr','MarkerFaceColor','r');
    hold on;
    %pause(0.1);
    plot(open(:,1)+0.5,open(:,2)+0.5,'sg','MarkerFaceColor','g');
    hold on;
    %pause(0.1);
    end
    

    十一、GetPath.m

    该函数功能为通过close中的数据反推出路径点。

    function path=GetPath(close,start)
    
        ind=1;
        path=[];
        while 1
            path=[path; close(ind,1:2)];
            if isequal(close(ind,1:2),start)   
                break;
            end
            for io=1:length(close(:,1))
                if isequal(close(io,1:2),close(ind,4:5))
                    ind=io;
                    break;
                end
            end
        end
    end
    

    整个运行过程如下图所示:
    在这里插入图片描述

    展开全文
  • 自动驾驶路径规划算法学习-RRT算法及matlab实现 参考手把手教用matlab做无人驾驶(六)-路径规划RRT RRT快速随机数算法 Rapid Random Tree 是基于采样的规划方法的一种。 快速搜索随机树,就是在环境中随机撒...

    自动驾驶路径规划算法学习-RRT算法及matlab实现

    参考 手把手教用matlab做无人驾驶(六)-路径规划RRT

            RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现

            RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现

            RRT 算法原理以及过程演示

    RRT快速随机数算法 Rapid Random Tree

    是基于采样的规划方法的一种。

    快速搜索随机树,就是在环境中随机撒一些点,这些点经过算法运算,最终可以连接起来,变成车辆可以运行的轨迹。

    1.算法原理

    RRT 适用于涉及非完整约束场合下的路径规划问题。

    过程中,算法不断在搜索空间中随机生成状态点,如果该点位于无碰撞位置,则寻找搜索树中离该节点最近的结点为基准结点,由基准结点出发以一定步长朝着该随机结点进行延伸,延伸线的终点所在的位置被当做有效结点加入搜索树中。这个搜索树的生长过程一直持续,直到目标结点与搜索树的距离在一定范围以内时终止。随后搜索算法在搜索树中寻找一条连接起点到终点的最短路径。
    计算实例参考 RRT 算法原理以及过程演示,这篇博客讲的非常清楚,如下:

    1.1计算实例

    Step 1.初始化一个环境,包括地图,起点,终点。如下图所示,黑色物体为障碍物,蓝色飞机位于起点位置,红色五角星为目标终点位置

    Step2:从环境中随机采样状态点,如下图所示,采样点为 Xrand

    Step3: 从所构建的树中寻找距离采样点 Xrand最近的结点 Xnear。现在树中只有起点一个结点,所有最近的结点就是起点

    Step4:开始树的生长过程。首先连接 Xnear 和 Xrand连接起来,这个连接线的方向就是树生长的方向。设置一个步长 Stepsize作为树一次生长的步长,在树生长的这个方向上生长一个步长,然后就会在生长的末端会产生一个新的结点 Xnew

    Step5:判断从 Xnear 到 Xrand是否穿过障碍物,如果穿过,则放弃该新的结点,如果没有,则将 Xnew 结点加入到树中。

    Step6:从步骤 2 开始再循环执行,从环境中随机采样状态点。

    .........

    重复上述树的生长过程,直到树新生成的结点到目标点的距离小于一个步长,则终止树的生长。直接将该新结点与目标点相连。

    整个步骤动图如下:

    1.2算法伪代码

    概述之:

    输入地图,起点,终点→起点先加入树节点nodes表→在地图随机采样一个点xrand(同时保证有一定的概率会选择到目标点,保证有节点会向着目标点的方向扩展)→找到树节点中离xrand最近的点xnearest→xnearest朝着xrand前进一个步长得到新的点xnew→判断xnearest到xnew连线进行碰撞检测,若有碰撞则放弃该节点重新选择,若无碰撞则将该节点加入树节点→重复xnew的扩展过程直到扩展的xnew在目标点附近。

    2.算法Matlab实现

    这里只介绍了关键代码的实现,非完整程序,完整代码链接附在文末。

    2.1 二维环境的搭建  CreateMap.m

    CreateMap.m的主要作用输入起点终点障碍物等信息,障碍物是一一个个实心圆表示。绘制起点终点障碍物信息,代码如下:

    %CreateMap.m
    start = [0,0];
    goal = [10,14];
    %障碍物(x,y,radiu)
    obstacle_list=[3,3,1.5;
                   12,2,3;
                   3,9,2;
                   9,11,2];
    %画出地图框及障碍物           
    axis([-2,18,-2,15])
    hold on
    for i=1:length(obstacle_list(:,1))
    %调用画障碍物函数
    plot_obstacle(obstacle_list(i,1),obstacle_list(i,2),obstacle_list(i,3))
    end
    plot(start(1),start(2),'og')
    hold on
    plot(goal(1),goal(2),'or')
    hold on
    

    2.2 随机采样

    隶属于RRT算法程序RRT_planning.m的一部分。

    在状态空间中随机采样p_rand(这里采样的是一个坐标点), 有一定的概率采样到目标点,确保路径能以一定的概率向着目标点前进。这里随机采样取得目标点的概率是0.3,这个参数越大,越快找到路径,但障碍物较多时可能反而要耗费更多时间绕开。

    %随机采样取得目标点的概率是0.3,这个参数越大,越快找到路径,但障碍物较多时可能反而要耗费更多时间绕开
    p=0.3
    %在环境中采样p_rand,p_int是start
    p_randx = randi(16)-1;  %x随机采样范围0-15
    p_randy = randi(13)-1;  %x随机采样范围0-12
    p_rand=[p_randx,p_randy];
    %一定概率采样点取得目标点
    if rand(1)<p
        p_rand=goal;
    end

    2.3 FindNearest.m

    从节点树中找到距随机采样点p_rand最近的节点p_nearest的程序FindNearest.m程序如下:

    这里有一个要注意的细节,运行时出错,因为nodes节点树很可能存在好几个点同时到p_rand随机采样点距离最小,这里设置的返回值必须只有一个,如果有多个最近点,只取第一个返回。

    function minID=FindNearest(p_rand,nodes)
    %dist矩阵存放p_rand到nodes节点每个节点的距离
    %nodes的节点数
    nodes_num = length(nodes(:,1));
    prand_matx=ones(nodes_num,1)*p_rand(1);
    prand_maty=ones(nodes_num,1)*p_rand(2);
    nodes_matx=nodes(:,1);
    nodes_maty=nodes(:,2);
    dist=((prand_matx-nodes_matx).^2+(prand_maty-nodes_maty).^2).^0.5;
    minID=find(dist==min(dist));
    minID=minID(1);  %万一有多个同样小的,只取其中一个
    end

    2.4 扩展新节点

    最近点朝着随机点走一个步长得到新节点。

    先求出随机点p_rand和最近点p_nearest连线与x轴所成角theta,然后计算pnew新节点,代码如下:

    %随机点和树中最近点连线与x轴夹角
    theta = atan2(p_rand(2)-p_nearest(2),p_rand(1)-p_nearest(1));
    %计算新节点
    pnew(1)= p_nearest(1)+ RRT_stepsize*cos(theta);
    pnew(2)= p_nearest(2)+ RRT_stepsize*sin(theta);
    %看该随机点是否已在随机树nodes中,是的话重新选择,防止pnew在nodes里出现两次
    father=FindFather(pnew, nodes);
    if ~isempty(father)   %如果father非空说明能找到父节点说明在nodes里,重复了,避免出错
        continue
    end

    特别注意,我在跑程序时开始跑很多次总有一两次会陷入死循环,怎么都找不到错误所在,后来发现是在回溯路径时出现了两个节点互为父节点father的情况,原因是在扩展新节点pnew时,没有判断pnew是否已经在nodes树节点中了,如果已经在nodes树节点中,就不应再作为新的扩展点,本次随机采样放弃,进入下一次随机采样。下面用图说明:

    假设

    P2是由父节点P0扩展出;P1是由父节点P2扩展出;此时新一次的扩展P1扩展出的pnew正好是P2

    我们程序里树节点存放在nodes中,是一层层往上堆的,后扩展的放在上面,但是在nodes中找父节点时又是从上往下,则会出现下图的情况

    对扩展的新节点判断是否已经在nodes树节点中,若是则放弃本次采样,pnew也不加入树节点nodes中,就不会陷入死循环:

    2.5 碰撞检测 collision_check.m

    计算障碍物圆心o到线段p_nearest-pnew的最短距离是否小于半径,是则会发生碰撞。

    障碍物的圆心o和线段p_nearest-pnew的距离计算三种情况(垂点在线段之间,垂点在线段下方,垂点在线段上方):

    点到线段最短距离d的计算方法为点到直线的距离

    点到线段最短距离d的计算方法为圆心o到p_nearest的距离

    点到线段最短距离d的计算方法为圆心o到p_new的距离

    点到线段最短距离的实现封装为distance_squared_point_to_segment.m,其返回值为最短距离的平方,其代码如下:

    程序中  x1--p_nearest, x2--pnew, x3--圆心

    向量(x3-x1)乘向量(x2-x1)求到O-pnearest在pnearest-pnew上的投影,投影/l2求到垂足在线段长度中的百分比,可能超过1或为负数。超过1时,最短距离取x3x2,小于0时距离取x3x1

    distance_squared_point_to_segment.m

    function dd=distance_squared_point_to_segment(x1,x2,x3)
    %""" 计算线段 vw 和 点 p 之间的最短距离""",x1 near v; x2 new w; x3 obstacle_圆心 p
    %点 v 和 点 w 重合的情况
    if isequal(x1,x2)
        dd=(x3(1)-x1(1))^2+(x3(2)-x1(2))^2;
        return
    end
    %线段 vw 长度的平方
    l2=(x2(1)-x1(1))^2+(x2(2)-x1(2))^2;
    t = max(0, min(1, (x3 - x1)*(x2 - x1)' / l2));   %向量(x3-x1)乘向量(x2-x1)求到O-pnearest在pnearest-pnew上的投影,投影/l2求导垂足在线段长度中的百分比,可能超过1或为负数。超过1时,最短距离取x3x2,小于0时距离取x3x1
    projection = x1 + t * (x2 - x1);
    dd = (x3 - projection)*(x3 - projection)';
    end

    整个碰撞检测函数collision_check.m代码如下:

    function collisionflag=collision_check(pnew,p_nearest,obstacle_list)
    collisionflag=0;
    for i=1:length(obstacle_list(:,1))
    x0=p_nearest;
    x1=pnew;
    x2=[obstacle_list(i,1),obstacle_list(i,2)];
    dd = distance_squared_point_to_segment(x0,x1,x2);
    if dd<(obstacle_list(i,3))^2   %%距离小于障碍物半径
        collisionflag=1;
        return
    end
    end

    如果发生碰撞,就放弃本次采样,直接进入下一次

    如果没有发生碰撞,计算出的新节点pnew加入节点树nodes,并在nodes存入pnew父节点为p_nearest

    2.6 判断是否到达目标点is_near_goal.m

    判断的原理就是计算通过碰撞检测的pnew新扩展节点到目标点距离是否小于一个步长RRT_stepsize,是的话,

    则说明达到,并将目标点加入节点树nodes,记录其父节点为此时的pnew,实现代码如下:

    function goalflag=is_near_goal(pnew,goal,RRT_stepsize)
    goalflag=0;
    dist=pdist([pnew;goal],'euclidean');
    if dist<RRT_stepsize
        goalflag=1;
        return
    end
    end

    3. 运行结果

    RRT算法的几个可调节参数   步长RRT_stepsize,随机采样取得目标点概率p

    RRT_stepsize越大,计算路径的速度越快,当步长过大可能来回震荡往复无法达到目标点附近;

    p越大,计算路径的速度越快,节点更快向目标点生长,但p过大时,遇到障碍物时要花费更多的时间才能绕开,反而使搜索速度下降。

    从运行结果来看,搜索到的并非最优路径,可以了解RRT相关的改进算法如RRT*等,考虑到路径代价的优化。

    运行视频:https://www.bilibili.com/video/BV1wK4y1R7H7/

    运行代码:https://download.csdn.net/download/weixin_39199083/18932919?spm=1001.2014.3001.5501

    展开全文
  • 自动驾驶路径规划算法学习-Dijkstra算法及matlab实现 参考 https://mp.weixin.qq.com/s/WFdqax2n-kMa6-Of_8JJHg https://blog.csdn.net/ck_Leo_Libra/article/details/105511406?spm=1001.2014.3001.5506 ...

    自动驾驶路径规划算法学习-Dijkstra算法及matlab实现

    参考

    https://mp.weixin.qq.com/s/WFdqax2n-kMa6-Of_8JJHg

    https://blog.csdn.net/ck_Leo_Libra/article/details/105511406?spm=1001.2014.3001.5506

    Coursera self driving car part4

    1.Dijkstra算法原理

    Dijkstra算法是路。径规划的一种经典的算法。比如说求解从节点A到节点G的最短路径这种类似的问题。

    先介绍"图"的几个概念:节点 node, 节点与节点之间的连线称为边edge,每条边有对应的权重weight,为路程的距离或者代价。要找到从起始节点到终节点的最小代价的路径 。Dijkstra迪杰斯特拉算法其实是一种广度优先的算法,其算法流程如下:

    简单的概括算法流程就是:程序有两个点集open, closed。open点集待搜索的点集,closed存放已经找到起点到其最短路径的节点。起始时起点放入open点集,然后将起点的邻居点集存放到open中,然后找到其中起点到其代价最小的点放入closed,同时又将这个最小点的邻居放入open点同时存入起点到这些邻居的代价,再找代价最小的点继续扩展,一直重复,直到目标点出现在open中就找到了最短路径。中间必须记录父节点信息。

    以下面例子详细说明Dijkstra算法是怎么工作的:

    求节点s到节点t的最短路径

    Step1:

    将起点s放入open点集,cost为0,其father为s

    Step2:

    看当前open中cost最小的就是s点,将s及s的cost放入closed点集,记录其father为s,同时将s的所有子节点a,b,c及相应的cost放入open表中,father 记录为s

    Step3:

    当前open表中cost最小的节点是c,将c及c的cost 2放入closed表中,其父节点为s;并将其子节点e与e的cost值放入open表中,e的cost就等于father的cost加上fatehr到e的cost(找open最小,加入closed并扩展子节点)

    Step4:

    当前open中cost最小节点是a,放入closed,扩展其子节点b,d,相应cost为5+1=6,5+2=7,这个时候发现b已经在open中了,比较通过a到达b的cost 6和表里的b的cost 7相比要小,则更新b的cost为6,同时b的father从s更新为a

    Step5:

    当前open中cost最小节点是b,将b及cost放入closed表,扩展子节点为e,b扩展的e已经在open中,通过b扩展的e cost为6+3=9比表里已有的e cost 10要小,更新父节点为b及cost更新为9

    Step6:

    当前open中cost最小节点是d, 将d及d cost 7放入closed表,扩展子节点为t,e,通过d扩展的t,e其对应的cost为7+1=8,7+7=14,e通过d扩展cost为14不比open表大不更新,那么将t及其cost 8放入open中,父节点为d

    Step7:

    其实每一步都需检查目标点t是否已经出现在open中,r若是,就已经找到最短路径,回溯

    t father d father a fatehr s

    最佳路径为s-a-d-t,最短距离为t cost即8

    总结一下上述计算流程:

    创建open表,closed表

    将起始点加入open表

    当open中没有出现目标点时,重复这一步,找到open表中cost最小的节点,将其放入closed中,并记录其father,扩展该最近点的子节点,子节点若在closed中则忽略,若子节点在open中看是否cost变小若变小则更新,若子节点不在closed不在open,加入open表中,记录cost和father。

    输出最短路径及路径长度

    原理也不复杂,下面直接贴代码

    2. matlab代码实现

    代码都是参考https://blog.csdn.net/ck_Leo_Libra/article/details/105511406?spm=1001.2014.3001.5506

    2.1 CreateMap.m

    我们重点关注Dijkstra算法的实现,地图的创建稍微了解一下即可

    创建地图,设定地图为50*50,障碍率为0.3,实际上整个地图Map创建为一个大矩阵,矩阵元素为-1则这一格为障碍物,矩阵元素为9998则可以通行。同时目标点,起始点由鼠标在图中选取

    clc;
    clear all;
    figure;
    
    % 参数初始化
    MAX_X=50;% 代表我们要画一个地图的长
    MAX_Y=50;% 代表我们要画一个地图的宽
    p_obstacle = 0.3;% 障碍率
    
    % 设置障碍点
    obstacle = ones(MAX_X,MAX_Y)*p_obstacle;
    % 将MAP矩阵中障碍点置为-1,非障碍点置为9998
    MAP = 9999*((rand(MAX_X,MAX_Y))>obstacle)-1;    % -1值代表障碍物
    j=0;
    x_val = 1;
    y_val = 1;
    axis([1 MAX_X+1 1 MAX_Y+1])   %x轴,y轴范围1-50的图像
    set(gca,'YTick',0:1:MAX_Y);   %x轴,y轴间隔为1
    set(gca,'XTick',0:1:MAX_X);
    grid on;
    hold on;
    % 绘制出地图上的障碍物
    for i=1:MAX_X
        for j=1:MAX_Y
            if MAP(i,j) == -1
                plot(i+.5,j+.5,'rx');
            end
        end
    end
    %%地图上选择起始位置
    pause(1);
    h=msgbox('Please Select the Vehicle initial position using the Left Mouse button');
    uiwait(h,5);% 5s后关闭消息框
    if ishandle(h) == 1
        delete(h);
    end
    xlabel('Please Select the Vehicle initial position ','Color','black');
    but=0;
    while (but ~= 1) %Repeat until the Left button is not clicked
        [xval,yval,but]=ginput(1);
        xval=floor(xval);
        yval=floor(yval);
    end
    xStart=xval;%Starting Position
    yStart=yval;%Starting Position
    MAP(xval,yval) = 0;
    plot(xval+.5,yval+.5,'bo');
    %%地图上选择目标点
    pause(1);
    h=msgbox('Please Select the Target using the Left Mouse button in the space');
    uiwait(h,5);
    if ishandle(h) == 1
        delete(h);
    end
    xlabel('Please Select the Target using the Left Mouse button','Color','black');
    but = 0;
    while (but ~= 1) %Repeat until the Left button is not clicked
        [xval,yval,but]=ginput(1);
    end
    xval = floor(xval);
    yval = floor(yval);
    xTarget = xval;
    yTarget = yval;
    MAP(xval,yval) = 9998;
    plot(xval+.5,yval+.5,'gd');
    text(xval+1,yval+.5,'Target');
    node = [xStart,yStart,xTarget,yTarget];
    save map MAP;
    save point node;
    close(figure(1));

    2.2 FindList.m

    这个m函数的作用是判断某个节点是否在open或者close中

    function [flag,targetInd]=FindList(m,open,close)   ,m为某节点(1x5), m=[x坐标,y坐标,该节点代价值G, 父节点x坐标,父节点y坐标]

    如果相邻节点(m存储其信息)  已经在Closelist中,则flag = 1  targetInd = 其所在close的行数,用来定位
    如果相邻节点(m存储其信息)    不在Openlist 中,则flag = 2  targetInd = []
    如果相邻节点(m存储其信息)  已经在Openlist 中,则flag = 3  targetInd = 其所在open的行数,用来定位

    function [flag,targetInd]=FindList(m,open,close)
    %{
    函数功能:
    如果相邻节点(m存储其信息)  已经在Closelist中,则flag = 1  targetInd = 其所在close的行数,用来定位
    如果相邻节点(m存储其信息)    不在Openlist 中,则flag = 2  targetInd = []
    如果相邻节点(m存储其信息)  已经在Openlist 中,则flag = 3  targetInd = 其所在open的行数,用来定位
    %}
    
    %如果openlist为空,则一定不在openlist中
    if  isempty(open)
        flag = 2;
        targetInd = [];
        
    else  %open不为空时,需要检查是否在openlist中
        %遍历openlist,检查是否在openlist中
        for io = 1:length(open(:,1))
            if isequal(  m(1:2) , open(io,1:2)  )  %在Openlist中
                flag = 3;
                targetInd = io;
                return;
            else  %不在Openlist中
                flag = 2;
                targetInd = [];
            end
        end
    end
    
    %如果能到这一步,说明:  一定不在Openlist中    那么需要判断是否在closelist中
    
    %遍历Closelist(注意closelist不可能为空)
    for ic = 1:length(close(:,1))
        if isequal(  m(1:2) , close(ic,1:2)  )  %在Closelist中
            flag = 1;
            targetInd = ic;
            return;%在Closelist中直接return
        end
    end
    end

    2.3 GetObstacle.m

    CreateMap后,将障碍物所在的坐标以及边界线所在坐标(x,y)提取出来汇总成obstacle矩阵

    function obstacle=GetObstacle(map,MAP)
    %获得地图的边界和障碍点的坐标
        % 生成边界的坐标,此处XMAX表示MAP的行数,YMAX表示MAP的列数
        boundary=[];
        for i1=0:(map.YMAX+1)
            boundary=[boundary;[0 i1]];
        end
        for i2=0:(map.XMAX+1)
            boundary=[boundary;[i2 0]];
        end
        for i3=0:(map.YMAX+1)
            boundary=[boundary;[map.XMAX+1 i3]];
        end
        for i4=0:(map.XMAX+1)
            boundary=[boundary;[i4 map.YMAX+1]];
        end
        obstacle = boundary;
        % 生成障碍点的坐标
        for i=1:map.XMAX
            for j=1:map.YMAX
                if MAP(i,j) == -1
                    obstacle=[obstacle;[i j]];
                end
            end
        end
    end
    

    2.4 isObstacle.m

    在2.3获得obstacle点集后,用下面的程序判断节点m是否在obstacle中,主要用到

    if isequal(obstacle(io,:),m(1:2))

    function flag = isObstacle( m,obstacle )
    
    %判断节点m是否为障碍点,如果是就返为1,不是就返回0
    for io=1:length(obstacle(:,1))
        if isequal(obstacle(io,:),m(1:2))
            flag=true;
            return;
        end
    end
    flag=false;
    end
    

    2.5 isopen.m

    判断某节点是否在open中

    节点在open中,isopenFlag = 1,.并反回索引号;不在open中,isopenFlag = 0

    function [isopenFlag,Id] = isopen( node,open )
    
    %判断节点是否在open列表中,在open中,isopenFlag = 1,不在open中,isopenFlag = 0 .并反回索引号
    
    isopenFlag = 0;
    Id = 0;
    
    %如果open列表为空,则不在open列表中
    if  isempty(open)
        isopenFlag = 0;
    
    else %open列表不为空时
        for i = 1:length( open(:,1) )
           if isequal(  node(1:2) , open(i,1:2)  )  %在Openlist中
                isopenFlag = 1;
                Id = i;
                return;
           end 
        end
    end
    
    end
    

    2.6 MotionModel.m

    小车的运动模型,这里就是矩阵的上下左右左上右上等...邻居一共8个,这里的设定是相邻的两个格子的距离为10(1格代表10,对角线14)

    function next = MotionModel()
    %当前节点  周围的八个相邻节点  与  当前节点的坐标差值(前两列)
    %当前节点  周围的八个相邻节点  与  当前节点的距离值(最后一列)
    next = [-1,1,14;...
        0,1,10;...
        1,1,14;...
        -1,0,10;...
        1,0,10;...
        -1,-1,14;...
        0,-1,10;...
        1,-1,14];
    end
    

    2.7 Dijkstra.m

    这个m函数是实现Dijkstra算法的部分

    按照算法流程的每个模块编写程序如下:

    function path=Dijkstra(obstacle,map)
    % 该程序为A*算法
    
    % 用于存储路径
    path = [];
    %OpenList
    open = [];
    %CloseList
    close = []; 
    % findFlag用于判断While循环是否结束
    findFlag=false;%目标标志
    
    %===================1.将起始点放在Openlist中======================
    %open变量每一行  [节点坐标,代价值G,父节点坐标]
    open =[map.start(1), map.start(2) ,0 , map.start(1) , map.start(2)];
    
    %更新状态--下一步的相邻点
    next = MotionModel();
    
    %=======================2.重复以下过程==============================
    while ~findFlag
    
        %--------------------首先判断是否达到目标点,或无路径-----
        if isempty(open(:,1))
            disp('No path to goal!!');
            return;
        end
        %------------------判断目标点是否出现在open列表中
        [isopenFlag,Id]=isopen(map.goal,open);
        if isopenFlag
            disp('Find Goal!!');
            close = [open(Id,:);close]
            findFlag=true;
            break;
        end
        %------------------a.按照Openlist中的第三列(代价函数F)进行排序,
        %--------------------查找F值最小的节点
        [Y,I] = sort(open(:,3)); % 对OpenList中第三列排序
        open=open(I,:);%open中第一行节点是F值最小的
        
        %------------------b.将F值最小的节点(即open中第一行节点),放到close
        %--------------------第一行(close是不断积压的),作为当前节点
        close = [open(1,:);close];
        current = open(1,:);
        open(1,:)=[];% 因为已经从open中移除了,所以第一列需要为空
        
        %--------------------c.对当前节点周围的相邻节点,算法的主体:------------------------
        for in=1:length(next(:,1))
            % 获得相邻节点的坐标,代价值F先等于0,代价值G先等于0  ,后面两个值是
            % 其父节点的坐标值,暂定为零(因为暂时还无法判断其父节点坐标是多少)
            m = [current(1,1)+next(in,1) , current(1,2)+next(in,2) , 0 , 0 ,0]; 
            m(3) = current(1,3) + next(in,3); % m(4)  相邻节点G值
            
            %>>如果它不可达,忽略它,处理下一个相邻节点  (注意,obstacle这个数
            %  组中是包括边界的)
            if isObstacle(m,obstacle)
                continue;
            end
            %flag == 1:相邻节点  在Closelist中  targetInd = close中行号
            %flag == 2:相邻节点不在Openlist中   targetInd = []
            %flag == 3:相邻节点  在Openlist中   targetInd = open中行号
            [flag,targetInd] = FindList(m,open,close);
            
            %>>如果它在Closelist中,忽略此相邻节点
            if flag==1
                continue;
            %>>如果它不在Openlist中,加入Openlist,并把当前节点设置为它的父节点
            elseif flag==2
                m(4:5)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                open = [open;m];%将此相邻节点加放openlist中
            %>>剩下的情况就是它在Openlist中,检查由当前节点到相邻节点是否更好,
            %  如果更好则将当前节点设置为其父节点,并更新G值;否则不操作
            else
                %由当前节点到达相邻节点更好(targetInd是此相邻节点在open中的行号 此行的第3列是代价函数G值)
                if m(3) < open(targetInd,3)
                    %更好,则将此相邻节点的父节点设置为当前节点,否则不作处理
                    m(4:5)=[current(1,1),current(1,2)];%将当前节点作为其父节点
                    open(targetInd,:) = m;%将此相邻节点在Openlist中的数据更新
                end
            end
        end
        plot_map(map,obstacle,open,close);
    end
    %追溯路径
    path=GetPath(close,map.start);
    end
    

    2.8 GetPath.m

    当发现goal在open中时,Dijkstra.m里同时将goal放到close表里第一行,所以回溯路径从close表里第一行去找父节点。

    function path=GetPath(close,start)
    
        ind=1;
        path=[];
        while 1
            path=[path; close(ind,1:2)];
            if isequal(close(ind,1:2),start)   
                break;
            end
            for io=1:length(close(:,1))
                if isequal(close(io,1:2),close(ind,4:5))   %close(1,4:5)就是goal点的父节点,这里是去找哪一行close(io,1:2)会等于goal的父节点,找到以后在继续这样去找谁等于这个父节点的父节点,直到父节点为start.
                    ind=io;
                    break;
                end
            end
        end
    end

    2.9 main.m

    创建地图-生成障碍物-搜索最优路径-绘制结果

    % 该文件为以map.mat为地图文件,point.mat为起止位置文件,
    % 进行Dijkstra算法路径规划的主程序
    clc
    clear all
    close all;
    disp('Dijkstra Path Planing start!!')
    load map.mat                    % 加载地图
    load point.mat                  % 加载起止位置点
    [map.XMAX,map.YMAX] = size(MAP); %%代表我们要画一个地图的长和宽
    map.start = node(1:2);  %起始点 注意必须在地图范围内
    map.goal = node(3:4);  %目标点 注意必须在地图范围内
    obstacle = GetObstacle(map,MAP);% 获取边界数据和障碍物坐标
    clear MAP node                  % 后续程序不再使用这两个变量
    %obstacle = [obstacle;4,1; 4,2; 4,3; 4,4; 3,4 ;2,4;];%全封死的情况,是没有路的
    
    % 画出地图和起止点
    figure(1)
    if length(obstacle)>=1
        plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
        % plot(obstacle(:,1),obstacle(:,2),'om');hold on;
    end
    pause(1);
    h=msgbox('Please confirm the map information and click the buttion "confirm".');
    uiwait(h,20);% 5s后关闭消息框
    if ishandle(h) == 1
        delete(h);
    end
    close 1
    figure(1)
    axis([1 map.XMAX+1 1 map.YMAX+1])
    set(gca,'YTick',0:1:map.YMAX);
    set(gca,'XTick',0:1:map.XMAX);
    grid on;hold on;
    % 绘制边界和障碍点
    plot(obstacle(:,1)+.5,obstacle(:,2)+.5,'rx');hold on;
    % 绘制起始点
    plot(map.start(1)+.5,map.start(2)+.5,'bo');hold on;
    % 绘制终止点
    plot(map.goal(1)+.5,map.goal(2)+.5,'gd');hold on;
    text(map.goal(1)+1,map.goal(2)+.5,'Target');
    % plot(map.start(1),map.start(2),'*r');hold on;
    % plot(map.goal(1),map.goal(2),'*b');hold on;
    
    % 采用Dijkstra算法进行路径规划
    path = Dijkstra(obstacle,map)% A*算法
    
    %画出路径
    %
    if length(path)>=1
        plot(path(:,1)+0.5,path(:,2)+0.5,'-m','LineWidth',5);hold on;
    end
    %}
    grid on;

    2.10 plot_map.m

    绘制结果,绿色为open节点,红色为closed节点

    function  plot_map( map,obstacle,open,close )
    
    % %画出障碍点、起始点、终点
    %绘制网格
    % for i = 1:map.XMAX+3
    %    line([-0.5,map.XMAX+1.5],[i-1.5,i-1.5]);
    % end
    % 
    % for j = 1:map.YMAX+3
    %    line([j-1.5,j-1.5],[-0.5,map.YMAX+1.5]);
    % end
    pause(0.1);
    title('黑色为障碍点和边界点,红色为close节点,绿色为open节点,连线为path');
    %绘制节点
    plot(close(:,1)+0.5,close(:,2)+0.5,'sr','MarkerFaceColor','r');
    hold on;
    %pause(0.1);
    plot(open(:,1)+0.5,open(:,2)+0.5,'sg','MarkerFaceColor','g');
    hold on;
    %pause(0.1);
    end
    

    3 运行结果

    Dijkstra扩展了很多closed节点,但是这些节点意义并不大,所以计算量大,耗时久。需要采用启发式的搜索算法A star,见我的另一篇博客会介绍。

    先运行CreateMap.m生成地图,然后运行main.m运行算法。

    运行视频见B站:https://www.bilibili.com/video/BV1W64y1d7iK/

    完整代码:https://download.csdn.net/download/weixin_39199083/18885402?spm=1001.2014.3001.5503

    展开全文
  • 接前述系列笔记,本文引用了【机器人路径规划】概率路线图(PRM)方法,理论和源代码在文章中均有详细介绍。
  • 近日,由于对无人驾驶课程的学习,为了学习作业...给定材料中包含的MATLAB代码,可以生成50X50的地图,x表示障碍物,起点和终点均已给定,用A *算法实现路径规划。提交PDF文档,包括A *算法MATLAB代码截图、MATLAB运...
  • 关注:决策智能与机器学习,深耕AI脱水干货作者 | 搬砖的旺财,地平线机器人算法工程师来源|知乎,https://zhuanlan.zhihu.com/p/79712897报道 |...
  • Dijkstra算法详解,另外还包含了自动驾驶学习资料的获取: 涵盖感知,规划和控制,ADAS,传感器; 1. apollo相关的技术教程和文档; 2.adas(高级辅助驾驶)算法设计(例如AEB,ACC,LKA等) 3.自动驾驶鼻祖mobileye的...
  • 自动驾驶路径规划,常用路径规划算法,包括curve based, graph search based
  • 自动驾驶路径规划DWA算法原理解析

    千次阅读 多人点赞 2019-09-25 22:50:36
    这篇文章详细介绍一下最新的ROS给出的DWA算法的结构,以及各个重要的cost function的含义,帮助你们理解DWA算法的构成。
  • Path Planning - Search algorithms ...成本函数以及如何将人类洞察力(比如“右转比左转更容易”)纳入我们的规划算法 寻找最佳解决方案与寻找足够好的解决方案相关的最优性和权衡 在线算法与离线算法, ...
  • 无人驾驶决策及路径规划算法入门

    千次阅读 2019-03-12 11:01:18
    将围绕无人驾驶中的决策及路径规划展开。传统路径规划可分为基于搜索以及基于采样两类。常见搜索算法包括Dijkstra算法、A算法等。...决策及路径规划算法的基础知识 基于搜索的路径规划算法的介绍 基...
  • 点击上方“AI算法修炼营”,选择加星标或“置顶”标题以下,全是干货文章来源:百度、焉知自动驾驶本文整合了网络上关于百度ApolloLattice Planner算法的详细介绍和相关Q&a...
  • Path Planning - Highway Driving project Github: ...Overview 在自动驾驶流程管道中, 我们按顺序处理以下模块: 感知: 负责使用传感器探测对象 融合: 负责使用传感器融合提供被检测...
  • 路径规划算法总结.zip

    2021-03-30 22:36:41
    路径规划算法总结与对比,另外还包含了自动驾驶学习资料的获取: 涵盖感知,规划和控制,ADAS,传感器; 1. apollo相关的技术教程和文档; 2.adas(高级辅助驾驶)算法设计(例如AEB,ACC,LKA等) 3.自动驾驶鼻祖...
  • 路径规划算法

    2021-05-10 16:29:08
    二、基于搜索的路径规划算法 1、Dijkstra算法 2、启发式搜索算法(A* ) 3、双向A *算法 4、任意时刻启发式搜索算法(ARA *) 5、实时学习A * 搜索算法(LRTA *) 6、实时适应性A *搜索算法(RTAA *) 7、动态...
  • 自动驾驶决策算法工程师 自动驾驶的决策是指给将知模块传递的信息,如何决策汽车的行为达到驾驶的目标。例如,汽车加速、减速、左转、右转、换道、超车都是决策模块的输出。决策需要考虑到汽车的安全性和舒适性,...
  • 自动泊车中的路径规划算法

    万次阅读 多人点赞 2019-01-06 15:37:49
    首先讲一下自动泊车和自主泊车的区别,自主泊车是自动...而本文仅仅是对自动泊车中的路径规划算法进行介绍。 轨迹规划  轨迹规划的输入为车的位置、车位的位置和周围障碍物的位置,输出为泊车轨迹和泊车动作指令...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 4,608
精华内容 1,843
关键字:

自动驾驶路径规划算法