JP3439957B2 - Servo control method and servo control device for nonlinear mechanism - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a servo control method and a servo control device for a non-linear mechanism, and more particularly, the ratio of the rotation angle of the drive motor to the movement amount (turning angle) of the controlled object is the position of the controlled object. The present invention relates to a servo control method and a servo control device for a non-linear mechanism that changes depending on the turning position).

[0002]

2. Description of the Related Art As a non-linear mechanism in which the ratio of the rotation angle of a drive motor to the amount of movement of a controlled object changes depending on the position of the controlled object, as shown in FIG.
00, a revolving structure (object to be controlled) 102 capable of revolving around a central axis of a pivot 104, and a horizontal slider 108 engaged with a horizontal guide rail 106 so as to be linearly movable in a horizontal direction.
And the vertical guide rail 1 of the horizontal slider 108 is provided.
Vertical slider 112 that is movable in the vertical direction by engaging with 10.
The revolving unit 102 and the lateral slider 108 are pivotally connected by a connecting pin 114 fixed to the screw member, and are screwed into a feed screw rod 118 which is rotationally driven by a drive motor (servo motor) 116 mounted on the base member 100. With the feed nut 120 present, the horizontal slider 108 is moved horizontally (see FIG.
Reciprocating body 1
The one that turns 02 around the central axis of the pivot 104 is
It has already been proposed by the same applicant as the present applicant (Japanese Patent Application No. 8-198035).

In this non-linear mechanism, since the connecting pin 114 can be moved up and down with respect to the horizontal slider 108 by the vertical slider 112, the movement locus of the connecting pin 114 is
As indicated by reference character A in FIG. 5, an arc is formed centering on the pivot 104 with respect to the linear movement of the lateral slider 108 and the feed nut 120.

The transfer function of this non-linear mechanism is the swing structure 10
2, the turning angle from the neutral position (upright position) is θ, the distance from the turning center (the pivot 104) to the connecting pin 114 is R,
When the linear movement amount of the feed nut 120 is X, it is expressed by the following equation.

Θ = sin ⁻¹ (X / R) (1) Hereinafter, this nonlinear mechanism may be referred to as a sine nonlinear mechanism.

As another non-linear mechanism, as shown in FIG. 6, the connecting pin 114 is directly fixed to the lateral slider 108, and the connecting pin 114 is the concave groove portion 122 of the revolving unit 102.
The same applicant as the applicant of the present application, which is engaged with the movable body 102 relatively displaceably, and horizontally swings the lateral slider 108 by the feed nut 120 to swing the revolving structure 102 around the central axis of the pivot 104. Japanese Patent Application No. 9
-112469.

In this non-linear mechanism, since the connecting pin 114 is fixed to the lateral slider 108, the connecting pin 11 is fixed.
The movement locus of No. 4 makes the same straight line as the linear movement of the lateral slider 108 and the feed nut 120, as indicated by the symbol B in FIG.

In any of the above-mentioned nonlinear mechanisms, the drive motor 1
The rotation angle of 16, that is, in other words, the linear movement amount X of the feed nut 120 and the turning angle θ of the turning body 102 that is the control target (the ratio of the movement amount changes depending on the turning position of the control target).

The transfer function of this non-linear mechanism is the swing structure 10
2, the turning angle from the neutral position (upright position) is θ, the distance from the turning center (the pivot 104) to the connecting pin 114 is R,
When the linear movement amount of the feed nut 120 is X, it is expressed by the following equation.

Θ = tan ⁻¹ (X / R) (2) Hereinafter, this nonlinear mechanism may be referred to as a tangent nonlinear mechanism.

[0011]

In the above-described non-linear mechanism, in order to rotate the revolving structure while keeping the revolving speed at a predetermined constant value, the linear movement amount X of the feed nut (the rotation angle of the drive motor) is required. Needs to be continuously changed according to the turning angle θ of the turning body.

The present invention provides a servo control method and a servo control device for controlling the moving speed of a controlled object in a non-linear mechanism with high accuracy, such as rotating the revolving structure of the non-linear mechanism at a predetermined constant turning speed, and more particularly, An object of the present invention is to provide a servo control method and a servo control device that can perform the control without applying a heavy arithmetic processing load.

[0013]

In order to achieve the above object, a servo control method for a non-linear mechanism according to a first aspect of the present invention comprises:
The base member has a revolving structure capable of revolving around the central axis of the pivot.
And engaged with a horizontal guide rail so that it can move linearly in the horizontal direction.
And a horizontal slider that is installed.
A vertical slide that engages with the straight guide rails and can move vertically.
By the connecting pin fixed to the lid,
The slider is pivotally connected and mounted on the base member.
The feed screw is driven by the servo motor
Move the horizontal slider horizontally by adjusting the feed nut.
By moving the swivel body in the direction of the pivot.
Which revolves around the central axis of
The turning angle from the neutral position (upright position) is θ
The distance from the turning center of the body to the connecting pin is R,
If the linear movement amount of the nut is X, the transfer function is θ = s
In ^-1 (X / R) is a sinusoidal nonlinear mechanism
In the servo control method, the rotation angle θ of the servo motor
The estimated value u of X / R is calculated from m and the servo motor
Position loop sensor of feedback loop for servo control
In is changed in proportion to the correction value y = √ (1-u ² ).
The rotation angle θm of the servo motor and the rotation of the controlled object.
The control is performed so that the ratio relationship with the turning angle θ becomes constant.

Further , in order to achieve the above-mentioned object, a claim
The servo control method for the non-linear mechanism according to Item 2 is based on the base member.
In addition, a revolving structure that can revolve around the center axis of the pivot and a horizontal
The side that is engaged with the idrail so that it can move linearly in the horizontal direction.
And a slider fixed to the lateral slider.
The swing pin and the lateral slider are pivotally connected by a connecting pin.
A servo motor mounted on the base member
The feed nut that is screwed into the feed screw rod
To move the horizontal slider linearly in the horizontal direction
This allows the revolving structure to rotate around the central axis of the pivot.
The revolving structure is in a neutral position (upright position).
From the center of rotation of the revolving structure.
The distance to the connecting pin is R, and the feed nut is linearly moved.
When the quantity is X, the transfer function is θ = tan ⁻¹ (X / R)
In the servo control method of the tangent type nonlinear mechanism represented by
Estimate X / R from the rotation angle θm of the servo motor
Calculate the value u and perform servo control of the servo motor.
Feedback loop loop position loop gain correction value y = 1
Rotation of the servo motor by changing in proportion to + u ^2.
The ratio relationship between the angle θm and the turning angle θ of the controlled object is constant
Control to become.

A servo control method for a non-linear mechanism according to claim 3
The method is a servo control of a nonlinear mechanism according to claim 1 or 2.
Method, subtract the feedback value from the position command
Obtained by multiplying the deviation obtained by
The speed command of the servo motor
The estimated value u of / R and the reciprocal of the position loop gain correction value y
It is corrected by the speed command correction value calculated accordingly.

Further, in order to achieve the above-mentioned object, a claim
The non-linear mechanism servo controller according to item 4 is:Base member
In addition, a revolving structure that can revolve around the center axis of the pivot and a horizontal
The side that is engaged with the idrail so that it can move linearly in the horizontal direction.
And a slider, and the vertical guide rail of the horizontal slider is provided.
Fixed to a vertical slider that can be moved vertically by engaging
The revolving structure and the lateral slider by means of the connected pin
Is pivotally connected to the servo motor mounted on the base member.
It is screwed into the feed screw rod that is driven to rotate by the motor.
Use the feed nut to move the horizontal slider horizontally in a straight line.
By moving the revolving structure to the central axis of the pivot.
What is made to turn around, the neutral position of the turning body
The turning angle from the (upright position) is θ, and the turning body is turning.
The distance from the core to the connecting pin is R,
When the linear movement amount is X, the transfer function is θ = sin
^-1 Servo of sine type non-linear mechanism represented by (X / R)
In the control device, the rotation angle θm of the servo motor
Means for calculating the estimated value u of X / R, and the servo mode
Position loop of the feedback loop for servo control of the
Correction value y = √ (1-u ^Two ) Changes in proportion to
And a rotation angle θm of the servo motor.
And the turning angle θ of the controlled object become constant.
Control.

Further , in order to achieve the above-mentioned object, a claim
The non-linear mechanism servo controller according to item 5 is a base member.
In addition, a revolving structure that can revolve around the center axis of the pivot and a horizontal
The side that is engaged with the idrail so that it can move linearly in the horizontal direction.
And a slider fixed to the lateral slider.
The swing pin and the lateral slider are pivotally connected by a connecting pin.
A servo motor mounted on the base member
Feed nut which is screwed to the feed Rineji rod which is rotated I
To move the horizontal slider linearly in the horizontal direction
This allows the revolving structure to rotate around the central axis of the pivot.
The revolving structure is in a neutral position (upright position).
From the center of rotation of the revolving structure.
The distance to the connecting pin is R, and the feed nut is linearly moved.
When the quantity is X, the transfer function is θ = tan ⁻¹ (X / R)
The tangent type non-linear mechanism servo controller represented by
Estimate X / R from the rotation angle θm of the servo motor
Means for calculating the value u and servo control of the servo motor
Correct the position loop gain of the feedback loop
Means for varying in proportion to the value y = 1 + u ² ,
Rotation angle θm of the servo motor and swing angle of the controlled object
Control is performed so that the ratio relationship with the degree θ is constant.

A non-linear mechanism servo control device according to claim 6
The servo control of the nonlinear mechanism according to claim 4 or 5.
In the device, subtract the feedback value from the position command
Obtained by multiplying the deviation obtained by
The speed command of the servo motor
The estimated value u of / R and the reciprocal of the position loop gain correction value y
Means for correcting the speed command correction value calculated according to
Have.

[0019]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described in detail below with reference to the drawings.

(Embodiment 1) FIG. 1 shows an embodiment of a servo control device of a sinusoidal nonlinear mechanism according to the present invention.

In FIG. 1, reference numeral 1 is a sine-type non-linear mechanism, 10 is a servo system for driving the non-linear mechanism, and 50 is a linear reference servo system added for error evaluation of the non-linear mechanism driving servo system 10. Are shown respectively. The symbol Gr is the rotation / linear motion conversion ratio of the feed screw, and Gr = 2π / P where P is the pitch of the feed screw.

The non-linear mechanism driving servo system 10 is of a hybrid feedback type and includes a position controller 12
, Speed control system 14, position loop gain correction value calculation unit 16, speed command correction value calculation unit 18, and primary delay circuit 2
It has 0 and.

The position controller 12 includes a position loop gain variable setter 22 and determines the deviation ε between the turning angle position command θ * and the turning angle feedback value θf generated by the interpolation by the numerical control device. The rotation / linear motion conversion ratio Gr is multiplied by the position loop gain Gp set by the position loop gain variable setter 22 to output the speed command V * to the speed control system 14.

The speed control system 14 has a speed loop gain Gs.
And drives the servo motor. The motor rotation angle θm is obtained by integrating the motor rotation speed Vm obtained by the speed control system 14 by the integrator 24.

The motor rotation angle θm is Gr · (X / R)
Therefore, when this is divided by the rotation / linear motion conversion ratio Gr by the calculator 26, the estimated value u of X / R can be obtained.

The position loop gain correction value calculation unit 16 calculates y
The position loop gain correction value y is calculated by the calculation of = √ (1−u ² ). Position loop gain correction value calculation unit 1
The position loop gain correction value y calculated by 6 is input to the position loop gain variable setter 22, and the position loop gain variable setter 22 sets the position loop gain Gp proportional to the position loop gain correction value y. That is, the position loop gain at the turning angle of 0 degree (neutral position) is set to G ₀ , and the position loop gain Gp is set by Gp = G ₀ (y).

Next, the transfer function θ = s of the sine nonlinear mechanism
A method of deriving the position loop gain correction value y from in ⁻¹ (X / R) will be described.

Transfer function of sinusoidal nonlinear mechanism θ = sin ⁻¹
The inverse transfer function is calculated from (X / R) = sin ⁻¹ (u).

[0029] u = sin (θ) (3) This is second-order differentiated at time (t).

[0030]

(1) (du / dt) = cos θ · (dθ / dt) (4) (d ² u / dt ² ) = cos θ · (d ² θ / dt ² ) −sin θ · (dθ / dt) ² ... (5) The position loop gain correction value y is set to s by using the coefficient of (d ² θ / dt ² ) in the ^second derivative equation as the position loop gain correction value y.
It is derived from the estimated value u of inθ.

Y = cos θ = √ (1.0-sin ² θ) y = cos θ = √ (1-u ² ) (6) As described above, the position loop gain Gp is obtained by Gp = G ₀ (y). Is changed in proportion to the position loop gain correction value y which is an estimated value of cos θ, the correction is confined in the Zaho control device, and the servo control for turning the revolving structure at a predetermined constant turning speed is large. It can be performed with high precision without applying.

In the position feedback, the motor rotation speed Vm is multiplied by the reciprocal z of the position loop gain correction value y calculated by the calculator 28 by the calculator 30 and integrated by the integrator 32, which is then calculated by the calculator 34. By dividing by the rotation / linear motion conversion ratio Gr of the feed screw, the estimated value θe of the turning angle and the output value θs of the turning angle sensor incorporated in the non-linear mechanism 1 are combined including the primary delay circuit 20. , Which is basically hybrid feedback compensation.

The speed command correction value calculation unit 18 calculates a lower value based on the estimated value u of X / R, the speed command V *, and the reciprocal z = 1 / y of the position loop gain correction value y calculated by the calculator 28. The speed command correction value Vc is calculated by the equation.

[0034]

## EQU00002 ## Vc = (k.multidot.u) (V * .z / Gr) (V * .z / Gs) (7) where k is a coefficient, and the coefficient k is determined by simulation, and is usually The value is about 0.8 to 1.3.

The speed command Vc * corrected by subtracting the speed command correction value Vc from the speed command V * is given to the speed control system 14, whereby the servo control for turning the revolving structure at a predetermined constant turning speed is further enhanced. It will be done with precision.

The motor rotation speed Vm is Gr · du / dt.
Since this is the feed screw rotation / linear motion conversion ratio G
By dividing by r, the estimated value of (du / dt) can be obtained.

Next, a method of deriving the speed command correction value Vc will be described.

Transfer function of sinusoidal nonlinear mechanism θ = sin
Differentiate ^-1 (u) with respect to time (t).

[0039]

(Dθ / dt) = (du / dt) / √ (1-u ² ) ... (8) Let z be the coefficient of (du / dt) in the differential equation of the transfer function.
z is derived from the position loop gain correction value y.

Z = 1 / y (9) The coefficient of (d ² θ / dt ² ) of the equation (5) of the second derivative of the inversion function is Gp according to the position loop gain correction value y obtained previously.
Although it is corrected, other corrections are required separately, and the following formula is corrected.

−sin θ · (dθ / dt) ² (10) Substituting (8) obtained by differentiating the transfer function into this correction formula, and expressing by using u, y, z obtained earlier. . -U · z ² (du / dt) ² (11) Since the automatic control of the speed loop acts so as to follow the speed command V *, the speed command V * is almost the same value as the motor rotation speed Vm. , As the data for the correction calculation,
Both can be identified. Therefore, the speed command V *
Can be represented by Gr (du / dt), which can be rotated
By dividing by the linear motion conversion ratio Gr, (dx / dt)
The estimated value of can be obtained.

Since the speed command correction value Vc is multiplied by (Gs) by the forward element of the speed loop and acts on the servomotor, the correction applied before the speed loop is (d ² u / d).
It must be (Gr / Gs) times the correction amount required for t ² ).

Applying these conditions to the correction equation (11), the correction amount to be added before the velocity loop is as follows.

[Formula 4] −u · z ² (Gr / Gs) (V * / Gr) ² … (12.1) −x · z ² (V * / Gr) (V * / Gs)… (12.2) This (12.2) It can be easily understood that the formula is almost the same as the formula (7). However, there is a numerical difference of about 19%. Equation (7) is determined by simulation, and it is presumed that this difference is not essential and is due to the influence of the calculation accuracy of simulation and the influence of feedback of the velocity loop.

The simulation result will be described with reference to FIG.

The simulation method is that the calculation formula based on FIG. 1 is described in C language, compiled by MS-C, and the execution result is graphed.

The distance from the turning center to the connecting pin is 750
The rotation angle was set to mm, and the rotation angle was given a rotation command with a constant angular velocity of 1000 degrees per minute from −33.3 degrees to +33 degrees. The calculation was performed every 0.01 degree, and the Runge-Kutta method was used for the integration.

Since the data is output every 0.01 degree from -33.3 degrees to -33.1 degrees and thereafter every 0.5 degrees, the left end of each graph is enlarged and displayed.

In the figure, is a graph of the turning movement result of the reference system. The reference system is a system that does not include a nonlinear mechanism.
Since the results of the turning movement in the case of having a non-linear mechanism almost overlap with those on the graph even without the correction of the present invention, the difference cannot be evaluated on the graph of the same scale. Therefore, the angular difference between the turning movement of the reference system and the turning movement of the system including the non-linear mechanism is displayed as 200 times larger.

It can be read from FIG. 2 that this error is about 0.05 degree at maximum.

When the position loop gain Gp is corrected according to the present invention, this error becomes as follows. Will be explained below and the numerical value is small, so it is plotted on the right scale.

If the position loop gain Gp is corrected, 0.
The error is reduced to 00009 degrees. Furthermore, it is a case where a correction is made to the speed command value.

When the correction coefficient k was 0.81, the error could be improved to 0.000005 degrees.

The linear reference servo system 50 added to evaluate the error of the non-linear mechanism driving servo system 10 has the same structure as a normal linear servo system, and includes a position controller 52 with a fixed position loop gain Gp. , Speed control system 54,
It has a first-order delay circuit 56.

The position controller 52 controls the turning angle position command θ *.
And the turning angle feedback value θf are multiplied by the rotation / linear motion conversion ratio Gr of the feed screw and the fixed position loop gain Gp to output the speed command V * to the speed control system 54.

The speed control system 54 has a speed loop gain Gs.
And drives the servo motor. The motor rotation angle Vm obtained by the speed control system 54 is integrated by the integrator 58 to obtain the motor rotation angle θm. The linear reference servo system 5 is obtained by dividing the motor rotation angle θm by the rotation / linear motion conversion ratio Gr by the calculator 60.
The regular turning angle θr at 0 is obtained, and the error of the non-linear mechanism driving servo system 10 can be evaluated from the deviation between the turning angle θr and the output value θs of the turning angle sensor.

(Embodiment 2) FIG. 3 shows an embodiment of a tangent type nonlinear mechanism servo controller according to the present invention. In FIG. 3, the same or the same constituents as those shown in FIG. 1 are designated by the same reference numerals as those shown in FIG. 1, and the description thereof will be omitted.

Also in this servo control device, the position controller 12
Includes a position loop gain variable setter 22. The rotation / linear motion conversion ratio Gr of the feed screw is calculated based on the deviation ε between the turning angle position command θ * generated by interpolation by the numerical controller and the turning angle feedback value θf. And the position loop gain Gp set by the position loop gain variable setter 22 are multiplied to output the speed command V * to the speed control system 14.

The position loop gain correction value calculation unit 16 calculates y
The position loop gain correction value y is calculated by the calculation of = (1 + u ² ). Position loop gain correction value calculation unit 16
The position loop gain correction value y calculated by is input to the position loop gain variable setter 22, and the position loop gain variable setter 22 sets the position loop gain Gp proportional to the position loop gain correction value y. Also in this case, the position loop gain Gp is set by Gp = G ₀ (y), where G ₀ is the position loop gain at the turning angle of 0 degrees (neutral position).

Next, the transfer function θ = t of the tangent nonlinear mechanism
A method of deriving the position loop gain correction value y from an ⁻¹ (X / R) will be described.

Transfer function of tangent nonlinear mechanism θ = tan ⁻¹
The inverse transfer function is obtained from (X / R) = tan ⁻¹ (u).

[0061] u = tan (θ) (13) This is second-order differentiated at time (t).

[0062]

[Equation 5] (du / dt) = sesθ · (dθ / dt) (14) (d ² u / dt ² ) = sesθ · (d ² θ / dt ² ) +2 sec θ · tan θ · (dθ / dt) ² (15) With the coefficient of (d ² θ / dt ² ) in the ^second differential equation as the position loop gain correction value y, the position loop gain correction value y is t
It is derived from the estimated value u of an θ.

Y = sec θ ² = 1 + tan ² θ y = (1 + u ² ) ... (16) As described above, the position loop gain Gp is the estimated value of sec θ ^{2 from} Gp = G ₀ (y). By changing in proportion to the correction value y, the correction is limited to the Zaho control device, and the servo control for turning the revolving structure at a predetermined constant turning speed can be performed with high accuracy without applying a large arithmetic processing load. it can.

The speed command correction value calculation unit 18 calculates the lower value based on the estimated value u of X / R, the speed command V *, and the reciprocal z = 1 / y of the position loop gain correction value y calculated by the calculator 28. The speed command correction value Vc is calculated by the equation.

[0065]

## EQU00006 ## Vc = (k.multidot.u) (V * .z / Gr) (V * / Gs) (17) Note that k is a coefficient, and the coefficient k is determined by simulation, and is usually 1.5 It is a value of about 3.0.

The speed command Vc * corrected by adding the speed command correction value Vc to the speed command V * is given to the speed control system 14, whereby the servo control for turning the revolving structure at a predetermined constant turning speed is further enhanced. It will be done with precision.

Next, a method for deriving the speed command correction value Vc will be described.

Transfer function θ = tan of tangent type nonlinear mechanism
Differentiate ^-1 (u) with respect to time (t).

[0069]

(7) (dθ / dt) = (du / dt) / (1 + u ² ) ... (18) Let z be the coefficient of (du / dt) in the differential equation of the transfer function.
z is derived from the position loop gain correction value y.

Z = 1 / y (19) The coefficient of (d ² θ / dt ² ) in the equation (15) of the second derivative of the inverse transfer function is Gp corrected by the previously obtained position loop gain correction value y. However, other than that, a separate correction is necessary, and the following formula is corrected.

[0071]

[Equation 8] + 2sec ² θ · tan θ · (dθ / dt) ² (20) Substituting (18) obtained by differentiating the transfer function into this correction formula, and further obtaining u, y, z Is represented by.

+ 2u · z (du / dt) ² (21) In this case as well, the speed command correction value Vc is multiplied by (Gs) by the forward element of the speed loop and acts on the servo motor, so that before the speed loop. The correction to be applied is (d ² u / dt
It must be (Gr / Gs) times the correction amount required for ² ).

Applying these conditions to the correction equation (21), the correction amount added before the velocity loop is as follows.

[Equation 9] + 2u · z (Gr / Gs) (V * / Gr) ² 2 (22.1) + 2u · z (V * / Gr) (V * / Gs) (22.2) This (22.2) is It is easy to understand that it is almost the same as equation (17). Equation (17) is determined by simulation, and this difference is not essential, and it is presumed that it is due to the influence of the calculation accuracy of the simulation and the influence of feedback of the velocity loop.

The simulation method is that the calculation formula based on FIG. 3 is described in C language, compiled by MS-C, and the execution result is graphed.

The distance (minimum distance) from the turning center to the connecting pin is 750 mm, and the turning angle is from -33.3 degrees to +
A turning command with a constant angular velocity of 1000 degrees per minute was given up to 33 degrees. The calculation was performed every 0.01 degree, and the Runge-Kutta method was used for the integration.

Since the data is output every 0.01 degree from -33.3 degrees to -33.1 degrees and thereafter every 0.5 degrees, the left end of each graph is displayed enlarged.

In the figure is a graph of the result of the turning movement of the reference system. Also in FIG. 4, the reference system is a system that does not include a nonlinear mechanism. The turning movement result with a non-linear mechanism is also
Even if the correction of the present invention is not performed, it almost overlaps on the graph, and therefore the difference cannot be evaluated on the graph of the same scale. Therefore, the angular difference between the turning movement of the reference system and the turning movement of the system including the non-linear mechanism is displayed as 200 times larger.

It can be read from FIG. 4 that this error is about 0.12 degrees at maximum.

When the position loop gain Gp is corrected according to the present invention, this error becomes as follows. Will be explained below and the numerical value is small, so it is plotted on the right scale.

If the position loop gain Gp is corrected, 0.
The error is reduced to 0002 degrees. Furthermore, it is a case where the correction for the speed command value according to the present invention is performed. The correction coefficient k used was +1.62. The error could be improved to 0.000025 degrees.

The position control of the machine is performed by adjusting the torque of the drive motor. Even when controlling a mechanism including a non-linear mechanical element, the torque of the electric motor must be able to exhibit the expected acceleration.

Considering that the interpolation of the numerical controller is performed assuming that all systems are linear, in order to correctly control the mechanism including the non-linear mechanical element, the inverse transfer function of the transfer function of the machine is used. Must be generated in the drive motor in proportion to the acceleration of the lateral slider, which is obtained by the second-order differentiation.

In the case of a linear mechanism, y = 1.0, but in a system including a non-linear mechanism, it is necessary to adjust the signal at the previously obtained coefficient y.

In the present invention, the position loop gain Gp is changed in proportion to y.
The same effect can be expected by changing any of the forward-direction elements such as the velocity loop gain Gs as well as p.

Even if the loop gain G is corrected by y described in the previous section, the equations (5) and (15) of the second derivative cannot be completely corrected. However, since the remaining error is slight, it is possible to use it as it is in machining that does not require such high accuracy or has a slow turning angular velocity.

When higher precision is required or when it is applied to machining with a high turning angular velocity, (10), (20) and
The corrections shown in the equations (11) and (21) may be performed.

[0087]

As can be understood from the above description, this invention is
According to the non-linear mechanism axis servo control method and device according to Ming, the loop gain in the feedback loop of the servo control device is changed according to the rotation angle of the drive motor, so that the rotation angle of the drive motor and the movement amount of the controlled object are changed. Since a characteristic equivalent to the case of controlling a linear mechanism with a constant ratio to is obtained, no special data processing is required on the side of the numerical controller, and a general-purpose numerical controller can be used to move the controlled object. The speed can be controlled with high accuracy, and the angle error can be set as it is with respect to the setting of the pitch error correction.

Further, according to the method and apparatus for servo control of the non-linear mechanism axis according to the present invention , the position loop gain is changed in proportion to the value obtained by the calculation of the range of the four arithmetic operations and the square root operation from the rotation angle of the drive motor. Thus, the characteristic equivalent to the case of controlling the linear mechanism in which the relationship between the rotation angle of the drive motor and the movement amount of the controlled object is constant is obtained,
The calculation processing load is not increased, the time delay that deteriorates the servo control performance is reduced, and the use of a numerical calculation coprocessor is not required, so that inexpensive control is possible.

Further, according to the method and apparatus for servo control of the non-linear mechanism axis according to the present invention, the speed command value of the drive motor obtained by multiplying the deviation obtained by subtracting the feedback value from the position command by the position loop gain, Since the correction is performed by adding or subtracting the speed command correction value by the product of the square of the speed command value and the value obtained by calculating the range of the four arithmetic operations and the square root calculation from the rotation angle of the drive motor, the moving speed of the controlled object The accuracy can be further improved, the time delay that deteriorates the servo control performance can be reduced without increasing the calculation processing load, and the use of the numerical calculation coprocessor is not necessary, which enables the inexpensive control. .

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of a servo control device for a sinusoidal nonlinear mechanism according to the present invention.

FIG. 2 is a graph showing a simulation result of a servo control device having a sinusoidal nonlinear mechanism.

FIG. 3 is a block diagram showing an embodiment of a servo controller of a tangent nonlinear mechanism according to the present invention.

FIG. 4 is a graph showing a simulation result of a tangent nonlinear mechanism servo controller.

FIG. 5 is a front view showing an example of a sine nonlinear mechanism.

FIG. 6 is a front view showing an example of a tangent nonlinear mechanism.

[Explanation of symbols]

1 Non-linear mechanism 10 Non-linear mechanism drive servo system 12 Position controller 14 Speed control system 16 Position loop gain correction value calculator 18 Speed command correction value calculator 20 First-order delay circuit 22 Position loop gain variable setter 24 integrator 26, 28, 30 calculator 32 integrator 34 arithmetic unit 50 Linear reference servo system 52 Position controller 54 Speed control system 56 First-order delay circuit 58 integrator 60 arithmetic unit

─────────────────────────────────────────────────── ─── Continuation of the front page (58) Fields surveyed (Int.Cl. ⁷ , DB name) G05D 3/00-3/20 G05B 11/00-13/04 G05B 19/18 B25J 1/00-21 / 02 B23Q 1/00-1/30

Claims (6)

(57) [Claims] 1. A base member is provided with a rotation about a central axis of a pivot.
Rotatable revolving structure and horizontal straight line on the horizontal guide rail
A lateral slider movably engaged is provided;
Engage with the vertical guide rail of the horizontal slider and move vertically.
By a connecting pin fixed to a movable vertical slider,
The swing body and the lateral slider are pivotally connected to each other, and the base is
Rotated by the servo motor mounted on the member
With the feed nut screwed on the feed screw rod,
By moving the rider linearly in the horizontal direction,
It revolves the revolving body around the central axis of the pivot.
The turning angle from the neutral position (upright position) of the revolving structure
Θ, the distance from the turning center of the revolving structure to the connecting pin
If the separation is R and the linear movement of the feed nut is X,
Of the sine type whose reaching function is represented by θ = sin ⁻¹ (X / R)
In the servo control method of the non-linear mechanism, the estimated value u of X / R is calculated from the rotation angle θm of the servo motor.
Is calculated and the servo control of the servo motor is performed.
The correction value y = √ (1
-U ² ) is changed in proportion to the rotation of the servo motor.
The relationship between the turning angle θm and the turning angle θ of the controlled object is
A servo control method of a non-linear mechanism that controls to be constant. 2. A base member, which is pivotable about a central axis of a pivot.
Rotatable revolving structure and horizontal straight line on the horizontal guide rail
A lateral slider movably engaged is provided;
With the revolving structure by a connecting pin fixed to the lateral slider
The lateral slider is pivotally connected and mounted on the base member.
A feed screw that is driven to rotate by a mounted servo motor
With the feed nut screwed on the rod,
Move the revolving structure to the front by moving it horizontally in a straight line.
It is a device for turning around the central axis of the pivot,
The turning angle from the neutral position (upright position) of the swing structure is θ
The distance from the turning center of the turning body to the connecting pin is R,
When the linear movement amount of the feed nut is X, the transfer function is
Tangent type nonlinear machine represented by θ = tan ⁻¹ (X / R)
In the system servo control method, the estimated value u of X / R is calculated from the rotation angle θm of the servo motor.
Is calculated and the servo control of the servo motor is performed.
The correction value y = 1 + u for the position loop gain of the feedback loop
The rotation angle of the servo motor is changed in proportion to ^2.
The relationship between the ratio of θm and the turning angle θ of the controlled object becomes constant.
Control method of non-linear mechanism to control like. 3. A feedback value is subtracted from the position command.
Obtained by multiplying the deviation obtained by
The speed command of the servo motor
The estimated value u of / R and the reciprocal of the position loop gain correction value y
Compensation by the speed command compensation value calculated according to
Item 3. A servo control method for a non-linear mechanism according to item 1 or 2. 4. A base member is provided with a rotation about a central axis of a pivot.
Rotatable revolving structure and horizontal straight line on the horizontal guide rail
A lateral slider movably engaged is provided;
Engage with the vertical guide rail of the horizontal slider and move vertically.
By a connecting pin fixed to a movable vertical slider,
The swing body and the lateral slider are pivotally connected to each other, and the base is
Rotated by the servo motor mounted on the member
With the feed nut screwed on the feed screw rod,
By moving the rider linearly in the horizontal direction,
It revolves the revolving body around the central axis of the pivot.
The turning angle from the neutral position (upright position) of the revolving structure
Θ, the distance from the turning center of the revolving structure to the connecting pin
If the separation is R and the linear movement of the feed nut is X,
Of the sine type whose reaching function is represented by θ = sin ⁻¹ (X / R)
In the non-linear mechanism servo control device, the estimated value u of X / R is calculated from the rotation angle θm of the servo motor.
And the servo control of the servo motor.
Feedback loop position loop gain correction value y
= (√ (1-u ² ))
Then, the rotation angle θm of the servo motor and the turning angle of the controlled object
A non-linear machine that controls the ratio relationship with the degree θ to be constant.
Servo control device. 5. A base member is provided with a rotation about a central axis of a pivot.
Rotatable revolving structure and horizontal straight line on the horizontal guide rail
A lateral slider movably engaged is provided;
With the revolving structure by a connecting pin fixed to the lateral slider
The lateral slider and is pivotally connected, tower to the base member
A feed screw that is driven to rotate by a mounted servo motor
With the feed nut screwed on the rod,
Move the revolving structure to the front by moving it horizontally in a straight line.
It is a device for turning around the central axis of the pivot,
The turning angle from the neutral position (upright position) of the swing structure is θ
The distance from the turning center of the turning body to the connecting pin is R,
When the linear movement amount of the feed nut is X, the transfer function is
Tangent type nonlinear machine represented by θ = tan ⁻¹ (X / R)
In the servo controller of the structure, the estimated value u of X / R is calculated from the rotation angle θm of the servo motor.
And the servo control of the servo motor.
Feedback loop position loop gain correction value y
= 1 + u ² and means for changing the rotation angle θm of the servo motor and the turning angle of the controlled object.
A non-linear machine that controls the ratio relationship with the degree θ to be constant.
Servo control device. 6. A feedback value is subtracted from the position command.
Obtained by multiplying the deviation obtained by
The speed command of the servo motor
The estimated value u of / R and the reciprocal of the position loop gain correction value y
Means for correcting the speed command correction value calculated according to
6. The servo control of the non-linear mechanism according to claim 4 or 5, wherein
Your device.