【路径规划】基于A星算法结合floyd和动态窗口法实现机器人栅格地图路径规划附matlab代码

✅作者简介：热爱科研的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_ngdu_max||s_du