Robot control device

Abstract

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

Description

[Technical Field]

[0001] This invention relates to a robot control device. [Background technology]

[0002] In recent years, robots have become widespread in the industrial sector. These robots are used, for example, in the assembly, welding, and transportation of electronic and mechanical components, contributing to the efficiency and automation of factory production lines.

[0003] Furthermore, development is progressing on collaborative robots that perform some of the tasks previously done by workers, either on behalf of the worker or alongside the worker, working alongside the worker. Since collaborative robots work alongside workers rather than in a safely enclosed area, ensuring the safety of the worker is crucial.

[0004] For example, if a collaborative robot comes into contact with a worker or other person, it is preferable to quickly stop the collaborative robot, but at this time, to return it to its state before contact so as to release any external force applied to it during the contact, before stopping it.

[0005] Patent Document 1 discloses a robot system that, upon detecting an external force acting on the robot, reverses the robot's movement trajectory based on a preset distance or time of movement, and further moves the robot to a safe position to reduce the applied external force. [Prior art documents] [Patent Documents]

[0006] [Patent Document 1] Japanese Patent Publication No. 2019-81234 [Overview of the project] [Problems that the invention aims to solve]

[0007] However, depending on the robot's posture at the time of contact with a worker, the external force applied to the robot and the distance traveled after contact will differ. In the robot system disclosed in Patent Document 1, when an external force is detected on the robot, the robot's motion trajectory is reversed based on a preset distance and time of travel. In other words, the robot system disclosed in Patent Document 1 does not take into account the robot's posture at the time of contact with a worker, and has the problem of not being able to release the external force applied to the robot and return it to its state before contact appropriately.

[0008] Therefore, the present invention aims to provide a robot control device that appropriately returns the manipulator to its pre-contact state and stops it, in order to release the external force applied to the manipulator in accordance with the posture of the manipulator when it comes into contact with a worker or the like. [Means for solving the problem]

[0009] A robot control device according to one aspect of the present invention is a robot control device for controlling a manipulator, comprising: a collision detection unit for detecting a collision of the manipulator; an external force calculation unit for calculating the external force applied to the manipulator when a collision of the manipulator is detected; a spring constant calculation unit for calculating the spring constant based on the posture of the manipulator when a collision of the manipulator is detected; a movement amount calculation unit for calculating the amount of movement of the manipulator based on the external force and the spring constant; a return time calculation unit for calculating the return time based on the amount of movement and the velocity of the manipulator when a collision of the manipulator is detected; and a manipulator return unit for returning the manipulator to the position and posture from the time of collision detection to before the return time.

[0010] According to this aspect, the movement amount calculation unit calculates the movement amount of the manipulator based on the external force applied to the manipulator at the time of collision detection of the manipulator calculated by the external force calculation unit and the spring constant based on the posture of the manipulator at the time of collision detection of the manipulator calculated by the spring constant calculation unit. Then, the return time calculation unit calculates the return time based on the movement amount of the manipulator and the speed at the time of collision detection of the manipulator, and the manipulator return unit returns the manipulator to the position and posture before the return time from the time of collision detection of the manipulator. Thereby, it is possible to appropriately return to the state before the collision detection so as to release the external force applied to the manipulator according to the posture of the manipulator at the time of collision detection of the manipulator. That is, it is possible to stop the manipulator in a state where the external force applied to the manipulator is released.

[0011] In the above aspect, a displacement amount calculation unit that calculates the displacement amount of the manipulator at the time of collision detection of the manipulator may be further provided, and the spring constant may include the spring constant of the manipulator at the time of collision detection of the manipulator calculated based on the external force and the displacement amount of the manipulator.

[0012] 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 calculation unit, and the spring constant includes the spring constant of the manipulator. Therefore, it is possible to appropriately calculate the spring constant according to the posture of the manipulator. As a result, it is possible to more appropriately return to the state before the collision detection so as to release the external force applied to the manipulator.

[0013] In the above aspect, the spring constant may include the spring constant of the collision part of the manipulator at the time of collision detection of the manipulator.

[0014] According to this embodiment, the spring constant includes the spring constant of the part of the manipulator that is hit when a collision is detected, so that the spring constant corresponding to the part of the manipulator that is hit can be appropriately calculated. As a result, the manipulator can be returned to its state before the collision detection more appropriately, taking into account the part of the manipulator that is hit, in order to release the external force applied to it.

[0015] In the above embodiment, the displacement calculation unit may calculate the displacement of the manipulator based on the joint angle, joint torque, and joint spring constant of each axis of the manipulator.

[0016] According to this embodiment, since the displacement is calculated from each axis of the manipulator, the spring constant corresponding to the posture of the manipulator can be calculated more appropriately.

[0017] In the above embodiment, the manipulator return unit may return the manipulator to the position and orientation of the manipulator before the return time from the time of collision detection, at a speed slower than the speed at which the manipulator detects the collision.

[0018] According to this embodiment, the manipulator return unit slowly returns the manipulator after detecting a collision. For example, if it collides with a worker, the manipulator can be returned while taking the worker's safety into consideration.

[0019] In the above embodiment, the spring constant of the collision target when the manipulator detects a collision may be set by pre-estimating the collision target that the manipulator is likely to collide with.

[0020] According to this embodiment, the manipulator can estimate the object it is likely to collide with and set the spring constant of the collision point of the object, thereby enabling the calculation of an appropriate spring constant with greater accuracy depending on the situation. [Effects of the Invention]

[0021] According to the present invention, it is possible to provide a robot control device that appropriately returns the manipulator to its pre-contact state and stops it, in order to release the external force applied to the manipulator in accordance with the posture of the manipulator when it comes into contact with a worker or the like. [Brief explanation of the drawing]

[0022] [Figure 1] This is a schematic diagram showing the configuration of a robot system 10 according to one embodiment of the present invention. [Figure 2] This is a functional block diagram showing the functions of a robot control device 100 according to one embodiment of the present invention. [Figure 3] This diagram schematically shows the collision system, which involves a manipulator 200 and an object 20 that collides with the manipulator 200. [Figure 4] This is a schematic diagram illustrating the configuration of the manipulator 200's movement range L in more detail. [Figure 5] This is a schematic diagram illustrating the displacement ΔP of the manipulator 200 during a collision between the manipulator 200 and the object being hit 20, using a uniaxial model. [Figure 6] This figure shows the progression of the external force applied to point P on the manipulator 200 (A) and the progression of the position of the manipulator 200 (B). [Figure 7] This flowchart shows the processing flow of a robot control method M100 that controls a manipulator 200, which is executed by a robot control device 100 according to one embodiment of the present invention. [Modes for carrying out the invention]

[0023] The embodiments of the present invention will be described below in detail with reference to the drawings. The embodiments described below are merely examples of how to implement the present invention and are not intended to limit the scope of the invention. Furthermore, to facilitate understanding of the explanation, the same reference numerals are used for identical components in each drawing whenever possible, and redundant explanations may be omitted.

[0024] <One Embodiment> [Robot System Configuration] Figure 1 is a schematic diagram showing the configuration of a robot system 10 according to one embodiment of the present invention. As shown in Figure 1, the robot system 10 comprises a robot control device 100 and a manipulator 200.

[0025] The manipulator 200 is an industrial robot that, for example, has a welding torch or robot hand attached as an end effector to the tip of its arm. Here, we will explain using a 6-axis manipulator as an example. Based on the operation instructions from the robot control device 100, the manipulator 200 rotates each axis 201 to 206 using motors to move and rotate the arm and wrist, etc., to move the end effector to the appropriate position and angle.

[0026] Furthermore, the manipulator 200 detects the torque applied to each axis 201-206 using torque sensors and notifies the robot control device 100 of the detected torque.

[0027] The robot control device 100 controls the movement of the manipulator 200. Specifically, the robot control device 100 controls the motors on each axis 201 to 206 of the manipulator 200, thereby rotating each axis 201 to 206 and moving and rotating the arm and wrist. In this way, the robot control device 100 controls the manipulator 200 to maintain an appropriate position and orientation.

[0028] Furthermore, 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 controls the manipulator 200 to return to its original position or stop as a processing step in the event of a collision.

[0029] [Handling of manipulator collisions in robot control systems] Next, the processing of the robot control device 100 when the manipulator 200 collides with an object will be explained in detail. The collision of the manipulator 200 is determined based on the torque applied to each axis 201 to 206 of the manipulator 200, and this includes collisions not only with the manipulator 200 itself but also with peripheral equipment such as end effectors attached to the manipulator 200. In other words, collisions with peripheral equipment such as end effectors affect the torque applied to each axis 201 to 206 of the manipulator 200, and it is appropriate to determine the collision as a collision of the manipulator 200 based on that torque.

[0030] Figure 2 is a functional block diagram showing the functions of a robot control device 100 according to one embodiment of the present invention. As shown in Figure 2, the robot control device 100 includes a current position storage unit 110, an external force calculation unit 120, a collision detection unit 130, a spring constant calculation unit 140, a movement amount calculation unit 150, a return time calculation unit 160, and a manipulator return unit 170. As described above, the robot control device 100 stores various data for controlling the operation of the manipulator 200 and has many functions, but here we will mainly describe the processing (functions) of the robot control device 100 in the event of a collision of the manipulator 200.

[0031] The current position memory unit 110 stores the current position and orientation of the manipulator 200. For example, the current position memory unit 110 can store the position and orientation of the manipulator 200 based on the joint angles of each axis 201 to 206 of the manipulator 200 at that time.

[0032] The collision detection unit 130 detects collisions of the manipulator 200. For example, the collision detection unit 130 calculates the external force applied to the manipulator 200 using the external force calculation unit 120, based on the torque applied to each axis 201 to 206 detected by torque sensors on each axis 201 to 206 of the manipulator 200, and detects that the manipulator 200 has collided. Specifically, in the case of a manipulator 200 controlled by a robot control device 100, if the torque of each axis 201 to 206 becomes an abnormal value compared to the value based on the control, the collision detection unit 130 should detect that the manipulator 200 has collided.

[0033] Figure 3 schematically shows a collision system, which is a collision between a manipulator 200 and an object 20 that collides with the manipulator 200. In Figure 3, the manipulator 200 and the object 20 are in collision, and the situation at (A) the start of the collision (start of contact) and (B) the time of collision detection (equilibrium state) are schematically shown.

[0034] In the manipulator 200, the tip portion 210 that is in contact with the object to be hit 20, and the elements 220 that have elasticity due to the joints of the manipulator, etc. For example, if a robot hand is attached to the manipulator 200 as an end effector, the tip portion 210 is the tip of the robot hand, and the elements 220 represent the material that makes up the manipulator, and the fact that the manipulator as a whole has elasticity due to the connection of multiple arms and parts at joints. Here, the spring constant of the elements 220 of the manipulator 200 is the first spring constant k1.

[0035] The object to be hit 20 consists of a surface portion 21 that is in contact with the tip portion 210 of the manipulator 200, and an elastic element 22 that constitutes the object to be hit 20. For example, if the object to be hit 20 is the 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 the second spring constant k2, and the spring constant of the element 22 is the third spring constant k3.

[0036] Furthermore, the second spring constant k2 and the third spring constant k3 are not limited to these two. For example, multiple spring constants may be set for the surface and internal structure of the object being hit 20, depending on its composition, material, and properties. Alternatively, a single spring constant may be set as a whole.

[0037] Figure 3(A) shows the state just before an external force is applied at point P of the tip 210 of the manipulator 200 due to collision (contact) with the object to be hit 20, but before the external force is applied. Figure 3(B) shows the state after the tip 210 of the manipulator 200 and the object to be hit 20 have collided, and at point P, equilibrium is achieved with an external force Fp.

[0038] In this case, the amount of displacement L of the manipulator 200 from the start of the collision (state 3(A) in Figure 3) to the state where equilibrium is achieved by the external force Fp (state 3(B) in Figure 3) is calculated using the following equation (Equation 1), where the spring constant of the entire collision system in state 3(B) is the overall spring constant k. L=Fp / k (Math. 1)

[0039] Here, the overall spring constant k is calculated using the first spring constant k1, the second spring constant k2, and the third spring constant k3 described above, as shown in (Equation 2) below. k=(k1·k2·k3) / (k1·k2+k1·k3+k2·k3) ···(Number 2)

[0040] The external force calculation unit 120 calculates the external force applied to the manipulator 200 when a collision is detected. For example, in the state shown in Figure 3(B), the external force Fp applied to point P of the tip 210 of the manipulator 200 is calculated by the torque T applied to each axis 201 to 206 detected by the torque sensor. j The calculation is performed based on (j: 1st to 6th axes). Specifically, since the forces are balanced at point P, the external force calculation unit 120 can use the principle of virtual work or the like to calculate the external force Fp applied to point P of the tip 210 of the manipulator 200.

[0041] (Regarding the first spring constant k1) The spring constant calculation unit 140 calculates the spring constant based on the attitude of the manipulator 200 when a collision is detected. Here, we will explain the first spring constant k1 on the manipulator 200 side, which is one of the first spring constant k1, second spring constant k2, and third spring constant k3 that make up the overall spring constant k.

[0042] The first spring constant k1 changes according to the posture of the manipulator 200. For example, its value (elasticity, spring constant) changes depending on the joint angles of each axis 201 to 206 of the manipulator 200 and the angle and extension / retraction state of the arm.

[0043] As shown in (Equation 1) above, due to the influence of the external force Fp applied to point P and the overall spring constant k, the manipulator 200 moves by a displacement L, which is composed of a portion dependent on the first spring constant k1 on the manipulator 200 side and a portion dependent on the second spring constant k2 and third spring constant k3 on the object being hit 20 side.

[0044] Figure 4 is a schematic diagram illustrating the configuration of the manipulator 200's movement L in more detail. As shown in Figure 4, the movement L of the manipulator 200 is calculated using the displacement ΔP of the manipulator 200 and the amount of indentation L2 on the object being hit 20, as shown in the following equation (Equation 3). L=ΔP+L2 (Math 3)

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

[0046] The first spring constant k1 on the manipulator 200 side when a collision is detected is calculated at point P using the external force Fp and the displacement ΔP as shown in (Equation 4) below. k1=Fp / ΔP (Math. 4)

[0047] FIG. 5 is a schematic diagram for explaining the displacement amount ΔP of the manipulator 200 in the collision between the manipulator 200 and the object to be collided 20 as a uniaxial model. As shown in FIG. 5, due to the external force Fp applied to point P, the tip of the manipulator 200 is displaced to point P' (displacement amount ΔP). For example, at that time, the joint angle θ of the manipulator 200 is displaced to θ'.

[0048] Here, the joint angle θ' (state of FIG. 3(B)) when point P is displaced to point P' due to the application of the external force Fp is calculated by the following (Equation 5) using the joint angle θ (state of FIG. 3(A)) of point P where the external force Fp has not been applied yet, the joint torque difference Td, and the joint spring constant k when the external force Fp is applied (immediately before). θ j ’ = θ j + Td [[ID=1 "]] j / k j ···(Equation 5)

[0049] Here, (Equation 5) calculates the joint angle θ j ’. 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 in the state where the external force Fp is not applied and the torque in the state where the external force Fp is applied due to the collision of the manipulator 200. Also, the joint spring constant k j is the spring constant based on the elasticity of the joints of each axis 201 to 206, and is set in advance as a fixed value according to the joints of each axis 201 to 206.

[0050] 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 by the following (Equation 6) and (Equation 7) using the joint angles θ and θ'. (P x , P y , P z) = f(θ1,θ2,θ3) ... (Math 6) (P x ',P y ',P z ')=f(θ1', θ2', θ3') (Math. 7)

[0051] Here, function f is a function that converts the joint angles of each axis to XYZ coordinate values, and it is preferable that this function be predetermined. Also, joint angles θ1 to θ3 are joint angles on any of the axes 201 to 206. For example, here, the position of point P is determined based on joint angle θ1 on the first axis 201, joint angle θ2 on the second axis 202, and joint angle θ3 on the third axis 203. x ,P y ,P z This indicates that the position of point P' (P x ',P y ',P z The same applies to ').

[0052] Then, from (Equation 6) and (Equation 7) above, the displacement ΔP of the manipulator 200 is calculated using (Equation 8) below. ΔP={(P x -P x ') 2 +(P y -P y ') 2 +(P z -P z ') 2} 1 / 2 ...(Number 8)

[0053] Thus, according to (Equations 5) to (Equation 8) above, the displacement calculation unit calculates the displacement amount ΔP of the manipulator 200 based on the joint angle θ of each axis 201 to 206 of the manipulator 200. j and θ j ', joint torque difference Td j , and joint spring constant k j It can be calculated based on this.

[0054] Furthermore, based on the displacement ΔP of the manipulator 200 calculated by the displacement calculation unit using the above (Equation 4), the first spring constant k1 can be calculated. In other words, the spring constant calculation unit 140 can calculate the first spring constant k1 according to the posture of the manipulator 200.

[0055] (Regarding the second spring constant k2 and the third spring constant k3) Next, we will explain the second spring constant k2 and the third spring constant k3 on the side of the object being hit 20, which are among the first spring constant k1, second spring constant k2, and third spring constant k3 that constitute the overall spring constant k.

[0056] The second spring constant k2 is the spring constant of the surface portion 21 of the object being hit 20, and the third spring constant k3 is the spring constant of the element 22 of the object being hit 20. Typically, these are preset according to the characteristics of the object being hit 20 that collides with the manipulator 200.

[0057] Alternatively, the combined 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 formula (Equation 9). k23=(k2·k3) / (k2+k3) ···(Number 9)

[0058] Returning to Figure 4, the amount of indentation L2 into the object being hit 20 is determined by the influence of the external force Fp applied at point P and the combined spring constant k23 on the object being hit 20, out of the amount of movement L of the manipulator 200. As shown in Figure 4(C), assuming that the manipulator 200 is a rigid body, the amount of indentation L2 into the object being hit 20 can be considered as the amount of indentation that occurs when the external force Fp is applied at point P, depending on the combined spring constant k23 on the object being hit 20.

[0059] In other words, the amount of indentation L2 into the object being hit 20 is calculated using the above composite spring constant k23 as shown in the following equation (Equation 10). L2 = Fp / k23 ... (Math 10)

[0060] Furthermore, the second spring constant k2, the third spring constant k3, and the combined spring constant on the object being hit 20 are as follows: k While 23 may be set to a fixed value in advance, it is not limited to this, and the user may select or set it according to, for example, the environment in which the manipulator 200 is installed, the worker, and other objects that may collide with the manipulator 200.

[0061] Furthermore, the spring constant for each part of the object that may collide with the manipulator 200 may be set in accordance with the attitude of the manipulator 200. For example, the object that may collide with the manipulator 200 but If the object is a worker, the collision will occur to the worker's torso or arms when the manipulator 200's arm is in a high position, or to the worker's feet when the arm is in a low position. In other words, the surface area and elastic elements of the object to be struck can be estimated in advance according to the position and orientation of the manipulator 200, and the spring constants of each part of the object to be struck can be set, selected, or dynamically changed.

[0062] (Regarding the travel distance L of manipulator 200) The displacement calculation unit 150 calculates the displacement L of the manipulator 200 based on the external force Fp and the overall spring constant k. Specifically, the displacement calculation unit 150 calculates the displacement L of the manipulator 200 based on the external force Fp calculated by the external force calculation unit 120 and the first spring constant k1, the second spring constant k2, and the third spring constant k3 on the collision target 20 side, which are calculated by the spring constant calculation unit 140 (as shown in (Equations 1) to (Equations 10) above).

[0063] Here, since the first spring constant k1 corresponding to the posture of the manipulator 200 is appropriately calculated by the spring constant calculation unit 140, the displacement calculation unit 150 can appropriately calculate the displacement L of the manipulator 200.

[0064] The return time calculation unit 160 calculates the return time T based on the amount of movement L of the manipulator 200 calculated by the amount of movement calculation unit 150 and the velocity V of the manipulator 200 at the time of collision detection. Here, the velocity V of the manipulator at the time of collision detection is obtained by the robot control device 100.

[0065] The return time T is the time from when the manipulator 200 detects a collision (state 3(B) in Figure 3) to when the collision begins (state 3(A) in Figure 3). In other words, it is the time to return to the state before the external force Fp is applied due to the collision (contact) between the manipulator 200 and the object 20 being hit, but before the external force Fp is applied.

[0066] The return time T is calculated using the distance L of the manipulator 200 and the velocity V of the manipulator 200 at the time of collision detection, as shown in the following equation (Equation 11). T=L / V (Number 11)

[0067] The manipulator return unit 170 returns the manipulator 200 to the position and orientation it was in before the return time T from the time of collision detection. Specifically, the manipulator return unit 170 retrieves the position and orientation of the manipulator 200 before the return time T, which is stored by the current position storage unit 110, and returns the manipulator 200 to that position and orientation. The manipulator 200 returns to the position and orientation it was in at the time (immediately before) when the external force Fp due to the collision (contact) between the manipulator 200 and the object 20 is applied, but before the external force Fp is applied.

[0068] Figure 6 shows the progression of the external force applied to point P on the manipulator 200 (A) and the progression of the position of the manipulator 200 (B). As shown in Figure 6, the external force applied to the manipulator 200 increases from the start of the collision and decreases from the start of the return process, which attempts to return the manipulator 200 to the position it was in at the start of the collision. The manipulator return unit 170 then returns the position and orientation of the manipulator 200 (in this case, the TCP (Tool Center Point) coordinates) to the position and orientation of the manipulator 200 before the return time T from the time of collision detection, as calculated by the return time calculation unit 160. 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 returns to the state it was in at the start of the collision. That is, it returns from the state shown in Figure 3(B) to the state shown in Figure 3(A).

[0069] In this way, the position and orientation of the manipulator 200 are returned to their pre-collision state so as to release any external forces acting on the manipulator 200 during a collision with, for example, a worker. This allows the manipulator 200 to be stopped appropriately when no external forces are acting on it.

[0070] Furthermore, the manipulator return unit 170 returns the manipulator 200 to a position T before the return time from the time of collision detection, at a speed slower than the speed V at which the manipulator 200 detects a collision. By returning the manipulator 200 slowly in this way, for example, if it has collided with a worker, the risk of colliding with the worker again can be reduced, thereby ensuring safety.

[0071] [Robot control method during manipulator collisions] Next, we will explain in detail the robot control method for the manipulator 200 during a collision.

[0072] Figure 7 is a flowchart showing the processing flow of a robot control method M100 that controls a manipulator 200, which is performed by a robot control device 100 according to one embodiment of the present invention. As shown in Figure 7, the robot control method M100 includes steps S110 to S180, each step being performed by a processor included in the robot control device 100.

[0073] In step S110, the robot control device 100 stores the current position and orientation of the manipulator 200 (current position storage step). Specifically, the current position storage unit 110 stores the position and orientation of the manipulator 200 at that time.

[0074] In step S120, the robot control device 100 calculates the external force applied to the manipulator 200 (external force calculation step). Specifically, the external force calculation unit 120 calculates the external force Fp applied to point P of the tip 210 of the manipulator 200 based on the torques applied to each axis 201 to 206 detected by the torque sensor.

[0075] 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). Specifically, the collision detection unit 130 detects that the manipulator 200 has collided based on the torque applied to each axis 201 to 206 detected by the torque sensors on each axis 201 to 206 of the manipulator 200. For example, if the torque exceeds a threshold indicating a collision, the unit detects that the manipulator 200 has collided ("Yes" in step S130). On the other hand, if the unit does not detect that the manipulator 200 has collided, it returns to the process in step S110 ("No" in step S130).

[0076] In step S140, the robot control device 100 calculates the spring constant based on the posture of the manipulator 200 when a collision is detected (spring constant calculation step). Specifically, the spring constant calculation unit 140 calculates a first spring constant k1 that changes according to the posture of the manipulator 200, and also obtains, for example, a preset second spring constant k2 and a third spring constant k3 on the object being hit 20 side, and calculates the overall spring constant k of the entire collision system from (Equation 2) described above.

[0077] In step S150, the robot control device 100 calculates the amount of movement L of the manipulator 200 based on the external force and the spring constant (movement amount calculation step). Specifically, the movement amount calculation unit 150 calculates the amount of movement L of the manipulator 200 based on the external force Fp calculated in step S120 and the overall spring constant k calculated in step S140, using the above-mentioned (Equation 1).

[0078] In step S160, the robot control device 100 calculates the return time based on the amount of movement and the velocity of the manipulator at the time of collision detection (return time calculation step). Specifically, the return time calculation unit 160 calculates the return time T based on the amount of movement L of the manipulator 200 calculated in step S150 and the velocity V of the manipulator 200 at the time of collision detection, using the above-mentioned (Equation 11).

[0079] 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). Specifically, the manipulator return unit 170 acquires the position and orientation of the manipulator 200 at that time, which was stored in step S110, regarding the position and orientation of the manipulator 200 before the return time T.

[0080] In step S180, the robot control device 100 returns the manipulator 200 to the position and orientation it had before the return time from the time of collision detection (manipulator return step). Specifically, the manipulator return unit 170 returns the manipulator 200 to the position and orientation it had before the return time T calculated in step S160 from the time of collision detection, which was acquired in step S170. By returning the position and orientation of the manipulator 200 in this way, it returns to the position and orientation it had at the time (immediately before) when the external force was applied due to the collision (contact) between the manipulator 200 and the object 20 being hit.

[0081] As described above, according to the robot control device 100 and robot control method M100 of one embodiment of the present invention, the movement amount calculation unit 150 calculates the movement amount L of the manipulator 200 based on the external force Fp applied to the manipulator 200 at the time of collision detection of the manipulator 200, which is calculated by the external force calculation unit 120, 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, which is calculated by the spring constant calculation unit 140. The return time calculation unit 160 then calculates the return time T based on the amount of movement L and the velocity V of the manipulator 200 at the time of collision detection. The manipulator return unit 170 then returns the manipulator 200 to the position and orientation before the return time T from the time of collision detection, thereby appropriately returning it to the state before collision detection in order to release the external force Fp applied to the manipulator 200 according to its orientation at the time of collision detection. In other words, the manipulator 200 can be stopped with the external force applied to it released.

[0082] In this embodiment, the manipulator 200 is described as a 6-axis vertical articulated robot, but it is not limited to this. For example, it could be a 7-axis vertical articulated robot, or any other type of robot if the spring constant changes depending on the robot's position and orientation, or if the spring constant of the object being hit needs to be considered.

[0083] The embodiments described above are provided to facilitate understanding of the present invention and are not intended to limit its interpretation. The elements, arrangement, materials, conditions, shapes, and sizes of the embodiments are not limited to those exemplified and can be modified as appropriate. Furthermore, it is possible to partially substitute or combine the configurations shown in different embodiments. [Explanation of Symbols]

[0084] 10...Robot system, 20...Object to be hit, 21...Surface of object to be hit, 22...Elements of object to be hit, 100...Robot control device, 110...Current position memory unit, 120...External force calculation unit, 130...Collision detection unit, 140... spring Constant calculation unit, 150... Movement amount calculation unit, 160... Return time calculation unit, 170... Manipulator return unit, 200... Manipulator, 201~206... Each axis, 210... Tip of the manipulator, 220... Elements of the manipulator, M100... Robot control method, S110~S180... Each step of robot control method M100

Claims

1. A robot control device for controlling a manipulator, A collision detection unit that detects collisions of the manipulator, An external force calculation unit that calculates the external force applied to the manipulator when a collision is detected by the manipulator, A spring constant calculation unit calculates the spring constant based on the posture of the manipulator when a collision is detected by the manipulator. A movement amount calculation unit that calculates the amount of movement of the manipulator based on the external force and the spring constant, A return time calculation unit calculates the return time based on the amount of movement and the speed of the manipulator at the time of collision detection. The manipulator return unit includes a manipulator return unit that returns the manipulator to the position and orientation it was in before the return time, from the time of collision detection of the manipulator. Robot control device.

2. The system further includes a displacement calculation unit that calculates the amount of displacement of the manipulator when a collision is detected with the manipulator, The spring constant includes the spring constant of the manipulator at the time of collision detection of the manipulator, which is calculated based on the external force and the displacement of the manipulator. The robot control device according to claim 1.

3. The spring constant includes the spring constant of the part of the manipulator that is hit when a collision is detected. The robot control device according to claim 2.

4. The displacement calculation unit calculates the displacement of the manipulator based on the joint angle, joint torque, and joint spring constant of each axis of the manipulator. The robot control device according to claim 2.

5. The manipulator return unit returns the manipulator to the position and orientation it had before the return time, at a speed slower than the manipulator's speed at the time of collision detection. The robot control device according to claim 1.

6. The spring constant of the collision target when the manipulator detects a collision is set by pre-estimating the collision target that the manipulator is likely to collide with. The robot control device according to claim 3.

Images