JP6834124B2 - Robot control device - Google Patents

Robot control device Download PDF

Info

Publication number
JP6834124B2
JP6834124B2 JP2015228656A JP2015228656A JP6834124B2 JP 6834124 B2 JP6834124 B2 JP 6834124B2 JP 2015228656 A JP2015228656 A JP 2015228656A JP 2015228656 A JP2015228656 A JP 2015228656A JP 6834124 B2 JP6834124 B2 JP 6834124B2
Authority
JP
Japan
Prior art keywords
arm
shaft
vibration
sensor
support portion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2015228656A
Other languages
Japanese (ja)
Other versions
JP2017056543A (en
Inventor
大介 川瀬
大介 川瀬
寛章 白取
寛章 白取
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Denso Wave Inc
Original Assignee
Denso Wave Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Denso Wave Inc filed Critical Denso Wave Inc
Publication of JP2017056543A publication Critical patent/JP2017056543A/en
Application granted granted Critical
Publication of JP6834124B2 publication Critical patent/JP6834124B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Manipulator (AREA)

Description

本発明は、ロボットを制御するロボット制御装置に関する。 The present invention relates to a robot control device that controls a robot.

ロボット制御の分野では、モデル化したロボットの動作を予めシミュレーションし、所望のシミュレーション結果、例えば所望の振動抑制効果を得られた制御内容を実際のロボットに適用することが一般的に行われている。また、ロボットの振動を抑制する振動抑制制御についても様々な検討がなされており、例えば特許文献1、2では、振動を抑制しながらロボットを動作させることができる制御装置が提案されている。 In the field of robot control, it is common practice to simulate the motion of a modeled robot in advance and apply a desired simulation result, for example, a control content obtained with a desired vibration suppression effect to an actual robot. .. In addition, various studies have been made on vibration suppression control for suppressing vibration of a robot. For example, Patent Documents 1 and 2 propose a control device capable of operating a robot while suppressing vibration.

特許第4038659号公報Japanese Patent No. 4038659 特許第5411687号公報Japanese Patent No. 5411687

しかしながら、シミュレーション上では所望の振動抑制効果が得られた制御内容を実際のロボットに適用した場合、シミュレーション通りの性能を発揮できないことがある。具体的には、シミュレーション上では発生していなかった振動が、実際のロボットにおいて発生していた。
本発明は上記事情に鑑みてなされたものであり、その目的は、シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供することにある。
However, in the simulation, when the control content obtained with the desired vibration suppression effect is applied to an actual robot, the performance as in the simulation may not be exhibited. Specifically, vibrations that did not occur in the simulation were generated in the actual robot.
The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a robot control device capable of suppressing vibration that does not appear in a simulation.

ロボットの制御分野では、モータとアーム間の減速機に存在するバネ要素による振動モードを考慮したいわゆる2慣性系のシミュレーションが利用されている。この振動モードは、減速機等の動力が伝達していく動力伝達機構における動作方向の剛性に起因する振動である。また、2慣性系のシミュレーションは、広く利用されており、その有効性は認知されていると考えられる。 In the field of robot control, a so-called two-inertial frame simulation considering a vibration mode due to a spring element existing in a speed reducer between a motor and an arm is used. This vibration mode is vibration caused by the rigidity of the operating direction in the power transmission mechanism through which the power of the speed reducer or the like is transmitted. Moreover, the simulation of the bi-inertial system is widely used, and its effectiveness is considered to be recognized.

このとき、アームの剛性等の機械的数値はシミュレーション条件に当然盛り込まれており、実際のロボットは、そのシミュレーション条件を満たすような機械的数値の範囲で設計されている。それにも関わらずロボットに振動が生じるということは、従来の2慣性系で考慮されていた振動モードとは異なる振動モードが存在していると考えられる。 At this time, mechanical values such as arm rigidity are naturally included in the simulation conditions, and the actual robot is designed within the range of mechanical values that satisfy the simulation conditions. The fact that the robot vibrates in spite of this is considered to mean that there is a vibration mode different from the vibration mode considered in the conventional two inertial system.

そして、発明者らは、振動を生じさせる原因の調査を重ねた結果、実際のロボットでは、2慣性系の振動モードにおける動力伝達機構の動作方向の剛性による振動(以下、便宜的に動作方向振動と称する)以外にも、動力伝達機構の動作方向とは異なる振動(上記した非動作方向振動)が存在していることを見いだした。換言すると、動作方向とは異なる向きの非動作方向振動がシミュレーションには現れなかった振動の原因であることが発明者らによって突き止められたとともに、その非動作方向振動によってロボットの可動部側が全体的に動作方向以外にも揺れ、その結果、シャフトの支持部に捩れ振動が発生し、その捩れ振動が手先の振動として現れていたことが明らかになった。 Then, as a result of repeated investigations of the cause of the vibration, the inventors of the actual robot vibrate due to the rigidity of the operation direction of the power transmission mechanism in the vibration mode of the two inertial system (hereinafter, the vibration in the operation direction for convenience). In addition to (referred to as), it was found that there is vibration different from the operating direction of the power transmission mechanism (the above-mentioned non-operating direction vibration). In other words, the inventors have determined that the non-operating direction vibration in a direction different from the operating direction is the cause of the vibration that did not appear in the simulation, and the non-operating direction vibration causes the moving part side of the robot as a whole. As a result, it became clear that torsional vibration was generated in the support part of the shaft, and the torsional vibration appeared as vibration of the hand.

さて、もしも非動作方向振動の存在が今まで認知されていたならば、非動作方向振動に対処するための制御方法が検討されているはずである。しかし、実際には、非動作方向振動の存在を示唆するような考察や非動作方向振動を抑制するための制御方法等は検討されていない。つまり、非動作方向振動は、今まで認知されていなかったと考えられる。そのため、発明者らは、なぜ今まで非動作方向振動が考慮されていなかったのかについて考察した。 Now, if the existence of non-operating directional vibration has been recognized so far, a control method for coping with non-operating directional vibration should be considered. However, in reality, no consideration suggesting the existence of non-operating directional vibration or a control method for suppressing non-operating directional vibration has been studied. In other words, it is considered that the non-operating directional vibration has not been recognized until now. Therefore, the inventors considered why the non-operating directional vibration was not considered so far.

最初期の産業用のロボットは、近年のロボットと比べて、格段にアームやギアあるいは軸受け部材などが相対的に太く且つ頑丈であった反面、アームおよび可動部分の全体の重量が相対的に大きかった。2慣性系の場合、共振周波数は、慣性(つまり重量)の逆数の平方根に比例することから、動作方向振動の共振周波数は低くなる。その一方で、アームを構成する部材等は非常に高剛性に作られており、非動作方向共振が存在していたとしても、その共振周波数は高くなっていたと考えられる。 Compared to recent robots, the earliest industrial robots had significantly thicker and stronger arms, gears, and bearing members, but the overall weight of the arms and moving parts was relatively heavy. It was. In the case of a two-inertial frame, the resonance frequency is proportional to the square root of the reciprocal of inertia (that is, weight), so that the resonance frequency of the vibration in the operating direction becomes low. On the other hand, the members and the like constituting the arm are made to have extremely high rigidity, and it is considered that the resonance frequency is high even if the non-operating directional resonance exists.

一般的に、複数の共振が存在する場合、低い共振周波数を持つ共振による影響が支配的となる場合が多い。つまり、非動作方向共振は、共振周波数が相対的に高かったことから、アームの位置応答や速度応答へ与える影響は無視できるほど小さかったと考えられる。また、発明者らの研究の結果、非動作方向振動の発生原因には例えば遠心力のように非動作方向に加わる力の存在があることが判明したが、最初期のロボットは、近年のロボットに比べて動作速度が相対的に遅かったため、遠心力による影響は無視できるほど小さかったと考えられる。 In general, when a plurality of resonances are present, the influence of the resonance having a low resonance frequency is often dominant. That is, since the resonance frequency of the non-operating direction resonance was relatively high, it is considered that the influence on the position response and the speed response of the arm was negligibly small. In addition, as a result of the research by the inventors, it was found that the cause of the non-operating direction vibration is the existence of a force applied in the non-operating direction such as centrifugal force, but the earliest robots are recent robots. Since the operating speed was relatively slow compared to that of the above, it is considered that the influence of centrifugal force was negligible.

これに対して、近年のロボットでは、アームを太く頑丈にする方向から、細く軽量化する方向へとその設計が変化してきている。つまり、アームが軽量化されてきた反面、アームを構成する部材は、最初期のロボットに比べれば低剛性化している。なお、低剛性化しているとはいっても、柔軟アームと呼ばれるようなアーム自体が捻れてしまうような状態ではなく、例えばクロスローラなどの軸支持部で、その回転軸以外の方向に微少振動が発生しているということである。 On the other hand, in recent years, the design of robots has changed from making the arm thick and sturdy to making it thinner and lighter. In other words, while the weight of the arm has been reduced, the members that make up the arm have lower rigidity than the earliest robots. Although the rigidity is reduced, the arm itself, which is called a flexible arm, is not in a twisted state. For example, a shaft support part such as a cross roller causes a slight vibration in a direction other than the rotation axis. It means that it is occurring.

そのため、複数の共振が存在している状態において支配的な共振が最初期のロボットとは入れ替わってきた、あるいは、両者の共振が近い共振周波数となって互いに影響し合うような状態になってきたと考えられる。さらに、近年のロボットの場合、最初期のロボットと比べてその動作速度が格段に高速化されており、速度の2乗に比例する遠心力の影響がより顕著に現れてきたと考えられる。 Therefore, in the state where multiple resonances exist, the dominant resonance has been replaced with the earliest robot, or the resonances of both have similar resonance frequencies and affect each other. Conceivable. Furthermore, in the case of robots in recent years, the operating speed has been significantly increased as compared with the earliest robots, and it is considered that the influence of centrifugal force proportional to the square of the speed has become more prominent.

例えば4軸の水平多関節型ロボットの場合であれば、シャフトを上下方向(Z方向)へ直動する支持部が、例えばセカンドアームの動きに連動して振動してしまう現象が確認されている。この場合、軸間干渉による振動のようにシャフトの移動方向への振動(動作方向振動に含まれる)とは異なる振動、具体的には、セカンドアームの円周方向(セカンドアームの動作方向)やセカンドアームの直径方向(セカンドアームに加わる遠心力の方向)への振動が発生している。なお、6軸の垂直多関節型ロボット(PUMA(Programmable Universal Manipulation Arm)型ロボット)や、いわゆる7軸ロボット等の他の構成のロボットにおいても、これに類似する現象により、非動作方向振動が発生している。 For example, in the case of a 4-axis horizontal articulated robot, it has been confirmed that the support portion that linearly moves the shaft in the vertical direction (Z direction) vibrates in conjunction with the movement of the second arm, for example. .. In this case, vibration different from vibration in the moving direction of the shaft (included in the operating direction vibration) such as vibration due to inter-axis interference, specifically, the circumferential direction of the second arm (operating direction of the second arm) or Vibration is generated in the radial direction of the second arm (the direction of the centrifugal force applied to the second arm). In addition, even in robots having other configurations such as a 6-axis vertical articulated robot (PUMA (Programmable Universal Manipulation Arm) type robot) and a so-called 7-axis robot, non-operation direction vibration occurs due to a phenomenon similar to this. doing.

そして、産業用のロボットにおいては、複数のアームが連結してロボットを構成することが多いため、ある軸が振動した結果、別の軸が干渉して振動するといった現象が発生する。このため、振動特性自体が単純な2慣性系モデルのようにはっきりとしたものとはならならず、複数の共振振動が周波数特定に現れること、また、周波数特性に現れる共振周波数と実際の振動波形に現れる共振周波数とに僅かな相違があることがある。そのため、非動作方向振動が存在していたとしても、非動作方向振動が原因となっていることを突き止めることが困難であったと考えられる。 In an industrial robot, a plurality of arms are often connected to form a robot, so that a phenomenon occurs in which one axis vibrates and another axis interferes with the vibration. For this reason, the vibration characteristics themselves are not as clear as in a simple two-inertial system model, and multiple resonance vibrations appear in the frequency specification, and the resonance frequency and the actual vibration waveform appearing in the frequency characteristics. There may be a slight difference from the resonance frequency that appears in. Therefore, even if the non-operating direction vibration is present, it is considered that it is difficult to find out that the non-operating direction vibration is the cause.

さらに、動作方向振動と非動作方向振動とでは振動周波数が異なることが多いものの、減速機として例えば波動歯車装置を用いている場合には、その剛性が入力されるトルクに応じて変化することが知られている等、モデル化誤差の原因となる要素が様々であることから、誰も非動作方向振動に想到することがなく、単に誤差として扱われていたものと考えられる。 Further, although the vibration frequency is often different between the operating direction vibration and the non-operating direction vibration, when a wave gear device is used as the speed reducer, for example, its rigidity may change according to the input torque. Since there are various factors that cause modeling errors, such as those known, it is probable that no one came up with vibrations in the non-operating direction and that they were simply treated as errors.

このような事情によって、シミュレーション上で現れた振動のうち最も影響度の大きいものを動作方向振動として扱い、それ以外は他の軸からの干渉などの誤差として扱っていたことから、非動作方向振動についての検討がなされてこなかったものと考えられる。つまり、最初期のロボットでは非動作方向振動がそれほど顕著ではなく、また、近年のロボットでは非動作方向振動が誤差として扱われていたことが、非動作方向振動が認知されていなかった理由であると推測された。 Due to these circumstances, the vibration that has the greatest influence among the vibrations that appear in the simulation is treated as the operating direction vibration, and the others are treated as errors such as interference from other axes. It is probable that no consideration has been given to. In other words, the non-moving direction vibration is not so remarkable in the earliest robots, and the non-moving direction vibration is treated as an error in recent robots, which is the reason why the non-moving direction vibration was not recognized. Was speculated.

そして、このような非動作方向振動の存在は、従来のシミュレーションの前提条件であった2慣性系モデルや干渉を含めた2慣性系モデルを拡張した例えば特許文献1でいう4慣性モデル等の制御モデルでは実際のロボットの振動特性をそもそも表現しきれていなかったこと示しており、極めて重大な技術的意義をもっている。 The existence of such non-operating direction vibration is an extension of the two-inertial frame model and the two-inertial frame model including interference, which are the preconditions of the conventional simulation, for example, the control of the four-inertia model referred to in Patent Document 1. model shows that did not completely originally expressed the vibration characteristics of the actual robot is in, we have a very serious technical significance.

さて、非動作方向振動の存在が判明したのであれば、その非動作方向振動に起因して生じる振動を抑制するような制御を行えば、シミュレーションには現れなかった手先に現れる振動を抑制することができると考えられる。
例えば水平多関節型ロボットの場合、回転軸である1軸目および2軸目の動作に連動して、Z軸直動部(シャフト)の支持部(シャフト支持部)で捩れ振動が発生していた。そして、この捩れ振動は、1軸目および2軸目の回転軸とRz軸(後述する図1参照)の回転だけでなく、Rx軸方向およびRy軸方向にも発生する。そのため、その振動を検出すれば、手先に現れる振動を抑制できると考えられる。
Now, if the existence of non-operating directional vibration is found, if control is performed to suppress the vibration caused by the non-operating directional vibration, the vibration that appears in the hand that did not appear in the simulation can be suppressed. Is thought to be possible.
For example, in the case of a horizontal articulated robot, torsional vibration is generated at the support part (shaft support part) of the Z-axis linear motion part (shaft) in conjunction with the movements of the first and second axes, which are the rotation axes. It was. Then, this torsional vibration occurs not only in the rotation of the rotation axis and the Rz axis (see FIG. 1 described later) of the first and second axes, but also in the Rx axis direction and the Ry axis direction. Therefore, if the vibration is detected, it is considered that the vibration appearing at the hand can be suppressed.

しかし、従来のロボットにおいてはRx軸方向およびRy軸方向の振動を検出するセンサが設けられておらず、Rx軸方向およびRy軸方向の振動を検出すること自体ができかった。そのため、振動の発生を検知してその振動を抑制する制御を行うこともできなかった。
つまり、従来のロボットは、非動作方向振動が考慮されていなかったため、回転方向以外の振動を検出する手段をそもそも備えておらず、また、非動作方向振動の検出自体ができていないことから、その非動作方向振動を収束させる制御行うこともできなかった。
However, in the conventional robot not provided a sensor for detecting vibration of the Rx axis direction and Ry direction, could not be itself be detected vibrations Rx axis direction and Ry axis. Therefore, it was not possible to detect the occurrence of vibration and control the suppression of the vibration.
That is, since the conventional robot does not consider the vibration in the non-operating direction, it does not have a means for detecting the vibration other than the rotation direction in the first place, and the vibration in the non-operating direction itself cannot be detected. It could not even be controlled to converge the inoperative direction vibration.

また、ロボットの手先にセンサを設けることは、一般に想像するよりも困難である。例えば有線式のセンサをロボットの手先に設ける場合には、各アームの回転やシャフトの回転および上下動からケーブルを保護するために、いわゆるケーブルリール等の保護部材が必要となって配線構造が複雑化する。かといって、無線式のセンサを手先に設けると、ロボットの動作中にセンサへ給電あるいは充電することが容易ではないことに加えて、ノイズによって無線通信が断続するおそれがある。 Moreover, it is more difficult than generally imagined to provide a sensor at the hand of a robot. For example, when a wired sensor is installed at the hand of a robot, a protective member such as a so-called cable reel is required to protect the cable from the rotation of each arm, the rotation of the shaft, and the vertical movement, and the wiring structure is complicated. To become. However, if a wireless sensor is provided at hand, it is not easy to supply power or charge the sensor while the robot is operating, and there is a risk that wireless communication will be interrupted due to noise.

そこで、請求項1に係る発明では、ロボット制御装置は、水平多関節型ロボットのファーストアームおよびセカンドアームに設けられている複数のセンサの検出結果から、シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、シャフトの先端部の振動に相関するシャフト支持部での捩れ角度を求めている。 Therefore, in the invention according to claim 1, the robot control device obtains the torque applied to the shaft support portion that supports the shaft from the detection results of a plurality of sensors provided on the first arm and the second arm of the horizontal articulated robot. It is estimated, and based on the estimated torque, the twist angle at the shaft support portion that correlates with the vibration of the tip portion of the shaft is obtained.

つまり、制御装置は、ロボットの手先(シャフトの先端部)ではなく、各アームに設けられているセンサの検出結果から、シャフトに生じる捩れ振動に相関しているとともに、ロボットの手先つまりはシャフトの先端部の振動にも相関しているシャフト支持部での捩れ角度を求めている。 In other words, the control device correlates with the torsional vibration generated in the shaft from the detection results of the sensors provided on each arm, not the robot's hand (the tip of the shaft), and the robot's hand, that is, the shaft. The twist angle at the shaft support, which is also correlated with the vibration of the tip, is calculated.

この場合、具体的な計算方法は、シャフト支持部にかかるトルクをN3、シャフト支持部での捩れ角度をφ3、シャフト支持部の剛性をK3、複数センサの検出値をS1〜SNとすると、次のように数式で記載することができる。
φ3=φ3est(N3)
N3=N3est(S1〜SN)
ただし、φ3estはシャフト支持部にかかるトルクからシャフト支持部での捩れ角度を求める関数であり、N3estは複数センサの検出値からシャフト支持部にかかるトルクを求める関数である。以下でも同様に、末尾にestが付帯された関数は、センサ検出値あるいは別の推定値から、目的とする値を推定する関数である。
In this case, the specific calculation method is as follows, assuming that the torque applied to the shaft support is N3, the twist angle at the shaft support is φ3, the rigidity of the shaft support is K3, and the detection values of the multiple sensors are S1 to SN. It can be described by a mathematical formula as follows.
φ3 = φ3est (N3)
N3 = N3est (S1-SN)
However, φ3est is a function for obtaining the twist angle at the shaft support portion from the torque applied to the shaft support portion, and N3est is a function for obtaining the torque applied to the shaft support portion from the detected values of a plurality of sensors. Similarly in the following, the function with est at the end is a function that estimates the target value from the sensor detection value or another estimated value.

この場合、ファーストアームに関しては、ファーストアームに設けられているセンサの検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボットでは各アームがリンクしていることから、セカンドアームに関しては、ファーストアームに設けられているセンサの検出結果とセカンドアームに設けられているセンサの検出結果とから、セカンドアームのみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、シャフトに関しても、それぞれのセンサの検出結果から、シャフト支持部にかかるトルクを求めることができる。 In this case, with respect to the first arm, the torque applied to the first arm support portion can be obtained from the detection result of the sensor provided on the first arm. In addition, since each arm is linked in the robot, the second arm is viewed only with the second arm from the detection result of the sensor provided on the first arm and the detection result of the sensor provided on the second arm. The value of the case can be obtained, and the torque applied to the second arm support portion can be obtained from the obtained value. Similarly, with respect to the shaft, the torque applied to the shaft support portion can be obtained from the detection results of each sensor.

そして、シャフト支持部にかかるトルクが求まれば、シャフトの剛性に基づいてシャフトの捩れ角度を求めることができる。
この場合、具体的な計算方法は、シャフト支持部の剛性をK3とすると、次のように数式で記載することができる。
φ3=φ3est(N3)=N3÷K3
このように、各アームに設けられているセンサからの複数の検出結果を利用することにより、ロボットの手先にセンサを設けなくても、ロボットの手先の振動の要因となるシャフト支持部での捩れ振動を求めることができる。
また、センサは、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。
Then, if the torque applied to the shaft support portion is obtained, the twist angle of the shaft can be obtained based on the rigidity of the shaft.
In this case, the specific calculation method can be described by a mathematical formula as follows, assuming that the rigidity of the shaft support portion is K3.
φ3 = φ3est (N3) = N3 ÷ K3
In this way, by utilizing a plurality of detection results from the sensors provided on each arm, even if the sensor is not provided on the robot's hand, the twist at the shaft support portion that causes vibration of the robot's hand Vibration can be calculated.
Further, since the sensor is provided on each arm, the wiring structure thereof can be simplified as compared with the case where the sensor is provided on the hand.

より詳細には、ロボット制御装置は、各アームに設けられているセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクからシャフト支持部にかかるトルクを推定し、推定したトルクに基づいてシャフト支持部での捩れ角度を求める。
この場合、具体的な計算方法は、ファーストアームの慣性力をFG1、ファーストアームの慣性トルクをNG1、セカンドアームの慣性力をFG2、セカンドアームの慣性トルクをNG2とすると、次のように数式で記載することができる。
More specifically, the robot controller calculates the inertial forces and inertial torques in each arm from the detection result of the sensor provided on each arm, the obtained inertial force and estimating the torque applied to the shaft supporting portion from the inertial torque Then, the twist angle at the shaft support is obtained based on the estimated torque.
In this case, the specific calculation method is as follows, assuming that the inertial force of the first arm is FG1, the inertial torque of the first arm is NG1, the inertial force of the second arm is FG2, and the inertial torque of the second arm is NG2. Can be described.

FG1=FG1est(S1〜SN)
FG2=FG2est(S1〜SN)
NG1=NG1est(S1〜SN)
NG2=NG2est(S1〜SN)
N3=N3est(FG1,FG2,NG1,NG2)
このような手順および数式により、シャフト支持部にかかるトルクを推定することができる。
FG1 = FG1est (S1-SN)
FG2 = FG2est (S1-SN)
NG1 = NG1est (S1-SN)
NG2 = NG2est (S1-SN)
N3 = N3est (FG1, FG2, NG1, NG2)
The torque applied to the shaft support can be estimated by such a procedure and a mathematical formula.

各アームに設けられているセンサは、具体的には、請求項3に係る発明のように、慣性センサ、回転角度センサおよびシャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフトの上下位置を直接的に検出できる。
このように、シャフト支持部にかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
Specifically, the sensor provided on each arm is composed of any combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft, as in the invention of claim 3. ing. For example, the velocity and acceleration can be obtained by calculation if either one is detected. However, in that case, the calculation time is required. In other words, if the calculation is unnecessary, the processing speed can be improved by that amount. Then, the inertial sensor can directly detect the inertial force, the rotation angle sensor can directly detect the rotation angle, and the position sensor can directly detect the vertical position of the shaft.
In this way, the processing speed can be improved by using a sensor that can directly detect various physical quantities required for estimating the torque applied to the shaft support portion.

さらに詳細には、請求項4に係る発明のように、慣性センサとして角速度を検出する角速度センサ、あるいは加速度を検出する加速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。
角速度センサは、請求項5に係る発明のように、複数方向への角速度を検出する角速度センサを用いることができる。
加速度センサは、請求項6に係る発明のように、複数方向への加速度を検出する加速度センサを用いることができる。
More specifically, as in the invention of claim 4, by using an angular velocity sensor that detects angular velocity or an acceleration sensor that detects acceleration as an inertial sensor, a calculation for obtaining torque at each arm support portion can be performed. It can be omitted or simplified.
As the angular velocity sensor, as in the invention of claim 5, an angular velocity sensor that detects angular velocities in a plurality of directions can be used.
As the acceleration sensor, as in the invention according to claim 6, an acceleration sensor that detects acceleration in a plurality of directions can be used.

回転角度センサは、請求項7に係る発明のように、ファーストアームをベースに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項8に係る発明のように、セカンドアームをファーストアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項9に係る発明のように、シャフトをセカンドアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、位置センサは、請求項10に係る発明のように、シャフトを上下駆動するモータに設けられているモータエンコーダを用いることができる。
このようにアームやシャフトを駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、回転角度やシャフトの上下位置を取得することができる。
As the rotation angle sensor, as in the invention of claim 7, a motor encoder provided in a motor that rotationally drives the first arm with respect to the base can be used.
Further, as the rotation angle sensor, as in the invention of claim 8, a motor encoder provided in a motor for rotationally driving the second arm with respect to the first arm can be used.
Further, as the rotation angle sensor, as in the invention of claim 9, a motor encoder provided in a motor that rotationally drives the shaft with respect to the second arm can be used.
Further, as the position sensor, a motor encoder provided in a motor that drives the shaft up and down can be used as in the invention of claim 10.
By using a motor encoder generally provided for a motor that drives an arm or a shaft in this way, it is possible to acquire a rotation angle and a vertical position of the shaft without requiring an additional member.

また、上記した請求項1に係る発明では、シャフト側の重心位置を求め、求めた重心位置とシャフト支持部との距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部の捩れ角度を生じさせる振動を抑制する振動抑制制御を行う。ここで、シャフト側の重心位置とは、シャフトにツールが取り付けられている場合にはシャフトとツールの重量を含んだ状態における重心位置を意味し、そのツールがワークを把持している場合には、シャフト、ツールおよびワークの重量を含んだ状態における重心位置を意味する。 Further, in the invention according to claim 1 described above, the position of the center of gravity on the shaft side is obtained, and the suppression frequency corresponding to the moment of inertia at the position of the center of gravity is obtained and obtained based on the distance between the obtained center of gravity position and the shaft support portion. By performing the notch filter calculation on the suppression frequency, vibration suppression control that suppresses the vibration that causes the twist angle of the shaft support is performed. Here, the position of the center of gravity on the shaft side means the position of the center of gravity in a state including the weight of the shaft and the tool when the tool is attached to the shaft, and when the tool grips the work. , Means the position of the center of gravity of the shaft, tool and workpiece including the weight.

シャフトの上下位置が変化すると、それに応じてシャフト側の重心位置が変化する。このとき、重心位置が変化すると、重心位置における慣性モーメントも変化する。つまり、シャフト支持部の捩れ角度を生じさせる要因となる慣性モーメントは、重心位置の変化に応じて変化する。このため、シャフト支持部の捩れ角度を生じさせる要因となる振動を抑制するためには、従来のように2次元空間を対象とするだけでは不十分であり、3次元空間を対象とする必要がある。 When the vertical position of the shaft changes, the position of the center of gravity on the shaft side changes accordingly. At this time, when the position of the center of gravity changes, the moment of inertia at the position of the center of gravity also changes. That is, the moment of inertia, which is a factor that causes the twist angle of the shaft support portion, changes according to the change in the position of the center of gravity. For this reason, in order to suppress the vibration that causes the twist angle of the shaft support portion, it is not enough to target the two-dimensional space as in the conventional case, and it is necessary to target the three-dimensional space. is there.

そこで、重心位置における慣性モーメントに対応する抑制周波数を求めることにより、3次元空間を対象とした場合における抑制すべき周波数である抑制周波数を求めることができる。そして、抑制周波数が求まれば、その抑制周波数を抑制するような演算、つまりは、重心位置の慣性モーメントに応じたノッチフィルタ演算を行うことにより、シャフト側の重心位置が変化した場合であっても、その変化に追従させてノッチフィルタ演算を行うことができる。つまり、シャフト支持部の捩れ角度を生じさせる振動を抑制することができる。
また、シャフトの重心位置に応じて慣性モーメントが変化するという従来技術では考慮されていなかった事象を考慮した上で振動抑制制御を行うことから、従来技術では考慮すらされていなかったために抑制することができなかった動作方向とは異なる向きに生じる非動作方向振動を、抑制することができる。
Therefore, by obtaining the suppression frequency corresponding to the moment of inertia at the position of the center of gravity, it is possible to obtain the suppression frequency, which is the frequency to be suppressed when the three-dimensional space is targeted. Then, when the suppression frequency is obtained, the operation that suppresses the suppression frequency, that is, the notch filter calculation according to the moment of inertia of the center of gravity position is performed to change the position of the center of gravity on the shaft side. However, the notch filter calculation can be performed by following the change. That is, it is possible to suppress the vibration that causes the twist angle of the shaft support portion.
In addition, since vibration suppression control is performed after considering the phenomenon that the moment of inertia changes according to the position of the center of gravity of the shaft, which was not considered in the prior art, it is suppressed because it was not even considered in the prior art. It is possible to suppress non-operating direction vibration that occurs in a direction different from the operating direction that could not be achieved.

より具体的には、請求項11に係る発明のように、ノッチフィルタ演算に用いるフィルタパラメータを、シャフト側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、シャフト支持部からシャフト側の重心位置までの距離に反比例する可変周波数に対して抑制周波数を求めることができる。 More specifically, as in the invention of claim 11 , the filter parameters used in the notch filter calculation are made variable by following the change in the antiresonance frequency that changes according to the position of the center of gravity on the shaft side. The suppression frequency can be obtained for a variable frequency that is inversely proportional to the distance from the shaft support portion to the position of the center of gravity on the shaft side.

第1実施形態によるロボットおよびロボット制御装置の構成を模式的に示す図The figure which shows typically the structure of the robot and the robot control device by 1st Embodiment. ファーストアームに生じるRy回転を模式的に示す図The figure which shows typically the Ry rotation which occurs in the first arm センサ位置を模式的に示す図The figure which shows the sensor position schematically 第2実施形態におけるセンサ位置を模式的に示す図 The figure which shows typically the sensor position in 2nd Embodiment 第3実施形態における重心位置を模式的に示す図The figure which shows typically the position of the center of gravity in 3rd Embodiment 重心位置における捩れを模式的に示す図The figure which shows the twist at the position of the center of gravity schematically 抑制周波数可変ノッチフィルタ処理の流れを示す図The figure which shows the flow of the suppression frequency variable notch filter processing

以下、本発明の複数の実施形態について、図面を参照しながら説明する。なお、各実施形態において実質的に共通する部位には同一の符号を付し、その詳細な説明は省略する。
(第1実施形態)
本発明の第1実施形態について図1から図3を参照しながら説明する。
Hereinafter, a plurality of embodiments of the present invention will be described with reference to the drawings. The parts that are substantially common in each embodiment are designated by the same reference numerals, and detailed description thereof will be omitted.
(First Embodiment)
The first embodiment of the present invention will be described with reference to FIGS. 1 to 3.

図1に示すように、ロボットシステム1は、ロボット2と、ロボット2を制御する制御装置3(ロボット制御装置に相当する)とを備えている。制御装置3は、制御部3aを備えており、ロボット2を制御する。この制御部3aは、詳細は後述するが、センサ8、9、10(図参照)の検出結果にもとづいてシャフト7を支持するシャフト支持部6a(図3参照)にかかるトルクを推定し、推定したトルクに基づいて当該シャフト支持部6aでの捩れ角度を求めることを含む実施形態で説明する種々の処理を実行する。このシャフト支持部6aでの捩れ角度は、捩れ振動に相関しているとともに、ロボット2の手先つまりはシャフト7の先端部7aの振動にも相関している。 As shown in FIG. 1, the robot system 1 includes a robot 2 and a control device 3 (corresponding to a robot control device) that controls the robot 2. The control device 3 includes a control unit 3a and controls the robot 2. Although the details will be described later, the control unit 3a estimates the torque applied to the shaft support unit 6a (see FIG. 3) that supports the shaft 7 based on the detection results of the sensors 8, 9, and 10 (see FIG. 3). Various processes described in the embodiments are performed, including obtaining the twist angle at the shaft support portion 6a based on the estimated torque. The twist angle at the shaft support portion 6a correlates with the torsional vibration, and also correlates with the vibration of the hand of the robot 2, that is, the tip portion 7a of the shaft 7.

本実施形態のロボット2は、ベース4と、ベース4に取り付けられ(ベース4に支持され)、当該ベース4に対して第1軸(J1)を中心として回転するファーストアーム5と、ファーストアーム5に取り付けられ(ファーストアーム5に支持され)、当該ファーストアーム5に対して第2軸(J2)を中心として回転するセカンドアーム6と、セカンドアーム6に取り付けられ(セカンドアーム6に支持され)、当該セカンドアーム6に対して上下移動および回転するシャフト7と、を有する水平多関節型ロボットを想定している。 The robot 2 of the present embodiment has a base 4, a first arm 5 attached to the base 4 (supported by the base 4), and a first arm 5 and a first arm 5 that rotate about the first axis (J1) with respect to the base 4. Attached to (supported by the first arm 5) and rotated about the second axis (J2) with respect to the first arm 5, and attached to the second arm 6 (supported by the second arm 6). A horizontal articulated robot having a shaft 7 that moves up and down and rotates with respect to the second arm 6 is assumed.

シャフト7は、本実施形態では、ボールねじスプラインを想定している。この場合、ロボット2の手先とは、シャフト7の下端側、つまり、セカンドアーム6に対して上下方向に相対的に移動するとともに、セカンドアーム6に対して相対的に回転する部分に相当する。なお、シャフト7は、非常に剛性が高く設計されており、シャフト7そのものが撓んだり変形したりすることは考えにくい。ただし、シャフト7の剛性が低く、シャフト7そのものが撓んだり変形したりしている場合であっても、その撓みや変形がシャフト支持部6aで起きているものとして等価変換することにより、同様の効果を得ることが出来る。 In this embodiment, the shaft 7 assumes a ball screw spline. In this case, the hand of the robot 2 corresponds to the lower end side of the shaft 7, that is, a portion that moves relative to the second arm 6 in the vertical direction and rotates relative to the second arm 6. The shaft 7 is designed to have extremely high rigidity, and it is unlikely that the shaft 7 itself will bend or deform. However, even if the rigidity of the shaft 7 is low and the shaft 7 itself is bent or deformed, the same can be achieved by equivalent conversion assuming that the bending or deformation occurs in the shaft support portion 6a. The effect of can be obtained.

以下、図1に示すように、シャフト支持部6aを原点とし、シャフト7と同軸となる天地方向をZ軸(Z方向)、Z軸に直交し、セカンドアーム6に沿った方向をX軸(X方向)、これらZ軸およびX軸に直交する方向をY軸(Y方向)として説明する。また、図1に示すZ軸回りの回転をRz回転と称し、X軸回りの回転をRx回転と称し、Y軸回りの回転をRy回転と称する。 Hereinafter, as shown in FIG. 1, the shaft support portion 6a is the origin, the vertical direction coaxial with the shaft 7 is the Z axis (Z direction), the direction orthogonal to the Z axis is orthogonal to the Z axis, and the direction along the second arm 6 is the X axis ( The X direction), and the directions orthogonal to the Z axis and the X axis will be described as the Y axis (Y direction). Further, the rotation around the Z axis shown in FIG. 1 is referred to as Rz rotation, the rotation around the X axis is referred to as Rx rotation, and the rotation around the Y axis is referred to as Ry rotation.

この場合、Rx回転とは、図1に示す状態においてシャフト支持部6aを中心としてセカンドアーム6がX軸回りに回転する状態、より平易に言えば、セカンドアーム6が捩れた状態に相当する。また、Ry回転とは、セカンドアーム6を例にすると、図1に示す状態においてシャフト支持部6aを中心としてセカンドアーム6がY軸回りに回転する状態、より平易に言えば、セカンドアーム6の先端側がZ方向に揺れる状態に相当する。 In this case, the Rx rotation corresponds to a state in which the second arm 6 rotates around the shaft support portion 6a in the state shown in FIG. 1, or more simply, a state in which the second arm 6 is twisted. Further, the Ry rotation is a state in which the second arm 6 rotates about the Y axis around the shaft support portion 6a in the state shown in FIG. 1, taking the second arm 6 as an example, or more simply, the second arm 6 This corresponds to a state in which the tip side swings in the Z direction.

このロボット2は、図3に示すように、複数のセンサ8、9、10を備えている。このうち、センサ8、9は、ファーストアーム5およびセカンドアーム6に設けられ、各アームの慣性を検出する慣性センサであり、センサ10は、セカンドアーム6に設けられ、シャフト7の上下位置を検出する位置センサである。各センサ8、9の配置は、基本的には任意に設定することができる。 As shown in FIG. 3, the robot 2 includes a plurality of sensors 8, 9, and 10. Of these, the sensors 8 and 9 are inertial sensors provided on the first arm 5 and the second arm 6 to detect the inertia of each arm, and the sensor 10 is provided on the second arm 6 to detect the vertical position of the shaft 7. It is a position sensor. The arrangement of the sensors 8 and 9 can be basically arbitrarily set.

より具体的には、本実施形態では、センサ8は、ファーストアーム5に設けられており、ファーストアーム5が動作した際のRx回転の角速度およびRy回転の角速度を検出する2軸角速度センサで構成されており、図2に示すファーストアーム5における動作方向とは異なる向き(非動作方向)に生じる角速度を検出する。また、センサ9は、セカンドアーム6に設けられ、当該セカンドアーム6が動作した際のRx回転の角速度およびRy回転の角速度を検出する2軸角速度センサで構成されており、セカンドアーム6における動作方向とは異なる向き(非動作方向)に生じる角速度を検出する。 More specifically, in the present embodiment, the sensor 8 is provided on the first arm 5, and includes a biaxial angular velocity sensor that detects the angular velocity of Rx rotation and the angular velocity of Ry rotation when the first arm 5 operates. The angular velocity generated in a direction different from the operating direction (non-operating direction) of the first arm 5 shown in FIG. 2 is detected. Further, the sensor 9 is provided on the second arm 6 and is composed of a biaxial angular velocity sensor that detects the angular velocity of Rx rotation and the angular velocity of Ry rotation when the second arm 6 operates, and is composed of an operating direction in the second arm 6. Detects the angular velocity that occurs in a direction different from (non-operating direction).

また、センサ10は、セカンドアーム6内のシャフト支持部6aに設けられており、シャフト7を上下駆動するモータの回転角度を検出するモータエンコーダである。なお、図示は省略するが、これら各センサ8、9、10は、ケーブルにて接続されている有線式のものを使用している。なお、図示は省略するが、ファーストアーム5を駆動するモータおよびセカンドアーム6を回転駆動するモータにも、モータエンコーダが設けられている。 Further, the sensor 10 is provided on the shaft support portion 6a in the second arm 6 and is a motor encoder that detects the rotation angle of the motor that drives the shaft 7 up and down. Although not shown, each of the sensors 8, 9 and 10 uses a wired type sensor connected by a cable. Although not shown, motor encoders are also provided in the motor that drives the first arm 5 and the motor that rotationally drives the second arm 6.

次に、上記した構成の作用について説明する。
問題となっているシャフト支持部6aでの捩れ振動の振動周波数は、減速機構での回転方向の共振振動の振動周波数よりも十分に低い場合が多い。一例としては、例えばシャフト7直動部の支持部での捩れ振動の振動周波数が10Hz程度以下であるのに対し、減速機構での回転方向の共振振動の振動周波数は、50Hz程度である。なお、シャフト7がボールねじスプラインの場合などでは、その共振振動周波数は100Hzを超えることもある。
Next, the operation of the above configuration will be described.
The vibration frequency of the torsional vibration at the shaft support portion 6a, which is a problem, is often sufficiently lower than the vibration frequency of the resonance vibration in the rotation direction of the reduction mechanism. As an example, for example, the vibration frequency of the torsional vibration in the support portion of the linear motion portion of the shaft 7 is about 10 Hz or less, while the vibration frequency of the resonance vibration in the rotation direction in the reduction mechanism is about 50 Hz. When the shaft 7 is a ball screw spline or the like, its resonance vibration frequency may exceed 100 Hz.

このように振動周波数の差が大きい場合には、モータエンコーダの値をアームのRz軸回転角度あるいはZ軸位置(シャフト位置)として問題ない。なお、回転方向の共振振動が問題になる場合には、減速機の出力側にエンコーダ(シャフト7の場合にはリニアエンコーダ等)を設けたり、角速度センサや加速度センサを追加したり、状態オブザーバにより共振振動を推定したりすることで、共振振動の影響を除外することができる。 When the difference in vibration frequency is large as described above, there is no problem in using the value of the motor encoder as the Rz axis rotation angle of the arm or the Z axis position (shaft position). If resonance vibration in the rotation direction becomes a problem, an encoder (linear encoder, etc. in the case of shaft 7) may be provided on the output side of the speed reducer, an angular velocity sensor or acceleration sensor may be added, or a state observer may be used. By estimating the resonance vibration, the influence of the resonance vibration can be excluded.

そこで、まず、各アームにおける各種のパラメータや変数を、以下のように定義する。なお、以下では、各種のパラメータや変数を総称してパラメータとしている。また、使用可能文字の関係上、明細書中では、例えばファーストアーム支持部捩れ角度などについては、「φ」と示すものとする。
<<アームに関するパラメータ>>
Therefore, first, various parameters and variables in each arm are defined as follows. In the following, various parameters and variables are collectively referred to as parameters. Further, due to the characters that can be used, for example, the twist angle of the first arm support portion is indicated as "φ" in the specification.
<< Parameters related to the arm >>

Figure 0006834124
Figure 0006834124

<<アームに関するパラメータ>> << Parameters related to the arm >>

Figure 0006834124
Figure 0006834124

<<シャフト7に関するパラメータ>> << Parameters related to shaft 7 >>

Figure 0006834124
Figure 0006834124

これらのパラメータのうち、以下のパラメータは、各センサ8〜10によって検出された値、あるいは、検出された値を演算することにより求めることができる。 Of these parameters, the following parameters can be obtained by calculating the values detected by the sensors 8 to 10 or the detected values.

Figure 0006834124
Figure 0006834124

なお、ファーストアーム5およびセカンドアーム6の重心位置での角速度は、ファーストアーム5およびセカンドアーム6内のどの点の角速度を測定しても、重心位置での角速度と同じ値が検出される。
また、各パラメータは、以下の関係を持っている。
The angular velocity of the first arm 5 and the second arm 6 at the center of gravity is the same as the angular velocity at the center of gravity regardless of which point in the first arm 5 and the second arm 6 is measured.
In addition, each parameter has the following relationship.

Figure 0006834124
Figure 0006834124

さて、トルクの推定手法について述べる前に、後述する演算の簡略化について説明する。
回転振動について、その回転軸を単位ベクトルv、その回転量をφとする。ただし、単位ベクトルvは、以下の(3)式である。このとき、回転行列Rは、ロドリゲスの回転公(Rodrigues' Rotation Formula)を用いると、以下の(4)式で表される。この、回転行列R中の行列Vは、単位ベクトルvにより、以下の(5)式のように定義される。そして、単位ベクトルvおよび回転量φは、以下の(6)式および(7)式のように求まる。
By the way, before describing the torque estimation method, the simplification of the calculation described later will be described.
For rotational vibration, let the axis of rotation be the unit vector v and the amount of rotation be φ. However, the unit vector v is the following equation (3). At this time, the rotation matrix R is represented by the following equation (4) using Rodrigues' Rotation Formula. The matrix V in the rotation matrix R is defined by the unit vector v as shown in the following equation (5). Then, the unit vector v and the rotation amount φ can be obtained as in the following equations (6) and (7).

Figure 0006834124
Figure 0006834124

ここで、例えばファーストアーム5とセカンドアーム6のアーム長の合計が1000mmであったとし、手先に現れる振動がおよそ1mm程度であったとすると、回転量φは0.001(rad)となる。このとき、φ<<1であることから、sinφ=φ、cosφ=1と近似すると、回転行列Rは、以下の(8)式のように表すことができる。 Here, for example, assuming that the total arm length of the first arm 5 and the second arm 6 is 1000 mm and the vibration appearing on the hand is about 1 mm, the rotation amount φ is 0.001 (rad). At this time, since φ << 1, the rotation matrix R can be expressed as the following equation (8) by approximating sinφ = φ and cosφ = 1.

Figure 0006834124
Figure 0006834124

ここで、vが単位ベクトルであることから、回転行列Rの非対角成分の絶対値は、φ以下となることが分かる。このことは、ある軸の値に対して、別の軸の値が最大でφだけ誤差として重畳してしまうことを示している。
さて、計測対象の軸に対して直交する別の軸からの影響度を一般的に他軸感度と呼ぶが、上記のように近似した値を用いることで回転行列をRz回転に限定したことによる他軸感度への影響は、0.1%となる。
Here, since v is a unit vector, it can be seen that the absolute value of the off-diagonal component of the rotation matrix R is φ or less. This indicates that the value of another axis is superimposed as an error by φ at the maximum with respect to the value of one axis.
By the way, the degree of influence from another axis orthogonal to the axis to be measured is generally called the other axis sensitivity, but it is because the rotation matrix is limited to Rz rotation by using the values approximated as described above. The effect on the sensitivity of the other axis is 0.1%.

これに対して、センサ8、9等の慣性センサ自体の他軸感度は、一般的に、数%程度(例えば4%程度)である。このため、回転行列をRz回転に限定することによる誤差は、慣性センサ自体の持つ他軸感度による誤差と比べて一桁以上小さくなっている。このため、Rz軸方向の回転のみを考慮し、捩れ角度の影響は無視しても、実用上つまりは実際にロボット2を制御する上では、大きな問題となることはない。そのため、本実施形態では、以下のように、シャフト支持部6aにかかるトルクを求める際には、回転行列についてはRz軸方向の回転のみを考慮し、捩れ角度の影響は無視することで、演算を簡略化することができるようになる。
さて、このように演算を簡略化した場合には、各回転行列は、以下の(9)式から(11)式のように表すことができる。
On the other hand, the other axis sensitivity of the inertial sensors such as the sensors 8 and 9 is generally about several percent (for example, about 4%). Therefore, the error due to limiting the rotation matrix to Rz rotation is one digit or more smaller than the error due to the sensitivity of the other axis of the inertial sensor itself. Therefore, even if only the rotation in the Rz axis direction is considered and the influence of the twist angle is ignored, it does not pose a big problem in practical use, that is, in actually controlling the robot 2. Therefore, in the present embodiment, when obtaining the torque applied to the shaft support portion 6a as described below, only the rotation in the Rz axis direction is considered for the rotation matrix, and the influence of the twist angle is ignored. Will be able to be simplified.
By the way, when the calculation is simplified in this way, each rotation matrix can be expressed as the following equations (9) to (11).

Figure 0006834124
Figure 0006834124

勿論、簡略化せずに、それぞれの支持部における捩れ角度の影響を含めて、Rx回転およびRy回転を考慮した回転行列を用いる場合にも、以下の手法によりトルクを推定することは可能である。
さて、ファーストアーム5およびセカンドアーム6の重心位置の加速度、慣性力、慣性トルクは、以下の(12)式から(17)式のようになる。
Of course, without simplification, it is possible to estimate the torque by the following method even when using a rotation matrix that takes into account Rx rotation and Ry rotation , including the effect of the twist angle at each support. ..
The acceleration, inertial force, and inertial torque at the center of gravity of the first arm 5 and the second arm 6 are as shown in the following equations (12) to (17).

Figure 0006834124
Figure 0006834124

これらより、ファーストアーム支持部およびセカンドアーム支持部の力およびトルクは、以下の(18)式から(21)式のようになる。 From these, the forces and torques of the first arm support portion and the second arm support portion are as shown in the following equations (18) to (21).

Figure 0006834124
Figure 0006834124

また、ファーストアーム支持部およびセカンドアーム支持部のトルクは、支持部の剛性を用いて、以下の(22)式および(23)式のように求まる。ただし、Rz軸方向のトルクについては、厳密に捩れ剛性を用いて求めなくても、モータトルクをギア比により各アーム支持部のRz軸方向のトルクに換算して用いても良い。 Further, the torques of the first arm support portion and the second arm support portion can be obtained by using the rigidity of the support portion as in the following equations (22) and (23). However, the torque in the Rz axis direction may not be obtained strictly by using the torsional rigidity, but the motor torque may be converted into the torque in the Rz axis direction of each arm support portion by the gear ratio and used.

Figure 0006834124
Figure 0006834124

そして、ファーストアーム支持部のトルクから、セカンドアーム支持部の力を求める。ただし、ここで求まるセカンドアーム支持部の力は、この時点では一意に求まることはなく、例えば(25)式のλL1や(26)式のλR2L1のように、ファーストアーム5のリンク長ベクトル方向に水平な未知の項を含んだ状態で求まる。そして、求めたセカンドアーム支持部の力より、シャフト支持部6aの力を以下の(27)式のように求める。なお、以下の式のNF21、F20は、式を置き換えるために定義したものである。 Then, the force of the second arm support portion is obtained from the torque of the first arm support portion. However, the force of the second arm support portion obtained here is not uniquely obtained at this point, and is in the direction of the link length vector of the first arm 5, for example, λL1 in equation (25) and λR2L1 in equation (26). It is obtained with a horizontal unknown term included. Then, the force of the shaft support portion 6a is obtained from the obtained force of the second arm support portion as shown in the following equation (27). Note that NF21 and F20 in the following equations are defined to replace the equations.

Figure 0006834124
Figure 0006834124

続いて、セカンドアーム支持部のトルクから、以下の(28)式および(29)式のように、シャフト支持部6aの力を消去する。 Subsequently, the force of the shaft support portion 6a is eliminated from the torque of the second arm support portion as in the following equations (28) and (29).

Figure 0006834124
Figure 0006834124

この(28)式から、未知項は、ファーストアームリンク長を、セカンドアーム回転角度だけRz回転させたベクトルとセカンドアーム6のリンク長の外積で表されることが明らかになった。これらリンク長ベクトルは、X軸方向にのみ大きな値であるものの、Y軸方向あるいはZ軸方向には小さい値となっている。
このため、セカンドアーム回転角度が0°の場合、未知項は、非常に小さな値となるため、無視しても問題ないレベルとなる。また、セカンドアーム回転角度が90°の場合、未知項は、ほぼRz軸方向のベクトルとなり、Rx軸方向あるいはRy軸方向については、その影響をほぼ無視しても問題ないレベルとなっている。
From this equation (28), it was clarified that the unknown term is represented by the outer product of the vector obtained by rotating the first arm link length by Rz by the second arm rotation angle and the link length of the second arm 6. Although these link length vectors have large values only in the X-axis direction, they have small values in the Y-axis direction or the Z-axis direction.
Therefore, when the rotation angle of the second arm is 0 °, the unknown term becomes a very small value, so that it can be ignored. Further, when the rotation angle of the second arm is 90 °, the unknown term becomes a vector in the Rz axis direction, and the influence in the Rx axis direction or the Ry axis direction can be ignored.

すなわち、上記した式には未知項が含まれているものの、その影響は、Rz軸方向へは大きく作用する場合があるものの、Rx軸方向あるいはRy軸方向ではその影響を無視できる。これを式として表すならば、リンク長ベクトルがX軸成分のみに値を持つとすれば、未知項は、Rz軸方向へのみ影響を表すことから、次の(30)式から(32)式のように示すことができる。ただし、(32)式における「×」は、外積を示す。 That is, although the above equation contains an unknown term, its influence may have a large effect in the Rz axis direction, but its influence can be ignored in the Rx axis direction or the Ry axis direction. If this is expressed as an equation, if the link length vector has a value only in the X-axis component, the unknown term expresses the influence only in the Rz-axis direction. Therefore, the following equations (30) to (32) Can be shown as However, "x" in the equation (32) indicates the outer product.

Figure 0006834124
Figure 0006834124

次に、セカンドアーム支持部のトルクからシャフト支持部6aのトルクを、以下の(33)式から(35)式のようにして求める。そして、このシャフト支持部6aのトルクと剛性とより、シャフト支持部捩れ角が、以下の(36)式のように求まる。ただし、シャフト支持部6aのトルクにも上記したRz軸方向へは未知項が影響しており、この未知項の影響により、シャフト支持部捩れ角は、Rx軸方向およびRy軸方向の捩れ角のみが有効なものとして導出されている。 Next, the torque of the shaft support portion 6a is obtained from the torque of the second arm support portion by the following equations (33) to (35). Then, from the torque and rigidity of the shaft support portion 6a, the twist angle of the shaft support portion can be obtained as shown in the following equation (36). However, the torque of the shaft support portion 6a is also affected by the unknown term in the Rz axis direction described above, and due to the influence of this unknown term, the twist angle of the shaft support portion is only the twist angle in the Rx axis direction and the Ry axis direction. Is derived as valid.

Figure 0006834124
Figure 0006834124

なお、シャフト支持部捩れ角は、ファーストアーム支持部あるいはセカンドアーム支持部の場合と同様に、4軸目モータトルクをギア比により変換することでRz軸方向トルクを求め、そのRz軸方向トルクからシャフト支持部捩れ角を求めてもよい。
このように、本実施形態の制御装置3は、手先では無く、各アームに設けた複数のセンサの検出結果に基づいてシャフト支持部6aのトルクを推定し、推定したシャフト支持部6aのトルクに基づいて、シャフト支持部捩れ角、つまりは、手先の振動の要因となるシャフト支持部6aでの捩れ振動を求めている。
As for the twist angle of the shaft support portion, the torque in the Rz axis direction is obtained by converting the motor torque of the fourth axis by the gear ratio, as in the case of the first arm support portion or the second arm support portion, and the torque in the Rz axis direction is used to obtain the torque in the Rz axis direction. The twist angle of the shaft support may be obtained.
As described above, the control device 3 of the present embodiment estimates the torque of the shaft support portion 6a based on the detection results of a plurality of sensors provided on each arm instead of the hand, and obtains the estimated torque of the shaft support portion 6a. Based on this, the torsional angle of the shaft support portion, that is, the torsional vibration at the shaft support portion 6a, which causes the vibration of the hand, is obtained.

以上説明した実施形態によれば、次のような効果を得ることができる。
ロボット制御装置(制御装置3)は、水平多関節型のロボット2のファーストアーム5およびセカンドアーム6に設けられている複数のセンサ8、9、10の検出結果から、シャフト7を支持するシャフト支持部6aにかかるトルク(N3)を推定し、当該推定したトルク(N3)とシャフト7の剛性とに基づいて、シャフト7の先端部7aの振動に相関するシャフト支持部6aでの捩れ角度を求めている。
According to the embodiment described above, the following effects can be obtained.
The robot control device (control device 3) supports the shaft 7 from the detection results of a plurality of sensors 8, 9, and 10 provided on the first arm 5 and the second arm 6 of the horizontal articulated robot 2. The torque (N3) applied to the portion 6a is estimated, and the twist angle at the shaft support portion 6a that correlates with the vibration of the tip portion 7a of the shaft 7 is obtained based on the estimated torque (N3) and the rigidity of the shaft 7. ing.

つまり、制御装置3は、ロボット2の手先(シャフト7の先端部7a)ではなく、各アームに設けられているセンサ8、9、10の検出結果から、シャフト7に生じる捩れ振動に相関しているとともに、ロボット2の手先つまりはシャフト7の先端部7aの振動にも相関しているシャフト支持部6aでの捩れ角度を求めている。 That is, the control device 3 correlates with the torsional vibration generated in the shaft 7 from the detection results of the sensors 8, 9 and 10 provided on each arm, not the hand of the robot 2 (the tip portion 7a of the shaft 7). At the same time, the twist angle at the shaft support portion 6a, which correlates with the vibration of the hand of the robot 2, that is, the tip portion 7a of the shaft 7, is obtained.

ファーストアーム5に設けられているセンサ8の検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボット2は各アームがリンクしていることから、ファーストアーム5に設けられているセンサ8の検出結果と、そのファーストアーム5に対して相対的に動作可能なセカンドアーム6に設けられているセンサ9の検出結果とから、セカンドアーム6のみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、センサ8〜10の検出結果から、シャフト支持部6aにかかるトルクを求めることができる。
そして、シャフト支持部6aにかかるトルクが求まれば、シャフト7の剛性に基づいて、シャフト7の捩れ角度を求めることができる。
これにより、ロボット2の手先にセンサを設けなくても、ロボット2の手先の振動の要因となるシャフト支持部6aでの捩れ振動を求めることができる。
From the detection result of the sensor 8 provided on the first arm 5, the torque applied to the first arm support portion can be obtained. Further, since each arm of the robot 2 is linked, the robot 2 is provided on the detection result of the sensor 8 provided on the first arm 5 and the second arm 6 which can move relative to the first arm 5. From the detection result of the sensor 9, the value when viewed only by the second arm 6 can be obtained, and the torque applied to the second arm support portion can be obtained from the obtained value. Similarly, the torque applied to the shaft support portion 6a can be obtained from the detection results of the sensors 8 to 10.
Then, if the torque applied to the shaft support portion 6a is obtained, the twist angle of the shaft 7 can be obtained based on the rigidity of the shaft 7.
As a result, the torsional vibration at the shaft support portion 6a, which causes the vibration of the hand of the robot 2, can be obtained without providing the sensor at the hand of the robot 2.

このとき、センサ8、9、10は、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。また、有線式であることから、電源供給等の問題が生じず、また、無線式のようにノイズにより通信が途切れることもないため、信頼性を向上させることができる。 At this time, since the sensors 8, 9, and 10 are provided on each arm, the wiring structure thereof can be simplified as compared with the case where the sensors 8, 9, and 10 are provided on the hand. Further, since it is a wired type, problems such as power supply do not occur, and communication is not interrupted due to noise unlike a wireless type, so that reliability can be improved.

より詳細には、制御装置3は、各アームに設けられているセンサ8、9、10の検出結果から各アームにおける慣性力を求め、求めた慣性力から各アームを支持するそれぞれのアーム支持部でのトルクを求める。そして、制御装置3は、求めた各アーム支持部でのトルクからシャフト支持部6aにかかるトルクを推定し、推定したトルクに基づいてシャフト支持部6aでの捩れ角度を求めている。
複数のセンサ8、9、10は、慣性センサ、回転角度センサおよびシャフト7の上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。
More specifically, the control device 3 obtains the inertial force in each arm from the detection results of the sensors 8, 9 and 10 provided on each arm, and each arm support portion that supports each arm from the obtained inertial force. Find the torque at. Then, the control device 3 estimates the torque applied to the shaft support portion 6a from the obtained torque at each arm support portion, and obtains the twist angle at the shaft support portion 6a based on the estimated torque.
The plurality of sensors 8, 9, and 10 are composed of any combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft 7.

例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフト7の上下位置を直接的に検出できる。 For example, the velocity and acceleration can be obtained by calculation if either one is detected. However, in that case, the calculation time is required. In other words, if the calculation is unnecessary, the processing speed can be improved by that amount. Then, the inertial sensor can directly detect the inertial force, the rotation angle sensor can directly detect the rotation angle, and the position sensor can directly detect the vertical position of the shaft 7.

このように、シャフト支持部6aにかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
より詳細には、慣性センサとして角速度を検出する角速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。また、位置センサとしてシャフト7を駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、シャフト7の上下位置を取得することができる。
また、各センサ8、9の配置は基本的には任意に設定することができるため、配線が過度に複雑化することを抑制できる。
As described above, the processing speed can be improved by using the sensor capable of directly detecting various physical quantities required for estimating the torque applied to the shaft support portion 6a.
More specifically, by using an angular velocity sensor that detects the angular velocity as the inertial sensor, it is possible to omit or simplify the calculation when obtaining the torque at each arm support portion. Further, by using a motor encoder generally provided in a motor for driving the shaft 7 as a position sensor, it is possible to acquire the vertical position of the shaft 7 without requiring an additional member.
Further, since the arrangement of the sensors 8 and 9 can be basically arbitrarily set, it is possible to prevent the wiring from becoming excessively complicated.

また、本実施形態の場合、常に振動を検出することが可能となるので、振動特性が外気温度(昼夜や季節による温度変動)や経年劣化あるいは暖機運転の状態(平常時と長期連休直後の相違)などにより変動することを考慮すると、例えば試験運転時にのみセンサを利用し、実稼働時にはセンサを利用しないような構成とは異なり、期待通りの振動抑制効果を発揮することができる。
また、ファーストアーム5およびセカンドアーム6に設けられている複数のセンサの検出結果からシャフト7を支持するシャフト支持部6aにかかるトルクを推定し、当該推定したトルクに基づいて、シャフト7の先端部7aの振動に相関するシャフト支持部6aでの捩れ角度を求める制御方法によっても、同様の効果を得ることができる。
Further, in the case of the present embodiment, since vibration can be detected at all times, the vibration characteristics are the outside air temperature (temperature fluctuation due to day / night and season), aging deterioration, or warm-up operation state (normal time and immediately after long holidays). Considering that it fluctuates due to (difference) and the like, it is possible to exhibit the expected vibration suppression effect, unlike a configuration in which the sensor is used only during test operation and the sensor is not used during actual operation.
Further, the torque applied to the shaft support portion 6a supporting the shaft 7 is estimated from the detection results of a plurality of sensors provided on the first arm 5 and the second arm 6, and the tip portion of the shaft 7 is estimated based on the estimated torque. The same effect can be obtained by a control method for obtaining the twist angle at the shaft support portion 6a that correlates with the vibration of 7a.

(第2実施形態)
以下、第2実施形態について説明する。第2実施形態の場合、センサの設置態様が第1実施形態と異なっている。
図4に示すように、本実施形態の場合、ファーストアーム5には、第1実施形態のセンサ8(角速度センサ)の代わりに、2軸の加速度センサであるセンサ11、12を設けている。なお、図示は省略するが、セカンドアーム6にも2軸の加速度センサが2つ設けられている。また、センサ11、12も含めて、各センサの配置は基本的には任意に設定することができる。
(Second Embodiment)
Hereinafter, the second embodiment will be described. In the case of the second embodiment, the installation mode of the sensor is different from that of the first embodiment.
As shown in FIG. 4, in the case of the present embodiment, the first arm 5 is provided with sensors 11 and 12, which are two-axis acceleration sensors, instead of the sensor 8 (angular velocity sensor) of the first embodiment. Although not shown, the second arm 6 is also provided with two 2-axis acceleration sensors. Further, the arrangement of each sensor including the sensors 11 and 12 can be basically set arbitrarily.

まずファーストアーム5について説明する。1軸回転中心(J1軸上の点C1)と2軸回転中心(J2軸上の点)とを通る仮想的な直線をCL1とし、CL1に沿った1軸回転中心(J1軸上の点C1)と2つのセンサ11、12のそれぞれの測定位置との間の距離をそれぞれLS11X、LS12Xとする。また、CL1からY方向への距離をそれぞれLS11Y、LS12Yとし、CL1からZ軸方向への距離をそれぞれLS11Z、LS12Zとする。また、各センサ11、12の測定軸は、Y軸とZ軸であるものとする。
このとき、各センサ11、12によって検出される加速度は、センサの数をnとすると、以下の(37)式のように表される。
First, the first arm 5 will be described. CL1 is a virtual straight line passing through the uniaxial rotation center (point C1 on the J1 axis) and the biaxial rotation center ( point on the J2 axis), and the uniaxial rotation center along CL1 (point C1 on the J1 axis). ) And the respective measurement positions of the two sensors 11 and 12, respectively, are LS11X and LS12X. Further, LS11Y the distance from C L1 in the Y direction, and LS12Y, LS11Z the distance in the Z axis direction from CL1, and LS12Z. Further, it is assumed that the measurement axes of the sensors 11 and 12 are the Y axis and the Z axis.
At this time, the acceleration detected by the sensors 11 and 12 is expressed by the following equation (37), where n is the number of sensors.

Figure 0006834124
Figure 0006834124

そして、Y軸加速度から、Rx軸角加速度とRz軸角加速度は、以下の(38)式から(41)式のように導出される。 Then, from the Y-axis acceleration, the Rx- axis angular acceleration and the Rz-axis angular acceleration are derived from the following equations (38) as equations (41).

Figure 0006834124
Figure 0006834124

同様に、Z軸加速度から、Rx軸角加速度とRy軸角加速度が、以下の(42)式から(45)式のように導出される。 Similarly, from the Z-axis acceleration, the Rx-axis angular acceleration and the Ry-axis angular acceleration are derived from the following equations (42) as in equation (45).

Figure 0006834124
Figure 0006834124

そして、求めたそれぞれの角加速度を積分することにより、角速度をそれぞれ求めることができる。なお、Rx軸角加速度はいずれの加速度からも求めることができるが、例えば平均値を利用したり、角速度および角加速度の誤差が小さくなるように補正したり、分母が大きい方の値を利用するなどにより、精度を向上させることができる。 Then, the angular velocities can be obtained by integrating the obtained angular accelerations. The Rx-axis angular acceleration can be obtained from any acceleration, but for example, the average value is used, the error of the angular velocity and the angular acceleration is corrected so as to be small, or the value having the larger denominator is used. The accuracy can be improved by such means.

次に、セカンドアーム6の場合を考える。セカンドアーム6の場合、2軸回転中心(J2軸上の点)の加速度をセンサの検出結果から差し引くことで、上記したファーストアーム5の場合と同様の演算にてX軸、Y軸、Z軸の角速度をそれぞれ求めることができる。この場合、2軸回転中心の加速度は、ファーストアーム5の角速度あるは角加速度から求めることができる。
そして、各アームにおける角速度が求まれば、(1)式に代入し、第1実施形態と同様に演算することで、シャフト支持部捩れ角、つまりは、手先が振動した場合の手先位置を取得することができる等、第1実施形態と同様の効果を得ることができる。
Next, consider the case of the second arm 6. In the case of the second arm 6, by subtracting the acceleration of the center of rotation of the two axes (point on the J2 axis) from the detection result of the sensor, the X-axis, Y-axis, and Z-axis can be calculated in the same manner as in the case of the first arm 5 described above. The angular velocities of can be obtained respectively. In this case, the acceleration at the center of biaxial rotation can be obtained from the angular velocity or the angular acceleration of the first arm 5.
Then, if the angular velocity in each arm is obtained, it is substituted into the equation (1) and calculated in the same manner as in the first embodiment to obtain the twist angle of the shaft support portion, that is, the hand position when the hand vibrates. It is possible to obtain the same effect as that of the first embodiment.

このとき、センサ位置は基本的には任意であるが、センサ位置を工夫することにより、効率的に演算を行うことができる。例えば、ファーストアーム5であれば、LS11X=LS12X、LS11Y=LS12Y、LS11Z=LS12Zのように配置するとよい。また、LS11Z=LS12Z=0とするとなおよい。セカンドアーム6も考慮すると、これらの関係は、以下の(46)式から(48)式のように表すことができる。 At this time, the sensor position is basically arbitrary, but by devising the sensor position, the calculation can be performed efficiently. For example, in the case of the first arm 5, it may be arranged as LS11X = LS12X, LS11Y = LS12Y, LS11Z = LS12Z. Further, it is even better to set LS11Z = LS12Z = 0. Considering the second arm 6, these relationships can be expressed as equations (46) to (48) below.

LSmX=LSmnX ・・・(46)
LSm=LSmnY ・・・(47)
LSmZ=LSmnZ(=0) ・・・(48)
なお、nはセンサの数であり、m=1の場合がファーストアーム5、m=2の場合がセカンドアーム6になる。つまり、n=1、m=1であれば、例えばLS11Xがセンサ11の配置(第1軸(J1)からの距離)を表し、n=2、m=1であれば、例えばLS12Yがセンサ12の配置(直線(CL1)からの距離)を表している。
LSm 1 X = LSmnX ... (46)
LSm 1 Y = L SmnY ... (47)
LSm 1 Z = LSmnZ (= 0) ... (48)
Note that n is the number of sensors, where m = 1 is the first arm 5 and m = 2 is the second arm 6. That is, if n = 1 and m = 1, for example, LS11X represents the arrangement of the sensor 11 (distance from the first axis (J1)), and if n = 2 and m = 1, for example, LS12Y is the sensor 12. Represents the arrangement (distance from the straight line (CL1)).

(第3実施形態)
以下、第3実施形態について説明する。第3実施形態では、上記した第1実施形態および第2実施形態で示したロボット制御装置において、手先に現れる振動を抑制するための手法について説明する。なお、構成については第1実施形態と共通する。また、図1から図4も参照しながら説明する。
(Third Embodiment)
Hereinafter, the third embodiment will be described. In the third embodiment, in the robot control device shown in the first embodiment and the second embodiment described above, a method for suppressing the vibration appearing at the hand will be described. The configuration is the same as that of the first embodiment . Further, it will be described with reference to FIGS. 1 to 4.

まず、上記した第1実施形態で説明した通り、水平多関節ロボットであるロボット2では、回転軸である1軸(J1)目および2軸(J2)目の動作に連動して、シャフト7を支持するシャフト支持部6aにおいて捩れ振動が発生している。そして、この捩れ振動は、1軸目および2軸目の回転軸とRz軸の回転だけでなく、Rx軸方向およびRy軸方向にも発生している。 First, as described in the first embodiment described above, in the robot 2 which is a horizontal articulated robot, the shaft 7 is moved in conjunction with the movements of the first axis (J1) and the second axis (J2) which are the rotation axes. Oite torsional vibration occurs in the support shafts supporting section 6 a. And this torsional vibration is generated not only in the rotation of the rotation axis and the Rz axis of the first axis and the second axis, but also in the Rx axis direction and the Ry axis direction.

しかし、従来の一般的なロボットの場合には、Rz軸方向に関してはシャフト7の回転軸となる4軸目に角度センサとしてモータエンコーダが取り付けられているものの、Rx軸方向およびRy軸方向にはその動作を検出できるセンサが設けられていなかった。そのため、Rx軸方向およびRy軸方向の振動に関しては、捩れ振動そのもののを観測できないため、捩れ振動の発生を検知してその捩れ振動を抑制することが困難であった。 However, in the case of a conventional general robot, although a motor encoder is attached as an angle sensor to the fourth axis which is the rotation axis of the shaft 7 in the Rz axis direction, it is in the Rx axis direction and the Ry axis direction. There was no sensor capable of detecting the operation. Therefore, with respect to the vibrations in the Rx-axis direction and the Ry-axis direction, the torsional vibration itself cannot be observed, and it is difficult to detect the occurrence of the torsional vibration and suppress the torsional vibration.

そして、ある回転軸で動作方向に受ける他の軸からの干渉力による捩れをその軸のモータ制御位置指令値の補正で抑制する手法等も提案されているが、上記したように、一般的な考え方では各軸において回転可能なのはRz軸方向のみであり、Rx軸方向あるいはRy軸方向には回転できないため、Rx軸方向およびRy軸方向の捻れを補償することはできなかった。また、それ以外にも他の軸からの影響を補正する様々な方法が提案されているが、動作方向ではない非動作方向について検討されているものはなかった。 Then, a method of suppressing the twist due to the interference force from another shaft received in the operation direction on a certain rotating shaft by correcting the motor control position command value of that shaft has been proposed, but as described above, it is general. The idea is that only the Rz axis direction can rotate in each axis, and the rotation cannot be performed in the Rx axis direction or the Ry axis direction. Therefore, the twist in the Rx axis direction and the Ry axis direction cannot be compensated. In addition, various methods for compensating for the influence from other axes have been proposed, but none have been examined for the non-operating direction other than the operating direction.

また、非動作方向も含めた各関節部の弾性を考慮して制御位置指令値を修正する方法も提案されていたものの、共振特性については何ら考慮されておらず、共振振動(非動作方向共振)の抑制には効果がなかった。また、手先先端の位置に関してはそのような方法で精度が向上すること自体は見込めるものの、Rx軸方向あるいはRy軸方向の慣性、弾性および剛性(バネ定数)から成る共振特性によって共振振動を励起してしまい、手先先端の姿勢、特にRx軸方向あるいはRy軸方向に対する姿勢が振動的になってしまうおそれがあった。この場合、一見すれば逆相に動かすことで振動抑制に効果が見込めそうに思われたものの、動作中には手先加速度による慣性力が非動作方向に直接作用し、動作終了後には手先加速度がゼロとなり慣性力を発生させなくなることから、共振振動の発生を抑制する効果も共振振動を減衰させる効果も得ることはできなかった。 In addition, although a method of correcting the control position command value in consideration of the elasticity of each joint including the non-operating direction has been proposed, no consideration is given to the resonance characteristics, and the resonance vibration (non-operating direction resonance). ) Was not effective. In addition, although it is expected that the accuracy of the position of the tip of the hand will be improved by such a method, the resonance vibration is excited by the resonance characteristic consisting of inertia, elasticity and rigidity (spring constant) in the Rx axis direction or the Ry axis direction. Therefore, there is a possibility that the posture of the tip of the hand, particularly the posture with respect to the Rx axis direction or the Ry axis direction, becomes vibrating. In this case, at first glance, it seemed that moving in the opposite phase would be effective in suppressing vibration, but the inertial force due to the hand acceleration acts directly in the non-moving direction during operation, and the hand acceleration increases after the operation ends. Since it becomes zero and no inertial force is generated, neither the effect of suppressing the generation of resonance vibration nor the effect of dampening the resonance vibration can be obtained.

さて、一般的なフィードバック制御においては、共振周波数付近ではフィードバック制御系の位相が進み、且つかつそのゲインも高いことから、十分に振動を抑制することができると見込まれる。しかし、反共振周波数付近は振動が検出できないため、フィードバック制御による振動の抑制が困難となっている。そのため、反共振周波数の付近、具体的には一般的に効果的であることが多いと考えられる反共振周波数よりも1〜2割程度低い周波数を振動抑制周波数として、ノッチフィルタなどの振動抑制フィルタを用いて振動の発生自体を抑制する制御が行われていた。 By the way, in general feedback control, since the phase of the feedback control system advances in the vicinity of the resonance frequency and its gain is high, it is expected that vibration can be sufficiently suppressed. However, since vibration cannot be detected near the antiresonance frequency, it is difficult to suppress the vibration by feedback control. Therefore, a vibration suppression filter such as a notch filter is set as a vibration suppression frequency in the vicinity of the antiresonance frequency, specifically, a frequency about 10 to 20% lower than the antiresonance frequency that is generally considered to be effective. Was used to control the vibration itself.

しかし、水平多関節ロボットにおいて、そのようなノッチフィルタを用いて振動抑制制御を行うと、動作条件によって振動抑制効果が異なるということが判明した。そこで、アームの回転方向の慣性モーメントの変化をパラメータとして振動抑制周波数を変動させたところ、振動抑制効果はあまり向上しなかった。
そのため、さらに発明者らが詳細に調査したところ、シャフト支持部6aに起因する共振振動は、アームの回転方向に直交する方向の慣性モーメントに依存していること、つまりは、シャフト7のZ軸方向における位置(以下、Z軸高さとも称する)によってその共振周波数および反共振周波数が変動することが明らかになった。そして、このことは、従来のように2次元平面上で動くシステムだけを考慮するのでは不十分であり、3次元空間上で動くシステムとして考えなければ、効果的な振動抑制効果を得るためには不十分であるということを示している。
However, it has been found that when vibration suppression control is performed using such a notch filter in a horizontal articulated robot, the vibration suppression effect differs depending on the operating conditions. Therefore, when the vibration suppression frequency was changed using the change in the moment of inertia in the rotation direction of the arm as a parameter, the vibration suppression effect was not improved so much.
Therefore, as a result of further detailed investigation by the inventors, the resonance vibration caused by the shaft support portion 6a depends on the moment of inertia in the direction orthogonal to the rotation direction of the arm, that is, the Z axis of the shaft 7. It has been clarified that the resonance frequency and the antiresonance frequency fluctuate depending on the position in the direction (hereinafter, also referred to as the Z-axis height). And, this is not enough to consider only the system that moves on the two-dimensional plane as in the past, and if it is not considered as a system that moves on the three-dimensional space, in order to obtain an effective vibration suppression effect. Indicates that is inadequate.

そこで、本実施形態では、以下に説明するようなノッチフィルタを用いることで、非動作方向振動の抑制を実現している。なお、このノッチフィルタは、制御部3aによって実行される。
まず、慣性モーメントをJとし、剛性をKとし、共振周波数をωとすると、周知のように、以下の(49)式の関係が成立する。
Therefore, in the present embodiment, the non-operating direction vibration is suppressed by using the notch filter as described below. This notch filter is executed by the control unit 3a.
First, assuming that the moment of inertia is J, the rigidity is K, and the resonance frequency is ω a , the relationship of the following equation (49) is established, as is well known.

Figure 0006834124
Figure 0006834124

ここで、慣性モーメントがシャフト支持部6aからみたシャフト7側の慣性モーメントであるとして、図5に示すように、シャフト7側の重心位置(Pg)における慣性モーメントをJc、シャフト7側の重量をM、シャフト支持部6a(Ps)から重心位置までの距離をLとすると、慣性モーメントは、以下の(50)式のように表すことができる。ここで、シャフト7側の重心位置とは、シャフト7と負荷との重心位置、具体的には、ツールが取り付けられている場合にはシャフト7とツールの重量を含んだ状態における重心位置を意味し、そのツールがワークを把持している場合にはシャフト7、ツールおよびワークの重量を含んだ状態における重心位置を意味する。
J=Jc+ML ・・・(50)
Here, assuming that the moment of inertia is the moment of inertia on the shaft 7 side as seen from the shaft support portion 6a, as shown in FIG. 5, the moment of inertia at the position of the center of gravity (Pg) on the shaft 7 side is Jc, and the weight on the shaft 7 side is set. Assuming that the distance from M and the shaft support portion 6a (Ps) to the position of the center of gravity is L, the moment of inertia can be expressed by the following equation (50). Here, the position of the center of gravity on the shaft 7 side means the position of the center of gravity between the shaft 7 and the load, specifically, when the tool is attached, the position of the center of gravity in a state including the weight of the shaft 7 and the tool. However, when the tool grips the work, it means the position of the center of gravity in a state including the weight of the shaft 7, the tool and the work.
J = Jc + ML 2 ... (50)

このとき、剛性はシャフト支持部6aの剛性であるものの、捩れ量が比較的小さいとすれば、sin(θ)=θ、cos(θ)=1と近似することができるため、シャフト7自体の剛性を等価的にシャフト支持部6aの剛性と考えることができる。そのため、剛性には、シャフト7自体の剛性を含めてもよい。具体的には、図6に示すように、シャフト支持部6aの剛性をK、シャフト7自体の剛性をK、重心位置に加わる力をfとし、その際の捩れ量をdとすると、捩れ量は、以下の(51)式のように求めることができる。 At this time, although the rigidity is that of the shaft support portion 6a, if the amount of twist is relatively small, it can be approximated as sin (θ) = θ and cos (θ) = 1, so that the shaft 7 itself The rigidity can be equivalently considered to be the rigidity of the shaft support portion 6a. Therefore, the rigidity may include the rigidity of the shaft 7 itself. Specifically, as shown in FIG. 6, stiffness K S of the shaft supporting portion 6a, the shaft 7 the rigidity of itself K B, the force applied to the barycentric position is f, when the torsion at that time is d, The amount of twist can be obtained by the following equation (51).

Figure 0006834124
Figure 0006834124

なお、捩れ量は、第1実施形態で説明した捩れ角度に基づいて求めることができる。
この(51)式から、シャフト支持部6aの剛性として、シャフト7自体の剛性を含めた全剛性を用いても、等価的に扱えることが数式の上からも明らかとなった。また、捩れ角度に相関する捩れ量が、シャフト7側の重心位置によって変化することも示された。なお、全剛性は、以下の(52)式である。
The amount of twist can be obtained based on the twist angle described in the first embodiment.
From this equation (51), it is clear from the mathematical formula that even if the total rigidity including the rigidity of the shaft 7 itself is used as the rigidity of the shaft support portion 6a, it can be treated equivalently. It was also shown that the amount of twist that correlates with the twist angle changes depending on the position of the center of gravity on the shaft 7 side. The total rigidity is given by the following equation (52).

Figure 0006834124
Figure 0006834124

また、捩れ量が小さいという条件より、シャフト支持部6aから重心位置までの距離との間には、d<<Lの関係が成立するものとして近似している。一般的な金属材料であればヤング率に対して引っ張り強度が1%以下であることから、換言すると、シャフト7自体に1%を超えるような捩れが発生した場合にはシャフト7が破断することから、上記したd<<Lの関係になるという条件は、常に成立する。
これらの式から、反共振周波数が以下の(53)式のように表されること、すなわち、Z軸高さによって反共振周波数が変化することが明らかになった。
Further, from the condition that the amount of twist is small, it is approximated that the relationship of d << L is established between the shaft support portion 6a and the position of the center of gravity. If it is a general metal material, the tensile strength is 1% or less with respect to Young's modulus. In other words, if the shaft 7 itself is twisted to exceed 1%, the shaft 7 is broken. Therefore, the above-mentioned condition of d << L is always satisfied.
From these equations, it was clarified that the antiresonance frequency is expressed as the following equation (53), that is, the antiresonance frequency changes depending on the height of the Z axis.

Figure 0006834124
Figure 0006834124

そして、Z軸高さによって反共振周波数が変化することが明らかになったことから、その変化に対応した可変周波数の振動抑制制御を行えばよいことが分かる。この場合、振動は、特に反共振周波数ωが低い領域で問題となることから、Jc<<MLとして、以下の(54)式のように近似して、シャフト支持部6aから重心位置までの距離Lに反比例する可変周波数に対して振動抑制制御を行ってもよい。 Since it has been clarified that the antiresonance frequency changes depending on the height of the Z-axis, it is understood that the vibration suppression control of the variable frequency corresponding to the change should be performed. In this case, vibration becomes a problem especially in the region where the anti-resonance frequency ω a is low. Therefore, Jc << ML 2 is approximated as shown in the following equation (54) from the shaft support portion 6 a to the position of the center of gravity. Vibration suppression control may be performed for a variable frequency that is inversely proportional to the distance L.

Figure 0006834124
Figure 0006834124

なお、従来ではアームの姿勢変化により共振周波数が変化することは考慮されているものの、動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することや、それにより共振周波数が変化することについては何ら考慮されていなかった。
また、慣性モーメントを推定してサーボゲインを自動調整する手法も提案されていたが、これは回転方向の慣性モーメントと回転運動の角加速度の関係を利用するという動作方向に対応する慣性モーメントの変化のみを考慮したものであり、非動作方向の慣性モーメントが変化することに対しては、何ら考慮されていなかった。また、Z軸高さと負荷の重量に依存して許容曲げモーメントを超えないように加速度を調整する手法も存在しているが、Z軸高さによって慣性モーメント及び共振周波数が変化することについては、何ら検討されていなかった。
In the past, it was considered that the resonance frequency changed due to the change in the posture of the arm, but only the change in the moment of inertia corresponding to the operating direction was considered, and the moment of inertia in the non-operating direction changed. No consideration was given to the change in the resonance frequency due to this.
In addition, a method of estimating the moment of inertia and automatically adjusting the servo gain has also been proposed, but this is a change in the moment of inertia corresponding to the operating direction by using the relationship between the moment of inertia in the rotational direction and the angular acceleration of the rotational motion. Only was taken into consideration, and no consideration was given to the change in the moment of inertia in the non-operating direction. There is also a method of adjusting the acceleration so as not to exceed the allowable bending moment depending on the Z-axis height and the weight of the load, but regarding the change in the moment of inertia and resonance frequency depending on the Z-axis height, Nothing was considered.

このように、上記した(53)式にて示すZ軸高さによって反共振周波数が変化することは、本願の発明者らによって新規に明らかになったものである。
そして、本実施形態では、図7に示す抑制周波数可変ノッチフィルタ処理により、非動作方向の振動を抑制している。なお、この抑制周波数可変ノッチフィルタ処理は、制御部3aによって実行されており、非動作方向の振動を抑制する制御方法の流れを示すものでもある。つまり、制御部3aは、以下に説明するように、Z軸高さを取得する処理を行うZ軸高さ取得部、重心位置までの距離を取得する処理を行う距離取得部、抑制周波数を求める処理を行う抑制周波数取得部、フィルタパラメータを更新する処理を行う更新部、およびノッチフィルタ演算を行う演算部として機能する。
As described above, it has been newly clarified by the inventors of the present application that the antiresonance frequency changes depending on the height of the Z-axis represented by the above equation (53).
Then, in the present embodiment, the vibration in the non-operating direction is suppressed by the suppression frequency variable notch filter processing shown in FIG. 7. The suppression frequency variable notch filter process is executed by the control unit 3a, and also shows the flow of the control method for suppressing vibration in the non-operating direction. That is, as described below, the control unit 3a obtains a Z-axis height acquisition unit that performs a process of acquiring the Z-axis height, a distance acquisition unit that performs a process of acquiring the distance to the center of gravity position, and a suppression frequency. It functions as a suppression frequency acquisition unit that performs processing, an update unit that performs processing to update filter parameters, and a calculation unit that performs notch filter calculation.

この抑制周波数可変ノッチフィルタ処理では、まず、モータエンコーダなどからZ軸高さを取得する(S1)。この場合、Z軸高さの誤差が大きく影響することはないため、モータエンコーダからの実測値ではなく、指令値を用いてもよい。
続いて、シャフト支持部6aから重心位置までの距離を取得する(S2)。このとき、Z軸高さがp=0でのシャフト支持部6aから重心位置までの距離をLとすると、Z軸の正方向を上昇方向とした場合、シャフト支持部6aから重心位置までの距離は、以下の(55)式となる。
L=L−p ・・・(55)
In this suppression frequency variable notch filter processing, first, the Z-axis height is acquired from a motor encoder or the like (S1). In this case, since the error of the Z-axis height does not have a great influence, the command value may be used instead of the measured value from the motor encoder.
Subsequently, the distance from the shaft support portion 6a to the position of the center of gravity is acquired (S2). At this time, assuming that the distance from the shaft support portion 6a to the center of gravity position when the Z-axis height is p Z = 0 is L 0 , when the positive direction of the Z axis is the ascending direction, from the shaft support portion 6a to the center of gravity position. The distance of is given by the following equation (55).
L = L 0 −p Z ... (55)

続いて、抑制周波数を取得する(S3)。この場合、抑制周波数は、上記した(53)式または(54)式のように求めればよい。
さて、ノッチフィルタFN(s)は、以下の(56)式のように定義される。
Subsequently, the suppression frequency is acquired (S3). In this case, the suppression frequency may be obtained as in the above equation (53) or (54).
By the way, the notch filter FN (s) is defined as the following equation (56).

Figure 0006834124
Figure 0006834124

ただし、(56)式は連続時間系での伝達関数であり、そのままでは制御演算ができない。そのため、(56)式を離散時間系に変換する。この場合、制御周期をTsとすると、離散時間系でのノッチフィルタFN(z−1)は、以下の(57)式のようになる。 However, Eq. (56) is a transfer function in a continuous time system, and control calculation cannot be performed as it is. Therefore, Eq. (56) is converted into a discrete-time system. In this case, assuming that the control period is Ts, the notch filter FN (z -1 ) in the discrete-time system is as shown in the following equation (57).

Figure 0006834124
Figure 0006834124

この(57)式に従ってフィルタパラメータを更新し(S4)、ノッチフィルタ演算を行うことで(S5)、抑制周波数可変ノッチフィルタ処理が実現される。
このように、重心位置の慣性モーメントを考慮してノッチフィルタを設計することにより、Z軸高さが変わった場合であっても、それに応じてノッチフィルタの抑制周波数が変化することから、捩れ量つまりは捩れ角度を生じさせる非動作方向への振動に対応することができ、適切にシャフト支持部6aの振動を抑制することができる。つまり、振動なくロボットを動かすことができる。
By updating the filter parameters according to the equation (57) (S4) and performing the notch filter calculation (S5), the suppression frequency variable notch filter processing is realized.
By designing the notch filter in consideration of the moment of inertia at the position of the center of gravity in this way, even if the Z-axis height changes, the suppression frequency of the notch filter changes accordingly, so the amount of twist That is, it is possible to cope with the vibration in the non-operating direction that causes the twisting angle, and it is possible to appropriately suppress the vibration of the shaft support portion 6a. That is, the robot can be moved without vibration.

以上説明したように、本実施形態によれば、次のような効果を得ることができる。
本実施形態では、シャフト7側の重心位置を求め、求めた重心位置とシャフト支持部6aとの距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部6aの捩れ角度を生じさせる振動を抑制する振動抑制制御を行っている。つまり、シャフト支持部6aの捩れ角度を生じさせる要因となる振動を抑制するために、従来のように2次元空間を対象とするだけでなく、3次元空間を対象として振動抑制制御を行っている。
As described above, according to the present embodiment, the following effects can be obtained.
In the present embodiment, the position of the center of gravity on the shaft 7 side is obtained, and the suppression frequency corresponding to the moment of inertia at the position of the center of gravity is obtained based on the distance between the obtained center of gravity position and the shaft support portion 6a. By performing the notch filter calculation, vibration suppression control is performed to suppress the vibration that causes the twist angle of the shaft support portion 6a. That is, in order to suppress the vibration that causes the twist angle of the shaft support portion 6a, the vibration suppression control is performed not only for the two-dimensional space as in the conventional case but also for the three-dimensional space. ..

そして、重心位置における慣性モーメントに対応する抑制周波数を求めることにより、3次元空間を対象とした場合における抑制すべき周波数である抑制周波数を求めることができ、抑制周波数が求まったことにより、その抑制周波数を抑制するような演算つまりは重心位置の慣性モーメントに応じたノッチフィルタ演算を行うことで、シャフト7側の重心位置が変化した場合であっても、その変化に追従させてノッチフィルタ演算を行うことができる。 Then, by obtaining the suppression frequency corresponding to the moment of inertia at the position of the center of gravity, it is possible to obtain the suppression frequency, which is the frequency to be suppressed when the three-dimensional space is targeted, and the suppression frequency is obtained. By performing a calculation that suppresses the frequency, that is, a notch filter calculation according to the moment of inertia of the center of gravity position, even if the position of the center of gravity on the shaft 7 side changes, the notch filter calculation is performed by following the change. It can be carried out.

このように、例えばアームの姿勢変化により共振周波数が変化する場合において動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することについては何ら考慮されていなかった従来技術とは異なり、本実施形態の振動抑制方法であれば、非動作方向の慣性モーメントが変化することに対応した振動抑制制御を行うことができる。すなわち、シャフト支持部6aの捩れ角度を生じさせる振動を抑制することができる。 In this way, for example, when the resonance frequency changes due to a change in the posture of the arm, only the change in the moment of inertia corresponding to the operating direction is considered, and no consideration is given to the change in the moment of inertia in the non-operating direction. Unlike the prior art, the vibration suppression method of the present embodiment can perform vibration suppression control corresponding to a change in the moment of inertia in the non-operating direction. That is, it is possible to suppress the vibration that causes the twist angle of the shaft support portion 6a.

また、シャフト7の重心位置に応じて慣性モーメントが変化するという従来技術では考慮されていなかった事象を考慮した上で振動抑制制御を行っていることから、従来技術では考慮すらされていなかったために抑制することができなかった動作方向とは異なる非動作方向振動を、抑制することができる。
このとき、ノッチフィルタ演算に用いるフィルタパラメータを、シャフト7側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、シャフト支持部6aからシャフト7側の重心位置までの距離に反比例する可変周波数に対して抑制周波数を求めることができる。
In addition, since the vibration suppression control is performed after considering the phenomenon that the moment of inertia changes according to the position of the center of gravity of the shaft 7, which was not considered in the prior art, it was not even considered in the prior art. Non-operating direction vibration different from the operating direction that could not be suppressed can be suppressed.
At this time, by making the filter parameter used for the notch filter calculation variable by following the change in the antiresonance frequency that changes according to the position of the center of gravity on the shaft 7 side, from the shaft support portion 6a to the position of the center of gravity on the shaft 7 side. The suppression frequency can be obtained for the variable frequency that is inversely proportional to the distance of.

また、上記したノッチフィルタは制御系のどこに挿入しても効果を発揮することができるものの、フィードバック制御系の内部に挿入した場合、抑制周波数が低くなるとフィードバック制御系の不安定化を招くおそれがある。そのため、位置指令値に対してノッチフィルタ演算を行うなど、フィードフォワード制御系として実施するとより効果的である。 In addition, although the above-mentioned notch filter can be effective no matter where it is inserted in the control system, if it is inserted inside the feedback control system, the feedback control system may become unstable if the suppression frequency becomes low. is there. Therefore, it is more effective to carry out as a feedforward control system, such as performing a notch filter calculation on the position command value.

また、シャフト7側の重心位置を求め、求めた重心位置とシャフト支持部6aとの距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部6aの捩れ角度を生じさせる振動を抑制する振動抑制制御を行う制御方法によっても、上記した効果を同様に得ることができる。 Further, the position of the center of gravity on the shaft 7 side is obtained, the suppression frequency corresponding to the moment of inertia at the position of the center of gravity is obtained based on the distance between the obtained center of gravity position and the shaft support portion 6a, and the notch filter calculation is performed on the obtained suppression frequency. The above-mentioned effect can be similarly obtained by the control method of performing the vibration suppression control that suppresses the vibration that causes the twist angle of the shaft support portion 6a.

(その他の実施形態)
本発明は、上記し且つ図面に記載した態様に限定されるものではなく、その要旨を逸脱しない範囲で種々の変形や拡張をすることができる。
実施形態で示した数値は一例であり、これに限定されない。
ファーストアーム5とセカンドアーム6のそれぞれに複数方向への角速度を検出可能な2軸角速度センサを設けたが、それ以外のセンサ設置方法でもよく、例えば加速度センサと角速度センサとを組み合わせてもよい。例えば、配線距離が長くなるセカンドアーム6には省配線化のために角速度センサを設け、ファーストアーム5には第2実施形態のように2つの加速度センサを設けることなどが考えられる。
(Other embodiments)
The present invention is not limited to the embodiments described above and shown in the drawings, and various modifications and extensions can be made without departing from the gist thereof.
The numerical values shown in the embodiments are examples, and the present invention is not limited thereto.
Although the first arm 5 and the second arm 6 are each provided with a biaxial angular velocity sensor capable of detecting angular velocities in a plurality of directions, other sensor installation methods may be used, for example, an acceleration sensor and an angular velocity sensor may be combined. For example, it is conceivable that the second arm 6 having a long wiring distance is provided with an angular velocity sensor for wiring saving, and the first arm 5 is provided with two acceleration sensors as in the second embodiment.

図面中、2はロボット、3は制御装置(ロボット制御装置)、3aは制御部(Z軸高さ取得部、距離取得部、抑制周波数取得部、更新部、演算部)、4はベース、5はファーストアーム、6はセカンドアーム、6aはシャフト支持部、7はシャフト、8、9、10、11、12はセンサ(慣性センサ、角速度センサ、加速度センサ、回転角度センサ、位置センサ)を示す。 In the drawing, 2 is a robot, 3 is a control device (robot control device), 3a is a control unit (Z-axis height acquisition unit, distance acquisition unit, suppression frequency acquisition unit, update unit, calculation unit), 4 is a base, and 5 Is the first arm, 6 is the second arm, 6a is the shaft support, 7 is the shaft, and 8, 9, 10, 11 and 12 are sensors (inertial sensor, angular velocity sensor, acceleration sensor, rotation angle sensor, position sensor).

Claims (11)

ベースと、前記ベースに取り付けられ、当該ベースに対して第1軸を中心として回転するファーストアームと、前記ファーストアームに取り付けられ、当該ファーストアームに対して第2軸を中心として回転するセカンドアームと、前記セカンドアームに取り付けられ、当該セカンドアームに対して上下移動および回転するシャフトと、を有する水平多関節型のロボットを制御するロボット制御装置であって、
前記ファーストアームおよび前記セカンドアームに設けられている複数のセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクから前記シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクと前記シャフトの剛性とに基づいて、前記シャフトの先端部の振動に相関する前記シャフト支持部での捩れ角度を求める処理を行う制御部を備え
前記制御部は、前記シャフト側の重心位置を求め、求めた重心位置と前記シャフト支持部との間の距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、前記シャフト支持部の捩れ角度を生じさせる振動を抑制する振動抑制制御を行うロボット制御装置。
A base, a first arm attached to the base and rotating about the first axis with respect to the base, and a second arm attached to the first arm and rotating about the second axis with respect to the first arm. A robot control device that controls a horizontal articulated robot attached to the second arm and having a shaft that moves up and down and rotates with respect to the second arm.
The inertial force and inertial torque of each arm are obtained from the detection results of a plurality of sensors provided on the first arm and the second arm, and the torque applied to the shaft support portion that supports the shaft is calculated from the obtained inertial force and inertial torque. A control unit is provided that performs a process of estimating and obtaining a twist angle at the shaft support portion that correlates with the vibration of the tip portion of the shaft based on the estimated torque and the rigidity of the shaft .
The control unit obtains the position of the center of gravity on the shaft side, obtains the suppression frequency corresponding to the moment of inertia at the position of the center of gravity based on the distance between the obtained center of gravity position and the shaft support portion, and determines the suppression frequency. A robot control device that performs vibration suppression control that suppresses vibration that causes a twisting angle of the shaft support portion by performing a notch filter calculation.
前記センサは、前記セカンドアームの動作方向とは異なる非動作方向への振動を検出するものである請求項1記載のロボット制御装置。The robot control device according to claim 1, wherein the sensor detects vibration in a non-operating direction different from the operating direction of the second arm. 複数の前記センサは、慣性センサ、回転角度センサおよび前記シャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されていることを特徴とする請求項1または2記載のロボット制御装置。 The robot control device according to claim 1 or 2, wherein the plurality of the sensors are composed of any combination of an inertial sensor, a rotation angle sensor, and a position sensor that detects the vertical position of the shaft. 前記慣性センサは、角速度を検出する角速度センサ、または、加速度を検出する加速度センサであることを特徴とする請求項3記載のロボット制御装置。 The robot control device according to claim 3, wherein the inertial sensor is an angular velocity sensor that detects an angular velocity or an acceleration sensor that detects an acceleration. 前記角速度センサは、複数方向への角速度を検出する角速度センサであることを特徴とする請求項4記載のロボット制御装置。 The robot control device according to claim 4, wherein the angular velocity sensor is an angular velocity sensor that detects angular velocities in a plurality of directions. 前記加速度センサは、複数方向への加速度を検出する加速度センサであることを特徴とする請求項4または5記載のロボット制御装置。 The robot control device according to claim 4 or 5, wherein the acceleration sensor is an acceleration sensor that detects acceleration in a plurality of directions. 前記回転角度センサは、前記ファーストアームを前記ベースに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項3から6のいずれか一項記載のロボット制御装置。 The robot control device according to any one of claims 3 to 6, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the first arm with respect to the base. 前記回転角度センサは、前記セカンドアームを前記ファーストアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項3から7のいずれか一項記載のロボット制御装置。 The robot control device according to any one of claims 3 to 7, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the second arm with respect to the first arm. 前記回転角度センサは、前記シャフトを前記セカンドアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項3から8のいずれか一項記載のロボット制御装置。 The robot control device according to any one of claims 3 to 8, wherein the rotation angle sensor is a motor encoder provided in a motor that rotationally drives the shaft with respect to the second arm. 前記位置センサは、前記シャフトを上下駆動するモータに設けられているモータエンコーダであることを特徴とする請求項3から9のいずれか一項記載のロボット制御装置。 The robot control device according to any one of claims 3 to 9, wherein the position sensor is a motor encoder provided in a motor that drives the shaft up and down. 前記制御部は、前記ノッチフィルタ演算に用いるフィルタパラメータを、前記シャフト側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、前記シャフト支持部から前記シャフト側の重心位置までの距離に反比例する可変周波数に対して前記抑制周波数を求めることを特徴とする請求項1から10のいずれか一項記載のロボット制御装置。The control unit makes the filter parameters used in the notch filter calculation variable by following a change in the antiresonance frequency that changes according to the position of the center of gravity on the shaft side, so that the filter parameter from the shaft support portion to the shaft side can be changed. The robot control device according to any one of claims 1 to 10, wherein the suppression frequency is obtained for a variable frequency that is inversely proportional to the distance to the center of gravity position.
JP2015228656A 2015-09-17 2015-11-24 Robot control device Active JP6834124B2 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2015184026 2015-09-17
JP2015184026 2015-09-17

Publications (2)

Publication Number Publication Date
JP2017056543A JP2017056543A (en) 2017-03-23
JP6834124B2 true JP6834124B2 (en) 2021-02-24

Family

ID=58388829

Family Applications (3)

Application Number Title Priority Date Filing Date
JP2015228655A Active JP6834123B2 (en) 2015-09-17 2015-11-24 Robot control device
JP2015228656A Active JP6834124B2 (en) 2015-09-17 2015-11-24 Robot control device
JP2015228657A Active JP6834125B2 (en) 2015-09-17 2015-11-24 Robot control device, robot control method

Family Applications Before (1)

Application Number Title Priority Date Filing Date
JP2015228655A Active JP6834123B2 (en) 2015-09-17 2015-11-24 Robot control device

Family Applications After (1)

Application Number Title Priority Date Filing Date
JP2015228657A Active JP6834125B2 (en) 2015-09-17 2015-11-24 Robot control device, robot control method

Country Status (1)

Country Link
JP (3) JP6834123B2 (en)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6834123B2 (en) * 2015-09-17 2021-02-24 株式会社デンソーウェーブ Robot control device
JP6661676B2 (en) 2018-01-18 2020-03-11 ファナック株式会社 Robot controller
JP7143633B2 (en) 2018-05-28 2022-09-29 セイコーエプソン株式会社 ROBOT SYSTEM, CONTROL DEVICE AND CONTROL METHOD
JP7147290B2 (en) * 2018-06-26 2022-10-05 セイコーエプソン株式会社 Robots and robotic systems
JP7183601B2 (en) 2018-07-20 2022-12-06 セイコーエプソン株式会社 ROBOT SYSTEM AND ROBOT SYSTEM CONTROL METHOD
JP7358744B2 (en) 2019-02-22 2023-10-11 セイコーエプソン株式会社 Robot system, control device, and control method
JP7404627B2 (en) 2019-03-13 2023-12-26 セイコーエプソン株式会社 Robot system, control device, and control method
JP7207094B2 (en) * 2019-03-29 2023-01-18 セイコーエプソン株式会社 Horizontal articulated robot
CN114711760B (en) * 2022-04-06 2023-01-24 哈尔滨工业大学 Joint axis calculation method

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09212203A (en) * 1995-11-30 1997-08-15 Sony Corp Robot controller
GB0702599D0 (en) * 2006-05-05 2007-03-21 Omnifone Ltd Data synchronization
US9001720B2 (en) * 2011-08-31 2015-04-07 Maarten Menzo Wentink Power save with data fetch time, with end of data indication, and with more data acknowledgement
JP6095730B2 (en) * 2015-06-16 2017-03-15 日立マクセル株式会社 Content transmission apparatus and method
JP6834123B2 (en) * 2015-09-17 2021-02-24 株式会社デンソーウェーブ Robot control device

Also Published As

Publication number Publication date
JP2017056542A (en) 2017-03-23
JP6834125B2 (en) 2021-02-24
JP2017056543A (en) 2017-03-23
JP2017056544A (en) 2017-03-23
JP6834123B2 (en) 2021-02-24

Similar Documents

Publication Publication Date Title
JP6834124B2 (en) Robot control device
JP3883544B2 (en) Robot control apparatus and robot control method
JP5417161B2 (en) Robot vibration control method and robot control apparatus
TWI577515B (en) Control device and control method for robot and the robot
EP3643453A1 (en) Robot system and method for controlling robot system
JP4148189B2 (en) Control device for flexible joint robot arm
US9950427B2 (en) Robot, control apparatus, and robot system
JP2016105686A (en) Control device of motor driving device, control device of biaxial motor driving device, and control method of motor driving device
JP6898649B2 (en) Vibration control device, vibration control method, vibration control system, program and recording medium
JP2014046417A (en) Driving device
CN110662636B (en) Speed reducer angle transmission error identification system and speed reducer angle transmission error identification method
JP6137379B2 (en) Robot equipment
JP5929150B2 (en) Robot device
CN110871456A (en) Robot
JP6672636B2 (en) Sensor position determination method, robot
JP5869991B2 (en) Drive device
JP3120028B2 (en) Control method for machine having link mechanism
JP5411687B2 (en) Robot control device
JP4389980B2 (en) Control method for articulated robot
JP2011220380A (en) Actuator and robot
JP2020037172A (en) robot
JP6672637B2 (en) Sensor position determination method, robot
CN112512758B (en) Deflection amount estimating device, robot control device, and deflection amount estimating method
CN113905853A (en) Robot control method
JP6036476B2 (en) robot

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20180621

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20190529

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A132

Effective date: 20190604

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20190805

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20200121

A601 Written request for extension of time

Free format text: JAPANESE INTERMEDIATE CODE: A601

Effective date: 20200228

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20200512

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20201104

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20201216

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20210105

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20210118

R150 Certificate of patent or registration of utility model

Ref document number: 6834124

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250