JP6314426B2 - ロボット制御装置およびロボット制御方法 - Google Patents

Description

本発明は、ロボット制御装置およびロボット制御方法に関するものである。

従来、ロボットアームを備えたロボットが知られている。ロボットアームは複数のリンクが関節部を介して連結され、最も先端側のリンクには、エンドエフェクターとして、例えば、ハンドが装着される。関節部はモーターにより駆動され、その関節部の駆動により、リンクが回動する。そして、ロボットを作動させる際は、エンコーダーでモーターの回転角度を検出し、ロボットの制御に用いる。
しかしながら、モーターに連結されているトルク伝達機構、減速機や、リンクは剛体ではなく、弾性を有しているので、それによりロボットの作動の際、ロボットアームが振動してしまうという問題がある。

特開２０１１−１３６３９５号公報

前記のような問題に対し、ロボットアームの先端部に慣性センサーを取り付け、そのロボットアームの先端部の加速度を検出し、ヤコビ行列等を用いてロボットアームの先端部の振動を抑制するための補正値を各リンクを回動させる各モーターに分配して制御を行い、ロボットアームの先端部の振動を抑制する方法が考案されている（例えば、特許文献１参照）。

しかしながら、このような方法をロボットアームの特異姿勢付近、すなわち、ロボットアームが伸びた姿勢で用いると、各モーターへの補正値の分配量が大きくなり、すなわち、各モーターへの過大なフィードバック量が生じ、ロボットの制御が不安定になるという問題がある。
本発明の目的は、ロボットアームの特異姿勢付近においてもそのロボットアームの作動を不安定にすることなく、ロボットアームの振動を抑制することができるロボット制御装置、ロボットおよびロボット制御方法を提供することにある。

このような目的は、下記の本発明により達成される。
（適用例１）
本発明に係わるロボット制御装置は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御装置であって、
前記慣性センサーの検出結果に基づいて、前記慣性センサーが設けられた前記リンクに設定された第１座標系における前記ロボットアームの前記慣性センサーの位置の振動情報を求める第１座標系振動演算部と、
前記第１座標系振動演算部により求められた前記第１座標系における前記振動情報を前記慣性センサーが設けられた前記リンクよりも基端側に配置された前記リンクに設定された第２座標系における前記振動情報に座標変換する第２座標系振動演算部と、
前記ロボットアームの姿勢に応じて、前記第２座標系における所定の座標軸の方向の前記振動情報の成分に対して重み付けを行う重み付け部と、
前記重み付け後の前記第２座標系における前記振動情報を前記第２座標系が設定された前記リンクと異なる前記リンクに設定された第３座標系における前記振動情報に座標変換する第３座標系振動演算部と、
前記第３座標系振動演算部により求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算部と、
前記駆動源の前記駆動指令と、前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御部と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、第２座標系における所定の座標軸の方向の振動情報の成分に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

（適用例２）
本発明に係わるロボット制御装置では、前記ロボットアームは、前記基台に連結されて第１回転軸を軸中心として回動する第１リンクと、
前記第１リンクに連結されて第２回転軸を軸中心として回動し、前記第２回転軸を前記第１回転軸と直交する回転軸または前記第１回転軸と直交する軸と平行な回転軸とする第２リンクと、
前記第２リンクに連結されて第３回転軸を軸中心として回動し、前記第３回転軸を前記第２回転軸と直交する回転軸または前記第２回転軸と直交する軸と平行な回転軸とする第３リンクと、
前記基台と前記第１リンクとを連結する第１関節部と、
前記第１リンクと前記第２リンクとを連結する第２関節部と、
前記第２リンクと前記第３リンクとを連結する第３関節部と、
前記第１関節部を駆動して前記第１リンクを回動させる第１駆動源と、
前記第２関節部を駆動して前記第２リンクを回動させる第２駆動源と、
前記第３関節部を駆動して前記第３リンクを回動させる第３駆動源と、を有し、
前記補正値演算部は、前記第３座標系振動演算部により求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記第１駆動源の駆動指令を補正する第１補正値および前記第３駆動源の駆動指令を補正する第２補正値を求め、
前記駆動源制御部は、前記第１補正値により前記第１駆動源の前記駆動指令を補正し、前記第２補正値により前記第３駆動源の前記駆動指令を補正することにより、前記ロボットアームの前記慣性センサーの位置の振動を抑制することが好ましい。
これにより、より確実かつ効率良く、ロボットアームの振動を抑制することができる。

（適用例３）
本発明に係わるロボット制御装置では、前記所定の座標軸の方向は、前記第１回転軸と直交する方向と前記第３回転軸と直交する方向とのいずれか一方であることが好ましい。
これにより、より確実かつ効率良く、ロボットアームの振動を抑制することができる。
（適用例４）
本発明に係わるロボット制御装置では、前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、〜１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記θの絶対値が第１の閾値以下または第４の閾値以上のときは、重み付け定数を０に設定することが好ましい。
これにより、ロボットアームの特異姿勢付近において、ロボットアームをより安定的に作動させることができる。

（適用例５）
本発明に係わるロボット制御装置では、前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、〜１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記θの絶対値が第２の閾値以上、第３の閾値以下のときは、重み付け定数を１に設定することが好ましい。
これにより、迅速に振動を抑制することができる。

（適用例６）
本発明に係わるロボット制御装置では、前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、〜１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記θの絶対値が第１の閾値よりも大きく、第２の閾値よりも小さいとき、または、第３の閾値よりも大きく、第４の閾値よりも小さいときは、前記θが増大するにしたがって、重み付け定数を連続的または段階的に増大させることが好ましい。
これにより、ロボットアームの特異姿勢付近において、ロボットアームをより安定的に作動させることができ、また、迅速に振動を抑制することができる。

（適用例７）
本発明に係わるロボット制御装置は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御装置であって、
前記慣性センサーの検出結果に基づいて、前記ロボットアームの前記慣性センサーの位置の振動情報を求める振動演算部と、
前記振動演算部により求められた前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算部と、
前記ロボットアームの姿勢に応じて、前記補正値に対して重み付けを行う重み付け部と、
前記駆動源の前記駆動指令と、前記重み付け後の前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御部と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、補正値に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

（適用例８）
本発明に係わるロボット制御装置は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御装置であって、
前記慣性センサーの検出結果に基づいて、前記ロボットアームの前記慣性センサーの位置の振動情報を求める振動演算部と、
前記ロボットアームの姿勢に応じて、前記振動演算部により求められた前記振動情報に対して重み付けを行う重み付け部と、
前記重み付け後の前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算部と、
前記駆動源の前記駆動指令と、前記重み付け後の前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御部と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、振動情報に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

（適用例９）
本発明に係わるロボットは、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、
本発明のロボット制御装置と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、重み付け部により重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。
（適用例１０）
本発明に係わるロボットでは、前記ロボットは、双腕ロボットであることが好ましい。
これにより、多彩な動作を行うことができ、多彩な作業を行うことができる。

（適用例１１）
本発明に係わるロボット制御方法は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御方法であって、
前記慣性センサーの検出結果に基づいて、前記慣性センサーが設けられた前記リンクに設定された第１座標系における前記ロボットアームの前記慣性センサーの位置の振動情報を求める第１座標系振動演算工程と、
前記第１座標系振動演算工程で求められた前記第１座標系における前記振動情報を前記慣性センサーが設けられた前記リンクよりも基端側に配置された前記リンクに設定された第２座標系における前記振動情報に座標変換する第２座標系振動演算工程と、
前記ロボットアームの姿勢に応じて、前記第２座標系における所定の座標軸の方向の前記振動情報の成分に対して重み付けを行う重み付け工程と、
前記重み付け後の前記第２座標系における前記振動情報を前記第２座標系が設定された前記リンクと異なる前記リンクに設定された第３座標系における前記振動情報に座標変換する第３座標系振動演算工程と、
前記第３座標系振動演算工程で求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算工程と、
前記駆動源の前記駆動指令と、前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御工程とを備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、第２座標系における所定の座標軸の方向の振動情報の成分に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

（適用例１２）
本発明に係わるロボット制御方法は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御方法であって、
前記慣性センサーの検出結果に基づいて、前記ロボットアームの前記慣性センサーの位置の振動情報を求める振動演算工程と、
前記振動演算工程で求められた前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算工程と、
前記ロボットアームの姿勢に応じて、前記補正値に対して重み付けを行う重み付け工程と、
前記駆動源の前記駆動指令と、前記重み付け後の前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制工程と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、補正値に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

（適用例１３）
本発明に係わるロボット制御方法は、基台と、
少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御方法であって、
前記慣性センサーの検出結果に基づいて、前記ロボットアームの前記慣性センサーの位置の振動情報を求める振動演算工程と、
前記ロボットアームの姿勢に応じて、前記振動演算部により求められた前記振動情報に対して重み付けを行う重み付け工程と、
前記重み付け後の前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算工程と、
前記駆動源の前記駆動指令と、前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御工程と、を備えることを特徴とする。
これにより、ロボットアームの姿勢に応じて、振動情報に対して重み付けを行うことにより、補正値を小さくすることができ、これによって、ロボットアームの特異姿勢付近においても、そのロボットアームを安定的に作動させつつ、ロボットアームの振動を抑制することができる。

本発明のロボットの第１実施形態におけるロボット本体の概略図である。 図１に示すロボットのブロック図である。 図１に示すロボットのブロック図である。 図１に示すロボットの重み付け定数を説明するための図である。 図１に示すロボットの振動情報および補正値を示す図である。 図１に示すロボットの振動情報および補正値を示す図である。 本発明のロボットの第２実施形態を示すブロック図である。 本発明のロボットの第３実施形態を示すブロック図である。 本発明のロボットの第３実施形態における振動情報および補正値を示す図である。 本発明のロボットの第４実施形態におけるロボット本体の概略図である。

以下、本発明のロボット制御装置、ロボットおよびロボット制御方法を添付図面に示す好適な実施形態に基づいて詳細に説明する。
図１は、本発明のロボットの第１実施形態におけるロボット本体の概略図である。図２は、図１に示すロボットのブロック図である。図３は、図１に示すロボットのブロック図である。図４は、図１に示すロボットの重み付け定数を説明するための図である。図５はおよび図６は、それぞれ、図１に示すロボットの振動情報および補正値を示す図である。
なお、以下では、説明の都合上、図１中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１中の基台側を「基端」、その反対側を「先端」と言う。また、図３には、ロボット本体側については、第３リンクに関する部分が代表的に示されている。

図１〜図３に示すロボット（産業用ロボット）１は、例えば腕時計のような精密機器等を製造する製造工程で用いることができ、ロボット本体（本体部）１０と、ロボット本体１０（ロボット１）の作動を制御するロボット制御装置（制御部）２０とを有している。ロボット本体１０と、ロボット制御装置２０とは、それぞれ、電気的に接続されている。また、ロボット制御装置２０は、例えば、ＣＰＵ（Central Processing Unit）が内蔵されたパーソナルコンピューター（ＰＣ）等で構成することができる。なお、ロボット本体１０とロボット制御装置２０とは、一体であってもよく、また、別体であってもよい。なお、ロボット制御装置２０については、後で詳述する。

ロボット本体１０は、基台（支持部）１１と、５本のリンク（腕部）１２、１３、１４、１５、１６と、２本のリンク（腕部）１８、１９を有するリスト１７と、７つの駆動源４０１、４０２、４０３、４０４、４０５、４０６、４０７とを備えるロボットアーム５を有している。すなわち、ロボット１は、基台１１と、リンク１２、１３、１４、１５、１６と、リスト１７とが基端側から先端側に向ってこの順に連結された垂直多関節（７軸）ロボットである。なお、リンク１２を「第１リンク」、リンク１３を「第２リンク」、リンク１４を「第３リンク」、リンク１５を「第４リンク」、リンク１６を「第５リンク」、リスト１７を「第６リンク、第７リンク」と分けて言うことができる。リスト１７にはエンドエフェクター等を取り付けることができる。

図１に示すように、リンク１２〜１６、リスト１７は、それぞれ、基台１１に対し独立して変位可能に支持されている。なお、リンク１２〜１６、リスト１７の長さは、それぞれ、特に限定されず、諸条件に応じて適宜設定される。
基台１１と第１リンク１２とは、第１関節部（ジョイント）１７１を介して連結されている。第１関節部１７１は、互いに連結された基台１１と第１リンク１２のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第１リンク１２は、基台１１に対し、鉛直方向と平行な第１回転軸Ｏ１を回転中心（軸中心）とし、その第１回転軸Ｏ１回りに回動自在となっている。第１回転軸Ｏ１は、ロボット１の設置面である床１０１の上面の法線と一致している。この第１回転軸Ｏ１回りの回動は、第１駆動源４０１の駆動によりなされる。また、第１駆動源４０１はモーター４０１Ｍとケーブル（図示せず）によって駆動され、このモーター４０１Ｍは電気的に接続されたモータードライバー３０１を介してロボット制御装置２０により制御される。第１駆動源４０１はモーター４０１Ｍとともに設けた減速機（図示せず）によってモーター４０１Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、ロボット本体１０の基台１１には、例えば、モーター４０１Ｍやモータードライバー３０１〜３０７が収納されている。

第１リンク１２と第２リンク１３とは、第２関節部（ジョイント）１７２を介して連結されている。第２関節部１７２は、互いに連結された第１リンク１２と第２リンク１３のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第２リンク１３は、第１リンク１２に対し、第２回転軸Ｏ２を回転中心とし、その第２回転軸Ｏ２回りに回動自在となっている。第２回転軸Ｏ２は、第１回転軸Ｏ１と直交している。この第２回転軸Ｏ２回りの回動は、第２駆動源４０２の駆動によりなされる。また、第２駆動源４０２は、モーター４０２Ｍとケーブル（図示せず）によって駆動され、このモーター４０２Ｍは電気的に接続されたモータードライバー３０２を介してロボット制御装置２０により制御される。第２駆動源４０２はモーター４０２Ｍの他に設けた減速機（図示せず）によってモーター４０２Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第２回転軸Ｏ２は、第１回転軸Ｏ１に直交する軸と平行であってもよい。なお、第１リンク１２には、例えば、モーター４０２Ｍが収納されている。

第２リンク１３と第３リンク１４とは、第３関節部（ジョイント）１７３を介して連結されている。第３関節部１７３は、互いに連結された第２リンク１３と第３リンク１４のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第３リンク１４は、第２リンク１３に対し、３回転軸Ｏ３を回転中心とし、その第３回転軸Ｏ３回りに回動自在となっている。第３回転軸Ｏ３は、第２回転軸Ｏ２と直交している。この第３回転軸Ｏ３回りの回動は、第３駆動源４０３の駆動によりなされる。また、第３駆動源４０３は、モーター４０３Ｍとケーブル（図示せず）によって駆動され、このモーター４０３Ｍは電気的に接続されたモータードライバー３０３を介してロボット制御装置２０により制御される。第３駆動源４０３はモーター４０３Ｍの他に設けた減速機（図示せず）によってモーター４０３Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第３回転軸Ｏ３は、第２回転軸Ｏ２に直交する軸と平行であってもよい。なお、第２リンク１３には、例えば、モーター４０３Ｍが収納されている。

第３リンク１４と第４リンク１５とは、第４関節部（ジョイント）１７４を介して連結されている。第４関節部１７４は、互いに連結された第３リンク１４と第４リンク１５のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第４リンク１５は、第３リンク１４に対し、第４回転軸Ｏ４を回転中心とし、その第４回転軸Ｏ４回りに回動自在となっている。第４回転軸Ｏ４は、第３回転軸Ｏ３と直交している。この第４回転軸Ｏ４回りの回動は、第４駆動源４０４の駆動によりなされる。また、第４駆動源４０４は、モーター４０４Ｍとケーブル（図示せず）によって駆動され、このモーター４０４Ｍは電気的に接続されたモータードライバー３０４を介してロボット制御装置２０により制御される。第４駆動源４０４はモーター４０４Ｍとともに設けた減速機（図示せず）によってモーター４０４Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第４回転軸Ｏ４は、第３回転軸Ｏ３に直交する軸と平行であってもよい。なお、第３リンク１４には、例えば、モーター４０４Ｍが収納されている。

第４リンク１５と第５リンク１６とは、第５関節部（ジョイント）１７５を介して連結されている。第５関節部１７５は、互いに連結された第４リンク１５と第５リンク１６のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、第５リンク１６は、第４リンク１５に対し、第５回転軸Ｏ５を回転中心とし、その第５回転軸Ｏ５回りに回動自在となっている。第５回転軸Ｏ５は、第４回転軸Ｏ４と直交している。この第５回転軸Ｏ５回りの回動は、第５駆動源４０５の駆動によりなされる。また、第５駆動源４０５は、モーター４０５Ｍとケーブル（図示せず）によって駆動され、このモーター４０５Ｍは電気的に接続されたモータードライバー３０５を介してロボット制御装置２０により制御される。第５駆動源４０５はモーター４０５Ｍとともに設けた減速機（図示せず）によってモーター４０５Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第５回転軸Ｏ５は、第４回転軸Ｏ４に直交する軸と平行であってもよい。なお、第４リンク１５には、例えば、モーター４０５Ｍが収納されている。

リスト１７は、第６リンク１８と第７リンク１９とを有している。このリスト１７には、その先端部に、エンドエフェクターとして、例えば、腕時計等のような精密機器を把持するハンド（図示せず）が着脱自在に装着される。なお、ハンドとしては、特に限定されず、例えば、複数本の指部（フィンガー）を有する構成のものが挙げられる。そして、このロボット１は、ハンドで精密機器を把持したまま、リンク１２〜１６やリスト１７等の動作を制御することにより、当該精密機器を搬送することができる。

また、第５リンク１６とリスト１７の第６リンクとは、第６関節部（ジョイント）１７６を介して連結されている。第６関節部１７６は、互いに連結された第５リンク１６とリスト１７の第６リンク１８のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、リスト１７の第６リンク１８は、第５リンク１６に対し、第６回転軸Ｏ６を回転中心とし、その第６回転軸Ｏ６回りに回動自在となっている。第６回転軸Ｏ６は、第５回転軸Ｏ５と直交している。この第６回転軸Ｏ６回りの回動は、第６駆動源４０６の駆動によりなされる。また、第６駆動源４０６は、モーター４０６Ｍとケーブル（図示せず）によって駆動され、このモーター４０６Ｍは電気的に接続されたモータードライバー３０６を介してロボット制御装置２０により制御される。第６駆動源４０６はモーター４０６Ｍとともに設けた減速機（図示せず）によってモーター４０６Ｍからの駆動を伝達されてもよく、また、減速機が省略されていてもよい。

また、リスト１７の第６リンクと第７リンク１９とは、第７関節部（ジョイント）１７７を介して連結されている。第７関節部１７７は、互いに連結されたリスト１７の第６リンク１８と第７リンク１９のうちの一方を他方に対し回動可能に支持する機構を有している。この場合、リスト１７の第７リンク１９は、第６リンク１８に対し、第７回転軸Ｏ７を回転中心とし、その第７回転軸Ｏ７回りに回動自在となっている。回転軸Ｏ７は、回転軸Ｏ６と直交している。この第７回転軸Ｏ７回りの回動は、第７駆動源４０７駆動によりなされる。また、第７駆動源４０７の駆動は、モーター４０７Ｍとケーブル（図示せず）によって駆動され、このモーター４０７Ｍは電気的に接続されたモータードライバー３０７を介してロボット制御装置２０により制御される。第７駆動源４０７はモーター４０７Ｍとともに設けた減速機（図示せず）によってモーター４０７Ｍからの駆動を伝達するように構成されていてもよく、また、減速機が省略されていてもよい。なお、第６回転軸Ｏ６は、第５回転軸Ｏ５に直交する軸と平行であってもよく、また、第７回転軸Ｏ７は、第６回転軸Ｏ６に直交する軸と平行であってもよい。なお、第５リンク１６には、例えば、モーター４０６Ｍ、４０７Ｍが収納されている。

また、駆動源４０１〜４０７には、それぞれのモーター４０１Ｍ〜４０７Ｍまたは減速機に、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３、第４角度センサー４１４、第５角度センサー４１５、第６角度センサー４１６、第７角度センサー４１７が設けられている。これらの角度センサー４１１〜４１７としては、例えば、エンコーダー、ロータリーエンコーダー等を用いることができる。これらの角度センサー４１１〜４１７により、それぞれ、駆動源４０１〜４０７のモーター４０１Ｍ〜４０７Ｍあるいは減速機の回転軸の回転角度を検出する。この角度センサー４１１〜４１７の検出結果、すなわち、角度センサー４１１〜４１７から出力される信号は、ロボット制御装置２０に入力される。そして、ロボット制御装置２０は、その検出結果に基づいて、所定の制御を行う。なお、この駆動源４０１〜４０７のモーター４０１Ｍ〜４０７Ｍとしては、それぞれ、特に限定されず、例えば、ＡＣサーボモーター、ＤＣサーボモーター等のサーボモーターを用いるのが好ましい。

また、第３リンク１４内には、慣性センサー３１が設置されている。本実施形態では、慣性センサー３１として、互いに直交する３つの検出軸であるｘ軸、ｙ軸、ｚ軸（いずれも図示せず）のそれぞれの回りの角速度を検出する角速度センサーを用いる。具体的には、例えば、ジャイロセンサー等を用いることができる。また、慣性センサー３１の姿勢は、特に限定されないが、例えば、３つの検出軸のうちの１つが、第３回転軸Ｏ３と平行または一致させることが好ましい。

この慣性センサー３１により、慣性センサー３１の設置個所におけるｘ軸の回りの角速度、ｙ軸の回りの角速度、ｚ軸の回りの角速度をそれぞれ検出する。この慣性センサー３１の検出結果、すなわち、慣性センサー３１から出力される信号は、ロボット制御装置２０に入力される。そして、ロボット制御装置２０は、その検出結果に基づいて、所定の制御を行う。

なお、このロボット１では、第３リンク１４の慣性センサー３１の設置個所の振動を抑制するために、前記のように第３リンク１４に慣性センサー３１を設置し、その慣性センサー３１の検出結果に基づいて駆動源４０１、４０２、４０３の作動を制御する。これにより、確実に、第３リンク１４の慣性センサー３１の設置個所の振動を抑制することができ、これによって、ロボットアーム５の先端部の振動を抑制することができる。
また、ロボット本体１０は、ロボット制御装置２０と電気的に接続されている。すなわち、駆動源４０１〜４０７、角度センサー４１１〜４１７、慣性センサー３１は、それぞれ、ロボット制御装置２０と電気的に接続されている。

そして、ロボット制御装置２０は、リンク１２〜１６、リスト１７をそれぞれ独立して作動させることができる、すなわち、モータードライバー３０１〜３０７を介して、駆動源４０１〜４０７それぞれ独立して制御することができる。この場合、ロボット制御装置２０は、角度センサー４１１〜４１７、慣性センサー３１等により検出を行い、その検出結果に基づいて、駆動源４０１〜４０７の駆動、例えば、角速度や回転角度等をそれぞれ制御する。この場合、ロボット制御装置２０は、例えば、インピーダンス制御（力制御）、位置制御等の所定の制御を行う。この制御プログラムは、ロボット制御装置２０に内蔵された記録媒体に予め記憶されている。

次に、図１〜図３を参照し、ロボット制御装置２０の構成について説明する。
ロボット制御装置２０は、ロボット本体１０全体、すなわち、第１駆動源４０１、第２駆動源４０２、第３駆動源４０３、第４駆動源４０４、第５駆動源４０５、第６駆動源４０６、第７駆動源４０７、リスト１７に装着されたハンドの駆動源等の作動をそれぞれ制御する装置である。

図２に示すように、ロボット制御装置２０は、第１駆動源４０１の作動を制御する第１駆動源制御部（制御部）２０１と、第２駆動源４０２の作動を制御する第２駆動源制御部（制御部）２０２と、第３駆動源４０３の作動を制御する第３駆動源制御部（制御部）２０３と、第４駆動源４０４の作動を制御する第４駆動源制御部（制御部）２０４と、第５駆動源４０５の作動を制御する第５駆動源制御部（制御部）２０５と、第６駆動源４０６の作動を制御する第６駆動源制御部（制御部）２０６と、第７駆動源４０７の作動を制御する第７駆動源制御部（制御部）２０７とを有している。

ここで、ロボット制御装置２０は、ロボット本体１０が行う処理の内容に基づいてリスト１７の先端部の目標位置、すなわち、リスト１７に装着されたハンドの目標位置を求め、その目標位置にハンドを移動させるための軌道を生成する。そして、ロボット制御装置２０は、その生成した軌道に沿ってハンド（リスト１７）が移動するように、各駆動源４０１〜４０７の回転角度を所定の制御周期ごとに測定し、この測定結果に基づいて演算した値をそれぞれ各駆動源４０１〜４０７の位置指令Ｐｃとして駆動源制御部２０１〜２０７に出力する。なお、前記および以下では、「値が入力、出力」等と表記しているが、これは、「その値に対応する信号が入力、出力」の意味である。

第１駆動源制御部２０１には、第１駆動源４０１の位置指令Ｐｃの他、第１角度センサー４１１から検出信号が入力され、また、ロボットアーム５の振動を抑制するための後述する補正値が入力される。第１駆動源制御部２０１は、第１角度センサー４１１の検出信号から算出される第１駆動源４０１の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によって第１駆動源４０１を駆動する。

すなわち、第１駆動源制御部２０１の第１減算器（図示せず）には、位置指令Ｐｃが入力され、また、後述する位置フィードバック値Ｐｆｂが入力される。第１駆動源制御部２０１では、第１角度センサー４１１から入力されるパルス数がカウントされるとともに、そのカウント値に応じた第１駆動源４０１の回転角度が位置フィードバック値Ｐｆｂとして第１減算器に出力される。第１減算器は、これら位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差（第１駆動源４０１の回転角度の目標値から位置フィードバック値Ｐｆｂを減算した値）を出力する。

また、第１駆動源制御部２０１は、第１減算器から入力された偏差と、予め定められた係数である比例ゲイン等を用いた所定の演算処理を行うことで、その偏差に応じた第１駆動源４０１の角速度の目標値を演算する。そして、その第１駆動源４０１の角速度の目標値（指令値）を示す信号を角速度指令（駆動指令）ωｃとして第２減算器（図示せず）に出力する。なお、ここでは、本実施形態では、フィードバック制御として、比例制御（Ｐ制御）がなされるが、これに限定されるものではない。

また、第１駆動源制御部２０１では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１駆動源４０１の角速度が算出され、その角速度が第２減算器に出力される。また、ロボットアーム５の振動を抑制するための後述する補正値（第１補正値）も第２減算器に出力される。この場合、第１角度センサー４１１の検出結果に基づいて求められた第１駆動源４０１の角速度と、前記補正値とは、そのまま第２減算器に出力されるわけではなく、それらを用いた所定の演算が行われ、その演算結果が、角速度フィードバック値ωｆｂとして第２減算器に出力される。なお、補正値については、後で詳述する。

第２減算器には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。第２減算器は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第１駆動源４０１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を出力する。
また、第１駆動源制御部２０１は、第２減算器から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第１駆動源４０１の角加速度（トルク）の目標値を演算する。そして第１駆動源制御部２０１は、その第１駆動源４０１の角加速度の目標値（指令値）を示す信号を角加速度指令（トルク指令）として生成する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。

第１駆動源制御部２０１は、その角加速度指令に基づいて、第１駆動源４０１の駆動信号（駆動電流）を生成し、モータードライバー３０１を介してモーター４０１Ｍに供給する（駆動源制御工程）。
このようにして、第１駆動源４０１の角加速度、すなわち、トルクがその目標値と可及的に等しくなり、かつ、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなるとともに、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第１駆動源４０１の駆動電流が制御される。

なお、第２駆動源制御部２０２および第３駆動源制御部２０３については、それぞれ、前記第１駆動源制御部２０１と同様であるので、その説明は省略する。
また、第４駆動源制御部２０４〜第７駆動源制御部２０７については、それぞれ、前記補正値による補正は行われず、角度センサー４１４〜４１７の検出結果に基づいて求められた駆動源４０４〜４０７の角速度が角速度フィードバック値ωｆｂとして第２減算器に出力される他は、前記第１駆動源制御部２０１と同様であるので、その説明は省略する。

次に、前記ロボットアーム５の振動を抑制するための補正値を用いた制振制御について説明する。
図３に示すように、ロボット制御装置２０は、振動演算部５０１と、補正値演算部５０２と、重み付け定数決定部５０３と、重み付け実行部５０４と、駆動源制御部２０１〜２０７とを有している。重み付け定数決定部５０３と重み付け実行部５０４とにより、重み付け部が構成される。

まず、図５に示すように、慣性センサー３１により、振動情報として、角速度ωを検出する。そして、振動演算部５０１は、慣性センサー３１により検出された角速度ωを、第３回転中心軸Ｏ３の回りの角速度成分と、第４回転中心軸Ｏ４の回りの角速度成分と、第３回転中心軸Ｏ３と第４回転中心軸Ｏ４とに直交する軸の回りの角速度成分とに分解する（振動演算工程）。

次に、補正値演算部５０２は、前記各角速度成分と、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３の検出結果とに基づいて、第１駆動源４０１の角速度指令（駆動指令）を補正する第１補正値（補正値）ω１と、第２駆動源４０２の角速度指令（駆動指令）を補正する補正値ω３と、第３駆動源４０３の角速度指令（駆動指令）を補正する第２補正値（補正値）ω２とをそれぞれ求める（補正値演算工程）。この際は、第３リンク座標系の角速度と駆動源の角速度との関係を示す所定のヤコビ行列（ヤコビアン）の逆行列（逆ヤコビアン）を用いる。なお、第１補正値ω１は、第１回転中心軸Ｏ１の回りの角速度の振動成分に相当し、第２補正値ω２は、第３回転中心軸Ｏ３の回りの角速度の振動成分に相当し、補正値ω３は、第２回転中心軸Ｏ２の回りの角速度の振動成分に相当する。

次に、ロボットアーム５の姿勢に応じて、重み付け定数を設定し、第１補正値ω１、第２補正値ω２に対してそれぞれ重み付けを行う（重み付工程）。
この際は、重み付け定数決定部５０３は、まず、角度センサー４１２により、第１回転軸Ｏ１と第３回転軸Ｏ３とのなす角度θを検出し、その検出された角度θと、予め設定されている第１の閾値、第２の閾値、第３の閾値、第４の閾値（図４参照）とを比較する。なお、本実施形態では、角度θは、−１８０°以上、１８０°以下の範囲で設定する。また、角度θは、反時計回りの方向を「正」、時計回りの方向を負とする。また、第１の閾値、第２の閾値、第３の閾値、第４の閾値の大小関係は、「第１の閾値＜第２の閾値＜第３の閾値＜第４の閾値」である。

ここで、図５に示すように、角度θが９０°に近いときは、第１補正値ω１、第２補正値ω２は、比較的小さく、問題がない。しかし、図６に示すように、角度θが１８０°に近いとき、または０°に近いとき、すなわち、特異姿勢（特異点）に近い場合は、第１補正値ω１、第２補正値ω２が大きくなってしまう。そこで、このロボット１では、重み付け定数決定部５０３は、角度θに応じて重み付け定数を設定し、第１駆動源４０１の角速度指令を補正する第１補正値ω１と、第３駆動源４０３の角速度指令を補正する第２補正値ω２に対し、それぞれ、重み付けを行う。なお、以下では、第１補正値ω１、第２補正値ω２、補正値ω３をそれぞれ単に、第１補正値（補正値）、第２補正値（補正値）、補正値とも言う。

重み付け定数決定部５０３は、角度θの絶対値が第１の閾値以下または第４の閾値以上のときは、重み付け定数を０に設定する。なお、この重み付け定数は、例えば、重み付け実行部５０４の増幅器の増幅率として設定される。
また、重み付け定数決定部５０３は、角度θの絶対値が第２の閾値以上、第３の閾値以下のときは、重み付け定数を１に設定する。

また、重み付け定数決定部５０３は、角度θの絶対値が第１の閾値よりも大きく、第２の閾値よりも小さいときは、角度θの絶対値が増大するにしたがって、重み付け定数を増大させる。この場合、重み付け定数を連続的に増大させてもよく、また、段階的に増大させてもよい。なお、図４には、重み付け定数を連続的に増大させる構成例が示されている。また、図４に示す構成例では、重み付け定数の増加率は一定であるが、一定でなくてもよい。

また、重み付け定数決定部５０３は、角度θの絶対値が第３の閾値よりも大きく、第４の閾値よりも小さいときは、角度θの絶対値が増大するにしたがって、重み付け定数を減少させる。この場合、重み付け定数を連続的に減少させてもよく、また、段階的に減少させてもよい。なお、図４には、重み付け定数を連続的に減少させる構成例が示されている。また、図４に示す構成例では、重み付け定数の減少率は一定であるが、一定でなくてもよい。

また、第１の閾値は、特に限定されず、諸条件に応じて設定されるものであるが、５°以上、２５°以下であることが好ましく、１０°以上、２３°以下であることがより好ましい。第１の閾値が前記下限値よりも小さいと、他の条件によっては、補正値が大きくなり、過大なフィードバック量が発生し、制御が不安定になるおそれがある。また、第１の閾値が前記上限値よりも大きいと、他の条件によっては、振動を抑制する効果が不十分となるおそれがある。

また、第２の閾値は、特に限定されず、諸条件に応じて設定されるものであるが、３５°以上、５０°以下であることが好ましく、３７°以上、４５°以下であることがより好ましい。第２の閾値が前記下限値よりも小さいと、他の条件によっては、補正値が大きくなり、過大なフィードバック量が発生し、制御が不安定になるおそれがある。また、第２の閾値が前記上限値よりも大きいと、他の条件によっては、振動を抑制する効果が不十分となるおそれがある。

また、第３の閾値は、特に限定されず、諸条件に応じて設定されるものであるが、１３０°以上、１４５°以下であることが好ましく、１３５°以上、１４３°以下であることがより好ましい。第３の閾値が前記下限値よりも小さいと、他の条件によっては、振動を抑制する効果が不十分となるおそれがある。また、第３の閾値が前記上限値よりも大きいと、他の条件によっては、補正値が大きくなり、過大なフィードバック量が発生し、制御が不安定になるおそれがある。

また、第４の閾値は、特に限定されず、諸条件に応じて設定されるものであるが、１５５°以上、１７５°以下であることが好ましく、１５７°以上、１７０°以下であることがより好ましい。第４の閾値が前記下限値よりも小さいと、他の条件によっては、振動を抑制する効果が不十分となるおそれがある。また、第４の閾値が前記上限値よりも大きいと、他の条件によっては、補正値が大きくなり、過大なフィードバック量が発生し、制御が不安定になるおそれがある。

次に、重み付け実行部５０４は、補正値演算部５０２から入力される第１補正値、第２補正値に対し、重み付け定数決定部５０３で設定された重み付け定数を用いて、重み付けを行う。なお、第２駆動源４０２の角速度指令を補正する補正値については重み付けを行う必要がない。重み付け後の第１補正値、第２補正値は、それぞれ、第１駆動源制御部２０１、第３駆動源制御部２０３に入力される。また、重み付けがなされない前記補正値は、第２駆動源制御部２０２に入力される。

前述したように、第１駆動源制御部２０１は、重み付け後の第１補正値と、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて算出した第１駆動源４０１の角速度とに基づいて、角速度指令ωｃにフィードバックする角速度フィードバック値ωｆｂを求める。また、第２駆動源制御部２０２は、重み付けがなされない補正値と、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて算出した第２駆動源４０２の角速度とに基づいて、角速度指令ωｃにフィードバックする角速度フィードバック値ωｆｂを求める。また、第３駆動源制御部２０３は、重み付け後の第２補正値と、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて算出した第３駆動源４０３の角速度とに基づいて、角速度指令ωｃにフィードバックする角速度フィードバック値ωｆｂを求める。なお、以降の第１駆動源制御部２０１、第２駆動源制御部２０２、第３駆動源制御部２０３の制御動作は、既に述べたので、ここではその説明は省略する。

以上、説明したように、このロボット１によれば、ロボットアーム５の姿勢に応じて、第１補正値、第２補正値に対して重み付けを行うことにより、第１補正値、第２補正値を小さくすることができ、これによって、ロボットアーム５の特異姿勢付近においても、そのロボットアーム５を安定的に作動させつつ、ロボットアーム５の振動を抑制することができる。

＜第２実施形態＞
図７は、本発明のロボットの第２実施形態を示すブロック図である。
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図７に示すように、第２実施形態のロボット１のロボット制御装置２０では、ロボットアーム５の姿勢に応じて、振動演算部５０１により求められた振動情報に対して重み付けを行う。すなわち、重み付け定数決定部５０３は、重み付け定数を設定し、重み付け実行部５０４は、慣性センサー３１により検出された角速度ωを分解してなる第３回転中心軸Ｏ３の回りの角速度成分と、第４回転中心軸Ｏ４の回りの角速度成分と、第３回転中心軸Ｏ３と第４回転中心軸Ｏ４とに直交する軸の回りの角速度成分について、それぞれ、重み付けを行う（重み付け工程）。

次に、補正値演算部５０２は、前記重み付け後の各角速度成分と、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３の検出結果とに基づいて、第１駆動源４０１の角速度指令（駆動指令）を補正する第１補正値（補正値）ω１と、第２駆動源４０２の角速度指令（駆動指令）を補正する補正値と、第３駆動源４０３の角速度指令（駆動指令）を補正する第２補正値（補正値）とをそれぞれ求める（補正値演算工程）。なお、第１補正値ω１、第２補正値ω２、補正値ω３は、それぞれ、第１実施形態における重み付後の第１補正値ω１、第２補正値ω２、補正値ω３に相当する。
なお、以降の動作については、前述した第１実施形態と同様であるので、説明を省略する。
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。

＜第３実施形態＞
図８は、本発明のロボットの第３実施形態を示すブロックである。図９は、本発明のロボットの第３実施形態における振動情報および補正値を示す図である。
以下、第３実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図８に示すように、第３実施形態のロボット１では、ロボット制御装置２０は、第１座標系振動演算部５０６と、第２座標系振動演算部５０７と、重み付け定数決定部５０３と、重み付け実行部５０４と、第３座標系振動演算部５０８と、補正値演算部５０２と、駆動源制御部２０１〜２０７とを有している。

このロボットでは、まず、慣性センサー３１により、振動情報として、角速度ωを検出する。そして、第１座標系振動演算部５０６は、慣性センサー３１により検出された角速度ωを、第３リンク１４に設定された第１座標系における第３回転中心軸Ｏ３の回りの角速度成分と、第４回転中心軸Ｏ４の回りの角速度成分と、第３回転中心軸Ｏ３と第４回転中心軸Ｏ４とに直交する軸の回りの角速度成分とに分解する（第１座標系振動演算工程）。

次に、図９（ａ）に示すように、第２座標系振動演算部５０７は、前記第１座標系における振動情報を第２リンク１３に設定された第２座標系における振動情報に座標変換する（第２座標系振動演算工程）。この場合、本実施形態では、第２座標系振動演算部５０７は、第１座標系における各角速度成分を、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３の検出結果に基づいて、第３回転軸Ｏ３の回りの振動成分（角速度）ω２３と、第２回転軸Ｏ２の回りの振動成分（角速度）ω２２と、第３回転軸Ｏ３および第２回転軸Ｏ２に直交する軸６１の回りの振動成分（角速度）ω２１とに座標変換する。

重み付け定数決定部５０３および重み付け実行部５０４は、前記第１実施形態と同様に、ロボットアーム５の姿勢に応じて、第２座標系における所定の座標軸の方向の前記振動情報の成分に対して重み付けを行う（重み付け工程）。前記所定の座標軸の方向は、第１回転軸Ｏ１と直交する方向と第３回転軸Ｏ３と直交する方向とのいずれか一方である。本実施形態は、前記所定の座標軸の方向を第３回転軸Ｏ３および第２回転軸Ｏ２に直交する方向とする。すなわち、図９（ｂ）に示すように、重み付け定数決定部５０３および重み付け実行部５０４は、ロボットアーム５の姿勢に応じて、第２座標系における第３回転軸Ｏ３および第２回転軸Ｏ２に直交する軸６１の回りの振動成分ω２１に対して重み付けを行う。これにより、補正値を小さくすることができる。

次に、図９（ｃ）に示すように、第３座標系振動演算部５０８は、重み付け後の第２座標系における振動情報を第３リンク１４に設定された第３座標系における振動情報に座標変換する（第３座標系振動演算工程）。この場合、本実施形態では、第３座標系振動演算部５０８は、第２座標系における振動成分ω２３、ω２２、ω２１を、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３の検出結果に基づいて、第３回転軸Ｏ３の回りの振動成分（角速度）ω３３と、第２回転軸Ｏ２の回りの振動成分（角速度）ω３２と、第３回転軸Ｏ３および第２回転軸Ｏ２に直交する軸６１の回りの振動成分（角速度）ω３１とに座標変換する。

次に、図９（ｄ）に示すように、補正値演算部５０２は、前記第１実施形態と同様に、第３座標系の角速度と駆動源の角速度との関係を示す所定のヤコビ行列の逆行列を用い、第３座標系振動演算部５０８により求められた第３座標系における前記振動情報と、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３の検出結果とに基づいて、第１駆動源４０１の角速度指令（駆動指令）を補正する第１補正値ω１と、第２駆動源４０２の角速度指令（駆動指令）を補正する補正値ω３と、第３駆動源４０３の角速度指令（駆動指令）を補正する第２補正値ω２とをそれぞれ求める（補正値演算工程）。
なお、以降の動作については、前述した第１実施形態と同様であるので、説明を省略する。

このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１では、ロボットアーム５の姿勢に応じて、第２座標系における所定の座標軸の方向の振動情報の成分に対して重み付けを行うことにより、補正値のうち、ロボットアーム５の振動の抑制への効果の少ない補正成分、すなわち不要な補正成分を小さくすることができる。これにより、迅速かつ確実に振動を抑制することができる。

なお、本実施形態では、第３リンク１４の第３回転O３回りの回転成分を残し、第３回転O３に直交する軸回りの回転成分に対して重み付を行うように構成されているが、これに限らず、例えば、第１リンク１２の第１回転O１回りの回転成分を残し、第１回転O１に直交する軸回りの回転成分に対して重み付を行うように構成してもよい。この場合は、第２座標系は、例えば、第１リンク１２に設定される。すなわち、第２座標系は、慣性センサー３１が設けられたリンクよりも基端側に配置されたリンクに設定されていればよい。
また、第３座標系は、第２座標系が設定されたリンクと異なるリンクに設定されていればよい。

＜第４実施形態＞
図１０は、本発明のロボットの第４実施形態におけるロボット本体の概略図である。
以下、第４実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１０に示すように、第４実施形態のロボット１は、双腕ロボットであり、そのロボット本体１０は、２つロボットアーム５と、各ロボットアーム５を支持する基台（支持部）としての胴部１１０とを備えている。このロボット１は、例えば、２つのハンド９１で、それぞれ、所定の対象物を把持して作業を行うことができる。

このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１では、多彩な動作を行うことができ、多彩な作業を行うことができる。
なお、ロボットアームの数は、２つに限らず、３つ以上であってもよい。
また、第４実施形態は、第２実施形態、第３実施形態にも適用することができる。

以上、本発明のロボット制御装置、ロボットおよびロボット制御方法を、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物や工程が付加されていてもよい。
また、本発明は、前記各実施形態のうちの、任意の２以上の構成（特徴）を組み合わせたものであってもよい。

また、各駆動源のモーターとしては、それぞれ、前記サーボモーターの他、例えば、ステッピングモーター等が挙げられる。また、モーターとしてステッピングモーターを用いる場合は、角度センサーとして、例えば、ステッピングモーターへ入力する駆動パルスの数を計測することで、モーターの回転角度を検出するものを用いてもよい。
また、各角度センサーの方式は、それぞれ、特に限定されず、例えば、光学式、磁気式、電磁式、電気式等が挙げられる。

また、前記実施形態では、ロボットアームの回転軸の数は、７つであるが、本発明では、これに限定されず、ロボットアームの回転軸の数は、例えば、３つ、４つ、５つ、６つまたは８つ以上でもよい。すなわち、本実施形態では、リストが２本のリンクを有しているので、ロボットアームのリンクの本数は、７本であるが、本発明では、これに限定されず、ロボットアームのリンクの本数は、例えば、３本、４本、５本、６本または８本以上でもよい。

また、本発明では、ロボット（ロボット本体）は、他の形式のロボット、例えば、脚部を有する脚式歩行（走行）ロボット等であってもよい。
また、前記実施形態では、慣性センサーは、基台から３番目のリンクに設けられているが、本発明では、これに限定されず、慣性センサーは、基台から４番目以降のリンクに設けられてもよい。

また、前記実施形態では、慣性センサーとして、角速度センサーを用いたが、本発明では、これに限定されず、慣性センサーとして、例えば、加速度センサー等を用いもよい。
また、前記実施形態では、慣性センサーは、ロボットアームに含まれているが、本発明では、これに限定されず、慣性センサーは、ロボットアームに含まれていなくてもよい。
また、前記実施形態では、角度センサーは、ロボットアームに含まれているが、本発明では、これに限定されず、角度センサーは、ロボットアームに含まれていなくてもよい。

１……ロボット（産業用ロボット）、１０……ロボット本体 １１０……胴部、１１……基台 １２、１３、１４、１５、１６、１８、１９……リンク １７……リスト １７１、１７２、１７３、１７４、１７５、１７６、１７７……関節部（ジョイント） ２０……ロボット制御装置 １０１……床 ２０１、２０２、２０３、２０４、２０５、２０６、２０７……駆動源制御部 ３０１、３０２、３０３、３０４、３０５、３０６、３０７……モータードライバー ４０１、４０２、４０３、４０４、４０５、４０６、４０７……駆動源 ４０１Ｍ、４０２Ｍ、４０３Ｍ、４０４Ｍ、４０５Ｍ、４０６Ｍ、４０７Ｍ……モーター ４１１、４１２、４１３、４１４、４１５、４１６、４１７……角度センサー ５……ロボットアーム ３１……慣性センサー ６１……軸 ５０１……振動演算部 ５０２……補正値演算部 ５０３……重み付け定数決定部 ５０４……重み付け実行部 ５０６……第１座標系振動演算部 ５０７……第２座標系振動演算部 ５０８……第３座標系振動演算部 ９１……ハンド Ｏ１、Ｏ２、Ｏ３、Ｏ４、Ｏ５、Ｏ６、０７……回転軸

Claims (7)

1. 基台と、
   少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
   前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
   前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御装置であって、
   前記慣性センサーの検出結果に基づいて、前記慣性センサーが設けられた前記リンクに設定された第１座標系における前記ロボットアームの前記慣性センサーの位置の振動情報を求める第１座標系振動演算部と、
   前記第１座標系振動演算部により求められた前記第１座標系における前記振動情報を前記慣性センサーが設けられた前記リンクよりも基端側に配置された前記リンクに設定された第２座標系における前記振動情報に座標変換する第２座標系振動演算部と、
   前記ロボットアームの姿勢に応じて、前記第２座標系における所定の座標軸の方向の前記振動情報の成分に対して重み付けを行う重み付け部と、
   前記重み付け後の前記第２座標系における前記振動情報を前記第２座標系が設定された前記リンクと異なる前記リンクに設定された第３座標系における前記振動情報に座標変換する第３座標系振動演算部と、
   前記第３座標系振動演算部により求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算部と、
   前記駆動源の前記駆動指令と、前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御部と、を備えることを特徴とするロボット制御装置。
2. 前記ロボットアームは、前記基台に連結されて第１回転軸を軸中心として回動する第１リンクと、
   前記第１リンクに連結されて第２回転軸を軸中心として回動し、前記第２回転軸を前記第１回転軸と直交する回転軸または前記第１回転軸と直交する軸と平行な回転軸とする第２リンクと、
   前記第２リンクに連結されて第３回転軸を軸中心として回動し、前記第３回転軸を前記第２回転軸と直交する回転軸または前記第２回転軸と直交する軸と平行な回転軸とする第３リンクと、
   前記基台と前記第１リンクとを連結する第１関節部と、
   前記第１リンクと前記第２リンクとを連結する第２関節部と、
   前記第２リンクと前記第３リンクとを連結する第３関節部と、
   前記第１関節部を駆動して前記第１リンクを回動させる第１駆動源と、
   前記第２関節部を駆動して前記第２リンクを回動させる第２駆動源と、
   前記第３関節部を駆動して前記第３リンクを回動させる第３駆動源と、を有し、
   前記補正値演算部は、前記第３座標系振動演算部により求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記第１駆動源の駆動指令を補正する第１補正値および前記第３駆動源の駆動指令を補正する第２補正値を求め、
   前記駆動源制御部は、前記第１補正値により前記第１駆動源の前記駆動指令を補正し、前記第２補正値により前記第３駆動源の前記駆動指令を補正することにより、前記ロボットアームの前記慣性センサーの位置の振動を抑制する請求項１に記載のロボット制御装置。
3. 前記所定の座標軸の方向は、前記第１回転軸と直交する方向と前記第３回転軸と直交する方向とのいずれか一方である請求項２に記載のロボット制御装置。
4. 前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記角度θの絶対値が第１の閾値以下または第４の閾値以上のときは、重み付け定数を０に設定する請求項２または３に記載のロボット制御装置。
5. 前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記角度θの絶対値が第２の閾値以上、第３の閾値以下のときは、重み付け定数を１に設定する請求項２ないし４のいずれか１項に記載のロボット制御装置。
6. 前記第１回転軸と前記第３回転軸とのなす角度θを−１８０°以上、１８０°以下の範囲で設定したとき、前記重み付け部は、前記重み付けの際、前記角度θの絶対値が第１の閾値よりも大きく、第２の閾値よりも小さいときは、前記角度θの絶対値が増大するにしたがって、重み付け定数を連続的または段階的に増大させ、前記角度θの絶対値が第３の閾値よりも大きく、第４の閾値よりも小さいときは、前記角度θの絶対値が増大するにしたがって、前記重み付け定数を連続的または段階的に減少させる請求項２ないし５のいずれか１項に記載のロボット制御装置。
7. 基台と、
   少なくとも３つのリンクと、隣り合う２つの前記リンクを連結するおよび最も基端側に配置された前記リンクと前記基台とを連結する少なくとも３つの関節部と、前記各関節部を駆動する少なくとも３つの駆動源と、を有するロボットアームと、
   前記基台から３番目以降の前記リンクに設けられた慣性センサーと、
   前記各関節部の回転角度を検出する少なくとも３つの角度センサーと、を備えたロボットの作動を制御するロボット制御方法であって、
   前記慣性センサーの検出結果に基づいて、前記慣性センサーが設けられた前記リンクに設定された第１座標系における前記ロボットアームの前記慣性センサーの位置の振動情報を求める第１座標系振動演算工程と、
   前記第１座標系振動演算工程で求められた前記第１座標系における前記振動情報を前記慣性センサーが設けられた前記リンクよりも基端側に配置された前記リンクに設定された第２座標系における前記振動情報に座標変換する第２座標系振動演算工程と、
   前記ロボットアームの姿勢に応じて、前記第２座標系における所定の座標軸の方向の前記振動情報の成分に対して重み付けを行う重み付け工程と、
   前記重み付け後の前記第２座標系における前記振動情報を前記第２座標系が設定された前記リンクと異なる前記リンクに設定された第３座標系における前記振動情報に座標変換する第３座標系振動演算工程と、
   前記第３座標系振動演算工程で求められた前記第３座標系における前記振動情報と、前記角度センサーの検出結果とに基づいて、前記駆動源の駆動指令を補正する補正値を求める補正値演算工程と、
   前記駆動源の前記駆動指令と、前記補正値と、前記角度センサーの検出結果とに基づいて、前記駆動源の作動を制御する駆動源制御工程とを備えることを特徴とするロボット制御方法。

Images