曲线插值方法简介（五次多项式路径规划）

曲线插值的方法是按照车辆在某些特定条件（安全、快速、高效）下，进行路线拟合，常见的有多项式曲线、双圆弧曲线、正弦函数曲线、贝塞尔曲线、B样条曲线。

曲线插值的法的核心思想就是基于预先构造的曲线类型，根据车辆期望达到的状态（比如要求车辆到达某点的速度和加速度为期望值），将此期望值作为边界条件代入曲线类型进行方程求解，获得曲线的相关系数。

曲线的相关系数一旦确定，轨迹就随之确定完成。

针对三次多项式曲线，最多能确定每一个期望点的两个维度的期望状态，一般来说就是位置和速度。针对五次多项式曲线，最多能确定每一个期望点的三个维度的期望状态，一般来说就是位置、速度、加速度。针对七次多项式曲线，最多能确定每一个期望点的四个维度的期望状态，一般来说就是位置、速度、加速度加加速度(冲击度，jerk)。故根据自身轨迹规划的需求，合理选择对应的多项式曲线。

以五次多项式曲线为例讲解曲线插值法轨迹规划：

位置：

           

速度：

           

加速度：

                

定义换道终点时间为t1，横纵向均有期望的位置、速度加速度，又分别可以得到三个方程，不再列出。

因此把起末两点的横纵向方程统一用矩阵表达为:

多项式曲线的自变量为时间t，故一旦求解系数矩阵，曲线唯一确定后，则曲线上每一点的导数就代表了车辆经过该点时的速度。这表明多项式曲线换道轨迹规划是路径+速度的耦合结果。

五次多项式换道轨迹曲线特指横向位置/纵向位置是关于时间t的五次多项式，而不是指纵向位置y关于横向位置x的五次多项式。

双圆弧段换道轨迹由弧AC+线段CD+弧DF构成;

在c点，轨迹曲率由弧AC段的定值突变为0，故为了让车辆能完全跟随轨迹，考虑到方向盘转角是一个连续缓变过程，车辆行驶到在C点后必须速度为0，让方向盘回正后才能继续行驶，因此无法应用于行车路径规划，而应用于泊车路径规划。

此代码可定义换道场景、车道标准宽度、直线段长度、车宽、车长，换道前后的起始状态和终止状态等。

下图为换道轨迹曲线：

下图为换道的纵向速度变化，本代码纵向速度保持不变，（可根据需要自行修改，更改成纵向速度变化的）

下图为横向速度、横向加速度变化曲线：

此代码可计算并画出J的变化曲线，可自定义ay_max,t1_max等：

、

同时为对比不同曲线效果：博主开发了不同曲线的对比，效果如下，可自行修改代码，得出需要的曲线：

代码放在咸鱼，账号名称：咸鱼程序猿，有需要可自取，可定制化开发，可继续修改。