✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
1 算法原理
A*算法结合Floyd算法和动态窗口法可以实现机器人在栅格地图上的路径规划。下面是一个基本的步骤：
- 栅格地图表示：将待规划的区域划分为一组栅格，每个栅格代表一个离散的位置，其中包括可通过的栅格和障碍物。
- Floyd算法处理地：使用Floyd算法计算任意两个栅格之间的最短路径距离，并生成对应的路径矩阵或邻矩阵。
- 初始化启发式值和开放列表：为每个栅格设置启发式值（如曼哈顿距离或欧几里得距离），并初始化开放列表来记录待访问的栅格。
- A*搜索过程：
- 选择起始栅格作为当前节点。
- 对于当前节点，计算其邻居栅格的估计代价（包括实际距离和启发式值）。
- 更新邻居栅格的代价信息和路径信息。
- 将更新后的邻居栅格加入开放列表。
- 从开放列表中选择下一个节点作为当前节点，重复执行以上步骤，直到达到终点或无法找到路径。
- 动态窗口法调整路径：将A*算法得到的初始路径进行平滑和优化，通过动态窗口法消除路径中的不必要转弯或避免碰撞等问题，以生成更加平滑和可行的最终路径。
- 输出最终路径：经过以上步骤，可以得到机器人在栅格地图上的规划路径，即一系列连续的栅格点。
需要注意的是，A*算法是一种启发式搜索方法，用于在离散状态空间中找到最优路径。Floyd算法用于计算任意两个节点之间的最短路径，而动态窗口法用于路径平滑和优化。将它们结合在一起可以应对不同类型的路径规划问题，并使规划的路径更加有效和可行。
2 算法流程
下面是A*算法结合Floyd算法和动态窗口法的基本流程：
- 栅格地图表示：将待规划的区域划分为一组栅格，每个栅格代表一个离散的位置，包括可通过的栅格、起始点和目标点等。
- 初始化数据结构和参数：
- 创建栅格地图的邻接矩阵，用于存储栅格之间的连接关系。
- 初始化起始点和目标点，并设置启发式值（如曼哈顿距离）和开放列表。
- 使用Floyd算法计算任意两个栅格之间的最短路径距离：
- 基于栅格地图的邻接矩阵，使用Floyd算法计算所有栅格之间的最短路径距离，并生成对应的路径矩阵。
- A*搜索过程：
- 如果邻居节点不可通过或者已在已访问集合中，则忽略。
- 更新邻居节点的代价信息和路径信息，计算估计总代价。
- 如果邻居节点不在开放列表中，将其加入开放列表。
- 如果邻居节点在开放列表中，并且新计算的总代价更小，则更新其代价信息和路径信息。
- 选择起始点作为当前节点。
- 初始化当前节点的代价和路径信息。
- 将当前节点加入已访问集合。
- 对于当前节点的所有邻居节点：
- 从开放列表中选择具有最小总代价的节点作为下一个当前节点，重复执行以上步骤。
- 如果达到目标点或开放列表为空（无可行解），则搜索结束。
- 动态窗口法调整路径：
- 根据A*算法得到的初始路径，根据窗口大小设定，将路径进行平滑和优化。
- 如果路径中存在转弯点或避障点，通过调整路径段来消除不必要的转弯或避免碰撞。
- 输出最终路径：
- 经过以上步骤，得到调整后的规划路径，即一系列连续的栅格点作为最终路径。
⛄ 部分代码
function a = DWA(Obs_Closed,Obs_d_j,Area_MAX,Goal,Line_path,path_node,Start0,s_du,angle_node,Kinematic,evalParam)
figure
num_obc=size(Obs_Closed,1); % 计算障碍物的数量
num_path=size(path_node,1);
xTarget=path_node(num_path,1);
yTarget=path_node(num_path,2);
%
% num_odL=size(Obst_d_d_line,1);
% Obst_d_line=[];
xm=path_node(1,1);
ym=path_node(1,2);
% 初始位置坐标
%angle_S=pi;
angle_node=sn_angle(path_node(1,1),path_node(1,2),path_node(2,1),path_node(2,2));
du_flog=1;
du_max=angle_node+pi/18;
du_min=angle_node-pi/18;
%zhuangjiao_node=angle_S-angle_node;
x=[xm ym s_du 0 0]';% 机器人的初期状态 x=[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
xt_yu=[xm ym];
%G_goal=path_node(num_path,:);% 目标点位置 [x(m),y(m)]
obstacle=Obs_Closed;
Obs_dong=Obs_d_j ;
obstacleR=0.8;% 静态障碍物 冲突判定用的障碍物半径
R_dong_obs=0.7; % 动态障碍物 冲突判定用的障碍物半径
global dt; % 全局变量
dt=0.1;% 时间[s]
% 机器人运动学模型
% [ 最高速度[m/s], 最高旋转速度[rad/s], 加速度[m/ss], 旋转加速度[rad/ss], 速度分辨率[m/s], 转速分辨率[rad/s] ]
%
% Kinematic=[2.0, toRadian(40.0), 0.5, toRadian(120.0), 0.01, toRadian(1)];
% %Kinematic=[1, toRadian(20.0), 0.3, toRadian(60), 0.01, toRadian(1)];
% % 评价函数参数 [heading,dist,velocity,predictDT][方位角偏差系数， 障碍物距离系数， 当前速度大小系数, 动态障碍物距离系数，预测是时间 ]
% evalParam=[0.2, 0.1, 0.3, 0.4, 3.0];%0.3 0.1 0.1 [0.05, 0.2, 0.1, 3.0] [0.2, 0.5, 0.3, 3.0]
MAX_X=Area_MAX(1,1);
MAX_Y=Area_MAX(1,2);
% 模拟区域范围 [xmin xmax ymin ymax]
% 模拟实验的结果
result.x=[];
goal=path_node(2,:);
tic;
% movcount=0;
% Main loop
for i=1:5000
dang_node=[x(1,1) x(2,1)];
dis_ng=distance(dang_node(1,1),dang_node(1,2),xTarget,yTarget);
dis_x_du=distance(xt_yu(1,1),xt_yu(1,2),goal(1,1),goal(1,2));
if num_path==2||dis_ng<0.5
Ggoal=[xTarget yTarget];
else
Ggoal=Target_node(dang_node,path_node,Obs_dong,xTarget,yTarget,goal,dis_x_du);
end
goal=Ggoal;
% obstacle=OBSTACLE(Obs_Closed,Obs_dong,path_node);
if (s_du>du_max||s_du<du_min)&&du_flog==1
[u,traj]=DynamicWindowApproach_du(x,Kinematic,goal,evalParam,obstacle,obstacleR,Obs_dong,R_dong_obs);
x=f(x,u);% 机器人移动到下一个时刻
result.x=[result.x; x'];
if angle_node>=(17/18)*pi
if x(3)>=du_min %x(3)==angle_node
du_flog=0;
end
elseif angle_node<=(-17/18)*pi
if x(3)<=du_max %x(3)==angle_node
du_flog=0;
end
else
if x(3)<=du_max&&x(3)>=du_min %x(3)==angle_node
du_flog=0;
end
end
else
[u,traj,xt_yu]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR,Obs_dong,R_dong_obs);
% u = [ 速度 转速 ] traj=[ 3s内的所有状态轨迹 ]
x=f(x,u);% 机器人移动到下一个时刻
result.x=[result.x; x'];
end
% 模拟结果的保存
% 是否到达目的地
%if norm(x(1:2)-G_goal')<0.2
if dis_ng<0.2
disp('Arrive Goal!!');break;
end
%====Animation====
hold off;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 画图
for i_obs=1:1:num_obc
x_obs=Obs_Closed(i_obs,1);
y_obs=Obs_Closed(i_obs,2);
fill([x_obs,x_obs+1,x_obs+1,x_obs],[y_obs,y_obs,y_obs+1,y_obs+1],'k');hold on;
end
plot( Line_path(:,1)+.5, Line_path(:,2)+.5,'b:','linewidth',1);
plot(Start0(1,1)+.5,Start0(1,2)+.5,'b^');
plot(Goal(1,1)+.5,Goal(1,2)+.5,'bo');
% dong_num=size(Obs_d_j,1);
% for i_d=1:1:dong_num
% x_do=Obs_d_j(i_d,1);
% y_do=Obs_d_j(i_d,2);
% fill([x_do,x_do+1,x_do+1,x_do],[y_do,y_do,y_do+1,y_do+1],[0.8 0.8 0.8]);
% end
% fill([7.2,7.8,7.8,7.2],[10.2,10.2,10.8,10.8],[0.8 0.8 0.8]);hold on;
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fill([7.2,7.8,7.8,7.2],[7.2,7.2,7.8,7.8],[0.8 0.8 0.8]);hold on;
ArrowLength=0.5;%
% 机器人
quiver(x(1)+0.5, x(2)+0.5, ArrowLength*cos(x(3)), ArrowLength*sin(x(3)),'ok');hold on;
% x=[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
plot(result.x(:,1)+0.5, result.x(:,2)+0.5,'-b');hold on;
plot(goal(1)+0.5,goal(2)+0.5,'*r');hold on;
% 探索轨迹
% % traj = [ （第一组5行 v w ）3s内的所有状态轨迹 31个点 1-5行31列；
% （第二组5行 v w ）3s内的所有状态轨迹 31个点 6-10行31列；
% 。。。。。。]
if ~isempty(traj)
for it=1:length(traj(:,1))/5 %
ind=1+(it-1)*5;
plot(traj(ind,:)+0.5,traj(ind+1,:)+0.5,'-g','linewidth',1.5);hold on;%%模拟轨迹
end
end
% axis(area);
% grid on;
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,'xtick',1:1:MAX_X+1,'ytick',1:1:MAX_Y+1,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
grid on;
drawnow;
%movcount=movcount+1;
%mov(movcount) = getframe(gcf);%
end
a=result.x;
toc
%movie2avi(mov,'movie.avi');
⛄ 运行结果
⛄ 参考文献
[1] 赵伟,吴子英.双层优化A*算法与动态窗口法的动态路径规划[J].计算机工程与应用, 2021, 57(22):9.DOI:10.3778/j.issn.1002-8331.2103-0434.
[2] 王洪斌,尹鹏衡,郑维,等.基于改进的A~*算法与动态窗口法的移动机器人路径规划[J].机器人, 2020, 42(3):8.DOI:10.13973/j.cnki.robot.190305.
[3] 陈欢,王志荣.基于A*与Floyd算法移动机器人路径规划研究[J].建设机械技术与管理, 2018, 31(3):3.DOI:CNKI:SUN:JCJX.0.2018-03-012.
[4] 华洪,张志安,施振稳,等.动态环境下多重A算法的机器人路径规划方法[J].计算机工程与应用, 2021.DOI:10.3778/j.issn.1002-8331.2007-0102.
