JP2019096234A - Actuator operation switching device - Google Patents

Abstract

To correctly detect an external force applied to a movable part even when the movable part is rapidly accelerated/decelerated or when its attitude is changed and switch the operation of an actuator on the basis of the external force.SOLUTION: The present invention comprises: an actuator 1 having a fixed part 11 and a movable part 12; a position detection unit 4 for detecting the position of the movable part 12 relative to the fixed part 11; an acceleration detection unit 5 for detecting the acceleration of the fixed part 11; an actuator control unit 61 for adjusting a gain for a difference between the detected position and a reference position Pr, and outputting a drive current Ia to the actuator 1 on the basis of a current command value Irp that is the result of the adjustment and the detected acceleration; an external force detection unit 62 for detecting an external force F applied to the movable part 12 on the basis of the current command value Irp or the detected acceleration and the current value of the outputted drive current Ia; a mode control unit 72 for controlling the actuator control unit 61 in a designated operation mode; and a mode switching unit 71 for switching the operation mode of the mode control unit 72 on the basis of the detected external force F.SELECTED DRAWING: Figure 1

Description

  The present invention relates to an actuator operation switching device that switches the operation of an actuator based on an external force applied to a movable portion of the actuator.

  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 actuator operation switching device that switches the operation of the actuator based on the external force applied to the movable portion of the actuator, and an improvement is 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 actuator operation switching device capable of switching the operation of an actuator based on the above.

  An actuator operation switching device according to the present invention comprises: an actuator having a fixed portion and a movable portion displaceable with respect to the fixed portion; a position detection unit detecting a position of the movable portion relative to the fixed portion; The gain is adjusted to the difference between the position detected by the position detection unit and the acceleration detection unit to be detected, and the actuator is adjusted based on the current command value as the adjustment result and the acceleration detected by the acceleration detection unit. Movable based on an actuator control unit that outputs a drive current for the motor, a current command value obtained by the actuator control unit, or an acceleration detected by the acceleration detection unit and a current value of the drive current output by the actuator control unit Control the actuator control unit in the instructed operation mode and an external force detection unit that detects the external force applied to the control unit A mode control unit that, characterized by comprising a mode switching unit for switching the operation mode of the mode control unit based on the detected external force by the external force detecting unit.

  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 operation of the actuator based on the external force Can be switched.

It is a figure which shows the structural example of the actuator operation | movement switching apparatus based 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 example of mode transition in the mode control part in Embodiment 1 of this invention. It is a figure which shows the structural example of the connector used with the actuator operation | movement switching apparatus based on Embodiment 1 of this invention. It is a flowchart which shows an example of the assembly operation | work of the connector by the actuator operation | movement switching apparatus based on Embodiment 1 of this invention. 8A to 8E are diagrams showing an example of the assembly operation of the connector by the actuator operation switching device according to the first embodiment of the present invention. FIG. 9 is a view showing an external force detected by an external force detection control unit in the assembly operation of the connector by the actuator operation switching device shown in FIG. 8;

Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.
Embodiment 1
FIG. 1 is a diagram showing an example of the configuration of an actuator operation switching device according to a first embodiment of the present invention.
The actuator operation switching device is a device that switches the operation of the actuator 1 based on an external force (reaction force) F applied to the movable portion 12 of the actuator 1. As shown in FIG. 1, the actuator operation switching device 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, and a work control unit 7. Further, the external force detection control unit 6 includes an actuator control unit 61 and an external force detection unit 62. Further, the work control unit 7 includes a mode switching unit 71 and a mode control unit 72.

  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 attached to the movable portion 12 and has a function of directly acting on the object 50. In FIG. 1, a gripper (hand) capable of gripping an object 50 is used as the end effector 2. In addition to the gripper, for example, a suction tool capable of suctioning the object 50 may be used as the end effector 2.

  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.

  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. The external 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 operation using the actuator 1 by the actuator operation switching device. The operation control unit 7 is a dedicated controller for realizing an operation using the actuator 1 by the actuator operation switching device.

  Mode switching unit 71 is a signal (mode switching instruction signal) instructing mode control unit 72 to switch the operation mode (control unit to be operated and control content thereof) based on external force F detected by external force detection unit 62 Output). The mode switching unit 71 instructs the external force F to switch the operation mode based on comparison with a preset threshold value or time-series pattern, a moving average, or the like. In addition to the external force F detected by the external force detector 62, the mode switcher 71 manages the position detected by the position detector 4, the acceleration detected by the acceleration detector 5, and the mode switcher 71. The switching of the operation mode may be instructed in consideration of the operating time and the like.

  The mode control unit 72 switches the operation mode based on the mode switching instruction signal output by the mode switching unit 71 and controls the actuator control unit 61 and the moving unit 3. The mode control unit 72 controls the actuator control unit 61 by changing the reference position Pr or the gain (loop gain). The mode control unit 72 shown in FIG. 1 also has a function of controlling the end effector 2. A configuration example of the mode control unit 72 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, the end effector 2, the position detection unit 4, and the acceleration detection unit 5 are also illustrated. Further, FIG. 2 shows a state in which the end effector 2 holds the object 50.
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 50. When the end effector 2 holds the object 50, the mass M1 of the movable portion 12, the mass M2 of the end effector 2 and the mass M3 of the object 50 are added (M1 + M2 + M3). Note that 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 50 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. The external force F applied to the movable portion 12 is a force generated when the end effector 2 contacts the object 50, and a force generated when the object 50 held by the end effector 2 contacts another object. 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 mode control unit 72 will be described with reference to FIG.
For example, as shown in FIG. 4, the mode control unit 72 includes an initialize unit 721, an origin return unit 722, a drive off unit 723, a position / speed control unit 724, a position control unit 725, a push / pull control unit 726, a force control unit 727, A force reset unit (force change control unit) 728 and an offset cancel unit 729 are included. FIG. 5 is a diagram showing an example of mode transition in the mode control unit 72. As shown in FIG. In FIG. 5, the functional parts shown by thick circles are functional parts that are particularly characteristic to the conventional configuration.

  The initialization unit 721 drives the actuator 1 to perform initialization. In initialization, the initialization unit 721 moves the actuator 1 to the physical origin and resets the position detected by the position detection unit 4 and then resets the actuator 1 to a preset home position (for example, 3 mm from the physical origin). Move to the position). The initialization unit 721 performs the above operation at least once after the power of the actuator operation switching device is turned on (after the main power is turned on). The initialization unit 721 also performs the above operation when the actuator operation switching device is operated again after the drive-off unit 723 cuts off the drive of the actuator 1.

  The origin return unit 722 forcibly moves the actuator 1 to the home position.

  The drive off unit 723 cuts off the drive of the actuator 1. At this time, the drive-off unit 723 determines the posture of the actuator 1 based on the acceleration detected by the acceleration detection unit 5 and moves the actuator 1 to a position at which no shock occurs due to the drive disconnection of the actuator 1. The drive of the actuator 1 is cut off.

  The position and speed control unit 724 moves the actuator 1 to the designated position while controlling the speed so as not to generate a rapid acceleration.

  The position control unit 725 does not change the position of the movable unit 12 with respect to the fixed unit 11 as in the case of the force sensor. Further, similarly to the position and speed control unit 724, the position control unit 725 can also move the actuator 1 to the designated position while controlling the speed so as not to generate a rapid acceleration.

  The push-pull control unit 726 causes the actuator 1 to generate positive or negative thrust in a state where the position of the actuator 1 is fixed (push-pull mode). In the push-pull control unit 726, the actuator 1 generates thrust in the positive direction to realize the pressing operation of the movable portion 12, and the actuator 1 generates thrust in the negative direction to realize the pulling-out operation of the movable portion 12. it can. In addition, the push-pull control unit 726 generates a thrust at a preset change rate from the operation start position. Further, the push-pull control unit 726 maintains the thrust when the thrust generated by the actuator 1 exceeds a preset threshold or when the position of the movable portion 12 exceeds a preset position. Operate in (force constant mode).

The force control unit 727 changes the position of the movable portion 12 with respect to the fixed portion 11 in accordance with the external force F applied to the movable portion 12 (spring mode). In addition, the force control unit 727 maintains the thrust when the thrust generated by the actuator 1 exceeds a preset threshold or when the position of the movable portion 12 exceeds a preset position. Operate (force constant mode).
As described above, in the force control unit 727, since the movable unit 12 is in a soft state, even if the end effector 2 collides with the object 50, the object 50 can be prevented from being crushed if it is within the effective stroke range.

  In addition, mechanical characteristics (gain) in the spring mode of the force control unit 727 can be set arbitrarily. A fixed value may be set in advance for the value of the mechanical property, or the state of the actuator 1 (the external force F detected by the external force detection unit 62, the position detected by the position detection unit 4, or the acceleration detection unit 5) The change may be made based on the detected acceleration or the like. Alternatively, for example, a pressure sensor may be attached to a portion where the end effector 2 contacts, and the value of the mechanical characteristic may be changed based on the pressure detected by the pressure sensor.

The force reset unit 728 resets the thrust generated by the actuator 1 to near zero. As a result, in a state where the end effector 2 is in contact with the object 50, almost no thrust can be generated. The operation by the force reset unit 728 is effective, for example, when the force control unit 727 continuously controls the force.
In the state where force control unit 727 is in operation, mode control unit 72 causes the thrust force generated by actuator 1 to exceed a preset threshold or the position of movable unit 12 exceeds a preset position. In this case, instead of the force control unit 727, the force reset unit 728 may be automatically switched to operate.

  In addition, in the above, the case where the force reset part 728 which resets the thrust which the actuator 1 is generate | occur | produced to near zero was shown as a force change control part. However, not limited to this, the force change control unit may be a control unit that changes the thrust generated by the actuator 1 to an arbitrary value.

  The offset cancellation unit 729 cancels the offset included in the external force F detected by the external force detection control unit 6. In the external force detection control unit 6, the external force F to be detected may be offset depending on the installation environment (temperature and the like) of the actuator operation switching device. Therefore, the offset can be canceled by operating the offset cancellation unit 729.

  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 50 contacts another object.

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 50, the following equation (2) holds.
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 the reaction force F received from the end effector 2 at the time of work is zero, that is, when the object 50 is not in contact with another object, the current command value Irp based on the difference between the reference position Pr and the actual position is also zero, ie, the position Means not to be displaced.
The reaction force F generated when the object 50 contacts another object 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 50 mass M3 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 50 sharply collides with another object 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 error or gripping position error of the object 50, a large external force F is generated when the object 50 contacts another object, and the object 50 or the other Objects may be damaged or damaged.

  As a countermeasure, a force sensor is installed between the robot and the end effector 2. When an excessive external force F is likely to occur when the object 50 contacts another object, the detection result of the force sensor is fed back to the robot. It is conceivable to prevent the generation of the external force F.

  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 object 50 or another object. Then, the amount of positional excess is proportional to the moving speed, so the speed at which the object 50 approaches other objects has to be reduced.

  For the above reasons, it is necessary to reduce the moving speed of the robot sufficiently in the area where the object 50 may come in contact with another object. However, in order to shorten the cycle time, the moving speed of the object 50 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 object 50 or another object. Therefore, it is not necessary to extremely slow the speed at which the object 50 approaches other objects, and it is possible to work 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 due to the contact of the object 50 with another object, and in order to make a distinction, the deceleration time of the robot must 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 bonding operation is not always constant, and often changes 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 actuator operation | movement switching device performs the assembly operation which fits the connector 51 to the connector 52 is shown. As shown in FIG. 6, 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.

  Then, the work control unit 7 realizes the assembly work by the actuator operation switching device by controlling 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. Do. 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. The work control unit 7 is a dedicated controller for realizing work using the actuator 1 by the actuator operation switching device, and based on the external force F or the like, within a high speed control cycle of 1 kHz or more (1 ms or less). The operation of the actuator 1 can be switched.

  Further, in FIG. 9, the horizontal axis indicates time [s], and the vertical axis indicates the external force F (force signal [mV]) detected by the external force detection control unit 6. Also, reference symbols a to e in FIG. 9 indicate the states in FIG. 8A to FIG. 8E, respectively.

  In assembling the connectors 51 and 52 by the actuator operation switching device, first, as shown in FIG. 7, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the position speed control unit 724, and the position speed control unit 724 The connector 51 is brought close to the connector 52 while controlling the speed so as not to generate a rapid acceleration (step ST1).

  Then, as shown in FIGS. 7 and 8A, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the force control unit 727, and the moving unit 3 continues until the connector 51 contacts the connector 52 with the force F1. The end effector 2 is moved in the direction of the connector 52 (step ST2). As shown in FIG. 9, the force F1 is a force capable of recognizing that the connector 51 abuts on the connector 52, and is sufficiently weak not to damage the connector 51, the connector 52 and peripheral devices.

  In this step ST2, initially, the movable portion 12 operates as a spring whose thrust changes linearly with respect to displacement (spring mode). Thereafter, when the connector 51 contacts the connector 52, the external force F applied to the movable portion 12 changes, and the information is transmitted from the external force detection unit 62 to the mode switching unit 71. Then, when the external force F (thrust generated by the actuator 1) exceeds the threshold, the mode switching unit 71 instructs the mode control unit 72 to be in the force constant mode. Thereby, the connectors 51 and 52 can be brought into contact without exerting an excessive force.

  In the above, the case where the moving unit 3 causes the connector 51 to contact the connector 52 with the force F1 is shown in step ST2. However, the invention is not limited thereto. In step ST2, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the push-pull control unit 726, and the push-pull control unit 726 causes the connector 51 to contact the connector 52 with a force F1. Alternatively, the end effector 2 may be pressed in the direction of the connector 52.

In addition, at the stage when the processing in step ST2 ends, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the force reset unit 728, and the force reset unit 728 sets the thrust generated by the actuator 1 to near zero. It may be reset to As a result, in a state in which the connectors 51 and 52 are in contact with each other, almost no thrust can be generated.
When the process in step ST2 is finished, if the stroke of the movable portion 12 has a margin and the thrust generated by the actuator 1 is low, the process by the force reset unit 728 is unnecessary. In FIG. 9, the case where the process by the force reset part 728 is not performed is shown.

  Then, as shown in FIGS. 7 and 8B, the moving unit 3 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 ST3). At this time, since an excessive force is not applied when the connectors 51 and 52 are in contact with each other, it is possible to shift (slide) the relative position of the connectors 51 and 52 while maintaining the contact between the connectors 51 and 52. FIG. 8B shows the case where the connector 51 is shifted in the height direction with respect to the connector 52 in FIG. 8A, 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. 9, the mode switching unit 71 determines that the connector 51 is opposed to the insertion hole 521 of the connector 52 when the force F1 is lost. Alternatively, the mode switching unit 71 determines, from the position of the connector 51, whether the connector 51 is opposed to the insertion hole 521 of the connector 52. When the mode switching unit 71 determines that the connector 51 is opposed to the insertion hole 521 of the connector 52, the mode switching unit 71 instructs the mode control unit 72 to stop the movement of the end effector 2.

  The work control unit 7 may update the reference position Pr in the external force detection control unit 6 when the process in step ST3 is completed. At this time, the work control unit 7 sets the position of the insertion destination of the connector 51 as the reference position Pr. Thereby, compliance control can be performed also in the fitting process of the connector 51.

  Next, as shown in FIGS. 7, 8C, and 8D, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the push-pull control unit 726, and the push-pull control unit 726 causes the external force F to be the force F2. The end effector 2 is pressed in the fitting direction so that the connector 51 is inserted into the insertion hole 521 of the connector 52 (step ST4). 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. 7 and 8E, the push-pull control unit 726 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 mode switching unit 71 It is determined whether the external force F reaches the force Fref (step ST5). As shown in FIG. 9, 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. FIGS. 8E and 9 show a case where the connector 51 is detached from the connector 52 by a force F3 which is weaker than the force Fref.
On the other hand, when the external force F reaches the force Fref, 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.

  In steps ST4 and ST5, the push-pull control unit 726 gradually changes the thrust generated by the actuator 1 to a predetermined thrust (push-pull mode). Then, the external force F applied to the movable portion 12 changes in the meantime, and the information is transmitted from the external force detection unit 62 to the mode switching unit 71. Then, when the external force F (thrust generated by the actuator 1) exceeds the threshold value, the mode switching unit 71 stops the change in thrust and instructs the mode control unit 72 to be in the force constant mode.

  In the above, the case where the connector 51 is fitted to the connector 52 by the push-pull control unit 726 is shown in step ST4. However, the present invention is not limited thereto. In step ST4, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the force control unit 727, and the moving unit 3 performs the connector 51 as the connector 52 until the external force F becomes the force F2. The end effector 2 may be moved in the fitting direction so as to be inserted into the insertion hole 521 of

  In the above, the case where the push-pull control unit 726 pulls out the connector 51 so as to be detached from the connector 52 in step ST5 is shown. However, the present invention is not limited thereto. In step ST5, the mode switching unit 71 switches the operation mode of the mode control unit 72 to the force control unit 727, and the moving unit 3 detaches the connector 51 from the connector 52. May be moved in the direction opposite to the fitting direction.

  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.

  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 instruction The mode control unit 72 controls the actuator control unit 61 in the selected operation mode, and the mode switching unit 71 switches the operation mode of the mode control unit 72 based on the external force F detected by the external force detection unit 62. Even when the movable portion 12 is rapidly accelerated or decelerated or the posture is changed, the external force F applied to the movable portion 12 can be correctly detected, and the operation of the actuator 1 can be rapidly switched based on the external force F.

  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 work control unit 11 fixed unit 12 movable unit 50 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 addition / subtraction unit 69 constant current control unit 71 mode switching unit 72 mode control unit 511 claw 521 insertion hole 522 engagement 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 672 Multiplier 672 Coefficient multiplication unit 691 Subtractor 692 Drive driver 693 Current detection unit 721 Initialization unit 722 Home position return unit 723 Drive OFF part 724 position Speed control unit 725 Position control unit 726 Push / pull control unit 727 Force control unit 728 Force reset unit (force change control)
729 Offset cancellation part

Claims (7)

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 mode control unit that controls the actuator control unit in a designated operation mode;
And a mode switching unit configured to switch the operation mode of the mode control unit based on the external force detected by the external force detection unit. The mode control unit
The actuator operation switching device according to claim 1, further comprising: a push-pull control unit configured to generate a positive direction or a negative direction thrust on the actuator in a state in which the position of the actuator is fixed. The actuator operation switching device according to claim 2, wherein the push-pull control unit maintains the thrust when the thrust generated by the actuator exceeds a preset threshold. The mode control unit
The actuator according to any one of claims 1 to 3, further comprising a force control unit capable of changing the position of the movable unit relative to the fixed unit according to an external force applied to the movable unit. Operation switching device. The actuator operation switching device according to claim 4, wherein the force control unit maintains the thrust when the thrust generated by the actuator exceeds a preset threshold. The mode control unit
The actuator operation switching device according to claim 4 or 5, further comprising: a force change control unit configured to change the thrust generated by the actuator to an arbitrary value. The mode control unit operates the force change control unit instead of the force control unit when the thrust generated by the actuator exceeds a preset threshold while the force control unit is operating. The actuator operation switching device according to claim 6, characterized in that:

Images