Online Trajectory Tracking Control Based on the Explicit Form of the Equations of Motion for Serial Manipulator Using the New Formulation
In this article, the novel approach to equations of motion for serial manipulators developed in literature by Bertrand and Bruneau (2013) is extended to make it usable for manipulators with general joints (i.e., prismatic and/or rotational). In this method, the dynamic model is explicitly and directly obtained from the structural parameters of the manipulator and matrix algebra without intermediate heavy calculations such as energy derivation. The correctness and efficiency of the described method are demonstrated through simulation of the dynamical equations of a 5-DOF SCARA robot. The simulation results obtained using the new formulation were compared with those derived by Kane’s method, Lagrange–Euler formulation, and GIM (generalized inertia matrix)-Christoffel’s algorithm, which proves the efficiency and correctness of the presented model. It was concluded that the new formulation requires less CPU time to generate explicit closed-form inverse dynamics. Finally, to illustrate the power of the new formulation in real-time control, a trajectory tracking control for the SCARA manipulator based on the numeric and symbolic computation of the inverse dynamic is established, and it is shown that the numeric and symbolic approaches based on our method are equivalent. As a consequence, the applicability of the new formulation in real-time model-based control is proved.
The dynamics of industrial manipulators are highly nonlinear. The precise, explicit, closed-form dynamics of robots play an important role in their design, simulation, and control. There are two categories of dynamics modeling: direct and inverse dynamics. Inverse dynamics modeling is used to calculate the required actuator generalized forces in order to achieve the planned trajectory and is also known as model-based control, whereas the direct dynamics model determines the end effector motion for a given actuator force/torque. The inverse dynamics are related to the control and design of the manipulator, whereas the direct dynamics are associated with simulation. As regards robot control, the dynamic model is the most important part to design controllers and determine controllers’ parameters; inverse dynamics are always employed to calculate the generalized forces as feed-forward data in order to follow the desired trajectory, which is called computed torque control or internal model control. The trajectory tracking performance of most commercial robots is limited because they are controlled by ignoring coupled dynamics and using a simple single-joint feedback controller, which is an imperfect model [1–4].
Several techniques have been employed to derive the dynamics model of manipulators: the Newton–Euler method [5–7], Lagrange–Euler method [4, 8–11], Kane’s method [12, 13], and Gibbs–Appell equations .
Using the Newton–Euler approach, the acceleration of each isolated rigid body and each reaction force and moment between the links should be determined, whereas the workless constraint moments and forces are not used to control the manipulator, which results in a large number of equations and leads to a high computational cost. The alternative one, the recursive Newton–Euler method [6, 7], gives the more efficient algorithm; however, it is not applied to the control design of the manipulator and does not give better insight into the form of the equations. For this purpose, a set of explicit closed-form equations of motion must be determined.
In Lagrange’s formulation, the manipulator is considered an entire system instead of isolating each link; all workless constraint forces and moments can be automatically eliminated in the beginning. However, its disadvantage is the calculation of complex derivatives of Lagrangians, which always leads to a huge number of intermediate and complicated computations. Therefore, the Lagrange–Euler equations are the most adequate form for controllers’ design and simulation of manipulators. Unfortunately, this method is consuming more computational time for online control of manipulators because the procedure involves laborious manipulation of energy expressions. So, the controller design of a manipulator was usually based on neglecting the centrifugal and Coriolis terms of the dynamic model.
Kane’s method is based on analytic mechanics and vector mechanics. To obtain the generalized forces by using Kane’s equation, the active forces and inertia forces are multiplied, respectively, by the intermediate vector entities such as partial angular speeds and partial speeds. It has the benefit of automatically eliminating workless forces and moments without applying the principle of virtual work. The introduction of intermediate variables results in a bit of laborious calculation.
The Gibbs–Appell’s formulation is formulated from the Gibbs function, which is considered the kinetic energy of acceleration. The dynamics model is derived by differentiating the Gibbs function with respect to the generalized velocities.
However, none of the previous formalisms cited above offers a simple and straightforward combination between the structural parameters of the manipulators and the dynamical equations. In fact, the dynamical equations are usually established based on recursive algorithms or require complicated calculations, such as the differentiation of energies. Therefore, this kind of model cannot directly provide a clear description and comprehension of the dynamical equations of a multibody system in order to exploit it efficiently and accurately in the robot control industrial architecture. Consequently, the dynamic equations have not been clarified yet, and the complete comprehension of the dynamics of a given multibody system has not been reached. One way to improve this point is to revisit the dynamical formalisms.
In addition, the motion of manipulators with variable or unknown loads may slow down or speed up, and the manipulators are not capable of tracking the planned trajectory closely. In order to control a manipulator with an unknown or variable load that follows a desired trajectory, it is necessary to calculate the generalized forces required to move all its joints repeatedly and accurately at an acceptable time step (between 10 ms and 1 ms).
In general, the control algorithm involves the calculation of the suitable input generalized forces employing the measured values of generalized speeds and coordinates and the calculated values of generalized accelerations, which are normally computed from dynamic formulations cited in literature. The execution time of the dynamics model is one of the main problems because none of the aforementioned formulations can be exploited numerically in real-time control. The challenging task for real-time control is that the dynamic model must be computed very frequently at every integration step. Consequently, the feasibility of online implementation is highly dependent on the dynamics formulation, which affects the computation cost.
A novel formulation that satisfies all of the above requirements has been proposed by . In this novel formulation, the dynamical equations were obtained by considering only the structural parameters and matrix algebra. This method is simple and straightforward, as the terms of the inertia matrix and coriolis/centrifugal matrix can be computed simultaneously and independently of the generalized velocities and accelerations. But it was developed especially for a single kinematic chain with only rotational joints. Using this formulation, a dynamical approach was developed for trajectory planning of a Mikron UCP710 multiaxis machining system and a KUKA anthropomorphic robot .
Therefore, we aim with the new formulation to obtain a precise and efficient inverse dynamic for both symbolic and numeric approaches without any simplification of the dynamical equations for the development of the explicit form of the equations of motion in the active joint space.
The contributions of this paper: the first objective is to improve the previous formalism  to make it usable for serial manipulators with general joints (i.e., prismatic and/or rotational). Furthermore, computation efficiency is an important issue in the derivation of inverse dynamics; however, the computational cost of this new formulation with respect to other approaches has not been compared yet, which is the second objective. The final objective is to demonstrate the feasibility of online control for manipulators based on the new formulation.
This study is organized as follows: Section 2 introduces the system and notation used in the new formalism. Section 3 presents the generalized new formalism for serial manipulators with general joints. In Section 4, the kinematic and dynamic properties of the 5-DOF SCARA manipulator based on this formalism are presented using the symbolic computer algebra software SYMPY. Section 5 discusses the simulation results. The future work and conclusions are highlighted in the last section.
This section reviews briefly some of the notations used for the new formalism.
2.1. General Presentation
The considered multibody system is shown in Figure 1. It is a serial manipulator composed of N rigid links and N single-degree-of-freedom joints of a general type, either revolute or prismatic. The base frame is numbered as a link 0. The joint i being the connection between link i − 1 and link i. The generalized actuated force associated with joint i is denoted by .
The notations and their descriptions are given in Table 1.
3. New Formulation of a Serial Manipulator with General Joints
The objective of this section is to extend the existing work presented in , which is developed especially for serial manipulators with only revolute joint to cover those with revolute or prismatic joints.
3.1. Accelerations of the Body in the Case of General Joints
The angular velocity of the body with respect to the base is expressed as follows:
Its dual matrix can be expressed aswhere the term denoted the dual tensor associated with a vector , which is defined by
The linear velocity and the linear acceleration of body with respect to frame are expressed, respectively, by
As shown in , the dual matrix can be established as follows:
The expression of the acceleration of the mass center can be derived as follows:
By using the relation between cross product and dual tensor, which is defined by , we can write
3.2. The Dynamic Generalized Forces
The dynamic generalized forces denoted by can be expressed bywhere is the torque vector generated by the dynamic forces applied about to body , and is the vector of the force due to the dynamic forces applied to the body.where and are described by the Newton–Euler equations as follows:
3.3. Generalized Force Due to the Environment Forces
The environmental generalized forces applied to the i-th DoF denoted by , can be denoted bywhere are the total external torques applied about on body, and are the total external forces applied to body.
Since the external torque is applied about , the vector can be written aswhere is the external force other than gravity applied on body, and is the external torque other than gravity applied on the body expressed about.
3.4. The Generalized New Formulation of Inverse Dynamics
The dynamic equations can be established by
The above expression for is very simple to obtain than appears at first sight because the coefficients and cannot both be nonzero simultaneously.
The inverse dynamics can be established in the following equivalent form:
The coefficients of the generalized inertia matrix , the coefficients relative to centrifugal forces , the coefficients relative to Coriolis forces , the gravity forces , and the external forces can be expressed in function of the system’s structural parameters independently of the generalized velocities and accelerations :
The advantages of this dynamic model are as follows.
This formulation uses 3 × 3 matrices and 3 × 1 vectors to describe the dynamics of the system, which can be exploited to reduce the computational complexity of the closed-form equations of motion.
This formulation significantly improves the understanding of the equations of motion of a specific system: It provides the ability to separate the dynamic forces with respect to its mass and inertia and with respect to its type of joint (translational or rotational) It may be employed to accurately identify the influence of a body on each joint force. Furthermore, the later operation can be accomplished with each part of the equations, such as the inertia, gravity, centrifugal, Coriolis, and external force terms, of each body applied to each joint.
For applications, in order to use this method, it is necessary to consider the attached bases of the different matrices and vectors by using the rotation matrices between the bases. As a consequence, equations (24) through (28) should be written as follows:
3.5. Matrix Form of the Equations of Motion
The dynamic equations of motion (23) can be established in a more closed form vector-matrix notation as follows:where is the generalized forces vector; are the joint positions, velocities, and accelerations vectors; is the generalized inertia matrix of the manipulator; is the centrifugal/Coriolis matrix; is the gravity force vector; and is the vector of external forces. In this formulation, the inertia and Coriolis/centrifugal matrices of the manipulator as well as the gravitational force is only dependent on the configuration of the manipulator .
The generalized inertia matrix M (q) can be denoted by
The centrifugal/Coriolis matrix C can be denoted bywhere is a matrix denoted as
4. Application Example: 5-DOF SCARA Manipulator
4.1. Presentation of the Manipulator
The proposed method has been employed for the 5-DOF SCARA robot (Figure 2) . : the generalized coordinates for revolute joints , and for prismatic joints , . : the lengths of the five links , , : the lengths from the origins to the centers of mass of the second, third, and fourth links , , : the masses of the links , , : the inertia matrix of the second, third, and fourth links
The general transformation matrix between the i − 1th link and the i-th link can be given by
The general rotation matrix between the i − 1th link and the i-th link can be given by
From equation (36), we can get the end-effecter coordinates:
For simplicity, the condition is set. Where is known, then the inverse kinematic model is given bywhere z and d1 are known.
4.3. The Inverse Dynamics of the 5-DOF SCARA Manipulator
The inverse dynamic model of the SCARA manipulator when the external forces are omitted can be expressed in a compact form as
Here, is the generalized mass matrix; is the centrifugal/Coriolis matrix; is the gravitational force; are the generalized coordinates, speeds, and accelerations, respectively; is the generalized forces vector.
To the opposite of the aforementioned methods, the main advantage of the new formalism is to derive the dynamics model directly from the physical parameters and configuration of the manipulator without complex computations, such as the differentiation of energies, and without recursive algorithms. Practically, it is only needed to introduce the physical parameters of the system and to apply matrix algebra.
This procedure is implemented in a symbolic computer software algebra called SYMPY based on the Python language, as shown in Figure 3.
In the next section, a comparative study in terms of computational cost between our approach and other formulations is made to illustrate the power of the new formulation.
5. Results and Discussion
In the previous section, the dynamic description of a 5-DOF SCARA manipulator is obtained by the generalized new formulation. However, the efficiency of this method with respect to other existing formulations has not been determined to date. The objective of this section is to validate the presented method, then compare it with other approaches in terms of dynamic formulation complexity, and finally prove its applicability for online control.
5.1. Model Validation
The direct dynamics are employed to validate the present model. For the direct dynamic problem, the generalized active forces are given. The objective is to determine the end-effector trajectory or motion. To prove the correctness of the dynamical model, the simulation results in terms of the joint profile obtained by our approach and those derived by existing methods like Kane’s method are compared with each other. Figure 4 shows the results calculated using the two methods. The results are identical, which means they produce the same output, which validates the correctness of the presented model.
5.2. Computational Cost
In order to prove the power of this new formulation with respect to other existing methods, we also employed the Lagrange–Euler method, Kane’s method, and GIM (generalized inertia matrix)-Christoffel’s algorithm to calculate the same equations of motion of the 5-DOF SCARA manipulator. The computer algebra software SYMPY is used to compute symbolically the dynamics model using these four methods. The algorithm is executed on a computer equipped with an Intel Core i3-6006U CPU. Table 3 shows the comparison of simulation results in terms of computational cost obtained by the aforementioned methods and our method.
The results confirm clearly the power of the new formalism for efficient generation of the explicit form of equations of motion. By using more advanced computer architecture, we can have the minimum CPU time. As the different terms of the dynamic equations can be computed simultaneously, the present method is more appropriate for parallel algorithms. Consequently, the robot model can be implemented for real-time control.
It is more desirable to get precise and efficient explicit forms of equations of motion because of their important role in model-based control. In Section 5.3, we will illustrate that the dynamics model obtained from our method could be implemented in real-time control.
5.3. Computed Torque Control CTC Based on the New Formulation
In the previous section, a comparative study in terms of CPU time between the different algorithms was achieved, and it was concluded that the new formulation is the more efficient one in providing the explicit form of equations of motion. In this subsection and based on the SCARA manipulator, we will demonstrate that the fully numeric computation and the symbolic computation of the inverse dynamic for online control based on the new formulation are equivalent.
The desired trajectory to follow denoted by is defined as
To track the desired trajectory, the model-based control scheme chosen in the present work is the computed-torque control as shown in Figure 5.
To illustrate the power of the new formulation in online computation of equations of motion, the numeric and symbolic computation of the inverse dynamic of the SCARA manipulator are elaborated and compared using the SymPy and NumPy libraries of the Python language. In the numerical methods, a numerical value is related to each variable that is added in model construction, and the equations of motion are repeated for each integration step. In symbolic methods, a symbol is associated with each variable, which represents their dependence on system parameters and states. Unfortunately, the generation of symbolic equations is an extremely complex problem for complicated mechanisms.
The control law is defined bywhere is the error vector in the joint space and is the difference between the desired values of the joint coordinates and the actual values, and Kp = diag (kp), Kd = diag (kd).
To simulate the manipulator motion, the direct dynamic is used, and then, equation (39) can be written aswhere represents the control torque vector. For a given value of generalized control input , the resulting motion can be obtained by the numerical integration of the generalized accelerations. As for the 5-DOF SCARA manipulator, we can get the discrete state space model as follows:where the integration step is chosen to be = 1 ms, the initial conditions of the system are given as , , the values of the CTC control gains are selected through numerical simulation as follows: kp = 200, . The numerical values of different parameters are given in Table 4. Finally, a Python program is developed to simulate the end-effector motion during the task using the developed algorithm.
After developing the manipulator model and the simulation environment and establishing the control laws to be used, a test trajectory in space was determined to make the manipulator model follow it.
The control simulation algorithms are executed by a Python program on the same computer mentioned before. Function “time” in Python gives the estimated run time of the control cycle of each algorithm; for the fully numeric algorithm, it is 0.016 s. Whilst, for the code generated from symbolic computation, the computational cost of the control cycle is about 0.0175 s.
It is shown that the computational costs of the numeric and symbolic approaches based on the new formulation are equivalent, which is not the cases when using the traditional methods. It can be concluded that the numeric approach based on the new formulation is more suitable for online control. In addition, the present method is more suitable for parallel programming as the different terms of the equations of motion can be computed separately and simultaneously.
By considering only the revolute joints, Figure 6 shows the desired and actual motions in the workspace. The actuation torques are shown in Figure 7. The trajectory tracking error in joint space is shown in Figure 8. The desired and actual trajectories in joint space are shown in Figure 9. These results validate the actuator torques computed by the developed model.
Imperfections are present in the robot model, such as friction in the joints and unknown external perturbations and uncertainties in the parameters. So, it is more important to take these effects into account in order to obtain precise and perfect explicit closed-form dynamics in the actuation space because of their crucial role for model-based control; the more perfect the model is, the better the performances of its model-based controllers are . However, when the dynamic model is employed in model-based control, the neglecting of any part of the dynamic equations will affect the control performance largely, which will be shown in Figure 10.
In this paper, the domain of multibody dynamics is restudied, and a new systematic approach is extended to derive explicit closed-form dynamic equations for a general serial manipulator (i.e., systems with revolute and/or prismatic joints). The method depends on matrix algebra prior to performing differentiation. Owing to the use of a 3 × 3 matrix instead of a 4 × 4 matrix and the fact to isolate the effect of each body with respect to its inertia and mass and with respect to the type of joint (translational or rotational), the computational redundancy is reduced from the modeling procedure, making it also an efficient automated method of model formation. Furthermore, it is shown that the inertia and centrifugal/Coriolis matrices can be calculated separately and independently of generalized velocities and accelerations, which makes the determination of the equations of motion straightforward and more compact. Based on the demonstration of the developed model, a 5-DOF SCARA (selective compliant assembly robot arm) manipulator is used to illustrate its simplicity and correctness as compared with other formalisms, which are proved by simulations. A comparative study of dynamics formulation complexity has demonstrated the power of the new formulation with respect to Kane’s method, Lagrange–Euler method, and GIM-Christoffel’s algorithm. It is shown that the derivation process using the new formulation for the explicit closed-form equations of motion is more efficient, and the computational cost is low enough for online model-based control. In future work, it is important to adapt the new formulation to parallel manipulators.
The datasets used to support the results of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
J. Angeles, Fundamentals of Robotic Mechanical Systems: Theory Methods and Algorithms, Springer, Berlin, Germany, 2007.
J. J. Craig, Introduction to Robotics: Mechanics and Control, Prentice Hall, Hoboken, NJ, USA, 2005.
W. Khalil and E. Dombre, Modeling, Identification and Control of Robots, Butterworth-Heinemann, Oxford, UK, 2004.
W. Khalil, “Dynamic modeling of robots using newton-euler formulation,” Informatics in Control Automation and Robotics, Lecture Notes in Electrical Engineering, vol. 89, 2011.View at: Google Scholar
K. Miller and R. Clavel, “The lagrange-based model of delta-4 robot dynamics,” Roboter Systeme, vol. 8, pp. 49–54, 1992.View at: Google Scholar
M. E. Kahn, “The near minimum-time control of open articulated kinematic chains,” Stanford University, Stanford, CA, USA, 1969, Ph.D. thesis.View at: Google Scholar
Z. Hussain and N. Z. Azlan, Kane’s Method for Dynamic Modeling, I2CACIS, Malaysia, 2016.
M. Vulliez, S. Lavernhe, and O. Bruneau, “Dynamic approach of the feedrate interpolation for trajectory planning process in multi-axis machining,” International Journal of Advanced Manufacturing Technology, vol. 88, pp. 2085–2096, 2017.View at: Google Scholar
R. Featherstone, Rigid Body Dynamics Algorithms, Springer, New York, NY, USA, 2008.