05 Apr 2019 - tsp

Last update 16 Apr 2019

Trajectory planning is the stage that decides if a motion profile is triangular (no constant maximum speed section) or trapezoidal (with a constant velocity phase after acceleration) as well as the number of steps spent in acceleration and deceleration as well as in constant speed modes.

The algorithms always assume that steppers start from zero velocity and the relative zero position. This leads to slightly simpler equations but to significant easier real-time calculations.

The following symbols are defined:

- $s_{max}$ is the target position in radians
- $n_{total}$ is the total number of steps that have to be taken to reach the final position
- $v_d$ is the target (maximum) speed in radians per second
- $\alpha$ is the step size of a single (micro)step in radians
- $a$ and $d$ are acceleration and deceleration in radians per squareseconds

One can easily calculate the required numbers of steps:

$n_{total} = \frac{s_{max}}{\alpha}$

As one knows from basic physics

$v(t) = \int a(t) \text{d}t$

for constant acceleration this is a linear equation

$v(t) = \int a \text{d}t = a * t + z_1$

The integration constant $z_1$ is the initial velocity $v(0)$ which is assumed to be zero. The distance can also be calculated via integration as usual:

$s(t) = \int v(t) \text{d}t = \int a*t \text{d}t = \frac{1}{2} a*t^2 + z_2$

The assumtion is again that the integration constant $z_2$ which corresponds to the initial position can be set to zero to achieve easier implementation.

The time required to reach maximum speed (assuming it will be reached in a trapezoidal profile) can be calculated via

$v_d = a*t \to t = \frac{v_d}{a}$

The distance after that time can be described as

$s(t) = \frac{1}{2} * a * \left(\frac{v_d}{a}\right)^2 = \frac{v_d^2}{2*a}$

Because we know that $n * \alpha = s(t)$ can be used to calculate the number of steps the number of steps required for acceleration to maximum speed can be identified as

$n_a = \frac{v_d^2}{2 * a * \alpha}$

The same process can be used to model deceleration with $a(t)=-d$ from starting velocity $v_d$ (i.e. decelerating from maximum velocity):

$v(t) = -d*t + v_d$

$s(t) = -\frac{1}{2}*d*t^2 + v_d*t$

Because we want to decelerate till stop (zero speed):

$v(t) = 0 = -d*t + v_d \to t = \frac{v_d}{d}$

$s = -\frac{1}{2}*d*\left(\frac{v_d}{d}\right)^2 + v_d * \frac{v_d}{d} = \frac{v_d^2}{2*d}$

This leads to the number of steps for deceleration:

$n_d = \frac{v_d^2}{2*d*\alpha}$

In case the motor doesnâ€™t reach maximum velocity (i.e. the total number of steps $n_t$ is smaller than $n_a + n_d$ that have been calculated before) one can calculate the number of steps required during the shorter acceleration and deceleration phases:

$n_{total} = n_{a,s} + n_{d,s} = \frac{v_{max}^2}{2*a*\alpha} + \frac{v_{max}^2}{2*d*\alpha}$

Transforming the expression to $v_max$:

$v_max = \sqrt{\frac{2*\alpha*n_{total}*a*d}{a + d}}$

Inserting $v_max$ into the expressions of $n_a$ and $n_d$ before one gets:

$n_{a,s}=\frac{v_{max}^2}{2*a*\alpha}=\frac{n_{total}*d}{a+d}$

$n_{d,s}=\frac{n_{total}*a}{a+d}$

Using the results above one can model calculate all required values for trapezoidal profiles:

$n_T = \frac{s_{max}}{\alpha}$

$n_a = \frac{v_d^2}{2*a*\alpha}$

$n_d = \frac{v_d^2}{2*d*\alpha}$

$n_c = n_T - n_a - n_d$

as well as all required values for triangular profiles in case $n_a + n_d > n_T$:

$n_T = \frac{s_{max}}{\alpha}$

$n_a = n_T * \frac{d}{a+d}$

$n_d = n_T - n_a$

Since the challenge is to drive the stepper by using a microcontroler one now has to calculate the timer counter values for each step duration. Assuming a timer frequency of $f$ in Hz the calculation for the counter values for constant time phases is easy:

$\delta t_i = \frac{\alpha}{v} \to c_i = \frac{\alpha}{v} * f$

During the accelerated and decelerated phase the counter values for each step change corresponding to the speed change:

$\delta t_i = \frac{c_i}{f}$

with $\delta t_i = t_i - t_{i-1}$ and $v_i$ being the speed at the instant $t_i$

Since one knows that every step has the same size one can assume that

$\alpha = v_i * \delta t_i \to v_i = \frac{\alpha * f}{c_i}$

Speed is modeled to change linearly because one assumes that acceleration and deceleration is constant:

$v_i = v_{i-1} + a * \delta t_{i-1}$

$\to \frac{\alpha * f}{c_i} = v_{i-1} + a*\delta t_{i-1}$

$\to c_i = \frac{\alpha f}{\frac{\alpha f}{c_{i-1}}+a * \frac{c_{i-1}}{f}} = \frac{c_{i-1}}{1 + c_{i-1}^2 * \frac{a}{\alpha * f^2}}$

One can identify the constant $R_a = \frac{a}{\alpha * f^2}$ as well as the same constant for deceleration phase: $R_d = -\frac{d}{\alpha * f^2}$

Using this constants one can calculate the time steps required recursively which is suiteable for realtime calculation:

$c_i = \frac{c_{i-1}}{1 + c_{i-1}^2 * R}$

Depending on acceleration and deceleration one uses $R_a$ or $R_d$

The initial time delay has to be calculated differently. For acceleration one can simply use $\alpha = \frac{1}{2} * a * \delta t_0^2$

$\delta t_0 = \sqrt{\frac{2 * \alpha}{a}}$

$\to c_0 = \sqrt{\frac{2*\alpha}{a}} * f$

Some papers cite an empirical correction for the calculated values since the first and last values have a larger deviation from the optimal speed profile. They sometimes use

$c_{i,corr} = c_i * \left(1 + \frac{0,08}{i}\right)$

for the first 5 values and

$c_{i,corr} = c_i * \left(1 + \frac{0,08}{n-i}\right)$

for the last 5 values. For deceleration one can use the same technique

$\alpha = -\frac{1}{2} * d * \delta t_0^2 + v_d \delta t$

$\to c_0 = f * \frac{v_d \pm \sqrt{v_d^2 - 2 * d * \alpha}}{d}$

Many of the values can be precalculated for a fixed acceleration, deceleration and maximum speed.

- Trapezoidal profile ($n_a + n_d < n_T$):
- $n_T = \frac{s_{max}}{\alpha}$
- Precalculated $n_a = \frac{v_d^2}{2*\alpha*a}$
- Precalculated $n_d = \frac{v_d^2}{2*\alpha*d}$
- $n_c = n_T - n_a - n_d$

- Triangular profile
- $n_T = \frac{s_{max}}{\alpha}$
- $n_a = n_T * \frac{d}{a+d}$ where $\frac{d}{a+d}$ can be precalculated
- $n_d = n_T - n_a$

- Acceleration: $c_0 = \sqrt{\frac{2 * \alpha}{a}} * f$ (can be precalculated) and $c_i = \frac{c_{i-1}}{1 + c_{i-1}^2 * R_a}$ with a precalculated $R_a$
- Deceleration: $c_0 = f * \frac{v_d \pm \sqrt{v_d^2 - 2*d*\alpha}}{d}$ (can be again precalculated) and $c_i = \frac{c_{i-1}}{1 + c_{i-1}^2 * R_d}$ with a precalculated $R_d$
- Constant speed (precalculated): $c_i = \frac{\alpha * f}{v_d}$

This article is tagged: Electronics, Mechanics, DIY, Home automation

Dipl.-Ing. Thomas Spielauer, Wien (webcomplains389t48957@tspi.at)

This webpage is also available via TOR at http://jugujbrirx3irwyx.onion/