根据(一)中的理论前提,我们通过matlab进行一次仿真验证:
clc
clear all
p0 = [ 0, 0];
p1 = [10,0.5];
p2= [20,3.5];
p3 = [30,4];
%设置控制点
i =1;
for u =0: 0.01:1
p(i,:)= (1-u)^3*p0 + 3*(1-u)^2*u*p1 + 3*(1-u)*u^2*p2 + u^3*p3;
i =i+1;
end
%生成三阶贝塞尔曲线的笛卡尔坐标系点位
for i = 1:100
pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));
pd(101) = 0;
end
%计算一阶导数
for i =2: 100
pdd(1) = 0;
pdd(101) = 0;
pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;
end
%计算二阶导数
figure
plot(1:101,pd);
figure
plot(1:101, pdd);
for i = 1:101
k(i) = (pdd(i))/(1+pd(i)^2)^(1.5);
end
figure
plot(1:101,k)
figure
for i = 1: length(p)
plot (p(i,1),p(i,2),'-o')
hold on
drawnow
end
%%
clc
clear all
p0 = [ 0, 0];
p1 = [7.5,0];
p2= [15,0];
p3 = [15,4];
p4 = [22.5,4];
p5 = [30,4];
%生成五阶贝塞尔曲线的笛卡尔坐标系点位
i =1;
for u =0: 0.01:1
p(i,:)= (1-u)^5*p0 + 5*(1-u)^4*u*p1 + 10*(1-u)^3*u^2*p2 +...
10*(1-u)^2*u^3*p3 +5*(1-u)*u^4*p4 + u^5*p5;
i =i+1;
end
for i = 1:100
pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));
pd(101) = 0;
end
%计算一阶导数
for i =2: 100
pdd(1) = 0;
pdd(101) = 0;
pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;
end
%计算二阶导数
figure
plot(1:101,pd);
figure
plot(1:101, pdd);
for i = 1:101
k(i) = (pdd(i))/(1+pd(i)^2)^(1.5);
end
figure
plot(1:101,k)
figure
for i = 1: length(p)
plot (p(i,1),p(i,2),'-o')
hold on
drawnow
end
三次和五次曲线的形状是有差别的:
进一步的,分析曲线曲率,可以看出五次曲线的连续性更加平缓:
三次曲线
五次曲线
对于车辆来说,平滑的曲率意味着平滑的车辆转角控制,也意味着平滑的方向盘转角控制。因此,在变道路径规划中,更推荐 使用五阶贝塞尔曲线。