【路径规划】基于动态衡量式A星算法及拐角优化算法实现栅格地图路径规划matlab源码

1 模型

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

1.1 A星算法

1.2 曼哈顿距离

1.3 具体原理

1.4 流程图

2 部分代码

%matlab初始化

clc;             %清除命令窗口的内容
clear all;       %清除工作空间的所有变量，函数，和MEX文件
close all;       %关闭所有的figure窗口

% [y,Fs] = audioread('001.wav'); sound(y,Fs); % 播放名为001的音乐，注意该文件需要跟matlab文件位于同一文件夹下，


%方格数、障碍物比例以及h（n）权重系数的设定
n = 50;   % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4;  % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=2;       %动态衡量启发式A星算法中的h（n）权重系数
%方格以及障碍物的创建
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵

% 路径规划中用到的一些矩阵的初始化
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {'R','L','D','U'};  %移动方向

%初始化一些进行路径的修正需要用到的变量
Parent_node=0; %Parent_node初始化，否则会报错
Expected_note=0;%Expected_note初始化，否则会报错
untext_ii=0;  %未经过检验的新的ii
amend_count=0;% 记录修正的次数


% 这个函数用来随机生成环境，障碍物，起点，终点
axishandle = createFigure(field,costchart,startposind,goalposind);    %将随机生成的方格及障碍物的数据生成图像

%%


   if ~isinf(costs(jj))   % 判断该点（方格）处没有障碍物
       
     % 判断一下该点是否 已经存在于setOpen 矩阵或者setClosed 矩阵中
     % 如果我们要处理的拓展点既不在setOpen 矩阵，也不在setClosed 矩阵中
     if ~max([setClosed; setOpen] == posinds(jj))
       fieldpointers(posinds(jj)) = movementdirections(jj);
       costchart(posinds(jj)) = costs(jj);
       setOpen = [setOpen; posinds(jj)];

       setOpenCosts = [setOpenCosts; costs(jj)];
       setOpenHeuristics = [setOpenHeuristics; heuristics(jj)];
       
     % 如果我们要处理的拓展点已经在setOpen 矩阵中
     elseif max(setOpen == posinds(jj))
       I = find(setOpen == posinds(jj));
       % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
       if setOpenCosts(I) > costs(jj)
         costchart(setOpen(I)) = costs(jj);
         setOpenCosts(I) = costs(jj);
         setOpenHeuristics(I) = heuristics(jj);
         fieldpointers(setOpen(I)) = movementdirections(jj);
       end
       
       % 如果我们要处理的拓展点已经在setClosed 矩阵中
     else
       I = find(setClosed == posinds(jj));
       % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
       if setClosedCosts(I) > costs(jj)
         costchart(setClosed(I)) = costs(jj);
         setClosedCosts(I) = costs(jj);
         fieldpointers(setClosed(I)) = movementdirections(jj);
       end
     end
   end
 end
 
%% 
 
 if isempty(setOpen) break; end
 set(axishandle,'CData',[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
 set(gca,'CLim',[0 1.1*max(costchart(find(costchart < Inf)))]);
 drawnow; 
end

%%

%调用findWayBack函数进行路径回溯，并绘制出路径曲线
if max(ismember(setOpen,goalposind))
 disp('Solution found!');
 p = findWayBack(goalposind,fieldpointers); % 调用findWayBack函数进行路径回溯，将回溯结果放于矩阵P中
 plot(p(:,2)+0.5,p(:,1)+0.5,'Color',0.2*ones(3,1),'LineWidth',4);  %用 plot函数绘制路径曲线
 drawnow;
 drawnow;
 clear sound

end

%%

3 仿真结果

4 参考文献

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