- Email: [email protected]
Load distribution and joint trajectory planning of coordinated manipulation for two redundant robots
PERGAMON
Mechanism and Machine Theory 34 (1999) 1155±1170
www.elsevier.com/locate/mechmt
Load distribution and joint trajectory planning of coordinated manipulation for two redundant robots Jing Zhao *, Shi-Xian Bai Department of Basic Science, Beijing Polytechnic University, Beijing 100022, People's Republic of China Received 23 June 1998
Abstract In this article, the problems of load distribution and joint trajectory planning in the coordinated manipulation of two redundant robots are researched. First, three optimal schemes, minimum load method, minimum torque method and multiple criterion method, are proposed. Because the quadratic programming is avoided in these methods, the computation amount is reduced greatly so that they are feasible for real-time control. Besides, the computation stability and global characteristics of these methods are improved obviously by using multiple criteria (including joint accelerations). Then, a simulation example for two planar 3R redundant robots handling a single object is presented and simulation results are analysed intensively. # 1998 Elsevier Science Ltd. All rights reserved.
1. Introduction For many operation tasks in industrial production, a single robot is not competent, while the coordinated manipulation of two or more robots is always necessary. For example, carrying a heavy or a ¯exible object, assembling complex parts and sawing etc. For the manipulation of a single robot, two-level control of joints and arms is enough. But for the coordinated manipulation of two robots, additional coordinating control must be added. That is to say, during the entire execution of the robot's task, the constraint relations between robots and a grasped object should be kept. Therefore, the dimensions and coupling degree of kinematic and dynamic equations for the coordinating robotic system will increase greatly. Recently, the research on coordinated manipulation of robots has been very active. Around this ®eld, many * Corresponding author. Tel.: 010 67391702; e-mail: by [email protected] 0094-114X/99/$ - see front matter # 1998 Elsevier Science Ltd. All rights reserved. PII: S 0 0 9 4 - 1 1 4 X ( 9 8 ) 0 0 0 6 4 - 0
1156
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
scholars have done a lot of research and have made much progress. For instance, Reference [1] studied the constraint relations of cooperated motion for two robots, and derived the constraint equations of position, orientation and velocity. Reference [2] extended a position/ force hybrid control scheme for a single robot to the coordinating control of two robots, and presented kinematics and statics formulas of coordinating robots. Reference [3] discussed the load distribution issue for two coordinating robots holding a single object, and two optimal distribution schemes, minimum torque method and minimum load method were proposed. In Ref. [4], load distribution, singularity, manipulability and force ellipsoid were investigated theoretically for two coordinating, redundant robots. Under the condition of known load distribution, Ref. [5] discussed the optimal torque control problem for a system of multiple coordinated redundant manipulators; a local optimal control and global optimal control with minimum joint torque were proposed and a simulation example was also given. References [6, 7] presented a two-level hierarchical control scheme which is suitable for real-time application. Reference [8] discussed dynamics control and dynamics simulation problems for two coordinating redundant manipulators. It is not dicult to ®nd that the above researches focused mainly on non-redundant robots. Although some of them considered redundant robots as objects of study, they discussed the problems of load distribution and joint trajectory planning separately. In fact, when two redundant robots perform a coordinated manipulation, load distribution and joint trajectory planning interact since there are both dynamic and kinematic redundancy in the robotic system. This paper will study the problems of load distribution and joint trajectory planning by taking their interaction into account. Firstly, the kinematic and dynamic constraint equations for a robotic system are established. Then, three optimal schemes, minimum load method, minimum torque method and multiple criterion method are proposed. Finally, the three methods are analysed and compared intensively using a simulation example.
2. Constraint equations When two robots grasp a common rigid object, the robots and the object constitute a closedchain system.In order to realize coordinated manipulations certain constraint relations between the grasped object and the robots should be followed. Next, we will study these constraint relationsand develop the dynamic equations of the robots and object. Firstly, the coordinate systems of a cooperating robotic system is set up as shown in Fig. 1. Where, B1X B1Y B1Z B1 is the base frame C1X C1Y C1Z C1 and C2X C2Y C2Z C2 are contact frames of end-eectors and object OX0Y0Z0 is object frame (O is the object's center of gravity, (c.g.);coordinate axes coincide with the principal axes of the object). For convenience sake, the two robots are called master robot and slave robot respectively, whose relative parameters are denoted by the subscript ``1'' and ``2''. Suppose that the two robots are identical, each with n joint DOF (degrees of freedom) and m end-eector parameters (for spatial robots, m = 6 for; for planar robots, m = 3) and the end-eectors can grasp the object tightly so that there is no relative motion between the end-eectors and the object.
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1157
Fig. 1. A coordinating robotic system.
2.1. Object and master robot According to Fig. 1, position constraint equations between the object and the master robot can be decided ®rstly. Then, dierentiating the equations with respect to time, it yields corresponding velocity constraint equations P_ 0 J_ L1 y_ 1 Ly_ 1
1
where P_ 0 2 R31 absolute velocity vector of object's c:g:; y_ 1 2 Rn1 master robot joint velocity vector; JL1 2 R3n position Jacobian matrix of master robot;
:
L @fC PCo g=@y1 Pco 2 R31 position vector of object's c:g: in the contact frame of master robot; C 2 R33 rotation transformation matrix between the contact frame of master robot and the base frame: Due to no motion between the end-eectors and the object,their angular velocity should be equal, that is o 0 JA1 y_ 1
2
1158
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
where o 0 2 R31 absolute angular velocity vector of the object about its principal axes; JA1 2 R3n orientation Jacobian matrix of master robot: Combining Eqs. (1) and (2), we get JL1 _ P_ 0 Ly_ 1 y : JA1 1 o0 0
3
Therefore, the velocity constraint equations of the object and master robot may be written in compact form _0 X _C T X 1 where _0 X
P_ 0 o0
4
2 R61
is the absolute velocity vector of the object; _ C J1 y_ 1 2 R61 X 1 is the absolute velocity of the end - eector of master robot; JL1 2 R6n J1 JA1 is the Jacobian matrix of master robot; Ly_ 1 2 R61 T 0 It is worth pointing out that T is determined once the motion of the object is known. 2.2. Master robot and slave robot Supposing that the object is symmetrical, i.e. PC1 C2 2PC1 O , and considering the above supposition that there is no relative motion between the end-eectors and the object, we can obtain the joint velocity constraint equations between the master robot and the slave robot _ C 2T X _C X 1 2 where _ C2 J2 y_ 2 2 R61 X is the absolute velocity vector of end-eector of slave robot;
5
J2
JL2 JA2
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1159
2 R6n
is the Jacobian matrix of slave robot; y_ 2 2 Rn1 is the joint velocity vector of slave robot. For two coordinating robots their velocity equations are _ C Jy_ X
6
where
_ C1 X _ XC _ 2 R121 XC2
is the absolute velocity vector of the end-eectors of master and slave robots; J1 0 J 2 R122n 0 J2 is the Jacobian matrix of master and slave robots _y y_ 1 2 R2n1 y_ 2 is the joint velocity vector of master and slave robot. Dierentiating Eq. (6) with respect to time, we obtain acceleration equations C Jy J_ y_ X
7
where
C1 X XC 2 R121 XC1
is the absolute acceleration vector of the end-eectors of master and slave robots; y y 1 2 R2n1 y 2 is the joint acceleration vector of master and slave robots. In addition, dierentiating Eqs. (4) and (5) with respect to time and rearranging them, we express the acceleration relations between the end-eectors and the object as 0 ÿ T_ X 8 XC X0 T_ where
1160
0 X
P 0 _0 o
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
2 R61
is the absolute acceleration vector of the object (linear and angular accelerations). The above derived results show that Eqs. (4), (5) and (8) are the constraint equations of the robot±object system. When these constraint equations are satis®ed,the two robots can perform coordinated manipulations. Generally speaking, in order to perform coordinated manipulations, the joint motion of the master robot should be determined ®rstly by using the constraint equations between the object and the master robot according to the desired motion of the object. Then, the corresponding joint motion of the slave robot can be decided by using the constraint equations between the master robot and the slave robot. If the both robots are redundant, i.e. n > 6, Eq. (4) shows that the joint velocity of the master robot y_ 1 has a in®nite number of _ 0 and o. The joint motion of the master robot can be solutions for the speci®ed object's motion X solved uniquely through ®rst redundancy resolution based on a performance criterion (minimum joint velocity or singularity avoidance). Then, taking the results into Eq. (5) to make second redundancy resolution, we can ®nally determine the robots' coordinated motion. Obviously, it is a kinematically coordinated motion. It is seen from this, that, like a single redundant robot, the two coordinating redundant robots not only can accomplish operating task, but also can get optimal joint con®guration by a criterion to improve the robot's performance.
3. Dynamic equations For the robot's coordinated manipulation, kinematic coordination is fundamental. Besides, dynamic coordination should be also considered. The dynamic coordination relations between the robots and the object are always expressed by their dynamic equations. 3.1. Robots According to Lagrange's equation, the dynamic equations of the coordinating robots in the joint space may be expressed as [9] Di y Hi Gi JTi Fi ti
i 1; 2
where Di 2 Rnn symmetric inertial matrix of a robot; Hi 2 Rn1 coriolis and centrifugal force vector of a robot; Gi 2 Rn1 gravity vector of a robot; Fi 2 Rm1 force vector applied to the object by a robot; t i 2 Rn1 join torque vector of a robot;
9
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1161
or in compact form Dy H G JT F t where
D1 D 0
F1 F F2
0 D2
2R
2R 2m1
10
2n2n
;
t ;t 1 t2
H1 H H2
2R
2n1
G1 ;G G2
2 R2n1
2 R2n1
3.2. Object When an object is held rigidly by two robots, its dynamic equations can be written as M 0 P 0 G F1 F2 0 I o_ 0 o0 o 0 Io
11
where M diagm0 ; m0 ; m0 2 R33 mass matrix of the object; I diagIx ; Iy ; Iz 2 R33 principal moments of inertia of the object; P 0 2 R31 acceleration vector of the object's c:g:; o_0 2 R31 angular acceleration vector of the object about its principal axes; G 0; m0 g; 0 2 R31 gravity vector of the object: Since the motion of the object is always known during coordinated maipulations, the left side of Eq. (11) can be calculated. Now we denote the left side of Eq. (11) as R, and rewrite Eq. (11) as R WF
12
where R 2 R61 resultant forces applied to the object; W 2 R612 load matrix: The load matrix W can be determined uniquely once the object's orientation and relative position between the object's c.g. and contact point are known. Combining Eqs. (10) and (12) results in the dynamic coordination equations of the two robots. When the joint motions of the
1162
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
robots are calculated, the above equations contain 2n + 6 scalar equations, but contain 2n + 12 unknowns in t1, t2, F1 and F2. Thus, the equations are a set of under-speci®ed equations, which have in®nitely many solutions of joint torques and forces applied to end-eectors. We call this situation dynamic redundancy. Similar to kinematic redundancythe dynamic redundancy can also be resolved using a performance criterion.
4. Load distribution and joint trajectory planning When two robots hold a common object, the object exerts forces (torques) on their endeectors. If the forces can be calculated, the two coordinating robots may be dealt with as two single robots related by the kinematic constraints. Determining the component forces applied to each robot's end-eector in terms of the resultant force exerted on the object (calculated from the object's motion) is called load distribution which is one of the important problems of coordinating robots. Since the coordination problem for non-redundant robots only contains dynamic redundancy, the load distribution is only concerned with dynamic redundancy resolution. While the coordination problem for redundant robots contains both dynamic redundancy and kinematic redundancy. Therefore, the load distribution will be aected by both the dynamic and kinematic redundancy. This condition will complicate the coordination problem, but at the same time, it also provides us with a greater space to optimize load distribution and joint trajectory. For convenience in the discussion, we rewite the dynamic coordination equations of the redundant robotic system as follows Dy H G JT F t C 0; Jy J_ y_ ÿ X R WF:
13a 13b 13c
Eq. (13) consists of 2n + 18 scalar equations and 4n + 12 unknowns in y , t and F. When 2n + 18>4n + 12, the equations have a in®nite number of solutions. Using a performance criterion as an objective function, we can determine an optimal solution of the equations. In the following, we will discuss three optimal schemes by combining load distribution and joint trajectory planning. 4.1. Minimum load method (ML method) In Eq. (13c), the component load (force) F has an in®nite number of solutions for a given resultant force R exerted on the object. If the square-sum of F is taken as an objective functionand W is of full rank, the component load can be determined uniquely so as to realize the optimal load distribution in least norm sense. In terms of the Moore±Penrose generalized inverse, the component load F may be represented by
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
F W R
1163
14
where W+ = WT(WWT)-1 $ R 12 6; Moore±Penrose generalized the inverse of W. In this case, F is independent of joint con®guration. Substituting Eq. (14) into Eq. (13c), we then can proceed to joint trajectory planning. In order to minimize joint torques, we use multiple criteria to make an objective function as follows Z C1 t T t C2 y T y :
15
In consideration of the kinematic constraint in Eq. (13b) the optimization problem can be expressed as Min Z C1 t T t C2 y T y
16
C 0: Sub:to Jy J_ y_ ÿ X
17
Introducing Lagrange multipliers l = [l1, l2, . . ., l m]T $ R m 1, the optimization problem becomes C : Min Z 0 t T t l T Jy J_ y_ ÿ X To minimize Z 0 , following conditions must be satis®ed 8 0 > < @Z 0 @y > : @Z 0 0: @l l
18
19
Solving Eq. (19) results in _ y J B X ÿ Jy I ÿ JB JBA
20
where T T ÿ1 2 R2n2m weighted generalized inverse of Jacobian; J B BJ JBJ
I 2 R2n2n identity matrix; B 2C1 D2 C2 Iÿ1 2 R2n2n ; A ÿ2C1 DH G JT W R 2 R2n1 ; C1 ; C2 weight factors: Letting C2 = 0 in Eq. (21) will give the least norm method (LN method) which can realize the joint trajectory planning with minimum acceleration. This redundancy resolution is in the kinematic sense.
1164
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
4.2. Minimum torque method (MT method) In Eq. (13) taking joint torque as a objective function and as design variables and considering Eqs. (13b) and (13c) as constraint equations, we can realize the load distribution and joint trajectory planning with minimum torques simultaneously. The optimization problem may be described as Min Z C1 t C2 y T y
21
C 0 Sub:to Jy J_ y_ ÿ X
22
R WF:
23
Through the same derivation as the above, one can obtain F PU ÿ PWT PW WPU ÿ R; y 2C1 BDfH G JT PU ÿ PWT Pw WPU ÿ Rg ÿ BJT Jÿ1
24
B
C ÿ 2C1 JBDfH G JT PU ÿ PWT Pw WPU ÿ Rgg fJ_ y_ ÿ X
25
where T 2 T ÿ1 P 2C1 JJT 4C21 JD JT Jÿ1 B JBDJ ÿ 4C1 JD DJ ;
U 4C21 JD D H G ÿ 4C21 JD JT Jÿ1 B JBD H G __ 2C1 JD JT Jÿ1 B Jy ÿ XC ÿ 2C1 J H G; Pw WPWT ÿ1 ; B 2C1 D2 C2 Iÿ1 ; JB JBJT ; JD JDB:
4.3. Multiple-criterion method (MC method) Based on the MT method, we present another optimal scheme, i.e. multiple-criterion method. In this method, a weighted linear combination of joint torques and component loads is used as an objective function to make load distribution and joint trajectory planning. Similar to Eqs. (21)±(23), one can achieve Min Z C1 t T t C2 y T y C3 FT F
26
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1165
C 0 Sub:to Jy J_ y_ ÿ X
27
R WF:
28
Solving the optimization problem through the above procedure yields F and y in the same expression as Eqs. (24) and (25), but some notations' meaning is modi®ed as follows B 2C1 D2 C2 Iÿ1 T 2 T ÿ1 P 2C1 JJT 2C3 I 4C21 JD JT Jÿ1 B JBDJ ÿ 4C1 JD DJ
C3 is a weight factor. Comparing the above three methods indicates that the ML method is a separate optimization method of load distribution and joint torque. In this method, the load distribution is determined ®rstly by using the component loads as design variables and their square-sum as an objective function. Then, the joint trajectory planning with minimum torque is conducted with the help of joint self-motion (choose a joint con®guration to minimize the required torques). Obviously, the load distribution and joint torque optimization in the method are independent of each other. The ML method has less computation, and can give smaller component loads, but the corresponding joint torques are relatively large. The MT method is a synchro optimization method of load distribution and joint torque. In this method, load distribution and joint trajectory are optimized simultaneously by taking the component load and joint acceleration as design variables, and the square-sum of joint torques as an objective function. For the MT method, joint torques are smaller, while the component load and computation amount are relatively great. The MC method is actually a compromise scheme of the above two methods, which is also a synchro method. Since this method utilizes the weighted linear combination of joint torque and component load as an objective function, the joint torque and the component load are all relatively smaller.
5. Simulation analysis and conclusion To show the performance of the proposed methods, we will present a simulation example for two planar 3R robots holding a common object shown in Fig. 2. The hard point contact between the object and the end-eectors is assumed. Thus, forces are only transmitted from the the end-eectors to the object. The gravity of the system is also neglected. Since only position parameters are considered here, i.e. n ÿ m = 1, the two planar 3R robots can be regarded as redundant robots. The joint displacement vectors of the two robots are denoted as, respectively, y1 = [y 11, y 12, y 13]T, y2 = [y 21, y 22, y 23]T. In terms of Eq. (8), the robot's motions are expressed as P 0 ÿ T_ XC 29 P0 T_
1166
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
Fig. 2. Two planar 3R robots holding a common object.
where T_
a cos j ÿa sin j 2 f f; ÿa sin j ÿa cos j
j is the rotation angle of the object (Fig. 2). If the object's motion is known, the exerted resultant force R may be represented as T R m0 P 0x ; m0 P 0y m0 g; Iz j Load matrix W is 2 1 0 1 0 4 W 0 1 0 1 : ÿa cos j a sin j a cos j ÿa sin j Further to obtain 2 1 0 62 6 60 1 6 2 W 6 61 6 62 0 4 0 1 2
the Moore±Penrose generalized inverse of W cos j 3 2a 7 sin j 7 7 2a 7 7: cos j 7 7 2a 7 5 sin j 2a
For further details about matrixes J, D, H and G, please refer to Ref. [9].
30
31
32
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1167
Table 1. Structural parameters and initial con®guration
Master robot Slave robot
Link length (m) l1 l2
l3
Link mass (kg) m1 m2
m3
Initial conf. (deg) y1 y2
y3
0.5 0.5
0.5 0.5
1 1
1 1
ÿ 45 60
60 ÿ 120
0.5 0.5
1 1
45 ÿ 60
Fig. 3. Joint con®gurations: (a) the ML method; (b) the MT method; (c) the MC method; (d) the LN method.
1168
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
In simulation calculations, the structural parameters and the initial con®gurations of the two robots are shown in Table 1. Simulation time T = 2(s), weight factors C1 = 1, C2 = 0.5, C3 = 1, object mass mo = 1 kg, object lenth 2a = 0.5 m, object motion PÈo = [0.3 sin(2pt/T), 1.2 sin(2pt/T)]T m/s2,j = jÇ = 0, j = p/2 (without rotation). Figures 3±5 are simulation results of joint con®guration, load and joint torque in the coordinated manipulation of the two robots. For convenience, the comparisons of the three methods are gathered in Table 2. Fig. 3(a)±(c) indicates, that there is no clear dierence among the joint trajectories obtained from the three proposed methods. The dierence is mainly from the fact that the three methods all take the joint torque as an objective funtion. Comparing the results with Fig. 3(d), which is the simulation result of the LN method, we can ®nd the joint trajectories have relatively obvious dierences. Table 2 shows for the load performance index the ML method is the best, the MC method takes second place, and the MT method is poor; for the torque performance index, the MT method is the best, the MC method takes second place, and the ML method is poor. Besides, the load and joint torque of the MC method are only slightly greater than those of the other two methods,which indicates that the comprehensive property of the MC method is optimal. From the theory of optimization, when the sum of two unknown variables is known, the square-sum of the two variables is minimized if the two variables are equal. It is because the ML method distributes the load evenly between the two robots that the square-sum of the two component loads is minimized. Therefore, in order to obtain optimal load distribution in a normal sense, the resultant load should be distribute as evenly as possible. Simulation results demonstrate that, in the MC method, the two component loads exerted on the end-eectors are very close so that their square-sum is relatively small, and, the torque dierences of the ®rst joint (of all three methods) are the most obvious. For most robots, since the ®rst joint always bears the loads of the following links and
Fig. 4. The total loads of the two robots.
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1169
Fig. 5. Total torques of the two robots.
joints, its torque is maximum and also becomes a principal factor that aects the total torques. Summing up the above comes to the following conclusions: the ML method can provide minimum loads; the MT method can give minimum joint torques; if we want both the loads and the torques all to be relatively small, the MC method can be adopted. Finally, it is worth noting the following. (1) Weight factors have some eects on the simulation results. Especially, if C2 = 0, the three methods have a very poor stability, and they always fail in simulations [9]. Besides, using the variable weight factors [9], the global characteristics of these methods can be further improved. (2) The component loads discussed Table 2. The comparisons of the three methods
Total load Robot 1 load Robot 2 load Total torque Robot 1 torque Robot 2 torque Torque 1 of robot Torque 2 of robot Torque 3 of robot Torque 1 of robot Torque 2 of robot Torque 3 of robot
1 1 1 2 2 2
ML method
MT method
MC method
small small small large large large large large large large large small
large large large small small small small small middle small middle large
middle middle middle middle middle middle middle middle small middle large middle
1170
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
here are the forces exerted on the object by the end-eectors which determine the motion of the object. Beside the forces, enough grasping must also be provided by the robots in order to keep the contact between the object and the end-eectors during the entire coordinated manipulation. The grasping forces which do not contribute to the motion of the object, can come from the null-force in the internal space, whose function is similar to that of joint selfmotion. References [1] J.Y.S. Luh, Y.F. Zheng, Constrained relations between two coordinated industrial robots for motion control, Int. J. Robotics Res. 6 (3) (1987) 60±70. [2] M. Uchiyama, N. Iwasawa, K. Hakomari, Hybrid position/force control for coordination of a two-arm robot, IEEE Int. Conf. on Robotics and Automation, Raleigh, NC (1987) 1242±1247. [3] Y.F. Zheng, J.Y.S. Luh, Optimal load distribution for two industrial robots handling a single object, IEEE Int. Conf. on Robotics and Automation, Philadelphia, PA (1988) 344±349. [4] J.M. Tao, J.Y.S. Luh, Coordination of two redundant robots, IEEE Int. Conf. on Robotics and Automation, Scotsdale, AZ (1989) 425±430. [5] Y.R. Hu, A.A. Goldenberg, Dynamic control of multiple coordinated redundant manipulators with torque optimization, IEEE Int. Conf. on Robotics and Automation, Cincinnati, OH (1990) 1000±1005. [6] F.T. Cheng, D.E. Orin, Optimal force distribution in multiple-chain robotic systems, IEEE Trans. Systems Man Cybernetics 21 (1) (1991) 13±24. [7] F.T. Cheng, D.E. Orin, Ecient formulation of the force distribution equations for simple closed-chain robotic systems, IEEE Trans. Systems Man Cybernetics 21 (1) (1991) 25±32. [8] F.T. Cheng, J.J. Lee, T.H. Chew, F.C. King, Control and simulation for a closed chain dual redundant manipulator system, J. Robotics Systems 12 (2) (1991) 119±133 [9] Z. Jing, B. Shi-Xian, The joint trajectory planning with minimal joint torque for redundant robots, Mechanical Science and Technology 16 (4) (1997) 623±626.
Mechanism and Machine Theory 34 (1999) 1155±1170
www.elsevier.com/locate/mechmt
Load distribution and joint trajectory planning of coordinated manipulation for two redundant robots Jing Zhao *, Shi-Xian Bai Department of Basic Science, Beijing Polytechnic University, Beijing 100022, People's Republic of China Received 23 June 1998
Abstract In this article, the problems of load distribution and joint trajectory planning in the coordinated manipulation of two redundant robots are researched. First, three optimal schemes, minimum load method, minimum torque method and multiple criterion method, are proposed. Because the quadratic programming is avoided in these methods, the computation amount is reduced greatly so that they are feasible for real-time control. Besides, the computation stability and global characteristics of these methods are improved obviously by using multiple criteria (including joint accelerations). Then, a simulation example for two planar 3R redundant robots handling a single object is presented and simulation results are analysed intensively. # 1998 Elsevier Science Ltd. All rights reserved.
1. Introduction For many operation tasks in industrial production, a single robot is not competent, while the coordinated manipulation of two or more robots is always necessary. For example, carrying a heavy or a ¯exible object, assembling complex parts and sawing etc. For the manipulation of a single robot, two-level control of joints and arms is enough. But for the coordinated manipulation of two robots, additional coordinating control must be added. That is to say, during the entire execution of the robot's task, the constraint relations between robots and a grasped object should be kept. Therefore, the dimensions and coupling degree of kinematic and dynamic equations for the coordinating robotic system will increase greatly. Recently, the research on coordinated manipulation of robots has been very active. Around this ®eld, many * Corresponding author. Tel.: 010 67391702; e-mail: by [email protected] 0094-114X/99/$ - see front matter # 1998 Elsevier Science Ltd. All rights reserved. PII: S 0 0 9 4 - 1 1 4 X ( 9 8 ) 0 0 0 6 4 - 0
1156
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
scholars have done a lot of research and have made much progress. For instance, Reference [1] studied the constraint relations of cooperated motion for two robots, and derived the constraint equations of position, orientation and velocity. Reference [2] extended a position/ force hybrid control scheme for a single robot to the coordinating control of two robots, and presented kinematics and statics formulas of coordinating robots. Reference [3] discussed the load distribution issue for two coordinating robots holding a single object, and two optimal distribution schemes, minimum torque method and minimum load method were proposed. In Ref. [4], load distribution, singularity, manipulability and force ellipsoid were investigated theoretically for two coordinating, redundant robots. Under the condition of known load distribution, Ref. [5] discussed the optimal torque control problem for a system of multiple coordinated redundant manipulators; a local optimal control and global optimal control with minimum joint torque were proposed and a simulation example was also given. References [6, 7] presented a two-level hierarchical control scheme which is suitable for real-time application. Reference [8] discussed dynamics control and dynamics simulation problems for two coordinating redundant manipulators. It is not dicult to ®nd that the above researches focused mainly on non-redundant robots. Although some of them considered redundant robots as objects of study, they discussed the problems of load distribution and joint trajectory planning separately. In fact, when two redundant robots perform a coordinated manipulation, load distribution and joint trajectory planning interact since there are both dynamic and kinematic redundancy in the robotic system. This paper will study the problems of load distribution and joint trajectory planning by taking their interaction into account. Firstly, the kinematic and dynamic constraint equations for a robotic system are established. Then, three optimal schemes, minimum load method, minimum torque method and multiple criterion method are proposed. Finally, the three methods are analysed and compared intensively using a simulation example.
2. Constraint equations When two robots grasp a common rigid object, the robots and the object constitute a closedchain system.In order to realize coordinated manipulations certain constraint relations between the grasped object and the robots should be followed. Next, we will study these constraint relationsand develop the dynamic equations of the robots and object. Firstly, the coordinate systems of a cooperating robotic system is set up as shown in Fig. 1. Where, B1X B1Y B1Z B1 is the base frame C1X C1Y C1Z C1 and C2X C2Y C2Z C2 are contact frames of end-eectors and object OX0Y0Z0 is object frame (O is the object's center of gravity, (c.g.);coordinate axes coincide with the principal axes of the object). For convenience sake, the two robots are called master robot and slave robot respectively, whose relative parameters are denoted by the subscript ``1'' and ``2''. Suppose that the two robots are identical, each with n joint DOF (degrees of freedom) and m end-eector parameters (for spatial robots, m = 6 for; for planar robots, m = 3) and the end-eectors can grasp the object tightly so that there is no relative motion between the end-eectors and the object.
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1157
Fig. 1. A coordinating robotic system.
2.1. Object and master robot According to Fig. 1, position constraint equations between the object and the master robot can be decided ®rstly. Then, dierentiating the equations with respect to time, it yields corresponding velocity constraint equations P_ 0 J_ L1 y_ 1 Ly_ 1
1
where P_ 0 2 R31 absolute velocity vector of object's c:g:; y_ 1 2 Rn1 master robot joint velocity vector; JL1 2 R3n position Jacobian matrix of master robot;
:
L @fC PCo g=@y1 Pco 2 R31 position vector of object's c:g: in the contact frame of master robot; C 2 R33 rotation transformation matrix between the contact frame of master robot and the base frame: Due to no motion between the end-eectors and the object,their angular velocity should be equal, that is o 0 JA1 y_ 1
2
1158
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
where o 0 2 R31 absolute angular velocity vector of the object about its principal axes; JA1 2 R3n orientation Jacobian matrix of master robot: Combining Eqs. (1) and (2), we get JL1 _ P_ 0 Ly_ 1 y : JA1 1 o0 0
3
Therefore, the velocity constraint equations of the object and master robot may be written in compact form _0 X _C T X 1 where _0 X
P_ 0 o0
4
2 R61
is the absolute velocity vector of the object; _ C J1 y_ 1 2 R61 X 1 is the absolute velocity of the end - eector of master robot; JL1 2 R6n J1 JA1 is the Jacobian matrix of master robot; Ly_ 1 2 R61 T 0 It is worth pointing out that T is determined once the motion of the object is known. 2.2. Master robot and slave robot Supposing that the object is symmetrical, i.e. PC1 C2 2PC1 O , and considering the above supposition that there is no relative motion between the end-eectors and the object, we can obtain the joint velocity constraint equations between the master robot and the slave robot _ C 2T X _C X 1 2 where _ C2 J2 y_ 2 2 R61 X is the absolute velocity vector of end-eector of slave robot;
5
J2
JL2 JA2
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1159
2 R6n
is the Jacobian matrix of slave robot; y_ 2 2 Rn1 is the joint velocity vector of slave robot. For two coordinating robots their velocity equations are _ C Jy_ X
6
where
_ C1 X _ XC _ 2 R121 XC2
is the absolute velocity vector of the end-eectors of master and slave robots; J1 0 J 2 R122n 0 J2 is the Jacobian matrix of master and slave robots _y y_ 1 2 R2n1 y_ 2 is the joint velocity vector of master and slave robot. Dierentiating Eq. (6) with respect to time, we obtain acceleration equations C Jy J_ y_ X
7
where
C1 X XC 2 R121 XC1
is the absolute acceleration vector of the end-eectors of master and slave robots; y y 1 2 R2n1 y 2 is the joint acceleration vector of master and slave robots. In addition, dierentiating Eqs. (4) and (5) with respect to time and rearranging them, we express the acceleration relations between the end-eectors and the object as 0 ÿ T_ X 8 XC X0 T_ where
1160
0 X
P 0 _0 o
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
2 R61
is the absolute acceleration vector of the object (linear and angular accelerations). The above derived results show that Eqs. (4), (5) and (8) are the constraint equations of the robot±object system. When these constraint equations are satis®ed,the two robots can perform coordinated manipulations. Generally speaking, in order to perform coordinated manipulations, the joint motion of the master robot should be determined ®rstly by using the constraint equations between the object and the master robot according to the desired motion of the object. Then, the corresponding joint motion of the slave robot can be decided by using the constraint equations between the master robot and the slave robot. If the both robots are redundant, i.e. n > 6, Eq. (4) shows that the joint velocity of the master robot y_ 1 has a in®nite number of _ 0 and o. The joint motion of the master robot can be solutions for the speci®ed object's motion X solved uniquely through ®rst redundancy resolution based on a performance criterion (minimum joint velocity or singularity avoidance). Then, taking the results into Eq. (5) to make second redundancy resolution, we can ®nally determine the robots' coordinated motion. Obviously, it is a kinematically coordinated motion. It is seen from this, that, like a single redundant robot, the two coordinating redundant robots not only can accomplish operating task, but also can get optimal joint con®guration by a criterion to improve the robot's performance.
3. Dynamic equations For the robot's coordinated manipulation, kinematic coordination is fundamental. Besides, dynamic coordination should be also considered. The dynamic coordination relations between the robots and the object are always expressed by their dynamic equations. 3.1. Robots According to Lagrange's equation, the dynamic equations of the coordinating robots in the joint space may be expressed as [9] Di y Hi Gi JTi Fi ti
i 1; 2
where Di 2 Rnn symmetric inertial matrix of a robot; Hi 2 Rn1 coriolis and centrifugal force vector of a robot; Gi 2 Rn1 gravity vector of a robot; Fi 2 Rm1 force vector applied to the object by a robot; t i 2 Rn1 join torque vector of a robot;
9
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1161
or in compact form Dy H G JT F t where
D1 D 0
F1 F F2
0 D2
2R
2R 2m1
10
2n2n
;
t ;t 1 t2
H1 H H2
2R
2n1
G1 ;G G2
2 R2n1
2 R2n1
3.2. Object When an object is held rigidly by two robots, its dynamic equations can be written as M 0 P 0 G F1 F2 0 I o_ 0 o0 o 0 Io
11
where M diagm0 ; m0 ; m0 2 R33 mass matrix of the object; I diagIx ; Iy ; Iz 2 R33 principal moments of inertia of the object; P 0 2 R31 acceleration vector of the object's c:g:; o_0 2 R31 angular acceleration vector of the object about its principal axes; G 0; m0 g; 0 2 R31 gravity vector of the object: Since the motion of the object is always known during coordinated maipulations, the left side of Eq. (11) can be calculated. Now we denote the left side of Eq. (11) as R, and rewrite Eq. (11) as R WF
12
where R 2 R61 resultant forces applied to the object; W 2 R612 load matrix: The load matrix W can be determined uniquely once the object's orientation and relative position between the object's c.g. and contact point are known. Combining Eqs. (10) and (12) results in the dynamic coordination equations of the two robots. When the joint motions of the
1162
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
robots are calculated, the above equations contain 2n + 6 scalar equations, but contain 2n + 12 unknowns in t1, t2, F1 and F2. Thus, the equations are a set of under-speci®ed equations, which have in®nitely many solutions of joint torques and forces applied to end-eectors. We call this situation dynamic redundancy. Similar to kinematic redundancythe dynamic redundancy can also be resolved using a performance criterion.
4. Load distribution and joint trajectory planning When two robots hold a common object, the object exerts forces (torques) on their endeectors. If the forces can be calculated, the two coordinating robots may be dealt with as two single robots related by the kinematic constraints. Determining the component forces applied to each robot's end-eector in terms of the resultant force exerted on the object (calculated from the object's motion) is called load distribution which is one of the important problems of coordinating robots. Since the coordination problem for non-redundant robots only contains dynamic redundancy, the load distribution is only concerned with dynamic redundancy resolution. While the coordination problem for redundant robots contains both dynamic redundancy and kinematic redundancy. Therefore, the load distribution will be aected by both the dynamic and kinematic redundancy. This condition will complicate the coordination problem, but at the same time, it also provides us with a greater space to optimize load distribution and joint trajectory. For convenience in the discussion, we rewite the dynamic coordination equations of the redundant robotic system as follows Dy H G JT F t C 0; Jy J_ y_ ÿ X R WF:
13a 13b 13c
Eq. (13) consists of 2n + 18 scalar equations and 4n + 12 unknowns in y , t and F. When 2n + 18>4n + 12, the equations have a in®nite number of solutions. Using a performance criterion as an objective function, we can determine an optimal solution of the equations. In the following, we will discuss three optimal schemes by combining load distribution and joint trajectory planning. 4.1. Minimum load method (ML method) In Eq. (13c), the component load (force) F has an in®nite number of solutions for a given resultant force R exerted on the object. If the square-sum of F is taken as an objective functionand W is of full rank, the component load can be determined uniquely so as to realize the optimal load distribution in least norm sense. In terms of the Moore±Penrose generalized inverse, the component load F may be represented by
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
F W R
1163
14
where W+ = WT(WWT)-1 $ R 12 6; Moore±Penrose generalized the inverse of W. In this case, F is independent of joint con®guration. Substituting Eq. (14) into Eq. (13c), we then can proceed to joint trajectory planning. In order to minimize joint torques, we use multiple criteria to make an objective function as follows Z C1 t T t C2 y T y :
15
In consideration of the kinematic constraint in Eq. (13b) the optimization problem can be expressed as Min Z C1 t T t C2 y T y
16
C 0: Sub:to Jy J_ y_ ÿ X
17
Introducing Lagrange multipliers l = [l1, l2, . . ., l m]T $ R m 1, the optimization problem becomes C : Min Z 0 t T t l T Jy J_ y_ ÿ X To minimize Z 0 , following conditions must be satis®ed 8 0 > < @Z 0 @y > : @Z 0 0: @l l
18
19
Solving Eq. (19) results in _ y J B X ÿ Jy I ÿ JB JBA
20
where T T ÿ1 2 R2n2m weighted generalized inverse of Jacobian; J B BJ JBJ
I 2 R2n2n identity matrix; B 2C1 D2 C2 Iÿ1 2 R2n2n ; A ÿ2C1 DH G JT W R 2 R2n1 ; C1 ; C2 weight factors: Letting C2 = 0 in Eq. (21) will give the least norm method (LN method) which can realize the joint trajectory planning with minimum acceleration. This redundancy resolution is in the kinematic sense.
1164
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
4.2. Minimum torque method (MT method) In Eq. (13) taking joint torque as a objective function and as design variables and considering Eqs. (13b) and (13c) as constraint equations, we can realize the load distribution and joint trajectory planning with minimum torques simultaneously. The optimization problem may be described as Min Z C1 t C2 y T y
21
C 0 Sub:to Jy J_ y_ ÿ X
22
R WF:
23
Through the same derivation as the above, one can obtain F PU ÿ PWT PW WPU ÿ R; y 2C1 BDfH G JT PU ÿ PWT Pw WPU ÿ Rg ÿ BJT Jÿ1
24
B
C ÿ 2C1 JBDfH G JT PU ÿ PWT Pw WPU ÿ Rgg fJ_ y_ ÿ X
25
where T 2 T ÿ1 P 2C1 JJT 4C21 JD JT Jÿ1 B JBDJ ÿ 4C1 JD DJ ;
U 4C21 JD D H G ÿ 4C21 JD JT Jÿ1 B JBD H G __ 2C1 JD JT Jÿ1 B Jy ÿ XC ÿ 2C1 J H G; Pw WPWT ÿ1 ; B 2C1 D2 C2 Iÿ1 ; JB JBJT ; JD JDB:
4.3. Multiple-criterion method (MC method) Based on the MT method, we present another optimal scheme, i.e. multiple-criterion method. In this method, a weighted linear combination of joint torques and component loads is used as an objective function to make load distribution and joint trajectory planning. Similar to Eqs. (21)±(23), one can achieve Min Z C1 t T t C2 y T y C3 FT F
26
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1165
C 0 Sub:to Jy J_ y_ ÿ X
27
R WF:
28
Solving the optimization problem through the above procedure yields F and y in the same expression as Eqs. (24) and (25), but some notations' meaning is modi®ed as follows B 2C1 D2 C2 Iÿ1 T 2 T ÿ1 P 2C1 JJT 2C3 I 4C21 JD JT Jÿ1 B JBDJ ÿ 4C1 JD DJ
C3 is a weight factor. Comparing the above three methods indicates that the ML method is a separate optimization method of load distribution and joint torque. In this method, the load distribution is determined ®rstly by using the component loads as design variables and their square-sum as an objective function. Then, the joint trajectory planning with minimum torque is conducted with the help of joint self-motion (choose a joint con®guration to minimize the required torques). Obviously, the load distribution and joint torque optimization in the method are independent of each other. The ML method has less computation, and can give smaller component loads, but the corresponding joint torques are relatively large. The MT method is a synchro optimization method of load distribution and joint torque. In this method, load distribution and joint trajectory are optimized simultaneously by taking the component load and joint acceleration as design variables, and the square-sum of joint torques as an objective function. For the MT method, joint torques are smaller, while the component load and computation amount are relatively great. The MC method is actually a compromise scheme of the above two methods, which is also a synchro method. Since this method utilizes the weighted linear combination of joint torque and component load as an objective function, the joint torque and the component load are all relatively smaller.
5. Simulation analysis and conclusion To show the performance of the proposed methods, we will present a simulation example for two planar 3R robots holding a common object shown in Fig. 2. The hard point contact between the object and the end-eectors is assumed. Thus, forces are only transmitted from the the end-eectors to the object. The gravity of the system is also neglected. Since only position parameters are considered here, i.e. n ÿ m = 1, the two planar 3R robots can be regarded as redundant robots. The joint displacement vectors of the two robots are denoted as, respectively, y1 = [y 11, y 12, y 13]T, y2 = [y 21, y 22, y 23]T. In terms of Eq. (8), the robot's motions are expressed as P 0 ÿ T_ XC 29 P0 T_
1166
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
Fig. 2. Two planar 3R robots holding a common object.
where T_
a cos j ÿa sin j 2 f f; ÿa sin j ÿa cos j
j is the rotation angle of the object (Fig. 2). If the object's motion is known, the exerted resultant force R may be represented as T R m0 P 0x ; m0 P 0y m0 g; Iz j Load matrix W is 2 1 0 1 0 4 W 0 1 0 1 : ÿa cos j a sin j a cos j ÿa sin j Further to obtain 2 1 0 62 6 60 1 6 2 W 6 61 6 62 0 4 0 1 2
the Moore±Penrose generalized inverse of W cos j 3 2a 7 sin j 7 7 2a 7 7: cos j 7 7 2a 7 5 sin j 2a
For further details about matrixes J, D, H and G, please refer to Ref. [9].
30
31
32
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1167
Table 1. Structural parameters and initial con®guration
Master robot Slave robot
Link length (m) l1 l2
l3
Link mass (kg) m1 m2
m3
Initial conf. (deg) y1 y2
y3
0.5 0.5
0.5 0.5
1 1
1 1
ÿ 45 60
60 ÿ 120
0.5 0.5
1 1
45 ÿ 60
Fig. 3. Joint con®gurations: (a) the ML method; (b) the MT method; (c) the MC method; (d) the LN method.
1168
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
In simulation calculations, the structural parameters and the initial con®gurations of the two robots are shown in Table 1. Simulation time T = 2(s), weight factors C1 = 1, C2 = 0.5, C3 = 1, object mass mo = 1 kg, object lenth 2a = 0.5 m, object motion PÈo = [0.3 sin(2pt/T), 1.2 sin(2pt/T)]T m/s2,j = jÇ = 0, j = p/2 (without rotation). Figures 3±5 are simulation results of joint con®guration, load and joint torque in the coordinated manipulation of the two robots. For convenience, the comparisons of the three methods are gathered in Table 2. Fig. 3(a)±(c) indicates, that there is no clear dierence among the joint trajectories obtained from the three proposed methods. The dierence is mainly from the fact that the three methods all take the joint torque as an objective funtion. Comparing the results with Fig. 3(d), which is the simulation result of the LN method, we can ®nd the joint trajectories have relatively obvious dierences. Table 2 shows for the load performance index the ML method is the best, the MC method takes second place, and the MT method is poor; for the torque performance index, the MT method is the best, the MC method takes second place, and the ML method is poor. Besides, the load and joint torque of the MC method are only slightly greater than those of the other two methods,which indicates that the comprehensive property of the MC method is optimal. From the theory of optimization, when the sum of two unknown variables is known, the square-sum of the two variables is minimized if the two variables are equal. It is because the ML method distributes the load evenly between the two robots that the square-sum of the two component loads is minimized. Therefore, in order to obtain optimal load distribution in a normal sense, the resultant load should be distribute as evenly as possible. Simulation results demonstrate that, in the MC method, the two component loads exerted on the end-eectors are very close so that their square-sum is relatively small, and, the torque dierences of the ®rst joint (of all three methods) are the most obvious. For most robots, since the ®rst joint always bears the loads of the following links and
Fig. 4. The total loads of the two robots.
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
1169
Fig. 5. Total torques of the two robots.
joints, its torque is maximum and also becomes a principal factor that aects the total torques. Summing up the above comes to the following conclusions: the ML method can provide minimum loads; the MT method can give minimum joint torques; if we want both the loads and the torques all to be relatively small, the MC method can be adopted. Finally, it is worth noting the following. (1) Weight factors have some eects on the simulation results. Especially, if C2 = 0, the three methods have a very poor stability, and they always fail in simulations [9]. Besides, using the variable weight factors [9], the global characteristics of these methods can be further improved. (2) The component loads discussed Table 2. The comparisons of the three methods
Total load Robot 1 load Robot 2 load Total torque Robot 1 torque Robot 2 torque Torque 1 of robot Torque 2 of robot Torque 3 of robot Torque 1 of robot Torque 2 of robot Torque 3 of robot
1 1 1 2 2 2
ML method
MT method
MC method
small small small large large large large large large large large small
large large large small small small small small middle small middle large
middle middle middle middle middle middle middle middle small middle large middle
1170
Z. Jing, B. Shi-Xian / Mechanism and Machine Theory 34 (1999) 1155±1170
here are the forces exerted on the object by the end-eectors which determine the motion of the object. Beside the forces, enough grasping must also be provided by the robots in order to keep the contact between the object and the end-eectors during the entire coordinated manipulation. The grasping forces which do not contribute to the motion of the object, can come from the null-force in the internal space, whose function is similar to that of joint selfmotion. References [1] J.Y.S. Luh, Y.F. Zheng, Constrained relations between two coordinated industrial robots for motion control, Int. J. Robotics Res. 6 (3) (1987) 60±70. [2] M. Uchiyama, N. Iwasawa, K. Hakomari, Hybrid position/force control for coordination of a two-arm robot, IEEE Int. Conf. on Robotics and Automation, Raleigh, NC (1987) 1242±1247. [3] Y.F. Zheng, J.Y.S. Luh, Optimal load distribution for two industrial robots handling a single object, IEEE Int. Conf. on Robotics and Automation, Philadelphia, PA (1988) 344±349. [4] J.M. Tao, J.Y.S. Luh, Coordination of two redundant robots, IEEE Int. Conf. on Robotics and Automation, Scotsdale, AZ (1989) 425±430. [5] Y.R. Hu, A.A. Goldenberg, Dynamic control of multiple coordinated redundant manipulators with torque optimization, IEEE Int. Conf. on Robotics and Automation, Cincinnati, OH (1990) 1000±1005. [6] F.T. Cheng, D.E. Orin, Optimal force distribution in multiple-chain robotic systems, IEEE Trans. Systems Man Cybernetics 21 (1) (1991) 13±24. [7] F.T. Cheng, D.E. Orin, Ecient formulation of the force distribution equations for simple closed-chain robotic systems, IEEE Trans. Systems Man Cybernetics 21 (1) (1991) 25±32. [8] F.T. Cheng, J.J. Lee, T.H. Chew, F.C. King, Control and simulation for a closed chain dual redundant manipulator system, J. Robotics Systems 12 (2) (1991) 119±133 [9] Z. Jing, B. Shi-Xian, The joint trajectory planning with minimal joint torque for redundant robots, Mechanical Science and Technology 16 (4) (1997) 623±626.