【路径规划】基于A星算法实现栅格地图全路径规划matlab源码

 1 模型

移动机器人路径规划一直是一个比较热门的话题,A星算法以及其扩展性算法被广范地应用于求解移动机器人的最优路径.该文在研究机器人路径规划算法中,详细阐述了传统A星算法的基本原理,并通过栅格法分割了机器人路径规划区域,利用MATLAB仿真平台生成了机器人二维路径仿真地图对其进行仿真实验,并对结果进行分析和研究,为今后进一步的研究提供经验.

1.1 A星算法

1.2 曼哈顿距离

1.3 具体原理

1.4 流程图

​

2 部分代码

clc
clear
close all
shange_num=20;%栅格数量
G=zeros(shange_num,shange_num); % G 地形图为01矩阵，如果为1表示障碍物
zhangai_point=[44:48 64:68 151:155 171:175 241:245 261:265];%障碍的数字
for i=1:length(zhangai_point)
   x_i=ceil(zhangai_point(i)/shange_num);
   y_i=mod(zhangai_point(i)-1,shange_num)+1;
   G(x_i,y_i)=1;
end
figure(1)%画障碍图
hold on
for i=1:shange_num
   for j=1:shange_num
       if G(i,j)==1
           y=[i-1,i-1,i,i];
           x=[j-1,j,j,j-1];
           h=fill(x,y,'k');
           set(h,'facealpha',0.5)
       end
       s=(num2str((i-1)*shange_num+j));
       %s=(['(',num2str(i),',',num2str(j),')']);
       text(j-0.7,i-0.5,s,'fontsize',5)
   end
end
axis([0 shange_num 0 shange_num])%限制图的边界
for i=1:shange_num
   plot([0 shange_num],[i-1 i-1],'k-');
   plot([i i],[0 shange_num],'k-');%画网格线
end
S=1 ;                            %最短路径的起始点
E=shange_num*shange_num;                        %最短路径的目的点
Sx=mod(S-1,shange_num)+0.5;
Sy=ceil(S/shange_num)-0.5;
Ex=mod(E-1,shange_num)+0.5;
Ey=ceil(E/shange_num)-0.5;
plot(Sx,Sy, 'p','markersize', 10,'markerfacecolor','r','MarkerEdgeColor', 'm')

           route_lin=starA(G,S,E);
           route=[route route_lin];
           S=E;
       end
   end
end        
for i=length(route):-1:2
   if route(i)==route(i-1)
       route(i)=[];
   end
end
xian_size=3;%线宽
information_ta=zeros(1,shange_num*shange_num);
information_ta(1)=1;
for i=2:length(route)
   if information_ta(route(i))==1;
       ii=ceil(route(i)/shange_num);
       jj=mod(route(i)-1,shange_num)+1;
       y=[ii-1,ii-1,ii,ii];
       x=[jj-1,jj,jj,jj-1];
       h=fill(x,y,'g');
       set(h,'facealpha',0.5)
   end
   Q1=[mod(route(i-1)-1,shange_num)+0.5,ceil(route(i-1)/shange_num)-0.5];
   Q2=[mod(route(i)-1,shange_num)+0.5,ceil(route(i)/shange_num)-0.5];
   plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',xian_size)
   information_ta(route(i))=1;
end     
figure(2)
hold on
for i=1:shange_num
   for j=1:shange_num
       if G(i,j)==1
           y=[i-1,i-1,i,i];
           x=[j-1,j,j,j-1];
           h=fill(x,y,'k');
           set(h,'facealpha',0.5)
       end
       s=(num2str((i-1)*shange_num+j));
       %s=(['(',num2str(i),',',num2str(j),')']);


       text(j-0.7,i-0.5,s,'fontsize',5)
   end
end
axis([0 shange_num 0 shange_num])%限制图的边界
for i=1:shange_num
   plot([0 shange_num],[i-1 i-1],'k-');
   plot([i i],[0 shange_num],'k-');%画网格线
end
S=1 ;                            %最短路径的起始点
Sx=mod(S-1,shange_num)+0.5;
Sy=ceil(S/shange_num)-0.5;
plot(Sx,Sy, 'p','markersize', 10,'markerfacecolor','r','MarkerEdgeColor', 'm')
set(gca,'YDir','reverse');
hd1=line('Color','r','Marker','p', 'MarkerSize',10);%用来设置运动点颜色/形状/大小
information_ta=zeros(1,shange_num*shange_num);
information_ta(1)=1;
for i=2:length(route)
   if information_ta(route(i))==1;
       ii=ceil(route(i)/shange_num);
       jj=mod(route(i)-1,shange_num)+1;
       y=[ii-1,ii-1,ii,ii];
       x=[jj-1,jj,jj,jj-1];
       h=fill(x,y,'g');
       set(h,'facealpha',0.5)
   end
   Q1=[mod(route(i-1)-1,shange_num)+0.5,ceil(route(i-1)/shange_num)-0.5];
   Q2=[mod(route(i)-1,shange_num)+0.5,ceil(route(i)/shange_num)-0.5];
   plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',xian_size)
   set(hd1,'xdata',mod(route(i)-1,shange_num)+0.5,'ydata',ceil(route(i)/shange_num)-0.5);  %用来给运动点设置运动位置的
   drawnow;                                                                %命令运动点动起来的
   pause(0.05);                                                         %控制运动速度的
   information_ta(route(i))=1;
end

3 仿真结果

4 参考文献

[1]周宇杭等. "基于A星算法的移动机器人路径规划应用研究." 电脑知识与技术 v.16.13(2020):7-9+16.