Motion Profiling

Paul’s code is pretty much psuedocode, I was able to code something up that ran off what he posted pretty quickly.

I’ve played around with the excellent Excel spreadsheet-simulators on the previous page, but I still have a couple more questions. Namely, in the way Paul proposed of designing a motion profile (the “double filter” scheme), how do the filters work, exactly? I see that the parameters T1 and T2 modify the filters, and I see that the filters’ role appears to be to shape the motion profile (defining the acceleration and deceleration especially).


You can think of a linear filter as a damper. It takes instantaneous change and spreads that change out over the length of the filter. In the case of this particular filter, it takes a step instantaneous velocity (target velocity) and spreads it out over the filter length.

The double filter just smooths out the velocity transitions a l little more. It just takes the output from the first filter and filters that.

The implementation is pretty simple. You have two loops (while loops, or whatever suits your style). The outer loop runs until the filter is empty (contains all zeros). The inner loop runs based on Npts that is calculated based on the path distance and programmed velocity. This is basically how many times you input a “1” into the filter.

So, you run the planning algorithm until the filter is completely empty, inside that filter you add a 1 as the filter input for Npts. Each outer loop you shift each filter position to the next filter position.

The Excel file demonstrates this exactly.

F1 is a low-pass FIR filter that imposes a rate limit on the input (where the input in this case is a “go full speed ahead” speed command). As a result, an acceleration limit is put on the output. The T1 parameter of F1 is the amount of time it takes to accelerate from 0 to full speed.

F2 is another low-pass FIR filter that imposes a rate limit on its input (which is the output of F1). As a result, a jerk (derivative of acceleration) limit is put on the output. The T2 parameter of F2 is the amount of time it takes to increase acceleration from 0 to the maximum defined by T1.

You could in theory add an F3 or even more successive FIR filters and achieve smoothness down to an arbitrary order.

Here are some examples…

If T1 is equal to 400ms and T2 is equal to 0 (use 1ms in the spreadsheet to avoid divide by zero), you get an acceleration-limited velocity, but there are no limits on the jerk (you immediately go to maximum acceleration).

If T1 is equal to 400ms and T2 is equal to 400ms, it takes the same amount of time to reach the maximum acceleration as velocity, so you get a triangular profile (as soon as you hit maximum acceleration it is immediately time to reduce acceleration)

If T1 is equal to 400ms and T2 is equal to 200ms (or any number 0 < T2 < 400ms), you spend a non-zero amount of time at your maximum acceleration before reducing acceleration.

How would this algorithm deal with changes in Vprog dynamically?

If another algorithm would like to impose velocity limits dynamically, would simply setting Vprog to the imposed velocity limit allow me to impose the limit, and would the trapezoid deal with it? Or do I need something else?

I suspect the short answer is “not well”, since Vprog is used both to scale the output velocity, and to determine how long to send the “full speed ahead” input. If you were to also scale T1 and T2 when you change Vprog it may work better (but I’d need to think that through a bit more).

By the way, I found a small error in my spreadsheet. In the Output Velocity column, the equation should read “=E13/$B$8*Vprog” for row 13 (for example). I was erroneously adding the sum of F1 to the result as well, resulting in brief discontinuities during edge conditions. Make the change for the whole column.

Ok, I had allot of posts to catch up on here… So I have been playing with the spreadsheet, which seems very nice and computationally simple, but for my application the 254 code is better as it allows real time adjustment of max velocity, acceleration etc.

As mentioned in my previous post I ported the ContinuousAccelFilter Class from the 254 code into a Borland Builder app so that I could graph the input output velocity and accel etc in real time. This way I could better understand how things work and make adjustments etc. This is now working very well, and I have a continuous tracked position target that conforms to the acceleration and max velocity, both of which can be changed in real time. However I did notice a small velocity glitch that I eventually tracked down to a negative velocity produced within the “maxAccelTime” function, this I have added an additional condition to the routine to trap this, I have to confess I didn’t quite understand why this was occurring, but it was fairly easy to trap the condition and remove it, and it has not effected the operation of the routine other than to remove the glitch. If anyone is using this code and are interested here is the change I made towards the end of the “maxAccelTime” routine in the “**** new ****” comments:

    //If it is too fast, we now know how long we get to accelerate for and how long to go at constant velocity
    double accel_time = 0;
    if(top_v > max_v)
        accel_time = (max_v - curr_vel) / max_a;
        const_time = (distance_left + (goal_vel * goal_vel - max_v * max_v) / (2.0 * max_a)) / max_v;
        accel_time = (top_v - curr_vel) / start_a;

        //  **** new ****
        //  negative acceleration time caused velocity glitches
        //  when top_v < max_v

        if( accel_time < 0 )
            accel_time = 0;

        // **** new ****

The next issue I have is making this work for three simultaneous axis. I guess I should explain that I intend to use this for a 3DOF arm, and up until now have been using a cubic spline routine to control the end effector X’Y’Z coordinates, however my cubuc spline routine does not have the ability to set acceleration and max velocity, only to move from point to point over a fixed time and thus the end effector although synchronized in all three axis is in a state of constant acceleration or deceleration. Hence my research into trapezoid velocity control.

So now I have a routine working for one axis, any thoughts on how to have three synchronized axis controlled from the same acceleration and max velocity parameters. I’m currently thinking I would have to calculate the three axis separately, then look for the axis that is going to take the longest time to complete the move, and thus reduce the max volocity of the other axis to time stretch them into synchronization… any thoughts??