一、 Trajectories with Constant Acceleration and Cycloidal/Cubic Blends
clc;
clear;
close all;
T = [0.125, 0.25, 0.125, 0.1, 0.125, 0.25, 0.125]'; %7段时间
vs = 10;
ve = 20;
qs = 10;
qe = 60;
dt = 0.001;
curveType = 1; %0:Cycloidal; 1:Cubic
m = zeros(16, 16);
if curveType == 0
m(1, 1) = -2.0 * T(1) / pi;
m(2, 1) = 0.0;
m(9, 1) = 0.0;
m(10, 1) = - (2.0 * T(1) / pi)^2;
m(3, 1) = T(2);
m(4, 1) = 2.0 * T(3) / pi;
m(11, 1) = T(2)^2 / 2.0 + (2.0 * T(3) / pi)^2;
m(12, 1) = 0.0;
m(5, 2) = 2.0 * T(5) / pi;
m(6, 2) = 0.0;
m(13, 2) = 0.0;
m(14, 2) = -(2.0 * T(5) / pi)^2;
m(7, 2) = T(6);
m(8, 2) = 2.0 * T(7) / pi;
m(15, 2) = T(6)^2 / 2.0 + (2.0 * T(7) / pi)^2;
m(16, 2) = 0.0;
else
m(1, 1) = 0.0;
m(2, 1) = T(1) / 2.0;
m(9, 1) = 0.0;
m(10, 1) = T(1)^2 / 6.0;
m(3, 1) = T(2);
m(4, 1) = T(3) / 2.0;
m(11, 1) = T(2)^2 / 2.0;
m(12, 1) = T(3)^2 / 3.0;
m(5, 2) = 0.0;
m(6, 2) = T(5) / 2.0;
m(13, 2) = 0.0;
m(14, 2) = T(5)^2 / 6.0;
m(7, 2) = T(6);
m(8, 2) = T(7) / 2.0;
m(15, 2) = T(6)^2 / 2.0;
m(16, 2) = T(7)^2 / 3.0;
end
m(1, 3) = 1.0;
for i = 2 : 7
m(i, i + 1) = 1.0;
m(i, i + 2) = -1.0;
end
m(8, 9) = 1.0;
m(9, 10) = 1.0;
for i = 10 : 15
m(i, i) = 1.0;
m(i, i + 1) = -1.0;
end
k = 1;
j = 3;
for i = 10 : 16
m(i, j) = T(k);
j = j + 1;
k = k + 1;
end
m(16, 16) = 1.0;
b = zeros(16, 1);
b(1) = vs;
b(8) = ve;
b(9) = qs;
b(16) = qe;
x = m \ b;
a1 = x(1);
a2 = x(2);
k11 = x(3);
k21 = x(4);
k31 = x(5);
k41 = x(6);
k51 = x(7);
k61 = x(8);
k71 = x(9);
k12 = x(10);
k22 = x(11);
k32 = x(12);
k42 = x(13);
k52 = x(14);
k62 = x(15);
k72 = x(16);
posArray = [];
velArray = [];
accArray = [];
jerkArray = [];
t = cumsum([0; T]);
tArray = (t(1) : dt : t(end))';
if curveType == 0
for i = 1 : length(tArray)
tt = tArray(i);
if tt < t(2)
pos = -a1 * (2.0 * T(1) / pi)^2 * sin(pi * (tt - t(1)) / (2.0 * T(1))) + k11 * (tt - t(1)) + k12;
vel = -a1 * (2.0 * T(1) / pi) * cos(pi * (tt - t(1)) / (2.0 * T(1))) + k11;
acc = a1 * sin(pi * (tt - t(1)) / (2.0 * T(1)));
jerk = a1 * (pi / (2.0 * T(1))) * cos(pi * (tt - t(1)) / (2.0 * T(1)));
elseif tt < t(3)
pos = a1 * (tt - t(2))^2 / 2.0 + k21 * (tt - t(2)) + k22;
vel = a1 * (tt - t(2)) + k21;
acc = a1;
jerk = 0.0;
elseif tt < t(4)
pos = -a1 * (2.0 * T(3) / pi)^2 * cos(pi * (tt - t(3)) / (2.0 * T(3))) + k31 * (tt - t(3)) + k32;
vel = a1 * (2.0 * T(3) / pi) * sin(pi * (tt - t(3)) / (2.0 * T(3))) + k31;
acc = a1 * cos(pi * (tt - t(3)) / (2.0 * T(3)));
jerk = -a1 * (pi / (2.0 * T(3))) * sin(pi * (tt - t(3)) / (2.0 * T(3)));
elseif tt < t(5)
pos = k41 * (tt - t(4)) + k42;
vel = k41;
acc = 0.0;
jerk = 0.0;
elseif tt < t(6)
pos = -a2 * (2.0 * T(5) / pi)^2 * sin(pi * (tt - t(5)) / (2.0 * T(5))) + k51 * (tt - t(5)) + k52;
vel = -a2 * (2.0 * T(5) / pi) * cos(pi * (tt - t(5)) / (2.0 * T(5))) + k51;
acc = a2 * sin(pi * (tt - t(5)) / (2.0 * T(5)));
jerk = a2 * (pi / (2.0 * T(5))) * cos(pi * (tt - t(5)) / (2.0 * T(5)));
elseif tt < t(7)
pos = a2 * (tt - t(6))^2 / 2.0 + k61 * (tt - t(6)) + k62;
vel = a2 * (tt - t(6)) + k61;
acc = a2;
jerk = 0.0;
else
pos = -a2 * (2.0 * T(7) / pi)^2 * cos(pi * (tt - t(7)) / (2.0 * T(7))) + k71 * (tt - t(7)) + k72;
vel = a2 * (2.0 * T(7) / pi) * sin(pi * (tt - t(7)) / (2.0 * T(7))) + k71;
acc = a2 * cos(pi * (tt - t(7)) / (2.0 * T(7)));
jerk = -a2 * (pi / (2.0 * T(7))) * sin(pi * (tt - t(7)) / (2.0 * T(7)));
end
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
jerkArray = [jerkArray; jerk];
end
if abs(tArray(end) - t(end)) > 1.0e-8
tArray = [tArray; t(end)];
posArray = [posArray; qe];
velArray = [velArray; ve];
accArray = [accArray; 0.0];
jerkArray = [jerkArray; -a2 * (pi / (2.0 * T(7)))];
end
else
for i = 1 : length(tArray)
tt = tArray(i);
if tt < t(2)
pos = (a1 / (6.0 * T(1))) * (tt - t(1))^3 + k11 * (tt - t(1)) + k12;
vel = (a1 / (2.0 * T(1))) * (tt - t(1))^2 + k11;
acc = (a1 / T(1)) * (tt - t(1));
jerk = a1 / T(1);
elseif tt < t(3)
pos = a1 * (tt - t(2))^2 / 2.0 + k21 * (tt - t(2)) + k22;
vel = a1 * (tt - t(2)) + k21;
acc = a1;
jerk = 0.0;
elseif tt < t(4)
pos = -(a1 / (6.0 * T(3))) * (tt - t(3))^3 + (a1 / 2.0) * (tt - t(3))^2 + k31 * (tt - t(3)) + k32;
vel = -(a1 / (2.0 * T(3))) * (tt - t(3))^2 + a1 * (tt - t(3)) + k31;
acc = -(a1 / T(3)) * (tt - t(3)) + a1;
jerk = -(a1 / T(3));
elseif tt < t(5)
pos = k41 * (tt - t(4)) + k42;
vel = k41;
acc = 0.0;
jerk = 0.0;
elseif tt < t(6)
pos = (a2 / (6.0 * T(5))) * (tt - t(5))^3 + k51 * (tt - t(5)) + k52;
vel = (a2 / (2.0 * T(5))) * (tt - t(5))^2 + k51;
acc = (a2 / T(5)) * (tt - t(5));
jerk = a2 / T(5);
elseif tt < t(7)
pos = a2 * (tt - t(6))^2 / 2.0 + k61 * (tt - t(6)) + k62;
vel = a2 * (tt - t(6)) + k61;
acc = a2;
jerk = 0.0;
else
pos = -(a2 / (6.0 * T(7))) * (tt - t(7))^3 + (a2 / 2.0) * (tt - t(7))^2 + k71 * (tt - t(7)) + k72;
vel = -(a2 / (2.0 * T(7))) * (tt - t(7))^2 +a2 * (tt - t(7)) + k71;
acc = -(a2 / T(7)) * (tt - t(7)) + a2;
jerk = -(a2 / T(7));
end
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
jerkArray = [jerkArray; jerk];
end
if abs(tArray(end) - t(end)) > 1.0e-8
tArray = [tArray; t(end)];
posArray = [posArray; qe];
velArray = [velArray; ve];
accArray = [accArray; 0.0];
jerkArray = [jerkArray; -(a2 / T(7))];
end
end
figure(1)
subplot(4, 1, 1)
plot(tArray, posArray);
xlabel('t');
ylabel('pos');
if curveType == 0
title('Trajectories with Constant Acceleration and Cycloidal Blends');
else
title('Trajectories with Constant Acceleration and Cubic Blends');
end
subplot(4, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('vel');
subplot(4, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('acc');
subplot(4, 1, 4)
plot(tArray, jerkArray);
xlabel('t');
ylabel('jerk');
二、参考文献
Trajectory Planning for Automatic Machines and Robots中章节：3.11 Trajectories with Constant Acceleration and Cycloidal/Cubic Blends