JP2869281B2 - Motor drive system controller - 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 device for a motor drive system, and more particularly, to a control device for a motor drive system that controls a motor drive system including a motor and a mechanical unit driven by the motor. Things.

[0002]

2. Description of the Related Art FIG. 4 shows a conventional motor drive system controller A.
It is a block diagram showing a schematic structure in an example of ₀₁ (A ₀₂ ). As shown in FIG. 4, a conventional motor drive system control device A ₀₁ (A ₀₂ ) includes a motor drive system including a motor 1 and a mechanical unit 3 driven by the motor 1 via a spring element 2 such as a gear. As the control target 4, the generated torque or torque command value τ of the motor 1 and the motor angle θ _M and the motor angular velocity θ _M ′ of the motor 1 output from the control target 4 are input, and the angle θ _{M of the} motor 1 and the motor The control object 4 is modeled using a state equation in which the angular velocity θ _M ′, the joint angle θ _L of the mechanical unit 3, and the joint angular velocity θ _L ′ are state variables, and the generated torque of the motor 1 or the torque command value τ is an input term. And an observer 5 for estimating the joint angle θ _L and the joint angular velocity θ _L ′ of the mechanical section 3. In the motor drive system controller A ₀₁ (A ₀₂ ), the joint angle θ _L and the joint angular velocity θ _L ′ of the mechanical section 3 estimated by the observer 5 are fed back and input to the control target 4.
The basic principle of the device A ₀₁ (A ₀₂ ) will be briefly described below.
Generally, the equation of motion of a motor drive system can be expressed by the following equation. _{J L θ L "+ B L} (θ L ', θ L) θ L' + Kθ L -Kθ M + C (θ L ', θ L) + d = 0 ... (1a) J M θ M" + B M θ M' + Kθ to _{M -Kθ L = τ ... (1b} ) here, θ _M: motor angle, θ _M ': motor angular velocity θ _M ": motor angular acceleration, θ _L: joint angle θ _L': joint angular velocity, θ _L": joint angular acceleration J _M: motor inertia, J _L: link inertia B _M: viscosity coefficient of the motor, B _L: term due to joint angle velocity theta _L 'such as viscosity of the link, K: spring constant of the joint tau: motor the generated torque or torque command value d: load torque, such as load fluctuation, C: was assumed to be negligible any disturbance factor in non-linear terms in the conventional apparatus a _01, the motor drive system interference or gravity or the like of each axis ( Special Notice 63-183
No. 9). That is, it is based on the following state equation (2a) or (2b) in which the nonlinear term C and the load torque d such as load fluctuation in the above equations (1a) and (1b) are omitted.

(Equation 3) Then, by applying a state estimation observer of the same dimension or the minimum dimension to the state equation (2a) or (2b), the joint angle θ _L and the joint angular velocity θ _L ′ corresponding to the load torque applied to the mechanical unit 3 are obtained. And feedback control is performed using the estimated values θ _L1 and θ _L1 ′ (conventional device A ₀₁ ). However, since a disturbance element (C + d) is actually added to the mechanical unit 3, it is difficult to secure the estimation accuracy of the state quantity of the mechanical unit 3, and accordingly it is difficult to increase the feedback gain. Therefore, the disturbance element (c +
considered device A ₀₂ to d) have been developed (JP-A-1-29
No. 6301). In this case, the above (1a), (1b)
A disturbance equation (c + d) in the equation is used as a disturbance torque D to form a state equation. That is, assuming that the disturbance torque D is an offset disturbance (D'≡0), the above (2a),
The state equation corresponding to the equation (2b) can be expressed by the following equation.

(Equation 4) Then, by applying a state estimation observer of the same dimension or the minimum dimension to the state equation (3a) or (3b), the joint angle θ _L and the joint angular velocity θ _L corresponding to the load torque applied to the mechanical unit 3 are obtained. And the disturbance torque D, and feedback control is performed using the estimated values θ _L1 , θ _L1 ′, and D ₁ (conventional device A ₀₂ ). still,
In the above equations, the parameter τ is the input amount X, the parameter θ
_M and θ _M ′ correspond to the output amount Y, and the parameters θ _L1 (or ε ₁ ) and θ _L1 ′ correspond to the feedback amount Z, respectively. As described above, in _the conventional apparatus _A02 , by taking into account the disturbance element (c + d), the estimation accuracy of the state quantity of the mechanical unit 3 (estimation accuracy of the feedback amount Z) is increased, and the feedback gain is increased to maintain a constant control performance. Was secured.

[0003]

In the conventional motor drive system control device _A02 described above, although a certain control performance can be secured by estimating the disturbance element (c + d) by the observer 5, the control object is controlled. Known information such as the non-linearity and interference of each axis, which is a problem in robots and NCs, is also estimated together with unknown disturbance elements, and this estimated value is used for feedback control. Therefore, there is room for further improving the estimation accuracy of the feedback amount by effectively utilizing the known information. SUMMARY OF THE INVENTION In order to solve the problems in the prior art, the present invention improves a motor drive system control device and improves the accuracy of estimating a feedback amount to perform more accurate control. The purpose is to provide.

[0004]

Means for Solving the Problems To achieve the above object, the invention according to claim 1 comprises a motor and a mechanical unit driven by the motor , wherein the motor and the mechanical unit are connected to each other.
A motor drive system, which is coupled via a spring element and receives the generated torque or command torque of the motor and the non-linear term of the load torque of the mechanical unit, and outputs the motor angle and motor angular velocity of the motor. To be controlled,
The generated torque or command torque of the motor input to the controlled object, the motor angle and motor angular velocity of the motor output from the controlled object, and the nominal value of the nonlinear term of the load torque of the mechanical unit are input. , Motor angle of the motor, motor angular velocity, joint angle of the mechanical part,
Using the joint angular velocity and the unknown disturbance torque as state variables, the control target is determined using a state equation in which the generated torque or command torque of the motor and the nominal value of the nonlinear term of the load torque of the mechanical section are input terms. Estimating means for modeling and estimating joint angles, joint angular velocities, and unknown disturbance torques of the mechanical unit are provided.
Spring force of the spring element generated by displacement with the motor
And at least the nonlinear term act on the motor,
And the driving force generated by the motor itself is small.
The force acting on the mechanical part and the inertia of the mechanical part
And the force acting on the motor and the motor.
The control model from the inertia force
And the nominal value of the nonlinear term in the control model
Is used as an input acting on the mechanical part, and
A disturbance force acts together with the spring force and the nonlinear term.
As such, the driving force is used as an input acting on the motor.
The estimating means is configured based on the control model.
And a feedback device configured to feed back the joint angle, the joint angular velocity, and the unknown disturbance torque of the mechanical unit estimated by the estimating means to the controlled object. Have been. Ma
The invention according to claim 2 is characterized in that the motor and the motor
The motor and the mechanical part are driven.
The torque generated by the above motor is
Or the command torque τ, and the load torque
The nonlinear term C is input, and the motor angle θ _M
And the motor drive system that outputs the motor angular velocity θ _M ′
The motor to be controlled
The raw torque or command torque τ and the
Motor angle θ _M and motor angular velocity of the above motor
θ _M ′ and the nominal value of the nonlinear term of the load torque of the mechanical part
C 'and is inputted, the motor angle theta _M, Mo of the motor
Angle velocity θ _M ′, joint angle θ _L of the above mechanical part , joint angular velocity
Degree θ _L ′, and the modeling error C−C ′ of the nonlinear term
Unknown disturbance torque including load torque d due to load fluctuation
D is the state variable, and the torque generated by the motor or the finger
Command torque τ and the non-linear term of
Expression (6a) or Expression (6b) using the nominal value C ′ as an input term
The above controlled object is modeled using the state equation expressed by
And the joint angles, joint angular velocities, and unknown
Estimating means for estimating the disturbance torque is provided.
The joint angle, joint angular velocity, and
Feedback unknown disturbance torque to the above control target
Control device for a motor drive system characterized by inputting
Place.

(Equation 5) However, J _M: motor inertia, J _L: Link Inertia B _M: viscosity coefficient of the motor, B _L: Links term due to the viscosity coefficient joint angle velocity theta _L like ', K: spring constant of the joint epsilon: Distortion The quantity (= θ _L −θ _M ) The invention according to claim 3 is described in claim 1 or 2 above.
In the control device of the motor drive system described above, the control target is a plurality of motors and a spring element by each motor.
And a plurality of mechanical units driven through the control unit. The estimating unit calculates the generated torque or command torque of each motor input to the control target and the motor angle of each motor output from the control target. and a motor angular velocity, and the nominal value of the nonlinear term of the load torque of each machine section are respectively inputted, the motor angle of each motor, the motor angular velocity, joint angle of each machine section, joint angular velocity, and states the unknown disturbance torque The controlled object is modeled by using the above-mentioned equation of state, in which the input torque and the generated torque or command torque of each motor and the nominal value of the non-linear term of the load torque of each machine are used as variables. a joint angular velocity, and the controller of the motor drive system characterized by comprising estimating the unknown disturbance torque. Claims
According to a fourth aspect of the present invention, a motor and a mechanical unit driven by the motor via a spring element having a non-linear characteristic are input with a generated torque or a command torque of the motor, and a motor angle and a motor angular velocity of the motor are input. The motor drive system that outputs the control target is a control target, and the generated torque or command torque of the motor input to the control target and the motor angle and motor angular velocity of the motor output from the control target are input. The state angle includes the motor angle of the motor, the motor angular velocity, the joint angle and the joint angular velocity of the mechanical part in the state variables , and uses the state equation including the generated torque or the command torque of the motor in the input term and the spring included in the state equation. The constant is expressed as a nonlinear function of the amount of distortion based on the motor angle of the motor and the joint angle of the mechanical part. Modeling the controlled object, the joint angle of the machine unit, comprising estimation means for estimating joint angular velocity, the said machine unit, the displacement between the machine part and the motor
Therefore, the generated spring force of the spring element and the nonlinear term are small.
At the same time as the reaction force of the spring force and the motor.
At least the driving force generated by the motor itself acts.
Balance between the force acting on the mechanical part and the inertial force of the mechanical part
Between the force acting on the motor and the inertia force of the motor
The control model is given from the relationship and the control model.
Dell states that the mechanical part has the spring force and the nonlinear term
The driving force is applied to the motor
Control model
Said estimating means is configured based on the joint angle of the machine part that is estimated by the estimating means, as a control device for by feeding back the joint angular velocity motor drive system characterized by comprising input to the controlled object It is configured.
Further, the invention according to claim 5 provides a motor and a non-linear characteristic.
Driven by the motor via a spring element K having
And the torque generated by the motor or the command
The torque τ is input, and the motor angle θ _M and
Control the motor drive system that outputs the motor angular velocity θ _M ′
Of the motor, which is input to the object to be controlled,
Torque or command torque τ and the output
Motor angle θ _M and motor angular velocity
theta _M 'and is inputted, the motor angle of the motor theta _M, Mo
Over motor angular velocity theta _M ', and the joint angle theta _L of the machine part, Seki
The nodal angular velocity θ _L ′ is included in the state variable, and the motor
Equation (6a) including the torque or command torque τ in the input term
Or using the equation of state expressed by (6b)
The spring constant K included in the equation of state is
Strain ε based on the angle θ _M and the joint angle θ _L of the mechanical part
The above controlled object is modeled as a nonlinear function of
Estimate the joint angle θ _L and joint angular velocity θ _L ′ of the above mechanical part
Estimating means, and the estimating means
Feedback on the joint angle and joint angular velocity of the mechanical part
Motor drive characterized by being input to the controlled object
It is configured as a system control device.

(Equation 6) However, J _M: motor inertia, J _L: Link Inertia B _M: viscosity coefficient of the motor, B _L: term due to the viscosity coefficient joint angle velocity theta _L like 'link, K: spring constant of the joints d: load load torque such variation, C: nonlinear term C interference or gravity or the like of each axis': nominal value of the nonlinear terms, D: unknown disturbance torque epsilon: strain amount (= θ _L -θ _M) also claim The invention according to claim 6 relates to any one of claims 1 to 5 described above.
2. The control device for a motor drive system according to claim 1, wherein a dynamic characteristic of a current loop of the motor is used for estimating the feedback amount by the estimating means. .

[0005]

According to the present invention, a motor and a mechanical unit driven by the motor are provided.
The motor drive system, which receives the generated torque or command torque of the motor and the non-linear term of the load torque of the mechanical part, and outputs the motor angle and motor angular velocity of the motor, Controlled. Then, using the motor angle of the motor, the motor angular velocity, the joint angle of the mechanical part, the joint angular velocity, and the unknown disturbance torque as state variables, the generated torque or command torque of the motor and the nonlinear term of the load torque of the mechanical part are obtained. Using the nominal value as an input term , for example, the above equation (6a) or (6b)
The control object is modeled using a state equation as described above, and the joint angle, joint angular velocity, and unknown disturbance torque of the mechanical unit are estimated by the estimating means. Therefore, by effectively utilizing known information such as the nonlinearity of the control target and the interference of each axis, which is a problem in robots, NCs, and the like, which has been conventionally estimated by the estimation means as a disturbance element, the state variable by the estimation means is effectively used. Can be improved. With the improvement of the estimation accuracy, the state feedback gain of the motor drive system can be increased, and the motor drive system can be controlled more accurately. Moreover, in addition to the non-linear term of the load torque, unknown disturbance torque due to, for example, a modeling error or a load fluctuation is added to the mechanical part to be controlled. It is estimated by the estimation means together with the joint angular velocity.
Therefore, it is not estimated together with the unknown disturbance torque due to the modeling error or the load fluctuation.
The estimation accuracy of the state variables can be improved. Further, the controlled object includes a plurality of motors and a plurality of mechanical parts driven by the respective motors via respective spring elements, and the estimating means determines the generated torque of each motor input to the controlled object. Alternatively, the command torque, the motor angle and motor angular velocity of each motor output from the control object, and the nominal value of the non-linear term of the load torque of each mechanical unit are input, respectively, and the motor angle, motor angular velocity, The joint angle, joint angular velocity,
The controlled object is modeled using the above- mentioned state equation in which the input torque and the generated torque or command torque of each motor and the nominal value of the nonlinear term of the load torque of each mechanical section are input variables. Then, the joint angle, the joint angular velocity, and the unknown disturbance torque of each mechanical part are estimated. Thus, for example, the robot of the motor drive system,
Instead of applying the estimating means to each axis such as NC, all axes are treated as one system and all axes are covered by one estimating means, so that vibrations caused by mechanical resonance of other axes are included. Can be estimated. Therefore, the estimation accuracy can be improved for a motor drive system including a plurality of motors and a plurality of driven parts. Further, the motor includes a motor and a mechanical unit driven by the motor via a spring element having a non-linear characteristic. The generated torque or command torque of the motor is input, and the motor angle and motor angular velocity of the motor are output. The motor drive system is the control target,
The generated torque or command torque of the motor input to the controlled object and the motor angle and motor angular speed of the motor output from the controlled object are input, and the motor angle, motor angular speed, and the mechanical unit of the motor are input. joint angle, joint angular velocity comprises the state variables, including for example, the formula input section for generating a torque or the command torque of the motor of
A state equation such as (6a) or (6b) is used, and a spring constant included in the state equation is expressed as a non-linear function of a distortion amount based on a motor angle of the motor and a joint angle of a mechanical unit. The joint angles and joint angular velocities of the mechanical part, which are modeled and estimated by the estimating means, are fed back and input to the control object. As described above, the control object is modeled by expressing the spring constant included in the state equation as a non-linear function of the amount of distortion based on the motor angle of the motor and the joint angle of the mechanical unit, and thereby the hysteresis of the spring element, Non-linear characteristics such as backlash and dead zone can be specifically taken into the estimating means, and the accuracy of estimating the feedback amount can be improved. Further, the dynamic characteristics of the current loop of the motor are used for estimating the joint angle and the joint angular velocity of the mechanical part by the estimating means. In this way, by incorporating the dynamic characteristics of the current loop of the motor into the estimating means, the joint angle of the mechanical part,
It is possible to improve the estimation accuracy of a state variable such as a joint angular velocity.

[0006]

Embodiments of the present invention will be described below with reference to the accompanying drawings to provide an understanding of the present invention. The following embodiments are examples embodying the present invention, and do not limit the technical scope of the present invention. here,
FIG. 1 is a block diagram showing a schematic configuration of a motor drive system controller A2 according to one embodiment of the present invention and a motor drive system controller A1 as a reference example. FIG. 2 is a block diagram showing another embodiment of the present invention. FIG. 3 is a block diagram showing a schematic configuration of a motor drive system controller A1 '(A2'), and FIGS. 3A and 3B are graphs (a) and (b) showing nonlinear characteristics of a spring element. Elements common to those in the block diagram showing the schematic configuration of an example of the conventional motor drive system control device A ₀₁ (A ₀₂ ) shown in FIG. 4 are denoted by the same reference numerals. As shown in FIG. 1, a motor drive system controller A1 described as a reference example and a motor drive system controller A2 according to one embodiment of the present invention include a motor 1 and a spring element 2 such as a gear by the motor 1. A motor drive system composed of a mechanical unit 3 driven through the motor 4 is set as a control target 4, and the generated torque or torque command value τ of the motor 1, the motor angle θ _M and the motor angular velocity of the motor 1 output from the control target 4 θ _M ′ is input, and the angle θ _{M of the} motor 1, the motor angular velocity θ _M ′, and the joint angle θ _L and the joint angular velocity θ _L ′ of the mechanical unit 3 are set as state variables, and the generated torque or torque command value of the motor 1 is used. Conventionally, an observer 5 (corresponding to an estimating means) for modeling the controlled object 4 using a state equation having τ as an input term and estimating the joint angle θ _L and the joint angular velocity θ _L ′ of the mechanical section 3 is provided. Same as the example. However,
In this embodiment, the nominal value C ′ of the nonlinear term applied to the control target 4
Is calculated by a computer (not shown), and the control object 4 is modeled by the above-mentioned equation of state included in the input term.
It differs from the conventional example in that the state variables are estimated by the observer 5. In the following, in this embodiment, parts different from the conventional example will be mainly described. First, the basic principle of the devices A1 and A2 according to the reference example and the present embodiment will be described. In the device A1 according to the reference example, in the system represented by the above-described equation of motion (1), in order to more accurately estimate the load torque such as the nonlinearity and interference of the mechanical unit 3, the nominal value of the nonlinear term of the load torque is used. A state estimation observer is applied to a system that inputs a value C ′ and a generated torque (or a command torque to the motor) τ of the motor 1. The state equation of this system can be described as follows by making it correspond to the state equations (2a) and (2b) shown in the conventional example.

(Equation 7) Here, θ _M : motor angle, θ _M ′: motor angular velocity θ _M ″: motor angular acceleration, θ _L : joint angle θ _L ′: joint angular velocity, θ _L ″: joint angular acceleration J _M : motor inertia, J _L : Link inertia B _M : Viscosity coefficient of motor, B _L : Term caused by joint angular velocity θ _L ′ such as viscosity coefficient of link, K: Spring coefficient of joint τ: Motor generated torque or torque command value C ′: Nominal value of nonlinear term such as interference of each axis and gravity

A state estimation observer with a reduced dimension such as the same dimension or the minimum dimension is applied to the state equation (4a) or (4b). Since the configuration of the same-dimensional observer is uniquely determined from the state equation, an example of the minimum-dimensional observer is shown below. θ _L1 = ΔΩ _e (θ _M −θ _L1 ) −θ _L1 ′ −Ω _e K ⁻¹ τ｝ dt + Ω _e K ⁻¹ J _M θ _M ′ + Ω _e K ⁻¹ B _M θ _M … (5a− 1) θ _L1 ′ = ∫ ｛Ω _U (θ _M −θ _L1 ) −J _L ⁻¹ B _L θ _L1 ′ + (J _L ⁻¹ −Ω _U K ⁻¹ ) τ−J _L ⁻¹ C ′｝ dt ^{_{+ (Ω U K -1 J M}} -J L -1 J M) θ M '+ (Ω U K -1 B M -J L -1 B M) θ M ... (5a-2) or, epsilon ₁ = ∫ ｛−Ω _e ε ₁ −θ _L1 ′ −Ω _e K ⁻¹ τ｝ dt + Ω _e K ⁻¹ J _M θ _M ′ + (Ω _e K ^-1 B _M -1) θ _M (5b-1) θ _L1 ′ = ∫ ｛−Ω _U ε ₁ −J _L ⁻¹ B _L θ _L1 ′ + (J _L ⁻¹ −Ω _U K ⁻¹ ) τ−J _L ⁻¹ C ′｝ dt + (Ω _U K ⁻ to ^{_{1 J M -J L -1 J M}} ) θ M '+ (Ω U K -1 B M -J L -1 B M) θ M ... (5b-2) where, theta _L1: estimate joint angles , θ _L1 ': estimated value of the joint angular velocity, epsilon _1: estimate of the distortion amount, Ω _e, Ω _U: convergence gain (manual input Initialization or variable value) Note According, the parameter τ in the above formulas the input quantity X, the parameter θ _M, θ _M 'is the output quantity Y, parameter theta _L1 (or ε
₁ ) and θ _L1 ′ correspond to the feedback amount Z, respectively. As described above, in the device A1, the nominal value C of the nonlinear term which is known information such as the nonlinearity of the control target 4 and the interference of each axis which is a problem in the robot, the NC, and the like, which is conventionally estimated by the observer 5 as the disturbance element. Since the controlled object 4 is modeled by a state equation having 'as an input term and the state variables are estimated by the observer 5, the estimation accuracy of the state variables to be fed back can be improved. With the improvement of the estimation accuracy, the state feedback gain of the motor drive system can be increased, and the motor drive system can be controlled more accurately.

Further, in the apparatus A2 according to the present embodiment, the controlled object 4 is modeled by a state equation including a disturbance torque (corresponding to an unknown disturbance torque) other than the nominal value C 'of the nonlinear term in a state variable. Thus, the state variables are estimated by the observer 5. That is, considering the unknown disturbance, C-C '
+ D (load torque d due to a modeling error of C or load fluctuation) is used as a disturbance torque D to form a state equation. Then, assuming that D is an offset disturbance (D '） 0), equations corresponding to the above equations (3a) and (3b) can be described as follows.

(Equation 8) For this state equation (6a) or (6b), a state estimation observer with reduced dimensions such as the same dimension or the minimum dimension in consideration of disturbance is applied. Since the configuration of the same-dimensional observer is uniquely determined from the state equation, an example of the minimum-dimensional observer is shown below. θ _L1 = ΔΩ _e (θ _M −θ _L1 ) −θ _L1 ′ −Ω _e K ⁻¹ τ｝ dt + Ω _e K ⁻¹ J _M θ _M ′ + Ω _e K ⁻¹ B _M θ _M … (7a− 1) θ _L1 ′ = ∫ ｛Ω _U (θ _M −θ _L1 ) −J _L ⁻¹ B _L θ _L1 ′ + (J _L ⁻¹ −Ω _U K ⁻¹ ) τ−J _L ⁻¹ (C ′ + d) _{1} dt + (Ω U K} -1 J M -J L -1 J M) θ M '+ (Ω U K -1 B M -J L -1 B M) θ M ... (7a-2) d 1 = Ω _d ｛∫ (τ−B _L θ _L1 ′ −C′−d ₁ ) dt−J _L θ _L1 ′ −J _M θ _M ′ −B _M θ _M … (7a-3) or ε ₁ = ∫ ｛−Ω _e ε ₁ −θ _L1 ′ −Ω _e K ⁻¹ τ｝ dt + Ω _e K ⁻¹ J _M θ _M ′ + (Ω _e K ⁻¹ B _M −1) θ _M (7b-1) θ _L1 ′ = ∫ ｛−Ω _U ε ₁ −J _L ⁻¹ B _L θ _L1 ′ + (J _L ⁻¹ −Ω _U K ⁻¹ ) τ−J _L ⁻¹ (C ′ + d ₁ )｝ dt + ( ^{_{Ω U K -1 J M -J L}} -1 J M) θ M '+ (Ω U K -1 B M -J L -1 B M) θ M ... (7b-2 ) D ₁ = ω _d ｛∫ (τ−B _L θ _L1 ′ −C′−d ₁ ) dt−J _L θ _L1 ′ −J _M θ _M ′ −B _M θ _M … (7b-3) , d ₁ is the estimated value of the disturbance torque d, which is also included in the feedback amount Z. Thus, the device A2, observer disturbance torque other than the nominal value of the nonlinear term 5
By performing the estimation, the estimation accuracy can be further improved.

Next, a control device A1 '(A2') for a motor drive system according to another embodiment will be briefly described with reference to FIG. As shown in FIG. 2, the device A1 '(A2')
A motor drive system including a plurality of motors 1 and a plurality of mechanical units 3 driven by the plurality of motors 1 via spring elements 2 is set as a control target 4. Then, the observer 5 outputs the generated torque or command torque τ of each motor 1 input to the control target 4, the motor angle θ _M and the motor angular velocity θ _M ′ of each motor 1 output from the control target 4, respectively, The nominal value C ′ of the nonlinear term of the load torque of each mechanical unit 3 is input, and the motor angle θ of each motor 1 is
_M , motor angular velocity θ _M ′, joint angle θ _L of each mechanical section 3,
Using the joint angular velocity θ _L ′ and the unknown disturbance torque as state variables, the generated torque of each motor 1 or the command torque τ,
The controlled object 4 is modeled using a state equation using the nominal value C 'of the nonlinear term of the load torque of each mechanical unit 3 as an input term, and the joint angle θ _L , joint angular velocity θ _L ', And an unknown disturbance torque are estimated. In this case, the parameters θ _M , which were treated as one-dimensional variables in the above equations (1) to (7b-3),
θ _L , J _M , J _L , B _M , B _L , K, τ, d, C, etc. θ _M : vector indicating motor angle, θ _L : joint angle vector J _M : inertia matrix of motor, J _L : Link matrix inertia matrix B _M : Motor viscosity coefficient matrix, B _L : Coefficient matrix caused by joint angular velocity vector θ _L ′ such as link viscosity, K _: Joint spring coefficient matrix τ: Torque vector, d: Load variable Load torque vector C: vector indicating nonlinear terms such as interference of each axis and gravity, etc. A state estimation observer of a multi-input multi-output system obtained by substituting into a vector and a determinant is used. A1 'is connected to the device A1
2 'has a configuration corresponding to the device A2. As described above, in the device A1 '(A2'), instead of applying an observer to each axis of a motor drive system, for example, a robot or NC, all the axes are treated as one system, and one axis is used for all the axes. Covering with one observer 5 enables estimation including vibration due to mechanical resonance of other axes. Therefore, the estimation accuracy can be improved for a motor drive system including a plurality of motors 1 and a plurality of mechanical units 3. Furthermore, the non-linear characteristic of the spring element is
The following describes a case where the data is taken into the device. A spring element such as a normal gear has nonlinear characteristics such as hysteresis, backlash, and dead zone, and this nonlinear characteristic may affect the estimation accuracy of the feedback amount. In this case, the spring coefficient (or spring coefficient matrix) K in the above equations (1) to (7b-3) is determined by the angle θ _M , θ _L or the amount of strain ε (= θ _L − θ _M ) or K (θ _L , θ _M ) or K
(Ε) or the like, and a state estimation observer describing non-linear characteristics such as hysteresis, backlash and dead zone of the spring element 2 is used. As an example of the function describing the non-linear characteristic of the spring element 2, the one shown in FIGS. 3 (a) and 3 (b) can be considered.

As described above, by incorporating the non-linear characteristics of the spring element 2 into the observer 5, the estimation accuracy of the state variable can be improved. Further, a case where the dynamic characteristic of the current loop of the motor 1 is taken into the observer 5 will be described. To accurately describe the dynamic characteristics of the current loop,
In the case of an AC motor or the like, the dynamic characteristics may be described after each phase is coordinate-transformed to the dq axes of the stationary coordinate system. However, for simplicity, the dynamic characteristics of the current loop are described by a first-order or second-order lag system. Describe. A description example in the first-order lag system is shown below. τ = 1 / (1 + T _IS ) × τ _REF (8) Here, τ represents a generated torque of the motor 1, τ _REF is a torque command value, and T _I is a time constant of a current loop. This equation (8) is added to the above-mentioned equation of motion (1), and the state equation (6a) and the corresponding state equation are derived as follows.

(Equation 9) For this state equation (9a), a state estimation observer in which dimensions such as the same dimension or the minimum dimension are reduced is used. As described above, the feedback amount Z can also be obtained by incorporating the dynamic characteristics of the current loop of the motor 1 into the observer 5.
Can be improved (especially in a system having a slow dynamic characteristic of the current loop). As described above, according to this embodiment, it is possible to obtain a motor drive system control device that can improve the estimation accuracy of state variables and perform more accurate control. In the apparatus A1 (A2) described above, the observer having the same dimension or the smallest dimension has been described. However, in actual use, a higher-dimensional observer does not cause any problem. In the apparatus A1 '(A2') described above, a motor drive system in which a plurality of motors 1 and a plurality of mechanical parts 3 are connected one to one via a spring element is exemplified. A similar device can be obtained for a motor drive system in which a plurality of mechanical parts are connected to a motor or a motor drive system in which a single mechanical part is connected to a plurality of motors. Note that the above-described embodiment apparatus A1
In (A2) and A1 '(A2'), the nominal value C 'of the nonlinear term calculated by the computer is used as the known disturbance element. However, in actual use, the nonlinear term itself that changes every moment instead of the nominal value is used. It is also conceivable to detect and directly input this to the observer 5.

[0011]

Since the present invention is configured as described above, the load such as the non-linearity of the controlled object and the interference of each axis which is a problem in the robot, NC, etc., which has been conventionally estimated by the estimation means as a disturbance element. By effectively utilizing the nominal value of the non-linear term of the torque, the estimation accuracy of the feedback amount can be improved. With the improvement of the estimation accuracy, the state feedback gain of the motor drive system can be increased, and the motor drive system can be controlled more accurately. Further, the estimation accuracy can be further improved by estimating the disturbance torque other than the nominal value of the non-linear term by the estimating means. Also, instead of applying estimating means to each axis of a motor drive system including a plurality of motors and a plurality of driven parts, for example, a robot, NC, etc.,
By treating all axes as one system and covering all axes with one estimation means, vibration due to mechanical resonance of other axes can be estimated as disturbance. Therefore, the estimation accuracy can be improved for a motor drive system including a plurality of motors and a plurality of driven parts. Further, by using the non-linear characteristics of the spring element such as hysteresis, backlash and dead zone for estimating the amount of feedback by the estimating means, the accuracy of estimating the amount of feedback can be improved. Further, by using the dynamic characteristic of the current loop of the motor for estimating the feedback amount by the estimating means, it is possible to improve the estimation accuracy of the feedback amount. As a result, it is possible to improve the accuracy of estimating the feedback amount and obtain a motor drive system control device that can perform more accurate control.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a schematic configuration of a motor drive system control device A2 (A1) according to one embodiment and a reference example of the present invention.

FIG. 2 is a block diagram showing a schematic configuration of a motor drive system control device A1 ′ (A2 ′) according to another embodiment of the present invention.

FIG. 3 is a graph (a) showing non-linear characteristics of a spring element,
(B).

FIG. 4 shows a conventional motor drive system controller A ₀₁ (A ₀₂ ).
FIG. 2 is a block diagram showing a schematic configuration in one example.

[Explanation of symbols]

A1, A2, A1 ', A2': Control device of motor drive system 1: Motor 2: Spring element 3: Machine part 4: Control target 5: Observer (corresponding to estimation means) C ': Nominal value of nonlinear term

──────────────────────────────────────────────────続 き Continued on the front page (72) Inventor Yoshinori Imaizumi 1-2 Nakahara, Miya-machi, Toyohashi-shi, Aichi Kobe Steel, Ltd. Toyohashi FA / Robot Center (72) Inventor Takayuki Nakagami Mitsaya, Toyohashi-shi, Aichi Town character Nakahara 1-2 Kobe Steel, Ltd. Toyohashi FA / Robot Center (56) References JP-A-1-296301 (JP, A) JP-A-4-281381 (JP, A) (58) Field (Int.Cl. ⁶ , DB name) H02P 5/00

Claims (6)

(57) [Claims] 1. A motor comprising: a motor; and a mechanical unit driven by the motor , wherein the motor and the mechanical unit are connected via a spring element.
The motor drive system to which the generated torque or command torque of the motor and the non-linear term of the load torque of the mechanical section are input and the motor angle and the motor angular velocity of the motor are output is controlled. The generated torque or command torque of the motor input to the controlled object, the motor angle and motor angular velocity of the motor output from the controlled object, and the nominal value of the nonlinear term of the load torque of the mechanical unit are input. The motor angle, motor angular velocity, joint angle of the mechanical part,
Using the joint angular velocity and the unknown disturbance torque as state variables, the control target is determined using a state equation in which the generated torque or command torque of the motor and the nominal value of the nonlinear term of the load torque of the mechanical section are input terms. Estimation means for modeling and estimating joint angles, joint angular velocities, and unknown disturbance torques of the mechanical unit are provided. The mechanical unit is provided with a displacement between the mechanical unit and the motor.
The spring force of the spring element and the nonlinear term
Also acts on the motor, the reaction force of the spring force and the motor
At least the driving force generated by itself acts on the mechanical part.
The balance between the force acting on the machine and the inertial force of the mechanical part,
Balance between the force acting on the motor and the inertia force of the motor
A control model from the relationship and the control model
The nominal value of the nonlinear term
The spring force and the nonlinearity
Assuming that a disturbance force acts together with the term
Treated as inputs acting on the motor,
The estimating means is configured on the basis of the joint angle of the mechanical part estimated by the estimating means,
A control device for a motor drive system, wherein a joint angular velocity and an unknown disturbance torque are fed back and input to the control target. 2. A motor and a machine driven by the motor.
The motor and the mechanical part are connected via an elastic body.
The torque generated by the motor or the finger
And the nonlinear term C of the load torque of the mechanical part
There is inputted, the motor angle of the upper SL motor theta _M and the motor angular
The motor drive system that outputs the speed θ _M ′ is the control target, and the generated torque of the motor input to the control target is
Or the command torque τ and the above
Motor angle θ _M and motor angular velocity θ _M ′
Input the nominal value C 'of the nonlinear term of the load torque of the mechanical part
Motor angle θ _M , motor angular velocity
θ _M ′, joint angle θ _L of the above mechanical part , joint angular velocity
θ _L ′, and the modeling error C−C ′ of the nonlinear term and the negative
Unknown disturbance torque D including load torque d due to load fluctuation
Is the state variable, the torque generated by the motor or the command
Nominal value of torque τ and nonlinear term of load torque of above mechanical part
Equation (6a) or (6b) using the value C ′ as an input term
Model the above controlled object using the expressed state equation
And the joint angles, joint angular velocities, and unknown
Estimating means for estimating turbulent torque, the joint angle of the mechanical part estimated by the estimating means,
Feedback on joint angular velocity and unknown disturbance torque
A motor input to the control object by using
Drive system controller. (Equation 1) However, J _M: motor inertia, J _L: Link Inertia B _M: viscosity coefficient of the motor, B _L: Links term due to the viscosity coefficient joint angle velocity theta _L like ', K: spring constant of the joint epsilon: Distortion Amount (= θ _L −θ _M ) 3. The control object comprises: a plurality of motors; and a plurality of mechanical units driven by the respective motors via respective spring elements. , The motor angle and motor angular velocity of each motor output from the control object, and the nominal value of the non-linear term of the load torque of each mechanical part, respectively. motor angular velocity, joint angle of each machine section, joint angular velocity, and the <br/> the unknown disturbance torque state variables, and the generated torque or the command torque of the motors, the nominal value of the nonlinear term of the load torque of each machine section modeling the controlled object using the above equation of state and input term bets, joint angle, joint angular velocity of each machine section, and an estimate of the unknown disturbance torque The control device for a motor drive system according to claim 1. 4. A motor comprising: a motor; and a mechanical unit driven by the motor via a spring element having nonlinear characteristics.
A motor drive system to which a generated torque or a command torque of the motor is input and a motor angle and a motor angular velocity of the motor are output is to be controlled, and the generated torque or the command torque of the motor input to the control target is The motor angle and the motor angular velocity of the motor output from the control object are input, and the motor angle, the motor angular velocity of the motor, the joint angle and the joint angular velocity of the mechanical unit are included in state variables , and the torque generated by the motor or Modeling the controlled object by using a state equation including the command torque in the input term and expressing the spring constant included in the state equation as a nonlinear function of the amount of distortion based on the motor angle of the motor and the joint angle of the mechanical unit, Estimating means for estimating a joint angle and a joint angular velocity of the mechanical unit are provided. Due to the displacement between the mechanical part and the motor
The spring force of the spring element and the nonlinear term
Also acts on the motor, the reaction force of the spring force and the motor
At least the driving force generated by itself acts on the mechanical part.
The balance between the force acting on the machine and the inertial force of the mechanical part,
Balance between the force acting on the motor and the inertia force of the motor
A control model from the relationship and the control model
In the above , the above-mentioned spring force and the above-mentioned nonlinear term
The driving force is applied to the motor
Are treated as inputs acting on the control model.
The estimating means is configured to calculate the joint angle of the mechanical part estimated by the estimating means,
A motor drive system control device characterized in that the joint angular velocity is fed back and input to the control object. 5. A motor and a spring element having a non-linear characteristic
And a mechanical unit driven by the motor through the K.
The torque generated by the motor or the command torque τ
The motor angle θ _M and motor angular velocity of the above motor
The motor drive system that outputs θ _M ′ is the control target, and the generated torque of the motor input to the control target is
Or the command torque τ and the above
The motor angle θ _M and the motor angular velocity θ _M ′ are entered.
The motor angle θ _M , motor angular velocity θ
_M ′, joint angle θ _L of the above mechanical part , joint angular velocity
θ _L ′ is included in the state variables, and the generated torque of the motor
Equation (6a) or (6)
b) using the state equation
The spring constant K included in the equation is calculated as the motor angle θ of the above motor.
Nonlinearity of strain amount ε based on _M and joint angle θ _L of mechanical part
Model the above controlled object by expressing it as a function,
Estimating means for estimating joint angles and joint angular velocities of the mechanical parts, the joint angles of the mechanical parts estimated by the estimating means,
Feedback the joint angular velocity and input it to the above control object.
A control device for a motor drive system, comprising: (Equation 2) However, J _M: motor inertia, J _L: Link Inertia B _M: viscosity coefficient of the motor, B _L: term due to the viscosity coefficient joint angle velocity theta _L like 'link, K: spring constant of the joints d: load Load torque such as fluctuation , C: Non-linear term such as interference of each axis or gravity, C ': Nominal value of the above-mentioned non-linear term , D: Unknown disturbance torque ε: Distortion amount (= θ _L −θ _M ) 6. The control of the motor drive system according to any one of claims 1-5, characterized in that the dynamic characteristics of the current loop of the motor formed by using the estimation of the state variables by the estimating means apparatus.