Dynamic loads coordination for multiple cooperating robot manipulators

Mechanism and Machine Theory 35 (2000) 985±995

www.elsevier.com/locate/mechmt

Dynamic loads coordination for multiple cooperating robot manipulators p Yong-Sheng Zhao*, Jing-Yi Ren, Zhen Huang Robotics Research Center, Mechanical Engineering Institute, Yanshan University, Qinhuangdao, Hebei, 066004 People's Republic of China Received 14 October 1998; accepted 12 July 1999

Abstract For the situation of multiple manipulators handling a single object, the dynamic model of the system that mapping directly the dynamics between object space and joint space, is established in terms of object dynamic, individual arm dynamics and joint driving torques. Then the methods are introduced which distribute directly the object dynamic and arm dynamics over the joint space in line with the joint-input based criteria. 7 2000 Elsevier Science Ltd. All rights reserved. Keywords: Dynamic load; Coordination; Multiple robotic mechanism

1. Introduction Recently, in the robotics research ®eld, the tasks where open-chain mechanisms must work together (e.g., walking machine, multi®ngered robot hand, and cooperating manipulators, they are called the multiple robotic mechanisms, Fig. 1) have enticed many scholars. These con®gurations result in closed kinematic chains with redundant actuation (we call it the overdeterminate inputs [15]). The closed chains tend to introduce additional dynamic coupling owing to the kinematic constraints between the cooperating mechanisms, and the redundant actuation o€ers an unlimited variety of control inputs that will e€ect any given system motion. The work was supported by National Natural Science Foundation of China. * Corresponding author. Tel.: +86-335-805-5357; fax: +86-335-805-7075. E-mail address: [email protected] (Y.-S. Zhao). p

0094-114X/00/$ - see front matter 7 2000 Elsevier Science Ltd. All rights reserved. PII: S 0 0 9 4 - 1 1 4 X ( 9 9 ) 0 0 0 5 2 - X

986

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

In the present study, most of the works laid the emphases on the mapping between the contact space and the joint space, or between the task space and the contact space. They discussed dynamic load distribution, grasp planning, task space dynamic performances (such as, dynamic manipulability, external force manipulability, internal force manipulability, etc.,) [1,3,5±14]. They usually distribute the dynamic load of the object over the contact space (in contact space, the general solution consists of least norm solution corresponding mainly to forces which cause motion of the object and null space solution corresponding only to internal object forces), then map the forces of contact space onto joint space, through the selection of the vector in the null solution space and in accordance with the criteria, to realize the dynamic load distribution in the joint space. In this paper, we establish the dynamic model of the system mapping directly the dynamics between object space and joint space, in terms of object dynamic, individual arm dynamics and joint driving torques. Then we distribute directly the object dynamic and individual arm's dynamics over the joint's space in accordance with the joint-input based criteria. At the end of paper, the numerical analysis is given for the case of two PUMA262 cooperatively handling a solid disk. 2. Dynamic modeling of the system 2.1. Preliminaries and notation We consider the task to be described as the motion (Position, Orientation) along a desired path of a rigid object with known mass and inertia properties. The object is assumed to be rigidly held by the end e€ectors of the cooperating manipulators, which may impart forces and moments to the object. The object motion equation may be expressed as follows [4,14] PˆWF

…1†

Fig. 1. Multiple cooperating robots system.

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

987

Where P is the dynamic load of the object. F ˆ ‰f T1 , nT1 , . . . ,f TL , nTL ŠT is vector of forces being imparted by the L arms, and W is the grasping matrix [4,14] In Eq. (1), when a desired object trajectory is speci®ed, the value of P is determined. In the previous works [3,5,8±10], the objective is to determine an end-e€ector force solution vector F that will satisfy both object motion constraints and also provide desired characteristics for the cooperative system. In these works, F was chosen to regulate internal forces on the object, or to decrease torque requirements for the speci®c actuator, and for the system with kinematic redundancy, a number of additional constraints are imposed in line with kinematic or dynamic criteria. In this paper, we will use virtual work principle and the readily available kinematic models of individual manipulators to establish the system dynamic model, in which the dynamic loads of the object and the dynamic loads and joint torques of individual arms are involved, and the dynamic mapping between object space and joint space is directly established. For an individual manipulator, the dynamic model has been considered by a number of researchers [1,2,4,6], the model is in the form     T T t yi ˆ Mi …yyi † y i ‡ y_ i Ni …yyi † y_ i ‡ gi …yyi † ÿ Jy i …yyi † Fi

…2†

Where ‰Mi …yyi †Š 2 Rdidi is the symmetric positive de®nite inertia matrix, ‰Ni …yyi †Š 2 Rdididi involves the centripetal and coriolis e€ects and the operation in Eq. (6) is performed plane-byT plane resulting in y_ i ‰Ni …yyi †Šy_ i 2 Rdi , gi …yyi † 2 Rdi is the vector of gravitational torques, Fi 2 R6 is the vector of end-e€ector force exerted on the object, ‰Jyi …yyi †Š 2 R6di is the Jacobian matrix relating end-e€ector coordinates to joint coordinates, t yi 2 Rdi is the vector of joint driving torques. One of the system's dynamic model is obtained by combining the above dynamics [4,14]    T  T t y ˆ M…y † y ‡ y_ N…y † y_ ‡ g…y † ÿ Jy …y † F

…3†

P Where y 2 Rd is the vector of all joint variables of the L manipulators, here d ˆ Liˆ1 di , ‰M…yy†Š 2 Rdd , ‰N…yy†Š 2 Rddd , g…yy† 2 Rd , F 2 R6L , and ‰Jy …yy†Š 2 R6Ld is a Jacobian matrix relating end-e€ector coordinates to joint coordinates. From Eqs. (2) and (3), a reduced model may be obtained for the closed chain system formed by several manipulators simultaneously grasping an object. The mobility h of the closed chain is calculated, and a set q of h generalized coordinates is chosen to formulate the dynamic model [4,14]:       t q ˆ M…q † q ‡ q_ T N…q † q_ ‡ g…q † ÿ Jq …q † T F

…4†

where M‰…q†Š 2 Rhh , ‰N…q†Š 2 Rhhh , g…q† 2 Rh , and ‰Jq …q†Š 2 R6Lh is a Jacobian matrix relating end-e€ector coordinates to generalized coordinates. In the previous works, the objective is to select F in Eq. (1) to perform the object motion, and to adjust the internal loads generated within the object by the cooperating system, or to decrease some speci®c joint torques, in choosing F, the e€ects of the dynamics (3) or (4) are taken into account.

988

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

2.2. New dynamic model of the system Here, we will use virtual work principle to establish another dynamic model in terms of generalized coordinates of the system. We choose u 2 Rh represent a common set of coordinates at the object, its origin is at the mass center of the object. Let ‰J uyi Š 2 Rhdi …i ˆ 1, 2, . . . ,L† be the Jacobian of the individual cooperating arms. Then for the case q  y , and assuming nonredundant arms in nonsingular trajectories …di ˆ h†, we may obtain h i   u ÿ1 2 Rdih J yi u ˆ J yi

i ˆ 1, 2, . . . ,L

…5†

By selecting h rows from f‰J yi u Š, i ˆ 1, 2, . . . ,Lg corresponding to each qi 2 q, we form the q hh matrix ‰J u Š 2 R , inverting this to obtain ‰J uq Š, then ‰J yi q Š is in the form h i h i  yi J yi J uq i ˆ 1, 2, . . . ,L q ˆ Ju

…6†

De®ne y K ˆ fyi jyi 2 y and yi2 = qg, by selecting …L ÿ 1†h rows from f‰J yi q Š, i ˆ 1, 2, . . . ,Lg K K yK corresponding to each yi 2 y , we form the matrix ‰J q Š, which is a Jacobian relating nongeneralized joint coordinates to generalized joint coordinates. By using virtual work principle, the system's dynamic model is in the form iT h iT h   tK t q ˆ J yq D…y † ‡ J uq T P ÿ J yK q y

…7†

T y2 T yL T Where ‰J yq ŠT ˆ ‰‰Jy1 y† ˆ ‰DT1 …yy1 †, DT2 …y2 †, . . . ,DTL …yyL †ŠT , here, Di …yyi † ˆ q Š , ‰Jq Š , . . . ,‰J q Š Š, D…y T ‰Mi …yyi †Šy i ‡yyi ‰Ni …yyi †Šy_ i ‡ gi …yyi † is the dynamic load of the ith arm. This dynamic model establishes directly the dynamic mapping between the object space and joint space, and involves joint driving torques, object dynamic load and individual arm's dynamic loads of the system. The objective is to distribute these dynamic loads over the joint space, that is to select t q and t K y in Eq. (7) to perform the object motion, and, in the meantime to satisfy some joint torque based criteria.

3. Load distribution The objective considered in this work is minimum weighted joint torque norm. First consider as the objective the minimum of t Ty t y : Expressing t Ty t y in terms of t q and t K y yields  fobj ˆ

tq tK y

T 

tq tK y



T

K ˆ t Tq t q ‡ t K y ty

Substituting Eq. (7) into Eq. (8) yields

…8†

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

fobj ˆ

h

J yq

iT

iT h   D…y † ‡ J uq T P ÿ J yK tK q y

T h

J yq

iT

iT h   D…y † ‡ J uq T P ÿ J yK tK q y

989



T

K ‡ tK y ty

…9†

Equating the derivative of Eq. (9) with respect to t K y to zero yields  h   ÿ1 h ih iT i h iT  K  u T yK yK yK y Jq Jq J q D…y † ‡ J q P t y opt ˆ J q ‡‰ I Š T tq gopt , then ftty gTopt ˆ ffttK tq gTopt g: Substituting fttK y gopt into Eq. (7) yields ft y gopt ;ft The weighted squared sum of the joint torques can be represented by   2  K
2 ÿ 
2 ÿ t ˆ Wq t q ‡ W K fobj ˆ ‰W Š qK ty y ty

…10†

…11†

Where ‰W Š is a diagonal weighting matrix, and "  # Wq 0 ‰W Š ˆ  K 0 Wy Substituting Eq. (7) into Eq. (11) yields   iT T  h iT h   h y iT  u T   yK fobj ˆ Wq Wq tK J q D…y † ‡ J q P ÿ J q J yq D…y † ‡ J uq T P y ÿ

h

J yK q

iT

 tK y

ÿ  K
T ÿ K  K
‡ WK W y ty y ty

The optimal values of fttK y gopt are found to be h ÿ1 h i i     K T  h yK iT  K T  K  T yK  J yK Wq Wq ‡ Wy Wy t y opt ˆ J q Wq Wq J q q 

h

J yq

iT

  D…y † ‡ J uq T P



…12†

…13†

Through selecting the entries of weighting matrix ‰Wq Š and ‰W K tK y Š, the values of ft y gopt and fttq gopt can be adjusted accordingly. For instance, if the entries on the diagonals of ‰Wq Š and K ‰W K y Š are the joint velocities corresponding to the components of t q and t y , the objective is the least square of the instantaneous input power of the system.

4. Numerical example The case is two PUMA262 manipulators (its mechanism parameters are shown in Table 1)

990

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

Table 1 Mechanism parameters of PUMA262 Linkage i

aiÿ1 (mm)

1 2 3 4 5 6

0 0 198.1 19.1 0 0

aiÿ1 (8)

di (mm)

yi (8)

0 ÿ90 0 ÿ90 90 ÿ90

0 73 51 203 0 0

90 0 ÿ90 0 0 0

Joint rotating scope (8) 308 314 292 578 244 534

cooperatively handling an object (Fig. 2). The object is a solid disk, its diameter is 180 mm, thickness is 10 mm, and density is 1  103 Kg/m3. We assume the grasping is rigid. The moving starting point of the object in the reference frame is 2 3 1 0 0 0 60 1 0 0 7 6 7 4 0 0 1 547:2 5 0 0 0 1 the end point is 2 1 0 0 60 1 0 6 40 0 1 0 0 0

3 100 ÿ100 7 7 350 5 1

For convenience, the path of the object is generated by the interpolating in the joint space. The time period is 10 s. The inertia of the linkages is neglected for the sake of reducing the work of derivation and programming. While in the process of numerical calculation, we take weighting matrix ‰W Š in Eq. (13) as

Fig. 2. Diagram of two cooperating PUMA262.

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

991

unit matrix and the diagonal matrix composed by the power of angular velocity of corresponding joint, respectively. The related results are corresponding to either least square of input torque criterion or least square of input power criterion, respectively. Figs. 3±8 show the variations of each joint input torque of the ®rst manipulator in line with these two criteria, and also in line with the case that it handles the object by itself, and Figs. 9±14 show the variations of each joint input torque of the second manipulator in line with these two criteria, while the object is moving along the given path and in the given period. It is evident that most of the time the input torque corresponding to the least input torque criterion is less than the one corresponding to the least input power criterion, and the each joint input torque needed for two manipulators cooperatively handling the object is much less than the one needed while the object is handled by single manipulator.

Fig. 3. Variations of ®rst joint input torque of ®rst manipulator.

Fig. 4. Variations of second joint input torque of ®rst manipulator.

Fig. 5. Variations of third joint input torque of ®rst manipulator.

992

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

Fig. 6. Variations of fourth joint input torque of ®rst manipulator.

Fig. 7. Variations of ®fth joint input torque of ®rst manipulator.

Fig. 8. Variations of sixth joint input torque of ®rst manipulator.

Fig. 9. Variations of ®rst joint input torque of second manipulator.

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

Fig. 10. Variations of second joint input torque of second manipulator.

Fig. 11. Variations of third joint input torque of second manipulator.

Fig. 12. Variations of ®fth joint input torque of second manipulator.

993

994

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

Fig. 13. Variations of fourth joint input torque of second manipulator.

Fig. 14. Variations of sixth joint input torque of second manipulator.

Fig. 15. Variations of the system's absolute value of input power.

Fig. 15 shows the variations of the system's absolute value of input power in line with these two criteria. It is evident that the input power under the least input power criterion is de®nitely lowered. 5. Conclusions A uni®ed approach to the modeling and load distribution of the general case of L cooperating robot arms rigidly manipulating a common object has been presented. The formulations of the object and arms dynamic loads distribution over the joint space have been made in line with the joint torque-based criteria. The ideas of this work can also apply to the

Y.-S. Zhao et al. / Mechanism and Machine Theory 35 (2000) 985±995

995

cases of multiple ®ngers grasping a common load, and also multilegged walking machine in contact with the ground. References [1] J.Y.S. Luh, Y.F. Zhen, Computation of input generalized forces for robots with closed kinematic chain mechanisms, IEEE Journal of Robotics and Automation RA-1 (1985) 95±103. [2] R. Freeman, D. Tesar, Dynamic modeling for the overconstrained mode of coordinated multiple-manipulator operation, in: Proc. Tenth U. S. National Congress of Applied Mechanics, 1986, pp. 291±303. [3] Y. Nakamura, K. Nagai, T. Yoshikawa, Mechanics of coordinative manipulation by multiple robotic mechanisms, in: Proc. IEEE Conf. on Robotics and Automation, 1987, pp. 991±998. [4] A. Freeman, Dynamic modeling of robotic linkage systems in terms of arbitrary generalized coordinates, in: Proc. IEEE Systems, Man, and Cybernetics Conf, 1987, pp. 787±797. [5] T. Yoshikawa, K. Nagai, Manipulating and grasping forces in manipulation by multi-®ngered hands, in: Proc. IEEE Conf. on Robotics and Automation, 1987, pp. 1998±2004. [6] F. Zheng, J.Y.S. Luh, Optimal load distribution for two industrial robots handling a single object, in: Proc. IEEE Conf. on Robotics and Automation, 1988, pp. 344±349. [7] H. Suh, K.G. Shin, Coordination of dual robot arms using kinematic redundancy, in: Proc. IEEE Conf. on Robotics and Automation, 1988, pp. 504±549. [8] I.D. Walker, S.I. Marcus, Distribution of dynamic loads for multiple cooperating robot manipulators, Journal of Robotic Systems 6 (1) (1989) 35±47. [9] Z.X. Li, P. Hsu, S.K. Sastry, Grasping and coordinated manipulation by a multi®ngered robot hand, The Int. J. of Robotics Research 8 (4) (1989) 33±50. [10] R.D. Walker, R.A. Freeman, S.I. Marcus, Analysis of motion and internal loading of objects grasped by multiple cooperating manipulators, The Int. J. of Robotics Research 10 (4) (1991) 396±407. [11] P. Chiacchio, S. Chiaverini, L. Sciavicco, B. Siciliano, Task space dynamic analysis of multiarm system con®gurations, The Int. J. of Robotics Research 10 (6) (1991) 708±715. [12] P. Chiacchio, S. Chiaverini, L. Sciavicco, B. Siciliano, Global task space manipulability ellipsoids for multiplearm systems, IEEE Transactions on Robotics and Automation 7 (5) (1991) 678±684. [13] Z.F. Li, T.J. Tarn, A.K. Bejczy, Dynamic workspace analysis of multiple cooperating robot arms, IEEE Transactions on Robotics and Automation 7 (5) (1991) 589±596. [14] I.D. Walker, Impact con®gurations and measures for kinematically redundant and multiple armed robot systems, IEEE Transactions on Robotics and Automation 10 (5) (1994) 670±683. [15] Z. Huang, Y.S. Zhao, The accordance and optimization-distribution equations of the over-Determinate inputs of walking machine, Int. J. of Mechanism and Machine Theory 29 (2) (1994) 327±332.