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

提供动态轨迹的Bezier轨迹，计算cost minimization 后取cost最小值

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