Manipulator Trajectory Tracking Based on Kinematics Model Predictive Control

: This paper explores the trajectory tracking strategy of a six-axis manipulator based on the kinematics model predictive control. Firstly, the kinematics of the manipulator was analyzed, and modeled mathematically. Next, the pose errors of the end effector were determined against the error parameters involving joints and rods. After that, the pose error model of the end effector was adopted to quantify how much the errors of different parameters affect the location of the manipulator. Finally, the geometric errors of manipulator trajectory were identified by the least squares (LS) method. Experimental results show the effectiveness of our model. The research results provide a reference for the theoretical analysis and application of manipulator trajectory tracking strategy.


INTRODUCTION
The rapid automation of modern industry has motivated the shift from traditional manufacturing to intelligent manufacturing, in the pursuit of more efficient production and better products. Featured by efficiency, flexibility, and precision, manipulators are now widely used in largescale production of various industries [1][2][3][4][5]. The mass application of manipulators is backed up by many incentive policies, such as Robotics Industry Development Program (2016 -2020) [6][7][8]. Manipulators are expected to achieve excellent control performance, which is manifested as smooth, continuous, and accurate trajectory and fast response. Therefore, it is of practical significance to study the error location and trajectory tracking of manipulators.
With the rising level of industrial intelligence, scholars at home and abroad are paying attention to the trajectory planning and optimization of industrial robots. The optimization algorithm must satisfy both kinematic and dynamic constraints [9][10][11][12]. After improving the Denavit-Hartenberg (DH) parameters, Ragaglia et al. [13] constructed a homogeneous transformation matrix between the rods of the industrial robot, built a kinematics model of the robot system through Newton-Euler iterative method, and calculated the working efficiency and error of the robot on MATLAB. Chiou and Sowmithran [14] transformed trajectory optimization into finite-dimensional secondorder cone programming, and obtained the numerical solution to the time-optimal trajectory; afterwards, Pan et al. [15] introduced the velocity-related torque constraints, and added the limitation of system energy consumption and total acceleration of motion, thereby effectively reducing the trajectory optimization time and energy consumption. Pan et al. [15] fitted the expected curve for the ideal robot trajectory by the segmented trajectory planning algorithm, and calculated the length of the curve with the compound Cotes formula. Based on the calculated result, the formulas were derived for the tangent direction and velocity curve at the interpolation position. Using the Jacobian matrix, the displacement, velocity, and acceleration vectors of the interpolation point were reversely mapped to the joint space of the robot, producing the real-time torque required for each joint.
The kinematics research of industrial robots mainly covers the modeling of robots, the transform of joint coordinate system, as well as the construction and reverse solving of kinematics equation [16,17]. Roveda et al. [18] investigated robot trajectory planning in two aspects, namely, the joint space for planning the motion parameters of each joint and the Cartesian space for planning the motion parameters of the end effector; the mechanics of the robot was also analyzed from two aspects: the static force in equilibrium state, and the dynamic force in the motion state; their algorithm was proved correct and reasonable through simulation.
The dynamic features of industrial robots must be considered due to the dynamic coupling between the movable base and the robot [19][20][21][22]. Upon classifying the space tasks of robots, Kubacki and Jakubowski [23] proposed a task planning method that can effectively acquire the motion sequence of the robot, while minimizing the consumption of time and energy, and provided the constraints and optimization rules under the assumptions of basic actions. Steinhauser and Swevers [24] presented the multi-objective optimization algorithm called nondominated sorting genetic algorithm II (NSGA-II), took the end velocity and maximum acceleration as decision variables, and optimized the trajectory with the objectives of minimizing time and energy consumptions; Huang et al. [25] also solved the pareto optimal set. Huang et al.
[25] constructed a dynamic model of industrial robots that can adjust the joint torque based on time scaling factor; the model can adjust the joint motion parameters when the torque exceeds the limit, making the online control of robot more efficient and reliable.
To sum up, the existing studies mostly handle trajectory optimization, fitting, and planning for manipulators. Few scholars have tried to locate the error of manipulator motions based on the kinematics model. After design and processing, the manipulator could have deviations in its pose during the operation, owing to various reasons. The precise tracking of the manipulator trajectory requires the recognition of ideal parameters. Therefore, this paper presents a trajectory tracking strategy for manipulators based on the kinematics model predictive control.
The main contents of this work are as follows: (1) Forward and inverse kinematics analyses were performed on a six-axis manipulator, and a kinematics mathematical model was established for the manipulator; (2) The pose errors of the end effector were determined according to the error parameters involving joints and rods; (3) The pose error model of the end effector was adopted to quantify how much the errors of different parameters affect the location of the manipulator. (4) The geometric errors of manipulator trajectory were identified by the least squares (LS) method; the effectiveness of our model was proved through experiments.

KINEMATICS ANALYSIS OF MANIPULATOR 2.1 Forward Kematics Analysis
Kinematics and dynamics analyses are the premise of the trajectory tracking and optimization of industrial manipulators. The former focuses on the motion features like joint position, rod velocity, and rod acceleration, while the latter tackles the effects of the forces or moments causing the joints to move, as well as the calculation of the matrix of each dynamic parameter. The improved DH parameter method was adopted for the forward kinematics analysis of the six-axis manipulator. The proposed coordinate system is shown in Fig. 1. Among the DH parameters, the length and torsional angle of a rod are denoted as ξ j−1 and β j−1 , respectively; the distance and angle between two rods are denoted as D j and ω j , respectively. The homogeneous transform between the adjacent rods in the proposed coordinate system can be implemented by: The DH parameters of the industrial manipulator in Eq. (1) can be derived from the structure and geometric parameters of the six-axis manipulator, as well as the improved DH coordinate system (Fig. 1). Substituting ξ j−1 , β j−1 , D j and ω j into Eq. (1), the homogeneous transform matrices can be obtained for every pair of adjacent rods in the six-axis manipulator: The forward kinematics analysis of the manipulator is to solve its motion equations, that is, to solve the location and pose of the end effector relative to the base coordinate system under the given DH parameters of the manipulator. In other words, the forward kinematic analysis aims to solve the homogeneous transform matrix below: Eq. (5) shows that the value of 0 6 R can be obtained through continued multiplication of the inverse homogeneous transform matrices of all the pairs of adjacent rods.

Inverse Kinematics Analysis
The inverse kinematics analysis of the manipulator is the opposite of the forward kinematics analysis. It aims to solve the joint variables that facilitate the expected pose of the end effector under the known DH parameters of ξ j−1 , β j−1 , D j , and ω j . Based on the spatial geometric projection relations between the rods, this paper chooses to solve the joint variables of the first three joint axes by the geometric projection method, which has a small computing load and an intuitive solving process.
To solve the joint angle ω 1 of joint axis 1, the first step is to construct the homogenous transformation matrix 0 6 R from the base coordinate system of the manipulator to the flange coordinate system of the sixth axis: The geometric projection method requires the last three axes of the six-axis manipulator to intersect at one point, and satisfy the projection relationship of the spatial set. It is assumed that, in the base coordinate system, the homogeneous coordinates of the intersection point can be expressed as [a v , b v , c v , 1] T . In the coordinate system of the sixth rod, the homogeneous coordinates can be represented Joint 1 is a left-right symmertic structure connected to the base. The angle ω 1 of the joint has another solution in addition to that expressed by Eq. (9): Let [ 1 a v , 1 b v , 1 c v , 1] T be the homogeneous coordinates of the wrist reference point in the coordinate system of rod 1. Then, the relationship between [ 1 a v , 1 b v , 1 c v , 1] T and [a v , b v , c v , 1] T can be described as: In Fig. 3, the angle ω ξ can be solved by the geometric relationship of the wrist reference point in the coordinate system of rod 1: The angle ω ε can be solved by: Then, the joint angle ω 2 of joint axis 2 can be calculated by: Similarly, joint 2 is left-right symmetric, and its joint angle ω 2 has two solutions. The other solution can be expressed as: Based on the geometric relationship of the projection of the wrist reference point in the coordinate system of rod 2 (Fig. 4), the angle ω v can be solved by: The angle ω ϕ can be solved by: Then, the joint angle ω 3 of joint axis 3 can be calculated by: The other solution to ω 3 can be expressed as:

Kinematics Modeling
Knowing the position, velocity and acceleration of each joint that characterize the trajectory of the manipulator, it is possible to solve the driving force or torque applied to each joint, thereby realizing the tracking, optimization, and control of manipulator trajectory. This paper establishes the Lagrangian dynamics equation for the six-axis manipulator. Then, the kinetic energy and potential energy of each rod, as well as the total kinetic energy E K and total potential energy Q of the manipulator were calculated in turn. Let the Lagrangian function LAG be the difference between E K and Q, J j , J' j and J'' j be the angular displacement, angular velocity, and angular acceleration of joint j, respectively. Then, the Lagrangian equation can be constructed as: Let G jl , CE jln and H j be the elements in the mass matrix, the centrifugal force matrix, and the gravity vector. The dynamic equation of the manipulator can be derived from Eq. (21):

MOTION ERROR LOCATION AND TRAJECTORY TRACKING 3.1 Pose Errors Between Joint Coordinate Systems
During actual operations of the manipulator, DH parameters like ξ j , D j , β j , and ω j have varied degrees of deviations, under the influence of all sorts of disturbances, such as handling, grinding, assemblage, and hightemperature operation. be the actual and theoretical homogenous transform matrices between rods, respectively. Then, there will exist a certain bias dR j between the two matrices, under the effect of the bias of DH parameters. Since dR j is relatively small, and the error between T j R and M j R is smaller than 0.001, the two matrices are approximately linearly correlated.

Figure 5 Sketch map of joint errors
Let Δξ j , ΔD j , Δβ j and Δω j be the errors corresponding to ξ j , D j , β j and ω j , respectively. Then, the relationship between the two matrices and dR j can be described as: According to the theorem of the total differential, dR j can be expressed as:

End Location Error Model
The precise location of the end effector requires the calculation of the effector's pose error based on the error parameters involving joints and rods. Similarly, there is a bias between the theoretical and actual differential homogenous transform matrices T m W and M m W . Let dW m be the differential homogeneous transform matrix. Then, the relationship between the two matrices and their error dW m can be described as: By cumulative calculation of the differential errors of the coordinate system of each joint, the total pose error of the end effector can be obtained: where, the second term dW characterizes the correlation between error dW m and the theoretical differential homogeneous transform σR j of joint j, O j , D and σ can be calculated by:

EXPERIMENTS AND RESULTS ANALYSIS
Our manipulator location error model was analyzed and verified on MATLAB. To ensure that the experimental results are in line with the actual working conditions, all joint variables ω of the manipulator were unified to the same time variable, so that the manipulator pose could change with time. Tab. 1 lists the DH parameters of the manipulator. The accuracy of the proposed manipulator location error model was tested by comparing the theoretical poses obtained by the motion model with ideal parameters against the actual poses obtained by the kinematics model with bias. It can be seen that the theoretical location error of our algorithm for the end effector was very close to the measured location error in actual operation. The slight gap between the two mainly arises from the removal of high-order differentials in model derivation.  Unlike the results of Tang G. et al. [19], two of the three structural parameters ξ, D and β were fixed, and only the last parameter was adjusted. Then, the rod torsional angle β changed obviously, compared to the previous error. This means the location error of the end effector is not greatly affected by rod spacing or rod length. During the processing and assemblage of the manipulator, it is important to ensure the tracking accuracy of torsional angle, in order to realize precise location and tracking.
To verify the effectiveness of the parameter identification method for the six-axis manipulator, the kinematics calibration simulation was carried out for the manipulator on MATLAB. Firstly, the pose errors between the coordinate systems of different joints were preset according to the parameters of the motion model and tolerance of the manipulator. Tab. 2 lists the core parameters related to the preset pose errors of some rods. Next, 20 joint angles, which are evenly distributed, were selected, and imported to the motion model with ideal parameters and the kinematics model with bias separately. The theoretical location coordinates were subtracted from the actual location coordinates of the same joint angle, producing the recognition errors in Tab. 3. Finally, the total error of the end effector was solved. Comparing the errors before and after the calibration can obtain the effect of the kinematics calibration of the manipulator. Substitute the kinematic parameters corrected according to Tab. 3 into the theoretical space position of the end effector trajectory of the manipulator. Compare it with the actual position of the end effector. The positioning error after calibration can be obtained. Fig. 8 shows the change curve of the positioning error of the end effector of the robotic arm before and after calibration.
The kinematics calibration effect of the manipulator can be obtained by comparing the errors before and after the calibration. After being corrected against Tab. 3, the kinematics parameters were imported to the theoretical spatial positions of the trajectory of the end effector, and compared again with the actual positions of the end effector, producing the post-calibration location errors. Fig. 8 shows the trend of location errors before and after calibration.
As shown in Fig. 8, the location error of the end effector peaked at 1.489 mm in direction B of the coordinate system, and was reduced to the minimum of 0.42 mm; the error peaked at 0.795 mm in direction A, and was reduced to the minimum of 0.415 mm; the error peaked at 0.924 mm in direction A, and was reduced to the minimum of 0.377 mm. The LS calibration clearly suppresses the location error of the end effector, a sign of improved tracking effect on the manipulator. This further verifies the effectiveness of the proposed algorithm.

CONCLUSIONS
Drawing on the kinematics model predictive control, this paper studies the trajectory tracking strategy of a sixaxis manipulator. First of all, the authors analyzed the kinematics of the manipulator, and built the relevant mathematical model. Based on the pose error model of the end effector, quantitative analysis was carried out to evaluate how much the errors of different parameters affect the location of the manipulator. In addition, the LS method was employed to recognize the geometric errors of manipulator trajectory. Then, the accuracy of the proposed manipulator location error model was tested by comparing the theoretical poses obtained by the motion model with ideal parameters against the actual poses obtained by the kinematics model with bias. The comparison verifies the accuracy of the proposed manipulator location error model. Furthermore, the errors before and after calibration were compared to confirm the effectiveness of our kinematics calibration approach for manipulators. The research findings shed new light on the trajectory tracking strategy of manipulators.