【路徑規划】OSQP曲線平滑 公式及代碼

參考與前言

---

1. apollo 代碼：https://github.com/ApolloAuto/apollo/tree/master/modules/planning/math/smoothing_spline

2. apollo readme：https://github.com/ApolloAuto/apollo/blob/master/docs/specs/qp_spline_path_optimizer.md

3. 自我測試的代碼：張聰明/OSQP_test (gitee.com)

本篇也主要基於參考二翻譯而來，背景是因為 GPIR代碼 里有這層做初始化，不太懂怎么干的和干啥，所以就搜索了一下，一開始一直在弄參考三使其能逐步斷點debug 以讓我能明了整個步驟，不過后來搜了一下看到了參考二 emm 就是我想要了解的 smooth到底是怎樣編程二次規划問題的。

以下大部分翻譯至參考二，其中添加了自己的思考句子（有時也可能就是問題

實際這個包的名字就：QP-Spline-Path Optimizer，基於二次規划的曲線路徑優化器

方法：Quadratic programming + Spline interpolation，二次規划 + 樣條差值

注意這全文都是講的path，而不是trajectory，至於兩者區別：Motion Planning 是什么 總結了下：

Path是靜態的，不包括時間；Trajectory是除了路徑還有其他信息可以說是時間，speed profile，這條路徑上速度的變化關系

這一段是我看完了寫的：emmm 看完了發現包裝的太好了 以至於就算看了整個公式 對應起來都是要跳轉進去的 果然是做軟件一套的。但是想着用的話 這樣也算是夠了的。所以整體其實還是拿五次樣條先生成了reference point 然后根據他們的s,l點來進行約束，cost function也已經給出

① 目標函數 Objective function

路徑長度

首先路徑的坐標系是以SL 也就是frenet坐標系下。其中 s 是從車輛當前位置到規划路徑的長度，類似於我們會提前給定一個 我們要規划多長的距離。

樣條線段

將路徑拆分成 n 段。每段都用多項式表示，比如這樣一個式子：

每段 i 沿着參考線的累積距離 \(d_i\) 的多項式函數公式如下：

\[l = f_i(s) = a_{i0} + a_{i1} \cdot s + a_{i2} \cdot s^2 + a_{i3} \cdot s^3 + a_{i4} \cdot s^4 + a_{i5} \cdot s^5 (0 \leq s \leq d_{i}) \tag{1} \]

實際代碼調用時，后者的 5 也就是五次多項式的意思

OsqpSpline2dSolver osqp_spline2d_solver(t_knots, 5);

線段的優化函數

也就是一個cost 和為速度，加速度，加加速度：

\[\begin{align} cost = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big) \end{align} \]

但實際test.cc這個代碼中只對加加速度進行了weight賦值，所以最后在計算曲率 curvature的時候 smooth后的效果明顯好很多（雖然點之間看不出來啥）

mutable_kernel->Add2dThirdOrderDerivativeMatrix(0.5);

將cost函數轉為QP形式

QP formulation:

\[\begin{aligned} minimize & \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. \qquad & LB \leq x \leq UB \\ & A_{eq}x = b_{eq} \\ & Ax \geq b \end{aligned} \]

下面是如何被優化函數objective function/cost function轉成QP形式，首先我們 (1)式 可以轉成向量相乘的形式，那么 \(f_i(s)\) 可以這樣表示

\[f_i(s) ＝ \begin{vmatrix} 1 & s & s^2 & s^3 & s^4 & s^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

\(f_i'(s)\) 求導再次表示

\[f_i'(s) = \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

\(f_i'(s)^2\) 兩個相乘一下：

\[f_i'(s)^2 = \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

然后就會獲得這樣的：

\[\int\limits_{0}^{d_i} f_i'(s)^2 ds ＝ \int\limits_{0}^{d_i} \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} ds \]

把常數項提到積分外面去：

\[\int\limits_{0}^{d_i} f'(s)^2 ds ＝ \begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 0 \\ 1 \\ 2s \\ 3s^2 \\ 4s^3 \\ 5s^4 \end{vmatrix} \cdot \begin{vmatrix} 0 & 1 & 2s & 3s^2 & 4s^3 & 5s^4 \end{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

\[＝\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \int\limits_{0}^{d_i} \begin{vmatrix} 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{vmatrix} ds \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

然后給矩陣里的都積分一下從0到\(d_i\) 積分一下 就可以獲得這樣的：

\[\int\limits_{0}^{d_i} f'_i(s)^2 ds =\begin{vmatrix} a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} \end{vmatrix} \cdot \begin{vmatrix} 0 & 0 & 0 & 0 &0&0\\ 0 & d_i & d_i^2 & d_i^3 & d_i^4&d_i^5\\ 0& d_i^2 & \frac{4}{3}d_i^3& \frac{6}{4}d_i^4 & \frac{8}{5}d_i^5&\frac{10}{6}d_i^6\\ 0& d_i^3 & \frac{6}{4}d_i^4 & \frac{9}{5}d_i^5 & \frac{12}{6}d_i^6&\frac{15}{7}d_i^7\\ 0& d_i^4 & \frac{8}{5}d_i^5 & \frac{12}{6}d_i^6 & \frac{16}{7}d_i^7&\frac{20}{8}d_i^8\\ 0& d_i^5 & \frac{10}{6}d_i^6 & \frac{15}{7}d_i^7 & \frac{20}{8}d_i^8&\frac{25}{9}d_i^9 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \]

所以現在應該發現了：如果是五次樣條曲線的導數cost函數是會獲得一個\(6\times6\)的矩陣

② 約束 Constraints

初始點/end點約束

init point/初始點

假設第一個點是 (\(s_0\), \(l_0\)), (\(s_0\), \(l'_0\)) and (\(s_0\), \(l''_0\)), 其中 \(l_0\) , \(l'_0\) and \(l''_0\) 是規划初始點的橫向offset及其一階導、二階導，可以由 \(f_i(s)\), \(f'_i(s)\), and \(f_i(s)''\) 計算得知

使用這個式子，將這些約束轉為QP等式約束

\[A_{eq}x = b_{eq} \]

下面就是轉換的步驟：

\[f_i(s_0) = \begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5}\end{vmatrix} = l_0 \]

\[f'_i(s_0) = \begin{vmatrix} 0& 1 & 2s_0 & 3s_0^2 & 4s_0^3 &5 s_0^4 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l'_0 \]

\[f''_i(s_0) = \begin{vmatrix} 0&0& 2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = l''_0 \]

其中 \(i\) 是指那段擁有初始點 \(s_0\) 的分段區，應該說是最初的那段，因為后面平滑約束的時候還有 \(f_{k+1}(s_0)\)上面三個一起形成了一個等式，即最后的形式如 式(2).

end point/結束點

與上述的initial point 初始點步驟一致，結束點 \((s_e, l_e)\) 也是已知的，而且應該是和前面的約束一致的形式，所以總一總可以得出

等式約束 \(A_{eq}x = b_{eq}\) 展開為以下形式

\[\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 0&1 & 2s_0 & 3s_0^2 & 4s_0^3 & 5s_0^4 \\ 0& 0&2 & 3\times2s_0 & 4\times3s_0^2 & 5\times4s_0^3 \\ 1 & s_e & s_e^2 & s_e^3 & s_e^4&s_e^5 \\ 0&1 & 2s_e & 3s_e^2 & 4s_e^3 & 5s_e^4 \\ 0& 0&2 & 3\times2s_e & 4\times3s_e^2 & 5\times4s_e^3 \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} = \begin{vmatrix} l_0\\ l'_0\\ l''_0\\ l_e\\ l'_e\\ l''_e\\ \end{vmatrix}\tag{2} \]

實際代碼中為：

// init point constraint
mutable_constraint->Add2dPointConstraint(0, Eigen::Vector2d(spline(a, 0), spline(b, 0)));
mutable_constraint->Add2dPointDerivativeConstraint(0, Eigen::Vector2d(spline_1st(a, 0), spline_1st(b, 0)));

// end point constraint
mutable_constraint->Add2dPointConstraint(t_end, Eigen::Vector2d(spline(a, t_end), spline(b, t_end)));
mutable_constraint->Add2dPointDerivativeConstraint(t_end, Eigen::Vector2d(spline_1st(a, t_end), spline_1st(b, t_end)));

分段之間平滑約束

這部分的約束主要在於將分段點連接處進行平滑操作，比如一階連續，二階連續，三階連續。

假設 \(seg_k\) 和 \(seg_{k+1}\) 是相連接的, \(seg_k\) 段累積的 s 距離值為 \(s_k\). 注意這里的 \(s_0\) 不同與前面說的為整段路徑的init point初始點，而是每段自身的起點處，所以對 \(seg_k\) 來說他的累積距離 \(s_k\) 正是 \(seg_{k+1}\) 的起點 \(s_0\)

\[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} \]

\[\begin{vmatrix} 1 & s_k & s_k^2 & s_k^3 & s_k^4&s_k^5 & -1 & -s_{0} & -s_{0}^2 & -s_{0}^3 & -s_{0}^4&-s_{0}^5\\ \end{vmatrix} \cdot \begin{vmatrix} a_{k0} \\ a_{k1} \\ a_{k2} \\ a_{k3} \\ a_{k4} \\ a_{k5} \\ 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} = 0 \]

使用 \(s_0\) = 0 在上式中，然后同樣添加如下的等式約束：

\[f'_k(s_k) = f'_{k+1} (s_0) \\ f''_k(s_k) = f''_{k+1} (s_0) \\ f'''_k(s_k) = f'''_{k+1} (s_0) \]

實際代碼中只添加了三階連續：(但是三階連續的前提不是一二階也連續嗎？)

mutable_constraint->Add2dThirdDerivativeSmoothConstraint();

對采樣點的邊界約束

沿着路徑均勻采樣 m 個點，檢查這些點是否觸碰到了障礙物的邊界，然后將其轉成QP問題的不等式約束：

\[Ax \geq b \]

首先根據道路寬度和周圍障礙物找到點 \((s_j, l_j)\) and \(j\in[0, m]\) 的下邊界 \(l_{lb,j}\)，然后不等式約束的計算如下：

\[\begin{vmatrix} 1 & s_0 & s_0^2 & s_0^3 & s_0^4&s_0^5 \\ 1 & s_1 & s_1^2 & s_1^3 & s_1^4&s_1^5 \\ ...&...&...&...&...&... \\ 1 & s_m & s_m^2 & s_m^3 & s_m^4&s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix}a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq \begin{vmatrix} l_{lb,0}\\ l_{lb,1}\\ ...\\ l_{lb,m}\\ \end{vmatrix} \tag{10} \]

• 為什么會有下界？根據采樣的想法應該只有上界才對，或者說下界可以為完全不進行橫向采樣擴張

  傑哥回復：這是一個box形狀，不是說從0-10是我的采樣增加區間 而是以ref為0，-10 - 10采樣增加區間

• 為什么實際代碼中是橫縱向都有限制？采樣點擴張如上問中

      lower_bound.emplace_back(d_lateral - lat_tol[i]);
      lower_bound.emplace_back(d_longitudinal - lon_tol[i]);
  
      upper_bound.emplace_back(d_lateral + lat_tol[i]);
      upper_bound.emplace_back(d_longitudinal + lon_tol[i]);
  

  如此手繪圖所示：

  

• 這里沒有辦法根據障礙物加進去邊界條件？

  好像可以對某一個 \(l_{ub,j}\) 進行擴大，使其表達障礙物的膨脹。但是我感覺這個點應該是說已知環境中，不包括動態障礙物，類似於在地圖已經建好的這層上去做邊界約束，而不是實時根據障礙物去做約束

  這樣的 因為輸入的邊界約束是station lateral boundary，其實是依據你的reference line 參考線然后有設定縱向和橫向的最大便宜角度進去，比如test代碼中是0.2

同樣的，上界 \(l_{ub,j}\) 同樣也可以以這種不等式約束計算：

\[\begin{vmatrix} -1 & -s_0 & -s_0^2 & -s_0^3 & -s_0^4&-s_0^5 \\ -1 & -s_1 & -s_1^2 & -s_1^3 & -s_1^4&-s_1^5 \\ ...&...-&...&...&...&... \\ -1 & -s_m & -s_m^2 & -s_m^3 & -s_m^4&-s_m^5 \\ \end{vmatrix} \cdot \begin{vmatrix} a_{i0} \\ a_{i1} \\ a_{i2} \\ a_{i3} \\ a_{i4} \\ a_{i5} \end{vmatrix} \geq -1 \cdot \begin{vmatrix} l_{ub,0}\\ l_{ub,1}\\ ...\\ l_{ub,m}\\ \end{vmatrix} \tag{11} \]

③ 代碼總結

首先完整的test代碼可見gitee鏈接，此處僅將上述提到的進行說明

  // solver
  OsqpSpline2dSolver osqp_spline2d_solver(t_knots, 5);

  mutable_kernel->Add2dReferenceLineKernelMatrix(t_coord, ref_ft, 0.5);

  // 添加連續平滑約束 三階
  mutable_constraint->Add2dThirdDerivativeSmoothConstraint();
  
  // 添加初始點
  mutable_constraint->Add2dPointConstraint(0, Eigen::Vector2d(spline(a, 0), spline(b, 0)));
  mutable_constraint->Add2dPointDerivativeConstraint(0, Eigen::Vector2d(spline_1st(a, 0), spline_1st(b, 0)));
  
  // 添加end point
  double t_end = t_coord.back();
  mutable_constraint->Add2dPointConstraint(t_end, Eigen::Vector2d(spline(a, t_end), spline(b, t_end)));
  mutable_constraint->Add2dPointDerivativeConstraint(t_end, Eigen::Vector2d(spline_1st(a, t_end), spline_1st(b, t_end)));

  // 添加邊界約束
  mutable_constraint->Add2dStationLateralBoundary(t_coord, ref_ft, ref_theta,lon_tol, lat_tol);

實際上看上去 數學公式都被隱去了 是因為osqp這層 apollo加了自己的spline，所以最后main呈現的就是這樣的形式，如果跳轉各個部分的更為仔細的可以發現更多，比如添加邊界約束：

bool Spline2dConstraint::Add2dStationLateralBoundary(
    const std::vector<double>& t, const vector_Eigen<Eigen::Vector2d>& ref_xy,
    const std::vector<double>& ref_theta, const std::vector<double>& lon_tol,
    const std::vector<double>& lat_tol) {
  if (t.size() != ref_xy.size() || ref_xy.size() != ref_theta.size() ||
      ref_theta.size() != lon_tol.size() || lon_tol.size() != lat_tol.size()) {
    return false;
  }

  Eigen::MatrixXd sl_constraints =
      Eigen::MatrixXd::Zero(2 * t.size(), total_param_);
  std::vector<double> lower_bound, upper_bound;

  for (uint32_t i = 0; i < t.size(); ++i) {
    const uint32_t index = FindSegStartIndex(t[i]);
    const double d_lateral = SignDistance(ref_xy[i], ref_theta[i]);
    const double d_longitudinal =
        SignDistance(ref_xy[i], ref_theta[i] - M_PI_2);
    const double t_corrected = t[i] - t_knots_[index];

    std::vector<double> lateral_coef = AffineCoef(ref_theta[i], t_corrected);
    std::vector<double> longitudianl_coef =
        AffineCoef(ref_theta[i] - M_PI_2, t_corrected);

    const uint32_t index_offset = index * spline_param_num_;

    // 構建公式10,11的左邊矩陣
    for (uint32_t j = 0; j < spline_param_num_; ++j) {
      sl_constraints(2 * i, index_offset + j) = lateral_coef[j];
      sl_constraints(2 * i, index_offset + total_param_ / 2 + j) =
          lateral_coef[spline_param_num_ + j];
      sl_constraints(2 * i + 1, index_offset + j) = longitudianl_coef[j];
      sl_constraints(2 * i + 1, index_offset + total_param_ / 2 + j) =
          longitudianl_coef[spline_param_num_ + j];
    }

    lower_bound.emplace_back(d_lateral - lat_tol[i]);
    lower_bound.emplace_back(d_longitudinal - lon_tol[i]);

    upper_bound.emplace_back(d_lateral + lat_tol[i]);
    upper_bound.emplace_back(d_longitudinal + lon_tol[i]);
  }

  return coxy_constraint_.AddConstraint(sl_constraints, lower_bound,
                                        upper_bound);
}

由這里我們可以看到公式 10 和公式 11的矩陣形式的構建

④ 效果示意

運行上面給的Gitee代碼，可以得到下面這樣一幅圖：

ref就是每小段都是五次樣條出來的，做圖就是sl坐標系下生成出來的，右圖為計算了他們的每個點連接處的曲率，可以看到經過osqp spline曲率約束 cost 都有明顯的生效（雖然在這個例子中展現的不大）