DWA算法原理

1 DWA算法基本原理

动态窗口法主要是在速度(v,w)空间中采样多组速度，并模拟机器人在这些速度下一定时间(sim_period)内的轨迹。

在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。

2 DWA算法实现流程

2.1 输入机器人原始状态

输入机器人的X、Y位置坐标，机器人朝向角，线速度，角速度

2.2 速度空间采样

速度空间 (v, w) 即机器人的速度范围，机器人的速度受到各种因素的限制

• 移动机器人受自身最大速度最小速度的限制

Vs 为机器人能够到达的所有矢量速度的集合；机器人受到最大最小线速度和角速度影响

• 移动机器人受电机性能的影响

由于加速度有一个范围限制，所以最大加速度或最大减速度一定时间内能达到的速度 ，才会被保留，表达式如下：

• 移动机器人受障碍的影响

为了能在碰到障碍物前停下来，在最大减速度的条件下，速度满足以下条件：

其中dist(v,w)为（v,w）对应的轨迹上离障碍物最近的距离

在上述 3 个约束条件下，速度空间 (v, w) 会有一定的范围，注意速度空间不是固定不变的，而是时变的，t 时刻的速度空间与 t + dt 时刻的速度空间是不同的，故将其称为动态窗口

得到 t 时刻的速度空间 (v, w) 后，以一定的分辨率对速度 v 和角速度 w 进行采样，分析机器人的运动学模型，根据采样的速度 (v, w) 模拟机器人的运动轨迹

DWA 算法适用于差速运动模型和全向运动模型，并不适用于阿克曼模型

差速运动模型

机器人只能向前运动或者旋转，图中有世界坐标系和机器人坐标系，下式中 v(t) 指机器人坐标系中 x 方向的速度

全向移动模型

在全向移动模型中，多了机器人坐标系下 y 方向的速度

令 Vy(t) = 0，则退化为差速运动模型，因此 ROS 自带的局部路径规划器使用上式计算

2.3 轨迹预测

采样时刻机器人坐标与坐标变化求和，得到下一时刻机器人坐标：

 即：

动态窗口采样的轨迹如下图所示：

2.4 轨迹评价

对速度空间进行采样后，根据机器人运动学模型能够预测出多条轨迹，需要对这些轨迹进行评价，选取最优的轨迹，机器人根据最优轨迹对应的速度进行运动

其中方向角评价函数：

其中  θgoal​是机器人在模拟轨迹末端和终点连线与x轴的夹角， θ * 是机器人在模拟轨迹末端的姿态角，旨在选出目标角度与指向角度间差值较小的轨迹，使机器人的前进方向对准终点。

障碍物评价函数：

旨在选出与障碍物距离较远的轨迹，避免机器人运行过程中发生碰撞， d 表示预测轨迹与障碍物的最近距离；d0​为障碍物评分最大值，若预测轨迹与障碍物间距超过  d0​则认为该轨迹安全；  dsafe​是机器人与障碍的安全间距，若预测轨迹与障碍物间距小于dsafe​则认为该轨迹发生碰撞。

速度评价函数：

旨在选出线速度较大的轨迹，使机器人的运动速度尽量快。

α、β 和 γ \gamma γ 均为评价函数的系数。由于局部路径规划的过程需要多传感器的采集, 采集信息无法做到连续, 这样也会使得评价后差别较大, 所以可以进行归一化处理（平滑处理）， 其中 σ \sigma σ 表示归一化。

• 归一化处理过程如下式 (6) 所示:

• 

• 其中， i i i 代表第 i i i条模拟轨迹， n n n 为约束条件下的全部采样轨迹总数。由上述公式 (5) 和公式 (6) 可以得出一条满足避开障碍物并朝着目标点快速行进的路径， 使得机器人完成局部路径最优规划。

综上所述，评价函数的物理意义是使机器人以较快的速度朝着目标运动并进行自主避障，实际应用中可根据需要改变评价函数及其权重系数

参考：https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf

局部路径规划 DWA 算法完全解析（理论推导+代码实现，包你看懂！）-CSDN博客