Matlab航迹规划仿真——A*算法_航迹起始算法 matlab_致守的博客-CSDN博客
matlab2016及以上可以运行
astar.m
function [] = astar(Spoint,Epoint,Matrix,m,n,h1,h2)
%%寻路
Matrix(Spoint(1),Spoint(2))=0;
Matrix(Epoint(1),Epoint(2))=inf;
G=Matrix;
F=Matrix;
openlist=Matrix;
closelist=Matrix;
parentx=Matrix;
parenty=Matrix;
openlist(Spoint(1),Spoint(2)) =0;
%closelist(Epoint(1),Epoint(2))=inf;
for i = 1:n+2
for j = 1:m+2
k = Matrix(i,j);
if(k == -inf)
%subplot(2,2,1);
h3 = plot(i,j,'k.');
% elseif(k == inf) % show green feasible point
% %subplot(2,2,1);
% plot(i,j,'gh');
% else
% %subplot(2,2,1);
% plot(i,j,'gh');
end
hold on
end
end
axis([0 m+3 0 n+3]);
%subplot(2,2,1);
plot(Epoint(1),Epoint(2),'b+');
%subplot(2,2,1);
plot(Spoint(1),Spoint(2),'b+');
while(1)
num=inf;
for p=1:m+2
for q=1:n+2
if(openlist(p,q)==0&&closelist(p,q)~=1)
Outpoint=[p,q];
if(F(p,q)>=0&&num>F(p,q))
num=F(p,q);
Nextpoint=[p,q];
end
end
end
end
closelist(Nextpoint(1),Nextpoint(2))=1;
for i = 1:3
for j = 1:3
k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1)
continue;
elseif (k == -inf)
G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1;
elseif (k == inf)
distance=((i-2)^2+(j-2)^2)^0.5;
G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance;
openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0;
% H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数
H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal);
% H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;
parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
else distance=((i-2)^2+(j-2)^2)^0.5;
if(k>(distance+G(Nextpoint(1),Nextpoint(2))))
k=distance+G(Nextpoint(1),Nextpoint(2));
% H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5; %欧几里德距离启发函数
H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);
% H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;
parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
end
end
if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
parentx(Epoint(1),Epoint(2))=Nextpoint(1);
parenty(Epoint(1),Epoint(2))=Nextpoint(2);
break;
end
end
if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
parentx(Epoint(1),Epoint(2))=Nextpoint(1);
parenty(Epoint(1),Epoint(2))=Nextpoint(2);
break;
end
end
if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
parentx(Epoint(1),Epoint(2))=Nextpoint(1);
parenty(Epoint(1),Epoint(2))=Nextpoint(2);
break;
end
end
P=[];
s=1;
while(1)
if(num==inf)
break;
end
%subplot(2,2,1);
h4 = plot(Epoint(1),Epoint(2),'b+');
P(s,:)=Epoint;
s=s+1;
% pause(1);
xx=Epoint(1);
Epoint(1)=parentx(Epoint(1),Epoint(2));
Epoint(2)=parenty(xx,Epoint(2));
if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))
%subplot(2,2,1);
plot(Epoint(1),Epoint(2),'b+');
P(s,:)=Epoint;
break;
end
end
P(s+1,:)=Spoint;
legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');
count=0;
for i=2:12
for j=2:12
if(G(i,j)~=inf&&G(i,j)~=-inf)
count=count+1;
end
end
end
count
end
main.m
clc
clear all
m = 30;n = 30;
%%构建地图
for i = 1:m+2
if i == 1
for j = 1:n+2
Matrix(i,j) = -inf;
end
elseif i == m+2
for j = 1:n+2
Matrix(i,j) = -inf;
end
else
for j = 1:n+2
if ((j == 1)|(j == n+2))
Matrix(i,j) = -inf;
else
Matrix(i,j) = inf;
end
end
end
end
%%障碍1
for j=15:18
Matrix(5,j)=-inf;end
for j=15:18
Matrix(8,j)=-inf;end
for i=5:8
Matrix(i,18)=-inf;end
for i=5:8
Matrix(i,15)=-inf;end
%%障碍2
for j=17:20
Matrix(12,j)=-inf;end
for j=17:20
Matrix(15,j)=-inf;end
for i=12:15
Matrix(i,17)=-inf;end
for i=12:15
Matrix(i,20)=-inf;end
%%障碍3
for j=15:18
Matrix(20,j)=-inf;end
for j=15:18
Matrix(23,j)=-inf;end
for i=20:23
Matrix(i,18)=-inf;end
for i=20:23
Matrix(i,15)=-inf;end
%%%%%%%%第一条航线
Spoint = [3 3]; %起始点坐标
Epoint = [7 30]; %目标点坐标
% 显示地图
%subplot(2,2,1);
h1 = plot(Spoint(1),Spoint(2),'rO');
hold on
h2 = plot(Epoint(1),Epoint(2),'rO');
astar(Spoint,Epoint,Matrix,m,n,h1,h2);
%%%%%%%%第二条航线
Spoint2 = [8 4]; %起始点坐标
Epoint2 = [15 28]; %目标点坐标
% 显示地图
h12 = plot(Spoint2(1),Spoint2(2),'gO');
hold on
h22 = plot(Epoint2(1),Epoint2(2),'gO');
astar(Spoint2,Epoint2,Matrix,m,n,h12,h22);
%%%%%%%%第三条航线
Spoint3 = [14 3]; %起始点坐标
Epoint3 = [19 29]; %目标点坐标
% 显示地图
h13 = plot(Spoint3(1),Spoint3(2),'yO');
hold on
h23 = plot(Epoint3(1),Epoint3(2),'yO');
astar(Spoint3,Epoint3,Matrix,m,n,h13,h23);
%%%%%%%%第四条航线
Spoint4 = [17 2]; %起始点坐标
Epoint4 = [25 30]; %目标点坐标
% 显示地图
h14 = plot(Spoint4(1),Spoint4(2),'bO');
hold on
h24 = plot(Epoint4(1),Epoint4(2),'bO');
astar(Spoint4,Epoint4,Matrix,m,n,h14,h24);
% %将得到的折现曲线拟合成光滑的曲线
% P=P';
% a=[];
% b=[];
% a=P(1,:);
% b=P(2,:);
% figure
% %subplot(2,2,3);
% plot(a,b);
% axis([0,n+3,0,n+3]);
%
% values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);
% figure
% %subplot(2,2,4);
% plot(values(1,:),values(2,:),'r');
% axis([0,m+3,0,m+3]);