Repeatable joint displacement generation for serial redundant robotic systems

MISSING IMAGE

Material Information

Title:
Repeatable joint displacement generation for serial redundant robotic systems
Physical Description:
vi, 129 leaves : ill. ; 29 cm.
Language:
English
Creator:
Chung, Yong Soeb, 1956-
Publication Date:

Subjects

Subjects / Keywords:
Mechanical Engineering thesis Ph. D
Dissertations, Academic -- Mechanical Engineering -- UF
Genre:
bibliography   ( marcgt )
non-fiction   ( marcgt )

Notes

Thesis:
Thesis (Ph. D.)--University of Florida, 1992.
Bibliography:
Includes bibliographical references (leaves 125-128).
Statement of Responsibility:
by Yong Soeb Chung.
General Note:
Typescript.
General Note:
Vita.

Record Information

Source Institution:
University of Florida
Rights Management:
All applicable rights reserved by the source institution and holding location.
Resource Identifier:
aleph - 001789095
oclc - 28951371
notis - AJL2743
System ID:
AA00003691:00001

Full Text

/


REPEATABLE JOINT DISPLACEMENT GENERATION
FOR SERIAL REDUNDANT ROBOTIC SYSTEMS



















BY
YONG SOEB CHUNG












A DISSERTATION PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
DOCTOR OF PHILOSOPHY

UNIVERSITY OF FLORIDA


1992












ACKNOWLEDGEMENTS


The author wishes to express his sincere appreciation to his committee chairman,

Dr. Joseph Duffy, for his encouragement and guidance. He is also grateful to the

committee members, Dr. Carl D. Crane, Dr. Gary K. Matthew, Dr. Ralph G.

Selfridge, Dr. Neil L. White, who showed great interest. He also would like to thank

his fellow students in the Center for Intelligent Machines and Robotics (CIMAR) for

sharing their knowledge. Also special thanks go to Dr. Michael W. Griffis,

postdoctoral associate, for his guidance.

Finally, the author extends his deepest appreciation to his family for their

financial and moral support.













TABLE OF CONTENTS

Page

ACKNOWLEDGEMENTS..................................... ii

ABSTRACT ............................................... v

CHAPTERS

1 INTRODUCTION ....... ......... ........................ 1

2 ANALYSIS OF SERIAL REDUNDANT MANIPULATORS ........ 4

2.1 Introduction to Plicker Line Coordinate ..................... 4
2.2 Development of the New Kinematic Control Strategy ........... 7
2.3 Relationship of New Control Strategy to Previous Control Strategies .. 15
2.4 Discussion of Repeatability Based Upon the Lie Bracket Condition .. 17

3 REPEATABILITY OF PLANAR SERIAL REDUNDANT
MANIPULATORS ........................................ 22

3.1 Planar 3R Manipulator ................................. 22
3.1.1 Derivation of Inverse Kinematic Equation ................. 22
3.1.2 Numerical Verification and Simulation of Repeatability ....... 30
3.2 Planar PRR Manipulator ................................ 41
3.2.1 Derivation of Inverse Kinematic Equation ................. 41
3.2.2 Numerical Verification and Simulation of Repeatability ....... 41
3.3 Planar 4R Manipulator ................................. 48
3.3.1 Derivation of Inverse Kinematic Equation ................. 48
3.3.2 Numerical Verification and Simulation of Repeatability ....... 51
3.3.3 Implementation ............................... ..... 52

4 REPEATABILITY OF A SPATIAL 7R MANIPULATOR .......... 57

4.1 Notation ............................................ 57
4.2 Mechanism Dimensions of 7R Manipulator ................... 57
4.3 Specification of Position and Orientation .................... 61
4.4 Derivation of Forward Kinematic Equation .................. 64








4.5 Derivation of Jacobian Matrix ............................. 68
4.6 Derivation of Inverse Kinematic Equation .................... 72
4.7 Numerical Verification and Simulation of Repeatability .......... 75

5 CONCLUSION .......................................... 80

APPENDICES

A DERIVATION OF INVERSE KINEMATIC EQUATION OF
PRR MANIPULATOR .................................. 81

B DERIVATION OF INVERSE KINEMATIC EQUATION OF
4R MANIPULATOR .................................... 87

C DERIVATION OF FORWARD KINEMATIC EQUATION OF
7R MANIPULATOR (SSRMS) ............................ 94

D DERIVATION OF JACOBIAN MATRIX OF
7R MANIPULATOR .................................... 100

E DERIVATION OF DERIVATIVE JACOBIAN MATRIX OF
7R MANIPULATOR .................................... 111

REFERENCES ............................................. 123

SUPPLEMENTARY BIBLIOGRAPHY ........................... 125

BIOGRAPHICAL SKETCH .................................... 129












Abstract of Dissertation Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Doctor of Philosophy

REPEATABLE JOINT DISPLACEMENT GENERATION
FOR SERIAL REDUNDANT ROBOTIC SYSTEMS

By

Yong Soeb Chung

December 1992

Chairman: Dr. Joseph Duffy
Major Department: Mechanical Engineering

This thesis presents a novel, practical, and theoretically sound kinematic control

strategy for serial redundant manipulators. This strategy yields repeatability in the

joint space of a serial redundant manipulator whose end effector undergoes some

general cyclic type motion. This is accomplished by deriving a new inverse kinematic

equation that is based on springs being theoretically or conceptually located in the

joints of the manipulator (torsional springs for revolute joints, translational springs

for prismatic joints).

Previous researchers have also derived an inverse kinematic equation for serial

redundant manipulators. However, to the author's knowledge, the new strategy is the

first to include the free angles of torsional springs and the free lengths of

translational springs. This is important because it ensures the repeatability in the

joint space of a serial redundant manipulator whose end effector undergoes a cyclic







type motion. Numerical verification for repeatability is done in terms of Lie bracket

condition.

Choices for the free angle and torsional stiffness of a joint (or the free length and

translational stiffness) are made based upon the mechanical limits of the joint. For

instance, the free angle of a joint is that angle which is midway between joint limits.

Joint stiffnesses are chosen so that the most dexterous joint is the most pliable, and

so that the least dexterous joint is the stiffest. This strategy helps to keep the joints

of the manipulator away from their respective joint limits.













CHAPTER 1
INTRODUCTION



There is an increased interest in using manipulators with seven or more degrees

of freedom because of the dexterity limitation of current six degree of freedom

manipulators. For example, the limited ranges of motion of the joints and torque

limits on motors restrict the movement of a six degree of freedom manipulator.

Because of these considerations, it is desirable for a general purpose robotic

manipulator to have more than six degrees of freedom.

Redundant manipulators have more degrees of freedom than is required for a

task. For example, a seven degree of freedom manipulator is redundant for the task

of positioning and orienting the end effector (three degrees of freedom are required

for position and three are required for orientation). The extra degrees) of freedom

of a redundant manipulator may be used to satisfy many supplementary criteria such

as keeping the joint variables within their physical limitations, providing greater

dexterity, placing the joint torques close to the midpoint of joint torque limits, and

minimizing kinetic energy, singularity avoidance, obstacle avoidance, etc.

In most work on serial redundant manipulators, the forward velocity equations

are frequently written in a Jacobian form, and the Moore-Penrose pseudo-inverse of

the Jacobian is used to give the minimum Euclidean norm solution [1]. The use of







2

this procedure gives a solution, subject to the condition of minimum norm, in cases

where all the joints are either revolute or prismatic, or, indeed, for the case of robots

where the joints are all of the same type. However, the use of the Moore-Penrose

pseudo-inverse has much more serious consequences when the serial redundant

manipulator consists of a mix of joint types. If there are both revolute and prismatic

joints present, for example, minimizing the Euclidean norm lacks formal meaning,

and the result so obtained is heavily dependent on the unit of length used to describe

the robot structure.

Indeed, it is also possible to arrive at other solutions by superposing other

allowable motions of the manipulator that do not entail end effector motion of any

kind in addition to the Moore-Penrose pseudo-inverse solution [2].

Whitney [3] applied the weighted pseudo-inverse solution to the inverse kinematic

problem for redundant manipulators. This differs from the Moore-Penrose solution

in that the sum of squares of actuator velocities is a weighted sum (a,68: + a26eo

+ ... + a, e,, where a1, a2, ..., a, are positive numbers chosen by the user). The

Moore-Penrose pseudo-inverse control or the weighted pseudo-inverse control may

drive the robot into an undesired manipulator configuration. Using these techniques,

the manipulator configurations themselves are not reproduced when an end effector

performs a cyclic motion (the joint displacements are not cyclic). Several researchers

have identified the repeatability problem of redundant manipulators [1,4,5].

This thesis describes a general pseudo-inverse solution to the inverse kinematic

problem for redundant manipulators. This solution is one that determines a suitable







3

set of actuator velocities (or increments) which minimize a general quadratic

function: a,,6E + a,6e2 + ... + a682 + a26eo,62 + a2626e3 + ...

+ a(,1),~6 .-e,. The coefficients aN, a, are not constants (they change as the

manipulator moves). In order to specify a,, ai (or [A] from the matrix expression, 6ae

[A] S6), this work bases its foundation on a pseudo-elastic potential energy function.

In addition to actuator stiffnesses (torsional or translational), this new function also

includes free angles of torsional springs and/or free lengths of translational springs.

This formulation yields a control strategy that facilitates repeatability in the joint

space of a manipulator whose end effector undergoes a cyclic type motion.













CHAPTER 2
ANALYSIS OF SERIAL REDUNDANT MANIPULATORS



This chapter contains a theoretical development and discussion of the new

control strategy.

2.1 Introduction to Pliicker Line Coordinate

Because lines (screws) will be used in subsequent sections, a review of line

coordinates is presented here.

Two distinct points, 1(x,,y,,z,) and r(x2,y2,z2), determine a line as shown in Fig. 2-

1. The vector S can be expressed in the form

S = Li + Mj + Nk, (2.1)

where

L = x2-x,, M = y,-yl, N = z2-z, (2.2)

are defined as the direction ratios. The direction ratios (L,M,N) are related to the

distance between the two points by

L2 + M2 + N2 = IS2. (2.3)

Let r represent a vector to any arbitrary point on the line. The vector (r-rE) will

be parallel to S and therefore the equation of the line may be written as

(r-r) x S = 0. (2.4)

This equation can be expressed in the form







5

rx S = S, (2.5)

where

S= 1 x S, (2.6)

is the moment of the line about the origin and is origin dependent. The vectors

(S;S) which must satisfy the orthogonality condition

S-So = 0, (2.7)

are the Pliicker coordinates of the line [6,7]. (S;&) are homogeneous coordinates

since from eq. (2.5) the coordinates (kS;k%) where k is a non-zero scalar determine

the same line. The vectors (S;S,) may be written in terms of their components as

(L,M,N;P,Q,R).


2-1: Development of Plucker Line Coordinates







6
A wrench on a screw will be represented as a general "ray" that is assigned an R6

vector w = [f: .] where f is a force vector in the direction of the wrench and m0

is a moment vector referenced to the origin. Ray coordinates for screws (direction

vector first) are assigned lower case labels. This ordering of screw coordinates is

based on Plicker's definition of a ray, which is a line formed from the join of two

points. However, a twist on a screw will be represented using two different formats,

one based on the ray, a = [Sr; 6S], and the other based on a general "axis" that is

assigned an R6 vector, D = [6; SR]. Axis coordinates for screws (direction vector

last) are assigned upper case labels. Plicker defined the axis to be dual to the ray,

and axis is thus a line dually formed from the meet of two planes.

For a twist, 61 and S6 both represent the direction of an infinitesimal rotation,

and Sr and SR both represent an infinitesimal displacement of the origin. Therefore,

the axis and ray representations of the same twist are related by

[f 1 [03 13 ] ]R
aJ oz I[ I'

6Sr 13 03 .

which can be more conveniently written as

d = [A]D, (2.8)

where [A] is the symmetric 6 x 6 matrix above. Since [A]1' = [A], a reversal in the

ordering of this same transformation yields

D = [A]a. (2.9)

The transformation described by eq. (2.8) and eq. (2.9) is an example of a more







7

general transformation, defined as a correlation, that maps an axis to a ray (or a ray

to an axis). It is important to distinguish this transformation from a collineation,

which maps a ray to a ray (or an axis to an axis).

2.2 Development of the New Kinematic Control Strategy

We consider an n degree of freedom manipulator illustrated in Fig. (2-2), with

joint coordinates e,, i = 1, 2,..., n, and a task described by m task coordinates rj

,j = 1, 2,..., m ( m 5 n ). Let the kinematic transformation from the joint space

to the task space be given by

r = f(e), (2.10)

where 9 = [el, e2, e and I = [r,, r2, rJf are the joint and task

coordinate vectors respectively. The superscript T denotes the transpose. Eq. (2.10)

will be assumed to be continuous and differentiable up to the second order in the

entire task space. Differentiating eq. (2.10) with respect to time yields

= J(_)_ (2.11)

where t = dr/dt E R" (m-dimensional Euclidean space), e = do/dt e R", and

J(9) = af(_)/3a e R" (the set of all m x n real matrices). The matrix J(a) is called

the Jacobian. From eq. (2.11), we see that the Jacobian is simply a linear

transformation that maps the joint velocity in R" into the task velocity in R".

The end effector is required to undergo an infinitesimal twist relative to ground.

Then an alternative expression of eq. (2.11) for the forward instantaneous motion can

be written in the form






8








$5

k




S $N k3









$21

Fig. 2-2 : Kinematic Model of n-Jointed Serial Manipulator










A Bn
D = z 6eiS,, (2.12)

where D is the axis coordinates of the twist, line $i is assigned axis coordinates Si,

and where S6e is an infinitesimal rotation of the i'h revolute axis. This can be more

conveniently written as

D = [J] e, (2.13)

where [J] is the m x n Jacobian matrix whose column is Si. It is observed that [J] can

be obtained by eq. (2.11) or eq. (2.13).

While the objective of this work is to develop a meaningful algorithm that effects

the incremental displacement control of the end effector of a serial redundant

manipulator, it is now assumed that at. each revolute joint there exists a torsional

spring which deflects due to the rotation of the joint. In general, the torsional spring

can be thought of as deflected away from its free position angle, and it is thus

initially loaded with a finite torque, 7, at any general configuration. Consequently, the

stiffness elements can be considered at each instant to be loaded and not to be at

their free positions. Then this system of springs would be in static equilibrium, with

an appropriate external resultant wrench providing a load necessary to balance the

finite torsional spring deflections. In an infinitesimal movement, the i'h torsional

spring is considered to have linear characteristics, and ri in the i'1 spring torque is

related to a joint rotation ei eoi by

r, = k,(e9 e~), (2.14)

where ei e0, is the difference between the current and the free position joint angle







10

and k, is the non zero i"h joint stiffness that has dimensions (FL). The eq. (2.14) may

be more conveniently written as

8 0 = [k]^., (2.15)

where [k] is an n x n diagonal matrix of joint stiffnesses.

The joint stiffness, kl, can also be a desired-joint-displacement stiffness, which is

defined as how stiff a joint is desired to be while contributing to the generation of

a known end effector twist. The larger k, will cause less joint rotation while the

smaller ki will cause more joint rotation. Thus a proper choice of k, based on

mechanical limits of each joint helps to avoid joint limitation during the specified

path.

The joint torques, acting in each joint, combine to give a resultant wrench (w =

[f: mlo]) given in ray coordinates at the end effector, and this can be expressed for

the it joint as

ri = S,' w, (2.16)

which can be more conveniently written in the form for all joints,

r = [J]T w, (2.17)

where r is the n-vector of the joint torques. Combining the stiffness relationships in

eq. (2.15) with eq. (2.17) gives the vector of joint motions

a o = [k]-[J]T w. (2.18)

Multiplying eq. (2.18) by [J] and rearranging eq. (2.18) yields

S= ([J][k]-[J]T) [J] ( -). (2.19)

This can be solved provided that [J] is of full rank. Kinematic singularities occur







11
when [J] loses rank. Such kinematic singularities are intrinsic to the structure of the

mechanism. Taking the derivative eq. (2.18) yields

6s = [k]'[J]T 6w + [k]6J]T w, (2.20)

The second term of the right-hand side of eq. (2.20) goes to the left-hand side,

6 [k][6J]T w = [k]-'J] 6w. (2.21)

The second term on the left-hand side of eq. (2.21) contains 68 in [6J]T. In order to

extract 6e from [SJ] the following will be accomplished. The ith component (i =

1, 2, ..., n) of the vector [6J]T w can be expressed in the form:

([6J] v), = E ( {(o2r,6.1)/(Baeae)}f,

= r.6ei, (2.22)
j=1 Q
where

ri = E {rrJ(aeiaej)}fi.

In vector notation, eq. (2.22) becomes

[6J]T w = [r]69. (2.23)

Since, by hypothesis, eq. (2.10) is continuous and differentiable in the task space, it

can be said that

Fr,/(ae,aej) = Fr1/(aeae).

From the above equation, it follows that [r] is an n x n symmetric matrix (ru = r,).

Substituting eq. (2.23) into eq. (2.21) yields

(I [k]-[r])68 = [k]l[J] 6w. (2.24)

Letting

[T = I [k]-[r],









then eq. (2.24) becomes

[T]68 = [k].[J] 6w, (2.25)

where [T] is an n x n matrix that consists of the joint angles, e,, the free angles of

torsional springs, o0i, an external wrench, w, and spring stiffnesses, ki, i = 1, ...,n.

Eq. (2.25) can be solved provided that [T] is of full rank (otherwise an algorithmic

singularity occurs). Thus,

Se = [T]'[k]-[J]T 6w. (2.26)

Letting

[A] = [k][T],

which is an n x n symmetric matrix and premultiplying the left and right sides of eq.

(2.26) by [J] yields

[J]69 = [J][A]-[J]T w. (2.27)

Substituting eq. (2.13) into eq. (2.27) yields

D = [J][A][J]T 6w, (2.28)

which can be expressed in the form

D = [C]6w, (2.29)

where [C] = [J][A]I[J]T is an m x m symmetric compliance matrix that relates the

end effector external wrench to its corresponding twist. Provided that [T] and [J] are

both full rank, then

6w = [C]'D, (2.30)

relates the necessary deflection twist of the end effector relative to ground to the

incremental change in resultant wrench.







13
In the problem of the inverse kinematics of the serial redundant manipulator, it

is necessary to determine the joint increments explicitly. The elastic compliance

relation of eq. (2.28) can be used to accomplish this objective. Substituting eq. (2.30)

into eq. (2.26) yields the general pseudo-inverse equation:

s = [A]-1[J]T [J][A]-[J]T}-1

= [A]-[J](C)-1D, (2.31)

Application of the general pseudo-inverse solution depends upon the knowledge

of [A]. Because [A] is manipulator configuration dependent, it must be calculated

every time eq. (2.31) is to be solved.

Eq. (2.31) can be also derived by using the Lagrangian multiplier method.

THEOREM 2.1. If J is an m x n matrix (m < n) of rank m, then the solution of the

equation D = [J]69 that minimizes 6ET[A]6e is given by

5s = [A]-'T[J] I[A]1[J]r *TD. (2.32)

Proof: We consider the method of Lagrange multipliers. The quantity D that

minimizes 69T[A] 6 subject to D = [J]68 can be found by minimizing

M = 6eT[A]6- + 21T(D [J]6_),

where I is an m x 1 column vector of Lagrange multipliers, and the factor of 2 is

inserted for convenience. Setting

aM/a6 e = 0, j = 1,..., n, M/lal, = 0, i = 1,..., m,

we find

[A]69 = [J]'T or 6S = [A]-[J]f. (2.33)

D = [J]6,. (2.34)









Substituting eq. (2.33) into eq. (2.34), we obtain

D = [J][A].'[J]T or A = ([J][A]'[JT)f),^, (2.35)

Substituting eq. (2.35) for 1 into (2.33) gives (2.32).

This solution, eq. (2.31), ensures the repeatability in the joint space of a serial

redundant manipulator whose end effector undergoes a cyclic type motion.

Taking the derivative of eq. (2.15) yields

S6 = [k]-1r. (2.36)

A further extension of this analysis can be achieved by combining eq. (2.13) and eq.

(2.36) with eq. (2.30), to obtain

6w = [C]-1 [J][k]-1 6., (2.37)

which is the forward static relationship for the serial redundant manipulator. Eq.

(2.37) describes an onto mapping of joint torque increments into wrench increments.

From the above theorem 2.1, the general pseudo-inverse equation minimizes

6eT[k][T]Se. It is important to recognize that potential energy is not being

minimized, i.e., pe = z 0.5ki(ei eoi)2. Furthermore, it will be now demonstrated that
i-1
the second derivative of potential energy, which is denoted by S'pe, is also not being

minimized. Now the relationship between 6eT[k][T]Se and the second derivative

potential energy function will be derived. The second derivative potential energy can

be written in the form

62pe = 6(fT) ),

= 6DTw + D 6w (2.38)

Premultiplying the left and right sides of eq. (2.25) by 6ST[k] yields











6eT[k][T]Se = 69a[J]T6V

= D' 6w (2.39)

Substituting eq. (2.39) into eq. (2.38) and rearranging eq. (2.38) yields

6a'[k][T]_E = 6pe 6DT ^ (2.40)

Because potential energy is not being minimized, the use of the control strategy

does not require at any time the robot to be in a minimum (or maximum) potential

energy state, i.e., where 6pe = 0. In fact the robot whose end effector undergoes

some general cyclic-type motion can begin in different initial configurations (different

initial joint angles). A theoretical proof of repeatability is given (section 2.4)

following a brief discussion of other control strategies.

2.3 Relationship of New Control Strategy to Previous Control Strategies

In this section, it is assumed that the manipulator system is initially stationary and

unloaded. Consequently, the stiffness elements can be considered at each instant to

be unloaded or at their free positions, i.e., e, = 60 and from eq. (2.19) w = 0.

Therefore from eq. (2.20)

e = [k]J] jw. (2.41)

Premultiplying the left and right sides of eq. (2.41) by [J] yields

[J]68 = [J][k]-[Jf 6w. (2.42)

Substituting eq. (2.13) into eq. (2.42) yields

D = [J][k][J] r aw, (2.43)

which can be expressed in the form











D = [C]6w, (2.44)

where [C] = [J][k]-[J] is an m x m symmetric compliance matrix that relates the end

effector external wrench to its corresponding twist. Provided that [J] is of full rank,

thus

6w = [C]-f), (2.45)

relates the necessary deflection twist of the end effector relative to ground to the

incremental change in the resultant wrench.

In the problem of the inverse kinematics of the serial redundant manipulator, it

is necessary to determine the joint increments explicitly. The elastic compliance

relation of eq. (2.41) can be used to accomplish this objective. Substituting eq. (2.45)

into eq. (2.41) yields the weighted pseudo-inverse equation:

6_ = [k]l[J]T([J][k][J]T)-l),

= [k]-[J]T(C)-'D. (2.46)

The result, which is the weighted pseudo-inverse of [J], represents a minimization of

the second derivative of elastic potential energy function 62pe:

62pe = 6Se [k] Se. (2.47)

The work here agrees with the established work of Whitney [3]. It is recognized that

eq. (2.46) does not ensure repeatability of cyclic motion of the end effector.

The Moore-Penrose pseudo-inverse solution can also be obtained from the eq.

(2.31) by disregarding the free angles and the joint stiffnesses. Thus, both [k] and [T]

are identity matrices and from eq. (2.31),











e = [J]T([J][J]T)-D. (2.48)

Eq. (2.48) is the same as that of Noble [8]. The result, which is the Moore-Penrose

pseudo-inverse of [J], represents a minimization of eT6'6. It is very important to

note that eq. (2.48) does not ensure repeatability of the cyclic motion of the end

effector. Furthermore it is not invariant with change of unit of length if there are

both revolute and prismatic joints present.

2.4 Discussion of Repeatability Based Upon the Lie Bracket Condition

Shamir and Yomdin [5] introduce a repeatability condition of redundant

manipulators. In order to explain the repeatability condition, they introduce 7

definitions and 4 theorems. In this section the definitions and the theorems are

discussed briefly.

DEFINITION 2.1: An n x m matrix function P = P(9) such that [J][P] = I, will be

called a local control strategy.

DEFINITION 2.2: Let x = x(t)(t E [0,1]) be a continuous curve in the task space. If

a = a(t) is a continuous curve in the joint space such that f(a(t)) = x(t), then a is

called a lifting of x.

DEFINITION 2.3: Let r0 be a point in the task space W. The set

f-(r0) = {e f(4) = r0}

will be called the fiber over r0.

DEFINITION 2.4: A control strategy P is repeatable over U (open subset of W) if

for any closed path x in U, every lifting of x determined by P is a closed path a in









f'(U), and a is never tangent to any fiber.

DEFINITION 2.5: A control P is repeatable at pe c if for any closed path x in U

that passed through f(op), the lifting determined by P and passing through p, is a

closed curve in c.

DEFINITION 2.6 [9, p. 88;10, p. 123]: An m-dimensional distribution D on a set A

in R" is a law that assigns to each point e A, a unique linear m-dimensional

subspace D,(of R"), with origin at e. It is sometimes also called an m-vector field

[11, p. 145].

DEFINITION 2.7: An integral surface S for an m-dimensional distribution D is a

smooth m-dimensional surface S c R", such that at each point e E S, the tangent

space to S is exactly the linear m-dimensional space D. assigned by the distribution.

THEOREM 2.2: Let P be our control matrix, which we assume to be a smooth

function in an open subset i of the joint space. Let U e f(t) be a simply_connected

region of the task space. Then the control is repeatable for any closed paths lying in

U, if and only if the following condition (LBC) holds.

LBC: For any two columns of Pi and Pj of P, their Lie Bracket [P,, P,] is a linear

combination of the columns of P.

The Lie bracket of two vectors P, and Pj, that are both functions of e is the

vector

[Pi, Pj] = (aPj/ae)P, (aP/ae)P,

where aP/~d and aP,/3e are the n x n matrix of partial derivatives.

To determine whether this is a linear combination of the columns, consider the








19
extended n x (m+1) matrix (P, [Pi, Pj]) and check if its (m+1) x (m+1) minors are

zero.

THEOREM 2.3 [9,10]: Let D be an m-dimensional distribution. Then there exists an

integral surface for D, containing a point 9, if and only if D is involutive in an open

neighborhood of e.

THEOREM 2.4: Let D be a distribution associated with a local control strategy P.

Let U be an open subset of W, and let op E o be a point such that f(ep) e U. Then

P is repeatable at over U, if and only if there exists an integral surface S

containing op, such that the restriction of f to S is a homeomorphism of S onto U.

THEOREM 2.5: Let D be a distribution associated with the control strategy P, and

let S be an integral surface for D, over a simply connected underlying set U c W.

Then S intersects each fiber at no more than one point.

It is well known from differential geometry (for example, [8]) that if S c R" is any

smooth m-dimensional surface, and u, v are any two tangent vectors to S at a point

e, then their Lie bracket [u, v] is also a tangent vector to S at e.

A distribution D is called involutive at if, for any two vectors u, v that belong

to a subspace D,(u and v are both evaluated at e), their Lie bracket [u, v] also

belongs to the subspace D,.

From theorem 2.3 and theorem 2.4, it is clear why repeatability implies the Lie

bracket condition (LBC) stated in theorem 2.2. If our control P is repeatable, then

there exists an integral surface for the distribution. Such a surface exists if and only

if LBC holds.







20

It is important to note that if the LBC holds at every point, then there is a

foliation of integral surfaces and the control is repeatable for any initial

configuration. If integral surfaces do exist for a given distribution, then they must

either be disjoint or coincide, because at each point, the distribution assigns a unique

tangent space. In other words, if the distribution is involutive at every point _e E ,

then we obtain a family of disjoint surfaces (called a foliation) which covers .

From the above definition 2.1, an inverse kinematic equation can be expressed

in the form

60 = P(e) .

For example, the general pseudo-inverse is given by

P,(a) = ([k][T])'[J{ {[J]([k][T])-'[J]T}',

the weighted pseudo-inverse is given by

w(a) = [k]-[J]T([J][k]'[J]')-,

and the Moore-Penrose pseudo-inverse is given by

P.p(e) = [J]T([J][J]'.

If we consider one degree of redundancy (n-m = 1), the Lie bracket condition

(LBC) can be expressed in the form

LBC = IP [P, P]I = 0.

Suppose that m = 3 and n = 4, then

LBC = IP[PI, P211 = IP[P,P 311 = IP[P3, P11 =0.

The computations involving the Lie bracket condition are very difficult to do by

hand, but they can be done with the use of Maple, Mathematica or another symbolic
6








21

software package.

Even the simplest case (a planar 3R manipulator) requires a huge amount of

memory in order to obtain a symbolic expression of the Lie bracket condition. In this

dissertation a numerical verification for repeatability is done in terms of the Lie

bracket condition.













CHAPTER 3
REPEATABILITY OF PLANAR SERIAL REDUNDANT MANIPULATORS



The proposed general pseudo-inverse control in chapter 2 is applied to planar

manipulators (3R, PRR, 4R).

3.1 Planar 3R Manipulator

Consider a 3 degree of freedom manipulator which moves in the (x,y) plane.

When only the position of the end effector is of concern, this manipulator is

kinematically redundant. The kinematic model of a planar 3R manipulator is shown

in Fig. 3-1.

3.1.1 Derivation of Inverse Kinematic Equation

The coordinates for a point P (see Fig. 3-1) in the end effector are given as

x = ac, + a23c1+2 + ac+2,3, (3.1)

y = aus1 + a2,s+2 + a4rs12+3,

where a2, a2, aM represent the length of each link, while the variables with subscript

are defined as

si = sin(e.), si+,j...+n = sin(,+ j+...+ )

c, = cos(e,), c,++...+ = cos(ei+ej+...+e.).

Eq. (3.1) for an instantaneous motion can be expressed as














P: Position(x, y) of end effector


a : Orientation


Fig. 3-1: Kinematic Model of a Planar 3R Manipulator










6x 61 e
= [J] 62 (3.2)
6y 6- E3


The Jacobian matrix is obtained as


-a2s,-a23s, 2-a34s +2+3 -a23s+22-a34sl+2+3 -a34S1+2+3
[J] = (3.3)
L.a2c+ a23c1+2+ a34c1+2+3 a23c1+2 a34cl+2+3 a34 +2+3

The joint torques acting in each joint combine to give resultant forces at the end

effector, and these can be expressed as



r, = [J] T (3.4)



Joint torques are related to joint rotations by


r1 ]1 01o
T2 = [k] e-2 (3.5)
T3 e 03

where [k] is a 3x3 diagonal matrix of joint stiffnesses as follows


ki 0 0
[k] = k2 0 ,
O 0 k3

k, is the nonzero ith joint stiffness that has dimensions (FL) and ei o,i is the








difference between the current and the free position joint angle, i = 1, 2, 3.

Thus eq. (3.5) can be written in the form


Substituting eq.


[


e1 001 T1
2 -o2 = [k]1 2
e3 03 3

(3.4) into eq. (3.6) yields

,0 80o
2 02 = [k]-[J]T
63 03


Premultiplying the left and right sides of eq. (3.7) by [J] yields

e0 01 f
[J] e2- 0e2 = [J][k]'l-J].r
3 3- 03 .-

Provided that [J] is of full rank, eq. (3.8) can be expressed in the form

f_ e1 eoi
]= ([J][k]-'[]T )-1 [J] e2 02
-3 60


Taking the derivative eq. (3.7) yields


(3.6)


(3.7)


(3.8)






(3.9)










681 [6f1
6e2 = [k]-[J]T + [k]'[6J (3.10)
6e3 LfJ [

where

6SJ S6Ju
[SJf]= 6J2 6J22 (3.11)
/L6J3 S6J32


se o 0 6e2 0 693 0
= [u] + [v] + [w]
0 692 .0 683 e. 6 9 .


ull U12
[u = U21 u ,
U31 U32


V11 V12
[V] = V21 v22,
[ V31 V32 J


Wnl W12
[W] = W2 W2 ,
S= 1 32 ),

sJnll = -a,2c6819 a23c,+2(6e+ 692) a34cl+2+3(6e+ 602+ 6e3),


6J2 = -a2s,6e, as+2(8+ 62) a34s,+2+3(698+ 682+ 63),











J21 = a23c+2(6e1+ 682) ac,34+2(691+ 682+ 683),

6J22 = a23s+2(68,+ 682) a3s42+3(6 1+ 682+ 6e3),

6J31 = a34C,23(66'+ 682+ 683),

6J32 = a34s,2,3(61+ 6e2+ 6e3),

ul= -a22c ac1+2- a34c1+2+3, 12 = 23 a34Sl+2+3

U21 = a23C1+2- a34 1+2+3 22 = a231+2 a34S1+2+3,

U31 = a34C+2+3, U32 = 34S1+2+3

VI = a23c+2 a34C1+2+3, V12 = -34s+2+3

V21= a231+2- a34c1+2+3, V22 = a34S1+2+3

V31 = a341+2+3, V32 = a34S+2+3,

W1 a34c1+2+3,12 = a2s1 a23S1+2- a34Sl+2+3,

W21= a341+2+3, W22 = -a23S1+2 a34+2+3

31 = a34c+2+3, w32 = a34s1+2+3.

The second term of the right-hand side of eq. (3.9) goes to the left-hand side,



682 [k]l[J]T = [k][J]T (3.12)

683] f- J f J

Substituting eq. (3.9) into eq. (3.12) and rearranging the left-hand side of eq.(3.12)

yields









S 1, 6f,
[T] S62 =[k]-'[J ]T
68e3 l


T1 T1 T13
[T] = T21 T2 T3
LT3 T32 T33

Tn = 1 ( unf + wfy )/kl,

T, = -( wif, + vf, )/k,,

T2 = 1- (V21f. + u22f )/k2,

T31 = -( u31f + w,2f, )/k3,

T33 = 1 ( W31f + V32L )/k3.

[T] is of full rank, eq. (3.13)


Tu = -( vjfx + uf, )/k,,

T21 = -( uA21f + w2f )/k2,

T = -( w21f + V22f )/k2,

T32 = -( Vf. + U32f )/k3,



can be expressed in the form


S68e
6e2
.693


1 [A]6f[]T
= [AJ]- [ j]
6^


Letting

[A] = [k][T],

and premultiplying the left and right sides of eq. (3.14) by [J] yields


where


(3.13)


Provided that


(3.14)









6e 6f,
[J] e:, = [J][A]`'[J]T



Substituting eq. (3.2) into eq. (3.15) yields

6x sfx
= [C]
6y sf


where


[C] = [J][A][J]T.


Provided [T] and [k] are both of full rank, eq. (3.16) can be expressed in the form


E 6f1


= [C]-1


6x

-6y


(3.17)


Substituting eq. (3.17) into eq. (3.14) yields a general pseudo-inverse equation


801
682
683
Sez .


= [A]-'[J]T( [J][A]-[]T )-'


= [A]-I[J][C]"1


6x
6y


(3.18)


6x

-6y


(3.15)


(3.16)









3.1.2 Numerical Verification and Simulation of Repeatability

For the first example, the system consists of three revolute joints as shown in Fig.

3-1 and link lengths are chosen as a. = 1.0, a3 = 1.0, and a4 = 1.0 (m). The ranges

of each joint limit are assumed as follows:

-170" e9 < 180"

-180" 5 e, < 170"

-175" 5 03 < 175"

Thus the stiffnesses for each joint are taken as being equal (ki = k2 = k3 = 1.0

Nm/radian) based on these joint limits. The free position angles of each joint are

chosen as eo0 = 5", e2 = -5*, o03 = 0*. The x-y coordinates of the four vertices of

the command path (1000 consecutive equidistant segments each side) in the task

space are successively given counterclockwise from the lower left vertex as follows:

(0.5, 0.6) (0.6, 0.6)

(0.5, 0.5) (0.6, 0.5)

where the units are meters. The end effector is to move along a predefined square

path with a constant speed of 10 mm/sec and a time step size At = 0.01 sec.

Section 2.4 demonstrated theoretically the requirement of repeatability for the

kinematic control strategy of the serial redundant manipulator. Succinctly, the joint

angles repeat for a cyclic motion of end effector only when the Lie bracket condition

holds for the kinematic control strategy. Section 2.4 also discussed how this

repeatability is independent of the initial configuration (choice of initial joint angles).

These concepts will now be validated numerically for the planar 3R manipulator.







31

Different initial configurations corresponding to the same end effector position

(x = 0.5, y = 0.5) can be obtained by assigning the end effector orientation a (see

Fig. 3-1). In the reverse analysis, two sets of solutions can be obtained for the given

position and orientation of the end effector (see Duffy [12]).

The numerical verification of the LBC is demonstrated by the orders of

magnitude difference between the LBC columns in Table 3-1 and 3-2 (twenty degree

increments for a are displayed. But the analysis was performed in one degree

increment.). Table 3-1 shows that Lie bracket condition values are near zeroes (10'

- 10") for different initial configurations when the general pseudo-inverse equation

is used. On the other hand Table 3-2 shows that numerical Lie bracket condition

values are not zeroes for different initial configurations when the Moore-Penrose

pseudo-inverse equation is used.

Three different initial configurations (see Fig. 3-2) for the same end effector

position are chosen from Table 3-1. For example, first initial configuration at

maximum potential energy (e1 = -155.7", e2 = -138.59", e3 = -65.7"), second initial

configuration at intermediate potential energy (e, = -129.060, e2 = 146.01, e3 =

63.04') and third initial configuration at minimum potential energy (e1 = -37.33",

e2 = 87.19", e3 = 110.13") are chosen. For these three different initial

configurations, Lie bracket condition values during the 10 cycle motion are shown in

Figs 3-3, 3-4, and 3-5. The initial and the final joint angles through 10 cycles of

motion of the end effector are generated in Table 3-3. At the end of the 10 cycles

the position difference (x, xtt, yfi yinitia.) and the joint angle difference (









Table 3-1

General Pseudo-Inverse Equation


a LBC
0 2.391285e-07
0 8.037865e-07
20 2.822548e-09
20 1.134500e-06
40 -1.699821e-07
40 1.845255e-08
60 2.862487e-07
60 -3.383497e-07
80 3.885419e-07
80 2.043804e-07
100 2.257927e-07
100 2.339417e-07
120 -5.409503e-08
120 1.207760e-07
140 -2.251315e-07
140 6.572133e-08
160 -4.203282e-07
160 4.425800e-08
180 -5.961847e-07
180 4.398267e-08
200 2.795830e-06
200 3.656248e-08
220 1.069061e-05
220 -1.577192e-07
240 3.336771e-07
240 1.612510e-06
260 -3.237957e-08
260 5.514746e-06
280 9.248140e-09
280 5.093987e-08
300 3.424381e-08
300 -5.399946e-07
320 6.938001e-08
320 -4.092887e-07
340 1.354084e-07
340 -1.913770e-07
360 2.414215e-07
360 8.035577e-07


X
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000


Y
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000


TH1
-155.7048
65.7048
-123.2720
83.7465
-70.4574
126.9086
-10.5431
-169.4516
16.9563
-129.0618
29.7393
-101.2229
37.7244
-77.9331
43.9929
-56.8635
49.8612
-37.3383
56.1950
-19.3265
63.8168
-3.1748
73.5563
10.5839
85.9640
21.6226
100.9853
30.2064
118.1286
37.0779
136.9206
43.0783
157.1842
49.0250
179.2173
55.9277
-155.7032
65.7036


TH2
-138.5904
138.5904
-152.9815
152.9815
-162.6340
162.6340
-158.9085
158.9085
-146.0181
146.0181
-130.9622
130.9622
-115.6575
115.6575
-100.8564
100.8564
-87.1995
87.1995
-75.5215
75.5215
-66.9916
66.9916
-62.9725
62.9725
-64.3415
64.3415
-70.7789
70.7789
-81.0507
81.0507
-93.8423
93.8423
-108.1592
108.1592
-123.2896
123.2896
-138.5932
138.5932


TH3
-65.7048
155.7048
-63.7465
143.2720
-86.9086
110.4574
-130.5483
70.5432
-150.9381
63.0437
-158.7770
70.2608
-162.0668
82.2757
-163.1364
96.0072
-162.6616
110.1389
-160.6734
123.8051
-156.8251
136.1833
-150.5837
146.4438
-141.6224
154.0361
-130.2062
159.0148
-117.0777
161.8716
-103.0781
163.0796
-89.0249
162.8160
-75.9276
160.7829
-65.7034
155.7034


PE
8.670344
6.307208
7.881616
6.648197
7.216641
7.038628
7.535189
8.601602
7.675252
5.969477
7.370122
4.511340
6.920488
3.602386
6.454121
3.080577
6.022204
2.876694
5.646737
2.935046
5.339623
3.187467
5.117468
3.547688
5.017191
3.933885
5.098199
4.306150
5.423747
4.670212
6.045742
5.047785
7.008238
5.453265
8.366526
5.883567
8.670371
6.307222










Table 3-2

Moore-Penrose Pseudo-Inverse Equation


a LBC
0 -1.664743e+00
0 3.449764e+00
20 -2.359110e+00
20 1.748641e+00
40 -8.067412e-01
40 6.070027e-01
60 -9.022816e-01
60 1.580794e+00
80 -2.840095e+00
80 2.233481e+00
100 -3.464188e+00
100 1.188109e+00
120 -3.064482e + 00
120 7.010876e-01
140 -3.166571e+00
140 5.498406e-01
160 -4.294162e+00
160 5.814914e-01
180 -7.471548e+00
180 8.518371e-01
200 -1.197545e+01
200 1.796072e+00
220 -8.342236e+00
220 5.095851e+00
240 -2.963700e + 00
240 1.142484e+01
260 -1.180420e + 00
260 9.990296e+00
280 -6.738749e-01
280 5.537862e+00
300 -5.441282e-01
300 3.557440e +00
320 -5.985302e-01
320 3.021057e + 00
340 -8.830314e-01
340 3.247780e + 00
360 -1.664950e+00
360 3.449667e+00


X
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000


Y
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000
0.5000


TH1
-155.7048
65.7048
-123.2720
83.7465
-70.4574
126.9086
-10.5431
-169.4516
16.9563
-129.0618
29.7393
-101.2229
37.7244
-77.9331
43.9929
-56.8635
49.8612
-37.3383
56.1950
-19.3265
63.8168
-3.1748
73.5563
10.5839
85.9640
21.6226
100.9853
30.2064
118.1286
37.0779
136.9206
43.0783
157.1842
49.0250
179.2173
55.9277
-155.7032
65.7036


TH2
-138.5904
138.5904
-152.9815
152.9815
-162.6340
162.6340
-158.9085
158.9085
-146.0181
146.0181
-130.9622
130.9622
-115.6575
115.6575
-100.8564
100.8564
-87.1995
87.1995
-75.5215
75.5215
-66.9916
66.9916
-62.9725
62.9725
-64.3415
64.3415
-70.7789
70.7789
-81.0507
81.0507
-93.8423
93.8423
-108.1592
108.1592
-123.2896
123.2896
-138.5932
138.5932


TH3
-65.7048
155.7048
-63.7465
143.2720
-86.9086
110.4574
-130.5483
70.5432
-150.9381
63.0437
-158.7770
70.2608
-162.0668
82.2757
-163.1364
96.0072
-162.6616
110.1389
-160.6734
123.8051
-156.8251
136.1833
-150.5837
146.4438
-141.6224
154.0361
-130.2062
159.0148
-117.0777
161.8716
-103.0781
163.0796
-89.0249
162.8160
-75.9276
160.7829
-65.7034
155.7034
















Max, pe


O -155.7, -138.5, -65.7 (deg)
( -129.0, 146.0, 63,0
( -37.3, 87.1, 110,1


Min, pe


Intermediate pe


Fig. 3-2: Three Different Initial Configurations of Planar 3R Manipulator


































LBC VS CYCLIC MOTION

INITIAL JOINT ANGLE: -155.7, -138.5, -65.6
3



2








m --- -
-J


-1







-3

0 2 4 6 8 10 12
CYCLE(SYMBOL: GENERAL PSEUDO-INVERSE. 5LID MOORE-PENROSE PSEUDO- INVERSE)


Fig. 3-3: Lie Bracket Condition Values Through 10 Cycles of the Motion






























LBC VS CYCLIC MOTION

Initial Joint Angle: -129, 146, 63







3


U
m









-1 ----I I I I I I- !
0 2 4 6 8 10 12
CycleC Syrool: General Pseudo Inverse. Sol a Moore-Penrose PseudOonverse)


Fig. 3-4: Lie Bracket Condition Values Through 10 Cycles of the Motion

































LBC VS CYCLIC MOTION

Initial Joint Angle: -37.3, 87.1, 110.1




0.8



0.6



m 0.4
-J


0.2







-0.2 I I I I I
02 4 6 8 10 12
CycleC 5ymlol: General Pseudoonverse, Solid Moore-Penrose PseuOoinverse)


Fig. 3-5: Lie Bracket Condition Values Through 10 Cycles of the Motion








38

(i)inarl (ei)nita, i = 1, 2, 3) are also shown in Table 3-3. The simulation results

given in Figs 3-3, 3-4, 3-5, and Table 3-3 are a clear comparison of the general

pseudo-inverse control and the Moore-Penrose pseudo-inverse control. By the orders

of magnitude comparison, it is clear that the general pseudo-inverse control (eq. 3.18)

is repeatable in practice whereas the Moore-Penrose pseudo-inverse control (eq.

2.48) is not.

An additional explanation for repeatability can be done by considering potential
n
energy curves (pe = 0.5ki(ei e0,)2) for different initial configurations. If these

curves are never intersecting each other during the cyclic motion, then pe is cyclic

and the initial joint angles return to their initial values when the end effector returns

to its initial position. Fig. 3-6 shows the overall potential energy curves from

maximum pe curve to low pe curve. Fig. 3-7 shows curves of near intermediate pe

curve of Fig. 3-6. From Figs 3-6 and 3-7, the potential energy curves are not

intersecting each other during the cyclic motion when the general pseudo-inverse

equation is used.

As stated in section 2.4, if the Lie bracket condition holds at every point e e ,

then we obtain a family of disjoint surfaces (called a foliation) which covers The

potential energy curves in Fig. 3-6 and 3-7 are a family of potential energy curves

which are never intersecting each other.

In the second example, the link lengths are chosen as a, = 0.3, az = 0.425, a4

= 0.1 (m), the general pseudo-inverse equation is also used. The ranges of each joint

limit are also assumed as follows:












Table 3-3
Simulation Results Through 10 Cycles of the Motion

General Pseudo-Inverse Equation


LBC
2.513085e-07
2.502815e-07


1.983124e-07
2.080915e-07


4.482646e-08
4.406034e-08


X Y
0.5000 0.5000
0.5009 0.5008
0.0009 0.0008

0.5000 0.5000
0.5008 0.5009
0.0008 0.0009

0.5000 0.5000
0.5010 0.5008
0.0010 0.0008


Moore-Penrose Pseudo-Inverse Equation


LBC
-1.664750e+00
-2.043967e + 00


2.233483e + 00
1.848593e + 00



5.815293e-01
5.977754e-01


X Y TH1
0.5000 0.5000 -155.7048
0.5009 0.5008 -146.5125
0.0009 0.0008 9.1923


0.5000 0.5000
0.5008 0.5009
0.0008 0.0009


0.5000 0.5000
0.5010 0.5008
0.0010 0.0008


-129.0618
-118.6704


TH2 TH3
-138.5904 -65.7048
-143.4404 -63.5879
-4.8500 2.2169


146.0181
140.9852


63.0437
64.5052


10.3914 -5.0329 1.4615


-37.3383 87.1995 110.1389
-34.7540 85.4007 112.0434


2.5843


-1.7988 1.9045


CYCLE
0
10
DIFF.

0
10
DIFF.

0
10
DIFF.


TH1
-155.7048
-155.7293
-0.0245

-129.0618
-129.0571
0.0047

-37.3383
-37.3323
0.0060


TH2
-138.5904
-138.6359
-0.0455

146.0181
146.0755
0.0574

87.1995
87.1560
-0.0435


TH3
-65.7048
-65.5864
0.1184

63.0437
62.9211
-0.1226

110.1389
110.1093
-0.0296


CYCLE
0
10
DIFF.


0
10
DIFF.


0
10
DIFF.


--


--


--



































0 0.2 0.4 0.5 0.
I SQUARE PATH C 0 0.25 0.5 0.75 1 )
Fig. 3-6: Overall Potential Energy Curve Profiles


P.E. VS CYCLIC


MOTION


0 0.2 0.4 0.6 0.8 1 1.2
SOUARE PATH C 0 0.25 0.5 0.75 1 )
Fig. 3-7: Detailed Potential Energy Curve Profiles


P. E. VS CYCLIC MOTION


w
I]
a









-80* < e1 5 90*

-160" e2 < 180"

-160" < e3 < 180"

Thus the free position angles of each joint can be also chosen as e01 = 50, 802 = 003

= 10*). The initial joint-angles are 9 = [40", 40", 70*]. The end effector is to track

a straight line from point (0.21701, 0.66138) to point(-0.34299, 0.66138) with a

constant speed of 20 mm/sec. Fig. 3-8 shows the simulation result when the joint

stiffnesses are given values such as k, = k2 = k3 = 1 (Nm/radian). The first joint

angle (e1 = 90.99*) is not within the range of joint limit while the second (e2 =

33.1*)and the third joint angle (e3 = 50.52*) are within. Fig. 3-9 shows the

simulation result when the joint stiffnesses are given values such that the first joint

is stiffer than the other two joints (k, = 10, k2 = 1, k3 = 1). All the joint angles (e8

= 85.22", e2 = 45.240, e3 = 26.55*) are within the range of joint limits during the

specified path. These two figures (Fig. 3-8 and Fig. 3-9) indicate that stiffer joint

moves less than softer joint. Thus a proper choice of stiffness, ki, based on the

mechanical limits of joint helps to avoid the joint limit during the specified path.

3.2 Planar PRR Manipulator

3.2.1 Derivation of Inverse Kinematic Equation

The kinematic model of a planar PRR manipulator is shown in Fig. 3-10.

The coordinates for a point P (see Fig. 3-10) in the end effector are given as

x = ssi + azc2 + a342+3 (3.19)

y = azs2 + a34s23,





























Fig. 3-8: Simulation Result with k, = k2 = k3 = 1.0 (Nm/radian)


Fig. 3-9: Simulation Result with k, = 10, k2 = k3 = 1.0 (Nm/radian)
















kk2


Fig. 3-10: Kinematic Model of a Planar PRR Manipulator


P

Oa3


0


02







44

where az, a3 represent the length of each link and ss, is defined as a displacement

of the prismatic joint.

The Jacobian matrix is obtained as

1.0 -aas2a3S2+3 -a34S2+3
[J] = (3.20)
0.0 a3c2 + a34C3 a34c2+3

The general pseudo-inverse equation is obtained as


6ssi 6x
6S2 = [T]l[k]''[J]( [J][T1-[k]-1[J]T )- (3.21)
.69 Sy -

Detailed derivation of the inverse kinematic equation for a planar PRR redundant

serial manipulator is presented in Appendix A.

3.2.2 Numerical Verification and Simulation of Repeatability

For the first example, the system consists of one prismatic and two revolute joints

as shown Fig. 3-10. Link lengths are chosen as a = a3 = 1 (m). The ranges of each

joint limit are assumed as follows:

-1 (m) < ssi < 2 (m)

-180" < e92 160"

-180* < e3 < 160".

The stiffnesses for each joint are taken as k, = 1 (N/m), and k2 = k3 = 1.0

(Nm/radian). The free position angles of each joint can be chosen sseo = 0.5 (m), and

eo2 = oa3 = -10". The initial values are ss, = 1.0 (m), e2 = 30", and e3 = -150".







45

The x-y coordinates of the four vertices of the command path (1000 consecutive

equidistant segments each side) in the task space are successively given

counterclockwise from the lower left vertex as follows:

(1.3660, -0.2660) (1.4660, -0.2660)

(1.3660, -0.3660) (1.4660, -0.3660)

where the units are meters. The end effector is to move along the square path with

a constant speed of 10 mm/sec and a time step size At = 0.01 sec.

Lie bracket condition values through 10 cycles of the motion are shown in Fig.

3-11. The initial and the final joint angles through each cyclic motion of the end

effector are generated in Table 3-4. At the end of the 10 cycles the position

difference ( xfn, xii., ya. yiial) and the joint angle difference ( (ss),, (ssji)

(eoi), (e)init i = 2, 3) are also shown in Table 3-4. By the orders of magnitude
comparison, it is clear that the general pseudo-inverse control (eq. 3.21) is repeatable

in practice whereas the Moore-Penrose pseudo-inverse control (eq. 2.48) is not.

In the second example, the same link lengths, initial joint angles and joint

stiffnesses as the first example are used. This example is to illustrate that the results

are independent of choice of unit of length. The desired end effector twist is D =

( 0.0001 m, 0.0 m )T. The resulting joint motions are obtained via an application of

the general pseudo-inverse equation:

s = ( 6ssI m, 6e2 deg, 6e3 deg )T

= (0.000119 m, -0.001092 deg, -0.000799 deg )T.




























LBC VS CYCLIC MOTION

LBC: LIE BRACKET CONDITION










S2-
-J





01 -



0 2 4 6 8 10 12
CYCLECSYMBOL:GENERAL PSEUDO- INVERSE, SOL ID:MOORE-PENROSE PSEUDO- INVERSE)


Fig. 3-11: Lie Bracket Condition Values Through 10 Cycles of the Motion









Table 3-4

Simulation Results Through 10 Cycles of the Motion

General Pseudo-Inverse Equation


LBC
-0.0000038
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004
-0.000004


x
1.366025
1.366105
1.366186
1.366267
1.366347
1.366428
1.366509
1.366589
1.366670
1.366751
1.366831
0.000806


Y
-0.366025
-0.365935
-0.365844
-0.365754
-0.365664
-0.365574
-0.365484
-0.365394
-0.365303
-0.365213
-0.365123
0.000902


SSi
1.000000
1.000105
1.000234
1.000364
1.000493
1.000622
1.000751
1.000880
1.001010
1.001139
1.001268
0.001268


TH2 TH3
30.000013 -150000066
29.997632 -1500L062
30.003800 -15(006499
30.009970 -150012316
30.016142 -150018132
30.022316 -150.02948
30.028491 -15Q029763
30.034669 -1505577
30.040849 -15Q01391
30.047031 -151047"2
30.053214 -15053016
0.053201 -0.05235


Moore-Penrose Pseudo-Inverse Equation


SSi
1.000000
0.987656
0.975260
0.962832
0.950435
0.938130
0.925970
0.913997
0.902246
0.890743
0.879506
-0.120494


TH2 TH3
30.000013 -15Q00066
30.692766 -149.473779
31.361522 -148940235
31.996296 -148396360
32.595150 -147845347
33.157023 -147290315
33.681691 -146734159
34.169666 -146179456
34.622052 -145628402
35.040392 -145.82806
35.426522 -144544104
5.426509 5.45596


CYCLE
0.000
1.000
2.000
3.000
4.000
5.000
6.000
7.000
8.000
9.000
10.000
DIFF.


CYCLE
0.000
1.000
2.000
3.000
4.000
5.000
6.000
7.000
8.000
9.000
10.000
DIFF.


LBC
2.610043
2.610684
2.600207
2.578829
2.547220
2.506420
2.457731
2.402590
2.342458
2.278733
2.212683


x
1.366025
1.366110
1.366194
1.366278
1.366361
1.366444
1.366527
1.366610
1.366692
1.366775
1.366858
0.000833


Y
-0.366025
-0.365932
-0.365839
-0.365747
-0.365654
-0.365561
-0.365468
-0.365374
-0.365280
-0.365185
-0.365090
0.000935








48

These same results are obtained if the unit of length measure for all elements

is changed to centimeters. In this case, the stiffnesses are ki =0.01 (N/cm), and k2

= k3 = 100 (Ncm/radian). The results can be recalculated and shown to be identical

to the above, bearing in mind the changed dimension for the incremental length

change of the prismatic joint, viz.

6 = ( 6ssi cm, 680 deg, 683 deg )

= (0.0119 cm, -0.001092 deg, -0.000799 deg )T.

This clearly illustrates that the analysis is independent of the chosen unit of

length. This is not the case when the Moore-Penrose pseudo-inverse equation is used,

which is analogous to the minimum-norm solution. In such a case, there is no means

available in the analysis whereby the units of the physical dimensions or of the

mechanical properties can be accounted for. Here, this compensation takes place by

virtue of the elastic relationships that are modeled from the real manipulator system

or are specified by virtue of n desired-joint-stiffnesses.

3.3 Planar 4R Manipulator

Consider a 4 degree of freedom manipulator which moves in the (x,y) plane. The

position and the orientation of the end effector is of concern, thus making it

kinematically redundant. The kinematic model of a planar 4R manipulator is shown

in Fig. 3-12.

3.3.1 Derivation of Inverse Kinematic Equation

The coordinates for a point P (see Fig. 3-12) in the end effector are

x = alC + a23c1 + a34c+2+3 + ac41+2+3+4, (3.19)


















045


aCk /


0O23


0(


Fig. 3-12: Kinematic Model of a Planar 4R Manipulator










y = aus1 + + a3+2 S1+2+3 + a4s+2+3+4,

a = e+ + 2 3 + 4.

where a2, az3, a34, a4 represent the length of each link and a represents the

orientation of the end effector. By analogy with the section 3.1.1, the Jacobian matrix

is obtained as

11 J12 J13 J14
[J]= 21 22 23 24 (3.20)
[1.0 1.0 1.0 1.0
where

J1 = -a2s1 a23s+2 a34s1+2+3 a45S+2+3+4,

J12 = a23sl+2 a34s+2+3 a45s+2+3+4,

J13 = -a34S+2+3 a4551+2+3+4,

J14 = -45s1+2+3+4,

21 = a,2c" + ac12 + a34c1+2+3 + a45c1+2+3+4,

J22 = a23cl+2 + a341+2+3 + a45l1+2+3+4,J23 = a34c1+2+3 + a45c1+2+3+4,

J24 = a45cl+2+3+4-

The general pseudo-inverse equation is obtained as



6 [ 6x

682 = [A]-1[J]T( [J][A]I[J]T )-1 6y (3.21)
693 6 a
684


Detailed derivation of the inverse kinematic equation for a planar 4R serial









redundant manipulator is presented in Appendix B.

3.3.2 Numerical Verification and Simulation of Repeatability

Link lengths are given as

a, = 0.7, a% = 0.9,

a3 = 0.145, as = 0.1 (m).

The ranges of each joint limit are assumed as follows:

47* < o, < 129", -120" 5 e < -50*,

-118" < eo < 60*, -180" 5 04 180".

Based on above the range of each joint limitation the free position angles of each

joint can be chosen as

001 = (129 + 47)/2 = 88, 02 = (-120 50)/2 = -85",

o03 = (-118 + 60)/2 = -29-, 0o4 = (-180 + 180)/2 = 0",

and the stiffness for each joint are also taken as

ki = 360/(129 47) = 4.4, k2 = 360/(-50 + 120) = 5.14,

k3 = 360/(118 + 60) = 2.02, k4 = 360/(180 + 180) = 1.0 Nm/radian.

The initial joint-angle values are a = [100", -110", 30", 30O]T. The x-y coordinates

of the four vertices of the command path (2000 consecutive equidistant segments

each side) in the task space are successively given counterclockwise from the lower

left vertex as follows:

(0.9653, 0.9592) (1.2653, 0.9592)

(0.9653, 0.6592) (1.2653, 0.6592)

where the units are meters. The end effector is to track the square path with a








52
constant speed of 15 mm/sec, a constant orientation a = 50', and a time step size

At = 0.01 sec.

Lie bracket condition values through 10 cycles of the motion are shown in Fig.

3-13. The initial and the final joint angles through each cyclic motion of the end

effector are generated in Table 3-5. At the end of the 10 cyclic motions of the end

effector, the position difference ( x. X,,tia, Ynal yiatw) and the joint angle

difference ( (ei)ii (e)aw, i = 1, 2, 3, 4) are also shown in Table 3-5. By the

orders of magnitude comparison, it can be said that the general pseudo-inverse

control (eq. 3.21) is repeatable in practice whereas the weighted pseudo-inverse

control (eq. 2.46) is not.

3.3.3 Implementation

Actual implementation is accomplished on a planar GE 4R manipulator (see Fig.

3-14). The same link lengths, initial joint angles, free angles, spring stiffnesses, and

the square path motion of the end effector as section 3.3.2 are used.

The final manipulator configurations after 10 cycles of the motion are shown in

Figs. 3-15 and 3-16, respectively. From the Figs 3-15 and 3-16, it is clear that the

general pseudo-inverse control is repeatable whereas the weighted pseudo-inverse

control is not.
































LBC VS CYCLIC MOTION

LBC: LIE BRACKET CONDITION
0.




0.3




0.2

m
-.J

0.1








0.1 I-------- -------------II
0 2 4 6 8 10 12
CYCLECSYMBOL: GENERAL PSEUDO-INVERSE, SOLID: WEIGHTED PSEUOO- INVERSE)


Fig. 3-13: Lie Bracket Condition Values Through 10 Cycles of the Motion









Table 3-5

Simulation Results Through 10 Cycles of the Motion

General Pseudo-Inverse Equation


CY. LBC
0 -8.201034e-07
1 -8.181804e-07
2 -8.176153e-07
3 -8.158862e-07
4 -8.144222e-07
5 -8.130164e-07
6 -8.115498e-07
7 -8.106662e-07
8 -8.090450e-07
9 -8.076453e-07
10 -8.066382e-07
DIFF.


X
0.965307
0.965408
0.965510
0.965611
0.965713
0.965814
0.965916
0.966017
0.966118
0.966220
0.966321
0.001014


Y
0.659279
0.659363
0.659446
0.659530
0.659614
0.659697
0.659781:
0.659864
0.659948
0.660032
0.660115
0.000836


TH1 TH2
100.0000 -110.0000
99.990615 -109.9958
99.982685 -109.9842
99.974756 -109.9726
99.966828 -109.9611
99.958901 -109.9535
99.950974 -109.9460
99.943049 -109.9364
99.935124 -109.9248
99.927201 -109.9133
99.919278 -109.9017
0.000836 0.08073


Weighted Pseudo-Inverse Equation


CY. LBC
0 3.614851e-01
1 3.567477e-01
2 3.517397e-01
3 3.464733e-01
4 3.409614e-01
5 3.352178e-01
6 3.292568e-01
7 3.230936e-01
8 3.167435e-01
9 3.102225e-01
10 2.945564e-01
DIFF.


X
0.965307
0.965408
0.965510
0.965612
0.965714
0.965816
0.965919
0.966021
0.966124
0.966227
0.966329
0.001022


TH3
30.0000
30.0026
30.0034
30.0043
30.0051
30.0059
30.0067
30.0076
30.0084
30.0092
30.0101
0.0101


TH4
30.0000
30.0025
29.9980
29.9936
29.9891
29.9847
29.9802
29.9757
29.9713
29.9668
29.9624
0.0376


Y
0.659279
0.659363
0.659446
0.659530
0.659613
0.659695
0.659778
0.659861
0.659943
0.660025
0.660107
0.000828


TH1
100.0000
100.1065
100.2098
100.3082
100.4016
100.4899
100.5733
100.6515
100.7246
100.7926
100.8555
0.8555


TH2
-110.0000
-109.9503
-109.8983
-109.8115
-109.7200
-109.6239
-109.5233
-109.4384
-109.3493
-109.2962
-109.2594
0.7406


TH3
30.0000
29.3482
28.7963
28.0469
27.2007
26.2586
25.3914
24.6900
23.0649
22.2070
21.3369
-8.6631


TH4
30.0000
30.7754
31.1421
31.9064
32.8677
33.6252
34.4785
35.2269
36.0697
36.7066
37.5437
7.5437




































Fig. 3-14: Initial Configuration of a Planar GE 4R Manipulator
(eI = 10(), 02 = -110", e, = 30, e4 = 30)




























Fig. 3-15: Implementation Result after 10 Cycles with the General Pseudo-
inverse Equation (Final Configuration: 9, = 100.03, e = -
110.09, 83 = 30.0", e4 = 30.01)


Fig. 3-16: Implementation Result after 10 Cycles with the Weighted Pseudo-
inverse Equation (Final Configuration: e, = 100.75", = -
109.49", 93 = 21.12", 94 = 37.6)













CHAPTER 4
REPEATABILITY OF A SPATIAL 7R MANIPULATOR


The proposed general right pseudo inverse method in the chapter 2 is also

applied to a spatial 7R manipulator. It is therefore necessary to develop its forward

analysis.

4.1 Notation

The notation used throughout this analysis is that developed by Duffy [12]. Briefly

stated, a manipulator is composed of a series of rigid links. One such link is shown

in Fig. 4-1. In this figure it is shown that the link connects the two kinematic pair

(joint) axes S. and S. The perpendicular distance between the pair axes is ay and the

vector along this mutual perpendicul:: i' aj. The twist angle between the pair axes

is labelled aij and is measured in a right handed sense about the vector aj.

The particular kinematic pair under consideration is the revolute joint which is

shown in Fig. 4-2. The perpendicular distance between links, or more specifically the

perpendicular distance between the vectors aij and ajk, is labelled as the offset

distance Sj. The relative angle between two links is shown as eO and is measured in

a right handed sense about the vector S.

4.2 Mechanism Dimensions of 7R Manipulator

Shown in Fig. 4-3 is a kinematic model of the 7R Manipulator (SSRMS). In the

kinematic model the joint axes are labeled sequentially with the unit vectors












































Fig. 4-1: Spatial Link

















Ijk











sj



9,j


Fig. 4-2: Revolute Pair


~~Bi_
1







60








04/ a 34



S~ a 45


sS
5 5 75 67 0 7
a 23 Z a7 3

1 1 56




S







Fig. 4-3: Kinematic Model of the 7R Manipulator (SSRMS)







61

S. (i = 1, 2,..., 7). The directions of the common normal between two successive

joint axes Si and S are labeled with the unit vectors aj (ij = 12, 23,..., 78).

As previously stated, the link lengths aij, the offsets Sj, and the twist angles aij are

constants which are specific to the geometry of a particular manipulator. The values

of these constants are tabulated in Table 4-1 for the SSRMS. In addition to the

constant dimensions, S, and a7, are selected such that the point at the end of vector

a2 is the point of interest of the tool connected to the manipulator. For example this

point may be the tip of a welding rod that the manipulator is moving along a path.

Once a particular tool is selected, constant values for S7 and a78 are known.

Furthermore it is noted that the link lengths a2, a6, equal zero. However it is still

necessary to specify the direction of the unit vectors a2, a in order to have an axis

about which to measure the corresponding twist angles. The vector aQ must be

perpendicular to the plane defined by the vectors S, and Sj and as such can have two

possible directions. For the vectors al, ~, this direction is arbitrarily selected as the

direction parallel to the vector SxS,. The values for the corresponding twist angles

a,, a,7 listed in Table 4-1 are determined based upon this convention.

4.3 Specification of Position and Orientation

The first step of the analysis is to establish a fixed coordinate system. For this

analysis a fixed coordinate system is established as shown in Fig. 4-4 such that the

origin is at the intersection of the vectors S, and S2. The Z axis is chosen to be

parallel to the S, vector and the X axis bisects the allowable range of rotation of the












Table 4-1
Mechanism Parameters for SSRMS


a, = 0 m


a, = 90*


S2 = 0.635


S3 = 0.504



S4 = 0.504



Ss = 0.504



S6 = 0.635



S7 = 0.300*


az3 = 0.38


a3 = 6.85



a45 = 6.85



ass = 0.38



a67 = 0


= 0.1'


S7, a7s: Tool Dependent Parameters

a78: an Arbitrary Twist Angle


Si = Om


az3 = 270*


a34 = 0



a45 = 00



a56 = 90*



a67 = 90*


an = 90*







63












*s*
NO












R









_s8


a78


Fig. 4-4: Hypothetical Closure Link
N/


NC
N7

Fi. -4 ypthtca Cour Nn







64
angle 0,. Throughout the rest of this analysis, this coordinate system will be referred

to as the fixed coordinate system.

Using this fixed coordinate system it is possible to specify the position and

orientation of the end effector by specifying the vector to the tool tip, 4, (see Fig.

4-4) and the pair of orthogonal unit vectors & and as. Although Rp, a,, and S have

a total of nine components, the latter two are related by the three conditions,

a78' 78 = 1,

a78.S = 0, (4.1)

S8. = 1,

so that the three vectors (Rp, a7s, &) represent the 9-3 = 6 independent parameters

necessary to locate a rigid body in space (see [13]).

4.4 Derivation of Forward Kinematic Equation

The vector loop equation of the equivalent closed loop spatial mechanism in the

first coordinate system can be written by (see set 1 in Table 4-2, [12])

R) = SS( )+ + a2a( SS + + SS%' + aa3_ a + S4(1 + + S )

+ aS, + SXI)1 + S77(l + + a78-a), (4.2)

%(1) = [0,-s, cu]T,

= [0, -1, 0f, (4.3)

a1' = [c2, U21, U2fT,

= [c2, 0, s2, (4.4)

S-, = [XI Z, T,
= [-s2, 0, c2I, (4.5)











Table 4-2
Direction Cosines Spatial Octagon


set 1:

S ( 0, 0, 1)

2 ( 0, -S12, C )

S( X,, Y, ,2 )

4 (X32, Y3, Z3 )

I ( X4n, Y43, Z432 )

S6 ( X5432, Y5432, Z5432 )

a7 ( X5432, Y65432, Z65432 )

S8 ( X765432, Y765432, Z765432 )

set 8:

84 (0, 0, 1)

Si ( 0, -s81, c81 )

2 ( X, 1 Z, )

S( Xl, Y21, ,21 )

S4 ( X32, Y32x, Z321)

I ( X4321, Y432, Z4321 )

6 ( X54321, Y54321, 5432 )

, (X4321, Y654321, Z654321)


an ( 1, 0, 0 )

23 ( C2, U21, U21)

34( W32, -U321, U321 )

4 ( W432, -U4321*, U4321 )

a56 ( 5432 1, U521 54321 )

67 ( W62, -U654321, U65432 )

a78 ( 765432, -U7654321, U7654321 )

a1 ( c, -s,, 0 )



a ( 1, 0, 0 )

a12 (ci, Us*, Usi )

23 ( W21, -U21, U21 )

34 ( W321, -U321, U321 )

45 (W4321, -U4321 U4321 )

56(W 54321, -U54321*, U54321 )

67 ( W65421 -U654321, U654321 )

78 ( W7654321, -U7654321, U7654321)









() = [W32 -U321, U321T,

= [C2C3, S3, S ,3]T,

41) = [X32, Y32, Z32T,

= [-s2, c2],

45) = [ 432, -U432, U432,]

= [c2c3+4, S3+4, 2C3+4]T,

5(1) = [X432 Y432, Z432],

= [-s2, 0, c2T,

) = [W5432, U54321, U54321 T,

= [C2C3+4+5, S3+4+5, S2C3+4+5]T,

61) = [X5432, Y432, Z5432,

= [C2S3+4+5, -C3+4+5, S2S3+4+5]T

S7(1) = [X65432, Y65432, Z65432]T,

= [S6C2C3+4+5+S2C6, S6S3+4+5, S6S2C3+4+5- CC6]T,

781) = [W765432, -U7654321*, U76543211T

= [-S2s6c7+ C2(s7s3+4+5 + c6C7C3+4+5), -s7c3+4+5 + c6c7s3+4+5,

+ s2(s7s3+4+5 + c6C7C3+4+5)]T,

Substituting eq. (4.7) through (4.17) into eq. (4.6) yields

RP() = [Rpx(), R,(), R (' T,


(4.14)


where


(4.6)



(4.7)



(4.8)



(4.9)



(4.10)



(4.11)



(4.12)


C2S6C7

(4.13)







67

Rp1 =- a232 S3s2 + a3c2c3 S4s2 + a4c2c3+4 Ss2 + a56c2c34+ +

S6c2s3+4+5 + S7(s6C3+4+c,2 + c6s2) + a7(-s2s6c7 + c2(s7s3,4+5 +

C6C7C3+4+5) ),

R,() = -S2 + a34S + a45s3+4 + a56s3+4+5 S63+4, + S76S3+4+5 + a7,(-

S7C3+4+5 + C6C7S3+4+5),

P'() = a23s2 + S3c2 + a34S2c3 + S4c2 + a4s2c3+4 + S5c2 + as2c3+4+5 +

S6s2s3+4+5 + S7(s6c3+4+5s2 c6c2) + a78(C2s6c7 + S2(S7S3+4+5 +

C6c73+4+5) )9

the orientation vector of end effector,

8() = [X765~, Y765432, z765432T

= [c2(c6S7C3+4+5 C7s3+4+5) 82S6S7, C6S7S3+4+5 + C7c3+4+5,

S2(C6S7C3+4+5 C7s3+4+5) + C2S67]T. (4.15)

Position and orientation vectors known in terms of the first coordinate system can

be found in terms of the fixed coordinate system as follows:

R = [M][Ro)], (4.16)

a78 = [M][a7()], (4.17)

S = [M][Sg )], (4.18)


where


cosoi
M = sino,
0


-sin01
COS01
cos
0


0
S .
1


Detailed derivation of these kinematic equations for the position and orientation of

end effector is presented in Appendix C. As previously stated, although eq. (4.16),







68

(4.21), and (4.22) have a total of nine components, eq. (4.17) and (4.18) are related

by the three conditions of equation set (4.1) so that eqs. (4.16), (4.17), (4.18)

represent the 9-3 = 6 independent parameters necessary to position and orient the

end effector in three dimensional space. Throughout the remainder of this paper, si

and ci will represent the sine and cosine of ei while sij and c,, will represent the sine

and cosine of ai. The terms s,.j..., and ci+j...+, represent the sine and cosine of the

angle (O + ei + .. + e).

4.5 The Derivation of Jacobian Matrix

Fig. 4-4 shows a coordinate system attached to ground. All line coordinates will

be found in terms of a coordinate system which is attached to the SSRMS and has

its origin located at the intersection of the vectors S and a7,. The Z axis of this

coordinate system will be along & and the X axis will be along a78. The Plicker

coordinates for each of the seven axes of the SSRMS will now be found in terms of

this new coordinate system--eighth coordinate system. It is important to recognize

that the conditions for the linear dependency of the line coordinates are independent

of the choice of coordinate system.

The Plicker coordinates of each of the seven joint axes are comprised of the

direction of the joint axes and the moment of that line about the origin of the

coordinate system. The directions of each joint axis measured with respect to the

current coordinate system are listed directly as set 8 in Table 4-2. It has also been

shown in Duffy [12] that the moment of each line about the origin is equal to the







69
dual of the direction vector. The Plucker axis coordinates of the seven axis lines,

$= [l;oiS are listed as follows:
$18) = [(-a7A78s8 S 7( S6S) a5.86 S- S58) a,) SA 48 a..(8)

S3%3 a 23 S(())xS(8) ; S() ], (4.19)
28) = [(-a7 78(8)- SS7) S66) a6- (8) S(8) a454() S4S) a.4348)

S3( a23a8))x(8) ; %(8) ]T,
$(8) = [(-aa,(8) S7) SS a56 (8) SS () a4(8) S() aa(8)

)X_%, ; S3(8) IT,
4(8) = [(-a7 g788( S S7 S(8) 8) S a568 (8))x () ; &8() ]f,

1() = [(-aa 7(8)- SS7) SS6) aZ()xS() ; (8) ]r,

(g) = [(-a7,a8 ) S7))x(8) ; () ]T,

$7) = [-a78~78, x S) ; S() ]T,
The Jacobian matrix can be expressed as

[J] = [1(8) $8) $(8) ) 5(8) (8)]. (4.20)
The Pliicker coordinates for each of the seven lines in eq. (4.19) can be simplified

by substituting the actual SSRMS mechanism parameters from Table 4-1 into the
equations. The simplified Plicker axis line coordinates are as follows:

$1(8 = [ So Soy SSoz ; S Sy SJ]T, (4.21)

$2() = [ S2x S02y S2z ; S2 S2y S2.]T,
3(8) = [ o3x S03y S03z ; S3, S3 S3T,
$4( = [S SO,, S, So; S5 84, s, 4]

$5) = [ SoSx SS, Soz ; S,, s, Sz]T,









$s) = [ S06 So( SoZ ; S, SSy Sf],

$7() = [ SoI S 07y So,; S7, S7 SZJ ,

So, = {-az3sc2 +S2C1 + S3ss,2 a34(slcc3 + cs3) + S4sIs2 a4,(sc2c3 + cs34) + S5ss2 -

a56(,c2c345 + c1s345) + S6(ccC345 s2s345) S7(S1S6C4Sc2 + s1c6sz + cIs6s345) +

a78(S1S2S6C7 S1C2(S7S345 + C6c7c34) + cS7C34s 1C6C7S345))},

Soy = {a23Cc2 + S2s S3C1S2 + a34(CC2C3 SS3) S4c1s2 + a45(C2C34 s1l34) Sscs2 +

a56(cc2cC34 81s345) + S6(s1C345 + c12s345) + S7(CS6C345c2 + CC6s2 SS6S345) + a78(-

CIS2S6C7 + CiC2(S7S345 + QC6C345) + SS7C345 SC6C7S345))},

Soz = 0, Si = 0, Sly = 0, Si = 1,

So2x = {-a23 1S2 S3CaC2 ac23 S4c 45234 S a54ccc -s 4 S5c -345s S6CLS28345 -

S,(CIS6C352 CiC6C2) a78(CC2S6C7 + C1S2(S7S345 + C6C7C345))},

S02y = {-a23S1S2 S3SC2 a34sls2C3 S4s1c2 a45s1s2c34 S5s1c2 a56ss2c345 S6ss2ss345 -

S7(S1s6c345s2 scc2) a78(s1c2s6c7 + ss2(s7s345 + cc7c45))},

So2 = {a23c2 Ss2 + a45c2c34 5s2s + a56c2c345 + 62s345 + S7(6c345c +

C6S2) + a78(-s2S6C7 + 2(s7S345 + C6CC345))},

S2x = Sy = -C1, S2. = 0,

S03x = {-a34(CC2S3 + SlC3) a45(cc2s34 + sc34) a56(cs2s345 + SC1345) + S6(Cc2C345 SS345) -

S7(CIS6S345C2+ SIS6345) + a7,(cc2(s7c345 ccs345) s1(s7s345 + C7C345))},

S3y {a34(s 3 CC3 ) c 45(3)- a45sc2s 34 -c a561(css345 CLC345) + S6(1c2c345 + cls4) -

S7(SS6S345C2 C1s6C345) + a78(sc2(s7c345- c6cs345) + c1(s7s345 + c6c7C345))},

S03z {-a34s2s3 a45ss34 a56ss234 + S6s2c, S7 5s782(ss345 c6C7345))},

S3x = -CS2, S3y = -S1S2, S3. = C2,







71
SX = {-a4,(c,2s, + sc3) a(c,1css + sIcs) + S6(c c2c3 sS,3) S7((CSSMC2 +

SS6Ci34) + a78(cC2(s7C345 c6c7s5) s1(s7345 + C(C7C345))},

So4 = {-a45(s2s34 + clc3) a6(sc2s + cc345) + S,(sic2c34 + c1s34) S,(s1s6s3c2-

csc3s) + a((scS7C34s C6C7s3) + c,(s7S34 + c,6c7c3))},

So4 = {-a4ss234 a5us345 + S6c2c, 5- S7s6s3s2 + a78s2(s7c34 c6c7s3)},

S4x = -12, S4, = -S1S2, 54 = C2,

S0 = {-a5(cc24s + s1345) + S6(cc4c ss3) S7(cs6S345C2 + sis6c34) + a78(cc2(s7c5

cC7S345) sI(S7S34 + CC7C34S))I,

S05y = -a56(SlC2S3 CC345) + S6(SC2C345 + CS34) S7(SiS6S345C2-c6C345) + a78(SC2(S7C345-

6CS345) + C1(SS345 + C6C3))},

Soz = {-a56s2s345 + S62c345 S7s6s3452 + a78S2(S7C345 C67s345)},

Ss -CIS2, S = -sIs2, S5z = 2,

So6x = {S7(CC6C34c2 ClS6S2 SlC6S345) aT7(czs2c6c7 + CiC2S6C7C345 S16CS345)},

So6 = {S7(sCC345C2 SIS6S2 + ccs345) a78(S1s2c6c + 1C2S6C7C345 + cS6C7S345)},

Soz = {S7(c6c34,s2 + s62) + a7(c2cc7 s2s6cc345)},

S6x = c2S345 + sc345, S6y = sc12S34 CC345, S6z = s2345,

S07O = a78(cis2s6s7 + c1c2(c7s345 c6s7c345) + s1(c7c345 + cs7s345)),

S07Y = a78(sis2s6s7 + sc12(c07345 c6s7c345) cI(cC345 + cs7s345)),

So7 = a7,(-c2s6s7 + s2(c7345 scS345)),

ST, = CC2SC345 + ClS2C6 SS6S345, ST, = Si2S6c345 + sS2C6 + CS6S345,

S7z = s2sc5 C2C6.







72
A detailed derivation of Plicker axis coordinates of the seven joint axes of the 7R

manipulator (SSRMS) is presented in Appendix D. Now the Plicker axis coordinates

of the seven joint axes of the 7R manipulator (SSRMS) are known.

4.6 The Derivation of Inverse Kinematic Equation

Joint torques are related to joint rotations by

[r1 T2 r3 74 75 r6 7]T = [k][01-01ox 2-802 e3-o03 4-004 e5-05 E6-806 e7-e07]'. (4.22)

where 01-0o, and ei eo, (i = 2,...,7) are differences between the current and the free

position joint angle and [k] is a 7 x 7 diagonal matrix of joint stiffnesses as follows:

kl 0 0 0 0 0 0
0 k2 0 0 0 0 0 (4.23)
0 0 k3 0 0 0 0
[k] =
0 0 0 k4 0 0 0
0 0 0 0 k5 0 0
0 0 0 0 0 k 0
0 0 0 0 0 0 k7


The eq. (4.22) may be more conveniently written as

[01-001 92-802 e3-803 84-8040 e5-005 e6-806 E 7-8]T = [k].l[r, T2 73 T4 5 T 77]T. (4.24)

The joint torques, acting in each joint, combine to give forces at the end effector,

and this can be expressed as

[7" T2 73 74 75 6 T7 ]T = [J]T[fx fy fm my n11T. (4.25)

Combining the stiffness relationships in eq. (4.24) with eq. (4.25) gives the joint

motions

[10-00o1 2-02 3-o03 0,-4 0 5- 05 6-06 7e-07]T = [k].q[J]T[f fy my myM1. (4.26)







73
Premultiplying the left and right sides of eq. (4.26) by [J] and rearranging eq. (4.26)

yields

[f, f m mz]T = ([J][k]n[J]T)-'[J][1-o0l e2-e2 ,, e-806 ,e-e7]'. (4.27)

This can be solved provided that [J] is of full rank. Taking the derivative of eq. (4.26)

yields

[641 682 683 6e4 6e5 606, 6]T = [k]'[J]T[6ff 6f5 6f, 6n 6my 6mZ1 +

[k] -[J][f fy m y my mj (4.28)

where

6J1n 6J21 J31 6J41 653 6J1
6J2 6J22 J32 42 6J52 J62
6J3 6J23 6J33 6J43 6J53 6J63
[6J]T = 6Ji4 6J24 6J34 6J4 654 6J64
6J,5 J25 635 6J45 6J55 6J65
6J6 6J26 66 6J46 6J56 J66
6J17 7 37 J7 7 6 45765 6J 67

Jpq = Upq~ + Vpqe2 + Wp6e3 + UVpq5e4 + vwpq6e + uwpq6e + uvwpqe7,

p = 1, 2, ..., 6, q = 1, 2, ..., 7.

Detailed derivation of upq, Vp, wpq, uvpq, vwp, uwq and uvwq is in appendix E.

The second term of right hand side of eq. (4.28) goes to the left hand side,

[65, 6e2 6e3 6e4 6S5 6E6 6e7]T [k]-l[J]T[fx f ff rM my mrT

= [k]Fl[J][fx S6f 6f, 5m, 6my 6rnmT. (4.29)

Substituting eq. (4.27) into eq. (4.29) and rearranging the left hand side of eq. (4.29),

this can be expressed in the form







74
[T][6p,0 68, 3 64e 695 6e, 6e7,] = [k]-' []T[6f 6 f 6t m, 6n, 6m]T, (4.30)

where [T] is a 7 x 7 matrix as follows

T11 T T13 T14 T5 T16 T17
T21 T22 T T24 T25 T26 T27
T31 T32 T33 T T35 T36 T37
[T]=
T41 T42 T43 T44 T45 T4 T47
T51 T52 T53 T,4 T55 T56 T57
T61 T62 T63 T64 T65 T66 T67
T71 T7n Tn T74 T75 T76 T7
3 6
T, = I- (3 u, f, + 6u m,) /k, (if p = 1, I = 1, else I = 0, p = 1,2,...,7)

Tp = I- (z 3Vf,f + 6 vqmq) /k2 (if p = 2, I = 1, else I = 0, p = 1,2,...,7)
3 6
Tp3 = I- ( wpfq + q=4wqpm) /k3 (if p = 3, I = 1, else I = 0, p =1,2,...,7)
6
T = I (uvqpfq + Z uvqpmq) /k4 (if p = 4, I = 1, else I = 0, p = 1,2,...,7)
q=1 q=4
3 6
TP = 1- ( v1 f + 4v,,qpm,) /k5 (if p = 5, I = 1, else I = 0, p = 1,2,...,7)
T, = I- (z3uw ,fq + uw,m,q) /k (if p = 6, I = 1, else I = 0, p = 1,2,...,7)

T,, = I- (qZuvwq,fq + E uvwqpmq) /k7 (if P = 7, I = 1, else I = 0, p = 1,2,...,7)
q=1 q=4
Eq. (4.30) can be also solved provided that [T] is of full rank. Thus,

[601 682 6e3 684 65e 6(6 6,7]T = [A]i[J]T[6fS 6fx 5 6m,. 6mn 6mj]T. (4.31)

Letting

[A] = [k][T],

and premultiplying the left and right sides of eq. (4.31) by [J] yields

[J][651 682 683 654 5, 686 687]1 = [J][A]-[J]T[6f, 6f, St 6m. 6my 6mj,]. (4.32)

The forward instantaneous motion equation for the manipulator is given by







75

[J][6i 682 683 684 6e5 Se 68 1T = [6x 6y 6z 6a S6p 6y]. (4.33)

Substituting eq. (4.33) into eq. (4.32) yields

[6x 6y 6z 6a 6S 6y]T = [J][A]'[J]T[6f 6fy 6f, 6m, 6mn 6SmT, (434)

which can be expressed in the form

[6x 6y 6z 6a 6S 6y]T = [C][6f, 6fy 6 5m, 6m 6nTSm (4.35)

where [C] = [J][A]I[J] is an 6 x 6 symmetric compliance matrix that relates the end

effector external wrench to its corresponding twist. Provided that [T] and [J] are both

full rank, then

[6fS 6 Sfy m 6fm, 6rrmy T = [C]11[6x y 6z 6S 66 6y]T, (4.36)

relates the necessary deflection twist of the end effector relative to ground to the

incremental change in resultant wrench.

In the problem of the inverse kinematics of the redundant serial manipulator, it

is required to determine the joint increments explicitly. The elastic compliance

relation of eq. (4.35) can be used to accomplish this objective. Substituting eq. (4.36)

into eq. (4.31) yields the general pseudo-inverse equation:



[618 6e2 683 684 685 6 6 7]T = [A]-[J]T([J][A]l[J]-T'[6x Sy 6z 6a 6f 6y]T
= [A]'[J]T(C)-[6x 6y 6z 6a S6 6y]T. (4.37)

4.7 Numerical Verification and Simulation of Repeatability

Mechanism dimensions are given as table 4-1. The joint limits are selected as

follows:


-180 5< < 180",


-180 : o, 6 60,


-180" < e3 < 180",









-180* < e4 < 180, -180 e5s 5 1000, -180 < e6, 5 180,

-180 < e7 < 120".

The free position angles and the stiffnesses of each joint can be chosen as

0o, = (-180 + 180)/2 = 0", 902 = (-180 + 60)/2 = -60",

Oo3 = (-180 + 180)/2 = 0", 04e = (-180 + 180)/2 = 0",

e5 = (-180 + 100)/2 = -40, 9o6 = (-180 + 180)/2 = 0*,

eo7 = (-180 + 120)/2 = -30",

k, = 360/360 = 1.0, k2 = 360/240 = 1.5, k3 =360/360 = 1.0,

k4 = (360/360 = 1.0, k5 = 360/280 = 1.28, k6 =360/360 = 1.0,

k7 = 360/300 = 1.2 Nm/radian.

The initial joint-angle values are O =. [190%, 1900, 190*, 190%, 190*, 190*, 190"]T.

The x-y coordinates of the four vertices of the command path (3000 consecutive

equidistant segments horizontal side and 1000 segments vertical side) in the

workspace are successively given counterclockwise from the lower left vertex as

follows:

(-0.7535, -0.8966) (-0.4535, -0.8966)

(-0.7535, -0.9866) (-0.4535, -0.9866)

where the units are meters. The end effector is to track the rectangular path with a

constant speed of 10 mm/sec, a constant orientation (a, = -0.86994, a, = 0.49171,

a78 = -0.03704, S, = -0.49310, Sy = 0.86615, S, = 0.86615), and a time step size At

= 0.01 sec.







77

Lie bracket condition values through 5 cycles of the motion are shown in Fig. 4-5.

The initial and final joint angles through each cyclic motion of the end effector are

generated in Table 4-3. At the end of 5 cyclic motion of the end effector, the position

difference ((Rp) (Rp,)i~.) and the joint angle difference ((Pi)r (p1)ina (ei)i-

(e)inia, i = 2, 3,,,, 7) are also shown in Table 4-3. By the orders of magnitude

comparison, it is clear that the general pseudo-inverse control (eq. 4.37) is repeatable

in practice whereas the weighted pseudo-inverse control (eq. 2.46) is not.



























LBC VS CYCLIC MOTION

LBC: LIE BRACKET CONDITION
0.1

0(BOeeOO-OOO OOOOOOOOOeOOOOOOOOOOOOOOOO OGO OO

SI I I N
1 2 3 4 5
CYCLECSYMBOL :GENERAL PSEUDO-INVERSE, SOLID:WEIGHTED PSEUDO- INVERSE)


Fig. 4-5: Lie Bracket Condition Values Through 5 Cycles of the Motion









Table 4-3
Simulation Results Through 5 Cycles of the Motion

General Pseudo-Inverse Equation


19
18
18
18
18
18


PHI1 TH2
0.0000 190.0000
9.9600 190.0127
9.9205 190.0252
9.8817 190.0376
9.8435 190.0499
9.8047 190.0629


TH3 TH4
190.0000 190.0000


189.9615
189.9234
189.8858
189.8487
189.8124


-0.1953 0.0629 -0.006


189.9991
189.9981
189.9972
189.9963
1 8999401


TH5
190.0000
189.9996
189.9992
189.9989
189.9986
18R99972


-0.006 -0.0028


1
1
1
1
1
1


TH6 TH7
.90.0000 190.0000
90.0145 190.0005
90.0288 190.0011
.90.0430 190.0018
.90.0570 190.0025
90.0716 190.0045
0.0716 0.0045


LBC
5.247532e-6
5.251108e-6
5.256534e-6
5.261504e-6
5.265164e-6
5.269522e-6


CY. LBC
0 -7.080107e-01
1 -7.064213e-01
2 -7.011823e-01
3 -6.923246e-01
4 -6.871246e-01
5 -6.826156e-01
DIFF.


Weighted Pseudo-Inverse Equation


PHI1
190.0000
190.8676
191.7274
192.5792
193.4232
194.2600


TH2
190.0000
189.7794
189.5745
189.3841
189.2067
1 9.0409


TH3
190.0000
190.7038
191.3862
192.0502
192.6974
913 3296


4.2600 -0.9591 3.3296


TH4 TH5
190.0000 190.0000
189.9964 190.0549
189.9928 190.1164
189.9880 190.1820
189.9819 190.2508
189.9746 190.3220
-0.0254 0.3220


LBC
-7.080107e-01
-7.064213e-01
-7.011823e-01
-6.923246e-01
-6.871246e-01
-6.826156e-01


CY. LBC
0 5.247532e-6
1 5.251108e-6
2 5.256534e-6
3 5.261504e-6
4 5.265164e-6
5 5269522e-6
DIFF.


CY.
0
1
2
3
4
5
DIFF.


Rpx
-0.75357
-0.75345
-0.75333
-0.75321
-0.75309
-0.75297
0.00078


R,
-0.98664
-0.98659
-0.98654
-0.98650
-0.98645
-0.98645
0.00019


Rpz
-1.69162
-1.69163
-1.69163
-1.69163
-1.69163
-1.69163
0.00001


TH6
190.0000
189.7342
189.4839
189.2479
189.0248
188.8132
-1.1868


TH7
190.0000
190.1011
190.2099
190.3269
190.4515
190.5831
0.5831


CY.
0
1
2
3
4
5
DIFF.


Rpx
-0.75357
-0.75345
-0.75334
-0.75323
-0.75311
-0.75299
0.00058


RN
-0.98664
-0.98643
-0.98637
-0.98632
-0.98626
-0.98621
0.00043


R,
-1.69162
-1.69163
-1.69165
-1.69166
-1.69167
-1.69168
0.00006


1 R.4 1. 062 Q7


.... V V01 V40


193 1296












CHAPTER 5
CONCLUSION



The analysis presented here has provided a new closed-form solution for the

inverse instantaneous kinematic problem of any serial redundant manipulator. An

essential requirement for this solution is to provide the free angles (e ) and spring

stiffnesses (k,). Here a hypothetical stiffness matrix was constructed using joint limit

data of the robot manipulator. Free angles of torsional springs are also conveniently

chosen based on joint limits. This procedure yields a control strategy that facilitates

repeatability in the joint space of a manipulator whose the end effector undergoes

a cyclic type motion. The numerical verification for repeatability is done in terms of

the Lie bracket condition. From the numerical verification and simulation results, it

can be said that the general pseudo-inverse control is repeatable in practice for

different initial configurations whreas the weighted pseudo-inverse and the Moore-

Penrose control are not. The implementation for repeatability was accomplished

successfully on the planar GE 4R manipulator.

A symbolic proof for repeatability will be a future research work.













APPENDIX A
DERIVATION OF INVERSE KINEMATIC EQUATION OF
PRR MANIPULATOR


The kinematic model of a planar PRR manipulator is shown in Fig. 3-10.

The coordinates for a point P in the end effector are

x = ssi + a3c2 + a34c2+3 (A.1)

y = a23s + a34s2+3

The Jacobian matrix is obtained as


[ 1.0
[J] =
.0.0


-a23S2-a34S2+3


The joint spring force acting in the firs

third joints combine to give forces at

as




72 = []r
L 73 J


t joint and joint torques acting in the second,

the end effector, and these can be expressed


(A.3)


Joint torques are related to joint rotations by


-a34s2+3

a^e^+3


(A.2)









f
,2 = [k]
J 3


ss, ss
' SS1 SSolI
02 E02
3 ) 03


where [k] is a 3x3 diagonal matrix of joint stiffnesses as follows


[k] =
[k] = 0
L0


0
0
k3 .


ssi sso0 is the difference between current and the free position displacement while

8, 8o, are the difference between current and free position joint angles, i = 2, 3.

The eq. (A.4) can be written in the form


SS1 SSo1 If
ssl ssol jf
82 -802 = [k]"1 T2
e3 803 .r3 J


Substituting eq. (A.3) into eq. (A.5) yields

SSt SSol
ss, sso1
82- 02 = [k]' [J]T
83 E03

Multiplying eq. (A.6) by [J] yields


El


(A.5)


(A.6)


(A.4)









SSi SSo, ] f 1
[J] e 2- 2 = [J][k][J]T (A.7)
e3 eo f

Rewriting eq. (A.7) yields

SSi SSol
= ( [J][k]. [J]T )- [] e2- 02 (A.8)
L ea03

Taking derivative of eq. (A.6) yields


6ss 1f 1 [f
Se2 = [k]-[JT + [k]-A[6J] (A.9)
683 J

where


A11 A12
[6J]' = A21 A, (A.10)
LA3 A32

Anl = 0.0, A, = 0.0

A21 = (-a232 a342+3) 682 a342+3 683

A22 = (-azs2 a3s2+3) 6e2 a4S2+3 683

A31 = a3,c2+3 682 a34c2+3 683

A32 = a,4s2+3 6e2 a34s2+3 683








Rewriting eq. (A.9) yields

6ss, 6f,
6e, [k] [k][]T k] 7[J]T (A.11)
603, f, ,

Rearranging left hand side of eq. (A.11) yields

6ss, 4 Sf
[T] e = [k]-'[J]T (A.12)
[6e3 j Sf

where

Tn T,1 T1
MT] = T21 T22 T23
T3, T32 T33

TI, = 1.0, TU = 0.0, T3 = 0.0, T21 = 0.0

T, = 1 { (-a23C a3c2+3) f + (-a2s, aMs2) f }/k2

T23 = ( -a3c2+3 f a4S2 3 f, )/k2

T31 = 0.0, T3 = ( -a34C2+3 f a34S23 fy )/k3

T33 = 1 ( -a34C2,3 f a34s2+3 f )/k3

Rewriting eq. (A.12) yields









6ssi f -
682 = [T1 [k]-[J (A.13)
.893 6f

Multiplying eq. (A.13) by [J] yields

6ss, f -
[J] 2, =[J][-[k][J (A.14)


The forward instantaneous motion equation for the manipulator is given by

6ss [Sx
[J] 682 = (A.15)


Substituting eq. (A.15) into eq. (A.14) yields

s6x Sf~
= [C] (A.16)
sy sf, .

where

[C] = [Jl[T]n[k][J]T

Rewriting eq. (A.16) yields


= [C]-' (A.17)
6Sf -y







86


Substituting eq. (A.17) into eq. (A.13) yields the general pseudo-inverse equation


[sse

6D2
-6 3


= [T]-j[k]-'[J]( [J][T]-'[k]'-[]T )-


L Lx

6y


(A.18)


E6x


- [T]i'[k] '[J]T[C]"I










APPENDIX B
DERIVATION OF INVERSE KINEMATIC EQUATION OF
4R MANIPULATOR

The cinematic model of a planar 4R manipulator is shown in Fig. 3-12.

The coordinates for a point P in the end effector are

x = ac + + c+ a341+2+3 + a4c1+2++4, (B.1)

y = ausx + a23s12 + a34s1+2+3 + as4+2+3+4,

a = e1 + e2 + 03 + e4,

where a2, a2, a34, a, represent the length of each link. Eq. (B.1) for an instantaneous

motion can be expressed as

[Sx 6y 6a]T = [J][60x 602 6 63 S]T. (B.2)

The Jacobian matrix is obtained as

J11 J12 J13 J14
[J] = J21 J22 23 24 (B.3)
[1.0 1.0 1.0 1.01


where

J1 = -a2s, a23s1+2 a34s+2+3 a45s+2+3+4,

J12 = -a23S+2 a34s1+2+3 a45s1+2+3+4,

J13 = -a34si+2+3 a45s1+2+3+4

J14 = -a45Sl+2+3+4,

J21 = a12c + a23c1+2 + a34c1+2+3 + a45c1+2+3+4,

J22 = a231+2 + a34l1+2+3 + a45c1+2+3+4,









J23 = a34c1+2+3 + a4,c1+2+3+4*

J24 = a45c1+2+3+4,

Joint torques acting in each joint combine to give forces at the end effector, and

these can be expressed as



T2
= [J] fT (B.4)
7T3
ST4


Joint torques are related to joint rotations by


T1 Ie Oe0

r2 e2 02 (B.5)
= [k]
73 3 e03
T4 0o4


where [k] is a 4x4 diagonal matrix of joint stiffnesses as follows


ki 0 0 0

0 k2 0 0
[k] =
0 0 k3 0
0 0 0 k4


ei eno are differences between current and free position joint angles, i = 1, 2, 3, 4.


The eq. (B.5) can be written in the form










, 001
e2 o02
63 e,3
64 ,04


= [k]-


Substituting eq. (B.4) into eq. (B.6) yields


e o60
02 802

e3 o03
64 e04


= [k]'[J]T


Premultiplying the left and right sides of eq. (B.7) by [J] yields


1 e01

62 02
63 e03
4 ,04


= [J][k]'[J]T


Rewriting eq. (B.8) yields


f] = ([J][k]'-[J]T) [J]

rMo


62 -02
e3 e03
e4 04 -


Taking the derivative of eq. (B.7) yields


(B.6)


(B.7)


(B.8)


(B.9)


f,
rnom,










605 1 j'fx ]

2 = [k]-[Jf]T 6 + [k[6J]T f (B.10)

653 m mo
604


where

6Jn 6Ji2 0.0

65,, 21 ,22 0.0
[6J]T = 2 (B.11)
6J1 6J32 0.0

5J4x 8J42 0.0

6J, = Un1e6 + vn1682 + w11603 + uv1n604,

6J2 = u,260, + v2682 + w,2603 + uv2604,

6J21 = u21,6, + v2,602 + w21683 + uv21,64,

6J,2 = u226O, + v2682 + w2603 + uv22694,

6J31 = u31681 + V31682 + w31603 + uv31(e4,

6J32 = u360 + v3260 + W3263 + UV32 64,

6J41 = u41601 + v4162 + w416%3 + UV41604,

642 = u,6e, + v42602 + 42603 + UV42604,

Un = -ac a23+2 a34c,+2+3 45c1+2+3+4,

U12 =- aC31+2 a34+2+3 a45C1+2+3+4,

13 = a34C12+3 a45c1+2+3+4~ U14 = a45sl+2+3+4,

U21 = -a2s a 1+2 a34s+2+3 a45S1+2+3+4

U22 = a23S1+2 341+2+3 a45S1+2+3+4,










U23 = a34+2+3 a45s+2+3+4, U24 = -a4,5s+2+3+4,

V = 23C1+2 a34c1+2+3 a451+2+3+4-

V12= a231+2 a341+2+3 a451+2+3+4,

V1 = 34C1+2+3 a451+2+3+4, V14 = 45C1+2+3+4,

V21 = 231+2 a341+2+3 45S1+2+3+4,

V22 = a23S1+2 a34s1+2+3 a41+2+3+4,

V23 = a34S1+2+3 a451+2+3+4, V24 = a451+2+3+4,

11 = a34c1+2+3 a45C1+2+3+4, W2 = a34C1+2+3 45c1+2+3+4,

13 =- 341+2+3- a45C1+2+3+4, W14 = 451+2+3+4,

w21= 34S1+2+3- a45s1+2+3+4 W22 = 341+2+3 a45S1+2+3+4

23 =- a34s1+2+3 a4551+2+3+4,. W24 a451+2+3+4,

UV1 = -a45c1+2+3+4, UV2 = 45C1+2+3+4,

UV13 -45C1+2+3+4 UV14 = 845C1+2+3+4,

UV21 45s1+2+3+4, UV22 = a45S1+2+3+4,

UV23 = a45s1+2+3+4, UV24 = 451+2+3+4

Rewriting eq. (B.10) yields


61 [fx 1 [

Se6 [k]-[6J]T = [k]-l[J]T f (B.12)

683 6m
6e4


Rearranging left hand side of eq. (B.12) yields
















Where


I =


[ 6f
= [k]-[J 6f ,
.6mo .


Tl = 1 ( uf + u21f )/k,,

T1 = -( wAf + w,2f )/k1,

T2, = -( u,f, + u22, )/k,,

T23= -( wt + wf22 )/k2,

T31 = -( u13f + uzf )/k3,

T33 = 1 ( w,3 + w, )/k3,

T41 = -( u14fA + U4fy )/k4,

T43 = -( w4f + w4 )/k4,

Rewriting eq. (B.13) yields

668


683
6 e4


= -( Vif +v2f, )/kj,

= -( uvfx + uvf, )/k,,

= 1- ( vf + vf2 )/k2,

= -( uvA2 + uv22f )/k2,

= -( v1fV + vf2 )/k3,

= -( uv13fA + uv23, )/k3,

= -( vlf~ + V24f )/k4,

= 1- ( uv4 + uv24 )/k4,,


6fx


6


6681
682
683
s.e4


(B.13)


(B.14)









Premultiplying the left and right sides of eq. (B.14) by [J] yields


681
602
683
. 6e4 .


= [J][A]-'[J]T


6ff]
[sm 6fj


(B.15)


(B.16)


Substituting eq. (B.2) into eq. (B.15) yields

6x 6' '
Sy = [C] 6f4 ,
6a 6rno.

where

[C] = [J][A]'[J]T.

Rewriting eq. (B.16) yields


6f, 6x
S6f = [C]-1 6y .
n. o 6a


(B.17)


Substituting eq. (B.17) into eq. (B.14) yields the general pseudo-inverse equation



6,2 = [A]-[J]T( [J][A]-[J]T )- Sy (B.1
603 6a
604


8)












APPENDIX C
DERIVATION OF FORWARD KINEMATIC EQUATION OF
7R MANIPULATOR (SSRMS)



The vector loop equation of the equivalent closed loop spatial mechanism in the

first coordinate system can be written by (see set 1 in Table 4-2, [12])

Rp() = S2(I) + a23,) + S3(1) + a4a,(1) + S44-) + a455) + S5w(1) +

a.56(1) + S666s'(1 + SS,7(1) + a7,a78' (C.1)

S2x) = 0, S2() = s1, S(= c2 (C.2)

a = 2, a23) U21, 23 U21 (C.3)

U21" = S2C2, U21 = S2S2

S3() = X2 S31) Y2 S3z )= Z2 (C.4)
X2= 232 2= (sc,23 + CUS2C2 )
Z2= C223 ss23C2

a34x(1)= W32, a -U321 a3) U321 (C.5)

U31 = U3212- V32SU, U321' = U32s2 + V32c

U32 = S3S3, V32 = ( 2C3 + c2s33 )

W, = c2c3 Sz3c2

S4x() = X2, S4y) = Y32, S4 ) = Z32 (C.6)

X32 = X3 C Y3 S2, Y32 = C(X3 S2 + Y3 c2) Z3

Za2 = s((X3 S2 + Y3 c2) + CUZ3