## Bezier(贝塞尔)曲线(三阶)的轨迹规划在自动驾驶中的应用(四)

``````clc
clear all
p0 = [ 0, 0];

for i = 1:10
j =10;
p3(i*j-9, 1 ) = 19+i;
p3(i*j-8, 1 ) = 19+i;
p3(i*j-7, 1 ) = 19+i;
p3(i*j-6, 1 ) = 19+i;
p3(i*j-5, 1 ) = 19+i;
p3(i*j-4, 1 ) = 19+i;
p3(j*i-3, 1 ) = 19+i;
p3(j*i-2, 1 ) =19+i;
p3(j*i-1, 1) = 19+i;
p3(j*i, 1 ) = 19+i;

p3(i*j-9, 2 ) = 2;
p3(j*i-8, 2 ) = 2.2;
p3(j*i-7, 2) =2.4;
p3(j*i-6, 2) = 2.6;
p3(j*i-5, 2 ) = 2.8;
p3(i*j-4, 2 ) = 3;
p3(j*i-3, 2 ) = 3.2;
p3(j*i-2, 2) =3.4;
p3(j*i-1, 2) = 3.6;
p3(j*i, 2 ) = 3.8;
end

p1(:,1) = p3(:,1)/2;
p1(:,2) = 0;
p2(:,1) = p1(:,1);
p2(:,2) = p3(:,2);

for i = 1:length(p3)
P0 = p0;
P1 = p1(i,:);
P2 = p2(i,:);
P3=  p3(i,:);
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: length(p)
plot (p(i,1),p(i,2),'bo')

hold on
%  drawnow
end
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,'-o')

%  figure
%  for i = 1: length(p)
%  plot (p(i,1),p(i,2),'-o')
%  hold on
%  drawnow
%  end
``````