Motion Profiling

[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

[AdamHeard]

Originally Posted by Paul Copioli

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.

[Tom Line]

Originally Posted by Jared341

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.