参考线平滑-CostThetaSmoother-Ipopt

CostThetaSmoother是Apollo中离散点参考线平滑方法的一种。

1. 优化目标

1.1 平滑性

参考线平滑的首要目标当然是平滑性，使用向量$P_1{P}_2$和$P_2{P}_3$之间夹角的余弦值来表示，显然$\cos \theta$越小，三个点$P_1,P_2,P_3$越接近一条直线，越平滑。因为$\theta$越接近$0$，$\cos \theta$越大，所以应该使$-\cos \theta$越小。
$$J_{smooth}=-\sum _{i=2}^{N-1}\cos {\theta }_i=-\sum _{i=2}^{N-1}\frac{P_{i-1}{P}_i\cdot P_i{P}_{i+1}}{|P_{i-1}{P}_i||P_i{P}_{i+1}|}=-\sum _{i=2}^{N-1}\frac{(x_i-x_{i-1})(x_{i+1}-x_i)+(y_i-y_{i-1})(y_{i+1}-y_i)}{\sqrt{(x_{i-1}-x_i{)}^2+(y_{i-1}-y_i{)}^2}\sqrt{(x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2}}\text{\hspace{1em}(1-1)}$$

1.2 几何性

平滑后的参考线，希望能够保留原始道路的几何信息，不会把弯道的处的参考线平滑成一条直线。使用平滑后点与原始点的距离来表示。
$$J_{deviation}=\sum _{i=1}^N|P_{r,i}{P}_i{|}^2=\sum _{i=1}^N((x_i-x_{i,r}{)}^2+(y_i-y_{i,r}{)}^2)\text{\hspace{1em}(1-2)}$$

1.3 均匀性

平滑后的参考线的每两个相邻点之间的长度尽量均匀一直。
$$J_{length}=\sum _{i=1}^{N-1}|P_i{P}_{i+1}{|}^2=\sum _{i=1}^{N-1}((x_{i+1}-x_i{)}^2+(y_{i+1}-y_i{)}^2)\text{\hspace{1em}(1-3)}$$

因此，参考线平滑的优化目标可以定义为：
$$J=w_{smooth}\ast J_{smooth}+w_{deviation}\ast J_{deviation}+w_{length}\ast J_{length}\text{\hspace{1em}(1-4)}$$

2. 约束条件

只考虑边界约束，即：
$$\begin{gathered} x_{i,lower}\le x_i\le x_{i,upper} \\ {y}_{i,lower}\le y_i\le y_{i,upper}\text{\hspace{1em}(2-1)} \end{gathered}$$
可以转化为：
$$\begin{gathered} x_{i,r}-bound\le x_i\le x_{i,r}+bound \\ {y}_{i,r}-bound\le y_i\le y_{i,r}+bound\text{\hspace{1em}(2-2)} \end{gathered}$$
对参考线的起点和终点进行约束，令其等于原始参考线上的点：
$$\begin{gathered} x_{1,r}\le x_1\le x_{1,r} \\ {y}_{1,r}\le y_1\le y_{1,r}\text{\hspace{1em}(2-3)} \end{gathered}$$

3. Ipopt

根据Ipopt求解器使用要求，需要求解梯度向量函数、雅可比矩阵和黑森矩阵。需要注意的是，在Apollo中通过配置可以使用ADOL-C自动求梯度向量函数、雅可比矩阵和黑森矩阵，也可以自己求解。但是Apollo这里的代码实现有问题，在不使用ADOL-C时，cost function代码中没有$J_{length}$优化项，并且$J_{deviation}$项少了权重系数。

bool CosThetaIpoptInterface::eval_f(int n, const double* x, bool new_x,
                                    double& obj_value) {
  CHECK_EQ(static_cast(n), num_of_variables_);
  // 使用ADOL-C自动求解优化目标函数
  if (use_automatic_differentiation_) {
    eval_obj(n, x, &obj_value);
    return true;
  }

  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);
  }
  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]));
  }
  // 缺少了点之间线段长度均匀紧凑的优化项
  return true;
}

下面的代码是通过ADOL-C求解的，可见是完全按照第一部分叙述的优化目标计算的。

/** Template to return the objective value */
template 
bool CosThetaIpoptInterface::eval_obj(int n, const T* x, T* obj_value) {
  *obj_value = 0.0;
  for (size_t i = 0; i < num_of_points_; ++i) {
    size_t index = i << 1;
    *obj_value +=
        weight_anchor_points_ *
        ((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));
  }
  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]))) /
        (sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
              (x[mindex + 1] - x[findex + 1]) *
                  (x[mindex + 1] - x[findex + 1])) *
         sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
              (x[lindex + 1] - x[mindex + 1]) *
                  (x[lindex + 1] - x[mindex + 1])));
  }

  // Total length
  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;
}