A list,

DWA algorithm is called Dynamic Window Approach. Its principle is to sample multiple sets of velocities in the velocity space (V, W) and simulate the movement trajectory of these velocities within a certain time. Then, these trajectory are scored by an evaluation function, and the optimal velocity is selected and sent to the next computer.

1 Principle Analysis







2 Speed sampling

The trajectory model of the robot is available, and the trajectory can be calculated according to the speed. So you just sample a lot of velocities, calculate trajectories, and evaluate how good those trajectories are. (a) mobile robot restricted by their own maximum speed minimum speed (2) of the mobile robot is affected by the motor performance: due to the motor torque is limited, there is the biggest limit acceleration and deceleration, so the cycle of the mobile robot path forward simulation sim_period inside, there is a dynamic window, the window is the speed of the robot can achieve actual speed: (3) Considering the safety of mobile robot: In order to stop before encountering obstacles, there is a range of speed under the condition of maximum deceleration.

Ii. Source code

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');
Copy the code

3. Operation results



Fourth, note

Complete code or simulation consultation to add QQ2449341593