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