JP2014046402A - ロボット - Google Patents

Abstract

【課題】使用するセンサーの数を少なくしても、容易かつ確実に、振動を抑制することができるロボットを提供すること。
【解決手段】ロボットは、基台と、第１回転軸で回動する第１アームと、第１回転軸と異なる方向の第２回転軸で回動する第２アームと、第２回転軸と平行な第３回転軸で回動する第３アームと、第１慣性センサーと、第２ａ慣性センサーと、第１角度センサーと、第３角度センサーと、各アームを回動する駆動源と、を備え、第１アームに第１慣性センサーを、および第３アームに第２ａ慣性センサーを設置し、第１駆動源に第１角度センサーを、および第３駆動源に第３角度センサーを設置し、第１慣性センサーと第１角度センサーの検出から求まる角速度を第１駆動源制御手段にフィードバックし、第２ａ慣性センサーと第３角度センサーの検出から求まる角速度を第２駆動源制御手段にフィードバックする。
【選択図】図１

Description

本発明は、ロボットに関するものである。

従来、基台（ベース）と、複数のアームリンク部とを備えるロボットが知られている（例えば、特許文献１参照）。各アームリンク部は、関節部を介して、その基端側のアームリンク部に対して回動自在に連結され、最も基台側のアームリンク部は、関節部を介して、その基台に対して回動自在に連結されている。
このようなロボットでは、基台とアームリンク部とを連結する関節部や、アームリンク部とアームリンク部とを連結する関節部は、そのバネ要素の影響により基台やアームリンク部に比べて剛性が低くなっている。このため、アームリンク部の回動やアームリンク部に加わる外乱等によってアームリンク部に振動が生じ易いという問題がある。

そこで、特許文献１に記載のロボットでは、そのロボットの最も先端側のアームリンク部の先端部に加速度センサーを設置し、その加速度センサーにより検出された加速度に基づいて、最も先端側のアームリンク部の先端部に生じた振動を抑制するように各関節部のモーターへの指令値を補償する各関節部の補償成分を算出するとともに、算出された各関節部の補償成分を対応する各関節部の指令値から減算する。

特開平１０−１０００８５号公報

しかしながら、前記特許文献１に記載のロボットでは、下記の課題があることを見出した。
最も先端側のアームリンク部の先端部に加速度センサーを設置しているので、その加速度センサーが検出する加速度を各関節部のものに換算して補正することとなる。このときヤコビ変換と呼ばれる座標軸変換を行う必要があり、多数のｓｉｎ、ｃｏｓの積を係数に持つ行列演算が必要になるため演算量が膨大になる。しかも、刻一刻と変化する各関節部のモーターの回転角度に合わせて係数を計算する必要があるので、常にこの膨大な演算を実行する必要がある。これにより、応答速度が遅くなるという欠点がある。

また、計算精度が低下すると正確な加速度や速度をフィードバックできないので、振動抑制能力が低下したり、制御性能が損なわれることがあり、このため、高速な演算器が必要になる等、制御システムの設計の制約を受ける。
また、前記座標軸変換の計算において、特異点と呼ばれる座標軸変換解がない領域（計算できない領域）があり、その領域では振動抑制能力が低下したり、かえって振動を拡大してしまうことがある。
本発明の目的は、使用するセンサーの数を少なくしても、容易かつ確実に、振動を抑制することができるロボットを提供することにある。

このような目的は、下記の本発明により達成される。
（適用例１）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームを第１角速度指令で回動させる第１駆動源と、
前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
前記第１駆動源の回転角度を検出する第１角度センサーと、
前記第２アームを第２角速度指令で回動させる第２駆動源と、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第３アームを回動させる第３駆動源と、
前記第３アームに設置され、前記第３アームの前記第２回転軸の角速度または加速度を検出する第２ａ慣性センサーと、
前記第３駆動源の回転角度を検出する第３角度センサーと、
前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
前記第２ａ慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第２補正成分をフィードバックして前記第２駆動源を制御する第２駆動源制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を少なくしても、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出する１つの慣性センサーによって、駆動源の制御のフィードバックを行う。すなわち、第２ａ慣性センサーが検出する角速度によって第２駆動源の制御をフィードバックする。

これによって、膨大な演算が不要となり、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を少なくすることができ、コストを低減することができ、また、回路構成を簡素化することができる。

そして、特に、第２駆動源制御手段では、前記角速度ωＡ３と前記角速度ωＡ２ｍと前記角速度ωＡ３ｍとから導かれる第２補正成分をフィードバックして制御する。すなわち、第２アームを回動させる第２駆動源に対して、第２アームよりも大きな振動が生じる先端側の第３アームに設置されている第２ａ慣性センサーの検出結果を用いて振動を抑える制御を行うので、振動を抑制する効果を高めることができる。また、第３アームよりも基端側の第２アームを回動させる第２駆動源の作動を制御することにより、振動を抑制する効果を高めることができる。

（適用例２）
本発明のロボットでは、前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックする前記第２駆動源制御手段と、を備えることが好ましい。
異なる方向の回転軸である第１駆動源制御手段と第２駆動源制御手段に対して、それぞれに適したフィードバックゲインを乗算した補正成分により角速度指令を補正するフィードバック制御をするので、より確実にロボットの振動を抑制することができる。

（適用例３）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームを第１角速度指令で回動させる第１駆動源と、
前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
前記第１駆動源の回転角度を検出する第１角度センサーと、
前記第２アームを回動させる第２駆動源と、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第３アームを第３角速度指令で回動させる第３駆動源と、
前記第３アームに設置され、前記第３アームの前記第２回転軸の角速度または加速度を検出する第２ａ慣性センサーと、
前記第３駆動源の回転角度を検出する第３角度センサーと、
前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
前記第２ａ慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を少なくしても、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出する１つの慣性センサーによって、駆動源の制御のフィードバックを行う。すなわち、第２ａ慣性センサーが検出する角速度によって第３駆動源の制御をフィードバックする。

これによって、膨大な演算が不要となり、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を少なくすることができ、コストを低減することができ、また、回路構成を簡素化することができる。

そして、特に、第３駆動源制御手段は、角速度ωＡ３と角速度ωＡ２ｍと角速度ωＡ３ｍとから導かれる第３補正成分をフィードバックして制御する。すなわち、第３アームを回動させる第３駆動源に対して、第２アームよりも大きな振動が生じる先端側の第３アームに設置されている第２ａ慣性センサーの検出結果を用いて振動を抑える制御を行うので、振動を抑制する効果を高めることができる。

（適用例４）
本発明のロボットでは、前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第３補正成分により前記第３角速度指令をフィードバックする前記第３駆動源制御手段と、を備えることが好ましい。
異なる方向の回転軸である第１駆動源制御手段と第３駆動源制御手段に対して、それぞれに適したフィードバックゲインを乗算した補正成分により角速度指令を補正するフィードバック制御をするので、より確実にロボットの振動を抑制することができる。

（適用例５）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームを第１角速度指令で回動させる第１駆動源と、
前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
前記第１駆動源の回転角度を検出する第１角度センサーと、
前記第２アームを第２角速度指令で回動させる第２駆動源と、
前記第２アームに設置され、前記第２アームの前記第２回転軸の角速度または加速度を検出する第２ｂ慣性センサーと、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第３アームを回動させる第３駆動源と、
前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーの検出結果から得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
前記第２ｂ慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、から導かれる第２補正成分をフィードバックして前記第２駆動源を制御する第２駆動源制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を少なくしても、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出する１つの慣性センサーによって、駆動源の制御のフィードバックを行う。すなわち、第２ｂ慣性センサーが検出する角速度によって第２駆動源の振動制御をフィードバックする。

これによって、膨大な演算が不要となり、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を少なくすることができ、コストを低減することができ、また、回路構成を簡素化することができる。
また、第３アームよりも基端側の第２アームを回動させる第２駆動源の作動を制御することにより、振動を抑制する効果を高めることができる。

（適用例６）
本発明のロボットでは、前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
前記角速度ωＡ２から前記角速度ωＡ２ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックする前記第２駆動源制御手段と、を備えることが好ましい。
異なる方向の回転軸である第１駆動源制御手段と第２駆動源制御手段に対して、それぞれに適したフィードバックゲインを乗算した補正成分により角速度指令を補正するフィードバック制御をするので、より確実にロボットの振動を抑制することができる。

（適用例７）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームを第１角速度指令で回動させる第１駆動源と、
前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
前記第１駆動源の回転角度を検出する第１角度センサーと、
前記第２アームを回動させる第２駆動源と、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第２アームに設置され、前記第２アームの前記第２回転軸の角速度または加速度を検出する第２ｂ慣性センサーと、
前記第３アームを第３角速度指令で回動させる第３駆動源と、
前記第３駆動源の回転角度を検出する第３角度センサーと、
前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１の駆動源を制御する第１駆動源制御手段と、
前記第２ｂ慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を少なくしても、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出する１つの慣性センサーによって、駆動源の制御のフィードバックをおこなう。すなわち、第２ｂ慣性センサーが検出する角速度によって第３駆動源の制御をフィードバックする。

これによって、膨大な演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を少なくすることができ、コストを低減することができ、また、回路構成を簡素化することができる。

（適用例８）
本発明のロボットでは、前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
前記角速度ωＡ２から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第３補正成分により前記第３角速度指令をフィードバックする前記第３駆動源制御手段と、を備えることが好ましい。
異なる方向の回転軸である第１駆動源制御手段と第３駆動源制御手段に対して、それぞれに適したフィードバックゲインを乗算した補正成分により角速度指令を補正するフィードバック制御をするので、より確実にロボットの振動を抑制することができる。

（適用例９）
本発明のロボットでは、前記第１慣性センサーを設置した前記第１アームと、
前記第２ａ慣性センサーを設置した前記第３アームと、
慣性センサーを未設置とした前記第２アームと、を備えることが好ましい。
慣性センサーの数を少なくすることができるので、装置コストを低減できる。また、第２アームを軽量化できる。

（適用例１０）
本発明のロボットでは、前記第１慣性センサーを設置した前記第１アームと、
前記第２ｂ慣性センサーを設置した前記第２アームと、
慣性センサーを未設置とした前記第３アームと、を備えることが好ましい。
慣性センサーの数を少なくすることができるので、装置コストを低減できる。また、第３アームを軽量化できる。

（適用例１１）
本発明のロボットでは、前記第１慣性センサーは、前記第１アームの先端部に設置され、
前記第２ａ慣性センサーは、前記第３アームの先端部に設置されていることが好ましい。
これにより、第１慣性センサーは、第１アームの振動が最大の部位において第１アームの角速度または加速度を検出し、第２ａ慣性センサーは、第３アームの振動が最大の部位において第３アームの角速度または加速度を検出するので、より確実にロボットの振動を抑制することができる。

（適用例１２）
本発明のロボットでは、前記第１慣性センサーは、前記第１アームの先端部に設置され、
前記第２ｂ慣性センサーは、前記第２アームの先端部に設置されていることが好ましい。
これにより、第１慣性センサーは、第１アームの振動が最大の部位において第１アームの角速度または加速度を検出し、第２ｂ慣性センサーは、第２アームの振動が最大の部位において第２アームの角速度または加速度を検出するので、より確実にロボットの振動を抑制することができる。

（適用例１３）
本発明のロボットでは、前記第１回転軸は、前記基台の設置面の法線と一致することが好ましい。
これにより、ロボットの制御を容易に行うことができる。
（適用例１４）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１回転軸と異なる方向の第２回転軸を軸中心として回動する第２アームと、
前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームの角速度を検出する第１慣性センサーと、
前記第１アームの駆動源の回転角度を検出する第１角度センサーと、
前記第２アームの駆動源の回転角度を検出する第２角度センサーと、
前記第３アームの角速度を検出する第２ａ慣性センサーと、
前記第３アームの駆動源の回転角度を検出する第３角度センサーと、
前記第１角度センサーの検出結果から導かれる角速度と前記第１慣性センサーから検出される角速度とをフィードバックして制御する前記第１アームの駆動源の制御手段と、
前記第２角度センサーの検出結果から導かれる角速度と前記第３角度センサーの検出結果から導かれる角速度と前記第２ａ慣性センサーから検出される角速度とをフィードバックして制御する前記第２アームの駆動源の制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を最少としつつも、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出するひとつの慣性センサーによって、駆動源の制御のフィードバックを行う。すなわち、第２ａ慣性センサーが検出する角速度によって第２駆動源の制御をフィードバックする。

これによって、膨大な演算が不要となり、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を最少とすることができ、コストを低減することができ、また、回路構成を簡素化することができる。

そして、第２アームを回動させる第２駆動源に対して、第２アームよりも大きな振動が生じる先端側の第３アームに設置されている第２ａ慣性センサーの検出結果を用いて振動を抑える制御を行うので、振動を抑制する効果を高めることができる。また、第３アームよりも基端側の第２アームを回動させる第２駆動源の作動を制御することにより、振動を抑制する効果を高めることができる。

（適用例１５）
本発明のロボットは、基台と、
前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
前記第１回転軸と異なる方向の第２回転軸を軸中心として回動する第２アームと、
前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
前記第１アームの角速度を検出する第１慣性センサーと、
前記第１アームの駆動源の回転角度を検出する第１角度センサーと、
前記第２アームの駆動源の回転角度を検出する第２角度センサーと、
前記第３アームの角速度を検出する第２ａ慣性センサーと、
前記第３アームの駆動源の回転角度を検出する第３角度センサーと、
前記第１角度センサーの検出結果から導かれる角速度と前記第１慣性センサーから検出される角速度とをフィードバックして制御する前記第１アームの駆動源の制御手段と、
前記第２角度センサーの検出結果から導かれる角速度と前記第３角度センサーの検出結果から導かれる角速度と前記第２ａ慣性センサーから検出される角速度とをフィードバックして制御する前記第３アームの駆動源の制御手段と、を備えることを特徴とする。

本発明では、使用するセンサーの数を最少としつつ、容易かつ確実に、ロボットの振動を抑制することができる。第２回転軸の方向と第３回転軸の方向とを平行にし、この回転軸の方向の角速度を検出するひとつの慣性センサーによって、駆動源の制御のフィードバックを行う。すなわち、第２ａ慣性センサーが検出する角速度によって第３駆動源の制御をフィードバックする。

これによって、膨大な演算が不要となり、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を削減することができ、コストを低減することができ、また、回路構成を簡素化することができる。
そして、特に、第３アームを回動させる第３駆動源に対して、第２アームよりも大きな振動が生じる先端側の第３アームに設置されている第２ａ慣性センサーの検出結果を用いて振動を抑える制御を行うので、振動を抑制する効果を高めることができる。

（適用例１６）
本発明のロボットは、複数の異なる回転軸を有し、前記異なる回転軸毎に対応して一つの慣性センサーと一つの角度センサーとが設けられ、前記角度センサーおよび前記慣性センサーから得られる角速度を当該角度センサーおよび当該慣性センサーに対応する回転軸毎にフィードバック制御することを特徴とする。
これにより、３次元空間を自在に作業できる多関節ロボットでありながら、容易かつ確実に、駆動による振動を抑制するロボットを提供することができる。

本発明のロボットでは、複数の異なる回転軸を有し、前記異なる回転軸毎に対応して一つの慣性センサーと一つの角度センサーとが設けられ、前記角度センサーおよび前記慣性センサーから得られる角速度を当該角度センサーおよび当該慣性センサーに対応する回転軸毎にフィードバック制御するので、慣性センサーからの情報は少なくともひとつの座標軸の情報のみでよい。従って、例えばヤコビ変換等の座標軸変換が不要であり、簡便な演算で制御部へフィードバックできる。

すなわち、膨大な情報に基づいて制御部へフィードバックする為の演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。なお、異なる回転軸毎に対応して一つの慣性センサーと一つの角度センサーとが設けられ、とは平行な回転軸が複数ある場合ではその中の一つの回転軸を選択して、この一つの回転軸と異なる回転軸から一つの回転軸を選択し、それぞれに一つの慣性センサーと一つの角度センサーとが設けられてもよい。

本発明のロボットの第１実施形態を正面側から見た斜視図である。 図１に示すロボットを背面側から見た斜視図である。 図１に示すロボットの概略図である。 図１に示すロボットの概略図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 本発明のロボットの第２実施形態の主要部を示すブロック図である。 本発明のロボットの第３実施形態を示す概略図である。 図１２に示すロボットの主要部のブロック図である。 本発明のロボットの第３実施形態の主要部を示すブロック図である。 本発明のロボットの第６実施形態の主要部を示すブロック図である。 本発明のロボットの他の構成例を示す正面図である。

以下、本発明のロボットを添付図面に示す好適な実施形態に基づいて詳細に説明する。
＜第１実施形態＞
図１は、本発明のロボットの第１実施形態を正面側から見た斜視図である。図２は、図１に示すロボットを背面側から見た斜視図である。図３および図４は、それぞれ、図１に示すロボットの概略図である。図５は、図１に示すロボットの主要部のブロック図である。図６〜図１０は、それぞれ、図１に示すロボットの主要部のブロック図である。

なお、以下では、説明の都合上、図１〜図４、図６中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１〜図４中の基台側を「基端」、その反対側を「先端」と言う。また、図４では、回転軸Ｏ２、Ｏ３は、それぞれ、誇張して図示されている。また、図４では、慣性センサー３１、３２ａは、それぞれ、その存在を明確にするため、アーム１２、１４の外部に図示されている。

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

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

図３および図４に示すように、アーム１２〜１５、リスト１６は、それぞれ、基台１１に対し独立して変位可能に支持されている。このアーム１２〜１５、リスト１６の長さは、それぞれ、特に限定されないが、図示の構成では、アーム１２〜１４の長さが、他のアーム１５およびリスト１６よりも長く設定されている。なお、例えば、第３アーム１４の長さを第１アーム１２および第２アーム１３の長さよりも短くしてもよい。

基台１１と第１アーム１２とは、関節（ジョイント）１７１を介して連結されている。そして、第１アーム１２は、基台１１に対し、鉛直方向と平行な第１回転軸Ｏ１を回転中心とし、その第１回転軸Ｏ１回りに回動自在となっている。第１回転軸Ｏ１は、基台１１の設置面である床１０１の上面の法線と一致している。この第１回転軸Ｏ１回りの回動は、第１駆動源４０１の駆動によりなされる。また、第１駆動源４０１はモーター４０１Ｍとケーブル（図示せず）によって駆動され、このモーター４０１Ｍは電気的に接続されたモータードライバー３０１を介して制御装置２０により制御される（図５参照）。第１駆動源４０１はモーター４０１Ｍとともに設けた減速機（図示せず）によってモーター４０１Ｍからの駆動を伝達されても良く、また、減速機が省略されていてもよい。

第１アーム１２と第２アーム１３とは、関節（ジョイント）１７２を介して連結されている。そして、第２アーム１３は、第１アーム１２に対し、水平方向と平行な第２回転軸Ｏ２を軸中心として回動自在となっている。第２回転軸Ｏ２は、第１回転軸Ｏ１と直交している。この第２回転軸Ｏ２回りの回動は、第２駆動源４０２の駆動によりなされる。また、第２駆動源４０２はモーター４０２Ｍとケーブル（図示せず）によって駆動され、このモーター４０２Ｍは電気的に接続されたモータードライバー３０２を介して制御装置２０により制御される（図５参照）。第２駆動源４０２はモーター４０２Ｍの他に設けた減速機（図示せず）によってモーター４０２Ｍからの駆動を伝達されても良く、また、減速機が省略されていてもよい。

第２アーム１３と第３アーム１４とは、関節（ジョイント）１７３を介して連結されている。そして、第３アーム１４は、第２アーム１３に対して水平方向と平行な回転軸Ｏ３を回転中心とし、その第３回転軸Ｏ３回りに回動可能となっている。第３回転軸Ｏ３は、第２回転軸Ｏ２と平行である。この第３回転軸Ｏ３回りの回動は、第３駆動源４０３の駆動によりなされる。また、第３駆動源４０３は、モーター４０３Ｍとケーブル（図示せず）によって駆動され、このモーター４０３Ｍは電気的に接続されたモータードライバー３０３を介して制御装置２０により制御される（図５参照）。第３駆動源４０３はモーター４０３Ｍの他に減速機（図示せず）も設けてモーター４０３Ｍからの駆動を伝達されても良く、また、減速機が省略されていてもよい。

第３アーム１４と第４アーム１５とは、関節（ジョイント）１７４を介して連結されている。そして、第４アーム１５は、第３アーム１４（基台１１）に対し、第３アーム１４の中心軸方向と平行な第４回転軸Ｏ４を回転中心とし、その第４回転軸Ｏ４回りに回動自在となっている。第４回転軸Ｏ４は、第３回転軸Ｏ３と直交している。この第４回転軸Ｏ４回りの回動は、第４駆動源４０４の駆動によりなされる。また、第４駆動源４０４は、モーター４０４Ｍとケーブル（図示せず）によって駆動され、このモーター４０４Ｍは電気的に接続されたモータードライバー３０４を介して制御装置２０により制御される（図５参照）。第４駆動源４０４はモーター４０４Ｍとともに設けた減速機（図示せず）によってモーター４０４Ｍからの駆動を伝達されてもよく、また、減速機が省略されていてもよい。なお、第４回転軸Ｏ４は、第３回転軸Ｏ３に直交する軸と平行であってもよい。

第４アーム１５とリスト１６とは、関節（ジョイント）１７５を介して連結されている。そして、リスト１６は、第４アーム１５に対して水平方向（ｙ軸方向）と平行な第５回転軸Ｏ５を回転中心とし、その第５回転軸Ｏ５回りに回動自在となっている。第５回転軸Ｏ５は、第４回転軸Ｏ４と直交している。この第５回転軸Ｏ５回りの回動は、第５駆動源４０５の駆動によりなされる。また、第５駆動源４０５は、モーター４０５Ｍとケーブル（図示せず）によって駆動され、このモーター４０５Ｍは電気的に接続されたモータードライバー３０５を介して制御装置２０により制御される（図５参照）。第５駆動源４０５はモーター４０５Ｍとともに設けた減速機（図示せず）によってモーター４０５Ｍからの駆動を伝達されてもよく、また、減速機が省略されていてもよい。また、リスト１６は、関節（ジョイント）１７６を介して、第５回転軸Ｏ５と垂直な第６回転軸Ｏ６を回転中心とし、その第６回転軸Ｏ６回りにも回動自在となっている。回転軸Ｏ６は、回転軸Ｏ５と直交している。この第６回転軸Ｏ６回りの回動は、第６駆動源４０６の駆動によりなされる。また、第６駆動源４０６の駆動は、モーター４０６Ｍとケーブル（図示せず）によって駆動され、このモーター４０６Ｍは電気的に接続されたモータードライバー３０６を介して制御装置２０により制御される（図５参照）。第６駆動源４０６はモーター４０６Ｍの他に減速機（図示せず）も設けてモーター４０６Ｍからの駆動を伝達されても良く、また、減速機が省略されていてもよい。なお、第５回転軸Ｏ５は、第４回転軸Ｏ４に直交する軸と平行であってもよく、また、第６回転軸Ｏ６は、第５回転軸Ｏ５に直交する軸と平行であってもよい。

また、第１アーム１２には、第１慣性センサー３１が設置されている。この第１慣性センサー３１により第１アーム１２の第１回転軸Ｏ１の回りの角速度を検出する。第１アーム１２における第１慣性センサー３１の設置位置は、特に限定されないが、第１アーム１２の先端部が好ましい。本実施形態では、第１慣性センサー３１は、第１アーム１２の内部の先端部に設置されている。第１アーム１２の振動は、その先端部において最大になるので、これにより、より確実にロボット１の振動を抑制することができる。なお、第１慣性センサー３１が、第１アーム１２の基端部に設置されていてもよいことは、言うまでもない。

また、第３アーム１４には、第２ａ慣性センサー３２ａが設置されている。この第２ａ慣性センサー３２ａにより第３アーム１４の第２回転軸Ｏ２の回りの角速度を検出する。第３アーム１４における第２ａ慣性センサー３２ａの設置位置は、特に限定されないが、第３アーム１４の先端部が好ましい。本実施形態では、第２ａ慣性センサー３２ａは、第３アーム１４の内部の先端部に設置されている。第３アーム１４の振動は、その先端部において最大になるので、これにより、より確実にロボット１の振動を抑制することができる。なお、第２ａ慣性センサー３２ａが、第３アーム１４の基端部に設置されていてもよいことは、言うまでもない。
また、第１慣性センサー３１、第２ａ慣性センサー３２ａとしては、それぞれ、特に限定されず、本実施形態では、例えば、ジャイロセンサー、加速度センサー等を用いることができる。

ここで、このロボット１では、アーム１２、１３および１４の振動を抑制することにより、ロボット１全体の振動を抑制する。但し、アーム１２、１３および１４の振動を抑制するために、アーム１２、１３および１４のすべてに慣性センサーを設置するのではなく、前記のようにアーム１２、１４のみに第１慣性センサー３１、第２ａ慣性センサー３２ａを設置し、その第１慣性センサー３１、第２ａ慣性センサー３２ａ検出結果に基づいて駆動源４０１、４０２の作動を制御する。これにより、アーム１２、１３および１４のすべてに慣性センサーを設置する場合に比べ、慣性センサーの数を削減することができ、コストを低減することができ、また、回路構成を簡素化することができる。

駆動源４０１〜４０６には、それぞれのモーターまたは減速機に、第１角度センサー４１１、第２角度センサー４１２、第３角度センサー４１３、第４角度センサー４１４、第５角度センサー４１５、第６角度センサー４１６が設けられている。これらの角度センサーとして、エンコーダー、ロータリーエンコーダー等が用いることができる。これらの角度センサー４１１〜４１６により、それぞれ、駆動源４０１〜４０６のモーターあるいは減速機の回転軸の回転角度を検出する。この駆動源４０１〜４０６のモーターとしては、それぞれ、特に限定されず、例えば、ＡＣサーボモーター、ＤＣサーボモーター等のサーボモーターを用いるのが好ましい。また、前記各ケーブルは、それぞれ、ロボット本体１０を挿通していてもよい。

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

図１、図２に示すように、基台１１は、ロボット１が垂直多関節ロボットの場合、当該垂直多関節ロボットの最も下方に位置し、設置スペースの床１０１に固定される部分である。この固定方法としては、特に限定されず、例えば、図１、図２に示す本実施形態では、複数本のボルト１１１による固定方法を用いている。なお、基台１１の設置スペースでの固定箇所としては、床の他に、設置スペースの壁や天井とすることもできる。

基台１１は、中空の基台本体（ハウジング）１１２を有している。基台本体１１２は、円筒状をなす円筒状部１１３と、当該円筒状部１１３の外周部に一体的に形成された、箱状をなす箱状部１１４とに分けることができる。そして、このような基台本体１１２には、例えば、モーター４０１Ｍやモータードライバー３０１〜３０６が収納されている。
アーム１２〜１５は、それぞれ、中空のアーム本体２と、駆動機構３と、封止手段４とを有している。なお、以下では、説明の都合上、第１アーム１２が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ａ」、「駆動機構３ａ」、「封止手段４ａ」と言い、第２アーム１３が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｂ」、「駆動機構３ｂ」、「封止手段４ｂ」と言い、第３アーム１４が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｃ」、「駆動機構３ｃ」、「封止手段４ｃ」と言い、第４アーム１５が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｄ」、「駆動機構３ｄ」、「封止手段４ｄ」と言うことがある。

また、関節１７１〜１７６は、それぞれ、回動支持機構（図示せず）を有している。この回動支持機構は、互いに連結された２本のアームのうちの一方を他方に対し回動可能に支持する機構、互いに連結された基台１１と第１アーム１２のうちの一方を他方に対し回動可能に支持する機構、互いに連結された第４アーム１５と第５リスト１６のうちの一方を他方に対し回動可能に支持する機構である。互いに連結された第４アーム１５とリスト１６とを一例とした場合、回動支持機構は、リスト１６を第４アーム１５に対し回動させることができる。また、各回動支持機構は、それぞれ、対応するモーターの回転速度を所定の減速比で減速して、その駆動力を対応するアーム、リスト１６のリスト本体１６１、支持リング１６２に伝達する減速機（図示せず）を有している。

第１アーム１２は、基台１１の上端部（先端部）に水平方向に対し傾斜した姿勢で連結されている。この第１アーム１２では、駆動機構３ａがモーター４０２Ｍを有しており、アーム本体２ａ内に収納している。また、アーム本体２ａ内は、封止手段４ａにより気密封止されている。
第２アーム１３は、第１アーム１２の先端部に連結されている。この第２アーム１３では、駆動機構３ｂがモーター４０３Ｍを有しており、アーム本体２ｂ内に収納している。また、アーム本体２ａ内は、封止手段４ｂにより気密封止されている。

第３アーム１４は、第２アーム１３の先端部に連結されている。この第３アーム１４では、駆動機構３ｃがモーター４０４Ｍを有しており、アーム本体２ｃ内に収納している。また、アーム本体２ｃ内は、封止手段４ｃにより気密封止されている。
第４アーム１５は、第３アーム１４の先端部に、その中心軸方向と平行に連結されている。このアーム１５では、駆動機構３ｄがモーター４０５Ｍ、４０６Ｍを有しており、アーム本体２ｄ内に収納している。また、アーム本体２ｄ内は、封止手段４ｄにより気密封止されている。

第４アーム１５の先端部（基台１１と反対側の端部）には、リスト１６が連結されている。このリスト１６には、その先端部（第４アーム１５と反対側の端部）に、例えば、腕時計等のような精密機器を把持するマニピュレーター（図示せず）が着脱自在に装着される。なお、マニピュレーターとしては、特に限定されず、例えば、複数本の指部（フィンガー）を有する構成のものが挙げられる。そして、このロボット１は、マニピュレーターで精密機器を把持したまま、アーム１２〜１５やリスト１６等の動作を制御することにより、当該精密機器を搬送することができる。

リスト１６は、円筒状をなすリスト本体（第６アーム）１６１と、リスト本体１６１と別体で構成され、当該リスト本体１６１の基端部に設けられ、リング状をなす支持リング（第５アーム）１６２とを有している。
リスト本体１６１の先端面１６３は、平坦な面となっており、マニピュレーターが装着される装着面となる。また、リスト本体１６１は、関節１７６を介して、第４アーム１５の駆動機構３ｄに連結されており、当該駆動機構３ｄのモーター４０６Ｍの駆動により、回転軸Ｏ６回りに回動する。
支持リング１６２は、関節１７５を介して、第４アーム１５の駆動機構３ｄに連結されており、当該駆動機構３ｄのモーター４０５Ｍの駆動により、リスト本体１６１ごと回転軸Ｏ５回りに回動する。

次に、図５、図６〜図１０を参照し、制御装置２０の構成について説明する。
図５、図６〜図１０に示すように、制御装置２０は、第１駆動源４０１の作動を制御する第１駆動源制御部（第１駆動源制御手段）（第１角速度指令）２０１と、第２駆動源４０２の作動を制御する第２駆動源制御部（第２駆動源制御手段）（第２角速度指令）２０２と、第３駆動源４０３の作動を制御する第３駆動源制御部（第３駆動源制御手段）（第３角速度指令）２０３と、第４駆動源４０４の作動を制御する第４駆動源制御部（第４駆動源制御手段）（第４角速度指令）２０４と、第５駆動源４０５の作動を制御する第５駆動源制御部（第５駆動源制御手段）（第５角速度指令）２０５と、第６駆動源４０６の作動を制御する第６駆動源制御部（第６駆動源制御手段）（第６角速度指令）２０６と、を有している。

図６に示すように、第１駆動源制御部２０１は、減算器５１１と、位置制御部５２１と、減算器５３１と、角速度制御部５４１と、回転角度算出部５５１と、角速度算出部５６１と、減算器５７１と、変換部５８１と、補正値算出部５９１と、加算器６０１とを有している。
図７に示すように、第２駆動源制御部２０２は、減算器５１２と、位置制御部５２２と、減算器５３２と、角速度制御部５４２と、回転角度算出部５５２と、角速度算出部５６２と、加減算器６２２と、変換部５８２と、補正値算出部５９２と、加算器６０２とを有している。

図７に示すように、第３駆動源制御部２０３は、減算器５１３と、位置制御部５２３と、減算器５３３と、角速度制御部５４３と、回転角度算出部５５３と、角速度算出部５６３とを有している。
図８に示すように、第４駆動源制御部２０４は、減算器５１４と、位置制御部５２４と、減算器５３４と、角速度制御部５４４と、回転角度算出部５５４と、角速度算出部５６４とを有している。

図９に示すように、第５駆動源制御部２０５は、減算器５１５と、位置制御部５２５と、減算器５３５と、角速度制御部５４５と、回転角度算出部５５５と、角速度算出部５６５とを有している。
図１０に示すように、第６駆動源制御部２０６は、減算器５１６と、位置制御部５２６と、減算器５３６と、角速度制御部５４６と、回転角度算出部５５６と、角速度算出部５６６とを有している。

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

図６に示すように、第１駆動源制御部２０１には、第１駆動源４０１の位置指令Ｐｃの他、第１角度センサー４１１、第１慣性センサー３１からそれぞれ検出信号が入力される。第１駆動源制御部２０１は、第１角度センサー４１１の検出信号から算出される第１駆動源の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によって第１駆動源４０１を駆動する。

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

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

減算器５３１には、角速度指令ωｃが入力され、また、後述する角速度フィードバック値ωｆｂが入力される。減算器５３１は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第１駆動源４０１の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４１に出力する。
角速度制御部５４１は、減算器５３１から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第１駆動源４０１の駆動信号（駆動電流）を生成し、モータードライバー３０１を介してモーター４０１Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第１駆動源４０１の駆動電流が制御される。

次に、第１駆動源制御部２０１における角速度フィードバック値ωｆｂについて説明する。
角速度算出部５６１では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１駆動源４０１の角速度ωｍ１が算出され、その角速度ωｍ１は、加算器６０１に出力される。

また、角速度算出部５６１では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１アーム１２の回転軸Ｏ１の回りの角速度ωＡ１ｍが算出され、その角速度ωＡ１ｍは、減算器５７１に出力される。なお、角速度ωＡ１ｍは、角速度ωｍ１を、第１駆動源４０１のモーター４０１Ｍと第１アーム１２との間、すなわち、関節１７１における減速比で除算した値である。

また、第１慣性センサー３１により、第１アーム１２の回転軸Ｏ１の回りの角速度が検出される。そして、その第１慣性センサー３１の検出信号、すなわち、第１慣性センサー３１により検出された第１アーム１２の回転軸Ｏ１の回りの角速度ωＡ１は、減算器５７１に出力される。
減算器５７１には、角速度ωＡ１および角速度ωＡ１ｍが入力され、減算器５７１は、この角速度ωＡ１から角速度ωＡ１ｍを減算した値ωＡ１ｓ（＝ωＡ１−ωＡ１ｍ）を変換部５８１に出力する。この値ωＡ１ｓは、第１アーム１２の回転軸Ｏ１の回りの角速度の振動成分（振動角速度）に相当する。以下、ωＡ１ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ１ｓ（詳細には、振動角速度ωＡ１ｓに基づいて生成した値であるモーター４０１Ｍにおける角速度ωｍ１ｓ）が後述するゲインＫａ倍されて駆動源４０１の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ１ｓが可及的に０になるように、駆動源４０１に対してフィードバック制御がなされる。これにより、ロボット１の振動を抑制することができる。なお、このフィードバック制御では、駆動源４０１の角速度が制御される。

変換部５８１は、振動角速度ωＡ１ｓを第１駆動源４０１における角速度ωｍ１ｓに変換し、その角速度ωｍ１ｓを補正値算出部５９１に出力する。この変換は、振動角速度ωＡ１ｓに、第１駆動源４０１のモーター４０１Ｍと第１アーム１２との間、すなわち、関節１７１における減速比を乗算することで得ることができる。
補正値算出部５９１は、角速度ωｍ１ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第１補正成分）Ｋａ・ωｍ１ｓを求め、その補正値Ｋａ・ωｍ１ｓを加算器６０１に出力する。
加算器６０１には、角速度ωｍ１が入力され、また、補正値Ｋａ・ωｍ１ｓが入力される。加算器６０１は、角速度ωｍ１と補正値Ｋａ・ωｍ１ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３１に出力する。なお、以降の動作は、前述した通りである。

図７に示すように、第２駆動源制御部２０２には、第２駆動源４０２の位置指令Ｐｃの他、第２角度センサー４１２、第２ａ慣性センサー３２ａからそれぞれ検出信号が入力される。また、第２駆動源制御部２０２には、第３駆動源制御部２０３から第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが入力される。第２駆動源制御部２０２は、第２角度センサー４１２の検出信号から算出される第２駆動源４０２の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によって第２駆動源４０２を駆動する。

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

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

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

このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第２駆動源４０２の駆動電流が制御される。なお、回転軸Ｏ２は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けず、第１駆動源４０１に対して独立して第２駆動源４０２の作動を制御することができる。

次に、第２駆動源制御部２０２における角速度フィードバック値ωｆｂについて説明する。
角速度算出部５６９では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度ωｍ２が算出され、その角速度ωｍ２は、加算器６０２に出力される。

また、角速度算出部５６２では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２アーム１３の回転軸Ｏ２の回りの角速度ωＡ２ｍが算出され、その角速度ωＡ２ｍは、加減算器６２２に出力される。なお、角速度ωＡ２ｍは、角速度ωｍ２を、第２駆動源４０２のモーター４０２Ｍと第２アーム１３との間、すなわち、関節１７２における減速比で除算した値である。

また、第２ａ慣性センサー３２ａにより、第３アーム１４の回転軸Ｏ２の回りの角速度が検出される。そして、その第２ａ慣性センサー３２ａの検出信号、すなわち、第２ａ慣性センサー３２ａにより検出された第３アーム１４の回転軸Ｏ２の回りの角速度ωＡ３は、加減算器６２２に出力される。なお、回転軸Ｏ２、Ｏ３は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けずに、容易かつ確実に、第３アーム１３の回転軸Ｏ２の回りの角速度を求めることができる。

また、後述する第３駆動源制御部２０３の角速度算出部５６３からは、第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが加減算器６２２に出力される。
加減算器６２２には、角速度ωＡ３、角速度ωＡ２ｍおよび角速度ωＡ３ｍが入力され、加減算器６２２は、角速度ωＡ３から角速度ωＡ２ｍおよび角速度ωＡ３ｍを減算した値ωＡ２ｓ（＝ωＡ３−ωＡ２ｍ−ωＡ３ｍ）を変換部５８２に出力する。この値ωＡ２ｓは、第２アーム１３と第３アーム１４の回転軸Ｏ２の回りの合計の角速度の振動成分（振動角速度）に相当する。以下、ωＡ２ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ２ｓ（詳細には、振動角速度ωＡ２ｓに基づいて生成した値であるモーター４０２Ｍにおける角速度ωｍ２ｓ）が後述するゲインＫａ倍されて第２駆動源４０２の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ２ｓが可及的に０になるように、第２駆動源４０２に対してフィードバック制御がなされる。これにより、ロボット１の振動を抑制することができる。なお、このフィードバック制御では、第２駆動源４０２の角速度が制御される。

変換部５８２は、振動角速度ωＡ２ｓを第２駆動源４０２における角速度ωｍ２ｓに変換し、その角速度ωｍ２ｓを補正値算出部５９２に出力する。この変換は、振動角速度ωＡ２ｓに、第２駆動源４０２のモーター４０２Ｍと第２アーム１３との間、すなわち、関節１７２における減速比を乗算することで得ることができる。
補正値算出部５９２は、角速度ωｍ２ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第２補正成分）Ｋａ・ωｍ２ｓを求め、その補正値Ｋａ・ωｍ２ｓを加算器６０２に出力する。なお、この第２駆動源制御部２０２におけるゲインＫａと、第１駆動源制御部２０１におけるゲインＫａとは、同一でもよく、また、異なっていてもよい。
加算器６０２には、角速度ωｍ２が入力され、また、補正値Ｋａ・ωｍ２ｓが入力される。加算器６０２は、角速度ωｍ２と補正値Ｋａ・ωｍ２ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３２に出力する。なお、以降の動作は、前述した通りである。

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

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

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

また、角速度算出部５６３では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３駆動源４０３の角速度が算出され、その角速度が角速度フィードバック値ωｆｂとして減算器５３３に出力される。
減算器５３３には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。減算器５３３は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第３駆動源４０３の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４３に出力する。

角速度制御部５４３は、減算器５３３から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第３駆動源４０３の駆動信号（駆動電流）を生成し、モータードライバー３０３を介してモーター４０３Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。

このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第３駆動源４０３の駆動電流が制御される。
なお、駆動源制御部２０４〜２０６については、それぞれ、前記第３駆動源制御部２０３と同様であるので、その説明は省略する。
以上説明したように、このロボット１によれば、容易かつ確実に、ロボット１の振動を抑制することができる。

まず、ロボット１の制御において、膨大な演算が不要であり、これにより、ロボット１の制御における応答速度を速くすることができ、また、制御装置２０の構成を簡素化することができる。
また、ロボット１の制御において、特異点の存在する演算が不要であるので、確実に、ロボット１の制御を行うことができ、これにより、確実に振動を抑制することができる。

また、第２アーム１３を駆動する第２駆動源４０２に対して、第２アーム１３よりも大きな振動が生じる先端側の第３アーム１４に設置されている第２ａ慣性センサー３２ａの検出結果を用いて、振動を抑える制御を行うので、ロボット１の振動を抑制する効果を高めることができる。また、第３アーム１４よりも基端側の第２アーム１３を回動させる第２駆動源４０２の作動を制御することにより、ロボット１の振動を抑制する効果を高めることができる。

また、第１アーム１２と第３アーム１４にそれぞれ慣性センサー３１、３２ａを設置するとともに、第１アーム１２の回動の第１回転軸Ｏ１と、第２アーム１３の回動の第２回転軸Ｏ２および第３アーム１４の回動の第３回転軸Ｏ３とを互いに直交させたので、第１アーム１２と第３アーム１４の角速度が互いに混在しない単純な回転成分として検出できる。よってこれらを用いた演算による制御を行うので、より容易に、精度良く、確実に、ロボット１の振動を抑制することができる。

加えて、第１アーム１２の回動の第１回転軸Ｏ１と、第２アーム１３の回動の第２回転軸Ｏ２および第３アーム１４の回動の第３回転軸Ｏ３とを互いに直交させたので、第１アーム１２と第３アーム１４の角速度が互いに混在しない単純な回転成分として検出でき、これらの混在無き回転成分にそれぞれ、フィードバックゲインを乗算するので、それぞれが高い精度で補正できる。

＜第２実施形態＞
図１１は、本発明のロボットの第２実施形態の主要部を示すブロック図である。
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１１に示すように、第２実施形態のロボット１では、制御装置２０の第２駆動源制御部２０２、第３駆動源制御部２０３がそれぞれ第１実施形態と異なっている。すなわち、このロボット１では、第１慣性センサー３１、第２ａ慣性センサー３２ａの検出結果に基づいて第１駆動源４０１、第３駆動源４０３の作動を制御する。以下、第２駆動源制御部２０２、第３駆動源制御部２０３について説明する。

図１１に示すように、第２駆動源制御部２０２は、減算器５１７と、位置制御部５２７と、減算器５３７と、角速度制御部５４７と、回転角度算出部５５７と、角速度算出部５６７とを有している。
また、第３駆動源制御部２０３は、減算器５１８と、位置制御部５２８と、減算器５３８と、角速度制御部５４８と、回転角度算出部５５８と、角速度算出部５６８と、加減算器６１８と、変換部５８８と、補正値算出部５９８と、加算器６０８とを有している。

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

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

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

また、角速度算出部５６７では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度が算出され、その角速度が角速度フィードバック値ωｆｂとして減算器５３７に出力される。
減算器５３７には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。減算器５３７は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第２駆動源４０２の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４３に出力する。

角速度制御部５４７は、減算器５３７から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第２駆動源４０２の駆動信号（駆動電流）を生成し、モータードライバー３０２を介してモーター４０２Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第２駆動源４０２の駆動電流が制御される。

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

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

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

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

このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第３駆動源４０３の駆動電流が制御される。なお、回転軸Ｏ３は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けず、第１駆動源４０１に対して独立して第３駆動源４０３の作動を制御することができる。

次に、第３駆動源制御部２０３における角速度フィードバック値ωｆｂについて説明する。
角速度算出部５６８では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３駆動源４０３の角速度ωｍ３が算出され、その角速度ωｍ３は、加算器６０８に出力される。

また、角速度算出部５６８では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが算出され、その角速度ωＡ３ｍは、加減算器６１８に出力される。なお、角速度ωＡ３ｍは、角速度ωｍ３を、第３駆動源４０３のモーター４０３Ｍと第３アーム１４との間、すなわち、関節１７３における減速比で除算した値である。

また、第２ａ慣性センサー３２ａにより、第３アーム１４の回転軸Ｏ２の回りの角速度が検出される。そして、その第２ａ慣性センサー３２ａの検出信号、すなわち、第２ａ慣性センサー３２ａにより検出された第３アーム１４の回転軸Ｏ２の回りの角速度ωＡ３は、加減算器６１８に出力される。なお、回転軸Ｏ２、Ｏ３は、それぞれ、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けずに、容易かつ確実に、第３アーム１４の回転軸Ｏ２の回りの角速度を求めることができる。

加減算器６１８には、角速度ωＡ３、角速度ωＡ２ｍおよび角速度ωＡ３ｍが入力され、加減算器６１８は、角速度ωＡ３から角速度ωＡ２ｍおよび角速度ωＡ３ｍを減算した値ωＡ３ｓ（＝ωＡ３−ωＡ２ｍ−ωＡ３ｍ）を変換部５８８に出力する。この値ωＡ３ｓは、アーム１３とアーム１４の回転軸Ｏ２の回りの合計の角速度の振動成分（振動角速度）。以下、ωＡ３ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ３ｓ（詳細には、振動角速度ωＡ３ｓに基づいて生成した値であるモーター４０３Ｍにおける角速度ωｍ３ｓ）が後述するゲインＫａ倍されて第３駆動源４０３の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ３ｓが可及的に０になるように、第３駆動源４０３に対してフィードバック制御がなされる。これにより、ロボット１の振動を抑制することができる。なお、このフィードバック制御では、第３駆動源４０３の角速度が制御される。

変換部５８８は、振動角速度ωＡ３ｓを第３駆動源４０３における角速度ωｍ３ｓに変換し、その角速度ωｍ３ｓを補正値算出部５９８に出力する。この変換は、振動角速度ωＡ３ｓに、第３駆動源４０３のモーター４０３Ｍと第３アーム１４との間、すなわち、関節１７３における減速比を乗算することで得ることができる。

補正値算出部５９８は、角速度ωｍ３ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第３補正成分）Ｋａ・ωｍ３ｓを求め、その補正値Ｋａ・ωｍ３ｓを加算器６０８に出力する。なお、この第３駆動源制御部２０３におけるゲインＫａと、第１駆動源制御部２０１におけるゲインＫａとは、同一でもよく、また、異なっていてもよい。

加算器６０８には、角速度ωｍ３が入力され、また、補正値Ｋａ・ωｍ３ｓが入力される。加算器６０８は、角速度ωｍ３と補正値Ｋａ・ωｍ３ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３８に出力する。なお、以降の動作は、第１実施形態と同様である。
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。」」」

＜第３実施形態＞
図１２は、本発明のロボットの第２実施形態を示す概略図である。図１３および図１４は、それぞれ、図１２に示すロボットの主要部のブロック図である。
なお、以下では、説明の都合上、図１２中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１２中の基台側を「基端」、その反対側を「先端」と言う。また、図１２では、回転軸Ｏ２、Ｏ３は、それぞれ、誇張して図示されている。また、図１２では、慣性センサー３１、３２ｂは、それぞれ、その存在を明確にするため、アーム１２、１３の外部に図示されている。

以下、第３実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１２に示すように、第３実施形態のロボット１では、第３アーム１４に代えて、第２アーム１３に、第２ｂ慣性センサー３２ｂが設置されている。この第２ｂ慣性センサー３２ｂにより第２アーム１３の回転軸Ｏ２の回りの角速度を検出する。第２アーム１３における第２ｂ慣性センサー３２ｂの設置位置は、特に限定されないが、第２アーム１３の先端部が好ましい。本実施形態では、第２ｂ慣性センサー３２ｂは、第２アーム１３の内部の先端部に設置されている。第２アーム１３の振動は、その先端部において最大になるので、これにより、より確実にロボット１の振動を抑制することができる。なお、第２ｂ慣性センサー３２ｂが、第２アーム１３の基端部に設置されていてもよいことは、言うまでもない。

また、第２ｂ慣性センサー３２ｂとしては、特に限定されず、本実施形態では、例えば、ジャイロセンサー等を用いることができる。
図１３、図１４に示すように、このロボット１では、制御装置２０の第２駆動源制御部２０２、第３駆動源制御部２０３がそれぞれ第１実施形態と異なっている。すなわち、このロボット１では、第１慣性センサー３１、第２ｂ慣性センサー３２ｂの検出結果に基づいて第１駆動源４０１、第２駆動源４０２の作動を制御する。以下、第２駆動源制御部２０２、第３駆動源制御部２０３について説明する。

図１３に示すように、第２駆動源制御部２０２は、減算器５１９と、位置制御部５２９と、減算器５３９と、角速度制御部５４９と、回転角度算出部５５９と、角速度算出部５６９と、減算器５７９と、変換部５８９と、補正値算出部５９９と、加算器６０９とを有している。
図１４に示すように、第３駆動源制御部２０３は、減算器５１１０と、位置制御部５２１０と、減算器５３１０と、角速度制御部５４１０と、回転角度算出部５５１０と、角速度算出部５６１０とを有している。

図１３に示すように、第２駆動源制御部２０２には、第２駆動源４０２の位置指令Ｐｃの他、第２角度センサー４１２、第２ｂ慣性センサー３２ｂからそれぞれ検出信号が入力される。第２駆動源制御部２０２は、第２角度センサー４１２の検出信号から算出される第２駆動源の回転角度（位置フィードバック値Ｐｆｂ）が位置指令Ｐｃになり、かつ、後述する角速度フィードバック値ωｆｂが後述する角速度指令ωｃになるように、各検出信号を用いたフィードバック制御によって第１駆動源４０２を駆動する。

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

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

減算器５３９には、角速度指令ωｃが入力され、また、後述する角速度フィードバック値ωｆｂが入力される。減算器５３９は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第２駆動源４０２の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４９に出力する。

角速度制御部５４９は、減算器５３９から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第２駆動源４０２の駆動信号（駆動電流）を生成し、モータードライバー３０２を介してモーター４０２Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。

このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第２駆動源４０２の駆動電流が制御される。なお、回転軸Ｏ２は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けず、第１駆動源４０１に対して独立して第２駆動源の作動を制御することができる。

次に、第２駆動源制御部２０２における角速度フィードバック値ωｆｂについて説明する。
角速度算出部５６９では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度ωｍ１が算出され、その角速度ωｍ２は、加算器６０９に出力される。

また、角速度算出部５６９では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２アーム１３の回転軸Ｏ２の回りの角速度ωＡ２ｍが算出され、その角速度ωＡ２ｍは、減算器５７９に出力される。なお、角速度ωＡ２ｍは、角速度ωｍ２を、第２駆動源４０２のモーター４０２Ｍと第２アーム１３との間、すなわち、関節１７２における減速比で除算した値である。

また、第２ｂ慣性センサー３２ｂにより、第２アーム１３の回転軸Ｏ２の回りの角速度が検出される。そして、その第２ｂ慣性センサー３２ｂの検出信号、すなわち、第２ｂ慣性センサー３２ｂにより検出された第２アーム１３の回転軸Ｏ２の回りの角速度ωＡ２は、減算器５７９に出力される。なお、回転軸Ｏ２は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けずに、容易かつ確実に、第２アーム１３の回転軸Ｏ２の回りの角速度を求めることができる。

減算器５７９には、角速度ωＡ２および角速度ωＡ２ｍが入力され、減算器５７９は、この角速度ωＡ２から角速度ωＡ２ｍを減算した値ωＡ２ｓ（＝ωＡ２−ωＡ２ｍ）を変換部５８９に出力する。この値ωＡ２ｓは、第２アーム１３の回転軸Ｏ２の回りの角速度の振動成分（振動角速度）に相当する。以下、ωＡ２ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ２ｓ（詳細には、振動角速度ωＡ２ｓに基づいて生成した値であるモーター４０２Ｍにおける角速度ωｍ２ｓ）が後述するゲインＫａ倍されて駆動源４０２の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ２ｓが可及的に０になるように、駆動源４０２に対してフィードバック制御がなされる。これにより、ロボット１の振動を抑制することができる。なお、このフィードバック制御では、駆動源４０２の角速度が制御される。

変換部５８１は、振動角速度ωＡ１ｓを第１駆動源４０１における角速度ωｍ１ｓに変換し、その角速度ωｍ１ｓを補正値算出部５９１に出力する。この変換は、振動角速度ωＡ１ｓに、第１駆動源４０１のモーター４０１Ｍと第１アーム１２との間、すなわち、関節１７１における減速比を乗算することで得ることができる。
補正値算出部５９９は、角速度ωｍ２ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第２補正成分）Ｋａ・ωｍ２ｓを求め、その補正値Ｋａ・ωｍ２ｓを加算器６０９に出力する。
加算器６０９には、角速度ωｍ２が入力され、また、補正値Ｋａ・ωｍ２ｓが入力される。加算器６０９は、角速度ωｍ２と補正値Ｋａ・ωｍ２ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３９に出力する。なお、以降の動作は、第１実施形態と同様である。

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

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

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

また、角速度算出部５６１０では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３駆動源４０３の角速度が算出され、その角速度が角速度フィードバック値ωｆｂとして減算器５３１０に出力される。
減算器５３１０には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。減算器５３１０は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第３駆動源４０３の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４１０に出力する。

角速度制御部５４１０は、減算器５３１０から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第３駆動源４０３の駆動信号（駆動電流）を生成し、モータードライバー３０３を介してモーター４０３Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第３駆動源４０３の駆動電流が制御される。
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。

＜第４実施形態＞
図１５は、本発明のロボットの第４実施形態の主要部を示すブロック図である。
以下、第４実施形態について、前述した第３実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１５に示すように、第４実施形態のロボット１では、制御装置２０の第２駆動源制御部２０２、第３駆動源制御部２０３がそれぞれ第３実施形態と異なっている。すなわち、このロボット１では、第１慣性センサー３１、第２ｂ慣性センサー３２ｂの検出結果に基づいて第１駆動源４０１、第３駆動源４０３の作動を制御する。以下、第２駆動源制御部２０２、第３駆動源制御部２０３について説明する。

図１５に示すように、第２駆動源制御部２０２は、減算器５１１１と、位置制御部５２１１と、減算器５３１１と、角速度制御部５４１１と、回転角度算出部５５１１と、角速度算出部５６１１とを有している。
また、第３駆動源制御部２０２は、減算器５１１２と、位置制御部５２１２と、減算器５３１２と、角速度制御部５４１２と、回転角度算出部５５１２と、角速度算出部５６１２と、減算器５７１２と、変換部５８１２と、補正値算出部５９１２と、加算器６０１２とを有している。

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

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

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

また、角速度算出部５６１１では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度が算出され、その角速度が角速度フィードバック値ωｆｂとして減算器５３１１に出力される。
減算器５３１１には、角速度指令ωｃが入力され、また、角速度フィードバック値ωｆｂが入力される。減算器５３１１は、これら角速度指令ωｃと角速度フィードバック値ωｆｂとの偏差（第２駆動源４０２の角速度の目標値から角速度フィードバック値ωｆｂを減算した値）を角速度制御部５４１１に出力する。

角速度制御部５４１１は、減算器５３１１から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第２駆動源４０２の駆動信号（駆動電流）を生成し、モータードライバー３０２を介してモーター４０２Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。
このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第２駆動源４０２の駆動電流が制御される。

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

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

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

角速度制御部５４１２は、減算器５３１２から入力された偏差と、予め定められた係数である比例ゲイン、積分ゲイン等を用い、積分を含む所定の演算処理を行うことで、その偏差に応じた第３駆動源４０３の駆動信号（駆動電流）を生成し、モータードライバー３０３を介して第３駆動源４０３のモーター４０３Ｍに供給する。なお、ここでは、本実施形態では、フィードバック制御として、ＰＩ制御がなされるが、これに限定されるものではない。

このようにして、位置フィードバック値Ｐｆｂが位置指令Ｐｃと可及的に等しくなり、かつ、角速度フィードバック値ωｆｂが角速度指令ωｃと可及的に等しくなるように、フィードバック制御がなされ、第３駆動源４０３の駆動電流が制御される。なお、回転軸Ｏ３は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けず、第１駆動源４０１に対して独立して第３駆動源４０３の作動を制御することができる。

次に、第３駆動源制御部２０３における角速度フィードバック値ωｆｂについて説明する。
角速度算出部５６１２では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３駆動源４０３の角速度ωｍ３が算出され、その角速度ωｍ３は、加算器６０１２に出力される。

また、角速度算出部５６１２では、第３角度センサー４１３から入力されるパルス信号の周波数に基づいて、第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが算出され、その角速度ωＡ３ｍは、加減算器６１１２に出力される。なお、角速度ωＡ３ｍは、角速度ωｍ３を、第３駆動源４０３のモーター４０３Ｍと第３アーム１４との間、すなわち、関節１７３における減速比で除算した値である。

また、第２ｂ慣性センサー３２ｂにより、第２アーム１３の回転軸Ｏ２の回りの角速度が検出される。そして、その第２ｂ慣性センサー３２ｂの検出信号、すなわち、第２ｂ慣性センサー３２ｂにより検出された第２アーム１３の回転軸Ｏ２の回りの角速度ωＡ２は、加減算器６１１２に出力される。なお、回転軸Ｏ２は、回転軸Ｏ１に対して直交しているので、第１アーム１２の動作や振動の影響を受けずに、容易かつ確実に、第２アーム１３の回転軸Ｏ２の回りの角速度を求めることができる。

減算器５７１２には、角速度ωＡ２および角速度ωＡ２ｍが入力され、減算器５７１２は、角速度ωＡ２から角速度ωＡ２ｍを減算した値ωＡ３ｓ（＝ωＡ２−ωＡ２ｍ）を変換部５８８に出力する。この値ωＡ３ｓは、アーム１３の回転軸Ｏ２の回りの角速度の振動成分（振動角速度）。以下、ωＡ３ｓを振動角速度と言う。本実施形態では、この振動角速度ωＡ３ｓ（詳細には、振動角速度ωＡ３ｓに基づいて生成した値であるモーター４０３Ｍにおける角速度ωｍ３ｓ）が後述するゲインＫａ倍されて第３駆動源４０３の入力側に戻るフィードバック制御を行う。具体的には、振動角速度ωＡ３ｓが可及的に０になるように、第３駆動源４０３に対してフィードバック制御がなされる。これにより、ロボット１の振動を抑制することができる。なお、このフィードバック制御では、第３駆動源４０３の角速度が制御される。
変換部５８１２は、振動角速度ωＡ３ｓを第３駆動源４０３における角速度ωｍ３ｓに変換し、その角速度ωｍ３ｓを補正値算出部５９１２に出力する。この変換は、振動角速度ωＡ３ｓに、第３駆動源４０３のモーター４０３Ｍと第３アーム１４との間、すなわち、関節１７３における減速比を乗算することで得ることができる。

補正値算出部５９１２は、角速度ωｍ３ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第３補正成分）Ｋａ・ωｍ３ｓを求め、その補正値Ｋａ・ωｍ３ｓを加算器６０１２に出力する。なお、この第３駆動源制御部２０３におけるゲインＫａと、第１駆動源制御部２０１におけるゲインＫａとは、同一でもよく、また、異なっていてもよい。

加算器６０１２には、角速度ωｍ３が入力され、また、補正値Ｋａ・ωｍ３ｓが入力される。加算器６０１２は、角速度ωｍ３と補正値Ｋａ・ωｍ３ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３８に出力する。なお、以降の動作は、第３実施形態と同様である。
このロボット１によれば、前述した第３実施形態と同様の効果が得られる。

以上、本発明のロボットを、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物が付加されていてもよい。
また、本発明は、前記各実施形態のうちの、任意の２以上の構成（特徴）を組み合わせたものであってもよい。
なお、各モーターとしては、それぞれ、前記サーボモーターの他、例えば、ステッピングモーター等が挙げられる。

また、前記実施形態では、角度センサーとして、それぞれ、エンコーダーを用いているが、本発明では、これに限定されず、例えば、レゾルバー、ポテンショメーター等、モーターのローターの回転角度を検出する他の各種のセンサーを用いてもよく、また、タコジェネレーター等、モーターのローターの回転速度を検出する各種のセンサーを用いてもよい。なお、モーターとしてステッピングモーターを用いる場合は、例えば、ステッピングモーターへ入力する駆動パルスの数を計測することで、モーターのローターの回転角度や回転速度を検出してもよい。

また、前記実施形態では、各慣性センサーとして、それぞれ、ジャイロセンサーを用いているが、本発明では、これに限定されず、例えば、アームの角速度を検出する他の各種の角速度センサーを用いてもよく、また、アームの加速度を検出する各種の加速度センサーを用いてもよい。なお、加速度センサーを用いる場合は、加速度センサーの検出値を用いて角速度を算出する。
また、各角度センサー、各慣性センサーの方式は、それぞれ、特に限定されず、例えば、光学式、磁気式、電磁式、電気式等が挙げられる。
また、前記実施形態では、ロボットの回転軸の数は、６つであるが、本発明では、これに限定されず、ロボットの回転軸の数は、３つ、４つ、５つまたは７つ以上でもよい。

すなわち、前記実施形態では、リストが２本のアームを有しているので、ロボットのアームの本数は、６本であるが、本発明では、これに限定されず、ロボットのアームの本数は、３本、４本、５本または７本以上でもよい。
また、前記実施形態では、ロボットは、複数のアームを回動自在に連結してなるアーム連結体を１つ有する単腕ロボットであるが、本発明では、これに限定されず、例えば、図１６に示すように、複数のアームを回動自在に連結してなるアーム連結体１８を２つ有する双腕ロボット１Ａ等、前記アーム連結体を複数有するロボットであってもよい。

１Ａ……ロボット（産業用ロボット） ２、２ａ〜２ｄ……アーム本体 ３、３ａ〜３ｄ……駆動機構 ４、４ａ〜４ｄ……封止手段 １０……ロボット本体 １０１……床 １１……基台 １１１……ボルト １１２……基台本体 １１３……円筒状部 １１４……箱状部 １２、１３、１４、１５……アーム（リンク） １６……リスト（リンク） １６１……リスト本体 １６２……支持リング １６３……先端面 １７１、１７２、１７３、１７４、１７５、１７６……関節（ジョイント） １８……アーム連結部 ３１、３２ａ、３２ｂ……慣性センサー ４、４ａ、４ｂ、４ｃ、４ｄ……封止手段 ２０……制御装置 ２０１〜２０６……駆動源制御部 ３０１、３０２、３０３、３０４、３０５、３０６……モータードライバー ４０１、４０２、４０３、４０４、４０５、４０６……駆動源 ４０１Ｍ〜４０６Ｍ……モーター ４１１、４１２、４１３、４１４、４１５、４１６……角度センサー ５１１、５１２、５１３、５１４、５１５、５１６、５１７、５１８、５１９、５１１０、５１１１、５１１２……減算器 ５２１、５２２、５２３、５２４、５２５、５２６、５２７、５２８、５２９、５２１０、５２１１、５２１２……位置制御部 ５３１、５３２、５３３、５３４、５３５、５３６、５３７、５３８、５３９、５３１０、５３１１、５３１２……減算器 ５４１、５４２、５４３、５４４、５４５、５４６、５４７、５４８、５４９、５４１０、５４１１、５４１２……角速度制御部 ５５１、５５２、５５３、５５４、５５５、５５６、５５７、５５８、５５９、５５１０、５５１１、５５１２……回転角度算出部 ５６１、５６２、５６３、５６４、５６５、５６６、５６７、５６８、５６９、５６１０、５６１１、５６１２……角速度算出部 ５７１、５７９、５７１２……減算器 ５８１、５８２、５８８、５８９、５８１２……変換部 ５９１、５９２、５９８、５９９、５９１２……補正値算出部 ６０１、６０２、６０８、６０９、６０１２……加算器 ６１８、６２２……加減算器 ６１１２……加減算器 Ｏ１、Ｏ２、Ｏ３、Ｏ４、Ｏ５、Ｏ６……回転軸

Claims (16)

1. 基台と、
   前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
   前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
   前記第１アームを第１角速度指令で回動させる第１駆動源と、
   前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
   前記第１駆動源の回転角度を検出する第１角度センサーと、
   前記第２アームを第２角速度指令で回動させる第２駆動源と、
   前記第２駆動源の回転角度を検出する第２角度センサーと、
   前記第３アームを回動させる第３駆動源と、
   前記第３アームに設置され、前記第３アームの前記第２回転軸の角速度または加速度を検出する第２ａ慣性センサーと、
   前記第３駆動源の回転角度を検出する第３角度センサーと、
   前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
   前記第２ａ慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第２補正成分をフィードバックして前記第２駆動源を制御する第２駆動源制御手段と、を備えることを特徴とするロボット。
2. 前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
   前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックする前記第２駆動源制御手段と、を備える請求項１に記載のロボット。
3. 基台と、
   前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
   前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
   前記第１アームを第１角速度指令で回動させる第１駆動源と、
   前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
   前記第１駆動源の回転角度を検出する第１角度センサーと、
   前記第２アームを回動させる第２駆動源と、
   前記第２駆動源の回転角度を検出する第２角度センサーと、
   前記第３アームを第３角速度指令で回動させる第３駆動源と、
   前記第３アームに設置され、前記第３アームの前記第２回転軸の角速度または加速度を検出する第２ａ慣性センサーと、
   前記第３駆動源の回転角度を検出する第３角度センサーと、
   前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
   前記第２ａ慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段と、を備えることを特徴とするロボット。
4. 前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
   前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第３補正成分により前記第３角速度指令をフィードバックする前記第３駆動源制御手段と、を備える請求項３に記載のロボット。
5. 基台と、
   前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
   前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
   前記第１アームを第１角速度指令で回動させる第１駆動源と、
   前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
   前記第１駆動源の回転角度を検出する第１角度センサーと、
   前記第２アームを第２角速度指令で回動させる第２駆動源と、
   前記第２アームに設置され、前記第２アームの前記第２回転軸の角速度または加速度を検出する第２ｂ慣性センサーと、
   前記第２駆動源の回転角度を検出する第２角度センサーと、
   前記第３アームを回動させる第３駆動源と、
   前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーの検出結果から得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
   前記第２ｂ慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、から導かれる第２補正成分をフィードバックして前記第２駆動源を制御する第２駆動源制御手段と、を備えることを特徴とするロボット。
6. 前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
   前記角速度ωＡ２から前記角速度ωＡ２ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックする前記第２駆動源制御手段と、を備える請求項５に記載のロボット。
7. 基台と、
   前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
   前記第１アームに連結され、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
   前記第１アームを第１角速度指令で回動させる第１駆動源と、
   前記第１アームに設置され、前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
   前記第１駆動源の回転角度を検出する第１角度センサーと、
   前記第２アームを回動させる第２駆動源と、
   前記第２駆動源の回転角度を検出する第２角度センサーと、
   前記第２アームに設置され、前記第２アームの前記第２回転軸の角速度または加速度を検出する第２ｂ慣性センサーと、
   前記第３アームを第３角速度指令で回動させる第３駆動源と、
   前記第３駆動源の回転角度を検出する第３角度センサーと、
   前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１補正成分をフィードバックして前記第１駆動源を制御する第１駆動源制御手段と、
   前記第２ｂ慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段と、を備えることを特徴とするロボット。
8. 前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値に対してフィードバックゲインを乗算した前記第１補正成分により前記第１角速度指令をフィードバックする前記第１駆動源制御手段と、
   前記角速度ωＡ２から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第３補正成分により前記第３角速度指令をフィードバックする前記第３駆動源制御手段と、を備える請求項７に記載のロボット。
9. 前記第１慣性センサーを設置した前記第１アームと、
   前記第２ａ慣性センサーを設置した前記第３アームと、
   慣性センサーを未設置とした前記第２アームと、を備える請求項１ないし４のいずれかに記載のロボット。
10. 前記第１慣性センサーを設置した前記第１アームと、
    前記第２ｂ慣性センサーを設置した前記第２アームと、
    慣性センサーを未設置とした前記第３アームと、を備える請求項５ないし８のいずれかに記載のロボット。
11. 前記第１慣性センサーは、前記第１アームの先端部に設置され、
    前記第２ａ慣性センサーは、前記第３アームの先端部に設置されている請求項１ないし４のいずれかに記載のロボット。
12. 前記第１慣性センサーは、前記第１アームの先端部に設置され、
    前記第２ｂ慣性センサーは、前記第２アームの先端部に設置されている請求項５ないし８のいずれかに記載のロボット。
13. 前記第１回転軸は、前記基台の設置面の法線と一致する請求項１ないし１２のいずれかに記載のロボット。
14. 基台と、
    前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
    前記第１回転軸と異なる方向の第２回転軸を軸中心として回動する第２アームと、
    前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
    前記第１アームの角速度を検出する第１慣性センサーと、
    前記第１アームの駆動源の回転角度を検出する第１角度センサーと、
    前記第２アームの駆動源の回転角度を検出する第２角度センサーと、
    前記第３アームの角速度を検出する第２ａ慣性センサーと、
    前記第３アームの駆動源の回転角度を検出する第３角度センサーと、
    前記第１角度センサーの検出結果から導かれる角速度と前記第１慣性センサーから検出される角速度とをフィードバックして制御する前記第１アームの駆動源の制御手段と、
    前記第２角度センサーの検出結果から導かれる角速度と前記第３角度センサーの検出結果から導かれる角速度と前記第２ａ慣性センサーから検出される角速度とをフィードバックして制御する前記第２アームの駆動源の制御手段と、を備えることを特徴とするロボット。
15. 基台と、
    前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、
    前記第１回転軸と異なる方向の第２回転軸を軸中心として回動する第２アームと、
    前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、
    前記第１アームの角速度を検出する第１慣性センサーと、
    前記第１アームの駆動源の回転角度を検出する第１角度センサーと、
    前記第２アームの駆動源の回転角度を検出する第２角度センサーと、
    前記第３アームの角速度を検出する第２ａ慣性センサーと、
    前記第３アームの駆動源の回転角度を検出する第３角度センサーと、
    前記第１角度センサーの検出結果から導かれる角速度と前記第１慣性センサーから検出される角速度とをフィードバックして制御する前記第１アームの駆動源の制御手段と、
    前記第２角度センサーの検出結果から導かれる角速度と前記第３角度センサーの検出結果から導かれる角速度と前記第２ａ慣性センサーから検出される角速度とをフィードバックして制御する前記第３アームの駆動源の制御手段と、を備えることを特徴とするロボット。
16. 複数の異なる回転軸を有し、前記異なる回転軸毎に対応して一つの慣性センサーと一つの角度センサーとが設けられ、前記角度センサーおよび前記慣性センサーから得られる角速度を当該角度センサーおよび当該慣性センサーに対応する回転軸毎にフィードバック制御することを特徴とするロボット。

Images