机器人局部避障的动态窗口法(dynamic window approach) （转）
源：机器人局部避障的动态窗口法(dynamic window approach)
首先在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) 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仿真代码：
% ------------------------------------------------------------------------- % % 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;
