DWA算法实现

概要

动态窗口算法(Dynamic Window Approaches, DWA) 属于局部路径规划算法，能有效避开障碍物。

本文主要介绍dwa算法的一种c++实现，源代码库见（GitHub - goktug97/DynamicWindowApproach: The Dynamic Window Approach (DWA) planning algorithm written in C with Python Bindings）。

DWA算法步骤

1. 速度采样。根据当前的状态，计算可能存在的采样空间，也就是加速度和角加速度。采样空间通常需要考虑速度边界限制，加速度限制，环境障碍物限制等。
2. 轨迹推算。个人理解就是将连续的轨迹抽取成一些离散点（分辨率），便于下一步进行轨迹评价。
3. 轨迹评价。对采样空间中的每一种状态（加速度和角加速度）生成的轨迹进行打分。通常打分需要考虑航向角，距离障碍物的最短距离以及速度。

代码实现

主流程

Velocity
planning(Pose pose, // 当前的位置和航向角度
        Velocity velocity,  // 当前的速度和角速度
        Point goal, // 目标位置
         PointCloud *pointCloud, // 障碍物对于点的位置 
         Config config) { // 配置文件
  DynamicWindow *dw;
  createDynamicWindow(velocity, config, &dw); // 创建采样空间，障碍物由cost来避免
  Velocity pVelocity; // post velocity
  Pose pPose = pose; // post pose
  float total_cost = FLT_MAX; // 开销，越小表示轨迹越佳
  float cost; // 当前轨迹的开销
  Velocity bestVelocity;
  for (int i = 0; i < dw->nPossibleV; ++i) { // 遍历采样空间中的速度值
    for (int j = 0; j < dw->nPossibleW; ++j) { // 遍历采样空间中的角速度值
      pPose = pose;
      pVelocity.linearVelocity = dw->possibleV[i];
      pVelocity.angularVelocity = dw->possibleW[j];
      pPose = motion(pPose, pVelocity, config.predictTime); // 计算config.predictTime时间后的姿态和航向
      cost = 
        config.velocity * calculateVelocityCost(pVelocity, config) + // 速度开销
        config.heading * calculateHeadingCost(pPose, goal) + // 航向角度开销
        config.clearance * calculateClearanceCost(pose, pVelocity, pointCloud, config); // 躲避障碍物的开销
      if (cost < total_cost) { // 保留开销最小的状态值
        total_cost = cost;
        bestVelocity = pVelocity;
      }
    }
  }
  freeDynamicWindow(dw);
  return bestVelocity; // 返回开销最小的状态值
}

创建采样空间

void
createDynamicWindow(Velocity velocity, Config config, DynamicWindow **dynamicWindow) {
  // 计算速度的最小值
  float minV = max(config.minSpeed, velocity.linearVelocity - config.maxAccel * config.dt);
  // 计算速度的最大值
  float maxV = min(config.maxSpeed, velocity.linearVelocity + config.maxAccel * config.dt);
  // 计算角速度的最小值
  float minW = max(-config.maxYawrate, velocity.angularVelocity - config.maxdYawrate * config.dt);
  // 计算角速度的最大值
  float maxW = min(config.maxYawrate, velocity.angularVelocity + config.maxdYawrate * config.dt);

  // 根据配置文件的速度分辨率和角速度分辨率计算可能的状态值
  int nPossibleV = (maxV - minV) / config.velocityResolution;
  int nPossibleW = (maxW - minW) / config.yawrateResolution;
  *dynamicWindow = malloc(sizeof(DynamicWindow));

  (*dynamicWindow)->possibleV = malloc(nPossibleV * sizeof(float));
  (*dynamicWindow)->possibleW = malloc(nPossibleW * sizeof(float));
  (*dynamicWindow)->nPossibleV = nPossibleV;
  (*dynamicWindow)->nPossibleW = nPossibleW;

  // 赋值
  for(int i=0; i < nPossibleV; i++) {
    (*dynamicWindow)->possibleV[i] = minV + (float)i * config.velocityResolution;
  }

  for(int i=0; i < nPossibleW; i++) {
    (*dynamicWindow)->possibleW[i] = minW + (float)i * config.yawrateResolution;
  }
}

轨迹推算

Pose motion(Pose pose, Velocity velocity, float dt){
  Pose new_pose;  // 新的状态值                                                                       
  new_pose.yaw = pose.yaw + velocity.angularVelocity * dt;
  new_pose.point.x = pose.point.x + velocity.linearVelocity * cos(new_pose.yaw) * dt;
  new_pose.point.y = pose.point.y + velocity.linearVelocity * sin(new_pose.yaw) * dt;
  return new_pose;
}

而这段时间是否会和障碍物接触会由开销函数来保证。

轨迹评价

计算速度开销

算法还是希望能高速运行

float calculateVelocityCost(Velocity velocity, Config config) {
  return config.maxSpeed - velocity.linearVelocity; // 更新后的速度越大越好
}

计算航向角度开销

float calculateHeadingCost(Pose pose, Point goal) {
  float dx = goal.x - pose.point.x;
  float dy = goal.y - pose.point.y;
  float angleError = atan2(dy, dx); // 更新后的位置点与目标点所成的夹角
  float angleCost = angleError - pose.yaw; // 与状态值的航向角度的差值，越小越好
  return fabs(atan2(sin(angleCost), cos(angleCost)));
}

计算障碍物的开销

float
calculateClearanceCost
(Pose pose, Velocity velocity, PointCloud *pointCloud, Config config) {
  Pose pPose = pose;
  float time = 0.0; // 当前时间，按照时间分辨率移动到预测的时间
  float minr = FLT_MAX; // 与障碍物的最小距离
  float r;
  float dx;
  float dy;

  float x;
  float y;

  while (time < config.predictTime) { // 通过离散的方法近似计算轨迹与障碍物的最短距离
    pPose = motion(pPose, velocity, config.dt); 
      
    for(int i = 0; i < pointCloud->size; ++i) { // 遍历所有的障碍物点
      dx = pPose.point.x - pointCloud->points[i].x;
      dy = pPose.point.y - pointCloud->points[i].y;
      x = -dx * cos(pPose.yaw) + -dy * sin(pPose.yaw);
      y = -dx * -sin(pPose.yaw) + -dy * cos(pPose.yaw);
      if (x <= config.base.xmax &&
          x >= config.base.xmin &&
          y <= config.base.ymax &&
          y >= config.base.ymin){
        return FLT_MAX;
      }
      r = sqrtf(dx*dx + dy*dy);
      if (r < minr)
        minr = r; // 保留最短距离
    }
    time += config.dt;
  }
  return 1.0 / minr; // 距离越小，开销越大
}

配置文件

typedef struct {
  float maxSpeed; // 允许的最大速度
  float minSpeed; // 允许的最小速度
  float maxYawrate; // 允许的最大角速度
  float maxAccel; // 允许的最大加速度
  float maxdYawrate; // 允许的最大角加速度
  float velocityResolution; // 采样空间的速度分辨率
  float yawrateResolution; // 采样空间的角速度分辨率
  float dt; // 用于计算距离障碍物距离的时间分辨率
  float predictTime; // 单步的前行时间
  float heading; // 航向角度的开销系数
  float clearance; // 避开障碍物的开销系数
  float velocity; // 速度的开销系数
  Rect base;
} Config;

参考文献

DWA算法总结 - 知乎

【精选】【路径规划】局部路径规划算法——DWA算法（动态窗口法）|（含python实现 | c++实现）-CSDN博客