Joint Motion Planning of Industrial Robot Based on Modified Cubic Hermite Interpolation with Velocity Constraint

Featured Application

Industrial robot joint smooth movement via multiple positions with constrained velocities.

Abstract

As for industrial robots’ point-to-point joint motion planning with constrained velocity, cubic polynomial planning has the problem of discontinuous acceleration; quintic polynomial planning requires acceleration to be specified in advance, which will likely cause velocity to fluctuate largely because appropriate acceleration assigned in advance is hardly acquired. Aiming at these problems, a modified cubic Hermite interpolation for joint motion planning was proposed. In the proposed methodology, knots of cubic Hermite interpolation need to be reconfigured according to the initial knots. The formulas for how to build new knots were put forward after derivation. Using the newly-built knots instead of initial knots for cubic Hermite interpolation, joint motion planning was carried out. The purpose was that the joint planning not only satisfied the displacement and velocity constraints at the initial knots but also guaranteed C² continuity and less velocity fluctuation. A study case was given to verify the rationality and effectiveness of the methodology. Compared with the other two planning methods, it proved that the raised problems can be solved effectively via the proposed methodology, which is beneficial to the working performance and service life of industrial robots.

1. Introduction

Industrial robots are widely used in handling, painting, welding, assembly, packaging and other fields. With the development of robot technology, industrial robots are gradually applied to polishing, monitoring, automatic manufacture system and so on [1,2]. The manipulator movements become more complex and need to satisfy higher working performances. Therefore, trajectory planning of industrial robots has to meet higher requirements. The most basic task of industrial robot motion planning is to meet the requirements for end-effector displacement and velocity [3]. Moreover, favorable trajectory planning should ensure smooth movements, fewer vibrations, impacts and mechanical wear so that it can improve working performance and extend service life [4].

Motion planning of industrial robots can be divided into two types: one is in the workspace for end-effector, the other is in joint space for joints. For motion planning in the workspace, the inverse kinematics function needs to be solved repeatedly to calculate angular displacements and angular velocities of joints, which requires a large amount of calculation [5]. Furthermore, end-effector singularity possibly appears and will make joints rotate incorrectly so that the end-effector cannot complete work successfully. For motion planning in joint space, only a few inverse kinematics solutions are needed to find out the angular displacements and angular velocities at several target points of joints. Therefore, motion planning in joint space can reduce inverse kinematics calculation largely so that computation efficiency is improved, and also can avoid joints singularity and redundancy [6]. As a result, motion planning in joint space is more suitable for the point-to-point (PTP) motion of industrial robots.

The main methods of joint motion planning are reviewed below. Linear planning with parabolic transition has the advantage that joint speed remains constant in the linear phase, but sufficient transition time must be ensured, and angular acceleration is not continuous. The widely used cubic polynomial planning has the advantages of simple principle and less computation. However, when it is used to multiple points with constrained velocities, the angular acceleration at the connection points is broken, which will lead to vibrations and impacts and make the robot arm tremble. Quintic polynomial planning has continuous angular acceleration. However, quintic polynomial interpolation requires specifying in advance the angular acceleration value of the joint at each target point. If it is not preset properly, the angular velocities will fluctuate back and forth, which is not conducive to joint motion control. Frustratingly, it is difficult to give appropriate angular acceleration values in advance [7]. In addition, quintic multinomial has a higher degree than cubic polynomial so that velocities and accelerations change more intensely, and larger velocity and acceleration peaks appear. B spline interpolation is also used in motion planning [8,9,10]. However, when the cubic B-spline curve is used for multiple points with constrained velocities, acceleration is not continuous at connecting points as well. Some researchers carried out lots of studies about cubic B spline and realized G² continuity at the connecting knots [11,12]. However, G² continuity refers to curvature continuity, and continuity of second-order derivative C² is not fully achieved.

In order to solve the complicated joint planning with multiple points further, Xiangrong Xu proposed a 3-5-3 polynomial splicing method [13]. This method allows to specify angular velocity at the intermediate target points arbitrarily, and angular acceleration is continuous. However, when there are more than 5 knots, angular acceleration at the target knot needs to be given in advance for quintic polynomial. Because it is hard to assign appropriate acceleration in advance, the method will also cause large angular velocity fluctuation. Saeed B. Niku proposed a 4-3-4 polynomial splicing method [14], which guarantees C² continuity at connection points. However, only at the beginning and the end target points can velocities be specified. If angular velocities at the intermediate target points are specified, angular acceleration will have broken points and be discontinuous. P. A. Parikh et al. used seventh-order polynomials and ninth-order polynomials to carry out joint planning [15]. The joint rotation trajectory was smooth enough in high order and the joint velocity and acceleration were continuous. However, the computation of high-order polynomial was large, the velocity and acceleration were easy to fluctuate, and the peak values of acceleration and velocity were much higher.

In summary, for PTP joint motion planning via multiple points with constrained velocities, if planning with low-order functions is adopted, angular accelerations at intermediate target points are discontinuous, which will bring additional impacts and vibrations. If planning with high-order functions is adopted [16,17], the problem of acceleration discontinuity can be solved. However, it is necessary to give angular acceleration values at the target points in advance. Since it is hard to give appropriate angular accelerations in advance, angular velocities are likely to fluctuate largely, which is unfavorable to motion control. Moreover, the higher order of planning function, the more intense change of planning curves and the larger the peak value of velocity and acceleration. All these are unfavorable for the steady operation of the robot arm. In view of the problems in the plannings mentioned above, a modified Hermite interpolation is proposed in the paper. In the proposed planning, velocities at knots can be specified at will and C² continuity is guaranteed. Moreover, there will be no drastic fluctuations in angular velocity since the maximum order of the planning function is no more than three and acceleration preset is not needed.

2. Point-to-Point Joint Motion via Multiple Points with Constrained Velocities

The End-effector of the industrial robot is the final executive component performing operations. The main types of end-effector motion are PTP (point to point) movement and CP (continuous path) movement. The movement of the end-effector is the result of joint motion. So there are corresponding relationships between end-effector movement and joint motion.

2.1. Point-to-Point Joint Motion

PTP (point to point) motion of the industrial robot is suitable for spot welding, handling, loading and unloading, circuit board insertion and other operations. In PTP motion mode, the end-effector moves from a start point to an end point. PTP motion only needs to specify positions and orientations of end-effector at the start point and end point, but the trajectory and orientations between the start point and end point are not specified or required.

When the end-effector moves in PTP mode, joints rotates normally in PTP mode as well. As shown in Figure 1, take joint 1 as an example. When the end-effector wants to move from start point P_a to end point P_b, joint 1, which is one of the six joints, needs to rotate from the start point A to end point B. In general, other joints rotate similarly. Finally, rotations of six joints work together to form the movement of the end-effector.

There is a corresponding functional relationship between the position and orientation of the end-effector and the angular displacement of each joint, as shown in formula (1). There is also a corresponding functional relationship between the end-effector velocity and the angular velocities of each joint, as shown in formula (2).

$$f({\mathit{\theta }}_i)={[x y z \alpha \beta \gamma ]}^T$$

(1)

$$\mathit{J}\cdot {\mathit{\omega }}_i{}^T={[v_x v_y v_z {\omega }_x {\omega }_y {\omega }_z]}^T$$

(2)

In formula (1), [x y z] and [α β γ] are position and orientation of end-effector, θ_i are angular displacements of each joint. In formula (2), [v_x v_y v_z] and [ω_x ω_y ω_z] are linear velocity and angular velocity of end-effector, ω_i are angular velocities of each joint, and J is the velocity Jacobian matrix.$f({\mathit{\theta }}_i)$ and J vary in different robots. Formulas (1) and (2) are called forward motion functions, by which the end-effector motion can be obtained from joint motion.

According to the position, orientation and speed of the end-effector, angular displacement and velocity of each joint can be obtained, which is called inverse kinematics, as shown in formulas (3) and (4) below. The speeds of the end-effector at the start and end points are zero in PTP movement so that angular velocities at the start and end points of each joint are also zero. As seen in Figure 1, the angular velocities of joint 1 at the start and end points are all equal to zero, ω_a = ω_b = 0.

$${\mathit{\theta }}_i{}^T=f^{-1}({\mathit{\theta }}_i)\cdot {[x y z \alpha \beta \gamma ]}^T$$

(3)

$${\mathit{\omega }}_i{}^T={\mathit{J}}^{-1}\cdot {[v_x v_y v_z {\omega }_x {\omega }_y {\omega }_z]}^T$$

(4)

On the basis of angular displacement and velocity constraints at the start point and end point of each joint, motion plannings of each joint can be carried out and these joint motion plannings are finally synthesized into the end-effector motion, as shown in Figure 2.

2.2. Point-to-Point Joint Motion via Multiple Points with Constrained Velocities

When there are multiple intermediate target points between the start and end points [18], and the end-effector needs to pass through these intermediate points at specified speeds, we briefly call it PTP motion with constrained speed. When the end-effector moves in speed-constrained PTP mode, joints correspondingly perform velocity-constrained PTP rotation, and both angular displacements and angular velocities at intermediate target points of joints must be specified, so as to meet position, orientation and speed requirements of the end-effector at the target points.

As shown in Figure 3, the end-effector moves from start point P_a to end point P_d. It needs to pass through two intermediate target points P_b and P_c at specified speeds, and joints should rotate accordingly. Taking joint 1 as an example, joint 1 correspondingly rotates from the start point A to end point D, passing through the intermediate target points B and C at constrained velocities. Therefore, joint 1 carries out PTP motion via multiple points at constrained velocities. By solving the inverse kinematics function, all joints’ angular displacements and angular velocities at each target point can be obtained from the positions, orientations and speeds of the end-effector, so that the rotation trajectory of joint 1 can be piecewise-planned, from point A to point B, from B to C, and from C to D. Other joints can carry out similar rotation plannings. Finally, the movement of the end-effector is synthesized by all joints’ rotations together.

3. Joint Motion Planning Method

The most used joint planning methods are cubic polynomial and quintic polynomial.

3.1. Cubic Polynomial Planning

Polynomial interpolation is commonly used to interpolate between discrete points, which is consistent with PTP motion characteristics of industrial robots, so it is often used in motion planning of industrial robots. Among the polynomials, the cubic polynomial is widely used in the PTP motion planning of industrial robots. Cubic polynomial interpolation function is shown as below formula (5):

$$\theta (t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3$$

(5)

where t is the time variable and a₀, a₁, a₂ and a₃ are four coefficients of the cubic polynomial, which need to be found out according to the boundary constraints of angular displacement and angular velocity at two adjacent target points.

Put the start time, the end time, the angular displacements and the angular velocities at the two target points into formula (5) and its first derivative, respectively. After sorting, Equation (6) for solving the coefficients can be obtained.

$$\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\\ a_3\end{array}\right]={\left[\begin{array}{cccc}1& t_0& t_0^2& t_0^3\\ 1& t_1& t_1^2& t_1^3\\ 0& 1& 2t_0& 3t_0^2\\ 0& 1& 2t_1& 3t_1^2\end{array}\right]}^{-1}\left[\begin{array}{c}{\theta }_0\\ {\theta }_1\\ {\omega }_0\\ {\omega }_1\end{array}\right]$$

(6)

where, t₀ is the start time, and t₁ is the end time. θ₀ and ω₀ are respectively angular displacement and angular velocity at the first target point of a joint, and θ₁ and ω₁ are respectively angular displacement and angular velocity at the next target point. θ₀, θ₁, ω₀, ω₁ can be obtained through inverse kinematics. For general PTP motion, ω₀ and ω₁ are both zeros. For velocity-constrained PTP motion, ω₀ and ω₁ may not be zeros. After finding out the coefficients, the cubic polynomial planning function can be obtained, which will satisfy the boundary constraints of angular displacement and angular velocity at the target points.

Because of simplicity, clarity and small computation, the cubic polynomial is widely used in PTP joint motion planning. However, if a cubic polynomial is applied to PTP motion planning with constrained velocity, angular acceleration is discontinuous at the connecting points between every two adjacent planning segments, as shown in Figure 4c. Broken points of acceleration will cause vibrations and impacts, which is disadvantageous to the working performance and service life of the industrial robot.

3.2. Quintic Polynomial Planning

A quintic polynomial is also commonly used in the joint motion planning of industrial robots [19,20]. Quintic polynomial planning can guarantee angular acceleration continuity at the connection points between every two adjacent planning segments. A quintic polynomial function is shown in formula (7) below.

$$\theta (t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$$

(7)

where t is the time variable. a₀, a₁, a₂, a₃, a₄ and a₅ are six coefficients of the cubic polynomial, which need to be solved according to the boundary constraints of angular displacement, angular velocity and angular acceleration at two adjacent target points.

Similarly, put the start time, the end time, the angular displacements, the velocities, and the preset accelerations at two target points into formula (7) and its first and second derivatives, respectively. After sorting, Equation (8) for solving the coefficients can be obtained.

$$\left[\begin{array}{c}{a}_0\\ a_1\\ a_2\\ a_3\\ a_4\\ a_5\end{array}\right]={\left[\begin{array}{cccccc}1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 1& t_1& t_1^2& t_1^3& t_1^4& t_1^5\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 1& 2t_1& 3t_1^2& 4t_1^3& 5t_1^4\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\\ 0& 0& 2& 6t_1& 12t_1^2& 20t_1^3\end{array}\right]}^{-1}\left[\begin{array}{c}{\theta }_0\\ {\theta }_1\\ {\omega }_0\\ {\omega }_1\\ {\epsilon }_0\\ {\epsilon }_1\end{array}\right]$$

(8)

where, t₀, t₁, θ₀, θ₁, ω₀, ω₁ have the same physical meanings as in formula (6) and ε_a and ε₁ are the angular accelerations at the target points, which need to be given arbitrarily in advance. After finding out the coefficients, the quintic polynomial planning function can be obtained. As can be seen from Equation (8), as long as the angular accelerations at the connection point of two adjacent plannings are set equal, the angular acceleration continuity is consequent.

When a quintic polynomial is used for joint motion planning, it is required to specify in advance the angular acceleration values at target points. If the angular acceleration value is not preset appropriately, the angular velocity will fluctuate greatly. As shown in Figure 5, due to the improper preset of angular acceleration at points A and B, the angular velocity planning curve ${\omega }_1(t{)}^{\prime }$ and curve ${\omega }_1(t{)}^{″}$ between points A and B fluctuate largely. The joint motor needs to accelerate and decelerate back and forth, which is disadvantageous to the control of joint angular velocity.

4. Joint Motion Planning Based on Modified Hermite Interpolation

4.1. Function Transformation between Different Intervals

Since the variable interval of cubic Hermite interpolation is [0,1], and the variable interval of industrial robot motion planning is generally not [0,1], function transformation between different intervals is needed. There is an arbitrary function expressed as $y=f(u), u\in [u_0,u_1]$. Given that the function is symbolically independent, this function can also be expressed as $y=f(x), x\in [x_0,x_1], x_0=u_0, x_1=u_1$. When the interval [x₀, x₁] is not equal to the interval [u₀, u₁], that is to say, the variable interval changes, the function expression should be transformed into the following formulas (9) and (10) in order to keep the function values of y unchanged.

$$u=u_0+\frac{(u_1-u_0)(x-x_0)}{(x_1-x_0)},u\in [u_0,u_1],x\in [x_0,x_1]$$

(9)

$$y=f(u)=f\left[u_0+(u_1-u_0)\frac{x-x_0}{x_1-x_0}\right]=f(x),x\in [x_0,x_1]$$

(10)

When function interval is changed from [u₀, u₁] to [x₀, x₁], the transformed function $y=f(x)$ can keep the values of the initial function $y=f(u)$ unchanged, but the first-order and second-order derivatives of the function y with respect to u are no longer equal to that of the function y with respect to x, as shown in Equations (11) and (12).

$$\frac{dy}{du}=\frac{dy}{dx}\cdot \frac{dx}{du}=\frac{dy}{dx}\cdot \frac{1}{\frac{du}{dx}\cdot }=\frac{dy}{dx}\cdot \frac{x_1-x_0}{u_1-u_0}$$

(11)

$$\frac{d^{2}y}{d{u}^2}=\frac{d(\frac{dy}{du}) }{du}=\frac{d(\frac{dy}{du}) }{dx}\cdot \frac{dx}{du}=\frac{d(\frac{dy}{dx}\cdot \frac{x_1-x_0}{u_1-u_0}) }{dx}\cdot \frac{1}{\frac{du}{dx}}=\frac{d^{2}y}{d{x}^2}\cdot {(\frac{x_1-x_0}{u_1-u_0})}^2$$

(12)

Since the variable interval of cubic Hermite interpolation is [0,1], the following Equations below can be obtained in the interval [x₀,x₁] instead of the interval [0,1] according to the above eqations. These formulas can be used in the following study.

$$u=\frac{(x-x_0)}{(x_1-x_0)},u\in [0,1],x\in [x_0,x_1]$$

(13)

$$y=f(\frac{x-x_0}{x_1-x_0}),x\in [x_0,x_1]$$

(14)

$$\frac{dy}{du}=\frac{dy}{dx}\cdot (x_1-x_0)$$

(15)

$$\frac{d^{2}y}{d{u}^2}=\frac{d^{2}y}{d{x}^2}\cdot {(x_1-x_0)}^2$$

(16)

4.2. Cubic Hermite Interpolation

Cubic Hermite interpolation is a commonly used interpolation method. There is n points to be interpolated, (x₀, y₀), (x₁, y₂), …, (x_n-1, y_n-1). Cubic Hermite interpolation function of the i-th curve in the interval [x_i-1, x_i], whose interval is normalized, can be expressed as the following formula (17) in the normalized interval [21].

$$y_i(u)=\left[1\hspace{1em}u\hspace{1em}{u}^2\hspace{1em}{u}^3\right]\left[\begin{array}{cccc}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill -3\hfill & \hfill 3\hfill & \hfill -2\hfill & \hfill -1\hfill \\ \hfill 2\hfill & \hfill -2\hfill & \hfill 1\hfill & \hfill 1\hfill \end{array}\right]\left[\begin{array}{c}{y}_{i-1}\\ y_i\\ {\mathrm{\Delta }}_i{\dot{y}}_{i-1}\\ {\mathrm{\Delta }}_i{\dot{y}}_i\end{array}\right], u\in [0,1]\hspace{1em}(i=1,2,\cdots ,n-1)$$

(17)

where n is the number of interpolated points. u is the variable derived from variable x, and the interval of u is [0,1] obtained by normalization of the interval [x_i-1, x_i]. y_i-1 and y_i are the two interpolated data points of the i-th interpolation curve. ${\dot{y}}_{i-1}$ and ${\dot{y}}_i$ are the two first-order derivatives of y with respect to x at the points x = x_i−1 and x = x_i. Δ_i is the interval span without normalization, ${\mathrm{\Delta }}_i=x_i-x_{i-1}$. According to formula (15), ${\mathrm{\Delta }}_i{\dot{y}}_{i-1}$ and ${\mathrm{\Delta }}_i{\dot{y}}_i$ are the two first-order derivatives of y with respect to u at the points u = 0 and u = 1.

According to Equations (13) and (14), formula (17) can be rewritten as the following formula (18), and the variable interval is changed from [0,1] to [x_i−1, x_i]. The piecewise cubic Hermite interpolation curve is shown in Figure 6.

$$y_i(x)=\left[1\hspace{1em}\frac{x-x_{i-1}}{x_i-x_{i-1}}\hspace{1em}{(\frac{x-x_{i-1}}{x_i-x_{i-1}})}^2\hspace{1em}{(\frac{x-x_{i-1}}{x_i-x_{i-1}})}^3\right]\left[\begin{array}{cccc}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill -3\hfill & \hfill 3\hfill & \hfill -2\hfill & \hfill -1\hfill \\ \hfill 2\hfill & \hfill -2\hfill & \hfill 1\hfill & \hfill 1\hfill \end{array}\right]\left[\begin{array}{c}{y}_{i-1}\\ y_i\\ (x_i-x_{i-1}){\dot{y}}_{i-1}\\ (x_i-x_{i-1}){\dot{y}}_i\end{array}\right],\hspace{1em}x\in [x_{i-1},x_i]\hspace{1em}(i=1,2,\cdots ,n-1)$$

(18)

It can be seen from formula (17) or formula (18), C⁰ and C¹ continuity can be guaranteed as long as positions and first-order derivatives at the connection point between every two adjacent piecewise curves are specified equal. Whatever the values of y₀, y₁, …, y_n−1 and ${\dot{y}}_0$,${\dot{y}}_1$, …, ${\dot{y}}_{n-1}$ are assigned to, the continuity of y(x) and $\dot{y}(x)$ is guaranteed.

However, arbitrarily specified ${\dot{y}}_0$, ${\dot{y}}_1$, …, ${\dot{y}}_{n-1}$ cannot guarantee the continuity of the second-order derivative $\ddot{y}(x)$. Therefore, in order to guarantee C² continuity of cubic Hermite curves at each knot, ${\dot{y}}_0$,${\dot{y}}_1$, …,${\dot{y}}_{n-1}$ must satisfy certain conditions.

4.3. Requirement for C² Continuity of Cubic Hermite Interpolation Curves

The requirement for C² continuity at the connecting point between two adjacent cubic Hermite curves y_i(x) and y_i+1(x) is shown as the following Equation (19):

$${\ddot{y}}_i(x_i)={\ddot{y}}_{i+1}(x_i)$$

(19)

According to Equation (18), we can obtain Equations (20) and (21) below.

$${\ddot{y}}_i(x_i)=\frac{6(y_{i-1}-y_i)+2(x_i-x_{i-1}){\dot{y}}_{i-1}+4(x_i-x_{i-1}){\dot{y}}_i}{{(x_i-x_{i-1})}^2}$$

(20)

$${\ddot{y}}_{i+1}(x_i)=\frac{6(y_i-y_{i+1})+4(x_{i+1}-x_i){\dot{y}}_i+2(x_{i+1}-x_i){\dot{y}}_{i+1}}{{(x_{i+1}-x_i)}^2}$$

(21)

By sorting out Equations (19)–(21), formula (22) can be obtained below. First-order derivatives ${\dot{y}}_{i-1}$, ${\dot{y}}_i$ and ${\dot{y}}_{i+1}$ at every three adjacent knots must meet formula (22) to guarantee C² continuity at connection points.

$$(x_{i+1}-x_i){\dot{y}}_{i-1}+2(x_{i+1}-x_{i-1}){\dot{y}}_i+(x_i-x_{i-1}){\dot{y}}_{i+1}=3\frac{(x_{i+1}-x_i)}{(x_i-x_{i-1})}(y_i-y_{i-1})+3\frac{(x_i-x_{i-1})}{(x_{i+1}-x_i)}(y_{i+1}-y_i)$$

(22)

formula (22) can also be rewritten as formula (23) as below.

$${\mathrm{\Delta }}_{i+1}{\dot{y}}_{i-1}+2({\mathrm{\Delta }}_i+{\mathrm{\Delta }}_{i+1}){\dot{y}}_i+{\mathrm{\Delta }}_i{\dot{y}}_{i+1}=3\frac{{\mathrm{\Delta }}_{i+1}}{{\mathrm{\Delta }}_i}(y_i-y_{i-1})+3\frac{{\mathrm{\Delta }}_i}{{\mathrm{\Delta }}_{i+1}}(y_{i+1}-y_i)$$

(23)

It can be seen from formula (22), it is almost impossible that arbitrarily specified ${\dot{y}}_0$, ${\dot{y}}_1$, …, ${\dot{y}}_{n-1}$ at knots happen to meet formula (22). That is to say, when ${\dot{y}}_0$, ${\dot{y}}_1$, …, ${\dot{y}}_{n-1}$ at knots are constrained arbitrarily, second-order derivatives of cubic Hermite curves at connecting points are discontinuous. In fact, as for cubic Hermtime interpolation curves, only one of them can be satisfied, constraining first-order derivatives willfully at knots or C² continuity. In order to solve the matter, a kind of modified cubic Hermite interpolation with willful constraints on first-order derivatives at knots is presented while the interpolation curves are C² continuous.

4.4. Modified Cubic Hermite Interpolation

Firstly, take three points of cubic Hermite interpolation as an example. Here are three data points to be interpolated A(x₀, y₀), B(x₁, y₁), C(x₂, y₂), and the first-order derivatives at knots A, B, C are assigned willfully to ${\dot{y}}_0$, ${\dot{y}}_1$ and ${\dot{y}}_2$, as can be seen in Figure 7a. Since ${\dot{y}}_0$, ${\dot{y}}_1$ and ${\dot{y}}_2$ are specified willfully without following formula (22), the cubic Hermit interpolation curves y(x) at connection point B is not C² continuous.

In order to specify first-order derivatives willfully at points A, B, C and guarantee C² continuity, new points are built. As seen in Figure 7b, new point M is built between initial point A and B, and new point N is built between initial point B and C. In order to distinguish the new interpolation function from the old one, the new function is marked as r(t) instead of y(x). Newly-built knots become A(t₀, r₀), M(t₁, r₁), N(t₂, r₂) and C(t₃, r₃), and 1st derivatives at A, M, N, and C are marked respectively as ${\dot{r}}_0$, ${\dot{r}}_1$, ${\dot{r}}_2$ and ${\dot{r}}_3$. The first and last point are unchanged, (x₀, y₀) = (t₀, r₀), (x₂, y₂) = (t₃, r₃), ${\dot{y}}_0={\dot{r}}_0$, ${\dot{y}}_2={\dot{r}}_3$.

Newly-built knots A, M, N, and C instead of initial knots A, B, and C are interpolated by cubic Hermite interpolation, and interpolating function r(t) is obtained. Function r(t) must meet the following situations: (1) Interpolating curves must pass by the knots A, B, and C; (2) First-order derivatives at point A, B, and C are equal to ${\dot{y}}_0$, ${\dot{y}}_1$ and ${\dot{y}}_2$; (3) Second-order derivatives at newly-built knots M, and N are continuous.

According to formula (18), positions and 1st derivatives at every two newly-built knots are needed to carry out interpolation. Since the first and the last point are unchanged and known, positions and 1st derivatives of new knots except point A and C must be found out. That is new points positions M(t₁, r₁), N(t₂, r₂), and their derivatives ${\dot{r}}_1$,${\dot{r}}_2$ must be found out.

Because the calculation of new knots’ ordinates t₀, t₁, …, t_m-1 will be described in detail in a later section, t₀, t₁, …, t_m-1 can be considered as the knowns. Therefore, firstly explain how to find out r₁, r₂, ${\dot{r}}_1$,${\dot{r}}_2$.

As shown in Figure 7b, the interpolation curve r₂(t) must pass by the initial knot B. According to formula (18), the second interpolation curve r₂(t) can be expressed function (24) as below.

$$r_2(t)=\left[1\hspace{1em}\frac{t-t_1}{t_2-t_1}\hspace{1em}{(\frac{t-t_1}{t_2-t_1})}^2\hspace{1em}{(\frac{t-t_1}{t_2-t_1})}^3\right]\left[\begin{array}{cccc}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill -3\hfill & \hfill 3\hfill & \hfill -2\hfill & \hfill -1\hfill \\ \hfill 2\hfill & \hfill -2\hfill & \hfill 1\hfill & \hfill 1\hfill \end{array}\right]\left[\begin{array}{c}{r}_1\\ r_2\\ (t_2-t_1){\dot{r}}_1\\ (t_2-t_1){\dot{r}}_2\end{array},\hspace{1em}\right]t\in [t_1,t_2]$$

(24)

where t is the variable, and (t₁, r₁), ${\dot{r}}_1$, (t₂, r₂) and ${\dot{r}}_2$ are positions and 1st derivatives at new knot M and N, respectively.

The initial knot B(x₁, y₁) and its derivative ${\dot{y}}_1$ must meet the interpolation function r₂(t) in formula (24), which can be expressed as the following Equation (25).

$$\left\{\begin{array}{c}{r}_2(x_1)=y_1\\ {\dot{r}}_2(x_1)={\dot{y}}_1\end{array}\right.$$

(25)

Put x₁, y₁ and ${\dot{y}}_1$ into formula (24). Then Equations (26) and (27) could be obtained respectively after sorting out.

$$\left[1-\frac{3K_1^2}{H_2}-\frac{2K_1^3}{H_2}\hspace{1em}\frac{3K_1^2}{H_2}+\frac{2K_1^3}{H_2}\hspace{1em}-K_1-2K_1^2-K_1^3\hspace{1em}-K_1^2-K_1^3\right]\left[\begin{array}{c}{r}_1\\ r_2\\ {\dot{r}}_1\\ {\dot{r}}_2\end{array}\right]=y_1$$

(26)

$$\left[\frac{6K_1}{H_2}+\frac{6K_1^2}{H_2}\hspace{1em}-\frac{6K_1}{H_2}-\frac{6K_1^2}{H_2}\hspace{1em}1+4K_1+3K_1^2\hspace{1em}2K_1+3K_1^2\right]\left[\begin{array}{c}{r}_1\\ r_2\\ {\dot{r}}_1\\ {\dot{r}}_2\end{array}\right]={\dot{y}}_1$$

(27)

where, $x_1-t_1$ is marked as K₁, and $t_2-t_1$ is marked as H₂, shown in Figure 7c. As mentioned above, the calculation of t₀, t₁, …, t_m will be described in detail in a later section, so K₁ and H₂ could be considered as the knowns. Besides, y₁ and ${\dot{y}}_1$ are certainly the knowns of initial knot B. Therefore, Equations (26) and (27) are linear equations of the unknowns r₁, r₂, ${\dot{r}}_1$, ${\dot{r}}_2$. We need to solve out the unknowns r₁, r₂, ${\dot{r}}_1$, ${\dot{r}}_2$ while there are only two Equations so that another two linear equations are required.

In order to guarantee second-order continuity at connection points, positions and first-order derivatives of new knots A, M, N and C must meet formula (22). Every three adjacent new knots among four new knots need to meet the formula (22). Specifically, knots A, M and N all together should meet formula (22). After sorting out, function (28) can be obtained.

$$\left(\frac{3H_2}{H_1}\hspace{1em}\frac{3H_1}{H_2}-\frac{3H_2}{H_1}\hspace{1em}-\frac{3H_1}{H_2}\hspace{1em}{H}_2\hspace{1em}2(H_1+H_2)\hspace{1em}{H}_1\right)$$

(28)

where, $t_1-t_0$ and $t_2-t_1$ are marked as H₁ and H₂ respectively, shown in Figure 7c. As mentioned above, H₁ and H₂ could be considered as the knowns. Since r₀ and ${\dot{r}}_0$ are the ordinate and 1st derivative of knot A, they are known constants. Hence, Equation (28) is a linear one of the unknowns r₁, r₂, ${\dot{r}}_1$ and ${\dot{r}}_2$.

Moreover, new knots M, N and C all together should also meet the formula (22). Similarly, Equation (29) can be obtained after sorting out.

$$\left[\frac{3H_3}{H_2}\hspace{1em}\frac{3H_2}{H_3}-\frac{3H_3}{H_2}\hspace{1em}-\frac{3H_2}{H_3}\hspace{1em}{H}_3\hspace{1em}2(H_2+H_3)\hspace{1em}{H}_2\right]\left[\begin{array}{c}{r}_1\\ r_2\\ r_3\\ {\dot{r}}_1\\ {\dot{r}}_2\\ {\dot{r}}_3\end{array}\right]=0$$

(29)

where, $t_2-t_1$ and $t_3-t_2$ are marked as H₂ and H₃. H₂ and H₃ could be considered as known constants. Since r₃ and ${\dot{r}}_3$ are the ordinate and 1st derivative of knot C, they are the knowns. Hence, Equation (29) is also a linear one of the unknowns r₁, r₂, ${\dot{r}}_1$ and ${\dot{r}}_2$.

These four Equations (26)–(29) are all linear ones of the unknowns r₁, r₂, ${\dot{r}}_1$ and ${\dot{r}}_2$. By solving the Equations, the values of r₁, r₂, ${\dot{r}}_1$ and ${\dot{r}}_2$ can be obtained. After acquiring the positions and the 1st derivatives at newly-built knots A, M, N, and C, modified cubic Hermite interpolation curves can be obtained according to formula (18).

Now let us look at the situation with four initial points interpolated. As seen in Figure 8a, there are four initial knots to be interpolated A(x₀, y₀), B(x₁, y₁), C(x₂, y₂), and D(x₃, y₃). The first-order derivatives at knots A, B, C and D are assigned willfully to ${\dot{y}}_0$, ${\dot{y}}_1$, ${\dot{y}}_2$, and ${\dot{y}}_3$. Rebuild new data points. As seen in Figure 8b, new knot M is built between the first and the second knots A and B, and new knot N between the penultimate and the last knot C and D. It must be noted that two new knots P and Q need to be built between knot B and C. If only one new knot is built between knots B and C, set of Equations cannot be solved because the number of Equations is greater than that of the unknowns. Similarly, we need to find out the unknowns r₁, r₂, r₃, r₄, ${\dot{r}}_1$, ${\dot{r}}_2$, ${\dot{r}}_3$, ${\dot{r}}_4$ of the newly-built knots except the first and the last one.

In the same way, the initial knot B(x₁, y₁) and its derivative ${\dot{y}}_1$ must meet the interpolation function r₂(t). The initial knot C(x₂, y₂) and its derivative ${\dot{y}}_2$ must meet the interpolation function r₄(t), shown in Figure 8b. Then put x₁, y₁, ${\dot{y}}_1$ and x₂, y₂, ${\dot{y}}_2$ into r₂(t) and r₄(t) derived from formula (18), and we can get four Equations similar to Equations (26) and (27). When it is extended to more knots, Equations (30) and (31) can be obtained after sorting and inducting.

$$\left[\begin{array}{cccc}1-\frac{3K_i^2}{H_{j+1}}-\frac{2K_i^3}{H_{j+1}}& \frac{3K_i^2}{H_{j+1}}+\frac{2K_i^3}{H_{j+1}}& -K_i-2K_i^2-K_i^3& -K_i^2-K_i^3\end{array}\right]\left[\begin{array}{c}{r}_j\\ r_{j+1}\\ {\dot{r}}_j\\ {\dot{r}}_{j+1}\end{array}\right]=y_i\hspace{1em}\begin{array}{c}(i=1,2,3,\cdots ,n-2)\\ (j=1,3,5,\cdots ,m-3)\end{array}$$

(30)

$$\left[\begin{array}{cccc}\frac{6K_i}{H_{j+1}}+\frac{6K_i^2}{H_{j+1}}& -\frac{6K_i}{H_{j+1}}-\frac{6K_i^2}{H_{j+1}}& 1+4K_i+3K_i^2& 2K_i+3K_i^2\end{array}\right]\left[\begin{array}{c}{r}_j\\ r_{j+1}\\ {\dot{r}}_j\\ {\dot{r}}_{j+1}\end{array}\right]={\dot{y}}_i\hspace{1em}\begin{array}{c}(i=1,2,3,\cdots ,n-2)\\ (j=1,3,5,\cdots ,m-3)\end{array}$$

(31)

where, n is the number of the initial knots ($n\ge 3$), and m ($m=2n-2$) is the number of the newly-built knots. The subscript i is related to initial knots while subscript j is related to newly-built knots. Array y is the ordinates of initial knots, y₀, y₁, …, y_n-1, which are all knowns. The array $\dot{y}$ is the first-order derivatives of initial knots, ${\dot{y}}_0$,${\dot{y}}_1$, …, ${\dot{y}}_{n-1}$, which are all knowns. Array r is the ordinates of newly-built knots, r₀, r₁, …, r_m-1, in which r₀ and r_m-1 are knowns and others are unknowns. The array $\dot{r}$ is the first-order derivatives of newly-built knots ${\dot{r}}_0$,${\dot{r}}_1$, …, ${\dot{r}}_{m-1}$, in which ${\dot{r}}_0$ and ${\dot{r}}_{m-1}$ are knowns and others are unknowns. So, r₁, r₂, … , r_m-2 and ${\dot{r}}_1$, ${\dot{r}}_2$, …, ${\dot{r}}_{m-2}$ are unknown.

The expressions of array K and array H are shown in Equations (32) and (33) below respectively, seen as in Figure 8c for graphic representation. Since all of the initial knots’ abscissas x₀, x₁, … , x_n-1 are given and the newly-built knots’ abscissas t₀, t₁, … , t_m-1 can be considered as the knowns described in the latter section, array K and array H are known constants. Hence, Equations (30) and (31) are linear ones.

$$K_i=x_i-t_j(i=1,2,3,\cdots ,n-2,j=1,3,5,\cdots ,m-3)$$

(32)

$$H_j=t_j-t_{j-1} ( j=1,2,\cdots ,m-1)$$

(33)

Synthesizing formulas (30) and (31), there are $4(n-2)$ unknowns and $2(n-2)$ Equations. Hence, the set of Equations has no unique solution unless other Equations are to be added.

Similarly, in order to guarantee C² continuity at newly-built knots, the positions and first-order derivatives of new knots A, M, N, P, Q and D in Figure 8b must meet the formula (22). Every three adjacent new knots need to meet the formula (22). When it is extended to more knots, Equation (34) can be obtained after sorting and inducting.

$$\left[\frac{3H_{j+1}}{H_j}\hspace{1em}\frac{3H_j}{H_{j+1}}-\frac{3H_{j+1}}{H_j}\hspace{1em}-\frac{3H_j}{H_{j+1}}\hspace{1em}{H}_{j+1}\hspace{1em}2(H_j+H_{j+1})\hspace{1em}{H}_j\right]\left[\begin{array}{c}{r}_{j-1}\\ r_j\\ r_{j+1}\\ {\dot{r}}_{j-1}\\ {\dot{r}}_j\\ {\dot{r}}_{j+1}\end{array}\right]=0\hspace{1em}(j=1,2,3,\cdots ,m-2)$$

(34)

where, the meanings of m$(m=2n-2)$, r, $\dot{r}$ and H are the same as that of the above. Equation (34) are linear ones. It is pointed out that r₀, $r_{m-1}$,${\dot{r}}_0$, ${\dot{r}}_{m-1}$ of the first and last new point are knowns. The unknowns are r₁, r₂, … , r_m-2 and ${\dot{r}}_1$, ${\dot{r}}_2$, …, ${\dot{r}}_{m-2}$. Here are $4(n-2)$ the unknowns and $2(n-2)$ Equations. Combining with formulas (30), (31) and (34), the number of the unknowns is $4(n-2)$, and the number of the Equations is $4(n-2)$. Therefore, set of Equations (30), (31) and (34) are all linear Equations and have a unique solution.

After positions and first-order derivatives of newly-built knots are found out, modified piecewise cubic Hermite curves can be obtained according to formula (18). The modified curves would pass by the initial knots, and first-order derivatives at the initial knots can be specified at will while the curves are C² continuous.

For more than four initial points to be interpolated, every two new points need to be built between every two initial points from the second and penultimate initial point. The method is similar to the one for four initial points.

4.5. Arrangement for The Abscissas of Newly-Built Knots

Given n initial knots to be interpolated denoted as (x_i−1, y_i−1), I = 1, 2, …, n, ($n\ge 3$). The abscissas of initial knots are x₀, x₁, … , x_n−1. New knots are built for proposed interpolation, marked as (r_j−1, t_j−1), j = 1, 2, …, m, ($m=2n-2$). The abscissas of new knots are t₀, t₁, … , t_m−1. We need to arrange t₀, t₁, … , t_m−1 according to x₀, x₁, … , x_n−1.

The first idea is to average t₀, t₁, … , t_m−1. Uniform abscissa knots are beneficial to the smoothness of the interpolation curves. However, averaging t₀, t₁, … , t_m−1 may possibly lead to the situation that there are two initial knots existing between two new knots, seen in Figure 9. Since a piecewise curve must pass by the two initial points, too many constraints are imposed on this curve so that the curve function cannot be solved and interpolation functions cannot be found out.

To avoid the trouble mentioned above, the following scheme is proposed on the basis of homogenization as much as possible. Take five initial knots for example. As can be seen in Figure 10, two newly-built knots R₂ and R₃ are built between the second and the third initial as described above.

Based on the principle of average for new knots, the horizontal distance between the second and the third initial points is divided into three segments, so the abscissas t₂ and t₃ of the new knots R₂ and R₃ can be expressed as Equations (35) and (36) below. The same scheme is applied whenever there are two newly-built knots between every two adjacent initial points.

$$t_2=x_1+\frac{1}{4}(x_2-x_1)$$

(35)

$$t_3=x_1+\frac{3}{4}(x_2-x_1)$$

(36)

Between the first and the second initial points, is there only one newly-built knot R₁ inserted, seen in Figure 10. The horizontal distance between the first and the second initial knots is divided by new point R₁ into two segments with unequal length, so the golden ratio is a reasonable method. Consequently, the abscissa t₁ of newly-built knot R₁ can be expressed by Equation (37). The same is as for the last newly-built knot R₆, and the abscissa t₆ of the newly-built knot R₆ can be expressed by Equation (38).

$$t_1=x_0+0.618(x_1-x_0)$$

(37)

$$t_6=x_4-0.618(x_4-x_3)$$

(38)

Extended to the case that the number of the initial points is n ($n\ge 4$), formula (39) can be obtained. Abscissas of all newly-built knots t are acquired from the abscissas of initial knots x. If there are only three initial points ($n=3$), take the first two Equations and the last two Equations in formula (39).

$$\left\{\begin{array}{l}{t}_0=x_0\\ t_1=x_0+0.618(x_1-x_0)\\ t_j=x_{i-1}+\frac{1}{4}(x_i-x_{i-1})\hspace{1em}i=2,3,\cdots ,n-2,\hspace{1em}j=2,4,6,\cdots ,m-4\\ t_{j+1}=x_{i-1}+\frac{3}{4}(x_i-x_{i-1})\\ t_{m-2}=x_{n-1}-0.618(x_{n-1}-x_{n-2})\\ t_{m-1}=x_{n-1}\end{array}\right.$$

(39)

where n and m ($m=2n-2$) are respectively the numbers of initial and newly-built knots. The subscript i is related to the initial knots while subscript j is related to the newly-built knots.

4.6. Summary

Sum up the steps of modified cubic Hermit interpolation as follows:

(1)

According to the abscissas of given initial knots x₀, x₁, … , x_n−1, ($n\ge 3$), the abscissas of newly-built knots, t₀, t₁, … , t_m−1, ($m=2n-2$), are arranged and calculated by the formula (39).

(2)

According to the given positions and the 1st derivatives of the initial points and the abscissas of the newly-built knots achieved in step 1, a set of linear equations are obtained by formulas (30)–(34). Then, the ordinates and the first derivatives of the newly-built knots are acquired via solving the set of linear equations.

(3)

According to the positions and first-order derivatives of newly-built knots achieved in previous steps, modified cubic Hermite interpolation curves are obtained by formula (18).

5. Case Study

In order to verify the rationality and effectiveness of the proposed methodology, the following study case is employed. The usage of the proposed methodology is further described in detail during the study case analysis.

Case: In order to accomplish a task, joint 1 of the industrial robot needs to carry out velocity-constrained PTP motion. As shown in Figure 11a, joint 1 turns from start target point A to intermediate target points B, C and to end target point D in turn. The velocities at the intermediate target points B and C are specified. The rotational requirements at each target point are shown in

Table 1. Constraint requirements for target points of joint 1.

Target Point	Angular Displacement θ (°)	Angular Velocity ω (°/s)	Time x (s)
A	0	0	0
B	30	8	5
C	90	8	15
D	180	0	25

. Using the proposed planning of modified cubic Hermite interpolation presented above, carry out velocity-constrained PTP motion planning for joint 1.

There are four initial knots A, B, C, and D to be interpolated. As seen in Figure 11b, newly-built knot M needs to be inserted between initial A and initial B, and N between C and D, and P, Q between B and D, according to the proposed method. Therefore, the initial knots A, B, C, D are replaced by the newly-built knots A, M, P, Q, N, and D. The initial time knots become the newly-built time knots correspondingly. In order to be unified with the previous formula, the initial time knot is denoted as x, and the newly-built time knots is denoted as t. According to formula (39), the newly-built time knots are arranged from the initial time knots x = 0, 5, 15, 25 (s) and the newly-built time knots are t = 0, 3, 8, 13, 19, 25 (s) respectively with proper rounding.

We need to find out the angular displacements and angular velocities at the newly-built knots M, P, Q and N from the initial knots A, B, C, and D. According to formula (30), 31, 32, 33, and 34, we can obtain Equation (40) to solve out the angular displacements and angular velocities of the rebuilt knots, θ_M, θ_P, θ_Q, θ_N, ω_M, ω_P, ω_Q, ω_N.

$$\left[\begin{array}{cccccccc}\hfill -0.4\hfill & \hfill -0.225\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 2\hfill & \hfill 0.375\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0.3\hfill & \hfill 0\hfill & \hfill -0.3\hfill & \hfill 0\hfill & \hfill 0.5\hfill & \hfill 2\hfill & \hfill 0.5\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0.327\hfill & \hfill -0.1\hfill & \hfill -0.227\hfill & \hfill 0\hfill & \hfill 0.546\hfill & \hfill 2\hfill & \hfill 0.455\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0.25\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0.5\hfill & \hfill 2\hfill \\ \hfill 0.648\hfill & \hfill 0.352\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0.72\hfill & \hfill -0.48\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill -0.288\hfill & \hfill 0.288\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -0.12\hfill & \hfill -0.32\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0.741\hfill & \hfill 0.259\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0.889\hfill & \hfill -0.444\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill -0.222\hfill & \hfill 0.222\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -0.333\hfill \end{array}\right]\left[\begin{array}{c}{\theta }_{\mathrm{M}}\\ {\theta }_{\mathrm{P}}\\ {\theta }_{\mathrm{Q}}\\ {\theta }_{\mathrm{N}}\\ {\omega }_{\mathrm{M}}\\ {\omega }_{\mathrm{P}}\\ {\omega }_{\mathrm{Q}}\\ {\omega }_{\mathrm{N}}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\\ 45\\ 30\\ 8\\ 90\\ 8\end{array}\right]$$

(40)

The values of θ_M, θ_P, θ_Q, θ_N, ω_M, ω_P, ω_Q, ω_N can be obtained by solving the linear Equations. In addition to the previously calculated amount of time, the information of the newly-built knots for joint motion planning can be listed in

Table 2. Constraint conditions at newly-built targets points.

Target Point	Angular Displacement θ (°)	Angular Velocity ω (°/s)	Time t (s)
A	0	0	0
M	14.21	7.548	3
P	51.747	5.952	8
Q	76.147	5.805	13
N	129.443	11.53	19
D	180	0	25

.

According to the motion conditions at newly-built target points in Table 2, the piecewise motion planning of joint 1 can be carried out by formula (18).

The planning function of AM segment is shown in Equation (41) below.

$${\theta }_1(t)=2.221t^2-0.214t^3\hspace{1em}0\le t\le 3$$

(41)

The planning function of the MP segment is shown in Equation (42) below.

$${\theta }_2(t)=-4.142+4.142t+0.84t^2-0.061t^3\hspace{1em}3\le t\le 8$$

(42)

The planning function of the PQ segment is shown in Equation (43) below.

$${\theta }_3(t)=-76.069+31.115t-2.531t^2+0.08t^3\hspace{1em}8\le t\le 13$$

(43)

The planning function of the QN segment is shown in Equation (44) below.

$${\theta }_4(t)=125.677-15.442t+1.05t^2-0.012t^3\hspace{1em}13\le t\le 19$$

(44)

The planning function of the ND segment is shown in Equation (45) below.

$${\theta }_5(t)=1057.847-162.627t+8.796t^2-0.148t^3\hspace{1em}19\le t\le 25$$

(45)

Validate whether the motion planning functions meet the requirements of angular displacements and angular velocities at the initial target points, as shown in

Table 3. Rotational requirement validation at the initial target points.

Initial Target Point	Planning Function	Angular Displacement θ (°)	Angular Velocity ω (°/s)	Time t (s)
A	${\theta }_1(t)$	${\theta }_1(0)=0$	${\dot{\theta }}_1(0)=0$	0
B	${\theta }_2(t)$	${\theta }_2(5)=30$	${\dot{\theta }}_2(5)=8$	5
C	${\theta }_4(t)$	${\theta }_4(15)=90$	${\dot{\theta }}_4(15)=8$	15
D	${\theta }_5(t)$	${\theta }_5(25)=180$	${\dot{\theta }}_5(25)=0$	25

. Validation results show the planning functions meet the requirements.

Validate the continuities of the motion planning functions at the connecting points between every two adjacent planning segments, as shown in

Table 4. Continuity validation at the connecting points.

Connecting Point	Planning Function	Angular Displacement θ (°)	Angular Velocity ω (°/s)	Angular Acceleration ε (°/s2)	Time t (s)
M	${\theta }_1(t)$	${\theta }_1(3)=14.21$	${\dot{\theta }}_1(3)=7.55$	${\ddot{\theta }}_1(3)=0.59$	3
	${\theta }_2(t)$	${\theta }_2(3)=14.21$	${\dot{\theta }}_2(3)=7.55$	${\ddot{\theta }}_2(3)=0.59$
P	${\theta }_2(t)$	${\theta }_2(8)=51.75$	${\dot{\theta }}_2(8)=5.96$	${\ddot{\theta }}_2(8)=-1.23$	8
	${\theta }_3(t)$	${\theta }_3(8)=51.75$	${\dot{\theta }}_3(8)=5.96$	${\ddot{\theta }}_3(8)=-1.23$
Q	${\theta }_3(t)$	${\theta }_3(13)=76.15$	${\dot{\theta }}_3(13)=5.81$	${\ddot{\theta }}_3(13)=1.17$	13
	${\theta }_4(t)$	${\theta }_4(13)=76.15$	${\dot{\theta }}_4(13)=5.81$	${\ddot{\theta }}_4(13)=1.17$
N	${\theta }_4(t)$	${\theta }_4(19)=129.44$	${\dot{\theta }}_4(19)=11.53$	${\ddot{\theta }}_4(19)=0.74$	19
	${\theta }_5(t)$	${\theta }_5(19)=129.44$	${\dot{\theta }}_5(19)=11.53$	${\ddot{\theta }}_5(19)=0.74$

. The validation results show that angular displacements, angular velocities and angular accelerations at connecting points are all continuous.

According to the motion planning functions above, the motion planning curves can be obtained, shown in Figure 12. As can be seen from Figure 12a, the angular displacement curve is very smooth and meets the angular displacement requirements at the initial target points A, B, C, D. As can be seen from Figure 12b, the angular velocity curves are also smooth, the velocity fluctuation is gentle, and meets the angular velocity requirements at the initial target points A, B, C, D. As can be seen from Figure 12c, the angular acceleration curves are continuous at the connecting points M, P, Q, N, and the whole acceleration curves does not fluctuate much.

The motion planning curves once more verify that the motion planning functions can not only meet the constrained requirements of angular displacements and angular velocities at the initial target points but also guarantee the continuity of angular acceleration and angular velocity fluctuates gently. It proves that the proposed method can be applied well to joint motion planning with velocity constraints, and avoid the problems of discontinuous angular acceleration in cubic polynomial planning and large fluctuation of angular velocity in the quintic polynomial.

6. Comparison and Discussion

In order to compare with the proposed planning methodology, the most commonly used PTP joint planning method, cubic polynomial and quintic polynomial, are applied to the same case study above as well. The results of the study and analysis below reflect there are more or fewer deficiencies for these two methods to be used for joint planning with constrained velocity. The detailed comparison and analysis process are as follows.

Figure 13 shows the angular displacement curves obtained by the proposed methodology, cubic polynomial, and quintic polynomial respectively. As can be seen from Figure 13a,b, the angular displacement curve obtained by the proposed method is similar to the curves obtained by the cubic polynomial. Both of the curves are smooth and the fluctuation is very small. In Figure 13c, the angular displacement curves obtained by quintic polynomial fluctuate a little bit. In fact, a curve with a higher order is more likely to fluctuate than a curve with a lower order. Therefore, we should try to avoid using higher-order functions for planning.

Figure 14 is the angular velocity curves obtained by these three methods. In Figure 14a, the velocity curves obtained by the proposed method are smooth, the velocity fluctuation is gentle, and the variation trend is roughly similar to that in Figure 14b by the cubic polynomial. In Figure 14b, the overall velocity waveform by a cubic polynomial is simple and appropriate. However, there are turning points at junctions B and C, which will lead to discontinuous angular acceleration at the connection points. In Figure 14c, the overall fluctuation of the velocity curves by a quintic polynomial is obviously large. In particular, because of inappropriate preset angular acceleration at point C, the velocity near point C fluctuates back and forth sharply, which is not conducive to the control of joint rotation speed. This shows that quintic polynomial planning is likely to cause angular velocity to fluctuate too much. Moreover, the higher the order of the polynomial is, the higher the velocity peak will be, which makes greater volatility. These are the weaknesses of quintic polynomial planning.

Figure 15 is the angular acceleration curves obtained by these three methods. As can be seen from Figure 15a,c, the angular acceleration curves, obtained by proposed methodology and quintic polynomial respectively, are both continuous at connection points. Because the order of the quintic polynomial is high, the angular acceleration in Figure 15c is also easy to fluctuate. In Figure 15b, the angular acceleration curves obtained by cubic polynomial are obviously broken at the connecting points B and C. The angular acceleration discontinuity of the joint will cause vibrations and impacts, which will degrade the working performance and decrease the service life of industrial robots.

It can be seen from the above comparative analysis of this case, as for joint PTP motion planning with constrained velocity, the angular acceleration of cubic polynomial planning is discontinuous, which will lead to vibrations and impacts. Angular velocity of quintic polynomial planning fluctuates greatly back and forth, which is not conducive to velocity control. The proposed motion planning methodology not only meets the constraints of angular displacements and angular velocities at target points but also guarantees the continuity of angular acceleration. The maximum function order of proposed planning is three, which effectively avoids the fluctuation caused by high-order functions.

7. Conclusions

For the industrial robots’ PTP joint motion planning with velocity constraint, cubic polynomial planning has the problem of angular acceleration discontinuity, which will cause vibrations and impacts. As for quintic polynomial planning, angular accelerations at target points need to be given in advance and because it is hard to give appropriate angular accelerations in advance, it will lead to large velocity fluctuations which are not conducive to velocity control.

In order to solve these problems in joint planning, a kind of methodology based on modified Hermite interpolation is proposed. On the basis of Hermite interpolation, new target knots are reconstructed from the initial target knots. Then, newly-built knots instead of old initial knots are used to carry out joint motion planning. Using the proposed methodology, not only does the motion planning meet the requirements for angular displacement and angular velocity at the initial knots, but also the angular acceleration of the whole planning is continuous without broken points. Moreover, the maximum order of the planning functions is three, which avoids the velocity fluctuation caused by the higher-order functions such as quintic polynomial. Therefore, the problems existing in cubic polynomial planning and quintic polynomial planning can be solved well.

A study case is given to verify the rationality and effectiveness of the proposed method. The case study results show that, compared with the other two planning methods, the proposed methodology is applied well to PTP joint motion planning with velocity constraints, and is conducive to the working performance and service life of industrial robots.

In the future, efforts will be made on optimization investigation as continued research in the area. The basic motion planning of industrial robots, including improved motion planning, focuses on the continuity and smoothness of the trajectory. However, efficiency, energy, impact and other problems in the actual operation requirements are not considered. Thus, optimization of single or multiple objectives among efficiency, energy, the impact will be the further potential studies on the basis of the motion planning. Moreover, motion planning needs to be realized through motion control. Due to strong nonlinear coupling between each joint and end-effector, there will be disturbances, errors and other uncertain factors, so optimal control will also be considered as a further investigation to guarantee motion planning realized precisely.