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

前言

---

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

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

---

回归正文

一、效果视频

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

DWA演示视频

二、算法原理

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

2.1 算法整体流程

图1 总体流程图

2.2 速度采样及轨迹生成

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

$$\left[\begin{array}{c}x(t+△t)\\ y(t+△t)\\ \theta (t+△t)\\ \upsilon (t+△t)\\ \omega (t+△t)\end{array}\right]=\left[\begin{array}{c}x(t)+v(t)\ast cos(\theta (t))\ast △t\\ y(t)+v(t)\ast sin(\theta (t))\ast △t\\ \theta (t)+w(t)\ast △t\\ v(t)+a(t)\ast △t\\ w(t)+\dot{w(t)}\ast △t\end{array}\right]\text{\hspace{1em}(1)}$$

假设$△t$足够小，则轨迹拟合越符合真实情况，具体代码实现可以看下面：

/**
// State 机器人当前状态：位姿，v,w
// v 采样的速度
// w 采样的角速度
// dt 时间周期
*/
void DWAPlanner::updateState(State &state, const float &v, const float &w, const float &dt) {
  float unit_dt = 0.005;  // 最小时间周期，越小拟合越准确
  int dt_size = dt / unit_dt;  // 累计计算次数
  for (int i = 0; i < dt_size; i++) {
  // 根据差速模型进行机器人状态推算
    state.yaw += w * unit_dt;
    state.x += v * std::cos(state.yaw) * unit_dt;
    state.y += v * std::sin(state.yaw) * unit_dt;
  }
  state.linear_vel = v;
  state.angular_vel = w;
}

根据若干组速度进行差速运动学模型的轨迹推导即可以得到如下图的若干条预测轨迹

图2 轨迹生成

2.3 轨迹评价

一般轨迹评价会包含：
1、方向角评价函数：预测轨迹终点与目标点夹角越小越好
2、距离评价函数：距离障碍物远近，越大越好
3、速度评价函数：容许范围内越大越好
4、轨迹贴合评价函数：越贴合全局路径越好
… …
下面我们以几个基本的评价函数做对应代码分析，

// 轨迹贴合评价函数
/**
dst_traj 待评价轨迹
src_traj 需要跟踪的全局轨迹
输出了距离及角度的累计误差作为cost
*/
float DWAPlanner::PathCost(const std::vector<State> &dst_traj,
                           const std::deque<Eigen::Vector3f> &src_traj) {
                                 
  float angular_error = 0.0, linear_error = 0.0;
  std::vector<float> src_start_point(2, 0);
  std::vector<float> dst_start_point(2, 0);
  int src_traj_size = src_traj.size();
  int dst_traj_size = dst_traj.size();
  for (int i = 0; i < src_traj_size; i++) {
    float dx = dst_traj[i].x - src_traj[i].x();
    float dy = dst_traj[i].y - src_traj[i].y();
    if (i != 0) {
      float ddx = dst_traj[i].x - dst_traj[i - 1].x;
      float ddy = dst_traj[i].y - dst_traj[i - 1].y;
      float sdx = src_traj[i].x() - src_traj[i - 1].x();
      float sdy = src_traj[i].y() - src_traj[i - 1].y();
      float d_angle = fabs(angleNormalize(atan2(ddy, ddx) - atan2(sdy, sdx)));
      angular_error += d_angle;
    }
    linear_error += std::sqrt(dx * dx + dy * dy);
  }
  return linear_error + angular_error;
}

// 速度评价函数
/**
traj 待评价轨迹
max_vel 容许的最大线速度
max_angular_vel 容许的最大角速度
输出了轨迹末端点与最大容许速度的差值作为cost
*/
float DWAPlanner::SpeedCost(const std::vector<State> &traj,
                                const float max_vel, const float max_angular_vel) {
  float cost = fabs(max_vel - traj.back().linear_vel) + fabs(max_angular_vel - traj.back().angular_vel);
  return cost;
}

计算得到每种评价指标的值后会进行归一化处理，然后为每种指标设定一个比例因子，用于调整控制的侧重点。
一般表示为：

$$G(v,w)=\alpha \ast velCost(v,w)+\beta \ast pathCost(v,w)+\gamma \ast headingCost(v,w)+\delta \ast distCost(v,w)+......\text{\hspace{1em}(2)}$$
其中代价值越低越好，选择代价值最低的一组$(v,w)$作为DWA的控制输出。

三、Corner Case及想法

1、当机器人当前朝向与路径初始方向夹角太大时，应该如何处理？
2、跟踪过程中机器人与路径朝向太大或距离太大时（定位误差或者控制误差导致），应该如何处理？
3、目标点距离障碍物很近时，机器人运动是否一定正常？
4、遇到窄口、窄道时，能否保证机器人的通过性能？
… …
想法：传入的全局路径轨迹中可以包含其属性（包括但不限于起点、终点、窄口、窄道、急弯… …），然后根据当前跟踪轨迹属性去动态调整控制参数（包括但不限于评价比例因子、采样分辨率、预测时间… …）

个人见解：如果DWA调好了，可以非常极限过一些窄口窄道。

---

梦想是我们每个人与生俱来的财富，脚踏实地，去追逐梦想！

---