Friday, February 12, 2010
My approach for motor control  spline segments
On the message boards, I talk about using spline segments instead of linear segments on RepRap. While I am still working out the firmware, I wanted to document the technique as I have had a few requests for how I might do 'complicated' spline curves in firmware without using too many cpu cycles or multiplications or what have you.
It basically boils down to an iterative, stepwise approach. I allow the host to perform all of the expensive calculations beforehand, and then pass off and buffer a set of integral value 'stepwise splines' onto the firmware. As you iterate the computation, the values follow a particular cubic spline at each step.
On the host side, you have some curve through 5D space that you want to approximate:
 x(t) = some function that describes the position of the X axis at time t.
 y(t) = some function that describes the position of the Y axis at time t.
 z(t) = some function that describes the position of the Z axis at time t.
 e(t) = some function that describes the 'position' of the extrusion motor at time t.
 c(t) = some function that describes the 'position' or tempurature of the extruder at time t.
The 'position' for e(t) and c(t) are abstract position; Taking the derivative of e(t) describes feed rate, and the curve for c(t) will most likely be a constant temperature, so in these cases, a spline curve may not quite fit the problem domain; however, as you will see, the computations do not add a lot of overhead, and it may yield flexibility later, if, for example, you wanted to vary the extrusion rate, or ramp up or down tempurature as you slowly moved the extruder head along some path.
For stepwise spline approximation to work, the 't' time variable must be segmented to the point where you only need the specific positions at t=0, t=1, t=2, t=3, ...; this involves choosing a particular spline step rate per second that will set your maximum spline accuracy. Too high of a value, and the precision required to prevent too great of error becomes a bottleneck. Too low of a value, and you may end up with steps that must be passed into a linear interpolation phase (For example, if x(3) = 10, x(4) = 30, y(3) = 15, y(4) = 3, you need to add bresenham on top of step spline. Not a big deal, but it does increase risk for bugs and code complexity.)
My computations indicate that 48 bit is fine for up to about 20003000 steps per spline segment. A few bits can be saved because accelleration and jerk will never be very high, relatively speaking, and can be simple 32 bit values, sign extending to the 48 bit value where needed. My computations also indicate I should be able to attain more or less 5000 spline steps per second.
Before I go into how we compute the step splines, I want to briefly reinforce the simplicity of the algorithm on the firmware side. I outline the primary firmware loop here:
LOOP FOREVER
Perform comms time slice
start_new_spline = false
if t > number_spline_steps
then
t = 0
number_spline_steps = next number_spline_steps
start_new_spline = true
else
t = t + 1
start_new_spline = false
end if
for axis = 0 to 5
begin
if start_new_spline
axis_vel[axis] = next axis_vel[axis]
axis_acc[axis] = next axis_acc[axis]
axis_jer[axis] = next axis_jer[axis]
end if
axis_pos[axis] += axis_vel[axis]
axis_vel[axis] += axis_acc[axis]
axis_acc[axis] += axis_jer[axis]
target = Get top MSB portion of axis_pos[axis]
if axis is stepper,
then
Step motor in direction defined by sign of target  current; possibly no step if sign is 0.
increment or decrement current defined by the same sign. possibly unchanged if sign is 0.
else if axis is PID control
Set pid target = target computed above.
Execute pid cycle on pid controller.
end if
end loop
END LOOP FOREVER

The comms logic feeds in and buffers values associated with 'next' variables, possibly buffering multiple segements to allow a few short segments to exceed the communication rate, assuming that they are preceeded and followed by a few longer segments. At most, you must perform multiword additions and a few bit shift operations.
On the host side, it will perform some math to compute a standard cubic spline. For the example I'll give below, the curve is one that accelerates across X,Y starting from a standing stop and accelerating to 90cm/minute in a 1mm diagonal span, where one linear step on X or Y should cause the extruder to advance one step for feed rates.. (I do make some assumptions about extruder,motor capability that may be a bit optimistic, everything assumes a motor step yields .1mm motion.)
x(0) = 0, x(1000) = 10, dx(0) = 0, dx(1000) = 0.03
y(0) = 0, y(1000) = 10, dy(0) = 0, dy(1000) = 0.03
z(t) = 10, dz(t) = 0
e(0) = 0, e(1000) = 14.14, de(0) = 0, de(1000) = 0.0424
c(t) = 210, dc(t) = 0
For each axis, I follow the general outline described in my blog post on stepwise splines, and arrive at:
xaxis_pos = 0, xaxis_vel = 43, xaxis_acc = 258, xaxis_jer = 258
yaxis_pos = 0, yaxis_vel = 43, yaxis_acc = 258, yaxis_jer = 258
zaxis_pos = 42,949,672,960 (=10.00 fixed point), zaxis_vel = 0, zaxis_acc = 0, zaxis_jer = 0
eaxis_pos = 0, eaxis_vel = 61, eaxis_acc = 364, eaxis_jer=364
caxis_pos = 901,943,132,160 (=210.00 fixed point), caxis_vel = 0, caxis_acc = 0, caxis_jer = 0

If you run the algorithm above with these initial values, you should find that after 1000 iterations, x and y have moved 10 units each, the extruder has 'moved' 14.14 units. You'll find that the error for the fixed point iterations is about 10 microns.
At the assumed rates of 5000 iterations per second, this spline and motor accelleration occurs in about .2 seconds.
I've had great luck using 6th order polynomials to plan trajectories. I wonder if it may help here.
x(t)=At^5+Bt^4+Ct^3+Dt^2+Et+F
where:
x = intended position
t = relative time of motion segment, 0<t<1
T = total actual time to traverse motion segment
The host software can just A, B, C, D, E, F, and T where T is the number of msec to complete the segment, there are few motions that can't be performed.
The reason I continuously use this process is because the motion is so smooth and fluid. Encapsulated in AF is initial and final position, velocity, acceleration, and jerk. The host software will calc the values so that the last segments x(final)= the next segments x(initial). Same goes for v and a.
The calculation on the firmware side is trivial and fast and the movements are the smoothest you've ever seen.
Chief Geek
I'll take a look at it though, certainly. This idea should just extend cubic spline interpolation into a quintic spline interpolation field, and if the motions you've seen have significantly better performance, it may certainally warrent the investigation.
http://en.wikipedia.org/wiki/Digital_Differential_Analyzer
I'm certainly open to using the current firmware if there is some technique I could use to convert the inverse kinematics for the dual disk design so I can get straight lines to print out.
The advantage for using the 3rd order splines instead of 1st order linear segments to approximate curves is simply accuracy for the same amount of data. This spline design requires 2x to 3x more data per segment, but, in the RepolaRap design, will require far fewer segments for a straight line in model space (E.G, the straight line in the model space is the inverse kinemetic solution for x = cos(a) + cos(a+b), y = sin(a) + cos(a+b); needless to say, it is not a simple matter of simply applying a series of first order differential equations, as far as I am aware?)
Specifically, I mean to suggest that for motion along a constant line, I can change another axis at a nonconstant rate more easily with a spline curve than with a line segment description of the curve. For example, suppose you wanted to print a line from 0 to 100, but you wanted the thickness of the fillament to start out at 1mm, but taper off near the end to .1 mm. In order to accomplish this, you could send multiple GCode commands to change the extruder rate as you approach the end (I.E, you've approximated the curve thru 5D space with a sequence of linear segments), vs. sending the reduced number of spline segments that taper it using the curvature of a 3rd degree polynomial over time.
Did I misinterpret your statement about DDA as to being something other than linking two axies together to create constant motion, vs. the nonconstant motion I was suggesting? Or am I still confused?
So are you saying that even with a Cartesian bot and only straight lines coming from the STL, that the splines method has advantages?
I can see that you need it for the RepOla as nothing is linear.
There may not be any significant advantage on a straight linear platform, with current extrusion limitations being the primary bottleneck. The potential I suggested would be one of future development possibility
I see only two potential advantages to using splines on a linear platform for linear segments:
1. the possibility of limiting and calculating motor accelerations thru corners that more accurately match the physical limitations of inertia and motor accelerations, and building that directly into the path equation. Since bottleneck now seems to be extruder related, this does not have any advantage for current linear platforms.
2. The possibility of reducing the traffic if one or more axies change in a nonlinear fashion; the only example I can think of that makes sense would be, for example, printing a long sharp point, where you want to reduce the rate of extrusion as you approach the point, and then increase it again as you return to the base. This can also be done on the linear side with adaption, so again, the advantage may simply be one of having intrinsic ability vs having to add another component to the DDA engine (to manage rate over time instead of position over time, effectively changing it from a 1st degree basis to a 2nd degree  as mentioned, the cubic splines give you 3rd degree specification  initial position, initial velocity, and change in accelleration..)
If these advantages do not exist, then this spline firmware indeed only really has a place in the nonlinear platforms. Using cubic splines would more likely reduce performance because of the extra overhead, compared to something like Triffid_Hunter's rewrite. Also, there will be inherent greater complexity in the code running on the host, so even if there are firmware advantages on linear platforms, if you look at the entire toolchain, the spline solution may be overkill and greater risk for having bugs.
Until I get something working though, I won't be able to give a more complete analysis in terms of comparing code size, performance, steps per second, etc. Hopefully, something I'll be able to do within the next week.
I think the output of the extruder has exponential curves in response to a step change in input. The flow rate is proportional to pressure, pressure is proportional to how much plastic is squashed into in the melt zone and the rate of change of that is the input flow rate minus the output flow. I think that makes it a first order differential equation.
By having very fast reverse and fast forward again I have made it close to a step response, but it can never be a perfect step, so the motors should start and stop with exponential acceleration to suit.
I think the mathematical model for the extruder output would not be very complex but will have a time constant dependent on the plastic springiness and viscosity. I don't know how we would calculate or measure that.
No iteration necessary, just direct calculation of AF. The inputs to the equations are inital position & velocity and final position & velocity. Then AF pop out the other end.
I've used this on bridgeport style vertical mills and gantries and we were able to increase the throughput of the machines substantially. While most motion control systems seem to be acceleration/torque limited, mechanical stiffness plays a huge roll and the 5th order polynomial method smooths out the motion and vibration dramatically. Also, when making simple moves, AC usually drop out.
I've prototyped an arduinomega 168 based DC servo amplifier that works really well, 250 usec loop closure. AF + T input, encoder feedback.
The way you solve the cumulative error is to choose T to land on some known increment like on an even msec and since you've defined x_final, v_final, and a_final of each segment, you know exactly where you are. Also I always use x as absolute position. Haven't gotten lost yet.
Chief Geek
as some might have noticed on the DeltaRepRap forum, i'm implementing a 3rd degree polynomials approximation of the movement and transformation needed in the Delta robot. I managed, with some basic math, to get quite accurate results at high speeds. Hopefully it'll see the delta reprap svn repository soon.
Why 3rd ? well it already approximated quite well the paths needed even with acceleration taken into account etc. 5th or 6th might do but i think most of the parameters will have the 5th or 6th component empty. It would be nice to check it though.

(E.G, the straight line in the model space is the inverse kinemetic solution for x = cos(a) + cos(a+b), y = sin(a) + cos(a+b);

now that's math, but to get an easy solution could be tricky ;) but interesting.
I ran the numbers thru the 5th degree polynomial. To use the technique I use for the cubic, using fixed point and addition only, the bit fractional portion must be expanded to 64 bits to get any kind of precision at all for large numbers of steps (1000+).
So it appears the 5th degree, while a good candidate if you have fast floating point available, does not seem as viable for AVR based firmware, at least, not without extra code to take samples along the time axis, and then executing a bresenham like algorithm between those samples (linear interpolation.)
However, one possible workaround to this would be to simply approximate the quintic formulas with several cubics. Simply subdivide until all samples are within specified error tolerence to the quintic. I suspect you may only need at most 5 or 6 cubic spline segments to achieve each quintic  trading off host complexity and bandwidth for execution speed and simplicity on the firmware side.
Links to this post:
<< Home