JPH01197811A - Profile teaching control system - Google Patents

Abstract

PURPOSE:To automatically obtain the normal direction of the surface of executing a profile operation and the moving direction being vertical to said direction by executing a speed command in the normal direction of the plane based on the calculated control direction, detected force and set force. CONSTITUTION:A control direction calculating part 9 recognizes a plane which a contact point or its adjacent point passes through based on three contact points, calculates the control direction of a control object 1, and sends it out to a position control means 3 and a force control means 4. The position control means 3 executes a speed command in the direction running along the plane of the control object 1 based on the control direction and the present position which has been detected by a position detecting part 5 and the set position. Also, the force control means 4 executes a speed command to the normal direction of the surface based on the present received force which has been detected by a force detecting part 6, the set force and the control direction which has been calculated by a control direction calculating part 9. In such a way, even with respect to the surface having an arbitrary position or shape, a profile operation can be executed easily and with high reliability.

Description

[Detailed Description of the Invention] [Table of Contents] Overview Industrial Field of Application Prior Art Figures 4, 8, and 9 Problems to be Solved by the Invention Figure 5 Means for Solving the Problems Figure 1 Effects Figure 1 Embodiment Figures 2 to 8 Effects of the invention (outline) A controlled object, an operation system for operating the controlled object,
- When the control object is made to perform a tracing motion along the surface of the object in a system that has a position detection section that detects the 0 position of the control object and a force detector that detects the force that the control object receives. Regarding the scanning teaching control method that teaches the surface of the object, when performing the scanning operation, the scanning teaching control method can automatically obtain the normal direction of the surface on which the scanning operation is performed and the direction of movement perpendicular to this. The control direction is calculated by recognizing a plane passing through or near the contact points based on the positions of three or more contact points where the control object is brought into contact with the surface of the object. a control direction calculation section, a position control means for issuing a speed command in the copying direction based on the calculated control direction, the position detected by the position detection section and the set position; This configuration includes a set force and a force control means for issuing a speed command in the normal direction of the plane based on the set force.

(Industrial Application Field) The present invention relates to a copying teaching control system for a copying operation in which a robot performs machining or other operations along the surface of an object.1. In particular, a system that includes a controlled object, an operation section that performs an operation on the controlled object, a position detection section that detects the position of the controlled object, and a force detection section that detects a force that the controlled object receives. The present invention relates to a tracing teaching control method for teaching the surface of an object when a measuring tool is caused to perform a tracing operation along the surface of the object.

(Prior art) Conventionally, processing along the surface of an 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 8, in order to control the copying operation on the plane S1, a manipulator 1 is used as a control object.
1 is installed in the robot reference coordinate system (xr, yr,
zr) and hand coordinate system (Xh, Yh, Zh)
The coordinate values of the start point A and the end point B of the copying operation in and,
It is necessary to indicate the direction of movement, the magnitude of the force applied to the plane to maintain contact with the plane, and the normal direction of the plane.

Conventionally, the moving direction and the normal direction of the plane are determined by using a simple plane such as a horizontally spread plane S, or by measuring the surface of the object and determining the normal vector of the surface in advance. I was trying to find out the vector in the direction of movement and give it to you.

That is, in the conventional tracing teaching control method.

As shown in FIG. 9, there is a manipulator 81 that performs an operation 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 position detection section 85 that detects the force that the manipulator 81 receives. It has a force detection section 86 that performs detection, a position control means 83 that commands the speed in the moving direction of the manipulator 81 based on the moving direction, and a force control means 84 that commands the speed in the normal direction of the surface. It is.

In this example, in order to perform a copying operation on the surface of a predetermined object, the position detected by the position detecting section 85 and the set position are sent to the position control means 85, and the moving direction on the plane in which the copying operation is performed is determined in advance. is set, and the force detected by the force detection section 86 and the setting force are sent to the force control means 84, and the normal direction of the plane on which the copying operation is performed, which has been checked in advance, is set. , the teaching of a copying motion on the surface of a predetermined object is controlled.

(Problems 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. It is necessary to teach motions based on the control QfD system.

Therefore, when performing a tracing operation on a plane arbitrarily placed within 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 perform a simple tracing operation. Unlike the case of a flat surface, it is necessary to perform complicated measurements, which has the problem of being time-consuming.

Therefore, the present invention has been made to solve the above problems, and when performing a copying operation, it is possible to automatically obtain the normal direction of the surface on which the copying operation is performed and the direction of movement perpendicular to this. This was done for the purpose of providing a copying teaching control system.

[Means for solving problems]

In order to solve the above problems, the present invention, as shown in FIG. A system including a position detection unit 5 that detects a position and a force detection unit 6 that detects a force applied to the controlled object 1,
In a tracing teaching control method in which teaching regarding the surface of the object is performed when the control object 1 performs a tracing operation along the surface of the object, three or more points on the surface of the object are brought into contact with the control object 1; a control direction calculation section 9 that calculates the control direction of the controlled object 1 by recognizing a plane passing through the contact point or its vicinity based on the position of the contact point; a position control means 3 that issues a speed command in the tracing direction along the plane based on the detected position and the set position; and a position control means 3 that issues a speed command in the tracing direction along the plane based on the calculated control direction, the detected force, and the set position; It has a force control means 4 for giving a speed command in the direction.

(Function) When the present invention causes the surface of an object to perform a motion that follows the controlled object 1, the operation section 2 sends out a predetermined operation amount to the controlled object 1 so that the surface of the object contact with.

When the controlled object 1 comes into contact with the object, the force detection section 6 detects the force applied to the controlled object 1, and when the force control means 4 recognizes the contact, the position detection section 5 detects the contact. The current position of the controlled object 1 is detected and the position coordinates are output. Repeat this motion at at least three different points on the surface you are trying to copy,
Obtain the position coordinates of at least three contact points.

Then, the control direction calculation unit 9 recognizes the plane through which the contact point or its neighboring point passes based on the three contact points, and determines the control direction of the control object 1 (the normal direction of the plane and the plane perpendicular to the direction). moving direction), and the position control means 3
and sends it to the force control means 4.

The position control means 3 issues a speed command in a direction along the plane of the controlled object 1 based on the control direction and the current position and set position detected by the position detection section 5, and the force control means 4 gives a speed command in the direction along the plane of the controlled object 1. A speed command in the normal direction of the surface is given based on the force currently being received by the force detection section 6, the setting force, and the control direction calculated by the control direction calculation section 9.

〔Example〕

Next, the present embodiment will be explained.

As shown in FIG. 2, this embodiment includes a manipulator 11 as a controlled object l, an operation section 12 for controlling the manipulator 11, a position detection section 15 for detecting the position of the manipulator 11, and a manipulator 11 for controlling the manipulator 11. a force detection section 16 that detects the force applied to the hand section 11a (shown in FIGS. 4 and 5); and a control direction calculation section that calculates the control direction based on the position detected by the position detection section 15. 19, the control direction calculated by the calculation unit 19,
a position control means 13 for instructing the speed of the manipulator 11 in the copying direction based on the position detected by the position detection section 15 and the set position; and the force detection section 16
a force control means 14 that issues a speed command in the normal direction to the manipulator 11 based on the force detected by the above, the set force, and the normal direction of the plane as the control direction calculated by the calculation unit 19; have.

The control direction calculation unit 19 includes a position and orientation calculation unit 19a that converts the coordinates displayed in the relative angle of each joint detected by the position detection unit 15 into coordinates of the robot reference system, and 3 that came into contact with the surface of the object
a position coordinate storage unit 19a that temporarily stores the position coordinates of the point;
It has a control direction calculation unit 19C that determines a plane equation based on the three contact points and calculates a control direction.

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 detecting section 15 has a counter/encoder 15a 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 (RT) calculation unit 3.
8, the selection matrix (Sf) calculation unit 37, and the other selection matrix (Sf)
1-3r) 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 “
When expressed as a unit vector "oII" in the position control direction given by n"' and a moving speed command V in the scanning direction, 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, it becomes n" (nx*nysnz)T O" (o×+0VsOZ)T. Here, T indicates a transposed matrix.

Using these vectors, the robot reference coordinate system (x
, y, z) to the tracing coordinate system ("*'J"*Z"), the orthogonal coordinate transformation matrix R is expressed as follows.

In Fig. 6, the tracing coordinate system (x'sy' *z
), the X′ force direction is the force control direction, and the y′ and 2° directions are the position control directions, so that the selection matrix calculation unit 32°37.
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 c is given as follows with respect to the reference coordinate system.

In addition, the position feedback gain Cp is similarly is given by .

I of the selection matrix represents an identity 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.

The operation of this embodiment will be explained.

For example, a case where a copying operation is performed on a plane placed as shown in FIG. 5 will be described.

The hand portion 11a of the manipulator 11 is brought close to the surface of the object until it comes into contact with the surface. When the hand portion 11a contacts point A, the force sensor 16a converts the force received at the time of contact into the hand coordinate system (Xh, Yh,
Zh) is detected as the component Fh, and the coordinate transformation unit 16b
The force components are transferred to the robot reference coordinate system (Xr, Yr
, Zr).

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 portion 11a when brought into contact are determined by the joint angle θ6 detected by the encoder and counter 15a.
It is calculated by the position and orientation calculation unit 18 from .

The contact position thus obtained is stored in the position coordinate storage section 19a of the control direction calculation section 19.

Similarly, when the position coordinates of three points on the surface of the object are obtained, the position coordinates are sent to the control direction calculating section 19b.

The control direction calculating section 19b 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 a” (al + a2 + a3)
,b”(bl,b211)3),C=(C1+C21
When detected as shown in C3), the position vector a” of each point
(aXlas'1aZ), b=(bX.

by, b2) and "" (CX + CV + cz) are determined.

This gives the plane S2 plane equation that passes through the three points A, B, and C %) and the vertical unit vector n of the plane S2 = (a-b) x (a-c) / I (a-b)
a-c) l will be calculated.

Furthermore, the movement speed command V. By specifying the unit vector a in the position control direction given by
and the transposed orthogonal transformation matrix RT are obtained from equation 0.

The R and R" thus obtained are the position control unit 13a described above.
and (transposition) orthogonal transformation matrix calculation unit 33 of the force control unit 14b
, 31, 36, and 38, and a tracing operation is performed along the surface of the object.

That is, the speed command vf output from the force control section 14b
is V(=C(R8fR”(Fo −Fr)) using equations ■, ■, ■ and the deviation between output Fr and force command Fo.
"" ■Also, the speed command v2 output from the position control section 13b is calculated using the formulas ■, ■, ■, the current position Xr, and the target position X.
. By which deviation, Vp=cpR(1-3f)R" (xo-X-++ea
■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 Fct' as a force acting in the position control direction is given by Fc,... (1-3r)R" (Fo-Fr), and the allowable force expressed in the tracing coordinate system In the dead band calculation unit 39 which has set the range Ur00) as the dead band width, when 1Fci≦U1, that is, when the monitoring force is smaller than the allowable force range, a speed command V is obtained by adding 2〇 and ■, and IFc , l>U.

In other words, when the monitoring force is larger than the allowable force range, the speed command V of the force control section 14b is Vt = Cr R(
SrR''(Fo-Fr)''Fcra).

In this case, Fcra is determined by Fcra=Fcr-Ur (Fcr〉0)=Fcr"
It is given by Ur (Fcr(0)).Of course, the speed command VD in the position control section 13b may be expressed by equation (2).

In this way, the obtained speed command V is calculated by the inverse Jacobian matrix calculation unit 1
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. 7, the surface of the object is an arbitrary curved surface S3.
In the case of , the minute curved surface S3 obtained by dividing the curved surface S3 is
It is preferable to trace the entire curved surface 83 by taking out the angle S3 and performing the same operation as that for the above-described arbitrary plane S2 to match it with other divided minute curved surfaces. In other words, the minute curved surface S3 of S3
Approximately regarding the angle as a plane, the normal direction and movement direction are obtained in the same manner as in the case of the plane described above, and a tracing operation is performed on the plane determined by points A, B, and C.

In reality, since the surface S3' is a curved surface, the force applied to the surfaces 3 and f 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 331.

By performing similar operations on other minute curved surfaces and connecting their respective movement directions and normal directions, the entire curved surface 83 can be traced.

〔Effect of the invention〕

As explained above, in the present invention, when performing a copying operation,
The position coordinates of three or more points on the object surface are determined, and the plane of the object surface is automatically taught from the coordinates to perform the tracing operation, so it can be applied to any surface at any position or shape. It is also possible to easily and reliably perform a copying operation.

[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. Figure 5 is a diagram showing the plane on which the copying operation is performed, Figure 6 is a diagram explaining the unit vector in the robot reference coordinate system, Figure 7 is an explanatory diagram in the case of a curved surface, and Figure 8 is the robot reference FIG. 9, which is a diagram showing a coordinate system and a hand coordinate system, is a block diagram according to a conventional example. 1.11... Manipulator 3.13... Position control means 4.14... Force control means 5.15... Position detection section 6.16... Force detection section 9.19... Control Direction calculation part

Claims (1)

[Claims] A controlled object (1) that performs operations such as processing on a target object, and an operating section (2) that performs operations on the controlled object (1).
and a position detection unit (
5) and a force detection unit (6) that detects the force applied to the controlled object (1), when the controlled object (1) is caused to perform a tracing motion along the surface of the object. In a tracing teaching control method for teaching about the surface of an object, the control object (1) is brought into contact with the surface of the object (3).
a control direction calculation unit (9) that calculates the control direction of the controlled object (1) by recognizing a plane passing through the contact point or its vicinity based on the position of the contact point or more; the calculated control direction; a position control means (3) that issues a speed command in the tracing direction along the plane based on the position detected by the position detection section (5) and the setting position; and the calculated control direction, the detected force, and the setting. force control means (
4) A tracing teaching control system comprising: