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

提供动态轨迹的Bezier轨迹&＃xff0c;计算cost minimization 后取cost最小值

clcclear allp0 &＃61; [ 0, 0];
% p1 &＃61; [7.5,7.5*4/30];
% p2&＃61; [15,0];
% p3 &＃61; [15,4];
% p4 &＃61; [22.5,22.5*4/30];
% p5 &＃61; [30,4];for i &＃61; 1:10j &＃61;10;p5(i*j-9, 1 ) &＃61; 19&＃43;i;p5(i*j-8, 1 ) &＃61; 19&＃43;i;p5(i*j-7, 1 ) &＃61; 19&＃43;i;p5(i*j-6, 1 ) &＃61; 19&＃43;i;p5(i*j-5, 1 ) &＃61; 19&＃43;i;p5(i*j-4, 1 ) &＃61; 19&＃43;i;p5(j*i-3, 1 ) &＃61; 19&＃43;i;p5(j*i-2, 1 ) &＃61;19&＃43;i;p5(j*i-1, 1) &＃61; 19&＃43;i;p5(j*i, 1 ) &＃61; 19&＃43;i;p5(i*j-9, 2 ) &＃61; 2;p5(j*i-8, 2 ) &＃61; 2.2;p5(j*i-7, 2) &＃61;2.4;p5(j*i-6, 2) &＃61; 2.6;p5(j*i-5, 2 ) &＃61; 2.8;p5(i*j-4, 2 ) &＃61; 3;p5(j*i-3, 2 ) &＃61; 3.2;p5(j*i-2, 2) &＃61;3.4;p5(j*i-1, 2) &＃61; 3.6;p5(j*i, 2 ) &＃61; 3.8;
endp1(:,1) &＃61; p5(:,1)/4;
p1(:,2) &＃61; 0;
p2(:,1) &＃61; p5(:,1)/2;
p2(:,2) &＃61; 0;
p3(:,1) &＃61; p2(:,1);
p3(:,2) &＃61; p5(:,2);
p4(:,1) &＃61; p5(:,1)*3/4;
p4(:,2) &＃61; p5(:,2);for i &＃61; 1:length(p5)P0 &＃61; p0;P1 &＃61; p1(i,:);P2 &＃61; p2(i,:);P3&＃61; p3(i,:);P4&＃61; p4(i,:);P5&＃61; p5(i,:);for u &＃61;0: 0.01:1p(i,:)&＃61; (1-u)^5*P0 &＃43; 5*(1-u)^4*u*P1 &＃43; 10*(1-u)^3*u^2*P2 &＃43;...10*(1-u)^2*u^3*P3 &＃43;5*(1-u)*u^4*P4 &＃43; u^5*P5;i &＃61;i&＃43;1;endfor i &＃61; 1: length(p)plot (p(i,1),p(i,2),&＃39;&＃43;r&＃39;)hold onendend

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

蓝色的为三阶曲线&＃xff0c;红色的为五阶曲线。