A high-precision non-uniform rational B-spline interpolator based on S-shaped feedrate scheduling

Parametric interpolation has been widely used in robot control because of its advantages over traditional linear or circular interpolation. The NURBS interpolation based on S-shaped feedrate scheduling can realize the smooth continuous motion of robots, but it also faces two challenges. One challenge is that when the NURBS curve is split into multiple blocks, if one or more blocks are too short, the final feedrate profile is often discontinuous. The other challenge is that the NURBS curve cannot be obtained entirely due to calculation errors. In this paper, a high-precision NURBS interpolator based on S-shaped feedrate scheduling is proposed. In this interpolator, a bidirectional velocity scanning scheme is developed to effectively handle the too short split blocks of the NURBS curve. Meanwhile, an optimization algorithm is proposed to ensure the precision of NURBS curves. The feasibility and advantages of the proposed method are verified through case studies.


Introduction
Parametric interpolation for the non-uniform rational B-spline (NURBS) curve has become an important part of the control of robot path planning [1,2]. An effective NURBS interpolator can not only obtain accurate contour trajectories but also have smooth dynamic performance. The discontinuities in feed motion can evoke the natural modes of the mechanical structure and the servo control system, which will degrade the contouring accuracy [3]. To achieve a shock-free and smooth machining process, researchers have considered jerks when designing interpolators.
With the inequality constraints of physical limits including joint velocity, acceleration, and jerk, an iterative optimization strategy that takes the motion time as the optimization goal was proposed in [4,5]. In [6], Hashemian et al. proposed to utilize the reparameterization technique which expresses the relationship between the motion path parameter of the flank machining toolpath and the system time variable by an optimal transfer function to minimize the total jerk of the tool's motion. Compared with these optimization strategies, the S-shaped feedrate profile does not require iterative optimization while satisfying the physical limits, so it is widely used in the field of robotics. An S-shaped feedrate scheduling approach was proposed for the NURBS interpolator to ensure the continuity of acceleration and control the range of jerk in [7,8]. Ni et al. [9] also developed a bidirectional adaptive feedrate scheduling method based on the S-shaped ACC/DEC algorithm for high-speed twp-axis machining. Though the S-shaped method is widely used in smooth feedrate scheduling because of its simplicity and smoothness, the conventional S-shaped feedrate profile usually cannot deal with all the conditions, e.g., the target displacement is too short. In this case, the robot cannot reach the ending velocity from the starting velocity even by accelerating or decelerating with the max acceleration and jerk. To address this issue, this paper proposes a bidirectional velocity scanning scheme to overcome this drawback of the S-shaped feedrate profile.
Another big challenge is the calculation error that consists of two parts: the error caused by the inaccurate estimation of the arc length, and the truncation error caused by Taylor expansion [10,11]. With these errors, it is difficult to achieve highly precise and integral parametric planning. Many studies have been carried out in this field. A high-order polynomial spline was utilized to fit the relationship of arc length and the curve parameter in [12]. A quartic equation-based interpolation method was proposed in [13], which works well in reducing feedrate fluctuation. Lo proposed a feedback parametric interpolation method, which approximates the desired point iteratively [14]. A realtime predictor-corrector interpolator was proposed by Tsai and Chen which can guarantee that the feedrate command error is within a specified tolerance [15]. Besides, a feedback interpolator with arc-length compensation and feedback correction was developed by Zhao et al. in [16,17], which achieves good efficiency and accuracy. In this paper, an interpolation method is proposed based on the S-shaped profile with arc length prediction and feedback correction.
The process of the proposed method is as follows. Firstly, the NURBS curve is partitioned into several continuous blocks, and the speed at the junctions is generated based on the limitation of the maximum tangential acceleration, the maximum velocity, the chord error tolerance, etc. Then, a bidirectional velocity scanning is employed to calculate the ultimate feedrate at the junctions between the blocks. Afterward, the Simpson rule and the adaptive-corrected method proposed in this study are utilized to estimate the arc length of each NURBS sub-curve. Finally, the NURBS parameter u is calculated through Taylor's expansion.
The main contributions of this paper are summarized as follows: • A bidirectional velocity scanning scheme is proposed to deal with the case of too short target displacement of S-shaped feedrate profile. • An interpolation feedrate interpolator based on S-shaped profile with arc length prediction and feedback correction is proposed to ensure precise and integral NURBS curve planning.
The rest of this paper is organized as follows. In Sect. 2, the background including the definition of the NURBS curve, the derivatives of the NURBS curve, the curvature of the NURBS curve, and the method to calculate its arc length are briefly introduced. Then, a complete feedrate scheduling scheme for the NURBS curve is introduced in Sect. 3. In Sect. 4, experiments are conducted on a 6-DOF industrial robot system, and the conclusions are presented in Sect. 5.

Definition of a NURBS curve
A NURBS curve C(u) can be defined as follows: where {P i } indicates the control points; {w i } indicates the corresponding weights of {P i } ; (n + 1) is the number of control points; p is the degree of a NURBS curve; {N i,p (u)} indicates the B-spline basis functions of the p-th degree defined on the non-uniform knot vector (m + 1) is the number of knots. Generally, it is assumed that a = 0 , b = 1, and w i > 0 for all i . The degree p , the number of control points (n + 1), and the number of knots (m + 1) satisfy m = n + p + 1.
The B-spline basis functions {N i,p (u)} of the p-th degree are recursively defined as follows: In this paper, the knot vector is derived following the Rosenfeld method [18]: where l i is the straight-line distance between the (i − 1)-th control point and i-th control point, and L is the sum of l 1 , l 2 , … , l n . Moreover, when w i P i and {w i Q} are represented as the control points, two B-spline curves A(u), B(u) can be defined as: where Q is a constructed vector. It has the same dimension as the P i , and all elements of Q are 1. Then, the NURBS curve Eq. (1) can be rewritten as: where B st (u) represents the first term of vector B(u).

Curvature of a NURBS curve
For a NURBS curve of the p-th degree, it has derivatives of the d-th order where d ≤ p . According to Eq. (7), the firstand second-order derivatives of a NURBS curve, i.e., C � (u) and C ε (u) , can be evaluated as follows: As shown in Eqs. (5) and (6), A(u) and B(u) can be treated as two different B-spline curves. Assuming that H i = w i P i , the k-order derivatives of B-spline curve A(u) can be written as: where k is the order of derivatives, and Similarly, B <k> (u) can be obtained. Therefore, C � (u) and C �� (u) can be calculated based on A <k> (u) and B <k> st (u) . Then, the curvature of a NURBS curve is calculated, as shown in Eq. (12).

Arc length of a NURBS curve
The arc length In this study, a numerical integration method, i.e., the Simpson rule, is adopted to approximately estimate the arc length of a NURBS curve. S(u i , u i+1 ) represents the approximate arc length of the NURBS curve C(u) on the interval [u i , u i+1 ] , and it can be calculated by: However, the direct calculation of the Simpson rule will cause approximation error in the numerical result. The issue can be addressed by bisecting the parameter interval [u i , u i+1 ] into two equal sub-intervals, which is called the composite Simpson rule. Then, Eq. (14) is repeatedly applied over each subinterval: Assuming that is the tolerance for the parameter interval, we have: If the condition in Eq. (16) cannot be satisfied, the two subintervals in Eq. (15) will be divided into two subintervals again. The loop repeats until Eq. (16) holds for each subinterval. Finally, the sum of the arc lengths of all subintervals is the real length of the arc. In this study, f s (a, b) is defined as the iteratively calculated arc length between a and b.

NURBS parameter and curve interpolation
The second-order Taylor's expansion is widely used to establish the relationship between the NURBS parameter and the arc length. It is shown as follows: where u i is the curve parameter when t = t i , and T s is the sampling period. After derivation, it can be rewritten as follows: where V(t) and A(t) are the desired velocity and acceleration of the NURBS curve, respectively.

S-shaped feedrate scheduling based NURBS interpolator
In this section, the division of the curve into several blocks by using the curvatures is introduced in Sec 3.1. Then, the S-shaped feedrate profile is introduced briefly in Sect. 3.2. In 3, the precise calculation of the arc length of blocks and NURBS parameter is described.

Critical points and curve segmentation
In NURBS curves, the sharp corners with large curvature can undermine the dynamic performance of high-speed machining. Thus, it is critical to segment each block at the key points that are commonly referred to as sharp corners. The feedrate velocity over the candidate points can be calculated based on the two constraints of chord error and centripetal acceleration [19]. Constrained by the chord error, the feedrate should satisfy Eq. (19): where i is the curvature at the i-th interpolation point; max is the maximum chord error, and T s is the sampling period.
Constrained by the centripetal acceleration, the feedrate should also satisfy Eq. (20): where A cmax is the maximum centripetal acceleration, so the restricted feedrate v R i can be expressed as follows: where V max is the allowable max feedrate of the actuator. Ultimately, the critical points are found from the candidate points, where v R i is less than the feedrate command. Based on these critical points, the NURBS curve can be split into several continuous blocks.

S-shaped feedrate profile
The S-shaped feedrate profile consisting of seven phases is shown in Fig. 1. The expressions of S-shaped velocity, acceleration, and jerk in seven phases are summarized in Table 1. The symbols t i (i = 1, 2, … , 7) represent the time parameters.where v s and v e are the starting and ending velocities, respectively; A m is the allowable maximum acceleration, and J m is the maximum jerk of the actuator. As the change of target displacement, initial velocity, end velocity, maximum acceleration, and jerk, there are 17 types of the S-shape feedrate profile. It is critical to formulate an S-shape feedrate profile basic function that can deal with all 17 cases. The velocity and acceleration profiles of the 17 cases are provided in the Appendix. A more detailed formula expression of the 17 types S-shape feedrate profile is given by Sencer et al. in [4].

Bidirectional velocity scanning
If the target displacement is too short but the velocity calculated by Eq. (21) is big, the system cannot reach the ending velocity from the starting velocity even by accelerating or decelerating with the max acceleration and jerk.
Besides an S-shape feedrate profile basic function that can deal with all 17 cases, a bidirectional velocity scanning scheme (forward velocity and backward velocity scanning) is also needed to correct the starting or ending velocity in these cases.

Forward velocity scanning
In this case, v s < v e , where v s and v e are the starting and ending velocities, respectively. According to the 7-segment S-shape velocity profile, the minimum acceleration time t a and displacement s a required for the velocity from v s to v e in the pure acceleration stage can be expressed as: Assume that s is the target displacement. If s < s a , then the velocity cannot reach v e within this displacement, and the ending velocity needs to be revised. As shown in Eq.
indicate the original planning trajectory and the updated after velocity scanning, respectively.

The high-precision NURBS interpolator
The Simpson bisection iterative algorithm can obtain the accurate arc length of each block after multiple iterations. Unfortunately, due to the truncation error of the Taylor expansion, the calculated parameter u usually cannot be accurately planned to 1. Therefore, the actuator cannot completely track the NURBS curve. To handle this problem, this paper proposes an iteration algorithm to calculate the block displacement with NURBS parameter u as the optimization target. The algorithm is as follows: Assume that the number of breakpoints is n . Then, the number of blocks is (n + 1) , and the arc length of blocks is s 0 , s 1 , ⋯ , s n . Denote the total arc length of the NURBS curve as S . S H and S L satisfy the inequality S H > S > S L , and ΔS = S H − S L .
Firstly, the Simpson bisection iterative algorithm is used to calculate the arc length of each block. The velocities and accelerations of blocks can be calculated using the S-shape feedrate profile. Then, with these velocities and accelerations, the NURBS parameter u can be calculated by Eq. (18). Usually, the NURBS parameter u cannot meet the precision requirement due to the truncation error. Thus, S H and S L are modified by a dichotomy searching, and the arc length of each block is modified according to Eq. (26). The loop will  The whole algorithm is described in Algorithm 1:where Δl is a small offset, and eps is the accuracy tolerance. They are set to 5mm and 0.000005 in this paper, respectively.
To describe the test curve as accurately as possible, the curve including 100,001 sampling points with a sampling spacing Δu = 0.00001 is shown in Fig. 3a. The curvature curve corresponding to the sampling points is shown in Fig. 3b. As shown in Fig. 3b, the curvature threshold is set as to 0.25, and 19 curvature extreme points can be selected. According to the constraints of chord error and centripetal acceleration, the critical points are found from the sampling points, and they are also shown in Fig. 3a as black dots. The maximum velocity at each critical point can be determined by Eq. (21).
The interpolator parameters used for the butterfly-shaped test curve are listed in Table 2.
The Simpson iterative algorithm and the method proposed in this paper are used to calculate the arc length of each block. The calculation results obtained by the Simpson iterative algorithm are as follows: s 0 = 0.585141 , It can be found that for compensating the truncation error, the arc length of each block calculated by the proposed method in this paper increases little compared with that calculated by the Simpson iterative algorithm. The arc length of the first and last block is all too short for the S-shaped feedrate profile, so this study corrects the velocity of these junctions through the bidirectional velocity scanning scheme. Figure 4 shows the velocity, acceleration, and jerk curves of the S-shaped feedrate profile calculated by our method. The ending velocities obtained by the Simpson iteration method and our method are 0.0043165 mm∕s and 0.00042 mm∕s , respectively. Obviously, our proposed method achieves better accuracy. The NURBS parameter u of the two methods can be calculated by Eq. (18). As shown in Fig. 5a, the parameter u obtained by the two methods is basically the same, i.e., 0.998752 and 0.999993.
As shown in Fig. 6, the method based on the Simpson iteration algorithm leaves a small blank space at the end of the NURBS curve that cannot be executed. By contrast, the method proposed in this study obtains the NURBS curve precisely. Figure 5b shows the iterative process of calculating the total arc length of the NURBS curve using the method proposed in this paper. It is found that the ideal situation is reached immediately after 10 iterations, and the whole process takes less than 2.5 s. It indicates that the proposed algorithm improves the NURBS interpolation precision for offline trajectory planning.

Experimental results
Corresponding to the simulation tests, the butterflyshaped curve is drawn by a real industrial robot. As shown in Fig. 7, the experiment was implemented on a 6-DOF ROKAE (XB7) light-weight industrial robot, and a carbon pen was installed on the wrist of the robot. Meanwhile, the existing commercial motor drive of the ROKAE was utilized to control the joint position. The task-level and intermediate-layer computation was run on a 64-Bit Linuxbased real-time controller equipped with an Intel Core-i7 CPU with 8 GB memory. The controller communicates with the motor drive via the EtherCAT protocol at a sampling rate of 1 kHz. The butterfly-shaped NURBS curve drawn by the 6-DOF industrial robot is shown in Fig. 8. As shown in the video of the attachment, the robot runs smoothly and stably throughout the execution process.

Conclusion
This paper proposes a high-precision S-shaped feedrate scheduling scheme for the NURBS interpolator. It consists of curve segmentation, bidirectional velocity scanning, and NURBS curve interpolation. Meanwhile, an interpolation method is proposed based on Taylor's expansion with arc length prediction and feedback correction. This method effectively improves the accuracy of NURBS curve planning.