JP3220589B2 - Control device for mechanical system - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for servo-controlling a mechanical system such as a robot.
The present invention relates to a mechanical system control device that can control a mechanical system having a low rigidity of a transmission mechanism.

[0002]

2. Description of the Related Art Generally, when controlling a mechanical system such as a robot, a current control system, a speed control system and a position control system are independently constituted for each axis. That is, as shown in FIG. 16, a displacement detector 4 such as an encoder for detecting a displacement of a motor 5 for driving a robot or the like is provided for each axis, and a motor displacement is obtained from a motor displacement amount output from the displacement detector 4. A difference element 22 for calculating the speed is provided. The position control system includes a comparison element 12 for feeding back a motor displacement amount to a positioning command value and a proportional element 8 for providing a position loop gain. The speed control system includes a comparison element 11 for feeding back the motor speed to the output of the position control system, and a proportional integration element 7 for providing a speed loop gain. The output of the speed control system is input to the current control system 6, and the drive current from the current control system 6 is given to the motor 5.

[0003] In the configuration shown in FIG. 16, the positioning command value input to the comparison element 12 and the motor displacement as the feedback value match, and the speed from the proportional element 8 input to the proportional integration element 7 is adjusted. Feedback control is performed such that the command and the motor speed that is the feedback value match. However, in the speed control system, only the motor speed is fed back. That is, a semi-closed loop in which the state quantity on the load side is not fed back is provided. When only the motor speed is fed back to the speed control system, vibration occurs when a robot or the like having a low rigidity of the transmission mechanism is driven at high acceleration / deceleration. Therefore,
It is necessary to reduce the gain of the control system and drive at low acceleration / deceleration.

[0004] In view of the above, there has been proposed a method of performing all state feedback in which a state quantity on the load side is fed back in addition to the motor speed in a speed control system. As an example of such a method, there is a control method described on pages 1018 to 1025 of Electron Theory D, Vol. 107, No. 8, (Showa 62). FIG. 17 is a block diagram showing the configuration of the system. When feeding back the state quantity on the load side, the state quantity may be directly detected by a sensor. But,
It is difficult to select the mounting location of the sensor and design the filtering of the sensor output. Therefore, as shown in FIG. 17, an observer for estimating a state quantity to be fed back is incorporated.

In the method described in the above document, the observer 21 estimates the amount of strain of the spring element and its differential value from the current command value (torque command value) and the motor speed.
The strain amount of the spring element estimated by the observer unit 21 and its differential value are fed back to the current command value via the state feedback unit 3 as it is. Further, the motor speed and the motor displacement are also fed back as it is.

[0006]

Since the conventional mechanical system control device is constructed as described above, the state quantity on the load side is estimated by the observer 21, but the estimated state quantity and the motor The speed and motor displacement are fed back as is. When the control target is a robot or the like, parameters such as the inertia of the load greatly fluctuate. Also, gravity and interference terms from other axes act as disturbances. As described above, when the parameter fluctuation of the control target is large or a large disturbance acts, the characteristics of the observer unit 21 are significantly reduced, and as a result, the characteristics of the speed control system are also significantly reduced. Therefore, the vibration cannot be sufficiently suppressed. Therefore, the robot or the like must be driven at a low acceleration / deceleration. Even if the performance of the motor 5 is good, the performance of the motor 5 cannot be fully exhibited.
That is, only a performance lower than the performance limit of the motor 5 can be obtained. Therefore, there is a problem that the time required for the operation of the robot or the like becomes long, and the working time using the robot or the like cannot be reduced.

SUMMARY OF THE INVENTION The present invention has been made to solve the above-described problems, and a mechanical system having a low transmission rigidity even when a parameter to be controlled has a large fluctuation and a large disturbance acts. An object of the present invention is to provide a control device for a mechanical system that can be driven at a reduced speed and that can reduce the working time using a mechanical system such as a robot.

[0008]

According to a first aspect of the present invention, there is provided a control system for a mechanical system, comprising: information on an output of a motor and an input or output of a current control system or information on an input and an output of a motor. A state observer unit that outputs a state amount and a state amount on the load side, a disturbance observer around the motor, a disturbance observer around the transmission mechanism, and a disturbance observer around the load, and the disturbance observer includes: A disturbance observer unit for estimating a disturbance torque from the state amount estimated by the state observer unit and correcting the state amount by the state observer unit, and a disturbance torque feedback unit for feeding back an estimated value of the disturbance torque estimated by the disturbance observer unit to a torque command. And the state quantity corrected by the disturbance observer It is obtained by a state quantity feedback unit for back.

According to a second aspect of the present invention, there is provided the mechanical system control device according to the first aspect, wherein information relating to the input or output of the current control system provided to the state observer is transmitted to the disturbance observer. And an input correcting means for correcting using the estimated value of the disturbance torque.

According to a third aspect of the present invention, there is provided a mechanical system control device according to the first aspect, further comprising: a feedforward term calculating means for calculating a feedforward term of a disturbance torque; and a state observer. The information processing apparatus further includes input correction means for correcting the input information or output information of the current control system using the feedforward term calculated by the feedforward term calculation means.

According to a fourth aspect of the present invention, there is provided a mechanical system control device according to the first to third aspects, further comprising: a state observer unit;
The apparatus further includes control system parameter setting means for setting parameters in the disturbance observer section and the state quantity feedback means according to the operation state of the motor.

[0012]

According to the first aspect of the present invention, the disturbance observer corrects the state quantity estimated by the state observer, estimates the disturbance torque and feeds it back to the torque command, and controls the parameter of the mechanical system to be controlled. Reduce the effects of fluctuations and disturbances on the mechanical system.

The input correcting means in the invention according to claim 2 corrects the input of the estimation processing of the state observer section with the estimated value of the disturbance torque estimated by the disturbance observer section, thereby improving the accuracy of the estimation processing of the state observer section. Let it.

According to a third aspect of the present invention, the input correcting means corrects the input of the estimation processing of the state observer section using the feedforward term calculated by the feedforward term calculation means, and thereby the accuracy of the estimation processing of the state observer section. Improve.

The control system parameter setting means in the invention according to claim 4 sets parameters used by the state observer unit, the disturbance observer unit and the like according to the operating state of the control object at that time.

[0016]

【Example】

Embodiment 1 FIG. FIG. 1 is a block diagram showing a configuration of a control device of a mechanical system according to a first embodiment of the present invention. As shown in the figure, a displacement detector 4 for detecting a displacement of a motor 5 for driving a robot or the like is provided. The position control system includes a comparison element 12 for feeding back a motor displacement amount to a command value and a proportional element 8 for providing a position loop gain. The speed control system includes a comparison element 11 for feeding back the motor speed to the output of the position control system, and a proportional integration element 7 for providing a speed loop gain.

In this case, in the speed loop, the state observer 1 for estimating the state quantity on both the motor side and the load side from the drive current and the motor displacement, and the state quantity estimated by the state observer 1 is corrected. A disturbance observer unit 2 for estimating disturbance torque, a state feedback unit 3 for giving a predetermined gain (generally, a constant) to an output of the disturbance observer unit 2, and an output of the state feedback unit 3 to an output of a proportional integration element 7. And a summing element 9 for feeding back the estimated value of the disturbance torque by the disturbance observer unit 2. The output of the speed control system is input to the current control system 6, and the drive current from the current control system 6 is given to the motor 5.

The adding element 9 constitutes a disturbance torque feedback means, and the state feedback section 3 and the comparison element 10 constitute a state quantity feedback means.

Hereinafter, a robot will be described as an example of the mechanical system. The model of each axis of the robot is considered to be a two-inertia system composed of a motor 30, a transmission mechanism 31 using a spring element, and a load 32, as described in the above document.
This model is shown in FIG. Also, reference numerals are given as follows. J _M: motor inertia J _L: Load inertia K _B: transmission mechanism spring constant tau _m: motor drive torque tau _f: torsion torque omega _m: motor speed omega _l: loading rate theta _m: motor displacement

Then, the model shown in FIG. 2 is represented by a block diagram in FIG. The state equation of this model is expressed by equation (1). However, (1), in addition to the portion corresponding to the model shown in FIG. 2, also includes partially a motor displacement theta _m. That is, the state vector x _b is represented by θ _m , ω _m , ω
_l and τ _f in four dimensions.

[0021]

(Equation 1)

Therefore, when the state equation of the model shown in FIG. 2 is extended to the motor displacement θ _m , the observer having the same dimension as the state vector for the model is expressed as follows.

[0023]

(Equation 2)

Here, the subscript h indicates an estimated value, and G _{1 to} G
₄ is an observer gain. FIG. 4 is a block diagram showing a configuration of the state observer unit 1 corresponding to the expression (4). The state observer 1 includes a comparison element 15 for feeding back the motor displacement estimation value θ _mh to the motor displacement θ _m.
1. Observer gains G _{1 to} G are applied to the output of the comparison element 151.
₄ with each gain element 101-104.

An addition element 111 for adding the output of the gain element 101 and the estimated motor speed value ω _mh , an addition element 1
11, an integral element 121 for obtaining an estimated motor displacement θ _mh by integrating the output of the gain element 102 and a coefficient element 131.
And an integration element 1 that integrates the output of the addition element 112 to obtain an estimated motor speed ω _mh
22, an addition element 113 for adding the output of the gain element 103 and the output of the coefficient element 132, an integration element 123 for integrating the output of the addition element 113 to obtain a load speed estimated value ω _lh , an output of the gain element 104 and a coefficient element summing element 114 for adding an output of 133, the integral element 124 to obtain a torsional torque estimate tau _fh integral output to a summing element 114, subtracting element 141 which subtracts the torsional torque estimate tau _fh from the motor drive torque tau _m , A subtraction element 142 for subtracting the load speed estimation value ω _lh from the motor speed estimation value ω _{mh, a} coefficient element 131 for multiplying the output of the subtraction element 141 by a constant (1 / J _M ), and a constant (1) for the torsional torque estimation value τ _fh. / J _L ) is multiplied by a constant (KB) to the output of the coefficient element 132 multiplied by / J _L ) and the output of the subtraction element 142.
Is multiplied by a coefficient element 133.

FIG. 5 is a block diagram for explaining a method of configuring a disturbance observer. As shown in the figure, an input x and a disturbance _Td are added to a system model 100 modeled by a transfer function (1 / JS), and an output y from the system model 100 is obtained.
Is output. Then, the output y is expressed as follows: y = (x + T _d ) 1 / JS (5)

From the equation (5), the disturbance T _d is obtained from the input and output as T _d = JSy-x (6). However, in practice, the estimated disturbance value T _dh is obtained based on the following equation: T _dh = (JSy−x) / (TS + 1) (7) using a first-order lag filter in consideration of noise and the like.

FIG. 6 is a block diagram showing the configuration of the disturbance observer around the load in the disturbance observer unit 2. As shown in FIG. The disturbance observer estimates disturbance torque around the load based on the estimated torsional torque value τ _fh and the estimated load speed value ω _lh estimated by the state observer 1, and outputs the estimated disturbance torque τ _lih around the load. is there. That is,
In equation (7), J = J _L , x = τ _fh , y = ω _lh , T
_This corresponds to the case where _dh = τ _fih .

Accordingly, the disturbance observer around the load performs an operation on the torsional torque estimated value τ _{fh with} respect to (1 / TS + 1) and an operation on the estimated load speed ω _lh with (J _L S / TS + 1). From the outputs of the operation elements 232 and 231,
_Is subtracted to obtain an estimated disturbance torque τ _lih around the load.

FIG. 7 is a block diagram showing a configuration of a disturbance observer around the transmission mechanism in the disturbance observer unit 2. As shown in FIG. This disturbance observer _calculates the difference between the estimated motor speed ω _mh estimated by the state observer 1 and the estimated load speed ω _lh ,
And state observer 1 estimates the disturbance torque estimate tau _tih around transmission mechanism based on the difference between the disturbance torque estimate tau _LiH around load disturbance observer estimated around load estimated torsion torque estimate tau _fh Things. That is, in equation _{(7), J = 1 / K B,} x = ω mh -
ω _lh , y = τ _{fh −τ lih} and T _dh = τ _tih .

Therefore, the disturbance observer around the transmission mechanism _generates a subtraction element 203 for subtracting the estimated load speed ω _lh from the estimated motor speed ω _mh, and an estimated disturbance torque τ _lih around the load from the estimated torsion torque τ _fh. A subtraction element 204 that obtains a corrected torsional torque estimated value τ _fha by subtraction, an arithmetic element 221 that performs an operation related to (1 / TS + 1) on the output of the subtraction element 203, and a corrected torsional torque estimated value τ _fha (S / consists K _B (TS + 1)) of the operation on performing arithmetic element 222 and the output of the operational elements 221 subtracts the output of the operational elements 222 to obtain a disturbance torque estimate tau _{ti h} around transmission mechanism subtraction element 223.

FIG. 8 is a block diagram showing a configuration of a disturbance observer around the motor in the disturbance observer unit 2. As shown in FIG.
This disturbance observer _calculates the difference between the motor drive torque τ _m and the estimated torsional torque τ _fh estimated by the state observer 1,
And it is intended to estimate a disturbance torque estimate tau _mih around the motor based on the difference between the motor speed estimated value omega _mh the disturbance torque estimate tau _tih around transmission mechanism. That is,
In the equation (7), J = J _M , x = τ _m −τ _fh , y = ω
_{mh -τ tih,} corresponds to the case of T _dh = τ _mih.

Therefore, the disturbance observer around the motor is:
A subtraction element 201 for subtracting the torsional torque estimated value τ _fh from the motor driving torque τ _m, and a motor speed estimated value ω _mha corrected by subtracting the disturbance torque estimated value τ _tih around the transmission mechanism from the motor speed estimated value ω _mh. obtaining subtraction element 202, the output of the subtraction element 201 (1 / TS + 1) computation element 211 for performing operations on, corrected for the motor speed estimation value _{ω mha (J M S / TS} + 1) for operation on performing arithmetic element 212 and operation It comprises a subtraction element 213 for subtracting the output of the calculation element 212 from the output of the element 211 to obtain an estimated disturbance torque τ _mih around the motor.

FIG. 9 is a block diagram showing a configuration of the disturbance observer 2 including a disturbance observer around the load, a disturbance observer around the transmission mechanism, and a disturbance observer around the motor.

FIG. 10 is a block diagram showing the configuration of the state feedback section 3. As shown in FIG. The state feedback unit 3
A subtraction element 301 for subtracting the load speed estimate ω _lh from the state observer 1 from the motor speed estimate ω _mha corrected by the disturbance observer 2, and a gain for multiplying the corrected torsional torque estimate τ _fha by a predetermined value K _1p Element 31
1, a gain element 312 for multiplying the output of the subtraction element 301 by a predetermined value K _2p , and an addition element 321 for adding the outputs of the gain elements 311 and 312.

Next, the operation will be described. State observer unit 1 inputs the motor displacement theta _m from the displacement measuring device 4. Further, a drive current value of the motor is input from the current control system 6. The motor drive torque τ _m is calculated by multiplying the drive current value by a torque constant. State observer part 1
Is calculated from the motor displacement θ _m and the motor driving torque τ _m by the configuration according to the equation (4) shown in FIG.
Estimate motor speed, load speed and torsional torque.
Then, the estimated motor displacement value θ _mh , the estimated motor speed value ω _mh , the estimated load speed value ω _lh, and the estimated torsional torque value τ
Outputs _fh . Among these, the estimated motor speed ω _mh ,
The estimated load speed ω _lh and the estimated torsional torque τ _fh are
It is given to the disturbance observer unit 2. Further, the estimated motor displacement value θ _mh is fed back to the motor displacement θ _m .

In the disturbance observer unit 2, the disturbance observer around the load inputs the estimated load speed ω _lh and the estimated torsional torque τ _fh . Then, the disturbance torque around the load is estimated. That is, the estimated disturbance torque τ _lih around the load is output. Estimated value of disturbance torque τ around load
_As described later, _lih is fed back to the torque command via a disturbance observer around the transmission mechanism and a disturbance observer around the motor. Since the estimated disturbance torque around the load is fed back, the influence of the disturbance acting on the load and the influence of the fluctuation of the load inertia J _L are reduced. Since the influence of the disturbance around the load is considered to be small, the effect of the fluctuation of the load inertia J _L is actually reduced.

The disturbance observer around the transmission mechanism is composed of the estimated torsion torque τ _fh by the state observer 1 and the estimated disturbance torque τ around the load by the disturbance observer around the load.
_lih and the corrected torsional torque estimate τ
Output _fha . The disturbance torque around the transmission mechanism is estimated using the difference between the estimated motor speed ω _mh and the estimated load speed ω _lh by the state observer 1 and the corrected estimated torsional torque τ _fha . That is, the estimated disturbance torque τ _tih around the transmission mechanism is output.

Estimated disturbance torque τ _tih around the transmission mechanism
Is fed back to the torque command via a disturbance observer around the motor, as described later. Since the disturbance torque estimated value around the transmission mechanism is fed back, the effect of disturbance that acts on the transmission mechanism and the influence of the variation of the transmission mechanism spring constant K _B is reduced. It is considered that the influence of disturbance around the transmission mechanism is small, in reality, effects of changes in the transmission mechanism spring constant K _B is reduced.

The disturbance observer around the motor outputs the corrected estimated motor speed ω _mha by taking the difference between the estimated motor speed ω _mh by the state observer 1 and the estimated disturbance torque τ _tih around the transmission mechanism. . The disturbance torque around the motor is estimated from the difference between the motor drive torque τ _m and the estimated torsional torque τ _fh by the state observer 1 and the corrected estimated motor speed ω _mha . That is, the estimated disturbance torque τ _mih around the motor is output. The estimated disturbance torque τ _mih around the motor is fed back to a torque command to the current control system 6. Since the disturbance torque estimated value about the motor is fed back, gravity acting on the motor, friction, and the effects of changes in external disturbance influences the motor inertia J _M of the interference or the like from the other axis is reduced. Since the fluctuation of the motor inertia J _M is very small, the influence of the disturbance acting on the motor is eventually reduced.

The corrected torsional torque estimated value τ _fha by the disturbance observer around the transmission mechanism, the corrected motor speed estimated value ω _mha by the disturbance observer around the motor, and the load speed estimated value ω _lh by the state observer 1 are represented by state feedback. Input to section 3. Therefore, here, the estimated load speed ω _lh is not corrected. After giving them predetermined gains K _1p and K _2p , the state feedback unit 3 feeds back to the output of the proportional-integral element 7 which gives the speed loop gain.

The speed command from the proportional element 8 and the motor speed as the feedback value match so that the positioning command value input to the comparison element 12 and the motor displacement as the feedback value match. Further, feedback control is performed so that the output of the comparison element 11 and the state quantity from the state feedback unit 3 match. Here, the corrected estimated motor speed ω _mha is used as the motor speed input to one output of the comparison element 11. As the motor displacement input to one output of the comparison element 12, the motor displacement theta _m from the displacement measuring device 4 is used as it is.

In this embodiment, the state observer 1
While it is using the same order observer to estimate based on the motor displacement theta _m as may be used observer to estimate the motor speed omega _m origin. Another observer such as a minimum dimension observer may be used.

In this embodiment, the observer gains G _{1 to} G ₄ in the state observer 1 are proportional gains. However, as shown in FIG. 11, a gain element 105 to which an integral gain is added can be used. Further, although the output of the current control system 6 is used here, the input to the current control system 6 may be an observer input.

In this embodiment, the motor displacement θ from the displacement measuring device 4 is used as the motor displacement to be fed back.
_m is used as it is. This is because the motor displacement estimation value θ _mh may have an offset due to disturbance such as gravity. On the other hand, when the resolution of the encoder used as the displacement measuring device 4 is coarse, pulse-like noise is added to the current value. Therefore, the motor displacement estimation value θ _mh
If the offset can be made smaller than the resolution of the encoder, the estimated motor displacement value θ _mh may be fed back.

Although the corrected estimated motor speed ω _mha is used as the motor speed fed back to the output of the position control system, a motor speed based on a difference between the outputs of the encoder may be used. At that time, the motor speed may be used instead of the estimated motor speed value ω _mh provided to the disturbance observer unit 2.

Embodiment 2 FIG. FIG. 12 is a block diagram showing the configuration of the disturbance observer 2 in the control device of the mechanical system according to the second embodiment of the present invention. Other configurations are the same as those shown in FIG. In this case, a coefficient element 243 is provided on the output side of the disturbance observer around the load.
A coefficient element 24 is provided on the output side of the disturbance observer around the transmission mechanism.
2 and a coefficient element 241 on the output side of the disturbance observer around the motor.

The coefficient element 243 multiplies the estimated disturbance torque τ _lih around the load by a predetermined coefficient K _disc and supplies the estimated disturbance torque τ _lih to a disturbance observer around the transmission mechanism. Coefficient element 242
_{Multiplies the} estimated disturbance torque τ _tih around the transmission mechanism by a predetermined coefficient K _disb, and then supplies the estimated disturbance torque τ _tih to a disturbance observer around the motor. Then, the coefficient element 241 multiplies the estimated disturbance torque τ _mih around the motor by a predetermined coefficient K _disa and then feeds it back to the torque command. By doing so, the noise of the current can be reduced.

Embodiment 3 FIG. When the disturbance observer 2 is configured as shown in FIG. 12, it is possible to reduce the amount of drop of the hand portion of the robot when the power is turned on as follows. The motion equation of the i-th axis of the n-degree-of-freedom robot is expressed as follows.

[0050] _{τ i = J mi (dω mi} / dt) + M ii (dω li / dt) + {Σ j M ij (dω lj / dt) -M ii (dω li / dt)} + h i + g i ·· (8) where M _ij is the element of the inertia matrix, g _i is the gravitational term on the i-axis,
h _i is the centrifugal force, Coriolis force and friction force in the section i axis, the third term is the interference term.

When power is turned on, an initial setting means (not shown)
Computes the value of the gravity term g _i and a take hand load hand of joint displacement and remotely controlled robot that time. Then, the initial value of the integrator in the proportional integral element 7 that gives the speed loop gain is set as follows. g _i · (1−K _disa ) · a Then, the start of the motor is started and the brake is released.

Further, the initial setting means may set the initial value of the estimated value of the disturbance torque around the motor to g _i · b. Here, a and b are constants, for example,
｛1 / (reduction ratio / torque constant) is adopted.

Embodiment 4 FIG. The state observer unit 1 is shown in FIG.
In the case where the configuration using the gain element 105 to which the integral gain is added as shown in (1), it is possible to reduce the amount of drop of the hand portion of the robot when the power is turned on in the following manner.

Initial setting means (not shown) when power is turned on
Computes the value of the gravity term g _i and a take hand load hand of joint displacement and remotely controlled robot that time. Then, the initial value of the integrator in the gain element 105 is set to (−g _i
/ J _Mi ) · After setting c, start the motor and release the brake. Here, c is a constant, for example, the reciprocal of the reduction ratio is adopted.

Embodiment 5 FIG. FIG. 13 is a block diagram showing a configuration of a control device for a mechanical system according to a fifth embodiment of the present invention. As shown in the figure, in this case, an input correcting means 13 for correcting the motor driving torque τ _m applied to the state observer unit 1 is provided.

In this case, the input correction means 13 inputs the motor driving torque τ _m and the estimated disturbance torque τ _mih around the motor from the disturbance observer unit 2. And τ
_A corrected motor driving torque is obtained by subtracting a value obtained by multiplying _mih by a predetermined value (a value corresponding to the degree of influence of τ _mih ) from τ _m . The input correcting means 13 gives the corrected motor driving torque to the state observer unit 1. In this case, the state observer unit 1
Uses a modified motor drive torque in place of the motor driving torque tau _m in the first embodiment. Subsequent operations are the first
This is the same as the embodiment.

In this case, since the state observer unit 1 uses the corrected motor driving torque as an input for estimation, the influence of disturbance is further reduced. Note that, also in this embodiment, the configuration shown in FIG. 11 can be used as the state observer 1.

Embodiment 6 FIG. When the control device of the mechanical system is configured as shown in FIG. 13 and the state observer 1 is configured as shown in FIG. 11, the drop amount of the robot hand at power-on is as follows. Can be reduced.

When power is turned on, initial setting means (not shown)
Computes the value of the gravity term g _i and a take hand load hand of joint displacement and remotely controlled robot that time. Then, the initial value of the integrator in the gain element 105 is set as follows. −g _i · (1−K _disa ) · a / J _Mi Also, the initial value of the estimated disturbance torque around the motor is −g
Set as _i · b. Then, the start of the motor is started and the brake is released.

Embodiment 7 FIG. FIG. 14 is a block diagram showing a configuration of a control device of a mechanical system according to a seventh embodiment of the present invention. As shown in the figure, in this case, the input correcting means 14 for correcting the motor driving torque τ _m applied to the state observer 1 and the feedforward term calculating means for performing the feedforward calculation of the gravity term, centrifugal force and Coriolis force 15 are provided.

The model shown in FIG. 2 reflects the first and second terms on the right side of equation (8). Therefore, in order to reflect the other terms, the feedforward term calculation unit 15 calculates, for example, the gravity term g _i by using the positioning instruction value and the like. Input correction means 14 inputs the motor driving torque tau _m and g _i. Then, g _i is subtracted from τ _m to obtain a corrected motor drive torque. The input correcting means 13 gives the corrected motor driving torque to the state observer unit 1. The subsequent operation is the same as in the first embodiment.

Also in this case, since the state observer unit 1 uses the corrected motor drive torque as an input for estimation, the influence of disturbance is further reduced. Note that, also in this embodiment, the configuration shown in FIG. 11 can be used as the state observer 1.

Embodiment 8 FIG. FIG. 15 is a block diagram showing a configuration of a control device for a mechanical system according to an eighth embodiment of the present invention. As shown in the figure, in this case, a control system parameter setting means 16 for setting parameters, a speed loop gain and a position loop gain in the state observer unit 1, the disturbance observer unit 2 and the state feedback unit 3 is provided. Control system parameter setting means 1
6 is the load inertia J _L of the parameters in the state observer 1 and the values of the observer gains G _{1 to} G ₄ , the load inertia J _L of the parameters in the disturbance observer 2, and the gain K of the state feedback unit 3. _1p , _K2p , speed loop gain and position loop gain are determined. Other parameters are fixed values.

The control system parameter setting means 16 externally inputs information on the driving state of the motor 5, information on the operation start or end point, and operation conditions including the hand load value. Then, in the robot-driven acceleration section, parameters in the acceleration section are determined from the operation start point and the hand load. In the deceleration section driven by the robot, parameters in the deceleration section are determined from the operation end point and the hand load. The determined parameters are given to the state observer unit 1, the disturbance observer unit 2, the state feedback unit 3, the proportional integral element 7 for providing the speed loop gain, and the proportional element 8 for providing the position loop gain. In the constant velocity section, a parameter in the acceleration section or a parameter in the deceleration section is given to them. Subsequent operations are the same as in the first embodiment.

Next, an example of a parameter setting method will be described. First, the control system parameter setting unit 16, the motor inertia J _M, load inertia J _L, transmission mechanism spring constant K _B
A relational expression between the gain and the observer gains G _{1 to} G ₄ , the gains K _1p and K _2p in the state feedback unit, the speed loop gain and the position loop gain is obtained in advance. In the acceleration section, the diagonal term M _ii of the inertia matrix M is calculated from the joint displacement at the operation start point and the value of the hand load, and the value of M _ii is set as the load inertia J _L of each axis. Next, the calculated J _L
Then, the observer gains G _{1 to} G ₄ , the gains K _1p and K _2p in the state feedback unit, the speed loop gain, and the position loop gain are obtained from the following relational expressions. In the deceleration section, similar processing is performed based on the values of the joint displacement and the hand load at the operation end point.

Although the case where the control system parameter setting means 16 is provided for the configuration shown in FIG. 1 has been described here, the control system parameter setting means 16 is provided for the configuration shown in FIG. 13 or FIG. It can also be applied.

In each of the above embodiments, a robot has been described as an example of a mechanical system. However, the present control device provides an effective control environment for mechanical systems other than the robot.

[0068]

As described above, according to the first aspect of the present invention, the control device for the mechanical system estimates the state quantity on the motor side and the state quantity on the load side, and calculates the disturbance torque from the estimated state quantity. And the estimated state quantity is corrected, and the estimated value of the disturbance torque is fed back to the torque command, and the corrected state estimate is fed back to the speed control system. Parameter fluctuation is large, and
Even when a large disturbance acts, it is possible to drive a mechanical system having a low transmission rigidity with high acceleration / deceleration, and as a result, there is an effect that a working time using a mechanical system such as a robot can be reduced. .

According to the second aspect of the present invention, the control device of the mechanical system corrects the information on the input or output of the current control system provided to the state observer using the estimated value of the disturbance torque. Therefore, in addition to the effect of the first aspect of the present invention, there is an effect that the accuracy of the estimation process of the state observer unit can be improved and the effect of disturbance can be further reduced. Therefore, the followability of the robot or the like to the target trajectory is further improved.

According to the third aspect of the present invention, the control device of the mechanical system uses the feedforward term calculated by the feedforward term calculation means to obtain information on the input or output of the current control system provided to the state observer. Since the configuration is modified, in addition to the effect of the invention described in claim 1, there is an effect that the accuracy of the estimation process of the state observer unit is improved and the effect of disturbance can be further reduced.

According to the fourth aspect of the present invention, the control device of the mechanical system is configured to set the parameters in the state observer section, the disturbance observer section and the state quantity feedback means according to the operating state of the motor. Therefore, in addition to the above effects, there is an effect that a parameter can be finely set according to the operation situation, and a device that always realizes good vibration suppression can be obtained.

[Brief description of the drawings]

FIG. 1 is a block diagram showing a configuration of a control device of a mechanical system according to a first embodiment of the present invention.

FIG. 2 is an explanatory diagram illustrating an example of a model of each axis of the robot.

FIG. 3 is a configuration diagram showing a block diagram of the model shown in FIG. 2;

FIG. 4 is a block diagram illustrating a configuration of a state observer unit.

FIG. 5 is a block diagram for explaining a method of configuring a disturbance observer.

FIG. 6 is a block diagram showing a configuration of a disturbance observer around a load.

FIG. 7 is a block diagram illustrating a configuration of a disturbance observer around a transmission mechanism.

FIG. 8 is a block diagram showing a configuration of a disturbance observer around the motor.

FIG. 9 is a block diagram illustrating a configuration of a disturbance observer unit.

FIG. 10 is a block diagram illustrating a configuration of a state feedback unit.

FIG. 11 is a block diagram showing another configuration of the state observer unit.

FIG. 12 is a block diagram showing a configuration of a disturbance observer in a control device for a mechanical system according to a second embodiment of the present invention.

FIG. 13 is a block diagram showing a configuration of a control device for a mechanical system according to a fifth embodiment of the present invention.

FIG. 14 is a block diagram showing a configuration of a control device for a mechanical system according to a seventh embodiment of the present invention.

FIG. 15 is a block diagram showing a configuration of a control device for a mechanical system according to an eighth embodiment of the present invention.

FIG. 16 is a block diagram showing a configuration of a control device of a conventional mechanical system.

FIG. 17 is a block diagram showing a configuration of a control device of another conventional mechanical system.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 State observer part 2 Disturbance observer part 3 State feedback part (state quantity feedback means) 5 Motor 6 Current control system 7 Proportional integral element (speed control system) 8 Proportional element (position control system) 9 Addition element (disturbance torque feedback means) REFERENCE SIGNS LIST 10 comparison element (state quantity feedback means) 11 comparison element (speed control system) 12 comparison element (position control system) 13 input correction means 14 input correction means 15 feedforward term calculation means 16 control system parameter setting means

Claims (4)

(57) [Claims] 1. A motor for driving a mechanical system, a current control system for controlling a current supplied to the motor, a position control system for feeding back information on the position of the mechanical system to a positioning command, and a speed of the mechanical system. And a speed control system that generates a torque command given to the current control system by feeding back information on the output of the position control system to the output of the position control system. A state observer unit for outputting a state quantity on the motor side and a state quantity on the load side for driving the mechanical system using the input or output or information on the input and output, and a disturbance torque around the motor.
Disturbance observer around motor that outputs estimated value , transmission
Outputs the disturbance torque around the transmission mechanism Outputs the disturbance observer around the transmission mechanism and the disturbance torque around the load
At least one of the disturbance observers around the loading load
The disturbance observer estimates an estimated value of the disturbance torque from the state quantity by the state observer unit and estimates the state quantity by the state observer unit in advance.
Estimated disturbance torque around the load or around the transmission mechanism
A disturbance observer unit that corrects based on the disturbance torque estimated value, a disturbance torque feedback unit that feeds back an estimated value of the disturbance torque by the disturbance observer unit to the torque command to the current control system, and the disturbance observer unit corrects the disturbance torque. A control device for a mechanical system, comprising: a state quantity feedback unit that feeds back a state quantity to an output of the speed control system . 2. The mechanical system according to claim 1, further comprising an input correcting means for correcting information relating to an input or an output of the current control system provided to the state observer using an estimated value of a disturbance torque by the disturbance observer. Control device. 3. A feed-forward term calculating means for calculating a feed-forward term of a disturbance torque, and an input correcting means for correcting information relating to an input or an output of a current control system provided to a state observer using the feed-forward term. The control device for a mechanical system according to claim 1, further comprising: 4. Based on a relational expression between an operation state of the motor, a motor inertia moment, a transmission mechanism spring constant, a load inertia moment, and parameters in a state observer, a disturbance observer, and a state quantity feedback unit, which are stored in advance. 4. The control device for a mechanical system according to claim 1, further comprising a control system parameter setting unit that sets parameters in a state observer unit, a disturbance observer unit, and a state quantity feedback unit. 5.