Self-Teaching Robot Feedback System

This invention relates to a self-teaching robot feedback system using a remote center compliance device.

Conventional robot devices operate to assemble and mate parts or a part and a tool automatically. Typically, alignment errors are not detected or corrected.

RCC devices, which are passively compliant, are available for assembly, mating and insertion tasks where it is desired to quickly and easily accommodate for relatively small misalignments. Three different types of RCC devices are disclosed in U.S. Pat. Nos. 4,098,001, 4,155,169, U.S. Pat. No. 4,337,579 and incorporated herein by reference. These devices have now been instrumented, U.S. Pat. No. 4,316,329, incorporated herein by reference, so that they are able to passively adjust to assemble misaligned parts and also indicate a measure of the misalignment.

Typically industrial robots are taught by introducing information into a robot data base such as by moving the robot through a sequence of operations and recording the various forces, velocities, and/or displacements that occur. Such information might also be obtained from external sources such as CAD or CAM data bases generated during design and manufacture of a product. If the input data is not entirely accurate or the environment of the robot or the robot itself has been altered then the robot has to be re-taught. Re-teaching may also be required because of shifts in the workpiece or work site, temperature drift, tolerance variations, aging, and other short and long term variable effects.

Various sensing schemes are used to attempt to monitor the robot's operation and reflect its interaction with its work environment. The sensor readings may eventually become data for robot operation but it may not be assumed that this data is completely accurate. It is customary and proper in control theory to assume rather that all sensor readings are corrupted by noise of various kinds which may result in wasted time and energy as well as unstable and possibly damaging behavior. Present sensing schemes do not provide for long-term accumulation of experience, tracking of drift, correction of teaching or alignment errors, etc.

It is therefore an object of this invention to provide an improved self-teaching robot feedback system using an RCC device.

It is a further object of this invention to provide such a system which utilizes the accumulated experience of the robot over a period of time.

It is a further object of this invention to provide such a system which employs an instrumented RCC device to accommodate for displacements between the RCC device and workpiece and provide a measure of the displacements for adjusting the system to shift the RCC device position for subsequent operation.

This invention features a self-teaching RCC device robot feedback system including a robot mechanism and an RCC device carried by the robot mechanism. There is a robot driver unit for directing motion of the RCC device in an environment and means for determining the displacement of an RCC device with respect to the robot. An adaptive learning system responsive to the means for determining displacement detects a pattern of displacements of the RCC device relative to its environment arising from repeated executions of the same robot task and generates commands to the robot driver unit to direct motion of the RCC device. In a preferred embodiment the adaptive learning system includes an optimal linear estimator, optimal linear filter, or state observer/reconstructor. The generic term "Kalman Filter" is often used to denote such filtering or estimating techniques.

Other objects, features and advantages will occur from the following description of a preferred embodiment and the accompanying drawings, in which:

FIG. 1 is a block diagram of a self-teaching RCC device robot feedback system according to this invention;

FIG. 2 is a more detailed block diagram of the adaptive learning system of FIG. 1;

FIG. 3 is a more detailed block diagram of the statistical updating calculation block of FIG. 2;

FIG. 4 is a more detailed block diagram of the displacement detector of FIG. 1 and the statistical updating calculation block of FIG. 2;

FIGS. 5A and B are schematic diagrams showing various dimensions involved in locating a peg in a hole using the system according to this invention;

FIG. 6 is a more detailed block diagram of a Kalman filter circuit of FIG. 5; and

FIG. 7 is a more detailed block diagram of the Kalman filter gain circuit of FIG. 5.

There is shown in FIG. 1 a self-teaching RCC device robot feedback system 10 according to this invention including an RCC device 12 with operator member 14 interconnected with displacement detector 15. RCC device 12 and RCC displacement detector 15 may together be referred to as an instrumented remote center compliance device or IRCC device 16. Displacement detector 15 is mounted on a robot mechanism 18 driven by robot drive unit 20. IRCC device 16 senses displacement of operator member 14 as it engages the workpiece, and delivers a signal, which is a measurement of any displacement or misalignment, to adaptive learning system 22. RCC 12, due to its compliant properties, enables operator member 14 to engage a workpiece even though the workpiece and operator member 14 may be misaligned, and thereby eliminates wasteful hunting operations. However, measurements of any misalignment supplied by displacement detector 14 on line 24 enable adaptive learning system 22 to determine any trends in misalignment between the workpiece and operator member 14 and, over line 26, to adjust robot drive unit 20 accordingly.

Adaptive learning system 22 may include statistical updating calculation circuit 30, FIG. 2, which receives the sensed data on line 24 and may receive certain a priori information on line 32. The information on line 32 may, for example, take the form of estimates of statistics and probabilities which might alternatively be learned over time. The updated signal is provided on line 34 to the data base 36 for controlling and teaching the robot. Data base 36 also may receive a priori information such as that obtained by leading a robot manually through a sequence of operations, as discussed previously. The output from data base 36 on line 26 is submitted to robot drive unit 20 to directly drive robot mechanism 18.

Statistical upgrading calculation circuit 30 may be implemented with a Kalman filter circuit, FIG. 3, which includes a mathematical model of the system being measured, 40, and a mathematical model of sensors obtaining partial measurements, 42, along with a circuit 44 containing the filter gain K of the filter, and summing circuit 46. Sensed data on line 24 is combined in summer 46 with the prediction of partial measurements obtained from mathematical model 42. The output from summer 46 is multiplied by the gain K in circuit 44 and then submitted to mathematical model 40 of the system being measured. This provides an output equivalent to the estimated state of the system on line 34 which is fed back through mathematical model 42 and is also delivered to data base circuit 36.

Displacement detector 15 operates to provide a signal representative of the displacement of the operator member 14 with respect to a reference in the interior of the sensor. In the ideal case, the sensor provides signal e which is the difference between reference position y and operator member position x. In reality, signal e is corrupted by additive noise v so that the sensor's output is z, the sum of e and v. This output z is provided to Kalman filter circuit 60 in statistical updating calculation circuit 30.

This description of the operation of the displacement sensor is shown schematically in FIG. 4, where summing circuit 52 represents the ideal operation of the sensor, determining the difference between sensor reference position y and operator member position x. Summing circuit 54 represents unwanted but unavoidable additive noise which corrupts signal e.

To illustrate the usefulness of this invention, consider the following simplified example. See FIG. 5. The robot is supposed to place the operator member 14 in a succession of holes, to perform an assembly, apply glue, etc., but cannot do so adequately because of errors in teaching or in the CAD data base which represents the holes. By means of this invention the error will be automatically discovered and corrected.

In FIG. 5A are shown two representative holes. Their positions are denoted x.sub.k and x.sub.k-1 with respect to a reference in the robot's environment. The position of the robot (and of the sensor's reference) as it approaches the k.sup.th hole is denoted y.sub.k. The spacing between the holes is denoted by s. The teaching error consists of teaching the robot the wrong value of s or equivalently of receiving an incorrect value of s from the CAD data base. Thus when the robot gets the operator member near the hole the operator member will be in the wrong position by an amount equal to the error in spacing s. If that error is not too large then, as the robot begins to insert operator member 14 into the k.sup.th hole, the operator member will displace slightly under the action of the compliant RCC and will come to rest in or part way in the hole, FIG. 5B. But the position of the operator member now will be x.sub.k corresponding to the hole position, whereas the robot's position will be a different value y.sub.k, the originally taught but erroneous position. Thus x.sub.k will not equal y.sub.k and the displacement sensor will detect the difference.

The operation of the instrumented RCC device in conjunction with Kalman filter circuit 60 is explained in more detail for this example as follows: The positions of the k-1th hole and the kth hole may be expressed as

where s is the distance between the two holes. The kth robot taught position with respect to the k-1th will then be:

where s.sub.o is the robot's initial estimated or taught distance between the hole positions. The robot actual performance in moving from one taught position to the next may then be expressed as:

where u.sub.k-1 represents a correction factor to be calculated by the Kalman filter circuit and w.sub.k-1 represents robot noise and general inability to achieve the y.sub.k target. Due to the accommodation of the IRCC device, the operator member will enter into a hole in spite of a misalignment error y.sub.k -x.sub.k, which the IRCC device accommodates the measures by means of the sensors.

If e.sub.k is defined as

then the taught data should ideally be updated by:

Thus for the next hole, the k+1th hole, the misalignment will not be so great as it was with the kth hole. In fact, e.sub.k is unknown, but z.sub.k, the output of the displacement sensor, is known:

where v.sub.k represents unavoidable additive noise in the displacement sensor.

The job of the Kalman Filter is then to estimate, in spite of noise inputs w and v, the robot's true position, the hole's true position, the true spacing s, and/or the spacing error s.sub.o -s. Both the hole position x.sub.k and the robot position y.sub.k may be estimated using system state equations (1) and (3). Alternatively, only the error e.sub.k may be estimated, which requires only one system state equation, obtained by subtracting equation (3) from equation (1):

Finally, an estimate of spacing error s.sub.o -s may also be obtained by defining a new system state variable. This will be explained below. First a review of general Kalman Filter theory will be presented.

In a Kalman filter the system state equations are expressed in general in a standard form as follows:

where x.sub.o is the given initial state. x.sub.k-1 is the system state vector including one or more pieces of information sufficient to describe the state of the system at time k-1. u.sub.k-1 is a vector of controls to be applied at time k-1, and w.sub.k-1 is a vector of disturbance noise inputs at time k-1. Included in vector x is any information needed to describe the robot's position or the positions of other things in the robot's environment, such as taught locations, which may need to be estimated or measured, or whose performance is to be monitored. Equation (8) demonstrates the combination of these quantities using matrices .PHI., B and .GAMMA. to produce x.sub.k, the condition of the system at time k. Matrix .PHI. is the state transition matrix, matrix B is a control transfer matrix, and matrix .GAMMA. is a noise transfer matrix.

There is the following correlation between equations (7) and (8):

The Kalman Filter equation represents the behavior of x.sub.k, which is the filter's estimate of the true system state, x.sub.k :

where x.sub.o is the initial given or estimated value of x.sub.o. K.sub.k is the filter gain matrix calculated infra; z.sub.k is the vector of measurements at time k:

where H represents scaling or combination of components of x.sub.k and v.sub.k represents noise on the measurements. x.sub.k is the best estimate of x.sub.k prior to measuring and obtaining z.sub.k.

Filter theory establishes that if the best estimate of x.sub.k-1 is known, namely x.sub.k-1, and if u.sub.k-1 is known and the noise w.sub.k-1 is zero mean, then the best value for x.sub.k is gotten from equation (8), using x.sub.k-1 in place of x.sub.k-1, and zero in place of w.sub.k-1 :

Thus z.sub.k -Hx.sub.k is the difference between z.sub.k and the best prediction of z.sub.k, given x.sub.k-1 and u.sub.k-1. K.sub.k multiplied by the difference is used as a correction term in the filter.

Filter theory provides the following equations for determining filter gain K.sub.k, where superscript T means matrix transpose and superscript -1 means matrix inverse:

where

and

and P.sub.o =covariance matrix of the error in x.sub.o :

where E denotes average, and Q=covariance matrix of noise w:

and R=covariance matrix of noise v:

In terms of the present example:

Equation (18) does not include s.sub.o -s because that is of course unknown. Therefore the best estimate of e.sub.k before measuring is obtained from equation (18) using known e.sub.k-1 and u.sub.k-1. The Kalman filter equation for this example may be written:

with ##EQU1##

This example is pursued further, for example, as the robot moves from hole 3 to hole 4. e.sub.3 and P.sub.3 have been calculated and according to equation (5) the stored robot command in seeking hole 4 will be modified by the amount u.sub.3 =-e.sub.3. Thus if it is estimated that the robot was 0.01 inch too far to the right, this amount is deducted from the a priori position of the robot at hole 4 so that the robot comes closer to hole 4 than it did to hole 3. When the robot reaches hole 4, the IRCC device is read, and using equation (6), z.sub.4 is obtained. P.sub.3 is known and M.sub.4 can be calculated from equation (22) for use in equation (21) to calculate P.sub.4, from which in turn can be calculated K.sub.4 from equation (20). There is now available all the information needed to calculate e.sub.4 from equation (19):

However, since u.sub.3 =-e.sub.3 in this specific example, equation (23) reduces to

In more complex examples this reduction does not occur, but it does occur here because no estimate is being made of the ongoing error s.sub.o -s. Rather, corrections are made to the previous alignment without removing its cause. Therefore e.sub.k does not depend on e.sub.k-1 : there is no learning concerning the hole spacing error, that is s.sub.o -s, in the filter as thus far expressed.

To provide this learning facility, the hole spacing error must be a system state variable, and it must be estimated as well. The hole spacing error may be designated as d:

As a constant, its system equation is simply:

The combination of equations (26) and (7) yields two system state equations:

which can then be put in the standard form of equation (8). ##EQU2## Thus it is apparent that ##EQU3##

Estimating both e and d, u.sub.k is used as the correction:

Thus, analogously, the filter operates as follows: ##EQU4## where ##EQU5## so that

The procedure required to go from hole 3 to hole 4 is analogous to that given above. It is assumed that there is available e.sub.3, d.sub.3 and P.sub.3 and a 2.times.2 matrix, and that according to equation (33) the stored robot command aiming for hole 4 is modified by -(e.sub.3 +d.sub.3). z.sub.4 can be obtained using equation (35) by reading the IRCC when the robot has reached hole 4. Equation (13) is then used to calculate M.sub.4, another 2.times.2 matrix, and equation (14) uses P.sub.4 and also a 2.times.2 matrix. This also allows the calculation of K.sub.4 from equation (12). Since R is still a scalar and H.sup.T is a 2.times.1 vector, K.sub.4 also resolves into a 2.times.1 vector.

The information is now available to estimate equation (11) and equation (9) (expressed here as equation (34)) to obtain e.sub.4 and a.sub.4, and the cycle is complete. The result overall is that e and d are both estimated and successively learned with greater and greater accuracy. e goes toward zero and d goes toward the actual spacing error s.sub.o -s. Thus the spacing has been learned. To allow a robot to learn some other feature of itself or its environment requires that a new .PHI., B, Q, R, H, etc., be determined such that equation (8) represents the environment, the new features, and the robot correctly. Examples include tracking a conveyor with uncertain and unsteady velocity, estimating hole positions in two dimensions or positions with variable spacing errors, tracking dimensional drift in manufactured parts, sensing deterioration of robot performance, and so on. This method may be implemented in hardware or programmed on a microprocessor or on a digital computer, for example. Note that, in this example, spacing error s.sub.o -s was estimated without ever being directly measured. To ensure that this is possible, filter theory also provides rigorous mathematical tests, called observability tests to determine in a given case whether or not a proposed filter will work.

Kalman filter circuit 60 may be implemented by a pair of summing circuits 80, 82, FIG. 6, and a number of matrix multiplier circuits including control transmission matrix circuit 84, observation matrix 86, system state transition matrix circuits 88 and 90, and control gain matrix circuits 92 and 94. There is also included a one time unit delay circuit 96 and the filter gain matrix 44.

The output from filter 60, x.sub.k+1, is delivered to and multiplied by matrix 94, whose output provides the next drive signal u.sub.k+1 to the robot unit 96 to matrix multiplication circuits 88, 92, and 90. The output from matrix multiplication circuit 90 is delivered to and multiplied by matrix 86 and then combined with the input signal z.sub.k in summer 80. The output of summer 80 is multiplied by the filter gain matrix 44 and then combined in summer 82 with the outputs from matrix multiplications 84 and matrix 88 to form the final output of Kalman filter circuit 60.

The Kalman filter gain K.sub.k may be calculated by filter gain circuit 62, FIG. 7, which includes a pair of summing circuits 100, 102, inverters 104, 105, and one time unit delay circuit 106. Filter gain circuit 62 also includes a number of matrix multiplying circuits including observation matrix H, 108, the transposed observation matrices H.sup.T, 110 and 112, noise transmission matrix .GAMMA., 114, and the transposed noise transmission matrix .GAMMA..sup.T, 116. Also included are system state transition matrix .PHI. 118 and the transposed matrix .PHI..sup.T, 120, thereof, and the inverse of covariance matrix R.sup.-1, 122.

In operation, the output from summer 102 represents the inverse of the covariance matrix of error in the estimates after measurement, P.sub.k.sup.-1. This is input to inverter 104. The output of inverter 104, the covariance matrix of error in the estimate after measurement, P.sub.k, is then multiplied by matrix 112, the output of which is multiplied by matrix 122 to provide the final output, K.sub.k, the Kalman filter gain. The output from inverter 104, P.sub.k, is also provided to delay 106, whose output P.sub.k-1 is delivered to and multiplied by matrix .PHI.118, whose output in turn is delivered to and multiplied by matrix .PHI..sup.T 120, whose output is delivered to summer 100. The other input to summer 100 is derived from matrix .GAMMA..sup.T, 116, via the output of matrix .GAMMA., 114, which receives at its input the signal Q representative of the covariance of noise w. Thus the output from summer 100, M.sub.k, via inverter 105, becomes the inverse, M.sup.-1, input to summer 102. The other input to summer 102 is derived from matrix 108, which in turn receives an input from matrix 110 that is provided with the inverse of the informational matrix R.sup.-1.

Other embodiments will occur to those skilled in the art and are within the following claims: