简介
在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)算法的思想如下:
- 在控制空间中离散采样多组速度
dx,dy,dtheta
- 对每个采样的速度向量
dx,dy,dtheta
,模拟机器人在这种速度下,预测前进一个或者多个采样时间段机器的行走轨迹以及会发生什么。 - 对每个前进预测进行分析打分,
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