在繁忙的智能仓库里，机器人正有条不紊地执行着搬运任务。这里货架林立，货物堆积如山，叉车往来穿梭，地面上还散落着一些临时放置的工具。一台小巧灵活的移动机器人，肩负着将特定货物从角落搬运至出货口的重任。只见它以稳定的速度朝着目标前进，突然，前方不远处一辆叉车急速转弯，挡住了它的去路。机器人没有丝毫犹豫，瞬间调整方向，轻巧地侧身绕过叉车，继续前行。没走多远，又遇到了几个工作人员正在搬运大型货物，占据了大半通道。机器人迅速判断形势，以精准的角度和速度，在狭窄的缝隙中蜿蜒穿行，成功避开了障碍，最终稳稳地将货物送达出货口。​
这一系列行云流水般的避障操作，背后究竟隐藏着怎样的智慧？是什么样的神奇算法，赋予了机器人如此敏锐的环境感知和精准的决策能力？其实，这其中很大一部分功劳要归于动态窗口法。它就像机器人避障的智能
“导航仪”，指引着机器人在复杂多变的环境中畅行无阻。那么，动态窗口法究竟是如何运作的，才能让机器人实现如此精准的避障操作呢？让我们一同深入探索其中的奥秘。

定义及发展

动态窗口法，全称为 Dynamic Window Approach，是一种应用于机器人路径规划与运动控制的经典算法。它聚焦于机器人当下的速度、加速度等动态参数，在速度空间内构建一个随时间实时变化的 “窗口”。这个窗口涵盖了机器人在当前时刻，基于自身运动学与动力学约束，能够安全且合理达到的速度集合。在每一个控制周期里，算法都会对动态窗口内的众多速度组合进行评估，从中筛选出最优解，以此驱动机器人运动，使其既能朝着目标点稳步前进，又能巧妙避开途中的各类障碍物。

在机器人路径规划领域，动态窗口法占据着举足轻重的地位。机器人在实际运行时，所处环境复杂多变，可能遭遇静止的障碍物，如墙壁、固定设备，也会面临动态的干扰，像移动的行人、车辆。动态窗口法凭借其对机器人动态特性的精准把握，以及实时调整运动策略的能力，为机器人在复杂环境中规划出安全、高效的行进路径，堪称路径规划领域的中流砥柱。

动态窗口法的起源可追溯到上世纪末，彼时机器人技术蓬勃发展，对路径规划算法的实时性与可靠性提出了更高要求。早期的路径规划算法，如 A * 算法，虽能在静态环境中寻找到理论上的最优路径，但面对动态变化的场景，其计算量过大、响应迟缓的弊端逐渐暴露。在此背景下，科研人员开始探索新的算法思路。1997 年，Dietrich Fox、Wolfram Burgard 和 Sebastian Thrun 等学者正式提出动态窗口法，该算法一经问世，便凭借其在动态环境下的出色表现，迅速引起学界与工业界的广泛关注。随后的几十年间，众多研究团队投身于动态窗口法的优化与拓展研究。从最初仅考虑机器人的运动学约束，到逐渐融入动力学因素；从简单的二维平面应用，拓展至复杂的三维空间场景；从单一机器人的路径规划，发展到多机器人协作的路径协调。动态窗口法在不断的改进与创新中，持续焕发出新的活力，为机器人技术的发展注入源源不断的动力。

核心原理

速度空间

机器人在执行任务过程中，其速度并非毫无限制，速度限制可分为线速度限制与角速度限制，这两者在动态窗口法中起着关键作用。线速度 $v$ 体现了机器人在直线方向上的移动快慢，而角速度 $\omega$ 则反映了机器人绕自身轴转动的速度。受限于机器人的硬件性能、机械结构以及所执行任务的安全需求，线速度存在最大值 $v_{max}$ 与最小值 $v_{min}$ ，角速度同样存在最大值 ${\omega }_{max}$ 与最小值 ${\omega }_{min}$ 。例如，室内服务机器人为避免碰撞人员或家具，其线速度一般不会超过 $1m/s$ ，即 $v_{max}=1m/s$ ，且为了保证移动的稳定性，最小线速度也会设定在 $0.1m/s$ 左右，即 $v_{min}=0.1m/s$ ；对于角速度而言，在狭窄空间内转弯时，其最大角速度可能限制在 $1rad/s$ ，即 ${\omega }_{max}=1rad/s$ ，最小角速度接近 $0rad/s$ ，但考虑到电机控制精度等因素， ${\omega }_{min}$ 通常会设定一个极小值，如 $0.01rad/s$ 。

加速度限制同样不容忽视，它对速度空间的动态变化有着深刻影响。加速度分为线加速度 $a$ 和角加速度 $\alpha$ ，它们决定了机器人速度变化的快慢。在每个控制周期内，机器人的速度变化需遵循加速度限制。以线加速度为例，机器人当前时刻的线速度为 $v(t)$ ，在经过一个时间间隔 $\Delta t$ 后，其线速度 $v(t+\Delta t)$ 的变化范围需满足：

$v(t)+a_{min}\Delta t\le v(t+\Delta t)\le v(t)+a_{max}\Delta t$

其中 $a_{min}$ 和 $a_{max}$ 分别为最小线加速度和最大线加速度。类似地，对于角速度，当前时刻的角速度为 $\omega (t)$ ，经过 $\Delta t$ 后，其角速度 $\omega (t+\Delta t)$ 的变化范围为：

$\omega (t)+{\alpha }_{min}\Delta t\le \omega (t+\Delta t)\le \omega (t)+{\alpha }_{max}\Delta t$

其中 ${\alpha }_{min}$ 和 ${\alpha }_{max}$ 分别为最小角加速度和最大角加速度。

基于上述速度与加速度限制，动态窗口范围得以确定。在速度空间中，动态窗口可表示为一个二维区域，横坐标代表线速度 $v$ ，纵坐标代表角速度 $\omega$ 。在当前时刻 $t$ ，根据加速度限制，动态窗口的线速度范围下限 $v_{min}^{\prime }$为：

$v_{min}^{\prime }=\max (v(t)+a_{min}\Delta t,v_{min})$

上限 $v_{max}^{\prime }$ 为：$v_{max}^{\prime }=\min (v(t)+a_{max}\Delta t,v_{max})$

角速度范围下限 ${\omega }_{min}^{\prime }$ 为：${\omega }_{min}^{\prime }=\max (\omega (t)+{\alpha }_{min}\Delta t,{\omega }_{min})$

上限 ${\omega }_{max}^{\prime }$ 为： ${\omega }_{max}^{\prime }=\min (\omega (t)+{\alpha }_{max}\Delta t,{\omega }_{max})$

如此，这个由 $[v_{min}^{\prime },v_{max}^{\prime }]\times [{\omega }_{min}^{\prime },{\omega }_{max}^{\prime }]$ 所界定的区域，便是机器人在当前时刻能够安全且合理达到的速度范围，即动态窗口范围。在后续的轨迹生成与避障决策过程中，都将在这个动态窗口内进行相关操作。

轨迹生成

在动态窗口法中，轨迹生成的首要步骤是在动态窗口内对速度进行采样。采样方法通常采用均匀采样或随机采样。均匀采样是按照一定的线速度间隔 $\Delta v$ 和角速度间隔 $\Delta \omega$ 在动态窗口 $[v_{min}^{\prime },v_{max}^{\prime }]\times [{\omega }_{min}^{\prime },{\omega }_{max}^{\prime }]$ 内进行取值。例如，若线速度范围是 $[0.1m/s,1m/s]$ ，设定间隔 $\Delta v=0.1m/s$ ，则会取 $0.1m/s$ 、 $0.2m/s$ 、 $0.3m/s$ 等一系列值；对于角速度范围 $[-1rad/s,1rad/s]$ ，若间隔 $\Delta \omega =0.2rad/s$ ，则会取 $-1rad/s$ 、 $-0.8rad/s$ 、 $-0.6rad/s$ 等数值。这种均匀采样的依据在于其能够全面且均衡地覆盖动态窗口内的速度组合空间，确保在不同速度区间都有候选速度用于轨迹计算，为后续筛选最优轨迹提供丰富的数据基础。随机采样则是在动态窗口范围内随机生成线速度和角速度组合，其优势在于能够快速探索速度空间，尤其适用于对计算效率要求较高且对速度空间覆盖均匀性要求相对较低的场景。

基于采样得到的速度组合，接下来需依据机器人运动学模型计算未来一段时间内的运动轨迹。以常见的差动驱动机器人为例，其运动学模型可由以下方程描述：

$x_{t+1}=x_t+v_t\cos ({\theta }_t)\Delta t$
$y_{t+1}=y_t+v_t\sin ({\theta }_t)\Delta t$
${\theta }_{t+1}={\theta }_t+{\omega }_t\Delta t$

其中 $(x_t,y_t,{\theta }_t)$ 为机器人在时刻 $t$ 的位置和姿态（$x$、$y$ 为平面坐标，$\theta$ 为航向角）， $v_t$ 和 ${\omega }_t$ 分别是时刻 $t$ 的线速度和角速度， $\Delta t$ 为时间间隔。

具体计算步骤如下：假设在某一时刻 $t$ ，采样得到的线速度为 $v_{sample}$ ，角速度为 ${\omega }_{sample}$ ，机器人当前位置为 $(x_0,y_0)$ ，航向角为 ${\theta }_0$ 。首先，根据上述方程计算经过第一个时间间隔 $\Delta t$ 后的位置和姿态。

$x_1=x_0+v_{sample}\cos ({\theta }_0)\Delta t$
$y_1=y_0+v_{sample}\sin ({\theta }_0)\Delta t$
${\theta }_1={\theta }_0+{\omega }_{sample}\Delta t$

得到新位置 $(x_1,y_1)$ 和新航向角 ${\theta }_1$ 后，将其作为下一个计算周期的起始条件，再次代入运动学方程，计算经过下一个时间间隔 $\Delta t$ 后的位置和姿态，即：

$x_2=x_1+v_{sample}\cos ({\theta }_1)\Delta t$
$y_2=y_1+v_{sample}\sin ({\theta }_1)\Delta t$
${\theta }_2={\theta }_1+{\omega }_{sample}\Delta t$

以此类推，通过多次迭代计算，就能得到在未来一段时间（假设为 $T$ ，迭代次数 $n=T/\Delta t$ ）内机器人基于采样速度的运动轨迹点序列 $\{(x_i,y_i,{\theta }_i){\}}_{i=0}^n$ 。这些轨迹将作为后续评价函数计算和最优轨迹选择的重要输入，用于指导机器人在避障过程中的实际运动方向与速度控制。

评价函数

• 距离目标评价
  计算轨迹终点与目标位置距离是衡量轨迹优劣的重要指标。假设目标位置坐标为 $(x_g,y_g)$ ，某条轨迹的终点坐标为 $(x_{end},y_{end})$ ，则两者之间的欧氏距离 $d_{goal}$ 可通过以下公式计算：

  $d_{goal}=\sqrt{(x_{end}-x_g{)}^2+(y_{end}-y_g{)}^2}$

  该距离的意义在于直观反映了机器人沿着此轨迹运动最终到达的位置与目标位置的接近程度。距离越小，表明机器人越接近目标，轨迹在趋近目标方面表现越好。为将此距离转化为评价得分 $s_{goal}$ ，通常采用反比例函数形式，例如：

  $s_{goal}=\frac{1}{1+d_{goal}}$

  这样设置的原因是，当 $d_{goal}$ 趋近于 $0$ 时， $s_{goal}$ 趋近于 $1$ ，表示轨迹终点与目标位置非常接近，得分高；随着 $d_{goal}$ 增大， $s_{goal}$ 逐渐减小，反映轨迹在趋近目标方面的表现变差。

• 障碍物距离评价
  检测轨迹与障碍物的最小距离，对于确保机器人避障安全至关重要。在实际操作中，首先需借助传感器（如激光雷达、超声波传感器等）获取障碍物信息，并构建环境地图。假设机器人周围存在多个障碍物，对于某条轨迹，其与障碍物的距离可通过计算轨迹上各点与障碍物表面各点之间的距离来确定，取其中的最小值作为该轨迹与障碍物的最小距离 $d_{obs}$ 。为计算障碍物距离评价得分，常利用指数函数，例如：

  $s_{obs}=e^{-k(d_{safe}-d_{obs})}$

  其中 $k$ 为常数，用于调整得分对距离的敏感程度， $d_{safe}$ 为预先设定的安全距离。当 $d_{obs}\ge d_{safe}$ 时，说明机器人与障碍物之间的距离在安全范围内， $s_{obs}$ 趋近于 $1$ ，得分较高； 当$d_{obs}<d_{safe}$ 时， $s_{obs}$ 随着 $d_{obs}$ 减小而迅速趋近于 $0$ ，表示机器人与障碍物距离过近，存在碰撞风险，得分低。通过这种方式，突出了轨迹避开障碍物的重要性。

• 运动平滑度评价
  考量轨迹的速度和转向变化可使机器人运动更加平稳，避免不必要的急加速、急减速和急转弯。具体计算指标和方式如下：
  对于速度变化，计算相邻时刻线速度的差值 $\Delta v_i$ 和角速度的差值 $\Delta {\omega }_i$ 。假设轨迹上有 $n$ 个时间点，线速度序列为 {v1,v2,⋯ ,vn}\{v_1, v_2, \cdots, v_n\}{v1​,v2​,⋯,vn​} ，角速度序列为 {ω1,ω2,⋯ ,ωn}\{\omega_1, \omega_2, \cdots, \omega_n\}{ω1​,ω2​,⋯,ωn​} ，则：

  $\Delta v_i=v_{i+1}-v_i,i=1,2,\cdots ,n-1$
  $\Delta {\omega }_i={\omega }_{i+1}-{\omega }_i,i=1,2,\cdots ,n-1$

  运动平滑度得分 $s_{smooth}$ 可通过对这些差值的平方和进行处理得到，例如：

  $s_{smooth}=\frac{1}{1+{\sum }_{i=1}^{n-1}(\Delta v_i^2+\Delta {\omega }_i^2)}$

  当速度和角速度变化平稳时， ${\sum }_{i=1}^{n-1}(\Delta v_i^2+\Delta {\omega }_i^2)$ 的值较小， $s_{smooth}$ 趋近于 $1$ ，得分高；反之，当速度和转向变化剧烈时，该值增大， $s_{smooth}$ 减小，反映轨迹的运动平滑度差。

• 综合评价得分
  综合评价得分 $S$ 通过将上述三个方面得分加权求和得到，公式为：

  $S=\alpha s_{goal}+\beta s_{obs}+\gamma s_{smooth}$

  其中 $\alpha$、$\beta$、$\gamma$ 为权重系数，且 $\alpha +\beta +\gamma =1$ 。权重系数的作用是根据不同场景的需求，调整各评价指标在综合评价中的相对重要性。例如，在空旷环境中，障碍物较少，可适当增大 $\alpha$ ，强调距离目标评价，使机器人更快速地趋近目标；而在障碍物密集的环境中，则增大 $\beta$ ，突出障碍物距离评价，确保机器人安全避障；若对机器人运动平稳性要求较高，如医疗服务机器人在病房内作业时，可适当提高 $\gamma$ 的值。权重系数的调整需通过实验和经验不断优化，以适应不同的应用场景和任务需求，从而使动态窗口法能在各种复杂环境下为机器人规划出最优运动轨迹。

动态窗口法在机器人避障中的运作流程

1. 环境感知与建模环节
   机器人要实现避障，首先需精准感知周围环境，这依赖于多种传感器。激光雷达是常用传感器之一，它通过发射激光束并测量反射光返回的时间，计算出机器人与周围物体的距离。例如，每秒可发射上万束激光，构建出周围环境的点云图，清晰呈现障碍物的位置和轮廓。摄像头则利用图像识别技术，通过对拍摄图像进行分析，识别出各类物体，如行人、墙壁、其他机器人等。它能够获取丰富的视觉信息，像物体的颜色、纹理等，为环境感知提供更多维度的数据支持。超声波传感器在近距离感知方面表现出色，其工作原理是发射超声波，根据声波反射回来的时间计算距离。当机器人靠近障碍物时，超声波传感器能快速检测到，并发出警报信号。

   获取传感器数据后，需将其转化为环境模型。以栅格地图构建为例，先将机器人所处环境划分为一个个大小相同的栅格。对于激光雷达数据，根据测量得到的距离信息，确定每个栅格是否被障碍物占据。若某个栅格内存在障碍物反射的激光点，则将该栅格标记为障碍物栅格；反之，标记为可通行栅格。摄像头数据则通过图像分割、目标检测等算法，识别出图像中的障碍物，并将其位置信息映射到栅格地图中。超声波传感器数据用于补充近距离的障碍物信息，确保地图的准确性。拓扑地图构建过程有所不同，它重点关注环境中的关键节点，如房间出入口、走廊交汇处等，以及这些节点之间的连接关系。通过分析传感器数据，确定机器人在不同位置之间的可达路径，形成拓扑结构，为机器人提供宏观的环境导航框架。

2. 动态窗口确定步骤
   依据机器人自身的速度和加速度限制来确定动态窗口。设机器人当前线速度为$v$，角速度为$\omega$，最大线加速度为$a_{vmax}$，最小线加速度为$a_{vmin}$，最大角加速度为$a_{\omega max}$，最小角加速度为$a_{\omega min}$，在一个控制周期$\Delta t$内，动态窗口的

   线速度范围下限$v_{min}^{\prime }$为：$v_{min}^{\prime }=\max (v+a_{vmin}\Delta t,v_{min})$
   上限$v_{max}^{\prime }$为：$v_{max}^{\prime }=\min (v+a_{vmax}\Delta t,v_{max})$
   角速度范围下限${\omega }_{min}^{\prime }$为：${\omega }_{min}^{\prime }=\max (\omega +a_{\omega min}\Delta t,{\omega }_{min})$
   上限${\omega }_{max}^{\prime }$为：${\omega }_{max}^{\prime }=\min (\omega +a_{\omega max}\Delta t,{\omega }_{max})$

   其中$v_{min}$和$v_{max}$是机器人线速度的硬件限制，${\omega }_{min}$和${\omega }_{max}$是角速度的硬件限制。

   在不同场景下，动态窗口范围变化明显。例如在空旷的大仓库中，机器人可高速行驶，线速度限制范围较大，如$v_{min}=0.5m/s$，$v_{max}=2m/s$，且加速度限制相对宽松，假设$a_{vmin}=-1m/s^2$，$a_{vmax}=1m/s^2$，当前线速度$v=1m/s$，则动态窗口线速度范围为$[0,2]m/s$（这里取整数值便于理解）。

   而在狭窄的货架通道内，为避免碰撞，线速度限制范围缩小，如$v_{min}=0.1m/s$，$v_{max}=0.5m/s$，加速度限制更严格，假设$a_{vmin}=-0.5m/s^2$，$a_{vmax}=0.5m/s^2$，当前线速度$v=0.3m/s$，此时动态窗口线速度范围变为$[0.05,0.5]m/s$，角速度范围也会因转弯空间受限而相应变化，以适应复杂环境下的安全行驶需求。

3. 避障决策过程
   在动态窗口内生成候选轨迹，通常采用均匀采样或随机采样方式。均匀采样时，按照设定的线速度间隔$\Delta v$和角速度间隔$\Delta \omega$在动态窗口$[v_{min}^{\prime },v_{max}^{\prime }]\times [{\omega }_{min}^{\prime },{\omega }_{max}^{\prime }]$内取值。例如，若线速度范围是$[0.1m/s,0.5m/s]$，间隔$\Delta v=0.1m/s$，角速度范围是$[-0.5rad/s,0.5rad/s]$，间隔$\Delta \omega =0.1rad/s$，则可得到众多速度组合，如$(0.1m/s,-0.5rad/s)$、$(0.1m/s,-0.4rad/s)$等。对于每个速度组合，根据机器人运动学模型计算未来一段时间内的运动轨迹。

   通过评价函数对候选轨迹进行评估。评价函数包含距离目标评价、障碍物距离评价和运动平滑度评价。距离目标评价计算轨迹终点与目标位置的欧氏距离$d_{goal}$，并转化为得分$s_{goal}=\frac{1}{1+d_{goal}}$。障碍物距离评价检测轨迹与障碍物的最小距离$d_{obs}$，通过指数函数$s_{obs}=e^{-k(d_{safe}-d_{obs})}$计算得分，其中$k$为常数，$d_{safe}$为安全距离。

   运动平滑度评价通过计算相邻时刻速度和角速度的差值平方和${\sum }_{i=1}^{n-1}(\Delta v_i^2+\Delta {\omega }_i^2)$，并转化为得分$s_{smooth}=\frac{1}{1+{\sum }_{i=1}^{n-1}(\Delta v_i^2+\Delta {\omega }_i^2)}$。综合评价得分$S=\alpha s_{goal}+\beta s_{obs}+\gamma s_{smooth}$，其中$\alpha$、$\beta$、$\gamma$为权重系数。通过比较各候选轨迹的综合评价得分，筛选出得分最高的轨迹，作为机器人避障的执行轨迹。

4. 实时调整机制
   机器人在运动过程中，持续接收新的环境信息，实时更新动态窗口。例如，当激光雷达检测到新出现的障碍物时，机器人的速度和加速度限制可能需要重新评估。若障碍物靠近机器人行驶路径，为确保安全，可能会减小线速度和角速度的最大限制值，从而缩小动态窗口范围。根据新的速度和加速度限制，按照动态窗口计算方法重新确定$v_{min}^{\prime }$、$v_{max}^{\prime }$、${\omega }_{min}^{\prime }$和${\omega }_{max}^{\prime }$。

   同时，避障策略也需相应调整。若新的环境信息表明原避障轨迹存在碰撞风险，机器人会重新生成候选轨迹，并再次通过评价函数进行评估。例如，原本规划的轨迹因新出现的障碍物导致与障碍物最小距离小于安全距离，此时机器人会调整速度采样范围，生成更多不同的候选轨迹，重新计算综合评价得分，选择新的最优轨迹，以确保持续有效避障，在复杂多变的环境中安全、稳定地完成任务。