1 Introduction

Trajectory planning, as the core technology of industrial robots and CNC machine tool control, has been studied by a large number of researchers and achieved certain results [1,2,3]. With the extensive demand and application of industrial robots and machine tools, trajectory planning technology has gradually become mature. Currently, most common research on trajectory planning technology focuses on achieving smooth transition at the corner of space trajectory, minimum contour error under various kinematic constraints or the shortest time of space trajectory planning, etc. By improving smoothness and continuity of machine trajectory, the stability and reliability of robot motion control can be improved.

Industrial robots account for more than 50% of the global robot market. Guan et al. planned the trajectory of the robot by combining the third-order and the fourth-order polynomials to smooth the robot trajectory [4]. The fourth-order polynomial was used to interpolate the trajectory segments at both ends, and the third-order polynomial was used to interpolate the middle trajectory segment. In order to improve the level of industrial manufacturing, the role of robot trajectory planning technology cannot be ignored. In order to realize fast and smooth motion of industrial robots, Wang et al. adopted four-order polynomial to plan the acceleration curve between adjacent two points under kinematic constraints [5]. Similarly, the method of trajectory planning for jerk curve of robot motion is quite common in relevant fields. Fang et al. have proposed two different trajectory planning algorithms successively by this method [6, 7]. One was to divide the jerk curve into acceleration section, constant section, and deceleration section. Under the constraints of velocity, acceleration, and jerk, the acceleration section and deceleration section of jerk curve were planned with S-curve, and a new jerk curve was established by the integration of S-curve and straight line of constant section. The other was to construct the jerk curve of robot motion using sine function and established a new constraint criterion, which enabled the proposed algorithm to achieve the adjustability of velocity and smoothness of trajectory. This kind of method avoids the chattering caused by the change of velocity during the movement of the robot and keeps the motion process with maximum velocity, acceleration, and jerk as possible, so as to ensure the stability of the motion process and improve the working efficiency of the robot. In order to improve the production efficiency of industrial manufacturing, optimization of the time-consuming of trajectory is also one of the hot topics in related fields. Liu et al. used spline curves to plan the trajectory in Cartesian space, then used B-spline curves to plan the trajectory in the joint space, and finally used the SQP method to plan the trajectory with minimum time [8]. Gao et al. optimized the trajectory with particle swarm optimization (PSO) and 4–3-4 polynomial interpolation [9]. Fares et al. constructed the trajectory of the robot with third-order polynomial and optimized it with genetic algorithm under three different constraints: kinematics, dynamics, and effective load, and the optimal time trajectory planning algorithm was proposed finally [10]. Huang et al. [11] constructed the optimal trajectory with fifth-order B-spline applying the NSGA-II algorithm proposed by Deb et al. [12]. The resulting motion trajectory algorithm also achieves the continuity of jerk curve. Unlike above literature, Paolo and Dario [13] carried out trajectory planning from the point of view of energy consumption. In addition, there are also some algorithms for specific robots, such as Liu et al. and Wu et al. also proposed different trajectory planning algorithms for Delta robot [14, 15].

Trajectory planning of CNC machine tool with high velocity and precision promotes the efficiency and quality of industrial manufacturing. Zhao et al. put forward a two-stage velocity planning algorithm, reducing the motion time and velocity fluctuation by secondary optimization of a minimum time trajectory planning [16]. Zhang et al. attempted to solve the problem of excessive fluctuation of velocity, acceleration, and jerk at the corner between adjacent straight segments. Under the jerk constraint, to realize smooth transition of motion path, a transition curve with smooth curvature between adjacent straight segments was constructed using fifth-order B-spline curve with constraint on kinematic parameters of the robot. In the subsequent study, a new trajectory planning algorithm with the optimal velocity was proposed based on the linear programming algorithm [17, 18]. Constructing smooth transition at the corner between adjacent straight segments is one of the main methods to improve the smoothness of machine tool movement, and the searchers in related fields have also given many different solutions [19,20,21]. Besides, the machining accuracy is also a frequent consideration in trajectory planning. In order to reduce the chord height error caused by trajectory planning, Liu et al. constructed a smooth transition trajectory with NURBS curves at the sharp corners of the continuous path [22]. Li et al. and Chen et al. also proposed different solutions to reduce contour error [23, 24].

Most of the algorithms mentioned in the above literatures are trajectory planning for the current segment or adjacent two segments of the robot motion path. Although the planned trajectory can satisfy the constraints on velocity, acceleration, and jerk in the current segment, for multi-segment continuous trajectory planning, the current segment planning is affected by all the previous and afterward segments. In recent years, look-ahead trajectory planning technology has gradually become a research focus, such as Tajima and Sencer [25] and Huang [26], respectively, carried out corresponding research. In this paper, a real-time multi-axis synchronous look-ahead trajectory planning algorithm for a large number of sequentially connected path segments under the kinematic constraints. The planned trajectory is further processed with S-shape-type filter, and a real-time velocity tuning algorithm is proposed to meet the requirements of industrial applications. Finally, simulation is carried out in MATLAB environment to verify the feasibility of the algorithm, and experiments are conducted to verify the performance of the algorithm.

2 Multi-axis synchronous look-ahead trajectory planning

2.1 Problem description

In order to control the industrial robot with multiple joints, the path likes line or arc in task space should be interpolated with a large number of sampling points which lie in the line or arc path, and then these points are converted into multi-axis position sequence in the joint space by inverse kinematic of the industrial robot. All the successively adjacent two positions constitute a segment sequence, and the velocity of each segment can be set by the robot operator or simply general distance dividing sampling period.

Suppose there are m positions in the position sequence \(Q\), which can be defined as \(Q = \left[ {\mathop {\vec q}\nolimits_1 ,\mathop {\vec q}\nolimits_2 , \cdots ,\mathop {\vec q}\nolimits_m } \right]\), and each position is a n-dimensional vector, namely, \(\mathop {\vec q}\nolimits_i = \mathop {\left[ {{q_{i1}}, \cdots ,{q_{in}}} \right]}\nolimits^T\), where n is the number of robot’s joints, that is, n axes. The adjacent positions \({\vec q_i}\) and \({\vec q_{i + 1}}\) constitute the segment i (where \(1\le i\le m-1\)), and the generalized setting velocity of the segment i is \(\mathop V\nolimits_{\mathop F\nolimits_i }\). The constraints on maximum velocity and acceleration of each axis are \(\mathop V\nolimits_{\max }^j\) and \(\mathop A\nolimits_{\max }^j\), respectively (\(1\le j\le n\)).

Briefly, given m positions \(Q\), which constitute m − 1 segments, and the setting velocity sequence of segments VF, under the maximum velocity \(\mathop V\nolimits_{\max }^j\) and acceleration \(\mathop A\nolimits_{\max }^j\) (\(1\le j\le n\)), n axis synchronous look-ahead trajectory planning algorithm is required, and the algorithm must support real-time planning applying the dynamic sliding window technique while the robot is operating and real-time velocity tuning.

2.2 Transition between two adjacent segments

As shown in Fig. 1, the adjacent two segments consist of three adjacent positions, and the generalized length of the two segments is defined as \({L_i} = \left\| {{{\vec q}_{i + 1}} - {{\vec q}_i}} \right\|\) and \({L_{i + 1}} = \left\| {{{\vec q}_{i + 2}} - {{\vec q}_{i + 1}}} \right\|\). The radius of transition zone is \({L_s} \leq \frac{1}{2}\min \left( {{L_i},{L_{i + 1}}} \right)\) and the transition velocity is \({v_t}\).

Fig. 1
figure 1

Transition curve construction between two adjacent segments

Full size image

Define the cosine ratio of each axis of each segment as

$$\begin{aligned} {{\vec R}_i} &= {\left[ {{R_{i1}}, \cdots ,{R_{in}}} \right]^T} \\ &= {[\cos {\theta_{i1}}, \cdots ,\cos {\theta_{in}}]^T} \\ &= \frac{{{{\vec q}_{i + 1}} - {{\vec q}_i}}}{{L_i}} \\ \end{aligned}$$
(1)
$${\vec R_{i + 1}} = \frac{{{{\vec q}_{i + 2}} - {{\vec q}_{i + 1}}}}{{{L_{i + 1}}}}$$
(2)

The maximum generalized velocity and the maximum generalized acceleration in the segment i are shown in Eq. (3).

$${V_{\max ,j}} = \mathop {\min }\limits_{1 \leq j \leq n} \{ \frac{{V_{\max }^j}}{{{R_{i,j}}}}\} ,{A_{\max ,j}} = \mathop {\min }\limits_{1 \leq j \leq n} (\frac{{A_{\max }^j}}{{{R_{i,j}}}})$$
(3)

Then the cosine ratio difference between two adjacent segments is calculated and normalized. Equation is as follows: \(\overrightarrow {\Delta R} = {\vec R_{i + 1}} - {\vec R_i}\), and normalize \(\overrightarrow {\Delta R}\) to \(\overrightarrow {\Delta DR} = \overrightarrow {\Delta R} /\left\| {\overrightarrow {\Delta R} } \right\|\).

The generalized acceleration of the transition zone is \({A_p} = \mathop {\min }\limits_{1 \leq j \leq n} \{ \frac{{A_{\max }^j}}{{\Delta D{R_j}}}\}\); then, the acceleration vector of the transition zone is \({\vec A_t} = \overrightarrow {\Delta DR} \cdot {A_p}\).

Let the transition time be \({t_m}\), and the generalized velocity at the start and end point of the transition zone remains unchanged, and it leads to Eq. (4).

$${v_t}{\vec R_{i + 1}} = {v_t}{\vec R_i} + {\vec A_t}{t_m}$$
(4)

That is, \({v_t}\overrightarrow {\Delta R} = {\vec A_t}{t_m} = \overrightarrow {\Delta DR} \cdot {A_p}{t_m}\), so we have Eq. (5).

$${v_t} = \frac{1}{{\left\| {\overrightarrow {\Delta R} } \right\|}}{A_p}{t_m}$$
(5)

Thus, there is \({v_t} \propto {t_m}\).

The trajectory equation of the transition curve is shown in Eq. (6).

$$\begin{aligned} {L_s}{{\vec R}_{i + 1}}& = - {L_s}{{\vec R}_i} + \int_0^{t_m} {({v_t}{{\vec R}_i} + {{\vec A}_t}t} )dt \\& = - {L_s}{{\vec R}_i} + {v_t}{{\vec R}_i}{t_m} + 0.5{{\vec A}_t}t_m^2 \\ \end{aligned}$$
(6)

And then there is

$$\begin{aligned} {L_s}({{\vec R}_{i + 1}} + {{\vec R}_i})& = {v_t}{{\vec R}_i}{t_m} + 0.5{{\vec A}_t}t_m^2 \\& = 0.5({{\vec R}_{i + 1}} + {{\vec R}_i}){A_p}t_m^2/\left\| {\overrightarrow {\Delta R} } \right\| \\ \end{aligned}$$

Therefore, Eq. (7) can be deduced.

$$2{L_s} = {A_p}t_m^2/\left\| {\overrightarrow {\Delta R} } \right\|$$
(7)

Thus, there are \({L_s} \propto t_m^2\).

According to the above analysis, if the transition radius \({L_s}\) is given, then there is \({t_m} \propto \sqrt {L_s}\) and \({v_t} \propto \sqrt {L_s}\).

2.3 Determination of maximum allowable transition velocity

  • Special situation \(\overrightarrow {\Delta R} = \overrightarrow 0\)

    In this case, it means that the cosine ratio of the two adjacent segments is the same, and \({L_s} = 0\). There is no transition zone, and the maximum transition velocity can be set as \({v_t} = \min ({V_{\max ,i}},{V_{\max ,i + 1}},{V_{F_i}},{V_{{F_{i + 1}}}})\).

  • \(\overrightarrow {\Delta R} \ne \overrightarrow 0\)

First, the transition radius is given as \({L_s} = {1 \mathord{\left/ {\vphantom {1 {2\min ({L_i},{L_{i + 1}})}}} \right. \kern-\nulldelimiterspace} {2\min ({L_i},{L_{i + 1}})}}\), and calculate the transition time by Eq. (8).

$${t_m} = \sqrt {\frac{{2\left\| {\overrightarrow {\Delta R} } \right\|{L_s}}}{{A_p}}}$$
(8)

Then the maximum transition velocity is calculated as Eq. (9).

$${v_t} = \frac{{{A_p}{t_m}}}{{\left\| {\overrightarrow {\Delta R} } \right\|}} = \sqrt {\frac{{2{A_p}{L_s}}}{{\left\| {\overrightarrow {\Delta R} } \right\|}}}$$
(9)

If \({v_t} > \min ({V_{F_i}},{V_{{F_{i + 1}}}})\), then take \({v_t} = \min ({V_{F_i}},{V_{{F_{i + 1}}}})\), and the transition radius is calculated by Eq. (10).

$${L_s} = {v_t}\left\| {\overrightarrow {\Delta R} } \right\|/(2{A_p})$$
(10)

2.4 Reverse velocity planning

Suppose k (k < m) segments need to be looked ahead, that is the length of sliding window is k. In the reverse velocity planning stage, the final velocity of the last segment (segment k) is set to be zero, and then velocity planning is conducted from segment k to segment k − 1, …, until from segment 2 to segment 1. Take segment i + 1 and segment i as example for detailing the reverse velocity planning algorithm, as shown in Fig. 2.

Fig. 2
figure 2

Reverse look-ahead trajectory planning

Full size image

If \({v_s} \geq {v_t}\), then \({v_t}\) is reachable and no further processing is required. If \({v_s} < {v_t}\), then it is necessary to determine whether \({v_t}\) can be reached. Firstly, \(v^{'}_t = \sqrt {v_s^2 + 2{A_{\max }}{L_c}}\) is calculated. If \(v^{'}_t \geq {v_t}\), then \({v_t}\) is reachable. Otherwise, it is necessary to calculate the maximum transition velocity that can be reached and revise transition radius.

Let the maximum allowable transition velocity be \(v_t^{^{\prime\prime}}\). According to Fig. 2, there is Eq. (11).

$$v_t^{^{\prime\prime}2} - v_t^{^{\prime}2} = 2{A_{\max }}a{L_s} = 2a{A_{\max }}{L_s}$$
(11)

Since \(v_t^{^{\prime\prime}} = {v_t}\sqrt {1 - a}\), Eq. (12) is obtained.

$$\begin{aligned} a &= {{(v_t^2 - v_t^{^{\prime}2})} \mathord{\left/ {\vphantom {{(v_t^2 - v_t^{^{\prime}2})} {(2{A_{\max }}{L_s} + v_t^2)}}} \right. \kern-\nulldelimiterspace} {(2{A_{\max }}{L_s} + v_t^2)}} \\ &= {{(v_t^2 - v_s^2 - 2{A_{\max }}{L_c})} \mathord{\left/ {\vphantom {{(v_t^2 - v_s^2 - 2{A_{\max }}{L_c})} {(2{A_{\max }}{L_c} + v_t^2)}}} \right. \kern-\nulldelimiterspace} {(2{A_{\max }}{L_c} + v_t^2)}} \\ \end{aligned}$$
(12)

Then the transition length and transition velocity can be updated by Eq. (13).

$${L_{sn}} = (1 - a){L_s},{v_{tn}} = {v_t}\sqrt {1 - a}$$
(13)

Taking \({v_{tn}}\) as \({v_s}\) of the previous segment (segment i − 1), the reverse velocity planning continues until to the segment 1.

2.5 Forward velocity planning

Forward velocity planning is carried out for the first segment, as shown in Fig. 3, which is mainly divided into two cases.

Fig. 3
figure 3

Forward velocity planning

Full size image
  1. 1.

    \({v_s} \geq {v_t}\)

Reverse velocity planning already ensures that \({v_s}\) can slow down to \({v_t}\) and \({v_s} \leq {V_F}\). Firstly, the critical velocity \({v_c}\) is calculated by the following equation:

$$v_c^2 - v_s^2 + v_c^2 - v_t^2 = 2{A_{\max }}{L_c}$$

Thus Eq. (14) can be obtained.

$${v_c} = \sqrt {{{(v_s^2 + v_t^2 + 2{A_{\max }}{L_c})} \mathord{\left/ {\vphantom {{(v_s^2 + v_t^2 + 2{A_{\max }}{L_c})} 2}} \right. \kern-\nulldelimiterspace} 2}}$$
(14)

If \({v_c} \leq {V_F}\), the velocity curve is shown in the first figure in Fig. 4. If \({v_c} > {V_F}\), the velocity curve is shown in the second figure in Fig. 4. If \({v_s} = {v_c}\), the velocity curve is shown in the third figure in Fig. 4.

Fig. 4
figure 4

Deceleration case

Full size image
  1. 2.

    \({v_s} < {v_t}\)

This situation can be further divided into two cases.

  1. 2.1

    \(v_t^2 - v_s^2 < 2{A_{\max }}{L_c}\)

This situation indicates that the velocity can be raised to \({v_t}\), and the critical speed is calculated as Eq. (15).

$${v_c} = \sqrt {{{\left( {v_t^2 + v_s^2 + 2{A_{\max }}{L_c}} \right)} \mathord{\left/ {\vphantom {{\left( {v_t^2 + v_s^2 + 2{A_{\max }}{L_c}} \right)} 2}} \right. \kern-\nulldelimiterspace} 2}}$$
(15)

If \({v_c} \leq {V_F}\), the velocity curve is shown in the left figure in Fig. 5. If \({v_c} > {V_F}\), the velocity curve is shown in the right figure in Fig. 5.

Fig. 5
figure 5

Acceleration case 1

Full size image
  1. 2.2

    \(v_t^2 - v_s^2 > 2{A_{\max }}{L_c}\)

This situation indicates that the velocity cannot be raised to \({v_t}\). If the transition radius is 0, namely, \({L_s} = 0\), the velocity can be increased directly. The maximum velocity can be calculated by Eq. (16).

$${v_{tn}} = \sqrt {v_s^2 + 2{A_{\max }}{L_c}}$$
(16)

The velocity curve is shown in Fig. 6.

Fig. 6
figure 6

Acceleration case 2

Full size image

If \({L_s} \ne 0\), it is necessary to reduce the transition radius and increase the velocity as much as possible. As shown in Fig. 7, if the ratio of reducing the length of the transition section is \(a\), there are the following three equations:

$$v^{'}_t = \sqrt {v_s^2 + 2{A_{\max }}{L_c}}$$
(17)
$$v_t^{^{\prime\prime}} = {v_t}\sqrt {1 - a}$$
(18)
$$v_t^{^{\prime\prime}2} - v_t^{^{\prime}2} = 2{A_{\max }}a{L_s}$$
(19)
Fig. 7
figure 7

The transition radius and transition velocity revise

Full size image

By solving the above three equations, Eq. (20) can be obtained.

$$a = {{\left( {v_t^2 - v_s^2 - 2{A_{\max }}{L_c}} \right)} \mathord{\left/ {\vphantom {{\left( {v_t^2 - v_s^2 - 2{A_{\max }}{L_c}} \right)} {\left( {2{A_{\max }}{L_s} + v_t^2} \right)}}} \right. \kern-\nulldelimiterspace} {\left( {2{A_{\max }}{L_s} + v_t^2} \right)}}$$
(20)

Then Eq. (21) can be obtained to update the transition radius and transition velocity.

$$\begin{array}{l} {L_{cn}} = {L_c} + a{L_s} \\ {L_{sn}} = \left( {1 - a} \right){L_s} \\ {v_{tn}} = {v_t}\sqrt {1 - a} \\ \end{array}$$
(21)

The velocity curve obtained is shown in Fig. 6.

2.6 Transition trajectory planning

There is no transition zone while in the last segment or the direction cosine vectors of two adjacent segments are equal. Otherwise, if the transition zone exists, that is, \({L_s} \ne 0\),then trajectory planning for the transition zone is required. Since the generalized velocity of the transition zone is constant, the transition trajectory can be calculated easily.

As shown in Fig. 8, \({\vec q_{ts}}\) and \({\vec q_{te}}\) are the starting and ending velocity vector of the transition zone, \({v_t}\) is the generalized velocity, \({L_s}\) is the transition radius, \({A_p}\) is the generalized acceleration, and \({\vec A_t} = \overrightarrow {\Delta DR} \cdot {A_p}\) is the acceleration vector. According to Eq. (5) and Eq. (7), the transition duration is \({t_m} = {{2{L_s}} \mathord{\left/ {\vphantom {{2{L_s}} {v_t}}} \right. \kern-\nulldelimiterspace} {v_t}}\), and the velocity of all the axis at the given time can be obtained from Eq. (22).

$$\vec V(t) = {v_t}{\vec R_i} + {\vec A_t} \cdot t$$
(22)
Fig. 8
figure 8

Trajectory planning in the transition zone

Full size image

The corresponding position of all the axis at the given time can be obtained from Eq. (23).

$$\vec q(t) = {\vec q_{ts}} + \int_0^t {\vec V(t)dt} = {\vec q_{ts}} + {v_t}{\vec R_i} \cdot t + 0.5{\vec A_t} \cdot {t^2}$$
(23)

2.7 Multi-axis synchronous look-ahead trajectory planning algorithm

2.7.1 [Algorithm 1] Multi-axis synchronous look-ahead trajectory planning (MSLTP)

The flowchart of the algorithm, shown in Fig. 9, can be described as the following:

  1. 1.

    Given k + 1 positions, every position is a vector with dimension n, constituting k segments, given feed velocity sequence \({V_F} = [{V_{F_1}},...,{V_{F_k}}]\), the maximum velocity bound \(V_{\max }^j1 \leq j \leq n)\), and the maximum acceleration bound \(A_{\max }^j1 \leq j \leq n)\).

  2. 2.

    Construct the transition zones between all the adjacent segments and determinate the maximum allowable transition velocity according to Sects. 2.2 and 2.3.

  3. 3.

    Set the velocity of the last position (the velocity usually sets as zero), conduct reverse velocity look-ahead planning from the segment k to segment 1, and revise transition zones and the maximum allowable transition velocity according to Sect. 2.4.

  4. 4.

    Forward trajectory planning for the first segment and transition trajectory planning for the first transition zone according to Sects. 2.5 and 2.6.

Fig. 9
figure 9

Flowchart of multi-axis synchronous look-ahead trajectory planning algorithm

Full size image

3 Dynamic real-time velocity tuning

3.1 Dynamic real-time acceleration and deceleration

Real-time velocity tuning is designed to improve dynamic performance and flexibility. It is the basis for dynamic tracking, such as visual tracking, weld tracking, or other guidance functions.

Real-time velocity change mainly includes real-time velocity increase and real-time velocity decrease. Real-time velocity increase can be realized only by updating the target feed velocity \({V_{F_i}}\left( {1 \leq i \leq m - 1} \right)\) of all segments and execute the MSLTP algorithm again immediately.

In order to achieve real-time deceleration, firstly, update the feed velocity \({V_{F_i}}\left( {1 \leq i \leq m - 1} \right)\); secondly, recalculate the first transition zone and the maximum transition velocity by the following methods; and finally, execute the MSLTP algorithm again.

As shown in Fig. 10, the absolute values of the generalized length, the initial generalized velocity, and the generalized maximum acceleration of the current segment are set as \({L_c}\), \({v_s}\), and \({A_m}\), respectively. The allowable transition radius, transition velocity, and generalized maximum acceleration of the transition segment are set as \({L_s}\), \({v_t}\), and \({A_p}\), respectively. The feed velocities of the current and the next segment are \({V_{F_1}}\) and \({V_{F_2}}\), respectively, and the cosine ratios of the two adjacent segments are \({\vec R_1}\) and \({\vec R_2}\), respectively.

Fig. 10
figure 10

Trajectory planning for the first and second segments

Full size image

Set \({v_{tm}} = \min \left( {{v_t},{V_{F_1}},{V_{F_2}}} \right)\) and \(\overrightarrow {\Delta R} = {\vec R_2} - {\vec R_1}\), then equations \({L_{sn}} = {{\left\| {\overrightarrow {\Delta R} } \right\|v_{tm}^2} \mathord{\left/ {\vphantom {{\left\| {\overrightarrow {\Delta R} } \right\|v_{tm}^2} {\left( {2{A_p}} \right)}}} \right. \kern-\nulldelimiterspace} {\left( {2{A_p}} \right)}}\) and \({L_{cn}} = {L_c} + {L_s} - {L_{sn}}\) can be calculated, update \({L_s} = {L_{sn}},{L_c} = {L_{cn}}\).

If \({v}_{s}>{v}_{tm}\), calculate \(v^{'}_{tm} = \sqrt {v_s^2 - 2{A_m}{L_c}}\). If \(v^{'}_{tm} \leq {v_{tm}}\), update the data according to Eq. (24).

$${v_{tn}} = {v_{tm}},{V_F} = {v_{tn}}$$
(24)

If \(v^{'}_{tm} > {v_{tm}}\), then \({L_c}\) and \({L_s}\) need to be revised and transition velocity must be recalculated. As shown in Fig. 11, since the velocity cannot be reduced to \({v_{tm}}\), \({L_c}\) needs to be reduced. Let the reduction ratio be \(a\); there is the following equation:

$$v_t^{^{\prime}2} - v_{tm}^2 = {{2{A_p}a{L_c}} \mathord{\left/ {\vphantom {{2{A_p}a{L_c}} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}}$$
(25)
$$v_s^2 - v_t^{^{\prime}2} = 2{A_m}(1 - a){L_c}$$
(26)
Fig. 11
figure 11

Real-time deceleration for the first transition zone

Full size image

According to Eq. (25) and Eq. (26), Eq. (27) can be obtained.

$$a = {{\left( {v_s^2 - v_{tm}^2 - 2{A_m}{L_c}} \right)} \mathord{\left/ {\vphantom {{\left( {v_s^2 - v_{tm}^2 - 2{A_m}{L_c}} \right)} {\left( {2{L_c}\left( {{{A_p} \mathord{\left/ {\vphantom {{A_p} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}} - {A_m}} \right)} \right)}}} \right. \kern-\nulldelimiterspace} {\left( {2{L_c}\left( {{{A_p} \mathord{\left/ {\vphantom {{A_p} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}} - {A_m}} \right)} \right)}}$$
(27)

Then the corresponding data can be updated according to the following formula:

$${L_{sn}} = a{L_c} + {L_s},{L_{cn}} = (1 - a){L_c}$$
(28)
$${v_{tn}} = v^{'}_t = \sqrt {{{2{A_p}{L_{sn}}} \mathord{\left/ {\vphantom {{2{A_p}{L_{sn}}} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}}} ,{V_F} = {v_{tn}}$$
(29)

3.2 Real-time trajectory planning for stop command

When receiving the stop command, the first step is to judge whether the actuator can stop in the current segment. Calculate \(L = {L_c} + {L_s}\) and \({L^{'}} = {{v_s^2} \mathord{\left/ {\vphantom {{v_s^2} {\left( {2{A_m}} \right)}}} \right. \kern-\nulldelimiterspace} {\left( {2{A_m}} \right)}}\). If \({L^{'}} \leq L\), it means that the velocity can be reduced to 0 in the current segment, then only this segment needs trajectory planning. Otherwise, the transition zone should be revised to reduce the velocity as much as possible. Set the velocity after deceleration (the transition velocity) as \({v_{tm}}\), then the following equation can be obtained.

$$L^{'}_s = v_{tm}^2\frac{{\left\| {\overrightarrow {\Delta R} } \right\|}}{{2{A_p}}},L^{'}_c = L - L^{'}_s = \frac{{v_s^2 - v_{tm}^2}}{{2{A_m}}}$$

Then Eq. (30) is obtained from above.

$$v_{tm}^2 = \frac{{v_s^2 - 2{A_m}L}}{{1 - {{A_m} \mathord{\left/ {\vphantom {{A_m} {\left( {{{A_p} \mathord{\left/ {\vphantom {{A_p} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right)}}} \right. \kern-\nulldelimiterspace} {\left( {{{A_p} \mathord{\left/ {\vphantom {{A_p} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right. \kern-\nulldelimiterspace} {\left\| {\overrightarrow {\Delta R} } \right\|}}} \right)}}}},L^{'}_s = \frac{{\left\| {\overrightarrow {\Delta R} } \right\|}}{{2{A_p}}}v_{tm}^2$$
(30)

Let \({v_t} = {v_{tm}},{L_s} = L^{'}_s\), and continue to plan the next segment using the same trajectory planning method until the velocity in a segment can be reduced to 0 (Fig. 12).

Fig. 12
figure 12

Real-time trajectory planning for stop command response

Full size image

4 Digital simulation and result analysis

4.1 S-type digital filtering

The velocity curve obtained by algorithm MSLTP is trapezoidal, which can be transformed into S-type velocity curve through an S-type digital filter. S-type digital filter is designed based on the principle of digital convolution, which is very suitable for the application of real-time planners.

S-type digital filter is realized by series connection of two linear-type filters. The discrete transfer function of linear-type filters is shown in the following formula:

$${H_L}(z) = \frac{1}{m}\frac{{1 - {z^{ - m}}}}{{1 - {z^{ - 1}}}}$$
(31)

The transfer function of S-type digital filter is shown in Eq. (32).

$$\begin{aligned} {H_s}(z) &= {H_L}(z)*{H_L}(z) \\& = \frac{1}{m}\frac{{1 - {z^{ - m}}}}{{1 - {z^{ - 1}}}}*\frac{1}{m}\frac{{1 - {z^{ - m}}}}{{1 - {z^{ - 1}}}} \\ \end{aligned}$$
(32)

The effect of linear-type filter and S-type filter on input step signals is shown in Fig. 13. If the input signal is set to \({V_i}(k),k = 1,2, \ldots ,{\left. {{V_i}(k)} \right|_{k \leq 0}} = 0\), the digital implementation of S-type filter can adopt the following way:

$$\begin{array}{l} {V_{ot}}(k) = \frac{1}{m}\left( {{V_i}(k) - {V_i}(k - m)} \right) + {V_{ot}}(k - 1) \hfill \\ {V_o}(k) = \frac{1}{m}\left( {{V_{ot}}(k) - {V_{ot}}(k - m)} \right) + {V_o}(k - 1) \hfill \\ \end{array}$$
(33)
Fig. 13
figure 13

Linear-type and S-type digital filters

Full size image

4.2 Multi-axis real-time synchronous look-ahead trajectory planning algorithm

4.2.1 [Algorithm 2] multi-axis real-time synchronous look-ahead trajectory planning (MRTSLTP)

The flowchart of the algorithm, shown in Fig. 14, can be described as the following:

  1. 1.

    Given m positions, every position is a vector with dimension n, constituting m − 1 segments, given feed velocity sequence \({V_F} = [{V_{F_1}},...,{V_{{F_{m - 1}}}}]\), the maximum velocity bound \(V_{\max }^j(1 \leq j \leq n)\), and the maximum acceleration bound \(A_{\max }^j1 \leq j \leq n)\), the number of segments to look-ahead k.

  2. 2.

    If the look-ahead segment number k is larger than the number of total segment m − 1, let k = m − 1. Get the 1 ~ k + 1 positions from position sequence\(Q=\left[{\overrightarrow q}_1,{\overrightarrow q}_2,\cdots,{\overrightarrow q}_m\right]\), which constitute k segments. And then execute the MSLTP algorithm.

Fig. 14
figure 14

Flowchart of multi-axis real-time synchronous look-ahead trajectory planning algorithm

Full size image
  1. 3.

    According to the MSLTP algorithm, trajectory of the first segment and transition zone is obtained, then the positions of multi-axis can be sampled periodically from the trajectory, push them into a FIFO buffer. These positions will pass through the S-type filter and feed into the servo device finally.

  2. 4.

    If velocity tuning command is received while sampling, then update feed velocity according to Sect. 3.1, and execute the MSLTP algorithm again. If the stop command is received while sampling, then begin the trajectory planning according to Sect. 3.2, and the algorithm will terminate sampling is finished.

  3. 5.

    When the first segment and the transition zone are sampled finished, the segment and transition zone will be deleted, the positions Q will be updated. If Q is not empty, then return to step (2); otherwise, the algorithm will terminate.

4.3 Simulation

Each joint of a 6-DOF serial industrial robot corresponds to a motion axis. The coordinate system configuration of the robot is shown in Fig. 15, and the corresponding robot DH parameters are shown in Table 1. The ai and di in Fig. 15 correspond to the \({a_i}\) and \({d_i}\) in Table 1, respectively, and the rightmost column of Table 1 is the range of joint angles. The velocity range of the six axes is set to [− 4,4] rad/s, and the acceleration range is [− 30,30] rad/s2.

Fig. 15
figure 15

Coordinate system configuration of 6-DOF serial robot

Full size image
Table1 DH parameters of 6-DOF serial robot
Full size table

The orientation and position of a rigid body can be represented as a 4 × 4 homogeneous matrix T = [R, p; 0, 0, 0, 1], where R is the 3 × 3 orthogonal orientation matrix and p is the 3 × 1 position vector. Construct an arc path with start point [− 0.898, 0.066, 0.435, 98.09; − 0.215, 0.798, − 0.563, 55.47; − 0.385, − 0.599, − 0.702, − 537.73; 0, 0, 0, 1], middle point [− 0.901, − 0.001, 0.434, 53.26; 0.003, 1.000, 0.009, 380.43; − 0.434, 0.009, − 0.901, − 419.81; 0, 0, 0, 1], and end point [− 0.898, − 0.066, 0.435, 228.00; 0.215, 0.797, 0.564, 87.47; − 0.384, 0.600, − 0.702, − 396.72; 0, 0, 0, 1]. Firstly, without considering the velocity and acceleration limits of axes, the EOT (end of tool) of the robot moves along the arc path with a S-type velocity profile in the task space. Secondly, 202 points in the path are obtained by sampling with 2-ms period. The corresponding position sequence of the six axes is obtained with inverse kinematics, as shown in Fig. 16. The corresponding velocity and acceleration sequences can also be calculated easily, as shown in Figs. 17 and 18.

Fig. 16
figure 16

Displacement curve of original target trajectory

Full size image
Fig. 17
figure 17

Velocity curve of the original target trajectory

Full size image
Fig. 18
figure 18

Acceleration curve of original target trajectory

Full size image

The 202 positions constitute 201 segments; the feed velocity of every segment can be set as length of the segment dividing 2 ms, look-ahead 100 segments, a fine interpolation period of 1 ms. The MRTSLTP algorithm is carried out; the position, velocity, and acceleration obtained after planning are shown in Figs. 19, 20, and 21. If the velocity is dynamically reduced to 1/10 of the original at 400 ms and then increased to 10 times of the current at 1500 ms, MRTSLTP algorithm can response immediately, and the velocity and acceleration curves obtained are shown in Figs. 22, 23, and 24.

Fig. 19
figure 19

Displacement curve planned by MRTSLTP algorithm

Full size image
Fig. 20
figure 20

Velocity curve planned by MRTSLTP algorithm

Full size image
Fig. 21
figure 21

Acceleration curve planned by MRTSLTP algorithm

Full size image
Fig. 22
figure 22

Displacement curve planned by MRTSLTP with velocity tuning

Full size image
Fig. 23
figure 23

Velocity curve planned by MRTSLTP with velocity tuning

Full size image
Fig. 24
figure 24

Acceleration curve planned by MRTSLTP with velocity tuning

Full size image

It can be seen from Fig. 17 and Fig. 18 that the velocity and acceleration of the given target trajectory are far beyond the given range of velocity and acceleration. After trajectory planning with MRTSLTP algorithm under the constraints of velocity and acceleration, the target trajectory is significantly improved, which are shown in Fig. 20 and Fig. 21; the velocity and acceleration of the planned target trajectory are effectively constrained within the given range, and both curves are very smooth, which can make the robot move more stable in practical applications. Figure 23 shows the velocity curve of the target trajectory after real-time velocity tuning; the MRTSLTP algorithm can respond to velocity tuning command very quickly.

5 Algorithm implementation and experiments

5.1 Control schemes and implementation of MRTSLTP algorithm

The industrial six-axis motion control platform is built to test the practicability of the algorithm. The hardware platform of the control system consists of one master station equipment and multiple slave station equipment. The CTH3-C series motion controller of Hexin is selected as the master station equipment, the H1A servo driver of Hexin is selected as the slave station equipment, and the M series servo motor is selected as the driving device. The communication between the master station and the slave station is carried out through EtherCAT (Ethernet Control Automation Technology). Figure 25 is the overall framework of the control system. Figure 26 shows the hardware platform of the industrial six-axis motion control system.

Fig. 25
figure 25

Overall diagram of the control system

Full size image
Fig. 26
figure 26

Hardware platform of six-axis motion control system

Full size image

Figure 27 is the framework diagram of the control system software platform; the software develop environment is CODESYS (V3.5 sp11 version) in the industrial personal computer. The control system software consists two modules, including the human–computer interface module and the task module. The human–computer interface module provides the parameter input, system operation state information, and system control buttons. The task module composes two sub-tasks: one is the trajectory planning task whose cycle is 20 ms and the other is the communication task whose cycle is 1 ms. The communication task feeds position data to servo device every 1 ms periodically, and has the highest priority.

Fig. 27
figure 27

Control system software platform block diagram

Full size image

Figure 28 is the control system data flow diagram. There are four buffers in this system. The buffer1 mainly consists all the position sequence needed to be planned and feed velocity sequence, buffer2 mainly consists the current k segments to be planned, buffer3 consists the sampling positions after the trajectory planning, and buffer4 consists the smooth positions after passing through the S-type filter.

Fig. 28
figure 28

Control system data flow diagram

Full size image

5.2 Experiment and analysis

According to the same DH model, let the orientation be R = [− 0.8979, 0.0653, 0.4354; − 0.2155, 0.7972, − 0.5640; − 0.3839, − 0.6002, − 0.7017]; construct an arc path with start point [98.09, 55.47, − 537.7], middle point [161.6, 3.531, − 407.3], and end point [245.8, 17.6, − 270.1]; and construct a line path with start point [93.59, 214, − 63.34] and end point [98.09, 55.47, − 537.7]. 1.25 m/s is used as the generalized velocity of line path, and 0.8 m/s is used as the generalized velocity of arc path, taking the line and arc in the task space as the target path, respectively. The target path is rough interpolated, and the positions in joint space are obtained by inverse kinematics of the interpolation points. Taking the position sequence as input, the servo motors are controlled by the control system software to track the positions with feed generalized velocity.

Figures 29, 30, 31, 32, 33, and 34 show the original displacement, velocity, and acceleration curves of the joints by rough interpolation and inverse kinematics along the line and arc path, respectively. Figures 35, 36, 37, and 38 show the results obtained by experiment. The preset values shown in Figs. 35, 36, 37, and 38 are the theoretical value obtained by the MRTSLTP algorithm, and the actual values are the feedback of the servo motor.

Fig. 29
figure 29

Joint displacement curve of the original line path

Full size image
Fig. 30
figure 30

Joint velocity curve of the original line path

Full size image
Fig. 31
figure 31

Joint acceleration curve of the original line path

Full size image
Fig. 32
figure 32

Joint displacement curve of the original arc path

Full size image
Fig. 33
figure 33

Joint velocity curve of the original arc path

Full size image
Fig. 34
figure 34

Joint acceleration curve of the original arc path

Full size image
Fig. 35
figure 35

Joint displacement curve of the line path by experiment

Full size image
Fig. 36
figure 36

Joint velocity curve of the straight path by experiment

Full size image
Fig. 37
figure 37

Joint displacement curve of the arc path by experiment

Full size image
Fig. 38
figure 38

Joint velocity curve of the arc path by experiment

Full size image

5.3 Analysis of path error

Figures 35, 36, 37, and 38 show that the preset and the actual trajectories of all the joints are nearly coincident. The differences between the preset and the actual values are calculated for comparison and analysis. The error curves of joint displacement corresponding to the two trajectories are shown in Figs. 39 and 40. The error of joint displacement is effectively controlled, and the maximum error is within 0.001 rad.

Fig. 39
figure 39

Error curve of joint displacement of line path

Full size image
Fig. 40
figure 40

Error curve of joint displacement of arc path

Full size image

To further verify the performance of tracking line and arc paths in task space, the joint trajectories are converted into space trajectories by the robot forward kinematics, and the results are shown in Figs. 41, 42, 43, and 44.

Fig. 41
figure 41

Line path obtained by forward kinematics of joint displacement

Full size image
Fig. 42
figure 42

Error curve of line path obtained by forward kinematics of joint displacement

Full size image
Fig. 43
figure 43

Arc path obtained by forward kinematics of joint displacement

Full size image
Fig. 44
figure 44

Error curve of arc path obtained by forward kinematics of joint displacement

Full size image

Figures 41 and 43 show that the initial paths, preset paths, and actual paths of both line and arc trajectories have good overlap. It can be seen from Figs. 42 and 44 that the maximum errors of the line and the arc path are around 1 mm and 0.6 mm, respectively.

The errors shown in Figs. 42 and 44 are relatively large. The length of the line path is 500 mm, and the actual moving time is around 0.8 s; it is inferred that the large error is caused by the high velocity. If the rough interpolation period is set to 10 ms, that is slowing down the velocity five times. The experiments are conducted again according to the above conditions, and the results are shown in Figs. 45, 46, 47, 48, 49, 50, 51, 52, 53, and 54.

Fig. 45
figure 45

Joint displacement curve of the line path obtained by the experiment with low velocity

Full size image
Fig. 46
figure 46

Joint velocity curve of the line path obtained by the experiment with low velocity

Full size image
Fig. 47
figure 47

Joint displacement curve of the arc path obtained by the experiment with low velocity

Full size image
Fig. 48
figure 48

Joint velocity curve of the arc path obtained by the experiment with low velocity

Full size image
Fig. 49
figure 49

Error curve of joint displacement of line path with low velocity

Full size image
Fig. 50
figure 50

Error curve of joint displacement of arc path with low velocity

Full size image
Fig. 51
figure 51

Line path obtained by forward kinematics of joint displacement with low velocity

Full size image
Fig. 52
figure 52

Error curve of line path obtained by forward kinematics of joint displacement with low velocity

Full size image
Fig. 53
figure 53

Arc path obtained by forward kinematics of joint displacement with low velocity

Full size image
Fig. 54
figure 54

Error curve of arc path obtained by forward kinematics of joint displacement with low velocity

Full size image

As can be seen from Figs. 45, 46, 47, 48, 49, and 50, the trajectories of each joint under the above conditions almost completely overlap, and the joint angle errors are constrained to be within [− 0.0004, 0.0003] radians. As can be seen from Figs. 51, 52, 53, and 54, the initial, preset, and actual trajectories also have very good overlap, and the maximum position errors of the line and arc paths are constrained to be below 0.1 mm and 0.06 mm, respectively. It can be concluded that the proposed MRTSLTP algorithm can meet the performance requirements of robot trajectory planning, and has good executability in practical applications.

6 Conclusion

Focus on kinematic constraints and smooth transition of a large number of continuous segments in trajectory planning, a multi-axis real-time synchronous look-ahead trajectory planning algorithm is proposed. The algorithm performs multi-axis look-ahead trajectory planning in real time according to given position sequence, feed velocity sequence under the constraints on maximum velocity, and acceleration of the axes. The algorithm also supports online real-time velocity tuning and real-time stop command response. Experiments show that the trajectory planned by the proposed algorithm can satisfy the constraints very well whatever the feed velocity and acceleration of the given initial trajectory are. In addition, the experimental results also show that the algorithm can respond to the velocity increasing and decreasing in real time. The subsequent work is to optimize the algorithm considering the position error of the robot in the actual trajectory tracking, and to connect the servo motor to the load for further experimental verification.