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

本文介绍贝塞尔五阶曲线最为curve smoother的使用，分析关于时间的一阶和二阶导数的连续性，以及曲率的连续性，并给出一个smoother的实例进行效果展示。

clc
 clear all
  p0 = [ 0, 0];
 p1 = [4 4*sqrt(3)];
 p2= [5.5 5.5*sqrt(3)];
 p3 = [8.5 5.5*sqrt(3)];
 p4 = [ 10 4*sqrt(3)];
 p5 = [14 0];
  %生成五阶贝塞尔曲线的笛卡尔坐标系点位
  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(2:100,pd(2:100));
 xlabel('sampling numbers')
ylabel('first derivatives')
title('First Derivative Continuity')
set(gca,'LineWidth',2,'FontSize',11,'FontWeight','normal','FontName','Times');
grid on;
legend('First Derivatives');


figure
 plot(2:100, pdd(2:100));
 xlabel('sampling numbers')
ylabel('second derivatives')
title('Second Derivative Continuity')
set(gca,'LineWidth',2,'FontSize',11,'FontWeight','normal','FontName','Times');
grid on;
legend('Second Derivatives');

 for i  = 1:101
     k(i) = (pdd(i))/(1+pd(i)^2)^(1.5);
 end
 figure
 plot(2:100,k(2:100))
 xlabel('sampling numbers')
ylabel('kappa(1/m)')
title('Curvature Continuity')
set(gca,'LineWidth',2,'FontSize',11,'FontWeight','normal','FontName','Times');
grid on;
legend('Curvature');

%%
 figure
 for i = 1: length(p)
 plot (p(i,1),p(i,2),'-bo')

  hold on

 end
 
 for x = 0:0.1:7
     y = sqrt(3)*x;
     plot(x,y,'+k')
   
     hold on
 end
 
 for x = 7:0.1:14
          y = -sqrt(3)*x + 14*sqrt(3);
     plot(x,y,'+k')
     hold on
 end
 
% legend('Generated Bezier Curve','Control Points','Original Centerline');

 plot(p0(1),p0(2),'-sr',p1(1),p1(2),'-sr',p2(1),p2(2),'-sr',p3(1),p3(2),'-sr',p4(1),p4(2),'-sr',p5(1),p5(2),'-sr')

   
xlabel('x(m)')
ylabel('y(m)')
title('Path Generation with Bezier Curve')
set(gca,'LineWidth',2,'FontSize',11,'FontWeight','normal','FontName','Times');

grid on;

效果图如下：