Trajectory Planning in Joint Space for a Pointing Mechanism Based on a Novel Hybrid Interpolation Algorithm and NSGA-II Algorithm

In order to improve the motion stability of a pointing mechanism, the trajectory planning and trajectory optimization were conducted. To obtain better motion stability, the jerk is required continuous. Therefore, higher order polynomial or B-spline are needed and the calculation amount increases in trajectory planning. A novel hybrid interpolation algorithm “7-order polynomial + improved quartic uniform B-spline curve + 7-order polynomial” was proposed for trajectory planning, which cannot only ensure the trajectory passes through all path points but also ensure the jerk is continuous. From the first path point to the second path point and from the penultimate path point to the last path point, 7-order polynomial is chosen as the interpolation algorithm. The intermediate path points are interpolated by improved quartic uniform B-spline curve which is newly proposed. The improved quartic uniform B-spline curve can pass through all path points and does not require complicated calculation. To carry out the trajectory planning of a X-Y pointing mechanism, the kinematic model was derived at first. Secondly, the hybrid interpolation algorithm was presented. Thirdly, the NSGA-II algorithm was applied to the multi-objective optimization which are time, acceleration and jerk. Finally, the trajectory planning experiment was conducted to prove the validity of the proposed hybrid interpolation algorithm. The experimental results show that the pointing accuracy is improved by using the hybrid interpolation algorithm and the NSGA-II algorithm. This hybrid interpolation algorithm for trajectory planning can be applied to other robots as well.


I. INTRODUCTION
Pointing mechanisms are widely used in aerospace fields, which are important devices for antenna and some optical payloads [1]- [3]. With the development of aerospace science and technology, the quality requirements of pointing mechanism become increasingly high. In order to achieve high quality of pointing mechanisms, trajectory planning is a critical portion. The trajectory planning is also the foundation of control. Trajectory planning is generally divided into joint space trajectory planning and Cartesian space trajectory planning. Joint space trajectory planning is to plan the motion of driving joints. The trajectory planning of the end-effector belongs to Cartesian space trajectory planning. The joint The associate editor coordinating the review of this manuscript and approving it for publication was Pedro Neto . space trajectory planning is relatively simple and applicable to point-to-point motion. Joint space trajectory planning is used in many workplaces where mechanisms or robots need to move from one point to another point. When there are some special requirements, it needs to pass through some intermediate points. In order to satisfy the constraint conditions of path points, trajectory planning algorithm needs to be introduced. Trajectories are usually planned by interpolation algorithm. Trajectory planning algorithms mainly include polynomial function and B-spline curve. Boryga et al. presented a planning mode of trajectory motion for serial-link manipulators with high-degree polynomials application. The linear acceleration profiles of end-effector were planned as the polynomials of degrees 9, 7 and 5 [5]. Gosselin and Foucault used both fifth-degree polynomial formulation and trigonometric formulation for point-to-point trajectory planning of a two-DOF cable-suspended parallel robot [6]. Sidobre and Desormeaux proposed sequences of segment of third-degree polynomial functions for Human-Robot Interactions trajectories [7]. Zhang et al. used quintic polynomial function to interpolate several given points of each joint for trajectory planning of a 6-DOF robot [8]. Macfarlane et al. used a concatenation of fifth-order polynomials to provide a smooth trajectory between two points and used a sine wave template to calculate the end conditions for ramps from zero acceleration to nonzero acceleration [9]. Because of special advantages, B-spline curve was applied to trajectory planning by many scholars. Wang described how B-splines can be used to construct joint trajectories for robot manipulators [10]. Meike et al. presented a novice industrial robot manipulator trajectory optimization method for series of point-to-point type movements, which is based on asynchronous motion profile smoothening using cubic B-spline interpolation [11]. Cubic splines and fifth-order B-splines were applied to the trajectory planning of a 6-DOF robot in reference [12]. Zou et al. proposed a novel trajectory planning algorithm for robot manipulator using Non-Uniform Rational B-Spline (NURBS) curve interpolation and introduced velocity adaptive control methodology to adjust the steps of robot manipulator interpolation and further to reduce the chord error [13].
Based on interpolation algorithm for trajectory planning, trajectory optimization is also necessary to improve efficiency and stability. The trajectory optimization is usually a multi-objective optimization problem (MOP). Numerous metaheuristic algorithms can be found in literatures addressing MOPs. Population based algorithms are very widely used, such as Non-dominated Sorting Genetic Algorithm (NSGA), Differential Evolution (DE) Particle Swarm Optimization (PSO) and Ant Colony Optimization (ACO) [14]- [19]. For specific problems, many new algorithms have been proposed and applied to address MOPs [20]- [23]. Most MOPs do not lend themselves to a single solution and have, instead, a set of solutions. Pareto optimal solutions are widely used to represent trade-offs between conflicting objectives [17], [22], [23]. Based on metaheuristic algorithms, plenty of research works have been done on trajectory optimization. Saravanan et al. used Non-dominated Sorting Genetic Algorithm (NSGA-II) and Multi-objective Differential Evolution (MODE) for obtaining optimal trajectory of an industrial robot manipulator [24]. The trajectory was planned in the joint space by means of 5-order B-spline and then optimized by the elitist non-dominated sorting genetic algorithm (NSGA-II) for traveling time and mean jerk along the whole trajectory in reference [25]. Lin et al. Proposed a fast and unified approach based on particle swarm optimization (PSO) with K-means clustering to solve the near-optimal solution of a minimum-jerk joint trajectory [26]. Wang et al. proposed a trajectory planning beetle swarm optimization (TPBSO) algorithm for solving trajectory planning of robots [27]. In order to minimize energy consumption and running time while ensuring the smoothness of collaborative welding robot with multiple manipulators, Liu et al. proposed a methodology for optimal trajectory planning by means of cubic B-spline and optimized motion time nodes based on particle swarm optimization (PSO) algorithm [28].
Pointing accuracy is of great importance to a pointing mechanism. High motion stability is beneficial to pointing accuracy and it usually means that the trajectory planning should take the jerk into account. When the jerk is required continuous and the trajectory is required to pass through relatively more path points, the process of trajectory planning and the calculation are complex with current interpolation algorithms. In order to make the interpolation algorithm simpler and more efficient, a hybrid interpolation algorithm ''7-order polynomial + improved quartic uniform B-spline + 7-order polynomial'' was proposed, which cannot only ensure that the trajectory passes through all path points, but also ensure the jerk continuity with less calculation. The NSGA-II reduces the complexity of non-inferior sorting genetic algorithm and has the advantages of fast running speed and good convergence of solution set. It has been proven most effective in MOPs [29], [30]. In addition, it is very popular in trajectory optimization [31]- [33]. Therefore, the proposed hybrid interpolation algorithm combined with the NSGA-II algorithm is applied to the trajectory planning and optimization of a X-Y pointing mechanism. The rest of this paper is organized as follows. In section II, the kinematic model of the X-Y pointing mechanism is presented. In section III, the hybrid interpolation algorithm is presented. Section IV presents the trajectory optimization. The pointing experiment is in section V. Conclusions are given in Section VI.

II. THE KINEMATIC MODEL OF THE X-Y POINTING MECHANISM A. THE ARCHITECTURE AND COORDINATE SYSTEM
The X-Y pointing mechanism has two rotational degrees of freedom. The physical prototype is shown in Fig. 1.
The X-Y pointing mechanism mainly consists of the base, U-type frame, shafting 1, shafting 2 and the upper platform, The feature of the X-Y pointing mechanism is that shafting 1 is perpendicular to shafting 2.  The simplified structure and the coordinates are shown in Fig. 2.
The reference frame is O 0 -X 0 Y 0 Z 0 . The Z 0 axis is coincident with shafting 1. The X 0 axis is straight up and perpendicular to the Z 0 axis. The Y 0 axis is determined by right-hand rule. The coordinate attached to link 1 is O 1 − X 1 Y 1 Z 1 . The Z 1 axis is coincident with Z 0 axis. The X 1 axis is coincident with O 1 O 2 and the Y 1 axis is determined by right-hand rule. The coordinate attached to link 2 is O 2 − X 2 Y 2 Z 2 . The Z 2 axis is coincident with shafting 2. The X 2 axis is coincident with O 2 O 3 and the Y 2 axis is determined by right-hand rule. The coordinate attached to end-effector is The D-H method is applied to establish the kinematic model of the X-Y pointing mechanism. There are four parameters of each link, which are α, l, d, and θ. The D-H parameters of each link are shown in Tab. 1.

B. THE FORWARD KINEMATICS
Forward kinematics of the pointing mechanism refers to the process of solving the position and attitude of end-effector.
The transformation matrix between adjacent links is generally expressed as: where, s stands for sine and c stands for cosine. θ 1 is the rotation angle of shafting 1 and θ 2 is the rotation angle of shafting 2.
The transformation matrix of the end-effector can be obtained by successively multiplying the matrixes of all the adjacent links: The conversion from expected position and attitude of the end-effector to joint space angle variables is the inverse kinematics. Here, the Paul inverse transformation method is used to solve the inverse kinematics of the pointing mechanism.    When multiply both sides of Eq. (3) by 0 According to the corresponding equality of the fourth elements in the second row of the matrix on both sides of Eq. (4), it can be obtained as follows: According to the corresponding equality of the fourth elements in the third row of the matrix on both sides of Eq. (4), it can be obtained as follows: The end-effector of X-Y pointing mechanism is usually represented by azimuth-pitch angle. The azimuth angle is denoted as γ and the pitch angle is denoted as β. The relationship between azimuth-pitch angle and the angle of joints is given below.

III. INTERPOLATION ALGORITHM FOR TRAJECTORY PLANNING
According to all the path points in the workspace, a series of joint angles can be obtained by inverse kinematics. Then the trajectory planning can be carried out by interpolation algorithms. The common interpolation algorithms include polynomial and B-spline curve.

A. 7-ORDER POLYNOMIAL INTERPOLATION ALGORITHM
The initial joint angle is denoted as θ 0 and the terminated angle is denoted as θ f . When there are requirements of the velocity, acceleration and jerk of the initial path point and the terminated path point, there are eight constraint conditions. Therefore, 7-order polynomial interpolation algorithm is adopted. The 7-order polynomial interpolation function is expressed as: The expression of velocity function can be obtained by differentiating Eq. (9): The expression of acceleration function can be obtained by differentiating Eq. (10): The expression of jerk function can be obtained by differentiating Eq. (11): The boundary constraint conditions of the first point and the last point can be described as: Hence, the coefficients of 7-order polynomial function can be obtained: where,

B. IMPROVED QUARTIC UNIFORM B-SPLINE CURVE
The equation of B-spline curve is described as follows: where, p i is the ith control point of the B-spline. N i,k (u) is the ith basis function of the kth order B-spline. k is the degree of B-spline curve.
The basis function is expressed as: Since the jerk of the pointing mechanism should be continuous, it is necessary to require the spline curve to be cubic differentiable. According to the properties of B-spline curve, at least quartic B-spline curve should be selected.
Matrix formulas of B-spline curves have the advantage of simple computation of points on curves. Hence, we use the matrix formulas of uniform quartic B-spline curve here. It can be expressed as follows [34]: It needs five control points to determine a section of uniform quartic B-spline curve. The curve determined by P 0 , P 1 , P 2 , P 3 and P 4 is shown in Fig. 3.
According to Eq.
It can be seen from Fig. 3 that the starting point of the quartic uniform B-spline curve P a is on the line of P 12 P 03 and the ending point of the quartic uniform B-spline curve P b is VOLUME 8, 2020 on the line of P 23 P 14 . P 12 is the midpoint of P 1 P 2 , P 03 is the midpoint of P 0 P 3 , P 23 is the midpoint of P 2 P 3 and P 14 is the midpoint of P 1 P 4 . According to Eq (18), the distance from P a to P 12 is one-twelfth of the distance of P 12 P 03 and the distance from P b to P 23 is one-twelfth of the distance of P 23 P 14 .
The quartic uniform B-spline curve does not pass the path points. To make it pass through all the path points, a new method is put forward based on the constraints of the starting point and the ending point of the section of uniform B-spline. Add four extra control points to replace each original path point, which is shown in Fig. 4. In fact, the four extra points are collinear. The original path point is on the line made up by the adding four points.
The method of adding extra control points is as follows: (1) Add P i,1 and P i, 3 to the left of P i . Add P i,2 and P i,4 to the right of P i . The four points are collinear. The line made up by the four extra control points is parallel to the line P i−1 P i+1 . There are some relationships as follows: (2) For the first point P 0 , it is only needed to add four control points overlapped with P 0 . The method of adding extra control points for the last point is same with the first point.
Every five control points constitute a section of B-spline curve, which moves back one point in turn to form a new section of B-spline curve. Finally, they constitute a B-spline curve passing through all the path points. The Fig. 5 shows the characteristic.
To ensure quartic B-spline curve passing through n path points, it is only need to add 4n extra control points to replace the path points.
Apart from the first path point and the last path point, the calculation formulas of extra control points are as follows: where, i = 1, 2, · · ·, n − 1, n is the number of path point. According to Eq (17), the matrix formulas of the improved quartic uniform B-spline curve can be derived as: where, i = 0, 1, · · ·, n − 1, n is the number of path point. The improved quartic uniform B-spline algorithm retains the original basis function of the original B-spline curve and inherits the characteristics of continuity, convex hull and locality. At the same time, the improved quartic uniform B-spline can pass through all the path points and has simple calculation.

C. HYBRID INTERPOLATION ALGORITHM
Polynomial interpolation algorithm is mainly used for pointto-point trajectory planning. Because the jerk is required continuous, 7-order polynomial curves should be solved between every two adjacent path points. Therefore, a lot of parameters need to be set artificially when there are more path points.    When using B-spline curve to carry out the trajectory planning, there are still problems. If the path points are control vertices, the B spline curve cannot pass through all the path points. If the trajectory passes through all the path points, complex multivariate equation set must be calculated. With the order increasing, the difficulty to solve the multivariate equation set increases. In order to avoid setting a lot of parameters and the calculation of complex multivariate equation set, we tried to apply the improved quartic uniform B-spline interpolation algorithm to the trajectory planning. When it is used for trajectory planning, the acceleration curve and the jerk curve exist obvious abrupt change at the first point and the last point as shown in Fig. 6 and Fig. 7. Therefore, we proposed the hybrid interpolation algorithm, which is ''7-order polynomial + improved quartic uniform B-spline curve + 7-order polynomial''. Using the ''7-order polynomial + improved quartic uniform B-spline curve + 7-order polynomial'' hybrid interpolation algorithm for trajectory planning, the problem can be solved as shown in Fig. 8 and Fig. 9. 7-order polynomial interpolation algorithm is used for trajectory planning from the first path point to the second path point and from the penultimate path point to the last path point. The improved quartic uniform B-spline interpolation algorithm is used for trajectory planning at the intermediate path points. Compared with the 7-order polynomial interpolation algorithm and quartic B-spline interpolation algorithm, there is no need to set a lot of parameters of intermediate path points and calculate complex multivariate equation set with the proposed hybrid interpolation algorithm. It obviously reduces the computational complexity and improves the computational efficiency.
It should be noted that the advantage of the hybrid interpolation algorithm for trajectory planning is also its limitation. We cannot easily set the parameters when there are requirements of the kinematic parameters of the intermediate path points. In most situations, there is no requirements of the kinematic parameters of the intermediate path points. In addition, the proposed hybrid interpolation algorithm is more suitable for trajectory with more path points.

IV. THE TRAJECTORY OPTIMIZATION
The NSGA-II optimization algorithm is applied to the multiobjective optimization, which are time, acceleration and jerk. The velocity, acceleration and jerk of the first point and the last point are all set to zero.
To carry out the optimization, the pointing points of end-effector should be given at first. Then, path points of each joint can be calculated based on the kinematic model. According to the hybrid interpolation algorithm for trajectory planning, the objective functions can be established. After the constraints and parameters of NSGA-II optimization algorithm are determined, the optimization result can be obtained through calculation.
In order to ensure the efficiency and stability of the pointing mechanism, the motion time, acceleration and jerk are taken into consideration in the optimization. The objective functions are described as: where, T is the total motion time, M is the number of joints, a is the acceleration and j is the jerk.
According to the motion requirements, the constraints of optimization can be determined. The constraints are shown in Tab. 2.
By referring to other literatures and application experience [25], [31]- [33], the parameters of the NSGA-II algorithm are shown in Tab. 3.  According to the inverse kinematics, the angles of each joint can be obtained.
The Pareto optimal front is obtained through the optimization, which is shown in Fig. 10.
According to the Pareto optimal front, a set of solution is selected, which is shown in Tab. 4.
The theoretical trajectories of joint 1 and joint 2 by hybrid interpolation are shown in Fig. 11. The theoretical trajectory of the end-effector is shown in Fig. 12, which is calculated through the forward kinematics.

V. THE TRAJECTORY PLANNING EXPERIMENT
To prove the validity of the proposed trajectory planning algorithm, the pointing experiment was conducted. The experiment system is shown in Fig. 13. In the experiment, the rotation angles of each joint are acquired by encoders, and the trajectory of end-effector is measured by a laser tracker.
Trapezoidal curve is the simplest interpolation algorithm for the trajectory planning. Even there is no need to optimize the trajectory. Moreover, many motor control cards integrate this algorithm. But the trapezoidal curve cannot ensure the continuity of jerk. The pointing accuracy of the  pointing mechanism is very important. The continuity of jerk is beneficial to the motion stability. The proposed hybrid interpolation algorithm can ensure the continuity of jerk and  has the advantages of simplicity and high computational efficiency. In order to achieve the overall performance, the multiobjective optimization was conducted. The comparison is between the trajectory planned by trapezoidal curve and the trajectory planned by the hybrid interpolation algorithm and optimized by NSGA-II algorithm. The comparison can reflect the validity of the hybrid interpolation algorithm combined with the NSGA-II optimization algorithm for trajectory planning and optimization. Ordinary interpolation trajectories of each joint were studied in the experiment. The velocities of each joint determined by trapezoidal function. The angle curves of each joint measured by encoders are shown in Fig. 14.  When the trajectories of each joint obtained by hybrid interpolation algorithm are as the inputs, the angle curves of each joint acquired by encoders are shown in Fig. 15.
According to the experiments using hybrid interpolation algorithm and ordinary interpolation algorithm, the position trajectories of end-effector measured by laser tracker are shown in Fig. 16.
From Fig. 14, Fig. 15 and Fig. 16, it can be found that the motions of each joint and the end-effector using hybrid interpolation algorithm are smoother than that using ordinary interpolation algorithm. The optimized trajectory using hybrid interpolation algorithm takes 23.732s, while trajectory using the ordinary interpolation algorithm takes 28.641s. The hybrid trajectory planning algorithm combined with NSGA-II algorithm achieves better performance and takes less time.
The torque data of each joint are also acquired. The torque curves of each joint using unoptimized trajectories obtained by ordinary interpolation algorithm are shown in Fig. 17. The torque curves of each joint using optimized trajectories obtained by hybrid interpolation algorithm are shown in Fig. 18.
It can be seen from Fig. 17 and Fig. 18 that the shock of torque of joint 1 is greatly reduced by using optimized joint trajectory obtained by hybrid interpolation algorithm. The shock of joint 2 is also reduced to some extent.
Factually, the pointing accuracy is of great importance to a pointing mechanism. Therefore, the pointing accuracy of each joint was studied, which is shown in Fig. 19.
From Fig. 19, it can be seen that pointing error of joints using optimized trajectories obtained by hybrid interpolation algorithm and NSGA-II algorithm is between 0 • − 0.026 • . The pointing error of joints using unoptimized trajectories obtained by ordinary interpolation algorithm is between 0 • − 0.264 • . The average error of joint 1 using optimized trajectories obtained by hybrid interpolation algorithm and NSGA-II algorithm is 0.013 • , which is 0.0596 • lower than that using unoptimized trajectories obtained by ordinary interpolation algorithm. The average error of joint 2 using optimized trajectories obtained by hybrid interpolation algorithm and NSGA-II algorithm is 0.0104 • , which is 0.0088 • lower than that using unoptimized trajectories obtained by ordinary interpolation algorithm.
The proposed interpolation algorithm is simple and efficient. Meanwhile, it can be seen that the hybrid interpolation algorithm combined with NSGA-II algorithm for trajectory planning can obviously improve the precision of the pointing mechanism. It is widely known that clearance is unavoidable in revolute joints. The joint clearance can affect the precision of the pointing mechanism. The trajectory planning using the hybrid interpolation algorithm ensures the continuity of jerk and the trajectory optimization using NSGA-II algorithm makes the mean jerk of the path points small. Therefore, it is beneficial to the reduction of the effect of joint clearance and the improvement of the precision.

VI. CONCLUSION
Trajectory planned by the proposed hybrid interpolation algorithm ''7-order polynomial + improved quartic uniform B-spline curve + 7-order polynomial'' can pass through all the path points with less calculation. Combined with the NSGA-II optimization algorithm, it was applied to the trajectory planning and optimization of a X-Y pointing mechanism. The pointing experiment was conducted. The trajectory planning and optimization can improve the efficiency and stability of the pointing mechanism. The azimuth pointing accuracy was obviously improved and the elevation pointing accuracy was also improved to some extent. The experimental results of the X-Y pointing mechanism proved the validity of the proposed trajectory planning algorithm. The hybrid interpolation algorithm can be applied to the trajectory planning of other robots in joint space as well, such as welding robots, where the robots need to pass through more path points and require better motion stability.