一、简介
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
