Mech. Mach. Theory Vol. 24, No. 1, pp. 917, 1989
0094114X/89 $3.00+0.00 Copyright © 1989 Pergamon Press plc
Printed in Great Britain. All rights reserved
OPTIMAL JOINT TRAJECTORY PLANNING FOR COORDINATED POINTTOPOINT MOTION OF TWOARM MANIPULATORS B. B E N H A B I B a n d E. T A B A R A H Robotics and Automation Laboratory, Department of Mechanical Engineering, University of Toronto, Toronto, Ontario, Canada (Received 29 April 1988)
AbstractThis paper addresses the area of trajectory planning for pointtopoint motion of twoarm manipulators. This type of motion is very suitable and sufficient for many industrial applications, where motion trajectories are planned for joint displacements (or velocities). In our previous paper we had introduced a position control scheme, which accepts preplanned motion joint trajectories as inputs to the controllers. This algorithm monitors the task space trajectory of the object carried by the twoarm manipulator, using a position control module, and recomputes some intermediate joint configurations in order not to drop (or excessively squeeze) the object. In this paper we present an offline trajectory planning algorithm, which minimizes the probability of encountering excessive task space errors that would require the position control module to recompute joint configurations. The proposed methods uses cubic polynomials for all joint trajectories as means of obtaining continuous velocities and accelerations throughout the motion. A twoloop multivariable optimization algorithm is carried out to obtain optimal joint trajectories. The main objective of the optimization is: to minimize the number of intermediate joint configurations, to be used in fitting cubic splines to joint displacements, thus reducing computational burden during the realtime control of the arm. The primary constraint of the optimization problem is: to determine joint trajectories such that task space errors remains under specified tolerance values.
I. INTRODUCTION The introduction of twoarm robotic systems to complex industrial assembly tasks could be facilitated by the rapid growth of research efforts in this area. Such systems have recently been addressed in various technical publications. In the area of kinematics and dynamics, most papers make use of the Jacobian matrix in developing motion control models for continuous path trajectories[l3]. The topic of assembling two parts into a rigid product has also been investigated in this context[46]. In the area of realtime control, various strategies, such as adaptive control, have been applied to dualarm configurations, usually based on hybrid position/ force control[710]. Methods for collisionfree path planning of multiarm manipulators cooperating on a task have been developed, usually, within automatic workcell context[l 114]. Another area of investigation has been the structuring of computers for realtime distribution of computing tasks[1517]. This paper addresses the area of trajectory planning for pointtopoint (PTP) motion of twoarm manipulators. PTP motion type is very common in industrial assembly tasks, where trajectories in task space cannot be easily defined or they are immaterial. For such cases, motion trajectories are planned for joint displacements (or velocities) as opposed to trajectories planned for the end effectors of the twoarm system, as it is done for continuous path (CP) motion type.
In our previous paper, we have introduced an efficient position control scheme for twoarm manipulators cooperating on a task[18]. This algorithm accepts preplanned PTP joint trajectories as inputs to the motion control module which monitors them for feasibility. If at any preplanned intermediate configuration, the object faces the possibility of being dropped (or excessively squeezed), the control scheme incrementally changes joint displacement commands. The new (feasible) displacement commands are then submitted to the joint controllers. In this paper, we present an offline joint trajectory planning algorithm, which minimizes the probability of encountering excessive task space errors that would require the motion control module to interfere. In the case of "perfect" mathematical modelling of the manipulator and the motion, this probability could be reduced to "zero", therefore, inverse kinematic solutions would not be required during the realtime control. The basic assumption for this control algorithm is that some known compliance exists in the end effectors of the arms carrying an object in PTP motion mode. The proposed joint trajectory planning technique is based on the use of cubic polynomials for the displacement curves. Optimal trajectories are obtained by determining intermediate joint configurations for one of the arms, and then by using spline fits to yield continuous joint displacements, velocities and accelerations. Similar optimization algorithms have been developed for onearm manipulators, mini
10
B. BENHABIBand E. TABARAH
mizing motion times or deviations from continuous pointtopoint task space trajectories[19,20]. This paper consists of five section. Section 2 summarizes the motion control algorithm earlier reported in[18], as an introduction to Section 3 which describes the proposed method for joint trajectory planning. Section 3 defines the problem at hand and describes the optimization algorithm used to determine best cubic polynomial joint trajectories. Section 4 presents optimal joint trajectories obtained for an 8 d.f. planar twoarm manipulator. The conclusions of our study are presented in Section 5.
•~ .
Df
/*r. ~
2
f d Fo
/
Fig.2. Deviation in relative displacement.
2.1. Direct kinematics submodule
2. PTP MOTION POSITION CONTROL The PTP motion position control algorithm explained in this section aims at reducing the number of inverse kinematics solutions required during the realtime control of a twoarm system. The global objective of the algorithm is to supervise the motion of the arms, to ensure that the object carried by the end effectors is neither dropped or excessively squeezed. To achieve this objective, task space trajectories are monitored for possible errors in relative locations of both end effectors. Corrective action is taken to incrementally change joint displacements through the use of inverse kinematic solutions, to eliminate task space errors which are above allowable tolerance values, only when required. This strategy will reduce the computational burden encountered during the realtime control of twoarm manipulators due to inverse solutions. The motion control (MC) module, which monitors task space trajectories, determines the feasibility of joint displacement commands by evaluating corresponding task space errors (Fig. 1).
MOTI ONTRAJECTORY PLANNI NG MODULE I4 (MTP) I l q (t ) I
I
"MOTIONCONTROL MODULE(MC)"
This submodule determines the locations of both end effectors, at any time, from the planned joint displacements q( t ),
TASKSPACE ERRORSSUBMODULE
lr,ltr;1
[°T~ ] = g(q(t)),
(2)
2.2. Task space errors submodule
This submodule determines deviations from the desired relative displacement between the two end effectors. The desired relative displacement (i.e. zero deviations) can be defined by a (4 x 4) transformation matrix (Fig. 2) as follows:
[°T,]* ['O:]= [°T:]*;
(3)
where [.]* are the location transformation matrices at either initial or final manipulator configuration (zero task space errors). Deviations from the desired relative displacement will occur during the PTP motion due to joint trajectory approximations. This deviation can be determined for the follower arm with respect to the leader arm, assuming the location of the leader's end effector is as desired, as follows: [°E] = [°T~]  [°T~];
(4)
where
[°T:] = [°TTl['O:].
(5)
The leader/follower role selection procedure for the
[E]
I TOLERANCE TASKSPACE SUBMODULEI fiST)
~
(1)
where [°T~] and [°T~] are the (4 × 4) transformations matrices, specifying the actual (a) locations of the leader (/) and follower ( f ) arms' end effectors with respect to base coordinate frame, F 0 (Fig. 2).
DIRECTKINEMATICS [
SUBMODULE
[°T~] = f(q(t)),
CONFIGLIRAnON SPACE] CORRECTION [ SUBMODULE(CSC)]
arms is immaterial at the realtime control level, as long as the controller has a prior knowledge of the roles of each arm. The control algorithm is setup such that corrections are determined and applied to only one of the arms, in this case the follower arm. 2.3. Task space tolerance submodule
I
)
I"
..... I'"""° rr=
Fig. I. "PointtoPoint Motion Control" module.
This submodule can be considered as a comparator between actual error values and maximum allowable "tolerance" values. For simplification purposes, the deviation in relative displacement can also be
Optimal joint trajectory planning expressed by a (6 x 1) error vector e in end effector frame coordinates of the follower arm[18],
re, = hi([°E])
i = l, 6.
(6)
Task space tolerances, therefore, can also be expressed in a vector form, defining absolute maximum values for the error vector e, lYeA~< ~k~ i = 1, 6,
(7)
where the tolerance values, ~,~, can be defined as constants or as functions of time, ~(t)[24].
2.4. Configuration space correction submodule This submodule is accessed only if the task space tolerance submodule indicates an "infeasible" task space trajectory location. This module consists of an algorithmic procedure to solve the inverse kinematics relations in order to determine joint displacement corrections for the follower arm. The method is a Newton Raphson based technique such that it updates joint displacements for reducing the error vector values e, to under tolerance values, ~,, as follows: q(t) = q(t) + c~qC;
(8)
6q~= [rj], .re.
(9)
where
The procedure of calculating 6q c and updating the Jocobian matrix of the follower arm to recalculate q(t) in equation (8) is continued, in an iterative manner, until task space tolerance values are satisfied, equation (7). The procedure can be considered as evaluating an inverse kinematics solution at this configuration, q(t). To summarize this section, we can conclude that for a better performance of the motion control module, the joint trajectories should be planned in such a manner that the realtime computational burden is minimized. The main PTP motion control algorithm, as described in Fig. 1, attempts to minimize the total number of inverse solutions throughout the complete trajectory. The goal of this paper is to develop an offline joint trajectory planning method which would achieve the above objective. Such a method is presented in the next sections. 3. JOINT TRAJECTORY PLANNING In this section an "optimal joint trajectory planning" algorithm is presented for twoarm manipulators cooperating on a task. This method utilizes cubic polynomial joint displacement trajectories for both arms. In order not to restrict the use of the realtime control algorithm illustrated in Section 2, both arms will be treated equally and referred to as arm No. l and arm No. 2 in the development presented in this section. The output of the algorithm will be a set of cubic polynomial displacement equations for one arms' joints and cubic spline fit displacement equations for the other's, all as functions
11
of time. The primary objective of the optimization procedure is to determine the minimal number of nodes used in the spline fits. The primary constraints of the optimization is to reduce task space errors to under tolerance values.
3.1. Cubic polynomial joint trajectories Task space errors, that might result in either dropping or excessively squeezing the carried object, occur due to approximation of a continuous path task space trajectory by PTP joint space trajectories. Whenever a task space trajectory is not defined, such an approximation method can be used to ease the computational burden on realtime controllers. However, as shown in Fig. 2, task space errors will occur due to this approximation that should be corrected. In order to eliminate task space errors, the joint displacements of one of the two arms can be corrected using equations (1)(9). This case can be considered as assuming "perfect" task space trajectory for one of the arms (i.e. "perfect" joint trajectories), and determining a "corrected" trajectory for the other one, (i.e. correcting joint trajectories). In order to accomplish this task, first, a set of "corrected" intermediate joint configurations are obtained, using inverse kinematics, for the arm with the "imperfect" joint trajectories. For the sake of simplicity this arm will be referred to as arm No. 2. These intermediate joint configurations plus the initial and final arm configurations will be referred to as "nodes" throughout this paper. Thus, for (K + 2) nodes,
[,Q] = pq0, ,qq,
(10)
[2QI = [2q0, 2ql . . . . . 2qX+ l ],
(11)
and
where [2Q] is the ( W x 2) joint displacement matrix of the 'Njoint arm No. 1; and; [2Q] is the (2N x K + 2) joint displacement matrix of 2Njoint arm No. 2. The initial joint configuration of both arms are given respectively by ,q0 and 2q0. The final configurations of both arms are given by ,qd and 2qd= 2qX+,. The corrected intermediate joint configurations are q' to qX in equation (11). A possible approach to constructing joint trajectories, which have more than 2 nodes, is the use of cubic spline polynomials between adjacent nodes. Such an interpolation would yield continuous second derivatives at all nodes of arm No. 2 given in equation (11). For a 2node displacement trajectory curve the cubic spline reduces to one cubic polynomial equation, as defined by equation (10) for the joints of arm No. 1.
Cubic spline interpolation. Let % < z~ < ~2 < " " "< ZK+ l be the set of time values corresponding to joint configurations of arm No. 2, equation (11), with task space errors less than E (tolerance value for the inverse kinematics solution). The onedimensional
12
B. BENHABIBand E. TABARAH
joint trajectories are interpolated using a set of cubic polynomials 2qo(t ) of the form
change motion time, equation (14), without affecting task space errors, based on the following equality,
2q0(t) = ao(t  zj) s + bo(t  zj) 2 + co(t  zj) + d o,
i=l,2N
j = 0 , K,
q(t)l .... = q*(t)[,=~,+, (12)
where a0, b0, co and d o are the cofficients of the cubic polynomials; 2q¢(t) is the displacement function of joint i in the interval (zj, ~j+ ~), of arm No. 2,[21]. For the joints of arm No. l K is equal to 0, yielding one cubic polynomial equation per joint, lqi(t). 3.2. Decoupling of task space errors and time using cubic spline fits One of the main reasons of using cubic polynomial spline fits for joint trajectories is the decoupling permitted between task space errors and time. This decoupling is vital during the optimization procedure to determine best joint trajectories as functions of time. Although the primary objective of the algorithm is to determine joint trajectories, which yield task space errors under specified tolerance values, it also has to yield joint velocities and accelerations which are under limit values during the PTP motion. A feasibility index, 2, similar to the one developed in[22] is used in this paper to determine whether joint velocity/acceleration trajectories, obtained using cubic splines, are feasible, 2 = max(21,2~/:)
(13)
where
1, L"'
2~ is the maximum ratio of maximum joint velocity encountered in any of the time intervals, for any of the joints, over maximum allowable joint velocity, coi. Similarly 2: is obtained for joint accelerations. A value of one or less for 2 indicates a feasible solution, where a value above 1 would require the motion time to be extended, at least by a multiplier of )~, as follows, K
Ty>~ ~
,~(Zj'fj_l).
(14)
j=0
Equation (14) assures joint velocities and accelerations to be under limit values, (to, at). This operation of time extension (or reduction, when 2 < 1.0) does not affect task space errors, i.e. "total decoupling". Task space errors for arm No. 2 can be computed at any time t by using the cubic polynomial displacement equations for the joints of both arms, (i.e. "Direct Kinematics"). Once acceptable task space errors are determined for a set of joint trajectories the user can also check whether these trajectories are feasible by calculating 2, equation (13). The modularity of the proposed algorithm due to total decoupling of errors and time, would then allow the user to
(15)
where qi(t) is the i'th joint displacement value calculated from equation (12) at time t +, and q*(t) is the ith joint displacement value calculated from equation (12) at time 2t +, which uses the new modified set of times, equation (14), when obtaining the cubic spline joint trajectories. 3.3. Optimization problem definition The problem of feasible trajectory planning for twoarm systems must be augmented with an optimization procedure which ensures task space errors under tolerance values. There exist various objectives to be achieved with this optimization procedure. Most objectives can be formulated as constraints and subprocedures, reducing the problem to a single objective function to be minimized. This objective was set as minimum number of intermediate nodes, K, in this paper, Mm Z = K,
(16)
subject to equation (7) for task space errors
I:eA ~<~0,
i = 1,6,
(7)
and ,~ ~ 1.0.
(17)
The above formulation would yield optimal and feasible joint trajectories. In order to achieve "minimum K", a secondary objective of "best set of time intervals" has to be seeked for a given number of intermediate nodes. This set should yield lowest function value of "absolute maximum task space error" during the entire PTP motion. For the sake of simplicity this subproblem can be formulated by combining all error and tolerance terms into single term expressions respectively, as follows, Iqetll ~< 11411
(18)
where I1' II is the Euclidean norm operator. The problem, then, reduces to finding the best set of (variables) which would mimimize the maximum task space error, Fx, encountered in any of the time intervals [defined by the K + 2 nodes in equation 02)] Min Fr(z/; j = 0, K + 1) = max [lleY(t)ll]. t
(19)
The global objective of minimum K, equation (16), would then be obtained when MinFxlx= x. <. ((¢ [[ for an optimal number of nodes, K*. The optimization procedure to obtain minimum F x should be carried out to provide the realtime controller with maximum contingency to deal with task space errors which could occur due to inaccurate
Optimal joint trajectory planning
I Input: q°,q~,Tf',,1.0
13
I
qj,~_t,am rol.
,
J One  dimensionalsearch: [ I
Determinea new K
I
I Fit cubic polynomial displacement] trajectories to the joints of arm no.11
lq(t)~r t
.
.
.
.
.
.
.
.
.
~
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
"Flexible. tolerance: method"
a new ~_ i" I Determine I.. x Jf Determine K intermediate joint ]
corLfiguratign.sfor armno. 1 ['Ol['s',,'S
xl
1lo I
I
I DetermineK intermediatejoint I configurations for arm no.2
[2Q]. [2g.,,... ?cpl l'Ol
l Fit cubicspline displacement [ trajectories to the jointsof armno.2[ ..J Determine maximum task space [ lq(t~) errors using direct kinematics
W ...........
~t,~'ai
i
N°<.r:K ~"" L "~" ~ ",,
"" Determine).
,~ Y~ ~ ~ " . ~
I ~,~1 Expand motion I
.................. tar'mntne~ha;ng;t::jl::;fan.d ~ [ repeat process to obtain K 14 r
tile I
I
I Determineall ~lnt trajectories ~~)lbl M°ti°n c°ntr°l I using K" and TI module"MC" Fig. 3. Motion trajectory planning optimization flowchart.
modelling of the manipulator during the trajectory planning phase. Both optimization procedures are explained in detail in the next section and illustrated with a flowchart. 3.4. Algorithmic optimization solution A two loop optimization procedure is proposed in this section to determine optimal joint trajectories for a PTP motion of the minipulator, initially defined by the first and last (desired) configurations, [q0, qd]. The outer loop is a singlevariable search procedure resulting in an optimal K (number of intermediate nodes) value. The inner loop is a multivariable search procedure resulting in an optimal set of
zj(j = 0, K + i) values for a contrast number K, thus, yielding the optimal joint trajectories. The detailed algorithmic procedure is illustrated in Fig. 3. This twoloop search is also summarized as follows: Outer Loop: Given initial and final configurations, an optimal number of intermediate nodes is determined using equation (16) subject to constraint equation (7). Inner Loop: Given a number K, an optimal set of time values ~j, j = 0, K + 1, is determined using equation (19) subject to a constraint equation, thus yielding the minimum of the maximum task space error. Let Oj be the maximum error
14
B. BENHABmand E. TABARAH measured in the time interval rj to zj+~, then the constraint of minimum Fx can be formulated as in equation (19), rewritten as follows,
MinFx=max(gp j, j=O,K). J
For the inner loop the "Flexible Polyhedron Search" method was used as an efficient multivariable optimization technique[23]. For the outer loop any onedimensional simple search technique can be used, since the objective function value monotonously decreases as a function of the variable K, see Table 1 in Section 4. The results of this optimization, KI: = K*, correspond to the assumption made in Subsection 3.1. stating that the joint trajectories of arm No. 2 are corrected according to the presumed "correct" joint trajectories of arm No. 1. Since these roles were arbitrarily set, the proposed optimization algorithm makes an additional iteration in completely repeating both outer and inner loops for a switchedroles case to determine K2~. The results of both interations K~2 and K21 are then compared to se!ect best joint trajectories:   I f Kt 2 > K21 joint displacements of arm No. 2 are corrected;   I f K2~ > Kl2 joint displacements of arm No. 1 are corrected;   I f K~2 = Kzl task space errors are checked to determine and select the case with lower maximum task space error as the optimal. The optimization described above fits cubic polynomials to joint displacements as functions of time. Since task space errors are decoupled from time, the proposed algorithm uses a unit time for the PTP motion time (cycle time), T~ = 1.0, for the initial simulation. This unit time can be substituted later by any user supplied motion time TI without altering
the shape of the error vs time curve. Once optimal trajectories are obtained, for the user supplied motion time Ts, by the twoloop procedure, the velocity/ acceleration limit feasibility coefficient 2 is calculated as in equation (13). According to this coefficient the user can decide whether to extend, reduce or not change the motion time Ts. If TI is changed, cubic polynomials are refitted to all joint displacement trajectories and the coefficients of these time function equations are transferred to the "Motion Trajectory Planning Module", as in Fig. I. As described in Section 2, the realtime control algorithm accepts joint trajectories as inputs to the "Motion Control Module". The decision made, in the planning of trajectories, whether arm No. 1 or arm No. 2 should be considered as having "correct" trajectories is immaterial for the Motion Control Module. The user can arbitrarily select any of the two arms as the "leader" and the other as the "follower", thus, not interfering with the modularity of the control program. In the modified control algorithm presented in[24], it has been illustrated that the roles of the arms can be switched during the PTP motion to facilitate the control of the manipulator. Therefore, once more showing that the choice of roles is immaterial.
Y
°
zf~O
~

o
,0) =*
Xo
Fig. 4. Task space location parameters.
4.0
3.0
Positional
Error
2.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
(×10 ~ )
1.0
0.0 0.0
I
I
0.4
0.8
I 1.2 Time (see.)
Fig. 5. Positional task space error vs time.
I 1.6
2.0
Optimal joint trajectory p l a n n i n g
15
g
, 0
i
•
~8
';o

8
g
•~ e
P. o
n
~ ? .~ °
uo i
o.o0 o . 4 o o . 0 o 1.2o 1.~o Time
2 . o 0 2.40
0.40
:
Ig
~
8
,~,.
8.
?.
~
0.40
0.00
0.00
1.20
1.60
2.00
2.40
,..
1.00
2..
1.20
1.00
2.00
2.40
s.ao
t.so
a.oo
a.40
82
"',
°0.00
cs
~ 0.00
Time
?"t'" ""
.
•
81
8
~t~
?
1~20
1.00
2.00
" "',
~ " ",0.00
2.40
0.40
0.80
Time
Time
83
84
o
d I
o
eli
/
d '
d,
t,
8
•~ .
'
:%40
.~ g
T ~
?
~ O.O0
0.40
0,80
I.~0
heO
2.00
8.40
Time
0.40
0.00
i 0.00
0.40
! Ii
8s
0.00
o
t.20
1.00
a.oo
1.40
o.oo
Time
0.00 Time
86
0.40
0.00
Time
e7
ee
Fig. 6. Joint displacement trajectories.
16
B. BENHABIBand E. TABARAH 4. AN EXAMPLE PTP MOTION TRAJECTORY
PLANNING The flow of information illustrated in Fig. 3 was simultated to determine optimal PTP motion joint trajectories. As an example system a twoarm 8 d.f. manipulator was chosen. For the trajectory planning phase, it was assumed that the two first joints (q~, q2) common to both arms had "correct" trajectories. The algorithm then was simulated to determine the optimal solution, also indicating which arm was chosen as having the "correct" joint trajectories: Arm No. 1: (q3, q4, qs), or
Arm No. 2: (q6, q7, qs)' The initial and final joint configurations were determined using the pseudoinverse method, proposed in[18], for the inverse kinematics solution. This method uses pseudoinverses to invert the Jacobian matrix in the iterative inverse solution of the kinematic model yielding an optimal final joint configuration. The objective function of the optimization in minimum joint displacements when moving from the intial to the final configuration of the twoarm manipulators. Using this method the following final configuration, qd, was determined, for the given initial configuration, q0, q°(rad) = (1.00434,0.29464,1.11438, 1.78025,
0.63311,  1,02390,1.46576,1.40075), q~rad) = (0.47006, 0.02803,0.54127, 0.80099,
The maximum allowable joint velocity, e), and acceleration values, ~,, are also given as follows, (o(rad/s) = 1.0, • (rad/s 2) = 20.0.
The objective of the planning phase is to determine optimal cubic polynomial joint trajectories for the specified PTP motion, to be complete in Tf = 2.0 sec. This objective is achieved by carrying out the simulation procedure described in Section 3. The results of the optimization procedure are given as follows: (i) Minimum number of nodes for arm No. 2, determined as the arm with "corrected" joint trajectories, is equal to 5. (ii) The optimal normalized time values are, z(sec) = (0.0, 0.2846, 0.4890, 0.6921, 1.0).
(iii) The positional task space error curve is plotted as a function of time in Fig. 5, with maximum error calculated as: A~a~ = 1.9052 × 10 4 (iv) The coefficient 2 is calculated as 0.7344 ( < 1.0), indicating that the required Tr= 2.0 sec is a feasible motion time. (v) The cubic polynomial joint displacement trajectories are plotted as functions of time in Fig. 6. The motion simulation of the manipulator is illustrated in Fig. 7, and the monotonously decreasing characteristic of maximum task space error F r as a function of the number of nodes K is illustrated in Table 1.
 1.40405, 0.69278,0.86051,1.31010). The lower qt, and upper, q", limits on joint displacements are given as ql(rad) =  rt
i=1,8,
qU(rad) = 7t
i = 1, 8.
5. CONCLUSIONS An efficient offline pointtopoint motion joint trajectory planning method is presented in this paper.
The manipulator is required to carry a retangular object from the first to the last task space location, corresponding to q0 and qd respectively. The tolerance limits on maximum allowable task space errors are defined as, ~bp= 0.0002 >~ Ap ~Oo = 0.00005 ~> Ao where ~,p is the tolerance value for positional errors Ap, and qJo is the tolerance value for orientational errors Ao, defined as follows (Fig. 4), Ap = (AX 2 + Ay2) u, and ~o ~ A~.
Fig. 7. PTP motion simulation.
Optimal joint trajectory planning Table 1. Maximum positional task space error vs number of nodes Number of nodes (K) 2 3 4 5 7
Maximum task space error (Fie) 5.01 x 5.14 x 6.05 x 1.90 × 7.75 x
10 2 10 3 10 4 10 4 10 5
The proposed method uses cubic polynomials for all joint trajectories as means of obtaining continuous velocities and accelerations throughout the motion. The overall objective of the algorithm is to supply the realtime control module with simple trajectories (to minimize computational burden) which will satisfy specified task space error tolerances. As illustrated in the numerical example, very stringent tolerances can be achieved with minimal number of nodes using cubic spline joint trajectories. This efficient optimization search would save considerable computation time during the realtime control spent in correcting joint trajectories, in order not to drop or excessively squeeze the object carried by the two end effectors.
REFERENCES
1. T. J. Tam, A. K. Bejczy and X. Yun, IEEE Proc. 25th Conf. on Decision Control, p. 1267 (1986). 2. G. Duelen, J. Held, U. Kirschhoffand H. Munch, IEEE Proc. 25th Conf. on Decision Control, p. 1273 (1986). 3. R. Zapata, A. Fournier and P. Dauchz, IEEE Proc. Conf. on Robotics and Automation, p. 1255 (1987).
4. M. A. Unseren and A. J. Koivo, IEEE Proc. Conf. on Systems, Man, and Cybernetics, p. 798 (1987). 5. I. H. Suh and K. G. Shin, IEEE Proc. o f Conf. on Systems, Man, and Cybernetics, p. 802 (1987). 6. Y. F. Zheng, J. Robot. Systems 4, 585 (1987). 7. S. Hayati, IEEE Proc. Conf. on Robotics and Automation, p. 82 (1986). 8. U. Ozguner, S. Yurkovich and F. AIAbbas, J. Robot. Systems 4(3), 377 (1987). 9. J. Y. S. Luh and Y. F. Zheng, Int. J. Robot. Res. 6(3), 60 (1987). 10. H. Seraji, J. Robot Systems 4(5), 653 (1987). I 1. R. Zapata, P. Coiffet and A. Fournier, Digit. Systems Indust. Autom, 2(2), 115 (1984). 12. E. Freund and H. Hoyer, IEEE Proc. Conf. on Robotics and Automation, p. 103 (1986). 13. S. Fortune, G.L Wilfong and C. Yap, IEEEProc. Conf. on Robotics and Automation, p. 1216 (1986). 14. B. Faverjon and P. Tournassoud, IEEE Proc. Conf. on Robotics and Automation, p. !152 (1987). 15. C. O. Alford and S. M. Belyeu, 1EEE Proc. Conf. on Robotics and Automation, p. 468 (1984). 16. F. Miyazaki, S. Matsubayashi, T. Yoshimi and S. Arimoto, IEEE Proc. Conf. on Robotics and Automation, p. 977 (1984). 17. Y. F. Zheng, J. Y. S. Luh and P. F. Jia, 1EEE Proc. Conf. on Robotics and Automation, p. 1236 (1987). 18. B. Benhabib, G. Zak and E. Tabarah, J. Robotic Systems 5, 103 (1988). 19. B. Benhabib, R. G. Fenton and A. A. Goldenberg, Proc. 3rd Canadian CAD~CAM and Robotics Conf., p. 10.I (1984). 20. B. Benhabib, A. A. Goldenberg and R. G. Fenton, Trans. A S M E J. Enng Indust. 108, 213 (August, 1986). 21. H. Spath, Spline Algorithms for Curves and Surfaces. Utilitas Mathematica (1974). 22. C. S. Lin, P. R. Chan and J. Y. S. Luh, IEEE Trans. on Automatic control AC2g(12), 1066 (1983). 23. M. D. Himmelblau, Applied NonLinear Programming. McGraw Hill, New York (1972). 24. E. Tabarah, B. Benhabib and G. Zak, IEEE Proc. Conf. on Robotics and Automation, p. 541 (1988).
GI~NI~RATION DE TRAJECTOIRES OPTIMALES DES ARTICULATIONS D'UN MANIPULATEUR A DEUX BRAS POUR UN MOVEMENT COORDONNI~ D'UN POINT A A UN POINT B ResemrCct article traite d'un algorithme de grnrration de trajectoires pour des drplacements d'un point A fi un point B de manipulateurs ~ deux bras. Ce genre de mouvement, o/l les trajectoires sont prrdrterminres pour le drplacement (ou la vitesse) des articulations, convient ~i un grand nombre d'applications industrieUes. Dans un article prrcrdent, nous avons 6tabli un algorithme de commande de position, lequel accepte en tant qu¢ donnres d'entrre aux contr61eurs, les trajectoires prrdrterminres des articulations. L'algorithme vrrifie la trajectoire de l'objet transport6 par 1¢ manipulateur ~i deux bras par le biais d'un module de contr61e de position. De plus, il effectue le calcul de quelques configurations interm&liaires d'articulations afin que le manipulateur n'rchappe (nine comprime ~ l'excrs) I'objet en question. Dans le prrsent article, nous prrsentons un algorithme qui sert ~i prrdrtcrminer les trajectoires. Celuici minimise la probabilit6 que les erreurs de position des organs terminaux du manipulateurs soient excessives. Ces erreurs rendraient nrcessaire le recalcul de confgurations des articulations. La mrthode proposre est basre sur l'utilisation de polynrmes cubiques lesquels drfinissent les trajectoires des articulations de fagon ~i ce que les vitesses et les accrirrations soient continues tout au long du mouvement. Les trajectoires optimales des articulations sont drterminres ~i l'aide d'un algorithme d'optimisation comprenant deux cycles et ~i plusieurs variables. L'objcctif principal de l'optimisation est de minimiser le nombre de configurations interm&liaires, lesqueUes servent ~i adapter les drplacements des articulations scion des splines cubiques. Cela rrduit la ~che de l'ordinateur lots du contr61e en temps rrel. La contrainte principale de l'optimisation est de drterminer les trajectoires des articulations teUes que les erreurs de position des mains demeurent ende~i des tolrrances sprcifires. MMT 24/IB
17