APOLLO规划算法Lattice plan算法学习

    最近在看百度阿波罗的动态规划算法，当前版本主要使用的是lattice plan,2.0版本使用的是EM plan，本篇文章主要记录Lattice plan算法：

Lattice planner程序入口：

Status LatticePlanner::PlanOnReferenceLine(
    const TrajectoryPoint& planning_init_point, Frame* frame,
    ReferenceLineInfo* reference_line_info)

    在lattice_planner.cc文件中，通过for循环进入规划，执行函数为PlanOnReferenceLine.该函数具体执行流程如下：

    1.获得当前参考线（车辆未来轨迹），并通过TodiscretizedReferenceLine进行参考线的离散化.

  auto ptr_reference_line =
      std::make_shared>(ToDiscretizedReferenceLine(
          reference_line_info->reference_line().reference_points()));

    2.使用规划原点匹配第一步离散化后的轨迹，找出匹配点matched_point。

  PathPoint matched_point = PathMatcher::MatchToPath(
      *ptr_reference_line, planning_init_point.path_point().x(),
      planning_init_point.path_point().y());

    3.使用ComputeInitFrenetState计算当前状态下，Frenet坐标系下的点，S和D值，横向速度，纵向速度等。

ComputeInitFrenetState(matched_point, planning_init_point, &init_s, &init_d);

    4.加入感知信息，将之前感知模块内容加入到path_time_grapth，也就是所谓的ST-graph

auto ptr_prediction_querier = std::make_shared(
      frame->obstacles(), ptr_reference_line);

加入感知信息时，里面涉及多个函数，具体包括障碍物筛选，去掉与车辆未来轨迹不发生冲突的障碍物，设置动态障碍物。

在这里还有一个planning_target判断，即判断是否有停车点，如果有的话打印出停车点的s值（Frenet坐标系下的纵向值）.

 

5. 这一步很关键，该步是生成横纵向轨迹：

Trajectory1dGenerator trajectory1d_generator(
      init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
  std::vector> lon_trajectory1d_bundle;
  std::vector> lat_trajectory1d_bundle;
  trajectory1d_generator.GenerateTrajectoryBundles(
      planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);

涉及的函数很多，具体包括纵向轨迹生成以及横向轨迹生成，通过设置初始状态、末状态，通过五次多项式拟合，得出相关系数，再通过求导，即可得出速度，再求导得出加速度。再求导得出的加加速度，也就是Jerk，这是用来评价轨迹的舒适度的变量。

void Trajectory1dGenerator::GenerateLongitudinalTrajectoryBundle(
    const PlanningTarget& planning_target,
    Trajectory1DBundle* ptr_lon_trajectory_bundle) const {
  // cruising trajectories are planned regardlessly.
  GenerateSpeedProfilesForCruising(planning_target.cruise_speed(),
                                   ptr_lon_trajectory_bundle);

  GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);

  if (planning_target.has_stop_point()) {
    GenerateSpeedProfilesForStopping(planning_target.stop_point().s(),
                                     ptr_lon_trajectory_bundle);
  }
}

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
    Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  if (!FLAGS_lateral_optimization) {
    auto end_conditions = end_condition_sampler_.SampleLatEndConditions();

    // Use the common function to generate trajectory bundles.
    GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
                                  ptr_lat_trajectory_bundle);
  } else {
    double s_min = init_lon_state_[0];
    double s_max = s_min + FLAGS_max_s_lateral_optimization;

    double delta_s = FLAGS_default_delta_s_lateral_optimization;

    auto lateral_bounds =
        ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);

    // LateralTrajectoryOptimizer lateral_optimizer;
    std::unique_ptr lateral_optimizer(
        new LateralOSQPOptimizer);

    lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);

    auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();

    ptr_lat_trajectory_bundle->push_back(
        std::make_shared(lateral_trajectory));
  }
}

首先，生成纵向轨迹边界：包括生成巡航模式下速度文件，生成障碍物的速度文件，若果有停车点，就专门生成一个针对停车点的速度文件。

void Trajectory1dGenerator::GenerateSpeedProfilesForStopping(
    const double stop_point,
    Trajectory1DBundle* ptr_lon_trajectory_bundle)

   上述的生成障碍物速度文件函数较为复杂，里面又包含了生成跟车模式下以及生成超车模式下的终止状态采样点。

void Trajectory1dGenerator::GenerateSpeedProfilesForPathTimeObstacles(
    Trajectory1DBundle* ptr_lon_trajectory_bundle) const {
  auto end_conditions =
      end_condition_sampler_.SampleLonEndConditionsForPathTimePoints();
  if (end_conditions.empty()) {
    return;
  }

再说一下横向控制，

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
    Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  if (!FLAGS_lateral_optimization) {
    auto end_conditions = end_condition_sampler_.SampleLatEndConditions();

    // Use the common function to generate trajectory bundles.
    GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
                                  ptr_lat_trajectory_bundle);
  } else {
    double s_min = init_lon_state_[0];
    double s_max = s_min + FLAGS_max_s_lateral_optimization;

    double delta_s = FLAGS_default_delta_s_lateral_optimization;

    auto lateral_bounds =
        ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);

    // LateralTrajectoryOptimizer lateral_optimizer;
    std::unique_ptr lateral_optimizer(
        new LateralOSQPOptimizer);

    lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);

    auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();

    ptr_lat_trajectory_bundle->push_back(
        std::make_shared(lateral_trajectory));
  }

横向控制是根据纵向控制来进行采样  其中横向位移为{0，-0.5,0.5}、纵向位移为{10,20,40,80}分别采样，最后得到多组横向的轨迹，同样也是使用5次多项式拟合。注意：横向的轨迹是以纵向位移s为自变量.

6.轨迹评价函数

  上一步生成了许多条轨迹，这里就需要选择出cost最小的一条轨迹作为未来轨迹，

TrajectoryEvaluator trajectory_evaluator(
      init_s, planning_target, lon_trajectory1d_bundle, lat_trajectory1d_bundle,
      ptr_path_time_graph, ptr_reference_line);

   Evaluate评估的时候有5个cost值，分别是

1.    cost of missing the obkective ;
2.   cost  of logitudinal jerk
3.   cost of logitudinal collision
4.  cost of lateral offsets
5.  cost of laterlal comfort

    具体评估函数比较清晰，可以自行观看。

   接下来对候选的轨迹集按照cost大小排序，最终选择cost最小且无碰撞的轨迹

最后生成轨迹，通过下列函数，进行轨迹赋值

reference_line_info->SetTrajectory(combined_trajectory);