Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 1

最新推荐文章于 2025-06-01 09:05:03 发布

linxigjs

最新推荐文章于 2025-06-01 09:05:03 发布

阅读量3k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：代码解析文章标签：无人驾驶 Baidu Apollo 轨迹规划 EM Planner

本文链接：https://blog.youkuaiyun.com/linxigjs/article/details/99672231

大家好，我已经把优快云上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

QpSplinePathOptimizer 的入口函数在该类的Process()，在Process()中构造了QpSplinePathGenerator类的实例path_generator，轨迹生成工作由path_generator.Generate()完成。文件路径：apollo3_5/modules/planning/tasks/optimizers/qp_spline_path/qp_spline_path_optimizer.cc

Status QpSplinePathOptimizer::Process(const SpeedData& speed_data,
                                      const ReferenceLine& reference_line,
                                      const common::TrajectoryPoint& init_point,
                                      PathData* const path_data) {
  QpSplinePathGenerator path_generator(spline_solver_.get(), reference_line,
                                       qp_spline_path_config_,
                                       reference_line_info_->AdcSlBoundary());
  path_generator.SetDebugLogger(reference_line_info_->mutable_debug());
  path_generator.SetChangeLane(reference_line_info_->IsChangeLanePath());

  double boundary_extension = 0.0;
  bool is_final_attempt = false;
  //第一次调用Generate()时，boundary_extension=0.0。
  bool ret = path_generator.Generate(
      reference_line_info_->path_decision()->obstacles().Items(), speed_data,
      init_point, boundary_extension, is_final_attempt, path_data);
    ...
    boundary_extension = qp_spline_path_config_.cross_lane_lateral_extension();
    is_final_attempt = true;
    //若生成轨迹失败，则将boundary_extension增大为cross_lane_lateral_extension()（默认值1.2m)
    //以放松优化过程中的限制条件，使得更易求解，第二次调用Generate()
    ret = path_generator.Generate(
        reference_line_info_->path_decision()->obstacles().Items(), speed_data,
        init_point, boundary_extension, is_final_attempt, path_data);
    ...
  }

  return Status::OK();
}

QpSplinePathGenerator::Generate()中，主要有以下关键步骤：初始化样条曲线InitSpline()，初始化用来计算自车轨迹横向约束的类QpFrenetFrame::Init()，添加约束AddConstraint()，添加目标函数AddKernel()，优化问题求解器Solve()，最后将求得的轨迹点封装成DiscretizedPath。接下来一一分析。

//boundary_extension第一次尝试=0，第二次尝试增大了
bool QpSplinePathGenerator::Generate( ... ) {
  ...
  if (!InitSpline(start_s, end_s)) { ... }
  QpFrenetFrame qp_frenet_frame(reference_line_, speed_data, init_frenet_point_,
                                qp_spline_path_config_.time_resolution(),
                                evaluated_s_);
  if (!qp_frenet_frame.Init(obstacles)) { ... } 
  if (!AddConstraint(qp_frenet_frame, boundary_extension)) { ... }
  AddKernel();
  bool is_solved = Solve();
  ...
  // extract data
  //将求得的轨迹点封装成DiscretizedPath
  ...
}

InitSpline()主要完成对spline segment和纵向s采样点的初始化。

bool QpSplinePathGenerator::InitSpline(const double start_s, const double end_s) {
  uint32_t number_of_spline = static_cast<uint32_t>(
      (end_s - s