曲线插值的方法是按照车辆在某些特定条件(安全、快速、高效)下,进行路线拟合,常见的有多项式曲线、双圆弧曲线、正弦函数曲线、贝塞尔曲线、B样条曲线。
曲线插值的法的核心思想就是基于预先构造的曲线类型,根据车辆期望达到的状态(比如要求车辆到达某点的速度和加速度为期望值),将此期望值作为边界条件代入曲线类型进行方程求解,获得曲线的相关系数。
曲线的相关系数一旦确定,轨迹就随之确定完成。
针对三次多项式曲线,最多能确定每一个期望点的两个维度的期望状态,一般来说就是位置和速度。针对五次多项式曲线,最多能确定每一个期望点的三个维度的期望状态,一般来说就是位置、速度、加速度。针对七次多项式曲线,最多能确定每一个期望点的四个维度的期望状态,一般来说就是位置、速度、加速度加加速度(冲击度,jerk)。故根据自身轨迹规划的需求,合理选择对应的多项式曲线。
以五次多项式曲线为例讲解曲线插值法轨迹规划:
位置:
速度:
加速度:
定义换道终点时间为t1,横纵向均有期望的位置、速度加速度,又分别可以得到三个方程,不再列出。
因此把起末两点的横纵向方程统一用矩阵表达为:
多项式曲线的自变量为时间t,故一旦求解系数矩阵,曲线唯一确定后,则曲线上每一点的导数就代表了车辆经过该点时的速度。这表明多项式曲线换道轨迹规划是路径+速度的耦合结果。
五次多项式换道轨迹曲线特指横向位置/纵向位置是关于时间t的五次多项式,而不是指纵向位置y关于横向位置x的五次多项式。
双圆弧段换道轨迹由弧AC+线段CD+弧DF构成;
在c点,轨迹曲率由弧AC段的定值突变为0,故为了让车辆能完全跟随轨迹,考虑到方向盘转角是一个连续缓变过程,车辆行驶到在C点后必须速度为0,让方向盘回正后才能继续行驶,因此无法应用于行车路径规划,而应用于泊车路径规划。
此代码可定义换道场景、车道标准宽度、直线段长度、车宽、车长,换道前后的起始状态和终止状态等。
下图为换道轨迹曲线:
下图为换道的纵向速度变化,本代码纵向速度保持不变,(可根据需要自行修改,更改成纵向速度变化的)
下图为横向速度、横向加速度变化曲线:
此代码可计算并画出J的变化曲线,可自定义ay_max,t1_max等:
、
同时为对比不同曲线效果:博主开发了不同曲线的对比,效果如下,可自行修改代码,得出需要的曲线:
代码放在咸鱼,账号名称:咸鱼程序猿,有需要可自取,可定制化开发,可继续修改。