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<std::vector<PathPoint>>(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_predictio