路径规划: 局部路径规划 - 动态窗口法(dynamic window approach)

简介

在ROS中有很多种局部路径的规划方法,这里来介绍动态窗口法(dynamic window approach)动态窗口法是 Dieter Fox, Wolfram Burgard, 和 Sebastian Thrun在1997年提出的”The dynamic window approach to collision avoidance“,一种在线避障策略,该策略将栅格地图中planner生成的运动轨迹,代入一个函数(value function)计算出dx,dy,dtheta的速度发送给底层。

算法

动态窗口Dynamic Window Approach (DWA)算法的思想如下:
  1. 在控制空间中离散采样多组速度dx,dy,dtheta
  2. 对每个采样的速度向量dx,dy,dtheta,模拟机器人在这种速度下,预测前进一个或者多个采样时间段机器的行走轨迹以及会发生什么。
  3. 对每个前进预测进行分析打分,

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

参考资料

http://wiki.ros.org/dwa_local_planner
http://adrianboeing.blogspot.hk/2012/05/dynamic-window-algorithm-motion.html

转自:https://blog.csdn.net/DinnerHowe/article/details/80107480

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页