JP2017056543A - ロボット制御装置 - Google Patents

Abstract

【課題】シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供する。【解決手段】実施形態のロボット制御装置（制御装置３）は、ファーストアーム５およびセカンドアーム６に設けられている複数のセンサ８、９、１０の検出結果からシャフト７を支持するシャフト支持部６ａにかかるトルクを推定し、推定したトルクに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求める。【選択図】図１

Description

本発明は、ロボットを制御するロボット制御装置に関する。

ロボット制御の分野では、モデル化したロボットの動作を予めシミュレーションし、所望のシミュレーション結果、例えば所望の振動抑制効果を得られた制御内容を実際のロボットに適用することが一般的に行われている。また、ロボットの振動を抑制する振動抑制制御についても様々な検討がなされており、例えば特許文献１、２では、振動を抑制しながらロボットを動作させることができる制御装置が提案されている。

特許第４０３８６５９号公報 特許第５４１１６８７号公報

しかしながら、シミュレーション上では所望の振動抑制効果が得られた制御内容を実際のロボットに適用した場合、シミュレーション通りの性能を発揮できないことがある。具体的には、シミュレーション上では発生していなかった振動が、実際のロボットにおいて発生していた。
本発明は上記事情に鑑みてなされたものであり、その目的は、シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供することにある。

ロボットの制御分野では、モータとアーム間の減速機に存在するバネ要素による振動モードを考慮したいわゆる２慣性系のシミュレーションが利用されている。この振動モードは、減速機等の動力が伝達していく動力伝達機構における動作方向の剛性に起因する振動である。また、２慣性系のシミュレーションは、広く利用されており、その有効性は認知されていると考えられる。

このとき、アームの剛性等の機械的数値はシミュレーション条件に当然盛り込まれており、実際のロボットは、そのシミュレーション条件を満たすような機械的数値の範囲で設計されている。それにも関わらずロボットに振動が生じるということは、従来の２慣性系で考慮されていた振動モードとは異なる振動モードが存在していると考えられる。

そして、発明者らは、振動を生じさせる原因の調査を重ねた結果、実際のロボットでは、２慣性系の振動モードにおける動力伝達機構の動作方向の剛性による振動（以下、便宜的に動作方向振動と称する）以外にも、動力伝達機構の動作方向とは異なる振動（上記した非動作方向振動）が存在していることを見いだした。換言すると、動作方向とは異なる向きの非動作方向振動がシミュレーションには現れなかった振動の原因であることが発明者らによって突き止められたとともに、その非動作方向振動によってロボットの可動部側が全体的に動作方向以外にも揺れ、その結果、シャフトの支持部に捩れ振動が発生し、その捩れ振動が手先の振動として現れていたことが明らかになった。

さて、もしも非動作方向振動の存在が今まで認知されていたならば、非動作方向振動に対処するための制御方法が検討されているはずである。しかし、実際には、非動作方向振動の存在を示唆するような考察や非動作方向振動を抑制するための制御方法等は検討されていない。つまり、非動作方向振動は、今まで認知されていなかったと考えられる。そのため、発明者らは、なぜ今まで非動作方向振動が考慮されていなかったのかについて考察した。

最初期の産業用のロボットは、近年のロボットと比べて、格段にアームやギアあるいは軸受け部材などが相対的に太く且つ頑丈であった反面、アームおよび可動部分の全体の重量が相対的に大きかった。２慣性系の場合、共振周波数は、慣性（つまり重量）の逆数の平方根に比例することから、動作方向振動の共振周波数は低くなる。その一方で、アームを構成する部材等は非常に高剛性に作られており、非動作方向共振が存在していたとしても、その共振周波数は高くなっていたと考えられる。

一般的に、複数の共振が存在する場合、低い共振周波数を持つ共振による影響が支配的となる場合が多い。つまり、非動作方向共振は、共振周波数が相対的に高かったことから、アームの位置応答や速度応答へ与える影響は無視できるほど小さかったと考えられる。また、発明者らの研究の結果、非動作方向振動の発生原因には例えば遠心力のように非動作方向に加わる力の存在があることが判明したが、最初期のロボットは、近年のロボットに比べて動作速度が相対的に遅かったため、遠心力による影響は無視できるほど小さかったと考えられる。

これに対して、近年のロボットでは、アームを太く頑丈にする方向から、細く軽量化する方向へとその設計が変化してきている。つまり、アームが軽量化されてきた反面、アームを構成する部材は、最初期のロボットに比べれば低剛性化している。なお、低剛性化しているとはいっても、柔軟アームと呼ばれるようなアーム自体が捻れてしまうような状態ではなく、例えばクロスローラなどの軸支持部で、その回転軸以外の方向に微少振動が発生しているということである。

そのため、複数の共振が存在している状態において支配的な共振が最初期のロボットとは入れ替わってきた、あるいは、両者の共振が近い共振周波数となって互いに影響し合うような状態になってきたと考えられる。さらに、近年のロボットの場合、最初期のロボットと比べてその動作速度が格段に高速化されており、速度の２乗に比例する遠心力の影響がより顕著に現れてきたと考えられる。

例えば４軸の水平多関節型ロボットの場合であれば、シャフトを上下方向（Ｚ方向）へ直動する支持部が、例えばセカンドアームの動きに連動して振動してしまう現象が確認されている。この場合、軸間干渉による振動のようにシャフトの移動方向への振動（動作方向振動に含まれる）とは異なる振動、具体的には、セカンドアームの円周方向（セカンドアームの動作方向）やセカンドアームの直径方向（セカンドアームに加わる遠心力の方向）への振動が発生している。なお、６軸の垂直多関節型ロボット（ＰＵＭＡ（Programmable Universal Manipulation Arm）型ロボット）や、いわゆる７軸ロボット等の他の構成のロボットにおいても、これに類似する現象により、非動作方向振動が発生している。

そして、産業用のロボットにおいては、複数のアームが連結してロボットを構成することが多いため、ある軸が振動した結果、別の軸が干渉して振動するといった現象が発生する。このため、振動特性自体が単純な２慣性系モデルのようにはっきりとしたものとはならならず、複数の共振振動が周波数特定に現れること、また、周波数特性に現れる共振周波数と実際の振動波形に現れる共振周波数とに僅かな相違があることがある。そのため、非動作方向振動が存在していたとしても、非動作方向振動が原因となっていることを突き止めることが困難であったと考えられる。

さらに、動作方向振動と非動作方向振動とでは振動周波数が異なることが多いものの、減速機として例えば波動歯車装置を用いている場合には、その剛性が入力されるトルクに応じて変化することが知られている等、モデル化誤差の原因となる要素が様々であることから、誰も非動作方向振動に想到することがなく、単に誤差として扱われていたものと考えられる。

このような事情によって、シミュレーション上で現れた振動のうち最も影響度の大きいものを動作方向振動として扱い、それ以外は他の軸からの干渉などの誤差として扱っていたことから、非動作方向振動についての検討がなされてこなかったものと考えられる。つまり、最初期のロボットでは非動作方向振動がそれほど顕著ではなく、また、近年のロボットでは非動作方向振動が誤差として扱われていたことが、非動作方向振動が認知されていなかった理由であると推測された。

そして、このような非動作方向振動の存在は、従来のシミュレーションの前提条件であった２慣性系モデルや干渉を含めた２慣性系モデルを拡張した例えば特許文献１でいう４慣性モデル等の制御モデルでは実際のロボットの振動特性をそもそも表現しきれていなかったこと示しており、極めて重大な技術的意義をもっている。

さて、非動作方向振動の存在が判明したのであれば、その非動作方向振動に起因して生じる振動を抑制するような制御を行えば、シミュレーションには現れなかった手先に現れる振動を抑制することができると考えられる。
例えば水平多関節型ロボットの場合、回転軸である１軸目および２軸目の動作に連動して、Ｚ軸直動部（シャフト）の支持部（シャフト支持部）で捩れ振動が発生していた。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸（後述する図１参照）の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生する。そのため、その振動を検出すれば、手先に現れる振動を抑制できると考えられる。

しかし、従来のロボットにおいてはＲｘ軸方向およびＲｙ軸方向の振動を検出するセンサが設けられておらず、Ｒｘ軸方向およびＲｙ軸方向の振動を検出すること自体ができ無かった。そのため、振動の発生を検知してその振動を抑制する制御を行うこともできなかった。
つまり、従来のロボットは、非動作方向振動が考慮されていなかったため、回転方向以外の振動を検出する手段をそもそも備えておらず、また、非動作方向振動の検出自体ができていないことから、その非動作方向振動を収束させる制御も行うこともできなかった。

また、ロボットの手先にセンサを設けることは、一般に想像するよりも困難である。例えば有線式のセンサをロボットの手先に設ける場合には、各アームの回転やシャフトの回転および上下動からケーブルを保護するために、いわゆるケーブルリール等の保護部材が必要となって配線構造が複雑化する。かといって、無線式のセンサを手先に設けると、ロボットの動作中にセンサへ給電あるいは充電することが容易ではないことに加えて、ノイズによって無線通信が断続するおそれがある。

そこで、請求項１に係る発明では、ロボット制御装置は、水平多関節型ロボットのファーストアームおよびセカンドアームに設けられている複数のセンサの検出結果から、シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、シャフトの先端部の振動に相関するシャフト支持部での捩れ角度を求めている。

つまり、制御装置は、ロボットの手先（シャフトの先端部）ではなく、各アームに設けられているセンサの検出結果から、シャフトに生じる捩れ振動に相関しているとともに、ロボットの手先つまりはシャフトの先端部の振動にも相関しているシャフト支持部での捩れ角度を求めている。

この場合、具体的な計算方法は、シャフト支持部にかかるトルクをＮ３、シャフト支持部での捩れ角度をφ３、シャフト支持部の剛性をＫ３、複数センサの検出値をＳ１〜ＳＮとすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）
Ｎ３＝Ｎ３ｅｓｔ（Ｓ１〜ＳＮ）
ただし、φ３ｅｓｔはシャフト支持部にかかるトルクからシャフト支持部での捩れ角度を求める関数であり、Ｎ３ｅｓｔは複数センサの検出値からシャフト支持部にかかるトルクを求める関数である。以下でも同様に、末尾にｅｓｔが付帯された関数は、センサ検出値あるいは別の推定値から、目的とする値を推定する関数である。

この場合、ファーストアームに関しては、ファーストアームに設けられているセンサの検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボットでは各アームがリンクしていることから、セカンドアームに関しては、ファーストアームに設けられているセンサの検出結果とセカンドアームに設けられているセンサの検出結果とから、セカンドアームのみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、シャフトに関しても、それぞれのセンサの検出結果から、シャフト支持部にかかるトルクを求めることができる。

そして、シャフト支持部にかかるトルクが求まれば、シャフトの剛性に基づいてシャフトの捩れ角度を求めることができる。
この場合、具体的な計算方法は、シャフト支持部の剛性をＫ３とすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）＝Ｎ３÷Ｋ３
このように、各アームに設けられているセンサからの複数の検出結果を利用することにより、ロボットの手先にセンサを設けなくても、ロボットの手先の振動の要因となるシャフト支持部での捩れ振動を求めることができる。
また、センサは、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。

より詳細には、請求項２に係る発明のように、ロボット制御装置は、各アームに設けられているセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクシャフト支持部にかかるトルクを推定し、推定したトルクに基づいてシャフト支持部での捩れ角度を求める。
この場合、具体的な計算方法は、ファーストアームの慣性力をＦＧ１、ファーストアームの慣性トルクをＮＧ１、セカンドアームの慣性力をＦＧ２、セカンドアームの慣性トルクをＮＧ２とすると、次のように数式で記載することができる。

ＦＧ１＝ＦＧ１ｅｓｔ（Ｓ１〜ＳＮ）
ＦＧ２＝ＦＧ２ｅｓｔ（Ｓ１〜ＳＮ）
ＮＧ１＝ＮＧ１ｅｓｔ（Ｓ１〜ＳＮ）
ＮＧ２＝ＮＧ２ｅｓｔ（Ｓ１〜ＳＮ）
Ｎ３＝Ｎ３ｅｓｔ（ＦＧ１，ＦＧ２，ＮＧ１，ＮＧ２）
このような手順および数式により、シャフト支持部にかかるトルクを推定することができる。

各アームに設けられているセンサは、具体的には、請求項３に係る発明のように、慣性センサ、回転角度センサおよびシャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフトの上下位置を直接的に検出できる。
このように、シャフト支持部にかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。

さらに詳細には、請求項４に係る発明のように、慣性センサとして角速度を検出する角速度センサ、あるいは加速度を検出する加速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。
角速度センサは、請求項５に係る発明のように、複数方向への角速度を検出する角速度センサを用いることができる。
加速度センサは、請求項６に係る発明のように、複数方向への加速度を検出する加速度センサを用いることができる。

回転角度センサは、請求項７に係る発明のように、ファーストアームをベースに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項８に係る発明のように、セカンドアームをファーストアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項９に係る発明のように、シャフトをセカンドアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、位置センサは、請求項１０に係る発明のように、シャフトを上下駆動するモータに設けられているモータエンコーダを用いることができる。
このようにアームやシャフトを駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、回転角度やシャフトの上下位置を取得することができる。

請求項１１に係る発明では、シャフト側の重心位置を求め、求めた重心位置とシャフト支持部との距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部の捩れ角度を生じさせる振動を抑制する振動抑制制御を行う。ここで、シャフト側の重心位置とは、シャフトにツールが取り付けられている場合にはシャフトとツールの重量を含んだ状態における重心位置を意味し、そのツールがワークを把持している場合には、シャフト、ツールおよびワークの重量を含んだ状態における重心位置を意味する。

シャフトの上下位置が変化すると、それに応じてシャフト側の重心位置が変化する。このとき、重心位置が変化すると、重心位置における慣性モーメントも変化する。つまり、シャフト支持部の捩れ角度を生じさせる要因となる慣性モーメントは、重心位置の変化に応じて変化する。このため、シャフト支持部の捩れ角度を生じさせる要因となる振動を抑制するためには、従来のように２次元空間を対象とするだけでは不十分であり、３次元空間を対象とする必要がある。

そこで、重心位置における慣性モーメントに対応する抑制周波数を求めることにより、３次元空間を対象とした場合における抑制すべき周波数である抑制周波数を求めることができる。そして、抑制周波数が求まれば、その抑制周波数を抑制するような演算、つまりは、重心位置の慣性モーメントに応じたノッチフィルタ演算を行うことにより、シャフト側の重心位置が変化した場合であっても、その変化に追従させてノッチフィルタ演算を行うことができる。つまり、シャフト支持部の捩れ角度を生じさせる振動を抑制することができる。
また、シャフトの重心位置に応じて慣性モーメントが変化するという従来技術では考慮されていなかった事象を考慮した上で振動抑制制御を行うことから、従来技術では考慮すらされていなかったために抑制することができなかった動作方向とは異なる向きに生じる非動作方向振動を、抑制することができる。

より具体的には、請求項１２に係る発明のように、ノッチフィルタ演算に用いるフィルタパラメータを、シャフト側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、シャフト支持部からシャフト側の重心位置までの距離に反比例する可変周波数に対して抑制周波数を求めることができる。

第１実施形態によるロボットおよびロボット制御装置の構成を模式的に示す図 ファーストアームに生じるＲｙ回転を模式的に示す図 センサ位置を模式的に示す図 第１実施形態におけるセンサ位置を模式的に示す図 第３実施形態における重心位置を模式的に示す図 重心位置における捩れを模式的に示す図 抑制周波数可変ノッチフィルタ処理の流れを示す図

以下、本発明の複数の実施形態について、図面を参照しながら説明する。なお、各実施形態において実質的に共通する部位には同一の符号を付し、その詳細な説明は省略する。
（第１実施形態）
本発明の第１実施形態について図１から図６を参照しながら説明する。

図１に示すように、ロボットシステム１は、ロボット２と、ロボット２を制御する制御装置３（ロボット制御装置に相当する）とを備えている。制御装置３は、制御部３ａを備えており、ロボット２を制御する。この制御部３ａは、詳細は後述するが、センサ８、９、１０（図２参照）の検出結果にもとづいてシャフト７を支持するシャフト支持部６ａ（図３参照）にかかるトルクを推定し、推定したトルクに基づいて当該シャフト支持部６ａでの捩れ角度を求めることを含む実施形態で説明する種々の処理を実行する。このシャフト支持部６ａでの捩れ角度は、捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関している。

本実施形態のロボット２は、ベース４と、ベース４に取り付けられ（ベース４に支持され）、当該ベース４に対して第１軸（Ｊ１）を中心として回転するファーストアーム５と、ファーストアーム５に取り付けられ（ファーストアーム５に支持され）、当該ファーストアーム５に対して第２軸（Ｊ２）を中心として回転するセカンドアーム６と、セカンドアーム６に取り付けられ（セカンドアーム６に支持され）、当該セカンドアーム６に対して上下移動および回転するシャフト７と、を有する水平多関節型ロボットを想定している。

シャフト７は、本実施形態では、ボールねじスプラインを想定している。この場合、ロボット２の手先とは、シャフト７の下端側、つまり、セカンドアーム６に対して上下方向に相対的に移動するとともに、セカンドアーム６に対して相対的に回転する部分に相当する。なお、シャフト７は、非常に剛性が高く設計されており、シャフト７そのものが撓んだり変形したりすることは考えにくい。ただし、シャフト７の剛性が低く、シャフト７そのものが撓んだり変形したりしている場合であっても、その撓みや変形がシャフト支持部６ａで起きているものとして等価変換することにより、同様の効果を得ることが出来る。

以下、図１に示すように、シャフト支持部６ａを原点とし、シャフト７と同軸となる天地方向をＺ軸（Ｚ方向）、Ｚ軸に直交し、セカンドアーム６に沿った方向をＸ軸（Ｘ方向）、これらＺ軸およびＸ軸に直交する方向をＹ軸（Ｙ方向）として説明する。また、図１に示すＺ軸回りの回転をＲｚ回転と称し、Ｘ軸回りの回転をＲｘ回転と称し、Ｙ軸回りの回転をＲｙ回転と称する。

この場合、Ｒｘ回転とは、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＸ軸回りに回転する状態、より平易に言えば、セカンドアーム６が捩れた状態に相当する。また、Ｒｙ回転とは、セカンドアーム６を例にすると、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＹ軸回りに回転する状態、より平易に言えば、セカンドアーム６の先端側がＺ方向に揺れる状態に相当する。

このロボット２は、図３に示すように、複数のセンサ８、９、１０を備えている。このうち、センサ８、９は、ファーストアーム５およびセカンドアーム６に設けられ、各アームの慣性を検出する慣性センサであり、センサ１０は、セカンドアーム６に設けられ、シャフト７の上下位置を検出する位置センサである。各センサ８、９の配置は、基本的には任意に設定することができる。

より具体的には、本実施形態では、センサ８は、ファーストアーム５に設けられており、ファーストアーム５が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、ファーストアーム５における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。また、センサ９は、セカンドアーム６に設けられ、当該セカンドアーム６が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、セカンドアーム６における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。

また、センサ１０は、セカンドアーム６内のシャフト支持部６ａに設けられており、シャフト７を上下駆動するモータの回転角度を検出するモータエンコーダである。なお、図示は省略するが、これら各センサ８、９、１０は、ケーブルにて接続されている有線式のものを使用している。なお、図示は省略するが、ファーストアーム５を駆動するモータおよびセカンドアーム６を回転駆動するモータにも、モータエンコーダが設けられている。

次に、上記した構成の作用について説明する。
問題となっているシャフト支持部６ａでの捩れ振動の振動周波数は、減速機構での回転方向の共振振動の振動周波数よりも十分に低い場合が多い。一例としては、例えばシャフト７直動部の支持部での捩れ振動の振動周波数が１０Ｈｚ程度以下であるのに対し、減速機構での回転方向の共振振動の振動周波数は、５０Ｈｚ程度である。なお、シャフト７がボールねじスプラインの場合などでは、その共振振動周波数は１００Ｈｚを超えることもある。

このように振動周波数の差が大きい場合には、モータエンコーダの値をアームのＲｚ軸回転角度あるいはＺ軸位置（シャフト位置）として問題ない。なお、回転方向の共振振動が問題になる場合には、減速機の出力側にエンコーダ（シャフト７の場合にはリニアエンコーダ等）を設けたり、角速度センサや加速度センサを追加したり、状態オブザーバにより共振振動を推定したりすることで、共振振動の影響を除外することができる。

そこで、まず、各アームにおける各種のパラメータや変数を、以下のように定義する。なお、以下では、各種のパラメータや変数を総称してパラメータとしている。また、使用可能文字の関係上、明細書中では、例えばファーストアーム支持部捩れ角度などについては、「φ」と示すものとする。
＜＜ファーストアーム５に関するパラメータ＞＞

＜＜セカンドアーム６に関するパラメータ＞＞

＜＜シャフト７に関するパラメータ＞＞

これらのパラメータのうち、以下のパラメータは、各センサ８〜１０によって検出された値、あるいは、検出された値を演算することにより求めることができる。

なお、ファーストアーム５およびセカンドアーム６の重心位置での角速度は、ファーストアーム５およびセカンドアーム６内のどの点の角速度を測定しても、重心位置での角速度と同じ値が検出される。
また、各パラメータは、以下の関係を持っている。

さて、トルクの推定手法について述べる前に、後述する演算の簡略化について説明する。
回転振動について、その回転軸を単位ベクトルｖ、その回転量をφとする。ただし、単位ベクトルｖは、以下の（３）式である。このとき、回転行列Ｒは、ロドリゲスの回転公（Rodrigues' Rotation Formula）を用いると、以下の（４）式で表される。この、回転行列Ｒ中の行列Ｖは、単位ベクトルｖにより、以下の（５）式のように定義される。そして、単位ベクトルｖおよび回転量φは、以下の（６）式および（７）式のように求まる。

ここで、例えばファーストアーム５とセカンドアーム６のアーム長の合計が１０００ｍｍであったとし、手先に現れる振動がおよそ１ｍｍ程度であったとすると、回転量φは０．００１（ｒａｄ）となる。このとき、φ＜＜１であることから、ｓｉｎφ＝φ、ｃｏｓφ＝１と近似すると、回転行列Ｒは、以下の（８）式のように表すことができる。

ここで、ｖが単位ベクトルであることから、回転行列Ｒの非対角成分の絶対値は、φ以下となることが分かる。このことは、ある軸の値に対して、別の軸の値が最大でφだけ誤差として重畳してしまうことを示している。
さて、計測対象の軸に対して直交する別の軸からの影響度を一般的に他軸感度と呼ぶが、上記のように近似した値を用いることで回転行列をＲｚ軸回転に限定したことによる他軸感度への影響は、０．１％となる。

これに対して、センサ８、９等の慣性センサ自体の他軸感度は、一般的に、数％程度（例えば４％程度）である。このため、回転行列をＲｚ軸回転に限定することによる誤差は、慣性センサ自体の持つ他軸感度による誤差と比べて一桁以上小さくなっている。このため、Ｒｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視しても、実用上つまりは実際にロボット２を制御する上では、大きな問題となることはない。そのため、本実施形態では、以下のように、シャフト支持部６ａにかかるトルクを求める際には、回転行列についてはＲｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視することで、演算を簡略化することができるようになる。
さて、このように演算を簡略化した場合には、各回転行列は、以下の（９）式から（１１）式のように表すことができる。

勿論、簡略化せずに、それぞれの支持部における捩れ角度の影響を含めて、Ｒｘ軸回転およびＲｙ軸回転を考慮した回転行列を用いる場合にも、以下の手法によりトルクを推定することは可能である。
さて、ファーストアーム５およびセカンドアーム６の重心位置の加速度、慣性力、慣性トルクは、以下の（１２）式から（１７）式のようになる。

これらより、ファーストアーム支持部およびセカンドアーム支持部の力およびトルクは、以下の（１８）式から（２１）式のようになる。

また、ファーストアーム支持部およびセカンドアーム支持部のトルクは、支持部の剛性を用いて、以下の（２２）式および（２３）式のように求まる。ただし、Ｒｚ軸方向のトルクについては、厳密に捩れ剛性を用いて求めなくても、モータトルクをギア比により各アーム支持部のＲｚ軸方向のトルクに換算して用いても良い。

そして、ファーストアーム支持部のトルクから、セカンドアーム支持部の力を求める。ただし、ここで求まるセカンドアーム支持部の力は、この時点では一意に求まることはなく、例えば（２５）式のλＬ１や（２６）式のλＲ２Ｌ１のように、ファーストアーム５のリンク長ベクトル方向に水平な未知の項を含んだ状態で求まる。そして、求めたセカンドアーム支持部の力より、シャフト支持部６ａの力を以下の（２７）式のように求める。なお、以下の式のＮＦ２１、Ｆ２０は、式を置き換えるために定義したものである。

続いて、セカンドアーム支持部のトルクから、以下の（２８）式および（２９）式のように、シャフト支持部６ａの力を消去する。

この（２８）式から、未知項は、ファーストアームリンク長を、セカンドアーム回転角度だけＲｚ軸回転させたベクトルとセカンドアーム６のリンク長の外積で表されることが明らかになった。これらリンク長ベクトルは、Ｘ軸方向にのみ大きな値であるものの、Ｙ軸方向あるいはＺ軸方向には小さい値となっている。
このため、セカンドアーム回転角度が０°の場合、未知項は、非常に小さな値となるため、無視しても問題ないレベルとなる。また、セカンドアーム回転角度が９０°の場合、未知項は、ほぼＲｚ軸方向のベクトルとなり、Ｒｘ軸方向あるいはＲｙ軸方向については、その影響をほぼ無視しても問題ないレベルとなっている。

すなわち、上記した式には未知項が含まれているものの、その影響は、Ｒｚ軸方向へは大きく作用する場合があるものの、Ｒｘ軸方向あるいはＲｙ軸方向ではその影響を無視できる。これを式として表すならば、リンク長ベクトルがＸ軸成分のみに値を持つとすれば、未知項は、Ｒｚ軸方向へのみ影響を表すことから、次の（３０）式から（３２）式のように示すことができる。

次に、セカンドアーム支持部のトルクからシャフト支持部６ａのトルクを、以下の（３３）式から（３５）式のようにして求める。そして、このシャフト支持部６ａのトルクと剛性とより、シャフト支持部捩れ角が、以下の（３３）式のように求まる。ただし、シャフト支持部６ａのトルクにも上記したＲｚ軸方向へは未知項が影響しており、この未知項の影響により、シャフト支持部捩れ角は、Ｒｘ軸方向およびＲｙ軸方向の捩れ角のみが有効なものとして導出されている。

なお、シャフト支持部捩れ角は、ファーストアーム支持部あるいはセカンドアーム支持部の場合と同様に、４軸目モータトルクをギア比により変換することでＲｚ軸方向トルクを求め、そのＲｚ軸方向トルクからシャフト支持部捩れ角を求めてもよい。
このように、本実施形態の制御装置３は、手先では無く、各アームに設けた複数のセンサの検出結果に基づいてシャフト支持部６ａのトルクを推定し、推定したシャフト支持部６ａのトルクに基づいて、シャフト支持部捩れ角、つまりは、手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めている。

以上説明した実施形態によれば、次のような効果を得ることができる。
ロボット制御装置（制御装置３）は、水平多関節型のロボット２のファーストアーム５およびセカンドアーム６に設けられている複数のセンサ８、９、１０の検出結果から、シャフト７を支持するシャフト支持部６ａにかかるトルク（Ｎ３）を推定し、当該推定したトルク（Ｎ３）とシャフト７の剛性とに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求めている。

つまり、制御装置３は、ロボット２の手先（シャフト７の先端部７ａ）ではなく、各アームに設けられているセンサ８、９、１０の検出結果から、シャフト７に生じる捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関しているシャフト支持部６ａでの捩れ角度を求めている。

ファーストアーム５に設けられているセンサ８の検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボット２は各アームがリンクしていることから、ファーストアーム５に設けられているセンサ８の検出結果と、そのファーストアーム５に対して相対的に動作可能なセカンドアーム６に設けられているセンサ９の検出結果とから、セカンドアーム６のみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、センサ８〜１０の検出結果から、シャフト支持部６ａにかかるトルクを求めることができる。
そして、シャフト支持部６ａにかかるトルクが求まれば、シャフト７の剛性に基づいて、シャフト７の捩れ角度を求めることができる。
これにより、ロボット２の手先にセンサを設けなくても、ロボット２の手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めることができる。

このとき、センサ８、９、１０は、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。また、有線式であることから、電源供給等の問題が生じず、また、無線式のようにノイズにより通信が途切れることもないため、信頼性を向上させることができる。

より詳細には、制御装置３は、各アームに設けられているセンサ８、９、１０の検出結果から各アームにおける慣性力を求め、求めた慣性力から各アームを支持するそれぞれのアーム支持部でのトルクを求める。そして、制御装置３は、求めた各アーム支持部でのトルクからシャフト支持部６ａにかかるトルクを推定し、推定したトルクに基づいてシャフト支持部６ａでの捩れ角度を求めている。
複数のセンサ８、９、１０は、慣性センサ、回転角度センサおよびシャフト７の上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。

例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフト７の上下位置を直接的に検出できる。

このように、シャフト支持部６ａにかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
より詳細には、慣性センサとして角速度を検出する角速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。また、位置センサとしてシャフト７を駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、シャフト７の上下位置を取得することができる。
また、各センサ８、９の配置は基本的には任意に設定することができるため、配線が過度に複雑化することを抑制できる。

また、本実施形態の場合、常に振動を検出することが可能となるので、振動特性が外気温度（昼夜や季節による温度変動）や経年劣化あるいは暖機運転の状態（平常時と長期連休直後の相違）などにより変動することを考慮すると、例えば試験運転時にのみセンサを利用し、実稼働時にはセンサを利用しないような構成とは異なり、期待通りの振動抑制効果を発揮することができる。
また、ファーストアーム５およびセカンドアーム６に設けられている複数のセンサの検出結果からシャフト７を支持するシャフト支持部６ａにかかるトルクを推定し、当該推定したトルクに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求める制御方法によっても、同様の効果を得ることができる。

（第２実施形態）
以下、第２実施形態について説明する。第２実施形態の場合、センサの設置態様が第１実施形態と異なっている。
図４に示すように、本実施形態の場合、ファーストアーム５には、第１実施形態のセンサ８（角速度センサ）の代わりに、２軸の加速度センサであるセンサ１１、１２を設けている。なお、図示は省略するが、セカンドアーム６にも２軸の加速度センサが２つ設けられている。また、センサ１１、１２も含めて、各センサの配置は基本的には任意に設定することができる。

まずファーストアーム５について説明する。１軸回転中心（Ｊ１軸上の点Ｃ１）から２つのセンサ１１、１２の測定位置までの距離をそれぞれＬＳ１１Ｘ、ＬＳ１２Ｘとする。また、Ｊ１軸とＪ２軸とを通る仮想的な直線をＣＬ１とし、ＣＬ１からＹ方向への距離をそれぞれＬＳ１１Ｙ、ＬＳ１２Ｙとし、ＣＬ１からＺ軸方向への距離をそれぞれＬＳ１１Ｚ、ＬＳ１２Ｚとする。また、各センサ１１、１２の測定軸は、Ｙ軸とＺ軸であるものとする。
このとき、各センサ１１、１２によって検出される加速度は、センサの数をｎとすると、以下の（３７）式のように表される。

そして、Ｙ軸加速度から、Ｒｙ軸角加速度とＲｚ軸角加速度は、以下の（３８）式から（４１）式のように導出される。

同様に、Ｚ軸加速度から、Ｒｘ軸角加速度とＲｙ軸角加速度が、以下の（４２）式から（４５）式のように導出される。

そして、求めたそれぞれの角加速度を積分することにより、角速度をそれぞれ求めることができる。なお、Ｒｘ軸角加速度はいずれの加速度からも求めることができるが、例えば平均値を利用したり、角速度および角加速度の誤差が小さくなるように補正したり、分母が大きい方の値を利用するなどにより、精度を向上させることができる。

次に、セカンドアーム６の場合を考える。セカンドアーム６の場合、２軸回転中心（Ｊ２軸上の点）の加速度をセンサの検出結果から差し引くことで、上記したファーストアーム５の場合と同様の演算にてＸ軸、Ｙ軸、Ｚ軸の角速度をそれぞれ求めることができる。この場合、２軸回転中心の加速度は、ファーストアーム５の角速度あるは角加速度から求めることができる。
そして、各アームにおける角速度が求まれば、（１）式に代入し、第１実施形態と同様に演算することで、シャフト支持部捩れ角、つまりは、手先が振動した場合の手先位置を取得することができる等、第１実施形態と同様の効果を得ることができる。

このとき、センサ位置は基本的には任意であるが、センサ位置を工夫することにより、効率的に演算を行うことができる。例えば、ファーストアーム５であれば、ＬＳ１１Ｘ＝ＬＳ１２Ｘ、ＬＳ１１Ｙ＝−ＬＳ１２Ｙ、ＬＳ１１Ｚ＝ＬＳ１２Ｚのように配置するとよい。また、ＬＳ１１Ｚ＝ＬＳ１２Ｚ＝０とするとなおよい。セカンドアーム６も考慮すると、これらの関係は、以下の（４６）式から（４８）式のように表すことができる。

ＬＳｍｎＸ＝ＬＳｍｎＸ ・・・（４６）
ＬＳｍｎＹ＝−ＬＳｍｎＹ ・・・（４７）
ＬＳｍｎＺ＝ＬＳｍｎＺ（＝０） ・・・（４８）
なお、ｎはセンサの数であり、ｍ＝１の場合がファーストアーム５、ｍ＝２の場合がセカンドアーム６になる。つまり、ｎ＝１、ｍ＝１であれば、例えばＬＳ１１Ｘがセンサ１０の配置（第１軸（Ｊ１）からの距離）を表し、ｎ＝２、ｍ＝１であれば、例えばＬＳ１２Ｙがセンサ１１の配置（直線（ＣＬ１）からの距離）を表している。

（第３実施形態）
以下、第３実施形態について説明する。第３実施形態では、上記した第１実施形態および第２実施形態で示したロボット制御装置において、手先に現れる振動を抑制するための手法について説明する。なお、構成については第１実施形態と共通するので、図１から図４も参照しながら説明する。

まず、上記した第１実施形態で説明した通り、水平多関節ロボットであるロボット２では、回転軸である１軸（Ｊ１）目および２軸（Ｊ２）目の動作に連動して、シャフト７（いか、Ｚ軸直動部とも称する）を支持するシャフト支持部６ａ（以下、Ｚ軸支持部とも称する）において捩れ振動が発生している。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生している。

しかし、従来の一般的なロボットの場合には、Ｒｚ軸方向に関してはシャフト７の回転軸となる４軸目に角度センサとしてモータエンコーダが取り付けられているものの、Ｒｘ軸方向およびＲｙ軸方向にはその動作を検出できるセンサが設けられていなかった。そのため、Ｒｘ軸方向およびＲｙ軸方向の振動に関しては、捩れ振動そのもののを観測できないため、捩れ振動の発生を検知してその捩れ振動を抑制することが困難であった。

そして、ある回転軸で動作方向に受ける他の軸からの干渉力による捩れをその軸のモータ制御位置指令値の補正で抑制する手法等も提案されているが、上記したように、一般的な考え方では各軸において回転可能なのはＲｚ軸方向のみであり、Ｒｘ軸方向あるいはＲｙ軸方向には回転できないため、Ｒｘ軸方向およびＲｙ軸方向の捻れを補償することはできなかった。また、それ以外にも他の軸からの影響を補正する様々な方法が提案されているが、動作方向ではない非動作方向について検討されているものはなかった。

また、非動作方向も含めた各関節部の弾性を考慮して制御位置指令値を修正する方法も提案されていたものの、共振特性については何ら考慮されておらず、共振振動（非動作方向共振）の抑制には効果がなかった。また、手先先端の位置に関してはそのような方法で精度が向上すること自体は見込めるものの、Ｒｘ軸方向あるいはＲｙ軸方向の慣性、弾性および剛性（バネ定数）から成る共振特性によって共振振動を励起してしまい、手先先端の姿勢、特にＲｘ軸方向あるいはＲｙ軸方向に対する姿勢が振動的になってしまうおそれがあった。この場合、一見すれば逆相に動かすことで振動抑制に効果が見込めそうに思われたものの、動作中には手先加速度による慣性力が非動作方向に直接作用し、動作終了後には手先加速度がゼロとなり慣性力を発生させなくなることから、共振振動の発生を抑制する効果も共振振動を減衰させる効果も得ることはできなかった。

さて、一般的なフィードバック制御においては、共振周波数付近ではフィードバック制御系の位相が進み、且つかつそのゲインも高いことから、十分に振動を抑制することができると見込まれる。しかし、反共振周波数付近は振動が検出できないため、フィードバック制御による振動の抑制が困難となっている。そのため、反共振周波数の付近、具体的には一般的に効果的であることが多いと考えられる反共振周波数よりも１〜２割程度低い周波数を振動抑制周波数として、ノッチフィルタなどの振動抑制フィルタを用いて振動の発生自体を抑制する制御が行われていた。

しかし、水平多関節ロボットにおいて、そのようなノッチフィルタを用いて振動抑制制御を行うと、動作条件によって振動抑制効果が異なるということが判明した。そこで、アームの回転方向の慣性モーメントの変化をパラメータとして振動抑制周波数を変動させたところ、振動抑制効果はあまり向上しなかった。
そのため、さらに発明者らが詳細に調査したところ、シャフト支持部６ａに起因する共振振動は、アームの回転方向に直交する方向の慣性モーメントに依存していること、つまりは、シャフト７のＺ軸方向における位置（以下、Ｚ軸高さとも称する）によってその共振周波数および反共振周波数が変動することが明らかになった。そして、このことは、従来のように２次元平面上で動くシステムだけを考慮するのでは不十分であり、３次元空間上で動くシステムとして考えなければ、効果的な振動抑制効果を得るためには不十分であるということを示している。

そこで、本実施形態では、以下に説明するようなノッチフィルタを用いることで、非動作方向振動の抑制を実現している。なお、このノッチフィルタは、制御部３ａによって実行される。
まず、慣性モーメントをＪとし、剛性をＫとし、共振周波数をω_ａとすると、周知のように、以下の（４９）式の関係が成立する。

ここで、慣性モーメントがシャフト支持部６ａからみたシャフト７側の慣性モーメントであるとして、図５に示すように、シャフト７側の重心位置（Ｐｇ）における慣性モーメントをＪｃ、シャフト７側の重量をＭ、シャフト支持部６ａ（Ｐｓ）から重心位置までの距離をＬとすると、慣性モーメントは、以下の（５０）式のように表すことができる。ここで、シャフト７側の重心位置とは、シャフト７と負荷との重心位置、具体的には、ツールが取り付けられている場合にはシャフト７とツールの重量を含んだ状態における重心位置を意味し、そのツールがワークを把持している場合にはシャフト７、ツールおよびワークの重量を含んだ状態における重心位置を意味する。
Ｊ＝Ｊｃ＋ＭＬ^２ ・・・（５０）

このとき、剛性はシャフト支持部６ａの剛性であるものの、捩れ量が比較的小さいとすれば、ｓｉｎ（θ）＝θ、ｃｏｓ（θ）＝１と近似することができるため、シャフト７自体の剛性を等価的にシャフト支持部６ａの剛性と考えることができる。そのため、剛性には、シャフト７自体の剛性を含めてもよい。具体的には、図６に示すように、シャフト支持部６ａの剛性をＫ_Ｓ、シャフト７自体の剛性をＫ_Ｂ、重心位置に加わる力をｆとし、その際の捩れ量をｄとすると、捩れ量は、以下の（５１）式のように求めることができる。

なお、捩れ量は、第１実施形態で説明した捩れ角度に基づいて求めることができる。
この（５１）式から、シャフト支持部６ａの剛性として、シャフト７自体の剛性を含めた全剛性を用いても、等価的に扱えることが数式の上からも明らかとなった。また、捩れ角度に相関する捩れ量が、シャフト７側の重心位置によって変化することも示された。なお、全剛性は、以下の（５２）式である。

また、捩れ量が小さいという条件より、シャフト支持部６ａから重心位置までの距離との間には、ｄ＜＜Ｌの関係が成立するものとして近似している。一般的な金属材料であればヤング率に対して引っ張り強度が１％以下であることから、換言すると、シャフト７自体に１％を超えるような捩れが発生した場合にはシャフト７が破断することから、上記したｄ＜＜Ｌの関係になるという条件は、常に成立する。
これらの式から、反共振周波数が以下の（５３）式のように表されること、すなわち、Ｚ軸高さによって反共振周波数が変化することが明らかになった。

そして、Ｚ軸高さによって反共振周波数が変化することが明らかになったことから、その変化に対応した可変周波数の振動抑制制御を行えばよいことが分かる。この場合、振動は、特に反共振周波数ω_ａが低い領域で問題となることから、Ｊｃ＜＜ＭＬ^２として、以下の（５４）式のように近似して、シャフト支持部６ａから重心位置までの距離Ｌに反比例する可変周波数に対して振動抑制制御を行ってもよい。

なお、従来ではアームの姿勢変化により共振周波数が変化することは考慮されているものの、動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することや、それにより共振周波数が変化することについては何ら考慮されていなかった。
また、慣性モーメントを推定してサーボゲインを自動調整する手法も提案されていたが、これは回転方向の慣性モーメントと回転運動の角加速度の関係を利用するという動作方向に対応する慣性モーメントの変化のみを考慮したものであり、非動作方向の慣性モーメントが変化することに対しては、何ら考慮されていなかった。また、Ｚ軸高さと負荷の重量に依存して許容曲げモーメントを超えないように加速度を調整する手法も存在しているが、Ｚ軸高さによって慣性モーメント及び共振周波数が変化することについては、何ら検討されていなかった。

このように、上記した（５３）式にて示すＺ軸高さによって反共振周波数が変化することは、本願の発明者らによって新規に明らかになったものである。
そして、本実施形態では、図７に示す抑制周波数可変ノッチフィルタ処理により、非動作方向の振動を抑制している。なお、この抑制周波数可変ノッチフィルタ処理は、制御部３ａによって実行されており、非動作方向の振動を抑制する制御方法の流れを示すものでもある。つまり、制御部３ａは、以下に説明するように、Ｚ軸高さを取得する処理を行うＺ軸高さ取得部、重心位置までの距離を取得する処理を行う距離取得部、抑制周波数を求める処理を行う抑制周波数取得部、フィルタパラメータを更新する処理を行う更新部、およびノッチフィルタ演算を行う演算部として機能する。

この抑制周波数可変ノッチフィルタ処理では、まず、モータエンコーダなどからＺ軸高さを取得する（Ｓ１）。この場合、Ｚ軸高さの誤差が大きく影響することはないため、モータエンコーダからの実測値ではなく、指令値を用いてもよい。
続いて、シャフト支持部６ａから重心位置までの距離を取得する（Ｓ２）。このとき、Ｚ軸高さがｐ_Ｚ＝０でのシャフト支持部６ａから重心位置までの距離をＬ_０とすると、Ｚ軸の正方向を上昇方向とした場合、シャフト支持部６ａから重心位置までの距離は、以下の（５５）式となる。
Ｌ＝Ｌ_０−ｐ_Ｚ ・・・（５５）

続いて、抑制周波数を取得する（Ｓ３）。この場合、抑制周波数は、上記した（５３）式または（５４）式のように求めればよい。
さて、ノッチフィルタＦＮ（ｓ）は、以下の（５６）式のように定義される。

ただし、（５６）式は連続時間系での伝達関数であり、そのままでは制御演算ができない。そのため、（５６）式を離散時間系に変換する。この場合、制御周期をＴｓとすると、離散時間系でのノッチフィルタＦＮ（ｚ^−１）は、以下の（５７）式のようになる。

この（５７）式に従ってフィルタパラメータを更新し（Ｓ４）、ノッチフィルタ演算を行うことで（Ｓ５）、抑制周波数可変ノッチフィルタ処理が実現される。
このように、重心位置の慣性モーメントを考慮してノッチフィルタを設計することにより、Ｚ軸高さが変わった場合であっても、それに応じてノッチフィルタの抑制周波数が変化することから、捩れ量つまりは捩れ角度を生じさせる非動作方向への振動に対応することができ、適切にシャフト支持部６ａの振動を抑制することができる。つまり、振動なくロボットを動かすことができる。

以上説明したように、本実施形態によれば、次のような効果を得ることができる。
本実施形態では、シャフト７側の重心位置を求め、求めた重心位置とシャフト支持部６ａとの距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部６ａの捩れ角度を生じさせる振動を抑制する振動抑制制御を行っている。つまり、シャフト支持部６ａの捩れ角度を生じさせる要因となる振動を抑制するために、従来のように２次元空間を対象とするだけでなく、３次元空間を対象として振動抑制制御を行っている。

そして、重心位置における慣性モーメントに対応する抑制周波数を求めることにより、３次元空間を対象とした場合における抑制すべき周波数である抑制周波数を求めることができ、抑制周波数が求まったことにより、その抑制周波数を抑制するような演算つまりは重心位置の慣性モーメントに応じたノッチフィルタ演算を行うことで、シャフト７側の重心位置が変化した場合であっても、その変化に追従させてノッチフィルタ演算を行うことができる。

このように、例えばアームの姿勢変化により共振周波数が変化する場合において動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することについては何ら考慮されていなかった従来技術とは異なり、本実施形態の振動抑制方法であれば、非動作方向の慣性モーメントが変化することに対応した振動抑制制御を行うことができる。すなわち、シャフト支持部６ａの捩れ角度を生じさせる振動を抑制することができる。

また、シャフト７の重心位置に応じて慣性モーメントが変化するという従来技術では考慮されていなかった事象を考慮した上で振動抑制制御を行っていることから、従来技術では考慮すらされていなかったために抑制することができなかった動作方向とは異なる非動作方向振動を、抑制することができる。
このとき、ノッチフィルタ演算に用いるフィルタパラメータを、シャフト７側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、シャフト支持部６ａからシャフト７側の重心位置までの距離に反比例する可変周波数に対して抑制周波数を求めることができる。

また、上記したノッチフィルタは制御系のどこに挿入しても効果を発揮することができるものの、フィードバック制御系の内部に挿入した場合、抑制周波数が低くなるとフィードバック制御系の不安定化を招くおそれがある。そのため、位置指令値に対してノッチフィルタ演算を行うなど、フィードフォワード制御系として実施するとより効果的である。

また、シャフト７側の重心位置を求め、求めた重心位置とシャフト支持部６ａとの距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、シャフト支持部６ａの捩れ角度を生じさせる振動を抑制する振動抑制制御を行う制御方法によっても、上記した効果を同様に得ることができる。

（その他の実施形態）
本発明は、上記し且つ図面に記載した態様に限定されるものではなく、その要旨を逸脱しない範囲で種々の変形や拡張をすることができる。
実施形態で示した数値は一例であり、これに限定されない。
ファーストアーム５とセカンドアーム６のそれぞれに複数方向への角速度を検出可能な２軸角速度センサを設けたが、それ以外のセンサ設置方法でもよく、例えば加速度センサと角速度センサとを組み合わせてもよい。例えば、配線距離が長くなるセカンドアーム６には省配線化のために角速度センサを設け、ファーストアーム５には第２実施形態のように２つの加速度センサを設けることなどが考えられる。

図面中、２はロボット、３は制御装置（ロボット制御装置）、３ａは制御部（Ｚ軸高さ取得部、距離取得部、抑制周波数取得部、更新部、演算部）、４はベース、５はファーストアーム、６はセカンドアーム、６ａはシャフト支持部、７はシャフト、８、９、１０、１１、１２はセンサ（慣性センサ、角速度センサ、加速度センサ、回転角度センサ、位置センサ）を示す。

Claims (12)

1. ベースと、前記ベースに取り付けられ、当該ベースに対して第１軸を中心として回転するファーストアームと、前記ファーストアームに取り付けられ、当該ファーストアームに対して第２軸を中心として回転するセカンドアームと、前記セカンドアームに取り付けられ、当該セカンドアームに対して上下移動および回転するシャフトと、を有する水平多関節型のロボットを制御するロボット制御装置であって、
   前記ファーストアームおよび前記セカンドアームに設けられている複数のセンサの検出結果から前記シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、前記シャフトの先端部の振動に相関する前記シャフト支持部での捩れ角度を求める処理を行う制御部を備えることを特徴とするロボット制御装置。
2. 前記制御部は、各アームに設けられている前記センサの検出結果から各アームにおける慣性力および慣性トルクを求め、
   求めた慣性力および慣性トルクから前記シャフト支持部にかかるトルクを推定し、
   推定したトルクに基づいて、前記シャフト支持部での捩れ角度を求めることを特徴とする請求項１記載のロボット制御装置。
3. 複数の前記センサは、慣性センサ、回転角度センサおよび前記シャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されていることを特徴とする請求項１または２記載のロボット制御装置。
4. 前記慣性センサは、角速度を検出する角速度センサ、または、加速度を検出する加速度センサであることを特徴とする請求項３記載のロボット制御装置。
5. 前記角速度センサは、複数方向への角速度を検出する角速度センサであることを特徴とする請求項４記載のロボット制御装置。
6. 前記加速度センサは、複数方向への加速度を検出する加速度センサであることを特徴とする請求項４または５記載のロボット制御装置。
7. 前記回転角度センサは、前記ファーストアームを前記ベースに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から６のいずれか一項記載のロボット制御装置。
8. 前記回転角度センサは、前記セカンドアームを前記ファーストアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から７のいずれか一項記載のロボット制御装置。
9. 前記回転角度センサは、前記シャフトを前記セカンドアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から８のいずれか一項記載のロボット制御装置。
10. 前記位置センサは、前記シャフトを上下駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から９のいずれか一項記載のロボット制御装置。
11. 前記制御部は、前記シャフト側の重心位置を求め、求めた重心位置と前記シャフト支持部との間の距離に基づいて当該重心位置における慣性モーメントに対応する抑制周波数を求め、求めた抑制周波数に対してノッチフィルタ演算を行うことで、前記シャフト支持部の捩れ角度を生じさせる振動を抑制する振動抑制制御を行うことを特徴とする請求項１から９のいずれか一項記載のロボット制御装置。
12. 前記制御部は、前記ノッチフィルタ演算に用いるフィルタパラメータを、前記シャフト側の重心位置に応じて変化する反共振周波数の変化に追従させて可変とすることにより、前記シャフト支持部から前記シャフト側の重心位置までの距離に反比例する可変周波数に対して前記抑制周波数を求めることを特徴とする請求項１１記載のロボット制御装置。

Images