JPH0756606B2 - Method of teaching a moving body route - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention moves a moving body, sequentially stores points on a path traveled by the moving body, and then sequentially reads out this memory,
The present invention relates to a method of teaching a route of a moving body that moves the moving body based on this memory.

In the present invention, an industrial robot or a certain type of machine tool can be considered as a moving body, but in the following description, the moving body will be described as an industrial robot.

2. Description of the Related Art A teaching method for an industrial robot that has been conventionally used is such that each time a path control target portion of the industrial robot moves for a certain distance or more,
Either that point is stored, or the position of the route control target portion is stored at regular time intervals, and during reproduction, a straight line is reproduced between these stored points.

For this reason, the reproduction accuracy is deteriorated unless the interval between the teaching points is made very small. Therefore, in order to prevent the deterioration of the reproduction accuracy, a large number of path position data are required and a large capacity storage device is required.

Further, when the teaching path is bent sharply, the robot arm vibrates near the bent portion during playback after teaching.

On the other hand, as an attempt to solve the problem of storage capacity, Japanese Patent Laid-Open No.
-176705 is available. In the disclosed technique, a distance l from the latest newest point P _n-1 stored in the temporary storage means to the industrial robot current position P is calculated, and when the distance 1 reaches a predetermined pitch l, stores the point P _n in the temporary storage means, said point P _n-1 of the previous and P _n-2 points stored in the point P _n-1 of a line connecting the point P _n-1 and the point the angle a _n of the line connecting the P _n stored in said temporary storing means as data constituting the point P _n and a counter, sequentially oldest corner a ₁ and the one of the temporary stored the angle a _n When the difference from the angle a _n becomes larger than the predetermined allowable difference Δa, the oldest point P ₁ and the latest point P _{n and} the other one point P _m between these two points, a total of 3 points and the transfer position data in the storage means, while stored as position data circular interpolation during reproduction, the data of the latest point P _n and the angular a _n in the temporary storage means as an initial value P _1, a ₁ However, the teaching is proceeded by erasing all other data.By dividing the route into parts with a curvature within a certain range and circularly interpolating each of these divided routes, It aims to realize a small storage capacity even with a complicated shape having many shapes.

However, in the disclosed technique, the _n points from the oldest point P ₁ stored in the temporary storage means to the current newest point P _n−1, and the P _n point and the angles a ₁ to a _n paired with them. Must be stored in the temporary storage means, and if there is an arc of a considerable distance or a path portion with a small change in curvature in the path, the temporary storage means itself must have a large storage capacity. It is considered that the total storage capacity will not be significantly reduced. Further, in this disclosed technique, the route is approximated by several arcs, but it can be said that adjacent arcs may not be smoothly connected to each other. Due to the dynamic characteristics of, there is a possibility that vibration or the like may occur in the route control target part.

Problems to be Solved by the Invention As described above, the conventional techniques do not substantially improve the saving of storage capacity. In addition, the route is divided into several parts by a certain method, and these parts are circularly interpolated. However, the directions of adjacent interpolated circular arcs are discontinuous at the connection points, and due to the dynamic characteristics of the robot, vibration etc. Will occur.

In view of the above problems, the present invention aims to reduce the storage capacity of teaching route data and to provide a route teaching method for a moving body in which the direction of the playback route is smooth.

Means for Solving the Problems In order to solve the above problems, a method for teaching a route of a moving body according to the present invention is a method of moving a moving body and sequentially storing points on a route along which the moving body moves. ∠P ₁ P _n-1 P _{n for} the three points P ₁ , P _n-1 and P _n on the teaching path sequentially stored in the storage means
Path lengths S and P ₁ from the initial position P ₁ to the current position P _n
When the straight line distance l from the point to P _n is calculated and a ∠P ₁ P _n-1 P _n is larger than a predetermined determination angle α, the length of the path from the P ₁ point to the P _n point S and the linear distance l between the two points are calculated. If the difference between the a-1 path length S and the linear distance l is larger than a predetermined determination length d, the point P ₁ is The data of P _n-1 point is stored in the storage means, and the temporary storage means stores a new initial position.
The data of the P ₁ point is left as it is, the data of the current position P _n is left as the data of the new P _n−1 point, and a-2 the difference between the path length S and the linear distance 1 is determined in advance. If the length is smaller than the length d, the data of the current position P _n point is left as the new P _n-1 point data together with the data of the P ₁ point in the temporary storage means, while P ₂ , P ₃ , ... P _n-1
If b ∠P ₁ P _n-1 P _n is smaller than the predetermined determination angle α without leaving the data of each point, the data of the P ₁ point is stored in the main storage means and P ₂ is stored in the temporary storage means. , P ₃ , ………, P _n -2 point data is not left, the bending point P _n-1 point data is left as new initial position P ₁ point data, and the current position P _n point leaving the data as data of a new P _n-1 point, data of P _n-1 points left as data of the new initial position P ₁ point is not stored in the storage unit, then the new path data P _n points are sampled, the above determination is repeated for newly set three points P ₁ , P _n-1 , P _n to store the route data in the main storage means, and the stored route data are stored in higher order. The position data is interpolated by the interpolation method of.

Operation The present invention uses the above method to start from point P ₁ when there is no bend in the teaching path and ∠P ₁ P _n-1 P _n > α.
P ₂ , P ₃ ………, P _{n In the} process of sampling sequentially,
The difference between the length S of the arc along the path between the points P ₁ and P _n and the length l of the chord connecting the points P ₁ and P _n with a straight line is larger than the judgment length d, that is, S− When l> d and the curvature of the path becomes relatively large, P sampled immediately before P ₁ point to P _n-1 point
storage in the _n-2 points to the (n-2) number of points of the storage unit 29 is only one point of _point P.

That is, while S-l <d, the curvature of the path is relatively small and is close to a straight line, so that each angle in the sampling stage ∠P ₁ P ₂ P ₃ , ∠P ₁ P ₃ P ₄ , ∠P ₁ P ₄ P ₅ , ……… ， ∠P ₁ P _n-2 P
The data at each point of P ₂ , P ₃ , P ₄ , ..., P _n-2 located at the top of _n-1 are sequentially provisional storage means 30 until the comparison of the sizes of S-1 and d is completed. it is necessary to store in, S-l and d result both comparing the, S-l <while the state of d persists, the P _2, P this state each time to be confirmed as a result of sampling The data of each point of ₃ , P ₄ , ..., P _n-2 does not need to remain in the temporary storage means 30, and is sequentially erased from the temporary storage means 30.

Once the above-mentioned state is changed to a state where the curvature of the path is relatively large, that is, S-1> d, only the P ₁ point is stored in the main storage means 29 as the point to be reproduced, and the current position P _n point. Data as new P _n-1 point data, and
Temporary storage means 3 with _n-1 point data as new P ₁ point data
You can leave it at 0.

If there is a bend in the teaching path and ∠P ₁ P _n-1 P _n ≤α, (n-3) points from point P _{2 to} point P _n-2 are not stored at all. It is possible to solve the problem of storage capacity, which is the biggest drawback in the route teaching.

Furthermore, if the teaching path bends sharply, ∠P ₁ P _n-1
The path data near the bend is not stored in the storage means based on the determination of P _n , and a high-order interpolation method is used to interpolate the storage points using a high-order function curve so that the paths are smoothly connected. Vibration does not occur even at the bending point during playback, and smooth operation can be performed.

Embodiment One embodiment of the present invention will be described below with reference to the accompanying drawings.

In FIG. 2, R is the main body of the industrial robot,
Is a base and 2 is a rotary table. Teeth 3 are provided around the rotary table 2. On the other hand, the electric motor 4 is fixed to the base 1, and the pinion gear 5 is directly connected to this. Then, the pinion gear 5 and the tooth 3 are meshed with each other so that the rotary table 2 can turn about the vertical turning axis in the θ direction indicated by the arrow. Pin the first arm 6 to the rotary table 2 (not shown)
To pivot at. The head 8 of the hydraulic actuator 7 is pin 9
Is pivotally attached to the rotary table 2, and the rod 10 is pivotally attached to the first arm 6 by the pin 11. Then, the hydraulic actuator 7 allows the first arm 6 to swing in the φ direction indicated by the arrow. The second arm 12 is pivotally attached to the first arm 6 with a pin (not shown). The head 14 of the hydraulic actuator 13 is pivotally attached to the first arm 6 by the pin 15, and the rod 16 is pivotally attached to the second arm 12 by the pin 17. And the hydraulic actuator 13
Thus, the second arm 12 can be turned in the arrow ψ direction. A first swing motor 18 having a horizontal swing axis and swingable in the arrow γ direction is provided at the tip of the second arm 12. Second to the first swing motor 18
Mount the swing motor 19. The second rocking motor 19 has a rocking axis that is at a right angle to the rocking axis of the first rocking motor 18, and can rock in the direction of arrow β. The second swing motor 19 is provided with a pair of fingers 20, and the fingers 20 hold the sealant application gun 21.

Position detecting means 22, 23, 24, 25 and 26, which are also shown in FIG. 3, are provided, and the attitude θ of the rotary table 2 with respect to the base 1, the attitude φ of the first arm 6 with respect to the rotary table 2, and the attitude φ with respect to the first arm 6. The posture ψ of the second arm 12, the posture γ of the first swing motor 18 with respect to the second arm 12, and the posture β of the second swing motor 19 with respect to the first swing motor 18 are detected.

In FIG. 3, 27 is a judgment angle value setting means, 28 is a judgment length value setting means, 29 is a main storage means, 30 is a temporary storage means, and 31 is a calculation processing means such as a microcomputer. The main storage means 29 stores a specific point on the path, which will be described later, as position data for interpolation by the Akishima method in the present embodiment during reproduction.

Here, in one of the high-order interpolation method from the method of Akima, between teaching points connected by cubic curve y = f (x) = ax 3 + bx 2 + cx + d, as the slope of the tangent is equal in joint It is an interpolation method that can be connected smoothly and smoothly (see "Numerical Calculation Method" by Hideyo Nagashima, published by Maki Shoten).

The arithmetic processing means 31 shows its processing contents in FIG. 1, but the present position P _{n of the} coating gun 21 is received in response to the outputs of the position detecting means 22 to 26.
Is calculated. The position data of the initial position P ₁ stored in the temporary storage means 30 is read out from the temporary storage means 30, and as shown in FIG. 5, the linear distance l between the point P ₁ and the current position P _n is read.
Is calculated. Here, the current position P _n point is the _n-th point counted from the point P ₁ .
This is the second sampling point, and the path data is sampled every time the coating gun 21 moves a certain distance or more. present location
The position data of the P _n-1 point sampled before the P _n point is read from the temporary storage means 30, and this point P _n-1 point and the current position P _n
The length S'of the path between the points is calculated, the length S of the path between the points P ₁ and P _n-1 stored in the temporary storage means 30 is read, and the length S of this path is read. The _sum of the path length S ′ between the points P _n−1 and P _n is added to the temporary storage means 30 as the path length between the points P ₁ and P _n. To update. In addition, ∠P ₁ P _n-1 P _n is calculated from the position data of P ₁ , P _n-1 and P _n points stored in the temporary storage means 30, and ∠P ₁ P _n-1 P _n is determined. When it is larger than the value of the determination angle α obtained from the angle value setting means 27, the following process (I) is performed.

(I) The difference between the length S of the path from the point P _{1 to the} point Pn obtained above and the straight line distance 1 is obtained and compared with the value of the judgment length d obtained from the judgment length value setting means 28. The difference between the path length S and the straight line distance 1 is larger than the determination length d, that is, S-1> d.
When the curvature of the path becomes relatively large and it is not possible to treat this point P _n as not to be stored like the sampling points until then, it is stored in the temporary storage means 30 and the sampling is performed. A judgment flag indicating whether or not the point is a bending point is read out, and if the judgment flag is 0 indicating that the P ₁ point is not a bending point, the route data of the P ₁ point is written in the main storage means 29, and the judgment flag is If 1 indicates that point P ₁ is a bending point, the route data for point P _{1 is} not stored. Then, the length S of the route stored in the temporary storage means 30
To the distance S ′ between the P _n-1 point and the P _n point, leaving the data at the P _n-1 point as the data at the initial position P ₁ point and the data at the current position P _n point at the P _n-1 point data. Then, the determination flag of the temporary storage means 30 is reset to 0 indicating that the new point P ₁ is not the bending point.

Next, as shown in FIG. 6, the curvature of the path is relatively small,
As long as the difference between the path length S and the straight line distance l is S-1 <d, which is smaller than the judgment length d, ∠P ₁ P ₂ P ₃ and ∠P ₁ P ₃ P
₄ , ∠P ₁ P ₄ P ₅ , ………, ∠P ₁ P _n-2 P _n-1 located at the top of P ₂ ,
While sequentially erasing the data of each of the (n-3) points of P ₃ , ..., P _n-2 from the temporary storage means 30, the data of the current position P _{n is} the data of P _n-1. Then, the sampling is continued and S-1> d is established, and when the curvature of the path becomes relatively large, the data at the point P ₁ is written in the main storage means 29 as described above.

As shown in FIG. 7, when ∠P ₁ P _n-1 P _n is smaller than the value of the determination angle α, the following process (II) is performed.

(II) The path length S stored in the temporary storage means 30 is set to P
rewriting the distance S 'between the _n-1 point and P _n points, leaving data P _n-1 point as data of the initial position P ₁ point, the data of the current position P _n point as data of P _n-1 point The determination flag of the temporary storage means 30 is set to _{1 and the} fact that the apex angle at the original point P _n-1 is smaller than α and is a bending point is stored.

Here, from P ₂ point, which is the bending point, to P _n-2 point (n−
3) Do not store the points.

Further, the arithmetic processing means 31 receives the outputs of the position detecting means 22 to 26 and newly calculates the current position P _n of the coating gun 21. Similarly, the storage of the route data in the main storage means 29 is repeated.

Now, from the values of the outputs θ, φ, ψ, γ, β of the position detecting means 22 to 26, the values X _n , Y _n , Z _n on the orthogonal coordinates of the tip of the coating gun 21 are obtained.
(Note that P _n (X _n , Y _n , Z _n )) is Can be found at. Conversely, the values on Cartesian coordinates X _n , Y _n ,
The _values of θ, φ, ψ, γ, β from Z _n are Can be found at. A is a matrix determined by the configuration of the industrial robot and is an inverse matrix of A ^-1 A. Also, the point
The angle θ formed by the straight line connecting P ₁ and the point P _n-1 and the line connecting the point P _{n-1 and the} point P _n is the value of each X, Y, Z of the three points P ₁ , P _n-1 and P _n. Can be obtained from

Note that the storage of the point _{P 1 (X, Y, Z} ), or (theta, phi,
The value of ψ, γ, β) is stored. The route data stored in this storage means is interpolated by the method of Akima at the time of reproduction, but in the method of Akima, there are four unknowns a, b, c, d in the cubic curve, so at least four interpolation nodes are required. The linear interpolation is used when the taught points are the start point and the end point, and the circular interpolation is used when the taught points are three points.

Next, a specific example of the teaching performed by this method will be shown.

4 (a) and 4 (b) are obtained by applying the route teaching method of the present invention to a teaching route having a right angle portion shown by a solid line.

FIG. 4 (a) is a diagram showing the sampling of each point in the vicinity of the corner portion of the teaching route, and the teaching route is shown by a solid line.

In FIG. 4 (a), the data at each point of P ₂ , ..., P ₆ are temporarily stored in the temporary storage means 30 until sampling and determination are made. Since the curvature is small between points and there is no large change in the path, the data of each point is erased from the temporary storage means 30 every time the determination is completed.

Then, at the sampling point P ₈ , ∠P ₁ P ₇ P ₈ > α, and S−1 = Therefore, the P ₁ point is stored in the main storage means 29 as the adopted point.

Next, FIG. 4 (b) shows a case where the teaching is advanced. At this time, the teaching is advanced with the point P ₇ in FIG. 4 (a) as a new point P _{1 and the} point P _{8 as} point P _n-1 .

In FIG. 4 (b), the points of P ₂ , ..., P ₈ are temporarily stored in the temporary storage means 30 until a determination is made every time they are sampled.
However, since the curvature between the points is small and there is no large change in the path, the data of the points are erased from the temporary storage means 30 each time the determination is completed.

Then, at the sampling point P ₁₀ , it is determined that ∠P ₁ P ₉ P ₁₀ <α. At this time, the data of P ₁ point in FIG. 4B is stored in the main storage means 29, and the data of P _n-1 point, that is, the data of P ₉ point in this case is left as the data of the initial position P ₁ point,
The data of the current position P _n point, that is, P ₁₀ point is left as the data of P _n-1 point. In addition, the original P ₉ that became this new P ₁ point
Since the point is determined to be a bending point, the data of this new P ₁ point is not stored in the main storage means 29 even if the route teaching proceeds thereafter.

In this way, the route teaching is advanced, and the points finally remaining in the main storage means 29 are shown as adopted points in FIGS. 4 (a) and 4 (b). Then, the result of interpolating these points by the Akima method becomes as shown by the broken line in the figure, and it becomes possible to reproduce smoothly. In addition, the storage capacity can be reduced by erasing from the temporary storage means without employing many sampled points.

Also, many points sampled in the vicinity of a right-angled portion that is sharply bent by the determination using the determination angle α are not adopted. Therefore, if the adopted points remaining in the main storage means from the sampling points are interpolated by the method of Akima, it becomes possible to reproduce smoothly as shown by the broken line, and many sampled points are not adopted and By deleting from the storage means, the storage capacity can be reduced.

By the effect of the invention The present invention is more or storage from _a point P to _{P n-2 (n-2} ) of the number of points is one point ₁ point P, or from P ₁ point P
_Since the memory of (n-1) points up to _n-1 points is not stored at all, the problem of memory capacity, which is the biggest drawback in the route teaching, can be solved, and when the taught route bends sharply, Since the route data near the part is not stored in the main storage means, the playback route interpolated by the high-order interpolation method at the time of playback becomes smooth.

[Brief description of drawings]

FIG. 1 is a flow chart showing a route teaching method in an embodiment of the present invention, FIG. 2 is a perspective view of an industrial robot to which the embodiment of the present invention is applied, and FIG. 3 is a teaching in an embodiment of the present invention. FIG. 4 (a) is a block diagram showing the apparatus, FIG. 4 (a) is a diagram for explaining sampling of each point on the teaching path when obtaining a reproduction path by the teaching method of the present invention, and FIG. 4 (b) is the same figure (a). Sampling of each point in the vicinity of the corner portion of FIG. 4 is described, and a diagram showing the reproduction path obtained by the teaching method of the present invention in comparison with the teaching path is shown in FIG. FIG. 6 is a diagram showing a case where the teaching path is large and FIG. 6 is a diagram showing a case where the teaching path has no bending portion and a relatively small curvature, and FIG. 7 is a diagram showing a case where the teaching path has a bending portion. 22,23,24,25,26 ... Position detecting means, 27 ... Judgment angle value setting means, 28 ... Judgment length value setting means, 29 ... Main storage means, 30 ... Temporary storage means, 31 ... ... arithmetic processing means.

Front Page Continuation (72) Inventor Hitoshi Kubota 1006 Kadoma, Kadoma City, Osaka Prefecture Matsushita Electric Industrial Co., Ltd. (56) References JP 58-176705 (JP, A) JP 56-114685 (JP, JP, 114685) A) JP-A-51-151646 (JP, A)

Claims (1)

[Claims] 1. A method of moving a moving body to sequentially store points on a route along which the moving body moves, in which three points P ₁ , P _n-1 , P _n on a teaching route sequentially stored in a temporary storage means. About ∠P ₁ P
_n-1 P _n and the initial position P ₁ to the current position P _n point The path length S and the straight line distance l from P ₁ point to P _n point are calculated, and a ∠P ₁ P _n-1 P _{When n} is larger than a predetermined determination angle α, the difference between the length S of the path from the P ₁ point to the P _n point and the straight line distance l between these two points is calculated, and the a-1 path length is calculated. If the difference between the length S and the straight line distance 1 is larger than the predetermined determination length d, the point P ₁ is stored in the main storage means, and the temporary storage means stores the data at the point P _n-1 as a new initial value. position
The data of the P ₁ point is left as it is, the data of the current position P _n is left as the data of the new P _n−1 point, and a-2 the difference between the path length S and the linear distance 1 is determined in advance. If the length is smaller than the length d, the data of the current position P _n point is left as the new P _n-1 point data together with the data of the P ₁ point in the temporary storage means, while P ₂ , P ₃ , ... P _n-1
If b ∠P ₁ P _n-1 P _n is smaller than the predetermined determination angle α without leaving the data of each point, the data of the P ₁ point is stored in the main storage means and P ₂ is stored in the temporary storage means. , P ₃ , ………, P _n -2 point data is not left, the bending point P _n-1 point data is left as new initial position P ₁ point data, and the current position P _n point leaving the data as data of a new P _n-1 point, data of P _n-1 points left as data of the new initial position P ₁ point is not stored in the storage unit, then the new path data P _n points are sampled, the above determination is repeated for newly set three points P ₁ , P _n-1 , P _n to store the route data in the main storage means, and the stored route data are stored in higher order. A method for teaching a path of a moving body, characterized in that the position data is interpolated by the interpolation method described in 1.