Apollo星火计划学习笔记——Apollo路径规划算法原理与实践

文章目录

前言
TASK系列解析文章
1. 路径规划算法总体介绍
2. 基于二次规划的路径规划算法
3. 路径规划算法代码解读
4. 路径规划算法实践

前言

Apollo星火计划课程链接如下
星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

TASK系列解析文章

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

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等多种语言。
二次规划问题的标准形式为： $\begin{array}{lllllllllllllll}{{\rm{minimize}}}&{\frac{1}{2}{x^T}Px + {q^T}x}\\{{\rm{subject to}}}&{l \le Ax \le u}\end{array}$ where $\in {{\bf{R}}^n}$ is the optimization variable. The objective function is defined by a positive semidefinite matrix $\in {\bf{S}}_ + ^n$ and vector $\in {{\bf{R}}^n}$ . The linear constraints are defined by matrix $\in {{\bf{R}}^{m \times n}}$ and vectors $l$ and $u$ so that ${l_i} \in {\bf{R}} \cup \{ - \infty \}$ and ${`u_i} \in {\bf{R}} \cup \{ + \infty \}$ for all $\in \{ 1, \ldots ,m{\rm{\} }}$ .
二次规划优化问题为二次型，其约束为线性型。 $x$ 是要优化的变量，是一个 $n$ 维的向量。 $p$ 是二次项系数，是正定矩阵。 $q$ 是一次项系数，是 $n$ 维向量。 $A$ 是一个 $m$ x $n$ 的矩阵， $A$ 为约束函数的一次项系数， $m$ 为约束函数的个数。 $l$ 和 $u$ 分别为约束函数的下边界和上边界。

常用二次规划求解器：

OSQP :使用ADMM方法求解。对于规模大的，含有大量等式或不等式约束的问题有较好的求解效率。
qpOASES: 用可行域法，对于约束较少的小规模问题，qpOASES求解更快。

二次规划问题的求解往往有以下几个步骤：定义目优化变量、设计目标函数、设计约束、求解器求解等几个步骤。

2.2 定义优化变量

在这里插入图片描述路径规划一般是在Frenet坐标系中进行的。 $s$ 为沿着参考线的方向， $l$ 为垂直于坐标系的方向。如图所示，将障碍物分别投影到SL坐标系上。在 $s$ 方向上，以间隔 $\Delta s$ 作为一个间隔点，从 $s_0$ , $s_1$ , $s_2$ 一直到 $s_{n-1}$ ，构成了规划的路径。选取每个间隔点的 $l$ 作为优化的变量，同时也将 $\dot l$ 和 $\ddot l$ 也作为优化变量。
在这里插入图片描述如此，就构成了优化变量 $x$ ， $x$ 有三个部分组成：从 $l_0$ , $l_1$ , $l_2$ 到 $l_{n-1}$ ，从 $\dot l_0$ , $\dot l_1$ , $\dot l_2$ 到 $\dot l_{n-1}$ ,从 $\ddot l_0$ , $\ddot l_1$ , $\ddot l_2$ 到 $\ddot l_{n-1}$ .

2.3 设计目标函数

对于目标函数的设计，我们需要明确以下目标：

确保安全、礼貌的驾驶，尽可能贴近车道中心线行驶： $\left| {{l_i}} \right| \downarrow$
确保舒适的体感，尽可能降低横向速度/加速度/加加速度： $\left| {{{\dot l}_i}} \right| \downarrow$ ， $\left| {{{\ddot l}_i}} \right| \downarrow$ ， $\left| {{{l'''}_{i \to i + 1}}} \right| \downarrow$
确保终点接近参考终点（这个往往用在靠边停车场景之中国）： ${l_{end}} = {l_{ref}}$

最后会得到以下目标函数：
在这里插入图片描述对每个点设计二次的目标函数并对代价值进行求和，式中的 ${w_l},{w_{dl}},{w_{ddl}},{w_{dddl}}$ 都是对于优化变量的惩罚项，以及偏移终点的惩罚项 ${w_{end - l}},{w_{end - dl}},{w_{end - ddl}},{w_{end - dddl}}$ 。

ps：三阶导的求解方式为： $\frac{{{{l''}_{i + 1}} - {{l''}_i}}}{{\Delta s}}$
按照二次型的标准型，将目标函数的二次项系数和一次项系数用矩阵表示： $\begin{array}{lllllllllllllll}{{\rm{minimize}}}&{\frac{1}{2}{x^T}Px + {q^T}x}\\{{\rm{subject to}}}&{l \le Ax \le u}\end{array}$ 在这里插入图片描述

2.4 设计约束

接下来谈谈约束的设计。
约束要满足：
• 主车必须在道路边界内，同时不能和障碍物有碰撞 ${l_i} \in (l_{\min }^i,l_{\max }^i)$ 边界的约束已经在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$ 的二阶导于曲率有如上关系，但我们无法直接将其应用于约束的设计（约束函数为一次）之中，对此需要对其进行简化。
假设1：参考线规划： $\theta - {\theta _{ref}} = \Delta \theta \approx 0$ ，即航向角几乎为0.
假设2：规划的曲率数值上很小，所以两个曲率相乘近乎为0. $0{\rm{ }} < {\kappa _{ref}} < \kappa \ll 1 \to {\kappa _{ref}}\kappa {\rm{ }} \approx {\rm{ }}0$ 依据上述假设，我们将上述关系简化为： $\frac{{{d^2}l}}{{d{s^2}}} = \kappa - {\kappa _{ref}}$ 根据车辆运动学关系计算最大曲率： ${\kappa _{\max }} = \frac{{\tan ({\alpha _{\max }})}}{L}$ 得到 $\ddot l$ 的约束范围： ${\kappa _{\max }} - {\kappa _{ref}} < {\ddot l_i} < {\kappa _{\max }} - {\kappa _{ref}}$ 另外还得满足曲率变化率的要求（即规划处的路径能使方向盘在最大角速度下能够及时的转过来）： $\frac{{{d^3}l}}{{d{s^3}}} = \frac{{d\frac{{{d^2}t}}{{d{l^2}}}}}{{dt}} \cdot \frac{{dt}}{{ds}}$ 主路行驶中，实际车轮转角很小 $α \to 0$ ，近似有 $t an α \approx α$ ,从而有： $\frac{{{d^2}l}}{{d{s^2}}} \approx \kappa - {\kappa _{ref}} = \frac{{\tan ({\alpha _{\max }})}}{L} - {\kappa _{ref}} \approx \frac{\alpha }{L} - {\kappa _{ref}}$ 同时假设，在一个周期内规划的路径上车辆的速度是恒定的 $\frac{{ds}}{{dt}}$ 代入三阶导公式得到三阶导的边界 $\frac{{{d^3}l}}{{d{s^3}}} = \frac{{\alpha '}}{{Lv}} < \frac{{{{\alpha '}_{\max }}}}{{Lv}}$

总结

必须在道路边界内，同时不能和障碍物有碰撞 ${l_i} \in (l_{\min }^i,l_{\max }^i)$
根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：

在这里插入图片描述

必须满足基本的物理原理：

在这里插入图片描述三阶导 ${l'''}$ 可以积成二阶导 ${l''}$ ，二阶导 ${l''}$ 可以积成一阶导 ${l'}$ ,一阶导 ${l'}$ 可以积成 $l$ 。三阶导 ${l'''}$ 为常量，二阶导 ${l''}$ ，一阶导 ${l'}$ ， $l$ 为连续可导的。每个点之间都是三界多项的关系。
将上述内容转化为约束矩阵。在这里插入图片描述 $l_0=l_{init}$ , $\dot l_0=l_{init}$ , $\ddot l_0=l_{init}$ 满足的是起点的约束，即为实际车辆规划起点的状态。

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之中。之后按照路径规划的任务，依次执行上图中的任务。任务包括了决策器以及优化器的任务。

决策器的入口函数。输入每一帧的数据结构总类frame、参考线的中间计算结果current_reference_line_info到求解器中进行求解，最后结果保存在current_reference_line_info中。
在这里插入图片描述

优化器的入口函数。输入speed_data、reference_line、init_point、path_reusable、final_path_data等信息到求解器中求解，最后将结果保存在reference_line中。

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$ 的边界。有四种边界决策：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开关可以显示和关闭每一条候选路径. 在这里插入图片描述

借道绕行

自车道行驶

在这里插入图片描述

自车道行驶

自车道绕行（Nudge障碍物）

在这里插入图片描述

自车道绕行（Nudge障碍物）
在这里插入图片描述

变道

在这里插入图片描述

变道

4.2 借道绕行场景，调整 $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模块（必须步骤）。
在这里插入图片描述靠边停车目标点产生惩罚项使得规划出的轨迹偏离原参考线。