CA1318954C - Vision seam tracking method and apparatus for a manipulator - Google Patents

Abstract

VISION SEAM TRACKING METHOD AND APPARATUS
FOR A MANIPULATOR
Gregory Webb Keith R. Wehmeyer ABSTRACT OF THE INVENTION
A three-dimensional vision seam tracking method and apparatus for controlling the motion of the center point of a tool mounted on a manipulator. The manipulator is provided with a control to operate the manipulator members to move the tool center point at programmed velocities along a programmed path corresponding to the seam. The manipulator is provided with a vision system including a laser scanner/camera head mounted ahead of the function element. The control is programmed with dynamically varying orientation angles to control the orientation of the tool during seam tracking. The laser scanner/camera head scans the seam at predetermined time intervals. A
set of coordinates for the center point of the seam in a coordinate system relative to the camera is provided for each scan. The sets of camera coordinates are transformed to the coordinate system of the manipulator using transformation matrices employing the orienation angles in effect at the time of the scan for the set of camera coordinates being transformed. The transformed coordinates are stored in a FIFO buffer until needed.
The seam tracking routine of the manipulator control removes FIFO entries at some later time and alters the manipulator's programmed path to move the tool center point along the seam.

Description

13~ 89~

l VISIQ~ s--æ-AM T~AC~I~G METHOD ~ APPARATUS
FOR A MA~IP~LATU~
Gregory W~b~
K~ith R. Wehmeyer TECHNICAL FIE~~
The invention relates to a three-dimensional vision seam tracking system for a manipulator, and more partîcularly to such a system capable of compensating for dynamically varying orientation angles.
BACKGROUND A~T
A manipulator prov/ded w:ith the vision seam tracking system of the present invention can be employed in many processing applications including applying sealant, contour mapping and, in g~neral, in any processing application requiring adaptive path control. For purposes of an esemplary showing, the invention will be taught in its application to seam welding.
Once a manipulator is taught a program, it will move along the programmed path with high repeatability. While this is satisfactory for many applications, others require adaptive path control.
For e~ample, in a welding operation, if the parts being ~oined are not uniform in seam or ~oint location by virtue of manufacturing tolerances, heat distortion, or the like, the manipulator will weld along its taught path, even though the seam may be located elsewhere.
In order to overcome this problem, and to broaden the applications in which manipulators can be usea, prior art workers have devised a number of vision guided seam tracking systems. The teachings of U.S.
Patents 4,542,279 and 4,590,356 are Pxemplary of such systems. In general, the prior art approach has required the vision system to guide the manipulator.
Because of the guidance requirement, constraints are 2 13~9~

1 put on the prior art systems. Furthermore, prior art systems generally require a constant relationship between the camera and the part, inhibiting orientation changes while tracking takes place.
As applications for vision-guided welding systems increase in comple~ity, more processing and interpretation of data are required. Prior art workers ha~e attempted to maximize the proceesing capabilities of the vision guiding equipment. A major drawback to this approach, however, lies in the requirement for large amounts of manipulator information in order to make the necessary coordinate transformations and to control the entire weld process, resulting in communication and performance limitations. As a further problem, the large amounts of manipulator information must be processed in real time.
The present invention is based upon the concept of guiding the manipulator by means of the manipulator control and requiring of the vision system only data as to the position of the seam in camera coordinates. The present invention includes means for taking camera data in camera coordinates and tran~forming the camera coordinates to manipulator coordinates independently of orientation angles. Matrices are used which accommodate the attitudinal changes at the camera introduced by changes of the orientation angles. The manipulator control has access to and the capability of processing the large quantity of manipulator data ~position, orientation, etc.) required to build the transformation matrices. The result is a vision seam tracking method and apparatus capable of compensating for dynamically varying orientation angles. The manipulator's orientation angles may be user programmed as re~uired by the weld process to achieve desired attack an~ push/pull angles and the like, without ~318~

3 _ -degrading tracking perfQrmance. This capability allows the welding of many parts which could not be performed if the orientation angles had to remain ~ixed.
Here described is a three dimensional vision seam tracking method and apparatus for a manipulator. The manipulator is provided with a vision system including a laser scanner/camera head which previews the seam to be welded ahead o~ the weld torch. Data from the vision system is supplied to the manipulator control. The manipulator control derives from manipulator data matrices by which the vision system data in camera coordinates aan be transformed to manipulator coordinates. The trans~ormed data ia stored in a FIFO buf~er until required. The orientation angles o~ the manipulator can be user preprogrammed and can vary dynamically throughout the seam welding process, without degrading the seam tracking function. The seam tracking routine of the manipulator control removes FIFO entries at some later time and then alters the manipulator's preprogrammed path to pass through them.
In accordance with a first aspect of the invention there is provided, a method controlling a manipulator to sffect motion of a tool centerpoint associated with a function element carried by the manipulator such that the tool centerpoint mov~s along a path datected by a sensor during execution of a program defining a cycle o~ operation of the manipulator, the method comprising the steps o~:
a. periodically producing intermediate point signals representing coordinates of an intermediate location along a prede$ermined path between two programmed locations, the intermediate location defining displacement along the predetermined path to be ef~ected in a predetermined time interYal;
b. producing target coordinate signals in response to the intermediate point signals and the sensor, the karget coordinate signals representing coordinates of a location to which the tool centerpoint is to be .~

'~

- 3a ~
moved in the direction fr~m the tool cent~rp~int current location toward the detected path;
c. aalculating a li~it value defining, for the predetermined time interval, a maximum deviation of the path o~ a~tual tool cent~erpoint motion ~rom the predetermined path;
d. calculating aoordinates o~ a predicted location of the tool centarpoint, the predicted location being de~ined by the intermediate point and an of~set between the ¢urrent tool centerpoint location and the immediately preceding intermediate point;
e. cal~ulating the magnitude o~ the distance between the target point an~ the predicted location;
f. determining whether the magnitude of the distance exceeds the limit value; and g. modifying the target coordinate signals in response to determining that the magnitude of the distance exceeds the limit value, the modi~ied target ; coordinate signals de~ining a location to produce a d~viation equal to the limit.
In accordance with a second aspect of the invention there ~ is provided, an apparatus for controlling a manipulator to :~ e~fect motion o~ a tool centerpoint associated with a function :~ element carried by the manipulator su¢h that the tool centerpoint moves along a path detected by a sensor during : execution of a program defining a cycle o~ operation o~ the manipulator, the apparatus comprising;
a. means for periodically producing intermediate point signals representing coordinates of an intermediate location along a predetermined path b~tween two programmed locations, the intermediate location defining displacement along th~ predetermined path to be ef~ected in a predetermined time interval;
b. means for producing target coordinate signals in response to the intermediats point signals and the ~r ~

~ 3~8~
- 3b sensor, the target coordinate signals representing coordinates of a location to which the tool centerpoint is to be moved in the direction from the tool centerpoint current location toward the detected path;
c. mean~ for calculating a limit value defining, ~or the predetermined time interval, a maximum deviation o~ the path of actual tool centerpoint motion ~rom the predetermined path;
d. means for calculating coordinates of a pxedicted location of the tool centerpoint, the pred:Lcted location being ds~ined by the intermediate point and an of~set between t~e current tool centerpoint location and the lmmediately preceding intermediate point;
e. means for calculating the magnitude of the distance : between t/he target point and the predicted location;f. mean~ for determining whether the magnitude of th~
distance exceeds the limit value; and y. ~eans responsive to the determining means for modifying the target coordinate signals to define a loca~ion resulting in a deviation e~ual to the limit.
Embodiments of the invention will now be described with reference to the accompanying drawings wherein:
: 25 Figure 1 i5 a simplified, diagrammatic representation of an industrial manipulator and its control, provided with the vision seam tracking system e~bodying the present invention.
Figure 2 is a block diagram of the control of Figure 1.
Figure 3 is a schematic representation of the manipulator shown in the manipulator coordinates.
Figure 4 is an elevational view, partly in cross section, illustrating the manipulator wrist and itq axe~ of rotation.
Figure 5 is a schematic representation o~ the ~`

~ ' 4 7 318~

hand coordinate system and the orientation angles D, E, R.
Figure 6 is a schematic re]?resenation depicting the motion e~ected b~ th~ control:Led manipulator in an automatic mode o operation.
Figures 7 and 8 ar~ flow chart~ of the two principal control procedures efecting motion control.
Figures 9 13 are 10w chart~; of subroutines and major segments of the flow chart o Figure 7.
Figure 14 is a diagrammatic representation of the laser scanner/camera head tracklng procedure.
Figure 15 is a diagrammatic representation of the corrections made in the progra~med path o~ the tool center point in acco~dance with ~he vision seam tracking system embodying the present invention.
Figure 16 is a flow chart illustrating the overall repetitive loop performed by the vision data processor.
Figure 17 is a flow chart r~presenting the message handling procedure of tha vi~lon data processor.
~ igure 18~ iæ a ~low chart illustrating the procedure or aetivating the vision system utilizing a user de~ined template.
Figure l9 is 3 flow chart ~llustrating the procedure for determlning whether the most recently : produced set of vision system seam coordinates are to be loaded into the FIF0.
Figurs 20 is a flow chart illustrat;ng the procedure for deactivating the vision system Figure 21 is a flow chart depicting the overall procedure for producing coordinal:es o~ an intermediate point on the linear path to the seam coordina~es.
Figure 22 is a ~low chart of the detailed procedure for producing the intermediate point coordinates.

~ f ~ 8 ~
1 Figure 23 is a flow chart representing the coordinate adjust routine.
Figure 24 is a flow chart of a sufbroutine of ~igure ~.
DETAILED DESCRIPTION OF THE PREFER~ED EMBODIMENTS
For a thorough un~erstanding o t~e vision guided seam tracking method and ap~aratus embodying the present invention, it will be necessary to describe an e~emplary manipulator and its control. To this end, Figure 1 illustrates a manipulator corresponding to those manufactured by Cincinnati Milacron Xndustries, Inc., the assignee of the present invention. The manipulator shown is *xemplary only, and the present invention may be applied to any appropriate si~-lS degrees-of-freedom mechanical manipulator. While the detailed description of the preferred embo~iment will necessarily reflect the actual implementation, such detail should not be construed as limitation on the . present invention as defin~d in the app~nded claims.
:~ 20 Referrin~ to Figure 1, an esemplary robot or manipulator is generally indica~ed at 1. The man-ipula~or 1 comprise~ a base plate 2 supporting an upstanding base memb~r 3. An upper arm 4 is pivotally 1: affixed to base m~mber 3 at pivot 5. A forearm 6 is pivotally attached to upper arm 4 at pi~ot 7.
At it~ free forward end, the forearm 6 supports a : wrist 8. As ~s known in the art, the wrist 8 comprises a flrst segment 9 having an integral shaft portion 10 ro~atably supported within forearm 6 and having an axis parallel to or colncident with the longitudinal center lin~ o forearm 6. Wrist 8 further comprises a second segment 11 rotat~vely mounted on first segment 9 and having an integral slhaft portion 12. The shaft 12 terminates in a face plate 13. Affi~ed to face plate 13 there: is a welding tool 1~ and the laser . .

6 ~ 3 4 1 scanner/camera head 15 of the vision system to be described hereinafter.
The manipulator 1 is so constructed that motion of its members describe axes o~ rotation. The first a~is is the vertical base axis shown in broken lines at 16 and defined by rotation of base plate 2 and base member 3. The upper arm 4 rotates about hori~ontal shoulder axis 17 through the center of pivot 5. The forearm 6 rotates about the horizontal elbow a~is 18 e~tending through the center of pivot 7.
Wrist 8 provides three more a~es of rotation.
The first is rotation of segment 9 about the a~is of shaft 10. The second is rotation of segment 11 about an a~is perpendicular to the abutting surfaces of segments 9 and 11. The third comprises the axis about which face plate 13 rotates, which a2is is perpen-dicular to the face plate.
Rotations about base a2is 16, shoulder a~is 17 and elbow a~is 18 define the operating envelope of the center point 19 of tool 14. Rotations of wrist segment 9, wrist segment 11, and fac~ plate 13 about their respective a~es control orientations of the tool 14 and laser scanner/camera head lS at the programmed locations in accordance with programmed orientation angles.
The manipulator control is diagrammatically indicated at 20. Each of the si~ a~es of motion is servo controlled by connection of command and feed back signals to servo control 21 of control 20. Operation of welding tool 14 is accomplished by a machine interface 22 which responds to the programmed cycle of operation controlled by the cycls control 23. The cycle control 23 operates upon stored location, velocity and function data to produce control signals for the servo control 21 and the machine interface 22.

7 ~ 3189~4 1 In this particular e~emplary instance, the tool 14 is a welding torch and control of the welding process is effected through the machine interface in response to stored function signals. Control 20 also incorporates a vision data processor 24 which rleceives information from the vision sytem comprising t:he laser scanner/
camera head 15, a camera control unit 25 and a signal processing computer 26. The vision system will be fur-ther described hereinafter.
Rotations of the upper arm 4 and the forearm 6 about their respective a~es are achieved by linear motions of screws 27 and 28 throuqh nuts 29 and 30, respectively. Rotation of the nuts 29 and 30 is imparted through pulleys 31 and 32, respectively, by drive motors not shown. Rotation of base plate 2 and base member 3 is accomplished throuqh transmission 33 driving pinion 34 which, in turn, drives ring gear 35 affi~ed to base plate 2. The drive to transmission 33 is provided by motor 36.
Rotation of wrist segments 9 and 11 and face plate 13 is accomplished through concentric torque tubes ~not shown~ within forearm 6 and driven by motors 37, 38 and 39 through transmission 40. Pos;tion signals for each movable manipulator member are produced by position transducers ~such as resolvers) which may be mechanically driven by motion of the manipulator movable members, themselves, or the drive motors therefor.
Reference is now made to Figure 2 which consti-tutes a block diagram for the control 20 of Figure 1.
The control 20 is implemented using a plurality of microprocessors communicating through a commo~ bus.
Applicants have chosen the commercially available 80186 microprocessor produced by Intel Corporation. It will be understood by those skilled in the art that other 13~833~

1 appropriate general purpose digital computers could be used to implement the control algorithms to be described hereinafter.
As indicated above, the control 20 includes a vision data processor 24. The vi'sion data processor 24 includes a microprocessor 41 and a memory 42 both tied directly to the system bus 43. The purpose of vision data processor 24 will be more fully described hereinafter.
Cycle control 23 includes a microprocessor 44 and an interrupt control 45, both ti0d directly to the system bus 43. Operating system programs e~ecuted by microprocessor 44 are stored in memory 46 and incluae programs identified as mode control 47, auto mode 48, and teach mode 49. Data defining the user specified locations, velocities and functions are stored in data store 50. Memory 46 is connected directly to system bus 43. The set of programs identified as auto mode 48 is used by microprocessor 44 to execute the cycle of operation defined by ths data stored in data store 50.
E~ecution of the auto mode programs is carried out by : microprocessor 44 independently until e~ecution is interrupted by an interrupt signal processed by interrupt control 45.
Servo control 21 (Figure 1~ is shown at 51 in Figure 2 and includes a servo supervisor which processes servo command signals produced by cycle control 23 and the servo input/output modules to be described, and associated with the actual control of the manipulator a~is drive motors. The servo super-visor comprises a microprocessor 52, a programmable timer 53 and a local memory 54 which contains the servo control process programs 55. Servo interrupt signals are periodically produced by the programmable timer 53. The period between interrupt signals de~ines the 9 3 ~
1 interval of each iteration of path control procedures e~ecuted by cycle control 23. The elements comprising the servo supervisor receive manipulator axes command signals representing increments of motion of the manipulator relative to its a~es of rotation to be effected during the iteration interval. These manipulator axis command signals are processed by the servo supervisor to produce servo command signals defining sub-increments for each of the manipulator a~es effective for sub-divisions of the iteration interval. The servo command signals are distributed to the servo input/output modules at predefined sub-intervals of the iteration interval. The serYo input/output modules use the servo command signals and feedback signals produced by position transducers to control the manipulator a~es drive motors. It will be noted that elements 52, 53 and 54 are connected to a local bus 56 which, in turn, is connected to the system bus 43 through dual port 57.
Servo input/output is accomplished by microprocessor 52, and memory 54, through drive interface circuitry 58, and manipulator axis position measuring transducer interface 59. These elements are interconnected by local bus 56. Microprocessor 52 controls manipulator axis driv~ motors 60 and 61 through amplifiers 60a and 60b in response to the servo command signals and the feedback signals according to servo input/output programs 62 stored in memory 54.
Each manipulator a~is servo command signal is compared to the corresponding current position signal as defined ~y position transducer 63 or 64 to produce a position error signal which is then applied to the drive interface circuitry 58 to produce a drive control signal for application to the asso- ciated manipulator a~is drive motor.

~ 3 ~

1 It will be noted that the block diagram of Figure 2 illustrates servo input/output intsrfaces repre-sented by blocks 65 and 67. It will be understood that each of blocks 65 and 67 represent both a driver interface circuitry similar to drive interface circuitry 58 and a machine a~is position measuring transducer interface similar to that shown at 59. It will further be understood that each of blocks 66 and 68 represents two drive motors and an amplifier and a resolver associated with each. Servo command signals represent increments of manipulator a~is motion to be effected within a few milliseconds. By contrast, the manipulator a~is command signals represent increments of manipulator member motion to be effected within the iteration interval of 10 to 20 milliseconds.
In addition to the generation of the manipulator a~is command signals, cycle control 23 controls the esecution of functions represented by input signals and associated with the tool 14 (Figure l) to be performed at programmed locations. Function commands are stored with the program data in data store 50 and are executed in accordance with function e~ecution subroutines associated with the auto mode programs 48. Control of manipulator functions is effected through inputfoutput devices, such as limit switches, push buttons, relays and solenoids. The manipulator input/output devices are directly controlled by the data input~output-device input/output board 69 in cooperation with the machine input/output interface circuitry 70. Data is trans-ferred to and from the system bus 43 through dual portdevice 71. The dual port device 71 is connected to the local bus 72 of board 69. Operation of board 69 is controlled by the local microprocessor 73 connected to bus 72 which e~ecutes programs stored in local memory 74.

8 9 ~ ~

1 Connection of the manipulator input/output inter-face circuitry 70 to board 69 is accomplished through a serial interface circuit 75. A control cabinet op-erator panel 76 is connected by parallel lines to a parallel interface circuit 77. Monitoring and control of the interface circuits 75 and 77 are accomplished by the microprocessor 73 in response to respectively the manipulator input/output program 78 and the operator panel input/output program 79 stored in local memory 74. Current conditions of the manipulator input/output devices are reflected in device status signals trans-ferred from the panel 69 through the dual port device 71 to the cycle control 23. Function command signals produced by cycle control 23 in accordance with the stored program of operation are transferred over system bus 43 through ~ual port 71 and ultimately to the appropriate manipulator inpllt~output interface device by the serial interface 75.
In addition to signal e~chanqe between manipu-lator device interfaces, the control permits exchange of data through the panel 69 and its associated interfaces. While the cycle control 23 is operating under the auto mode programs 48, location data may be exchanged in response to function commands. This in-process data e~change takes place between ~he vision system signal processing computer 26 (see also Figure 1) and the control 20 through panel 69. Location data from the data store 50 is transferred from system bus 43 to panel 69 through its dual port device 71. The microprocessor 73 operating under control of the vision system data input/output program 81 stored in its local memory 7~ transfers the initialization data ~rom dual port device 71 to the serial channel interfacP 82 ~or transmission to vision system signal processing computer 26. In reverse, data from the vision system 12 13~8~

1 signal processing computer 26 is input to the serial channel interface a2 and transferred therefrom on local bus 72 to the dual port device 71. From there data is available to the vision data processor 24 on system bus 43.
In addition to the in-process data e~change just described, program data may be e~changed between data store 50 of memory 46 and a bulk data store through a bulk data transceiver 83. E~amples of a bulk data store include serial tape and data disk devices. Data is e~changed by microprocessor 73 operating in response to the bulk data transceiver input/output program 84 stored in memory 74. Data is transferred to and from the bulk data transceiver 83 through the serial channel interface 82 to the local bus 72 and through the dual port device 71 connected to system bus 43.
A final data inputfoutput device, the teach pendant 85, is associated with the e~ecution o~ teach mode programs 49 by the cycle control 23. Location and function data defining a program~ed cycle of operation may be generated by an operator using the teach pendant 85. This pendant per~its manual operation of manipu-lator 1 and the storage of location and function data in data store 50. As with other data input/output devices, data is transferred to and from pendant 85 through the serial channel interface 82 to local bus 72 and therefrom through the dual port device 71 to the system bus 43. As the teach mode of operation does not form a part o the present invention, further details thereof shall not be given herein. Additional details of the operation of the manipulator in the teach mode may be found in U.S. Patent 3,920,972.
Since it is an object of the auto mode programs 48 to control the motion of the manipulator members to produce motion of the tool center point 19 along a ~~` 13 ~ 3~

1 straight line path between programmed locations, a schematic representation of the manipulator can be useful in deseribing the auto mode control algorithms.
Such a schematic representation is depicted in Figure 3.
In Figure 3 a first line segment 87 is shown corresponding to the manipulator upper arm 4 of Figure 1. A second line segment 88 is shown corresponding to the manipulator forearm 6 of Figure 1. The shoulder a~is 17, elbow a~is 18, wrist 8 and the tool center point 19 of Figure 1 are also indicated in Figure 3.
The wrist 8 and tool center point 19 are shown joined by line segment 89.
The line segments 87, 88, and 89 of Figure 3 are dimensioned respectively with lengths L~, L2, and L3.
Length Ll represents the length of the upper arm 4 between shoulder a~is 17 and elbow a~is 18. The length L2 represents the length of orearm 6 from the elbow a2is 18 to the center of wrist 8. Finally, length L3 represents the distance between the center of wrist 8 and the center point 19 of tool 14.
In Figure 3, the line segments 87, sa, and 89 are shown in relation to the rectangular coordinate s~stem X, Y, Z of the manipulator 1. The manipulator coordinate system is frequently referred to as the manipulator "world~ coordinate syst~m. The origin of this coordinate system corresponds to a point on the manipulator 1 located at the intersection of vertical base a~is 16 and horizontal shoulder a~is 17. It will be noted from Figure 3 that the vertical Z axis of tha manipulator coordinate system is coa$ial with base axis 16. ~hen the manipulator 1 is in its "ho~e~ position, the horizontal Y a~is is coa~ial with shoulder a~is 17 . and the horizontal X a~is is perpendicular to and passes through the intersection of base a~is 16 and 14 ~ 3 ~

1 shoulder a~is 17.
A second coordinate system i5 used in association with the manipulator. This coordinate system, ~, y, and z, has its origin at the center point of wrist 8.
The center point of wrist 8 i~; defined as the intersection of its three axes of rotation. This second coordinate system is frequently referred to as the "hand" coordinate sy~tem. The line segment 89 of Figure 3, representing the distanc~ between the center point of wrist 8 and the center point 19 of tool 14 comprises a vector in the hand coordinate system.
At any given instant in time the three orientation angles define the orientation of the hand axis system x, y, z to the manipulator a~is system X, Y, Z. As a consequence, and as is known in the art, transformations from the manipulator coordinate system to the hand coordinate system and from the hand coordinate system to the manipulator coordinate system can he performed and stated as follows:
X
Z z manipulator hand s y = ~Q]-l y hand manipulator Where rQ] represents an appropriate transformation matrix.
Returning to Figure 3, rotation of the manipulator about its base asis 16 is dimensioned by the magnitude of angle a. Rotation of the manipulator upper arm is dimensioned by the magnitude of angle b.
Rotation of the manipulator forearm is dimensioned by the magnitude of angle c. It will be noted that angle c is measured from the center line of the manipulator upper arm. It will be appreciated by those skilled in ~ 3~

1 the art with reference to Figure 3 that the lengths Ll and L2 and the magnitudes of angles a, b, and c completely define the location of the center point of wrist 8. If line segment 89 were coaxial with line segment 88, then lengths L1, L2, and L3 together with the magnitudes of angles a, b, and c would completely define the location of the tool center point 19.
The introduction of rotation at the a~es of wrist 8 provides for orientation of a function element associated with tool 14 through the tool center point 19. The wrist 8 is shown enlarged in Figure 4. As indicated above, the wrist comprises first segment 9 with its integral shaft 10 by which the wrist 8 is attached to the manipulator forearm 6. The wrist further comprises second segment 11 and its integral shaft 12, which supports face plate 13. The first axis of rotation 90 of wrist 8 is defined b~ rotation of wrist segment 9. The second a~is of rotation of wrist 8 is defined by rotation of wrist segment 11. This asis is indicated at 91 and is perpendicular to the abutting faces of wrist segments 9 and 11. The third a~is of rotation of wrist 8 is defined by rotation of face plate 13. This a~is is indicated at 92 and is perpendicular to 4ace plate 13. Rotation about wrist a~is 90 is designated pl. Rotation about wrîst axis 91 is designated p2. Finally, rotation of face plate 13 is designated by p3.
Figure 5 illustrates how the orientation angles are defined with respect to the hand coordinate system, having its origin at the center of wrist 8. The axes of the hand coordinate system (x, y, z) would be parallel to the X, Y, and Z a~es, respectively, of the manipulator (world) coordinate system when the orientation angles are zero (0). The orientation angles D, E, and ~ define rotations relative to the ~ 3~ 3~
1 hand coordinate system as follows:
(1) E is the magnitude of a rotation about 2 to align s and y with ~1 and Y
respectively;
(2) D is th~ magnitude of a rotation about Yl to align ~1 with ~2 and z with Z2 and (3) R is th0 magnitudle of a rotation about x2 to align Yl with y3 and Z2 with It will now be appreciated by one skilled isl the art that, when the lengths hl, ~2, and L3 are known, the position of tool center point 19 can be complet~ly defined in terms of the three translation values X, Y, and Z and the three orientation angles D, E, and ~.
In the control system embodying the present invention, when a path is being computed in the auto mode, or the manipulator 1 is being taught in the teach mode, the manipulator ~world) ~oordinate system, X, Y, Z, D, E, R
is used. ~ignals to the servo system must be in terms of joint angle ~ositions ., motor position~), requently termed ~joint space". AS a result, th~se siqnals are in terms of a, ~b, c, pl, p2, p3. As is known in the art, there :25 are matri~ transormation operations for transformations between X, Y, Z, D, E, R and a, b~ c, pl, p2, p3.
Reference is now made to the geometric schematic representation of Figure 6. In this figure, the straight line segment 93 repr~sent~ the path of motion along which the tool center po~nt 19 is to procaed in traveling from point P0 to Pl defined by stored input signals. Th~ man~pulator control in auto mode interpolates intermedlate points Pi along this straight lin~ path, effecting the path motion in 17 ~3~8~

1 incremental intervals aT. The coordinates of points P0, Pl, and the intermediate points Pi are all defined with respect to the manipulator ~world~
coordinate system. Assuming that the tool center point 19 is to start from rest at point P0 and to come to rest again at Pl, then the path control scheme provides automatic acceleration and deceleration to and from a programmed velocity. While the increments defined by the intermediate points Pi are produced in accordance with a fi~ed increment interYal period, the actual incremental distance between points Pi varies as the velocity of the tool center point varies. Thus, the incremental distance ~S which corresponds to the distance traversed during a constant incremental interval ~T is seen to vary between the constant velocity portion of the motion and the accPleration and deceleration phases of the prescribed motion.
In the practice of the present invention, ~T is equal to 15 milliseconds. Thus, the servo input/output board 51 receives position commands from cycle control board 23 every 15 milliseconds, and the cycle control board 23 is calculating fast enough to provide these signals. The cycle control 23 computes how far the tool center point can move along line P0, Pl based on acceleration, deceleration, programmed velocity, and the like, and provides the servo input/output board 51 with appropriate joint space signals to accomplish this.
The overall cycle of operation of the manipulator 1 in the automatic mode is illustrated by the flow chart of Figure 7. At process step 94, the control initializes the span data required to interpolate the motions between the programmed points P0 and Pl at the programmed velocity Vp. Values for the Programmed span length Sp, and the coordinate 18 1 3~8~;3~

1 component ratios NCc ars produced by the procedure associated with step 94. At process step 95, an increment along the path is interpolated to produce the incremental distance ~S and the accumulated incremental distance Sk. Proce~ss step 96 calls the subroutine to effect a transformation of the interpolated intermediate point from manipulator coordinates to joint angle positions. The subroutine associated with process step 96 produces the a~is command signals and stores these signals in a buffer for access by the servo supervisor on the occurance of the servo interrupt signal. Following the e~ecution of the ~ubroutine of process step 36, a test is made at decision step 97 to determine whether or not a flag indicating tAat no deceleration is required as been set. If the no deceleration flag has been set the process continues at decision step 102 where it is determined whether or not the present increment being interpolated is the last increment of the current linear span between programmed locations. If not, another incremènt is interpolatea through the connector L 2. If the current increment is the last increment of the span, the process continues through connector L-3 to process step 104 where the function associated with the end of span is e~ecuted. Thereafter, decision step 105 determines whether or not the present location is the last location of the program. If not, further programmed location data will be processed hy proceeding through connector L 1. If it is the last, the entire programmed cycle of operation will be re-executed as indicated by terminal 106.
Returning to decision step 97, had it been determined that the no deceleration flag was not set, then the execution of the procPdure continues at process step 99. This step c~lculates the distance l remaining SRD in the current span prior to the point at which a deceleration to stop must begin. At decision step lO0, the magnitude of the remaining distance SRD is tested against the current incremental distance a s to determine whether or not the remaining distance SRD is less than the current incremental distance ~ S. Xf the remaining distance is not less than the current incremental distance, the process continues at step lOl where execution of a velocity modification subroutine is intiated by a subroutine call. A velocity modification subroutine may respond to any process parameter to produce a change in the incremental velocity to adapt the tool center point velocity to process changes or to constrain manipulator a~is velocities to their associated limits. Upon completion of this subroutine, the procedure continues through the loop at process step 95 whsre the span increment is interpolated using the most recently created value of incremental velocity.
Returning to decision step lO0, had it been determined that the distance remaining SRD was less than the incremental distance ~S, then execution of the procedure would continue at decision step 102.
This step determines whether or not motion is to continue through the programmed location defining the end of the current linear span. If the tool center point is to come to rest at the end of the current span, then decision step 102 will determine that the programmed location is not a continue point, and the e~ecution of the procedure will continue at process step 103 where e~ecution of the deceleration subroutine is initiated by the call setting the final velocity parameter signal Vf equal to zero. Upon completion of the execution of the deceleration subroutine, execution of the overall procedure continues at process ~ J~

1 step 104 where the function command associated with the end of the current span will be e~ecuted. Thereafter, at decision step 105, it is determined whsther the present programmed location is the last programmed location of the program. If it is, program e~ecution is resumed by beginning at the first location of the stored program through the terminal 106 indicating that the program should be recycled. If the present location is not the last location of the stored program, e~ecution proceeds through the connector L-l to interpolate the ne~t programmed lin~ar span.
Referring again to decision step 102, if it had heen determined that the programmed location for the end of the current span is one through which motion of the tool center point is to continue, execution of the overall cycle control procedure advances to process step 107 where a new end position for the current span is computed. Because the interpolated intermediate positions are spaced apart by an incremental distance de~ined by the incremental interval period and the effective increment velocity, it ;s anticipated that when motion is to continue through a programmed position the distance remaining in the span from the deceleration decision position to the programmed end position will not always equal an integral multiple of the current incremental distance. To account for this discrepancy, the current span is effectively terminated at the integral multiple of the current incremental distance nearest the programmed end position. To accomplish this, the distance remaining Sp - Sk is divided by the incremental distance ~S to produce the interger multiple and remainder. The current span length is then reduced by the remainder to produce the continue span length Scp. This distance then redefines the end position of the current span and the 21 1 3189~4 1 start position of the ne~t span.
At decision step 108, it is determined whether the angle included between the current span and the ne~t span is less than 120. If the included angle is less than 120 the motion of the tool center point will be terminated at the final location Pl of the currsnt span. The determination of the value of the included angle relies upon the rule of cosines. Two values for the span distance between the programmed location at the beginning of the present span and the programmed location at the completion of the next span are produced independently. That is, the span Length s2 is computed as the sum of the squares of the rectanyular coordinate components while the value s2 (120) is computefl as the sum of the square of the span length Scp computed in process step 107, the square of the span length Sn of the ne~t span, and the product of the ne~t span length Sn and the span length oF the present span Scp. If ST is less than ST ~120), then the included angle is less than 120.
If the included angle is less than 120, then it is necessary to bring the motion to a halt before proceeding and the process would again continue through the call to the deceleration procedure at process step 103. If the included angle is 120 or greater, then the process continues at decision step 109. Decision step 109 determines whether or not the program velocity of the ne~t span is grPater than or equal to the current programmed velocity. If the programmed velocity of the ne~t span is less than the programmed velocity of the current span, then it is necessary to effect a deceleration in the current span so that the motion in the ne~t span will be started at the velocity programmed for that span. Following the deceleration 22 ~ ~18~

1 procedure called by process stap 110, the overall cycle is continued at process step 104 through connector L-3. If the programmed velocity for the next span is in fact greater than or equal to the programmed velocity of the current span, then the process from dacision step 109 continues at the process step 111 which sets the no deceleration flag. Thereafter, the overall cycle of operation previously described is resumed through the connector L-2.
It will now be apparent that the overall cycle control comprises the repeated e~ecution of the stored program. Motion between programmed locations involves the iterative production of incremental moves along the straight line path therebetween. The overall cycle comprises manipulator motion and the e~ecution of programmed functions associated with the programmed locations.
Referring to Figure 8, the servo interrupt service routine e~ecuted by the servo control begins at decision step 112, where it is determined whether or not the buffer in which axis command signals are temporarily stored is empty. I~ the buffer is Pmpty it is assumed that the cycle control ha~ reached the end of span and an end of span signal is sent at process step 113. If the buffer is found not to be empty by decision step 112, then the axis motion increments represented by the a~is command signals are retrieved from the buffer at step 114. At terminal 115 the servo interrupt routine is ended and e~ecution of the overall cycle of operation by the cycle control 23 resumes. As was previously stated, the servo supervisor divides the increments, represented by the axis command signals, into subincrements which are used to command servo position during predetermined subintervals of the iteration interval. The process of division and output 23 ~3~89~

1 f the servo command signals continues concurrently with the e~ecution of the overall automatic mode procedure by the cycle control 23.
The description of the major segments and subroutines associated with the flow chart of Figure 7 will be described with respect to Figures 9-19. The flow chart of Figure 9 corresponds to the process step 94 of Figure 7. At process step l:L6, programmed span data corresponding to preprogrammed input signals is recalled from data store 50 of memory 46. The start point P0, end point Pl and programmed velocity Vp define the motion to be next e~ecuted by tool center point 19. The programmed coordinate data recalled from memory are shown subscripted in correspondence with the programmed point designation. At process step 117, the total span length is computed using the rectangular coordinates of the two programmed points and e~tracting the square root of the sum of the squares of the coordinate components. At process step 118, the in-a~is component ratios of the programmed coordinate system are computed by dividing the coordinate components by the span length. These ratios are designated ~XX' Nyy~ Nzz, ~DD' ~EE' and NRR. Axis component ratios for the orientation angles are computed in the same way as the component ratios for the rectangular coordinate axes. Once this preliminary computation of span variables is completed, the overall cycle of operation continues through the terminal 120 to the process block 95 of Figure 7.
Referring now to Figure 10, the process step of interpolation of a span increment corresponding to block g5 of Figure 7 is e~panded into a more detailed flow chart. Beginning at decision step 121, the determination is made whether or not an adaptive or nonprogrammed velocity flag has been set by the :; ~

l subroutine called by process step lOl of Figure 7. If there is no adaptive velocity function programmed for the current span, or if the adaptive velocity function has not resulted in a change to the incremental velocity value, then the adaptive velocity flag will not be set and the process will continue at decision step 122. Decision step 122 tests the value of the current incremental velocity signal VK, that is, the incremental velocity of the current iteration K, against the value of the programmed velocity signal Vp to determine whether or not the current incremental velocity is less than the programmed velocity. If so, the process continues at process step 123 which calls the acceleration subroutine.
lS The effect of the acceleration subroutine is to produce a new value for the incremental velocity in accordance with a predetermined function of acceleration and produce a corresponding distance increment ~S. Thus, at the beginning of a move where the tool center point starts rom rest, the initial value of the incremental velocity shall he zero, the programmed velocity will, of course, be a non-zero valus, and the call to the acceleration subroutine at process step 123 begins the change of the incremental velocity values VK and produces an incremental dis-tance signal AS. When the incremental velocity VK
is not less ~han the programmed velocity Vp, it is not necessary to calculate a new value for the incremental distance signal, and it is only necessary to increment the iteration counter as indicated by process step 12S. In either event, the process ultimately resumes at process step 126 where the value of the accumulated increment signal Sx is calculated in accordance with the equation set forth to the right of process step 1~6. The overall cycle of operation is ~3~g~ ~

1 then continued by the continue of flow chart terminal 127. At this point, the overall cycle of operation continues by the call o~ ~rocess step 96 of Figurs 7, calling the transform subroutine illustrated in the flow chart of Figure 11.
P~e~erring to Figure 11, at process step 128, the values of the end point coordinate signals repre~enting coordinates in the manipulator coordinate system o~ the accumulated interpolation distance are calculated.
This is accomplished by summln~ the in-asis components of the interpolated incremental distanc~ represented by the increment distance signal ~ S with the previous values of the end point coordinate signal~ subscripted - K-l. At this point step 123 calls the coordinate ad~ust routine to be described hereindfter. This routine will supply adjusted coordinates if required, for e~ample, to follow a seam. This routine also limits the magni~ude of the devia~ion off the programmed path in accordancs with a user specified parameter. Thereafter, ~he coordinates with reference to the program coordinate system of the interpolatea point are transformed to ~oint angle positions a, b, c, pl, p2, p3 hy step 130. To avoid inverse trigonometric functions, an iterative approximation algorithm is used that produces an error function based -.
on the current joint angle positions. Details of this approach are shown in U.S. Patent 3~909,600, which describes this transformation algorithm. At process step 131, the change in the joint angle positions is calculated and at the process step 132, the incremental difference of the joint angle positions are stored in the buffer for access by the servo interrupt servicel routine. When the transformation routine is completed, the processing of the overall 26 :~318~4 1 cycle of operation is resumed by the return through ths terminal 133. This, then, brings the process to decision step 97 of Figure 7. Following step 97, the overall cycle continues at process step 99 where the remaining distance to the beginning of the deceieration span is calculated and the detailed flow chart of this process step is shown in Figure 12.
Referring to Figure 12, process step 134 calculates the period required to decelerate using a predetermined value of acceleration. Thus, the appro~imate time reguired to decelerate to stop is equal to the ~uotient of the current incremental velocity vR and this constant of acceleration. At process step 135, a value of a deceleration distance signal representing the distance requirea to decelerate is calculated assuming that an average velocity equal to half the current incremental velocity would be in effect during the interval calculated in process step 134. Fina~ly, at process step 136, a value of a span remainder signal SRD is calculated representing the remaining distance within the programmed span length from the current interpolated position to the point at which the just calculated deceleration distance must begin. Thus, the distance remaining SRD is equal to the programmed span length Sp less the portion of the span represented by the accumulated int~rpolated distance SK and the portion of the span necessary to decelerate SD. Followin~ this calculation, the overall cycle of operatio~ continues at decision step 100 of Figure 7 wherein it is determined whether or not the remaining distance SRD is less than the current effective incremental distance ~ S. If the remaining distance SRD is not less than the current effective incremental distance ~ S, then another increment of that distance can be interpolated. If, however, the 27 ~ 318~

1 current effective incremental distance ~ S is greater than the calculated remaining distance SRD, it is necessary to immediately begin deceleration. The deceleration subroutine is illustrated in the flow chart of Figure 13.
The deceleration subroutine iteratively produces decreasing values of the incremental velocity variable VK in accordance with a predetermined function of acceleration. ~t the start of the acceleration subroutine, the incremental velocity is recomputed to adjust for round-off errors of the interger arithrnetic e~ecuted by the computer used by applicants. Referring now to Figure 13, process step 138 sets the deceleration iteration counter equal to zero. At process step 139 the period of deceleration TD is calculated as two times the quotient of the as yet une~ecuted span length and the change in velocity between the current incremental velocity VK value and the final velocity VF at which the current span is to finish. The final velocity VF is zero in all cases e~cept when the ne~t programmed position is a continue point and the included angle between the current span and the ne~t span is equal to or greater than 120. At process step 140, the number of iterations RD
required to effect the deceleration is computed as the quotiPnt of the calculated period of deceleration TD
and the predefined increment interval ~ t. At process step 141, a new value for the incremental velocity VK is calculated as the sum of the final velocity VF and two times the quotient of the remaining distance of the current span and the calculated period for deceleration TD. As was previously stated, this recalculation of the incremental velocity ~K is necessitated by the use of interger arithmetic in applicant's digi~al computer.

28 ~ ?~ 1 8 ~ ~ ~

- 1 Also at process step 141 an incremental change in velocity signal ~YKD is computed. This signal determines the change of magnitude of velocity for each iteration of the deceleration subroutine. The incremental change of velocity signal is calculated by dividing the velocity difference between the initial and final velocities by the number of deceleration iterations KD. ~t process step 143, the deceleration iteration counter is incremented. At: process step 145, the incremental distance ~S is calculated as the product of the average incremental velocities of the current and previous increments and the incremental interval signal Q t. At process step 146, the accumulated interpolated distance SK is calculated as the sum of the previous interpolated distance and the incremental distance ~ S. At process step 147, the transformation subroutine is called and at decision step 148 the iteration counter is tested for equality with the previously calculated deceleration it~ration value. If they are equal, the deceleration subroutine is complete and the overall cycle of operation is resumed by the return of the terminal 149. However, if the iteration counter is not equal to the deceleration iteration value, then a new increment velocity VRH is calculated at process step 148a as the difference between the current increment velocity VK and the change in velocity VKD. Thereafter, the deceleration iterative loop is repeated by the return line bringing processing back to process step 143.
The manipulator 1, its control 20, and its basic mode o~ operation having been described, reference is again made to Figure 1, and in particular to the vision system 15, 25, and 26. Applicants have chosen to use the commercially available vision system produced by Optische Industrie De Oude Delft of Delft, Holland, ~3~8~
1 under the mark SEAMPILOT . It will be understood by tho e skilled in the art that any appropriate vision system can be used, capable o scanning a seam and providing vision data procassor 24 ~see Figure 2) with points along the seam in camera coordinates.
~s is known, the laser scanner/camQra head 15 contains a las~r focusad onto a servo controlled mirror, generating a side-to-sida scanning motion o~
the laser beam. The camara portlon o~ the laser scanner/cam2ra head 15 views the location where the laser beam strikes the workpiece. Tha laser scanner/camera head 15 i8 provided with approprlate coolant means, shielding gas for protection of the disposable optical windows, and the like, all as is known in the art.
The camera control u~it 25 turns the laser on and off, controls the scanning of the laser beam, and transmits the appropriate camera da~a to the signal processing computer 26. Th~ signal processing computer 26 compareR each co~tour pro~ile sf the workpiece obtained by the scanning laser to a specification of the profile (a template) selectea by the manipulator operator from vision data proce~sor memory 42 (see Figure 2). If the selected profile is found, the location of points of interest (the center o the seam) on the profile are computed by ~he si~nal processing computer 26 in camera coordinates. The camera coordinates are transmitted to the vision data processor 24 (SeQ Figure 2) through serial channel interface 82, local bus 72, dual port 71, and system bus 43. ~s will be apparent hereinaf~ar, tha vision system position values are uæed to offset the manipulator's programmed Ipath ln ord~r to guide the tool 14 through a dosired contour. Th~s information can also be used to adjust for part placement errors, *
Trade Mark 8 9 ~ ~

1 variations in manufacturing tolerances, heat distortions of the part, irregular seam geometries and the like. While tracking, the manipulator's position is modified to k~ep the tool 14 in the seam. The manipulator's orientation values may be programmed as required by the weld process, without degrading tracking performance. This capabiility allows the welding of many part~ which could not be performed if the orientation angles had to remain fixed.
It is necessary that the location and orientation of the camera head o~ element 15 ~see Figure 1), relative to the hand coordinate system, be determined so that a transformation matrix can be formed to relate camera coordinates to manipulator (world) coordinates.
We have already discussed transformation of hand coordinates to manipulator coordinates ~world coordinates~.
To determine the camera location in hand coordinates, camera coordinates of a known target location are determined and stored in the teach mode of operation using a calibration fizture secured within the manipulator's working volume. The manipulator (world) coordinates of the calibration fixture are obtained from the manipulator by touching the calibration fi~ture with the manipulator's end effector ~i.e., tool center 19). The same calibration fisture target point i5 then viewed by the camera from three different locations. For each location the camera coordinat~s of the fix~ure target and ~he manipula~or (world) coordinates of the tool center point are stored. Yectors from each location to the fixture target are calculated and transformed to hand coordinates by:
g X
Y = [Q3-1 Y
z Z
hand manipulator .

31 ~ 3~ 8t~ ~

1 With three sets of hand coordinate values for the target point and three corresponding sets of camera coordinates for the target point, a resulting camera coordinate system-to-hand coordinate system transformation matrix can be generated. The three sets of coordinates are used to form a first 4~4 matrix including the camera coordinates and the coordinate components of the unit vector normal to the plane defined by the three locations from which the views were taken. A second 4~4 matri~ is formed including the hand coordinates of the three locations and the unit vector normal to the plane defined by the three locations. A transformation matri~ for transforming camera coordinates to hand coordinates is then formed as the product of the inverse of the first 4~4 matri~
and the second 4x4 matri~. The resultant transformation matrix [M] operates as follows:
~SH ~SC
YSH = [M] ~SC
ZSH ZSC
hand camera XSC ~SH
Y~C == [M]-l YsH
ZSC ZSH
camera hand Unlike the [Q] matrix, the tM] matrix may be considered a constant since the relationship betwaen th~ camera coordinates and the hand coordinates does not change unless the camera is bumped or th~ like.
The relationship between the hand coordinates and the manipulator (world) coordinates will change as a function of the different orientation angles desired.
As indicated above, the vision system signal processing computer feeds camera coordinate data to vision data processor 24. The vision data processor 24 converts the camera coordinate data to manipulator ~3~8~
1 (world) coordinate data. This may be indicated as follows XS ~SC
~S ~ ~Q]~M] YSC
ZS ZSC
manipulator camera Specifically, the camera data is transformed through the [M~ matri~ which trans~orms it from ca~era coordinates to hand coordlnates. Thereafter, ~y supplying the orientation values that were in effact at the time the camera scan was taken a tQ~ matri~ can be formed. The hand coordinates are fecl through the [Q]
matri~ and manipulator (world~ coordinates resu~t.
In the practice of the present methodl the laser scan/camera head 15~ pr~views a seam ahead of the tool or weld torch 14. The seam prof~le is processed and its location is stored. At a later time, this seam data is used to guide tool center point 19 so that i~
passes directly through the tracking point. This is implemented by providing a FIF0 buffer for tha desired tracking points. The FIF0 is written to by that routine of vision data processor 24 which accepts seam data and computes the track point in manipulator (world) coordinates. The FIF0 is read by the algorithm which calls for the ne~t point and then heads toward it. The FIF0 cons~ltutes a part of tha vision data processor memory 42.
Figure 14 illustrates this type of tracking. In Figure 14, the tool center point 19 is to be considered to be located at position PK. At this same instant, the laser scanner~camera head is performing a preview scan, shown ahead of the tool center point at a position PSK. However, the tool cen~er point is tracking to the tracking point specif iad during scan PSx 5, which is five scans behind the current one.
Thus, the FIF0 would have to be at least Si2 elements ~89~4 1 long to perform this weld. When tool center point reaches position PSK 5, the laser scanner/camera head begins scan PSK+l. It is therefore obvious that two routines must be provided in the vision data processor for proper seam tracking operation. First, the seam processing routine accepts data from the vision system;
determines the tracking point; transforms the tracking point from camera coordinates to manipulator (world) coordinates and then stores the X, Y, and Z world coordinates of the seam point in the FIF0. The seam tracking routine determines the current robot position and the current seam point FIF0 entry; offsets the manipulator's path to head toward this desired point;
and when this point has been reached selects a new seam point from the FIF0. Thus, the seam processing routine is responsible for the transformation of the seam data as received from the laser scanner/camera head into manipulator (world) coordinates, and placing these results in the FIF0. The seam tracking routine is responsible for removing FIF0 entries at some later time, and then alterin~ the robots path to pass through them.
It will be appreciated from Figure 14 that the distance between the laser scanner/camera head 15 and the tool center 19 has a significant effect on the size of the FIF0. Similarly, the velocity of tool center point 19 affects the FIF0 size, slower speeds causing more spans to be taken over the given preview distance. In addition, if measurements are taken too close together, the inaccuracies of the measuring system becomes as large as the actual distance being measured.
To keep the FIF0 size within practical limits, it is necessary to devise a method which will selectively discard certain scans, so that the FIF0 is never filled ~ 31~95~L

1 to overflowing.
To implement FIF0 compression when tracking, the distance between the tool center point 19 and the center of the field of view is computed. Since a point within the field of view can be of~Eset a maximum of about two inches from the field of view center, a two inch offset is added to this distance to obtain a worst-case distance. Thereafter, the total is divided by a number of FIF0 entries (say 35) to obtain a tracking distance STF which defines the minimum separation between seam points that will be loaded in the FIF0. A seam point sensed by the camera which is not more than the tracking distance from the last FIF0 entry is not loaded into the FIF0.
The FIF0 is loaded for the tracking algorithm by acquiring seam points and storing them in the FIF0.
The acquisition begins in the searching mode and assumes no seam has yet been found. The process waits for the vision system signal processing computer 26 to send a data message, and reviews it to see if a seam has been found and matched to the operator selected template. Each trac~ing point to be loaded in the FIF0 is trans~or~ed from camera coordinates to hand coordinates using the [MJ matri~. Since this data is from a scan which began about 450 milliseconds ago, it is associated with the manipulator coordinates which were valid some 30 updates ago. By forming the EQ]
matri~ from the old D, E, R values, the location of the tracking point can b~ found in manipulator (world) coordinates. The point is then compared to the last point in the queue, and pushed into the FIF0 if it exceeds the minimum distance required ~i.e., the FIF0 tracking distance). If the seam is found for three consecutive scans, the seam start is assumed to be valid. In addition, if the seam is lost aterwards for -three consecutive scans, it is assumed that the seam is ~--lost. It will be apparent that in both seam finding and seam tracking, the process functions are substantially the same. Data is received, trans~ormed from camera to hand coordinates, converted to manipulator (world) coordinates, checked, and put in the FIF0 if needed. An error will result if the process attempts to push a new seam point onto a completely filled FIFO.
Using the transformation and control techniques of the present invention for generating seam location data in world coordinates, together with the facility for recognition of the beginning or end of a particular seam, coordinate data may be created defining location of workpiece features with respect to which a particular manipulator program is to be e~ecuted. For example, the windshield ope~ing of a vehicle body may be located by detecting the beginning or end of seams which have terminal locations in the windshield surround. The coordinates of these locations can then be used to define the necessary offsets, translations or rotations to correct the programmed coordinates for a windshield insertion routine to the actual location of the windshield opening.
The actual tracking operation can be divided into two subprocesses. ThQ first subprocess decides which point in the FIF0 is the point to head towards and then adjusts the FIF0 accordingly. If the distance from the too]. center point to the first FIFO position is equal to or less than twice the incremental distance ~S, the first FIF0 position is discarded and the next FIF0 position is recalled or popped. When the appropriate FIF0 point has been selected, the second subprocess performs the necessary calculations to move toward that position. This is illustrated in Figure 15. In Fiqure 36 ~3~

1 15, the current tool center point is shown at PK 1 The point PSl is the appropriate FIFO point founa by the seam point acqui~ition process. By subtracting the last tool center point location PK 2from the current tool center point location PK_l, the vector U1 can be determined which indicates the current direction of travel. The vector Ul may or may nol: be the programmed direction vector.
In any event, it will be noted that the target point PSl does not lie on vector Ul with the result that the manipulator must change the direction of the tool center point. To do this, the vector WI from the current tool center point location PK 1 toward the target point PSl is formed, and the target distance ST between points PK 1 and PSl is calculated as shown in Figure 15.
Since, as stated above, the target distance ST
must be two or more tirnes the incremental distance ~S, it is known that the tool center point cannot go from point PK_l to point Psl in a single iteration. As a conssquence, the coordinates of the point PSK which will be reached by the tool center point in a single iteration distance ~S along vector Wl are calculated, as shown in Figure 15. The manipulator then guides the tool center point to point SK' If the remaining distance between point PSK
and point PSl is still greater than twice the incremental distance ~S, the same calculations will be formed using point PSK as the current position of the tool center point and the point PSl as the target point. On the other hand, if the distance between points PSK and PSl does not exceed twice the incremental distance ~S, the point PSl will be discarded and 3 new target point Ps~ will be popped ~3~93~

1from the FIF0. The same calculations are then repeated to establish vector W2 utilizing point PS2 as the target point and point PSK as the current position of the tool center point.
5Reference is now made to Figure 16 which illustrates the overall repetitive loop performed by the vision data processor 24. At decision step 161, a check is made to see if the vision clata processor 24 has received a complete message from the vision system signal processor computer 26. If a complete message has not been received, at process step 162, the vision data processor 24 will communicate with the vision system signal processing computer 26 using a standard data communication protocol.
15If at decision step 161 it is determinea that a complete message has been received from the vision system signal processing computer 26, the YES side will be followed to process step 163. Process step 163 calls the proc~ss seam subroutine to see if the seam coordinates from the vision system signal processing : computer conform to the tracking FIF0 distance and should be entered into the FIF0. Decision step 164 follows process steps 162 or 163 and determines whether the cycle control 23 has transferred a message to the vision data processor 24. If the answer is no, then the process steps just described are repeated~ If the answer is yes, the vision data processor 24 will comply with the message from cycle control 23 at process step 165.
30Depending upon the nature of the cycle control message at process step 165, the appropriate one of process steps 166, 167, and 16~ will then be esecuted.
At process step 166, an appropriate template will be provided to the signal processing computer by the vision data processor 24 and the tracking FIF0 distance 38 ~.31~

1 will be calculated.
If process step 167 is e~ecuted, the vision data processor 24 will transmit an "OFF" signal to the vision system signal processing computer 26.
If process step 168 is e~ecuted, the appropriate FIF0 entry will be selected and the ne~t tool center point position PSK will be calculated and stored in a predetermined location for access by cycle control 23.
At the termination of any one of process st:eps 166, 167, and 168, the vision data processor 24 wil:L send an appropriate signal response to cycle control 23 at process step 169, depending upon which of the process steps 166, 167, and 168 had been performed. Following process step 169, the routine of Figure 16 is repeated, completing the loop.
The subroutine associated with process step 162 of Figure 16 is illustrated in Figure 17. This is a simple subroutine consisting of only one process step 170 wherein the vision data processor 24 communicates with the vision system signal processing computer in accordance with the protocol DIN 66019, as is known in the art.
The subroutine associated with process step 163 of Figure 16 is illustrated in Figure 19. In this subroutine, the irst process step 171 requires the vision data procsssor 24 to transform the coordinates Xsc, YSc, and ZSC~ supplied by the sensor-camera of the vision system to the manipulator (world) coordinates X~, Ys~ and ZS In process step 172 the coordinates Xcs, YCs, and ZCS of the vector from the last point loaded in the FIF0 toward the vision system point Xs, Ys~ and ZS is calculated by subtracting the coordinates of the previous FIF0 point FIF0 X, FIF0 Y, and FIF0 Z from coordinates of the new vision system point Xs, Ys~ and ZS

39 ~ 31 89t~

1 Thereafter, process step 172 calculates the magnitude of that vector, distance Ss, by taking the square root of the sum of the squares of vector coordinate components XGs~ Yc~ and ZCS~
At decision step 173 of Figure 19, the scan distance Ss is compared with predetermined tracking FIFO distance STF. If Ss is not equal to or greater than STF, the subroutine of Figure 19 returns to the routine of Figure 16 just ahead of decision step 1~ lS4 therein. If Ss is equal to or greater than STF, the coordinates Xs, Ys~ and ZS of the point provided by the vision syste~ are loaded or pushed into the FIFO in step 174. Following step 174, the subroutine of Figure 19 terminat s and returns to the routine of Figure 16 just ahead of decision step 164.
The subroutine relating to process step 16~ of Figure 16 is illustrated in Figure 18. The subroutine of Figure 18 begins with process step 175 whsrein a user selected t mplate is transmitted to the vision system signal processing computer 26 by the vision data processor 24 from its memory 42. Process step 175 is followed by decision step 176 wherein it is determined whether or not the vision system is on. If the answer to this is yes, the YES bypass line brings the . subroutine of Figure 18 to an end and tha process returns to the routine of Figure 16 just ahead of process step 169. If at decision step 176 it is determined that the vision tracking system is not on, an ~ON~ signal is sent to the vision system signal processing computer 26 by the ~ision data processor 24 in step 177.
In step 178, the tracking FIFO distance STF is calculated in the manner shown in Figure 18 wherein Xc, Yc~ and ZC are the coordinates of the center ~ 3~5~

1 Of the field of view of the camera in the hand system and XT~ YT, and ZT are the tool center point coordinates in the hand system. The constant 4 represents the square of the two inch offset from the center of the field of view that a tracking point may lie on. The number 35 represents the number of FIFO
positions, At the end of step 178 of Figure 18, this particular subroutine is terminated and the process returns to the routine of Figure 16 just ahead of process step 169.
The subroutine associated with process step 167 of Figure 16 is illustrated in Figure 20. Asain, this is a simple subroutine containing only one process step 179. In process step 179, the vision data processor 24 transmits an "OFF~ signal to the signal processing computer 26 of the vision system. This having been accomplished, the subroutine is terminated, returning the process to the routine of Figure 16 just ahead sf process step 169.
Figure 21 illustrates the suhroutine associated with process step 168. The subroutine of Figure 21 has a first process step 180 wherein the tool center point velocity VK is recalled from the cycle control auto mode 48. This subroutine has a second process step 181 wherein it calls the calculate correction subroutine of Figure 22. The subroutine of Figure 22 begins with a decision step 182 where it is determined whether twice the incremental distance ~S would be equal to or greater than the target distance ST, i.e., the distance from the tool center point to the active seam point Ps~ If the answer is no, the ne~t two steps are skipped via the NO bypass line. If the answer is yes, then it is known that the FIFO entries under investigation are too close to the current location of 41 1~ 89.S~

1 the tool center point. A yes answer to decision step 182 leads to decision step 183 wherein t.he FIF0 is checked to see if it is empty. I the FIF0 is empty, the YES bypass line returns the process to the end o the subroutine of Figure 21, which in turn returns the process to the routine of Figure 16 just ahead of process step 169. Should this occur, procless step 169 will inform cycle control 23 that there is no tracking data and the routine of Figure 16 will loop to its beginning point. In the meantime, the manipulator will track the tool center point along the programmed path including any previously made offset sorrection.
Returning to decisio~ step 183 of Figure 22, if it is found that the FIF0 is not empty, the current FIF0 coordinates under investigation will be discarded and the next set of FI~0 coordinates will be popped for use.
The N0 bypass line from decision step 182 and process step 184 both lead to process step 185. Here, the FIF0 entry to be used is removed from the FIF0 and is temporarily stored as the new target point coordinates Xs, Ys~ and ZS In process step lB6, the coordinates to which the tool center point went last (i.e., the current position of the tool center point) are recalled.
In the nest three process steps 187, 188, and 189, calculations are made similar to those described with respect to Figure 15. Thus, in process step 187, the coordinates of the vector which e~tends from th tool center point location toward the target point are determined. Using this inormation, in step 188 the distance ST between the tool center point location and the target point is calculated. Finally, the coordinates of the nest position PSK along the new vector to which the tool center point will go in one 42 ~ 3 ~ 8 ~ ~ 4 1iteration is calculated in process step 189. The XSK' Ys~' ZSK f thi5 location are stored in a predetermined location for access by cycle control 23.
5Following step 190, the subroutine of Figure 22 is terminated and returns to the end of the subroutine of Figure 21. The end of the subroLItine of Figure 21, in turn, returns to the routine of Figure 16 just before process step 169.
10Figurs 23 illustrates a subroutine performed by cycle control 23. It will be rernembered that this subroutine is called up by step 129 of subroutine of - Figure 11.
There may be occasions when the correction called for by the vision data processor 24 in conjunction with the vision tracking system is greater than a ma~imum correction programmed by the manipulator operator. If this occurs, it is necessary to adjust or scale down this correction. This compression of the correction is the purpose of the subroutine of Figure 23.
; This subroutine begins with decision step 191 whereby it is determin~d whether or not seam tracking is taking place. If the answer is no, the subroutine of Figure 23 terminates, returning to the subroutine of Figure 11. If the answer is yes, the subroutine of Figure 23 will call for a status check in process step 192. Process step 192 will call up the vision data processor st~tus check subroutine illustrated in Figure ?A4. The subroutine of Figure 24 begins with a decision step 193 which checks for an error on the vision data board. If no error is found, the N0 bypass line terminates the subroutine of Figure 24 which returns to the subroutine of Figure 23 just below process step 192.
35If decision step 193 of Figure 24 determines that ~3t8~

1 an error e~ists, process step 194 reacts appropriately, depending on the nature of the error. Depending upon the error, process step 194 is capable of reporting the error to the manipulator operator; stopping the manipulator; turning on warning lights or the like;
taking the manipulator out o~ cycle; etc. Process step 194 terminates the subroutine of Figure 24. Whether the subroutine of Figure 24 will return to the subroutine of Figure 23 following process step 94 depends upon the action taken during process step 94.
Returning to Figure 23, assuming that the subroutine of Figure 24 returns to Figure 23 just helow process step 192, the ne~t step 195 in the subroutine is a decision step to determine whether or not valid vision data processing coordinates have been received by cycle control 23. These coordinates are designated XsR, YsK, and ZSK' If it is determined that these coordinates are valid, step 196 calculate~ the coordinate components X~, Y~, and Z~ of the vector which e~tends from the last interpolated point produced by step 95 of Figure 7, including any previous offset, (which point has coordinates XK + OFFX, YK + OFFy, and ZK ~ OFFz) toward the new target point XsR~ Ys~, and ZSK By taking the square root of the sum of the squares of the vector coordinates, the distance MAG~l between these point~ is calculated.
Process step 196 is followed by decision step 197 wherein it is determined whether MAG~l e~ceeds a limit permitted for deviation from the programmed path. A temporary holding variable TEMP is defined as the product MAX (Vp) ~T, where MAX is a ma~imum correction value (from O to 1.5) preprogrammad by the manipulator operator, Vp is the programmed velocity and ~T is the time of one increment interval.

` 44 ~ ~ 8~

1 Therefore, (Vp) aT constitutes th~ increment distance at programmed velocity and TEMP represents a multiple of this distance.
If TEMP is not less than MA~ the next three process steps 198, l99 and 200 are bypassed by the ~O bypass line. If TEMP is less than MAG~l, it is known that the vision data point PSK lies at a distance on vector X~, Y~, and Z~ greater than the preprogrammed limit. At process steps 198 and l99 new values for XsK, YSK, and ZSX are produced which place location PSx on the same vector, the magnitude of TEMP away from P~. The coordinate components of this vector with magnitude TEMP are defined as the product of TEMP and ~he ratio of the coordinate components X~, Y~, Z~ to the magnitude MAG~l. For the sake of efficiency the common division is done first at process step 198 by dividing TEMP by MAG~l to create MA~2- In process step l99, a set of new values for XsR, YSK, ZSR is calculated using MAG~2. This new point XsK, YSK, ZSK lies along the original vector ~, Y~, Z~ at a distance from khe point at which the manipulator would have placed the tool center line equal to TEMP.
Process step 200 forces the actual distance MAG~3 between the points P SX and P X 1 to equal ~S. First, the actual distance between the tool center point location, i.e., XK l + OFFX, YK 1 +
OFFy, ZK l + OFFz and the n~wly calculated XsK, YSK, ZSK is computed by calculating the coordinate component differences X~, Ya~ and Z~ and then e~tracting the square root of the sum o~ their squares. This distance is designated as MAGA3.
Next the distance on the vector to PSK from P~_1 is scaled to the increment distance ~S using the same ~ 3~ 8~

1 procedure as shown at steps 198 and 199. Specifically, a magnitude MAGQ4 is calculated by dividing ~ by MAG~3- Utilizing MA~4 the coordinates of a third and final target point XsK, Ys~ ZSK are calculated. This point constit:utes a final target point corrected both as to distance and veloci.ty.
Finally, in process step 201, the X/ Y, and Z offsets are calculated as shown.
If in the decision step 1~5 it had been determined that no valid coordinates had been received, the cycle control would simply have calculated a path parallel to the programmed path and offset therefrom by any offset existing at that time. This is shown in process step 202. At process step 203 the adjusted ; 15 point is substituted for the point Xx, YK and ZK
in the subroutine of Figure 11. Either of process steps 201 or 202 terminate this subroutine and the process is returned to the subprocess of Figure 11.
The invention has now been described in dPtail.
Modifications may be made in the invention without departing from the spirit of it.

Claims (14)

1. A method of controlling a manipulator to effect motion of a tool centerpoint associated with a function element carried by the manipulator such that the tool centerpoint moves along a path detected by a sensor during execution of a program defining a cycle of operation of the manipulator, the method comprising the steps of:
a. periodically producing intermediate point signals representing coordinates of an intermediate location along a predetermined path between two programmed locations, the intermediate location defining displacement along the predetermined path to be effected in a predetermined time interval;
b. producing target coordinate signals in response to the intermediate point signals and the sensor, the target coordinate signals representing coordinates of a location to which the tool centerpoint is to be moved in the direction from the tool centerpoint current location toward the detected path;
c. calculating a limit value defining, for the predetermined time interval, a maximum deviation of the path of actual tool centerpoint motion from the predetermined path;
d. calculating coordinates of a predicted location of the tool centerpoint, the predicted location being defined by the intermediate point and an offset between the current tool centerpoint location and the immediately preceding intermediate point;
e. calculating the magnitude of the distance between the target point and the predicted location;
f. determining whether the magnitude of the distance exceeds the limit value; and g. modifying the target coordinate signals in response to determining that the magnitude of the distance exceeds the limit value, the modified target coordinate signals defining a location to produce a deviation equal to the limit.

2. The method of claim 1 wherein the step of producing target coordinate signals further comprises the steps of:
a. periodically producing detected path signals representing coordinates of a location on the detected path to which the tool centerpoint is to be moved;
b. calculating an incremental distance magnitude defining the distance through which the tool centerpoint is to be moved in the predetermined time interval; and c. calculating coordinates of a location the incremental distance from the current tool centerpoint in the direction toward the location on the detected path.

3. The method of claim 2 wherein the programmed cycle of operation defines a program velocity and the limit value is calculated as a distance through which the tool centerpoint would move in the predetermined time interval at a rats proportional to the program velocity.

4. The method of claim 2 wherein the step of periodically producing detected path signals further comprises the steps of:
a. periodically producing sensor coordinate signals representing the coordinates of the location on the detected path relative to a frame of reference defined by the sensor;
b. transforming the sensor coordinates to define the location relative to a frame of reference associated with the manipulator;
c. selecting transformed sensor coordinates which represent a location at least a predetermined distance from the location represented by the immediately previously selected coordinates;

d. storing the selected transformed coordinates; and e. periodically recalling stored selected transformed coordinates.

5. The method of claim 4 wherein the step of producing target coordinates further comprises the step of selecting detected path signals which represent a location more than twice the incremental distance from the current location of the tool centerpoint.

6. The method of claim 1 wherein the sensor is a vision system including a laser scanner/camera head which is fixedly mounted to the function element.

7. The method of claim 6 wherein the programmed cycle of operation defines changes of orientation of the function element between the two programmed locations.

8. An apparatus for controlling a manipulator to effect motion of a tool centerpoint associated with a function element carried by the manipulator such that the tool centerpoint moves along a path detected by a sensor during execution of a program defining a cycle of operation of the manipulator, the apparatus comprising:
a. means for periodically producing intermediate point signals representing coordinates of an intermediate location along a predetermined path between two programmed locations, the intermediate location defining displacement along the predetermined path to be effected in a predetermined time interval;
b. means for producing target coordinate signals in response to the intermediate point signals and the sensor, the target coordinate signals representing coordinates of a location to which the tool centerpoint is to be moved in the direction from the tool centerpoint current location toward the detected path;
c. means for calculating a limit value defining, for the predetermined time interval, a maximum deviation of the path of actual tool centerpoint motion from the predetermined path;
d. means for calculating coordinates of a predicted location of the tool centerpoint, the predicted location being defined by the intermediate point and an offset between the current tool centerpoint location and the immediately preceding intermediate point;
e. means for calculating the magnitude of the distance between the target point and the predicted location;
f. means for determining whether the magnitude of the distance exceeds the limit value; and g. means responsive to the determining means for modifying the target coordinate signals to define a location resulting in a deviation equal to the limit.

9. An apparatus in accordance with claim 8 wherein the means for producing target coordinate signals further comprises:
a. means for periodically producing detected path signals representing coordinates of a location on the detected path to which the tool centerpoint is to be moved;
b. means for calculating an incremental distance magnitude defining a distance through which the tool centerpoint is to be moved in the predetermined time interval; and c. means for calculating coordinates of a location the incremental distance from the current tool centerpoint location in the direction toward the location defined by the detected path signals.

10. The apparatus in accordance with claim 9 wherein the programmed cycle of operation defines a program velocity between the two predetermined points and the means for calculating the limit value calculates a distance through which the tool centerpoint would move in the predetermined time interval at a rate proportional to the program velocity.

11. An apparatus in accordance with claim 9 wherein the means for producing detected path signals further comprises:
a. means for periodically producing sensor coordinate signals representing the coordinates of the location on the detected path relative to a frame of reference defined by the sensor;
b. means for calculating transformed sensor coordinates defining the location on the detected path relative to a frame of reference associated with the manipulator;
c. means for selecting transformed sensor coordinates which represent a location at least a predetermined distance from the location of the immediately previously selected transformed sensor coordinates;
and d. means for storing the elected transformed sensor coordinates.

12. The apparatus in accordance with claim 11 wherein the means for producing target coordinate signals further comprises means for electing detected path signals which represent a location more than twice the incremental distance for the current location of the tool centerpoint.

13. The apparatus in accordance with claim 8 wherein the sensor is a vision system including a laser scanner/camera head which is fixedly mounted to the function element.

14. The apparatus in accordance with claim 13 wherein the programmed cycle of operation defines a change of orientation of the function element between the two programmed locations,