1 简介
DWA 算法是基于机器人运动学与动力学理论的一种局部避障算法，它将对机器人的位置控制转换为对机器人的速度控制。DWA 算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样空间约束在一定范围内; 二是根据机器人运动学对采样后的速度进行模拟得到预轨迹; 三是设定评价函数对预轨迹进行评分以获取最优轨迹对应的执行速度。
2 部分代码
function [] = DynamicWindowApproachSample()
close all;
clear all;
disp('Dynamic Window Approach sample program start!!')
%% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
% x=[0 0 pi/2 0 0]'; % 5x1矩阵 列矩阵 位置 0，0 航向 pi/2 ,速度、角速度均为0
x = [0 0 pi/10 0 0]';
% 下标宏定义 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]
POSE_X = 1; %坐标 X
POSE_Y = 2; %坐标 Y
YAW_ANGLE = 3; %机器人航向角
V_SPD = 4; %机器人速度
W_ANGLE_SPD = 5; %机器人角速度
goal = [10,10]; % 目标点位置 [x(m),y(m)]
% 障碍物位置列表 [x(m) y(m)]
obstacle=[0 2;
2 4;
2 5;
4 2;
% 4 4;
5 4;
% 5 5;
5 6;
5 9
8 8
8 9
7 9];
% obstacle=[0 2;
% 4 2;
% 4 4;
% 5 4;
% 5 5;
% 5 6;
% 5 9
% 8 8
% 8 9
% 7 9
% 6 5
% 6 3
% 6 8
% 6 7
% 7 4
% 9 8
% 9 11
% 9 6];
obstacleR = 0.5;% 冲突判定用的障碍物半径
global dt;
dt = 0.1;% 时间[s]
% 机器人运动学模型参数
% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],
% 速度分辨率[m/s],转速分辨率[rad/s]]
Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];
%定义Kinematic的下标含义
MD_MAX_V = 1;% 最高速度m/s]
MD_MAX_W = 2;% 最高旋转速度[rad/s]
MD_ACC = 3;% 加速度[m/ss]
MD_VW = 4;% 旋转加速度[rad/ss]
MD_V_RESOLUTION = 5;% 速度分辨率[m/s]
MD_W_RESOLUTION = 6;% 转速分辨率[rad/s]]
% 评价函数参数 [heading,dist,velocity,predictDT]
% 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间
evalParam = [0.05, 0.2 ,0.1, 3.0];
area = [-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]
% 模拟实验的结果
result.x=[]; %累积存储走过的轨迹点的状态值
tic; % 估算程序运行时间开始
% movcount=0;
%% Main loop 循环运行 5000次 指导达到目的地 或者 5000次运行结束
for i = 1:5000
% DWA参数输入 返回控制量 u = [v(m/s),w(rad/s)] 和 轨迹
[u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
x = f(x,u);% 机器人移动到下一个时刻的状态量 根据当前速度和角速度推导 下一刻的位置和角度
% 历史轨迹的保存
result.x = [result.x; x']; %最新结果 以列的形式 添加到result.x
% 是否到达目的地
if norm(x(POSE_X:POSE_Y)-goal')<0.5 % norm函数来求得坐标上的两个点之间的距离
disp('Arrive Goal!!');break;
end
%====Animation====
hold off; % 关闭图形保持功能。 新图出现时，取消原图的显示。
ArrowLength = 0.5; % 箭头长度
% 机器人
% quiver(x,y,u,v) 在 x 和 y 中每个对应元素对组所指定的坐标处将向量绘制为箭头
quiver(x(POSE_X), x(POSE_Y), ArrowLength*cos(x(YAW_ANGLE)), ArrowLength*sin(x(YAW_ANGLE)), 'ok'); % 绘制机器人当前位置的航向箭头
hold on; %启动图形保持功能，当前坐标轴和图形都将保持，从此绘制的图形都将添加在这个图形的基础上，并自动调整坐标轴的范围
plot(result.x(:,POSE_X),result.x(:,POSE_Y),'-b');hold on; % 绘制走过的所有位置 所有历史数据的 X、Y坐标
plot(goal(1),goal(2),'*r');hold on; % 绘制目标位置
%plot(obstacle(:,1),obstacle(:,2),'*k');hold on; % 绘制所有障碍物位置
DrawObstacle_plot(obstacle,obstacleR);
% 探索轨迹 画出待评价的轨迹
if ~isempty(traj) %轨迹非空
for it=1:length(traj(:,1))/5 %计算所有轨迹数 traj 每5行数据 表示一条轨迹点
ind = 1+(it-1)*5; %第 it 条轨迹对应在traj中的下标
plot(traj(ind,:),traj(ind+1,:),'-g');hold on; %根据一条轨迹的点串画出轨迹 traj(ind,:) 表示第ind条轨迹的所有x坐标值 traj(ind+1,:)表示第ind条轨迹的所有y坐标值
end
end
axis(area); %根据area设置当前图形的坐标范围，分别为x轴的最小、最大值，y轴的最小最大值
grid on;
drawnow; %刷新屏幕. 当代码执行时间长，需要反复执行plot时，Matlab程序不会马上把图像画到figure上，这时，要想实时看到图像的每一步变化情况，需要使用这个语句。
%movcount = movcount+1;
%mov(movcount) = getframe(gcf);% 记录动画帧
end
toc %输出程序运行时间 形式：时间已过 ** 秒。
%movie2avi(mov,'movie.avi'); %录制过程动画 保存为 movie.avi 文件
%% 绘制所有障碍物位置
% 输入参数：obstacle 所有障碍物的坐标 obstacleR 障碍物的半径
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,'-m');hold on;
end
% plot(obstacle(:,1),obstacle(:,2),'*m');hold on; % 绘制所有障碍物位置
3 仿真结果
4 参考文献
[1]王永雄, 田永永, 李梁华,等. 基于自适应动态窗口的移动机器人局部动态路径规划方法:.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
