JPH0934504A - Motion controller for double inertia drive system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide the motion controller which can perform the track and position control operations with high precision despite the variance of parameter by estimating and compensating the disturbance that affects a double inertia drive system by means of a disturbance observer which inputs the load side position and the motor output torque command of the drive system. SOLUTION: A motion controller of a double inertia drive system uses a disturbance observer and defines the moment of inertia of the actuator of a nominal model as Jmo, together with the moment of inertia of the load and the spring constant of an elastic element defined as Jio and Kso respectively. Under such conditions, an amplifier having its gain of JmoJio/Kso is used together with a disturbance observer that is designed to show the dynamics of a controlled system in quardruple integration as q1 =(v+d)/s<4> when the deviation between the load output and its target value, the normalized input, the normalized disturbance and the Laplacian operator are referred to as q1 , (v), (d) and (s) respectively. Thus, it is possible to obtain a track follow-up controller which can suppress both disturbance and system vibrations and also uses a model system as its base.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention has characteristics as a two-inertia drive system because a speed reducer and a hydraulic / pneumatic transmission device are interposed between an actuator and a load like a joint axis drive system of a large robot. The present invention relates to a motion control device for an inertial drive system.

[0002]

2. Description of the Related Art Currently, various motion control systems are used in robots, machine tools, automobiles, etc. to drive an object (load) following a set target. The PID control system is most widely used as these motion control systems. However, when the PID control system is applied to a drive system in which physical parameters such as weight and moment of inertia change with time, there is a problem that it is difficult to secure stability or control accuracy. A control system using a disturbance observer has been developed aiming at application to such a drive system.

[0003]

However, the target to which the disturbance observer can be applied at present is a drive system having a characteristic as one inertial system, and a multi-inertial system (two or more inertia moments and elastic elements connecting them). It has a serious defect that a large vibration is generated. Therefore, at present, no control system using the disturbance observer can be put to practical use as a control system of a multi-inertia system.

The present invention has been made in view of the above,
The purpose of this is a two-inertia drive system capable of highly accurate trajectory control and position control even if there are parameter fluctuations in a two-inertia drive system modeled as two inertia moments and an elastic element connecting them. The purpose of the present invention is to provide a motion control device.

[0005]

In order to achieve the above object, the present invention according to claim 1 is a two inertial drive system motion controller using a disturbance observer, wherein the inertial moment of the nominal model actuator is J _m0 , the moment of inertia of the load J _l0, when the spring constant of the elastic element and K _s0, an amplifier gain J _m0 J _l0 / K _s0, the deviation of the output and the target value of the load q _l, normalized input v , normalization disturbance d, when the Laplace operator and s, the use of disturbance observer designed as the dynamics of the controlled object is represented by a quadruple integral as _{q l = (v + d)} / s 4 Use as a summary.

The present invention according to claim 1 can be applied to a two-inertia drive system in which physical parameters such as weight and moment of inertia change with time, and good motion control in which unnecessary vibration does not occur. The device can be realized.

The present invention according to claim 2 is based on claim 1.
In the invention described above, the gist is that the output of the disturbance observer is a normalized estimated disturbance.

[0008] Further, the present invention according to claim 3 provides the present invention as claimed in claim 1.
In the invention described above, the gist is that the output of the disturbance observer is a differential value up to the third order of the deviation between the normalized estimated disturbance, the output of the load and the target value.

According to a fourth aspect of the present invention, in the invention according to any one of the first to third aspects, the gain J of the amplifier is set.
_The gist is that _m0 J _l0 / K _s0 is changed in real time by its estimated value according to the change of the load inertia moment.

[0010]

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

As an example, FIG. 1 shows a model of a two-inertia system in which a motor and a load are coupled by a spring. The equation of motion of this two-inertia system is expressed by the following equation.

[0012]

[Equation 1] Here, J _m and J _l are the moments of inertia of the motor and the load, respectively, and q _m and q _l are the rotation angles of the motor and the load, respectively.
K _s is the spring constant of the spring, D _m is the viscous friction of the motor

[Outside 1] Minutes.

Next, the configuration of the motion control device for the two-inertia drive system according to the embodiment of the present invention will be described with reference to FIG.
In FIG. 2, reference numeral 1 denotes a two-inertia drive system to be controlled, which constitutes, for example, a device that changes a load angle by a motor. Reference numeral 2 denotes a control device, which includes a disturbance observer 21 that is a processing unit that estimates a disturbance value, a positioning control unit 22 that generates a command value such that the target value and the rotation angle of the load match, and the output of the positioning control unit 22. And disturbance observer 2
It is composed of an amplifier 23 for multiplying the output difference of 1 by a proportional constant, and a feedforward input generation unit 24 for generating a feedforward input corresponding to a target value. The control device 2 is also applied to the control of the conventional one-inertia system.

The control device 2 includes a disturbance observer 21
It is applied to an inertial system, and its characteristic is the configuration of the disturbance observer 21 and the amplifier 23. Next, they will be described.

The equations (1) and (2) are subjected to Laplace transform and q _l
Solving for, we get

[0016]

[Equation 2] Here, if the disturbance torque acting on the load is converted into the disturbance torque acting on the motor rotation shaft, d _m can be expressed as the following equation.

[0017]

(Equation 3) However, d _l is assumed to be a class disturbance in which d _m is proper. At this time, the following equation is derived from the equation (3).

[0018]

(Equation 4) This equation shows that the input u and the output q _l are expressed as a minimum phase system, in other words, a stable inverse function required for the configuration of the control system can be realized.

Incidentally, conventionally, the rotation angle of the motor is generally taken as the output, but in this case, the present inventors have a problem that a stable inverse function necessary for the configuration of the control system cannot be realized. Has been revealed by.

Next, a nominal model for constructing the disturbance observer 21 will be described. In the present invention, input u
The following equation is used as a nominal model showing the relationship between the output and the output q _l .

[0021]

(Equation 5) Here, J _m0 , J _l0 , and K _s0 are J _m and J _{m of the} actual drive system, respectively.
It is a nominal value for J _l and K _s .

Equation (7) shows that when the disturbance torque d _m is 0, J _m ,
When J _l and K _s are equal to their nominal values, they are determined so as to agree with the characteristics of equation (6) in the high frequency region.

Here, v defined by the following equation is newly introduced as a variable in place of the driving torque u of the motor and is called a normalized input.

[0024]

(Equation 6) Therefore, the gain of the amplifier 23 is given by J _m0 J _l0 / K _s0 .

The normalized disturbance d is defined by the following equation.

[0026]

(Equation 7) At this time, the rotation angle q _{l of the} load is expressed by the following equation using v and d from the equation (6).

[0027]

(Equation 8) This equation shows that the control target can be expressed as a system having quadruple integral type dynamics by introducing the disturbance d defined above.

Now, when q _r is the target value for the rotation angle of the load, this is subtracted from both sides of the above equation and rewritten into the expression in the time domain to obtain the following equation.

[0029]

[Equation 9] Here, Δq _l = q _l −q _r is the deviation angle between the rotation angle of the load and the target value, and Δv = v−q _r ⁽⁴⁾ is the normalization expressed by the difference between the normalized input v and the feedforward input. It is a correction input. Since d cannot be obtained directly, an observer using Δq _l and Δv

[Outside 2] The Ming disturbance observer is characterized by estimating the disturbance d by regarding the controlled object as having quadruple integration type dynamics.

The control system using the disturbance observer for estimating only the disturbance has been described above. Next, a control device using a disturbance observer that simultaneously estimates a state vector in addition to the disturbance will be described. FIG. 3 shows the configuration of the control device in this case. Also in this configuration, except for the fact that the gain of the amplifier is given by J _m0 J _l0 / K _s0 and the disturbance observer is designed based on the quadruple integral type dynamics, the conventional one-inertia system and the basic structure are the same. Is the same. The difference from the control device of FIG. 2 is that the disturbance observer simultaneously estimates the disturbance as the disturbance / state observer 31 and the state vector Δq _l ⁽ⁱ⁾ (i ≦ 3) useful for positioning control. This has the advantage that the configuration of the control device is compact.

Next, an example of a method of designing a disturbance / state observer for simultaneously estimating the disturbance and the state vector in the control device of FIG. 3 will be described. This is one of the design examples of the disturbance observer used in the control device shown in FIG.

In the equation (11), the disturbance d is assumed to be a step disturbance, and is represented by d _s , the following equation is obtained.

[0033]

(Equation 10) At this time, the disturbance / state observer is designed as follows using the same-dimensional observer, which is one of the standard observer construction methods.

[0034]

[Equation 11]

[Outside 3] The convergence speed of the estimation can be adjusted within the range where the same-dimensional observer of the equation (4) maintains stability. For example, if a design method called the pole placement method is used, ω _f is an appropriate positive constant and K _f
Can be designed as follows.

[0035]

(Equation 12) This indicates that sufficient disturbance estimation is possible in the frequency region lower than ω _f . Therefore, the resonance frequency of at least two inertial systems

(Equation 13) If such a disturbance compensation is performed, the vibration mode peculiar to the two-inertia system to be controlled is suppressed, and it is represented by a normalized nominal model (= 1 / s ⁴ ). It is considered that quadruple integration type dynamics will be realized.

V _c is a positioning command input given to the position servo system so that the load rotation angle reaches the target value. Considering that state feedback is performed using the estimated state vector, the following equation holds.

[0037]

[Equation 14] In the above, an example of the design method of the control system by the disturbance / state observer that simultaneously estimates the disturbance and the state vector has been described. Next, design examples and simulation results when specific physical parameters are given to the controlled object will be described.

The following values are assumed as specific nominal values of the physical parameters of the two-inertia system.

[0039]

Equation 15] ^{_{J m0 = 0.004Kgm 2, J l0}} = 0.031Kgm 2, K s0 = 5Nm / rad, D m0 = 0.05Nms / addition rad (21), the inertia of these nominal values are the actual load It is assumed that there is an error of about 10 to 20% from the moment and the spring constant, and the motor input is accompanied by a dead time of 2 msec.

The resonance frequency of the two-inertia system corresponding to the nominal parameter of the equation (21) is 37.6 rad / s.
Considering that the gain K _{f of the} disturbance observer is sufficiently quicker than the resonance frequency, ω _f in Eq. (15) is set to 30
It was selected as 0 rad / s and designed as follows.

[0041]

K _f = [1.5 × 10 ³ 9 × 10 ⁵ 2.8 × 10 ⁸ 4.1 × 10 ¹⁰ 2.4 × 10 ¹² ] ^T (22)

[Equation 17] Next, the state feedback gain K _r for positioning control is designed. Here, in equation (23), the following 2
Evaluation function of the form

(Equation 18) The optimum regulator that minimizes Weighting functions Q and p are

[Equation 19] Q = diag (10 ⁶ , 10 ³ , 25, 0.5), p = 5.63 × 10 ^-5 (25) was selected, and the feedback gain K _r was designed as follows.

[0042]

K _r = [1.33 × 10 ⁵ 2.63 × 10 ⁴ 2.5 × 10 ³ 1.2 × 10 ² ] (26) Next, using the designed gain, the position shown in FIG. 4 is obtained. The simulation by the control device will be described. A step input of 1 rad is given to the target angle. As the pre-filter F, the following equation is used so that the feedforward input generation filter s ⁴ F becomes proper.

[0043]

(Equation 21) FIG. 5 shows the simulation result. It is assumed that the inertia moment and the spring constant have a parameter fluctuation of about 10 to 20%, the motor input has a dead time of 2 msec, and the viscous friction of the motor has D _m = 0.05 Nmsec / rad. Also, at t = 0.5 sec, the load

(Equation 22) The disturbance torque represented by is acting. From the figure, it can be confirmed that the load rotation angle can be made to follow the target angle in a stable manner even when parameter fluctuations or disturbance torques are applied.

The configuration method and the simulation result of the control device using the disturbance observer for simultaneously estimating the disturbance and the state vector have been described above.

Next, the motion control when the load inertia of the two-inertia drive system fluctuates greatly will be described using the above-mentioned simulation model. When the controlled object is a drive system such as a joint shaft of a robot manipulator, the moment of inertia on the load side largely varies depending on the posture of the manipulator. In this case, it is expected that a large error will occur between the actual moment of inertia of load and its nominal value, and the operational control performance will be significantly deteriorated. On the other hand, in a complicated mechanical system such as a robot manipulator, even if it is difficult to formulate its dynamics model and identify physical parameters precisely, there are cases where it is possible to estimate approximate values of the structure and physical parameters. Not a few exist. In the control device of FIGS. 2 and 3, the physical parameter to be controlled is reflected positively only in the gain J _m0 J _l0 / K _s0 of the amplifier. It can be changed based on the inertia matrix model. The following simulation is performed to confirm this effect.

FIG. 6 shows that when the load inertia moment changes about 10 times, the parameter J _{10 of the} equation (21) is increased 10 times (0.31) according to some model.
Kgm ² ) is assumed and the simulation result is shown. Again, parameter fluctuations of about 10-20% for moment of inertia and spring constant, 2 for motor input
It is assumed that the disturbance torque of the formula (28) is applied to the load and the dead time of msec. However, the observer and state feedback gain used in the controller are not changed. Although the moment of inertia of the load changes about 10 times, the same target angle as in the case of FIG. 5 is given, so the torsion angle on the motor side and the load side increases at the rising edge, but it is still stable. It can be seen that the track following control device is realized.

[0047]

As described above, according to the present invention,
2 Suppression of disturbance acting on the motor or load and suppression of system vibration by estimating / compensating for disturbance acting on the system by using the disturbance observer that receives the position of the load side of the inertial system and the motor output torque command In addition, it is possible to configure a trajectory tracking control device based on the model system. Further, by reflecting the information about the inertia matrix of the manipulator and the load weight as the nominal model of the disturbance observer, it is possible to reduce the influence of a large load / inertia change.

[Brief description of drawings]

FIG. 1 is a diagram showing a model of a two-inertia system that is a control target of a motion control device for a two-inertia drive system according to the present invention.

FIG. 2 is a block diagram showing a configuration of a motion control device for a two-inertia drive system according to an embodiment of the present invention.

FIG. 3 is a block diagram showing the configuration of a motion control device of a two-inertia drive system in which a disturbance observer simultaneously estimates a disturbance and a state vector.

FIG. 4 is a block diagram showing a configuration of a trajectory tracking control system used for simulation.

FIG. 5 is a diagram showing a trajectory following simulation result of a two-inertia drive system.

FIG. 6 is a diagram showing a trajectory following simulation result of the two-inertia drive system when the load inertia moment changes ten times.

[Explanation of symbols]

 1 2 Inertial drive system 2 Controller 21 Disturbance observer 22 Positioning controller 23 Amplifier 24 Feedforward input generator

Claims (4)

[Claims] 1. In a motion control system for a two-inertia drive system using a disturbance observer, the inertial moment of the nominal model actuator is J _m0 and the inertial moment of the load is J _m0 .
_l0 and the spring constant of the elastic element are K _s0 , the gain is J
_When the amplifier of _m0 J _l0 / K _s0 and the output of the load and the deviation of the target value are q _l , the normalized input is v, the normalized disturbance is d, and the Laplace operator is s, the dynamics of the controlled object is q _l. =
A motion control device for a two-inertia drive system, which uses a disturbance observer designed as represented by quadruple integration such as (v + d) / s ⁴ . 2. The motion control device for a two-inertia drive system according to claim 1, wherein the output of the disturbance observer is a normalized estimated disturbance. 3. The motion control apparatus for a two-inertia drive system according to claim 1, wherein the output of the disturbance observer is a differential value up to the third order of the deviation between the normalized estimated disturbance, the output of the load and the target value. . 4. The gain J _m0 J _l0 / K _{s0 of the} amplifier is changed in real time according to the estimated value of the gain J _m0 J _l0 / K _s0 according to the change of the moment of inertia of the load. Two-inertia drive system motion controller.