JPS6333167B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a trajectory interpolation method for an automatic position control device that generates a continuous trajectory from a discrete point sequence.

A method of providing trajectory information for automatic position control devices such as NC machine tools and industrial robots is a method of providing a discrete point sequence on the trajectory (PTP).
Orbit information) There is a method (CP orbit information) that provides the orbit as continuous information.

Comparing these two, PTP orbit information is better. Γ The amount of information is much smaller. Easy to generate Γ orbit Partial correction or addition of Γ orbit is easy. Γ Easy speed setting It has many advantageous properties.

However, in order to actually generate a trajectory with an automatic position control device, it is ultimately necessary to give continuous position commands to the servo system.

Therefore, in order to generate a trajectory for the automatic position control device using PTP trajectory information, it is necessary to convert the PTP trajectory information to CP trajectory information. This conversion is called the trajectory interpolation method.

Conventionally well-known trajectory interpolation methods include Γ broken line interpolation, Γ circular interpolation, and Γ spline function interpolation. The content, features, and drawbacks of these trajectory interpolation methods will be briefly explained.

The broken line interpolation method interpolates between two points using a straight line, and is the simplest trajectory interpolation method.
In a trajectory obtained by this interpolation method, the position is continuous, but the first derivative (velocity) is generally discontinuous.

The circular interpolation method interpolates between three points using the same circular arc. Position, velocity, and acceleration are continuous on the same arc, but generally at the point of contact between two arcs, the position is continuous but the first derivative (velocity) is discontinuous.

Spline function interpolation interpolates between discrete point sequences using an n-th power function; in this case, (n-1)
Continuity up to the next differential coefficient is guaranteed. but,
Since spline function calculation requires information on the point sequence of the entire trajectory, the amount of calculation is enormous, and it is not a method suitable for online processing with ordinary automatic machines.

The present invention is a trajectory interpolation method suitable for performing so-called contour control in an automatic position control device, and the position and first-order differential coefficient (velocity) are continuous over the entire trajectory. Furthermore, since a trajectory can be generated from only three consecutive points and predetermined specified curve information, and the amount of calculation is not large, this method is suitable for online processing using ordinary automatic machines. A trajectory generated by the trajectory interpolation method of the present invention is composed of a portion of a straight line connecting two points and a portion of a predetermined designated curve. It is practical to configure a part of the trajectory using a predetermined designated curve because it allows consideration of the dynamic characteristics of the servo motor or mechanism. The present invention will be described below based on embodiments shown in the drawings. Figure 1 is a block diagram of trajectory generation in a two-dimensional plane, and shows three points from trajectory data.
When P _-1 , P, P ₊₁ are given, P _-1 ~P ~ _{P +1}
Interpolate the interval using a continuous trajectory. At this time, the orbit is generally divided into several orbits. However, the divided orbits can be expressed by a type of orbit information (formula representing the orbit).

Information regarding the divided trajectories is stored in the memory 1, and the divided trajectory information is sequentially taken out one by one from this memory and sent to the trajectory generator 2, which generates position commands to each servo system 3x, 3y. When all the trajectory information stored in the memory 1 is sent to the trajectory generator (2), new trajectory data is requested.

The divided trajectory information includes: Γ Orbit function form (straight line, arc, parabola, ellipse, spiral, exponential function, logarithmic function, etc.) Γ Start point Γ End point Γ Data accompanying the function form (center, focus, etc. other than the start and end points) (a quantity that specifies a functional form).

For two-dimensional problems, if you use a function generator like the one described in Japanese Patent Publication No. 53-22225,
Any function can be generated.

For three-dimensional problems, the following methods can be considered.

1 Direct generation of three-dimensional orbits.

2 Change the problem to a two-dimensional problem by coordinate transformation,
A trajectory is generated as a two-dimensional problem, and the generated trajectory is returned to a three-dimensional problem by inverse coordinate transformation.

The first method above can be realized by normal linear interpolation or circular interpolation if the trajectory is a straight line or circular arc.
For other functions, direct orbit generation is currently not possible. (Of course, it would be better if the trajectory could be generated directly.) Therefore, for general functions, the second method will be adopted. The flowchart is shown in Figure 2. In the same figure, the three-dimensional orthogonal coordinate system O-
When information about three points P _-1 , P, P ₊₁ is given among the orbit data in XYZ, the coordinates where the X' and Y' axes are on the plane defined by P _-1 , P, P ₊₁ System O′-
Determine X'Y', Z', and use the coordinate system O-X, Y,
Perform coordinate transformation with Z. Let the matrix of the coordinate transformation be A (a 4×4 matrix), and let the transformed new points be P _-1 ′, P ′, and P ₊₁ ′, respectively, and the following relationship holds true.

P _-1 ′=AP _-1 , P′=AP, P ₊₁ ′=AP _+1This coordinate system
In O′−X′Y′Z′, there are three points P _-1 ′ on the plane where Z′=0,
Since P′ and P ₊₁ ′ exist, the trajectory related to these points can be treated as two-dimensional trajectory data, and the position command can be obtained by the process shown in Figure 1 above.
x'(t) and y'(t) can be obtained. If we perform the inverse transformation A ^-1 (inverse matrix to A) of this position command, we can obtain the position command x(t), y in the O-XYZ coordinate system.
(t), z(t) can be obtained. That is, the following relationship holds true.

In this way, a three-dimensional problem can also be treated as a two-dimensional problem by using coordinate transformation and inverse transformation, so for the sake of simplicity, a two-dimensional problem will be explained below.

FIG. 3 shows the geometric relationship of the trajectory of the present invention generated by a circular arc of radius R and a straight line.

The orbit is constructed as follows.

P _-1 ~ Q _-1 : Line segment _{-1 -1} Q _-1 ~ S _-1 : Starting point Q _-1 , Ending point S _-1 , Center O ₁ Radius R
Arc S _-1 ~ _{S +1} : Starting point S _-1 , End point S ₊₁ , Center O ₂ Radius R
Arc S ₊₁ ~ Q ₊₁ : Start point S ₊₁ , end point Q ₊₁ , center O ₃ radius R
Arc Q ₊₁ ~ _{P +1} : Points Q _-1 , Q ₊₁ , necessary to define the line segment _{+1 +1} trajectory
S _-1 , S ₊₁ , O ₁ , O ₂ , and O ₃ are determined as follows.

For ease of explanation, a vector representation shown in FIG. 4 will be used. The position on the plane defined by the three points P _-1 , P, and P ₊₁ is expressed as a vector from the origin O.

P _-1 , P, P ₊₁ are P _-1 ―→, P→, P ₊₁ ― respectively.
→
It is expressed as Here, e: Unit vector parallel to line segment _-1 f: Unit vector orthogonal to e g: Unit vector parallel to line segment ₊₁ h: Unit vector i orthogonal to f: ∠P _-1 PP ₊₁ (≦180゜) Parallel to the bisector and P _-1
Assuming that it is a unit vector directed to the bisecting point of P ₊₁ , e~i is given as follows. Here, |a| represents the absolute value of an arbitrary vector a, that is, the length.

e=(P _-1 -→-P→)/‖P _-1 -→-P→‖ g=(P ₊₁ -→-P→)/‖P ₊₁ -P→‖ i=(e+g)/‖ e+g‖ When the above e and g are expressed as components e=(e ₁ , e ₂ ), g=(g ₁ , g ₂ ) f=(e ₂ , −e ₁ ) g=(g ₂ , −g ₁ ) becomes. ...(1) Equation P _-1 -→, P→, P ₊₁ -→ and unit vector e,
f,
A point on the orbit is expressed as follows using g, h, and i.

Q _-1 -→=P→+R(cosθ+2sinφ)・e O ₁ —→=Q ₋₁ —→+R・f O ₂ —→=P→+R・i S ₋₁ −→=1/2(O→ ₁ +O ₂ -→) Q ₊₁ -→=P→+R (cosθ+2sinφ)・g O ₃ —→=Q ₊₁ —→+R・h S ₊₁ —→=1/2 (O ₂ —→+O ₃ —→) However, θ =1/2∠P _-1 PP ₊₁ (≦90°), φ=cos ^-1 {1/2 (1+sinθ)} (＜90°) ...Equation (2) Therefore, the trajectory generation in the above example is The flowchart is shown in Figure 5. In other words, three points P _-1 , P, P ₊₁ giving trajectory information and velocities V _-1 , V ₊₁ (V _-1 is the velocity from P _-1 → P, V ₊₁ is P
→P ₊₁ velocity) is given, the unit vectors e, f, g, h, and i are calculated using the above equation (1), and θ and φ are also calculated.

Points defining the orbit Q _-1 , O ₁ , O ₂ , S _-1 , Q ₊₁ ,
O ₃ and S ₊₁ are determined in this order by vector calculation using equation (2) above.

The trajectory between P _-1 and Q _-1 is the velocity V ₁ on the line segment _{-1 -1}
occurs as a straight trajectory.

The trajectory between Q _-1 and S _-1 is generated as an arcuate trajectory with a starting point Q _-1 , an ending point S _-1 , a center O ₁ , and a velocity V ₁ .

The trajectory between S _-1 and P is the starting point S _-1 , the ending point P, and the center
It occurs as an arcuate trajectory with O ₂ and velocity V ₁ .

The trajectory between P and S ₊₁ is the starting point P, the ending point S ₊₁ , and the center.
It occurs as an arcuate trajectory with O ₂ and velocity V ₂ .

A trajectory between S ₊₁ and Q ₊₁ is generated as a circular arc trajectory with a starting point S ₊₁ , an ending point Q ₊₁ , a center O ₃ , and a velocity V ₂ .

The trajectory between Q ₊₁ and P ₊₁ is the velocity V ₂ on the line segment _{+1 +1}
occurs as a straight trajectory.

In this way, continuous trajectories up to speed can be generated by straight lines and circular arcs. In general,
The position command given to the automatic position control device is three or more points, and the sequence of these points is Pi (i=1, 2, 3...
…, n), three consecutive points Pi _-1 , Pi, Pi ₊₁
i is incremented when the servo system passes the midpoint of line segment ₊₁ , and the updated Pi _-1
The midpoint of the line segment connecting Pi and Pi is P _-1 , Pi is P, Pi and Pi ₊₁
Data for the next trajectory interpolation is calculated from three points with the midpoint of P ₊₁ , and a new trajectory is generated.

In addition, although the above-mentioned example shows an interpolation trajectory composed of straight lines and circular arcs, the curve may be a quadratic or higher-order function such as a parabola, ellipse, spiral, exponential function, or logarithmic function. can do. For example, when a parabola is used, the velocity is a linear straight line and the acceleration is a step function, and in the case of a cubic function, the acceleration is continuous. Furthermore, when an exponential function is used, it becomes continuous up to higher-order differential coefficients.

If these trajectory interpolations or trajectory generation are less than the quantization unit of a digital device such as a digital arithmetic device or a digital servo system, the trajectory becomes discontinuous, but from a macroscopic point of view it is continuous, and in this invention, such quantization Discontinuities of less than a unit are also treated as continuity.

As described above, in the present invention, trajectory information consisting of a discrete point sequence and velocity data associated with each point is given in advance, and based on this trajectory information, each specified point is In an automatic position control device that has multiple servo axes and generates a continuous trajectory that passes through points, when three arbitrary consecutive points P _-1 , P, P ₊₁ are given from trajectory information, three points It passes through P _-1 , P, P ₊₁ in this order, and is on the plane defined by the three points P _-1 , P, P ₊ 1.It is smooth between P _-1 and P ₊₁ (at least the first-order differential coefficient (speed) is continuous) and P _-1 for any point Q _-1 between line segment _-1
The path from Q _-1 to Q -1 is line segment _{-1 -1} , and the path from Q _-1 to P is 2 because the center of curvature of this trajectory in the vicinity of Q _-1 is 2 points P _-1 and a straight line passing through P. The point P ₊₁ is in the area where it does not exist, and the center of curvature of this trajectory in the vicinity of P is in the area where ∠P _-1 PP ₊₁ <180°, and furthermore, from Q _-1 It has only one inflection point between P, and for any point between line segment ₊₁ , the path from P to Q ₊₁ is near P and the center of curvature of this trajectory is ∠P _-1 PP ₊₁ <180°,
In the vicinity of Q ₊₁ , the center of curvature of this orbit is two points P,
A point in the area bisected by a straight line passing through P ₊₁
is in the region where P _-1 does not exist, and further from P
It is a continuous trajectory that has only one inflection point between Q ₊₁ , and the path from Q ₊₁ to P ₊₁ is a line segment _{+1 +1} .
Since this is a trajectory interpolation method for an automatic position control device that is characterized by interpolating between points, it has the following effects.

A trajectory interpolation method suitable for contour control.
The position and first derivative (velocity) are continuous over the entire trajectory.

Since a trajectory can be generated from only three consecutive points and predetermined specified curve information, and the amount of calculation is not large, this method is suitable for online processing using ordinary automatic machines.

The trajectory generated by the trajectory interpolation method of the present invention is composed of a part of a straight line connecting two points and a part of a predetermined specified curve, so it can be specified appropriately by considering the dynamic characteristics of the servo motor or mechanism. A curve can be selected.

[Brief explanation of the drawing]

Fig. 1 is a block diagram showing the configuration of trajectory generation on a two-dimensional plane according to the present invention, Fig. 2 is a flowchart showing a method for processing data in a three-dimensional coordinate system as a two-dimensional problem, and Fig. 3 is a flow chart showing a method for processing data in a three-dimensional coordinate system as a two-dimensional problem. The figure is an explanatory diagram showing the geometric relationship of trajectories generated by straight lines and circular arcs,
FIG. 4 is an explanatory diagram of the vector display used in the embodiment of the present invention, and FIG. 5 is a flowchart showing the trajectory generation process.

Claims (1)

[Claims] 1 Trajectory information consisting of a discrete point sequence and velocity data associated with each point is given in advance, and each point designated at a designated velocity is given in advance based on this trajectory information. In an automatic position control device with multiple servo axes that generates a continuous trajectory that passes through the
A trajectory interpolation method for an automatic position control device, characterized in that when P _-1 , P, and P ₊₁ are given, interpolation is performed between the three points in a continuous trajectory as shown by the following conditions. It passes through the three points P _-1 , P, P ₊₁ in this order. It is on the plane defined by the three points P _-1 , P, P ₊₁ and is smooth between P _-1 and P ₊₁ (at least of first order). Continuous up to the differential coefficient) _{, and P -1 for} any point Q -1 between line segment _-1
The path from Q _-1 to Q -1 is line segment _{-1 -1} , and the path from Q _-1 to P is 2 because the center of curvature of this trajectory in the vicinity of Q _-1 is 2 points P _-1 and a straight line passing through P. The point P ₊₁ is in the area where it does not exist, and the center of curvature of this trajectory in the vicinity of P is in the area where ∠P _-1 PP ₊₁ <180°, and furthermore, from Q _-1 It has only one inflection point between P, and for any point between line segment ₊₁ , the path from P to Q ₊₁ is near P and the center of curvature of this trajectory is ∠P _-1 PP ₊₁ <180°,
In the vicinity of Q ₊₁ , the center of curvature of this orbit is two points P,
A point in the area bisected by a straight line passing through P ₊₁
is in the region where P _-1 does not exist, and further from P
It has only one inflection point between Q ₊₁ , and the path from Q ₊₁ to P ₊₁ is line segment _{+1 +1} . 2 Three discrete points Pi _-1 , Pi, Pi ₊₁ (i=1, 2,
3,...) is given as trajectory data while sequentially incrementing i, a patent claim that the midpoint of line segment _-1 is P _-1 , Pi is P, and the midpoint of line segment ₊₁ is P ₊₁ . A trajectory interpolation method for the automatic position control device according to item 1.