MC_MoveAbsolute() 4th Order Real-Time Trajectory Generation Function Algorithm and Implementation

Featured Application

The theoretical results and practical implementation of the trajectory generator presented in this article are the outcome of two research projects: The first was issued by the Polish Ministry of Science and Higher Education and the second one was part of the European Union’s 7th Framework Programme (see Funding Section).

Abstract

This paper presents the issue of generating motion trajectories in a digital servo drive in accordance with the PLCopen Motion Control standard. This standard does not limit the details of motion generation in the electromechanical systems, but indicates its interface and set of necessary parameters. Moreover, it is placed within a state machine, which allows the individual software elements to integrate with it seamlessly. This work discusses time-optimal point-to-point trajecto-ries, i.e., the initial and final reference speeds are zero, and they are compliant with the MC_MoveAbsolute() function defined in the PLCopen Motion Control standard. The smoothness of the resulting trajectory can be attributed to the use of a fourth order trajectory generator, which defines the bounds up to snap – the second derivative of acceleration. One of the aims of this article was to bridge the theoretical aspect of trajectory generation with the algorithms practical implementation, by the means of PLC code generation using the MATLAB/Simulink package.

1. Introduction

Servo drive systems are exploited widely in industries due to their efficiency, high-performance in motion control, and fast response times to dynamic reference signals [1,2]. In order to comply with the technological requirements, modern automatic machines have to guarantee decent dynamics for position and velocity control of motors, these servo drives solutions are utilized in Computer Numerically Controlled (CNC) machining, robotics, factory automation, and many other areas.

The most common control scheme implemented in servo drives to control position and velocity is the cascade control algorithm [3]. Its task is to regulate current, velocity, and position of the motor in separate closed control loops, in order to minimize overshoot and reduce the influence of external disturbances.

Nowadays, an increasing number of commercial control system manufacturers provide open architecture systems [4], meeting the need to constantly improve the servo drives functionality. This approach allows for design, testing, and integration of new control algorithms and techniques such as control laws, multi-sensor integration, or Artificial Intelligence (AI).

Simultaneously, evolution of the servo drive system programming languages is progressing. Currently, most of the commercial systems allow the user to implement custom code in the native control structure. These modifications can be written in popular programming languages such as C, C++, or Structured Text (ST).

Automatic code generation [5] is one of the ideas which facilitates implementing the code in various industrial control platforms. It is based on text languages (C, ST) and allows to compile and transfer developed code or algorithms between devices of different classes and manufacturers with minimal effort on the programmer’s part.

In the following work, generation of motion profiles (especially position) for servo drives is considered. This is a crucial aspect of applications that demand a high motion fidelity, e.g., CNC feed drives and multi-axis manipulators. In this case, constraining the acceleration profile obtained by the second order trajectory generator leads to oscillations whenever the movement direction changes (i.e., zero crossing of set velocity). This is the main reason for using third and higher order trajectory generators, for defining constraints on jerk or snap [6].

This paper is organized as follows: Section 1 serves as a brief introduction to control of digital servo drives; Section 2 is devoted to PLCopen Motion Control, particularly to one of its motion functions—MC_MoveAbsolute(). Section 3 presents a step by step algorithm to find a motion profile with boundaries defined up to the fourth derivative of position, and the trajectory generator of the fourth order is described. This topic is expanded upon in Section 4, where a real-time calculation of the beforementioned profile is discussed. Finally, Section 5 puts these solutions in context of the Programmable Logic/Automation Controller (PLC/PAC) devices that are IEC 61131-3 compliant, while Section 6 summarizes the paper.

2. Trajectory Generation in Digital Servo Drives: PLCopen Motion Control

In References [6,7,8,9], trajectory generation algorithms used in motion solutions are presented. The dynamics of mechanical aspects of the systems are considered in order to develop a feedforward controller. The following work is further expanding the ideas presented in Reference [9], such that the mathematical foundation laid there could be implemented in a real-time system.

In Reference [10], the concept of utilizing the PLCopen Motion Control standard for controlling the digital servo drive of the CNC machine is given. In the PLCopen Motion Control standard, several states are defined that outline the operation of a digital servo drive. These states depict the actual status of the motion system, being the type of movement executed (if any), present errors, stopping, or awaiting commands from the control system. Of particular interest are the two states tied to the motion execution:

(a)

Discrete Motion – movement to a defined position;

(b)

Continuous Motion – movement without a setpoint (e.g., shaft rotating with set velocity).

The transition between particular states is achieved by calling related functions, e.g., MC_MoveAbsolute (absolute movement) or MC_MoveAdditive (additive moment in a specified direction). The entire state machine is presented in Figure 1.

MC_MoveAbsolute() – Function Description

This function block starts a controlled movement to a specified absolute position. All of the parameters needed to start the movement are transferred on a rising edge of the Execute input (Figure 2). The axis changes to the Discrete Motion state after all of them have been successfully transferred. Before the function block can be used for a real axis, the axis must be homed. States in which this function block can be used: Standstill, Discrete Motion, and Continuous Motion.

In the following figures, the timing diagrams for MC_MoveAbsolute() possible usage scenarios are shown. In Figure 3a, the first function block (MC_MoveAbsolute_0) stopped its movement before the second function block (MC_MoveAbsolute_1) was started. In Figure 3b, the second function block (MC_MoveAbsolute_1) was started before the first function block (MC_MoveAbsolute_0) completed its movement. In both cases, as seen in Figure 3a,b, the movements finish in the same position.

3. Fourth Order Position Profile Generation

In this work, the position profile will be understood as a setpoint trajectory with a sampling time specified for a real-time algorithm implementation. The following convention, adopted from [9], was used to denote the parameters: $x$ – position, displacement (profile, $m$); $v$ – velocity (profile, $m/s$); $a$ – acceleration (profile, $m/s^2$); $j$ – jerk (profile, $m/s^3$); $d$ – derivative of jerk (profile, $m/s^4$); $\overline{x}$, $\overline{v}$, $\overline{a}$, $\overline{j}$, $\overline{d}$ – bound on $\left|x\right|$, $\left|v\right|$, $\left|a\right|$, $\left|j\right|$, $\left|d\right|$, respectively; $t_{\overline{d}}$, $t_{\overline{j}}$, $t_{\overline{a}}$, $t_{\overline{v}}$ – time interval during in which $\left|d\right|$, $\left|j\right|$, $\left|a\right|$, $\left|v\right|$ obtains its bound, respectively; $T_s$ – sampling time in seconds.

The fourth order position trajectory generation problem was considered within this document. One input was used to define the desired position (i.e., increment from an actual position to the new one) and another four input parameters described the movement profile including velocity, acceler-ation, jerk, and snap. These latter parameters described the boundaries which cannot be crossed while executing the movement. The generated trajectory was time-optimal, i.e., the position was reached in the shortest possible time. For simplicity we assumed that the shape of the profiles was symmetric no matter what the direction of movement was.

$$u=\left|{\left[\begin{array}{ccccc}\overline{x}& \overline{v}& \overline{a}& \overline{j}& \overline{d}\end{array}\right]}^T\right|$$

(1)

The direction of movement can be calculated based on the following equation:

$$sd=\mathrm{sgn}\left(\overline{x}\right)$$

(2)

where the $sd$ variable equals $1$ if the movement direction is positive, or $-1$ when it is negative.

While considering the real-time implementation with respect to sampling time of code execu-tion, it is necessary to round up calculated time values:

$$t_{corr}=T_s\mathrm{ceil}\left(\frac{t_{calc}}{T_s}\right)$$

(3)

Fourth order trajectory profiles are shown in Figure 4.

In the following sections, calculations for the type of trajectory generation for MC_MoveAbsolute() are shown in an algorithmic way.

3.1. Calculation of Time t_d

The following equations are used for calculation of time $t_{\overline{d}}$ which covers the duration of a single movement section with applied snap boundary (i.e., its constrained value), as in Figure 4.

$$t_{\overline{d}}{}_{d_{\max }}=\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}$$

(4)

Rounding up (4) with respect to the sampling time T_s:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{d}}{}_{d_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}}{T_s}\right)$$

(5)

$$dd=\left[\begin{array}{c}{T}_s\mathrm{ceil}\left(\frac{\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}}{T_s}\right)\\ \frac{\overline{x}}{8{\left(T_s\mathrm{ceil}\left(\frac{\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}}{T_s}\right)\right)}^4}\end{array}\right]$$

(6)

If the following Equation (7) holds true:

$$\overline{v}\ge v_{\max }=2\frac{\overline{x}}{8{\left(T_s\mathrm{ceil}\left(\frac{\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}}{T_s}\right)\right)}^4}{\left[T_s\mathrm{ceil}\left(\frac{\sqrt[4]{\frac{\overline{x}}{8\overline{d}}}}{T_s}\right)\right]}^3$$

(7)

Then:

$$dd1=dd$$

(8)

Otherwise:

$$t_{\overline{d}}{}_{v_{\max }}=\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}$$

(9)

This rounded up with respect to the sampling time ($T_s$) yields the following equation:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{d}}{}_{v_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}}{T_s}\right)$$

(10)

Then:

$$dd1=\left[\begin{array}{c}{T}_s\mathrm{ceil}\left(\frac{\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}}{T_s}\right)\\ \frac{\overline{v}}{2{\left(T_s\mathrm{ceil}\left(\frac{\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}}{T_s}\right)\right)}^3}\end{array}\right]$$

(11)

If the following Equation (12) holds true:

$$\overline{a}\ge a_{\max }=\frac{\overline{v}}{2{\left(T_s\mathrm{ceil}\left(\frac{\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}}{T_s}\right)\right)}^3}{\left[T_s\mathrm{ceil}\left(\frac{\sqrt[3]{\frac{\overline{v}}{2\overline{d}}}}{T_s}\right)\right]}^2$$

(12)

Then:

$$dd2=dd1$$

(13)

Otherwise:

$$t_{\overline{d}}{}_{a_{\max }}=\sqrt[2]{\frac{\overline{a}}{\overline{d}}}$$

(14)

This rounded up with respect to the sampling time ($T_s$) yields the following equation:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{d}}{}_{a_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\sqrt[2]{\frac{\overline{a}}{\overline{d}}}}{T_s}\right)$$

(15)

Then:

$$dd2=\left[\begin{array}{c}{T}_s\mathrm{ceil}\left(\frac{\sqrt[2]{\frac{\overline{a}}{\overline{d}}}}{T_s}\right)\\ \frac{\overline{a}}{{\left[T_s\mathrm{ceil}\left(\frac{\sqrt[2]{\frac{\overline{a}}{\overline{d}}}}{T_s}\right)\right]}^2}\end{array}\right]$$

(16)

If the following Equation (17) holds true:

$$\overline{j}\ge j_{\max }=\frac{\overline{a}}{{\left(T_s\mathrm{ceil}\left(\frac{\sqrt[2]{\frac{\overline{a}}{\overline{d}}}}{T_s}\right)\right)}^2}\left[T_s\mathrm{ceil}\left(\frac{\sqrt[2]{\frac{\overline{a}}{\overline{d}}}}{T_s}\right)\right]$$

(17)

Then:

$$dd3=dd2$$

(18)

Otherwise:

$$t_{\overline{d}}{}_{j_{\max }}=\frac{\overline{j}}{\overline{d}}$$

(19)

This rounded up with respect to the sampling time ($T_s$) yields the following equation:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{d}}{}_{j_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\frac{\overline{j}}{\overline{d}}}{T_s}\right)$$

(20)

Then:

$$dd3=\left[\begin{array}{c}{T}_s\mathrm{ceil}\left(\frac{\frac{\overline{j}}{\overline{d}}}{T_s}\right)\\ \frac{\overline{j}}{T_s\mathrm{ceil}\left(\frac{\frac{\overline{j}}{\overline{d}}}{T_s}\right)}\end{array}\right]$$

(21)

Time $t_{\overline{d}}$ is derived from Equation (21) as:

$$t_{\overline{d}}=dd{3}_1$$

(22)

The next section shows how to evaluate value of time ($t_{\overline{j}}$).

3.2. Calculation of Time t_j

When time ($t_{\overline{d}}$) is calculated, the very next step is to calculate the value of time ($t_{\overline{j}}$), while taking into account movement constraints and sampling time rounding of calculated values. The value $t_{\overline{j}}$ means the time of jerk (3rd order derivative of position), as shown in Figure 4. Derivation of time ($t_{\overline{j}}$) starts from calculations:

$$\begin{array}{ccc}\hfill P& =& -\frac{1}{9}{t}_{\overline{d}}{}^2\hfill \\ \hfill Q& =& -\frac{1}{27}{t}_{\overline{d}}{}^3-\frac{\overline{x}}{4d{d}_3\left[2\right]t_{\overline{d}}}\hfill \\ \hfill D& =& \sqrt{P^3+Q^2}\hfill \\ \hfill D& =& \sqrt[3]{-Q+D}\hfill \\ \hfill t_{\overline{j}j\max }& =& R-\frac{P}{R}-\frac{5}{3}{t}_{\overline{d}}\hfill \end{array}$$

(23)

This rounded up with respect to the sampling time (T_s) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{j_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{R-\frac{P}{R}-\frac{5}{3}{t}_{\overline{d}}}{T_s}\right)$$

(24)

Then:

$$dd4=\left[\begin{array}{c}{t}_{\overline{d}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{j_{\max }}}{T_s}\right)\\ \frac{1}{2}\frac{\overline{x}}{t_{\overline{d}}\left\{4t_{\overline{d}}{}^3+\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{j_{\max }}}{T_s}\right)\right)\left[8t_{\overline{d}}{}^2+\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{j_{\max }}}{T_s}\right)\right)\left(5t_{\overline{d}}+T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{j_{\max }}}{T_s}\right)\right)\right]\right\}}\end{array}\right]$$

(25)

If the following Equation (26) holds true:

$$\overline{v}\ge {v^{\prime }}_{\max }=dd{4}_{1}dd{4}_3\left[2dd{4}_1{}^2+dd{4}_2\left(3dd{4}_1+dd{4}_2\right)\right]$$

(26)

Then:

$$dd5=dd4$$

(27)

Otherwise:

$$t_{\overline{j}}{}_{v_{\max }}=\sqrt{\frac{1}{4}{t}_{\overline{d}}{}^2+\frac{\overline{v}}{dd{3}_2{t}_{\overline{d}}}}-1.5t_{\overline{d}}$$

(28)

This rounded up with respect to the sampling time ($T_s$) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{v_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\sqrt{\frac{1}{4}{t}_{\overline{d}}{}^2+\frac{\overline{v}}{dd{3}_2{t}_{\overline{d}}}}-1.5t_{\overline{d}}}{T_s}\right)$$

(29)

Then:

$$dd5=\left[\begin{array}{c}{t}_{\overline{d}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{v_{\max }}}{T_s}\right)\\ \frac{\overline{v}}{t_{\overline{d}}\left\{2t_{\overline{d}}{}^2+\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{v_{\max }}}{T_s}\right)\right)\left[3t_{\overline{d}}+T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{v_{\max }}}{T_s}\right)\right]\right\}}\end{array}\right]$$

(30)

If the following Equation (31) holds true:

$$\overline{a}\ge {a^{\prime }}_{\max }=dd{5}_{3}dd{5}_1\left(dd{5}_1+dd{5}_2\right)$$

(31)

Then:

$$dd6=dd5$$

(32)

Otherwise:

$$t_{\overline{j}}{}_{a_{\max }}=\frac{\overline{a}}{dd{3}_2{t}_{\overline{d}}}-t_{\overline{d}}$$

(33)

This rounded up with respect to the sampling time ($T_s$) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{a_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\frac{\overline{a}}{dd{3}_2{t}_{\overline{d}}}-t_{\overline{d}}}{T_s}\right)$$

(34)

Then:

$$dd6=\left[\begin{array}{c}{t}_{\overline{d}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{a_{\max }}}{T_s}\right)\\ \frac{\overline{a}}{t_{\overline{d}}\left[t_{\overline{d}}+T_s\mathrm{ceil}\left(\frac{t_{\overline{j}}{}_{a_{\max }}}{T_s}\right)\right]}\end{array}\right]$$

(35)

Time $t_{\overline{j}}$ is derived from Equation (35) as:

$$t_{\overline{j}}=dd{6}_2$$

(36)

The following section considers calculations of acceleration phase time ($t_{\overline{a}}$).

3.3. Calculation of Time t_a

When times $t_{\overline{d}}$ and $t_{\overline{j}}$ are calculated, taking into account movement constraints and sam-pling time rounding of calculated values, the next step is to calculate value of time ($t_{\overline{a}}$). This means the time of acceleration (second order derivative of position), as shown in Figure 4. The derivation of time ($t_{\overline{a}}$) starts from the following calculations:

$$c=\left[\begin{array}{c}dd{6}_1\left(dd{6}_1+dd{6}_2\right)\\ 3dd{6}_1\left\{2dd{6}_1{}^2+dd{6}_2\left(3dd{6}_1+dd{6}_2\right)\right\}\\ 2dd{6}_1\left\{dd{6}_1\left[4dd{6}_1{}^2+dd{6}_2\left(8dd{6}_1+5dd{6}_2\right)\right]+dd{6}_2{}^3\right\}\end{array}\right]$$

(37)

And:

$$t_{\overline{a}}{}_{a_{\max }}=\frac{1}{2}\frac{-c_2+\sqrt{c_2{}^2-4c_1\left(c_3-\frac{\overline{x}}{dd{6}_3}\right)}}{c_1}$$

(38)

This rounded up with respect to the sampling time ($T_s$) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{a_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\frac{1}{2}\frac{-c_2+\sqrt{c_2{}^2-4c_1\left(c_3-\frac{\overline{x}}{dd{6}_3}\right)}}{c_1}}{T_s}\right)$$

(39)

Then:

$$dd7=\left[\begin{array}{c}{t}_{\overline{d}}\\ t_{\overline{j}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{a_{\max }}}{T_s}\right)\\ \frac{\overline{x}}{\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{a_{\max }}}{T_s}\right)\right)\left[c_1\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{a_{\max }}}{T_s}\right)\right)+c_2\right]+c_3}\end{array}\right]$$

(40)

If the following Equation (41) holds true:

$$\overline{v}\ge {v^{″}}_{\max }=dd{7}_{4}dd{7}_1\left[dd{7}_1\left(2dd{7}_1+3dd{7}_2+dd{7}_3\right)+dd{7}_2\left(dd{7}_2+dd{7}_3\right)\right]$$

(41)

Then:

$$dd8=dd7$$

(42)

Otherwise:

$$t_{\overline{a}}{}_{v_{\max }}=\frac{dd{6}_1\left[dd{6}_1\left(-2dd{6}_1-3dd{6}_2\right)-dd{6}_2{}^2\right]+\frac{\overline{v}}{dd{6}_3}}{dd{6}_1\left(dd{6}_1+dd{6}_2\right)}$$

(43)

This rounded up with respect to the sampling time ($T_s$) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{v_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\frac{dd{6}_1\left[dd{6}_1\left(-2dd{6}_1-3dd{6}_2\right)-dd{6}_2{}^2\right]+\frac{\overline{v}}{dd{6}_3}}{dd{6}_1\left(dd{6}_1+dd{6}_2\right)}}{T_s}\right)$$

(44)

Then:

$$dd8=\left[\begin{array}{c}{t}_{\overline{d}}\\ t_{\overline{j}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{v_{\max }}}{T_s}\right)\\ \frac{\overline{v}}{t_{\overline{d}}\left\{t_{\overline{d}}\left[2t_{\overline{d}}+3t_{\overline{j}}+T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{v_{\max }}}{T_s}\right)\right]+t_{\overline{j}}\left[t_{\overline{j}}+T_s\mathrm{ceil}\left(\frac{t_{\overline{a}}{}_{v_{\max }}}{T_s}\right)\right]\right\}}\end{array}\right]$$

(45)

Time $t_{\overline{a}}$ is derived from Equation (45) as:

$$t_{\overline{a}}=dd{8}_3$$

(46)

The following section considers further calculations of the fourth order trajectory profile.

3.4. Final Calculation of Movement Profile Times and Direction

Final calculations (including time of constant velocity movement ($t_{\overline{v}}$)) start with:

$$t_{\overline{v}}{}_{v_{\max }}=\frac{\overline{x}-dd{8}_4\left[t_{\overline{a}}\left(c_1{t}_{\overline{a}}+c_2\right)+c_3\right]}{\overline{v}}$$

(47)

This rounded up with respect to the sampling time ($T_s$) yields the following:

$$T_s\mathrm{ceil}\left(\frac{t_{\overline{v}}{}_{v_{\max }}}{T_s}\right)=T_s\mathrm{ceil}\left(\frac{\frac{\overline{x}-dd{8}_4\left[t_{\overline{a}}\left(c_1{t}_{\overline{a}}+c_2\right)+c_3\right]}{\overline{v}}}{T_s}\right)$$

(48)

Then:

$$dd9=\left[\begin{array}{c}{t}_{\overline{d}}\\ t_{\overline{j}}\\ t_{\overline{a}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{v}}{}_{v_{\max }}}{T_s}\right)\\ sd\frac{\overline{x}}{t_{\overline{a}}\left(c_1{t}_{\overline{a}}+c_2\right)+c_3+\left(T_s\mathrm{ceil}\left(\frac{t_{\overline{v}}{}_{v_{\max }}}{T_s}\right)\right)t_{\overline{d}}\left[t_{\overline{d}}\left(2t_{\overline{d}}+3t_{\overline{j}}+t_{\overline{a}}\right)+t_{\overline{j}}\left(t_{\overline{j}}+t_{\overline{a}}\right)\right]}\end{array}\right]$$

(49)

Where $sd$ is calculated in Equation (2).

The column vector in Equation (49) is the final part of the fourth order trajectory profile generation. Now we have all the time values necessary to create the movement profile based on the parameters given by the user. This part of the algorithm is called the trajectory planner. The next part of the algorithm is called the profile generator. It is described in the following section and its purpose is to generate a movement profile out of the times $t_{\overline{d}},t_{\overline{j}},t_{\overline{a}},t_{\overline{v}}$ were calculated in previous sections.

4. Generation of Profile

The generation of a movement profile starts with the calculation of switching times $t_0\dots t_{15}$ as shown in Figure 4:

$$t16=\left[\begin{array}{cccc}0& 0& 0& 0\\ 1& 0& 0& 0\\ 1& 1& 0& 0\\ 2& 1& 0& 0\\ 2& 1& 1& 0\\ 3& 1& 1& 0\\ 3& 2& 1& 0\\ 4& 2& 1& 0\\ 4& 2& 1& 1\\ 5& 2& 1& 1\\ 5& 3& 1& 1\\ 6& 3& 1& 1\\ 6& 3& 2& 1\\ 7& 3& 2& 1\\ 7& 4& 2& 1\\ 8& 4& 2& 1\end{array}\right]\left[\begin{array}{c}{t}_{\overline{d}}\\ t_{\overline{j}}\\ t_{\overline{a}}\\ T_s\mathrm{ceil}\left(\frac{t_{\overline{v}}{}_{v_{\max }}}{T_s}\right)\end{array}\right]$$

(50)

For simplicity, we can assume:

$$t_{\overline{v}}\approx T_s\mathrm{ceil}\left(\frac{t_{\overline{v}}{}_{v_{\max }}}{T_s}\right)$$

(51)

Then Equation (50) can be rewritten in the following form:

$$t16=S_{M4}\left[\begin{array}{c}{t}_{\overline{d}}\\ t_{\overline{j}}\\ t_{\overline{a}}\\ t_{\overline{v}}\end{array}\right]$$

(52)

where $S_{M4}$ describes the switching matrix for the fourth order trajectory profile generation. After these calculations, the following holds true as shown in Figure 5:

$$t16=\left[\begin{array}{c}{t}_0\\ t_1\\ t_2\\ t_3\\ t_4\\ t_5\\ t_6\\ t_7\\ t_8\\ t_9\\ t_{10}\\ t_{11}\\ t_{12}\\ t_{13}\\ t_{14}\\ t_{15}\end{array}\right]=\left[\begin{array}{c}0\\ t_{\overline{d}}\\ t_{\overline{d}}+t_{\overline{j}}\\ 2t_{\overline{d}}+t_{\overline{j}}\\ 2t_{\overline{d}}+t_{\overline{j}}+t_{\overline{a}}\\ 3t_{\overline{d}}+t_{\overline{j}}+t_{\overline{a}}\\ 3t_{\overline{d}}+2t_{\overline{j}}+t_{\overline{a}}\\ 4t_{\overline{d}}+2t_{\overline{j}}+t_{\overline{a}}\\ 4t_{\overline{d}}+2t_{\overline{j}}+t_{\overline{a}}+t_{\overline{v}}\\ 5t_{\overline{d}}+2t_{\overline{j}}+t_{\overline{a}}+t_{\overline{v}}\\ 5t_{\overline{d}}+3t_{\overline{j}}+t_{\overline{a}}+t_{\overline{v}}\\ 6t_{\overline{d}}+3t_{\overline{j}}+t_{\overline{a}}+t_{\overline{v}}\\ 6t_{\overline{d}}+3t_{\overline{j}}+2t_{\overline{a}}+t_{\overline{v}}\\ 7t_{\overline{d}}+3t_{\overline{j}}+2t_{\overline{a}}+t_{\overline{v}}\\ 7t_{\overline{d}}+4t_{\overline{j}}+2t_{\overline{a}}+t_{\overline{v}}\\ 8t_{\overline{d}}+4t_{\overline{j}}+2t_{\overline{a}}+t_{\overline{v}}\end{array}\right]$$

(53)

Movement starts in time ($t_0$), while time moment ($t_{15}$) is the endpoint of the generated move-ment.

$$t_{final}=t{16}_{16}=8t_{\overline{d}}+4t_{\overline{j}}+2t_{\overline{a}}+t_{\overline{v}}$$

(54)

Time values from Equation (53) are shown in the following Figure 5.

4.1. Execution of Trajectory Profile Generation

When the trajectory profile generation is executed (i.e., by the user) and output of the reference position trajectory has started, then the generation of snap profile is based on the following calcula-tions.

When:

$$start=\mathrm{TRUE}$$

(55)

Then:

$$d{d}_{local}\leftarrow dd{9}_5$$

(56)

The new value of $t_{16}$ is calculated based on Equation (52) and the resulting Equation (53). The variable $start$ is set based on the algorithm shown in the following part of this subsection.

Until:

$$t_{exec}\le t{16}_1$$

(57)

Then:

$$p_1=0$$

(58)

When:

$$t_{exec}>t{16}_1$$

(59)

Then:

$$p_1=dd{9}_5$$

(60)

where $t_{exec}$ stands for the actual profile generation execution time. Calculations of $p_2\dots p_{16}$ values are based on the following conditions/equations:

$$\begin{array}{l}If{t}_{exec}\le t{16}_2,p_2=0\\ If{t}_{exec}>t{16}_2,p_2=dd{9}_5\end{array}$$

(61)

Which gives the following algorithm:

$$\begin{array}{l}\mathrm{FOR}j=1\dots 16,\\ If{t}_{exec}\le t{16}_j,p_j=0\\ If{t}_{exec}>t{16}_j,p_j=dd{9}_5\end{array}$$

(62)

$p_1\dots p_{16}$ are calculated iteratively in each time sample of algorithm execution. Then the actual value/shape of derivative of jerk signal is calculated with the following equation:

$$p_{d_{jerk}}=dir\left[\begin{array}{c}\left(p_1-p_2-p_3+p_4\right)+\dots \\ \dots +\left(-p_5+p_6+p_7-p_8\right)+\dots \\ \dots +\left(-p_9+p_{10}+p_{11}-p_{12}\right)+\dots \\ \dots +\left(p_{13}-p_{14}-p_{15}+p_{16}\right)\end{array}\right]$$

(63)

where the $dir$ variable stands for direction of movement. It is a two-valued variable:

$$dir=\left\{1,-1\right\}$$

(64)

calculated with Equations (65)–(68).

When the previous trajectory profile generation is finalized, i.e., time of execution ($t_{exec}$) is greater than the sum of $t_{final}\left[s\right]$ calculated with Equation (54) and some non-zero settling time ($T_{set}\left[s\right]$) has passed, then the system is ready for a new trajectory generation:

$$ready=\mathrm{TRUE}$$

(65)

Determining direction of movement for Equation (63) starts with verifying the condition:

$$di{r}_{calc,k}=\left(dir>0\right)AND\left(ready=\mathrm{TRUE}\right)ANDNOT\left(enable=\mathrm{TRUE}\right)$$

(66)

Input $EN=\mathrm{TRUE}$ means that the user wishes for execution of the trajectory profile calculation (and profiled movement generation) and information about the readiness of the module for calculations is passed from the input variable $ready$ to the variable $ENstart$. The rising edge of the condition:

$$\left(di{r}_{calc,k-1}\right)OR\left(ENstart\right)$$

(67)

enables determination of direction, i.e., calculation of variable $dir$:

$$IFdi{r}_{k-1}\le 0THENdi{r}_k=-1ELSEdi{r}_k=1$$

(68)

Equation (68) is used when we want to switch operation mode of the movement, i.e., motor/servo is mounted in an opposite direction.

4.2. Generation of Start Signal

Generation of the $start$ signal that triggers Equation (56) and results in a new value of the $t_{16}$ variable (calculated based on Equation (52) and consequently Equation (53)) is connected with the equation:

$$IF\left(ENstar{t}_k-ENstar{t}_{k-1}\right)>0THENGenStar{t}_k=1$$

(69)

The same $start$ variable is used for triggering new profile calculations (Equations (1)–(53)).

$$star{t}_k=GenStar{t}_{k-1}$$

(70)

4.3. Calculation of Execution Time

Calculation of trajectory profiling execution time ($t_{exec}$) is based on the following expressions:

$$\begin{array}{l}IF\left(GenStar{t}_k\ge 0.5\right)THEN\left({t^{\prime }}_{exec,k}=-\frac{T_s}{2}\right)ELSE\left({t^{\prime }}_{exec,k}=t_{exec,k}+T_s\right)\\ t_{exec,k}={t^{\prime }}_{exec,k-1}\end{array}$$

(71)

This finishes the algorithm of trajectory profile generation. The next step is to calculate actual values out of the process involving integrating snap signal values. It is shown in the next subsection.

4.4. Generation of Motion Profile Step

To output the motion profile of all the derivatives and the position itself, the following steps need to be executed in a real-time control platform:

• Reference signal $d_k$ (derivative of jerk/snap) is the zero-order-hold sampled value of $p_{d_{jerk}}$ variable.
• Reference signal $j_k$ (jerk) is the numerically integrated value of signal $d_k$ with respect to the sampling time (T_s):

  $$j_k=T_s{d}_{k-1}+j_{k-1}$$

  (72)
• Reference signal $a_k$ (acceleration) is the numerically integrated value of signal $j_k$ with respect to the sampling time ($T_s$):

  $$a_k=T_s{j}_{k-1}+a_{k-1}$$

  (73)
• Reference signal $v_k$ (velocity) is the numerically integrated value of signal $a_k$ with respect to the sampling time ($T_s$):

  $$v_k=T_s{a}_{k-1}+v_{k-1}$$

  (74)
• Reference signal $x_k$ (position) is the numerically integrated value of signal $v_k$ with respect to the sampling time ($T_s$):

  $$x_k=T_s{v}_{k-1}+x_{k-1}$$

  (75)

These steps finish the trajectory generation procedure.

5. Implementation of Trajectory Generator According to IEC 61131-3

The proposed algorithm was prepared to be transferrable to real-time implementation (e.g., PLC/PAC), which could later be used in industrial control platforms.

The algorithm was divided into the following sections:

• Plan Trajectory (Figure 6) – Equations (1)–(49);
• Generate profile (Figure 6) – Equations (50)–(71); while
• Generate Motion (Figure 6) subsystem calculates Equations (72)–(75).

In Figure 7, two examples of generated trajectories are shown:

The function blocks code for the 4th order trajectory generator is presented in Figure 8, with the Simulink model presented in Figure 6 serving as a base for this conversion. Structured text code generated with a Simulink PLC Coder is about 900 lines long, and the total memory allocated for this function block in PLC is 3 kB. This means that the devised solution is implementable in the majority of PLC/PAC controllers which are IEC-61131-3 [11,12] standard compliant.

6. Summary

The article presents, in a step by step manner, the procedure for implementation of a 4th order trajectory generator that mirrors the MC_MoveAbsolute() function. Due to the straightforward nature of Equations (1)–(75), it is easily implementable in any of the available textual or graphical programming languages for real-time control systems.

The implementation of the presented algorithm is possible in control systems adhering to the IEC 61131-3 standard (e.g., PLC), and its source code was generated as a result of applying the fast prototyping paradigms, i.e., it was generated from the Simulink model. As an example, generated profiles are given with different parameters (i.e., boundaries). It is also possible to generate motion profiles of lower orders than fourth—the algorithm itself is scalable and following the procedure presented here will lead to obtaining simpler solutions constrained on third or second derivatives.

The most important contribution of this work is the unique nature of the presented approach, as it allows for practical implementation of this algorithm in commonly used real-time control systems.

The algorithm presented here is fully parametrized. It can be fully integrated by way of interfaces and its functionality, with other motion control features (i.e., collision avoidance or trajectory generation profile correction). As proposed in this paper, the implementation of MC_MoveAbsolute() is parametrized through its inputs (as it can be found in all functions and function blocks in graphical IEC 61131-3 compliant programming languages). Modularity of the approach enables the user to combine a proposed solution with an extension boom elasticity estimation feature (loader or forestry cranes applications) or dynamic collision avoidance (multi-axis serial robotic manipulators).

The function presented here can be implemented without any problems in all PLC/PAC-based control systems. The short sampling time (not more than 200 microseconds) requirement is fulfilled by most available control platforms. In the case of PLC/PACs with higher sampling times proposed, the algorithm can be extended with sampling time correction. In other cases, the integer multiple of sampling time requirement needs to be fulfilled by all times for t₀–t₁₅ times in Equation (53).