算法学习之DWA局部路径规划算法

DWA算法是近日工作中常用到的算法，在此分享一下。

DWA算法又名动态窗口法，其原理较为简单，属于局部路径规划算法，常用于局部避障.

简单介绍一下算法流程:

根据车体模型不同,DWA对于预测点的方式有所不同,对于前轮前驱模型,DWA预测方式为对前轮角度和速度的预测.

对于两轮差速模型,DWA预测方式为左右轮速.

假设车体模型为前轮前驱模型:

1 .  已知前轮偏角范围-pi/2 ~ pi/2, 间隔0.1取角度值. 假设有A个取值

2.   已知前轮速度区间 0~ v1, 间隔 0.1m/s 取值.假设有B个取值.

3.   已知预测时长 假设为t s . 

4 .  构造DWA预测轨迹:  以上三个条件构造三层循环(条件2也可设置为恒定速度,减少计算量和轨迹数目),预测定位点在 未来 t s 时间 在角度速度组合下的 运动轨迹,每条轨迹单独存储,存储轨迹上每个点的位姿.  可计算出A*B 条运动预测轨迹.

5.  碰撞检测: 依此对每条轨迹做碰撞检测,若轨迹上有一个点会发生碰撞,当前轨迹废弃,依此类推,保存无碰撞轨迹.

6 . 构造评价函数:  对于步骤5 剩余的无碰撞轨迹, 根据目标点位置和航向,构造评价函数,常用评价指标有: 航向差异大小,速度大小等,可自行发挥.总之选择出最合适的一组速度和角度即可.

对于两轮差速模型: 就不存在偏转角度,只需要对左右轮角度进行预测即可,替换上述步骤的1和2.

假设左右轮的速度均为v1  -  v2区间.

步骤1 . 2 只需要将速度间隔取值即可.后续步骤一致.

 碰撞检测部分是比较关键的一部分:后续有空单独讲解 .  碰撞模型的不同会对路径选择具有较大的影响.

下面是DWA实现,工作原因,代码就不贴了. 实现也比较简单,贴个简图.效果还是不错的.