區域性路徑規劃之曲線插值法

演算法簡介

• 曲線插值法是在滿足某些特定條件下，對路徑曲線的擬合。

• 常見的擬合曲線有：多項式曲線、雙圓弧段曲線、正弦函式曲線、貝塞爾曲線、B樣條曲線等；

演算法思想

• 曲線插值法的核心思想是基於預先構造的曲線型別，根據車輛期望達到的狀態（比如要求車輛到達某點的速度和加速度為期望值），將此期望值作為邊界條件代入曲線型別進行方程求解，獲得曲線的相關係數；

• 曲線所有的相關係數一旦確定，軌跡規劃隨之完成。

多項式曲線

• 多項式曲線分為三次多項式曲線、五次多項式曲線、七次多項式曲線；

• 我們注意到上述的多項式曲線都是奇數次，事實上，多項式曲線一般而言都是奇數次，這是由邊界條件引起的。邊界條件一般包括車輛的起始狀態和終止狀態（如換道軌跡的起點狀態和終點狀態），由於偶數個狀態導致有唯一解的方程係數為偶數，故偶數個係數的多項式也就是奇數多項式

  。

• 三次多項式曲線：最多能確定每一個期望點的兩個維度的期望狀態，一般來說就是位置和速度

\[\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3\\ \end{cases}\]

• 五次多項式曲線：最多能確定每一個期望點的三個維度的期望狀態，一般來說就是位置、速度和加速度

\[\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\ \end{cases}\]

• 七次多項式曲線
  ：最多能確定每一個期望點的三個維度的期望狀態，一般來說就是位置、速度、加速度和加加速度（衝擊度，jerk）

\[\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5 + a_6t^6+a_7t^7\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5 +b_6t^6+b_7t^7\\ \end{cases}\]

以五次多項式為例的曲線插值法

1. 首先，我們定義如下的五次多項式：

\[\begin{cases} x\left( t \right)= a_0 +a_1t+a_2t^2+a_3t^3 +a_4t^4+a_5t^5\\ y\left( t \right)=b_0 +b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5\\ \end{cases}\]

2. 我們將起始時刻定義為\(t_0\)

   ，起始時刻的位置、速度和加速度均已知，我們構造如下的縱向和橫向方程：
   • 起始時刻的位置方程：
   \[\begin{cases} x\left( t_0 \right)= a_0 +a_1t_0+a_2t_0^2+a_3t_0^3 +a_4t_0^4+a_5t_0^5\\ y\left( t_0 \right)=b_0 +b_1t_0+b_2t_0^2+b_3t_0^3+b_4t_0^4+b_5t_0^5\\ \end{cases}\]
   • 起始時刻的速度方程：
   \[\begin{cases} x^\prime\left( t_0 \right)= a_1 +2a_2t_0+3a_3t_0^2 +4a_4t_0^3+5a_5t_0^4\\ y^\prime\left( t_0 \right)=b_1 +2b_2t_0+3b_3t_0^2+4b_4t_0^3+5b_5t_0^4\\ \end{cases}\]
   • 起始時刻的加速度方程：
   \[\begin{cases} x^{\prime\prime}\left( t_0 \right)= 2a_2+6a_3t_0 + 12a_4t_0^2+20a_5t_0^3\\ y^{\prime\prime}\left( t_0 \right)=2b_2+6b_3t_0 + 12b_4t_0^2+20b_5t_0^3\\ \end{cases}\]

3. 同理，我們可以構造終止時刻\(t_1\)的的橫向和縱向方程，這裡就不再詳細描述；

4. 我們將起始位置和終止位置的橫縱向方程統一用矩陣表達為：

\[X=\left[ \begin{array}{c} x_0 \\ x_0^\prime\\ x_0^{\prime\prime}\\ x_1\\ x_1^\prime\\ x_1^{\prime\prime}\\ \end{array} \right] = \left[ \begin{array}{c} t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\ 5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\ 20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\ t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\ 5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\ 20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\ \end{array} \right] \left[ \begin{array}{c} a_5 \\ a_4\\ a_3\\ a_2\\ a_1\\ a_0\\ \end{array} \right]=T\times A\]\[Y=\left[ \begin{array}{c} y_0 \\ y_0^\prime\\ y_0^{\prime\prime}\\ y_1\\ y_1^\prime\\ y_1^{\prime\prime}\\ \end{array} \right] = \left[ \begin{array}{c} t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\ 5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0^1 & 1 & 0 \\ 20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0 \\ t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\ 5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1^1 & 1 & 0 \\ 20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \\ \end{array} \right] \left[ \begin{array}{c} b_5 \\ b_4\\ b_3\\ b_2\\ b_1\\ b_0\\ \end{array} \right]=T\times B\]

5. 對上述矩陣進行求解（\(A=T^{-1} \times X\)，\(B=T^{-1} \times Y\)），我們可以得到唯一的係數矩陣解，即得到唯一的多項式曲線，該曲線上每一點的導數代表著車輛經過該點時的速度。多項式曲線插值法軌跡規劃出的曲線是路徑+速度的耦合結果。

注意：曲線插值法得到的軌跡曲線是關於時間\(t\)的函式，而非座標\(y\)關於座標\(x\)的函式。

與雙圓弧段換道軌跡進行對比：

• 雙圓弧段換道軌跡由弧AC+線段CD+弧DF構成，如下圖所示：
  
• 在C點，軌跡曲率由弧AC段的定值突變為0，考慮到方向盤的轉角是一個連續緩變的過程，為了讓車輛能夠完全跟隨軌跡，車輛行駛到C點後必須速度為0，讓方向盤回正後才能沿給定的軌跡繼續行駛，因此雙圓弧段換道無法應用於行車路徑規劃，而應用於泊車路徑規劃。

程式碼實現

待更新，後續會上傳到Github上。