JP2017019058A - Robot control device, robot, and robot system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a technique for reducing the possibility of erroneously detecting collision.SOLUTION: A robot control device controls a movable portion. The robot control device comprises a threshold value setting section setting a threshold value to a value greater than that of first operation in second operation in which an acceleration limitation of the movable portion is more alleviated than that in the first operation at the time of determination of occurrence of collision at the movable portion when a diagnostic value indicating a state of the movable portion is greater than the threshold value.SELECTED DRAWING: Figure 5

Description

  The present invention relates to a robot control device, a robot, and a robot system.

  A technique for detecting a collision of a robot by comparing a collision evaluation amount related to a disturbance applied to the robot from the outside and a threshold is known (see Patent Document 1). In Patent Document 1, the disturbance is derived from a motion equation model. The threshold value is changed according to the error range of the parameter of the equation of motion.

JP 2005-102427 A

  However, even when the robot is operating normally, there has been a problem that erroneous detection of a collision may occur depending on the operation of the robot.

  A robot control apparatus for solving at least one of the above problems is a robot control apparatus for controlling a movable part of a robot, and when a diagnostic value indicating a state of the movable part becomes larger than a threshold value, the movable part In the second operation in which the limitation on the acceleration of the movable part is relaxed compared to the first operation, a threshold setting unit is provided that sets a threshold value larger than that of the first operation.

  In the above configuration, when the diagnostic value indicating the state of the movable part is larger than the threshold value, it can be determined that the state of the movable part is disturbed by the collision, and the collision can be detected. Here, in the second operation in which the limitation of the acceleration of the movable part is relaxed compared to the first operation, the threshold value is set to a value larger than that in the first operation. Thereby, in the second operation in which rapid acceleration is allowed, the state of the movable part can be allowed to be disturbed to some extent, and the possibility of erroneously detecting a collision can be reduced.

  The movable part may be a part in which the robot can move by its own power, and may be a manipulator or the like that is movably connected to the main body. Also good. Further, the movable part may be defined for each joint such as a manipulator. That is, for each of the plurality of joints, it may be determined whether or not the diagnostic value indicating the state of the movable part is greater than the threshold value. The diagnostic value may be an index value indicating the degree of possibility that the state of the movable part is normal. For example, the diagnostic value may be a value that increases as the actual state of the movable part deviates from the ideal state. Thereby, it can be determined that a collision has occurred when the state deviates from the ideal state. Further, the diagnostic value may be a value to which a band pass filter that passes a frequency component suitable for a change in the state of the movable part due to a collision is applied. This

  Relaxing the limitation of the acceleration of the movable part in the second operation may be changing the upper limit value of the acceleration set in the first operation to a larger value, or the acceleration set in the first operation. The upper limit value may be invalidated. The acceleration may include deceleration (negative acceleration), and the acceleration limitation may be an absolute value limitation. The threshold setting unit only needs to set the threshold to a value larger than that of the first operation in at least the second operation, and may set different threshold values in each of the three or more types of operations. When it is determined that a collision has occurred in the movable part, the robot control device may stop the movable part or notify the occurrence of the collision. Stopping the movable part may be to stop generation of a driving force for driving the movable part, or to electrically or mechanically brake the movable part.

  Note that the function of each means described in the claims is realized by hardware resources whose function is specified by the configuration itself, hardware resources whose function is specified by a program, or a combination thereof. The functions of these means are not limited to those realized by hardware resources that are physically independent of each other.

It is a schematic diagram of a robot system. It is a block diagram of a robot system. (3A) and (3D) are graphs of angular velocity, (3B) and (3E) are graphs of angular velocity deviation, and (3C) and (3F) are graphs of diagnostic values. It is a list of operations. It is a graph of a 1st threshold value. It is a schematic diagram of the robot system of other embodiment.

Hereinafter, embodiments of the present invention will be described in the following order with reference to the accompanying drawings. In addition, the same code | symbol is attached | subjected to the corresponding component in each figure, and the overlapping description is abbreviate | omitted.
(1) Robot system configuration:
(2) Other embodiments:

(1) Robot system configuration:
FIG. 1 is a schematic diagram of a robot system according to a first embodiment of the present invention. As shown in FIG. 1, the robot system includes a robot 1 and a control device 3. The control device 3 constitutes the robot control device of the present invention. The control device 3 may be a dedicated computer or a general-purpose computer in which a program for the robot 1 is installed.

  The robot 1 is a single-arm robot including one arm A. The arm A includes a base T and six joints J1 to J6. The base T is fixed to the work table. The base T and the six arm members A1 to A6 are connected by the joints J1 to J6. The arm members A1 to A6 and the end effector 2 are movable parts of the present invention. Joints J2, J3, and J5 are bending joints, and joints J1, J4, and J6 are torsional joints. The force sensor FS and the end effector 2 are attached to the arm member A6 on the most distal side of the arm A. The end effector 2 grips the workpiece W with a gripper. A predetermined position of the end effector 2 is represented as a tool center point (TCP). The position of the TCP is a reference position for the end effector 2. The force sensor FS is a 6-axis force detector. The force sensor FS detects the magnitudes of the forces in the three detection axis directions orthogonal to each other and the magnitudes of the torques around the three detection axes.

  A coordinate system that defines the space in which the robot 1 is installed is referred to as a robot coordinate system. In the present embodiment, the robot coordinate system is a three-dimensional orthogonal coordinate system defined by an X axis and a Y axis that are orthogonal to each other on a horizontal plane, and a Z axis that has a vertically upward direction as a positive direction. A rotation angle around the X axis is represented by RX, a rotation angle around the Y axis is represented by RY, and a rotation angle around the Z axis is represented by RZ. An arbitrary position in the three-dimensional space can be expressed by a position in the X, Y, and Z directions, and an arbitrary posture (rotation direction) in the three-dimensional space can be expressed by a rotation angle in the RX, RY, and RZ directions. Hereinafter, unless otherwise indicated, when it is expressed as a position, it can also mean a posture. Unless otherwise indicated, when expressed as force, it can also mean torque acting in the RX, RY, and RZ directions. The controller 3 controls the position of the TCP in the robot coordinate system by driving the arm A. The end effector 2 is fixed so as not to move relative to the force sensor FS, and the magnitude and direction of the force detected by the force sensor FS are considered to coincide with the magnitude and direction of the force acting on the TCP.

  FIG. 2 is a block diagram of the robot system. A control program for controlling the robot 1 is installed in the control device 3. The control device 3 includes a computer including a processor, RAM, and ROM. When the computer executes a control program recorded on a recording medium such as a ROM, the control device 3 includes a drive control unit 31, a stop control unit 32, and a threshold setting unit 33 as functional configurations. The drive control unit 31, the stop control unit 32, and the threshold setting unit 33 are realized by the cooperation of hardware resources and software resources of the control device 3. The drive control unit 31, the stop control unit 32, and the threshold setting unit 33 may be realized by an ASIC (Application Specific Integrated Circuit) or the like.

  The drive control unit 31 controls the arm A so that the target position and the target force are realized by TCP. The drive control unit 31 sets a target position and a target force for each control cycle, and controls the arm A. In the present embodiment, the length P of the control cycle is 4 milliseconds. By setting a target position for each control period on the TCP trajectory, the TCP can be moved on the trajectory.

The target force is a force to be detected by the force sensor FS and is a force to be applied to the TCP. The letter S represents one of the axis directions (X, Y, Z, RX, RY, RZ) that define the robot coordinate system. For example, in the case of S = X, X-direction component of the position specified in the robot coordinate system is denoted as S _t = X _t, X-direction component of the desired force is denoted as f _St = f _Xt. S represents the position in the S direction (rotation angle).

  The robot 1 includes motors M1 to M6 as driving units, encoders E1 to E6, and a braking unit B in addition to the configuration illustrated in FIG. Motors M1 to M6 and encoders E1 to E6 are provided corresponding to the joints J1 to J6, respectively, and the encoders E1 to E6 detect the driving positions of the motors M1 to M6. Controlling the arm A means controlling the motors M1 to M6 as drive units. The drive control unit 31 can communicate with the robot 1. The drive control unit 31 stores a correspondence U between a combination of drive positions of the motors M1 to M6 and a TCP position in the robot coordinate system.

Drive control unit 31 obtains the drive position D _a of the motor M1-M6, based on the correspondence relationship U, the position S (X of TCP the driving position D _a in the robot coordinate system, Y, Z, RX, RY , RZ). Based on the TCP position S and the detected value of the force sensor FS, the drive control unit 31 specifies the acting force f _S actually acting on the force sensor FS in the robot coordinate system. The force sensor FS detects a detection value in a unique coordinate system. However, since the relative position and direction of the force sensor FS and TCP are stored as known data, the drive control unit 31 is a robot. The acting force f _S in the coordinate system can be specified. The drive control unit 31 performs gravity compensation for the acting force f _S. Gravity compensation is to remove the gravity component from the acting force f _S. The acting force f _S subjected to gravity compensation can be regarded as a force other than gravity acting on the TCP. The gravity component of the acting force f _S acting on the fitting member for each TCP posture is investigated in advance, and the drive control unit 31 subtracts the gravity component corresponding to the TCP posture from the acting force f _S to reduce the gravity. Compensation is realized.

（１）式の左辺は、ＴＣＰの位置Ｓの２階微分値に仮想慣性係数ｍを乗算した第１項と、ＴＣＰの位置Ｓの微分値に仮想粘性係数ｄを乗算した第２項と、ＴＣＰの位置Ｓに仮想弾性係数ｋを乗算した第３項とによって構成される。（１）式の右辺は、目標力ｆ_Stから現実の作用力ｆ_Sを減算した力偏差Δｆ_S（ｔ）によって構成される。（１）式における微分とは、時間による微分を意味する。 The drive control unit 31 specifies the force-derived correction amount ΔS by substituting the target force f _St and the acting force f _S into the equation of motion for impedance control. Equation (1) is an equation of motion for impedance control.

The left side of the equation (1) is a first term obtained by multiplying the second-order differential value of the TCP position S by the virtual inertia coefficient m, a second term obtained by multiplying the differential value of the TCP position S by the virtual viscosity coefficient d, And a third term obtained by multiplying the position S of the TCP by the virtual elastic coefficient k. The right side of the equation (1) is constituted by a force deviation Δf _S (t) obtained by subtracting the actual acting force f _S from the target force f _St. The differentiation in the equation (1) means differentiation with time.

The impedance control is control for realizing a virtual mechanical impedance by the motors M1 to M6. The virtual inertia coefficient m means the mass that the TCP virtually has, the virtual viscosity coefficient d means the viscous resistance that the TCP virtually receives, and the virtual elastic coefficient k is the spring constant of the elastic force that the TCP virtually receives. means. Each parameter m, d, k may be set to a different value for each direction, or may be set to a common value regardless of the direction. The force-derived correction amount ΔS means the size of the position S where the TCP should move in order to eliminate the force deviation Δf _S (t) from the target force f _St when the TCP receives mechanical impedance. To do. Drive control unit 31, the target position S _t, by adding a force from the correction amount [Delta] S, identifies the correction target position in consideration of the impedance control (S _t + ΔS).

Then, based on the correspondence relationship U, the drive control unit 31 sets the corrected target position (S _t + ΔS) in the direction of each axis that defines the robot coordinate system as the target drive position that is the target drive position of each motor M1 to M6. into a position D _t. Then, the drive control unit 31, by the target drive position D _t subtracting the actual drive position D _a of the motor M1-M6, calculates the driving position deviation _{D e (= D t -D a} ). Drive control unit 31 to be a value obtained by multiplying the position control gain K _p to the driving position deviation D _e angular speed command _{V c (= K p · D} e), a time differential value of the actual drive position D _a drive The control amount D _c is specified by adding the value obtained by multiplying the drive speed deviation, which is the difference from the speed, by the speed control gain K _v . The control amount D _c is specified for each of the motors M1 to M6.

Further, the drive control unit 31 acquires the upper limit value A _max of the angular acceleration corresponding to the operations Q1 to Q6 currently being performed by the arm A, and the absolute value of the actual angular acceleration A _a is greater than the upper limit value A _max . If the value is also larger, the control amount _Dc is corrected downward. For example, the control amount D _c may be corrected to 0. Hereinafter, when expressed as angular acceleration A _a, it shall mean the absolute value of the angular acceleration A _a unless otherwise indicated. This allows the arm member A1~A6 and end effector 2 is to limit the angular acceleration A _a which rotates about the joints J1 to J6. Further, it can be said that the higher the upper limit value A _max of the angular acceleration A _a is large, the angular acceleration A _a restriction is relaxed. The actual angular acceleration A _a is obtained by subtracting the actual drive position D _a (n) in the current control cycle from the actual drive position D _a (n−1) in the immediately preceding control cycle, and the control cycle length P May be obtained by dividing by the square of, or may be obtained based on a measurement value of a gyro sensor or the like. Details of the operations Q1 to Q6 will be described later. n is a natural number indicating a control cycle number.

The configuration of the drive control unit 31 described above, it is possible to control the arm A on the basis of the target position S _t and the target force f _St. Here, a control force control for the actual acting force f _S and the target force f _St, a control position control for the position S of the TCP reality and the target position S _t.
In the present embodiment, the drive control unit 31 can perform both position control and force control, and can perform only position control, according to the work content to be performed by the robot 1. For example, regardless of the actual applied force f _S , it is possible to substantially perform only the position control by assuming that the force-derived correction amount ΔS in FIG. 2 is always 0.

The stop control unit 32 stops the movable unit when the detected speed, which is the speed of the movable unit detected by the sensor, deviates from the speed command. The movable parts are movable arm members A1 to A6 and the end effector 2. The detection rate is the angular velocity of the rotating arm member A1~A6 an end effector 2 around the rotation axis of the joint J1 to J6, derived from actual driving position D _a detected by the encoder E1~E6 as a sensor The detected angular velocity V _a . For example, angular velocity detected V _a of the arm member A1 is the angular velocity of rotating the arm member A1 around the rotational axis of the joint J1, deriving the arm member A1 from the output signal of the encoder E1 of the joint J1 for connecting a base T Is done.

The detected angular velocity V _a is obtained by dividing the value obtained by subtracting the actual driving position D _a (n) in the current control cycle by the actual driving position D _a (n−1) in the immediately preceding control cycle by the length P of the control cycle. It may be obtained by doing, and may be obtained based on measurement values, such as a gyro sensor.

The speed command is the angular velocity arm member A1~A6 an end effector 2 is to be rotated around the rotation axis of the joint J1 to J6, an angular velocity command V _c described above. Specifically, angular velocity command V _c is a position control gain K _p to the target drive position D _t driving position actual drive position D _a motor M1~M6 was subtracted from the deviation _{D e (= D t -D a} ) Multiplyed value.

The stop control unit 32 stops the movable unit when the diagnostic value indicating the state of the movable unit becomes larger than the first threshold value. The diagnostic value indicating the state of the movable part is a value obtained by multiplying the difference between the speed command and the detected speed by a high-pass filter (HPF), and an angular speed deviation (V) that is a difference between the angular speed command V _c and the detected angular speed V _{a. c−} V _a ) is a value obtained by applying a high-pass filter to the absolute value (hereinafter referred to as the first diagnosis value HV1).

FIG. 3A is a graph of the angular velocity V of the arm member A1 as a movable part. The vertical axis in FIG. 3A indicates the angular velocity V, and the horizontal axis indicates time t. In FIG. 3A, it is assumed that the arm member A1 accelerates, then rotates at a constant speed, and finally decelerates and stops. FIG. 3B shows an example of the angular velocity deviation (V _c −V _a ) in the case of FIG. 3A (when the target position _St is sequentially set based on FIG. 3A). The vertical axis in FIG. 3B indicates angular velocity deviation (V _c −V _a ), and the horizontal axis indicates time t. As shown in FIG. 3B, to follow behind the actual drive position D _a slightly with respect to the target position S _t, the angular velocity deviation (V _c -V _a) shake positively in accelerating, during deceleration Will cause the angular velocity deviation (V _c −V _a ) to swing negatively. Therefore, the angular velocity deviation (V _c −V _a ) includes a low frequency component corresponding to the acceleration and deceleration cycles.

FIG. 3C is a graph of the first diagnostic value HV1 in the case of FIG. 3A. By applying a high-pass filter to the absolute value of the angular velocity deviation (V _c −V _a ), the low frequency component corresponding to the acceleration and deceleration cycles can be reduced. The pass frequency band of the high pass filter is desirably a frequency band higher than the frequency at which acceleration and deceleration are repeated.

As shown in FIG. 3B, when a collision (collision that hinders movement of the arm member A1) occurs at time t indicated by x, the angular velocity deviation (V _c −V _a ) decreases sharply. Since the component of the change in the angular velocity deviation (V _c −V _a ) corresponding to the collision is steep, even if a high-pass filter is applied as shown in FIG.

  The stop control unit 32 stops the arm A when the first diagnosis value HV1 is larger than the first threshold value TH1. The first threshold value TH1 is set for each of the arm members A1 to A6 as the movable part and the end effector 2, and the stop control part 32 sets the first threshold value TH1 for each of the arm members A1 to A6 and the end effector 2. Make a decision.

  That is, the stop control unit 32 determines the first diagnosis value HV1 and the first threshold value TH1 based on the signal from the encoder E1 for the arm member A1, and the first diagnosis value HV1 based on the signal from the encoder E2 for the arm member A2. And the first threshold value TH1 is determined, the first diagnosis value HV1 and the first threshold value TH1 are determined for the arm member A3 based on the signal of the encoder E3, and the first diagnosis is performed for the arm member A4 based on the signal of the encoder E4. The value HV1 and the first threshold value TH1 are determined. For the arm member A5, the first diagnosis value HV1 and the first threshold value TH1 are determined based on the signal from the encoder E5. The arm member A6 (end effector 2) is determined by the encoder E6. The first diagnosis value HV1 and the first threshold value TH1 are determined based on the signal.

  In any one of the arm members A1 to A6 as the movable part and the end effector 2, when it is determined that the first diagnosis value HV1 is larger than the first threshold value TH1, the stop control unit 32 causes a collision in the movable part. Then, the motors M1 to M6 are stopped, and the entire arm A is stopped. The stop control unit 32 stops not only the entire arm A but only one motor M1 to M6 that directly drives the end effector 2 and the arm members A1 to A6 whose first diagnosis value HV1 is the first threshold value TH1. You may let them.

  In the configuration of the present embodiment described above, it is possible to easily detect a collision in the movable part based on whether the detected speed and the speed command are deviated, and when the collision is detected, the collision occurs in the movable part. Can be determined. That is, if the detection speed and the speed command can be acquired, the collision can be detected, and the disturbance received by the movable part due to the collision may not be extracted from the force acting on the movable part. Therefore, it is not necessary to solve the equation of motion in order to extract the disturbance received by the movable part due to the collision, or to investigate the parameter applied to the equation of motion in advance, and the collision can be easily detected.

Further, a value obtained by multiplying the difference between the speed command and the detected speed by a high-pass filter, that is, the first diagnostic value HV1 obtained by multiplying the absolute value of the angular velocity deviation (V _c −V _a ) by the high-pass filter is greater than the first threshold value TH1. In this case, the arm members A1 to A6 and the end effector 2 as the movable parts are stopped. Since the first diagnostic value HV1 can be derived from the value used in servo control, the calculation load for deriving the first diagnostic value HV1 can be suppressed, the first diagnostic value HV1 can be derived early, and the collision determination can be performed early. Can be completed. In addition, by applying a high-pass filter, it is possible to obtain the first diagnosis value HV1 with a good S / N ratio, and to reduce the possibility of erroneous determination. That is, the fluctuation of the angular velocity deviation (V _c −V _a ) that appears in the acceleration / deceleration cycle becomes noise in the collision determination, and this noise can be suppressed by the high-pass filter. Furthermore, the first threshold value TH1 can be set to a small value by using the first diagnosis value HV1 with a good S / N ratio, and the collision determination can be performed at a higher speed.

  Next, details of the first threshold TH1 will be described. As described above, the first threshold value TH1 is set for each of the arm members A1 to A6 as the movable part, but the first threshold value TH1 is further set for each of the operations Q1 to Q6.

Figure 4 is a table showing the relationship between the operating Q1~Q6 the upper limit value A _max of the first threshold value TH1 and the angular acceleration A _a. As shown in FIG. 4, the operation Q1 is a normal operation, the operation Q2 is a torque saturation operation, the operation Q3 is a jog operation, the operation Q4 is a stop operation, the operation Q5 is a tracking operation, and the operation Q6 Is a singularity avoidance operation. The upper limit value A _max of the angular acceleration A _a is set for each of the operations Q1 to Q6, and the upper limit value of the angular acceleration greater than the upper limit value A _max1 of the angular acceleration A _a in the normal operation (Q1) is set for the operations Q2 to M6. Values A _{max2 to} A _max6 are set. The upper limit value A _Max6 large angular acceleration A _a than the upper limit value _{A max1} ~A max6 angular acceleration A _a behavior Q1~Q5 is set in the singularity avoidance operation (Q6). In addition, the operations Q1 to Q6 are operations that perform only position control without performing force control.

The torque saturation operation (Q2) is an operation that is set when the torque of the motors M1 to M6 is predicted to be saturated, and by relaxing the upper limit value A _max2 of the angular acceleration A _a than the normal operation (Q1). , Set to protect the motors M1 to M6. Jog operation (Q3) is an operation for driving only motors M1~M6 operation period operation unit (not shown) is operated, relieving the upper limit value A _max3 angular acceleration A _a than the normal operation (Q1) Therefore, it is set to improve the operation responsiveness. Stop operation (Q4) is an operation for stopping the driving of the motor M1~M6 in the abnormality or the like, by relaxing the upper limit value A _max4 angular acceleration A _a than the normal operation (Q1), rapidly arm A Is set so that it can be stopped. The stop operation (Q4) is also an operation for stopping the driving of the motors M1 to M6, but the driving of the motors M1 to M6 is stopped more slowly than when the stop control unit 32 stops the driving of the motors M1 to M6 at the time of collision. Is the action.

Tracking operation (Q5) is an operation to follow the end effector 2 to the transport device such as a belt conveyor, the angular acceleration A _a than the normal operation (Q1) by relaxing the upper limit value A _max5, to the conveying device Set to improve followability. The singular point avoiding operation (Q6) is an operation that greatly drives (for example, rotates about 180 degrees) any one of the joints J1 to J6 in order to avoid becoming a singular point (singular posture), and is a normal operation (Q1). By _setting the upper limit value A _max6 of the angular acceleration A _a more relaxed, it is set to shorten the period required to avoid the singular point.

As described above, in the operation of the upper limit value A _max of the angular acceleration is reduced, may large angular acceleration A _a than the upper limit value A _max1 of the angular acceleration A _a in the normal operation is permitted. The upper limit value A _max of the angular acceleration A _a is also a value set for each joint J1 to J6.

3A is a graph of the angular velocity V of the arm member A1 in the normal operation (Q1), and FIG. 3D is a graph of the angular velocity V of the arm member A1 in the jog operation (Q3). In the jog operation (Q3), a faster acceleration / deceleration (a steep slope in the graph) than that in the normal operation (Q1) is allowed. Figure 3B typically illustrates operation of the (Q1) angular deviations in (V _c -V _a), FIG. 3E shows an angular deviation in jog operation _{(Q3) (V c -V a} ). Since the responsiveness of the arm A has a certain limit, in the jog operation (Q3) in which rapid acceleration / deceleration is performed, the follow-up delay of the detected angular velocity V _a with respect to the rapid change of the velocity command V _c becomes remarkable, and usually The angular velocity deviation (V _c −V _a ) will swing more greatly than the operation (Q1). FIG. 3C shows the first diagnostic value HV1 in the normal operation (Q1), and FIG. 3F shows the first diagnostic value HV1 in the jog operation (Q3). Although the fluctuation of the first diagnosis value HV1 that appears in the acceleration and deceleration cycles is reduced by applying the high-pass filter, the fluctuation of the first diagnosis value HV1 in the jog operation (Q3) becomes larger than the normal operation (Q1). Yes. That is, in the operations Q2 to Q6 in which the upper limit value A _max of the angular acceleration A _a is relaxed compared to the normal operation, the noise of the first diagnosis value HV1 in the collision determination can be increased.

Therefore, the threshold value setting unit 33 sets the first threshold value TH1 to a value larger than that of the first operation in the second operation in which the limitation on the acceleration of the movable part is relaxed compared to the first operation. That is, the threshold setting unit 33 performs the torque saturation operation (Q2) and the jog operation as the second operation in which the upper limit value A _max of the angular acceleration A _a is relaxed to a value larger than the normal operation (Q1) as the first operation. In (Q3), the stop operation (Q4), the tracking operation (Q5), and the singularity avoidance operation (Q6), a first threshold value TH1 larger than the normal operation (Q1) is set.

FIG. 5 is a graph showing the relationship between the upper limit value A _max of the angular acceleration A _a and the first threshold value TH1. The vertical axis of FIG. 5 shows a first threshold value TH1, the horizontal axis represents the upper limit value A _max of the angular acceleration A _a. The black circles in FIG. 5 indicate the first diagnostic value HV1 when an experiment is performed in which the joints J1 to J6 of the arm A are driven for each upper limit value A _max of the angular acceleration A _a in a state where no object collides. Yes. Assuming all operating conditions, various controls (PTP (Point to Point) control and CP (Continuous Path) control, etc.) are tested under various loads (workpiece W mass), eccentric position and inertia conditions. It is desirable to go to obtain the first diagnostic value HV1 (maximum value during continuous operation). As indicated by a one-dot chain line in FIG. 5, an approximate function R (A _max ) approximating a large number of first diagnostic values HV1 obtained by experiments is calculated by, for example, the least square method, etc., and the threshold setting unit 33 can read out Record it. The approximate function R (A _max ) is calculated and recorded for each of the joints J1 to J6.

Threshold setting unit 33 obtains the operation Q1~Q6 that carry out the current arm A, substituting the upper limit value A _max of the angular acceleration A _a corresponding to the operation Q1~Q6 the approximation function R (A _max) To do. If the current operation is any of the operations Q1 to Q5, the threshold setting unit 33 sets a value obtained by multiplying a value obtained by the approximation function R (A _max ) by n (n is an integer of 2 or more) as the first threshold. Set as TH1. If the current action is the singularity avoidance action (Q6), the threshold setting unit 33 sets the value obtained by multiplying the value obtained by the approximation function R (A _max ) m times (m is an integer greater than n) as the first value. Set as threshold TH1. In FIG. 5, n = 3 and m = 4. In FIG. 5, the transition of the first threshold value TH1 when the upper limit value A _max of the angular acceleration A _a continuously changes (actually a discontinuous value for each of the operations Q1 to Q6) is indicated by a solid line. The threshold value setting unit 33 sets the first threshold value TH1 calculated as described above in the stop control unit 32.

In the configuration of the present embodiment described above, when the first diagnosis value HV1 indicating the state of the movable part is larger than the first threshold value TH1, it can be determined that the state of the movable part is disturbed by the collision, Can be detected. Here, in the operations Q2 to Q6 as the second operation in which the upper limit value A _max of the acceleration of the movable part is relaxed compared to the normal operation as the first operation, the first threshold value TH1 is set to a larger value than the first operation. Is done. Thereby, in the operations Q2 to Q6 in which rapid acceleration is allowed, the first diagnostic value HV1 can be allowed to be disturbed to some extent, and the possibility of erroneously detecting a collision can be reduced.

  Further, since the second operation includes a singular point avoiding operation for avoiding the singular point of the movable part, the possibility of erroneously detecting a collision may be reduced even in the singular point avoiding operation in which the first diagnosis value HV1 is likely to be disturbed. it can. Here, if the operations Q1 to Q5 are regarded as the first operation, only the singularity avoidance operation (Q6) can be regarded as the second operation. That is, in the present embodiment, the first operation is considered to include not only the normal operation (Q1) but also the torque saturation operation (Q2), the jog operation (Q3), the stop operation (Q4), and the tracking operation (Q5). be able to.

In the present embodiment, when the movable part performs the third operation, the stop control unit 32 does not stop the movable part regardless of the magnitude of the first diagnosis value HV1. As shown in FIG. 4, the third operation includes a brake release operation (Q9) for releasing the mechanical braking of the movable part, and the movable part so that the acting force f _S acting on the movable part becomes the target force f _St. And at least one of force control operations (Q7) for controlling. Further, the third operation includes a torque control operation (Q8) for controlling the motors M1 to M6 so as to generate a target torque. In the force control operation (Q7) and the torque control operation (Q8), priority is given to controlling the motors M1 to M6 so that the target force f _St and the target torque are realized, so the angular acceleration A _a No upper limit value A _max is set. The braking release operation (Q9) is an operation for releasing the mechanical braking of the arm A by the braking unit B, and is an operation when the force that the braking unit B supports the arm A against gravity is lost. in the angular acceleration a _a due to gravitational acceleration is an operation when the joint J1~J6 rotates. For rotating joints J1~J6 in angular acceleration A _a due to gravitational acceleration is unavoidable, the upper limit value A _max of the angular acceleration also brake release operation (Q9) is not set.

As described above, in the operations Q7 to Q9 as the third operation, the upper limit value A _max of the angular acceleration A _a is not set, and the S / N ratio of the first diagnosis value HV1 can be ensured. It may not be possible. Therefore, when the movable part performs the third operation, the stop control unit 32 may erroneously detect a collision by not stopping the movable part regardless of the magnitude of the first diagnosis value HV1. Can be reduced.

(2) Other embodiments:
By applying a high-pass filter to the value indicating the state of the movable part, it is possible to obtain a diagnostic value that can determine the state of the movable part that fluctuates rapidly according to the collision. By applying a high-pass filter, it is possible to extract a sudden fluctuation component caused by a disturbance from a value indicating the state of the movable part without solving the equation of motion model. The diagnostic value is not particularly limited as long as it is a value indicating the state of the movable part, and may be the following value, for example.

  For example, the stop control unit 32 may stop the movable unit when a value obtained by applying a high-pass filter to the torque generated by the drive unit that drives the movable unit is larger than the second threshold value. Specifically, the stop control unit 32 samples the current flowing through the motors M1 to M6 every minute sampling period, acquires a value obtained by multiplying the current by a high-pass filter as the second diagnostic value, and the second diagnostic value May be determined that a collision has occurred. When a collision occurs, the motors M1 to M6 receive a sudden torque due to the force received by the arm members A1 to A6 and the end effector 2 due to the collision, and a sudden change occurs in the currents of the motors M1 to M6. Become. By applying a high-pass filter to the torque of the motors M1 to M6, the amplitude (low frequency component) of the torque that should be generated by the motors M1 to M6 can be reduced, and the presence or absence of a sudden torque change can be determined. Furthermore, the stop control unit 32 may stop the movable unit when a value obtained by applying a high-pass filter to a value obtained by differentiating the torque generated by the drive unit that drives the movable unit is larger than a threshold value.

  Furthermore, the movable part may be provided with an angular velocity detector. For example, as shown in FIG. 6, each of the arm members A1 to A5 may be provided with gyro sensors GY1 to GY5, and the stop control unit 32 may acquire output signals of the gyro sensors GY1 to GY5. The stop control unit 32 may acquire the angular velocity of the end effector 2 from the force sensor FS. The stop control unit 32 may stop the movable unit when a value obtained by applying a high-pass filter to a value obtained by differentiating the angular velocity of the movable unit output from the angular velocity detector is greater than the third threshold value. Specifically, the stop control unit 32 acquires an angular velocity from the gyro sensors GY1 to GY5 and the force sensor FS, acquires a value obtained by multiplying the torque proportional value obtained by differentiating the angular velocity by a high-pass filter as a third diagnostic value, You may determine with the collision having arisen when the said 3rd diagnostic value is larger than a 3rd threshold value. By detecting the angular velocity using the gyro sensors GY1 to GY5 and the force sensor FS, it is possible to obtain a diagnostic value with better responsiveness than detecting the torque based on the current flowing through the motors M1 to M6, and to collide early. Can be determined. Also in other embodiments, it is desirable that the high-pass filter pass frequency band be higher than the frequency at which acceleration and deceleration are repeated. Note that the gyro sensors GY1 to GY5 are not necessarily provided in each of the arm members A2 to A6, and may be provided in one or more of the arm members A1 to A6.

  Further, the stop control unit 32 may stop the movable unit when a value obtained by multiplying the second-order differential of the torque generated by the drive unit that drives the movable unit by a high-pass filter is larger than a threshold value. . The arm members A1 to A6 and the end effector 2 may be provided with an acceleration sensor, and the stop control unit 32 may determine the presence or absence of a collision based on the acceleration output from the acceleration sensor. In addition, a plurality of determinations of the presence or absence of a collision by the first threshold value to the third threshold value may be performed in parallel, or the determination of the presence or absence of a collision by any one of the first threshold value to the third threshold value is performed independently. May be. The second threshold value and the third threshold value may also be set according to the operations Q1 to Q6.

  In the embodiment, the collision determination based on the first diagnosis value HV1 is not performed in the force control operation (Q7). However, the stop control unit 32 also performs the first diagnosis value in the force control operation (Q7). A collision determination based on HV1 may be performed. In this case, in the force control operation (Q7) for controlling the movable part while being in contact with another object, the threshold setting unit 33 is more than the operations Q1 to Q6 for controlling the movable part without being in contact with the other object. The first threshold value TH1 may be set to a large value. A force control operation (Q7) is performed when the end effector 2 is brought into contact with a planned contact object such as the workpiece W.

By performing the impedance control, it is possible to perform an operation of controlling the acting force f _S acting between the end effector 2 and the planned contact object and applying a force to the planned contact object. Further, by realizing a virtual mechanical impedance, it is possible to prevent the end effector 2 and a contact object from being damaged. In this way, in the force control operation (Q7) in which the end effector 2 is scheduled to contact the object to be contacted, the first diagnosis value HV1 can be larger than the operations Q1 to Q6. Is set to a large value to prevent erroneous determination of a collision. Further, since the angular acceleration A _a can increase as the target force f _S increases, the first diagnosis value HV1 can also increase. Therefore, the stop control unit 32 may set the first threshold value TH1 to a larger value as the target force f _S is larger. Further, when the virtual viscosity coefficient d, virtual elastic coefficient k, and virtual inertia coefficient m in impedance control are set so that the end effector 2 is softly brought into contact with the contact object, the first threshold value TH1 is set to a small value. May be set.

  Further, the stop control unit 32 does not necessarily have to acquire the operations Q1 to Q9 performed by the robot 1 from the drive control unit 31. For example, when the speed at which the end effector 2 is moving falls below a predetermined threshold, the stop control unit 32 determines that the end effector 2 is in a state in which the end effector 2 can contact the planned contact object, and the first The threshold value TH1 may be set to a value larger than the current value. Further, the stop control unit 32 can contact the end effector 2 when the speed of the end effector 2 turns below a threshold value during deceleration where the speed of the end effector 2 is continuously decreasing. You may determine with being in a state. That is, the stop control unit 32 may maintain the first threshold value TH1 at the current value as long as the end effector 2 is not decelerating even when the speed of the end effector 2 has turned below the threshold value. . Thereby, it is possible to suppress a collision detection omission when the speed of the end effector 2 is to be accelerated from a state close to zero. Furthermore, when the movement of the TCP is completed by a distance or time of a predetermined ratio (for example, 90 to 95%) of the route length or the required time of the TCP movement route scheduled in the normal operation (Q1) or the like. Further, it may be determined that the end effector 2 is in a state in which the end effector 2 can contact the planned contact object, and the first threshold value TH1 may be set to a value larger than the current value. Of course, when it is determined that the end effector 2 is in a state in which it is possible to contact the planned contact object by the method described above, the stop control unit 32 may invalidate the detection of the collision based on the first diagnosis value HV1.

By the way, the state of the movable part may change gradually due to the collision. For example, in an operation in which the torques and angular velocities of the motors M1 to M6 are limited to a predetermined value or less (low power operation), when a movable part collides with a stationary obstacle, a fluctuation component of a diagnostic value due to the collision It is also conceivable that cannot pass through the high-pass filter. Therefore, the stop control part 32, together with the determination of a collision based on the first diagnostic value HV1 multiplied by the high-pass filter on the angular velocity difference (V _c -V _a), hung high pass filter on the angular velocity difference (V _c -V _a) A collision may be determined based on no diagnostic value. As a result, it is possible to reduce the possibility of a loose collision detection failure and to detect a sudden collision.

Note that the stop control unit 32 does not necessarily have to determine whether or not the detected speed deviates from the speed command based on the difference between the detected speed and the speed command. For example, the stop control unit 32 may determine that a collision has occurred when the value obtained by dividing the angular velocity command V _c by the detected angular velocity V _a is away from the reference value by 1 or more. Further, the stop control unit 32 may apply not only a high-pass filter but also a low-pass filter to the angular velocity deviation (V _c -V _a ) when high-frequency noise is mixed in the angular velocity deviation (V _c -V _a ). . Further, the above-described operations Q1 to Q9 are merely examples, and the drive control unit 31 may not perform any one of the operations Q1 to Q9, or may perform an operation other than the operations Q1 to Q9. Good.

  The robot 1 is not necessarily a 6-axis single-arm robot, and the number of joints of the arm A is not particularly limited. The robot 1 may be a double arm robot or a scalar robot. The force detector may not be the force sensor FS, but may be a torque sensor that detects torque acting on the joints J1 to J6 for each joint J1 to J6. Further, torque may be detected based on loads of the motors M1 to M6 instead of the torque sensor. The control device 3 may not be a device independent of the robot 1 but may be built in the robot 1. In this case, the drive control unit 31, the stop control unit 32, and the threshold setting unit 33 are provided in the robot 1.

DESCRIPTION OF SYMBOLS 1 ... Robot, 2 ... End effector, 3 ... Control apparatus, 31 ... Drive control part, 32 ... Stop control part, 33 ... Threshold setting part, A ... Arm, A1-A6 ... Arm member, d ... Virtual viscosity coefficient, D _a ... driving position, D _c ... control amount, D _e ... driving position deviation, D _t ... target drive position, E1 to E6 ... encoder, f ... force, f _S ... action force, f _S ... target force, FS ... force Satoru sensor, f _St ... target force, J1 to J6 ... joint, k ... virtual modulus, K _p ... position control gain, K _v ... speed control gain, m ... virtual inertia coefficient, M1-M6 ... motor, W ... workpiece , Δf _S ... force deviation, ΔS ... force-derived correction amount

Claims (10)

A robot control device for controlling a movable part of a robot,
In determining that a collision has occurred in the movable part when a diagnostic value indicating the state of the movable part is greater than a threshold,
A threshold setting unit configured to set the threshold to a value larger than that of the first operation in the second operation in which the limitation of the acceleration of the movable unit is relaxed compared to the first operation;
A robot control device comprising: When it is determined that a collision has occurred in the movable part, further comprising a stop control part for stopping the movable part,
The robot control apparatus according to claim 1. The first operation includes a normal operation for driving the movable part so that an acceleration does not become larger than an upper limit value.
The second operation includes an operation of allowing the acceleration to be larger than the upper limit value and driving the movable part.
The robot control apparatus according to claim 1 or 2. The second operation includes a singular point avoidance operation for avoiding a singular point of the movable part,
The robot control apparatus according to any one of claims 1 to 3. The first operation is:
A saturation operation in which the output of the drive unit that drives the movable unit is saturated;
A jog operation to move the movable part only during an operation period for the operation part;
A stop operation for stopping the movable part;
Including at least one of a tracking operation for causing the movable unit to follow the transfer device,
The robot control apparatus according to any one of claims 1 to 4. When the movable part is performing the third operation, it is not determined whether or not a collision has occurred in the movable part based on the magnitude of the diagnostic value.
The robot control apparatus according to any one of claims 1 to 5. The third operation is:
A brake releasing operation for releasing mechanical braking of the movable part;
Including at least one of a force control operation for controlling the movable part such that a force acting on the movable part becomes a target force.
The robot control device according to claim 6. The threshold value setting unit sets the threshold value to a larger value in an operation of controlling the movable unit in a state of being in contact with another object than in an operation of controlling the movable unit in a state of not being in contact with another object.
The robot control apparatus according to any one of claims 1 to 7.   The robot by which the said movable part is controlled by the robot control apparatus as described in any one of Claims 1-8. The robot control device according to any one of claims 1 to 8,
A robot whose movable part is controlled by the robot control device,
Robot system.

Images