附MATLAB代码：
1.主程序
%%基于A*算法的全局路径规划
clc
clear
%%
%建立地图和设置目标点
%设置地图的长和宽
map_XYMAX=20;
map=zeros(map_XYMAX,map_XYMAX);
%绘制地图
for i=1:map_XYMAX+1
line([-0.5,map_XYMAX-0.5],[i-1.5,i-1.5]);
line([i-1.5,i-1.5],[-0.5,map_XYMAX-0.5]);
hold on
end
%设置起点和终点
map_start=[0,0];
map_goal=[19,19];
%绘制起点和终点
plot(map_start(1),map_start(2),'or');
hold on
plot(map_goal(1),map_goal(2),'ob');
hold on
%%
%随机生成障碍物
obstacle=[];
obstacle_number=150;
ob=floor(rand([obstacle_number,2])*map_XYMAX);
removeInd=[];
for io=1:length(ob(:,1))
if(isequal(ob(io,:),map_start) || isequal(ob(io,:),map_goal))
removeInd=[removeInd;io];
end
end
ob(removeInd,:)=[];
obstacle=[obstacle;ob];
for z=1:map_XYMAX+2
obstacle=[obstacle;-1 z-2];
obstacle=[obstacle;z-2 -1];
obstacle=[obstacle;20 z-2];
obstacle=[obstacle;z-2 20];
end
%将障碍物填充颜色
for i = 1:length(obstacle(:,1))
x = obstacle(i,1);
y = obstacle(i,2);
X = [x-0.5,x+0.5,x+0.5,x-0.5];
Y = [y-0.5,y-0.5,y+0.5,y+0.5];
fill(X,Y,'k');
hold on;
end
%%
%路径规划
%创建存储路径
path=[]; %此列表是从map_start向map_goal方向搜寻的路径列表
path_1=[]; %此列表是从map_goal向map_start方向搜寻的路径列表
%创建open列表
open=[]; %此列表是从map_start向map_goal方向搜寻open列表
open_1=[]; %此列表是从map_goal向map_start方向搜寻open列表
%创建close列表
close=[]; %此列表是从map_start向map_goal方向搜寻close列表
close_1=[]; %此列表是从map_goal向map_start方向搜寻close列表
%用于判断while循环是否结束
findFlag=false;
%计算起始点和目标点之间的距离
dis=10*abs(map_start(1)-map_goal(1))+10*abs(map_start(2)-map_goal(2));
%将起始点添加到open列表中去
open=[map_start(1),map_start(2),0+dis,0,0,0];
%将目标点添加到open_1列表中去
open_1=[map_goal(1),map_goal(2),0+dis,0,0,0];
%进入循环
while ~findFlag
%%从map_start向map_goal搜寻路径
%判断open列表是否为空
if isempty(open(:,1))
disp('No path!');
return;
end
%判断目标点是否在open列表中
for i=1:length(open(:,1))
if isequal(map_goal(1:2),open(i,1:2))
disp('Find Goal!');
close = [open(i,:);close];
findFlag=true;
break;
end
end
%将F值进行排序
[Y,I]=sort(open(:,3));
open=open(I,:);
%将此时open列表中的第一行移除，添加到close列表中
close=[open(1,:);close];
current=open(1,:);
open(1,:)=[];
%计算更新后的8个节点的F值
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];
for j=1:length(next(:,1))
%下一个节点参数
m=[current(1,1)+next(j,1),current(1,2)+next(j,2),0,0,0,0];
%从当前节点到下一个节点的G值
m(4)=current(4)+next(j,3);
%从当前节点到下一个节点的H值
H=10*abs(m(1)-map_goal(1))+10*abs(m(2)-map_goal(2));
%从当前节点到下一个节点的F值
m(3)=m(4)+H;
%判断当前节点是否为障碍物,如果是障碍物则忽略
if isObstacle(m,obstacle)
continue;
end
%判断下一节点所在列表
[flag,targetInd]=FindList(m,open,close);
%case_1:在close列表中
if flag==1
continue;
%case_2:不在列表中
elseif flag==2
m(5:6)=[current(1,1),current(1,2)];%将当前节点作为其父节点
open=[open;m];
%case_3:在open列表中
elseif m(3)<open(targetInd,3)
%将当前节点作为其父节点
m(5:6)=[current(1,1),current(1,2)];
open(targetInd,:)=m;
end
end
pause(0.01);
%将open列表和close列表填充颜色
for i=1:length(close(:,1))
x=close(i,1);
y=close(i,2);
X=[x-0.5,x+0.5,x+0.5,x-0.5];
Y=[y-0.5,y-0.5,y+0.5,y+0.5];
fill(X,Y,'r');
end
%%从map_goal向map_start方向搜寻路径
%判断open——1列表是否为空
if isempty(open_1(:,1))
disp('No path!');
return;
end
%判断起始点是否在open_1列表中
for i=1:length(open_1(:,1))
if isequal(map_start(1:2),open_1(i,1:2))
disp('Find Goal!');
close_1=[open_1(i,:);close_1];
findFlag=true;
break;
end
end
%将F值进行排序
[Y,I]=sort(open_1(:,3));
open_1=open_1(I,:);
%将此时open_1列表中的第一行移除，添加到close_1列表中
close_1=[open_1(1,:);close_1];
current_1=open_1(1,:);
open_1(1,:)=[];
%计算更新后的8个节点的F值
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];
for j=1:length(next(:,1))
%下一个节点参数
n=[current_1(1,1)+next(j,1),current_1(1,2)+next(j,2),0,0,0,0];
%从当前节点到下一个节点的G值
n(4)=current_1(4)+next(j,3);
%从当前节点到下一个节点的H1值
H1=10*abs(n(1)-map_start(1))+10*abs(n(2)-map_start(2));
%从当前节点到下一个节点的F值
n(3)=n(4)+H1;
%判断当前节点是否为障碍物,如果是障碍物则忽略
if isObstacle(n,obstacle)
continue;
end
%判断下一节点所在列表
[flag,targetInd]=FindList(n,open_1,close_1);
%case_1:在close_1列表中
if flag==1
continue;
%case_2:不在列表中
elseif flag==2
n(5:6)=[current_1(1,1),current_1(1,2)];%将当前节点作为其父节点
open_1=[open_1;n];
%case_3:在open_1列表中
elseif n(3)<open_1(targetInd,3)
%将当前节点作为其父节点
n(5:6)=[current_1(1,1),current_1(1,2)];
open_1(targetInd,:)=n;
end
end
pause(0.01);
%将open_1列表和close_1列表填充颜色
for i=1:length(close_1(:,1))
x=close_1(i,1);
y=close_1(i,2);
X=[x-0.5,x+0.5,x+0.5,x-0.5];
Y=[y-0.5,y-0.5,y+0.5,y+0.5];
fill(X,Y,'y');
end
end
%%
% 绘制path路线
ind=1;
while true
path=[path; close(ind,1:2)];
if isequal(close(ind,1:2),map_start)
break;
end
for i3=1:length(close(:,1))
if isequal(close(i3,1:2),close(ind,5:6))
ind=i3;
break;
end
end
end
if length(path)>=1
plot(path(:,1),path(:,2),'-c','LineWidth',5);
end
disp("The line of cyan is from start to goal.")
% 绘制path_1路线
ind=1;
while true
path_1=[path_1; close_1(ind,1:2)];
if isequal(close_1(ind,1:2),map_goal)
break;
end
for i3=1:length(close_1(:,1))
if isequal(close_1(i3,1:2),close_1(ind,5:6))
ind=i3;
break;
end
end
end
if length(path_1)>=1
plot(path_1(:,1),path_1(:,2),'-g','LineWidth',5);
end
disp("The line of green is from goal to start.")
%%
%判断两条路径用时的长短
Flag=false;
while ~Flag
for k1=1:length(close(:,1))
if isequal(map_goal(1:2),close(k1,1:2))
disp("The line of cyan used less time.")
Flag=true;
break
else
continue
end
end
for k2=1:length(close_1(:,1))
if isequal(map_start(1:2),close_1(k2,1:2))
disp("The line of green used less time.")
Flag=true;
break
else
continue
end
end
end
axis([-2 map_XYMAX+1 -2 map_XYMAX+1])
2、FindList.m
function [flag,targetInd]=FindList(m,open,close)
%如果open为空，则一定不在open列表中
if isempty(open)
flag = 2;
targetInd = [];
%open不为空时，需要检查是否在open列表中
else
for io = 1:length(open(:,1))
%在Open列表中
if isequal( m(1:2) , open(io,1:2) )
flag = 3;
targetInd = io;
return;
%不在Open列表中
else
flag = 2;
targetInd = [];
end
end
end
%在Close列表中
for ic = 1:length(close(:,1))
if isequal( m(1:2) , close(ic,1:2) ) %
flag = 1;
targetInd = ic;
return;%在Closelist中直接return
end
end
end
3、isObstacle.m
function flag = isObstacle( m,obstacle )
%判断节点m是否为障碍点，如果是就返为true，不是就返回false
for io=1:length(obstacle(:,1))
if isequal(obstacle(io,:),m(1:2))
flag=true;
return;
end
end
flag=false;
end
关注公众号回复【A*算法】可获取源代码
扫一扫
1182
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
