Apollo Planning决策规划系列文章
Apollo Planning决策规划代码详细解析 (1)：Scenario选择
Apollo Planning决策规划代码详细解析 (2)：Scenario执行
Apollo Planning决策规划代码详细解析 (3)：stage执行
Apollo Planning决策规划代码详细解析 (4)：Stage逻辑详解
Apollo Planning决策规划代码详细解析 (5)：规划算法流程介绍
Apollo Planning决策规划代码详细解析 (6):LaneChangeDecider
Apollo Planning决策规划代码详细解析 (7): PathReuseDecider
Apollo Planning决策规划代码详细解析 (8): PathLaneBorrowDecider
Apollo Planning决策规划代码详细解析 (9): PathBoundsDecider
算法详细介绍系列文章：
Apollo决策规划算法Planning : 路径规划 Piecewise Jerk Path Optimizer的python实现
码字不易，辛苦各位看官点赞加关注！
正文如下：
一、概述
PiecewiseJerkPathOptimizer 是lanefollow 场景下，所调用的第 5 个 task，属于task中的optimizer类别它的作用主要是：
1、根据之前decider决策的reference line和 path bound，以及横向约束，将最优路径求解问题转化为二次型规划问题；
2、调用osqp库求解最优路径；
二、PiecewiseJerkPathOptimizer的具体逻辑如下：
1、PublicRoadPlanner 的 LaneFollowStage 配置了以下几个task 来实现具体的规划逻辑，PiecewiseJerkPathOptimizer 是第五个task，PiecewiseJerkPathOptimizer根据之前decider决策的reference line和 path bound，以及横向约束，将最优路径求解问题转化为二次型规划问题，之后调用osqp库求解最优路径。
scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: ST_BOUNDS_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
# task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
task_type: RSS_DECIDER
}PiecewiseJerkPathOptimizer被调用流程与其他task类似：
2、路径规划流程如下，其中Path Optimizer即对应本文介绍的PiecewiseJerkPathOptimizer：
输入信息：const SpeedData& speed_data, const ReferenceLine& reference_line, const common::TrajectoryPoint& init_point, const bool path_reusable, PathData* const final_path_data ；
其中包括参考线，起始点、速度信息、路径是否重规划等
输出信息：OptimizePath函数得到最优的路径，信息包括opt_l, opt_dl, opt_ddl。在Process函数中最终结果保存到了task基类的变量reference_line_info_中。
3、PiecewiseJerkPathOptimizer 的代码流程及框架如下，接下来小结会讲解具体代码：
上文讲过每条reference line会生成fullback path bound以及另外一条规划算法正常运行时的path bound，在这里会分别为这两个path bound 生成最优轨迹。
4、判断是否要重规划，与其他task逻辑一样：
common::Status PiecewiseJerkPathOptimizer::Process(
const SpeedData& speed_data, const ReferenceLine& reference_line,
const common::TrajectoryPoint& init_point, const bool path_reusable,
PathData* const final_path_data) {
// 跳过piecewise_jerk_path_optimizer 如果路径重复使用
if (FLAGS_enable_skip_path_tasks && path_reusable) {
return Status::OK();
}
... ...5、将起始点转化为frenet坐标，并根据是否处于变道场景选择不同的配置，这两个操作都是调用
ReferenceLine的方法。
... ...
const auto init_frenet_state =
reference_line.ToFrenetFrame(planning_start_point);
// 为lane-change选择lane_change_path_config
// 否则, 选择default_path_config
const auto& config = reference_line_info_->IsChangeLanePath()
? config_.piecewise_jerk_path_optimizer_config()
.lane_change_path_config()
: config_.piecewise_jerk_path_optimizer_config()
.default_path_config();
... ...根据选择的配置生成权重矩阵：
std::array<double, 5> w = {
config.l_weight(),
config.dl_weight() *
std::fmax(init_frenet_state.first[1] * init_frenet_state.first[1],
5.0),
config.ddl_weight(), config.dddl_weight(), 0.0};可以看到这两种配置，主要是二次优化问题求解的cost 权重不同。
default_task_config: {
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
piecewise_jerk_path_optimizer_config {
default_path_config {
l_weight: 1.0
dl_weight: 20.0
ddl_weight: 1000.0
dddl_weight: 50000.0
}
lane_change_path_config {
l_weight: 1.0
dl_weight: 5.0
ddl_weight: 800.0
dddl_weight: 30000.0
}
}
}6、 遍历每个path bound并进行path 优化，最后输出结果。
其中要注意path 优化时的ref_path, 以及end state, 会根据不同的场景赋不同的值。
代码以及详细注释如下：
// 获取path_boundaries_decider 生成的path bounds
const auto& path_boundaries =
reference_line_info_->GetCandidatePathBoundaries();
ADEBUG << "There are " << path_boundaries.size() << " path boundaries.";
// 获取上一周期规划的path,做为此周期的参考path
const auto& reference_path_data = reference_line_info_->path_data();
std::vector<PathData> candidate_path_data;
// 遍历每条path bounds
for (const auto& path_boundary : path_boundaries) {
size_t path_boundary_size = path_boundary.boundary().size();
// if the path_boundary is normal, it is possible to have less than 2 points
// skip path boundary of this kind
// 如果该path 点太少则跳过这个path bound
if (path_boundary.label().find("regular") != std::string::npos &&
path_boundary_size < 2) {
continue;
}
int max_iter = 4000;
// lower max_iter for regular/self/
if (path_boundary.label().find("self") != std::string::npos) {
max_iter = 4000;
}
CHECK_GT(path_boundary_size, 1U);
// 建立每个采样点的3个变量l、dl、ddl的 vector
std::vector<double> opt_l;
std::vector<double> opt_dl;
std::vector<double> opt_ddl;
// 路径终点的状态l、dl、ddl = {0.0, 0.0, 0.0}
std::array<double, 3> end_state = {0.0, 0.0, 0.0};
// 判断是否处于pull over scenario，如果处于pull over scenario，调整end_state
if (!FLAGS_enable_force_pull_over_open_space_parking_test) {
// pull over scenario
// set end lateral to be at the desired pull over destination
const auto& pull_over_status =
injector_->planning_context()->planning_status().pull_over();
if (pull_over_status.has_position() &&
pull_over_status.position().has_x() &&
pull_over_status.position().has_y() &&
path_boundary.label().find("pullover") != std::string::npos) {
common::SLPoint pull_over_sl;
reference_line.XYToSL(pull_over_status.position(), &pull_over_sl);
end_state[0] = pull_over_sl.l();
}
}
// TODO(all): double-check this;
// final_path_data might carry info from upper stream
// 读取上一周期的 path_data
PathData path_data = *final_path_data;
// updated cost function for path reference
// 将path_reference_l全部置初始化为0，
// 之后如果是regular场景，会将path_reference_l更新为上一周期path
std::vector<double> path_reference_l(path_boundary_size, 0.0);
bool is_valid_path_reference = false;
size_t path_reference_size = reference_path_data.path_reference().size();
if (path_boundary.label().find("regular") != std::string::npos &&
reference_path_data.is_valid_path_reference()) {
ADEBUG << "path label is: " << path_boundary.label();
// when path reference is ready
for (size_t i = 0; i < path_reference_size; ++i) {
common::SLPoint path_reference_sl;
// 如果是regular场景，会将path_reference_l更新为上一周期path
reference_line.XYToSL(
common::util::PointFactory::ToPointENU(
reference_path_data.path_reference().at(i).x(),
reference_path_data.path_reference().at(i).y()),
&path_reference_sl);
path_reference_l[i] = path_reference_sl.l();
}
// 更新end_state
end_state[0] = path_reference_l.back();
path_data.set_is_optimized_towards_trajectory_reference(true);
is_valid_path_reference = true;
}
// 计算横向运动学的约束
const auto& veh_param =
common::VehicleConfigHelper::GetConfig().vehicle_param();
const double lat_acc_bound =
std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) /
veh_param.wheel_base();
std::vector<std::pair<double, double>> ddl_bounds;
for (size_t i = 0; i < path_boundary_size; ++i) {
double s = static_cast<double>(i) * path_boundary.delta_s() +
path_boundary.start_s();
double kappa = reference_line.GetNearestReferencePoint(s).kappa();
ddl_bounds.emplace_back(-lat_acc_bound - kappa, lat_acc_bound - kappa);
}
// 优化算法
bool res_opt = OptimizePath(
init_frenet_state, end_state, std::move(path_reference_l),
path_reference_size, path_boundary.delta_s(), is_valid_path_reference,
path_boundary.boundary(), ddl_bounds, w, max_iter, &opt_l, &opt_dl,
if (res_opt) {
for (size_t i = 0; i < path_boundary_size; i += 4) {
ADEBUG << "for s[" << static_cast<double>(i) * path_boundary.delta_s()
<< "], l = " << opt_l[i] << ", dl = " << opt_dl[i];
}
auto frenet_frame_path =
ToPiecewiseJerkPath(opt_l, opt_dl, opt_ddl, path_boundary.delta_s(),
path_boundary.start_s());
path_data.SetReferenceLine(&reference_line);
path_data.SetFrenetPath(std::move(frenet_frame_path));
if (FLAGS_use_front_axe_center_in_path_planning) {
auto discretized_path = DiscretizedPath(
ConvertPathPointRefFromFrontAxeToRearAxe(path_data));
path_data.SetDiscretizedPath(discretized_path);
}
path_data.set_path_label(path_boundary.label());
path_data.set_blocking_obstacle_id(path_boundary.blocking_obstacle_id());
candidate_path_data.push_back(std::move(path_data));
}
}
if (candidate_path_data.empty()) {
return Status(ErrorCode::PLANNING_ERROR,
"Path Optimizer failed to generate path");
}
reference_line_info_->SetCandidatePathData(std::move(candidate_path_data));
return Status::OK();6、OptimizePath( ) 是优化算法的实现，将最优路径求解问题转化为二次型规划问题，之后调用osqp库求解最优路径。
OptimizePath( )的实现原理可以参考我的另一篇博客：
Apollo决策规划算法Planning : 路径规划 Piecewise Jerk Path Optimizer的python实现_自动驾驶 Player的博客-CSDN博客