Kinematic Analysis of the

6R Manipulator of General Geometry

Madhusudan RaghavanPowertrain Systems Research Lab

General Motors R&D CenterWarren, MI 48090-9055

andBernard Roth, Professor

Design DivisionMechanical Engineering Department

Stanford University, CA 94305

1 Introduction

This paper presents a new solution to the inverse kinematics problem forthe 6R series manipulator of general geometry. Such a manipulator has 6revolute (R) joints arranged in series. The link lengths, twist angles andoffsets, may be arbitrary real numbers. The inverse kinematics problem maybe stated as follows :Given(i) the manipulator geometry (i.e. location of the base, link lengths, twistangles, and offsets),(ii) the goal position to which the hand must be moved,Findthe values of the joint angles which will place the hand in the goal position.

When stated in mathematical terms, this problem reduces to the solutionof a principal system of multivariate polynomials (i.e. n polynomials in nunknowns). In this paper we present a new solution procedure based ondialytic elimination. This procedure exploits the structure of the ideal of theclosure equations of the 6R manipulator and results in a compact eliminationalgorithm. The result is a 16th degree polynomial in the tangent of the half-

1

angle of one of the joint variables.

2 Previous Work

The earliest systematic assault upon this problem appears to have been madeby PIEPER[1968]. Pieper proposed iterative numerical techniques for com-plex manipulators of general geometry. He presented closed-form solutionsfor manipulators with special geometries (e.g. 3 consecutive joint axes inter-secting at a point, etc.). For 6R manipulators with general geometry, Piepershowed that a naive elimination strategy would yield a univariate polynomialof degree 524,288.

ROTH, RASTEGAR and SCHEINMAN[1973] using synthetic argumentsshowed that this problem has in general at most 32 solutions, i.e. thereare 32 distinct configurations of the manipulator, in which its hand is at thedesired goal position. Their proof was non-constructive.

ALBALA and ANGELES[1979], using indicial notation, presented the firstconstructive solution to this problem. Their solution was in the form of a12 × 12 determinant whose entries were quartic polynomials in the tangentof the half-angle of one of the joint variables.

DUFFY and CRANE[1980] later provided a different solution by combiningspherical trigonometry and dialytic elimination to obtain a 32nd degree poly-nomial in the tangent of the half-angle of one of the joint variables. Their32nd degree polynomial alway yielded extraneous or false roots in additionto the desired values.

TSAI and MORGAN[1985] used homotopy continuation to solve the inversekinematics problem and found only 16 solutions for various 6R manipulatorsof different geometries. They therefore conjectured that this problem has atmost 16 solutions.

Recently, LEE and LIANG[1988a and 1988b], used extensions of Duffy’smethod to obtain by dialytic elimination a 16th degree polynomial in thetangent of the half-angle of one of the joint variables, thus confirming the

2

conjecture of TSAI and MORGAN[1985]. The solution presented in thispaper is the result of our quest for a simpler solution than the procedureused by LEE and LIANG[1988b].

3 Notational Preliminaries

The notation used in this paper is identical to that appearing in the paperby TSAI and MORGAN[1985]. We present the essential features here.

The links of the 6R manipulator are numbered from 1 to 7, the fixed orbase link being 1, and the outermost link or hand being 7. A coordinatesystem is attached to each link to facilitate a mathematical description ofthe linkage and the relative arrangement of the links. The coordinate systemattached to the ith link is numbered i. The 4 × 4 transformation matrixrelating coordinate systems i + 1 and i is as follows :

Ai =

ci −siλi siµi aici

si ciλi −ciµi aisi

0 µi λi di

0 0 0 1

(1)

wheresi = sinθi, ci = cosθi,λi = cosαi, µi = sinαi,ai is the length of link i + 1,αi is the twist angle between the axes of joints i and i + 1,di is the offset distance at joint i,θi is the joint rotation angle at joint i.

The interested reader will find a more detailed description of the notation inthe paper by TSAI and MORGAN[1985].

3

4 The Problem Statement in Mathematical

Terms

The closure equation for the 6R manipulator is the following matrix equation

A1A2A3A4A5A6 = Ahand (2)

Ahand is the 4 × 4 transformation matrix describing the Cartesian coordi-nate system 7, attached to the hand (or last link) with respect to coordinatesystem 1, attached to the base link. The entries of this matrix are knownbecause the hand coordinates in the goal position are specified. The left-hand side of the above matrix equation describes coordinate system 7 withrespect to coordinate system 1, in terms of the relative arrangements of theintermediate coordinate systems.

The quantities ai, di, λi, µi, i = 1, . . . , 6, appearing in the matrices on theleft-hand side of equation ( 2) are all known. The unknown quantities areθi, i = 1, . . . , 6, and the above matrix equation must be solved for them.

This is the inverse kinematics problem for the 6R series manipulator.

5 The Solution

5.1 Derivation of p1, p2, p3, l1, l2, l3

The matrix equation ( 2), is equivalent to 12 scalar equations. Of these only6 equations are independent, because the submatrix comprised of the first 3rows and columns is orthogonal. We perform operations on these multivari-ate equations and eliminate all but one variable, thus reducing the problemto the solution of a single equation in one variable. This is in the spirit ofGauss Elimination applied to linear systems of equations, except that theequations involved in this problem are non-linear.

Equation ( 2) may be rewritten as

A3A4A5 = A−12 A−1

1 AhandA−16 (3)

4

Let Ahand be equal to

lx mx nx ρx

ly my ny ρy

lz mz nz ρz

0 0 0 1

.

The 6 scalar equations obtained from columns 3 and 4 of equation ( 3) aredevoid of θ6. We therefore work with these 6 equations, with the goal ofeliminating 4 of the 5 remaining variables. These 6 equations are :

c3f1 + s3f2 = c2h1 + s2h2 − a2 (4)

s3f1 − c3f2 = −λ2(s2h1 − c2h2) + µ2(h3 − d2) (5)

f3 = µ2(s2h1 − c2h2) + λ2(h3 − d2) (6)

c3r1 + s3r2 = c2n1 + s2n2 (7)

s3r1 − c3r2 = −λ2(s2n1 − c2n2) + µ2n3 (8)

r3 = µ2(s2n1 − c2n2) + λ2n3 (9)

where

f1 = c4g1 + s4g2 + a3

f2 = −λ3(s4g1 − c4g2) + µ3g3

f3 = µ3(s4g1 − c4g2) + λ3g3 + d3

r1 = c4m1 + s4m2

r2 = −λ3(s4m1 − c4m2) + µ3m3

r3 = µ3(s4m1 − c4m2) + λ3m3

g1 = c5a5 + a4

g2 = −s5λ4a5 + µ4d5

g3 = s5µ4a5 + λ4d5 + d4

h1 = c1p + s1q − a1

h2 = −λ1(s1p− c1q) + µ1(r − d1)

h3 = µ1(s1p− c1q) + λ1(r − d1)

5

m1 = s5µ5

m2 = c5λ4µ5 + µ4λ5

m3 = −c5µ4µ5 + λ4λ5

n1 = c1u + s1v

n2 = −λ1(s1u− c1v) + µ1w

n3 = µ1(s1u− c1v) + λ1w

p = −lxa6 − (mxµ6 + nxλ6)d6 + ρx

q = −lya6 − (myµ6 + nyλ6)d6 + ρy

r = −lza6 − (mzµ6 + nzλ6)d6 + ρz

u = mxµ6 + nxλ6

v = myµ6 + nyλ6

w = mzµ6 + nzλ6

Equations ( 4)-( 9) are the same as equations (41)-(46) in the paper by TSAIand MORGAN[1985]. We rearrange these 6 equations as follows :

Equation ( 4) is rewritten as :

p1 : c3f1 + s3f2 + a2 = c2h1 + s2h2 (10)

Equation ( 5)×(−λ2) + Equation ( 6)×µ2 gives :

p2 : µ2f3 − λ2(s3f1 − c3f2) = s2h1 − c2h2 (11)

Equation ( 5)×µ2 + Equation ( 6)×λ2 gives :

p3 : µ2(s3f1 − c3f2) + λ2f3 + d2 = h3 (12)

We rewrite equation ( 7) without any change :

l1 : c3r1 + s3r2 = c2n1 + s2n2 (13)

6

Equation ( 8)×(−λ2) + Equation ( 9)×µ2 gives :

l2 : µ2r3 − λ2(s3r1 − c3r2) = s2n1 − c2n2 (14)

Equation ( 8)×µ2 + equation ( 9)×λ2 gives :

l3 : µ2(s3r1 − c3r2) + λ2r3 = n3 (15)

p1, p2, p3, l1, l2, and l3 are labels used to refer to these equations in subsequentderivations.

5.2 The Structure of p1, p2, p3, l1, l2, l3

It is noteworthy that f1, f2, f3, r1, r2, and r3 are linear combinations of theterms s4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1. h1, h2, h3, n1, n2 and n3 are linearcombinations of the terms s1, c1, 1. Equations ( 10)-( 15) may be written inmatrix form as :

(A)

s4s5

s4c5

c4s5

c4c5

s4

c4

s5

c5

1

= (B)

s1s2

s1c2

c1s2

c1c2

s1

c1

s2

c2

(16)

where A is a 6× 9 matrix whose entries are linear combinations of s3, c3, 1,B is a 6× 8 matrix whose entries are all constants.

5.3 The Ideal of p1, p2, p3, l1, l2, l3

The Ideal generated by a set of polynomials q1, q2, . . . , qr in the variablesx1, x2, . . . , xm is the set of all elements of the form q1β1 + q2β2 + . . . + qrβr,where β1, β2, . . . , βr are arbitrary elements of the set of all polynomials inx1, x2, . . . , xm.

The Ideal generated by p1, p2, p3, l1, l2, l3 has the following interesting prop-erties :

7

Property 1 : The function p21 + p2

2 + p23 has the same power products as

equations ( 10)-( 15) i.e. it has the structure :

(M)

s4s5

s4c5

c4s5

c4c5

s4

c4

s5

c5

1

= (N)

s1s2

s1c2

c1s2

c1c2

s1

c1

s2

c2

(17)

where M is a 1× 9 matrix whose entries are linear combinations of s3, c3, 1,N is a 1× 8 matrix whose entries are constants.

By power products, we mean “terms” (e.g. the power products of the poly-nomial 5x2y + 3xz + 9y2 + 4z = 0 are x2y, xz, y2, and z).

p21 + p2

2 + p23 : f 2

1 + f 22 + f 2

3 + a22 + d2

2 + 2a2(c3f1 + s3f2) +

2d2(µ2(s3f1 − c3f2) + λ2f3) = h21 + h2

2 + h23 (18)

Property 1 is true because f 21 + f 2

2 + f 23 is a linear combination of s4s5, s4c5,

c4s5, c4c5, s4, c4, s5, c5, 1, and h21 + h2

2 + h23 is a linear combination of s1, c1, 1.

Property 2 : p1l1 + p2l2 + p3l3 has the same power products as equations( 10)-( 15).

p1l1 + p2l2 + p3l3 : f1r1 + f2r2 + f3r3 + a2(c3r1 + s3r2) +

d2(λ2r3 + µ2(s3r1 − c3r2)) =

h1n1 + h2n2 + h3n3 (19)

f1r1+f2r2+f3r3 is a linear combination of s4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1.h1n1 + h2n2 + h3n3 is a linear combination of s1, c1, 1.

Property 3 : p1l2 − p2l1 has the same power products as equations ( 10)-( 15).

p1l2 − p2l1 : a2(µ2r3 − λ2(s3r1 − c3r2)) + µ2c3(f1r3 − f3r1) +

8

λ2(f1r2 − f2r1) + µ2s3(f2r3 − f3r2) =

h2n1 − h1n2 (20)

f1r3 − f3r1, f1r2 − f2r1, and f2r3 − f3r2 are all linear combinations ofs4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1. h2n1 − h1n2 is a linear combination ofs1, c1, 1.

Property 4 : p2l3 − p3l2 has the same power products as equations ( 10)-( 15).

p2l3 − p3l2 : s3(f3r1 − f1r3) + c3(f2r3 − f3r2) + r1d2λ2s3 − r2d2λ2c3

−r3d2µ2 = s2(h1n3 − h3n1) + c2(h3n2 − h2n3) (21)

As stated earlier, f3r1 − f1r3 and f2r3 − f3r2 are linear combinations ofs4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1. h1n3 − h3n1 and h3n2 − h2n3 are linearcombinations of s1, c1, 1.

Property 5 : p3l1 − p1l3 has the same power products as equations ( 10)-( 15).

p3l1 − p1l3 : µ2(f1r2 − f2r1) + λ2s3(f3r2 − f2r3) + λ2c3(f3r1 − f1r3) +

r1(d2c3 − a2µ2s3) + r2(d2s3 + a2µ2c3)− a2λ2r3 =

s2(h3n2 − h2n3) + c2(h3n1 − h1n3) (22)

f1r2 − f2r1, f3r2 − f2r3, and f3r1 − f1r3 are linear combinations ofs4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1. h3n2 − h2n3 and h3n1 − h1n3 are linearcombinations of s1, c1, 1.

Property 6 : (p21 + p2

2 + p23)l1 − 2(p1l1 + p2l2 + p3l3)p1 has the same power

products as equations ( 10)-( 15).

(p21 + p2

2 + p23)l1 − 2(p1l1 + p2l2 + p3l3)p1 :

f1(2(pu + qv + (r − d1)w)c3) +

f2(2(pu + qv + (r − d1)w)s3) +

r1(−2d2µ2a2s3 − (p2 + q2 + (r − d1)2 − a2

1 + a22 − d2

2)c3) +

9

r2(−(p2 + q2 + (r − d1)2 + a2

1 − a22 − d2

2)s3 + 2a2d2µ2c3) +

r3(−2a2d2λ2) +

(f1r2 − f2r1)2d2µ2 +

(f2r3 − f3r2)(−2d2λ2s3) +

(f3r1 − f1r3)2d2λ2c3 +

((f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1)c3 +

((f 21 + f 2

2 + f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2)s3 −

2a2(f1r1 + f2r2 + f3r3) +

2a2(pu + qv + (r − d1)w) + δ1(c3r1 + s3r2)− δ2(c3f1 + s3f2 + a2) =

2a1s2(h2n1 − h1n2) + δ1(c2n1 + s2n2)− δ2(c2h1 + s2h2) (23)

where δ1 = p2 + q2 + (r − d1)2 − a2

1, δ2 = pu + qv + (r − d1)w.

The functions (f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1 and (f 2

1 + f 22 +

f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2 are linear combinations of s4s5, s4c5, c4s5,

c4c5, s4,c4, s5, c5, 1.

Property 7 : (p21 + p2

2 + p23)l2 − 2(p1l1 + p2l2 + p3l3)p2 has the same set of

power products as equations ( 10)-( 15).

(p21 + p2

2 + p23)l2 − 2(p1l1 + p2l2 + p3l3)p2 :

f1(2(pu + qv + (r − d1)w)(−λ2s3)) +

f2(2(pu + qv + (r − d1)w)λ2c3) +

f3(2(pu + qv + (r − d1)w)µ2) +

r1(−λ2s3(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2)) +

r2(λ2c3(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2)) +

r3(µ2(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2)) +

f1(−2λ2s3(pu + qv + (r − d1)w)) +

f2(2λ2c3(pu + qv + (r − d1)w)) +

f3(2(pu + qv + (r − d1)w)µ2) +

(f1r2 − f2r1)2a2λ2 +

(f2r3 − f3r2)(2a2µ2s3 − 2d2c3) +

10

(f3r1 − f1r3)(−2a2µ2c3 − 2d2s3) +

((f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1)(−λ2s3) +

((f 21 + f 2

2 + f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2)λ2c3 +

((f 21 + f 2

2 + f 23 )r3 − 2(f1r1 + f2r2 + f3r3)f3)µ2 +

δ1(c3r1 + s3r2)− δ2(c3f1 + s3f2 + a2) =

−2a1c2(h2n1 − h1n2) + δ1(c2n1 + s2n2)− δ2(c2h1 + s2h2) (24)

The functions (f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1, (f

21 + f 2

2 + f 23 )r2 −

2(f1r1 + f2r2 + f3r3)f2, and (f 21 + f 2

2 + f 23 )r3 − 2(f1r1 + f2r2 + f3r3)f3 are

linear combinations of s4s5, s4c5, c4s5, c4c5, s4, c4, s5, c5, 1.

Property 8 : (p21 + p2

2 + p23)l3 − 2(p1l1 + p2l2 + p3l3)p3 has the same set of

power products as equations ( 10)-( 15).

(p21 + p2

2 + p23)l3 − 2(p1l1 + p2l2 + p3l3)p3 :

r1((a22 − d2

2)µ2s3 − 2a2d2c3) +

r2((a22 − d2

2)(−µ2c3)− 2a2d2s3) +

r3((a22 − d2

2)λ2) +

(f1r2 − f2r1)(−2a3µ2) +

(f2r3 − f3r2)2a2λ2s3 +

(f3r1 − f1r3)(−2a2λ2c3) +

µ2s3((f21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1)

−µ2c3((f21 + f 2

2 + f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2)−

2(f1r1 + f2r2 + f3r3)d2 =

(h21 + h2

2 + h23)n3 − 2(h1n1 + h2n2 + h3n3)h3 (25)

(h21 +h2

2 +h23)n3−2(h1n1 +h2n2 +h3n3)h3 is a linear combination of s1, c1, 1.

The existence of Properties 1 - 8 may be explained as follows : p1, p2, p3 areanalogs of l1, l2, l3 (modulo the constant terms) in the sense that, replacingf1, f2, f3, in p1, p2, p3, by r1, r2, r3, yields l1, l2, l3. (p2

1 + p22 + p2

3) is an analogof 2(p1l1 + p2l2 + p3l3) in the same sense (i.e. replacement of fi by ri). If we

11

treat (p1, p2, p3, p21 + p2

2 + p23) and (l1, l2, l3, 2(p1l1 + p2l2 + p3l3)) as 2 points in

4-space and form the following 4× 2 array :

∣∣∣∣∣∣∣∣∣

p1 l1p2 l2p3 l3

p21 + p2

2 + p23 2(p1l1 + p2l2 + p3l3)

∣∣∣∣∣∣∣∣∣

then, the functions in Properties 3 - 8, are obtained by taking all possiblesub-determinants of the above array in the spirit of Grassmann determinants(KLEIN[1935]). This suggests an underlying connection between the alge-braic and geometric points of view, associated with this problem.

Before proceeding further, it seems appropriate to summarize what has beendone so far. Starting with 6 scalar components of the closure equation, con-taining the 5 variables θ1, θ2, θ3, θ4, θ5 we simply rearranged these equationsto get the 6 equations p1, p2, p3, l1, l2, l3. Our goal is to eliminate 4 of the 5variables. We then demonstrated the existence of 8 “higher order” equationsin the ideal of p1, p2, p3, l1, l2, l3, which are linearly independent of p1, p2,p3, l1, l2, l3, but have the same power products as p1, p2, p3, l1, l2, l3. In thenext few sections, we show how all but one variable, can be eliminated usingthese 8 “new” equations together with p1, p2, p3, l1, l2, l3.

5.4 Elimination of θ1 and θ2

Equations ( 12), ( 15), ( 18), ( 19), ( 20), and ( 25), constitute a set of 6linearly independent equations in the 4 variables θ1, θ3, θ4, and θ5. They maybe written in matrix form as :

(ζ)

s4s5

s4c5

c4s5

c4c5

s4

c4

s5

c5

1

= (σ)

(s1

c1

)(26)

12

where ζ is a 6× 9 matrix whose entries are linear combinations of s3, c3, 1,σ is a 6× 2 matrix whose entries are constants.

We may use any 2 of the 6 scalar equations in equation ( 26) to eliminate s1

and c1 from the remaining 4 equations to bring equation ( 26) to the form :

(η)

s4s5

s4c5

c4s5

c4c5

s4

c4

s5

c5

1

= 0 (27)

where η is a 4× 9 matrix whose entries are linear combinations of s3, c3, 1.

Equations ( 10), ( 11), ( 13), ( 14), ( 21), ( 22), ( 23), and ( 24) constitute aset of 8 linearly independent equations in the 5 variables θ1, θ2, θ3, θ4, and θ5.We may eliminate θ1 and θ2 from this set of equations to obtain 2 equationscontaining just the 3 variables θ3, θ4, and θ5, as follows :

µ21

2a1{equation( 23)− δ1 × equation( 13) + δ2 × equation( 10)}

−λ1µ1{equation( 21)}+ µ1w{equation( 11)}−µ1(r − d1){equation( 14)} gives the following equation :

f1(µ2

1

a1

(pu + qv + (r − d1)w)c3 − µ1wλ2s3) +

f2(µ2

1

a1

(pu + qv + (r − d1)w)s3 + µ1wλ2c3) +

f3(µ1µ2w) +

r1{µ21

a1

((−p2 − q2 − (r − d1)2 + a2

1 − a22 + d2

2)c3 −2d2µ2a2s3)− d2λ1λ2µ1c3 − µ1(r − d1)λ2c3}+

r3{ µ21

2a1

(−2d2λ2a2) + λ1µ1d2µ2 − µ1µ2(r − d1)}+

13

(f1r2 − f2r1)(µ2

1

a1

d2µ2) +

(f2r3 − f3r2)(−µ21

a1

d2λ2s3 − λ1µ1c3) +

(f3r1 − f1r3)(µ2

1

a1

d2λ2c3 − λ1µ1s3) +

((f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1)

µ21

2a1

c3 +

((f 21 + f 2

2 + f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2)

µ21

2a1

s3 +

(f1r1 + f2r2 + f3r3)(−µ21

a1

a2) +

µ21

a1

a2(pu + qv + (r − d1)w) = 0 (28)

µ21

2a1{equation( 24)} − λ1µ1{equation( 22)} − µ1w{equation( 10)}+

µ1(r − d1){equation( 13)} gives the following equation :

f1(−µ21

a1

λ2s3(pu + qv + (r − d1)w)− µ1wc3) +

f2(µ2

1

a1

λ2c3(pu + qv + (r − d1)w)− µ1ws3) +

f3(µ2

1

a1

µ2(pu + qv + (r − d1)w)) +

r1(− µ21

2a1

λ2s3(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2)−λ1µ1(d2c3 − a2µ2s3) + µ1(r − d1)c3) +

r2(µ2

1

2a1

λ2c3(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2)−λ1µ1(d2s3 + a2µ2c3) + µ1(r − d1)s3) +

r3(µ2

1

2a1

µ2(−p2 − q2 − (r − d1)2 + a2

1 + a22 + d2

2) + λ1µ1a2λ2) +

(f1r2 − f2r1)(µ2

1

a1

a2λ2 − λ1µ1µ2) +

14

(f2r3 − f3r2)(µ2

1

a1

(a2µ2s3 − d2c3) + λ1µ1λ2s3) +

(f3r1 − f1r3)(µ2

1

a1

(−a2µ2c3 − d2s3)− λ1µ1λ2c3) +

((f 21 + f 2

2 + f 23 )r1 − 2(f1r1 + f2r2 + f3r3)f1)(− µ2

1

2a1

λ2s3) +

((f 21 + f 2

2 + f 23 )r2 − 2(f1r1 + f2r2 + f3r3)f2)(

µ21

2a1

λ2c3) +

((f 21 + f 2

2 + f 23 )r3 − 2(f1r1 + f2r2 + f3r3)f3)(

µ21

2a1

µ2)

−µ1a2w = 0 (29)

Equations ( 27), ( 28), and ( 29) constitute a set of 6 linearly independentequations in the 3 unknowns θ3, θ4, and θ5. In matrix form they may bewritten as :

(Σ)

s4s5

s4c5

c4s5

c4c5

s4

c4

s5

c5

1

= 0 (30)

where Σ is a 6× 9 matrix whose entries are linear combinations of s3, c3, 1.

5.5 Elimination of θ4 and θ5

We make the following substitutions in equation ( 30) :

s4 ← 2x4

1 + x24

c4 ← 1− x24

1 + x24

s5 ← 2x5

1 + x25

15

c5 ← 1− x25

1 + x25

where x4 = tan( θ4

2), x5 = tan( θ5

2).

We then multiply each equation by (1+x24) and (1+x2

5) to clear denominators.Equation ( 30) then takes the form

(Σ′)

x24x

25

x24x5

x24

x4x25

x4x5

x4

x25

x5

1

= 0 (31)

where Σ′ is a 6× 9 matrix whose entries are linear combinations of s3, c3, 1.We make the following substitutions in equation ( 31) :

s3 ← 2x3

1 + x23

c3 ← 1− x23

1 + x23

We multiply the first 4 scalar equations in equation ( 31) by (1+x23) to clear

denominators. The resulting equation is of the form

(Σ′′)

x24x

25

x24x5

x24

x4x25

x4x5

x4

x25

x5

1

= 0 (32)

16

where Σ′′ is a 6×9 matrix. The entries in the first 4 rows of Σ′′ are quadraticpolynomials in x3. The entries in the last 2 rows are rational functions of x3,the numerators being quadratic polynomials in x3, the denominators being(1 + x2

3). It is noteworthy that the determinant of the 6× 6 array comprisedof any set of 6 columns of Σ′′ is always an 8th degree polynomial and not arational function.

We now eliminate x4 and x5 dialytically (see SALMON[1885]) as follows :Multiplying equation ( 32) by x4, we get the following equation :

(Σ′′)

x34x

25

x34x5

x34

x24x

25

x24x5

x24

x4x25

x4x5

x4

= 0 (33)

Equations ( 32) and ( 33) taken together may be written in matrix form as :

(Σ′′ 00 Σ′′

)

x34x

25

x34x5

x34

x24x

25

x24x5

x24

x4x25

x4x5

x4

x25

x5

1

= 0 (34)

Equation ( 34) constitutes a set of 12 linearly independent equations in the12 terms x3

4x25, x

34x5, x

34, x

24x

25, x

24x5, x

24, x4x

25, x4x5, x4, x

25, x5, 1. This is clearly

an overconstrained linear system. In order for this system to have a non-trivial solution, the coefficient matrix must be singular. The determinant

17

of the coefficient matrix is a 16th degree polynomial in x3. The roots ofthis polynomial give the values of x3 corresponding to the 16 solutions ofthe inverse kinematics problem. For each value of x3 thus obtained, thecorresponding value of θ3 may be computed using the formula θ3 = 2tan−1x3.

5.6 The Remaining Joint Variables

For each value of θ3, we may compute the remaining joint variables as follows :

We insert the numerical values of θ3 in the matrix Σ′ of equation ( 31). Thisequation may then be written in the form

(Γ)

x25

x5

1

= 0 (35)

where Γ is a 6 × 3 matrix whose entries are quadratic polynomials in x4.Any 3 scalar equations in equation ( 35) constitute a set of 3 linear equa-tions in the terms x2

5, x5, 1. In order for such a system to have a non-trivialsolution the 3 × 3 coefficient matrix must be singular. Its determinant is a6th degree polynomial in x4. The roots of this polynomial will yield upto6 candidate values of θ4. Only real roots roots are candidate values. Twocandidate values of θ5 corresponding to each candidate value of θ4 may becomputed by inserting the numerical value of θ4 in the first scalar equationin equation ( 35) and solving the resulting quadratic in x5. At the end ofthese operations, we could have as many as 12 candidate (θ4, θ5) pairs. Weretain only those which satisfy all 6 scalar equations in equation ( 35).

We may compute the value of θ1 corresponding to the candidate (θ3, θ4, θ5)tuples obtained so far by substituting values for θ3, θ4 and θ5 in any 2 scalarequations in equation ( 26). The resulting equations may be solved for s1

and c1. This determines the corresponding θ1 uniquely.

We may then substitute the candidate (θ1, θ3, θ4, θ5) tuples in equations ( 10)and ( 11) to obtain 2 linear equations in s2 and c2. Solving for s2 and c2, wemay compute a unique corresponding value of θ2.

18

Finally, substitution of the candidate (θ1, θ2, θ3, θ4, θ5) tuples into the (1,1)and (2,1) elements of the following equation,

A6 = A−15 A−1

4 A−13 A−1

2 A−11 Ahand (36)

gives 2 linear equations in s6 and c6, which may be solved to determine aunique corresponding value of θ6.

At the end of the above operations, we will have several candidate solutiontuples (θ1, θ2, θ3, θ4, θ5, θ6). Only one of these will satisfy all scalar equationsin equation ( 3), and is the “true” solution. There may be as many as 16such “true” tuples for a given Ahand. In practice we find fewer than 16 realtuples for most cases.

In the next section, this procedure for computing the solutions to the inversekinematics problem is illustrated by means of a numerical example.

6 Numerical Example

Let the link parameters for a 6R manipulator be :

ai di αi(deg)0.8 0.9 20.01.2 3.7 31.00.33 1.0 45.01.8 0.5 81.00.6 2.1 12.02.2 0.63 100.0

Let the hand location at the goal position be :

Ahand =

0.354937475307970 0.4616395739917430.876709605247149 0.1376161858179770.324653132880913 −0.876327957516839

0 0

∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣∣

−0.812962663562556 6.821518371502130.460914366741046 1.46146704002829−0.355878707125018 5.36950521368663

0 1

19

The numerical values of the matrices ζ and σ in equation ( 26) are as follows:(let ζi,j be the (i, j)th entry of ζ)

ζ1,1 = −4.8341E − 002s3

ζ1,2 = 0.2185c3 + 0.3636

ζ1,3 = 3.4182E − 002c3 + 5.6889E − 002

ζ1,4 = 0.3090s3

ζ1,5 = 1.0682s3 + 0.6555c3 + 1.0909

ζ1,6 = 0.9270s3 − 0.7553c3 − 1.2571

ζ1,7 = −0.2158c3 + 0.3591

ζ1,8 = 0.0000

ζ1,9 = 0.1699s3 − 0.3017c3 + 1.0562

ζ2,1 = 7.5718E − 002c3 + 0.1260

ζ2,2 = 1.6751E − 002s3

ζ2,3 = 0.1070s3

ζ2,4 = −1.1845E − 002c3 − 1.9713E − 002

ζ2,5 = 0.4975s3

ζ2,6 = −0.3518c3 − 0.5855

ζ2,7 = 0.0000

ζ2,8 = 7.4786E − 002c3 − 0.1244

ζ2,9 = −5.5726E − 002c3 + 0.8456

ζ3,1 = −0.3577s3 − 0.2252c3 − 6.1948E − 002

ζ3,2 = −1.0182s3 + 1.6169c3 + 3.5396

ζ3,3 = −0.1592s3 + 0.2529c3 + 0.5537

ζ3,4 = 2.2867s3 + 1.4400c3 + 0.3960

ζ3,5 = 4.8504s3 + 9.8289c3 + 11.9878

ζ3,6 = 10.3802s3 − 1.2697c3 − 11.0482

ζ3,7 = 1.0056s3 − 1.5970c3 + 4.0886

20

ζ3,8 = 2.1600

ζ3,9 = 2.6637s3 − 1.4408c3 − 15.0776

ζ4,1 = −0.1764s3 + 0.2801c3 + 0.6132

ζ4,2 = 6.1980E − 002s3 + 3.9029E − 002c3 + 1.0733E − 002

ζ4,3 = 0.3962s3 + 0.2494c3 + 6.8610E − 002

ζ4,4 = 2.7597E − 002s3 − 4.3826E − 002c3 − 9.5937E − 002

ζ4,5 = 1.8410s3 + 1.1593c3 + 0.3188

ζ4,6 = 0.8197s3 − 1.3018c3 − 2.8497

ζ4,7 = 0.3742

ζ4,8 = −0.1742s3 + 0.2767c3 − 0.7084

ζ4,9 = 0.1298s3 − 0.2061c3 + 2.6476

ζ5,1 = 0.1644s3 − 0.3399c3 − 0.3064

ζ5,2 = 0.1096s3 − 0.1816c3 − 0.2744

ζ5,3 = −0.5161s3 − 0.1698c3 − 0.1044

ζ5,4 = −0.2450s3 − 8.1432E − 002c3 − 0.1761

ζ5,5 = −1.0721s3 − 0.6735c3 − 0.2927

ζ5,6 = −0.6006s3 + 0.6420c3 − 0.1367

ζ5,7 = 0.1570c3 − 0.2613

ζ5,8 = 7.4786E − 002s3 − 0.4064c3 + 0.2560

ζ5,9 = −5.5726E − 002s3 − 0.5107c3 + 1.9591

ζ6,1 = 2.0411s3 − 1.1193c3 − 2.1400

ζ6,2 = 2.1491s3 − 1.4159c3 − 1.2087

ζ6,3 = −2.0724s3 − 2.8764c3 − 0.1921

ζ6,4 = −2.0994s3 − 1.7043c3 − 1.1407

ζ6,5 = −5.8557s3 − 14.0583c3 − 7.2511

ζ6,6 = −12.2310s3 + 3.3148c3 + 12.6847

ζ6,7 = −0.4413s3 + 1.8434c3 − 4.7130

ζ6,8 = 2.1372s3 − 1.5535c3 + 2.1265

ζ6,9 = −0.6635s3 − 1.3177c3 + 12.3027

21

σ =

1.9376 0.17170.2037 −1.8978E − 0020.8036 −9.0644

−4.4390E − 002 −0.4766−0.5046 2.462210.4252 9.6928

Equation ( 28) and ( 29) are respectively,

(3.6518E − 002s3 + 6.4023E − 002c3 − 0.2570)s4s5 +

(−0.1845s3 + 0.3376c3 − 0.1509)s4c5 +

(0.1950s3 − 0.1511c3 − 5.5346E − 002)c4s5 +

(0.4357s3 + 0.1141c3 − 3.2619E − 002)c4c5 +

(0.4444s3 − 1.6433c3 − 0.3307)s4 +

(−0.8596s3 − 0.3953c3 + 0.8940)c4 +

(−7.8657E − 002s3 − 0.3024c3 − 0.2097)s5 +

(0.1161s3 + 0.2475c3 + 0.2567)c5 +

(−0.6323s3 + 0.5392c3 − 0.3468) = 0,

and

(−0.1312s3 − 5.1791E − 002c3 − 0.1401)s4s5 +

(−0.3451s3 − 0.2183c3 − 6.4256E − 002)s4c5 +

(2.8118E − 002s3 + 0.2959c3 + 8.7621E − 003)c4s5 +

(−0.1619s3 + 0.4665c3 − 5.3125E − 002)c4c5 +

(1.1322s3 + 0.7809c3 − 0.2345)s4 +

(0.5675s3 − 0.4617c3 + 0.7077)c4 +

(0.3335s3 − 3.9865E − 002c3 − 9.9402E − 002)s5 +

(−0.3226s3 + 3.6251E − 002c3 + 0.1035)c5 +

(−0.5640s3 − 0.6135c3 − 6.9881E − 002) = 0

The 16th degree polynomial in x3 obtained from the determinant of the co-

efficient matrix

(Σ′′ 00 Σ′′

)in equation ( 34) is :

x16 + 0.5103x15 − 0.2149x14

22

+4.753213 + 1.774512 + 9.801311

+14.1216x10 − 2.5832x9 + 15.8339x8

−9.9780x7 + 3.1607x6 + 2.6546x5

−1.2420x4 + 7.1100x3 + 1.5436x2

+2.0053x + 1.0870 = 0

The roots of this polynomial are :

±0.7328 + 0.1569−0.4142

±0.8787− 0.04110.8077± 0.3415±0.8677− 0.0126

−0.7271±1.2465− 0.18791.2843± 1.2066±0.2041− 1.6917

There are only 2 real roots. This implies that for this particular example,there are at most 2 configurations in which the manipulator can reach the goalposition. The values of θ3 corresponding to x3 = −0.4142, and x3 = −0.7271,are respectively −44.9999 deg and −72.0441 deg.

For θ3 = −44.9999 deg, we now demonstrate the procedure for computingthe other joint variables. Substituting this value of θ3 in equation ( 31) weget the following equation :

6.3308 0.7855 6.6708 4.7417 1.6913 8.69652.2800 0.2171 2.1599 −0.3666 0.9506 −0.30081.1513 0.5418 6.5753 25.5052 −11.9936 41.80448.2007 0.7953 7.6329 −0.4562 3.6907 −1.0164−1.5823 −0.1592 −0.1537 −4.7455 −1.1134 −3.7228

−5.9739E − 002 −1.3158 −0.1392 −1.1912 −0.3938 −1.0720

∣∣∣∣∣∣∣∣∣∣∣∣∣∣

23

∣∣∣∣∣∣∣∣∣∣∣∣∣∣

−6.9844 2.0980 −7.1841−0.4014 −3.3475E − 002 −0.6083−32.3873 6.4032 −29.2034−0.7111 0.6876 −2.05341.8912 −1.5655 2.1012−1.0234 −0.3872 0.7306

x24x

25

x24x5

x24

x4x25

x4x5

x4

x25

x5

1

= 0 (37)

The matrix Γ of equation ( 35) is :

6.3308x24 + 4.7417x4 − 6.9844

2.2800x24 − 0.3666x4 − 0.4014

1.1513x24 + 25.5052x4 − 32.3873

8.2007x24 − 0.4562x4 − 0.7111

−1.5823x24 − 4.7455x4 + 1.8912

−5.9739E − 002x24 − 1.1912x4 − 1.0234

∣∣∣∣∣∣∣∣∣∣∣∣∣∣

∣∣∣∣∣∣∣∣∣∣∣∣∣∣

0.7855x24 + 1.6913x4 + 2.0980 6.6708x2

4 + 8.6965x4 − 7.18410.2171x2

4 + 0.9506x4 − 3.3475E − 002 2.1599x24 − 0.3008x4 − 0.6083

0.5418x24 − 11.9936x4 + 6.4032 6.5753x2

4 + 41.8044x4 − 29.20340.7953x2

4 + 3.6907x4 + 0.6876 7.6329x24 − 1.0164x4 − 2.0534

−0.1592x24 − 1.1134x4 − 1.5655 −0.1537x2

4 − 3.7228x4 + 2.1012−1.3158x2

4 − 0.3938x4 − 0.3872 −0.1392x24 − 1.0720x4 + 0.7306

We may treat the first 3 rows of equation ( 35) as a system of linear equationsin the terms x2

5, x5, 1. For a non-trivial solution to exist, the 3× 3 coefficientmatrix must be singular. Its determinant is :

−1.6204x64−7.2619x5

4−83.2504x44−13.8063x3

4+35.4224x24+1.5860x4+8.9615 = 0

The roots of this polynomial are :

±0.4201i− 0.0292,−0.8581, 0.7132,±6.8084i− 2.1390

The values of θ4 corresponding to the real roots are :

x4 = −0.8581 ⇒ θ4 = −81.2665 deg

24

x4 = 0.7132 ⇒ θ4 = 70.9999 deg

Substituting each of the candidate x4 values into the first scalar equation inequation ( 37) gives a quadratic equation in x5, the roots of which may beused to compute the corresponding values of θ5.

For x4 = −0.8581, the first scalar equation in equation( 37) becomes :

−6.3917x25 + 1.2252x5 − 9.7345 = 0

The roots of this equation are ±1.2303i + 0.0958. Clearly θ4 = −81.2665 degis no longer a candidate.

For x4 = 0.7132, the corresponding quadratic equation in x5 is :

−0.3811x25 + 3.7041x5 + 2.4130 = 0

The roots of this equation are −0.6128 and 10.3310. The corresponding val-ues of θ5 are respectively −63.0000 deg and 168.9424 deg.

At the end of these operations, we have the following candidate (θ4, θ5) pairs:

(70.9999,−63.0000), (70.9999, 168.9424)

Of these, only the first pair satisfies all 6 scalar equations in equation ( 37)and therefore becomes the sole candidate, i.e. our candidate (θ3, θ4, θ5) tupleis (−44.9999, 70.9999,−63.0000).

Substituting these values for θ3, θ4 and θ5 in the first 2 scalar equations inequation ( 26) gives the following 2 equations :

1.9376s1 + 0.1717c1 = 0.6354

0.2037s1 − (1.8978E − 002)c1 = 3.0882E − 002

Solving we get :

s1 = 0.2419, c1 = 0.9702,⇒ θ1 = 14.0000 deg

Our candidate solution tuple becomes (θ1, θ3, θ4, θ5) = (14.0000, −44.9999,70.9999, −63.0000).

25

Substituting these values of θ1, θ3, θ4 and θ5 in equation ( 10) and ( 11) gives:

−0.2888s2 + 4.5755c2 = 3.8313

4.5755s2 + 0.2888c2 = 2.5178

Solving, we get :

s2 = 0.4954, c2 = 0.8686,⇒ θ2 = 29.6999 deg

Finally, θ6 may be computed from the following matrix equation :

A6 = A−15 A−1

4 A−13 A−1

2 A−11 Ahand

Since we have computed θ1, θ2, θ3, θ4, and θ5, the right-hand side is known.The (1,1) and (2,1) elements from the above equation give the following 2scalar equations :

s6 = 0.9848

c6 = 0.1736

The value of θ6 satisfying the above 2 relations is 10.0000 deg.

Thus one solution to the inverse kinematics problem is the tuple(14.0000, 29.6999,−44.9999, 70.9999,−63.0000, 10.0000).

The solution tuple corresponding to the other real root (x3 = −0.7271) ofthe 16th degree polynomial in x3, is (13.1098, 50.9923, −72.0441, 72.0649,−7.1962,−37.8523)

7 Conclusion

In this paper, a new solution procedure for the inverse kinematics problem ofthe 6R manipulator of general geometry, was presented. This procedure in-volved a sequential elimination of variables, resulting in a univariate polyno-mial of degree 16 in the tangent of the half-angle of one of the joint variables.Some new and interesting properties of the ideal of the closure equations ofthe 6R manipulator were presented. The appearance of extraneous roots,which is common in sequential elimination methods was avoided by exploit-ing these properties during the elimination procedure.

26

8 Acknowledgement

Prof. Bernard Roth gratefully acknowledges the financial support of the Sys-tem Development Foundation and Control Data Corporation.

9 References

ALBALA and ANGELES[1979 ]Albala, H., and Angeles, J., Numerical Solution to the Input-OutputDisplacement Equation of the General 7R Spatial Mechanism, Proceed-ings of the Fifth World Congress on Theory of Machines and Mecha-nisms, pp. 1008-1011.

DUFFY and CRANE[1980 ]Duffy, J., and Crane, C., A Displacement Analysis of the General Spa-tial 7R Mechanism, Mechanisms and Machine Theory, Vol. 15, pp.153-169.

KLEIN[1935 ]Klein, F., Elementary Mathematics from an Advanced Standpoint, Trans-lated from the Third German Edition by E.R. Hedrick and C.A. Noble,The Macmillan Company, New York.

LEE and LIANG[1988a ]Lee, H-Y., and Liang, C-G., A New Vector Theory for the Analysis ofSpatial Mechanisms, Mechanisms and Machine Theory, Vol. 23, No.3, pp. 209-217.

LEE and LIANG[1988b ]Lee H-Y., and Liang, G-G., Displacement Analysis of the General Spa-tial 7-Link 7R Mechanism, ibid, pp. 219-226.

PIEPER[1968 ]Pieper, D., The Kinematics of Manipulators Under Computer Control,Ph.D. Thesis, Stanford University.

ROTH, RASTEGAR and SCHEINMAN[1973 ]Roth, B., Rastegar, J., and Scheinman, V., On the Design of Computer

27

Controlled Manipulators, On the Theory and Practice of Robots andManipulators, Vol. 1, First CISM-IFToMM Symposium, September,pp. 93-113.

SALMON[1885 ]Salmon, G., Lessons Introductory to the Modern Higher Algebra, ChelseaPublishing Co., New York.

TSAI and MORGAN[1985 ]Tsai, L-W., and Morgan, A., Solving the Kinematics of the Most Gen-eral Six- and Five-Degree-of-Freedom Manipulators by ContinuationMethods, Transactions of the ASME, Journal of Mechanisms, Trans-missions, and Automation in Design, Vol. 107, June, pp. 189-200.

28