JP6845103B2 - Control systems, control methods, and control programs - Google Patents

Description

The present invention relates to control systems, control methods, and control programs.

Patent Document 1 discloses a fully closed control device for a motor. In Patent Document 1, a fully closed control device that performs position control based on a load position signal includes a model of an equivalent rigid system velocity loop and a bandpass filter.

In recent years, for example, in order to drive a link constituting a robot arm with high accuracy, a servomotor has been adopted for a joint portion of the robot arm. Here, since the load member such as the link driven by the motor is connected to the motor via a low-rigidity member such as a gear (reducer), resonance occurs due to the inertia (moment of inertia) of the motor and the load member. To do. In Patent Document 1, since the velocity loop is a model of the equivalent rigid system velocity loop, resonance caused by the low-rigidity member is not considered. Therefore, there is a problem that the control band cannot be expanded beyond the resonance frequency.

To solve such a problem, Patent Document 2 discloses a control system having a self-resonant canceling control unit. The control system disclosed in Patent Document 2 determines a virtual rotation angle for self resonance cancellation control (SRC) based on the motor shaft rotation angle and the output shaft rotation angle detected by the two encoders. It is generating. Resonance can be suppressed by feeding back this virtual angle of rotation.

Japanese Unexamined Patent Publication No. 2004-80973 Japanese Unexamined Patent Publication No. 2014-164498

The inventors have found the following problems with respect to the control system.
In the control system disclosed in Patent Document 2, the structure of the position control loop is derived for the purpose of canceling the resonance between the controlled bi-inertial frame model and the entire position feedback. Therefore, the structure of the position control system cannot be freely designed. This structure makes it difficult to design the pole arrangement used in the control system design. Therefore, since each parameter must be determined by trial and error, the number of design parameters is large and the design becomes complicated.

The present invention has been made in view of such circumstances, and provides a control system, a control method, and a control program capable of suppressing resonance and simplifying a design.

The control system according to one aspect of the present invention includes a drive means for driving a load member, a first sensor provided on the drive means side and detecting information on a drive amount of the drive means as first sensor information, and the above-mentioned. A control unit that controls the drive means by a second sensor provided on the load member side and detecting information on the displacement amount of the load member as second sensor information, and double feedback control having an inner loop and an outer loop. In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value, and in the inner loop, a weighting ratio according to the node at the center of resonance is used. The virtual speed is calculated from the drive speed which is the time differential of the drive amount and the displacement speed which is the time differential of the displacement amount, and the virtual speed is set to the target speed based on the position deviation between the position command value and the displacement amount. The feedback control is performed so that the speed follows. According to this configuration, resonance can be suppressed and the design can be easily performed.

In the above control system, the outer loop may perform P control for the position deviation, and the inner loop may perform PI control for the speed deviation between the target speed and the virtual speed. .. As a result, the gain of PI control can be set by the pole arrangement, so that the number of design parameters can be reduced and the displacement amount of the load member can be made to follow the position command value.

In the above control system, the outer loop is provided with a phase compensator for inputting the position deviation between the position command value and the displacement amount of the load member, and the phase compensation is performed by the phase compensator. The target speed may be obtained based on the phase compensation value. As a result, vibration can be suppressed.

In the above control system, the speed reduction ratio of the reduction gear provided between the load member and the drive means and r, the moment of inertia of the driving means side J _M, the viscous friction coefficient of the driving means side D is _M, the moment of inertia of the load member side _{J L,} the viscous friction coefficient of the load member side is _{D L, J SRC = J M} + J L / r, when the _{D SRC = D M + D L} / r, wherein The weighting ratio of the moment of inertia is J _M / J _SRC and (J _L / r) / J _SRC , and the weighting ratio of the viscous friction coefficient is D _M / D _SRC and ( _DL / r) / D _SRC. It may be.

The control method according to one aspect of the present invention includes a driving means for driving the load member, a first sensor provided on the driving means side and detecting information on the driving amount of the driving means as the first sensor information, and the above-mentioned. In a control system including a second sensor provided on the load member side and detecting information on the displacement amount of the load member as second sensor information, the double feedback control having an inner loop and an outer loop is used. It is a control method for controlling the driving means. In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value, and in the inner loop, it corresponds to the node at the center of resonance. Using the weighting ratio, the virtual speed is calculated from the drive speed, which is the time differential of the drive amount, and the displacement speed, which is the time differential of the displacement amount, and is based on the position deviation between the position command value and the displacement amount. The feedback control is performed so that the virtual speed follows the target speed. According to this configuration, resonance can be suppressed and the design can be easily performed.

The control program according to one aspect of the present invention includes a drive means for driving the load member, a first sensor provided on the drive means side and detecting information on the drive amount of the drive as first sensor information, and the load. Control that controls a control system including a second sensor provided on the member side and detecting information on the displacement amount of the load member as second sensor information by double feedback control having an inner loop and an outer loop. A control program for causing a computer to execute the method. In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value, and in the inner loop, a node that is the center of resonance. A virtual speed is calculated from the drive speed, which is the time differential of the drive amount, and the displacement speed, which is the time differential of the displacement amount, using the weighting ratio according to the above, and the position between the position command value and the displacement amount is calculated. Feedback control is performed so that the virtual speed follows the target speed based on the displacement. According to this configuration, resonance can be suppressed and the design can be easily performed.

INDUSTRIAL APPLICABILITY According to the present invention, it is possible to provide a control system, a control method, and a control program capable of suppressing resonance and easily performing a design.

It is a block diagram which shows the control system which concerns on this embodiment. It is a control block diagram of the control system which concerns on this embodiment.

Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. However, the present invention is not limited to the following embodiments. Further, in order to clarify the explanation, the following description and drawings have been simplified as appropriate.
The term "resonance is canceled" includes not only the case where the resonance is completely canceled but also the case where a part of the resonance remains.

First, the control system according to the present embodiment will be described with reference to FIG. The control system according to the present embodiment controls the drive of each joint such as the wrist joint, elbow joint, shoulder joint, ankle joint, knee joint, and hip joint of the robot, for example.

FIG. 1 is a block diagram showing a control system. The control system 1 according to the first embodiment includes a motor 3 for driving a load member, a first sensor 4 provided on the motor 3 side, a second sensor 5 provided on the load member side, and a motor 3. It includes a control unit 6 for controlling.

The motor 3 is a specific example of a driving means for driving the joint portion 2 to be controlled, and is, for example, an AC (alternating current) servomotor. The motor 3 is driven based on the control signal output from the control unit 6. The control signal is, for example, a PWM (Pulse Width Modulation) signal. The motor 3 is provided in the joint portion 2 and rotationally drives the joint portion 2. Further, the motor 3 may be a linear motor. The joint portion 2 has a speed reducer 2a, a link 2b, and the like. The link 2b, which is a load member, is connected to the motor 3 via the speed reducer 2a. The speed reducer 2a is provided between the motor shaft of the motor 3 and the output shaft fixed to the link 2b of the joint portion 2.

The first sensor 4 is provided on the motor 3 side of the speed reducer 2a. The first sensor 4 detects information on the driving amount of the motor 3 as the first sensor information. The information regarding the driving amount of the motor 3 is, for example, the rotation angle, the angular velocity, or the angular acceleration of the motor 3. When the first sensor 4 is an encoder of the motor 3, the first sensor 4 detects the angular velocity of the motor 3 as the first sensor information. Then, by integrating the angular velocity over time, the rotation angle, which is the driving amount of the motor 3, can be obtained. When the motor 3 is a linear motor, the position, speed, or acceleration in the straight-ahead direction is information regarding the driving amount of the motor 3.

The second sensor 5 is provided on the link 2b side of the speed reducer 2a. The second sensor detects information about the displacement amount of the link 2b as the second sensor information. The information regarding the displacement amount of the link 2b is the rotation angle (rotation position), the straight-ahead position, the angular velocity, the angular acceleration, and the like of the link 2b. When the second sensor 5 is an encoder on the load side, the second sensor 5 detects the angular velocity of the link 2b as the second sensor information. Then, by integrating the angular velocity over time, the rotation angle, which is the amount of displacement, can be obtained. By integrating the angular velocity over time, the rotation angle, which is the displacement amount of the link 2b, can be obtained. The second sensor 5 is provided on the link 2b side from the speed reducer 2a. The first sensor 4 and the second sensor 5 are not limited to the encoder, and may be configured by a potentiometer or the like.

In the present embodiment, the joint portion 2 is applied to the rotary joint portion, but the present invention is not limited to this, and for example, a translational movable portion that can be translated may be applied, and the joint portion 2 can be applied to an arbitrary movable portion of the robot. ..

In the following description, since the motor 3 is a rotary motor, the drive amount of the motor 3 and the displacement amount of the link 2b are described as angles, but when the motor 3 is a linear motor, the drive amount and the displacement amount Is the position or distance along the linear motion direction. That is, the driving amount of the motor 3 and the displacement amount of the link 2b mean an angle, a position, or a distance. Similarly, the drive speed, which is the time derivative of the drive amount of the motor 3, and the displacement speed, which is the time derivative of the displacement amount of the link 2b, are described as angular speeds, but when the motor 3 is a linear motor, the drive speed, And the displacement speed is the speed along the linear motion direction. That is, the velocity corresponds to the time derivative (time change amount) of the driving amount or the displacement amount, and means the angular velocity or the velocity. In other words, when the drive amount or displacement amount is indicated by an angle, the drive speed and displacement speed which are the time parts are angular velocities, and when the drive amount or displacement amount is indicated by the distance, the drive speed and displacement which are the time parts. Velocity becomes velocity.

The control unit 6 performs so-called feedback control in which the motor 3 is rotationally driven based on the first and second sensor information output from the first sensor 4 and the second sensor 5. The control unit 6 includes, for example, a CPU (Central Processing Unit) that performs arithmetic processing, control processing, and the like, a ROM (Read Only Memory) and a RAM (Random Access Memory) that store arithmetic programs, control programs, and the like executed by the CPU. The hardware is configured around a microcomputer consisting of a memory composed of a memory, an interface unit (I / F) for inputting / outputting signals to and from the outside, and the like. The CPU, memory, and interface unit are connected to each other via a data bus or the like. When the control unit 6 executes the control program, the control method described below is implemented.

When the link 2b and the motor 3 are connected via a highly elastic speed reducer 2a, resonance occurs due to the inertia (moment of inertia) between the motor 3 and the load (link 2b). Therefore, the control unit 6 performs self-resonance canceling control. The self-resonance canceling control can more reliably reduce the resonance generated in the joint portion 2 of the robot. Moreover, the control band can be expanded beyond the resonance frequency. In particular, since a harmonic gear or the like is used in the joint portion 2 of a humanoid robot, an industrial robot, or the like, the viscous friction becomes larger, which causes a problem. On the other hand, by performing self-resonance canceling control, a more accurate resonance suppression effect can be expected.

In the self-resonance canceling control, a controlled object having a low coupling rigidity between the motor 3 and the load is modeled as a two-inertial system. The two inertial system can be said to be a system in which the motor and the load are connected by an elastic body (reducer or the like). The control system of the control system 1 will be described with reference to FIG. FIG. 2 is a control block diagram of the control system 1.

In FIG. 2, in FIG. 2, s means differentiation and 1 / s means integration. For simplification, the reduction ratio (gear ratio) r is omitted in FIG. In other words, FIG. 2 shows the case of r = 1. Each symbol shown in FIG. 2 is as follows.
J _M : Moment of inertia on the motor side J _L : Moment of inertia on the load side J _SRC : Sum of moments of inertia (J _SRC = J _M + J _L )
D _M : Viscous friction coefficient on the motor side D _L : Viscous friction coefficient on the load side D _SRC : Sum of viscous friction coefficients ( _DSRC = D _M + D _L )
K: stiffness T _M between the motor 3 and the link _2b: drive torque of the motor 3 omega _M: motor 3 based on the first sensor information angular (driving speed)
ω _L : Angular velocity (displacement velocity) of link 2b based on the second sensor information
ω _c : APF cutoff frequency ω _SRC : Virtual angular velocity that offsets resonance
θ _L ^ref : Position command value (target angle of link 2b)
θ _L : Angle (displacement amount) of link 2b based on the second sensor information
ω _SRC ^ref : Target angular velocity of the node that is the center of resonance

As shown in FIG. 2, the control unit 6 controls the motor 3 by double feedback control including the outer loop 20 and the inner loop 30. The inner loop 30 is provided inside the outer loop 20. The inner loop 30 has a PI controller 31 and performs speed feedback control. The outer loop 20 has an APF (all-pass filter) 21 and a P controller 22, and performs position feedback control. Specifically, the control unit 6 feedback-controls the position of the link 2b by SRC-P-PI control.

_{The position command value θ L} ^ref corresponding to the target position (target angle) of the link 2b is input to the control unit 6. _{The angular velocity ω M of the} motor 3 based on the first sensor information, the angular velocity ω _L of the link 2b based on the second sensor information, _{and the angle θ L of the} link 2b based on the second sensor information are input to the control unit 6. ing.

A deviation (θ _L ^ref −θ _L ) between the position command value θ _L ^ref and the angle θ _{L of the} link 2b is input to the APF 21. This deviation (θ _L ^ref −θ _L ) is defined as the position deviation (strictly speaking, the angle deviation). The APF 21 is a phase compensator that performs phase compensation. APF21 changes only the phase characteristic without changing the amplitude characteristic. That is, the APF 21 changes only the phase characteristic without changing the gain of the position deviation. Specifically, in a frequency higher than the cut-off frequency omega _c, the phase is delayed 180 °, in the following frequency cut-off frequency omega _c, the phase does not change. The cutoff frequency ω _c can be determined based on the resonance frequency. By using a phase compensator such as APF21, the outer loop 20 can have a vibration damping function. As a result, vibration can be suppressed. It is also possible to replace the APF 21 with a phase compensator that performs phase lead compensation.

The value for which the phase compensation control is performed by the APF 21 is defined as the phase compensation value. The phase compensation value from the APF 21 is input to the P controller 22. The P controller 22 performs P control with respect to the phase compensation value. The P controller 22 outputs a value obtained by multiplying the phase compensation value by the P gain (proportional gain) as the target angular velocity ω _SRC ^ref.

The deviation between the target angular velocity ω _SRC ^ref and the virtual angular velocity ω _{SRC is input to the PI controller 31.} This deviation (ω _SRC ^ref −ω _SRC ) is defined as the velocity deviation (strictly speaking, the angular velocity deviation). The PI controller 31 performs PI control with respect to the speed deviation. The PI controller 31 synthesizes a value obtained by multiplying the speed deviation by the P gain (proportional gain) and a value obtained by multiplying the time integration of the speed deviation by the I gain (integrated gain). PI controller 31 outputs the value synthesized by the PI control to the motor 3 as a driving torque T _M. Motor 3 drives the link 2b by a drive torque _{T M.}

In the inner loop 30, the virtual angular velocity ω _SRC that cancels the resonance is calculated based on the angular _{velocity ω M of the motor 3 and the angular velocity ω L of the link 2b.} Specifically, the control unit 6 calculates the _{virtual angular velocity ω SRC} by using the weighting ratio according to the node that is the center of resonance. In the control unit 6, a weighting ratio is set according to the node that is the center of resonance of the entire two inertial system. Here, the weighting ratios of the moment of inertia are J _M / J _SRC and J _L / J _SRC , and the weighting ratios of the viscous friction coefficient are D _M / D _SRC and D _L / D _SRC .

Since the values obtained by multiplying the angular velocities ω _M and the angular velocities ω _L by the weighting ratio are added, the virtual angular velocities ω _SRC correspond to the angular velocities of the nodes that are the centers of resonance of the entire system. In other words, in the inner loop 30, _{the virtual angular velocity ω SRC of the} node that is the center of resonance of the entire two inertial system is obtained from the _{angular velocity ω M based on the first sensor information and the angular velocity ω L} based on the second sensor information. ing. As described above, in the PI controller 31, feedback control is performed so that the virtual angular velocity ω _{SRC follows the target angular velocity ω SRC} ^ref.

The above weighting ratio is when the speed reducer 2a does not decelerate, that is, when the reduction ratio is 1. If you reduced at the speed reduction ratio r, _{D L} replaces the _{D L} / r, _J L is replaced by _J L / r. That is, D _SRC = D _M + D _L / r and J _SRC = J _M + J _L / r. In this case, the weighting ratios of the moment of inertia are J _M / J _SRC and (J _L / r) / J _SRC , and the weighting ratios of the viscous friction coefficient are D _M / D _SRC and ( _DL / r) / D. It becomes _SRC.

In the outer loop 20, feedback control is performed so that _{the displacement amount θ L} of the link 2b follows the position command value θ _L ^ref. _{In the inner loop 30, the angular velocity ω M , which is the time derivative of the drive amount of the motor 3, and the angular velocity ω L} , which is the time derivative of the displacement amount of the link 2b, are used by using a weighting ratio according to the node that is the center of resonance. The virtual angular velocity ω _SRC is calculated. The virtual angular velocity ω _SRC is a virtual angular velocity that cancels out the resonance. In the inner loop 30, feedback control is performed so that the virtual angular velocity ω _{SRC follows the target angular velocity ω SRC} ^ref based on the position deviation between the _{position command value θ L} ^ref and the displacement amount θ _L.
As described above, the outer loop 20 is subjected to the position feedback control, and the inner loop 30 is subjected to the speed feedback control. The following effects can be obtained by the feedback control of this double loop.

In the outer loop 20, feedback control is performed based on the position deviation (θ _L ^ref −θ _{L ) between the position command value θ L} ^ref and the angle θ _{L of the link 2b.} Therefore, with respect to the position command value theta _L ^ref, it is possible to directly feedback control, it can be compared to Patent Document 2, to improve the followability to the position command value theta _L ^ref.

Since the inner loop 30 is rigid, the P gain and I gain of the PI controller 31 can be designed in a polar arrangement. Therefore, the number of design parameters can be reduced and the design can be facilitated. That is, two gains (P gain and I gain) in the PI controller 31 can be automatically set by designating the poles of the pole design arrangement. There are only two design parameters that the designer should design by trial and error: the P gain in the P controller 22 and the cutoff frequency ω _{c in the APF 21.} On the other hand, in Patent Document 2, three design parameters are P gain, I gain, and D gain in the PID controller. Therefore, in the control system 1 of the present embodiment, the number of design parameters can be reduced as compared with Patent Document 2, so that the control design becomes easy.

Further, since the self-resonance canceling control that cancels the resonance is performed, the control band can be expanded beyond the resonance frequency. In order to cancel the resonance, the control unit 6 calculates the virtual angular velocity of the node at the center of the resonance and controls it by the inner loop 30 which is a minor loop. As a result, the control band of the inner loop 30 can be increased. The band of the inner loop 30 can be increased to increase the disturbance response. On the other hand, in Patent Document 1, since the band of the band pal filter cannot be freely designed, it is not possible to increase the band. In the control system 1 of the present embodiment, the control band can be increased as compared with Patent Document 1. As described above, according to the present embodiment, the disturbance suppression characteristic can be improved, and the degree of freedom in designing the outer loop 20 can be increased.

Since the outer loop 20 is provided with the APF 21 as a phase compensator, the outer loop 20 can be provided with a vibration damping function. The phase compensator such as APF21 can be omitted. In this case, the position deviation is directly input to the P controller 22. When the viscous friction coefficient is large, there is a sufficient phase margin. In such a case, the disturbance suppression performance on the load side can be improved without using a phase compensator such as APF21.

The present invention is not limited to the above embodiment, and can be appropriately modified without departing from the spirit. For example, the driving means for driving the load member is not limited to the electric motor, but may be a pressure motor such as a hydraulic motor or a hydraulic motor, or any other actuator.

1 Control system 2 Joint 2a Reducer 2b Link 3 Motor 4 1st sensor 5 2nd sensor 6 Control 20 Outer loop 21 APF
22 P controller 30 Inner loop 31 PI controller

Claims (5)

The driving means for driving the load member and
A first sensor provided on the drive means side and detecting information on the drive amount of the drive means as the first sensor information.
A second sensor provided on the load member side and detecting information on the displacement amount of the load member as second sensor information.
A control unit that controls the driving means by double feedback control having an inner loop and an outer loop is provided.
In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value.
In the inner loop
Using the weighting ratio according to the node that is the center of resonance, the virtual speed is calculated from the drive speed that is the time derivative of the drive amount and the displacement speed that is the time derivative of the displacement amount of the load member.
A target speed based on the position deviation between the displacement and the position command value, have rows feedback control such that the virtual velocity to follow,
In the outer loop, P control is performed for the position deviation, and the position deviation is controlled.
In the inner loop, a control system that performs PI control on the speed deviation between the target speed and the virtual speed. The outer loop is provided with a phase compensator for inputting the position deviation between the position command value and the displacement amount of the load member.
The control system according to claim 1 , wherein the target speed is obtained based on the phase compensation value phase-compensated by the phase compensator. Let r be the reduction ratio of the speed reducer provided between the drive means and the load member.
The moment of inertia J _M of the driving means _side, the viscous friction coefficient of the driving means side and D _M,
The moment of inertia of the load member side J _L, the viscous friction coefficient of the load member side is D _L,
If J _SRC = J _M + J _L / r and D _SRC = D _M + D _L / r,
The weighting ratio of the moment of inertia is J _M / J _SRC and (J _L / r) / J _SRC .
The control system according to claim 1 or 2 , wherein the weighting ratio of the viscous friction coefficient is D _M / D _SRC and ( _DL / r) / D _SRC. The driving means for driving the load member and
A first sensor provided on the drive means side and detecting information on the drive amount of the drive means as the first sensor information.
A second sensor provided on the load member side and detecting information on the displacement amount of the load member as second sensor information.
A control method for controlling the driving means by double feedback control having an inner loop and an outer loop.
In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value.
In the inner loop
Using the weighting ratio according to the node that is the center of resonance, the virtual speed is calculated from the drive speed that is the time derivative of the drive amount and the displacement speed that is the time derivative of the displacement amount.
Feedback control is performed so that the virtual speed follows the target speed based on the position deviation between the position command value and the displacement amount.
In the outer loop, P control is performed for the position deviation, and the position deviation is controlled.
In the inner loop, PI control is performed for the speed deviation between the target speed and the virtual speed.
Control method. The driving means for driving the load member and
A first sensor provided on the drive means side and detecting information on the drive amount of the drive as first sensor information.
A control system including a second sensor provided on the load member side and detecting information on the displacement amount of the load member as second sensor information is controlled by double feedback control having an inner loop and an outer loop. It is a control program for making a computer execute the control method to be performed.
In the outer loop, feedback control is performed so that the displacement amount of the load member follows the position command value.
In the inner loop
Using the weighting ratio according to the node that is the center of resonance, the virtual speed is calculated from the drive speed that is the time derivative of the drive amount and the displacement speed that is the time derivative of the displacement amount.
Feedback control is performed so that the virtual speed follows the target speed based on the position deviation between the position command value and the displacement amount.
In the outer loop, P control is performed for the position deviation, and the position deviation is controlled.
In the inner loop, PI control is performed for the speed deviation between the target speed and the virtual speed.
Control program.

Images