Position control device for a rotary axis

Abstract

Position control device (100) having a configuration, - which determines a torque command value of a servo torque through a feedback loop of position and speed and a feedforward loop of speed and acceleration and deceleration torque and - controls a rotation angle of a rotation axis of a numerically controlled machine by a servo motor according to a position command value from a priority device, the position control device (100) comprising: - a signal vector generating unit (1) which generates a signal vector based on the angular acceleration and the angle of rotation thereof on the basis of the actual movement of the rotation axis; - an adaptive loop calculation unit (2), -- which performs adaptive identification of the signal vector and a moment command value of the rotation axis conversion, with respect to an identification model based on an equation of motion of a target apparatus having a rotation axis, the equation of motion having a term relating to the angular acceleration of the rotation axis and a term relating to the angle of rotation, and -- which calculates an estimate of a parameter vector comprising an element relating to a coefficient of a term relating to the angular acceleration of the identification model and an element relating to a coefficient of a term relating to the angle of rotation; - an inertia coefficient calculation unit (5) which calculates a moment of inertia coefficient based on the element relating to the coefficient of the term relating to the angular acceleration in the estimated value of the parameter vector; and - a calculation unit for the nonlinear element (6) that calculates the nonlinear element moment compensation based on the element related to the coefficient of the term involving the rotation angle of the estimated value of the parameter vector and the rotation angle of the rotation axis, wherein the position control device (100) corrects the moment command value by the inertia moment coefficient and the nonlinear moment compensation.

Description

BACKGROUND OF THE INVENTIONField of the invention

[0001] The present invention relates to a position control device applied to the rotation axis of a numerically controlled machine. Description of the state of the art

[0002] Fig. 3A and Fig. 3B are diagrams showing an example of a mechanical model of a rotary axis, which is a control target apparatus. A rotary table 50 is rotated by a servomotor (not shown) about a Zu axis by a rotation angle θ. The intersection point between the Zu axis and a rotation locus plane described by a center of gravity G of the rotary table 50 is represented by an origin point Ou, an Xu axis is recorded in a direction perpendicular to the rotation locus plane with respect to gravity, and a remaining Yu axis is recorded such that a right-handed coordinate system is formed including the Zu axis and the Xu axis.

[0003] The center of gravity G is located at a distance L from the origin point Ou and at the position of angle α when θ = 0. Different types of jigs / tools and workpieces are arranged on the rotary table 50 according to respective machining methods, so the load state changes and the position of the center of gravity G also fluctuates. Note that g represents the gravitational acceleration, and angle b is an angle formed between the Yu axis and a plane perpendicular to the direction of gravity.

[0004] Fig. 4 is a block diagram showing an example of a conventional position control device 200 for controlling the above-mentioned rotation angle θ of the rotation axis to a position command value θc generated in an override device (not shown).

[0005] This device has a feedforward configuration for increasing the speed of a command response. Specifically, the position command value θc is time-differentiated by a differentiator 54 to become a feedforward velocity Vf, and Vf is time-differentiated by a differentiator 55 to become a feedforward acceleration Af. A gain Cb in an amplifier Cb is a constant that determines a feedforward acceleration and deceleration torque τf, which corresponds to a motor torque converted to the rotation axis for generating the acceleration Af in the rotation axis. Typically, Cb corresponds to the sum of the inertia of the transmission system, including a motor whose inertia has been rotation-axis converted, and the inertia of the turntable 50 itself without any objects mounted thereon.

[0006] The feedforward configuration of the conventional position control device is as follows. First, a rotation angle θ detected by a rotation angle detector (not shown) is subtracted from the position command value θc by a subtractor 51, and the position error, which is the output thereof, is amplified by a position error amplifier Gp. Further, the output thereof is added to the feedforward speed Vf by an adder 52 to become a speed command value Vc. A subtractor 53 subtracts from the speed command value Vc a rotation angular speed ω in which the rotation angle θ has been differentiated by a differentiator 56, and the speed error, which is the output thereof, is usually proportionally integrally amplified by a speed error amplifier Gv.This output and the feedforward and retard torque τf are added together by an adder circuit 57 to become a rotation-axis-converted torque command value τc, which is Ct-amplified by a power amplifier Ct. Ct is a constant determined according to the servomotor characteristics; this output τ becomes the rotation-axis-converted generated torque of the servomotor, and the rotary table 50 is driven.

[0007] As described above, in the conventional position control device, an overall configuration in which feedforward control is added with respect to a nominal linear characteristic to increase the speed of a command response is used as feedback control to compensate for a nonlinear characteristic resulting from gravity and to ensure the internal stability of the control system. However, as mentioned above, since various types of jigs / tools and workpieces are arranged on the rotary table, the moment of inertia itself increases beyond that of the table, and the center of gravity changes.When this occurs, the feedforward acceleration and deceleration torque τf is insufficient, the feedback control band reduces, and the nonlinear characteristic resulting from gravity increases, so that the controllability deteriorates, resulting in a drop in positioning performance and response variations resulting from the positioning angle during positioning.

[0008] Furthermore, since these fluctuation factors cannot be detected, acceleration conditions and speed conditions resulting from centrifugal force could not be evaluated for function generation of the position command value θc on the priority device side. As a result, efficient function generation was not possible. Furthermore, during a direct drive application that does not have a deceleration mechanism, the aforementioned fluctuation factors experience a relative increase, which tends to exacerbate these problems.

[0009] US Pat. No. 6,184,644 B1 discloses a device for controlling the position of an object driven by a servomotor via a ball screw drive system. In the device, parameters of the ball screw drive system are determined from a position command value, an acceleration command value, a torque command value, and a motor speed, based on which a dynamic deflection amount is calculated while the object is driven at an adjustable speed. To compensate for this dynamic deflection, a deflection compensation command section calculates a deflection compensation value, a deflection velocity compensation value, and a deflection torque compensation value. These compensation values ​​are individually added to the corresponding sections in the position control device and together compensate for the dynamic deflection.

[0010] US 6,861,816 B2 discloses a position control device for controlling the position of a controlled object using a motor. In this device, when generating a torque command value serving as a command value for the servomotor of the controlled object system, a friction compensation value for use when the controlled object initiates a movement is calculated based on a speed command value and a torque command value determined according to a position command value supplied from a higher-level device. JP 2002-366230 A discloses a position controller with a backlash correction function that operates accurately regardless of an operating state of a controlled object in a fully closed-loop position controller for controlling the position of the controlled object using a servomotor.This position controller is provided with a backlash correction operation part that sorts the conditions for backlash occurrence in operation from a speed command value, an acceleration command value and a torque command value, distinguishes the presence / absence of backlash occurrence in operation with a reference matching the respective conditions, and determines a backlash correction value with accurate direction and magnitude.

[0011] Furthermore, JP 2000 - 172 341 A discloses a servo controller and a control method for a linear motor, and JP H11 - 184 529 A discloses a servo control method and servo control system for a feed drive system. SUMMARY OF THE INVENTION

[0012] Regarding these problems in the prior art, it is an object of the present invention to provide a position control device in which feedback control or regulation and feedforward control or regulation, which are suitable for responding to changes in the moment of inertia and the center of gravity, are configured.

[0013] The position control device of the present invention determines each parameter of an identification model from an error between the actual movement of a rotary axis of a numerically controlled machine, which is a control target, and a movement calculated by the identification model, in which the control target is represented by a mathematical model, and corrects the input to the control target based on these parameters.

[0014] The identification model is an equation of motion that describes the motion of the rotation axis and includes a term related to the angular acceleration and a term related to the rotation angle. With respect to this identification model, the position control device identifies the parameters of the identification model based on the actual angular acceleration and rotation angle of the rotation axis of the control target and a torque command value related to the numerically controlled machine. Furthermore, the position control device uses the identified parameters to correct the torque command value and perform control consistent with the current state of the control target.

[0015] Specifically, in the position control device of the present invention, the identification model includes the term corresponding to the rotation angle. Consequently, the position control device can correct the torque command value in accordance with the load fluctuating with the rotation angle. For example, when the center of gravity of the control target shifts from the rotation axis line and this rotation axis line is not vertical, the load fluctuates with the rotation angle due to the gravity acting on the center of gravity. Even with such a control target, the position control device can perform control that follows the load fluctuation. BRIEF DESCRIPTION OF THE DRAWINGS Fig. 1 is a block diagram illustrating the configuration of a position control device of a rotary axis of an embodiment of the present invention; Fig. 2 is a block diagram describing the configuration of an adaptive loop calculation unit of the embodiment; Fig. 3A and Fig. 3B is a general mechanical model of a rotary axis, which is a targeting device; and Fig. 4 is a block diagram showing an example configuration of a conventional position control device of a rotary axis. DETAILED DESCRIPTION OF THE INVENTION

[0016] An embodiment of the present invention will be described below with reference to the drawings.

[0017] With regard to the target apparatus of Fig. 3A and Fig. 3B, its equation of motion is derived. In this case, it is sufficient to take the angle of rotation θ as the generalized coordinate system, and an equation of motion (1) is obtained. τ=(ML2+Iz+Im)(dω / dt)+M⋅g⋅L⋅SINb⋅COS(θ+a)

[0018] Here, M is the mass of the entire rotary table, including the various types of fixtures / tools and workpieces arranged and fixed on it, Iz is the moment of inertia of the entire rotary table about an axis parallel to the Zu axis passing through the center of gravity G, and Im is the rotation axis-converted moment of inertia of the transmission system including motor.

[0019] Now, a three-dimensional signal vector ξ and a parameter vector γ are defined by expressions (2) and (3) (hereafter, the transposition of matrix and vectors is defined by appending the exponent “ τ “ shown). ξ=[dω / dt,COSθ,−SINθ]τ γ=[ML2+Iz+Im,MgLSbCa,MgLSbSa]τ

[0020] (Here: Sb = SINb, Ca = COSa and Sa = SINa).

[0021] The equation of motion of expression (1) can be expressed by the actual system model of expression (4). τ=Ct⋅τc=ξτγ

[0022] Fig. 1 is a block diagram illustrating the configuration of a position control device 100 according to the present invention. Note that in this block diagram, the same names and reference numerals are assigned to the same parts as those in Fig. 4 (conventional example), and a repeated description thereof is omitted. An adaptive loop calculation unit 2 is a block that calculates an identification parameter vector γ ID which is an estimate of the parameter vector γ of the torque command value τc and the signal vector ξ.

[0023] Fig. Figure 2 is a block diagram illustrating the configuration of the adaptive loop calculation unit 2. A generalized adaptive law 21 calculates the identification parameter vector γ IDby the well-known expressions (5) and (6) (Introduction to Robust Adaptive Control, published by Ohmsha, p. 62, etc.). (Hereinafter, (k) represents the signal of the k-th cycle in a time series signal per a certain sample time). γID(k)=γID(k−1)−P(k−1)⋅ξ(k)⋅ξ(k)τΦ(k) P(k)−1=C1(k)⋅P(k−1)−1+C2(k)⋅ξ(k)ξ(k)τ

[0024] Here applies: 0 <C 1 (k)≤1, 0≤C 2 (k)<2, P(0)>0.

[0025] The signal vector ξ(k) and the identification parameter vector γ ID (k) are multiplied in a multiplier 22 to obtain a scalar signal ξ(k) τ γ ID (k). The signal obtained when the torque τ(k) generated by the servo motor is subtracted from this scalar signal ξ(k) by a subtractor 23 τ γ ID (k) is subtracted, a scalar following error ξ(k) τ Φ(k). In other words, the scalar following error ξ(k) τΦ(k) can be expressed by expression (7), which also uses expression (5), and consequently using γ ID (k-1) can be calculated from a previous cycle. ξ(k)τΦ(k)=ξ(k)τγID(k)−τ(k)=ξ(k)τ{γID(k−1)−P(k−1)⋅ξ(k)⋅ξ(k)τΦ(k)}

[0026] Here, the matrix K(k) of expression (8), in which the signal vector ξ(k) τ arranged in a row direction in a time series. K(k)=[ξ(1),ξ(2),...,ξ(k)]τ

[0027] It is known that if the matrix K(k) has continuous full column rank, k → ∞, γ ID (k) → γ and a parameter identification is established, and if the change in the parameter vector γ is sufficiently smaller than the approximate response, the identification parameter vector γ ID (k) following the change is obtained.

[0028] In expression (6), since P(k)>0 is guaranteed for any value of k, the singular value σ(P(k)) of P(k) coincides with the eigenvalue λ(P(k)). For this reason, if the matrix K(k) of expression (8) continues to lose a full column rank from a certain point in time, then if C 1 (k)<1, the expression (6) that λmax(P(k)) → ∞. Since this means that a certain element {pij} of P(k) → ∞, this eventually leads to P(k) becoming uncomputable. From the above, it is clear that even if the matrix K(k) continues to lose full column rank, P(k) remains bound and C 1 (k) → 1 is equivalent.

[0029] In this example, a constant tracking algorithm is used within the well-known generalized adaptive law. This is because, if tr[P(k)]>λmax(P(k)), P(k) can remain bound if regulated as tr[P(k)]=tr[P(0)]:(a specific value) by an arbitrary k. For this, C 1 determined by expression (9). C1(k)=1−[|P(k−1)ξ(k)|2 / {C+ξ(k)τP(k−1)}]⋅[1 / tr{P(0)}]

[0030] Here: C = C 1 (k) / C 2 (k) : (specific value) > 0.5.

[0031] Then, since P(k) remains bound even if the matrix K(k) continues to lose full column rank for the reason mentioned above, C 1(k) → 1 and P (k-1)ξ(k) → 0 from expression (9); consequently, it is clear that the parameter identification process of expression (5) stops. Conversely, when the matrix K(k) regains its full column rank, λmax(P(k)) becomes smaller from expression (6) and tr{P(k)} is constant, causing other eigenvalues ​​to become larger. Then, C 1 (k) in expression (9) is less than 1, and the parameter identification process of expression (50) is retrieved and executed.

[0032] Returning to Fig. 1, the operation of the above embodiment will be described. A signal vector generation unit 1 performs a trigonometric function calculation with respect to the rotation angle θ, calculates COSθ and -SINθ, differentiates the rotation angular velocity ω, and determines dω / dt. In other words, the signal vector generation unit 1 becomes a block that generates a source signal of the signal vector ξ in expression (2). Here, this output is multiplied in a multiplier 4 by a binary (1 or 0) output IDC from a parameter identification control unit 3 and becomes a signal vector ξ, which is the input of an adaptive loop calculation unit 2.

[0033] Next, the operation of the parameter identification control unit 3 is described. The motion equation (1) does not include errors and noise applied to the target device. For this reason, if noise or errors are present, an error will occur in the identification result. Furthermore, the parameter identification process will not work if the matrix K(k) of expression (8) cannot guarantee full column rank, which makes dω / dt ≠ 0 in the signal vector ξ of expression (2) a required condition for parameter identification. In addition, as for the power amplifier Ct in Fig. 1, the controllability decreases as the rotational angular velocity ω increases, and it becomes difficult for the power amplification of Ct times to be accurately realized.

[0034] From the above, the parameter identification control unit 3 applies the logical expressions of the set of expressions (10) to (12) with respect to the feedforward speed Vf, the feedforward acceleration Δf, and the binary signal for parameter identification control OIDC from the priority device, outputs the binary signal IDC, and controls the execution / stop of the parameter identification process. If expression (12) is satisfied, 1 is output as IDC. cond1=(0 <Vf≤Vmax)∩(0<Af) cond2=(−Vmax≤Vf<0)∩(Af<0) IDC=(cond1Ucond2)∩OIDC

[0035] Here, Vmax (>0) is a speed limit value at which the power amplifier Ct performs the power amplification of Ct times accurately, cond1 extracts an acceleration state in the plus direction, and cond2 extracts an acceleration state in the minus direction. A deceleration state causes the identification process to stop in such a way that the control parameters (later described as G ID and τfn) do not change during positioning. As for the binary signal for parameter identification control (OIDC), there is processing and contact from the external environment to the turntable 50, and if it is assumed that a disturbance is applied, the parameter identification process can be overridden by outputting 0 from the override device.

[0036] A first element of the identification parameter vector γ ID the adaptive loop calculation unit 2, an identification value (ML 2 +Iz+Im) IDof the moment of inertia of the equation of motion (1) in the target apparatus of expression (3). A moment of inertia coefficient calculation unit 5 uses this as input to calculate a moment of inertia coefficient G ID by expression (13). GID=(ML2+Iz+Im)ID / (Cb⋅Ct)

[0037] The moment of inertia coefficient G ID is multiplied by a multiplier 8 with an output of an adder 57. For this reason, the feedforward acceleration and deceleration torque τf becomes a suitable quantity corresponding to the linear part of the first term of the right side in the equation of motion (1).

[0038] Furthermore, if Gv(s) represents the transfer characteristic of a speed error amplifier Gv initially set in accordance with the moment of inertia of the transmission system including the motor and the turntable 50 alone, it is required that the transfer characteristic Gv'(c) applying a certain loop characteristic for speed control satisfies expression (14). Gv'(s) / (ML2+Iz+Im)=Gv(s) / (Cb⋅Ct)

[0039] Consequently, as for multiplying the moment of inertia coefficient G ID from expression (13), the transfer characteristic of the speed error amplifier Gv G ID Gv(s) and agrees with Gv'(s) of Expression (14), so the loop characteristic for velocity control is made constant. In other words, the feedback control band encompassing the position loop is made constant.

[0040] Second and third elements of the identification parameter vector γ ID the adaptive loop calculation unit 2 (MgLSbCa) ID and (MgLSbSa) ID of Expression (3). A nonlinear element calculation unit 6 performs trigonometric function calculations with respect to the position command value θc, calculates COSθc and -SINθc, and determines the nonlinear element moment compensation τf by calculating Expression (15) with the second and third elements of the identification parameter vector γ ID . τfn=(1 / Ct){(MgLSbCa)ID⋅COSθc+(MgLSbSa)ID⋅(−SINθc)}

[0041] Here, Ct·τfn can be considered as MgLSb·COS(θc+a), thus becoming an estimate of a second term on the right-hand side, which is a nonlinear element of the equation of motion (1). Consequently, by adding τfn to the output of the multiplier 8 through an adder 9, feedforward compensation with respect to the nonlinear element can be achieved.

[0042] A condition information calculation unit 7 performs the calculation of expression (16) from the identification parameter vector γ ID by the adaptive loop calculation unit 2. (MgLSb)ID={(MgLSbCa)ID2+(MgLSbSa)ID2}1 / 2

[0043] (MgLSb) ID means the maximum amplitude of the nonlinear element, which allows the calculation of expression (17) with a rotation axis converted generated moment limit τ LIM of the servo motor, which is already known information. ALIM={τLIM−(MgLSb)ID} / (ML2+Iz+Im)ID

[0044] A LIM represents the maximum angular acceleration that the position control device can achieve under the current load condition. Next, since the gravitational acceleration g and the angle b are already known, expression (18) can be derived from (MgLSb) ID can be calculated from expression (16). ML ID can be described as (mass) x (distance between the center of gravity and the center of rotation) under the current loading condition. MLID=(MgLSb)ID / (gSb)

[0045] The condition information calculation unit 7 uses A LIM and ML ID calculated as condition information under the current load condition and outputs it to the priority device. The function generation of the position command value θc is performed within the range of these pieces of control information.

[0046] ML ID ω 2 , which consists of the angular velocity ω and ML ID , which is output by the position control device according to the invention, can be referred to as the centrifugal force exerted on the Zu-axis, ie, the rotational center axis of the turntable 50. For this reason, the priority device can determine the maximum angular velocity V LIM which can be allowed by the tolerated radial load of the drive unit, and the optimal function generation of the position command value θc corresponding to the load condition can be carried out in order to operate at the above-mentioned maximum angular velocity A LIM to agree.

[0047] According to the present embodiment, the feedback control band is made constant with respect to fluctuations in the center of gravity and the moment of inertia in the rotation axis of a numerically controlled machine. Furthermore, with respect to the linear characteristic and the nonlinear characteristic, the feedforward control is made appropriate, so that controllability does not deteriorate and a drop in positioning performance and response variations resulting from the positioning angle during positioning can be mitigated. Furthermore, acceleration conditions corresponding to the load condition and speed conditions resulting from the centrifugal force can be incorporated on the priority device side, thereby enabling optimal function generation of the position command θc.Furthermore, the resulting control effect becomes even more remarkable in the direct drive application, which has no deceleration mechanism.

Claims

[1] Position control device (100) having a configuration, - which determines a torque command value of a servo torque through a feedback loop of position and speed and a feedforward loop of speed and acceleration and deceleration torque and - controls a rotation angle of a rotation axis of a numerically controlled machine by a servo motor according to a position command value from a priority device, the position control device (100) comprising: - a signal vector generating unit (1) which generates a signal vector based on the angular acceleration and the angle of rotation thereof on the basis of the actual movement of the rotation axis; - an adaptive loop calculation unit (2), -- which performs adaptive identification of the signal vector and a moment command value of the rotation axis conversion, with respect to an identification model based on an equation of motion of a target apparatus having a rotation axis, the equation of motion having a term relating to the angular acceleration of the rotation axis and a term relating to the angle of rotation, and -- which calculates an estimate of a parameter vector comprising an element relating to a coefficient of a term relating to the angular acceleration of the identification model and an element relating to a coefficient of a term relating to the angle of rotation; - an inertia coefficient calculation unit (5) which calculates a moment of inertia coefficient based on the element relating to the coefficient of the term relating to the angular acceleration in the estimated value of the parameter vector; and - a non-linear element calculation unit (6) that calculates the non-linear element moment compensation based on the element relating to the coefficient of the term relating to the rotation angle of the estimated value of the parameter vector and the rotation angle of the rotation axis, wherein the position control device (100) corrects the moment command value by the moment of inertia coefficient and the non-linear moment compensation. [2] The position control device (100) according to claim 1, further comprising a parameter identification control unit (3) that controls whether or not to perform adaptive identification of the adaptive loop calculation unit (2), wherein the parameter identification control unit (3) performs control to perform adaptive identification when the absolute value of the feedforward speed is equal to or less than a predetermined value and a value of feedforward acceleration is not 0. [3] The position control device (100) according to claim 1, further comprising a condition information calculation unit (7) that calculates a maximum angular velocity that the position control device (100) can achieve under current conditions on the basis of each element of the estimated value of the parameter vector and outputs a predetermined generated torque limit value of the servo motor and this achievable maximum angular velocity to the override device of the position control device (100). [4] The position control device (100) according to claim 1, further comprising a condition information calculation unit (7) which calculates the product of the mass of the target apparatus and the distance between the center of gravity and the rotation center axis on the basis of the element relating to the coefficient of the term relating to the rotation angle of the estimated value of the parameter vector, gravitational acceleration and inclination angle of the rotation axis and outputs this to the priority device as the condition information.

Images