【路径规划】基于matlab DWA算法机器人局部避障路径规划【含Matlab源码 890期】

⛄一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【路径规划】基于matlab DWA算法机器人局部避障路径规划【含Matlab源码 890期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

⛄二、DWA算法简介

DWA算法全称为dynamic window approach，其原理主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给下位机。
1 原理分析

2 速度采样

机器人的轨迹运动模型有了，根据速度就可以推算出轨迹。
因此只需采样很多速度，推算轨迹，然后评价这些轨迹好不好就行了。
（一）移动机器人受自身最大速度最小速度的限制
（二）移动机器人受电机性能的影响:由于电机力矩有限,存在最大的加減速限制,因此移动机器人軌迹前向模拟的周期sim_period内,存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度:
（三）基于移动机器人安全的考虑:为了能够在碰到障碍物前停下来, 因此在最大减速度条件下, 速度有一个范围。

⛄三、部分源代码

% -------------------------------------------------------------------------
%
% File : DynamicWindowApproachSample.m
%
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach
%
% Environment : Matlab

% -------------------------------------------------------------------------

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
endaxis(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; % 绘制所有障碍物位置

%% DWA算法实现
% model 机器人运动学模型最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], 速度分辨率[m/s],转速分辨率[rad/s]]
% 输入参数：当前状态、模型参数、目标点、评价函数的参数、障碍物位置、障碍物半径
% 返回参数：控制量 u = [v(m/s),w(rad/s)] 和轨迹集合 N * 31 （N：可用的轨迹数）
% 选取最优参数的物理意义：在局部导航过程中，使得机器人避开障碍物，朝着目标以较快的速度行驶。
function [u,trajDB] = DynamicWindowApproach(x,model,goal,evalParam,ob,R)
% Dynamic Window [vmin,vmax,wmin,wmax] 最小速度最大速度最小角速度最大角速度速度
Vr = CalcDynamicWindow(x,model); % 根据当前状态和运动模型计算当前的参数允许范围

% 评价函数的计算 evalDB N*5 每行一组可用参数分别为速度、角速度、航向得分、距离得分、速度得分
% trajDB 每5行一条轨迹每条轨迹都有状态x点串组成
[evalDB,trajDB]= Evaluation(x,Vr,goal,ob,R,model,evalParam); %evalParam 评价函数参数 [heading,dist,velocity,predictDT]

if isempty(evalDB)
disp(‘no path to goal!!’);
u=[0;0];return;
end

% 各评价函数正则化
evalDB = NormalizeEval(evalDB);

% 最终评价函数的计算
feval=[];
for id=1:length(evalDB(:,1))
feval = [feval;evalParam(1:3)*evalDB(id,3:5)']; %根据评价函数参数前三个参数分配的权重计算每一组可用的路径参数信息的得分
end
evalDB = [evalDB feval]; % 最后一组

[maxv,ind] = max(feval);% 选取评分最高的参数对应分数返回给 maxv 对应下标返回给 ind
u = evalDB(ind,1:2)';% 返回最优参数的速度、角速度

%% 评价函数内部负责产生可用轨迹
% 输入参数：当前状态、参数允许范围（窗口）、目标点、障碍物位置、障碍物半径、评价函数的参数
% 返回参数：
% evalDB N*5 每行一组可用参数分别为速度、角速度、航向得分、距离得分、速度得分
% trajDB 每5行一条轨迹每条轨迹包含前向预测时间/dt + 1 = 31 个轨迹点（见生成轨迹函数）
function [evalDB,trajDB] = Evaluation(x,Vr,goal,ob,R,model,evalParam)
evalDB = [];
trajDB = [];
for vt = Vr(1):model(5):Vr(2) %根据速度分辨率遍历所有可用速度：最小速度和最大速度之间速度分辨率递增
for ot=Vr(3):model(6):Vr(4) %根据角度分辨率遍历所有可用角速度：最小角速度和最大角速度之间角度分辨率递增
% 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻到预测时刻之间的轨迹（由轨迹点组成）
[xt,traj] = GenerateTrajectory(x,vt,ot,evalParam(4),model); %evalParam(4),前向模拟时间;
% 各评价函数的计算
heading = CalcHeadingEval(xt,goal); % 前项预测终点的航向得分偏差越小分数越高
dist = CalcDistEval(xt,ob,R); % 前项预测终点距离最近障碍物的间隙得分距离越远分数越高
vel = abs(vt); % 速度得分速度越快分越高
stopDist = CalcBreakingDist(vel,model); % 制动距离的计算
if dist > stopDist % 如果可能撞到最近的障碍物则舍弃此路径（到最近障碍物的距离大于刹车距离才取用）
evalDB = [evalDB;[vt ot heading dist vel]];
trajDB = [trajDB;traj]; % 每5行一条轨迹
end
end
end

⛄四、运行结果

⛄五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]王豪杰,马向华,代婉玉,靳午煊.改进DWA算法的移动机器人避障研究[J].计算机工程与应用.

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除