Motion Profiling

[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]

Originally Posted by 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

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]

Originally Posted by 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

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]

Originally Posted by winchymatt

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.

[AustinSchuh]

Originally Posted by flameout

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.

[flameout]

Originally Posted by AustinSchuh

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.

[AustinSchuh]

Paul,

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

Originally Posted by flameout

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.




Motion Profile - Copioli Method-2.xlsx

[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:

(Column C being position in inches and Column D being speed in inches / second)

[Ether]

Originally Posted by z_beeblebrox

acceleration limiting

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

[AustinSchuh]

Originally Posted by z_beeblebrox

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]

Originally Posted by winchymatt

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