Implementing Trapezoidal Motion Profile Using Discrete Method

[Martin Rand]

Hi,

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.

Thanks,

Martin

 

[JhpMeer]

I am not familiar with PID controllers. But I would implement lineair acceleration like this:

int main {

int target_Position = 100;
int max_Velocity = 20;
int acceleration = 5;

int velocity = 0;
int position = 0;

for (int x = 0; x < max_velocity; x += acceleration) {
    if (x >= max_velocity) {
        velocity = max_velocity;
    } else {
        velocity += acceleration;
    }
    position += velocity;
    motor(100 + velocity); //Use the velocity as parameter for the DC motor
    sleep(100);
}

int endOfConstantVelocity = position; //The position when we need to decelerate

while (position <= (target_position - endOfConstantVelocity)) {
    motor(100 + velocity); //Use the velocity as parameter for the DC motor
    sleep(100);
}

for (int x = 0; x > 0; x -= acceleration) {
    if (x <= 0) {
        velocity = 0
    } else {
        velocity -= acceleration;
    }
    position += velocity;
    motor(100 + velocity); //Use the velocity as parameter for the DC motor
    sleep(100);
}

return 0;
}

This (theoretical) program will accelerate until the maximum velocity is reached. And decelerate before the target destination is reached. The flaw it has is that the max velocity is reached in 300ms because of the time resolution I chose. In the program each velocity is kept for 100ms(see sleep(100);). The problem if you make the sleep bigger(sleep(1000); = 1s) the motor will stutter. It will be very obvious you accelerate in steps. While when you take a smaller resolution it will be a much smoother motion. The hard task is to translate the program to real life.
I hope this is what you were looking for.

 

[Martin Rand]

Hey,
Thanks for that!

However, could you explain why Acceleration is divided by 2 in the equation, whenever position is calculated?

 

[JhpMeer]

Because of the formula for distance traveled from acceleration and velocity.
s = Vi×t + ½×a×(t^2)

Where
s = distance
Vi = velocity
t = time
a = acceleration

But because time is always (theoretically) 1 second when we calculate the distance we get:
s = Vi×1 + ½a×(1^2) = 1Vi + ½a×1 = Vi + ½a

But in our program we want to add the distance covered in this 1 second with the distance covered in the past. So:
s = s + Vi + ½a which explains PK= PK + VK + A/2;

You are able to implement this in the program to calculate the covered distance more accurately.
You only have to add an acceleration variable besides the velocity and position variables and implement this.
Basically you where right all along with the Newtonian Physics idea.You just didn't know where time went.

If you wondered why my program didn't have this. It is because I took my steps digitally like Vout in this picture:

Whatever the case. My acceleration would always be 0. And thus it would not be necessary to include in the function for distance.

 

[Martin Rand]

Thanks for all this. It's all beginning to make sense now.

I noticed something which might create problems, although let me know if I'm wrong:

When Velocity exceeds MaxVelocity and Velocity become MaxVelocity, will this not cause timing problems?
Let's say we're using time intervals of 1 second, Accel = 1.5 and MaxVelocity = 7.

The velocity should reach maxvel in 4.66 secs, however, it will actually take us 5 secs in the implementation.
I suppose the smaller the time interval, the smaller the error.

Any ideas?

 

[JhpMeer]

Yes, you are right. That is why I chose a time interval of 100ms (1/10th of a second). You only have to adjust your variables to be 10 times larger than reality which can be tricky.

 

[Martin Rand]

Not quite sure I understand what you mean by adjusting the variables.
Can you provide an example?
Thanks

 

[JhpMeer]

The trick is to avoid using floating point calculations(Calculations using float or double data type) and always use Integers because most functions only take integers as parameters.
Currently the distance in our program updates every 0.1 seconds. But our program behaves like we update every 1 second.
Let's say we have a velocity of 10 for 2 seconds with an acceleration of 0 (constant velocity of 10) in reality. In reality we have covered a distance of:
2 × 10 = 20.
We took a step of 10 for every second. But our program will take a step of 10 for every 0.1 seconds because it behaves like it's 1 second.
Because there are 20 steps of 0.1 in 2:
(2/0.1) = 20
our program thinks we have covered:
(2/0.1) × 10 = 200
Which is a factor 10 too large.You will have to adjust for this.
Good practice is to take a variable to help with this.

int main {

int target_Position = 100;
int max_Velocity = 20;
int acceleration = 10;
int timeFactor = 10;

int velocity = 0;
int position = 0;

//Acceleration only
for (int x = 0; x < max_velocity; x += (int) (acceleration/timeFactor)) {
    if (x >= max_velocity) {
        velocity = max_velocity;
    } else {
        velocity += (int) acceleration/timeFactor;  
    //Casting to int to prevent floating point calculations done to a Integer. That would give problems
    }
    position += velocity/timeFactor; //Dividing by time factor for correct position
    motor(100 + velocity); //Use the velocity as parameter for the DC motor
    sleep(1000/timeFactor);
}

return 0;
}

Variable timeFactor is used to do the time division. This way whenever you want to make the time interval/resolution larger or smaller you only have to change that one variable. Now your program is more modular and saves you work when you want to change something.
Notice though that when your timeFactor is larger than acceleration you can get divisions like 7/10. Now the casting to Integer will take care of that by rounding it to 1 or 0. But it won't be as accurate anymore and the purpose is gone.
Also, I am not sure if above program's for loop will work due to the cast. But you can try.