Methods for refining original robot command signals

Abstract

A method for refining an original command signal which is intended to locate a robot at a desired position and orientation (pose) comprising the steps of identifying the actual system parameters of the robot; predicting, through utilization of those actual system parameters the anticipated pose of the robot which would actually be obtained as the result of the original command signal; calculating corrected command signals required to minimize the difference between the predicted anticipated pose and the desired pose; and employing the corrected command signals to place the robot. Preferably, a plurality of desired calibration pose command signals are used in the step of identifying the actual system parameters to place the robot in a plurality of actual calibration poses in the measuring range of at least one pose sensor. The differences between the desired calibration poses and the correponding actual poses are calculated and these differences are used to solve a set of equations which relates those differences to the actual system parameters of the robot, to thereby obtain the actual system parameters of the robot.

Description

BACKGROUND OF THE INVENTION

I. Field of the Invention

The present invention relates to methods and apparatus for improving robot accuracy and long-term repeatability by refining original robot command signals.

II. Background of the Invention

Most of today's robots are programmed online, that is in the actual environment where the robot is to be used. An operator "trains" the robot by moving it through a series of configurations and causing those configurations to be stored in memory. Online programming is a very inefficient way to program robots and robot manufacturers and users alike are presently trying to develop offline programmed systems. The inefficiencies relate to having a programmer spend his time to initially program the robot and to adjust the program on a regular basis. In addition if the task is to be transferred to a different robot, if the current robot requires maintenance, or the task is to be slightly modified, the robot may require reprogramming. Each of these functions could be performed automatically if the robot could be programmed offline.

In offline programmed robot systems, a CAD/CAM (computer aided design/computer aided manufacturing) system generates the robot program. The programs are composed of commanded actions (REACH, GRASP, PALLETIZE), poses where the actions are to occur (at pose A, B) and sensory queries that are used for branching and program control. In future generations of offline programmed robotic systems, the offline generated program will be downloaded to the robot, eliminating much, if not all of the need for an operator to train the robot. The transition between online and offline programming has been going on for the last few years. For example, there are in some research facilities task level programs that can use retrieval, or in some limited cases, generative techniques to decompose high level tasks desired of robots (like assemble the Model #43 gearbox) to much lower level commands (such as place gear A on sprocket B). Although the transition to offline programming is moving at a continued but modest pace, there are a number of problems which must be solved.

Current robots are not accurate enough to be used in precision assembly tasks when they are programmed offline. A state-of-the-art robot manipulator is a repeatable but inaccurate device. If a manipulator is repeatably commanded to a desired position and orientation (pose), it will repeatably return to approximately the same achieved pose but the achieved pose will not be equal to the desired pose. The achieved poses are not equal to the desired poses because of a number of reasons which are discussed in Reference No. 4 listed below [Colson and Perreira, 1985-1]. A dominant source of pose errors is the machining, assembly and measurement errors in the mechanical linkages and joint actuators that make up the robot manipulator.

For an online programmed robot an operator "trains" the manipulator by placing it at a series of configurations and causing the configurations to be stored in memory. Because robots are repeatable, when they are commanded using the stored configurations they will behave adaquately for some duration. The configurations can be stored as either a set of joint variables measured at the linkage joints or by a pose variable which is directly related to the joint variables through a set of preprogrammed forward kinematic equations. A preprogrammed set of reverse kinematic equations can convert the pose variable back into the original set of joint variables.

Storing joint configurations can cause inefficiencies for online programmed robots if the robot's characteristics change (through electrical and mechanical drifts) because the robot will move to the wrong pose when it is commanded to an original joint configuration. In addition, storing configurations as poses can cause similar problems because the preprogrammed forward kinematic equations used to convert the joint variables to a pose variable may no longer be correct.

In the prior art there have been no known real-time, automatic devices to determine the causes of the robot pose errors (the mechanical linkage errors and joint actuator errors) and to use the knowledge of those errors to both calibrate the robot and correct its trajectory. An object of the subject invention is effectively to determine the actual equations that describe the forward and inverse kinematics of the linkage at any point in time those equations are needed.

In the prior art, robots need to be frequently retaught because there are drifts in its electronics and mechanical linkages. These are no real time devices to correct the trajectory of a online programmed robot by converting the taught joint configurations to actual robot poses thus improving the long term repeatability. The forward kinematic equations used in robot controllers convert the taught joint configurations into an inaccurate description of the taught pose.

In the prior art, a robot task program created by teaching one robot, can not be used on other robots (even of the same type) due to the subtle kinematic differences between the robots. In the prior art a robot normally needs to be retaught its task after maintenance or after it has been moved. An other object of the subject invention is to adjust the joint and pose commands automatically so that the robot does not need to be retaught.

Additional objects and advantages of the invention will be set forth in part from the description that follows, and in part will be obvious from that description or may be learned in practice of that invention.

SUMMARY OF THE INVENTION

Our invention improves robot accuracy, thus greatly increasing the potential feasibility and uses for offline programmed robots. Both the position and orientation accuracy are improved. Manipulator accuracy is improved by first determining the actual linkage parameters of the robot and then correcting the pose errors of the end effector that would be caused by using the nominal as opposed to the actual linkage parameters in the robot controller. The actual linkage parameters are not used in present day controllers because they are not known and if they were known would require solving the general inverse kinematics problem; that is to determine the joint values which would cause a general linkage to achieve a desired pose. Present robot controllers are capable of solving the inverse kinematics problem for simple nominal linkages, but not the more complicated actual robot linkages. Our invention includes solutions to the general inverse kinematics problem. The anticipated pose of the end effector is corrected before the robot moves, which is critical for highly precise assembly operations where groping is not allowed. Groping should be minimized when the robot throughput is important. The linkage and joint actuator errors change with time and temperature. Thus, even if the parameters are known at a point in time, they may differ at a latter point. Our system allows the periodic recalibration of the robot in a simple and inexpensive manner, in its factory environment with minimal modifications to the robot work cell.

Our invention improves the long-term repeatability of online (as well as offline) programmed manipulators. Our invention will result in a decreased need for operator retraining of robot tasks, thus causing an increase in robot efficiency. Our invention makes robot task programs, which were taught on one robot, applicable to another robot of the same linkage arrangement or even a different arrangement. A discussion on linkage arrangements can be found in Reference No. 3 listed below [Colson and Perreira, 1983]. The robot will not need to be retaught its job after maintenance. Our invention works at all configurations of any linkage arrangement. It is not hindered by singularities.

Our invention advances the prior art in the manufacturer and construction of robot manipulators. The manufacturer can use our invention to determine the deviations from the nominal values of the mechanical linkages and joint actuators of his robot. Our invention can supply the robot manufacturer with information to improve both his control software and his manufacturing process.

Our invention advances the prior art in the design of robot manipulators. The robot designer can use data provided by these invention to determine expected errors in the manipulator linkages and to improve his design and its performance.

To achieve the foregoing objects, and in accordance with the purposes of the invention there is provided a method for refining an original command signal which is intended to place a robot at a desired position and orientation (pose), comprising the steps of: (a) identifying actual system parameters of the robot; (b) predicting, through the utilization of the actual system parameters and the original command signal, the anticipated pose of the robot which would actually be obtained as a result of the original command signal; (c) calculating corrected command signals required to minimize the difference between the predicted anticipated pose and the desired pose; and (d) employing the corrected command signals to place the robot in the desired pose.

Preferably, the step of identifying the actual system parameters of the robot includes the substeps of: (i) employing a plurality of calibration pose command signals, based upon nominal system parameters of the robot, to place the robot in a plurality of actual calibration poses, each of which actual calibration pose is within the measuring range of at least one pose sensor; (ii) measuring the actual calibration poses of the robot by operation of the pose sensors; (iii) calculating the differences between the plurality of desired calibration poses and the corresponding actual calibration poses; and (iv) solving a set of equations which relate those differences to the actual system parameters of the robot to thereby obtain the actual system parameters of the robot.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a diagram used to explain kinematic notations;

FIG. 2 is a diagram showing a standard robot command technique;

FIG. 3 is a perspective view of the interrelationship between a robot manipulator, a robot controller, a robot sensor, and a sensor controller;

FIG. 4a is a diagram of a system having joint input commands which incorporates the teachings of the subject invention;

FIG. 4b is a diagram of a system having pose input commands which incorporates the teachings of the subject invention;

FIG. 5a is a flow diagram of process operations according to the subject invention for an online programmed robot;

FIG. 5b is a flow diagram of process operations according to the subject invention for an offline programmed robot;

FIG. 6a is a diagram showing a position error vector;

FIG. 6b is a diagram showing an orientation error vector;

FIG. 7 is a diagram showing desired, achieved and corrected poses;

FIG. 8 is a graphical depiction of a pose correction algorithm incorporating the teachings of the subject invention;

FIG. 9 is a diagram showing differential displacements and rotations along and about the n+1th frame;

FIG. 10 illustrates the effect of parameter and joint variable errors in the i,i+1 link on the hand pose.

FIG. 11 demonstrates a cause of a singular Jacobian for a known robot.

FIG. 12 is a flow diagram of a calibration procedure incorporating the teachings of the subject invention; and

FIG. 13 illustrates the Hartenburg Link Transformation Notation.

DESCRIPTION OF THE PREFERRED EMBODIMENT

In order to program a manipulator offline, it is necessary to be able to command it to a specific pose within its workspace. In the prior art, online programming has been possible because the knowledge of a robot's particular system parameters are not needed when the robot is simply expected to achieve a set of joint parameters that it has been taught. As used throughout, the term system parameters comprises the robot's linkage (or kinematic) parameters which include: (1) d; (2) .theta.; (3) a; and (4) .alpha., described in more detail below. In offline programming of robots, it is necessary to determine the required joint values to achieve the desired pose. If this is to be done analytically, the actual kinematic parameters of the manipulator must be known because the robot pose is a function of these actual parameters, not the manufacturer's specified parameters. If the wrong parameter values are used within the robot controller, errors in the pose of the manipulator end effector will occur. Our invention determines the kinematic parameter errors (ie, the difference between the actual and the manufacturer's specified kinematic parameters) and uses these kinematic parameter errors to generate commands to the manipulator joint actuators to correct (reduce) the pose placement errors of the manipulator end effector.

I. Pose Definitions

1. Desired Pose is the pose defined by task level control and is downloaded to the robot. In the prior art, the robot was commanded with this pose. 2. Achieved Pose is the pose that the robot actually achieves when it is commanded with a pose command. 3. Commanded Pose is the pose with which the robot is actually commanded. In most robots this is the same as the desired pose. In our invention, this is the desired pose plus a correction. 4. Test Pose is a pose within the measurement range of a pose sensor. 5. Measured Pose is the pose measured by a pose sensor. It is defined as the achieved pose plus a possible measurement error. 6. Pose Correction is determined by our invention. The invention determines a pose correction so that when the robot is commanded with the desired pose plus the pose correction, the robot will achieve the desired pose. 7. Anticipated Achieved Pose is the pose that the robot is predicted to achieve when commanded with the desired pose. Our invention predicts the anticipated achieved pose based on an accurate robot model calculated and stored by the invention. The anticipated achieved pose is used to determine the pose correction before the robot moves to the desired pose.

II. Linkage Kinematics

A serial link manipulator consists of a sequence of links connected together by actuated joints. These joints can be single degree of freedom revolute (rotating) or prismatic (sliding) joints, multiple degree of freedom joints such as cylindrical, flat, global or helixal joints, or compound joints such as a differential joint. In an N degree of freedom manipulator, there are N links and N joints (compound joints are considered as a number of joints with links of zero length between the joints). A link maintains a fixed relationship between the manipulator joints at the ends of the link.

There are four kinematic parameters that determine the relative pose of two sequential joints in a manipulator. In FIG. 1, the spatial relationship between the coordinate system (x.sub.i-1,i,y.sub.i-1,i,z.sub.i) at O.sub.i (the ith coordinate system) and (x.sub.i,i+1,y.sub.i,i+1,z.sub.i) at O.sub.i+1 (the i+1th coordinate system) is shown. This relationship can be described in terms of four linkage or kinematic parameters d.sub.i, a.sub.i,i+1, .theta..sub.i, and .alpha..sub.i,i+1. One of the kinematic parameters is called the joint variable because it changes as the joint changes. For a revolute joint .theta..sub.i is the joint variable and for a prismatic joint d.sub.i is the joint variable. The remaining parameters remain fixed as the joint changes. The relationship between the ith coordinate system and the i+1th coordinate system can be described in terms of the matrix T.sub.i,i+1 : ##EQU1## where: c.theta..sub.i =cosine .theta..sub.i

s.theta..sub.i =sin .theta..sub.i c.alpha..sub.i,i+1 =cosine .alpha..sub.i,i+1 s.alpha..sub.i,i+1 =sin .alpha..sub.i,i+1

From this matrix, the distance beteen the two origins (O.sub.i and O.sub.i+1) can be determined as well as the orientation between the axes of the two coordinate systems. Also, as more links are added to the manipulator, the matrices corresponding to each link are multiplied together. Therefore the entire manipulator is described by the matrix:

T.sub.O,N+1 =T.sub.O,1 T.sub.1,2 T.sub.2,3 . . . T.sub.N,N+1 (2.2)

For a more detailed analysis and description of the mathematics of serial link manipulators see Reference No. 16 listed below [Paul, 1981].

In a typical robot work station, the robot base is placed in a "world" system. Part feeders and other fixtures are placed at known poses in the world coordinate system. In order to program a robot offline, it is necessary for the robot controller to accurately know both the achieved and desired pose of the robot end effector in the world coordinate system. From T.sub.O,N+1, the achieved pose of the manipulator end effector relative to the world coordinate system can be determined for any configuraton of the manipulator joints, provided the actual kinematic parameters are known.

III. Functional Description of Typical Manipulator Software

Robot links and joints can not be manufactured and constructed perfectly and the placement of the base of the manipulator in the world coordinate system is not exactly known. Therefore, all of the kinematic parameters of each of the actual T.sub.i,i+1 matrices will be slightly different than their nominal values. Thus, in the present art, robot controllers use T.sub.i,i+1 matrices based on nominal kinematic parameters which are slightly in error. Therefore, if the manipulator controller commands the manipulator end effector to a certain desired pose in space, the actual pose obtained by the end effector will be different.

The software present in the controller of a typical robot manipulator is composed of routines called the forward and reverse kinematics and the forward and reverse Jacobians. Different manufacturers may have different names for these software routines, but functionally their routines can be subdivided into these groups. In the prior art, all of these routines are based on the nominal kinematic parameters and thus cause pose errors.

(1) Forward Kinematics Given the value of the joint variable for each joint of the manipulator, the forward kinematics determines the pose of the end effector. (2) Reverse Kinematics Given the desired pose of the manipulator end effector, the reverse kinematics determines the value of each joint variable so that the manipulator end effector will be at the desired pose. (3) Forward Jacobian Given incremental changes in the joint variables, the forward Jacobian determines the corresponding incremental changes in the end effector pose. (4) Inverse Jacobian Given desired incremental changes in the pose of the end effector, the inverse Jacobian determines the incremental changes in the joint variables necessary to bring about the desired incremental changes in the end effector pose.

In the prior art, robots have been commanded using the technique shown in FIG. 2. The desired pose information is input to the robot and the nominal reverse kinematics is run to determine nominal joint commands. The robot is commanded with the nominal joint commands and an actual achieved pose results. The actual achieved pose is not equal to the desired pose because of the error sources previously discussed.

IV. Overview of the Operation of the Invention

Our calibration and control system improves robot accuracy by eliminating the effects of the kinematic parameter errors on pose placement. This is done by first determining a very accurate model of the robot which includes the exact kinematic parameters of the robot. Then, prior to commanding the robot to a desired pose, the model is used to generate joint variable (or pose) corrections which when added to the nominal commanded joint variables (or pose) to form new joint (or pose) commands will cause the actual achieved robot pose to be nearly equal to that desired.

The kinematic parameter model is created by a calibration process. In that process, the robot is commanded to a series of desired poses and the difference between the desired poses and those actually achieved is measured. The pose error data is then used to determine the accurate robot model as described below. The error between the desired and achieved poses can be measured using any of a variety of contact or non-contact pose measurement sensors such as the one described in co-pending U.S. patent application Ser. No. 794,798 filed Nov. 4, 1985 contents of which are expressly incorporated herein by reference. The measurement range of at least one sensor must include each of the poses where the error data used during the calibration process is to be obtained. The sensors will be mounted at poses surrounding the robot using a structure such as the one shown in FIG. 3, comprising a sensor base 10 and a plurality of sensors 12. During calibration, the robot is commanded to each of a plurality of sensor poses and the pose errors are measured. Sensors 12 are mounted very accurately in relation to each other, but they are not mounted accurately relative to the robot. The base fixture 10 of the sensors will establish a world coordinate system in which the robot will be made accurate. The base fixtures could be mounted relative to each other using a variety of measuring instruments including: a coordinate measuring machine, a laser interferometer, or a theodolite.

Prior to commanding the robot to a desired pose, the invention's pose correction algorithm uses the accurate robot model to predict the pose placement errors and to generate joint variable corrections which are then added to the joint variable solutions obtained by the nominal robot reverse kinematics to create corrected joint variable commands. The corrected commands cause the end effector pose to be nearly equal to the desired pose. The prediction and correction are preformed before the robot moves to the commanded pose. Thus there are no groping actions by the end effector which could cause catastrophic results in delicate assembly tasks. The pose correction algorithm is described below.

V. Operation of the Invention

The operation of the invention is as follows:

During an initial calibration operation, the actual forward kinematics 20 and the actual inverse Jacobian 22 of the robot are calculated by:

a. commanding the manipulator to a series of desired poses adjacent fixed sensors; b. using the detected pose errors to determine kinematic parameter errors; c. using the determined kinematic parameter errors to calculate a model of the actual forward kinematics for the manipulator; and d. taking the derivative of the determined actual forward kinematics with respect to each of the robot's joint variables to calculate the actual robot inverse Jacobian.

In the process of determining the kinematic parameter errors, a comparison is made between the achieved pose as measured by the sensors (measured pose) and the commanded pose. As will be explained below, by knowing the achieved (measured) pose, the commanded pose, and the nominal kinematic parameters, a model of the actual forward kinematics for the manipulator can be obtained.

The overall operation of the invention will now be explained for a robot controller which receives joint commands referring to FIG. 4a. The nominal reverse kinematics 14 and the robot manipulator 16 are as before in FIG. 2. However, the subject matter within dotted line 18, which includes a model of the actual forward kinematics 20 and the actual robot inverse Jacobian 22, has been added. The actual forward kinematics 20 is used to convert the nominal joint commands to an expected achieved pose. The difference between the desired pose and expected achieved pose comprises a predicted pose error. The predicted pose error is converted into joint correction commands by the actual robot inverse Jacobian 22. The nominal joint commands and the joint correction commands are combined and input to the physical robot manipulator 16 causing the robot to achieve the desired pose.

It is also possible, as shown in FIG. 4a to provide a closed loop system in which the achieved pose is in the area of a sensor 12 (FIG. 3) and the resultant measured pose is used to provide pose error data due to unmodelled sources.

Specifically, the invention can also be used in a direct feedback mode. In this mode, the robot operates within the work volume of one or more of the pose sensors 12. In addition to the joint correction signals resulting from the previously described modes of operation, the invention generates joint correction signals based on any difference between the desired pose and the actual pose. This direct feedback mode of operation still includes the calibration step to determine the robot's system parameters. The calibration step must be preformed because the robot is a very complicated structure which can only be modelled using non-linear equations. Simply measuring the difference between the desired and actual poses and determining joint correction signals using the nominal robot Jacobian will result in a "limit cycle" about the desired pose. The "limit cycle" results because the kinematic influences of small joint corrections on the hand pose corrections are slightly different for the actual robot than for the nominal robot. For highly accurate pose placement control, the actual Jacobian must be used rather than the nominal Jacobian because the actual Jacobian is based on the actual linkage parameters while the nominal Jacobian is based on the nominal kinematic parameters. The accurate model of the robot derived through the calibration procedure is used, as in the invention's other modes of operation, to determine the actual robot Jacobian.

In the system of FIG. 4b, the corrections provided by the invention are combined with the desired pose commands and input to the robot controller's nominal reverse kinematics 14. The system in FIG. 4b is used when the robot controller accepts only desired pose commands as its input. Thus the nominal robot forward Jacobian 26 is used to convert the joint correction commands to pose correction commands.

In either embodiment, the actual pose that the end effector would achieve if it were commanded by the inputted nominal joint commands is calculated using an actual forward kinematics routine stored a data processor memory of the invention. This forward kinematics uses the actual kinematic parameters determined earlier in time by our calibration process. To understand the invention, it is important to distinguish between the following two poses:

(i) The desired pose of the end effector, which is the pose commanded by the traditional manipulator controller. This is the pose which the manipulator software "thinks" that the end effector will go to if the nominal joint commands determined by the (flawed) manipulator reverse kinematics were input to the manipulator joints.

(ii) The anticipated achieved pose which the end effector will go to if commanded by the nominal joint commands determined by the (flawed) manipulator software. The anticipated actual pose is determined by the (correct) software in the invention which takes into account the actual manipulator kinematic parameters.

The difference between the desired end effector pose (i) and the anticipated actual end effector pose (ii) is determined by software in the invention. The kinematic parameter errors cause the differences between the anticipated actual pose and the desired pose, and therefore, result in the need for an incremental correction in the end effector pose to be made. The manipulator actual inverse Jacobian based on the actual kinematic parameters is executed to determine the incremental changes in the joints necessary to make the correction between the desired end effector pose and the anticipated actual pose. The nominal inverse Jacobian cannot be used in this step because it will lead to a limit cycle about the desired pose.

The incremental joint corrections are added to the joint commands obtained above to obtain the corrected joint commands. The corrected joint commands constitute the output of the invention. The manipulator is commanded using the corrected joint commands and will therefore move to the desired pose.

Note that calibration is not required before every pose command. Calibration is required only once to obtain the pose error data and the actual kinematic parameters and thereafter when the robot preformance has changed for some reason. During a robot's operation calibration may be re-preformed automatically, such as between manufacturing batches. The procedure above is the basic operation of the invention. As described below, operation can be enhanced by looping through the pose correction procedure until a minimum error criterion has been met.

As noted above, the invention is implemented in two slightly different ways depending on the type of robot controller which with it is used. FIG. 4a shows the first implementation. In this case, the robot controller has an input port which accepts joint commands. Our invention determines joint command corrections, adds them to the nominal joint commands to create the corrected joint commands, and then commands the robot with the corrected joint commands. FIG. 4b shows the implementation for the case where the robot accepts only pose commands. In this implementation, our invention first determines the joint corrections as before, but then converts the joint corrections into a pose correction by multiplying them by the nominal forward Jacobian. The pose correction is added to the desired pose to create a corrected pose command which is input to the robot.

VI. Process Operation

The pose correction process operates in either of two ways. If all of the required poses for an assembly task have been predetermined (for example, a batch job), then the joint variable corrections may be computed for all of the poses before the task begins. In this case, the system does not have to operate in real time. In the alternative operation, the pose corrections and corresponding joint variable corrections are computed by a fast processor between poses, and sent to the controller as needed.

Functional descriptions of the process operation are shown in FIG. (5a) for an online programmed robot, and in FIG. (5b) for an offline programmed robot. The two algorithms used to obtain the actual linkage model and the pose correction algorithm may be written so that they can be applied to any robot or robot type with any number of degrees of freedom.

The process operation for an online programmed robot is as follows. The operator moves the robot's hand to each of the test poses where sensors are located and the pose error is measured. The operator can move the robot to the sensor using either a teach pendant or by a preprogrammed routine. The operator then trains the robot to do its task and stores the joint positions for each pose. Again he moves the hand to each sensor to verify that the performance of the robot has not changed. Using the pose error data, the data processor determines the exact linkage model of the robot and uses the model to convert the joint positions stored for each of the task into the actual task pose. These actual poses are stored by the data processor as the task program data. The robot is then allowed to perform the desired tasks as required. Periodically, the robot is recalibrated by moving its hand to each of the sensor test poses and determining its actual linkage model. If the actual robot model has changed (due to drift or wear) then the actual poses are converted to new corrected joint positions and the task continues. By storing the task program as actual taught poses, rather than joint positions, the task program has been made independent of the performance of the robot and even the particular robot or robot kinematic arrangement used. If the robot requires maintenance, or if the same task is to be performed by a different robot, the task does not need to be retaught. The recalibration can be done automatically without requiring human intervention.

The process operation for an offline programmed robot is as follows. A CAD system is used, offline, to develop a sequence of desired poses which are stored as a task program. The task program and task program data are then downloaded to the robot which is to complete the task. Before the robot is to preform the task program, it is commanded to each of the test poses within the measurement range of the pose sensors. The pose error is measured at each sensor and the data processor determines the actual linkage model. The data processor uses the model to convert the desired poses of the task (which were generated offline) to the commanded poses necessary to make the actual poses of the robot equal to those desired. Periodically, as the performance of the robot changes, the robot is recalibrated by commanding it to each of the sensor test poses and then redetermining the actual linkage model. This is done with no operator intervention. By using our process, an offline generated task program is made independent of the robot's performance and of the particular robot or robot kinematic arrangement used.

DETAILED DESCRIPTION OF POSE CORRECTION ALGORITHM

I. Error Vector and Parameter Definitions

The error between an achieved pose and a desired pose is described by two error vectors, the position error vector, .sub.P .epsilon., and the orientation error vector, .sub.R .epsilon.. The position error vector defines the distance and the direction of the achieved origin of a body relative to the desired origin of the body. The orientation error vector describes the orientation of a frame fixed in a body at its achieved pose relative to the orientation of the same frame at the desired pose of the body. The magnitude of the orientation vector describes the amount that the desired pose must be rotated about the direction of the orientation error vector to obtain the achieved orientation. Typical position and orientation error vectors are shown in FIGS. 6a and 6b as they relate to a desired and achieved pose.

The pose error vectors, .sub.P .epsilon. and .sub.R .epsilon., are separated into those that are predicted, .sub.P e and .sub.R e, and those which are not, .sub.p E and .sub.R E, as given below and shown in FIG. 7

.sub.R .epsilon.=.sub.R e+.sub.R E (4.1a) .sub.P .epsilon.=.sub.P e+.sub.P E (4.1b)

The predicted errors are due to the kinematic (linkage) parameter and joint variable errors and are correctable by proper selection of pose corrections .sub.P .xi. and .sub.R .xi.. The unpredicted errors are due to other error sources such as backlash and link compliance. This pose correction technique can be expanded to include these other error sources within the predicted pose error vectors.

There are three distinguishable sets of kinematic (linkage) parameters used in the software of this invention: the sets of nominal, actual and corrected kinematic parameters. The nominal set of kinematic parameters are specified by the manufacturer. In the prior art, the robot motion planning and control software is based on the nominal kinematic parameters. The nominal kinematic parameters are denoted by .sub.N d.sub.i, .sub.N .theta..sub.i, .sub.N a.sub.i,i+1, and .sub.N .alpha..sub.i,i+1. Linkage transformations based on the nominal kinematic parameters are .sub.N T.sub.i,i+1, and .sub.N T.sub.0,n+1. The actual kinematic parameters, including the actual joint variable, are denoted by .sub.A d.sub.i, .sub.A .theta..sub.i, .sub.A a.sub.i,i+1, and .sub.A .alpha..sub.i,i+1, where: ##EQU2##

The actual errors in the nominal kinematic parameters, including the joint variable error, are .sub.A dd.sub.i, .sub.A d.theta..sub.i, .sub.A da.sub.i,i+1, and .sub.A d.alpha..sub.i,i+1. Estimates of these errors can be found using the calibration algorithms to be described below. Depending on whether joint i is prismatic or revolute, d.sub.i or .theta..sub.i is the joint variable q.sub.i, respectively. Thus the following equation is used to replace either Eq. (4.4a) or (4.4c).

.sub.A q=.sub.N q+.sub.A dq (4.5)

The link transformations based on the actual kinematic parameters are .sub.A T.sub.i,i+1, and .sub.A T.sub.0,n+1. A corrected set of joint variables are used to make the achieved pose of the end effector close to that desired. The corrected kinematic parameters are equal to the actual kinematic parameters for the kinematic parameters which cannot be actively modified. But for the joint variable, which can be modified, the corrected kinematic parameter is different than the actual kinematic parameter. For a prismatic joint and revolute joint, respectively:

.sub.C d.sub.i =.sub.A d.sub.i +.sub.C dd.sub.i (4.6a) .sub.C .theta..sub.i =.sub.A .theta..sub.i +.sub.C d.theta..sub.i (4.6b) Or more generally: .sub.C q.sub.i =.sub.A q.sub.i +.sub.C dq.sub.i (4.7) The linkage transformation based on the corrected kinematic parameters are .sub.C T.sub.i,i+1, and .sub.C T.sub.0,n+1.

II. The Pose Correction Algorithm

A higher level motion planning and control algorithm is used in the prior art to generate the desired pose, .sub.D T.sub.0,n+1, of the end effector or tool. In online systems the higher level algorithms can be based on human experience, while in offline systems the algorithms can be based on either retrieval or generative techniques. The nominal joint variables, .sub.N q, which would cause the nominal robot to go to the desired pose, are determined by solving the nominal robot inverse kinematics.

.sub.N T.sub.0,n+1 (.sub.N q)=.sub.D T.sub.0,n+1 (4.8)

If the robot joints were commanded to achieve the nominal joint variables, .sub.N q, the robot will not go to the desired pose, .sub.D T.sub.0,n+1, but will end up at a different achieved pose which is predicted by the invention once the actual kinematic parameters are known. To compensate for the predicted pose errors, in accordance with the present invention, the predicted pose is to be corrected in an iterative manner as shown in FIG. 8. The pose corrections when added to the predicted pose errors will cause the actual error between the desired pose and the predicted achieved pose to be zero and the difference between the desired pose and the pose actually achieved to be minimized. The pose corrections are used to determine the required joint variable corrections, .sub.C dq. The joint variable corrections are added to the nominal joint variables to obtain the corrected joint variables. The corrected joint variables are then used to predict the robot pose which would be achieved if the actual robot were commanded with the corrected joint variables. This pose is called the corrected pose. The corrected pose is compared to the desired pose to determine if it is within the robot resolution of the desired pose (a discussion on robot resolution can be found in Reference No. 6 listed below [Colson and Perreira, 1985; 1-31]). If the corrected pose is within the robot resolution of the desired pose, the robot is commanded to the corrected pose. If the corrected pose is not within the robot resolution of the desired pose, the nominal joint variables are set equal to the corrected joint variables and the procedure is repeated until the corrected pose is within the pose resolution of the desired pose.

The procedure used to determine the joint variable corrections can be implemented in either of two ways. If all of the required poses for an assembly task have been predetermined (for example, a batch job), then the joint variable corrections can be computed for all of the poses before the task begins. In this case, the pose correction algorithm does not have to operate in real time. Alternatively, the pose corrections and corresponding joint variable corrections can be computed by a fast processor between poses, and sent to the robot controller as needed.

A property of this algorithm is that it treats the joint variable corrections and the kinematic parameter errors in exactly the same way. Therefore, the end effector pose will be corrected to the same "accuracy" to which the kinematic parameter errors were determined.

PREDICTED POSE ERRORS AND THE REQUIRED POSE CORRECTION

If the robot joints are commanded to achieve the nominal joint variables, .sub.N q, the robot will not go to the desired pose, .sub.D T.sub.1,n+1, but will end up at a different achieved pose and a pose error results. Before the robot is commanded, the pose correction algorithm determines a pose correction so that the sum of the pose correction and the predicted pose error will be zero:

.sub.R e+.sub.R .xi.=0 (4.9a) .sub.R e+.sub.R .xi.=0 (4.9b) The pose correction, .sub.C dT.sub.1,n+1, the difference between the desired pose and predicted achieved pose, is found from, .sub.C dT.sub.1,n+1 =.sub.D T.sub.1,n+1 -.sub.A T.sub.1,n+1 (4.10) where .sub.A T.sub.1,n+1 is the predicted achieved pose due to the kinematic parameter errors. The differential homogeneous transformation given in Eq. (4.10) is written in terms of the product of the predicted achieved pose and an infinitesimal screw rotation matrix, .sup.n+1.sub.C .DELTA.. .sub.C dT.sub.1,n+1 =.sub.A T.sub.1,n+1 .sup.n+1.sub.C .DELTA.(4.11) where ##EQU3##

Equation (4.11) is valid only if the required pose correction is small. In cases where the pose correction is not small, either a finite screw rotation matrix or an iterative form of the procedure described here must be used. If the pose corrections are not small, the diagonal terms will not be zero, and a nonskew symmetric rotation submatrix will occur. In the algorithm presented below, the values required for .sup.n+1.sub.C .delta..sub.x, .sup.n+1.sub.C .delta..sub.y and .sup.n+1.sub.C .delta..sub.z are taken to be the average of the appropriate elements of .sup.n+1.sub.C .DELTA.. The upper left 3.times.3 skew symmetric submatrix deals with orientation errors, while the 3.times.1 submatrix in the upper right deals with the position errors. The differential displacements and rotations .sup.n+1.sub.C d.sub.x, .sup.n+1.sub.C d.sub.y, .sup.n+1.sub.C d.sub.z, .sup.n+1.sub.C .delta..sub.x, .sup.n+1.sub.C .delta..sub.y and .sup.n+1.sub.C .delta..sub.z are along and about the n+1th frame as shown in FIG. 9.

The relationship between the screw rotation matrix and the orientation correction vector, .sub.R .xi., and the position correction vector, .sub.P .xi. is determined by first combining Eqs. (4.10) and (4.11) to yield:

.sup.n+1.sub.C .DELTA.=.sub.A T.sup.-1.sub.1,n+1D T.sub.1,n+1 -I (4.13) The differential translation and rotation matrix is made up of components of the two pose correction vectors, which are written in the n+1th basis as: ##EQU4##

The components of the correction vectors are determined by first recalling the following general rules of homogeneous transformations and their inverses, and then substituting these expressions into Eq. (4.13). A homogeneous trasformation between the i and j frames is written as: ##EQU5## where the first three columns of T.sub.i,j represent the components of j frame unit vectors (x.sub.j-1,j, y.sub.j-1,j, and z.sub.j) written in terms of the i frame unit vectors (x.sub.i-1,i, y.sub.i-1,i, and z.sub.i). The fourth column of T.sub.i,j represents the vector, r.sub.i,j, from O.sub.i to O.sub.j written in terms of the i frame unit vectors. The unit vectors and reference frames are shown in FIG. 1. Whenever we wish to describe the jth frame unit vectors and the position vector from O.sub.i to O.sub.j in terms of the ith frame directions, we will indicate their use by n.sub.i,j, s.sub.i,j, a.sub.i,j, and p.sub.i,j, respectively, where: ##EQU6## The inverse of the transformation T.sub.i,j is: ##EQU7##

Equations used to evaluate the pose correction vectors are determined by first substituting Eqs. (4.15) and (4.17) into Eq. (4.13), followed by simplifying the results using Eqs. (4.12), (4.14), and (4.16). The results of the simplification are: ##EQU8## Thus, both correction vectors are based on known quantities.

POSE AND JOINT VARIABLE CORRECTIONS

The relationship between the pose correction vectors and the required joint variable corrections is found by first taking the total derivative of Eq. (4.3) (when the T.sub.i,i+1 's are based on the actual linkage parameters), and writing the resulting expression in terms of the individual link differential transformations. It is assumed that the joint variable corrections are very small and higher order terms can be neglected. This assumption will not reduce the generality of this pose correction algorithm when the required joint variable corrections are large since this is an iterative procedure as shown in FIG. 8. ##EQU9## Equation (4.19) expresses the pose correction transformation, .sub.C dT.sub.1,n+1, in terms of the joint variable corrections. Each joint transformations, .sub.A T.sub.i,i+1, has an associated correction transformation, .sub.C dT.sub.i,i+1, due to the joint variable correction, dq.sub.i.

.sub.C dT.sub.i,i+1 =.sup.i.sub.C .DELTA..sub..theta.d A T.sub.i,i+1 (4.20)

The expression for .sup.i.sub.C .DELTA..sub..theta.d is very similar to Eq. (4.12). It can have either non-zero .sub.C .delta.z.sub.i or non-zero .sub.C .delta.z.sub.i, since joint i is either a prismatic or a revolute. ##EQU10## Substitution of Eqs. (4.21) and (4.20) into Eq. (4.19) results in an expression that directly relates the pose correction transformation to the joint variable corrections. ##EQU11##

The relationship between the pose correction vectors, .sub.P .xi. and .sub.R .xi., and the joint variable corrections is determined by first substituting Eq. (4.11) into Eq. (4.22), and then multiplying the result by the inverse of .sub.A T.sub.1,n+1. ##EQU12## Equations (4.14a-b) are then used with Eq. (4.23) to obtain the pose correction vectors in terms of the joint variable corrections. ##EQU13##

Equations (4.18a-b) have been used so that the resulting expression can be written independently of the coordinate system. The effect of each of the two types of joint variable corrections, dz.sub.i or d.theta..sub.i, in the i,i+1 link on the position of the hand pose is shown in FIG. 10.

SYSTEM EQUATIONS

Equations (4.24a-b) result in a system of n equations in n unknowns that are used to determine the joint variable corrections. The form of the system of equations is examined in this section. Two matrix equations are written, one for each type of pose correction. These equations are,

.sub.C .delta.X=.sub.A R.sub.1C .delta.q (4.25a) .sub.C dX=.sub.A P.sub.C .delta.q+.sub.A R.sub.2C dq (4.25b)

The system equations include only n unknowns, since a joint is either a prismatic or a revolute; either .sub.C d.theta..sub.i or .sub.C dd.sub.i is zero. The joint variable corrections due to revolute joints are represented by .sub.C .delta.q, while the joint variable corrections due to prismatic joints are represented by .sub.C dq. The pose orientation corrections are represented by .sub.C .delta.X, while the pose position corrections are represented by .sub.C dX. ##EQU14## The matrices .sub.A R.sub.1, .sub.A R.sub.2 and .sub.A P are given by, ##EQU15##

The indices i, . . . , j, . . . , k represent the indices of the revolute joints of the robot. If there are l revolute joints, then .sub.A R.sub.1 is of dimension 3.times.l. The indices 1, . . . , m, . . . , r represent the indices of the prismatic joints of the robot. If there are .kappa. prismatic joints, then .sub.A R.sub.2 is of dimension 3.times..kappa.. Note that .kappa.+l=n. The equations are written in terms of a single matrix, the actual Jacobian, ##EQU16##

SOLUTION TECHNIQUES

The joint variable corrections are determined using the system of scaler equations resulting from Eq. (4.28). The corrections are solved for, and the joint variables are updated. The system of equations could be non-singular, singular, square or non-square. Particular methods to determine the joint variable corrections for each of these cases are given below.

When the actual Jacobian is square and non-singular, the solution is straightforward. .sub.A J is square when the robot has six degrees of freedom. When .sub.A J is square and non-singular the solution is given by, ##EQU17## In the special case when .sub.A R.sub.1, .sub.A R.sub.2, and .sub.A P are also square, as for a particular robot, the IBM 7565, Eq. (4.29) reduces to,

.sub.C .delta.q=.sub.A R.sub.1.sup.-1.sub.C .delta.X (4.30a) .sub.C dq=.sub.A R.sub.2.sup.-1 [.sub.C dX-.sub.A P.sub.A R.sub.1.sup.-1.sub.C .delta.q] (4.30b)

There are three additional important cases, which commonly occur among industrial manipulators. In these cases the actual Jacobian is not square or singular and the technique given above can not be used. The three cases and the descriptions of the methods to solve for the joint variable corrections are given below.

(1) When the robot has six degrees of freedom, .sub.A J is square but also singular. This occurs in two situations. In the first situation, at least two different .sub.C d.theta..sub.i 's exert the same influence over the orientation corrections, .sup.n+1.sub.C .delta..sub.x, .sup.n+1.sub.C .delta..sub.y, .sup.n+1.sub.C .delta..sub.z, and the position corrections, .sup.n+1.sub.C d.sub.x, .sup.n+1.sub.C d.sub.y and .sup.n+1.sub.C d.sub.z. In the second situation, two different .sub.C dd.sub.i 's exert the same influence over the position corrections, .sup.n+1.sub.C d.sub.x, .sup.n+1.sub.C d.sub.y and .sup.n+1.sub.C d.sub.z. In both cases two columns of the .sub.A J matrix are multiples of each other and the robot has "lost" a degree of freedom. Equation (4.28) no longer represents a set of six equations in six unknowns, but a reduced system of equations that is still square. In many industrial manipulators this occurs when two wrist joint axes are coincident. In this regard, see Reference No. 3 listed below [Colson and Perreira, 1983]. For a Puma 560, for example, this results in columns 4 and 6 of the Jacobian being identical.

(2) When a robot has less than six degrees of freedom, all poses within the work volume can not be achieved. It may not be possible for the robot to make the correction from the achieved to the desired pose because .sub.A J is of rank less than six. The "best" correction possible is determined using a least squares approach.

(3) When the robot has more than six degrees of freedom, there are an infinite number of combinations of joint variable corrections which will correct the pose from the achieved to the desired. A minimum norm of the joint variable corrections is used to obtain the "best" solution.

When the Jacobian cannot be inverted using standard techniques, the invention's software uses generalized inverse methods to obtain the joint corrections necessary to cause the desired pose correction. Robots can be classified by their number of degrees of freedom; less than, equal to, or greater than six degrees of freedom. Singularities of six degree of freedom manipulators are due to the causes listed above as cause number 1. In addition to cause number 2, a linkage of less than six degrees of freedom could also have singular points due to causes listed in cause number 1. A careful survey of kinematic arrangements used in industrial manipulators reveals that no such arrangement currently exists (see Reference No. 3 listed below [Colson and Perreira, 1983]). Robots with more than six degrees of freedom have singular points due to the causes listed in causes number 1 and 3. Methods for finding the generalized inverse Jacobian for each of these three classes of robots are given below.

III. Generalized Inverses

Generalized inverse techniques are used to find the inverse of the Jacobian for the three classes of robots when any of the above mentioned causes of singularity hold. The Moore-Penrose inverse may be used in these cases (see Reference No. 1 listed below [Ben-Israel and Greville, 1974]). The unique matrix X is called the Moore-Penrose inverse of matrix J if it satisfies one or more of the following conditions.

(1) JXJ=J (4.31a) (2) XJX=X (4.31b) (3) (JX).sup.T =JX (4.31c) (4) (XJ).sup.T =XJ (4.31d) A generalized inverse is symbolized with a superscript (+), so X=J.sup.+. A generalized inverse of a matrix J which satisfies only the ith and jth Moore-Penrose conditions is symbolized by J.sup.{i,j}. The Moore-Penrose inverse satisfying all four conditions is termed the complete Moore-Penrose inverse. The complete Moore-Penrose inverse gives the minimal least squares solution to a set of equations. The least squares solution to Equation (4.28) is the vector Dq which minimizes .parallel.DX-J Dq.parallel.. The minimal solution to Eq. (4.28) is the solution vector DX that minimizes .parallel.DX.parallel.. In both cases .parallel..multidot..parallel. is the Euclidean norm.

IV. Application of Generalized Inverse Techniques

An inverse satisfying the four Moore-Penrose conditions is always a sufficient way of obtaining the solution for the joint corrections; but it is not always necessary that all four conditions (4.31a-4.31d) be met. For each of the three classes of robots, less than, equal to, or more than six degrees of freedom, the use of the Moore-Penrose inverse which satisfies all four conditions will result in the solution to the joint corrections. Although it is not necessary to use the Moore-Penrose inverse on any particular robot, the Moore-Penrose inverse can be used to obtain generic inverses for general purpose applications. Less general methods of obtaining the joint corrections are also presented below.

ROBOTS WITH SIX DEGREES OF FREEDOM

Robots with six degrees of freedom will always have Jacobians that are square. In many cases, the Jacobian can be inverted using standard techniques. When the causes listed above in cause number 1 for a singular Jacobian arise, the joint variable corrections cannot be solved by standard matrix inversion. Only the first cause listed in cause number 1 is of practical significance. Examples of this singularity are in the wrist axes of most six degree of freedom robots. In the case of a robot such as the the nominal IBM 7565, as shown in FIG. 11, the Jacobian becomes singular when the pitch angle, .theta..sub.5, is zero and the illustrated hand points straight down. When .theta..sub.5 is zero, the yaw, .theta..sub.4, and the roll, .theta..sub.6, angles exert exactly the same kinematic influence over each of the six differential motions of the hand, d.sub.x, d.sub.y, d.sub.z, .delta..sub.x, .delta..sub.y and .delta..sub.z. This is because the 4th and 6th joint axes are coincident. In this event, two columns of the Jacobian become multiples of each other and thus are not independent. At a singular point due to causes listed in cause number 1 above, a robot loses a degree of freedom. In the case of the IBM 7565, no joint correction can cause a hand pose correction in the .delta..sub.x direction.

JOINT VARIABLE CORRECTIONS OBTAINED USING MOORE-PENROSE INVERSE

The Moore-Penrose inverse satisfying all four conditions results in the minimal least squares solution for the joint corrections necessary to cause the desired hand pose corrections. When the Jacobian is singular, the least squares solution is required because the range of the Jacobian is not the entire differential motion space. For the IBM 7565 example, when the desired set of hand pose corrections includes motion in the .delta..sub.x direction, the correction cannot be made. The joint corrections which cause the hand pose corrections to be as close as possible, in a least square sense, to the desired are determined. The projection of the desired correction onto the range of J is equivalent to the closest least squares correction (see Reference No. 1 [Ben-Israel and Greville, 1974 pg. 104]). There are an infinite number of joint correction vectors which will cause the hand corrections to satisfy the least squares solution. The minimal solution is needed so that a unique solution results. Any vector in the differential correction space may be written as a direct sum of its projection onto R(J).sup.p and its projection onto R(J) (see Reference No. 13 listed below [Kreyszig, 1978]). When the nominal IBM 7565, for example, is singular, the least squares property of the Moore-Penrose inverse will result in the following mapping. ##EQU18## The minimal property of the Moore-Penrose inverse is needed because the negative rotation of the yaw joint .theta..sub.4 and positive rotation of the roll joint .theta..sub.6 have exactly the same effect on the hand when the Jacobian is singular. Thus, whenever .theta..sub.4 =.theta..sub.6 ##EQU19## Because there exists non-zero vectors in N(J), an infinite number of solutions will cause any desired hand pose correction. The minimal property of the Moore-Penrose inverse will result in a unique solution.

IMPLEMENTATION

A procedure which preforms the same mappings as the complete Moore-Penrose inverse but which is specific to the particular linkage is given here. For clarity, an IBM 7565 linkage arrangement is used. Similar techniques can be used for other robots of this class. This implementation is choosen because it is computationally more efficient than computing the singular value decomposition. The IBM 7565 loses a degree of freedom at the singularity. The reduced set of 5 equations and 5 unknowns are solved. ##EQU20## The solutions for .theta..sub.6 and .theta..sub.4 are then determined from ##EQU21## The inverse given in Eq. (4.34) and the procedure given above maps the vectors in R(J).sup.p to the {O} vector, and finds the minimum norm solution for the projection of DX onto R(J) just as the complete Moore-Penrose inverse.

If this procedure is used, as an alternative to the complete Moore-Penrose inverse, the Jacobian must be checked for singularity. In the nominal IBM 7565, it is easy to determine that a singular Jacobian exists, because it is singular whenever .theta..sub.5 is zero; this is because the 4th and 6th joint axes are coincident. When the robot's kinematic parameter errors are taken into account, the joint variable values that result in a singular Jacobian are harder to determine. The 4th and 6th revolute joint axes may not be coincident when .theta..sub.5 is zero. Conversely, when .theta..sub.5 is not zero, the 4th and 6th axes may be coincident. Physical insight into the causes of robot singularities are used to develop efficient algorithms that detect the singular nature of the Jacobian and the required reduction in the system equations. A procedure which checks if the 4th and 6th axes are coincident is required.

One approach is to compare the entire first row of the Jacobian, the row that contains the kinematic influence of the joint variables on motion in the .delta..sub.x direction, to a threshold value which is close to zero. The threshold value is dependent on the particular computer on which the invention is implemented and the accuracy required. If every element of the first row is less than the threshold, no motion in the .delta..sub.x direction is considered possible, and the reduced system of equations is solved. Alternatively, the Euclidean norm of the first row of the Jacobian can be compared to a threshold.

ROBOTS WITH LESS THAN SIX DEGREES OF FREEDOM

The Jacobian of a robot with less than six degrees of freedom is not square and therefore cannot be inverted using standard procedures. For example, a SCARA (Selective Compliance Robot Arm for Assembly) robot with 4 degrees of freedom has a Jacobian of dimension 6.times.4 and Eq. (4.28) becomes, ##EQU22## When a robot has less than six degrees of freedom, the range of J is not the entire differential correction space. There is no guarantee that a desired set of differential corrections can be made by the end effector. The best least squares correction is the projection of the desired differential correction vector onto R(J). The complete Moore-Penrose inverse gives the solution to ##EQU23##

IMPLEMENTATION

Either the complete Moore-Penrose inverse or a robot specific pseudo-inverse can be used to obtain the least squares solution. A pseudo-inverse which satisfies only the 1st and 3rd Moore-Penrose conditions is sufficient to determine these joint corrections (see Reference No. 1 listed below [Ben-Israel and Greville, 1974 pg. 104]). One, of a number of inverses that satisfies these conditions, is

J.sup.+ =J.sup.{1,3} =(J.sup.T J).sup.-1 J.sup.T (4.38) This pseudo-inverse is valid only if the Jacobian J is of rank n. It cannot be used if the robot has a singularity caused by one the reasons listed above in cause number 1. Since no industrial robots with less than six degrees of freedom that meet either of these conditions have been found, the pseudo-inverse given in Eq. (4.38) is sufficient. If a linkage of less than six degrees of freedom did have a singular point due to one of the reasons listed above in cause number 1, the cause of the singularity should be removed using the method presented in the implementation of the technique for robots with six degrees of freedom. The pseudo-inverse of Eq. (4.38) can then be used on the reduced system of equations.

ROBOTS WITH MORE THAN SIX DEGREES OF FREEDOM

Robots with more than six degrees of freedom have non-square Jacobians. These Jacobians cannot be inverted using standard techniques. For example, a seven degree of freedom robot has a Jacobian which is 6.times.7. ##EQU24## Although the range of J, for these robots, is the entire differential correction space, the null space of J contains an infinite number of vectors other than the null vector. There are an infinite number of joint correction vectors which will cause the desired hand pose correction. The minimal property of the Moore-Penrose inverse is needed to determine a unique solution.

IMPLEMENTATION

Either the complete Moore-Penrose inverse or a robot specific pseudo-inverse can be used to obtain the minimal solution. A pseudo-inverse which satisfies only the 1st and 4th Moore-Penrose conditions is sufficient to determine these joint corrections when the robot is not at a singular point due to one of the reasons listed above in cause number 1 (see Reference No. 1 listed below [Ben-Israel and Greville, 1974 pg. 114]). One pseudo-inverse which satisfies these conditions is

J.sup.+ =J.sup.{1,4} =J.sup.T (JJ.sup.T).sup.-1 (4.40) At a singular point, due to one of the reasons listed above in cause number 1, the cause of the singularity should be removed using the method presented in the implementation of the technique for robots with six degrees of freedom. The pseudo-inverse of Eq. (4.40) is then used on the reduced system of equations.

CALIBRATION PROCEDURE

In accordance with the present invention, a calibration procedure, is used to determine the actual kinematic parameters of the robot. As shown in FIG. 12 by way of example and not limitation, this calibration procedure consists of the following steps:

(1) Placing one or more pose measuring sensors at locations within the workspace of the robot. If more than one sensor is employed, each is placed at a known location relative to one sensor, called the reference sensor. A number of calibration poses, each of which is within the measuring range of at least one pose measuring sensor, comprise the first input to the calibration process. (2) Commanding the actual robot system 42 to move to each of the calibration poses. The difference between each commanded calibration pose and the pose actually achieved is measured. The resultant measured pose errors constitute the second input to the calibration process. (3) Solving a set of linear equations 44, which model the actual robot by relating the pose errors to the errors in the kinematic parameters, to obtain the kinematic parameter errors, 46. The set of equations is derived by linearizing the robot's kinematic equations at the linkage configuration which gives the desired calibration pose. The output of the calibration process consists of the kinematic parameter errors. These errors are used to determine the actual system model 44.

We use one of two methods to solve for the kinematic parameter errors. The first we call the deterministic method. In this method, the number of equations is the same as the number of unknown kinematic parameters. The second method we call the least squares method. In this method, the number of equations is more than the number of unknown kinematic parameter errors. The equations are solved in a standard least squares technique for the kinematic parameter errors. In the deterministic method, the pose errors are made exactly zero at the locations of the sensors. In the least squares method, the pose errors at the sensor locations are minimized. The least squares method is less sensitive to measurement errors and other sources of pose errors.

I. Deterministic Method

Recent publications present methods for determining the linkage parameter and joint variable errors in some ways similar to that presented here, (see References Nos. 25, 10, 11 listed below [Wu 1983, 1984; Hayati 1983; and Hayati and Mirmirani 1984]). A number of small yet meaningful improvements to these techniques are presented below. These improvements concern the singular nature of the resulting system equations and procedures for large pose errors. The approach presented here can be used on all modern day robots, as it incorporates techniques to take modified inverses of the Jacobians or of the matrices of first order kinematic influence coefficients that typically occur. A particular method of parameter identification, known as a learning model technique, is used here. A complete, unsummarized version of the technique discussed here is given in Reference No. 12 listed below [Ibarra and Perreira, 1985].

DETERMINISTIC ERROR

In this derivation, the measured pose errors, .sub.P .epsilon..sup.m and .sub.R .epsilon..sup.m, are related to predicted pose errors, .sub.P e.sup.m and .sub.R e.sup.m, using ##EQU25## where .sub.P E.sup.m and .sub.R E.sup.m, are non-predicted pose errors.

Pose placement errors are due to a number of mechanical sources. Error sources include those associated with sensing, machining, and assembly of the linkage. Errors from a second group of sources can be analyzed using generalizations of the procedures described here; these sources are associated with the affect of linkage weight, payload, and dynamics on both the servo controller error and the linkage deformation. The second group also includes errors due to mechanical backlash and thermal affects.

It may be assumed for purpose of illustration and not limitation, that all pose errors are only due to errors between the actual and nominal linkage parameters and joint variables and that all pose measurements are exact and the non-predicted pose errors are identically zero. An alternative algorithm included in the invention, is based on minimizing the least square of the non-predicted pose errors. This algorithm is given in detail in Reference No. 15 listed below [Nanayakkara and Perreira, 1985]. This latter algorithm, which is described below, is less sensitive to unmodeled pose error sources and pose measurement errors but requires more computations. Thus, in applications of the invention where speed of operation is more important than very high accuracy, the deterministic algorithm is used, while in applications where high accuracy is more important than speed or when a very fast processor is available, the least squares algorithm is used.

COORDINATE FRAMES AND HOMOGENEOUS TRANSFORMATIONS

Kinematic chains are composed of a number of links interconnected by joints which allow relative motion. For all kinematic chains composed of these lower order pair connectors, it has been determined (see Reference No. 8 [Denavit and Hartenburg, 1955]) that homogeneous transformations could be used to define the relative pose of frames fixed to each of the links. The placement of the required reference coordinate systems on the individual links is not arbitrary and allows only four parameters to define the relative pose between two interconnected bodies. The approach typically used is to associate with each link a joint, S, and a Z axis (see References Nos. 8, 16, 9 [Denavit and Hartenburg 1955, Paul 1981, and Duffy 1977]). The joint must be either revolute, R, or prismatic, P. The common perpendicular between Z axes associated with two neighboring or interconnecting links is identified as a X axis. The selection made here and in Duffy's Unified System is shown in FIG. 1. The i frame and joint S.sub.i are situated at O.sub.i. The i frame is associated with the x.sub.i-1,i, y.sub.i-1,i, and z.sub.i unit vectors and X.sub.i-1,i, Y.sub.i-1,i, and Z.sub.i axes. In order to traverse the kinematic chain from the origin O.sub.i to the origin O.sub.i+1, four sequential steps are used. In each step a new coordinate axis is defined. The following description is summarized by the 4.times.1 link description matrix, known as the Hartenburg's Link Transformation Notation, shown in the upper left hand corner of FIG. 13. First, move along z.sub.i a positive distance d.sub.i ; this move defines the x.sub.i-1,i, y.sub.i-1,i, and z.sub.i,i+1 unit vectors (z.sub.i =z.sub.i,i+1). Secondly, rotate the x.sub.i-1,i, y.sub.i-1,i, and z.sub.i,i+1 unit vectors about z.sub.i,i+1 using a right hand rotation of magnitude .theta..sub.i. The resulting unit vectors are x.sub.i, y.sub.i, and z.sub.i,i+1. Next, move a positive distance a.sub.i,i+1 along x.sub.i ; this move defines the x.sub.i,i+1, y.sub.i, and z.sub.i,i+1 unit vectors (x.sub.i =x.sub.i,i+1). Rotate the x.sub.i,i+ 1, y.sub.i, and z.sub.i,i+1 unit vectors by the angle .alpha..sub.i,i+1 about x.sub.i,i+1 in a right hand fashion. The resulting unit vectors are x.sub.i,i+1, y.sub.i,i+1 and z.sub.i+1.

The origins O.sub.i and O.sub.i+1 define the two ends of link i,i+1. The vector from O.sub.i to O.sub.i+1 is r.sub.i,i+1. The i,i+1 link is described kinematically by the parameters; d.sub.i, .theta..sub.i, a.sub.i,i+1 and .alpha..sub.i,i+1 and the unit vectors noted above. Three of these kinematic link identifiers are typically taken to be constant parameters, while the fourth identifier, known as a joint or pair variable, is usually associated with relative link motion. If joint i is prismatic (S.sub.i =P.sub.i), d.sub.i is the joint variable, while if joint i is a revolute (S.sub.i =R.sub.i), .theta..sub.i is the joint variable. The relative pose of the i+1 frame to the i frame is described by the homogeneous transformation matrix ##EQU26##

The origin O.sub.1 is placed on the z.sub.1 connection axis between the reference body and the first moving link. O.sub.0 is placed at an arbitrary location in the reference body. The transformation for the robot base relative to the world, T.sub.0,1, and the transformation of a tool relative to the robot hand, T.sub.n+1,n+2, are assumed to be known and constant and thus do not contribute to the pose placement error of the robot's end effector. The technique discussed here can be easily modified to include the case where these assumptions are not valid.

The pose of the hand frame relative to the ground reference frame is determined by evaluating the expression ##EQU27## The evaluation of Eqs. (5.2) and (5.3) require the use of either nominal or actual linkage parameters and joint variables. The nominal kinematic parameters are donated by .sub.N d.sub.i, .sub.N .theta..sub.i, .sub.N a.sub.i+1, and .sub.N .alpha..sub.i,i+1. The linkage transformations based on the nominal kinematic parameters are .sub.N T.sub.i,i+1, and .sub.N T.sub.1,n+1. The actual kinematic parameters and joint variables are donated by .sub.A d.sub.i, .sub.A .theta..sub.i, .sub.A a.sub.i,i+1, and .sub.A .alpha..sub.i,i+1, where ##EQU28## The errors in the nominal kinematic parameters and joint variables are, dd.sub.i, d.theta..sub.i, da.sub.i,i+1, and d.alpha..sub.i,i+1. The link transformations, based on the actual kinematic parameters are .sub.A T.sub.i,i+1, and .sub.A T.sub.1,n+1.

DIFFERENTIAL TRANSFORMATIONS AND POSE ERRORS

When moving a manipulator, based on offline generated code, it is typically assumed that the measured joint variables and the nominally specified kinematic linkage parameters are the actual values. In reality, the actual and the nominal values differ. Thus, the actual and the nominal manipulator hand pose will also differ. The difference is given by

dT.sup.m.sub.1,n+1 =T.sub.A.sup.m.sub.1,n+1 -T.sub.N.sup.m.sub.1,n+1 (5.4) The nominal transformations are computed, while the actual transformations are based on measured pose data. The superscript m is used to identify particular pose placements where the pose error is to be described.

The pose error can be determined by using either experimental data or from a predictive model. The pose error can also be expressed in a number of reference frames. In this description, the hand or n+lth frame is used. This frame is selected because it minimizes the errors that result from using pose measurement devices. In addition, characterizing manipulator performance in the hand frame results in information required in manufacturing operations. Thus, the algorithm can be used by the robot designer to relate the manufacturing and assembly tolerance of the robot linkage to pose error predictions and required robot performance.

The differential homogeneous transformation given in Eq. (5.4) is related to a differential screw rotation or translation and rotation transformation, .sup.n+1 .DELTA..sup.m as in References Nos. 18 and 16 [Suh and Radcliff, 1978; Paul 1983].

dT.sup.m.sub.1,n+1 =.sub.N T.sup.m.sub.1,n+1.sup.n+1 .DELTA..sup.m (5.5a) where ##EQU29##

Equation (5.5) is valid only if the pose error is small. In cases where the pose error is not small either a finite screw rotation matrix or an iterative form of the procedure described here must be used. The pose error vectors can be determined in the latter case using the procedures outlined either in Reference No. 2 [Colson, 1984] or Reference No. 12 [Ibarra and Perreira, 1985].

MEASURED POSE PLACEMENT ERRORS

Conventional procedures and devices are used to measure the actual pose achieved by a manipulator, .sub.N T.sup.m.sub.1,n+1. The relationship between this transformation and the orientation error vector, .sub.R .epsilon..sup.m, and the position error vector, .sub.P .epsilon..sup.m is determined by first combining Eqs. (5.4) and (5.5) to yield.

.sup.n+1 .DELTA..sup.m =.sub.N T.sup.m.spsp.-1.sub.1,n+1 A T.sup.m.sub.1,n+1 -I (5.6) The differential translation and rotation matrix is made up of components of the two pose error vectors which are written in the n+1 basis as, ##EQU30##

The components of the error vectors are determined by first recalling the following general rules of homogeneous transformations and their inverses and then substituting these expressions into Eq. (5.6). A homogeneous transformation between the i and j frames is written as, ##EQU31## where the first three columns of T.sub.i,j represent the components of j frame unit vectors (x.sub.j-1,j, y.sub.j-1,j, and z.sub.j) written in terms of the i frame unit vectors (x.sub.i-1,i, y.sub.i-1,i, and z.sub.i). The fourth column of T.sub.i,j represents the vector, r.sub.i,j, from O.sub.i to O.sub.j written in terms of the i frame unit vectors. The unit vectors and reference frames are shown in FIG. 1. Whenever we wish to describe the jth frame unit vectors and the position vector from O.sub.i to O.sub.j in terms of the ith frame directions, we will indicate their use by n.sub.i,j, s.sub.i,j, a.sub.i,j, and p.sub.i,j respectively, where,

x.sub.j-1,j =n.sub.xi,j x.sub.i-1,i +n.sub.yi,j y.sub.i-1,i +n.sub.zi,j z.sub.i =n.sub.i,j (5.9a) y.sub.j-1,j =s.sub.xi,j x.sub.1-1,i +s.sub.yi,j y.sub.i-1,i +s.sub.zi,j z.sub.i =s.sub.i,j (5.9b) z.sub.j =a.sub.xi,j x.sub.i-1,i +a.sub.yi,j y.sub.1-1,i a.sub.zi,j z.sub.i =a.sub.i,j (5.9c) r.sub.i,j =p.sub.xi,j x.sub.i-1,1 +p.sub.yi,j y.sub.i-1,i +p.sub.zi,j z.sub.i =p.sub.i,j (5.9d) The inverse of the transformation T.sub.i,j is, ##EQU32## Equations used to evaluate the measured pose error vectors are determined by first substituting Eqs. (5.8) and (5.10) into Eq. (5.6) followed by simplifying the results by using Eqs. (5.5b), (5.7), and (5.9). The results of the simplification are ##EQU33## Both error vectors are thus based on known quantities. Typical pose error vectors are shown in FIG. 3.

POSE PLACEMENT ERRORS DUE TO KINEMATIC PARAMETER AND JOINT VARIABLE ERRORS

The relationship between the pose error vectors and the errors in the nominal linkage parameter and joint variables is found by first taking the total derivative of Eq. (5.3) and writing the resulting expression in terms of the individual link differential transformations. It is assumed that the linkage parameters and joint variables errors are very small and thus higher order terms can be neglected, ##EQU34##

The individual link homogeneous transformation errors are the result of errors in the link parameters and joint variables,

dT.sub.i,i+1 =.sup.i .DELTA..sub..theta.d T.sub.i,i+1 +T.sub.i,i+1.sup.i+1 .DELTA..sub..alpha.a (5.13) where the infinitesimal translation and rotation matrices about z.sub.i and x.sub.i,i+1 respectively, are ##EQU35## The substitutions ##EQU36## have been made to enumerate that each of the parameter and joint variable errors is associated with a particular axis and coordinate system. Substitution of Eq. (5.13) into Eq. (5.12) results in an expression that directly relates the pose transformation error to the nominal linkage parameters and joint variables and their errors. ##EQU37##

The relationship between the predicted pose error vectors, .sub.P e.sup.m and .sub.R e.sup.m, and linkage parameter and joint variable errors is determined by first substituting Eq. (5.5a) into Eq. (5.15) and multiplying the result by the inverse of .sub.N T.sup.m.sub.1,n+1 resulting in ##EQU38## Expressions similar to those given by Eqs. (5.5b) and (5.7) can be written to relate the predicted pose error vectors to the differential error transformation matrix defined in Eq. (5.16). Substitution of Eqs. (5.8) and (5.10) into Eq. (5.16) result in, ##EQU39## Equations Eqs. (5.9a) through (5.9b) have been used so that the resulting expression can be written independent of the coordinate system. The effect of the parameter and joint variable errors in the i,i+1 link on the position of the hand pose is shown in FIG. 10.

SYSTEM EQUATIONS

The substitution of Eqs. (5.11a and b) and Eqs. (5.17a and b) into Eqs. (5.1a and b) result in a set of system equations that can be used to determine the linkage parameter and joint variable errors. The substitution results in 2M vector equations or 6M scalar equations in the 4n unknowns dz.sub.i, .delta.z.sub.i, dx.sub.i,i+1, and .delta.x.sub.i,i+1 where i equals 1 through n (m equals 1 through M). ##EQU40## Each of the known vectors used in Eqs. (5.18a and b) have been written in terms of the n,n+1 vector basis for the reasons identified earlier. The components of the rotation error vector are taken to be the average of the differences of off-diagonal terms of the infinitesimal screw rotation matrix defined in Eq. (5.6). The average is used to reduce the error caused by employing the infinitesimal screw rotation matrix to approximate the finite screw rotation matrix.

CANONICAL FORM

Two matrix equations can be written, one for each type of pose errors. These equations are

.delta.X=R.delta.q (5.19a) dX=P.delta.q+R dq (5.19b) The linkage parameter and joint variable orientation errors are represented by .delta.q while the linkage parameter and joint variable position errors are represented by dq. The measured pose orientation errors are represented by .delta.X while the measured pose position errors are represented by dX. Where, ##EQU41## The R matrix is based on known quantities and is of dimension 3M.times.2n, where ##EQU42## The P matrix which is of dimension 3M.times.2n is also based on known quantities, where ##EQU43##

Equation (5.19) can be further simplified to yield ##EQU44## A similar form results when using the least squares approach shown below and listed in Reference No. 15 [Nanayakkara and Perreira, 1985] and in a pose correction algorithm shown above and listed in Reference No. 23 [Tucker and Perreira, 1985-3].

SOLUTION METHOD

In the case where R is not square (singular or non-singular) the solution techniques presented in Reference No. 21 [Tucker and Perreira, 1985-2] should be used. The following solution technique assumes that R is square (3m=2n).

NONSINGULAR CASE

Equation (5.19a) is first used to determine the 2n linkage parameter and joint variable errors that effect the orientation of the hand frame. The resulting solutions are then used in Eq. (5.19b) to determine the linkage parameter and joint variable errors that only effect the position of the hand.

.delta.q=R.sup.-1 .delta.X (5.21a) dq=R.sup.-1 [dX-P.delta.q] (5.21b) The inverse matrix can be found by a number of computational techniques; one of the most practical being the use of a maximum pivot strategy and applying the Gauss Jordan approach, either with an augmented matrix or in place.

CHECK FOR SINGULARITY

The solution technique given by Eqs. (5.21a and b) cannot be used when R is singular. For R to be singular, (1) two nominal joint axes, (2) two common perpendiculars between joint axes, or (3) a joint axis and a common perpendicular between two joint axes must be parallel at all of the selected measurement poses. That is, for some value of j and k (j.noteq.k) and for all values of m ##EQU45##

In the normal operation of the invention, several iterations of this calibration algorithm are used. The iterations are terminated when the values obtained for actual system parameters converge (ie the values obtained at an iteration are the same or approximately the same as the ones obtained at the previous iteration). The check for singularity is preformed at the start of each iteration resulting in a significant advantage over the methods listed in References Nos. 10 and 11 [Hayati, 1983, Hayati and Mirmirani, 1984] because in most cases our method will result in solutions for the linkage parameter errors which cause singularities in the nominal robot. Although a nominal robot may have parallel axes, when the actual linkage parameters are taken into account, the axes may not be exactly parallel. In most cases, only during the first iteration of the algorithm with the Jacobian be singular; during successive iterations the axes will "move away from being parallel" as the actual linkage parameters are updated, and the Jacobian will become non-singular.

SINGULAR CASE

When R is singular, an alternative solution procedure, also based on Eqs. (5.19a) and (5.19b), is used. A similar approach can be used when taking the inverse of any square singular robotic system Jacobian.

Let us assume that, in the particular robot being examined, the j th and the j+1 th z axes always remain parallel. We then define a matrix *R, where, ##EQU46##

The use of the modified inverse in Eq. (5.21a) results in

*I.delta.q=*R.sup.-1 .delta.X (5.24) and will directly determine the solution for .delta.z.sub.1 through .delta.z.sub.j-1 ; .delta.z.sub.j+2 through .delta.z.sub.n ; and .delta.x.sub.1,2 through .delta.x.sub.n,n+1. In addition, expressions of the form ##EQU47## result from the j and j+1 rows of Eq. (5.24). Note that .delta.z.sub.j =.delta.q.sub.j and .delta.z.sub.j+1 =.delta.q.sub.j+1. Although the right hand side of Eq. (5.21b) should equal zero identically, it will not if there are errors in the pose orientation measurements or if the orientation errors of the linkage is other than infinitesimal.

Equation (5.21b) becomes

*Idq=*R.sup.-1 [dX-P.delta.q] (5.26) To solve Eq. (5.26), .delta.z.sub.j and .delta.z.sub.j+1 must first be determined. This is done with the aid of the j+1 row of Eq. (5.26), which is ##EQU48## Equation (5.27) is rewritten so that only known quantities appear on the right hand side. ##EQU49## Equations (5.25a) and (5.28) are then used to solve for .delta.z.sub.j and .delta.z.sub.j+1.

Upon determining .delta.z.sub.j and .delta.z.sub.j+1, the column vector .delta.q and all terms on the right hand side of Eq. (5.26) are known, and solutions for dz.sub.1 through dz.sub.j-1, dz.sub.j+2 through dz.sub.n, and dx.sub.1,2 through dx.sub.n,n+1 can be determined. The jth row of Eq. (5.26) results in ##EQU50## where dz.sub.j =dq.sub.j and dz.sub.j+1 =dq.sub.j+1. Because the jth and j+1th Z axes are parallel, it is impossible to discriminate between dz.sub.j and dz.sub.j+1.

II. Least Squares Method

The second calibration algorithm included in this invention, is the least squares algorithm. This algorithm is based on minimizing the least square of the non-predicted pose errors. This algorithm is less sensitive to unmodeled pose errors sources and pose measurement errors than is the deterministic algorithm, but it requires more computations and therefore will not execute as fast. Thus, as stated above, there are some applications of the invention where the deterministic algorithm is used and some applications where the least squares algorithm is used.

LEAST SQUARE ERROR

The pose placement errors, .sub..beta. .epsilon., are due to kinematic parameter errors, .sub..beta. e and to a number of other sources, .sub..beta. E. A thorough discussion of these error sources is given in Reference No. 5 [Colson and Perreira. 1985-1,2]. Therefore, ##EQU51## as shown in FIG. 7.

The standard deviation, s, of the sampled set of errors is determined from, ##EQU52## We would like to minimize the variance of the sample by proper choice of the independent variable, dq.sub.j, equal to each of the linkage kinematic errors .delta.z.sub.j, dz.sub.j, .delta.x.sub.j,j+1, and dx.sub.j,j+1. The resulting minimization requires, ##EQU53## Where it has been noted that .sub..beta. .epsilon..sup.m is not an explicit function of the dq.sub.j 's. Equation (6.3) results in the required system equations. ##EQU54##

The orientation error vector, .sub.R e.sup.m, as given by Eq. (5.17a), is a function of 2n kinematic error variables, .delta.z.sub.i, and .delta.x.sub.i,i+1. Thus, the use of .beta.=R in Eq. (6.4) results in 2n equations which are used in determining the 2n unknowns .delta.z.sub.i, and .delta.x.sub.i,i+1 by the the technique described below. Substitution of .beta.=P into Eq. (6.4) results in 2n equations in the 2n unknowns, dz.sub.i, and dx.sub.i,i+1. Although the position error vector, .sub.P e.sup.m, as given by Eq. (5.17b), is a function of 4n kinematic error variables, .delta.z.sub.i, dz.sub.i, .delta.x.sub.i,i+1, and dx.sub.i,i+1 only 2n of these variables remain unknown. Thus, Eq. (6.4) represents 4n scalar equations in 4n unknowns.

CANONICAL FORM

The system equations given above are next rewritten in canonical form. We have found this form to apply to a number of differential kinematic problems in robotic manipulators. Equations (6.4) are reduced by first noting that ##EQU55##

Substitution of Eqs. (5.12), (5.176), and (5.21) into Eq. (6.4) with .beta.=R, and using Eqs. (5.8a) through (5.8d) and (5.110) to simplify the results gives; ##EQU56##

Substitution of Eqs. (5.12), (5.17), and (5.21) into Eq. (6.4) with .beta.=P and using Eqs. (5.10a) through (5.10d) to simplify the results gives: ##EQU57##

The resulting Eqs. (6.7a), (6.7b), (6.8a) and (6.8b) can be further reduced to yield,

.delta.X=R.delta.q (6.9a) dX=P.delta.q+Rdq (6.9b)

The pose orientation error matrix, .delta.X, and the pose position error matrix, dX, are defined by ##EQU58## The linkage parameter and joint variable orientation errors are represented by .delta.q, the rotation solution vector. The linkage parameter and joint variable position errors are represented by dq, the position solution vector, where ##EQU59##

The R matrix is of dimension 2n.times.2n, where ##EQU60## The R matrix is symmetric because .sub.R A.sub.ij =.sub.R A.sub.ji, .sub.R B.sub.ij =.sub.R C.sub.ji, and .sub.R D.sub.ij =.sub.R D.sub.ji. The P matrix is also of dimension 2n.times.2n, where ##EQU61## Equations (6.9a) and (6.9b) can be further condensed to yield ##EQU62## Where J is the Least Square Jacobian.

SOLUTION

The solution of Eqs. (6.9a) and (6.9b) is rather simple, since both R and P are square matrices. When R is singular, either a modification of the solution technique derived in Reference No. 12 [Ibarra and Perreira, 1985] or a generalized inverse solution technique given in Reference No. 20 [Tucker and Perreira, 1985-1] should be used. In the case that R is not singular the following approach is adequate;

.delta.q=R.sup.-1 .delta.X (6.11a) dq=R.sup.-1 [dX-P.delta.q] (6.11b) The inverse matrix can be found by a number of computational techniques; one of the most practical being the use of a maximum pivot strategy and applying the Gauss Jordan approach, either with an augmented matrix or in place.

REFERENCES

Each of the following materials, which may or may not be prior art, are expressly incorporated herein by reference:

1. Ben-Israel, A., and Greville, N. E., Generalized Inverses: Theory and Applications, Wiley, New York, N.Y., 1974. 2. Colson, J. C., "Performance Measures for Robotic Systems," Masters Thesis, University of Texas at Austin, December 1984. 3. Colson, J. C., and N. D. Perreira, "Kinematic Arrangements Used In Industrial Robots," 13th International Conference Proceedings, 20.1-20.18, Robotics International of SME (April 1983); also appearing in Lee, G. C. S., R. C. Gonzalez, and K. S. Fu, Ed., Tutorial on Robotics, Institute of Electronics and Electrical Engineering, 1983, 1985. 4. Colson, J. C., and N. D. Perreira, "The Need for Performance Measures for Robotic Systems and Some Qualitative Definitions," presented at and published in the conference proceedings of the Workshop on Robot Standards, cosponsored by NBS, ASTM, ANSI, IEEE and EIQ, Detroit, Mich., June 6-7, 1985. 5. Colson, J. C., and N. D. Perreira, "Statistics and Experimental Procedures for Determining Performance Parameters of Robotic Systems," presented at and published in the conference proceedings of the Workshop on Robot Standards, cosponsored by NBS, ASTM, ANSI, IEEE and EIQ, Detroit, Mich., June 6-7, 1985. 6. Colson, J., and N. D. Perreira, "Robot System Pose Performance: Definitions and Analysis," presented at and published in the conference proceedings of the International Computers in Engineering Conference and Exhibition, sponsored by ASME, Boston, Mass., Aug. 4-8, 1985. 7. Colson, J. C., and N. D. Perreira, "Pose Measurement Devices, Pose Error Determination and Robot Performance," presented at and published in the conference proceedings of Sensors' 85, sponsored by SME, Detroit, Mich., Nov. 4-7, 1985. 8. Denavit, J., and R. S. Hartenburg, "A Kinematic Notation for Lower-Pair Mechanisms Based On Matrices," Journal of Applied Mechanics, Vol. 22, Transactions of the American Society of Mechanical Engineers, Vol 77, 1955, pp. 215-221. 9. Duffy, J., Analysis of Mechanisms and Robot Manipulators, John Wiley & Sons: New York, N.Y., 1979. 10. Hayati, R., "Robot Arm Geometric Link Parameter Estimation," Proceedings of the 22nd IEEE Conference on Decision and Control, December 1983, pp. 1477-1483. 11. Hayati, S., and M. Mirmirani, "Puma 600 Robot Arm Geometric Calibration," International Conference on Robotics, Atlanta, Ga., Mar. 13-15, 1984 (supplemental pages). 12. Ibarra, R., and N. D. Perreira, "Determination of Linkage Parameter and Pair Variable Errors in Open Chain Kinematic Linkages Using a Minimal Set of Pose Measurement Data", presented at the 1985 Design Automation Conference, Cincinnati, Ohio, Sept. 10-13, 1985, and to be published in the ASME Transactions, Journal of Mechanisms, Transmissions, and Automation in Design. 13. Kreyszig, E., Introductory Functional Analysis with Applications, New York, Wiley, 1978. 14. Lau, K., L. Haynes and R. Hocken, "Robot End Point Sensing Using Laser Tracking System", presented at and published in the conference proceedings of the Workshop on Robot Standards, cosponsored by NBS, ASTM, ANSI, IEEE and EIQ, Detroit, Mich., June 6-7, 1985. 15. Nanayakkara, S. A., and N. D. Perreira, "A Least Square Technique to Determine Linkage Parameter and Pair Variable Errors in Open Kinematic Chains," presented at and to be published in the conference proceedings of the SIAM Conference on Geometric Modeling and Robotics, Albany, N.Y., July 15-18, 1985. 16. Paul, R., Robot Manipulators: Mathematics, Programming and Control, MIT Press, Cambridge Mass., 1981. 17. Phillips, N., J. R. Cowell, R. Bell, "The Measurement Capability of an Industrial Robot", 24th IMTDR Conference, 1985. 18. Suh, C. H., and C. W. Radcliffe, Kinematics and Mechanism Design, John Wiley and Sons, New York, N.Y., 1978. 19. Tucker, M. and N. D. Perreira, "Robot Calibration Device and Usage," Working Paper, Computer Integrated Manufacturing Systems Engineering Program, University of Nevada-Reno, December, 1984. 20. Tucker, M., and N. D. Perreira, "A Device to Measure the Pose of a Robot," Working Paper, Computer Integrated Manufacturing Systems Engineering Program, University of Nevada-Reno, March 1985. 21. Tucker, M., and N. D. Perreira, "Generalized Inverses for Robot Manipulators," presented at and to be published in the conference proceedings of the SIAM Conference on Geometric Modeling and Robotics, Albany, N.Y., July 15-18, 1985. 22. Tucker, M., and N. D. Perreira, "Motion Planning and Control for Improved Robot Performance," presented at the International Industrial Control Conference, Long Beach, Calif., Sept. 16-21, 1985. 23. Tucker, M., and N. D. Perreira, "A Pose Correction Algorithm," Proceedings of the ASME 1985 Winter Annula Meeting, Miami Beach, Fla., Nov. 17-22, 1985, and submitted for publication in the ASME Transactions, Journal of Dynamic Systems, Measurements, and Control. 24. Whitney, D. E., et al, "Short-Term and Long Term Robot Feedback," Prepared for the National Science Foundation under Grant No. MEA-82-07167, April, 1985. 25. Wu, C., "A Kinematic CAD Tool for the Design and Control of a Robot Manipulator," The International Journal of Robotics Research, Vol. 3, No. 1, Spring 1984, pp. 58-67.

The following represents one illustrative program embodying the teachings of the invention: ##SPC1##

Claims

1. A method for refining original command signals which are intended, by conversion to corresponding pair variable signals, to place, in response to said pair variable signals, a robot having mechanical linkage and joint actuators at a desired position and orientation (pose), comprising the steps of:

a. identifying actual system parameters of said robot which describe said mechanical linkage and joint actuators of said robot;

b. predicting, through the utilization of said actual system parameters and said original command signals, a predicted anticipated pose of said robot which would actually be obtained as a result of said original command signals;

c. calculating, as a function of the difference between said desired pose and said predicted anticipated pose, corrected command signals required to minimize the difference between said desired pose and said predicted anticipated pose; and

d. employing said corrected command signals to place said robot in said desired pose.

2. A method of claim 1 wherein said step of predicting includes utilizing forward kinematics of said robot based upon said actual system parameters.

3. A method of claim 1 wherein said actual system parameters include parameters which locate placement of said robot.

4. A method for refining original command signals which are intended, by conversion to corresponding pair variable signals, to place, in response to said pair variable signals, a robot having mechanical linkage and joint actuators at a desired position and orientation (pose), comprising the steps of:

a. identifying actual system parameters of said robot, which describe said mechanical linkage and joint actuators of said robot, by:

i. employing a plurality of calibration pose command signals which, based upon nominal system parameters of said robot, would place said robot in a plurality of desired calibration poses, to place said robot in a plurality of corresponding, actual calibration poses, each of which desired and actual calibration poses is within the measuring range of at least one pose sensor;

ii. measuring said actual calibration poses of said robot by operation of said pose sensors;

iii. calculating the differences between said plurality of said desired calibration poses and said corresponding measured actual calibration poses;

iv. solving a set of equations which relate said differences to said actual system parameters of said robot to obtain said actual system parameters of said robot;

b. predicting, through utilization of said actual system parameters and said original command signal, a predicted anticipated pose of said robot which would actually be obtained as a result of said original command signals;

c. calculating, as a function of the difference between said desired pose and said predicted anticipated pose, corrected command signals required to minimize said difference between said desired, pose and said predicted anticipated pose; and

d. employing said corrected command signals to place said robot in said desired pose.

5. A method of claim 4 wherein said step of identifying the actual system parameters includes the substep of positioning a plurality of said pose sensors and said robot adjacent to each other.

6. A method of claim 5 wherein said step of identifying the actual system parameters includes the substep of locating said robot in a plurality of desired calibration poses, with each of said desired calibration poses falling within the measuring range of at least one of said plurality of pose sensors.

7. A method of claim 4 wherein said substep of solving said set of equations includes the sub-substeps of:

A. employing said set of equations to determine pairs, if any, of said actual system parameters which cause said set of equations to be singular;

replacing selective ones of said actual system parameters in said set of equations with new actual system parameters which are each the sum of a respective one of said pairs;

C. partially solving said set of equations to thereby obtain said actual system parameters, except for said pairs, and to thereby obtain said sums of said pairs.

8. A method of claim 7 wherein said substep of solving said set of equations includes the further sub-substeps of:

D. employing previously solved sets of said equations for actual system parameters and related sums of said pairs to determine said sets of equations which remain singular; and

E. repeating said substep of solving said set of equations using previously obtained actual system parameters and said pairs and thereafter solving said set of equations to obtain all of said actual system parameters at times when said equations become non-singular.

9. A method of claim 4 wherein said robot remains within the measurement range of said pose sensors during said step of employing said corrected command signals to place said robot, and wherein said pose sensors during said step of employing provide additional measurement of the difference between the actual pose of said robot and the desired pose of said robot in the form of additional difference signals, and wherein said step of calculating corrected command signals required to minimize the difference between said predicted anticipated pose and said desired pose includes utilization of said additional difference signals.

10. A method of claim 4 wherein said actual system parameters include parameters which locate placement of said robot relative to said at least one pose sensor.

11. A method of claim 1, 4 or 5 wherein said step of identifying the actual system parameters includes locating said robot at a plurality of desired calibration poses as a function of nominal system parameters of said robot.

12. A method of claim 1 or 4 wherein said step of calculating corrected command signals includes employing an actual inverse Jacobian of said robot, while using the difference between said desired pose and/said predicted anticipated pose.

13. A method of claim 1 or 4 wherein said step of identifying the actual system parameters includes use of a least squares technique.

14. A method of claim 13 wherein said least squares technique comprises the substeps of:

i. employing a plurality of calibration pose command signals which, based upon nominal system parameters of said robot, would place said robot in a plurality of desired calibration poses, to place said robot in a plurality of corresponding actual calibration poses, each of which desired and actual calibration poses is within the measuring range of at least one pose sensor;

ii. measuring said actual calibration poses of said robot by operation of said pose sensors;

iii. calculating differences between said plurality of said desired calibration poses and said corresponding measured actual calibration poses and summing the squares of said differences over all said poses to obtain resultant sums;

iv. solving a set of equations which relate said resultant sums to said actual system parameters of said robot to obtain said actual system parameters of said robot.

15. The method of claim 14 wherein said substep of solving said set of equations includes the sub-substeps of:

A. employing said set of equations to determine pairs, if any, of said actual system parameters which cause said set of equations to be singular;

replacing selective ones of said actual system parameters in said set of equations with new actual systems parameters which are each the sum of a respective one of said pairs; and

C. partially solving said set of equations to thereby obtain said actual system parameters, except for said pairs, and to thereby obtain said sums of said pairs.

16. A method of claim 15 wherein said substep of solving said set of equations includes the further sub-substeps of:

D. employing previously solved sets of equations for actual system parameters and related sums of said pairs to determine said sets of equations which remain singular; and

E. repeating said substep of solving said set of equations using previously obtained actual system parameters and said pairs and thereafter solving said set of equations to obtain all of said actual system parameters at times when said equations become non-singular.

17. A method of claims 1 or 4 wherein said step of calculating corrected command signals comprises calculating corrections required of said original command signals and combining said corrections and said original command signals to obtain said corrected command signals.

18. A method of claim 1 or 4 wherein said step of employing includes converting said corrected command signals to corresponding pair variable signals without any alteration of said step of converting based upon said actual system parameters.