百度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 ;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 ;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
