(57)【要約】【課題】シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供する。【解決手段】実施形態のロボット制御装置（制御装置３）は、ファーストアーム５およびセカンドアーム６に設けられている複数のセンサ８、９、１０の検出結果からシャフト７を支持するシャフト支持部６ａにかかるトルクを推定し、推定したトルクに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求める。【選択図】図１
2017056542.tif 000002
【特許請求の範囲】
【請求項１】
ベースと、前記ベースに取り付けられ、当該ベースに対して第１軸を中心として回転するファーストアームと、前記ファーストアームに取り付けられ、当該ファーストアームに対して第２軸を中心として回転するセカンドアームと、前記セカンドアームに取り付けられ、当該セカンドアームに対して上下移動および回転するシャフトと、を有する水平多関節型のロボットを制御するロボット制御装置であって、
前記ファーストアームおよび前記セカンドアームに設けられている複数のセンサの検出結果から前記シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、前記シャフトの先端部の振動に相関する前記シャフト支持部での捩れ角度を求める処理を行う制御部を備えることを特徴とするロボット制御装置。
【請求項２】
前記制御部は、各アームに設けられている前記センサの検出結果から各アームにおける慣性力および慣性トルクを求め、
求めた慣性力および慣性トルクから前記シャフト支持部にかかるトルクを推定し、
推定したトルクに基づいて、前記シャフト支持部での捩れ角度を求めることを特徴とする請求項１記載のロボット制御装置。
【請求項３】
複数の前記センサは、慣性センサ、回転角度センサおよび前記シャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されていることを特徴とする請求項１または２記載のロボット制御装置。
【請求項４】
前記慣性センサは、角速度を検出する角速度センサ、または、加速度を検出する加速度センサであることを特徴とする請求項３記載のロボット制御装置。
【請求項５】
前記角速度センサは、複数方向への角速度を検出する角速度センサであることを特徴とする請求項４記載のロボット制御装置。
【請求項６】
前記加速度センサは、複数方向への加速度を検出する加速度センサであることを特徴とする請求項４または５記載のロボット制御装置。
【請求項７】
前記回転角度センサは、前記ファーストアームを前記ベースに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から６のいずれか一項記載のロボット制御装置。
【請求項８】
前記回転角度センサは、前記セカンドアームを前記ファーストアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から７のいずれか一項記載のロボット制御装置。
【請求項９】
前記回転角度センサは、前記シャフトを前記セカンドアームに対して回転駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から８のいずれか一項記載のロボット制御装置。
【請求項１０】
前記位置センサは、前記シャフトを上下駆動するモータに設けられているモータエンコーダであることを特徴とする請求項３から９のいずれか一項記載のロボット制御装置。
【請求項１１】
前記制御部は、前記シャフトの上下位置に応じて変化する当該シャフトの重心位置に対する運動方程式によって予め定義され、前記シャフト支持部の捩れ角度を生じさせる共振特性を含む状態空間モデルに対して、制御系を設計することを特徴とする請求項１から９のいずれか一項記載のロボット制御装置。
【請求項１２】
前記制御部は、前記状態空間モデルに対して状態フィードバックおよび状態オブザーバを構築することで、制御系を設計することを特徴とする請求項１１記載のロボット制御装置。
【請求項１３】
前記状態方程式は、前記シャフトの重心位置における動作方向に直交する向きの慣性モーメントを含んで定義されていることを特徴とする請求項１１または１２記載のロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットを制御するロボット制御装置に関する。
【背景技術】
【０００２】
ロボット制御の分野では、モデル化したロボットの動作を予めシミュレーションし、所望のシミュレーション結果、例えば所望の振動抑制効果を得られた制御内容を実際のロボットに適用することが一般的に行われている。また、ロボットの振動を抑制する振動抑制制御についても様々な検討がなされており、例えば特許文献１、２では、振動を抑制しながらロボットを動作させることができる制御装置が提案されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第４０３８６５９号公報
【特許文献２】特許第５４１１６８７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、シミュレーション上では所望の振動抑制効果が得られた制御内容を実際のロボットに適用した場合、シミュレーション通りの性能を発揮できないことがある。具体的には、シミュレーション上では発生していなかった振動が、実際のロボットにおいて発生していた。
本発明は上記事情に鑑みてなされたものであり、その目的は、シミュレーション上では現れなかった振動を抑制することができるロボット制御装置を提供することにある。
【課題を解決するための手段】
【０００５】
ロボットの制御分野では、モータとアーム間の減速機に存在するバネ要素による振動モードを考慮したいわゆる２慣性系のシミュレーションが利用されている。この振動モードは、減速機等の動力が伝達していく動力伝達機構における動作方向の剛性に起因する振動である。また、２慣性系のシミュレーションは、広く利用されており、その有効性は認知されていると考えられる。
【０００６】
このとき、アームの剛性等の機械的数値はシミュレーション条件に当然盛り込まれており、実際のロボットは、そのシミュレーション条件を満たすような機械的数値の範囲で設計されている。それにも関わらずロボットに振動が生じるということは、従来の２慣性系で考慮されていた振動モードとは異なる振動モードが存在していると考えられる。
【０００７】
そして、発明者らは、振動を生じさせる原因の調査を重ねた結果、実際のロボットでは、２慣性系の振動モードにおける動力伝達機構の動作方向の剛性による振動（以下、便宜的に動作方向振動と称する）以外にも、動力伝達機構の動作方向とは異なる振動（上記した非動作方向振動）が存在していることを見いだした。換言すると、動作方向とは異なる向きの非動作方向振動がシミュレーションには現れなかった振動の原因であることが発明者らによって突き止められたとともに、その非動作方向振動によってロボットの可動部側が全体的に動作方向以外にも揺れ、その結果、シャフトの支持部に捩れ振動が発生し、その捩れ振動が手先の振動として現れていたことが明らかになった。
【０００８】
さて、もしも非動作方向振動の存在が今まで認知されていたならば、非動作方向振動に対処するための制御方法が検討されているはずである。しかし、実際には、非動作方向振動の存在を示唆するような考察や非動作方向振動を抑制するための制御方法等は検討されていない。つまり、非動作方向振動は、今まで認知されていなかったと考えられる。そのため、発明者らは、なぜ今まで非動作方向振動が考慮されていなかったのかについて考察した。
【０００９】
最初期の産業用のロボットは、近年のロボットと比べて、格段にアームやギアあるいは軸受け部材などが相対的に太く且つ頑丈であった反面、アームおよび可動部分の全体の重量が相対的に大きかった。２慣性系の場合、共振周波数は、慣性（つまり重量）の逆数の平方根に比例することから、動作方向振動の共振周波数は低くなる。その一方で、アームを構成する部材等は非常に高剛性に作られており、非動作方向共振が存在していたとしても、その共振周波数は高くなっていたと考えられる。
【００１０】
一般的に、複数の共振が存在する場合、低い共振周波数を持つ共振による影響が支配的となる場合が多い。つまり、非動作方向共振は、共振周波数が相対的に高かったことから、アームの位置応答や速度応答へ与える影響は無視できるほど小さかったと考えられる。また、発明者らの研究の結果、非動作方向振動の発生原因には例えば遠心力のように非動作方向に加わる力の存在があることが判明したが、最初期のロボットは、近年のロボットに比べて動作速度が相対的に遅かったため、遠心力による影響は無視できるほど小さかったと考えられる。
【００１１】
これに対して、近年のロボットでは、アームを太く頑丈にする方向から、細く軽量化する方向へとその設計が変化してきている。つまり、アームが軽量化されてきた反面、アームを構成する部材は、最初期のロボットに比べれば低剛性化している。なお、低剛性化しているとはいっても、柔軟アームと呼ばれるようなアーム自体が捻れてしまうような状態ではなく、例えばクロスローラなどの軸支持部で、その回転軸以外の方向に微少振動が発生しているということである。
【００１２】
そのため、複数の共振が存在している状態において支配的な共振が最初期のロボットとは入れ替わってきた、あるいは、両者の共振が近い共振周波数となって互いに影響し合うような状態になってきたと考えられる。さらに、近年のロボットの場合、最初期のロボットと比べてその動作速度が格段に高速化されており、速度の２乗に比例する遠心力の影響がより顕著に現れてきたと考えられる。
【００１３】
例えば４軸の水平多関節型ロボットの場合であれば、シャフトを上下方向（Ｚ方向）へ直動する支持部が、例えばセカンドアームの動きに連動して振動してしまう現象が確認されている。この場合、軸間干渉による振動のようにシャフトの移動方向への振動（動作方向振動に含まれる）とは異なる振動、具体的には、セカンドアームの円周方向（セカンドアームの動作方向）やセカンドアームの直径方向（セカンドアームに加わる遠心力の方向）への振動が発生している。なお、６軸の垂直多関節型ロボット（ＰＵＭＡ（Programmable Universal Manipulation Arm）型ロボット）や、いわゆる７軸ロボット等の他の構成のロボットにおいても、これに類似する現象により、非動作方向振動が発生している。
【００１４】
そして、産業用のロボットにおいては、複数のアームが連結してロボットを構成することが多いため、ある軸が振動した結果、別の軸が干渉して振動するといった現象が発生する。このため、振動特性自体が単純な２慣性系モデルのようにはっきりとしたものとはならならず、複数の共振振動が周波数特定に現れること、また、周波数特性に現れる共振周波数と実際の振動波形に現れる共振周波数とに僅かな相違があることがある。そのため、非動作方向振動が存在していたとしても、非動作方向振動が原因となっていることを突き止めることが困難であったと考えられる。
【００１５】
さらに、動作方向振動と非動作方向振動とでは振動周波数が異なることが多いものの、減速機として例えば波動歯車装置を用いている場合には、その剛性が入力されるトルクに応じて変化することが知られている等、モデル化誤差の原因となる要素が様々であることから、誰も非動作方向振動に想到することがなく、単に誤差として扱われていたものと考えられる。
【００１６】
このような事情によって、シミュレーション上で現れた振動のうち最も影響度の大きいものを動作方向振動として扱い、それ以外は他の軸からの干渉などの誤差として扱っていたことから、非動作方向振動についての検討がなされてこなかったものと考えられる。つまり、最初期のロボットでは非動作方向振動がそれほど顕著ではなく、また、近年のロボットでは非動作方向振動が誤差として扱われていたことが、非動作方向振動が認知されていなかった理由であると推測された。
【００１７】
そして、このような非動作方向振動の存在は、従来のシミュレーションの前提条件であった２慣性系モデルや干渉を含めた２慣性系モデルを拡張した例えば特許文献１でいう４慣性モデル等の制御モデルでは実際のロボットの振動特性をそもそも表現しきれていなかったこと示しており、極めて重大な技術的意義をもっている。
【００１８】
さて、非動作方向振動の存在が判明したのであれば、その非動作方向振動に起因して生じる振動を抑制するような制御を行えば、シミュレーションには現れなかった手先に現れる振動を抑制することができると考えられる。
例えば水平多関節型ロボットの場合、回転軸である１軸目および２軸目の動作に連動して、Ｚ軸直動部（シャフト）の支持部（シャフト支持部）で捩れ振動が発生していた。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸（後述する図１参照）の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生する。そのため、その振動を検出すれば、手先に現れる振動を抑制できると考えられる。
【００１９】
しかし、従来のロボットにおいてはＲｘ軸方向およびＲｙ軸方向の振動を検出するセンサが設けられておらず、Ｒｘ軸方向およびＲｙ軸方向の振動を検出すること自体ができ無かった。そのため、振動の発生を検知してその振動を抑制する制御を行うこともできなかった。  つまり、従来のロボットは、非動作方向振動が考慮されていなかったため、回転方向以外の振動を検出する手段をそもそも備えておらず、また、非動作方向振動の検出自体ができていないことから、その非動作方向振動を収束させる制御も行うこともできなかった。
【００２０】
また、ロボットの手先にセンサを設けることは、一般に想像するよりも困難である。例えば有線式のセンサをロボットの手先に設ける場合には、各アームの回転やシャフトの回転および上下動からケーブルを保護するために、いわゆるケーブルリール等の保護部材が必要となって配線構造が複雑化する。かといって、無線式のセンサを手先に設けると、ロボットの動作中にセンサへ給電あるいは充電することが容易ではないことに加えて、ノイズによって無線通信が断続するおそれがある。
【００２１】
そこで、請求項１に係る発明では、ロボット制御装置は、水平多関節型ロボットのファーストアームおよびセカンドアームに設けられている複数のセンサの検出結果から、シャフトを支持するシャフト支持部にかかるトルクを推定し、当該推定したトルクに基づいて、シャフトの先端部の振動に相関するシャフト支持部での捩れ角度を求めている。
【００２２】
つまり、制御装置は、ロボットの手先（シャフトの先端部）ではなく、各アームに設けられているセンサの検出結果から、シャフトに生じる捩れ振動に相関しているとともに、ロボットの手先つまりはシャフトの先端部の振動にも相関しているシャフト支持部での捩れ角度を求めている。
【００２３】
この場合、具体的な計算方法は、シャフト支持部にかかるトルクをＮ３、シャフト支持部での捩れ角度をφ３、シャフト支持部の剛性をＫ３、複数センサの検出値をＳ１〜ＳＮとすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）
Ｎ３＝Ｎ３ｅｓｔ（Ｓ１〜ＳＮ）
ただし、φ３ｅｓｔはシャフト支持部にかかるトルクからシャフト支持部での捩れ角度を求める関数であり、Ｎ３ｅｓｔは複数センサの検出値からシャフト支持部にかかるトルクを求める関数である。以下でも同様に、末尾にｅｓｔが付帯された関数は、センサ検出値あるいは別の推定値から、目的とする値を推定する関数である。
【００２４】
この場合、ファーストアームに関しては、ファーストアームに設けられているセンサの検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボットでは各アームがリンクしていることから、セカンドアームに関しては、ファーストアームに設けられているセンサの検出結果とセカンドアームに設けられているセンサの検出結果とから、セカンドアームのみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、シャフトに関しても、それぞれのセンサの検出結果から、シャフト支持部にかかるトルクを求めることができる。
【００２５】
そして、シャフト支持部にかかるトルクが求まれば、シャフトの剛性に基づいてシャフトの捩れ角度を求めることができる。
この場合、具体的な計算方法は、シャフト支持部の剛性をＫ３とすると、次のように数式で記載することができる。
φ３＝φ３ｅｓｔ（Ｎ３）＝Ｎ３÷Ｋ３
このように、各アームに設けられているセンサからの複数の検出結果を利用することにより、ロボットの手先にセンサを設けなくても、ロボットの手先の振動の要因となるシャフト支持部での捩れ振動を求めることができる。
また、センサは、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。
【００２６】
より詳細には、請求項２に係る発明のように、ロボット制御装置は、各アームに設けられているセンサの検出結果から各アームにおける慣性力および慣性トルクを求め、求めた慣性力および慣性トルクシャフト支持部にかかるトルクを推定し、推定したトルクに基づいてシャフト支持部での捩れ角度を求める。
この場合、具体的な計算方法は、ファーストアームの慣性力をＦＧ１、ファーストアームの慣性トルクをＮＧ１、セカンドアームの慣性力をＦＧ２、セカンドアームの慣性トルクをＮＧ２とすると、次のように数式で記載することができる。
【００２７】
ＦＧ１＝ＦＧ１ｅｓｔ（Ｓ１〜ＳＮ）
ＦＧ２＝ＦＧ２ｅｓｔ（Ｓ１〜ＳＮ）
ＮＧ１＝ＮＧ１ｅｓｔ（Ｓ１〜ＳＮ）
ＮＧ２＝ＮＧ２ｅｓｔ（Ｓ１〜ＳＮ）
Ｎ３＝Ｎ３ｅｓｔ（ＦＧ１，ＦＧ２，ＮＧ１，ＮＧ２）
このような手順および数式により、シャフト支持部にかかるトルクを推定することができる。
【００２８】
各アームに設けられているセンサは、具体的には、請求項３に係る発明のように、慣性センサ、回転角度センサおよびシャフトの上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフトの上下位置を直接的に検出できる。
このように、シャフト支持部にかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
【００２９】
さらに詳細には、請求項４に係る発明のように、慣性センサとして角速度を検出する角速度センサ、あるいは加速度を検出する加速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。
角速度センサは、請求項５に係る発明のように、複数方向への角速度を検出する角速度センサを用いることができる。
加速度センサは、請求項６に係る発明のように、複数方向への加速度を検出する加速度センサを用いることができる。
【００３０】
回転角度センサは、請求項７に係る発明のように、ファーストアームをベースに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項８に係る発明のように、セカンドアームをファーストアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、回転角度センサは、請求項９に係る発明のように、シャフトをセカンドアームに対して回転駆動するモータに設けられているモータエンコーダを用いることができる。
また、位置センサは、請求項１０に係る発明のように、シャフトを上下駆動するモータに設けられているモータエンコーダを用いることができる。
このようにアームやシャフトを駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、回転角度やシャフトの上下位置を取得することができる。
【００３１】
請求項１１に係る発明では、シャフトの上下位置に応じて変化する当該シャフトの重心位置に対する運動方程式によって予め定義され、シャフト支持部の捩れ角度を生じさせる共振特性を含む状態空間モデルに対して、制御系を設計する。つまり、状態空間モデルは、シャフトの上下位置の変化を考慮して定義されている。
シャフトの先端部つまりはロボットの手先の振動は、シャフトが下方に位置して重心位置が下方にある場合には相対的に大きくなると考えられる一方、シャフトが上方に位置して重心位置が比較的上方にある場合には相対的に小さくなると考えられる。つまり、シャフトの先端部の振動は、シャフトの重心位置によって変化すると考えられる。そして、シャフトの重心位置は、シャフトの上下位置によって変化すると考えられる。
そのため、状態空間モデルをシャフトの重心位置に対する運動方程式によって定義することで、状態空間モデルにシャフトの重心位置の変化による影響を加味した状態で制御系を設計することができる。そして、その制御系に対して振動抑制制御を行うことで、ロボットの手先の振動を効果的に抑制することができる。
【００３２】
この場合、請求項１２に係る発明のように状態空間モデルに対して状態フィードバックおよび状態オブザーバを構築することで、フィードバック制御により振動抑制制御を行うことができる。また、状態オブザーバを構築することで、直接的には物理量を取得できない手先の振動を推定することができる。
また、請求項１３に係る発明のように状態方程式にシャフトの重心位置における動作方向に直交する向きへの慣性モーメントを含めることで、動作方向とは異なる非動作方向への慣性モーメントを加味した状態で振動を抑制することができ、非動作方向振動を効果的に抑制することができる。
【図面の簡単な説明】
【００３３】
【図１】第１実施形態によるロボットおよびロボット制御装置の構成を模式的に示す図
【図２】ファーストアームに生じるＲｙ回転を模式的に示す図
【図３】センサ位置を模式的に示す図
【図４】第１実施形態におけるセンサ位置を模式的に示す図
【図５】第３実施形態によるフィードバックシステムを示すブロック線図
【発明を実施するための形態】
【００３４】
以下、本発明の複数の実施形態について、図面を参照しながら説明する。なお、各実施形態において実質的に共通する部位には同一の符号を付し、その詳細な説明は省略する。
（第１実施形態）
本発明の第１実施形態について図１から図４を参照しながら説明する。
【００３５】
図１に示すように、ロボットシステム１は、ロボット２と、ロボット２を制御する制御装置３（ロボット制御装置に相当する）とを備えている。制御装置３は、制御部３ａを備えており、ロボット２を制御する。この制御部３ａは、詳細は後述するが、センサ８、９、１０（図２参照）の検出結果にもとづいてシャフト７を支持するシャフト支持部６ａ（図３参照）にかかるトルクを推定し、推定したトルクに基づいて当該シャフト支持部６ａでの捩れ角度を求めることを含む実施形態で説明する種々の処理を実行する。このシャフト支持部６ａでの捩れ角度は、捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関している。
【００３６】
本実施形態のロボット２は、ベース４と、ベース４に取り付けられ（ベース４に支持され）、当該ベース４に対して第１軸（Ｊ１）を中心として回転するファーストアーム５と、ファーストアーム５に取り付けられ（ファーストアーム５に支持され）、当該ファーストアーム５に対して第２軸（Ｊ２）を中心として回転するセカンドアーム６と、セカンドアーム６に取り付けられ（セカンドアーム６に支持され）、当該セカンドアーム６に対して上下移動および回転するシャフト７と、を有する水平多関節型ロボットを想定している。
【００３７】
シャフト７は、本実施形態では、ボールねじスプラインを想定している。この場合、ロボット２の手先とは、シャフト７の下端側、つまり、セカンドアーム６に対して上下方向に相対的に移動するとともに、セカンドアーム６に対して相対的に回転する部分に相当する。なお、シャフト７は、非常に剛性が高く設計されており、シャフト７そのものが撓んだり変形したりすることは考えにくい。ただし、シャフト７の剛性が低く、シャフト７そのものが撓んだり変形したりしている場合であっても、その撓みや変形がシャフト支持部６ａで起きているものとして等価変換することにより、同様の効果を得ることが出来る。
【００３８】
以下、図１に示すように、シャフト支持部６ａを原点とし、シャフト７と同軸となる天地方向をＺ軸（Ｚ方向）、Ｚ軸に直交し、セカンドアーム６に沿った方向をＸ軸（Ｘ方向）、これらＺ軸およびＸ軸に直交する方向をＹ軸（Ｙ方向）として説明する。また、図１に示すＺ軸回りの回転をＲｚ回転と称し、Ｘ軸回りの回転をＲｘ回転と称し、Ｙ軸回りの回転をＲｙ回転と称する。
【００３９】
この場合、Ｒｘ回転とは、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＸ軸回りに回転する状態、より平易に言えば、セカンドアーム６が捩れた状態に相当する。また、Ｒｙ回転とは、セカンドアーム６を例にすると、図１に示す状態においてシャフト支持部６ａを中心としてセカンドアーム６がＹ軸回りに回転する状態、より平易に言えば、セカンドアーム６の先端側がＺ方向に揺れる状態に相当する。
【００４０】
このロボット２は、図３に示すように、複数のセンサ８、９、１０を備えている。このうち、センサ８、９は、ファーストアーム５およびセカンドアーム６に設けられ、各アームの慣性を検出する慣性センサであり、センサ１０は、セカンドアーム６に設けられ、シャフト７の上下位置を検出する位置センサである。各センサ８、９の配置は、基本的には任意に設定することができる。
【００４１】
より具体的には、本実施形態では、センサ８は、ファーストアーム５に設けられており、ファーストアーム５が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、ファーストアーム５における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。また、センサ９は、セカンドアーム６に設けられ、当該セカンドアーム６が動作した際のＲｘ回転の角速度およびＲｙ回転の角速度を検出する２軸角速度センサで構成されており、セカンドアーム６における動作方向とは異なる向き（非動作方向）に生じる角速度を検出する。
【００４２】
また、センサ１０は、セカンドアーム６内のシャフト支持部６ａに設けられており、シャフト７を上下駆動するモータの回転角度を検出するモータエンコーダである。なお、図示は省略するが、これら各センサ８、９、１０は、ケーブルにて接続されている有線式のものを使用している。なお、図示は省略するが、ファーストアーム５を駆動するモータおよびセカンドアーム６を回転駆動するモータにも、モータエンコーダが設けられている。
【００４３】
次に、上記した構成の作用について説明する。
問題となっているシャフト支持部６ａでの捩れ振動の振動周波数は、減速機構での回転方向の共振振動の振動周波数よりも十分に低い場合が多い。一例としては、例えばシャフト７直動部の支持部での捩れ振動の振動周波数が１０Ｈｚ程度以下であるのに対し、減速機構での回転方向の共振振動の振動周波数は、５０Ｈｚ程度である。なお、シャフト７がボールねじスプラインの場合などでは、その共振振動周波数は１００Ｈｚを超えることもある。
【００４４】
このように振動周波数の差が大きい場合には、モータエンコーダの値をアームのＲｚ軸回転角度あるいはＺ軸位置（シャフト位置）として問題ない。なお、回転方向の共振振動が問題になる場合には、減速機の出力側にエンコーダ（シャフト７の場合にはリニアエンコーダ等）を設けたり、角速度センサや加速度センサを追加したり、状態オブザーバにより共振振動を推定したりすることで、共振振動の影響を除外することができる。
【００４５】
そこで、まず、各アームにおける各種のパラメータや変数を、以下のように定義する。なお、以下では、各種のパラメータや変数を総称してパラメータとしている。また、使用可能文字の関係上、明細書中では、例えばファーストアーム支持部捩れ角度などについては、「φ」と示すものとする。
＜＜ファーストアーム５に関するパラメータ＞＞
【００４６】
【数１】
2017056542.tif 000003
【００４７】
＜＜セカンドアーム６に関するパラメータ＞＞
【００４８】
【数２】
2017056542.tif 000004
【００４９】
＜＜シャフト７に関するパラメータ＞＞
【００５０】
【数３】
2017056542.tif 000005
【００５１】
これらのパラメータのうち、以下のパラメータは、各センサ８〜１０によって検出された値、あるいは、検出された値を演算することにより求めることができる。
【００５２】
【数４】
2017056542.tif 000006
【００５３】
なお、ファーストアーム５およびセカンドアーム６の重心位置での角速度は、ファーストアーム５およびセカンドアーム６内のどの点の角速度を測定しても、重心位置での角速度と同じ値が検出される。
また、各パラメータは、以下の関係を持っている。
【００５４】
【数５】
2017056542.tif 000007
【００５５】
さて、トルクの推定手法について述べる前に、後述する演算の簡略化について説明する。
回転振動について、その回転軸を単位ベクトルｖ、その回転量をφとする。ただし、単位ベクトルｖは、以下の（３）式である。このとき、回転行列Ｒは、ロドリゲスの回転公（Rodrigues' Rotation Formula）を用いると、以下の（４）式で表される。この、回転行列Ｒ中の行列Ｖは、単位ベクトルｖにより、以下の（５）式のように定義される。そして、単位ベクトルｖおよび回転量φは、以下の（６）式および（７）式のように求まる。
【００５６】
【数６】
2017056542.tif 000008
【００５７】
ここで、例えばファーストアーム５とセカンドアーム６のアーム長の合計が１０００ｍｍであったとし、手先に現れる振動がおよそ１ｍｍ程度であったとすると、回転量φは０．００１（ｒａｄ）となる。このとき、φ＜＜１であることから、ｓｉｎφ＝φ、ｃｏｓφ＝１と近似すると、回転行列Ｒは、以下の（８）式のように表すことができる。
【００５８】
【数７】
2017056542.tif 000009
【００５９】
ここで、ｖが単位ベクトルであることから、回転行列Ｒの非対角成分の絶対値は、φ以下となることが分かる。このことは、ある軸の値に対して、別の軸の値が最大でφだけ誤差として重畳してしまうことを示している。
さて、計測対象の軸に対して直交する別の軸からの影響度を一般的に他軸感度と呼ぶが、上記のように近似した値を用いることで回転行列をＲｚ軸回転に限定したことによる他軸感度への影響は、０．１％となる。
【００６０】
これに対して、センサ８、９等の慣性センサ自体の他軸感度は、一般的に、数％程度（例えば４％程度）である。このため、回転行列をＲｚ軸回転に限定することによる誤差は、慣性センサ自体の持つ他軸感度による誤差と比べて一桁以上小さくなっている。このため、Ｒｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視しても、実用上つまりは実際にロボット２を制御する上では、大きな問題となることはない。そのため、本実施形態では、以下のように、シャフト支持部６ａにかかるトルクを求める際には、回転行列についてはＲｚ軸方向の回転のみを考慮し、捩れ角度の影響は無視することで、演算を簡略化することができるようになる。
さて、このように演算を簡略化した場合には、各回転行列は、以下の（９）式から（１１）式のように表すことができる。
【００６１】
【数８】
2017056542.tif 000010
【００６２】
勿論、簡略化せずに、それぞれの支持部における捩れ角度の影響を含めて、Ｒｘ軸回転およびＲｙ軸回転を考慮した回転行列を用いる場合にも、以下の手法によりトルクを推定することは可能である。
さて、ファーストアーム５およびセカンドアーム６の重心位置の加速度、慣性力、慣性トルクは、以下の（１２）式から（１７）式のようになる。
【００６３】
【数９】
2017056542.tif 000011
【００６４】
これらより、ファーストアーム支持部およびセカンドアーム支持部の力およびトルクは、以下の（１８）式から（２１）式のようになる。
【００６５】
【数１０】
2017056542.tif 000012
【００６６】
また、ファーストアーム支持部およびセカンドアーム支持部のトルクは、支持部の剛性を用いて、以下の（２２）式および（２３）式のように求まる。ただし、Ｒｚ軸方向のトルクについては、厳密に捩れ剛性を用いて求めなくても、モータトルクをギア比により各アーム支持部のＲｚ軸方向のトルクに換算して用いても良い。
【００６７】
【数１１】
2017056542.tif 000013
【００６８】
そして、ファーストアーム支持部のトルクから、セカンドアーム支持部の力を求める。ただし、ここで求まるセカンドアーム支持部の力は、この時点では一意に求まることはなく、例えば（２５）式のλＬ１や（２６）式のλＲ２Ｌ１のように、ファーストアーム５のリンク長ベクトル方向に水平な未知の項を含んだ状態で求まる。そして、求めたセカンドアーム支持部の力より、シャフト支持部６ａの力を以下の（２７）式のように求める。なお、以下の式のＮＦ２１、Ｆ２０は、式を置き換えるために定義したものである。
【００６９】
【数１２】
2017056542.tif 000014
【００７０】
続いて、セカンドアーム支持部のトルクから、以下の（２８）式および（２９）式のように、シャフト支持部６ａの力を消去する。
【００７１】
【数１３】
2017056542.tif 000015
【００７２】
この（２８）式から、未知項は、ファーストアームリンク長を、セカンドアーム回転角度だけＲｚ軸回転させたベクトルとセカンドアーム６のリンク長の外積で表されることが明らかになった。これらリンク長ベクトルは、Ｘ軸方向にのみ大きな値であるものの、Ｙ軸方向あるいはＺ軸方向には小さい値となっている。
このため、セカンドアーム回転角度が０°の場合、未知項は、非常に小さな値となるため、無視しても問題ないレベルとなる。また、セカンドアーム回転角度が９０°の場合、未知項は、ほぼＲｚ軸方向のベクトルとなり、Ｒｘ軸方向あるいはＲｙ軸方向については、その影響をほぼ無視しても問題ないレベルとなっている。
【００７３】
すなわち、上記した式には未知項が含まれているものの、その影響は、Ｒｚ軸方向へは大きく作用する場合があるものの、Ｒｘ軸方向あるいはＲｙ軸方向ではその影響を無視できる。これを式として表すならば、リンク長ベクトルがＸ軸成分のみに値を持つとすれば、未知項は、Ｒｚ軸方向へのみ影響を表すことから、次の（３０）式から（３２）式のように示すことができる。
【００７４】
【数１４】
2017056542.tif 000016
【００７５】
次に、セカンドアーム支持部のトルクからシャフト支持部６ａのトルクを、以下の（３３）式から（３５）式のようにして求める。そして、このシャフト支持部６ａのトルクと剛性とより、シャフト支持部捩れ角が、以下の（３３）式のように求まる。ただし、シャフト支持部６ａのトルクにも上記したＲｚ軸方向へは未知項が影響しており、この未知項の影響により、シャフト支持部捩れ角は、Ｒｘ軸方向およびＲｙ軸方向の捩れ角のみが有効なものとして導出されている。
【００７６】
【数１５】
2017056542.tif 000017
【００７７】
なお、シャフト支持部捩れ角は、ファーストアーム支持部あるいはセカンドアーム支持部の場合と同様に、４軸目モータトルクをギア比により変換することでＲｚ軸方向トルクを求め、そのＲｚ軸方向トルクからシャフト支持部捩れ角を求めてもよい。
このように、本実施形態の制御装置３は、手先では無く、各アームに設けた複数のセンサの検出結果に基づいてシャフト支持部６ａのトルクを推定し、推定したシャフト支持部６ａのトルクに基づいて、シャフト支持部捩れ角、つまりは、手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めている。
【００７８】
以上説明した実施形態によれば、次のような効果を得ることができる。
ロボット制御装置（制御装置３）は、水平多関節型のロボット２のファーストアーム５およびセカンドアーム６に設けられている複数のセンサ８、９、１０の検出結果から、シャフト７を支持するシャフト支持部６ａにかかるトルク（Ｎ３）を推定し、当該推定したトルク（Ｎ３）とシャフト７の剛性とに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求めている。
【００７９】
つまり、制御装置３は、ロボット２の手先（シャフト７の先端部７ａ）ではなく、各アームに設けられているセンサ８、９、１０の検出結果から、シャフト７に生じる捩れ振動に相関しているとともに、ロボット２の手先つまりはシャフト７の先端部７ａの振動にも相関しているシャフト支持部６ａでの捩れ角度を求めている。
【００８０】
ファーストアーム５に設けられているセンサ８の検出結果から、ファーストアーム支持部にかかるトルクを求めることができる。また、ロボット２は各アームがリンクしていることから、ファーストアーム５に設けられているセンサ８の検出結果と、そのファーストアーム５に対して相対的に動作可能なセカンドアーム６に設けられているセンサ９の検出結果とから、セカンドアーム６のみでみた場合の値を求めることができ、求めた値からセカンドアーム支持部にかかるトルクを求めることができる。同様に、センサ８〜１０の検出結果から、シャフト支持部６ａにかかるトルクを求めることができる。
そして、シャフト支持部６ａにかかるトルクが求まれば、シャフト７の剛性に基づいて、シャフト７の捩れ角度を求めることができる。
これにより、ロボット２の手先にセンサを設けなくても、ロボット２の手先の振動の要因となるシャフト支持部６ａでの捩れ振動を求めることができる。
【００８１】
このとき、センサ８、９、１０は、各アームに設けられているため、手先に設ける場合と比べて、その配線構造を簡略化することができる。また、有線式であることから、電源供給等の問題が生じず、また、無線式のようにノイズにより通信が途切れることもないため、信頼性を向上させることができる。
【００８２】
より詳細には、制御装置３は、各アームに設けられているセンサ８、９、１０の検出結果から各アームにおける慣性力を求め、求めた慣性力から各アームを支持するそれぞれのアーム支持部でのトルクを求める。そして、制御装置３は、求めた各アーム支持部でのトルクからシャフト支持部６ａにかかるトルクを推定し、推定したトルクに基づいてシャフト支持部６ａでの捩れ角度を求めている。
複数のセンサ８、９、１０は、慣性センサ、回転角度センサおよびシャフト７の上下位置を検出する位置センサのうちいずれかの組み合わせで構成されている。
【００８３】
例えば、速度と加速度は、いずれか一方を検出すれば演算により他方を求めることができる。ただし、その場合には、演算時間を要することになるが、換言すれば、演算が不要であればその分だけ処理速度を向上させることができる。そして、慣性センサであれば、直接的に慣性力を検出でき、回転角度センサであれば、回転角度を直接的に検出でき、位置センサであればシャフト７の上下位置を直接的に検出できる。
【００８４】
このように、シャフト支持部６ａにかかるトルクを推定するために必要となる各種の物理量を直接的に検出できるセンサを用いることにより、処理速度の向上を図ることができる。
より詳細には、慣性センサとして角速度を検出する角速度センサを用いることで、各アーム支持部でのトルクを求める際の演算を省略あるいは簡略化することができる。また、位置センサとしてシャフト７を駆動するモータに一般的に設けられているモータエンコーダを用いることで、追加部材を必要とすること無く、シャフト７の上下位置を取得することができる。
また、各センサ８、９の配置は基本的には任意に設定することができるため、配線が過度に複雑化することを抑制できる。
【００８５】
また、本実施形態の場合、常に振動を検出することが可能となるので、振動特性が外気温度（昼夜や季節による温度変動）や経年劣化あるいは暖機運転の状態（平常時と長期連休直後の相違）などにより変動することを考慮すると、例えば試験運転時にのみセンサを利用し、実稼働時にはセンサを利用しないような構成とは異なり、期待通りの振動抑制効果を発揮することができる。
また、ファーストアーム５およびセカンドアーム６に設けられている複数のセンサの検出結果からシャフト７を支持するシャフト支持部６ａにかかるトルクを推定し、当該推定したトルクに基づいて、シャフト７の先端部７ａの振動に相関するシャフト支持部６ａでの捩れ角度を求める制御方法によっても、同様の効果を得ることができる。
【００８６】
（第２実施形態）
以下、第２実施形態について説明する。第２実施形態の場合、センサの設置態様が第１実施形態と異なっている。
図４に示すように、本実施形態の場合、ファーストアーム５には、第１実施形態のセンサ８（角速度センサ）の代わりに、２軸の加速度センサであるセンサ１１、１２を設けている。なお、図示は省略するが、セカンドアーム６にも２軸の加速度センサが２つ設けられている。また、センサ１１、１２も含めて、各センサの配置は基本的には任意に設定することができる。
【００８７】
まずファーストアーム５について説明する。１軸回転中心（Ｊ１軸上の点Ｃ１）から２つのセンサ１１、１２の測定位置までの距離をそれぞれＬＳ１１Ｘ、ＬＳ１２Ｘとする。また、Ｊ１軸とＪ２軸とを通る仮想的な直線をＣＬ１とし、ＣＬ１からＹ方向への距離をそれぞれＬＳ１１Ｙ、ＬＳ１２Ｙとし、ＣＬ１からＺ軸方向への距離をそれぞれＬＳ１１Ｚ、ＬＳ１２Ｚとする。また、各センサ１１、１２の測定軸は、Ｙ軸とＺ軸であるものとする。
このとき、各センサ１１、１２によって検出される加速度は、センサの数をｎとすると、以下の（３７）式のように表される。
【００８８】
【数１６】
2017056542.tif 000018
【００８９】
そして、Ｙ軸加速度から、Ｒｙ軸角加速度とＲｚ軸角加速度は、以下の（３８）式から（４１）式のように導出される。
【００９０】
【数１７】
2017056542.tif 000019
【００９１】
同様に、Ｚ軸加速度から、Ｒｘ軸角加速度とＲｙ軸角加速度が、以下の（４２）式から（４５）式のように導出される。
【００９２】
【数１８】
2017056542.tif 000020
【００９３】
そして、求めたそれぞれの角加速度を積分することにより、角速度をそれぞれ求めることができる。なお、Ｒｘ軸角加速度はいずれの加速度からも求めることができるが、例えば平均値を利用したり、角速度および角加速度の誤差が小さくなるように補正したり、分母が大きい方の値を利用するなどにより、精度を向上させることができる。
【００９４】
次に、セカンドアーム６の場合を考える。セカンドアーム６の場合、２軸回転中心（Ｊ２軸上の点）の加速度をセンサの検出結果から差し引くことで、上記したファーストアーム５の場合と同様の演算にてＸ軸、Ｙ軸、Ｚ軸の角速度をそれぞれ求めることができる。この場合、２軸回転中心の加速度は、ファーストアーム５の角速度あるは角加速度から求めることができる。
そして、各アームにおける角速度が求まれば、（１）式に代入し、第１実施形態と同様に演算することで、シャフト支持部捩れ角、つまりは、手先が振動した場合の手先位置を取得することができる等、第１実施形態と同様の効果を得ることができる。
【００９５】
このとき、センサ位置は基本的には任意であるが、センサ位置を工夫することにより、効率的に演算を行うことができる。例えば、ファーストアーム５であれば、ＬＳ１１Ｘ＝ＬＳ１２Ｘ、ＬＳ１１Ｙ＝−ＬＳ１２Ｙ、ＬＳ１１Ｚ＝ＬＳ１２Ｚのように配置するとよい。また、ＬＳ１１Ｚ＝ＬＳ１２Ｚ＝０とするとなおよい。セカンドアーム６も考慮すると、これらの関係は、以下の（４６）式から（４８）式のように表すことができる。
【００９６】
ＬＳｍｎＸ＝ＬＳｍｎＸ          ・・・（４６）
ＬＳｍｎＹ＝−ＬＳｍｎＹ        ・・・（４７）
ＬＳｍｎＺ＝ＬＳｍｎＺ（＝０）  ・・・（４８）
なお、ｎはセンサの数であり、ｍ＝１の場合がファーストアーム５、ｍ＝２の場合がセカンドアーム６になる。つまり、ｎ＝１、ｍ＝１であれば、例えばＬＳ１１Ｘがセンサ１０の配置（第１軸（Ｊ１）からの距離）を表し、ｎ＝２、ｍ＝１であれば、例えばＬＳ１２Ｙがセンサ１１の配置（直線（ＣＬ１）からの距離）を表している。
【００９７】
（第３実施形態）
以下、第３実施形態について説明する。第３実施形態では、上記した第１実施形態および第２実施形態で示したロボット制御装置において、手先に現れる振動を抑制するための手法について説明する。なお、構成については第１実施形態と共通するので、図１から図４も参照しながら説明する。
【００９８】
まず、上記した第１実施形態で説明した通り、水平多関節ロボットであるロボット２では、回転軸である１軸（Ｊ１）目および２軸（Ｊ２）目の動作に連動して、シャフト７（いか、Ｚ軸直動部とも称する）を支持するシャフト支持部６ａ（以下、Ｚ軸支持部とも称する）において捩れ振動が発生している。そして、この捩れ振動は、１軸目および２軸目の回転軸とＲｚ軸の回転だけでなく、Ｒｘ軸方向およびＲｙ軸方向にも発生している。
【００９９】
しかし、従来の一般的なロボットの場合には、Ｒｚ軸方向に関してはシャフト７の回転軸となる４軸目に角度センサとしてモータエンコーダが取り付けられているものの、Ｒｘ軸方向およびＲｙ軸方向にはその動作を検出できるセンサが設けられていなかった。そのため、Ｒｘ軸方向およびＲｙ軸方向の振動に関しては、捩れ振動そのもののを観測できないため、捩れ振動の発生を検知してその捩れ振動を抑制することが困難であった。
【０１００】
そして、ある回転軸で動作方向に受ける他の軸からの干渉力による捩れをその軸のモータ制御位置指令値の補正で抑制する手法等も提案されているが、上記したように、一般的な考え方では各軸において回転可能なのはＲｚ軸方向のみであり、Ｒｘ軸方向あるいはＲｙ軸方向には回転できないため、Ｒｘ軸方向およびＲｙ軸方向の捻れを補償することはできなかった。また、それ以外にも他の軸からの影響を補正する様々な方法が提案されているが、動作方向ではない非動作方向について検討されているものはなかった。
【０１０１】
また、非動作方向も含めた各関節部の弾性を考慮して制御位置指令値を修正する方法も提案されていたものの、共振特性については何ら考慮されておらず、共振振動（非動作方向共振）の抑制には効果がなかった。また、手先先端の位置に関してはそのような方法で精度が向上すること自体は見込めるものの、Ｒｘ軸方向あるいはＲｙ軸方向の慣性、弾性および剛性（バネ定数）から成る共振特性によって共振振動を励起してしまい、手先先端の姿勢、特にＲｘ軸方向あるいはＲｙ軸方向に対する姿勢が振動的になってしまうおそれがあった。この場合、一見すれば逆相に動かすことで振動抑制に効果が見込めそうに思われたものの、動作中には手先加速度による慣性力が非動作方向に直接作用し、動作終了後には手先加速度がゼロとなり慣性力を発生させなくなることから、共振振動の発生を抑制する効果も共振振動を減衰させる効果も得ることはできなかった。
【０１０２】
また、従来では、共振振動を減衰させるために、共振系のフィードバック制御における状態フィードバック制御などの方法により、共振特性に適切な減衰を与えて振動を速やかに収束することが行われている。また、直接的に共振特性の状態量が測定できない場合には、状態オブザーバなどの方法により推定した値を測定値の代わりに使うことが行われている。
しかし、水平多関節ロボットにおいてそのような状態フィードバックと状態オブザーバにより振動抑制制御を行うと、動作条件によって振動抑制効果が異なるということが問題となっていた。そして、検証のためにアームの回転方向の慣性モーメントに応じてフィードバックゲインを変動させてみたところ、振動抑制効果はあまり向上しないという結果が得られた。
【０１０３】
そのため、さらに発明者らが詳細に調査したところ、シャフト支持部６ａに起因する共振振動は、アームの回転方向に直交する方向の慣性モーメントに依存していること、つまりは、シャフト７のＺ軸方向における位置（以下、Ｚ軸高さと称する）によってその共振周波数および反共振周波数が変動することが明らかになった。このことは、従来のように動作方向だけを考慮してトルクと角加速度から慣性モーメントを求めるのではなく、非動作方向を含めた３次元空間上で考えなければ、効果的な振動抑制効果を得るためには不十分であるということを示している。
【０１０４】
そこで、本実施形態では、以下に説明するような状態フィードバック制御を用いることにより、非動作方向振動を抑制できるフィードバック制御を実現している。なお、このフィードバック制御は、制御部３ａによって実行されており、非動作方向の振動を抑制する制御方法を示すものでもある。
まず、１軸目の、アーム側からみたモータ角度をθＭ１、モータ慣性をＪＭ１、モータ粘性をＤＭ１、モータトルクをτＭ１、ギア剛性をＫＳ１、アーム角度をθＡ１、アーム慣性をＪＡ１、アーム粘性をＤＡ１とする。また、ファーストアーム支持部からシャフト支持部６ａまでの水平距離をＬＸＹ１、シャフト支持部６ａの剛性をＫＢ、シャフト支持部６ａの粘性をＤＢ、シャフト支持部６ａの捩れ角（捩れ角度に相当する）をθＢとする。また、シャフト７の重心位置での非動作方向、ここでは動作方向と直交する向きへの慣性モーメントをＪＢＣ、重量をＭＢ、シャフト支持部６ａから重心までの距離をＬＢＺとする。なお、ここでいうシャフト７の重心位置とは、シャフト７にツールやワーク等の負荷が取り付けられている場合には、それらシャフト７、ツールおよびワークを一体とみた場合における重心位置を意味している。
このとき、運動方程式は、下記の通りになる。
【０１０５】
【数１９】
2017056542.tif 000021
【０１０６】
また、このときの状態方程式は、下記の通りになる。
【０１０７】
【数２０】
2017056542.tif 000022
【０１０８】
【数２１】
2017056542.tif 000023
【０１０９】
【数２２】
2017056542.tif 000024
【０１１０】
この式から、行列Ｅにはシャフト支持部６ａから重心までの距離ＬＢＺが含まれており、シャフト７のＺ軸高さに応じてその共振特性が変動することが明らかである。
そして、上記した運動方程式で示されるシステムに対して状態フィードバックおよび状態オブザーバを構築すると、次の通りとなる。ただし、ｗはオブザーバ状態量である。また、このシステムのブロック線図を図５に示す。
【０１１１】
【数２３】
2017056542.tif 000025
【０１１２】
このとき、フィードバックゲインＫおよびオブザーバゲインＦは、極配置法などにより設計することができる。また、各ゲインはシャフト支持部６ａから重心までの距離ＬＢＺ応じて可変となるように設計することもできる。
とろこで、上記した例では剛性ＫＢはシャフト支持部６ａの剛性を用いたが、捩れ角θＢが十分に小さいとすればｓｉｎ（θ）＝θ、ｃｏｓ（θ）＝１と近似することができるため、シャフト７自体の剛性を等価的にシャフト支持部６ａの剛性と考えることができる。
【０１１３】
そのため、剛性ＫＢには、シャフト７自体の剛性を含めてもよい。この場合、シャフト支持部６ａの剛性をＫＢ０、シャフト７自体の剛性をＫＢ１、重心にかかる力をｆとすると、捩れ角θＢは、以下の（６３）式の通りとなる。また、シャフト７自体の剛性含めた場合の剛性ＫＢは、重心にかかる力ｆが重心の位置を変化させた際の変位量を求めることで、以下の（６４）式の通りとなる。
【０１１４】
【数２４】
2017056542.tif 000026
【０１１５】
これらの式から、シャフト支持部６ａの剛性ＫＢとしてシャフト７自体の剛性を含めた同姓を用いる場合であっても等価的に扱うことができることが、数式の上からも明らかである。
このとき、一般的な金属材料であればヤング率に対して引っ張り強度が１％以下であることから、換言すると、シャフト７自体に１％を超えるような捩れが発生した場合にはシャフト７が破断することから、上記した捩れ角θＢが十分に小さいという条件は、常に成立する。
【０１１６】
以上説明したように、本実施形態によれば、シャフト支持部６ａの共振特性を含めた状態空間モデルに対して制御系を設計することで、振動抑制に効果的なフィードバック制御を実施することができる。
具体的には、シャフト７の上下位置に応じて変化する当該シャフト７の重心位置に対する運動方程式によって予め定義され、シャフト支持部６ａの捩れ角度を生じさせる共振特性を含む状態空間モデルに対して、制御系を設計することにより、シャフト７の重心位置の変化による影響を加味した状態の状態空間モデルを定義できる。そして、その状態空間モデルに対して制御系を設計することで、つまりは、その制御系に対して振動抑制制御を行うことで、ロボット２の手先の振動を効果的に抑制することができる。
【０１１７】
また状態空間モデルに対して状態フィードバックおよび状態オブザーバを構築しているので、フィードバック制御により振動抑制制御を行うことができるとともに、状態オブザーバにより、直接的には物理量を取得できない手先の振動を推定しながら振動抑制制御を行うことができる。
また、状態方程式にシャフト７の重心位置における動作方向に直交する向きへの慣性モーメントを含めることで、動作方向とは異なる非動作方向への慣性モーメントを加味した状態で振動を抑制することができ、非動作方向振動を効果的に抑制することができる。
【０１１８】
このように、例えばアームの姿勢変化により共振周波数が変化する場合において動作方向に対応する慣性モーメントの変化のみが考慮されており、非動作方向の慣性モーメントが変化することについては何ら考慮されていなかった従来技術とは異なり、本実施形態の振動抑制方法であれば、非動作方向の慣性モーメントが変化することに対応した振動抑制制御を行うことができる。
また、シャフト７のＺ軸高さによって慣性モーメントが変化するという従来では考慮されていなかった事実に基づいて振動抑制制御を行うので、振動抑制効果をより一層高めることができる。
【０１１９】
（その他の実施形態）
本発明は、上記し且つ図面に記載した態様に限定されるものではなく、その要旨を逸脱しない範囲で種々の変形や拡張をすることができる。
実施形態で示した数値は一例であり、これに限定されない。
ファーストアーム５とセカンドアーム６のそれぞれに複数方向への角速度を検出可能な２軸角速度センサを設けたが、それ以外のセンサ設置方法でもよく、例えば加速度センサと角速度センサとを組み合わせてもよい。例えば、配線距離が長くなるセカンドアーム６には省配線化のために角速度センサを設け、ファーストアーム５には第２実施形態のように２つの加速度センサを設けることなどが考えられる。
【符号の説明】
【０１２０】
図面中、２はロボット、３は制御装置（ロボット制御装置）、３ａは制御部、４はベース、５はファーストアーム、６はセカンドアーム、６ａはシャフト支持部、７はシャフト、８、９、１０、１１、１２はセンサ（慣性センサ、角速度センサ、加速度センサ、回転角度センサ、位置センサ）を示す。
【図１】
2017056542.tif 000027
【図２】
2017056542.tif 000028
【図３】
2017056542.tif 000029
【図４】
2017056542.tif 000030
【図５】
2017056542.tif 000031