A Real-time Look-ahead Trajectory Planning Methodology for Multi Small Line Segments Path

Abstract

When a robot is required to machine a complex curved workpiece with high precision and speed, the tool path is typically dispersed into a series of points and transmitted to the robot. The conventional trajectory planning method requires frequent starts and stops at each dispersed point to complete the task. This method not only reduces precision but also causes damage to the motors and robot. A real-time look-ahead algorithm is proposed in this paper to improve precision and minimize damage. The proposed algorithm includes a path-smoothing algorithm, a trajectory planning method, and a bidirectional scanning module. The path-smoothing method inserts a quintic Bezier curve between small adjacent line segments to achieve $G^{2}$ continuity at the junctions. The trajectory planning method utilizes a quartic polynomial and a double-quartic polynomial that can achieve a constant velocity at the velocity limitation. The bidirectional scanning module calculates the velocity at each trajectory planning segment point, simplifying calculation complexity and can be run in real time. The feasibility of the proposed algorithm is verified through simulations and experiments, which can be run in real time. In addition, high machining precision can be achieved by adjusting the relevant parameters.

1 Introduction

With the increasing demand for the high-precision and high-speed machining of complex curved workpieces, the processing ability of computer numerical control (CNC) and industrial robots that machine small line segments has become critical [1, 2]. The keys are the path-smoothing algorithm, trajectory planning method, calculation of the velocity at each point, and the conditions must be satisfied to reach the specified position at the specified speed, acceleration, and time. Path-smoothing and look-ahead were introduced as effective methods for enhancing the precision and efficiency in machining [3,4,5].

Curve fitting and curve transition methods are two major approaches that have been proposed in previous studies to achieve smoothness of small line segments. Curve fitting methods connect all points in a sequence to form a curve, including curve fitting algorithms [6], curve interpolation, curve derivation, and curve-arc length algorithms [7, 8], which have been applied in robot controllers and advanced CNC systems [9]. However, this method has several disadvantages. First, the calculation for fitting a curve through all the assigned points is cumbersome to be performed in real time. Second, the range of the curve parametric $u$ is 0 to 1, which incurs increased calculation costs and interpolation difficulty. Curve transition methods, circular arcs, and spline curves are generally employed for insertion between adjacent line segments. The advantage of this method is that the following trajectory can be calculated when machining the previous trajectory.

There are two continuous types, which are the $G^{2}$ continuity and $C^{2}$ continuity. The definition of $C^{2}$ continuity is that the 0th, first, and second derivatives are continuous. In the $G^{2}$ continuity, the curves share a common center of curvature at the joining point. Because the acceleration at the connection points of lines and curves should be continuous, the $G^{2}$ continuity should be ensured. The curve transition technique has been proposed for constructing the $G^{2}$ continuous path. The theory of this technique is that sharp corners between adjacent line segments are replaced with a spline curve, and the adjacent lines and curves have $G^{2}$ continuity, which has already been adopted in the robot domain [10, 11]. Jouaneh et al. [12] inserted a circular arc between the adjacent line segments for rapid cornering. However, the circular arc exhibits continuous motion transition only to the velocity, except for acceleration transition. This technique can achieve real-time performance in the NC system because of the minimal computation of circular arc. Because accelerations at junctions between the line segments and circular arcs are noncontinuous, the order of the blending curve must be increased while controlling the fitting tolerance and corner geometry. Pateloup et al. [13] proposed a two-dimensional curve transition algorithm that utilizes cubic B-splines constructed by eight control points to round the pocket contour. Similarly, Zhang et al. [14] employed double cubic B-splines. Walton and Meek [15] proposed utilizing Bezier or PH quintic curves as the transition curves. Bi et al. [16] proposed a cubic Bezier curve with two identical control points in the middle and achieved continuous-curvature path-smoothing.

This becomes a $G^{2}$ continuity path, which is constructed using multiple small line segments and blended with spline curves. The next problem is trajectory planning along a path. Because of the existence of spline curves, the velocity at the point on the curves must be lowered such that the limitations of acceleration and jerk are not exceeded in the curved sections. The kinematic limitations of machine tools have been fully considered to obtain a time-optimal feed rate curve [17]. Various of acceleration and deceleration trajectory planning algorithms, such as polynomial, trapezoidal, and S-shaped jerk-limited methods [18,19,20] and the jerk-continuous methods [21,22,23], are available, and the S-shaped acceleration and deceleration algorithm is widely used [24,25,26]. After the path-smoothing and trajectory planning methods are selected, the look-ahead function that calculates the velocity at each point should be determined. Bidirectional scanning algorithms can be adopted in the look-ahead function to constrain the maximum velocity, acceleration, and jerk during the motion process [27,28,29].

In this study, a method for constructing the $G^{2}$ continuity transition curve between adjacent line segments and a jerk-limited look-ahead trajectory planning method were developed. The remainder of this paper is organized as follows. The $G^{2}$ continuity transition curve that Bezier curves is presented in Section 2. A jerk-limited real-time bidirectional look-ahead algorithm and the proposed double-quartic polynomial trajectory planning are presented in Section 3. Section 4 describes the designed simulations and experiments. In Section 5, the conclusions are presented.

2 Transition Scheme

A quintic Bezier curve is inserted between adjacent line segments to improve the machining efficiency and precision (Figure 1), which can be defined as [30]

$$\begin{aligned} B\left( u \right) & = \sum\limits_{i = 0}^{5} {C_{5}^{i} } u^{i} \left( {1 - u} \right)^{5 - i} P_{i} \\ & = \left( {1 - u} \right)^{5} P_{0} + 5u\left( {1 - u} \right)^{4} P_{1} + 10u^{2} \left( {1 - u} \right)^{3} P_{2} \\ & \quad + 10u^{3} \left( {1 - u} \right)^{2} P_{3} + 5u^{4} \left( {1 - u} \right)P_{4} + u^{5} P_{5}, {u \in \left[ {0,1} \right]}, \\ \end{aligned}$$

(1)

where $P_{0}$, $P_{1}$, …, $P_{5}$ are the control points of the quintic Bezier curve, and $u$ is the parameter of the Bezier curve.

Figure 1

Quintic Bezier curve between two adjacent line segments

2.1 Construction of Quintic Bezier Curve

The quintic Bezier curve is inserted between the two adjacent line segments, as shown in Figure 1. For example, $P_{{{\text{start}}}}$ is the start point of the $k^{th}$ line segment, $P_{{{\text{trans}}}}$ is the endpoint of the $k^{th}$ line segment and the start point of the $\left( {k + 1} \right)^{th}$ line segment, $P_{{{\text{end}}}}$ is the endpoint of the $\left( {k + 1} \right)^{th}$ line segment, and $\theta$ is the angle between the two line segments. Because the order of the inserted Bezier curve is quintic, the continuity of $G^{2}$ is ensured, and two adjacent line segments have the same normal and tangent directions with the inserted Bezier curve at the connection points. Differentiating Eq. (1) at the parameter $u$ yields the first and second derivatives of the Bezier curve:

$$\begin{aligned} B_{{\text{u}}} & = \frac{{{\text{d}}B\left( u \right)}}{{{\text{d}}u}} \\ & = 5\left( {1 - u} \right)^{4} \left( {P_{1} - P_{0} } \right) + 20u\left( {1 - u} \right)^{3} \left( {P_{2} - P_{1} } \right) \\ & \quad + 30u^{2} \left( {1 - u} \right)^{2} \left( {P_{3} - P_{2} } \right) + 20u^{3} \left( {1 - u} \right)\left( {P_{4} - P_{3} } \right) \\ & \quad + 5u^{4} \left( {P_{5} - P_{4} } \right), \\ \end{aligned}$$

(2)

$$\begin{aligned} B_{{{\text{uu}}}} & = \frac{{{\text{d}}^{2} B\left( u \right)}}{{{\text{d}}u^{2} }} \\ & = 20\left( {1 - u} \right)^{3} \left( {P_{2} - 2P_{1} + P_{0} } \right) + 60u\left( {1 - u} \right)^{2} \left( {P_{3} - 2P_{4} + P_{5} } \right). \\ \end{aligned}$$

(3)

The following relationship can be derived by analyzing Eqs. (1)–(3) at the start, $u = 0$, and the end, $u = 1$, of the Bezier curve:

$$\left\{ {\begin{array}{*{20}l} {\begin{array}{*{20}l} {\begin{array}{*{20}l} {\left. B \right|_{u = 0} = P_{0} ,} \hfill \\ {\left. {B_{{\text{u}}} } \right|_{u = 0} = 5\left( {P_{1} - P_{0} } \right),} \hfill \\ \end{array} } \hfill \\ {\left. {B_{{{\text{uu}}}} } \right|_{u = 0} = 20\left( {P_{2} - 2P_{1} + P_{0} } \right),} \hfill \\ \end{array} } \hfill \\ {\left. B \right|_{u = 1} = P_{5} ,} \hfill \\ {\left. {B_{{\text{u}}} } \right|_{u = 1} = 5\left( {P_{5} - P_{4} } \right),} \hfill \\ {\left. {B_{{{\text{uu}}}} } \right|_{u = 0} = 20\left( {P_{5} - 2P_{4} + P_{3} } \right).} \hfill \\ \end{array} } \right.$$

(4)

As is well known, control points $P_{0}$ and $P_{5}$ should be the points from the line segment to transition curve, and the transition curve to the next line segment, respectively, if the $G^{0}$ continuity should be ensured. If $G^{1}$ continuity is ensured, the unit vector corresponding to $\left. {B_{{\text{u}}} } \right|_{u = 0}$ should be equal to the unit direction vector of the first line, as should that of the second line corresponding to $\left. {B_{{\text{u}}} } \right|_{u = 1}$. From this point and Eq. (4), $P_{1}$ and $P_{4}$ should be on the line segments $P_{{{\text{start}}}} P_{{{\text{trans}}}}$ and $P_{{{\text{trans}}}} P_{{{\text{end}}}}$, respectively. Because the second derivative of a line segment is a zero vector, $\left. {B_{{{\text{uu}}}} } \right|_{u = 0}$ and $\left. {B_{{{\text{uu}}}} } \right|_{u = 1}$ are zero vectors, another relationship between the control points can be derived:

$$\left\{ {\begin{array}{*{20}c} {P_{2} - P_{1} = P_{1} - P_{0} ,} \\ {P_{5} - P_{4} = P_{4} - P_{3} .} \\ \end{array} } \right.$$

(5)

The Bezier curve is determined when the parameters $c$, $d$, and $L_{{\text{T}}}$ have been calculated.

2.2 Curvature Optimization and Cornering Errors

The normal acceleration and jerk are calculated by the curvature, $k$, and the corresponding velocity, $v$, at this point. Thus, $v_{{{\text{limit}}}}$ used for look-ahead planning can be calculated using the maximum normal acceleration ${}^{{\text{n}}}a_{\max }$, maximum jerk $j_{\max }$, and curvature $k$, which can be expressed as:

$${}^{{\text{a}}}v_{{{\text{limit}}}} = \sqrt {\frac{{{}^{{\text{n}}}a_{\max } }}{k}},$$

(6)

$${}^{{\text{j}}}v_{{{\text{limit}}}} = \sqrt[3]{{\frac{{j_{\max } }}{{k^{2} }}}},$$

(7)

where ${}^{{\text{a}}}v_{{{\text{limit}}}}$ and ${}^{{\text{j}}}v_{{{\text{limit}}}}$ are the velocity limitations corresponding to the maximum normal acceleration and maximum jerk, respectively. $v_{{{\text{limit}}}}$ is assigned to the minimum of ${}^{{\text{a}}}v_{{{\text{limit}}}}$, ${}^{{\text{j}}}v_{{{\text{limit}}}}$, and $v_{\max }$. The higher the value of ${}^{{\text{n}}}a_{\max }$ and $j_{\max }$, and the smaller the value of curvature $k$, the better to run at high speeds for robots.

The curvature $k\left( {u_{i} } \right)$ of the Bezier curve $B\left( u \right)$ at parameter $u_{i}$ is expressed as:

$$k\left( {u_{i} } \right) = \frac{{\left\| {B_{{\text{u}}} \left( {u_{i} } \right) \times B_{{{\text{uu}}}} \left( {u_{i} } \right)} \right\|}}{{\left\| {B_{{\text{u}}} \left( {u_{i} } \right)} \right\|^{3} }}.$$

(8)

As shown in Figure 1, the corner distance, $L_{{\text{T}}}$, is the only scale factor in the construction of the transition curve:

$$L_{{\text{T}}} = 2c + d.$$

(9)

Typically, the range of $L_{{\text{T}}}$ is the minimum length of two adjacent line segments multiplied by a factor range in $\left( {0,0.5} \right)$. We set $n$ as:

$$n = c/d.$$

(10)

After the parameter $n$ is determined, the Bezier curve and its maximum curvature are confirmed. A simple power regression is applied [31] to reduce the maximum curvature and obtain the optimal curvature value $k$ for cornering angles $\theta$:

$$n\left( \theta \right) = \frac{1}{2.0769}\theta^{0.9927} .$$

(11)

$\delta$ is the cornering error between the midpoint of the transition curve and the point of intersection of the adjacent line segments. The smaller the cornering error, the closer to the original trajectory, and the better the machining effect. The values of $c$ and $d$ can be obtained using the technique adopted in Ref. [32] to constrain the cornering error $\delta$ within a specific range for a curvature optimum curve:

$$\left\{ \begin{aligned} c & = \frac{{32\delta }}{{\left( {7n + 16} \right)\sqrt {2 + 2\cos \left( \theta \right)} }}n, \hfill \\ d & = \frac{{32\delta }}{{\left( {7n + 16} \right)\sqrt {2 + 2\cos \left( \theta \right)} }}. \hfill \\ \end{aligned} \right.$$

(12)

2.3 Adaptive Interpolation Design

After path-smoothing based on the approach presented in Sections 2.1 and 2.2, the original path, consisting of many line segments, becomes a $G^{2}$ continuity path comprising curves and lines. The displacement at each millisecond can be calculated using the motion-planning module, and the interpolation module calculates the point on the line or curve using the displacement. The interpolation module is simple for line segments:

$$P_{{{\text{current}}}} = \frac{{s_{i} - s_{{{\text{current}}}} }}{{s_{i} }}P_{i} + \frac{{s_{{{\text{current}}}} }}{{s_{i} }}P_{i + 1},$$

(13)

where $P_{{{\text{current}}}}$ is the current interpolation point, $s_{i}$ is the length of the current line, $s_{{{\text{current}}}}$ is the displacement on this line, and $P_{i}$ and $P_{i + 1}$ are the start and end points of the line.

The interpolation module is more complicated for curved segments. The second-order Taylor’s expansion is a good method for precisely obtaining the interpolated points on the curve segments:

$$u\left( {t_{i + 1} } \right) = u\left( {t_{i} } \right) + \left. {\frac{{{\text{d}}u}}{{{\text{d}}t}}} \right|_{{t = t_{i} }} \Delta t + \frac{1}{2}\left. {\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}} \right|_{{t = t_{i} }} \Delta t^{2} + o\left( h \right),$$

(14)

where $u\left( {t_{i} } \right)$ is the curve parameter $u$ at last time $t_{i}$, $\frac{{{\text{d}}u}}{{{\text{d}}t}}$, and $\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}$ are the first and second derivatives for $u$ with respect to $t$. $\frac{{{\text{d}}u}}{{{\text{d}}t}}$ can be expressed as:

$$\frac{{{\text{d}}u}}{{{\text{d}}t}} = \frac{{{\text{d}}u}}{{{\text{d}}s}}\frac{{{\text{d}}s}}{{{\text{d}}t}} = \frac{v}{{{{{\text{d}}s} \mathord{\left/ {\vphantom {{{\text{d}}s} {{\text{d}}u}}} \right. \kern-0pt} {{\text{d}}u}}}}.$$

(15)

The calculation of $\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}$ depends on acceleration $a$:

$$\begin{gathered} a = \frac{{{\text{d}}^{2} s}}{{{\text{d}}t^{2} }} = \frac{{\text{d}}}{{{\text{d}}t}}\left( {\frac{{{\text{d}}s}}{{{\text{d}}u}}\frac{{{\text{d}}u}}{{{\text{d}}t}}} \right) = \frac{{\text{d}}}{{{\text{d}}t}}\left( {\frac{{{\text{d}}s}}{{{\text{d}}u}}} \right)\frac{{{\text{d}}u}}{{{\text{d}}t}} + \frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}\frac{{{\text{d}}s}}{{{\text{d}}u}} \\ = \frac{{{\text{d}}^{2} s}}{{{\text{d}}u^{2} }}\left( {\frac{{{\text{d}}u}}{{{\text{d}}t}}} \right)^{2} + \frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}\frac{{{\text{d}}s}}{{{\text{d}}u}}. \\ \end{gathered}$$

(16)

Therefore, $\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}$ can be expressed by

$$\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }} = {{\left( {a - \frac{{{\text{d}}^{2} s}}{{{\text{d}}u^{2} }}\frac{{v^{2} }}{{\left( {{{{\text{d}}s} \mathord{\left/ {\vphantom {{{\text{d}}s} {{\text{d}}u}}} \right. \kern-0pt} {{\text{d}}u}}} \right)^{2} }}} \right)} \mathord{\left/ {\vphantom {{\left( {a - \frac{{{\text{d}}^{2} s}}{{{\text{d}}u^{2} }}\frac{{v^{2} }}{{\left( {{{{\text{d}}s} \mathord{\left/ {\vphantom {{{\text{d}}s} {{\text{d}}u}}} \right. \kern-0pt} {{\text{d}}u}}} \right)^{2} }}} \right)} {\left( {{{{\text{d}}s} \mathord{\left/ {\vphantom {{{\text{d}}s} {{\text{d}}u}}} \right. \kern-0pt} {{\text{d}}u}}} \right)}}} \right. \kern-0pt} {\left( {{{{\text{d}}s} \mathord{\left/ {\vphantom {{{\text{d}}s} {{\text{d}}u}}} \right. \kern-0pt} {{\text{d}}u}}} \right)}},$$

(17)

where $\frac{{{\text{d}}s}}{{{\text{d}}u}}$ and $\frac{{{\text{d}}^{2} s}}{{{\text{d}}u^{2} }}$ are the first and second derivatives of arc length $s$ with respect to parameter $u$, which are the norms of Eqs. (2) and (3), respectively.

Therefore, $u\left( {t_{i + 1} } \right)$ is obtained; however, a parametric truncation error $o\left( h \right)$ exists, which is completely ignored, resulting in interpolated points that are not real points. This causes sharp variations in velocity, acceleration, and jerk [31].

Referring to Eq. (14) and assuming that $u^{\prime}\left( {t_{i + 1} } \right)$ is $u\left( {t_{i + 1} } \right)$ but ignores the parametric truncation error:

$$u^{\prime}\left( {t_{i + 1} } \right) = u\left( {t_{i} } \right) + \left. {\frac{{{\text{d}}u}}{{{\text{d}}t}}} \right|_{{t = t_{i} }} \Delta t + \frac{1}{2}\left. {\frac{{{\text{d}}^{2} u}}{{{\text{d}}t^{2} }}} \right|_{{t = t_{i} }} \Delta t^{2} .$$

(18)

Eq. (14) can be rewritten as:

$$u\left( {t_{i + 1} } \right) = u^{\prime}\left( {t_{i + 1} } \right) + \varepsilon,$$

(19)

where $\varepsilon$ is the parametric truncation error $o\left( h \right)$. The interpolation accuracy is high when $\varepsilon$ is compensated for. The next step is to determine $\varepsilon$.

$$\begin{gathered} v\left( {u\left( {t_{i + 1} } \right)} \right) = \hfill \\ \frac{{\sqrt {\left[ {X\left( {u\left( {t_{i + 1} } \right)} \right) - X\left( {u\left( {t_{i} } \right)} \right)} \right]^{2} + \left[ {Y\left( {u\left( {t_{i + 1} } \right)} \right) - Y\left( {u\left( {t_{i} } \right)} \right)} \right]^{2} } }}{\Delta t}, \hfill \\ \end{gathered}$$

(20)

where $X\left( {u\left( {t_{i + 1} } \right)} \right)$ and $Y\left( {u\left( {t_{i + 1} } \right)} \right)$ are the x- and y- coordinates of the point on the curve, respectively, which can be expressed as:

$$\begin{aligned} X\left( {u\left( {t_{i + 1} } \right)} \right) & = X\left( {u^{\prime}\left( {t_{i + 1} } \right) + \varepsilon } \right) \\ & = X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right) + \frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}\varepsilon , \\ \end{aligned}$$

(21)

$$\begin{aligned} Y\left( {u\left( {t_{i + 1} } \right)} \right) & = Y\left( {u^{\prime}\left( {t_{i + 1} } \right) + \varepsilon } \right) \\ & = Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right) + \frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}\varepsilon . \\ \end{aligned}$$

(22)

Assuming that

$$\left\{ {\begin{array}{*{20}l} {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right) = X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right) - X\left( {u\left( {t_{i} } \right)} \right),} \hfill \\ {\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right) = Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right) - Y\left( {u\left( {t_{i} } \right)} \right).} \hfill \\ \end{array} } \right.$$

(23)

By substituting $X\left( {u\left( {t_{i + 1} } \right)} \right)$ and $Y\left( {u\left( {t_{i + 1} } \right)} \right)$ described by Eqs. (21) and (22), respectively, into Eq. (20) and multiplying $\Delta t$ on both sides of Eq. (20) simultaneously, and Eq. (20) can be rewritten as:

$$\begin{gathered} v\left( {u\left( {t_{i + 1} } \right)} \right)\Delta t = \hfill \\ \sqrt {\left[ {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right) + \frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}\varepsilon } \right]^{2} + \left[ {\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right) + \frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}\varepsilon } \right]^{2} } . \hfill \\ \end{gathered}$$

(24)

Therefore, the quadratic equation for $\varepsilon$ can be expressed as:

$$\begin{gathered} \left[ {\left( {\frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right)^{2} + \left( {\frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right)^{2} } \right]\varepsilon {}^{2} + \hfill \\ 2\left[ {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)\frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}} +\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)\frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right]\varepsilon + \hfill \\ \left( {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)} \right)^{2} + \left( {\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)} \right)^{2} - v^{2} \left( {u\left( {t_{i + 1} } \right)} \right)\Delta t^{2} = 0. \hfill \\ \end{gathered}$$

(25)

Assuming that:

$$A = \left( {\frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right)^{2} + \left( {\frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right)^{2},$$

$$B = 2\left[ {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)\frac{{{\text{d}}X\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}} +\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)\frac{{{\text{d}}Y\left( {u^{\prime}\left( {t_{i + 1} } \right)} \right)}}{{{\text{d}}u}}} \right],$$

$$C = \left( {\Delta X^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)} \right)^{2} + \left( {\Delta Y^{\prime}\left( {u\left( {t_{i + 1} } \right)} \right)} \right)^{2} - v^{2} \left( {u\left( {t_{i + 1} } \right)} \right)\Delta t^{2}.$$

Eq. (25) can be simplified as:

$$A\varepsilon^{2} + B\varepsilon + c = 0.$$

(26)

The parametric truncation error, $\varepsilon$, can be obtained by solving this equation. Eq. (26) has complex roots or only a real root if $B^{2} - 4AC \le 0$. If $B^{2} - 4AC > 0$ is satisfied, then the equation has two real roots:

$$\varepsilon_{1} = \frac{{ - B + \sqrt {B^{2} - 4AC} }}{2A},\varepsilon_{2} = \frac{{ - B - \sqrt {B^{2} - 4AC} }}{2A}.$$

(27)

Because the value of $\varepsilon_{1}$ is small (close to 0), $\varepsilon_{{2}}$ is negative and has a larger absolute value. A smaller value of root $\varepsilon_{1}$ is desirable to achieve a relatively more accurate compensation during the interpolation process. A more accurate $u\left( {t_{i + 1} } \right)$ can be obtained by substituting $\varepsilon_{1}$ into Eq. (19).

3 Look-ahead Function

In the previous section, a path with a continuous curvature and a mixture of small line segments and curves was obtained. This section presents a real-time bidirectional look-ahead algorithm. The look-ahead function consists of two main modules. The first module is a bidirectional scanning module that utilizes the proposed trajectory planning method to calculate the velocities at each trajectory planning segment point. The second module is trajectory planning based on the quartic polynomial, which includes the proposed double-quartic polynomial trajectory planning and quartic polynomial trajectory planning for line segments and curves, respectively.

3.1 Bidirectional Scanning Look-ahead Module

In the first module, the array of ${\varvec{v}}_{{{\text{limit}}}}$ should be calculated before running the bidirectional scanning module, as expressed by Eq. (28), and the initial parameters should be transmitted to the bidirectional scanning module. ${\varvec{v}}_{{{\text{limit}}}}$ consists of the velocities at the starting and ending points, at the junctions between the line segments and transition curves, and in the middle of the transition curves. The point in the middle of the transition curve was adopted because of the maximum curvature of these curves at the middle points in general. This method can ensure the acceleration and jerk within the limitations in the middle of curves and improve the machining efficiency.

$${\varvec{v}}_{{{\text{limit}}}} \left( i \right) = \min \left( {{}^{{\text{a}}}{\varvec{v}}_{{{\text{limit}}}} \left( i \right),{}^{{\text{j}}}{\varvec{v}}_{{{\text{limit}}}} \left( i \right),v_{{{\text{max}}}} } \right) ,$$

(28)

where ${}^{{\text{a}}}{\varvec{v}}_{{{\text{limit}}}}$ and ${}^{{\text{j}}}{\varvec{v}}_{{{\text{limit}}}}$ are calculated using Eqs. (6) and (7), respectively.

For the global look-ahead module, as shown in Figure 2, the forward scanning look-ahead module is performed first. Velocity $v_{{{\text{start}}}}$ at the start point $P_{1} \left( {P_{v,1} } \right)$ is set to 0 to ensure start running with a zero velocity for robots. ${\varvec{v}}_{{{\text{limit}}}} \left( i \right)$ and ${\varvec{v}}_{{{\text{limit}}}} \left( {i + 1} \right)$, as the initial velocities, are transmitted to the trajectory planning module. If the maximum acceleration or jerk exceeds the limitations of the acceleration or jerk, an appropriate new ${\varvec{v}}_{{{\text{limit}}}} \left( {i + 1} \right)$ that can ensure the acceleration and jerk within the limitations will be calculated by the trajectory planning module to substitute the old value. When the forward scanning is completed, the array of ${\varvec{v}}_{{{\text{limit}}}}$ has preliminary update completed with the start velocity equal to 0, but the end velocity is not usually equal to 0. Hence, in the next step, the backward scanning is performed to ensure that the end velocity is equal to 0.

Figure 2

Schematic diagram of look-ahead module

In the backward scanning look-ahead module, the velocity $v_{{{\text{end}}}}$ at end point $P_{n} \left( {P_{v,3n - 4} } \right)$ was set to 0 to ensure that the robot stopped running at a zero velocity. ${\varvec{v}}_{{{\text{limit}}}} \left( {i{ + 1}} \right)$ and ${\varvec{v}}_{{{\text{limit}}}} \left( i \right)$ in ${\varvec{v}}_{{{\text{limit}}}}$ are transmitted to the trajectory planning module as the initial velocities. If the maximum acceleration or jerk exceeds the limitations of the acceleration and jerk, an appropriate new ${\varvec{v}}_{{{\text{limit}}}} \left( i \right)$ that can ensure that the acceleration and jerk are within the limitations is calculated by the trajectory planning module to substitute the old value.

3.2 Quartic Polynomial Trajectory Planning

For smooth start-stop motion, quartic polynomial and double-quartic polynomial trajectory planning are employed in the curve segments and line segments, respectively. Quartic polynomial trajectory planning can achieve acceleration or deceleration, except constant velocity. Double-quartic polynomial trajectory planning has the advantage of a quartic polynomial, and can achieve a constant velocity. The displacement, $s$, of the quartic polynomial is expressed as:

$$s = c_{0} + c_{1} t + c_{2} t^{2} + c_{3} t^{3} + c_{4} t^{4}.$$

(29)

Because the displacement at the starting point is 0, the velocity at the starting point is $v_{0}$, and the acceleration at the starting point is 0, the partial parameters of the quartic polynomial can be obtained as:

$$\left\{ {\begin{array}{*{20}l} {c_{0} = 0,} \hfill \\ {c_{1} = v_{0} ,} \hfill \\ {c_{2} = 0.} \hfill \\ \end{array} } \right.$$

(30)

Based on the quartic polynomials, two critical equations can be derived:

$$c_{4} = \frac{{ - c_{3} }}{2T},$$

(31)

$$T = \frac{2s}{{v_{0} + v_{1} }},$$

(32)

where $T$ is the total running time, and $s$ is the path length. The maximum acceleration and jerk occur at $\frac{T}{2}$ and 0, respectively.

$$a_{{\text{m}}} = \frac{{6c_{3} T}}{2} + 12c_{4} \left( \frac{T}{2} \right)^{2} ,$$

(33)

$$j_{{\text{m}}} = 6c_{3}.$$

(34)

If $a_{{\text{m}}} > a_{\max }$ or $j_{{\text{m}}} > j_{\max }$, the end velocity of this segment should be changed. Assuming that the maximum jerk just reaches the jerk limitation, $c_{3}$ can be obtained:

$$c_{3} = \frac{1}{6}j_{\max }.$$

(35)

Substituting Eq. (35) into Eq. (31) yields

$$c_{4} = - \frac{{j_{\max } }}{12T}.$$

(36)

Substituting Eqs. (30), (35), and (36) into Eq. (29) gives:

$$\frac{{j_{\max } }}{6}T^{3} + 2v_{0} T - 2s = 0.$$

(37)

Eq. (37) is a cubic equation, the $T$ can be obtained by solving this equation. The velocity corresponding to the end point can then be obtained:

$${\varvec{v}}_{{{\text{limit}}}} \left( i \right) = \min \left( {{\varvec{v}}_{{{\text{limit}}}} \left( i \right),c_{1} + 2c_{2} T + 3c_{3} T^{2} + 4c_{4} T^{3} } \right).$$

(38)

3.3 Double-quartic Polynomial Trajectory Planning

The proposed double-quartic polynomial trajectory planning is similar to quartic polynomial trajectory planning. The differences are that this method may have a constant velocity period and consists of two quartic polynomial trajectory planning, which improves the efficiency for machining long line segments. Double-quartic polynomial trajectory planning has seven forms, as shown in Figure 3. It includes all the advantages of quartic polynomial trajectory planning, except that it is more computationally intensive than quartic polynomial trajectory planning.

Figure 3

Velocity classification of double-quartic trajectory planning

The flowchart of the double-quartic polynomial trajectory planning is shown in Figure 4. If $v_{{{\text{start}}}} = v_{{{\text{end}}}} = v_{{{\text{max}}}}$, then there exists only a constant velocity period. Otherwise, calculate the length of the current path ($s$), $s_{{{\text{vm}}}}$ the length of displacement from $v_{{{\text{start}}}}$ to $v_{\max }$ plus the length of displacement from $v_{\max }$ to $v_{{{\text{end}}}}$, and $s_{{{\text{se}}}}$ the length of displacement from $v_{{{\text{start}}}}$ to $v_{{{\text{end}}}}$. If $s \ge s_{{{\text{vm}}}}$, three cases are possible, each of which includes two of the three of acceleration, deceleration, and constant velocity. Otherwise, there are three cases, including two that can be achieved by quartic polynomial trajectory planning, and the last case that requires double-quartic polynomial trajectory planning.

Figure 4

Flowchart of double-quartic polynomial trajectory planning

When the double-quartic polynomial trajectory planning is determined and the velocity cannot reach $v_{\max }$, the maximum velocity $v^{\prime}_{\max }$ reached during running can be calculated using the algorithm (Table 1).

Table 1 Pseudocode of double-quartic polynomial trajectory planning

Based on the relationships between $v_{{{\text{start}}}}$, $v_{{{\text{end}}}}$, $v_{\max }$, and $s$, the double-quartic polynomial trajectory planning was constructed, and double-quartic polynomial trajectory planning was achieved using the proposed algorithm.

4 Simulation and Experimental Results

The proposed path-smoothing technique and bidirectional look-ahead algorithm were tested through simulations and experiments. First, the proposed method was compared with the B spline and the Blend methods on a regular path in the simulation. Subsequently, an experiment on the drawing horse was conducted using a SCARA robot.

4.1 Simulation Results

For a regular path such as a semicircular outline, the velocity of the look-ahead method gradually increases until it reaches the maximum velocity at the midpoint of the path, and then decreases to 0. The velocity in the Blend method frequently accelerates and decelerates [32]. The velocity of the B spline method [33] may be similar to the velocity of the look-ahead method, but may be less smooth. A simulation was designed to investigate our approach and the proposed algorithms. A regular path with a semicircular outline was selected and divided into 150 line segments. The average length of these small line segments was approximately 1 mm, as shown in Figure 5.

Figure 5

Comparison of semicircular trajectories for the three methods

For these three methods, the limitations of velocity, acceleration, and jerk were set to $2 \times 10^{3} {\text{mm/s}}$, $4 \times 10^{4} {\text{mm/s}}^{{2}}$, and $1.8 \times 10^{7} {\text{mm/s}}^{{3}}$, respectively. The number of look-ahead segments was set to 150, and the blend ratio of the Blend method was set to 50%. The trajectory points, velocity, acceleration, and jerk in the simulation were recorded; the comparison curves are shown in Figures 6–8, respectively.

Figure 6

Comparison of velocity values for the three methods

As shown in Figure 6, the maximum velocity of the look-ahead method is significantly larger than that of the Blend method for a regular path but smaller than that of the B spline method. B spline trajectory planning is the fastest because its trajectory is an entire curve, which is easier to accelerate. However, it is only suitable for offline machining because it uses optimization algorithms, which are computationally intensive and may have no solution. The velocity of look-ahead accelerates to the maximum gradually and then decelerates to 0 gradually. However, the velocity of the Blend method oscillates around a certain value, the maximum value of the Blend method is significantly lower than that of the proposed method. The velocity of the B spline method accelerates to the maximum speed, then is equivalent to a constant speed, and finally decelerates to 0.

As shown in Figures 7 and 8, the acceleration and jerk of the Blend method oscillate around the X-axis, and its maximum acceleration and jerk are significantly larger than those of the proposed method. In addition, the acceleration and jerk of the B spline method sometimes change sharply and their maximum values are larger than those of the look-ahead method. This simulation proves that the operation of the algorithm satisfies our requirements, verifying the effectiveness of the proposed algorithm.

Figure 7

Comparison of acceleration values for the three methods

Figure 8

Comparison of jerk values for the three methods

4.2 Experimental Results

In the actual experiment, a SCARA robot manufactured by RobotPhoenix Automation Technology Co., Ltd. was adopted. A self-developed Gorilla controller was utilized to test the machining ability of the proposed algorithm for a path that consisting of many small continuous line segments. The central processing unit of the Gorilla controller was I5-8500T, and it controlled the motion of the robot using CODESYS Control RTE, which is a real-time software PLC for PC-based industrial controllers in Windows programmable with the IEC 61131-3 development system, CODESYS.

For a horse outline consisting of 760 linear segments with an average length of approximately 1 mm, the robot drew a horse using the algorithm proposed in this study, as shown in Figure 9. As shown in Figure 10, the robot with a pen at the end, the proposed method, and the Blend algorithm were used to draw a picture of the horse. Figure 10(a) and (b) shows that the horse drawn by the look-ahead algorithm is smoother than that drawn by the Blend algorithm. Blend method dithering occurs because the soft tip cannot keep up with the movement of the robot in the case of frequent acceleration and deceleration, demonstrating the effectiveness of the proposed algorithm. The strands in Figure 10(a) do not maintain consistent strand widths because the platform where the paper is located is not parallel to the XY plane of the robot base frame.

Figure 9

Robot drawing a horse

Figure 10

Comparison of paths of proposed and Blend method

The running time of the Blend method and B spline method are equal to that of the proposed method. During the period when the robot drew the picture of a horse, the trajectory points, velocity, and acceleration of the proposed method, B spline method and Blend method were recorded. The trajectory points are shown in Figure 11. Comparisons of the velocity, acceleration, and jerk of the three methods are shown in Figures 12–14, respectively.

Figure 11

Comparison of trajectory paths plotted using the three methods

Figure 12

Comparison of velocity values using the three methods

Figure 12 shows the order of the velocity curve compliance as the look-ahead, B spline, and Blend methods. The acceleration and jerk of the Blend method are significantly larger than those of the B spline and look-ahead methods(Figures 13 and 14), proving the efficiency of the proposed methods. The time required to calculate the look-ahead algorithm with respect to this path was 24.016 ms when looking ahead 20 linear segments at a time, and approximately 0.032 ms were required for each linear segment in the look-ahead module.

Figure 13

Comparison of acceleration values obtained using the three methods

Figure 14

Comparison of jerk values obtained using the three methods

5 Conclusions

A practical path-smoothing method and look-ahead algorithm are presented in this study. A Quintic Bezier curve was used to blend the line segments as continuous curvature transitions. A bidirectional scanning algorithm and quartic polynomial trajectory planning algorithm can decrease the total running time and minimize vibration. The simulation and experimental results verified the effectiveness of this method for robot machining of multiple small line segments.

Compared with previous methods, the algorithms proposed in this study have advantages:

(1) Realization of $G^{2}$ continuity, which mixes line segments and curves.

(2) The bidirectional scanning algorithm efficiently reduces the time spent looking forward and backward, thus achieving the look-ahead function in real time.

(3) The quartic polynomial trajectory planning algorithm can reduce the vibration of the robot at the start and stop.

(4) Compensation of the interpolation algorithm can eliminate truncation error and further enhance movement smoothness.