JPH07261843A - Control unit for mechanical system - Google Patents

Abstract

PURPOSE:To obtain the control unit which can drive the mechanical system of low transmission rigidity with high acceleration and deceleration even if parameter variation of a controlled system is large and large disturbance operates. CONSTITUTION:A state observer part 1 estimates the state quantity of a motor side and the state quantity of a load side by using information regarding the output of the motor 5 and information regarding the input or output of a current control system 6. A disturbance observer part 2 estimates disturbance torque from the state quantity estimated by the state observer part 1 and also corrects the state quantity obtained by the state observer part 1. The estimated value of the disturbance torque estimated by the disturbance observer 2 is fed back to a torque command and the state quantity corrected by the disturbance observer part 2 is fed back to a speed control system 7.

Description

Detailed Description of the Invention

[0001]

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

[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 configured for each axis. That is, as shown in FIG. 16, a displacement detector 4 such as an encoder for detecting the displacement of a motor 5 for driving a robot or the like is provided for each axis, and the motor displacement amount output from the displacement detector 4 determines the motor displacement. A difference element 22 for calculating the speed is provided. The position control system is composed of a comparison element 12 for feeding back the motor displacement amount to the positioning command value and a proportional element 8 for giving a position loop gain. The speed control system is composed of a comparison element 11 for feeding back the motor speed to the output of the position control system and a proportional integral element 7 for giving 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.

In the structure shown in FIG. 16, the positioning command value input to the comparison element 12 and the motor displacement, which is a feedback value, match each other, and the speed from the proportional element 8 input to the proportional integral element 7 is increased. Feedback control is performed so 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, it is a semi-closed loop in which the state quantity on the load side is not fed back. When only the motor speed is fed back to the speed control system, vibration occurs when a robot or the like having a low transmission mechanism rigidity is driven at high acceleration / deceleration. Therefore,
It is necessary to reduce the gain of the control system and drive at low acceleration / deceleration.

Therefore, in the speed control system, there has been proposed a method of performing all-state feedback in which the state quantity on the load side is fed back in addition to the motor speed. As an example of such a technique, there is a control method described on pages 1018 to 1025 of Electron Theory D, Vol. 107, No. 8 (1987). FIG. 17 is a block diagram showing the configuration of that system. When feeding back the state quantity on the load side, it is possible to detect the state quantity directly with a sensor. But,
It is difficult to select the sensor mounting location and design the sensor output filtering. Therefore, as shown in FIG. 17, an observer for estimating the state quantity to be fed back is incorporated.

In the method described in the above document, the observer section 21 estimates the strain amount 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 directly fed back to the current command value via the state feedback unit 3. Further, the motor speed and the motor displacement are directly fed back.

[0006]

Since the conventional controller for the mechanical system is constructed as described above, the observer section 21 estimates the state quantity on the load side, but the estimated state quantity, motor The speed and motor displacement are directly fed back. When the control target is a robot or the like, parameters such as load inertia change greatly. In addition, gravity and interference terms from other axes act as disturbances. Thus, when the parameter fluctuation of the controlled object is large or when a large disturbance acts, the characteristics of the observer unit 21 are significantly deteriorated, and as a result, the characteristics of the speed control system are also significantly deteriorated. Therefore, again, the vibration cannot be sufficiently suppressed. Therefore, the robot or the like must be driven with low acceleration / deceleration, and even if the performance of the motor 5 is good, the ability of the motor 5 cannot be fully exerted.
That is, only the 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 shortened.

The present invention has been made in order to solve the above-mentioned problems, and a mechanical system having a low transmission rigidity can be highly added even when a parameter fluctuation of a controlled object is large and a large disturbance acts. An object of the present invention is to obtain a control device for a mechanical system that enables driving with deceleration and can reduce work time using a mechanical system such as a robot.

[0008]

According to a first aspect of the present invention, there is provided a mechanical system control device, which uses information about an output of a motor and input or output of a current control system or information about an input and an output of a motor side. A state observer unit that outputs a state quantity and a state quantity on the load side, and at least one of a disturbance observer around the motor, a disturbance observer around the transmission mechanism, and a disturbance observer around the load, and by the disturbance observer, A disturbance observer unit that estimates the disturbance torque from the state amount estimated by the state observer unit and corrects the state amount by the state observer unit, and a disturbance torque feedback unit that feeds back the estimated value of the disturbance torque estimated by the disturbance observer unit to the 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 a mechanical system control apparatus according to the first aspect, wherein the disturbance observer section is provided with information about input or output of the current control system given to the state observer section. The input correction means is further provided for correcting using the estimated value of the disturbance torque.

A control device for a mechanical system according to a third aspect of the present invention is the control device for a mechanical system according to the first aspect, further comprising: feedforward term calculation means for calculating a feedforward term of disturbance torque, and a state observer section. The present invention further comprises an input correction means for correcting the information about the input or output of the current control system given by using the feedforward term calculated by the feedforward term calculation means.

A control device for a mechanical system according to a fourth aspect of the present invention is the control device for a mechanical system according to any one of the first to third aspects, in which a state observer section,
The system further comprises control system parameter setting means for setting parameters in the disturbance observer section and the state quantity feedback means according to the operating state of the motor.

[0012]

The disturbance observer unit according to the first aspect of the invention corrects the state quantity estimated by the state observer unit, estimates the disturbance torque and feeds it back to the torque command, and controls the parameters of the mechanical system to be controlled. Reduces the effects of fluctuations and disturbances on the mechanical system.

According to the second aspect of the present invention, the input correction means 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

According to the third aspect of the present invention, the input correction 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 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 the parameters used by the state observer unit, the disturbance observer unit and the like according to the operating state of the controlled object at that time.

[0016]

【Example】

Example 1. 1 is a block diagram showing the configuration of a control device for a mechanical system according to a first embodiment of the present invention. As shown in the figure, a displacement detector 4 for detecting the displacement of a motor 5 that drives a robot or the like is provided. The position control system is composed of a comparison element 12 for feeding back a motor displacement amount to a command value and a proportional element 8 for giving 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 giving a speed loop gain.

In this case, in the speed loop, the state observer section 1 for estimating the state quantities 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 section 1 are corrected. In order to feed back the output of the state feedback unit 3 to the output of the disturbance observer unit 2 that estimates the disturbance torque, the state feedback unit 3 that gives a predetermined gain (generally a constant) to the output of the disturbance observer unit 2, and the output of the proportional-plus-integral element 7. The comparison element 10 and the addition element 9 for feeding back the estimated value of the disturbance torque by the disturbance observer unit 2 are provided. 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 addition element 9 constitutes disturbance torque feedback means, and the state feedback section 3 and the comparison element 10 constitute state quantity feedback means.

Hereinafter, a robot will be taken as an example of the mechanical system. The model of each axis of the robot is considered to be a two-inertia system including a motor 30, a transmission mechanism 31 including a spring element, and a load 32, as described in the above document.
This model is shown in FIG. Moreover, the following signs are attached. J _M : Motor inertia J _L : Load inertia K _B : Transmission mechanism spring constant τ _m : Motor driving torque τ _f : Torsional torque ω _m : Motor speed ω _l : Load speed θ _m : Motor displacement

Then, the model shown in FIG. 2 is represented by the block diagram of FIG. The state equation of this model is expressed by the equation (1). However, the expression (1) includes not only the part corresponding to the model shown in FIG. 2 but also the part relating to the motor displacement θ _m . That is, the state vector x _b is θ _m , ω _m , ω
_{It has} four dimensions, _l and τ _f .

[0021]

[Equation 1]

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

[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 the configuration of the state observer unit 1 corresponding to the equation (4). The state observer unit 1 compares the estimated motor displacement value θ _mh with the comparison element 15 for feeding back the estimated motor displacement θ _m to the motor displacement θ _m.
1. Observer gains G _{1 to} G at the output of the comparison element 151
_It has each gain element 101-104 which gives ₄ .

Further, an addition element 111 and an addition element 1 for adding the output of the gain element 101 and the estimated motor speed value ω _mh
11 is integrated to obtain the motor displacement estimated value θ _mh , the output of the gain element 102 and the coefficient element 131.
Element 112 that adds the output of the above and the output of the addition element 112, and the integral element 1 that obtains the estimated motor speed value ω _mh
22, an addition element 113 that adds the output of the gain element 103 and the output of the coefficient element 132, an integration element 123 that integrates the output of the addition element 113 to obtain the load speed estimated value ω _lh , the output of the gain element 104 and the 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 that subtracts the load speed estimated value ω _lh from the motor speed estimated value ω _{mh, a} coefficient element 131 that multiplies the output of the subtraction element 141 by a constant (1 / J _M ), and a torsion torque estimated value τ _fh that is a constant (1 / J _L ), a constant (KB) is output to the coefficient element 132 and the subtraction element 142.
It has a coefficient element 133 that multiplies by.

FIG. 5 is a block diagram for explaining the method of constructing the disturbance observer. As shown in the figure, an input x and a disturbance T _d are added to a system model 100 modeled by a transfer function (1 / JS), and an output y is output from the system model 100.
Is output. Then, the output y is represented by 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, practically, the disturbance estimated value T _dh is obtained based on the 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 structure of the disturbance observer around the load in the disturbance observer unit 2. This disturbance observer estimates the disturbance torque around the load based on the torsion torque estimation value τ _fh and the load speed estimation value ω _lh estimated by the state observer 1, and outputs the disturbance torque estimation value τ _lih around the load. is there. That is,
In equation (7), J = J _L , x = τ _fh , y = ω _lh , T
_This corresponds to the case of _dh = τ _fih .

Therefore, the disturbance observer around the load performs the calculation element 231 for calculating the torsional torque estimated value τ _fh regarding (1 / TS + 1) and the calculation for the load speed estimated value ω _lh regarding (J _L S / TS + 1). From the output of the calculation element 232 and the calculation element 231 to the calculation element 232
The subtraction element 233 obtains the estimated disturbance torque τ _lih around the load by subtracting the output of _Eq .

FIG. 7 is a block diagram showing the structure of the disturbance observer in the disturbance observer section 2 around the transmission mechanism. This disturbance observer is the difference between the motor speed estimated value ω _mh estimated by the state observer 1 and the load speed estimated value ω _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 It is a thing. That is, in equation _{(7), J = 1 / K B,} x = ω mh -
This corresponds to the case of ω _lh , y = τ _{fh −τ lih} , and T _dh = τ _tih .

Therefore, the disturbance observer around the transmission mechanism subtracts the load speed estimation value ω _lh from the motor speed estimation value ω _mh and the subtraction element 203, and the torsion torque estimation value τ _{fh gives} the disturbance torque estimation value τ _lih around the load. A subtraction element 204 for obtaining a corrected twist torque estimated value τ _fha by subtraction, a calculation element 221 that performs an operation regarding (1 / TS + 1) with respect to the output of the subtraction element 203, and a corrected twist torque estimated value τ _fha (S / It is composed of a calculation element 222 that performs a calculation regarding K _B (TS + 1) and a subtraction element 223 that subtracts the output of the calculation element 222 from the output of the calculation element 221 to obtain a disturbance torque estimated value τ _{ti h} around the transmission mechanism.

FIG. 8 is a block diagram showing the structure of the disturbance observer around the motor in the disturbance observer unit 2.
This disturbance observer is the difference between the motor drive torque τ _m and the torsion torque estimated value τ _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 equation (7), J = J _M , x = τ _m −τ _fh , y = ω
_This corresponds to the case of _{mh −τ tih} and T _dh = τ _mih .

Therefore, the disturbance observer around the motor is
A subtraction element 201 for subtracting the torsion torque estimation value τ _fh from the motor drive torque τ _m, and a motor speed estimation value ω _mha corrected by subtracting the disturbance torque estimation value τ _tih around the transmission mechanism from the motor speed estimation value ω _mh. A subtraction element 202 to be obtained, an operation element 211 for performing an operation on (1 / TS + 1) for the output of the subtraction element 201, an operation element 212 for performing an operation on (J _M S / TS + 1) for the corrected motor speed estimated value ω _mha , and an operation The subtraction element 213 is configured to subtract the output of the calculation element 212 from the output of the element 211 to obtain the estimated disturbance torque value τ _mih around the motor.

FIG. 9 is a block diagram showing the structure of the disturbance observer unit 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. The state feedback unit 3 is
A subtraction element 301 for subtracting the load speed estimated value ω _lh by the state observer unit 1 from the motor speed estimated value ω _mha corrected by the disturbance observer unit 2, a gain for multiplying the corrected torsion torque estimated value τ _fha by a predetermined value K _1p Element 31
1, a gain element 312 that multiplies the output of the subtraction element 301 by a predetermined value K _2p , and an addition element 321 that adds the outputs of the gain elements 311 and 312.

Next, the operation will be described. The state observer unit 1 inputs the motor displacement θ _m from the displacement measuring device 4. Further, the 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 the torque constant. State observer section 1
Is calculated from the motor displacement θ _m and the motor drive 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 τ
Output _fh . Of 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 section 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 load speed estimated value ω _lh and the torsion torque estimated value τ _fh . Then, the disturbance torque around the load is estimated. That is, the disturbance torque estimated value τ _lih around the load is output. Estimated disturbance torque around load τ
_As will be 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 value 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 influence of the fluctuation of the load inertia J _L is actually reduced.

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

Disturbance torque estimated value τ _tih around the transmission mechanism
Is fed back to the torque command via a disturbance observer around the motor, as will be described later. Since the estimated disturbance torque value around the transmission mechanism is fed back, the influence of the disturbance acting on the transmission mechanism and the influence of the fluctuation of the transmission mechanism spring constant K _B are reduced. Since it is considered that the influence of the disturbance around the transmission mechanism is small, the influence of the fluctuation of the transmission mechanism spring constant K _B is actually reduced.

The disturbance observer around the motor takes the difference between the estimated motor speed value ω _mh by the state observer 1 and the estimated disturbance torque value τ _tih around the transmission mechanism, and outputs the corrected estimated motor speed value ω _mha . . Further, the disturbance torque around the motor is estimated from the difference between the motor drive torque τ _m and the estimated torque value τ _fh by the state observer 1 and the corrected estimated motor speed value ω _mha . That is, the estimated disturbance torque value τ _mih around the motor is output. The disturbance torque estimated value τ _mih around the motor is fed back to the torque command for the current control system 6. Since the estimated value of the disturbance torque around the motor is fed back, the influence of disturbance such as gravity acting on the motor, friction, interference from other shafts, etc. and the influence of fluctuation of the motor inertia J _M are 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 torsion torque estimated value τ _fha by the disturbance observer around the transmission mechanism, the motor speed estimated value ω _mha corrected by the disturbance observer around the motor, and the load speed estimated value ω _lh by the state observer 1 are the state feedbacks. Input to part 3. Therefore, the load speed estimate ω _lh is not modified here. The state feedback unit 3 gives them predetermined gains K _1p and K _2p, and then feeds them back to the output of the proportional-plus-integral element 7 which gives a velocity loop gain.

Then, so that the positioning command value input to the comparison element 12 and the motor displacement that is the feedback value match, the speed command from the proportional element 8 and the motor speed that is 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 motor speed estimated value ω _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 θ _m from the displacement measuring device 4 is used as it is.

In this embodiment, the state observer 1
As the above, the same-dimensional observer that estimates based on the motor displacement θ _m is used, but an observer that estimates based on the motor speed ω _m may be used. Further, 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, it is possible to use a gain element 105 to which an integral gain is added. Furthermore, 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 is. This is because the estimated motor displacement θ _mh may have an offset due to a disturbance such as gravity. On the other hand, when the resolution of the encoder used as the displacement measuring device 4 is coarse, pulsed noise is added to the current value. Therefore, estimated motor displacement θ _mh
When the offset of can be made small with respect to the resolution of the encoder, the estimated motor displacement value θ _mh may be fed back.

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

Example 2. FIG. 12 is a block diagram showing the configuration of the disturbance observer 2 in the control device for 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, the 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 _supplies the disturbance torque estimated value τ _lih around the load to the disturbance observer around the transmission mechanism after being multiplied by a predetermined coefficient K _disc . Coefficient element 242
_Supplies the disturbance torque estimated value τ _tih around the transmission mechanism to a disturbance observer around the motor after being multiplied by a predetermined coefficient K _disb . Then, the coefficient element 241 multiplies the estimated disturbance torque value τ _mih around the motor by a predetermined coefficient K _disa and feeds it back to the torque command. By doing so, current noise can be reduced.

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

Τ _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) Here, M _ij is the element of the inertia matrix, g _i is the gravity term of 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.

Initial setting means (not shown) at power-on
Calculates the value of the gravity term g _i from the joint displacement of the robot at that time and the hand load applied to the hand of the robot. Then, the initial value of the integrator in the proportional integral element 7 which gives the velocity loop gain is set as follows. g _i (1-K _disa ) -a Then, the motor is started and the brake is released.

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

Example 4. The state observer unit 1 is shown in FIG.
In the case where the gain element 105 with the integral gain is used as shown in, the drop amount of the hand of the robot when the power is turned on can be reduced as follows.

Initial setting means (not shown) at power-on
Calculates the value of the gravity term g _i from the joint displacement of the robot at that time and the hand load applied to the hand of the robot. Then, the initial value of the integrator in the gain element 105 is ( _-gi
/ J _Mi ) ・ c and then start the motor and release the brake. Here, c is a constant, and for example, the reciprocal of the reduction ratio is adopted.

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

In this case, the input correction means 13 inputs the motor drive torque τ _m and the estimated disturbance torque τ _mih around the motor from the disturbance observer unit 2. And τ
A value obtained by multiplying _mih by a predetermined value (a value corresponding to the degree of influence of τ _mih ) is subtracted from τ _m to obtain a corrected motor drive torque. The input correction means 13 gives the corrected motor drive torque to the state observer unit 1. In this case, the state observer unit 1
Uses the corrected motor drive torque in place of the motor drive torque τ _m in the first embodiment. After that, the first operation
This is the same as the case of the embodiment.

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. It should be noted that the configuration shown in FIG. 11 can be used as the state observer 1 in this embodiment as well.

Example 6. 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 amount of fall of the robot hand when the power is turned on is as follows. Can be reduced.

Initializing means (not shown) at power-on
Calculates the value of the gravity term g _i from the joint displacement of the robot at that time and the hand load applied to the hand of the robot. 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 disturbance torque estimated value about the motor, -g
Set as _i・ b. Then, the motor is started and the brake is released.

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

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

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

Example 8. FIG. 15 is a block diagram showing the configuration of the control device for the mechanical system according to the eighth embodiment of the present invention. As shown in the figure, in this case, control system parameter setting means 16 for setting the parameters in the state observer unit 1, the disturbance observer unit 2 and the state feedback unit 3, the velocity loop gain and the position loop gain is provided. Control system parameter setting means 1
6, the load inertia J _L, the gain K in the state feedback section 3 of the parameters load inertia J _L and the value of the observer gain G ₁ ~G ₄ of the parameters at the state observer unit 1, the disturbance observer unit 2 _1p , K _2p , velocity loop gain and position loop gain are determined. Other parameters are fixed values.

The control system parameter setting means 16 inputs the operating conditions including the driving state of the motor 5, information on the operation start or end point and the hand load value from the outside. Then, in the robot-driven acceleration section, the parameter in the acceleration section is determined from the operation start point and the hand load. In the robot-driven deceleration section, 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 that gives the velocity loop gain, and the proportional element 8 that gives the position loop gain. In the constant velocity section, parameters for the acceleration section or parameters for the deceleration section are given to them. The subsequent operation is the same as in the case of the first embodiment.

Next, an example of a parameter setting method will be described. First, the control system parameter setting means 16 controls the motor inertia J _M , the load inertia J _L , and the transmission mechanism spring constant K _B.
And the observer gains G _{1 to} G ₄ , the gains K _1p and K _2p in the state feedback unit, the velocity loop gain and the position loop gain, are 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 motion starting 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. Then, 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 velocity loop gain and the position loop gain are obtained from the relational expressions obtained in advance. In the deceleration section, similar processing is performed based on the joint displacement at the operation end point and the value of the hand load.

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

In each of the above embodiments, the robot has been described as an example of the mechanical system, but 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 invention, the controller of the mechanical system estimates the state quantity on the motor side and the state quantity on the load side, and the disturbance torque is estimated from the estimated state quantity. And the estimated state quantity is corrected, the estimated value of the disturbance torque is fed back to the torque command, and the corrected state estimation quantity is fed back to the speed control system. There is a large fluctuation in the parameters of
Even when a large disturbance acts, it is possible to drive a mechanical system with low transmission rigidity with high acceleration / deceleration, and as a result, there is an effect that work time using a mechanical system such as a robot can be shortened. .

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

According to the third aspect of the present invention, the controller of the mechanical system uses the feedforward term calculated by the feedforward term calculation means for the information regarding the input or output of the current control system given to the state observer section. 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 influence of disturbance can be further reduced.

According to the fourth aspect of the invention, the controller 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-mentioned effects, there is an effect that fine parameters can be set according to the operating condition, and one that always achieves good vibration suppression can be obtained.

[Brief description of drawings]

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

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

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

FIG. 4 is a block diagram showing 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 showing a configuration of a disturbance observer around a transmission mechanism.

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

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

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

FIG. 11 is a block diagram showing another configuration of a 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 conventional controller for a mechanical system.

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

[Explanation of symbols]

1 state observer section 2 disturbance observer section 3 state feedback section (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) 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

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI Technical indication H02P 5/00 X

Claims (4)

[Claims] 1. A motor for driving a mechanical system, a current control system for controlling a current applied to the motor, a position control system for feeding back information regarding the position of the mechanical system to a positioning command, and a speed of the mechanical system. In a controller of a mechanical system including a speed control system for generating 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, information on the output of the motor and the current control system. The state observer unit that outputs the state quantity on the motor side and the state quantity on the load side that drives the mechanical system by using the input or output or the information about the input and output, the disturbance observer around the motor, and the disturbance around the transmission mechanism. Disturbances around observers and loads A disturbance observer unit for estimating a disturbance torque from the state quantity by the state observer unit and correcting the state quantity by the state observer unit by at least one of the disturbance observers; and a disturbance torque by the disturbance observer unit. 2. A control device for a mechanical system, comprising: a disturbance torque feedback means for feeding back the estimated value of 1 to the torque command; and a state quantity feedback means for feeding back the state quantity corrected by the disturbance observer section. 2. The mechanical system according to claim 1, further comprising input correction means for correcting the information about the input or output of the current control system given to the state observer unit by using the estimated value of the disturbance torque by the disturbance observer unit. Control device. 3. A feedforward term calculating means for calculating a feedforward term of the disturbance torque, and an input correcting means for correcting the information on the input or output of the current control system given to the state observer section by using the feedforward term. The control device for the mechanical system according to claim 1, further comprising: 4. The parameters in the state observer section, the disturbance observer section and the state quantity feedback means are set as follows:
The control device for the mechanical system according to claim 1, 2 or 3, further comprising control system parameter setting means for setting the motor according to an operating state of the motor.