参考线平滑-QpSplineReferenceLineSmoother

Apollo中提供了QpSplineReferenceLineSmoother，即分段五次多项式曲线平滑参考线方法。将其构造成二次规划问题形式，使用OSQP求解。OSQP求解的问题形式如下：
$$\begin{gathered} minimize\frac{1}{2}{x}^{T}Px+q^{T}x \\ subjecttol\le Ax\le u\text{\hspace{1em}(0)} \end{gathered}$$

1. 分段多项式

将原始参考线以$25m$的长度切片，每段的$x,y$分别是$s$的五次多项式，即：
$$\{\begin{array}{l}x=f(s)=a_0+a_{1}s+a_2{s}^2+a_3{s}^3+a_4{s}^4+a_5{s}^5\\ y=g(s)=b_0+b_{1}s+b_2{s}^2+b_3{s}^3+b_4{s}^4+b_5{s}^5\end{array}\text{\hspace{1em}(1-1)}$$
此外，将每段多项式的$s$范围归一化到$[0,1]$范围内，将原始参考点在$UTM$上的坐标转换为基于第一个参考点的相对坐标。

bool QpSplineReferenceLineSmoother::Sampling() {
  const double length = anchor_points_.back().path_point.s() -
                        anchor_points_.front().path_point.s();
  //以25m的长度切片
  uint32_t num_spline =
      std::max(1u, static_cast<uint32_t>(
                       length / config_.qp_spline().max_spline_length() + 0.5));
  // 将多项式自变量归一化到[0,1]
  for (std::uint32_t i = 0; i <= num_spline; ++i) {
    t_knots_.push_back(i * 1.0);
  }
  // normalize point xy
  // 将坐标变化为相对坐标
  ref_x_ = anchor_points_.front().path_point.x();
  ref_y_ = anchor_points_.front().path_point.y();
  return true;
}

在这里使用了参数化方程，是因为自动驾驶车辆的行驶路径曲线在多数时候无法使用显示的表达式$y=f(x)$，而是需要隐式表达式$F(x,y)=0$，例如车辆在环岛行驶的路径为$x^2+y^2=r^2$。此外，参数化方程增加了一个维度$s$，可以使低维下无法表示处理的几何在高纬度上方便的表现出来。

阶次越高的曲线可以拟合的曲线就越多，但是也容易出现龙格现象，因此使用了分段五次多项式，分段可以较为精确的拟合复杂道路，例如直角弯、$U$型弯和$S$型弯等，五次多项式又可以保证拟合曲线的高阶信息的平滑性(二阶对应速度，三阶对应加速度，四阶对应冲击度，因此五次多项式可以保证拟合的曲线足够平滑)。

将公式$(1-1)$写为：
$$\{\begin{array}{l}{x}_i=f_i(s)=a_{i0}+a_{i1}s+a_{i2}{s}^2+a_{i3}{s}^3+a_{i4}{s}^4+a_{i5}{s}^5=A_i^{T}S\\ y_i=g_i(s)=b_{i0}+b_{i1}s+b_{i2}{s}^2+b_{i3}{s}^3+b_{i4}{s}^4+b_{i5}{s}^5=B_i^{T}S\end{array}\text{\hspace{1em}(1-2)}$$
其中$i$表示第$i$段五次多项式，$A_i=[a_{i0},a_{i1},a_{i2},a_{i3},a_{i4},a_{i5}{]}^T,B_i=[b_{i0},b_{i1},b_{i2},b_{i3},b_{i4},b_{i5}{]}^T,S=[1,s,s^2,s^3,s^4,s^5{]}^T$。需要优化求解的即是$A_i^T,B_i^T$。假设有$N$段多项式，则需要求解的优化变量数目为$2\ast 6\ast N$。

2. cost function

因为是平滑问题，那么优化问题的cost function自然是多项式曲率以及其变化率最小，由于曲率计算公式是非线性的，可以近似为多项式的二阶导数和三阶导数和最小，因此参考线平滑的cost function为：
$$J=\sum _{i=1}^N({\int }_{s_{i,start}}^{s_{i,end}}(f^{\prime \prime }(s){)}^{2}ds+{\int }_{s_{i,start}}^{s_{i,end}}(g^{\prime \prime }(s){)}^{2}ds+{\int }_{s_{i,start}}^{s_{i,end}}(f^{\prime \prime \prime }(s){)}^{2}ds+{\int }_{s_{i,start}}^{s_{i,end}}(g^{\prime \prime \prime }(s){)}^{2}ds)\text{\hspace{1em}(2-1)}$$
由于每段多项式因变量的范围都是$[0,1]$，所有公式$(2-1)$可以写为：
$$J=\sum _{i=1}^N({\int }_0^1(f^{\prime \prime }(s){)}^{2}ds+{\int }_0^1(g^{\prime \prime }(s){)}^{2}ds+{\int }_0^1(f^{\prime \prime \prime }(s){)}^{2}ds+{\int }_0^1(g^{\prime \prime \prime }(s){)}^{2}ds)\text{\hspace{1em}(2-2)}$$
cost function有四部分组分，下面分别计算五次多项式的一阶导数、二阶导数和三阶导数的积分。

2.1 一阶导数积分

$${\int }_0^1(f^{\prime }(s){)}^{2}ds={\int }_0^1(A^T{S}^{\prime }{)}^{2}ds={\int }_0^1(A^T{S}^{\prime }{S}^{\prime T}A)ds=A^T{\int }_0^1(S^{\prime }{S}^{\prime T})dsA\text{\hspace{1em}(2-3)}$$

其中，$S^{\prime }$是$S$的导数向量，$S^{\prime }=[1,s,s^2,s^3,s^4,s^5{]}^{\prime T}=[0,1,2s,3s^2,4s^3,5s^4{]}^T$。代入公式$(2-3)$可得：
$$\begin{array}{rl}{\int }_0^1(f^{\prime }(s){)}^{2}ds& =A^T{\int }_0^1(S^{\prime }{S}^{\prime T})dsA\\ & =A^T({\int }_0^1\left[\begin{array}{c}0\\ 1\\ 2s\\ 3s^2\\ 4s^3\\ 5s^4\end{array}\right]\left[\begin{array}{cccccc}0& 1& 2s& 3s^2& 4s^3& 5s^4\end{array}\right]ds)A\\ & =A^T({\int }_0^1\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 1& 2s& 3s^2& 4s^3& 5s^4\\ 0& 2s& 4s^2& 6s^3& 8s^4& 10s^5\\ 0& 3s^2& 6s^3& 9s^4& 12s^5& 15s^6\\ 0& 4s^3& 8s^4& 12s^5& 16s^6& 20s^7\\ 0& 5s^4& 10s^5& 15s^6& 20s^7& 25s^8\end{array}\right]ds)A\\ & =A^T\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& s& s^2& s^3& s^4& s^5\\ 0& s^2& \frac{4}{3}{s}^3& \frac{6}{4}{s}^4& \frac{8}{5}{s}^5& \frac{10}{6}{s}^6\\ 0& \frac{3}{3}{s}^3& {\frac{6}{4}}^4& \frac{9}{5}{s}^5& \frac{12}{6}{s}^6& \frac{15}{7}{s}^7\\ 0& \frac{4}{4}{s}^4& \frac{8}{5}{s}^5& \frac{12}{6}{s}^6& \frac{16}{7}{s}^7& \frac{20}{8}{s}^8\\ 0& \frac{5}{5}{s}^5& \frac{10}{6}{s}^6& \frac{15}{7}{s}^7& \frac{20}{8}{s}^8& \frac{25}{9}{s}^9\end{array}\right]A\\ & =A^T\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 1& 1& 1& 1& 1\\ 0& 1& \frac{4}{3}& \frac{6}{4}& \frac{8}{5}& \frac{10}{6}\\ 0& 1& \frac{6}{4}& \frac{9}{5}& \frac{12}{6}& \frac{15}{7}\\ 0& 1& \frac{8}{5}& \frac{12}{6}& \frac{16}{7}& \frac{20}{8}\\ 0& 1& \frac{10}{6}& \frac{15}{7}& \frac{20}{8}& \frac{25}{9}\end{array}\right]A\end{array}\text{\hspace{1em}(2-4)}$$
令
$$Q^1=\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 1& 1& 1& 1& 1\\ 0& 1& \frac{4}{3}& \frac{6}{4}& \frac{8}{5}& \frac{10}{6}\\ 0& 1& \frac{6}{4}& \frac{9}{5}& \frac{12}{6}& \frac{15}{7}\\ 0& 1& \frac{8}{5}& \frac{12}{6}& \frac{16}{7}& \frac{20}{8}\\ 0& 1& \frac{10}{6}& \frac{15}{7}& \frac{20}{8}& \frac{25}{9}\end{array}\right]\text{\hspace{1em}(2-5)}$$
则
$$Q_{ij}^1=\{\begin{array}{llcc}0,& i=0& or& j=0\\ \frac{i\ast j}{i+j-1},& i\ne 0& and& j\ne 0\end{array}\text{\hspace{1em}(2-6)}$$

Eigen::MatrixXd SplineSegKernel::DerivativeKernel(const uint32_t num_params,
                                                  const double accumulated_x) {
  if (num_params > reserved_order_ + 1) {
    // 计算s^n的系数，得到的结果是Q1矩阵
    CalculateDerivative(num_params);
  }
  Eigen::MatrixXd term_matrix;
  // 计算s的积分，由于s在区间[0,1]，因此积分后的元素值为1
  IntegratedTermMatrix(num_params, accumulated_x, "derivative", &term_matrix);
  return kernel_derivative_.block(0, 0, num_params, num_params)
      .cwiseProduct(term_matrix);
}

2.2 二阶导数积分

同一阶导数积分，可得：
$${\int }_0^1(f^{\prime \prime }(s){)}^{2}ds=A^T{Q}^{2}A\text{\hspace{1em}(2-7)}$$
其中：
$$Q_{ij}^2=\{\begin{array}{llcc}0,& i\le 1& or& j\le 1\\ \frac{(i\ast i-i)(j\ast j-j)}{i+j-3},& i>1& and& j>1\end{array}\text{\hspace{1em}(2-8)}$$

Eigen::MatrixXd SplineSegKernel::SecondOrderDerivativeKernel(
    const uint32_t num_params, const double accumulated_x) {
  if (num_params > reserved_order_ + 1) {
    CalculateSecondOrderDerivative(num_params);
  }
  Eigen::MatrixXd term_matrix;
  IntegratedTermMatrix(num_params, accumulated_x, "second_order", &term_matrix);
  return kernel_second_order_derivative_.block(0, 0, num_params, num_params)
      .cwiseProduct(term_matrix);
}

2.3 三阶导数积分

同一阶导数积分，可得：
$${\int }_0^1(f^{\prime \prime \prime }(s){)}^{2}ds=A^T{Q}^{3}A\text{\hspace{1em}(2-9)}$$
其中：
$$Q_{ij}^3=\{\begin{array}{llcc}0,& i\le 2& or& j\le 2\\ \frac{(i\ast i-i)(i-2)(j\ast j-j)(j-2)}{i+j-5},& i>2& and& j>2\end{array}\text{\hspace{1em}(2-10)}$$

Eigen::MatrixXd SplineSegKernel::ThirdOrderDerivativeKernel(
    const uint32_t num_params, const double accumulated_x) {
  if (num_params > reserved_order_ + 1) {
    CalculateThirdOrderDerivative(num_params);
  }
  Eigen::MatrixXd term_matrix;
  IntegratedTermMatrix(num_params, accumulated_x, "third_order", &term_matrix);
  return (kernel_third_order_derivative_.block(0, 0, num_params, num_params))
      .cwiseProduct(term_matrix);
}

3. Constraint

约束有边界约束、起点约束和连续性约束。

3.1 边界约束

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-I1xhYtIx-1668078058001)(images/coord-trans.png)]

如上图所示，$P_0$是参考线的第一个点，$P_i$是参考线的第$i$个点，$\theta$是点$P_i$的航向，点$P_i$处的航向方向为$\overrightarrow{P_{i}x}$，其法向量方向为$\overrightarrow{P_{i}y}$。边界约束即是约束平滑后的点和原始点在$\overrightarrow{P_{i}x}$和$\overrightarrow{P_{i}y}$方向上误差在一定阈值内。根据坐标变换公式可得：
$$\{\begin{array}{l}|\overrightarrow{P_{i}A}|=-(|\overrightarrow{P_{i}B}|+|\overrightarrow{AB}|)=-(\Delta x\cos \theta +\Delta y\sin \theta )=-x_i\cos \theta -y_i\sin \theta \\ |\overrightarrow{P_{i}M}|=-(|\overrightarrow{P_{i}M}|-|\overrightarrow{NM}|)=-(\Delta y\cos \theta -\Delta x\sin \theta )=x_i\sin \theta -y_i\cos \theta \end{array}\text{\hspace{1em}(3-1)}$$

const double d_lateral = SignDistance(ref_point[i], angle[i]);
const double d_longitudinal = SignDistance(ref_point[i], angle[i] - M_PI / 2.0);

double Spline2dConstraint::SignDistance(const Vec2d& xy_point,
                                        const double angle) const {
  return common::math::InnerProd(
      xy_point.x(), xy_point.y(),
      -common::math::sin(common::math::Angle16::from_rad(angle)),
      common::math::cos(common::math::Angle16::from_rad(angle)));
}

从Apollo的代码实现来看，以P_i为原点的坐标的$xy$轴方向是和图中相反的。d_longitudinal是指航向方向投影，d_lateral是指航向的法向量方向投影。

计算平滑后的点在原参考点坐标系上的投影，同样使用了公式$(3-1)$：

std::vector<double> Spline2dConstraint::AffineCoef(const double angle,
                                                   const double t) const {
  const uint32_t num_params = spline_order_ + 1;
  std::vector<double> result(num_params * 2, 0.0);
  double x_coef = -common::math::sin(common::math::Angle16::from_rad(angle));
  double y_coef = common::math::cos(common::math::Angle16::from_rad(angle));
  for (uint32_t i = 0; i < num_params; ++i) {
    result[i] = x_coef;
    result[i + num_params] = y_coef;
    x_coef *= t;
    y_coef *= t;
  }
  return result;
}

最终，边界约束的代码实现为：

bool Spline2dConstraint::Add2dBoundary(
    const std::vector<double>& t_coord, const std::vector<double>& angle,
    const std::vector<Vec2d>& ref_point,
    const std::vector<double>& longitudinal_bound,
    const std::vector<double>& lateral_bound) {
  if (t_coord.size() != angle.size() || angle.size() != ref_point.size() ||
      ref_point.size() != lateral_bound.size() ||
      lateral_bound.size() != longitudinal_bound.size()) {
    return false;
  }
  Eigen::MatrixXd affine_inequality =
      Eigen::MatrixXd::Zero(4 * t_coord.size(), total_param_);
  Eigen::MatrixXd affine_boundary =
      Eigen::MatrixXd::Zero(4 * t_coord.size(), 1);
  for (uint32_t i = 0; i < t_coord.size(); ++i) {
    // 计算原始参考点的横纵向投影
    const double d_lateral = SignDistance(ref_point[i], angle[i]);
    const double d_longitudinal =
        SignDistance(ref_point[i], angle[i] - M_PI / 2.0);
    // 计算点在第几段五次多项式上
    const uint32_t index = FindIndex(t_coord[i]);
    // 将s归一化到[0,1]内
    const double rel_t = t_coord[i] - t_knots_[index];
    // y = g(s)的多项式系数对应的优化变量的索引值
    const uint32_t index_offset = 2 * index * (spline_order_ + 1);
    // 计算平滑后点的做投影计算后各个系数
    std::vector<double> longi_coef = AffineCoef(angle[i], rel_t);
    std::vector<double> longitudinal_coef =
        AffineCoef(angle[i] - M_PI / 2, rel_t);
    for (uint32_t j = 0; j < 2 * (spline_order_ + 1); ++j) {
      // upper longi
      affine_inequality(4 * i, index_offset + j) = longi_coef[j];
      // lower longi
      affine_inequality(4 * i + 1, index_offset + j) = -longi_coef[j];
      // upper longitudinal
      affine_inequality(4 * i + 2, index_offset + j) = longitudinal_coef[j];
      // lower longitudinal
      affine_inequality(4 * i + 3, index_offset + j) = -longitudinal_coef[j];
    }
    // 上下边界约束
    affine_boundary(4 * i, 0) = d_lateral - lateral_bound[i];  // 这里应该是 d_lateral + lateral_bound[i]
    affine_boundary(4 * i + 1, 0) = -d_lateral - lateral_bound[i];
    affine_boundary(4 * i + 2, 0) = d_longitudinal - longitudinal_bound[i];  //这里应该是 d_longitudinal + longitudinal_bound[i]
    affine_boundary(4 * i + 3, 0) = -d_longitudinal - longitudinal_bound[i];
  }
  return AddInequalityConstraint(affine_inequality, affine_boundary);
}

3.2 连续性约束

相邻两段五次多项式在连接处需要满足零阶、一阶和二阶导数相等。即：
$$\{\begin{array}{l}{f}_i(1)=a_{i,0}+a_{i,1}+a_{i,2}+a_{i,3}+a_{i,4}+a_{i,5}=a_{i+1,0}=f_{i+1}(0)\\ g_i(1)=b_{i,0}+b_{i,1}+b_{i,2}+b_{i,3}+b_{i,4}+b_{i,5}=b_{i+1,0}=g_{i+1}(0)\\ f_i^{\prime }(1)=a_{i,1}+2a_{i,2}+3a_{i,3}+4a_{i,4}+5a_{i,5}=a_{i+1,1}=f_{i+1}^{\prime }(0)\\ g_i^{\prime }(1)=b_{i,1}+2b_{i,2}+3b_{i,3}+4b_{i,4}+5b_{i,5}=b_{i+1,1}=g_{i+1}^{\prime }(0)\\ f_i^{\prime \prime }(1)=2a_{i,2}+6a_{i,3}+12a_{i,4}+20a_{i,5}=2a_{i+1,2}=f_{i+1}^{\prime \prime }(0)\\ g_i^{\prime \prime }(1)=2b_{i,2}+6b_{i,3}+12b_{i,4}+20b_{i,5}=2b_{i+1,2}=g_{i+1}^{\prime \prime }(0)\end{array}\text{\hspace{1em}(3-2)}$$

bool Spline2dConstraint::AddSecondDerivativeSmoothConstraint() {
  if (t_knots_.size() < 3) {
    return true;
  }
  Eigen::MatrixXd affine_equality =
      Eigen::MatrixXd::Zero(6 * (t_knots_.size() - 2), total_param_);
  Eigen::MatrixXd affine_boundary =
      Eigen::MatrixXd::Zero(6 * (t_knots_.size() - 2), 1);

  for (uint32_t i = 0; i + 2 < t_knots_.size(); ++i) {
    const double rel_t = t_knots_[i + 1] - t_knots_[i];
    const uint32_t num_params = spline_order_ + 1;
    const uint32_t index_offset = 2 * i * num_params;
    std::vector<double> power_t = PolyCoef(rel_t);
    std::vector<double> derivative_t = DerivativeCoef(rel_t);
    std::vector<double> second_derivative_t = SecondDerivativeCoef(rel_t);
    for (uint32_t j = 0; j < num_params; ++j) {
      affine_equality(6 * i, j + index_offset) = power_t[j];
      affine_equality(6 * i + 1, j + index_offset) = derivative_t[j];
      affine_equality(6 * i + 2, j + index_offset) = second_derivative_t[j];
      affine_equality(6 * i + 3, j + index_offset + num_params) = power_t[j];
      affine_equality(6 * i + 4, j + index_offset + num_params) =
          derivative_t[j];
      affine_equality(6 * i + 5, j + index_offset + num_params) =
          second_derivative_t[j];
    }
    affine_equality(6 * i, index_offset + 2 * num_params) = -1.0;
    affine_equality(6 * i + 1, index_offset + 2 * num_params + 1) = -1.0;
    affine_equality(6 * i + 2, index_offset + 2 * num_params + 2) = -2.0;
    affine_equality(6 * i + 3, index_offset + 3 * num_params) = -1.0;
    affine_equality(6 * i + 4, index_offset + 3 * num_params + 1) = -1.0;
    affine_equality(6 * i + 5, index_offset + 3 * num_params + 2) = -2.0;
  }
  return AddEqualityConstraint(affine_equality, affine_boundary);
}

3.3 起点约束

从Apollo的代码实现可以看出，当采用参考线拼接时，要求平滑轨迹在原始参考线(未平滑)的第一个点位置的航向方向一致，但是大小不要求一致，同时不要求$(x,y)$坐标一致。结合第3.1小节中的图，也就是要求，在$\overrightarrow{P_{0}x}$方向上投影方向一致，即可以转化为不等式约束，如果参考点投影在$x$轴正方向，则平滑后点在$x$轴的投影值应该大于零，如果参考点投影在$x$轴负方向，则平滑后点在$x$轴的投影值应该小于零。同时平滑后点在$y$轴的投影值应该等于零。

bool Spline2dConstraint::AddPointAngleConstraint(const double t,
                                                 const double angle) {
  const uint32_t index = FindIndex(t);
  const uint32_t num_params = spline_order_ + 1;
  const uint32_t index_offset = index * 2 * num_params;
  const double rel_t = t - t_knots_[index];

  // add equality constraint
  // 在航向的法向量上投影为0
  Eigen::MatrixXd affine_equality = Eigen::MatrixXd::Zero(1, total_param_);
  Eigen::MatrixXd affine_boundary = Eigen::MatrixXd::Zero(1, 1);
  std::vector<double> line_derivative_coef = AffineDerivativeCoef(angle, rel_t);
  for (uint32_t i = 0; i < line_derivative_coef.size(); ++i) {
    affine_equality(0, i + index_offset) = line_derivative_coef[i];
  }

  // 在航向方向上投影应该保证同向，判断参考点的在航向方向上投影的方向
  // add inequality constraint
  Eigen::MatrixXd affine_inequality = Eigen::MatrixXd::Zero(2, total_param_);
  const Eigen::MatrixXd affine_inequality_boundary =
      Eigen::MatrixXd::Zero(2, 1);
  std::vector<double> t_coef = DerivativeCoef(rel_t);
  int x_sign = 1;
  int y_sign = 1;
  double normalized_angle = fmod(angle, M_PI * 2);
  if (normalized_angle < 0) {
    normalized_angle += M_PI * 2;
  }
  
  if (normalized_angle > (M_PI / 2) && normalized_angle < (M_PI * 1.5)) {
    x_sign = -1;
  }

  if (normalized_angle >= M_PI) {
    y_sign = -1;
  }

  for (uint32_t i = 0; i < t_coef.size(); ++i) {
    affine_inequality(0, i + index_offset) = t_coef[i] * x_sign;
    affine_inequality(1, i + index_offset + num_params) = t_coef[i] * y_sign;
  }
  if (!AddEqualityConstraint(affine_equality, affine_boundary)) {
    return false;
  }
  return AddInequalityConstraint(affine_inequality, affine_inequality_boundary);
}

4. 正则化

在机器学习中，损失函数后面一般都会添加一个额外项，一般为$l_1-norm$和$l_2-norm$。

• $L1$正则化是指权值向量$w$中各个元素的绝对值之和，表示为$||w|{|}_1$，可以用来进行特征选取；
• $L2$正则化是指权值向量$w$中各个元素的平方和，表示为$||w|{|}_2$，可以防止模型过拟合；

在本问题中，正则化中权值向量相对于多项式的系数，也就是优化问题的决策变量，选择$L2$正则化。