本代码来源于：路径规划与轨迹跟踪系列算法学习_第8讲_B样条曲线法

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

% B样条曲线法 主函数main.m

%作者:Ally

%日期:2021/2/6

clc

clear

close all

%% 数据定义

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开始计数

k= 4; % k阶、k-1次B样条

flag =2; %1,2分别绘制均匀B样条曲线、准均匀B样条曲线

%% 生成B样条曲线

path=[];

Bik = zeros(n+1,1);

if flag ==1 %均匀в样条

 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+l,1)= BaseFunction(i,k-l,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

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

免责声明：本文系网络转载或改编，未找到原创作者，版权归原作者所有。如涉及版权，请联系删