Dynamic Window Apporach
- 概念：动态窗口方法的基本思想是根据机器人运动模型在速度空间（V，ω）中采样多组速度，创建一个具有所有可行速度的动态窗口，预测机器人在时间t中的每组速度（v，ω）潜在轨迹，使用特定的评估功能同时评估集合的轨迹，并执行具有最高评估的集合，直到下一个执行。
DWA（Dynamic Window Approach）算法是一种用于机器人局部路径规划的经典实时避障算法。它通过评估在动态环境中机器人可能的速度和方向选择（线速度与角速度的组合），寻找一条最优轨迹，使机器人沿着该轨迹移动到目标点，同时避免碰撞。
- 核心思想：机器人通过短时间内的运动模拟，预测自身轨迹，评估不同速度组合在短时间内的效果，然后选择最优的速度组合（线速度和角速度）作为下一时刻的控制命令。
DWA算法的输入
机器人当前的状态：
- 当前的线速度 ${v}$
- 当前的角速度 ${\omega}$
- 当前的位置和方向${(x,y,\theta)}$
机器人的运动学约束：
- 最大和最小的线速度：${v_{min} \leq v \leq v_{max}}$
- 最大和最小的角速度：${\omega_{min} \leq \omega \leq \omega_{max} }$
- 最大线加速度和角加速度：${a_v}$ 和 ${a_\omega}$
目标位置：
- 目标点的坐标${(x_{goal}, y_{goal)}}$
障碍物信息：
- 障碍物的环境地图（通过雷达或摄像头获得环境中障碍物的分布）。
机器人动态模型：
- 模拟机器人在短时间内的运动轨迹。
DWA算法的主要步骤
1. 确定动态窗口
动态窗口是指在当前时刻内，根据机器人的运动学约束和动态限制划定可能的速度范围：
$$
v_{range} = [v_{min}, v_{max}] \cap [v - a_v \cdot \Delta t, v + a_v \cdot \Delta t]\
$$
$$
\omega_{range} = [\omega_{min}, \omega_{max}] \cap [\omega - a_\omega \cdot \Delta t, \omega + a_\omega \cdot \Delta t]
$$
其中，${[v - a_v \cdot \Delta t, v + a_v \cdot \Delta t]}$和${[\omega - a_\omega \cdot \Delta t, \omega + a_\omega \cdot \Delta t]}$是根据加速度限制计算的瞬时速度范围。
动态窗口比单纯使用机器人物理限制更实际，因为它考虑了短时间内速度的动态变化范围。
2. 基于速度采样模拟轨迹
在动态窗口内，对可能的速度组合（线速度 ${v}$和角速度 ${\omega}$ 进行采样，生成轨迹。轨迹的预测基于机器人运动模型，例如：
假设机器人当前的状态为位置 ${(x, y, \theta)}$，一个时间间隔 ( T ) 的预测轨迹可以通过如下公式模拟：
$$
x’ = x + v \cdot \cos(\theta) \cdot T
$$
$$
y’ = y + v \cdot \sin(\theta) \cdot T
$$
$$
\theta’ = \theta + \omega \cdot T
$$
3. 轨迹评估
传统 DWA 算法中对速度对（v, w，即线速度和角速度）的选择，依赖一个**评价函数（objective function）**来评估每个候选速度对轨迹的优劣。算法流程就是在速度空间内采样可行的速度对，并用评价函数给它们打分，最终选择分数最高的速度对来执行。
3.评价函数的表达式
3.1. 经典形式（Fox et al. 1997 原论文）：
原始论文中的 DWA 评价函数一般为如下加权和形式：
$$
G(v, w) = \alpha \cdot heading(v, w) + \beta \cdot dist(v, w) + \gamma \cdot vel(v, w)
$$
其中：
- ${v}$ ：线速度
- ${\omega}$：角速度
- ${\alpha, \beta, \gamma}$：权重系数（用于平衡各项重要性）
3.2. 各项含义解释
- heading(v, w)：朝向目标的得分
- 描述当前轨迹末端（模拟一小段时间后的位置）与目标点之间的距离或方向误差。
- 目的是鼓励轨迹朝向目标前进。
- 通常值越大，表示末端越靠近目标朝向。
具体公式：
$$
heading(v,\omega)=\pi-|\theta_g - \theta_t|
$$
- ${\theta_g}$ : 机器人当前点指向目标点的连线和x轴之间的角度
- ${\theta_t}$: 机器人在预测轨迹的末端（目标点）的姿态角与水平线/x轴之间的夹角
- dist(v, w)：距离障碍物的得分
- 评估沿着当前轨迹行进时，能保持多大的安全距离（离障碍物最近的距离）。
- 目的是让轨迹尽可能远离障碍物，提高安全性。
- 可以定义为轨迹上离障碍物的最近距离，或与安全距离的差值。
具体公式：（这里归一化了，防止过于高）,距离最近的障碍物的距离越大越好
$$
dist(v,w)=min(1, \frac{d_{min}}{d_{safe}})
$$
- ${d_{min}}$：轨迹中离障碍物最近的距离.
- ${d_{safe}}$：定义的安全距离（如 0.5 米，得分最大为1）
- vel(v, w)：速度得分
- 对当前速度分量 v 的奖励，偏好较高的行进速度加快到达目标。
- 通常直接用 (v) 或用 ${\frac{v}{v_{max}}}$归一化。
3.3. 综合
将这三项做加权和，得到的得分越高表示该速度对优先级越高。
论文原文公式参考
原文给出：
“The value function G(v,ω) for each sampled (v,ω) pair is calculated as a weighted sum of the following three criteria:
- heading(v,ω): The difference between the robot’s heading and the direction to the goal after a short forward simulation,
- dist(v,ω): The minimum distance to the nearest obstacle along the trajectory generated by (v,ω),
- vel(v,ω): The forward velocity (encouraging faster trajectories).”
传统 DWA 的评价函数是目标朝向、避障安全和速度的加权求和，这三者在决策速度和转向时作为优先级评估基础。
4. 选择最优速度
在所有的速度采样中，选择得分最高的速度组合（线速度${v}$ 和角速度${ \omega}$），作为当前时刻的控制命令。
5. 更新状态，循环执行
根据选取的速度组合更新机器人的运动状态，重复上述过程，直至机器人到达目标点。
DWA算法的优缺点
优点：
- 实时性强：DWA以短时间窗口内的动态信息进行评估，适合实时路径规划。
- 高效：通过动态窗口限制搜索范围，大大减少了计算量。
- 普遍适用性：适用于具有速度和加速度约束的机器人，易于部署。
- 灵活性：可以权衡多种评价指标（如速度、目标距离、障碍物距离），适应不同场景。
缺点：
- 局部最优问题：因只关注短时间内的轨迹，可能导致机器人陷入局部最优（如某些复杂障碍物死角）。
- 对参数敏感：权重参数的设置对性能影响很大，调整不当会导致效果欠佳。
- 不适应全局路径规划：DWA本质是局部路径规划方法，如缺乏合理的全局规划指引，可能偏离全局最优路径。
- 复杂障碍物环境适应性较差：在障碍物密集或动态障碍物较多的场景下，DWA可能存在效率和安全性问题。
总结
传统DWA通过动态窗口对速度空间进行约束，对每种可能的速度组合进行评估并选出最优速度，适合于动态实时机器人局部规划任务。然而，由于其是短时间范围的局部优化方法，容易受到局部最优、复杂障碍环境等问题影响。