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

最新推荐文章于 2024-08-15 23:30:31 发布

linxigjs

最新推荐文章于 2024-08-15 23:30:31 发布

阅读量859

点赞数

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

本文链接：https://blog.csdn.net/linxigjs/article/details/103359814

版权

代码解析专栏收录该内容

25 篇文章 49 订阅

订阅专栏

本文深入解析BaiduApollo项目中EMPlanner模块的QPPathOptimizer2，重点介绍QpSplinePathGenerator::AddConstraint()方法，详细阐述了路径优化过程中添加的11种约束条件，包括多项式系数约束、起终点函数值及导数约束、各采样点导数约束等。

摘要由CSDN通过智能技术生成

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

本文是对Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 2这篇之前的博客的重新梳理。我最近又回顾了一遍代码，觉得以前的注释与排版都不太清晰，故重新发一篇，只介绍QpSplinePathGenerator::AddConstraint()。

EM Planner 中的所谓约束constraint，在底层代码上都是依托于 Spline1dConstraint 和 AffineConstraint 2个类。文件路径为 apollo\modules\planning\math\smoothing_spline\spline_1d_constraint.cc和 apollo\modules\planning\math\smoothing_spline\affine_constraint.cc。

我整理后发现，Path的优化过程一共添加了11种约束，我在注释中编码为0~10，稍后一一列出。

要规划的横向轨迹可以表示为d(s)，即横向坐标是纵向坐标的函数。则，一阶导是轨迹的tan(heading angle)；根据曲率计算公式，曲率可以用二阶导表示。

$y = f(x) \\ kappa = \frac{\mid y'' \mid}{(1+y'^{2})^{\frac{3}{2}}}$

bool QpSplinePathGenerator::AddConstraint(const QpFrenetFrame& qp_frenet_frame,
                                          const double boundary_extension) {
  ...
  constexpr double param_range = 1e-4;
  //0、添加spline多项式系数中的最高次项系数取值范围约束，不等式约束
  //不太有把握，也可能是最低次项系数
  for (int i = qp_spline_path_config_.spline_order(); i < dim;
       i += qp_spline_path_config_.spline_order() + 1) {
    Eigen::MatrixXd mat = Eigen::MatrixXd::Zero(1, dim);
    Eigen::MatrixXd bd = Eigen::MatrixXd::Zero(1, 1);
    //对5次多项式，有[0,0,0,0,0,-1] * [a, b, c, d, e, f].T >= -param_range
    //即 f <= param_range
    mat(0, i) = -1;
    bd(0, 0) = -param_range;
    spline_constraint->AddInequalityConstraint(mat, bd);
    //[0,0,0,0,0,1] * [a, b, c, d, e, f].T >= -param_range
    //即 f >= -param_range
    mat(0, i) = 1;
    bd(0, 0) = -param_range;
    spline_constraint->AddInequalityConstraint(mat, bd);
    //综合起来，限定了 -param_range <= f <= param_range
  }

  // add init status constraint, equality constraint
  //1、添加起始点处函数值范围约束，不等式约束
  //限定了min_d <= d(s) <= max_d
  const double kBoundaryEpsilon = 1e-4;
  spline_constraint->AddPointConstraintInRange(init_frenet_point_.s(),
                                               init_frenet_point_.l() - ref_l_,
                                               kBoundaryEpsilon);

  //2、添加起始点处一阶导范围约束，不等式约束
  //限定了min_d' <= d'(s) <= max_d'
  spline_constraint->AddPointDerivativeConstraintInRange(
      init_frenet_point_.s(), init_frenet_point_.dl(), kBoundaryEpsilon);

  //path二阶导其实是曲线的曲率kappa，若过U形弯速度快，则不对kappa添加约束
  if (init_trajectory_point_.v() > qp_spline_path_config_.uturn_speed_limit()) {
    //3、添加起始点处二阶导范围约束，不等式约束
    //限定了min_d'' <= d''(s) <= max_d''
    spline_constraint->AddPointSecondDerivativeConstraintInRange(
        init_frenet_point_.s(), init_frenet_point_.ddl(), kBoundaryEpsilon);
  }

  // add end point constraint, equality constraint
  //lat_shift看不懂，为什么要与ref_l_反向？
  //推测：换道时有参考线切换，自车横向坐标在目标车道参考线坐标系下是ref_l_，目标点应该在目标车道参考线上采样，
  //所以目标点在当前车道参考线坐标系下横向坐标大概是fabs(ref_l_)，而正负一定相反
  //如果不变道，ref_l_=0，如果变道，ref_l_=init_frenet_point_.l();
  double lat_shift = -ref_l_;
  if (is_change_lane_path_) {
    double lane_change_lateral_shift =
        GetLaneChangeLateralShift(init_trajectory_point_.v());
    lat_shift = std::copysign(
        std::fmin(std::fabs(ref_l_), lane_change_lateral_shift), -ref_l_);
  }

  const double kEndPointBoundaryEpsilon = 1e-2;
  constexpr double kReservedDistance = 20.0;
  const double target_s =
      std::fmin(qp_spline_path_config_.point_constraint_s_position(),
                kReservedDistance + init_frenet_point_.s() +
                    init_trajectory_point_.v() * FLAGS_look_forward_time_sec);
  //4、添加终止点处函数值范围约束，不等式约束
  //为什么此处使用target_s，而不使用evaluated_s_.back()？
  spline_constraint->AddPointConstraintInRange(target_s, lat_shift,
                                               kEndPointBoundaryEpsilon);

  //5、添加终止点处一阶导范围约束，不等式约束 
  //如果是变道，则不约束一阶导即tan(heading angle)
  //如果不是变道，则采样点的tan=0，即沿s轴
  if (!is_change_lane_path_) {
    spline_constraint->AddPointDerivativeConstraintInRange(
        evaluated_s_.back(), 0.0, kEndPointBoundaryEpsilon);
  }

  // add first derivative bound to improve lane change smoothness
  //dl_bound是0.1，arctan(0.1)=5.71度，这角度会不会太小了？
  std::vector<double> dl_lower_bound(evaluated_s_.size(), -FLAGS_dl_bound);
  std::vector<double> dl_upper_bound(evaluated_s_.size(), FLAGS_dl_bound);

  //6、添加各采样点朝向角范围约束，不等式约束
  if (!spline_constraint->AddDerivativeBoundary(evaluated_s_, dl_lower_bound,
                                                dl_upper_bound)) { ... }

  // kappa bound is based on the inequality:
  // kappa = d(phi)/ds <= d(phi)/dx = d2y/dx2，此式如何得来？
  std::vector<double> kappa_lower_bound(evaluated_s_.size(), -FLAGS_kappa_bound);
  std::vector<double> kappa_upper_bound(evaluated_s_.size(), FLAGS_kappa_bound);
  //7、添加各采样点曲率Kappa范围约束，不等式约束
  if (!spline_constraint->AddSecondDerivativeBoundary(
          evaluated_s_, kappa_lower_bound, kappa_upper_bound)) { ... }

  std::vector<double> dkappa_lower_bound(evaluated_s_.size(), -FLAGS_dkappa_bound);
  std::vector<double> dkappa_upper_bound(evaluated_s_.size(), FLAGS_dkappa_bound);
  //8、添加各采样点3阶导范围约束，不等式约束
  if (!spline_constraint->AddThirdDerivativeBoundary(
          evaluated_s_, dkappa_lower_bound, dkappa_upper_bound)) { ... }

  // add map bound constraint
  double lateral_buf = boundary_extension;
  if (is_change_lane_path_) {
    lateral_buf = qp_spline_path_config_.cross_lane_lateral_extension();
  }
  std::vector<double> boundary_low;
  std::vector<double> boundary_high;
  //综合各种横向区间约束，求取最终的横向约束
  for (uint32_t i = 0; i < evaluated_s_.size(); ++i) {
    auto road_boundary = qp_frenet_frame.GetMapBound().at(i);
    auto static_obs_boundary = qp_frenet_frame.GetStaticObstacleBound().at(i);
    auto dynamic_obs_boundary = qp_frenet_frame.GetDynamicObstacleBound().at(i);
    //不明白这种情形指什么？
    if (evaluated_s_.at(i) - evaluated_s_.front() <
        qp_spline_path_config_.cross_lane_longitudinal_extension()) {
    //扩宽边界约束
      //右边界
      road_boundary.first =
          std::fmin(road_boundary.first, init_frenet_point_.l() - lateral_buf);
      //左边界
      road_boundary.second =
          std::fmax(road_boundary.second, init_frenet_point_.l() + lateral_buf);
    }
    boundary_low.emplace_back(common::util::MaxElement(
        std::vector<double>{road_boundary.first, static_obs_boundary.first,
                            dynamic_obs_boundary.first}));
    boundary_high.emplace_back(common::util::MinElement(
        std::vector<double>{road_boundary.second, static_obs_boundary.second,
                            dynamic_obs_boundary.second}));
    ...
  }
  ...

  //不等式约束
  const double start_l = ref_l_;
  //求横向相对坐标
  std::for_each(boundary_low.begin(), boundary_low.end(),
                [start_l](double& d) { d -= start_l; });
  std::for_each(boundary_high.begin(), boundary_high.end(),
                [start_l](double& d) { d -= start_l; });
  //9、添加各采样点函数值（横向坐标）范围约束，不等式约束
  if (!spline_constraint->AddBoundary(evaluated_s_, boundary_low, boundary_high)) { ... }

  // add spline joint third derivative constraint
  //10、添加各采样点处0~3阶导连续约束，等式约束
  if (knots_.size() >= 3 &&
      !spline_constraint->AddThirdDerivativeSmoothConstraint()) { ... }
  return true;
}

0. 添加spline多项式系数中的最高次项系数取值范围约束，详见代码和注释

1. 添加起始点处函数值范围约束

bool Spline1dConstraint::AddPointConstraintInRange(const double x,
                                                   const double fx,
                                                   const double range) {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddBoundary, this, _1, _2, _3), x, fx, range);
}

AddConstraintInRange() 又调用了Spline1dConstraint::AddBoundary()。

//添加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);
}

AddBoundary()最终还是调用了 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;
}

AddBoundary() 中的 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::AddPointDerivativeConstraintInRange(
    const double x, const double dfx, const double range) {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddDerivativeBoundary, this, _1, _2, _3),
      x, dfx, range);
}

//同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() 与上面相同。

3. 添加起始点处二阶导范围约束

bool Spline1dConstraint::AddPointSecondDerivativeConstraintInRange(
    const double x, const double ddfx, const double range) {
  return AddConstraintInRange(
      std::bind(&Spline1dConstraint::AddSecondDerivativeBoundary, this, _1, _2, _3),
      x, ddfx, range);
}

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() 与上面相同。

4. 添加终止点处函数值范围约束，与1.类似

5. 添加终止点处一阶导范围约束，与2.类似

6. 添加各采样点一阶导（朝向角）范围约束，与2.类似

7. 添加各采样点曲率Kappa范围约束，与3.类似

8. 添加各采样点3阶导范围约束，与3.大同小异

bool Spline1dConstraint::AddThirdDerivativeBoundary(
    const std::vector<double>& x_coord, const std::vector<double>& lower_bound,
    const std::vector<double>& 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;
  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, 0, 0, 6, 24, 60] * [a, b, c, d, e, f].T
    // >= filtered_lower_bound
    for (uint32_t j = 3; j < num_params; ++j) {
      inequality_constraint(i, j + index * num_params) = coef * j * (j - 1) * (j - 2);
      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, 0, 0, -6, -24, -60] * [a, b, c, d, e, f].T
    // >= -filtered_upper_bound ==>
    //f'''(x) = [0, 0, 0, 6, 24, 60] * [a, b, c, d, e, f].T
    // <= filtered_upper_bound
    for (uint32_t j = 3; j < num_params; ++j) {
      inequality_constraint(i + filtered_lower_bound.size(),
                            j + index * num_params) = coef * j * (j - 1) * (j - 2);
      coef *= corrected_x;
    }
    inequality_boundary(i + filtered_lower_bound.size(), 0) = -filtered_upper_bound[i];
  }
  return inequality_constraint_.AddConstraint(inequality_constraint,inequality_boundary);
}

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

9. 添加各采样点函数值（横向坐标）范围约束，与1.类似

10. 添加各采样点处0~3阶导连续约束

此处的代码逻辑比较抽象复杂，但要实现的式子很简单，如下。

$f_k(s_k) = f_{k+1} (s_0)$

$\begin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \end{vmatrix} = \begin{vmatrix} 1 & s_{0} & s_{0}^2 & s_{0}^3 & s_{0}^4&s_{0}^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{k+1,0} \\ a_{k+1,1} \\ a_{k+1,2} \\ a_{k+1,3} \\ a_{k+1,4} \\ a_{k+1,5} \end{vmatrix}$

//添加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() 与上面相同。

添加约束到此结束。