Logarithmic complexity dynamics formulation for computed torque control of articulated multibody systems

Mechanism and Machine Theory 116 (2017) 481–500

Contents lists available at ScienceDirect

Mechanism and Machine Theory journal homepage: www.elsevier.com/locate/mechmachtheory

Research paper

Logarithmic complexity dynamics formulation for computed torque control of articulated multibody systems Cameron Kingsley, Mohammad Poursina∗, Sahand Sabet, Arman Dabiri Department of Aerospace and Mechanical Engineering, University of Arizona, Tucson, AZ 85721, USA

a r t i c l e

i n f o

Article history: Received 14 September 2016 Revised 1 May 2017 Accepted 2 May 2017 Available online 29 June 2017

a b s t r a c t Divide-and-Conquer Algorithm is extended and integrated with the method of Computed Torque Control (CTC) to model and control fully-actuated multibody systems. CTC uses inverse and forward dynamics to control a multibody problem. This may impose unnecessary computational cost on the simulation if the corresponding equations are not formed and solved wisely. Additionally, this technique mainly uses the dynamic equations formulated based on the minimum number of joint coordinates. Herein, the Generalized Divide-andConquer Algorithm (GDCA) with the capability of accommodating both spatial and generalized forces is used to efficiently form the forward dynamic equations. Furthermore, a new mathematical formulation is generated to adjust the inverse-GDCA containing a redundant set of spatial and generalized coordinates with the joint-coordinate-based inverse dynamics utilized in CTC. Both GDCA and iGDCA apply a series of assembly and disassembly passes to form and solve the forward and inverse dynamic equations without generating the mass and Jacobian matrices of the entire system. This paper also addresses the challenges which may appear in controlling fully-actuated multibody systems with kinematic loops. The GDCA-based CTC is then equipped with popular control techniques such as PID, pole-placement-based state feedback, and linear quadratic regulator, and used to control selected multibody systems. © 2017 Elsevier Ltd. All rights reserved.

1. Introduction Multibody dynamics plays a key role in the modeling, simulation, and control of many systems ranging from macroscale robots [1–3] to nano-scale articulated molecular problems [4–8]. The dynamics of these systems is inherently highly nonlinear which makes controlling them along a desired trajectory troublesome and difficult. The method of computed torque control (CTC) is an application of feedback linearization of a nonlinear system in which the necessary forces to drive it along a desired reference trajectory are computed based on the tracking error and inverse dynamics [9]. As such, unlike other methods which attempt to linearize all or parts of the system dynamics [10–12], in this scheme system’s nonlinearities are considered in the design of the controller. CTC is traditionally used in the context of robotics to control robotic manipulators [1,13–15,16]. While most robotic systems may be considered as multibody problems, the application of CTC in this area is generally limited to the traditional Lagrange method to derive the equations of motion and the inverse dynamics. This may be due to the fact that most robotic systems are extremely simple multibody problems, and do not ∗

Corresponding author. E-mail addresses: [email protected] (C. Kingsley), [email protected] (M. Poursina), [email protected] (S. Sabet), [email protected] (A. Dabiri). http://dx.doi.org/10.1016/j.mechmachtheory.2017.05.004 0094-114X/© 2017 Elsevier Ltd. All rights reserved.

482

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

suffer from the computational burden that large multibody simulations might experience. In general, a CTC simulation of a multibody system has three main components: 1) A feedback loop such as a stabilizing PD, PID, or any other type of controller 2) An inverse dynamic solver to provide the “generalized driving forces” [16] to the multibody problem 3) A forward dynamic solver to integrate the equations of motion and move the system forward in time The last two components may impose unnecessary computational cost on the closed-loop control of large multibody systems if the corresponding dynamic equations are not formed and solved wisely. For instance, following traditional techniques such as Lagrange method or Newton-Euler approach, the inverse dynamics may contain O(n3 )-complexity quantities [2], where n denotes the number of degrees of freedom of the system. Furthermore, computational complexity of the forward dynamic simulation of an unconstrained problem increases as a cubic function of the number of degrees of freedom of the system if one utilizes the traditional methods which form and use Jacobian and mass matrices of the entire system. These computational hurdles prohibit the real time simulation and control of multibody problems as the complexity of the system in terms of the number of degrees of freedom and kinematic constraints increases. Examples of such systems can be found in hyper redundant and human robots [1,17]. Additionally, the methods used to model the forward dynamics and the inverse dynamics should be very similar in the structure, such that the simulation tool uses the shared information of the system properties in both inverse and forward dynamics. Finally, numerical tools used for such simulations should easily lend themselves to the parallel computing framework. High performance algorithms have been developed to linearly scale the forward dynamic simulation turnaround time with the increase in the system’s degrees of freedom [2,18–28]. A discussion on the underlying similarities between these algorithms has been provided in [29]. New methods have also been developed to efficiently exploit parallel computing resources [30–38]. The first techniques with O(log n) complexity for the forward dynamic simulation of manipulators using O(n2 ) and O(n) processors, are respectively presented in [39] and [40]. For the inverse dynamic computations, the recursive Newton-Euler formulation has been developed to express the equations of motion of the system in terms of recurrence relations [2]. This algorithm is a very effective, and frequently used, method for control scheme implementation in real time, as it has computational complexity of O(n) [2,9,41–43]. The divide-and-conquer algorithm (DCA) developed by Featherstone [44,45] is one of the more efficient parallel multibody algorithms capable of achieving O(log n) performance with O(n) processors for fairly general system topologies. This technique has been extended and used for the forward dynamic simulation and analysis of multibody systems with rigid and deformable components in various applications [46–57]. A detailed investigation on the DCA-based techniques has been provided in [58,59]. In this paper, the Generalized-DCA [52] is extended and integrated with the method of CTC to model and control fully-actuated large multibody systems in a more time effective manner. GDCA uses an efficient mapping which properly transforms each given generalized driving force to the associated dynamically equivalent spatial driving force. This technique builds the equations of motion in the Newton-Euler framework along with a redundant set of spatial and generalized coordinates. However, in many applications, CTC equations are formed based on the generalized (joint) coordinates and ordinary differential equations [60]. Therefore, a new mathematical formulation is developed to appropriately modify inverse Generalized-Divide-and-Conquer Algorithm (iGDCA) including both spatial and generalized coordinates with the inverse dynamics used in the CTC scheme. iGDCA efficiently computes the generalized driving forces in conjunction with the CTC method to reduce the cost of control force/torque calculations. In both inverse and forward dynamics, the formation of the mass and Jacobian matrices of the entire system is avoided. Therefore, with O(n) complexity in serial implementations, and O(log n) complexity in parallel implementations (in the existence of O(n) processors), the presented formulation improves the efficiency of the modeling and control of large multibody problems. This paper also presents the challenges, in addition to the appropriate solutions, which may appear in controlling fully-actuated multibody systems with closed kinematic chains. The developed GDCA-based CTC is then combined with several control schemes to control fully-actuated multibody systems. The rest of the paper is organized as follows. In Section 2, a brief review of the method of CTC is provided. This is followed by presenting the forward dynamic formulation in the GDCA scheme in Section 3. The main focus of Section 4 is to adjust iGDCA containing a redundant set of spatial and generalized coordinates with CTC. The challenges associated with controlling fully-actuated multibody systems with kinematic loops are addressed in Section 5. The presented GDCA-based CTC is integrated with popular control schemes in Section 6, and then applied to control selected multibody problems in Section 7. Finally, conclusions are made. 2. Computed torque control method Herein, the overview of the classical formulation of the computed torque control algorithm is presented. CTC is a method of feedback linearization utilizing the inverse dynamics of a system to calculate external control torques and forces necessary to drive it along a desired trajectory. This technique avoids the issues associated with controlling nonlinear problems by controlling the system’s tracking error, which has linear dynamics. To illustrate this method, we focus on the equations of motion of a fully-actuated multibody system using the minimum number of generalized coordinates defined as joint coordinates. In this scheme, the governing equations are expressed as

M (q )q¨ + N (q, q˙ ) + f˜d = f˜,

(1)

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

483

Fig. 1. Different components of CTC to control multibody systems.

where q is the column matrix of joint coordinates, M(q) represents the mass matrix of the entire multibody problem, and f˜ denotes the column matrix of generalized control forces at the joints of the systems. In this equation, f˜d represents the column matrix of disturbance which is essentially the bias to the driving forces and torques applied to the system by its actuators. All other generalized forces due to non-control internal and external loads such as gravity, springs, dampers, as well as centripetal and Coriolis loads are contained in N (q, q˙ ). For this system, the tracking error is defined as the difference between a desired joint coordinate trajectory qd (t), and state feedback of the actual joint coordinate trajectory q(t),

e(t ) = qd (t ) − q(t ).

(2)

The first and second derivative of Eq. (2) are evaluated to obtain the tracking error in the system’s joint speeds and accelerations as

e˙ (t ) = q˙ d (t ) − q˙ (t ),

(3)

e¨ (t ) = q¨ d (t ) − q¨ (t ).

(4)

By solving Eq. (1) for q¨ , and substituting the results into the second derivative of the tracking error at the acceleration level, Eq. (4) is represented in terms of the desired joint acceleration and the system dynamics as

e¨ (t ) = q¨ d (t ) + M−1 (q )(N (q, q˙ ) − f˜ + f˜d ).

(5)

The tracking error at the acceleration level can be thought of as a combination of terms from the control input, which is driven by the system dynamics as

w = q¨ d (t ) + M−1 (q )(N (q, q˙ ) − f˜).

(6)

Using Eqs. (3)–(6), the tracking error at the velocity and acceleration levels can be represented as a linear combination of the tracking error at the position and velocity levels, as well as the control input [9],





e˙ (t ) e¨ (t )



=



0

I

0

0



e(t ) e˙ (t )

 

+

0 I

w+

  0 I

v,

(7)

where v = M−1 (q ) f˜d . As depicted in Fig. (1), the entire closed-loop system contains three major components: 1) the inverse dynamics to determine the generalized forces at the joints of the multibody system which drive it along the desired trajectory, 2) the forward dynamics to move the system’s states forward in time, and 3) the control law to provide a stable closed-loop system, while capturing system’s nonlinearities [9]. Each one of these constituent parts is associated with a limiting factor. The computational cost of the first two components is significantly important for the real time control of a multibody problem, while the last part should be chosen to guarantee the stability and the desired performance of the feedback control system. It should be mentioned that it is possible to adopt the CTC method for spatial (natural) coordinate-based system in which the equations of motion are described by a set of differential-algebraic equations (DAEs). In this situation, the solution requires additional projections [61] that results in an ODE [60]. However, complexity of the simulation may significantly increase due to these repeated projections [60]. Furthermore, in the presence of uncertainty in the system model, performance of CTC can be good if the controller gains are selected large enough [9]. Other techniques such as a variable structure controller CTC [62], neural network-based controller [63], fuzzy controller [64], and adaptive fuzzy controller [65] have been developed to improve the robustness of CTC. Finally, the method of polynomial chaos expansion [66] has been integrated with CTC in [67,68] to handle uncertainties in the dynamics and control of multibody systems. Based on the CTC formulation, it is also important to distinguish between the inverse dynamics used in regular multibody problems and the one used in CTC-based control systems. In the inverse dynamics used in regular multibody problems, it is desired to find the actuator/constraint loads for a desired/prescribed position, velocity, and acceleration. This mainly works

484

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

̃ ̃

−

−1

−1 −1

̃

2 1

−1

− 1: −1

Assembly

1

−1

2

−

+1

̃

−1

+1

̃

̃ 1

−1

−1

−1

2

−1

−

+1

̃

+1

Fig. 2. Using GDCA to assemble two consecutive bodies in forward dynamic analysis.

̃

2

Body Level ̃

1

̃

̃ ̃

̃

2

1

̃

3

̃ ̃

̃

6

5

4

3

2

̃ ̃

̃

̃

Body Level

5

4

3

1

̃ Root Level ̃

̃

2

1

̃

̃

3

̃

6

6

5

1

̃

4

̃

̃ 5

̃

1

̃

̃

2

1

4

̃ ̃

3

̃

3

6

̃

5

̃

5

Disassembly

̃

̃

Disassembly

Assembly

̃

4

Assembly

̃

5

6

Root Level ̃

1

Fig. 3. iGDCA uses CTC to compute generalized driving forces. These actuating forces and torques are then fed into GDCA to perform the forward dynamic simulation.

when the system starts off with the initial conditions lying exactly on the desired trajectory at the position, velocity, and acceleration levels. However, control problems experience a different scenario in which it is desired to follow a trajectory while the initial condition is not exactly on the desired path. Also, in these problems, the coordinates, velocities, and accelerations may be distracted from their desired values due to disturbance. A closer look into Eq. (6) indicates that CTC works based on the current values of the joints’ positions and velocities and the desired values of the joints’ accelerations. In other words, the inverse dynamics used in the CTC method computes the actuating forces and torques based on the measured joints’ coordinates and speeds of the system, as well as the desired joints’ accelerations. 3. Forward dynamics in the generalized divide-and-conquer scheme The generalized divide-and-conquer algorithm (GDCA) [6,52], as an extension of the DCA [44,45], is a highly parallelizable method which provides a suitable framework to form and solve the forward dynamic equations of motion in CTC applications. This technique uses a series of recursive assembly and disassembly sweeps to form and solve the equations of motion. In the assembly pass, adjacent articulated bodies/subsystems are recursively combined to generate larger encompassing subsystems as shown in Fig. 2. In this process which starts at the body (leaf) level of the binary tree (see Fig. 3(a)), the handle equations of each body are first formed. Essentially, a handle is defined as a point on the body through which it has an interaction with the environment. For instance, consider body k shown in Fig. 2 with two handles H1k and H2k located at the joints connecting it to bodies k − 1 and k + 1, respectively. Due to the existence of the kinematic joints between this body and its child and parent bodies, it experiences unknown spatial constraint forces Fkic (i = 1, 2 ) at the ith handles. Also assume that f˜k and f˜k+1 indicate the column matrices of known generalized driving forces associated with the motion of body k relative to bodies k − 1 and k + 1, respectively. In other words, f˜k is a set of generalized driving forces calculated by CTC and then applied to body k by body k − 1 through the connecting joint Jk . Similarly, f˜k+1 is a set of necessary generalized driving forces that should be applied from body k to body k + 1 through the connecting joint J k+1 . By using the concept of “dynamically equivalent systems”, it has been shown in [52] that the spatial forces corresponding to this type

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

485

of generalized forces lie in the normalized subspace describing the relative motion of the consecutive bodies at the corresponding connecting joint, called joint free-motion map [69]. The joint free-motion map and its orthogonal complement are explained in Appendix A. For a fully-actuated multibody system, these forces lie in the entire normalized space of the joint k k+1 free-motion map, i.e., PJ and PJ , respectively [52]. Applying these control loads to the system, motion of an arbitrary body k is described in terms of the two handle equations as

Ak1 =

k k+1 k k k η11 (Fk1c + PJ f˜k ) + η12 (Fk2c − PJ f˜k+1 ) + φ13 ,

(8)

Ak2 =

k k+1 k k k η21 (Fk1c + PJ f˜k ) + η22 (Fk2c − PJ f˜k+1 ) + φ23 ,

(9)

Aki (i

where = 1, 2 ) represent the unknown spatial accelerations of the ith handle of body k with respect to the inertial reference frame. The terms ηikj (i, j = 1, 2 ) are associated with the known inertia properties of the body, and φik3 (i = 1, 2 ) represent the column matrices corresponding to the known non-control spatial applied forces, as well as centripetal and Coriolis terms. As it is observed, in the GDCA scheme, the generalized driving forces are distinguished from the constraint loads due to the kinematic constraints. Therefore, this formulation creates a convenient framework to use the generalized driving forces calculated by CTC to form and solve the forward dynamic equations as a part of the control process. Since f˜k , f˜k+1 , and φik3 (i = 1, 2 ) are known quantities, they can be consolidated in new terms. As such, the handle equations of body k are revised as

Ak1 =

k k k k k η11 F1c + η12 F2c + η13 ,

(10)

Ak2 =

k k k k k η21 F1c + η22 F2c + η23 ,

(11)

where k k J ˜k k η13 = η11 P f − η12 PJ k

k+1

k f˜k+1 + φ13 ,

(12)

k k+1 k k k k η23 = η21 PJ f˜k − η22 PJ f˜k+1 + φ23 .

Because of the existence of the kinematic joint level at this connecting joint is given by

(13) Jk

between bodies k − 1 and k, the kinematic constraint at the acceleration

k k k k Ak1 = Ak2−1 + P˙ J q˙ J + PJ q¨ J ,

k q˙ J

(14) k q¨ J

k DJ

where represent the known joint speed, and represent the unknown joint acceleration. Defining matrix as the orthogonal complement of the joint free-motion map at Jk , this constraint equation is modified by the elimination of the unknown joint acceleration as

(DJ )T (Ak1 − Ak2−1 − P˙ J q˙ J ) = 0. k

k

k

(15)

By using the constraint Eq. (15), and following the appropriate mathematical operations, one can eliminate the unknown spatial constraint forces and accelerations associated with the common handles of bodies k − 1 and k [46,52]. Therefore, the handle equations of these consecutive bodies are combined, resulting in the following handle equations

Ak1−1 = Ak2 =

k−1:k k−1 k−1:k k k−1:k η11 F1c + η12 F2c + η13 ,

k−1:k k−1 k−1:k k k−1:k η21 F1c + η22 F2c + η23 ,

(16)

(17)

where ηikj−1:k are known intermediate terms [52]. Using these equations, the forward dynamics of the entire assembly k − 1 : k, as shown in Fig. 2, is presented by the two handle equations. These equations essentially express the spatial accelerations of the terminal handles in terms of the constraint loads due to the existence of the kinematic constraints at these handles. Further, as it is shown in this figure, the generalized driving force f˜k is embedded in the assembly k − 1 : k since it is a known quantity which has already been calculated by CTC, and fed into the forward dynamic tool. This process, as seen in Fig. 3(a), continues to assemble all bodies/subassemblies. The generalized driving forces are known quantities calculated by an inverse dynamic solver. Therefore, as it is observed in this figure, they are embedded in the equations governing the forward dynamics of the assemblies. The assembly pass stops when the forward dynamics of the entire system is shown by a set of handle equations in which the spatial accelerations of the terminal handles of the entire system are expressed in terms of the spatial constraint loads at these handles. The disassembly pass will then start by solving these handle equations for the spatial accelerations of the handles and the spatial constraint loads that these handles experience. This process continues such that all constraint loads and spatial accelerations are calculated at the end of the disassembly pass. The detail of the assembly and disassembly sweeps in the forward dynamic analysis is presented in [46,52].

486

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

4. Inverse GDCA for computed torque control applications 4.1. CTC-based kinematic constraint between adjacent bodies The equations derived in Section 2 to formulate the CTC method are associated with the situation in which the dynamics of the system is formulated in terms of the minimum number of generalized (joint) coordinates. However, GDCA uses a redundant set of both spatial and relative (joint) coordinates to express the equations of motion. Therefore, the question is raised on how to formulate iGDCA to perform the inverse dynamics in the CTC scheme, while these techniques use different sets of coordinates to formulate and describe system’s motion. Referring back to Section 2, the control input w is chosen to stabilize Eq. (7) such that the system obtains the desired performance. As defined in Eq. (6), this quantity is a known function of system’s forward dynamics and desired trajectory. As such, for a system with independent coordinates defined as joint variables, Eq. (6) can be solved for the generalized driving forces as

f˜ = M (q )(q¨ d − w ) + N (q, q˙ ).

(18)

Since the array of control load f˜ is calculated based on a properly chosen control input w, the resulting forces and torques will drive the multibody problem along the desired trajectory. Now consider the situation in which one is interested in solving the inverse dynamic problem associated with Eq. (1) when a set of joint positions, velocities, and accelerations are given. Comparing Eqs. (1) and (18), one can observe that finding the generalized driving forces in the CTC scheme is essentially equivalent to solving the inverse dynamics of a regular multibody system, while replacing the joint acceleration q¨ by q¨ d − w [9]. On the other hand, in the GDCA formulation, joint accelerations only appear in the kinematic constraint equations at the connecting joints, not the handle equations at the body level (see Fig. 3(a)). As such, we conclude that in order to formulate iGDCA in the CTC scheme, the kinematic constraint at the acceleration level associated with the connecting joint Jk between bodies k − 1 and k should be modified from its original form in Eq. (14) as k

Ak1 = Ak2−1 + P˙ J q˙ J + PJ (q¨ Jd − wJ ). k

k

k

k

(19)

Jk

In this equation, q¨ d is the desired joint acceleration evaluated based on the desired joint trajectory. Furthermore, comparing the forward dynamic problem in Eq. (1) and the inverse dynamics problem in Eq. (18), one observes that in the CTC scheme, both forward and inverse dynamic use the same coordinates and speeds at each time step. Therefore, as it is indicated in k

k

J Eq. (19), the known quantity q˙ J is used instead of q˙ d to represent the joint speed.

4.2. Assembly pass for the inverse dynamic formulation In order to perform the inverse dynamics in the context of CTC, one starts with the handle equations of the bodies. It should be reminded that the main purpose of the inverse dynamics is to find the generalized driving forces. As such, unlike the forward dynamic formulation, in this analysis, both constraint loads due to the kinematic constraints and generalized driving forces at each handle are unknown. By defining the following total unknown loads at the terminal handles of an arbitrary body k k F¯ k1 = Fk1c + PJ f˜k ,

(20)

k+1 F¯ k2 = Fk2c − PJ f˜k+1 ,

(21)

the two handle equations associated with the inverse dynamics of this body are revised as

Ak1 =

k ¯k k ¯k k ψ11 , F1 + ψ12 F2 + ψ13

(22)

Ak2 =

k ¯k k ¯k k ψ21 . F1 + ψ22 F2 + ψ23

(23)

Similarly, the two handle equations for the inverse dynamics of the parent body k − 1 are expressed as

Ak1−1 =

k−1 ¯ k−1 k−1 ¯ k−1 k−1 F1 + ψ12 F2 + ψ13 ψ11 ,

Ak2−1 =

k−1 ¯ k−1 k−1 ¯ k−1 k−1 F1 + ψ22 F2 + ψ23 ψ21 .

(24)

(25)

Unlike the kinematic constraint in Eq. (14) in forward dynamic analysis, the term

Jk q¨ d

k − wJ

in Eq. (19) used in the inverse k

dynamics is a known quantity. Therefore, there is no need to pre-multiply this equation by (DJ )T to eliminate this term in the assembly pass. By substituting Eqs. (22) and (25) into Eq. (19), the total unknown load at the connecting joint Jk can be represented in terms of the entire unknown loads at the handles of bodies k − 1 and k as k

k ¯k k ¯k k k−1 ¯ k−1 k−1 ¯ k−1 k−1 ψ11 = ψ21 + P˙ J q˙ J + PJ (q¨ Jd − wJ ). F1 + ψ12 F2 + ψ13 F1 + ψ22 F2 + ψ23 k

k

k

k

(26)

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

487

Using the property F¯ k1 = −F¯ k2−1 , Eq. (26) can be solved for the entire unknown load at the common handles as a function of the entire unknown loads at the inward handle of body k − 1 and the outward handle of body k as

F¯ k1 = −F¯ k2−1 = W T F¯ k1−1 − Z T F¯ k2 + X Y,

(27)

where k k−1 −1 X = (ψ11 + ψ22 ) ,

(28) k

Y =

k−1 k ψ23 − ψ13 + P˙ J q˙ J + PJ (q¨ Jd − wJ ),

(29)

W =

k−1 T ψ12 X ,

(30)

Z=

k

k

k

k

k ψ21 XT .

(31)

Eq. (27) can then be substituted into Eqs. (23) and (24) to eliminate the interior total unknown loads between bodies k − 1 and k. As such, the inverse dynamics of the combined k − 1 : k assembled system can be expressed in terms of the unknown spatial accelerations and loads of the terminal handles H1k−1 an H2k as

Ak1−1 = Ak2 =

k−1:k ¯ k−1 k−1:k ¯ k k−1:k ψ11 , F1 + ψ12 F2 + ψ13

k−1:k ¯ k−1 k−1:k ¯ k k−1:k F1 + ψ22 F2 + ψ23 ψ21 ,

(32)

(33)

where k−1:k k−1 k−1 ψ11 = ψ11 − ψ12 WT,

(34)

k−1:k k−1 T ψ12 = ψ12 Z ,

(35)

k−1:k k−1 k−1 ψ13 = ψ13 − ψ12 X Y,

(36)

k−1:k k ψ21 = ψ21 WT,

(37)

k−1:k k k T ψ22 = ψ22 − ψ21 Z

(38)

k−1:k k k ψ23 = ψ23 + ψ21 X Y.

(39)

This process continues by assembling the consecutive bodies/assemblies to generate new subassemblies as shown in Fig. 3(b). As it is observed, unlike the assembly pass of GDCA forward dynamics in Fig. 3(a), the unknown generalized driving forces are eliminated in the assembly pass of iGDCA, thus not shown in Fig. 3(b). Finally, by performing the hierarchic assembly of all bodies/assemblies, the inverse dynamics of the entire system with nb bodies (see Fig. 3(b)) can be expressed in terms of the handle equations associated with the terminal handles of the system as [70]

A11 = n

A2b =

1:nb ¯ 1 1:n 1:nb ψ11 F1 + ψ12 b F¯ nb 2 + ψ13 ,

1:nb ¯ 1 1:n 1:nb ψ21 F1 + ψ22 b F¯ nb 2 + ψ23 .

(40)

(41)

It should be mentioned that in each time step, both inverse and forward dynamics use the same joint coordinates and speeds. Therefore, only at the body level (see Fig. 3(a)), the coefficients used for the handle equations of an arbitrary body k in the forward dynamics (Eqs. (10) and (11)) are the same as those used for the handle equations of the same body in the inverse dynamics (Eqs. (22) and (23)), i.e.,

ψikj = ηikj ,

i, j = 1, 2,

(42)

ψik3 = φik3 ,

i = 1, 2.

(43)

Knowing this property, one should avoid reevaluating these coefficients at the body level in the forward dynamic component of the CTC method.

488

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

4.3. Disassembly pass for the inverse dynamics Eqs. (40) and (41) represent the assembled inverse dynamics of a fully-actuated mulitbody system. Boundary conditions must then be applied to solve for the unknown spatial loads and/or accelerations. In this section, it is assumed that the n multibody system is an open-chain problem in which H11 is connected to the ground via a kinematic joint, while H2 b is unconstrained (see Fig. 3(b)). A detailed investigation on systems with closed kinematic chains will be covered in Section 5. For this problem, the spatial acceleration at H11 is known from the CTC method. Therefore, the two handle equations governing the inverse dynamics of the entire system are revised as 1

1 1 1 1 P˙ J q˙ J + PJ (q¨ Jd − wJ ) =

n

A2b =

1:nb ¯ 1 1:n ψ11 F1 + ψ13 b ,

(44)

1:nb ¯ 1 1:n F1 + ψ23 b . ψ21

(45)

Eq. (44) is then solved for the entire load at the inward handle

H11

as

1

1 1 1 1 1:n 1:n F¯ 11 = (ψ11 b )−1 (P˙ J q˙ J + PJ (q¨ Jd − wJ ) − ψ13 b ).

(46) n

Therefore, knowing the value of F¯ 11 , one can solve Eq. (45) for the spatial acceleration of H2 b . Evaluating the total spatial load and acceleration of the terminal handles of the entire system, one can solve the twohandle equations of the inverse dynamics of the constituent subassemblies for the spatial accelerations and loads at the common joint. This process is repeated in a hierarchic disassembly of the binary tree where the known information on the terminal handles of the assemblies are used to solve the inverse dynamic equations of the constituent subassemblies as shown in Fig. 3(b). For instance, consider an arbitrary assembly k − 1 : k for which the terminal loads and spatial accelerations are known. Since F¯ k1−1 and F¯ k2 have already been calculated, the entire load at the common handle Jk is calculated using Eq. (27). This is followed by the calculation of the spatial accelerations at the common handles by using the handle Eqs. (22) and (25). At the end of the disassembly pass, all spatial accelerations and loads at the handles are known. However, the main goal of the inverse dynamic solver is to compute the generalized driving forces to be fed into the forward dynamic simulation. k k+1 In other words, it is aimed to find the value of PJ f˜k and −PJ f˜k+1 , and substitute them into the forward dynamic GDCA (Eqs. (8) and (9)) as actuating forces and torques based on CTC law. Consider the total spatial loads F¯ k1 and F¯ k2 which have already been evaluated in the disassembly pass as known quantities χ1k and χ2k , respectively: k F¯ k1 = Fk1c + PJ f˜k = χ1k ,

(47)

k+1 F¯ k2 = Fk2c − PJ f˜k+1 = χ2k .

(48)

We know that the spatial loads at the handles due to the kinematic constraints lie in the space of the orthogonal complement of the associated joint free-motion map. Therefore, these equations are revised as k k DJ f1kc + PJ f˜k =

DJ where

k+1

f1kc

f2kc − PJ and

k+1

f2kc

χ1k ,

f˜k+1 =

(49)

χ2k ,

(50)

are the lists of numbers associated with the constraint loads due to the kinematic constraints [48]. k

Premultiplying Eq. (49) by (PJ )T , and Eq. (50) by (PJ arrive at the following relations

(PJ )T PJ f˜k = (PJ )T χ1k , k

k

− ( PJ

k+1

)T PJ

k

k+1

)T , and using the orthogonality between D and P spaces, one can (51)

k+1 f˜k+1 = (PJ )T χ2k .

k

k+1

(52) k

k

Since PJ is a normalized matrix, the term (PJ )T PJ associated with any arbitrary joint becomes an identity matrix. Therefore, the dynamically equivalent spatial driving forces at handles H1k and H2k which must be fed in to the handle equations of body k for the forward dynamic simulations (Eqs. (8) and (9)) are computed as k k k PJ f˜k = PJ (PJ )T χ1k ,

−PJ

k+1

k+1 k+1 f˜k+1 = PJ (PJ )T χ2k .

(53)

(54)

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

Region 1

2

1

3

Level 2

Level 5

̃

6

̃

̃

4

8

7

6

4-5-6-7

8

6-7

̃

8

1-2-3-4-5-6-7

Disassembly

Level 4

̃

4-5

1-2-3

6

5

4

1-2 Assembly

Level 3

Independent

Independent 4

̃

Region 4

Region 3

Region 2 Independent

Level 1

489

1-2-3-4-5-6-7-8 Fig. 4. Efficient structure to perform assembly-disassembly sweeps for inverse dynamics of spatial closed-chain systems.

5. GDCA-based computed torque control of fully-actuated closed-chain systems To control a fully-actuated multibody system with kinematic loops, desired motion can only be applied to a specific number of joint coordinates which is essentially equal to the number of degrees of freedom of the uncontrolled system. As such, we categorized the joints of the closed-chain system into two types: independent and dependent. The generalized driving forces are only applied to the independent joints. By applying the generalized driving forces to these independent joints, the forward dynamic analysis is conducted using the regular assembly-disassembly pass explained in Section 3 to form and solve the equations of motion of the system. In other words, this does not affect the way we formulate the forward dynamics in the GDCA since this component of the CTC method generates all joint accelerations due to the application of the control torques and forces to the independent joints. Furthermore, for a multibody system with loop closure, the handle equations of the entire system in the disassembly pass can be easily solved using the mathematical formulations derived in [48]. This, however, creates new challenges that must be appropriately taken care of in the inverse dynamic calculations. Essentially, in the inverse dynamic analysis, the dependent joints do have kinematically enforced accelerations associated with desired accelerations being fed to the independent ones. By the nature of the kinematic relationship, the Jacobian matrix of the entire system may be formed and used to solve for these dependent joint accelerations since the system is constrained. However, this may impose a huge computational load on the simulation tool. Another way to solve this problem is to treat the joint accelerations of the dependent joints as unknown accelerations in the inverse dynamic problem, and solve for them in the disassembly process. In other words, following the binary tree in Fig. 3(b), if two consecutive bodies/assemblies are connected by a dependent joint, Eq. (15) is used to create a new assembly. However, if two consecutive bodies/assemblies are connected by an independent joint, the desired joint acceleration as well as the controller input must be kept in the assembly process. As such, one uses Eq. (19) to combine them and create a new assembly. After conducting the assembly sweep for the entire system, the disassembly pass will be performed to solve for the generalized driving forces applied to the independent joints. Since the main goal of the inverse dynamics is to find the generalized driving forces, one can alternatively use the new assembly-disassembly scheme explained here for closed-chain systems to avoid unnecessary calculations. Consider the system shown in Fig. 4. This figure shows the schematics of a spatial closed chain in which each joint allows the relative motion between the consecutive bodies along a single direction, i.e. prismatic and/or cylindrical joints. Therefore, the entire system contains 3 degrees of freedom with dependent joints (white circles) and independent joints (black-filled circles). This system is broken down into the regions connected to each other by independent joints. In other words, each region only contains the dependent joints. In this strategy, the assembly process is first applied to the handle equations governing the inverse dynamics inside the regions. Since all joints within these regions are dependent, Eq. (15) is used to assemble the constituent bodies/subassemblies. These assemblies for this example are represented by assemblies (1 − 2 − 3 ), (4 − 5 ), and (6 − 7 ) in Fig. 4. The resulting assemblies are then combined together to form the gray assemblies. Since the connecting joints between these regions are independent, Eq. (19) is used in this process. After finishing the assembly pass of the entire system, unlike the traditional DCA disassembly scheme, one should not perform the disassembly process for all levels of the binary tree. For instance, the inverse dynamic equations of the existing nodes in levels 5 and 4 of the binary tree are solved in the disassembly pass. The results can then be used to solve the inverse dynamic equations of the assembly (4 − 5 − 6 − 7 ) in level 3 for f˜4 and f˜8 . This is then followed by solving the inverse dynamic equations of the assembly (6 − 7 ) for f˜6 . Therefore, the presented assembly-disassembly scheme avoids performing the disassembly sweep of the entire nodes of the

490

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

Fig. 5. Appearance of the generalized driving forces in the boundaries of closed-chain systems.

binary tree. This reduces the computational cost of the disassembly pass as one can stop this process immediately after obtaining the necessary generalized driving forces in the inverse dynamic component of CTC. Another challenge in the inverse dynamic formulation of closed-chain problems in the CTC scheme is treating the boundaries to solve the handle equations of the entire system. Solving these equations when both boundaries contain unknown accelerations of dependent joints is similar to the forward dynamic simulations as discussed in [46]. In the following section, we address two further cases in which either one or both boundaries may involve with independent joints. 5.1. Boundaries with independent joints For the case shown in Fig. 5(a) in which both terminal handles of the entire system are independent joints, the CTC-based handle equations governing the inverse dynamics of the entire system are revised as



1:nb 1:nb ψ11 ψ12 1:nb 1:nb ψ21 ψ22



F¯ 11 n F¯ b



 =

−P˙ J

2



1

1 1 1 1 1:n P˙ J q˙ J + PJ (q¨ Jd − wJ ) − ψ13 b nb +1

q˙ J

nb +1

− PJ

nb +1

n +1

n +1

1:nb (q¨ Jd b − wJ b ) − ψ23

.

(55)

n Since both joint accelerations on the right hand side are known, these equations can be solved for F¯ 11 and F¯ 2b . Therefore, driving forces applied to the terminal handles are calculated as



1 PJ f˜1 nb +1 −PJ f˜nb +1





PJ ( PJ )T

=

1

1

PJ

0

 ×



0 nb +1

n +1

( PJ b )T

1:nb 1:nb ψ11 ψ12 1:nb 1:nb ψ21 ψ22

−1

1

1 1 1 1 1:n P˙ J q˙ J + PJ (q¨ Jd − wJ ) − ψ13 b

−P˙ J

nb +1

q˙ J

nb +1

− PJ

nb +1

n +1

n +1

1:nb (q¨ Jd b − wJ b ) − ψ23



(56)

.

5.2. Boundaries with both dependent and independent joints Consider the system shown in Fig. 5(b) in which J1 and J nb +1 are respectively independent and dependent joints. For this system, the inverse dynamic handle equations of motion of the entire system are revised as 1

1 1 1 1 PJ (q¨ Jd − wJ ) + P˙ J q˙ J =

−PJ

nb +1

q¨ J

nb +1

− P˙ J

nb +1

q˙ J

nb +1

1:nb ¯ 1 1:n n 1:n ψ11 F1 + ψ12 b F¯ 2b + ψ13 b ,

=

1:nb ¯ 1 1:n n 1:n ψ21 F1 + ψ22 b F¯ 2b + ψ23 b .

Eq. (58) is first premultiplied by (DJ

− ( DJ

nb +1

)T P˙ J

nb +1

q˙ J

nb +1

= ( DJ

(57)

nb +1

nb +1

)T to eliminate the unknown quantity q¨ J

1:nb ¯ 1 1:n n 1:n F1 + ψ22 b F¯ 2b + ψ23 b ]. )T [ψ21

n F¯ 2b

(58) nb +1

, resulting in

(59)

J nb +1

It is aimed to isolate in the above equation. Since is a dependent joint, it is free from any generalized driving force. Therefore, one can use the following expression for the entire load applied to this joint n n F¯ 2b = F2cb = DJ

nb +1

n

f2cb .

(60)

This modifies Eq. (59) as n +1

n +1

n +1

n +1

n +1

1:nb J b n 1:n 1:n (DJ b )T ψ22 D f2cb = (DJ b )T [−P˙ J b q˙ J b − ψ21 b F¯ 11 − ψ23 b ].

(61)

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500 n +1

n +1

1:n

491

n

Using the inverse of (DJ b )T ψ22 b DJ b , the above equation is solved for f2cb . Therefore, the following relation is used to n express F¯ 2b in terms of the unknown term F¯ 11 as n F¯ 2b = DJ

nb +1

n 1:n 1:n f2cb = γ [ψ21 b F¯ 11 + ψ23 b + P˙ J

nb +1

q˙ J

nb +1

],

(62)

where n +1

n +1

n +1

n +1

1:nb J b γ = −DJ b [(DJ b )T ψ22 D ]−1 (DJ b )T .

(63)

Eq. (62) can now be directly plugged into Eq. (57) to solve for 1

F¯ 11

as

1:n 1:n 1:n 1:n 1:n F¯ 11 = (ψ11 b + ψ12 b γ ψ21 b )−1 [PJ (q¨ Jd − wJ ) + P˙ J q˙ J − ψ12 b γ (ψ23 b + P˙ J 1

1

1

1

nb +1

q˙ J

nb +1

1:nb ) − ψ13 ].

(64)

Finally, the spatial driving force applied to the inward handle of the system is computed using the following relation 1 n +1 n +1 1 1 1 1 1 1 1 1:n 1:n 1:n 1:n 1:n 1:n PJ f˜1 = PJ (PJ )T (ψ11 b + ψ12 b γ ψ21 b )−1 [PJ (q¨ Jd − wJ ) + P˙ J q˙ J − ψ12 b γ (ψ23 b + P˙ J b q˙ J b ) − ψ13 b ].

(65)

6. GDCA-based CTC with selected controllers k

Up to this point, wJ has been treated as a known term that stabilizes the tracking error dynamics. In general, any control k scheme could be used to generate wJ . In the following, frequently-used control schemes such as PID, PD, Pole-placementbased state feedback controller, and Linear Quadratic Regulator (LQR) [71] are investigated to be appropriately integrated with the developed GDCA-based CTC. 6.1. PD-PID controller A PID controller is generally used when disturbances are included in the generalized driving forces, otherwise a PD controller suffices. In general, a PID controller is defined as,



w = −K p e − Kv e˙ − Ki

0

t

e ( τ )d τ ,

(66)

in which Kp , Kv , and Ki are diagonal matrices chosen to satisfy the stability of the feedback control system. In iGDCA, a generalized coordinate describes the relative motion of the child body with respect to its parent body. As such, for the k connecting joint Jk between bodies k − 1 and k, the controller wJ is expressed as k

k

k

k

k

wJ = −K pJ eJ − KvJ e˙ J − KiJ

k



0

t

eJ ( τ )d τ , k

(67)

where k

k

k

k

k

k

eJ = qJd − qJ ,

(68)

e˙ J = q˙ Jd − q˙ J .

(69)

Therefore, the inverse dynamic constraint Eq. (19) is revised as k

k

k

k

k

k k k k k Ak1 = Ak2−1 + P˙ J q˙ J + PJ [q¨ Jd + K pJ (qJd − qJ ) + KvJ (q˙ Jd − q˙ J ) + KiJ

k



t 0

k

(qJd − qJ )dτ ]. k

(70)

For the implementation of the PD controller in the simulation, the integration terms associated with Ki may be dropped. As previously described, Eq. (70) is then used at the constraint level of iGDCA to obtain the generalized driving forces to be fed into the forward dynamic solver. The PID controller gain selection can be performed following the strategy explained in [9]. To make this paper self-contained, this method is described in Appendix B. 6.2. Pole placement-based state feedback controller In the case of state feedback control [71], Eq. (7) can be essentially rewritten as

e˙ = Ae + Bw,

(71)

w = Ke, where K is a diagonal matrix. Therefore, the dynamic equation of the connecting joint written as k k k k k E˙ J = AJ E J + BJ wJ ,

(72) Jk

between bodies k − 1 and k can be

(73)

492

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

where

E



Jk

=

0

⎡ k

AJ =

t

e (τ )dτ , e , e˙ Jk

0 1

0

0

0

Jk

,

(74)

⎤

⎣0 0 1⎦, 0



k

T Jk

(75)

T

BJ = 0

0 1 .

(76)

Since the state space in Eq. (71) is completely state controllable, the state variables are measurable, and the control input is unconstrained, the closed-loop poles can be placed at desired locations by using the pole-placement technique. Finally, the k controller gain K J is designed to generate the following control effort k

k

k

wJ = K J E J ,

(77) Jk

Jk

such that the poles of the closed-loop system (i.e. Ac = AJ − BJ K J ) are located at desired locations μi , i = 1, 2, 3. The k

k

k

Jk

Jk

Jk

k

resulting w is then used in Eq. (19) to perform the assembly pass. Since A and B are time-invariant, gain matrix K J is time-independent and is evaluated in the preprocessing step prior to the simulation. It should be mentioned that similar to the PID controller, this controller uses the integral of the error in the joint coordinates. 6.3. Linear quadratic regulator (LQR) State feedback controller can be extended to the LQR [71] with the optimum feedback gains for linear time invariant k systems. For the linear time invariant system shown in Eq. (71), the cost function J J for the connecting joint Jk between bodies k − 1 and k is defined as k

JJ =



∞ 0

 k T k k k k T k (E J ) Q J E J + λJ (wJ ) wJ dt

(78)

where E J is defined in Eq. (74), Q J ∈ R3 is a positive definite matrix, and λJ ∈ R+ . It can be shown that the cost function k

k JJ

k

k

is minimum if and only if the controller k

Jk

k wJ

in Eq. (71) is chosen using Kopt as

J Kopt = (λJ )−1 (BJ )T P J , k

k

k

(79)

k

k

in which where BJ is defined in Eq. (76), and P J is obtained by solving the algebraic Riccati differential equation [71] defined as

(AJ )T PJ + PJ AJ − λ−1 PJ BJ (BJ )T PJ + Q J = 0. k

Jk

k

k

k

k

k

k

k

k

(80)

Jk

Since A and B are time-invariant, the optimization is performed prior to the simulation; therefore, it does not impose any computational cost on the simulation. 7. Simulation results In the proceeding sections, the simulation results of controlling selected chain systems with different boundary conditions are demonstrated. To perform these simulations, GDCA-based CTC is integrated with the control strategies described Jk

Jk

Jk

Jk

in the previous section. PID or PD gains are selected as K p = 100, Kv = 20, and Ki = 250 (Ki = 0 for PD controller). The Jk

Jk

Jk

desired poles in the pole-placement technique are located at μ1 = −2, μ2 = −2.1, and μ3 = −2.2. Finally, the LQR gains are selected using λ = 0.01, while Q Jk

Jk

J

is a 3 × 3 identity matrix. This results in Kopt = [10 18.2674 11.6848]T .

7.1. Control of constrained-Free chain systems Consider a planar triple pendulum system following the coordinate representation in Fig. 6(a). Each body is treated as a rod with the unit length, as well as the unit mass distributed uniformly along it. Herein, the initial conditions of the simulation are set to q1 = 5π /6 (rad ), q2 = 3π /4 (rad ), q3 = 2π /3 (rad ) and q˙ 1 = q˙ 2 = q˙ 3 = 0 (rad/s ). The desired trajectory for qdk is chosen as

qdk = 0.6 sin

 2π  1.5

t ,

k = 1, 3,

(81)

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

493

Fig. 6. Coordinates and parameter representation of two constrained-free chains to be controlled by GDCA-based CTC.

Fig. 7. Angular positions and the error in the angular positions at joints of the triple pendulum system in Fig. 6(a): desired trajectory (black), PID (red), pole placement (green), and LQR (cyan). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

qdk = 0.6 cos

 2π  1.5

t ,

k = 2.

(82)

A constant disturbance of 0.2 N m is also introduced to each generalized driving force which is essentially the torque applied from each parent body to its child body as shown in Fig. 6(a). The results of a 5-s simulation using the GDCA-based CTC can be seen in Fig. 7. Here, it is easy to see that the closed-loop system is stable with the desired performance. Fig. 8 also k k shows the actuator torques f˜k and the controller output wJ . It is observed that wJ does not become zero. This is due to the application of the disturbance in the actuating torque which results is non-zero integration error. We also extend the simulations to the spatial double pendulum system shown in Fig. 6(b). Each body represents a cuboid with length a = 1 (m), width b = 0.1 (m), and height c = 0.1 (m), as well as the unit mass distributed uniformly along it.

494

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

Fig. 8. Generalized actuating force and control output at joints of the triple pendulum system in Fig. 6(a): PID (red), pole placement (green), and LQR (cyan). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

In this system, xyz represents the Newtonian frame of reference, while x1 y1 z1 and x2 y2 z2 are respectively attached to the center of mass of the first and second bodies. These reference frames are selected such that z1 is always parallel to z, and y2 is always parallel to y1 . The revolute joint J1 connects the first body to the ground in a way that it can rotate on the xy plane about the z axis. Further, the second body is connected to the first body via the revolute joint J2 such that its axis of rotation is always parallel to y1 . Herein, the initial conditions of the simulation are set to q1 = π /2 (rad), q2 = 0 (rad), and q˙ 1 = q˙ 2 = 1 (rad/s). The desired trajectory for q is chosen as

qd1 = sin

 2π 

 2π 

1.5

1.5

t , qd2 = cos

t .

(83)

Similar to the previous example, a constant disturbance of 0.2 N m is also introduced to each actuating torque. The results of a 5-s simulation using the GDCA-based CTC can be seen in Figs. 9 and 10. 7.2. Control of a closed-chain system Consider a fully-actuated planar closed chain consisting of 5 rods connected to each other via revolute joints as shown in Fig. 11. The lengths of the rods are Lk = 1 (k = 1, 2, 4, 5 ) (m ) while L3 = 0.5 (m ). The mass of each body is set to 1 (kg ) which is uniformly distributed along the rod. Motors/actuators provide torques at handles H1k (k = 1, 2, 3 ). As such, the control torques are applied to the independent joints shown by black-filled circles in Fig. 11, while joints represented by white circles are dependent. The initial joint coordinates of the system are set to q1 = −π /8 (rad ), q2 = 5π /8 (rad ), q3 = 3π /8 (rad ), q4 = 3π /8 (rad ) and q5 = 5π /8 (rad ), while the joint velocities are set to q˙ k = 0 (k = 1, 2, 3, 4, 5 ) (rad/s ). The desired joint coordinates of the independent joints are chosen to be qd1 = 0 (rad ), qd2 = π /4 (rad ), and qd3 = −3π /4 (rad ). In the absence of the disturbance, the PD controller is used to drive the system to the desired position. The results of a 10-s simulation are observed in Figs. 12 and 13. 7.3. O(n)-complexity control of the multiple pendulum problem To examine the efficiency of the presented method, the described planar system in Section 7.1 is extended to an N-body control problem. In this case, the desired trajectory qd is set to zero. The initial values of the joint coordinates are selected

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

495

Fig. 9. Angular positions and the error in the angular positions at joints of the spatial pendulum in Fig. 6(b): desired trajectory (black), PID (red), pole placement (green), and LQR (cyan). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

Fig. 10. Generalized actuating force and control output at joints of the spatial pendulum in Fig. 6(b): PID (red), pole placement (green), and LQR (cyan). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

Fig. 11. Coordinates and parameter representation of a planar closed-chain system.

496

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

Fig. 12. Angular positions and the error in the angular positions at joints of the closed chain system in Fig. 11: Desired trajectory (black), PID (red), pole placement (green), and LQR (cyan). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

as

qdk = (−1 )k

π  6

,

k = 1, 2, . . . nb − 1,

(84)

while the initial joint speeds are set to zero. No disturbance is introduced to the system; therefore, a PD controller is used. This control scheme is then applied to a series of chain systems with 2, 4, 8, and 16 bodies. First, both iGDCA and GDCA are used for inverse and forward dynamic calculations, respectively. The same simulations are then performed using O(n )-Newton-Euler [2] and GDCA to conduct the forward and inverse dynamics, respectively. The average computation time per iteration to perform the inverse dynamics using iGDCA and recursive Newton-Euler is measured for each simulation. The results shown in Fig. 14 demonstrate a linear increase in computation time for the inverse dynamic evaluations. It should be mentioned that these results have been obtained using ode45 on PC with CPU core i7-4500U 1.8 GHz. As it was predicted, iGDCA is not optimal in the serial implementation. However, DCA-based methods can lend themselves to the parallel computing framework. Theoretically, with O(n) processors, computational complexity of these methods increases as a logarithmic function of the number of degrees of freedom of the system. The detailed parallel implementation of the DCA with the associated challenges using CPU and GPU has been discussed in detail in [72]. 8. Conclusion In this paper, the Divide-and-Conquer Algorithm (DCA) has been extended and integrated with the computed torque control (CTC) technique to efficiently model and control fully-actuated large multibody problems. Following CTC, both inverse and forward dynamics are necessary to control the system. However, using traditional techniques of forming and solving the equations of motion of the system may impose unnecessary computational burden on the simulation tool and prohibit the real time control. Herein, high performance Generalized-Divide-and-Conquer Algorithm (GDCA) has been used to perform the forward dynamics simulation. Furthermore, the inverse Generalized-Divide-and-Conquer Algorithm (iGDCA) has been utilized to form the inverse dynamic formulation under the computed torque control law. Since iGDCA uses both spatial and generalized coordinates, a new mathematical platform has been designed to modify this technique to appropriately address CTC in the inverse dynamics. The application of iGDCA efficiently solves the inverse dynamics and provides the generalized driving forces to drive the multibody system along the desired trajectory. The GDCA-based CTC has then been integrated with several popular control schemes such as PD/PID, pole placement-based state feedback controller, and

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

497

Fig. 13. Generalized actuating force and control output at joints of the closed chain system in Fig. 11: PID: Blue, Pole placement: Green, and LQR: Cyan. (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

Fig. 14. Demonstration of O(n) complexity of the inverse dynamic computation per iteration: iGDCA (blue), recursive Newton-Euler (red). (For interpretation of the references to color in this figure legend, the reader is referred to the web version of this article.)

498

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

LQR. Finally, the developed technique has been successfully applied to control several test cases with different boundary conditions. Appendix A. Joint free-motion map and its orthogonal complement map Free floating bodies can move through 6 degrees of freedom relative to each other. Using the kinematic joints between the consecutive bodies restricts some of these relative motions. Consider bodies k − 1 and k connected by the kinematic joint Jk which allows the relative motion between these bodies. Due to the application of this joint, the number of relative degrees of freedom between these bodies reduces to ν k . As such, the 6 dimensional relative free-motion map between two k bodies is partitioned into free modes of motion described by the matrix PJ which has the dimension of 6 × ν k . In other k words, PJ is the matrix of the free-modes of motion [69] characterized by the ν k degree of freedom permitted at the joint k Jk . Further, the orthogonal complement of this joint free-motion map DJ has the dimension of 6 × (6 − ν k ). This space supports the constraint loads due to the kinematic joint. In other words, these spatial loads lie in the space of orthogonal complement of the joint free-motion map. The orthogonality condition is expressed as

( DJ ) T P J = 0 . k

k

(85)

Appendix B. Controller gain selection The following guidelines are discussed in the context of control torque in [9], and also covered as a more general case in [71]. The gains for Kv and KP can be derived by studying the characteristic polynomial of the PID controller with Ki = 0. As such, it is necessary to analyze the closed loop error dynamics of a PID controller in terms of its characteristic polynomial. If the defined PID controller from Eq. (66) is used and substituted into Eq. (5), knowing that Eq. (5) is in fact a combination of the control input w and disturbance input z, one arrives at,

z = e¨ + Kv e˙ + K p e + Ki ε ,

(86)

where ε is the time integral of the error defined as,



ε=

t o

e ( τ )d τ ,

(87)

Eq. (86) can also be seen by substituting Eq. (5) into Eq. (7) to obtain,

⎡

⎤ ⎡ ⎤⎡ ⎤ ⎡ ⎤ ε 0 I 0 ε˙ 0 ⎣e˙ (t )⎦ = ⎣ 0 0 I ⎦⎣e(t )⎦ + ⎣0⎦z. −Ki −K p −Kv e˙ (t ) I e¨ (t )

(88)

For the case Ki = 0, or PD control, the resulting characteristic polynomial is,

z(s ) = e(s )(Is2 + Kv s + K p ),

(89)

where I is the identity matrix. The gains are chosen as nb × nb diagonal matrices [9,71] where nb is the number of indeJm Jm Jm Jm pendent joints of the system. They can therefore be written as, Kv = diag(kv ), K p = diag(k p ), and Ki = diag(ki ) where kv , Jm

Jm

k p , and ki

are scalars. Eq. (89) can then be written for individual joints as, m

m

zJ (s ) = eJ (s )(s2 + kJv s + kJp ), m

m

m = 1, 2, . . . nb .

(90)

A general, second-order characteristic polynomial under classical control theory is,

T (s ) = s2 + 2ζ ωn s + ωn2 ,

(91)

where ζ is the damping ratio and ωn is the natural frequency. The natural frequency is based on the time constant τ of the closed loop system and is defined as,

ωn = 1/τ . Jm

The gains for kv and m

kJp = m

ωn2

kJv = 2ζ ωn

(92) Jm kp

can be immediately read from Eq. (91) by comparing s terms with Eq. (90), yielding

(93)

(94)

For good tracking, a time constant of 0.1 s or less should be selected [9]. Note that, it may be desired to design the gains around critical damping to prevent overshoot and possible collisions between bodies in a real world application.

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

499

Expanding to PID, we can gather from the above logic that the closed loop expression of the error dynamics for individual joints is m

m

m

zJ (s ) = eJ (s )(s3 + kJv s2 + kJp s + kJi ), m = 1, 2, . . . nb . m

m

(95)

The gains obtained in Eqs. (93) and (94) can now be used with the above expression in the Routh test. In doing so, one will Jm Jm Jm find that ki must be smaller than the product of kv and k p to maintain a stable system [9], m

m

m

kJi < kJv kJp ,

(96)

while all gains are positive. References [1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17]

[18] [19] [20] [21] [22] [23] [24] [25] [26] [27] [28] [29] [30] [31] [32] [33] [34] [35] [36] [37] [38] [39] [40] [41]

A. Codourey, Dynamic modeling of parallel robots for computed-torque control implementation, Int. J. Rob. Res. 17 (12) (1998) 1325–1336. R. Featherstone, Robot Dynamics Algorithms, Kluwer Academic Publishing, 1987. A. Jain, Robot and Multibody Dynamics Analysis and Algorithms, Springer, 2011. N.V. Vaidehi, A. Jain, W.A. Goddard, Constant temperature constrined molecular dynamics: the Newton-Euler inverse matrix operator method, J. Phys. Chem. 100 (1996) 10508–10517. M. Poursina, K.D. Bhalerao, S. Flores, K.S. Anderson, A. Laederach, Strategies for articulated multibody-based adaptive coarse grain simulation of RNA, in: M.L. Johnson, L. Brand (Eds.), Methods in Enzymology, Computer Methods Part C, 487, ScienceDirect, 2011, pp. 73–98. M. Poursina, K.S. Anderson, Canonical ensemble simulation of biopolymers using a coarse-grained articulated generalized divide-and-conquer scheme, Comput. Phys. Commun. 184 (3) (2013) 652–660. http://dx.doi.org/10.1016/j.cpc.2012.10.029. M. Haghshenas-Jaryani, A. Bowling, Modeling flexibility in Myosin V using a multiscale articulated multi-rigid body approach, ASME. J. Comput. Nonlinear Dyn. 10 (1) (2014) 011015. P. Malczyk, J. Fraczek, Molecular dynamics simulation of simple polymer chain formation using divide and conquer algorithm based on the augmented lagrangian method, Proc. Inst. Mech. Eng., Part K: J. Multibody Dyn. 229 (2) (2015) 116–131. F.L. Lewis, D.M. Dawson, C.T. Abdallah, Robot Manipulator Control: Theory and Practice, Marcel Dekker, Inc, 2003. A. Isidori, Nonlinear Control Systems, Springer, 1989. A. Basudhar, S. Missoum, Reliability assessment using probabilistic support vector machines, Int. J. Reliab. Saf. 7 (2) (2013) 156–173, doi:10.1504/IJRS. 2013.056378. R. Seifried, Dynamics of Underactuated Multibody Systems: Modeling, Control and Optimal Design, Springer, Switzerland, 2014. A. Karim, Adaptive computed torque control of robot manipulators, Colorado State University, 1989 Ph.D. thesis. M. Uebel, L. Minis, K. Cleary, Improved computed torque control for industrial robots, in: International Conference of Robotics and Automation, 1992. I. Lammerts, Adaptive computed reference computed torque control of flexible manipulators, Technische Universiteit Eindhoven, 1993 Ph.D. thesis. P. Masarati, Computed torque control of redundant manipulators using general-purpose software in real-time, Multibody Syst. Dyn. 32 (4) (2014) 403–428. N.A. Radford, P. Strawser, K.A. Hambuchen, J.S. Mehling, W.K. Verdeyen, A.S. Donnan, J. Holley, J. Sanchez, V. Nguyen, L.B. Bridgwater, R. Berka, R.O. Ambrose, M.M. Markee, N.J. Fraser-Chanpong, C. McQuin, J.D. Yamokoski, S. Hart, R. Guo, A. Parsons, B. Wightman, P. Dinh, B. Ames, C. Blakely, C. Edmondson, B. Sommers, R. Rea, C. Tobler, H. Bibby, B. Howard, L. Niu, A. Lee, M. Conover, L. Truong, R. Reed, D. Chesney, R.P. Jr., G. Johnson, C.-L. Fok, N. Paine, L. Sentis, E.A. Cousineau, R.W. Sinnet, J. Lack, M.J. Powell, B. Morris, A.D. Ames, J. Akinyode, Valkyrie: NASA’s first bipedal humanoid robot., J. Field Rob. 32 (3) (2015) 397–419. A.F. Vereshchagin, Computer simulation of the dynamics of complicated mechanisms of robot-manipulators, Eng. Cybern. 12 (6) (1974) 65–70. W.W. Armstrong, Recursive solution to the equations of motion of an n-link manipulator, in: Fifth World Congress on the Theory of Machines and Mechanisms, 1979, pp. 1342–1346. J.S.Y. Luh, M.W. Walker, R.P.C. Paul, On-line computational scheme for mechanical manipulators, J. Dyn. Syst. Meas. Control 102 (2) (1980) 69–76. M.W. Walker, D.E. Orin, Efficient dynamic computer simulation of robotic mechanisms, J. Dyn. Syst. Meas. Control 104 (3) (1982) 205–211. R. Featherstone, The calculation of robotic dynamics using articulated body inertias, Int. J. Rob. Res. 2 (1) (1983) 13–30. D.E. Rosenthal, M.A. Sherman, High performance multibody simulations via symbolic equation manipulation and Kane’s method, J. Astronaut. Sci. 34 (3) (1986) 223–239. P.E. Neilan, Efficient computer simulation of motions of multibody systems, Stanford University, 1986 Ph.D. thesis. H. Brandl, R. Johanni, M. Otter, A very efficient algorithm for the simulation of robots and similar multibody systems without inversion of the mass matrix, in: IFAC/IFIP/IMACS Symposium, Vienna, Austria, 1986, pp. 95–100. D.S. Bae, E.J. Haug, A recursive formation for constrained mechanical system dynamics: Part I, open loop systems, Mech. Struct. Mach. 15 (3) (1987) 359–382. K.S. Anderson, Recursive derivation of explicit equations of motion for efficient dynamic/control simulation of large multibody systems, Stanford University, 1990 Ph.D. thesis. D. Rosenthal, An order n formulation for robotic systems, J. Astronaut. Sci. 38 (4) (1990) 511–529. A. Jain, Unified formulation of dynamics for serial rigid multibody systems, J. Guidance Control Dyn. 14 (3) (1991) 531–542. L.H. Lathrop, Parallelism in manipulator dynamics, Int. J. Rob. Res. 4 (2) (1985) 80–102. H. Kasahara, H. Fujii, M. Iwata, Parallel processing of robot motion simulation, in: Proceedings IFAC 10th World Conference, 1987. D.-S. Bae, J.G. Kuhl, E.J. Haug, A recursive formulation for constrained mechanical system dynamics: Part III. Parallel processor implementation, Mech. Based Des. Struct. Mach. 16 (2) (1988) 249–269. R.S. Hwang, D. Bae, J.G. Kuhl, E.J. Haug, Parallel processing for real-time dynamics systems simulations, J. Mech. Des. 112 (4) (1990) 520–528. A. Avello, J.M. Jimenez, J.G.J. E. Bayo, A simple and highly parallelizable method for real-time dynamic simulation based on velocity transformations, Comput. Methods Appl. Mech. Eng. 107 (3) (1993) 313–339. S. Chung, E.J. Haug, Real-time simulation of multibody dynamics on shared memory multiprocessors, J. Dyn. Syst. Meas. Control 115 (4) (1993) 627–637. J.H. Critchley, K.S. Anderson, Parallel logarithmic order algorithm for general multibody system dynamics, Multibody Syst. Dyn. 12 (1) (2004) 75–93. K.D. Bhalerao, J. Critchley, K.S. Anderson, An efficient parallel dynamics algorithm for simulation of large articulated robotic systems, Mech. Mach. Theory 53 (2012) 86–98. L. Mraz, M. Valasek, Solution of three key problems for massive parallelization of multibody dynamics, Multibody Syst. Dyn. 29 (1) (2013) 21–39, doi:10.1007/s11044- 012- 9311- 1. A. Fijany, A.K. Bejczy, Techniques for parallel computation of mechanical manipulator dynamics. part II Forward dynamics, in: C. Leondes (Ed.), Advances in Robotic Systems and Control, 40, Academic Press, 1991, pp. 357–410. A. Fijany, I. Sharf, G.M.T. D’Eleuterio, Parallel O(log n) algorithms for computation of manipulator forward dynamics, IEEE Trans. Rob. Autom. 11 (3) (1995) 389–400. Y. Stepanenko, M. Vukobratovi, Dynamics of articulated open-chain active mechanisms, Math. Biosci. 28 (1) (1976) 137–170. http://dx.doi.org/10.1016/ 0 025-5564(76)90 099-7.

500

C. Kingsley et al. / Mechanism and Machine Theory 116 (2017) 481–500

[42] B. Dasgupta, T. Mruthyunjaya, A Newton-Euler formulation for the inverse dynamics of the Stewart platform manipulator, Mech. Mach. Theory 33 (8) (1998) 1135–1152. http://dx.doi.org/10.1016/S0 094-114X(97)0 0118-3. [43] Z.M. Di, B. Kang, An inverse dynamic model of over-constrained parallel kinematic machine based on Newton–Euler formulation, J. Dyn. Syst. Meas. Control 136 (4) (2014) 041001–041009. [44] R. Featherstone, A divide-and-conquer articulated body algorithm for parallel O(log (n)) calculation of rigid body dynamics. Part 1: basic algorithm, Int. J. Rob. Res. 18 (9) (1999a) 867–875. [45] R. Featherstone, A divide-and-conquer articulated body algorithm for parallel O(log (n)) calculation of rigid body dynamics. Part 2: trees, loops, and accuracy, Int. J. Rob. Res. 18 (9) (1999b) 876–892. [46] R. Mukherjee, K.S. Anderson, An orthogonal complement based divide-and-conquer algorithm for constrained multibody systems, Nonlinear Dyn. 48 (1–2) (2007a) 199–215. [47] R. Mukherjee, K.S. Anderson, A logarithmic complexity divide-and-conquer algorithm for multi-flexible articulated body systems, Comput. Nonlinear Dyn. 2 (1) (2007b) 10–21. [48] R.M. Mukherjee, K.S. Anderson, Efficient methodology for multibody simulations with discontinuous changes in system definition, Multibody Syst. Dyn. 18 (2) (2007c) 145–168. [49] R.M. Mukherjee, K.D. Bhalerao, K.S. Anderson, A divide-and-conquer direct differentiation approach for multibody system sensitivity analysis, Struct. Multidiscip. Optim. 35 (5) (2007) 413–429. [50] K.D. Bhalerao, M. Poursina, K.S. Anderson, An efficient direct differentiation approach for sensitivity analysis of flexible multibody systems, Multibody Syst. Dyn. 23 (2) (2010) 121–140. [51] P. Malczyk, J. Fraczek, A divide and conquer algorithm for constrained multibody system dynamics based on augmented lagrangian method with projections-based error correction, Nonlinear Dyn. 70 (1) (2012) 871–889, doi:10.1007/s11071- 012- 0503- 2. [52] M. Poursina, K.S. Anderson, An extended divide-and-conquer algorithm for a generalized class of multibody constraints, Multibody Syst. Dyn. 29 (3) (2012) 235–254, doi:10.1007/s11044- 012- 9324- 9. [53] I. Khan, K. Anderson, Performance investigation and constraint stabilization approach for the orthogonal complement-based divide-and-conquer algorithm, Mech. Mach. Theory 67 (2013) 111–121. http://dx.doi.org/10.1016/j.mechmachtheory.2013.04.009. [54] M. Poursina, Extended divide-and-conquer algorithm for uncertainty analysis of multibody systems in polynomial chaos expansion framework, J. Comput. Nonlinear Dyn. 11 (3) (2015) 031015–031023. [55] K. Chadaj, P. Malczyk, J. Fraczek, Efficient parallel formulation for dynamics simulation of large articulated robotic systems, in: 20th International Conference on Methods and Models in Automation and Robotics, MMAR 2015, Mikedzyzdroje, Poland, August 24–27, 2015, pp. 441–446, doi:10.1109/ MMAR.2015.7283916. [56] P. Malczyk, J. Fraczek, F. Gonzalez, J. Cuadrado, Index-3 divide and conquer algorithm for real-time multibody dynamics simulations, in: Proceedings of the ASME 2016 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference, Charlotte, North Carolina, USA, 2016. [57] K. Chadaj, P. Malczyk, J. Fraczek, A parallel Hamiltonian formulation for forward dynamics of closed-loop multibody systems, Multibody Syst. Dyn. (2016) 1–27, doi:10.1007/s11044- 016- 9531- x. [58] J. Laflin, K.S. Anderson, I.M. Khan, M. Poursina, Advances in the application of the divide-and-conquer algorithm to multibody system dynamics, J. Comput. Nonlinear Dyn. 9 (2014a) 041003, doi:10.1115/1.4026072. [59] J. Laflin, K.S. Anderson, I.M. Khan, M. Poursina, New and extended applications of the divide-and-conquer algorithm for multibody dynamics, J. Comput. Nonlinear Dyn. 9 (2014b) 041004–041011, doi:10.1115/1.4027869. [60] A. Zelei, L.L. Kovacs, G. Stepan, Computed torque control of an under-actuated service robot platform modeled by natural coordinates, Rob. Autom. 16 (5) (2011) 2205–2217. [61] J. Kovecses, J.C. Piedboeuf, C. Lange, Dynamics modeling and simulation of constrained robotic systems, IEEE/ASME Trans. Mechatron. 8 (2) (2003) 165–177, doi:10.1109/TMECH.2003.812827. [62] S.W. Wijesoma, R.J. Richards, Robust trajectory following of robots using computed torque structure with VSS, Int. J. Control 52 (4) (1990) 935–962, doi:10.1080/0 02071790 08953575. [63] M. Zhihong, X.H. Yu, K. Eshraghian, M. Palaniswami, A robust adaptive sliding mode tracking control using an RBF neural network for robotic manipulators, in: IEEE International Conference on Neural Networks, 1995. Proceedings, 5, 1995, pp. 2403–2408, doi:10.1109/ICNN.1995.487738. [64] Z. Song, J. Yi, D. Zhao, X. Li, A computed torque controller for uncertain robotic manipulator systems: Fuzzy approach, Fuzzy Sets Syst. 154 (2) (2005) 208–226. http://dx.doi.org/10.1016/j.fss.20 05.03.0 07. [65] Y. Chen, G. Ma, S. Lin, J. Gao, Adaptive fuzzy computed-torque control for robot manipulator with uncertain dynamics, Int. J. Adv. Rob. Syst. 9 (6) (2012) 237–245, doi:10.5772/54643. [66] N. Wiener, The homogeneous chaos, Am. J. Math. 60 (4) (1936) 897–936. [67] S. Sabet, M. Poursina, Computed torque control of fully-actuated nondeterministic multibody systems, Multibody Syst. Dyn. (2017). doi:10.1007/ s11044- 017- 9577- 4. [68] S. Sabet, M. Poursina, Robust framework for the computed torque control of nondeterministic multibody dynamic systems, in: ASME 2015 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference, 12th International Conference on Multibody Systems, Nonlinear Dynamics, and Control, DETC2016-60072, Charlot, NC, 2016. [69] R.E. Roberson, R. Schwertassek, Dynamics of Multibody Systems, Springer-Verlag, 1988. [70] C. Kingsley, M. Poursina, Computed torque control of articulated multibody systems in the generalized divide and conquer algorithm framework, in: ASME 2015 International Design Engineering Technical Conferences and Computers and Information in Engineering Conference, 27th Conference on Mechanical Vibration and Noise, DETC2015-46853, Boston, MA, 2015. [71] R.C. Dorf, R.H. Bishop, Modern Control Systems, Pearson Education, 2011. [72] J.J. Laflin, K.S. Anderson, M. Hans, Enhancing the Performance of the DCA When Forming and Solving the Equations of Motion for Multibody Systems, Springer International Publishing, Cham, pp. 19–31. doi:10.1007/978- 3- 319- 30614- 8_2.