DWA（动态窗口）算法是用于局部路径规划的算法，已经在ROS中实现，在move_base堆栈中：http://wiki.ros.org/dwa_local_planner
DWA算法第一次提出应该是1997年，发在了《IEEE Robotics and Automation Magazines》上
路径规划算法主要包括全局路径规划和局部路径规划。局部路径规划主要用于动态环境下的导航和避障，对于无法预测的障碍物DWA算法可以较好地解决。DWA算法的优点是计算负复杂度较低，由于考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小。采样的速度即形成了一个动态窗口。
对于轨迹的评价函数主要包括三个方面：与目标的接近程度、机器人前进的速度、与下一个障碍物的距离。简而言之就是在局部规划出一条路径，希望与目标越来越近，且速度较快，与障碍物尽可能远。评价函数权衡以上三个部分得到一条最优路径。
该论文相对于之前的创新点在于：
- 该方法是由一个移动机器人的运动动力学推导出来的
- 考虑到机器人的惯性（代码中计算了刹车距离），这对于具有扭矩限制机器人在高速行驶时很重要。
- 在动态杂乱环境中速度可以较快，对于速度较快的机器人以及低电动机转矩的机器人较为实用。
相关工作
该部分作者说明了全局路径规划的优点在于计算时可以离线进行，但是目前ROS中全局路径也在导航过程中不断变化。缺点在于不能适应环境变化以及计算复杂度太高，尤其是环境不断变化时。局部路径规划的缺点在于不能保证得到最优解，容易陷入局部最优（如U形障碍环境）。优点在于计算速度快，适合环境不断变化。
对比了其他的局部路径规划算法的优缺点，势场法计算速度很快，但是在狭窄区域会产生震荡，如果目标点在两个很近的障碍物之间，则可能找不到路径。
同步传动机器人运动学方程
为了使运动学方程更加接近实际，将模型的速度设为随时间变化的分段函数，在该假设下，机器人轨迹可看做许多的圆弧积分组成，采用该方法使得障碍物碰撞检测很方便，因为圆弧与障碍物的交点很好求。
$令x(t)$,$y(t)$,$ heta(t)$分别表示机器人在$t$时刻的x坐标、y坐标以及朝向角，$x(t_0)$和$x(t_n)$分别表示机器人在$t_0$和$t_1$时刻的x坐标，令$v(t)$表示机器人的平移速度。
$$x(t_n)=x(t_0)+int_{t_0}^{t_n}v(t)*cos heta (t)dt ag{1}$$
$$y_(t_n)=y(t_0)+int_{t_0}^{t_n}v(t)*sin heta (t)dt ag{2}$$
等式(1)和等式(2)取决于机器人的速度，但机器人速度不能直接设定。机器人速度$v(t)$取决于初始时刻$t_0$的速度以及时间$t_0$、机器人在时间间隔$hat{t}in [t_0,t]$的平移加速度$dot{v}(hat{t})$。同样的，$ heta(t)$也是初始转向角$ heta(t_0)$函数（设$t_0$时刻的初始旋转速度为$w(t_0)$，$hat{t}in [t_0,t]$的旋转加速度为$dot{w}(hat{t})$，则式(1)可写为：
$$x(t_n)=x(t_0)+int_{t_0}^{t_n}left(v(t_0)+int_{t_0}^{t}dot{v}(hat{t})dhat{t} ight)*cosleft ( heta (t_0)+int_{t_0}^{t}left((w(t_0)+int_{t_0}^{ar{t}}dot{w}(widetilde{t})dwidetilde{t} ight)dwidetilde{t} ight )dt ag{3}$$
此时机器人的轨迹由初始时刻的状态以及加速度决定，可以认为这些状态是可控的，同时由于机器人内部结构原因，其加速度也不是一直变化（类似于连续函数），因此可以将$t_0$到$t_n$看作是很多个时间片，积分可以转换为求和，假设有n个时间片，在每个时间片$[t_i,t_{i+1}]$，机器人的加速度$dot{v_i}$和$dot{w}$保持不变，设$Delta_t^i=t-t_i$则式(3)可以转化为：
$$x(t_n)=x(t_0)+sum_{i=0}^{n-1}int_{t_i}^{t_{i+1}}left (v(t_i)+dot{v_i}*Delta_t^i ight )*cosleft ( heta(t_i)+w(t_i)*Delta_t^i+frac{1}{2}dot{w_i}*(Delta_t^i)^2 ight )dt ag{4}$$
等式(4)虽然与机器人的动力控制相关，但是不能决定机器人具体的驾驶方向，对于障碍物与机器人轨迹的交点也很难求出，继续进行简化，既然时间间隔都很小，因此可以假设在每一个时间片内速度保持不变，则$v(t_i)+dot{v_i}*Delta_t^i$可近似为$v_iin[v(t_i),v(t_{i+1})]$，同理，$ heta(t_i)+w(t_i)*Delta_t^i+frac{1}{2}dot{w_i}*(Delta_t^i)^2$近似为$ heta (t_i)+w_i*Delta_t^i$，其中$w(i)in [w(t_i),w(t_{i+1}]$，则式(4)可写为：
$$x(t_n)=x(t_0)+sum_{i=0}^{n-1}int_{t_i}^{t_{i+1}}v_i*cosleft ( heta(t_i)+w_i*(hat{t}-t_i) ight )dhat{t} ag{5}$$
解这个积分方程，简化为：
$$x(t_n)=x(t_0)+sum_{i=0}^{n-1}F_x^i(t_{i+1}) ag{6}$$
其中：
$$F_x^i(t_{i})=
left{
egin{array}{lr}
frac{v_i}{w_i}left (sin heta(t_i)-sinleft ( heta(t_i)+w_i*(t-t_i)
ight)
ight),w_i
e 0 \
v_i cosleft( heta(t_i)
ight)*t,w_i=0
end{array}
ight.
ag{7}$$
同理，对应的y坐标公式为：
$$y(t_n)=y(t_0)+sum_{i=0}^{n-1}F_y^i(t_{i+1}) ag{8}$$
$$F_y^i(t_{i})=
left{
egin{array}{lr}
-frac{v_i}{w_i}left (cos heta(t_i)-cosleft ( heta(t_i)+w_i*(t-t_i)
ight)
ight),w_i
e 0 \
v_i sinleft( heta(t_i)
ight)*t,w_i=0
end{array}
ight.
ag{9}$$
（推导了一遍总感觉式(7）和式(9)的第一项前面少了一个负号）
当$w_i=0$时，机器人行走轨迹为一条直线，当$w_i e =0$时，机器人轨迹为圆弧，设
$$M_x^i = -frac{v_i}{w_i}*sin heta(t_i) ag{10}$$
$$M_y^i = frac{v_i}{w_i}*cos heta(t-i) ag{11}$$
则：
$$(F_x^i-M_x^i)^2+(F_x^i-M_x^i)^2=(frac{v_i}{w_i})^2 ag{12}$$
式(12)说明第i段圆弧轨迹的圆心为$(M_x^i,M_y^i)$，半径为$frac{v_i}{w_i}$。
根据上述公式可以求出机器人的轨迹，即通过一系列分段的圆弧和直线来拟合轨迹。
误差上界：
将机器人轨迹进行分段会在控制点之间产生线性误差，即$t_{i+1}-t_i$之间的误差，设x坐标和y坐标的误差分别为$E_x^i$和$E_y^i$，$Delta t_i=t_{i+1}-t_i$，由于$v_iin [v(t_i),v(t_{i+1})]$，易知最大误差$E_x^i,E_y^ileq|v(t_{i+1})-v(t_i)|*Delta t_i$，在$Delta t_i$内是线性的。注意该上界误差仅仅可用于机器人内部预测，而实际机器人位置一般通过里程计测量。
动态窗口法
动态窗口法在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，在使用代价函数进行评价。
主要算法步骤如下
- 速度搜索空间，根据以下三点进行速度空间降采样
- 圆弧轨迹：动态窗口法仅仅考虑圆弧轨迹，该轨迹由采样速度$(v,w)$决定，这些速度构成一个速度搜索空间。
- 允许速度：如果机器人能够在碰到最近的障碍物之前停止，则该采样速度将被评估。
- 动态窗口：由于机器人加速度的限制，因此只有在加速时间内能达到的速度才会被保留。
- 求最优值
代价函数：
$$G(v,w)=sigmaleft(alpha *heading(v,w)+eta *dist(v,w)+gamma *vel(v,w) ight) ag{13}$$
最大值即使最优值最大：
（a）Target heading:heading用于评价机器人与目标位置的夹角，当机器人朝着目标前进时，该值取最大。
（b）Clearance:dist 用于表示与机器人轨迹相交的最近的障碍物距离。
（c） Velocity:vel 表示机器人的前向移动速度，支持快速移动。
其中sigma 使得三个部分的权重更加平滑，使得轨迹与障碍物之间保持一定的间隙。
安全的速度是指机器人能够在撞掉障碍物之前停下，$dist(v,w)$为机器人轨迹上与障碍物的最近距离，设刹车时的加速度为$dot v_b$和$dot w_b$，则$V_a$为机器人不与障碍物碰撞的速度集合：
$$V_a=left{v,w | vleqsqrt{2*dist(v,w)*dot v_b } cap wleq sqrt{2*dist(v,w)*dot w_b} ight} ag{14}$$
考虑到机器人的动力加速度，搜索空间降采样到动态窗口，只保留以当前加速度可到达的速度，设$t$为时间间隔，$(v_a,w_a)$为实际速度，则动态窗口的速度集合为$V_d$:
$$V_d=left{v,w | vin [v_a-dot v*t,v_a+dot v*t] cap win [w_a-dot w*t,w_a+dot w*t ] ight} ag{15}$$
该集合以外的速度都不能在该时间间隔内达到。
综上，最终的搜索空间：
$$V_r=V_scap V_acap V_d ag{16}$$
最终的代价函数(13)是基于速度$V_r$计算的。其中：
Target Heading:表示机器人与目标点的对齐程度，用$180- heta$表示，$ heta$为机器人与目标夹角，夹角越大，代价值越小。
Clearance：表示与机器人轨迹相交的最近障碍物的距离，如果障碍物与机器人轨迹不相交，则设为一个较大的值。
Velocity：机器人某条轨迹的速度$v$。
平滑处理：
评价函数的三个部分都被正则化到$[0,1]$上，实验中设置了$alpha = 2$，$eta=0.2$，$gamma=0.2$，平滑处理可以使机器人与障碍物之间有一定的间隙（裕度）。
实现细节：
- 当机器人陷入局部最优时（即不存在路径可以通过），使其原地旋转，直到找到可行路径。
- 安全裕度：在路径规划时，设定一安全裕度，即在路径和障碍物之间保留一定间隙，且该间隙随着速度增大线性增长。
参数设定：
- $alpha$占比重太大，机器人运动自由度大，窄的区域不容易通过，$alpha$占比重太小，机器人轨迹则不够平滑。因此$alpha$越大，越适合在窄区域，$alpha$越小，越适合在宽区域。