JPH01229306A - Copy teach control system - Google Patents

Abstract

PURPOSE:To obtain the force control direction of a face executing a copy action and a position control direction which is horizontal with respect to said direction by giving a speed command toward the position control direction based on the force setting force of a force detection part, the position and the setting position of a position detection part. CONSTITUTION:The position detection part 5 detects a suitable position which is the controlled system 1, which is positioned except for the surface of an object, and an operation part 2 transmits a prescribed manipulated variable to the controlled system 1, which is brought in contact with the surface of the object. When the force detection part 6 detects force which the controlled system 1 has received, and a force control means 4 recognizes contact at that time, the detection part 5 detects the contact position of the controlled system 1 at the time of contact, and outputs a position coordinate. At the time of the copy action, the means 4 gives the speed command toward the force control direction based on force and setting which the detection part 6 has detected and which have been given. A position control means 3 gives the speed command toward the position control direction based on the present position which the position control means has detected and the setting position.

Description

[Detailed Description of the Invention] (Table of Contents) Overview Industrial Field of Application Prior Art Figures 4, 9, and 10 Problems to be Solved by the Invention Figure 5 Means for Solving the Problems
Figure 1 Effect Figure 1 Embodiment Figures 2 to 9 Effects of the invention [Summary] This is a system that operates a controlled object that performs a tracing operation along the surface of an object based on a speed command. To provide a copying teaching control method that can automatically obtain a force control direction for a surface on which a copying operation is performed, a position control direction perpendicular thereto, and the directions of these control directions. For the purpose of a control direction calculation unit that calculates a control direction and a position control direction; a direction determination unit that determines the direction of the position control direction and determines the direction of the force control direction using the stored appropriate position and contact position as elements; It is configured to include a direction and a force control means for issuing a speed command in the direction based on the direction, and a position control means for issuing a speed command in the direction based on the position control direction and the direction.

[Industrial application field]

The present invention relates to a scanning teaching control method for a robot to perform a copying operation such as machining along the surface of an object, and particularly relates to a control object that performs a copying operation along the surface of an object, and a speed control method for controlling the control object. Tracing teaching that teaches the speed command in a system that includes an operation section that operates based on a command, a position detection section that detects the position of the controlled object, and a force detection section that detects the force that the controlled object receives. Regarding control method.

[Conventional technology]

Conventionally, processing along the surface of the object, such as deburring,
It is already known that a robot having a force sense is used for tasks such as wiping, and a tracing control method used at this time has already been proposed.

As shown in FIGS. 4 and 9, in order to control the tracing operation on the plane S0, the robot reference coordinate system (Xr,
Coordinate values of the start point SA and end point SB of the copying operation in the hand coordinate system (Xh, Yh, Zh);
The direction of movement (hereinafter referred to as "position control direction"), the direction of the position control direction, the magnitude of the force applied to the plane to maintain contact with the plane, the normal direction of the plane (hereinafter referred to as "force control direction") ) and the direction of the force control direction.

Conventionally, such directions and orientations have been determined using a simple plane, such as the horizontally spread plane 81, or by measuring the surface of the object and determining the normal vector of the surface and the vector of the movement direction in advance. I tried to give it to him.

That is, in the conventional tracing teaching control system, the 10th
As shown in the figure, a manipulator 81 that performs operations such as a copying operation, an operation section 82 that operates the manipulator 81, a position detection section 85 that detects the current position of the manipulator 81, and a detection section 85 that detects the force that the manipulator 81 receives. A force detecting section 86 that performs this, a position control means 83 that gives a speed command in the position control direction of the manipulator 81, and a force control means 84 that gives a speed command in the surface force control direction.

Then, the position detected by the position detecting section 85 and the set position are sent to the position control means 85, and the position control direction and direction on the plane in which the copying operation is performed are set in advance, and the force control means 84 The force detection unit 86 also sends out the force detected by the force detection unit 86 and the set force, and also sets the force control direction and orientation of the plane on which the tracing operation is performed, which has been checked in advance, to perform the tracing operation on the surface of a predetermined object. It was designed to control the teaching of

(Problem to be Solved by the Invention) By the way, in the conventional copying motion teaching control system, the operator examines the plane equation of the surface of the object to be copied, the normal vector of the plane, etc., and performs the control. It is necessary to teach the movement by imitating the system.

Therefore, for example, when performing a copying operation on a plane arbitrarily placed in the robot reference coordinate system set on the robot as shown in Figure 5, or on a curved surface with a complex shape, it is difficult to Unlike the case of a flat plane, it is necessary to perform complicated measurements, which has the problem of being time-consuming.

Therefore, the technical problem for solving the above-mentioned problems of the present invention is to be able to automatically obtain the force control direction of the surface on which the copying operation is performed and the position control direction perpendicular to this when performing the copying operation. In particular, it is an object of the present invention to provide a tracing teaching control system that also determines these directions.

[Means to solve the problem]

As shown in FIG. 1, the means of the present invention for solving the above problems is such that, prior to the above-mentioned copying operation, a position detecting section 5 detects an appropriate position of a controlled object 1 located other than on the surface of the object. and a position storage unit 7 that stores three or more contact positions of the controlled object 1 brought into contact with the surface, and a force control direction and force on the surface of the controlled object 1 during a copying operation using the stored contact positions as elements. a control direction calculation unit 8 that calculates a position control direction perpendicular to the control direction; and a direction determination unit that determines the direction of the position control direction and determines the direction of the force control direction using the stored appropriate position and contact position as elements. a force control means 4 that issues a speed command in the force control direction based on the force control direction, the direction of the force control direction, the force detected by the force detection section 6, and the set force during the copying operation; and a position control means 3 for issuing a speed command in the position control direction based on the position detected by the position detection section 5 and the set position during the copying operation. It is something.

[Effect]

When performing copying teaching according to the present invention, prior to the copying operation, the position detecting section 5 first detects an appropriate position of the controlled object 1 located other than on the surface of the object. after that,
The operating unit 2 sends out a predetermined amount of operation to the controlled object 1 to bring it into contact with the surface of the object. At this time, when the force that the controlled object 1 receives is detected by the force detection section 6 and the contact is recognized by the force control means 4, the position detection section 5 detects the contact position of the controlled object 1 at the time of the contact. is detected and the corresponding position coordinates are output. Then, the above-mentioned contacting operation is repeatedly performed at at least three different points on the surface on which the tracing operation is to be performed, and the position coordinates of at least three contact positions are obtained.

The appropriate position and contact position detected by the position detection section 5 are stored in the position storage section 7.

Next, the control direction calculation unit 8 recognizes, for example, a plane through which the contact point or its neighboring point passes, based on the three contact positions, and calculates the force control direction and position control direction of the control object 1.

Further, the direction determining section 9 determines the direction of the position control direction, and also determines the direction of the force control direction using the stored appropriate position and contact position as elements.

During the copying operation, the force control means 4 determines the direction of the force control direction based on the force control direction, the direction of the force control direction, the currently received force detected by the force detection unit 6, and the set force. At the same time, the position control means 3 issues a speed command in the position control direction based on the current position detected by the position detection section 5 and the set position. will be emitted.

〔Example〕

Next, the present embodiment will be explained.

As shown in FIGS. 2 and 7, this embodiment includes a manipulator 11 as the controlled object 1, and the manipulator 11
an operating section 12 that controls the operation section 12; and a position detection section 15 that detects the position of the tip of the hand section 11a of the manipulator 11.
and a force detection section 1 that detects the force applied to the hand section 11a (shown in FIGS. 4 and 5) of the manipulator 11.
6, detected by the position detection unit 15 prior to the copying operation,
An appropriate position Z of the tip of the hand portion 11a located other than on the surface of the object, and three contact positions A of the tip of the hand portion 11a brought into contact with the surface.

A position storage unit 17 that stores the contact positions A, B, and C calculates a plane equation including the three contact positions A, B, and C stored above to determine the force control direction and the force control direction of the manipulator 11 on the surface during the copying operation. a control direction calculation unit 18 that calculates a vertical position control direction; and the stored contact position A;
A direction determining unit 1 that determines the direction of the force control direction using B and C as elements, and determines the direction of the position control direction using the coordinates of the start point SA and end point SB of the copying operation as elements.
9 and the above force control direction during copying operation.

a force control means 14 that issues a speed command in the direction of the force control direction based on the direction of the force control direction, the force detected by the force detection unit 16, and the set force; and a position control means 13 that issues a speed command in the position control direction based on the position detected by the position detection section 15 and the set position.

Further, the control direction calculating unit 18 and the direction determining unit 19 calculate a set trajectory plane SK, which will be described later, from the start point SA, end point SB, and the stored appropriate position Z of the tracing motion trajectory, and calculate the set trajectory plane SK, which will be described later, from the contact positions A and B.

It also has a function of calculating the direction of movement of the manipulator 11 toward C and its orientation.

Further, the operating section 12 includes a servo motor 12a, a power amplifier 12b, a D/A converter 12c, and a compensator 12d.

Further, the position detection section 15 includes a counter and an encoder 1.
5a and a tachometer 15b.

Subsequently, the force detection section 16 has a force sensor 16a and a coordinate conversion section 16b from the hand coordinate system to the robot reference coordinate system.

Furthermore, the force control means 14 and the position control means 13
Adding unit 10b that performs addition of the speeds output from
and an inverse Jacobi transform unit 10a that converts the added velocity into angular velocity θ of each joint of the manipulator.

The force control means 14 consists of a force control section 14b and a deviation section 14a, and the position control means 13 consists of a deviation section 13a and a position control section 1.
3b.

Next, details of the position control section 13b and force control section 14b are shown in FIG. 3.

The position control unit 13b is a transposed orthogonal transformation matrix (R”) calculation unit 3.
1, the selection matrix (I-3f) calculation unit 32, and the orthogonal matrix (I-3f)
R) Arithmetic unit 33 and position feedback gain (C2)
It has an arithmetic unit 34.

The force control unit 14b also includes a transposed orthogonal matrix (R”) calculation unit 3.
8, the selection matrix (Sf) calculation unit 37, and the other selection matrix (Sf)
I-3f) Calculation unit 40, dead zone calculation unit 39, orthogonal matrix calculation unit 36, and position feedback gain calculation unit 35
and has.

Here, as shown in FIG. 6, force command F is used to press the object. The unit vector in the force control direction given by (
The normal vector of the surface of the object on which the tracing operation is performed) is “
1nII, the unit vector in the position control direction given by the movement speed command Vo in the scanning direction is expressed as 0p, yet another orthogonal vector "a"° to be position controlled is a=nX.

is given by

When each of these is expressed as a component using the robot reference coordinate system, n=(nx+ny+nz)T O=(OXIOYIOZ)T. Here, T indicates a transposed matrix.

Using these vectors, the robot reference coordinate system (x
, y, z) to the tracing coordinate system (X'', y', z') is expressed as follows.

In Fig. 6, the tracing coordinate system (x' + 3" + z
), the X' force direction is the force control direction, and the y' and 2' directions are the position control directions.
．． The 40 selection matrices are expressed in the following equation.

is given by In this case, in general, pressing such as tightening a screw or tightening a lid of a can applies torque around the axis of the direction, so S=1 is set when torque is applied, and S=0 when no torque is applied.

The feedback gain calculation unit cf is given by the following with respect to the reference coordinate system.

Similarly, the position feedback gain Cp is is given by

1 in the selection matrix represents a unit matrix.

Furthermore, in this embodiment, a dead zone calculation section 39 is provided.

The calculation unit 39 detects that when the position control direction does not match the tangential plane at the point of contact with the object during the scanning operation, a large frictional force is generated on the surface of the object and scratches the surface, causing damage to the surface of the object or the workpiece. It is something that can be prevented from being given. That is, when a force exceeding an allowable range is applied to the contact point of the object, force feedback is also applied in the position control direction.

Next, the operation of this embodiment will be explained. Now, a case will be described in which tracing teaching control is performed on a plane S2 placed as shown in FIGS. 5 and 7, for example.

As shown in FIG. 7, prior to the copying operation, first, the current appropriate position Z of the tip of the hand portion 11a of the manipulator 11 is stored in the position storage section 17. Next, the control direction calculating unit 18 and the direction determining unit 19 calculate a set trajectory plane SK including the starting point SA, ending point SB, and appropriate position Z of the tracing motion trajectory, and set the contact positions A and B.
, C and its direction are calculated and determined. Then, the tip of the hand portion 11a is moved based on the calculation and determination result. During this movement, only force and speed commands are given to the operating section 12, and no position speed commands are given to the operating section 12. In this case, first, the tip of the hand portion 11a is moved along the set trajectory plane SK and brought into contact with the surface of the object, and is automatically stopped when the set force is reached.

By performing this operation twice, the contact positions A and B are detected and stored. Next, the tip of the hand portion 11a is moved so that it is not on the set trajectory plane SK, and brought into contact with the surface of the object. This contact position C is detected and stored.

That is, when the hand part 11a contacts point A, the force sensor 16a detects the force received at the time of contact as a component Fh of the hand coordinate system, and sends it to the coordinate conversion part 16b to convert the force component to the robot reference. Convert to component Fr expressed in a coordinate system.

The said Fr is the force command F by the deviation part 14a. Which deviation is taken and sent to the force controller 14b.

The position and orientation of the hand section 11a when it is stopped are determined by the joint angles θ1 detected by the encoder and counter 15a.
It is calculated by the position and orientation calculation unit 20 from .

The thus obtained position at the moment of contact is stored in the position storage section 17. Then, when the three position coordinates of the contact positions A, B, and C are obtained, the position coordinates are sent to the control direction calculation section 18 and the direction determination section 19. □ The control direction calculation unit 18 automatically obtains a plane equation determined by the three points as follows.

That is, as shown in FIG. 5, the three points A.

The position coordinates of B and C are respectively a=(al+a2+a3),
b = (b engineering, b2.b3), C = (C1+C2+C3)
When detected as follows, C'' (C1+C2+C3) is determined.

As a result, the plane S2 plane equation passing through three points a, b, and c p=ra+sb+sc (where r+S+t=1)
is required.

Furthermore, the vertical unit vector of plane S2 will be calculated.

Furthermore, a unit vector along the trajectory on the plane S2 is calculated.

Next, the direction determining section 19 first determines the direction of the unit vector along the trajectory on the plane S2 from the positional relationship between the start point SA and the end point SB.

Furthermore, in the direction determination unit 19, the position coordinates 2 of the contact position mZ stored in the position storage unit 17 are calculated as follows:
3), this contact position Z is expressed as a coordinate system formed by unit vectors o, n, a (a=n In this way, the direction of the vertical unit vector n of the plane S2 is determined.

Next, during the scanning operation, the orthogonal transformation matrix R and the transposed orthogonal transformation matrix RT are obtained from equation 0, and the thus obtained R and R'' are the (transposed ) Orthogonal transformation matrix calculation unit 33, 31, 3
6.38, and a tracing operation is performed along the surface of the object.

That is, the speed command V output from the force control section 14b,
Vf = cfR3fRT (Fo - Fr) based on the equations ■, ■, ■ and the deviation between the output F and the force command Fo.
In addition, the speed command vp output from the position control unit 13b is determined by the equations ■, ■, ■ and the deviation between the current position X, , and the target position Xo.
■It is given by.

In this case, the selection matrix calculation unit 40 and the dead zone calculation unit 39 are provided in the force control unit 14b in order to also monitor the force acting in the position control direction. These means can prevent damage to the object surface or workpiece when the position control direction does not match the tangential plane at the object contact point during the tracing operation. That is, when a force exceeding an allowable range is applied to the contact point of the object, force feedback is also applied in the position control direction.

In FIG. 3, the monitoring force Fcr as a force acting in the position control direction is given by For・(I-3f)RT(Fo-Fr),
In the dead zone calculation unit 39 where the force tolerance range U,,00) expressed in the scanning coordinate system is set to be in the dead zone, when IFC,l≦Ur, that is, when the monitoring force is smaller than the force tolerance range, the formula A speed command V is obtained by adding ■ and
It is given by RT (Fo-Fr) + Fora).

In this case, Fcra is Fcra-Fcr-0f (Fcr〉0)4cr"U depending on the allowable force range and the magnitude of the monitoring force.
It is given by f (F,,r(0)).Of course, the speed command Vp in the position control section 13b may be expressed by equation (2).

In this way, the obtained speed command V is
At 0a, the joint velocity θ is determined based on θ=J−1·V, and this joint velocity is input to the compensator 12d, subjected to D/A conversion, and then amplified to drive the servo motor 12a.

Incidentally, as shown in FIG. 8, the surface of the object is an arbitrary curved surface S3.
In the case of , the minute curved surface 83 obtained by dividing the curved surface S3 is
It is preferable to take out 83' and perform the same operation as that for the arbitrary plane S2 described above to match it with other divided minute curved surfaces, thereby teaching the entire curved surface S3. In other words, the minute curved surface 83' of S3 is approximately regarded as a plane, and the force control direction and position control direction are obtained in the same manner as in the case of the plane described above, and points A, B,
The robot is taught to perform a copying motion on a plane determined by C.

In reality, since the surface S3T is a curved surface, the force applied to the surface S3° varies depending on the difference between the plane determined by points A, B, and C and the surface S3°. Therefore, even if uneven force occurs, the pressing force is instructed so as to always maintain contact with the surface S3T.

By performing similar operations on other minute curved surfaces and connecting each force control direction and position control direction, the entire curved surface S3 can be traced.

〔Effect of the invention〕

As explained above, according to the present invention, when performing copying teaching, it is possible to automatically obtain the force control direction and the position control direction perpendicular to the surface on which the copying operation is performed, and also to obtain the force control direction and the position control direction. Since it is possible to obtain the direction of the direction,
It is no longer necessary for the operator to investigate the plane equation of the surface of the object to be scanned, the normal vector of the plane, etc., and then teach the control system how to scan the object. When performing a tracing operation on an object having a surface, there is no need to perform complicated measurements, unlike in the case of a simple plane, and work efficiency can be improved.

[Brief explanation of the drawing]

Fig. 1 is a block diagram of the principle of the present invention, Fig. 2 is a block diagram of an embodiment, Fig. 3 is a diagram showing the position control section and force control section of the embodiment, and Fig. 4 shows the locus of the copying operation. FIG. 5 is a diagram showing the plane on which the copying operation is performed, FIG. 6 is a diagram explaining the unit vector in the robot reference coordinate system, FIG. FIG. 9 is a diagram showing a robot reference coordinate system and a hand coordinate system, and FIG. 10 is a block diagram of a conventional example. 1.11... Manipulator (controlled object) 3.13.
...Position control means 4.14...Force control means 5.15...Position detection section 6.16...Force detection section 7.17...Position storage section 8.18...Control direction calculation Part 9.19...Orientation determining part A, B, C...Contact position 2...Appropriate position Copying operation #L line 1 Small 1 Figure 4 Fig. 2 □ Copying ~ I operation Yuzu fJ wash 1+H

Claims (1)

[Claims] Controlled object (1) that performs a tracing operation along the surface of the object
, an operation unit (2) that operates the controlled object (1) based on a speed command, a position detection unit (5) that detects the position of the controlled object (1), and a force that the controlled object (1) receives. In the scanning teaching control system that teaches the speed command using a force detection unit (6) that detects a position storage unit (7) for storing an appropriate position of the controlled object (1) located other than above and three or more contact positions of the controlled object brought into contact with the surface; and a copying operation using the stored contact positions as elements. a control direction calculation unit (8) that calculates a force control direction and a position control direction perpendicular to the force control direction with respect to the surface of the controlled object (1) at the time; a direction determination unit (9) that determines the direction of the force control direction using the position and the contact position as elements; and a direction determination unit (9) that determines the direction of the force control direction using the position and the contact position as elements; and a force control means (4) that issues a speed command in the force control direction based on the set force.
), and a position control means () that issues a speed command in the position control direction based on the position control direction, the direction of the position control direction, the position detected by the position detection section (5), and the set position during the copying operation. 3) A tracing teaching control system characterized by comprising: