Kinematic Analysis of the 6R Manipulator of General Geometry

Transcription

1 Kinematic Analysis of the 6R Manipulator of General Geometry Madhusudan Raghavan Powertrain Systems Research Lab General Motors R&D Center Warren, MI and Bernard Roth, Professor Design Division Mechanical Engineering Department Stanford University, CA Introduction This paper presents a new solution to the inverse kinematics problem for the 6R series manipulator of general geometry. Such a manipulator has 6 revolute (R) joints arranged in series. The link lengths, twist angles and offsets, may be arbitrary real numbers. The inverse kinematics problem may be stated as follows : Given (i) the manipulator geometry (i.e. location of the base, link lengths, twist angles, and offsets), (ii) the goal position to which the hand must be moved, Find the values of the joint angles which will place the hand in the goal position. When stated in mathematical terms, this problem reduces to the solution of a principal system of multivariate polynomials (i.e. n polynomials in n unknowns). In this paper we present a new solution procedure based on dialytic elimination. This procedure exploits the structure of the ideal of the closure equations of the 6R manipulator and results in a compact elimination algorithm. The result is a 16 th degree polynomial in the tangent of the half- 1

2 angle of one of the joint variables. 2 Previous Work The earliest systematic assault upon this problem appears to have been made by PIEPER[1968]. Pieper proposed iterative numerical techniques for complex manipulators of general geometry. He presented closed-form solutions for manipulators with special geometries (e.g. 3 consecutive joint axes intersecting at a point, etc.). For 6R manipulators with general geometry, Pieper showed that a naive elimination strategy would yield a univariate polynomial of degree 524,288. ROTH, RASTEGAR and SCHEINMAN[1973] using synthetic arguments showed that this problem has in general at most 32 solutions, i.e. there are 32 distinct configurations of the manipulator, in which its hand is at the desired goal position. Their proof was non-constructive. ALBALA and ANGELES[1979], using indicial notation, presented the first constructive solution to this problem. Their solution was in the form of a determinant whose entries were quartic polynomials in the tangent of the half-angle of one of the joint variables. DUFFY and CRANE[1980] later provided a different solution by combining spherical trigonometry and dialytic elimination to obtain a 32 nd degree polynomial in the tangent of the half-angle of one of the joint variables. Their 32 nd degree polynomial alway yielded extraneous or false roots in addition to the desired values. TSAI and MORGAN[1985] used homotopy continuation to solve the inverse kinematics problem and found only 16 solutions for various 6R manipulators of different geometries. They therefore conjectured that this problem has at most 16 solutions. Recently, LEE and LIANG[1988a and 1988b], used extensions of Duffy s method to obtain by dialytic elimination a 16 th degree polynomial in the tangent of the half-angle of one of the joint variables, thus confirming the 2

3 conjecture of TSAI and MORGAN[1985]. The solution presented in this paper is the result of our quest for a simpler solution than the procedure used by LEE and LIANG[1988b]. 3 Notational Preliminaries The notation used in this paper is identical to that appearing in the paper by TSAI and MORGAN[1985]. We present the essential features here. The links of the 6R manipulator are numbered from 1 to 7, the fixed or base link being 1, and the outermost link or hand being 7. A coordinate system is attached to each link to facilitate a mathematical description of the linkage and the relative arrangement of the links. The coordinate system attached to the i th link is numbered i. The 4 4 transformation matrix relating coordinate systems i + 1 and i is as follows : A i = c i s i λ i s i µ i a i c i s i c i λ i c i µ i a i s i 0 µ i λ i d i where s i = sinθ i, c i = cosθ i, λ i = cosα i, µ i = sinα i, a i is the length of link i + 1, α i is the twist angle between the axes of joints i and i + 1, d i 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 in the paper by TSAI and MORGAN[1985]. (1) 3

4 4 The Problem Statement in Mathematical Terms The closure equation for the 6R manipulator is the following matrix equation A 1 A 2 A 3 A 4 A 5 A 6 = A hand (2) A hand is the 4 4 transformation matrix describing the Cartesian coordinate system 7, attached to the hand (or last link) with respect to coordinate system 1, attached to the base link. The entries of this matrix are known because the hand coordinates in the goal position are specified. The lefthand side of the above matrix equation describes coordinate system 7 with respect to coordinate system 1, in terms of the relative arrangements of the intermediate coordinate systems. The quantities a i, d i, λ i, µ i, i = 1,..., 6, appearing in the matrices on the left-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 p 1, p 2, p 3, l 1, l 2, l 3 The matrix equation ( 2), is equivalent to 12 scalar equations. Of these only 6 equations are independent, because the submatrix comprised of the first 3 rows and columns is orthogonal. We perform operations on these multivariate equations and eliminate all but one variable, thus reducing the problem to the solution of a single equation in one variable. This is in the spirit of Gauss Elimination applied to linear systems of equations, except that the equations involved in this problem are non-linear. Equation ( 2) may be rewritten as A 3 A 4 A 5 = A 1 2 A 1 1 A hand A 1 6 (3) 4

5 Let A hand be equal to l x m x n x ρ x l y m y n y ρ y l z m z n z ρ z The 6 scalar equations obtained from columns 3 and 4 of equation ( 3) are devoid of θ 6. We therefore work with these 6 equations, with the goal of eliminating 4 of the 5 remaining variables. These 6 equations are :. c 3 f 1 + s 3 f 2 = c 2 h 1 + s 2 h 2 a 2 (4) where s 3 f 1 c 3 f 2 = λ 2 (s 2 h 1 c 2 h 2 ) + µ 2 (h 3 d 2 ) (5) f 3 = µ 2 (s 2 h 1 c 2 h 2 ) + λ 2 (h 3 d 2 ) (6) c 3 r 1 + s 3 r 2 = c 2 n 1 + s 2 n 2 (7) s 3 r 1 c 3 r 2 = λ 2 (s 2 n 1 c 2 n 2 ) + µ 2 n 3 (8) r 3 = µ 2 (s 2 n 1 c 2 n 2 ) + λ 2 n 3 (9) f 1 = c 4 g 1 + s 4 g 2 + a 3 f 2 = λ 3 (s 4 g 1 c 4 g 2 ) + µ 3 g 3 f 3 = µ 3 (s 4 g 1 c 4 g 2 ) + λ 3 g 3 + d 3 r 1 = c 4 m 1 + s 4 m 2 r 2 = λ 3 (s 4 m 1 c 4 m 2 ) + µ 3 m 3 r 3 = µ 3 (s 4 m 1 c 4 m 2 ) + λ 3 m 3 g 1 = c 5 a 5 + a 4 g 2 = s 5 λ 4 a 5 + µ 4 d 5 g 3 = s 5 µ 4 a 5 + λ 4 d 5 + d 4 h 1 = c 1 p + s 1 q a 1 h 2 = λ 1 (s 1 p c 1 q) + µ 1 (r d 1 ) h 3 = µ 1 (s 1 p c 1 q) + λ 1 (r d 1 ) 5

6 m 1 = s 5 µ 5 m 2 = c 5 λ 4 µ 5 + µ 4 λ 5 m 3 = c 5 µ 4 µ 5 + λ 4 λ 5 n 1 = c 1 u + s 1 v n 2 = λ 1 (s 1 u c 1 v) + µ 1 w n 3 = µ 1 (s 1 u c 1 v) + λ 1 w p = l x a 6 (m x µ 6 + n x λ 6 )d 6 + ρ x q = l y a 6 (m y µ 6 + n y λ 6 )d 6 + ρ y r = l z a 6 (m z µ 6 + n z λ 6 )d 6 + ρ z u = m x µ 6 + n x λ 6 v = m y µ 6 + n y λ 6 w = m z µ 6 + n z λ 6 Equations ( 4)-( 9) are the same as equations (41)-(46) in the paper by TSAI and MORGAN[1985]. We rearrange these 6 equations as follows : Equation ( 4) is rewritten as : p 1 : c 3 f 1 + s 3 f 2 + a 2 = c 2 h 1 + s 2 h 2 (10) Equation ( 5) ( λ 2 ) + Equation ( 6) µ 2 gives : p 2 : µ 2 f 3 λ 2 (s 3 f 1 c 3 f 2 ) = s 2 h 1 c 2 h 2 (11) Equation ( 5) µ 2 + Equation ( 6) λ 2 gives : p 3 : µ 2 (s 3 f 1 c 3 f 2 ) + λ 2 f 3 + d 2 = h 3 (12) We rewrite equation ( 7) without any change : l 1 : c 3 r 1 + s 3 r 2 = c 2 n 1 + s 2 n 2 (13) 6

7 Equation ( 8) ( λ 2 ) + Equation ( 9) µ 2 gives : l 2 : µ 2 r 3 λ 2 (s 3 r 1 c 3 r 2 ) = s 2 n 1 c 2 n 2 (14) Equation ( 8) µ 2 + equation ( 9) λ 2 gives : l 3 : µ 2 (s 3 r 1 c 3 r 2 ) + λ 2 r 3 = n 3 (15) p 1, p 2, p 3, l 1, l 2, and l 3 are labels used to refer to these equations in subsequent derivations. 5.2 The Structure of p 1, p 2, p 3, l 1, l 2, l 3 It is noteworthy that f 1, f 2, f 3, r 1, r 2, and r 3 are linear combinations of the terms s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. h 1, h 2, h 3, n 1, n 2 and n 3 are linear combinations of the terms s 1, c 1, 1. Equations ( 10)-( 15) may be written in matrix form as : s 4 s 5 s 4 c s 1 s 2 5 s c 4 s 5 1 c 2 c 4 c c 1 s 2 5 c (A) s 4 = (B) 1 c 2 (16) c 4 s 1 s c 1 5 s c 5 2 c 1 2 where A is a 6 9 matrix whose entries are linear combinations of s 3, c 3, 1, B is a 6 8 matrix whose entries are all constants. 5.3 The Ideal of p 1, p 2, p 3, l 1, l 2, l 3 The Ideal generated by a set of polynomials q 1, q 2,..., q r in the variables x 1, x 2,..., x m is the set of all elements of the form q 1 β 1 + q 2 β q r β r, where β 1, β 2,..., β r are arbitrary elements of the set of all polynomials in x 1, x 2,..., x m. The Ideal generated by p 1, p 2, p 3, l 1, l 2, l 3 has the following interesting properties : 7

8 Property 1 : The function p p p 2 3 has the same power products as equations ( 10)-( 15) i.e. it has the structure : (M) s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 s 4 c 4 s 5 c 5 1 = (N) s 1 s 2 s 1 c 2 c 1 s 2 c 1 c 2 s 1 c 1 s 2 c 2 (17) where M is a 1 9 matrix whose entries are linear combinations of s 3, c 3, 1, N is a 1 8 matrix whose entries are constants. By power products, we mean terms (e.g. the power products of the polynomial 5x 2 y + 3xz + 9y 2 + 4z = 0 are x 2 y, xz, y 2, and z). p p p 2 3 : f f f a d a 2 (c 3 f 1 + s 3 f 2 ) + 2d 2 (µ 2 (s 3 f 1 c 3 f 2 ) + λ 2 f 3 ) = h h h 2 3 (18) Property 1 is true because f f f 2 3 is a linear combination of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1, and h h h 2 3 is a linear combination of s 1, c 1, 1. Property 2 : p 1 l 1 + p 2 l 2 + p 3 l 3 has the same power products as equations ( 10)-( 15). p 1 l 1 + p 2 l 2 + p 3 l 3 : f 1 r 1 + f 2 r 2 + f 3 r 3 + a 2 (c 3 r 1 + s 3 r 2 ) + d 2 (λ 2 r 3 + µ 2 (s 3 r 1 c 3 r 2 )) = h 1 n 1 + h 2 n 2 + h 3 n 3 (19) f 1 r 1 +f 2 r 2 +f 3 r 3 is a linear combination of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. h 1 n 1 + h 2 n 2 + h 3 n 3 is a linear combination of s 1, c 1, 1. Property 3 : p 1 l 2 p 2 l 1 has the same power products as equations ( 10)- ( 15). p 1 l 2 p 2 l 1 : a 2 (µ 2 r 3 λ 2 (s 3 r 1 c 3 r 2 )) + µ 2 c 3 (f 1 r 3 f 3 r 1 ) + 8

9 λ 2 (f 1 r 2 f 2 r 1 ) + µ 2 s 3 (f 2 r 3 f 3 r 2 ) = h 2 n 1 h 1 n 2 (20) f 1 r 3 f 3 r 1, f 1 r 2 f 2 r 1, and f 2 r 3 f 3 r 2 are all linear combinations of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. h 2 n 1 h 1 n 2 is a linear combination of s 1, c 1, 1. Property 4 : p 2 l 3 p 3 l 2 has the same power products as equations ( 10)- ( 15). p 2 l 3 p 3 l 2 : s 3 (f 3 r 1 f 1 r 3 ) + c 3 (f 2 r 3 f 3 r 2 ) + r 1 d 2 λ 2 s 3 r 2 d 2 λ 2 c 3 r 3 d 2 µ 2 = s 2 (h 1 n 3 h 3 n 1 ) + c 2 (h 3 n 2 h 2 n 3 ) (21) As stated earlier, f 3 r 1 f 1 r 3 and f 2 r 3 f 3 r 2 are linear combinations of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. h 1 n 3 h 3 n 1 and h 3 n 2 h 2 n 3 are linear combinations of s 1, c 1, 1. Property 5 : p 3 l 1 p 1 l 3 has the same power products as equations ( 10)- ( 15). p 3 l 1 p 1 l 3 : µ 2 (f 1 r 2 f 2 r 1 ) + λ 2 s 3 (f 3 r 2 f 2 r 3 ) + λ 2 c 3 (f 3 r 1 f 1 r 3 ) + r 1 (d 2 c 3 a 2 µ 2 s 3 ) + r 2 (d 2 s 3 + a 2 µ 2 c 3 ) a 2 λ 2 r 3 = s 2 (h 3 n 2 h 2 n 3 ) + c 2 (h 3 n 1 h 1 n 3 ) (22) f 1 r 2 f 2 r 1, f 3 r 2 f 2 r 3, and f 3 r 1 f 1 r 3 are linear combinations of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. h 3 n 2 h 2 n 3 and h 3 n 1 h 1 n 3 are linear combinations of s 1, c 1, 1. Property 6 : (p p p 2 3)l 1 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 1 has the same power products as equations ( 10)-( 15). (p p p 2 3)l 1 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 1 : f 1 (2(pu + qv + (r d 1 )w)c 3 ) + f 2 (2(pu + qv + (r d 1 )w)s 3 ) + r 1 ( 2d 2 µ 2 a 2 s 3 (p 2 + q 2 + (r d 1 ) 2 a a 2 2 d 2 2)c 3 ) + 9

10 r 2 ( (p 2 + q 2 + (r d 1 ) 2 + a 2 1 a 2 2 d 2 2)s 3 + 2a 2 d 2 µ 2 c 3 ) + r 3 ( 2a 2 d 2 λ 2 ) + (f 1 r 2 f 2 r 1 )2d 2 µ 2 + (f 2 r 3 f 3 r 2 )( 2d 2 λ 2 s 3 ) + (f 3 r 1 f 1 r 3 )2d 2 λ 2 c 3 + ((f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 )c 3 + ((f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 )s 3 2a 2 (f 1 r 1 + f 2 r 2 + f 3 r 3 ) + 2a 2 (pu + qv + (r d 1 )w) + δ 1 (c 3 r 1 + s 3 r 2 ) δ 2 (c 3 f 1 + s 3 f 2 + a 2 ) = 2a 1 s 2 (h 2 n 1 h 1 n 2 ) + δ 1 (c 2 n 1 + s 2 n 2 ) δ 2 (c 2 h 1 + s 2 h 2 ) (23) where δ 1 = p 2 + q 2 + (r d 1 ) 2 a 2 1, δ 2 = pu + qv + (r d 1 )w. The functions (f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 and (f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 are linear combinations of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4,c 4, s 5, c 5, 1. Property 7 : (p p p 2 3)l 2 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 2 has the same set of power products as equations ( 10)-( 15). (p p p 2 3)l 2 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 2 : f 1 (2(pu + qv + (r d 1 )w)( λ 2 s 3 )) + f 2 (2(pu + qv + (r d 1 )w)λ 2 c 3 ) + f 3 (2(pu + qv + (r d 1 )w)µ 2 ) + r 1 ( λ 2 s 3 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2)) + r 2 (λ 2 c 3 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2)) + r 3 (µ 2 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2)) + f 1 ( 2λ 2 s 3 (pu + qv + (r d 1 )w)) + f 2 (2λ 2 c 3 (pu + qv + (r d 1 )w)) + f 3 (2(pu + qv + (r d 1 )w)µ 2 ) + (f 1 r 2 f 2 r 1 )2a 2 λ 2 + (f 2 r 3 f 3 r 2 )(2a 2 µ 2 s 3 2d 2 c 3 ) + 10

11 (f 3 r 1 f 1 r 3 )( 2a 2 µ 2 c 3 2d 2 s 3 ) + ((f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 )( λ 2 s 3 ) + ((f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 )λ 2 c 3 + ((f f f 2 3 )r 3 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 3 )µ 2 + δ 1 (c 3 r 1 + s 3 r 2 ) δ 2 (c 3 f 1 + s 3 f 2 + a 2 ) = 2a 1 c 2 (h 2 n 1 h 1 n 2 ) + δ 1 (c 2 n 1 + s 2 n 2 ) δ 2 (c 2 h 1 + s 2 h 2 ) (24) The functions (f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1, (f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2, and (f f f 2 3 )r 3 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 3 are linear combinations of s 4 s 5, s 4 c 5, c 4 s 5, c 4 c 5, s 4, c 4, s 5, c 5, 1. Property 8 : (p p p 2 3)l 3 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 3 has the same set of power products as equations ( 10)-( 15). (p p p 2 3)l 3 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )p 3 : r 1 ((a 2 2 d 2 2)µ 2 s 3 2a 2 d 2 c 3 ) + r 2 ((a 2 2 d 2 2)( µ 2 c 3 ) 2a 2 d 2 s 3 ) + r 3 ((a 2 2 d 2 2)λ 2 ) + (f 1 r 2 f 2 r 1 )( 2a 3 µ 2 ) + (f 2 r 3 f 3 r 2 )2a 2 λ 2 s 3 + (f 3 r 1 f 1 r 3 )( 2a 2 λ 2 c 3 ) + µ 2 s 3 ((f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 ) µ 2 c 3 ((f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 ) 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )d 2 = (h h h 2 3)n 3 2(h 1 n 1 + h 2 n 2 + h 3 n 3 )h 3 (25) (h h h 2 3)n 3 2(h 1 n 1 + h 2 n 2 + h 3 n 3 )h 3 is a linear combination of s 1, c 1, 1. The existence of Properties 1-8 may be explained as follows : p 1, p 2, p 3 are analogs of l 1, l 2, l 3 (modulo the constant terms) in the sense that, replacing f 1, f 2, f 3, in p 1, p 2, p 3, by r 1, r 2, r 3, yields l 1, l 2, l 3. (p p p 2 3) is an analog of 2(p 1 l 1 + p 2 l 2 + p 3 l 3 ) in the same sense (i.e. replacement of f i by r i ). If we 11

12 treat (p 1, p 2, p 3, p p p 2 3) and (l 1, l 2, l 3, 2(p 1 l 1 + p 2 l 2 + p 3 l 3 )) as 2 points in 4-space and form the following 4 2 array : p 1 l 1 p 2 l 2 p 3 l 3 p p p 2 3 2(p 1 l 1 + p 2 l 2 + p 3 l 3 ) then, the functions in Properties 3-8, are obtained by taking all possible sub-determinants of the above array in the spirit of Grassmann determinants (KLEIN[1935]). This suggests an underlying connection between the algebraic and geometric points of view, associated with this problem. Before proceeding further, it seems appropriate to summarize what has been done so far. Starting with 6 scalar components of the closure equation, containing the 5 variables θ 1, θ 2, θ 3, θ 4, θ 5 we simply rearranged these equations to get the 6 equations p 1, p 2, p 3, l 1, l 2, l 3. Our goal is to eliminate 4 of the 5 variables. We then demonstrated the existence of 8 higher order equations in the ideal of p 1, p 2, p 3, l 1, l 2, l 3, which are linearly independent of p 1, p 2, p 3, l 1, l 2, l 3, but have the same power products as p 1, p 2, p 3, l 1, l 2, l 3. In the next few sections, we show how all but one variable, can be eliminated using these 8 new equations together with p 1, p 2, p 3, l 1, l 2, l Elimination of θ 1 and θ 2 Equations ( 12), ( 15), ( 18), ( 19), ( 20), and ( 25), constitute a set of 6 linearly independent equations in the 4 variables θ 1, θ 3, θ 4, and θ 5. They may be written in matrix form as : (ζ) s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 s 4 c 4 s 5 c 5 1 ( ) s1 = (σ) c 1 12 (26)

13 where ζ is a 6 9 matrix whose entries are linear combinations of s 3, c 3, 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 s 1 and c 1 from the remaining 4 equations to bring equation ( 26) to the form : (η) s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 s 4 c 4 s 5 c 5 1 = 0 (27) where η is a 4 9 matrix whose entries are linear combinations of s 3, c 3, 1. Equations ( 10), ( 11), ( 13), ( 14), ( 21), ( 22), ( 23), and ( 24) constitute a set 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 equations containing just the 3 variables θ 3, θ 4, and θ 5, as follows : µ 2 1 2a 1 {equation( 23) δ 1 equation( 13) + δ 2 equation( 10)} λ 1 µ 1 {equation( 21)} + µ 1 w{equation( 11)} µ 1 (r d 1 ){equation( 14)} gives the following equation : f 1 ( µ2 1 a 1 (pu + qv + (r d 1 )w)c 3 µ 1 wλ 2 s 3 ) + f 2 ( µ2 1 a 1 (pu + qv + (r d 1 )w)s 3 + µ 1 wλ 2 c 3 ) + f 3 (µ 1 µ 2 w) + r 1 { µ2 1 a 1 (( p 2 q 2 (r d 1 ) 2 + a 2 1 a d 2 2)c 3 2d 2 µ 2 a 2 s 3 ) d 2 λ 1 λ 2 µ 1 c 3 µ 1 (r d 1 )λ 2 c 3 } + r 3 { µ2 1 2a 1 ( 2d 2 λ 2 a 2 ) + λ 1 µ 1 d 2 µ 2 µ 1 µ 2 (r d 1 )} + 13

14 (f 1 r 2 f 2 r 1 )( µ2 1 a 1 d 2 µ 2 ) + (f 2 r 3 f 3 r 2 )( µ2 1 a 1 d 2 λ 2 s 3 λ 1 µ 1 c 3 ) + (f 3 r 1 f 1 r 3 )( µ2 1 a 1 d 2 λ 2 c 3 λ 1 µ 1 s 3 ) + ((f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 ) µ2 1 2a 1 c 3 + ((f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 ) µ2 1 2a 1 s 3 + (f 1 r 1 + f 2 r 2 + f 3 r 3 )( µ2 1 a 1 a 2 ) + µ 2 1 a 1 a 2 (pu + qv + (r d 1 )w) = 0 (28) µ 2 1 2a 1 {equation( 24)} λ 1 µ 1 {equation( 22)} µ 1 w{equation( 10)}+ µ 1 (r d 1 ){equation( 13)} gives the following equation : f 1 ( µ2 1 a 1 λ 2 s 3 (pu + qv + (r d 1 )w) µ 1 wc 3 ) + f 2 ( µ2 1 a 1 λ 2 c 3 (pu + qv + (r d 1 )w) µ 1 ws 3 ) + f 3 ( µ2 1 a 1 µ 2 (pu + qv + (r d 1 )w)) + r 1 ( µ2 1 2a 1 λ 2 s 3 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2) λ 1 µ 1 (d 2 c 3 a 2 µ 2 s 3 ) + µ 1 (r d 1 )c 3 ) + r 2 ( µ2 1 2a 1 λ 2 c 3 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2) λ 1 µ 1 (d 2 s 3 + a 2 µ 2 c 3 ) + µ 1 (r d 1 )s 3 ) + r 3 ( µ2 1 2a 1 µ 2 ( p 2 q 2 (r d 1 ) 2 + a a d 2 2) + λ 1 µ 1 a 2 λ 2 ) + (f 1 r 2 f 2 r 1 )( µ2 1 a 1 a 2 λ 2 λ 1 µ 1 µ 2 ) + 14

15 (f 2 r 3 f 3 r 2 )( µ2 1 a 1 (a 2 µ 2 s 3 d 2 c 3 ) + λ 1 µ 1 λ 2 s 3 ) + (f 3 r 1 f 1 r 3 )( µ2 1 a 1 ( a 2 µ 2 c 3 d 2 s 3 ) λ 1 µ 1 λ 2 c 3 ) + ((f f f 2 3 )r 1 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 1 )( µ2 1 2a 1 λ 2 s 3 ) + ((f f f 2 3 )r 2 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 2 )( µ2 1 2a 1 λ 2 c 3 ) + ((f f f 2 3 )r 3 2(f 1 r 1 + f 2 r 2 + f 3 r 3 )f 3 )( µ2 1 2a 1 µ 2 ) µ 1 a 2 w = 0 (29) Equations ( 27), ( 28), and ( 29) constitute a set of 6 linearly independent equations in the 3 unknowns θ 3, θ 4, and θ 5. In matrix form they may be written as : s 4 s 5 s 4 c 5 c 4 s 5 c 4 c 5 (Σ) s 4 = 0 (30) c 4 s 5 c 5 1 where Σ is a 6 9 matrix whose entries are linear combinations of s 3, c 3, Elimination of θ 4 and θ 5 We make the following substitutions in equation ( 30) : s 4 2x x 2 4 c 4 1 x x 2 4 s 5 2x x

16 where x 4 = tan( θ 4 2 ), x 5 = tan( θ 5 2 ). c 5 1 x x 2 5 We then multiply each equation by (1+x 2 4) and (1+x 2 5) to clear denominators. Equation ( 30) then takes the form (Σ ) x 2 4x 2 5 x 2 4x 5 x 2 4 x 4 x 2 5 x 4 x 5 x 4 x 2 5 x 5 1 = 0 (31) where Σ is a 6 9 matrix whose entries are linear combinations of s 3, c 3, 1. We make the following substitutions in equation ( 31) : s 3 2x x 2 3 c 3 1 x x 2 3 We multiply the first 4 scalar equations in equation ( 31) by (1 + x 2 3) to clear denominators. The resulting equation is of the form (Σ ) x 2 4x 2 5 x 2 4x 5 x 2 4 x 4 x 2 5 x 4 x 5 x 4 x 2 5 x 5 1 = 0 (32) 16

17 where Σ is a 6 9 matrix. The entries in the first 4 rows of Σ are quadratic polynomials in x 3. The entries in the last 2 rows are rational functions of x 3, the numerators being quadratic polynomials in x 3, the denominators being (1 + x 2 3). It is noteworthy that the determinant of the 6 6 array comprised of any set of 6 columns of Σ is always an 8 th degree polynomial and not a rational function. We now eliminate x 4 and x 5 dialytically (see SALMON[1885]) as follows : Multiplying equation ( 32) by x 4, we get the following equation : x 3 4x 2 5 x 3 4x 5 x 3 4 x 2 4x 2 (Σ 5 ) x 2 4x 5 = 0 (33) x 2 4 x 4 x 2 5 x 4 x 5 x 4 Equations ( 32) and ( 33) taken together may be written in matrix form as : x 3 4x 2 5 x 3 4x 5 x 3 4 x 2 4x 2 5 ( ) x 2 4x 5 Σ 0 x 2 0 Σ 4 x 4 x 2 = 0 (34) 5 x 4 x 5 x 4 x 2 5 x 5 1 Equation ( 34) constitutes a set of 12 linearly independent equations in the 12 terms x 3 4x 2 5, x 3 4x 5, x 3 4, x 2 4x 2 5, x 2 4x 5, x 2 4, x 4 x 2 5, x 4 x 5, x 4, x 2 5, x 5, 1. This is clearly an overconstrained linear system. In order for this system to have a nontrivial solution, the coefficient matrix must be singular. The determinant 17

18 of the coefficient matrix is a 16 th degree polynomial in x 3. The roots of this polynomial give the values of x 3 corresponding to the 16 solutions of the inverse kinematics problem. For each value of x 3 thus obtained, the corresponding value of θ 3 may be computed using the formula θ 3 = 2tan 1 x 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). This equation may then be written in the form (Γ) x 2 5 x 5 1 = 0 (35) where Γ is a 6 3 matrix whose entries are quadratic polynomials in x 4. Any 3 scalar equations in equation ( 35) constitute a set of 3 linear equations in the terms x 2 5, x 5, 1. In order for such a system to have a non-trivial solution the 3 3 coefficient matrix must be singular. Its determinant is a 6 th degree polynomial in x 4. The roots of this polynomial will yield upto 6 candidate values of θ 4. Only real roots roots are candidate values. Two candidate values of θ 5 corresponding to each candidate value of θ 4 may be computed by inserting the numerical value of θ 4 in the first scalar equation in equation ( 35) and solving the resulting quadratic in x 5. At the end of these operations, we could have as many as 12 candidate (θ 4, θ 5 ) pairs. We retain 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 scalar equations in equation ( 26). The resulting equations may be solved for s 1 and c 1. 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 s 2 and c 2. Solving for s 2 and c 2, we may compute a unique corresponding value of θ 2. 18

19 Finally, substitution of the candidate (θ 1, θ 2, θ 3, θ 4, θ 5 ) tuples into the (1,1) and (2,1) elements of the following equation, A 6 = A 1 5 A 1 4 A 1 3 A 1 2 A 1 1 A hand (36) gives 2 linear equations in s 6 and c 6, which may be solved to determine a unique corresponding value of θ 6. At the end of the above operations, we will have several candidate solution tuples (θ 1, θ 2, θ 3, θ 4, θ 5, θ 6 ). Only one of these will satisfy all scalar equations in equation ( 3), and is the true solution. There may be as many as 16 such true tuples for a given A hand. In practice we find fewer than 16 real tuples for most cases. In the next section, this procedure for computing the solutions to the inverse kinematics problem is illustrated by means of a numerical example. 6 Numerical Example Let the link parameters for a 6R manipulator be : a i d i α i (deg) Let the hand location at the goal position be : A hand =

20 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 = E 002s 3 ζ 1,2 = c ζ 1,3 = E 002c E 002 ζ 1,4 = s 3 ζ 1,5 = s c ζ 1,6 = s c ζ 1,7 = c ζ 1,8 = ζ 1,9 = s c ζ 2,1 = E 002c ζ 2,2 = E 002s 3 ζ 2,3 = s 3 ζ 2,4 = E 002c E 002 ζ 2,5 = s 3 ζ 2,6 = c ζ 2,7 = ζ 2,8 = E 002c ζ 2,9 = E 002c ζ 3,1 = s c E 002 ζ 3,2 = s c ζ 3,3 = s c ζ 3,4 = s c ζ 3,5 = s c ζ 3,6 = s c ζ 3,7 = s c

21 ζ 3,8 = ζ 3,9 = s c ζ 4,1 = s c ζ 4,2 = E 002s E 002c E 002 ζ 4,3 = s c E 002 ζ 4,4 = E 002s E 002c E 002 ζ 4,5 = s c ζ 4,6 = s c ζ 4,7 = ζ 4,8 = s c ζ 4,9 = s c ζ 5,1 = s c ζ 5,2 = s c ζ 5,3 = s c ζ 5,4 = s E 002c ζ 5,5 = s c ζ 5,6 = s c ζ 5,7 = c ζ 5,8 = E 002s c ζ 5,9 = E 002s c ζ 6,1 = s c ζ 6,2 = s c ζ 6,3 = s c ζ 6,4 = s c ζ 6,5 = s c ζ 6,6 = s c ζ 6,7 = s c ζ 6,8 = s c ζ 6,9 = s c

22 σ = E E Equation ( 28) and ( 29) are respectively, and (3.6518E 002s E 002c )s 4 s 5 + ( s c )s 4 c 5 + (0.1950s c E 002)c 4 s 5 + (0.4357s c E 002)c 4 c 5 + (0.4444s c )s 4 + ( s c )c 4 + ( E 002s c )s 5 + (0.1161s c )c 5 + ( s c ) = 0, ( s E 002c )s 4 s 5 + ( s c E 002)s 4 c 5 + (2.8118E 002s c E 003)c 4 s 5 + ( s c E 002)c 4 c 5 + (1.1322s c )s 4 + (0.5675s c )c 4 + (0.3335s E 002c E 002)s 5 + ( s E 002c )c 5 + ( s c E 002) = 0 The 16 th degree( polynomial ) in x 3 obtained from the determinant of the coefficient matrix Σ 0 0 Σ in equation ( 34) is : x x x 14 22

23 x x x x x x x x x x = 0 The roots of this polynomial are : ± ± ± ± ± ± ± 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 goal position. The values of θ 3 corresponding to x 3 = , and x 3 = , are respectively deg and deg. For θ 3 = deg, we now demonstrate the procedure for computing the other joint variables. Substituting this value of θ 3 in equation ( 31) we get the following equation : E

24 E The matrix Γ of equation ( 35) is : x 2 4x 2 5 x 2 4x 5 x 2 4 x 4 x 2 5 x 4 x 5 x 4 x 2 5 x x x x x x x x x x x E 002x x = 0 (37) x x x x x x E x x x x x x x x x x x x x x x x x x We may treat the first 3 rows of equation ( 35) as a system of linear equations in the terms x 2 5, x 5, 1. For a non-trivial solution to exist, the 3 3 coefficient matrix must be singular. Its determinant is : x x x x x x = 0 The roots of this polynomial are : ±0.4201i , , , ±6.8084i The values of θ 4 corresponding to the real roots are : x 4 = θ 4 = deg 24

25 x 4 = θ 4 = deg Substituting each of the candidate x 4 values into the first scalar equation in equation ( 37) gives a quadratic equation in x 5, the roots of which may be used to compute the corresponding values of θ 5. For x 4 = , the first scalar equation in equation( 37) becomes : x x = 0 The roots of this equation are ±1.2303i Clearly θ 4 = deg is no longer a candidate. For x 4 = , the corresponding quadratic equation in x 5 is : x x = 0 The roots of this equation are and The corresponding values of θ 5 are respectively deg and deg. At the end of these operations, we have the following candidate (θ 4, θ 5 ) pairs : ( , ), ( , ) 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 ) tuple is ( , , ). Substituting these values for θ 3, θ 4 and θ 5 in the first 2 scalar equations in equation ( 26) gives the following 2 equations : Solving we get : s c 1 = s 1 (1.8978E 002)c 1 = E 002 s 1 = , c 1 = , θ 1 = deg Our candidate solution tuple becomes (θ 1, θ 3, θ 4, θ 5 ) = ( , , , ). 25

26 Substituting these values of θ 1, θ 3, θ 4 and θ 5 in equation ( 10) and ( 11) gives : s c 2 = Solving, we get : s c 2 = s 2 = , c 2 = , θ 2 = deg Finally, θ 6 may be computed from the following matrix equation : A 6 = A 1 5 A 1 4 A 1 3 A 1 2 A 1 1 A hand 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 2 scalar equations : s 6 = c 6 = The value of θ 6 satisfying the above 2 relations is deg. Thus one solution to the inverse kinematics problem is the tuple ( , , , , , ). The solution tuple corresponding to the other real root (x 3 = ) of the 16 th degree polynomial in x 3, is ( , , , , , ) 7 Conclusion In this paper, a new solution procedure for the inverse kinematics problem of the 6R manipulator of general geometry, was presented. This procedure involved a sequential elimination of variables, resulting in a univariate polynomial 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 of the 6R manipulator were presented. The appearance of extraneous roots, which is common in sequential elimination methods was avoided by exploiting these properties during the elimination procedure. 26

27 8 Acknowledgement Prof. Bernard Roth gratefully acknowledges the financial support of the System Development Foundation and Control Data Corporation. 9 References ALBALA and ANGELES[1979 ] Albala, H., and Angeles, J., Numerical Solution to the Input-Output Displacement Equation of the General 7R Spatial Mechanism, Proceedings of the Fifth World Congress on Theory of Machines and Mechanisms, pp DUFFY and CRANE[1980 ] Duffy, J., and Crane, C., A Displacement Analysis of the General Spatial 7R Mechanism, Mechanisms and Machine Theory, Vol. 15, pp KLEIN[1935 ] Klein, F., Elementary Mathematics from an Advanced Standpoint, Translated 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 of Spatial Mechanisms, Mechanisms and Machine Theory, Vol. 23, No. 3, pp LEE and LIANG[1988b ] Lee H-Y., and Liang, G-G., Displacement Analysis of the General Spatial 7-Link 7R Mechanism, ibid, pp 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

28 Controlled Manipulators, On the Theory and Practice of Robots and Manipulators, Vol. 1, First CISM-IFToMM Symposium, September, pp SALMON[1885 ] Salmon, G., Lessons Introductory to the Modern Higher Algebra, Chelsea Publishing Co., New York. TSAI and MORGAN[1985 ] Tsai, L-W., and Morgan, A., Solving the Kinematics of the Most General Six- and Five-Degree-of-Freedom Manipulators by Continuation Methods, Transactions of the ASME, Journal of Mechanisms, Transmissions, and Automation in Design, Vol. 107, June, pp