Control System for Robot Manipulator

The present invention relates to a control system for a controlled body, such as a robot manipulator.

Laid-open Japanese patent application No. 58-169203 discloses a control system for a robot where the optimum feedback gain values are stored, each for position assumed by a robot and load applied to the robot. In this conventional control system, a feedback gain provided when the load is applied is different from a feedback gain provided when no load is applied to the robot even with the same position assumed by the robot.

If this conventional control system is implemented in a controller for a multi-articulation robot manipulator using an electric motor as an actuator for driving each of the articulations, since the feedback gain is selected from the initially set fixed stored values even if there occur variation in friction at each of the articulations which is caused by environment temperature/humidity and aging of the mechanical parts, a problem arises that the motor is controlled with the motor driving electric current saturated or with most of the motor power potential left unconsumed, failing to assure the complete stability in motion and failing to accomplish speed-up of the motion.

According to the present invention, there is provided a control system for a controlled body whose motion is controlled by a motor wherein the control characteristic of the controlled body is variable in response to a feedback gain. According to the control system, a value indicative of an electric current used for driving the motor is detected and compared with a predetermined level band based on the saturated level of the electric current, and the feedback gain is adjusted so that the detected value reaches within the predetermined level band.

An object of the present invention is to provide a control system which assures stable and high speed motion of a controlled body.

FIG. 1 is a block diagram showing a control system for a manipulator of a robot according to the present invention;

FIG. 2a is a timing diagram showing a change in an input signal, viz., position indicative signal;

FIG. 2b is a timing diagram showing a pattern in which an articulation angle is to vary in response to the change shown in FIG. 2a;

FIG. 2c is a timing diagram showing a pattern in which the ariculation angular velocity is to vary in response to the change shown in FIG. 2a;

FIG. 3 is a timing diagram showing cycle-by-cycle variation in electric motor current;

FIG. 4 is a flowchart; and

FIG. 5 is a computer based control system for a manipulator of a robot according to the present invention.

Referring to FIG. 1, one embodiment of a control system for an articulated manipulator of a robot according to the present invention is shown in a block diagram.

Reference numeral 1 designates a desired pattern generator which, when a manipulator 9 is to change from an initial state to a final state, generates an articulation angle .theta.r and an articulation angular velocity .theta.r which vary with time t in a predetermined desired pattern immediately after the occurrence of a change in an input representative of the state to be assumed by the manipulator 9. The input r, .theta.r and .theta.r are matrices.

Reference numeral 2 designates a summation point where a difference is given between the desired ariculation angle .theta.r generated by the desired pattern generator 1 and an actual articulation angle .theta. which is an output of an integrator 12 where an output of a tachogenerator 11 attached to the manipulator 9 is integrated. This difference is referred to as a positional error e. The positional error e is applied as an input to an integrator 3 for integration.

The output of the integrator 3 is multiplied with a feedback gain K.sub.1 at a gain providing block 5. The output of the gain providing block 5 is applied to a summation point 7.

Reference numeral 4 is a summation point where there is given a difference between the desired articulation angular velocity .theta.r and an actual articulation angular velocity .theta. which is an output of the tachogenerator 11. This difference is multiplied with a feedback gain K.sub.2 at a gain providing block 6. The output of the gain providing block 6 is applied to the summation point 7.

At the summation point 7, the outputs of the both of the gain providing blocks 5 and 6 are added and the output thereof indicative of the summation is applied as an servo input u to a compensator 8.

The feedback gains K.sub.1 and K.sub.2 are variable by a gain adjustor 14 where the feedback gains K.sub.1 and K.sub.2 are increased or decreased in response to comparison result of a motor electric current used to drive each of the articulations of the manipulator 9 such that the maximum level of the motor electric current falls in a predetermined band defined between the saturated electric current level I.sub.max of the motor and I.sub.MU. The band has predetermined maximum and minimum levels I.sub.max and I.sub.MU.

Reference numeral 10 designates a motor serving as an actuator for each of the articulations of the manipulator 9. Reference numeral 13 designates a detector which detects the intensity of electric current passing though each motor 10.

The compensator 8 receives inputs u, .theta., .theta. and performs arithmetic operation which will be discussed hereinafter to output a torque command .tau.. This torque command .tau. is applied as a compensator control input to the manipulator 9.

The system including the multi-articulation manipulator 9 can be expressed by the following equation:

where

.theta.=[.theta..sub.1, . . . .theta..sub.n ].sup.t (.theta..sub.i, i=1 . . . n: the articulation angle of each articulation, n: the number of articulations)

.tau.=[.tau..sub.1, . . . .tau..sub.n ].sup.t (.tau..sub.i, i=1 . . . n: the driving torque applied to each articulation)

J(.theta.): the inertial moment matrix

f(.theta., .theta.); the term of Coliolis, centrifugal force

V(.theta.): the term of the viscous friction of each joint

g(.theta.): the term of the gravity.

As will be readily understood from the equation (1), this system is a coupled non-linear dynamic system which requires elimination of non-linear property and coupling property among the articulations in order to realize feedback control so as to provide high speed high precision motion control of the manipulator 9.

If the driving torque .tau. is expressed by equation (2), the system becomes generally linear as expressed by equation (3), so that the feedback control turns out to be more effective.

where, F.sub.1, F.sub.2, G are constant matrices (more particularly, F.sub.1 and F.sub.2 are diagonal matrices for decoupling) and u is the servo input (see FIG. 1).

The compensator 8 has stored therein various data regarding the manipulator 9, viz., the position of center of gravity of each link expressed in terms of a coordinate system fixed to each articulation, the weight of each link, the inertial moment, the coordinate system coefficient, the commutator matrix between the link's coordinate systems, the gains F.sub.1, F.sub.2, and G. Inputs to the compensator 8 are articulation angle .theta., ariculation angular velocity .theta., and servo input u. Based on these inputs, the compensator 8 calculates the equation (2) by using Luh's algorithm, viz., an algorithm which allows high speed calculation of an equation of the type shown in the equation (1). The result of the calculation of the equation (2) is output as the torque command .tau. and applied to the motor 10.

With the compensator 8, the system becomes generally linear and the axes of the articulations are now allowed to be considered as being independent from each other. This allows independent servo control of each of the axes. Thus, the following description relates with the operation with regard to one axis only.

The desired pattern generator 1 generates a desired articulation angle .theta.r and a desired articulation angular velocity .theta.r in response to a change in articulation state r as shown in FIG. 2a. The desired articulation angle .theta.r and angular velocity .theta.r vary against time t as shown in FIGS. 2b and 2c, respectively. The servo control system performs control in such a manner as to follow up the variation of these variables.

The generator patterns of the articulation angle .theta.r and angular velocity .theta.r may be determined at the discretion of a designer. For example, they can be given in the form of a transfer function as follows: ##EQU1##

It is possible to determine the servo control system such that its criterion function becomes minimum. The criterion function is expressed by the following equation: ##EQU2##

Finding u which minimize the criterion function (the process of finding the solution is taught by the book entitled "LINEAR SYSTEM CONTROL THEORY" by Katsuhisa YOSHIDA, issued by SHOKO DO), ##EQU3## The above equation is integrated to give ##EQU4## Since K.sub.2 satisfies a well known relation with K.sub.3, viz., K.sub.2 =-K.sub.3, ##EQU5## Therefore, the servo control system may be constructed as shown in FIG. 1.

The feedback gains K.sub.1 and K.sub.2 given at the gain multipliers 5 and 6 may be chosen such that the criterion function as expressed by the equation (5) is minimized, and the adjustment parameter .lambda. may be determined at the discretion of the designer taking into account the capacity of the motor 10 such that the motor electric current passing through the motor 10 will not be saturated.

Although the detailed solution to equation 8 is omitted, the feedback gains which minimizes the equation (5) are given by:

where A.sub.1, A.sub.2, C.sub.1 and C.sub.2 are constants determined from F.sub.1, F.sub.2, G of the equation (3) and a.sub.1, a.sub.2, and b.sub.1 of the equation (4).

As will be understood from the above equation (9), an increase in .lambda. causes a decrease in each of the gains K.sub.1,K.sub.2. When the gains are decreased, the electric current used to drive the motor 10 drops. When the gains are increased, the electric current used to drive the motor 10 rises.

In the gain adjuster 14, the electric current i passing through the motor 10 as detected by the electric current detector 13 is monitored and compared with a predetermined level band that is set based on the saturated electric current level I.sub.max (viz., a level band defined between I.sub.max and I.sub.MU) and a decision is made to determine whether or not the electric current was during one cycle of operation performed (viz., whether or not the maximum value .vertline.i.sub.max .vertline. of the motor electric current has deviated from the level band (see step S1 shown in FIG. 4). If the electric current is saturated, the value .lambda. set during the preceding cycle is increased by a predetermined small value .DELTA..lambda. (see step S2), and thus the feedback gains K.sub.1 and K.sub.2 are decreased (see step S5), thereby to prevent the electric current from being saturated during the operation in the next cycle. If the motor electric current stays below the level band, the value .lambda. is decreased by the predetermined small value .DELTA..lambda. (see step S3) and thus the feedback gains K.sub.1 and K.sub.2 are increased (see step S5), thereby to cause the maximum value .vertline.i.sub.max .vertline. of the motor electric current to reach the level band. If the maximum value .vertline.i.sub.max .vertline. stays in the level band, the value .lambda. is unmodified (step S4), and thus the feedback gains K.sub.1 and K.sub.2 are unmodified either (see step S5).

The execution of the flow of jobs as shown in FIG. 4 is repeated after completion of each operation so that the maximum value .vertline.i.sub.max .vertline. of the motor electric current used in each operation is so adjusted that it always stays near the saturated level.

The above control system may be implemented by a discrete circuit or by a microcomputer-based system.

Referring to FIG. 5, the microcomputer based control system is shown. Reference numeral 20 designates a microcomputer processor unit MPU whose power source and oscillator circuit have been omitted. Reference numeral 21 designates a D/A converter, reference numeral 22 designates an electric current amplifier. Via the converter 21 and the amplifier 22, a DC motor 10 is driven. Reference numeral 23 designates a pulse generator with which the articulation angular velocity is detected. Reference numeral 24 designates a counter with which the articulation angle is detected. Reference numeral 25 designates an electric current detecting resistor and reference numeral 26 an A/D converter. With the resistor 25 and A/D converter 26, the electric current passing through the motor 10 is detected. The microcomputer 20 performs all of the functions described hereinbelow.

It will now be understood that with the above discussed control system, the variation in control performance which is caused by the variation in friction at each of the articulations of the manipulator, by the aging of the mechanical parts and by the variation in motion of the motor is suppressed, and thus the maximum performance of the motor power is fully used. Thus, stable and high speed motion of the manipulator have been assured.