APOLLO混合A*算法详解

Apollo中的规划渐渐的以一个个的场景为主体来组织，可是现实生活中场景是无数的、是变化的，不知道场景的识别、切换能否cover得住？针对特定场景的特定解决方案与调优是必需的，那么“通用基础规划方法”和“特定场景特定方案特定调优”的结合和分界又在哪呢？

好，言归正传，Open Space Planner 采用了分层规划的思路，首先Hybrid A*规划一条drivable的无碰的路径，为后面的优化作warm start，然后利用数值优化方法求解一条平滑无碰的轨迹。本文主要分析Hybrid A*算法的实现，文件路径为（5.0版本）：apollo\modules\planning\open_space\coarse_trajectory_generator\http://hybrid_a_star.cc。从文件命名上也能看出来，Hybrid A*输出粗糙的轨迹，使用其他有同等效果的其他规划方法也是可以的。该文件夹内的其他文件这里就略过了，仅需要注意一点：http://grid_search.cc中定义了2种搜索最短路径的方法，A*（对应GenerateAStarPath()）和Dynamic Programming（对应GenerateDpMap()），但是，A*方法并没有被调用。

同A*算法类似，Hybrid A*的规划是建立在栅格地图基础上的。和A*不同的是，A*在搜索周边节点时并没有考虑运动主体（机器人或车辆）的运动学约束，而Hybrid A*考虑了这种约束，限制了扩展节点时前进的方向，所以其输出轨迹一定是drivable的。对Hybrid A*的理解，大家可以去查阅其论文。

1. 节点的定义

首先在这里介绍“节点”的概念，即Node3d类。

double x_ = 0.0;    //x,y,phi是node的坐标
double y_ = 0.0;
double phi_ = 0.0;
//step_size_是一段路径（如Reed-Shepp曲线）所包含的路径点数，而该node就是这段路径的终点
size_t step_size_ = 1;
//traversed_x,traversed_y,traversed_phi是node连接的一串点的坐标集合
std::vector<double> traversed_x_;
std::vector<double> traversed_y_;
std::vector<double> traversed_phi_;

A*中的node单纯的指一个节点或一个状态，一般是其（x，y）坐标。这里的Node3d不同，除了包含一个点坐标（x_，y_，phi_）之外，还包含了一串这样的坐标集合。

Node3d::Node3d(const std::vector<double>& traversed_x,
               const std::vector<double>& traversed_y,
               const std::vector<double>& traversed_phi,
               const std::vector<double>& XYbounds,
               const PlannerOpenSpaceConfig& open_space_conf) {
  ...
  x_ = traversed_x.back();
  y_ = traversed_y.back();
  phi_ = traversed_phi.back();
  ...
  traversed_x_ = traversed_x;
  traversed_y_ = traversed_y;
  traversed_phi_ = traversed_phi;
  ...
  step_size_ = traversed_x.size();
}

从上面的构造函数可以看出，（x_，y_，phi_）点就是这一串点中的最后一个。其实，这些点都是在1个grid内的。即，1个grid包含1个Node3d（下文便以node指代），1个Node3d包含了以（x_，y_，phi_）为终点、同在1个grid内的、一串路径点集的信息。

2. Hybrid A*的规划主流程

Hybrid A*的主流程在HybridAStar::Plan()。

//Hybrid A*规划的主流程
bool HybridAStar::Plan(
    double sx, double sy, double sphi, double ex, double ey, double ephi,
    const std::vector<double>& XYbounds,
    const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,
    HybridAStartResult* result) {
  // clear containers
  //每次规划，清空之前的缓存数据
  open_set_.clear();
  close_set_.clear();
  open_pq_ = decltype(open_pq_)();
  final_node_ = nullptr;

  std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec;
  //构造障碍物轮廓线段
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {
    size_t vertices_num = obstacle_vertices.size();
    std::vector<common::math::LineSegment2d> obstacle_linesegments;
    //我认为这里有错，少构造了一条线段
    //(obstacle_vertices[vertices_num - 1], obstacle_vertices[0])
    for (size_t i = 0; i < vertices_num - 1; ++i) {
      common::math::LineSegment2d line_segment = common::math::LineSegment2d(
          obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }
  obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);

  // load XYbounds
  XYbounds_ = XYbounds;
  // load nodes and obstacles
  //构造规划的起点和终点，并检查其有效性
  ...
  if (!ValidityCheck(start_node_)) { ... }
  if (!ValidityCheck(end_node_)) { ... }

  //使用动态规划DP来计算目标点到某点的启发代价（以目标点为DP的起点），即A*中的H
  //生成graph的同时获得了目标点到图中任一点的cost，作为缓存，这就是DPMap的用处
  grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
                                                  obs