算法簡介
-
曲線插值法是在滿足某些特定條件下，對路徑曲線的擬合。
-
常見的擬合曲線有：多項式曲線、雙圓弧段曲線、正弦函數曲線、貝塞爾曲線、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}\]
曲線插值法（以五次多項式為例）
- 首先，我們定義如下的五次多項式：
\[\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}\]
-
我們將起始時刻定義為\(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}\] -
同理，我們可以構造終止時刻\(t_1\)的的橫向和縱向方程，這里就不再詳細描述；
-
我們將起始位置和終止位置的橫縱向方程統一用矩陣表達為：
\[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\]
- 對上述矩陣進行求解（\(A=T^{-1} \times X\)，\(B=T^{-1} \times Y\)），我們可以得到唯一的系數矩陣解，即得到唯一的多項式曲線，該曲線上每一點的導數代表着車輛經過該點時的速度。多項式曲線插值法軌跡規划出的曲線是路徑+速度的耦合結果。
注意：曲線插值法得到的軌跡曲線是關於時間\(t\)的函數，而非坐標\(y\)關於坐標\(x\)的函數。
與雙圓弧段換道軌跡進行對比：
- 雙圓弧段換道軌跡由弧AC+線段CD+弧DF構成，如下圖所示：
- 在C點，軌跡曲率由弧AC段的定值突變為0，考慮到方向盤的轉角是一個連續緩變的過程，為了讓車輛能夠完全跟隨軌跡，車輛行駛到C點后必須速度為0，讓方向盤回正后才能沿給定的軌跡繼續行駛，因此雙圓弧段換道無法應用於行車路徑規划，而應用於泊車路徑規划。
代碼實現
待更新，后續會上傳到Github上。