Linear Interpolation vs Trapezoid Motion Interpolation

As mentioned in the Drawbot Overview, Linear interpolation

Position = Here + (There - Here) * time_passed/time_total
Time = ( There - Here ) / vmax

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

V = AT;
P = V0t + ATT/2;

? That means we can say

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;
}

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

3 Responses to “Linear Interpolation vs Trapezoid Motion Interpolation”

  1. nope says:

    Don’t invent interpolation schemes. While relatively easy, other methods are better (e.g. spline), but they still don’t solve the fundamental problem of engines. The “jerk” of the motor should be continuous. Trapezoidal interpolation is continuous only on position and velocity. You need position, velocity, acceleration, and jerk if you want a good motor.

    For god sakes, read
    http://www.pmdcorp.com/news/articles/html/Mathematics_of_Motion_Control_Profiles.cfm

    • Dan says:

      I’m not inventing, billgates. This method is good enough for subtractive and additive manufacturing. For god sakes, read up on GRBL. Jerk & Snap are important and I’ll go more in depth in a future article.

  2. [...] Nice work here from Dan Royer – including a very clear and illuminating write-up of the maths behind hanging-v plotters and this grand acceleration / movement scheme. [...]

Leave a Reply