Motion Profiling

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

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

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]

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.