I'm trying to program an arduino to generate a Trapezoidal Motion Profile to control a DC motor with a quadrature encoder.
Essentially, the user will input the desired Target Position, Max Velocity and Acceleration (decel = -accel) and the code will calculate the target position versus time which will then be compared with the actual position. The result will then be subject to a PID calculation
My initial assumption was that I could use basic Newtonian physics to determine position (i.e. PT = P0 + V0T + 1/2AT2, VT = V0 + AT). However, after reading through documentation for pre-existing motion controllers, I discovered that the prevalent method was to use a discrete time method, which is as follows:
VK = VK-1 + A (A = Acceleration)
PK = PK-1 + VK-1 + A/2
I'm having a hard time understanding quite how this equation would generate the target position versus time. In the case of Velocity, it seems to just add the acceleration to the current velocity. But what about everything in between?
Could anybody take a shot at explaining to me how this method is used? I've spent ages searching for answers online but have had no such luck.
Can you provide some links describing the use of those equations?
They look a bit like the Euler method of numerical integration of the equations of motion, where the acceleration is assumed constant during the time step and the time step is equal to 1. However, in the equations you posted, the acceleration is used to update both the velocity and the position, which is not correct according to the standard Euler method. The simplest Euler method of numerical integration with time step = 1 is as follows:
Pk = Pk-1 + Vk-1
Vk = Vk-1 + A
Neither of these approaches generates the target position directly. They might be used in combination with PID to adjust the acceleration as the current position approaches the target position.
I believe the method is used in conjunction with PID, although my main priority at the moment is to generate theoretical position versus time. To be honest, I'm not quite sure I understand why we need to calculate velocity. Surely only position is needed for the PID loop...?
If you'd like, I can PM you some code using these equations, which mean zilch to me.
This is the discrete analog to calculus - for every differential equation you
can simply use finite differences instead to get an approximation to the analytic
result.
The advantage is you normally only need to add and subtract, making it very
hardware efficient on an 8-bit micro controller.
For simple cases like this you can be exact since things are nice and linear.
One reason for calculating the velocity separately is to be able to limit it, aside from any calculations on position. That is the "trapezoidal" aspect of motion control. The other reason is to convert a second order differential equation into two first order DEQs.
I agree with your interpretation of the position calculation, but there are notational errors or typos in both publications. As mentioned above, the Euler equations are correct and will work well for numerical integration, as long as the time step is short enough. The justification for the operation Pk = Pk-1 + Vk-1 + A/2 (if V is used and also updated for the next integration step) is not given in either link, but it might be a hybrid method, first order in velocity and second order in position.
In my experience, PID is incredibly robust with regard to both computational and measurement errors, as long as all the signs are correct. So try them both and see what works better.
If you have some C code for trajectory calculation (with useful comments), please post it. This is a public discussion, so I won't use PM.
MarkT:
This is the discrete analog to calculus - for every differential equation you
can simply use finite differences instead to get an approximation to the analytic
result.
The advantage is you normally only need to add and subtract, making it very
hardware efficient on an 8-bit micro controller.
For simple cases like this you can be exact since things are nice and linear.
Thanks!
Would you have to perform the calculation at set intervals or could you determine position using a timer as input for time?
The time step is assumed to be constant and must be so for these methods to work. Furthermore the position, velocity and acceleration have to be scaled properly according to the actual time step of the calculation, in order for the result to be physically useful.
jremington:
One reason for calculating the velocity separately is to be able to limit it, aside from any calculations on position. That is the "trapezoidal" aspect of motion control. The other reason is to convert a second order differential equation into two first order DEQs.
I agree with your interpretation of the position calculation, but there are notational errors or typos in both publications. As mentioned above, the Euler equations are correct and will work well for numerical integration, as long as the time step is short enough. The justification for the operation Pk = Pk-1 + Vk-1 + A/2 (if V is used and also updated for the next integration step) is not given in either link, but it might be a hybrid method, first order in velocity and second order in position.
In my experience, PID is incredibly robust with regard to both computational and measurement errors, as long as all the signs are correct. So try them both and see what works better.
If you have some C code for trajectory calculation (with useful comments), please post it. This is a public discussion, so I won't use PM.
Thanks!
Can you suggest how I might implement this with an example (I love examples!).
Let's say the user submits the following:
Target Position = 2000
Acceleration = 30
Max Velocity = 70
Deceleration = -Acceleration
And the calculation is to be performed every 5ms (or whatever).
What steps would be involved, using the Euler equations, to determine position?
If you have some pseudo code available, that would be a great help.
As for the C code, I'm afraid I'm not at liberty to post it in a public forum as I was asked not to distribute it online by the developer who sent it to me.
Didn't think it particularly mattered.
Let's go with encoder counts and seconds.
Feel free to increase the values - I guess 2000 encoder counts probably isn't very far.
It doesn't make much sense to propose arbitrary numbers. Consistent units must be assigned to all quantities. Encoder counts are proportional to distance and can be used, but then velocity must be measured in units of encoder counts/second and acceleration in encoder counts/second^2.
You need to decide how to implement PID to control the motor and hence determine the acceleration. In principle you can have several PID loops, one each for the independent variables position, velocity and acceleration, but most people use position or velocity.
The calculated position and velocity at each time step can be computed by the simple methods listed in the previous discussion, and compared to those obtained from the encoder counts at each time tick. Page 20 (appendix E) of the Microchip technical document goes through this in some detail.
There are some useful formulas in the documents you linked. For example, the number of encoder steps required to stop can be estimated from the maximum velocity VMAX and the constant deceleration A, Pdecel = VMAX^2/(2A), which would be 82 steps in your example (VMAX = 70 and A = 30).
Wow, a lot of this is going way over my head. Maybe I should just stick to writing "HelloWorld"s and flashing LEDs....
Are you suggesting that this method needs to be used in tandem with PID for it to work, because I'd rather not be thinking about that part just yet. To simplify, let's forget the motor control and just say I want to generate a table of positions for a trapezoidal move, with 5ms time steps. How am I going to plug my variables into the formulas so that I can obtain these values?
And just to clarify, what does "k" represent in the equation? Does it refer to the previous term in the sequence or something?
I have no trouble understanding how this algorithm work, so perhaps if you could explain how these two methods differ, I might have a better time getting to grips with the discrete method.
Thanks for all your help, really appreciate it.
All these methods require the ability to define the acceleration or the velocity or both. It should be obvious that you need to be able to control the motor to produce the defined quantity.
If you don't already have a test bed with a motor, encoder, motor controller and an Arduino, that should be your first priority. Build that and then, your first experiments should be to learn how to count encoder steps, measure rotational velocity, and set up a PID loop to control that velocity. A PID library is available for Arduino, and the Pololu Orangutan series of motor controllers can be programmed with the Arduino environment.
An equation like
Pk = Pk-1 + V
means: calculate the current position (Pk) by adding V to the previous position (Pk-1). The assumption here is that the time step = 1.
VK = VK-1 + A (A = Acceleration)
PK = PK-1 + VK-1 + A/2
If it helps think of this as calculating the running area under the graph of velocity.
Vk is incremented for each step to the right by the slope (acceleration) to give
the new height of the graph.
Then we add the new piece of the area, being a rectangle of height Vk and a
triangle of height A. These equations are assuming a unit timestep. The area
under the velocity graph/curve is its integral, ie position.
If it helps think of this as calculating the running area under the graph of velocity.
Vk is incremented for each step to the right by the slope (acceleration) to give
the new height of the graph.
Then we add the new piece of the area, being a rectangle of height Vk and a
triangle of height A. These equations are assuming a unit timestep. The area
under the velocity graph/curve is its integral, ie position.
It the timestep was dt, then they would become
VK = VK-1 + dt.A
PK = PK-1 + dt. (VK-1 + dt.A/2)
Or as C code:
V += dt * A ;
P += dt * (V - dt * A / 2) ;
Thanks!
Now it's beginning to make more sense to me.
Few questions:
In your C code example did you mean to write P += dt * (V + dt * A / 2); instead of P += dt * (V - dt * A / 2) ;?
Also, how are we to determine position when Accel = 0 (during the flat region of the trapezoid), as we'd just be adding 0 to current velocity.
I presume this method wouldn't work if we had irregular time steps, for example, if we performed the calculation during an encoder interrupt. Is this correct?
Out of interest, have you implemented such a method before?
jremington:
All these methods require the ability to define the acceleration or the velocity or both. It should be obvious that you need to be able to control the motor to produce the defined quantity.
If you don't already have a test bed with a motor, encoder, motor controller and an Arduino, that should be your first priority. Build that and then, your first experiments should be to learn how to count encoder steps, measure rotational velocity, and set up a PID loop to control that velocity. A PID library is available for Arduino, and the Pololu Orangutan series of motor controllers can be programmed with the Arduino environment.
An equation like
Pk = Pk-1 + V
means: calculate the current position (Pk) by adding V to the previous position (Pk-1). The assumption here is that the time step = 1.
Thanks!
Surely though theoretical position is the same, regardless of how you control your motor?