百度apollo planning-Apollo\modules\planning\lattice\trajectory_generation\EndConditionSampler类详解

概述

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

从类名来看，应该是终端条件采样器类？

从代码来看EndConditionSampler类主要是实现：
简而言之，该类就是对于不同的场景，跟车/超车/巡航/刹停等场景下横纵向的末端状态空间进行采样，生成不同的可行的边界条件，后续可以用于生成不同的路径及速度，作为后续路径规划和速度规划的搜索空间。

1.储存车辆初始的横纵向状态(s,s’,s’‘), (d,d’,d’'), 路径时间图对象(主要实现自车ST图的构建)，预测查询器对象(主要就是将障碍物的预测速度投影到参考线上)等信息；
2.在s=10,20,40,80m的每个纵向位置s上进行终端可能的横向位置的采样，分别采样0,-0.5m,0.5m;(原理见附录图片1)
3.针对巡航场景，对终端的可能纵向速度进行采样；(原理见附录图片2)
4.针对刹停stopping场景，对终端的可能的纵向位置进行采样；(既然刹停，那么终端纵向位置为初始纵向位置 或 参考停止点之间的较小值)；(原理见附录图片3)
5.针对跟车或超车场景下障碍物的路径-时间点信息进行终端纵向状态(s,v,a)的采样；
6.获取自车跟车或超车的路径时间采样点？实际上就是7或8里的一种，跟车或超车的路径时间点？
7.查询针对障碍物跟车时自车的路径时间点集(s,t,v)，跟车时自车的s上界为障碍物的s(推测应该用的时障碍物的start_s)减去自车前轴到质心的距离，跟车时自车的s下界为s上界再减去5m， 采样3个点（s下界，s下界+2.5m, s上界), 采样的路径时间点的速度即为障碍物的速度v;(原理见附录图片4)
8.查询针对障碍物自车超车时的路径时间点集(s,t,v)，超车时自车的s始终为障碍物的s(推测应为障碍物的end_s+5m)，自车的速度为障碍物的速度；(原理见附录图片5)

*State类数据类型是一个三个元素的数组
State init_s 就是(s,ds,dds)
State init_d就是(L,dL,ddL)
Condition条件类对象就是描述了一个纵向位置s对应的横向d,d’,d’‘，或是一个时间t对应的纵向位置的s,s’,s’’

end_condition_sampler.h

#pragma once

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

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/lattice/behavior/feasible_region.h"
#include "modules/planning/lattice/behavior/path_time_graph.h"
#include "modules/planning/lattice/behavior/prediction_querier.h"

namespace apollo {
namespace planning {

//定义结构体采样点,包含s,t,v信息
struct SamplePoint {
  STPoint path_time_point;
  double ref_v;
};

// Input: planning objective, vehicle kinematic/dynamic constraints,
// Output: sampled ending 1 dimensional states with corresponding time duration.
//该类的输入：规划的目标，车辆运动/动力学约束
//该类的输出：对应相对时间的采样的1维离散状态，横向，纵向
class EndConditionSampler {
 public:
 //终端条件采样器，输入初始的纵向状态init_s(s,s',s'')，初始的横向状态init_d(d,d',d''),路径时间图对象，预测查询器对象
//带参构造函数
//这些输入参数去初始化类成员
  EndConditionSampler(
      const std::array<double, 3>& init_s, const std::array<double, 3>& init_d,
      std::shared_ptr<PathTimeGraph> ptr_path_time_graph,
      std::shared_ptr<PredictionQuerier> ptr_prediction_querier);

//默认析构函数
  virtual ~EndConditionSampler() = default;

//函数采样终端的横向边界条件，返回的结果是一个condition类的vector数组
//using Condition = std::pair<State, double>;Condition类由横向状态，(s,s',s'')或(d,d',d'')，以及对应的纵坐标s构成
//这个函数就是生成所有的候选的终端横向边界条件{{{d0,d0',d0''},s0},{{d1,d1',d1''},s1}},...}，最后生成的效果见附录图1
  std::vector<std::pair<std::array<double, 3>, double>> SampleLatEndConditions()
      const;

//纵向巡航的终端条件采样，返回的结果是一个condition类的vector
//输入参数:参考巡航速度ref_cruise_speed
//为什么返回的终端的s始终输入0?还是说这里主要是采样s'和t的对应关系
  std::vector<std::pair<std::array<double, 3>, double>>
  SampleLonEndConditionsForCruising(const double ref_cruise_speed) const;

//纵向停止的终端条件采样，返回的结果是一个condition类的vector
//输入参数:参考停止点ref_stop_point
//为什么返回的终端的s始终输入0?还是说这里主要是采样s'和t的对应关系
  std::vector<std::pair<std::array<double, 3>, double>>
  SampleLonEndConditionsForStopping(const double ref_stop_point) const;

//纵向终端条件采样，为了路径时间点？根据自车跟车或超车的路径时间点去构建
  std::vector<std::pair<std::array<double, 3>, double>>
  SampleLonEndConditionsForPathTimePoints() const;

 private:
 //采样点vector数组，查询路径时间障碍物采样点
//获取自车跟车或超车的路径时间采样点？
  std::vector<SamplePoint> QueryPathTimeObstacleSamplePoints() const;

//查询跟车的路径时间点，输入参数是车辆配置，障碍物id，sample_points应该是用于存放输出结果的
//对跟车场景下的纵向终端状态空间进行采样，其实就是跟车时撒点可以贴着前车，留5m间隙，留2.5m间隙这三种纵向位置，然后车速保持和障碍物一样
  void QueryFollowPathTimePoints(
      const apollo::common::VehicleConfig& vehicle_config,
      const std::string& obstacle_id,
      std::vector<SamplePoint>* sample_points) const;

//查询超车的路径时间点，输入参数是车辆配置，障碍物id，计算的结果采样点放入sample_points中,查询超车的路径时间点？
//对超车场景下自车纵向终端状态空间进行采样
//其实就是终端状态始终压着障碍物5m，然后车速与其相同
  void QueryOvertakePathTimePoints(
      const apollo::common::VehicleConfig& vehicle_config,
      const std::string& obstacle_id,
      std::vector<SamplePoint>* sample_points) const;

 private:
  std::array<double, 3> init_s_;
  std::array<double, 3> init_d_;
  FeasibleRegion feasible_region_;
  std::shared_ptr<PathTimeGraph> ptr_path_time_graph_;
  std::shared_ptr<PredictionQuerier> ptr_prediction_querier_;
};

}  // namespace planning
}  // namespace apollo

end_condition_sampler.cc

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

#include <algorithm>

#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

//定义状态是一个3个元素的数组
using State = std::array<double, 3>;
//定义一个条件，其是由状态State和double共同构成的数据对？
using Condition = std::pair<State, double>;

//带参构造函数
//输入参数初始的纵向状态init_s(s,ds,dds)，初始的横向状态init_d(L,dL,ddL)，ptr_path_time_graph路径时间图对象，ptr_prediction_querier预测查询器类
//这些输入参数去初始化类成员
EndConditionSampler::EndConditionSampler(
    const State& init_s, const State& init_d,
    std::shared_ptr<PathTimeGraph> ptr_path_time_graph,
    std::shared_ptr<PredictionQuerier> ptr_prediction_querier)
    : init_s_(init_s),
      init_d_(init_d),
      feasible_region_(init_s),
      ptr_path_time_graph_(std::move(ptr_path_time_graph)),
      ptr_prediction_querier_(std::move(ptr_prediction_querier)) {}

//函数采样终端的横向边界条件，返回的结果是一个condition类的vector数组
//using Condition = std::pair<State, double>;Condition类由横纵向状态，(s,s',s'')或(d,d',d'')，以及对应的时间t或纵坐标s构成一个条件对象
//这个函数就是生成所有的候选的终端横向边界条件{{{d0,d0',d0''},s0},{{d1,d1',d1''},s1}},...}，最后生成的效果见附录图1
std::vector<Condition> EndConditionSampler::SampleLatEndConditions() const {
  //定义了空数组，是Condition类型的vector
  std::vector<Condition> end_d_conditions;
  //定义了一3个元素的数组,是候选的终端的相对参考线的横向位置0.0/-0.5/0.5m
  std::array<double, 3> end_d_candidates = {0.0, -0.5, 0.5};
  //定义了一4个元素的数组，是候选的终端的相对纵向位置10.0/20.0/40.0/80.0m
  std::array<double, 4> end_s_candidates = {10.0, 20.0, 40.0, 80.0};
  //遍历候选的终端的相对纵向位置{10.0, 20.0, 40.0, 80.0}
  for (const auto& s : end_s_candidates) {
    //遍历候选的终端的相对参考线的横向位置{0.0, -0.5, 0.5}
    for (const auto& d : end_d_candidates) {
      //该次循环终端横向状态(d,d',d''), d',d''都认为是0 {d, 0.0, 0.0}
      State end_d_state = {d, 0.0, 0.0};
      //然后将横向的终端状态和s一起塞入横向的终端条件end_d_conditions
      end_d_conditions.emplace_back(end_d_state, s);
    }
  }
  return end_d_conditions;
}

//采样巡航场景下的纵向终端条件，返回的是一系列时间t及对应的s,s',s''，输入的是参考速度
//对巡航场景下的(1.0,2.0,...,8.0s)时刻下的各时刻的终端纵向条件进行采样，主要是对终端纵向速度进行采样，返回结果就是一些列的time以及对应的不同可能的采样速度(成为纵向终端条件)，纵向速度
std::vector<Condition> EndConditionSampler::SampleLonEndConditionsForCruising(
    const double ref_cruise_speed) const {
  CHECK_GT(FLAGS_num_velocity_sample, 1U);
  
  //时间采样点的数量为9，num_of_time_samples
  static constexpr size_t num_of_time_samples = 9;
  //定义一个9个元素的数组，里面存放的就是9个采样时刻
  std::array<double, num_of_time_samples> time_samples;
  //time_samples采样时刻数组里第2-9个元素依次填充1.0，2.0，3.0，4.0，...，8.0s
  for (size_t i = 1; i < num_of_time_samples; ++i) {
    //FLAGS_trajectory_time_length去planning_gflags.cc里取出trajectory_time_length的值就是8s
    auto ratio =
        static_cast<double>(i) / static_cast<double>(num_of_time_samples - 1);
    time_samples[i] = FLAGS_trajectory_time_length * ratio;
  }
  //time_samples采样时刻数组里第1个元素填充0.01s
  time_samples[0] = FLAGS_polynomial_minimal_param;
  //定义1个空的终端纵向条件vector数组
  std::vector<Condition> end_s_conditions;
  //遍历每一个采样时刻(t=0.01, 1.0, 2.0, 3.0, 4.0, ..., 8.0s)
  for (const auto& time : time_samples) {
    //计算车辆在时刻time处以最大加速度能达到的速度v_upper，但不能超过巡航速度
    double v_upper = std::min(feasible_region_.VUpper(time), ref_cruise_speed);
    //计算车辆在时刻time处以最大减速度能达到的最小速度VLower
    double v_lower = feasible_region_.VLower(time);

    //time时刻终端纵向状态下界(这里是不考虑s?)  {0.0, v_lower, 0.0}
    State lower_end_s = {0.0, v_lower, 0.0};
    //time时刻对应的终端纵向状态下界，以及time一起塞入终端纵向s条件数组
    end_s_conditions.emplace_back(lower_end_s, time);

    //time时刻对应终端纵向状态的上界upper_end_s (s,s',s'')为{0.0, v_upper, 0.0}
    //time时刻对应的终端纵向状态上界，以及time一起塞入终端纵向s条件数组
    State upper_end_s = {0.0, v_upper, 0.0};
    end_s_conditions.emplace_back(upper_end_s, time);

    //time时刻对应的最小速度到最大速度的速度跨度v_range
    double v_range = v_upper - v_lower;
    //计算time时刻最大速度和最小速度中间插入采样点的数量
    //num_velocity_sample在planning_gflags.cc里定义为6，意思是加上最大最小速度，采样后保证速度点最多6个
    //min_velocity_sample_gap为1.0m/s
    //采样中间点的数量=(6-2 或 速度跨度/1.0)里的较小值
    size_t num_of_mid_points =
        std::min(static_cast<size_t>(FLAGS_num_velocity_sample - 2),
                 static_cast<size_t>(v_range / FLAGS_min_velocity_sample_gap));

    //如果中间的速度采样点数量>0
    if (num_of_mid_points > 0) {
      //计算最大速度和最小速度之间增加速度采样点后，相邻速度点之间的速度间隔velocity_seg。比如最大速度10,最小速度7,中间采样速度点个数为3，相邻采样速度点之间的速度间隔为 (10-7)/(3+1)=0.75m/s
      double velocity_seg =
          v_range / static_cast<double>(num_of_mid_points + 1);
      //遍历中间的速度采样点
      for (size_t i = 1; i <= num_of_mid_points; ++i) {
        //纵向终端状态end_s，速度分别取为(最低速度 + i个速度间隔)
        //以最大速度10m/s,最小速度7m/s为例的话 则终端纵向状态采样为v=7+i*0.75
        //计算到的中间采样速度，塞入终端纵向状态(s,s',s'')
        State end_s = {0.0, v_lower + velocity_seg * static_cast<double>(i),
                       0.0};
        //终端纵向状态以及对应的时刻time一起塞入终端纵向条件中
        end_s_conditions.emplace_back(end_s, time);
      }
    }
  }
  //返回的是纵向的终端条件
  return end_s_conditions;
}

//纵向停止的终端条件采样，返回的结果是一个condition类的vector
//输入参数:参考停止点ref_stop_point
//采样刹停场景下终端纵向可能的位置
//纵向的终端状态end_s为初始纵向位置 或 参考停止点里的较大值，s',s''均为0
std::vector<Condition> EndConditionSampler::SampleLonEndConditionsForStopping(
    const double ref_stop_point) const {
  //定义时间采样点的数量为9
  static constexpr size_t num_of_time_samples = 9;
  //定义的采样时间time_samples为(t=0.01,1,2,3,...8s)，是一个时间的数组
  std::array<double, num_of_time_samples> time_samples;
  for (size_t i = 1; i < num_of_time_samples; ++i) {
    auto ratio =
        static_cast<double>(i) / static_cast<double>(num_of_time_samples - 1);
    time_samples[i] = FLAGS_trajectory_time_length * ratio;
  }
  //时间采样点中的第一个取为0.01s
  time_samples[0] = FLAGS_polynomial_minimal_param;
//纵向终端条件由纵向状态(s,s',s'')和对应的时间构成？
  std::vector<Condition> end_s_conditions;
  //如果是刹停stopping场景的话，所有的终端纵向条件都是初始的纵向s 或 参考停止点里的较小值
  for (const auto& time : time_samples) {
    //纵向的终端状态end_s为初始纵向位置和参考停止点里的较大值，s',s''均为0
    State end_s = {std::max(init_s_[0], ref_stop_point), 0.0, 0.0};
    //纵向终端状态以及对应的时间time塞入纵向终端条件
    end_s_conditions.emplace_back(end_s, time);
  }
  //返回纵向的终端条件数组
  return end_s_conditions;
}

//纵向终端条件采样，为了路径时间点？根据自车跟车或超车的路径时间点去构建
std::vector<Condition>
EndConditionSampler::SampleLonEndConditionsForPathTimePoints() const {
  //定义一个空的纵向的终端条件vector数组
  std::vector<Condition> end_s_conditions;

//定义一个采样点数组，查询路径时间障碍物采样点？会根据跟车或是超车场景的自车ST路径点采样
  std::vector<SamplePoint> sample_points = QueryPathTimeObstacleSamplePoints();
  for (const SamplePoint& sample_point : sample_points) {
   //如果路径点时间小于0.01的话则直接跳到下一个采样点
    if (sample_point.path_time_point.t() < FLAGS_polynomial_minimal_param) {
      continue;
    }
    //获取第i个采样点的s,v,t后
    double s = sample_point.path_time_point.s();
    double v = sample_point.ref_v;
    double t = sample_point.path_time_point.t();
    //如果采样点的s超出了s的上下界，直接跳到下一个采样点
    if (s > feasible_region_.SUpper(t) || s < feasible_region_.SLower(t)) {
      continue;
    }
    //定义一个纵向终端状态就是就是采样点的s,v,a=0
    State end_state = {s, v, 0.0};
    //将这个纵向状态和对应的时间塞入纵向终端条件
    end_s_conditions.emplace_back(end_state, t);
  }
  return end_s_conditions;
}

//采样点vector数组，查询路径时间障碍物采样点
//获取自车跟车或超车的路径时间采样点？
std::vector<SamplePoint>
EndConditionSampler::QueryPathTimeObstacleSamplePoints() const {
  //车辆物理参数配置
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  //定义一个空的采样点vector数组
  std::vector<SamplePoint> sample_points;
  //遍历路径时间障碍物列表
  for (const auto& path_time_obstacle :
       ptr_path_time_graph_->GetPathTimeObstacles()) {
    //障碍物id=路径时间障碍物id
    std::string obstacle_id = path_time_obstacle.id();
    //查询跟车的路径时间点放入sample_points,SamplePoint包含s,t,v信息
    QueryFollowPathTimePoints(vehicle_config, obstacle_id, &sample_points);
    //查询超车的路径时间点放入sample_points，SamplePoint包含s,t,v信息
    //连续执行这两句不是sample_points被覆盖了吗？可能是默认的是跟车的路径时间点，满足超车条件的话就是超车的
    QueryOvertakePathTimePoints(vehicle_config, obstacle_id, &sample_points);
  }
  //返回采样的路径点
  return sample_points;
}

//查询跟车的路径时间点，输入参数是车辆配置，障碍物id，sample_points应该是用于存放输出结果的
//对跟车场景下的纵向终端状态空间进行采样，其实就是跟车时撒点可以贴着前车，留5m间隙，留2.5m间隙这三种纵向位置，然后车速保持和障碍物一样
void EndConditionSampler::QueryFollowPathTimePoints(
    const common::VehicleConfig& vehicle_config, const std::string& obstacle_id,
    std::vector<SamplePoint>* const sample_points) const {
 
 //获取输入障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)，对其进行二次时间采样变成时间间隔为1.0s(t_min_density)得一系列(s,t)点？
  std::vector<STPoint> follow_path_time_points =
      ptr_path_time_graph_->GetObstacleSurroundingPoints(
          obstacle_id, -FLAGS_numerical_epsilon, FLAGS_time_min_density);

//遍历障碍物从开始阻塞参考线到结束阻塞时间段里一系列的路径点
  for (const auto& path_time_point : follow_path_time_points) {
    //将t时刻障碍物速度投影到参考线上？
    double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
        obstacle_id, path_time_point.s(), path_time_point.t());
    // 产生候选的s
    //候选s的上界就是障碍物的s坐标-车辆前轴到车辆质心的距离
    double s_upper = path_time_point.s() -
                     vehicle_config.vehicle_param().front_edge_to_center();
    //候选s的下界就是上界再-5m
    //所以跟车距离最多就是5m+车辆前轴到质心距离？如果高速呢？
    //default_lon_buffer为5m
    double s_lower = s_upper - FLAGS_default_lon_buffer;
    CHECK_GE(FLAGS_num_sample_follow_per_timestamp, 2U);
    //s的gap间隙？为 5/(3-1)    也就是2.5m
    double s_gap =
        FLAGS_default_lon_buffer /
        static_cast<double>(FLAGS_num_sample_follow_per_timestamp - 1);
    //num_sample_follow_per_timestamp默认为3
    //遍历i=0到2，总共采样3个点
    for (size_t i = 0; i < FLAGS_num_sample_follow_per_timestamp; ++i) {
      //第i个点的s为   s的下界+2.5*i  因为采样间隔为2.5m
      double s = s_lower + s_gap * static_cast<double>(i);
      //采样点sample_point
      SamplePoint sample_point;
      //采样点的路径时间点s,t,v
      sample_point.path_time_point = path_time_point;
      //设置采样点的s,v（v为和障碍物投影在参考线上一样的速度）
      sample_point.path_time_point.set_s(s);
      sample_point.ref_v = v;
      //采样点都塞入sample_points里
      sample_points->push_back(std::move(sample_point));
    }
  }
}

//查询超车的路径时间点，输入参数是车辆配置，障碍物id，计算的结果采样点放入sample_points中,查询超车的路径时间点？
//对超车场景下自车纵向终端状态空间进行采样
//其实就是终端状态始终压着障碍物5m，然后车速与其相同
void EndConditionSampler::QueryOvertakePathTimePoints(
    const common::VehicleConfig& vehicle_config, const std::string& obstacle_id,
    std::vector<SamplePoint>* sample_points) const {
  //定义超车的路径时间点ST点数组
   //获取输入障碍物从开始阻塞参考线到结束阻塞时间段里一系列得(s,t)，对其进行二次时间采样变成时间间隔为1.0s(t_min_density)得一系列(s,t)点？
  std::vector<STPoint> overtake_path_time_points =
      ptr_path_time_graph_->GetObstacleSurroundingPoints(
          obstacle_id, FLAGS_numerical_epsilon, FLAGS_time_min_density);

//遍历障碍物的路径时间点
  for (const auto& path_time_point : overtake_path_time_points) {
    //将障碍物的速度投影到参考线上v
    double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
        obstacle_id, path_time_point.s(), path_time_point.t());
   //定义了采样点sample_point
    SamplePoint sample_point;
    //采样点里的时间路径点为障碍物的时间路径点
    sample_point.path_time_point = path_time_point;
    //设置采样点的S为障碍物的时间路径点的s坐标+5m的纵向缓冲区，终端状态是要领先障碍物5m?
    sample_point.path_time_point.set_s(path_time_point.s() +
                                       FLAGS_default_lon_buffer);
   //保持和障碍物同样的速度
    sample_point.ref_v = v;
    sample_points->push_back(std::move(sample_point));
  }
}

}  // namespace planning
}  // namespace apollo

附录

图1
横向终端条件采样

图2

图3

图4

图5