【二维路径规划】基于动态窗口法实现机器人动态障碍环境下的路径规划附matlab代码

1 简介

DWA 算法是基于机器人运动学与动力学理论的一种局部避障算法，它将对机器人的位置控制转换为对机器人的速度控制。DWA 算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样空间约束在一定范围内; 二是根据机器人运动学对采样后的速度进行模拟得到预轨迹; 三是设定评价函数对预轨迹进行评分以获取最优轨迹对应的执行速度。

2 部分代码

function [] = ob()
close all;
clear ;
% 障碍物位置列表 [x(m) y(m)]
obstacle=[%0 2;
          3 10*rand(1);
%           4 4;
%          5 4;
%            5 5;
          6 10*rand(1);
%          5 9
%          7 8
          8 10*rand(1);
          2 5;      
          4 2;
          7 7;
          9 9 ];
%边界障碍物，防止跑出图外
 for i =-1
    for j = -1:12
        obstacle = [obstacle; [i,j]];
    end
 end     
for i =12
    for j = -1:12
        obstacle = [obstacle; [i,j]];
    end
end 
for j =-2
    for i = -1:12
        obstacle = [obstacle; [i,j]];
    end
end 
for j=13
    for i= -1:12
        obstacle = [obstacle; [i,j]]; 
    end
end 
  grid on; 
obstacleR = 0.5;% 冲突判定用的障碍物半径
flag_obstacle = [1-2*rand(1) 1-2*rand(1) 1-2*rand(1)];
vel_obstacle = 0.2;
area= [-3 14 -3 14];% 模拟区域范围 [xmin xmax ymin ymax]
 %启动图形保持功能，当前坐标轴和图形都将保持，从此绘制的图形都将添加在这个图形的基础上，并自动调整坐标轴的范围
 %% Main loop   循环运行 5000次 指导达到目的地 或者 5000次运行结束
for i = 1:5000  
    % DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
    %====Animation====
    for j = 1:3
        if obstacle(j,2) > 10 && flag_obstacle(j) > 0 || obstacle(j,2) < 0 && flag_obstacle(j) < 0
            flag_obstacle(j) = -flag_obstacle(j);
        end
       obstacle(j,2)=obstacle(j,2)+flag_obstacle(j)*vel_obstacle;
    end
 plot(obstacle(:,1),obstacle(:,2),'*k');hold on;         % 绘制所有障碍物位置
 DrawObstacle_plot(obstacle,obstacleR);
 hold off;
 drawnow;
end
    axis(area); %根据area设置当前图形的坐标范围，分别为x轴的最小、最大值，y轴的最小最大值
    grid on;
function [] = DrawObstacle_plot(obstacle,obstacleR)
r = obstacleR; 
theta = 0:pi/20:2*pi;
for id=1:length(obstacle(:,1))
 x = r * cos(theta) + obstacle(id,1); 
 y = r  *sin(theta) + obstacle(id,2);
 plot(x,y,'-b'); 
  grid on;
end
end
end

3 仿真结果

4 参考文献

[1]李东正. 复杂环境下多机器人路径规划方法研究[D]. 哈尔滨工程大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

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