CORDIC-BASED ARCHITECTURES FOR ROBOT DIRECT KINEMATICS AND JACOBIAN COMPUTATIONS

'-LA. =

C. S. G. Lee

bcnool of Electrical Engineering %due University

West Lafayette, Indiana 47907

cos Oi - cos g sin Oi sin a, sin Oi ai cos Oi sin Bi cos a, cos Oi - sin a, cos Oi ai sin Oi

(2) 0 sina, cos a, 4 - 0 0 0 1 -

ABSTRACT Two efficient CORDIC-based architectures, designed to take the advantage

of the algorithmic characteristics of the kinematic equation. are proposed for the real-time computation of manipulator direct kinematics and Jacobian. The kinematic equation of an N-jointed manipulator involves the chain product of N homogeneous link transformation matrices and reveals the requirement for com- puting a large set of elementary operations: multiplications, additions, and tri- gonometric functions. Since these elementary operations, in general, cannot be efficiently computed in general-purpose uniprocessor computers. the Coordinate Rotation DIgital Computer (CORDIC) algorithms are used for efficiently com- puting these elementary operations. It is found that a general homogeneous link transformation matrix can be decomposed into a product of two matrices, each of which can be computed by two CORDIC processors arranged in parallel, forming a generic 2-stage cascade CORDIC computational module. Extending this idea to an N-jointed manipulator, N generic CORDIC computational modules, con- sisting of 4N CORDIC processors, can be concatenated to form an efficient pipe- lined architecture for computing the position and orientation of the end-effector of the manipulator and the manipulator Jacobian as well. A more cost-effective CORDIC co-processor architecture is also proposed at the expense of pipelining. This architecture uses a single CORDIC processor together with an adder to com- pute the manipulator direct kinematics. Using off-the-shelf components, the pro- posed CORDIC co-processor can compute the direct kinematics solution of a 6- jointed manipulator in less than 0.2 millisecond. Characteristics and performance analysis of both architectures are discussed.

1. Introduction Robot manipulators are highly nonlinear systems and their motion control

is usually specified in terms of the Cartesian path traveled by the end-effector in Cartesian coordinates. If the manipulator is moving in a crowded environment, knowing the pose (position and orientation) of its end-effector in real time, as well as its intermediate rigid links, will assist it tremendously in negotiating

I around the obstacles. The problem of computing the pose of the end-effector of a manipulator from the measured data of angular displacements of all the joints is known as the direct kinematics (position) problem [I]. Using the Denavit- Hartenberg matrix representation [21 to describe the translational and rotational relationship between adjacent link coordinate fxames. the direct kinematic posi- tion problem reduces to the problem of chain multiplying N 4x4 homogeneous link transformation matrices for an N-jointed manipulator.

The chain multiplication of N homogeneous link transformation matrices yields a set of twelve equations, nine for orientation matrix and three for position information. These equations involve a large set of elementary operations: scalar multiplications, scalar additions, and transcendental functions (sine and cosine). However, these elementary operations, in general, cannot be efficiently computed in general-purpose uniprocessor computers. Moreover, in order to achieve a real-time computation of the endeffector location, time-consuming transcendental functions are implemented as table look-up at the expense of the solution accuracy. Other methods for computing the time-consuming transcen- dental functions such as the DKS chip with table generation of trigonometric function [3] and the Taylor series expansion [4] with VLSI implementation have also been proposed. These methods optimize critical bottleneck computations in the kinematic equation.

This paper presents two efficient CORDIC-based architectures for the real-time computation of the N homogeneous link transformation matrices which yield the pose of the end-effector and the manipulator Jacobian of an N-jointed manipulator. Both CORDIC-based architectures function as peripheral devices aaached to a conventional host computer which provides the measured data of the manipulator's joint displacements for the computations. The design of these CORDIC-based architectures is based on the functionaVdata flow of a general homogeneous transformation matrix and the kinematic equation. The design of the first architecture, which is a CORDIC-based pipeline, was tackled in two separate, but coherent, phases of design. First, a general 4x4 homogeneous link transformation matrix i-'Ai for link i of a manipulator was examined and decomposed into a product of two matrices, each of which can be computed and realized by 2 CORDIC processors arranged in parallel, forming a generic 2-stage cascade CORDIC computational module. Next, for an N-jointed manipulator, N

Thir w d w u supponcd m pan by the National Science Fcundntim under Grant CDR-8803017,

nx s, a, P,

XN YN ZN PN n s a p fly sY ay py T i [ , 0 0 1 ] 4 [ 0 0 0 1 ] 4 n r s , a , p ,

- 0 0 0 1 -

(4)

0-8186-2012-9/89/oooO/0609$01 .OO 0 1989 IEEE 609

P~ =Cl[d6(CuC$s+S~C)+Spd4+a3C23+aZCJ -St(ddS*Ss+di)

A = ~ ~ ( C B C S - sncds) + Cpd4- aJa-aS2

si sin e,, cij E COS (e, + ej). and sij E &(ei + ej).

f r

( 5 4 where di and 0, are known PUMA’S W j i n t pa”. and Ci = cape,,

An examination of the above equations shows a large set of elementary operations: scalar multiplicarions, scalar additiorw. and trensccndeatal functions. One can use an interconnection of micropncessors with c0-pwes.m~ and an appropriate table look-up technique for the transcendental functions to compute the end-effector location. Although this mic--based computing system is widely used in present-day robot controllers, it suf€ers from the solution BCCU- racy, and lacks flexibility and modularity. The solution inaccuracy is due to the table look-up, while the tlexibility is due to the Deed for changing the link u~~rdi- nateframes of themanipulator ifdesired. Furtkrmore, ifone wants tpobrain the

instead of the robt’s base coordinate fnune. rhen an addumal homogeneous transformatiion matrix relating the base coodinate frame to the extemal world coordinate frame must be iocluded in Eq. (3). Thus. canputing a fixed set of equations as in Eqs. (LaF(5.d) does not present an amactive solution to the real- time computation of the manipulator ead-effectar location. A beua solution to the problem is to design a computational prchitectun that will improve solution accuracy and achieve flexibility and modularity f a computing thc pose of the endeffector of a manipulator in real time. We pmpose the use of CORDIC p m cessors to form a computing machine that efficiently computes the direct kinematics solution and the Jacobian as well. Two eflicient CORDIC-based architectures are proysed for this purpose, and t !~y are discussed in Sections 4 and 5. An intmduchon to CORDIC algorithms and piccessom is given in the next se4xion.

3. CORDIC Algorithms and Proecssors The kinematic eq-n of an N -jointed manipulator requires the computa-

tion of a large sx of elementary operations: multiplications. additions, and tri- gonometric functions. Howeva. these elementary opemiam, in general. cannot be efficiently computed by genaal-purpose Uniprocessor computen. In conven- tional uniprocessor computas, computation of elementary functions and their inverse consumes a considerable amount of effort than the multiplication opera- tion. These elementary fnnctions CM be efficiently computed by the CORDIC algorithms. The CORDIC algorithms [81-[111, are the natural candidam for efficiently computing these elementary operations. They repesent an efficient way to compute a variety of fnnctions related to coordinate transformations with iterative procedures involving only shift-and-add operarions in each step. Thus CORDIC pmcessing elements are extremely simple and quite compact to realize [SI, [9], [ll]. An i n ” e c t i o n of CORDIC to form a maximum p i p e w CORDIC architwxun for manipulator merse lUnematiC position com- putation has been explored [6].

The basic iterative equations of CORDIC algorithms are summarized as follows:

pose of the manipulator end-effectox with respe~t D a wyld coodmte frame

z , + l = z , + a , q (8) where z , + ~ is an auxiljary variable introduced to accumulate the “ion aftex each iteration, ai ( = f l ) is thc binary variable chasm based on thedirection of rotation, (s(i)) is a nondecreasing integer shift sequence, m = 1.0, -1 indicatts respectively the circular, linear, and hyperbok coordinate system (or type of rotations), and

q =mJnm-’[mHp(i)] (9) is the angle mtated in the i th mtath. By appmpriately c W n g ai such that after n rotations either ya --f 0 or z,, -B 0. one may compte different elementary functions. Equations (6) and (7) indicate that, by restricting the angular rotation magnitude according to Eq. (9). xi+, can be obrained by adding (or subwding) a shifted value of yi to x i , while simultaneously yi+l can be okaincd by subtracting (or adding) a shifted value of xi from yi . Thus, the computation of an elementary

function can be accomplished with n shift-and-add operations. which is compar- able to conventional multiplidom. This makes a CORDIC arithmetic logic unit (ALU) a vay appeahg dtemative to h traditional ALU for impheating the e i e m e n t a r y f ” F i i l s w v n a r i z e r v a r i o ~ ~ e l a n e a t a r y ~ t h a t can be obtained from tbe CORDIC algaithms as given by Eqs. (6x9). In this figure. a CORDIC algorith@” is depicted as a box with thnc inputs xgl YO. and z,,, which are the initial values of the iteaative equations in Eqs. (6)-(9), as well as three outputs. x,,. y,. and I,,. that carespond to the final values of the deskdelementiwy functions.

Haviland and Tuszynsh [5] realized the CORDIC algorithms on a CMOS chipandshowedthatthe~singtimeoftheCORDICchipisabout40~ls. They also showed the SPICE analysis of the chip and suggested n = 13 as the minimum cycle time of a two-byte. (24-St) fixed point opaaticq however, in practice, they used n = 24. Design and implementation issues of CORDIC algo- rithms such as domain of convergence, angular nsolution. --off e m , scal- ing. word length, and shift sequence can be found in [5], [8]-[11]. We shall con-

diredlcinematicssolutionandthemanipulatorJ~mmatm.

4. CORDIC-Bad Pipelined Architechre Our first approach to the direct kinematics problem is to utilize these

CORDIC pocessm to take the advantage of pipelining. These CORDIC prcces- sots will be configured and connected, besedon thedecomposition ofthe homo- geneous Iink transformation maeix of a manipulator, to arrive at an efticient CORDIC-based pipelined srchitech~e for the comptation of the direct kinematic posiriOn solution, and later extend to the Jacobian computation. We first examine

dencies in order to functionally decompose the computations into a cascade of cornpurorianal modules (CMs) with an objective that each CM will be realizable by a CORDIC gocessor. For an N-jointed maniplktor, the. kinematic e q u a h reveals the cham produd of d v e homogcncous link transformation ~ o f i - ’ A i , i = 1 , 2 , .N. Thus,wefirstl&atthepossiile~p sition of the link i homogeneous transformation mabrix ’-‘Ai. For ‘-‘b. it can be decomposed as a product of four basic homogerreous aanslation a rotation matriCesas[l].

which can be computed as apmductof two mahim as.

centra& on how to utilize the. ”IC a l g o r i e for computing the

the d i r e c t l r i n c m a t i c e * foritscompltational flow and data depl-

’-‘Ai = T ? ~ ( ~ i - l d i ) R o t ( z i - l . B i ) T?uJI(x~P~) Rot(~i ,&) (10)

wSei -sinei o o 1 o o 4. sine, cape, o o O C O S ~ -sin% o

,” 0 1 d ] [ ,” “0” 0 0 1

. (11)

As stated earlier, the homogenems link transformation matrix ‘-‘Ai is used to transform a vector expressed .in the ith coordinate frame D the same vector expresed in the (i-1)th coordlllllte frame. this coordinate esnsformation can be performed in two sequential step as indicated in Eq. (1 1). ”t is, the 6rst step is to transform a vector pi 8 (xi .y. .zi ,I)‘ in the ith coordl~lilte h C t o a n intamediate vccu)r p: e_ (xi” ,y: J,A, l)T,

L J L J Looking at the elementary functions computed by CORDIC p” in Figure 1. Eq. (12) can be computed and realized by two CORDIC arranged in parauel as foibws Step1-a: CORDICRocessor . CIRCl

xl, = y i c q - z i s q ly:

y l , = q C q + y i s q *$ zl,=Not&

Stcp 1-b : CORDIC Processor . m1 x2, =Not used

22, =Not used

610

Note that steps 1-a and 1-b are computed in parallel. Similarly, Eq. (13) can be computed and realized by two CORDIC processors arranged in parallel as fol- lows:

Step 2-a: CORDIC Processor: CIRC 1

xo=y2. EX,! x3. =x,!cei -y,!sei

Input= yo=x i . =y,! ; output= y3, =y,+cei +*:'sei [ Z O = e i [ 23. = Not used

Substituting xi" and y," from Eq. (12) into the above output equations, we have

x3, = x i C e i -y iCqSe i +ziSaiSBi +aiCOi output = y3, =xise, + y , c q c e i -Zisqcei +aisei =yi-l

z 3, = Not used

x4, =Not used

z o = y l " ~ z , A 1 24, =Not Used

1 Step 2-b : CORDIC Processor : LIN 1

Input = yo = di ; Output = y4, = 2: + d,

Substituting 2; from Eq. (12) into the above output equations, we have

x4, =Not used Output = y4, = y i S q + r , C q + d i -zi-l [ 24, =Not used

Note that the outputs in steps 2-a and 2-b (i.e., x 3, .y 3,. and y4,) correspond to the result of the matrix-vector multiplication in Eq. (1).

Since the outputs of the CORDIC processors in steps 1-a and 1-b are fed into the inputs of the CORDIC processors in steps 2-a and 2-b. the interconnec- tion of these four CORDIC processors forms a generic 2-stage cascade CORDIC computational module (CCM) for computing a general homogeneous link transformation matrix i - l ~ i ofa manipulator with either rotary or prismatic joint. This generic CORDIC computational module is shown in Figure 2-(a) and can be simply depicted as a block digram with three inputs, h x outputs, and geometric parameters as in Figure 2-(b). Extending this idea to an N-jointed manipulator, we need to cascade N CCMs, consisting of 4N CORDIC processors, to form a pipelined architecture for computing the N homogeneous link transformation matrices in the. kinematic equation, and the outputs of this pipelined architecture transform the vector pN (input to this pipeline) expressed in the Nth coordinate frame to the same vector expressed in the base coordinate frame of the manipula- tor. Since the input vector pN can te chosen selectively, if we let pN = (0 , 0 , 0 ,I)', then the output of the CORDIC-based pipeline is the position of the origin of the link N coordinate frame with respect to the base coordinate frame. Similarly, letting pN = (1 .O,O, pN = (0.1 ,O, 1)'. and pN = (0.0.1 , l)T, we, respectively, obtain the normal, sliding, and approach vec- tors (orientation) of the link N coordinate frame with respect to the base coordi- nate frame. Thus, in order to obtain the pose of the end-effector of a manipula- tor, we need to pipe a set of 4 input vectors or "basis vectors" ((0,0,0,1),(0,0,1,0).(0,1,0,0).(1,0,0,0)] into the proposed pipelined architecture to obtain the [ n ,s , a , p I. Note that the fourth element of these basis vectors is not used in the pipeline. For a PUMA robot arm in [l], where N = 6, a pipelined architecture of 6 CORDIC computational modules with 24 CORDIC processors can be used to compute the direct kinematics solution and is shown in Figure 3.

Several key features and characteristics about this CORDIC-based pipe- lined architecture should be addressed and discussed:

Flea'bifity. The generic 2-stage cascade CCM as shown in Figure 2 com- putes a general homogeneous link transformation matrix i - lAi . Thus, the generic CCM is suitable for any manipulator (with prismatic or rotary joints) whose link coordinate frames are described by 4x4 homogeneous transfor- mation matrices. The inputs to the CORDIC processors in the CCM are lWjoint parameters (i.e., di ,ei ,ai .q) of linWjoint i and a vector pi = (xi .y, ,zi ,I)' expressed in the ith coordinate frame. Thus, changing the link coordinate frames of the manipulator will only affect the input values of the CORDIC processors and will not alter the structure of the CCM nor the proposed pipelined architecture. Modularity. The idea of using 4 CORDIC processors to form a generic 2- stage cascade CCM provides a modular approach in designing a pipelined architecture. This modularity is based on the characteristics of the kinematic equation which involves the computation of the chain product of N homo- geneous link transformation matrices for an N-jointed manipulator. Each CCM becomes a building block (or computational block) for computing one of the N homogeneous link transformation matrices in the kinematic equa- tion in Eq. (3). If one wants to relate the link N coordinate frame to an external world coordinate frame instead of the robot's base cwdinate frame, then an additional CCM can be appropriately put in cascade with the existing N CCMs. This additional CCM computes the homogeneous

5. CORDIC Co-Processor Architecture Applying twenty four CORDIC processors for computing the direct

kinematics solution would be very expensive. This motivates us to design a more cost-effective CORDIC co-processor architecture which uses a single CORDIC processor together with an adder (or a processor) to compute the direct kinemat- ics solution at the expense of pipelining. This CORDIC co-processor would be used in conjunction with a host computer such as a Sun 3 workstation or a VAX 11/180 computer. The link/joint parameters (g , ai, d, , and e i ) would be down- loaded or written into the local memory of the co-processor and the CORDIC co-processor would compute the [ U , s , a , p I vectors, store them in the shared memory, and interrupt the host computer when finished. The CORDIC c e processor will start execution with an ENABLE signal and it signals END-CALC when the result is available.

As discussed in the previous section, equation (12) can be computed and realized by two CORDIC processors computed in parallel, one in the CIRCl mode and the other in the LINl mode. Since the CORDIC processor operating in the LINl mode is functioning as an adder, one can obtain a more cost-effective solution by replacing the CORDIC processor in the LINl mode with an off-the- shelf processor. Hence a single CORDIC processor operating in the CIRCl mode, together with a processor which functions as an adder, are used to compute "one-half" of the general homogeneous transformation matrix (i.e., Eq. (12)). The "second-half' computation of the general homogeneous transformation matrix is governed by Eq. (13). Using the same hardware. one can route the out- puts of the CORDIC co-processor back to the input to compute Eq. (13). In this way, it requires two iterations through the CORDIC co-processor to compute one general homogeneous transformation matrix, 2N iterations to compute one vector of the manipulator hand matrix, and W iterations to compute the pose of the end-effector of an N-jointed manipulator. Figure 4 shows a block diagram of the proposed CORDIC co-processor architecture. Multiplexors at the CORDWadder inputs determine whether the vector lransformation is from pi to pp (i.e., Eq. (12)) or from p; to the desired pi-1 (i.e., Eq. (13)). The input multi- plexors can also select the four "basis vectors'' [ (O,O,O), (0.0.1). (0.1.0). (1,0,0) ] to compute the desired [U, s , a , p I. Furthermore, the CORDIC co-processor also contains three random access memories (RAMS) which store the link/joint parameters ( d i .ai .q .ei )and the results of the computations [ n , s ,a , p 1.

With the block diagram shown in Figure 4. it is necessary to define a con- troller for controlling the various latches and switches to handle the data flow. Three main states or conligurations of the CORDIC co-processor are identified and must be switched by this controller. First, a basis vector together with ai and ai must be set up at appropriate inputs. Second, it must latch the results of the computation and route them back as inputs to be used in the second half of the transformation matrix computation; other liwjoint parameters ei and d, will then be read from the RAMS as inputs. Third, the outputs will be routed back again to begin the next transformation matrix computation. These last two steps are repeated N - 1 more times to produce one of the four output vectors. The whole computation is repeated again from the start with the three remaining basis vectors [ (0.0.1). (0.1.0). (1.0.0) ). Thus, it requires 8N iterations through the CORDIC co-processor to compute the pose of an N-jointed manipulator (or 48 iterations for a 6-jointed manipulator).

transformation matrix which relates the base coordinate frame to the exter- nal world coordinate frame. Similarly, an additional CCM can be appropri- ately cascaded into the existing pipeline to compute the hnmn"-%us transformation ma& which relates the tool coordinate frame to the link N coordinate frame of the manipulator. This homogeneous-transformation- matrix-based CORDIC module concept changes the building block of our architectm from CORDIC processors to CCMs. Thus, the number of gen- eric CCMs in the pipelined architecture directly corresponds to the number of homogeneous link transformation matrices in the kinematic equation of the manipulator. Solution Accuracy. As indicated in [ 5 ] , CORDIC algorithms were realized on a CMOS chip with 24-bit data processing. Based on fixed-point arith- metic, the iterative algorithm converges with an e m of 2-%. This solution accuracy is much be- than those quoted in [31, VI. Compufafwnaf Time. Let us assume that the execution time of a CORDIC processor is T ps, then the processing time of the generic CCM is 25 ju . For a pipeline consisting of N generic CCMs (with 4N CORDIC proces- sors) for an N-jointed manipulator, the initial delay time in the pipeline is 2 N ps and the pipelined time is T ps . Thus, in order to obtain the position and orientation of the endeffector of a manipulator, we need to pipe a set of 4 input vectors (or a 4x4 identity matrix) into the proposed pipelined archi- tecture to obtain the [ U , s, a , p l. The first output from this set of 4 input vectors will take an initial delay time of 2 N ps, then the successive outputs will be T ps apart because of the pipelined architecture. This yields a total computation time of ( 2 N + 3 ~ ) p for computing the pose of the end- effector of an N-jointed manipulator. Using the execution time of a CMOS CORDIC processor as 4Op~ (7 = 40) [SI, the computation time for obtaining the pose of the end-effector of a PUMA robot arm is the initial delay time (for position) plus 3 pipelined time (for [ U , s , a 1) for a total of a. The proposed pipelined architecture consists of 6 CCMs with 24 CORDIC pro- cessors and is shown in Figure 3.

61 I

With referenced to Figure 4, a m m detailed description of the basic states that the CORDIC goes h g h can be summarized in the following conml/daIa flow scheme: Step1 [Initialidon.] Initializeallcounters. S e t t e 4 a n d i t N . C o u n t e r

k indicates the remining n u m b of input V e d a s pocesped by the CO-processor. and counter i indicates the mnainhg number of itera- tions through the CORDIC CO-- for each value o f t .

step 2 Downlod the wjoh pa".] The host computer writes the linyjoint parameters ( d j . a j . a . , e j ) , j = ~ . N - 1 ; . . . 1 into the

step3 [set up inputs for the "first-half" computation.] Latch q into RAMl andRAM2. Set(* .y , I $ t (0.0.0).

RAMI-LATCH and ai into W - L A T C H . Step 4 [Initiate execution.] Initiate the execution with CORDIC ENABLE and

ADD-WABLE signals. Read Oi from RAMl and di from RAM2 at the same time. [wait for end execution signal.] Wait for the end of the execution by

[set up inputs for the "second-half" c m q ~ a h ~ ] Latch Oi into

Step 5

Step6 &tectingthehe-CALcsignalfrOmtheCORDICprocessor.

RAMlLLATCH and d; into RAMz_LATcH. Larch the ~ ~ t m t s XN. YN, and ADD-OUT. klect ADD-OUT at XO-MUX. XN-LkICH at YO-MUX. and YN-LA= at ADD-MUX.

Srep 7 [Initiate the "d-half computation.] Initiate the execution with CORDIC ENABLE and ADDBABLE signals. Read q-, from RAMl andai-l fromRAM2at thcsametime..

Step 8 [wait for end execution signal.] Wait for the end of the execution by

Step 9 [Check counteri.1 set i t i -1. I f i =0, then go to Step 12; else continue.

Step 10 [Set up inputs for next tr;msfonnation computation.] Latch q into

YN, and ADD-OUT. Select YN-LATCH at XO-MUX. ADD-OUT at YO-hNJX. and XN-LATCH at ADD-MUX.

Step 11 [Looping.] Go to Step 4. S r ~ p 12 [Output results.] Write the results into RAM3 (i.e.. bX, py , p,) for the

first boping).

detecting the he-cALc signal from the CORDIC processor.

RAMI-LATCH and ai into RAM2-LATCH. Latch the ~ t p l t s XN,

Step13 [ C h e c L ~ t ~ t . ] S e t k t t - 1 . I f k = O , t h e n s t o p ; ~ l s e c ~ n t i ~ ~ ~ e . Step 14 Wpeat for other input vectors.] Set i t N and set (x .y :z), respec-

tivfy. to (O,O,l).(O.l,O),(l.O.O). Repeat Steps 3-13 to obmn the other desredvectors [n .s . r ] .

A complete state table description of the control si@ is shown in Table 1. Assuming that the processor which only performs as an adder (e.g. MC68MO) were constructed from off-the-shelf components on a VME bus card, it would not be UNeaSonable to exmt a 16.7 megahatz clock rate (this would be compatiile with a Sun 3/50 workstation). To complete the 100 states (i.e., four times the state table in Table 1 for the four desircd output vectors) of the conml loop would require appmximately 6 p, while the CORDIC pxucesm wil l requk approximately 40p.v [5] for each computation. Given ths, the processor will spend 1920 p in wait sultcs (for 48 ite.rations) while the CORDIC co-pmcessor ptafoms its dculations for a &jointed manipulator. This makes the switching and propagation time negligible. Thus. it is estimated that the CORDIC c e p" could r e m the [n ,s,a .p] vectors for a 6jointed manipulator in less than 2 milliseconds. If the computation time of the CORDIC processor dmpped substantially to 3 p as suggested in VI, then the direct kinematics solution could be obrained in less than 02 millisecond.

6. Extension to Jacobian Matrix Computation The CORDIC pipelined architecture discussed in section 4 can be easily

extended to compute the manipulator Jacobian matrix while the direct kinematics solution is being calculated. Mathematically, a Jacobian relates the derivatives of a vector space to the derivatives of another vector space. In robotics. the pose of the manipulator hand relative to a fixed inertial coordinate system can be described by a set of 6 algebraic equations containing the joint variables

x = T o (14) where xP(p, .py .p, ,G$,..G$~ ,?, )' is a &dimensional vector describing the Cartesian position and Onentahon of the manipulator hand with respect to the inenial coordinate frame, q is an N-dimensional pit-variable vector for an N- jointed manipulator, where

41 ~ 4 2 . " ' .4N

Oi, i f& = O

d i , i f h , = 1 , qi P and h, is ajoint indicator and is defined as

0 , iflinkiisrotational h, P{ 1 , if link i is himslational.

Differentiating Eo. (14) with respect to time yields

where v and b are. respectively. the linear and angular velocities of the manipu- lator hand. P (4 .42, - . , qM 1' is the joint-velocity vector of the manipula- tor. and J(q) is thew Jscobian matrix and it is a funalon ofthe p i t variables. Fwbmore, themaspose of the Jacobien matrix relates static contact forces and momentstoasetofjointlnquesTas

r=JT(9)F (18) where F ( Fx ,Fy .F, ..Mz ,My ,M, )r is a ddimensional static foxelmoment Vector. andr is an#-dnnensional pin t t q u e vector. Consequently, the Jam- bian computation is an important aspect in robotic kinematics and control.

Various techniques and methcds have bem pmposed for computing the Jacobian matrix [12]-[17l. We shall use the diffemtial transform mtthod as sug- gested by Paul [14]. The 4x4 homogeneous link transformation. can be usedtoobtaindifferential hadation andmtation a b o u t a " a t e frame from which the Jacobian matrix canbederived. With theRfemnce hame set to the manipulator hand coordinate flame, the pmposed CORDIC pipelin-.! ar~hitecturc c ~ l l be td to c ~ m p ~ t e the Jacobian mahix obtaioed frmr tbe UL- fmntial transform technique. Let us define the composite link transformation matrixu, as

(19) U, 4 '-lA, 'A,+1 . . . N - ' A ~

10 0 0 1J

Wbac i -14 is delined as in Eq. (2). Physically, the ui matrix indicates the posi- tion and oaieqtatioo of the manipllator cnd-effectornpicsscd with respect to the (i-1)th coodmtc fnam Using m. (19) and (2). the diffeamthl change of the end-cffecta with respect to the manipulator hand CoDCdinatc frame due to adif- fmntial change ofpint i can be obrained as [14]

wheze dei and &fi are, respectively. the differential chan e of a rotary pint i and a prismatic pin t i. ['d, , ' 4 , 'd. IT and ['sa , 'S, , '& I' are, respectively, the diffem~tia! himslation and rotation about the principal axes of the end- effector "ate frame. Equation (20) gives us the ith column of the W JacobUmatriX. ThustheJacobianmatrix~beobtainedfori =l.2. ... N.

['&*'$*'ss *'d.~'~.'~,d,lr=J(9)[d4i.d4~, ... ,&NI' (21) Whm 4i is defined as in Eq. (15).

The J d i matrix computation requires the computation of the U, matrica which can be easily computed using the proposed CORDIC pipelined a~~hitecturc in section 4. Fmm the discussion in section 4, it is not difficult to see that the outputs from the ith CORDIC computational module correspond to the Ui matrix. These outputs can then be used in Eq. (20) to compute the ith column of the Jacobian matrix. Thus the Jacobian matrix can be computed at the same time as the direct kinematic position solution is pipelining through the CORDIC pipelined architecture. It is worh noting that the pmposed CORDIC pipelined architecture is balanced and synchronous while the cumputation of the columns of the Jacobian matrix is asynchronous. That is. the computation of the Jacobian matrix will not start until all the upper 3x4 entries of the Ui matrices, i=1,2;..,N,havebeenobtained. Thus,thetoraltimeforcomputingthe Jacobian mamix is qual to the direct kinematics solution computation plus the

6N multiplications and ,3N additio&ubtractions for a manipdip with all rotary pints or z 6 ( 1 - X , ) multiplications and x 3 ( 1 - & )

a d d i t i ~ t i o n s for a manipulator with rotary and/or prismatic joints, and if the manipulator has all prismatic joints. no extra computation is needed to obrain b e Jacobian matrix. Using the proposed CORDIC pipelined architecwe, the computational complexity of the Jacobian matrix is obviously of Order 0 (N).

7. conclusion

required time to compte Eq. (20). The computation of Eq. (20) requires at most

1-1 1-1

The kinematic equation of an N -jointed manipulator requires the computa- tion of N homogeneous l i transformation matrices. The d t m m ~ t i o n of a general homogemus link transformation matrix into a product of two matrices reveals its computation can be accomplished by a generic W g e "IC com- putational module consisting of 4 CORDIC p". Thus, a cascade of N

(16)

612

CCMs forming a pipelined architecture can be used for computing the pose of the end-effector of an N-jointed manipulator. The proposed CORDIC pipeline can compute the direct kinematics solution of an N-jointed manipulator in (2rN + 3r )p where the initial delay time of the pipeline is 2 W p and the pipelined time is 7 p. This pipelined architecture, although expensive, can be easily extended to compute the Jacobian matrix with an additional computation of

C 6( 1 - hi ) multiplications and C 3 ( 1 - h, ) additionslsubtractions. Our i = l i=l second architecture is a more cost-effective CORDIC co-processor architecture which uses a single CORDIC processor together with an adder to compute the manipulator direct kinematics at the expense of pipelining. Although slower, this CORDIC CO-processor architecture can still compute the direct kinematics solu- tion of a 6-jointed manipulator in less than 0.2 millisecond.

_ _ ~ SIGNAL XO_MUXAl

A0 YO-MUXAl

A0 ADD_MUXAl

A0 RAM-MUXAl

A0 CORDICENABLE

ADD-ENABLE XN-LATCH YNLATCH

ADDLATCH R A M L A T C H RAM2LATCH

RAM1/2A3 A2 A1 A0

RAMlREAD RAM2READ

RAM3A3 A2 A1 A0

RESULTLATCH RAMBWRITE

8. References

[l]

[2]

K. S. Fu, R. C. Gonzalez, and C. S. G. Lee, Robotics: Confrol, Sensing. Vision. and Intelligence, McGraw-Hill, 1987. J . Denavit and R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," J. of Applied Mechanics, vol. 77, no. 2, pp. 215-221, 1955. S. S . h u n g and M. A. Shanblatt, "Real-Time DKS on a Single Chip," IEEE J. of Robotics and Automation, vol. RA-3, no. 4. pp. 281-290, August 1987. V. Seshadri, "A Real-Time VLSI Architecture for Direct Kinematics." Proc. of I987 IEEE Int'l Con5 on Robotics and Automation. Raleigh, NC,

G. L. Haviland and A. A. Tuszynski, "A CORDIC Arithmetic Processor Chip," IEEE Trans. Comput., vol. (2-29, no. 2. pp. 68-79, Feb. 1980. C. S. G. Lee and P. R. Chang. "A Maximum Pipelined CORDIC Architec- ture for Robot Inverse Kinematic Position Computation." IEEE J. of Robot- ics a n d h t o m i o n , vol. RA-3, no. 5 , pp. 445458, October 1987.

[3]

[4]

pp. 11 16-1 120, Mach 30 - April 3,1987. [ 5 ]

[6]

1 2 z z y y z z y y z z y y x x x x 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0

Y. Wang and S . E. Bulner, "A New Architecture for Robot Control," Proc. of 1987 IEEE Int'l Cor$ on Robotics and Automation, Raleigh, NC, pp. 664-670, March 30 -April 3,1987. J. E. Volder, "The CORDIC Trigonometric Computing Technique," IRE Tram. Elecrrom'c Compufers, vol. EC-8, no. 3, pp. 330-334, Sept. 1959. H. M. Ahmed, J. M. Delosme and M. Mod, "Highly Concurrent Comput- ing Structures for Matrix Arithmetic and Signal Processing," IEEE Com- puter, vol. 15, no. 1, pp. 65-82, Jan. 1982. J. S. Walther, "A Unified Algorithm for Elementary Function," AFIPS Conf. Proc.. vol. 38, 1971 SJCC, pp. 379-385. P. Dewide et al., "Parallel and Pipelined VLSI Implementation of Signal Processing Algorithms," in VUI and Modern Signal Processing. S . Y. Kung, H. J. Whitehouse, T. Kailath, (eds.), Prentice-Hall, Inc., E n g l e w d Cliffs, NJ, pp. 257-276, 1985. M. Renaud, "Geometric and Kinematic Models of a Robot Manipulator: Calculation of the Jacobian Matrix and its Inverse," Proc. 11th Interna- tional Symp. Industrial Robots, pp. 757-763, OcL 1981. K. J. Waldron, "Geometrically Based Manipulator Rate Control Algo- rithms," Mechanism and Theory. vol. 17, no. 6, pp. 379-385, 1982. R. P. Paul, B. Shimano and G. E. Mayer, "Differential Kinematic Control Equations for Simple Manipulators," IEEE Tram. on Systems, Man, and Cybern ... vol. SMC-11, no. 6,pp. 456460, June 1981. D. E. Whitney, "The Mathematics of Coordinated Control of Prosthetic Arms and Manipulators," Trans. ASME. J . Dynamic Syst., Measurement, Contr., pp. 303-309, December 1972. D. E. Orin and W. W. Schrader, "Efficient Computation of the Jacobian for Robot Manipulators," Int'l J. of Robotics Research, vol. 3, no. 4, pp. 66-75, Winter 1984. T. B. Yeung and C. S. G. Lee, "Efficient Parallel Algorithms and VLSI Architectures for Manipulator Jacobian Computation," Proc. of 1988 IEEE Int'l Cod on Robotics and Automation. Philadelphia. PA, pp. 1158-1163, April 24-29.1988.

Table 1 Control signals for computing n , s , a, or p vectors.

__- 1 1 0 0 1 1

1 1

__

e 0 0 x x x x __ 0 1 0 1 1 0 1 0 1 0 1 0 1 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0

~

-

-

__ 1 1 1 1 1 1 1 1 1 1 1 1 x x x x ____ 0 1

1 0 1 0 1 0 1 0 1 0 0 0 0 0 1 1 0 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0

O L

-

2

U 1 1

1 1

1 1 0 0

L L 5 0 1 E.2 1 0 1 0 1 0 1 0 1 0 0 0 0 1 1 0 1 0 1 1 1 1 0 0 0 0 0 0

0 0 0 0

x x

-

-

-__

- NOTES: z,y = (0,O) for end-effecter posifion vector. z,y = (0,l) for end-effecter normal vector. z,y = (1,O) for end-effecter sliding vector. z,y = (1,l) for end-efecter approach vector.

After RESULTLATCH, output will be written into RAM3 using RAM3_MUX, concurrent with the calculation of the next vector (RAM3 addresses shown as don't care).

Vertical double bars indicate that controller inserts wait states until CORDIC processor has completed its computation (this assumes that the adder chip is at least as fast).

613

23 24 25 l l x o o x l l x o o x l l x O O X

x x x x x x 0 1 0 0 1 0 1 0 1 1 0 1 1 0 1 1 0 1 1 0 1 1 0 0 0 0 0 1 0 0 1 0 0 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0

0 0 1 0 0 0

m = o m = O

Pigure 1 Elementary functions computed by CORDIC processors.

I

I

I

Figure3 A CORDIC-based pipelined architecture for direct kinematics computation. pN = (xi ,yi , zi lT is the input vector and po=(xo,yo,zo)* is the output vector. When pN = ( o , o , o I*, po is the position vector of the end-effector. When pN =(I,o,o)*, po is the normal vector of the end-effector. When p~ = ( 0.1 , 0 )* , po is the sliding vector of the end-effector. When pN = (0 , O , 1 )*, po is the approach vector of the end-effector.

~

614

(b)

Figure2 (a) A generic 2-stage CORDIC computational m u l e for computing i-l~i.

(b) A block diagram representation of (a).

MIX A1

UTCH

CO

HOST COMPUTER BUS

Figure 4 The CORDIC CO-processor block diagr;im.