百度apollo planning代码学习-Apollo\modules\planning\lattice\trajectory_generation\Trajectory1dGenerator类详解

概述

Trajectory1dGenerator类是apollo planning模块下modules\planning\lattice\trajectory_generation\trajectory1d_generator.cc/.h实现

从类名来看，应该是Trajectory1dGenerator是1维轨迹生成器类？

从代码来看Trajectory1dGenerator类主要是实现：
1.储存纵向初始状态，横向初始状态，ST图信息及横向边界信息ptr_path_time_graph，障碍物的预测速度在参考线上的投影信息ptr_prediction_querier；
2.根据1中储存的信息生成横纵向的轨迹束；
3.根据巡航速度，用四次多项式生成巡航的速度规划的纵向轨迹束；
4.根据停止点信息，用5次多项式生成停止场景的速度规划的纵向轨迹束；
5.考虑障碍物的巡航速度规划纵向轨迹束生成；
6.求解QP优化问题生成最优的横向轨迹塞入横向候选轨迹束(里面就一条最优横向轨迹？)，所以就是纵向用lattice撒点生成多个终端条件生成多个轨迹，横向就QP直接求出一条最优的轨迹？

简而言之，该类主要就是根据纵向初始状态，横向初始状态，ST图信息及横向边界信息，障碍物的预测速度在参考线上的投影信息拿到一起，然后去生成横纵向的候选轨迹束。基本不含具体实现，都是调用其他类的接口。

trajectory1d_generator.h

#pragma once

#include <memory>
#include <utility>
#include <vector>

#include "modules/planning/lattice/behavior/path_time_graph.h"
#include "modules/planning/lattice/behavior/prediction_querier.h"
#include "modules/planning/lattice/trajectory_generation/end_condition_sampler.h"
#include "modules/planning/lattice/trajectory_generation/lattice_trajectory1d.h"
#include "modules/planning/math/curve1d/curve1d.h"
#include "modules/planning/math/curve1d/quartic_polynomial_curve1d.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
#include "modules/planning/proto/lattice_structure.pb.h"

namespace apollo {
namespace planning {

class Trajectory1dGenerator {
 public:
 //1维轨迹生成器构造函数
//输入参数纵向初始状态，横向初始状态，ST图信息及横向边界信息ptr_path_time_graph，障碍物的预测速度在参考线上的投影信息ptr_prediction_querier
  Trajectory1dGenerator(
      const std::array<double, 3>& lon_init_state,
      const std::array<double, 3>& lat_init_state,
      std::shared_ptr<PathTimeGraph> ptr_path_time_graph,
      std::shared_ptr<PredictionQuerier> ptr_prediction_querier);

  virtual ~Trajectory1dGenerator() = default;

//生成轨迹束函数(一系列的候选轨迹)
//输入规划目标(停止点和巡航速度信息)
//生成的纵向轨迹束放入ptr_lon_trajectory_bundle指针 Trajectory1DBundle是1维曲线的vector数组
//生成的横向轨迹束放入ptr_lat_trajectory_bundle指针 Trajectory1DBundle是1维曲线的vector数组
  void GenerateTrajectoryBundles(
      const PlanningTarget& planning_target,
      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle,
      std::vector<std::shared_ptr<Curve1d>>* ptr_lat_trajectory_bundle);

//生成横向轨迹束的函数 输入参数指针ptr_lat_trajectory_bundle用于存放生成的横向轨迹束
  void GenerateLateralTrajectoryBundle(
      std::vector<std::shared_ptr<Curve1d>>* ptr_lat_trajectory_bundle) const;

 private:
 //针对巡航产生速度规划函数，输入参数目标速度，生成的结果存放在输入参数的指针ptr_lon_trajectory_bundle，返回巡航的纵向轨迹束？
  void GenerateSpeedProfilesForCruising(
      const double target_speed,
      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
//产生的停止场景下的速度规划，输入参数是停止点s? ptr_lon_trajectory_bundle用于存放生成的一系列纵向轨迹
  void GenerateSpeedProfilesForStopping(
      const double stop_point,
      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
//考虑障碍物的巡航速度规划纵向轨迹束生成，输入参数只有指针ptr_lon_trajectory_bundle，用来存放生成的结果
  void GenerateSpeedProfilesForPathTimeObstacles(
      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;
//生成纵向的轨迹束
//输入参数 规划目标(巡航速度以及停止点信息)
//输入参数 ptr_lon_trajectory_bundle用于存放生成的结果
  void GenerateLongitudinalTrajectoryBundle(
      const PlanningTarget& planning_target,
      std::vector<std::shared_ptr<Curve1d>>* ptr_lon_trajectory_bundle) const;

//一个生成轨迹的模板函数，N代表N次多项式生成的方法，输入初始状态，中断状态，生成结果放入轨迹束指针ptr_trajectory_bundle里
  template <size_t N>
  void GenerateTrajectory1DBundle(
      const std::array<double, 3>& init_state,
      const std::vector<std::pair<std::array<double, 3>, double>>&
          end_conditions,
      std::vector<std::shared_ptr<Curve1d>>* ptr_trajectory_bundle) const;

 private:
  std::array<double, 3> init_lon_state_;

  std::array<double, 3> init_lat_state_;

  EndConditionSampler end_condition_sampler_;

  std::shared_ptr<PathTimeGraph> ptr_path_time_graph_;
};

//4次多项式的方法生成轨迹束，输入参数初始状态，候选的终端条件集合，输入参数ptr_trajectory_bundle指针用于存放生成的结果，调用相应的类QuarticPolynomialCurve1d实现
template <>
inline void Trajectory1dGenerator::GenerateTrajectory1DBundle<4>(
    const std::array<double, 3>& init_state,
    const std::vector<std::pair<std::array<double, 3>, double>>& end_conditions,
    std::vector<std::shared_ptr<Curve1d>>* ptr_trajectory_bundle) const {
  CHECK_NOTNULL(ptr_trajectory_bundle);
  //确保end_conditions候选的终端条件集合里非空，
  ACHECK(!end_conditions.empty());
  //终端条件里面有多少个，轨迹束指针容量就新增多少
  ptr_trajectory_bundle->reserve(ptr_trajectory_bundle->size() +
                                 end_conditions.size());
  //遍历每一个终端条件
  for (const auto& end_condition : end_conditions) {
  //QuarticPolynomialCurve1d四次多项式一维轨迹类，输入初始状态，终端速度s'，加速度s''，以及终端的时间end_condition.second
    auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(
        std::shared_ptr<Curve1d>(new QuarticPolynomialCurve1d(
            init_state, {end_condition.first[1], end_condition.first[2]},
            end_condition.second)));

//设置目标速度，将终端条件的速度设置为1维轨迹指针的目标速度
    ptr_trajectory1d->set_target_velocity(end_condition.first[1]);
    //设置目标时间，将终端条件的时间设置为1维轨迹指针的目标时间
    ptr_trajectory1d->set_target_time(end_condition.second);
    //轨迹指针塞入轨迹束指针里
    ptr_trajectory_bundle->push_back(ptr_trajectory1d);
  }
}

//5次多项式的方法生成轨迹束，输入参数初始状态，候选的终端条件集合，输入参数ptr_trajectory_bundle指针用于存放生成的结果，和上面四次的类似，也是调用相应的类QuinticPolynomialCurve1d实现
template <>
inline void Trajectory1dGenerator::GenerateTrajectory1DBundle<5>(
    const std::array<double, 3>& init_state,
    const std::vector<std::pair<std::array<double, 3>, double>>& end_conditions,
    std::vector<std::shared_ptr<Curve1d>>* ptr_trajectory_bundle) const {
  CHECK_NOTNULL(ptr_trajectory_bundle);
  ACHECK(!end_conditions.empty());

  ptr_trajectory_bundle->reserve(ptr_trajectory_bundle->size() +
                                 end_conditions.size());
  for (const auto& end_condition : end_conditions) {
    auto ptr_trajectory1d = std::make_shared<LatticeTrajectory1d>(
        std::shared_ptr<Curve1d>(new QuinticPolynomialCurve1d(
            init_state, end_condition.first, end_condition.second)));

    ptr_trajectory1d->set_target_position(end_condition.first[0]);
    ptr_trajectory1d->set_target_velocity(end_condition.first[1]);
    ptr_trajectory1d->set_target_time(end_condition.second);
    ptr_trajectory_bundle->push_back(ptr_trajectory1d);
  }
}

}  // namespace planning
}  // namespace apollo

trajectory1d_generator.cc

#include "modules/planning/lattice/trajectory_generation/trajectory1d_generator.h"

#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/trajectory1d/constant_deceleration_trajectory1d.h"
#include "modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.h"
#include "modules/planning/common/trajectory1d/standing_still_trajectory1d.h"
#include "modules/planning/lattice/trajectory_generation/lateral_osqp_optimizer.h"
#include "modules/planning/lattice/trajectory_generation/lateral_qp_optimizer.h"

namespace apollo {
namespace planning {

//一个通用函数，当给定初始状态和终端状态时生成轨迹束(一些列轨迹的集合)
typedef std::array<double, 3> State;   //状态类，就是一个3维向量
typedef std::pair<State, double> Condition; //终端条件
//1维轨迹束，是1维轨迹的vector数组
typedef std::vector<std::shared_ptr<Curve1d>> Trajectory1DBundle;
//1维轨迹生成器构造函数
//输入参数纵向初始状态，横向初始状态，ST图信息及横向边界信息ptr_path_time_graph，障碍物的预测速度在参考线上的投影信息ptr_prediction_querier
Trajectory1dGenerator::Trajectory1dGenerator(
    const State& lon_init_state, const State& lat_init_state,
    std::shared_ptr<PathTimeGraph> ptr_path_time_graph,
    std::shared_ptr<PredictionQuerier> ptr_prediction_querier)
    : init_lon_state_(lon_init_state), //输入的横纵向初始状态都塞入类成员中
      init_lat_state_(lat_init_state),
      //end_condition_sampler_所属类就是对于不同的场景，跟车/超车/巡航/刹停等场景下横纵向的末端状态空间进行撒点采样，生成不同的可行的边界条件，输入横纵向初始状态以及ST图信息，以及障碍物的预测速度信息
      end_condition_sampler_(lon_init_state, lat_init_state,
                             ptr_path_time_graph, ptr_prediction_querier),
      //用输入的ST图信息赋值给类成员
      ptr_path_time_graph_(ptr_path_time_graph) {}

//生成轨迹束函数(一系列的候选轨迹)
//输入规划目标(停止点和巡航速度信息)
//生成的纵向轨迹束放入ptr_lon_trajectory_bundle指针 Trajectory1DBundle是1维曲线的vector数组
//生成的横向轨迹束放入ptr_lat_trajectory_bundle指针 Trajectory1DBundle是1维曲线的vector数组
void Trajectory1dGenerator::GenerateTrajectoryBundles(
    const PlanningTarget& planning_target,
    Trajectory1DBundle* ptr_lon_trajectory_bundle,
    Trajectory1DBundle* ptr_lat_trajectory_bundle) {
  //这个函数就是个大函数，调用了生成纵向轨迹束的函数，调用了生成横向轨迹束的函数
  GenerateLongitudinalTrajectoryBundle(planning_target,
                                       ptr_lon_trajectory_bundle);

  GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
}

//针对巡航产生速度规划函数，输入参数目标速度，生成的结果存放在输入参数的指针ptr_lon_trajectory_bundle，返回巡航的纵向轨迹束？
void Trajectory1dGenerator::GenerateSpeedProfilesForCruising(
    const double target_speed,
    Trajectory1DBundle* ptr_lon_trajectory_bundle) const {
  ADEBUG << "cruise speed is  " << target_speed;
 //首先将类成员里的终端采样器对象调用其成员函数撒点生成一系列巡航的场景下的纵向终端条件(s.s',s'')，其原理详见我另一篇关于此类的博客。
  auto end_conditions =
      end_condition_sampler_.SampleLonEndConditionsForCruising(target_speed);
  //如果最后生成的终端条件的数目为空，则直接返回
  if (end_conditions.empty()) {
    return;
  }

  // For the cruising case, We use the "QuarticPolynomialCurve1d" class (not the
  // "QuinticPolynomialCurve1d" class) to generate curves. Therefore, we can't
  // invoke the common function to generate trajectory bundles.
  //对于巡航场景，我们用QuarticPolynomialCurve1d四次多项式1维曲线类(而不是5此多项式)来生成曲线，我们不能调用公共函数以生成轨迹束？？？apollo原注释
  //输入参数初始纵向状态，终端条件，ptr_lon_trajectory_bundle用于存放生成的结果，存放在输入的参数指针里
  GenerateTrajectory1DBundle<4>(init_lon_state_, end_conditions,
                                ptr_lon_trajectory_bundle);
}

//产生的停止场景下的速度规划，输入参数是停止点s? ptr_lon_trajectory_bundle用于存放生成的一系列纵向轨迹
void Trajectory1dGenerator::GenerateSpeedProfilesForStopping(
    const double stop_point,
    Trajectory1DBundle* ptr_lon_trajectory_bundle) const {
  ADEBUG << "stop point is " << stop_point;
  //输入停止点信息，调用终端采样器获取终端条件的采样点
  auto end_conditions =
      end_condition_sampler_.SampleLonEndConditionsForStopping(stop_point);
  //如果终端采样条件的个数为空则直接返回
  if (end_conditions.empty()) {
    return;
  }

  // Use the common function to generate trajectory bundles.
  //使用通用的函数来生成轨迹束  5次多项式
  //输入初始的纵向状态，终端条件采样序列，生成的轨迹束放入输入的参数指针ptr_lon_trajectory_bundle里
  GenerateTrajectory1DBundle<5>(init_lon_state_, end_conditions,
                                ptr_lon_trajectory_bundle);
}

//考虑障碍物的巡航速度规划纵向轨迹束生成，输入参数只有指针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;
  }

  // Use the common function to generate trajectory bundles.
  //使用通用的函数生成轨迹束，五次多项式，输入参数初始纵向状态，终端状态空间采样点集，指针参数ptr_lon_trajectory_bundle用来存放最终的生成结果
  GenerateTrajectory1DBundle<5>(init_lon_state_, end_conditions,
                                ptr_lon_trajectory_bundle);
}

//生成纵向的轨迹束
//输入参数 规划目标(巡航速度以及停止点信息)
//输入参数 ptr_lon_trajectory_bundle用于存放生成的结果
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);

//又调用函数生成考虑障碍物巡航下的速度规划，输入参数 ptr_lon_trajectory_bundle用于存放生成的轨迹束
  GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);
//如果规划目标有停止点
  if (planning_target.has_stop_point()) {
  //调用函数生成停车的速度规划，生成的结果存放在输入的参数指针ptr_lon_trajectory_bundle里
    GenerateSpeedProfilesForStopping(planning_target.stop_point().s(),
                                     ptr_lon_trajectory_bundle);
  }
}

//生成横向轨迹束的函数 输入参数指针ptr_lat_trajectory_bundle用于存放生成的横向轨迹束
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
    Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  //如果没有开启横向的优化，lateral_optimization在planning_gflags.cc里被定义
  //默认是true，是开启的，所以这个if不会被执行直接看else
  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 { //开启横向优化的情况下
  //定义最小的s为初始纵向状态的纵向位置
    double s_min = init_lon_state_[0];
    //定义最大的s = 初始的纵向位置s + 最大的横向优化的s为60m
    double s_max = s_min + FLAGS_max_s_lateral_optimization;
    //横向优化时的s间隔为1.0m default_delta_s_lateral_optimization在planning_gflags.cc里设置
    double delta_s = FLAGS_default_delta_s_lateral_optimization;
	//用路径时间图对象(在构造该类时就会输入)，参见我另一篇博客 百度apollo自动驾驶planning代码学习-Apollo\modules\planning\lattice\behavior\PathTimeGraph类代码详解，获取自车在整条纵向轨迹上的横向边界，纵向每隔1.0m对横向边界采样一次，lateral_bounds理解为一个数组存放着smin到smax之间的各个纵向坐标对应的横向边界，比如s=s0处对应的横向边界[d0_lower,d0_upper]
    auto lateral_bounds =
        ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);

    // LateralTrajectoryOptimizer lateral_optimizer;
    //构建一个横向QP优化对象 lateral_optimizer
    std::unique_ptr<LateralQPOptimizer> lateral_optimizer(
        new LateralOSQPOptimizer);
    //根据LateralQPOptimizer类里定义的二次规划的目标函数，输入参数初始的横向状态，纵向采样间隔，横向边界序列，求解横向轨迹的QP问题
    lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);

 	//用横向QP优化对象 lateral_optimizer取出二次规划求解出的最优轨迹lateral_trajectory 
    auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();
	//将求解到的最优的横向轨迹塞入横向轨迹束指针里
    ptr_lat_trajectory_bundle->push_back(
        std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));
  }
}

}  // namespace planning
}  // namespace apollo