JP2000190261A - Manipulator - Google Patents

Abstract

PROBLEM TO BE SOLVED: To secure the safety of an operator, and restrain working performance in a carrying operation from being lowered down. SOLUTION: In this manipulator 10 permitting a work to be moved to the direction of external force applied by an operator, when the direction of the aforesaid force is dislocated from the direction set-up in the movement of a work, which is obtained based on the moving locus of the work stored in the memory in advance, the moving speed of the work is decelerated from the normal working speed in response to the quantity of dislocation in the moving direction. This constitution thereby allows the work W to be moved at the normal working speed by the manipulator 10 when force is applied to the set-up direction or the direction very close to the aforesaid direction, but also allows the work to be gradually moved by the manipulator 10, when force is applied by the operator to the direction entirely different from the set-up direction.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a manipulator that works in cooperation with an operator when moving a work.

[0002]

2. Description of the Related Art Generally, a manipulator that moves a workpiece in the direction of an external force applied by an operator monitors the magnitude of the external force in consideration of safety. The manipulator is forcibly stopped (see Japanese Patent Application Laid-Open No. 4-344505).

[0003]

In the above-described manipulator, even when an operator applies a force equal to or more than a predetermined value in a moving direction of the work or in a direction close thereto, the manipulator is stopped immediately. However, if the operator applies a force in the direction to move the work, even if the magnitude of the force is a certain value or more, the manipulator should not move beyond the normal working speed. If this is the case, the safety of workers is sufficiently ensured. Therefore, emergency stop of the manipulator until the above case is an excessive safety measure, and is not preferable in terms of work efficiency.

[0004] Therefore, claims 1 to 5 of the present invention.
According to the invention described in (1), the direction of the external force applied by the worker is compared with the moving direction of the work stored in the memory, and the operating speed of the manipulator is reduced from the normal working speed in accordance with the direction deviation between the two, so that the worker The object of the present invention is to suppress a decrease in the work efficiency of the work transfer work while ensuring the safety of the work.

[0005]

The above object is achieved by a manipulator according to the present invention. According to the present invention, when the direction of the external force applied to the manipulator for moving the work by the operator is shifted with respect to the movement direction (set direction) of the work stored in advance, the manipulator shifts in the direction. The moving speed of the work is reduced accordingly. In other words, the manipulator moves the work at almost the normal working speed if the worker applies a force in or near the set direction, but if the worker applies a force in a direction completely different from the set direction, the manipulator slowly moves the work. Move. Therefore, even if force is applied in the wrong direction due to an erroneous operation by the operator, the safety of the operator is ensured because the manipulator moves slowly. Also, even if the operator applies an external force equal to or greater than the specified value to the manipulator, only the moving speed of the work changes in accordance with the direction of the external force. Absent. For this reason, it is possible to suppress a decrease in workability in the work transfer operation.

[0006] The above-mentioned problem is solved by a manipulator according to the present invention. According to the present invention, it is a point on a line segment connecting the set coordinates of the work obtained from the movement locus of the work stored in advance and the current position coordinates of the work corresponding to the set coordinates. The trajectory of a point at which the distance to the current position coordinates is divided at a fixed ratio is stored in the memory as the movement trajectory of a new work. Therefore, each time the number of times of the work transfer operation overlaps, the movement locus of the work stored in the memory approaches the movement locus of the work actually moved by the operator. That is, since the manipulator learns so as to approach the movement of the worker, handling of the manipulator is facilitated and work efficiency is improved.

[0007] The above object is achieved by a manipulator according to a third aspect. According to the present invention, in order to determine not only the shift in the movement direction of the work but also the shift in the direction of the posture change and determine the movement speed of the work,
Safety is further improved.

[0008] The above-mentioned problem is solved by a manipulator according to a fourth aspect. According to the present invention, for each predetermined moving distance, in order to compare the moving direction of the work and the direction of the external force obtained based on the moving locus of the work stored in the memory, the manipulator during the work transfer work It can always be operated at an appropriate speed.

[0009] The above-mentioned problem is solved by a manipulator according to claim 5. According to the present invention, the manipulator according to claim 4, for comparing the moving direction of the work and the direction of the external force obtained based on the moving trajectory of the work stored in the memory at every predetermined time. As in the case of (1), the manipulator can be operated at an appropriate speed.

[0010]

DESCRIPTION OF THE PREFERRED EMBODIMENTS (First Embodiment) FIG.
A description will be given of a manipulator according to the first embodiment of the present invention with reference to FIGS. The manipulator 10 is a device for moving the work W in the direction of an external force applied by the worker M, and includes a multi-joint manipulator main body 12 and a controller 13 for controlling the multi-joint manipulator main body 12. The following description will be made with the front-rear direction of the manipulator 10 as the X-axis direction, the left-right direction as the Y-axis direction, and the height direction as the Z-axis direction.

The multi-joint manipulator main body 12 has a clamp 12k for gripping a workpiece W at a tip end thereof.
A probe 12p operated by an operator M is mounted near the clamp 12k. Further, the probe 12
The probe M is attached to the mounting portion of
A 6-axis force sensor 12s for detecting the force and torque applied to p is mounted. The 6-axis force sensor 12s is configured to detect the forces applied in the X, Y, and Z axis directions,
This sensor is capable of detecting the torque around the axis, and the output signal of the six-axis force sensor 12s is a sensor amplifier 14 (FIG. 2).
) Is input to the controller 13.

The controller 13 is a six-axis force sensor 12
s based on the signal from the multi-joint manipulator body 12
And a CPU, RO (not shown)
M, RAM and the like. The output signal of the controller 13 is converted into an analog signal by a D / A converter, input to the servo amplifier of the multi-joint manipulator body 12, and drives the motor of each joint via the servo amplifier.

Next, FIG.
The operation of the manipulator 10 according to the present embodiment will be described based on the flowchart of FIG. Note that a program for executing the flowchart is stored in the RAM, and is repeatedly executed at predetermined times from the start to the end of the work. First, the first transfer work of the workpiece W will be described. Note that a standard movement locus of the center of gravity of the work is stored in advance at address n (hereinafter referred to as memory) of the memory of the controller 13 in a preparation stage of the work transfer operation.

First, the coordinates P. of the start point from the movement locus of the center of gravity of the work (hereinafter referred to as the work locus) stored in the memory. 0 is read (step 10 in FIG. 4).
2) The center of gravity of the work W is actually the start point P. It is determined whether it is 0 (step 103). The work W starts at the start point P. If it is not 0, a message indicating that the manipulator 10 is abnormal is output and the program is terminated (step 109).

When the workpiece W has a start point P. If it is 0, n = 0 in step 104, and 1 is added to n in step 105. Therefore, at this stage, n = 1. Next, the process proceeds to step 106, where a moving speed determination process and a memory coordinate calculation process are performed. The moving speed determination processing and the memory coordinate calculation processing are executed based on the flowchart shown in FIG. First, at step 121, the set coordinates P. of the work based on the work movement locus. n-1 and the set coordinates P. n are read from the memory. Hereinafter, the set coordinates P. n-1 and the set coordinates P. n is simply the coordinate P.N. n
-1, coordinates P. It is called n. Since n is set to 1 (see step 105), the coordinates read from the memory are P.N. 0 and P. Is one. Coordinates P. 0 is the start point as described above, and the coordinates P.O. 1 is a coordinate on the work movement locus, which is a start point P. These coordinates are located at a distance of 2R from 0 (FIG. 3A).

The coordinates P. 0 and coordinate P. The moving direction A of the workpiece W is obtained from the above. That is, the moving direction A of the work W is the coordinate P. 0 to coordinate P. It is equal to the direction of the vector towards 1. Next, the direction F of the force actually applied to the probe 12p by the operator M is determined by the six-axis force sensor 1
Measured by 2s (step 122).

Next, the coordinates P. 0 and coordinate P. The moving speed V of the work W, that is, the operating speed V of the manipulator 10 is calculated based on the angle θ formed by the moving direction A of the work W obtained from 1 and the direction F of the force applied to the probe 12p (step 123). ). The moving speed V of the work W is given by the following equation. V = V1 + | (V2-V1) COSθ |
Here, -π / 2 ≦ θ ≦ π / 2, where V1 is the safety reference speed, and V2 is the work speed (≒ 2 × V1). In addition,
When θ is other than −π / 2 ≦ θ ≦ π / 2, that is, when the moving direction A of the workpiece W and the direction F of the force are opposite or close to each other, the moving speed V becomes the safety reference speed V1. Becomes For this reason, if the worker M applies the force F in the moving direction A or a direction close thereto, the manipulator 10 moves the work W substantially at the working speed V2, but the worker M applies the force in a direction completely different from the moving direction A. For example, the manipulator 10 moves the work W slowly at the safety reference speed V1.

When the manipulator 10 moves the work W in the direction of the force F at the speed V in this manner, step 1
At 24, the coordinates (current coordinates) of the center of gravity of the work W are changed to the position coordinates P. It is determined whether or not (n-1) w is separated by a distance 2R. Since n = 1 is set as described above (see step 105), the position coordinates P.P. (N-1) W is the coordinate P. 0 (start point).

Then, the manipulator 10 moves the work W in the direction of the force F, and the work W 0
When the distance from the (start point) is 2R, the work W
Current position coordinates (P.1W) and coordinates P. 1 and the memory coordinates P. 1M is obtained (step 125) (FIG. 3 (A)
reference). Memory coordinates P. 1M indicates the current position coordinates (P.1W) of the workpiece W and the coordinates P.1M. 1 is a point on a line segment connecting the current position coordinates (P.1W) of the workpiece W to the coordinates P.1 The point is that the distance L to 1 is divided by the ratio of a: b. That is, from the current position coordinates (P.1W) of the work W to the memory coordinates P.W. The distance to 1M is a × L ÷ (a + b), coordinates P. 1 to the memory coordinates P. The distance to 1M is represented by b × L ÷ (a + b).

When the processing in step 125 is completed in this way, it is determined in step 107 (FIG. 4) whether or not the point is the final point. Since n = 1, the process proceeds to step 105, where 1 is added to n, and n = 2. Next, in step 121 of FIG. 1 and coordinates P. 2 is read from the memory. Coordinates P. 2 is the coordinates on the work locus, and the coordinates P. These are coordinates located at a distance of 2R from 1. Then, the coordinates P. 1 and coordinates P. The moving direction A of the workpiece W is obtained from the above (see FIG. 3B).

Next, the probe 1 is actually
The direction of the force F applied to 2p is 6-axis force sensor 12s
(Step 122), the coordinates P. 1 and coordinates P. 2, the moving speed V of the work W, that is, the operating speed V of the manipulator 10 is calculated based on the angle θ formed between the moving direction A of the work W and the direction of the force F applied to the probe 12p (step 123). ). The arithmetic expression is the above-mentioned expression.

When the moving speed V of the work W is determined in this way, the manipulator 10 moves the work W at the speed V in the direction of the force F. Then, in step 124, the coordinates (current coordinates) of the work W are set to the position coordinates P.P. Distance 2R from 1w
Is determined (step 124). When the workpiece W has the position coordinates P. 1W, the current position coordinates (P.2W) and coordinates P.W. 2 and the memory coordinates P. 2M is obtained (step 125) (see FIG. 3B).

When the processing of step 125 is completed in this way, it is determined in step 107 whether or not it is the final point. Since n = 2, the process proceeds to step 105,
Here, 1 is added to n, so that n = 3. Next, at step 121, the coordinates P. 2 and coordinates P. 3 is read from the memory. Coordinates P. 3 is the coordinates on the workpiece movement locus, and the coordinates P. These are coordinates located at a distance of 2R from 2. Then, the coordinates P. 2 and coordinates P. The moving direction A of the workpiece W from 3
(See FIG. 3C).

Next, the probe 1 is actually
The direction of the force F applied to 2p is 6-axis force sensor 12s
(Step 122), the coordinates P. Two
And coordinates P. The moving speed V of the work W is calculated based on the angle θ between the moving direction A of the work W obtained from step 3 and the direction of the force F applied to the probe 12p (step 12).
3). The arithmetic expression is the above-mentioned expression.

When the moving speed V of the work W is determined in this way, the manipulator 10 moves the work W at the speed V in the direction of the force F. Then, in step 124, the coordinates (current coordinates) of the work W are set to the position coordinates P.P. Distance 2R from 2w
Is determined. When the workpiece W has the position coordinates P. 2W, the current position coordinates (P.3W) and the coordinates P.W. 3 and the memory coordinates P. 3M is obtained (step 125) (FIG. 3)
(B)). Then, when the processing of step 125 is completed, it is determined in step 107 whether or not it is the final point.

In this way, when the processing from step 105 to step 107 in FIG. 4 is repeatedly executed and the work W reaches the final point n = e, the coordinates P.D. 1 ... coordinates P. e is the new memory coordinate P. 1M ... coordinates P. Rewritten to eM. That is, the first standard work movement locus is rewritten to a new work movement locus, and the first work transfer operation is completed. The work movement locus is composed of e = (work locus length J) / 2R coordinates. For example, R
= 0.5mm, work path length J = 10000mm, e = 100
It becomes 00. Therefore, the processing from step 105 to step 107 in FIG. 4 is repeatedly executed 10,000 times. The second and subsequent work transport operations are performed in the same procedure as the first work based on the rewritten new work movement trajectory.

As described above, according to the manipulator according to the present embodiment, if the worker M applies the force F in the direction A obtained from the movement trajectory of the work or in a direction close thereto, the manipulator 10 is operated at the normal work speed V2. Although the work W is moved, the manipulator 10 operates slowly if the worker M applies a force in a direction completely different from the set direction. For this reason, even if the force F is applied in the wrong direction due to an erroneous operation of the worker M, the safety of the worker M is ensured because the manipulator 10 moves slowly. Further, even if the worker M applies an external force F to the manipulator 10 that is equal to or greater than a specified value, only the moving speed of the work W changes in accordance with the direction of the external force F. There is no emergency stop. For this reason, it is possible to suppress a decrease in workability in the work transfer operation.

According to the manipulator 10,
Work coordinates P. determined from the work movement trajectory n and its coordinates P. n of the current position of the work corresponding to P.n. nW is a point on a line segment connecting nW and the coordinates P. n to the current position coordinates P. of the work. nW to a constant ratio (b:
The trajectory of the point (P.nM) to be divided in a) is stored in the memory as the movement trajectory of the new work. Therefore, each time the number of times of the work transfer operation overlaps, the movement trajectory of the work stored in the memory comes closer to the trajectory where the worker M actually moves the work W. That is, since the manipulator 10 learns to approach the movement of the worker M, the handling of the manipulator 10 is facilitated, and the work efficiency is improved.

Further, according to the manipulator 10,
In order to compare the direction A of the external force F with the moving direction A of the work obtained based on the moving locus of the work stored in the memory at every predetermined moving distance (2R), the manipulator 10 during the work transfer operation of the work is used. Can always be operated at an appropriate speed.

(Second Embodiment) Hereinafter, a manipulator according to a second embodiment of the present invention will be described with reference to FIGS. Manipulator 2 according to the present embodiment
0 is such that the change in the posture of the work is taken into account in the moving speed determination processing and the memory coordinate calculation processing of the manipulator 10 according to the first embodiment, and for the other, the manipulator according to the first embodiment. Same as 10. That is, in the manipulator 20 according to the present embodiment, in step 221 of FIG. n−1 and the coordinates P. n is read from the memory and the moving direction A of the work W is read.
Is obtained, the posture change vector ΔKn is obtained from the posture vector (Kn−1, Kn) of the work W at the coordinates (Pn−1, Pn) (step 22).
2).

Next, the probe 1 is actually
The force F applied to 2p is measured by the 6-axis force sensor 12s, and the direction of the force F and the posture change vector ΔKRn of the work W are obtained from the measured value (step 223). Then, the coordinates P. n−1 and the coordinates P. Work W obtained from n
The moving speed V of the workpiece W from the moving direction A of the workpiece and the direction of the force F.
Is determined (step 224). The method of determining the moving speed is the same as that of the manipulator 10 according to the first embodiment.

Next, the posture vector (Kn-
1, K. n) based on the angle θk formed by the posture change vector ΔKn obtained from n) and the actual posture change vector ΔKRn obtained from the measurement value of the six-axis force sensor 12s.
The moving speed V is corrected. The moving speed V is corrected based on the following equation. The corrected moving speed Vh = V1 + | (V-V1) | COS θk where -π / 2 ≦ θk ≦ π / 2, where V1 is the safety reference speed and V is the moving speed. Note that θ is −π / 2 ≦
In cases other than θ ≦ π / 2, the moving speed Vh becomes the safety reference speed V1.

In this manner, the manipulator 20 moves the work W in the direction of the force F at the speed Vh, and the work W (current coordinates) has the position coordinates P. When the distance from the (n-1) w is 2R (step 226), the current position coordinates (P.nW) and the coordinates P.N. n and the memory coordinates P.N. nM is determined, and the memory coordinates P.n are calculated from the actual attitude change vector ΔKRn and the attitude change vector ΔKn. nM, a new posture vector K. nM is determined (step 2
27).

The memory coordinates P. nM, a new posture vector K. nM is determined by the following procedure. First, a new posture change vector ΔKM is obtained by adding the actual posture change vector ΔKRn × a and the posture change vector ΔKn × b.
Find n. Next, the memory coordinates P. (N-1) New posture vector K. (N-1) M and the new attitude change vector ΔKMn are added to the memory coordinates P.N. nM, a new posture vector K. Find nM. here,
a + b = 1. Note that the current position coordinates (P.nW) and the coordinates P.N. n and the memory coordinates P.N. The method for obtaining nM is the same as that of the manipulator 10 according to the first embodiment.
Is the same as

In this way, when the process of step 227 is completed, it is determined in step 207 (see FIG. 7) whether or not the workpiece is at the final point. The stored coordinates P.
1-P. e is the new memory coordinate P. 1M-P. eM and the posture vector K. 1 to K. e is the new posture vector K. 1M-K. Rewritten to eM.

As described above, according to the manipulator 20 according to the present embodiment, not only the shift in the moving direction of the work but also the shift in the direction of the posture change is monitored, and the moving speed Vh of the work is determined. The performance is further improved.

Hereinafter, a manipulator according to a third embodiment of the present invention will be described with reference to FIG. 1st, 2nd
While the manipulators 10 and 20 according to the embodiment compare the moving direction A and the direction of the external force F obtained based on the moving trajectory of the work at every predetermined moving distance 2R, the present embodiment The manipulator 30 compares the moving direction A with the direction of the external force F at each reference sampling time. For this reason, in the moving speed determination processing and the memory coordinate calculation processing (FIG. 8), the memory coordinates P.P. n
-1M (see step 125), the memory coordinates P.P. nM is determined (see steps 124 and 125). Therefore, the first,
Like the manipulators 10 and 20 according to the second embodiment, the manipulator 30 can be operated at an appropriate speed during the work of transporting the work W.

[0038]

According to the present invention, the manipulator moves the work at a normal working speed when the worker applies a force in or near the set direction, but the worker applies a force in a direction completely different from the set direction. The manipulator moves the work slowly. For this reason, the safety of the worker is ensured, and since the manipulator does not stop urgently during the work as in the related art, a decrease in workability in the work transfer work can be suppressed.

[Brief description of the drawings]

FIG. 1 is a side view illustrating a usage example of a manipulator according to a first embodiment of the present invention.

FIG. 2 is an overall block diagram of the manipulator according to the first embodiment of the present invention.

FIGS. 3A, 3B, and 3C are schematic plan views illustrating a movement locus of a work of the manipulator according to the first embodiment of the present invention.

FIG. 4 is a flowchart illustrating an operation of the manipulator according to the first embodiment of the present invention.

FIG. 5 is a flowchart illustrating an operation of the manipulator according to the first embodiment of the present invention.

FIG. 6 is a flowchart illustrating an operation of the manipulator according to the second embodiment of the present invention.

FIG. 7 is a flowchart illustrating an operation of the manipulator according to the second embodiment of the present invention.

FIG. 8 is a flowchart illustrating an operation of the manipulator according to the third embodiment of the present invention.

[Explanation of symbols]

 W Work 10 Manipulator 12 Manipulator body 12p Probe 12s 6-axis force sensor 13 Controller

 ────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Kenichi Mitsuda 2-6-36, Daimyo, Chuo-ku, Fukuoka-shi, Fukuoka B F Co., Ltd. F-term (reference) 3F059 AA01 BA03 CA00 FC02 FC15

Claims (5)

[Claims] 1. A manipulator for moving a work in a direction of an external force applied by an operator, wherein the direction of the external force is deviated from a movement direction of the work obtained based on a movement locus of the work stored in a memory in advance. A manipulator that reduces the movement speed of the work from the normal work speed according to the deviation in the direction. 2. The manipulator according to claim 1, wherein a set coordinate of the work determined from a movement locus of the work stored in advance and a current position coordinate of the work corresponding to the set coordinate are on a line segment. A manipulator that stores, in a memory, a trajectory of a point that divides a distance from a set coordinate of a work to a current position coordinate of the work at a fixed ratio as a movement trajectory of a new work. 3. The manipulator according to claim 1, wherein the direction of the posture change of the work determined by the direction of the external force with respect to the direction of the posture change determined from the posture of the work at each position stored in the memory. When the direction is shifted, the manipulator corrects the moving speed of the work according to the direction change of the posture change. 4. The manipulator according to claim 1, wherein for each predetermined moving distance, the moving direction of the work obtained based on the moving trajectory is compared with the direction of the external force. 5. The manipulator according to claim 1, wherein the direction of the external force and the moving direction of the workpiece obtained based on the moving trajectory are compared at predetermined times.