JP2000052286A - Robot control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a collision detection device capable of detecting a collision with high sensivity even in a case of a robot arm and the like, in which the stiffness of a speed reducer is low. SOLUTION: In a robot control device driving its joint using each servo motor 2 by way of each speed reducer 7, the device is equipped with a control means 1 controlling each servo motor 2, a condition estimation observer operation means 4 estimating disturbance force acting on a robot arm 3 side from the side of each speed reducer 7 by using a model in which the spring element of the speed reducer is taken into consideration, based on a torque commanding value to the servo motor 2 within the control means 1 and the position of the servo motor 2, a collision judgement means 5 judging the occurrence of a collision against an external environment by monitoring the estimation value computed by the condition estimation observer operation means 4, and with a device protecting means 6 making change-over for the operation condition of the servo motor 2 forcibly in such a way that the operation of the robot arm 3 will not be continued as is when a collision is found to have occurred.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot using a servomotor as a driving source, and detects that a jig such as a tool or a hand loaded on the robot body or the robot collides with another object such as a work. And a robot controller for minimizing damage.

[0002]

2. Description of the Related Art Conventionally, protection when a robot body, a tool, a jig or the like collides with another object is performed by a method as shown in FIG. The configuration will be described below. In FIG. 5, reference numeral 51 denotes control means for controlling a servomotor;
52 is a servomotor for driving the robot arm, 53 is a load applied to the servomotor 52 (mainly, the sum of the inertia of the motor and the inertia of the robot arm), and 54 is a spring of a reduction gear for estimating a disturbance force applied to the arm. Rigid system disturbance estimation observer calculating means 55 that does not take into account elements, 55 is a collision determining means that monitors the disturbance estimated value calculated by the rigid system disturbance estimating observer calculating means 54 to determine whether a collision has occurred, and 56 is a device protection means. Then, when it is determined that a collision has occurred, the servo motor 52 that drives the control target is stopped immediately to prevent damage to the device.

[0003] Among them, the rigid system disturbance estimation observer operation means 54 has performed the following operation. The following configurations are disclosed in JP-A-6-292379, "Method of changing torque limit at abnormal load", JP-A-6-245561, "Method of detecting and controlling abnormal load of servo motor", and
No. 131050, "Method of detecting collision of movable part driven by servomotor", and Japanese Patent Application Laid-Open No. 3-196313, "Method of detecting collision by disturbance observer". In FIG. 6, the solid line indicates the actual machine control unit 61. A portion within a broken line is a block diagram of a control target model (hereinafter, referred to as an observer model) 62, which is a model corresponding to a mechanical configuration of an actual machine. As described above, the portion corresponding to the mechanical configuration of the actual machine is configured with only the inertia J, which is the sum of the inertia of the motor and the inertia of the robot arm, without considering the spring element of the speed reducer.

The state equation of the model is as shown in equation (1). θ represents a motor position, Kt represents a torque constant, J represents inertia, and TL represents a disturbance torque. In the figure, S represents a Laplace operator, which means differentiation.

(Equation 1) The “•” on each variable represents the first time derivative of that variable, and “‥”
Represents the second time derivative. From this equation (1),
By the general method of assembling observers,

(Equation 2) When the same-dimensional observer for estimating the disturbance TL is configured,
An observer 73 shown by a dashed line in FIG. K3, K
4 is a parameter of the disturbance estimation observer 73, and an estimated value is obtained by selecting these parameters so that the observer system is stable.

[0005]

However, in the prior art, the spring element of the speed reducer between the servomotor 52 and the robot arm is not considered as a model of the observer. Even the torsional torque between the robot arms was estimated as a disturbance applied to the arms. As a result, the set value for detection cannot be made smaller than the torsional torque during operation, and there is a problem that the collision sensitivity cannot be increased with a robot arm or the like having a low rigidity of the speed reducer.

FIG. 9 shows a result of a simulation of a disturbance estimation value. FIG. 9A is a diagram of a speed command to the motor, and FIG. 9B is a diagram of an actual disturbance torque value (5.2 N · m) applied on the step and an observer disturbance estimated value. . As is apparent from this figure, the torsion torque between the motor and the robot arm during acceleration / deceleration appears in the disturbance estimation value. This allows, for example,
Even when the collision sensitivity set value is set to 5.3 N · m, in the figure, the portion A in the frame ○ exceeds the set value,
Although the actual disturbance is 5.2 N · m, it is erroneously detected as a collision. That is, when a conventional rigid disturbance observer is used,
There is a problem that the collision sensitivity can be set only to a value larger than the torsional torque. Therefore, an object of the present invention is to provide a collision detection device that can detect a robot arm with low rigidity of a speed reducer with high sensitivity.

[0007]

To achieve the above object, a robot controller according to the present invention is a robot controller for driving a joint via a speed reducer using a servomotor. A disturbance acting on the robot arm side from the speed reducer based on the torque command value to the servo motor inside the control means and the position of the servo motor, using a control means for performing the control, and a model considering a spring element of the speed reducer. State estimation observer calculating means for estimating the force, collision determining means for determining the occurrence of a collision with the external environment by monitoring the disturbance force estimated value calculated by the state estimating observer calculating means, and when determining a collision, Device protection means for forcibly switching the motion state of the servomotor so that the operation of the robot arm does not continue as it is; and A. As the device protection unit, after the collision is determined, the control unit forcibly sets the speed command to 0, sets the integral element of the speed loop to 0, and multiplies the feedback speed of the motor by the gain to obtain the motor torque command. Value. Alternatively, the device protection means includes means for applying a torque command value in a direction opposite to the operation direction for a certain period of time after the collision determination.

[0008]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram illustrating a basic configuration of the present invention. In FIG. 1, 1 is a servo motor control means, 2 is a servo motor, 3 is a robot arm to be controlled, 4 is a 2 inertia system state estimating observer calculating section in consideration of a spring element of a speed reducer, and 5 is a collision judging section. , 6 is a device protection processing unit, and 7 is a reduction gear between the motor and the arm. A block diagram of an observer model for realizing the present embodiment is shown by a broken line in FIG. The explanation of the symbols used in the figures is shown below. Kp: Position loop gain Kv: Speed loop gain Ki: Speed loop integral gain Kt: Motor torque constant S: Laplace operator (representing differentiation) 1 / S: Representing integration JL: Load inertia Jm: Motor inertia N: Deceleration Ratio K: Reduction gear spring constant C: Reduction gear viscosity coefficient

In FIG. 2, reference numeral 11 (solid line) denotes a servo motor 1
2 is a control unit for controlling the control unit 2. In the embodiment, the position control system is proportional control, and the speed control system is proportional integral control. FIG.
Reference numeral 12 denotes a servomotor. Reference numeral 13 (broken line portion) is a portion corresponding to a control target such as a motor, a speed reducer or a robot arm, and this is an observer model. In this, the spring constant of the speed reducer is modeled by K. Thus, a model including two inertia of the motor inertia Jm and the load inertia JL via the spring element is configured. This is called a two inertial model. The observer calculation unit 4 estimates the load-side disturbance force d2 acting on the robot arm using the two inertia model shown in FIG. less than,
Write the observer's relational expression.

[0010]

(Equation 3) Here, when an estimated value of a certain variable is expressed, the variable is indicated by adding ∧ to the variable. k is a natural number and represents the number of control cycles.
(K = 1, 2, 3,...) That is, the data next to the k-th data is the (k + 1) -th data.

(Equation 4) A variable with a dot “•” added to it is defined as the one-time derivative of that variable. [] ^T added after a matrix represents a transposed matrix of the matrix.

(Equation 5) d2: Load side disturbance force uref: Motor acceleration command Ts: Observer calculation cycle

[0011]

(Equation 6) MAT_B = [0 Ts 0 Ts / N 0] ^T (5) MAT_L = [L1 L2 L3 L4 L5] ^T (6) A23 = − (K * Ts) / (N * Jm)・ (7) A24 = − (C * Ts) / (N * Jm) (8) A43 = − (K / (N ² * Jm) + K / JL) * Ts (9) A44 = − (C / (N ² * Jm) + C / JL) * Ts + 1 (10) L1 to L5 are (MAT_A−MAT_L * [10]
[0 0 0]).
Equation (11) will be solved for MAT_L. Here, p1 to p5 are eigenvalues (poles) that can be set arbitrarily. | SI- (MAT_A-MAT_L * [100.000]) | = (s-p1) (s-p2) (s-p3) (s-p4) (s-p5) ... (11) By doing

(Equation 7) Can be estimated.

In the collision judging section 5 shown in FIG.

(Equation 8) By subtracting the calculated gravity and the calculated friction acting on each axis motor calculated in advance, only the true disturbance force is calculated.

(Equation 9) Mg: Calculated gravity value Fric: Calculated friction value When the true disturbance estimated value exceeds a certain set threshold limit, it is determined that the robot arm or tool to be controlled has collided with an external device, and the device protection processing unit Send a signal to 6.

FIG. 8 shows a waveform of an estimated value of disturbance estimated by this method. 8A is a diagram of a speed command to the motor, and FIG. 8B is a diagram of an actual disturbance torque value (5.2 N · m) applied on the step and an observer disturbance estimated value. . In the conventional disturbance estimation value of the rigid disturbance observer shown in FIG. 9, the torsional torque between the motor and the robot arm appears at the time of acceleration / deceleration. You can see that it is not. As described above, in this method, since an accurate actual machine model taking into account the spring constant of the speed reducer is used, no torsional torque appears in the disturbance estimation value, and only the disturbance torque to be detected is accurately estimated. . As a result, the collision sensitivity can be set to a value smaller than the torsional torque.
This makes it possible to detect a collision with high sensitivity even for a robot having a low rigidity or a robot having a large torsion torque.

FIG. 4 is a block diagram showing a configuration of a robot control device for carrying out the present invention. In the figure,
The robot controller 40 has a main processor board 41 which controls the whole. The processor board 41 has a processor 41a, a ROM 41b, a RAM 41c, and a nonvolatile memory 41d. The processor 41a is a ROM 41
According to b, the entire robot controller 40 is controlled. R
Various data are stored in the AM 41c. An operation program of the robot 100 and the like are loaded from the ROM 41b into the nonvolatile memory 41d. Processor board 41 is coupled to bus 47. Each calculation for the collision handling control shown in this embodiment is a software function calculated inside the digital servo control circuit 42. The digital servo control circuit 42 is connected to a bus 47, and in accordance with a command from the processor board 41,
Via the servo amplifier 43, the servo motors 401, 4
02, 403, 404, 405 and 406 are driven. These servo motors are built in the robot 100 and operate each axis of the robot 100. The serial port 44 is connected to a bus 47 and is connected to a teaching operation panel 48 and other RS232C devices 49. The teaching operation panel 48 is used for inputting teaching points to the robot. The large-capacity memory 45 stores teaching data and the like. In addition, input / output of data, signals, and the like with the outside via the I / O 46 is performed.

Hereinafter, the processing of the apparatus protection section processing section 6 of FIG. 1 will be specifically described. First, as a function of the device protection processing unit 6, after the collision determination, the control unit forcibly sets the speed command to 0, sets the integral element of the speed loop to 0, and multiplies the feedback speed of the motor by the gain. A first embodiment in which a torque command value of a motor is used will be described. FIG. 3 is a block diagram illustrating a control system after the device protection processing according to the first embodiment. Immediately after the collision determination, the device protection processing unit 6 of FIG. 1 forcibly sets the speed command to 0, sets the integral element of the speed loop to 0, and sets the gain K of a certain magnitude to the feedback speed of the motor. The value multiplied by 'is used as the motor torque command value. At this time, if the arm is subject to gravity due to the interruption of the speed integration calculation, the arm falls, so the gravity compensation value is added to the torque command value to the motor. The gravity compensation value is calculated from the posture and weight parameters of the robot arm, or is obtained in advance by measurement. As a result, the motor-generated torque acts in a damper manner in proportion to the arm speed to reduce the impact, and after the collision, the robot arm becomes flexible with respect to the disturbance force received at the time of the collision.

Next, as a device protection means, after a collision is determined,
A second embodiment in which a motor torque command value of a certain magnitude is applied in a direction opposite to the operation direction for a fixed time will be described. Until the collision determination unit 5 determines a collision, the processing is the same as in the first embodiment, and a description thereof will be omitted. In the second embodiment, the collision determination unit 5
Is determined to be a collision, the device protection processing unit 6 immediately issues a torque command in a direction opposite to the direction of the collision to the robot arm for a certain period of time. Thus, the relay is activated by the alarm, the electromagnetic brake is activated, and the robot arm can be stopped earlier than the time when the arm stops after coasting. Therefore, it is possible to prevent damage to the robot arm, the tool loaded on the distal end of the robot arm, and the work or peripheral device on the side of the collision due to the coasting of the robot arm at the time of the collision, thereby minimizing the damage. Further, since the arm does not stop in a state where the arm enters the inside of the collision side, which can be caused by the conventional stop by the brake, recovery after the collision is very easily performed.

[0017]

As described above, according to the present invention, the following effects can be obtained. (1) Since the observer model is configured in consideration of the spring element of the speed reducer, only a disturbance force acting on the arm side from the speed reducer required for collision detection is accurately estimated. , It is possible to detect a collision with high sensitivity. (2) After the collision is detected, whether a damper function is provided in the control system
Or, by issuing a torque command in the opposite direction, as compared with the conventional method in which a stop command is generated and the brake is actuated, the control target, the jig and the like loaded on the control target, and the work and the device on the collision side are compared. Damage can be minimized, and the method of issuing a reverse torque command simplifies recovery work after a collision.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a specific configuration example of the present invention.

FIG. 2 is a block diagram showing an observer model of the present invention.

FIG. 3 is a block diagram illustrating a configuration of a first exemplary embodiment of the present invention.

FIG. 4 is a block diagram illustrating a hardware configuration for realizing the present invention.

FIG. 5 is a block diagram showing a configuration of a conventional collision stop control device.

FIG. 6 is a block diagram showing a conventional observer model.

FIG. 7 is a diagram showing a conventional type observer.

FIG. 8 is a waveform diagram of an observer disturbance estimated value according to the present invention.

FIG. 9 is a waveform diagram of a conventional observer disturbance estimated value.

[Explanation of symbols]

1 Servo motor control means, 2 Servo motor, 3
Robot arm, 4 2 inertial system state estimation observer calculation unit, 5 collision determination unit, 6 device protection processing unit, 7 reduction gear,
11 control unit, 12 servo motor, 13 controlled object,
40 robot controller, 41 processor board, 4
1a processor, 41b ROM, 41c RAM,
41d nonvolatile memory, 42 digital servo control circuit, 43 servo amplifier, 44 serial port, 4
5 large-capacity memory, 46 I / O, 47 bus, 48
Teaching operation panel, 49 RS232C device, 401, 40
2,403,404,405,406 servo motor,
Reference Signs List 100 robot 51 control means, 52 servomotor, 53 load, 5
4 Rigid system disturbance estimation observer calculation means, 55 collision determination means, 56 device protection means, 61 actual machine control unit, 62
Controlled model, 73 disturbance estimation observer

 ────────────────────────────────────────────────── ─── Continued on the front page (72) Inventor Masao Ojima 2-1 Kurosaki Castle Stone, Yawatanishi-ku, Kitakyushu-shi, Fukuoka F-term (reference) 3F059 CA05 CA07 CA10 FB29 FC03 GA00 5H004 GA07 GA29 GB16 HA07 HB07 HB08 JB22 KA69 KB02 KB04 KB39 KC39 KC55 LB06

Claims (3)

[Claims] 1. A control device for a robot that drives a joint via a speed reducer using a servo motor, wherein the control device controls the servo motor, and a model considering a spring element of the speed reducer is used. State estimation observer calculating means for estimating a disturbance force acting on the robot arm side from the speed reducer based on a torque command value to the servo motor inside the control means and the position of the servo motor, and a state estimation observer calculating means for calculating the disturbance force. Collision judgment means for judging the occurrence of a collision with the external environment by monitoring the estimated disturbance force value, and forcing the motion state of the servo motor so that the operation of the robot arm does not continue as it is when the collision is judged. And a device protection unit for performing a process of switching to (a). 2. As the device protection means, after a collision determination,
2. The motor control device according to claim 1, wherein a speed command is forcibly set to zero, an integral element of the speed loop is set to zero, and a value obtained by multiplying a feedback speed of the motor by a gain is used as a motor torque command value. 2. The control device for a robot according to 1. 3. The device protection means according to claim 1, further comprising:
2. The robot control device according to claim 1, wherein a motor torque command value of a certain magnitude is applied in a direction opposite to the operation direction for a predetermined time.