JP2015170208A - Control device, control method and control program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To cancel resonance by driving a controlled system by two pieces of drive means.SOLUTION: A control device includes: first drive means which is provided at a driving side to drive a controlled system and drives the controlled system via torque transmission means; second drive means which is provided at a controlled system side and drives the controlled system; command generation means which generates a torque command value for driving the first and second drive means; and resonance cancellation means which multiplies the torque command value generated by the command generation means by a first gain including inertia coefficients and viscosity coefficients at the driving side and the controlled system side and a speed reduction rate of the torque transmission means, multiplies the torque command value by a second gain including the inertia coefficients and the viscosity coefficients at the driving side and the controlled system side and the speed reduction rate of the torque transmission means, and cancels resonance by adjusting a ratio of the torque command value to the first and second drive means.

Description

  The present invention relates to a control device, a control method, and a control program for driving and controlling a controlled object by two driving means.

  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.

  In contrast, a plurality of detection means for detecting information on the drive means side and the control target side are provided, and self-resonance canceling control for canceling resonance by driving the drive means based on the information detected by each detection means. There is a known control device (see Non-Patent Document 1, for example).

Motonobu Aoki, Hiroshi Fujimoto, Yoichi Hori, Taro Takahashi, Robust resonance suppression control of humanoid robot using n-inertia self-resonance cancellation control and self-resonance cancellation disturbance observer

  However, the control device drives the control object by one drive unit and cancels the resonance, and does not drive the control object by two drive units and cancels the resonance.

  The present invention has been made to solve such problems, and provides a control device, a control method, and a control program capable of canceling resonance by driving a controlled object by two driving means. Main purpose.

One aspect of the present invention for achieving the above object is provided on a drive side for driving a controlled object, and is provided on a first drive means for driving the controlled object via a torque transmitting means, and on the controlled object side. A second driving means for driving the controlled object, a command generating means for generating a torque command value for driving the first and second driving means, and a torque command value generated by the command generating means. Multiplying the torque command value by the first gain including the inertia coefficient, the viscosity coefficient, and the reduction ratio of the torque transmission means on the drive side and the control target side. Resonance cancellation means for canceling resonance by multiplying a second gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means, and adjusting a ratio of the torque command value to the first and second drive means When, Comprising, it is a control apparatus according to claim.
In this aspect, the resonance canceling unit multiplies the torque command value generated by the command generating unit by a first gain (α + β / s) and a second gain (γ + δ / s), respectively. The resonance may be canceled by adjusting the ratio of the torque command value to the first and second driving means.
In this aspect, the resonance cancellation means includes a first filter that extracts a high frequency component from the torque command value generated by the torque command generation means, and a low frequency component from the torque command value generated by the torque command generation means. A high frequency torque command value extracted by the first filter and multiplying the high frequency torque command value by a first gain and a second gain, respectively, to adjust the torque command value. The first and second driving means may be controlled by canceling the resonance, and the first driving means may be controlled based on the low-frequency torque command value extracted by the second filter.
In this aspect, the resonance cancellation means includes a first filter that extracts a high frequency component from the torque command value generated by the torque command generation means, and a low frequency component from the torque command value generated by the torque command generation means. has a second filter, the of extracting, with respect to the first high-frequency torque command values extracted by the filter, the first gain _{(α V + β V / s} ), and, second gain (gamma _V + δ _V / s), respectively, and adjusting the torque command value cancels the resonance, thereby controlling the first and second driving means, and at the same time, the low frequency extracted by the second filter Using the torque command value as well, the first driving means is controlled to adjust the parameter α _V (0 ≦ α _V ≦ 1), and parameters β _V , γ _V , and δ are adjusted based on the adjusted parameter α _V. the _V It may be set using the serial type.
In this aspect, the first filter is a high-pass filter (1-F (s)),
The second filter may be a low pass filter (F (s)).
In this aspect, the resonance canceling means, after setting the parameter alpha _V, based on the set parameter alpha _V adjusts the cutoff frequency of the high pass filter and a low pass filter may be set.
In this aspect, the control target may be a joint part of a robot.
One aspect of the present invention for achieving the above object is provided on a drive side for driving a controlled object, and is provided on a first drive means for driving the controlled object via a torque transmitting means, and on the controlled object side. And a second driving means for driving the controlled object, comprising: generating a torque command value for driving the first and second driving means; and the generated The torque command value is multiplied by a first gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, and the drive side and The resonance is achieved by multiplying the second gain including the inertia coefficient, viscosity coefficient, and reduction ratio of the torque transmission means on the control target side, and adjusting the ratio of the torque command value to the first and second drive means. Offset And a control method of a control device characterized by comprising:
In this aspect, the generated torque command value is multiplied by a first gain (α + β / s) and a second gain (γ + δ / s), respectively, and the first and second driving means are The resonance may be canceled by adjusting the ratio of the torque command value.
In this aspect, the method further includes: extracting a high frequency component from the generated torque command value by a first filter; and extracting a low frequency component from the generated torque command value by a second filter; The extracted high-frequency torque command value is multiplied by the first and second gains respectively, and the torque command value is adjusted to cancel resonance and control the first and second drive means. At the same time, the first driving means may be controlled using the extracted low-frequency torque command value.
In this aspect, the method further includes: extracting a high frequency component from the generated torque command value by a first filter; and extracting a low frequency component from the generated torque command value by a second filter;
The extracted high frequency torque command value is multiplied by a first gain (α _V + β _V / s) and a second gain (γ _V + δ _V / s), respectively, and the torque command value is adjusted. Accordingly, the first and second driving units are controlled by canceling the resonance, and the first driving unit is controlled based on the extracted low-frequency torque command value, and the parameter α _V (0 ≦ α _V ≦ 1) may be adjusted, and parameters β _V , γ _V , and δ _V may be set using the following formula based on the adjusted parameter α _V.
In this aspect, the first filter is a high-pass filter (1-F (s)),
The second filter may be a low pass filter (F (s)).
In this one embodiment, after setting the parameter alpha _V, based on the set parameter alpha _V adjusts the cutoff frequency of the high pass filter and a low pass filter may be set.
In this aspect, the control target may be a joint part of a robot.
One aspect of the present invention for achieving the above object is provided on a drive side for driving a controlled object, and is provided on a first drive means for driving the controlled object via a torque transmitting means, and on the controlled object side. A control program for a control device comprising: a second driving means for driving the control object; and a process for generating a torque command value for driving the first and second driving means, and the generated The torque command value is multiplied by a first gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, and the drive side and The resonance is achieved by multiplying the second gain including the inertia coefficient, viscosity coefficient, and reduction ratio of the torque transmission means on the control target side, and adjusting the ratio of the torque command value to the first and second drive means. cancel To execute a management and the computer can be a control program of the control device according to claim.

  According to the present invention, it is possible to provide a control device, a control method, and a control program that can perform resonance cancellation by driving a controlled object by two driving means.

It is a figure which shows an example of the robot which the control apparatus which concerns on Embodiment 1 of this invention controls. It is a figure which shows the structure of each joint part. It is the figure which modeled the control apparatus of the 2 inertia system which concerns on Embodiment 1 of this invention. It is a block diagram which shows the control apparatus of 2 inertia systems. It is a block diagram which shows the system configuration | structure of the control apparatus which concerns on Embodiment 1 of this invention. It is a block diagram of the resonance cancellation part of the control apparatus which concerns on Embodiment 1 of this invention. It is a block diagram of the resonance cancellation part of the control apparatus which concerns on Embodiment 2 of this invention. It is a Bode diagram of transfer functions P _LM (s), P _{L, ARC} (s), and P _FS-ARC (c). when the securing the f _LPF to 0 (Hz) alpha _V was varied from 0 to 1, a Nyquist diagram. The f _LPF from 0 Fixed alpha _V to 0 when changing the ∞, a Nyquist diagram. It is. It is a Nyquist diagram when three conditions are set. It is a figure which shows an example of an experimental apparatus. It is a Bode diagram of a measurement result of a frequency characteristic, and a two inertia system model by the measurement result. It is a figure which shows an example of the nominal value of the plant determined based on the frequency characteristic of FIG. It is the Nyquist diagram measured using the experimental apparatus. It is a figure which shows the step response waveform of the rotation angle of a load side motor. It is a figure which shows the torque waveform of the load side motor of Case1 and 2 in a step response. It is a figure which shows the disturbance response waveform of the rotation angle of a load side motor.

Embodiment 1
Embodiments of the present invention will be described below with reference to the drawings.
FIG. 1 is a diagram illustrating an example of a robot controlled by the control device according to the first embodiment of the present invention. For example, the control device according to the first embodiment controls driving of the legged robot 100 having three joints. The legged robot 100 has a hip joint 101, a knee joint 102, and an ankle joint 103. As shown in FIG. 2, each joint unit 101 to 103 is provided with a motor. Each motor is engaged with each joint part 101-103 via the harmonic gear 104 and the timing belt 105, for example.

  The control device according to the first embodiment drives and controls the joint portions 101 to 103 that are the control targets. The control device is modeled as a two-inertia system in consideration of the characteristics of a spring element such as the timing belt 105, for example. FIG. 3 is a diagram obtained by modeling the two-inertia control device according to the first embodiment.

The control device 1 according to the first embodiment includes a rotation sensor 2, a drive side motor 3, a load side motor 4, a spring element 5, a speed reducer 6, and a control device 7.
The rotation sensor 2 is, for example, a potentiometer, a rotary encoder or the like provided in the load (joint portion) 8 or the load side motor 4 and detects rotation information (rotation angle, rotation angular velocity, rotation angular acceleration) of the load 8. The rotation sensor 2 outputs the detected rotation information of the load 8 to the control device 7.

  The drive side motor 3 is a specific example of the first drive means, and is provided on the drive side that drives the load 8. The drive motor 3 is engaged with a load 8 via a spring element 5 and a speed reducer 6 (one specific example of torque transmitting means). The drive motor 3 drives the load 8 via the spring element 5 and the speed reducer 6.

  The load side motor 4 is a specific example of the second drive means, and is provided on the load side (control target side). The load side motor 4 is connected to the speed reducer 6. The load 8 is directly connected to the load side motor 4 via the speed reducer 6. Thus, the torque of the drive side motor 3 is decelerated via the speed reducer 6 and transmitted to the load 8. On the other hand, the torque of the load side motor 4 is directly transmitted to the load 8 without being decelerated by the speed reducer 6.

  In FIG. 3, T is torque, ω is angular velocity, θ is rotation angle, J is moment of inertia, B is a viscous friction coefficient, K is a spring constant of the spring element, and r is a reduction ratio of the speed reducer 6. . A subscript M attached to each parameter T, ω, θ, J, B indicates a drive side, and a subscript L indicates a load side. In the present embodiment, the reduction gear 6 may be configured not to decelerate (r = 1).

  For example, the control device 7 performs feedback control on the drive side motor 3 and the load side motor 4 based on the rotation information from the rotation sensor 2. The control device 7 includes, for example, a CPU (Central Processing Unit) 7a that performs arithmetic processing, control processing, etc., a ROM (Read Only Memory) or a RAM (Random Access Memory) that stores arithmetic programs executed by the CPU 7a, control programs, etc. ), And a microcomputer including an interface unit (I / F) 7c for inputting / outputting signals to / from the outside. The CPU 7a, the memory 7b, and the interface unit 7c are connected to each other via a data bus or the like.

  Here, a general two-inertia control device will be described. FIG. 4 is a block diagram showing a two-inertia control device. The transfer function of the control device shown in FIG. 4 can be expressed by the following equations (1) to (5).

However, the transfer function from the drive side torque T _M to the motor rotation angle θ _M is P _MM , the transfer function from the drive side torque T _M to the load rotation angle θ _L is P _LM , and the load side torque _TL to the motor rotation angle θ The transfer function up to _M is P _ML , and the transfer function from the load side torque _TL to the load rotation angle θ _L is P _LL .

From the above equation, the 2-inertia control device has a resonance angular frequency ω _p shown in the following equation (6) from the denominator polynomial and an anti-resonance angular frequency ω _z shown in the following equation (7) from the numerator polynomial. Resonance occurs.

  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, the present applicant has disclosed in Japanese Patent Application No. 2013-034706, a resonance cancellation control (SRC: Self Resonance) by a single input multiple output (SIMO) controller using one actuator and a plurality of sensors. Cancellation).

  As a dual control system, the control device 1 according to the first embodiment is a multiple input single output (MISO), and resonance cancellation control (ARC: Actuation Resonance Cancellation) by a plurality of actuators. I do. Thereby, similarly to SRC, robust control with respect to vibration suppression control and modeling error can be realized.

The control device 1 according to the first embodiment multiplies the torque command values for the drive side and load side motors 3 and 4 by a first gain including inertia coefficients, viscosity coefficients, and reduction ratios on the drive side and load side, The torque command value is multiplied by a second gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio on the drive side and the load side. Thereby, the ratio of the torque command value to the drive side and load side motors 3 and 4 can be adjusted, and the shaft torsional resonance caused by the spring element 5 can be canceled.
Normally, when axial torsional resonance is caused by the spring element 5, as described above, the control band cannot be increased beyond the resonance frequency. On the other hand, according to the control device 1 according to the first embodiment, the control band can be increased by canceling the shaft torsional resonance, and the control performance can be improved.

  As shown in FIG. 3, the control device 1 according to the first embodiment mounts a load side motor 4 on the load side of the two-inertia system in addition to the drive side motor 3 and performs resonance cancellation control. FIG. 5 is a block diagram showing a system configuration of the control device according to the first embodiment. The control device 7 according to the first embodiment includes a command value generation unit 71 that generates a torque command value and a resonance cancellation unit 72 that performs resonance cancellation control.

The command value generation unit 71 is a specific example of command generation means, and generates a torque command value for driving the drive side motor 3 and the load side motor 4 based on the rotation information from the rotation sensor 2. Command value generation unit 71 outputs the resonance offset portion 72 the generated torque command value as the input torque T _in.

  The resonance canceling unit 72 is a specific example of the resonance canceling means, and a first gain (α + β / s) including an inertia coefficient, a viscosity coefficient, and a reduction ratio is added to the torque command values for the drive side and load side motors 3 and 4. Multiplying the torque command values for the drive side and load side motors 3 and 4 by the second gain (γ + δ / s) including the inertia coefficient, the viscosity coefficient, and the speed reduction ratio allows the drive side and load side motors 3 to be multiplied. 4 is adjusted to cancel the resonance.

FIG. 6 is a block diagram of the resonance canceling unit of the control device according to the first embodiment.
The resonance canceling unit 72 multiplies the input torque T _in from the command value generating unit 71 by the first gain (α + β / s) to calculate the load side torque _TL .
In the controlled object model, the deviation of the value obtained by multiplying the reduction ratio r of the rotation angle theta _L of the load-side motor 4, and the rotation angle theta _M of the drive-side motor 3 is calculated. The calculated deviation is multiplied by the spring constant K and the reduction ratio r, and the calculated multiplication value is added to the load side torque _TL calculated by the resonance canceling unit 72. The added value is multiplied by a first-order lag transfer function (1 / (J _L s + B _L )), and an integration process is performed, whereby the rotation angle θ _L of the load side motor 4 is calculated.

Similarly, the resonance offset portion 72, a second gain (γ + δ / s) by multiplying the input torque T _in from the command value generating unit 71 calculates the drive-side torque T _M.
In the controlled object model, the deviation of the value obtained by multiplying the reduction ratio r of the rotation angle theta _L of the load-side motor 4, and the rotation angle theta _M of the drive-side motor 3 is calculated. The calculated deviation is multiplied by a spring constant K, and the calculated multiplication value is subtracted from the load side torque _TL calculated by the resonance canceling unit 72. The subtracted value is multiplied by a first-order lag transfer function (1 / (J _M s + B _M )), and an integration process is performed to calculate the rotation angle θ _M of the drive side motor 3.

The transfer function of the control system shown in FIG. 6 can be expressed by the following equations (8) and (9).
However, the transfer function from the input torque T _in to the rotation angle θ _L of the load side motor 4 is P _L (s), and the transfer function from the input torque T _in to the rotation angle θ _M of the drive side motor 3 is P _M ( s).

Coefficients α, β, γ, and δ are set so that the resonance terms disappear in the transfer functions P _L (s) and P _M (s). The coefficients α, β, γ, and δ can be set as shown in the following equations (10) to (13) without using the spring constant K.

In the above equation, α is an apparent inertia ratio when viewed from the drive side, and takes a value of 0 to 1.
Coefficients as shown in the above (10) to (13) α, β, γ, by setting the [delta], of the load-side motor 4 from the input torque T _in the rotational angle θ of the transfer function up to _L P _{L (s} ), and, the transfer function from the input torque _{T in} to the rotational angle theta _M of the drive-side motor 3 _P M (s), respectively, can be expressed by the following (14) and (15). Therefore, there is no resonance term in these transfer functions P _L (s) and P _M (s).

  As described above, the resonance canceling unit 72 has the first gain (α + β / s) based on α, β, γ, and δ set by the above equations (10) to (13) with respect to the torque command value. And by multiplying the second gain (γ + δ / s) respectively, the ratio of the torque command values for the drive side and load side motors 3 and 4 can be adjusted to cancel the resonance.

  In the first embodiment, the load side motor 4 drives the load 8 directly. That is, the load 8 is directly driven by the load-side motor 4 and needs to be decelerated by the speed reducer 6, or the load 8 is directly driven by the load-side motor 4 and needs to be decelerated by the speed reducer 6. It can also be applied to configurations that do not. The coefficients α, β, γ, and δ do not include the spring constant K. For this reason, the control device 1 according to the first embodiment has robustness with respect to the spring constant K.

  As described above, in the control device 1 according to the first embodiment, the torque command values for the drive side and load side motors 3 and 4 are multiplied by the first gain including the inertia coefficient, viscosity coefficient, and reduction ratio on the drive side and load side. Then, the torque command value is multiplied by a second gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio on the drive side and the load side. Thereby, the ratio of the torque command value for the drive side and load side motors 3 and 4 can be adjusted to cancel the resonance. That is, resonance cancellation can be performed by driving the controlled object by two actuators.

Embodiment 2
The control apparatus according to the second embodiment of the present invention performs resonance cancellation control (FS-ARC) with separated drive frequencies. FIG. 7 is a block diagram showing a resonance canceling unit of the control device according to the second embodiment. The resonance canceling unit 20 according to the second embodiment further includes a low-pass filter 21 and a high-pass filter 22 in the configuration of the resonance canceling unit 72 according to the first embodiment.

The resonance cancellation unit 20 extracts a high frequency component from the input torque T ′ _in from the command value generation unit 71 using the high-pass filter (1 -F (s)) 22. The resonance canceling unit 20 multiplies the high-frequency input torque T _in extracted by the high-pass filter 22 by the first gain (α + β / s) and the second gain (γ + δ / s), respectively, and loads the load side torque _TL and the drive side. by calculating the torque T _M, and controls the load side and the drive side motor while offsetting the resonance.

The resonance canceling unit 20 extracts a low frequency component from the input torque T ′ _in from the command value generating unit 71 using the low-pass filter (F (s)) 21. Resonance cancellation unit 20 by calculating a driving-side torque T _M on the basis of the input torque of the low-frequency extracted by the low-pass filter 21, and controls the drive-side motor 3.

In FIG. 7, F (s) is a first-order low-pass filter having a cutoff frequency f _LPF [Hz], and can be expressed by the following equation (16).

  The low-pass filter F (s) and the high-pass filter (1-F (s)) are examples, and the present invention is not limited to this. Instead of the low-pass filter F (s), for example, a second-order Butterworth filter may be used. The low-pass filter F (s) and the high-pass filter (1-F (s)) are reduced from the torque command value. Any filter having a function of extracting a frequency component and a high-frequency component can be used.

The transfer function P _FS-ARC (s) from the input torque T ′ _in to the rotation angle θ _L of the load side motor 4 can be expressed by the following equation (17).

  As shown in FIG. 7, a low-pass filter 21 and a high-pass filter 22 are configured. Thereby, the load 8 is driven by the motor torque of the driving motor 3 through the low-pass filter 21 in the low frequency region where there is no resonance. On the other hand, in a relatively high frequency region where resonance exists, the load 8 is driven while performing resonance cancellation control by the motor torque of the drive side motor 3 and the load side motor 4 via the high pass filter 22.

In the second embodiment, the drive side motor 3 is driven by an input torque having a relatively low frequency, and the load side motor 4 is driven by an input torque having a relatively high frequency. Since the torque having a relatively high frequency component is a torque that may excite vibration, and the relatively low frequency component is a torque that has a low possibility of exciting vibration, the load-side motor 4 may excite vibration. Since the motor is driven only for a certain torque, a motor with a small rated output can be used as the load side motor 4. For example, by suppressing the output of the load side motor 4 to be small, it is not necessary to provide a large motor, which leads to miniaturization of the robot itself.
In the second embodiment, the same parts as those in the first embodiment are denoted by the same reference numerals, and detailed description thereof is omitted.

Embodiment 3
In the resonance cancellation control (hereinafter referred to as ARC) according to the first embodiment, the resonance is canceled and the resonance mode from the input torque to the load-side rotation angle is made unobservable. On the other hand, in the resonance cancellation control (hereinafter referred to as FS-ARC) according to the second embodiment, it is possible to make the resonance mode observable and suppress vibration due to disturbance by not canceling the resonance completely. . Thereby, even when a disturbance is input to the system, unexpected vibrations can be prevented.

In the third embodiment, α used in the above equation (17) is a variable control parameter α _V (0 ≦ α _V ≦ 1) that varies between 0 and 1. Thereby, the vibration suppression effect of a response and a disturbance response can be adjusted from an input. Using the variable control parameter α _V , β _V , γ _V , and δ _V can be expressed as the following formulas (18) to (20).

When the variable control parameter α _V = α and the cutoff frequency f _LPF = 0 (Hz), the FS-ARC according to the third embodiment is equivalent to the ARC according to the first embodiment.
FIG. 8 is a Bode diagram of the transfer functions P _LM (s), P _{L, ARC} (s), and P _FS-ARC (c). In FS-ARC (s), the cut-off frequency f _LFP of the low-pass filter F (s) is set to 5 (Hz), which is smaller than 55 (Hz) of resonance, for example.

  As shown in FIG. 8, it can be seen that the application of ARC eliminates the resonance of the two-inertia system. It can be seen that there is almost no resonance even in the FS-ARC in which the drive frequency is separated. Therefore, it can be seen that the resonance can be eliminated by applying ARC and FS-ARC. Furthermore, the final phase of ARC and FS-ARC is −180 (deg), and it can be said that the system is easy to control.

Next, a feedback control system is constructed for the FS-ARC system shown in FIG. The feedback controller C (s) is designed for _{the rigidified} PL _{, ARC} (s). The feedback controller C (s) is designed so that the sensitivity function S _ARC (s) = (1 + PL _{, ARC} (s) C (s)) ⁻¹ is stable.

The feedback controller C (s) is, for example, a PID controller and is designed with pole arrangement. In this design, the poles of the sensitivity function S _ARC (s) are arranged in quadruples at 20 Hz. Since the FS-ARC according to the third embodiment only needs to design the controller for the rigid body mode, the parameters of the controller can be easily determined.

Next, the stability of the FS-ARC control system according to the third embodiment is analyzed. The stability of the FS-ARC control system when the variable control parameter α _V and the cut-off frequency f _LPF , which are parameters of the controller, are changed is analyzed.

9, when changing to fix the cut-off frequency f _LPF to 0 (Hz), the variable control parameter alpha _V from 0 to 1, a Nyquist diagram. 10 fixes the cut-off frequency f _LPF to 0, the variable control parameter alpha _V 0 at the time of changing the ∞, a Nyquist diagram. 9 and 10, the solid line shows the case where the variable control parameter α _V = 0 and the cutoff frequency f _LPF = 0 (Hz). In this case, the resonance mode of P _FS-ARC (s) matches the resonance mode of P _LL (s). In FIG. 9, the broken line shows the case where the variable control parameter α _V = 1 and the cutoff frequency f _LPF = 0 (Hz). In this case, the resonance mode of P _FS-ARC (s) matches the resonance mode of P _LM (s). In FIG. 10, the alternate long and short dash line shows the case where the variable control parameter α _V = 1 and the cutoff frequency f _LPF = ∞. In this case, the resonance mode of P _FS-ARC (s) matches the resonance mode of P _LM (s).

From the above, the size and direction of the circle drawn by the resonance mode of P _FS-ARC (s) on the Nyquist diagrams shown in FIGS. 9 and 10 are the resonance modes of P _LL (s) and P _LM (s). It can be seen that it can be set freely between. That is, the resonance suppression effect and the phase stabilization of the resonance mode can be easily adjusted by using two parameters of the variable control parameter α _V and the cutoff frequency f _LPF .

For example, the first vibration suppressing effect, adjusted by the variable control parameter alpha _V. Next, by selecting the cutoff frequency f _LPF so as to maximize the phase margin of the closed loop system, the phase of the resonance mode can be stabilized. More specifically, the cutoff frequency f _LPF is selected so that the resonance mode is as far as possible from the point −1 + j0.

Next, the frequency characteristics of the ARC and FS-ARC described above are evaluated by simulation. In this simulation, the following three conditions (Case1-3) are set.
・ Case1:
ARC is applied, the variable control parameter α _V = α and the cut-off frequency f _LPF = 0 (Hz), and the pole of the sensitivity function S _ARC (s) is set to 30 (Hz).
This Case 1 is designed with emphasis on vibration suppression in response to a torque command value.
・ Case2:
FS-ARC is applied, the variable control parameter α _V = α, the cutoff frequency f _LPF = 1.0 (Hz), and the pole of the sensitivity function S _ARC (s) is set to 25 (Hz).
This Case 2 is designed with an emphasis on miniaturization of the load side motor 4.
・ Case3:
Apply FS-ARC, set variable control parameter α _V = 0.1 and cut-off frequency f _LPF = 5.0 (Hz), and set the pole of sensitivity function S _ARC (s) to 32 (Hz) To do.
Case 3 is designed with an emphasis on disturbance suppression performance.
In case 1 above, the phase margin is 43.5 °, so the poles of the sensitivity function S _ARC (s) and the cutoff frequency f _LPF are designed so that the phase margins of Case 2 and Case 3 are equal. Yes.

  FIG. 11 is a Nyquist diagram when the above three conditions are set. As shown in FIG. 11, it can be seen that Cases 1 and 2 cancel out the resonance. On the other hand, in the case of Case 3, it is understood that the resonance mode is arranged so as to be away from −1 + j0, and the phase is stabilized.

  Next, the frequency characteristics and time response of ARC and FS-ARC are evaluated using an experimental apparatus. FIG. 12 is a diagram illustrating an example of an experimental apparatus. On the load side, a motor 106 for reproducing a load (joint portion) such as disturbance torque is provided.

FIG. 13 shows a measurement result of frequency characteristics from the drive side torque T _M of the knee joint part to the angular velocity θ _M dot of the drive side motor, and a two-inertia model in which model fitting is performed based on the measurement result (FIG. 5). ) Is a Bode diagram of FIG. FIG. 14 is a diagram illustrating an example of the nominal value of the plant determined based on the frequency characteristics of FIG. Here, an error due to nonlinear friction occurs in a low-frequency gain.

In the experiment using the experimental apparatus, the same three conditions (Case1-3) as in the simulation are set.
(1) Frequency response FIG. 15 is a Nyquist diagram measured using an experimental apparatus. As shown in FIG. 15, in the case of Case 1 and 2, it can be seen that the resonance is substantially cancelled. On the other hand, in the case of Case 3, the resonance mode is arranged away from the point -1 + j0, and it can be seen that phase stabilization is achieved. Cases 1 to 3 show the same tendency as the simulation results.

(2) Time response ARC and FS-ARC are evaluated according to the step response and the time response of the step disturbance.

(2-1) Step response The step response of ARC and FS-ARC is evaluated. FIG. 16 is a diagram illustrating a step response waveform of the rotation angle of the load side motor. As shown in FIG. 16, in the case of Case 1, it can be seen that vibration is suppressed. It can be seen that vibration is suppressed also in Case2. However, since the pole of the sensitivity function S _ARC (s) is delayed, the overshoot is larger than Case1.

It can be seen that the vibration is suppressed also in Case 3. Although the overshoot is smaller than Csae2, since the variable control parameter α _V ≠ α, the vibration remains slightly.

  FIG. 17 is a diagram showing torque waveforms of the load side motors of Cases 1 and 2 in the step response. As shown in FIG. 17, it can be seen that the torque peak of Case2 is 25% lower than the torque peak of Case1. Therefore, it can be seen that the load-side motor 4 can be downsized by applying FS-ARC as in Case 2.

(2-2) Disturbance response The disturbance response of ARC and FS-ARC is evaluated. A stepwise disturbance of 0.02 (N · m) is applied to the load side for 0.1 (s). At this time, the rotation angle theta _L of the load motor 4 has a 0 (rad). FIG. 18 is a diagram illustrating a disturbance response waveform of the rotation angle of the load-side motor. As shown in FIG. 18, it can be seen that the responses of Cases 1 and 2 vibrate due to disturbance. On the other hand, in Case 3 where the variable control parameter α _V ≠ α, it can be seen that the disturbance suppressing effect is high and vibration can be suppressed.

Based on the above experimental results, Case 1 is designed with emphasis on vibration suppression of response to torque command value, Case 2 is designed with emphasis on miniaturization of load side motor 4, and Case 3 focuses on disturbance suppression performance. It can be confirmed that it has been designed.
Here, α _V used in the above equation (17) can be determined by balancing the trade-offs of the following (a) and (b).
(A) Emphasize followability and vibration characteristics with respect to the torque command value.
(B) Emphasis is placed on disturbance suppression characteristics (disturbance suppression characteristics) and vibration characteristics.

“Disturbance” is, for example, a force that interferes with the robot's movement from the outside, and “disturbance suppression characteristics” is a torque command value that is not affected by such a force. It refers to the performance that continues to follow.
In the case where the above (a) is most important, the variable control parameter α _V = α is set as described above. On the other hand, when (b) is emphasized a little, adjustment (0 ≦ α _V ≦ 1) is performed using the variable control parameter α _V.

For example, α _V that optimizes the trade-off balance is set while considering the followability and vibration characteristics with respect to the torque command value from the step response waveform of FIG. 16 and the disturbance suppression characteristics and vibration characteristics from the disturbance response waveform of FIG. To do. Note that although α _V is set using FIG. 16 and FIG. 18 in which the horizontal axis represents a response waveform representing time, the present invention is not limited to this. For example, α _V may be set using a response waveform plotting a transfer function from a load-side disturbance to an output (rotation angle) or a response waveform plotting a sensitivity function.
In the third embodiment, the same parts as those in the first and second embodiments are denoted by the same reference numerals, and detailed description thereof is omitted.

  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.

  In the above embodiment, the control device 1 performs the resonance cancellation control (ARC and FS-ARC) of the humanoid robot, but is not limited thereto. The control device 1 according to the present embodiment may perform, for example, resonance cancellation control of a machine tool. Thereby, large torque, high speed, and highly accurate control can be realized using a plurality of motors. Further, the control device 1 may perform resonance cancellation control of the semiconductor manufacturing apparatus. Thereby, the influence of the mechanical elasticity which becomes a problem at the time of ultra-precision positioning can be reduced. Furthermore, the control device 1 may perform resonance cancellation control for hybrid control of the vehicle engine and the electric motor. As a result, shaft torsional resonance that occurs between the engine and the electric motor is suppressed, and high-speed and high-precision control can be realized.

The present invention can also be realized, for example, by causing the CPU 7a to execute a computer program for the processing of the resonance canceling unit 72.
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 apparatus, 2 Rotation sensor, 3 Drive side motor, 4 Load side motor, 5 Spring element, 6 Reducer, 7 Control apparatus, 71 Command value generation part, 72 Resonance cancellation part

Claims (15)

A first drive means provided on the drive side for driving the controlled object, and driving the controlled object via a torque transmitting means;
A second drive means provided on the control target side for driving the control target;
Command generating means for generating torque command values for driving the first and second driving means;
The torque command value generated by the command generation means is multiplied by a first gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, and the torque command value Is multiplied by a second gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, and the torque command value for the first and second drive means is Resonance canceling means for canceling resonance by adjusting the ratio;
A control device comprising: The control device according to claim 1,
The resonance cancellation unit multiplies the torque command value generated by the command generation unit by a first gain (α + β / s) and a second gain (γ + δ / s), respectively, 2. A control device that cancels resonance by adjusting a ratio of the torque command value to the driving means.
However,
J _M is the inertia coefficient on the drive side, B _M is the viscosity coefficient on the drive side, J _L is the inertia coefficient on the control target side, and B _L is the viscosity coefficient on the control target side. Yes, r is the reduction ratio of the torque transmission means. The control device according to claim 1 or 2,
The resonance canceling means includes
A first filter that extracts a high-frequency component from the torque command value generated by the torque command generation means;
A second filter for extracting a low frequency component from the torque command value generated by the torque command generation means;
Have
The high frequency torque command value extracted by the first filter is multiplied by the first and second gains respectively, and the torque command value is adjusted to cancel the resonance, thereby the first and second driving. While controlling the means,
The control device characterized in that the first driving means is controlled also using a low-frequency torque command value extracted by the second filter. The control device according to claim 1,
The resonance canceling means includes
A first filter that extracts a high-frequency component from the torque command value generated by the torque command generation means;
A second filter for extracting a low frequency component from the torque command value generated by the torque command generation means;
Have
The high-frequency torque command value extracted by the first filter is multiplied by a first gain (α _V + β _V / s) and a second gain (γ _V + δ _V / s), respectively, and the torque command Canceling the resonance by adjusting the value to control the first and second driving means,
Based on the low frequency torque command value extracted by the second filter, the first drive means is controlled,
A parameter α _V (0 ≦ α _V ≦ 1) is adjusted, and parameters β _V , γ _V , and δ _V are set using the following formula based on the adjusted parameter α _V. .
The control device according to claim 3 or 4,
The first filter is a high-pass filter (1-F (s)),
The control device, wherein the second filter is a low-pass filter (F (s)). The control device according to claim 4,
The resonance canceling means, the parameter after setting the alpha _V, based on the set parameter alpha _V adjusts the cutoff frequency of the first and second filter sets, the control device, characterized in that. The control device according to any one of claims 1 to 6,
The control apparatus is characterized in that the control target is a joint part of a robot. A first drive means provided on the drive side for driving the controlled object, and driving the controlled object via a torque transmitting means;
A control method of a control device, comprising: a second drive unit that is provided on the control target side and drives the control target;
Generating a torque command value for driving the first and second driving means;
Multiplying the generated torque command value by a first gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, The ratio of the torque command value to the first and second driving means is adjusted by multiplying the second gain including the inertia coefficient, viscosity coefficient, and reduction ratio of the torque transmitting means on the drive side and the control target side. Canceling the resonance by
A control method for a control device, comprising: A control method for a control device according to claim 8,
A ratio of the torque command value to the first and second driving means is obtained by multiplying the generated torque command value by a first gain (α + β / s) and a second gain (γ + δ / s), respectively. The control method of the control device, wherein the resonance is canceled by adjusting
However,
J _M is the inertia coefficient on the drive side, B _M is the viscosity coefficient on the drive side, J _L is the inertia coefficient on the control target side, and B _L is the viscosity coefficient on the control target side. Yes, r is the reduction ratio of the torque transmission means. A control method for a control device according to claim 8 or 9, wherein
Extracting a high frequency component from the generated torque command value by a first filter;
Extracting a low frequency component from the generated torque command value by a second filter;
Further including
The extracted high-frequency torque command value is multiplied by the first and second gains respectively, and the torque command value is adjusted to cancel resonance and control the first and second drive means. With
The control method of the control device, wherein the first driving means is controlled also using the extracted low-frequency torque command value. A control method for a control device according to claim 8,
Extracting a high frequency component from the generated torque command value by a first filter;
Extracting a low frequency component from the generated torque command value by a second filter;
Further including
The extracted high frequency torque command value is multiplied by a first gain (α _V + β _V / s) and a second gain (γ _V + δ _V / s), respectively, and the torque command value is adjusted. Thereby canceling the resonance and controlling the first and second driving means,
Based on the extracted low frequency torque command value, the first drive means is controlled,
A parameter α _V (0 ≦ α _V ≦ 1) is adjusted, and parameters β _V , γ _V , and δ _V are set using the following formula based on the adjusted parameter α _V. Control method.
A control method for a control device according to claim 10 or 11,
The first filter is a high-pass filter (1-F (s)),
The control method of the control device, wherein the second filter is a low-pass filter (F (s)). A control method for a control device according to claim 11, comprising:
Control method of the control device, wherein said rear set parameters alpha _V, adjusted to set the cut-off frequency of said first and second filter based on the set parameters alpha _V, it. A control method for a control device according to any one of claims 8 to 13,
A control method of a control device, wherein the control object is a joint part of a robot. A first drive means provided on the drive side for driving the controlled object, and driving the controlled object via a torque transmitting means;
A control program for a control device, comprising: a second drive unit that is provided on the control target side and drives the control target;
Processing for generating torque command values for driving the first and second driving means;
Multiplying the generated torque command value by a first gain including an inertia coefficient, a viscosity coefficient, and a reduction ratio of the torque transmission means on the drive side and the control target side, The ratio of the torque command value to the first and second driving means is adjusted by multiplying the second gain including the inertia coefficient, viscosity coefficient, and reduction ratio of the torque transmitting means on the drive side and the control target side. Processing to cancel resonance by
A control program for a control apparatus, characterized by causing a computer to execute.