算法简介
-
曲线插值法是在满足某些特定条件下，对路径曲线的拟合。
-
常见的拟合曲线有：多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、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上。