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

自动驾驶路径规划算法学习-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