• 一般运动方程

  

  Figure 1: 一般运动方程

由于速度不能直接设置，使用加速度进一步得到：

这里只依赖初始速度和位姿的加速度。我们假定一个时间片段内加速度是相同的，那 么可以得到离散形式：

• 估计的运动方程 我们进一步估计，当我们取的时间片段足够小的时候，可以认为速度是常数，这样可 以得到：

  

  Figure 4: 均匀速度

  进一步可以得到：

  

  Figure 5: 线速度和角速度

  

  Figure 6: 线速度和角速度

  

  Figure 7: 圆形方程

  可以看出，这是以线速度和角速度比值(也就是机器人行走的半径)为半径的，圆心为 (Mx,My)的一个圆。

确定速度窗口，根据三个步骤：

• 在当前位置，考虑各种速度（线速度和角速度），这些速度将构成一些列环形的轨迹。

• 可允许的速度：要保证该该速度形式能在最近的障碍物前停下来。

  

  Figure 8: 可允许的速度

• 动态窗口：以当前速度为中心，一个确定的加速度（该加速度有限制）和时间窗口， 可以确定这是时间片段内能达到的速度边界。

  

  Figure 9: 动态窗口

上面三种考虑得到的速度的集合就是最终的速度窗口。

上面根据各种条件确定了一个以当前速度为中心的速度窗口，如何选择这些速度呢？根 据一些约束条件，构建一个评价函数，对速度窗口里面的速度一次计算评价函数值，选 择评价函数值最大的速度。

评价函数怎么选择呢？

• 目标朝向：180-theta，theta是机器人方向与目标点的夹角。说明，偏角越小，该值 就越大。这一项保证了机器人能尽可能地往目标点前进，不至于在某个地方徘徊。
• 与最近障碍物的距离：每个速度都对应一条直线(角速度=0)或一个弧线，线条延长之 后可能会与障碍物相交，说明照此速度一直走就会撞到障碍物，这个距离越远说明越 安全。因此，距离越大，这个值越大。
• 速度大小：尽量选择速度比较大的，这样可能保证以最快的速度到达目标点。

每一项都有一个权重，代表他们的影响程度。 除此之外，我们还可以加上其他的一些因素，比如所得路径是否贴合全局路径等等。

• 当前速度的影响：当前的速度决定了能走的空间。比如在靠近障碍物的地方要转弯， 如果速度太快，可调的速度可能不能实现转弯。
• 加速度的影响：加速度是认为选择的，加速度越大，速度窗口就会越大，反之依然。