局部路径规划算法-B样条曲线

前提：B样条曲线基于贝塞尔曲线改进而来

B样条曲线是B-样条基函数（给定区间上的所有样条函数组成一个线性 空间。）的线性组合。

贝塞尔：顶点数（n+1）决定了阶数（n）,n值越大，曲线峰谷值越多，且贝塞尔曲线无法进行局部修改

控制点较多时，B样条曲线性能更优

控制点用于定义样条曲线的走向、界限范围

（1）K阶B样条曲线的定义——>B样条基函数

（2）程序中转换德布尔-考克斯递推式

（3）根据递推式可有：不同基函数的非零域/三角计算格式

一个B样条曲线是由若干段贝塞尔曲线构成

open B样条曲线：首末两点没有相连（汽车轨迹规划）

均匀B样条曲线：当节点沿参数轴均匀等距分布，为均匀B样条曲线，当n和k一定时，均匀B样条的 基函数呈周期性，所有基函数有相同形状，每个后续基函数 仅仅是前面基函数在新位置上的重复。 

准均匀B样条曲线：两端节点具有重复度k，中间节点非递减 的序列，准均匀B样条曲线保留了贝塞尔曲线在两个端点处的性质：样条曲线在端点处的切线 即为倒数两个端点的连线。准均匀B样条曲线用途最为广泛。

一般来说，次数越高，则曲线的导数次数也会较高，那么将会有很多零点存在，较多的导数零点就导致原曲线存在较多的极值，使曲线出现较多的峰谷值；次数越低，样条曲线逼近控制点效果越好。另一方面，三次B样条曲线能够实现二阶导数连续，故最终选择准均匀三次B样条曲线作为轨迹规划的曲线比较合适

%% 生成B样条曲线

path=[];
Bik = zeros(n+1, 1);

if flag == 1     % 均匀B样条
    NodeVector = linspace(0, 1, n+k+1); %节点矢量
    for u = (k-1)/(n+k+1) : 0.001 : (n+2)/(n+k+1)
        for i = 0 : 1 : n
            Bik(i+1, 1) = BaseFunction(i, k-1 , u, NodeVector);
        end
        p_u = P * Bik;
        path = [path; [p_u(1,1),p_u(2,1)]];
    end
    
elseif flag == 2  % 准均匀B样条
    NodeVector = U_quasi_uniform(n, k-1); % 准均匀B样条的节点矢量
    for u = 0 : 0.005 : 1-0.005
        for i = 0 : 1 : n
            Bik(i+1, 1) = BaseFunction(i, k-1 , u, NodeVector);
        end
        p_u = P * Bik;
        path=[path; [p_u(1),p_u(2)]];
    end
else
    fprintf('error!\n');
end

补充：轨迹点的曲率是否符合车辆的运动学定义（曲率是否连续）

分割线————————————————————————————————————————