DWA局部路径规划算法论文阅读：The Dynamic Window Approach to Collision Avoidance。

DWA（动态窗口）算法是用于局部路径规划的算法，已经在ROS中实现，在move_base堆栈中：http://wiki.ros.org/dwa_local_planner

DWA算法第一次提出应该是1997年，发在了《IEEE Robotics and Automation Magazines》上

路径规划算法主要包括全局路径规划和局部路径规划。局部路径规划主要用于动态环境下的导航和避障，对于无法预测的障碍物DWA算法可以较好地解决。DWA算法的优点是计算负复杂度较低，由于考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小。采样的速度即形成了一个动态窗口。

对于轨迹的评价函数主要包括三个方面：与目标的接近程度、机器人前进的速度、与下一个障碍物的距离。简而言之就是在局部规划出一条路径，希望与目标越来越近，且速度较快，与障碍物尽可能远。评价函数权衡以上三个部分得到一条最优路径。

该论文相对于之前的创新点在于：

该方法是由一个移动机器人的运动动力学推导出来的
考虑到机器人的惯性（代码中计算了刹车距离），这对于具有扭矩限制机器人在高速行驶时很重要。
在动态杂乱环境中速度可以较快，对于速度较快的机器人以及低电动机转矩的机器人较为实用。

同步传动机器人运动学方程

为了使运动学方程更加接近实际，将模型的速度设为随时间变化的分段函数，在该假设下，机器人轨迹可看做许多的圆弧积分组成，采用该方法使得障碍物碰撞检测很方便，因为圆弧与障碍物的交点很好求。

$令x(t)$,$y(t)$,$\theta(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\theta (t)dt \tag{1}$$

$$y_(t_n)=y(t_0)+\int_{t_0}^{t_n}v(t)*sin\theta (t)dt \tag{2}$$

等式(1)和等式(2)取决于机器人的速度，但机器人速度不能直接设定。机器人速度$v(t)$取决于初始时刻$t_0$的速度以及时间$t_0$、机器人在时间间隔$\hat{t}\in [t_0,t]$的平移加速度$\dot{v}(\hat{t})$。同样的，$\theta(t)$也是初始转向角$\theta(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})d\hat{t}\right)*cos\left (\theta (t_0)+\int_{t_0}^{t}\left((w(t_0)+\int_{t_0}^{\bar{t}}\dot{w}(\widetilde{t})d\widetilde{t}\right)d\widetilde{t}\right )dt\tag{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\right )*cos\left (\theta(t_i)+w(t_i)*\Delta_t^i+\frac{1}{2}\dot{w_i}*(\Delta_t^i)^2\right )dt\tag{4}$$

等式(4)虽然与机器人的动力控制相关，但是不能决定机器人具体的驾驶方向，对于障碍物与机器人轨迹的交点也很难求出，继续进行简化，既然时间间隔都很小，因此可以假设在每一个时间片内速度保持不变，则$v(t_i)+\dot{v_i}*\Delta_t^i$可近似为$v_i\in[v(t_i),v(t_{i+1})]$，同理，$\theta(t_i)+w(t_i)*\Delta_t^i+\frac{1}{2}\dot{w_i}*(\Delta_t^i)^2$近似为$\theta (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*cos\left (\theta(t_i)+w_i*(\hat{t}-t_i) \right )d\hat{t}\tag{5}$$

解这个积分方程，简化为：

$$x(t_n)=x(t_0)+\sum_{i=0}^{n-1}F_x^i(t_{i+1})\tag{6}$$

其中：

$$F_x^i(t_{i})=
\left\{
\begin{array}{lr}
\frac{v_i}{w_i}\left (sin\theta(t_i)-sin\left (\theta(t_i)+w_i*(t-t_i) \right) \right),w_i\ne 0 \\
v_i cos\left(\theta(t_i)\right)*t,w_i=0
\end{array}
\right.
\tag{7}$$

同理，对应的y坐标公式为：

$$y(t_n)=y(t_0)+\sum_{i=0}^{n-1}F_y^i(t_{i+1})\tag{8}$$

$$F_y^i(t_{i})=
\left\{
\begin{array}{lr}
-\frac{v_i}{w_i}\left (cos\theta(t_i)-cos\left (\theta(t_i)+w_i*(t-t_i) \right) \right),w_i\ne 0 \\
v_i sin\left(\theta(t_i)\right)*t,w_i=0
\end{array}
\right.
\tag{9}$$

（推导了一遍总感觉式(7）和式(9)的第一项前面少了一个负号）

当$w_i=0$时，机器人行走轨迹为一条直线，当$w_i\ne =0$时，机器人轨迹为圆弧，设

$$M_x^i = -\frac{v_i}{w_i}*sin\theta(t_i)\tag{10}$$

$$M_y^i = \frac{v_i}{w_i}*cos\theta(t-i)\tag{11}$$

则：

$$(F_x^i-M_x^i)^2+(F_x^i-M_x^i)^2=(\frac{v_i}{w_i})^2\tag{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_i\in [v(t_i),v(t_{i+1})]$，易知最大误差$E_x^i,E_y^i\leq|v(t_{i+1})-v(t_i)|*\Delta t_i$，在$\Delta t_i$内是线性的。注意该上界误差仅仅可用于机器人内部预测，而实际机器人位置一般通过里程计测量。

动态窗口法

动态窗口法在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，在使用代价函数进行评价。

主要算法步骤如下

速度搜索空间，根据以下三点进行速度空间降采样
- 圆弧轨迹：动态窗口法仅仅考虑圆弧轨迹，该轨迹由采样速度$(v,w)$决定，这些速度构成一个速度搜索空间。
- 允许速度：如果机器人能够在碰到最近的障碍物之前停止，则该采样速度将被评估。
- 动态窗口：由于机器人加速度的限制，因此只有在加速时间内能达到的速度才会被保留。
求最优值

　　　代价函数：

$$G(v,w)=\sigma\left(\alpha *heading(v,w)+\beta *dist(v,w)+\gamma *vel(v,w)\right)\tag{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 | v\leq\sqrt{2*dist(v,w)*\dot v_b } \cap w\leq \sqrt{2*dist(v,w)*\dot w_b} \right\}\tag{14}$$

考虑到机器人的动力加速度，搜索空间降采样到动态窗口，只保留以当前加速度可到达的速度，设$t$为时间间隔，$(v_a,w_a)$为实际速度，则动态窗口的速度集合为$V_d$:

$$V_d=\left\{v,w | v\in [v_a-\dot v*t,v_a+\dot v*t] \cap w\in [w_a-\dot w*t,w_a+\dot w*t ] \right\}\tag{15}$$

该集合以外的速度都不能在该时间间隔内达到。

综上，最终的搜索空间：

$$V_r=V_s\cap V_a\cap V_d\tag{16}$$

最终的代价函数(13)是基于速度$V_r$计算的。其中：

Target Heading:表示机器人与目标点的对齐程度，用$180-\theta$表示，$\theta$为机器人与目标夹角，夹角越大，代价值越小。

Clearance：表示与机器人轨迹相交的最近障碍物的距离，如果障碍物与机器人轨迹不相交，则设为一个较大的值。

Velocity：机器人某条轨迹的速度$v$。

平滑处理：

评价函数的三个部分都被正则化到$[0,1]$上，实验中设置了$\alpha = 2$，$\beta=0.2$，$\gamma=0.2$，平滑处理可以使机器人与障碍物之间有一定的间隙（裕度）。

实现细节：

当机器人陷入局部最优时（即不存在路径可以通过），使其原地旋转，直到找到可行路径。
安全裕度：在路径规划时，设定一安全裕度，即在路径和障碍物之间保留一定间隙，且该间隙随着速度增大线性增长。

参数设定：

$\alpha$占比重太大，机器人运动自由度大，窄的区域不容易通过，$\alpha$占比重太小，机器人轨迹则不够平滑。因此$\alpha$越大，越适合在窄区域，$\alpha$越小，越适合在宽区域。