B样条曲线_原理 &用于路径规划的matlab代码

B样条曲线在贝塞尔曲线的基础上有了更进一步的提高。

一 由于贝塞尔曲线存在以下不足：

1 ）缺乏局部修改性，即改变某一控制点对整个曲线都有影响。

2 ）n 较大时，特征多边形的边数较多，对曲线的控制减弱。

3 ）幂次过高难于修改。（而在外形设计中，局部修改是随时要进行的）。

二 所以提出了B样条曲线，B样条曲线有以下优点：

1 ）逼近特征多边形的精度更高。

2 ）多边形的边数与基函数的次数无关。

  3 ）具有局部修改性。
B样条曲线的原理学习可参考下边链接，内容丰富，步骤精细。

https://www.cnblogs.com/icmzn/p/5100761.html

用于路径规划的matlab代码：

1.B_spline_curve.m 主函数

%% 三次B样条曲线
clc;
clear;
close all;

%% 数据定义
road_width=3.5;
% 6个控制点 
P=[0 10 25 25 40 50;
    -road_width/2 -road_width/2 -road_width/2+0.5 road_width/2-0.5 road_width/2 road_width/2];
% 从0开始算 6个控制点 n=5
n=size(P,2)-1;
% K阶 也就是K-1次B样条
k=4;
path=[];
Bik=zeros(n+1,1);
flag=2;
%% 准均匀B样条
if flag==2
    NodeVector=unEven(n,k-1);
    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,1),p_u(2,1)]];
    end  
elseif 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
end
%% 绘图
% 车宽 车长 路长
car_width=1.8;
car_length=4.5;
road_length=50;
%灰色路面区域设置 填充 车辆位置填充
Road_zone=[-5 -road_width-0.5;
           -5 road_width+0.5;
           road_length road_width+0.5;
           road_length -road_width-0.5];
fill(Road_zone(:,1),Road_zone(:,2),[0.5 0.5 0.5]);
hold on
fill([0 0 -car_length -car_length],...
[-road_width/2-car_width/2 -road_width/2+car_width/2 ...
-road_width/2+car_width/2 -road_width/2-car_width/2],'b')

% 绘制道路中间虚线 两侧实线
plot([-5 road_length],[0 0],'w--','linewidth',2);
plot([-5 road_length],[road_width,road_width],'w','linewidth',2);
plot([-5 road_length],[-road_width,-road_width],'w','linewidth',2);

% 横纵坐标长度设置
axis equal
set(gca,'XLim',[-5,road_length]);
set(gca,'YLim',[-road_width-0.5,road_width+0.5]);

scatter(path(:,1),path(:,2),'.b');%绘制路径散点
scatter(P(1,:),P(2,:),'g');
plot(P(1,:),P(2,:),'r');

2.BaseFunction.m 基函数的递归运算.

function Bik_u = BaseFunction(i,k,u,NodeVector)
%基函数    Bik

    if k==0
        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
            Length1=1;
        end
        if Length2 == 0
            Length2 = 1;
        end
        % 递归 Bik_u=（u-u_i）/(u_i+k-u_i)*B_i_k-1_u
        %             +(u_i+k+1-u)/(u_i+k+1-u_i+1)* B_i+1_k-1_u
        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

3.unEven.m 准均匀B样条的节点向量生成

                              节点向量前面和后面各有k+1个重复值 前面是K个0 后面是K个1

function NodeVector = unEven(n,k)
%% 准均匀B样条曲线节点向量计算。传入的n从0开始，所以共n+1个控制顶点。k阶曲线为k-1次。节点向量个数为n+1+k+1=n+k+2
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个重复值 前面是K个0 后面是K个1

end

准均匀的B样条效果图