无人驾驶算法——Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑

Date: 2020/12/15
Editor:萧潇子（Jesse）
Contact: 1223167600@qq.com

Apollo 参考线平滑类

Apollo主要的参考线平滑类有三个：QpSplineReferenceLineSmoother、SpiralReferenceLineSmoother和DiscretePointsReferenceLineSmoother。

reference_line_provider.cc

ReferenceLineProvider::ReferenceLineProvider(
    const common::VehicleStateProvider *vehicle_state_provider,
    const hdmap::HDMap *base_map,
    const std::shared_ptr<relative_map::MapMsg> &relative_map)
    : vehicle_state_provider_(vehicle_state_provider) {
  if (!FLAGS_use_navigation_mode) {
    pnc_map_ = std::make_unique<hdmap::PncMap>(base_map);
    relative_map_ = nullptr;
  } else {
    pnc_map_ = nullptr;
    relative_map_ = relative_map;
  }

  ACHECK(cyber::common::GetProtoFromFile(FLAGS_smoother_config_filename,
                                         &smoother_config_))
      << "Failed to load smoother config file "
      << FLAGS_smoother_config_filename;
  //参考线平滑的几种方式
  if (smoother_config_.has_qp_spline()) {
    smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));
  } else if (smoother_config_.has_spiral()) {
    smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));
  } else if (smoother_config_.has_discrete_points()) {
  //散点平滑
    smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_));
  } else {
    ACHECK(false) << "unknown smoother config "
                  << smoother_config_.DebugString();
  }
  is_initialized_ = true;
}

其中DiscretePointsReferenceLineSmoother中包含了两个方法：CosThetaSmoother和FemPosDeviationSmoother。本文主要介绍散点平滑的建模过程（代码中公式的物理意义以及推导）

---

代价函数

cos_theta_ipopt_interface.cc

以$n$个点为例子$P_0,P_1,P_2,P_3,\cdots ,P_n$

初始化点

//初始化点
bool CosThetaIpoptInterface::get_starting_point(int n, bool init_x, double* x,
                                                bool init_z, double* z_L,
                                                double* z_U, int m,
                                                bool init_lambda,
                                                double* lambda) {
  CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
  std::random_device rd;
  std::default_random_engine gen = std::default_random_engine(rd());
  std::normal_distribution<> dis{0, 0.05};
  //在原始参考点基础上添加随机数初始化点
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i << 1;
    x[index] = ref_points_[i].first + dis(gen);
    x[index + 1] = ref_points_[i].second + dis(gen);
  }
  return true;
}

代价函数包含3部分

//代价函数包含2部分
bool CosThetaIpoptInterface::eval_f(int n, const double* x, bool new_x,
                                    double& obj_value) {
  CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
  if (use_automatic_differentiation_) {
    eval_obj(n, x, &obj_value);
    return true;
  }
//第1部分——与参考线的距离
  obj_value = 0.0;
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i << 1;
    obj_value +=
        (x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
        (x[index + 1] - ref_points_[i].second) *
            (x[index + 1] - ref_points_[i].second);
  }

第1部分——参考点距离代价
$$ob{j}_{value}1=\sum _{i=0}^{n-1}(x_i-x_{i-ref}{)}^2+(y_i-y_{i-ref}{)}^2$$

//第2部分——cos_theta 
for (size_t i = 0; i < num_of_points_ - 2; i++) {
    size_t findex = i << 1;
    size_t mindex = findex + 2;
    size_t lindex = mindex + 2;
    obj_value -=
        weight_cos_included_angle_ *
        (((x[mindex] - x[findex]) * (x[lindex] - x[mindex])) +
         ((x[mindex + 1] - x[findex + 1]) * (x[lindex + 1] - x[mindex + 1]))) /
        std::sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
                  (x[mindex + 1] - x[findex + 1]) *
                      (x[mindex + 1] - x[findex + 1])) /
        std::sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
                  (x[lindex + 1] - x[mindex + 1]) *
                      (x[lindex + 1] - x[mindex + 1]));
  }
  }

第2部分——平滑性代价
$$ob{j}_{value}2=-\sum _{i=0}^{n-2}\frac{(x_{i+1}-x_i)\times (x_{i+2}-x_{i+1})+(y_{i+1}-y_i)\times (y_{i+2}-y_{i+1})}{\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}\sqrt{(x_{i+2}-x_{i+1}{)}^2+(y_{i+2}-y_{i+1}{)}^2}}$$

//第3部分——总长度
  for (size_t i = 0; i < num_of_points_ - 1; ++i) {
    size_t findex = i << 1;
    size_t nindex = findex + 2;
    *obj_value +=
        weight_length_ *
        ((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
         (x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
  }

  return true;
}

第3部分——总长度代价
$$ob{j}_{value}3=\sum _{i=0}^{n-1}(x_i-x_{i+1}{)}^2+(y_i-y_{i+1}{)}^2$$

总代价函数：
$$ob{j}_{value}=w_1\times ob{j}_{value}1+w_2\times ob{j}_{value}2+w_3\times ob{j}_{value}3$$
这里解释一下第2部分代价函数代码中的公式的意义：

cos_theta方法： = 前面是 - 号

连续3点 $P_0,P_1,P_2$ 如上图所示，其中向量$\overrightarrow{P_0{P}_1}=(x_1-x_0,y_1-y_0)$和$\overrightarrow{P_1{P}_2}=(x_2-x_1,y_2-y_1)$之间的夹角$\theta$
$$\begin{array}{rl}cos\theta & =\frac{\overrightarrow{P_0{P}_1}\cdot \overrightarrow{P_1{P}_2}}{|\overrightarrow{P_0{P}_1}||\overrightarrow{P_1{P}_2}|}\\ & =(x_1-x_0)\times (x_2-x_1)+(y_1-y_0)\times (y_2-y_1)/\sqrt{(x_1-x_0{)}^2+(y_1-y_0{)}^2}/\sqrt{(x_2-x_1{)}^2+(y_2-y_1{)}^2}\end{array}$$
$cos\theta$值越大，$\theta$越小，$P_0,P_1,P_2$ 越接近直线，曲线越平滑

---

fem_pos_deviation_ipopt_interface.cc

代价函数：

以$n$个点为例子$P_0,P_1,P_2,P_3,\cdots ,P_n$

/** Template to return the objective value */
template <class T>
bool FemPosDeviationIpoptInterface::eval_obj(int n, const T* x, T* obj_value) {
  *obj_value = 0.0;

  // 第1部分——参考点距离代价
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i * 2;
    *obj_value +=
        weight_ref_deviation_ *
        ((x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
         (x[index + 1] - ref_points_[i].second) *
             (x[index + 1] - ref_points_[i].second));
  }





  return true;
}

第1部分——参考点距离代价
$$ob{j}_{value}1=\sum _{i=0}^{n-1}(x_i-x_{i-ref}{)}^2+(y_i-y_{i-ref}{)}^2$$

  // 第2部分——平滑性代价
  for (size_t i = 0; i + 2 < num_of_points_; ++i) {
    size_t findex = i * 2;
    size_t mindex = findex + 2;
    size_t lindex = mindex + 2;

    *obj_value += weight_fem_pos_deviation_ *
                  (((x[findex] + x[lindex]) - 2.0 * x[mindex]) *
                       ((x[findex] + x[lindex]) - 2.0 * x[mindex]) +
                   ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]) *
                       ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]));
  }

第2部分——平滑性代价
$$ob{j}_{value}2=\sum _{i=0}^{n-2}(x_i+x_{i+2}-2\times x_{i+1}{)}^2+(y_i+y_{i+2}-2\times y_{i+1}{)}^2$$

  // 第3部分——总长度代价
  for (size_t i = 0; i + 1 < num_of_points_; ++i) {
    size_t findex = i * 2;
    size_t nindex = findex + 2;
    *obj_value +=
        weight_path_length_ *
        ((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
         (x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
  }

第3部分——总长度代价
$$ob{j}_{value}3=\sum _{i=0}^{n-1}(x_i-x_{i+1}{)}^2+(y_i-y_{i+1}{)}^2$$

  // 第4部分——对于松弛变量的惩罚
  for (size_t i = slack_var_start_index_; i < slack_var_end_index_; ++i) {
    *obj_value += weight_curvature_constraint_slack_var_ * x[i];
  }

第4部分——对于松弛变量的惩罚也是常规做法，可以在后面约束的部分看到松弛变量的意义。

总代价函数：
$$ob{j}_{value}=w_1\times ob{j}_{value}1+w_2\times ob{j}_{value}2+w_3\times ob{j}_{value}3$$
这里解释一下第2部分代价函数代码中的公式的意义：

Fem_pos_deviation方法：= 前面是 + 号
$$(x_i+x_{i+2}-2\times x_{i+1}{)}^2+(y_i+y_{i+2}-2\times y_{i+1}{)}^2$$

其中上式的物理意义如上图中$\overrightarrow{P_{1}P’}$向量的模的平方，它可以理解为：向量$\overrightarrow{P_1{P}_0}$和 向量$\overrightarrow{P_1{P}_2}$相加结果新的向量$\overrightarrow{P_{1}P’}$的模平方。如果这三个点在一条直线上，那么这个值最小；三个点组成的两个线段的夹角$\theta$越大，即曲线越平滑。

---

约束

约束条件：

/** Template to compute contraints */
template <class T>
bool FemPosDeviationIpoptInterface::eval_constraints(int n, const T* x, int m,
                                                     T* g) {
  // a. 位置约束
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i * 2;
    g[index] = x[index];
    g[index + 1] = x[index + 1];
  }

  // b. 曲率约束
  for (size_t i = 0; i + 2 < num_of_points_; ++i) {
    size_t findex = i * 2;
    size_t mindex = findex + 2;
    size_t lindex = mindex + 2;
    g[curvature_constr_start_index_ + i] =
        (((x[findex] + x[lindex]) - 2.0 * x[mindex]) *
             ((x[findex] + x[lindex]) - 2.0 * x[mindex]) +
         ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]) *
             ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1])) -
        x[slack_var_start_index_ + i];
  }

  // c. 松弛变量的约束
  size_t slack_var_index = 0;
  for (size_t i = slack_constr_start_index_; i < slack_constr_end_index_; ++i) {
    g[i] = x[slack_var_start_index_ + slack_var_index];
    ++slack_var_index;
  }
  return true;
}

---

边界条件：

bool FemPosDeviationIpoptInterface::get_bounds_info(int n, double* x_l,
                                                    double* x_u, int m,
                                                    double* g_l, double* g_u) {
  CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
  CHECK_EQ(static_cast<size_t>(m), num_of_constraints_);
  // 状态变量
  // a. for x, y
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i * 2;
    // x
    x_l[index] = -1e20;
    x_u[index] = 1e20;

    // y
    x_l[index + 1] = -1e20;
    x_u[index + 1] = 1e20;
  }
  // b. for slack var
  for (size_t i = slack_var_start_index_; i < slack_var_end_index_; ++i) {
    x_l[i] = -1e20;
    x_u[i] = 1e20;
  }

  // 约束
  // a. 点的上下左右边界约束
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i * 2;
    // x
    g_l[index] = ref_points_[i].first - bounds_around_refs_[i];
    g_u[index] = ref_points_[i].first + bounds_around_refs_[i];

    // y
    g_l[index + 1] = ref_points_[i].second - bounds_around_refs_[i];
    g_u[index + 1] = ref_points_[i].second + bounds_around_refs_[i];
  }

  // b. 曲率约束上下界
  double ref_total_length = 0.0;
  auto pre_point = ref_points_.front();
  for (size_t i = 1; i < num_of_points_; ++i) {
    auto cur_point = ref_points_[i];
    double x_diff = cur_point.first - pre_point.first;
    double y_diff = cur_point.second - pre_point.second;
    ref_total_length += std::sqrt(x_diff * x_diff + y_diff * y_diff);
    pre_point = cur_point;
  }
  double average_delta_s =
      ref_total_length / static_cast<double>(num_of_points_ - 1);
  double curvature_constr_upper =
      average_delta_s * average_delta_s * curvature_constraint_;

  for (size_t i = curvature_constr_start_index_;
       i < curvature_constr_end_index_; ++i) {
    g_l[i] = -1e20;
    g_u[i] = curvature_constr_upper * curvature_constr_upper;
  }

  // c. 松弛变量约束
  for (size_t i = slack_constr_start_index_; i < slack_constr_end_index_; ++i) {
    g_l[i] = 0.0;
    g_u[i] = 1e20;
  }

  return true;
}

这里解释一下对曲率约束的上届：

对于除去首尾的每个anchor point（因为需要三个点计算曲率，故首尾无法计算），需要满足：
$$\begin{gathered} (x_i+x_{i+2}-2\times x_{i+1}{)}^2+(y_i+y_{i+2}-2\times y_{i+1}{)}^2-stac{k}_i\le (curvature\_constr\_upper{)}^2 \\ curvature\_constr\_upper=average\_delta\_s\times average\_delta\_s\times curvature\_constraint\_ \end{gathered}$$
该约束是之前提到的那个矢量的模的平方减去松弛变量。首先计算了原始的anchor points之间的平均距离average_delta_s，定义最大曲率curvature_constraint = 0.2，该约束的上界设置为$average\_delta\_s^2\times curvature\_constraint$。该约束没有下界，而计算其上界的具体物理意义如下图所示，黄色线段可以理解为松弛变量。

如图所示，假设 $P_0,P_1,P_2$三点处在同一个圆上，当${\theta }_1$较小时，向量$\overrightarrow{P_1{P}_0}$和 向量$\overrightarrow{P_1{P}_2}$的模近似等于弧长，因此有：
$${\theta }_1=\frac{dS}{R}$$
根据$O-P_0-P_1$等腰三角形几何关系有：
$${\theta }_2=\frac{\pi -{\theta }_1}{2}$$
由于$|\overrightarrow{P_1{P}_0}|=|\overrightarrow{P_1{P}_2}|$，所以$C$是$P_1{P}_3$的中点，有$|\overrightarrow{P_{1}C}|=|\overrightarrow{C{P}_3}|$关系，由此得：
$$|\overrightarrow{P_1{P}_3}|=2P_{1}C$$
根据$C-P_0-P_1$直角三角形几何关系有：
$$P_{1}C=dS\times cos({\theta }_2)$$
把$P_{1}C$带入$|\overrightarrow{P_1{P}_3}|$得：
$$\begin{array}{rl}|\overrightarrow{P_1{P}_3}|& =2P_{1}C\\ & =2\times dS\times cos({\theta }_2)\end{array}$$
把${\theta }_2$带入得：
$$\begin{array}{rl}|\overrightarrow{P_1{P}_3}|& =2\times dS\times cos({\theta }_2)\\ & =2\times dS\times cos(\frac{\pi -{\theta }_1}{2})\\ & =2\times dS\times cos(\frac{\pi }{2}-\frac{{\theta }_1}{2})\\ & =2\times dS\times sin(\frac{{\theta }_1}{2})\\ & =2\times dS\times \frac{{\theta }_1}{2}\\ & =dS\times {\theta }_1\end{array}$$

把${\theta }_1$带入得：
$$\begin{array}{rl}|\overrightarrow{P_1{P}_3}|& =dS\times {\theta }_1\\ & =dS\times \frac{dS}{R}\\ & =d{S}^2\times \frac{1}{R}\\ & =d{S}^2\times cur\end{array}$$

---

fem_pos_deviation_osqp_interface.cc和fem_pos_deviation_sqp_osqp_interface.cc文件中二次规划代价函数矩阵形式如下：

void FemPosDeviationOsqpInterface::CalculateKernel(
    std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
    std::vector<c_int>* P_indptr) {
  CHECK_GT(num_of_variables_, 4);

  // Three quadratic penalties are involved:
  // 1. Penalty x on distance between middle point and point by finite element
  // estimate;
  // 2. Penalty y on path length;
  // 3. Penalty z on difference between points and reference points

  // General formulation of P matrix is as below(with 6 points as an example):
  // I is a two by two identity matrix, X, Y, Z represents x * I, y * I, z * I
  // 0 is a two by two zero matrix
  // |X+Y+Z, -2X-Y,   X,       0,       0,       0    |
  // |0,     5X+2Y+Z, -4X-Y,   X,       0,       0    |
  // |0,     0,       6X+2Y+Z, -4X-Y,   X,       0    |
  // |0,     0,       0,       6X+2Y+Z, -4X-Y,   X    |
  // |0,     0,       0,       0,       5X+2Y+Z, -2X-Y|
  // |0,     0,       0,       0,       0,       X+Y+Z|

  // Only upper triangle needs to be filled
  std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  columns.resize(num_of_variables_);
  int col_num = 0;

  for (int col = 0; col < 2; ++col) {
    columns[col].emplace_back(col, weight_fem_pos_deviation_ +
                                       weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  for (int col = 2; col < 4; ++col) {
    columns[col].emplace_back(
        col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
                                       2.0 * weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  int second_point_from_last_index = num_of_points_ - 2;
  for (int point_index = 2; point_index < second_point_from_last_index;
       ++point_index) {
    int col_index = point_index * 2;
    for (int col = 0; col < 2; ++col) {
      col_index += col;
      columns[col_index].emplace_back(col_index - 4, weight_fem_pos_deviation_);
      columns[col_index].emplace_back(
          col_index - 2,
          -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
      columns[col_index].emplace_back(
          col_index, 6.0 * weight_fem_pos_deviation_ +
                         2.0 * weight_path_length_ + weight_ref_deviation_);
      ++col_num;
    }
  }

  int second_point_col_from_last_col = num_of_variables_ - 4;
  int last_point_col_from_last_col = num_of_variables_ - 2;
  for (int col = second_point_col_from_last_col;
       col < last_point_col_from_last_col; ++col) {
    columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
    columns[col].emplace_back(
        col - 2, -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
                                       2.0 * weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  for (int col = last_point_col_from_last_col; col < num_of_variables_; ++col) {
    columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
    columns[col].emplace_back(
        col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
    columns[col].emplace_back(col, weight_fem_pos_deviation_ +
                                       weight_path_length_ +
                                       weight_ref_deviation_);
    ++col_num;
  }

  CHECK_EQ(col_num, num_of_variables_);

  int ind_p = 0;
  for (int i = 0; i < col_num; ++i) {
    P_indptr->push_back(ind_p);
    for (const auto& row_data_pair : columns[i]) {
      // Rescale by 2.0 as the quadratic term in osqp default qp problem setup
      // is set as (1/2) * x' * P * x
      P_data->push_back(row_data_pair.second * 2.0);
      P_indices->push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  P_indptr->push_back(ind_p);
}

代价函数——二次规划代价函数矩阵形式

以$n$个点为例子$P_0,P_1,P_2,P_3,\cdots ,P_n$

第1部分——参考点距离代价
$$ob{j}_{value}1=\sum _{i=0}^{n-1}(x_i-x_{i-ref}{)}^2+(y_i-y_{i-ref}{)}^2$$
第2部分——平滑性代价
$$ob{j}_{value}2=\sum _{i=0}^{n-2}(x_i+x_{i+2}-2\times x_{i+1}{)}^2+(y_i+y_{i+2}-2\times y_{i+1}{)}^2$$
第3部分——总长度代价
$$ob{j}_{value}3=\sum _{i=0}^{n-1}(x_i-x_{i+1}{)}^2+(y_i-y_{i+1}{)}^2$$

总代价函数：
$$ob{j}_{value}=w_1\times ob{j}_{value}1+w_2\times ob{j}_{value}2+w_3\times ob{j}_{value}3$$
其中$w_1{w}_2{w}_3$分别代表上述三种代价的权重，经过化简得到矩阵形式：
$$ob{s}_{value}=\frac{1}{2}{x}^{T}Px+q^{T}x$$
其中$P$和$q$矩阵形式如下：
$$P=\begin{array}{ccccccc}{W}_2+W_3+W_1& -2W_2-W_3& W_2& 0& 0& 0& 0\\ -2W_2-W_3& 5W_2+2W_3+W_1& -4W_2-W_3& W_2& 0& 0& 0\\ W_2& -4W_2-W_3& 6W_2+2W_3+W_1& -4W_2-W_3& W_2& 0& 0\\ 0& \ddots & \ddots & \ddots & \ddots & \ddots & 0\\ 0& 0& W_2& -4W_2-W_3& 6W_2+2W_3+W_1& -4W_2-W_3& W_2\\ 0& 0& 0& W_2& -4W_2-W_3& 5W_2+2W_3+W_1& -2W_2-W_3\\ 0& 0& 0& 0& W_2& -2W_2-W_3& W_2+W_3+W_1\end{array}$$

$$q=-2W_1{X}_{ref}$$

约束方程矩阵形式：
$$l\le Ax\le u$$
$A$为单位矩阵$I_{n\times n}$，$l$和$u$为$xy$设置的上下界