Apollo6.0代码Lattice算法详解——Part5: 生成横纵向轨迹
- 0.前置知识
- 1.涉及主要函数
- 2.函数关系
- 3.部分函数代码详解
- 3.1 lattice_planner.cc中代码部分
- 3.2 函数-GenerateTrajectoryBundles
- 3.3 函数-GenerateLongitudinalTrajectoryBundle
- 3.4 函数-SampleLonEndConditionsForCruising
- 3.5 函数-SUpper / SLower / VUpper / VLower
- 3.6 函数-GenerateTrajectory1DBundle<4>
- 3.7 函数-QuarticPolynomialCurve1d::ComputeCoefficients
- 3.8 函数-SampleLonEndConditionsForPathTimePoints
- 3.9 函数-QueryPathTimeObstacleSamplePoints
- 3.10 函数-QueryFollowPathTimePoints
- 3.11 函数-GetObstacleSurroundingPoints
- 3.12 函数-ProjectVelocityAlongReferenceLine
- 3.13 函数-QueryOvertakePathTimePoints
- 3.14 函数-GenerateSpeedProfilesForStopping
- 3.15 函数-SampleLonEndConditionsForStopping
- 3.16 函数-GenerateLateralTrajectoryBundle
- 3.17 函数-SampleLatEndConditions
- 3.18 函数-GetLateralBounds
- 3.19 函数-UpdateLateralBoundsByObstacle
- 3.20 函数-CalculateKernel
- 3.21 函数-optimize
- 3.22 函数-GetOptimalTrajectory
- 4.参考资料
0.前置知识
- reference_line和reference_line_info 的区别及联系: ReferenceLineInfo 结构中包含了ReferenceLine，还有一些其他信息，例如车辆状态、决策信息、障碍物信息、轨迹信息等等。Planning内部的数据，都是由ReferenceLineInfo 这个结构进行传递的。
- 横向轨迹规划的QP求解方法中, 有用到csc_matrix(压缩矩阵), 需要了解一下:
参考链接: https://blog.csdn.net/weixin_34945803/article/details/106576629
1.涉及主要函数
1) 函数名: GenerateTrajectoryBundles(const PlanningTarget& planning_target, Trajectory1DBundle* ptr_lon_trajectory_bundle, Trajectory1DBundle* ptr_lat_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 生成轨迹束
2) 函数名: GenerateLongitudinalTrajectoryBundle(const PlanningTarget& planning_target, Trajectory1DBundle* ptr_lon_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 生成纵向轨迹束
3) 函数名: GenerateSpeedProfilesForCruising( const double target_speed, Trajectory1DBundle* ptr_lon_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 巡航速度规划
4) 函数名: GenerateSpeedProfilesForPathTimeObstacles( Trajectory1DBundle* ptr_lon_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 考虑障碍物的巡航速度规划
5) 函数名: GenerateSpeedProfilesForStopping( const double stop_point, Trajectory1DBundle* ptr_lon_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 停车速度规划
6) 函数名: SampleLonEndConditionsForCruising( const double ref_cruise_speed)
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 巡航采样纵向停止条件
7) 函数名: VUpper(const double t) / VLower(const double t)
函数位置: modules/planning/lattice/behavior/feasible_region.cc
函数作用: 求取V的上下限值
8) 函数名: 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>* ptr_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.h
函数作用: 生成一维四次多项式轨迹束
9) 函数名: QuarticPolynomialCurve1d( const double x0, const double dx0, const double ddx0, const double dx1, const double ddx1, const double param)
函数位置: modules/planning/math/curve1d/quartic_polynomial_curve1d.cc
函数作用: 一维四次多项式类构造
10) 函数名: ComputeCoefficients( const double x0, const double dx0, const double ddx0, const double dx1, const double ddx1, const double p)
函数位置: modules/planning/math/curve1d/quartic_polynomial_curve1d.cc
函数作用: 一维四次多项式求解
11) 函数名: SampleLonEndConditionsForPathTimePoints()
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 考虑障碍物的采样结束条件
12) 函数名: QueryPathTimeObstacleSamplePoints()
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 查询采样点周围障碍物的ST图
13) 函数名: QueryFollowPathTimePoints( const common::VehicleConfig& vehicle_config, const std::string& obstacle_id, std::vector< SamplePoint >* const sample_points)
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 查询跟车采样点
14) 函数名: GetObstacleSurroundingPoints( const std::string& obstacle_id, const double s_dist, const double t_min_density)
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 获取障碍物周围点的ST
15) 函数名: ProjectVelocityAlongReferenceLine( const std::string& obstacle_id, const double s, const double t)
函数位置: modules/planning/lattice/behavior/prediction_querier.cc
函数作用: 沿参考线速度投影
16) 函数名: QueryOvertakePathTimePoints( const common::VehicleConfig& vehicle_config, const std::string& obstacle_id, std::vector* sample_points)
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 查询超车采样点
17) 函数名: GenerateSpeedProfilesForStopping( const double stop_point, Trajectory1DBundle* ptr_lon_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 停车速度规划
18) 函数名: GenerateLateralTrajectoryBundle(Trajectory1DBundle* ptr_lat_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.cc
函数作用: 生成横向轨迹束
19) 函数名: SampleLatEndConditions()
函数位置: modules/planning/lattice/trajectory_generation/end_condition_sampler.cc
函数作用: 采样横向停止条件
20) 函数名: 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>* ptr_trajectory_bundle)
函数位置: modules/planning/lattice/trajectory_generation/trajectory1d_generator.h
函数作用: 生成一维五次多项式轨迹束
21) 函数名: ComputeCoefficients( const double x0, const double dx0, const double ddx0, const double x1, const double dx1, const double ddx1, const double p)
函数位置: modules/planning/math/curve1d/quintic_polynomial_curve1d.cc
函数作用: 一维五次多项式求解
22) 函数名: GetLateralBounds( const double s_start, const double s_end, const double s_resolution)
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 获得横向边界
23) 函数名: UpdateLateralBoundsByObstacle( const SLBoundary& sl_boundary, const std::vector& discretized_path, const double s_start, const double s_end, std::vector<std::pair<double, double>>* const bounds)
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 考虑障碍物,更新横向边界
24) 函数名: optimize( const std::array<double, 3>& d_state, const double delta_s, const std::vector<std::pair<double, double>>& d_bounds)
函数位置: modules/planning/lattice/trajectory_generation/lateral_osqp_optimizer.cc
函数作用: OSQP优化
25) 函数名: CalculateKernel( const std::vector<std::pair<double, double>>& d_bounds, std::vector<c_float>* P_data, std::vector<c_int>* P_indices, std::vector<c_int>* P_indptr)
函数位置: modules/planning/lattice/trajectory_generation/lateral_osqp_optimizer.cc
函数作用: P矩阵构建
26) 函数名: GetOptimalTrajectory()
函数位置: modules/planning/lattice/trajectory_generation/lateral_qp_optimizer.cc
函数作用: 获得优化的最优轨迹
2.函数关系
3.部分函数代码详解
3.1 lattice_planner.cc中代码部分
// 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
// 分别生成1维的横纵向轨迹
// 传入：初始s，初始l，障碍物ST图，障碍物信息
Trajectory1dGenerator trajectory1d_generator(
init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
// 纵向轨迹束
std::vector<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
// 横向轨迹束
std::vector<std::shared_ptr<Curve1d>> lat_trajectory1d_bundle;
// 生成横向和纵向的轨迹束
trajectory1d_generator.GenerateTrajectoryBundles(
planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);
ADEBUG << "Trajectory_Generation_Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
3.2 函数-GenerateTrajectoryBundles
代码部分:
// 生成轨迹束
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);
}
3.3 函数-GenerateLongitudinalTrajectoryBundle
代码部分:
// 生成纵向轨迹束
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);
// 基于障碍物的纵向规划和基于巡航规划没有本质区别，主要差异在于：末点的采样→根据起点和末点计
// 算五次多项式系数，由四次多项式换为五次多项式。原因在于根据障碍物ST图，我们可以确定末点的
// s、v、a(a=0)，即明确了六个变量(起始点s、v、a和末点的s、v、a)
GenerateSpeedProfilesForPathTimeObstacles(ptr_lon_trajectory_bundle);
if (planning_target.has_stop_point()) {
GenerateSpeedProfilesForStopping(planning_target.stop_point().s(),
ptr_lon_trajectory_bundle);
}
}
3.4 函数-SampleLonEndConditionsForCruising
代码部分:
std::vector<Condition> EndConditionSampler::SampleLonEndConditionsForCruising(
const double ref_cruise_speed) const {
CHECK_GT(FLAGS_num_velocity_sample, 1);
// time interval is one second plus the last one 0.01
static constexpr size_t num_of_time_samples = 9;
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; // 8.0
}
// 0.01, 1, 2, 3, 4, 5, 6, 7, 8
time_samples[0] = FLAGS_polynomial_minimal_param; // 0.01
std::vector<Condition> end_s_conditions;
for (const auto& time : time_samples) {
// 采样v的上下限
double v_upper = std::min(feasible_region_.VUpper(time), ref_cruise_speed);
double v_lower = feasible_region_.VLower(time);
// s,s',s" 即s,v,a
State lower_end_s = {0.0, v_lower, 0.0};
end_s_conditions.emplace_back(lower_end_s, time);
State upper_end_s = {0.0, v_upper, 0.0};
end_s_conditions.emplace_back(upper_end_s, time);
double v_range = v_upper - v_lower;
// Number of sample velocities
// t=0.01时，v_range=0.04-(-0.06)=0.1,所以num_of_mid_points=min(4,static_cast<size_t>(0.1))=min(4,0)=0
// t=1时，v_range=4-(-6)=10,所以num_of_mid_points=min(4,10)=4 (1以后都为4)
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));
if (num_of_mid_points > 0) {
double velocity_seg =
v_range / static_cast<double>(num_of_mid_points + 1);
for (size_t i = 1; i <= num_of_mid_points; ++i) {
State end_s = {0.0, v_lower + velocity_seg * static_cast<double>(i),
0.0};
end_s_conditions.emplace_back(end_s, time);
}
}
}
return end_s_conditions;
}
3.5 函数-SUpper / SLower / VUpper / VLower
图解:
feasible_region_涉及的几个函数放在一张图里理解.
代码部分:
FeasibleRegion::FeasibleRegion(const std::array<double, 3>& init_s) {
init_s_ = init_s;
double v = init_s[1];
CHECK_GE(v, 0.0);
const double max_deceleration = -FLAGS_longitudinal_acceleration_lower_bound;
// 自车从当前速度下，按照设定的最大减速度，将速度减到0所需要的时间， t = v/a
t_at_zero_speed_ = v / max_deceleration;
// 自车从当前速度下，按照设定的最大减速度，将速度减到0所经过的s的长度(需加上初始s)
s_at_zero_speed_ = init_s[0] + v * v / (2.0 * max_deceleration);
}
double FeasibleRegion::SUpper(const double t) const {
ACHECK(t >= 0.0);
// s = s0 + s'*t + 1/2*a*t^2
// 表示自车初始位置在s0，当前速度下以设定的最大加速度行驶 t 秒之后，自车所在的s的位置
return init_s_[0] + init_s_[1] * t +
0.5 * FLAGS_longitudinal_acceleration_upper_bound * t * t;
}
double FeasibleRegion::SLower(const double t) const {
// 如果传入的t小于t_at_zero_speed_，说明在t时间内自车不会刹停
if (t < t_at_zero_speed_) {
// s = s0 + s'*t + 1/2*a*t^2
// 表示自车初始位置在s0的位置，在当前速度下以设定的最大减速度行驶 t 秒之后，自车所在的s的位置
return init_s_[0] + init_s_[1] * t +
0.5 * FLAGS_longitudinal_acceleration_lower_bound * t * t;
}
// 如果传入的t大于t_at_zero_speed_，说明在t时间内自车会刹停，所以直接取s_at_zero_speed_
return s_at_zero_speed_;
}
double FeasibleRegion::VUpper(const double t) const {
// v = v0 + at
// 表示自车在初始速度v0下，以设定的最大加速度行驶 t 秒之后的速度值
return init_s_[1] + FLAGS_longitudinal_acceleration_upper_bound * t;
}
double FeasibleRegion::VLower(const double t) const {
// v = v0 + at
// 如果t < t_at_zero_speed_，说明速度不会减为0，则取公式计算出的速度，否则就直接取0
return t < t_at_zero_speed_
? init_s_[1] + FLAGS_longitudinal_acceleration_lower_bound * t
: 0.0;
}
3.6 函数-GenerateTrajectory1DBundle<4>
代码部分:
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);
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 QuarticPolynomialCurve1d(
init_state, {end_condition.first[1], end_condition.first[2]}, // 起始s,s',s", {结束s',s"}
end_condition.second))); // 结束时间
ptr_trajectory1d->set_target_velocity(end_condition.first[1]);
ptr_trajectory1d->set_target_time(end_condition.second);
ptr_trajectory_bundle->push_back(ptr_trajectory1d); // 压入本条轨迹
}
}
3.7 函数-QuarticPolynomialCurve1d::ComputeCoefficients
图解:
代码部分:
void QuarticPolynomialCurve1d::ComputeCoefficients(
const double x0, const double dx0, const double ddx0, const double dx1,
const double ddx1, const double p) {
CHECK_GT(p, 0.0);
coef_[0] = x0;
coef_[1] = dx0;
coef_[2] = 0.5 * ddx0;
double b0 = dx1 - ddx0 * p - dx0;
double b1 = ddx1 - ddx0;
double p2 = p * p;
double p3 = p2 * p;
coef_[3] = (3 * b0 - b1 * p) / (3 * p2);
coef_[4] = (-2 * b0 + b1 * p) / (4 * p3);
}
3.8 函数-SampleLonEndConditionsForPathTimePoints
代码部分:
std::vector<Condition>
EndConditionSampler::SampleLonEndConditionsForPathTimePoints() const {
std::vector<Condition> end_s_conditions;
std::vector<SamplePoint> sample_points = QueryPathTimeObstacleSamplePoints();
// 对得到的采样点for循环遍历
for (const SamplePoint& sample_point : sample_points) {
if (sample_point.path_time_point.t() < FLAGS_polynomial_minimal_param) {
continue;
}
double s = sample_point.path_time_point.s();
double v = sample_point.ref_v;
double t = sample_point.path_time_point.t();
// 排除掉不合理的s
if (s > feasible_region_.SUpper(t) || s < feasible_region_.SLower(t)) {
continue;
}
State end_state = {s, v, 0.0};
// 最终放入合理的end_state和t
end_s_conditions.emplace_back(end_state, t);
}
return end_s_conditions;
}
3.9 函数-QueryPathTimeObstacleSamplePoints
代码部分:
std::vector<SamplePoint>
EndConditionSampler::QueryPathTimeObstacleSamplePoints() const {
// 获取车辆配置信息
const auto& vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
std::vector<SamplePoint> sample_points;
// for循环获取动态障碍物的ST信息，有几个障碍物就循环几次
for (const auto& path_time_obstacle :
ptr_path_time_graph_->GetPathTimeObstacles()) {
std::string obstacle_id = path_time_obstacle.id();
// 跟车情况对应的采样点：
QueryFollowPathTimePoints(vehicle_config, obstacle_id, &sample_points);
// 超车情况对应的采样点：
QueryOvertakePathTimePoints(vehicle_config, obstacle_id, &sample_points);
}
return sample_points;
}
3.10 函数-QueryFollowPathTimePoints
图解:
代码部分:
void EndConditionSampler::QueryFollowPathTimePoints(
const common::VehicleConfig& vehicle_config, const std::string& obstacle_id,
std::vector<SamplePoint>* const sample_points) const {
// 获取障碍物周围点的ST
std::vector<STPoint> follow_path_time_points =
ptr_path_time_graph_->GetObstacleSurroundingPoints(
obstacle_id, -FLAGS_numerical_epsilon, FLAGS_time_min_density); // 1e-6, 1.0
// for循环遍历ST下边界点
for (const auto& path_time_point : follow_path_time_points) {
// 沿参考线速度投影,求出障碍物的速度在参考线方向的分量
double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
obstacle_id, path_time_point.s(), path_time_point.t());
// Generate candidate s
double s_upper = path_time_point.s() -
vehicle_config.vehicle_param().front_edge_to_center();
double s_lower = s_upper - FLAGS_default_lon_buffer; // 5.0
CHECK_GE(FLAGS_num_sample_follow_per_timestamp, 2); // 3
double s_gap = // = 5.0/2.0 = 2.5
FLAGS_default_lon_buffer / // 5.0
static_cast<double>(FLAGS_num_sample_follow_per_timestamp - 1); // 3
// 三个点，从s_lower开始，包括s_lower,每隔2.5m取一个点
for (size_t i = 0; i < FLAGS_num_sample_follow_per_timestamp; ++i) {
double s = s_lower + s_gap * static_cast<double>(i);
SamplePoint sample_point;
sample_point.path_time_point = path_time_point;
sample_point.path_time_point.set_s(s);
sample_point.ref_v = v;
sample_points->push_back(std::move(sample_point));
}
}
}
3.11 函数-GetObstacleSurroundingPoints
图解:
代码部分:
std::vector<STPoint> PathTimeGraph::GetObstacleSurroundingPoints(
const std::string& obstacle_id, const double s_dist,
const double t_min_density) const {
ACHECK(t_min_density > 0.0);
std::vector<STPoint> pt_pairs;
// 找不到障碍物就返回
if (path_time_obstacle_map_.find(obstacle_id) ==
path_time_obstacle_map_.end()) {
return pt_pairs;
}
// 通过ID拿到障碍物相关信息
const auto& pt_obstacle = path_time_obstacle_map_.at(obstacle_id);
double s0 = 0.0;
double s1 = 0.0;
double t0 = 0.0;
double t1 = 0.0;
// s_dist > 0.0的时候表示的是超车
if (s_dist > 0.0) {
s0 = pt_obstacle.upper_left_point().s();
s1 = pt_obstacle.upper_right_point().s();
t0 = pt_obstacle.upper_left_point().t();
t1 = pt_obstacle.upper_right_point().t();
// else的时候代表的是跟车的情况，跟车的话是考虑ST图的下面
} else {
// 获取ST图的左下角和右下角的s
s0 = pt_obstacle.bottom_left_point().s();
s1 = pt_obstacle.bottom_right_point().s();
// 获取ST图的左下角和右下角的t
t0 = pt_obstacle.bottom_left_point().t();
t1 = pt_obstacle.bottom_right_point().t();
}
// 时间差
double time_gap = t1 - t0;
ACHECK(time_gap > -FLAGS_numerical_epsilon);
time_gap = std::fabs(time_gap);
// t_min_density默认为1.0
// num_sections表示按照设定时间间隔,在障碍物的delte_t内能取多少段. + 1是为了保证num_sections的准确度,举个例子:
// 例如 t_min_density=2, time_gap=15, 那么time_gap / t_min_density=7.5, 那么数据类型在强转size_t的时候就
// 取了整数,那其实就丢失了0.5的精度, 如果 +1 的话, 7.5+1=8.5, static_cast<size_t>(8.5)=8, 这样就可以保证多出
// 来的0.5也会被考虑进去了
size_t num_sections = static_cast<size_t>(time_gap / t_min_density + 1);
// 设定时间间隔
double t_interval = time_gap / static_cast<double>(num_sections);
// 这部分暂时没弄懂
for (size_t i = 0; i <= num_sections; ++i) {
double t = t_interval * static_cast<double>(i) + t0;
double s = lerp(s0, t0, s1, t1, t) + s_dist;
STPoint ptt;
ptt.set_t(t);
ptt.set_s(s);
pt_pairs.push_back(std::move(ptt));
}
return pt_pairs;
}
3.12 函数-ProjectVelocityAlongReferenceLine
代码部分:
double PredictionQuerier::ProjectVelocityAlongReferenceLine(
const std::string& obstacle_id, const double s, const double t) const {
ACHECK(id_obstacle_map_.find(obstacle_id) != id_obstacle_map_.end());
// 获取障碍物轨迹
const auto& trajectory = id_obstacle_map_.at(obstacle_id)->Trajectory();
// 获取轨迹点数量
int num_traj_point = trajectory.trajectory_point_size();
// 认为是静态障碍物
if (num_traj_point < 2) {
return 0.0;
}
// 如果传入的t不在障碍物轨迹点的t的范围之内，说明该障碍物不会造成影响，直接返回
if (t < trajectory.trajectory_point(0).relative_time() ||
t > trajectory.trajectory_point(num_traj_point - 1).relative_time()) {
return 0.0;
}
// 查找最靠近时间点t的轨迹点
auto matched_it =
std::lower_bound(trajectory.trajectory_point().begin(),
trajectory.trajectory_point().end(), t,
[](const common::TrajectoryPoint& p, const double t) {
return p.relative_time() < t;
});
// 获得轨迹点的v、heading
double v = matched_it->v();
double theta = matched_it->path_point().theta(); // 这个theta是障碍物轨迹的heading
double v_x = v * std::cos(theta);
double v_y = v * std::sin(theta);
// 查找该点在参考线上的对应点
common::PathPoint obstacle_point_on_ref_line =
common::math::PathMatcher::MatchToPath(*ptr_reference_line_, s);
auto ref_theta = obstacle_point_on_ref_line.theta(); // 这个theta是参考线在该s下的heading
// 叉乘，速度投影到参考线上
return std::cos(ref_theta) * v_x + std::sin(ref_theta) * v_y;
}
3.13 函数-QueryOvertakePathTimePoints
图解:
查询超车和查询跟车采样点基本逻辑一样,所以用跟车的图解解释.
代码部分:
void EndConditionSampler::QueryOvertakePathTimePoints(
const common::VehicleConfig& vehicle_config, const std::string& obstacle_id,
std::vector<SamplePoint>* sample_points) const {
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) {
double v = ptr_prediction_querier_->ProjectVelocityAlongReferenceLine(
obstacle_id, path_time_point.s(), path_time_point.t());
SamplePoint sample_point;
sample_point.path_time_point = path_time_point;
sample_point.path_time_point.set_s(path_time_point.s() +
FLAGS_default_lon_buffer); // 5.0
sample_point.ref_v = v;
sample_points->push_back(std::move(sample_point));
}
}
3.14 函数-GenerateSpeedProfilesForStopping
代码部分:
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.
GenerateTrajectory1DBundle<5>(init_lon_state_, end_conditions,
ptr_lon_trajectory_bundle);
}
3.15 函数-SampleLonEndConditionsForStopping
代码部分:
std::vector<Condition> EndConditionSampler::SampleLonEndConditionsForStopping(
const double ref_stop_point) const {
// time interval is one second plus the last one 0.01
static constexpr size_t num_of_time_samples = 9;
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; // 8.0
}
// 0.01, 1, 2, 3, 4, 5, 6, 7, 8
time_samples[0] = FLAGS_polynomial_minimal_param;
std::vector<Condition> end_s_conditions;
for (const auto& time : time_samples) {
// 取车辆当前s和停止点的s中最大的
State end_s = {std::max(init_s_[0], ref_stop_point), 0.0, 0.0};
end_s_conditions.emplace_back(end_s, time);
}
return end_s_conditions;
}
3.16 函数-GenerateLateralTrajectoryBundle
代码部分:
void Trajectory1dGenerator::GenerateLateralTrajectoryBundle( // 横向轨迹规划通过S-L的关系来进行障碍物的规避
Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
// 常规撒点采样方法
if (!FLAGS_lateral_optimization) { // true
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 {
// 大致这么个意思：在一段长60m的里程中，以ds=1m进行采样，遍历每个采样点的横向可达范围，
// 即每个点的横向约束，通过这么多约束构建对应的二次型，最后通过调用OSQP进行二次规划求解
double s_min = init_lon_state_[0];
double s_max = s_min + FLAGS_max_s_lateral_optimization; // 60.0
double delta_s = FLAGS_default_delta_s_lateral_optimization; // 1.0
auto lateral_bounds =
ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);
// LateralTrajectoryOptimizer lateral_optimizer;
std::unique_ptr<LateralQPOptimizer> 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<PiecewiseJerkTrajectory1d>(lateral_trajectory));
}
}
3.17 函数-SampleLatEndConditions
图解:
图是直接拿的别人的, 画得非常清楚了, 参考链接: https://blog.csdn.net/weixin_34945803/article/details/107592706?spm=1001.2014.3001.5502
代码部分:
std::vector<Condition> EndConditionSampler::SampleLatEndConditions() const {
std::vector<Condition> end_d_conditions;
// 为什么±0.5？apollo中参考线意味着道路中心线，即车辆的规划是基于当前车道进行的，所以车
// 辆横向偏移也应保持车辆在车道内
std::array<double, 3> end_d_candidates = {0.0, -0.5, 0.5};
std::array<double, 4> end_s_candidates = {10.0, 20.0, 40.0, 80.0};
for (const auto& s : end_s_candidates) {
for (const auto& d : end_d_candidates) {
State end_d_state = {d, 0.0, 0.0};
end_d_conditions.emplace_back(end_d_state, s);
}
}
return end_d_conditions;
}
3.18 函数-GetLateralBounds
代码部分:
std::vector<std::pair<double, double>> PathTimeGraph::GetLateralBounds(
const double s_start, const double s_end, const double s_resolution) {
CHECK_LT(s_start, s_end);
CHECK_GT(s_resolution, FLAGS_numerical_epsilon);
std::vector<std::pair<double, double>> bounds;
std::vector<double> discretized_path;
double s_range = s_end - s_start;
double s_curr = s_start;
// s_range范围内有多少个点，1m一个，应该每次都是60
size_t num_bound = static_cast<size_t>(s_range / s_resolution);
const auto& vehicle_config =
common::VehicleConfigHelper::Instance()->GetConfig();
// 自车宽
double ego_width = vehicle_config.vehicle_param().width();
// Initialize bounds by reference line width
// 对采样点num_bound进行for循环遍历，这一遍找到的是不考虑障碍物的边界!!!
for (size_t i = 0; i < num_bound; ++i) {
double left_width = FLAGS_default_reference_line_width / 2.0;
double right_width = FLAGS_default_reference_line_width / 2.0;
// 查询参考线的车道宽
ptr_reference_line_info_->reference_line().GetLaneWidth(s_curr, &left_width,
&right_width);
// 获得自车l的左右边界
double ego_d_lower = init_d_[0] - ego_width / 2.0;
double ego_d_upper = init_d_[0] + ego_width / 2.0;
// 将每个点对应的横向最大值最小值存入bound
bounds.emplace_back(
std::min(-right_width, ego_d_lower - FLAGS_bound_buffer), // 0.1
std::max(left_width, ego_d_upper + FLAGS_bound_buffer));
// 将每个采样点的s存入discretized_path
discretized_path.push_back(s_curr);
s_curr += s_resolution;
}
// 根据障碍物更新bound
for (const SLBoundary& static_sl_boundary : static_obs_sl_boundaries_) {
UpdateLateralBoundsByObstacle(static_sl_boundary, discretized_path, s_start,
s_end, &bounds);
}
// 再考虑进去车宽，更新bound
for (size_t i = 0; i < bounds.size(); ++i) {
bounds[i].first += ego_width / 2.0;
bounds[i].second -= ego_width / 2.0;
// 对处理过后的异常值
if (bounds[i].first >= bounds[i].second) {
bounds[i].first = 0.0;
bounds[i].second = 0.0;
}
}
return bounds;
}
3.19 函数-UpdateLateralBoundsByObstacle
图解:
代码部分:
void PathTimeGraph::UpdateLateralBoundsByObstacle(
const SLBoundary& sl_boundary, const std::vector<double>& discretized_path,
const double s_start, const double s_end,
std::vector<std::pair<double, double>>* const bounds) {
// 排除掉不在这60m范围之内的障碍物
if (sl_boundary.start_s() > s_end || sl_boundary.end_s() < s_start) {
return;
}
// 找到障碍物起始s对应的60m中的点
auto start_iter = std::lower_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
// 找到障碍物结束s对应的60m中的点, 这里应该是sl_boundary.end_s()
auto end_iter = std::upper_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
size_t start_index = start_iter - discretized_path.begin();
size_t end_index = end_iter - discretized_path.begin();
// 相当于判断 start<0 和 end>0,相当于障碍物的左右边界横跨 s=0 的线(即参考线)
if (sl_boundary.end_l() > -FLAGS_numerical_epsilon && // 1e-6
sl_boundary.start_l() < FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < end_index; ++i) {
// 左右边界都是0
bounds->operator[](i).first = -FLAGS_numerical_epsilon;
bounds->operator[](i).second = FLAGS_numerical_epsilon;
}
return;
}
// 障碍物在参考线右边
if (sl_boundary.end_l() < FLAGS_numerical_epsilon) {
// bounds->size()即为60，之前做无障碍物边界的时候已经插入了
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
// 只更新边界的start_l,取原边界的start_l 和 障碍物边界的end_l中的最大值
bounds->operator[](i).first =
std::max(bounds->operator[](i).first,
sl_boundary.end_l() + FLAGS_nudge_buffer);
}
return;
}
// 障碍物在参考线左边
if (sl_boundary.start_l() > -FLAGS_numerical_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
// 只更新边界的end_l,取原边界的end_l 和 障碍物边界的start_l中的最小值
bounds->operator[](i).second =
std::min(bounds->operator[](i).second,
sl_boundary.start_l() - FLAGS_nudge_buffer);
}
return;
}
}
3.20 函数-CalculateKernel
图解:
代码部分:
void LateralOSQPOptimizer::CalculateKernel(
const std::vector<std::pair<double, double>>& d_bounds,
std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
const int kNumParam = 3 * static_cast<int>(d_bounds.size()); // 180
P_data->resize(kNumParam);
P_indices->resize(kNumParam);
P_indptr->resize(kNumParam + 1);
for (int i = 0; i < kNumParam; ++i) {
// data:原始矩阵所有非0元素的值都在里面
// 因为构建的P是一个对角阵,所以只塞入值就行
if (i < static_cast<int>(d_bounds.size())) { // 0-59
P_data->at(i) = 2.0 * FLAGS_weight_lateral_offset + // 1.0
2.0 * FLAGS_weight_lateral_obstacle_distance; // 0.0
} else if (i < 2 * static_cast<int>(d_bounds.size())) { // 60-119
P_data->at(i) = 2.0 * FLAGS_weight_lateral_derivative; // 500
} else { // 120-179
P_data->at(i) = 2.0 * FLAGS_weight_lateral_second_order_derivative; // 1000
}
// indices:是一个行索引数组,表示原始矩阵中第i列的非0元素所在的行索引
// 因为构建的P是一个对角阵,所以P_indices里插入的其实就是当前元素的行数
P_indices->at(i) = i;.
// indptr:表示稀疏矩阵的第i列(包括i列)之前一共有多少非0元素;
// 因为构建的P是一个对角阵，所以每一列的非0元素个数就只有一个，所以这里的P_indptr插入的就是i的值
P_indptr->at(i) = i;
} // 整体看下来，采用这种vector嵌套vector的方法来表示矩阵，在其中塞入对角阵的值有个好处，就是可以缩小构造的columns的大小，以及在
// 后面将矩阵里的内容压缩的时候，减小for循环访问矩阵内容的次数，节约时间
P_indptr->at(kNumParam) = kNumParam;
CHECK_EQ(P_data->size(), P_indices->size());
}
3.21 函数-optimize
图解:
这部分涉及图片比较多,需要花点时间仔细理解下.
代码部分:
bool LateralOSQPOptimizer::optimize(
const std::array<double, 3>& d_state, const double delta_s,
const std::vector<std::pair<double, double>>& d_bounds) {
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
delta_s_ = delta_s;
const int num_var = static_cast<int>(d_bounds.size()); // 60 60个点
const int kNumParam = 3 * static_cast<int>(d_bounds.size()); // 3*60=180 60个点，每个点3个变量，s,v,a
const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3; // 180+3*(60-1)+3 = 360
// 边界约束
c_float lower_bounds[kNumConstraint]; // lower_bounds[360]
c_float upper_bounds[kNumConstraint]; // upper_bounds[360]
const int prime_offset = num_var; // 60
const int pprime_offset = 2 * num_var; // 120
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(kNumParam); // 180
int constraint_index = 0;
// d_i+1'' - d_i'' 位置约束
// columns [120,0] -> [158,58] 都为-1；[121,0] -> [159,58] 都为1
// constraint_index在下面的循环里为 0->58
for (int i = 0; i + 1 < num_var; ++i) { // 0-58,共59个
// pprime_offset + i : 120->178
columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
// pprime_offset + i + 1 : 121->179
columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds[constraint_index] =
-FLAGS_lateral_third_order_derivative_max * delta_s_; // -0.1*1.0
upper_bounds[constraint_index] =
FLAGS_lateral_third_order_derivative_max * delta_s_; // 0.1*1.0
++constraint_index;
}
// constraint_index到这里为 59
// d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'') 导数约束(光滑)
// constraint_index在下面的循环里为 59->117
for (int i = 0; i + 1 < num_var; ++i) { // 0-58,共59个
// prime_offset + i : 60->118
columns[prime_offset + i].emplace_back(constraint_index, -1.0);
// prime_offset + i + 1 : 61->119
columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
// pprime_offset + i : 120->178
columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
// pprime_offset + i + 1 : 121->179
columns[pprime_offset + i + 1].emplace_back(constraint_index,
-0.5 * delta_s_);
lower_bounds[constraint_index] = 0.0;
upper_bounds[constraint_index] = 0.0;
++constraint_index;
}
// constraint_index到这里为 118
// d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2 连续性约束(连续)
// constraint_index在下面的循环里为 118->176
for (int i = 0; i + 1 < num_var; ++i) {
// i : 0->58
columns[i].emplace_back(constraint_index, -1.0);
// i + 1 : 1->59
columns[i + 1].emplace_back(constraint_index, 1.0);
// prime_offset + i : 60->118
columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
// pprime_offset + i : 120->178 0307
columns[pprime_offset + i].emplace_back(constraint_index,
-delta_s_ * delta_s_ / 3.0);
// pprime_offset + i + 1 : 121->179
columns[pprime_offset + i + 1].emplace_back(constraint_index,
-delta_s_ * delta_s_ / 6.0);
lower_bounds[constraint_index] = 0.0;
upper_bounds[constraint_index] = 0.0;
++constraint_index;
}
// constraint_index到这里为 177
// columns[0][177] = 1.0
columns[0].emplace_back(constraint_index, 1.0);
// lower_bounds[177] = l_start
lower_bounds[constraint_index] = d_state[0];
// upper_bounds[177] = l_start
upper_bounds[constraint_index] = d_state[0];
++constraint_index;
// constraint_index到这里为 178
// columns[60][178] = 1.0
columns[prime_offset].emplace_back(constraint_index, 1.0);
// lower_bounds[178] = l_start'
lower_bounds[constraint_index] = d_state[1];
// upper_bounds[178] = l_start'
upper_bounds[constraint_index] = d_state[1];
++constraint_index;
// constraint_index到这里为 179
// columns[120][179] = 1.0
columns[pprime_offset].emplace_back(constraint_index, 1.0);
// lower_bounds[179] = l_start"
lower_bounds[constraint_index] = d_state[2];
// upper_bounds[179] = l_start"
upper_bounds[constraint_index] = d_state[2];
++constraint_index;
// constraint_index = 180
const double LARGE_VALUE = 2.0;
for (int i = 0; i < kNumParam; ++i) { // i < 180
// i : 0->179 constraint_index : 180->359
columns[i].emplace_back(constraint_index, 1.0);
// 如果 i<60, 就用之前获得的60m范围内的lateral_bounds的上下边界值
if (i < num_var) {
// lower_bounds[180]->lower_bounds[239] = 当前点的右边界(负值)
lower_bounds[constraint_index] = d_bounds[i].first;
// upper_bounds[180]->upper_bounds[239] = 当前点的左边界(正值)
upper_bounds[constraint_index] = d_bounds[i].second;
} else { // 超出60就用2填充边界，也就是上下边界都是2m
lower_bounds[constraint_index] = -LARGE_VALUE;
upper_bounds[constraint_index] = LARGE_VALUE;
}
++constraint_index;
}
CHECK_EQ(constraint_index, kNumConstraint);
// change affine_constraint to CSC format
std::vector<c_float> A_data; // 原始矩阵所有 非0 元素值都在里面
std::vector<c_int> A_indices; // 原始矩阵当前列中 非0元素 所在的 行索引值
std::vector<c_int> A_indptr; // indptr[i+1]表示稀疏矩阵的第i列(包括i列)之前一共有多少个非零元素
int ind_p = 0;
for (int j = 0; j < kNumParam; ++j) {
A_indptr.push_back(ind_p); // 值为0->180
for (const auto& row_data_pair : columns[j]) {
// 这个地方他的操作相当于是把矩阵中 非0 元素的值给放到A_data里,但是为什么这样不用筛选就
// 可以把 非0 元素直接塞进去?
// 因为这一套代码中, columns并不是一个真实的矩阵,他其实本质是一个vector嵌套一个vector,
// 矩阵中对应的 0 元素实际并没有被push_back进去,所以这个地方直接遍历vector里的元素就可
// 以把 非0 元素赛到A_data里了.
A_data.push_back(row_data_pair.second);
A_indices.push_back(row_data_pair.first);
++ind_p;
}
}
A_indptr.push_back(ind_p);
// offset
// 偏差项:用以控制轨迹和参考线的偏离程度
double q[kNumParam]; // 180
for (int i = 0; i < kNumParam; ++i) { // 180
if (i < num_var) {
q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
(d_bounds[i].first + d_bounds[i].second);
} else {
q[i] = 0.0;
}
}
// Problem settings
OSQPSettings* settings =
reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
// Define Solver settings as default
osqp_set_default_settings(settings);
/* relaxation parameter 松弛参数 */
settings->alpha = 1.0; // Change alpha parameter
/* absolute convergence tolerance 绝对收敛偏差 */
settings->eps_abs = 1.0e-05;
/* relative convergence tolerance 相对收敛偏差 */
settings->eps_rel = 1.0e-05;
/* maximum iterations to take 最大迭代次数 */
settings->max_iter = 5000;
/* 由于ADMM算法求解中精度问题收敛很快，因此我们先用ADMM算法得到中精度的结果，再对这
个结果进一步打磨（Polishing），得到更高精度的结果.
ADMM solution polish: 1 打磨精度*/
settings->polish = true;
/* print output */
settings->verbose = FLAGS_enable_osqp_debug;
// Populate data
OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
// number of variables n
data->n = kNumParam; // 180
// number of constraints m
data->m = kNumConstraint; // 360
// the upper triangular part of the quadratic cost matrix P in csc format (size n x n).
data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
P_indices.data(), P_indptr.data());
// dense array for linear part of cost function (size n)
data->q = q;
// linear constraints matrix A in csc format (size m x n)
data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
A_indices.data(), A_indptr.data());
// dense array for lower bound (size m)
data->l = lower_bounds;
// dense array for upper bound (size m)
data->u = upper_bounds;
// Workspace
OSQPWorkspace* work = nullptr;
// osqp_setup(&work, data, settings);
work = osqp_setup(data, settings);
// Solve Problem
osqp_solve(work);
// extract primal results
for (int i = 0; i < num_var; ++i) {
opt_d_.push_back(work->solution->x[i]);
opt_d_prime_.push_back(work->solution->x[i + num_var]);
opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
}
opt_d_prime_[num_var - 1] = 0.0;
opt_d_pprime_[num_var - 1] = 0.0;
// Cleanup
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return true;
}
3.22 函数-GetOptimalTrajectory
代码部分:
PiecewiseJerkTrajectory1d LateralQPOptimizer::GetOptimalTrajectory() const {
ACHECK(!opt_d_.empty() && !opt_d_prime_.empty() && !opt_d_pprime_.empty());
// 构造optimal_trajectory,传入横向轨迹起点的 l, l', l"
PiecewiseJerkTrajectory1d optimal_trajectory(
opt_d_.front(), opt_d_prime_.front(), opt_d_pprime_.front());
for (size_t i = 1; i < opt_d_.size(); ++i) { // opt_d_.size()=60
// j = delta_l" / delta_s
double j = (opt_d_pprime_[i] - opt_d_pprime_[i - 1]) / delta_s_;
optimal_trajectory.AppendSegment(j, delta_s_);
}
return optimal_trajectory;
}
4.参考资料
1.参考视频：Lattice.
2.参考资料：
Lattice Planner从学习到放弃（一）.额不…到实践.
基于OSQP的二次规划.
Piecewise Jerk Path Optimizer.
csc_matrix稀疏矩阵理解.
OSQP使用说明.
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
