一、简介

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



2 速度采样

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

二、源代码

function varargout = main(varargin)

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @main_OpeningFcn, ...
                   'gui_OutputFcn',  @main_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before main is made visible.
function main_OpeningFcn(hObject, eventdata, handles, varargin)

handles.output = hObject;

%initial values
load('graph.mat');
load('position.mat');
load('labelnodeposition.mat');
load('labeledge.mat');

handles.graph = graph;
handles.graph_backup = graph;
handles.position = position;
handles.labelnodeposition = labelnodeposition;
handles.labeledgeposition = getlabeledgeposition;
handles.labeledge = labeledge;
handles.source = 1;
handles.destination = 1;
handles.street = 'J1 ';
guidata(hObject, handles);

axes(handles.axes1);
hold on;
gplot(handles.graph, handles.position,'-ok');
for i = 1:27
    text(handles.labelnodeposition(i,1),handles.labelnodeposition(i,2),int2str(i),'FontSize',7,'Color','k');  
end

for i= 1:36
    text(handles.labeledgeposition(i,1),handles.labeledgeposition(i,2),handles.labeledge(i,1:3),'FontSize',7,'Color','b');
end
axis off;


% --- Outputs from this function are returned to the command line.
function varargout = main_OutputFcn(hObject, eventdata, handles) 
varargout{1} = handles.output;


% --- Executes when figure1 is resized.
function figure1_ResizeFcn(hObject, eventdata, handles)


% --- Executes on selection change in popupmenu2.
function popupmenu2_Callback(hObject, eventdata, handles)
val = get(hObject,'Value');
string_list = get(hObject,'String');
source =char(string_list(val));
handles.source = str2num(source);
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function popupmenu2_CreateFcn(hObject, eventdata, handles)
val = get(hObject,'Value');
string_list = get(hObject,'String');
source = char(string_list(val));
handles.source = str2num(source);
guidata(hObject, handles);

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on selection change in popupmenu3.
function popupmenu3_Callback(hObject, eventdata, handles)
val = get(hObject,'Value');
string_list = get(hObject,'String');
destination = char(string_list(val));
handles.destination = str2num(destination);
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function popupmenu3_CreateFcn(hObject, eventdata, handles)
val = get(hObject,'Value');
string_list = get(hObject,'String');
destination = char(string_list(val));
handles.destination = str2num(destination);
guidata(hObject, handles);

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
axes(handles.axes1);
[cost rute] = showShortestPath(handles.graph,handles.position,handles.source,handles.destination);


% --- Executes on selection change in popupmenu4.
function popupmenu4_Callback(hObject, eventdata, handles)
val = get(hObject,'Value');
string_list = get(hObject,'String');
handles.street = char(string_list(val));
guidata(hObject, handles);
function [node1 node2]=street2node(street)

node1=1;
node2=2;
switch street
    case 'J1'
        node1 = 1;
        node2 = 2;
    case 'J2'
        node1 = 2;
        node2 = 3;
    case 'J3'
        node1 = 3;
        node2 = 4;
    case 'J4'
        node1 = 4;
        node2 = 5;
    case 'J5'
        node1 = 22;
        node2 = 26;
    case 'J6'
        node1 = 26;
        node2 = 27;
    case 'J7'
        node1 = 8;
        node2 = 9;
    case 'J8'
        node1 = 9;
        node2 = 10;
    case 'J9'
        node1 = 10;
        node2 = 11;
    case 'J10'
        node1 = 6;
        node2 = 7;
    case 'J11'
        node1 = 7;
        node2 = 12;
    case 'J12'
        node1 = 12;
        node2 = 13;
    case 'J13'
        node1 = 13;
        node2 = 14;
    case 'J14'
        node1 = 14;
        node2 = 15;
    case 'J15'
        node1 = 15;
        node2 = 16;
    case 'J16'
        node1 = 17;
        node2 = 18;
    case 'J17'
        node1 = 18;
        node2 = 19;
    case 'J18'
        node1 = 19;
        node2 = 20;
    case 'J19'
        node1 = 2;
        node2 = 7;
    case 'J20'
        node1 = 22;
        node2 = 8;
    case 'J21'
        node1 = 8;
        node2 = 12;
    case 'J22'
        node1 = 12;
        node2 = 17;
    case 'J23'
        node1 = 17;
        node2 = 23;
    case 'J24'
        node1 = 9;
        node2 = 13;
    case 'J25'
        node1 = 13;
        node2 = 18;
    case 'J26'
        node1 = 18;
        node2 = 24;
    case 'J27'
        node1 = 3;
        node2 = 26;
    case 'J28'
        node1 = 26;
        node2 = 10;
    case 'J29'
        node1 = 10;
        node2 = 14;
    case 'J30'
        node1 = 14;
        node2 = 19;
    case 'J31'
        node1 = 19;
        node2 = 25;
    case 'J32'
        node1 = 4;
        node2 = 27;
    case 'J33'
        node1 = 27;
        node2 = 11;
    case 'J34'
        node1 = 11;
        node2 = 15;
    case 'J35'
        node1 = 15;
        node2 = 20;
    case 'J36'
        node1 = 20;
        node2 = 21;

end

三、运行结果

四、备注

版本：2014a
完整代码或代写加1564658423