学习笔记：B样条曲线法_准均匀b样条曲线

一、算法简介

1、样条是一根富有弹性的细木条或塑料条，在应用CAD/CAM技术以前，航空、船舶和汽车制造业普遍采用手工绘制自由曲线。绘制员用压铁压住样条，使其通过所有给定的型值点，再适当地调整压铁，改变样条形态，直到符合设计要求。

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

二、算法思想

贝塞尔曲线有以下缺陷:
1、确定了多边形的顶点数(n+1)，也就决定了所定义的Bezier曲线的阶次(n次)，这样很不灵活。
2、当顶点数（n+1)较大时，曲线的次数较高，曲线的导数次数也会较高，因此曲线会出现较多的峰谷值。
3、贝塞尔曲线无法进行局部修改。
B样条曲线除了保持Bezier曲线所具有的有点外，还弥补了上述所有的缺陷。

三、算法过程

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

2、准均匀B样条曲线:两端节点具有重复度k，中间节点非递减的序列，如U ={0,0,0,1,2,3,4,5,5,}准均匀B样条曲线保留了贝塞尔曲线在两个端点处的性质：样条曲线在端点处的切线即为倒数两个端点的连线。准均匀B样条曲线用途最为广泛。

3、一般来说，次数越高，则曲线的导数次数也会较高，那么将会有很多零点存在，较多的导数零点就导致原曲线存在较多的极值，使曲线出现较多的峰谷值;次数越低，样条曲线逼近控制点效果越好。

4、另一方面，三次B样条曲线能够实现二阶导数连续，故最终选择准均匀三次B样条曲线作为轨迹规划的曲线比较合适。

function NodeVector = U_quasi_uniform(n, k)
% 准均匀B样条的节点向量计算，共n+1个控制顶点，k次B样条，k+1阶
NodeVector = zeros(1, n+k+2);
piecewise = n - k + 1;       % 曲线的段数
if piecewise == 1            % 只有一段曲线时，n = k
    for i = k+2 : n+k+2
        NodeVector(1, i) = 1;
    end
else
    flag = 1;           % 不止一段曲线时
    while flag ~= piecewise
        NodeVector(1, k+flag+1) = NodeVector(1, k + flag) + 1/piecewise;
        flag = flag + 1;
    end
    NodeVector(1, n+2 : n+k+2) = 1;   % 节点向量前面和后面有（k+1）个重复值（阶数）
end

% B样条曲线法
% 作者：Ally
% 日期：2021/2/6
clc
clear
close all
 
%% 数据定义
k = 4;                                    % k阶、k-1次B样条
flag = 2;                                  %1,2分别绘制均匀B样条曲线、准均匀B样条曲线
d = 3.5;
P=[0, 10, 25, 25, 40, 50;
    -d/2,-d/2,-d/2+0.5,d/2-0.5,d/2,d/2 ];   %n=5, 6个控制点，可以满足曲率连续
n = size(P,2)-1;                          % n是控制点个数，从0开始计数
 
%% 生成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
 
%% 画图
d = 3.5;               % 道路标准宽度
W = 1.8;               % 汽车宽度
L = 4.7;               % 车长
figure
len_line = 50;
P0 = [0, -d/2];
 
% 画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
fill([P0(1),P0(1),P0(1)-L,P0(1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b')  
 
% 画分界线
plot([-5, len_line],[0, 0], 'w--', 'linewidth',2);  %分界线
plot([-5,len_line],[d,d],'w','linewidth',2);     %左边界线
plot([-5,len_line],[-d,-d],'w','linewidth',2);  %左边界线
 
% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]); 
set(gca, 'YLim',[-4 4]); 
 
% 绘制路径
scatter(path(:,1),path(:,2),100, '.b');%路径点
scatter(P(1,:),P(2,:),'g')
plot(P(1,:),P(2,:),'r');%路径点

function Bik_u = BaseFunction(i, k , u, NodeVector)
 
if k == 0       % 0次B样条
    if u >= NodeVector(i+1) && u < NodeVector(i+2)
        Bik_u = 1;
    else
        Bik_u = 0;
    end
else
    Length1 = NodeVector(i+k+1) - NodeVector(i+1);
    Length2 = NodeVector(i+k+2) - NodeVector(i+2);      % 支撑区间的长度
    if Length1 == 0       % 规定0/0 = 0
        Length1 = 1;
    end
    if Length2 == 0
        Length2 = 1;
    end
    Bik_u = (u - NodeVector(i+1)) / Length1 * BaseFunction(i, k-1, u, NodeVector) ...
        + (NodeVector(i+k+2) - u) / Length2 * BaseFunction(i+1, k-1, u, NodeVector);
end

学习自B站：小黎的Ally

视频链接：路径规划与轨迹跟踪系列算法学习_第8讲_B样条曲线法_哔哩哔哩_bilibili