Posted on

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

Leave a Reply

Your email address will not be published. Required fields are marked *