Self-teaching robot feedback system

A self-teaching RCC device robot feedback system including: a robot mechanism; an RCC device carried by the robot mechanism; a robot driver unit for directing motion of the RCC device in an environment; means for determining the displacement of an RCC device with respect to the robot; and an adaptive learning system, responsive to the means for determining displacement, for detecting a pattern of displacements of the RCC device relative to its environment and generating commands to the robot driver unit to direct motion of the RCC device.

FIELD OF INVENTION 
This invention relates to a self-teaching robot feedback system using a 
remote center compliance device. 
BACKGROUND OF INVENTION 
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. 
SUMMARY OF INVENTION 
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. 
DISCLOSURE OF PREFERRED EMBODIMENT 
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 
EQU x.sub.k =x.sub.k-1 +s (1) 
where s is the distance between the two holes. The kth robot taught 
position with respect to the k-1th will then be: 
EQU y.sub.k =y.sub.k-1 +s.sub.o (2) 
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: 
EQU y.sub.k =y.sub.k-1 +s.sub.o +u.sub.K-1 +w.sub.k-1 (3) 
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 
EQU e.sub.k =y.sub.k -x.sub.k (4) 
then the taught data should ideally be updated by: 
EQU u.sub.k =-e.sub.k (5) 
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: 
EQU z.sub.k =e.sub.k +v.sub.k (6) 
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): 
EQU e.sub.k =e.sub.k-1 +s.sub.o -s+u.sub.k-1 +w.sub.k-1 (7) 
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: 
EQU x.sub.k =.PHI.x.sub.k-1 +Bu.sub.k-1 +.GAMMA.w.sub.k-1 (8) 
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): 
______________________________________ 
Standard Form 
Name Example 
______________________________________ 
x.sub.k State vector e.sub.k (Scalar) 
u.sub.k Control u.sub.k (Scalar) 
w.sub.k Noise w.sub.k + s.sub.o - s (Scalar) 
(s.sub.o - s is an unknown 
constant and is included 
with the noise) 
.PHI. 1 
B 1 
.GAMMA. 1 
______________________________________ 
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 : 
EQU x.sub.k =.PHI.x.sub.k-1 +Bu.sub.k-1 +K.sub.k [z.sub.k -Hx.sub.k ](9) 
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: 
EQU z.sub.k =Hx.sub.k +v.sub.k (10) 
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 : 
EQU x.sub.k =.PHI.x.sub.k-1 +Bu.sub.k-1 (11) 
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: 
EQU K.sub.k =P.sub.k H.sup.T R.sup.-1 (12) 
where 
EQU M.sub.k =.PHI.P.sub.k-1 .PHI..sup.T +.GAMMA.Q.GAMMA..sup.T (13) 
and 
EQU P.sub.k =(M.sub.k.sup.-1 +H.sup.T R.sup.-1 H).sup.-1 (14) 
and P.sub.o =covariance matrix of the error in x.sub.o : 
EQU P.sub.o =E[(x.sub.o -x.sub.o)(x.sub.o -x).sup.T ] (15) 
where E denotes average, and Q=covariance matrix of noise w: 
EQU Q=E[ww.sup.T ] (16) 
and R=covariance matrix of noise v: 
EQU R=E[vv.sup.T ] (17) 
In terms of the present example: 
______________________________________ 
Standard Form Example 
______________________________________ 
P.sub.k P.sub.k (scalar) 
M.sub.k M.sub.k (scalar) 
Q q = variance of w (scalar) 
R r = variance of v (scalar) 
H 1 
Equation (11) -e.sub.k = e.sub.k-1 + u.sub.k-1 (18) 
______________________________________ 
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: 
EQU e.sub.k =e.sub.k-1 +u.sub.k-1 +K.sub.k [z.sub.k -(e.sub.k-1 
+u.sub.k-1)](19) 
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): 
EQU e.sub.4 =e.sub.3 +u.sub.3 +K.sub.4 [z.sub.4 -(e.sub.3 +u.sub.3)](23) 
However, since u.sub.3 =-e.sub.3 in this specific example, equation (23) 
reduces to 
EQU e.sub.4 =K.sub.4 z.sub.4 (24) 
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: 
EQU d=s.sub.o -s (25) 
As a constant, its system equation is simply: 
EQU d.sub.k =d.sub.k-1 (26) 
The combination of equations (26) and (7) yields two system state 
equations: 
EQU e.sub.k =e.sub.k-1 +d.sub.k-1 +u.sub.k-1 +w.sub.k-1 (27) 
EQU d.sub.k =d.sub.k-1 (28) 
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: 
EQU u.sub.k =-(e.sub.k +d.sub.k) (33) 
Thus, analogously, the filter operates as follows: 
##EQU4## 
where 
##EQU5## 
so that 
EQU H=[1 0] (36) 
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: