TheDynamicWindowApproachtoCol。。。
DWA算法第⼀次提出应该是1997年，发在了《IEEE Robotics and Automation Magazines》上
路径规划算法主要包括全局路径规划和局部路径规划。局部路径规划主要⽤于动态环境下的导航和避障，对于⽆法预测的障碍物DWA算法可以较好地解决。DWA算法的优点是计算负复杂度较低，由于考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较⼩。采样的速度即形成了⼀个动态窗⼝。 对于轨迹的评价函数主要包括三个⽅⾯：与⽬标的接近程度、机器⼈前进的速度、与下⼀个障碍物的距离。简⽽⾔之就是在局部规划出⼀条路径，希望与⽬标越来越近，且速度较快，与障碍物尽可能远。评价函数权衡以上三个部分得到⼀条最优路径。 t恤制作
该论⽂相对于之前的创新点在于：
该⽅法是由⼀个移动机器⼈的运动动⼒学推导出来的
大功率同轴固定衰减器
考虑到机器⼈的惯性（代码中计算了刹车距离），这对于具有扭矩限制机器⼈在⾼速⾏驶时很重要。
在动态杂乱环境中速度可以较快，对于速度较快的机器⼈以及低电动机转矩的机器⼈较为实⽤。
相关⼯作
该部分作者说明了全局路径规划的优点在于计算时可以离线进⾏，但是⽬前ROS中全局路径也在导航过程中不断变化。缺点在于不能适应环境变化以及计算复杂度太⾼，尤其是环境不断变化时。局部路径规划的缺点在于不能保证得到最优解，容易陷⼊局部最优（如U形障碍环境）。优点在于计算速度快，适合环境不断变化。
对⽐了其他的局部路径规划算法的优缺点，势场法计算速度很快，但是在狭窄区域会产⽣震荡，如果⽬标点在两个很近的障碍物之间，则可能不到路径。
同步传动机器⼈运动学⽅程
为了使运动学⽅程更加接近实际，将模型的速度设为随时间变化的分段函数，在该假设下，机器⼈轨迹可看做许多的圆弧积分组成，采⽤该⽅法使得障碍物碰撞检测很⽅便，因为圆弧与障碍物的交点很好求。
$令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$内是线性的。注意该上界误差仅仅可⽤于机器⼈内部预测，⽽实际机器⼈位置⼀般通过⾥程计测量。
动态窗⼝法
动态窗⼝法在速度空间中进⾏速度采样，并对随机采样的速度进⾏限制，减⼩采样数⽬，在使⽤代价函数进⾏评价。
主要算法步骤如下
1. 速度搜索空间，根据以下三点进⾏速度空间降采样
1. 圆弧轨迹：动态窗⼝法仅仅考虑圆弧轨迹，该轨迹由采样速度$(v,w)$决定，这些速度构成⼀个速度搜索空间。
2. 允许速度：如果机器⼈能够在碰到最近的障碍物之前停⽌，则该采样速度将被评估。
3. 动态窗⼝：由于机器⼈加速度的限制，因此只有在加速时间内能达到的速度才会被保留。
2. 求最优值
代价函数：
$$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$越⼩，越适合在宽区域。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议