ros 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