Estimating workpiece pose using the feature points method

A robot assembly for acquiring unorientated workpieces from a bin. A sensing system views the bin and collects data. A computer analyzes the data to determine candidate holdsites on the workpiece. The hand of the robot assembly then engages a workpiece at a selected holdsite. The workpiece is moved to a pose where the position and orientation of the workpiece are determined. After this determination, the workpiece may be disengaged, or moved to an intermediate or final goalsite. The method is applicable to workpieces that have six continuous unknown degrees of freedom. Furthermore, partial occlusion of the workpiece by the robot hand is allowed.

BACKGROUND OF THE INVENTION 
After workpieces are made and before they are needed for final assembly, 
they are typically stored unoriented in bulk in tubs and boxes (bins). The 
workpieces must be oriented in order to be fed properly into machines. 
There are a wide variety of assembly line operations involving the handling 
and processing of such individual workpieces. In such operations, attempts 
have been made in implementing automated workpiece handling systems, which 
involve as a necessary initial step, the acquisition of individual 
workpieces from a supply bin. Because workpieces are usually randomly 
oriented in the bin, each acquired workpiece must be viewed and properly 
oriented before it can be subsequently processed. Viewing is normally 
accomplished by a video camera which generates a video signal 
representative of the actual orientation of the acquired workpiece. If 
necessary, appropriate corrections are made to the orientation of the 
acquired workpiece before further processing. See for example U.S. Pat. 
No. 3,804,270. 
The known robot systems have generally failed to gain commercial acceptance 
because of their inability to handle randomly oriented workpieces in a 
reliable and consistent manner. In general, additional workpiece handling 
systems have been necessary to orient the workpiece for the robot. 
SUMMARY OF THE INVENTION 
The present invention is directed to a robot system and a method of using 
the same, which system acquires a workpiece from randomly arranged 
workpieces by examining data which matches holdsites and places the 
acquired workpiece at a goalsite. The system does not require complete 
knowledge of the position and orientation of a workpiece in a bin. The 
system broadly includes a robot assembly having an arm and hand (gripper) 
for sequentially engaging the workpieces, a sensing system for collecting 
data about the workpieces in different states and a computer which 
communicates with both the assembly and the sensing system and controls 
the movement of the robot assembly. 
The robot assembly includes a hand to engage the unoriented workpieces and 
an arm to translate and rotate the hand. 
In a preferred embodiment the transfer of a workpiece from a bin to a final 
goalsite embodies a plurality of steps after the robot system has been 
calibrated and initialized. In initialization a base coordinate system is 
established, an image coordinate system is established and certain poses 
and trajectories (spatial relationships of the hand to workpiece to supply 
bin and goalsite and hand and/or workpiece to supply bin and goalsites) 
are established. 
Although an attraction force gripper is implied by the description, a 
clamping force gripper may be employed in a similar manner with minor 
changes which would be obvious to one skilled in the art. Similarly, the 
choice between using a single camera and two presentation poses or two 
cameras and a single presentation pose to obtain stereo measurements is 
made for convenience. Here the single camera method is employed. 
To acquire a workpiece from the bin, the imaging system views the bin and 
candidate holdsites are established. These holdsites are potential 
surfaces on the workpiece which the hand can engage. The hand moves into 
contacting engagement with a holdsite on a piece and engages the 
workpiece. The workpiece is then removed from the bin. 
Next, the position and orientation of the workpiece in the hand is 
determined, the hand moves to a predetermined position and orientation, 
the presentation pose and the imaging system views the workpiece. 
The hand is now in the presentation pose. Features are extracted from the 
workpiece image data. The hand is rotated to a second presentation pose. 
Features are extracted from the second workpiece image data. Stereo 
correspondence is performed to identify feature pairs which appear in both 
image data. A camera model is employed to locate the feature points in 
space. The locations of the points are matched to a three-dimensional 
workpiece model having a standard position and orientation. The pose of 
the workpiece is computed. The workpiece is then moved to goalsite with 
compensation being made for workpiece rotation and translation. If the 
workpiece is first transferred to an intermediate goalsite, then it can be 
moved through a fixed trajectory to a final goalsite. 
In one aspect the invention comprises a robot assembly (arm and hand), the 
arm adapted for movement through at least two linear orthogonal axes and 
through at least two rotary axes; a video camera to image workpieces in a 
bin and a computer for control and computation. The camera views the bin. 
A binary image of the workpieces in the bin is obtained by setting all 
pixels whose brightness is below a threshold to zero, the rest to one. 
Solid regions (of ones) corresponding to the surface area of the hand are 
defined as candidate holdsites, and one holdsite of the holdsites is 
transformed into base coordinates. The arm is moved and the hand descends 
and contacts the holdsite thus engaging the workpiece. 
In another aspect of the invention, an acquired workpiece is moved to a 
presentation pose where it is viewed by a video camera. An image of the 
workpiece is obtained and features are extracted. The workpiece is moved 
to a second presentation pose where it is viewed again by the same video 
camera. A second image of the workpiece is obtained and similar features 
are extracted. (Stereo) correspondence between feature pairs which appear 
in both images is obtained. A camera model is employed to locate the 
feature points in space. The points are matched to a pre-established 
three-dimensional workpiece model. The pose of the workpiece is computed. 
The workpiece is transferred to an intermediate goalsite and/or then moved 
through a fixed trajectory to a final goalsite. 
In the preferred embodiment, the one and the other aspects of the invention 
are combined.

DESCRIPTION OF THE PREFERRED EMBODIMENT 
A robot system 10 embodying the invention is shown functionally in FIG. 1 
and comprises robot assembly 20; an imaging system 40, a computer 60 and a 
data source 70. Referring to FIG. 2, the robot assembly transfers 
workpieces 80 from a supply bin 82 to one of two intermediate goalsites 
100 and 110, respectively; and to a final goalsite 130. 
THE COMPUTER 
The control of the computer 60 is accomplished through the instructions. 
The instructions are written in terms of the particular mode of operation 
desired. The computer thus has stored in its memory, the programs or 
routines corresponding to each mode or operation of the computer. It is 
well known to those skilled in the art, that the computer comprises 
suitable control, storage and computational units for performing various 
arithmetic and logical functions on data which it processes in digital 
form. Any standard computer language consistent with the capability of the 
computer can be used for the instructions. All subroutines are not 
described in detail, since they can be written in any desired notations, 
formats or sequence, depending upon the particular computer being 
utilized, computer language, etc. Programs and instructions described 
before are put in terms of structural flow. When necessary and applicable 
for purposes of the invention, individual programs are described. For the 
computer 60 (Computer Automation Model LSI-2), the manufacture's Handbook 
91-20400-00A2, October 1974, sets forth the necessary program which 
includes the sequence of internal interconnections which have been added 
by the preparation and loading of the program into the internal memory of 
the computer. 
The computer 60, as shown in FIG. 1, interfaces with a robot assembly 
controller 30 through an interface 62 such as a 16 bit I/O Module (CAI 
Model 13213-00). The imaging system 40 comprises two video cameras 42 and 
44 (FIG. 2) both of which interface with the computer 60 through an 
interface 64 such as a direct memory access board. The computer has a 
sixteen bit wordlength, software floating point arithmetic and 32K words 
of memory. All image processing and robot assembly computations are 
performed in the computer. Manual arm joint servoing is effected by a 
keyboard 32 or automatically by the computer. 
Programs are written in both FORTRAN and assembly language modules. The 
assembly language modules are used to implement the interface handlers and 
to perform manipulations which are not suited for FORTRAN such as byte 
(eight bit) mode arithmetic and bit-based computations. 
Approximately 12K words are available for application dependent programs. 
The image provided by the imaging system occupies 8K words (16K bytes) for 
gray scale processing. About 12K words are needed to contain the operating 
system and the library of FORTRAN support routines. 
ROBOT ASSEMBLY 
Referring to FIG. 2, the robot assembly 20 consists of the arm 22, and hand 
26, and possesses six degrees of freedom about 3 linear orthogonal axes 
(X, Y, Z) and 3 rotary axes (.theta.4, .theta.5, .theta.6). For purposes 
of the preferred embodiment the arm is defined as including a wrist 24. 
The linear axes of the arm 22 are defined by three positioning tables, 
such as Anorad positioning tables 28. As is well known, the position of 
these tables along the three axes is measured by a rotary encoder which 
produces a dual channel sinusoidal wave and has a resolution of 0.001 inch 
per encoder count. The controller (servo control system not shown) 
embodied in the tables, interfaces with the computer 60 via a sixteen bit 
I/O board. The digital interface accepts the following commands: 
1. Home Signal--causes all axes to move in a negative direction until a 
home switch is sensed at which time the encoder counter is set to zero. 
2. Axis select--selects one of the six axes. 
3. Read/Write--determines if position should be read or written to the 
controller. 
4. Position command--these signals indicate absolute position in binary. 
5. Shift line--determines which byte is being written or read. 
6.Start--initiates reading or writing of position information. 
7. Ready--indicates when all axes are in position. 
8. Reset--resets all controller logic. 
9. Limit switch interrupt--indicates that an axis hit a limit. 
10. Limit switch word--decodes which axis hit a limit. 
Further, the front panel of the controller includes an LED readout that 
reflects the current position of each axis. A ready and limit indicator 
are also mounted on the front panel for each axis. 
The wrist 24 adapted for motion about the three rotary joint axis is 
secured to arm 22. The .theta.4 axis rotates through 286.degree.; the 
.theta.5 axis through 233.degree. and the .theta.6 axis through 
310.degree.. Encoder units have a two channel, 250 pulses per revolution, 
sinusoidal wave output. The controller 30 multiplies the encoder output by 
four, which results in an angular resolution of 0.36 degrees per pulse. 
The limit switches associated with the axes of the wrist communicate with 
and are controlled by the controller 30 associated with the tables. The 
specifications for the particular arm and wrist are described in "General 
Methods to Enable Robots with Vision to Acquire, Orient and Transport 
Workpieces", Fourth Report, J. Birk et al., Appendix 5, presented at the 
Eighth International Symposium on Industrial Robots, Stuttgart, Germany, 
May 30-June 1, 1978, which Report including Appendices is hereby 
incorporated by reference in this application in its entirety. 
Depending from the wrist 24, is a hand 26. The hand 26 is described in 
detail in Appendix 7 of the above-referenced report. The hand terminates 
in a surface-adapting vacuum cup 28. It can acquire a large variety of 
randomly-orientated workpieces having surfaces which will allow a vacuum 
cup seal. Although other types of grasping mechanisms could be used, such 
as jaws, fingers, electromagnets, etc., vacuum cups are preferred. The 
vacuum cups do not occlude the workpiece during the grasping process in 
that they need only one contact surface for holding. However, partial 
occlusion of the workpiece by the robot hand is allowed if jaw or finger 
type grasping mechanisms are employed. Secondly, vacuum cups can be 
interchanged in the hand mechanism allowing a variety of vacuum pressures 
and cup diameters, depending upon the workpiece to be acquired. Basically, 
the vacuum cup 28 is flexibly secured, such as by a compression spring 29 
to a piston-cylinder arrangement within the hand 26. The compression 
spring 29 allows flexibility of the vacuum cup in selecting 
randomly-oriented workpieces. Once a workpiece is engaged, the compression 
spring-vacuum cup is withdrawn into the wrist and locked. 
FIG. 3 illustrates the relationship between base coordinate system of the 
robot assembly and the hand coordinate system in terms of the arm joint 
cordinate system. Three joints are linear orthogonal sliding joints. Their 
displacements are denoted by the parameter values S.sub.1, S.sub.2, and 
S.sub.3. Three joints are rotary. The rotary axes intersect at a point 
(the wrist center). Their rotations are denoted by the parameter values 
.theta.4, .theta.5 and .theta.6. The hand (vacuum cap) is displaced from 
.theta..sub.5 axis by a fixed distance denoted by the parameter value h. 
The origin of the base coordinate system is defined to be at the wrist 
center when the robot assembly is initialized. Displacements in the 
X.sub.0 direction correspond to S.sub.1, displacements in the Y.sub.0 
direction to S.sub.2 and displacements in the directions Z.sub.0 to 
S.sub.3. 
The coordinate transformation matrix relating coordinate system 3 (the 
wrist center) into the base coordinate system, designated by "0" is: 
##EQU1## 
The three rotary joints are designed so that the three axes of rotation 
intersect at a point, the wrist center. The three coordinate 
transformation matrices which describe the individual joint rotations are 
##EQU2## 
where C.theta.=cosine .theta., S.theta.=sine .theta.. 
Thus the base-hand transformation matrix is given by 
##EQU3## 
A procedure for obtaining an arm joint solution is given in flowchart form 
in FIG. 4. Given the base-hand transformation matrix. 
##EQU4## 
and the value of the parameter h, this procedure solves for the three 
sliding joint values 
EQU S.sub.1, S.sub.2, S.sub.3 
and for the two sets of three rotary joint angles values 
EQU .theta..sub.4, .theta..sub.5, .theta..sub.6 and .theta.'.sub.4, 
.theta.'.sub.5, .theta.'.sub.6. 
These two sets of angles are related by 
EQU .theta..sub.4 =.theta..sub.4 .+-.180.degree., 
EQU .theta..sub.5 =-.theta..sub.5, 
EQU .theta..sub.6 =.theta..sub.6 .+-.180.degree., 
where -180.degree.&lt;.theta..sub.4, .theta..sub.5, .theta..sub.6 
.ltoreq.180.degree. and both sets correspond to the same .sup.0 T.sub.h. 
Hence, both will achieve the same hand coordinate system pose. A choice 
between the two joint angle sets can be made based upon physical 
considerations such as using the set of arm joint values which minimizes 
the time to move and arm joint values which can actually be achieved. 
The structure of the flowchart presented in FIG. 4 exhibits two major 
branches. The left branch corresponds to the general case where two sets 
of angle values are obtained. The right branch corresponds to the common, 
degenerate case obtained whenever the hand is pointing straight down, 
.theta..sub.5 =0. This degeneracy occurs because under this kinematic 
condition, the axes for .theta..sub.4 and .theta..sub.6 are colinear. 
Hence, only the combined value of rotation angle, .theta..sub.4,6, can be 
determined and many assignments of .theta..sub.4 and .theta..sub.6 are 
possible. There is also a degeneracy for .theta..sub.5 =180.degree.. This 
corresponds to the hand pointing straight up. The two conditions are 
summarized below as 
EQU .theta..sub.5 =0 .theta..sub.4 +.theta..sub.6 =.theta..sub.4,6, 
and 
EQU .theta..sub.5 =180.degree. .theta..sub.4 -.theta..sub.6 =.theta..sub.4,6 
THE CAMERA SYSTEM 
Referring to FIGS. 2 and 5, the video camera 42 is secured to the arm 22 
and its optical axis (line of sight) is parallel to the Z axis. 
Illumination of the workpiece is provided by lamps 43. 
The video camera 44, the workstation camera, is rigidly mounted and faces 
parallel to the X.sub.0 axis. Both cameras are aligned with the robot 
assembly axes of motion x,y and z to simplify the transformations from 
camera to robot coordinates, as shown most clearly in FIG. 5. Illumination 
of the workpiece symetrical about the optical axis is provided by lamps 
45. 
The first camera 42 is used to select holdsights on workpieces 80 in the 
bin 82. Because it is mounted on the arm it can also be used to view 
workpieces nearly anywhere in the bin 82 and goal sites 100, 110 and 130. 
The second camera 44 is used to compute the workpiece presentation pose in 
the hand 26. 
The relationship between the picture measurements (expressed in pixels) and 
hand pose (expressed in encoder counts of joint values) must be 
determined. By aligning the optical axes with the arm axes, the 
relationship between the camera measurements and the hand pose is 
simplified. 
As will be described after obtaining a view of the bin and selecting a 
holdsite, the arm is positioned such that the hand is vertically above the 
holdsite. This position is easily achieved because movements of the robot 
along the X.sub.0 or Y.sub.0 axis correspond to movements of the picture 
image along the X.sub.a or Y.sub.a axis, respectively. Given this 
alignment (FIG. 5), the following camera parameters are required: 
1. The conversion factors (encoder counts/pixel) in the X and Y directions 
(Cepx, Cepy). Because the camera has a square lattice of sensors, the arm 
22 has equal resolution in the X.sub.0 and Y.sub.0 directions, and the 
X.sub.0 Y.sub.0 and X.sub.a Y.sub.a planes are nearly parallel, these two 
conversion factors should be equal. 
2. x.sub.0, y.sub.0, x.sub.a, and y.sub.a are coordinates of a vertical 
line passing through the hand (x.sub.00, y.sub.00, x.sub.a0, and y.sub.a0) 
and a visible object. 
The robot assembly values X,Y that position the hand over a specific 
holdsite with image coordinates x.sub.h, y.sub.h are obtained as 
EQU x=(x.sub.h -x.sub.a0)C.sub.epx +x.sub.00 
EQU y=(y.sub.h -y.sub.a0)C.sub.epy +y.sub.00 
Arm camera 42 alignment is achieved by a rotation about the camera axis. 
A semiautomatic alignment procedure is given below: 
a. Position a calibration piece in the bin, such as, a white circular chip 
on a black background. 
b. Move the arm to the bin view pose. 
c. Move the arm to four poses that differ from the bin view pose, two along 
X.sub.0 and two along Y.sub.0. Compute displacement of the center of 
gravity (CG) of the chip. 
d. If the camera is aligned such that movement in x.sub.0 only displaces 
the CG in x.sub.a and movement in y.sub.0 only displaces the CG in 
y.sub.a, then skip step e; otherwise 
e. Rotate the camera slightly. Direction of rotation can be obtained from 
inspection of CG trajectories. Go back to step b. 
f. The camera is aligned. The same procedure of step c is repeated but with 
8 points in each direction. A regression algorithm over the CG's 
trajectory will give the conversion factors in each direction (C.sub.epx, 
C.sub.epy). 
Steps b, c, and f are automatic. Placement of the calibration piece, 
rotation of the camera, and decisions about alignment are operator tasks. 
On a TV monitor, the operator can also reject poor images or ones with 
only parts of the piece. A typical trajectory for the center of gravity is 
shown in Table I. Joint coordinates and image coordinates of the vertical 
line which passes through the chip are obtained as follows: 
1. Position the arm in the bin view pose and record the CG values of the 
calibration chip as x.sub.a0 y.sub.a0. 
2. Under Keyboard Control, position the arm with the vacuum cup touching 
the center of the chip. The x.sub.0 and y.sub.0 joint values are recorded 
as x.sub.00 and y.sub.00. 
TABLE I 
______________________________________ 
Typical CG Trajectories for the Arm Camera 
After Alignment Procedure 
Move in X.sub.0 Move in Y.sub.0 
x.sub.a y.sub.a x.sub.a 
y.sub.a 
______________________________________ 
084 056 050 089 
076 056 050 081 
067 056 050 073 
059 056 050 064 
050 056 050 056 
042 056 050 048 
033 056 050 039 
025 056 050 031 
017 056 050 022 
______________________________________ 
The workstation camera 44 is used to obtain a view of the workpiece in the 
hand. From this view a set of features is extracted and compared with a 
similar set for a taught presentation pose. One of the finite number of 
states of the workpiece in the hand is recognized. The difference between 
the position and orientation of the two feature sets can be converted into 
a translation and a rotation on the plane on which the workpiece is held. 
The computation of workpiece translation and rotation in the hand is 
simplified when the hand plane is parallel to the image plane. 
To achieve this situation for the current system architecture, the 
following alignments are selected. At the presentation pose, the hand is 
to point along the positive X.sub.0 axis and the holding surface is to be 
parallel to the Y.sub.0 Z.sub.0 plane. The image plane is also to be 
parallel to the Y.sub.0 Z.sub.0 plane. Details of this mounting can be 
seen in FIG. 6. 
As with the arm camera, computations are simplified if the X.sub.w and 
Y.sub.w axes of the image are aligned with the Y.sub.0 and Z.sub.0 axes of 
the robot. In this case only the two conversion factors relating encoder 
counts to pixels need be known. An absolute reference is not needed since 
the only translation information required is the shift in workpiece 
location between instruction phase and execution phase. 
The semiautomatic alignment procedure for the workstation camera 44 is 
given below: 
(a) With the arm at an initial estimate for the presentation pose, place 
the calibration poker chip in the hand. 
(b) The arm is moved on Y.sub.0 Z.sub.0 plane until the CG of the chip is 
in the center of the field of view. 
(c) Move the arm to two poses that differ only in the x.sub.0 coordinate. 
(d) If the two poses indicate alignment (computed CG's at these poses are 
the same) several additional poses along X.sub.0 are collected. If all 
these poses confirm alignment, skip step e. 
(e) If not, the operator corrects camera alignment (requires alignment in 
the two degrees of freedom which adjust the direction of the camera axis). 
Go back to b. 
(f) Follow a procedure similar to that specified for the arm camera to 
align camera rotation about its axis until X.sub.W and Y.sub.W are aligned 
with Y.sub.0 and Z.sub.0. 
(g) The arm is moved through several poses along Y.sub.0 and then along 
Z.sub.0. The conversion factors (encoder counts/pixel) are obtained using 
linear regression. Typical CG trajectories are similar to those shown for 
the arm camera. 
Alternatively a two plane method of camera calibration allows rays in space 
to be computed which are projections of the image pixel location. This is 
called a line of sight ray knowledge of which is necessary when deep bins 
and wide field of view imaging systems are used. This is described in a 
paper Estimating Workpiece Post Using The Feature Points Method, N. Chen, 
J. Birk and R. Kelly et al Report #5, "General Methods to Enable Robots 
with Vision to Acquire, Orient and Transport Workpieces", National Science 
Foundation Grant APR 74-13935, Appendix 8 pp. A8-1 through A8-32 first 
distributed on or about Sept. 25, 1979, on file at the National Technical 
Information Service, and hereby incorporated by reference in its entirety 
in this application. 
As set forth in this report, to accurately locate a point in space by using 
vision, the relation between a point on the image plane and the projection 
ray associated with this point should be known precisely. Several 
investigators used pinhole camera models to relate image points to 
projection rays. In reality, all the projection rays may not intersect at 
a single point. 
For this study the camera was stationary relative to the reference 
coordinate system. Since a very accurate XYZ motion arm was available 
(controlled to 0.001 in), a light on the arm was used to calibrate the 
camera. The light was mounted on the cantilever of the URI Mark IV arm. 
For calibration, the light was moved in two planes which did not need to 
be precisely perpendicular to the optical axis of the camera. The light 
was moved until the centroid of the light's binary image was within a 
small tolerance (0.2 pixels) of each point on a square grid. For ever grid 
point, two points in space were stored which determined a calibration ray. 
For an arbitrary image point, a ray in space was computed using 
two-dimensional interpolation. 
A program was written to calibrate a camera automatically. This program 
used test movements to compute the conversion factor of pixel difference 
to arm encoder counts and to determine the new location in space where the 
light should be after the test movement. 
An experiment was conducted to determine the number of image points on a 
square grid necessary for accurate calibration. Camera calibration data 
were collected using square grids of size 2 by 2, 4 by 4, 6 by 6 and 8 by 
8. Ten image test points were selected at random to cover the 
128.times.128 image. The same test points were used to compute error for 
each grid size. For each image test point, the light was moved to the 
interpolated points on the two calibration planes. The absolute value of 
difference between the centroid of the light's binary image and the image 
test point was calculated, resulting in a total of twenty error 
measurements. 
The data collected in camera calibration can also be used to find the image 
coordinates of a point in space. The ability to compute image points from 
points in space has been very useful for testing the validity of workpiece 
models and for checking the accuracy of estimated workpiece poses. 
OPERATION 
Briefly, the hand 26 descends into the bin 82 to acquire a workpiece 80. 
After the workpiece has been acquired, the arm 22 and wrist 24 are moved 
such that the arm is held in the presentation pose (FIG. 3). After the 
position of the workpiece 80 has been verified it is moved either to an 
intermediate goal site (FIGS. 8 and 9) or a final goal site. 
Prior to the actual operation of the robot system for the transportation of 
the workpieces from the bin to a goal site, the computer must be 
initialized. 
This initialization is divided into the instruction phase and the execution 
phase. The instruction phase has two segments. The first segment consists 
of those initialization activities which need only be performed once 
regardless of how many different workpieces are processed. The second 
segment of the instruction phase is workpiece-related and must be repeated 
for each different workpiece. 
In the first segment of the instruction phase, there are three activities: 
(a) initializing the robot assembly; (b) calibrating the imaging system; 
and (c) specifying poses (which are a function of the system 
architecture). 
The robot assembly is initialized by the following program which moves the 
arm 22 to its home switches (X, Y, Z,) and the wrist 24 to its negative 
limit switches; and from there to a pre-assigned offset pose with all 
encoders set to zero. The center of the wrist (where the three axes 
intersect) at this pre-assigned pose is the origin of the base coordinate 
system. 
As discussed above, the imaging system consists of two cameras 42 and 44. 
The procedure for aligning the cameras with the robot assembly axes has 
been previously explained. 
Five poses are specified during the first segment of instruction: the first 
and second presentation poses bin view pose; the descent pose; and the 
supply bin drop-off pose. These poses are specified by an operator moving 
the robot assembly/via the keyboard 32. After placement of the arm at each 
of these poses, the arm joint values are read by the computer and stored. 
In the presentation poses the workpiece is presented to the workstation 
camera (FIG. 6). 
The bin view pose is the arm configuration used to take a picture of the 
bin with the arm camera 42 positioned over the center of the bin. 
The descent pose is the pose above the bin to which the arm rapidly moves 
just prior to moving downward to grasp a workpiece. The height (Z value) 
of this pose is specified during instruction. Horizontal locations (X and 
Y values) are a function of which workpiece is in the bin selected. For 
the descent pose, the wrist points downwardly toward the bin. 
The supply bin drop-off pose is used to return a workpiece which could not 
be processed such as returning from an intermediate goalsite. The operator 
moves the arm from a goalsite back to a position over the bin. 
The second segment of instruction phase consists of seven activities: 
1. Specification of hand parameters 
2. Specification of the bin floor pose 
3. Goal site specification(s) 
4. Specification of goal to workpiece relationships 
5. Specification of hand to workpiece relationships 
6. Workpiece model specification 
All of these activities are workpiece related and therefore must be 
repeated for each different workpiece. 
The diameter of the hand vacuum cup 28 must match the requirements of the 
workpiece. It must not be larger than accessible surface areas. Yet it 
must be large enough to support the piece against gravity and acceleration 
forces. The minimum cup diameter necessary to support the piece against 
gravity and acceleration forces. The minimum cup diameter necessary to 
support the piece against gravity may be computed using the following 
equation: 
EQU D.sub.c =2(w/.pi.v).sup.1/2 
where D.sub.c =vacuum cup diameter (inches), w=weight of the piece 
(pounds), and v=vacuum (psig). Once the cup diameter is specified, the 
minimum size (in pixels) of a holdsite in the bin image may be computer 
using the following equation: 
EQU D.sub.i =D.sub.c C 
where D.sub.i =diameter of holdsite (pixels) and C=conversion factor from 
pixels/inches for the arm camera at the bin view pose. The distance from 
the .theta..sub.6 endplate (end of wrist FIG. 6) to the tip of the cup 
when the hand is locked must be specified. This parameter is needed to 
control hand pose using arm joints. The hand coordinate system origin is 
at the center of the vacuum cup gripping surface. Similar specification 
and parameter setting activities are necessary when a clamping type 
gripper is used to hold the workpiece. 
The pose of the bin floor is specified to allow pieces very close to the 
bin floor to be acquired while preventing the vacuum cup from attempting 
to pick up the bin itself. The bin floor pose is specified relative to the 
tip of the vacuum cup when the hand is unlocked and must be respecified 
whenever the vacuum cup is changed. This pose is specified under keyboard 
32 control by moving the hand until it makes contact with the bin floor, 
the wrist is pointing downward and the hand is unlocked. The Z axis 
coordinate is stored as Z.sub.b. During execution the arm is inhibited 
from moving to or beyond Z.sub.b during the process of acquiring a 
workpiece from the bin. 
After the workpiece 80 has been analyzed at the second presentation pose, 
it may be deposited at a final goalsite by the robot assembly. Many times, 
workpieces are required to be placed in hostile environments, extreme 
heat, corrosive vapors, etc., or where physical access is restricted by 
structures in the work space. Also, the workpieces may not be properly 
oriented. Therefore, in many situations, it may be desirable to transfer 
the workpiece to an intermediate goalsite, from where it is finally 
transferred by another device to a final goalsite (workstation). For a 
fuller understanding of the present invention intermediate goalsites are 
described. 
Referring to FIGS. 1,7 and 8, there are two intermediate goalsites on which 
the hand can deposit oriented workpieces. The first goalsite is a vacuum 
cup at the end of an insertion tool 102. The second goalsite is a vacuum 
cup on a regrasping station 110. Since vacuum cups are a function of the 
workpiece, so are the poses of these goalsites. The insertion tool 102 
simply comprises a vacuum cup 104, and housing 106. The housing 106 is 
structured such that the hand 26 is telescopically received in the housing 
106 and the vacuum cup 28 locks to the housing 106. As shown in FIG. 7, 
the assembly 20 carries the tool 102 with it. The gripping station 110 is 
simply a fixed post 112 with a vacuum cup 114 secured to its top. 
Intermediate goalsite specification is performed by moving the robot 
assembly with the keyboard until the hand vacuum cup 28 is aligned with 
the intermediate goalsite vacuum cup of the insertion tool 102. The wrist 
is pointed vertically downward and the hand is locked. Once the cups are 
aligned, the arm joint values are read by the computer and the base-hand 
t-matrix, .sup.0 T.sub.h, is computed. The base-goal t-matrix, .sup.0 
T.sub.g, is defined to equal .sup.0 T.sub.h, at this pose. 
To compute the pose of a workpiece relative to the robot hand, to the 
intermediate goalsite and to the terminal goal site, there must be a 
coordinate system affixed to the workpiece. This affixment is arbitrary. 
Drawings of the workpiece may be used to define an affixment. For this 
robot system, the workpiece coordinate system is affixed by definition of 
its relationship to one of the fixed coordinate systems: an intermediate 
goalsite or terminal goalsite. This relationship is then used to compute 
the relationships with the other coordinate systems. 
A workpiece is placed on the insertion tool 102. A workpiece coordinate 
system is affixed to the workpiece by defining the coordinate systems of 
the workpiece and the goal site to be coincident. Thus, for any workpiece 
placed on the insertion tool, the workpiece pose is the same. Because the 
workpiece pose is known, the relationships between the workpiece, the 
hand, the regrasping station and the terminal goalsite are determined. 
This is done by grasping the workpiece with the hand when the workpiece is 
in a known pose and by moving the workpiece from its (defined) pose on the 
insertion tool to the regrasping station and the terminal goalsite. 
First, the robot hand-workpiece relationships are obtained for the 
workpiece on the insertion tool goalsite. The hand is brought to grasp the 
workpiece (with the hand locked in retracted position). The hand-workpiece 
relationship is computed from the robot arm joint values. Similarly, for 
each of the other distinct surfaces on which the workpiece can be held and 
placed on the insertion tool goal site, the robot arm joint readings are 
obtained and these hand-workpiece relationships are computed. 
Next, the hand attaches to the insertion tool 102 and transfers the 
workpiece to the terminal goal site 130. The robot arm joint readings are 
used to measure the change in the pose of the workpiece. This permits the 
terminal goal-workpiece relationship to be computed. The insertion tool 
102 is then used to transfer the workpiece to the regrasping station 110. 
The regrasping station-workpiece relationship is computed from the change 
in arm joint values. 
Finally, the insertion tool 102 is returned to its bracket and the hand 
brought to grasp the workpiece at the regrasping station 110. Reading the 
arm joint values allows the robot hand-workpiece relationship to be 
computed for placements at the regrasping station in analogy to placements 
on the insertion tool. This procedure results in deriving all the required 
workpiece pose relationships by using the robot arm as the measuring 
device. 
For each intermediate goalsite, the robot assembly is translated to a thru 
pose near the goalsite using the keyboard. The base-workpiece t-matrix, 
.sup.0 T.sub.w, at the thru pose is then computed and stored. The thru 
pose is specified to guarantee that the approach and departure of the 
workpiece from each of the two sites is collision free. 
Workpiece model matching involves computing image features necessary to 
determine orientation in the hand. 
The steps in the workpiece model matching are as follows: 
1. While holding the workpiece, the assembly moves to the first 
presentation pose. 
2. An image of the workpiece is formed using the workstation camera and 
image features are extracted. Their locations are computed and stored 
along with their properties. 
3. The assembly moves the workpiece to the second presentation pose. 
4. Another image of the workpiece is formed using the workstation camera 
and image features are extracted. Their locations are computed and stored 
along with their properties. 
5. Features appearing in both images are paired and located in space by 
using the camera model. The feature points in space are matched to the 
pre-established workpiece features point model to determine the 
hand-workpiece relationship. 
6. The assembly moves to a goalsite via the thru pose and places the piece 
with the proper pose at the goal. 
This process is totally automatic. FIGS. 11 and 12 are flowcharts of the 
algorithms to place a piece at and acquire a piece from an intermediate 
goalsite. 
Once a workpiece is oriented and positioned at the goalsite, it is fed to 
the final goalsite. In this embodiment, the workpiece is fed in one of two 
ways. Either the robot assembly 20 picks up the insertion tool 102 and 
transports the piece on it to the final goalsite (FIG. 7) or the robot 
assembly hand picks up the insertion tool 102, moves it to pick up the 
piece at the regrasping station 110, and then transports the piece to the 
final goalsite (FIG. 8). 
Thus, several additional sets of arm joint values must be specified during 
instructions. 
1. Path to pick up insertion tool 
2. Path to pick up piece at regrasping station 
3. Path to place piece in final goal site 
The path to the insertion tool is independent of the workpiece. The path to 
pick up the piece at the regrasping station and the path to place the 
piece in the machine are dependent on the workpiece. 
Execution is the period during which the task described in the instruction 
is performed. Execution phase consists of three major activities: (a) 
acquiring a workpiece from a bin; (b) determining workpiece orientation in 
the hand; and, (c) placing the piece at a goal site. 
For workpiece acquisition, the following program is used: 
Workpiece acquisition begins with the selection of candidate hold sites in 
the bin. The algorithm selected for initial investigation determines solid 
square regions of a specified size which are a specified distance apart. A 
typical image of a bin is shown in FIG. 9. Solid regions in a binary image 
as shown usually correspnd to workpiece surfaces without holes or grooves. 
Therefore, by identifying solid regions, (as previously described) 
surfaces on which a vacuum cup may grasp a piece are identified as 
candidate holdsites. 
The robot assembly moves to the bin view pose. A binary image is formed 
using the arm camera 42 and solid regions in the image are located. The 
solid region closest to the center is initially selected for attempting 
acquisition. In this embodiment, the vacuum cup 28 is flexibly mounted to 
the wrist to accomodate a surface angle upon contact. The location of a 
solid region in the image is converted to base coordinates using a 
conversion from pixels to encoder counts and a pair of absolute locations 
in the arm and image coordinates. 
Following visual analysis, the assembly 20 moves to the descent pose over 
the selected site. The hand 26 is then unlocked. The hand descends 
vertically until either contact with a workpiece is sensed or until the 
bin floor is reached. In the latter situation, the assembly returns to the 
descent pose. If contact is sensed, the vacuum is turned on and the hand 
continues to descend slowly until either grasping is sensed or until the 
hand is moved downwardly a fixed distance. 
Following successful acquisition of a workpiece from the bin, workpiece 
orientation in the robot hand is found. First, the robot assembly 20 moves 
to the first presentation pose (FIG. 6) and a check is made to ensure that 
the piece is still being grasped. If the piece was not dropped, an image 
is formed using the workstation camera 44 and image features are 
extracted. The robot assembly 20 moves to the second presentation pose. A 
second image is formed using the workstation camera 44 and image features 
are extracted. Stereo correspondence between features appearing in both 
images is performed. The camera model is used to locate feature points in 
space. The located points are matched with the stored three-dimensional 
workpiece model to determine the relationship of the actual workpiece 
coordinate system to the stored model coordinate system. 
If a high quality feature match is not found, the arm moves to the supply 
bin drop-off pose, releases the piece, and the system returns to the 
beginning of the execution sequence. 
Other modifications to the invention will be apparent to those skilled in 
the art. Different types of hands may be used. For the imaging system 
camera positioning may be varied. For example, the arm camera may be fixed 
to view the bin(s) at all times and not affixed to the arm. 
Two intermediate goalsites have been described for a fuller understanding 
of the invention. However, once the position of the workpiece is 
determined at the presentation pose, the robot system may transport the 
piece directed to a fixed site or reorient in the hand. The sensing of the 
workpieces, regardless of where located, bin, goalsite, etc., may be 
non-tactile sensors, such as sensors which receive energy which could 
provide the necessary data, e.g., sound or light energy; or tactile 
sensors which would provide the necessary data by contacting the 
workpiece. 
The specific robot assembly described has a number of axes. The robot arm 
has been described as including a wrist. The degrees of freedom of motion 
may be divided among the robot assembly and workstation structures to 
accomplish the necessary movements.