JP3529575B2 - Force control robot and control method thereof - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control method for controlling a hand effector of a robot so that a predetermined pressing force is applied to a work object, a force control robot controlled by the control method, and a control method. The present invention relates to a control device for executing a control method.

[0002]

2. Description of the Related Art When a robot performs finishing work such as grinder work, polishing work, chamfering work, deburring work, etc., there are work shape errors, setup errors, robot absolute positioning errors, etc. , Beautiful finish can not be done. When performing such finishing work, a force control robot having a function of controlling the tool (hand-effect device) of the robot to press the work (working object) with a predetermined pressing force is effective. .

In a force control robot, a target position, a target speed or a target torque of a hand effector is usually given as a target value so that a predetermined pressing force with respect to a work object can be obtained. . An example of the case where the target position is given as the target value is shown below.

In the conventional force control robot, since the work is performed under a predetermined pressing force, the target position r in the pressing direction is given by the following force control formula.

[0005]

[Equation 1] Here, in the above equation (1), the vector rp is the target trajectory at the time of position control calculated based on the teaching point, Kf is the feedback gain (force control gain), and Fd is the magnitude of the target pressing force in the designated direction. Now, F is the magnitude of the detection force in the designated direction, the vector Fa is the correction direction unit vector, and the second term on the right side is the correction amount vector (correction amount) (see FIG. 12).

That is, with respect to the target position trajectory rp,
The target position is corrected by the correction amount obtained by multiplying the deviation Fd-F between the target pressing force and the detected pressing force by the feedback gain Kf. Note that the above formula (1) is represented by a continuous system in order to make the meaning of the formula easy to understand, but naturally, since a normal robot is digitally controlled, in an actual control system, sampling is performed. It is a discrete formula.

As shown in the above equation (1), when the actual pressing force does not reach the target pressing force, the target position is corrected in the direction of pressing the work object, while the pressing force is the target. If the pressing force is larger than the target pressing force, the target position is corrected in the direction away from the work object. By correcting the target position in this way, force control is performed so that a predetermined pressing force can be obtained. In addition to the method of giving the target position as the target value as described above, there is also a method of giving the target speed and the target torque.

Conventionally, in such a force control robot, a force control gain that determines a command value to be corrected so as to obtain a target pressing force based on the detected pressing force (for example, Equation 1 above).
A constant value (constant) is used as the feedback gain Kf in.

[0009]

However, when chamfering work and deburring work are performed on a work such as a casting, it is not always said that the tool is in contact with the work because of variations in the shape of each work. Not exclusively. That is, for example, when there is an unexpected hollow in the work due to chipping of the casting or when the shape error of the casting is large, the tool is separated from the work. If the tool continues to run away from the workpiece in this way, the machining quality will be significantly reduced. For example, in the chamfering work, a region where chamfering cannot be performed will occur, and in the deburring work, a burr residue will occur. Therefore, when the tool is separated from the work, the contact between the tool and the work needs to be promptly restored.

By the way, when the control is performed based on the above equation (1), the approach speed of the tool to the work is determined by the force control gain Kf when the tool is separated from the work. Become. That is, since the pressing force F is 0 while the tool is away from the work, the product of the target pressing force Fd and the force control gain Kf is the approach speed. Therefore, if it is desired to increase the approach speed, the force control gain Kf should be increased.

Further, when the work has an unexpected convex portion, an excessive force may act to damage the work, the tool, the robot, etc. Therefore, the excessive force is promptly removed. Need to be controlled. Also in this case, it is effective to increase the force control gain Kf.

However, it is generally difficult to sufficiently increase the force control gain Kf when the rigidity of the robot mechanism or the work object is high. That is, if the force control gain Kf is excessively increased, it is possible to quickly recover from an unsteady state such as the state where the tool is separated from the work or a state where an excessive force is applied to the tool to the steady state. However, there is a problem that the control in the steady state becomes unstable.

This problem becomes noticeable when, for example, the casting surface is grinder-finished or when the machining burrs on the ridgeline between the casting surface and the machined surface are removed by chamfering. That is, for example, when grind finishing is performed on the casting surface, if the force control gain is increased, even fine delicate irregularities on the casting surface are followed, and conversely a beautiful chamfered surface cannot be obtained. In the finishing process and the burr chamfering process as described above, the beauty of the finished surface is the first requirement, and therefore it is preferable to follow the delicate irregularities of the casting surface to some extent. In order to satisfy this requirement, the force control gain must be reduced to reduce the followability to the shape change of the work.

As described above, there is a demand that the force control gain must be increased and a conflicting demand that the force control gain must be reduced depending on the situation.

The present invention has been made in consideration of the above, and even for a work having a large shape error,
It is an object of the present invention to provide a force control robot capable of obtaining sufficient finishing accuracy. Specifically, when the tool is separated from the work due to a dent in the work, the contact between the tool and the work is promptly restored, and when an excessive pressing force acts due to the protrusion of the work, etc. , It is possible to quickly release excessive pressing force, and it is possible to obtain a beautifully finished surface by following a certain degree of dullness without being overly sensitive to subtle unevenness such as casting surface. An object of the present invention is to provide a force control robot and a control method thereof.

In order to achieve such an object, the present invention comprises a hand effector for performing work on a work object, and at least one control shaft for controlling the position, speed, force, etc. of the hand effector, In a robot control method for controlling the control axis based on a predetermined target value relating to the action of the hand effector, a step of detecting the pressing force of the hand effector with respect to the work object, and the detected pressing force. A step of calculating a deviation from a predetermined target pressing force, a step of calculating a feedback gain for correcting the deviation based on the deviation, and a step of calculating a correction amount by the deviation and the feedback gain. , the correction amount and a step of correcting the target value is added to the target value, before
The feedback gain has a predetermined absolute value of the deviation.
The first feedback applied if it is less than the threshold
Gain and the absolute value of the deviation are greater than the specified threshold.
From the second feedback gain applied when the threshold
And the magnitude of the second feedback gain is
It is set to be larger than the size of 1 feedback gain,
The magnitude of the first feedback gain is
It is set to increase continuously as the absolute value increases.
And the magnitude of the second feedback gain is constant.
Before the absolute value of the deviation is equal to the threshold value
The magnitude of the first feedback gain is the same as that of the second feedback.
It is characterized by being equal to the magnitude of the feedback gain .

According to the present invention, the deviation becomes large when the hand effector is separated from the work object or when an excessive pressing force is exerted by the protrusion of the work or the like, so that the feedback gain is reduced. It is set to a large value. Therefore, the contact between the hand effector and the work target can be quickly recovered, and an excessive pressing force can be quickly released. On the other hand, when the hand effector follows the work object to some extent, the deviation is small, and therefore the feedback gain is set to a small value. For this reason, it is possible to obtain a beautifully finished surface by following a certain degree of dullness without excessively sensitively following delicate irregularities such as a cast surface.

[0018]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings.

First Embodiment First, the first embodiment will be described. FIG. 1, FIG. 2, FIG. 8 and FIG. 12 are views showing a first embodiment of the present invention.

First, the overall structure of the force control robot according to the present invention will be described with reference to FIG. As shown in FIG.
The force control robot 1 includes a robot body 10 and a control device 20 for controlling the robot body 10.

As shown in FIG. 8, the robot body 10 includes an arm 11 and a hand effector 12 provided at the tip of the arm 11 for finishing the work object 2. Hereinafter, the case where the robot body 10 is a 6-axis cylindrical coordinate type robot will be described, but the form of the robot body 10 is not particularly limited, and it may be any type such as a rectangular coordinate type, a polar coordinate type, and an articulated type. It may be in the form.

Here, the finishing work is represented by a grinder work, a polishing work, a chamfering work, a deburring work, etc., and means a work requiring smoothness and beauty of a processed surface (finished surface). Therefore, the hand effector 12 in this case
As such, a grinder in general (for example, a grindstone grinder), a chamfering cutter, and a feather cloth grinder are used.

The arm 11 comprises a plurality of arm elements 11i, and each arm element 11i is driven by the corresponding servo motor 14i and drive mechanism. Each servo motor 14i has a controller 20 which will be described later.
The servo driver 28 of FIG.

Further, each servo motor 14i has an angle sensor 16i for detecting a rotation angle θi with respect to the reference position of each servo motor 14i (the position of the tip of each arm element 11i is detected by these rotation angles). For example, a rotary encoder is provided.

On the tip side of the arm 11, a 6-axis force sensor 1 as a pressing force detecting device (pressing force detecting means) for detecting the pressing force F of the hand effector 12 against the work object 2.
7 is provided.

Further, the control device 20 has a control configuration as shown in FIG. 1, and the control device 20 sets the target trajectory of the hand effector 12 as an example of a predetermined target value for the action of the hand effector 12. It has a trajectory calculator 21 for calculating. In addition, the trajectory calculation unit 21 calculates the target angle of each servo motor 14i (that is, the target position of the tip of each arm element 11i) based on the target trajectory.
2 is connected.

Further, the controller 20 detects the rotation speed dθi / dt of each servo motor 14i (that is, the moving speed of the tip of each arm element 11i) based on the change in the angle data from the angle sensor 16i. Detector 25
Is provided.

In addition, the control device 20 includes an inverse coordinate transformation unit 2
A position control calculation unit 23 and a speed control calculation unit 24 that give a command value to the servo driver 28 so as to follow the target position of each arm element 11i tip calculated by 2 (that is, the rotation angle of each servo motor 14i) are provided. ing.

In this embodiment, the speed control calculation by the speed control calculation 24 is performed by digital control, and the servo driver 28 has an analog control configuration. This configuration is one configuration example and is not limited. There is no problem even if the speed control calculation is configured by analog.

A position / speed control system is constituted by the trajectory calculation unit 21, the inverse coordinate conversion unit 22, the position control calculation unit 23, the speed control calculation unit 24, the speed detection unit 25, and the angle sensor 16i described above (Fig. 1 is indicated by a dashed-dotted line frame). This position / speed control system is a normal position / speed control system, and a detailed description of its operation is omitted.

Further, the control device 20 has a force data processing unit 26 for extracting a pressing force F in a predetermined direction based on a signal from the force sensor 17, and data of the pressing force F from the force data processing unit 26. And a force control calculation unit 27 that performs a force control calculation based on the above. This force control calculation unit 27
This will be described in detail in the description of the operation of this embodiment.

Next, the operation of this embodiment having such a configuration will be described.

First, the trajectory calculator 21 determines the hand position effect and posture, that is, the hand point effect, as shown in FIG. 12, based on the teaching points of the hand effector 12 (the target position and posture at the start and end points of each operation). A target trajectory (vector rp in the following expressions (2-1) and (2-2)) is calculated as a predetermined target value related to the operation of the device 12. Note that FIG. 12 shows an example in which a line segment connecting two teaching points is set as the target trajectory, but the present invention is not limited to this, and for example, the target trajectory may be an arc passing through three teaching points, or a plurality of arcs may be formed. The target trajectory may be determined by spline interpolation or the like based on the position of the teaching point.

The target trajectory thus obtained is corrected by adding the correction amounts calculated by the method described below. That is, first, the force data obtained by the force sensor 17 is sent to the force control calculation unit 27 via the force data processing unit 26. Then, the force control calculation unit 27 calculates the correction amount (including the direction) with respect to the target trajectory by the force control calculation. Hereinafter, this force control calculation will be described in detail.

First, the force control calculation unit 27 first calculates the deviation Fd-F between the target pressing force Fd (or target moment) and the detected pressing force F and its absolute value | Fd-F |.

Next, the force control calculator 27 compares the deviation with a predetermined threshold value Ft, and the absolute value of the deviation |
The force control gain, that is, the feedback gain (either Kf1 or Kf2) is selected based on the magnitude relation between Fd-F | and the threshold value Ft. In this case, the feedback gain Kf2 (second) selected when the absolute value of the force deviation is large is set larger than the (first) feedback gain Kf1 selected when the absolute value of the force deviation is small. Has been done.

Then, the force control calculation unit 27 adds the deviation Fd-to the selected feedback gain (Kf1 or Kf2).
The correction amount (the second term on the right side of the following formula (2-1) or (2-2)) is calculated by multiplying F and performing integration with respect to time t.

Next, the force control calculation unit 27 causes the target trajectory calculated by the trajectory calculation unit 21 (the following equation (2-1) or (2-
The correction amount is added to the first term on the right side of 2) (see section A in FIG. 1) to calculate a corrected target trajectory (left side of the following equation (2-1) or (2-2)). The threshold value Ft,
Parameters such as feedback gains Kf1 and Kf2 are
An appropriate value may be set by simulation or experiment.

The above items can be expressed by the following mathematical expressions.

[0040]

[Equation 2] In the above equations (2-1) to (2-3), Fd is the magnitude of the target pressing force in the designated direction, F is the magnitude of the detected force in the designated direction, and Ft is preset. Threshold of force deviation (in this case a positive value), Kf
1, Kf2 are feedback gains (positive values in this case) for force control, vector Fa is a correction direction unit vector, and vector rp is a target trajectory at the time of position control calculated based on a teaching point.

In the above formula, the equal sign is added below the inequality sign in the formula (2-2), but the present invention is not limited to this, and the equal sign below the inequality sign in the formula (2-1). May be attached. Further, although the integration rule is used in the above formulas (2-1, 2-2), a proportional term and a double integral term may be added as necessary. The above formulas (2-1) and (2-2)
Is expressed as a continuous system to make the meaning of the equation easier to understand, but of course, since an ordinary robot is digitally controlled, in an actual control system, it becomes a discrete system equation according to sampling. There is.

Next, based on the data of the target trajectory corrected in this way, the inverse coordinate transformation unit 22 causes the target position of the tip of each arm element 11i (that is, each servo motor 14i).
The target rotation angle of is calculated. And each arm element 11
Position control calculation unit 2 so as to follow the target position of the tip of i
3. The speed control calculation unit 24 generates a command value, and the robot body 10 is driven based on this command value.

As described above, according to this embodiment, when the absolute value of the force deviation | Fd-F | is small, that is, it is determined that the hand effector 12 is operating following the work object 2. If possible, set the feedback gain to a small value (Kf1 in this case) so that the hand effector 12 does not excessively sensitively follow subtle unevenness such as a casting surface, but makes it follow slowly. You can On the other hand, when the absolute value of the force deviation | Fd-F | is large, that is, when the hand effector 12 is separated from the work object 2 or collides with a large protrusion, the feedback gain is set to a large value ( In this case, by setting Kf2), it is possible to cause the hand effector 12 to quickly approach the work object 2 or to operate to quickly release an excessive force.

In the above embodiment, the target trajectory (position) is set as the predetermined target value for the action of the hand effector.
However, the present invention is not limited to this. That is, the target speed may be given as a predetermined target value. In this case, the control equation is the following equation (3-
It can be represented as 1) to (3-3).

[0045]

[Equation 3]

Second Embodiment Next, a second embodiment will be described. The second embodiment is different from the first embodiment in that the feedback gain is set so as to be proportional to the deviation within a predetermined deviation range, and is otherwise substantially the same as the first embodiment. is there.
In the following description of the second embodiment, only the feedback gain setting method, which is a difference from the first embodiment, will be described, and detailed description of the other parts will be omitted.

In the second embodiment, the feedback gain is set as follows. That is, first, similarly to the first embodiment, the force control calculation unit 27.
Compares the absolute value of the force deviation | Fd-F | with a threshold Ft, and calculates the absolute value of the deviation | Fd-F | with a threshold Ft.
Set the feedback gain based on the magnitude relationship with.

In this embodiment, the feedback gain is set according to the following equation (4).

[0049]

[Equation 4] Here, Fd is the magnitude of the target pressing force in the designated direction, F is the magnitude of the detected force in the designated direction, and F is the magnitude.
t is a preset force deviation threshold value (in this case, a positive value), Kf1 and Kf2 are constants that determine the feedback gain for force control, vector Fa is the correction direction unit vector, and vector rp is , A target trajectory at the time of position control calculated based on the teaching point, and a threshold value Ft, a constant Kf
1 and Kf2 may be set to appropriate values through simulations and experiments.

As can be seen from the above equations (4-1, 4-2), when the absolute value of deviation | Fd-F | is smaller than the threshold Ft, the (first) feedback gain is {Kf1 + Kf2.
| Fd-F | / Ft}. That is, in this case, the feedback gain is the absolute value of deviation | Fd-F |
There is a relationship that increases in proportion to the increase of.

When the absolute value of the deviation | Fd-F | is larger than the threshold value Ft, the (second) feedback gain is set to a constant value {Kf1 + Kf2}.

Further, when the absolute value of the deviation | Fd-F | is equal to the threshold value Ft, the feedback gain is given by the equation (4-
When either 1) or the formula (4-2) is applied, {Kf1 + Kf2} is obtained. That is, the feedback gain is such that the absolute value of the deviation | Fd-F | does not cross the threshold value Ft and become discontinuous.

Note that the feedback gain when the absolute value of deviation | Fd-F | is smaller than the threshold value Ft is smaller than the feedback gain when the absolute value of deviation | Fd-F | is greater than or equal to the threshold value Ft. Is the same as that of the first embodiment.

On the other hand, unlike the first embodiment, the magnitude relationship between Kf1 and Kf2 is not limited, and Kf1 is replaced by Kf2.
May be set to a value equal to or Kf1 may be set to a value larger than Kf2.

In the above formulas (4-1, 4-2), an equal sign is added below the inequality sign in the formula (4-2), but the present invention is not limited to this, and the formula (4-1) is used. ) May be followed by an equal sign.

The target trajectory is corrected based on the feedback gain determined as described above, and the robot main body 10 is controlled based on the corrected target trajectory in the same manner as in the first embodiment.

According to the present embodiment, the following advantageous effects can be obtained in addition to the first embodiment. That is, in the present embodiment, when the detected pressing force F has a value near the target pressing force Fd, that is, when the hand effector 12 follows the work target 2 very well, the feedback gain is more Can be set small. For this reason, it is possible to make the tracking of subtle unevenness such as a cast surface less sensitive, and to obtain a more beautiful finished surface.

Further, since the change of the feedback gain with respect to the change of the detected pressing force F continuously changes across the threshold value Ft, the detected pressing force F crosses the threshold value Ft. If it fluctuates at, the feedback gain does not change abruptly. Therefore, the threshold F
The control in the vicinity of t can be performed more stably.

A graph showing the change of the feedback gain with respect to the change of the deviation Fd-F when the formulas (4-1), (4-2) and (4-3) are applied to determine the feedback gain. Is shown in FIG.

FIG. 4 is a graph showing changes in the feedback gain with respect to changes in the deviation Fd-F when the threshold Ft of the absolute value of deviation | Fd-F | is set equal to the target pressing force Fd. .

In the embodiment shown in FIG. 4, the threshold Ft of the absolute value of deviation | Fd-F | is set to the target pressing force F.
A case where the feedback gain is set to be a positive value Kf1 when the detected pressing force F and the target pressing force Fd are set equal to d is shown, but the present invention is not limited to this. .

That is, Kf1 = 0 may be set. The relationship between the deviation Fd-F and the feedback gain in this case is shown in FIG. With this configuration, the feedback gain approaches 0 when the detected pressing force F and the target pressing force Fd are extremely close to each other, that is, when the hand effector 12 is following the work object 2 well. More stable control can be performed.

In the above embodiment, the feedback gain when the absolute value of deviation | Fd-F | is smaller than the threshold value Ft is {Kf1 + Kf2 | Fd-F | / Ft}.
However, the proportional expression part Kf2 | Fd-F | / Ft of this feedback gain is defined as Kf2 | Fd-F |
The feedback gain may be {Kf1 + Kf2 | Fd-F |}. In this case, the feedback gain when the absolute value of deviation | Fd-F | is larger than the threshold value Ft is {Kf1 + Kf2 | Ft |}.

Further, in the above embodiment, the feedback gain when the absolute value of the deviation | Fd-F | is smaller than the threshold value Ft is set so as to linearly increase (see FIGS. 3 to 5). However, it is not limited to this. That is, it may be set so as to change in a quadratic curve as shown in FIG.

Further, the feedback gain may be changed in a trigonometric function as shown in FIG. In the case of changing in a trigonometrical manner in this way, as shown in FIG. 7, a curve showing the feedback gain when the absolute value of deviation | Fd-F | is smaller than the threshold value Ft, and the absolute value of deviation |
It is preferable to smoothly connect the curve (a straight line in the case of FIG. 7) showing the feedback gain when Fd-F | is larger than the threshold value Ft. That is, the absolute value of the deviation | Fd
-F | Absolute value of deviation when smaller than threshold Ft |
| Fd-F | = Ft when the feedback gain expressed as a function of Fd-F | is differentiated by the deviation Fd-F.
Of the feedback gain Fd when the absolute value of the deviation | Fd-F | is larger than the threshold Ft.
It is preferable to set the feedback gain so as to be equal to the differential coefficient at −F = Ft (= 0 in the case of FIG. 7). By doing so, the control in the vicinity of the threshold value Ft can be performed more stably.

Further, the function that determines the feedback gain when the absolute value of the deviation | Fd-F | is smaller than the threshold value Ft may be another function. That is, the function for determining the feedback gain can be arbitrarily changed as needed, as long as it satisfies the condition that it tends to increase as the absolute value of the deviation increases. Further, in the graphs shown in FIG. 2 to FIG. 7, the line indicating the relationship between the pressing force and the feedback gain is symmetrical with respect to the straight line F = Fd, but the line is not limited to this and is necessary. It is also possible to make them left-right asymmetrical according to.

In this embodiment as well, similar to the first embodiment, a target speed, a target torque or the like may be given as a target value related to the action of the hand effector.
In this case, the configuration of the control system shown in FIG. 1 is changed appropriately.
Further, the configurations of the robot and the control system shown in the first and second embodiments are merely examples for implementing the present invention, and can be appropriately applied to a force control robot having the same function. It is possible.

[0068]

The results of applying the control method according to the present invention to an actual force control robot will be specifically described below. In the following examples, the case where the hand effector and the work target are separated from each other is an initial state, and then the hand effector and the work target are brought into contact with each other, and chamfering work is performed by force control. It shows the result of verifying the effectiveness of with a real robot.

First, the test conditions will be described. As shown in FIG. 8, a 6-axis cylindrical coordinate type robot (robot body)
An air-driven cutter 12 is attached to the tip of the arm 11 of 10 as a hand effector, and a casting work 2 is used as a work target.
The chamfering work was performed. Then, the work result by the conventional force control method and the work result by the force control method according to the present invention were compared.

In this comparative test, as common conditions, the tool feed rate is 20 [mm / sec], the chamfering distance is 80 [mm], and the target pressing force Fd is 4.9 [N].
(0.5 kgf). The feed direction of the tool is a direction orthogonal to the paper surface of FIG.

FIG. 9 shows a result of the work when the force control is performed according to the conventional force control method, that is, the equation (1). The feedback gain Kf is 0.001 [m /
s]. FIG. 9A shows the change over time in the position of the tip of the chamfering tool with respect to the pressing direction. Also in FIG.
(B) is a diagram showing a change over time in the actual pressing force measured by the force sensor 17. The horizontal axis of FIG. 9A and FIG.
The horizontal axes in (b) correspond to each other.

As shown in FIG. 9A, first, the force control is started from the position where the cutter 12 is separated from the casting work 2 by about 1.5 [mm] (t = about 0.2 [sec]). Time point). In this case, since the casting work 2 and the tip of the cutter 12 are separated from each other, the reaction force received by the tip of the cutter 12 from the casting work 2, that is, the magnitude F of the detection force in the designated direction is substantially zero. Therefore, since Fd-F becomes positive, the tip of the cutter 12 is controlled so as to approach the casting work 2.

Next, after the tip of the cutter 12 comes into contact with the casting work 2 (at time t = about 0.9 [sec]), a predetermined pressing force of 4.9 [N] (0.5 kgf) is set by force control.
So that it follows the casting work 2.

As shown in FIG. 9A, the coordinates of the position of the tip of the cutter 12 gradually increase from the reference point, and 0.8 [mm] for a chamfering distance of 80 [mm].
It is understood that the surface to be processed of the cast work 2 is tilted with respect to the reference surface.

FIG. 10 also shows the result of the work by the conventional force control method based on the equation (1), but the force control gain is twice that in the case of FIG. 9 (Kf = 0.002 [m / s]). Comparing the initial inclinations of FIG. 9A and FIG. 10A that show the case where the tip of the cutter 12 comes into contact with the casting work 2, the inclination of FIG. It turns out that is large. This means that the approach speed of the tip of the cutter 12 to the casting work 2 is high. Comparing the case of FIG. 10 (a) and the case of FIG. 9 (a), the case of FIG. 10 (a) approaches the casting work 2 at a speed of about twice.

However, looking at the change over time in the position of the tip of the cutter 12 and the change over time in the actual pressing force after the cutter 12 and the casting work 2 have come into contact, FIG. It can be seen that the force control is unstable because it is very oscillatory. Therefore, if the force control gain is simply increased, the approach speed to the work becomes faster, but the force control becomes unstable.

On the other hand, the work result of the force control method according to the second embodiment of the present invention is shown in FIG. The parameters in equation (3) are Kf1 = 0.001 [m / s], K
f2 = 0.001 [m / s], target pressing force Fd = 4.9
It was set to [N]. In this case, the change in the feedback gain with respect to the deviation Fd-F has the relationship shown in FIG.

From FIGS. 11A and 11B, the stability of force control similar to that of FIG. 9 is obtained, and the approaching speed to the work is quick as in FIG. I can confirm.

That is, according to the present embodiment, when the deviation of the force is small, the force control gain is set to a small value, so that the cutter 12 follows the subtle unevenness such as the casting surface excessively sensitively. Instead, it can be made to follow bluntly.
On the other hand, when the cutter 12 is separated from the casting work 2, the force deviation becomes large and the force control gain is set to a large value. Therefore, the cutter 12 can be operated so as to quickly approach the work. I know that I can do it.

[0080]

As described above, according to the present invention,
It is possible to obtain a force control robot which can obtain a beautiful finished surface for a work object having a large shape error and can effectively prevent damage to the hand effector and the robot body.

[Brief description of drawings]

FIG. 1 is a diagram showing a first embodiment of the present invention,
The figure which shows the structure of the control system of a force control robot.

FIG. 2 is a diagram showing a method of setting a feedback gain according to the first embodiment.

FIG. 3 is a diagram showing a method of setting a feedback gain according to the second embodiment of the present invention.

FIG. 4 is a diagram showing a modified example of a feedback gain setting method according to the second embodiment.

FIG. 5 is a diagram showing a modified example of a feedback gain setting method according to the second embodiment.

FIG. 6 is a diagram showing a modified example of a feedback gain setting method according to the second embodiment.

FIG. 7 is a diagram showing a modified example of a feedback gain setting method according to the second embodiment.

FIG. 8 is a diagram showing an example of a configuration of a force control robot according to the present invention.

FIG. 9 is a diagram showing changes over time in the pressing direction displacement and the pressing force when chamfering work is performed by applying a conventional control method.

FIG. 10 is a diagram showing changes over time in displacement in the pressing direction and pressing force when chamfering work is performed by applying a conventional control method.

FIG. 11 is a diagram showing changes over time in the pressing direction displacement and the pressing force when chamfering work is performed by applying the control method of the present invention.

FIG. 12 is a diagram showing a coordinate system in which force control is performed.

[Explanation of symbols] 1 Force control robot 2 Work object (cast work) 10 Robot body 11 arms 12 Hand effector (Chamfering cutter) 17 Pressing force detection means (force sensor) 20 Control device 27 Force control calculator

─────────────────────────────────────────────────── ─── Continuation of the front page (56) References JP-A-64-73402 (JP, A) JP-A-1-281884 (JP, A) JP-A-4-13588 (JP, A) (58) Field (Int.Cl. ⁷ , DB name) B23Q 15/00 B25J 9/10

Claims (6)

(57) [Claims] 1. A hand effector for performing work on a work object, and at least one control shaft for controlling the position, speed, force, etc. of the hand effector, the predetermined effect relating to the action of the hand effector. In the control method of the robot for controlling the control axis based on the target value of, the step of detecting the pressing force of the hand effector against the work object, the detected pressing force and a predetermined target pressing force The step of calculating the deviation, the step of calculating the feedback gain for correcting the deviation based on the deviation, the step of calculating the correction amount based on the deviation and the feedback gain, and the correction amount Adding to a predetermined target value to correct the predetermined target value, the feedback gain, when the absolute value of the deviation is smaller than a predetermined threshold value. And a second feedback gain that is applied when the absolute value of the deviation is larger than a predetermined threshold value. The magnitude of the second feedback gain is equal to that of the first feedback gain. And the magnitude of the first feedback gain is greater than the magnitude.
It is set to increase continuously as the absolute value of the difference increases.
And the magnitude of the second feedback gain is constant.
And the absolute value of the deviation is equal to the threshold value
The magnitude of the first feedback gain of the
A control method for a robot , which is equal to the feedback gain . 2. The magnitude of the first feedback gain is proportional to the absolute value of the deviation.
The control method of the robot according to claim 1 . 3. The work performed by the hand effector on the work target is a grinder work, a polishing work, a chamfering work,
Characterized by finishing work such as deburring work
The control method of the robot according to claim 1 . 4. A hand effector for performing work on a work object, and at least one control axis for controlling the position, speed, force, etc. of the hand effector and acting based on a predetermined command value. A pressing force detecting means for detecting the pressing force of the hand effector against the work object, and a feedback gain for correcting the deviation based on the deviation between the detected pressing force and a predetermined target pressing force. Calculated, a correction amount is calculated based on the deviation and the feedback gain, the correction amount is added to a predetermined target value regarding the action of the hand effector to calculate a correction target value, and the correction target value is calculated based on the correction target value. And a controller for generating the command value, the feedback gain defined by the controller is
Applicable when the absolute value of deviation is smaller than the specified threshold
First feedback gain and absolute value of the deviation
The second fis to be applied if is greater than a predetermined threshold
Feedback gain, and the second feedback
The magnitude of the gain is the magnitude of the first feedback gain.
Is larger than the first feedback gain.
The magnitude of is continuously increased as the absolute value of the deviation increases.
Is set to be large, and the second feedback
The size of the in is constant, and the absolute value of the deviation is
Of the first feedback gain when the threshold value is equal to
The magnitude is equal to the magnitude of the second feedback gain.
There, the force control robot, characterized in that. 5. Grinder work, polishing work, chamfering work,
In a force control robot for finishing work such as deburring work, a hand effector that performs work on a work object and the position, speed, force, etc. of the hand effector are controlled and a predetermined command value is set. At least one acting on the basis of
The control axis, the pressing force detecting means for detecting the pressing force of the hand effector against the work object, and the deviation is corrected based on the deviation between the detected pressing force and a predetermined target pressing force. A feedback gain for calculating a correction amount based on the deviation and the feedback gain, and adding the correction amount to a predetermined target value relating to the action of the hand effector to calculate a correction target value. A control device that generates the command value based on the corrected target value,
And the feedback gain defined by the controller is
Applicable when the absolute value of deviation is smaller than the specified threshold
First feedback gain and absolute value of the deviation
The second fis to be applied if is greater than a predetermined threshold
Feedback gain, and the second feedback
The magnitude of the gain is the magnitude of the first feedback gain.
Is larger than the first feedback gain.
The magnitude of is continuously increased as the absolute value of the deviation increases.
Is set to be large, and the second feedback
The size of the in is constant, and the absolute value of the deviation is
Of the first feedback gain when the threshold value is equal to
The magnitude is equal to the magnitude of the second feedback gain.
There, the force control robot, characterized in that. 6. A control for controlling a robot body comprising a hand effector for performing work on a work object and at least one control axis for controlling the position, speed, force, etc. of the hand effector. In the device, a target value calculation unit that calculates a predetermined target value relating to the action of the hand effector, and feedback for correcting the deviation based on the deviation between the detected pressing force and a predetermined target pressing force. calculating a gain, it said calculating a correction amount based on the feedback gain and the deviation, and a force control computing section for calculating a corrected target value by adding the correction amount to the predetermined target value, the force The feedback gain defined by the control calculation unit
Is the absolute value of the deviation is smaller than a predetermined threshold
Of the first feedback gain applied to
The first applied if the absolute value is greater than a given threshold
2 feedback gains, and the second feed
The magnitude of the back gain is the first feedback gain.
Larger than the size of the first feedback
The magnitude of the gain increases as the absolute value of the deviation increases.
The second feed bar, which is set to continuously grow.
The magnitude of the gain is constant, and the absolute value of the deviation is
The first feedback gain when equal to the threshold
The magnitude of IN is the magnitude of the second feedback gain.
A control device characterized by being equal to .