JP3217351B2 - Force control device and robot using the same - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Object of the Invention] (Industrial application field) The present invention controls a robot by detecting a pressing force of a hand effector attached to a hand portion against a workpiece. The present invention relates to a force control device and a force control robot.

(Prior Art) A force control robot that attaches a hand effector such as a tool to a hand of a robot and presses the work with a predetermined force to work the work, and a force control device that controls the force control robot have been proposed. ing.

As a force control robot, for example, in a grinder working robot, etc., a 6-axis force torque sensor is provided between a tool and a hand portion of the robot in order to press a grinder, which is a hand effector, into a work in a given direction with a predetermined pressing force. It is fixed.

The force control device controls the forces in the respective axial directions and the moments about the respective axes detected by the six-axis force torque sensor or the combined forces thereof to have predetermined values.

In other words, force control, compliance control, hybrid control, etc. are performed by directly detecting the force or moment in the direction to be detected as it is and incorporating the detected force or moment into a control loop for controlling the force control robot. .

However, in the above-described robot, since the force torque sensor has a relatively heavy tool attached thereto, when acceleration is applied to the tool, the inertial force generated by the acceleration is naturally detected by the force torque sensor. Will be.

Further, the force detected by the force torque sensor is a composite force of the pressing force and the inertial force, and cannot be detected separately from the pressing force and the inertial force.

Therefore, with the above-described configuration of the force control device and the force control robot, it has been difficult to accurately measure only the pressing force of the hand effector against the work.

Also, when performing force control, etc., with the conventional force detection method, the inertial force of the tool due to the drive of the robot arm and the inertial force of the tool due to the vibration of the robot arm are not distinguished from the pressing force even in the case of contact with the workpiece. Feedback to the control system. This inertial force cannot be ignored because it has the same level or greater than the pressing force to be controlled. If the work is performed ignoring this inertial force, the desired work on the work cannot be performed.

In addition, even if the vibration of the tool is very small, the inertial force is fed back to the control system, so that the vibration becomes larger and causes oscillation.

Therefore, the gain of the force loop cannot be increased, and control with good responsiveness cannot be performed, so that high-precision machining cannot be performed.

As a method for solving such a problem, a method has been proposed in which the inertial force is calculated in advance by the operation of the robot and the amount thereof is removed, but it is necessary to calculate in advance for each operation, and It cannot deal with unpredictable inertial force such as arm vibration at all. Other methods using a filtering method have been proposed, but this method is not sufficiently effective and requires many calculations.

In addition, the self-weight compensation for the six-axis force torque sensor of the tool must always be calculated according to the posture of the tool. Therefore, there were problems such as an increase in the load on the computer. Another problem is that if the shape of the work to be processed is known, the tool is pressed in the normal direction of the work and the hand effect Change the posture of the vessel and always keep the hand effector
Work can be performed, but when the shape of the work is not known in advance, the robot cannot determine how to change the posture by itself, so it can be used to process a work with an unknown shape. I couldn't do it.

Even if the shape of the work is known in advance, the work of teaching the shape to the robot and inputting data corresponding to the shape to the control device is only required if the shape of the work and the finished shape of the work become complicated. Requires enormous effort.

On the other hand, as a research stage, a robot that grinds a workpiece with an unknown shape by incorporating a specially shaped whetstone or a special special force sensor into a tool has been proposed, but a general grinder is lacking in versatility. It cannot be applied to work. In addition, the cost is relatively high because a specially shaped grindstone and a special special force sensor are incorporated in the tool.

(Problems to be Solved by the Invention) As described above, in the conventional force control robot, since the inertial force of the tool as the hand effector is detected, the accurate pressing force between the tool and the work can be detected. However, since the inertia force or the like is detected, the gain of the force loop cannot be increased, the response of the force control is poor, and it is difficult to perform machining with high accuracy. In addition, there was another problem that the self-weight compensation for the six-axis torque sensor of the tool had to be calculated.

Furthermore, in the conventional force control robot and force control device,
It cannot be applied to workpieces with unknown shapes. For this reason, there has been a problem that enormous labor is required to teach and input the shape of the workpiece to the robot.

Further, as a research stage, a robot for grinding a workpiece having an unknown shape has been proposed, but it is poor in versatility and cannot be applied to general grinder work. There is also a problem that the cost is high.

Therefore, the present invention can easily eliminate the effects of gravity and inertial force, accurately detect the pressing force of the hand effector against the work, improve the response of force control and the like, and improve the processing accuracy. It is an object of the present invention to provide a force control device and a force control robot capable of reducing the load on a computer.

[Structure of the Invention] (Means for Solving the Problems) In the invention according to claim 1 for achieving the above object,
A force control device for detecting a pressing force of a hand effector attached to an end of an arm of a robot against a workpiece, and controlling the robot so that the pressing force becomes a target pressing force. Detecting means disposed between an end portion and the hand effector, for detecting a force and a moment based on a reaction force of the pressing force applied to the hand effector from the workpiece, From the force and the moment, coordinate transformation is performed to determine a moment around the center of gravity of the hand effector, and the pressing force is calculated from the moment, and the pressing force calculated by the calculating unit is the target force. And a control means for controlling the robot so as to match the pressing force.

In the invention of claim 2, the pressing force of the hand effector attached to the end of the arm against the workpiece is detected,
A robot controlled so that the pressing force becomes a target pressing force, wherein the pressing force is disposed between the arm end and the hand effector and is applied to the hand effector from the workpiece. Detecting means for detecting a force and a moment based on the reaction force of the above, and a coordinate transformation is performed from the force and the moment detected by the detecting means to obtain a moment around the center of gravity of the hand effector, and from this moment, the pressing is performed. A first calculating means for calculating a force; and a control means for controlling the hand effector such that the pressing force calculated by the first calculating means matches the target pressing force. To provide a robot.

(Operation) According to the first and second aspects of the present invention, the detection position of the detection means is apparently moved to the center of gravity of the hand effector by the calculation means, whereby the moment around the center of gravity of the hand effector is obtained and detected. From the moment about the center of gravity of the hand effector, an accurate pressing force of the hand effector on the workpiece is detected.

In many cases, the hand effector presses the workpiece in a fixed posture and in a fixed direction, and the portion in contact with the workpiece is often almost fixed.

Therefore, the pressing force of the hand effector against the workpiece is reduced to F
Then, the pressing force can be calculated by F = MG / r (1) using the moment MG around the center of gravity of the hand effector detected by the detecting means and the vertical distance r from the center of gravity.

In addition, even if inertial force such as vibration acts on the hand effector to control the pressing force, or if acceleration α is generated in the pressing direction of the intended hand effector, the inertial force mα (m The weight of the hand effector) is not detected at all in the moment MG around the center of gravity.

Therefore, since the accurate pressing force can be detected without being affected by the inertial force or the like, the hand effector can be pressed against the workpiece based on the detected pressing force.

Therefore, it is possible to provide a highly accurate force control device having high responsiveness such as force control and a robot using the same.

In addition, since the moment MG around the center of gravity of the hand effector is detected, even if the posture of the robot changes, the influence of the self-weight of the hand effector does not occur, so that the calculation of the self-weight compensation is not required.

Furthermore, an accurate pressing force can be detected without being restricted by hardware such as a support mechanism or a sensor for each tool or a limitation on a method of supporting the tool.

Next, an embodiment of a force control device and a force control robot according to the present invention will be described.

First Embodiment FIG. 2 shows a force control device according to a first embodiment and a force control robot 1 controlled by the force control device. The force control robot 1 is a robot having a six-axis cylindrical coordinate system, and has six axes in Z, R, θ, α, β, and γ directions. A grinder 5 is attached to a hand portion 3 of the force control robot 1 as a hand effector. The grinder 5 grinds a workpiece 7 (see FIG. 1) as a workpiece.

A six-axis force sensor 11 is arranged between the end of the arm 9 of the force control robot 1 and the grinder 5, and a high-frequency vibration of the grinder 5 is provided between the six-axis force sensor 11 and the grinder 5. A rubber damper 13 for blocking is provided.

As shown in FIG. 1, the grinder 5 of the hand 3
Rotates the disk-shaped grindstone 15 with the motor 17 to grind the surface of the work 7. In this case grinding wheel 15 is inclined relative to the surface of the workpiece 7 at a predetermined angle phi _CG, moves on the surface of the workpiece 7.

In FIG. 1, the coordinate system Σc along the surface of the work 7 (the normal direction Z _{c of} the work 7, the grinder feed direction
X _c , grinder lateral direction Y _c ), sensor coordinate system {s, grinder coordinate system obtained by moving s parallel to the center of gravity of the grinder}
Set G. In the first view, O _G indicates the center of gravity of the grinder 5, the distance from the contact portion between the grinding wheel 15 and the surface of the workpiece 7 to the center of gravity O _G and r.

If you grinder working surface of the workpiece 7 is generally presses the grinder 5. normal Z _C direction relative to the workpiece 7. The grinder 5 is performing work while maintaining a constant contact angle phi _CG relative to the workpiece 7.

Therefore, the moment of the center of gravity O _G around the detection by the reaction force F _WZ and the sensor 11 pressing the grinder 5 (in this case Y _G)
M _GY and the relationship between the vertical distance r from the center of gravity O _G of the grinder 5 to F _WZ becomes _{... M GY = F WZ × r} (2). Can be measured reaction force F _WZ pressing force to the workpiece 7 of the grinder 5 by detecting the center of gravity O _G moment around the M _GY the equation (2). FIG. 3 is a symbolic diagram of the force control robot 1, in which an absolute coordinate system ΣO fixed to the base fixing portion is set.

Next, the force control device 19 for controlling the force control robot 1 will be described with reference to FIG. FIG. 4 shows the force control device 19
FIG. 3 is a block diagram showing the configuration of FIG.

The force control device 19 receives a target pressing force from the computer 21 to the work 7 of the grinder 5 and outputs the target pressing force to a driving coordinate converter, which will be described later. A target position output unit that receives a target position from the operation unit 25 and outputs the target position
27, and a target posture position output unit 31 that outputs a target posture position from the operation unit 29 and outputs the target posture position.

Further, the force control device 19 receives a target pressing output signal from the target pressing force output unit 23, a target position signal from the target position output unit 27, and a target posture position signal from the target posture position output unit 31. Are output and the target angle θ _id is output to the servo drive and motor.
33 are provided.

Also, 6 6 axial force signal detected by the force sensor 11 obtains a center of gravity moment around the M _G of the grinder 5, the coordinate converter 35 is provided to be output further predetermined pressing force from the moment around the center of gravity Have been.

And offset of each axis direction between the center of gravity O _G of the origin O _S and grinder 5 of the six-axis force sensor 11 for detecting the center of gravity moment around the grinder 5, and the axial force that can be detected by a sensor, angular The origin of the sensor can be apparently moved to the center of gravity of the grinder by performing coordinate conversion by the coordinate converter 35 using the moment about the axis.
In this case, the moment M _GY about the center of gravity O _G is M _SY , F _SZ , F
_SX, Z _GS, the _{X GS, M GY = M SY} + F SZ · X GS -F SX · Z GS ... (3) and expressed. Note that M _SY , F _SZ and F _SX are Y in the sensor coordinate system.
Moment about the axis, Z, X direction force, Z _GS, X _GS indicates the distance in each axis direction of the center of gravity O _G sensor origin and grinder 5. The sign also changes depending on the coordinate system. Further, the moment of the other axis are _{similarly, M GX = M SX + F} SY · Z GS -F SZ · Y GS ... (4) M GZ = M SZ + F SX · Y GS -F SY · X GS ... (5) can be obtained.

In a case where the work 7 is pressed with a constant pressing force in the normal direction and the surface of the work 7 is uniformly ground, the grinder 5 is used.
May be pressed with a constant force in the normal direction of the work 7 and moved in a direction along the surface.

A work coordinate system Σc along the surface of the work 7 is set,
Normal direction of the pressing force F _WZ work 7 detected by torque sensor, so that the pressing force F _WZd target output from the target pressing force output section 23 Z _C direction to a target position Z
Give _Cd . That is, Z _{Cd (n)} = K _f (F _WZd −F _Wz ) + Z _{Cd (n−1)} (6) Note that Z _{Cd (n−1)} is the target position one sampling before. Of course, where the, F _WZ is detected from the moment acting on the center of gravity O _G around the grinder 5.

The directions X _C and Y _C along the workpiece 7 are calculated by using a joystick or calculation, and the posture α _{C of the} grinder 5,
Regarding β _C and γ _C , target positions are given so as to keep a constant angle on the surface of the work 7.

Accordingly, the detected pressing force F _wz is _equal to the target pressing force F
_{If WZd} is not satisfied, the target position is further given to the work 7 and pressed toward the work 7 until the target pressing force is _achieved .

During this time, position control is performed in other directions, and the grinder 5 is translated in the pressing direction.

When the target positions X _Cd and Y _Cd along the surface of the work 7 are fixed, the pressing can be performed while maintaining the positions. By giving the target positions X _Cd and Y _Cd with a computer or a joystick as appropriate, the grinder 5 can be advanced at an arbitrary feed speed on the surface of the work 7.

Also, the target values α _Cd , β _Cd , γ _Cd of the attitude of the grinder 5
For, the target position is given to the work so as to keep it constant. For a flat plate, α _Cd , β _Cd , and γ _Cd are constant, and for a curved surface, α _Cd ,
Change β _Cd and γ _Cd .

For example, at the time of manual grinder work, the coordinate axis of the joystick corresponds to the work coordinate system along the work surface, so that even with various inclinations, only two-dimensional operation of the joystick can keep the work constant. Work can be performed while pressing with the force of.

In the automatic grinding, the trajectories X _C and Y _C on the surface of the work 7 are used.
By preparing the above, an arbitrary region can be automatically ground.

Next, a control method for controlling the force control robot 1 using the force control device 19 will be described below.

The target pressing force F _wzd is output from the computer 21 to the target pressing force output unit 23. The target pressing force F _WZd output from the target pressing force output unit is converted by the adder into the coordinate converter 35.
_Is compared with the reaction force F _{Wz of the} pressing force output from. At this time, if there is a difference between the reaction force F _Wz and the target pressing force F _WZd , the target position Z _Cd is changed so as to eliminate this difference.

In this case, the coordinate converter 35 within, output or from the six-axis force sensor 11, the detected gravity center O _G moments about M _G
Is entered. To detect this center of gravity O _G around the moment, the offset of each axis direction between the origin O _S and the center of gravity O _G of the grinder 5 of the six-axis force sensor 11, each axis detected by the six-axis force sensor 11 Using the directional force and the moment about each axis, coordinate transformation is performed by the above-described equations (3) to (5). This makes it possible to move the origin of the force sensor 11 to the center of gravity O _G of apparent on grinder 5.

As described above, the pressing force is output to the driving coordinate converter 33 so that the grinder 5 is pressed and processed with the target pressing force on the work 7. Based on the target pressing force, the servo drive operates a motor of a joint drive mechanism (not shown) to press the grinder 5 against the work 7.

Therefore, the pressing force of the grinder 5 against the work 7 can be pressed with the target pressing force _FWZd , and the responsiveness of force control and the like can be improved. Further, the processing accuracy of the work 7 by the grinder 5 can be improved.

The experimental results of this embodiment will be described with reference to FIGS. 8 and 9. The influence of the detection of the inertial force by the difference of the force detection method was investigated.

Using the force control robot shown in FIG. 1, the reaction force F in the pressing direction detected by the six-axis force sensor 11 when a step input is simply given to the speed input of the vertical axis in a non-contact state.
It is a figure showing _WZ .

For this reason, since there is no contact, no pressing force acts on the grinder 5, but an inertial force of the grinder 5 is generated by the vertical speed step input. FIG. 8 shows a case where the force acting in the vertical axis direction is detected as it is. Naturally, in this case, since the force in the vertical axis direction is detected as it is, the inertial force of the grinder 5 is detected by the speed step input.

Actually, in the case of the grinder work, while trying to control the pressing force of about 1 to 2 kg, a much larger inertia force is detected.

Figure 9 is a case of detecting the pressing force from the moment acting around the origin O _s of the sensor. As this case is shown in FIG. 9, since there in the center of gravity O _G out of the origin O _S and grinder sensor, but detects slightly rotational inertia force caused by this deviation, as compared with FIG. 8 inertia The influence of force is small.

Further, according to the present embodiment, as shown in FIG.
So that to match the origin O _S and the center of gravity O _G of the grinder 5 of the sensor, not under the influence of the inertial force.

FIG. 10, the center of gravity O _G moments about the grinder 5 without using the counterweight 37 is a result of detection by the equation (3). Even in this case, the effect of the inertial force is not exerted, but the weight of the grinder part is significantly reduced because the counter weight is not attached.

FIGS. 12 and 13 show the method of detecting the pressing force F _WZd by the detection method of FIGS. 8 and 10, and translating the center of gravity so that the pressing force F _WZ becomes the target pressing force F _WZd. This is the result of performing the grinder work by performing the force control. Touch from the non-contact state as if pressing down with 1 kg
FIG. 6 is a diagram comparing pressing forces until the pressure becomes. Although the force gain is increased to such an extent that they do not oscillate, it is about 10 times higher in Fig. 13. Obviously, FIG. 13 shows that the response is better. With the response as shown in FIG. 12, if the ground surface is slightly inclined, it cannot follow the surface sufficiently. Conversely, if the response is as shown in FIG. 13, even if the ground surface is slightly inclined, it can follow sufficiently.

Second Embodiment Next, a description will be given with reference to FIGS. 5 (a) and 5 (b). The second embodiment is a mounting counterweight 37 to grinder 5 of the hand portion 3, and is matched with the Z _S axis and the Z _G axis, an example of detecting the center of gravity O _G moment about M _GZ grinder 5 directly. Alternatively, without mounting a counterweight, the center of gravity O _G directly grinder 5 may be fixed to coincide with the Z _S axis.

That is, the center of gravity of the grinder 5 is as shown in Figure 1, with respect to Z _S axis direction, it is located grindstone 15 side. By mounting the counterweight 37 on the buttocks side of the motor 17 from this for Z _S axis, the center of gravity of the grinder 5 is moved to the buttocks side of the motor 17 to set the weight of the counterweight 37 in a suitable weight it is thus possible to coaxial and Z _S axis and the Z _G axis.

According to the present embodiment, since _FWZ is considered as the direction of the reaction force of the pressing force of the grinder 5 against the work 7, the moment _MGG around the center of gravity of the grinder in this direction may be detected. For this reason, only one axis is matched.

In other cases, when moments occur around a plurality of axes, not only one axis but also the origin may need to be completely matched.

Thus, the axes of the grinder 5 coordinate system of the sensor coordinate system, If it is possible to match by adjusting the counterweight 37 and its mounting position, it is possible to detect the center of gravity moment around the M _G direct Grinder 5 I can do it.

In this case, in the case of a general six-axis force sensor, the method of attaching the grinder 5 is limited, and the weight increases due to the counterweight 37, but the coordinate conversion calculation such as the parallel movement of the sensor origin becomes unnecessary. .

As described above, since the measured moment M _G about the center of gravity of the grinder 5 in the present embodiment, in order to any pressing force, even when driving the pressing direction Te by the force control, grinder 5 Also, the inertia force due to the feed and the inertia force due to the vibration of the arm are not detected.

The vibration of the arm vibrates in the translation direction with respect to the grinder 5 and hardly vibrates in the rotation direction with respect to the grinder 5. Therefore, the vibration of the arm, it is hardly a moment M _G about the center of gravity of the grinder 5 is vibrated.

Accordingly, the pressing force F _WZ of the grinder 5 can be accurately detected, it is possible to improve the control response.

Next, FIG. 11 shows the experimental results of this embodiment.
Figure 11 shows a case where is balanced by the counterweight 37 to the origin O _S around the sensor, it is confirmed that almost no effects of the inertial force.

Third Embodiment Next, a third embodiment will be described with reference to FIGS. 6 (a) to 6 (c). The third embodiment is an example of cutting a flat plate, while the first and second embodiments are examples of a grinding operation (grinder operation).

As shown in FIG. 6B, the grindstone 15 of the grinder 5 of the hand portion 3 cuts the flat plate 39. At the time of this cutting, the force F is applied as a reaction force to the grindstone 15 of the grinder 5 when the grinder 5 is cut, although it depends on the cutting depth of the grinder 5 and the feeding direction. This force F
Are the component force F _{X in} the thickness direction of the flat plate 39 and the component force in the surface direction of the flat plate 39
It can be _divided into F _GY .

Since the vertical distance r of the grindstone 15 from the gravity O _{G of the} grinder 5 is always constant, the X _{G of} the grinder 5,
The pressing force can be obtained from the moment about Y _G.

The reaction force F is Can be calculated by Also in this case, the control response is the same as that in the case of grinding because the inertial force does not affect the control response. When it is necessary to consider the torque of the motor 17, the value may be measured in advance and the division may be performed.

As an application example, in each of the above embodiments, a cylindrical coordinate type robot is described. However, the present invention is not limited to this robot, and can be applied to a rectangular coordinate type, a polar coordinate type, an articulated coordinate type, and a robot.

In each of the above embodiments, a six-axis force sensor is used as the pressing force detecting sensor. However, the present invention is not limited to this, and a force sensor that detects only one direction may be used as long as a force in a required direction can be detected.

Thus, downsizing and cost reduction can be achieved. In the case of a six-axis force sensor, measures such as an emergency stop can be performed when an excessive force is applied in a direction other than the pressing direction.

Fourth Embodiment Next, a fourth embodiment will be described with reference to FIGS. 7 (a) and 7 (b). The present embodiment is an example in which a force acting on a gripper or the like is detected by applying the present invention.

The forces F _GX , F _GY , and F _GZ acting on the gripper are distances from the point of action P of the gripper to the sensor origin (X _GP , Y _GP ,
Z _GP ) and the moment M _GX acting around the center of gravity of the gripper,
If M _GX and M _GY and a force component in one direction, for example, F _GZ , are known, it can be detected by solving the following equations (8) to (10).

Axial components F _GX , F _GY , F of the force acting on the gripper
_GY and gripper around the center of gravity of the moment M _GX, M _GZ, M _GY
And the vertical distance in each axis direction to the center of gravity of the force acting on the gripper
X _GP, Y _GP, relation of Z _GP is _{M GX = F GZ · Y GS} -F GY · Z GS ... (8) M GY = F GX · Z GS -F GZ · X GS ... (9) M GZ = F _GY · X _GS −F _GX · Z _GS (10) _Therefore , from these equations, the force F _GX acting in the translational direction can be _calculated as
F _GY, and _{F GZ, M GX, M GZ} , M GY, X GP, Y GP, can be measured by means of a Z _GP. However, omnidirectional forces F _GX , F _GY , F
_GY and _{F GX, M GZ, M GY} , X GP, Y GP, but can not be determined only from Z _GP, or when there is something conditions, for 1 axially directly, using the axial force If so, you can ask.

In this case, since the force acting in the axial direction is detected in one direction (for example, F _GZ ), the influence of the inertial force cannot be eliminated for the force in that direction.

However, in the case of a robot arm, the direction in which the arm easily oscillates is often determined, so in the direction in which the arm does not easily oscillate, the axial force is directly detected, and in the other direction, the force is detected. By detecting from the moment acting around the center of gravity of the gripper, it is possible to detect the force acting on the gripper in a form in which the influence of the inertial force is removed as much as possible.

In addition, the direction for direct detection is the direction with the least influence for each posture depending on the posture of the arm, and the direction for direct detection is the direction with the least influence for each posture depending on the posture of the arm. As a result, detection with a small inertial force can be performed. Naturally, not only should the sensor origin be translated to the center of gravity of the gripper,
It is better to appropriately rotate the coordinate axes as needed.

Fifth Embodiment Next, a fifth embodiment will be described with reference to FIG. This embodiment is another embodiment of the force control device 41 for controlling when the force control robot 1 shown in FIGS. 1 to 3 processes a workpiece 7 having an unknown shape.

In FIG. 1, the grinder coordinate system ΣG is represented by Y _G
Set the coordinate system Σw rotated by φ _GW degree around the axis. The sensor coordinate system ΣS is the sensor, ΣG, Σw is the grinder 5
Are coordinate systems fixed respectively. The O _G indicates the center of gravity of the grinder 5, the distance from the contact portion between the grinding wheel 15 and the surface of the workpiece 7 to the center of gravity O _G and r. Φ _CG is the pitch angle of the grinder 5 with respect to the work 7. Also, the third
In the figure, an absolute coordinate system ΣO fixed to a base fixing portion
Set.

Further, the pressing direction of the Z _W to the work 7 grinder 5, longitudinal direction of travel of the X _W grinder 5, the Y _W and lateral feeding direction of the grinder 5. That is, each direction is determined by the attitude of the grinder 5.

Therefore, if Z _W and Z _C are parallel, grinder 5
Can press the grindstone 15 in the normal direction of the work 7. Furthermore, XW, by moving the grinder 5 in the Y _W direction, can be moved in the tangential direction of the work 7. φ _GW is the target pitch angle of the grinder 5 with respect to the work 7.

Then, at the time of grinding work, by detecting the change in the detection value of the six-axis force sensor 11, so as to always Z _W and Z _W are parallel,
If the attitude of the grinder 5 can be changed, it is possible to perform a grinding operation with an arbitrary pressing force while always maintaining a predetermined attitude with respect to the workpiece 7 with respect to an unknown flat or curved workpiece.

As shown in FIG. 14, the force control device 41 sends the target moment of the grinder 5 from the host computer 43.
M _GXd , target pressing force F _WZd , target yaw angle φ _Gd , target correction amounts ΔX _Wd , ΔY _Wd (ΔX _Wd , ΔY _Wd φ _Gd are input from an operation unit such as a joystick in a manual operation).

Then, the force sensor coordinate converter 45 the detection value of the six-axis force sensor 11, a coordinate conversion of the center of gravity O _G of the grinder 5.
Next, F _Wz or M _GX
From the target roll angle θ _Gd . Next, a grinder target pitch angle φ _Gd is calculated from F _Wz . The integral term is not shown. From the target roll angle θ _Gd , target pitch angle φ _Gd, and preset target yaw angle φ _Gd , the grinder attitude coordinate converter 47 uses, for example, an Euler in an absolute coordinate system fixed to the base of the force control robot 1. The target postures α _Od , β _Od , and γ _Od that are displayed as corners are calculated.

Specifically, it is not possible to simultaneously create a target roll angle, a pitch angle, and the like in a single coordinate transformation, so for example, first create a target posture that only considers correction of the roll direction, and then prepare a target posture in the pitch direction. By creating the target posture in consideration of the correction, the target posture in consideration of the correction in the roll direction and the pitch direction is created. (Consider the correction in the yaw direction if necessary.) Then, calculate the target correction amount ΔZ _Wd in the pressing direction from F _WZ , and set the target correction amount ΔZ _Wd and the target correction amounts in the traveling direction and the lateral direction that are set in advance. From the ΔX _Wd and ΔY _Wd , using the grinder position coordinate converter 49 determined by the target postures α _Od , β _Od and γ _Od , the target position X expressed in the absolute coordinate system
Calculate _Od , Y _Od and Z _Od .

Further, the target position and orientation X _Od , Y _Od , Z _Od , α _Od , β _Od , and γ _Od expressed in the absolute coordinate system calculated as described above are each converted into 1 to 6 axes by the driving coordinate converter 51. Calculate the target angle of the joint axis. Then, each axis constituted by the position and speed loops is driven.

Next, a control method for controlling the force control robot 1 with the force control device 41 will be described.

First, in order to control the direction Z _W pressing determines the moment of each axis by the formula of the first embodiment (2) to (5). Then, the control may be performed so that the reaction force F _wz of the pressing force to the work calculated by the equation (2) becomes the target pressing force F _WZd .

The target position of the grinder 5 in the pressing direction is corrected so that the reaction force F _Wz becomes the target pressing force F _WZd . The correction amount ΔZ _Wd of the pressing direction is given by the following equation.

ΔZ _Wd = K _fz (F _WZd −F _WZ ) (11) where K _fZ is a force gain. The target position in the _ZW direction is corrected by equation (11) for each sample. For example, when the detected force is smaller than the target force, the target position of the work 7 is corrected in the pressing direction.

Then, to control the longitudinal traveling direction X _W and lateral feed direction Y _W of the grinder 5 is a target amount of movement ΔX of each sampling before and after the traveling direction and the lateral feeding direction Y _W of the grinder 5
_Xd , _YWd , for example, by previously preparing a method of moving the grinder 5 on the work 7 by a computer, an arbitrary area of the work can be automatically ground.

Or, by giving from a joystick,
Manual grinder work is possible.

The above-described correction amounts, movement amounts, ΔX _wd , ΔY _Wd , and ΔZ _Wd in the pressing direction, the advancing direction, and the traverse direction are coordinate-transformed using the following target postures of the grinder 5 to generate the target positions of the grinder 5. I do.

Next, a method of controlling the attitude of the grinder 5 will be described. First, a control method of the pitch direction of the grinder 5 (φ _G : around the Y _G axis) will be described.

The pitch angle φ _CG of the grinder 5 with respect to the workpiece 7 is φ _GW
When the grinder 5 is moved in a state where the pressure is equal to the above, there is almost no error in the pressing force controlled by the equation (11).

However, as shown in FIG. 16, phi _CG is at phi _GW larger state, when advancing the grinder 5 (-X _W direction),
Since the grinder 5 attempts to move in the pressing direction (the direction of the arrow in the drawing), the pressing force increases. For example, when the _sheet is sent at a constant speed, the pressing force F _WZ constantly includes an error with respect to the target pressing force F _WZd .

Conversely, if the grinder is moved forward (−X _w direction) while φ _CG is smaller than φ _GW , the pressing force is reduced because the grinder 5 tries to move in the direction in which it is going to separate.
The reverse is true when the grinder is moved backward.

Thus the pressing force F _WZ so becomes the target pressing force F _ZWD, by varying the pitch angle phi _G of the grinder 5 can be always kept phi _GW pitch angle with respect to the workpiece 7 of the grinder 5. That as for formula _{(11), φ Gd (n} ) = K fφ (F WZd -F WZ) + φ Gd (n-1) ... it is represented as (12). Note that K _fφ is a gain, and φ _{Gd (n−1)} is
This is the target pitch angle one sample before.

Incidentally, by rotating the Y _G axis while fixing the center of gravity O _G, because that would alter the pressing force by the change in the attitude, the rotation in the pitch direction, the grinding wheel tip clogging grindstone 15
And the work 7 is rotated around the contact point.

Therefore, although the pitch angle with respect to the workpiece 7 changes according to the equation (11), the control of the pressing direction is not adversely affected.

Actually, since the grinder moves forward and backward, it cannot always follow the curved surface most smoothly when rotated around the contact point. When moving forward, the center of rotation is on the grinder side of the contact point with respect to the contact point. Change as appropriate as needed.

When grinding the like plane, by the equation (11), but eventually the pitch angle phi _GC of the workpiece can be made phi _GW, etc. poor curved trackability without raised the gain K _Ffai the example, when not cope well proportional control of the first term, the integral term is added, the formula (11) _{φ Gd (n) = K fφ} (F WZd -F Wd) + K fφ1 Σ (F WZd - F _WZ ) + _{φGd (n−1)} (13) The tracking performance may be improved. Basically, it is sufficient to control the pitch angle φ _CG to φ _GW .

In Expression (12) or Expression (13), the pitch angle φ with the workpiece 7 is always maintained even when the grinder is moving in the front-rear direction even for an arc-shaped curved surface or a curved surface having a continuously changing curvature. because it is controlled so that the _CG to phi _GW, it is possible to follow.

Since the direction to be rotated is different between forward and reverse, the sign in equation (12) or equation (13) needs to be changed as appropriate.

By combining the control method of the pitch angle with the control method of the pressing direction of the grinder 5 and the forward and backward traveling directions, for example, a curved surface as shown in FIG. 17 can be obtained without previously teaching a change in curvature or the like. It can be ground in the middle direction. However, in order to give the roll angle and the yaw angle of the grinder 5 to the work, the inclination angle of the curved surface is necessary in advance.

Next, a control method of the roll direction (θ axis: around the X _G axis) of the grinder 5 will be described.

When moving the grinder 5 next process, i.e. Y _W direction can be considered in exactly the same manner as the control in the pitch direction above. Roll angle relative to the work of the grinder 5 can not be carried out smoothly grinder work unless the direction of 90 ° (X _W axis and X _C axis match). That is, when the grinder 5 is moved in the horizontal direction in a state where the roll angle of the grinder with respect to the workpiece 7 is equal to 90 °, the equation (12) is obtained.
The error of the pressing force controlled by the control hardly occurs.

However, as in the case of the pitch direction, when the grinder 5 is moved in the lateral direction in a state where the roll angle of the grinder 5 with respect to the workpiece is shifted from 90 °, the grinder 5 advances in the pressing direction or the direction in which the grinder 5 is to be separated. Therefore, the pressing force increases and decreases.

Therefore, similarly to the equation (12) or the equation (13), the roll angle θ _G of the grinder 5 is changed by changing the roll angle θ _G of the grinder 5 so that the pressing force F _WZ becomes the target pressing force F _Wd. The angle can always be kept at 90 °. That _{θ Gd (n) = K fθ1} (F WZd -F WZ) + θ Gd (n-1) ... is represented as (14). Alternatively, by adding an integral, it is _{expressed as} θ _{Gd (n)} = K _fθ1 (F _Wzd −F _Wz ) + K _fθ1 Σ (F _WZd −F _Wz + θ _{Gd (n−1)} (15).

The formula (14), the equation (15), when moving the grinder 5 in the lateral direction, that Y _W direction, the roll angle of the grinder 5 with respect to the workpiece 7 can be kept 90 °.

The target pressing force F _WZ when feeding in the horizontal direction is changed to the target pressing force F
_WZd may be made smaller than the target pressing force during _forward and backward travel.

By combining the control method of the roll angle and the control method of the pressing direction of the grinder 5 and the forward and backward traveling directions, a quadratic curve as shown in FIG. Can be ground in any direction. However, the inclination of the curved surface is required in advance to give the pitch angle and the yaw angle of the grinder 5 with respect to the work 7.

In the above method, the roll angle cannot be corrected when the grinder 5 is moved in the front-rear direction or when the grinder 5 is stopped. Therefore,
Next, a method of controlling the roll angle when the grinder moves forward and backward or stops will be described.

For example, as shown in FIG. 18, since when the roll angle with respect to the workpiece 7 is deviated from 90 DEG is out Z _W and Z _C, because it would impose a tilted direction, X _G axis moment _MGX changes. In a state not subjected to grinding, in the case where the roll angle with respect to the workpiece 7 is pressed in a 90 ° state, does not work moment M _GX, but target moment M _GXD is zero, in the state that grind ,
It does not become zero because the cutting resistance and motor torque work. Also, it differs depending on the type of the grindstone and the work and the pressing force. Therefore, the roll angle with respect to the work 7 is 90
By the moment M _GX during ° gives the target roll angle so that the target moment M _GXD, suppresses roll angle, such as before and after traveling or when stopped grinder 5. That is, θ _{Gd (n)} = K _fθ2 (M _GXd −M _GX ) + θ _{Gd (n−1)} (15) It is necessary to obtain the target moment _MGXd in advance from pressing, etc., but it is also necessary to _check in advance the conditions of the grinder (type of grindstone, pressing force, traveling speed, lateral feed pitch amount depending on the type of work), etc. Considering that, there is no particular burden.

The rotation in the roll direction is, like the rotation in the pitch direction, performed around the tip of the grindstone, that is, the vicinity of the contact point between the grindstone and the work.

Then the yaw direction: Control method (phi _G X about _{the G} axis) will be described.

When the yaw direction is changed, the traveling direction of the grinder 5 changes. Therefore, it is set in advance depending on the grinding area. There is no need to change during grinding,
When it is necessary to change the grinding direction and the grinding area, change them as necessary.

As for the method of changing the posture, a method of rotating around each axis of the coordinate system ΣG has been described, but the posture can be similarly changed by rotating around each axis of the coordinate system Σw.

The above method teaches the shape of the 3D curved surface in advance by controlling the pressing direction of the grinder, the forward / backward moving direction, the translation direction of the lateral feed direction, and the roll, pitch, and yaw postures. Without this, the posture is automatically changed so as to maintain the grinder posture with respect to the workpiece, and the grinder work can be performed with an arbitrary pressing force.

For example, when performing an irregular grinder work in a special environment, etc., even in a work where the shape of the work or the position of the work with respect to the robot cannot be set in advance, If the approximate positional relationship and the attitude of the grinder can be given through a monitor or the like, the work can be performed easily and efficiently.

In the manual operation using the joystick, the worker only needs to be given an approximate initial posture, and then only needs to perform a two-dimensional operation of the joystick for instructing the movement of the grinder 5, and the pressing force and the grinder posture are automatically set. Grinding can be performed while controlling the temperature. In addition, by setting in advance the moving direction of the grinder 5 and the movement pattern in the lateral direction, it is possible to easily and automatically grind an arbitrary region.

Thus, an example of an experimental result obtained by performing a grinding operation without previously teaching a workpiece shape by the above control method will be described. A workpiece having a cross-sectional shape as shown in FIG. 19 is moved in the direction of the arrow in the figure, with a target pressing force F _WZd = 1.2 kgf, a grinder moving speed ΔX _WZ = 2.5 cm / s, and a target pitch angle φ _GW = 25 for the workpiece.
゜ And the pitch angle of the grinder with respect to the initial work
40 ° (accordingly, the error is 15 °), the grinding wheel calculated from the pressing force of the grinder against the work, the pitch angle (representing the angle from the horizontal), and the joint angle of 1 to 6 axes when grinding. The trajectory of the tip is shown in FIGS. 20 to 22. From these figures, it can be confirmed that the posture is changed so that the pitch angle with respect to the workpiece is 25 °, and the grinding is performed with a constant pressing force.

Sixth Embodiment Next, a description will be given with reference to FIG. This embodiment is another embodiment of the force control device. In the fifth embodiment, a method of performing a grinding operation while pressing a workpiece having an unknown shape with a constant force has been described. However, in some cases, it is desired to finish a workpiece having an unknown shape into an arbitrary shape. The case will be described below. The block shown in FIG. 15 shows only a portion different from the force control device 41 shown in FIG.

First, in the first grinding, since the force control robot has not confirmed the work shape, the grinding operation is performed while pressing the work having an unknown shape with a constant force by the force control device of the fourth embodiment. At that time, the joint angles of the first to sixth axes are converted by the inverse coordinate converter 53 into the actual grinder position, orientation X _O , Y _O , Z _O , α _O , β _O , γ represented in the absolute coordinate system. Calculate _O ,
This is stored in the work shape data storage device 55. Thereby, the force control robot can recognize the shape of the work.

In the subsequent grinding, the grinder target position / posture creation device 57 represents the absolute coordinate system based on the data stored in the work finish shape data storage device 59 and the data stored in the work shape data storage device 55 given from the computer 43. Target position and orientation X _OP , Y _OP , Z _OP , α _OP , β _OP , γ
Create an _OP .

Then, similarly, each of the above data is converted to a driving coordinate converter.
Input to 51 to drive each axis. Thereby, the workpiece 7 having an unknown shape can be finished into an arbitrary shape.

In the grinder target position / posture creating device 57, when the shapes are different between the finished shape data and the stored data, force control is provided in the normal direction of the work 7 and a target position corresponding to the moving speed is provided in the moving direction. If there is no difference in shape or if the shape is small, position control may be performed. In any case, since the shape data of the work is already known at this point, a conventional control method may be applied.

Also, especially when not working with tools,
It can also be used as a means for checking an unknown shape.

As an application example of the present invention, in each of the embodiments described above, a cylindrical coordinate type robot has been described. However, the present invention is not limited to this type of robot, and can be applied to a rectangular coordinate type, polar coordinate type, and articulated coordinate type robot. .

In each of the above embodiments, the six-axis force sensor is used as the pressing force detecting sensor. However, the present invention is not limited to this, and it is sufficient that a force in a required direction can be detected.

In addition, the moment _MWG acting around the center of gravity of the grinder 5 is used to detect the pressing force. However, if the small grinder is not much affected by the inertial force, the direct pressing force can be detected. good.

[Effects of the Invention] As described above, according to the force control device and the force control robot according to the present invention, the gravity and the inertial force are easily eliminated, and the accurate pressing force of the hand effector on the workpiece is detected. This makes it possible to provide an excellent force control robot having good responsiveness in force control and the like and a high accuracy.

[Brief description of the drawings]

FIG. 1 is a side view showing a hand portion, FIG. 2 is a perspective view showing an external appearance of a force control robot, FIG. 3 is a model diagram of a model of the force control robot, and FIG. 4 shows a configuration of a force control device. FIG. 5A is a block diagram, FIG. 5A is a plan view showing a hand portion of the second embodiment, FIG. 5B is a side view, and FIG. FIG. 7 (a) is a plan view showing a gripper, showing a fourth embodiment, FIG. 7 (b) is a side view, FIGS. 8 to 13 are views showing experimental results of pressing force, FIG. FIG. 14 is a block diagram showing a force control device according to the fifth embodiment, FIG. 15 is a block diagram showing a part of the force control device according to the sixth embodiment, and FIG. 16 is a hand portion of the fifth embodiment. Side view,
FIG. 17 is a perspective view showing an example of a two-dimensional work, FIG. 18 (a) is a side view showing a hand part, and FIG. FIG. 19 is a side view showing a cross-sectional shape of a workpiece, FIGS. 20 to 22 show experimental results, FIG. 20 is a diagram showing a relationship between time and a grinder pressing force, and FIG. FIG. 22 is a diagram showing the relationship between time and the pitch angle, and FIG. 22 is a diagram showing the relationship between time and the trajectory of the grinding wheel tip. DESCRIPTION OF SYMBOLS 1 ... Force control robot 3 ... Hand part 5 ... Grinder 7 ... Work (workpiece) 11 ... 6-axis force torque sensor 19, 41 ... Force control device 35 ... Coordinate converter 37 ... Counter Weight 45 Sensor coordinate converter 47 Grinder coordinate converter 49 Grinder position coordinate converter 51 Drive coordinate converter 53 Reverse coordinate converter 55 Work shape data storage device 57 Grinder Target position / posture creation device 59 …… Work finish shape data storage device

──────────────────────────────────────────────────続 き Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) B25J 19/02 B25J 13/08 B25J 17/02 B24B 27/00 B24B 49/16

Claims (3)

(57) [Claims] 1. A force control device for detecting a pressing force of a hand effector attached to an end of an arm of a robot against a workpiece and controlling the robot so that the pressing force becomes a target pressing force. Detecting means disposed between the arm end and the hand effector, for detecting a force and a moment based on a reaction force of the pressing force applied from the workpiece to the hand effector; Calculating means for performing a coordinate transformation from the force and moment detected by the detecting means to obtain a moment around the center of gravity of the hand effector, and calculating the pressing force from the moment; and Control means for controlling the robot such that the pressing force matches the target pressing force. 2. A robot which detects a pressing force of a hand effector attached to an end of an arm against a workpiece and controls the pressing force to be a target pressing force. Detecting means for detecting a force and a moment based on a reaction force of the pressing force applied to the hand effector from the workpiece to be processed, which is disposed between the portion and the hand effector; From the force and the moment, coordinate transformation is performed to obtain a moment around the center of gravity of the hand effector, and the pressing force is calculated from the moment, and the first calculating means calculates the pressing force. Control means for controlling the hand effector so that the pressing force matches the target pressing force. 3. An output signal of a target pressing force of the hand effector against a workpiece, an output signal of a target position of the hand effector, and an output signal of a target posture position of the hand effector. 3. The robot according to claim 2, further comprising second calculating means for outputting a target angle.