机器人局部避障的动态窗口法(dynamic window approach)

首先在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 未经允许请勿用于商业用途）

首先在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 未经允许请勿用于商业用途）