JP2014164498A - Control system, disturbance estimation system, control method, control program and design method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reliably reduce resonance generated on a control object.SOLUTION: A control system 1 comprises: drive means 3 for driving a control object; first detection means 4 installed on the drive means side, for detecting angle information or position information on the drive means side; second detection means 5 installed on the control object 2 side, for detecting angle information or position information on the control object side; and self resonance cancel control means 61 for multiplying a predetermined coefficient associated with inertia and viscosity of the drive means by the angle information or the position information detected by the first detection means, multiplying a predetermined coefficient associated with inertia and viscosity of the control object by the angle information or the position information detected by the second detection means, and adding the multiplication results so as to cancel the resonance.

Description

  The present invention relates to a control system, a disturbance estimation system, a control method, a control program, and a design method that control a controlled object such as a robot while suppressing resonance.

  In recent years, the declining birthrate and aging population have become serious, and attention has been focused on the use of robots as a labor force that can be transformed into people. By the way, for example, since a speed change mechanism such as a humanoid robot has low rigidity, resonance appears at a low frequency. For this reason, the control band cannot be increased, and it is difficult to further improve the exercise performance. Therefore, it is important to introduce resonance suppression control in such a robot. On the other hand, there is known a drive device that cancels resonance that appears when a rigid body is formed from an operation point to be controlled to a measurement point (see, for example, Patent Document 1).

International Publication No. 2012/102060

  However, in the driving device shown in Patent Document 1, viscous resistance is ignored when the resonance cancellation is performed. For this reason, for example, in a drive device or the like designed so that the frictional force is particularly small, the error can be small even if the viscous resistance is ignored. On the other hand, in a drive device including a harmonic reduction device such as a robot, if resonance cancellation is performed while ignoring viscous resistance as described above, the error becomes large and resonance may remain.

  The present invention has been made to solve such problems, and provides a control system, a disturbance estimation system, a control method, a control program, and a design method that can more reliably reduce resonance generated in a control target. Is the main purpose.

One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, A second detection unit provided on the control target side for detecting angle information or position information on the control target side; and an inertia and viscosity of the driving unit with respect to the angle information or position information detected by the first detection unit. Self that cancels resonance by multiplying the angle information or the position information detected by the second detection means by a predetermined coefficient related to the inertia and viscosity of the controlled object, and adding the multiplication result. And a resonance cancellation control means.
In this aspect, the self-resonance cancellation control unit multiplies the angle information or the position information detected by the first detection unit by predetermined coefficients α and β, respectively, and detects the angle detected by the second detection unit. Information or position information is multiplied by predetermined coefficients γ and δ, respectively, and the multiplication results are added, and the predetermined coefficients α, β, γ, and δ represent the transfer function in the controlled object as a rigid body mode and a resonance mode. It may be set based on an expression that is not separated into product forms.
In this aspect, the self-resonance canceling control unit may set the predetermined coefficients α, β, γ, and δ based on the following expression (13).
In this aspect, the angle information or the position information detected by the second detection means is obtained by multiplying the angle information or the position information detected by the first detection means by a predetermined coefficient relating to the inertia and viscosity of the drive means. A disturbance observer that calculates a disturbance estimated value based on the addition result obtained by multiplying the control object by a predetermined coefficient related to the inertia and viscosity of the control target and canceling the resonance by adding the multiplication results. .
In this aspect, the self-resonance canceling control unit may generate a torque command value for the driving unit based on a disturbance estimated value calculated by the disturbance observer.
In this aspect, the self-resonance cancellation control unit may calculate an angular velocity not including the resonance component based on the following equation (15) and output the angular velocity to the disturbance observer.
In this aspect, the disturbance observer may calculate the disturbance estimated value based on the following equations (16) and (17).
In this aspect, the self-resonance cancellation control unit includes at least one second detection unit that detects angle information or position information of the control target, and the self-resonance cancellation control unit includes a plurality of inertial systems related to the control target and the drive unit. On the other hand, a feedback controller generated based on an expression that does not include a resonance component generated by calculating an equation of motion and adding the calculated equations of motion may be included.
In this one aspect, the self-resonance canceling control means includes each value obtained by multiplying the angle information or the position information detected and fed back by the first and at least one or more second detecting means by a first control gain, respectively. Deviations of the input angle command value multiplied by the second control gain are calculated, and a torque command value for the driving means is generated based on the value obtained by adding the deviations, or input A difference between the angle command value to be detected and the angle information detected and fed back by the second detection means is multiplied by the first control gain, and a torque command value for the drive means is generated based on the sum of these values. Also good.
In this one aspect, the self-resonance canceling control means includes each value obtained by multiplying the angle information or the position information detected and fed back by the first and at least one or more second detecting means by a first control gain, respectively. And calculating a deviation between the input angle command value and a value obtained by multiplying the second control gain, and generating a first torque command value for the driving means based on the value obtained by adding the deviations, The self-resonance canceling control means multiplies the angle information or the position information detected by the first and at least one or more second detection means by a third control gain, respectively, and adds a value obtained by adding the multiplication values. The disturbance observer may output a disturbance estimated value based on the value output from the self-resonance cancellation control means.
In this aspect, the self-resonance canceling control unit may generate a second torque command value based on the estimated disturbance value from the disturbance observer and the generated first torque command value.
In this aspect, the self-resonance canceling control unit outputs the generated second torque command value to the disturbance observer, and the disturbance observer receives the second torque command value from the self-resonance canceling control unit, and the The disturbance estimated value may be generated based on a value obtained by adding the multiplication values.
In this embodiment, an equation that does not include the resonance component may be generated by adding n equations of motion of the inertial system represented by the following equation (29).
In this aspect, the apparatus further includes an acceleration sensor and / or a gyro sensor that detects acceleration information of the control target, and the control is performed based on the acceleration information and / or the angular velocity information detected by the acceleration sensor and / or the gyro sensor. The angle information or position information of the target may be calculated.
In this aspect, the control target may be a movable part of a robot.
One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, A second detection means provided on the control target side for detecting angle information or position information on the control target side; the angle information or position information detected by the first detection means is multiplied by a predetermined coefficient; A disturbance observer for calculating a disturbance estimated value based on the addition result obtained by multiplying the angle information or the position information detected by the two detection means by a predetermined coefficient and adding the multiplication result to cancel the resonance. The disturbance estimation system characterized by providing may be sufficient.
One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, A second detection unit provided on the control target side for detecting angle information or position information on the control target side; and the angle information or position information detected by the first and second detection units is multiplied by a predetermined coefficient. A self-resonance canceling control means for canceling resonance by adding the multiplication results, wherein the self-resonance canceling control means is for a plurality of inertial systems related to the controlled object and the driving means. A control system including a feedback controller that is generated based on an equation that does not include a resonance component that is generated by adding an equation of motion and adding the calculated equations of motion It may be.
In this aspect, the self-resonance canceling control means is inputted with each value obtained by multiplying the angle information detected and fed back by the first and at least one or more second detecting means by a first control gain. Deviations from the value obtained by multiplying the angle command value by the second control gain may be calculated, and the torque command value for the driving means may be generated based on the value obtained by adding the deviations.
In this one aspect, the self-resonance canceling control means includes each value obtained by multiplying the angle information or the position information detected and fed back by the first and at least one second detection means by a first control gain, respectively. Deviations between the input angle command value and the value obtained by multiplying the second control gain are calculated, and a first torque command value for the driving means is generated based on the sum of the deviations. The resonance cancellation control means multiplies the angle information or the position information detected by the first and second detection means by a third control gain, and adds the multiplication value to the disturbance observer. And the disturbance observer may generate a disturbance estimated value based on the value output from the self-resonance cancellation control means.
In this aspect, the self-resonance canceling control unit may generate a second torque command value based on the estimated disturbance value from the disturbance observer and the generated first torque command value.
One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, And a second detection unit that is provided on the control target side and detects angle information or position information on the control target side, wherein the angle information or the position information is detected by the first detection unit. Is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, the angle information or the position information detected by the second detecting means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the control object, and the multiplication result The control method of the control system characterized by canceling the resonance by adding.
One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, A control program for a control system, comprising: a second detection unit that is provided on the control target side and detects angle information or position information on the control target side, the angle information or the position information detected by the first detection unit Is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, the angle information or the position information detected by the second detecting means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the control object, and the multiplication result The control program of the control system may be characterized by canceling resonance by adding and causing the computer to execute processing.
One aspect of the present invention for achieving the above object is a drive means for driving a controlled object, a first detection means provided on the drive means side for detecting angle information or position information on the drive means side, A control system design method comprising: a plurality of second detection means provided on the control target side for detecting angle information or position information on the control target side, the detection method being detected by the first and second detection means A control system design method for performing self-resonance canceling control that cancels resonance by multiplying and adding a predetermined coefficient to angle information or position information, and for a plurality of inertial systems related to the controlled object and driving means Respectively, a step of calculating an equation of motion, a step of adding the calculated equations of motion to generate an expression that does not include a resonance component, and feedback based on the generated expression And generating a control vessel, and it may be a method of designing a control system according to claim.

  According to the present invention, it is possible to provide a control system, a disturbance estimation system, a control method, a control program, and a design method that can more reliably reduce resonance that occurs in a controlled object.

It is a block diagram which shows the schematic system configuration | structure of the control system which concerns on Embodiment 1 of this invention. It is a block diagram of DOB. It is a control block diagram of the self-resonance cancellation control part of the control apparatus which concerns on Embodiment 1 of this invention. It is a figure which shows an example applied to the 3 joint leg robot. It is a figure which shows the actual value of the frequency characteristic of plant Pmm. It is a figure which shows the actual value of the frequency characteristic of plant Pmm. It is a figure which shows the nominal value of the plant determined based on the actually measured frequency characteristic. It is a figure which shows the simulation result of the self-resonance cancellation control which concerns on Embodiment 1 of this invention, and the conventional self-resonance cancellation control. It is a block diagram which shows the schematic system configuration | structure of the control system which concerns on Embodiment 2 of this invention. It is a control block diagram of the self-resonance cancellation control disturbance observer according to the second embodiment of the present invention. It is a Bode diagram which compared the self-resonance cancellation control disturbance observer. It is a Bode diagram which compared the self-resonance cancellation control disturbance observer. It is a Bode diagram which compared the self-resonance cancellation control disturbance observer. It is a Bode diagram which compared the self-resonance cancellation control disturbance observer. FIG. 6 is a block diagram showing a control stem that uses a self-resonance cancellation control unit according to the first embodiment and a self-resonance cancellation control disturbance observer according to the second embodiment. It is a figure which shows the simulation result by the control method which concerns on Embodiment 2 of this invention, and the conventional control method. It is a block diagram of a three inertia system. It is a Bode diagram of a three inertia system model in a hip joint. It is a Bode diagram of a three inertia system model in a hip joint. It is a figure which shows an example of the nominal value of the model parameter of a plant. It is a block diagram of self-resonance cancellation control of a three inertia system. FIG. 6 is a block diagram of a three-inertia self-resonance cancellation control disturbance observer. It is a control block diagram of a self-resonance cancellation control disturbance observer of an n inertia system. It is a Bode diagram of a transfer function of 1 / s ² . It is a Bode diagram of a transfer function when P _SRC when a modeling error occurs and P _SRC are nominalized by a self-resonance cancellation control disturbance observer.

Embodiment 1 FIG.
Embodiments of the present invention will be described below with reference to the drawings. The control system according to Embodiment 1 of the present invention controls driving of each joint portion such as an ankle joint portion, a knee joint portion, and a hip joint portion of a robot, for example.

  FIG. 1 is a block diagram showing a schematic system configuration of a control system according to the first embodiment. The control system 1 according to the first embodiment includes a motor 3 that drives a joint portion 2 that is a control target, a first angle sensor 4 that is provided on the motor 3 side and detects angle information on the motor 3 side, and a joint portion. A second angle sensor 5 provided on the second side for detecting angle information on the joint portion 2 side, and a control device 6 for controlling the motor 3 based on the angle information detected by the first and second angle sensors 4 and 5; .

  The motor 3 is a specific example of a driving unit, and is provided in the joint portion 2 to rotationally drive the joint portion 2. The first angle sensor 4 is a specific example of first detection means, and detects angle information (rotation angle, rotation angular velocity, rotation angular acceleration, etc.) on the motor 3 side. The second angle sensor is a specific example of the second detection means, and detects angle information (rotation angle, rotation angular velocity, rotation angular acceleration, etc.) on the joint unit 2 side. The 1st and 2nd angle sensors 4 and 5 are comprised by the encoder, the potentiometer, etc., for example. In this embodiment, the joint part 2 is applied to the rotary joint part. However, the present invention is not limited to this, and may be a translational movable part such as a translational movable part, and can be applied to any movable part of the robot. It is.

  The control device 6 performs so-called feedback control in which the motor 3 is rotationally driven based on the angle information output from the first and second angle sensors 4 and 5. The control device 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 executed by the CPU, control programs, and the like. The hardware is composed mainly of a microcomputer comprising a memory comprising a memory, an interface unit (I / F) for inputting / outputting signals to / 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 controlling each joint part of the robot, problems such as computational complexity and parameter identification errors occur. On the other hand, by using a disturbance observer (DOB), centrifugal force and Coriolis are used. Force, gravity, frictional force, and identification errors are all regarded as disturbances, and robust motion control that makes the plant nominal can be realized.

Here, the disturbance observer will be described in detail. FIG. 2 is a block diagram of the DOB. When P is an actual plant, Pn is a nominal plant, T is an input torque, d is a torque disturbance, and Q is a filter, the output can be expressed as the following equation (1).

  Although the plant can be nominalized as described above, this nominalization is applied to only one inertial system. This is because, when applied to two or more inertial systems, it is difficult to nominalize the plant by inducing large vibrations due to load inertia and springs.

  On the other hand, the control device 6 according to the first embodiment controls the joint 2 of the robot using so-called self-resonance cancellation control (SRC), which is a two-inertia resonance suppression method. It is carried out.

  When the drive of the joint portion 2 of the robot is controlled, a joint angle error occurs due to bending of a gear or belt constituting the joint portion, twisting of the shaft, or the like. Therefore, in the first embodiment, in order to suppress the joint angle error, in addition to the first angle sensor 4 on the motor 3 side, the second angle sensor 5 is also provided on the load side (gear output stage of the joint portion 2). And configures a control object with one input and two outputs. The control device 6 according to the first embodiment applies a predetermined coefficient to the angle information output and fed back from the first angle sensor 4 on the motor 3 side and the second angle sensor 5 on the joint 2 (load) side. The self-resonance canceling control for canceling the apparent plant resonance of the open loop transfer function is performed by multiplying and adding together. Since the self-resonance canceling control can be designed without using the spring constant K, it is robust to fluctuations in the spring constant K.

  Further, in the conventional self-resonance canceling control, the viscosity term is neglected by performing approximation or the like at the time of the resonance canceling, so that there is a possibility that the resonance may remain. Therefore, in the control device 6 according to the first embodiment, the predetermined coefficient is determined by strictly considering the viscosity term that has been ignored when generating the angle information that cancels the resonance, and the self-resonance cancellation control is performed. I do.

  That is, the control device 6 according to the first embodiment multiplies the angle information detected by the first angle sensor 4 by a predetermined coefficient related to the inertia and viscosity of the motor 3, and is detected by the second angle sensor 5. A self-resonance canceling control unit (one specific example of self-resonance canceling control means) 61 that cancels resonance by multiplying the angle information by a predetermined coefficient related to the inertia and viscosity of the joint 2 and adding the multiplication results. doing.

  Thereby, the resonance which arises in the joint part 2 of a robot can be reduced more reliably. In particular, joints such as humanoid robots and industrial robots use harmonic gears and the like, and the viscous friction becomes larger and becomes a problem. On the other hand, as described above, a more accurate resonance suppression effect can be expected by determining the predetermined coefficient in consideration of the viscosity term and performing self-resonance canceling control.

  Next, a specific description will be given of a method for determining the predetermined coefficient in consideration of the viscosity term in the control device 6 according to the first embodiment.

First, the moment of inertia of the motor 3, the viscosity damping coefficient of the motor 3, the angle information of the motor 3 output from the first angle sensor 4, the moment of inertia of the joint portion 2, the viscosity coefficient of the joint portion 2, and the second angle sensor 5 The angle information and the spring constant on the joint 2 (load) side to be output are J _M , B _M , θ _M , J _L , B _L , θ _L and K, respectively. Also, the nominal moment values of the moment of inertia of the motor 3, the viscosity damping coefficient of the motor 3, the moment of inertia on the joint 2 side, the viscosity coefficient of the joint 2 and the spring constant are respectively shown as J _Mn , B _Mn , J _Ln , B _Ln. , K _n and the gear ratio of the joint 2 is n. Hereinafter, everything with the subscript n is assumed to be a nominal value. Transfer function P _MM from T _M to θ _M (see FIG. 3), Transfer function P _ML from T _M to θ _L , Transfer function P _LM from d _L to θ _M , Transfer function from d _L to θ _L P _LL can be expressed by the following formulas (3) to (8).

  In the self-resonance cancellation control according to the first embodiment, feedback control design and apparent plant resonance cancellation of the open-loop transfer function are performed separately. Thereby, a control apparatus can be designed freely.

FIG. 3 is a control block diagram of the self-resonance canceling control unit of the control device according to the first embodiment. Here, when F is a model response, the closed loop transfer function G _C and the open loop transfer function G _O from the angle command value θ _L ^ref to θ _L on the joint portion 2 side (load side), the respective nominal values G _Cn , G _On can be expressed as the following formulas (9) to (12).

Further, the predetermined coefficients α, β, γ, and δ can be determined as shown in the following equation (13) so that the apparent plant resonance term of G _On shown in the above equation (12) is canceled out. In particular, the viscosity term can be strictly considered by considering the β and δ terms that have been ignored in the past.

Conventionally, the transfer function in the controlled object is expressed in the form of a product of “rigid body mode” and “resonance mode”, and since the viscosity term cannot be separated in the form of product, it has been ignored by approximation. On the other hand, in the first embodiment, the viscosity term can be strictly considered by using the following equation (13) that does not separate the transfer function in the form of the product of the “rigid body mode” and the “resonance mode”.

At this time, the following expression (14) is established.
G _On = P _SRCn C _FB
P _SRCn = 1 / J _Mn s ² (14)

As shown in the above equation (14), there is no viscosity term of the resonance component in the apparent plant P _SRCn . Therefore, since there is no viscosity term that has been ignored in the past by approximation, resonance does not remain.

The self-resonance cancellation controller 61 of the control device 6 according to the first embodiment sets the predetermined coefficients α, β, γ, and δ based on the above equation (13). Then, as shown in FIG. 3, the self-resonance canceling control unit 61 sets the predetermined coefficients α and β for the angle information θ _M from the first angle sensor 4 and the angle information θ _L from the second angle sensor 5. , Γ, and δ, respectively. Further, the self-resonance cancellation control unit 61 adds the multiplied results by the adder, and generates a motor torque command value based on the addition result.

More specifically, as shown in FIG. 3, the self-resonance cancellation controller 61 includes the model response value based on the input angle command value θ _L ^ref and the angle information θ _L of the second angle sensor 5 fed back. And the deviations are multiplied by predetermined coefficients γ and δ, respectively. Furthermore, the self-resonance cancellation control unit 61 performs integration processing only on the value obtained by multiplying the deviation by the predetermined coefficient δ.

The self-resonance canceling control unit 61 multiplies the input angle command value θ _L ^ref by the control gain, calculates a deviation between the multiplied value and the angle information of the fed back first angle sensor, and the deviation Is multiplied by predetermined coefficients α and β, respectively. Furthermore, the self-resonance cancellation control unit 61 performs integration processing only on the value obtained by multiplying the deviation by the predetermined coefficient β. Self-resonant cancellation controller 61, a predetermined coefficient as noted above alpha, beta, gamma, adds the value which is calculated by respectively multiplying the [delta], calculates a motor torque command value T _M on the basis of the added value.

  In this way, the predetermined coefficient that is the opposite phase of the control target can be determined while strictly considering the viscosity coefficients of the motor 3 and the joint portion 2, and the resonance can be greatly reduced. Moreover, since feedback control can be designed independently, feedback control of the control apparatus 6 can be designed freely.

  Next, the simulation result of the self-resonance cancellation control according to the present embodiment will be described. As shown in FIG. 4, an example applied to a three-joint leg robot will be described. The hip joint of the three-legged robot is modeled by a two-inertia system, and the self-resonance canceling control according to the first embodiment is compared with the conventional self-resonant canceling control.

5A and 5B are diagrams showing measured values of frequency characteristics of the plant Pmm. The frequency characteristics are actually measured in a state where the knee joint is bent at 90 degrees and a state where the hip joint is perpendicular to the ground. FIG. 6 shows the nominal value of the plant determined based on the actually measured frequency characteristics. The Q filter is set as shown in the following equation (24), both poles are 200 Hz, and the position command value for each joint is 0 [rad]. A step disturbance of 10 [Nm] is applied to the load side 1 sec after the start.
Q = ω _Q / (s + ω _Q ) (24) Equation ω _Q = 150 × 2π [rad / sec]

  FIG. 7 is a diagram illustrating simulation results of the self-resonance cancellation control according to the first embodiment and the conventional self-resonance cancellation control. As shown in FIG. 7, when conventional self-resonance canceling control is performed (solid line), it can be seen that the control diverges due to the remaining resonance. On the other hand, when the self-resonance canceling control according to the first embodiment is performed (dotted line), it can be seen that the control converges without any resonance remaining.

  As described above, in the control system 1 according to the first embodiment, the angle information detected by the first angle sensor 4 is multiplied by the predetermined coefficient relating to the inertia and viscosity of the motor 3 and detected by the second angle sensor 5. The obtained angle information is multiplied by a predetermined coefficient relating to the inertia and viscosity of the joint portion 2, and the multiplication result is added to cancel the resonance. As a result, the viscous term that has been ignored in the past can be strictly taken into account, so that the resonance generated in the joint portion 2 of the robot can be more reliably reduced.

Embodiment 2. FIG.
FIG. 8 is a block diagram showing a schematic system configuration of the control system according to the second embodiment. The control device 62 of the control system 10 according to the second embodiment includes a self-resonance cancellation control disturbance observer (SRCDOB: Self Resonance) that combines the two-inertia self-resonance cancellation control unit and the disturbance observer according to the first embodiment. Cancellation Control Disturbance Observer) 63 is provided. As a result, the conventional disturbance observer can limit the nominalization to only one inertial system, and can solve the two problems simultaneously that the self-resonance cancellation control is weak to the modeling error of the moment of inertia and the viscosity coefficient. That is, in the control system according to the second embodiment, disturbance can be suppressed and the control band can be improved even if the model varies in a two-inertia system.

First, the angular velocity (hereinafter referred to as the joint angular velocity) of the joint portion 2 of the robot that performs resonance cancellation and enters the rigid body mode can be defined by the following equation (15). In the following equation (15), the predetermined coefficients α, β, γ, and δ can be obtained using the above equation (13).

  In Embodiment 2, the self-resonance cancellation control disturbance observer 63 is configured by designing a disturbance observer for the output shown in the above equation (15). That is, conventionally, when designing a disturbance observer, there are two types of angle information of the second angle sensor on the joint side and angle information of the first angle sensor on the motor side, at least one of which is input to the disturbance observer. . However, since any angle information includes noise due to resonance, disturbance cannot be completely suppressed.

  On the other hand, in the self-resonance cancellation control disturbance observer 63 according to the second embodiment, the self-resonance cancellation control unit uses the predetermined coefficients α, β, γ, δ determined by the above equation (13) and the above equation (15). Then, the joint angular velocity that completely cancels the resonance is calculated, and the calculated joint angular velocity is input to the disturbance observer. Therefore, the disturbance observer can more accurately suppress the disturbance because the disturbance estimated value can be estimated with high accuracy based on the joint angular velocity that completely cancels the resonance.

FIG. 9 is a control block diagram of the self-resonance cancellation control disturbance observer according to the second embodiment. In FIG. 9, the disturbance estimated value d _SRC hat can be expressed by the following equation (16). In the following equation (16), modeling errors ΔP _MM , ΔP _ML , ΔP _LM , ΔP _LL can be represented by the following equation (17).

In the disturbance estimated value shown in the above equation (16), the resonance component depends only on the term generated by the modeling error. In the conventional disturbance observer, the resonance component was present in the coefficient of the load disturbance d _L even when the modeling error is not present. On the other hand, in the self-resonance cancellation control disturbance observer 63 according to the second embodiment, although the resonance is not canceled due to a modeling error immediately after the start of disturbance estimation, the plant is gradually made nominal and the resonance is canceled and stabilized. I understand that.

Further, assuming that Q≈about 1, the transfer function from the apparent motor torque command value T ′ _M to the differential value (joint angular velocity) of θ _SRC can be expressed by the following equation (18).

The above equation (18) coincides with the apparent plant of the above equation (14). Robust self-resonance cancellation control against fluctuations in the spring constant K, and a disturbance observer that can nominalize the apparent plant of this self-resonance cancellation control, combined with a 2-inertia feedback control that is robust to all parameter variations Can be configured. At this time, the transfer function from the apparent motor torque command value T ′ _M to θ _M can be expressed by the following equation (19).

Furthermore, since the modeling error can be expressed by the following equations (21) and (22), it can be confirmed that the rigid body mode of the plant is nominalized.

Further, based on the (19) equation, the resonant ratio H is the ratio of the resonant frequency omega _r and antiresonant frequency omega _ar can be expressed by the following equation (23), it can be seen that the resonance ratio is nominal reduction .

Next, the nominalization of the above-described self-resonance cancellation control disturbance observer 63 according to the second embodiment will be described with a specific example. For example, when the moment of inertia J _L of the load (joint 2) is changed, the plant by self-resonance cancellation control disturbance observer can be confirmed by the Bode diagram to be nominal reduction. Note that the modeling errors of J _M , B _M , and B _L can also be made nominal in the same way as J _L, and thus detailed description thereof is omitted.

FIGS. 10A and 10B are Bode diagrams comparing the case of J _L = J _Ln , the case of J _L = 2J _Ln , and the case of applying a self-resonance cancellation control disturbance observer (SRCDOB) to J _L = 2J _Ln . is there. The Q filter is set as shown in the following equation (25).
Q = ω _Q / (s + ω _Q ) (25) Equation ω _Q = 150 × 2π [rad / sec]

As shown in FIGS. 10A and 10B, it can be seen that the rigid body mode is nominalized by using the self-resonance cancellation control disturbance observer 63 according to the second embodiment. Furthermore, as shown in FIGS. 11A and B, T 'transfer function P _SRC from _M to the differential value of theta _SRC is nominal reduction, it can be confirmed that the resonance of apparent plant is able to cancel.

  Next, the control simulation in the self-resonance cancellation control disturbance observer 63 according to the second embodiment will be described in detail. Here, a control system using the self-resonance canceling control unit 61 according to the first embodiment and the self-resonance canceling control disturbance observer 63 according to the second embodiment, and a conventional PID control and an external observer are used together. Compare with control system.

FIG. 12 is a block diagram showing a control stem in which the self-resonance cancellation control unit according to the first embodiment and the self-resonance cancellation control disturbance observer according to the second embodiment are used together. Each control system controls the nominal plant P _SCRn in a state where the knee joint is bent at 90 degrees. In the control method according to the second embodiment, ω _C = −200 × 2π and the cutoff frequency of the Q filter is set as ω _Q = 150 × 2π. On the other hand, in the conventional control method, ω _C = −100 × 2π and the cutoff frequency of the Q filter is set as ω _Q = 75 × 2π. Thus, each parameter is set to a band in which control does not diverge. Under the above conditions, J _L = 2J _Ln , a step disturbance of 10 Nm is input after 1 sec, and the responses of the control method according to the second embodiment and the conventional control method are compared.

  FIG. 13 is a diagram illustrating simulation results obtained by the control method according to the second embodiment and the conventional control method. As shown in FIG. 13, in the conventional control method, it can be seen that the vibration is generated by the load side disturbance. On the other hand, according to the control method according to the second embodiment, it can be seen that the band is increased and further no vibration is generated.

  As described above, the control system 10 according to the second embodiment includes the self-resonance cancellation control disturbance observer 63 in which the two-inertia self-resonance cancellation control unit and the disturbance observer are combined. As a result, nominalization is possible even in a two-inertia system, and the modeling error of the moment of inertia and the viscosity coefficient in the self-resonance cancellation control can be greatly improved.

Embodiment 3 FIG.
In the third embodiment of the present invention, the self-resonance cancellation control unit 61 according to the first embodiment and the self-resonance cancellation control disturbance observer 63 according to the second embodiment are applied to an n inertia system (n is 3 or more). It is characterized by the point. For example, in the hip joint portion of a humanoid robot, a plurality of resonances appear at a low frequency. For this reason, resonance suppression control that is robust to modeling errors in a multi-inertia system is required. On the other hand, the self-resonance cancellation control unit and the self-resonance cancellation control disturbance observer according to the third embodiment are not only the first angle sensor 4 on the motor 3 side but also n−1 (plural) on the load side. The second angle sensor 5 is provided, and the angle information detected by the angle sensors 4 and 5 is multiplied by a predetermined coefficient and added to cancel the resonance of n−1. As a result, even when shaft torsion or deflection occurs at a plurality of locations, resonance can be canceled out at all times, so that control performance can be improved.

  Note that when a plurality of second angle sensors are provided as described above, for example, the displacement of the third (or fourth or more) inertia part can be measured with an encoder or the like due to hardware limitations of the robot. It can be difficult. In this case, the displacement of the part having the inertia may be calculated based on the acceleration information detected by the acceleration sensor and / or the gyro sensor.

  Next, an example will be described in which the self-resonance cancellation control unit and the self-resonance cancellation control disturbance observer according to the third embodiment are applied to a three-joint leg robot and its three-inertia system model. First, the basic configuration of the three-joint leg robot will be described.

  The three-joint leg robot includes three joint portions, a hip joint portion, a knee joint portion, and an ankle joint portion. As shown in FIG. 4, each motor 3 is connected to a harmonic gear via a timing belt of each joint portion 2. Each joint 3 is rotationally driven by each motor 3 being rotationally driven. Here, one resonance occurs due to the influence of the spring characteristic of the timing belt. Further, another resonance occurs in the hip joint. Therefore, the three-inertia system must be modeled at the hip joint.

FIG. 14 is a block diagram of the above-described three inertia system. The nth moment of inertia and the viscous damping coefficient are defined as _Jn and _Bn , respectively. The spring constant and the gear ratio between the nth inertia and the (n + 1) th inertia are set to K _{n, n + 1} and rn _{, n + 1} , respectively. The nominal value is assumed to have N added at the upper right of the character. In addition, in this Embodiment 3, although the case where it applies to a hip joint part among three joints is demonstrated, it is applicable similarly to a hip joint part also about a knee joint part and an ankle joint part.

Here, the frequency characteristic around the hip joint will be described. 15A and 15B are measurement results of the frequency characteristic P ₁₁ from the torque command value T ₁ to the first inertia to the joint angle θ ₁ in the hip joint part, and Bode diagrams of the three-inertia system model based on the measurement results. is there. Measurement is performed with the thigh and lower leg of the three-joint leg robot being perpendicular to the ground and the toes being horizontal to the ground. As shown in FIGS. 15A and B, the resonance occurring at 60 Hz is due to the timing belt. FIG. 16 shows nominal values of plant model parameters determined based on the frequency characteristics.

Next, the theory of the self-resonance canceling control and the self-resonant canceling control disturbance observer for the above-described three-inertia plant will be described. The equation of motion in a three inertia plant can be expressed by the following equation (26).

Furthermore, when all the above three equations of motion are added, the following equation (27) is derived.

Since the resonance component does not exist in P _SRC in the above equation (27), a feedback controller may be configured for this P _SRC . Actually, since the controller is configured for the nominal value, the feedback controller may be configured so as to satisfy the following equation (28).

  As described above, in the self-resonance canceling control according to the third embodiment, the equations of motion are calculated for the three inertial systems related to the joint unit 2 and the motor 3, respectively. Then, a feedback controller is generated based on an expression that does not include a resonance component generated by adding the calculated equations of motion.

As a result, even though P ₁ , ₁ and P _{2, 1} originally have resonance, the apparent resonance of the plant in the feedback control is canceled by adding the three signals to each other by a predetermined coefficient. This is the self-resonance canceling control of the three inertia system. In the above example, the three-inertia system has been described, but the same can be derived for four or more inertial systems. In this manner, a feedback controller that naturally cancels the resonance component can be configured based on an equation obtained by simply adding the equations of motion for each inertial system.

FIG. 17 is a block diagram of the above-described three-inertia self-resonance cancellation control. As shown in FIG. 17, the self-resonance canceling control unit receives each value obtained by multiplying the angle information θ ₁ , θ ₂ , θ ₃ detected by the first to third angle sensors by the first control gain. Deviations e ₁ , e ₂ , e ₃ from the values obtained by multiplying the angle command values θ ₁ ^ref , θ ₂ ^ref , θ ₃ ^ref by the second control gain are calculated. Then, the self-resonance cancellation controller generates a torque command value T ₁ for the motor based on a value e _SRC obtained by adding the calculated deviations e ₁ , e ₂ , e ₃ .

FIG. 18 is a block diagram of the above-described three-inertia self-resonance cancellation control disturbance observer. As shown in FIG. 18, the self-resonance canceling control unit receives each value obtained by multiplying the angle information θ ₁ , θ ₂ , θ ₃ detected by the first to third angle sensors by the first control gain. Deviations e ₁ , e ₂ , e ₃ from the values obtained by multiplying the angle command values θ ₁ ^ref , θ ₂ ^ref , θ ₃ ^ref by the second control gain are calculated. Then, the self-resonance cancellation control unit generates a first torque command value T ₁ based on the value e _SRC obtained by adding the calculated deviations e ₁ , e ₂ , e ₃ . Further, the self-resonance canceling control unit adds the respective values obtained by multiplying the angle information θ ₁ , θ ₂ , θ ₃ detected by the first to third angle sensors by the third control gain, respectively, thereby adding the joint angular velocity (θ _SRC Is calculated and output to the disturbance observer. The disturbance observer generates a disturbance estimated value d _SRC hat based on the joint angular velocity θ _SRC from the self-resonance control unit, and outputs it to the self-resonance cancellation control unit. The self-resonance canceling control unit obtains a deviation between the disturbance estimated value d _SRC hat from the disturbance observer and the generated first torque command value T ₁ ′, and sets the deviation as the second torque command value T ₁ .
The self-resonance cancellation control and the self-resonance cancellation control disturbance observer shown in FIGS. 17 and 18 described above are examples, and are not limited thereto. For example, the block diagrams shown in FIGS. It may be a control system based on deformation.

Note that, as described above, an example in which the present invention is applied to a three-inertia system model has been described. However, the present invention can be similarly applied to an n (4 ≦ n) inertia system model. In this case, all n equations of motion are added as shown in the following equation (29).

The following formula (30) is the result of adding the above formula (29).

There is no resonance component in _PSRC in the above equation (30). Since the controller is actually configured with respect to the nominal value, the controller is configured so that the nominal value of the open loop transfer function is expressed by the following equation (31).

  FIG. 19 is a control block diagram of the n-inertia self-resonance cancellation control disturbance observer configured as described above.

Next, a detailed description will be given of a simulation of control using the above-described three-inertia self-resonance cancellation control unit and the self-resonance cancellation control disturbance observer according to the third embodiment. In this simulation, comparison is made between the conventional PID control and disturbance observer control, and the control using the three-inertia self-resonance cancellation control and the self-resonance cancellation control disturbance observer according to the third embodiment. Here, a simulation is performed by giving a modeling error of moment of inertia of load J _L = 2J _Ln .

When a modeling error of J ₃ = 2J _N ³ occurs in the moment of inertia of the load, _PSRC is nominalized by the self-resonance cancellation control disturbance observer. FIG. 20A is a Bode diagram of a 1 / s ² transfer function. FIG. 20B shows P _SRC when a modeling error occurs (J ₂ = 2J ₂ ⁿ ) and P _SRC when a modeling error occurs (J ₂ = 2J ₂ ⁿ ) using a self-resonance cancellation control disturbance observer. It is a Bode diagram of a transfer function when it is nominalized. As shown in FIGS. 20A and 20B, by using the self-resonance canceling control disturbance observer, the SRC plant P _SRC is nominalized and the resonance is almost canceled even when the inertia moment J _L of the load fluctuates. I understand. Although some resonance remains, this is because Q ≠ 1, and when ω _Q → ∞, the resonance is completely cancelled.

  As described above, in the third embodiment, the equation of motion is calculated for each of the plurality of inertia systems related to the joint portion and the motor. Then, a feedback controller is generated based on an expression that does not include a resonance component generated by adding the calculated equations of motion. As a result, even when shaft twisting or bending occurs at a plurality of locations, the resonance can be canceled out, so that the control performance can be improved.

Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention.
For example, in the above-described embodiment, the present invention is applied to a joint portion of a robot as a control target. However, the present invention is not limited to this and can be applied to any movable portion.

  The present invention can also be realized, for example, by causing the CPU to execute a computer program for the self-resonance cancellation control and the disturbance observer processing.

  The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

  The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

DESCRIPTION OF SYMBOLS 1 Control system 2 Joint part 3 Motor 4 1st angle sensor 5 2nd angle sensor 6 Control apparatus

Claims (23)

Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
Second detection means provided on the control target side for detecting angle information or position information on the control target side;
The angle information or position information detected by the first detection means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, and the control is performed on the angle information or position information detected by the second detection means. Self-resonance canceling control means for canceling resonance by multiplying a predetermined coefficient relating to inertia and viscosity of the object and adding the multiplication results;
A control system comprising: The control system according to claim 1,
The self-resonance cancellation control means multiplies the angle information or position information detected by the first detection means by a predetermined coefficient α and β, respectively, and adds the angle information or position information detected by the second detection means. Respectively, by multiplying predetermined coefficients γ and δ, respectively, and adding the multiplication results,
The control system is characterized in that the predetermined coefficients α, β, γ, and δ are set based on an expression that does not separate a transfer function in the controlled object into a product of a rigid body mode and a resonance mode. The control system according to claim 2,
The self-resonance canceling control unit sets the predetermined coefficients α, β, γ, and δ based on the following formulas.
α = J _Mn
β = B _Mn
γ = J _Ln / n
δ = B _Ln / n
Where B _Mn is the nominal value of the viscous damping coefficient of the driving means, J _Mn is the nominal value of the inertia moment of the driving means, J _Ln is the nominal value of the inertia moment of the control object, and B _Ln is the The nominal value of the viscosity coefficient of the controlled object, n is the gear ratio in the controlled object. The control system according to any one of claims 1 to 3,
The angle information or position information detected by the first detection means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, and the control is performed on the angle information or position information detected by the second detection means. A control system, further comprising a disturbance observer that calculates a disturbance estimated value based on an addition result obtained by multiplying a predetermined coefficient related to inertia and viscosity of a target and adding the multiplication results to cancel resonance. The control system according to claim 4,
The self-resonance canceling control unit generates a torque command value for the driving unit based on a disturbance estimated value calculated by the disturbance observer. The control system according to claim 4 or 5, wherein
The self-resonance canceling control unit calculates an angular velocity not including the resonance component based on the following equation, and outputs the angular velocity to the disturbance observer.
In the above equation, θ _M is the angle information detected by the first detection means, and θ _L is the angle information detected by the second detection means. The control system according to any one of claims 4 to 6,
The said disturbance observer calculates the said disturbance estimated value based on a following formula, The control system characterized by the above-mentioned.
In the above equation, Q is a filter, and ΔP _MM , ΔP _ML , ΔP _LM , and ΔP _LL are modeling errors. The control system according to any one of claims 1 to 7,
Comprising at least one or more second detection means for detecting angle information or position information of the control object;
The self-resonance canceling control unit calculates a motion equation for each of a plurality of inertial systems related to the control target and the driving unit, and based on an expression that does not include a resonance component generated by adding the calculated motion equations. A control system comprising a generated feedback controller. The control system according to claim 8, comprising:
The self-resonance canceling control means includes each value obtained by multiplying the angle information or the position information detected and fed back by the first and at least one or more second detection means by a first control gain, and an input angle. Deviations between the command value and the value obtained by multiplying the second control gain are calculated, and a torque command value for the driving means is generated or input based on the value obtained by adding the deviations. And a deviation of the angle information detected and fed back by the second detecting means is multiplied by the first control gain, respectively, and a torque command value for the driving means is generated based on the added value. Control system. The control system according to claim 8, comprising:
The self-resonance canceling control means includes each value obtained by multiplying the angle information or the position information detected and fed back by the first and at least one or more second detection means by a first control gain, and an input angle. Deviations of the command value multiplied by the second control gain are calculated, and a first torque command value for the driving means is generated based on the value obtained by adding the deviations.
The self-resonance cancellation control means multiplies the angle information or the position information detected by the first and at least one or more second detection means by a third control gain, and adds a value obtained by adding the multiplication values. Output to the disturbance observer,
The said disturbance observer produces | generates a disturbance estimated value based on the value output from the said self-resonance cancellation control means, The control system characterized by the above-mentioned. The control system according to claim 10, comprising:
The self-resonance cancellation control means generates a second torque command value based on a disturbance estimated value from the disturbance observer and the generated first torque command value. The control system according to claim 11, comprising:
The self-resonance cancellation control means outputs the generated second torque command value to the disturbance observer,
The disturbance observer generates the estimated disturbance value based on a second torque command value from the self-resonance canceling control unit and a value obtained by adding the multiplication values. The control system according to any one of claims 8 to 12,
A control system that generates an equation that does not include the resonance component by adding n equations of motion of the inertial system represented by the following equation.
In the above equation, J _n is the n-th moment of inertia, K _n is the n-th gear ratio, B _n is the n-th viscosity damping coefficient, and θ _n is the angle information detected by the n-th angle sensor. is there. A control system according to any one of claims 1 to 13,
An acceleration sensor and / or a gyro sensor for detecting acceleration information of the control object;
A control system characterized by calculating angle information or position information of the control object based on acceleration information and / or angular velocity information detected by the acceleration sensor and / or gyro sensor. The control system according to any one of claims 1 to 14,
The control system is characterized in that the control target is a movable part of a robot. Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
Second detection means provided on the control target side for detecting angle information or position information on the control target side;
The angle information or position information detected by the first detection means is multiplied by a predetermined coefficient, the angle information or position information detected by the second detection means is multiplied by a predetermined coefficient, and the multiplication result is obtained. A disturbance observer that calculates a disturbance estimated value based on the addition result that cancels the resonance by adding,
A disturbance estimation system comprising: Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
Second detection means provided on the control target side for detecting angle information or position information on the control target side;
Self-resonance canceling control means for canceling resonance by multiplying the angle information or position information detected by the first and second detecting means by a predetermined coefficient and adding the multiplication result;
A control system comprising:
The self-resonance canceling control unit calculates a motion equation for each of a plurality of inertial systems related to the control target and the driving unit, and based on an expression that does not include a resonance component generated by adding the calculated motion equations. A control system comprising a generated feedback controller. The control system according to claim 17, wherein
The self-resonance canceling control means includes a value obtained by multiplying the angle information detected and fed back by the first and at least one or more second detecting means by a first control gain, and an input angle command value. 2. A control system, characterized in that a deviation between each value multiplied by a control gain is calculated, and a torque command value for the driving means is generated based on a value obtained by adding the deviations. The control system according to claim 17, wherein
The self-resonance canceling control unit is configured to multiply the angle information or the position information detected and fed back by the first and at least one or more second detecting units by a first control gain, and an input angle command. A difference between the value and a value obtained by multiplying the value by the second control gain is calculated, and a first torque command value for the driving unit is generated based on a value obtained by adding the deviations.
The self-resonance canceling control means multiplies the angle information or the position information detected by the first and second detection means by a third control gain, and adds the multiplied values to the disturbance. Output to the observer,
The said disturbance observer produces | generates a disturbance estimated value based on the value output from the said self-resonance cancellation control means, The control system characterized by the above-mentioned. The control system according to claim 19, wherein
The self-resonance canceling control unit generates a second torque command value based on the estimated disturbance value from the disturbance observer and the generated first torque command value.
A control system characterized by that. Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
A control method of a control system comprising: second detection means provided on the control target side for detecting angle information or position information on the control target side,
The angle information or position information detected by the first detection means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, and the angle information or position information detected by the second detection means is subject to the control object. A control method for a control system, comprising: multiplying a predetermined coefficient related to inertia and viscosity of the control unit and adding the multiplication results to cancel resonance. Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
A control program for a control system comprising: second detection means provided on the control target side for detecting angle information or position information on the control target side,
The angle information or position information detected by the first detection means is multiplied by a predetermined coefficient relating to the inertia and viscosity of the drive means, and the angle information or position information detected by the second detection means is subject to the control object. A control program for a control system, characterized by causing a computer to execute processing to cancel resonance by multiplying a predetermined coefficient relating to inertia and viscosity of, and adding the multiplication results. Driving means for driving the controlled object;
First detection means provided on the drive means side for detecting angle information or position information on the drive means side;
A control system design method comprising: a plurality of second detection means provided on the control target side for detecting angle information or position information on the control target side;
A control system design method for performing self-resonance canceling control for canceling resonance by multiplying and adding a predetermined coefficient to angle information or position information detected by the first and second detecting means,
Calculating a motion equation for each of a plurality of inertial systems related to the controlled object and the driving means;
Adding the calculated equations of motion to generate an expression that does not include a resonance component;
Generating a feedback controller based on the generated equation;
A control system design method characterized by comprising: