曲线插值的方法是按照车辆在某些特定条件(安全、快 速、高效)下,进行路径的曲线拟合,
常见的有多项式 曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、B样 条曲线等。
曲线插值法的核心思想就是基于预先构造的曲线类型,根据车辆期望达到的状态(比如要求车辆到达某点的速度和 加速度为期望值),将此期望值作为边界条件代入曲线类型进行方程求解,获得曲线的相关系数。曲线所有的相关系数一旦确定,轨迹规划随之完成。
多项式曲线分为
三次多项式曲线(位置和速度)
五次多项式曲线(位置、速度、加速度)
七次多项式曲线(位置、速度、加速度、加加速度(冲击度))
边界条件引起使得偶数个系数的多项式也就是奇数多项式。
以五次多项式为例:
1.已知初始位置的时间,位置,速度,加速度和终点位置期望的时间,位置,速度,加速度
2.根据五次多项式公式得出关于位置,速度,加速度的方程组(速度=位置的一阶导,加速度=位置的二阶导)
3.起末两点的横纵向方程统一用矩阵表示
4.矩阵求解能得出五次多项式方程的系数值
多项式曲线换道轨迹规划是路径+速度的耦合结果。
双圆弧段换道轨迹由弧AC+线段CD+弧DF构成
CD 既是弧AB的切线也是弧EF的切线
AC段曲率为1/R,CD段曲率为0
在c点,轨迹曲率由弧AC段的定值突变为0,故为了让车辆能完全跟随轨迹,考虑到方向盘转角是一个连续缓变过程,车辆行驶到在C点后必须速度为0,让方向盘回正后才能继续行驶,因此无法应用于行车路径规划,而应用于泊车路径规划。
部分代码:
% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]);
set(gca, 'YLim',[-4 4]);
% 将时间从t0到t1离散化,获得离散时刻的轨迹坐标
t=(t0:0.05:t1)';
path=zeros(length(t),4);%1-4列分别存放x,y,vx,vy
for i = 1:length(t)
% 纵向位置坐标
path(i,1) = [t(i)^5, t(i)^4, t(i)^3, t(i)^2, t(i), 1] * A;
% 横向位置坐标
path(i,2) = [t(i)^5, t(i)^4, t(i)^3, t(i)^2, t(i), 1] * B;
% 纵向速度
path(i,3) = [5*t(i)^4, 4*t(i)^3, 3*t(i)^2, 2*t(i), 1, 0] * A;
% 横向速度
path(i,4) = [5*t(i)^4, 4*t(i)^3, 3*t(i)^2, 2*t(i), 1, 0] * B;
end