动态窗口算法运动规划

image

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

动态窗口方法是一种基于速度的局部规划器，可计算达到目标所需的机器人的最佳无碰撞（“允许”）速度。它将笛卡尔目标（x，y）转换为移动机器人的速度（v，w）命令。

该算法有两个主要的步骤，计算有效的速度 搜索空间，并选择 最佳速度。搜索空间由一组能够产生安全的运动轨迹（即允许机器人在碰撞前停止）的速度构成，这些速度是机器人在其动态（“动态窗口”）的下一个时间片中可以实现的速度集。选择最佳速度以最大化机器人与障碍物的间隙，最大化速度并获得最接近目标的航向。

下面是DWA算法的伪代码：

BEGIN DWA(robotPose,robotGoal,robotModel)
   desiredV = calculateV(robotPose,robotGoal)
   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 = (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

代码解释

首先，我们可以根据当前位置（robotPose）和目的地（robotGoal）计算到目标的所需速度。（例如，如果我们离得很远就走得快，如果我们离得很近就慢。）
给定小车底盘再运动学上允许的速度（v，w）那么当前速度的allowable_v的范围受到加速度的制约应该为[v-at, v+at],其中a为加速度，t为时间片。角速度w也同样适用。
搜索所有允许的速度（v， w）。
对每一个速度进行碰撞检测。
确定到最近障碍物的距离是否在机器人停止距离内。如果机器人无法及时停止，则忽略此建议的机器人速度。
否则，速度是“可接受的”，因此我们现在可以计算目标函数所需的值。在我们的例子中，机器人前进和清除。
计算建议速度的“cost”。如果cost优于其他任何目标，请将此设置为我们的最佳选择。
最后，将机器人所需的轨迹设置为最佳建议速度。