算法简介
曲线插值法是在满⾜某些特定条件下，对路径曲线的拟合。
棉花糖制造机常见的拟合曲线有：多项式曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、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上。
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议