机器人动态窗方法

[cpp] view
plaincopy





首先在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) //如果能够及时刹车，该对速度可接收

如果这组速度可接受，接下来利用评价函数对其评价，找到最优的速度组









[cpp] view
plaincopy





来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html

BEGIN DWA(robotPose,robotGoal,robotModel)

laserscan = readScanner()

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

heading = hDiff(robotPose,goalPose, v,w)

//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》

3.http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html



运动模型：

4. http://adrianboeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html
5.https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf

6.http://rossum.sourceforge.net/papers/DiffSteer/

最后贴出matlab仿真代码：

[python] view
plaincopy





% -------------------------------------------------------------------------

%

% File : DynamicWindowApproachSample.m

%

% Discription : Mobile Robot Motion Planning with Dynamic Window Approach

%

% Environment : Matlab

%

% Author : Atsushi Sakai

%

% Copyright (c): 2014 Atsushi Sakai

%

% License : Modified BSD Software License Agreement

% -------------------------------------------------------------------------



function [] = DynamicWindowApproachSample()



close all;

clear all;



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



x=[0 0 pi/2 0 0]';% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)]

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]



% 机器人运动学模型

% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss],

% 速度分辨率[m/s],转速分辨率[rad/s]]

Kinematic=[1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)];



% 评价函数参数 [heading,dist,velocity,predictDT]

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),前向模拟时间;

% 各评价函数的计算

heading=CalcHeadingEval(xt,goal);

dist=CalcDistEval(xt,ob,R);

vel=abs(vt);

% 制动距离的计算

stopDist=CalcBreakingDist(vel,model);

if dist>stopDist %

evalDB=[evalDB;[vt ot heading dist vel]];

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



function heading=CalcHeadingEval(x,goal)

% heading的评价函数计算



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



heading=180-targetTheta;



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;



function radian = toRadian(degree)

% degree to radian

radian = degree/180*pi;



function degree = toDegree(radian)

% radian to degree

degree = radian/pi*180;