JP3513989B2 - Command interpolation method for positioning control system - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control method for a positioning control system such as a robot or NC (numerical controller), and more particularly to a command interpolation method for realizing smooth acceleration / deceleration without delay or deterioration of accuracy. Regarding

[0002]

2. Description of the Related Art Industrial robots, NCs and the like are required to have complicated operations and have various uses. Therefore, these robots and NCs do not control the digital servo system for each axis by using a single arithmetic unit, but discretely issue position or speed commands based on given teaching point data or numerical control data. It is customary to provide a high-order arithmetic unit for outputting and a low-order arithmetic unit for controlling the digital servo system of each axis based on a command from the high-order arithmetic unit. Compared with the output interval of the command from the higher-order arithmetic unit, the sampling cycle for controlling the digital servo system of each axis in the lower-order arithmetic unit is significantly shorter, so to match the sampling cycle in the lower-order arithmetic unit, It is necessary to interpolate and level the discrete commands output by the higher-order arithmetic unit.
That is, it is necessary to determine an interpolation value for each sampling cycle in the lower-order arithmetic unit and give this interpolation value as a command to the lower-order arithmetic unit. The lower-order operation unit, based on the interpolation value, for example,
Calculates the pulse movement amount and issues a command to the digital servo system.

At this time, in order to perform more advanced positioning control, interpolation is performed so that the command waveform calculated by the higher-order arithmetic unit can be reproduced as much as possible while realizing smooth acceleration / deceleration for preventing vibration of the working point. Method is required. Conventionally, interpolation or leveling is performed by processing such as inputting a command output by a higher-order arithmetic unit to a low-pass filter, or obtaining a moving average for a command output by a higher-order arithmetic unit, and as a command for a lower-order arithmetic unit. I was there. Further, Japanese Laid-Open Patent Publication No. 64-7207 discloses a method of obtaining a cubic equation representing a spline curve that fits the position command from the position command output by the higher-order arithmetic unit, and performing interpolation based on the calculated equation. It is disclosed.

[0004]

However, in the conventional interpolation method using the low-pass filter and the moving average, a corresponding delay occurs in order to give a command for realizing smooth acceleration / deceleration to the lower-order arithmetic unit. Further, there is a problem that the deterioration of accuracy is unavoidable and the command to the lower-order arithmetic unit does not faithfully reflect the command output from the higher-order arithmetic unit. The method of obtaining the spline curve and performing the interpolation has a problem that the amount of calculation increases. The purpose of the present invention is to
Providing a command interpolation method for the positioning control system that can execute interpolation that realizes smooth acceleration / deceleration with a small amount of calculation for discrete position commands calculated by the host computer, without delay or deterioration in accuracy To do.

[0005]

A command interpolation method for a positioning control system according to the present invention has a sampling cycle shorter than the output interval of the position command from the higher-order computing unit that discretely calculates the position command and the higher-order computing unit. In a command interpolation method in a control system that has a servo device including a lower-order arithmetic unit that controls a digital servo system, and a corresponding digital servo system is commanded by the servo device to position each axis, The current position command is P ₀ , the time corresponding to the current position command P ₀ is T ₀ , the next position command from the higher-order arithmetic unit is P ₁ , the time corresponding to the next position command P ₁ is T ₁ , a speed command that was last paid out by the previous interpolation and v _0, the parameters determined by the position command P ₂ of after next from the next position command P ₁ and the upper level computing section as b, the time t ( However, three functions f _a (t) = v ₀ (t−T ₀ ) + P ₀ ; having T ₀ ≦ t ≦ T ₁ ) as an independent variable; f _b (t) = (P ₁ −P ₀ ) (t− T ₀ ) / (T ₁ −T ₀ ) + P
_{0; f c (t) =} b (t-T 1) + P 1; Defines a continuous T ₀ ≦ t ≦ T ₁ and T ₀ <t <T ₁
And a function that can be differentiated twice with respect to t is used as a weighting coefficient ,
Using the weighting factors, F (T ₀ ) = P ₀ and F (T ₁ ) =
The three functions f _a (t), f _b (t), and f _c (t) are combined so as to be P ₁ to obtain the function F (t), and the value of the function F (t) is set at the time t (however, The position command is T ₀ ≤t ≤T ₁ ).

In the present invention, the parameter b may be changed not only in the next and subsequent position commands from the higher-order arithmetic unit, but also in accordance with the information of the position commands in the subsequent commands. Further, when the present invention is applied to a digital servo system or the like that receives the pulse movement amount as an input, the value of the function F (t) is calculated for each sampling cycle in the lower-order arithmetic unit, and the previously calculated function F (t) is calculated. It is possible to give the difference between the value of the above-mentioned value and the value of the function F (t) obtained this time as the movement amount to the lower-order calculation unit.

[0007]

The function f _a (t), f _b (t), f _c (t) is obtained based on the command from the higher-order arithmetic unit from this time to at least the next time, and the twice differentiable function is used as the weighting coefficient. These functions f _a (t), f _b (t), and f _c (t) are combined, and interpolation is performed by the combined function. Therefore, with a small amount of calculation, there is no delay or deterioration in accuracy, and smooth operation is possible. Acceleration / deceleration can be realized.

[0008]

EXAMPLES Next, examples of the present invention will be described. This
Here, given teaching point data and numerical values
Depending on the control data, the higher-order arithmetic unit can change
Calculates and outputs the position command for the axis servo motor,
On the other hand, the lower-order arithmetic unit is configured to generate a pulse for each sampling period δt.
Digital servo control of the servo motor based on the amount of movement
Shall be. Here, assuming that T> δt, each sample
The interpolated position command value (interpolation value) is
Calculated by the command interpolation method based on the invention,
The pulse movement amount of this time by the difference between the inter-val value and the interpolation value of this time
Shall be calculated. Time T output by the host processor
₀, T₁(= T₀+ T), T₂(= T₁+ T), T₃(= T₂+ T), ...
Position command for each₀, P₁, P₂, P₃, ...
It On the other hand, the time output by the lower-order arithmetic unit is t₀(= T₀), t₁
(= T₀+ Δt), t₂(= T₁+ Δt), ...
Position command for each sampling₀, x₁, x₂,…When
To do. In the present invention, the time T₀And T₁Of position commands between
Go between x₀, x₁, x₂, When seeking time T₀
The speed command issued by interpolation immediately before₀When
Position command P₁And P₂B is the parameter determined by
And then the three functions f_a(t) = v₀(T-T₀) + P₀; f_b(t) = (P₁-P₀) (T-T₀) / T + P₀; f_c(t) = b (t−T₁) + P₁; Is defined. b is, for example, b = (P₂-P₁) / T
Establish. Also, f₁(T₀) = 1, f₁(T₁) = 0, T₀<T <
T₁Continuous function f that can be differentiated twice by₁(t) and f₂(T ₀) =
0, f₂(T₁) = 1, T₀<T <T₁Continuously differentiable twice with
Function f₂Consider (t). These functions f₁(t), f₂(t)
Is defined as an S-shaped function, for example. And
F (T₀) = P₀, F (T₁) = P₁So that the function f
₁(t), f₂the function f using (t)_a(t), f_b(t), f_c(t)
Synthesis is performed to obtain the function F (t). Where the function f₁(t), f
₂(t) is the function f_a(t), f_b(t), f_cWeighting for (t)
It is used as a coefficient. In particular, F (t) = f_b(t) + f₁(t) ・ (f_a(t) -f_b(t)) + f₂
(t) ・ (f_c(t) -f_b(t)) Synthesis is performed as follows. The function thus obtained
Interpolation is performed by F (t). Note that the above formula is used for synthesis.
If₁(T₀) = 1, f₁(T₁) = 0, f₂(T₀) = 0, f₂
(T₁) = 1, F (T₀) = P₀, F (T₁) = P₁But
To establish. To generate a smooth position command by interpolation
To achieve this, at least the acceleration command must be continuous.
And for that, here the function f₁(t), f₂(t) and
Then, a function that can be differentiated twice is used.

Actually, instead of instructing the lower order arithmetic unit of the interpolated value based on the function F (t) for each sampling period δt, the interpolated value at the previous sampling time (t = t _n-1 ) is x.
_n-1 (= F (t _n-1 )), at the time of this sampling (t = t _n
= T _n-1 + δt) is set as x _n (= F (t _n )),
S _n = x _n −x _n−1 may be calculated, and this S _n may be given to the lower-order arithmetic unit as the pulse movement amount of the servo motor for each sampling cycle. FIG. 1 is a diagram for illustrating the above-described processing, in which the horizontal axis represents time t and the vertical axis represents position x of the servo motor.
Represents the plane as. In the figure, a circle indicates a position command by the higher-order arithmetic unit. The straight lines f _a , f _b , and f _c represent the straight lines represented by x = f _a (t), x = f _b (t), and x = f _c (t), respectively. Further, each of the vector V _0, V _1, V ₂ which is parallel to the direction of the straight line _{f a, f b, f c} , shows the increase or decrease direction of the position command. Specifically, (tangent of the angle and direction and the t-axis of the vector V ₀₎ the slope of the vector V ₀ denotes the velocity command v ₀ paid out by interpolation just prior to time t _0, the vector V ₁ the slope (P ₁ -P ₀₎
/ T, and the slope of the vector V ₂ is the parameter b.

By defining in this way, the straight line f _a becomes _a straight line that passes through the position command P _{0 of} this time and has the speed command v ₀ finally paid out in the interpolation just before, and the straight line f _b is the position of this time. It becomes a straight line passing through the command P ₀ and the next position command P ₁ . Since b = (P ₂ −P ₁ ) / T is determined,
The straight line f _c is a straight line that passes through the next position command P ₁ and the next position command P ₂ . Then, the above-mentioned functions f ₁ (t), f ₂ (t)
These three straight lines f _a , f _b , and f _c are combined with each other as a weighting coefficient. As a result of the synthesis, a curve as shown by the broken line in the figure is obtained. This curve is x = F using the function F (t) above.
(t). This curve is T ₀ <t <
The interpolated value at an arbitrary time t of T ₁ is shown.
Here, a specific example of the functions f ₁ (t) and f ₂ (t) will be described. For the functions f ₁ (t) and f ₂ (t), τ is an appropriate constant, for example,

[0011]

[Equation 1]

Can be used. FIG. 2 is a graph showing the functions f ₁ (t) and f ₂ (t) illustrated here. As is clear from the above description, the method of the present invention does not involve a time-dependent integral operation, and therefore does not cause a delay with respect to the position command from the higher-order arithmetic unit. Further, since the function F (t) is determined so that F (T ₀ ) = P ₀ and F (T ₁ ) = P ₁ , the deterioration of the accuracy with respect to the position command of the higher-order arithmetic unit does not occur. By the way, in this embodiment, since the cycle T of the command from the higher-order arithmetic unit is constant,
It is possible to repeatedly use a set of functions f ₁ (t) and f ₂ (t) at each output interval from the higher-order arithmetic unit. Further, since the sampling cycle δt is constant and the interpolated value only needs to be output at the time of sampling by the lower-order arithmetic unit, the values of the functions f ₁ (t) and f ₂ (t) need only be determined at the time of sampling. , Therefore, the function f at each sampling point
The values of ₁ (t) and f ₂ (t) can be calculated in advance and stored in a table format. In addition, the function f _a (t), f _b (t), f
_{Since all c} (t) are linear functions, the value at each sampling can be obtained by obtaining the initial value and adding the increment value for each sampling period. Therefore, when determining the interpolation value for each sampling time, the value of the function f ₁ (t), f ₂ (t) obtained by drawing the table and the function f _a (t), f obtained by adding the increments _b (t), may be carried out a simple product sum calculation of the value of f _c (t), compared with the case of obtaining an interpolated value based on the spline curve functions, it is possible to greatly reduce the amount of calculation .

The embodiment of the present invention has been described above. There, the value of the above-mentioned parameter b is set to b = (P _{2 in} consideration of the next position command P ₁ and the position command P _{2 one} after another.
Although −P ₁ ) / T is used, the parameter b can be determined in consideration of the position commands P ₃ , ... The value of the parameter b is the same as the slope v ₂ of the vector V ₂ in the time-position space shown in FIG. Therefore,
For example, as shown in FIG. 3A, the vector representing the increasing / decreasing direction from P ₁ to P ₂ is V _2a, and the vector representing the increasing / decreasing direction from P ₂ to P ₃ is moved symmetrically with respect to the vector V _2a . things and vector V _2b, the time - the vectors V _2a on position plane, the inclination of each v _2a of V _2b, as _{v 2b, v 2 = v 2a} + kv 2b (k: 0 constant of ≦ k ≦ 1) it is possible to define a slope v ₂ that parameter b of the vector V ₂ as. The case of k = 0 corresponds to the case of b = (P ₂ −P ₁ ) / T described above.

Next, the present invention will be described in more detail by describing the result of the simulation performed based on the embodiment (the parameter b is determined in consideration of the position command P ₃ one after another). FIG. 4 shows a simulation result in the case of interpolating a ramp (ramp) type position command discretely output from the higher-order arithmetic unit, where the horizontal axis represents time t and the vertical axis represents position x. ing. In the figure,
A mark 41 indicates a discrete position command from the higher-order arithmetic unit, a solid line 42 indicates a position command after interpolation, and a dotted line 43 indicates a speed command (movement amount) at this time. In this example, a position command around the time t = 0.03 is output from the higher-order arithmetic unit as a boundary until the stationary state until then and a constant velocity linear motion state after that. In the subsequent position command, the rising to the constant velocity linear motion is smoothly interpolated and quickly converges to reproduce the straight line in the constant velocity linear motion.

Similarly, FIG. 5 shows a simulation result when the sinusoidal position command is interpolated. ○ mark 5
Reference numeral 1 indicates a discrete position command from the higher-order arithmetic unit, and a solid line 5
2 shows the position command after interpolation, and the dotted line 53 shows the speed command at this time. In this example, the higher-order arithmetic unit outputs a position command that shifts from the stationary state to the sinusoidal vibration state at the boundary of the position command around time t = 0.03. The transition from the stationary state to the sinusoidal vibration state is smoothly interpolated and quickly converges to the sinusoidal vibration state, and the sinusoidal waveform in the time-position plane is accurately reproduced. As is clear from the above simulation results, according to the present invention, the increase / decrease of the command changes across a specific bending point in the time-position plane, and the increase / decrease of the command on each side sandwiching the bending point. Is a constant position command, the bending portion is interpolated into a smooth command by a curve of quadratic or higher order. Further, since a function that can be differentiated twice or more is used and the information is considered up to at least two points ahead, if the position command is represented by a curve of quadratic or higher, the bend in the position command The interpolation is performed so as to have a bulge corresponding to the condition. In any case, in the present invention, the curve obtained by the interpolation passes all of the position commands from the higher-order arithmetic unit.

[0016]

As described above, according to the present invention, the function f _a (t), f is calculated based on the command from this time to at least the next one.
_b (t), f _c (t) is obtained, and these functions f _a (t), f _b (t), f _c (t) are calculated by using a twice differentiable function as a weighting coefficient.
By synthesizing, interpolation that can realize smooth acceleration / deceleration without delay or deterioration of accuracy, requires a small amount of calculation, and can reproduce the order of the command calculated by the higher-order arithmetic unit. It is possible to perform the positioning control with high accuracy.

[Brief description of drawings]

FIG. 1 is a diagram illustrating a concept of a command interpolation method according to the present invention.

FIG. 2 is a diagram showing an example of convergence functions f ₁ (t) and f ₂ (t).

FIG. 3 is a diagram illustrating an example of how to determine a vector V ₂ .

FIG. 4 is a diagram showing a simulation result when interpolation is performed with respect to a position command that changes to a ramp type.

FIG. 5 is a diagram showing a simulation result when interpolation is performed for a position command that changes to a sine wave type.

[Explanation of symbols]

f _a , f _b , f _c Straight line P ₀ , P ₁ , P ₂ , P ₃ Position command V ₀ , V ₁ , V ₂ vector

─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Yuatsu Inazumi No. 2 Kurosaki Shiroishi, Yawatanishi-ku, Kitakyushu, Fukuoka Prefecture Yasukawa Electric Co., Ltd. (56) Reference JP-A-2-148111 (JP, A) Flat 2-202607 (JP, A) (58) Fields surveyed (Int.Cl. ⁷ , DB name) G05B 19/4103

Claims (3)

(57) [Claims] 1. A servo device comprising a higher-order arithmetic unit for discretely calculating a position command and a lower-order arithmetic unit for controlling a digital servo system at a sampling cycle shorter than the output interval of the position command from the upper-order arithmetic unit. Then, in the command interpolation method in the control system in which the corresponding digital servo system is commanded by the servo device and each axis is positioned, the current position command from the higher-order arithmetic unit corresponds to P ₀ and the current position command P ₀ . Is T ₀ , the next position command from the higher-order arithmetic unit is P ₁ , the time corresponding to the next position command P ₁ is T ₁ , and the speed command finally paid out by the previous interpolation is v _0. , B is a parameter determined by the next position command P ₁ and the position command P ₂ from the higher-order arithmetic unit one after another, and three functions f _{a having} time t (where T ₀ ≤t ≤T ₁ ) as an independent variable ( _{) = V 0 (t-T} 0) + P 0; f b (t) = (P 1 -P 0) (t-T 0) / (T 1 -T 0) + P
₀ ; f _c (t) = b (t−T ₁ ) + P ₁ ; and T ₀ ≦ t ≦ T ₁ is continuous and T ₀ <t <T ₁ with respect to t
Twice differentiable function as a weighting factor Te, the weighting
Using the coefficients , three functions f _a (t), f _b (t), and f _c (t) are combined so that F (T ₀ ) = P ₀ and F (T ₁ ) = P ₁ A command interpolation method for a positioning control system, characterized in that a function F (t) is obtained and the value of the function F (t) is used as a position command at time t (where T ₀ ≤t ≤T ₁ ). 2. The command interpolation method for a positioning control system according to claim 1, wherein the parameter b is changed in accordance with the information of the position command from the higher-order arithmetic unit one after another. 3. The value of the function F (t) is calculated for each sampling cycle in the lower-order arithmetic unit, and the difference between the value of the function F (t) obtained last time and the value of the function F (t) obtained this time is moved. The command interpolation method for a positioning control system according to claim 1, wherein the command interpolation method is provided as a quantity to the lower-order arithmetic unit.