天天看點

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

添加限制到此結束。 

繼續閱讀