JPH01171004A - Control device for robot - Google Patents

Abstract

PURPOSE:To compensate a mutual interference, to eliminate the need for a torque sensor and to improve a control accuracy by operating a compensation torque to compensate a torque due to the mutual interference between plural freedom degrees of a robot arm, giving a control input value to add the position compensation value in accordance with the compensation torque to a driving control system and compensating an orbit. CONSTITUTION:The title device is provided with a coordinate converter 1 to convert the target position of the end effector of a robot to a control target value at the local coordinate system of respective freedom degrees, a compensation torque computing element 2 to execute the interference compensation, a compensation torque converter 3 and a command position converter 4 to form a reference control input value deriving means to obtain the reference position command for the driving control of an arm. A position command value thetaref,i and a position compensation value DELTAthetaref,i are added by an adder 5 given to an arm position control system 10, and then, the forecasting control based on the motion equation of a robot arm is executed. Consequently, while the mutual interference of plural freedom degrees is compensated, the position control of respective servo motors 20 can be executed. For the arm position control system 10 used at such a time, an ordinary position control system can be used. Thus, the compensation of the mutual interference is executed, the torque sensor is made unnecessary and the control accuracy is improved.

Description

DETAILED DESCRIPTION OF THE INVENTION (Field of Industrial Application) The present invention relates to a robot control device that controls the trajectory of a robot arm by predictive control.

(Prior art and its problems) FIG. 3 is a block diagram showing a conventional robot control device. In the figure, the arm position control system 10 is a position IIJ control system equipped with speed and position feedback for controlling the servo motor 20 that drives the robot arm, and shows the control system per degree of freedom. . The arm position control system 10 receives a position command value θ. f and position feedback value θ
an amplifier 12 that amplifies the output of the subtracter 11, and a sample hold circuit that holds the output of the amplifier 12 at regular intervals (rs/1- (abbreviated as J in the figure)). 13, a subtracter 14 that takes the difference between the output value V of the sample hold circuit 13 and the speed feedback value V, and the subtracter 14.
an amplifier 15 that amplifies the output of and provides a current command value I to the servo motor 20, and a current position value θ of the servo motor 20;
■The speed feedback value.

and a converter 16' for converting the current position value θ. and a converter 17 for converting the position feedback value θ into a position feedback value θ. Note that 2.2. and 3 indicate the respective proportional gains. In addition, the entire robot control device includes an arm position control system 10 shown in the figure for each of the servo motors 20 corresponding to each of the multiple degrees of freedom.
Give a position command value θ to each arm position control system 10.
In order to determine the target position of the end effector, the position (rotation angle) command value θ of each servo motor 20 is set.

It has a coordinate converter 1' for converting into . However, the target position X is a position on a predetermined target trajectory C.

The arm position control system 10 as described above is capable of sufficiently accurate position control when the robot has only one degree of freedom. However, if the robot has multiple degrees of freedom, the axes of each degree of freedom will interfere with each other, so simply giving each position command value θ to each arm position control system 10 will not move the end effector toward the target trajectory. It was impossible to precisely control along C. In particular, direct drive motors, which have been widely used in recent years, do not use reduction gears, and torque disturbances on the load side become disturbances to the motor.
There is a high need to compensate for motor torque disturbance due to mutual interference between multiple axes.

In order to compensate for such multi-axis interference, robot control devices that perform predictive control that takes into account equations of motion between multiple axes are conventionally known. FIG. 4 is a block diagram showing a robot control device that performs such predictive control. The predictive control system shown in the figure includes a coordinate converter 1'' that converts the end effector's target position In response, a torque command value τ to the servo motor 20 is given. A torque calculator 31 that calculates f, a torque detector 42 that detects the torque of the servo motor 20 and obtains a torque feedback value τ, and a subtraction that takes the difference between the torque command value τ and the torque feedback value τ. device 43 and subtractor 4
Torque command current value I corresponding to the output of 3.

It is equipped with a converter 44 (proportional gain Kt) for converting into . In this control device, the main control is performed by applying the torque command current value I to the servo motor 20, but an arm position control system similar to that shown in FIG. 3 is also used to perform position/velocity feedback. 10' is provided. The torque command current value It and the current command value I obtained by position/velocity feedback are calculated by the adder 45.
and is added to the servo motor 20. In a position control system based on such a torque command value τ, ef is the torque that should be given to each servo motor 20 by the torque calculator 41 based on an equation of motion that takes into account mutual interference force between multiple axes, centrifugal force, Coriolis, gravity, etc. Calculate the torque command value τ.

af However, the servo motor 2o has rotation ripple.

Motor core loss, PW built into the servo motor 20
There is significant nonlinearity due to the torque dead zone of the M control circuit. Therefore, in a control system in which the torque command current value (■1), which is a main command value, is input to the highly nonlinear servo motor 20, there is a problem that accurate control is difficult. Furthermore, it is difficult to obtain a highly accurate torque sensor that is necessary to obtain an accurate torque feedback value τ, and this poses a problem in that there is a limit to control accuracy.

(Purpose of the Invention) This invention was made to solve the above-mentioned problems, and it is possible to compensate for the mutual interference of multiple degrees of freedom of the robot arm, and to control the robot with high control accuracy without the need for a torque sensor. The purpose is to obtain equipment.

(Means for Achieving the Object) The robot control device of the present invention is provided with drive means for each of a plurality of degrees of freedom, and provides control input to the drive control means provided corresponding to the drive means. This device is configured as a device for controlling the trajectory of the end effector of a robot, and this device includes a reference control input value that should be given as the control input when mutual interference of the arms is ignored, and a desired value of the end effector. a reference control input value deriving means calculated based on the control target value of the arm calculated from the trajectory; and generalized compensation to be output by the driving means to compensate for the influence caused on the driving state of the driving means due to the interference. Generalized compensation force deriving means for calculating a force value based on the control target value;
control input compensation value deriving means for calculating a value of a control input compensation value to be given as the control input in order to output the generalized compensation force to the drive means; and synthesizing the reference control input value and the control input compensation value. and a synthesizing means for supplying the control input to the drive control means as the control input.

(Embodiment) A. Configuration of robot control device FIG. 1 is a block diagram showing a robot control device according to an embodiment of the present invention. In the figure, the arm position control system 10 is the same as the arm position control system 10 in the conventional robot control device shown in FIG. 3, but the servo motor 20 is shown as a detailed block. That is, the servo motor 20 includes a subtracter 21 that takes the difference between the current command value I and the current feedback value I, an amplifier 22 that amplifies the output of the subtractor 21 to obtain a voltage □ command value ■, and a voltage command value (2) and a voltage feedback value vf, and a winding 24 of a servo motor 20. And the current value of the servo motor 20 ■. is the current torque value τ in block 25. is converted to

Also, the current current value I. The torque generated by τ.

is subtracted by the torque disturbance Δτ in block 26, and the reduced torque (τ.−Δτ) is converted to the current speed value θ in block 27. is converted to Current speed value θ. is block 2
8 is the current position value θ. is converted into. The servo motor 20 also includes a converter 29 that converts the current current value 1c into a current feedback value (2), and converts the electromotive force based on the current speed value θ into a voltage feedback value V.

A converter 30 is provided. Moreover, the above amplifier 22. Block 25. The transducers 29, 30 have proportional gains K·, , , 1, respectively. It has KB. Further, the symbol of the winding 24 in the figure indicates the winding resistance R and the winding inductance L, and the symbol of the block 27 indicates the inertia J of the motor.

The other configuration of the arm position control system 10 is as described above,
Subtractor 11. Amplifier 12. Sample hold circuit 13.
Subtractor 14. Amplifier 15. A converter 16.17 is provided, as well as an amplifier 12.15 and a converter 17.18.
have proportional gains K , K , and K , respectively, of 3.

V2 Furthermore, in the robot control II device according to this embodiment, the target position X of the end effector of the robot is set to the target position nθi in the local coordinate system of each degree of freedom.

These are collectively called the "control target value." ). This control target value θ.

Of θ and θ, the value of the target position θ is given to the command position converter (arm motor amplifier inverse characteristic filter) 4. This command position converter 4 ignores the mutual interference of multiple degrees of freedom (arms) and calculates the arm drive υ from the value of the target position θ.
It forms a reference control input value deriving means for obtaining a reference control input value for control (specifically, a reference position command value θ,).

The apparatus shown in FIG. 1 is also provided with a compensation torque calculator 2 and a compensation torque converter 7 for interference compensation. Among these, the compensation torque calculator 2 converts the value of the compensation torque Δτ (generalized compensation force) for compensating the influence caused on the drive state of the servo motor 20 due to the interference into the control target values θ, θ, θ. A generalized compensating force deriving means is formed based on the above.

Furthermore, the compensation torque converter 3 calculates a position compensation value Δθ · (control input compensation ref, +1 value) for the servo motor 20 to additionally output a torque corresponding to the compensation torque Δτ. It forms an i value deriving means. Then, the reference position command value θ, and the position ref,
+ To the compensation value〇 means that the adder 5 (synthesizing means)
, + are caulked (synthesized) and given to the arm position control system 10 as a control input signal.

B. Equation of Motion of Robot Arm Next, an equation for determining the compensation torque Δτi from the control target values θ, θ, θ in the robot control device described above will be explained.

In general, the equation for a robot arm with multiple degrees of freedom is
It is expressed as follows using Lagranche's equation of motion.

...(1) Here, τi is the torque as the generalized force of freedom 1ii,
θ· and θ respectively indicate joint angles as generalized displacements in degrees of freedom j. Further, the robot has n degrees of freedom, and 1≦j≦n and 1≦≦n. Also, the dots on the symbols indicate time differentiation. More Hij, hijk
each has the degree of freedom i.

” or the coefficient 0, which is determined for each combination of i and j, indicates a gravitational term.

Note that the first term on the right side of equation (1) is a term representing inertial force, the second term is a term representing centrifugal force and Coriolis, and the third term is a term representing gravity.

The torque τi based on this equation of motion is divided into an inertial force τ1 of only the self of the degree of freedom i and another interference force (hereinafter referred to as “compensation torque J”) Δτ·.

＝τ・・＋Δτ, ...(2) Therefore, τ・・=H・・θ・ ...(3
)+1111 In addition to the self-inertial force τ... and the compensation torque Δτ1, the self-inertial force τii is realized by ref, + by giving the reference position command value θ to the arm position control system 10. . However, the reference position command value θ, and re(
The value to be given as + needs to be determined in consideration of the transfer function G(S) of the arm position control system 10.

That is, the input to the arm position control system 10 is only the reference position command value θ, and the output is the current position raf, + value θ. When yelling, the following relationship holds true between the two.

θ −G(S)θ,. fj...(5
) by transforming Nire to θ, = 00...(6)
ref・' G(S) Therefore, in order to match the current position value θ with the target position θ・ while ignoring interference, θ0=θ, and θ calculated by equation (6), is set as arm +
ref, + must be given to the position control system 10. Therefore, in the embodiment shown in FIG. 1, the target position θ, determined by the coordinate converter 1, is converted to
The position command value θ is obtained by inputting it to the arm motor amplifier inverse characteristic filter) 4.

ref, + On the other hand, the compensation torque Δτi is determined by the compensation torque calculator 2 based on the above equation (4). Also, this is transmitted from the amplifier 12 inside the arm position control system 10 to the block 25.
The position compensation value Δθ is converted into a position compensation value Δθ· based on the transfer characteristics up to this point, and is provided to the arm position control system 10. That is, the compensation torque Δτi as a disturbance to the shaft torque of the servo motor 20 is the position compensation value Δθref as a disturbance to the position command value θ in the arm position control system 10.

ref,+ It can be simply converted into , and is expressed by the following formula.

Based on the above equation (7), the compensation torque Δτi is set to the converter 3.
Convert to position supplementary value by .

re(+ Reference position command value θ obtained as above, 7.
1 and the position compensation value Δθ · by adding ref, + by the shearer 5 and giving it to the arm position control system 10,
Predictive control can be performed in consideration of the equation of motion expressed by equation (1) above.

First in a C0 robot with two horizontal degrees of freedom Next, a case will be described in which the above predictive control is applied to a robot with two horizontal degrees of freedom.

FIG. 2 is a diagram showing the mechanism of a robot arm of a horizontal two-degree-of-freedom robot. In the figure, the first and second arms A1
．． A2 is the motor M1. M2, and its position is defined by the rotation angles θ and θ. Further, an end effector E is provided at the tip of the second arm A2.

Note that the first and second arms A1. A2 is quality ff
i m 1 , m 2 and effective length l , 12, and the respective distances from the respective centers of motors M, M, 2 to the centers of gravity of each arm A, A2 are S , S
Assume that 2.

The Lagranche equation of motion for the parts driven by the first and second motors M and M2, respectively, can be expressed as follows using equation (1).

"1"H1tθ1+H12θ2 -2m I S sinθ ・θ θ・θ ・
...(8) 2nd j! 182 Sinθ22 τ =Hθ +H22θ2 4m I S sinθ CO1-(9)21
2 Matadashi H11=m18″1 4m <1′″+S+211S2CO5θ2)+1
1+ IH1+ 12...(10
)H=m (S +j!182 CO3θ, 2)
+-12...(11) H22= m 2 S 2 + I 2
・= (12) Note that ■ and ■ are the first and second 7-m A1. The moment of inertia of A2 with respect to their center of gravity, I, is the inertia of motor M1, and the inertia of the other motor M2 is included in second and I2. Note that since this robot has two horizontal degrees of freedom, the gravitational term in the third term of equation (1) does not exist.

The torques τ and τ shown in equations (8) and (9) are respectively replaced by the self-only inertia forces τ and τ and the compensation torque Δτ1.
Divide into Δτ2. First, the portion driven by the first motor M1 is as follows.

'r: 1=r1suΔτ1...(1
3a)" 11=H11θ1...
・(13b)Δτ =H12θ2-2m I S sinθ ・θ θ
-m 1 3 stnθ ・θ...(13
c) Note that the compensation torque Δτ1 may be given by equation (13c) including the variation in the second term of equation (10). That is, Δτ′=Δτ+2m I S cosθ ・θ・
...(13C') may be used as the compensation torque; in this case, (13b)
The above variation is removed from the coefficient H11 of the equation. In other words,
At this time, static interference (i.e., the second arm A
The shadow Y on the first motor M1 due to the existence of
1) and dynamic interference (the influence on the first motor M1 due to the relative movement of the first and second arms A and A2) are included in the compensation torque Δτ'1. . On the other hand, the compensation torque Δτ1 in equation (13c) includes only dynamic interference. In other words, it is not necessary to separate all interferences in this invention.

The parts driven by the second motor M2 are as follows.

τ2=τ22+Δτ2...(14
a)...(14c) Each arm A shown in the above formulas (13b) and (i4b)
, A's own inertial force, and τ are obtained by converting each plane angle θ1゜θ2 using the inverse characteristic of the arm position control system 10 according to the above equation (6), and calculating the reference position command value θ. It is sufficient to give the arm position ref, + to the position control system 10. That is, θ “=□
θ・・・・(15)ゝ””G(S)
' However, i=1.2.

Further, the transfer function G(S) of the arm position control system 10 is expressed by the following equation.

G (S)-to, KvK, to, / (K, KvK, to, to 3 + (KVKi K, to 2 + to 8,) S+ (to R+, to 1) JS + JLS )... (16 ) Especially when performing digital control, equation (11) can be changed to Z
The converted transfer function G(Z,) can be expressed as follows.

...(11) Here, mo and do-d3 are constants. Therefore, it can be expressed as Here, ri is a constant.

Substituting equation (18) into equation (15) yields the following equation.

This is expressed as the sample value θ・(n) at time n.■ In other words, the following formula is obtained.

θref, i (n)= , ;oo, e, (n-1>
...(20) Therefore, if the position target value θ is converted into the reference position command value θ by the converter 4 based on equation (20), then l
ref, + It is possible to obtain a reference position command value θ corresponding to the inertial force of only the self in each axis.

ref, + On the other hand, the position compensation value Δθ, corresponding to the compensation torque Δτ1.

(To find i, first use the compensation torque calculator 2 (1
Compensation torque Δτi of each motor M, M2 is calculated based on equation (3c) or equation (14c). Next, substitute this into equation (7) to obtain the position compensation value Δθ. All you have to do is find f and t.

In particular, when performing digital control, equation (7) can be changed to Z
Convert to obtain the following equation.

=1/K (1+NZ-1)・Δθ ref,i ...(21) Here, ■ is a sampling period, and K and N are constants.

If this is expressed as the sample value Δτ, (n) at time n, the following equation is obtained.

Δθ ・ (n) ref, + = 1 Δr−(n) 10KNΔr・ (n−1)...(
22) Therefore, based on equation (22), converter 3 converts the compensation torque Δτ into a position compensation value Δθ ・+
ref, +, it is possible to obtain the position compensation value Δθ · corresponding to the interference force in each axis.

ref, + The position command value θ, obtained as described above. ,.

, and 〇 to the position compensation value are added by adder 5 +
By applying ref, + to the arm position control system 10, predictive control based on the equation of motion (formula (1)) of the robot arm can be performed. Therefore, since the position of each servo motor 20 can be controlled while compensating for mutual interference of multiple degrees of freedom, the trajectory of the end effector E can be controlled with high precision. Also,
The arm position control system 10 used at this time can be a normal position control system, which has the advantage of not requiring a precise torque sensor.

D. Modifications The present invention is not limited to the above embodiments, and the following modifications are also possible.

■In the above embodiment, the arm position control system 10 has a reference position command value θ and a position compensation value Δθ ref, +
Although it is supposed to be input by adding ref, + position compensation value Δθ,. (Since the contribution of i is smaller than the reference position command value θ ・raf,+, input the reference position command value θ ・ref
, + compared to the frequency (for example, once every 1 μs) of the position compensation value Δ
The frequency of inputting θ· may be low (for example, once every 3 to 4 μs).

■In robots that use a direct drive motor as the servo motor 20, the interference force between multiple degrees of freedom tends to affect the torque of the motor, so this invention is particularly effective; however, it is not limited to direct drive motors, and other types It is also applicable to robots that use a motor or a linear actuator as a driving means.

(2) Although the above embodiment shows an example in which the present invention is applied to a robot having two horizontal degrees of freedom, it goes without saying that the present invention is not limited to this and can be applied to any other robot having multiple degrees of freedom.

(Effects of the Invention) As explained above, according to the present invention, a compensation torque is calculated to compensate for torque due to mutual interference between multiple degrees of freedom of a robot arm, and control is performed in which a position compensation value corresponding to the compensation torque is added. Since the input value is given to the drive control system to compensate for the trajectory of the end effector, mutual interference of multiple degrees of freedom can be compensated for, a torque sensor is not required, and control accuracy is high.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing an embodiment of the present invention, FIG. 2 is a diagram showing the configuration of a robot arm to which an embodiment of the present invention is applied, and FIGS. 3 and 4 are conventional robot control lll. It is a figure showing an apparatus. DESCRIPTION OF SYMBOLS 1... Coordinate converter, 2... Compensation torque calculator, 3... Compensation torque converter, 4... Command position converter 5... Adder, 10... Arm position control system, 20... Servo motor

Claims (1)

[Claims] (1) Drive control of a drive means provided for each degree of freedom of an arm of a robot having multiple degrees of freedom by giving a control input to a drive control means provided corresponding to the drive means. , a control device for a robot that controls the trajectory of an end effector of the robot, wherein a reference control input value that should be given as the control input when mutual interference of the arms is ignored is set to a desired value of the end effector. a reference control input value deriving means calculated based on the control target value of the arm calculated from the trajectory; and a general control input value deriving means that the driving means should output in order to compensate for the influence caused on the driving state of the driving means due to the interference. generalized compensating force deriving means for determining a value of a generalized compensating force based on the control target value; and determining a value of a control input compensation value to be given as the control input in order to cause the driving means to output the generalized compensating force. A robot control device comprising: a control input compensation value deriving means; and a composition means for synthesizing the reference control input value and the control input compensation value and supplying the synthesized result to the drive control means as the control input.