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

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

1 算法原理

A*算法结合Floyd算法和动态窗口法可以实现机器人在栅格地图上的路径规划。下面是一个基本的步骤：

1. 栅格地图表示：将待规划的区域划分为一组栅格，每个栅格代表一个离散的位置，其中包括可通过的栅格和障碍物。

2. Floyd算法处理地：使用Floyd算法计算任意两个栅格之间的最短路径距离，并生成对应的路径矩阵或邻矩阵。

3. 初始化启发式值和开放列表：为每个栅格设置启发式值（如曼哈顿距离或欧几里得距离），并初始化开放列表来记录待访问的栅格。

4. A*搜索过程：

   • 选择起始栅格作为当前节点。

   • 对于当前节点，计算其邻居栅格的估计代价（包括实际距离和启发式值）。

   • 更新邻居栅格的代价信息和路径信息。

   • 将更新后的邻居栅格加入开放列表。

   • 从开放列表中选择下一个节点作为当前节点，重复执行以上步骤，直到达到终点或无法找到路径。

5. 动态窗口法调整路径：将A*算法得到的初始路径进行平滑和优化，通过动态窗口法消除路径中的不必要转弯或避免碰撞等问题，以生成更加平滑和可行的最终路径。

6. 输出最终路径：经过以上步骤，可以得到机器人在栅格地图上的规划路径，即一系列连续的栅格点。

需要注意的是，A*算法是一种启发式搜索方法，用于在离散状态空间中找到最优路径。Floyd算法用于计算任意两个节点之间的最短路径，而动态窗口法用于路径平滑和优化。将它们结合在一起可以应对不同类型的路径规划问题，并使规划的路径更加有效和可行。

2 算法流程

下面是A*算法结合Floyd算法和动态窗口法的基本流程：

1. 栅格地图表示：将待规划的区域划分为一组栅格，每个栅格代表一个离散的位置，包括可通过的栅格、起始点和目标点等。

2. 初始化数据结构和参数：

   • 创建栅格地图的邻接矩阵，用于存储栅格之间的连接关系。

   • 初始化起始点和目标点，并设置启发式值（如曼哈顿距离）和开放列表。

3. 使用Floyd算法计算任意两个栅格之间的最短路径距离：

   • 基于栅格地图的邻接矩阵，使用Floyd算法计算所有栅格之间的最短路径距离，并生成对应的路径矩阵。

4. A*搜索过程：

   • 如果邻居节点不可通过或者已在已访问集合中，则忽略。

   • 更新邻居节点的代价信息和路径信息，计算估计总代价。

   • 如果邻居节点不在开放列表中，将其加入开放列表。

   • 如果邻居节点在开放列表中，并且新计算的总代价更小，则更新其代价信息和路径信息。

   • 选择起始点作为当前节点。

   • 初始化当前节点的代价和路径信息。

   • 将当前节点加入已访问集合。

   • 对于当前节点的所有邻居节点：

   • 从开放列表中选择具有最小总代价的节点作为下一个当前节点，重复执行以上步骤。

   • 如果达到目标点或开放列表为空（无可行解），则搜索结束。

5. 动态窗口法调整路径：

   • 根据A*算法得到的初始路径，根据窗口大小设定，将路径进行平滑和优化。

   • 如果路径中存在转弯点或避障点，通过调整路径段来消除不必要的转弯或避免碰撞。

6. 输出最终路径：

   • 经过以上步骤，得到调整后的规划路径，即一系列连续的栅格点作为最终路径。

⛄ 部分代码

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.

🍅 仿真咨询

1.卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

2.图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

3.旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划

4.无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

5.传感器部署优化、通信协议优化、路由优化、目标定位

6.信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号

7.生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化

8.微电网优化、无功优化、配电网重构、储能配置

9.元胞自动机交通流 人群疏散 病毒扩散 晶体生长

⛳️ 代码获取关注我

❤️部分理论引用网络文献，若有侵权联系博主删除

❤️ 关注我领取海量matlab电子书和数学建模资料