JP2001353687A - Robot controller and method for controlling the same - Google Patents

Abstract

PROBLEM TO BE SOLVED: To solve the problem of a conventional robot collision discremination based on turbulence torque in which a high threshold value is set considering beforehand the maximum amount of gravitational torque and the inertial forces of other axes or the like, because a robot is influenced by gravitational force affected by the change of its attitude, to add the gravitational torque component to motor torque, or it is influenced by the inertial forces of the other axes when operating at high speeds, to increase turbulence torque. SOLUTION: A filter part for cutting the gravitational force component out of the turbulence torque calculated by a turbulence detector is provided downstream the turbulence detector. And a threshold changing means is provided for changing a threshold value for the discrimination of collision in accordance with the operating speed of the robot.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a collision determination process for robot control.

[0002]

2. Description of the Related Art Conventionally, a collision detection process is performed to prevent a mechanical part of an industrial robot driven by a servo motor from being damaged by a collision with an obstacle or a work.
Then, in the collision detection, a disturbance torque generated in the servomotor when the mechanism unit collides is estimated by a disturbance estimation observer, and when the estimated disturbance torque exceeds a set threshold value, it is determined that a collision has occurred. I do. However, since the disturbance torque generated when the robot collides includes the frictional force of the machine, the reaction force of the spring system, and the gravitational force applied to the machine, as described in JP-A-3-196313, the collision determination is performed. Is set to a value larger than the above-described frictional force, reaction force, and gravity and smaller than the withstand strength of the mechanism. FIG. 17 shows this configuration, which will be described below. First, the disturbance estimator 2 estimates the disturbance torque from the command torque calculated by the control means 1 and the actual motor speed. Next, the determining means 4 calculates the estimated disturbance torque,
A comparison is made with a preset threshold value. If the disturbance torque exceeds the threshold value, it is determined that a collision has occurred, and the protection processing unit 5 executes damage prevention processing.

[0003]

In the above-described conventional collision determination processing, the disturbance torque estimated by the disturbance estimator includes a frictional force generated by the movement of the movable portion, and a gravity force applied to the machine or a spring system such as a speed reducer. Therefore, it is difficult to accurately estimate only the disturbance torque generated by the collision. Therefore, the threshold value for collision determination cannot be set smaller than the level of disturbance torque other than collision. Also, since gravity, frictional force, and reaction force are constantly changing, it takes time to detect a collision depending on the posture and speed.

In addition, if the conventional estimated disturbance torque is merely determined as a threshold value, when a large inertia force due to the basic axis is applied to a mechanism such as a robot wrist axis, the inertia force is reduced to a disturbance torque due to a collision. In some cases, the judgment was made and an erroneous judgment of collision occurred.

[0005]

In order to achieve the above object, a first configuration of the present invention comprises a differentiating means for differentiating a disturbance torque calculated by a disturbance detector, and a differential signal and a threshold value. A determination means for performing a comparison determination is provided.

Further, a second configuration of the present invention is provided with threshold value changing means capable of changing a judgment threshold value of the wrist axis according to the operation speed of the robot basic axis.

Further, the third configuration of the present invention is provided with a filter section for cutting a gravitational component applied to the mechanism section from the disturbance torque calculated by the disturbance detector.

Further, in an eighth configuration of the present invention, there is provided higher-order determination means for determining a collision based on the output results from all the disturbance detectors in the control unit of each motor of the robots that cooperate with a plurality of robots.

According to a first method of the present invention, the calculated disturbance torque is differentiated, and the differentiated signal is compared with a threshold.

Further, according to the second method of the present invention, the judgment threshold value of the wrist axis is varied according to the operation speed of the robot basic axis, and the judgment threshold value is compared with the calculated disturbance torque.

Further, in the third method of the present invention, of the calculated disturbance torque, a gravitational component applied to the mechanical section is cut by a filter, and a signal after the filtering and a threshold value are compared and determined.

Further, in an eighth method of the present invention, collision determination is performed based on disturbance torque of all axes calculated by each motor control unit, including a robot operated by a plurality of robots in cooperation.

[0013]

According to the first configuration and the first method of the present invention, it is possible to judge a collision immediately after a collision of a mechanical part such as a robot occurs.

Next, according to the second configuration and the second method of the present invention, it is possible to avoid erroneous determination of collision occurrence due to inertial force generated by high-speed operation of the basic shaft.

Next, according to the third configuration and the third method of the present invention, the gravitational component included in the disturbance torque can be cut.

Next, according to the eighth configuration and the eighth method of the present invention, it is possible to immediately detect a collision judgment based on the estimated disturbance torque of all the operation axes of the robot operating in cooperation.

(Embodiment 1) Hereinafter, a first embodiment of the present invention will be described.

In the description of the embodiment of the present invention, a disturbance estimator is used as one means for detecting a disturbance torque applied to a robot. However, it goes without saying that a configuration using a sensor or the like may be used.
The control device and control method of the present invention can be configured irrespective of the method of detecting disturbance torque.

First, FIG. 1 shows a control configuration for instantaneously determining a collision. Command torque T output from control means 1
The disturbance torque Td is calculated by the disturbance estimator 2 from c and the actual speed ω of the motor. Next, the calculated disturbance torque T
d is differentiated by the differentiating means 3 and output to the determining means 4 as a differentiated signal corresponding to the disturbance torque Td. The judging means 4 makes a comparison judgment between a preset threshold value and an input differential signal, judges that a collision has occurred when the threshold value is exceeded, and gives a protection processing section 5 An execution command is issued to protect the drive machine from damage.

As shown in FIG. 2 (a), the change in the disturbance torque Td when a collision occurs rises to the right, almost constant, and the disturbance torque Td is the time Ts from the occurrence of the collision until the collision is determined. Later the threshold is exceeded. On the other hand, as shown in FIG. 2B, in the case of the signal dTd / dt obtained by differentiating the disturbance torque Td, the value increases steeply according to the gradient of the disturbance torque Td at the same time when the collision occurs, and the instantaneous ( (dTs) is exceeded.

Therefore, it is possible to make a collision determination immediately after the occurrence of a collision.

(Embodiment 2) Hereinafter, a second embodiment of the present invention will be described.

For simplicity of description, as shown in FIG. 3, a motor 6 corresponding to a basic axis, a motor 7 and a link 8 corresponding to a wrist axis, a fixed base 9 and a load 10 are provided. The following description is made with a robot arm having two degrees of freedom. Further, the same components as those of the first invention are denoted by the same reference numerals and description thereof will be omitted.

First, as shown in FIG. 3, a robot arm is connected to a rotating shaft of a motor 6 fixed to a fixed base 9 by a link 8.
And attach the motor 7 to the end of the link 8,
A load 10 is attached to the motor 7. The position of the load 10 at the tip of the arm is controlled by controlling the rotation angles of the motor 6 and the motor 7. At this time, the inertia force Fi due to the rotational force of the motor 6 is applied to the motor 7.
Is working.

Therefore, when the rotation direction of the motor 6 is reversed, the inertial force Fi is applied to the motor 7 as a disturbance torque. Therefore, when the motor 6 is reversed at such a speed that the inertial force Fi exceeds the threshold value, the motor 7 makes an erroneous determination of collision occurrence.

Therefore, in order to consider the inertial force of the motor 6, which is the basic shaft, the configuration shown in FIG. 4 is adopted. The configuration of FIG. 4 will be described below.

The threshold value changing means 11 changes the threshold value according to the rotation speed ωo of the basic shaft motor 6,
Is output to the determination means 4. Judgment means 4
Then, a comparison is made between the differential signal corresponding to the disturbance torque output from the differentiating means 3 and the threshold value output from the threshold value changing means 11.

Embodiment 3 Hereinafter, a third embodiment of the present invention will be described.

First, the configuration will be described with reference to FIG. Also, the first
The same components as those of the first invention are denoted by the same reference numerals, and description thereof is omitted. The disturbance torque calculated by the disturbance estimator 2 is output to a filter unit 12 that cuts low frequencies such as a gravitational component, the gravitational component is cut by the filter unit 12, and output to the determination unit 4. Here, the gravity component included in the disturbance torque will be described. When the robot performs the joint operation of the motor 6 from p1 to p2 as shown in FIG.
The calculated disturbance torque has a waveform as shown by the solid line in FIG. Therefore, if the disturbance torque to be estimated includes a gravity component, the absolute value of the calculated value fluctuates due to the gravity term, and the determination threshold value may be reduced below the maximum value of the gravity term. Can not. However, if the disturbance torque is filtered by the filter unit 12, the fluctuation due to gravity can be cut as shown by the solid line in FIG. Further, when the robot collides with an obstacle at a certain position Pa during operation as shown in FIG. 6, the disturbance torque waveform when the filtering process is not performed becomes a waveform shown by a dotted line in FIG. 7A. However, the disturbance torque waveform when the filtering process is performed is a waveform as shown by a dotted line in FIG. Therefore, as shown in FIGS. 7A and 7B, the time Ts from the occurrence of a collision to the determination of a collision when filter processing is not performed and the time from the occurrence of a collision to the determination of collision when filter processing is performed. It can be seen that Ts> Ts 'clearly holds for Ts'.

Embodiment 4 Hereinafter, a fourth embodiment of the present invention will be described.

First, the configuration will be described with reference to FIG. Also, the first
The same components as those of the first invention are denoted by the same reference numerals, and description thereof is omitted.

The average value calculating means 13 receives the disturbance torque from the disturbance estimator 2 as input, calculates the average value of the disturbance torque within the fixed time period, and outputs the average disturbance torque. The storage means 14 stores the average value calculation means 13
Each time the average disturbance torque from is stored, it is stored and output after a certain period of time. The computing unit 15 obtains a difference between the disturbance torque from the disturbance estimator 2 and the average disturbance torque from the storage unit 14 and outputs the difference to the determination unit 4. The judging means 4 compares the output of the arithmetic unit 15 with a threshold to make a collision judgment.

Further details will be described with reference to FIG. FIG.
(A) shows the disturbance torque and its average value. Assuming that a section having a constant interval Ta is provided on the time axis and a section to which the current time belongs is defined as a section n where n is an integer, a section i natural numbers before that is defined as a section ni. Tdn (t)
Represents the disturbance torque in the section n, and TD (n-1)
Represents the average disturbance torque in the section n-1. FIG.
(B) is a section k (k =..., N−2, n−1, n,
..) And its Td
It shows the difference from the average disturbance torque TD (k-1) in the section one before the section to which k (t) belongs.

In the conventional method, the threshold value for determining the occurrence of a collision must be set to be larger than the maximum value of Td (t) as shown in FIG. According to a fourth embodiment of the invention, as shown in FIG.
It is sufficient that the difference is larger than the difference between the current disturbance torque and the average disturbance torque in the section immediately before the section to which the disturbance torque belongs. That is, according to the fourth embodiment of the present invention, the threshold value for judging the occurrence of a collision can be set smaller than that of the related art, so that the occurrence of a collision can be detected quickly.

The average disturbance torque that is different from the current disturbance torque Td (t) may be the one in two or more previous sections, but the average disturbance torque in the immediately preceding section may be used. When using, the threshold value for collision detection is
It can be set smaller.

Embodiment 5 Hereinafter, a fifth embodiment of the present invention will be described.

First, the configuration will be described with reference to FIG. Further, the same components as those of the fourth invention are denoted by the same reference numerals and description thereof will be omitted.

The speed-dependent average value calculating means 16 receives the disturbance torque from the disturbance estimator 2 as input, and for each time section that changes in accordance with the operation speed V of the robot, calculates the average value of the disturbance torque within that time section. And outputs the average disturbance torque.

Further details will be described with reference to FIG. FIG.
(A) shows the operation speed of the robot, and FIG.
Shows the disturbance torque and the average value at that time. 11B is different from FIG. 9A in that the time section changes according to the operation speed of the robot. For other points such as reference numerals, see FIG.
It has the same meaning as (a). A method of setting a time section is described in, for example, FIGS. 11 (a) and 11 (b).
In, according to the speed at a certain point, set a section starting from that point, at the end of the section,
The next section is set according to the speed at that time, and this is sequentially repeated. At this time, the length of the section is set so as to decrease as the speed increases.

FIGS. 11A and 11B show that as the speed increases, the rate of change in the posture of the robot increases, and the rate of change in the detected disturbance torque also increases. I have.

FIG. 11C shows the state of FIG.
Td (t) indicated by 1 (b) and its Td (t)
Shows the difference from the average disturbance torque in the section immediately before the section to which the section belongs. The method of setting the threshold value for collision detection is the same as that described in the fourth embodiment of the present invention.

Here, when the time section is constant regardless of the speed, the disturbance torque and its average value are shown in FIG.
In addition, Td (t) shown in FIG.
FIG. 11E shows the difference from the average disturbance torque in the section immediately before the section to which d (t) belongs. The operation speed of the robot at this time is the same as that shown in FIG.

As can be seen by comparing FIG. 11 (c) and FIG. 11 (e), the section of the time for averaging the disturbance torque is changed according to the speed, and the section of the time becomes shorter as the speed is higher. In such a case, when the time section is constant regardless of the speed, the threshold value for collision detection is
It can be seen that the size can be further reduced.

(Embodiment 6) A sixth embodiment of the present invention will be described below.

First, as shown in FIG. 12, when two robots performing cooperative operations collide with each other, any one of the motors 6 and 7 which are the motion axes of each other has an estimated disturbance torque. When the threshold value exceeds the threshold value, the robots execute protection processing against the occurrence of a collision. Next, the configuration will be described with reference to FIG. Each axis control unit 100
, The disturbance torque of all the motors constituting the robot performing the cooperative operation is calculated. Communication means 101 between the control units including between the robots
Connected by The communication unit 101 includes a higher-order determination unit 13 and a higher-order command generator 14. The higher-order determination means 13 includes all the axis control units 100
The value of the disturbance torque calculated in is output. The higher-order determination unit 13 performs a comparison determination between a threshold value preset for each axis and the disturbance torque calculated by each axis control unit 100. At this time, when the disturbance torque of any one axis exceeds the threshold value, the higher-order determination means 13 immediately outputs a determination of the occurrence of a collision to the higher-order command generator 14. Upon recognizing the collision determination, the higher-order command generator 14 outputs a command for the collision protection processing to all the motors immediately. As described above, the disturbance torque of all the motors performing the cooperative operation is converted into one command generation unit 14.
Can manage the collision with the motor having the highest detection power, and all the motors can immediately execute the collision protection operation.

(Embodiment 7) Hereinafter, a seventh embodiment of the present invention will be described.

First, as shown in FIG. 14, the motor 6, the motor 7, the link 8, and the
When the robot constituted by the above collides with the obstacle 19 during a certain operation, the disturbance torques calculated by the motors 6 and 7 are Td6 and Td7, respectively. The sum of these is Tda
It is determined that a collision has occurred when the total sum Tda of the disturbance torques of all the axes exceeds a set threshold value. Next, FIG.
The configuration will be described with reference to FIG. For simplicity, the description will be made based on the configuration of the robot shown in FIG.

First, the communication means 102 for performing communication between the respective motors constituting the robot is provided by the disturbance torque summing means 20.
A higher order determining unit 17 for comparing the sum of the disturbance torques calculated by the disturbance torque summing unit 20 with a preset sum total threshold value; and a higher order command generator 18 for generating a command for all operation axes. Be composed. Next, disturbance torques Td6 and Td7 are calculated in the axis control units 100 of the motors 6 and 7, respectively. Then, the disturbance torque summing means 20 uses the disturbance torque calculated for each axis.
Calculates the sum Tda of the disturbance torques of all the axes, and outputs the calculated sum Tda to the higher-order determination means 17. The higher-order determining means 17 calculates the total sum Tda of the disturbance torque and a preset total threshold value αa
When Tda exceeds the sum threshold value αa, it is determined that a collision has occurred, and the host command generation unit 18 is requested to execute a protection process. Immediately upon receiving a request for execution of the protection process from the upper-level determination unit 17, the upper-order command generation unit 18 issues a command for executing the protection process to the axis controller 100
Output to

Here, α6 is a threshold value for judging the collision of the motor 6, and α7 is a threshold value for judging the collision of the motor 7. FIG.
Time T from the occurrence of each collision to the collision determination shown in FIG.
s, Tda> αa rather than judging the occurrence of a collision when Td6> α6, or, and Td7> α7.
It can be clearly understood that the determination that a collision has occurred at the point of time can clearly shorten the time until the determination.

[0050]

As is apparent from the above description, the first, sixth, and seventh configurations and the first, sixth, and seventh methods of the present invention enable instantaneous detection of the occurrence of a robot collision. According to the second configuration and the second method of the present invention, it is possible to detect a collision without causing a malfunction due to an inertial force of another axis during a high-speed operation. Further, according to the third, fourth, and fifth configurations and the third, fourth, and fifth methods of the present invention, accurate disturbance torque can be calculated without being affected by gravity in changing the posture of the robot. Accurate collision detection is possible, which is extremely useful in practice.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a first exemplary embodiment of the present invention.

FIG. 2 is a diagram showing a change in disturbance torque at the time of occurrence of a collision and its differential signal.

FIG. 3 is a configuration diagram of a two-degree-of-freedom robot arm for explaining a second embodiment of the present invention;

FIG. 4 is a block diagram showing a configuration of a second exemplary embodiment of the present invention.

FIG. 5 is a block diagram showing a configuration of a third exemplary embodiment of the present invention.

FIG. 6 is a configuration diagram of a two-degree-of-freedom robot arm for explaining a third embodiment of the present invention;

FIG. 7 is a diagram showing a fluctuation of disturbance torque under the influence of gravity.

FIG. 8 is a block diagram showing a fourth embodiment of the present invention.

FIG. 9 is a diagram for explaining a fourth embodiment of the present invention.

FIG. 10 is a block diagram showing a fifth embodiment of the present invention.

FIG. 11 is a view for explaining a fifth embodiment of the present invention;

FIG. 12 is a configuration diagram of a two-degree-of-freedom robot arm for describing a sixth embodiment of the present invention;

FIG. 13 is a block diagram showing the configuration of a sixth embodiment of the present invention.

FIG. 14 is a configuration diagram of a two-degree-of-freedom robot arm for explaining a seventh embodiment of the present invention;

FIG. 15 is a block diagram showing the configuration of a seventh embodiment of the present invention.

FIG. 16 is a diagram showing a change in disturbance torque for explaining a seventh embodiment of the present invention;

FIG. 17 is a block diagram showing a configuration of a conventional collision determination process.

[Explanation of symbols]

 Reference Signs List 1 control means 2 disturbance estimator 3 differentiating means 4 determining means 5 protection processing unit 6 motor 7 motor 8 link 9 fixed base 10 load 11 threshold value changing means 12 filter unit 13 average value calculating means 14 storage means 15 computing unit 16 speed Different average value calculation means 17 High-order determination means 18 High-order command generation unit 19 Obstacle 20 Disturbance torque summation means 100 Axis control unit 101 Communication means 102 Communication means

──────────────────────────────────────────────────の Continued on the front page (51) Int.Cl. ⁷ Identification symbol FI Theme coat ゛ (Reference) G05D 3/12 306 G05D 3/12 306Z (72) Inventor Takashi Nakatsuka 1006 Odakadoma, Kazuma, Osaka Matsushita Electric Within Sangyo Co., Ltd. (72) Inventor Masahiro Ooto 1006, Kadoma, Kadoma City, Osaka Prefecture Matsushita Electric Industrial Co., Ltd.F-term (reference) KB06 KC55 5H303 BB07 BB15 BB20 DD01 DD26 JJ05 KK04 KK11 KK17

Claims (8)

[Claims] A robot configured by a plurality of motors; a control unit for controlling the motor; a disturbance detector for inputting torque and speed of the motor to estimate a disturbance torque applied to the motor; A control device for a robot, comprising: differentiating means for differentiating a signal corresponding to disturbance torque output from a detector; and determining means for comparing a signal output from the differentiating means with a threshold value. 2. One or more basic shafts and one or more
The threshold value of the wrist axis when the operation speed of the basic axis is high is higher than the threshold value of the wrist axis when the operation speed of the basic axis of the robot having two or more wrist axes is low. 2. The control device for a robot according to 1. A control unit for controlling the motor of the robot constituted by a plurality of motors; a disturbance detector for inputting torque and speed of the motor to estimate a disturbance torque applied to the motor; 3. The control device for a robot according to claim 1, further comprising a filter section for cutting only a frequency component having a disturbance torque output from the device. 4. The robot according to claim 1, wherein a signal is input from a disturbance detector of each axis of each robot in a robot that performs a cooperative operation by connecting a plurality of control units by a communication unit. Control device. 5. A step of inputting a torque and a speed of the motor of a robot constituted by a plurality of motors to estimate a disturbance torque applied to the motor, a step of differentiating a signal corresponding to the disturbance torque, A method for controlling a robot, comprising a step of comparing a signal calculated in a differentiating step with a threshold value. 6. One or more basic shafts and one or more basic shafts
The threshold value of the wrist axis when the operation speed of the basic axis is high is higher than the threshold value of the wrist axis when the operation speed of the basic axis of the robot having two or more wrist axes is low. 6. The method for controlling a robot according to 5. 7. A step of estimating a disturbance torque of the motor by inputting a torque and a speed of the motor of a robot constituted by a plurality of motors, and cutting only a certain frequency component of the calculated disturbance torque by a filter. The robot control method according to claim 5, wherein 8. The robot according to claim 5, wherein a signal is input from a disturbance detector of each axis of each robot in a robot that performs a cooperative operation by connecting a plurality of control units by communication means. Control method.