首先，在V_mV_d的范围内对速度进行采样。 allowable _ v=generate window (robotv，robotModel ) allowable_w=generatewindow ) robotw， robotModel (并且，根据是否可以立即制动消除不安全的速度，使用foreachvinallowable _ vforeachwinallowable _ wd ist=find _ dist (如果有的话) robot model (break dist=calculatebreakingdistance ) v ) /制动距离if ) distbreakdist ) /如果能够及时地制动，则其相对速度可以接受，这里
资料来源： http 3360//Adrian Boeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.htmlbegindwa (robot model (lasers can=readscanner ) allowable_v=generatewindow robotmodel ) allowable _ w=generate window robot model (foreachvinallowable _ vforeachwinallowable _ wd ist=find _ dist ) robot model (break dist=calculatebreakingdistance ) v ) if ) distbreakdist/canstopintimeheading=h diff ) robotpose， ganstopintimeheading(hdiff ) robotpose //clearance与原论文略有不同，clearance=(dist-breakdist )/(dmax - breakDist ) cost ABS(desired_v-v ) ) if ) costoptimal ) best _ v=VB est _ w=w optimal=costsetrobottrajectorytobest _ v，best _ best
(转载请注明作者和出处。 http://blog.csdn.net/heyijia0327不允许用于商业用途)
参考：
dwa :
1 .福克斯. 《The Dynamic Window Approach To CollisionAvoidance》
2.MarijaSeder. 《dynamic window based approach tomobile robot motion control in the presence of moving obstacles》
3.http://Adrian Boeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html
运动模型：
4.http://Adrian Boeing.blogspot.com.au/2010/09/circular-motion-in-2d-for-graphics-and.html
5.https://www.cs.Princeton.edu/courses/archive/fall 11/cos 495/cos 495-lecture5- odometry.pdf
6.http://rossum.source forge.net/papers/diff steer /