JP2015168050A - Robot control device, robot and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot control device capable of reliably inserting a protrusion of a first member into an opening of a second member, and further to provide a robot and a robot control method.SOLUTION: A robot control device comprises: a robot arm; a force sensor a control section controlling operation of a robot performing work supporting one of a first member having a protrusion and a second member having an opening by an end effector mounted to the robot arm and inserting the protrusion into the opening, having a relative movement direction at the time of insertion of the protrusion into the opening being a Z-axis direction, moving the end effector in an X-axis direction at the time of the work, converting force in each axis direction detected by the force sensor into displacement and controlling operation of the robot arm by force control on the basis of each displacement; and a position specification section specifying a position of the opening or the protrusion on the basis of the displacement in the X-axis direction and a Y-axis direction. Elasticity terms of the force control in the Z-axis direction is set to be greater than elasticity terms thereof in the X-axis direction and the Y-axis direction.

Description

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

  Conventionally, a robot provided with a robot arm is known. A plurality of links are connected to the robot arm via joints, and a hand is attached to the most distal link as an end effector, for example. The joint is driven by a motor, and the link is rotated by driving the joint. Then, for example, the robot grips an insert having a protrusion with a hand, and performs an operation of inserting the protrusion into an opening (hole) provided in the insertion object.

  In addition, a force sensor is provided between the most distal link and the hand, and the robot detects force and moment with the force sensor during the work, and the force sensor Based on the detection result, impedance control (force control) is performed (see, for example, Patent Document 1). Further, the robot grasps the position of the opening based on the output signal of the force sensor, and inserts the protrusion of the insert into the opening of the insertion object.

JP 2010-137299 A

However, the conventional robot has the following problems.
The output signal of the force sensor has a lot of noise and a low S / N ratio. The force sensor is the principle of converting the deformation of the detection mechanism into an electrical signal, and in order to increase its sensitivity, the detection mechanism must have a low rigidity. In this case, the detection mechanism is deformed. The hand is displaced by force and moment, and accurate positioning is not possible. For this reason, a design that increases the rigidity of the detection mechanism unit in order to suppress the deformation of the detection mechanism unit is usually performed. In this case, however, the sensitivity of the force sensor decreases and the S / N ratio decreases. Therefore, a signal indicating the true force or moment is buried in noise, and it is difficult to accurately detect the force or moment. In particular, it is difficult to correctly determine the magnitude of the output signal of the force sensor with respect to the threshold value.

In addition, when a low-pass filter is introduced to remove noise from the output signal of the force sensor, the narrow-band low-pass filter that has a sufficient effect to improve the low S / N ratio is the output signal of the force sensor. In particular, it is difficult to accurately perform detection while moving at a high speed.
Because of the above problems, it is particularly difficult to correctly determine the magnitude of the threshold with respect to the output signal of the force sensor, and therefore the position of the opening cannot be obtained accurately.
An object of the present invention is to provide a robot control device, a robot, and a robot control method capable of reliably inserting the protrusion of the first member into the opening of the second member.

Such an object is achieved by the present invention described below.
(Application example 1)
A robot control device according to the present invention includes a robot arm to which an end effector can be attached;
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
The end effector supports one of a first member having a protrusion and a second member having an opening, and operates a robot that performs an operation of inserting the protrusion of the first member into the opening of the second member. A robot controller for controlling,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
A force displacement conversion processing unit that converts the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction detected by the force sensor into displacements; One member or the second member is supported, the end effector is moved in the X-axis direction, and the operation of the robot arm is controlled by force control based on each displacement converted by the force displacement conversion processing unit. A control unit to control;
A position specifying unit that specifies the position of the opening or the projection based on the displacement in the X-axis direction and the displacement in the Y-axis direction,
The elastic term in the Z-axis direction of the force control is set to be larger than the elastic term in the X-axis direction and the elastic term in the Y-axis direction.

Thereby, the position of the opening of the second member or the protrusion of the first member can be reliably identified, and the protrusion of the first member can be inserted into the opening of the second member.
For example, when the work is performed while the first member is supported by the end effector, when the position of the opening of the second member is obtained, the elastic term in the Z-axis direction of force control is the elastic term in the X-axis direction and the Y-axis direction. Since it is larger than the elastic term, when the tip of the projection of the first member reaches the opening, the tip of the projection is easily, quickly and reliably inserted into the opening. Thus, the projection of the first member is restrained in the X-axis direction and the Y-axis direction by the opening, and the force sensor can reliably detect the forces in the X-axis direction and the Y-axis direction. The displacement of the protrusion from the target position in the X-axis direction and the Y-axis direction can be reliably detected. Moreover, by converting the force detected by the force sensor into displacement, noise can be reduced and the S / N ratio can be increased. Thereby, the position of the opening of a 2nd member can be calculated | required reliably.

(Application example 2)
In the robot control apparatus according to the present invention, it is preferable that the end effector is pressed in the Z-axis direction when the end effector is moved in the X-axis direction.
Thereby, the position of the opening of the second member or the protrusion of the first member can be specified more reliably, and the protrusion of the first member can be inserted into the opening of the second member.

(Application example 3)
In the robot control apparatus according to the present invention, it is preferable that scanning for moving the end effector in the X-axis direction is performed a plurality of times while changing the position in the Y-axis direction.
Thereby, the position of the opening of the second member or the protrusion of the first member can be specified more reliably, and the protrusion of the first member can be inserted into the opening of the second member.

(Application example 4)
In the robot control device according to the present invention, the robot control device includes a first comparison unit that compares the absolute value of the displacement in the X-axis direction converted by the force displacement conversion processing unit with a first threshold value,
In the scanning, the position specifying unit, when the projection passes through the central portion of the opening in the Y-axis direction, when the absolute value of the displacement in the X-axis direction exceeds the first threshold value In addition, it is preferable to determine that the protrusion is located at an edge of the opening in the X-axis direction.
Thereby, the position of the opening of the second member or the protrusion of the first member can be specified more reliably, and the protrusion of the first member can be inserted into the opening of the second member.

(Application example 5)
In the robot control device according to the present invention, the end effector is configured to support the first member, and to specify the position of the opening by the position specifying unit,
A first comparison unit that compares the absolute value of the displacement in the X-axis direction converted by the force displacement conversion processing unit with a first threshold;
In the scanning, the position specifying unit, when the projection passes through the central portion of the opening in the Y-axis direction, when the absolute value of the displacement in the X-axis direction exceeds the first threshold value In addition, it is preferable to determine that the protrusion is located at an edge of the opening on the downstream side in the traveling direction in the X-axis direction.
Thereby, the position of the opening of the second member can be specified more reliably, and the protrusion of the first member can be inserted into the opening of the second member.

(Application example 6)
In the robot control device according to the present invention, the robot control device includes a second comparison unit that compares the absolute value of the displacement in the Y-axis direction converted by the force displacement conversion processing unit with a second threshold value.
The position specifying unit, when the projection passes the edge in the Y-axis direction of the opening during the scanning, when the absolute value of the displacement in the Y-axis direction exceeds the second threshold value In addition, it is preferable to determine that the protrusion is located at the center of the opening in the X-axis direction.
Thereby, the position of the opening of the second member or the protrusion of the first member can be specified more reliably, and the protrusion of the first member can be inserted into the opening of the second member.

(Application example 7)
In the robot control apparatus according to the present invention, it is preferable that the control unit includes a low-pass filter that processes a signal output from the force sensor.
Thereby, the noise contained in the signal output from a force sensor can be reduced, and S / N ratio can be made high.

(Application example 8)
In the robot control device according to the present invention, it is preferable that the elastic term in the X-axis direction and the elastic term in the Y-axis direction of the force control are set to be equal.
Thereby, the X-axis direction and the Y-axis direction can be handled equally in force control, and control can be easily performed.

(Application example 9)
A robot according to the present invention includes a robot arm to which an end effector can be attached;
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
A robot that supports one of a first member having a protrusion and a second member having an opening by the end effector, and performs an operation of inserting the protrusion of the first member into the opening of the second member. ,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
The robot control device of the present invention for controlling the operation of the robot is provided.

Thereby, the position of the opening of the second member or the protrusion of the first member can be reliably identified, and the protrusion of the first member can be inserted into the opening of the second member.
For example, when the work is performed while the first member is supported by the end effector, when the position of the opening of the second member is obtained, the elastic term in the Z-axis direction of force control is the elastic term in the X-axis direction and the Y-axis direction. Since it is larger than the elastic term, when the tip of the projection of the first member reaches the opening, the tip of the projection is easily, quickly and reliably inserted into the opening. Thus, the projection of the first member is restrained in the X-axis direction and the Y-axis direction by the opening, and the force sensor can reliably detect the forces in the X-axis direction and the Y-axis direction. The displacement of the protrusion from the target position in the X-axis direction and the Y-axis direction can be reliably detected. Moreover, by converting the force detected by the force sensor into displacement, noise can be reduced and the S / N ratio can be increased. Thereby, the position of the opening of a 2nd member can be calculated | required reliably.

(Application Example 10)
In the robot according to the present invention, the robot is preferably a double-arm robot.
Thereby, various operations can be performed and various operations can be performed.
(Application Example 11)
A robot control method according to the present invention includes a robot arm to which an end effector can be attached,
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
The end effector supports one of a first member having a protrusion and a second member having an opening, and operates a robot that performs an operation of inserting the protrusion of the first member into the opening of the second member. A robot control method for controlling,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
During the work, the end effector supports the first member or the second member, moves the end effector in the X-axis direction, and detects the X-axis direction and the Y-axis detected by the force sensor. Converting the force in the direction and the Z-axis direction into displacement, and controlling the operation of the robot arm by force control based on each displacement;
Identifying the position of the opening or the projection based on the displacement in the X-axis direction and the displacement in the Y-axis direction,
The elastic term in the Z-axis direction of the force control is set to be larger than the elastic term in the X-axis direction and the elastic term in the Y-axis direction.

Thereby, the position of the opening of the second member or the protrusion of the first member can be reliably identified, and the protrusion of the first member can be inserted into the opening of the second member.
For example, when the work is performed while the first member is supported by the end effector, when the position of the opening of the second member is obtained, the elastic term in the Z-axis direction of force control is the elastic term in the X-axis direction and the Y-axis direction. Since it is larger than the elastic term, when the tip of the projection of the first member reaches the opening, the tip of the projection is easily, quickly and reliably inserted into the opening. Thus, the projection of the first member is restrained in the X-axis direction and the Y-axis direction by the opening, and the force sensor can reliably detect the forces in the X-axis direction and the Y-axis direction. The displacement of the protrusion from the target position in the X-axis direction and the Y-axis direction can be reliably detected. Moreover, by converting the force detected by the force sensor into displacement, noise can be reduced and the S / N ratio can be increased. Thereby, the position of the opening of a 2nd member can be calculated | required reliably.

It is the schematic of the robot main body in 1st Embodiment of the robot of this invention. It is a block diagram of the robot shown in FIG. It is a block diagram of the robot shown in FIG. It is a side view which shows operation | movement of the principal part of the robot shown in FIG. It is a side view which shows operation | movement of the principal part of the robot shown in FIG. It is a figure which shows the output signal of the force sensor of the robot shown in FIG. It is a figure which shows the output signal of the force sensor of the robot shown in FIG. It is a figure which shows the output signal of the force sensor of the robot shown in FIG. It is a figure which shows the displacement amount from the target position of the front-end | tip of the insert in the robot shown in FIG. It is a figure which shows the displacement amount from the target position of the front-end | tip of the insert in the robot shown in FIG. It is the schematic of the robot main body in 2nd Embodiment of the robot of this invention.

Hereinafter, a robot control device, a robot, and a robot control method of the present invention will be described in detail based on preferred embodiments shown in the accompanying drawings.
FIG. 1 is a schematic view of a robot body in the first embodiment of the robot of the present invention. FIG. 2 is a block diagram of the robot shown in FIG. FIG. 3 is a block diagram of the robot shown in FIG. 4 and 5 are side views showing the operation of the main part of the robot shown in FIG. 1, respectively. 6 to 8 are diagrams showing output signals of the force sensor of the robot shown in FIG. FIG. 9 and FIG. 10 are diagrams showing the amount of displacement from the target position of the tip of the insert in the robot shown in FIG.

  In the following, for convenience of explanation, the upper side in FIG. 1 is referred to as “upper” or “upper”, and the lower side is referred to as “lower” or “lower”. Further, the base side in FIG. 1 is referred to as a “base end”, and the opposite side is referred to as a “tip”. Further, FIG. 3 shows a circuit portion used when impedance control (force control) and the position of the hole 47 of the insertion object 46 are obtained, and each circuit portion used for impedance control is shown in FIG. A portion of the first drive source control unit in the drive source control unit is representatively shown.

  The robot (industrial robot) 1 shown in FIGS. 1 to 3 can be used in a manufacturing process for manufacturing precision equipment such as a wristwatch, for example, and includes a robot main body (main body portion) 10 and a robot main body 10 (robot 1). And a robot control device (control unit) 20 for controlling the operation of. The robot body 10 and the robot control device 20 are electrically connected to each other. Moreover, the robot control apparatus 20 can be comprised by the personal computer (PC) etc. which incorporated CPU (Central Processing Unit), for example. Note that the robot body 10 and the robot control device 20 may be integrated or separate. The robot controller 20 will be described later in detail.

  The robot body 10 includes a base (support portion) 11, five links (arm portions) 12, 13, 14, 15, 16, and a list 17 having two links (arm portions) 18, 19, The robot arm 5 includes seven drive sources 401, 402, 403, 404, 405, 406, and 407. That is, the robot 1 is a vertical articulated (7-axis) robot in which the base 11, the links 12, 13, 14, 15, 16 and the wrist 17 are connected in this order from the base end side to the tip end side. is there. The link 12 is “first link”, the link 13 is “second link”, the link 14 is “third link”, the link 15 is “fourth link”, the link 16 is “fifth link”, and the list 17 is It can be said separately as “sixth link, seventh link”. An end effector or the like can be attached to the wrist 17.

As shown in FIG. 1, the links 12 to 16 and the list 17 are supported so as to be independently displaceable with respect to the base 11. The lengths of the links 12 to 16 and the list 17 are not particularly limited, and are appropriately set according to various conditions.
The base 11 and the first link 12 are connected via a joint 171. The joint 171 has a mechanism that supports one of the base 11 and the first link 12 connected to each other so as to be rotatable with respect to the other. In this case, the first link 12 is rotatable about the first rotation axis O1 with respect to the base 11 with the first rotation axis O1 parallel to the vertical direction as the rotation center (axis center). The first rotation axis O1 coincides with the normal line of the upper surface of the floor 101 that is the installation surface of the robot 1. The rotation about the first rotation axis O <b> 1 is performed by driving the first drive source 401. The first drive source 401 is driven by a motor 401M and a cable (not shown), and the motor 401M is controlled by the robot control device 20 via an electrically connected motor driver 301. The first drive source 401 may be configured to transmit drive from the motor 401M by a speed reducer (not shown) provided together with the motor 401M, or the speed reducer may be omitted. The base 11 of the robot body 10 houses, for example, a motor 401M and motor drivers 301 to 307.

  The first link 12 and the second link 13 are connected via a joint (joint) 172. The joint 172 has a mechanism that supports one of the first link 12 and the second link 13 connected to each other so as to be rotatable with respect to the other. In this case, the second link 13 is rotatable with respect to the first link 12 about the second rotation axis O2 with the second rotation axis O2 as the rotation center. The second rotation axis O2 is orthogonal to the first rotation axis O1. The rotation around the second rotation axis O <b> 2 is performed by driving the second drive source 402. The second drive source 402 is driven by a motor 402M and a cable (not shown), and the motor 402M is controlled by the robot control device 20 via an electrically connected motor driver 302. The second drive source 402 may be configured to transmit drive from the motor 402M by a speed reducer (not shown) provided in addition to the motor 402M, and the speed reducer may be omitted. The second rotation axis O2 may be parallel to an axis orthogonal to the first rotation axis O1. Note that the first link 12 houses a motor 402M, for example.

  The second link 13 and the third link 14 are connected via a joint (joint) 173. The joint 173 has a mechanism that supports one of the second link 13 and the third link 14 connected to each other so as to be rotatable with respect to the other. In this case, the third link 14 is rotatable with respect to the second link 13 about the third rotation axis O3 with the third rotation axis O3 as the rotation center. The third rotation axis O3 is orthogonal to the second rotation axis O2. The rotation about the third rotation axis O <b> 3 is performed by driving the third drive source 403. The third drive source 403 is driven by a motor 403M and a cable (not shown), and the motor 403M is controlled by the robot control device 20 via an electrically connected motor driver 303. The third drive source 403 may be configured to transmit drive from the motor 403M by a speed reducer (not shown) provided in addition to the motor 403M, and the speed reducer may be omitted. Note that the third rotation axis O3 may be parallel to an axis orthogonal to the second rotation axis O2. For example, a motor 403M is accommodated in the second link 13.

  The third link 14 and the fourth link 15 are connected via a joint (joint) 174. The joint 174 has a mechanism that supports one of the third link 14 and the fourth link 15 connected to each other so as to be rotatable with respect to the other. In this case, the fourth link 15 is rotatable with respect to the third link 14 around the fourth rotation axis O4 with the fourth rotation axis O4 as the rotation center. The fourth rotation axis O4 is orthogonal to the third rotation axis O3. The rotation about the fourth rotation axis O4 is performed by driving the fourth drive source 404. The fourth drive source 404 is driven by a motor 404M and a cable (not shown), and the motor 404M is controlled by the robot control device 20 via an electrically connected motor driver 304. The fourth drive source 404 may be configured to transmit drive from the motor 404M by a speed reducer (not shown) provided together with the motor 404M, or the speed reducer may be omitted. The fourth rotation axis O4 may be parallel to an axis orthogonal to the third rotation axis O3. Note that the third link 14 houses, for example, a motor 404M.

  The fourth link 15 and the fifth link 16 are connected via a joint (joint) 175. The joint 175 has a mechanism that supports one of the fourth link 15 and the fifth link 16 connected to each other so as to be rotatable with respect to the other. In this case, the fifth link 16 is rotatable with respect to the fourth link 15 about the fifth rotation axis O5 with the fifth rotation axis O5 as the rotation center. The fifth rotation axis O5 is orthogonal to the fourth rotation axis O4. The rotation about the fifth rotation axis O5 is performed by driving the fifth drive source 405. The fifth drive source 405 is driven by a motor 405M and a cable (not shown), and the motor 405M is controlled by the robot control device 20 via an electrically connected motor driver 305. The fifth drive source 405 may be configured to transmit the drive from the motor 405M by a speed reducer (not shown) provided together with the motor 405M, or the speed reducer may be omitted. The fifth rotation axis O5 may be parallel to an axis orthogonal to the fourth rotation axis O4. For example, the motor 405M is accommodated in the fourth link 15.

  The list 17 has a sixth link 18 and a seventh link 19. A hand 91 that holds a precision device such as a wristwatch, for example, as an end effector is detachably attached to the front end of the list 17. In addition, it does not specifically limit as the hand 91, For example, the thing of the structure which has a several finger part (finger) is mentioned. The number of finger portions of the hand 91 is two in the illustrated configuration, but is not limited to two and may be three or more. The robot 1 can transport the precision device by controlling the operations of the links 12 to 16 and the wrist 17 while holding the precision device with the hand 91.

  Further, the fifth link 16 and the sixth link 18 of the list 17 are connected via a joint 176. The joint 176 has a mechanism that supports one of the fifth link 16 and the sixth link 18 of the wrist 17 that are connected to each other so as to be rotatable with respect to the other. In this case, the sixth link 18 of the list 17 is rotatable with respect to the fifth link 16 about the sixth rotation axis O6 with the sixth rotation axis O6 as the rotation center. The sixth rotation axis O6 is orthogonal to the fifth rotation axis O5. The rotation about the sixth rotation axis O <b> 6 is performed by driving the sixth drive source 406. The sixth drive source 406 is driven by a motor 406M and a cable (not shown), and the motor 406M is controlled by the robot control device 20 via an electrically connected motor driver 306. The sixth drive source 406 may receive drive from the motor 406M by a speed reducer (not shown) provided together with the motor 406M, and the speed reducer may be omitted.

  Further, the sixth link 18 and the seventh link 19 of the list 17 are connected via a joint 177. The joint 177 has a mechanism for supporting one of the sixth link 18 and the seventh link 19 of the wrist 17 connected to each other so as to be rotatable with respect to the other. In this case, the seventh link 19 of the list 17 is rotatable with respect to the sixth link 18 about the seventh rotation axis O7 with the seventh rotation axis O7 as the rotation center. The seventh rotation axis O7 is orthogonal to the sixth rotation axis O6. The rotation about the seventh rotation axis O7 is performed by driving the seventh drive source 407. The drive of the seventh drive source 407 is driven by a motor 407M and a cable (not shown), and the motor 407M is controlled by the robot control device 20 via a motor driver 307 that is electrically connected. The seventh drive source 407 may be configured to transmit the drive from the motor 407M by a speed reducer (not shown) provided together with the motor 407M, and the speed reducer may be omitted. Note that the sixth rotation axis O6 may be parallel to an axis orthogonal to the fifth rotation axis O5, and the seventh rotation axis O7 may be parallel to an axis orthogonal to the sixth rotation axis O6. Good. The fifth link 16 houses, for example, motors 406M and 407M.

  The drive sources 401 to 407 include a first angle sensor 411, a second angle sensor 412, a third angle sensor 413, a fourth angle sensor 414, a fifth angle sensor 415, a first motor 401M to 407M, or a speed reducer. A six angle sensor 416 and a seventh angle sensor 417 are provided. As these angle sensors 411-417, an encoder, a rotary encoder, etc. can be used, for example. These angle sensors 411 to 417 detect the rotation angles of the motors 401M to 407M of the drive sources 401 to 407 or the rotation shaft of the speed reducer, respectively. Detection results of the angle sensors 411 to 417, that is, signals output from the angle sensors 411 to 417 are input to the robot control device 20. The motors 401M to 407M of the drive sources 401 to 407 are not particularly limited, and for example, a servo motor such as an AC servo motor or a DC servo motor is preferably used.

Further, the robot body 10 includes a force sensor 81 provided at the tip of the link 19 of the wrist 17 of the robot arm 5 (between the link 19 of the wrist 17 and the hand 91). Note that the force sensor 81 is not limited to the above position, and may be provided, for example, at the proximal end portion of the hand 91 or the like.
The force sensor 81 detects a force or moment such as a reaction force received through an insert 41 (to be described later) held by the hand 91. The force sensor 81 is not particularly limited, and various types of sensors can be used. For example, the axial direction of three axes (X axis, Y axis, Z axis) orthogonal to each other is an example. 6-axis force sensor that detects the force and the moment around each axis. In the following, the force and the moment are referred to as a force. The detection result of the force sensor 81, that is, a signal output from the force sensor 81 is input to the robot control device 20.

The robot body 10 is electrically connected to the robot control device 20. That is, the drive sources 401 to 407, the angle sensors 411 to 417, and the force sensor 81 are electrically connected to the robot control device 20, respectively.
The robot controller 20 can operate the links 12 to 16 and the list 17 independently, that is, to control the drive sources 401 to 407 independently via the motor drivers 301 to 307, respectively. Can do. In this case, the robot control device 20 performs detection using the angle sensors 411 to 417, the force sensor 81, and the like, and controls driving of the drive sources 401 to 407, for example, angular velocity, rotation angle, and the like based on the detection results. To do. In this case, the robot control device 20 performs predetermined control such as impedance control (force control) and position control. This control program is stored in advance in a recording medium built in the robot controller 20.

  As shown in FIGS. 4 and 5, the robot 1 controls the operation of the robot main body 10 (arms 12 to 16, wrist 17, hand 91, etc.) by the robot control device 20. An insert 41 that is a first member (first assembly object) having a protrusion (insertion portion) 42 and an insertion object 46 that is a second member (second assembly object) having a hole (opening) 47. One of them is gripped (supported), and the operation (operation) of inserting the protrusion 42 into the hole 47 is performed. Note that the insert 91 may be gripped by the hand 91 and the insertion target 46 may be gripped. However, in the present embodiment, a case where the insert 41 is typically gripped is illustrated. explain. When gripping the insert 41 with the hand 91, the position of the hole 47 is determined (specified), the protrusion 42 of the insert 41 is moved to the position of the hole 47, and the protrusion 42 is inserted into the hole 47. Further, when the insertion object 46 is gripped by the hand 91, the position of the protrusion 42 is obtained (specified), the hole 47 of the insertion object 46 is moved to the position of the protrusion 42, and the protrusion 42 is inserted into the hole 47. To do.

The shape of the insert 41 and the shape of the insertion object 46 are not particularly limited, but in the present embodiment, the shape of the hole 47 of the insert 41 and the insertion object 46 is typically cylindrical and the insertion object, respectively. A case where the overall shape of the object 46 is a rectangular parallelepiped will be illustrated and described. In the illustrated configuration, the insert 41 itself is a protrusion 42, and the tip of the insert 41 is sharp.
Further, the hole 47 formed in the insertion object 46 includes a through hole and a hole that does not penetrate, that is, a bottomed hole (concave portion). A hole is illustrated and described. When the hole 47 is a through hole, the insert 41 may pass through the through hole in the work.

Next, the configuration of the robot control device 20 will be described with reference to FIGS.
The robot controller 20 includes the entire robot body 10, that is, the first drive source 401, the second drive source 402, the third drive source 403, the fourth drive source 404, the fifth drive source 405, the sixth drive source 406, 7 is a device for controlling the operation of the drive source of the hand 91 mounted on the wrist drive source 407 and the wrist 17, respectively.

  As shown in FIG. 2, the robot control device 20 includes a first drive source control unit (control unit) 201 that controls the operation of the first drive source 401 and a second drive source that controls the operation of the second drive source 402. A control unit (control unit) 202, a third drive source control unit (control unit) 203 that controls the operation of the third drive source 403, and a fourth drive source control unit (control) that controls the operation of the fourth drive source 404. Section) 204, a fifth drive source control section (control section) 205 that controls the operation of the fifth drive source 405, and a sixth drive source control section (control section) 206 that controls the operation of the sixth drive source 406. And a seventh drive source control unit (control unit) 207 for controlling the operation of the seventh drive source 407.

  Here, the robot control device 20 obtains the target position of the tip portion of the list 17, that is, the target position of the hand 91 attached to the list 17, based on the contents of the processing performed by the robot body 10, and moves the hand to the target position. A trajectory for moving 91 is generated. Then, the robot controller 20 measures the rotation angle of each of the drive sources 401 to 407 for each predetermined control cycle so that the hand 91 (list 17) moves along the generated trajectory, and the measurement result is The calculated values are output to the drive source control units 201 to 207 as position commands Pc of the drive sources 401 to 407, respectively. In the above and the following, “value is input and output” and the like are described, which means “a signal corresponding to the value is input and output”.

  In addition to the position command Pc of the first drive source 401, a detection signal is input from the first angle sensor 411 to the first drive source control unit 201. In the first drive source control unit 201, the rotation angle (position feedback value Pfb) of the first drive source 401 calculated from the detection signal of the first angle sensor 411 becomes a position command Pc, and an angular velocity feedback value ωfb described later. The first drive source 401 is driven by feedback control using each detection signal so that becomes an angular velocity command ωc described later.

  That is, a position command Pc is input to a first subtracter (not shown) of the first drive source control unit 201, and a position feedback value Pfb described later is input. In the first drive source control unit 201, the number of pulses input from the first angle sensor 411 is counted, and the rotation angle of the first drive source 401 corresponding to the count value is used as the position feedback value Pfb. Is output. The first subtracter outputs a deviation between the position command Pc and the position feedback value Pfb (a value obtained by subtracting the position feedback value Pfb from the target value of the rotation angle of the first drive source 401).

  In addition, the first drive source control unit 201 performs a predetermined calculation process using the deviation input from the first subtracter and a proportional gain that is a predetermined coefficient, etc. The target value of the angular velocity of one drive source 401 is calculated. And the signal which shows the target value (command value) of the angular velocity of the 1st drive source 401 is output to a 2nd subtracter (not shown) as angular velocity command (1st angular velocity command) (omega) c. Here, in this embodiment, proportional control (P control) is performed as feedback control, but the present invention is not limited to this.

Further, the first drive source control unit 201 calculates the angular velocity of the first drive source 401 based on the frequency of the pulse signal input from the first angle sensor 411, and the angular velocity is subjected to the second subtraction as the angular velocity feedback value ωfb. Is output to the instrument.
The second subtracter receives an angular velocity command ωc and an angular velocity feedback value ωfb. The second subtracter outputs a deviation between the angular velocity command ωc and the angular velocity feedback value ωfb (a value obtained by subtracting the angular velocity feedback value ωfb from the target value of the angular velocity of the first drive source 401).

  Further, the first drive source control unit 201 uses a deviation input from the second subtractor and a predetermined coefficient, such as a proportional gain and an integral gain, to perform predetermined calculation processing including integration, A target value of angular acceleration (torque) of the first drive source 401 corresponding to the deviation is calculated. Then, the first drive source control unit 201 generates a signal indicating the target value (command value) of the angular acceleration of the first drive source 401 as an angular acceleration command (torque command). Here, in this embodiment, PI control is performed as feedback control, but the present invention is not limited to this.

The first drive source control unit 201 generates a drive signal (drive current) for the first drive source 401 based on the angular acceleration command, and supplies it to the motor 401M via the motor driver 301.
In this way, the angular acceleration, that is, the torque of the first drive source 401 becomes as equal as possible to the target value, and the position feedback value Pfb becomes as equal as possible to the position command Pc, and the angular velocity feedback. Feedback control is performed so that the value ωfb is as equal as possible to the angular velocity command ωc, and the drive current of the first drive source 401 is controlled.

Note that the second drive source control unit 202 to the seventh drive source control unit 207 are the same as the first drive source control unit 201, respectively, and thus description thereof is omitted.
Next, the control operation of the robot controller 20 in the operation in which the robot 1 grips the insert 41 with the hand 91 and inserts the protrusion 42 of the insert 41 into the hole 47 of the insertion object 46 will be described. Since the control operations of the first drive source control unit 201 to the seventh drive source control unit 207 are the same, the control operation of the first drive source control unit 201 will be typically described below.

  As shown in FIG. 3, the robot controller 20 includes a subtractor 51, a control circuit 52, a low-pass filter 53, a force displacement converter 54, an adder 55, a first comparison unit 561, and a second comparison unit. A comparison circuit 56 having 562 and a position specifying unit 57 are provided. The subtractor 51, the control circuit 52, the low-pass filter 53, the force displacement conversion unit 54, and the adder 55 constitute a part of the control unit. Note that an X axis, a Y axis, and a Z axis that are orthogonal to each other are assumed (see FIGS. 4 and 5). In this case, the direction (scanning direction) in which the hand 91 (insert 41) is moved when determining the position of the hole 47 of the insert 46 in the work is the X-axis direction, and the protrusion 42 of the insert 41 is inserted into the insert 42. The relative movement direction of the projection 42 and the hole 47 when inserted into the hole 47 of the 46 is the Z-axis direction.

  Each of the drive source control units 201 to 207 of the robot control device 20 uses forces in the X axis direction, the Y axis direction, and the Z axis direction detected by the force sensor 81 during work, respectively, by force displacement conversion functions. Based on each displacement, the operation of the robot main body 10 (robot arm 5), that is, the operation of each of the drive sources 401 to 407 is controlled based on each displacement. By performing impedance control, objects such as the insert 41 and the insertion object 46 can be made to follow each other, and even if the insert 41 is in contact with another object, an excessive force is exerted on the insert 41. It can be prevented from joining.

  This impedance control (compliance control) has the advantage of high versatility and safety, although the control is complicated. The compliance control which is one of the impedance controls will be described. Compliance means the reciprocal of the spring constant, and the spring constant represents hardness, whereas compliance means softness. Control that gives compliance, which is mechanical flexibility when an interaction is exerted between the robot 1 and the environment, is called compliance control. For example, a force sensor 81 is attached to the arm of the robot 1 (robot arm 5), and the arm of the robot 1 is postured according to sensor information (force / torque information) obtained by the force sensor 81. Is programmed to change. Specifically, the robot 1 is controlled as if a virtual spring is attached to the tip of the arm. For example, it is assumed that the spring constant of the virtual spring is 100 kg / m. If this is pressed with a force of 5 kg, the virtual spring will shrink by 5 cm. In other words, if it shrinks by 5 cm, it can be said that it is pushed with a force of 5 kg. That is, force information and position information are correlated linearly and symmetrically.

In compliance control, control is performed as if this virtual spring is attached to the tip of the arm. Specifically, the robot 1 operates in response to the input of the force sensor 81 and is controlled to move backward by 5 cm with respect to the illustrated 5 kg weight, and the position information changes in accordance with the force information. To be controlled.
Such simple compliance control does not include a time term, but the control including the time term and considering the second order term is impedance control. Specifically, the second-order term is a mass term, the first-order term is a viscosity term, and the impedance control model can be expressed by an equation of motion as shown in the following equation (1).

In the above equation (1), m is mass, μ is a viscosity term, k is an elastic term, f is a force, and x is a displacement from a target position. The first and second derivatives of x correspond to speed and acceleration, respectively. In the impedance control, a control system for providing the end effector section (hand 91), which is the tip of the arm, with the characteristic of the above equation (1) is configured. That is, control is performed as if the tip of the arm has the virtual mass, virtual viscosity term, and virtual elasticity term represented by the above equation (1).
As described above, impedance control is control that makes a contact with an object with a viscosity term and an elastic term set as objectives in a model in which a viscous element and an elastic element are connected to the mass of the tip of the arm in each direction. .

The elastic term in the Z-axis direction for impedance control is set to be larger than the elastic term in the X-axis direction and the elastic term in the Y-axis direction. As a result, when the position of the hole 47 of the insertion object 46 is obtained, when the tip of the protrusion 42 of the insert 41 reaches the hole 47, the tip of the protrusion 42 is easily and quickly and reliably inserted into the hole 47. go. Thereby, the projection 42 of the insert 41 is restrained in the X-axis direction and the Y-axis direction by the hole 47, and the force sensor 81 reliably detects the forces in the X-axis direction and the Y-axis direction. It is possible to reliably generate the displacement of the protrusion 42 of the object 41 from the target position in the X-axis direction and the Y-axis direction. Thereby, the position of the hole 47 can be calculated | required correctly.
Further, the magnitude of the elastic term in the X-axis direction and the elastic term in the Y-axis direction for impedance control is not particularly limited, and the elastic term in the X-axis direction and the elastic term in the Y-axis direction may be equal, It may be different. For example, the control can be performed more easily by setting the elastic term in the X-axis direction and the elastic term in the Y-axis direction of impedance control to be equal.

  Further, the ratio (Az / Ax) of the elastic term (Az) in the Z-axis direction and the elastic term (Ax) in the X-axis direction, the elastic term (Az) in the Z-axis direction, and the elastic term (Ay) in the Y-axis direction The ratio (Az / Ay) is not particularly limited, and is appropriately set according to various conditions, but is preferably 1.5 or more and 10 or less, and more preferably 2 or more and 5 or less. preferable. As a result, when the position of the hole 47 of the insertion object 46 is obtained, when the tip of the protrusion 42 of the insert 41 reaches the hole 47, the tip of the protrusion 42 is inserted into the hole 47 more easily, quickly and reliably. As a result, displacement of the projection 42 of the insert 41 in the X-axis direction and the Y-axis direction can be more reliably generated. Thereby, the position of the hole 47 can be calculated | required more correctly.

  As shown in FIG. 4, at the time of work, first, in order to obtain the position of the hole 47 of the insertion object 46, an operation of moving the hand 91 (insert 41) in the X-axis direction, that is, in the X-axis direction. Scanning is performed a plurality of times by changing the position in the Y-axis direction. Further, this scanning is performed while pressing the hand 91 in the Z-axis direction, thereby pressing the tip of the protrusion 42 of the insert 41 against the pressing surface. Further, the pressing surface is provided on the insertion object 46 or a structure installed in the operating range of the robot 1. In addition, as said structure, a work table etc. are mentioned, for example.

  During the work, force is detected by the force sensor 81, and signals (data) from the force sensor 81, that is, signals indicating forces in the X-axis direction, the Y-axis direction, and the Z-axis direction ( Data) is output. As shown in FIG. 6, the signals indicating the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction output from the force sensor 81 include noise in addition to the true signal. The signals indicating the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction output from the force sensor 81 have their high frequency components removed by the low-pass filter 53 and are input to the force displacement conversion unit 54. The low-pass filter 53 can reduce noise included in the signal. Hereinafter, signals (data) in three axes such as signals (data) indicating forces in the X-axis direction, the Y-axis direction, and the Z-axis direction are also simply referred to as “signals (data)”.

  7 shows the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction output from the force sensor 81 when the protrusion 42 of the insert 41 passes through the center portion of the hole 47 in the Y-axis direction. The signals shown are shown. Further, FIG. 8 shows the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction output from the force sensor 81 when the projection 42 of the insert 41 passes near the edge of the hole 47 in the Y-axis direction. A signal indicating is shown.

  In the force-displacement conversion unit 54, calculation processing using a force-displacement conversion function is performed on the input data, and data (signal) related to force is converted into data (signal) related to displacement. Since this conversion is usually a second-order integral or a second-order lag characteristic (equivalent to a second-order low-pass filter), a high-frequency component such as noise attenuates in proportion to the square of the frequency. Therefore, noise can be greatly reduced by this conversion. This conversion can reduce noise. Hereinafter, the data (signal) related to the force is also referred to as “force data (force signal)”, and the data (signal) related to the displacement is also referred to as “displacement data (displacement signal)”.

  Next, the adder 55 adds the displacement signal and the signal output from the angle sensor 411 to obtain a position feedback value Pfb. As described above, the position feedback value Pfb is input to the subtractor 51. The subtractor 51 outputs a deviation between the position command Pc and the position feedback value Pfb. This deviation is input to the control circuit 52 at the subsequent stage of the subtractor 51, and the control circuit 52 performs the above-described processes.

  Further, the displacement signals in the X-axis direction and the Y-axis direction are respectively input from the force displacement conversion unit 54 to the comparison circuit 56, compared with a predetermined threshold value in the comparison circuit 56, and a comparison signal is output from the comparison circuit 56. Is done. The comparison circuit 56 sets the level of the comparison signal to be output to a high level when the absolute value of the magnitude of the displacement signal is larger than the threshold value, and outputs it when the absolute value of the magnitude of the displacement signal is smaller than the threshold value. The level of the comparison signal is set to a low level. Accordingly, a rectangular comparison signal is output from the comparison circuit 56 (see FIGS. 9 and 10). The comparison signal output from the comparison circuit 56 is input to the position specifying unit 57. This will be described in detail below.

First, since the elastic term of impedance control in the Z direction is large, the tip of the protrusion 42 of the insert 41 is positioned at the hole 47 regardless of the position of the protrusion 41 of the insert 41 in the Y-axis direction. When reaching the tip, the tip of the protrusion 42 is quickly inserted into the hole 47 with good followability.
As shown in FIG. 9, when the protrusion 42 of the insert 41 passes through the central portion of the hole 47 in the Y-axis direction, the protrusion 42 is near the edge of the hole 47 on the downstream side in the traveling direction of the X-axis direction. When it reaches, the tip end portion of the protrusion 42 is caught in the hole 47 and is difficult to move, so that the displacement amount (displacement) in the X-axis direction, that is, the displacement signal in the X-axis direction increases rapidly. For this reason, a predetermined first threshold value to be compared with the displacement signal in the X-axis direction is set, and the absolute value of the magnitude (displacement) of the displacement signal in the X-axis direction exceeds the first threshold value. It can be determined that the protrusion 42 is located in the vicinity of the edge of the hole 47 on the downstream side in the traveling direction in the X-axis direction.

At this time, it is preferable to set the elastic term of the impedance control with respect to the rotation (moment) around the Y axis of the robot arm 5 high. As a result, the protrusion 42 of the insert 41 is less likely to escape from the hole 47, the insert 41 moves in the X-axis direction, and the amount of displacement of the insert 41 in the X-axis direction increases substantially equal to the amount of movement.
Since no force is generated in the Y-axis direction, the amount of displacement (displacement) in the Y-axis direction, that is, the absolute value of the magnitude of the displacement signal in the Y-axis direction is almost zero.

Further, as shown in FIG. 10, when the protrusion 42 of the insert 41 passes near the edge of the hole 47 in the Y-axis direction, the protrusion 42 of the insert 41 near the edge of the hole 47 has the hole 47. Is slightly inserted and abuts against the surface inclined with respect to the X-axis direction, which is the direction of travel of the insert 41, so that the force generated in the X-axis direction is small, and the force in the X-axis direction is proportional to this. The displacement is also small.
On the other hand, the displacement amount in the Y-axis direction, that is, the displacement signal in the Y-axis direction is large, and is almost maximum at the center portion of the hole 47 in the X-axis direction. For this reason, a predetermined second threshold value to be compared with the displacement signal in the Y-axis direction is set, and when the absolute value of the magnitude of the displacement signal in the Y-axis direction exceeds the second threshold value, the projection It can be determined that 42 is located at the center of the hole 47 in the X-axis direction.

  Here, the first threshold value to be compared with the displacement signal in the X-axis direction in the comparison circuit 56 is the displacement in the X-axis direction when the protrusion 42 of the insert 41 passes through the central portion of the hole 47 in the Y-axis direction. When the projection 42 of the insert 41 passes near the edge (end) of the hole 47 in the Y-axis direction, the absolute value of the magnitude of the displacement signal in the X-axis direction is smaller than the absolute value of the signal. Set to a larger value.

  In addition, the absolute value of the second threshold value to be compared with the displacement signal in the Y-axis direction in the comparison circuit 56 is the Y-axis direction when the protrusion 42 of the insert 41 passes through the central portion of the hole 47 in the Y-axis direction. If the protrusion 42 of the insert 41 passes near the edge of the hole 47 in the Y-axis direction, it is smaller than the absolute value of the magnitude of the displacement signal in the Y-axis direction. Is set to the value

  As shown in FIG. 9, when the protrusion 42 of the insert 41 passes through the center of the hole 47 in the Y-axis direction, the rectangular signal for the comparison signal in the X-axis direction indicates that the protrusion 42 has a hole. It occurs when it is located in the vicinity of the edge on the downstream side in the traveling direction of 47 in the X-axis direction. Also, no rectangular signal is generated for the comparison signal in the Y-axis direction. As a result, when the comparison signal in the X-axis direction is rectangular, the position specifying unit 57 determines that the protrusion 42 is located near the edge of the hole 47 on the downstream side in the traveling direction in the X-axis direction. That is, the first comparison unit 561 compares the absolute value of the magnitude of the displacement signal in the X-axis direction with the first threshold value, and the position specifying unit 57 determines that the absolute value of the magnitude of the displacement signal in the X-axis direction is When the first threshold value is exceeded, it is determined that the protrusion 42 is located at the edge of the hole 47 on the downstream side in the traveling direction in the X-axis direction.

  As shown in FIG. 10, when the protrusion 42 of the insert 41 passes near the edge of the hole 47 in the Y-axis direction, a rectangular signal is not generated for the comparison signal in the X-axis direction. As for the comparison signal in the Y-axis direction, the rectangular signal is generated when the protrusion 42 is positioned at the center of the hole 47 in the X-axis direction. Thereby, the position specifying unit 57 determines that the protrusion 42 is located at the center of the hole 47 in the X-axis direction when the comparison signal in the Y-axis direction is rectangular. That is, the second comparison unit 562 compares the absolute value of the magnitude of the displacement signal in the Y-axis direction with the second threshold value, and the position specifying unit 57 determines that the absolute value of the magnitude of the displacement signal in the Y-axis direction is When the second threshold value is exceeded, it is determined that the protrusion 42 is located at the center of the hole 47 in the X-axis direction.

The position specifying unit 57 obtains (specifies) the position of the hole 47, particularly the center position of the hole 47, based on the comparison signal in the X-axis direction and the comparison signal in the Y-axis direction input from the comparison circuit 56.
In the storage unit (not shown) of the position specifying unit 57, for example, the dimension of the hole 47 is added in advance, the position where the rectangular signal is generated for the comparison signal in the X axis direction, and the comparison signal in the Y axis direction is rectangular. A table indicating the relationship between the position where the shape signal is generated and the position of the center of the hole 47 is stored. The position specifying unit 57 obtains the position of the center of the hole 47 based on the position where the comparison signal in the X-axis direction input from the comparison circuit 56 is generated, the position where the comparison signal in the Y-axis direction is generated, and the table.

Next, as shown in FIG. 5, the robot arm 5 and the hand 91 are operated to insert the protrusion 42 of the insert 41 into the hole 47. In this case, impedance control is also performed.
As described above, according to the robot 1, when the work is performed while the insert 91 is supported by the hand 91, when obtaining the position of the hole 47 of the insertion object 46, the elastic term in the Z-axis direction of impedance control is obtained. Is larger than the elastic term in the X-axis direction and the elastic term in the Y-axis direction. Therefore, when the tip of the protrusion 42 of the insert 41 reaches the hole 47, the tip of the protrusion 42 can be easily, quickly and reliably. Will be inserted into. Thereby, the projection 42 of the insert 41 is restrained in the X-axis direction and the Y-axis direction by the hole 47, and the force sensor 81 reliably detects the forces in the X-axis direction and the Y-axis direction. The displacement of the protrusion 42 of the object 41 from the target position in the X-axis direction and the Y-axis direction can be reliably detected. Also, by converting the force detected by the force sensor 81 into displacement, noise can be reduced and the S / N ratio can be increased. Thereby, the position of the hole 47 of the insertion object 46 can be obtained reliably, and the protrusion 42 of the insertion object 41 can be inserted into the hole 47.

Second Embodiment
FIG. 11 is a schematic view of a robot body in the second embodiment of the robot of the present invention.
Hereinafter, the second embodiment will be described with a focus on the differences from the first embodiment described above, and the description of the same matters will be omitted.
As shown in FIG. 11, the robot 1 according to the second embodiment is a double-arm robot, and the robot body 10 has two robot arms 5 and a base (support) that supports each robot arm 5. The body part 110 is provided. For example, the robot 1 performs an operation by holding the insert 41 on one of the two hands 91 and holding the insertion object 46 on the other hand.

According to this robot 1, the same effects as those of the first embodiment described above can be obtained.
Since the robot 1 is a double-arm robot, it can perform various operations and perform various operations.
The number of robot arms is not limited to two and may be three or more.
The robot control device, the robot, and the robot control method of the present invention have been described based on the illustrated embodiment. However, the present invention is not limited to this, and the configuration of each unit is an arbitrary function having the same function. It can be replaced with the configuration of Moreover, other arbitrary structures and processes may be added to the present invention.

Further, the present invention may be a combination of any two or more configurations (features) of the above embodiments.
In addition to the servo motor, examples of the motor of each drive source include a stepping motor. Moreover, when using a stepping motor as a motor, you may use what detects the rotation angle of a motor by measuring the number of the drive pulses input into a stepping motor as an angle sensor, for example.

In the embodiment, the number of rotation axes of the robot arm is seven. However, the present invention is not limited to this, and the number of rotation axes of the robot arm is two, three, four, for example. There may be one, five, six or more than eight. That is, in this embodiment, since the list has two links, the number of robot arm links is seven. However, the present invention is not limited to this, and the number of robot arm links is not limited thereto. May be, for example, 2, 3, 4, 5, 6 or 8 or more.
In the present invention, the robot (robot main body) may be another type of robot, for example, a legged walking (running) robot having legs.

  DESCRIPTION OF SYMBOLS 1 ... Robot (industrial robot), 10 ... Robot main body 110 ... Trunk, 11 ... Base 12, 13, 14, 15, 16, 18, 19 ... Link 17 ... List 171, 172, 173, 174, 175, 176, 177 ... Joint (joint) 20 ... Robot control device 101 ... Floor 201, 202, 203, 204, 205, 206, 207 ... Drive source control unit 301, 302, 303, 304, 305, 306, 307 ... Motor driver 401, 402, 403, 404, 405, 406, 407 ... Drive source 401M, 402M, 403M, 404M, 405M, 406M, 407M ... Motor 411, 412, 413, 414, 415, 416, 417 ... Angle sensor 5 ... Robot arm 41 ... Insertion Object 42 ... Protrusion 46 ... Insert object 47 ... Hole 51 ... Subtractor 52 ... Control circuit 53 ... Low-pass filter 54 ... Force-displacement converter 55 ... Adder 56 ... Comparison circuit 561 ... First comparison unit 562 …… Second comparison unit 57 …… Position specifying unit 81 …… Force sensor 91 …… Hand O1, O2, O3, O4, O5, O6, 07 …… Rotation axis

Claims (11)

A robot arm to which an end effector can be attached;
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
The end effector supports one of a first member having a protrusion and a second member having an opening, and operates a robot that performs an operation of inserting the protrusion of the first member into the opening of the second member. A robot controller for controlling,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
A force displacement conversion processing unit that converts the forces in the X-axis direction, the Y-axis direction, and the Z-axis direction detected by the force sensor into displacements; One member or the second member is supported, the end effector is moved in the X-axis direction, and the operation of the robot arm is controlled by force control based on each displacement converted by the force displacement conversion processing unit. A control unit to control;
A position specifying unit that specifies the position of the opening or the projection based on the displacement in the X-axis direction and the displacement in the Y-axis direction,
The robot control apparatus according to claim 1, wherein an elastic term in the Z-axis direction of the force control is set to be larger than an elastic term in the X-axis direction and an elastic term in the Y-axis direction.   The robot control device according to claim 1, wherein when the end effector is moved in the X-axis direction, the end effector is pressed in the Z-axis direction.   The robot control apparatus according to claim 1, wherein scanning for moving the end effector in the X-axis direction is performed a plurality of times by changing the position in the Y-axis direction. A first comparison unit that compares the absolute value of the displacement in the X-axis direction converted by the force displacement conversion processing unit with a first threshold;
In the scanning, the position specifying unit, when the projection passes through the central portion of the opening in the Y-axis direction, when the absolute value of the displacement in the X-axis direction exceeds the first threshold value The robot control device according to claim 3, wherein the protrusion is determined to be located at an edge of the opening in the X-axis direction. The end effector is configured to support the first member and to specify the position of the opening by the position specifying unit;
A first comparison unit that compares the absolute value of the displacement in the X-axis direction converted by the force displacement conversion processing unit with a first threshold;
In the scanning, the position specifying unit, when the projection passes through the central portion of the opening in the Y-axis direction, when the absolute value of the displacement in the X-axis direction exceeds the first threshold value The robot control device according to claim 3, wherein the protrusion is determined to be located at an edge of the opening on the downstream side in the traveling direction in the X-axis direction. A second comparison unit that compares the absolute value of the displacement in the Y-axis direction converted by the force displacement conversion processing unit with a second threshold value;
The position specifying unit, when the projection passes the edge in the Y-axis direction of the opening during the scanning, when the absolute value of the displacement in the Y-axis direction exceeds the second threshold value The robot control device according to claim 3, wherein the protrusion is determined to be located at a central portion of the opening in the X-axis direction.   The robot control apparatus according to claim 1, wherein the control unit includes a low-pass filter that processes a signal output from the force sensor.   The robot control device according to any one of claims 1 to 7, wherein an elastic term in the X-axis direction and an elastic term in the Y-axis direction of the force control are set to be equal. A robot arm to which an end effector can be attached;
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
A robot that supports one of a first member having a protrusion and a second member having an opening by the end effector, and performs an operation of inserting the protrusion of the first member into the opening of the second member. ,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
A robot comprising the robot control device according to any one of claims 1 to 8, which controls the operation of the robot.   The robot according to claim 9, wherein the robot is a double-arm robot. A robot arm to which an end effector can be attached;
Provided between the robot arm and the end effector that detects forces in the X-axis direction, the Y-axis direction, and the Z-axis direction when assuming an X axis, a Y axis, and a Z axis that are orthogonal to each other. A force sensor,
The end effector supports one of a first member having a protrusion and a second member having an opening, and operates a robot that performs an operation of inserting the protrusion of the first member into the opening of the second member. A robot control method for controlling,
The relative movement direction of the protrusion and the opening when the protrusion is inserted into the opening is the Z-axis direction,
During the work, the end effector supports the first member or the second member, moves the end effector in the X-axis direction, and detects the X-axis direction and the Y-axis detected by the force sensor. Converting the force in the direction and the Z-axis direction into displacement, and controlling the operation of the robot arm by force control based on each displacement;
Identifying the position of the opening or the projection based on the displacement in the X-axis direction and the displacement in the Y-axis direction,
The robot control method according to claim 1, wherein an elastic term in the Z-axis direction of the force control is set larger than an elastic term in the X-axis direction and an elastic term in the Y-axis direction.

Images