Apollo星火计划学习笔记——Apollo路径规划算法原理与实践
文章目录
- 1. 路径规划算法总体介绍
-
- 1.1 Task： LANE_CHANGE_DECIDER
- 1.2 Task： PATH_REUSE_DECIDER
- 1.3 Task： PATH_BORROW_DECIDER
- 1.4 Task： PATH_BOUNDS_DECIDER
- 1.5 Task： PIECEWISE_JERK_PATH_OPTIMIZER
- 1.6 Task： PATH_ASSESSMENT_DECIDER
- 1.7 Task： PATH_DECIDER
- 2. 基于二次规划的路径规划算法
-
- 2.1 二次规划问题标准型
- 2.2 定义优化变量
- 2.3 设计目标函数
- 2.4 设计约束
- 2.5 求解器求解
-
- 2.5.1 设定OSQP求解参数
- 2.5.2 计算QP系数矩阵
- 2.5.3 构造OSQP求解器
- 2.5.4 获取优化结果
- 3. 路径规划算法代码解读
-
- 3.1 总流程和决策\优化的入口函数
- 3.2 产生换道决策
- 3.3 产生借道决策
- 3.4 产生路径边界
- 3.5 路径优化
- 3.6 选择最优路径
- 4. 路径规划算法实践
-
- 4.1 观察借道绕行、自车道绕行障碍物、自车道巡航、换道时路径的边界和规划的路径
- 4.2 借道绕行场景，调整 l , l ’ , l ’’ l,l’,l’’ l,l’,l’’的权重系数，观察路径的变化
- 4.3 靠边停车实践，开启靠边停车功能，观察路径的变化
1. 路径规划算法总体介绍
Apollo中对路径规划解耦，分为路径规划与速度规划两部分。并将规划分为决策与优化两个部分。
• 路径规划 —— 静态环境（道路，静止/低速障碍物）
• 速度规划 —— 动态环境（中/高速障碍物）
路径规划的配置文件在lane_follow_config.pb.txt中
// /home/yuan/apollo-edu/modules/planning/conf/scenario/lane_follow_config.pb.txt
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: 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
_DECIDER结尾的为决策部分 _OPTIMIZER结尾的为优化部分。
1.1 Task： LANE_CHANGE_DECIDER
产生是否换道的决策，更新换道状态
首先判断是否产生多条参考线，若只有一条参考线，则保持直行。若有多条参考线，则根据一些条件（主车的前方和后方一定距离内是否有障碍物，旁边车道在一定距离内是否有障碍物）进行判断是否换道，当所有条件都满足时，则进行换道决策。
1.2 Task： PATH_REUSE_DECIDER
路径是否可重用,提高帧间平顺性
主要判断是否可以重用上一帧规划的路径。若上一帧的路径未与障碍物发生碰撞，则可以重用，提高稳定性，节省计算量。若上一帧的规划出的路径发生碰撞，则重新规划路径。
1.3 Task： PATH_BORROW_DECIDER
产生是否借道的决策
该决策有以下的判断条件：
• 是否只有一条车道
• 是否存在阻塞道路的障碍物
• 阻塞障碍物是否远离路口
• 阻塞障碍物长期存在
• 旁边车道是实线还是虚线
当所有判断条件都满足时，会产生借道决策。
1.4 Task： PATH_BOUNDS_DECIDER
产生路径边界
利用前几个决策器，根据相应条件，产生相应的SL边界。这里说明以下Nudge障碍物的概念——主车旁边的障碍物未完全阻挡主车，主车可以通过绕行避过障碍物（注意图中的边界）。
1.5 Task： PIECEWISE_JERK_PATH_OPTIMIZER
基于二次规划算法，对每个边界规划出最优路径.
1.6 Task： PATH_ASSESSMENT_DECIDER
路径评价，选出最优路径
依据以下规则，进行评价。
路径是否和障碍物碰撞
路径长度
路径是否会停在对向车道
路径离自车远近
哪个路径更早回自车道
…
路径两两进行对比，选出最优的路径。
1.7 Task： PATH_DECIDER
根据选出的路径给出对障碍物的决策
若是绕行的路径，则产生绕行的决策；若前方有障碍物阻塞，则产生停止的决策。
2. 基于二次规划的路径规划算法
2.1 二次规划问题标准型
牛津大学推出过二次规划的求解器，支持C/C++、python、Matlab等多种语言。
二次规划问题的标准形式为： m i n i m i z e 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u \begin{array}{lllllllllllllll}{{\rm{minimize}}}&{\frac{1}{2}{x^T}Px + {q^T}x}\\{{\rm{subject to}}}&{l \le Ax \le u}\end{array} minimizesubjectto21xTPx+qTxl≤Ax≤u where x ∈ R n x \in {{\bf{R}}^n} x∈Rn is the optimization variable. The objective function is defined by a positive semidefinite matrix P ∈ S + n P \in {\bf{S}}_ + ^n P∈S+nand vector q ∈ R n q \in {{\bf{R}}^n} q∈Rn . The linear constraints are defined by matrix A ∈ R m × n A \in {{\bf{R}}^{m \times n}} A∈Rm×n and vectors l l l and u u u so that l i ∈ R ∪ { − ∞ } {l_i} \in {\bf{R}} \cup \{ - \infty \} li∈R∪{−∞} and ‘ u i ∈ R ∪ { + ∞ } {`u_i} \in {\bf{R}} \cup \{ + \infty \} ‘ui∈R∪{+∞} for all i ∈ { 1 , … , m } i \in \{ 1, \ldots ,m{\rm{\} }} i∈{1,…,m} .
二次规划优化问题为二次型，其约束为线性型。 x x x是要优化的变量，是一个 n n n维的向量。 p p p是二次项系数，是正定矩阵。 q q q是一次项系数，是 n n n维向量。 A A A是一个 m m mx n n n的矩阵， A A A为约束函数的一次项系数， m m m为约束函数的个数。 l l l和 u u u分别为约束函数的下边界和上边界。
常用二次规划求解器：
- OSQP :使用ADMM方法求解。 对于规模大的，含有大量等式或不等式约束的问题有较好的求解效率。
- qpOASES: 用可行域法，对于约束较少的小规模问题，qpOASES求解更快。
二次规划问题的求解往往有以下几个步骤：定义目优化变量、设计目标函数、设计约束、求解器求解等几个步骤。
2.2 定义优化变量
路径规划一般是在Frenet坐标系中进行的。 s s s为沿着参考线的方向， l l l为垂直于坐标系的方向。
如图所示，将障碍物分别投影到SL坐标系上。在 s s s方向上，以间隔 Δ s \Delta s Δs作为一个间隔点，从 s 0 s_0 s0, s 1 s_1 s1, s 2 s_2 s2一直到 s n − 1 s_{n-1} sn−1，构成了规划的路径。选取每个间隔点的 l l l作为优化的变量，同时也将 l ˙ \dot l l˙和 l ¨ \ddot l l¨也作为优化变量。
如此，就构成了优化变量 x x x， x x x有三个部分组成：从 l 0 l_0 l0, l 1 l_1 l1, l 2 l_2 l2到 l n − 1 l_{n-1} ln−1，从 l ˙ 0 \dot l_0 l˙0, l ˙ 1 \dot l_1 l˙1, l ˙ 2 \dot l_2 l˙2到 l ˙ n − 1 \dot l_{n-1} l˙n−1,从 l ¨ 0 \ddot l_0 l¨0, l ¨ 1 \ddot l_1 l¨1, l ¨ 2 \ddot l_2 l¨2到 l ¨ n − 1 \ddot l_{n-1} l¨n−1.
2.3 设计目标函数
对于目标函数的设计，我们需要明确以下目标：
- 确保安全、礼貌的驾驶，尽可能贴近车道中心线行驶： ∣ l i ∣ ↓ \left| {{l_i}} \right| \downarrow ∣li∣↓
- 确保舒适的体感，尽可能降低横向速度/加速度/加加速度： ∣ l ˙ i ∣ ↓ \left| {{{\dot l}_i}} \right| \downarrow l˙i ↓， ∣ l ¨ i ∣ ↓ \left| {{{\ddot l}_i}} \right| \downarrow l¨i ↓， ∣ l ′ ′ ′ i → i + 1 ∣ ↓ \left| {{{l'''}_{i \to i + 1}}} \right| \downarrow ∣l′′′i→i+1∣↓
- 确保终点接近参考终点（这个往往用在靠边停车场景之中国）： l e n d = l r e f {l_{end}} = {l_{ref}} lend=lref
最后会得到以下目标函数：
对每个点设计二次的目标函数并对代价值进行求和，式中的 w l , w d l , w d d l , w d d d l {w_l},{w_{dl}},{w_{ddl}},{w_{dddl}} wl,wdl,wddl,wdddl都是对于优化变量的惩罚项，以及偏移终点的惩罚项 w e n d − l , w e n d − d l , w e n d − d d l , w e n d − d d d l {w_{end - l}},{w_{end - dl}},{w_{end - ddl}},{w_{end - dddl}} wend−l,wend−dl,wend−ddl,wend−dddl。
ps：三阶导的求解方式为： l ′ ′ i + 1 − l ′ ′ i Δ s \frac{{{{l''}_{i + 1}} - {{l''}_i}}}{{\Delta s}} Δsl′′i+1−l′′i
按照二次型的标准型，将目标函数的二次项系数和一次项系数用矩阵表示： m i n i m i z e 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u \begin{array}{lllllllllllllll}{{\rm{minimize}}}&{\frac{1}{2}{x^T}Px + {q^T}x}\\{{\rm{subject to}}}&{l \le Ax \le u}\end{array} minimizesubjectto21xTPx+qTxl≤Ax≤u
2.4 设计约束
接下来谈谈约束的设计。
约束要满足：
• 主车必须在道路边界内，同时不能和障碍物有碰撞 l i ∈ ( l min i , l max i ) {l_i} \in (l_{\min }^i,l_{\max }^i) li∈(lmini,lmaxi) 边界的约束已经在1.4 Task： PATH_BOUNDS_DECIDER里讲述过了。
• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
首先是曲率的约束，车辆在行驶时有最大曲率半径的限制，根据Frenet坐标的转换公式（该公式来源于这篇论文——Werling M, Ziegler J, Kammel S, et al. Optimal trajectory generation for dynamic street scenarios in a frenet frame[C]//2010 IEEE International Conference on Robotics and Automation. IEEE, 2010: 987-993.）：
l l l的二阶导于曲率有如上关系，但我们无法直接将其应用于约束的设计（约束函数为一次）之中，对此需要对其进行简化。
假设1：参考线规划： θ − θ r e f = Δ θ ≈ 0 \theta - {\theta _{ref}} = \Delta \theta \approx 0 θ−θref=Δθ≈0，即航向角几乎为0.
假设2：规划的曲率数值上很小，所以两个曲率相乘近乎为0. 0 < κ r e f < κ ≪ 1 → κ r e f κ ≈ 0 0{\rm{ }} < {\kappa _{ref}} < \kappa \ll 1 \to {\kappa _{ref}}\kappa {\rm{ }} \approx {\rm{ }}0 0<κref<κ≪1→κrefκ≈0 依据上述假设，我们将上述关系简化为： d 2 l d s 2 = κ − κ r e f \frac{{{d^2}l}}{{d{s^2}}} = \kappa - {\kappa _{ref}} ds2d2l=κ−κref 根据车辆运动学关系计算最大曲率： κ max = tan ( α max ) L {\kappa _{\max }} = \frac{{\tan ({\alpha _{\max }})}}{L} κmax=Ltan(αmax) 得到 l ¨ \ddot l l¨的约束范围： − κ max − κ r e f < l ¨ i < κ max − κ r e f - {\kappa _{\max }} - {\kappa _{ref}} < {\ddot l_i} < {\kappa _{\max }} - {\kappa _{ref}} −κmax−κref<l¨i<κmax−κref 另外还得满足曲率变化率的要求（即规划处的路径能使方向盘在最大角速度下能够及时的转过来）： d 3 l d s 3 = d d 2 t d l 2 d t ⋅ d t d s \frac{{{d^3}l}}{{d{s^3}}} = \frac{{d\frac{{{d^2}t}}{{d{l^2}}}}}{{dt}} \cdot \frac{{dt}}{{ds}} ds3d3l=dtddl2d2t⋅dsdt 主路行驶中，实际车轮转角很小 α → 0 α→0 α→0，近似有 t a n α ≈ α tan α ≈ α tanα≈α,从而有： d 2 l d s 2 ≈ κ − κ r e f = tan ( α max ) L − κ r e f ≈ α L − κ r e f \frac{{{d^2}l}}{{d{s^2}}} \approx \kappa - {\kappa _{ref}} = \frac{{\tan ({\alpha _{\max }})}}{L} - {\kappa _{ref}} \approx \frac{\alpha }{L} - {\kappa _{ref}} ds2d2l≈κ−κref=Ltan(αmax)−κref≈Lα−κref 同时假设，在一个周期内规划的路径上车辆的速度是恒定的 v = d t d s v = \frac{{dt}}{{ds}} v=dsdt 代入三阶导公式得到三阶导的边界 d 3 l d s 3 = α ′ L v < α ′ max L v \frac{{{d^3}l}}{{d{s^3}}} = \frac{{\alpha '}}{{Lv}} < \frac{{{{\alpha '}_{\max }}}}{{Lv}} ds3d3l=Lvα′<Lvα′max
总结
- 必须在道路边界内，同时不能和障碍物有碰撞 l i ∈ ( l min i , l max i ) {l_i} \in (l_{\min }^i,l_{\max }^i) li∈(lmini,lmaxi)
- 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
- 必须满足基本的物理原理：
三阶导 l ′ ′ ′ {l'''} l′′′可以积成二阶导 l ′ ′ {l''} l′′，二阶导 l ′ ′ {l''} l′′可以积成一阶导 l ′ {l'} l′,一阶导 l ′ {l'} l′可以积成 l l l。三阶导 l ′ ′ ′ {l'''} l′′′为常量，二阶导 l ′ ′ {l''} l′′，一阶导 l ′ {l'} l′， l l l为连续可导的。每个点之间都是三界多项的关系。
将上述内容转化为约束矩阵。
l 0 = l i n i t l_0=l_{init} l0=linit, l ˙ 0 = l i n i t \dot l_0=l_{init} l˙0=linit, l ¨ 0 = l i n i t \ddot l_0=l_{init} l¨0=linit满足的是起点的约束，即为实际车辆规划起点的状态。
2.5 求解器求解
最后是运用求解器进行求解。求解器求解同样需要四个步骤：设定OSQP求解参数、计算QP系数矩阵、构造OSQP求解器、获取优化结果。
2.5.1 设定OSQP求解参数
// /home/yuan/apollo-edu/modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.cc
OSQPSettings* PiecewiseJerkSpeedProblem::SolverDefaultSettings() {
// Define Solver default settings
OSQPSettings* settings =
reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
osqp_set_default_settings(settings);
settings->eps_abs = 1e-4;
settings->eps_rel = 1e-4;
settings->eps_prim_inf = 1e-5;
settings->eps_dual_inf = 1e-5;
settings->polish = true;
settings->verbose = FLAGS_enable_osqp_debug;
settings->scaled_termination = true;
return settings;
}
2.5.2 计算QP系数矩阵
// /home/yuan/apollo-edu/modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
OSQPData* PiecewiseJerkProblem::FormulateProblem() {
// calculate kernel
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
CalculateKernel(&P_data, &P_indices, &P_indptr); // 二次项系数P的矩阵
// calculate affine constraints
std::vector<c_float> A_data;
std::vector<c_int> A_indices;
std::vector<c_int> A_indptr;
std::vector<c_float> lower_bounds;
std::vector<c_float> upper_bounds;
CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,
&upper_bounds); // 约束项系数A的矩阵
// calculate offset
std::vector<c_float> q;
CalculateOffset(&q); // 一次项系数q的向量
OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
CHECK_EQ(lower_bounds.size(), upper_bounds.size());
size_t kernel_dim = 3 * num_of_knots_;
size_t num_affine_constraint = lower_bounds.size();
data->n = kernel_dim;
data->m = num_affine_constraint;
data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data),
CopyData(P_indices), CopyData(P_indptr));
data->q = CopyData(q);
data->A =
csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),
CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));
data->l = CopyData(lower_bounds);
data->u = CopyData(upper_bounds);
return data;
}
2.5.3 构造OSQP求解器
OSQPWorkspace* osqp_work = nullptr;
osqp_work = osqp_setup(data, settings);
2.5.4 获取优化结果
osqp_solve(osqp_work);
auto status = osqp_work->info->status_val;// 获取优化值
if (status < 0 || (status != 1 && status != 2)) {
AERROR << "failed optimization status:\t" << osqp_work->info->status;
osqp_cleanup(osqp_work);
FreeData(data);
c_free(settings);
return false;
} else if (osqp_work->solution == nullptr) {
AERROR << "The solution from OSQP is nullptr";
osqp_cleanup(osqp_work);
FreeData(data);
c_free(settings);
return false;
}
// extract primal results
x_.resize(num_of_knots_);
dx_.resize(num_of_knots_);
ddx_.resize(num_of_knots_);
for (size_t i = 0; i < num_of_knots_; ++i) {
x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0];
dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1];
ddx_.at(i) =
osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2];
} //优化变量的取值
// Cleanup
osqp_cleanup(osqp_work);
FreeData(data);
c_free(settings);
return true;
3. 路径规划算法代码解读
3.1 总流程和决策\优化的入口函数
路径规划从参考线平滑开始，参考线模块结束后，会将中间计算结果保存在ReferenceLineinfo之中。之后按照路径规划的任务，依次执行上图中的任务。任务包括了决策器以及优化器的任务。
3.2 产生换道决策
// /home/yuan/apollo-edu/modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc
void LaneChangeDecider::UpdateStatus(double timestamp,
ChangeLaneStatus::Status status_code,
const std::string& path_id) {
auto* lane_change_status = injector_->planning_context()
->mutable_planning_status()
->mutable_change_lane();
lane_change_status->set_timestamp(timestamp);
lane_change_status->set_path_id(path_id);
lane_change_status->set_status(status_code);
}
产生换道的状态，之后将结果保存在injector中。
3.3 产生借道决策
// /home/yuan/apollo-edu/modules/planning/tasks/deciders/path_lane_borrow_decider/path_lane_borrow_decider.cc
// By default, don't borrow any lane.
reference_line_info->set_is_path_lane_borrow(false);
// Check if lane-borrowing is needed, if so, borrow lane.
if (Decider::config_.path_lane_borrow_decider_config()
.allow_lane_borrowing() &&
IsNecessaryToBorrowLane(*frame, *reference_line_info)) {
reference_line_info->set_is_path_lane_borrow(true);
}
当产生阶导决策时，会将相应标志位置true。
if (!left_borrowable && !right_borrowable) {
mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
return false;
} else {
mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
if (left_borrowable) {
mutable_path_decider_status->add_decided_side_pass_direction(
PathDeciderStatus::LEFT_BORROW);
}
if (right_borrowable) {
mutable_path_decider_status->add_decided_side_pass_direction(
PathDeciderStatus::RIGHT_BORROW);
}
}
同时，在进行借道决策时，会对左右借道进行判断。借道的状态保存在injetor里。
3.4 产生路径边界
根据现有决策在参考线上进行采样，获得每个点在 l l l的边界。有四种边界决策：GenerateRegularPathBound（自车道行驶）、GenerateFallbackPathBound（失败回退）、GenerateLaneChangePathBound、GeneratePullOverPathBound。最后将边界保存在SetCandidatePathBoundaries中，供下一步使用。
3.5 路径优化
piecewise_jerk_problem.set_x_ref(std::move(weight_x_ref_vec),
path_reference_l_ref);
}
// for debug:here should use std::move
piecewise_jerk_problem.set_weight_x(w[0]);
piecewise_jerk_problem.set_weight_dx(w[1]);
piecewise_jerk_problem.set_weight_ddx(w[2]);
piecewise_jerk_problem.set_weight_dddx(w[3]);
piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
auto start_time = std::chrono::system_clock::now();
piecewise_jerk_problem.set_x_bounds(lat_boundaries);
piecewise_jerk_problem.set_dx_bounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);
// Estimate lat_acc and jerk boundary from vehicle_params
const auto& veh_param =
common::VehicleConfigHelper::GetConfig().vehicle_param();
const double axis_distance = veh_param.wheel_base();
const double max_yaw_rate =
veh_param.max_steer_angle_rate() / veh_param.steer_ratio() / 2.0;
const double jerk_bound = EstimateJerkBoundary(std::fmax(init_state[1], 1.0),
axis_distance, max_yaw_rate);
piecewise_jerk_problem.set_dddx_bound(jerk_bound);
bool success = piecewise_jerk_problem.Optimize(max_iter);
调用piecewise_jerk_problem类进行求解，会设置一些权重以及一些约束，利用Optimize函数进行求解。
const auto& path_boundaries =
reference_line_info_->GetCandidatePathBoundaries();
ADEBUG << "There are " << path_boundaries.size() << " path boundaries.";
const auto& reference_path_data = reference_line_info_->path_data();
std::vector<PathData> candidate_path_data;
for (const auto& path_boundary : path_boundaries) {
size_t path_boundary_size = path_boundary.boundary().size();
reference_line_info_->GetCandidatePathBoundaries();保存候选路径。
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));
3.6 选择最优路径
bool ComparePathData(const PathData& lhs, const PathData& rhs,
const Obstacle* blocking_obstacle) {
ADEBUG << "Comparing " << lhs.path_label() << " and " << rhs.path_label();
// Empty path_data is never the larger one.
if (lhs.Empty()) {
ADEBUG << "LHS is empty.";
return false;
}
if (rhs.Empty()) {
ADEBUG << "RHS is empty.";
return true;
}
调用ComparePathData函数，对路径进行两两比较。
*(reference_line_info->mutable_path_data()) = valid_path_data.front();
reference_line_info->SetBlockingObstacle(
valid_path_data.front().blocking_obstacle_id());
将最优的路径保存在reference_line_info中。将阻塞障碍物最近的放在reference_line_info中，供速度规划进一步处理。
4. 路径规划算法实践
云实验地址——Apollo规划之路径规划仿真调试
4.1 观察借道绕行、自车道绕行障碍物、自车道巡航、换道时路径的边界和规划的路径
(1）在终端中输入以下指令，启动dreamview
bash scripts/bootstrap_neo.sh
(2）模式选择Mkz Standard Debug，地图选择Apollo Virutal Map，打开Sim_Control模式，打开PNC Monitor,等待屏幕中间区域出现Mkz车模型和地图后即表示成功进入仿真模式。
(3）点击左侧Tab栏Module Controller，启动Planning,Prediction，Routing模块，如果需要录制数据则打开Recorder模块。
(4）模块启动完成后，点击左侧Tab栏Profile，选择Scenario Profiles里的course场景集，右上角选择场景场景开始仿真，分别点击自车道内行驶，绕行障碍物，借道绕行，换道场景。观察路径曲线和路径边界.
Layer Menu中控制Planning的各个path和boundary开关可以显示和关闭每一条候选路径.
4.2 借道绕行场景，调整 l , l ’ , l ’’ l,l’,l’’ l,l’,l’’的权重系数，观察路径的变化
在modules/planning/conf/planning_config.pb.txt配置文件中，包含了路径规划任务的相关参数，我们可以过对这些参数的调整，达到我们期望路径规划效果。
(1)使用在线编辑工具修改/apollo/modules/planning/conf目录下的planning_config.pb.txt文件，增大default_path_config的l_weight，减小dl_weight ddl_weight dddl_weight。
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
}
}
}
(2)修改好代码参数后，保存这个文件，在ModuleController中重启planning模块（必须步骤)。
(3）重新选择借道绕行场景，观察轨迹和调整前有何变化
提前借道，提前回到原先车道，轨迹曲率更大。
4.3 靠边停车实践，开启靠边停车功能，观察路径的变化
恢复参数为默认值，在planning.conf中添加命令行参数–enable_scenario_pull_over=true，启动靠边停车，修改好代码参数后，保存这个文件，在Module Controller中重启planning模块（必须步骤）。
靠边停车目标点产生惩罚项使得规划出的轨迹偏离原参考线。