一、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
% end
w = 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;
else
y_obs(jj) = y_obs(jj) + (y_obs0(jj)-y_obs(jj))*0.2*SamplingPeriod;
end
ss(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;
else
x_obs(jj) = x_obs(jj) + (x_obs0(jj)-x_obs(jj))*0.2*SamplingPeriod;
end
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;
else
y_obs(jj) = y_obs(jj) + (y_obs0(jj)-y_obs(jj))*0.2*SamplingPeriod;
end
%Check Obstacles
ray_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_obs
if( (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;
end
end
if( (2*ray_pos(1)/W_Zone)^100+(2*ray_pos(2)/H_Zone)^100>1 || s>3)
Obstacle_Found = 1;
end
if(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];
end
end
ray_i = ray_i + 1;
rayplot_i = rayplot_i + 3;
end
j=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_obs
x_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); end
end
plot(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]);
end
end
%% 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.
