自动驾驶路径规划DWA算法原理解析

这篇文章详细介绍一下最新的ROS给出的DWA算法的结构，以及各个重要的cost function的含义，帮助你们理解DWA算法的构成。

动态窗口法的核心在于采样和动态规划。采样会对速度和运动参数进行采样，我们称之为采样速度，这个是时间维度的采样。另外还会对位置采样，称之为轨迹采样，这就是空间维度的采样。

对于动态规划来说，我们回去考虑局部目标，代价地图，全局路径等，根据这些核心点考虑如何表达代价函数，代价函数的选择可以是多样的，用户有很大的操作空间，这里介绍七个主要的代价函数

首先，我们解释一下采样速度：

对于小车来说，采样速度包含了线速度和角速度，对应车来说，就是纵向速度和横向速度。但是在采样过程中，我们会设定很多限制，比如说，速度的范围必须是a到b的，以及允许的最大加速度是a,那么在delta_t时间内，速度的变化就会有上下限，同样的原理适用于角度的采样：

就如图中所示的，我们组合不同的线速度和角速度，就可以得到不同的采样结果：

这样就可以生成不同的采样轨迹：

现在，我们开始介绍摘要中介绍的七个cost function 代价函数：

1. oscillation cost function:
   
   这个函数的主要目的就是保证车辆在纵向速度上面不会往前和往后来回震荡。实现的方式就是加入一个reset distance 参数，比如我把它设置为5m, 意思就是，我在规划过程中，车辆一直前进，但是前面碰到障碍物了，我的规划可能要停车，甚至倒车，这时候，车子稍微倒车一点，规划发现现在没这么危险了，可能又要前进了，这就可能引起前进后退来回切换，我们就引入了这个reset distance, 保证车子倒车至少要5m, 才能切换为前进。

2. twirling cost function
   
   这个function 很简单，意思就是字面意思，我们惩罚大的转角度，因为这个直接影响到了舒适性。

3. obstacle function
   
   这个函数用来避障。 值得注意的是，我们把本车的碰撞体积分为了两部分，一个是车身周围的，另一个是车辆中心的，或者是车辆内部的。这样做的原因是有可能车子速度很快，一下就会掠过障碍物，所以为了保险，我们在车辆内部还加了碰撞体积，如果碰到障碍物，依旧会做出很大的惩罚。
   
   对于最后cost 的计算，就像图例所示，是各个位置的累加。主要的原则就是，一旦碰到障碍物，这里的cost 就必须非常大，可以一票否决这条路径。
   

4. goal cost function
   
   意思是偏离当前选定的轨迹线段的终点的距离。虽然全局规划的轨迹会很早给出，但是在行进途中，我们还是会把轨迹拆分成很多段，每次你只会考虑其中的一段，然后根据规划出来的路径，计算实时的离期望终点的偏差，生产对应的cost, 看到上面的图示，中间蓝色的框框，就是当前我们考虑的局部路径区域，我要去算的就是我的三个采样点离右上角的灰色点（局部的goal）的距离。

在这里面，于是我把包括我现在点在内的，往前的两个点，总共三个点计算离期望轨迹终点的距离，可以是最大值，或者累计，还可以是乘积。

5. goal front cost function

这个function和上面的function一个意思，但是考虑的参数是朝向问题：所以要做切线，计算采样点，与规划路径线段终点的切线的角度差作为cost,

比如这里，我们选定轨迹末端的切线为期望轨迹朝向。采样的三个点，也分别做切线，计算与期望轨迹切线朝向的差值，就获得了cost.

6. path cost function

这个function 和 goal cost function 的区别是不是考虑到轨迹局部终点的距离，而是考虑到轨迹的距离。这就是用于惩罚横向偏差的function，我们总是希望，每个时刻，都跟随期望轨迹，做到没有横向误差。

最后的function 和上面一个很像，就是不仅希望横向没误差，还希望每个点的朝向也是个规划路径上的点一模一样。

总共七个cost function, 总结一下分别的作用就是：
1， 保证不会前后移动频繁切换
2， 保证没有大角度转动
3，保证没有碰撞
4，保证与局部路径终点距离短
5，保证与局部路径终点朝向一致
6，保证与局部路径横向偏差小
7，保证与局部路径朝向一致