JP6834123B2 - Robot control device - Google Patents
Robot control device Download PDFInfo
- Publication number
- JP6834123B2 JP6834123B2 JP2015228655A JP2015228655A JP6834123B2 JP 6834123 B2 JP6834123 B2 JP 6834123B2 JP 2015228655 A JP2015228655 A JP 2015228655A JP 2015228655 A JP2015228655 A JP 2015228655A JP 6834123 B2 JP6834123 B2 JP 6834123B2
- Authority
- JP
- Japan
- Prior art keywords
- arm
- shaft
- sensor
- vibration
- control device
- 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
Links
- 230000001133 acceleration Effects 0.000 claims description 36
- 230000005484 gravity Effects 0.000 claims description 25
- 238000001514 detection method Methods 0.000 claims description 22
- 238000000034 method Methods 0.000 claims description 19
- 230000033001 locomotion Effects 0.000 claims description 13
- 238000013461 design Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 2
- 230000001629 suppression Effects 0.000 description 19
- 238000004364 calculation method Methods 0.000 description 14
- 238000004088 simulation Methods 0.000 description 13
- 239000011159 matrix material Substances 0.000 description 11
- 239000013598 vector Substances 0.000 description 10
- 230000008859 change Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 9
- 230000007246 mechanism Effects 0.000 description 5
- 239000003638 chemical reducing agent Substances 0.000 description 4
- 230000000875 corresponding effect Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 230000035945 sensitivity Effects 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000011835 investigation Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000032683 aging Effects 0.000 description 1
- 230000002238 attenuated effect Effects 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001276 controlling effect Effects 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 239000007769 metal material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001681 protective effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Description
本発明は、ロボットを制御するロボット制御装置に関する。 The present invention relates to a robot control device that controls a robot.
ロボット制御の分野では、モデル化したロボットの動作を予めシミュレーションし、所望のシミュレーション結果、例えば所望の振動抑制効果を得られた制御内容を実際のロボットに適用することが一般的に行われている。また、ロボットの振動を抑制する振動抑制制御についても様々な検討がなされており、例えば特許文献１、２では、振動を抑制しながらロボットを動作させることができる制御装置が提案されている。
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,
しかしながら、シミュレーション上では所望の振動抑制効果が得られた制御内容を実際のロボットに適用した場合、シミュレーション通りの性能を発揮できないことがある。具体的には、シミュレーション上では発生していなかった振動が、実際のロボットにおいて発生していた。
本発明は上記事情に鑑みてなされたものであり、その目的は、シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供することにある。
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.
ロボットの制御分野では、モータとアーム間の減速機に存在するバネ要素による振動モードを考慮したいわゆる２慣性系のシミュレーションが利用されている。この振動モードは、減速機等の動力が伝達していく動力伝達機構における動作方向の剛性に起因する振動である。また、２慣性系のシミュレーションは、広く利用されており、その有効性は認知されていると考えられる。 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.
このとき、アームの剛性等の機械的数値はシミュレーション条件に当然盛り込まれており、実際のロボットは、そのシミュレーション条件を満たすような機械的数値の範囲で設計されている。それにも関わらずロボットに振動が生じるということは、従来の２慣性系で考慮されていた振動モードとは異なる振動モードが存在していると考えられる。 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.
そして、発明者らは、振動を生じさせる原因の調査を重ねた結果、実際のロボットでは、２慣性系の振動モードにおける動力伝達機構の動作方向の剛性による振動（以下、便宜的に動作方向振動と称する）以外にも、動力伝達機構の動作方向とは異なる振動（上記した非動作方向振動）が存在していることを見いだした。換言すると、動作方向とは異なる向きの非動作方向振動がシミュレーションには現れなかった振動の原因であることが発明者らによって突き止められたとともに、その非動作方向振動によってロボットの可動部側が全体的に動作方向以外にも揺れ、その結果、シャフトの支持部に捩れ振動が発生し、その捩れ振動が手先の振動として現れていたことが明らかになった。 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.
最初期の産業用のロボットは、近年のロボットと比べて、格段にアームやギアあるいは軸受け部材などが相対的に太く且つ頑丈であった反面、アームおよび可動部分の全体の重量が相対的に大きかった。２慣性系の場合、共振周波数は、慣性（つまり重量）の逆数の平方根に比例することから、動作方向振動の共振周波数は低くなる。その一方で、アームを構成する部材等は非常に高剛性に作られており、非動作方向共振が存在していたとしても、その共振周波数は高くなっていたと考えられる。 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.
そのため、複数の共振が存在している状態において支配的な共振が最初期のロボットとは入れ替わってきた、あるいは、両者の共振が近い共振周波数となって互いに影響し合うような状態になってきたと考えられる。さらに、近年のロボットの場合、最初期のロボットと比べてその動作速度が格段に高速化されており、速度の２乗に比例する遠心力の影響がより顕著に現れてきたと考えられる。 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.
例えば４軸の水平多関節型ロボットの場合であれば、シャフトを上下方向（Ｚ方向）へ直動する支持部が、例えばセカンドアームの動きに連動して振動してしまう現象が確認されている。この場合、軸間干渉による振動のようにシャフトの移動方向への振動（動作方向振動に含まれる）とは異なる振動、具体的には、セカンドアームの円周方向（セカンドアームの動作方向）やセカンドアームの直径方向（セカンドアームに加わる遠心力の方向）への振動が発生している。なお、６軸の垂直多関節型ロボット（ＰＵＭＡ（Programmable Universal Manipulation Arm）型ロボット）や、いわゆる７軸ロボット等の他の構成のロボットにおいても、これに類似する現象により、非動作方向振動が発生している。 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.
そして、産業用のロボットにおいては、複数のアームが連結してロボットを構成することが多いため、ある軸が振動した結果、別の軸が干渉して振動するといった現象が発生する。このため、振動特性自体が単純な２慣性系モデルのようにはっきりとしたものとはならならず、複数の共振振動が周波数特定に現れること、また、周波数特性に現れる共振周波数と実際の振動波形に現れる共振周波数とに僅かな相違があることがある。そのため、非動作方向振動が存在していたとしても、非動作方向振動が原因となっていることを突き止めることが困難であったと考えられる。 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.
そして、このような非動作方向振動の存在は、従来のシミュレーションの前提条件であった２慣性系モデルや干渉を含めた２慣性系モデルを拡張した例えば特許文献１でいう４慣性モデル等の制御モデルでは実際のロボットの振動特性をそもそも表現しきれていなかったことを示しており、極めて重大な技術的意義をもっている。
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
さて、非動作方向振動の存在が判明したのであれば、その非動作方向振動に起因して生じる振動を抑制するような制御を行えば、シミュレーションには現れなかった手先に現れる振動を抑制することができると考えられる。
例えば水平多関節型ロボットの場合、回転軸である１軸目および２軸目の動作に連動して、Ｚ軸直動部（シャフト）の支持部（シャフト支持部）で捩れ振動が発生していた。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸（後述する図１参照）の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生する。そのため、その振動を検出すれば、手先に現れる振動を抑制できると考えられる。
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.
しかし、従来のロボットにおいてはＲｘ軸方向およびＲｙ軸方向の振動を検出するセンサが設けられておらず、Ｒｘ軸方向およびＲｙ軸方向の振動を検出すること自体ができなかった。そのため、振動の発生を検知してその振動を抑制する制御を行うこともできなかった。つまり、従来のロボットは、非動作方向振動が考慮されていなかったため、回転方向以外の振動を検出する手段をそもそも備えておらず、また、非動作方向振動の検出自体ができていないことから、その非動作方向振動を収束させる制御を行うこともできなかった。 However, the conventional robot is not provided with a sensor for detecting the vibration in the Rx-axis direction and the Ry-axis direction, and cannot detect the vibration in the Rx-axis direction and the Ry-axis direction itself. Therefore, it was not possible to detect the occurrence of vibration and control the suppression of the vibration . This means that the conventional robot, since the non-operating direction vibration is not considered, not provided originally a means for detecting the vibration of the non-rotational direction, since no can detect itself inoperative direction vibration , 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.
そこで、請求項１に係る発明では、ロボット制御装置は、水平多関節型ロボットのファーストアームおよびセカンドアームに設けられている複数のセンサの検出結果から、シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、シャフトの先端部の振動に相関するシャフト支持部での捩れ角度を求めている。
Therefore, in the invention according to
つまり、制御装置は、ロボットの手先（シャフトの先端部）ではなく、各アームに設けられているセンサの検出結果から、シャフトに生じる捩れ振動に相関しているとともに、ロボットの手先つまりはシャフトの先端部の振動にも相関しているシャフト支持部での捩れ角度を求めている。 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.
この場合、具体的な計算方法は、シャフト支持部にかかるトルクをＮ３、シャフト支持部での捩れ角度をφ３、シャフト支持部の剛性をＫ３、複数センサの検出値をＳ１〜ＳＮとすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）
Ｎ３＝Ｎ３ｅｓｔ（Ｓ１〜ＳＮ）
ただし、φ３ｅｓｔはシャフト支持部にかかるトルクからシャフト支持部での捩れ角度を求める関数であり、Ｎ３ｅｓｔは複数センサの検出値からシャフト支持部にかかるトルクを求める関数である。以下でも同様に、末尾にｅｓｔが付帯された関数は、センサ検出値あるいは別の推定値から、目的とする値を推定する関数である。
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.
そして、シャフト支持部にかかるトルクが求まれば、シャフトの剛性に基づいてシャフトの捩れ角度を求めることができる。
この場合、具体的な計算方法は、シャフト支持部の剛性をＫ３とすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）＝Ｎ３÷Ｋ３
このように、各アームに設けられているセンサからの複数の検出結果を利用することにより、ロボットの手先にセンサを設けなくても、ロボットの手先の振動の要因となるシャフト支持部での捩れ振動を求めることができる。
また、センサは、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。
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.
より詳細には、ロボット制御装置は、各アームに設けられているセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクからシャフト支持部にかかるトルクを推定し、推定したトルクに基づいてシャフト支持部での捩れ角度を求める。
この場合、具体的な計算方法は、ファーストアームの慣性力をＦＧ１、ファーストアームの慣性トルクをＮＧ１、セカンドアームの慣性力をＦＧ２、セカンドアームの慣性トルクをＮＧ２とすると、次のように数式で記載することができる。
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)
The torque applied to the shaft support can be estimated by such a procedure and a mathematical formula.
各アームに設けられているセンサは、具体的には、請求項３に係る発明のように、慣性センサ、回転角度センサおよびシャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフトの上下位置を直接的に検出できる。
このように、シャフト支持部にかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
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
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.
さらに詳細には、請求項４に係る発明のように、慣性センサとして角速度を検出する角速度センサ、あるいは加速度を検出する加速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。
角速度センサは、請求項５に係る発明のように、複数方向への角速度を検出する角速度センサを用いることができる。
加速度センサは、請求項６に係る発明のように、複数方向への加速度を検出する加速度センサを用いることができる。
More specifically, as in the invention of
As the angular velocity sensor, as in the invention of
As the acceleration sensor, as in the invention according to
回転角度センサは、請求項７に係る発明のように、ファーストアームをベースに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項８に係る発明のように、セカンドアームをファーストアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項９に係る発明のように、シャフトをセカンドアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、位置センサは、請求項１０に係る発明のように、シャフトを上下駆動するモータに設けられているモータエンコーダを用いることができる。
このようにアームやシャフトを駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、回転角度やシャフトの上下位置を取得することができる。
As the rotation angle sensor, as in the invention of
Further, as the rotation angle sensor, as in the invention of
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
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.
請求項１１に係る発明では、シャフトの上下位置に応じて変化する当該シャフトの重心位置に対する運動方程式によって予め定義され、シャフト支持部の捩れ角度を生じさせる共振特性を含む状態空間モデルに対して、制御系を設計する。つまり、状態空間モデルは、シャフトの上下位置の変化を考慮して定義されている。
シャフトの先端部つまりはロボットの手先の振動は、シャフトが下方に位置して重心位置が下方にある場合には相対的に大きくなると考えられる一方、シャフトが上方に位置して重心位置が比較的上方にある場合には相対的に小さくなると考えられる。つまり、シャフトの先端部の振動は、シャフトの重心位置によって変化すると考えられる。そして、シャフトの重心位置は、シャフトの上下位置によって変化すると考えられる。
そのため、状態空間モデルをシャフトの重心位置に対する運動方程式によって定義することで、状態空間モデルにシャフトの重心位置の変化による影響を加味した状態で制御系を設計することができる。そして、その制御系に対して振動抑制制御を行うことで、ロボットの手先の振動を効果的に抑制することができる。
In the invention according to
The vibration at the tip of the shaft, that is, the hand of the robot, is considered to be relatively large when the shaft is located below and the center of gravity is located below, while the shaft is located above and the center of gravity is relatively located. If it is above, it is considered to be relatively small. That is, it is considered that the vibration of the tip of the shaft changes depending on the position of the center of gravity of the shaft. The position of the center of gravity of the shaft is considered to change depending on the vertical position of the shaft.
Therefore, by defining the state space model by the equation of motion with respect to the position of the center of gravity of the shaft, it is possible to design the control system in a state in which the influence of the change in the position of the center of gravity of the shaft is added to the state space model. Then, by performing vibration suppression control on the control system, it is possible to effectively suppress the vibration of the robot's hand.
この場合、請求項１２に係る発明のように状態空間モデルに対して状態フィードバックおよび状態オブザーバを構築することで、フィードバック制御により振動抑制制御を行うことができる。また、状態オブザーバを構築することで、直接的には物理量を取得できない手先の振動を推定することができる。
また、請求項１３に係る発明のように状態方程式にシャフトの重心位置における動作方向に直交する向きへの慣性モーメントを含めることで、動作方向とは異なる非動作方向への慣性モーメントを加味した状態で振動を抑制することができ、非動作方向振動を効果的に抑制することができる。
In this case, vibration suppression control can be performed by feedback control by constructing a state feedback and a state observer for the state space model as in the invention according to
Further, as in the invention of claim 13, by including the moment of inertia in the direction orthogonal to the operating direction at the position of the center of gravity of the shaft in the state equation, a state in which the moment of inertia in the non-operating direction different from the operating direction is added. Vibration can be suppressed, and vibration in the non-operating direction can be effectively suppressed.
以下、本発明の複数の実施形態について、図面を参照しながら説明する。なお、各実施形態において実質的に共通する部位には同一の符号を付し、その詳細な説明は省略する。
（第１実施形態）
本発明の第１実施形態について図１から図３を参照しながら説明する。
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.
図１に示すように、ロボットシステム１は、ロボット２と、ロボット２を制御する制御装置３（ロボット制御装置に相当する）とを備えている。制御装置３は、制御部３ａを備えており、ロボット２を制御する。この制御部３ａは、詳細は後述するが、センサ８、９、１０（図３参照）の検出結果にもとづいてシャフト７を支持するシャフト支持部６ａ（図３参照）にかかるトルクを推定し、推定したトルクに基づいて当該シャフト支持部６ａでの捩れ角度を求めることを含む実施形態で説明する種々の処理を実行する。このシャフト支持部６ａでの捩れ角度は、捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関している。
As shown in FIG. 1, the
本実施形態のロボット２は、ベース４と、ベース４に取り付けられ（ベース４に支持され）、当該ベース４に対して第１軸（Ｊ１）を中心として回転するファーストアーム５と、ファーストアーム５に取り付けられ（ファーストアーム５に支持され）、当該ファーストアーム５に対して第２軸（Ｊ２）を中心として回転するセカンドアーム６と、セカンドアーム６に取り付けられ（セカンドアーム６に支持され）、当該セカンドアーム６に対して上下移動および回転するシャフト７と、を有する水平多関節型ロボットを想定している。
The
シャフト７は、本実施形態では、ボールねじスプラインを想定している。この場合、ロボット２の手先とは、シャフト７の下端側、つまり、セカンドアーム６に対して上下方向に相対的に移動するとともに、セカンドアーム６に対して相対的に回転する部分に相当する。なお、シャフト７は、非常に剛性が高く設計されており、シャフト７そのものが撓んだり変形したりすることは考えにくい。ただし、シャフト７の剛性が低く、シャフト７そのものが撓んだり変形したりしている場合であっても、その撓みや変形がシャフト支持部６ａで起きているものとして等価変換することにより、同様の効果を得ることが出来る。
In this embodiment, the
以下、図１に示すように、シャフト支持部６ａを原点とし、シャフト７と同軸となる天地方向をＺ軸（Ｚ方向）、Ｚ軸に直交し、セカンドアーム６に沿った方向をＸ軸（Ｘ方向）、これらＺ軸およびＸ軸に直交する方向をＹ軸（Ｙ方向）として説明する。また、図１に示すＺ軸回りの回転をＲｚ回転と称し、Ｘ軸回りの回転をＲｘ回転と称し、Ｙ軸回りの回転をＲｙ回転と称する。
Hereinafter, as shown in FIG. 1, the
この場合、Ｒｘ回転とは、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＸ軸回りに回転する状態、より平易に言えば、セカンドアーム６が捩れた状態に相当する。また、Ｒｙ回転とは、セカンドアーム６を例にすると、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＹ軸回りに回転する状態、より平易に言えば、セカンドアーム６の先端側がＺ方向に揺れる状態に相当する。
In this case, the Rx rotation corresponds to a state in which the
このロボット２は、図３に示すように、複数のセンサ８、９、１０を備えている。このうち、センサ８、９は、ファーストアーム５およびセカンドアーム６に設けられ、各アームの慣性を検出する慣性センサであり、センサ１０は、セカンドアーム６に設けられ、シャフト７の上下位置を検出する位置センサである。各センサ８、９の配置は、基本的には任意に設定することができる。
As shown in FIG. 3, the
より具体的には、本実施形態では、センサ８は、ファーストアーム５に設けられており、ファーストアーム５が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、図２に示すファーストアーム５における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。また、センサ９は、セカンドアーム６に設けられ、当該セカンドアーム６が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、セカンドアーム６における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。
More specifically, in the present embodiment, the
また、センサ１０は、セカンドアーム６内のシャフト支持部６ａに設けられており、シャフト７を上下駆動するモータの回転角度を検出するモータエンコーダである。なお、図示は省略するが、これら各センサ８、９、１０は、ケーブルにて接続されている有線式のものを使用している。なお、図示は省略するが、ファーストアーム５を駆動するモータおよびセカンドアーム６を回転駆動するモータにも、モータエンコーダが設けられている。
Further, the
次に、上記した構成の作用について説明する。
問題となっているシャフト支持部６ａでの捩れ振動の振動周波数は、減速機構での回転方向の共振振動の振動周波数よりも十分に低い場合が多い。一例としては、例えばシャフト７直動部の支持部での捩れ振動の振動周波数が１０Ｈｚ程度以下であるのに対し、減速機構での回転方向の共振振動の振動周波数は、５０Ｈｚ程度である。なお、シャフト７がボールねじスプラインの場合などでは、その共振振動周波数は１００Ｈｚを超えることもある。
Next, the operation of the above configuration will be described.
The vibration frequency of the torsional vibration at the
このように振動周波数の差が大きい場合には、モータエンコーダの値をアームのＲｚ軸回転角度あるいはＺ軸位置（シャフト位置）として問題ない。なお、回転方向の共振振動が問題になる場合には、減速機の出力側にエンコーダ（シャフト７の場合にはリニアエンコーダ等）を設けたり、角速度センサや加速度センサを追加したり、状態オブザーバにより共振振動を推定したりすることで、共振振動の影響を除外することができる。 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 >>
＜＜アームに関するパラメータ＞＞ << Parameters related to the arm >>
＜＜シャフト７に関するパラメータ＞＞
<< Parameters related to
これらのパラメータのうち、以下のパラメータは、各センサ８〜１０によって検出された値、あるいは、検出された値を演算することにより求めることができる。
Of these parameters, the following parameters can be obtained by calculating the values detected by the
なお、ファーストアーム５およびセカンドアーム６の重心位置での角速度は、ファーストアーム５およびセカンドアーム６内のどの点の角速度を測定しても、重心位置での角速度と同じ値が検出される。
また、各パラメータは、以下の関係を持っている。
The angular velocity of the
In addition, each parameter has the following relationship.
さて、トルクの推定手法について述べる前に、後述する演算の簡略化について説明する。
回転振動について、その回転軸を単位ベクトルｖ、その回転量をφとする。ただし、単位ベクトルｖは、以下の（３）式である。このとき、回転行列Ｒは、ロドリゲスの回転公（Rodrigues' Rotation Formula）を用いると、以下の（４）式で表される。この、回転行列Ｒ中の行列Ｖは、単位ベクトルｖにより、以下の（５）式のように定義される。そして、単位ベクトルｖおよび回転量φは、以下の（６）式および（７）式のように求まる。
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).
ここで、例えばファーストアーム５とセカンドアーム６のアーム長の合計が１０００ｍｍであったとし、手先に現れる振動がおよそ１ｍｍ程度であったとすると、回転量φは０．００１（ｒａｄ）となる。このとき、φ＜＜１であることから、ｓｉｎφ＝φ、ｃｏｓφ＝１と近似すると、回転行列Ｒは、以下の（８）式のように表すことができる。
Here, for example, assuming that the total arm length of the
ここで、ｖが単位ベクトルであることから、回転行列Ｒの非対角成分の絶対値は、φ以下となることが分かる。このことは、ある軸の値に対して、別の軸の値が最大でφだけ誤差として重畳してしまうことを示している。
さて、計測対象の軸に対して直交する別の軸からの影響度を一般的に他軸感度と呼ぶが、上記のように近似した値を用いることで回転行列をＲｚ回転に限定したことによる他軸感度への影響は、０．１％となる。
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%.
これに対して、センサ８、９等の慣性センサ自体の他軸感度は、一般的に、数％程度（例えば４％程度）である。このため、回転行列をＲｚ回転に限定することによる誤差は、慣性センサ自体の持つ他軸感度による誤差と比べて一桁以上小さくなっている。このため、Ｒｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視しても、実用上つまりは実際にロボット２を制御する上では、大きな問題となることはない。そのため、本実施形態では、以下のように、シャフト支持部６ａにかかるトルクを求める際には、回転行列についてはＲｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視することで、演算を簡略化することができるようになる。
さて、このように演算を簡略化した場合には、各回転行列は、以下の（９）式から（１１）式のように表すことができる。
On the other hand, the other axis sensitivity of the inertial sensors such as the
By the way, when the calculation is simplified in this way, each rotation matrix can be expressed as the following equations (9) to (11).
勿論、簡略化せずに、それぞれの支持部における捩れ角度の影響を含めて、Ｒｘ回転およびＲｙ回転を考慮した回転行列を用いる場合にも、以下の手法によりトルクを推定することは可能である。
さて、ファーストアーム５およびセカンドアーム６の重心位置の加速度、慣性力、慣性トルクは、以下の（１２）式から（１７）式のようになる。
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
これらより、ファーストアーム支持部およびセカンドアーム支持部の力およびトルクは、以下の（１８）式から（２１）式のようになる。 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).
また、ファーストアーム支持部およびセカンドアーム支持部のトルクは、支持部の剛性を用いて、以下の（２２）式および（２３）式のように求まる。ただし、Ｒｚ軸方向のトルクについては、厳密に捩れ剛性を用いて求めなくても、モータトルクをギア比により各アーム支持部のＲｚ軸方向のトルクに換算して用いても良い。 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.
そして、ファーストアーム支持部のトルクから、セカンドアーム支持部の力を求める。ただし、ここで求まるセカンドアーム支持部の力は、この時点では一意に求まることはなく、例えば（２５）式のλＬ１や（２６）式のλＲ２Ｌ１のように、ファーストアーム５のリンク長ベクトル方向に水平な未知の項を含んだ状態で求まる。そして、求めたセカンドアーム支持部の力より、シャフト支持部６ａの力を以下の（２７）式のように求める。なお、以下の式のＮＦ２１、Ｆ２０は、式を置き換えるために定義したものである。
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
続いて、セカンドアーム支持部のトルクから、以下の（２８）式および（２９）式のように、シャフト支持部６ａの力を消去する。
Subsequently, the force of the
この（２８）式から、未知項は、ファーストアームリンク長を、セカンドアーム回転角度だけＲｚ回転させたベクトルとセカンドアーム６のリンク長の外積で表されることが明らかになった。これらリンク長ベクトルは、Ｘ軸方向にのみ大きな値であるものの、Ｙ軸方向あるいはＺ軸方向には小さい値となっている。
このため、セカンドアーム回転角度が０°の場合、未知項は、非常に小さな値となるため、無視しても問題ないレベルとなる。また、セカンドアーム回転角度が９０°の場合、未知項は、ほぼＲｚ軸方向のベクトルとなり、Ｒｘ軸方向あるいはＲｙ軸方向については、その影響をほぼ無視しても問題ないレベルとなっている。
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
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.
すなわち、上記した式には未知項が含まれているものの、その影響は、Ｒｚ軸方向へは大きく作用する場合があるものの、Ｒｘ軸方向あるいはＲｙ軸方向ではその影響を無視できる。これを式として表すならば、リンク長ベクトルがＸ軸成分のみに値を持つとすれば、未知項は、Ｒｚ軸方向へのみ影響を表すことから、次の（３０）式から（３２）式のように示すことができる。ただし、（３２）式における「×」は、外積を示す。 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.
次に、セカンドアーム支持部のトルクからシャフト支持部６ａのトルクを、以下の（３３）式から（３５）式のようにして求める。そして、このシャフト支持部６ａのトルクと剛性とより、シャフト支持部捩れ角が、以下の（３６）式のように求まる。ただし、シャフト支持部６ａのトルクにも上記したＲｚ軸方向へは未知項が影響しており、この未知項の影響により、シャフト支持部捩れ角は、Ｒｘ軸方向およびＲｙ軸方向の捩れ角のみが有効なものとして導出されている。
Next, the torque of the
なお、シャフト支持部捩れ角は、ファーストアーム支持部あるいはセカンドアーム支持部の場合と同様に、４軸目モータトルクをギア比により変換することでＲｚ軸方向トルクを求め、そのＲｚ軸方向トルクからシャフト支持部捩れ角を求めてもよい。
このように、本実施形態の制御装置３は、手先では無く、各アームに設けた複数のセンサの検出結果に基づいてシャフト支持部６ａのトルクを推定し、推定したシャフト支持部６ａのトルクに基づいて、シャフト支持部捩れ角、つまりは、手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めている。
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
以上説明した実施形態によれば、次のような効果を得ることができる。
ロボット制御装置（制御装置３）は、水平多関節型のロボット２のファーストアーム５およびセカンドアーム６に設けられている複数のセンサ８、９、１０の検出結果から、シャフト７を支持するシャフト支持部６ａにかかるトルク（Ｎ３）を推定し、当該推定したトルク（Ｎ３）とシャフト７の剛性とに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求めている。
According to the embodiment described above, the following effects can be obtained.
The robot control device (control device 3) supports the
つまり、制御装置３は、ロボット２の手先（シャフト７の先端部７ａ）ではなく、各アームに設けられているセンサ８、９、１０の検出結果から、シャフト７に生じる捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関しているシャフト支持部６ａでの捩れ角度を求めている。
That is, the
ファーストアーム５に設けられているセンサ８の検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボット２は各アームがリンクしていることから、ファーストアーム５に設けられているセンサ８の検出結果と、そのファーストアーム５に対して相対的に動作可能なセカンドアーム６に設けられているセンサ９の検出結果とから、セカンドアーム６のみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、センサ８〜１０の検出結果から、シャフト支持部６ａにかかるトルクを求めることができる。
そして、シャフト支持部６ａにかかるトルクが求まれば、シャフト７の剛性に基づいて、シャフト７の捩れ角度を求めることができる。
これにより、ロボット２の手先にセンサを設けなくても、ロボット２の手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めることができる。
From the detection result of the
Then, if the torque applied to the
As a result, the torsional vibration at the
このとき、センサ８、９、１０は、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。また、有線式であることから、電源供給等の問題が生じず、また、無線式のようにノイズにより通信が途切れることもないため、信頼性を向上させることができる。
At this time, since the
より詳細には、制御装置３は、各アームに設けられているセンサ８、９、１０の検出結果から各アームにおける慣性力を求め、求めた慣性力から各アームを支持するそれぞれのアーム支持部でのトルクを求める。そして、制御装置３は、求めた各アーム支持部でのトルクからシャフト支持部６ａにかかるトルクを推定し、推定したトルクに基づいてシャフト支持部６ａでの捩れ角度を求めている。
複数のセンサ８、９、１０は、慣性センサ、回転角度センサおよびシャフト７の上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。
More specifically, the
The plurality of
例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフト７の上下位置を直接的に検出できる。
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
このように、シャフト支持部６ａにかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
より詳細には、慣性センサとして角速度を検出する角速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。また、位置センサとしてシャフト７を駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、シャフト７の上下位置を取得することができる。
また、各センサ８、９の配置は基本的には任意に設定することができるため、配線が過度に複雑化することを抑制できる。
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
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
Further, since the arrangement of the
また、本実施形態の場合、常に振動を検出することが可能となるので、振動特性が外気温度（昼夜や季節による温度変動）や経年劣化あるいは暖機運転の状態（平常時と長期連休直後の相違）などにより変動することを考慮すると、例えば試験運転時にのみセンサを利用し、実稼働時にはセンサを利用しないような構成とは異なり、期待通りの振動抑制効果を発揮することができる。
また、ファーストアーム５およびセカンドアーム６に設けられている複数のセンサの検出結果からシャフト７を支持するシャフト支持部６ａにかかるトルクを推定し、当該推定したトルクに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求める制御方法によっても、同様の効果を得ることができる。
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
（第２実施形態）
以下、第２実施形態について説明する。第２実施形態の場合、センサの設置態様が第１実施形態と異なっている。
図４に示すように、本実施形態の場合、ファーストアーム５には、第１実施形態のセンサ８（角速度センサ）の代わりに、２軸の加速度センサであるセンサ１１、１２を設けている。なお、図示は省略するが、セカンドアーム６にも２軸の加速度センサが２つ設けられている。また、センサ１１、１２も含めて、各センサの配置は基本的には任意に設定することができる。
(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, the
At this time, the acceleration detected by the
そして、Ｙ軸加速度から、Ｒｘ軸角加速度とＲｚ軸角加速度は、以下の（３８）式から（４１）式のように導出される。 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).
同様に、Ｚ軸加速度から、Ｒｘ軸角加速度とＲｙ軸角加速度が、以下の（４２）式から（４５）式のように導出される。 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).
そして、求めたそれぞれの角加速度を積分することにより、角速度をそれぞれ求めることができる。なお、Ｒｘ軸角加速度はいずれの加速度からも求めることができるが、例えば平均値を利用したり、角速度および角加速度の誤差が小さくなるように補正したり、分母が大きい方の値を利用するなどにより、精度を向上させることができる。 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.
次に、セカンドアーム６の場合を考える。セカンドアーム６の場合、２軸回転中心（Ｊ２軸上の点）の加速度をセンサの検出結果から差し引くことで、上記したファーストアーム５の場合と同様の演算にてＸ軸、Ｙ軸、Ｚ軸の角速度をそれぞれ求めることができる。この場合、２軸回転中心の加速度は、ファーストアーム５の角速度あるは角加速度から求めることができる。
そして、各アームにおける角速度が求まれば、（１）式に代入し、第１実施形態と同様に演算することで、シャフト支持部捩れ角、つまりは、手先が振動した場合の手先位置を取得することができる等、第１実施形態と同様の効果を得ることができる。
Next, consider the case of the
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.
このとき、センサ位置は基本的には任意であるが、センサ位置を工夫することにより、効率的に演算を行うことができる。例えば、ファーストアーム５であれば、ＬＳ１１Ｘ＝ＬＳ１２Ｘ、ＬＳ１１Ｙ＝ＬＳ１２Ｙ、ＬＳ１１Ｚ＝ＬＳ１２Ｚのように配置するとよい。また、ＬＳ１１Ｚ＝ＬＳ１２Ｚ＝０とするとなおよい。セカンドアーム６も考慮すると、これらの関係は、以下の（４６）式から（４８）式のように表すことができる。
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
ＬＳｍ１Ｘ＝ＬＳｍｎＸ ・・・（４６）
ＬＳｍ１Ｙ＝ＬＳｍｎＹ ・・・（４７）
ＬＳｍ１Ｚ＝ＬＳｍｎＺ（＝０） ・・・（４８）
なお、ｎはセンサの数であり、ｍ＝１の場合がファーストアーム５、ｍ＝２の場合がセカンドアーム６になる。つまり、ｎ＝１、ｍ＝１であれば、例えばＬＳ１１Ｘがセンサ１１の配置（第１軸（Ｊ１）からの距離）を表し、ｎ＝２、ｍ＝１であれば、例えばＬＳ１２Ｙがセンサ１２の配置（直線（ＣＬ１）からの距離）を表している。
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
（第３実施形態）
以下、第３実施形態について説明する。第３実施形態では、上記した第１実施形態および第２実施形態で示したロボット制御装置において、手先に現れる振動を抑制するための手法について説明する。なお、構成については第１実施形態と共通する。また、図１から図４も参照しながら説明する。
(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.
まず、上記した第１実施形態で説明した通り、水平多関節ロボットであるロボット２では、回転軸である１軸（Ｊ１）目および２軸（Ｊ２）目の動作に連動して、シャフト７を支持するシャフト支持部６ａにおいて捩れ振動が発生している。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生している。
First, as described in the first embodiment described above, in the
しかし、従来の一般的なロボットの場合には、Ｒｚ軸方向に関してはシャフト７の回転軸となる４軸目に角度センサとしてモータエンコーダが取り付けられているものの、Ｒｘ軸方向およびＲｙ軸方向にはその動作を検出できるセンサが設けられていなかった。そのため、Ｒｘ軸方向およびＲｙ軸方向の振動に関しては、捩れ振動そのもののを観測できないため、捩れ振動の発生を検知してその捩れ振動を抑制することが困難であった。
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
そして、ある回転軸で動作方向に受ける他の軸からの干渉力による捩れをその軸のモータ制御位置指令値の補正で抑制する手法等も提案されているが、上記したように、一般的な考え方では各軸において回転可能なのはＲｚ軸方向のみであり、Ｒｘ軸方向あるいはＲｙ軸方向には回転できないため、Ｒｘ軸方向およびＲｙ軸方向の捻れを補償することはできなかった。また、それ以外にも他の軸からの影響を補正する様々な方法が提案されているが、動作方向ではない非動作方向について検討されているものはなかった。 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.
また、非動作方向も含めた各関節部の弾性を考慮して制御位置指令値を修正する方法も提案されていたものの、共振特性については何ら考慮されておらず、共振振動（非動作方向共振）の抑制には効果がなかった。また、手先先端の位置に関してはそのような方法で精度が向上すること自体は見込めるものの、Ｒｘ軸方向あるいはＲｙ軸方向の慣性、弾性および剛性（バネ定数）から成る共振特性によって共振振動を励起してしまい、手先先端の姿勢、特にＲｘ軸方向あるいはＲｙ軸方向に対する姿勢が振動的になってしまうおそれがあった。この場合、一見すれば逆相に動かすことで振動抑制に効果が見込めそうに思われたものの、動作中には手先加速度による慣性力が非動作方向に直接作用し、動作終了後には手先加速度がゼロとなり慣性力を発生させなくなることから、共振振動の発生を抑制する効果も共振振動を減衰させる効果も得ることはできなかった。 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.
また、従来では、共振振動を減衰させるために、共振系のフィードバック制御における状態フィードバック制御などの方法により、共振特性に適切な減衰を与えて振動を速やかに収束することが行われている。また、直接的に共振特性の状態量が測定できない場合には、状態オブザーバなどの方法により推定した値を測定値の代わりに使うことが行われている。
しかし、水平多関節ロボットにおいてそのような状態フィードバックと状態オブザーバにより振動抑制制御を行うと、動作条件によって振動抑制効果が異なるということが問題となっていた。そして、検証のためにアームの回転方向の慣性モーメントに応じてフィードバックゲインを変動させてみたところ、振動抑制効果はあまり向上しないという結果が得られた。
Further, conventionally, in order to attenuate the resonance vibration, the resonance characteristic is appropriately attenuated and the vibration is quickly converged by a method such as state feedback control in the feedback control of the resonance system. Further, when the state quantity of the resonance characteristic cannot be measured directly, the value estimated by a method such as a state observer is used instead of the measured value.
However, in a horizontal articulated robot, when vibration suppression control is performed by such state feedback and a state observer, there is a problem that the vibration suppression effect differs depending on the operating conditions. Then, when the feedback gain was changed according to the moment of inertia in the rotation direction of the arm for verification, the result that the vibration suppression effect was not improved so much was obtained.
そのため、さらに発明者らが詳細に調査したところ、シャフト支持部６ａに起因する共振振動は、アームの回転方向に直交する方向の慣性モーメントに依存していること、つまりは、シャフト７のＺ軸方向における位置（以下、Ｚ軸高さと称する）によってその共振周波数および***振周波数が変動することが明らかになった。このことは、従来のように動作方向だけを考慮してトルクと角加速度から慣性モーメントを求めるのではなく、非動作方向を含めた３次元空間上で考えなければ、効果的な振動抑制効果を得るためには不十分であるということを示している。
Therefore, as a result of further detailed investigation by the inventors, the resonance vibration caused by the
そこで、本実施形態では、以下に説明するような状態フィードバック制御を用いることにより、非動作方向振動を抑制できるフィードバック制御を実現している。なお、このフィードバック制御は、制御部３ａによって実行されており、非動作方向の振動を抑制する制御方法を示すものでもある。
まず、１軸目の、アーム側からみたモータ角度をθＭ１、モータ慣性をＪＭ１、モータ粘性をＤＭ１、モータトルクをτＭ１、ギア剛性をＫＳ１、アーム角度をθＡ１、アーム慣性をＪＡ１、アーム粘性をＤＡ１とする。また、ファーストアーム支持部からシャフト支持部６ａまでの水平距離をＬＸＹ１、シャフト支持部６ａの剛性をＫＢ、シャフト支持部６ａの粘性をＤＢ、シャフト支持部６ａの捩れ角（捩れ角度に相当する）をθＢとする。また、シャフト７の重心位置での非動作方向、ここでは動作方向と直交する向きへの慣性モーメントをＪＢＣ、重量をＭＢ、シャフト支持部６ａから重心までの距離をＬＢＺとする。なお、ここでいうシャフト７の重心位置とは、シャフト７にツールやワーク等の負荷が取り付けられている場合には、それらシャフト７、ツールおよびワークを一体とみた場合における重心位置を意味している。
このとき、運動方程式は、下記の通りになる。
Therefore, in the present embodiment, feedback control capable of suppressing vibration in the non-operating direction is realized by using the state feedback control as described below. This feedback control is executed by the
First, the motor angle seen from the arm side of the first axis is θ M1 , the motor inertia is J M1 , the motor viscosity is D M1 , the motor torque is τ M1 , the gear rigidity is K S1 , the arm angle is θ A1 , and the arm inertia is Let JA1 and arm viscosity be DA1 . Further, the horizontal distance from the first arm support portion to the
At this time, the equation of motion is as follows.
また、このときの状態方程式は、下記の通りになる。 The equation of state at this time is as follows.
この式から、行列Ｅにはシャフト支持部６ａから重心までの距離ＬＢＺが含まれており、シャフト７のＺ軸高さに応じてその共振特性が変動することが明らかである。
そして、上記した運動方程式で示されるシステムに対して状態フィードバックおよび状態オブザーバを構築すると、次の通りとなる。ただし、ｗはオブザーバ状態量である。また、このシステムのブロック線図を図５に示す。
From this equation, the matrix E includes the distance L BZ from the
Then, when the state feedback and the state observer are constructed for the system represented by the above equation of motion, it becomes as follows. However, w is an observer state quantity. A block diagram of this system is shown in FIG.
このとき、フィードバックゲインＫおよびオブザーバゲインＦは、極配置法などにより設計することができる。また、各ゲインはシャフト支持部６ａから重心までの距離ＬＢＺ応じて可変となるように設計することもできる。
ところで、上記した例では剛性ＫＢはシャフト支持部６ａの剛性を用いたが、捩れ角θＢが十分に小さいとすればｓｉｎ（θ）＝θ、ｃｏｓ（θ）＝１と近似することができるため、シャフト７自体の剛性を等価的にシャフト支持部６ａの剛性と考えることができる。
At this time, the feedback gain K and the observer gain F can be designed by a pole arrangement method or the like. Further, each gain can be designed to be variable according to the distance LBZ from the
By the way , in the above example, the rigidity KB uses the rigidity of the
そのため、剛性ＫＢには、シャフト７自体の剛性を含めてもよい。この場合、シャフト支持部６ａの剛性をＫＢ０、シャフト７自体の剛性をＫＢ１、重心にかかる力をｆとすると、捩れ角θＢは、以下の（６３）式の通りとなる。また、シャフト７自体の剛性含めた場合の剛性ＫＢは、重心にかかる力ｆが重心の位置を変化させた際の変位量を求めることで、以下の（６４）式の通りとなる。
Therefore, the stiffness K B, may include the stiffness of the
これらの式から、シャフト支持部６ａの剛性ＫＢとしてシャフト７自体の剛性を含めた剛性を用いる場合であっても等価的に扱うことができることが、数式の上からも明らかである。
このとき、一般的な金属材料であればヤング率に対して引っ張り強度が１％以下であることから、換言すると、シャフト７自体に１％を超えるような捩れが発生した場合にはシャフト７が破断することから、上記した捩れ角θＢが十分に小さいという条件は、常に成立する。
From these equations, it is clear from the mathematical formulas that even when the rigidity including the rigidity of the
At this time, 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
以上説明したように、本実施形態によれば、シャフト支持部６ａの共振特性を含めた状態空間モデルに対して制御系を設計することで、振動抑制に効果的なフィードバック制御を実施することができる。
具体的には、シャフト７の上下位置に応じて変化する当該シャフト７の重心位置に対する運動方程式によって予め定義され、シャフト支持部６ａの捩れ角度を生じさせる共振特性を含む状態空間モデルに対して、制御系を設計することにより、シャフト７の重心位置の変化による影響を加味した状態の状態空間モデルを定義できる。そして、その状態空間モデルに対して制御系を設計することで、つまりは、その制御系に対して振動抑制制御を行うことで、ロボット２の手先の振動を効果的に抑制することができる。
As described above, according to the present embodiment, by designing the control system for the state space model including the resonance characteristic of the
Specifically, for a state space model that is defined in advance by an equation of motion with respect to the position of the center of gravity of the
また状態空間モデルに対して状態フィードバックおよび状態オブザーバを構築しているので、フィードバック制御により振動抑制制御を行うことができるとともに、状態オブザーバにより、直接的には物理量を取得できない手先の振動を推定しながら振動抑制制御を行うことができる。
また、状態方程式にシャフト７の重心位置における動作方向に直交する向きへの慣性モーメントを含めることで、動作方向とは異なる非動作方向への慣性モーメントを加味した状態で振動を抑制することができ、非動作方向振動を効果的に抑制することができる。
In addition, since the state feedback and the state observer are constructed for the state space model, the vibration suppression control can be performed by the feedback control, and the vibration of the hand whose physical quantity cannot be directly obtained can be estimated by the state observer. However, vibration suppression control can be performed.
Further, by including the moment of inertia in the direction orthogonal to the operating direction at the position of the center of gravity of the
このように、例えばアームの姿勢変化により共振周波数が変化する場合において動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することについては何ら考慮されていなかった従来技術とは異なり、本実施形態の振動抑制方法であれば、非動作方向の慣性モーメントが変化することに対応した振動抑制制御を行うことができる。
また、シャフト７のＺ軸高さによって慣性モーメントが変化するという従来では考慮されていなかった事実に基づいて振動抑制制御を行うので、振動抑制効果をより一層高めることができる。
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.
Further, since the vibration suppression control is performed based on the fact that the moment of inertia changes depending on the Z-axis height of the
（その他の実施形態）
本発明は、上記し且つ図面に記載した態様に限定されるものではなく、その要旨を逸脱しない範囲で種々の変形や拡張をすることができる。
実施形態で示した数値は一例であり、これに限定されない。
ファーストアーム５とセカンドアーム６のそれぞれに複数方向への角速度を検出可能な２軸角速度センサを設けたが、それ以外のセンサ設置方法でもよく、例えば加速度センサと角速度センサとを組み合わせてもよい。例えば、配線距離が長くなるセカンドアーム６には省配線化のために角速度センサを設け、ファーストアーム５には第２実施形態のように２つの加速度センサを設けることなどが考えられる。
(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
図面中、２はロボット、３は制御装置（ロボット制御装置）、３ａは制御部、４はベース、５はファーストアーム、６はセカンドアーム、６ａはシャフト支持部、７はシャフト、８、９、１０、１１、１２はセンサ（慣性センサ、角速度センサ、加速度センサ、回転角度センサ、位置センサ）を示す。
In the drawing, 2 is a robot, 3 is a control device (robot control device), 3a is a control unit, 4 is a base, 5 is a first arm, 6 is a second arm, 6a is a shaft support portion, 7 is a shaft, 8, 9,
Claims (13)
前記ファーストアームおよび前記セカンドアームに設けられている複数のセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクから前記シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクと前記シャフトの剛性とに基づいて、前記シャフトの先端部の振動に相関する前記シャフト支持部での捩れ角度を求める処理を行う制御部を備えるロボット制御装置。 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 robot control device including a control unit that performs a process of estimating and obtaining a twist angle at the shaft support portion that correlates with vibration of the tip portion of the shaft based on the estimated torque and the rigidity of the shaft.
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 |
|---|---|
| JP2017056542A JP2017056542A (en) | 2017-03-23 |
| JP6834123B2 true JP6834123B2 (en) | 2021-02-24 |
Family
ID=58388829
Family Applications (3)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2015228656A Active JP6834124B2 (en) | 2015-09-17 | 2015-11-24 | Robot control device |
| JP2015228655A Active JP6834123B2 (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 |
|---|---|---|---|
| JP2015228656A Active JP6834124B2 (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) | JP6834124B2 (en) |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6834124B2 (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)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH09212203A (en) * | 1995-11-30 | 1997-08-15 | Sony Corp | Robot controller |
| GB0702596D0 (en) * | 2006-05-05 | 2007-03-21 | Omnifone Ltd | Big book one |
| 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 |
| JP6834124B2 (en) * | 2015-09-17 | 2021-02-24 | 株式会社デンソーウェーブ | Robot control device |
-
2015
- 2015-11-24 JP JP2015228656A patent/JP6834124B2/en active Active
- 2015-11-24 JP JP2015228655A patent/JP6834123B2/en active Active
- 2015-11-24 JP JP2015228657A patent/JP6834125B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP6834124B2 (en) | 2021-02-24 |
| JP6834125B2 (en) | 2021-02-24 |
| JP2017056544A (en) | 2017-03-23 |
| JP2017056542A (en) | 2017-03-23 |
| JP2017056543A (en) | 2017-03-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6834123B2 (en) | Robot control device | |
| JP3883544B2 (en) | Robot control apparatus and robot control method | |
| JP5417161B2 (en) | Robot vibration control method and robot control apparatus | |
| EP3643453A1 (en) | Robot system and method for controlling robot system | |
| JP6664138B2 (en) | Control method and robot device | |
| JP5919142B2 (en) | Drive device | |
| JP6575200B2 (en) | Robot, control device and robot system | |
| JP4148189B2 (en) | Control device for flexible joint robot arm | |
| US20150120049A1 (en) | Robot, control device, and robot system | |
| JP2020101541A (en) | Method and device for torque estimation | |
| JP6898649B2 (en) | Vibration control device, vibration control method, vibration control system, program and recording medium | |
| JP6137379B2 (en) | Robot equipment | |
| JP5929150B2 (en) | Robot device | |
| JP6672636B2 (en) | Sensor position determination method, robot | |
| JP5869991B2 (en) | Drive device | |
| JP5411687B2 (en) | Robot control device | |
| JP2010217110A (en) | Method for creating drive shaft assy model | |
| CN110871456A (en) | Robot | |
| JP6672637B2 (en) | Sensor position determination method, robot | |
| JP2011220380A (en) | Actuator and robot | |
| JP6645083B2 (en) | Control model acquisition method, robot controller | |
| JP2020037172A (en) | robot | |
| JP6036476B2 (en) | robot | |
| WO2024053079A1 (en) | Robot system, and diagnostic device, diagnostic method, and diagnostic program for robot | |
| JP2023155027A (en) | Modeling method of joint robot, robot control device and robot system |
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: 6834123 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |