A Recursive Dynamic Modeling and Control for Dual-arm Manipulator With Elastic Joints

A recursive dynamic modeling and control is presented for the dual-arm manipulator with elastic joints carrying a common object. For decoupling the effect of elastic joints, the dynamic modeling approach is based on the classic recursive Newton-Euler method but involves high-order derivatives of motion and force variables. The high-order inverse kinematics which is needed in the motion control is presented firstly. With the classic Recursive Newton-Euler (RNEA) method, two-order dynamic model of the dual-arm robot is established, for decoupling the effect of the elastic joints, the form of four-order dynamic model is presented, meanwhile, combining with the high-order dynamic model of the carried object, the completed Dual-Arm Elastic Joints Newton-Euler Algorithm (DA-EJNEA) is established. Then the feedback linearization method is adopted to the motion control based on the DA-EJNEA. Finally, to verify the effectiveness of the proposed method, feedback linearization method based on the DA-EJNEA and the computed torque method based on the dynamic model with rigid joints are used to control the dual-arm coordinated system respectively, the simulation results illustrate the feedback linearization method based on the DA-EJNEA has an obvious advantage for trajectory tracking of the object in the operation space, it behaves reasonable potentials for the model-based control.


I. INTRODUCTION
There is an increasing trend of using dual-arm robots to do some complicated applications that are beyond the capability of a single manipulator, for example, dual-arm robots are often applied to manipulate massive and bulky objects. And some non-rigid objects are needed to handle by dual-arm robots. In the free assembly, such as the classic peg-in-hole assembly, dual-arm robots are widely used. More complicated and dexterous tasks were handled in a coordinated manner can be found in [1], [2].
Dynamic modeling of the dual-arm manipulator is the foundation of the model-based control, efficient and accurate dynamic models are essential for task execution. Most papers regarded joints of dual-arm manipulators as rigid bodies generally. The biggest difference between dual-arm coordinated operation and single-arm operation is the closed-chain constraint formed by the dual arms and the load, in general, the dynamics of the dual-arm coordinated system includes The associate editor coordinating the review of this manuscript and approving it for publication was Wen-Sheng Zhao . the manipulator and the load. In the past thirty years, many researchers regarded the joints are rigid bodies and used the Lagrangian method to establish the model of two single arms, and the model of load was established based on the force balance equation and the torque balance equation [3], [4]. The Udwadia-Kalaba equation presents a new idea of dynamic modeling of dual-arm coordinated systems, but the dynamic modeling of the unconstrained systems still depends on the Lagrangian equation which makes the model quite tedious [5].
The presence of elasticity at the joint level is not a desirable feature in industrial manipulators traditionally. Elastic joints are ubiquitous in the existing dual-arm manipulators because of the widely using of the Harmonic Drive, which can be modeled as ideal linear/torsional springs [6]. Therefore, the dynamic model of an elastic manipulator has two more orders than a rigid manipulator. Hence, if neglecting the joint elasticity, the rigid-body dynamic model may cause systematic instability when it is used in the model-based control. Consequently, the joint elasticity can limit their applications to real-world robot manipulators [7]. Recently, the designers VOLUME 8, 2020 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ of lightweight manipulators, introduced joint elasticity for physical human-robot interaction deliberately, considering the joint elasticity can limit the effect of accidents as mechanical absorbing layer [8], [9]. Therefore, it is necessary to take the joint elasticity into account when models a manipulator, especially when the robot must make fast and precise movement or carry large loads. This needs to pay more attention when models a coordinated dual-arm system because the closed chain may be damaged. In order to describe the distinct, dynamically related positions of the driving motors and of the driven links, dynamic modeling of robots with elastic joints requires the consideration of a double set of variables, the dynamic model generally takes the usual, so-called Spong model assumptions [10]. So far, very few papers have addressed the dynamic modeling of dual-arm manipulators with elastic joints. The earliest study can be found is about a coordinated system consisting of two prismatic arms and the carried object, and each arm has only one elastic joint and the linear dynamic model of the completed system can be easily established [11]. After that, some researchers addressed non-linear dynamics of robots with multiple elastic joints, they established dynamic model of each elastic manipulator with the Lagrangian method, meanwhile, modeled the carried object by the Newton-Euler method, then the completed dynamic model of multi-arm coordinated system was obtained by eliminating the same force variables in the dynamic equations [12]- [19]. Recently, some researchers considered cooperative manipulators with flexible links, they used the recursive Gibbs-Appell formulation model the manipulators and the Newton-Euler approach model the object [20]. In this paper, the joints of the robot are elastic because of using the Harmonic Drive, the links can be considered as rigid bodies.
In general, the computational load of Lagrangian model-based control for manipulators with N rigid joints is O(N 4 ), however, the computational load for manipulators with elastic joints whose Lagrangian model is obtained by differentiating the rigid model twice is O(N 6 ), which is hard to implement in the real-time robot system [7]. Although some dynamic modeling methods are more efficient, such as the recursive Lagrangian formulation [21], and the recursive Gibbs-Appell equations [20], [22], all of them assumed that the joints are rigid, obviously, these methods could not be used in this paper directly. Recent years, a recursive Newton-Euler algorithm for robots with elastic joints was proposed, the developed tools are generic, easy to use, and do not require symbolic Lagrangian modeling and customization, thus being of particular interest when the number N of elastic joints becomes large [23]. Müller modified the above algorithm with concepts from screw and Lie group theory, the Lie group approach provides a high level of compactness while ensuring the computational efficiency [24].
In this paper, inspired by the works [23], [24] on the high-order inverse dynamics of serial manipulators, the main contributions include two parts: firstly, we established the dynamic model of the coordinated dual-arm manipulator with elastic joints, meanwhile, the high-order form of the object dynamic model was considered, the completed dynamic model of the dual-arm system can be called as the Dual-Arm Elastic Joints Newton-Euler Algorithm (DA-EJNEA). The computational load of the DA-EJNEA is O(N ), obviously, it is advantageous respect to the mentioned Lagrangian method, and the DA-EJNEA does not require symbolic Lagrangian modeling and customization. Secondly, the DA-EJNEA was used to the position control with the feedback linearization method, the result further illustrated the joint flexibility could not be neglected.
The remainder of this paper is organized as follows: in Section II, kinematic relationship of the dual-arm closed chain system is analyzed, and high-order inverse kinematics is presented. In Section II, high-order dynamic model of the dual-arm system is established based on the high-order recursive Newton-Euler equations. In Section IV, the feedback linearization method is used to track the desired trajectory of the object. For validating the effectiveness of the DA-EJNEA-based feedback linearization method in the motion control, contrast simulations with the computed torque method which is based on the rigid dynamic model are presented in Section V. Finally, conclusions are presented in Section VI.

II. KINEMATICS OF THE DUAL-ARM SYSTEM
This section is the foundation of the system of dual-arm coordinated manipulation, it is necessary to give a detailed illustration. The dual-arm system under consideration in this paper consists of two arms that each arm has N (N = 7) elastic revolute joints, here we use Arm-k (k = a, b) to classify different arms. In general, the standard D-H notation can be used to describe the geometry of a manipulator, but it is not reasonable for the dual-arm robot with tree structure or closed chain structure in which some links have one more transmitting axes. This paper establishes the coordinate frames as shown in Fig. 1, which can be clearly to illustrate the homogeneous transform relationship between adjacent links [25].
Let k i (i = 1 . . . 7) be the frame of the link i of Arm-k, k e be the frame of end-effector of Arm-k, B denotes the base frame; h k i (i = 1 . . . 6) represents the distance from the origin of k i to the origin of k i+1 , h k e is the height of end-effector of Arm-k, w k represents the distance from the origin of B to the origin of k 1 , let generalized coordination q k ∈ R N be the joint position vector in the link side, θ k ∈ R N be the joint position vector in the motor side.
The two arms in the dual-arm system are the same in the construction, the only difference between them is the direction with respect to the base frame, they retain as the mirror symmetry in the initial configuration. We get the homogeneous transforms matrixes as follows: where, c k i , s k i (i = 1, . . . , N , k = a, b) represent cos(q k i ) and sin(q k i ), respectively, B T k 1 is the homogenous transforms matrix of frame k 1 with respect to the base frame B , represents the homogenous transforms matrix of k i+1 with respect to k i , 7 T k e is the homogenous transforms matrix of end-effector frame k e with respect to k 7 . The homogeneous transforms matrixes can be used for the purpose of changing the coordinate frame with respect to which a point in space is expressed and describing the relation between two coordinate frames [29], these matrixes are crucial in the process of dynamic modeling derivation.
A system of dual-arm manipulator carrying a common object is considered. And it is assumed the object grasped by the manipulator is rigid and tightly attached to each end-effector, as shown in Fig. 2.
The Euler angles ϕ k = [φ, θ, ψ] T are used to describe the orientation of the frame k e with respect to the frame B , let p k ∈ R 3×1 be the position vector of the end-effectors in the frame B , then X k = [p T k ϕ T k ] T can be collected to describe the position and orientation of the end-effectors. Similarly, we can use X c = [p T c ϕ T c ] T to describe the position and orientation of the object, where p c ∈ R 3×1 is the position vector of the object in the frame B , and ϕ c ∈ R 3×1 is the vector of angular coordinates describing the orientation of the object frame c with respect to the frame B .
If letṗ k ∈ R 3×1 and ω k ∈ R 3×1 be the translational and rotational velocities of the end-effector with respect to the base, collected by , then the following velocity relationship holds: is the geometric Jacobian matrix of Arm-k, k (ϕ k ) denotes a transformation matrix. Letṗ c ∈ R 3×1 and ω c ∈ R 3×1 be the translational and rotational velocities of the object with respect to the base,  with respect to k e , then the following velocity relationship holds: , J o is always full rank but not square, and we assume the object is grasped by the end-effectors rigidly and therefore, the following velocity relationship between the object and the end-effectors holds: High order inverse kinematics solutions for velocity, acceleration, jerk, and snap can be resolved respectively as follows [28]: where, J † = J T (JJ T ) −1 is the pseudo-inverse matrix, joint position q usually can be obtained by time integration of (10).

III. DYNAMIC MODEL AND IT'S HIGH ORDER FORM OF DUAL-ARM SYSTEM WITH ELASTIC JOINTS A. DYNAMIC MODEL OF THE DUAL-ARM ROBOT
Each arm of the dual-arm robot contains N elastic joints, each elastic joint has the same structure as shown in Fig. 3. Generally, we build the dynamic model of robot with elastic joints is based on Spong model assumptions [10].
The elastic joint torque is defined as follows: where, τ e,k ∈ R 7×1 is the vector of elastic joint torque of Arm-k, K k ∈ R 7×7 is the constant, diagonal joint stiffness matrix of Arm-k.
The dynamic model of series robots with rigid joints based on Newton-Euler equation was outlined in detail in past many years [29]. Supposing that the desired joint positions q i , joint velocitiesq i and the joint accelerationsq i are given for all links, and the force f N +1 ∈ R 3×1 and moment n N +1 ∈ R 3×1 exerted on the end-effector by the grasped object are also given. Before the procedure, the related variables are defined in Fig. 4.
Firstly, we calculate the angular velocity ω i k , the angular accelerationω i k , the linear accelerationp i k of link i k with respect to the base frame, and the linear acceleration of the mass center of link i k with respect to the base frames i k , all quantities will be expressed in the link frame for convenience, starting from the base and moving to the end-effectors, with i k = 1 . . . N , and let 0p 0 = −g, g is the gravitational field vector, the expressions can be given as: where, i k −1 R i k represents the rotational matrix of i k with respect to k i−1 , i k −1p i k denotes the position of the origin O i k in the frame k i−1 , i k −1p i k and i k −1 R i k can be extracted from (1)-(5), e z = [0, 0, 1] T .
Secondly, the total external forcef i k and the total external momentn i k acting on the center of mass of link i k can be calculated based on the Newton-Euler equations, expressed in the frame k i as follows: where, m i is the mass including the link i k and the joint i k +1, i k I i k is the inertia tensor of link i k expressed in the frame i k , i ks i k denotes the acceleration of the mass center C i k of link i k expressed in i k . Thirdly, we calculate the force f i k and the moment n i k which acting on the joint i k , expressed in the frame i k , starting from the end-effectors moving to the base, with i k = N . . . 1, the expressions can be given as: Finally, we can get the elastic torque acting on the link i k and the driving torque of motor i k as follows: where, B i k is the constant drive inertia moment of joint i k ,θ i k is the angular acceleration of joint i k in the motor side.
The recursive equations (15)- (24) are the equations of motion of Arm-k based on the Newton-Euler formulations, it can be collected as: where, τ k ∈ R N ×1 is the vector of motor torque, B k ∈ R N ×N is the constant, diagonal matrix with the drive inertia moment of Arm-k. As we know, the dynamic model based on the Lagrangian method is equivalent to the dynamic model based on the Newton-Euler method, we can easily obtain the Lagrangian model from the Newton-Euler model: where, M k (q k ) is the positive definite inertia matrix contains the link masses, the link inertias, the motor masses, and all the inertial components of the motors, n k (q k ,q k ) contains centrifugal, Coriolis and gravity, F k ∈ R 6×1 is the generalized force exerted on the object by the end-effectors of the Arm-k.

B. THE HIGH ORDER FORM OF THE INVERSE DYNAMIC OF THE DUAL-ARM ROBOT
It is obvious that the dynamic model is coupled with θ k and q k in (25) or (26), to decouple this relation, we can differentiate (14) once and twice [27]: From (27), we know thatθ k can be obtained by resolvingτ e,k , differentiating (15)- (22) once and twice, as follows: Equations (15)- (18) and (28)-(33) need to be propagated from the base to the end-effectors, equations (19)- (22) and (34)-(41) need to be propagated from the end-effectors to VOLUME 8, 2020 the base. Thenτ e,i andτ e,i can be obtained from the following: then the vector of motor torque τ k can be resolved as: the high-order dynamic model of Arm-k can be collected as: the high-order dynamic model of the dual arms is:

C. DYNAMIC MODEL AND IT'S HIGH ORDER FORM OF THE CONSTRAINED OBJECT
The constrained object by the dual-arm manipulator is rigidly grasped, the equation of motion can be obtained by the force balance equation and the moment balance equation. We build the model based on the Newton-Euler method as follows [14]: designates the generalized force exerted on the object by the end-effectors of the Arm-a and Arm-b. m o ∈ R 3×3 is the diagonal matrix with the object mass m o in the diagonal; I B = B R c I c B R T c is the object inertia matrix defined in the base frame, I c designates the object inertia matrix in the frame c , B R c is the rotational matrix of c with respect to B .
From (48),we can resolve F as follows: is the pseudo-inverse matrix, F I ∈ R 12×1 denotes the internal force, which does not contribute the motion of the object. It locates in the null space of J T o , the values of the internal force can be decided according to desired squeezing force on the object and the load distribution [25], [26].

D. THE COMPLETED HIGH-ORDER INVERSE DYNAMIC MODEL OF DUAL-ARM SYSTEM
When the dual-arm robot carries a common object, the desired general force acting on the center of mass of the object needs to be distributed to each end-effector as (49). Note that the general force F is expressed in the base frame B , the force N +1 f k N +1 and the moment N +1 n k N +1 used in the dynamic model based on Newton-Euler method are expressed in the end-effector frame k e , we need to transfer the acting force on the end-effectors to the joint space from the Cartesian space: where, e R k B is the rotation matrix of frame B with respect to frame k e of Arm-k, N +1 f k N +1 and N +1 n k N +1 are the force and moment exerted on the end-effector of Arm-k expressed in the frame of k e . Summarizing, given an initial position and velocity of the object X c,0 , V c,0 , then we can use the desired object velocity V c,d and it's first three derivativesV c,d ,V c,d , ... V c,d , to compute the desired q d and its first four derivativeṡ q d ,q d , ... q d , .... q d by employing the high-order inverse kinematics method (10)- (13). Meanwhile, the desired force N +1 f k N +1 and moment N +1 n k N +1 and their first two derivatives can be calculated from (49)-(54). Finally, the nominal inverse dynamic torque of dual-arm system can be obtained by the recursive algorithm (15)-(18), (28)-(33),(19)- (22), (34)-(41), we can collect the completed high-order inverse dynamic algorithm as follows: where, τ d ∈ R 2N ×1 is the desired torque vector which obtained by the desired object motion.

IV. THE MOTION CONTROL OF DUAL-ARM SYSTEM BY FEEDBACK LINEARIZATION
It is well known that the computed torque method for fully rigid series manipulators behaves well in the motion control. Similarly, using the high order nonlinear state feedback, the dual-arm system with elastic joints can be transformed into exactly linear and decoupled systems. The flowchart of   control based on the DA-EJNEA and feedback linearization method is shown in Fig. 5 , K 0 , . . . , K 3 ∈ R 2N are diagonal matrices, with their diagonal elements K ·,i being such that the polynomials [23]: p i (s) = s 4 + K 3,i s 3 + K 2,i s 2 + K 1,i s 1 + K 0,i , i = 1, . . . , 2N are Hurwitz, therefore, it can guarantee the system stable, and the trajectory error converges to zero.
As we know, to use the above control law, q,q,q, ... q need to be measured, usually q can be read from the encoders located in the link side, andq can resort to numerical differentiation of position measurements, butq, ... q are not easily to be measured by the sensors directly, and that multiple on-line numerical differentiation of position measurements would introduce excessive noise and/or delays in a discrete-time implementation, they can be estimated from (26), as follows: In practice, the correctness of the above acceleration and jerk is based on the precision of the elastic joint torque τ e reading from the torque sensors, and the knowledge of the model which is hard to obtain the accurate model usually. When the torque sensors are not sufficiently precise, an observer can be relied on. If the knowledge of the model is poor which will be lead to much systematic errors, a kinematic-Kalman-filter-based approach can be used [30].

V. VALIDATION OF THE DYNAMIC MODEL A. VALIDATION OF A SINGLE ARM DYNAMIC MODEL WITH ELASTIC JOINTS
The dynamic equations (25) or (26) with elastic joints need to be validated. It is the foundation to verify the effect of the DA-EJNEA next. We established the dynamic model with  two elastic joints as (26) , it is easily to extend the model with N-degree elastic joints, meanwhile, we built the same model in Adams. To verify the correctness of the dynamic model with elastic joints, the key problem is coinciding the motion output of the rigid-flexible coupling model with the physical model in Adams.
The kinematic parameters of the model are h k 1 = 0.11m, h k 2 = 0.15m, and dynamic parameters are list in Tab. I. Initial positions and velocities of each link and motor are given to zeros, the imposed torques for motors of joint 1 and joint 2 are set as −50 cos(2πt), with the sampling rate of 1ms. where, ( i s ix , i s iy , i s iz ) represent the positions of the center of mass of the links expressed in link frames. All the drive inertia moments B i of each joints are assumed 3 Kg.m 2 , and all the joint stiffnesses K i have been assumed 1000 Nm/rad, the gravity term and external force are neglected there.
The resulting motions of each link are shown in Fig. 6, the errors of joints position and velocity are shown in Fig. 7, the positions and velocities of our model coincides well with To validate the advantage of the feedback linearization based on the DA-EJNEA in the motion control, we compared the trajectory tracking precision of the object with the computed torque method based on the rigid inverse dynamics. All the simulations were executed in Matlab2018. The kinematic parameters of dual-arm coordinated system read from the 3D model are: h k 1 = 0.11m, h k 2 = 0.15m, h k 3 = 0.1m, h k 4 = 0.15m, h k 5 = 0.1m, h k 6 = 0.17m, h k e = 0.2m and the dynamic parameters including each links and end-effectors and the object are listed in Tab. II. Because all the dynamic parameters of each arm of the dual-arm manipulator are the same, we do not classify Arm-k in Tab. II.
All the drive inertia moments B i of each joints are assumed 3 Kgm 2 , and all the joint stiffnesses K i are assumed 1000 Nm/rad. The object is a cube with 0.2m sides, the center of mass of the cube locates in the center of geometry, the gravity term is g = [00 -9.8] T .
The initial joint velocities are assumed to be zeros, and the initial joint angle of each arm and distance of each arm to the base frame are assumed to be: q a t0 = q b t0 = [π/2; −π/6; 0; −5π/12; 0; π/4; 0] w a = w b = 0.5m Given the object a velocity trajectory in the Z direction of the operation space asż d = −0.3 sin(2πt), with the sampling rate of 1ms, we obtained the positions and velocities of the object in Z direction as shown in Fig. 8. The position and velocity trajectory tracking precision in Z direction are shown in Fig. 9 and Fig. 10. The results illustrated that the proposed DA-EJNEA with feedback linearization has better tracking performances.

VI. CONCLUSION
In this paper, the dynamic modeling and control method of the system of dual-arm manipulator with elastic joints cooperated carrying a common object was presented. The proposed DA-EJNEA takes the manipulator high-order dynamics and the object high-order dynamics into consideration, which is based on a differential extension of the classic recursive Newton-Euler algorithm. The DA-EJNEA is efficient, and easily to extend the model with N degrees, especially convenient for the redundant robot. The requirement to implement of the DA-EJNEA is that the arbitrary two terms of link positions, motor positions and elastic torques are measurable, it is not a difficulty in modern industrial robots. The computed torque method for rigid inverse dynamics and the feedback linearization method for rigid-elastic coupling inverse dynamics were executed to control the dual-arm system, the simulation results illustrate the effectiveness of the proposed DA-EJNEA in the position control. However, the DA-EJNEA requires the exact model parameters of the dual-arm system in theory. Future work will address the passive-based control for the case of model parameters uncertainty, meanwhile, the experimental validations will be added for control algorithm approval in the future.