JP2024006697A - robot control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control device which causes a manipulator to stop by properly restoring it into a before-contact-state, so that external force applied to the manipulator is released according to a posture of the manipulator at the time of contact between the manipulator and an operator or the like.

SOLUTION: A robot control device 100 includes: a collision detection part 110 for detecting collision of a manipulator 200; an external force calculation part 120 for calculating an external force Fp applied to the manipulator 200; a spring constant calculation part 140 for calculating a spring constant k on the basis of a posture of the manipulator 200; a movement amount calculation part 150 for calculating a movement amount L of the manipulator on the basis of the external force Fp and the spring constant k; a restoring time calculation part 160 for calculating a restoring time T, on the basis of the movement amount L and a speed V at the time of collision detection of the manipulator 200; and a manipulator restoration part 170 for restoring the manipulator 200 to its position posture assumed earlier by the restoring time T than the collision detection of the manipulator 200.

SELECTED DRAWING: Figure 2

COPYRIGHT: (C)2024,JPO&INPIT

Description

The present invention relates to a robot control device.

In recent years, many robots have become widespread in industry. The robots are used, for example, to assemble, weld, and transport electronic and mechanical parts, and to improve the efficiency and automation of factory production lines.

Further, the development of collaborative robots is progressing, in which a robot performs part of the work that was conventionally performed by a worker, instead of or together with the worker, while standing next to the worker. Collaborative robots do not work in a safely enclosed area, but work next to a worker, so it is important to ensure the safety of the worker.

For example, when a collaborative robot comes into contact with a worker, etc., the collaborative robot is quickly stopped, but at this time, the collaborative robot is returned to its pre-contact state to release the external force applied to the collaborative robot during the contact. It is preferable to stop it after returning it.

In Patent Document 1, when an external force applied to the robot is detected, the robot's motion trajectory is reversed based on a preset movement distance and movement time, and the robot is evacuated so that the applied external force is reduced. A robot system has been disclosed that allows the robot to perform various tasks.

JP2019-81234A

However, depending on the posture of the robot at the time of contact between the robot and a worker, the external force applied to the robot may vary, and the distance traveled after contact may vary, but the robot system disclosed in Patent Document 1 When an external force applied to the robot is detected, the robot's motion trajectory is reversed based on a preset travel distance and travel time. In other words, in the robot system disclosed in Patent Document 1, the posture of the robot at the time of contact between the robot and the worker is not taken into account, and the external force applied to the robot is released and the contact is appropriately controlled. The problem is that it is not possible to return to the previous state.

Therefore, the present invention aims to appropriately return the manipulator to the state before the contact and stop the manipulator so as to release the external force applied to the manipulator according to the posture of the manipulator when the manipulator contacts the worker, etc. The purpose of the present invention is to provide a robot control device that allows

A robot control device according to one aspect of the present invention is a robot control device that controls a manipulator, and includes a collision detection unit that detects a collision of the manipulator, and an external force calculation unit that calculates an external force applied to the manipulator when a collision of the manipulator is detected. a spring constant calculation unit that calculates a spring constant based on the posture of the manipulator when a collision of the manipulator is detected; a movement amount calculation unit that calculates the amount of movement of the manipulator based on the external force and the spring constant; The present invention includes a return time calculation unit that calculates a return time based on the speed at the time of collision detection, and a manipulator return unit that returns the manipulator to the position and orientation from the time of collision detection to the position and orientation before the return time.

According to this aspect, the movement amount calculation unit is based on the external force applied to the manipulator when the collision of the manipulator is detected, which is calculated by the external force calculation unit, and the attitude of the manipulator when the collision of the manipulator is detected, which is calculated by the spring constant calculation unit. The amount of movement of the manipulator is calculated based on the spring constant. The return time calculation unit calculates the return time based on the amount of movement of the manipulator and the speed of the manipulator at the time of collision detection, and the manipulator return unit returns the manipulator to the position and orientation from the time of detection of the collision of the manipulator to the position and orientation before the return time. Return. Thereby, it is possible to appropriately return the manipulator to the state before the collision is detected, so as to release the external force applied to the manipulator according to the attitude of the manipulator when the collision of the manipulator is detected. That is, the manipulator can be stopped while the external force applied to the manipulator is released.

The above aspect further includes a displacement calculation unit that calculates the displacement amount of the manipulator when the manipulator collision is detected, and the spring constant is calculated based on the external force and the displacement amount of the manipulator when the manipulator collision is detected. may include a spring constant.

According to this aspect, the spring constant of the manipulator is calculated based on the external force and the displacement amount of the manipulator calculated by the displacement amount calculating section, and the spring constant includes the spring constant of the manipulator. It is possible to appropriately calculate a spring constant according to the posture of the user. As a result, it is possible to more appropriately return the manipulator to the state before the collision was detected, so as to release the external force applied to the manipulator.

In the above aspect, the spring constant may include a spring constant of a collided portion at the time of detecting a collision of the manipulator.

According to this aspect, since the spring constant includes the spring constant of the collided part when the collision of the manipulator is detected, it is possible to appropriately calculate the spring constant according to the collided part. As a result, it is possible to more appropriately return the manipulator to the state before the collision was detected, taking into consideration the collision site and releasing the external force applied to the manipulator.

In the above aspect, the displacement amount calculation unit may calculate the displacement amount of the manipulator based on the joint angle, joint torque, and joint spring constant in each axis of the manipulator.

According to this aspect, since the displacement amount is calculated from each axis of the manipulator, it is possible to more appropriately calculate the spring constant according to the posture of the manipulator.

In the above aspect, the manipulator return unit may return the manipulator to the position and orientation before the return time after the collision of the manipulator is detected at a speed lower than the speed when the collision of the manipulator is detected.

According to this aspect, the manipulator return unit returns the manipulator slowly after detecting a collision with the manipulator, so that, for example, in the case of a collision with a worker, when returning the manipulator, the manipulator is returned in consideration of the safety of the worker. can be returned.

In the above aspect, the spring constant of the collided part at the time of detecting the collision of the manipulator may be set by estimating in advance the collided part that has a possibility of colliding with the manipulator.

According to this aspect, the manipulator estimates the object to be collided with and sets the spring constant of the part of the object to be collided with, so that the spring constant can be set more accurately according to the situation. can be calculated.

According to the present invention, the manipulator is appropriately returned to the state before the contact and stopped in order to release the external force applied to the manipulator according to the posture of the manipulator at the time of contact between the manipulator and the worker. It is possible to provide a robot control device that allows

1 is a schematic diagram showing the configuration of a robot system 10 according to an embodiment of the present invention. 1 is a functional block diagram showing each function in a robot control device 100 according to an embodiment of the present invention. FIG. FIG. 2 is a diagram schematically showing a collision system that shows a collision between a manipulator 200 and a collided object 20 that collides with the manipulator 200. FIG. FIG. 2 is a schematic diagram for explaining the configuration of the movement amount L of the manipulator 200 in more detail. FIG. 3 is a schematic diagram for explaining the displacement amount ΔP of the manipulator 200 in a collision between the manipulator 200 and the object 20 using a single axis model. FIG. 3 is a diagram showing a transition (A) of an external force applied to a point P on the manipulator 200 and a transition (B) of the position of the manipulator 200. It is a flowchart which shows the flow of processing of robot control method M100 which controls manipulator 200 performed by robot control device 100 concerning one embodiment of the present invention.

Embodiments of the present invention will be specifically described below with reference to the drawings. In addition, the embodiment described below is merely a specific example for implementing the present invention, and is not intended to be interpreted in a limited manner. Furthermore, in order to facilitate understanding of the explanation, the same components in each drawing may be given the same reference numerals as much as possible, and redundant explanation may be omitted.

<One embodiment>
[Robot system configuration]
FIG. 1 is a schematic diagram showing the configuration of a robot system 10 according to an embodiment of the present invention. As shown in FIG. 1, the robot system 10 includes a robot control device 100 and a manipulator 200.

The manipulator 200 is, for example, an industrial robot equipped with a welding torch, a robot hand, or the like as an end effector at the tip of an arm, and here, a 6-axis manipulator will be described as an example. The manipulator 200 uses a motor to rotate each axis 201 to 206 based on an operation instruction from the robot control device 100 to move and rotate the arm, wrist, etc., and move the end effector to an appropriate position. Move to position and angle.

Furthermore, the manipulator 200 detects the torque applied to each of the axes 201 to 206 using a torque sensor, and notifies the robot control device 100 of the detected torque.

Robot control device 100 controls the operation of manipulator 200. Specifically, the robot control device 100 controls the motors in each of the axes 201 to 206 of the manipulator 200 to rotate each of the axes 201 to 206, thereby moving and rotating the arm and wrist. Thereby, the robot control device 100 controls the manipulator 200 so that it assumes an appropriate position and orientation.

Further, the robot control device 100 determines a collision of the manipulator 200 based on the torque applied to each axis 201 to 206 of the manipulator 200, and as a process at the time of the collision, returns the position and orientation of the manipulator 200, control to stop.

[About processing when a manipulator collides with the robot control device]
Next, the processing of the robot control device 100 when the manipulator 200 collides with an object to be collided will be described in detail. Note that a collision of the manipulator 200 is determined based on the torque applied to each of the axes 201 to 206 of the manipulator 200, but it is determined based on the torque applied to each of the axes 201 to 206 of the manipulator 200. This includes those caused by collisions. That is, a collision with a peripheral device such as an end effector affects the torque applied to each of the axes 201 to 206 of the manipulator 200, and is preferably determined as a collision of the manipulator 200 based on the torque.

FIG. 2 is a functional block diagram showing each function of the robot control device 100 according to an embodiment of the present invention. As shown in FIG. 2, the robot control device 100 includes a current position storage section 110, an external force calculation section 120, a collision detection section 130, a spring constant calculation section 140, a movement amount calculation section 150, and a return time calculation section. 160 and a manipulator return section 170. As described above, the robot control device 100 stores various data for controlling the operation of the manipulator 200 and has many functions. Here, the robot control device 100 mainly includes the following functions. The processing (function) of the manipulator 200 at the time of a collision will be explained.

The current position storage unit 110 stores the current position and orientation of the manipulator 200. For example, the current position storage unit 110 may store the position and orientation of the manipulator 200 based on the joint angles of the respective axes 201 to 206 of the manipulator 200 at that time.

The collision detection unit 130 detects a collision of the manipulator 200. For example, the collision detection unit 130 calculates the external force applied to the manipulator 200 by the external force calculation unit 120 based on the torque applied to each axis 201 to 206 detected by the torque sensor on each axis 201 to 206 of the manipulator 200. and detects that the manipulator 200 has collided. Specifically, in the manipulator 200 controlled by the robot control device 100, when the torque of each axis 201 to 206 becomes an abnormal value with respect to the value based on the control, the collision detection unit 130 detects that the manipulator 200 It is sufficient to detect that a collision has occurred.

FIG. 3 is a diagram schematically showing a collision system in which the manipulator 200 and the object 20 that collides with the manipulator 200 collide. In FIG. 3, the manipulator 200 and the collided object 20 are colliding, and the states of (A) at the start of the collision (at the start of contact) and (B) at the time of collision detection (balanced state) are schematically shown. There is.

The manipulator 200 includes a tip portion 210 that is in contact with the object 20 to be collided with, and an element 220 that has elasticity due to each joint of the manipulator. For example, when a robot hand is attached to the manipulator 200 as an end effector, the tip 210 is the tip of the robot hand, and the element 220 is a material that constitutes the manipulator and a plurality of arms and parts in the manipulator. This shows that the joints have elasticity as a whole because they are connected at the joints. Here, the spring constant of the element 220 of the manipulator 200 is defined as a first spring constant k1.

In the object 20 to be collided, there is a surface portion 21 that is in contact with the tip 210 of the manipulator 200, and an element 22 having elasticity that constitutes the object 20 to be collided. For example, when the object 20 to be collided is an arm of a worker, the surface portion 21 is the skin surface of the arm, and the element 22 is the elastic muscle of the arm. Here, the spring constant of the surface portion 21 is a second spring constant k2, and the spring constant of the element 22 is a third spring constant k3.

Note that the second spring constant k2 and the third spring constant k3 are not limited to these, and for example, a plurality of springs may be used depending on the structure, material, and properties of the surface and internal structure of the object 20. A constant may be set, or a single spring constant synthesized as a whole may be set.

In FIG. 3A, at point P of the tip end 210 of the manipulator 200, an external force is applied (immediately) due to a collision (contact) with the object 20 to be collided, and no external force has been applied yet. In FIG. 3B, the tip 210 of the manipulator 200 and the object 20 collide with each other, and are balanced by the external force Fp at point P.

In this case, the amount of movement L of the manipulator 200 from the start of the collision (the state in FIG. 3(A)) to the state in which it is balanced by the external force Fp (the state in FIG. 3(B)) is the state in FIG. 3(B). The spring constant of the entire collision system at is set as the overall spring constant k, and is calculated by the following (Equation 1).
L=Fp/k...(Math. 1)

Here, the overall spring constant k is calculated by the following (Equation 2) using the first spring constant k1, second spring constant k2, and third spring constant k3 described above.
k=(k1・k2・k3)/(k1・k2+k1・k3+k2・k3) ...(Math. 2)

The external force calculation unit 120 calculates the external force applied to the manipulator 200 when the collision of the manipulator 200 is detected. For example, in the state shown in FIG. 3B, the external force calculation unit 120 converts the external force Fp applied to the point P of the tip 210 of the manipulator 200 into the torque T _j ( j: 1st to 6th axes). Specifically, since the forces are balanced at point P, the external force calculation unit 120 may calculate the external force Fp applied to point P of the tip portion 210 of the manipulator 200 using the principle of virtual work or the like. .

(About the first spring constant k1)
The spring constant calculation unit 140 calculates a spring constant based on the attitude of the manipulator 200 when the collision of the manipulator 200 is detected. Here, among the first spring constant k1, second spring constant k2, and third spring constant k3 that constitute the overall spring constant k, the first spring constant k1 on the manipulator 200 side will be explained.

The first spring constant k1 changes depending on the posture of the manipulator 200, and its value (elasticity, spring constant) changes.

As shown in the above (Equation 1), the manipulator 200 moves by the amount of movement L due to the influence of the external force Fp applied to point P and the overall spring constant k, but it depends on the first spring constant k1 on the manipulator 200 side. and a portion depending on the second spring constant k2 and third spring constant k3 on the collided object 20 side.

FIG. 4 is a schematic diagram for explaining the configuration of the movement amount L of the manipulator 200 in more detail. As shown in FIG. 4, the movement amount L of the manipulator 200 is calculated by the following (Equation 3) using the displacement amount ΔP of the manipulator 200 and the pushing amount L2 with respect to the object 20 to be collided.
L=ΔP+L2...(Math. 3)

The displacement amount ΔP of the manipulator 200 is determined by the influence of the external force Fp applied to the point P and the first spring constant k1 on the manipulator 200 side, out of the movement amount L of the manipulator 200, and is determined by a displacement calculation section (not shown). For example, it is calculated using the following procedure.

The first spring constant k1 on the manipulator 200 side when the manipulator 200 detects a collision is calculated by the following (Equation 4) using the external force Fp and the displacement amount ΔP at point P.
k1=Fp/ΔP...(Math. 4)

FIG. 5 is a schematic diagram for explaining the displacement amount ΔP of the manipulator 200 in a collision between the manipulator 200 and the object 20 using a single axis model. As shown in FIG. 5, the tip of the manipulator 200 is displaced to the P' point (displacement amount ΔP) due to the external force Fp applied to the P point, and for example, the joint angle θ of the manipulator 200 at that time becomes θ'. It is displaced.

Here, the joint angle θ' when the point P is displaced to the point P' due to the application of the external force Fp (state in FIG. 3(B)) is when the external force Fp is applied (just before), and the external force Fp has not yet been applied. It is calculated using the following (Equation 5) using the joint angle θ at point P (state in FIG. 3A), the joint torque difference Td, and the joint spring constant k.
θ _j '=θ _j +Td _j /k _j ... (Math. 5)

Here, (Equation 5) is for calculating the joint angle θ _j ' on the j-th axis, and for example, when the manipulator 200 is a 6-axis manipulator, j=1 to 6. The joint torque difference Td _j is the difference between the torque when no external force Fp is applied and the torque when the external force Fp is applied due to the collision of the manipulator 200. Further, the joint spring constant k _j is a spring constant based on the elasticity of the joint of each axis 201 to 206, and is set in advance as a fixed value according to the joint of each axis 201 to 206.

The position of point P (P _x , P _y , P _z ) and the position of point P' (P _x ', P _y ', P _z ') are calculated using the joint angles θ and θ' as shown below (Equation 6). It is calculated using (Equation 7).
(P _x , P _y , P _z ) = f (θ ₁ , θ ₂ , θ ₃ ) (Equation 6)
(P _x ', P _y ', P _z ') = f (θ ₁ ', θ ₂ ', θ ₃ ') ... (Equation 7)

Here, the function f is a function for converting into XYZ coordinate values based on the joint angle of each axis, and is preferably defined in advance. Further, the joint angles θ ₁ to θ ₃ are joint angles in any one of the axes 201 to 206, and for example, here, the joint angle θ ₁ in the first axis 201, the joint angle θ 1 in the second axis 202, This shows that the position of point P (P _x , P _y , P _z ) is calculated based on the joint angle θ ₂ at , and the joint angle θ ₃ at the third axis 203 . The same applies to the position of point P' (P _x ', P _y ', P _z ').

Then, from the above (Equation 6) and (Equation 7), the displacement amount ΔP of the manipulator 200 is calculated using the following (Equation 8).
ΔP={(P _x - _{P x} ') ² + (P _y - P _y ') ² + (P _z - P _z ') ² } ^1/2 ... (Math. 8)

In this way, according to (Equation 5) to (Equation 8) above, the displacement amount calculation unit calculates the displacement amount ΔP of the manipulator 200 by the joint angles θ _j and θ _j ′ of the manipulator 200 at each of the manipulator axes 201 to 206, It can be calculated based on the joint torque difference Td _j and the joint spring constant k _j .

Furthermore, according to the above (Equation 4), the first spring constant k1 can be calculated based on the displacement amount ΔP of the manipulator 200 calculated by the displacement amount calculating section. That is, the spring constant calculation unit 140 can calculate the first spring constant k1 according to the attitude of the manipulator 200.

(Regarding the second spring constant k2 and the third spring constant k3)
Next, among the first spring constant k1, second spring constant k2, and third spring constant k3 that constitute the overall spring constant k, the second spring constant k2 and third spring constant k3 on the collided object 20 side will be explained. .

The second spring constant k2 is the spring constant of the surface portion 21 of the object 20 to be collided, and the third spring constant k3 is the spring constant of the element 22 of the object 20 to be collided, and typically collides with the manipulator 200. Each of these values is set in advance according to the characteristics of the object 20 to be collided with.

Note that the composite spring constant k23 obtained by combining the second spring constant k2 and the third spring constant k3 on the collided object 20 side may be calculated using the following (Equation 9).
k23=(k2・k3)/(k2+k3)...(Math. 9)

Returning to FIG. 4, the pushing amount L2 against the object 20 depends on the influence of the external force Fp applied to the point P and the composite spring constant k23 on the object 20 side, out of the amount L of movement of the manipulator 200. As shown in FIG. 4(C), when the manipulator 200 is assumed to be a rigid body, the pushing amount L2 against the object 20 is determined by applying an external force Fp to the point P. It can be considered that the amount of compression depends on the composite spring constant k23.

That is, the pushing amount L2 with respect to the object 20 to be collided is calculated by the following (Equation 10) using the composite spring constant k23.
L2=Fp/k23...(Math. 10)

Note that the second spring constant k2, third spring constant k3, and composite spring constant 23 on the collided object 20 side may be set to fixed values in advance, but are not limited to this. For example, the manipulator 200 The user may select or set the settings depending on the environment in which the manipulator 200 is installed, the operator, or other objects that may collide with the manipulator 200.

Furthermore, the spring constant may be set for each part of the manipulator 200 based on the part of the object to be collided with which the manipulator 200 is likely to collide, in accordance with the attitude thereof. For example, if the object to be collided with the manipulator 200 is a worker, and if the collision occurs when the arm of the manipulator 200 is at a high position, then the body or arm of the worker, when the arm of the manipulator 200 is at a high position, If it collides with the worker's foot, it is the worker's foot. That is, the surface portion and elastic elements of the collided object are estimated in advance according to the position and orientation of the manipulator 200, and the spring constant of each collided portion of the collided object is set, selected, or dynamically changed. It doesn't matter if you do it like this.

(About the amount of movement L of the manipulator 200)
The movement amount calculation unit 150 calculates the movement amount L of the manipulator 200 based on the external force Fp and the overall spring constant k. Specifically, the movement amount calculation unit 150 calculates the external force Fp calculated by the external force calculation unit 120, the first spring constant k1 according to the attitude of the manipulator 200 calculated by the spring constant calculation unit 140, and the collided object 20. The movement amount L of the manipulator 200 is calculated based on the second spring constant k2 and the third spring constant k3 on the side ((Equation 1) to (Equation 10) described above).

Here, since the first spring constant k1 corresponding to the posture of the manipulator 200 has been appropriately calculated by the spring constant calculation unit 140, the movement amount calculation unit 150 can appropriately calculate the movement amount L of the manipulator 200. I can do it.

The return time calculation unit 160 calculates the return time T based on the movement amount L of the manipulator 200 calculated by the movement amount calculation unit 150 and the speed V of the manipulator 200 at the time of collision detection. Here, the speed V of the manipulator 200 when the collision is detected is obtained in the robot control device 100 as the speed V of the manipulator when the collision is detected.

The return time T is the time from when the manipulator 200 detects a collision (state in FIG. 3(B)) to when the collision starts (state in FIG. 3(A)). In other words, how much time should we go back to the time when the external force Fp is applied due to the collision (contact) between the manipulator 200 and the object 20 to be collided (just before), and when the external force Fp is not yet applied? It is.

The return time T is calculated using the following (Equation 11) using the movement amount L of the manipulator 200 and the speed V of the manipulator 200 at the time of collision detection.
T=L/V...(Math. 11)

The manipulator return unit 170 returns the manipulator 200 to the position and orientation before the return time T after the collision detection of the manipulator 200. Specifically, the manipulator return unit 170 acquires the position and orientation of the manipulator 200 before the return time T, which is stored in the current position storage unit 110, and returns the manipulator 200 to the position and orientation. The manipulator 200 returns to the position and orientation when the external force Fp due to the collision (contact) between the manipulator 200 and the object 20 to be collided is applied (immediately), and when no external force Fp is applied yet.

FIG. 6 is a diagram showing the transition (A) of the external force applied to point P on the manipulator 200 and the transition (B) of the position of the manipulator 200. As shown in FIG. 6, the external force applied to the manipulator 200 increases from the start of the collision, and decreases from the time the manipulator 200 starts returning to the position at the start of the collision. Then, the manipulator return unit 170 changes the position and orientation (here, TCP (Tool Center Point) coordinates) of the manipulator 200 to the position and orientation of the manipulator 200 from the time of collision detection to the time before the return time T calculated by the return time calculation unit 160. Return to As a result, the external force applied to the manipulator 200 approaches approximately 0 [N], and the position and orientation of the manipulator 200 also return to the state at the start of the collision. That is, the state shown in FIG. 3B returns to the state shown in FIG. 3A.

In this way, the position and orientation of the manipulator 200 is returned to the state before the collision so as to release the external force applied to the manipulator 200 when the manipulator 200 collides with, for example, a worker. Then, the manipulator 200 can be appropriately stopped in a state where no external force is applied to the manipulator 200.

Note that the manipulator return unit 170 returns the manipulator 200 to the position before the return time T after the collision detection of the manipulator 200 at a speed lower than the velocity V of the manipulator 200 when the collision is detected. In this way, by slowly returning the manipulator 200, for example, if the manipulator 200 has collided with a worker, the risk of colliding with the worker again can be reduced, and safety can be ensured.

[Robot control method during manipulator collision]
Next, a method for controlling the robot of the manipulator 200 when the manipulator 200 collides will be specifically explained in detail.

FIG. 7 is a flowchart showing the process flow of a robot control method M100 for controlling the manipulator 200, which is executed by the robot control device 100 according to an embodiment of the present invention. As shown in FIG. 7, the robot control method M100 includes steps S110 to S180, and each step is executed by a processor included in the robot control device 100.

In step S110, the robot control device 100 stores the current position and orientation of the manipulator 200 (current position storage step). As a specific example, the current position storage unit 110 stores the position and orientation of the manipulator 200 at that time.

In step S120, the robot control device 100 calculates the external force applied to the manipulator 200 (external force calculation step). As a specific example, the external force calculation unit 120 calculates the external force Fp applied to the point P of the tip portion 210 of the manipulator 200 based on the torque applied to each of the axes 201 to 206 detected by the torque sensor.

In step S130, the robot control device 100 detects a collision of the manipulator 200 based on the external force Fp applied to the manipulator 200 calculated in step S120 (collision detection step). As a specific example, the collision detection unit 130 detects, for example, whether a threshold value indicating a collision has been exceeded, based on the torque applied to each of the manipulator axes 201 to 206, which is detected by the torque sensor on each of the manipulator axes 201 to 206 of the manipulator 200. In this case, it is sufficient to detect that the manipulator 200 has collided (“Yes” in step S130). On the other hand, if it is not detected that the manipulator 200 has collided, the process returns to step S110 ("No" in step S130).

In step S140, the robot control device 100 calculates a spring constant based on the attitude of the manipulator 200 when the collision of the manipulator 200 is detected (spring constant calculation step). As a specific example, the spring constant calculation unit 140 calculates a first spring constant k1 that changes depending on the attitude of the manipulator 200, and also calculates a second spring constant k2 on the collided object 20 side that is set in advance. and the third spring constant k3 are obtained, and the overall spring constant k of the entire collision system is calculated from the above-mentioned (Equation 2).

In step S150, the robot control device 100 calculates the movement amount L of the manipulator 200 based on the external force and the spring constant (movement amount calculation step). As a specific example, the movement amount calculation unit 150 calculates the movement amount of the manipulator 200 by the above-mentioned (Equation 1) based on the external force Fp calculated in step S120 and the overall spring constant k calculated in step S140. Calculate L.

In step S160, the robot control device 100 calculates the return time based on the movement amount and the speed of the manipulator at the time of collision detection (return time calculation step). As a specific example, the return time calculation unit 160 calculates the return time by the above-mentioned (Equation 11) based on the movement amount L of the manipulator 200 calculated in step S150 and the speed V of the manipulator 200 at the time of collision detection. Calculate T.

In step S170, the robot control device 100 acquires the position and orientation of the manipulator 200 before the return time calculated in step S160 (position information acquisition step). As a specific example, with respect to the position and orientation of the manipulator 200 before the return time T, the manipulator return unit 170 acquires the position and orientation of the manipulator 200 at that time, which was stored in step S110.

In step S180, the robot control device 100 returns the manipulator 200 to the position and orientation before the return time after the collision detection of the manipulator 200 (manipulator return step). As a specific example, the manipulator return unit 170 returns the position and orientation of the manipulator 200 before the return time T calculated in step S160 after the collision detection of the manipulator 200 to the position and orientation of the manipulator 200 acquired in step S170. In this way, by returning the position and orientation of the manipulator 200, it is returned to the position and orientation at the time (immediately) when an external force is applied due to the collision (contact) between the manipulator 200 and the object 20 to be collided.

As described above, according to the robot control device 100 and the robot control method M100 according to an embodiment of the present invention, the movement amount calculation section 150 calculates the amount of movement of the manipulator 200 when the collision of the manipulator 200 is detected, which is calculated by the external force calculation section 120. of the manipulator 200 based on the external force Fp applied to the spring constant and the spring constant (overall spring constant k including the first spring constant k1) based on the posture of the manipulator 200 at the time of collision detection of the manipulator 200 calculated by the spring constant calculation unit 140. Calculate the amount of movement L. Then, the return time calculation unit 160 calculates the return time T based on the movement amount L and the speed V of the manipulator 200 at the time of the collision detection, and the manipulator return unit 170 calculates the return time T from the time of the collision detection of the manipulator 200. By returning the manipulator 200 to the previous position and orientation, the external force Fp applied to the manipulator 200 is released according to the orientation of the manipulator 200 at the time of the collision detection. can be returned to the state. That is, the manipulator 200 can be stopped while the external force applied to the manipulator 200 is released.

In this embodiment, the manipulator 200 has been described as a 6-axis vertically articulated robot, but it is not limited to this; for example, it may be a 7-axis vertically articulated robot, or Other robots may be used as long as the spring constant changes depending on the position and orientation, or the spring constant of the object to be collided is taken into consideration.

The embodiments described above are intended to facilitate understanding of the present invention, and are not intended to be interpreted as limiting the present invention. Each element included in the embodiment, as well as its arrangement, material, conditions, shape, size, etc., are not limited to those illustrated, and can be changed as appropriate. Further, it is possible to partially replace or combine the structures shown in different embodiments.

DESCRIPTION OF SYMBOLS 10... Robot system, 20... Collided object, 21... Surface portion of collided object, 22... Elements of collided object, 100... Robot control device, 110... Current position storage section, 120... External force calculation section, 130... Collision Detection section, 140... constant calculation section, 150... movement amount calculation section, 160... return time calculation section, 170... manipulator return section, 200... manipulator, 201 to 206... each axis, 210... tip of manipulator, 220... manipulator Elements of M100...Robot control method, S110 to S180...Each step of robot control method M100

Claims (6)

A robot control device that controls a manipulator,
a collision detection unit that detects a collision of the manipulator;
an external force calculation unit that calculates an external force applied to the manipulator when a collision of the manipulator is detected;
a spring constant calculation unit that calculates a spring constant based on the posture of the manipulator when a collision of the manipulator is detected;
a movement amount calculation unit that calculates a movement amount of the manipulator based on the external force and the spring constant;
a return time calculation unit that calculates a return time based on the amount of movement and the speed of the manipulator at the time of collision detection;
a manipulator return unit that returns the manipulator to the position and orientation from when the collision of the manipulator is detected to before the return time;
Robot control device. further comprising a displacement amount calculation unit that calculates a displacement amount of the manipulator when a collision of the manipulator is detected,
The spring constant includes a spring constant of the manipulator when a collision of the manipulator is detected, which is calculated based on the external force and the displacement amount of the manipulator.
The robot control device according to claim 1. The spring constant includes a spring constant of a collided part when the collision of the manipulator is detected.
The robot control device according to claim 2. The displacement amount calculation unit calculates the displacement amount of the manipulator based on a joint angle, a joint torque, and a joint spring constant in each axis of the manipulator.
The robot control device according to claim 2. The manipulator return unit returns the manipulator to the position and orientation from when the collision of the manipulator is detected to before the return time at a speed slower than the speed when the collision of the manipulator is detected.
The robot control device according to claim 1. The spring constant of the collided part when the collision of the manipulator is detected is set by estimating in advance the collided part that has a possibility of colliding with the manipulator.
The robot control device according to claim 3.