JP2019188519A - Pin insertion device - Google Patents

Abstract

To enable acceleration of a working speed compared to a conventional one.SOLUTION: A pin insertion device comprises: actuators 1-1 to 1-5 which can be displaced in different directions out of 5 degrees of freedom excluding a rotation direction around one axis, and are connected in multiple stages; position detection parts 4-1 to 4-5 which detect positions of movable parts 12-1 to 12-5 with respect to stationary parts 11-1 to 11-5; acceleration detection parts 5-1 to 5-5 which detect accelerations of the stationary parts 11-1 to 11-5; an actuator control part 61 which adjusts a gain for a difference between the detected position and a reference position Pr, and outputs a driving circuit Ia on the basis of a current instruction value Irp as an adjustment result and the detected acceleration; an external force detection part 62 which detects an external force added to the movable parts 12-1 to 12-5 on the basis of the current instruction value Irp, or the detected acceleration and a current value of the driving circuit Ia; and a work control part 7 which controls the actuator control part 61 on the basis of the detected external force.SELECTED DRAWING: Figure 1

Description

  The present invention relates to a pin insertion device that performs a pin insertion operation for inserting a pin into a hole of a component.

  2. Description of the Related Art Conventionally, industrial robots (hereinafter referred to as robots) and the like are often used in working apparatuses that perform operations such as assembly, pressing, and polishing. This robot has an end effector such as a hand attached to the tip of an arm, and performs work by gripping an object (part or workpiece).

  On the other hand, the operation of the robot is generally controlled by position control. Therefore, when the target position programmed in advance differs from the actual position due to dimensional error or gripping position error of the object, a large external force is generated when the object comes into contact with another object, and the object is scratched or damaged. May occur.

  As a countermeasure, there is a case where a jig (so-called “buffer”) that absorbs the force generated by the position error of the object is separately installed. However, since this buffer requires different characteristics depending on the shape and material of the object, it is necessary to prepare buffers different in the number of types of objects, which are designed each time. Therefore, there are problems that the cost is increased and the apparatus is enlarged.

  On the other hand, a force sensor is installed between the robot and the end effector, and when an excessive external force is likely to be generated when an object comes into contact, the detection result of the force sensor is fed back to the robot so that no excessive external force is generated. There is also. In this case, a buffer is not necessary. However, force sensors are expensive.

  Further, when the force sensor is used, there is a problem that it is difficult to shorten the working time for the following reason.

That is, if there is an error in the position where an object comes into contact with another object, a stop command is issued by detecting that an excessive external force has been generated at the time of contact, but a robot with a large and heavy moving part and a deceleration mechanism is suddenly I can't stop.
The external force generated at the time of contact is the sum of the impact force due to inertia and the force generated by the robot at the time of contact. Here, the impact force due to inertia is proportional to the product of the mass and moving speed of the object and the robot movable part. However, since the robot has a large and heavy mechanism, it is necessary to slow down the moving speed immediately before contact in order to reduce the impact force due to inertia.

  Also, even if a stop command is issued after detecting the occurrence of excessive external force, the robot will not stop suddenly, so even if it decelerates suddenly from the time when the stop command is issued, it will stop at a position that deviates from the contact position. , Crush the object. Since the overshoot amount of the position is proportional to the moving speed, the speed at which the object is brought closer to another object must be slowed down.

  For the above reasons, it is necessary to sufficiently reduce the moving speed of the robot in an area where the object may come into contact with another object. However, in order to shorten the cycle time, it is necessary to increase the speed of transferring the object. As a result, the speed is drastically reduced in the vicinity of the contact area.

However, the end effector is attached to the tip of the force sensor. Therefore, when the robot rapidly decelerates, a force proportional to the acceleration in the negative direction is generated in the force sensor due to the influence of the mass of the end effector.
However, it is difficult to distinguish between the force proportional to the acceleration and the external force generated by contact with the object, and the deceleration time of the robot has to be significantly increased in order to distinguish.

  In addition, when a force sensor is used, there is a problem that it is difficult to compensate for the influence of gravity in real time for the following reason.

That is, the posture that the robot can take when performing operations such as assembly, pressing, and polishing is not always constant, and is often changed according to the state of the operation. For example, in an operation of polishing while tracing a curved surface, the posture needs to be continuously changed.
However, as described above, the end effector is attached to the tip of the force sensor. Therefore, when the posture of the robot is not horizontal, the force sensor has a force corresponding to the posture of the robot and the mass of the end effector due to the influence of gravity acceleration. Occurs.

  On the other hand, as a gravity compensation means for compensating the influence of gravity acceleration, for example, a method disclosed in Patent Document 1 can be cited. In this patent document 1, the force generated in the force sensor due to the influence of gravity corresponding to the posture is learned offline in advance. The work force is calculated by subtracting the learned force from the force generated during actual work. However, in this method, it is necessary to perform learning every time the object changes. Further, learning must be performed before contact with an object, and gravity compensation cannot be performed when the robot continuously changes its posture. Further, this method cannot cope with rapid acceleration / deceleration, so it must be moved at a low speed.

  In the above, the external force applied to the movable part is shown as the force generated when the object and another object come into contact with each other. However, the force generated when the end effector and the object come into contact with each other is not limited to this. It is the same.

JP 2012-115912 A

  As described above, when an operation such as assembly is performed using a robot and a force sensor, the operation time becomes longer. On the other hand, if an attempt is made to shorten the work time, the object is damaged, crushed, and contact cannot be detected correctly. It is also difficult to perform gravity compensation in real time. As described above, when the force sensor is used, it cannot cope with the rapid acceleration / deceleration of the robot and needs to be moved at a low speed. This problem also applies to a pin insertion device that performs a pin insertion operation for inserting a pin into a hole of a component, and improvement is required.

  The present invention has been made in order to solve the above-described problems, and an object of the present invention is to provide a pin insertion device that can increase the work speed in the automation of the pin insertion work as compared with the prior art.

  The pin insertion device according to the present invention has a fixed portion and a movable portion that can be displaced with respect to the fixed portion, and can be displaced in different directions out of five degrees of freedom excluding the rotation direction around one axis. A plurality of actuators connected in multiple stages, a plurality of position detectors for detecting the position of the movable part relative to the fixed part for each actuator, a plurality of acceleration detectors for detecting the acceleration of the fixed part for each actuator, and position detection Actuator control unit that adjusts the gain with respect to the difference between the position detected by the unit and the reference position, and outputs a drive current for the actuator based on the current command value that is the adjustment result and the acceleration detected by the acceleration detection unit And the current command value obtained in the actuator control unit, or the acceleration detected by the acceleration detection unit and the actuator control unit. An external force detection unit that detects an external force applied to the movable part based on the current value of the applied drive current, and a work control unit that controls the actuator control unit based on the external force detected by the external force detection unit It is characterized by.

  According to this invention, since it comprised as mentioned above, in the automation of pin insertion operation | work, it becomes possible to increase work speed rather than before.

It is a figure which shows the structural example of the pin insertion apparatus which concerns on Embodiment 1 of this invention. It is a figure which shows the structural example of the external force detection control part in Embodiment 1 of this invention. It is a figure which shows the structural example of the gain adjustment part in Embodiment 1 of this invention. It is a figure which shows the structural example of the work control part in Embodiment 1 of this invention. It is a flowchart which shows an example of the pin insertion operation | work by the pin insertion apparatus which concerns on Embodiment 1 of this invention. 6A to 6C are diagrams showing an example of pin insertion work by the pin insertion device according to Embodiment 1 of the present invention. 7A and 7B are diagrams showing an example of pin insertion work by the pin insertion device according to Embodiment 1 of the present invention. 8A to 8B are diagrams showing an example of pin insertion work by the pin insertion device according to Embodiment 1 of the present invention.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1 FIG.
1 is a diagram showing a configuration example of a pin insertion device according to Embodiment 1 of the present invention.
The pin insertion device is a device that performs a pin insertion operation for inserting the pin 50 into the hole 511 of the component 51. As shown in FIG. 1, this pin insertion device includes actuators 1-1 to 1-5, end effector 2, moving unit 3, position detecting units 4-1 to 4-5, and acceleration detecting unit 5 connected in multiple stages. -1 to 5-5, an external force detection control unit 6 and a work control unit 7 are provided. The external force detection control unit 6 includes an actuator control unit 61 and an external force detection unit 62. In FIG. 1, the position detectors 4-1 to 4-4 and the acceleration detectors 5-1 to 5-4 are not shown for easy viewing.

  The actuator 1-1 has a fixed portion 11-1 (not shown) and a movable portion 12-1 (not shown) that can be displaced (linearly moved) in the X-axis direction with respect to the fixed portion 11-1. By supplying a current to the coil placed in the magnetic field, the movable portion 12-1 can be displaced with respect to the fixed portion 11-1. The actuator 1-1 is attached to the moving unit 3, and is transferred as a whole and its posture is changed. The moving unit 3 is not an essential component and may be removed from the pin insertion device. Below, the case where the moving part 3 is used is described.

  The actuator 1-2 includes a fixed portion 11-2 (not shown) attached to the movable portion 12-1, and a movable portion 12-2 that can be displaced (rotated) around the X axis with respect to the fixed portion 11-2. The movable portion 12-2 is displaceable with respect to the fixed portion 11-2 by supplying a current to a coil placed in a magnetic field.

  The actuator 1-3 includes a fixed portion 11-3 (not shown) attached to the movable portion 12-2, and a movable portion 12- that can be displaced (linearly moved) in the Y-axis direction with respect to the fixed portion 11-3. 3 (not shown), and the current is supplied to the coil placed in the magnetic field, so that the movable portion 12-3 can be displaced with respect to the fixed portion 11-3.

  The actuator 1-4 includes a fixed portion 11-4 (not shown) attached to the movable portion 12-3, and a movable portion 12-4 that can be displaced (rotated) around the Y axis with respect to the fixed portion 11-4. The movable portion 12-4 can be displaced with respect to the fixed portion 11-4 by supplying a current to a coil placed in a magnetic field.

  The actuator 1-5 has a fixed portion 11-5 attached to the movable portion 12-4, and a movable portion 12-5 that can be displaced (linearly moved) in the Z-axis direction with respect to the fixed portion 11-5. The movable portion 12-5 can be displaced with respect to the fixed portion 11-5 by supplying current to the coil placed in the magnetic field.

  The end effector 2 is a mechanism that is attached to the movable portion 12-5 and can hold the pin 50. In FIG. 1, a gripper (hand) that can grip the pin 50 is used as the end effector 2. As the end effector 2, for example, an adsorber that can adsorb the pin 50 may be used in addition to the gripper. There may be a moving unit that moves the component 51.

  The moving unit 3 moves (transfers and changes the posture) the actuators 1-1 to 1-5. In FIG. 1, the moving unit 3 is a robot having an actuator 1-1 (fixed unit 11-1) attached to the tip and capable of moving the actuators 1-1 to 1-5.

  The position detector 4-1 is provided in the actuator 1-1 and detects the position (relative position) of the movable part 12-1 with respect to the fixed part 11-1. A signal (position signal) indicating the position detected by the position detector 4-1 is output to the actuator controller 61.

  The position detection unit 4-2 is provided in the actuator 1-2, and detects the position (relative position) of the movable unit 12-2 with respect to the fixed unit 11-2. A signal (position signal) indicating the position detected by the position detector 4-2 is output to the actuator controller 61.

  The position detection unit 4-3 is provided in the actuator 1-3, and detects the position (relative position) of the movable unit 12-3 with respect to the fixed unit 11-3. A signal (position signal) indicating the position detected by the position detection unit 4-3 is output to the actuator control unit 61.

  The position detection unit 4-4 is provided in the actuator 1-4 and detects the position (relative position) of the movable unit 12-4 with respect to the fixed unit 11-4. A signal (position signal) indicating the position detected by the position detection unit 4-4 is output to the actuator control unit 61.

  The position detection unit 4-5 is provided in the actuator 1-5 and detects the position (relative position) of the movable unit 12-5 with respect to the fixed unit 11-5. A signal (position signal) indicating the position detected by the position detection unit 4-5 is output to the actuator control unit 61.

  The acceleration detection unit 5-1 is provided in the fixed unit 11-1, and detects the acceleration of the fixed unit 11-1. At this time, the acceleration detector 5-1 detects an acceleration (αg + α1) obtained by adding one or both of the gravitational acceleration αg and the moving acceleration α1 of the fixed unit 11-1. A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 5-1 is output to the actuator controller 61.

  The acceleration detection unit 5-2 is provided in the fixed unit 11-2 and detects the acceleration (angular acceleration) of the fixed unit 11-2. At this time, the acceleration detection unit 5-2 detects an acceleration (αg + α2) obtained by adding one or both of the gravitational acceleration αg and the movement acceleration α2 of the fixed unit 11-2. A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 5-2 is output to the actuator controller 61.

  The acceleration detection unit 5-3 is provided in the fixed unit 11-3 and detects the acceleration of the fixed unit 11-3. At this time, the acceleration detection unit 5-3 detects an acceleration (αg + α3) obtained by adding one or both of the gravitational acceleration αg and the movement acceleration α3 of the fixed unit 11-3. A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 5-3 is output to the actuator controller 61.

  The acceleration detection unit 5-4 is provided in the fixed unit 11-4 and detects the acceleration (angular acceleration) of the fixed unit 11-4. At this time, the acceleration detection unit 5-4 detects an acceleration (αg + α4) obtained by adding one or both of the gravitational acceleration αg and the movement acceleration α4 of the fixed unit 11-4. A signal (acceleration signal) indicating the acceleration detected by the acceleration detection unit 5-4 is output to the actuator control unit 61.

  The acceleration detection unit 5-5 is provided in the fixed unit 11-5 and detects the acceleration of the fixed unit 11-5. At this time, the acceleration detection unit 5-5 detects the acceleration (αg + α5) obtained by adding one or both of the gravitational acceleration αg and the movement acceleration α5 of the fixed unit 11-5. FIG. 2 shows a case where the acceleration detection unit 5-5 detects acceleration (αg + α5). A signal (acceleration signal) indicating the acceleration detected by the acceleration detector 5-5 is output to the actuator controller 61.

  The actuator controller 61 adjusts the gain (loop gain) with respect to the difference between the position detected by the position detectors 4-1 to 4-5 and the reference position Pr, and the current command value Irp as the adjustment result and Based on the acceleration detected by the acceleration detectors 5-1 to 5-5, the drive current Ia for the actuators 1-1 to 1-5 is output.

The external force detection unit 62 is a current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection units 5-1 to 5-5 and the drive current Ia output by the actuator control unit 61. Based on the value, an external force (reaction force) applied to the movable parts 12-1 to 12-5 is detected. The external force detector 62 detects a force F as an external force applied to the movable parts 12-1, 12-3, and 12-5, and detects a moment Mo as an external force applied to the movable parts 12-2 and 12-4.
Configuration examples of the actuator control unit 61 and the external force detection unit 62 will be described later.

  The work control unit 7 realizes pin insertion work by the pin insertion device. At this time, the work control unit 7 controls the actuator control unit 61, the end effector 2, and the moving unit 3 based on the external force detected by the external force detection unit 62, thereby realizing the pin insertion work. The work control unit 7 controls the actuator control unit 61 by changing the reference position Pr or the gain. In addition to the external force detected by the external force detection unit 62, the work control unit 7 detects the positions detected by the position detection units 4-1 to 4-5 and the acceleration detection units 5-1 to 5-5. The pin insertion work may be realized in consideration of the acceleration, the time managed by the work control unit 7, and the like. A configuration example of the work control unit 7 will be described later.

Next, a configuration example of the external force detection control unit 6 will be described with reference to FIG. In FIG. 2, the actuator 1-5, the end effector 2, the position detector 4-5, and the acceleration detector 5-5 are also illustrated. Below, only the structure with respect to the actuator 1-5 among the structures which the external force detection control part 6 has is shown, but the external force detection control part 6 has the structure with respect to the actuators 1-1 to 1-4 similarly. FIG. 2 shows a state where the end effector 2 holds the pin 50.
As shown in FIG. 2, the external force detection control unit 6 includes a position / velocity conversion unit 63, a subtractor 64, a gain adjustment unit 65, a mass estimation unit 66, an acceleration compensation unit 67, an adder / subtractor 68, a constant current control unit 69, and An external force detector 62 is provided. In the external force detection control unit 6 shown in FIG. 2, the function units (position / speed conversion unit 63, subtractor 64, gain adjustment unit 65, mass estimation unit 66, acceleration compensation unit 67, adder / subtractor 68, and the external force detection unit 62 are excluded. The constant current control unit 69) constitutes an actuator control unit 61.

  The position / velocity conversion unit 63 differentiates the position detected by the position detection unit 4-5 and converts it into a speed. This speed indicates the speed (relative speed) of the movable part 12-5 with respect to the fixed part 11-5. A signal (speed signal) indicating the speed converted by the position speed conversion unit 63 is output to the adder / subtractor 68.

  The subtracter 64 subtracts the position detected by the position detection unit 4-5 from the reference position Pr. A signal indicating the result of subtraction by the subtractor 64 is output to the gain adjustment unit 65.

  The gain adjusting unit 65 adjusts the gain with respect to the subtraction result (positional deviation) by the subtractor 64 and outputs a current command value Irp. The gain is a compliance value in the actuator 1-5, and the compliance is the reciprocal of the spring constant, and is an index indicating hardness and softness. In the gain adjusting unit 65, the function indicating the relationship between the position deviation and the current command value Irp may be linear or non-linear. As shown in FIGS. 2 and 3, the gain adjustment unit 65 includes a loop gain measurement unit 651, a gain intersection control unit 652, and a variable gain adjustment unit 653.

  The loop gain measurement unit 651 measures the gain of the signal output from the subtracter 64. At this time, as shown in FIG. 3, the loop gain measuring unit 651 sets the signal output from the subtractor 64 to a reference frequency at which the gain should be 1 (0 dB) by the oscillator 654, that is, the gain intersection. The sine wave having the reference frequency is added via an adder 655. The signals before and after the addition of the sine wave by the loop gain measuring unit 651 are output to the gain intersection control unit 652.

  As shown in FIG. 3, the gain intersection control unit 652 compares the amplitude ratios of the signals before and after the addition of the sine wave by the loop gain measurement unit 651 by the comparator 656. A signal indicating the comparison result by the gain intersection control unit 652 is output to the variable gain adjustment unit 653.

  The variable gain adjustment unit 653 sets the gain of the signal output from the subtractor 64 as an adjustment value so that the magnification of the amplitude ratio compared by the gain intersection control unit 652 is 1, adjust. That is, the variable gain adjustment unit 653 has a higher amplitude level Eb of the signal after the addition of the sine wave than the amplitude level Ea of the signal before the addition of the sine wave by the loop gain measurement unit 651 (Ea <Eb). Increases the adjustment value and decreases the adjustment value when the amplitude level Eb of the signal after addition of the sine wave is lower than the amplitude level Ea of the signal before addition of the sine wave (Ea> Eb). Then, the gain is adjusted to be 1. The signal whose gain is adjusted by the variable gain adjusting unit 653 is output to the adder / subtractor 68 as a current command value Irp. In addition, a signal indicating the gain adjustment value by the variable gain adjustment unit 653 is output to the mass estimation unit 66.

  The reason why the sine wave having the reference frequency that should be multiplied by 1 is added by the oscillator 654 is that Ea / Eb = 1 at the frequency at which the gain is multiplied by 1, so that Ea / Eb = 1. This is because the gain intersection can always be maintained at 1 by adjusting the gain.

  The subtractor 64 and the gain adjustment unit 65 constitute position control means (phase control loop) that outputs a current command value Irp based on the difference between the position detected by the position detection unit 4-5 and the reference position Pr. .

The mass estimation unit 66 estimates the mass on the movable unit 12-5 side from the gain adjustment value by the variable gain adjustment unit 653. That is, the mass estimation unit 66 uses the principle that the change in the gain adjustment value is proportional to the change in the mass. Here, the mass on the movable part 12-5 side is the total mass of the configuration ahead of the movable part 12-5 including the movable part 12-5. For example, when the end effector 2 does not hold the pin 50, the mass on the movable unit 12-5 side is the mass obtained by adding the mass M1 of the movable unit 12-5 and the mass M2 of the end effector 2 ( M1 + M2), and when the end effector 2 holds the pin 50, the mass obtained by adding the mass M1 of the movable portion 12-5, the mass M2 of the end effector 2, and the mass M3 of the pin 50 (M1 + M2 + M3) It is. Note that FIG. 2 shows a case where the mass estimation unit 66 estimates the mass (M1 + M2 + M3) obtained by adding the mass M1 of the movable unit 12-5, the mass M2 of the end effector 2, and the mass M3 of the pin 50.
For example, if the mass on the movable part 12-5 side is twice the specified value, the gain is 1/2 of the reciprocal number, and Ea / Eb = 1/2. On the other hand, in order to make the gain 1 time, the variable gain adjustment unit 653 adjusts the gain with a double adjustment value. Then, the mass estimation unit 66 can estimate from the adjustment value of the variable gain adjustment unit 653 that the mass on the movable unit 12-5 side has changed to twice the specified value.
A signal indicating the mass estimated by the mass estimation unit 66 is output to the acceleration compensation unit 67.

  In addition, although the case where the mass by the mass estimation part 66 estimated the mass by the side of the movable part 12-5 was shown above, not only this but the information which shows the mass by the side of the movable part 12-5 is acquired using another method. May be.

  Moreover, although the mass by the side of movable part 12-5 was demonstrated above, it is the same also about the mass by the side of movable parts 12-1-12-4. That is, the mass on the movable parts 12-1 to 12-4 side is the total mass of the configuration ahead of the movable parts 12-1 to 12-4 including the movable parts 12-1 to 12-4. For example, when the end effector 2 does not hold the pin 50, the mass on the movable portion 12-4 side is the mass M1 of the movable portion 12-5, the mass M2 of the end effector 2, and the mass of the fixed portion 11-5. This is a mass (M1 + M2 + M4 + M5) obtained by adding M4 and the mass M5 of the movable portion 12-4.

  The acceleration compensation unit 67 outputs an acceleration compensation value Irc for correcting the disturbance torque. The acceleration compensation unit 67 includes a multiplier 671 and a coefficient multiplication unit 672.

  The multiplier 671 multiplies the acceleration detected by the acceleration detection unit 5-5 and the mass estimated by the mass estimation unit 66. A signal indicating a multiplication result by the multiplier 671 is output to the coefficient multiplier 672 and the external force detector 62.

  The coefficient multiplication unit 672 multiplies the multiplication result by the multiplier 671 by a coefficient (1 / Kt). Kt is a torque constant that represents the ratio between the thrust generated by the actuator 1-5 and the drive current Ia. A signal indicating the multiplication result by the coefficient multiplier 672 is output to the adder / subtractor 68 as an acceleration compensation value Irc.

  The adder / subtracter 68 adds the acceleration compensation value Irc output from the acceleration compensation unit 67 to the current command value Irp output from the gain adjustment unit 65 and subtracts the velocity signal output from the position / velocity conversion unit 63. . A signal indicating the result of addition / subtraction by the adder / subtractor 68 is output to the constant current control unit 69 as a current command value Ir.

  The constant current control unit 69 controls the drive current Ia for driving the actuator 1-5 to coincide with the current command value Ir. The constant current control unit 69 includes a subtracter 691, a drive driver 692, and a current detection unit 693.

  The subtractor 691 subtracts the current value of the drive current Ia detected by the current detector 693 from the current command value Ir output from the adder / subtractor 68. A signal indicating the result of subtraction by the subtractor 691 is output to the drive driver 692.

  The drive driver 692 generates a drive current Ia corresponding to the result of subtraction by the subtractor 691. The drive current Ia generated by the drive driver 692 is output to the actuator 1-5 via the current detection unit 693.

  The current detection unit 693 detects the current value of the drive current Ia generated by the drive driver 692. A signal indicating the current value detected by the current detector 693 is output to the subtractor 691.

  The external force detection unit 62 is based on the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5-5 and the current value of the drive current Ia output by the actuator control unit 61. The external force (force F) applied to the movable part 12-5 is detected. Specifically, the external force detection unit 62 detects the external force applied to the movable unit 12-5 based on the current command value Irp or the result of subtracting the acceleration compensation value Irc from the current value of the drive current Ia. The external force applied to the movable portion 12-5 includes a force generated when the end effector 2 comes into contact with the pin 50 and a force generated when the pin 50 held by the end effector 2 comes into contact with the component 51. Is mentioned. In FIG. 2, the external force detection unit 62 detects the external force applied to the movable unit 12-5 based on the acceleration detected by the acceleration detection unit 5-5 and the current value of the drive current Ia output by the actuator control unit 61. Shows when to do. The external force detection unit 62 illustrated in FIG. 2 includes a coefficient multiplication unit 621, a subtracter 622, and a coefficient multiplication unit 623.

  The coefficient multiplication unit 621 multiplies the multiplication result by the multiplier 671 of the acceleration compensation unit 67 by a coefficient (1 / Kt). A signal indicating the multiplication result by the coefficient multiplier 621 is output to the subtractor 622.

  The subtractor 622 subtracts the multiplication result by the coefficient multiplication unit 621 from the current value of the drive current Ia generated by the constant current control unit 69. A signal indicating the result of subtraction by the subtractor 622 is output to the coefficient multiplier 623.

  The coefficient multiplication unit 623 obtains an external force by multiplying the result of subtraction by the subtractor 622 by a coefficient (Kt). A signal indicating the external force obtained by the coefficient multiplication unit 623 is output to the work control unit 7.

  In addition, when the external force detection part 62 detects the external force added to the movable part 12-5 based on the electric current command value Irp obtained in the actuator control part 61, it has a coefficient multiplication part. The coefficient multiplication unit obtains an external force by multiplying the current command value Irp output from the gain adjustment unit 65 by a coefficient (Kt). A signal indicating the external force obtained by the coefficient multiplication unit is output to the work control unit 7.

Next, a configuration example of the work control unit 7 will be described with reference to FIG.
As illustrated in FIG. 4, the work control unit 7 includes a contact control unit 71 and a posture change control unit 72.

  The contact control unit 71 moves the end effector 2 in the direction of the hole 511 of the component 51 until the pin 50 held by the end effector 2 contacts the component 51 with a force (first force) F1. The force F1 is a force that can recognize that the pin 50 is in contact with the component 51, and is a sufficiently weak force that does not damage the pin 50, the component 51, and peripheral devices.

  The posture change control unit 72 performs the posture of the end effector 2 so that the external force applied to the movable units 12-1 to 12-5 is within the force F1 and the moment (first moment) Mo1 after the processing by the contact control unit 71. The end effector 2 is moved by a specified amount in the direction of the hole 511 while changing. The moment Mo1 is a sufficiently weak moment that does not damage the pin 50, the component 51, and the peripheral device.

Note that the contact control unit 71 and the posture change control unit 72 realize the above operation in the push-pull mode or the spring mode. Hereinafter, each operation mode will be described by taking the operation of the contact control unit 71 and the posture change control unit 72 with respect to the actuator 1-5 as an example.
The push-pull mode is an operation mode in which the actuator 1-5 generates a positive or negative thrust while the position of the actuator 1-5 is fixed. In this push-pull mode, the actuator 1-5 can generate a thrust in the positive direction to realize the pressing operation of the movable portion 12-5, and the actuator 1-5 can generate a thrust in the negative direction to move the movable portion 12-. 5 can be realized. In the push-pull mode, thrust is generated at a preset change rate from the operation start position. In the push-pull mode, the thrust is maintained when the thrust generated by the actuator 1-5 exceeds a preset threshold value or when the position of the movable portion 12-5 exceeds a preset position. (Force constant mode).
The spring mode changes the position of the movable portion 12-5 relative to the fixed portion 11-5 in accordance with an external force applied to the movable portion 12-5 in a state where the position of the actuator 1-5 can be moved by the moving portion 3. This is an operation mode in which the state is enabled. In this spring mode, when the thrust generated by the actuator 1-5 exceeds a preset threshold value or when the position of the movable portion 12-5 exceeds a preset position, the thrust is maintained. Operates (force constant mode).

  Next, the operation principle of the external force detection control unit 6 will be described. Hereinafter, as the actuator 1-5, a direct drive type linear actuator in which the generated thrust is directly transmitted to the end effector 2 will be described, and the operation principle of the external force detection control unit 6 with respect to the actuator 1-5 will be described. The actuator 1-5 is driven by the drive current Ia generated by the constant current control unit 69 in accordance with the current command value Ir.

On the other hand, the position detection unit 4-5 detects the position of the movable unit 12-5 with respect to the fixed unit 11-5.
Further, the position / velocity conversion unit 63 differentiates the position detected by the position detection unit 4-5 and converts it into a speed. This speed indicates the speed of the movable part 12-5 with respect to the fixed part 11-5.

  Moreover, the acceleration detection part 5-5 detects the acceleration of the fixing | fixed part 11-5. Hereinafter, it is assumed that the acceleration detection unit 5-5 detects an acceleration (α5 + αg) obtained by adding the movement acceleration α5 of the fixed unit 11-5 and the gravitational acceleration αg of the fixed unit 11-5.

  Further, the position detected by the position detector 4-5 is compared with the reference position Pr by the subtractor 64, and the difference is a current that is one of the elements constituting the current command value Ir via the gain adjuster 65. It is given to the adder / subtracter 68 as the command value Irp.

The current command value Ir is composed of an acceleration compensation value Irc for correcting disturbance torque in addition to the current command value Irp, and is represented by the following equation (1).
Ir = Irp + Irc (1)

  If the position is simply fed back, the control system becomes unstable. Therefore, in practice, the speed signal from the position / speed converter 63 is added as a minor loop to the minus output of the adder / subtractor 68 for stabilization.

  Further, the gain adjustment unit 65 can change the compliance value in the actuator 1-5 by changing the gain of the position control loop.

Here, focusing on the drive current Ia, the current value becomes zero when there is no disturbance torque, but the current value also changes in proportion to the disturbance torque when there is disturbance torque.
As a general disturbance torque, a reaction force F received from the end effector 2 during work, a force generated by the gravitational acceleration αg and the movement acceleration α5, a loss torque of the speed reducer, and the like can be considered. Here, since the actuator 1-5 is a direct drive type linear actuator, it does not have a speed reducer and there is little need to consider the loss torque. Therefore, the drive current Ia has a value proportional to the reaction force F received from the end effector 2 during work, the force generated by the gravitational acceleration αg, and the movement acceleration α5. In the following, it is assumed that the reaction force F is a force generated when the pin 50 contacts the component 51.

Here, the driving current Ia of the actuator 1-5, the reaction force F received from the end effector 2 during work, the movement acceleration α5 of the fixed portion 11-5, the gravitational acceleration αg of the fixed portion 11-5, and the mass of the movable portion 12-5 From M1, the mass M2 of the end effector 2, and the mass M3 of the pin 50, the relationship of the following formula (2) is established.
F + (α5 + αg) · (M1 + M2 + M3) = Kt · Ir = Kt · (Irp + Irc)
(2)
Kt is a torque constant that represents the ratio between the thrust generated by the actuator 1-5 and the drive current Ia.

Further, the acceleration compensation value Irc for correcting the disturbance torque in the equation (2) is set as the following equation (3).
(Α5 + αg) · (M1 + M2 + M3) = Kt · Irc (3)

When the acceleration compensation value Irc is set as in Expression (3), the terms α5, αg, M1, M2, and M3 disappear from Expression (2), and are rearranged as in Expression (4) below.
F = Kt · Irp (4)

  As described above, when the acceleration compensation value Irc for correcting the disturbance torque is set as shown in the equation (3), the reaction force F received from the end effector 2 during the work and the current command value Irp have a proportional relationship. .

This is because when the reaction force F received from the end effector 2 during work is zero, that is, when the pin 50 is not in contact with the component 51, the current command value Irp based on the difference between the reference position Pr and the actual position is also zero, that is, the position is It means no displacement.
The reaction force F generated when the pin 50 comes into contact with the component 51 can be known by monitoring the current command value Irp.

The equation (4) includes the movement acceleration α5 of the fixed portion 11-5, the gravitational acceleration αg of the fixed portion 11-5, the mass M1 of the movable portion 12-5, the mass M2 of the end effector 2, and the mass M3 of the pin 50. Items are not included.
That is, even when the robot suddenly moves or stops and the movement acceleration α5 occurs, and even when the robot continuously changes its posture and the gravity acceleration αg changes, the movable portion 12-5 of the actuator 1-5 changes. The reaction force F can be detected correctly without shaking.
The compliance value can also be set freely.

As described above, the reaction force F generated when the pin 50 suddenly collides with the component 51 can be known by monitoring the current command value Irp. Further, since an induced current is generated in the actuator 1-5 so as to antagonize the reaction force F, the reaction force F can also be detected from the drive current Ia.
However, in the position control loop, the response of the current command value Irp to the reaction force F is generally not fast. On the other hand, the response of the drive current Ia to the reaction force F is relatively fast because it is due to the induced current generated by the movement of the movable portion 12-5. Therefore, the reaction force F is detected not by directly monitoring the current command value Irp but by monitoring the drive current Ia.

Here, Formula (2) is as follows.
F + (α5 + αg) · (M1 + M2 + M3) = Kt · Ir = Kt · (Irp + Irc)
(2)

On the other hand, the drive current Ia can be expressed by the following equation (5).
Ia = Ir = Irp + Irc (5)

Therefore, the following equation (6) is obtained from the equations (2) and (5).
F + (α5 + αg) · (M1 + M2 + M3) = Kt · Ia (6)

Then, by subtracting ((α5 + αg) · (M1 + M2 + M3)), which is the left side of equation (3), from both sides of equation (6), the following equation (7) is obtained.
F = Kt · (Ia− (α5 + αg) · (M1 + M2 + M3) / Kt) (7)

  As shown in the equation (7), the reaction force F can be obtained by subtracting the acceleration compensation value (α5 + αg) · (M1 + M2 + M3) / Kt from the drive current Ia and applying the torque constant Kt.

Next, effects of the external force detection control unit 6 will be described.
The operation of the robot is generally controlled by position control. Therefore, when the target position programmed in advance differs from the actual position due to a dimensional error or gripping position error of the pin 50 and the component 51, a large external force is generated when the pin 50 contacts the component 51, and the pin 50 or The part 51 may be damaged or broken.

  As a countermeasure, a force sensor is installed between the robot and the end effector 2, and if an excessive external force is likely to be generated when the pin 50 and the component 51 are in contact with each other, the detection result of the force sensor is fed back to the robot. A method for preventing the occurrence of the problem is conceivable.

  However, even if a stop command is issued after detecting the occurrence of excessive external force, the robot does not stop suddenly, so even if it decelerates suddenly from the time when the stop command is issued, it stops at a position that deviates from the contact position. As a result, the pin 50 and the component 51 are crushed. Since the overshoot amount of the position is proportional to the moving speed, the speed at which the pin 50 is brought close to the component 51 has to be slowed down.

  For the above reason, it is necessary to sufficiently reduce the moving speed of the robot in the region where the pin 50 may come into contact with the component 51. However, in order to shorten the cycle time, the speed of moving the pin 50 needs to be increased. As a result, the speed is drastically reduced in the vicinity of the contact area.

  On the other hand, in the first embodiment, the actuators 1-1 to 1-5 are attached to the tip of the robot (moving unit 3), and the external force detection control unit 6 is configured to move the actuators 1-1 to 1-5 abruptly or Even when the movement accelerations α1 to α5 are generated by being stopped, and when the gravitational acceleration αg is changed by changing the posture of the actuators 1-1 to 1-5, the reaction applied to the movable parts 12-1 to 12-5 is counteracted. Force can be detected correctly and the compliance value can be changed arbitrarily. Therefore, although the robot cannot be stopped suddenly, the pin 50 and the component 51 are not crushed due to excessive position. Therefore, it is not necessary to extremely slow the speed at which the pin 50 approaches the component 51, and the work can be performed safely.

Further, when a force sensor is installed between the robot and the end effector 2, when the robot rapidly decelerates, a force proportional to the negative acceleration is generated in the force sensor due to the influence of the mass M2 of the end effector 2. .
However, it is difficult to distinguish between the force proportional to the acceleration and the external force generated by contact with the component 51 of the pin 50, and in order to distinguish, the deceleration time of the robot must be greatly increased.

  On the other hand, the external force detection control unit 6 can correctly detect the external force even when the actuators 1-1 to 1-5 are suddenly accelerated and decelerated, and detects the external force only at the time of contact. There is no need to increase the deceleration time.

In addition, when a force sensor is used, there is a problem that it is difficult to compensate for the influence of gravity in real time.
That is, the posture that the robot can take when performing the pin insertion work is not always constant, and is changed according to the work state.
However, when a force sensor is installed between the robot and the end effector 2, if the posture of the robot is not horizontal, the force sensor is affected by the gravitational acceleration αg to the posture of the robot and the mass M2 of the end effector 2. A corresponding force is generated.

  On the other hand, since the external force detection control unit 6 can correctly detect the external force even when the postures of the actuators 1-1 to 1-5 are changed and the gravitational acceleration αg is changed, the influence of gravity can be compensated in real time.

Next, an operation example of the work control unit 7 will be described with reference to FIGS. In the following, it is assumed that the pin 50 is held by the end effector 2, the component 51 is fixed to a work table or the like, and the pin 50 is moved to a position facing the hole 511 of the component 51 by the moving unit 3. FIG. 6 shows a case where the inner diameter of the hole 511 is sufficiently large and the pin 50 is greatly inclined with respect to the outer diameter of the pin 50 for easy understanding of the drawing. It is assumed that the outer diameter and the inner diameter of the hole 511 substantially coincide with each other and the pin 50 has almost no inclination. In FIG. 6, the end effector 2 is not shown.
The work control unit 7 controls the actuator control unit 61, the end effector 2, and the moving unit 3 based on the external force detected by the external force detection unit 62, thereby realizing the pin insertion work by the pin insertion device. . The work control unit 7 controls the actuator control unit 61 by changing the reference position Pr or the gain. Here, the gain adjustment unit 65 outputs the current command value Irp based on the position deviation, but the change in the gain means a change in a function indicating the relationship between the position deviation and the current command value Irp. ing. Moreover, the change of the function includes a change of the function inclination.

  In the pin insertion work by the pin insertion device, as shown in FIGS. 5 and 6A, first, the contact control unit 71 starts the end effector 2 until the pin 50 held by the end effector 2 comes into contact with the component 51 with the force F1. Is moved in the direction of the hole 511 of the component 51 (step ST1). The force F1 is a force that can recognize that the pin 50 is in contact with the component 51, and is a sufficiently weak force that does not damage the pin 50, the component 51, and peripheral devices. Thereby, for example, as shown in FIG. 6B or FIG. 6C, the pin 50 comes into contact with the component 51. Moreover, since the contact control part 71 implement | achieves said operation | movement by the push-pull mode or the spring mode, the pin 50 and the components 51 are not overloaded.

  Next, the posture change control unit 72 changes the posture of the end effector 2 in the hole 511 while changing the posture of the end effector 2 so that the external force applied to the movable units 12-1 to 12-5 is within the force F1 and the moment Mo1. A specified amount is moved in the direction (step ST2). That is, when at least one of the external forces applied to the movable parts 12-1 to 12-5 becomes a specified value (force F1 or moment Mo1), the posture change control unit 72 causes the external force to be less than the specified value. Further, one or more of the actuators 1-1 to 1-5 are driven to perform retraction. Thereafter, the posture change control unit 72 performs insertion by moving the pin 50 in the direction of the hole 511. The posture change control unit 72 repeats the above retraction and insertion until the insertion amount of the pin 50 reaches a specified amount. The moment Mo1 is a sufficiently weak moment that does not damage the pin 50, the component 51, and the peripheral device.

For example, as shown in FIG. 6B or 6C, when one point of the pin 50 is in contact with the component 51, a force (translation force) F and a moment Mo act on the pin 50. Therefore, in this case, the posture change control unit 72 cancels the external force by using a linear actuator (actuators 1-1, 1-3, 1-5) and a rotary actuator (actuators 1-2, 1- 1). 4) Drive at least one of the above.
For example, as shown in FIG. 6B, when the left end of the pin 50 abuts against the component 51 and a rightward force (translational force) F is applied to the pin 50, the posture change control unit 72 is shown in FIG. As shown in 7B, the pin 50 is moved (inserted) downward while being moved (retracted) to the right.

  For example, as shown in FIG. 8A, when two points of the pin 50 are in contact with the component 51, the force F is canceled between the two points, and only the moment Mo acts on the pin 50. Therefore, in this case, the posture change control unit 72 first drives the rotary actuator so as to cancel the moment Mo. Then, for example, as shown in FIG. 8B, the moment Mo with respect to the pin 50 is eliminated, but the force F does not cancel, so the force F acts on the pin 50. Therefore, the posture change control unit 72 drives the linear actuator so as to cancel the force F. Therefore, when the two points of the pin 50 are in contact with the component 51, the posture change control unit 72 drives both the linear actuator and the rotary actuator.

  Thereafter, the end effector 2 releases the holding of the pin 50, and the pin insertion device ends the pin insertion operation.

  With the above operation, the pin insertion operation can be performed without breaking the pin 50, the component 51 or the actuators 1-1 to 1-5 and without reducing the operation speed.

  In the above description, the moving unit 3 is a robot. However, not limited to this, a linear motion mechanism or a rotation mechanism may be used as the moving unit 3.

  As described above, according to the first embodiment, the pin insertion device includes the fixed portions 11-1 to 11-5 and the movable portion 12- that can be displaced with respect to the fixed portions 11-1 to 11-5. A plurality of actuators 1-1 to 1-5 connected in multiple stages, and displaceable in different directions out of five degrees of freedom excluding the rotation direction around one axis, A plurality of position detectors 4-1 to 4-5 that detect the positions of the movable parts 12-1 to 12-5 with respect to the fixed parts 11-1 to 11-5 for each of 1-1 to 1-5, and an actuator 1- Positions detected by a plurality of acceleration detectors 5-1 to 5-5 and position detectors 4-1 to 4-5 that detect the accelerations of the fixing units 11-1 to 11-5 every 1 to 1-5 And the reference position Pr, the gain is adjusted, and the current command value Irp, which is the adjustment result, is adjusted. Based on the acceleration detected by the acceleration detectors 5-1 to 5-5, the actuator controller 61 that outputs the drive current Ia for the actuators 1-1 to 1-5, and the current command value obtained in the actuator controller 61 External force applied to the movable parts 12-1 to 12-5 based on the Irp or the acceleration detected by the acceleration detectors 5-1 to 5-5 and the current value of the drive current Ia output by the actuator controller 61 The external force detection unit 62 that detects the force and the work control unit 7 that controls the actuator control unit 61 based on the external force detected by the external force detection unit 62 are provided. As a result, the pin insertion device according to the first embodiment can increase the work speed compared to the prior art in automating the pin insertion work.

  In the present invention, any constituent element of the embodiment can be modified or any constituent element of the embodiment can be omitted within the scope of the invention.

1-1 to 1-5 Actuator 2 End effector 3 Movement unit 4-1 to 4-5 Position detection unit 5-1 to 5-5 Acceleration detection unit 6 External force detection control unit 7 Work control unit 11-1 to 11-5 Fixed unit 12-1 to 12-5 Movable unit 50 Pin 51 Parts 61 Actuator control unit 62 External force detection unit 63 Position / velocity conversion unit 64 Subtractor 65 Gain adjustment unit 66 Mass estimation unit 67 Acceleration compensation unit 68 Addition / subtraction unit 69 Constant current control Unit 71 contact control unit 72 posture change control unit 511 hole 621 coefficient multiplication unit 622 subtractor 623 coefficient multiplication unit 651 loop gain measurement unit 652 gain intersection control unit 653 variable gain adjustment unit 654 oscillator 655 adder 656 comparator 671 multiplier 672 Coefficient multiplier 691 Subtractor 692 Drive driver 693 Current detector

Claims (3)

A plurality of actuators having a fixed part and a movable part displaceable with respect to the fixed part and capable of being displaced in different directions out of five degrees of freedom excluding the rotation direction around one axis, and connected in multiple stages When,
A plurality of position detectors for detecting the position of the movable part with respect to the fixed part for each actuator;
A plurality of acceleration detectors for detecting the acceleration of the fixed part for each actuator;
The gain is adjusted with respect to the difference between the position detected by the position detection unit and the reference position, and the drive current for the actuator is calculated based on the current command value as the adjustment result and the acceleration detected by the acceleration detection unit. An actuator controller to output,
The external force applied to the movable part is detected based on the current command value obtained in the actuator control unit, or the acceleration detected by the acceleration detection unit and the current value of the drive current output by the actuator control unit. An external force detector;
A pin insertion device comprising: a work control unit that controls the actuator control unit based on an external force detected by the external force detection unit. An end effector that is attached to a movable part at the tip of the movable part and can hold a pin,
The work control unit
The contact control unit that moves the end effector in the direction of the hole until the pin held by the end effector contacts the part having the hole with a first force. Pin insertion device. The work control unit
After the processing by the contact control unit, the end effector is defined in the direction of the hole while changing the posture of the end effector so that the external force applied to the movable unit is within the first force and within the first moment. The pin insertion device according to claim 2, further comprising a posture change control unit that moves the amount.

Images