写这个系列的第一篇文章是自动驾驶路径规划算法学习(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