MADRID, BADAN 1996 - Heuristic Search Method for Continuous-Path Tracking Optimization on High-Performance Industrial Robots

7/30/2019 MADRID, BADAN 1996 - Heuristic Search Method for Continuous-Path Tracking Optimization on High-Performance I

1/11

~ ) Pergarnon

PII:S0967-0661 (97)00101-9

Con t r o l Eng. Pract ice, Vol. 5, No.9, pp.1261-1271, 1997Copyright 1997 Elsevier Science LtdPrinted in Great B ritain. All fights reserved00967-0661/97 $17.00+0.00

H E U R I S T IC S E A R C H M E T H O D F O R C O N T I N U O U S - P A T H T R A C K I N GO P T I M I Z A T I O N O N H I G H - P E R F O R M A N C E I N D U S T R I A L R O B O T S

M. K . Ma d r i d a n d A . G. P . B a d a nLSMR -Laboratdrio de Sistema s Modulares Robdticos, DSCE/FEEC /UNICAMP ,CP 6101-13081.970, Camp inas/SP-Brazil(badan@ sce.fee, unicamp,br )

( R e c e i v e d J u l y 1 9 9 6 ; i n f i n a l f o r m M a y 1 9 9 7 )A b s t r a c t : The paper proposes an on-line heuristic search method to solve the robotcontinuous-path tracking control problem. It is a numerical technique that does notrequire inverse kinematic modelling of the robot, and gives accurate tracking withgood performance in the presence of disturbances. It can be applied to serial-chainrobots with any number of degrees of freedom. Its convergence does not depend onthe initial approximation, and it is not sensitive to robot singularities. Its practicalapplication was tested on a 5 DOF robot, without loss of generality.

C o p y r i g h t 1 9 9 7 E l s e v i e r S c ie n c e L t dKe yw or ds : Continuous-path control, heuristic search, real-time control, industrialrobots, trajectory control.

1. INTRODUCTION

The fundamental problem of robot tr ajectory con-trol is to make its gripper, or end-effector, trackthe required traj ectory in position and orientationwithin the workspace. Assuming that the robothas the minimum degrees of freedom requiredfor optimal tracking, the solution of the problementails specifying exactly the positions assumedby each joint during tracking. Such a solutioncan be obtained by algebraic methods (Chang e ta l . , 1992; Craig, 1989; Spong, 1987), or by geome-tric methods (Fu e t a l . , 1987; Snyder, 1985), bothmaking use of inverse kinematics resolutions. Theequations of these inverse kinematics are usuallycomplex, even for robots w ith on ly a few degrees offreedom (Lewis e t a l . , 1993; Dawson e t a l . , 1992).Their solutions for a given point in space canbe multiple, and further information is needed tofind the optimum. Its utilization in the real-timecontrol of high-performance medium- and high-velocity robots therefore makes large computa-tional demands.

The m ethod proposed here is based on a heuristicsearch process, and enables th e generalized vectorcomprising the joint set-point coordinates to bedetermined in real time without needing to solvethe inverse kinematic model.

2. CONTINUOUS-PATH TRACKINGContinuous-path position and orientation track-ing - one of the most important tasks of high-performance robots - demands accuracy, repeata-bility and robustness, allied to low operationalcost and investment. The problem will thereforebe considered as one of optimization, within aspace of candidate objects, namely the joint co-ordinates. The optimal solution is given by thedefinition of those that lead to the best pat htracking in real time. The computational solutionof the problem is based on the following threeprinciples:

Specify the objects - the joint coordinates -making a representation of how each acts inthe context.

1261


2/11

1262 M.K. Madrid and A.G.P. Badan P r o d u c e c o m p u t a t i o n a l r o u t in e s t o e v a lu a t e

p o s s i b le j o i n t s t a t e - v a r i a b l e v e c t o r s , a c c o r d -i n g t o a p r i o r i t y c r i t e r i o n , a n d d r i v e t h e a c -t i o n s o f e a c h r o b o t j o i n t .

P r o c e s s t h i s i n f o r m a t i o n r a p i d l y a n d e ff i-c i e n t l y ( M a d r i d a n d M o n a s t ~ r i o , 1 9 9 3 ; K o r f ,1 9 8 8 ; P e a r l , 1 9 8 5 ) .

3 . T H E H E U R I S T I C S E A R C H M E T H O DR e p r e s e n t i n g t h e t r a j e c t o r y , a n d a l s o t h e r o b o te n d - e f f e c t o r p o s i t i o n / o r i e n t a t i o n s , b y t h e i r h o m o -g e n e ou s t r a n s f o r m a t i o n m a t r ic e s T a n d A (Fu etal., 1 9 8 7 ) ,

n T z 8 T x a T , P T x ]a T y P T y ]

i8 T z a T zo

P A y [A = I n A y S A y aA y p ~ z j0

( 1 )

t h e t r a j e c t o r y i s d i s c r e t iz e d a t k p o i n t s , e a c hb e i n g r e p r e s en t e d b y a m a t r i x T k. T h e r o b o t e n d -e f f e c to r h a s k o b j e c t i v e s t o m e e t . T h e p o i n t s t ob e t r a c k e d a r e s a m p l e d a c c o r d i n g t o t h e d i s c r e t es e q u e n c e o f t h e t r a j e c t o r y . A s l o n g a s m a t r i x Ai s d i f f er e n t f r o m t h e c o r r e s p o n d i n g m a t r i x T k, t h eo b j e c t i v e h a s n o t b e e n r e a c h e d - i . e . , t h e r e i s at r a c k i n g e r r o r f o r t h e k- th t r a c k e d o b j e c t i v e . T h em e a s u r e o f t h is e r r o r i s b a s e d o n t h e c a l c u l a t io no f a h e u r i s t i c h . B o t h T a n d A a r e f o r m e d b y t h ec o m p o n e n t s i n t h e d i r e c t i o n s o f f o u r v e r s o rs n , s ,a , a n d p . T h e f i r st t h r e e g i v e t h e o r i e n t a t i o n , a n dt h e l a s t t h e p o s i t i o n . D e f i n i n g fo u r e r r o r v e c t o r sen , es , ea a n d ep , r e p r e s e n t i n g t h e o r i e n t a t i o n a n dp o s i t i o n i n g e r r o r s i n t h e s e d i r e c t i o n s , i n t h e f o r m ,

e~t

e a

" n A , - - n T , 1 [ S A , - - S T , 1nA y nTy | e~ ---- |SA y ST y[n A z r i T z J L S A z S T z J

- a a - a l raAv aTu | ep = I P A Y P T ya A z a T z A L P A z P T z

( 2 )

a h e u r i s t i c h i s t h e n d e f i n e de L e a + ( 3)= eT . en + e s .e s +

F o r a p r e - d e t e r m i n e d p o i n t k t o b e r e a c h e d , t h er e s p e c t i v e m a t r i x T k is m a i n t a i n e d w i t h c o n s t a n te l e m e n t s , a n d t h e t r a c k i n g m a n a g e m e n t a l g o r i t h ms e a r c h e s f o r a b e t t e r j o i n t m o v e m e n t b y v a r y i n gm a t r i x A , u n t i l t h e e r r o r b e c o m e s z e r o o r r e m a i n sw i t h i n t h e l i m i ts o f t h e t o l e r a n c e i m p o s e d .

I n g e n e r a l , s u p p o s e t h a t t h e r o b o t h a s N d e g r e e so f f r e e d o m , a n d t h a t e a c h o f t h e s e c a n c o n t r i b u t et o a tr a c k i n g s o l u t i o n t h a t m i n i m i z e s h . D e p e n d -i n g o n t h e s t r u c t u r a l c o n f i g u r a t i o n , a n d T k - 1 , as i ng l e c a n d i d a t e m a y b e a b l e t o s o l v e t h e p r o b -l e m c o m p l e t e l y . T h i s is , h o w e v e r , u n li k e l y . O n t h eo t h e r h a n d , e v e n i f a d e t e r m i n e d c a n d i d a t e c a n n o to f f e r a c o m p l e t e s o l u t i o n , i t c a n o f f e r a c o n t r i b u -t i o n t o a b e t t e r e n d - e f f e c t o r m o v e m e n t w i t h t h el o w e r h . T h e i m p r o v e m e n t t h a t a s in g l e c a n d i d a t ec a n o f f er c a n b e d e t e r m i n e d b y f i n d in g t h e l o c a lm i n i m u m o f h , m a i n t a i n i n g a ll o t h e r c a n d i d a t e si n v a r i a n t .O p t i m a l t r a c k i n g b y t h i s p r o c e d u r e i s a lw a y s p o s -s i bl e f o r r o b o t s w i t h a t l e a s t 6 D O F , s i n ce t h et r a j e c t o r y i s t o t a l l y c o n t a i n e d i n t h e w o r k s p a c e .T h e a m b i g u i t y in t h e c a s e o f r e d u n d a n t r o b o t s( N > 6 ) i s e l i m i n a t e d , s i n c e t h e m i n i m i z a t i o n o fh i s o b t a i n e d i n s t e p s , g e t t i n g t h e i n d i v i d u a l c o n -t r i b u t i o n o f e a c h c a n d i d t e , a n d c h o o s i n g t h e o n et h a t o f fe r s t h e b e s t r e d u c t i o n o f h a t e a c h s t e p .T h e m i n i m u m h c a n th u s b e r e a c h e d w i t h o u ta ll th e N j o i n t s n e c e s s a r il y c o n t r i b u t i n g t o t h em o v e m e n t t h a t s e a r c h e s f o r t h e k- th p o i n t o f t h et r a j e c t o r y . I n t h e c a s e o f r o b o t s w i t h N < 6 , t h eo p t i m a l t r a c k i n g c a n a l s o b e o b t a i n e d , b u t w i t hr e s t r i c t i o n s , s i n c e t h e y d o n o t h a v e t h e n e e d e dd e g r e e s o f f r e e d o m t o o r i e n t a t e a n d p o s i t i o n t h er o b o t a t a l l p o i n t s o f t h e w o r k s p a c e . I n s p e c i f i cc a s e s - a n d t h e y a r e m a n y i n p r a c t i c e - i n w h i c ht h e t r a j e c t o r i e s d o n o t r e q u i r e c o m p l e t e o r i e n t a -t i o n a n d p o s i t i o n i n g , t h e m e t h o d c a n b e a p p l i e de f f i c ie n t l y a s i n t h e p r e v i o u s c a s e s .T h e s e a r c h m e t h o d w i l l n o w b e d e s c r i b e d . I n as e q u e n c e o f s t e p s i , i = 1 , . . . , N , s o l v i n g Oh / 0 8 i ,m a i n t a i n a t e a c h s t e p 0 j i n v a r i a n t f o r j ~ i ,j = 1 , . . . , N . A s h i s d e f i n e d b y t h e e q u a t i o n ( 3 ) ,t h e r e w i l l b e N f u n c t i o n s s u c h a s

Oh = 2 . - - 0 [ e T en + eTes + eTea + eTep] . (4 )OOi 0 iF o r a p r e d e f i n e d w o r k s p a c e , t h e r e s t r i c t i o n s f o r 0 ia x e d e t e r m i n e d b y

0 imi~ < 0 i


3/11

Heuristic Search Method or Continuous=Path Tracking Optimization 1263th at were found as set-points of the correspondingdrivers are implemented, repeating the procedurefor point k l of the trajectory. If h is not accept-able, or if after a sequence of N steps it is notwithin the limits [hi < , where c is the radius ofconvergence established b y the specified tolerance,a new step-sequence is repeated, and so on untilthe objective is achieved.This process is convergent, since the points thatdefine the trajectories are within the space reach-able by the robot, in orientation as well as in po-sition,*for any value of N. If in an y step-sequencea single joint is moved, the robot will executediscrete joint movements, which can cause abrupt,undesirable var iations of acceleration and velocity.This also slows the convergence, since each jointmoves only after the previous one. The procedureis thus applied simultaneously within the samestep-sequence, defining which of the joints willparticipate in the displacement from the point k-1to point k.When a joint is a candidate and has not yet beenranked, it is said to be "open", and is "closed"when it has alrea dy been ranked. Start ing from aninitial robot configuration, detected by the jointposition sensors, from the resolution of its directkinematic model - or alternatively by an artificialsensing system, for example vision - and fromthe path specification, the simultaneous iterationalgorithm first sets all joints open. That is, itassumes initially tha t all are candidates that canoffer solutions. In the same step-sequence of thisalgorithm, once a certain joint has been chosenor ranked, this now. "closed" joint will no longerparticipate until the sequence is completed. Itsrespective movement is memorized. If the valueof h is insufficient for the tra ject ory requirements,the algorithm continues the search for a bettersolution, considering the new values obtained forthe already closed joints. This process continuesuntil all candidates have been ranked, or untilthe value of h obta ined satisfies Ihl _< c. At theend of each step-sequence loop, the algorithmsends the memorized values of movements to eachjoint, and the position set-points to the jointservomechanisms. If the value of h is still notsatisfactory, a new step in the k-th search isinitiated, maintaining the same matrix Tk, andso on until satisfactory. Once the k-th point isreached, the algorithm checks if there is a nextpoint to track. If so, its respective matrix Tk+l isadopted, and the iterative process of searching isrepeated until the last point is tracked.In this paper t he method is applied to a robot with5 DOF, choosing trajectories that can be reachedin all their points, both in position and in ori-entation. The Jeca H robot, whose characteristicsare described in Section 4, is used as a paradigm.

Although the number of degrees of freedom of thisrobot is too small to represent in a general waythe problems of ambiguity that arise in redund antrobots, there is no loss of generality in the applica-tion of the method for establishing and searchingfor the solutions of the general equations (4) and(5), or for any others wi th serial-chain kinematics.Fortunately, the higher the number of degrees offreedom a robot has, the better will be its dexter-ity, and consequently the more candidates it willhave available to participate in the intermediatesolutions that drive its action during trajectorytracking.

4. THE MULTI-TASK JECA H ROBOTThe project was to build a robot that couldbe used for education as well as in industrialapplications, having in mind the creation of aBrazilian technology for the building and oper-ation of robots. The research and testing steps ofthis project, called Projeto Jeca, were developedat the LSMR-Laboratdrio de Sistemas ModulatesRob6ticos of the Departamento de Sistemas e Con-trole de Energia of the Faculdade de EngenhariaEldtrica e de Computafdo at UNICAMP. Thetechnology developed is now incorporated in theexperimental Jeca H robot (Madrid, 1994).

Fig. 1. View of the Jeca H robot.The Jeca II, Fig.l, besides having five degrees offreedom, has a gripper wi th two fingers. Its jointsare driven by armature-controlled dc permanentmagnet motors, controlled by PWM (Pulse WidthModulation). Its torque amplification is obtainedby screw drives, belts and timing belt pulleys toeliminate the backlash of mechanical transmis-sion. It has digital and analog sensors, strategi-cally adapted in structure, with the respectiveinterface circuits to make their signals compat-ible with the micro-controlled circuits. This al-lows precise measurement of position, velocity,acceleration, torque, and strain at the controlled


4/11

1264 M.K. Madrid and A.G.P. Badanjoints. I t has also a leveling system, and electrome-chanical protection to avoid damage during move-ment. Its electronic control system is based ontwo hierarchical levels (Master/Slaves), in whichthe Master is an IBM/PC/AT compatible. TheSlaves are a group of six microcontrollers withindependen t microprocessors for the direct digitalcontrol of each joint (Badan e t aL , 1992; Madr idand Badan, 1990). Communication between theMaster and th e Slaves is parallel and bidirectionalthrough a common pattern-bus, and is managedby a communication protocol. This allows the in-sertion of new technologies, and facilitat es modu-larization. The robot can thus be adapted to workin diverse environments, making use of Slavesbased on processors with different performancesand defined by the performance demands of eachjoint.The Master is responsible for management at thetrajectory level. The Jeca H geometry and itsdirect kinematic model was the starting point forthe development of the search technique and thetrajectory control algorithm. This was sufficientto develop the entire algorithm.

z 2 /" ? ~ " o sxO, Xl

Z3 ," "04 ,

,, 8

~ ) - ~ , , j - "~ ..01

Fig. 3. The b ody-link axes of the Jeca I I .Based on the D - H parameters, the homogeneoustransformation matrix between each pair of robot-links is

C O i - C e q . S O i S a i . S O i a i.C O i"i _ l A i : S O l C ~ i . C O i - S ~ i . C O i a i . SOi0 Scri Ca i di (6)

0 0 0 1

5. USING THE METHOD IN THE J E C A HBecause of the basic geometry of the Jeca Hshown in Fig.2, its body-link axes can be sche-matically represented according to the conven-tions of D e n a v i t - H a r t e n b e r g ( D - H ) (Denavit andHartenberg, 1955), as in Fig.3.

iA (~ O z (-)03 (-')05r m i F o r e - a r m . . , . ." x i . . . . ~ l ~ , f ~ G r i p p e r

. . . . . ~ P - . _

W a ' ~ , . I i . ~ -Q rig in

: , ~ : 15 = 0 . 1 0 0 m(_~ol

where: C = cosine and S = sine.Then, the matrix A, defined in equation (1), isfound in the form A = A 1 ) A 2 . 2 A 3 . 3 A 4 . 4 A s , itselements being:

n A z = ( 6 1 S 2 3 C 4 -- S 1 ~ 4 ) C 5 -- C 1 6 2 3 ~ 5nAy : (SLS2 3C4 n - CIS4)C5 - - SIC23S5naz = C23C4C5 + S23S5

8 A z =S A y =S A z =

($1S4 - C1 S23 C4) & - CIC23C5- ( 6 1 ~ 4 -4- S 1 S 2 3 6 4 ) $ 5 - $162365$ 2 3 6 5 - 6 2 3 6 4 ~ 5

aAz = 61S23S4 q- S 1 6 4aAy = S 1 S 2 3 S 4 - - C1C4aaz = C23S4

(7 )

Fig. 2. Basic geometric configuration of theJeca I I .

Th e J e c a H D - H parameters are as in Table 1.Table 1. D - H Parameters of The Jeca HRobot

J o i n t i 0 i ~ a i d i1 0 1 - 9 0 0 l l2 02 0 12 03 03 90 0 04 04 90 0 13 + 145 05 0 15 0

P A z = / 2 6 1 6 2 - ( 13 - 4 - / 4 ) 6 1 6 2 3 - 1 5 6 1 6 2 3 S 5 3 -- 4 - 1 5 ( 6 1 S 2 3 6 4 - S 1 ~ 4 ) 6 5

P A y ---- 1 2 S 1 C 2 - ( / 3 - 4 - / 4 ) S 1 C 2 3 - 1 5 S 1 6 2 3 3 5 A -+ 1 5 ( S 1 & 3 C 4 + C~S4)C5

P A z = l l + 1 2 S 2 + ( / 3 - t - / 4 ) $ 2 3 " ~ - / 5 S 2 3 S 5 - i -+ 1 5 C 2 3 C 4 C 5

where:S n = sin(0n) and C n = cos(0n)S,,~ = sin(0n + 0m) and C,~,~ = cos(0n + 0m)


5/11

Heuris tic Search Me thod for Cont inuous-Path Tracking Opt imization 1265A s t h e J e c a H h a s 5 D O F , t h e r e a r e fi v e c a n d i d a t e -o b j e c t s 0 i i = 1 , 2 , . . - , 5. F o r t h e m t o b e u s e d b y t h ep r o p o s e d t e c h n i q u e , t h e f u n c t i o n t h a t i n d i c a t e st h e v a r i a t i o n t o m i n i m i z e h i s f o u n d f o r e ac h .T h e d e t e r m i n a t i o n o f /9 ~ i = 1 , 3 , 4 , 5 t h a t i n d i c a t et h e r e s p e c t i v e l o c a l m i n i m u m s o f h , c a n b e o b -t a i n e d a n a l y t i c a l l y w i t h t h e e q u a t i o n s Oh/O/ i = O .S u c h e q u a t i o n s h a v e a n a l y t i c a l s o l u t i o n s , s i n c et h e y a r e s e c o n d - d e g r e e a l g e b ra i c . I n t h e c a s e o f/9 ~, t h e e q u a t i o n 0 h / 0 / 9 2 = 0 i s a f o u r t h - d e g r e ee q u a t i o n . A n a n a l y t i c a l s o l u t i o n i s t h e r e f o r e im -p o s s i b le , s o a n u m e r i c a l p r o c e d u r e i s a d o p t e d .

5 .1 M i n i m i z a t i o n o f h (O i ) i = 1 , 3 , 4 , 5C o n s i d e r i n g / 9 i E [/ imin, / im~z ] var i an t i= 1 , 3 , 4 , 5an d /j = co ns t a n t fo r j= 1 ,2 , 3, 4, 5 , s i n ce j i t h eh e u r i s t i c h ( / 9 ~ ) i s d e f i n e d , b a s e d o n t h e e q u a t i o n( 5 ) , a s

h ( O i ) = h o ( O i ) + h p ( O i ) (S )w h e r e ,ho( / i) : H e u r i s t i c ' o f g r i p p e r - o r i e n t a t io n .hp( / i) : H e u r i s t i c o f g r i p p e r - p o s i t i o n .T h e h e u r i s t ic f o u n d b y t h e v a r i a t i o n s o f /9 i c a n b ee x p r e s s e d i n th e f o r m ( s ee A p p e n d i x A ) :

h ( / 9 ~ ) = 2 . A i . S ~ + 2 . B i . C ~ + N i (9 )T o d e t e r m i n e / 9 ~ o n e s e ts :

h' ( /9~) - Oh (/ i) _ 2 . A i . C i - 2 . B , . S ~ = 0 ; ( 10 )0/9~/ 9 i i s t h e n g i v e n b y

/ i = t a n - l ( A - ~ ) (11)

A c c o r d i n g t o ( 1 1 ) , t h e m a x i m u m a n d t h e m i n i -m u m v a l u e s o f ( 1 0) a l w a y s o c c u r p h a s e - s h i ft e d b y7 r . S o o n l y t h e s e t w o v a l u e s n e e d t o b e t e s t e d i n( 9 ) , c h o o s i n g / 9 * t o g i v e t h e s m a l l e s t h( / i) . T h i sc h o i c e c a n a l s o b e m a d e b y u s i n g t h e s i g n a l o ft h e h ' s s e c o n d - d e r i v a t i v e w i t h r e s p e c t t o t h e / 9 iv a r i a t i o n s . T h e d i f f e r e n c e i n t e r m s o f p r o c e s s i n gt i m e i s , h o w e v e r , n e g l i g i b l e .

5 .2 M i n i m i z a t i o n o f h ( 0 2 ) i o n = e r e , j # 2I n t h e c a s e o f t h e s e c o n d j o i n t, h( /92) i s f o u n da c c o r d i n g t o t h e p r e v i o u s p r o c e d u r e . I t c a n b ee x p r e s s e d i n th e f o r m ( s ee A p p e n d i x B ) :

h(/92) = 2 . 1 2 . ~ / A 2 + C 2o . s in(2/92 + a) +( 1 2 )

+ 2 . ~ / F 2 + G o . s in (/ 92 + f ~) + N 2 = 0 .

T h e e q u a t i o n 0 h ( / 9 2 ) / 0 / 9 2 = 0 i s t h u s a f o u r t h -d e g r e e a lg e b r a i c a l e q u a t i o n , w h i c h c a n b e e x -p r e s s e d i n t h e f o r m :

h ' (/ 9 2 ) = 4 .1 2 . ~ o + C ~ . cos(2/92 q- or) +( 1 3 )

+ 2 . ~ / F 2 + G ~ . c o s( /9 2 + / ~ ) = 0 .B a s e d o n e q u a t i o n ( 1 3 ) o n l y , t h e r e i s n o m o r ei n f o r m a t i o n t o g e n e r a t e r e s t r ic t i o n s t h a t c o u l dl e a d t o t h e i r a n a l y t i c a l f a c t o r i n g . S o t h e v a l u eo f / 9 2 w h i c h m i n i m i z e s h( /92) i s s o u g h t t h r o u g ha n u m e r i c a l a l g o r i t h m . S i n c e t h e o b j e c t i v e is t oa p p l y t h e s e a r c h m e t h o d i n re a l ti m e , s p e c i a l c a r ei s t a k e n t o d o t h i s w h i l e m a i n t a i n i n g i t s e f fi c ie n c y .O b s e r v i n g t h a t :

( a ) T o c a l c u l a t e h (o o 2 ) a n d ht( /92) f r o m t h ee q u a t i o n s ( 1 2 ) a n d ( 1 3 ) , o n l y a v e r y s m a l lc o m p u t a t i o n a l t i m e i s n e c e s s a ry , a n d( b ) h ( / 9 2 ) a n d h '( /9 2 ) a r e a l w a y s p e r i o d i c ,c o n t i n u o u s a n d s m o o t h ,

t h e m i n i m u m s a r e s o u g h t in t w o d i s ti n c t s t a g e s ,a s f o l l o w s :STAGE I ( f a s t s e a r c h ) : A r o u g h s e a r c h w i t h l o wp r e c i s i o n i s d o n e , u s i n g a l a r g e s w e e p i n g p r o g r e s -s i o n , t o i d e n t i fy a s u b - i n t e r v a l t h a t c o n t a i n s t h el o w e s t v a l u e s o f h ( / 9 2 ) , r e s t r i c t i n g t h i s s u b - i n t e r v a ls u c h t h a t h '( /9 2) i n c r e a s e s m o n o t o n i c a l l y w i t h i n i t .S TA G E I I ( h i g h - p r e c i s i o n s e a r c h ) : H e r e t h e N e w t o n -R a p h s o n m e t h o d i s a p p l i e d o v e r t h e s u b - i n t e r v a lo b t a i n e d i n ST A G E I , o b s e r v i n g t h a t i n t h i s s u b -i n te r v a l t h e m e t h o d i s c o n v e r g e n t t o t h e g l o b a lm i n i m u m . T h e N e w t o n - R a p h s o n m e t h o d i s c h o s e ns i n c e i t i s f a s t a n d e f f i c i e n t f o r e q u a t i o n s w i t ht h e c h a r a c t e r i s t i c s o f t h o s e o b t a i n e d f o r h'( oo 2)( D a h l q u i s t a n d B j o r c k , 1 9 7 4 ) .T h e N e w t o n - R a p h s o n m e t h o d u s e d h e r e c o n s i s t so f s t a r t i n g f r o m a n e s t i m a t e d p o i n t o b t a i n e d i nS TA G E I , f i n d i n g t h e t a n g e n t o f t h e c u r v e o f h'( /9 2 )a t t h is p o i n t , a n d a d o p t i n g a s t h e n e x t e s t i m a t et h e i n t e r s e c t i o n o f t h i s t a n g e n t w i t h t h e / 9 2 a x i s .S e e t h e z o o m e d c u r v e d e p i c t e d i n F ig . 4 ( c ). T a k i n ga l o o k a t F i g . 4 , i t c a n b e s e e n t h a t :

h'[/92(0)] (14)/ 92 ( 1) = / 9 2 ( 0 ) - t a n ( 7 )S i n c e t a n ( 7 ) = h " [ / 9 2 ( 0 )1 , t h e n :

h ' [ 0 2 ( 0 ) ]0 2 ( 1 ) = / 9 2 ( 0 ) h " [ / 9 2 ( 0 ) ] " (15)T o s e a r c h f o r t h e g e n e r i c p o i n t 0 2 ( k ) , t h e f o l l o w i n gr e c u r r e n t e q u a t i o n i s w r i t t e n :

/92(k + 1) = 02 (k) h ' [ O 2 ( k ) ]h " [ O 2 ( k ) ] " ( 1 6 )


6/11

1266 M.K. Madrid and A.G.P. Badan' h ( ~ 2 ) I n t e r v a l g o t t e n b y ,r o u g h s w e e p i n g . " -- ~,

I I: I I: I I I I

2~ r.. I: : o 2

I n t e r v a l o f s e a r c hb y f i n e s w e e p i n g .

. , ' f , /

_ _ . . . . ~ : ; r . . " ~ . . , - -

F i r s t e s t i m a t i o n9 2 ( I ) 0 2 ( 3 )

Fig. 4. Refining of ~ 2 b y Newton-Raphson.

( a )

( b )

0 2

(c)

O n e c a n s e e f r o m e q u a t i o n s ( 1 5) a n d ( 1 6) t h a tbes ides the f i r s t -de r iva t ive func t ion of h (02) , t hesecond-de r iva t ive func t ion i s a l so needed . Thi s i so b t a i n e d s i m p l y f r o m e q u a t i o n ( 1 3 ) :

h " ( 8 2 ) = - 8 . / 2 . ~ 0 2 + C 2 . s in (2 82 + a ) +

- 2 .~ F ~ + G02 . s in (~2 4- ~ ) = 0 .(17)

N o t i c e t h a t t h e v a l u e o f 8~ f o u n d b e l o n g s t o a su b -in t e rva l i n which h ' (02) i nc reases monotonica l ly .So once th e rough -sweep done in STAGE I hasg iven a f i r s t e s t ima te of ~2 whose va lue i s i nth i s sub- in t e rva l , t he fa s t - sw eep us ing the Newton-R~tphson r e c u r r e n c e , e x p r e s se d b y t h e e q u a t i o n(16) , converges to 8~ . Thi s i s a lways poss ib l e , s incet h e d e t e r m i n a t i o n o f t h e f a s t - sw e e p i n g i n te r v a l,shown in F ig .4(a ) , i s t r i v i a l .F o r r o b o t s w i t h d e g r e e s o f f r e e d o m w i t h m u t u a l l yc o p l a n a r m o v e m e n t s , t h e d e t e r m i n a t i o n o f t h e i rr e sp e c t i v e h e u r i s t i c s d e m a n d s so l u t i o n s t o e q u a -t i o n s o f f o u r t h d e g r e e o r h i g h e r, d e p e n d i n g o n t h en u m b e r o f t h e se m o v e m e n t s . I n t h e se c a se s t h i sn u m e r i c m e t h o d i s u se d , b e a r i n g i n m i n d t h a t t h esm a l l i n cr e a se s a n d d e c r e a se s o f c o m p u t a t i o n a lt i m e d e p e n d d i r e c t ly o n t h e n u m b e r O f o p e r a t i o n sn e c e s sa r y t o f in d t h e so l u t i o n s o f t h e p o i n t s p e r -t a in ing to the i r f i e lds . I t i s no t necessa ry to con-s i d e r t h e d e g r e e s o f t h e se e q u a t i o n s , t o c o n c l u d et h a t t h e c o n v e r g e n c e o f t h e m e t h o d i s e v i d e n ti n t h e s e n se t h a t t h e f u n c t i o n s i n v o l v e d i n t h e

d i r e c t k i n e m a t i c m o d e l i n g a r e p e r i o d i c , sm o o t ha n d b o u n d e d , a n d t h e h i g h - p r e c i s i o n s e a r c h e s a r ea lways done in a sub- in t e rva l where h(0 i ) i n -c r e a se s m o n o t o n i c a l l y b y v i r t u e o f t h e i m p o s i t io no f S T A G E I .

6. F L O W O F T H E H E U R I S T I C S E A R C HM E T H O D" ~ S t a r t ~

F i r s t p o i . t ~ f = ~ e t r a j e c t o r y- I

S a m p l i n g o f t h e p o i n t t o t r a c ki n t h e k - t h i t e r a t i o n . M a t r i x T k o b t a i n i n g .~ a d i n g t h e p o s i t i o n i n d i c a t e d tb y t h e j o i n t s e n s o r s . 1

F - M a t r i x A o b t a i n i n z . I ( J ~A p p l i c a t i o n o f t h e r o ' b o t I ~ Id i r e c t k i n e m a t i c m o d e l , j \[ Calculuso~h I

Y e s ~N o

b l

S e t - p o i n t sto s l a v e sO p e n a l l j o i n t s

I F i n d h ( O i ) /h (Oi ) - -- - m i n i s i ( h )

S e n d i n g t h e c l o s e dj o i n t p o s i t i o n s t ot h e r e s p e c t i v e s l a v e s .

Yes~j ~ n u m b e r o f t h e j o i n tb e t w e e n t h e o p e n e d j o i n t s ,w h i c h o f f e r s t h e s m a I l e s t h .

!

I j o i n t ( j ) m c lo s e d I

F i g . 5 . F l o w o f t h e H e u r i s t i c S e a r c h M e t h o d .7. C O M P U T A T I O N A L A S P E C T S

A bas i c p r inc ip l e fo r t he a ppl i ca t ion of a com-p u t a t i o n a l a l g o r i t h m i n r e a l t i m e i s t o m i n i m i z et h e n u m b e r o f n e c e s sa r y o p e r a t i o n s , i n su ch aw a y t h a t t h e t i m e t a k e n i s l e ss th a n t h e r e a c t i o nt i m e o f t h e p h y s i c a l e le m e n t s o f t h e p r o c e s s t ob e c o n t r o l l e d . T h e a l g o r i t h m p r o p o se d h e r e i si t e ra t ive , and each i t e ra t ion re spec t s t h i s bas i cr u l e, m a i n t a i n i n g g o o d p e r f o r m a n c e d u r i n g t h ep a t h t r a ck i n g . L o o k i n g a t t h e e q u a t i o n s o f t h ei n d i v i d u a l j o i n t m o v e m e n t s , t h e t r a c k i n g c o n d i -t i o n s i m p o se d b y t h e t r a j e c t o r ie s , a n d a l so t h ec o n f i g u r a t i o n s a s su m e d b y t h e r o b o t , i t i s c l e a rt h a t t h e r e q u i r e d a r i t h m e t i c o p e r a t i o n s a r e n o t


7/11

Heuristic Search Method for Continuous-Path Tracking Optimization 1267o v e r l y c o m p l e x . T h e m o s t i m p o r t a n t a s p e c t , f r o mt h e c o m p u t a t i o n a l p o i n t o f v ie w , is t h a t t h e r ea r e r e p e t i t i v e p r o c e d u r e s . M o r e o v e r , i n a s i n g l ei t e r a t i o n , s i n c e t h e s e t - p o i n t o f e a c h j o i n t i s c a lc u -l a t e d w h i l e t h e o t h e r s a r e h e l d s t a t i c , t h e c a l c u l u so f t h e s i n e s a n d t h e c o s in e s c a n b e d o n e a l l a to n c e a n d k e p t c o n s t a n t t i l l t h e e n d o f e a c h s te p ,m e a n i n g t h a t d u r i n g p r a c t i c a l l y a l l t h e p r o c e s s i n g ,o n l y t h e f o u r b a s ic a r i t h m e t i c o p e r a t i o n s n e e d b ec a l c u l a t e d . A l s o , d u r i n g t h e p r o c e s s i n g e v o l u t i o nt h e r e a r e i n t e r m e d i a t e d a t a t h a t c a n b e s t o r e dt e m p o r a r i l y a n d r e u s e d l a t e r , a v o i d i n g r e p e t i t i v eo p e r a t i o n s . A n o t h e r i m p o r t a n t o b s e r v a t i o n i s t h a ts e v e ra l c a l c u l a t io n s d o n o t n e e d t o b e p r o c e s se di n r e a l t i m e ( S e e A p p e n d i x ) . T h e y a r e p r e s e n t e ds i m p l y f o r a n a l y s i s .S o f t w a r e w a s d e v e l o p e d o n t h e b a s is o f t h e a l -g o r i t h m s h o w n i n F ig . 5 , a n d i n t h e e q u a t i o n sg e n e r a t e d b y t h e t e c h n i q u e a p p l i c a t i o n , t o s e e h o wi t d r iv e s t h e r o b o t m o v e m e n t s . T h i s s o f t w a r e h a sa g r a p h i c a l i n t e r f a c e t h a t s h o w s , a t e a c h i t e r a t io n ,t h e e v o l u t i o n o f h t o t h e s t r u c t u r a l c o n f i g u r a t i o na s s u m e d b y t h e r o b o t a t a s p e c if i ed p r o c e ss i n gm o m e n t , a n d t o t h e p o i n t b e i n g t ra c k e d .T o s h o w h o w t h e s e a r c h a l g o r i t h m p r o c e e d s , at r a c k i n g i n s t a n t o f a c o n t i n u o u s p a t h i s c h a r a c t e r -i z ed , c o n s id e r i n g t h a t t h e p o i n t k- 1 i s f a r f r o m t h ep o i n t k . A l t h o u g h , h e r e , c o n t i n u o u s t r a j e c t o r i e sa r e d e a l t w i t h , t h e m e t h o d w a s t e s t e d i n c l u d i n gd i s t u r b a n c e . T h e e n d - e f f e c t o r w a s d i s t u r b e d f r o mi t s o ri g i n a l p l a n n e d t r a j e c t o r y , b y e x t e r n a l a c t i o n .T h e p u r p o s e w a s to t e s t t h e r o b u s t n e s s o f t h em e t h o d . C l e a r l y , b e t w e e n t w o c o n s e c u t i v e p o i n t so n a c o n t i n u o u s t r a j e c t o r y , t h e p e r f o r m a n c e w i l lb e b e t t e r , a n d m o r e o v e r t h e t r a c k i n g o f 0 i b e t w e e n- T r a n d 7r w i l l n o t a l w a y s b e n e c e s s a r y , r e m e m b e r -i n g t h a t f o r n e a r b y p o i n t s t h e i r v a r i a t i o n s w i ll b es m a l l .A s a n e x a m p l e , g i v e n t h e m a t r i x Tk:

0 . 18 0 . 69 0 . 70 0 . 4043"0 . 93 0 . 09 --0 . 34 0 . 3165Tk = - -0 . 30 0 . 72 - -0 . 63 0 . 8286

0 0 0 1a n d t h e j o i n t s o f t h e Jeca II , a f t e r t h e d i s t u r -ban ce , a re po s i t io ned in : 01 = 60 . 0 , 02 = 30 . 0 ,03 = 10.0 , 04 :-- 20 . 0 , 05 = 60 . 0 . W he n the d i -r e c t k i n e m a t i c m o d e l i s a p p l i ed t o t h e f i r s t st e p o ft h e s e a r c h , t h e m a t r i x A w i l l b e :

- 0 . 3 3 - 0 . 2 0 0 .9 2 0 .0 25 4"- 0 . 2 3 - 0 . 9 3 - 0 . 2 8 0 .0 7 82A - " 0 . 9 2 - 0 . 3 0 0 . 2 6 0 . 8 2 4 5

0 0 0 1

I n t h i s f i r s t s t e p , f i v e c u r v e s , e a c h c o r r e s p o n d i n gt o t h e e v o l u t i o n o f h f o r a j o i n t - t h e o t h e r s b e i n gs t a t i c - a r e p r e s e n t e d i n F i g . 6 .

2.001.801.601.401.201.000.800.600.400.20

0

~(0,) 2 , ? i a l

0 2 r ~ O i

F i g . 6 . E v o l u t i o n s o f h i n t h e f i r s t s t e p .T h e " D i a l " o n t h e g r a p h i n d i ca t e s t h e j o i n t t h a t o f-f e r s t h e b e s t m o v e m e n t , a n d w h i c h w i ll b e c h o se na t t h i s s t e p . W h e n j o i n t 3 is c lo s e d, t h e a l g o r i t h mp r o c e e d s t o t h e s e c o n d s t e p . T h e c u r v e s ar e s h o w nin F ig . 7 .2.40 h ( O i ) 22.20 ~2.001.801.601.401.201.000.800.600.400.20o i , :t

2~r O

F i g . 7 . E v o l u t i o n s o f h i n t h e s e c o n d s t e p , w i t h 0 3c losed .

I n t h e s e c o n d s t e p , j o i n t 2 i s c h o s e n . T o s h o w t h ef i rs t c o m p l e t e i t e r a t i o n o f t h e a l g o r i t h m , t h e n e x tt h r e e f i g u r e s ( F i g s. 8 , 9 a n d 1 0 ) a r e b a s e d o n j o i n t sa l r e a d y c h o s e n . T h e y w e r e g e n e r a t e d a c c o r d i n g tot h e n e x t t h r e e s u c c e s s i v e s t e p s .2.20 h(Oi)2.00 11.801.60 / /1.401.20 51.00 Jo.8o , /0.600.400.20

00 2rr ~Oi

F i g . 8 . E v o l u t i o n s o f h i n t h e t h i r d s t e p , w i t h 02a n d 0 3 c l o s e d .

A f t e r t h i s f ir s t s t e p - s e q u e n c e , t h e s e t - p o i n t s o f t h ejo i n t s e rv om ech anis m s a re : 01 = 40 . 15 , 02 = 65 . 0 ,03 = 109 . 3 , 04 = 44 . 57 , 05 = 56 . 42 . W i th thesen e w p o s i t i o n s , t h e m a t r i x A b e c o m e s

0.41 0.75 0.51 0.3987"0 . 8 6 - 0 . 1 3 - 0 . 5 0 . 3 8 7 1A = - 0 . 3 1 0 .6 4 - 0 . 7 0 0 .7 0 1 4

0 0 0 1


8/11

1268 M.K . Madrid and A.G.P."Badan

2.50

2.00

1.50

1.00

0.50

0

h(0,)

J

2.50

2,00

1.50

1.00

0.50

0

h(e~)

F i g . 9 . E v o l u t i o n s o f h i n t h e f o u r t h s t e p , w i t h (71,(?2 and (?3 closed.

2 . 5 0

2 . 0 O

1.50

1.00

0.50

0

h(01)

5

2~r i

F i g . 1 0. E v o l u t i o n s o f h i n t h e f i f t h s t e p , w i t h (71,(?2, (?3 and (?4 closed.

B e f o r e t h e f i r s t s t e p - s e q u e n c e is p r o c e s s e d , t h eh e u r i s t i c c a l c u l a t e d t h r o u g h t h e e q u a t i o n ( 3 ) w a s447 .62 10 -3 . N ow i t i s 145 .61 10 -3 .I f t h e r e i s n o m o r e t i m e t o p r o c e s s a n e w s t e p -s e q u e n c e, b e c a u s e o f t h e c o m p u t a t i o n a l c a p a c i t yo f t h e c o n t r o l s y s t e m , a n d i f t h e t a s k i n e x e c u t i o nd e m a n d s b e t t e r t r a c k in g a c c u r a c y t h a n t h a t o b -t a i n e d s o fa r , t h e s e t - p o i n t s f o u n d a r e i m m e d i a t e l ys e n t t o t h e s e r v o m e c h a n i s m s . A n e w s e a r c h is t h e ni n i t ia t e d , k e e p i n g t h e s a m e m a t r i x Tk w h o s e o b -j e c t i v e w a s n o t a c h i e v e d . I f t h e r e s t i ll r e m a i n s t i m et o p r o c e s s a n e w s t e p - s e q u e n c e b e f o r e s e n d in g o f ft h e n e w s e t - p o i n t s , t h e n t h i s i s d o n e .E x e c u t i n g a s e c o n d s te p - s e q u e n c e , t h e n e w p o -sit ion s are : (71 = 35-0 , (?2 = 64 .13 , (?3 = 91. 25 ,(?a = 47 .08 , (?5 = 40 .2 . Th e c o r re s po nd i ng ma -t r i x A i s t h e n :

F i g . 1 1. E v o l u t i o n s o f h a f t e r t h e t e n t h s t e p -s e q u e n c e .

A =0 .18 0 .69 0 .70 0 .4043"0 . 9 3 0 . 0 9 - 0 . 3 4 0 . 3 1 6 7

- 0 . 3 0 0 . 7 2 - 0 . 6 3 0 . 8 2 8 60 0 0 1

a n d t h e h e u r i s t i c : 0 . 1 6 * 1 0 - 3 .A f t e r 1 0 i t e r a t i o n s , t h e p o s i t i o n i n g e r r o r i s i n t h er a n g e o f 0 .1 m m , a n d i n o r i e n t a t i o n , b a s e d o n t h en u m e r i c a l p r e c i s io n u s e d o n b o t h m a t r i c e s Tk a n dA , i t i s nu l l .T h e a v e r a g e t i m e r e q u i r e d f o r e a c h o f t h e s e s t e p -s e q u e n c e s w a s 9 . 2 7 6 m s u s i n g a n Intel ~86DX2CPU, a n d 5 . 8 2 3 m s u s i n g a n Intel ~86DX~ CPU.I t i s i m p o r t a n t t o n o t i c e t h a t f o r t w o c o n s e c u t i v ep o i n t s o n a c o n t i n u o u s t r a j e c t o r y , t h e h v a l u esa r e v e r y s m a ll ; t h e r e f o r e t h e n u m b e r o f n e ce s s a r ys t e p - s e q u e n c e s w i l l b e m u c h l o w e r t h a n i n t h ee x a m p l e g i v en .N o t i c e t h a t t h e f i g u r e s p r e s e n t e d i n th e e x a m p l es h o w t h e e v o l u t i o n s o f h w h e n e a c h j o i n t i s h y p o -t h e t i c a l l y i n m o t i o n a n d a l l t h e o t h e r s a r e s t a t i c ,a l w a y s c o n s i d e r i n g t h e s a m e i n i t i a l c o n d i t i o n s , a sa f u n c t i o n o f t h e e n d - e f f e c to r p o s i ti o n i n g a n d o ft h e p o i n t t r a c k e d . S h o w i n g a l l t h e s e e v o l u t i o n si s a i m e d a t p r e s e n t i n g a g r a p h i c a l i n t e r p r e t a -t i o n o f t h e p r o c e d u r e s e x e c u t e d d u r i n g t h e s t e p -s e q u e n c e s . O n c e t h e a n a l y t i c a l e q u a t i o n s h a v eb e e n d e t e r m i n e d , o r g i v e n a n u m e r i c a l m e t h o dt o f i n d t h e m i n i m u m s o f t h e c u r v e s i n d i c a t e d b yt h e " D i al " , it i s s u f f i c ie n t t o c a l c u l a t e t h e v a l u e s o ft h e s e m i n i m u m s d i r e c t l y a t e a c h s t e p .

A =0 . 3 3 0 . 6 9 0 . 6 4 0 . 4 0 0 1 '0 .9 2 - 0 . 0 9 - 0 . 3 8 0 .3 4 84

- 0 . 2 0 0 .7 2 - 0 . 6 6 0 .8 0 450 0 0 1

N o w t h e h e u r i s t i c i s: 4 0 . 2 1 * 1 0 - 3 . P r o c e e d i n g i nt h i s w a y , a f t e r 1 0 s t e p - s e q u e n c e s t h e e v o l u t i o n s o fh t o t h e i n d i v i d u a l m o v e m e n t s o f e a c h j o i n t a r es h o w n i n th e g r a p h o f F i g . l l .T h e c h o s e n j o i n t s i n t h i s i t e r a t i o n h a v e t h e f o l lo w -ing val ues : (71 = 30 .02 , (?2 = 60 .0 , (?3 = 85. 01 ,(?4 = 50 -0 , (?5 = 20 .05 . T he m a t r ix A re m a ins :

8 . C O N C L U S I O NV e r i f y in g t h e a p p l i c a t i o n o f t h e p r o p o s e d t e c h -n i q u e i n t h e Jeca H r o b o t , i t i s n o t e d t h a t t h ec o m p u t a t i o n a l a l g o r i t h m r e s p o n s i b l e f o r t h e t r a -j e c t o r y c o n t r o l o f t h i s r o b o t c a n b e e x e c u t e d b y ac o m p u t e r w i t h l o w c o m p u t a t i o n a l c a p a c i t y , e v e ni n r e a l t i m e , b e c a u s e t h i s a l g o r i t h m i s v e r y s i m p l e ,a n d i s d e p e n d e n t 0 n l y o n t h e d i r e c t k i n e m a t i cm o d e l o f t h e r o b o t .I n t h e m a n n e r p r e s e n t e d h e r e , t h e p r o p o s e d te c h -n i q u e c a n b e ap p l i e d t o r o b o t s w i t h a n y n u m b e r o f


9/11

Heuristic Search M e t h o d f o r C o n t i n u o u s- P a t h Tracking Optimization 1269d e g r e e s o f f r e e d o m , b e c a u s e t h e s e a r c h e s a r e d o n ei n s e q u e n t i a l f o r m , f i n d i n g t h e b e s t c o n t r i b u t i o nt h a t e a c h j o i n t c a n o f f e r t o t h e m o v e m e n t o f ad e t e r m i n e d s t e p . I t i s a l so c o n v e r g e n t w i t h i n t h e i rw o r k sp a c e s , s i n c e t h e t r a j e c t o r i e s a r e r e a c h a b l ei n p o s i t i o n a n d o r i e n t a t i o n , e v e n w h e n N < 6 . I fthe re i s no scheduled re s t r i c t ion in the a lgor i thm,a n d i f t h e r e i s m o r e t h a n o n e so l u t i o n fo r t h e s a m et r a c k i n g p r o b l e m , t h e so l u t i o n s a r e i n t r i n s i c a l l yc o n v e r g e n t to t h o se w i t h t h e m i n i m u m u se o f e n -e r g y , d e p e n d i n g o n t h e a s su m e d c o n f i g u r a t i o n s o ft h e s t r u c t u r e o f t h e r o b o t s , a n d o f t h e p o i n t s b e i n gt racked . Clea r ly , t he de l ay un t i l t he convergencesa r e a c h i e v e d c a n b e g r e a t e r o r l e s s , d e p e n d i n go n t h e p r e c i s i o n i n v o l v e d , a n d t h e c o m p u t a t i o n a lc a p a c i t y o f th e c o n t r o l sy s t e m e m p l o y e d . I n t h i scase , t he d ec i s ive c r i t e r i a a re tho se tha t o f fe r t heb e s t c o s t / b e n e f i t e c o n o m i c r e l a t i o n .T h e h i g h e r t h e n u m b e r o f d e g r e e s o f f r e e d o ma r o b o t h a s , t h e h i g h e r w i l l b e t h e n u m b e r o fe l e m e n t a r y o p e r a t i o n s r e q u i r e d t o c a l c u l a t e t h ee l ement s o f i t s ma t r i x A . F i r s t ly , t h i s t ends tod e m a n d m o r e c o m p u t a t i o n a l c a p a c i t y in t h e u s e o fo t h e r t e c h n i q u e s o f o n - l in e c o n t r o l o f tr a j e c t o r i e sd u e t o a n i n c r e ase o f t h e c o m p l e x i t y o f m a t h e m a t -i ca l mod e l l ing of t he r obo t . A l so , ana lyz ing thet e r m s i n s i ne s a n d c o s i n es o f t h e m a t r i x A , n o t i c etha t t hey a lways a r i se in a repe t i t i ve way , be ingc o n s i d e r ed a s c o n s t a n t s i f t h e c a l c u l a ti o n i s d o n eo n l y o n c e a t t h e b e g i n n i n g o f e a c h s t e p - se q u e n c eo f t h e p r o p o se d a l g o r i t h m . T h e se t r i g o n o m e t r i c a lo p e r a t i o n s a r e t h e o n e s t h a t d e m a n d t h e h i g h e s tn u m b e r o f m a c h i n e - c l o c k c y c le s d u r i n g t h e a l g o -r i t h m p r o c e s s in g , e sp e c i a l ly w h e n a p p l i e d t o ar o b o t w i t h a l a r g e n u m b e r o f d e g r e e s o f f r e e d o m .However , i t i s s t i l l poss ib l e to use i t t o t he on-l i n e p a t h t r a c k i n g c o n t r o l w i t h o u t n e e d i n g l a r g ec o m p u t a t i o n a l c a p a c i t y .A n o t h e r b i g a d v a n t a g e i s t h e p o s s i b i l it y o f u s i n gt h i s t e c h n i q u e i n t h e c o n t r o l o f t h e c o n t i n u o u s -p a t h t r a c k i n g i n r e d u n d a n t r o b o t s , t o f i n d so l u -t i o n s f o r t h e s i n g u l a r p o i n t s o f t h e i r s t r u c t u r e s .A l so , due to ve r i f i ca t ions rea l i zed dur ing the de -v e l o p m e n t o f t h e e q u a t i o n s f o r t h e s e c o n d D O F o fth e Jeca H r o b o t , t h i s t e c h n i q u e c o u l d b e u se d t oh e l p in t h e m e c h a n i c a l p r o j e c t f o r r o b o t s t r u c t u r e sr e f e r r e d t o e a r l i e r . S t a r t i n g f r o m m a t h e m a t i c a lm o d e l s m o r e su i t e d t o t h e a p p l i c a ti o n s , o p t i m i z e ds t r u c t u r e s c a n b e o b t a i n e d , a v o i d in g , f o r e x a m p l e ,t h e j o i n t s w i t h c o l li n e a r r o t a t i o n a l a x e s .

9 . R E F E R E N C E SB a d a n , A . G . P , M . K . M a d r i d , M . A . D i a s a n d S . A .O h f u g i ( 1 9 9 2 ) . S o f t w a r e o p e r a t i o n a l s t r u c -

t u r e t o r o b o t m a n i p u l a t o r c o n t r o l v i a p a r a ll e lp rocess ing . V C o n g r e s o L a t i n o - A m e r i c a n o d eCont ro l A u tomdt i co .

C h a n g , Y . H . , T . T . L e e a n d C . H . L i u ( 1 9 9 2 ).O n - l i n e a p p r o x i m a t e c a r t e s i a n p a t h t r a j e c -t o r y p l a n n i n g f o r r o b o t i c m a n i p u l a t o r s . I E E ETrans . on Sys t ems , Man, and Cyberne t i c sV o l . 2 2 , N o . 3 , 5 4 2 - 5 4 7 .Cra ig , J . (1989) . In t roduc t ion to Robot i c s : Me-chanics and Control . A d d i so n - W e s l e y , P u b -

l i sh i n g C o m p a n y . M a ss . , U S A .D a h l q u i s t , G . a n d A . B j o r c k ( 1 9 7 4 ) . N u m e r i c a l

Methods . Pren t i ce Ha l l , Eng lewo od C l if fs . N J ,U S A .

D a w so n , D . M , Z . Q u a n d J . J C a r r o l l ( 1 9 9 2 ) .Track ing cont ro l o f ri g id- link e l ec t r i ca l ly -d r i v e n r o b o t m a n i p u l a t o r s . In t . J . Cont ro lV o l . 56 , N o . 5 , 9 9 1 - 1 0 0 6 .

D e n a v i t , J . a n d R . S . H a r t e n b e r g ( 1 9 5 5 ) . A k i n e -m a t i c n o t a t i o n f o r l o w e r p a i r m e c h a n i sm sb a se d o n m a t r i c e s . A S M E J . A p p . M e c h .V o l . 7 7 , 2 1 5 - 2 2 1 .Fu , K .S . , R .C. Gonza lez and C.S .G . Lee(1987). Robot ics , Control , Sens ing, Inte l l i -gence. M c G r a w - H i ll B o o k C o m p a n y .

Kor f , R .E . (1988) . Rea l - t im e heur i s t i c sea rch : Newresu l t s . A u t o m a t e d R e a s o n i n g p p . 1 3 9 - 1 4 4 .L e w i s , F . L . , C . T A b d a l l a h a n d D . M . D a w -

son (1993) . Contro l o f Robot Manipula tor s .M a c m i l l a n P u b l i sh i n g C o . . N Y , U S A .M a d r i d , M . K . a n d A . G . P . B a d a n ( 1 9 9 0 ) . C o n t r o l e

d e p o s i ~ o e v e l o c i d a d e d e m a n i p u l a d o r e sm e c ~ n i co s c or n j u n t a s e m m a l h a a b e r t a ema lha fechada , 8 congresso bras i l e i ro de au-t o m t i c a . S B A V o l . 1 , 5 0 6 - 5 1 2 .

Madr id , M.K. and F .H .M Monas t@rio (1993) .B f i sq u e d a h e u r i s ti c a e n t i e m p o r e a l p a r a l ag e n e r a c i 6 n d e t r a y e c t o r i a s d e r o b o t s m a n i p -u l a d o r e s . V Conferenc ia de La Asoc iac i6nEspa~ola Para La In te l igenc ia Ar t i f i c ia l ,M a d r i d - S p a i n V o l . 1 , 2 6 9 - 2 7 9 .

Ma dr id , M .K. (1994) . Con t ro l e de t ra -j e t 6 r i a s c o n t i n u a s p o r s e c c i o n a m e n t o e m su b -t ra j e t6r i a s usando in t e l ig@nc ia a r t i f i c i a l numr o b 5 m u l t i- t a r ef a s . D o c t o r a l t e s is . F a c u l d a d ede Engenhar i a E l@t r i ca de de Computaq~o- U n i c a m p . C i d a d e U n i v e r s i t r i a " Z e f e r i n oVaz" , Campinas SP - Braz i l .

Pea r l , J . (1985) . Heur i s t i c s - In t e l l igen t SearchSt ra teg ies For Computer Problem Solv ing .A d d i so n - W e s l e y P u b l i sh i n g C o m p a n y , I n c . .Mass . , USA.

S n y d e r , W . E . ( 1 9 8 5 ) . Indus t r ia l Robot s - Com-puter In t e r fac ing and Cont ro l . Prent i ce Ha l l ,Inc . En glewo od Cl if fs . N J , USA.

S p o n g , M . W . ( 1 9 8 7 ) . M o d e l i n g a n d c o n t r o l o fe l a s t i c j o in t robot s . T r a n s . A S M E , J . D y n .Sys . Meas . Ctr l . V o l . 1 0 9 , 3 1 0 - 3 1 9 .


10/11

1270 M.K . Madr id and A .G .P . BadanA P P E N D I X A

T e r m s f o r t h e d e t e r m i n a t i o n o f 8 ~ i = 1 , 3 , 4 , 5 .I n t h e c a s e o f i = 1 t o o b t a i n h (0 x) = h o ( 0 1 ) + h p ( 0 D ,t h e f o l l o w i n g e r r o r v e c t o r s a r e f ir s t o b t a i n e d :

(S23C405 - - C23S5).61 - - ( 8 4 6 5 ) . S 1 - r i t z ]e n ( ~ l } : ( S 4 C 5 ) . C I + ( $ 2 3 C 4 C 5 - 0 2 3 S 5 ) . 8 1 - " T y ]C 2 3 6 4 0 5 + $ 2 3 S 5 - r i T z )

[ - ( 8 2 3 C 4 8 5 + 6 2 3 0 5 ) . 6 1 + ( $ 4 S 5 ) . 8 1 - S T = ] 8 ( ~ 1 ) = / - - ( 8 4 S 5 ) ' 6 1 - - ( S 2 3 6 4 S 5 "4" 6 2 3 0 5 ) . S 1 S T y ]. ( S 2 3 6 5 - - 6 2 3 6 4 S 5 - - S T z )

[ ( 8 2 3 S 4 ) . 6 1 + ( 6 4 ) . S I - C L T z ]e a ( @ l ) = / - - ( 6 4 ) . 6 1 " 4- ( $ 2 3 5 4 ) . S 1 - - O,T y ]( 6 2 3 S 4 - a T z )T h e q u a d r a t ic e r r o r s e n T ( e l ) . e n ( 8 1 ) , s r ( o 1 ) . e s ( 8 1 ) ,a n d e a T ( 8 1 ) . e a ( 8 , ) r e c a l cu l a te d , n d t h r o u g h s i m -p l e a l ge h r a i c a l m a n i p u l a t i o n s t h e t e r m s a r e r e-a r r a n g e d t o s h o w t h o s e t h a t a r e f u n c t i o ns o f 8 1.T h e s e m a n i p u l a t i o n s y i el d :e n T ( @ l ) . e n ( 8 1 ) =( 52 ~ 64 65 - c ~ s W + s ~ c ~ + c ~ c ~ c ~2 2+ 2 . $ 2 3 6 2 3 6 4 S 5 6 5 + $ 2 3 S 5- 2 . ( $ 2 3 6 4 C 5 - 6 2 3 S s ) . ~ t T z. C l + 2 . S 4 C s . n T z . S l + n ~ - , z- 2 . ( $ 2 3 C 4 6 5 - 6 2 3 S s ) . n T y . S I - 2 . S 4 C5 . n T tl . C l + n ~ . y- - 2 " 0 2 3 0 4 0 5 " n T Z - - 2 " $ 2 3 S 5 " " T z + " ~ z

E s T( O 1 ) .E S ( O 1 ) : 2 ~ + s23c~( 8 2 3 C 4 8 5 "4- C 2 3 C 5 ) 2 + S4S5~2 f,262- 2 . $ 2 3 C 2 3 6 4 S 5 6 5 + ~F23t-,4 o 5+ 2 . ( $ 2 3 C 4 S 5 + 6 2 3 C 5 ) . 8 T z . 6 1 - - 2 . $ 4 8 5 . 8 T z . S 1 + S~.=+ 2 . ( $ 2 3 6 4 S 5 + 6 2 3 6 5 ) . 8 T y . S 1 + 2 . S 4 S s . S T y . C l + STy2- - 2 . S 2 3 6 5 . 8 T z + 2 . C 2 3 6 4 S 5 . 8 T z + 5~ ,z

e a T ( 0 1 ) . e a ( 8 1 ) =s ~ + 6 2-2 .$23S4.aT=.CI - 2 . C 4 . a T = . S 1 -~- O*~z- 2 . S 2 a S 4 . a T y . S 1 + 2 . C 4 . a T y . C 1 + a~ ,y- 2 . C 2 a S 4 . a T z + a 2 z

N o w ho(O1) c a n b e d e t e r m i n e d , a d d i n g t h e e r r o rse n T ( O 1 ) . e n ( O 1 ), e s T ( 8 1 ) . e s ( O 1 ) , a n d e a T ( O 1 ) . e a ( O 1 ). T h et e r m s a r e a g a in r e - a r r a n g e d t o s h o w t h o s e t h a t a r ef u n c t i o n s o f 8 1, o b t a i n i n g :

e ~ { 0 1 ) : [ e ig x e ~ y e~ , ] T =[ 1 2 . C 2 - ( 1 3 + ~ 4 ) C 2 3 - - ~ 5 C 2 3 8 5 - I- ~ 5 S 2 3 C 4 C 5 ] C I . . . .( / 5 8 4 0 5 ) 0 1 + [ /2 6 2 - ( /3 + / 4 ) 6 2 S - / 5 6 2 3 8 5 . . ..ll + 12S2 + (Is +/4)$2s + isS2sS5 ....

- - ( 1 5 S 4 C 5 ) S 1 - P T z ]. . .. + 1 5 S 2 3 6 4 0 5 ] S 1 - P T y ]... + 1 5 6 2 3 6 4 6 5 - - P T z

a n d w i t h t h e f o l l o w i n g d e f i n i t i o n s :aO l = 1262 - (Is + 14)625 - 1 5 C 2 s& + 158256465bO l = / 5 S 4 6 5Cl ----- -PT=dOl - - - - - -PTyf 0 1 : / 1 + 12S2 "4- (13 + 14)S 23 + 1 5823 S5-4-

+ 1 5 C 2 3 C 4 6 5 - - P T z ,o n e c a n w r i t e :

[ aol.C1 - box.S1 + COle p ( 8 1 ) - - - - - ] b o l . C l + a O l . S l + d O l ] .

I . IoiC a l c u l a t i n g t h e q u a d r a t i c e r r o r e p T ( O z ) . e p ( 8 1 ) , a n dr e a r r a n g i n g t h e t e r m s t o s h o w t h o s e t h a t a r ef u n c t i o n s o f e l ,

h p ( 8 1 ) = 2 . A p l . S 1 + 2 . B p l . C 1 + N p l

w h e r eA p l = aO l .do1 - bOl .COlB p l = a o l .C ~ l + b o l. d O l

T h e n , t h e h e u r i s t i c f o u n d b y t h e v a r i a t i o n s of 0~c a n b e e x p r e s s e d i n t h e f o rm :

h(01) = 2 . A 1 . S 1 + 2 .B1 .C1 + N 1

w h e r eA 1 = A o l + A p lB 1 = B O l + B p lN 1 = N o l + N p l .

B y t h e s a m e p r o c e d u r e a d o p t e d t o i = 1 , h ( e ~ )i = 3 , 4 ,5 , c a n b e e x p r e s s e d i n t h e f o r m :

h o (0 1 ) = 2 . A o l . S 1 + 2 .B O l . 6 1 + N O l h ( O i ) = 2 . A i . & + 2 . B i . C i + N i

w h e r eA O l = S 4 6 5 . T gT z - - ( $ 2 3 6 4 6 5 - 6 2 3 S s ) . n T y - - $ 4 S 5 . 8 T z

+ ( $ 2 3 6 4 S 5 + 6 2 3 6 5 ) . 8 T y - - 6 4 . a T z - - S 2 3 S 4 . a T yB o l = - ( $ 2 3 6 4 6 5 - C 2 3 S 5 ) .n T = - - S 4 C s .f l, T v+(S2sC4& + C2sCs) .sT= + S4&.STv

- S 2 3 S 4 . a T z - 4 - C4.t%TyN Ol = 3 + . L + - ~ y + - ~ + s L + 4 y + 4 , ++ a 2 x + a ~ y + 4 ~

- - 2 . [ ( 6 2 3 6 4 6 5 + S 2 3 S s ). n T z ++ ( $ 2 3 C 5 - C 2 3 C 4 S s ) . S T z -4- C 2 s S 4 . a T z ] .

To d e te r min e h p ( 0 , ) , t h e e q u a t i o n f o r t h e e r r o rep(81) is w r i t t e n a c c o r d i n g t o :

w h e r eA i = A o ~ + A p iB i = B o i + B p iN i = N o i + N p l .

I n t h e c a s e o f i = 3 ,A 0 3 = - [ ( 6 2 6 4 6 5 + S 2 S s ) . ~ tT z + ( $ 2 6 5 - C 2 6 4 S 5 ) . 8 T z ++C2S4.aT=]C1

- - [ ( 6 2 C 4 6 5 + S 2 S s) . T t T y + ( $ 2 6 5 -- 6 2 6 4 S 5 ) . 8 T y ++62 S4.arv]Sl- ( C 2 & - & C 4 C s ) . n T , - - ( C 2C s + S2C4& ) . ST~++ S 2 S 4 , a T z

A p 3 = ( a o 3 . b o 3 + c 0 3 . g 0 3 ) S 2 + ( a o 3 . g o 3 - b o 3 . c o 3 ) 6 2 ++ 1 2 [ a o s . c o s ( 2 0 2 ) + c o 3 . s i n ( 2 8 2 ) ]


11/11

Heurist ic Search M ethod for Continuous-Path Tracking Optimization 1271B 0 3 = - [ ( $ 2 C 4 C 5 - C 2 S 5 ) . n T z - - ( S 2 6 4 S 5 + C 2 C 5 ) . 8 T z +

+ S 2 S 4 . a T z ] C l- [ ( 3 2 6 4 6 5 - C 2 S 5 ) . n T v - - ( $ 2 C 4 S 5 + C 2 C 5 ) . 8 T v +

+ S 2 S 4 . a T y ] S 1- ( C 2 6 4 6 5 + S 2 S 5) A % T z + ( 6 2 6 4 S 5 - S 2 65 ) . 8 T z +- - C 2 S 4 . a T z

B p 3 = ( a o 3 . 9 o 3 - - b o 3 . c o 3 ) $ 2 - - ( a o s . b o 3 + C 0 3 . 9 0 3 ) C 2 +--12 [a o 3 . s i n ( 2 0 2 ) - - c o 3 . c o s ( 2 0 2 ) ]

N o 3 = 3 + = ~ x + ~ , + ~ , + 4 ~ + 4 , + 4 z +2 2 2+ a T z + a T v + a T z ++ 2 . [S 1 ( S 4 C s . n T z - $ 4 S 5 . 8 T z - - C 4 . a T z )- e l ( S 4 C s. n T y - S 4 S s . S T y -- C 4 . a T y ) ]

N p 3 = 2 . 1 2 . ( 9 0 3 . $ 2 + b 0 3 . C 2 )where

ao3 = 1~C4C5b o 3 = d o s . C 1 + l o s . S 1co3 7- 3 + 14 + 1 5 S 5do3 = - 1 5 S 1 S 4 6 5 - P T zf o 3 ---- 1 5 C 1 S 4 6 5 - P T yg 0 3 = l l - - p T z

In the case of i = 4,A 0 4 = ( C s .r t Tx - - S S . 8 T x - - S 2 3 . a T , ) S I +

- - ( C s . n T y - - S S . S T y + S 2 3 . a T z ) C l - - C 2 3 . a T zA p 4 = l s ( b o 4 . C l - a 0 4 . S 1 ) C 5B o 4 7 - - - [ S 2 3 ( 6 5 . n T y -- S 5 . S T , ) W a T z ] S I +- - [ S 2 3 ( C s . n T x - - S 5 . S T z ) - - a T y ] C , +- - ( C 5 . n T z - - S 5 . S T z ) 6 2 3

B p 4 7 - 1 5 . [( a o 4 .C 1 + b o 4 . S 1 ) $ 2 3 + c o 4 . C 2 3 ] C 5

2 2 2+ a T ~ + a T ~ + a T z ++ 2 .[ ( C 2 3 6 5 . S T x + C 2 3 S 5 . ~ t T z ) C l ' 4 -+ ( 6 2 3 6 5 . 8 T , + 6 2 3 S s . n T , ) S l +- - ( C 5 . S T z + S 5 . n T z ) S 2 3 ]

- - 2 2 a o ~ + b o ~ + c o ~N p 4 -- 1 5 C 5 +where

a 0 4 7 - 1 2 6 1 6 2 - ( / 3 + 1 4 ) 6 1 C 2 3 - 1 5 C L C 2 3 S 5 - P T zb 0 4 7- 1 2 S 1 6 2 - ( /3 + 1 4 ) S L C 2 3 - 1 5 S L C 2 3 S 5 - P T yc 0 4 7 - l l + / 2 S 2 + ( 13 + / 4 ) $ 2 3 + 1 5 S 2 3 S 5 - P T z

In the case of i = 5,A o 5 7- [ 6 2 3 . ( n T z + C 4 . 8 T z ) + $ 4 . 8 T , ] 6 1 +

+ ( C 2 3 . . n T , - - S 4 . 8 T z + S 2 3 6 4 . S T , ) S 1- - 8 2 3 . n T z + C 2 3 6 4 . 8 T z

A p 5 = - 1 5 . [ ( a o s . C 1 + b o s . S 1 ) C 2 a - - c o 5 . $ 2 3 ]B o 5 7 - - - ( S 2 3 6 4 . n T z + S 4 . r t T y - - 6 2 3 . 8 T z ) 6 1+ ( S 4 . r t T x - - $ 2 3 64 . 1% T y + 6 2 3 . 8 T y ) S 1

- 6 2 3 6 4 . n T z + $ 2 3 .8 T zB p 5 - --- l s . [ ( a o 5 . C l + b o 5 . S 1 ) $ 2 3 C 4 +

+ ( b o 5 . C 1 - a o 5 . S 1 ) $ 4 + c o 5 . C 4 C 2 3 ]No~ = 3 + ~ + ~ , + ~ + ~ + , ~ , + ~ ++ a ~ + ~ , + ~ +

- 2 . [ ( 8 2 3 S 4 . a T x - C 4 . a T y ) C l ++ ( C 4 . a T z + S 2 3 S 4 . a T y ) S 1 + 6 2 3 S 4 . C t T z ]

Np~ = l~ + ~o~ + bo~ + ~o~where

a o 5 7 - 1 2 C 1 . C 2 - ( /3 + 1 4 ) 6 1 C 2 3 - - P T zb o 5 = 1 2 S 1 6 2 - (13 + 1 4 ) S 1 6 2 3 - P T yc 0 5 7 _ l l + 1 2 S 2 + ( /3 + / 4 ) $ 2 3 - P T z

A P P E N D I X BT e r m s f o r th e d e t e r m i n a t i o n o f 0 ~.I n t h e e q u a t i o n s ( 1 2 ) , ( 1 3 ) a n d ( 1 7 ):Ao = ao3Co 7 - co3Fo = Ao2 + 9o3.(12 + D o) + B o.EoGo = B o2 + Bo.(12 - Do) + go3.Eo

o t - - - - 0 3 + t a n - l ( ~ o )7 - ' a n - t ( ~ o )

a n dBo = bo3Do = CoC3 - AOSaEo = AoC3 + CoS3Ao2 = - 2 [ ( C 3 C 4 6 5 + S 3 S s) . n T z + ( $ 3 C 5 - C 3 C 4 S s ) . s T z+ C s S 4 . a T z ] C 1+ [ ( 0 3 0 4 0 5 -~- S 3 S s ) .n T y + ( $ 3 6 5 - 6 3 6 4 S 5 ) . 8 T y+ C 3 S 4 . a T y ] S l+ ( C 3 S 5 - S 3 C 4 C s ) m T ~ + ( C 3 C 5 + S 3 C 4 S s ) . S T ,- S 3 S 4 . a T z

B o 2 = - 2 [ ( $ 3 C 4 C 5 - C 3 S s ) . n T z - - ( $ 3 C 4 S 5 - 4- 3 C s ) . s T x+ S 3 S 4 . a T ~ ] C t+ [ ( 8 3 C 4 C 5 - C 3 S 5 ) .r t T y - - ( 8 3 C 4 S 5 + C 3 C 5 ) . 8 T y+ S 3 S 4 . a T y ] S I+ ( C 3 C 4 C 5 + S ~ S s ) . n T ~ - - ( C s C 4 S 5 - - S 3 C s ) s T ~+ C 3 S 4 . a T ~ .

Documents

MADRID, BADAN 1996 - Heuristic Search Method for Continuous-Path Tracking Optimization on High-Performance Industrial Robots