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

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（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 - start_s) / qp_spline_path_config_.max_spline_length() + 1.0);
  number_of_spline = std::max(1u, number_of_spline);
  common::util::uniform_slice(start_s, end_s, number_of_spline, &knots_);

  // spawn a new spline generator
  //产生number_of_spline条order阶spline
  spline_solver_->Reset(knots_, qp_spline_path_config_.spline_order());

  // set evaluated_s_
  uint32_t constraint_num = static_cast<uint32_t>(
      (end_s - start_s) / qp_spline_path_config_.max_constraint_interval() + 1);
  common::util::uniform_slice(start_s, end_s, constraint_num - 1, &evaluated_s_);
  return (knots_.size() > 1) && !evaluated_s_.empty();
  //难道max_spline_length和max_constraint_interval可以不相等吗？
  //难道constraint_num和number_of_spline可以不相等吗？
}

将纵向区间[start_s, end_s] 按照qp_spline_path_config_.max_spline_length()和qp_spline_path_config_.max_constraint_interval()的设置，均匀分割后存入knots_和evaluated_s_，每一段都会对应一条qp_spline_path_config_.spline_order()次多项式曲线。在这里，我认为knots_和evaluated_s_ 这2个记录s轴采样点的vector是应该完全相同的。不明白代码中为何有2种定义？

QpFrenetFrame::Init() 根据已知条件，计算对自车横向轨迹的约束。

1. HDMap和道路参考线对自车横向轨迹的约束

2. 静态动态障碍物及相应的decision对自车横向轨迹的约束

bool QpFrenetFrame::Init(const std::vector<const Obstacle*>& obstacles) {
  //根据SpeedData，计算自车纵向行经轨迹，存入vector<SpeedPoint>
  //即discretized_vehicle_location_
  if (!CalculateDiscretizedVehicleLocation()) { ... }

  //根据HDMap和道路参考线，计算自车轨迹的横向约束
  if (!CalculateHDMapBound()) { ... }

  //计算静态、动态障碍物及相应的decision对自车轨迹的横向约束
  if (!CalculateObstacleBound(obstacles)) { ... }
  return true;
}

Init()内调用了3个函数。

CalculateDiscretizedVehicleLocation()根据SpeedData按时间遍历计算了自车的纵向轨迹相关信息，存入vector<SpeedPoint>，即discretized_vehicle_location_。目前，SpeedData的来源我还没有搞清楚。

bool QpFrenetFrame::CalculateDiscretizedVehicleLocation() {
  for (double relative_time = 0.0; relative_time < speed_data_.TotalTime();
       relative_time += time_resolution_) {
    SpeedPoint veh_point;
    if (!speed_data_.EvaluateByTime(relative_time, &veh_point)) { ... }
    veh_point.set_t(relative_time);
    discretized_vehicle_location_.push_back(std::move(veh_point));
  }
  return true;
}

1. HDMap和道路参考线对自车横向轨迹的约束

CalculateHDMapBound()计算了沿纵轴s均匀采样所得点集中 各个点的横向约束，即左右边界。

//根据HDMap和道路参考线，计算自车轨迹的横向约束
//hdmap_bound_初始化为vector {evaluated_s_.size(), std::make_pair(-inf, inf)}
bool QpFrenetFrame::CalculateHDMapBound() {
  const double adc_half_width = vehicle_param_.width() / 2.0;
  for (uint32_t i = 0; i < hdmap_bound_.size(); ++i) {
    double left_bound = 0.0;
    double right_bound = 0.0;
    bool suc = reference_line_.GetLaneWidth(evaluated_s_[i], &left_bound, &right_bound);
    if (!suc) {
      AWARN << "Extracting lane width failed at s = " << evaluated_s_[i];
      right_bound = FLAGS_default_reference_line_width / 2;
      left_bound = FLAGS_default_reference_line_width / 2;
    }
    //按照右手坐标系，自车前方为s轴正方向，hdmap_bound_[i].first是右侧边界，second是左侧边界
    hdmap_bound_[i].first = -right_bound + adc_half_width;
    hdmap_bound_[i].second = left_bound - adc_half_width;

    //如果右边界>左边界，不合理，则缩短纵向s上限feasible_longitudinal_upper_bound_到当前考察点
    if (hdmap_bound_[i].first >= hdmap_bound_[i].second) {
      ...
      feasible_longitudinal_upper_bound_ =
          std::min(evaluated_s_[i], feasible_longitudinal_upper_bound_);
      ...
    }
  }
  return true;
}

2. 静态动态障碍物及相应的decision对自车横向轨迹的约束

CalculateObstacleBound()的处理分为2部分：静态障碍物和动态障碍物。首先判断障碍物是否有LateralDecision，若没有，则忽略，因为对path没有影响（这个阶段的path重点考虑横向运动）。LateralDecision主要是指横向的nudge，具体decision相关信息可查阅apollo3_5/modules/planning/proto/decision.proto。

//计算静态、动态障碍物及相应的decision对自车轨迹的横向约束
bool QpFrenetFrame::CalculateObstacleBound(const std::vector<const Obstacle*>& obstacles) {
  for (const auto ptr_obstacle : obstacles) {
    //path是横向轨迹，若没有LateralDecision，就对path优化没有影响，主要指nudge
    if (!ptr_obstacle->HasLateralDecision()) {
      continue;
    }
    if (ptr_obstacle->IsStatic()) {
      if (!MapStaticObstacleWithDecision(*ptr_obstacle)) { ... }
    } else {
      if (!MapDynamicObstacleWithDecision(*ptr_obstacle)) { ... }
    }
  }
  return true;
}

MapStaticObstacleWithDecision()考虑了静态障碍物以及相应的横向避让措施对可行驶区域宽度的影响，处理结果static_obstacle_bound_保存了evaluated_s_中各采样点处的横向可行驶范围。

//考虑静态障碍物以及相应的横向避让措施对自车轨迹横向的约束
//static_obstacle_bound_保存了evaluated_s_中采样点处的横向约束区间
bool QpFrenetFrame::MapStaticObstacleWithDecision(const Obstacle& obstacle) {
  if (!obstacle.HasLateralDecision()) { ... }
  const auto& decision = obstacle.LateralDecision();
  if (!decision.has_nudge()) { ... }
  if (!MapNudgePolygon(common::math::Polygon2d(obstacle.PerceptionBoundingBox()),
                       decision.nudge(), &static_obstacle_bound_)) { ... }
  return true;
}

处理静态障碍物的主要思路是将其轮廓端点映射到Frenet坐标系，结合nudge的方向计算障碍物占据的横向范围，由MapNudgePolygon()和MapNudgeLine()实现。 

//确定障碍物轮廓对纵向s范围内轨迹采样点的横向约束
bool QpFrenetFrame::MapNudgePolygon(
    const common::math::Polygon2d& polygon, const ObjectNudge& nudge,
    std::vector<std::pair<double, double>>* const bound_map) {
  std::vector<common::SLPoint> sl_corners;
  for (const auto& corner_xy : polygon.points()) {
    common::SLPoint corner_sl;
    if (!reference_line_.XYToSL(corner_xy, &corner_sl)) { ... }
    // shift box based on buffer
    // nudge decision buffer:
    // --- position for left nudge
    // --- negative for right nudge
    //nudge.distance_l()是带正负的，表示nudge的左右方向，同上
    corner_sl.set_l(corner_sl.l() + nudge.distance_l());
    sl_corners.push_back(std::move(corner_sl));
  }

  const auto corner_size = sl_corners.size();
  for (uint32_t i = 0; i < corner_size; ++i) {
    if (!MapNudgeLine(sl_corners[i], sl_corners[(i + 1) % corner_size],
                      nudge.type(), bound_map)) { ... }
  }
  return true;
}

MapNudgePolygon()中循环调用MapNudgeLine()，将障碍物的bounding box对自车轨迹的横向约束计算，转换为计算bounding box的4条边对自车轨迹的横向约束。 

//确定障碍物轮廓的一条边所对应纵向s范围内轨迹采样点的横向约束
bool QpFrenetFrame::MapNudgeLine( ... ) { 
  const common::SLPoint& near_point = (start.s() < end.s() ? start : end);
  const common::SLPoint& further_point = (start.s() < end.s() ? end : start);
  //impact_index表示evaluated_s_中受影响的区间范围，2个index可能是相等的
  std::pair<uint32_t, uint32_t> impact_index = FindInterval(near_point.s(), further_point.s());
  //s轴超出轨迹纵向范围
  if (further_point.s() < start_s_ - vehicle_param_.back_edge_to_center() ||
      near_point.s() > end_s_ + vehicle_param_.front_edge_to_center()) {
    return true;
  }

  const double distance = std::max(further_point.s() - near_point.s(), common::math::kMathEpsilon);
  const double adc_half_width = vehicle_param_.width() / 2;
  for (uint32_t i = impact_index.first; i <= impact_index.second; ++i) {
    double weight = std::fabs((evaluated_s_[i] - near_point.s())) / distance;
    weight = std::min(1.0, std::max(weight, 0.0));
    double boundary = near_point.l() * (1 - weight) + further_point.l() * weight;

    if (nudge_type == ObjectNudge::LEFT_NUDGE) {
      boundary += adc_half_width;
      //first是lower bound，second是upper bound
      //lower bound增大，即将自车可行驶区域向左移动，即left nudge
      //而upper bound不变，其初始化为INF
      (*constraint)[i].first = std::max(boundary, (*constraint)[i].first);
    } else {
      boundary -= adc_half_width;
      (*constraint)[i].second = std::min(boundary, (*constraint)[i].second);
    }

    //若可行驶区域宽度constraint太窄(<0.3m)，则收缩feasible_longitudinal_upper_bound_    
    if ((*constraint)[i].second < (*constraint)[i].first + 0.3) {
      if (i > 0) {
        feasible_longitudinal_upper_bound_ =
            std::min(evaluated_s_[i - 1] - kEpsilonTol, feasible_longitudinal_upper_bound_);
      } else {
        feasible_longitudinal_upper_bound_ = start_s_;
        return true;
      }
      ...
      break;
    }
  }
  return true;
}

MapDynamicObstacleWithDecision()也是和静态障碍物大体相似的思路。根据由SpeedData求得的自车位置信息discretized_vehicle_location_，按照时间戳一一匹配动态障碍物的轨迹点（应该是预测轨迹），计算该点处的障碍物bounding box对自车横向轨迹的影响。

bool QpFrenetFrame::MapDynamicObstacleWithDecision(const Obstacle& obstacle) {
  ...
  const auto& nudge = decision.nudge();
  for (const SpeedPoint& veh_point : discretized_vehicle_location_) {
    double time = veh_point.t();
    common::TrajectoryPoint trajectory_point = obstacle.GetPointAtTime(time);
    common::math::Box2d obs_box = obstacle.GetBoundingBox(trajectory_point);
    // project obs_box on reference line
    std::vector<common::math::Vec2d> corners;
    obs_box.GetAllCorners(&corners);
    std::vector<common::SLPoint> sl_corners;

    for (const auto& corner_xy : corners) {
      common::SLPoint cur_point;
      if (!reference_line_.XYToSL(corner_xy, &cur_point)) {        ...      }
      // shift box base on buffer
      cur_point.set_l(cur_point.l() + nudge.distance_l());
      sl_corners.push_back(std::move(cur_point));
    }

    for (uint32_t i = 0; i < sl_corners.size(); ++i) {
      common::SLPoint sl_first = sl_corners[i % sl_corners.size()];
      common::SLPoint sl_second = sl_corners[(i + 1) % sl_corners.size()];
      if (sl_first.s() < sl_second.s()) {
        std::swap(sl_first, sl_second);
      }
      //MapLateralConstraint()被调用4次，因为sl_corners由bounding box而来，有4个端点
      //因为是考虑横向约束，bounding box的上下2条横向线段端点对于求bound没有影响
      std::pair<double, double> bound = MapLateralConstraint( ... );
      // update bound map
      double s_resolution = std::fabs(veh_point.v() * time_resolution_);
      double updated_start_s = init_frenet_point_.s() + veh_point.s() - s_resolution;
      double updated_end_s = init_frenet_point_.s() + veh_point.s() + s_resolution;
      //纵向s超出考察范围
      if (updated_end_s > evaluated_s_.back() || updated_start_s < evaluated_s_.front()) {
        continue;
      }
      std::pair<uint32_t, uint32_t> update_index_range = FindInterval(updated_start_s, updated_end_s);

      for (uint32_t j = update_index_range.first; j <= update_index_range.second; ++j) {
        //可行驶区域的右边界bound，取max
        dynamic_obstacle_bound_[j].first = std::max(bound.first, dynamic_obstacle_bound_[j].first);
        //可行驶区域的左边界bound，取min
        dynamic_obstacle_bound_[j].second = std::min(bound.second, dynamic_obstacle_bound_[j].second);
      }
    }
  }
  return true;
}

MapLateralConstraint()用来计算障碍物bounding box的一条边对自车横向轨迹的约束。 

//返回值pair.first表示可行驶区域的右边界限定，second表示左边界限定，first<second
std::pair<double, double> QpFrenetFrame::MapLateralConstraint( ... ) {
  constexpr double inf = std::numeric_limits<double>::infinity();
  std::pair<double, double> result = std::make_pair(-inf, inf);
  //障碍物车在自车前方或后方，忽略，本函数只考虑横向影响
  if (start.s() > s_end || end.s() < s_start) {
    return result;
  }
  double s_front = std::max(start.s(), s_start);
  double s_back = std::min(end.s(), s_end);
  double weight_back = 0.0;
  double weight_front = 0.0;

  if (end.s() - start.s() > std::numeric_limits<double>::epsilon()) {
    weight_back = (s_back - end.s()) / (end.s() - start.s());
    weight_front = (s_front - start.s()) / (end.s() - start.s());
  }
  //将自车首尾点向障碍物车做映射，找对应点，以确定横向偏移
  //我觉得只简单的利用障碍物车的bounding box就够了
  //nudge的横向距离已经在调用该函数之前纳入障碍物车的start和end两点的l坐标了
  common::SLPoint front = common::math::InterpolateUsingLinearApproximation(
      start, end, weight_front);
  common::SLPoint back = common::math::InterpolateUsingLinearApproximation(
      start, end, weight_back);

  if (nudge_type == ObjectNudge::RIGHT_NUDGE) {
    //确认自车横向左方限定，取min使可行驶范围偏右，故nudge right
    result.second = std::min(front.l(), back.l());
  } else {
    //确认自车横向右方限定，取max使可行驶范围偏左，故nudge left
    result.first = std::max(front.l(), back.l());
  }
  return result;
}

以上都是QP的前期处理，我会继续更新文章分析接下来的AddConstraint()、AddKernel()、Solve()。敬请期待。