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_,
                                                  obstacles_linesegments_vec_);  
  // load open set, pq
  open_set_.insert(std::make_pair(start_node_->GetIndex(), start_node_));
  open_pq_.push(std::make_pair(start_node_->GetIndex(), start_node_->GetCost()));

  // Hybrid A* begins
  ...
  while (!open_pq_.empty()) {
    // take out the lowest cost neighboring node
    const std::string current_id = open_pq_.top().first;
    open_pq_.pop();
    std::shared_ptr<Node3d> current_node = open_set_[current_id];
    // check if an analystic curve could be connected from current configuration
    // to the end configuration without collision. if so, search ends.
    
    //true：如果生成了一条从当前点到目标点的ReedShepp曲线，就找到了最短路径
    //false：否则，继续Hybrid A*扩展节点
    if (AnalyticExpansion(current_node)) {
      break;
    }    
    close_set_.insert(std::make_pair(current_node->GetIndex(), current_node));
    for (size_t i = 0; i < next_node_num_; ++i) {
      //一个grid内的最后一个路径点叫node，该grid内可以有多个路径点，
      //该node的next_node一定在相邻的其他grid内
      std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
      // boundary check failure handle
      if (next_node == nullptr) {
        continue;
      }
      // check if the node is already in the close set
      if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
        continue;
      }
      // collision check
      if (!ValidityCheck(next_node)) {
        continue;
      }
      if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {        
        CalculateNodeCost(current_node, next_node);        
        open_set_.emplace(next_node->GetIndex(), next_node);
        open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
      }
    }
  }
  if (final_node_ == nullptr) { ... }
  if (!GetResult(result)) { ... }  
  return true;
}

3. 碰撞检测

HybridAStar::ValidityCheck() 负责碰撞检测，输入参数节点所连接的、在同一grid内的其他路径点也一起检测了。代码简单，此处略过。

4. ReedShepp曲线加速搜索

HybridAStar::AnalyticExpansion()是对搜索的加速，应该是借鉴了《Path Planning for Autonomous Vehicles in Unknown Semi-Structured Environments》中的技巧。一旦ReedShepp曲线段连接了当前节点和终点，并且通过了安全检测，本轮次的规划结束。

//尝试使用ReedShepp曲线连接当前点与目标点，若成功，则Hybrid A*规划完成
//允许返回false，其实只返回一次true
bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node) {
  ...
  //ReedShepp曲线都是从当前点到终点的
  if (!reed_shepp_generator_->ShortestRSP(current_node, end_node_,
                                          reeds_shepp_to_check)) { ... }
  //ReedShepp曲线段的碰撞检测与越界检测
  if (!RSPCheck(reeds_shepp_to_check)) { ... }

  // load the whole RSP as nodes and add to the close set
  //将连接到目标点的一段ReedShepp曲线封装成node，放入Hybrid A*的集合中
  final_node_ = LoadRSPinCS(reeds_shepp_to_check, current_node);
  return true;
}

5. 扩展相邻的节点

扩展节点的重点在于把车辆运动学模型的约束考虑进去，根据限定的steering angle 去搜索相邻的grid。

//扩展节点，扩展一个node就是扩展了一个grid，但是会产生多个在同一grid内的路径点
std::shared_ptr<Node3d> HybridAStar::Next_node_generator(
    std::shared_ptr<Node3d> current_node, size_t next_node_index) {
  double steering = 0.0;
  size_t index = 0;
  double traveled_distance = 0.0;
  //steering angle为什么这么算？
  //首先，根据next_node_index与next_node_num_的对比是可以区分运动方向的
  //这里的if-else就是区分运动正反方向讨论的（前进和倒车）
  //其次，车辆在当前的姿态下，既可以向左转、又可以向右转，那么steering angle的
  //取值范围其实是[-max_steer_angle_, max_steer_angle_]，在正向或反向下，
  //能取next_node_num_/2个有效值。
  //即，把[-max_steer_angle_, max_steer_angle_]分为（next_node_num_/2-1）份
  //所以，steering = 初始偏移量 + 单位间隔 × index
  //steering angle的正负取决于车的转向，而非前进的正反
  if (next_node_index < static_cast<double>(next_node_num_) / 2) {
    steering = -max_steer_angle_ +
        (2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *
            static_cast<double>(next_node_index);
    traveled_distance = step_size_;
  } else {
    index = next_node_index - next_node_num_ / 2;
    steering = -max_steer_angle_ +
        (2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *
            static_cast<double>(index);
    traveled_distance = -step_size_;
  }
  // take above motion primitive to generate a curve driving the car
  // to a different grid
  double arc = std::sqrt(2) * xy_grid_resolution_;
  std::vector<double> intermediate_x;
  std::vector<double> intermediate_y;
  std::vector<double> intermediate_phi;
  double last_x = current_node->GetX();
  double last_y = current_node->GetY();
  double last_phi = current_node->GetPhi();
  intermediate_x.push_back(last_x);
  intermediate_y.push_back(last_y);
  intermediate_phi.push_back(last_phi);
  //从当前grid前进到下一个grid，一个grid内可能有多个路径点
  for (size_t i = 0; i < arc / step_size_; ++i) {
    const double next_x = last_x + traveled_distance * std::cos(last_phi);
    const double next_y = last_y + traveled_distance * std::sin(last_phi);
    //看车辆运动学模型——自行车模型
    const double next_phi = common::math::NormalizeAngle(last_phi +
        traveled_distance / vehicle_param_.wheel_base() * std::tan(steering));
    intermediate_x.push_back(next_x);
    intermediate_y.push_back(next_y);
    intermediate_phi.push_back(next_phi);
    last_x = next_x;
    last_y = next_y;
    last_phi = next_phi;
  }
  // check if the vehicle runs outside of XY boundary
  ...
  std::shared_ptr<Node3d> next_node = std::shared_ptr<Node3d>(
      new Node3d(intermediate_x, intermediate_y, intermediate_phi, XYbounds_,
                 planner_open_space_config_));
  next_node->SetPre(current_node);
  next_node->SetDirec(traveled_distance > 0.0);
  next_node->SetSteer(steering);
  return next_node;
}

这里补充几个关于steering angle设置的注释。

// max_steer_angle --- vehicle max steer angle  
// max_steer_angle_rate -- vehicle max steer rate; how fast can the steering wheel turn
// min_steer_angle_rate --- vehicle min steer rate;
// steer_ratio --- ratio between the turn of steering wheel and the turn of wheels

6. 计算节点的代价

HybridAStar::CalculateNodeCost() 分别计算路径代价和启发代价，就是A*中的G和H。这里计算启发代价的方法很巧妙，用了DP，请参看 grid_a_star_heuristic_generator_->GenerateDpMap() 处的注释。

void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node,
                                    std::shared_ptr<Node3d> next_node) {
  //A*中走过的轨迹的代价G
  next_node->SetTrajCost(current_node->GetTrajCost() +
                         TrajCost(current_node, next_node));
  // evaluate heuristic cost
  double optimal_path_cost = 0.0;
  //A*中从当前点到目标点的启发式代价H，采用了动态规划DP来计算（以目标点为DP的起点）
  optimal_path_cost += HoloObstacleHeuristic(next_node);
  next_node->SetHeuCost(optimal_path_cost);
}

double HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node) {
  return grid_a_star_heuristic_generator_->CheckDpMap(next_node->GetX(),
                                                      next_node->GetY());
}

Hybrid A*规划给出了初步的路径，仅包含（x，y，phi）等静态信息，接下来进行一些求动态信息、分割、平滑、拼接等后续处理，就是最终的可供车辆行驶的轨迹了。

7. 路径后处理

初步路径的每个node都指向上一个node，HybridAStar::GetResult() 在把这些node反向后（由当前node指向终点node），得到了顺序正确的node集合。注意，此时形成了一个挨一个的node，还不是一个挨一个的轨迹点。因此，要调用 GetTemporalProfile(result) 来完成大部分的后处理，得到最终结果。

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，分别逆向翻转轨迹点
//然后重新拼接在一起，就是最终可以发布供车行驶的轨迹
bool HybridAStar::GetTemporalProfile(HybridAStartResult* result) {
  std::vector<HybridAStartResult> partitioned_results;
  if (!TrajectoryPartition(*result, &partitioned_results)) { ... }
  //将分段的轨迹拼接起来
  HybridAStartResult stitched_result;
  ...
  *result = stitched_result;
  return true;
}

7.1 路径分割

TrajectoryPartition()将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，并完善轨迹的静态、动态信息。

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，
//并完善轨迹的静态、动态信息
bool HybridAStar::TrajectoryPartition(
    const HybridAStartResult& result,
    std::vector<HybridAStartResult>* partitioned_result) {
  const auto& x = result.x;
  const auto& y = result.y;
  const auto& phi = result.phi;
  ...
  size_t horizon = x.size();
  partitioned_result->clear();
  partitioned_result->emplace_back();
  auto* current_traj = &(partitioned_result->back());
  double heading_angle = phi.front();
  const Vec2d init_tracking_vector(x[1] - x[0], y[1] - y[0]);
  double tracking_angle = init_tracking_vector.Angle();
  bool current_gear =
    std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle))<M_PI_2;
  //此时的result只有路径静态信息，x,y,phi
  //将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段
  for (size_t i = 0; i < horizon - 1; ++i) {
    heading_angle = phi[i];
    const Vec2d tracking_vector(x[i + 1] - x[i], y[i + 1] - y[i]);
    tracking_angle = tracking_vector.Angle();
    bool gear =
      std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle))<M_PI_2;
    if (gear != current_gear) {
      current_traj->x.push_back(x[i]);
      current_traj->y.push_back(y[i]);
      current_traj->phi.push_back(phi[i]);
      partitioned_result->emplace_back();
      current_traj = &(partitioned_result->back());
      current_gear = gear;
    }
    current_traj->x.push_back(x[i]);
    current_traj->y.push_back(y[i]);
    current_traj->phi.push_back(phi[i]);
  }
  current_traj->x.push_back(x.back());
  current_traj->y.push_back(y.back());
  current_traj->phi.push_back(phi.back());

  // Retrieve v, a and steer from path
  for (auto& result : *partitioned_result) {
    //2种不同的方式获取轨迹动态信息，v,a,steer。区别: 前者用数值优化的方法，
    //后者用相邻点静态信息
    if (FLAGS_use_s_curve_speed_smooth) {
      //使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
      if (!GenerateSCurveSpeedAcceleration(&result)) { ... }

      //原来的result中包含x,y,phi等路径静态信息，QP优化后添加了动态信息 s,v,a,steer
      //为什么下面还要再次将已知的动静态信息combine呢？我认为：
      //QP优化是根据静态信息求动态，求得动态信息后，再积分反推出静态信息，减小误差
      SpeedData speed_data;
      std::vector<common::PathPoint> path_points;
      const size_t size_x = result.x.size();
      for (size_t i = 0; i < size_x; ++i) {
        common::PathPoint path_point = common::util::MakePathPoint(
            result.x[i], result.y[i], 0.0, result.phi[i], 0.0, 0.0, 0.0);
        path_point.set_s(result.accumulated_s[i]);
        path_points.emplace_back(std::move(path_point));
        speed_data.AppendSpeedPoint( ... );
      }      
      DiscretizedPath discretized_path(path_points);
      //合并静态路径和速度等动态信息
      if (!CombinePathAndSpeedProfile(discretized_path, speed_data, &result)) { ... }
    } else {
      //根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steer
      if (!GenerateSpeedAcceleration(&result)) { ... }
    }
  }
  return true;
}

7.2 轨迹平滑，计算动态信息，完善轨迹

求动态信息的第一种方式：QP优化，与参考线平滑是一个道理。

//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {
  ...
  const size_t x_size = result->x.size();
  double accumulated_s = 0.0;
  std::vector<std::pair<double, double>> x_bounds;
  std::vector<std::pair<double, double>> dx_bounds;
  // Setup for initial point.
  result->accumulated_s.push_back(0.0);
  result->v.push_back(0.0);
  x_bounds.emplace_back(-10.0, 10.0);
  dx_bounds.emplace_back(0.0, 0.0);

  for (size_t i = 0; i + 1 < x_size; ++i) {
    //求s轴速度，XY方向速度分量向运动合速度方向投影
    const double discrete_v = ((result->x[i + 1] - result->x[i]) / delta_t_) *
                                  std::cos(result->phi[i]) +
                              ((result->y[i + 1] - result->y[i]) / delta_t_) *
                                  std::sin(result->phi[i]);
    accumulated_s += discrete_v * delta_t_;
    result->v.push_back(discrete_v);
    result->accumulated_s.push_back(accumulated_s);
    //设置取值范围，在优化时添加各点处取值范围不等式约束
    x_bounds.emplace_back(accumulated_s - 10, accumulated_s + 10);
    dx_bounds.emplace_back(discrete_v - 10, discrete_v + 10);
  }

  // Force last point velocity to be zero
  //我觉得最后的点速度和加速度设置为0，是为了安全。如果后续不再有轨迹计算和输出，
  //车自然停止。因为规划是高频快速进行的，后面输出的轨迹会覆盖前面还没有实际走完
  //的轨迹，车不会停止，正常行驶
  result->v[x_size - 1] = 0.0;
  dx_bounds[x_size - 1] = {0.0, 0.0};
  std::array<double, 3> init_s = {result->accumulated_s[0], result->v[0],
                                  (result->v[1] - result->v[0]) / delta_t_};
  // Start a PathTimeQpProblem
  std::array<double, 3> end_s = {result->accumulated_s[x_size - 1], 0.0, 0.0};

  //使用QP优化方法求frenet系下的轨迹
  PiecewiseJerkSpeedProblem path_time_qp(x_size, delta_t_, init_s);
  //设置各种权重和约束条件
  ...
  // Solve the problem
  if (!path_time_qp.Optimize()) { ... }
  // Extract output
  ...

  // load steering from phi
  for (size_t i = 0; i + 1 < x_size; ++i) {
    double discrete_steer = (result->phi[i + 1] - result->phi[i]) *
                            vehicle_param_.wheel_base() / step_size_;
    if (result->v[i] > 0.0) {
      discrete_steer = std::atan(discrete_steer);
    } else {
      discrete_steer = std::atan(-discrete_steer);
    }
    result->steer.push_back(discrete_steer);
  }

  return true;
}

7.3 计算动态信息，完善轨迹

求动态信息的第二种方式：根据前后相邻点的静态信息朴素地计算速度、加速度等动态信息。

//根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steer
bool HybridAStar::GenerateSpeedAcceleration(HybridAStartResult* result) {
  ...
  const size_t x_size = result->x.size();
  // load velocity from position
  // initial and end speed are set to be zeros
  result->v.push_back(0.0);
  for (size_t i = 1; i + 1 < x_size; ++i) {
    //求s轴速度，不同方向的速度怎么可以直接相加求合速度呢？
    double discrete_v = (((result->x[i + 1] - result->x[i]) / delta_t_) *
                             std::cos(result->phi[i]) +
                         ((result->x[i] - result->x[i - 1]) / delta_t_) *
                             std::cos(result->phi[i])) / 2.0 +
                        //上面是x方向上，利用连续3点的坐标求中间点的速度，平均速度
                        (((result->y[i + 1] - result->y[i]) / delta_t_) *
                             std::sin(result->phi[i]) +
                         ((result->y[i] - result->y[i - 1]) / delta_t_) *
                             std::sin(result->phi[i])) / 2.0;
                        //上面是y方向上，利用连续3点的坐标求中间点的速度，平均速度
    result->v.push_back(discrete_v);
  }
  result->v.push_back(0.0);

  // load acceleration from velocity
  for (size_t i = 0; i + 1 < x_size; ++i) {
    const double discrete_a = (result->v[i + 1] - result->v[i]) / delta_t_;
    result->a.push_back(discrete_a);
  }

  // load steering from phi
  for (size_t i = 0; i + 1 < x_size; ++i) {
    double discrete_steer = (result->phi[i + 1] - result->phi[i]) *
                            vehicle_param_.wheel_base() / step_size_;
    if (result->v[i] > 0.0) {
      discrete_steer = std::atan(discrete_steer);
    } else {
      discrete_steer = std::atan(-discrete_steer);
    }
    result->steer.push_back(discrete_steer);
  }
  return true;
}

至此，Hybrid A*轨迹规划结束，接下来由数值优化方法来做平滑。

8. Hybrid A*的调用

Planning模块关于规划场景的设置、规划方法的调用比较复杂，建议在看这一小节之前，先看一下这一篇，

silver bullet：Baidu Apollo代码解析之Planning的结构与调用流程（2）23 赞同 · 0 评论文章正在上传…重新上传取消

搞懂Scenario、Stage、Task之间的关系。同时要注意，Apollo中的PublicRoadPlanner其实是涵盖了OpenSpacePlanner的，OpenSpacePlanner用来处理public road所包含的u-turn和valet parking 2种场景。

在apollo/modules/planning/conf/planning_config.pb.txt 配置文件中，可以看到PublicRoadPlanner 支持 VALET_PARKING 场景。而该场景分为2个stage：VALET_PARKING_APPROACHING_PARKING_SPOT 和 VALET_PARKING_PARKING。其中，VALET_PARKING_PARKING stage 包含 OPEN_SPACE_TRAJECTORY_PROVIDER 这个task，该task就是OpenSpacePlanner的工作。

在 apollo/modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h 和 open_space_trajectory_optimizer.h 文件中分别定义了OpenSpaceTrajectoryProvider 和 OpenSpaceTrajectoryOptimizer类。在OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() 中会调用open_space_trajectory_optimizer_->Plan()。OpenSpaceTrajectoryOptimizer类有一个std::unique_ptr<HybridAStar> warm_start_ 成员变量。在 OpenSpaceTrajectoryOptimizer::Plan() 中，会调用 warm_start_->Plan()，从而进入第2小节所述的Hybrid A*的处理流程。