Linear Interpolation vs Trapezoid Motion Interpolation
As mentioned in the DrawBot Overview, Linear interpolation
[sourcecode]Position = Here + (There – Here) * time_passed/time_total
Time = ( There – Here ) / vmax[/sourcecode]
is great for simulating movement from Here to There. Unfortunately the real world has things like torque and momentum to deal with. A motor can only jump from zero to full speed successfully if vmax is really slow. Wouldn’t it be great if our robots could accelerate to maximum speed, cruise along, and then slow down at the right moment to stop just where we want? How about if it were easy for the computer to calculate, too? Trapezoid interpolation is the answer.
In the above image, red is acceleration, blue is velocity, and green is position. Note that they are not to scale: position in the trapezoid image would go off the top of the picture.
Calculating these is a little bit trickier it’s still high school level. Remember how
[sourcecode]V = AT;
P = V0t + ATT/2;[/sourcecode]
? That means we can say
[sourcecode]function trapezoidInterpolate(distance,v0,v3,vmax,a,t) {
// assumes t0=0
t1 = (vmax-v0) / a; // time from v0 to vmax (time to reach full speed)
t4 = (max-v3) / a; // time from vmax to v3 (time to brake)
d1 = v0*t1 + 0.5*a*t1*t1; // distance t0-t1
d2 = v3*t4 + 0.5*a*t4*t4; // distance t2-t3
if( d1+d2 < distance ) {
// plateau at vmax in the middle
tplateau = ( distance – d1 – d2 ) / vmax;
t2 = t1 + tplateau;
t3 = t2 + t4;
} else {
// start breaking before reaching vmax
// http://wikipedia.org/wiki/Classical_mechanics#1-Dimensional_Kinematics
t1 = ( sqrt( 2.0*a*brake_distance + v0*v0 ) – v0 ) / a;
t2 = t1;
t3 = t2 + ( sqrt( 2.0*a*(distance-brake_distance) + v3*v3 ) – v3 ) / a;
}
if(t<t1) {
return v0*t + 0.5*a*t*t;
}
if(t<t2) {
up = v0*t1 + 0.5*a*t1*t1;
plateau = vmax*(t-t1);
return up+plateau;
}
if(t<t3) {
up = v0*t1 + 0.5*a*t1*t1;
plateau = vmax*(t2-t1);
t4=t-t2;
v2 = accel * t1;
down = v2*t4 + 0.5*a*t4*t4;
return up+plateau+down;
}
return distance;
}[/sourcecode]
The best part? The rest of our code remains unchanged!
References:
Example implementation
GRBL CNC driver for Arduino
Dimensional Kinematics
SUVAT equations of motion
Bresenhan’s line algorithm
Special thanks:
robbat2 @ Vancouver Hack Space
Thank you for your post,
It’s really interesting, but can I know how do you get brake distance?
I simply don’t understand how the part where you start braking before reaching vmax works. I understand the need for that, but I can’t see that brake distance and neither I do understand the way you are getting the time.
Thank you very much,
GCH.
I assume acceleration and deceleration take the same amount of time. So if you know acceleration from 0 to vMax takes N steps and you have total steps T then you can check if( 2N > T ) { accelerate = decelerate = T/2; }