【路径规划】基于matlab GUI改进的DWA算法机器人动态避障路径规划【含Matlab源码 1271期】

一、DWA算法简介

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

2 速度采样

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

二、部分源代码

function varargout = Simulate(varargin)
clc
SamplingPeriod = 0.1;
time = 0;
timer1 = [];
txt_timer = [];
FuzzyLoaded = 0;
Z1 = [];
Z2 = [];
W_Zone = 20;
H_Zone = 20;data = importdata ('fuzzytabledata.mat');
OUT1 = data.OUT1;
OUT2 = data.OUT2;
Z1 = reshape(OUT1(102:end,1)',101,[]);
Z2 = reshape(OUT2(102:end,1)',101,[]);
FuzzyLoaded = 1;scrsz = get(0,'ScreenSize');% Create a figure that will have a uitable, axes and checkboxes
figure('Position',[15*scrsz(3)/100 70 scrsz(3)-2*(15*scrsz(3)/100) scrsz(4)-100],...'WindowStyle', 'normal',...'CloseRequestFcn',{@CloseFcn},...'Name', '具有动态障碍物的DWA仿真',...  % Title figure'NumberTitle', 'off') % Do not show figure number% figure('Position',[15*scrsz(3)/100 70 scrsz(3)-2*(15*scrsz(3)/100) scrsz(4)-100],...
%        'WindowStyle', 'normal',...
%        'CloseRequestFcn',{@CloseFcn},...
%        'Name', '具有动态障碍物的DWA仿真',...  % Title figure
%        'NumberTitle', 'off',... % Do not show figure number
%        'MenuBar', 'none');      % Hide standard menu bar menus% Create an axes set x and y limits to the value extremes, and format labels
W_Zone = ceil(W_Zone/10)*10;
H_Zone = ceil(H_Zone/10)*10;
haxes = axes('Units', 'normalized',...'Position', [.25 .05 0.75 0.9],...'XLim', [-W_Zone/2 W_Zone/2],...'YLim', [-H_Zone/2 H_Zone/2],...'XLimMode', 'manual',...'YLimMode', 'manual',...'XTick',-W_Zone/2:1:W_Zone/2,...'YTick',-H_Zone/2:1:H_Zone/2);%,...
%              'XTickLabel',...
%              {'-10 m','-5 m','0','5 m','10 m'},...
%              'YTickLabel',...
%              {'-10 m','-5 m','0','5 m','10 m'});
set(haxes,'DataAspectRatio',[1 1 1]);
title(haxes, '移动机器人动态避障仿真')   % Describe data set
% Prevent axes from clearing when new lines or markers are plotted
hold(haxes, 'all')
grid on;uicontrol('Style', 'pushbutton',...'Units', 'normalized',...'Position', [.02 .86 .2 .05],...'String', '开始仿真',...'Value', 0,...'Callback', {@StartSimulation});uicontrol('Style', 'pushbutton',...'Units', 'normalized',...'Position', [.02 .81 .2 .05],...'String', '重新仿真',...'Value', 0,...'Callback', {@ResetSimulation});% % Create a text to show timer;
txt_timer = uicontrol('Style', 'text',...'Units', 'normalized',...'Position', [.02 .915 .2 .035],...'FontWeight', 'bold',...'ForegroundColor', [0 .2 .8],...'fontname', 'Helvetica',...'fontsize', 14,...'BackgroundColor', 'w', 'String','00:00 00');% % Create some texts to monitor variables;
txt_inp1 = uicontrol('Style', 'text', 'Units', 'normalized', 'Position', [.05 .54 .08 .035],...'FontWeight', 'bold', 'ForegroundColor', [0 0 0], 'fontname', 'Helvetica', 'fontsize', 12, 'BackgroundColor', [0.9 0.9 0.9]);
txt_inp2 = uicontrol('Style', 'text', 'Units', 'normalized', 'Position', [.14 .54 .08 .035],...'FontWeight', 'bold', 'ForegroundColor', [0 0 0], 'fontname', 'Helvetica', 'fontsize', 12, 'BackgroundColor', [0.9 0.9 0.9]);
txt_out1 = uicontrol('Style', 'text', 'Units', 'normalized', 'Position', [.05 .47 .08 .035],...'FontWeight', 'bold', 'ForegroundColor', [0 0 0], 'fontname', 'Helvetica', 'fontsize', 12, 'BackgroundColor', [0.9 0.9 0.9]);
txt_out2 = uicontrol('Style', 'text', 'Units', 'normalized', 'Position', [.14 .47 .08 .035],...'FontWeight', 'bold', 'ForegroundColor', [0 0 0], 'fontname', 'Helvetica', 'fontsize', 12, 'BackgroundColor', [0.9 0.9 0.9]);uicontrol('Style', 'text','Units', 'normalized', 'Position', [.05 .61 .15 .025],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8], 'String', 'DWA权值调整');uicontrol('Style', 'text','Units', 'normalized', 'Position', [.00 .55 .05 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', 'INPs :');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.00 .48 .05 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', '误差值 :');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.05 .58 .08 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', '目标航向角');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.14 .58 .08 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', '障碍物');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.05 .51 .08 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', '左偏离');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.14 .51 .08 .02],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 9,...'BackgroundColor', [0.8 0.8 0.8], 'String', '右偏离');uicontrol('Style', 'text','Units', 'normalized', 'Position', [.09 .78 .12 .025],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8], 'String', 'X             Y');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.02 .74 .06 .03],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8], 'String', '起始点坐标：');
uicontrol('Style', 'text','Units', 'normalized', 'Position', [.02 .7 .06 .03],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8], 'String', '  目标点坐标：');% % Create a text get Robot X0 Pos;
edit_robot_x0 = uicontrol('Style', 'edit',...'Units', 'normalized',...'Position', [.09 .74 .06 .035],... %'String',... %'Use Plot check boxes to graph columns',...'FontWeight', 'bold',...'fontname', 'Helvetica',...'fontsize', 12);
% % Create a text get Robot Y0 Pos;
edit_robot_y0 = uicontrol('Style', 'edit',...'Units', 'normalized',...'Position', [.16 .74 .06 .035],... %'String',... %'Use Plot check boxes to graph columns',...'FontWeight', 'bold',...'fontname', 'Helvetica',...'fontsize', 12);% % Create a text get Goal X Pos;
edit_goal_x = uicontrol('Style', 'edit',...'Units', 'normalized',...'Position', [.09 .7 .06 .035],... %'String',... %'Use Plot check boxes to graph columns',...'FontWeight', 'bold',...'fontname', 'Helvetica',...'fontsize', 12);
% % Create a text get Goal Y Pos;
edit_goal_y = uicontrol('Style', 'edit',...'Units', 'normalized',...'Position', [.16 .7 .06 .035],... %'String',... %'Use Plot check boxes to graph columns',...'FontWeight', 'bold',...'fontname', 'Helvetica',...'fontsize', 12);uicontrol('Style', 'pushbutton',...'Units', 'normalized',...'Position', [.09 .655 .13 .04],... %'TooltipString', 'Update Values Independently',...'String', '更新',...'Value', 0,...'Callback', {@UpdateParams});slider_x = uicontrol('Style', 'slider','Units', 'normalized', 'Position', [.02 .41 .2 .025],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8],'Min',0,'Value',1,'Max',2,'SliderStep',[0.01 0.1]);slider_y = uicontrol('Style', 'slider','Units', 'normalized', 'Position', [.02 .37 .2 .025],...'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,...'BackgroundColor', [0.8 0.8 0.8],'Min',0,'Value',1,'Max',2,'SliderStep',[0.01 0.1]);% uicontrol('Style', 'pushbutton',...
%           'Units', 'normalized',...
%           'Position', [0.35 .95 .25 .05],... %'TooltipString', 'Update Values Independently',...
%           'String', 'Save Data',...
%           'Value', 0,...
%           'Callback', {@SaveData});% % Create an invisible marker plot of the data and save handles
% % to the lineseries objects; use this to simulate data brushing.
% % hmkrs = plot([1:10],[1:10].^2, 'LineStyle', 'none',...
% %                     'Marker', 'o',...
% %                     'MarkerFaceColor', 'y',...
% %                     'HandleVisibility', 'off',...
% %                     'Visible', 'on');% Create three check boxes to toggle plots for columns
% uicontrol('Style', 'checkbox',...
%           'Units', 'normalized',...
%           'Position', [.10 .96 .09 .035],...
%           'TooltipString', 'Check to plot column 1',...
%           'String', 'Col 1',...
%           'Value', 0,...
%           'Callback', {@plot_callback,1});
%
% % Create a text label to say what the checkboxes do
% uicontrol('Style', 'text',...
%           'Units', 'normalized',...
%           'Position', [.025 .955 .06 .035],...
%           'String', 'Plot',...
%           'FontWeight', 'bold');% Subfuntions implementing the two callbacks
% ------------------------------------------%     function plot_callback(hObject, eventdata, column)
%     % hObject     Handle to Plot menu
%     % eventdata   Not used
%     % column      Number of column to plot or clear
%
%     colors = {'b','m','r'}; % Use consistent color for lines
%     colnames = get(htable, 'ColumnName');
%     colname = colnames{column};
%     if get(hObject, 'Value')
%         % Turn off the advisory text; it never comes back
%         set(hprompt, 'Visible', 'off')
%         % Obtain the data for that column
%         ydata = get(htable, 'Data');
%         set(haxes, 'NextPlot', 'Add')
%         % Draw the line plot for column
%         plot(haxes, ydata(:,column),...
%             'DisplayName', colname,...
%             'Color', colors{column});
%     else % Adding a line to the plot
%         % Find the lineseries object and delete it
%         delete(findobj(haxes, 'DisplayName', colname))
%     end
%     endw = 0.5;l = 0.6;D = 0.4;x0 = -8;y0 = -8;teta0 = 0;Vr0 = 0;Vl0 = 0;x = x0;y = y0;teta = teta0;Vr = Vr0;Vl = Vl0;V_MAX = 5;x_Goal = 8.;y_Goal = 8;n_of_obs = 6;x_obs0(1) = 3;y_obs0(1) = 3;x_obs(1) = x_obs0(1);y_obs(1) = y_obs0(1);w_obs(1) = 2.5;h_obs(1) = 2.5;n_obs(1) = 50;%teta_obs(1) = pi/4; x_obs0(2) = -1;y_obs0(2) = 0.5;x_obs(2) = x_obs0(2);y_obs(2) = y_obs0(2);w_obs(2) = 0.5;h_obs(2) = 3;n_obs(2) = 50;%teta_obs(2) = pi/4; x_obs0(3) = 5;y_obs0(3) = -3;x_obs(3) = x_obs0(3);y_obs(3) = y_obs0(3);w_obs(3) = 5;h_obs(3) = 1;n_obs(3) = 50;%teta_obs(3) = pi/4; x_obs0(4) = -3;y_obs0(4) = 5;x_obs(4) = x_obs0(4);y_obs(4) = y_obs0(4);w_obs(4) = 2;h_obs(4) = 1;n_obs(4) = 50;%teta_obs(4) = pi/4; x_obs0(5) = -3;y_obs0(5) = -5;x_obs(5) = x_obs0(5);y_obs(5) = y_obs0(5);w_obs(5) = 2;h_obs(5) = 2;n_obs(5) = 2;%teta_obs(5) = pi/4; x_obs0(6) = -5;y_obs0(6) = 1;x_obs(6) = x_obs0(6);y_obs(6) = y_obs0(6);w_obs(6) = 1;h_obs(6) = 1;n_obs(6) = 2;%teta_obs(6) = pi/4; counter = 0;trace_x = [];trace_y = [];if(abs(y_obs(jj)-y_obs0(jj)) < get(slider_y,'Value'))y_obs(jj) = y_obs(jj) + (cos(time*4+rand(1)*3)*rand(1)*3)*SamplingPeriod;elsey_obs(jj) = y_obs(jj) + (y_obs0(jj)-y_obs(jj))*0.2*SamplingPeriod;endss(jj)= max(ss);[ss_min,jj] = min(ss);if(abs(x_obs(jj)-x_obs0(jj)) < get(slider_x,'Value'))x_obs(jj) = x_obs(jj) + ((sin(time*3+rand(1)*2)+0.2*(x-x_obs(jj)))*rand(1)*3)*SamplingPeriod;elsex_obs(jj) = x_obs(jj) + (x_obs0(jj)-x_obs(jj))*0.2*SamplingPeriod;endif(abs(y_obs(jj)-y_obs0(jj)) < get(slider_y,'Value'))y_obs(jj) = y_obs(jj) + (cos(time*4+rand(1)*3)*rand(1)*3)*SamplingPeriod;elsey_obs(jj) = y_obs(jj) + (y_obs0(jj)-y_obs(jj))*0.2*SamplingPeriod;end%Check Obstaclesray_i = 1;ray_x = []; ray_y = []; ray_s = []; ray_teta = [];rayplot_x = []; rayplot_y = []; rayplot_i = 1;for dir = linspace(-pi/4,pi/4,16)Obstacle_Found = 0;s = 0;while(Obstacle_Found==0)s = s + 0.02;ray_pos = [x + s*sin(teta+dir), y + s*cos(teta+dir)];for i = 1:n_of_obsif( (2*(ray_pos(1)-x_obs(i))/w_obs(i))^n_obs(i)+(2*(ray_pos(2)-y_obs(i))/h_obs(i))^n_obs(i) < 1 )Obstacle_Found = 1;endendif( (2*ray_pos(1)/W_Zone)^100+(2*ray_pos(2)/H_Zone)^100>1 || s>3)Obstacle_Found = 1;endif(Obstacle_Found==1)ray_x(ray_i) = ray_pos(1); ray_y(ray_i) = ray_pos(2);ray_s(ray_i) = s;ray_teta(ray_i) = dir;rayplot_x(rayplot_i:rayplot_i+2) = [x ray_pos(1) x];rayplot_y(rayplot_i:rayplot_i+2) = [y ray_pos(2) y];endendray_i = ray_i + 1;rayplot_i = rayplot_i + 3;endj=0;if(mod(counter,1)==0)cla(haxes);            robot_extents = [cos(teta) sin(teta) x; -sin(teta) cos(teta) y] * [-w/2,-l/2,1; w/2,-l/2,1; w/2,l/2,1; -w/2,l/2,1; -w/2,-l/2,1; w/2,-l/2,1]';        plot(haxes,robot_extents(1,:),robot_extents(2,:), 'LineWidth' ,3,'Color',[0 0 1]);robot_tyre = [cos(teta) sin(teta) x; -sin(teta) cos(teta) y] * [-w/2-0.1,-l/2-l/4,1; -w/2-0.1,-l/2+l/4,1; w/2+0.1,-l/2-l/4,1; w/2+0.1,-l/2+l/4,1; -w/2-0.1,l/2-l/4,1; -w/2-0.1,l/2+l/4,1; w/2+0.1,l/2-l/4,1; w/2+0.1,l/2+l/4,1; 0,l/2,1; 0,l/2+l/2,1]';plot(haxes,robot_tyre(1,1:2),robot_tyre(2,1:2), 'LineWidth' ,4,'Color',[0 0 0]); plot(haxes,robot_tyre(1,3:4),robot_tyre(2,3:4), 'LineWidth' ,4,'Color',[0 0 0]);plot(haxes,robot_tyre(1,5:6),robot_tyre(2,5:6), 'LineWidth' ,4,'Color',[0 0 0]); plot(haxes,robot_tyre(1,7:8),robot_tyre(2,7:8), 'LineWidth' ,4,'Color',[0 0 0]);plot(haxes,robot_tyre(1,9:10),robot_tyre(2,9:10), 'LineWidth' ,1,'Color',[0.4 0.4 0.4]);s = linspace(0,2*pi,360);for i = 1:n_of_obsx_o = (1./((sin(s)/(h_obs(i)/2)).^n_obs(i) + (cos(s)/(w_obs(i)/2)).^n_obs(i)).^(1/n_obs(i))).*cos(s) + x_obs(i);y_o = (1./((sin(s)/(h_obs(i)/2)).^n_obs(i) + (cos(s)/(w_obs(i)/2)).^n_obs(i)).^(1/n_obs(i))).*sin(s) + y_obs(i);plot(haxes,x_o,y_o, 'LineWidth' ,2,'Color',[0 0 0]);%if(gca == haxes), fill(x_o,y_o,[0.5 0.5 0.5],'LineWidth' ,2); endendplot(haxes,[-W_Zone/2 W_Zone/2 W_Zone/2 -W_Zone/2 -W_Zone/2],[-H_Zone/2 -H_Zone/2 H_Zone/2 H_Zone/2 -H_Zone/2], 'LineWidth' ,4,'Color',[0 0 0]);plot(haxes, rayplot_x, rayplot_y, 'LineWidth' ,1,'Color',[1 0.8 0]);plot(haxes, trace_x, trace_y, ':', 'LineWidth' ,2,'Color',[0.4 0.4 0.7]);plot(haxes, x_Goal, y_Goal,'*','MarkerSize',15,'MarkerEdgeColor','g','LineWidth',3);if(j>0)plot(haxes, ray_x(j), ray_y(j),'s','MarkerSize',8,'MarkerEdgeColor','r','LineWidth',3); end%plot(haxes, [x x_Goal], [y y_Goal],'-.', 'LineWidth' ,1,'Color',[0.8 0.95 0.2]);endend%% create and start timer1 to execute the function Command sequentially.timer1 = timer('TimerFcn', @Command, 'Period', SamplingPeriod, 'ExecutionMode', 'fixedRate');

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.

【路径规划】基于matlab GUI改进的DWA算法机器人动态避障路径规划【含Matlab源码 1271期】相关推荐

【路径规划】基于matlab DWA算法机器人局部避障路径规划【含Matlab源码 890期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[路径规划]基于matlab DWA算法机器人局部避障路径规划[含Matlab源码 890期] 获取代码方式2: 通过订阅紫极神光博客付费 ...
【路径规划】基于matlab A_star算法机器人动态避障【含Matlab源码 2571期】
⛄一.A_star算法简介 1 A Star算法及其应用现状进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标 ...
【路径规划】基于DWA实现机器人动态避障附matlab代码
1 内容介绍 DWA 算法是基于机器人运动学与动力学理论的一种局部避障算法,它将对机器人的位置控制转换为对机器人的速度控制.DWA 算法可以概括为三步:一是根据机器人自身的限制以及环境制约将速度的采样 ...
A星融合DWA的路径规划算法，可实现静态避障碍及动态避障,代码注释详细,matlab源码
A星融合DWA的路径规划算法,可实现静态避障碍及动态避障,代码注释详细,matlab源码 ID:4525679980340317云的歌儿
【光学】基于matlab GUI矩阵法和等效界面法光学薄膜对反射率影响【含Matlab源码 2102期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[光学]基于matlab GUI矩阵法和等效界面法光学薄膜对反射率影响[含Matlab源码 2102期] 点击上面蓝色字体,直接付费下载, ...
【单目标优化求解】基于matlab增强型黑猩猩优化器算法求解单目标优化问题【含Matlab源码 2013期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[单目标优化求解]基于matlab增强型黑猩猩优化器算法求解单目标优化问题[含Matlab源码 2013期] 点击上面蓝色字体,直接付费下 ...
【路径规划】基于matlab DWA动态避障路径规划【含Matlab源码 2356期】
⛄一.传统DWA算法在传统的动态窗口法中,在模拟机器人的移动轨迹前,需要建立机器人的运动模型.图1为典型的移动机器人运动学模型示意图. v (t)和w (t)分别代表了移动机器人在世界坐标系中的线速 ...
【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）
文章目录参考资料 1. DWA算法原理 1.1 简介 1.2 算法原理 1. 速度采样 2. 轨迹预测(轨迹推算) 3. 轨迹评价 2. Python实现 2.1 参数配置 2.2 机器人运动学模型 ...
【字符识别】基于matlab GUI模板匹配（区域生长法）字母+数字识别【含Matlab源码 1695期】
一.手写大写字母识别技术简介 0 引言在高校教学过程中,考试是最为普遍的一种教学评估.综合练习的教学手段,随着科技进步,考试阅卷的方式也发生了巨大的变革.传统的阅卷方式主要以人工阅卷为主, 存在效率 ...
【语音识别】基于matlab GUI MFCC+VAD端点检测智能语音门禁系统【含Matlab源码 451期】
⛄一.MFCC简介 1 引言语音识别是一种模式识别, 就是让机器通过识别和理解过程把语音信号转变为相应的文本或命令的技术.语音识别技术主要包括特征提取技术.模式匹配准则及模型训练技术3个方面.目前一 ...