提供动态轨迹的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
五阶和三阶贝塞尔在采样过程中覆盖的区域基本相同,但是由于五阶曲线本身在连续性上的性质更好一些,所以更加推荐五阶曲线。
蓝色的为三阶曲线,红色的为五阶曲线。