# 机器人局部避障的动态窗口法(dynamic window approach)

15 篇文章 137 订阅

首先在V_m∩V_d的范围内采样速度：
allowable_v = generateWindow(robotV, robotModel)
allowable_w  = generateWindow(robotW, robotModel)

for each v in allowable_v
for each w in allowable_w
dist = find_dist(v,w,laserscan,robotModel)
breakDist = calculateBreakingDistance(v)//刹车距离
if (dist > breakDist)  //如果能够及时刹车，该对速度可接收
如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组


来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
BEGIN DWA(robotPose,robotGoal,robotModel)
allowable_v = generateWindow(robotV, robotModel)
allowable_w  = generateWindow(robotW, robotModel)
for each v in allowable_v
for each w in allowable_w
dist = find_dist(v,w,laserscan,robotModel)
breakDist = calculateBreakingDistance(v)
if (dist > breakDist)  //can stop in time
//clearance与原论文稍不一样
clearance = (dist-breakDist)/(dmax - breakDist)
cost = costFunction(heading,clearance, abs(desired_v - v))
if (cost > optimal)
best_v = v
best_w = w
optimal = cost
set robot trajectory to best_v, best_w
END


#### （转载请注明作者和出处：http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途）

dwa：

1.Fox.《The Dynamic Window Approach To CollisionAvoidance》

2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》

% -------------------------------------------------------------------------
%
% File : DynamicWindowApproachSample.m
%
% Discription : Mobile Robot Motion Planning with Dynamic Window Approach
%
% Environment : Matlab
%
% Author : Atsushi Sakai
%
% Copyright (c): 2014 Atsushi Sakai
%
% -------------------------------------------------------------------------

function [] = DynamicWindowApproachSample()

close all;
clear all;

disp('Dynamic Window Approach sample program start!!')

goal=[10,10];% 目标点位置 [x(m),y(m)]
% 障碍物位置列表 [x(m) y(m)]
% obstacle=[0 2;
%           4 2;
%           4 4;
%           5 4;
%           5 5;
%           5 6;
%           5 9
%           8 8
%           8 9
%           7 9];
obstacle=[0 2;
4 2;
4 4;
5 4;
5 5;
5 6;
5 9
8 8
8 9
7 9
6 5
6 3
6 8
6 7
7 4
9 8
9 11
9 6];

obstacleR=0.5;% 冲突判定用的障碍物半径
global dt; dt=0.1;% 时间[s]

% 机器人运动学模型

evalParam=[0.05,0.2,0.1,3.0];
area=[-1 11 -1 11];% 模拟区域范围 [xmin xmax ymin ymax]

% 模拟实验的结果
result.x=[];
tic;
% movcount=0;
% Main loop
for i=1:5000
% DWA参数输入
[u,traj]=DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR);
x=f(x,u);% 机器人移动到下一个时刻

% 模拟结果的保存
result.x=[result.x; x'];

% 是否到达目的地
if norm(x(1:2)-goal')<0.5
disp('Arrive Goal!!');break;
end

%====Animation====
hold off;
ArrowLength=0.5;%
% 机器人
quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');hold on;
plot(result.x(:,1),result.x(:,2),'-b');hold on;
plot(goal(1),goal(2),'*r');hold on;
plot(obstacle(:,1),obstacle(:,2),'*k');hold on;
% 探索轨迹
if ~isempty(traj)
for it=1:length(traj(:,1))/5
ind=1+(it-1)*5;
plot(traj(ind,:),traj(ind+1,:),'-g');hold on;
end
end
axis(area);
grid on;
drawnow;
%movcount=movcount+1;
%mov(movcount) = getframe(gcf);%
end
toc
%movie2avi(mov,'movie.avi');

function [u,trajDB]=DynamicWindowApproach(x,model,goal,evalParam,ob,R)

% Dynamic Window [vmin,vmax,wmin,wmax]
Vr=CalcDynamicWindow(x,model);

% 评价函数的计算
[evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam);

if isempty(evalDB)
disp('no path to goal!!');
u=[0;0];return;
end

% 各评价函数正则化
evalDB=NormalizeEval(evalDB);

% 最终评价函数的计算
feval=[];
for id=1:length(evalDB(:,1))
feval=[feval;evalParam(1:3)*evalDB(id,3:5)'];
end
evalDB=[evalDB feval];

[maxv,ind]=max(feval);% 最优评价函数
u=evalDB(ind,1:2)';%

function [evalDB,trajDB]=Evaluation(x,Vr,goal,ob,R,model,evalParam)
%
evalDB=[];
trajDB=[];
for vt=Vr(1):model(5):Vr(2)
for ot=Vr(3):model(6):Vr(4)
% 轨迹推测; 得到 xt: 机器人向前运动后的预测位姿; traj: 当前时刻 到 预测时刻之间的轨迹
[xt,traj]=GenerateTrajectory(x,vt,ot,evalParam(4),model);  %evalParam(4),前向模拟时间;
% 各评价函数的计算
dist=CalcDistEval(xt,ob,R);
vel=abs(vt);
% 制动距离的计算
stopDist=CalcBreakingDist(vel,model);
if dist>stopDist %
trajDB=[trajDB;traj];
end
end
end

function EvalDB=NormalizeEval(EvalDB)
% 评价函数正则化
if sum(EvalDB(:,3))~=0
EvalDB(:,3)=EvalDB(:,3)/sum(EvalDB(:,3));
end
if sum(EvalDB(:,4))~=0
EvalDB(:,4)=EvalDB(:,4)/sum(EvalDB(:,4));
end
if sum(EvalDB(:,5))~=0
EvalDB(:,5)=EvalDB(:,5)/sum(EvalDB(:,5));
end

function [x,traj]=GenerateTrajectory(x,vt,ot,evaldt,model)
% 轨迹生成函数
% evaldt：前向模拟时间; vt、ot当前速度和角速度;
global dt;
time=0;
u=[vt;ot];% 输入值
traj=x;% 机器人轨迹
while time<=evaldt
time=time+dt;% 时间更新
x=f(x,u);% 运动更新
traj=[traj x];
end

function stopDist=CalcBreakingDist(vel,model)
% 根据运动学模型计算制动距离,这个制动距离并没有考虑旋转速度，不精确吧！！！
global dt;
stopDist=0;
while vel>0
stopDist=stopDist+vel*dt;% 制动距离的计算
vel=vel-model(3)*dt;%
end

function dist=CalcDistEval(x,ob,R)
% 障碍物距离评价函数

dist=100;
for io=1:length(ob(:,1))
disttmp=norm(ob(io,:)-x(1:2)')-R;%僷僗偺埵抲偲忈奞暔偲偺僲儖儉岆嵎傪寁嶼
if dist>disttmp% 离障碍物最小的距离
dist=disttmp;
end
end

% 障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重
if dist>=2*R
dist=2*R;
end

theta=toDegree(x(3));% 机器人朝向
goalTheta=toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目标点的方位

if goalTheta>theta
targetTheta=goalTheta-theta;% [deg]
else
targetTheta=theta-goalTheta;% [deg]
end

function Vr=CalcDynamicWindow(x,model)
%
global dt;
% 车子速度的最大最小范围
Vs=[0 model(1) -model(2) model(2)];

% 根据当前速度以及加速度限制计算的动态窗口
Vd=[x(4)-model(3)*dt x(4)+model(3)*dt x(5)-model(4)*dt x(5)+model(4)*dt];

% 最终的Dynamic Window
Vtmp=[Vs;Vd];
Vr=[max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))];

function x = f(x, u)
% Motion Model
% u = [vt; wt];当前时刻的速度、角速度
global dt;

F = [1 0 0 0 0
0 1 0 0 0
0 0 1 0 0
0 0 0 0 0
0 0 0 0 0];

B = [dt*cos(x(3)) 0
dt*sin(x(3)) 0
0 dt
1 0
0 1];

x= F*x+B*u;

degree = radian/pi*180;

• 228
点赞
• 1342
收藏
觉得还不错? 一键收藏
• 64
评论
04-04 1710
07-22 9058
06-19 487
11-15
01-15
07-18 1万+
12-19 641
08-30 2199

### “相关推荐”对你有帮助么？

• 非常没帮助
• 没帮助
• 一般
• 有帮助
• 非常有帮助

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。