(57)【要約】【課題】第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができるロボット制御装置、ロボットおよびロボットシステムを提供すること。【解決手段】本発明のロボット制御装置は、軸まわりに回動可能な胴体と、前記胴体に設けられ、前記胴体に対して回動可能な第１、第２のロボットアームと、第１、第２、第３の慣性センサーとを備えるロボットの作動を制御する。このロボット制御装置は、前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、Ｂ／Ａ＜０．２７を満足させる。【選択図】図１
2015085426.tif 000002
【特許請求の範囲】
【請求項１】
軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、を備えるロボットの作動を制御するロボット制御装置であって、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足させることを特徴とするロボット制御装置。
Ｂ／Ａ＜０．２７      ・・・（１）
【請求項２】
前記第１のロボットアームは、前記胴体に設けられて第１回動軸まわりに回動可能な第１リンクと、
前記第１リンクに設けられて第２回動軸まわりに回動可能で、前記第２回動軸を前記第１回動軸と直交する回動軸または前記第１回動軸と直交する軸と平行な回動軸とする第２リンクと、
前記第２リンクに設けられて第３回動軸まわりに回動可能で、前記第３回動軸を前記第２回動軸と直交する回動軸または前記第２回動軸と直交する軸と平行な回動軸とする第３リンクと、
前記第３リンクに設けられて第４回動軸まわりに回動可能で、前記第４回動軸を前記第３回動軸と直交する回動軸または前記第３回動軸と直交する軸と平行な回動軸とする第４リンクと、を備え、
前記第１のロボットアームを静止状態から目標位置に移動させる動作では、前記第１のロボットアームの前記第３リンクの長手方向に延びる中心軸と前記第４リンクの長手方向に延びる中心軸とのなす角を９０°とし、前記第１のロボットアームの前記第３リンクを９０°回動させる請求項１に記載のロボット制御装置。
【請求項３】
前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足させる請求項１または２に記載のロボット制御装置。
Ｄ／Ｃ＜０．２７      ・・・（２）
【請求項４】
前記第２のロボットアームは、前記胴体に設けられて第１回動軸まわりに回動可能な第１リンクと、
前記第１リンクに設けられて第２回動軸まわりに回動可能で、前記第２回動軸を前記第１回動軸と直交する回動軸または前記第１回動軸と直交する軸と平行な回動軸とする第２リンクと、
前記第２リンクに設けられて第３回動軸まわりに回動可能で、前記第３回動軸を前記第２回動軸と直交する回動軸または前記第２回動軸と直交する軸と平行な回動軸とする第３リンクと、
前記第３リンクに設けられて第４回動軸まわりに回動可能で、前記第４回動軸を前記第３回動軸と直交する回動軸または前記第３回動軸と直交する軸と平行な回動軸とする第４リンクと、を備え、
前記第２のロボットアームを静止状態から目標位置に移動させる動作では、前記第２のロボットアームの前記第３リンクの長手方向に延びる中心軸と前記第４リンクの長手方向に延びる中心軸とのなす角を９０°とし、前記第２のロボットアームの前記第３リンクを９０°回動させる請求項３に記載のロボット制御装置。
【請求項５】
前記第１の慣性センサー、前記第２の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、前記胴体、前記第１のロボットアームおよび前記第２のロボットアームの振動を抑制しつつ、作動を制御する制御部を備える請求項１ないし４のいずれか１項に記載のロボット制御装置。
【請求項６】
前記制御部は、前記第１の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、第１の補正成分を求め、前記第１の補正成分をフィードバックして前記第１のロボットアームの振動を抑制し、
前記第２の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、第２の補正成分を求め、前記第２の補正成分をフィードバックして前記第２のロボットアームの振動を抑制し、
前記第３の慣性センサーの検出結果に基づいて、第３の補正成分を求め、前記第３の補正成分をフィードバックして前記胴体の振動を抑制し、
前記第３の補正成分のフィードバックゲインは、前記第１の補正成分のフィードバックゲインよりも大きい請求項５に記載のロボット制御装置。
【請求項７】
前記第３の補正成分のフィードバックゲインは、前記第２の補正成分のフィードバックゲインよりも大きい請求項６に記載のロボット制御装置。
【請求項８】
前記制御部は、前記第１のロボットアームと前記第２のロボットアームとの少なくとも一方の姿勢に応じて、前記第３の補正成分のフィードバックゲインを調整する請求項６または７に記載のロボット制御装置。
【請求項９】
前記制御部は、前記ロボットの姿勢が第１の姿勢のときと、前記第１の姿勢よりも前記ロボットの前記軸まわりの慣性モーメントが大きい第２の姿勢のときとでは、前記第２の姿勢のときの方が前記第１の姿勢のときよりも前記第３の補正成分のフィードバックゲインを大きくする請求項８に記載のロボット制御装置。
【請求項１０】
前記胴体の前記軸まわりの前記角速度は、前記第３の慣性センサーの検出結果に基づいて求められる請求項１ないし９のいずれか１項に記載のロボット制御装置。
【請求項１１】
軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、
を備え、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足することを特徴とするロボット。
Ｂ／Ａ＜０．２７      ・・・（１）
【請求項１２】
前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足する請求項１１に記載のロボット。
Ｄ／Ｃ＜０．２７      ・・・（２）
【請求項１３】
軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、を有するロボットと、
前記ロボットの作動を制御するロボット制御装置と、
を備え、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足することを特徴とするロボットシステム。
Ｂ／Ａ＜０．２７      ・・・（１）
【請求項１４】
前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足する請求項１３に記載のロボットシステム。
Ｄ／Ｃ＜０．２７      ・・・（２）
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置、ロボットおよびロボットシステムに関するものである。
【背景技術】
【０００２】
多関節のリンクで構成されたロボットアームを有するロボットが知られている。ロボットアームの先端部には、エンドエフェクターとして、例えば、ハンド等が装着される。そして、ロボットは、組み立て装置等の多くの装置に活用されている。
このようなロボットがロボットアームを移動させて停止するとき、ロボットアームが振動する。ロボットアームが振動している間は、ロボットアームの先端部に装着されたハンドも振動する。そして、ハンドが振動している間は、ハンドがワークを把持する作業等を行うことが困難であるので、ハンドの振動が止まるのを待つ必要がある。
【０００３】
そこで、ロボットの生産性を向上させるためにハンドが振動する時間を短縮させる方法が、特許文献１に開示されている。この特許文献１に記載されているロボットは、基台と、基台に連結されたロボットアームとを有するスカラーロボットであるが、それによると、ロボットアームの回転角度を検出する角度センサーをロボットアームのアクチュエーターに配置する。さらに、ロボットアームの振動を検出する角速度センサーをロボットアームに配置する。そして、ローパスフィルターを用いて角度センサーの低周波成分を抽出し、ハイパスフィルターを用いて角速度センサーの出力の高周波成分を抽出する。次に、角度センサーの出力と角速度センサーの出力とを合成してロボットアームの動作を検出する。そして、ロボットアームの動作に対応してロボットアームを制御することにより、ロボットアームの振動を抑制する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００５−２４２７９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１に記載の方法は、静止する基台に連結されたロボットアームの振動を減衰させる方法として有効な方法である。
しかしながら、基台と、基台に回動可能に連結された胴体と、胴体に回動可能に連結された２つのロボットアームとを有する双腕ロボットでは、ロボットアームのみならず、胴体も振動する。従って、前記のような双腕ロボットに対し、特許文献１に記載の方法を用いてロボットアームの振動を減衰させても胴体の振動がロボットアームに伝わるため、ロボットアームの振動が減衰し難いという問題がある。
本発明の目的は、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができるロボット制御装置、ロボットおよびロボットシステムを提供することにある。
【課題を解決するための手段】
【０００６】
このような目的は、下記の本発明により達成される。
（適用例１）
本発明に係わるロボット制御装置は、軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、を備えるロボットの作動を制御するロボット制御装置であって、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足させることを特徴とする。
Ｂ／Ａ＜０．２７      ・・・（１）
【０００７】
これにより、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。そして、Ｂ／Ａ＜０．２７とすることにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
具体的には、例えば、胴体がロボットアームの動作の反作用で振動することを抑制することができ、また、その振動を抑制する効果を高めることができる。これにより、動作するロボットアーム、胴体、動作するロボットアームと反対側のロボットアームの３者とも振動が抑制され、素早い位置決めが可能になる。
【０００８】
（適用例２）
本発明に係わるロボット制御装置では、前記第１のロボットアームは、前記胴体に設けられて第１回動軸まわりに回動可能な第１リンクと、
前記第１リンクに設けられて第２回動軸まわりに回動可能で、前記第２回動軸を前記第１回動軸と直交する回動軸または前記第１回動軸と直交する軸と平行な回動軸とする第２リンクと、
前記第２リンクに設けられて第３回動軸まわりに回動可能で、前記第３回動軸を前記第２回動軸と直交する回動軸または前記第２回動軸と直交する軸と平行な回動軸とする第３リンクと、
前記第３リンクに設けられて第４回動軸まわりに回動可能で、前記第４回動軸を前記第３回動軸と直交する回動軸または前記第３回動軸と直交する軸と平行な回動軸とする第４リンクと、を備え、
前記第１のロボットアームを静止状態から目標位置に移動させる動作では、前記第１のロボットアームの前記第３リンクの長手方向に延びる中心軸と前記第４リンクの長手方向に延びる中心軸とのなす角を９０°とし、前記第１のロボットアームの前記第３リンクを９０°回動させることが好ましい。
これにより、より確実に、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。
【０００９】
（適用例３）
本発明に係わるロボット制御装置では、前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足させることが好ましい。
Ｄ／Ｃ＜０．２７      ・・・（２）
これにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００１０】
（適用例４）
本発明に係わるロボット制御装置では、前記第２のロボットアームは、前記胴体に設けられて第１回動軸まわりに回動可能な第１リンクと、
前記第１リンクに設けられて第２回動軸まわりに回動可能で、前記第２回動軸を前記第１回動軸と直交する回動軸または前記第１回動軸と直交する軸と平行な回動軸とする第２リンクと、
前記第２リンクに設けられて第３回動軸まわりに回動可能で、前記第３回動軸を前記第２回動軸と直交する回動軸または前記第２回動軸と直交する軸と平行な回動軸とする第３リンクと、
前記第３リンクに設けられて第４回動軸まわりに回動可能で、前記第４回動軸を前記第３回動軸と直交する回動軸または前記第３回動軸と直交する軸と平行な回動軸とする第４リンクと、を備え、
前記第２のロボットアームを静止状態から目標位置に移動させる動作では、前記第２のロボットアームの前記第３リンクの長手方向に延びる中心軸と前記第４リンクの長手方向に延びる中心軸とのなす角を９０°とし、前記第２のロボットアームの前記第３リンクを９０°回動させることが好ましい。
これにより、より確実に、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。
【００１１】
（適用例５）
本発明に係わるロボット制御装置では、前記第１の慣性センサー、前記第２の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、前記胴体、前記第１のロボットアームおよび前記第２のロボットアームの振動を抑制しつつ、作動を制御する制御部を備えることが好ましい。
これにより、より確実に、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。
【００１２】
（適用例６）
本発明に係わるロボット制御装置では、前記制御部は、前記第１の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、第１の補正成分を求め、前記第１の補正成分をフィードバックして前記第１のロボットアームの振動を抑制し、
前記第２の慣性センサーおよび前記第３の慣性センサーの検出結果に基づいて、第２の補正成分を求め、前記第２の補正成分をフィードバックして前記第２のロボットアームの振動を抑制し、
前記第３の慣性センサーの検出結果に基づいて、第３の補正成分を求め、前記第３の補正成分をフィードバックして前記胴体の振動を抑制し、
前記第３の補正成分のフィードバックゲインは、前記第１の補正成分のフィードバックゲインよりも大きいことが好ましい。
これにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００１３】
（適用例７）
本発明に係わるロボット制御装置では、前記第３の補正成分のフィードバックゲインは、前記第２の補正成分のフィードバックゲインよりも大きいことが好ましい。
これにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００１４】
（適用例８）
本発明に係わるロボット制御装置では、前記制御部は、前記第１のロボットアームと前記第２のロボットアームとの少なくとも一方の姿勢に応じて、前記第３の補正成分のフィードバックゲインを調整することが好ましい。
これにより、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００１５】
（適用例９）
本発明に係わるロボット制御装置では、前記制御部は、前記ロボットの姿勢が第１の姿勢のときと、前記第１の姿勢よりも前記ロボットの前記軸まわりの慣性モーメントが大きい第２の姿勢のときとでは、前記第２の姿勢のときの方が前記第１の姿勢のときよりも前記第３の補正成分のフィードバックゲインを大きくすることが好ましい。
これにより、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００１６】
（適用例１０）
本発明に係わるロボット制御装置では、前記胴体の前記軸まわりの前記角速度は、前記第３の慣性センサーの検出結果に基づいて求められることが好ましい。
これにより、別途慣性センサーを用意することなく、容易かつ確実に、前記角速度を求めることができる。
【００１７】
（適用例１１）
本発明に係わるロボットは、軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、
とを備え、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足することを特徴とする。
Ｂ／Ａ＜０．２７      ・・・（１）
【００１８】
これにより、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。そして、Ｂ／Ａ＜０．２７とすることにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
具体的には、例えば、胴体がロボットアームの動作の反作用で振動することを抑制することができ、また、その振動を抑制する効果を高めることができる。これにより、動作するロボットアーム、胴体、動作するロボットアームと反対側のロボットアームの３者とも振動が抑制され、素早い位置決めが可能になる。
【００１９】
（適用例１２）
本発明に係わるロボットでは、前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足することが好ましい。
Ｄ／Ｃ＜０．２７      ・・・（２）
これにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【００２０】
（適用例１３）
本発明に係わるロボットシステムは、軸まわりに回動可能な胴体と、
前記胴体に設けられ、前記胴体に対して回動可能な第１のロボットアームおよび第２のロボットアームと、
前記第１のロボットアームに設けられた第１の慣性センサーと、
前記第２のロボットアームに設けられた第２の慣性センサーと、
前記胴体に設けられた第３の慣性センサーと、を有するロボットと、
前記ロボットの作動を制御するロボット制御装置と、
を備え、
前記第２のロボットアームを静止状態とし、前記第１のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第１のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＡ、前記第１のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＢとしたとき、下記（１）式を満足することを特徴とする。
Ｂ／Ａ＜０．２７      ・・・（１）
【００２１】
これにより、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制することができる。そして、Ｂ／Ａ＜０．２７とすることにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
具体的には、例えば、胴体がロボットアームの動作の反作用で振動することを抑制することができ、また、その振動を抑制する効果を高めることができる。これにより、動作するロボットアーム、胴体、動作するロボットアームと反対側のロボットアームの３者とも振動が抑制され、素早い位置決めが可能になる。
【００２２】
（適用例１４）
本発明に係わるロボットシステムでは、前記第１のロボットアームを静止状態とし、前記第２のロボットアームを静止状態から前記軸まわりに回動させて目標位置に移動させる動作において、前記第２のロボットアームが前記目標位置に到達する前の前記胴体の前記軸まわりの角速度の振幅の最大値をＣ、前記第２のロボットアームが最初に前記目標位置に達した後の前記胴体の前記軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足することが好ましい。
これにより、胴体の軸の剛性が等価的に高くなり、第１のロボットアーム、第２のロボットアームおよび胴体の振動を抑制する効果を高めることができる。
【図面の簡単な説明】
【００２３】
【図１】本発明のロボットシステムの実施形態を示す斜視図である。
【図２】図１に示すロボットシステムのロボットの回動軸を表す概略図である。
【図３】図１に示すロボットシステムのロボットに装着されるエンドエフェクターの位置を示す図である。
【図４】図１に示すロボットシステムのロボットの制御系を示すブロック図である。
【図５】図１に示すロボットシステムのロボットの駆動制御を示すブロック図である。
【図６】図１に示すロボットシステムのロボットの駆動制御を示すブロック図である。
【図７】図１に示すロボットシステムのロボットの駆動制御を示すブロック図である。
【図８】図１に示すロボットシステムのロボットの角速度の測定条件を説明するための図である。
【図９】図１に示すロボットシステムのロボットの角速度の測定条件を説明するための図である。
【図１０】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【図１１】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【図１２】図１に示すロボットシステムのロボットの胴体の角速度を示すグラフである。
【図１３】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【図１４】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【図１５】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【図１６】図１に示すロボットシステムのロボットのロボットアームの先端部および胴体の角速度を示すグラフである。
【発明を実施するための形態】
【００２４】
以下、本発明のロボット制御装置、ロボットおよびロボットシステムを添付図面に示す好適な実施形態に基づいて詳細に説明する。
図１は、本発明のロボットシステムの実施形態を示す斜視図である。図２は、図１に示すロボットシステムのロボットの回動軸を表す概略図である。図３は、図１に示すロボットシステムのロボットに装着されるエンドエフェクターの位置を示す図である。図４は、図１に示すロボットシステムのロボットの制御系を示すブロック図である。図５〜図７は、それぞれ、図１に示すロボットシステムのロボットの駆動制御を示すブロック図である。図８および図９は、それぞれ、図１に示すロボットシステムのロボットの角速度の測定条件を説明するための図である。図１０、図１１、図１３〜図１６は、それぞれ、図１に示すロボットのロボットシステムのロボットアームの先端部および胴体の角速度を示すグラフである。図１２は、図１に示すロボットシステムのロボットの胴体の角速度を示すグラフである。
【００２５】
なお、以下では、説明の都合上、図１中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１および図２中のロボットアームの胴体側を「基端」、その反対側を「先端」と言う。
また、図３に示すように、互いに直交するＸ軸、Ｙ軸、Ｚ軸を想定する。この場合、Ｚ軸と後述する回動軸Ｏ１とを平行にする。
【００２６】
図１に示すロボットシステム１００は、例えば腕時計のような精密機器等を製造する製造工程で用いることができ、ロボット２００と、ロボット２００の作動を制御するロボット制御装置（制御部）９００とを有している。ロボット２００と、ロボット制御装置９００とは、電気的に接続されている。なお、ロボット２００とロボット制御装置９００とは、一体であってもよく、また、別体であってもよい。以下、ロボット２００およびロボット制御装置９００について順に説明する。
【００２７】
（ロボット）
図１に示すように、ロボット２００は、双腕ロボットであり、ベース（基台）２１０と、ベース２１０に連結されている（設けられている）胴体２２０と、胴体２２０の左右に連結されている一対のロボットアーム（第１のロボットアーム）２３０、ロボットアーム（第２のロボットアーム）２４０と、胴体２２０に設けられているステレオカメラ２５０と、各ロボットアーム２３０、２４０に設けられているハンドカメラ（図示せず）と、胴体２２０に設けられている信号灯２６０と、胴体２２０の背面側に設けられたモニター２７０とを有している。
【００２８】
このようなロボットシステム１００によれば、ステレオカメラ２５０や前記ハンドカメラを用いて作業台上の部品、工具等の位置を確認しながら作業を行うことができる。また、信号灯２６０によって、ロボットシステム１００の状態（駆動状態、正常停止状態、異常停止状態等）を容易に確認することができる。また、モニター２７０にロボットシステム１００に関する情報が表示されるため、ロボットシステム１００の状態を簡単に確認することができる。モニター２７０は、例えば、タッチパネルになっており、タッチパネルを操作することによって、表示画面を切り替えたり、ロボットシステム１００に指令を与えたり、与えた指令を変更したりすることができる。
【００２９】
−ベース−
ベース２１０には、ロボットシステム１００の移動を容易とする複数の車輪（回転部材）と、各車輪をロックするロック機構（図示せず）と、ロボットシステム１００を移動する際に把持するハンドル（把持部）２１１とが設けられている。ロック機構を解除し、ハンドル２１１を把持して押したり引いたりすることで、ロボットシステム１００を自在に移動させることができ、ロック機構によって車輪をロックすることで、ロボットシステム１００を所定の位置で固定することができる。このように、ロボットシステム１００を移動容易とすることで、ロボットシステム１００の利便性が向上する。なお、車輪、ロック機構およびハンドル２１１は、それぞれ、省略してもよい。
【００３０】
また、ベース２１０には、図示しない作業台に当接させるためのバンパー２１３が設けられている。バンパー２１３を作業台の側面に当接させることによって、ロボットシステム１００を所定の間隔を隔てて作業台と向き合わせることができる。そのため、ロボットシステム１００と作業台との意図しない接触等を防止することができる。なお、バンパー２１３は、作業台に当接する当接部２１３ａと、ベース２１０に固定される固定部２１３ｂを有し、図１では、当接部２１３ａが固定部２１３ｂよりも下側に位置するようにベース２１０に装着されている。このようなバンパー２１３は、ベース２１０に対して着脱可能であり、バンパー２１３の向きを上下反転することができる。すなわち、図１とは反対に、当接部２１３ａが固定部２１３ｂよりも上方に位置するようにバンパー２１３をベース２１０に装着することもできる。生産現場では、一般的に高さ７００ｍｍ程度と１０００ｍｍ程度の作業台が用いられているため、当接部２１３ａの高さを変更することによって、高さの異なるいずれの作業台にも対応することが可能となる。
また、ベース２１０には、非常停止ボタン２１４が設けられており、非常時にはこの非常停止ボタン２１４を押すことによって、ロボットシステム１００を緊急停止させることができる。
【００３１】
−胴体−
図２に示すように、胴体２２０は、関節機構３１０を介して、ベース２１０に対して回動軸（軸）Ｏ１まわりに回動可能に連結されている。また、前述したように、胴体２２０にはステレオカメラ２５０と信号灯２６０とが設けられている。
関節機構３１０の構成としては、胴体２２０をベース２１０に対して回動軸Ｏ１まわりに回動させることができれば特に限定されないが、図４に示すように、駆動源としてのモーター３１１と、モーター３１１の回転速度を減じる減速機（図示せず）と、モーター３１１の回転角度を検知する位置センサー３１２とを有する構成としている。モーター３１１としては、例えば、ＡＣサーボモーター、ＤＣサーボモーター等のサーボモーターを用いることができ、減速機としては、例えば、遊星ギア型の減速機、ハーモニックドライブ（「ハーモニックドライブ」は登録商標）等を用いることができ、位置センサー３１２としては、例えば、エンコーダー、ロータリーエンコーダー、レゾルバー、ポテンショメーター等を用いることができる。
【００３２】
−ロボットアーム
図１および図２に示すように、ロボットアーム２３０は、関節機構４１０を介して胴体２２０に連結されている第１肩部（第１リンク）２３１と、関節機構４２０を介して第１肩部２３１に連結されている第２肩部（第２リンク）２３２と、捻り機構４３０を介して第２肩部２３２の先端に連結されている上腕部（第３リンク）２３３と、関節機構４４０を介して上腕部２３３の先端に連結されている第１前腕部（第４リンク）２３４と、捻り機構４５０を介して第１前腕部２３４の先端に連結されている第２前腕部（第５リンク）２３５と、関節機構４６０を介して第２前腕部２３５の先端に連結されている手首部（第６リンク）２３６と、捻り機構４７０を介して手首部２３６の先端に連結されている連結部（第７リンク）２３７とを有している。本実施形態では、上腕部２３３、第１前腕部２３４、第２前腕部２３５は、長手形状をなしている。また、連結部２３７にはハンド部２３８が設けられており、ハンド部２３８には、図３に示すように、ロボットシステム１００に実行させる作業に応じたエンドエフェクター６１０が装着される。なお、ハンド部２３８とエンドエフェクター６１０との間に力覚センサーが設けられていてもよい。力覚センサーを設けることにより、例えば、インピーダンス制御等の力制御を行うことができる。
【００３３】
また、図２に示すように、関節機構４１０は、第１肩部２３１を胴体２２０に対して回動軸Ｏ１と直交する回動軸（第１回動軸）Ｏ２まわりに回動させ、関節機構４２０は、第２肩部２３２を第１肩部２３１に対して回動軸Ｏ２に直交する回動軸（第２回動軸）Ｏ３まわりに回動させ、捻り機構４３０は、上腕部２３３を第２肩部２３２に対して回動軸Ｏ３に直交する回動軸（第３回動軸）Ｏ４まわりに回動させ（捻り）、関節機構４４０は、第１前腕部２３４を上腕部２３３に対して回動軸Ｏ４に直交する回動軸（第４回動軸）Ｏ５まわりに回動させ、捻り機構４５０は、第２前腕部２３５を第１前腕部２３４に対して回動軸Ｏ５に直交する回動軸（第５回動軸）Ｏ６まわりに回動させ（捻り）、関節機構４６０は、手首部２３６を第２前腕部２３５に対して回動軸Ｏ６に直交する回動軸（第６回動軸）Ｏ７まわりに回動させ、捻り機構４７０は、連結部２３７を手首部２３６に対して回動軸Ｏ７に直交する回動軸（第７回動軸）Ｏ８まわりに回動させる（捻る）。このようなロボットアーム２３０によれば、比較的簡単な構成によって、人間の腕部と同様に、関節（肩、肘、手首）の曲げ伸ばし、上腕および前腕の捻りを実現することができる。
【００３４】
なお、回動軸Ｏ２は、回動軸Ｏ１に直交する軸と平行であってもよく、回動軸Ｏ３は、回動軸Ｏ２に直交する軸と平行であってもよく、回動軸Ｏ４は、回動軸Ｏ３に直交する軸と平行であってもよく、回動軸Ｏ５は、回動軸Ｏ４に直交する軸と平行であってもよく、回動軸Ｏ６は、回動軸Ｏ５に直交する軸と平行であってもよく、回動軸Ｏ７は、回動軸Ｏ６に直交する軸と平行であってもよく、回動軸Ｏ８は、回動軸Ｏ７に直交する軸と平行であってもよい。
【００３５】
関節機構４１０、関節機構４２０、捻り機構４３０、関節機構４４０、捻り機構４５０、関節機構４６０および捻り機構４７０の構成としては、それぞれ、特に限定されないが、本実施形態では、前述した関節機構３１０と同様の構成となっている。すなわち、図４に示すように、関節機構４１０は、駆動源としてのモーター４１１と、モーター４１１の回転速度を減じる減速機（図示せず）と、モーター４１１の回転角度を検知する位置センサー４１２とを有している。また、関節機構４２０は、駆動源としてのモーター４２１と、モーター４２１の回転速度を減じる減速機（図示せず）と、モーター４２１の回転角度を検知する位置センサー４２２とを有している。また、捻り機構４３０は、駆動源としてのモーター４３１と、モーター４３１の回転速度を減じる減速機（図示せず）と、モーター４３１の回転角度を検知する位置センサー４３２とを有している。また、関節機構４４０は、駆動源としてのモーター４４１と、モーター４４１の回転速度を減じる減速機（図示せず）と、モーター４４１の回転角度を検知する位置センサー４４２とを有している。また、捻り機構４５０は、駆動源としてのモーター４５１と、モーター４５１の回転速度を減じる減速機（図示せず）と、モーター４５１の回転角度を検知する位置センサー４５２とを有している。また、関節機構４６０は、駆動源としてのモーター４６１と、モーター４６１の回転速度を減じる減速機（図示せず）と、モーター４６１の回転角度を検知する位置センサー４６２とを有している。また、捻り機構４７０は、駆動源としてのモーター４７１と、モーター４７１の回転速度を減じる減速機（図示せず）と、モーター４７１の回転角度を検知する位置センサー４７２とを有している。なお、各減速機は、それぞれ、省略されていてもよい。
【００３６】
ロボットアーム２４０は、前述のロボットアーム２３０と同様の構成である。すなわち、図１および図２に示すように、ロボットアーム２４０は、関節機構５１０を介して胴体２２０に連結されている第１肩部（第１リンク）２４１と、関節機構５２０を介して第１肩部２４１に連結されている第２肩部（第２リンク）２４２と、捻り機構５３０を介して第２肩部２４２の先端に連結されている上腕部（第３リンク）２４３と、関節機構５４０を介して上腕部２４３の先端に連結されている第１前腕部（第４リンク）２４４と、捻り機構５５０を介して第１前腕部２４４の先端に連結されている第２前腕部（第５リンク）２４５と、関節機構５６０を介して第２前腕部２４５の先端に連結されている手首部（第６リンク）２４６と、捻り機構５７０を介して手首部２４６の先端に連結されている連結部２４７（第７リンク）とを有している。本実施形態では、上腕部２４３、第１前腕部２４４、第２前腕部２４５は、長手形状をなしている。また、連結部２４７にはハンド部２４８が設けられており、ハンド部２４８には、ロボットシステム１００に実行させる作業に応じたエンドエフェクター６２０が装着される。なお、ハンド部２４８とエンドエフェクター６２０との間に力覚センサーが設けられていてもよい。力覚センサーを設けることにより、例えば、インピーダンス制御等の力制御を行うことができる。
【００３７】
また、図２に示すように、関節機構５１０は、第１肩部２４１を胴体２２０に対して回動軸Ｏ１に直交する回動軸（第１回動軸）Ｏ２’まわりに回動させ、関節機構５２０は、第２肩部２４２を第１肩部２４１に対して回動軸Ｏ２’に直交する回動軸（第２回動軸）Ｏ３’まわりに回動させ、捻り機構５３０は、上腕部２４３を第２肩部２４２に対して回動軸Ｏ３’に直交する回動軸（第３回動軸）Ｏ４’まわりに回動させ（捻り）、関節機構５４０は、第１前腕部２４４を上腕部２４３に対して回動軸Ｏ４’に直交する回動軸（第４回動軸）Ｏ５’まわりに回動させ、捻り機構５５０は、第２前腕部２４５を第１前腕部２４４に対して回動軸Ｏ５’に直交する（第５回動軸）回動軸Ｏ６’まわりに回動させ（捻り）、関節機構５６０は、手首部２４６を第２前腕部２４５に対して回動軸Ｏ６’に直交する回動軸（第６回動軸）Ｏ７’まわりに回動させ、捻り機構５７０は、連結部２４７を手首部２４６に対して回動軸Ｏ７’に直交する回動軸（第７回動軸）Ｏ８’まわりに回動させる（捻る）。このようなロボットアーム２３０によれば、比較的簡単な構成によって、人間の腕部と同様に、関節（肩、肘、手首）の曲げ伸ばし、上腕および前腕の捻りを実現することができる。
【００３８】
なお、回動軸Ｏ２’は、回動軸Ｏ１’に直交する軸と平行であってもよく、回動軸Ｏ３’は、回動軸Ｏ２’に直交する軸と平行であってもよく、回動軸Ｏ４’は、回動軸Ｏ３’に直交する軸と平行であってもよく、回動軸Ｏ５’は、回動軸Ｏ４’に直交する軸と平行であってもよく、回動軸Ｏ６’は、回動軸Ｏ５’に直交する軸と平行であってもよく、回動軸Ｏ７’は、回動軸Ｏ６’に直交する軸と平行であってもよく、回動軸Ｏ８’は、回動軸Ｏ７’に直交する軸と平行であってもよい。
【００３９】
関節機構５１０、関節機構５２０、捻り機構５３０、関節機構５４０、捻り機構５５０、関節機構５６０および捻り機構５７０の構成としては、それぞれ、特に限定されないが、本実施形態では、前述した関節機構３１０と同様の構成となっている。すなわち、図４に示すように、関節機構５１０は、駆動源としてのモーター５１１と、モーター５１１の回転速度を減じる減速機（図示せず）と、モーター５１１の回転角度を検知する位置センサー５１２とを有している。また、関節機構５２０は、駆動源としてのモーター５２１と、モーター５２１の回転速度を減じる減速機（図示せず）と、モーター５２１の回転角度を検知する位置センサー５２２とを有している。また、捻り機構５３０は、駆動源としてのモーター５３１と、モーター５３１の回転速度を減じる減速機（図示せず）と、モーター５３１の回転角度を検知する位置センサー５３２とを有している。また、関節機構５４０は、駆動源としてのモーター５４１と、モーター５４１の回転速度を減じる減速機（図示せず）と、モーター５４１の回転角度を検知する位置センサー５４２とを有している。また、捻り機構５５０は、駆動源としてのモーター５５１と、モーター５５１の回転速度を減じる減速機（図示せず）と、モーター５５１の回転角度を検知する位置センサー５５２とを有している。また、関節機構５６０は、駆動源としてのモーター５６１と、モーター５６１の回転速度を減じる減速機（図示せず）と、モーター５６１の回転角度を検知する位置センサー５６２とを有している。また、捻り機構５７０は、駆動源としてのモーター５７１と、モーター５７１の回転速度を減じる減速機（図示せず）と、モーター５７１の回転角度を検知する位置センサー５７２とを有している。なお、各減速機は、それぞれ、省略されていてもよい。
【００４０】
−エンドエフェクター−
ロボットアーム２３０、２４０の先端に取り付けられるエンドエフェクター６１０、６２０は、例えば、対象物を把持する機能を有している。エンドエフェクター６１０、６２０の構成は実行させる作業によって異なるが、例えば、図３に示すように、第１の指６１１、６２１と第２の指６１２、６２２を有する構成とすることができる。このような構成のエンドエフェクター６１０、６２０では、第１の指６１１、６２１と第２の指６１２、６２２の離間距離を調整することにより、対象物を把持することができる。
【００４１】
−角速度センサー
図１に示すように、ロボット２００には、角速度センサー（第３の慣性センサー）７１０と、角速度センサー（第１の慣性センサー）７２０と、角速度センサー（第２の慣性センサー）７３０とが設けられている。角速度センサー７１０は、胴体２２０に配置され、角速度センサー７２０は、ロボットアーム２３０に配置され、角速度センサー７３０は、ロボットアーム２４０に配置されている。これら３つの角速度センサー７１０、７２０、７３０は、それぞれ、互いに直交する３軸（ｘ軸、ｙ軸およびｚ軸）の各軸まわりの角速度ωｘ、ωｙ、ωｚを独立して検知することのできる３軸角速度センサーである。
ここで、ロボットアーム２３０、２４０における角速度センサー７２０、７３０の配置としては、特に限定されないが、本実施形態では、角速度センサー７２０、７３０は、それぞれ、連結部２３７、２４７に配置されている。
【００４２】
なお、角速度センサー７１０、７２０、７３０の姿勢は、特に限定されないが、角速度センサー７１０は、３つの検出軸、すなわちｘ軸、ｙ軸およびｚ軸のうちの１つ（例えば、ｚ軸）が、胴体２２０の回動軸Ｏ１と平行になるように胴体２２０に設けられていることが好ましい。また、角速度センサー７２０は、ｘ軸、ｙ軸およびｚ軸のうちの１つが、連結部２３７の回動軸Ｏ８と平行になるように連結部２３７に設けられていることが好ましい。また、角速度センサー７３０は、ｘ軸、ｙ軸およびｚ軸のうちの１つが、連結部２４７の回動軸Ｏ８’と平行になるように連結部２４７に設けられていることが好ましい。
【００４３】
角速度センサー７１０、７２０、７３０としては、それぞれ、互いに直交する３軸（ｘ軸、ｙ軸およびｚ軸）の各軸まわりの角速度ωｘ、ωｙ、ωｚを検出することができれば、特に限定されず、公知の角速度センサーを用いることができる。また、角速度センサー７１０、７２０、７３０を互いに同じ構成とすることによって資材コストを低減することができる。なお、角速度センサー７１０、７２０、７３０としては、３軸角速度センサーに限定されず、例えば、２軸角速度センサーや１軸角速度センサーであってもよい。
【００４４】
角速度センサー７１０、７２０、７３０により、それぞれ、角速度センサー７１０、７２０、７３０の設置個所における各軸まわりの角速度ωｘ、ωｙ、ωｚを検出する。この角速度センサー７１０、７２０、７３０の検出結果、すなわち、角速度センサー７１０、７２０、７３０から出力される信号は、ロボット制御装置９００に入力される。そして、ロボット制御装置９００は、その検出結果に基づいて、胴体２２０、ロボットアーム２３０、ロボットアーム２４０の振動を抑制する制御等の所定の制御を行う。
【００４５】
（ロボット制御装置）
ロボット制御装置９００は、胴体２２０、ロボットアーム２３０、２４０をそれぞれ独立して作動させることができる。言い換えると、ロボット制御装置９００は、モータードライバー等を介して、各関節機構３１０、４１０、４２０、４４０、４６０、５１０、５２０、５４０、５６０および各捻り機構４３０、４５０、４７０、５３０、５５０、５７０が備える各モーター３１１、４１１〜４７１、５１１〜５７１を独立して制御することができる。この場合、ロボット制御装置９００は、位置センサー３１２、４１２〜４７２、５１２〜５７２、角速度センサー７１０〜７３０により検出を行い、その検出結果に基づいて、各モーター３１１、４１１〜４７１、５１１〜５７１の駆動（例えば角速度や回動角度等）を制御する、すなわち、胴体２２０、ロボットアーム２３０、２４０の振動を抑制しつつ、その作動を制御する。この制御プログラムは、ロボット制御装置９００に内蔵された記憶部９３０に予め記憶されている。
【００４６】
具体的には、図４に示すように、ロボット制御装置９００は、モーター３１１の駆動を制御する第１駆動源制御部（制御部）９０１と、モーター４１１の駆動を制御する第２駆動源制御部（制御部）９０２と、モーター４２１の駆動を制御する第３駆動源制御部（制御部）９０３と、モーター４３１の駆動を制御する第４駆動源制御部（制御部）９０４と、モーター４４１の駆動を制御する第５駆動源制御部（制御部）９０５と、モーター４５１の駆動を制御する第６駆動源制御部（制御部）９０６と、モーター４６１の駆動を制御する第７駆動源制御部（制御部）９０７と、モーター４７１の駆動を制御する第８駆動源制御部（制御部）９０８と、モーター５１１の駆動を制御する第９駆動源制御部（制御部）９０９と、モーター５２１の駆動を制御する第１０駆動源制御部（制御部）９１０と、モーター５３１の駆動を制御する第１１駆動源制御部（制御部）９１１と、モーター５４１の駆動を制御する第１２駆動源制御部（制御部）９１２と、モーター５５１の駆動を制御する第１３駆動源制御部（制御部）９１３と、モーター５６１の駆動を制御する第１４駆動源制御部（制御部）９１４と、モーター５７１の駆動を制御する第１５駆動源制御部（制御部）９１５とを有している。
第１〜第１５駆動源制御部９０１〜９１５の構成は、互いに同様であるため、以下では、第１駆動源制御部９０１、第２駆動源制御部９０２、第９駆動源制御部９０９について代表して説明し、その他の第３〜第８、第１０〜第１５駆動源制御部９０３〜９０８、９１０〜９１５についてはその説明を省略する。
【００４７】
図５に示すように、第１駆動源制御部９０１は、減算器９０１ａと、位置制御部９０１ｂと、減算器９０１ｃと、角速度制御部９０１ｄと、回動角度算出部９０１ｅと、角速度算出部９０１ｆと、減算器９０１ｇと、変換部９０１ｈと、補正値算出部９０１ｉと、加算器９０１ｊと、フィードバックゲイン調整部９０１ｋ、角速度算出部９０１ｍとを有している。なお、角速度算出部９０１ｍおよび角速度センサー７１０により、胴体２２０の回動軸Ｏ１のまわりの角速度を検出する角速度検出部が構成される。
【００４８】
ここで、ロボット制御部９００は、ロボットシステム１００が行う処理の内容に基づいてエンドエフェクター６１０（または６２０）の目標位置を演算し、その目標位置にエンドエフェクター６１０（または６２０）を移動させるための軌道を生成する。そして、ロボット制御部９００は、その生成した軌道に沿ってエンドエフェクター６１０（または６２０）が移動するように、各駆動源４０１〜４０７の回転角度を所定の制御周期ごとに測定し、この測定結果に基づいて演算した値をそれぞれ各モーターの位置指令Ｐｃとして各モーターに出力する。なお、前記および以下では、「値が入力、出力」等と表記しているが、これは、「その値に対応する信号が入力、出力」の意味である。
【００４９】
図５に示すように、第１駆動源制御部９０１には、モーター３１１の位置指令Ｐｃの他、位置センサー３１２、角速度センサー７１０からそれぞれ検出信号が入力される。第１駆動源制御部９０１は、位置センサー３１２の検出信号から算出されるモーター３１１の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によってモーター３１１を駆動する。
【００５０】
すなわち、第１駆動源制御部９０１の減算器９０１ａには、位置指令Ｐｃが入力され、また、回動角度算出部９０１ｅから後述する位置フィードバック値Ｐｆｂが入力される。回動角度算出部９０１ｅでは、位置センサー３１２から入力されるパルス数がカウントされるとともに、そのカウント値に応じたモーター３１１の回転角度が位置フィードバック値Ｐｆｂとして減算器９０１ａに出力される。減算器９０１ａは、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差（モーター３１１の回転角度の目標値から位置フィードバック値Ｐｆｂを減算した値）を位置制御部９０１ｂに出力する。
【００５１】
位置制御部９０１ｂは、減算器９０１ａから入力された偏差と、予め定められた係数である比例ゲイン等を用いた所定の演算処理を行うことで、その偏差に応じたモーター３１１の角速度の目標値を演算する。位置制御部９０１ｂは、そのモーター３１１の角速度の目標値（指令値）を示す信号を角速度指令ωｃとして減算器９０１ｃに出力する。なお、ここでは、本実施形態では、フィードバック制御として、比例制御（Ｐ制御）がなされるが、これに限定されるものではない。
減算器９０１ｃには、角速度指令ωｃが入力され、また、後述する角速度フィードバック値ωｆｂが入力される。減算器９０１ｃは、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（モーター３１１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部９０１ｄに出力する。
【００５２】
角速度制御部９０１ｄは、減算器９０１ｃから入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じたモーター３１１の駆動信号（駆動電流に比例）を生成し、モータードライバーを介してモーター３１１に供給する。なお、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、モーター３１１の駆動電流が制御される。
【００５３】
次に、第１駆動源制御部９０１における角速度フィードバック値ωｆｂについて説明する。
角速度算出部９０１ｆでは、位置センサー３１２から入力されるパルス信号の周波数に基づいて、モーター３１１の角速度ωｍ１が算出され、その角速度ωｍ１は、加算器９０１ｊに出力される。
【００５４】
また、角速度算出部９０１ｆでは、位置センサー３１２から入力されるパルス信号の周波数に基づいて、胴体２２０の回動軸Ｏ１のまわりの角速度ωＡ１ｍが算出され、その角速度ωＡ１ｍは、減算器９０１ｇに出力される。なお、角速度ωＡ１ｍは、角速度ωｍ１を、モーター３１１と胴体２２０との間、すなわち、関節機構３１０における減速比で除算した値である。
また、角速度センサー７１０により、各検出軸のまわりの角速度が検出され、その検出値は、角速度算出部９０１ｍに出力される。角速度算出部９０１ｍでは、前記検出値に基づいて、胴体２２０の回動軸Ｏ１のまわりの角速度ωＡ１が算出され、その角速度ωＡ１は、減算器９０１ｇに出力される。
【００５５】
減算器９０１ｇには、角速度ωＡ１および角速度ωＡ１ｍが入力され、減算器９０１ｇは、この角速度ωＡ１から角速度ωＡ１ｍを減算した値ωＡ１ｓ（＝ωＡ１−ωＡ１ｍ）を変換部９０１ｈに出力する。この値ωＡ１ｓは、胴体２２０の回動軸Ｏ１のまわりの角速度の振動成分（振動角速度）に相当する。以下、ωＡ１ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ１ｓ（詳細には、振動角速度ωＡ１ｓに基づいて生成した値であるモーター３１１における角速度ωｍ１ｓ）が後述するゲインＫｃ倍されてモーター３１１の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ１ｓが可及的に０になるように、モーター３１１に対してフィードバック制御がなされる。これにより、胴体２２０の振動を抑制することができ、これによってロボットシステム１００の振動を抑制することができる。なお、このフィードバック制御では、モーター３１１の角速度が制御される。
【００５６】
変換部９０１ｈは、振動角速度ωＡ１ｓをモーター３１１における角速度ωｍ１ｓに変換し、その角速度ωｍ１ｓを補正値算出部９０１ｉに出力する。この変換は、振動角速度ωＡ１ｓに、モーター３１１と胴体２２０との間、すなわち、関節機構３１０における減速比を乗算することで得ることができる。
補正値算出部９０１ｉは、角速度ωｍ１ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋｃを乗算し、補正値（第３の補正成分）Ｋｃ・ωｍ１ｓを求め、その補正値Ｋｃ・ωｍ１ｓを加算器９０１ｊに出力する。ゲインＫｃは、フィードバックゲイン調整部９０１ｋにより調整される。なお、このゲインＫｃの調整については、後で詳述する。
【００５７】
加算器９０１ｊには、角速度ωｍ１が入力され、また、補正値Ｋｃ・ωｍ１ｓが入力される。加算器９０１ｊは、角速度ωｍ１と補正値Ｋｃ・ωｍ１ｓとの加算値を角速度フィードバック値ωｆｂとして減算器９０１ｃに出力する。なお、以降の動作は、前述した通りである。
前述したように第２〜第８駆動源制御部９０２〜９０８の構成は、互いに同様であるため、代表的に、第２駆動源制御部９０２について説明する。
【００５８】
図６に示すように、第２駆動源制御部９０２は、減算器９０２ａと、位置制御部９０２ｂと、減算器９０２ｃと、角速度制御部９０２ｄと、回動角度算出部９０２ｅと、角速度算出部９０２ｆと、減算器９０２ｇと、変換部９０２ｈと、補正値算出部９０２ｉと、加算器９０２ｊと、フィードバックゲイン調整部９０２ｋ、角速度算出部９０２ｍとを有している。なお、角速度算出部９０２ｍおよび角速度センサー７２０により、第１肩部２３１の回動軸Ｏ２のまわりの角速度を検出する角速度検出部が構成される。
【００５９】
第２駆動源制御部９０２には、モーター４１１の位置指令Ｐｃの他、位置センサー４１２、角速度センサー７２０からそれぞれ検出信号が入力される。第２駆動源制御部９０２は、位置センサー４１２の検出信号から算出されるモーター４１１の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によってモーター４１１を駆動する。
【００６０】
すなわち、第２駆動源制御部９０２の減算器９０２ａには、位置指令Ｐｃが入力され、また、回動角度算出部９０２ｅから後述する位置フィードバック値Ｐｆｂが入力される。回動角度算出部９０２ｅでは、位置センサー４１２から入力されるパルス数がカウントされるとともに、そのカウント値に応じたモーター４１１の回転角度が位置フィードバック値Ｐｆｂとして減算器９０２ａに出力される。減算器９０２ａは、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差（モーター４１１の回転角度の目標値から位置フィードバック値Ｐｆｂを減算した値）を位置制御部９０２ｂに出力する。
【００６１】
位置制御部９０２ｂは、減算器９０２ａから入力された偏差と、予め定められた係数である比例ゲイン等を用いた所定の演算処理を行うことで、その偏差に応じたモーター４１１の角速度の目標値を演算する。位置制御部９０２ｂは、そのモーター４１１の角速度の目標値（指令値）を示す信号を角速度指令ωｃとして減算器９０２ｃに出力する。なお、ここでは、本実施形態では、フィードバック制御として、比例制御（Ｐ制御）がなされるが、これに限定されるものではない。
減算器９０２ｃには、角速度指令ωｃが入力され、また、後述する角速度フィードバック値ωｆｂが入力される。減算器９０２ｃは、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（モーター４１１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部９０２ｄに出力する。
【００６２】
角速度制御部９０２ｄは、減算器９０２ｃから入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じたモーター４１１の駆動信号（駆動電流に比例）を生成し、モータードライバーを介してモーター４１１に供給する。なお、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、モーター４１１の駆動電流が制御される。
【００６３】
次に、第１駆動源制御部９０２における角速度フィードバック値ωｆｂについて説明する。
角速度算出部９０２ｆでは、位置センサー４１２から入力されるパルス信号の周波数に基づいて、モーター４１１の角速度ωｍ２が算出され、その角速度ωｍ２は、加算器９０２ｊに出力される。
【００６４】
また、角速度算出部９０２ｆでは、位置センサー４１２から入力されるパルス信号の周波数に基づいて、第１肩部２３１の回動軸Ｏ２のまわりの角速度ωＡ２ｍが算出され、その角速度ωＡ２ｍは、減算器９０２ｇに出力される。なお、角速度ωＡ２ｍは、角速度ωｍ２を、モーター４１１と第１肩部２３１との間、すなわち、関節機構４１０における減速比で除算した値である。
また、角速度センサー７２０により、各検出軸のまわりの角速度が検出され、その検出値は、角速度算出部９０２ｍに出力される。角速度算出部９０２ｍでは、前記検出値に基づいて、第１肩部２３１の回動軸Ｏ２のまわりの角速度ωＡ２が算出され、その角速度ωＡ２は、減算器９０２ｇに出力される。
【００６５】
減算器９０２ｇには、角速度ωＡ２および角速度ωＡ２ｍが入力され、減算器９０２ｇは、この角速度ωＡ２から角速度ωＡ２ｍを減算した値ωＡ２ｓ（＝ωＡ２−ωＡ２ｍ）を変換部９０２ｈに出力する。この値ωＡ２ｓは、第１肩部２３１の回動軸Ｏ２のまわりの角速度の振動成分（振動角速度）に相当する。以下、ωＡ２ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ２ｓ（詳細には、振動角速度ωＡ２ｓに基づいて生成した値であるモーター４１１における角速度ωｍ２ｓ）が後述するゲインＫａ倍されてモーター４１１の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ２ｓが可及的に０になるように、モーター４１１に対してフィードバック制御がなされる。これにより、第１肩部２３１の振動を抑制することができ、これによってロボットシステム１００の振動を抑制することができる。なお、このフィードバック制御では、モーター４１１の角速度が制御される。
【００６６】
変換部９０２ｈは、振動角速度ωＡ２ｓをモーター４１１における角速度ωｍ２ｓに変換し、その角速度ωｍ２ｓを補正値算出部９０２ｉに出力する。この変換は、振動角速度ωＡ２ｓに、モーター４１１と第１肩部２３１との間、すなわち、関節機構４１０における減速比を乗算することで得ることができる。
補正値算出部９０２ｉは、角速度ωｍ２ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第１の補正成分）Ｋａ・ωｍ２ｓを求め、その補正値Ｋａ・ωｍ２ｓを加算器９０２ｊに出力する。ゲインＫａは、フィードバックゲイン調整部９０２ｋにより調整される。なお、このゲインＫａの調整については、後で詳述する。
加算器９０２ｊには、角速度ωｍ２が入力され、また、補正値Ｋａ・ωｍ２ｓが入力される。加算器９０２ｊは、角速度ωｍ２と補正値Ｋａ・ωｍ２ｓとの加算値を角速度フィードバック値ωｆｂとして減算器９０２ｃに出力する。なお、以降の動作は、前述した通りである。
【００６７】
なお、ここで、前記角速度センサー７１０の検出値に基づいて算出される胴体２２０の回動軸Ｏ１のまわりの角速度ωＡ１は、Ｚ軸まわりの角速度であり、そのＺ軸まわりの角速度をωｗｚとする。また、第２駆動源制御部９０２の制御動作において、角速度センサー７２０の検出値に基づいて算出される第１肩部２３１のＸ軸まわりの角速度をωａｘ、Ｙ軸まわりの角速度をωａｙ、Ｚ軸まわりの角速度をωａｚとすると、前記Ｚ軸まわりの角速度度ωａｚには、前記胴体２２０のＺ軸まわりの角速度ωｗｚが含まれている。このため、第１肩部２３１については、（ωａｚ−ωｗｚ）を真のＺ軸まわりの角速度として取り扱う。なお、前記Ｘ軸まわりの角速度ωａｘおよび前記Ｙ軸まわりの角速度ωａｙについては、その値を用いる。このように、第１肩部２３１についてのＺ軸まわりの角速度についての第１の補正成分は、角速度センサー７１０および７２０の検出結果に基づいて求められ、Ｘ軸まわりの角速度およびＹ軸まわりの角速度についての第１の補正成分は、角速度センサー７２０の検出結果に基づいて求められる。
前述したように第９〜第１５駆動源制御部９０９〜９１５の構成は、互いに同様であるため、代表的に、第９駆動源制御部９０９について説明する。
【００６８】
図７に示すように、第９駆動源制御部９０９は、減算器９０９ａと、位置制御部９０９ｂと、減算器９０９ｃと、角速度制御部９０９ｄと、回動角度算出部９０９ｅと、角速度算出部９０９ｆと、減算器９０９ｇと、変換部９０９ｈと、補正値算出部９０９ｉと、加算器９０９ｊと、フィードバックゲイン調整部９０９ｋ、角速度算出部９０９ｍとを有している。なお、角速度算出部９０９ｍおよび角速度センサー７３０により、第１肩部２４１の回動軸Ｏ２’のまわりの角速度を検出する角速度検出部が構成される。
【００６９】
第９駆動源制御部９０９には、モーター５１１の位置指令Ｐｃの他、位置センサー５１２、角速度センサー７３０からそれぞれ検出信号が入力される。第９駆動源制御部９０９は、位置センサー５１２の検出信号から算出されるモーター５１１の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によってモーター５１１を駆動する。
【００７０】
すなわち、第９駆動源制御部９０９の減算器９０９ａには、位置指令Ｐｃが入力され、また、回動角度算出部９０９ｅから後述する位置フィードバック値Ｐｆｂが入力される。回動角度算出部９０９ｅでは、位置センサー５１２から入力されるパルス数がカウントされるとともに、そのカウント値に応じたモーター５１１の回転角度が位置フィードバック値Ｐｆｂとして減算器９０９ａに出力される。減算器９０９ａは、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差（モーター５１１の回転角度の目標値から位置フィードバック値Ｐｆｂを減算した値）を位置制御部９０９ｂに出力する。
【００７１】
位置制御部９０９ｂは、減算器９０９ａから入力された偏差と、予め定められた係数である比例ゲイン等を用いた所定の演算処理を行うことで、その偏差に応じたモーター５１１の角速度の目標値を演算する。位置制御部９０９ｂは、そのモーター５１１の角速度の目標値（指令値）を示す信号を角速度指令ωｃとして減算器９０９ｃに出力する。なお、ここでは、本実施形態では、フィードバック制御として、比例制御（Ｐ制御）がなされるが、これに限定されるものではない。
減算器９０９ｃには、角速度指令ωｃが入力され、また、後述する角速度フィードバック値ωｆｂが入力される。減算器９０９ｃは、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（モーター５１１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部９０９ｄに出力する。
【００７２】
角速度制御部９０９ｄは、減算器９０９ｃから入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じたモーター５１１の駆動信号（駆動電流に比例）を生成し、モータードライバーを介してモーター５１１に供給する。なお、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、モーター５１１の駆動電流が制御される。
【００７３】
次に、第１駆動源制御部９０９における角速度フィードバック値ωｆｂについて説明する。
角速度算出部９０９ｆでは、位置センサー５１２から入力されるパルス信号の周波数に基づいて、モーター５１１の角速度ωｍ９が算出され、その角速度ωｍ９は、加算器９０９ｊに出力される。
【００７４】
また、角速度算出部９０９ｆでは、位置センサー５１２から入力されるパルス信号の周波数に基づいて、第１肩部２４１の回動軸Ｏ２’のまわりの角速度ωＡ９ｍが算出され、その角速度ωＡ９ｍは、減算器９０９ｇに出力される。なお、角速度ωＡ９ｍは、角速度ωｍ９を、モーター５１１と第１肩部２４１との間、すなわち、関節機構５１０における減速比で除算した値である。
また、角速度センサー７３０により、各検出軸のまわりの角速度が検出され、その検出値は、角速度算出部９０９ｍに出力される。角速度算出部９０９ｍでは、前記検出値に基づいて、第１肩部２４１の回動軸Ｏ２’のまわりの角速度ωＡ９が算出され、その角速度ωＡ９は、減算器９０９ｇに出力される。
【００７５】
減算器９０９ｇには、角速度ωＡ９および角速度ωＡ９ｍが入力され、減算器９０９ｇは、この角速度ωＡ９から角速度ωＡ９ｍを減算した値ωＡ９ｓ（＝ωＡ９−ωＡ９ｍ）を変換部９０９ｈに出力する。この値ωＡ９ｓは、第１肩部２４１の回動軸Ｏ２’のまわりの角速度の振動成分（振動角速度）に相当する。以下、ωＡ９ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ９ｓ（詳細には、振動角速度ωＡ９ｓに基づいて生成した値であるモーター５１１における角速度ωｍ９ｓ）が後述するゲインＫｂ倍されてモーター５１１の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ９ｓが可及的に０になるように、モーター５１１に対してフィードバック制御がなされる。これにより、第１肩部２４１の振動を抑制することができ、これによってロボットシステム１００の振動を抑制することができる。なお、このフィードバック制御では、モーター５１１の角速度が制御される。
【００７６】
変換部９０９ｈは、振動角速度ωＡ９ｓをモーター５１１における角速度ωｍ９ｓに変換し、その角速度ωｍ９ｓを補正値算出部９０９ｉに出力する。この変換は、振動角速度ωＡ９ｓに、モーター５１１と第１肩部２４１との間、すなわち、関節機構５１０における減速比を乗算することで得ることができる。
補正値算出部９０９ｉは、角速度ωｍ９ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋｂを乗算し、補正値（第２の補正成分）Ｋｂ・ωｍ９ｓを求め、その補正値Ｋｂ・ωｍ９ｓを加算器９０９ｊに出力する。ゲインＫｂは、フィードバックゲイン調整部９０９ｋにより調整される。なお、このゲインＫｂの調整については、後で詳述する。
加算器９０９ｊには、角速度ωｍ９が入力され、また、補正値Ｋｂ・ωｍ９ｓが入力される。加算器９０９ｊは、角速度ωｍ９と補正値Ｋｂ・ωｍ９ｓとの加算値を角速度フィードバック値ωｆｂとして減算器９０９ｃに出力する。なお、以降の動作は、前述した通りである。
【００７７】
なお、ここで、第９駆動源制御部９０９の制御動作において、角速度センサー７３０の検出値に基づいて算出される第１肩部２４１のＸ軸まわりの角速度をωａｘ’、Ｙ軸まわりの角速度をωａｙ’、Ｚ軸まわりの角速度をωａｚ’とすると、前記Ｚ軸まわりの角速度度ωａｚ’には、前記胴体２２０のＺ軸まわりの角速度ωｗｚが含まれている。このため、第１肩部２４１については、（ωａｚ’−ωｗｚ）を真のＺ軸まわりの角速度として取り扱う。なお、前記Ｘ軸まわりの角速度ωａｘ’および前記Ｙ軸まわりの角速度ωａｙ’については、その値を用いる。このように、第１肩部２４１についてのＺ軸まわりの角速度についての第２の補正成分は、角速度センサー７１０および７３０の検出結果に基づいて求められ、Ｘ軸まわりの角速度およびＹ軸まわりの角速度についての第２の補正成分は、角速度センサー７３０の検出結果に基づいて求められる。
【００７８】
ここで、このロボットシステム１００では、第３の補正成分のゲインＫｃは、第１の補正成分のゲインＫａおよび第２の補正成分のゲインＫｂよりも大きいことが好ましい。すなわち、Ｋｃ／Ｋａは、１よりも大きいことが好ましく、１．５以上であることがより好ましく、１．５以上、５以下であることがさらに好ましい。また、Ｋｃ／Ｋｂは、１よりも大きいことが好ましく、１．５以上であることがより好ましく、１．５以上、５以下であることがさらに好ましい。これにより、胴体２２０の回動軸Ｏ１の剛性が等価的に高くなり、ロボットアーム２３０、２４０、胴体２２０の振動を抑制する効果を高めることができる。なお、Ｋｃ／ＫａとＫｃ／Ｋｂは、同一でもよく、また、異なっていてもよい。
【００７９】
また、ロボット制御装置９００は、ロボットアーム２３０と、ロボットアーム２４０とのいずれか一方または両方の姿勢に応じて、ゲインＫｃを調整することが好ましい。
具体的には、ロボット２００の姿勢が第１の姿勢のときと、その第１の姿勢よりもロボット２００の回動軸Ｏ１まわりの慣性モーメントが大きい第２の姿勢のときとでは、第２の姿勢のときの方が第１の姿勢のときよりもゲインＫｃを大きくすることが好ましい。すなわち、ロボット２００の回動軸Ｏ１まわりの慣性モーメントが大きいほど、ゲインＫｃを大きくすることが好ましい。
これにより、ロボットアーム２３０、２４０、胴体２２０の振動を抑制する効果を高めることができる。
【００８０】
また、このロボットシステム１００では、ロボットアーム２４０を静止状態とし、ロボットアーム２３０を静止状態から回動軸Ｏ１まわりに回動させて目標位置に移動させる動作において、角速度センサー７１０の検出結果に基づいて求められ、ロボットアーム２３０が目標位置に到達する前の胴体２２０の回動軸Ｏ１まわりの角速度の振幅の最大値をＡ（図１２参照）、ロボットアーム２３０が最初に目標位置に達した後の胴体２２０の回動軸Ｏ１まわりの角速度の振幅の最大値をＢ（図１２参照）としたとき、下記（１）式を満足する。なお、前記ロボットアーム２３０を静止状態から目標位置に移動させる動作では、図８に示すようにロボットアーム２３０の上腕部２３３の長手方向に延びる中心軸（長手軸）２３３１（回動軸Ｏ４）と第１前腕部２３４の長手方向に延びる中心軸（長手軸）２３４１とのなす角を９０°とし、また、回動軸Ｏ１と回動軸Ｏ４とを平行にした状態で、ロボットアームの２３０の上腕部２３３を回動軸Ｏ４まわりに９０°回動させる。また、前記ロボットアーム２３０を静止状態から目標位置に移動させる動作では、静止状態のロボットアーム２４０および胴体２２０についても前述した振動を抑制する制御を行う。
Ｂ／Ａ＜０．２７      ・・・（１）
【００８１】
これにより、胴体２２０の回動軸Ｏ１の剛性が等価的に高くなり、ロボットアーム２３０を作動させる際のロボットアーム２３０、２４０および胴体２２０の振動を抑制する効果を高めることができる。
なお、Ｂ／Ａ＜０．１６であることがより好ましい。
ここで、前記「静止状態」とは、ロボットアーム２４０を積極的には移動させない状態であり、ロボットアーム２４０が完全に静止している場合のみならず、例えば、ロボットアーム２４０に振動が生じている状態や、ロボットアーム２４０が振動を抑制するために動作している状態等も含まれる。
【００８２】
例えば、ロボットシステム１００では、ティーチングペンダントを使って、以下のようなティーティングＪＯＢが生成さる。このＪＯＢで、ＰＯＳＩＴＩＯＮ／１、ＰＯＳＩＴＩＯＮ／３の速度が０であっても振動している場合があるが、それも静止している（静止状態）という。
ＰＯＳＩＴＩＯＮ／１：ユーザーがロボットを動かしてＰＯＳＩＴＩＯＮ／１に持っていき決定ボタンを押す
ＶＥＲＯＣＩＴＹ／０：速度入力
ＬＩＮＥＡＲ：ＰＯＳＩＴＩＯＮ／１からＰＯＳＩＴＩＯＮ／２まで直線で動く（動きも入力）
ＰＯＳＩＴＩＯＮ／２：
ＶＥＲＯＣＩＴＹ／ＭＩＤＤＬＥ：
ＬＩＮＥＡＲ：ＰＯＳＩＴＩＯＮ／２からＰＯＳＩＴＩＯＮ／３まで直線で動く
ＰＯＳＩＴＩＯＮ／３：
ＶＥＲＯＣＩＴＹ／０：
なお、ＰＯＳＩＴＩＯＮ／１が静止している動作開始点で、ＰＯＳＩＴＩＯＮ／３が停止する動作終了点である。
【００８３】
また、前記「胴体２２０の回動軸Ｏ１まわりの角速度の振幅」とは、図１２に示すように、グラフの横軸に時間、縦軸に胴体２２０の回動軸Ｏ１まわりの角速度をとったきのそのグラフの波形の振幅（例えば、図１２中のＡ、Ｂ）である。
また、前記角速度は、各角速度センサー７１０、７２０、７３０として、それぞれ、セイコーエプソン社製の６軸慣性センサー「ＡＨ−６１２０ＬＲ」を用い、角速度センサー７１０で測定した値である。
【００８４】
同様に、第１のロボットアームを静止状態とし、第２のロボットアームを静止状態から軸まわりに回動させて目標位置に移動させる動作において、第３の慣性センサーの検出結果に基づいて求められ、第２のロボットアームが目標位置に到達する前の胴体の軸まわりの角速度の振幅の最大値をＣ、第２のロボットアームが最初に前記目標位置に達した後の胴体の軸まわりの角速度の振幅の最大値をＤとしたとき、下記（２）式を満足する。なお前記ロボットアーム２４０を静止状態から目標位置に移動させる動作では、図９に示すように、ロボットアーム２４０の上腕部２４３の長手方向に延びる中心軸（長手軸）２４３１（回動軸Ｏ４’）と第１前腕部２４４の長手方向に延びる中心軸（長手軸）２４４１とのなす角を９０°とし、また、回動軸Ｏ１と回動軸Ｏ４’とを平行にした状態で、ロボットアームの２４０の上腕部２４３を回動軸Ｏ４’まわりに９０°回動させる。また、前記ロボットアーム２４０を静止状態から目標位置に移動させる動作では、静止状態のロボットアーム２３０および胴体２２０についても前述した振動を抑制する制御を行う。
Ｄ／Ｃ＜０．２７      ・・・（２）
【００８５】
これにより、胴体２２０の回動軸Ｏ１の剛性が等価的に高くなり、ロボットアーム２４０を作動させる際のロボットアーム２３０、２４０および胴体２２０の振動を抑制する効果を高めることができる。
なお、Ｄ／Ｃ＜０．１６であることがより好ましい。
また、Ｂ／ＡとＤ／Ｃは、同一でもよく、また、異なっていてもよい。
【００８６】
次に、ロボットアーム２３０についての第１の補正成分のゲインＫａ、ロボットアーム２４０についての第２の補正成分のゲインＫｂ、胴体２２０についての第３の補正成分のゲインＫｃを変更し、下記の実験を行った。なお、ゲインＫａ、Ｋｂを０、ゲインＫｃを０とした場合（比較例）と、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０とした場合（比較例）と、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０．２５とした場合（本発明）と、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０．５とした場合（本発明）の４パターンを行った。
【００８７】
図８に示すように、ロボットアーム２４０を静止状態とし、また、ロボットアーム２３０の上腕部２３３の長手方向に延びる中心軸２３３１と第１前腕部２３４の長手方向に延びる中心軸２３４１とのなす角を９０°とし、また、回動軸Ｏ１と回動軸Ｏ４とを平行にした状態で、ロボットアーム２３０を静止状態からその上腕部２３３を回動軸Ｏ４まわりに９０°回動させた。そして、角速度センサー７３０により、回動軸Ｏ１（Ｚ軸）まわりの胴体２２０の角速度を求め、また、角速度センサー７２０により、回動軸Ｏ１（Ｚ軸）まわりのロボットアーム２３０の先端部の角速度を求めた。その結果は、図１０および図１１のグラフに示す通りである。なお、図１１のグラフは、図１０のグラフの縦軸を拡大したものである。また、図１１中の静止開始タイミング（整定開始タイミング）が、ロボットアーム２３０が最初に目標位置に達したとき、すなわち、上腕部２３３が回動軸Ｏ４まわりに９０°回動したときである（図１２も同様）。
【００８８】
また、図１２には、Ｚ軸（回動軸Ｏ１）まわりの胴体２２０の角速度が示されている。また、図１２には、ロボットアーム２３０が目標位置に到達する前の胴体２２０の回動軸Ｏ１まわりの角速度の振幅の最大値Ａと、ロボットアーム２３０が最初に目標位置に達した後の胴体２２０の回動軸Ｏ１まわりの角速度の振幅の最大値Ｂとが図示されている。
また、図１３、図１４には、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０．５とした場合（本発明）の回動軸Ｏ１まわりのロボットアーム２３０の先端部の角速度、回動軸Ｏ１まわりのロボットアーム２３０の先端部の角速度が示されている。なお、図１４のグラフは、図１３のグラフの縦軸を拡大したものである。
【００８９】
また、図１５、図１６には、ゲインＫａ、Ｋｂを０、ゲインＫｃを０とした場合（比較例）の回動軸Ｏ１まわりのロボットアーム２３０の先端部の角速度、回動軸Ｏ１まわりのロボットアーム２３０の先端部の角速度が示されている。なお、図１６のグラフは、図１５のグラフの縦軸を拡大したものである。
そして、各々について、Ｂ／Ａを測定した。
【００９０】
その結果は、ゲインＫａ、Ｋｂを０、ゲインＫｃを０とした場合（比較例）は、０．３４、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０とした場合（比較例）は、０．３４であり、それぞれ、振動が非常に大きいものであった。
これに対して、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０．２５とした場合（本発明）は、０．２７、ゲインＫａ、Ｋｂを０．２５、ゲインＫｃを０．５とした場合（本発明）は、０．１５であり、それぞれ、振動は非常に小さいものであった。
【００９１】
以上説明したように、このロボットシステム１００によれば、ロボットアーム２３０、２４０および胴体２２０の振動を抑制することができる。そして、Ｂ／Ａ、Ｄ／Ｃを前記のように設定することにより、胴体２２０の回動軸Ｏ１の剛性が等価的に高くなり、ロボットアーム２３０、２４０および胴体２２０の振動を抑制する効果を高めることができる。
【００９２】
具体的には、例えば、胴体２２０がロボットアーム２３０、２４０の動作の反作用で振動することを抑制することができ、また、その振動を抑制する効果を高めることができる。これにより、動作するロボットアーム、胴体２２０、動作するロボットアームと反対側のロボットアームの３者とも振動が抑制され、素早い位置決めが可能になる。
以上、本発明のロボット制御装置、ロボットおよびロボットシステムを、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物が付加されていてもよい。
【００９３】
また、前記実施形態では、慣性センサーとして、角速度センサーを用いたが、本発明では、これに限定されず、慣性センサーとして、例えば、加速度センサー等を用いもよい。
また、各位置センサーの方式は、それぞれ、特に限定されず、例えば、光学式、磁気式、電磁式、電気式等が挙げられる。
また、前記実施形態では、移動可能なロボットについて説明したが、ロボットは、ボルト等によって作業部屋の床、天井、壁等に固定されていてもよい。
【００９４】
また、前記実施形態では、ロボットの回動軸の数が１５であるが、本発明では、これに限定されず、ロボットの回動軸の数は、１〜１４であってもよいし、１６以上であってもよい。
また、前記実施形態では、ロボットアームの数は、２つであるが、発明では、これに限らず、ロボットアームの数は、３つ以上であってもよい。
【符号の説明】
【００９５】
１００……ロボットシステム  ２００……ロボット  ２１０……ベース  ２１１……ハンドル  ２１３……バンパー  ２１３ａ……当接部  ２１３ｂ……固定部  ２１４……非常停止ボタン  ２２０……胴体  ２３０……ロボットアーム  ２３１……第１肩部  ２３２……第２肩部  ２３３……上腕部  ２３３１……中心軸  ２３４……第１前腕部  ２３４１……中心軸  ２３５……第２前腕部  ２３６……手首部  ２３７……連結部  ２３８……ハンド部  ２４０……ロボットアーム  ２４１……第１肩部  ２４２……第２肩部  ２４３……上腕部  ２４３１……中心軸  ２４４……第１前腕部  ２４４１……中心軸  ２４５……第２前腕部  ２４６……手首部  ２４７……連結部  ２４８……ハンド部  ２５０……ステレオカメラ  ２６０……信号灯  ２７０……モニター  ３１０……関節機構  ３１１……モーター  ３１２……位置センサー  ４１０……関節機構  ４１１……モーター  ４１２……位置センサー  ４２０……関節機構  ４２１……モーター  ４２２……位置センサー  ４３０……捻り機構  ４３１……モーター  ４３２……位置センサー  ４４０……関節機構  ４４１……モーター  ４４２……位置センサー  ４５０……捻り機構  ４５１……モーター  ４５２……位置センサー  ４６０……関節機構  ４６１……モーター  ４６２……位置センサー  ４７０……捻り機構  ４７１……モーター  ４７２……位置センサー  ５１０……関節機構  ５１１……モーター  ５１２……位置センサー  ５２０……関節機構  ５２１……モーター  ５２２……位置センサー  ５３０……捻り機構  ５３１……モーター  ５３２……位置センサー  ５４０……関節機構  ５４１……モーター  ５４２……位置センサー  ５５０……捻り機構  ５５１……モーター  ５５２……位置センサー  ５６０……関節機構  ５６１……モーター  ５６２……位置センサー  ５７０……捻り機構  ５７１……モーター  ５７２……位置センサー  ６１０……エンドエフェクター  ６１１……第１の指  ６１２……第２の指  ６２０……エンドエフェクター  ６２１……第１の指  ６２２……第２の指  ７１０……角速度センサー  ７２０……角速度センサー  ７３０……角速度センサー  ９００……ロボット制御装置  ９０１……第１駆動源制御部  ９０１ａ……減算器  ９０１ｂ……位置制御部  ９０１ｃ……減算器  ９０１ｄ……角速度制御部  ９０１ｅ……回動角度算出部  ９０１ｆ……角速度算出部  ９０１ｇ……減算器  ９０１ｈ……変換部  ９０１ｉ……補正値算出部  ９０１ｊ……加算器  ９０１ｋ……フィードバックゲイン調整部  ９０１ｍ……角速度算出部  ９０２……第２駆動源制御部  ９０２ａ……減算器  ９０２ｂ……位置制御部  ９０２ｃ……減算器  ９０２ｄ……角速度制御部  ９０２ｅ……回動角度算出部  ９０２ｆ……角速度算出部  ９０２ｇ……減算器  ９０２ｈ……変換部  ９０２ｉ……補正値算出部  ９０２ｊ……加算器  ９０２ｋ……フィードバックゲイン調整部  ９０２ｍ……角速度算出部  ９０３……第３駆動源制御部  ９０４……第４駆動源制御部  ９０５……第５駆動源制御部  ９０６……第６駆動源制御部  ９０７……第７駆動源制御部  ９０８……第８駆動源制御部  ９０９……第９駆動源制御部  ９０９ａ……減算器  ９０９ｂ……位置制御部  ９０９ｃ……減算器  ９０９ｄ……角速度制御部  ９０９ｅ……回動角度算出部  ９０９ｆ……角速度算出部  ９０９ｇ……減算器  ９０９ｈ……変換部  ９０９ｉ……補正値算出部  ９０９ｊ……加算器  ９０９ｋ……フィードバックゲイン調整部  ９０９ｍ……角速度算出部  ９１０……第１０駆動源制御部  ９１１……第１１駆動源制御部  ９１２……第１２駆動源制御部  ９１３……第１３駆動源制御部  ９１４……第１４駆動源制御部  ９１５……第１５駆動源制御部  ９３０……記憶部  ９４０……比較部  Ｏ１、Ｏ２、Ｏ２’、Ｏ３、Ｏ３’Ｏ４、Ｏ４’、Ｏ５、Ｏ５’、Ｏ６、Ｏ６’、Ｏ７、Ｏ７’、Ｏ８、Ｏ８’……回動軸
【図１】
2015085426.tif 000003
【図２】
2015085426.tif 000004
【図３】
2015085426.tif 000005
【図４】
2015085426.tif 000006
【図５】
2015085426.tif 000007
【図６】
2015085426.tif 000008
【図７】
2015085426.tif 000009
【図８】
2015085426.tif 000010
【図９】
2015085426.tif 000011
【図１０】
2015085426.tif 000012
【図１１】
2015085426.tif 000013
【図１２】
2015085426.tif 000014
【図１３】
2015085426.tif 000015
【図１４】
2015085426.tif 000016
【図１５】
2015085426.tif 000017
【図１６】
2015085426.tif 000018