apollo 二次规划算法解析(PiecewiseJerkPathOptimizer)

RedZero9

写这篇文章主要是想做个笔记，记录一下对apollo 横向规划算法的学习。中间参考了很多博客，就不一一列举了，如有侵权，请私信。（中间如果有啥错误话，欢迎评论探讨）
参考自：Apollo二次规划算法(piecewise jerk path optimizer)解析
https://www.cfanz.cn/resource/detail/pnkryVOVYmWzZ
steve：Apollo 6.0 QP（二次规划）算法解析
团与安奴：Apoollo中piecewise_jerk_problem中CSC矩阵及连续性约束相关问题
在lane_follow的场景下，会调用的task顺序如下

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
}任务代码调用入口

比如通过测试用例可得到如下的堆栈：
apollo::planning::PiecewiseJerkPathOptimizer::Process(apollo::planning::PiecewiseJerkPathOptimizer * const this, const apollo::planning::SpeedData & speed_data, const apollo::planning::ReferenceLine & reference_line, const apollo::common::TrajectoryPoint & init_point, const bool path_reusable, apollo::planning::PathData * const final_path_data) (piecewise_jerk_path_optimizer.cc:61)
apollo::planning::PathOptimizer::Execute(apollo::planning::PathOptimizer * const this, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * const reference_line_info) (path_optimizer.cc:45)
apollo::planning::scenario::lane_follow::LaneFollowStage::PlanOnReferenceLine(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ReferenceLineInfo * reference_line_info) (lane_follow_stage.cc:167)
apollo::planning::scenario::lane_follow::LaneFollowStage::Process(apollo::planning::scenario::lane_follow::LaneFollowStage * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame) (lane_follow_stage.cc:116)
apollo::planning::scenario::Scenario::Process(apollo::planning::scenario::Scenario * const this, const apollo::common::TrajectoryPoint & planning_init_point, apollo::planning::Frame * frame) (scenario.cc:76)
apollo::planning::PublicRoadPlanner::Plan(apollo::planning::PublicRoadPlanner * const this, const apollo::common::TrajectoryPoint & planning_start_point, apollo::planning::Frame * frame, apollo::planning::ADCTrajectory * ptr_computed_trajectory) (public_road_planner.cc:38)
apollo::planning::OnLanePlanning::Plan(apollo::planning::OnLanePlanning * const this, const double current_time_stamp, const std::vector<apollo::common::TrajectoryPoint, std::allocator<apollo::common::TrajectoryPoint> > & stitching_trajectory, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:572)
apollo::planning::OnLanePlanning::RunOnce(apollo::planning::OnLanePlanning * const this, const apollo::planning::LocalView & local_view, apollo::planning::ADCTrajectory * const ptr_trajectory_pb) (on_lane_planning.cc:417)
apollo::planning::PlanningTestBase::RunPlanning(apollo::planning::PlanningTestBase * const this, const std::__cxx11::string & test_case_name, int case_num, bool no_trajectory_point) (planning_test_base.cc:229)
apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test::TestBody(apollo::planning::SunnyvaleBigLoopTest_keep_clear_02_Test * const this) (sunnyvale_big_loop_test.cc:191可看出，
apollo http://PiecewiseJerkPathOptimizer.cc

PiecewiseJerkPathOptimizer 是lanefollow 场景下，所调用的第 5 个 task，属于task中的optimizer类别它的作用主要是：

• 根据之前decider决策的reference line和 path bound，以及横向约束，将最优路径求解问题转化为二次型规划问题；
  

2. 调用osqp库求解最优路径；
PiecewiseJerkPathOptimizer被调用流程与其他task类似：


在之前的task中，每条reference line会生成fall_back path bound以及另外一条规划算法正常运行时的path bound，在这里会分别为这两个path bound 生成最优轨迹。


判断是否要重规划，与其他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();
  }
  ... ...
将起始点转化为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
    }
  }
}遍历每个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 = 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 << ", dl = " << opt_dl;
      }
      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();OptimizePath( ) 是优化算法的实现，将最优路径求解问题转化为二次型规划问题，之后调用osqp库求解最优路径。
Apollo  http://PiecewiseJerkProblem.cc

一、二次规划定义



Apollo使用osqp进行二次规划问题的求解。第一行为代价函数，第二行为约束条件。二次优化的目标就是在满足约束条件的基础上，找到优化变量使得代价函数的值最小。二次规划只在代价函数为凸函数的时候能够收敛到最优解，因此这需要P矩阵为半正定矩阵，这是非常重要的一个条件。这反映在Apollo中的规划算法则为需要进行求解的空间为凸空间，这样二次规划才能收敛到一条最优路径。
二、Apollo 中二次规划问题的构造

在Apollo中，该task的代码在/modules/planning/tasks/optimizers/piecewise_jerk_path  下，而其中会调用/modules/planning/math/piecewise_jerk  下的类方法来帮助其完成二次规划问题的构造及求解工作。因此我们着重关注该文件夹下的几个文件，来解析Apollo对于二次规划问题的构思及实现。


可以看到该文件夹下包含三个类，piecewise_jerk_problem为基类，在这个基类中主要实现三个功能——整体最优化问题的框架、构造约束条件、求解二次优化问题。而piecewise_jerk_path_problem和piecewise_jerk_speed_problem则是这个基类的派生类，由此我们可以知道在Apollo中，path优化和speed优化的约束条件其实是一样的，都是在基类中实现的那个约束条件构造函数。而这两个派生类中又分别独自实现了代价函数的构造和一次项的构造这两个功能。下面我们详细来解读一下这三个类算法。

• 首先讲解的是piecewise_jerk_problem这个基类中的Optimize函数，该函数被task中的optimizer直接调用，因此算是一个入口函数。
  

该函数中会调用FormulateProblem来构造出二次规划问题的框架，再调用osqp库进行求解，从而求出最优path。需要注意的是，二次规划问题的求解方式有许多种，包括拉格朗日法，梯度下降等等。
在path优化中，优化变量x为frenet坐标系下的l坐标，l一阶导和l二阶导


.  FormulateProblem()


FormulateProblem  这个函数用于构造最优化问题具体矩阵。首先构造出P矩阵即代价函数，然后构造A矩阵即约束矩阵以及上下边界lower_bounds和upper_bounds，最后构建一次项q向量。构造完后将矩阵都存储进OSQPData这个结构体里，以便后续直接调用osqp库进行求解。以下详细解读这些矩阵的具体形式。






CalculateKnernel  用于构造代价函数即P矩阵
代码如下：
构建P:
参见公式10。
void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,
                                               std::vector<c_int>* P_indices,
                                               std::vector<c_int>* P_indptr) {
  const int n = static_cast<int>(num_of_knots_);
  const int num_of_variables = 3 * n;
  const int num_of_nonzeros = num_of_variables + (n - 1);
  std::vector<std::vector<std::pair<c_int, c_float>>> columns(num_of_variables);
  int value_index = 0;

  // x(i)^2 * (w_x + w_x_ref), w_x_ref might be a uniform value for all x(i)
  // or piecewise values for different x(i) 参见公式7
  for (int i = 0; i < n - 1; ++i) {
    columns.emplace_back(i, (weight_x_ + weight_x_ref_vec_) /
                                   (scale_factor_[0] * scale_factor_[0]));
    ++value_index;
  }
  // x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x)  
  columns[n - 1].emplace_back(
      n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /
                 (scale_factor_[0] * scale_factor_[0]));
  ++value_index;

  // x(i)'^2 * w_dx  参见公式8
  for (int i = 0; i < n - 1; ++i) {
    columns[n + i].emplace_back(
        n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
    ++value_index;
  }
  // x(n-1)'^2 * (w_dx + w_end_dx)
  columns[2 * n - 1].emplace_back(2 * n - 1,
                                  (weight_dx_ + weight_end_state_[1]) /
                                      (scale_factor_[1] * scale_factor_[1]));
  ++value_index;

  auto delta_s_square = delta_s_ * delta_s_;
  // x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)  参见公式9的对角线
  columns[2 * n].emplace_back(2 * n,
                              (weight_ddx_ + weight_dddx_ / delta_s_square) /
                                  (scale_factor_[2] * scale_factor_[2]));
  ++value_index;
  for (int i = 1; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(
        2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
                       (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }
  columns[3 * n - 1].emplace_back(
      3 * n - 1,
      (weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
          (scale_factor_[2] * scale_factor_[2]));
  ++value_index;

  // -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''  参见公式9的对角线下面的次对角线
  for (int i = 0; i < n - 1; ++i) {
    columns[2 * n + i].emplace_back(2 * n + i + 1,
                                    (-2.0 * weight_dddx_ / delta_s_square) /
                                        (scale_factor_[2] * scale_factor_[2]));
    ++value_index;
  }

  CHECK_EQ(value_index, num_of_nonzeros);

  int ind_p = 0;
  for (int i = 0; i < num_of_variables; ++i) {
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns) {
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}这里把代价函数P矩阵实际解构了出来，我觉得对于第一次接触这块代码的朋友，能够直观的看到这个矩阵还是会对理解Apollo的算法思想有很大帮助的。可以注意到每个元素前都乘以了2，这是为了和二次优化问题的一般形式中的1/2进行抵消的。


CalculateAffineConstraint()


该函数是用于构造最优化问题的限制条件，即A矩阵的。需要注意的是，该方法为基类中实现，因此对于path优化和speed优化来说，两者在Apollo  的算法思想里，所收到的约束是一样的。Apollo  该算法的精妙之处就在于，将path和speed分别在SL和ST空间中进行考虑，使得两者的优化思想非常类似，很巧妙地完成两个维度的求解。
同样，将A矩阵的具体形式用Latex表现了出来，大家可以对照着Apollo的代码，来实际自己写写看这些矩阵，会对二次规划问题的形式有更深刻的了解。




将这些矩阵表示为数学表达式，则为以下这些限制：




其中对于5，6两个约束需要尤其注意，网上一些文章提到这两个为连续性约束，我感觉讲的不是很清晰。这两个约束其实是保证优化的变量x，x一阶导，x二阶导之间的依赖关系的，具体推导过程如下：






(1-6)和(1-8)即为所求得的连续性约束，个人认为其目的是为了将零阶状态用一二阶状态进行线性表示，这样才更为合理地表示各界状态的关联关系，毕竟低阶的运动状态是由高阶状态积分得到的。至于代码中出现的scale_factor，个人认为：保证了优化项之间差别不至于过大。如果优化项之间差别过大（比如跨了几个数量级）可能会导致过小的那个优化项无效。
至此，二次规划的整体框架就介绍完毕了，最终求解得到的优化变量极为frenet坐标系下的path，发给控制模块前，还需转到笛卡尔坐标系下。有一点需要注意，该optimizer会生成多条path，每条path是对应前面decider生成的相应的reference_line和path_boundary，包括了有self_lane, lane_borrow, lane_change, parking这几个标签。对于每个标签，decider会生成对应的reference_line及凸空间。optimizer生成多条path后，再根据一些规则挑选出最优的一条path，用于和speed_profile进行融合。

mastertravels77

牛啊牛啊