一、簡(jiǎn)介

DWA算法全稱(chēng)為dynamic window approach，其原理主要是在速度空間（v,w）中采樣多組速度，并模擬這些速度在一定時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，再通過(guò)一個(gè)評(píng)價(jià)函數(shù)對(duì)這些軌跡打分，最優(yōu)的速度被選擇出來(lái)發(fā)送給下位機(jī)。
1 原理分析

2 速度采樣

機(jī)器人的軌跡運(yùn)動(dòng)模型有了，根據(jù)速度就可以推算出軌跡。
因此只需采樣很多速度，推算軌跡，然后評(píng)價(jià)這些軌跡好不好就行了。
（一）移動(dòng)機(jī)器人受自身最大速度最小速度的限制
（二） 移動(dòng)機(jī)器人受電機(jī)性能的影響:由于電機(jī)力矩有限,存在最大的加減速限制,因此移動(dòng)機(jī)器人軌跡前向模擬的周期sim_period內(nèi),存在一個(gè)動(dòng)態(tài)窗口,在該窗口內(nèi)的速度是機(jī)器人能夠?qū)嶋H達(dá)到的速度:
（三） 基于移動(dòng)機(jī)器人安全的考慮:為了能夠在碰到障礙物前停下來(lái), 因此在最大減速度條件下, 速度有一個(gè)范圍。

二、源代碼

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', '具有動(dòng)態(tài)障礙物的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', '具有動(dòng)態(tài)障礙物的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, '移動(dòng)機(jī)器人動(dòng)態(tài)避障仿真') ? % 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', '開(kāi)始仿真',... ? ? ? ? ?'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權(quán)值調(diào)整'); ? ? ? ? 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', '目標(biāo)航向角'); 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', '起始點(diǎn)坐標(biāo)：'); uicontrol('Style', 'text','Units', 'normalized', 'Position', [.02 .7 .06 .03],... ? ? ? ? ? ?'FontWeight', 'bold', 'fontname', 'Helvetica', 'fontsize', 10,... ? ? ? ? ? ?'BackgroundColor', [0.8 0.8 0.8], 'String', ' ?目標(biāo)點(diǎn)坐標(biāo)：'); ? ? ? ? % % 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');

三、運(yùn)行結(jié)果