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

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

clc
 clear all
 
 p0 = [ 0, 0];
%  p1 = [7.5,7.5*4/30];
%  p2= [15,0];
%  p3 = [15,4];
%  p4 = [22.5,22.5*4/30];
%  p5 = [30,4];


for i = 1:10
    j =10;
    p5(i*j-9, 1 ) = 19+i;
    p5(i*j-8, 1 ) = 19+i;
    p5(i*j-7, 1 ) = 19+i;
    p5(i*j-6, 1 ) = 19+i;
    p5(i*j-5, 1 ) = 19+i;
    p5(i*j-4, 1 ) = 19+i;
    p5(j*i-3, 1 ) = 19+i;
    p5(j*i-2, 1 ) =19+i;
    p5(j*i-1, 1) = 19+i;
    p5(j*i, 1 ) = 19+i;
    
    p5(i*j-9, 2 ) = 2;
    p5(j*i-8, 2 ) = 2.2;
    p5(j*i-7, 2) =2.4;
    p5(j*i-6, 2) = 2.6;
    p5(j*i-5, 2 ) = 2.8;
    p5(i*j-4, 2 ) = 3;
    p5(j*i-3, 2 ) = 3.2;
    p5(j*i-2, 2) =3.4;
    p5(j*i-1, 2) = 3.6;
    p5(j*i, 2 ) = 3.8;
end
 
p1(:,1) = p5(:,1)/4;
p1(:,2) = 0;
p2(:,1) = p5(:,1)/2;
p2(:,2) = 0;
p3(:,1) = p2(:,1);
p3(:,2) = p5(:,2);
p4(:,1) = p5(:,1)*3/4;
p4(:,2) = p5(:,2);

for i = 1:length(p5)
    P0 = p0;
    P1 = p1(i,:);
    P2 = p2(i,:);
   P3=  p3(i,:);
   P4=  p4(i,:);
   P5=  p5(i,:);
 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: length(p)
 plot (p(i,1),p(i,2),'+r')
 hold on
 end
 
end

五阶和三阶贝塞尔在采样过程中覆盖的区域基本相同，但是由于五阶曲线本身在连续性上的性质更好一些，所以更加推荐五阶曲线。

蓝色的为三阶曲线，红色的为五阶曲线。