Question about S-curve acceleration and deceleration control
The acceleration equation of a five-phase s-curve used is: $$ a(t) = \begin{cases}\ Jt&t=[0, T_m] \\ 2JT_m-Jt&t=(T_m, 2T_m]\\ 0&t=(2T_m, 2T_m+T_3]\\ 2JT_m - J(t-T_3)&t=(2T_m+T_3, 3T_m+T_3]\\ -4JT_m + J(t-T_3)&t=(3T_m+T_3, 4T_m+T_3] \end{cases}$$ And the speed equation of a five-phase s-curve used is: $$ v(t) = \begin{cases}\ v_s + \frac12Jt^2&t=[0, Tm]\\ v_s - JT_m^2 + 2JT_mt - \frac12Jt^2&t=(T_m, 2T_m]\\ V&t=(2T_m, 2T_m+T_3]\\ v_s-JT_m^2+2JT_m(t-T_3)-\frac12J(t-T_3)^2&t=(2T_m+T_3, 3T_m+T_3]\\ v_s-4JT_m^2+3JT_m(t-T_3)-\frac12J(t-T_3)^2&t=(3T_m+T_3, 4T_m+T_3] \end{cases} $$ where, $J$ is Jerk, $v_s$is the initial speed, $V$is the target speed,$T_m$is half of the acceleration time, and here the acceleration time is equal to the deceleration time.$T_3$ is the time of constant speed.
These equations are copied from a paper, and I think they are correct, but when I implemented these equations in Matlab, I didnot get the result as I expected.the speed curve I got is as follows:
From the picture above, you can see that the deceleration part is a little weird, it should be a s-curve as the acceleration part, but in a symmetrical way。Just like the following picture shows:
The matlab code is as follows:
L = 40; %Distance
J = 100; %mm/s^3
vs = 2.7;
ve = 2.7;
v = 20; %target speed
vout = zeros(2000, 1);
t = 0;
t3 = 0;
tm = sqrt((v - vs)/J);
sa = (v + vs) * tm; %acceleration distance
if L > 2*sa t3 = (L - 2*sa) / v;
else x1 = L * sqrt(J) / 4; x2 = sqrt(L^2*J/16 + 8*vs^3/27); tm = ((x1 + x2)^(1/3) + (x1 - x2)^(1/3))/(sqrt(J)); v = J*tm^2 + vs;
end
total = 4*tm+t3;
n = 1;
for t = 0:0.008:total if t >= 0 && t <= tm vout(n) = vs + J*t^2/2; elseif t > tm && t <= 2*tm vout(n) = vs - J*tm^2 + 2*J*tm*t-J*t^2/2; elseif t> 2*tm && t <= (2*tm+t3); vout(n) = v; elseif t > (2*tm+t3) && t <= (3*tm + t3); vout(n) = vs - J*tm^2 + 2*J*tm*(t-t3) - 0.5*J*(t-t3)^2; elseif t > (3*tm + t3) && t <= (4*tm+t3) vout(n) = vs - 4*J*tm^2 + 3*J*tm*(t-t3) - 0.5*J*(t-t3)^2; end n = n+1;
end
plot(0:0.008:total, vout(1:n-1));I am really confused why does this happen, and I don't find where the problem is? if there is anyone who is familiar with this, could you help me out? Thank you.
$\endgroup$ 71 Answer
$\begingroup$As noted in the comments to the question, the last segment should just be a mirror image of the first. Taking the first velocity segment, $v_s+\frac12Jt^2$, we see that this is an upward-pointing parabola with vertex on the $y$-axis, so the segment of this function for $t\in[-T_m,0]$ is a mirror image of the first segment. We then need to shift it right so that it ends at $t=4T_m+T_3$ instead of at $t=0$, which we do by making the substitution $t\to t-(4T_m+T_3)$, i.e., the last velocity segment is given by $$v_s+\frac12J(t-4T_m-T_3)^2.\tag{*}$$ With this correction, the velocity plot looks like what one might expect:
