JP2010284758A - Robot controller and robot control method - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To suppress the occurrence of vibration due to the delay of an inertial-sensor signal. <P>SOLUTION: A robot control method is used for a robot controller configured as follows. The robot controller includes: an arm 40 serving as a link rotatable with respect to a base body; a motor 20 serving as an actuator for driving the arm 40; a speed-reduction mechanism 60 that transmits torque of the motor 20 to the arm 40 at a reduction ratio N; an arm rotated by the link; an angle sensor that detects a rotation angle θm of the actuator; and an inertial sensor that detects a rotation angle θa of the arm. A delay amount of an inertial-sensor signal is added to the motion equation of a mechanism model system of a robot so as to set a feedback gain and to suppress vibration of the link due to the delay of the inertial-sensor signal. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a robot control device and a robot control method.

  A conventional robot having a multi-link structure includes a first link that can rotate with respect to a base, a first actuator that drives the first link, and a torque of the first actuator decelerated to the first link. A first torque transmission mechanism that transmits at a ratio N1, a first angle sensor that detects a rotation angle θM1 of the first actuator, and a first angular velocity sensor that detects an angular velocity ωL1 of the first link relative to the base body. The first link angle θM1 is calculated by using the high-frequency component of the integral value of ωL1 and the low-frequency component of θM1 * N1.

Further, the second link that can rotate with respect to the first link, the second actuator that drives the second link, and the torque of the second actuator are transmitted to the second link at a reduction ratio N2. A second torque transmission mechanism; a second angle sensor that detects a rotation angle θM2 of the second actuator; and a second angular velocity sensor that detects an angular velocity ωL2 of the second link with respect to the base body. The second link to the first link using a high frequency component equal to or higher than the first frequency in the integral value of ω _L2 −ω _L1 and a low frequency component equal to or lower than the first frequency of θM2 * N2. A robot control device that further calculates an angle has been proposed (see, for example, Patent Document 1).

Japanese Patent No. 3883544

  Such a two-link three-inertia robot according to Patent Document 1 is provided with an angle sensor, an acceleration sensor, and an angular velocity sensor, and attempts to accurately control the operation of the link using detection values of each sensor. Therefore, there is a case where vibration due to this signal delay occurs.

  In the case of an operation model that does not take into account signal delay, it is possible to control vibrations by arranging the poles of the controller, but in that case, the target value followability becomes worse, or the control result is sensitive to gain. Therefore, there is a problem that the robustness of control is lost.

  SUMMARY An advantage of some aspects of the invention is to solve at least a part of the problems described above, and the invention can be implemented as the following forms or application examples.

  Application Example 1 A robot control apparatus according to this application example includes a link that can rotate with respect to a base, an actuator that drives the link, and a speed reduction mechanism that transmits torque of the actuator to the link in a non-decelerating manner. And an angle sensor for detecting the rotation angle θm of the actuator and an inertial sensor for detecting the rotation angle θa of the link, and adding a delay amount β of the inertial sensor signal to the equation of motion of the mechanism model system to obtain a feedback gain. It is set to suppress vibration of the link due to delay of the inertial sensor signal.

  Application Example 2 In the control method of the robot control apparatus according to this application example, a link that is rotatable with respect to a base, an actuator that drives the link, and torque of the actuator are transmitted to the link in a non-decelerating manner. A control method of a robot control device comprising: a deceleration mechanism; an angle sensor that detects a rotation angle θm of the actuator; and an inertial sensor that detects a rotation angle θa of the link. A feedback gain is set in addition to the equation of motion of the mechanism model system of the robot, and the vibration of the link due to the delay of the inertial sensor signal is suppressed.

  According to such a configuration and a control method, if the feedback gain is designed by the state function of (Equation 11) that models the delay of the inertial sensor signal, the delay of the inertial sensor signal is taken into consideration in advance. Residual vibration can be suppressed without generating N vibration, and the establishment time of residual vibration can be shortened. As a result, loss of control robustness can be suppressed.

Explanatory drawing which represents typically the mechanism model of the robot which concerns on embodiment. The graph which shows the impulse response in the case of delay 0. Explanatory drawing which shows the basic drive of a robot. The graph showing an example of the residual vibration at the time of driving an arm. The graph showing the state which N vibration generate | occur | produced. The graph which shows an impulse response in case the delay amount (delay) (beta) of an inertial sensor signal is 0.05 second.

Hereinafter, embodiments of the present invention will be described with reference to the drawings.
(Embodiment 1)

  FIG. 1 is an explanatory diagram schematically showing a mechanism model of a robot according to the present embodiment. FIG. 1 shows a mechanism model of a robot 10 of a one-link two-inertia system, and includes a motor 20 and an arm 40 as a link that can rotate with respect to a base (not shown).

  Note that the robot 10 includes a calculation unit (not shown) that performs calculation processing related to the drive and control of the robot.

  A reduction mechanism 60 for transmitting the torque of the motor 20 to the arm 40 is provided between the motor 20 and the arm 40. The speed reduction mechanism 60 is considered to have a spring component 61 and a damper component 62.

  The motor 20 includes an encoder 50 as an angle sensor for measuring the rotation angle of the motor 20, and an acceleration sensor 70 as an inertial sensor is attached to the tip of the arm 40.

  The motor 20 rotates the arm 40 about the shaft 80 as a rotation axis via the speed reduction mechanism 60. At this time, when the motor 20 is stopped and the rotation of the arm 40 is stopped, the arm 40 does not stop immediately due to the influence of the spring component 61 and the damper component 62 of the speed reduction mechanism 60, and vibration (hereinafter referred to as residual vibration) is generated. May occur. When residual vibration occurs in the arm 40, it is difficult to accurately control the operation of the arm 40. The residual vibration is caused by the presence of the spring component 61 and the damper component 62 of the speed reduction mechanism 60.

  The residual vibration time can be shortened by attaching an acceleration sensor 70 to the arm 40, detecting the movement of the tip of the arm, and performing feedback control (Patent Document 1).

  However, the output signal of the acceleration sensor 70 has a delay such as a detection delay of the sensor element control circuit itself, a delay due to an external analog filter, and a delay due to a prefilter when converting the analog signal into a digital signal. To do. In the present embodiment, the detection delay of the sensor element control circuit itself and the delay due to the external analog filter are collectively referred to as the inertia sensor signal delay.

  The equation of motion of the mechanism model when there is no delay in the inertial sensor signal can be expressed as the following equation.

なお、θｍはエンコーダー５０で検出し、θａは慣性センサー（角速度センサー）７０で検出する。また、Ｄ_ｇは減速機構のダンパー成分の減衰係数、Ｋ_ｇはバネ成分の弾性常数である。

Θm is detected by the encoder 50, and θa is detected by the inertial sensor (angular velocity sensor) 70. Further, D _g is the damping coefficient of the damper components of the reduction mechanism, K _g is an elastic constant of the spring component.

  (Equation 2) represents the equation of motion on the motor 20 side, and (Equation 3) represents the equation of motion on the arm 40 side.

  By summing up (Equation 2) and (Equation 3), the following quadratic equation of motion is obtained.

（数４）は、さらに次式で表すことができる。

(Expression 4) can be further expressed by the following equation.

なお、（数５）におけるＪ_Ｌ、Ｄ_Ｌ、Ｋ_Ｌ、Ｂ_Ｌは次のように表すことができる。

Note that J _L , D _L , K _L , and B _L in (Equation 5) can be expressed as follows.

また、（数５）から、系の状態方程式を作成すると以下の式で表すことができる。

Further, when the state equation of the system is created from (Equation 5), it can be expressed by the following equation.

そして、（数６）におけるｘ、ｕ、ｙは（数８）、また、Ａ、Ｂ、Ｃ、Ｄは（数９）のように表される。

Then, x, u, and y in (Expression 6) are expressed as (Expression 8), and A, B, C, and D are expressed as (Expression 9).

  According to the control theory, the stability of the mechanism model of the present embodiment represented by the state equation (Equation 7) can be determined by the pole arrangement of the matrix A. That is, the pole arrangement when there is no delay of the inertial sensor signal (delay 0) is {−0.6770 + 24.8842i, −0.6770-24.8842i, 0, −0,2048}.

  FIG. 2 is a graph showing an impulse response in the case of delay 0 (no inertial sensor signal delay). As shown in FIG. 2, the impulse response in the case of delay 0 becomes stable for a while. Therefore, stable control can be performed by forming and controlling the servo enlargement system as it is.

Next, the driving state and residual vibration of the robot will be described.
FIG. 3 is an explanatory diagram showing basic driving of the robot according to the present embodiment. FIG. 3 shows one cycle in which the angular velocity of the arm 40 is controlled to rotate from an angle of 0 degrees to 180 degrees and stop. That is, the arm 40 is accelerated from time t1 to time t2, is driven at a constant speed from time t2 to time t3, is decelerated from time t3 to time t4, and is stopped at time t4.

In this way, when the arm 40 is accelerated, driven at a constant speed, driven at a reduced speed, and stopped, residual vibration is generated due to the spring component 61 and the damper component 62 of the speed reduction mechanism 60 (also sometimes referred to as an elastic component). To do.
FIG. 4 is a graph showing an example of residual vibration when the arm is driven as shown in FIG. FIG. 4 shows that residual vibration exists after the arm 40 is stopped.

  Although it is important for accurate control of the arm 40 to suppress the residual vibration caused by this elastic component and shorten the establishment time, when the control feedback gain is determined so as to shorten the establishment time, the inertial sensor signal A new vibration (hereinafter referred to as N vibration) is generated due to the delay of.

  FIG. 5 is a graph showing a state in which N vibration has occurred. As shown in FIG. 5, the characteristic of N vibration is that vibration is generated even when the arm 40 is stationary at an angle of 0 degrees and an angle of 180 degrees. If the control feedback gain is reduced so as not to generate the N vibration, the N vibration does not occur, but the establishment time for suppressing the vibration of the arm 40 becomes long.

  Consider the mechanism model system when the inertial sensor signal is delayed as a cause of N vibration. When the delay amount of the inertial sensor signal is β, it can be expressed by the following equation.

（数１０）のように近似すれば、（数４）に相当する系の運動方程式は、次式のように表すことができる。

If approximated as (Equation 10), the equation of motion of the system corresponding to (Equation 4) can be expressed as the following equation.

そして、このときの状態方程式は、次のように表すことができる。

The state equation at this time can be expressed as follows.

なお、（数１２）におけるＡ_Ｂ、Ｂ_Ｂ、Ｃ_Ｂ、Ｄ_Ｂは（数１３）のように表される。

In addition, A _B , B _B , C _B , and D _B in (Equation 12) are expressed as (Equation 13).

また、（数１３）におけるＪ_ＬＢ、Ｄ_ＬＢ、Ｋ_ＬＢ、Ｂ_ＬＢは（数１４）のように表される。

Further, J _LB , D _LB , K _LB , and B _LB in ( _Equation 13) are expressed as (Equation 14).

  According to the control theory, the stability of the mechanism model of this embodiment represented by the state equation (Equation 12) can be determined by the pole arrangement of the matrices A and B. For example, when β is −0.05 (sec), the poles are {1.8396 + 24.7228i, 1.8396-24.7228i, 0, −0.207}, and the impulse response in that case is shown in FIG. To express.

  FIG. 6 is a graph showing an impulse response when the delay amount β of the inertial sensor signal is 0.05 sec. That is, when the control of the system is designed with the state equation represented by (Equation 7), the actual system becomes the state equation represented by (Equation 12) and becomes unstable. As a result, when the feedback gain is determined so as to shorten the establishment time of the residual vibration, N vibration occurs (oscillation phenomenon).

  Therefore, if the feedback gain is designed by the state function of (Equation 12) that models the delay of the inertial sensor signal, the residual vibration is generated without generating N vibration because the delay β of the inertial sensor signal is taken into consideration in advance. And the establishment time of the residual vibration can be shortened. As a result, loss of control robustness can be suppressed.

  In this embodiment, the one-link two-inertia system robot control device and the control method have been described as examples. However, this technical idea can also be applied to a two-link three-inertia system robot control device.

  DESCRIPTION OF SYMBOLS 10 ... Robot, 20 ... Motor, 40 ... Arm as link, 50 ... Encoder as angle sensor, 60 ... Deceleration mechanism, 61 ... Spring component, 62 ... Damper component, 70 ... Angular velocity sensor as inertial sensor

Claims (2)

A link rotatable relative to the substrate;
An actuator for driving the link;
A speed reduction mechanism that transmits the torque of the actuator to the link in a non-speed reduction manner;
An angle sensor for detecting a rotation angle θm of the actuator;
An inertial sensor for detecting the rotation angle θa of the link,
A robot control apparatus characterized in that a feedback gain is set by adding a delay amount β of an inertial sensor signal to an equation of motion of a mechanism model system to suppress the vibration of the link caused by the delay of the inertial sensor signal. A link that is rotatable relative to a base, an actuator that drives the link, a speed reduction mechanism that transmits torque of the actuator to the link in a non-decelerating manner, an angle sensor that detects a rotation angle θm of the actuator, A control method of a robot control device comprising an inertial sensor for detecting a rotation angle θa of a link,
A control of the robot characterized in that a feedback gain is set by adding a delay amount β of the inertial sensor signal to the equation of motion of the mechanism model system of the robot to suppress vibration of the link due to the delay of the inertial sensor signal Method.

Images