JP2019115951A - Assembling device - Google Patents

Abstract

To correctly detect external force applied to a movable section when rapidly accelerating/decelerating the movable section and changing a posture, and perform an assembling work on the basis of the external force.SOLUTION: An assembling device includes: an actuator 1 which has a fixed section 11 and a movable section 12; a position detection section 4 which detects the position of the movable section 12 with respect to the fixed section 11; an acceleration detection section 5 which detects acceleration of the fixed section 11; an actuator control section 61 which adjusts gain with respect to a difference between the detected position and a reference position Pr and outputs driving current Ia to the actuator 1 on the basis of a current command value Irp to be the adjustment result and the detected acceleration; an external force detection section 62 which detects external force F applied to the movable section 12 on the basis of the current command value Irp or the detected acceleration and a current value of the output driving current Ia; a work control section 7 which controls the actuator control section 61 on the basis of the detected external force F; and a data processing section 9 which performs an operation correction instruction or a sign diagnosis for the work control section 7 on the basis of recorded data.SELECTED DRAWING: Figure 1

Description

  The present invention relates to an assembly apparatus that performs an assembly operation.

  BACKGROUND ART Conventionally, industrial robots (hereinafter, referred to as robots) and the like are often used in working devices that perform operations such as assembly, pressing, and polishing. In this robot, an end effector such as a hand is attached to the tip of an arm, and works by holding an object (part or work).

  On the other hand, the motion of the robot is generally controlled by position control. Therefore, when the pre-programmed target position and the actual position differ due to dimensional error or gripping position error of the object, a large external force is generated when the object contacts another object, and the object is scratched or damaged. May occur.

  As a countermeasure, a jig (so-called "buffer") for absorbing a force generated due to a position error of an object may be separately installed. However, since this buffer has different characteristics depending on the shape of the object and the material, it is necessary to prepare a buffer different by the number of types of the object, and it is designed each time. Therefore, there is a problem that the cost is increased and the size of the apparatus is increased.

  On the other hand, a method of installing a force sensor between the robot and the end effector and feeding back the detection result of the force sensor to the robot when an excessive external force is likely to occur when an object comes in contact, so that the excessive external force is not generated There is also. In this case, no buffer is required. However, force sensors are expensive.

  Moreover, when a force sensor is used, there is a problem that it is difficult to shorten the working time because of the reason described below.

That is, when there is an error in the position where an object contacts another object, it detects that an excessive external force is generated at the time of contact and issues a stop command, but the robot having a large and heavy movable part and a speed reduction mechanism Can not stop.
Further, the external force generated at the time of contact is the sum of an impact force due to inertia and a 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 of the object and the robot movable portion and the moving speed. However, since the robot has a large and heavy mechanism, in order to reduce the impact force due to inertia, it is necessary to slow the moving speed just before contact.

  In addition, the robot does not stop suddenly even if it detects that an excessive external force has been generated and issues a stop command, so even if it decelerates rapidly from the time the stop command is issued, it stops at a position deviated from the contact position. , Crush the object. Then, the amount of positional excess is proportional to the moving speed, so the speed at which an object approaches another object must be reduced.

  For the above reasons, in an area where an object may come in contact with another object, the moving speed of the robot needs to be sufficiently reduced. However, in order to shorten the cycle time, the speed at which the object is transported needs to be increased. As a result, the speed drops rapidly near the contact area.

However, the end effector is attached to the end of the force sensor. Therefore, when the robot decelerates rapidly, a force proportional to the negative acceleration is generated in the force sensor due to 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 the contact of the object, and in order to distinguish, it is necessary to make the deceleration time of the robot significantly longer.

  When a force sensor is used, there is a problem that it is difficult to compensate the influence of gravity in real time, for the reason described below.

That is, the posture that the robot can take when performing work such as assembly, pressing or polishing is not always constant, and often changes according to the state of the work. For example, in an operation of polishing while tracing a curved surface, it is necessary to continuously change the posture.
However, as described above, since the end effector is attached to the end of the force sensor, if the robot attitude is not horizontal, the force sensor exerts a force according to the robot attitude and the mass of the end effector under the influence of gravity acceleration. Occurs.

  On the other hand, as a gravity compensation means for compensating for the influence of gravitational acceleration, for example, the method disclosed in Patent Document 1 may be mentioned. In this patent document 1, the force generated in the force sensor is learned in advance by the influence of gravity according to the posture off-line. And work power is calculated by deducting the learned power from the power generated at the time of actual work. However, in this method, it is necessary to perform learning each time an object changes. In addition, learning must be performed before contact with an object, and gravity compensation can not be performed when the robot continuously changes its posture.

  In the above description, the external force applied to the movable portion is a force generated when an object comes in contact with another object. However, the present invention is not limited to this, and the force generated when an end effector contacts an object is also described. It is similar.

JP, 2012-115912, A

  As described above, when performing work such as assembly using a robot and a force sensor, the working time becomes longer. On the other hand, when trying to shorten the working time, the object is scratched, crushed, and the contact can not be detected correctly. It is also difficult to perform gravity compensation in real time. As described above, when a force sensor is used, there is a problem that the external force can not be detected correctly when the robot accelerates or decelerates rapidly or when the posture changes. This problem is the same as in the assembling apparatus for assembling parts and the like, and improvements are required.

  The present invention has been made to solve the above-described problems, and even when the movable part is rapidly accelerated or decelerated or when the posture is changed, the external force applied to the movable part can be correctly detected. An object of the present invention is to provide an assembly apparatus capable of performing an assembly operation based on the above.

  An assembling apparatus according to the present invention detects an acceleration of a fixed part, an actuator having a movable part movable with respect to the fixed part, a position detection part which detects the position of the movable part with respect to the fixed part, and the fixed part. The gain is adjusted with respect to the difference between the position detected by the acceleration detection unit and the position detection unit and the reference position, and the drive to the actuator is performed based on the current command value which is the adjustment result and the acceleration detected by the acceleration detection unit. Based on an actuator control unit that outputs a current, a current command value obtained by the actuator control unit, or an acceleration detected by the acceleration detection unit and a current value of a drive current output by the actuator control unit, An actuator control unit is controlled based on an external force detection unit that detects an external force applied and an external force detected by the external force detection unit. An operation correction to the work control unit based on the work control unit, a data recording unit for recording data indicating the external force detected by the external force detection unit and the position detected by the position detection unit, and the data recorded in the data recording unit And a data processing unit for performing an instruction or an indication diagnosis.

  According to the present invention, as configured as described above, the external force applied to the movable portion can be correctly detected even when the movable portion is rapidly accelerated or decelerated or the posture is changed, and the assembly operation is performed based on the external force. It can be carried out.

It is a figure which shows the structural example of the assembly 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 operation | work control part in Embodiment 1 of this invention. It is a figure which shows the structural example of the connector used with the assembly apparatus which concerns on Embodiment 1 of this invention. It is a flowchart which shows an example of the assembly operation of the connector by the assembly apparatus which concerns on Embodiment 1 of this invention. FIGS. 7A to 7E are diagrams showing an example of the assembly operation of the connector by the assembly apparatus according to the first embodiment of the present invention. 8A and 8B are diagrams for explaining the operation in the assembly operation of the connector by the assembling apparatus shown in FIG. 7, FIG. 8A is a diagram showing the thrust generated by the actuator, and FIG. 8B shows the position of the movable part FIG.

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1
FIG. 1 is a view showing a configuration example of an assembly apparatus according to Embodiment 1 of the present invention.
The assembling apparatus is an apparatus for performing an assembling operation of assembling the object 50a to an object (other object) 50b to be assembled. As shown in FIG. 1, the assembling apparatus includes an actuator 1, an end effector 2, a moving unit 3, a position detection unit 4, an acceleration detection unit 5, an external force detection control unit 6, a work control unit 7, a data recording unit 8 and data. A processing unit 9 is provided. Further, the external force detection control unit 6 includes an actuator control unit 61 and an external force detection unit 62.

  The actuator 1 has a fixed portion 11 and a movable portion 12 displaceable with respect to the fixed portion 11, and a current is supplied to a coil placed in a magnetic field, whereby the movable portion 12 is fixed to the fixed portion 11. It can be displaced in the linear movement direction or the rotational direction. The actuator 1 is attached to the moving unit 3 so that the whole is transported and its posture is changed. In addition, when the movable portion 12 or the end effector 2 has degrees of freedom in a plurality of directions and it is not necessary to move the entire actuator 1 and change the posture, the moving portion 3 may be omitted. In the following, the case where the moving unit 3 is used will be described.

  The end effector 2 is a mechanism attached to the movable portion 12 and capable of holding the object 50a. In FIG. 1, a gripper (hand) capable of gripping an object 50 a is used as the end effector 2. In addition to the gripper, for example, a suction tool capable of suctioning the object 50 a may be used as the end effector 2. Note that there may be a moving unit that moves the object 50b.

  The moving unit 3 moves (transfers and changes in attitude) the actuator 1. In FIG. 1, as the moving unit 3, a robot in which the actuator 1 (fixed unit 11) is attached to the tip and which can move the actuator 1 is illustrated.

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

  The acceleration detection unit 5 is provided on the fixed unit 11 and detects the acceleration of the fixed unit 11. At this time, the acceleration detection unit 5 detects an acceleration (αg + α1) in which one or both of the gravity acceleration αg and the movement acceleration α1 of the fixed unit 11 are added. FIG. 2 shows the case where the acceleration detection unit 5 detects the acceleration (αg + α1). A signal (acceleration signal) indicating the acceleration detected by the acceleration detection unit 5 is output to the actuator control unit 61.

  The actuator control unit 61 adjusts the gain (loop gain) with respect to the difference between the position detected by the position detection unit 4 and the reference position Pr, and is detected by the current command value Irp as the adjustment result and the acceleration detection unit 5 The drive current Ia for the actuator 1 is output based on the determined acceleration.

The external force detection unit 62 is movable based on the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. An external force (reaction force) F applied to the portion 12 is detected.
Configuration examples of the actuator control unit 61 and the external force detection unit 62 will be described later.

  The operation control unit 7 realizes an assembly operation by the assembly apparatus. At this time, the work control unit 7 realizes the assembly work by controlling the actuator control unit 61, the end effector 2 and the moving unit 3 based on the external force F detected by the external force detection unit 62. The work control unit 7 controls the actuator control unit 61 by changing the reference position Pr or the gain. Further, in addition to the external force F detected by the external force detection unit 62, the work control unit 7 manages the position detected by the position detection unit 4, the acceleration detected by the acceleration detection unit 5, and the work control unit 7. The above assembly operation may be realized in consideration of the time and the like. A configuration example of the work control unit 7 will be described later.

  The data recording unit 8 records data indicating the external force F detected by the external force detection unit 62 and the position detected by the position detection unit 4.

  The data processing unit 9 performs an operation correction instruction (instruction to change the reference position Pr or the gain to the actuator control unit 61) to the work control unit 7 or a symptom diagnosis based on the data recorded in the data recording unit 8.

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, the end effector 2, the position detection unit 4, and the acceleration detection unit 5 are also illustrated. Moreover, in FIG. 2, the state in which the end effector 2 hold | maintains the object 50a is shown.
As shown in FIG. 2, the external force detection control unit 6 includes a position / speed 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 external force detection control unit 6 shown in FIG. 2, functional 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 like) 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 and converts it into a velocity. This velocity indicates the velocity (relative velocity) of the movable portion 12 with respect to the fixed portion 11. A signal (velocity signal) indicating the velocity converted by the position / velocity converter 63 is output to the adder / subtractor 68.

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

  The gain adjustment 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 value of compliance at the actuator 1, and the compliance is an inverse number of a spring constant, and is an index indicating hardness and hardness. Further, in the gain adjustment 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 subtractor 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, ie, a gain intersection point. The sine waves of the reference frequency to be added are added via the adder 655. Signals before and after addition of sine waves by the loop gain measurement unit 651 are output to the gain intersection point control unit 652.

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

  The variable gain adjustment unit 653 sets the inverse of the magnification ratio to the adjustment value so that the magnification ratio of the amplitude ratio compared by the gain intersection control unit 652 is 1, and the gain of the signal output from the subtractor 64 is adjust. That is, the variable gain adjustment unit 653 is configured such that the amplitude level Eb of the signal after addition of the sine wave is higher than the amplitude level Ea of the signal before 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) And adjust so that the gain is 1 ×. The signal whose gain is adjusted by the variable gain adjustment unit 653 is output to the adder / subtractor 68 as the current command value Irp. Further, a signal indicating the adjustment value of the gain by the variable gain adjustment unit 653 is output to the mass estimation unit 66.

  It should be noted that adding a sine wave of a reference frequency at which the gain should be 1 in the oscillator 654 is Ea / Eb = 1 at a frequency at which the gain is 1 and therefore Ea / Eb = 1 The gain cross point can be always maintained at 1 by adjusting the gain to.

  Further, the subtractor 64 and the gain adjustment unit 65 constitute position control means (phase control loop) for outputting a current command value Irp based on the difference between the position detected by the position detection unit 4 and the reference position Pr.

The mass estimation unit 66 estimates the mass on the movable unit 12 side from the adjustment value of the gain by the variable gain adjustment unit 653. That is, the mass estimation unit 66 utilizes the principle that the change in gain adjustment value is proportional to the change in mass. Here, the mass on the movable portion 12 side is a mass (M1 + M2) in which the mass M1 of the movable portion 12 and the mass M2 of the end effector 2 are added when the end effector 2 does not hold the object 50a. When the end effector 2 holds the object 50a, the mass M1 of the movable portion 12, the mass M2 of the end effector 2 and the mass M3 of the object 50a are added (M1 + M2 + M3). FIG. 2 shows a case where the mass estimation unit 66 estimates a mass (M1 + M2 + M3) in which the mass M1 of the movable portion 12, the mass M2 of the end effector 2, and the mass M3 of the object 50a are added.
For example, assuming that the mass on the movable portion 12 side is twice the specified value, the gain is 1⁄2 of the reciprocal thereof, 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 an adjustment value of 2 times. Then, from the adjustment value of the variable gain adjustment unit 653, the mass estimation unit 66 can estimate that the mass on the movable unit 12 side has changed to twice the specified value.
The 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 side of the movable part 12 was estimated by the mass estimation part 66 was shown above, you may acquire the mass by the side of the movable part 12 using not only this but another method.

  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 by the mass estimated by the mass estimation unit 66. A signal indicating the multiplication result by the multiplier 671 is output to the coefficient multiplication unit 672 and the external force detection unit 62.

  The coefficient multiplication unit 672 multiplies the multiplication result of 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 and the drive current Ia. A signal indicating the multiplication result by the coefficient multiplication unit 672 is output to the adder / subtractor 68 as an acceleration compensation value Irc.

  The adder-subtractor 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 / speed conversion unit 63. . A signal indicating the addition / subtraction result of 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 to match the current command value Ir. The constant current control unit 69 includes a subtractor 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 detection unit 693 from the current command value Ir output from the adder / subtractor 68. A signal indicating the subtraction result by the subtractor 691 is output to the drive driver 692.

  The drive driver 692 generates a drive current Ia according to the subtraction result of the subtractor 691. The drive current Ia generated by the drive driver 692 is output to the actuator 1 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 detection unit 693 is output to the subtractor 691.

  The external force detection unit 62 is movable based on the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. The external force F applied to the portion 12 is detected. Specifically, external force detection unit 62 detects external force F applied to movable portion 12 based on the result of subtracting acceleration compensation value Irc from current command value Irp or the current value of drive current Ia. As the external force F applied to the movable portion 12, a force generated when the end effector 2 contacts the object 50a, and a force generated when the object 50a held by the end effector 2 contacts the object 50b It can be mentioned. Further, in FIG. 2, the external force detection unit 62 detects the external force F applied to the movable unit 12 based on the acceleration detected by the acceleration detection unit 5 and the current value of the drive current Ia output by the actuator control unit 61. It shows. The external force detection unit 62 illustrated in FIG. 2 includes a coefficient multiplication unit 621, a subtractor 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 multiplication unit 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 subtraction result by the subtractor 622 is output to the coefficient multiplication unit 623.

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

  When external force detection unit 62 detects external force F applied to movable unit 12 based on current command value Irp obtained by actuator control unit 61, it has a coefficient multiplication unit. The coefficient multiplying unit obtains the external force F by multiplying the current command value Irp output from the gain adjusting unit 65 by the coefficient (Kt). Then, a signal indicating the external force F 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.
The work control unit 7 has a contact control unit 71, an assembly position control unit 72, an assembly control unit 73, and a detachment control unit 74, as shown in FIG.

  The contact control unit 71 moves the end effector 2 in the direction of the object 50b until the object 50a held by the end effector 2 contacts the object 50b with the force (first force) F1. The force F1 is a force capable of recognizing that the object 50a is in contact with the object 50b, and is sufficiently weak so as not to damage the object 50a, the object 50b and peripheral devices.

  After the processing by the contact control unit 71, the assembly position control unit 72 maintains the contact of the object 50a held by the end effector 2 with the object 50b, and the position at which the object 50a is assembled to the object 50b. Move to The assembly position control unit 72 determines whether the object 50a is at a position where it can be assembled to the object 50b from the presence or absence of the force F1 or the position of the object 50a.

  After the processing by the assembly position control unit 72, the assembly control unit 73 controls the object 50a holding the end effector 2 on the end effector 2 until the external force F applied to the movable unit 12 becomes a force (second force) F2. Move in the direction of assembly (assembly direction).

  After the processing by the assembly control unit 73, the desorption control unit 74 moves the end effector 2 in the direction opposite to the assembly direction, and determines whether the external force F applied to the movable portion 12 reaches the force (third force) Fref. . The force Fref is a force that can confirm that the object 50a is properly assembled to the object 50b.

  Next, the operation principle of the external force detection control unit 6 will be described. In the following, as the actuator 1, a direct drive linear actuator in which the generated thrust is directly transmitted to the end effector 2 is used, and the movable portion 12 is linearly moved with respect to the fixed portion 11. The actuator 1 is driven by the drive current Ia generated by the constant current control unit 69 according to the current command value Ir.

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

  Further, the acceleration detection unit 5 detects an acceleration in the linear movement direction of the fixed unit 11. In the following, the acceleration detection unit 5 detects an acceleration (α1 + αg) in which the movement acceleration α1 in the linear movement direction component of the fixed unit 11 and the gravitational acceleration αg in the linear movement direction component of the fixed unit 11 are added. .

  Further, the position detected by the position detection unit 4 is compared with the reference position Pr by the subtractor 64, and the difference thereof is a current command value which is one of the elements constituting the current command value Ir via the gain adjustment unit 65. It is given to the adder / subtractor 68 as Irp.

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

  If the position is simply fed back, the control system becomes unstable. Therefore, the speed signal from the position / speed converter 63 is actually added as a minor loop to the minus output of the adder / subtractor 68 to perform stabilization, but this is omitted below.

  Further, in the gain adjustment unit 65, the value of the compliance in the actuator 1 can be changed 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 that when there is a disturbance torque.
As a general disturbance torque, the reaction force F received from the end effector 2 at the time of work, the force generated by the gravitational acceleration αg and the movement acceleration α1, the loss torque of the reduction gear, and the like can be considered. Here, since the actuator 1 is a direct drive linear actuator, there is no decelerator, and it is not necessary to consider the loss torque. Therefore, the drive current Ia has a value proportional to the force generated by the reaction force F, the gravitational acceleration αg and the movement acceleration α1 received from the end effector 2 at the time of operation. In the following, it is assumed that the reaction force F is a force generated when the object 50a contacts the object 50b.

Here, the drive current Ia of the actuator 1, the reaction force F received from the end effector 2 at the time of operation, the movement acceleration α1 in the linear movement direction component of the fixed portion 11, the gravitational acceleration αg in the linear movement direction component of the fixed portion 11 From the mass M1 of the above, the mass M2 of the end effector 2 and the mass M3 of the object 50a, the following equation (2) is established.
F + (α1 + αg) · (M1 + M2 + M3) = Kt · Ir = Kt · (Irp + Irc)
(2)
Kt is a torque constant representing the ratio between the thrust generated by the actuator 1 and the drive current Ia.

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

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

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

This is because, when the reaction force F received from the end effector 2 at the time of work is zero, that is, when the object 50a is not in contact with the object 50b, the current command value Irp based on the difference between the reference position Pr and the actual position is also zero. It means that it does not displace.
The reaction force F generated when the object 50a contacts the object 50b can be known by monitoring the current command value Irp.

Then, in equation (4), the movement acceleration α1 in the linear movement direction component of the fixed part 11, the gravitational acceleration αg in the linear movement direction component of the fixed part 11, the mass M1 of the movable part 12, the mass M2 of the end effector 2, the object The item of mass M3 of 50a is not included.
That is, even if the robot moves or stops rapidly and movement acceleration α1 occurs, or even if the robot continuously changes its posture and gravity acceleration αg changes, the movable part 12 of the actuator 1 does not shake and the movement is reversed. Force F can be detected correctly.
And the value of compliance can also be set freely.

As described above, the reaction force F generated when the object 50a collides sharply with the object 50b can be known by monitoring the current command value Irp. Further, since an induced current is generated in the actuator 1 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. Therefore, instead of directly monitoring the current command value Irp, the reaction force F is detected by monitoring the drive current Ia.

Here, Formula (2) is as follows.
F + (α1 + α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 + (α1 + αg) · (M1 + M2 + M3) = Kt · Ia (6)

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

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

Next, the effect of the external force detection control unit 6 will be described.
The motion of the robot is generally controlled by position control. Therefore, when the pre-programmed target position and the actual position differ due to dimensional errors or gripping position errors of the objects 50a and 50b, a large external force F is generated when the object 50a contacts the object 50b, and the object 50a or The object 50b may be scratched or broken.

  As a countermeasure, a force sensor is installed between the robot and the end effector 2, and when an excessive external force F is likely to be generated when the object 50a contacts the object 50b, the detection result of the force sensor is fed back to the robot. A method can be considered in which the external force F is not generated.

  However, the robot does not stop suddenly even if it detects that an excessive external force F has been generated and issues a stop command, so even if it decelerates rapidly from the time the stop command is issued, it stops at a position deviated from the contact position. And crush the objects 50a and 50b. Then, since the overshoot of the position is proportional to the moving speed, the speed of moving the object 50a closer to the object 50b must be reduced.

  For the above reasons, in an area where the object 50a may come into contact with the object 50b, it is necessary to sufficiently reduce the moving speed of the robot. However, in order to shorten the cycle time, the moving speed of the object 50a needs to be increased. As a result, the speed drops rapidly near the contact area.

  On the other hand, in the first embodiment, the actuator 1 is attached to the tip of the robot (moving unit 3), and the external force detection control unit 6 causes the movement acceleration α1 to occur when the actuator 1 is rapidly moved or stopped. Even when the posture of the actuator 1 is changed to change the gravitational acceleration αg, the reaction force F applied to the movable portion 12 can be correctly detected, and the compliance value can be arbitrarily changed. Therefore, although the point that the robot can not stop suddenly is the same, the overshoot of the position does not crush the objects 50a and 50b. Therefore, it is not necessary to extremely slow the speed at which the object 50a approaches the object 50b, and the operation can be performed safely.

Also, when a force sensor is installed between the robot and the end effector 2, when the robot decelerates rapidly, a force proportional to the negative acceleration is generated in the force sensor due to 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 F generated by the contact of the object 50a with the object 50b, and in order to make a distinction, the deceleration time of the robot has to be made significantly longer.

  On the other hand, the external force detection control unit 6 can correctly detect the external force F even when the actuator 1 is rapidly accelerated or decelerated, and the external force F is detected only at the time of contact.

In addition, when a force sensor is used, there is also a problem that it is difficult to compensate the influence of gravity in real time.
That is, the posture that the robot can take when performing the assembly operation is not always constant, and is often changed according to the state of the operation.
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 affects the robot posture and the mass M2 of the end effector 2 under the influence of the gravitational acceleration αg. A corresponding force is generated.

  On the other hand, in the external force detection control unit 6, even when the posture of the actuator 1 is changed and the gravitational acceleration αg is changed, the external force F can be detected correctly, so that 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. Below, the case where an assembly apparatus performs the assembly operation which fits the connector 51 to the connector 52 is shown. As shown in FIG. 5, the connector 51 has a claw 511, and the connector 52 has an insertion hole 521 into which the connector 51 is inserted and an engagement hole 522 into which the claw 511 is engaged when the connector 51 is inserted. have. The connector 51 is held by the end effector 2 and the connector 52 is fixed to a work table or the like.

  The operation control unit 7 controls the actuator control unit 61, the end effector 2 and the moving unit 3 based on the external force F or the like detected by the external force detection unit 62, thereby realizing an assembly operation by the assembling apparatus. The work control unit 7 controls the actuator control unit 61 by changing the reference position Pr or the gain. Here, although the gain adjustment unit 65 outputs the current command value Irp based on the position deviation, the change of the gain means a change of a function indicating the relationship between the position deviation and the current command value Irp. ing. In addition, the change of the function includes the change of the slope of the function.

  Further, in FIG. 8A, the horizontal axis represents time, and the vertical axis represents the thrust generated by the actuator 1. In FIG. 8B, the horizontal axis represents time, and the vertical axis represents the position of the movable portion 12. Further, reference symbols a to e in FIG. 8A and FIG. 8B respectively indicate the states in FIG. 7A to FIG. 7E.

  In the assembling operation of the connectors 51 and 52 by the assembling apparatus, first, as shown in FIGS. 6 and 7A, the contact control unit 71 sets the end effector 2 to the connector 52 until the connector 51 contacts the connector 52 with the force F1. It is moved in the direction (step ST1). As shown in FIG. 8A, the force F1 is a force capable of recognizing that the connector 51 is in contact with the connector 52, and is sufficiently weak not to damage the connector 51, the connector 52, and peripheral devices.

  Then, as shown in FIGS. 6 and 7B, the assembly position control unit 72 moves the end effector 2 in the direction in which the connector 51 and the connector 52 are fitted while maintaining the contact of the connector 51 with the connector 52. (Step ST2). FIG. 7B shows the case where the connector 51 is shifted in the height direction with respect to the connector 52 in FIG. 7A and the connector 51 is moved downward. In practice, alignment is required for all degrees of freedom in the direction perpendicular to the fitting direction, such as the lateral direction and the rotational direction, in addition to the height direction. As shown in FIG. 8, the assembly position control unit 72 determines that the connector 51 is opposed to the insertion hole 521 of the connector 52 when the force F1 is lost. Alternatively, the assembly position control unit 72 determines, from the position of the connector 51, whether the connector 51 is opposed to the insertion hole 521 of the connector 52.

  The work control unit 7 may update the reference position Pr when the processing by the assembly position control unit 72 is completed. At this time, the work control unit 7 sets the position of the insertion destination of the connector 51 inserted by the assembly control unit 73 as the reference position Pr. Accordingly, compliance control can be performed also in the fitting process of the connector 51 by the assembly control unit 73.

  Then, as shown in FIGS. 6, 7C, and 7D, the assembly control unit 73 inserts the end effector 2 so that the connector 51 is inserted into the insertion hole 521 of the connector 52 until the external force F becomes the force F2. It is moved in the mating direction (step ST3). Thus, the connector 51 can be inserted into the connector 52 with an appropriate force. Further, by inserting the connector 51 into the insertion hole 521 of the connector 52, the claw 511 of the connector 51 is engaged with the engagement hole 522 of the connector 52, and the connector 51 is engaged with the connector 52.

  Next, as shown in FIGS. 6 and 7E, the detachment control unit 74 moves the end effector 2 in the direction opposite to the fitting direction so that the connector 51 is pulled out of the connector 52, and the external force F is applied to the force Fref. It is determined whether it has reached (step ST4). As shown in FIG. 8A, the force Fref is a force that can confirm that the connector 51 is properly fitted to the connector 52.

Here, when the connector 51 is pulled out from the connector 52 before the external force F becomes the force Fref, it can be determined that the assembly of the connectors 51 and 52 has failed. The broken lines in FIG. 7E and FIG. 8 show the case where the connector 51 is disengaged from the connector 52 when the external force F is less than the force Fref.
On the other hand, when the external force F reaches the force Fref (solid line in FIG. 8), it can be determined that the assembly of the connectors 51 and 52 is successful. Thereafter, the end effector 2 releases the holding of the connector 51, and ends the assembly operation of the connectors 51 and 52.

  By the above-described operation, the assembly work for the connectors 51 and 52 can be performed without breaking the connectors 51 and 52 or the actuator 1 and reducing the working speed.

  The data recording unit 8 also records data indicating the external force F detected by the external force detection unit 62 and the position detected by the position detection unit 4. Then, based on the data stored in the data recording unit 8, the data processing unit 9 instructs the work control unit 7 to perform an operation correction or an indication diagnosis.

  For example, the data recording unit 8 records in advance data (reference data) indicating changes in the appropriate external force F and the position of the movable portion 12 in the assembly operation. Next, the operation control unit 7 carries out an assembly operation, and the data recording unit 8 records data (measured data) indicating the external force F and the position of the movable portion 12 at that time. Then, the data processing unit 9 performs feedback (performs an operation correction instruction) to the work control unit 7 so that the measured data does not deviate from the reference data by the threshold value or more. This makes it possible to correct in real time the assembly operation by the assembly apparatus to the optimal assembly conditions. Further, when the feedback amount to the work control unit 7 is equal to or more than a predetermined value, the data processing unit 9 determines that the correction amount is excessive and determines that the correction is abnormal.

  In addition, for example, the data recording unit 8 records data (measured data) indicating the external force F and the position of the movable portion 12 at that time, each time the work control unit 7 carries out the assembling operation. Then, the data processing unit 9 determines whether time-series changes (such as the external force F or the position of the movable portion 12 are gradually shifted) have occurred from the plurality of times of actual measurement data. When the data processing unit 9 determines that the above-described time-series change occurs, it determines that the data processing unit 9 is abnormal. As a result, the assembling apparatus can detect an abnormality before a failure occurs in the assembling operation, and enables a predictive diagnosis.

  Further, as described above, the data recording unit 8 records the data indicating the external force F in the assembling operation and the position of the movable portion 12 one by one, so that what kind of operation state at that time is when the trouble occurs. You can also check if it was a condition.

  The operation correction instruction and the symptom diagnosis to the work control unit 7 by the data processing unit 9 may be performed both or only one.

  In addition, in the above, the case where the actuator 1 which makes the movable part 12 displaceable in a linear motion direction was used was shown. However, the present invention is not limited to this, and it is also possible to use the actuator 1 capable of displacing the movable portion 12 in the rotational direction, as long as the acceleration detection unit 5 can detect angular acceleration.

  Moreover, the case where the movement part 3 was a robot was shown above. 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 actuator 1 having the fixed portion 11 and the movable portion 12, the position detection portion 4 for detecting the position of the movable portion 12 with respect to the fixed portion 11, and the acceleration of the fixed portion 11 The gain is adjusted with respect to the difference between the position detected by the position detection unit 4 and the acceleration detection unit 5 that detects the current position, and the reference position Pr. The actuator control unit 61 outputs the drive current Ia to the actuator 1 based on the acceleration and the current command value Irp obtained by the actuator control unit 61 or the acceleration detected by the acceleration detection unit 5 and the actuator control unit 61 An external force detection unit 62 that detects an external force F applied to the movable unit 12 based on the current value of the output drive current Ia, and an external force The operation control unit 7 that controls the actuator control unit 61 based on the external force F detected by the ejection unit 62, and the external force F detected by the external force detection unit 62 and the data indicating the position detected by the position detection unit 4 Since the data recording unit 8 and the data processing unit 9 for performing an operation correction instruction or sign diagnosis to the work control unit 7 based on the data recorded in the data recording unit 8, the movable unit 12 accelerates or decelerates rapidly Even when the posture is changed or the posture is changed, the external force F applied to the movable portion 12 can be correctly detected, and the assembly operation can be performed based on the external force F. Further, based on the external force F and the position of the movable portion 12, it is possible to perform operation correction or sign diagnosis of the assembly operation.

  In the present invention, within the scope of the invention, modifications of optional components of the embodiment or omission of optional components of the embodiment is possible.

Reference Signs List 1 actuator 2 end effector 3 moving unit 4 position detection unit 5 acceleration detection unit 6 external force detection control unit 7 operation control unit 8 data recording unit 9 data processing unit 11 fixed unit 12 movable unit 50a, 50b object 51, 52 connector 61 actuator control Unit 62 External force detection unit 63 Position / speed conversion unit 64 Subtractor 65 Gain adjustment unit 66 Mass estimation unit 67 Acceleration compensation unit 68 Add-subtractor 69 Constant current control unit 71 Contact control unit 72 Assembly position control unit 73 Assembly control unit 74 Detachment control unit 511 claw 521 insertion hole 522 engaging hole 621 coefficient multiplying unit 622 subtractor 623 coefficient multiplying unit 651 loop gain measuring unit 652 gain intersection control unit 653 variable gain adjusting unit 654 oscillator 655 adder 656 comparator 671 multiplier 672 coefficient multiplying unit 691 Subtractor 692 Drive Driver 693 Current Detector

Claims (5)

An actuator having a fixed part and a movable part displaceable with respect to the fixed part;
A position detection unit that detects the position of the movable unit with respect to the fixed unit;
An acceleration detection unit that detects an acceleration of the fixed unit;
The gain is adjusted for the difference between the position detected by the position detection unit and the reference position, and the drive current to 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 control unit that outputs
The external force applied to the movable portion is detected based on the current command value obtained by the actuator control unit or the current value of the acceleration detected by the acceleration detection unit and the drive current output by the actuator control unit. An external force detection unit,
A work control unit that controls the actuator control unit based on the external force detected by the external force detection unit;
A data recording unit that records data indicating the external force detected by the external force detection unit and the position detected by the position detection unit;
And a data processing unit for performing an operation correction instruction or an indication diagnosis to the operation control unit based on the data recorded in the data recording unit. An end effector attached to the movable portion and capable of holding an object;
The work control unit
A touch control unit is provided for moving the end effector in the direction of the other object until the object held by the end effector contacts the other object to be assembled with a first force. The assembly apparatus according to claim 1. The work control unit
After the processing by the contact control unit, the end effector is moved to a position where the object can be assembled to the other object while maintaining the contact between the object held by the end effector and the other object to be assembled. The assembly apparatus according to claim 2, further comprising an assembly position control unit. The work control unit
Having an assembly control unit for moving the end effector in a direction in which the object held by the end effector is assembled until the external force applied to the movable portion becomes a second force after processing by the assembly position control unit. The assembly apparatus according to claim 3, characterized in that: The work control unit
After the processing by the assembly control unit, the end effector is moved in a direction opposite to the direction, and a detachment control unit is determined to determine whether the external force applied to the movable portion reaches a third force. The assembling device according to Item 4.

Images