天天看点

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

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

EM Planner 中的QP Speed Optimizer 和 QP Path Optimizer 大同小异,在底层代码上都是依托于 Spline1dConstraint 和 AffineConstraint 2个类。文件路径为 apollo\modules\planning\math\smoothing_spline\spline_1d_constraint.cc 和 apollo\modules\planning\math\smoothing_spline\affine_constraint.cc。我在Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 2 中已经介绍过了,这里还是原原本本的把代码贴一遍。一是细节有些微不同,二是我再读代码时有了新的理解。

首先,速度规划的调用入口在QpSplineStSpeedOptimizer::Process()中,QpSplineStSpeedOptimizer类继承自SpeedOptimizer类。主要思路仍然是对于要规划的目标曲线,按时间采样N段,每一段速度轨迹采用多项式拟合,整体需要满足设定的各项约束条件,求取代价最小的多项式参数。

Status QpSplineStSpeedOptimizer::Process(...) {  
  ...
  constexpr double kBounadryEpsilon = 1e-2;
  for (auto boundary : st_graph_data.st_boundaries()) {
    //如果自车位置在障碍物占据范围内,即发生了碰撞,速度数据为空,不规划
    if (boundary->IsPointInBoundary({0.0, 0.0}) ||
        (std::fabs(boundary->min_t()) < kBounadryEpsilon &&
         std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
      speed_data->clear();
      const std::string msg = "Collision found in QpSplineStSpeedOptimizer!";
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }
  ...
  QpSplineStGraph st_graph(
      spline_solver_.get(), st_graph_data.path_length_by_conf(),
      st_graph_data.total_time_by_conf(), qp_st_speed_config_, veh_param,
      reference_line_info_->IsChangeLanePath());

  std::pair<double, double> accel_bound = {
      qp_st_speed_config_.preferred_min_deceleration(),
      qp_st_speed_config_.preferred_max_acceleration()};

  //速度规划进行了3次尝试,第2次相比第1次,放宽了加速度的上下限范围。
  //第3次使用了QpPiecewiseStGraph
  auto ret = st_graph.Search(st_graph_data, accel_bound, reference_speed_data,
                             speed_data);
  if (ret != Status::OK()) {
    ...
    accel_bound.first = qp_st_speed_config_.min_deceleration();
    accel_bound.second = qp_st_speed_config_.max_acceleration();
    ret = st_graph.Search(st_graph_data, accel_bound, reference_speed_data,
                          speed_data);

    // backup plan: use piecewise_st_graph
    if (ret != Status::OK()) {
      ...
      QpPiecewiseStGraph piecewise_st_graph(...);
      ret = piecewise_st_graph.Search(st_graph_data, speed_data, accel_bound);
      ...
    }
  }
  ...
}
           

规划的主体过程在QpSplineStGraph::Search()中,分为4个步骤:AddConstraint(),AddKernel(),Solve(),extract speed data。

Status QpSplineStGraph::Search(...) {
  ...
  // reset spline generator
  spline_solver_->Reset(t_knots_, qp_st_speed_config_.qp_spline_config().spline_order());

  if (!AddConstraint(st_graph_data.init_point(), st_graph_data.speed_limit(),
                     st_graph_data.st_boundaries(), accel_bound).ok()) { ... }
  if (!AddKernel(st_graph_data.st_boundaries(), st_graph_data.speed_limit()).ok()) { ...}
  if (!Solve().ok()) { ... }
  ...
  // extract output
  speed_data->clear();
  const Spline1d& spline = spline_solver_->spline();

  const double t_output_resolution = FLAGS_trajectory_time_min_interval;
  double time = 0.0;
  while (time < total_time_ + t_output_resolution) {
    double s = spline(time);
    double v = std::max(0.0, spline.Derivative(time));
    double a = spline.SecondOrderDerivative(time);
    double da = spline.ThirdOrderDerivative(time);
    speed_data->AppendSpeedPoint(s, time, v, a, da);
    time += t_output_resolution;
  }

  return Status::OK();
}
           

本文首先介绍AddConstraint() 部分。文件路径:apollo\modules\planning\tasks\optimizers\qp_spline_st_speed\qp_spline_st_graph.cc

Status QpSplineStGraph::AddConstraint(...) {
  //添加等式约束f(0) = s0 = 0
  if (!constraint->AddPointConstraint(0.0, 0.0)) { ... }

  //添加等式约束f'(0) = v0 = init_point_.v()
  if (!constraint->AddPointDerivativeConstraint(0.0, init_point_.v())) { ... }

  // monotone constraint 
  //单调性不等式约束,即随t增大,s(t)一定是不变或增大,因为车会停下或前进,不后退
  if (!constraint->AddMonotoneInequalityConstraint(t_evaluated_)) { ... }

  // smoothness constraint
  //添加joint points处0~3阶导连续 等式约束,函数名比较迷惑,不是只有3阶导
  if (!constraint->AddThirdDerivativeSmoothConstraint()) { ... }

  // boundary constraint
  std::vector<double> s_upper_bound;
  std::vector<double> s_lower_bound;

  //求不同时刻t,自车s应该处的范围,每一个t对应一个[lower_s, upper_s]
  for (const double curr_t : t_evaluated_) {
    double lower_s = 0.0;
    double upper_s = 0.0;
    GetSConstraintByTime(boundaries, curr_t, total_path_length_, &upper_s, &lower_s);
    s_upper_bound.push_back(upper_s);
    s_lower_bound.push_back(lower_s);    
  }

  //添加lower_s <= f(t) <= upper_s取值范围约束
  if (!constraint->AddBoundary(t_evaluated_, s_lower_bound, s_upper_bound)) { ... }

  // speed constraint
  std::vector<double> speed_upper_bound;
  if (!EstimateSpeedUpperBound(init_point, speed_limit, &speed_upper_bound).ok()) { ... }
  ...

  //添加speed_lower_bound <= f'(t) <= speed_upper_bound取值范围约束
  if (!constraint->AddDerivativeBoundary(t_evaluated_, speed_lower_bound, 
                                         speed_upper_bound)) { ... }
  
  // acceleration constraint
  std::vector<double> accel_lower_bound(t_evaluated_.size(), accel_bound.first);
  std::vector<double> accel_upper_bound(t_evaluated_.size(), accel_bound.second);

  bool has_follow = false;
  double delta_s = 1.0;
  for (const auto* boundary : boundaries) {
    if (boundary->boundary_type() == STBoundary::BoundaryType::FOLLOW) {
      has_follow = true;
      delta_s = std::fmin(
          delta_s, boundary->min_s() - fabs(boundary->characteristic_length()));
    }
  }
  //如果在跟车(即前方有障碍物)且距前车很近,设定第1个时间间隔内加速度上限为0
  if (FLAGS_enable_follow_accel_constraint && has_follow && delta_s < 0.0) {
    accel_upper_bound.front() = 0.0;
  }

  //添加accel_lower_bound <= f''(t) <= accel_upper_bound取值范围约束
  if (!constraint->AddSecondDerivativeBoundary(t_evaluated_, accel_lower_bound, 
                                               accel_upper_bound)) { ... }  
  return Status::OK();
}
           

为描述方便,设要规划的速度曲线方程为s(t) = f(t),那么s'(t) = f'(t) 就是速度。

AddConstraint() 中主要添加了如下约束:

  1. 起始点函数值约束 f(0) = 0。等式约束。
  2. 起始点速度约束 f'(0) = v0 = 初始速度。等式约束。
  3. 单调性约束,即随时间t 增大,s = f(t) 一定是不变或增大(单调递增),因为车会停下或前进,不后退。等式约束。
  4. 分界点处各阶导数连续平滑性约束,这里用了0~3阶导数。等式约束。
  5. 采样时刻t 对应的自车s 坐标范围约束。不等式约束。
  6. 采样时刻t 对应的自车速度范围约束。不等式约束。
  7. 采样时刻t 对应的自车加速度范围约束。不等式约束。

如何调用我已经在代码中做了注释。接下来咱们一一拆解。

1 起始点函数值约束

bool Spline1dConstraint::AddPointConstraint(const double x, const double fx) {
  //x应该是整条曲线上的坐标,而不是某seg上的相对坐标
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  //多项式参数个数
  const uint32_t num_params = spline_order_ + 1;
  //power_x是(相对x)的0~spline_order次方
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 0; i < num_params; ++i) {
    equality_constraint(0, index_offset + i) = power_x[i];
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = fx;
  //添加在某一点的函数值相等约束,对5次多项式曲线,如下
  //[1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T = fx
  //前者是power_x,中间是5次curve的系数,后者是该点处的函数值fx
  //为了引用矩阵计算工具,这里都转换成了矩阵形式
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}
           
bool Spline1dConstraint::AddEqualityConstraint(
    const Eigen::MatrixXd& constraint_matrix,
    const Eigen::MatrixXd& constraint_boundary) {
  return equality_constraint_.AddConstraint(constraint_matrix, constraint_boundary);
}
           

 Spline1dConstraint::AddEqualityConstraint() 最终还是调用了 AffineConstraint::AddConstraint()。

bool AffineConstraint::AddConstraint(
    const Eigen::MatrixXd& constraint_matrix,
    const Eigen::MatrixXd& constraint_boundary) {
  //假设添加的约束是Ax=b,constraint_matrix就是A,对5次多项式就是[1,p,p^2,p^3,p^4,p^5],
  //p是多项式曲线上某点的横坐标。constraint_boundary就是b,即函数值。x就是未知的多项式系数
  //自然A的一行对应的Ax的计算结果就是b的一行,所以A.rows==b.rows且,b.cols==1
  if (constraint_matrix.rows() != constraint_boundary.rows()) { ... }

  if (constraint_matrix_.rows() == 0) {
    constraint_matrix_ = constraint_matrix;
    constraint_boundary_ = constraint_boundary;
    return true;
  }
  if (constraint_matrix_.cols() != constraint_matrix.cols()) { ... }
  if (constraint_boundary.cols() != 1) { ... }

  Eigen::MatrixXd n_matrix(constraint_matrix_.rows() + constraint_matrix.rows(),
                           constraint_matrix_.cols());
  Eigen::MatrixXd n_boundary(
      constraint_boundary_.rows() + constraint_boundary.rows(), 1);

  n_matrix << constraint_matrix_, constraint_matrix;
  n_boundary << constraint_boundary_, constraint_boundary;
  constraint_matrix_ = n_matrix;
  constraint_boundary_ = n_boundary;
  return true;
}
           

FindIndex()用来寻找x 在分界点中的下标,以确定其所在的spline seg段。 

uint32_t Spline1dConstraint::FindIndex(const double x) const {
  //upper_bound(起始地址,结束地址,要查找的数值) 返回的是数值最后一个出现的位置。
  //即返回“元素值>查找值”的第一个元素的位置
  //因为x_knots_[0]=0,所以可以跳过,从x_knots_[1]开始查
  auto upper_bound = std::upper_bound(x_knots_.begin() + 1, x_knots_.end(), x);
  return std::min(static_cast<uint32_t>(x_knots_.size() - 1),
                  static_cast<uint32_t>(upper_bound - x_knots_.begin())) - 1;
  //最后-1,FindIndex()返回的是第一个比x大的元素的前一个元素下标index
  //即入参x位于x_knots_[index]~x_knots_[index+1]之间
}
           

2 起始点速度约束

bool Spline1dConstraint::AddPointDerivativeConstraint(const double x, const double dfx) {
  uint32_t index = FindIndex(x);
  std::vector<double> power_x;
  const uint32_t num_params = spline_order_ + 1;
  GeneratePowerX(x - x_knots_[index], num_params, &power_x);
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(1, (x_knots_.size() - 1) * num_params);
  uint32_t index_offset = index * num_params;
  for (uint32_t i = 1; i < num_params; ++i) {
    //该行矩阵中对应该spline seg是[0, 1, 2*x, 3*x^2, 4*x^3, 5*x^4]
    //注意equality_constraint的长度并不是6,此处只分析某一段的长度是6
    //注意equality_constraint被初始化为元素全为0的行矩阵
    equality_constraint(0, index_offset + i) = power_x[i - 1] * i;
  }
  Eigen::MatrixXd equality_boundary(1, 1);
  equality_boundary(0, 0) = dfx;
  //添加在某一点的一阶导数相等约束,对5次多项式曲线,如下
  //[0, 1, 2*x, 3*x^2, 4*x^3, 5*x^4] * [a, b, c, d, e, f].T = dfx
  //中间是5次curve的系数,后者是该点处的一阶导数值dfx
  return AddEqualityConstraint(equality_constraint, equality_boundary);
}
           

最后一行调用的AddEqualityConstraint() 与上面相同。

3 单调性约束(单调递增)

bool Spline1dConstraint::AddMonotoneInequalityConstraint(
    const std::vector<double>& x_coord) {
  ...
  const uint32_t num_params = spline_order_ + 1;
  //注意维度,有N个x,就有N-1个不等式f(xn-1) <= f(xn)
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      x_coord.size() - 1, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(x_coord.size() - 1, 1);

  uint32_t prev_spline_index = FindIndex(x_coord[0]);
  double prev_rel_x = x_coord[0] - x_knots_[prev_spline_index];
  std::vector<double> prev_coef;
  GeneratePowerX(prev_rel_x, num_params, &prev_coef);
  for (uint32_t i = 1; i < x_coord.size(); ++i) {
    uint32_t cur_spline_index = FindIndex(x_coord[i]);
    double cur_rel_x = x_coord[i] - x_knots_[cur_spline_index];
    std::vector<double> cur_coef;

    GeneratePowerX(cur_rel_x, num_params, &cur_coef);
    // if constraint on the same spline
    if (cur_spline_index == prev_spline_index) {
      for (uint32_t j = 0; j < cur_coef.size(); ++j) {
        //在同一段spline,则多项式系数是相同的。
        //记cur_rel_x-prev_rel_x=d, inequality_constraint对应该spline seg是
        //[1-1, d, d^2, d^3, d^4, d^5] * [a, b, c, d, e, f].T >= 0
        inequality_constraint(i - 1, cur_spline_index * num_params + j) =
            cur_coef[j] - prev_coef[j];
      }
    } else {
      // if not on the same spline
      for (uint32_t j = 0; j < cur_coef.size(); ++j) {
        //inequality_constraint对应相邻2个spline seg是
        //[-1, -px, -px^2, -px^3, -px^4, -px^5, 1, cx, cx^2, cx^3, cx^4, cx^5]
        // * [pa, pb, pc, pd, pe, pf, ca, cb, cc, cd, ce, cf].T >= 0
        inequality_constraint(i - 1, prev_spline_index * num_params + j) = -prev_coef[j];
        inequality_constraint(i - 1, cur_spline_index * num_params + j) = cur_coef[j];
      }
    }
    prev_coef = cur_coef;
    prev_spline_index = cur_spline_index;
  }

  return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary);
}
           

 最后一行调用的AddConstraint() 与上面相同。

4 分界点处各阶导数连续平滑性约束

//添加joint points处0~3阶导连续 等式约束,即
//fl(xl)=fr(xr); fl'(xl)=fr'(xr); fl''(xl)=fr''(xr); fl'''(xl)=fr'''(xr),转换为QP的形式,即
//fl(xl)-fr(xr)=0; fl'(xl)-fr'(xr)=0; fl''(xl)-fr''(xr)=0; fl'''(xl)-fr'''(xr)=0
//fl()和fr()是相邻的2段curve polynomial,xl和xr是同一个点在不同的段不同的起点下的相对坐标
bool Spline1dConstraint::AddThirdDerivativeSmoothConstraint() {
  //如果只有一段spline,就没有joint point了,也没有平滑一说了
  if (x_knots_.size() < 3) {
    return false;
  }

  const uint32_t n_constraint =
      (static_cast<uint32_t>(x_knots_.size()) - 2) * 4; //中间连接点的个数,4代表0~3阶导都连续
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd equality_constraint =
      Eigen::MatrixXd::Zero(n_constraint, (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd equality_boundary = Eigen::MatrixXd::Zero(n_constraint, 1);

  //这里的2层for循环很不直观,其实是把不同阶导数、不同的joint point、不同的系数杂糅在一起计算了
  //虽高效,但太复杂抽象了,最终计算后应该是下面的形式
  //每行左边6项是joint point左侧的curve系数,右边6项是joint point右侧的curve系数
  //0~3行是1个joint point的0~3阶导,每4行表示一个点
  // | 1 xl xl^2 xl^3  xl^4   xl^5   -1 -xr -xr^2 -xr^3  -xr^4   -xr^5   |   | al | = | 0 |
  // | 0 1  2xl  3xl^2 4xl^3  5xl^4   0 -1  -2xr  -3xr^2 -4xr^3  -5xr^4  |   | bl | = | 0 |
  // | 0 0  2    6xl   12xl^2 20xl^3  0  0  -2    -6xr   -12xr^2 -20xr^3 | * | cl | = | 0 |
  // | 0 0  0    6     24xl   60xl^2  0  0   0    -6     -24xr   -60xr^2 |   | dl | = | 0 |
  // | . .  .    .     .      .       .  .   .     .      .       .      |   | el | = | 0 |
  // | . .  .    .     .      .       .  .   .     .      .       .      |   | fl | = | 0 |
  //                                                                         | ar | = | 0 |
  //                                                                         | br | = | 0 |
  //                                                                         | cr | = | 0 |
  //                                                                         | dr | = | 0 |
  //                                                                         | er | = | 0 |
  //                                                                         | fr | = | 0 |

  //i循环x_knots_.size())-2次
  for (uint32_t i = 0; i < n_constraint; i += 4) {
    double left_coef = 1.0;
    double right_coef = -1.0;
    double left_dcoef = 1.0;
    double right_dcoef = -1.0;
    double left_ddcoef = 1.0;
    double right_ddcoef = -1.0;
    double left_dddcoef = 1.0;
    double right_dddcoef = -1.0;

    //第n段的最后一个点相对于第n段的第一点的坐标
    const double x_left = x_knots_[i / 4 + 1] - x_knots_[i / 4];
    //第n段的最后一个点相对于第n+1段的第一点的坐标
    const double x_right = 0.0;
    //j循环6次
    for (uint32_t j = 0; j < num_params; ++j) {
      equality_constraint(i, num_params * i / 4 + j) = left_coef;
      equality_constraint(i, num_params * (i / 4 + 1) + j) = right_coef;

      if (j >= 3) {
        equality_constraint(i + 3, num_params * i / 4 + j) =
            left_dddcoef * j * (j - 1) * (j - 2);
        equality_constraint(i + 3, num_params * (i / 4 + 1) + j) =
            right_dddcoef * j * (j - 1) * (j - 2);
        left_dddcoef = left_ddcoef;
        right_dddcoef = right_ddcoef;
      }

      if (j >= 2) {
        equality_constraint(i + 2, num_params * i / 4 + j) = left_ddcoef * j * (j - 1);
        equality_constraint(i + 2, num_params * (i / 4 + 1) + j) =
            right_ddcoef * j * (j - 1);
        left_ddcoef = left_dcoef;
        right_ddcoef = right_dcoef;
      }

      if (j >= 1) {
        equality_constraint(i + 1, num_params * i / 4 + j) = left_dcoef * j;
        equality_constraint(i + 1, num_params * (i / 4 + 1) + j) = right_dcoef * j;
        left_dcoef = left_coef;
        right_dcoef = right_coef;
      }

      left_coef *= x_left;
      right_coef *= x_right;
    }
  }
  return equality_constraint_.AddConstraint(equality_constraint, equality_boundary);
}
           

最后一行调用的AddConstraint() 与上面相同。 

5 采样时刻t 对应的自车s 坐标范围约束

//根据在time时刻各个boundary的类别,确定s的上界或下界
Status QpSplineStGraph::GetSConstraintByTime(...) const {
  *s_upper_bound = total_path_s;

  for (const STBoundary* boundary : boundaries) {
    double s_upper = 0.0;
    double s_lower = 0.0;

    if (!boundary->GetUnblockSRange(time, &s_upper, &s_lower)) { ... }
    //如果boundary的类型是如下3种,则寻找s的上界,s上界之上可以理解为有障碍物
    //如follow场景,我们只在意s的上界
    if (boundary->boundary_type() == STBoundary::BoundaryType::STOP ||
        boundary->boundary_type() == STBoundary::BoundaryType::FOLLOW ||
        boundary->boundary_type() == STBoundary::BoundaryType::YIELD) {
      *s_upper_bound = std::fmin(*s_upper_bound, s_upper);
    } else if (boundary->boundary_type() == STBoundary::BoundaryType::OVERTAKE) {
      //如果boundary的类型是OVERTAKE超车,则寻找s的下界,s下界之下可以理解为有障碍物
      //即自车要超过障碍物,则自车的s一定要大于障碍物的boundary的upper
      *s_lower_bound = std::fmax(*s_lower_bound, s_lower);
    } else { ... }
  }

  return Status::OK();
}
           

在添加约束前,先求在各采样时刻自车可能达到的s 范围,如上。

//添加lower_bound <= f(x_coord) <= upper_bound取值范围约束
bool Spline1dConstraint::AddBoundary(...) {
  ...
  //检验lower_bound和upper_bound中的元素值是否是有效值
  //将一一对应的x_coord -> lower_bound存入filtered_lower_bound_x和filtered_lower_bound
  //将一一对应的x_coord -> upper_bound存入filtered_upper_bound_x和filtered_upper_bound
  if (!FilterConstraints(x_coord, lower_bound, upper_bound,
                         &filtered_lower_bound_x, &filtered_lower_bound,
                         &filtered_upper_bound_x, &filtered_upper_bound)) { ... }
  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  //inequality_constraint矩阵中绝大多数元素都是0,只有x_coord所在的一段curve对应元素有值
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),  //行,即不等式个数
      (x_knots_.size() - 1) * num_params);                        //列
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);

  //由AddPointConstraintInRange()推来,其实就是已知val=f(x0),range>0,
  //x_coord=filtered_lower_bound_x=filtered_upper_bound_x,
  //filtered_lower_bound=lower_bound=val-range,
  //filtered_upper_bound=upper_bound=val+range,
  //由val-range < val < val+range ==>
  //f(filtered_lower_bound_x) >= filtered_lower_bound
  //且 f(filtered_upper_bound_x) <= filtered_upper_bound

  //这个for循环对应f(filtered_lower_bound_x) >= filtered_lower_bound的情况,
  //恰好符合QP中Ax>=b的形式    
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    //FindIndex()返回x_knots_中第一个大于入参的元素的前一个位置,
    //即入参位于x_knots_[index]~x_knots_[index+1]之间
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
    //corrected_x是每一段的相对起点的x坐标
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    //j=0~num_params,依次是corrected_x的0~num_params次项
    //[1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T >= lower_bound
    for (uint32_t j = 0; j < num_params; ++j) {
      //对特定行、特定curve的参数列更新,因为给定一个s,其根据定义域只对应一段curve
      inequality_constraint(i, j + index * num_params) = coef;    
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }

  //这个for循环对应f(filtered_upper_bound_x) <= filtered_upper_bound的情况,
  //为符合QP中Ax>=b的形式,将Ax<=b ==> -Ax>=-b,故coef赋初值-1,与上部分比,全为负数
  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    //-f(x) = [-1, -x, -x^2, -x^3, -x^4, -x^5] * [a, b, c, d, e, f].T
    // >= -upper_bound ==>
    //f(x) = [1, x, x^2, x^3, x^4, x^5] * [a, b, c, d, e, f].T
    // <= upper_bound   
    for (uint32_t j = 0; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),  //行
                            j + index * num_params) = coef;   //列
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) =   //行变化
        -filtered_upper_bound[i];
  }

  return inequality_constraint_.AddConstraint(inequality_constraint, nequality_boundary);
}
           

 最后一行调用的AddConstraint() 与上面相同。 

6 采样时刻t 对应的自车速度范围约束

//查找针对每个采样时刻t的速度上限
Status QpSplineStGraph::EstimateSpeedUpperBound(...) const {  
  // use v to estimate position: not accurate, but feasible in cyclic
  // processing. We can do the following process multiple times and use
  // previous cycle's results for better estimation.
  const double v = init_point.v();
  auto last_speed_data = GetHistorySpeed();
  //看不懂这个if-else???
  if(static_cast<double>(t_evaluated_.size() + speed_limit.speed_limit_points().size())
     < static_cast<double>(t_evaluated_.size()) * std::log(
              static_cast<double>(speed_limit.speed_limit_points().size()))) {
    uint32_t i = 0;
    uint32_t j = 0;
    while (i < t_evaluated_.size() && j + 1 < speed_limit.speed_limit_points().size()) {
      //2种不同的方式粗略估计时刻t可能会达到的s,一种是根据初始速度,一种是根据上一次规划的速度
      double distance = v * t_evaluated_[i];
      if (!last_speed_data.empty() && distance < last_speed_data.back().s()) {
        SpeedPoint p;
        last_speed_data.EvaluateByTime(t_evaluated_[i], &p);
        distance = p.s();
      }
      //根据估算的s 求匹配的speed limit
      ...
    } ...
  } else { ... }
  //如果正在变道,略增大速度上限(0.05)
  if (is_change_lane_) {
    for (uint32_t k = 0; k < t_evaluated_.size(); ++k) {
      speed_upper_bound->at(k) *= (1.0 + FLAGS_change_lane_speed_relax_percentage);
    }
  }

  const double kTimeBuffer = 1.0;
  const double kSpeedBuffer = 0.1;
  //提高前1s速度上限
  for (uint32_t k = 0; k < t_evaluated_.size() && t_evaluated_[k] < kTimeBuffer; ++k) {
    speed_upper_bound->at(k) =
        std::fmax(init_point_.v() + kSpeedBuffer, speed_upper_bound->at(k));
  }
  return Status::OK();
}
           

在添加约束前,先求在各采样时刻自车可能达到的速度范围,如上。 

//同AddBoundary()
bool Spline1dConstraint::AddDerivativeBoundary(...) {
  ...
  if (!FilterConstraints(...)) { ... }

  // emplace affine constraints
  const uint32_t num_params = spline_order_ + 1;
  Eigen::MatrixXd inequality_constraint = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(),  //行
      (x_knots_.size() - 1) * num_params);
  Eigen::MatrixXd inequality_boundary = Eigen::MatrixXd::Zero(
      filtered_upper_bound.size() + filtered_lower_bound.size(), 1);

  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_lower_bound_x[i]);
    const double corrected_x = filtered_lower_bound_x[i] - x_knots_[index];
    double coef = 1.0;
    //f'(x) = [0, 1, 2x, 3x^2, 4x^3, 5x^4] * [a, b, c, d, e, f].T
    // >= filtered_lower_bound    
    for (uint32_t j = 1; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef * j;
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }

  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    uint32_t index = FindIndex(filtered_upper_bound_x[i]);
    const double corrected_x = filtered_upper_bound_x[i] - x_knots_[index];
    double coef = -1.0;
    //-f'(x) = [0, -1, -2x, -3x^2, -4x^3, -5x^4] * [a, b, c, d, e, f].T
    // >= -filtered_upper_bound ==>
    //f'(x) = [0, 1, 2x, 3x^2, 4x^3, 5x^4] * [a, b, c, d, e, f].T
    // <= filtered_upper_bound
    for (uint32_t j = 1; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef * j;
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) = -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary);
}
           

最后一行调用的AddConstraint() 与上面相同。 

7 采样时刻t 对应的自车加速度范围约束

与6基本一致,这里简单带过。

bool Spline1dConstraint::AddSecondDerivativeBoundary(...) {
  ...
  if (!FilterConstraints(...)) { ... }
  ...
  for (uint32_t i = 0; i < filtered_lower_bound.size(); ++i) {
    ...
    //f''(x) = [0, 0, 2, 6x, 12x^2, 20x^3] * [a, b, c, d, e, f].T
    // >= filtered_lower_bound
    for (uint32_t j = 2; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef * j * (j - 1);
      coef *= corrected_x;
    }
    inequality_boundary(i, 0) = filtered_lower_bound[i];
  }

  for (uint32_t i = 0; i < filtered_upper_bound.size(); ++i) {
    ...
    //-f''(x) = [0, 0, -2, -6x, -12x^2, -20x^3] * [a, b, c, d, e, f].T
    // >= -filtered_upper_bound ==>
    //f''(x) = [0, 0, 2, 6x, 12x^2, 20x^3] * [a, b, c, d, e, f].T
    // <= filtered_upper_bound
    for (uint32_t j = 2; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef * j * (j - 1);
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) = -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary);
}
           

最后一行调用的AddConstraint() 与上面相同。 

添加约束到此结束。 

继续阅读