JP2011136395A - Vibration control method of robot and control device of robot - Google Patents

Abstract

The vibration of the tip of a robot is suppressed.
A vibration damping method for a robot that suppresses vibration of a robot tip that occurs when operation of a robot having a plurality of links and a plurality of motors driving a corresponding link is stopped. Then, the acceleration of the robot tip is detected by the acceleration sensor 52, and the speed of the robot tip is calculated based on the acceleration detected by the acceleration sensor. Based on the speed of the robot tip, the rotation angles of the plurality of motors at the acceleration detection timing of the acceleration sensor, and the inverse matrix J ⁻¹ of the Jacobian matrix J, the torsional angular velocity between each link and the corresponding motor is calculated. . Furthermore, the torsion angle is calculated based on the torsional angular velocity, and the amount of compensation added to the control input of each motor that eliminates the torsion generated between each link and the corresponding motor based on the torsional angular velocity and torsional angle. And a control input with the compensation amount added is output to each motor.
[Selection] Figure 3

Description

  The present invention relates to a vibration suppression method and a robot control apparatus that suppress vibrations of a robot.

  Conventionally, an articulated robot that operates by attaching a tool such as a welding gun or a spray gun to the tip has a problem that the tip vibrates immediately after the operation stops. This occurs when the robot link vibrates like a cantilever due to the inertia of the tool or the tip of the robot.

  As a method for dealing with the vibration of the link (vibration at the tip), for example, there is a method described in Patent Document 1. The method described in Patent Document 1 attaches an acceleration sensor to the tip of a robot, controls the motor that drives the link based on the acceleration at the tip detected by the acceleration sensor, and controls the vibration of the robot. Suppresses tip vibration.

JP-A-4-193490

  However, in the case of the above-mentioned Patent Document 1, there is a large time delay from when the acceleration sensor detects the acceleration at the tip of the robot until the link is controlled based on the detection result. More specifically, the acceleration at the tip of the robot detected by the acceleration sensor is calculated as a position change amount at the tip of the robot through speed calculation and coordinate calculation in this order. A position change command value capable of damping the link is calculated based on the position change amount, and the position change command value is converted into a command value for the motor that drives the link. In order to overlap a plurality of calculations in this way, the time delay from when the acceleration sensor detects the acceleration at the tip of the robot until it is reflected in the motor command increases. Depending on the vibration period of the link (especially when the period is short), the link cannot be sufficiently controlled by this time delay, and in the worst case, the vibration at the tip of the robot may be prolonged.

  Therefore, the present invention controls the link sufficiently by controlling the motor that drives the link based on the acceleration immediately after detecting the acceleration of the tip of the acceleration sensor robot immediately after the robot operation stops. The object is to suppress vibration and vibration at the tip.

In order to solve the above-described problem, the invention according to claim 1 of the present invention provides:
A vibration damping method for a robot that suppresses vibration of a robot tip that occurs when operation of a robot having a plurality of links and a plurality of motors that drive the corresponding links stops,
The acceleration of the robot tip is detected by an acceleration sensor,
Calculate the speed of the robot tip based on the acceleration detected by the acceleration sensor,
Based on the speed of the robot tip, the rotation angle of the plurality of motors at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix, the torsional angular velocity between each link and the corresponding motor is calculated,
Calculate the twist angle based on the twist angular velocity,
Based on the torsional angular velocity and torsional angle, calculate the compensation amount to be added to the control input of each motor, which eliminates the torsion that occurred between each link and the corresponding motor,
A control input to which the compensation amount is added is output to each motor.

The invention according to claim 2
A robot control device capable of suppressing vibration of a robot tip generated when operation of a robot having a plurality of links and a plurality of motors driving corresponding links is stopped,
An XYZ Cartesian coordinate system predefined outside the robot;
An acceleration sensor that detects acceleration in the X, Y, and Z axis directions of the robot tip and acceleration around the X, Y, and Z axes;
A plurality of encoders for detecting the rotation angle of the corresponding motor;
Speed calculating means for calculating the speed of the robot tip based on the acceleration detected by the acceleration sensor;
Calculate the torsional angular velocity between each link and the corresponding motor based on the velocity calculated by the velocity calculation means, the rotation angle detected by multiple encoders at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix Torsional angular velocity calculating means to
A twist angle calculating means for calculating a twist angle based on the twist angular speed calculated by the twist angular speed calculating means;
Based on the torsional angular velocity calculated by the torsional angular velocity calculating unit and the torsional angle calculated by the torsional angle calculating unit, the torsion generated between each link and the corresponding motor is eliminated and added to the control input of each motor. Compensation amount calculating means for calculating a compensation amount;
Motor control means for outputting a control input with the compensation amount added to each motor.

Furthermore, the invention according to claim 3
A robot control device capable of suppressing vibration of a robot tip generated when an operation of a robot having three links and three motors driving corresponding links stops.
An XYZ Cartesian coordinate system predefined outside the robot;
An acceleration sensor that detects acceleration in the X, Y, and Z axis directions of the robot tip;
A plurality of encoders for detecting the rotation angle of the corresponding motor;
Speed calculating means for calculating the speed of the robot tip based on the acceleration detected by the acceleration sensor;
Calculate the torsional angular velocity between each link and the corresponding motor based on the velocity calculated by the velocity calculation means, the rotation angle detected by multiple encoders at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix Torsional angular velocity calculating means to
A twist angle calculation means for calculating a twist angle based on the twist angular speed calculated by the twist angular speed calculation means;
Based on the torsional angular velocity calculated by the torsional angular velocity calculating unit and the torsional angle calculated by the torsional angle calculating unit, the torsion generated between each link and the corresponding motor is eliminated and added to the control input of each motor. Compensation amount calculating means for calculating a compensation amount;
Motor control means for outputting a control input with the compensation amount added to each motor.

Furthermore, the invention according to claim 4
The robot has a plurality of links and a plurality of motors that drive the corresponding links,
The three links are part of a plurality of links and are longer than other links.

  According to the present invention, immediately after the movement of the robot is stopped, the speed of the robot tip is calculated from the acceleration detected by the acceleration sensor, and the torsional angular speed between the link and the motor is calculated based on the speed and the inverse matrix of the Jacobian matrix. Calculated. Next, a torsion angle is calculated based on the torsional angular velocity, and a compensation amount for eliminating the torsion, which is added to the control inputs of the plurality of motors based on the torsional angular velocity and the torsion angle, is calculated. Each compensation amount is added to the control input of the corresponding motor.

  As a result, the compensation amount of the motor control input is calculated in a short time from the acceleration detected by the acceleration sensor, and the control input to which the compensation amount is added can be quickly used to eliminate the torsion between the motor and the corresponding link. Rotate. Therefore, the position change amount of the robot tip is obtained from the acceleration detected by the acceleration sensor, the position / orientation change command value capable of damping the link is calculated from the position change amount, and the position change command value is calculated as the motor command value. Compared to the case where the link is damped, the time from when the acceleration at the tip of the robot is detected until the link is damped is shortened. As a result, the link can be sufficiently damped, and vibration at the tip of the robot can be suppressed.

It is a figure which shows the link model of the robot which concerns on the 1st Embodiment of this invention. It is a figure for demonstrating the vibration of a link. It is a block diagram of a part of the control apparatus of the robot according to the first embodiment of the present invention. It is a figure for demonstrating the partial matrix of a Jacobian matrix. It is another figure for demonstrating the partial matrix of a Jacobian matrix. It is a block diagram of a part of the control device of the robot according to the second embodiment.

(First embodiment)
FIG. 1 shows a link model of the robot according to the first embodiment of the present invention. The robot 10 illustrated in FIG. 1 includes six joints 12 to 22 and includes a plurality of motors 36 to 46 (see FIG. 3) that drive the corresponding links 24 to 34. The motor 36 drives the link 24, the motor 38 drives the link 26, the motor 40 drives the link 28, the motor 42 drives the link 30, the motor 44 drives the link 32, and the motor 46 drives the link 34.

  In addition, the robot 10 includes a tool 50 such as a welding gun, a spray gun, or a hand tool at the tip 48.

Furthermore, the robot 10 includes a 6-axis sensor 52 that detects the acceleration of the tip 48 and a plurality of encoders 54 to 64 (see FIG. 3) that detect the rotation angle of the corresponding motor. The six-axis sensor 52 is used to detect the acceleration of the robot tip 48 with reference to a fixed reference coordinate system ΣB defined outside the robot. Specifically, for example, the output related to the sensor coordinate system of the six-axis sensor 52 is output by the known means in the X, Y, and Z axis directions of the reference coordinate system ΣB and the acceleration around the X, Y, and Z axes. Convert to The encoder 54 detects the rotation angle θ ₁ of the motor 36, the encoder 56 is the rotation angle θ ₂ of the motor 38, the encoder 58 is the rotation angle θ ₃ of the motor 40, the encoder 60 is the rotation angle θ ₄ of the motor 42, and the encoder 62 is The rotation angle θ ₅ of the motor 44 and the encoder 64 detect the rotation angle θ ₆ of the motor 46.

  Furthermore, the robot 10 (the control device for the robot 10) is configured to control the plurality of links 24 to 34 immediately after the operation of the robot 10 is stopped. That is, immediately after the operation of the robot 10 is stopped, the plurality of links 24 to 34 are not vibrated.

  First, before explaining the vibration control of the plurality of links 24 to 34, FIG. 2 shows the vibrations of the plurality of links 24 to 34 that occur immediately after the operation of the robot 10 stops (vibration that occurs when vibration is not controlled). The description will be given with reference.

  The occurrence of link vibration is mainly divided into three cases. The case where the link starts to vibrate when the operation of the other link stops (FIG. 2A), the case where the link starts to vibrate when the driving by the motor stops (FIG. 2B), and these 2 There are cases where two are combined.

For example, as shown in FIG. 2A, the link L starts to move from the state where the tool side end is located at the position P ₀ by the operation of another link, and the tool side end reaches the position P ₁ . When the other link stops, the link L bends due to the inertia of other links, motors, tools, etc. that are located on the tool side of the link L. The deflection of the link L, positioned at the position P ₂ the tool side end of the link L passes through the position P _1, torsion is generated between the motor M is connected to the link L and the link L (twist An angle α is generated). The link L continues to vibrate until the tool side end stops at the position P ₁ due to this twist. In the present specification, as shown in the figure, the “twist angle α” is defined as a straight line connecting a position P ₁ where the tool side end should be originally and the center of the motor M and a current position P ₂ of the tool side end. And the angle between the straight line connecting the center of the motor M.

Further, for example, as shown in FIG. 2 (B), the link L starts to rotate from the state where the tool side end is located at the position P ₀ by being driven by the motor M, and after rotating by the angle β (tool side end) There when) motor M when it reaches the position P ₁ stops, other links located from the link L to the tool side, the motor, the inertia of such tools, the link bends. Due to the deflection of the link, the tool side end of the link passes through the position P ₁ and is positioned at the position P ₂ , and a twist occurs between the link L and the motor M connected to the link L (twist angle α). Occurs). The link L continues to vibrate until the tool side end stops at the position P ₁ due to this twist.

  In order to suppress vibration generated due to the twist generated between the link and the motor, the control device of the robot 10 executes vibration suppression control on the plurality of motors 36 to 46.

  A configuration for executing vibration suppression control on the motors 36 to 46 is shown in FIG.

  FIG. 3 is a block diagram of components related to vibration suppression control of a part of the control device of the robot 10 and the plurality of motors 36 to 46.

  As shown in FIG. 3, the control device of the robot 10 includes a first filter 70 that calculates the speed of the robot tip 48, an inverse matrix calculation unit 72 that calculates an inverse matrix of the Jacobian matrix, links 24 to 34, and a motor 36. To the torsional angular velocity calculation unit 74 for calculating the torsional angular velocity between the motors 36 to 46, the integrator 76 for calculating the torsional angle between the links 24 to 34 and the motors 36 to 46, and the control inputs of the motors 36 to 46. A compensation amount calculation unit 78 that calculates a compensation amount to be performed, and a motor control unit 80 that outputs a control input to the motors 36 to 46.

、Ｙ軸方向の加速度

、Ｚ軸方向の加速度

、Ｘ軸まわりの加速度

、Ｙ軸まわりの加速度

、およびＺ軸まわりの加速度

に基づいて、ロボット先端４８のＸ軸方向の速度

、Ｙ軸方向の速度

、Ｚ軸方向の速度

、Ｘ軸まわりの速度

、Ｙ軸まわりの速度

、Ｚ軸まわりの速度

を算出する。具体的には、第１フィルタ７０は、６軸センサ５２から出力された信号からＨＰＦ（Ｈｉｇｈ Ｐａｓｓ Ｆｉｌｔｅｒ）によってＤＣ成分をカットし、その後に積分演算することにより、加速度

から速度

を算出する。 The first filter 70 is an acceleration in the X-axis direction of the robot tip 48 detected by the 6-axis sensor 52 immediately after the robot 10 is stopped.

, Acceleration in the Y-axis direction

, Acceleration in the Z-axis direction

, Acceleration around X axis

, Acceleration around Y axis

, And acceleration around the Z axis

Based on the speed of the robot tip 48 in the X-axis direction

, Y-axis speed

, Speed in the Z-axis direction

, Speed around X axis

, Speed around Y axis

, Speed around Z axis

Is calculated. Specifically, the first filter 70 cuts the DC component from the signal output from the 6-axis sensor 52 by a high pass filter (HPF), and then performs an integration operation to thereby accelerate the acceleration.

From speed

Is calculated.

The inverse matrix calculation unit 72 calculates J ^{−1 as} an inverse matrix of the Jacobian matrix J necessary for the twist angular velocity calculation unit 74 described later to calculate the twist angular velocity.

を検出するタイミングにおける複数のモータ３６〜４６の回転角度θ_１〜θ_６に基づいて、ヤコビ行列Ｊの逆行列Ｊ^−１を算出する。なお、複数のモータ３６〜４６の回転角度θ_１〜θ_６は、対応するモータの回転角度を検出する複数のエンコーダ５４〜６４から得られる。

Specifically, a Jacobian matrix J (Formula 1) of a function having the rotation angles θ _{1 to} θ ₆ of the plurality of motors 36 to 46 as variables is defined in advance (for example, stored in the storage unit of the control device). The inverse matrix calculation unit 72 substitutes the rotation angles θ _{1 to} θ ₆ for the Jacobian matrix J, and then calculates the inverse matrix J ⁻¹ of the Jacobian matrix J. More specifically, the inverse matrix calculation unit 72 is configured such that the 6-axis sensor 52 detects the acceleration of the robot tip 48.

The inverse matrix J ⁻¹ of the Jacobian matrix J is calculated based on the rotation angles θ _{1 to} θ ₆ of the plurality of motors 36 to 46 at the timing of detecting the. Note that the rotation angles θ _{1 to} θ ₆ of the plurality of motors 36 to 46 are obtained from the plurality of encoders 54 to 64 that detect the rotation angles of the corresponding motors.

を算出する。なお、

はリンク２４とモータ３６との間のねじれ角速度であって、

はリンク２６とモータ３８、

はリンク２８とモータ４０、

はリンク３０とモータ４２、

はリンク３２とモータ４４、

はリンク３４とモータ４６との間のねじれ角速度である。 The torsional angular velocity calculation unit 74 is a torsional angular velocity between the links 24-34 and the motors 36-46.

Is calculated. In addition,

Is the torsional angular velocity between the link 24 and the motor 36,

Is the link 26 and motor 38,

Is link 28 and motor 40,

Is link 30 and motor 42,

Is the link 32 and the motor 44,

Is the torsional angular velocity between the link 34 and the motor 46.

と、逆行列演算部７２が算出したヤコビ行列Ｊの逆行列Ｊ^−１とに基づいて、数式２を用いて、ねじれ角速度

を算出する。

Specifically, the speed of the robot tip 48 calculated by the first filter 70.

And the torsional angular velocity using Equation 2 based on the inverse matrix J ⁻¹ of the Jacobian matrix J calculated by the inverse matrix calculator 72.

Is calculated.

に基づいて、リンク２４〜３４とモータ３６〜４６との間のねじれ角度α_１〜α_６を算出する。 The second filter 76 has a twist angular velocity.

Based on the above, the twist angles α _{1 to} α ₆ between the links 24 to 34 and the motors 36 to 46 are calculated.

を積分することによってねじれ角度α_１〜α_６を算出する。 Specifically, the second filter 76 has a torsional angular velocity.

To calculate the twist angles α _{1 to} α ₆ .

  The compensation amount calculation unit 78 calculates a compensation amount to be added to the control input (current command) of the motor control unit 80 for each of the plurality of motors 36 to 46.

The compensation amounts I _{1 to} I ₆ of the plurality of motors 36 to 46 are calculated based on the inertia, the spring constant, the damper, and the torque constant of the link to be driven.

More specifically, assuming that the link inertia is H, the spring constant is K, the damper is D, the torsion angle between the link and the motor is α, and the torsional angular velocity is Δα, the link equation of motion is Can be expressed in

  In Equation 3, a is the link acceleration.

The motor current I _M necessary for realizing the link acceleration a can be expressed as Equation 4. Note that Kt in Equation 4 is a torque constant of the motor.

Since the motor current I _M expressed by Equation 4 is also the compensation amount I to be obtained, when Equation 4 is substituted into Equation 3, the equation of compensation amount I shown in Equation 5 is obtained.

Each of the compensation amounts I _{1 to} I ₆ calculated by the compensation amount calculation unit 78 is added to the control input of the corresponding motor so that the twist between the plurality of motors 36 to 46 and the corresponding link is eliminated. Rotate to. Thereby, immediately after the operation of the robot 10 is stopped, the occurrence of vibrations of the plurality of links 24 to 34 is suppressed. At this time, the operation of the robot 10 is stopped, but a control input necessary to maintain the stopped posture is output from the motor control unit 80 to each of the plurality of motors 36 to 46.

からロボット先端４８の速度

が算出され、その速度

とヤコビ行列Ｊの逆行列Ｊ^−１とに基づいてリンク２４〜３４とモータ３６〜４６との間のねじれ角速度

が算出される。次にねじれ角速度

に基づいてねじれ角度α_１〜α_６が算出され、ねじれ角速度

とねじれ角度α_１〜α_６に基づいて複数のモータ３６〜４６それぞれの制御入力に加算される、ねじれを解消する補償量Ｉ_１〜Ｉ_６が算出される。そして、補償量Ｉ_１〜Ｉ_６はそれぞれ、対応するモータの制御入力に加算される。 According to the present embodiment, the acceleration detected by the 6-axis sensor 52 immediately after the operation of the robot 10 is stopped.

To robot tip 48 speed

Is calculated and its speed

And the torsional angular velocity between the links 24-34 and the motors 36-46 based on the inverse matrix J- ¹ of the Jacobian matrix J

Is calculated. Next, the torsional angular velocity

Torsional angles α _{1 to} α ₆ are calculated based on

Based on the torsion angles α _{1 to} α ₆ , compensation amounts I _{1 to} I _{6 that} are added to the control inputs of the plurality of motors 36 to 46 to eliminate the torsion are calculated. The compensation amounts I _{1 to} I ₆ are respectively added to the control inputs of the corresponding motors.

からモータ３６〜４６の制御入力の補償量Ｉ_１〜Ｉ_６が短時間で算出され、その補償量Ｉ_１〜Ｉ_６が加算された制御入力によってモータ３６〜４６が対応するリンク２４〜３４との間のねじれを解消するためにすばやく回転する。したがって、加速度センサが検出した加速度からロボット先端の位置変化量を求め、位置変化量からリンクを制振できる位置姿勢変更指令値を算出し、その位置変更指令値をモータの指令値に演算してリンクを制振する場合に比べて、ロボット先端の加速度を検出してからリンクが制振されるまでの時間が短くなる。その結果、リンクを十分に制振でき、ロボット先端の振動を抑制することができる。 Thereby, the acceleration detected by the 6-axis sensor 52

The compensation amounts I _{1 to} I ₆ of the control inputs of the motors 36 to 46 are calculated in a short time, and the links 24 to 34 to which the motors 36 to 46 correspond by the control inputs to which the compensation amounts I _{1 to} I ₆ are added. Rotate quickly to eliminate the twist between. Therefore, the position change amount of the robot tip is obtained from the acceleration detected by the acceleration sensor, the position / orientation change command value capable of damping the link is calculated from the position change amount, and the position change command value is calculated as the motor command value. Compared to the case where the link is damped, the time from when the acceleration at the tip of the robot is detected until the link is damped is shortened. As a result, the link can be sufficiently damped, and vibration at the tip of the robot can be suppressed.

(Second Embodiment)
This embodiment is a more realistic and economical form compared to the first embodiment.

、Ｙ軸方向の加速度

、Ｚ軸方向の加速度

、Ｘ軸まわりの加速度

、Ｙ軸まわりの加速度

、およびＺ軸まわりの加速度

を検出している。しかしながら、実際に問題になるリンクやロボット先端の振動は、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向、またはこれらを組み合わせた方向の振動であるため、ロボット先端の加速度としてＲｘ，Ｒｙ，Ｒｚ成分を検出する必要は現実的にはあまりない。したがって、本実施形態は、第１の実施形態と異なり、ロボット先端のＸ軸方向の加速度

、Ｙ軸方向の加速度

、Ｚ軸方向の加速度

を検出する３軸センサを使用する。３軸センサは６軸センサに比べて安価であるため、３軸センサを使用することは第１の実施形態に比べて経済的に有利である。 In the case of the first embodiment, immediately after the robot 10 stops, the six-axis sensor 52 is used to control the plurality of links 24 to 34. That is, the acceleration in the X-axis direction of the robot tip

, Acceleration in the Y-axis direction

, Acceleration in the Z-axis direction

, Acceleration around X axis

, Acceleration around Y axis

, And acceleration around the Z axis

Is detected. However, since the vibration of the link or the robot tip that actually becomes a problem is the vibration in the X-axis direction, the Y-axis direction, the Z-axis direction, or a combination thereof, Rx, Ry, and Rz components are used as the robot tip acceleration In reality, there is not much need to detect. Therefore, this embodiment differs from the first embodiment in the acceleration in the X-axis direction of the robot tip.

, Acceleration in the Y-axis direction

, Acceleration in the Z-axis direction

A three-axis sensor for detecting Since the 3-axis sensor is less expensive than the 6-axis sensor, it is economically advantageous to use the 3-axis sensor as compared to the first embodiment.

  In the case of the first embodiment, vibration suppression is performed for all of the plurality of (six) links 24-34. However, in practice, it is not necessary to dampen all the links 24-34. Referring to FIG. 1, the robot 10 basically includes a wrist portion and an arm portion. The links 24 to 28 of the arm part have a longer link length than the links 30 to 34 of the wrist part in order to largely control the position and posture of the tool 50. On the other hand, the links 30 to 34 on the wrist part have a shorter link length than the links 24 to 28 on the arm part in order to finely control the position and posture of the tool 50. Therefore, the contribution to the vibration of the robot tip 48 is greater when the links 24 to 28 of the arm part having a longer link length are vibrated than when the links 30 to 34 of the wrist part having a shorter link length are vibrated. Therefore, in the present embodiment, vibration suppression control is performed on the motors 36 to 40 that perform vibration suppression on the links 24 to 28 having a long link length, that is, the links 24 to 28.

  Further, in the present embodiment, when using the three-axis sensor as described above, and for damping the links 24 to 28 of the arm portion having a long link length instead of all the links, the partial matrix of the Jacobian matrix J Is used.

Specifically, as shown in FIG. 4, the Jacobian matrix J is composed of three partial matrices J _a , J _b , and J _c . The partial matrix J _a is a matrix configured by extracting components from the first row to the third row and from the first column to the third column of the Jacobian matrix J. The partial matrix _Jb is a matrix configured by extracting components from the first row to the third row and the fourth column to the sixth column of the Jacobian matrix J. The partial matrix _Jc is a matrix configured by extracting components from the fourth row to the sixth row of the Jacobian matrix J.

からリンク２４〜２８とモータ３６〜４０との間のねじれ角速度

を算出するために絶対に必要な行列は、部分行列Ｊ_ａである。そこで、本実施形態は、第１の実施形態がヤコビ行列Ｊを使用しているのに対し、ヤコビ行列Ｊの部分行列Ｊ_ａを使用する（以下、「部分ヤコビ行列Ｊ_ａ」と称する）。６行６列のヤコビ行列Ｊに代って３行３列の部分ヤコビ行列Ｊ_ａを使用するので、第１の実施形態に比べて、必要となる演算量を大幅に削減できる。 The acceleration at the tip of the robot detected by the three-axis sensor among the three sub-matrices J _{a to} J _c constituting the Jacobian matrix J

Torsional angular velocity between links 24-28 and motors 36-40

Absolutely necessary matrix to calculate is the partial matrix J _a. Therefore, the present embodiment, while the first embodiment uses the Jacobian matrix J, using partial matrix J _a of the Jacobian matrix J (hereinafter, referred to as "partial Jacobian matrix J _a"). Since the 3 × 3 partial Jacobian matrix _Ja is used in place of the 6 × 6 Jacobian matrix J, the required amount of computation can be greatly reduced compared to the first embodiment.

Incidentally, the inventors, when using partial Jacobian J _a, instead of the Jacobian matrix J, it was confirmed that there is no problem substantially.

Specifically, in the six-joint robot whose link model is shown in FIG. 5, the amount of change in the position of the tip (flange F) in the reference coordinate system ΣB when the same control input is given to the six motors is shown in FIG. It was calculated using the Jacobian matrix J and partial Jacobian matrix J _a showing. Rotation angle of the six motors before giving control input, the rotation angle theta ₁ is 20 ° of the joint JT ₁ motor, the rotation angle theta ₂ is 20 ° of the joint JT ₂ motor, the rotation angle of the motor of the joint JT ₃ theta ₃ is 35 °, the rotation angle theta ₄ is -40 ° in the joint JT ₄ motor, the rotation angle theta ₅ is 30 ° of the motor of the joint JT _5, the rotation angle theta ₆ of the motor of the joint JT ₆ in -20 ° is there. From this state, each motor was rotated by 1 °. In the figure, the distance d ₀ is 385 mm, d ₁ is 21 mm, d ₂ is 1100 mm, d ₃ is 1300 mm, d ₄ is 0 mm, and d ₅ is 228 mm. Also as shown, the link _{L 1} between the joint _JT 1, JT _2, joint _JT 2, the link between JT _{3 L 2,} and joint _JT 3, the link _{L 3} between the JT ₄ is joint _JT 4, links between JT _5, is long the link length as compared to the link between the joint _JT 5, JT ₆ between the links, and a joint JT ₆ and the flange F.

The positional change amount (Δx ₁ , Δy ₁ , Δz ₁ ) of the flange F calculated using the Jacobian matrix J was (−24.6, −10.9, −29.0). On the other hand, the position variation of the flange F which is calculated by using the partial Jacobian matrix _{J a (Δx 2, Δy 2} , Δz 2) are (- 26.6, -7.42, -27.1) was . Error between the position change amount and the partial change in position of the Jacobian matrix _{J a} of the Jacobian matrix J (xe, ye, ze) is (2.0, -3.5, -1.9). When this error is evaluated with respect to the position change amount of the Jacobian matrix J, it is 5.5% ((xe ² + ye ² + ze ² ) / (Δx ₁ ² + Δy ₁ ² + Δz ₁ ² ) × 100 = 5.5). Minor. Therefore, no problem substantially even with partial Jacobian matrix J _a in place of the Jacobian matrix J.

  FIG. 6 is a block diagram of components related to vibration suppression control of the three motors 36 to 40 in a part of the robot control apparatus according to the second embodiment. The link, motor, encoder, and motor control unit are the same as those in the first embodiment.

を検出する。 The triaxial sensor 152 is an acceleration in the X, Y, and Z axis directions of the reference coordinate system ΣB.

Is detected.

、Ｙ軸方向の加速度

、Ｚ軸方向の加速度

に基づいて、ロボット先端４８のＸ軸方向の速度

、Ｙ軸方向の速度

、Ｚ軸方向の速度

を算出する。また、第１フィルタ１７０は、第１の実施形態の第１フィルタ７０と同様に、３軸センサ１５２の出力信号からＤＣ成分をカットする。 The first filter 170 is an acceleration in the X-axis direction of the robot tip 48 detected by the three-axis sensor 152 immediately after the robot 10 is stopped.

, Acceleration in the Y-axis direction

, Acceleration in the Z-axis direction

Based on the speed of the robot tip 48 in the X-axis direction

, Y-axis speed

, Speed in the Z-axis direction

Is calculated. Further, the first filter 170 cuts the DC component from the output signal of the three-axis sensor 152, similarly to the first filter 70 of the first embodiment.

The inverse matrix calculation unit 172 calculates an inverse matrix J _a ⁻¹ of the partial Jacobian matrix J _a necessary for the twist angular velocity calculation unit 174 described later to calculate the twist angular velocity.

を検出するタイミングにおける３つのモータ３６〜４０の回転角度θ_１〜θ_３に基づいて、部分ヤコビ行列Ｊ_ａの逆行列Ｊ_ａ ^−１を算出する。 Specifically, three motors 36 to 40 of the rotation angle theta ₁ through? ₃ portions Jacobian of the function whose variable J _{a (see} FIG. 4) and are defined in advance, an inverse matrix calculation unit 172, the substituting the rotation angle theta ₁ through? ₃ in the portion Jacobian matrix _{J a,} then calculates the inverse matrix _J ^{a -1} portions Jacobian matrix _{J a} a. More specifically, the inverse matrix calculation unit 172 has an acceleration of the robot tip 48 by the triaxial sensor 152.

Based on the rotational angle theta ₁ through? ₃ of the three motors 36 to 40 at the timing of detecting the to calculate the inverse matrix _J ^{a -1} portions Jacobian matrix _{J a.}

を算出する。 The torsional angular velocity calculation unit 174 is a torsional angular velocity between the links 24-28 and the motors 36-40.

Is calculated.

と、逆行列演算部１７２が算出した部分ヤコビ行列Ｊ_ａの逆行列Ｊ_ａ ^−１とに基づいて、数式６を用いて、ねじれ角速度

を算出する。

Specifically, the speed of the robot tip 48 calculated by the first filter 170.

And an inverse matrix J _a ⁻¹ of the partial Jacobian matrix J _a calculated by the inverse matrix calculation unit 172, using Equation 6, the torsional angular velocity

Is calculated.

に基づいて、リンク２４〜２８とモータ３６〜４０との間のねじれ角度α_１〜α_３を算出する。 The second filter 176 has a torsional angular velocity

Based on the above, torsion angles α _{1 to} α ₃ between the links 24 to 28 and the motors 36 to 40 are calculated.

を積分することによってねじれ角度α_１〜α_３を算出する。 Specifically, the second filter 176 has a torsional angular velocity.

Are integrated to calculate torsional angles α _{1 to} α ₃ .

The compensation amount calculation unit 178 calculates the compensation amounts I _{1 to} I ₃ to be added to the control inputs of the motor control unit 80 for the three motors 36 to 40 in the same manner as the compensation amount calculation unit 78 of the first embodiment. .

Each of the compensation amounts I _{1 to} I ₃ calculated by the compensation amount calculation unit 178 is added to the control input of the corresponding motor so that the twist between the three motors 36 to 40 and the corresponding link is eliminated. Rotate to. Accordingly, the occurrence of vibrations of the three links 24 to 28 is suppressed immediately after the operation of the robot 10 is stopped.

According to the present embodiment, since the three-axis sensor is used to detect the acceleration of the robot tip 48, it is more economical than the first embodiment using the six-axis sensor. Further, since the use of partial matrix J _a Jacobian matrix J of 6 rows and 6 columns, as compared with the first embodiment, can greatly reduce the amount of calculation required.

  Although the present invention has been described with reference to two embodiments, the present invention is not limited to the above-described two embodiments.

  For example, in the above-described embodiment, the robot is a six-joint robot, but the present invention is not limited to this.

In the case of the above-described embodiment, as shown in FIG. 3 (FIG. 6), the rotation angles θ _{1 to} θ ₆ (θ _{1 to} θ ₃ ) of each motor are substituted into the Jacobian matrix J (partial Jacobian matrix J _a ). Then, the inverse matrix J ⁻¹ (J _a ⁻¹ ) is calculated. As an alternative, an inverse matrix J ⁻¹ of the Jacobian matrix J (an inverse matrix J _a ⁻¹ of the partial Jacobian matrix J _a ) is obtained in advance, and the rotation angles θ _{1 to} θ ₆ of each motor are added to the inverse matrix J ^−1. (θ ₁ ~θ ₃₎ may be substituted for.

24-34 link 36-46 motor 52 acceleration sensor (6-axis sensor, 3-axis sensor)

Claims (4)

A vibration damping method for a robot that suppresses vibration of a robot tip that occurs when operation of a robot having a plurality of links and a plurality of motors that drive the corresponding links stops,
The acceleration of the robot tip is detected by an acceleration sensor,
Calculate the speed of the robot tip based on the acceleration detected by the acceleration sensor,
Based on the speed of the robot tip, the rotation angle of the plurality of motors at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix, the torsional angular velocity between each link and the corresponding motor is calculated,
Calculate the twist angle based on the twist angular velocity,
Based on the torsional angular velocity and torsional angle, calculate the compensation amount to be added to the control input of each motor, which eliminates the torsion that occurred between each link and the corresponding motor,
A vibration control method for a robot, wherein a control input including a compensation amount is output to each motor. A robot control device capable of suppressing vibration of a robot tip generated when operation of a robot having a plurality of links and a plurality of motors driving corresponding links is stopped,
An XYZ Cartesian coordinate system predefined outside the robot;
An acceleration sensor that detects acceleration in the X, Y, and Z axis directions of the robot tip and acceleration around the X, Y, and Z axes;
A plurality of encoders for detecting the rotation angle of the corresponding motor;
Speed calculating means for calculating the speed of the robot tip based on the acceleration detected by the acceleration sensor;
Calculate the torsional angular velocity between each link and the corresponding motor based on the velocity calculated by the velocity calculation means, the rotation angle detected by multiple encoders at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix Torsional angular velocity calculating means to
A twist angle calculating means for calculating a twist angle based on the twist angular speed calculated by the twist angular speed calculating means;
Based on the torsional angular velocity calculated by the torsional angular velocity calculating unit and the torsional angle calculated by the torsional angle calculating unit, the torsion generated between each link and the corresponding motor is eliminated and added to the control input of each motor. Compensation amount calculating means for calculating a compensation amount;
A control apparatus for a robot, comprising: motor control means for outputting a control input to which the compensation amount is added to each motor. A robot control device capable of suppressing vibration of a robot tip generated when an operation of a robot having three links and three motors driving corresponding links stops.
An XYZ Cartesian coordinate system predefined outside the robot;
An acceleration sensor that detects acceleration in the X, Y, and Z axis directions of the robot tip;
A plurality of encoders for detecting the rotation angle of the corresponding motor;
Speed calculating means for calculating the speed of the robot tip based on the acceleration detected by the acceleration sensor;
Calculate the torsional angular velocity between each link and the corresponding motor based on the velocity calculated by the velocity calculation means, the rotation angle detected by multiple encoders at the acceleration detection timing of the acceleration sensor, and the inverse matrix of the Jacobian matrix Torsional angular velocity calculating means to
A twist angle calculation means for calculating a twist angle based on the twist angular speed calculated by the twist angular speed calculation means;
Based on the torsional angular velocity calculated by the torsional angular velocity calculating unit and the torsional angle calculated by the torsional angle calculating unit, the torsion generated between each link and the corresponding motor is eliminated and added to the control input of each motor. Compensation amount calculating means for calculating a compensation amount;
A control apparatus for a robot, comprising: motor control means for outputting a control input to which the compensation amount is added to each motor. The robot has a plurality of links and a plurality of motors that drive the corresponding links,
The robot control apparatus according to claim 3, wherein the three links are a part of a plurality of links and are longer than other links.

Images