DWA（Dynamic Window Approach）局部路径规划算法详解及代码实现

前言

---

文学与文字表达是了不起的，它可以抽象、可以象形、可以传道解惑、更可以对抗遗忘，坚持去记录分享的原因也大抵如此。这几天有时间去看喜欢的书籍、有充分的时间去规划一些想做的事情。分享下近期

1. 某书中有两段非常细腻的表达：“原文书突出脊梁，呈人字压在桌上，整块沉重的黑纹大理石桌都是你的书签”、“罪恶感是古老而血统纯正的牧羊犬” 记忆很深刻，不知道大家有没有看过这本书（很难以一种平静的心情去读完的一本书，整个社会可能都是残忍的帮凶）。
2. 最近非常流行“现在所经历的都是学生时代知识的闭环”，我个人有一种错觉，很多时候正在经历的东西有种是成相似的感觉。从一个up主视频知道原来《美人鱼》中女主林允叫Lucy是有原因的，最早发现的直立行走的女性人类化石被取名为Lucy。

---

回归正文

一、效果视频

先看视频效果，能最直观感受

DWA演示视频

二、算法原理

一句话概况，就是假定机器人当前以若干组容许范围内的速度（差速轮为例：线速度V，角速度W）进行移动，并对这若干组速度进行轨迹计算，得到若干组轨迹，再根据若干条评分机制选择最好的轨迹所对应的速度作为dwa输出。

2.1 算法整体流程

图1 总体流程图

2.2 速度采样及轨迹生成

下面以差速模型为例进行说明，机器人只能进行原地旋转或者向前移动（分别对应角速度w，线速度v），经过一小段时间$△t$运动后，机器人的位置可以表示为:

[ x ( t + △ t ) y ( t + △ t ) θ ( t + △ t ) υ ( t + △ t ) ω ( t + △ t ) ] = [ x ( t ) + v ( t ) ∗ c o s ( θ ( t ) ) ∗ △ t y ( t ) + v ( t ) ∗ s i n ( θ ( t ) ) ∗ △ t θ ( t ) + w ( t ) ∗ △ t v ( t ) + a ( t ) ∗ △ t w ( t