局部路径规划学习记录——曲面插值法

曲线插值的方法是按照车辆在某些特定条件（安全、快 速、高效）下，进行路径的曲线拟合，

常见的有多项式 曲线、双圆弧段曲线、正弦函数曲线、贝塞尔曲线、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