DWA算法详解

 
 

   
   

    
    

   
   
   
   

    
    

     
     首先在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/