JP7469587B2 - Control device for articulated robot and articulated robot - Google Patents

Description

The present invention relates to a control device for an articulated robot and an articulated robot.

Some articulated robots, which have joints that connect multiple movable parts (e.g., arm parts) in a series and drive parts such as servo motors provided at each joint, are configured to be able to detect when a movable part collides with an object such as a workpiece. For example, the articulated robot described in Patent Document 1 is configured to control the torque of the drive part that caused the collision so that it is at a level that can support the robot's own weight, that is, at a level that can maintain the robot's posture. In this way, by suppressing the torque generated after a collision is detected, it becomes possible to move the movable part by pushing it with one's hand. This is preferable in terms of reducing the workload when, for example, pulling the movable part away from an object to retrieve the object, and improving the convenience of the worker.

Japanese Patent No. 4240517

Here, in the configuration in which the torque generated after a collision is suppressed as described above, the movable part is likely to deviate significantly from its original trajectory if it is pushed forcefully during the above-mentioned work. This is expected to be a factor in increasing the time required to return the articulated robot to its posture before the collision. In addition, if the movable part is pushed by the worker and displaced significantly, a secondary collision between the movable part and other objects is likely to occur. This is not desirable in terms of improving the safety of the robot. Thus, there is still room for improvement in the configuration related to the collision countermeasures of the articulated robot in terms of improving convenience and safety.

The present invention was made in consideration of the above problems, and its main objective is to optimally improve the convenience and safety of articulated robots when colliding with objects.

The following describes the means to solve the above problems.

First Means: A control device applied to an articulated robot having a plurality of joints that connect a plurality of movable parts in series and a drive unit that drives each of the joints,
a collision detection unit that detects when the movable unit collides with an object;
a switching unit that switches, when the collision is detected by the collision detection unit, a control mode of the drive unit to a special control mode that generates a holding torque that balances with a torque loaded on each drive unit by the weight of the articulated robot and that can displace the movable unit by an external force;
a resistance generating unit that generates a resistance to suppress the rotation of the joint unit when the joint unit rotates under a condition in which the control mode of the drive unit is the special control mode;
a storage unit that, when the collision is detected by the collision detection unit, acquires and stores, for each of the joints, a physical quantity that changes in response to an influence of the collision or a correlation parameter that is a correlation value calculated from the physical quantity,
The magnitude of the resistance generated by the resistance generating unit is determined for each of the joints based on the correlation parameter.

According to the above configuration, when it is detected that the moving part of the robot has collided with an object such as a workpiece, the control mode of the drive unit becomes a special control mode. In this special control mode, the torque of each drive unit is controlled to an extent that the robot's own weight can be supported, i.e., the robot's posture can be maintained. By minimizing the generated torque and increasing the flexibility of each joint, the workload when moving the moving part by pushing it with a hand, for example, can be reduced. This is preferable for facilitating the recovery of an object such as a workpiece that is located (sandwiched) between a placement part such as a table and the robot. It is also preferable for protecting the object such as a workpiece, since it is possible to avoid continuous application of force to the object.

Here, when the flexibility of each joint is increased as described above, the deviation from the target trajectory may become large when the movable part is pushed vigorously by hand. In addition, the deviation from the trajectory may become large when the movable part is easily bounced upon collision. There is a concern that such deviation from the trajectory may cause a secondary collision between the robot and another object. In this regard, in the above configuration, when the movable part is displaced under a situation in which the special control mode is set, that is, when the joint part rotates, a resistance that suppresses the rotation is added. This reduces the deviation from the trajectory and suppresses the occurrence of the secondary collision. This is preferable in terms of improving the safety of the robot and protecting the robot, etc. In particular, in the above configuration, a correlation parameter indicating the degree of correlation with the collision (indicating the strength of the association) is stored for each joint, and the magnitude of the resistance is determined for each joint using the correlation parameter. In this way, by reflecting the degree of correlation with the collision in the magnitude of the resistance, it is possible to suitably suppress unnecessary movement of each joint when the movable part is pushed by hand.

When a collision occurs, the control mode of each drive unit becomes the special control mode, and the force of the collision can be absorbed at each joint. With this configuration, it is possible to suppress significant deviations in trajectory caused by a collision, compared to a configuration in which the force of the collision is absorbed by only one joint, for example. This is preferable for quickly returning the robot to its pre-collision state (trajectory).

In addition, in a special control mode, it is also possible to configure the drive unit to generate a holding torque in a stopped state by speed control that issues a speed command of zero speed.

Second means: The resistance generating unit is configured to make the resistance smaller when the degree of correlation with the collision represented by the correlation parameter is greater than a predetermined degree than the resistance when the degree is less than the predetermined degree.

The above configuration reduces resistance for joints with a high degree of correlation (strong correlation) with a collision, and increases resistance for joints with a low degree of correlation (weak correlation) with a collision. This configuration reduces the workload when an operator, etc., pushes the robot's moving part away from the collision target (object) by hand, while preventing the moving part from being significantly displaced in areas with a low correlation with the collision due to this work. Suppressing unnecessary displacement in this way is preferable, for example, for shortening the time required to return the robot to its pre-collision position (posture) after a collision occurs.

The above configuration can also be expressed as "the resistance is generated such that when the value of the correlation parameter indicates that the degree of correlation with the collision is relatively large, the resistance is relatively large, and when the value of the correlation parameter indicates that the degree of correlation with the collision is relatively small, the resistance is relatively small." Also, it can be expressed as "when the value of the correlation parameter indicates that the degree of correlation with the collision is relatively large, the resistance is smaller than when the degree of correlation with the collision is relatively small."

Third means: the correlation parameter includes a resistance coefficient used in determining the magnitude of the resistance in the resistance generating unit as the correlation value,
It is specified that the resistance increases as the resistance coefficient increases,
A lower limit is set for the resistance coefficient,
The resistance coefficient corresponding to at least the joint portion having the highest degree of correlation with the collision among the joint portions is the lower limit value.

The resistance coefficient corresponding to the joint that was the main cause of the collision (having the highest degree of correlation) is the lower limit value. This makes it possible to effectively reduce the workload of an operator, etc., when pulling the moving part of the robot away from the object of collision (by pushing it with their hands, etc.).

Fourth Means: The resistance generating unit is configured to control the driving unit so as to generate a resistance torque as the resistance in a direction opposite to a rotation direction of the joint unit,
Under a situation in which the control mode of the drive unit is the special control mode, the resistance torque is generated in the drive unit for the joint unit while the joint unit is rotating, and the holding torque is generated in the drive unit while the joint unit is stationary.

When the joint rotates, a resistance torque is generated in the opposite direction to the rotation direction, which makes it possible to effectively suppress displacement of the moving part due to the rebound immediately after a collision or the influence of external forces. In addition, by switching between the holding torque and resistance torque when the moving part is displaced and when it is not displaced, it is possible to increase the flexibility of the joint while suppressing the excessive displacement caused by the displacement.

Fifth means: The resistance torque is determined based on the rotation speed of the joint and the correlation parameter, and the higher the rotation speed, the larger the resistance torque.

By taking the rotation speed of the joint into account in the resistance torque generated, it is possible to prevent the movable part from being displaced excessively when the movable part is pushed forcefully by hand. In addition, when the torque generated is switched from the holding torque for maintaining posture to the resistance torque (when a collision is detected), the rotation speed of the joint is reduced. In other words, the resistance torque at the beginning of the switch can be kept relatively small. This makes it possible to effectively prevent disturbances in behavior (such as the occurrence of vibrations) caused by a sudden change in torque.

Sixth Means: An acquisition unit that acquires the physical quantity for each of the joints,
The collision detection unit is configured to detect that the robot has collided with an object based on the physical quantity acquired by the acquisition unit,
a calculation unit that calculates the correlation value based on the physical quantity acquired by the acquisition unit,
The storage unit is configured to store the correlation value calculated by the calculation unit as the correlation parameter.

If a correlation value (correlation parameter) is calculated from the physical quantity used for collision detection, that is, if the acquired physical quantity is used in both collision detection and resistance determination, it is possible to prevent the configuration related to information acquisition and processing from becoming complicated in order to realize the function of determining resistance according to the degree of correlation with a collision as described above.

Seventh means: An acquisition unit that acquires the physical quantity for each of the joints,
the collision detection unit is configured to detect that the robot has collided with an object when the physical quantity acquired by the acquisition unit exceeds a pre-stored threshold value for collision detection,
The apparatus further includes a calculation unit that calculates the correlation value, which is the correlation parameter, by comparing the physical quantity acquired by the acquisition unit with the threshold value.

If the correlation value is calculated using the physical quantity used for collision detection, i.e., if the physical quantity is used in combination, the configuration related to information acquisition and processing can be prevented from becoming complicated. In addition, using a threshold value for collision detection to calculate the correlation value can contribute to simplifying the configuration related to the correlation value calculation.

Eighth means: An acquisition unit that acquires the physical quantity for each of the joints,
The vehicle speed sensor further includes a calculation unit that calculates, when the collision occurs, the correlation value, which is the correlation parameter, based on the physical quantities before and after the collision.

If the correlation value is calculated using the physical quantity used in collision detection, that is, if the physical quantity is used in combination, the configuration related to information acquisition and processing can be prevented from becoming complicated. Note that the difference between the physical quantity before and after the collision can be larger as the correlation with the collision becomes stronger. Therefore, by calculating the correlation value based on the difference, the degree of correlation can be appropriately reflected in the magnitude of resistance.

9th means: An articulated robot equipped with the control device.

By using an articulated robot to which the above-mentioned control device is applied, the convenience and safety of the robot during a collision can be suitably improved.

1 is a schematic diagram of a robot system according to a first embodiment. FIG. 2 is a block diagram showing the electrical configuration of the robot system. Schematic diagram comparing the stopping modes of the joints. Schematic diagram showing behavior during special stopping. 13 is a flowchart showing a collision handling process. Schematic diagram showing a collision. Schematic diagram showing the change in current during a collision. FIG. 4 is a schematic diagram showing a flow of resistance torque calculation. FIG. 4 is a schematic diagram showing the relationship between a damper coefficient and a correlation value. Schematic diagrams illustrating differences in damping coefficients for each motor. FIG. FIG. 10 is a flowchart showing a collision handling process in the second embodiment. Schematic diagram showing the change in current during a collision. FIG. 4 is a schematic diagram illustrating an example of a stop control mode for each motor. FIG. 13 is a schematic diagram showing a flow of resistance torque calculation in the third embodiment. FIG. 13 is a schematic diagram showing a modified example relating to selection of a special stop mode.

First Embodiment
DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS A first embodiment will now be described with reference to the drawings. This embodiment is embodied in a robot system for an industrial robot that cooperates with humans in a machine assembly factory.

As shown in FIG. 1, the robot system 10 includes a robot 20 and a motion controller 70 for the robot 20. The robot 20 is composed of a robot body 30, which is a vertically articulated (six-axis) industrial robot, and a servo amplifier 50 attached to the robot body 30.

The robot body 30 has a base 31 fixed to the floor or the like, a shoulder 32 supported by the base 31, a lower arm 33 supported by the shoulder 32, a first upper arm 34 supported by the lower arm 33, a second upper arm 35 supported by the first upper arm 34, a wrist 36 supported by the second upper arm 35, and a flange 37 supported by the wrist 36.

The base portion 31 and the shoulder portion 32 are formed with a first joint J1 that connects the base portion 31 and the shoulder portion 32, and the shoulder portion 32 (first joint J1) can rotate horizontally around the connecting axis AX1 of the first joint J1. The shoulder portion 32 and the lower arm portion 33 are formed with a second joint J2 that connects the shoulder portion 32 and the lower arm portion 33, and the lower arm portion 33 (second joint J2) can rotate up and down around the connecting axis AX2 of the second joint J2. The lower arm portion 33 and the first upper arm portion 34 are formed with a third joint J3 that connects the lower arm portion 33 and the first upper arm portion 34, and the first upper arm portion 34 (third joint J3) can rotate up and down around the connecting axis AX3 of the third joint J3. The first upper arm portion 34 and the second upper arm portion 35 are formed with a fourth joint J4 that connects the first upper arm portion 34 and the second upper arm portion 35, and the second upper arm portion 35 (fourth joint J4) is rotatable in a twisting direction around a connecting axis AX4 of the fourth joint J4. The second upper arm portion 35 and the wrist portion 36 are formed with a fifth joint J5 that connects the second upper arm portion 35 and the wrist portion 36, and the wrist portion 36 (fifth joint J5) is rotatable in a vertical direction around a connecting axis AX5 of the fifth joint J5. The wrist portion 36 and the flange portion 37 are formed with a sixth joint J6 that connects the wrist portion 36 and the flange portion 37, and the flange portion 37 (sixth joint J6) is rotatable in a twisting direction around a connecting axis AX6 of the sixth joint J6. In this embodiment, the shoulder portion 32, the lower arm portion 33, the first upper arm portion 34, the second upper arm portion 35, the wrist portion 36, and the flange portion 37 correspond to the "movable portion."

The shoulder section 32, lower arm section 33, first upper arm section 34, second upper arm section 35, wrist section 36, and flange section 37 are arranged in a series to form an arm in the robot body 30, and an end effector (hand) is attached to the flange section 37, which is the tip of the arm. The connecting axes AX1, AX4, and AX6 are parallel to the longitudinal direction of the arm, and the connecting axes AX2, AX3, and AX5 are perpendicular to the longitudinal direction.

A motor 41 (specifically, a servo motor) is provided at each of the joints J1 to J6 as a drive unit for rotating the joints J1 to J6. The motor 41 is connected to a servo amplifier 50, and the control unit 51 of the servo amplifier 50 controls the drive of the motor 41 based on commands received from the motion controller 70 described above. In this embodiment, the servo amplifier 50 corresponds to the "control device."

Here, referring to FIG. 2, a supplementary explanation of the electrical configuration of the robot system 10 will be given. The motion controller 70 receives an operation instruction from the upper controller C, reads an operation program corresponding to the operation instruction from the program storage unit, and identifies an operation target position from the read operation program. Thereafter, it generates a target trajectory that smoothly connects the identified operation target position and the current position of the arm (each movable part) of the robot 20, and sequentially transmits interpolated positions, which are positions obtained by subdividing the target trajectory, to the servo amplifier 50.

The control unit 51 of the servo amplifier 50 is provided with a position control unit, a speed control unit, a current control unit, and a memory unit 52 that stores various information. The position control unit is connected to the encoder 42 attached to the motor 41. The position control unit detects the rotational position of the motor 41 (i.e., the position of the arm) based on the encoder value. The position control unit and the speed control unit calculate the target torque and target rotation speed of each motor 41 based on the deviation between the detected arm position and the interpolated position included in the command received from the motion controller 70. The current control unit determines the power (current, voltage, pulses) to be supplied to each motor 41 based on the calculated target torque and target rotation speed, and supplies power to each motor 41.

The robot 20 described above may collide with an object such as a person or a workpiece due to various factors. The robot 20 shown in this embodiment has a protection function that prevents failure or damage to the robot 20 or the workpiece by halting movement to the interpolation position and stopping the arm when such a collision occurs. The protection function will be described below with reference to Figures 3 and 4.

As shown in FIG. 3, the robot 20 (specifically, each of the joints J1 to J6) has a normal stop mode and a special stop mode, which differ in the method of pre-excitation of the motor 41.

The normal stop mode is a stop mode in which the position of the arm when it is stopped, more specifically the position of each movable part, is stored as a reference position, and position control is performed to prevent the arm from deviating from the reference position (so-called servo lock). In other words, when the arm is stationary at the reference position, the motor 41 generates a holding torque to maintain the reference position, and when the arm is displaced from the reference position or has been displaced, the motor 41 is controlled to generate a drive torque for rotation so as to return the arm to the reference position. In this embodiment, this control mode corresponds to the "predetermined control mode".

In contrast, the special stop mode is a stop mode in which a holding torque is generated in the motor 41 while stopped by performing speed control that gives a speed command of zero speed to the motor 41. The holding torque generated in the special stop mode is set to a magnitude that balances the torque loaded on each motor 41 by the weight of the arm. In this way, the torque generated in the motor 41 is minimized, thereby reducing the workload when the stopped arm is moved by pushing it with a hand, etc. Here, in the special stop mode, unlike the normal stop mode, if the arm is moved by pushing it with a hand while stopped, the arm will stop at the position after movement without returning to the reference position. In this embodiment, this control mode corresponds to the "special control mode".

When the command from the motion controller 70 is completed and the robot moves to a standby state waiting for the next command, the stopping mode is the normal stopping mode, and when the robot 20 is stopped due to the above-mentioned collision, the stopping mode is the special stopping mode.

In the special stopping mode, the torque generated by the motor 41 is minimized as described above, so there is a possibility that the momentum of the collision will cause the arm to bounce off the object of collision. For example, as shown in FIG. 4, when a collision occurs while the arm is moving from position Z2 to position Z1 and the arm is stopped in the special stopping mode, the driving torque that rotates the motor 41 is cut in response to the collision. Since the holding torque is weaker than the driving torque, it is difficult to completely suppress the movement of the arm using this holding torque, and the arm that has collided will bounce back to position Z2 due to inertia. At this time, the joint rotates in the opposite direction to before the collision.

When the arm is in the special stop mode and the worker changes the position (posture) of the arm by pushing it with his/her hand, the changed position (posture) is maintained. Therefore, for example, when retrieving a workpiece that is located (trapped) between the arm and the table, the retrieval operation can be performed quickly and easily by pushing the arm away with his/her hand. However, there is a concern that stopping the arm in the special stop mode in response to a collision may cause a large rebound at the time of the collision, which may lead to a secondary collision with another object. One of the features of this embodiment is that it is designed to prevent such inconveniences from occurring. Specifically, when the arm is displaced in the special stop mode, it has a damper function that generates resistance to suppress the displacement. Here, the configuration for exerting the damper function will be described.

As shown in FIG. 2, the encoder 42 attached to the motor 41 is connected to the control unit 51 of the servo amplifier 50. The control unit 51 can detect the rotational position (position of the arm, angle of the joint), rotational direction (displacement direction of the arm, rotation direction of the joint), and rotational speed (displacement speed of the arm, rotation speed of the joint) of the motor 41 based on the encoder value obtained from the encoder 42.

When the arm is displaced during the special stop mode, i.e., when the angle of the joint changes, the drive control of the motor 41 is performed so that a resistance torque is generated in the opposite direction to the rotation direction of the joint. This suppresses excessive bouncing. For example, in the example shown in FIG. 4, excessive bouncing is suppressed by generating a resistance torque in the position Z2 → position Z1 direction so as to cancel out the displacement of the arm from position Z1 → position Z2.

5 , a collision response process executed as part of regular processing (drive control process) in the control unit 51 of the servo amplifier 50 will be described. In the following description, the motors 41 attached to the first joint J1 are referred to as "motor M1", the motor 41 attached to the second joint J2 as "motor M2", the motor 41 attached to the third joint J3 as "motor M3", the motor 41 attached to the fourth joint J4 as "motor M4", the motor 41 attached to the fifth joint J5 as "motor M5", and the motor 41 attached to the sixth joint J6 as "motor M6", for appropriate distinction.

In the collision response process, first, in step S101, it is determined whether or not the robot 20 is in a special stop. If a negative determination is made in step S101, the process proceeds to step S102, where the collision monitoring process is executed. In the collision monitoring process, the current values of the currents supplied to the motors M1 to M6 are acquired from current sensors (current detection units) attached to the current control unit as physical quantities that change due to the effects of a collision, and these acquired current values are stored in the memory unit 52. Then, each current value is compared with a threshold value for collision determination to determine whether or not a collision has occurred. If none of the current values for the motors M1 to M6 exceed the threshold value, a negative determination is made in step S103, and the collision response process ends.

In the example shown in FIG. 6 (a) to FIG. 6 (b), the first joint J1 (motor M1), the fourth joint J4 (motor M4), and the sixth joint J6 (motor M6) are stopped in a normal stop mode, the second joint J2 (motor M2) and the third joint J3 (motor M3) rotate at a low speed, and the fifth joint J5 (motor M5) collides with the workpiece W during a swing-down motion in which the second joint J2, the third joint J3, and the fifth joint J5 rotate at a high speed. Although the second joint J2, the third joint J3, and the fifth joint J5 all rotate toward the workpiece W, the amount of change in the current value differs due to the difference in the operating speed. FIG. 7 shows the change in the amount of current before and after the collision occurs for each joint J1 to J6 (motors M1 to M6) in the example shown in FIG. 6. In this example, the current value of the motor M5 changes significantly at the time of the collision and exceeds the threshold value, and the collision is detected from the current value of the motor M5. That is, in this example, the movement of the fifth joint J5 (motor M5) is the main cause of the collision.

If any of the current values for motors M1 to M6 exceeds the threshold, a positive determination is made in step S103 and the process proceeds to step S104. In step S104, the current position of the arm at the time of the collision (more specifically, the current position of each movable part) is stored in memory unit 52 as a reference position.

In the next step S105, a correlation value calculation process is executed. In the correlation value calculation process, a correlation value indicating the degree of correlation with a collision is calculated for each joint J1 to J6 by referring to the current value at the time of collision stored in the memory unit 52 and the above-mentioned threshold value for collision determination (see FIG. 8). Specifically, the correlation value is calculated for each joint J1 to J6 by substituting each value into the formula: correlation value = (current value at the time of collision) / (threshold value for collision determination) x 100%. In other words, the larger the correlation value, the greater the degree of correlation with a collision.

After calculating the correlation value in step S105, the process proceeds to step S106, where a calculation process is performed for a damper coefficient (corresponding to a "resistance coefficient"), which is a coefficient for determining the magnitude of the resistance torque. Although details will be described later, in this embodiment, the resistance torque is determined based on the rotation speed (angular velocity) of the joint and the damper coefficient (see FIG. 8 ).

The damper coefficient has a set range (upper limit (maximum value) to lower limit (minimum value)), and is determined within this set range with reference to the correlation value. Specifically, as shown in FIG. 9, in the range of 0% to 100% correlation value, the damper coefficient decreases proportionally as the correlation value increases. In other words, the damper coefficient is regulated so that the value approaches the lower limit as the degree of correlation with the collision increases, and the value approaches the upper limit as the degree of correlation with the collision decreases. The upper limit (maximum value) of the damper coefficient is the largest value at which no vibration occurs in the joint at rest, and the lower limit (minimum value) of the damper coefficient is the lowest value at which the angle of the joint does not move beyond the allowable range after a strong collision. Incidentally, it is sufficient if the damper coefficient decreases as the correlation value increases, and the damper coefficient is not necessarily limited to the relationship in which the damper coefficient decreases proportionally as the correlation value increases.

6 and 7, for example, the correlation value for the fifth joint J5 (motor M5) is 100% or more, so the damper coefficient is set to the minimum value. Also, as shown in FIG. 10, the correlation values for the first joint J1 (motor M1), fourth joint J4 (motor M4), and sixth joint J6 (motor M6) are low, so the damper coefficients for the first joint J1 (motor M1), fourth joint J4 (motor M4), and sixth joint J6 (motor M6) are high, and the correlation values for the second joint J2 (motor M2) and third joint J3 (motor M3) are medium, so the damper coefficients for the second joint J2 (motor M2) and third joint J3 (motor M3) are also medium.

Returning to the description of the collision response process in Fig. 5 , after the damper coefficient is calculated in step S106, a damper activation process is executed in step S107. In the following step S108, a special stop process is executed for all of the joints J1 to J6. In the special stop process, the control mode of the motor 41 is switched to a special control mode, which is one of the stop control modes. This stops the output of the drive torque for moving the arm along the target trajectory, and each of the joints J1 to J6 is stopped in the special stop mode.
do.

Returning to the explanation of step S101, if a special stop is in progress, a positive determination is made in step S101 and the process proceeds to step S109. In step S109, it is determined whether or not a release condition for the special stop is satisfied. For example, the release condition is satisfied when a predetermined waiting period has elapsed since all of the joints J1 to J6 came to a complete stop after a collision occurred, or when at least one of the joints J1 to J6 has rotated beyond a preset angle.

If the special stop release condition is not met, a negative judgment is made in step S109 and the process proceeds to step S110. In step S110, an external force input monitoring process is executed. Specifically, it is monitored whether the rotation angle of each joint J1 to J6 has been forcibly changed by the arm being pushed by the operator, etc., and if the rotation angle has changed, that is, if a forced displacement due to an external force has occurred, the rotation speed (angular velocity) is detected. If a forced displacement due to an external force has not occurred (if displacement is not occurring), a negative judgment is made in step S111 and the collision response process ends. If a forced displacement has occurred (if displacement is occurring), a positive judgment is made in step S111 and the process proceeds to step S112.

In step S112, the resistance torque to be generated by the motor 41 is calculated based on the rotation speed (angular velocity) acquired in step S110 and the damper coefficient calculated in step S106. The generated resistance torque is calculated from the formula: resistance torque = rotation speed x damper coefficient. For this reason, for example, when the rotation speed is 0, no resistance torque is generated, and the resistance torque increases as the rotation speed increases. When the torque generated by the motor 41 during the special stop mode is switched from the holding torque for maintaining posture to the resistance torque (when a collision is detected), the rotation speed of the joint is reduced. In other words, the resistance torque at the beginning of the switch can be kept relatively small. This makes it possible to effectively suppress disturbances in behavior (such as the occurrence of vibrations) caused by a sudden change in torque.

After calculating the resistance torque in step S112, the damper control process is executed in step S113, and then the collision response process is terminated. The damper control process corresponds to a "resistance generating unit", and while the rotation angle of the joint is changing due to the damper control process, a resistance torque is generated in the opposite direction.

Returning to the explanation of step S109, if a positive determination is made in step S109, i.e., if the special stop release condition is met, the release process of steps S114 to S115 is executed, and then this collision response process is terminated. Specifically, first, damper disabling process is executed in step S114, and then operation resumption process is executed in step S115. In the operation resumption process, the control mode of the motors 41 is switched from the stop control mode (special control mode) to the drive control mode. After the switch, the drive of each motor 41 is controlled so that the arm returns to the reference position stored in step S104.

The first embodiment described above is expected to have the following excellent effects:

According to the above configuration, when it is detected that the arm of the robot 20 has collided with an object such as a workpiece W, the control mode of each motor 41 becomes a special control mode. In this special control mode, the torque of each motor 41 is controlled to an extent that the arm's own weight can be supported, i.e., the posture of the arm can be maintained. By minimizing the generated torque and increasing the flexibility of each joint J1 to J6, the workload when moving the arm by pushing it with a hand, for example, can be reduced. This is preferable in that it makes it easier to retrieve an object such as a workpiece W that is located (squeezed) between a placement portion such as a table and the arm.

Here, if the flexibility of each joint J1 to J6 is increased as described above, the deviation from the target trajectory may become large when the arm is pushed vigorously by hand. In addition, the arm may easily bounce back upon collision, which may cause a large deviation from the trajectory. There is a concern that such deviation from the trajectory may cause a secondary collision between the arm and another object. In this regard, in the above configuration, when the arm is displaced under a special control state, that is, when the joints J1 to J6 rotate, a resistance torque is added to suppress the rotation. Assuming that the above-mentioned damper function is OFF or that the function is not provided, when the arm collides with the workpiece W and bounces back as shown in Figure 11 (a) → Figure 11 (b), no opposite resistance torque is generated to suppress the bounce. For this reason, it is assumed that the width of the bounce of the arm will be large. On the other hand, if the damper function is ON, when the arm collides with the workpiece W and bounces back as shown in Figure 12 (a) → Figure 12 (b), a resistance torque in the opposite direction to suppress the bounce is generated. As a result, the bounce width of the arm is smaller than when the damper function is OFF. This reduces deviation from the trajectory and suppresses the occurrence of the secondary collision mentioned above. This is desirable for improving the safety of the robot and protecting the robot, etc.

Here, in a configuration in which resistance torques are generated to suppress displacement during the special stop mode as described above, the following inconveniences may occur if all resistance torques are set uniformly. For example, if all resistance torques are set low, the joints may rotate in a manner different from the operator's intention, making it difficult to change the posture to the desired posture, and the effect of improving workability may not be achieved. In addition, excessive posture changes may occur, resulting in adverse effects such as an increase in the time required for the return operation to the reference position. In contrast, if all resistance torques are set high uniformly, the operating force required to change the posture by pushing with the hand, etc., may increase, greatly reducing the convenience for the operator.

In this respect, in the present embodiment, a correlation value indicating the degree of correlation with a collision is calculated for each joint, and a damper coefficient is calculated from the correlation value. By determining the magnitude of the resistance torque using the damper coefficient for each joint, the degree of correlation with a collision can be reflected in the magnitude of the resistance. In other words, the resistance is small for joints with a large degree of correlation with a collision (strong correlation), and the resistance is large for joints with a small degree of correlation with a collision (weak correlation). With this configuration, the workload of an operator, etc., when pulling the movable part of the robot away from the object of collision (object) by pushing it with his/her hand, can be reduced, while the movable part can be prevented from being significantly displaced in a part with a weak correlation with a collision due to the work. Suppressing unnecessary displacement in this way is also preferable in terms of shortening the time required to return the robot to its position (posture) before the collision after the occurrence of a collision.

When a collision occurs, the control mode of each drive unit becomes the special control mode described above, and the force of the collision can be absorbed by each of the joints J1 to J6. With this configuration, it is possible to prevent significant deviations in the trajectory due to a collision, compared to a configuration in which the force of the collision is absorbed by only one joint, for example. This is preferable for quickly returning the robot to its pre-collision state (trajectory).

For the joint that caused the collision detection, when the correlation value reaches 100%, the damper coefficient is set to the lower limit, and even if the correlation value exceeds 100%, the resistance torque is prevented from becoming extremely small and effectively becoming non-functional.

Second Embodiment
In the first embodiment, when the arm collides with an object such as a workpiece, all of the joints J1 to J6 are stopped in a special stopping mode regardless of the degree of correlation with the collision. In this embodiment, the configuration is different from the first embodiment in that when the arm collides with an object such as a workpiece, the target to be stopped in a special stopping mode is determined based on the degree of correlation with the collision. Below, the characteristic configuration of this embodiment will be described with reference to FIG. 13, focusing on the differences from the first embodiment.

In the collision response process in this embodiment, in step S201, it is determined whether or not at least any joint in the robot 20 is in a special stop. If the determination in step S201 is negative, the process proceeds to step S202, where the collision monitoring process is executed. In the collision monitoring process, the current values of the currents supplied to the motors M1 to M6 are acquired from the current sensors (current detection units) attached to the current control unit, and the acquired current values are stored in the storage unit 52. Then, each current value is compared with a first threshold value for collision determination to determine whether or not a collision has occurred. If none of the current values related to the motors M1 to M6 exceeds the threshold value, a negative determination is made in step S203 , and the collision response process is terminated. If any of the current values related to the motors M1 to M6 has reached the threshold value, a positive determination is made in step S203, and the process proceeds to step S204. In step S204, the position of the arm at the time of the collision (current position) is stored in the storage unit 52 as a reference position.

In the next step S205, a correlation value calculation process is executed. In the correlation value calculation process, a correlation value indicating the degree of correlation with a collision is calculated for each joint J1 to J6 by referring to the current value at the time of collision stored in the memory unit 52 and the above-mentioned collision determination threshold value. After the correlation value is calculated in step S205, the process proceeds to step S206 to determine the target of special stop, that is, the joint to be stopped in the special stop mode (motor 41 to be switched to the special control mode). Specifically, the joint whose correlation value calculated in step S205 is equal to or greater than the reference value (80%) is the target of special stop.

For example, when the first joint J1 (motor M1), the fourth joint J4 (motor M4), and the sixth joint J6 (motor M6) are stopped in a normal stop mode, the second joint J2 (motor M2) and the third joint J3 (motor M3) rotate at a low speed, and the fifth joint J5 (motor M5) rotates at a high speed, during a swing-down motion, the second joint J2, the third joint J3, and the fifth joint J5 all rotate toward the work W, but the amount of change in the current value differs due to the difference in the operating speed. In this example, as shown in FIG. 14, the current value of the motor M5 changes significantly upon collision and exceeds the threshold value, and the collision is detected from the current value of the motor M5. In other words, in this example, the movement of the fifth joint J5 (motor M5) is the main cause of the collision. Here, since the correlation value calculated from the current value of the motor M2 and the correlation value calculated from the current value of the motor M3 exceed the above-mentioned reference value, the second joint J2 (motor M2) and the third joint J3 (motor M3) are subject to the special stop as well as the fifth joint J5 (motor M5 ). On the other hand, since the correlation value calculated from the current value of the motor M1, the correlation value calculated from the current value of the motor M4, and the correlation value calculated from the current value of the motor M6 do not exceed the above-mentioned reference value, the first joint J1 (motor M1), the fourth joint J4 (motor M4), and the sixth joint J6 (motor M6) are not subject to the special stop and are stopped in the normal stop mode.

Returning to the explanation of FIG. 13, after the target of the special stop is determined in step S206, the process proceeds to step S207, where the calculation process of the damper coefficient for the target of the special stop is executed. In other words, the damper coefficient is calculated only for the target of the special stop in step S206. For example, in the example shown in FIG. 14, the correlation value is high for the fifth joint J5 (motor M5), which was the main cause of the collision (triggered the detection of the collision). Therefore, the damper coefficient is kept low. In contrast, the correlation values for the second joint J2 (motor M2) and the third joint J3 (motor M3) are medium, so the damper coefficients are also medium (see FIG. 15).

After the damper coefficient is calculated in step S207, a damper activation process is executed in step S208. In the following step S209, a special stop process is executed for the motor 41 of the joint determined in step S206, and a normal stop process is executed for the other motors 41. In the special stop process, the control mode of the motor 41 is switched to a special control mode, which is one of the stop control modes. In the normal stop process, the control mode of the motor 41 is switched to a normal control mode, which is one of the stop control modes . Note that the stop mode of the joint in which the motor 41 is stopped by the normal stop control becomes the normal stop mode, and if a deviation from the reference position occurs, the joint operates to eliminate the deviation.

Returning to the explanation of step S201, if a positive judgment is made in step S201, the process proceeds to step S210. In step S210, it is judged whether or not the conditions for canceling the special stop are met. If the conditions for canceling the special stop are not met, a negative judgment is made in step S210 and the process proceeds to step S211. In step S211, an external force input monitoring process is executed. If a forced displacement due to an external force has not occurred (if displacement is not occurring), a negative judgment is made in step S212 and the collision response process is terminated. If a forced displacement due to an external force has occurred (if displacement is occurring), a positive judgment is made in step S212 and the process proceeds to step S213.

In step S213, it is determined whether forced displacement is occurring in the joint that is the target of the special stop. If a positive determination is made in step S213, the process proceeds to step S214, where the resistance torque to be generated by the motor 41 is calculated based on the rotation speed (angular velocity) acquired in step S211 and the damper coefficient calculated in step S207. Thereafter, in step S215, a damper control process is executed, and this collision response process is terminated. While the rotation angle of the joint is changing due to the damper control process, a resistance torque is generated in the opposite direction.

Returning to the explanation of step S213, if a negative determination is made in step S213, i.e., if a forced displacement occurs in a joint that is normally stopped, the process proceeds to step S216. In step S216, a return process to the reference position is executed. In this return process, a drive torque is generated in the motor 41 to return the joint to the reference position.

Returning to the explanation of step S210, if a positive determination is made in step S210, i.e., if the conditions for canceling the special stop are met, the cancellation process of steps S217 to S218 is executed, and then the collision response process is terminated.

When a collision is detected, the control mode of some of the motors 41 is switched to a special control mode, which is one of the stop control modes, and the control mode of the remaining motors 41 is switched to a normal control mode, which is also one of the stop control modes. In the normal control mode, unlike the special control mode, the movable part is controlled to remain in the reference position (pulled back if displaced). With this configuration, while it is possible to change the robot's posture by pushing it with a hand, etc., it is possible to prevent the force from being transmitted to a drive part or the like that is not effective in pulling the robot away from an object, causing the robot to assume an unintended posture. This makes it possible to preferably reduce the possibility of the secondary collision described above.

Setting a motor 41 that is not correlated with a collision to a special control mode is undesirable because it can cause ineffective posture changes in terms of workability. On the other hand, restricting the special control mode to only the motor 41 that caused the collision detection is an obstacle to improving workability and safety. In this regard, if the reference value (corresponding to the "decision reference condition") that is the selection condition for the target of the special stop is made easier to reach than the threshold value (corresponding to the "detection reference condition") that is the condition for collision detection as described above, it becomes possible to set not only the motor 41 that caused the collision detection but also other motors 41 that are correlated with the collision to a special control mode, and the various inconveniences described above can be suitably eliminated.

In addition, by making at least the motor 41 (joint) that caused the collision the target for switching to a special control mode, it is possible to make the movement of the joint smoother when moving the arm by hand, etc., away from an object such as a workpiece.

Third Embodiment
In the first and second embodiments, a correlation value indicating the degree of correlation with a collision is calculated based on a current value acquired from a current detection unit and a threshold value for collision determination (see FIG. 8). In this embodiment, the configuration related to the calculation of the correlation value is different from that of the first embodiment. Hereinafter, the configuration related to the calculation of the correlation value in this embodiment will be described with reference to FIG. 16.

The control unit 51 of the servo amplifier 50 acquires the current value from the current detection unit each time the drive control process (periodic process) is executed. The memory unit 52 of the servo amplifier 50 is provided with a memory area capable of storing multiple current values in chronological order, and the most recent acquired current value is updated by overwriting the oldest current value.

If the collision occurs, the correlation value is calculated by referencing the current value at the time of collision and the current value immediately before the collision acquired earlier (for example, one cycle earlier). Specifically, the correlation value is calculated by substituting each current value into the following formula: correlation value = (1 - (current value at the time of collision) / (current value immediately before collision)) x 100%.

The difference in current value before and after a collision can be greater as the degree of correlation with the collision increases. Therefore, by focusing on this difference and calculating the correlation value, the degree of correlation can be appropriately reflected in the magnitude of resistance.

<Other embodiments>
The present invention is not limited to the above-described embodiment, and may be implemented, for example, as follows. Each of the following configurations may be applied individually to the above-described embodiment, or may be applied to the above-described embodiment in combination in part or in whole. It is also possible to arbitrarily combine all or part of the various configurations shown in the above-described embodiment. In this case, it is preferable that the technical significance (effects to be exerted) of each of the configurations to be combined is guaranteed.

- In the above embodiment, a correlation value indicating the degree of correlation with a collision is calculated, and the resistance torque to be generated in the motor 41 is determined using the damper coefficient calculated from the correlation value, but this is not limited to the above, and the resistance torque to be generated in the motor 41 may be determined directly from the correlation value without going through the damper coefficient. However, in order to prevent the resistance torque from becoming excessively large or small, it is technically significant to calculate a damper coefficient for which at least one of an upper limit and a lower limit is set in advance, and to determine the resistance torque using the damper coefficient.

- In the above embodiment, when the arm moves in a special stop state, the resistance torque to be generated by the motor 41 is determined by multiplying the angular velocity of the joint by the damper coefficient. In other words, the resistance torque changes according to the rotation speed of the joint. This may be modified as follows. In other words, the resistance torque to be generated by the motor 41 when the arm moves in a special stop state may be configured to be constant and not change according to the rotation speed.

- In each of the above embodiments, when the special stop mode is activated, specifically when the angle of the joint is being changed by an external force, the motor 41 generates a resistance torque to suppress the change in angle, but the specific configuration for generating the resistance that suppresses the change in angle is arbitrary. For example, each of the joints J1 to J6 may be provided with a braking mechanism such as an electromagnetic brake that can suppress the rotation of the joints J1 to J6, and the change in angle may be suppressed by the braking force of the braking mechanism. In this case, it is desirable to make it possible to change the braking force by the control unit 51.

In each of the above embodiments, the damper coefficient is configured to bottom out at a preset lower limit ( minimum value) when the correlation value reaches 100%, but it is also possible to eliminate such a lower limit .

-When calculating the presence or absence of a collision and the correlation value based on the acquired current value, the configuration may be such that the presence or absence of a collision is determined and the correlation value is calculated based on the deviation between a previously estimated current value and the actual current value.

The rotation speed (speed value) of each joint may be obtained from a speed sensor or encoder 42, and collision monitoring and correlation value calculation may be performed based on the obtained speed value. The drive torque (torque value) of each joint may be obtained from a torque sensor, and collision monitoring and correlation value calculation may be performed based on the obtained torque value.

- In each of the above embodiments, the current value is used as the "physical quantity" common to both collision monitoring and correlation value calculation, but the "physical quantity" used for collision monitoring and the "physical quantity" used for correlation value calculation may be set separately. For example, a configuration may be used in which the torque value is referenced as the "physical quantity" for collision monitoring, while the current value is referenced as the "physical quantity" for correlation value calculation.

- The damper coefficient may be specified to be the maximum value when the correlation value indicating the degree of correlation with a collision is equal to or less than a predetermined value (e.g., 10% or less). This is preferable in terms of suppressing unnecessary posture changes by reducing the resistance torque caused by the effects of current noise, etc.

- In the first embodiment described above, all of the joints J1 to J6 are configured to switch to the special stop mode when a collision occurs, but it is also possible to configure some of the joints (for example, the joints J1 to J3 that are the basic axes) to be excluded from switching to the special stop mode. In this case, it is preferable to interrupt the movement toward the target trajectory for the joints that are not targeted for switching to the special stop mode, but the stop mode of these joints may be, for example, the normal stop mode described above.

- In the second embodiment described above, the object to be switched to the special control mode is determined by comparing the correlation value with a reference value for the correlation value. However, it is also possible to change the object to be compared from the correlation value to the current value, and to determine the object to be switched to the special control mode by comparing the current value with a reference value for the current value.

- In the second embodiment described above, any correlation value exceeding the reference value (80%) is configured to be subject to switching to a special stop mode, but the reference value used to determine which joints J1 to J6 are subject to switching may be set individually depending on their positional relationship with the joint that caused the collision detection.

For example, in the example shown in FIG. 17, a threshold value is provided as a criterion for detecting a collision, and a reference value is provided as a selection criterion for selecting a target for switching to the special stop mode that is easier to reach than the threshold value. When a collision occurs during a downward movement of the second joint J2 (low speed), the third joint J3 (high speed), and the fifth joint J5 (low speed), the current value of the current supplied to the motor M3 for the third joint J3 changes significantly and exceeds the threshold value. This causes a collision to be detected. Here, the determination of whether or not to select the joints J1 to J2 and J4 to J6 other than the third joint J3 as targets for switching to the special stop mode differs between the fourth joint J4 to the sixth joint J6 located at the tip side of the arm relative to the third joint J3 and the first joint J1 to the second joint J2 located at the base end side of the arm relative to the third joint J3. Specifically, for the fourth joint J4 to the sixth joint J6, it is determined whether or not the correlation value exceeds a reference value B, and for the first joint J1 to the second joint J2, it is determined whether or not the correlation value exceeds a reference value A. The reference value B is lower than the reference value A, and therefore these joints are more likely to be selected as targets for switching to the special stop mode.

A configuration that increases the flexibility of the joints when a collision occurs is preferable when moving the arm by pushing it with a hand, etc. Here, by making it easier for the joints (drive units) on the arm tip side than the joints (drive units) that caused the collision (main cause) to switch to the special control mode than the joints (drive units) on the arm base end side, in other words, by giving priority to switching the joints (drive units) on the arm tip side to the special stop mode, it is possible to reduce the number of moving parts that move as one unit when the arm is pushed by a hand. This is preferable for reducing the load when moving the arm by hand.

- In the second embodiment described above, those whose correlation value has reached a reference value are subject to switching to the special control mode, but this may be modified as follows. That is, the correlation values of joints other than the joint that caused the collision detection may be compared, and the joint with the largest correlation value among those correlation values may be included as a subject to switching to the special control mode.

- In the second embodiment described above, the resistance torque for the motor 41 that is the target of switching to the special control mode is determined based on a damper coefficient set for each joint (motor), but it is also possible to omit this function and configure the resistance torque not to be differentiated for any joint (motor).

10... robot system, 20... robot, 30... robot body, 32... shoulder section (movable section), 33... lower arm section (movable section), 34... first upper arm section (movable section), 35... second upper arm section (movable section), 36... wrist section (movable section), 37... flange section (movable section), 41, M1 to M6... motors (drive sections), 50... servo amplifier (control device), 51... control section, 52... memory section, J1 to J6... joint sections.

Claims (9)

A control device applied to an articulated robot having a plurality of joints that connect a plurality of movable parts in a series and a drive unit that drives each of the joints,
a collision detection unit that detects when the movable unit collides with an object;
a switching unit that switches, when the collision is detected by the collision detection unit, a control mode of the drive unit to a special control mode that generates a holding torque that balances with a torque loaded on each drive unit by the weight of the articulated robot and that can displace the movable unit by an external force;
a resistance generating unit that generates a resistance to suppress the rotation of the joint unit when the joint unit rotates under a condition in which the control mode of the drive unit is the special control mode;
a storage unit that, when the collision is detected by the collision detection unit, acquires and stores, for each of the joints, a physical quantity that changes in response to an influence of the collision or a correlation parameter that is a correlation value calculated from the physical quantity,
A control device in which the magnitude of the resistance generated by the resistance generating unit is determined for each joint unit based on the correlation parameter. The control device according to claim 1, wherein the resistance generating unit is configured to make the resistance smaller when the degree of correlation with the collision represented by the correlation parameter is greater than a predetermined degree than when the degree is less than the predetermined degree. the correlation parameter includes a resistance coefficient used in determining the magnitude of the resistance in the resistance generating unit as the correlation value,
It is specified that the resistance increases as the resistance coefficient increases,
A lower limit is set for the resistance coefficient,
3. The control device according to claim 1, wherein the resistance coefficient corresponding to at least one of the joints having the highest degree of correlation with the collision is set to the lower limit value. the resistance generating unit is configured to control the driving unit so as to generate, as the resistance, a resistance torque in a direction opposite to a rotation direction of the joint unit,
4. The control device according to claim 1, wherein, under a situation in which the control mode of the drive unit is the special control mode, the resistance torque is generated in the drive unit for the joint unit while the joint unit is rotating, and the holding torque is generated in the drive unit while the joint unit is stationary. The control device according to claim 4, wherein the resistance torque is determined based on the rotation speed of the joint and the correlation parameter, and the higher the rotation speed, the larger the resistance torque. an acquisition unit that acquires the physical quantity for each of the joints,
The collision detection unit is configured to detect that the robot has collided with an object based on the physical quantity acquired by the acquisition unit,
a calculation unit that calculates the correlation value based on the physical quantity acquired by the acquisition unit,
The control device according to claim 1 , wherein the storage unit is configured to store the correlation value calculated by the calculation unit as the correlation parameter. an acquisition unit that acquires the physical quantity for each of the joints,
the collision detection unit is configured to detect that the robot has collided with an object when the physical quantity acquired by the acquisition unit exceeds a pre-stored threshold value for collision detection,
7. The control device according to claim 1, further comprising a calculation unit that calculates the correlation value, which is the correlation parameter, by comparing the physical amount acquired by the acquisition unit with the threshold value. an acquisition unit that acquires the physical quantity for each of the joints,
7. The control device according to claim 1 , further comprising a calculation unit that calculates, when the collision occurs, the correlation value, which is the correlation parameter, based on the physical quantities before and after the collision. An articulated robot equipped with a control device according to any one of claims 1 to 8.

Images