提供动态轨迹的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