机器人局部避障的动态窗口法(dynamic window approach)_seder, marija; petrovi膰, ivan. dynamic window base

作者：木道寻08 | 2024-08-08 14:46:31

踩

seder, marija; petrovi膰, ivan. dynamic window based approach to mobile ro

转自：http://blog.csdn.net/heyijia0327/article/details/44983551

[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;  

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/木道寻08/article/detail/948746