-
首先在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)
//如果能够及时刹车,该对速度可接收
-
如果这组速度可接受,接下来利用评价函数对其评价,找到最优的速度组
![](https://i-blog.csdnimg.cn/blog_migrate/6ec1254e1935c48d6218c8ef38ecef3c.png)
-
来源: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
![](https://i-blog.csdnimg.cn/blog_migrate/76b0a9cd9a5277a19dcfaa72a90a7938.png)
(转载请注明作者和出处:http://blog.csdn.net/heyijia0327 未经允许请勿用于商业用途)
参考:
dwa:
1.Fox.《The Dynamic Window Approach To CollisionAvoidance》
2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》
3.http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
运动模型:
4. http://adrianboeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html
5.https://www.cs.princeton.edu/courses/archive/fall11/cos495/COS495-Lecture5-Odometry.pdf
6.http://rossum.sourceforge.net/papers/DiffSteer/