# Online Trajectory Tracking Control Based on the Explicit Form of the Equations of Motion for Serial Manipulator Using the New Formulation

Authors: Otman El Baji; Nabil Ben Said Amrani; Driss Sarsri
Journal: Journal of Robotics
 (2022)

---

## Abstract

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.


---

## Body

## 1. Introduction

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 [14].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 [15]. 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 [16].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 [15] 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: Section2 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.


## 2. Notations

This section reviews briefly some of the notations used for the new formalism.


### 2.1. General Presentation

The considered multibody system is shown in Figure1. 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 τi.Figure 1
General scheme of the considered serial manipulator [15].


### 2.2. Notations

The notations and their descriptions are given in Table1.Table 1
Notations for the new formulation.
SymbolsDesignationsSiThei-th body or link∑i∪k=iNSiThei-th subsystemqTheN-dimensional generalized coordinates.q˙, q¨TheN-dimensional velocities and accelerations vectors of the joints.εi, ε¯i=1−εiThei-th joint type, εi is zero if the i-th joint type is a revolute, εi is one if the i-th joint type is a prismatic.oiThei-th joint center.ciThe centeroid of the linkSicij=oicjThe position vector ofcj relative to joint center oiuiThei-th joint axis.ugThe downward unit vector specifying the gravity direction.ΒiThei-th base that is attached to body Siℜioi,ΒiThe frame attached to bodySimiThe mass of bodySi.ΙiThe 3 × 3 inertia tensor associated with bodySi defined in ciJi=−Ii+1/2TrIiIThe 3 × 3 pseudoinertia tensor associated with bodySi defined in ciIThe 3 × 3 identity matrixτ=τ1……τNTTheN-vector of the joint generalized forces.RijThe rotation matrix of baseΒj with respect to base Βi


## 2.1. General Presentation

The considered multibody system is shown in Figure1. 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 τi.Figure 1
General scheme of the considered serial manipulator [15].


## 2.2. Notations

The notations and their descriptions are given in Table1.Table 1
Notations for the new formulation.
SymbolsDesignationsSiThei-th body or link∑i∪k=iNSiThei-th subsystemqTheN-dimensional generalized coordinates.q˙, q¨TheN-dimensional velocities and accelerations vectors of the joints.εi, ε¯i=1−εiThei-th joint type, εi is zero if the i-th joint type is a revolute, εi is one if the i-th joint type is a prismatic.oiThei-th joint center.ciThe centeroid of the linkSicij=oicjThe position vector ofcj relative to joint center oiuiThei-th joint axis.ugThe downward unit vector specifying the gravity direction.ΒiThei-th base that is attached to body Siℜioi,ΒiThe frame attached to bodySimiThe mass of bodySi.ΙiThe 3 × 3 inertia tensor associated with bodySi defined in ciJi=−Ii+1/2TrIiIThe 3 × 3 pseudoinertia tensor associated with bodySi defined in ciIThe 3 × 3 identity matrixτ=τ1……τNTTheN-vector of the joint generalized forces.RijThe rotation matrix of baseΒj with respect to base Βi


## 3. New Formulation of a Serial Manipulator with General Joints

The objective of this section is to extend the existing work presented in [15], which is developed especially for serial manipulators with only revolute joint to cover those with revolute or prismatic joints.


### 3.1. Accelerations of the BodySk in the Case of General Joints

The angular velocityωk/0 of the body Sk with respect to the base is expressed as follows:(1)ωk/0=∑j=1kε¯jωj/j−1=∑j=1kε¯jq˙juj.Its dual matrix can be expressed as(2)ω^k/0=∑j=1kε¯jω^j/j−1=∑j=1kε¯jq˙ju^j,where the term x^ denoted the dual tensor associated with a vector x=x1x2x3T, which is defined by(3)dualx=x^=0−x3x2x30−x1−x2x10.The linear velocityVojj−1 and the linear acceleration Γojj−1 of body Sj with respect to frame ℜj−1 are expressed, respectively, by(4)Vojj−1=εjq˙juj,Γojj−1=εjq¨juj.As shown in [17] and by using equations (1) and (2), the angular acceleration ak/0 of Sk relative to ℜ0 for the general joints can be derived as follows:(5)ak/0=∑j=i+1kεjq¨juj+q˙j∑n=1j−1ε¯nq˙nu^nuj.As shown in [15], the dual matrix a^k/0 can be established as follows:(6)α^k/0=∑j=1kε¯jq¨ju^j+q˙j∑n=1j−1ε¯nq˙nu^nu^j−u^ju^n.The expression of the accelerationΓck of the mass center ck can be derived as follows:(7)Γck=Γok+ak/0∧okck+ωk/0∧ωk/0∧okck,(8)Γok=∑j=1kεjΓojj−1+2ωj−1/0∧Vojj−1+∑j=1k−1ε¯jaj/0∧ojoj+1+ωj/0∧ωj/0∧ojoj+1.Consequently, including equations (8) in equation (7), we have(9)Γck=ak/0okck+ωk/0∧ωk/0∧okck+∑j=1kεjΓojj−1+2ωj−1/0∧Vojj−1+∑j=1k−1ε¯jaj/0∧ojoj+1+ωj/0∧ωj/0∧ojoj+1.By using the relation between cross product and dual tensor, which is defined byx∧y=x^⋅y=−y^⋅x, we can write(10)Γck=a^k/0+ω^k/0ω^k/0okck+∑j=1kεjΓojj−1+2ω^j−1/0Vojj−1+∑j=1k−1ε¯ja^j/0+ω^j/0ω^j/0ojoj+1.As demonstrated in [15] and taking into account (2) and (6), the vectors ∑j=1k−1ε¯ja^j/0+ω^j/0ω^j/0ojoj+1 and a^k/0+ω^k/0ω^k/0okck can be expressed by(11)∑j=1k−1ε¯ja^j/0ω^j/0ω^j/0ojoj+1=∑j=1kε¯jq¨ju^+jq˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^jojok,(12)a^k/0+ω^k/0ω^k/0okck=∑j=1kε¯jq¨ju^j+q˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^j⋅okck.By using (2) and (4), we get(13)∑j=1kεjΓojj−1+2ω^j−1/0Vojj−1=∑j=1kεjq¨juj+2q˙j∑n=1j−1ε¯nq˙nu^nuj.Lastly, based on equations (11)–(13), the acceleration Γck can be derived as follows:(14)Γck=∑j=1kεjq¨juj+2q˙j∑n=1j−1ε¯nq˙nu^nuj+ε¯jq¨ju^j+q˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^jcjk=∑j=1kεjq¨jI+2q˙j∑n=1j−1ε¯nq˙nu^n−ε¯jq¨jI+q˙j2u^j+2q˙j∑n=1j−1ε¯nq˙nu^nc^jkuj=∑j=1kεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj.


### 3.2. The Dynamic Generalized Forces

The dynamic generalized forces denoted byFdΣi can be expressed by(15)FdΣi=ε¯iδΣioi+εiRdΣi=∑k=iNε¯iδSkoi+εiRdSk,∀i∈1,N,where δoiSk is the torque vector generated by the dynamic forces applied about oi to body Sk, and RdSk is the vector of the force due to the dynamic forces applied to the bodySk.(16)FdΣi=∑k=iNε¯iε¯kδSkci+mkc^ik⋅Γck+εiRdSk,where RdSk and δSkck are described by the Newton–Euler equations as follows:(17)FdΣi=∑k=iNε¯iΙk⋅αk/0+ω^k/0Ιk⋅ωk/0+mkc^ik⋅Γck+εimkΓck.As shown in [15], by considering equations (1), (2), and (5), and by using equation (14), equation (17) becomes(18)FdΣi=∑k=iNε¯iε¯k∑j=1kε¯jq¨jIk+q˙j2u^jΙk+2q˙j∑n=1j−1ε¯nq˙ndualJkunuj+mkε¯ic^ik+εiI∑j=1kεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj=∑k=iN∑j=1kε¯iε¯kε¯jq¨jIk+q˙j2u^jΙk+2q˙j∑n=1j−1ε¯nq˙ndualJkun+mkε¯ic^ik+εiIεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj.


### 3.3. Generalized Force Due to the Environment Forces

The environmental generalized forces applied to thei-th DoF denoted by FeΣi, can be denoted by(19)FeΣi=ε¯iNoiΣi+εiReΣi=∑k=iNε¯iNoiSk+εiReSk,where NoiSk are the total external torques applied about oi on bodySk, and ReSk are the total external forces applied to bodySk.Since the external torque is applied aboutck, the vector FeΣi can be written as(20)FeΣi=∑k=iNε¯iMk+cik∧Fk−mkg⋅ug+εiFk−mkg⋅ug=∑k=iNε¯iMk+c^ikFk−mkg⋅ug+εiFk−mkg⋅ug,where Fk is the external force other than gravity applied on bodySk, and Mk is the external torque other than gravity applied on the body Sk expressed aboutck.


### 3.4. The Generalized New Formulation of Inverse Dynamics

The dynamic equations can be established by(21)τi=FdΣi+FeΣiui,∀i∈1,N.By using (18), (20), and (21), the generalized new formulation of the inverse dynamics for a serial manipulator with general joints can be derived as(22)τi=∑k=iNε¯i∑j=1kq¨jε¯jε¯kIk−mkc^ikc^jk+εjmkc^ik+q˙j2ε¯jε¯ku^jΙk−mkc^iku^jc^jk+2q˙j∑n=1j−1ε¯nε¯jε¯kdualJkun−mkc^iku^jc^jk+εjmkc^iku^nq˙nuj+Mk+c^ikFk−mkgugεi∑j=1kmkεjI−ε¯jc^jkq¨j-ε¯jmku^jc^jkq˙j2+2mkq˙j∑n=1j-1q˙nε¯nu^nεjI−ε¯jc^jkuj+Fk−mkgugui.The above expression forτi is very simple to obtain than appears at first sight because the coefficients εi and ε¯i cannot both be nonzero simultaneously.The inverse dynamics can be established in the following equivalent form:(23)τi=∑k=iN∑j=1kHijkq¨j+βijkq˙2+2q˙j∑n=1j−1ζijnkq˙n+Gik+Qik.The coefficients of the generalized inertia matrixHijk, the coefficients relative to centrifugal forces βijk, the coefficients relative to Coriolis forces ζijnk, the gravity forces Gik, and the external forces Qik can be expressed in function of the system’s structural parameters independently of the generalized velocities and accelerations q˙,q¨:(24)Hijk=ε¯iε¯jIk−mkc^ikc^jk+εjmkc^ik+εiεjmkI−ε¯jmkc^jkujui,(25)βijk=ε¯jε¯iε¯ku^jΙk−mkc^iku^jc^jk−εimku^jc^jkujui,(26)ζijnk=ε¯nε¯iε¯jε¯kdualJkun−mkc^iku^nc^jk+εjmkc^iku^n+εiu^nεjmkI−ε¯jmkc^jkujui,(27)Gik=−mkgε¯ic^ikug+εiugui,(28)Qik=ε¯iMk+c^ikFk+εiFkui.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 bodySk 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:(29)Hijk=ε¯iε¯jε¯kRikIkRkj−mkc^ikRijc^jk+εjmkc^ikRij+εiRijεjmkI−ε¯jmkc^jkujui,βijk=ε¯jε¯iε¯kRiju^jRjkIkRkj−mkc^ikRiju^jc^jk−εimkRiju^jc^jkujui,ζijnk=ε¯nε¯iε¯jε¯kRikdualJkRknunRkj−mkc^ikRinu^nRnjc^jk+εjmkc^ikRinu^nRnj+εiRinu^nRnjεjmkI−ε¯jmkc^jkujui,Gik=−mkgε¯ic^ikRi0ug+εiRi0ugui,Qik=ε¯iRikMk+c^ikRikFk+εiRikFkui.


### 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:(30)τ=Mqq¨+q˙TCqq˙+Gq+Q,where τ∈RN is the generalized forces vector; q,q˙,q¨∈RN are the joint positions, velocities, and accelerations vectors; Mq∈RN×N is the generalized inertia matrix of the manipulator; C∈RN×N×N is the centrifugal/Coriolis matrix; Gq∈RN is the gravity force vector; and Qq∈RN 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 q.The generalized inertia matrixM (q) can be denoted by(31)Mq=M11M12M13…M1NM21M22M23…M2NM31M32……………………MN1MN2……MNN=∑j=1NH11j∑j=2NH12j∑j=3NH13j…∑j=NNH1Nj∑j=2NH21j∑j=2NH22j∑j=3NH23j…∑j=NNH2Nj∑j=3NH31j∑j=3NH32j……………………∑j=NNHN1j∑j=NNHN2j……∑j=NNHNNj.The centrifugal/CoriolisN×N×N matrix C can be denoted by(32)C=C1C2…CNT,where Ci is a N×N matrix denoted as(33)Ci=Bi1Ci21Ci31…CiN1Ci21Bi2Ci32…CiN2Ci31Ci32……………………CiN1CiN2……BiN=∑j=maxi,1Nβi1j∑j=maxi,2Nζi21j∑j=maxi,3Nζi31j…∑j=maxi,NNζiN1j∑j=maxi,2Nζi21j∑j=maxi,2Nβi2j∑j=maxi,3Nζi32j…∑j=maxi,NNζiN2j∑j=maxi,3Nζi31j∑j=maxi,3Nζi32j……………………∑j=maxi,NNζiN1j∑j=maxi,NNζiN2j……∑j=maxi,NNβiNj.


## 3.1. Accelerations of the BodySk in the Case of General Joints

The angular velocityωk/0 of the body Sk with respect to the base is expressed as follows:(1)ωk/0=∑j=1kε¯jωj/j−1=∑j=1kε¯jq˙juj.Its dual matrix can be expressed as(2)ω^k/0=∑j=1kε¯jω^j/j−1=∑j=1kε¯jq˙ju^j,where the term x^ denoted the dual tensor associated with a vector x=x1x2x3T, which is defined by(3)dualx=x^=0−x3x2x30−x1−x2x10.The linear velocityVojj−1 and the linear acceleration Γojj−1 of body Sj with respect to frame ℜj−1 are expressed, respectively, by(4)Vojj−1=εjq˙juj,Γojj−1=εjq¨juj.As shown in [17] and by using equations (1) and (2), the angular acceleration ak/0 of Sk relative to ℜ0 for the general joints can be derived as follows:(5)ak/0=∑j=i+1kεjq¨juj+q˙j∑n=1j−1ε¯nq˙nu^nuj.As shown in [15], the dual matrix a^k/0 can be established as follows:(6)α^k/0=∑j=1kε¯jq¨ju^j+q˙j∑n=1j−1ε¯nq˙nu^nu^j−u^ju^n.The expression of the accelerationΓck of the mass center ck can be derived as follows:(7)Γck=Γok+ak/0∧okck+ωk/0∧ωk/0∧okck,(8)Γok=∑j=1kεjΓojj−1+2ωj−1/0∧Vojj−1+∑j=1k−1ε¯jaj/0∧ojoj+1+ωj/0∧ωj/0∧ojoj+1.Consequently, including equations (8) in equation (7), we have(9)Γck=ak/0okck+ωk/0∧ωk/0∧okck+∑j=1kεjΓojj−1+2ωj−1/0∧Vojj−1+∑j=1k−1ε¯jaj/0∧ojoj+1+ωj/0∧ωj/0∧ojoj+1.By using the relation between cross product and dual tensor, which is defined byx∧y=x^⋅y=−y^⋅x, we can write(10)Γck=a^k/0+ω^k/0ω^k/0okck+∑j=1kεjΓojj−1+2ω^j−1/0Vojj−1+∑j=1k−1ε¯ja^j/0+ω^j/0ω^j/0ojoj+1.As demonstrated in [15] and taking into account (2) and (6), the vectors ∑j=1k−1ε¯ja^j/0+ω^j/0ω^j/0ojoj+1 and a^k/0+ω^k/0ω^k/0okck can be expressed by(11)∑j=1k−1ε¯ja^j/0ω^j/0ω^j/0ojoj+1=∑j=1kε¯jq¨ju^+jq˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^jojok,(12)a^k/0+ω^k/0ω^k/0okck=∑j=1kε¯jq¨ju^j+q˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^j⋅okck.By using (2) and (4), we get(13)∑j=1kεjΓojj−1+2ω^j−1/0Vojj−1=∑j=1kεjq¨juj+2q˙j∑n=1j−1ε¯nq˙nu^nuj.Lastly, based on equations (11)–(13), the acceleration Γck can be derived as follows:(14)Γck=∑j=1kεjq¨juj+2q˙j∑n=1j−1ε¯nq˙nu^nuj+ε¯jq¨ju^j+q˙j2u^ju^j+2q˙j∑n=1j−1ε¯nq˙nu^nu^jcjk=∑j=1kεjq¨jI+2q˙j∑n=1j−1ε¯nq˙nu^n−ε¯jq¨jI+q˙j2u^j+2q˙j∑n=1j−1ε¯nq˙nu^nc^jkuj=∑j=1kεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj.


## 3.2. The Dynamic Generalized Forces

The dynamic generalized forces denoted byFdΣi can be expressed by(15)FdΣi=ε¯iδΣioi+εiRdΣi=∑k=iNε¯iδSkoi+εiRdSk,∀i∈1,N,where δoiSk is the torque vector generated by the dynamic forces applied about oi to body Sk, and RdSk is the vector of the force due to the dynamic forces applied to the bodySk.(16)FdΣi=∑k=iNε¯iε¯kδSkci+mkc^ik⋅Γck+εiRdSk,where RdSk and δSkck are described by the Newton–Euler equations as follows:(17)FdΣi=∑k=iNε¯iΙk⋅αk/0+ω^k/0Ιk⋅ωk/0+mkc^ik⋅Γck+εimkΓck.As shown in [15], by considering equations (1), (2), and (5), and by using equation (14), equation (17) becomes(18)FdΣi=∑k=iNε¯iε¯k∑j=1kε¯jq¨jIk+q˙j2u^jΙk+2q˙j∑n=1j−1ε¯nq˙ndualJkunuj+mkε¯ic^ik+εiI∑j=1kεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj=∑k=iN∑j=1kε¯iε¯kε¯jq¨jIk+q˙j2u^jΙk+2q˙j∑n=1j−1ε¯nq˙ndualJkun+mkε¯ic^ik+εiIεjI−ε¯jc^jkq¨j−ε¯jq˙j2u^jc^jk+2q˙j∑n=1j−1ε¯nq˙nu^nεjI−ε¯jc^jkuj.


## 3.3. Generalized Force Due to the Environment Forces

The environmental generalized forces applied to thei-th DoF denoted by FeΣi, can be denoted by(19)FeΣi=ε¯iNoiΣi+εiReΣi=∑k=iNε¯iNoiSk+εiReSk,where NoiSk are the total external torques applied about oi on bodySk, and ReSk are the total external forces applied to bodySk.Since the external torque is applied aboutck, the vector FeΣi can be written as(20)FeΣi=∑k=iNε¯iMk+cik∧Fk−mkg⋅ug+εiFk−mkg⋅ug=∑k=iNε¯iMk+c^ikFk−mkg⋅ug+εiFk−mkg⋅ug,where Fk is the external force other than gravity applied on bodySk, and Mk is the external torque other than gravity applied on the body Sk expressed aboutck.


## 3.4. The Generalized New Formulation of Inverse Dynamics

The dynamic equations can be established by(21)τi=FdΣi+FeΣiui,∀i∈1,N.By using (18), (20), and (21), the generalized new formulation of the inverse dynamics for a serial manipulator with general joints can be derived as(22)τi=∑k=iNε¯i∑j=1kq¨jε¯jε¯kIk−mkc^ikc^jk+εjmkc^ik+q˙j2ε¯jε¯ku^jΙk−mkc^iku^jc^jk+2q˙j∑n=1j−1ε¯nε¯jε¯kdualJkun−mkc^iku^jc^jk+εjmkc^iku^nq˙nuj+Mk+c^ikFk−mkgugεi∑j=1kmkεjI−ε¯jc^jkq¨j-ε¯jmku^jc^jkq˙j2+2mkq˙j∑n=1j-1q˙nε¯nu^nεjI−ε¯jc^jkuj+Fk−mkgugui.The above expression forτi is very simple to obtain than appears at first sight because the coefficients εi and ε¯i cannot both be nonzero simultaneously.The inverse dynamics can be established in the following equivalent form:(23)τi=∑k=iN∑j=1kHijkq¨j+βijkq˙2+2q˙j∑n=1j−1ζijnkq˙n+Gik+Qik.The coefficients of the generalized inertia matrixHijk, the coefficients relative to centrifugal forces βijk, the coefficients relative to Coriolis forces ζijnk, the gravity forces Gik, and the external forces Qik can be expressed in function of the system’s structural parameters independently of the generalized velocities and accelerations q˙,q¨:(24)Hijk=ε¯iε¯jIk−mkc^ikc^jk+εjmkc^ik+εiεjmkI−ε¯jmkc^jkujui,(25)βijk=ε¯jε¯iε¯ku^jΙk−mkc^iku^jc^jk−εimku^jc^jkujui,(26)ζijnk=ε¯nε¯iε¯jε¯kdualJkun−mkc^iku^nc^jk+εjmkc^iku^n+εiu^nεjmkI−ε¯jmkc^jkujui,(27)Gik=−mkgε¯ic^ikug+εiugui,(28)Qik=ε¯iMk+c^ikFk+εiFkui.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 bodySk 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:(29)Hijk=ε¯iε¯jε¯kRikIkRkj−mkc^ikRijc^jk+εjmkc^ikRij+εiRijεjmkI−ε¯jmkc^jkujui,βijk=ε¯jε¯iε¯kRiju^jRjkIkRkj−mkc^ikRiju^jc^jk−εimkRiju^jc^jkujui,ζijnk=ε¯nε¯iε¯jε¯kRikdualJkRknunRkj−mkc^ikRinu^nRnjc^jk+εjmkc^ikRinu^nRnj+εiRinu^nRnjεjmkI−ε¯jmkc^jkujui,Gik=−mkgε¯ic^ikRi0ug+εiRi0ugui,Qik=ε¯iRikMk+c^ikRikFk+εiRikFkui.


## 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:(30)τ=Mqq¨+q˙TCqq˙+Gq+Q,where τ∈RN is the generalized forces vector; q,q˙,q¨∈RN are the joint positions, velocities, and accelerations vectors; Mq∈RN×N is the generalized inertia matrix of the manipulator; C∈RN×N×N is the centrifugal/Coriolis matrix; Gq∈RN is the gravity force vector; and Qq∈RN 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 q.The generalized inertia matrixM (q) can be denoted by(31)Mq=M11M12M13…M1NM21M22M23…M2NM31M32……………………MN1MN2……MNN=∑j=1NH11j∑j=2NH12j∑j=3NH13j…∑j=NNH1Nj∑j=2NH21j∑j=2NH22j∑j=3NH23j…∑j=NNH2Nj∑j=3NH31j∑j=3NH32j……………………∑j=NNHN1j∑j=NNHN2j……∑j=NNHNNj.The centrifugal/CoriolisN×N×N matrix C can be denoted by(32)C=C1C2…CNT,where Ci is a N×N matrix denoted as(33)Ci=Bi1Ci21Ci31…CiN1Ci21Bi2Ci32…CiN2Ci31Ci32……………………CiN1CiN2……BiN=∑j=maxi,1Nβi1j∑j=maxi,2Nζi21j∑j=maxi,3Nζi31j…∑j=maxi,NNζiN1j∑j=maxi,2Nζi21j∑j=maxi,2Nβi2j∑j=maxi,3Nζi32j…∑j=maxi,NNζiN2j∑j=maxi,3Nζi31j∑j=maxi,3Nζi32j……………………∑j=maxi,NNζiN1j∑j=maxi,NNζiN2j……∑j=maxi,NNβiNj.


## 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 (Figure2) [10].q=q1q2q3q4q5T: the generalized coordinates for revolute joints i=2,3,4, qi=θi and for prismatic joints i=1,5, qi=di.l1,l2,l3,l4,l5: the lengths of the five linkslc2, lc3, lc4: the lengths from the origins to the centers of mass of the second, third, and fourth linksm1, m2m3m4, m5: the masses of the linksI2, I3, I4: the inertia matrix of the second, third, and fourth linksFigure 2
Scheme of the 5-DOF SCARA robot.


### 4.2. Kinematics

To write the transformations for each link, the Denavit–Hartenberg method [10, 17] has been used, whose parameters are mentioned in Table 2.Table 2
Parameters of Denavit–Hartenberg.
Jointiθidiaiαi10°l1+d100°2θ20l20°3θ30l30°4θ40l4180°50°l5+d500°The general transformation matrix between thei − 1th link and the i-th link can be given by(34)Ti−1,i=cosθi−cosαisinθisinαisinθiaicosθisinθicosαicosθi−sinαicosθiaisinθi0sinαicosαidi0001.The general rotation matrix between thei − 1th link and the i-th link can be given by(35)Ri−1,i=cosθi−cosαisinθisinαisinθisinθicosαicosθi−sinαicosθi0sinαicosαi.Substitute the DH parameters from Table2 in equation (34). Then, the homogenous transformation matrix between the end-effector and the base can be expressed by(36)T=c234s2340l2c2+l3c23+l4c234s234−c2340l2s2+l3s23+l4s23400−1l1+d1−l5−d50001.From equation (36), we can get the end-effecter coordinates:(37)x=l2c2+l3c23+l4c234,y=l2s2+l3s23+l4s234,z=l1+d1−l5−d5.For simplicity, the conditionφ=θ2+θ3+θ4 is set. Where φ is known, then the inverse kinematic model is given by(38)c3=x−l4cφ2+y−l4sφ2−l22−l322l2l3,s3=±1−c32,θ3=arctan2s3,c3,s2=y−l4sφl2+l3c3−l3s3x−l4cφl22+l32+2l2l3c3,c2=x−l4cφl2+l3c3−l3s3y−l4sφl22+l32+2l2l3c3,θ2=arctan2s2,c2,θ4=φ−θ3−θ2,d5=l1+d1−l5−z,where 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(39)τ=Mqq¨+q˙TCqq˙+Gq.Here,Mq∈R5×5 is the generalized mass matrix; C∈R5×5×5 is the centrifugal/Coriolis matrix; Gq∈R5×1 is the gravitational force; q,q˙,q¨∈R5×1 are the generalized coordinates, speeds, and accelerations, respectively; τ∈R5×1 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 Figure3.Figure 3
Equations of motion for the 5-DOF SCARA manipulator.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.


## 4.1. Presentation of the Manipulator

The proposed method has been employed for the 5-DOF SCARA robot (Figure2) [10].q=q1q2q3q4q5T: the generalized coordinates for revolute joints i=2,3,4, qi=θi and for prismatic joints i=1,5, qi=di.l1,l2,l3,l4,l5: the lengths of the five linkslc2, lc3, lc4: the lengths from the origins to the centers of mass of the second, third, and fourth linksm1, m2m3m4, m5: the masses of the linksI2, I3, I4: the inertia matrix of the second, third, and fourth linksFigure 2
Scheme of the 5-DOF SCARA robot.


## 4.2. Kinematics

To write the transformations for each link, the Denavit–Hartenberg method [10, 17] has been used, whose parameters are mentioned in Table 2.Table 2
Parameters of Denavit–Hartenberg.
Jointiθidiaiαi10°l1+d100°2θ20l20°3θ30l30°4θ40l4180°50°l5+d500°The general transformation matrix between thei − 1th link and the i-th link can be given by(34)Ti−1,i=cosθi−cosαisinθisinαisinθiaicosθisinθicosαicosθi−sinαicosθiaisinθi0sinαicosαidi0001.The general rotation matrix between thei − 1th link and the i-th link can be given by(35)Ri−1,i=cosθi−cosαisinθisinαisinθisinθicosαicosθi−sinαicosθi0sinαicosαi.Substitute the DH parameters from Table2 in equation (34). Then, the homogenous transformation matrix between the end-effector and the base can be expressed by(36)T=c234s2340l2c2+l3c23+l4c234s234−c2340l2s2+l3s23+l4s23400−1l1+d1−l5−d50001.From equation (36), we can get the end-effecter coordinates:(37)x=l2c2+l3c23+l4c234,y=l2s2+l3s23+l4s234,z=l1+d1−l5−d5.For simplicity, the conditionφ=θ2+θ3+θ4 is set. Where φ is known, then the inverse kinematic model is given by(38)c3=x−l4cφ2+y−l4sφ2−l22−l322l2l3,s3=±1−c32,θ3=arctan2s3,c3,s2=y−l4sφl2+l3c3−l3s3x−l4cφl22+l32+2l2l3c3,c2=x−l4cφl2+l3c3−l3s3y−l4sφl22+l32+2l2l3c3,θ2=arctan2s2,c2,θ4=φ−θ3−θ2,d5=l1+d1−l5−z,where 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(39)τ=Mqq¨+q˙TCqq˙+Gq.Here,Mq∈R5×5 is the generalized mass matrix; C∈R5×5×5 is the centrifugal/Coriolis matrix; Gq∈R5×1 is the gravitational force; q,q˙,q¨∈R5×1 are the generalized coordinates, speeds, and accelerations, respectively; τ∈R5×1 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 Figure3.Figure 3
Equations of motion for the 5-DOF SCARA manipulator.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. Figure4 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.Figure 4
Joints profile computed by (a) the proposed method and (b) Kane’s method.
(a)(b)


### 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. Table3 shows the comparison of simulation results in terms of computational cost obtained by the aforementioned methods and our method.Table 3
Computational cost.
FormulationsEstimated CPU time (s)Kane’s method46Lagrange formulation32GIM-Christoffel’s algorithm7New formulation3The 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 Section5.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 byXd=xdydzdφd is defined as(40)xdt=0,1.cost2cost+0,2,ydt=0,1.sint2cost+0,2,zdt=0,2,φt=π3.To track the desired trajectory, the model-based control scheme chosen in the present work is the computed-torque control as shown in Figure5.Figure 5
Computed-torque control scheme for trajectory tracking control of the manipulator.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 by(41)τ=Mq¨d+Kp⋅e+Kd⋅e˙+q˙TCq˙+G,where e=qd−q∈Rn 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 as(42)q¨=M−1τ−q˙TCq˙−G,where τ 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:(43)qk+1=qk+q˙k⋅dt+12q¨k⋅dt2,q˙k+1=q˙k+q¨k⋅dt,where the integration step is chosen to be dt = 1 ms, the initial conditions of the system are given as q0=0.1,−0.8,1.9,−0.1,0.284T, q˙0=0.1,0.4,0,−0.5,0.1T, the values of the CTC control gains are selected through numerical simulation as follows: kp = 200, kd=10. 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.Table 4
Parameters of the SCARA manipulator.
ParameterValue (kg)ParameterValue (m)ParameterValue (m)ParameterValue (kgm2)m11.228l10.524lc1—Izz1—m21.023l20.2lc20.229Izz20.0058m31.023l30.2lc30.229Izz30.0058m41.023l40.2lc40.229Izz40.0058m50.5114l50.14lc5-Izz5—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, Figure6 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.Figure 6
Desired and actual trajectories in the workspace.Figure 7
Actuation torques.Figure 8
The trajectory tracking error in joint space using perfect inverse dynamic.Figure 9
The desired and actual joint trajectories.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 [18]. 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.Figure 10
Trajectory tracking error using imperfect inverse dynamic.


## 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. Figure4 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.Figure 4
Joints profile computed by (a) the proposed method and (b) Kane’s method.
(a)(b)


## 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. Table3 shows the comparison of simulation results in terms of computational cost obtained by the aforementioned methods and our method.Table 3
Computational cost.
FormulationsEstimated CPU time (s)Kane’s method46Lagrange formulation32GIM-Christoffel’s algorithm7New formulation3The 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 Section5.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 byXd=xdydzdφd is defined as(40)xdt=0,1.cost2cost+0,2,ydt=0,1.sint2cost+0,2,zdt=0,2,φt=π3.To track the desired trajectory, the model-based control scheme chosen in the present work is the computed-torque control as shown in Figure5.Figure 5
Computed-torque control scheme for trajectory tracking control of the manipulator.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 by(41)τ=Mq¨d+Kp⋅e+Kd⋅e˙+q˙TCq˙+G,where e=qd−q∈Rn 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 as(42)q¨=M−1τ−q˙TCq˙−G,where τ 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:(43)qk+1=qk+q˙k⋅dt+12q¨k⋅dt2,q˙k+1=q˙k+q¨k⋅dt,where the integration step is chosen to be dt = 1 ms, the initial conditions of the system are given as q0=0.1,−0.8,1.9,−0.1,0.284T, q˙0=0.1,0.4,0,−0.5,0.1T, the values of the CTC control gains are selected through numerical simulation as follows: kp = 200, kd=10. 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.Table 4
Parameters of the SCARA manipulator.
ParameterValue (kg)ParameterValue (m)ParameterValue (m)ParameterValue (kgm2)m11.228l10.524lc1—Izz1—m21.023l20.2lc20.229Izz20.0058m31.023l30.2lc30.229Izz30.0058m41.023l40.2lc40.229Izz40.0058m50.5114l50.14lc5-Izz5—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, Figure6 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.Figure 6
Desired and actual trajectories in the workspace.Figure 7
Actuation torques.Figure 8
The trajectory tracking error in joint space using perfect inverse dynamic.Figure 9
The desired and actual joint trajectories.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 [18]. 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.Figure 10
Trajectory tracking error using imperfect inverse dynamic.


## 6. Conclusion

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.



---

*Source: 8715161-2022-11-21.xml*