Motion Profiling

[theNerd]

I've been starting to research into some more advanced techniques for robot controlling and I stumbled upon what's called motion profiling. I looked a little more on this on the internet and I found out a little about the trapezoidal and s-curve profiles. However all I know is the kinematic relationships (velocity, acceleration...) and I'm confused about how to take this to the useful level. Could the awesome robotics community out there help me in learning the basics of motion profiling and how to implement it into a robot design - especially a drive system?

[AustinSchuh]

FYI, 254's code uses trapezoidal motion profiles for all drivetrain moves. If you are willing to dredge through the code, you can see how a team used them last year.

I like to conceptually visualize motion profiles as a filter (even though it is not LTI, so this is an abuse of terminology to me). You tell the filter that you want to go to position "10", and it tells you what to do for the next time step to get closer to "10" while only moving with a maximum acceleration and velocity that you configure the filter with. This is essentially how we implemented it.

I would start by writing code on your local machine to generate motion profiles and debug it by plotting the output to see if it works or not. It is quite hard to debug this type of stuff on a running robot.

[Jared Russell]

To echo what Austin said: This is a great example of something you can develop and test without a running robot. Write the code to generate a trapezoidal motion profile using J2SE or on a desktop using C++, debug using Excel, Matlab, or Gnu plots, and you will be able to port it to your robot code with ease.

Of course, once you are generating your trapezoidal motion profiles, you will need a controller that makes sure the bot actually follows it! Here are a few ideas on how you could do this:

1. Use a PID loop on your drive motors (in distance mode). Use the speed output of your motion profile to limit the maximum command that PID is allowed to send to your motors.

2. Use a PID loop on your drive motors (in speed mode). As long as your speed loop has an integral term (the I gain is nonzero), this should get you where you are going.

3. Use both (1) and (2) to control both speed and distance; there is also a specialized "PIV" controller used in industrial servos that mixes both speed and distance to command the motors.

4. Just use the speed command you generated to drive the motors directly (open loop), but switch to a PID controller when you get close to the goal to go the last few inches.

5. Make a full-state controller using control theory and simulation; if you go this route you are either insane or 254 :)

[AustinSchuh]

Good point Jared. Once you have a profile, you need to follow it.

My favorite controller for following trajectories is as follows (I think this is a modification of 3). I used it before I got tired of hand-tuning the PD for the drivetrain and switched to state feedback.

pwm = P * error + D * ((error - prev_error) / dt - goal_velocity) + Kv * goal_velocity + Ka * goal_acceleration

This is close to a PD controller, but with some feed forward terms. The idea behind feed forwards is that if you have a pretty good idea about how much power it will take to do something, go ahead and add it in. The control loop will take up the slop from there when you are wrong, or someone bumped the bot.

The D term is special in that you subtract off the goal velocity. This falls out from the state feedback controllers. Think about it this way. If you are at the goal, and moving at the right speed, you don't want to apply corrective power to decelerate the robot (This is what D would do if you weren't trying to move but were moving and at the goal.)

The Kv and Ka feed forwards terms make a lot of sense. It takes power to move at a velocity, so add that power on... Same for acceleration. It takes power to accelerate, so just add the power right on. You can get the goal velocity and acceleration from your profile.

A properly tuned controller with motion profiles will produce some very nice and smooth motion.

[winchymatt]

Good point Jared. Once you have a profile, you need to follow it.

My favorite controller for following trajectories is as follows (I think this is a modification of 3). I used it before I got tired of hand-tuning the PD for the drivetrain and switched to state feedback.

pwm = P * error + D * ((error - prev_error) / dt - goal_velocity) + Kv * goal_velocity + Ka * goal_acceleration



Hi,

Realise this is an old thread, but hopefully you will pick this up..

I have been working with PID controllers for a while now, but have never looked at adding feed forward terms. This looks really interesting for a hydraulic system I'm currently working on.

I have a few questions with regards to your controller above:

1) What is your (dt) term within the D part of your controller.

2) Do you determine the goal_velocity and goal_acceleration from the goal position within the controller, or do you send these in from your trajectroy planner?

3) Can you point me to a code example of this controller?

I have a bunch of other questions but figured I would start with this.. :)

Matt.

[AustinSchuh]

DT is the period of your loop.

Acceleration, velocity and position are from the trajectory planner.

254's 2011 code has this as part of the drive train autonomous code. It should be available in the whitepapers on this site.

Austin

[winchymatt]

DT is the period of your loop.

Acceleration, velocity and position are from the trajectory planner.

254's 2011 code has this as part of the drive train autonomous code. It should be available in the whitepapers on this site.

Austin

Great thanks, yes I should have known dt would be discrete time! I checked out the code and the trajectory planner was a great help! Imported the ContinuousAccelFilter class into my borland builder test app, works really well! Thanks for the pointers.

So on a side note, if you were sending just position information to the PID controller, and did not have velocity and acceleration data, could one derive the velocity and acceleration by differentiating the position data with the previous position.. I guess this would not produce a true goal velocity, but may give some feed forward gain??

Matt.

[flameout]

So on a side note, if you were sending just position information to the PID controller, and did not have velocity and acceleration data, could one derive the velocity and acceleration by differentiating the position data with the previous position.. I guess this would not produce a true goal velocity, but may give some feed forward gain??

Matt.

You can, if and only if that signal is twice differentiable. If not, then you'll get bad spikes in your data -- if you have spots that are not twice differentiable, you'll need to differentiate on each side then use each section as a separate input, essentially "cutting out" the bad spots. This is much more useful if the position trajectory is derived via some analytical or numerical process -- you'll likely have a ton of noise if you try to do this with real sensor data.

Also, with full PID, having a feedforward on the velocity target will cause overshoot, just FYI.* If you're doing PID for position control, then having an acceleration feedforward should be fine; a velocity feedforward would be beneficial if you're doing PD.

If anyone disagrees with any of the statements in this post, please correct me -- this is operating at the edge of my knowledge of control theory.

* This statement is incorrect, please see AustinSchuh's post below.

[Paul Copioli]

So on a side note, if you were sending just position information to the PID controller, and did not have velocity and acceleration data, could one derive the velocity and acceleration by differentiating the position data with the previous position.. I guess this would not produce a true goal velocity, but may give some feed forward gain??

Matt.

Matt,

The problem with this method is that discrete differentiation induces noise into the control loop. By using the path planning algorithm you have a pure, non noisy, velocity and acceleration profile.

Paul

[AustinSchuh]

Also, with full PID, having a feedforward on the velocity target will cause overshoot, just FYI. If you're doing PID for position control, then having an acceleration feedforward should be fine; a velocity feedforward would be beneficial if you're doing PD.


I disagree with this statement. I'll give you a fuzzy explanation now, and try to find time to prove it for real with the math later. I'll probably cheat and give you the continuous time proof...

You can model the velocity feed forward as a disturbance. It adds a power to the motors that the control loop is not expecting. This power is constant (at least at constant velocity), so, since the loop is stable, it will stabilize out and converge. This is very similar from the loop's perspective to changing the goal, and since both of them are happening at the same time, it shouldn't have much effect.

Differentiating the goal signal will also introduce delay into it. This won't have a huge effect on the system since the time constant of a robot is very slow compared to a good loop frequency, but this can become more and more pronounced as your system's time constant gets closer to the period of your loop.

[Paul Copioli]

I agree with Austin on this one. Velocity feed forward (and acceleration FF, and current FF) all work to stabilize the PID loop. Instead of your proportional gain having to do all the heavy lifting, a small understanding of PWM in value vs actual rotational speed will decrease the proportional gain required. At full voltage the PID is ideally only dealing with disturbances. As the battery dies, the P will have to do a little more work since the FF mapping is no longer valid (but better than 0).

In our FRC auton application it is a huge advantage to use FF gain since you are presumably using a fully charged battery.

Put another way, the P gain is a multiplier. The smaller that multiplier is allowed to be, the less oscillation and general "what the heck just happened" sessions will happen.

[Tom Line]

To echo what Austin said: This is a great example of something you can develop and test without a running robot. Write the code to generate a trapezoidal motion profile using J2SE or on a desktop using C++, debug using Excel, Matlab, or Gnu plots, and you will be able to port it to your robot code with ease.

Of course, once you are generating your trapezoidal motion profiles, you will need a controller that makes sure the bot actually follows it! Here are a few ideas on how you could do this:

1. Use a PID loop on your drive motors (in distance mode). Use the speed output of your motion profile to limit the maximum command that PID is allowed to send to your motors.

2. Use a PID loop on your drive motors (in speed mode). As long as your speed loop has an integral term (the I gain is nonzero), this should get you where you are going.

3. Use both (1) and (2) to control both speed and distance; there is also a specialized "PIV" controller used in industrial servos that mixes both speed and distance to command the motors.

4. Just use the speed command you generated to drive the motors directly (open loop), but switch to a PID controller when you get close to the goal to go the last few inches.

5. Make a full-state controller using control theory and simulation; if you go this route you are either insane or 254 :)

I'm a bit lost as to why a trapezoidal profile would be better than a well tuned PID. It seems to me that you want to accelerate as quickly as possible for as long as possible, then decelerate as quickly as possible and stop at your location.

How does a 'profile' do that any better than PID?

[Paul Copioli]

That profile actually gives you a predictable and repeatable motion path almost regardless of battery voltage (I say almost because a dead battery is a dead battery). The PID should only be used for disturbance correction. All high end industrial robots that need ridiculously good repeatability use the motion planning method. FANUC uses trapezoidal Acceleration profile and triangular acceleration profile (for really short motions) to control motion. The biggest issue fighting repeatability is Jerk (the derivative of Acceleration) and the trapezoidal acceleration profile helps fight this.

Most wafer robots used in the silicon industry use triangular aceeleration, but the loads are a bit higher with a tradeoff of shorter time to target.

In general, it is good practice to do some sort of motion planning to increase reliability. Most of us FRC people are just too lazy to actually do it and use PID to control everything.

[flameout]

I disagree with this statement.

I think you're right -- I was making an assumption that I did not state in my post. I was assuming that the velocity feedforward gain (your Kv) is sufficient to cause essentially zero steady-state error without an integral component -- if it is less, than it is possible to have no overshoot. Also, I was not claiming that it would be unstable -- it would overshoot once then return to the setpoint in a perfectly stable manner.

Here's where I'm coming from, assuming Kv is as described above (i.e. would cause approximately 0 steady state error with no integral component):

Say you're moving steadily (tracking a target trajectory) at a velocity of 1 (arbitrary units) -- your P, I, and D terms are all approximately zero, since you are at steady state. The Kv term is the majority of your control signal.

Now suppose the target trajectory changes to a velocity of 2 -- this could happen either smoothly or instantaneously. Due to the system's inertia, it will find itself at less than the desired position. Therefore, the control system will correct, trying to bring the state towards the new setpoint.

During this time, the previously-small I term will be growing as the errors accumulate. However, due to the Kv term, the necessary I term to maintain zero steady state error at the setpoint is approximately zero. Since the I term will not begin decreasing until it has overshot, it is guaranteed to overshoot.

This overshoot may or may not be acceptable. Additionally, if Kv is less than the value I've described, then it would only decrease the Ki value necessary to prevent overshoot, and decrease the rise time of the controller, which may be desirable.

[Tom Line]

Ok. After reading a bit more online, I understand some of the terms. Jerk is caused by the transition between acceleration / velocity, velocity / deceleration, or acceleration / deceleration. Those are the 'sharp' points on the graph.

To create a motion profile utilizing acceleration and deceleration, don't you have experimentally measure these to accurately determine what your robot is capable of?

From what I understand, motion profiling would seem to get you to your position point more quickly that PID. With PID, you have your drive values taper off as you get closer to your setpoint. With motion profiling, you would stay at your maximum velocity until it's time to go into maximum deceleration.

Is this accurate?

[flameout]

Ok. After reading a bit more online, I understand some of the terms. Jerk is caused by the transition between acceleration / velocity, velocity / deceleration, or acceleration / deceleration. Those are the 'sharp' points on the graph.
Technically, it's just the derivative of acceleration, but yes, that is effectively correct.

To create a motion profile utilizing acceleration and deceleration, don't you have experimentally measure these to accurately determine what your robot is capable of?
You just need to get them within the robot's capabilities -- if you're too close to your limits, then you'll get wheel slippage anyway, throwing off your distance measurements. You could probably get close enough just by looking at your wheel's CoF.

From what I understand, motion profiling would seem to get you to your position point more quickly that PID. With PID, you have your drive values taper off as you get closer to your setpoint. With motion profiling, you would stay at your maximum velocity until it's time to go into maximum deceleration.
I think that motion profiling would be slower than directly doing PID or PD on position, since motion profiling usually has a more gentle acceleration. One tradeoff, however, is that where pure PID/PD would spin the wheels on acceleration, motion profiling should prevent wheel slippage.

To everyone: Please let me know if I've made any mistakes in my interpretation of "motion profiling" -- I personally see several ways to do this, so I hope I am responding with the correct interpretation in mind.

[Jared Russell]

A perfect motion profiler (and appropriate controller) will get you to your goal at least as fast as any other method. If the acceleration limit in your profiler is right on the cusp of the stick-slip point for the wheels, you have essentially implemented a form of traction control for your robot.

Of course, the stick-slip point depends on a lot of things (tread wear, carpet irregularities, the amount of weight being carried by each wheel instantaneously, etc.), so in practice you usually want to back off on the acceleration limit by a safety margin. The extra repeatability is worth the handful of milliseconds longer it takes for your robot to get to its goal.

In general, if all your feedback controller is doing is removing noise/error from your planned trajectory, you're going to have a good time. This is why feedforward gains are so powerful - in the absence of any disturbances, you theoretically will get exactly where you want to go without needing ANY feedback!

[Paul Copioli]

Tom,

I am sitting in the HK airport so I have some time to kill. I will try to go through a simple double linear filter motion profile scheme. This is the fastest way, computationally, to do real time motion profiling. However, for FRC autonomous mode applications, the real time filtering really isn't required.

Some definitiions:

itp = iteration time (loop time)
T1 = Time, in ms for the first filter
T2 = Time, in ms for the second filter
FL1 = Filter 1's length, unitless. Must be an integer. FL1=RoundUp(T1/itp)
FL2 = Filter 1's length, unitless. Must be an integer. FL2=RoundUp(T2/itp)

Vprog = Desired Max Speed, ft/sec (can be any units you desire, just be consistent)

Dist = Desired travel distance, ft (can be any units ...)

T4 = Time, in ms, to get to destination if always at Vprog. T4 = Dist / Vprog
N = Total number of inputs to the filter, Integer. N = RoundUp (T4/itp)

That is really all you need to do the filtering so here is an example with numbers:

Vprog = 10 ft/sec
Dist = 4 ft
itp = 50ms (doing this to make the math short and easy)
T1 = 200 ms
T2 = 100 ms (this makes it an even trapezoid, as this number increases, it becomes a more traingular accel profile)

T4 = 4/10 *1000 = 400
FL1 = 200/50 = 4
FL2 = 100/50 = 2
N = 400/50 = 8

Ok, now time to fill the filters. How this works is simple. FIlter 1 has FL1 number of boxes, Filter 2 has FL2 # of boxes, and your inputs are N # of 1s until all filters are cleared.

Step # Time Input FL1 FL2 Output (Vel)
1 0 0 0 0 0 0 0 0 0 * Vprog
2 .05 1 1 0 0 0 0 0 1/6 * Vprog
3 .10 1 1 1 0 0 0 0 1/3 * Vprog
4 .15 1 1 1 1 0 0 0 1/2 * Vprog
5 .20 1 1 1 1 1 0 0 2/3 * Vprog
6 .25 1 1 1 1 1 1 0 5/6 * Vprog
7 .30 1 1 1 1 1 1 1 1 * Vprog
8 .35 1 1 1 1 1 1 1 1 * Vprog
9 .40 1 1 1 1 1 1 1 1 * Vprog
10 .45 0 0 1 1 1 1 1 5/6 * Vprog
11 .50 0 0 0 1 1 1 1 2/3 * Vprog
12 .55 0 0 0 0 1 1 1 1/2 * Vprog
13 .60 0 0 0 0 0 1 1 1/3 * Vprog
14 .65 0 0 0 0 0 0 1 1/6 * Vprog
15 .70 0 0 0 0 0 0 0 0 * Vprog

This is now your velocity command. Some interesting statistics:

Total time to end point = (N + FL1 + FL2)*itp
Total time to Max Speed = (FL1 + FL2)*itp
Theoretical time to end point = N*itp. This now corresponds to the time when decel starts.

In theory, if your itp time is short enough, then you can simply do a velocity PI routine on each of these commands in the Jag and get great position control. In addition, you can manipulate the ratio between T1 and T2 to get different Velocity trajectories based on your robot's capabilities. At FANUC, the T1 = 2 * T2 was pretty much a golden rule, but I violated it once or twice on specific painting robot models.


Let me know if you have any questions.

Paul

[Jared Russell]

Paul,

Thanks for sharing that. That's a very elegant way to do it.

You say about T2, "this makes it an even trapezoid, as this number increases, it becomes a more traingular accel profile". I don't follow. In your spreadsheet I can't see the difference between the two filters (to me it looks like one filter of length FL1+FL2).

[Paul Copioli]

Jared,

Graph acceleration vs. time and you will see what I mean. Now I did just do that from memory while sitting in an airport so there is a chance I missed a step. When I get back to my notes I will make sure what I put in this post is correct.

Edit:
Yep, I missed a step. After filter 1 you are supposed to sum filter 1 and divide by the number of steps in filter 1 (in our case, 4) and use that as the input to filter 2. Everything else stays the same. I attached a new Excel file. The resolution of this example stinks so that is why you get the chop in the accel curve.


Paul

[Jared Russell]

Jared,

Graph acceleration vs. time and you will see what I mean. Now I did just do that from memory while sitting in an airport so there is a chance I missed a step. When I get back to my notes I will make sure what I put in this post is correct.

Edit:
Yep, I missed a step. After filter 1 you are supposed to sum filter 1 and divide by the number of steps in filter 1 (in our case, 4) and use that as the input to filter 2. Everything else stays the same. I attached a new Excel file. The resolution of this example stinks so that is why you get the chop in the accel curve.


Paul

Makes a lot more sense - thanks!

[Jared Russell]

Paul:

I turned your spreadsheet into something that lets the user more easily play with the various inputs and have everything update properly (OFFSET is my new favorite spreadsheet function). Once I played around with it for a couple minutes, it made perfect intuitive sense. Thanks again.

(Recommend making a scatter plot of Time vs. Pos/Vel/Acc and then changing values to visualize what is happening)

[AdamHeard]

In theory, if your itp time is short enough, then you can simply do a velocity PI routine on each of these commands in the Jag and get great position control.
l

How well would passing the position commands to a PD or PID positon controller work compared to the above suggestion?

Also, would you leave the Vprog, T1, T2 parameters constant for all movements (for traveling 1 ft, and also 100 ft?), or would you vary per distance?

[Paul Copioli]

Adam,

I think passing position commands work too. One interesting thing to note is that you could do a cascading PID with pos PID in the crio and velocity in the jag.

The motion profile usually needs 2 types of parameters: regular and short motion. The short motion routine is for motions where T4 is less than T1 +T2. Other than that, the profile constants adjust the profile pretty well.

[AustinSchuh]

Paul,

Thank you for posting your profile generator. It is fun to see how others do it and learn more.


During this time, the previously-small I term will be growing as the errors accumulate. However, due to the Kv term, the necessary I term to maintain zero steady state error at the setpoint is approximately zero. Since the I term will not begin decreasing until it has overshot, it is guaranteed to overshoot.


You are missing a term. Ka will accelerate the robot as required, so it should in theory track the profile perfectly and never have any error. You are trying to get Kp and Ki to do the work of Ka.

Especially for the drivetrain, if you saturate your motors, you loose all ability to make adjustments. This quickly shows up as not having power available to correct for the robot turning. By providing a profile that will never ask for full power, you avoid this whole problem and can still steer nicely as you accelerate and decelerate.

[flameout]

Ah, I didn't realize the Ka term was necessary.

That makes sense then. Thank you for the clarification!

[roystur44]

I added some slider controls to Paul's spreadsheet.




13342

[z_beeblebrox]

I'm on my third rewrite of this sort of code for Vex and I've learned that position PID with no acceleration limiting makes robots do wheelies and that speed PID is very hard to tune. My third iteration uses position PID with acceleration limiting and seems to be working very well. I'm curious why you would want to use motion profiling rather than just PID, as a well designed and tuned PID controller with acceleration limiting approximates the results of a trapezoidal speed curve, as can be seen in the data from running my program on a Vex robot:
http://www.bitbuckets.org/wp-content/uploads/2012/12/PIDDATA-page-001.jpg
(Column C being position in inches and Column D being speed in inches / second)

[Ether]

acceleration limiting

Are you sensing and limiting motor current, or are you sensing acceleration?

[z_beeblebrox]

I'm limiting change in motor current.

[Ether]

I'm limiting change in motor current.

Technically, acceleration is a function of current, not rate of change of current.

I suspect what you mean is you have a rate limiter on your motor commands?

[z_beeblebrox]

Correct.

[Paul Copioli]

If you try, I think you will find the motion profile algorithm takes less memory usage, is much more accurate, and makes PID much easier to tune.

[Ether]

I'm ruminating why a motion control algorithm plus PID would take less memory than a PID without motion control. What am I missing?

[marccenter]

Ether,
Without knowing exactly what Paul is thinking, I am assuming that the PID control algorithm does not work as well as expected and the necessary software additions required to get desirable PID performance are greater than the proposed motion profiling software and the "out of the box" PID software.
Thoughts?

[AustinSchuh]

I'm curious why you would want to use motion profiling rather than just PID, as a well designed and tuned PID controller with acceleration limiting approximates the results of a trapezoidal speed curve, as can be seen in the data from running my program on a Vex robot:

Your plot shows me exactly why you wouldn't want to do it that way. Look at the overshoot at the end of the trajectory. You can do a lot better with a trapezoidal profile, for a similar space/time cost.

Without reading your implementation, I would also ask how your robot handles the case when both sides of the drivetrain are artificially or physically saturated and you have a heading error. You somehow have to deal with the heading error. You can either try to close the loop on heading so that you are still aiming for the same point in the x, y, theta configuration space, or you have to eliminate heading errors very quickly to avoid missing your goal.

[Paul Copioli]

Ether,

What I meant was that the motion profile algorithm plus PID will take less memory than the method he was using to sense and control acceleration. The motion profile filter is very few lines of code and was designed to run real time back in the early 90s. It is very clean and fast.

Paul

[z_beeblebrox]

I'll try the motion profiling method and post my results.


Edit: One question: What's the easiest way to tune velocity PID?

[Adam.garcia]

I know that 254 has their drive code implemented into their 2011 robot code, but is there anybody willing to post drive code implementing this feature in the most stripped down form possible?

Being able to see this feature (in C++ or Java) singled out will really allow me to see what is going on here.

I think it will help everybody gain a more pragmatic understanding of this motion profiling implementation.

[z_beeblebrox]

I looked through 254's code but was very confused. First, I don't really understand C++. Second, I don't think that this is the simplest implementation of motion profiling. I mostly understand how Paul's system works, but am not sure how his algorithms translate into real time motion profiling code.

Would it be possible for someone to post pseudocode for his motion profiling technique?

[AdamHeard]

I looked through 254's code but was very confused. First, I don't really understand C++. Second, I don't think that this is the simplest implementation of motion profiling. I mostly understand how Paul's system works, but am not sure how his algorithms translate into real time motion profiling code.

Would it be possible for someone to post pseudocode for his motion profiling technique?

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

[kenavt]

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).

[Paul Copioli]

Colin,

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.

[Jared Russell]

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).

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.

[apalrd]

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?

[Jared Russell]

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.

[winchymatt]

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;
}
else
{
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??