Matlab航跡規(guī)劃仿真—A*算法

1. 初始化參數(shù)

主要參數(shù):

地圖大小

起始點(diǎn)和目標(biāo)點(diǎn)坐標(biāo)

clc
clear all
m = 30;n = 30;
Spoint = [3 3];  %起始點(diǎn)坐標(biāo)
Epoint = [29 22]; %目標(biāo)點(diǎn)坐標(biāo)

2. 構(gòu)建地圖

-inf表示不可到達(dá)的障礙物點(diǎn)

%%構(gòu)建地圖
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
%%障礙
for j=2:10
  Matrix(5,j)=-inf;
for j=2:15
   Matrix(24,j)=-inf;
for j=9:24
%for j=6:24
   Matrix(10,j)=-inf;
for j=20:31
  Matrix(15,j)=-inf;
for j=5:20
  Matrix(20,j)=-inf;
for j=18:27
  Matrix(28,j)=-inf;
for i=2:6 
  Matrix(i,18)=-inf;
for i=17:20
   Matrix(i,5)=-inf;
for i=23:25
   Matrix(i,20)=-inf;
for i=13:17
  Matrix(i,13)=-inf;
end
end
end
end
end
end
end
end
end
end
%end


% 顯示地圖
%subplot(2,2,1);
h1 = plot(Spoint(1),Spoint(2),'gO');
hold on
h2 = plot(Epoint(1),Epoint(2),'rO');

3. A*算法搜索路徑

%%尋路
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;%歐幾里德距離啟發(fā)函數(shù)
        H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比較復(fù)雜的對(duì)角線啟發(fā)函數(shù)
        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)));%比較簡(jiǎn)單的對(duì)角線函數(shù)
        
        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; %歐幾里德距離啟發(fā)函數(shù)
        H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比較復(fù)雜的對(duì)角線啟發(fā)函數(shù)
        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)));%比較簡(jiǎn)單的對(duì)角線函數(shù)
        
        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],'起始點(diǎn)','目標(biāo)點(diǎn)','障礙物','航跡點(diǎn)');


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

4. 路徑優(yōu)化

%將得到的折現(xiàn)曲線擬合成光滑的曲線
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]);

5. 效果圖

A*路徑

優(yōu)化后路徑

6. 下載鏈接

直接復(fù)制到matlab即可使用，或者也可以點(diǎn)擊下載。

審核編輯：湯梓紅