• TABLE OF CONTENTS
HIDE
 Title Page
 Dedication
 Acknowledgement
 Table of Contents
 Abstract
 Introduction
 Obstacle expansion
 Wrist center locale finding
 Collision-free path finding
 Conclusions and recommendation...
 Reference
 Biographical sketch
 Copyright














Title: Collision-free path finding for two cooperative spatial manipulators
CITATION THUMBNAILS PAGE IMAGE ZOOMABLE
Full Citation
STANDARD VIEW MARC VIEW
Permanent Link: http://ufdc.ufl.edu/UF00090189/00001
 Material Information
Title: Collision-free path finding for two cooperative spatial manipulators
Series Title: Collision-free path finding for two cooperative spatial manipulators
Physical Description: Book
Creator: Chen, Jau-Liang,
 Record Information
Bibliographic ID: UF00090189
Volume ID: VID00001
Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
Resource Identifier: alephbibnum - 001638749
oclc - 24160854

Table of Contents
    Title Page
        Page i
    Dedication
        Page ii
    Acknowledgement
        Page iii
    Table of Contents
        Page iv
        Page v
    Abstract
        Page vi
        Page vii
    Introduction
        Page 1
        Page 2
        Page 3
        Page 4
        Page 5
        Page 6
        Page 7
        Page 8
        Page 9
        Page 10
        Page 11
    Obstacle expansion
        Page 12
        Page 13
        Page 14
        Page 15
        Page 16
        Page 17
        Page 18
        Page 19
        Page 20
        Page 21
        Page 22
        Page 23
        Page 24
        Page 25
        Page 26
        Page 27
        Page 28
        Page 29
        Page 30
        Page 31
        Page 32
        Page 33
        Page 34
        Page 35
        Page 36
        Page 37
        Page 38
        Page 39
        Page 40
        Page 41
    Wrist center locale finding
        Page 42
        Page 43
        Page 44
        Page 45
        Page 46
        Page 47
        Page 48
        Page 49
        Page 50
        Page 51
        Page 52
        Page 53
        Page 54
        Page 55
        Page 56
        Page 57
        Page 58
        Page 59
        Page 60
        Page 61
        Page 62
        Page 63
        Page 64
        Page 65
        Page 66
        Page 67
        Page 68
        Page 69
        Page 70
    Collision-free path finding
        Page 71
        Page 72
        Page 73
        Page 74
        Page 75
        Page 76
        Page 77
        Page 78
        Page 79
        Page 80
        Page 81
        Page 82
        Page 83
        Page 84
        Page 85
        Page 86
        Page 87
        Page 88
        Page 89
        Page 90
        Page 91
        Page 92
        Page 93
        Page 94
        Page 95
        Page 96
        Page 97
        Page 98
        Page 99
        Page 100
        Page 101
        Page 102
        Page 103
        Page 104
        Page 105
        Page 106
        Page 107
        Page 108
    Conclusions and recommendations
        Page 109
        Page 110
    Reference
        Page 111
        Page 112
        Page 113
        Page 114
        Page 115
    Biographical sketch
        Page 116
        Page 117
        Page 118
    Copyright
        Copyright
Full Text












COLLISION-FREE PATH FINDING
FOR
TWO COOPERATIVE SPATIAL MANIPULATORS
















BY

JAU-LIANG CHEN


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


1990



















TO MY WIFE MEI-HSIANG AND DAUGHTER KATHERINE














ACKNOWLEDGEMENTS


The author would like to express his sincere appreciation

to his adviser Dr. Joseph Duffy for his guidance and support

of this work. He would like to thank Dr. Ali Seireg, Dr. John

Staudhammer, Dr. Gary K. Matthew and Dr. Carl D. Crane for

their help and participation in his committee.

The author would also like to thank all his colleagues

in CIMAR for sharing their knowledge and pleasure.

Special thanks are extended to Mr. and Mrs. Edward

Gilbert, without them this work cannot be presented.

Finally, the author wishes to thank his parents, parents-

in-law, and wife, Mei, for their encouragement, spirit and

patience.


iii















TABLE OF CONTENTS


page

ACKNOWLEDGEMENTS.. ..... ......................... iii

ABSTRACT........................................... .. vi

CHAPTERS

I INTRODUCTION...................................... 1

1.1 Literature Review........................... 2
1.1.1 Obstacle Avoidance................... 3
1.1.2 Multi-robot System................... 6

1.2 Objective of Research....................... 7

1.3 Problem Statement........................... 7

1.4 Assumptions................................. 7

1.5 Methodology.................................. 8

II OBSTACLE EXPANSION... ............................. 12

2.1 Expansion of Objects........................ 12
2.1.1 Expanding the Vertical Sides.......... 13
2.1.2 Expanding the Top Surfaces............ 17

2.2 Identification of Obstacles................. 23

III WRIST CENTER LOCALE FINDING..................... 42

3.1 Construction of Vertical Planes............. 42

3.2 Determination of Collision-Free Area (CFA).. 45

3.3 Determination of Wrist-Center Area (WCA).... 54
3.3.1 WCA for the First Robot.............. 57
3.3.2 WCA for the Second Robot............. 59

IV COLLISION-FREE PATH FINDING...................... 71

4.1 Collision Between End Effectors, Grasped
Workpiece and Obstacles..................... 72

iv









4.1.1 Identification of Obstacles........... 73
4.1.2 Perspective Projection............... 75

4.2 Collision Checking Between Two Robots Links. 77
4.2.1 Two Planes Intersect at One Line..... 78
4.2.2 Two Planes are Coincident............ 83
4.2.3 Two Planes are Parallel.............. 84

4.3 Collision-Free Path Generation.............. 85

4.4 Computer Graphics Simulation................ 87

V CONCLUSIONS AND RECOMMENDATIONS.................. 109

REFERENCES............................................. 111

BIOGRAPHICAL SKETCH................................... 116














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


COLLISION-FREE PATH FINDING
FOR
TWO COOPERATIVE SPATIAL MANIPULATORS

By

iJau-Liang Chen

December, 1990

Chairman: Dr. Joseph Duffy
Major Department: Mechanical Engineering


This study describes the development of finding

collision-free paths for a pair of cooperative spatial robot

manipulators as their end effectors grasp a workpiece in an

obstacle-strewn environment. After the initial and goal

positions of the wrist center are specified, a collision-free

path for this pair of manipulators to move the workpiece

safely to the final destination is generated.

It is assumed that the surrounding objects are composed

of vertical sides with either a flat top surface or an

inclined top surface, and their locations, sizes and shapes

are known. The objects are then expanded according to the

dimension of the links of the manipulators so that all the

links can be treated as straight line segments. Objects which








lie within the swept volume are identified as obstacles to the

movement of these two manipulators.

A series of vertical planes for each individual

manipulator is established to reduce the geometric problem

from three dimensions to two dimensions. Each of the planes

contains a vertex of the top surface of the expanded obstacles

and is offset a fixed distance from the first joint axis of

the robot. These planes generate cross-sections of the

obstacles and wrist reachable workspace.

In each cross-section of the wrist reachable workspace,

a wnst center locale (WCL) is determined so that an intermediate

position of the wrist center can be selected within the WCL.

During the selection of the intermediate position, the

collision between end effectors, the grasped workpiece and

the obstacles need to be considered. The intersection between

the two manipulators links is also taken into account during

the selecting procedure.

After all the intermediate positions are found, a path

which connects these intermediate positions together with the

initial and goal positions is modified to produce a smooth

collision-free path. Here, the algorithm is applied to the

PUMA robots and is demonstrated via computer graphics

animation on a Silicon Graphics IRIS 4D/70GT workstation.


vii














CHAPTER I

INTRODUCTION



Automation has become a necessary element in modern

manufacturing processes. Many of these processes are complex,

such as multi-part assemblies, fabrication of difficult

geometric shapes, and the handling of very heavy parts. Along

with the development of high level machine intelligence for

second and third generation robots, there is need to develop

robotic systems with multi-arm configurations for better

manipulation.

If we examine the way the human body adapts to

manipulative activity, we find that each arm and hand can be

used independently with skill and efficiency. Both sets of

arms and hands, however, can accomplish much more than double

the work of one. Working together, the two have a qualitative

effect that makes it possible to perform tasks impossible to

execute with a single arm and hand.

In similar fashion, a multi-robot system (MRS) offers

flexibility and an ability to process parts which would be too

heavy to be handled by a single arm. Multi-robot systems increase








2

productivity through parallelism, and provide flexibility to

accommodate diversified applications.

MRS offers other advantages, making it worthy of

consideration, even when the conventional single arm system

could be used. The multiple grasping of a workpiece by

several arms transforms the serial configuration of individual

robots into a closed kinematic chain. This creates a

structure far more rigid than its open-chain counterpart,

thereby increasing the operational reliability of a system.

In the previous work [Chen and Duffy 1988a, 1988b], the

motion capability and ranges of end effector orientations for

two cooperative robots, which grasp a workpiece and move it

along a straight line path, were discussed. This research is

a continuation of that study, and focuses on obstacle

avoidance.



1.1 Literature Review

In Section 1.1.1, a brief review of the literatures on

obstacle avoidance is presented. As far as the author aware,

no other work has appeared in literature concerning the

problems associated with two cooperative robots grasping a

workpiece while avoiding collision with the obstacles in the

environment. Most of the studies on the multi-robot system can be

divided into two groups. The first group considered the

collision avoidance between robots. Each robot performs








3

different tasks. The second group discussed multiple robots

grasp a workpiece, but only deals with the control issue. The

discussion of those studies on the multi-robot system is presented

in Section 1.1.2.



1.1.1 Obstacle Avoidance

The most common robot motion is transfer motion. In the

execution of transfer motion, the robot and the workpiece it

carries must be able to avoid collision with any obstacle that

may lie within the robot working area. The capability to plan

motions that avoid such obstacles is, therefore, essential to

task planning. Several obstacle avoidance algorithms have

been proposed. In this section, those studies which dealing

with obstacle avoidance in three dimensions are reviewed.

Until recently, most of this research could be

characterized as a "hypothesize and test" algorithm [Pieper

and Roth 1968, Boyse 1979, Ahuja et al. 1980]. Such an

algorithm uses three steps repeatedly: first, it hypothesizes

a candidate path; second, it checks this path for possible

collision; third, if a collision can occur, the path is then

modified. The main advantage of the hypothesize and test

technique is its simplicity. This method works fairly well

in rather uncluttered environments where crude heuristics can

be used to generate candidate paths. When the space is








4

cluttered, however, attempts to avoid collision with one

obstacle will typically lead to collision with another.

A more recent approach is based on the "configuration

space" first introduced by Udupa [1977] and then developed by

Lozano-Perez and Wesley [1979], Lozano-Perez [1987, 1986,

1983, 1981]. Basically, this approach consists of shrinking

the moving object to a point, while expanding obstacles

inversely to the original shape of the moving object. Then,

finding a path for the moving object is equivalent to finding

a path for a point among the expanded obstacles. Path finding

can be accomplished with a search algorithm operating on a

graph that represents interconnections between free-space

cells. This method is well suited for Cartesian manipulators,

but its extension to rotating movement is complicated [Chien

et al. 1984, Brooks and Lozano-Perez 1985]. If the

manipulator has many rotational degrees of freedom, selection

of a trial collision-free path is complicated. In the case

of a manipulator with many rotational links, this algorithm

utilizes many rough approximations and may, therefore, fail

to find collision-free paths even when they exist.

Another method, by Brooks [1983], represents free space

by a structured set of "freeways." Each freeway is modeled

using generalized cones. Then, "legal motion segments"

(possible collision-free paths) for the moving object are

defined by translations and possible reorientations along the

"freeway spines" (outer boundary of the cone). This method








5

has been extended to find collision-free paths involving four

degrees of freedom [Brooks 1984]. The method works fast in

a relatively uncluttered environment. This approach, like the

"configuration space" solution, has difficulty in a

complicated environment and may fail to uncover a possible

collision-free path.

Khatib [1985] proposed a penalty function approach which

satisfies the definition of a "potential field." The gradient

of this field, at a selected point on the robot, is

interpreted as a repelling force acting on that point. The

destination, on the other hand, is assumed to generate an

attracting force to the selected point. A collision-free path

is found after accounting for interactions between these two

forces, subject to the kinematic constraints. The penalty

function approach finds safe paths based on local information.

The procedure does not account for global information, thus

leading to situations where further progress is impossible.

This approach is more suitable for applications that require

only small modifications to a known path.

Different approaches to this task can also be found in

[Faverjon 1984, Wong and Fu 1985, Laugier and Germain 1985,

Canny 1986, Young 1986, Hasegawa and Terasaki 1988, Jun and

Shin 1988, Cameron 1990, Choi 1990].










1.1.2 Multi-robot System

There is growing interest in the development of multi-robot

systems. The potential of such systems covers a wide range of

applications extending from assembly tasks in automated

manufacturing to deep-sea exploration and service tasks in

outer space. These applications are complex and require

dexterous manipulation. Robot systems with two or more arms

are necessary.

Robot systems with two arms working in the same space

have been investigated by several researchers [Acker et al.

1985, Freund and Hoyer 1986, 1985, Lee 1986, Lee and Lee 1987,

Roach and Boaz 1987, Nagata et al. 1988]. In these studies,

each robot performed an independent working sequence rather

than a coordinated task. The primary interest of these

studies was collision avoidance between robots. Because each

of the two robots had an independent working sequence, there

was no kinematic relationship between their motion vectors.

If one robot had a closely coordinated motion with the other,

an important kinematic relationship would exist.

Since the 1970s, several researchers [Fujii and Kurono

1975, Ishida 1977, Alford and Belyeu 1984, Luh and Zheng 1987,

1986, Tarn et al. 1986, Tournassoud 1987, Lee and Lee 1988,

Benhabib et al. 1988, Tricam et al. 1988, Tao and Luh 1989,

Tao et al. 1990] have focused attention on the use of two

manipulators to handle a single workpiece. However, they have








7

almost exclusively dealt with control issues, leaving the

kinematic and geometric aspects virtually untouched.

This research will study collision avoidance for two

cooperative robots handling a workpiece within an obstacle-

strewn environment.



1.2 Objective of Research

The objective of this research is using a simple

geometric model to produce an algorithm which can be executed

quickly to find collision-free paths for two cooperating robot

manipulators. The algorithm will be applied to two PUMA

robots in an obstacle-strewn environment.



1.3 Problem Statement

The problem of this study can be stated as follows:

Given: The wrist center positions, and the end effector

orientations at the initial and the final positions

of a pair of cooperative robots.

Find: Paths for this pair of robots to move a workpiece

safely to a destination position without colliding

with obstacles in the environment.



1.4 Assumptions

Several assumptions have been made in order to simplify

the problem.








8

1) The surrounding objects have vertical sides with an

inclined top surface. The location and geometric description

of each object are known.

2) The initial and goal positions of each wrist center

are obstacle free.

3) The end effectors firmly grasp the workpiece, and

no slippage will occur during movement. The orientation of

the workpiece is the same as that of the end effector.

4) The joint angle limitations are considered

throughout the analysis.

5) The grasped workpiece is a long rectangular bar and

its cross-section dimensions are smaller than those of the

robot arm.



1.5 Methodology

Several terms are used repeatedly throughout the study:


Collision-free area (CFA):
An area within which the wrist center of an
individual robot can be located to avoid collision
between the robot arm and obstacles.


Wrist center area (WCA):
An area within which the wrist centers of the
cooperative robots can be located.


Wrist center locale (WCL):
An area within which the wrist centers of the
cooperative robots can be located to avoid collision
between the robot arms and obstacles.










Swept volume:
The space under the area swept by a robot arm along
a predetermined path.


The method for determining a collision-free path can be

summarized as follows:

1) Expand the surrounding objects.

2) Construct a straight line path for the wrist center

of the first robot, from the initial to the final positions.

The wrist center path of the second robot is determined

according to the end effector orientation and wrist center

position of the first robot.

3) Identify those obstacles that fall within or

intersect the swept volume of both robots.

4) Establish a series of vertical planes* for each

robot, to reduce the problem from three dimension to two

dimensions. Each vertical plane includes a vertex of the

obstacle's top surface and is offset a fixed distance from the

first joint axis of the robot.

5) Identify the collision-free area (CFA) on each vertical

plane.

6) Determine the wrist center area (WCA), keeping the

distance between the two wrist centers constant.





For the PUMA robot, those vertical planes are parallel
to the central axis of the first joint.








10

7) Superimpose (logical intersection) the CFA (5

above) and the WCA (6 above) to find the wristcenter locale (WCL).

8) Locate intermediate wrist center positions by

examining the intersection between the end effectors, the

grasped workpiece, and the obstacles.

9) Re-examine for possible collision between

manipulators' links when selecting intermediate positions.

10) Connect initial, intermediate, and goal positions

to produce a collision-free path, after locating intermediate

positions.

The two cooperative robots are positioned in locations

to facilitate development of the algorithm. The coordinate

system is constructed with its origin at the center of the

first robot's base. The Z-axis coincides with the central

axis of the first joint of the first robot, and the center of

the second robot's base is on the positive X-axis (see Fig.

1.1).

An algorithm for expanding objects and identifying those

objects which are obstacles is developed in Chapter II.

Chapter III describes the procedure of finding a wrist center locale

(WCL) on each vertical plane. Chapter IV describes the

determination of intermediate positions of wrist paths. That

chapter also examines the possibility of collision between two

manipulators links. Finally, some recommendations for future

study are listed in Chapter V.
















z
1'


Relative position of the PUMA robots.


Figure 1.1














CHAPTER II

OBSTACLE EXPANSION



Collision avoidance between a solid link and objects

scattered within a robot's working area is a complex problem.

To simplify the problem, each object is expanded by a factor

derived from the thickness and width of the link, so that all

the links can be treated as line segments. The problem can

then be reduced to locating intersections of the line segments

with expanded objects. Section 2.1 describes the expansion

in detail.

Section 2.2 develops an algorithm to determine whether

an expanded object will be an obstacle to the movement of each

robot. A straight line is used as a predetermined path for

the wrist center movement of each of the two cooperating

robots, moving from an initial position to the target

position.



2.1 Expansion of Objects

Figure 2.1 shows a PUMA 600 robot and the cross-section

of an object. In the case of a single robot, each object is

expanded according to the relationship between the links of

the robot and sides of the object. For example, if any








13

portion of the sides of an object can be reached by the upper

arm, these sides are expanded by a factor derived from the

dimensions of the upper arm. Otherwise, they are expanded by

a factor derived from the dimension of the forearm.'

For two cooperative robots, each object is expanded by

an amount calculated from the thickness and width of the upper

arm. Figure 2.2 illustrates the upper arm of the PUMA robot.

Because of its irregular form, it is difficult to use this

shape in calculating the expansion. To reduce complexity,

this arm is treated as a parallelepiped, utilizing its largest

dimension (see Fig. 2.2).

Figure 2.2 shows the cross-section of the upper arm.

Point P is the intersection point of the cross-section A-A and

the upper arm axis. This axis connects the centers of joints

2 and 3. H is the distance from point P to the lower corner

of the cross-section. V is the perpendicular distance from

point P to the base.

The expansion of the vertical sides of an object is

discussed in Section 2.1.1. The expansion of the top surface

of an object is discussed in Section 2.1.2.



2.1.1 Expanding the Vertical Sides

The vertical sides of an object are expanded by H. Each

vertex is then expanded to form two corners (see Fig. 2.3).


SFor details see Tseng 1987 pp. 11-24.








14

The coordinates for the new corners are calculated in two

steps. The first is to find the X and Y coordinates, and the

second is to find the Z coordinate.



2.1.1.1 Determine the X and Y coordinates

To facilitate calculation, the object is projected onto

the X-Y plane as shown in Fig. 2.4. Points A, B, C, and D are

the projections of the vertices, indexed sequentially in a

counterclockwise direction. The vertical sides are projected

as the line segments AB, BC, CD, and DA.

The line equations for lines AB and BC are given as



Li := X mY = n, (2.1)

Le := X pY = q, (2.2)

where m = (XA XB) / (YA YB)

n = XA m YA

p = (Xc XB) / (Yc YB),

q = XB p YB,



and (XA, YA), (XB, YB), and (Xc, Yc) are the X and Y coordinates

of points A, B, and C.

When the direction of the cross products OA x AB and OB

x BC point to the positive Z-axis (see Fig. 2.5a), the

equations for lines DE and EF are given by








15

L, := X mY = n + H (1 + m2)i/2, (2.3)

L4 := X pY = q + H (1 + p2)1/2. (2.4)



Otherwise (see Fig 2.5b), the equations for lines DE and

EF are given by



L, := X mY = n H (1 + m2)1/2, (2.3a)

L4 := X pY = q H (1 + p2)1/2. (2.4a)



Solving either pair of the simultaneous equations, the

coordinates of the intersection point E are



XE = (a, p b m) / (p m), (2.5)

YE = (a b) / (p m), (2.6)


where a, = n + H (1 + m2)/2,

b, = q H (1 + p2)12.



The (-) sign is used when lines DE and EF are defined by

Eqs. (2.3a) and (2.4a).

The equation for line BE can be expressed as



L, := X tY = s, (2.7)

where t = (XE XB) / (YE Y)

s = X, t YB.








16

Point I lies on line BE with a distance H from point B

and its coordinates are calculated as follows:



X, = X, + (XB XE)R, (2.8)

Y, = YB + (YB YE)R, (2.9)

where R = H [(XB XE)2 + (YB YE)2]1/2



The equation for line L6, which passes through point I

and is perpendicular to line L5, can be expressed as follows:



L6 := tX + Y = c, (2.10)

where c = t X, + Y,.



The coordinates of point N, (N2) are found by solving Eqs.

(2.3) and (2.10) ((2.4) and (2.10))



XN = (a, + c m) / (1 + t m), (2.11)

YNI = (c a, t) / (1 + t m), (2.12)

and XN2 = (bI + c p) / (1 + t p), (2.13)

YN2 = (c bi t) / (1 + t p). (2.14)


2.1.1.2 Determine the Z coordinate

Since the new corners lie on the same plane as the

original before expansion of the top surface, the Z coordinate

of each new corner can be computed as follows:








17

a) The equation of the plane containing all these

vertices can be expressed by



LX + MY + NZ P = 0, (2.15)

where L = (YA- YB) x (Zc Z) (Yc YB) x (ZA Z),

M = (ZA ZB) x (Xc XB) (Zc ZB) x (XA XB),

N = (XA XB) x (Yc Ya) (Xc XB) x (YA- YB),

P = LXA + MYA + NZA,



and (XA, YA, ZA) (XB, YB, Z) and (Xc, Yc, Zc) are the

coordinates of any three vertices A, B and C on the top

surface of the original object.

b) The Z coordinate of each new corner can then be

calculated as follows:



Z, = (P LXNi MYN) / N, i = 1, 2 (2.16)



where (XNi, Y,) are the X and Y coordinates of each new corner

found by Eqs. (2.11), (2.12), (2.13) and (2.14).



2.1.2 Expanding the Top Surface

In the previous section, coordinates for the corners of

the expanded side surface were calculated. The next step is

to expand the top surface along the Z-direction. This

expansion must recognize the position of the upper arm axis

relative to the top surface of the objects.








18

Figure 2.6 shows positions of the upper arm relative to

the top surface of two different objects. In both (a) and

(b), the axis of the upper arm is parallel to the slope of the

top surface of the object. If each top surface is expanded

by the amount of V/cosa, where a is the angle of the slope of

the top surface, the relative position the upper arm and the

object's top surface will remain the same.

Figure 2.7 shows two examples where the axis of the upper

arm is not parallel to the slope of an object's top surface.

When the top surface is expanded by V/cosa, a leading angle

0 is formed, due to side expansion. Therefore, the line

segment representing the upper arm will strike the expanded

object before it hits the real object.

According to the relative position of the upper arm to

the object, there are three different cases:

Case 1--The upper arm collides with the near corner2 of

the object.

Case 2--The upper arm collides with the far corner3 of

the object.

Case 3--The upper arm collides with the top surface of

the object.






2 The corner on the side closest to the robot's first joint.


3 The corner on the side away from the robot's first joint.









19

In the following discussion, the angle 0 and points O,

P, R, P' and R' are defined as follows:

0 -- Rotational angle of the upper arm, measured from the

X-axis.

O -- Center of joint 2.

P -- Point where the upper arm hits the original object.

R -- Point on the central axis of the upper arm

corresponding to point P.

P' -- Point where the line segment representing the upper

arm hits the expanded object.

R' -- Intersection point between the central axis of the

upper arm and the expanded top surface.



Case 1. The upper arm collides with the near corner of the
object.


Figure 2.7 shows two examples of case 1. In Fig. 2.8,

the X coordinate of points P' and R' are calculated as

follows:



X, = Xp PR'sin[(O+a)/2], (2.17)

Xp, = Xp H, (2.18)

where PR' = V / cos[(O-a)/2],



and Xp is the X coordinate of point P.

If X, is less than Xp,, the upper arm will hit the

original object before the line segment, representing the








20

upper arm, hits the expanded object as shown in Fig. 2.9.

From Eqs. (2.17) and (2.18),



H < V a / b, (2.19)

where a = sin[(8+a)/2],

and b = cos[(6-a)/2].



Expressing Eq. (2.19) as a function of 6 yields



tan(0/2) > (a' / b'), (2.20)

where a' = Hcos(a/2) Vsin(a/2),

and b' = Vcos(a/2) Hsin(a/2).



Solving Eq. (2.20) for 0 yields



0 > 2 arctan(a'/b'). (2.21)



Figure 2.10 shows the boundary value of 0 as a changes

from -90 degrees to 90 degrees according to Eq. (2.21).

Figure 2.10 also shows that the boundary value of 0 is always

greater than 90 degrees. If 0 is greater than 90 degrees, the

upper arm will not hit the object. However, if the upper arm

hits the object, 0 must be less than 90 degrees, and X, must

be greater than X,. From the sine rule,


P'R' OR'
S- (2.22)
sing sin(7-0+a-0)












Solving Eq. (2.22) for 0 yields


P'R'sin(0-a)
S= arctan[ ]. (2.23)
OR' P'R'cos(O-a)



Case 2. The upper arm collides with the far corner of the
object.

Figure 2.11 shows two examples of case 2. In Fig. 2.12,

the X coordinates of points R' and P' are calculated as

follows:



X, = Xp + PR'sin[(O+a)/2], (2.24)

Xp, = Xp + H, (2.25)

where PR' = V/cos[(O-a)/2].



If X, is greater than X,, the upper arm will hit the

original object before the line segment hits the expanded

object as shown in Fig. 2.13. From Eqs. (2.24) and (2.25),



H < V a / b, (2.26)

where a = sin[(8+a)/2],

b = cos[(8-a)/2].








22

Expressing Eq. (2.26) as a function of 0 yields



tan(0/2) > (a'/b'), (2.27)

where a' = Hcos(a/2) Vsin(a/2),

b' = Vcos(a/2) Hsin(a/2).



Solving Eq. (2.27) for 6 yields



0 > 2 arctan(a'/b'). (2.28)



The result (2.28) is the same as in Case 1. Hence a

leading angle 0 exists and is calculated by the same method

as described in Case 1.



Case 3. The upper arm collides with any part of the top
surface of the object.

Figure 2.14 shows an example of case 3. In Fig. 2.15,

the X coordinates of points R and R' can be calculated as

follows:



X, = Xp a V, (2.29)

X, = Xp b V, (2.30)

where a = sinO,

b = sin[ (+a)/2]/cos[(O-a)/2].








23

If Xr is less than X,,, the upper arm will hit the object

before the line segment hits the expanded object, as shown in

Fig. 2.16. From Eqs. (2.29) and (2.30), this situation will

happen only when a is greater than b. Table 2.1 shows the

values of (a b) for different values of a and 0.4 From this

table, a is always less than b. Hence a leading angle must

exist.



All three cases demonstrated that V/cosa can be used as

the expansion dimension for the top surface of the object.

This value is added to the Z coordinate calculated by Eq.

(2.16). The Z coordinate of each vertex, after expansion, is

then given by



ZN = Zn + V / cosa. (2.31)



Figure 2.17 illustrates an example of the object before

and after the expansion. The upper arm and forearm become two

line segments that lie on the same plane, with an offset to

the first joint (see Fig. 2.18).



2.2 Identification of Obstacles

This study assumed that the wrist center of the first

robot moves along a straight line path, from initial to final


4 Note that in this case -n/2 < 0 < a.








24

position. Further, the study recognizes that there may be a

number of points along this path that are inaccessible to a

wrist center and/or points where an upper arm or forearm may

collide with an obstacle. In these cases, the wrist center

position needs to be relocated such that it can be reached by

the wrist center and neither the forearm nor the upper arm

collides with the obstacle. The relocating of the wrist

center position will be discussed in Chapter IV.

Objects in the environment were expanded in Section 2.1.

Only those objects which lie within the sweptvolume formed by

the robots will be considered as obstacles in path finding.

The following procedure is used to determine which

objects are classified as obstacles:

1. Project the expanded object and the swept volume onto

the X-Y plane. Each object is evaluated in terms of two pairs

of angles on this plane. For example, [Oi, Of] and [08, 8!]

are used to evaluate object A (see Fig. 2.19). These angles

are measured between the offset of the robot and the X-axis

as shown in Fig. 2.19. The angles Bi and O8 indicate contact

with object A (as the robots approach A), where O, and 08

indicate the robots are clear of object A. The two pairs of

angles are used to denote the angular range of object A.









25

The 0 angles (0 and 0') are calculated by



0 = 6 + 4, (2.32)

where 6 = arctan(Y,/Xr), (2.33)

S= arccos[S/(X +Y2)12], (2.34)



and (Xr, Y,) are the X and Y coordinates of the vertex, and S

is the offset.

The angular range of the swept volume of the left hand

robot is represented by [a, B], and the angular range of the

swept volume of the right hand robot is represented by [B', a'].



2. In order to be classified as an obstacle, the angular

range of an object must satisfy one of the following

conditions:

a) Lie within the angular range of the swept volume of

both robots (for example, objects A, B, and C in Fig 2.19).

b) Lie within the angular range of the swept volume of one

of the robots and overlap with another (for example, object

D in Fig. 2.19).

c) Overlap the angular range of the swept volume of both

robots (for example, object E in Fig. 2.19).

Example: In Fig. 2.19, object F does not satisfy any of

the three conditions. Therefore, it is not counted as an

obstacle.








26

The objects in the environment were expanded according

to the dimensions of the robot. The upper arm and the forearm

of the robot becomes two line segments after the expansion of

objects. Those expanded objects which affect the motion of

the two cooperative robots were identified as obstacles. The

collision checking can then be accomplished by checking the

intersection between the line segments with the obstacles.

It will be discussed in the next chapter.






Table 2.1 Values of (a-b) for different a and 8.


aUl -85 -80 -70 -60 -50 -40 -30 -20 -10

-80 -0.0038 0

-70 -0.0115 -0.0152 0

-60 -0.0193 -0.0306 -0.0299 0

-50 -0.0275 -0.0465 -0.0603 -0.0437 0

-40 -0.0361 -0.0632 -0.0916 -0.0882 -0.0562 0

-30 -0.0454 -0.0810 -0.1245 -0.1340 -0.1133 -0.0670 0

-20 -0.0555 -0.1003 -0.1595 -0.1820 -0.1722 -0.1351 -0.0758 0

-10 -0.0669 -0.1216 -0.1975 -0.2332 -0.2340 -0.2053 -0.1527 -0.0822 0

0 -0.0799 -0.1457 -0.2395 -0.2886 -0.2997 -0.2788 -0.2321 -0.1657 -0.0862

10 -0.0951 -0.1737 -0.2870 -0.3501 -0.3711 -0.3572 -0.3152 -0.2518 -0.1737

20 -0.1136 -0.2070 -0.3420 -0.4196 -0.4501 -0.4423 -0.4038 -0.3420 -0.2639

30 -0.1368 -0.2480 -0.4076 -0.5000 -0.5394 -0.5364 -0.5000 -0.4382 -0.3584

40 -0.1674 -0.3008 -0.4885 -0.5959 -0.6428 -0.6428 -0.6064 -0.5425 -0.4592

50 -0.2104 -0.3724 -0.5924 -0.7141 -0.7660 -0.7660 -0.7267 -0.6580 -0.5686

60 -0.2764 -0.4771 -0.7334 -0.8660 -0.9180 -0.9129 -0.8660 -0.7885 -0.6896

70 -0.3931 -0.6481 -0.9397 -1.0723 -1.1133 -1.0940 -1.0321 -0.9397 -0.8264

80 -0.6620 -0.9848 -1.2764 -1.3737 -1.3785 -1.3268 -1.2368 -1.1199 -0.9848









Table 2.1 Continued.


a e 0 10 20 30 40 50 60 70 80

0 0

10 -0.0875 0

20 -0.1763 -0.0861 0

30 -0.2680 -0.1737 -0.0822 0

40 -0.3640 -0.2639 -0.1657 -0.0758 0

50 -0.4663 -0.3584 -0.2518 -0.1527 -0.0670 0

60 -0.5774 -0.4592 -0.3420 -0.2321 -0.1351 -0.0562 0

70 -0.7002 -0.5686 -0.4382 -0.3152 -0.2053 -0.1133 -0.0437 0

80 -0.8391 -0.6957 -0.5425 -0.4038 -0.2788 -0.1722 -0.0882 -0.0299 0


























Figure 2.1 PUMA and the cross-section of an object.


A ~-


2


A No


3


P
H
]


.-4.
A-A
cross-section


Figure 2.2 The upper arm of PUMA robot.


11.75"


11


I ~ I


















L /U A I
H F
M E

Figure 2.3 Expansion of vertical side.


Figure 2.4 Projection onto the X-Y plane.





























AC


Figure 2.5 Determination of points N1 and N2.






















































(b)

Figure 2.6 Expansion of the top surface





























(a)






















(b)

Figure 2.7 Upper arm hits the near corner of the object.







































2-
2


Figure 2.8 Calculate 0 for Case 1.































































Figure 2.9 Xr, < X,.








240 -

230 -

220

210

200 -

190

180

170 -

9 160

150

140

130

120

110

100 -

90


-90 -70 -50 -30 -10 10 30 5 70 90
-90 -70 -50 -30 -10 10 30 50 70 90


Figure 2.10 Boundary values of 0.



















































































Figure 2.11 Upper arm hits the far corner of the object.


2-_






















































2


2e
2 0


Figure 2.12 Calculate 0 for Case 2.










Y



0 x





R


P


Figure 213 X > X


Figure 2.13 Xr, > 0.


Figure 2.14 The upper arm hits the top surface.




























































Figure 2.15 Calculate 0 for Case 3.








40

Y



X





R R'
P'

PFigure 2.16 X





Figure 2.16 X, > X,,.


Figure 2.17 Object before and after expansion.








41











i e 2 Lie s ffret S









Figure 2.18 Line segments represent the PUMA robot.


wrist center


Figure 2.19 Identification of obstacles.














CHAPTER III

WRIST CENTER LOCALE FINDING



In the preceding chapter, objects were expanded and

obstacles were identified. In this chapter, a series of

vertical planes will be used to determine the wrist center locale

(WCL) for each robot. Section 3.1 describes how to construct

these vertical planes. The determination of a collision-free area

(CFA) for each individual robot on each vertical plane will be

discussed in Section 3.2.

For two cooperative robots which grasp a common

workpiece, the relative distance between the two wrist centers

is invariant. To determine the position for each wrist

center, this relative distance is considered to be a

constraint. In Section 3.3, this constraint will be applied

to find the wrist center area (WCA). The WCL will then be found by

superimposing the CFA and the WCA.



3.1 Construction of Vertical Planes

A series of vertical planes is formed for each of the two

cooperative robots. Each plane will include a vertex of the








43

obstacle's top surface and an offset, S, to the axis of the

robot's first joint (as shown in Fig. 3.1). The equation of

each plane is given by



Y = mX + b, (3.1)

where m = tanS, (3.2)

b = Yr mXr, (3.3)



and (Xr, Yr) are the X and Y coordinates of the vertex.

The angle 6 is computed as follows (see Fig. 3.2):



6 = B + a 7/2, (3.4)

where B = arctan(Y,/Xr), (3.5)

a = arccos(S/dr), (3.6)

dr = (X2 + Y2)2. (3.7)



For each vertical plane established for the first robot,

a corresponding plane can be found for the second robot

according to the predetermined path of the wrist center.

The equation for wrist center path of the first robot is

given as follows:



R = P + X(Q P), 0 \ 1. (3.8)



where P and Q are the coordinates of the initial and final

positions of the wrist center of the first robot.








44

The equation for wrist center path of the second robot

is given by



R' = R + r S, (3.9)



where r is the distance between two wrist centers, and S6 is

the end effector orientation of the first robot.

If the end effector orientations at the initial and final

positions are the same, a constant end effector orientation

of the first robot is assumed throughout the motion. In that

case, the wrist center path is a straight line.

If the end effector orientation at the initial and final

positions are different, the end effector orientation will be

varied throughout the motion. It is possible to determine

the axis and net angle of rotation about this axis that will

cause the end effector of the first robot to change

orientation as desired. According to the axis and net angle

of rotation, the end effector orientation, S6, at each

intermediate position can be determined [see Crane 1987 pp.

103-111]. In this case, the wrist center path of the second

robot becomes a curve as shown in Fig 3.3.

For each vertical plane, a corresponding plane for the

second robot can be found as follows (see Fig. 3.3):

(1) Determine the intersection point I of the plane and

path of the wrist center.

(2) Compute the end effector orientation, S6, at point I.








45

(3) Substitute the coordinates of point I from (1) and

the end effector orientation, S, from (2) into Eq. (3.9), to

find the corresponding point I' on the path of the second

robot.

(4) Use the point I' and the offset S, to construct the

corresponding plane of the second robot.

Following this construction, these planes are arranged

in a sequence according to the angle formed by the offset of

the first robot and the X-axis. During this arranging

process, any plane which lies outside the work area, or is a

duplicate of previous plane is deleted from the arrangement.



3.2 Determination of Collision-Free Area (CFA)

Each vertical plane cuts through obstacles and the

reachable workspace of the wrist center, and thus generates

cross-sections of obstacles and the reachable workspace of the

wrist center. The cross-sections are mapped via a rotational

transformation onto the X-Z plane. A CFA for each individual

robot can then be found on each transformed cross-section,

according to the relative positions of the obstacles and the

work area of the robot.

Two terms, "principal upper arm obstacle" and )principal forearm obstacle,"

have been established to locate the lower boundary of the

CFA:








46

1. The )principal upper arm obstacle" is defined as the first

obstacle that would be hit by the upper arm if it was rotated

downward from its vertical position (for instance, obstacle

A in Fig. 3.4a).

2. The 'principalforearm obstacle" is defined as the first

obstacle that would be hit by the forearm if it was rotated

downward from its extreme position and the upper arm and the

forearms are collinear (for instance, obstacle B in Fig.

3.4b). However, if the upper arm is halted by a principal upper

arm obstacle, the forearm may rotate further before it strikes an

obstacle (for instant, obstacle B in Fig. 3.4a). This

obstacle is also defined as the principalforeann obstacle.

According to the location of the principal obstacle,

determination of a lower boundary of the CFA will fall into

the following three groups.



Group 1: The angle from joint 2 to any vertex of the
principal obstacles is less than the joint
limitation of joint 2.

In this group, the lower boundary is determined by

rotating the forearm around the lowest position of joint 3.

According to the location of the principalforearm obstacle, there are

two different cases.

Case 1. The X coordinate of the far corner of the

obstacle is greater than the X coordinate of joint 3 (see Fig.










3.5). The actual curve of the lower boundary of the CFA is

a sixth degree polynomial [see Tseng 1987 pp. 121-123]. As

suggested, the straight line PR' rather than the sixth degree

polynomial curve, is used to represent the lower boundary.

Point R' denotes the bounding point of the CFA. Point P is

the vertex hit by the forearm. The X and Z coordinates of

point R' are derived as follows:



Xr, = /icos92min + 12COS83, (3.10)

Zr, = H + l1sin02min + 12sinO3, (3.11)



where 11 and 12 are the link length of the upper arm and the

forearm respectively. H is the height of joint 2, and 02min is

the angular limitation of joint 2. The angle 03 is derived as

follows:



03 = arctan[(Zp Zo3)/(Xp Xo3)], (3.12)

where Xo3 = 1cos2min,

Zo3 = H + lisinO2min,



and (Xp, Zp) are the X and Z coordinates of vertex P.



Case 2. The X coordinate of the far corner of the

obstacle is less than the X coordinate of the joint 3 (see








48

Fig. 3.6). In this case, the lower boundary of the CFA is

the far vertical side of this obstacle.



Group 2: The straight line O2R* hits the principalupperarm
obstacle first.

In this group, the lower boundary of the CFA is

determined by the forearm as it rotates around the joint 3

while the upper arm is fixed at its lowest position.

According to the location of the principalforearm obstacle, there are

three different cases.



Case 1. The forearm hits the far corner of the obstacle

(see Fig. 3.7). In this case, the lower boundary of the CFA

is formed by a line PR' and a circular arc R'R. Point R is

the intersection point of the straight line and the outer

boundary of the wrist work area. The X and Z coordinates of

point R are derived as follows:



Xr = (11 + 12)COS2, (3.13)

Zr = H + (11 + 12)sin02, (3.14)

where 02 = arctan[(Zq,-H)/Xq,], (3.15)



and (Xq,, Zq,) are the X and Z coordinates of vertex Q'.



* The line 02R represents the upper arm and the forearm
when they are collinear.








49

From Fig. 3.7, the X and Z coordinates of point R' can

be derived as expressed as



X,, = licos02 + 12CosO3, (3.16)

Z, = H + /isinO2 + 2sin03, (3.17)

where 83 = arctan[ (Zp-llsin2-H)/(X,-l1cos8) ] (3.18)



Case 2. The forearm hits the top surface of the

obstacle (see Fig. 3.8). In this case, the lower boundary of

the CFA is form by a circular arc R'R. Point R is defined as

the same in Case 1. The X and Z coordinates of point R is

computed by Eqs. (3.13) and (3.14). Point R' is the

intersection point of the forearm and the top surface. From

Fig. 3.8, the X and Z coordinates of point R' are derived as

follows:

The equation of line PIP2, which represents the top

surface of the obstacle, is given by



S = P1 + Ap(2 P1), (3.19)



where Pi and P2 are the coordinates of the two vertices.

The coordinates of the intersection point are given by


Xr = Xo3 + 12cos3,,


(3.20)










Zr = Z3 + /1sin03. (3.21)

where Xo3 = 11cos02, (3.22)

Zo = H + 11sin02, (3.23)



and 02 is calculated by Eq. (3.15).

Substituting Eqs. (3.20) and (3.21) into Eq. (3.19)

yields



Acos03 + Bsin03 + C = 0, (3.24)

where A = 12(Zp2 Zpl),

B = -12(Xp2 Xpl),

C = (Zp2-Zp,) (Xo3-Xpl) (Z03-Zpl) (Xp2-Xpl)



Solving Eq. (3.24) yields two values for 03.

Substituting 03 into Eq. (3.20) gives a pair of values for Xr,.

The value of X, which lies between Xp, and Xp2 is selected.



Case 3. The principal forearm obstacle is the same as the

principal upper arm obstacle (see Fig. 3.9). There are three

subcases:



Case 3.1. The upper arm hits the far corner of the obstacle,

and the forearm hits the far vertical side of the obstacle

(see Fig. 3.9a). In this case, the lower boundary of the CFA








51

is formed by a circular arc R'R. The X and Z coordinates of

point R are computed by Eqs. (3.13) and (3.14). Point R' is

the intersection point of obstacle and the forearm. The X and

Z coordinates of point R' can be derived as follows:



Xr, = Xq, (3.25)

Zr, = H + lsin82 + 2sinO3, (3.26)



where B3 = arccos[(Xq l1cos02) / 2], (3.27)



and 02 is calculated by Eq. (3.15).



Case 3.2. The upper arm hits the near corner of the obstacle,

and the forearm hits the top surface of the obstacle (see Fig.

3.9b). In this case, the lower boundary is formed by a

circular arc R'R. Point R' is the intersection point of the

forearm and the top surface. The X and Z coordinates of point

R' are derived as follows:

The equation of line PQ, which represents the top surface

of the obstacle, is given by



S = P + ( P), (3.28)



where P and Q are the coordinates of the two vertices.








52

The X and Y coordinates of the intersection point R' are

given by



X, = X03 + 12cos83, (3.29)

Z,, = Zo3 + l2sin03. (3.30)



where Xo3 and Zo3 are calculated by Eqs. (3.22) and (3.23).

Substituting Eqs. (3.29) and (3.30) into Eq. (3.28)

yields



Acos03 + Bsin03 + C = 0, (3.31)

where A = 2(Zq Zp),

B = -2(Xq Xp),

C = (Zq-Zp) (Xo3-Xp) (Zo3-Zp) (Xq-Xp) .



Solving Eq. (3.31) yields two values for 83.

Substituting 03 into Eq. (3.29) gives a pair of values for Xr,.

The value of X,, which lies between Xp and Xq is selected.



Case 3.3. The upper arm hits the near corner of the obstacle,

and the forearm hits the far corner of the obstacle (see Fig.

3.9c). In this case, the lower boundary is formed by a line

QR' and a circular arc R'R. Point Q is the far corner of the

obstacle. The X and Z coordinates of point R' are derived as

follows:












X,, = Xo3 + 12COS03, (3.32)

Zr, = Zo3 + 12sinO3, (3.33)

where 83 = arctan[(Zq Zo3)/(Xq Xo) ], (3.34)



X,3 and Zo3 are computed by Eqs. (3.22) and (3.23).



Group 3: The straight line 02R hits the principalforeann
obstacle first.

In this group, the lower boundary of the CFA is given by

line PR (see Fig. 3.10). The X and Z coordinates of point R

are derived as follows:



Xr = (lI + 12)cos02, (3.35)

Zr = H + (l1 + 12)sinO2, (3.36)

where 02 = arctan[(Zp-H)/Xp]. (3.37)



In all three groups, the lower boundary of the CFA on

the right side of the principal obstacle was determined. The

lower boundary between the principal obstacle and the left

most obstacle was formed by connecting the vertices of each

obstacle with a straight line (for instance, the line QQ' as

shown in Fig. 3.7).








54

3.3 Determination of Wrist CenterArea (WCA)

After the position of the wrist center of the first robot

is selected, a sphere is constructed with its center located

at this position, and with a radius equal to the distance

between the wrist centers of the two robots (see Fig. 3.11).

The intersection of the sphere and the vertical plane of the

second robot is a circle. The wrist center will then lay in

that part of the circumference of this circle which lies in

the work area.

The wrist center position of the second robot is

determined as follows:

1) The vertical plane 7rI which forms an angle 86 with

the X-axis (see Fig. 3.12) is considered. The normal vector

of this plane is nj = (-sin61, cos61, 0).

A new coordinate system OXYZl is constructed with the Y1-

axis parallel to this normal vector n, with an offset S. The

relationship between the original OXYZ and the new OXYZ1

coordinate systems is derived as follows:

l.a) Rotating the coordinate system about Z-axis through

angle 6, yields



X Ci -si 0 x
Y = si ci 0 y (3.38)
Z 0 0 1 z



where ci = cos51,

s, = sin6,.












l.b) A translation S along the y-axis is expressed by



x X, 0
y = YI + S (3.39)
z Zi 0



where S is the offset of the joint 2.


Hence, the transformation is given by



X ci -si 0 X,

Y s= s c 0 Yi+S (3.40)

Z 0 0 1 Z,


In the OXiYZ, coordinate system, a point on plane 71 can

be designated by (Xi, 0, ZJ). According to Eq. (3.40), the

same point in the OXYZ coordinate system is designated by

(cX1I sIS, siX1 + ciS, ZI) .

2) For the second robot, the normal vector of the

corresponding plane I, is n2 = (-sinS2, coss2, 0) (see Fig.

3.10). A coordinate system O2X2Y2Z2 is constructed with the Y2-

axis parallel to n2. The origin 02 has an offset with the base

of the first joint, and X2-Z2 plane coincides with the plane r2

(see Fig. 3.12). The relationship between the OXYZ and 02X2Y2Z2

coordinate systems can be obtained by the following steps:









56

2.a) A translation d along the X-axis is expressed by



X X' d
Y = Y' + 0 (3.41)
Z Z' 0



where d is the distance between two robots.

2.b) Rotating the coordinate system about the Z'-axis

through the angle 62 yields




X' c2 -S2 0 x"

Z' 0 0 1 z"



where C2 = cos62,

s2 = sin62.

2.c) A translation S along the y"-axis is expressed

by



x" X2 0

z" Z2 0



The relationship between the two coordinate systems is

therefore











X c2 -S2 0 X2 d

Y S2 c2 0 Y2+S + 0 (3.44)

Z 0 0 1 Z2 0



In the O2X2Y2Z2 coordinate system a point in plane 72 can

be designated by (X2, 0, Z2). According to Eq. (3.44), the

same point in the OXYZ coordinate system is designated by (cX2

- s2S + d, sX2 + c2S, Z2).

3) For two cooperative robots, the distance between the

two wrist centers is constant and equal to R. This condition

can be expressed mathematically as



[(c2X2-S2S+d) -(cXl-slS) ]2 + [(S2X2+C2S)- (sXl+CiS) ]2

+ [Z2-Z1]2 = R2. (3.45)


From Eq. (3.45), for given coordinates of either (Xj, Zj)

or (X2, Z2), the WCA for each robot can be found.



3.3.1 WCA for the First Robot

For a given position of the wrist center of the first

robot, given X, and Z,, Eq. (3.45) can be rewritten



(X2 a)2 + (Z2 Zi)2 = r2, (3.46)
where a = C1-2X 1.2S c2d, (3.47)

r = (R2 2)1/2, (3.48)








58

and p = (s2X-2[ (s.2-c12S.2) S+ (ci-c2c.2) d] X,

+ (l-ci) 2S2+2 [ S-S2-C2S1-2]Sd+s d2)1/2, (3.49)

c.2 = cos(61 62),

s,2 = sin(61 62).



If r < 0, there is no intersection between the sphere and

plane T2. This means that a wrist center position cannot

exist for the second robot. Hence, r must be greater than

zero, thus p must be less than R. From Eq. (3.49),



X 2aiXi + a2 < 0, (3.50)

where al = [ (c-c2c-2) d+ (s12-c1.2s1-2) S]/s-2,

a2= [(1-c1-2) 2S2+2 (s1-S2-2S1-2) Sd+s2d2-R2]/s.2.



By solving Eq. (3.50)



AI < XI, A2, (3.51)

where A, = a, (al a2)1/2,

A2 = a, + (a 2 a2)1/2



The reachable limits of the PUMA robot in the X direction

are


-34.1" < X, < 34.1".


(3.52)








59

From Eqs. (3.51) and (3.52),



X1Iow < Xi < Xiup. (3.53)



Figure 3.13 shows four different cases of WCA for the

first robot.



3.3.2 WCA for the Second Robot

Similarly, for a given set of coordinates of the wrist

center position of the second robot, Eq. (3.45) can be written

as a function of X1 and Z,:



(Xi b)2 + (Z1 Z2)2 = r'2, (3.54)

where b = c1-2X + cd + s,2S, (3.55)

r' = (R2 12)1/2 (3.56)

p = { s2X +2 [ (c2-CIc1-2) d+ (s.2-c-2s1.2) S ]X2

+ (1-C12)2S2+2 (s1-s2-cS1.2) Sd+s2d21/2. (3.57)



For the existence of the wrist center of the first robot,

r' must be greater than zero. From Eq. (3.56),



X2 2bX2 + b2 < 0, (3.58)



where bi = [(cici2-c2) d- (s,12-c-2S2) S]/s 2,

b2 [(-c.2) 2S2+2 (s,-s2-c.1-2) Sd+sd2 -R2]/s 2.












By solving Eq. (3.58),



BI < X2 < B2, (3.59)

where B, = b, (b b2)1/2,

B2 = b, + (b2 b2)12.



Due to the reachable limitation, X2 must also satisfy the

boundary conditions as defined in Eq. (3.52).



In the previous, the collision-free area (CFA) for each

individual robot and the wrist center area (WCA) for two robots

under cooperative motion, were determined. The wrist center locale

(WCL) for both robots is found by superimposing these two

areas. In the next chapter, determination of intermediate

positions from the WCL will be discussed.






























Figure 3.1 Series of vertical planes.

Figure 3.1 Series of vertical planes.


(Xr, Yr)


Figure 3.2 Determination of angle 6.




















Q'(Xq', Yq')


P(Xp, Yp)


P'(Xp', Yp')


FIgure 3.3 Determination of corresponding plane.


Q (Xq, Yq)


























































Figure 3.4 Principal obstacles.


































Figure 3.5 Group 1 Case 1.


R2


Figure 3.6 Group 1 Case 2.



































Group 2 Case 1.


Group 2 Case 2.


Figure 3.7


Figure 3.8

























































(b)

Figure 3.9 Group 2 Case 3.


































Figure 3.9 Continued


Figure 3.10 Group 3.































Figure 3.11 Determination of WCA.









YY


Y
x
0 / 0 X,


Figure 3.12 Projection onto X-Y plane.














69



70 -
















-7 0 -2------------------------0--------------,0--------
60



50 -



40 -



30 -



20 -



10



0



-10
-40 -20 0 20 40

xl



(a)







70



60 -



50 -



40 -



W 30 -



20 -



10 -



0-



-10
-40 -20 0 20 40

Xl



(b)



Figure 3.13 Examples of WCA.














70



70



60 I I I I



50 -



40 -



30



20 -



10 -



10



-10 -
-40 -20 0 20 40

X1



(C)







70



60 -



50 -



40 -



30 -



20 -



10 -







10
-40 -20 0 20 40

XI



(d)


Figure 3.13 Continued.














CHAPTER IV

COLLISION-FREE PATH FINDING



Path finding is accomplished by locating the intermediate

positions within the wrist center locale (WCL) and joining these

positions from the initial position to the final destination.

As mentioned in Section 2.2, a straight line path, from

the initial position to the final position of the wrist center

for the first robot, was assumed as a predetermined path.

This straight line intersects each vertical plane at one

point. The intersection point is called the "recommended wrist

center position." If this point lies outside the WCL, it is

necessary to shift the point into the WCL for further

consideration.

For example, in Fig. 4.1a, the recommended wrist centerposition,

E, lies under the WCL. In this case, it is shifted along the

Z-direction until it falls within the WCL, E'. In a second

example, point E lies outside the WCL (see Fig. 4.1b). It is

shifted first along the X-direction and then along the Z-

direction until it reaches the region of the WCL, E'.








72

Because the two robots are working in cooperation, it is

necessary to check the possibility of collision between the

end effectors, the grasped workpiece, and the obstacles during

the selection of the intermediate wrist center position. This

will be discussed in Section 4.1.

The collision between links of the two cooperative robots

also needs to be taken into account during the selection of

the intermediate wrist center positions. This will be

discussed in Section 4.2.



4.1 Collision Between End Effectors,
Grasped Workpiece and Obstacles

In Chapter III, the wrist centerlocale (WCL) on each vertical

plane was determined. During the determination of the WCL,

only the collision between the upper arm and the forearm with

respect to the obstacles has been discussed. It is now

desired to check the possibility of collision between the end

effectors and the grasped workpiece with the obstacles.

Since the end effector grasps the workpiece firmly and

no slippage occurs during the motion, the end effectors and

the grasped object can be treated as a rigid body. A line

segment PQ is used to represent this rigid body, with the

endpoints P and Q located at the wrist center position of each

robot respectively (see Fig. 4.2).

For a given position of the wrist center, P, the

corresponding position, Q, will lie on a circle. Thus, the








73

possible position for the end effectors and grasped workpiece

will form a cone-shaped region with the vertex at point P (see

Fig. 4.2). The collision checking can be simplified as the

determination of the intersection between this region (cone)

and the obstacles.

In Section 4.1.1, obstacles which affect the movement of

the end effectors and the grasped workpiece is identified.

Then in Section 4.1.2 a perspective projection is applied to

determine the possible position for point Q.



4.1.1 Obstacles Identification

To check if the obstacles affect the movement of the end

effectors and the grasped workpiece, the cone and the

obstacles are projected onto the plane Z = 0 (see Fig. 4.3).

Triangle PiQ1R1 is the image of the cone on the projection

plane. The image of any obstacle on the projection plane is

a polygon. If the polygon touches the triangle, the obstacle

will affect the movement of the end effectors and the grasped

workpiece.

Triangle PiQR can be represented by two sets of angular

ranges [a, B] and [B', a'] with respect to Pi and O'. The

point 0' is the center of the right hand robot's base. Angles

a and B are computed as follows:



a = arctan[(Yql Ypi)/(Xqi Xp)], (4.1)

B = arctan[(Y, Ypi)/(Xi Xp,)], (4.2)








74

where (Xp1, Yp,), (Xqi, Yqi), (Xr,, Yrj) are the coordinates of

points P1, Q, and Ri. Angles a' and B' are computed by Eq.

(2.32).

The angular range of each polygon with respect to O' is

represented by [0 O8]. Angles O8 and 6 are computed by Eq.

(2.32).

The detection of a possible intersection is achieved as

follows:

Step 1. Check the angular range [80, 0f] of each polygon

with respect to [B', a']. If it falls into one of the

following conditions, the polygon intersects the cone.

1. [68, Of] contains [B', a'] (for example, polygon A

in Fig. 4.4a).

2. [80, Of] overlaps with [B', a'] (for example, polygon

B in Fig. 4.4b).

3. [O8, Of] lies within [B', a'] (for example, polygon

C in Fig. 4.4c).

In any of the cases, it is necessary to proceed with Step

2.

Step 2. Exclude the portion of the polygon which is

outside the area formed by lines MP1 and MQ, (see Fig. 4.5).

The new vertices of the remaining portion can be found by

checking the intersection points) of each edge with line MP,

and MQ1. For example, polygon A becomes polygon A1A2A3A4A,6A as

shown in Fig. 4.5.








75

Step 3. Find the angle, Oi, between X-axis and the line

from P, to each vertex of the new polygon (see Fig. 4.6). This

angle is computed as follows:



0i = arctan[(Y, Yp1)/(Xi Xp,)], (4.3)



where (Xi, Yi) are the coordinates of i'h vertex of the polygon.

The angular range, [minf, Omax], for each polygon with

respect to PI can be calculated. The same conditions for

finding affected obstacle in Step 1 is applied here to exclude

those unaffected obstacles.

Step 4. Exclude the portion of polygon which lies

outside the area of triangle P1Q1Rj.



From the above steps, obstacles which affect the movement

of the end effectors and the grasped workpiece have been

identified. It is now desired to check the height of each

obstacle. If the height is lower than the lower point of the

cone, this obstacle will not affect the movement. In this

case, this obstacle is excluded from further consideration.



4.1.2 Perspective Projection

In the previous section, obstacles which will affect the

movement of the end effectors and the grasped workpiece have

been identified. It is now desired to know the effect of

these obstacles. For a given wrist center position, P, it is








76

necessary to determine if a corresponding wrist center Q

exists.

A perspective projection from point P to plane f' is

shown in Fig. 4.7. The coordinate system O2X2Y2Z2 on the plane

7' is used to find the transformation matrix. The

relationship between coordinate systems 02X2Y2Z2 and OXYZ is

given by'




x c2 S2 0 X-d 0

y = -S2 C2 0 Y S (4.4)

z 0 0 1 Z 0




where d is the distance between the bases of the two robots,

and S is the offset of joint 2 (see Fig. 3.12).

The transformation of this perspective projection is

given by




x' -yp Xp 0 x

y' 0 0 0 y (4.5)
Y Yp
z' 0 z, -y, z


1 This relationship is obtained from Eq. (3.46).








77

where (xP, yp, zP) are the coordinates of P on the O2X2Y2Z2

coordinate system.

In Fig. 4.8, polygon A is the image of an obstacle on

plane 7'. Circle C is generated by the possible positions of

wrist center Q for a given position of P. From the relative

positions among circle C, polygon A and the CFA, the

existence of wrist center Q can be determined. If any portion

of C lies outside the polygon A and within the area of CFA,

as shown in Fig. 4.8, the wrist center Q can be located on

that portion. Otherwise, a wrist center Q cannot exist. In

this case, it is necessary to re-select a position of P and

repeat the procedure until a corresponding wrist center Q is

found.



4.2 Collision Checking Between Two Robots Links

It is necessary to check the possibility of collision

between two robots links during the motion. Since the upper

arm and the forearm of each robot lie on a vertical plane at

each interval, a collision will occur on the intersection line

of the two planes. Section 4.2.1 discusses the case when two

planes intersect in one line. The cases when two planes are

coincident or parallel are discussed in Sections 4.2.2 and

4.2.3.








78

4.2.1 Two Planes Intersect at One Line

The equation of the vertical plane which contains the

upper arm and the forearm of the left hand robot is given by



Y = m1X + bl, (4.6)

where m, = tan(6,),

bi= Y1 miX,

and X, = S cos(80),

Y, = S sin(01),

08 = 61 + 7r/2.



Further, S is the offset of joint 2, and 6, is calculated by

Eq. (3.4) (see Fig. 3.2).

For the right hand robot, the plane equation is given by



Y = m2X + b2, (4.7)

where m2 = tan(62),

b2 = m,X,,

and X2 = S cos(82) + d,

Y2 = S sin(02),

02 = 62 + 7/2.



Further, d is the distance between the base of the two robots.

The equation of the line of intersection, L, can be

obtained from these two expressions:











b2 b,
X = (4.8a)
m, m2

mlb2 m2bb
Y = (4.8b)
mI m2



According to the relative position between the wrist

center (P or Q) and the line of intersection, L, the

possibility of collision can be detected.

If both (or one of the two) wrist centers locate(s)

between line L and the first joint axis of each robot, the

links between the two robots will not collide (see Fig. 4.9).

The coordinate system OXYZ, attached to the plane which

contains the upper arm and the forearm of the left hand robot,

has been discussed in Section 3.3. The relationship between

the coordinate systems OXYZ, and OXYZ is given by2




X, ci si 0 X 0

YI = -si c, 0 Y S (4.9)

Zi 0 0 1 Z 0



The equation of the line of intersection, L, in the

coordinate system OXYiZ can be obtained from these two

expressions:


2 This relationship is obtained from Eq. (3.42).










Xi = c, X + s, Y, (4.10a)

YI = -s, X + c, Y S = 0. (4.10b)



The coordinate system 2X2Y2Z2 attached to the plane which

contains the upper arm and the forearm of the left hand robot,

has been discussed in Section 3.3. The relationship between

the coordinate systems O2X2Y2Z2 and OXYZ is given by3




X2 c2 S2 0 X-d 0

Y2 = -s2 2 Y S (4.11)

Z2 0 0 1 Z 0


The equation of the line of intersection, L, in the

coordinate system 02X2Y2Z2 can be obtained from these two

expressions:



X2 = C2 (X d) + 2 Y, (4.12a)

Y2 = -s2 (X d) + c2 Y S = 0. (4.12b)


For the first robot, the wrist center position is denoted

by P, and the elbow position is denoted by P' (see Fig. 4.9).

For the second robot, the wrist center position is denoted by

Q, and the elbow position is denoted by Q'. When Xp > X, and


3 This relationship is obtained from Eq. (3.46).








81

X2q > X2, there exists the possibility that the two robots
links will collide with each other.

To determine which link will collide with another link,

it is necessary to calculate the X coordinate of the elbow

position. The X and Z coordinates (X1p, and ZI,,) of the elbow

position, P' on the coordinate system OXYZil, can be obtained

by solving the following equations:



(Xp,, Xp)2 + (Zip, ZP,)2 = l2, (4.13)

X, = 11 cos8, (4.14a)

and Zip, = H + 11 sinO, (4.14b)



where H is the height of the joint 2, 11 and 12 are the

dimensions of the upper arm and the forearm respectively.

Substituting Eq. (4.14) into Eq. (4.13) yields



AcosO + Bsin8 + C = 0, (4.15)

where A = -2 Xip 11,

B = 2 (H /i li Zip),

C = H2 + l + X[2 + Z2 1 2 H /l.



Solving Eq. (4.15) yields two values for 0. Substituting

0's into Eq. (4.14b) gives a pair of values for Zi,,. The value








82

of ZIp, which gives the robot an elbow up configuration4 is

selected. The procedure for determining the coordinates of

Q' is the same as for P'.

If X,, is greater than X1, there is a possibility that the

upper arm will collide with either the forearm or the upper

arm of the right hand robot (see Fig. 4.10a). If Xp, is less

than X,, there is a possibility that the forearm will collide

with either the forearm or the upper arm of the right hand

robot (see Fig. 4.10b). The Z coordinate of the intersection

point I, between the link (upper arm or forearm) and line L

can be computed as follows:

Case 1. The upper arm passes through the line L. In

this case, the Z coordinate of I, is given by



Z, = H + (ZIp, H), (4.16)

where X = Xi / X1p,.



Case 2. The forearm passes through the line L. In this

case, the Z coordinate of point I, is given by



Zil = Zip, + A(ZIp Zip,), (4.17)

where A = (X, Xp,) / (XP Xlp,)






4 In this study, only the elbow up configuration for the
robot is considered.








83

Similarly, the Z coordinate of the intersection point 1,,

Z,, of the second robot with the line L can be determined.

If the difference between Zi and Z2 is less than the

width of the two links, these two links will collide. In this

case, it is necessary to re-select the wrist center positions

for both robots.



4.2.2 Two Planes Are Coincident

In this case, it is necessary to check the intersection

between each link and the end effectors to detect the

collision. The coordinate of the elbow position for each

robot is computed as described in Section 4.2.1. Since these

two planes are coincident, it is necessary to transfer the

coordinates of points Q and Q' from the coordinate system

O2X2Y2Z2 to O1X1Y1Z1. The relationship between these two

coordinate systems is given by (see Fig. 4.11)



X, -1 0 0 X2 D

Y = 0 -1 0 Y2 + 0 (4.18)

Zi 0 0 1 Z2 0



where D = (d2 -4S2)1/2

If the X coordinate of point P, XP, is less than the X

coordinate of point Q, Xq, there will be no collision between

these two robots (see Fig. 4.12).








84

If X, > Xq, it is necessary to check the intersection

between the links and end effectors together with the grasped

workpiece. The types of intersection can be classified into

the following six different cases:

Case 1. The upper arm of the first robot collides with

the end effectors and grasped workpiece (see Fig. 4.13a).

Case 2. The upper arm of the first robot collides with

the forearm of the second robot (see Fig. 4.13a).

Case 3. The upper arm of the first robot collides with

the upper arm of the second robot (see Fig. 4.13b).

Case 4. The forearm of the first robot collides with

the forearm of the second robot (see Fig. 4.13c).

Case 5. The forearm of the first robot collides with

the upper arm of the second robot (see Fig. 4.13d).

Case 6. The upper arm of the second robot collides with

the end effectors and the grasped workpiece (see Fig. 4.13b).

If any one of the above six cases occurs, the two robots

collide. It is thus necessary to assign another set of wrist

center positions and repeat the checking procedure until none

of the above six cases occurs.



4.2.3 Two Planes Are Parallel

When two planes are parallel to each other, it is

necessary to calculate the distance between these two planes.

If the distance between these two planes is greater than the








85

thickness of the links, the two robots links will not collide

with one another.

If the distance is less than the thickness, it is

necessary to find the positions of the elbow and the wrist

center for each robot. These points are projected onto a

plane Y = mX, which is parallel to both planes. From the

relative position of P, P', Q and Q' on this projection plane

(see Fig. 4.14), the collision between the two robots can be

determined. The same procedure which is used for the case in

Section 4.2.2, can be applied to determine if these two robots

collide.



4.3 Collision-Free Path Generation

In the previous two sections, an algorithm was developed

to select the intermediate positions of the wrist center. The

collision-free wrist center path for each robot can thus be

generated by connecting these intermediate positions, from the

initial position to the final destination.

In general, the path which sequentially connects all the

intermediate positions is not a smooth one. For example, the

path which passes through point a, b, c, ..., 1, in Fig. 4.15

is generated by connecting the intermediate position. It is

desirable to shorten and to smooth this path.

The path modification is accomplished by the following

steps:








86

Step 1. Compute the slope of each line segment which

connects the starting position to every intermediate position.

Step 2. Find the line segment which has the greatest

slope; for example, this is the line segment ae in Fig. 4.15.

Step 3. Shift every intermediate position between the

starting position and the end position of the line segment

found in Step 2. For example, positions b, c, and d are

shifted to b', c', and d' in Fig. 4.15.

Step 4. Use the end position of the line segment found

in Step 2 as the new starting position and repeat the

procedure until reach the final position.

During the shifting, it is necessary to check if the new

positions lie outside the outer boundary of the wrist center

workspace. If a position lies outside the workspace, it must

be moved down until it reaches the workspace.

The path modification is based on the original path of

the first robot. Due to the cooperative operation of the two

robots, the path of the second robot is changed following the

modification of the first robot as shown in Figs 4.16 and

4.17. However, in some cases, the modified path of the second

robot cannot be followed by its wrist center. Figure 4.18

shows an example that part of the modified path of the second

robot lies outside the upper limit of the wrist center. In

this case, the original generated path is used without any

modification.








87

During the path modification, it is also necessary to

check again the collision between the two robots links. After

shifting the wrist center position, the two robots links may

collide with each other. In this case, the original generated

paths are used as the final wrist center paths. It should be

noted that the generated path of the second robot can also be

used to perform the modification. The path of the first robot

is then changed according to it.



4.4 Computer Graphics Simulation

The algorithms developed in this work were implemented

on the Silicon Graphics IRIS 4D/70GT workstation. The

programs were written in C Language and a built-in graphics

library on the system was used for basic graphics functions

such as line draw, polygon fill, and so on. The surface

models for the robot working environment and the PUMA robots

were built by utilizing a Z-buffer algorithm provided by the

system.

After data input, the collision-free paths were found

within several seconds. This included generating graphics

objects, and modification of the path. During the

computation, if collision-free path could not be generated,

the program would give an error message and stop running the

simulation. For example, as shown in Fig. 4.19, part of the

generated path lies outside the upper limit. Thus a

collision-free path cannot be found.








88

Figure 4.20 shows two PUMAs grasp a workpiece in an

obstacle-strewn environment. Figure 4.21 shows the obstacles

after expansion, and the upper arm and the forearm are

represented by two line segments. The generated path and the

path after modification are shown in Figs. 4.22 and 4.23.

Figures 4.24 4.26 show the consecutive configurations of the

two PUMAs as they move along the collision-free paths.



















































(b)

Figure 4.1 Shifting recommended wrist center position.




























Figure 4.2 Wrist center positions.





RI











7-7C x


Figure 4.3 Projection onto the plane Z = 0.























































Figure 4.4 Identification of obstacle (step 1).




























































Figure 4.4 Continued.
































Figure 4.5 Identification of obstacle (step 2).


Identification of obstacle (step 3).


Figure 4. 6




University of Florida Home Page
© 2004 - 2010 University of Florida George A. Smathers Libraries.
All rights reserved.

Acceptable Use, Copyright, and Disclaimer Statement
Last updated October 10, 2010 - - mvs