JP6111563B2 - ロボット - Google Patents

Description

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

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

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

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

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

また、計算精度が低下すると正確な加速度や速度をフィードバックできないので、振動抑制能力が低下したり、制御性能が損なわれることがあり、このため、高速な演算器が必要になる等、制御システムの設計の制約を受ける。
また、前記座標軸変換の計算において、特異点と呼ばれる座標軸変換解がない領域（計算できない領域）があり、その領域では振動抑制能力が低下したり、かえって振動を拡大してしまうことがある。

また、例えば、アームリンク部が伸びている場合と、アームリンク部が折り畳まれている場合等、アームリンク部の姿勢が異なる場合でも同様の制御を行うので、制御を安定させつつ、的確に振動を抑制することが困難である。
本発明の目的は、アームの姿勢に応じて、容易かつ確実に、振動を抑制することができるロボットを提供することにある。

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

これにより、アームの姿勢に応じてフィードバックゲインを調整することにより、さらに効果的かつ確実に、ロボットの振動を抑制することができる。
すなわち、膨大な演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置して各アームに対してその振動を抑える制御を行う場合に比べ、慣性センサーの数を削減することができ、コストを低減することができ、また、回路構成を簡素化することができる。

また、第２駆動源制御手段は、角速度ωＡ３、ωＡ２ｍおよびωＡ３ｍをフィードバックして第２駆動源を制御する、すなわち、第２アームを回動させる第２駆動源に対して、第２アームよりも大きな振動が生じる先端側の第３アームに設置されている第２ａ慣性センサーの検出結果を用いて振動を抑える制御を行うので、振動を抑制する効果を高めることができる。また、第３アームよりも基端側の第２アームを回動させる第２駆動源を制御することにより、振動を抑制する効果を高めることができる。
そして、特に、第２駆動源制御手段は、第２アームを基準とする第３アームの姿勢に応じて前記フィードバック制御におけるフィードバックゲインを調整するので、第３アームの姿勢に応じて、確実且つ効果的に振動を抑制することができる。

（適用例２）
本発明のロボットでは、前記角速度ωＡ１から前記角速度ωＡ１ｍを減算した値または該値から導かれる値にフィードバックゲインを乗算した前記第１補正成分をフィードバックして前記第１駆動源を制御する前記第１駆動源制御手段を備えていることが好ましい。
第２駆動源制御手段とともに、第１駆動源制御手段においてもフィードバックゲインを調整するので、さらに効果的に、ロボットの振動を抑制することができる。

（適用例３）
本発明のロボットでは、前記第３角度センサーを含み、前記第２アームの軸線と前記第３アームの軸線とのなす角θを検出する前記姿勢検出手段を備えていることが好ましい。
前記第３角度センサーによって前記第２アームの軸線と前記第３アームの軸線とのなす角θを検出するので、部品点数を削減することができ、構成を簡素化することができ、また、より確実に、第３アームの姿勢を検出することができる。

（適用例４）
本発明のロボットでは、前記第２アームの軸線と前記第３アームの軸線とのなす角θを検出し、前記角θが１８０°に近づくにつれて前記フィードバックゲインを大きくする前記姿勢検出手段を備えていることが好ましい。
これにより、第３アームの姿勢に応じて、より確実に振動を抑制することができる。

（適用例５）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量に基づいて、前記フィードバックゲインを調整する前記姿勢検出手段を備えていることが好ましい。
これにより、前記質量に応じて、確実に振動を抑制することができる。
（適用例６）
本発明のロボットでは、前記質量が大きくなるにつれて、前記フィードバックゲインを小さくする前記姿勢検出手段を備えていることが好ましい。
これにより、前記質量に応じて、より確実に振動を抑制することができる。

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

これにより、アームの姿勢に応じて、容易かつ確実に、ロボットの振動を抑制することができる。
すなわち、膨大な演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。

また、各アームに慣性センサーを設置し、各アームに対してその振動を抑える制御を行うので、より確実に、ロボットの振動を抑制することができる。
そして、特に、第１駆動源制御手段は、第１アームに対する第２アームの姿勢に応じて前記フィードバック制御におけるフィードバックゲインを調整するので、第２アームの姿勢に応じて、確実に振動を抑制することができる。

（適用例８）
本発明のロボットでは、前記角速度ωＡ２から前記角速度ωＡ２ｍを減算した値または該値から導かれた値にフィードバックゲインを乗算した前記第２補正成分をフィードバックして前記第２駆動源を制御する前記第２駆動源制御手段を備えていることが好ましい。
第１駆動源制御手段とともに、第２駆動源制御手段においてもフィードバックゲインを調整するので、さらに効果的に、ロボットの振動を抑制することができる。

（適用例９）
本発明のロボットでは、前記第２角度センサーを含み、前記第１アームの軸線と前記第２アームの軸線とのなす角θを検出する前記姿勢検出手段を備えていることが好ましい。
前記第２角度センサーによって前記第１アームの軸線と前記第２アームの軸線とのなす角θを検出するので、部品点数を削減することができ、構成を簡素化することができ、また、より確実に、第２アームの姿勢を検出することができる。

（適用例１０）
本発明のロボットでは、前記第１アームの軸線と前記第２アームの軸線とのなす角θを検出し、前記角θが０°以上１８０°未満の範囲で、前記角θが９０°に近づくにつれて、前記フィードバックゲインを大きくする、または前記角θが１８０°以上、３６０°未満の範囲で、前記角θが２７０°に近づくにつれてフィードバックゲインを大きくする、前記姿勢検出手段を備えていることが好ましい。
これにより、第２アームの姿勢に応じて、より確実に振動を抑制することができる。

（適用例１１）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量に基づいて、前記フィードバックゲインを調整する前記姿勢検出手段を備えていることが好ましい。
これにより、前記質量に応じて、確実に振動を抑制することができる。
（適用例１２）
本発明のロボットでは、前記質量が大きくなるにつれて、前記フィードバックゲインを大きくする前記姿勢検出手段を備えていることが好ましい。
これにより、前記質量に応じて、より確実に振動を抑制することができる。

（適用例１３）
本発明のロボットでは、前記第２回転軸に平行な第３回転軸を軸中心として回動自在となるように、前記第２アームと連結された第３アームと、前記第３アームを回動させる第３駆動源と、前記第３アームに設置され、前記第３アームの前記第２回転軸の角速度または加速度を検出する第３慣性センサーと、前記第３駆動源の回転角度を検出する第３角度センサーと、前記第３慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２ｂ慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段と、を備えていることが好ましい。
これにより、より確実に、ロボットの振動を抑制することができる。

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

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

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

（適用例１７）
本発明のロボットでは、前記基台の設置面の法線と一致する前記第１回転軸を軸中心として回動する第１アームを備えていることが好ましい。
これにより、ロボットの制御を容易に行うことができる。

（適用例１８）
本発明のロボットでは、前記第１回転軸と直交または前記第１回転軸に直交する軸と平行である前記第２回転軸を軸中心として回動する前記第２アームを備えていることが好ましい。
これにより、ロボットの制御を容易に行うことができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図１、図２に示すように、基台１１は、ロボット１が垂直多関節ロボットの場合、当該垂直多関節ロボットの最も下方に位置し、設置スペースの床１０１に固定される部分である。この固定方法としては、特に限定されず、例えば、図１、図２に示す本実施形態では、複数本のボルト１１１による固定方法を用いている。なお、基台１１の設置スペースでの固定箇所としては、床の他に、設置スペースの壁や天井とすることもできる。
基台１１は、中空の基台本体（ハウジング）１１２を有している。基台本体１１２は、円筒状をなす円筒状部１１３と、当該円筒状部１１３の外周部に一体的に形成された、箱状をなす箱状部１１４とに分けることができる。そして、このような基台本体１１２には、例えば、モーター４０１Ｍやモータードライバー３０１〜３０６が収納されている。

アーム１２〜１５は、それぞれ、中空のアーム本体２と、駆動機構３と、封止手段４とを有している。なお、以下では、説明の都合上、第１アーム１２が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ａ」、「駆動機構３ａ」、「封止手段４ａ」と言い、第２アーム１３が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｂ」、「駆動機構３ｂ」、「封止手段４ｂ」と言い、第３アーム１４が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｃ」、「駆動機構３ｃ」、「封止手段４ｃ」と言い、第４アーム１５が有するアーム本体２、駆動機構３、封止手段４をそれぞれ「アーム本体２ｄ」、「駆動機構３ｄ」、「封止手段４ｄ」と言うことがある。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ここで、このロボット１は、第２アーム１３を基準として第３アーム１４の姿勢を検出する姿勢検出手段として、第２アーム１３の軸線（中心軸）１３１と第３アーム１４の軸線（中心軸）１４１とのなす角θ（以下、単に「第３アーム１４の角度θ」、「アーム１４の角度θ」、「角度θ」とも言う）（図４参照）を検出する角度検出手段を有している。本実施形態では、この角度検出手段は、第３角度センサー４１３と、制御装置２０とで構成されている。すなわち、制御装置２０の第３駆動源制御部２０３の回転角度算出部５５３は、第３角度センサー４１３から入力されるパルス数をカウントし、そのカウント値に応じた第３駆動源４０３の回転角度を求め、これにより、現在の第３アーム１４の角度θを求める。

そして、第２駆動源制御部２０２（姿勢検出手段）は、前述したフィードバック制御において、前記角度検出手段の検出結果、すなわち、検出された第３アーム１４の角度θに基づいて、フィードバックゲインＫａを調整する（導く）。
この場合、第２駆動源制御部２０２は、第３アーム１４の角度θが１８０°に近いほど、フィードバックゲインＫａを大きく設定する。すなわち、第３アーム１４の角度θが０°以上、１８０°未満の範囲では、第３アーム１４の角度θが０°に近いほど、フィードバックゲインＫａを小さく設定し、同様に、第３アーム１４の角度θが１８０°以上、３６０°未満の範囲では、第３アーム１４の角度θが３６０°（０°）に近いほど、フィードバックゲインＫａを小さく設定する。なお、一般に、ロボットのアームの稼働範囲は、４５°〜３１５°程度であるが、本発明では、アームの稼働範囲が０°〜３６０°のうちのいずれかの範囲のロボットをすべてその対象としている。したがって、本明細書では、ロボットのアームの稼働範囲が０°〜３６０°であるものとして説明を行う。

フィードバックゲインＫａを前記のように設定する理由は、まず、第３アーム１４の角度θが１８０°の場合は、第３アーム１４が第２アーム１３に対して伸びた状態（以下、単に「伸長状態」、「伸長姿勢」とも言う）である。この第３アーム１４の角度θが１８０°の場合は、ロボット１の制御が最も安定しているが、第３アーム１４の回転軸Ｏ２の回りの慣性モーメントが最も大きく、振動が最も大きいので、フィードバックゲインＫａを大きく設定することにより、振動を抑制する効果を高めることができる。

また、第３アーム１４の角度θが０°（３６０°）（実際は、４５°または３１５°）の場合は、第３アーム１４が第２アーム１３に対して最も大きく曲がった状態である。この第３アーム１４の角度θが０°（３６０°）（実際は、４５°または３１５°）の場合は、第３アーム１４の第２回転軸Ｏ２の回りの慣性モーメントが最も小さく、最も制御が不安定であり、振動が発振し易いので、フィードバックゲインＫａを小さく設定することにより、ロボット１の発振を防止し、制御を安定させることができる。この場合、慣性モーメントが小さく、元々ロボット１の振動が小さいため、フィードバックゲインＫａを下げても振動が大きくなることはない。

なお、第３アーム１４の角度θが０°以上、１８０°未満の範囲では、第３アーム１４の角度θが１８０°に近いほど、ロボット１の制御が安定し、第３アーム１４の回転軸Ｏ２の回りの慣性モーメントが大きく、振動が大きい。すなわち、第３アーム１４の角度θが０°に近いほど、ロボット１の制御が不安定で、第３アーム１４の回転軸Ｏ２の回りの慣性モーメントが小さく、振動が小さい。

また、第３アーム１４の角度θが１８０°以上、３６０°未満の範囲では、第３アーム１４の角度θが１８０°に近いほど、ロボット１の制御が安定し、第３アーム１４の回転軸Ｏ２の回りの慣性モーメントが大きく、振動が大きい。すなわち、第３アーム１４の角度θが３６０°に近いほど、ロボット１の制御が不安定で、第３アーム１４の回転軸Ｏ２の回りの慣性モーメントが小さく、振動が小さい。
このように、このロボット１では、第３アーム１４の姿勢に応じてフィードバックゲインＫａを調整することにより、制御を安定させつつ、確実に振動を抑制することができる。すなわち、振動の抑制と、制御の安定性との両立を図ることができる。

なお、駆動源制御部２０１、２０２におけるフィードバックゲインＫａは、それぞれ、１未満に設定されることが好ましい。
その理由は、代表的に第１駆動源制御部２０１において説明すると、フィードバックゲインＫａを１に設定すると、加算器６０１において、第１角度センサー４１１により検出して求めた第１駆動源４０１の角速度ωｍ１と、補正値Ｋａ・ωｍ１ｓのうちの第１角度センサー４１１により検出して求めた第１駆動源４０１の角速度の成分とが相殺されてしまう。これにより、角速度フィードバック値ωｆｂのうち、第１角度センサーにより検出して求めた第１駆動源４０１の角速度の成分が０となり、制御が不安定になる。

また、フィードバックゲインＫａを１よりも大きく設定すると、角速度フィードバック値ωｆｂのうち、第１角度センサーにより検出して求めた第１駆動源４０１の角速度の成分が負となり、その角速度の成分が正帰還し、制御が不安定となる。
なお、例えば、角速度算出部５６１と加算器６０１との間に、第１角度センサー４１１により検出して求めた第１駆動源４０１の角速度ωｍ１に、予め定められた係数であるゲイン（フィードバックゲイン）Ｋｂを乗算し、補正値Ｋｂ・ωｍ１を求め、その補正値Ｋｂ・ωｍ１を加算器６０１に出力する図示しない補正値算出部を設けてもよい。この場合は、前記の理由から、フィードバックゲインＫａは、フィードバックゲインＫｂ未満に設定されることが好ましい。この場合は、フィードバックゲインＫａは、フィードバックゲインＫｂの値によっては、１以上に設定することができる。なお、以下では、フィードバックゲインＫａの値については、代表的に、角速度算出部５６１と加算器６０１との間に前記補正値算出部を設けない場合について説明する。

次に、第２駆動源制御部２０２におけるフィードバックゲインＫａの調整について説明する。
なお、ここでは代表的に、第３アーム１４の角度θが、θ１未満の場合と、θ１以上、θ２未満の場合と、θ２以上、θ３未満の場合と、θ３以上、θ４未満の場合と、θ４以上の場合に（θ１＜θ２＜θ３＜θ４）、それぞれ、フィードバックゲインＫａを設定する場合について説明する。

図１１に示すように、第２駆動源制御部２０２は、まず、第３駆動源制御部２０３を介して第３角度センサー４１３により、第３駆動源４０３の回転角度を検出する（ステップＳ１０１）。すなわち、第３駆動源制御部２０３は、第３角度センサー４１３から入力されるパルス数をカウントし、そのカウント値に応じた第３駆動源４０３の回転角度を求め、第２駆動源制御部２０２は、その第３駆動源４０３の回転角度の情報を取得する。

次いで、第３駆動源４０３の回転角度に基づいて、第３アーム１４の角度θを求める（ステップＳ１０２）。
次いで、第３アーム１４の角度θがθ１未満であるか否かを判断し（ステップＳ１０３）、第３アーム１４の角度θがθ１未満である場合は、フィードバックゲインＫａをＫａ１に設定する（ステップＳ１０４）。なお、第３アーム１４の稼働範囲は、本実施形態では、例えば、４５°〜３１５°に設定されるので、ステップＳ１０３では、第３アーム１４の角度θが４５°以上、θ１未満であるか否かが判断される。

また、ステップＳ１０３において第３アーム１４の角度θがθ１未満でない場合は、第３アーム１４の角度θがθ１以上、θ２未満であるか否かを判断し（ステップＳ１０５）、第３アーム１４の角度θがθ１以上、θ２未満である場合は、フィードバックゲインＫａをＫａ２に設定する（ステップＳ１０６）。
また、ステップＳ１０５において第３アーム１４の角度θがθ１以上、θ２未満でない場合は、第３アーム１４の角度θがθ２以上、θ３未満であるか否かを判断し（ステップＳ１０７）、第３アーム１４の角度θがθ２以上、θ３未満である場合は、フィードバックゲインＫａをＫａ３に設定する（ステップＳ１０８）。

また、ステップＳ１０７において第３アーム１４の角度θがθ２以上、θ３未満でない場合は、第３アーム１４の角度θがθ３以上、θ４未満であるか否かを判断し（ステップＳ１０９）、第３アーム１４の角度θがθ３以上、θ４未満である場合は、フィードバックゲインＫａをＫａ４に設定する（ステップＳ１１０）。
また、ステップＳ１０９において第３アーム１４の角度θがθ３以上、θ４未満でない場合は、第３アーム１４の角度θがθ４以上であるか否かを判断し（ステップＳ１１１）、第３アーム１４の角度θがθ４以上である場合は、フィードバックゲインＫａをＫａ５に設定する（ステップＳ１１２）。なお、前記のように第３アーム１４の稼働範囲は、本実施形態では、例えば、４５°〜３１５°に設定されるので、ステップＳ１１１では、第３アーム１４の角度θがθ４以上、３１５°以下であるか否かが判断される。

上記の各ステップは、制御周期毎に実行され、ステップＳ１０４、Ｓ１０６、Ｓ１０８、Ｓ１１０、Ｓ１１２の後、再度、ステップＳ１０１に戻り、ステップＳ１０１以降を実行する。
なお、Ｋａ１＜Ｋａ２＜Ｋａ３＞Ｋａ４＞Ｋａ５である。また、Ｋａ１とＫａ５とは、同一でもよく、また、異なっていてもよい。また、Ｋａ２とＫａ４とは、同一でもよく、また、異なっていてもよい。

ここで、θ１は、諸条件により適宜設定されるものであり、特に限定されないが、７０°以上、１１０°以下であることが好ましく、９０°以上、１００°以下であることがより好ましい。１例としては、例えば、９０°に設定される。
また、θ２は、諸条件により適宜設定されるものであり、特に限定されないが、１１５°以上、１５５°以下であることが好ましく、１２５°以上、１４５°以下であることがより好ましい。１例としては、例えば、１３５°に設定される。

また、θ３は、諸条件により適宜設定されるものであり、特に限定されないが、２０５°以上、２４５°以下であることが好ましく、２１５°以上、２３５°以下であることがより好ましい。１例としては、例えば、２２５°に設定される。
また、θ４は、諸条件により適宜設定されるものであり、特に限定されないが、２５０°以上、２９０°以下であることが好ましく、２６０°以上、２８０°以下であることがより好ましい。１例としては、例えば、２７０°に設定される。

これにより、より確実に、制御を安定させつつ、振動を抑制することができる。
そして、角度θがθ１未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ１に設定されるが、そのＫａ１は、０以上、０．３以下であることが好ましく、０以上、０．２以下であることがより好ましい。
また、角度θがθ１以上、θ２未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ２に設定されるが、そのＫａ２は、０．１以上、０．５以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。

また、角度θがθ２以上、θ３未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ３に設定されるが、そのＫａ３は、０．３以上、０．７以下であることが好ましく、０．３以上、０．６以下であることがより好ましい。
また、角度θがθ３以上、θ４未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ４に設定されるが、そのＫａ４は、０．１以上、０．５以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。
また、角度θがθ４以上の場合は、前述したように、フィードバックゲインＫａは、Ｋａ５に設定されるが、そのＫａ５は、０以上、０．３以下であることが好ましく、０以上、０．２以下であることがより好ましい。
これにより、より確実に、制御を安定させつつ、振動を抑制することができる。

以上説明したように、このロボット１によれば、容易かつ確実に、ロボット１の振動を抑制することができる。
まず、ロボット１の制御において、膨大な演算が不要であり、これにより、ロボット１の制御における応答速度を速くすることができ、また、制御装置２０の構成を簡素化することができる。

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

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

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

また、第３アーム１４の姿勢に応じてフィードバックゲインＫａを調整することにより、制御を安定させつつ、確実に振動を抑制することができる。
なお、本実施形態では、第３アーム１４の角度θを５つの範囲に分けたが、これに限らず、例えば、２つ、３つ、４つ、または、６つ以上の範囲に分けてもよい。また、段階的ではなく、連続する角度θに対して、連続するフィードバックゲインＫａを対応させてもよい。

また、以下に、Ｋａ１〜Ｋａ５の他の具体例を示す。
この具体例は、振動角速度ωＡ２ｓが逆位相となる場合を考慮したものであり、振動角速度ωＡ２ｓが逆位相のときは、フィードバックゲインＫａを負にすることで、正常な位相に変換することができる。
まず、Ｋａ１は、−０．２以上、０．３以下であることが好ましく、−０．１以上、０．１以下であることがより好ましい。

また、Ｋａ２は、０．１以上、０．４以下であることが好ましく、０．１以上、０．３以下であることがより好ましい。
また、Ｋａ３は、０．３以上、０．７以下であることが好ましく、０．３以上、０．６以下であることがより好ましい。
また、Ｋａ４は、０．１以上、０．４以下であることが好ましく、０．１以上、０．３以下であることがより好ましい。
また、Ｋａ５は、−０．２以上、０．３以下であることが好ましく、−０．１以上、０．１以下であることがより好ましい。

＜第２実施形態＞
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
第２実施形態のロボット１では、第２駆動源制御部２０２は、フィードバック制御において、ロボット１の先端部、すなわち、リスト１６の先端部における先端負荷質量に基づいて、フィードバックゲインＫａを調整する。

この先端負荷質量とは、最も先端側のアームであるリスト１６の先端部に装着されるマニピュレーターと、そのマニピュレーターが把持する対象物との合計の質量である。すなわち、マニピュレーターが対象物を把持しているときは、マニピュレーターと、そのマニピュレーターが把持している対象物との合計の質量であり、マニピュレーターが対象物を把持していないときは、マニピュレーターの質量である。

本実施形態では、マニピュレーターの質量およびマニピュレーターが把持する対象物の質量は、いずれも既知であり、その質量は、それぞれ、予め、制御装置２０の図示しない記憶部に記憶される。また、制御装置２０は、マニピュレーターが対象物を把持しているか否かを把持している。これにより、制御装置２０は、ロボット１の先端負荷質量を常に把握している。なお、マニピュレーターが把持する対象物として、質量の異なるものが複数種ある場合でもよい。

また、第２駆動源制御部２０２は、先端負荷質量が大きくなるにつれ、フィードバックゲインＫａを小さく設定する。
その理由は、先端負荷質量が大きいほど、ロボット１の振動が小さいが、制御が不安定であり、振動が発振し易いので、フィードバックゲインＫａを小さく設定することにより、ロボット１の発振を防止し、制御を安定させることができる。
換言すれば、先端負荷質量が小さいほど、ロボット１の制御が安定しているので、フィードバックゲインＫａを大きく設定することにより、振動を抑制する効果を高めることができる。

次に、フィードバックゲインＫａの調整について説明する。
なお、ここでは代表的に、第３アーム１４の角度θが、θ１未満の場合と、θ１以上、θ２未満の場合と、θ２以上、θ３未満の場合と、θ３以上、θ４未満の場合と、θ４以上の場合（θ１＜θ２＜θ３＜θ４）のそれぞれと、先端負荷質量が、１ｋｇの場合と、３ｋｇの場合と、５ｋｇの場合のそれぞれとの組み合わせにおいて、それぞれ、フィードバックゲインＫａを設定する場合について説明する。

まず、角度θがθ１未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０以上、０．４以下であることが好ましく、０．１以上、０．３以下であることがより好ましい。
また、角度θがθ１未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０以上、０．３以下であることが好ましく、０以上、０．２以下であることがより好ましい。
また、角度θがθ１未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、−０．２以上、０．２以下であることが好ましく、−０．１以上、０以下であることがより好ましい。

また、角度θがθ１以上、θ２未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．５以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。
また、角度θがθ１以上、θ２未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．５以下であることが好ましく、０．２以上、０．３以下であることがより好ましい。
また、角度θがθ１以上、θ２未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０以上、０．４以下であることが好ましく、０以上、０．３以下であることがより好ましい。

また、角度θがθ２以上、θ３未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．３以上、０．７以下であることが好ましく、０．４以上、０．６以下であることがより好ましい。
また、角度θがθ２以上、θ３未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．３以上、０．７以下であることが好ましく、０．３以上、０．６以下であることがより好ましい。
また、角度θがθ２以上、θ３未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．３以上、０．７以下であることが好ましく、０．３以上、０．６以下であることがより好ましい。

また、角度θがθ３以上、θ４未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．５以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。
また、角度θがθ３以上、θ４未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．５以下であることが好ましく、０．２以上、０．３以下であることがより好ましい。
また、角度θがθ３以上、θ４未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０以上、０．４以下であることが好ましく、０以上、０．３以下であることがより好ましい。

また、角度θがθ４以上で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０以上、０．４以下であることが好ましく、０．１以上、０．３以下であることがより好ましい。
また、角度θがθ４以上で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０以上、０．３以下であることが好ましく、０以上、０．２以下であることがより好ましい。
また、角度θがθ４以上で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、−０．２以上、０．２以下であることが好ましく、−０．１以上、０以下であることがより好ましい。

これにより、より確実に、制御を安定させつつ、振動を抑制することができる。
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１では、第３アーム１４の姿勢および先端負荷質量に応じてフィードバックゲインＫａを調整することにより、より確実に、制御を安定させつつ、振動を抑制することができる。

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

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

また、第２ｂ慣性センサー３２ｂとしては、特に限定されず、本実施形態では、例えば、ジャイロセンサー等を用いることができる。
ここで、このロボット１では、第１アーム１２および第２アーム１３の振動を抑制するために、前記のように第１アーム１２および第２アーム１３の両方に第１慣性センサー３１、第２ｂ慣性センサー３２ｂを設置し、その第１慣性センサー３１、第２ｂ慣性センサー３２ｂの検出結果に基づいて駆動源４０１、４０２の作動を制御する。これにより、確実に、第１アーム１２および第２アーム１３の振動を抑制することができ、これによって、ロボット１全体の振動を抑制することができる。
また、このロボット１では、制御装置２０の第２駆動源制御部２０２が第１実施形態と異なっている。

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

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

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

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

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

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

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

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

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

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

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

図１４に示す第３駆動源制御部２０３については、角速度ωＡ３ｍを算出して出力することを行わない以外は、前記第１実施形態と同様であるので、その説明は省略する。
ここで、このロボット１は、第１アーム１２を基準として第２アーム１３の姿勢を検出する姿勢検出手段として、第１アーム１２の軸線（中心軸）１２１（回転軸Ｏ１）と第２アーム１３の軸線（中心軸）１３１とのなす角θ（以下、単に「第２アーム１３の角度θ」、「アーム１３の角度θ」、「角度θ」とも言う）（図１２参照）を検出する角度検出手段を有している。本実施形態では、この角度検出手段は、第２角度センサー４１２と、制御装置２０とで構成されている。すなわち、制御装置２０の第２駆動源制御部２０２の回転角度算出部５５７は、第２角度センサー４１２から入力されるパルス数をカウントし、そのカウント値に応じた第２駆動源４０２の回転角度を求め、これにより、現在の第２アーム１３の角度θを求める。

そして、第１駆動源制御部２０１（姿勢検出手段）は、前述したフィードバック制御において、前記角度検出手段の検出結果、すなわち、検出された第２アーム１３の角度θに基づいて、フィードバックゲインＫａを調整する（導く）。
この場合、第１駆動源制御部２０１は、第２アーム１３の角度θが０°以上、１８０°未満の範囲では、第２アーム１３の角度θが９０°に近いほど、フィードバックゲインＫａを大きく設定する。すなわち、第２アーム１３の角度θが０°以上、９０°未満の範囲では、第２アーム１３の角度θが０°に近いほど、フィードバックゲインＫａを小さく設定し、同様に、第２アーム１３の角度θが９０°以上、１８０°未満の範囲では、第２アーム１３の角度θが１８０°に近いほど、フィードバックゲインＫａを小さく設定する。

また、第２アーム１３の角度θが１８０°以上、３６０°未満の範囲では、第２アーム１３の角度θが２７０°に近いほど、フィードバックゲインＫａを大きく設定する。すなわち、第２アーム１３の角度θが１８０°以上、２７０°未満の範囲では、第２アーム１３の角度θが１８０°に近いほど、フィードバックゲインＫａを小さく設定し、同様に、第２アーム１３の角度θが２７０°以上、３６０°未満の範囲では、第２アーム１３の角度θが３６０°（０°）に近いほど、フィードバックゲインＫａを小さく設定する。

その理由は、まず、第２アーム１３の角度θが９０°、２７０°の場合は、ロボット１の制御が最も安定しているが、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが最も大きく、振動が最も大きいので、フィードバックゲインＫａを大きく設定することにより、振動を抑制する効果を高めることができる。
また、第２アーム１３の角度θが０°、１８０°の場合は、第２アーム１３の第１回転軸Ｏ１の回りの慣性モーメントが最も小さく、最も制御が不安定であり、振動が発振し易いので、フィードバックゲインＫａを小さく設定することにより、ロボット１の発振を防止し、制御を安定させることができる。この場合、慣性モーメントが小さく、元々ロボット１の振動が小さいため、フィードバックゲインＫａを下げても振動が大きくなることはない。

なお、第２アーム１３の角度θが０°以上、９０°未満の範囲では、第２アーム１３の角度θが９０°に近いほど、ロボット１の制御が安定し、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが大きく、振動が大きい。すなわち、第２アーム１３の角度θが０°に近いほど、ロボット１の制御が不安定で、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが小さく、振動が小さい。

また、第２アーム１３の角度θが９０°以上、１８０°未満の範囲では、第２アーム１３の角度θが９０°に近いほど、ロボット１の制御が安定し、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが大きく、振動が大きい。すなわち、第２アーム１３の角度θが１８０°に近いほど、ロボット１の制御が不安定で、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが小さく、振動が小さい。

また、第２アーム１３の角度θが１８０°以上、２７０°未満の範囲では、第２アーム１３の角度θが２７０°に近いほど、ロボット１の制御が安定し、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが大きく、振動が大きい。すなわち、第２アーム１３の角度θが１８０°に近いほど、ロボット１の制御が不安定で、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが小さく、振動が小さい。

また、第２アーム１３の角度θが２７０°以上、３６０°未満の範囲では、第２アーム１３の角度θが２７０°に近いほど、ロボット１の制御が安定し、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが大きく、振動が大きい。すなわち、第２アーム１３の角度θが３６０°に近いほど、ロボット１の制御が不安定で、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが小さく、振動が小さい。
このように、このロボット１では、第２アーム１３の姿勢に応じてフィードバックゲインＫａを調整することにより、制御を安定させつつ、確実に振動を抑制することができる。すなわち、振動の抑制と、制御の安定性との両立を図ることができる。

次に、第１駆動源制御部２０１におけるフィードバックゲインＫａの調整について説明する。
なお、ここでは代表的に、第２アーム１３の角度θが、０°以上、１８０°未満の範囲においては、第２アーム１３の角度θが、θ１１未満の場合と、θ１１以上、θ１２未満の場合と、θ１２以上の場合に（θ１１＜θ１２）、それぞれ、フィードバックゲインＫａを設定し、同様に、第３アーム１４の角度θが、１８０°以上、３６０°未満の範囲においては、第２アーム１３の角度θが、θ２１未満の場合と、θ２１以上、θ２２未満の場合と、θ２２以上の場合に（θ２１＜θ２２）、それぞれ、フィードバックゲインＫａを設定する場合について説明する。

図１５に示すように、第２駆動源制御部２０２は、まず、第２角度センサー４１２により、第２駆動源４０２の回転角度を検出する（ステップＳ２０１）。すなわち、第２角度センサー４１２から入力されるパルス数をカウントし、そのカウント値に応じた第２駆動源４０２の回転角度を求める。
次いで、第２駆動源４０２の回転角度に基づいて、第２アーム１３の角度θを求める（ステップＳ２０２）。

次いで、第２アーム１３の角度θが１８０°未満であるか否かを判断し（ステップＳ２０３）、第２アーム１３の角度θが１８０°未満である場合は、第２アーム１３の角度θがθ１１未満であるか否かを判断し（ステップＳ２０４）、第２アーム１３の角度θがθ１１未満である場合は、フィードバックゲインＫａをＫａ１１に設定する（ステップＳ２０５）。なお、第２アーム１３の稼働範囲は、本実施形態では、例えば、５０°〜３１０°に設定されるので、ステップＳ２０４では、第２アーム１３の角度θが５０°以上、θ１１未満であるか否かが判断される。

また、ステップＳ２０４において第２アーム１３の角度θがθ１１未満でない場合は、第２アーム１３の角度θがθ１１以上、θ１２未満であるか否かを判断し（ステップＳ２０６）、第２アーム１３の角度θがθ１１以上、θ１２未満である場合は、フィードバックゲインＫａをＫａ１２に設定する（ステップＳ２０７）。
また、ステップＳ２０６において第２アーム１３の角度θがθ１１以上、θ１２未満でない場合は、第２アーム１３の角度θがθ１２以上であるか否かを判断し（ステップＳ２０８）、第２アーム１３の角度θがθ１２以上である場合は、フィードバックゲインＫａをＫａ１３に設定する（ステップＳ２０９）。

また、ステップＳ２０４において第２アーム１３の角度θが１８０°未満ではない場合は、第２アーム１３の角度θがθ２１未満であるか否かを判断し（ステップＳ２１０）、第２アーム１３の角度θがθ２１未満である場合は、フィードバックゲインＫａをＫａ２１に設定する（ステップＳ２１１）。
また、ステップＳ２１０において第２アーム１３の角度θがθ２１未満でない場合は、第２アーム１３の角度θがθ２１以上、θ２２未満であるか否かを判断し（ステップＳ２１２）、第２アーム１３の角度θがθ２１以上、θ２２未満である場合は、フィードバックゲインＫａをＫａ２２に設定する（ステップＳ２１３）。

また、ステップＳ２１２において第２アーム１３の角度θがθ２１以上、θ２２未満でない場合は、第２アーム１３の角度θがθ２２以上であるか否かを判断し（ステップＳ２１４）、第２アーム１３の角度θがθ２２以上である場合は、フィードバックゲインＫａをＫａ２３に設定する（ステップＳ２１５）。なお、前記のように第２アーム１３の稼働範囲は、本実施形態では、例えば、５０°〜３１０°に設定されるので、ステップＳ２１４では、第２アーム１３の角度θがθ２２以上、３１０°以下であるか否かが判断される。

上記の各ステップは、制御周期毎に実行され、ステップＳ２０５、Ｓ２０７、Ｓ２０９、Ｓ２１１、Ｓ２１３、Ｓ２１５の後、再度、ステップＳ２０１に戻り、ステップＳ２０１以降を実行する。
なお、Ｋａ１１＞Ｋａ１２＞Ｋａ１３であり、Ｋａ２１＜Ｋａ２２＜Ｋａ２３である。また、Ｋａ１１とＫａ２３とは、同一でもよく、また、異なっていてもよい。また、Ｋａ１２とＫａ２２とは、同一でもよく、また、異なっていてもよい。また、Ｋａ１３とＫａ２１とは、同一でもよく、また、異なっていてもよい。

ここで、θ１１は、諸条件により適宜設定されるものであり、特に限定されないが、１１５°以上、１５５°以下であることが好ましく、１２５°以上、１４５°以下であることがより好ましい。１例としては、例えば、１３０°に設定される。
また、θ１２は、諸条件により適宜設定されるものであり、特に限定されないが、１４５°以上、１７５°以下であることが好ましく、１５５°以上、１６５°以下であることがより好ましい。１例としては、例えば、１６０°に設定される。

また、θ２１は、諸条件により適宜設定されるものであり、特に限定されないが、１８５°以上、２１５°以下であることが好ましく、１９５°以上、２０５°以下であることがより好ましい。１例としては、例えば、２００°に設定される。
また、θ２２は、諸条件により適宜設定されるものであり、特に限定されないが、２０５°以上、２４５°以下であることが好ましく、２１５°以上、２３５°以下であることがより好ましい。１例としては、例えば、２３０°に設定される。
これにより、より確実に、制御を安定させつつ、振動を抑制することができる。
そして、角度θがθ１１未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ１１に設定されるが、そのＫａ１１は、０．２以上、０．７以下であることが好ましく、０．４以上、０．５以下であることがより好ましい。

また、角度θがθ１１以上、θ１２未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ１２に設定されるが、そのＫａ１２は、０．１以上、０．４以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。
また、角度θがθ１２以上の場合は、前述したように、フィードバックゲインＫａは、Ｋａ１３に設定されるが、そのＫａ１３は、０以上、０．３以下であることが好ましく、０．１以上、０．２以下であることがより好ましい。

また、角度θがθ２１未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ２１に設定されるが、そのＫａ２１は、０以上、０．３以下であることが好ましく、０．１以上、０．２以下であることがより好ましい。
また、角度θがθ２１以上、θ２２未満の場合は、前述したように、フィードバックゲインＫａは、Ｋａ２２に設定されるが、そのＫａ２２は、０．１以上、０．４以下であることが好ましく、０．２以上、０．４以下であることがより好ましい。
また、角度θがθ２２以上の場合は、前述したように、フィードバックゲインＫａは、Ｋａ２３に設定されるが、そのＫａ２３は、０．２以上、０．７以下であることが好ましく、０．４以上、０．５以下であることがより好ましい。
これにより、より確実に、制御を安定させつつ、振動を抑制することができる。

このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
なお、本実施形態では、第２アーム１３の角度θを６つの範囲に分けたが、これに限らず、例えば、２つ、３つ、４つ、５つ、または、７つ以上の範囲に分けてもよい。また、段階的ではなく、連続する角度θに対して、連続するフィードバックゲインＫａを対応させてもよい。

以下に、第２アーム１３の角度θを９つの範囲に分けた場合の具体例を示す。なお、ここでは、第２アーム１３の稼働範囲が２０°〜２７０°の場合について説明する。
まず、角度θが２０°以上、４０°未満の場合は、フィードバックゲインＫａは、０．１５以上、０．３以下であることが好ましい。
また、角度θが４０°以上、６０°未満の場合は、フィードバックゲインＫａは、０．３以上、０．４以下であることが好ましい。

また、角度θが６０°以上、１２０°未満の場合は、フィードバックゲインＫａは、０．４以上、０．７以下であることが好ましい。
また、角度θが１２０°以上、１４０°未満の場合は、フィードバックゲインＫａは、０．３以上、０．４以下であることが好ましい。
また、角度θが１４０°以上、１６０°未満の場合は、フィードバックゲインＫａは、０．１５以上、０．３以下であることが好ましい。

また、角度θが１６０°以上、２００°未満の場合は、フィードバックゲインＫａは、０．０５以上、０．１５以下であることが好ましい。
また、角度θが２００°以上、２２０°未満の場合は、フィードバックゲインＫａは、０．１５以上、０．３以下であることが好ましい。
また、角度θが２２０°以上、２４０°未満の場合は、フィードバックゲインＫａは、０．３以上、０．４以下であることが好ましい。
また、角度θが２４０°以上、２７０°以下の場合は、フィードバックゲインＫａは、０．４以上、０．７以下であることが好ましい。

＜第４実施形態＞
以下、第４実施形態について、前述した第３実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
第４実施形態のロボット１では、第２駆動源制御部２０２は、フィードバック制御において、ロボット１の先端部、すなわち、リスト１６の先端部における先端負荷質量に基づいて、フィードバックゲインＫａを調整する。

この先端負荷質量とは、リスト１６に装着されるマニピュレーターと、そのマニピュレーターが把持する対象物との合計の質量である。すなわち、マニピュレーターが対象物を把持しているときは、マニピュレーターと、そのマニピュレーターが把持している対象物との合計の質量であり、マニピュレーターが対象物を把持していないときは、マニピュレーターの質量である。

本実施形態では、マニピュレーターの質量およびマニピュレーターが把持する対象物の質量は、いずれも既知であり、その質量は、それぞれ、予め、制御装置２０の図示しない記憶部に記憶される。また、制御装置２０は、マニピュレーターが対象物を把持しているか否かを把握している。これにより、制御装置２０は、ロボット１の先端負荷質量を常に把握している。なお、マニピュレーターが把持する対象物として、質量の異なるものが複数種ある場合でもよい。

また、第２駆動源制御部２０２は、先端負荷質量が大きくなるにつれ、フィードバックゲインＫａを大きく設定する。
その理由は、先端負荷質量が大きいほど、ロボット１の制御が安定しているが、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが大きく、振動が大きいので、フィードバックゲインＫａを大きく設定することにより、振動を抑制する効果を高めることができる。
換言すれば、先端負荷質量が小さいほど、第２アーム１３の回転軸Ｏ１の回りの慣性モーメントが小さく、ロボット１の振動が小さいが、制御が不安定であり、振動が発振し易いので、フィードバックゲインＫａを小さく設定することにより、ロボット１の発振を防止し、制御を安定させることができる。

次に、フィードバックゲインＫａの調整について説明する。
なお、ここでは、第２アーム１３の稼働範囲が５０°〜３１０°の場合について説明する。また、ここでは代表的に、第２アーム１３の角度θが、５０°以上、１８０°未満の範囲においては、第２アーム１３の角度θが、θ１１未満の場合と、θ１１以上、θ１２未満の場合と、θ１２以上の場合（θ１１＜θ１２）のそれぞれと、先端負荷質量が、１ｋｇの場合と、３ｋｇの場合と、５ｋｇの場合のそれぞれとの組み合わせにおいて、それぞれ、フィードバックゲインＫａを設定する場合について説明する。

同様に、第２アーム１３の角度θが、１８０°以上、３１０°以下の範囲においては、第２アーム１３の角度θが、θ２１未満の場合と、θ２１以上、θ２２未満の場合と、θ２２以上の場合（θ２１＜θ２２）のそれぞれと、先端負荷質量が、１ｋｇの場合と、３ｋｇの場合と、５ｋｇの場合のそれぞれとの組み合わせにおいて、それぞれ、フィードバックゲインＫａを設定する場合について説明する。

まず、角度θが５０°以上、θ１１未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．２５以上、０．４以下であることが好ましい。
また、角度θが５０°以上、θ１１未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．３５以上、０．５以下であることが好ましい。
また、角度θが５０°以上、θ１１未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．４以上、０．７以下であることが好ましい。

また、角度θがθ１１以上、θ１２未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．２５以下であることが好ましい。
また、角度θがθ１１以上、θ１２未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．２以上、０．４以下であることが好ましい。
また、角度θがθ１１以上、θ１２未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．３以上、０．６以下であることが好ましい。

また、角度θがθ１２以上で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．０５以上、０．１５以下であることが好ましい。
また、角度θがθ１２以上で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．２以下であることが好ましい。
また、角度θがθ１２以上で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．２５以上、０．４以下であることが好ましい。

また、角度θがθ２１未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．０５以上、０．１５以下であることが好ましい。
また、角度θがθ２１未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．２以下であることが好ましい。
また、角度θがθ２１未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．２５以上、０．４以下であることが好ましい。

また、角度θがθ２１以上、θ２２未満で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．１以上、０．２５以下であることが好ましい。
また、角度θがθ２１以上、θ２２未満で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．２以上、０．４以下であることが好ましい。
また、角度θがθ２１以上、θ２２未満で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．３以上、０．６以下であることが好ましい。

また、角度θがθ２２以上、３１０°以下で、先端負荷質量が１ｋｇの場合は、フィードバックゲインＫａは、０．２５以上、０．４以下であることが好ましい。
また、角度θがθ２２以上、３１０°以下で、先端負荷質量が３ｋｇの場合は、フィードバックゲインＫａは、０．３５以上、０．５以下であることが好ましい。
また、角度θがθ２２以上、３１０°以下で、先端負荷質量が５ｋｇの場合は、フィードバックゲインＫａは、０．４以上、０．７以下であることが好ましい。
これにより、より確実に、制御を安定させつつ、振動を抑制することができる。

このロボット１によれば、前述した第３実施形態と同様の効果が得られる。
そして、このロボット１では、第２アーム１３の姿勢および先端負荷質量に応じてフィードバックゲインＫａを調整することにより、より確実に、制御を安定させつつ、振動を抑制することができる。
なお、この第４実施形態は、後述する第５実施形態にも適用することができる。

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

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

また、第３慣性センサー３３としては、特に限定されず、本実施形態では、例えば、ジャイロセンサー等を用いることができる。
このロボット１では、第１アーム１２、第２アーム１３および第３アーム１４の振動を抑制するために、前記のように第１アーム１２、第２アーム１３および第３アーム１４のすべてに第１慣性センサー３１、第２ｂ慣性センサー３２ｂ、第３慣性センサー３３を設置し、その第１慣性センサー３１、第２ｂ慣性センサー３２ｂ、第３慣性センサー３３の検出結果に基づいて第１駆動源４０１、第２駆動源４０２、第３駆動源４０３の作動を制御する。これにより、確実に、第１アーム１２、第２アーム１３および第３アーム１４の振動を抑制することができ、これによって、ロボット１全体の振動を抑制することができる。
また、このロボット１では、制御装置２０の第３駆動源制御部２０３が第３実施形態と異なっている。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

また、前記実施形態では、各慣性センサーとして、それぞれ、ジャイロセンサーを用いているが、本発明では、これに限定されず、例えば、アームの角速度を検出する他の各種の角速度センサーを用いてもよく、また、アームの加速度を検出する各種の加速度センサーを用いてもよい。なお、加速度センサーを用いる場合は、加速度センサーの検出値を用いて角速度を算出する。

また、各角度センサー、各慣性センサーの方式は、それぞれ、特に限定されず、例えば、光学式、磁気式、電磁式、電気式等が挙げられる。
また、前記実施形態では、ロボットの回転軸の数は、６つであるが、本発明では、これに限定されず、ロボットの回転軸の数は、２つ、３つ、４つ、５つまたは７つ以上でもよい。
すなわち、前記実施形態では、リストが２本のアームを有しているので、ロボットのアームの本数は、６本であるが、本発明では、これに限定されず、ロボットのアームの本数は、２本、３本、４本、５本または７本以上でもよい。

また、前記実施形態では、ロボットは、複数のアームを回動自在に連結してなるアーム連結体を１つ有する単腕ロボットであるが、本発明では、これに限定されず、例えば、図１８に示すように、複数のアームを回動自在に連結してなるアーム連結体１８を２つ有する双腕ロボット１Ａ等、前記アーム連結体を複数有するロボットであってもよい。
また、前記実施形態では、リストに、機能部として、マニピュレーターが着脱自在に装着されるようになっているが、本発明では、これに限定されず、機能部（エンドエフェクタ）としては、その他、例えば、ドリル、溶接機、レーザー照射機等が挙げられる。

また、前記実施形態では、姿勢検出手段として、現在の第１アームの軸線と第２アームの軸線とのなす角θを検出する角度検出手段や、現在の第２アームの軸線と第３アームの軸線とのなす角θを検出する角度検出手段を含むもの用いたが、本発明では、これに限定されない。姿勢検出手段としては、その他、例えば、駆動源の回転角度の目標値（指令値）を取得する手段を含むもの等が挙げられる。この姿勢検出手段は、例えば、次の制御周期における駆動源の回転角度の目標値と、現在の角度θとに基づいて、次の制御周期における角度θを求めるものである。この場合は、次の制御周期における角度θに基づいて、フィードバックゲインＫａを所定値に設定する。
また、例えば、アームの先端部の現在の位置、次の制御周期におけるアームの先端部の位置の指令値、ロボットの最も先端側のアームであるリストの先端部の現在の位置、次の制御周期におけるリストの先端部の位置の指令値等に基づいて、フィードバックゲインＫａを調整するように構成してもよい。

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

Claims (7)

1. 第１回転軸を軸中心として回動可能な第１アームと、前記第１回転軸の軸方向と異なる軸方向である第２回転軸を軸中心として回動可能な第２アームと、前記第２回転軸の軸方向と平行な軸方向である第３回転軸を軸中心として回動可能な第３アームと、前記第３回転軸の軸方向と異なる軸方向である第４回転軸を軸中心として回動可能な第４アームと、前記第４回転軸の軸方向と異なる軸方向である第５回転軸を軸中心として回動可能な第５アームと、前記第５回転軸の軸方向と異なる軸方向である第６回転軸を軸中心として回動可能な第６アームと、を含むアーム連結体と、
   前記第１アーム、前記第２アームおよび前記第３アームのうち少なくとも一つに設けられたジャイロセンサーと、を備え、
   前記アーム連結体が第１の姿勢である場合の前記ジャイロセンサーのゲインは、前記アーム連結体が前記第１の姿勢と異なる第２の姿勢である場合の前記ジャイロセンサーのゲインと異なる、
   ことを特徴とするロボット。
2. 前記アーム連結体の角度を検出する角度センサーを備え、
   前記アーム連結体の姿勢は、前記角度センサーの出力に基づいて求められる、
   ことを特徴とする請求項１に記載のロボット。
3. 前記ゲインは、前記角度センサーの出力と、前記ジャイロセンサーの出力と、に基づいた値に乗算される、
   ことを特徴とする請求項２に記載のロボット。
4. 前記ジャイロセンサーは、前記アーム連結体の内部に位置する、
   ことを特徴とする請求項１ないし３のいずれか一項に記載のロボット。
5. 前記ジャイロセンサーは、前記第１アームに設けられている、
   ことを特徴とする請求項１ないし４のいずれか一項に記載のロボット。
6. 前記アーム連結体に設けられたエンドエフェクタを備え、
   前記エンドエフェクタの質量または前記エンドエフェクタが把持する物の質量の少なくとも一方に基づいて、前記ゲインが変更される、
   ことを特徴とする請求項１ないし５のいずれか一項に記載のロボット。
7. 前記アーム連結体は、前記ジャイロセンサーの出力と前記ゲインに基づいて制御される、
   ことを特徴とする請求項１ないし７のいずれか一項に記載のロボット。

Images