JP5962340B2 - ロボット - Google Patents

Description

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

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

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

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

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

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

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

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

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

そして、特に、第２駆動源制御手段は、第２アームを基準とした第３アームの姿勢に応じて、第２Ａ補正成分をフィードバックして第２駆動源を制御する場合と、第２Ｂ補正成分をフィードバックして第２駆動源を制御する場合との一方を選択するので、振動を抑制する効果をより最適にして、制御の安定性との両立を図ることができる。
すなわち、第２アームを基準として検出される第３アームの姿勢によって第３アームの振動成分に第２駆動源の振動成分も含まれる場合は、第３アームに設置されている第３慣性センサーから得られる角速度を含む第２Ａ補正成分を第２駆動源の制御にフィードバックして、振動を抑制する効果を高めることができる。また、第２アームを基準として検出した第３アームの姿勢によって第３アームの振動成分よりも第２アームの振動成分が多く含まれる場合は、第２アームに設置されている第２慣性センサーから得られる角速度を含む第２Ｂ補正成分をフィードバックして振動を抑える制御を行うので、ロボットの振動を防止し、制御を安定させることができる。

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

（適用例３）
本発明のロボットでは、前記第２アームの軸線と前記第３アームの軸線とのなす角θを検出する前記姿勢検出手段と、前記角θが第１の閾値以上、該第１の閾値よりも大きい第２の閾値以下では、前記第２Ａ補正成分をフィードバックして前記第２駆動源を制御し、前記角θが前記第２の閾値よりも大きいまたは前記第１の閾値未満では、前記第２Ｂ補正成分をフィードバックして前記第２駆動源を制御する前記第２駆動源制御手段と、を備えていることが好ましい。

これにより、第２アームを基準として検出される第３アームの姿勢に応じて、より確実に振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。
すなわち、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値以上、第２の閾値以下の場合（伸長姿勢）は、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値未満または第２の閾値よりも大きい場合（折り畳み姿勢）に比べて、ロボットの制御が安定しているが、ロボットの慣性モーメントが大きく、振動が大きいので、第２Ａ補正成分をフィードバックして第２駆動源を制御することにより、振動を抑制する効果を高めることができる。

一方、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値未満または第２の閾値よりも大きい場合は、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値以上、第２の閾値以下の場合に比べて、ロボットの慣性モーメントが小さく、振動が小さいが、制御が不安定になりがちで、ロボットが振動し易いので、第２Ｂ補正成分をフィードバックして第２駆動源を制御することにより、ロボットの振動を防止し、制御を安定させることができる。

（適用例４）
本発明のロボットでは、前記第１の閾値は、６０°以上、１５０°以下の範囲内とし、前記第２の閾値は、２１０°以上、３００°以下の範囲内とすることが好ましい。
これにより、第２アームを基準として検出される第３アームの姿勢に応じて、より確実に振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。

（適用例５）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量が大きいほど、前記第１の閾値を大きくし、前記第２の閾値を小さくする前記第２駆動源制御手段を備えることが好ましい。
前記質量が大きいほど、ロボットの振動が振動し易いので、これにより、前記質量に応じて、確実に振動を抑制することができる。

（適用例６）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量を加味して、前記第２Ａ補正成分または前記第２Ｂ補正成分のいずれか一方を選択する前記第２駆動源制御手段を備えることが好ましい。
これにより、前記質量に応じて、確実に振動を抑制することができる。

（適用例７）
本発明のロボットでは、前記第３駆動源の回転角度を検出する前記第３角度センサーを含む前記姿勢検出手段を備えることが好ましい。
これにより、部品点数を削減することができ、構成を簡素化することができる。
（適用例８）
本発明のロボットでは、前記第３慣性センサーから得られる前記第３アームの前記第２回転軸の角速度ωＡ３と、前記第２慣性センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２と、前記第３角度センサーから得られる前記第３アームの前記第３回転軸の角速度ωＡ３ｍと、から導かれる第３補正成分をフィードバックして前記第３駆動源を制御する第３駆動源制御手段を備えることが好ましい。
これにより、各アームに対してその振動を抑える制御を行うので、より確実に、ロボットの振動を抑制することができる。

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

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

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

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

そして、特に、第１駆動源制御手段は、第１アームを基準として検出される第２アームの姿勢に応じて、第１Ａ補正成分をフィードバックして第１駆動源を制御する場合と、第１Ｂ補正成分をフィードバックして第１駆動源を制御する場合との一方を選択するので、振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。
すなわち、第１アームを基準として検出される第２アームの姿勢によって第２アームの振動成分に第１駆動源の振動成分も含まれる場合は、第２アームに設置されている第２慣性センサーから得られる角速度を含む第１Ａ補正成分を第１駆動源の制御にフィードバックして、振動を抑制する効果を高めることができる。また、第１アームを基準として検出した第２アームの姿勢によって第２アームの振動成分よりも第１アームの振動成分が多く含まれる場合は、第１アームに設置されている第１慣性センサーから得られる角速度を含む第１Ｂ補正成分をフィードバックして振動を抑える制御を行うので、ロボットの振動を防止し、制御を安定させることができる。

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

（適用例１４）
本発明のロボットでは、前記第１アームの軸線と前記第２アームの軸線とのなす角θを検出する前記姿勢検出手段と、前記角θが第１の閾値以上、該第１の閾値よりも大きい第２の閾値以下では、前記第２慣性センサーから得られる前記第２アームの前記第１回転軸の角速度ωＡ２と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、から導かれる第１Ａ補正成分をフィードバックして前記第１駆動源を制御し、前記角θが前記第１の閾値未満または前記第２の閾値よりも大きい場合は、前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ２と、前記第１角度センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１ｍと、から導かれる第１Ｂ補正成分をフィードバックして前記第１駆動源を制御する、前記第１駆動源制御手段と、を備えることが好ましい。

これにより、第１アームを基準として検出される第２アームの姿勢に応じて、より確実に振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。
すなわち、第１アームの軸線と第２アームの軸線とのなす角θが第１の閾値以上、第２の閾値以下の場合（伸長姿勢）は、第１アームの軸線と第２アームの軸線とのなす角θが第１の閾値未満または第２の閾値よりも大きい場合（折り畳み姿勢）に比べて、ロボットの制御が安定しているが、ロボットの慣性モーメントが大きく、振動が大きいので、第１Ａ補正成分をフィードバックして第１駆動源を制御することにより、振動を抑制する効果を高めることができる。

一方、第１アームの軸線と第２アームの軸線とのなす角θが第１の閾値未満または第２の閾値よりも大きい場合は、第１アームの軸線と第２アームの軸線とのなす角θが第１の閾値以上、第２の閾値以下の場合に比べて、ロボットの慣性モーメントが小さく、振動が小さいが、制御が不安定になりがちで、ロボットが振動し易いので、第１Ｂ補正成分をフィードバックして第１駆動源を制御することにより、ロボットの振動を防止し、制御を安定させることができる。

（適用例１５）
本発明のロボットでは、前記第１の閾値は、６０°以上、１５０°以下の範囲内とし、前記第２の閾値は、２１０°以上、３００°以下の範囲内とすることが好ましい。
これにより、第１アームを基準として検出される第２アームの姿勢に応じて、より確実に振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。

（適用例１６）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量が大きいほど、前記第１の閾値を大きくし、前記第２の閾値を小さくする前記第１駆動源制御手段を備えることが好ましい。
前記質量が大きいほど、ロボットが振動し易いので、これにより、前記質量に応じて、確実に振動を抑制することができる。

（適用例１７）
本発明のロボットでは、エンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量を加味して、前記第１Ａ補正成分または前記第１Ｂ補正成分のいずれか一方を選択する前記第１駆動源制御手段を備えることが好ましい。
これにより、前記質量に応じて、確実に振動を抑制することができる。

（適用例１８）
本発明のロボットでは、前記第２駆動源の回転角度を検出する前記第２角度センサーを含む前記姿勢検出手段を備えることが好ましい。
これにより、部品点数を削減することができ、構成を簡素化することができる。
（適用例１９）
本発明のロボットでは、前記第２慣性センサーから得られる前記第２アームの前記第１回転軸の角速度ωＡ２と、前記第１慣性センサーから得られる前記第１アームの前記第１回転軸の角速度ωＡ１と、前記第２角度センサーから得られる前記第２アームの前記第２回転軸の角速度ωＡ２ｍと、から導かれる第２補正成分をフィードバックして前記第２駆動源を制御する第２駆動源制御手段を備えることが好ましい。
これにより、各アームに対してその振動を抑える制御を行うので、より確実に、ロボットの振動を抑制することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

ここで、このロボット１では、第１アーム１２、第２アーム１３および第３アーム１４の振動を抑制するために、前記のように第１アーム１２、第２アーム１３および第３アーム１４のすべてに第１慣性センサー３１、第２慣性センサー３２、第３慣性センサー３３を設置し、その第１慣性センサー３１、第２慣性センサー３２、第３慣性センサー３３の検出結果に基づいて駆動源４０１、４０２、４０３の作動を制御する。これにより、確実に、第１アーム１２、第２アーム１３および第３アーム１４の振動を抑制することができ、これによって、ロボット１全体の振動を抑制することができる。

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

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

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

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

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

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

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

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

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

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

図６に示すように、第１駆動源制御部２０１は、減算器５１１と、位置制御部５２１と、減算器５３１と、角速度制御部５４１と、回転角度算出部５５１と、角速度算出部５６１と、減算器５７１と、変換部５８１と、補正値算出部５９１と、加算器６０１とを有している。
図７に示すように、第２駆動源制御部２０２は、減算器５１２と、位置制御部５２２と、減算器５３２と、角速度制御部５４２と、回転角度算出部５５２と、角速度算出部５６２と、加減算器６２２と、変換部５８２と、補正値算出部５９２と、加算器６０２と、切替部６３２、６４２とを有している。なお、切替部６３２は、後述する角速度ωＡ２と角速度ωＡ３とのいずれか一方を選択し、加減算器６２２に出力するものである。また、切替部６４２は、後述する角速度ωＡ３ｍを加減算器６２２に出力する場合と出力しない場合とを切り替えるものである。
図７に示すように、第３駆動源制御部２０３は、減算器５１３と、位置制御部５２３と、減算器５３３と、角速度制御部５４３と、回転角度算出部５５３と、角速度算出部５６３と、加減算器６１３と、変換部５８３と、補正値算出部５９３と、加算器６０３とを有している。

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

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

このロボット１では、第２駆動源制御部２０２は、後述する第２駆動源４０２のフィードバック制御において、前記角度検出手段（姿勢検出手段）の検出結果、すなわち、検出された第３アーム１４の角度θに基づいて、第２アーム１３の振動を抑制するための第２駆動源４０２の作動の制御に用いる慣性センサーを選択する。すなわち、第２駆動源制御部２０２は、検出された第３アーム１４の角度θに基づいて、第２慣性センサー３２と第３慣性センサー３３とのいずれか一方を選択し、その選択した慣性センサーの検出結果を用いて、第２駆動源４０２の作動を制御する。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次に、第２駆動源制御部２０２における角速度フィードバック値ωｆｂについて説明する。
まず、第２駆動源制御部２０２は、第３アーム１４の角度θに応じて、加減算器６２２に、後述する角速度ωＡ３、角速度ωＡ２ｍおよび角速度ωＡ３ｍが入力されるように切替部６３２、６４２を切り替える場合と、後述する角速度ωＡ２および角速度ωＡ２ｍが入力されるように切替部６３２、６４２を切り替える場合とを選択する。

すなわち、角度θが第１の閾値θ１以上、第１の閾値θ１よりも大きい第２の閾値θ２以下の場合は、第３アーム１４が第２アーム１３に対して伸びた状態または少し曲がった状態（以下、単に「伸長状態」、「伸長姿勢」とも言う）であり、この場合は、第２駆動源制御部２０２は、使用する慣性センサーとして第３慣性センサー３３を選択する。そして、第２駆動源制御部２０２は、加減算器６２２に角速度ωＡ３、角速度ωＡ２ｍおよび角速度ωＡ３ｍが入力されるように、切替部６３２、６４２を切り替える。なお、第１の閾値θ１は、１８０°未満の角度である。

また、角度θが第１の閾値θ１未満または第２の閾値θ２よりも大きい場合は、第３アーム１４が第２アーム１３に対して大きく曲がった状態（以下、単に「折り畳み状態」、「折り畳み姿勢」とも言う）であり、この場合は、第２駆動源制御部２０２は、使用する慣性センサーとして第２慣性センサー３２を選択する。そして、第２駆動源制御部２０２は、加減算器６２２に角速度ωＡ２および角速度ωＡ２ｍが入力されるように、切替部６３２、６４２を切り替える。なお、第２の閾値θ２は、１８０°よりも大きい角度である。
これにより、振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。

すなわち、第３アーム１４が伸長姿勢の場合は、折り畳み姿勢の場合に比べて、ロボット１の制御が安定しているが、ロボット１の慣性モーメントが大きく、振動が大きいので、第２アーム１３よりも大きな振動が生じる第２アーム１３の先端側の第３アーム１４に設置されている第３慣性センサー３３の検出結果を用いて第２駆動源４０２を制御することにより、振動を抑制する効果を高めることができる。

一方、第３アーム１４が折り畳み姿勢の場合は、伸長姿勢の場合に比べて、ロボット１の慣性モーメントが小さく、振動が小さいが、先端側の第３アーム１４の振動の位相が基端側の第２アーム１３の位相と異なるため、先端側の第３アーム１４に設置されている第３慣性センサー３３の検出結果を用いて基端側の第２アーム１３を駆動する第２駆動源４０２を制御すると制御が不安定になりがちとなり、ロボット１が振動し易い。そこで、基端側の第２アーム１３自身に設置されている第２慣性センサー３２の検出結果を用いて第２駆動源４０２を制御することにより、制御を安定化させることができ、ロボット１の振動を防止すことができる。

また、第１の閾値θ１は、特に限定されず、諸条件に応じて適宜設定されるものであるが、６０°以上、１５０°以下の範囲内で設定されることが好ましく、７０°以上、１４０°以下の範囲内で設定されることがより好ましく、１例としては、例えば、９０°に設定される。また、第２の閾値θ２は、特に限定されず、諸条件に応じて適宜設定されるものであるが、２１０°以上、３００°以下の範囲内で設定されることが好ましく、２２０°以上、２９０°以下の範囲内で設定されることがより好ましく、１例としては、例えば、２７０°に設定される。また、第２の閾値θ２は、「３６０°−θ１」であることが好ましい。これにより、より確実に、振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。

まずは、第３アーム１４の角度θが第１の閾値θ１未満または第２の閾値θ２よりも大きい場合について説明する。
この場合は、角速度算出部５６２では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度ωｍ２が算出され、その角速度ωｍ２は、加算器６０２に出力される。

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

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

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

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

次に、第３アーム１４の角度θが第１の閾値θ１以上、第２の閾値θ２未満の場合について説明する。
この場合は、角速度算出部５６２では、第２角度センサー４１２から入力されるパルス信号の周波数に基づいて、第２駆動源４０２の角速度ωｍ２が算出され、その角速度ωｍ２は、加算器６０２に出力される。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

次いで、第３駆動源４０３の回転角度に基づいて、第３アーム１４の角度θを求める（ステップＳ１０２）。
次いで、第３アーム１４の角度θが第１の閾値θ１未満であるか否かを判断し（ステップＳ１０３）、第３アーム１４の角度θが第１の閾値θ１未満である場合は、使用する慣性センサーとして第２慣性センサー３２を選択する（ステップＳ１０４）。

また、ステップＳ１０３において第３アーム１４の角度θが第１の閾値θ１未満でない場合は、第３アーム１４の角度θが第１の閾値θ１以上、第２の閾値θ２以下であるか否かを判断し（ステップＳ１０５）、第３アーム１４の角度θが第１の閾値θ１以上、第２の閾値θ２以下である場合は、使用する慣性センサーとして第３慣性センサー３３を選択する（ステップＳ１０６）。

また、ステップＳ１０５において第３アーム１４の角度θが第１の閾値θ１以上、第２の閾値θ２以下でない場合は、第３アーム１４の角度θが第２の閾値θ２よりも大きいか否かを判断し（ステップＳ１０７）、第３アーム１４の角度θが第２の閾値θ２よりも大きい場合は、使用する慣性センサーとして第２慣性センサー３２を選択する（ステップＳ１０８）。
上記の各ステップは、制御周期毎に実行され、ステップＳ１０４、Ｓ１０６、Ｓ１０８の後、再度、ステップＳ１０１に戻り、ステップＳ１０１以降を実行する。

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

また、慣性センサー３１、３２、３３は、先端側に比べて共振し難い基端側のアーム１２、１３、１４に設置されているので、慣性センサー３１、３２、３３により、確実に、アーム１２、１３、１４の角速度を検出することができ、確実に振動を抑制することができる。
また、第３アーム１４の姿勢に応じて第２駆動源４０２の制御に使用する慣性センサーを選択することにより、制御を安定させつつ、確実に振動を抑制することができる。

＜第２実施形態＞
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
第２実施形態のロボット１では、第２駆動源制御部２０２は、第３アーム１４の角度θに加えて、ロボット１の先端部、すなわち、リスト１６の先端部における先端負荷質量を加味して、第２アーム１３の振動を抑制するための第２駆動源４０２の作動の制御に用いる慣性センサーを選択する。すなわち、第２駆動源制御部２０２は、先端負荷質量を加味して、第２慣性センサー３２と第３慣性センサー３３とのいずれか一方を選択し、その選択した慣性センサーの検出結果を用いて、第２駆動源４０２の作動を制御する。

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

なお、本実施形態では、マニピュレーターの質量およびマニピュレーターが把持する対象物の質量は、いずれも既知であり、その質量は、それぞれ、予め、制御装置２０の図示しない記憶部に記憶される。また、制御装置２０は、マニピュレーターが対象物を把持しているか否かを把握している。これにより、制御装置２０は、ロボット１の先端負荷質量を常に把握している。なお、マニピュレーターが把持する対象物として、質量の異なるものが複数種ある場合でもよい。
具体的には、本実施形態では、第２駆動源制御部２０２は、第２駆動源４０２のフィードバック制御において、先端負荷質量に基づいて、第１の閾値θ１、第２の閾値θ２（θ２＞θ１）をそれぞれ調整する。この場合、先端負荷質量が大きいほど、第１の閾値θ１を大きく設定し、第２の閾値θ２を小さく設定する。

その理由は、先端負荷質量が大きいほど、制御が不安定であり、振動が発振し易い。このため、先端負荷質量が大きいほど、第１の閾値θ１を大きく設定し、第２の閾値θ２を小さく設定することにより、第３アーム１４がより伸長姿勢に近いときでも使用する慣性センサーとして第２慣性センサー３２が選択されるようになり、これにより、ロボット１の発振を防止し、制御を安定させることができる。
換言すれば、先端負荷質量が小さいほど、ロボット１の制御が安定しているため、第１の閾値θ１を小さく設定し、第２の閾値θ２を大きく設定することにより、第３アーム１４がより折り畳み姿勢に近いときでも使用する慣性センサーとして第３慣性センサー３３が選択されるようになり、これにより、振動を抑制する効果を高めることができる。

次に、具体例を挙げて説明する。
なお、ここでは代表的に、先端負荷質量が、１ｋｇの場合と、３ｋｇの場合と、５ｋｇの場合について説明する。
まず、先端負荷質量が１ｋｇの場合は、第１の閾値θ１は、６０°以上、１２０°以下の範囲内で設定されることが好ましく、１例としては、例えば、９０°に設定される。また、第２の閾値θ２は、２４０°以上、３００°以下の範囲内で設定されることが好ましく、１例としては、例えば、２７０°に設定される。

また、先端負荷質量が３ｋｇの場合は、第１の閾値θ１は、８０°以上、１４０°以下の範囲内で設定されることが好ましく、１例としては、例えば、１２０°に設定される。また、第２の閾値θ２は、２２０°以上、２８０°以下の範囲内で設定されることが好ましく、１例としては、例えば、２４０°に設定される。
また、先端負荷質量が５ｋｇの場合は、第１の閾値θ１は、９０°以上、１５０°以下の範囲内で設定されることが好ましく、１例としては、例えば、１３５°に設定される。また、第２の閾値θ２は、２１０°以上、２７０°以下の範囲内で設定されることが好ましく、１例としては、例えば、２２５°に設定される。
このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１では、第３アーム１４の姿勢および先端負荷質量に応じて使用する慣性センサーを選択することにより、より確実に、制御を安定させつつ、振動を抑制することができる。

＜第３実施形態＞
図１２は、本発明のロボットの第３実施形態を示す概略図である。図１３は、図１２に示すロボットの概略図である。図１４は、図１２に示すロボットの主要部のブロック図である。
なお、以下では、説明の都合上、図１２中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１２、図１３中の基台側を「基端」、その反対側を「先端」と言う。また、図１２、図１３では、慣性センサー３１、３２は、それぞれ、その存在を明確にするため、アーム１２、１３の外部に図示されている。
以下、第３実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。

図１２および図１３に示す第３実施形態のロボット１Ｂは、スカラーロボットと呼ばれているものである。
このロボット１Ｂのロボット本体１０Ｂは、基台１１と、２本のアーム（リンク）１２、１３と、シャフト（作業軸）１９と、３つの駆動源４０１、４０２、４０７とを備えている。基台１１と、第１アーム１２と、第２アーム１３と、シャフト１９とは、基端側から先端側に向ってこの順に連結されている。また、シャフト１９は、その下端部（先端部）に、機能部（エンドエフェクター）が着脱自在に装着される取付部１９１を有している。なお、シャフト１９は、アーム（第３アーム）、すなわち、最も先端側のアームとみなすこともできる。

図１２および図１３に示すように、第１アーム１２、第２アーム１３、シャフト１９は、それぞれ、基台１１に対し独立して変位可能に支持されている。
基台１１と第１アーム１２とは、関節（ジョイント）１７１を介して連結されている。そして、第１アーム１２は、基台１１に対し、鉛直方向と平行な第１回転軸Ｏ１を回転中心とし、その第１回転軸Ｏ１回りに回動自在となっている。第１回転軸Ｏ１は、基台１１の設置面である床１０１の上面の法線と一致している。この第１回転軸Ｏ１回りの回動は、第１駆動源４０１の駆動によりなされる。また、第１駆動源４０１の駆動（作動）は、第１駆動源４０１とケーブル（図示せず）を介して電気的に接続されたモータードライバー３０１を介して制御装置２０により制御される。

第１アーム１２と第２アーム１３とは、関節（ジョイント）１７２を介して連結されている。そして、第２アーム１３は、第１アーム１２（基台１１）に対し、鉛直方向と平行な第２回転軸Ｏ２を回転中心とし、その第２回転軸Ｏ２回りに回動自在となっている。第２回転軸Ｏ２は、第２回転軸Ｏ１と平行である。この第２回転軸Ｏ２回りの回動は、第２駆動源４０２の駆動によりなされる。また、第２駆動源４０２の駆動は、第２駆動源４０２とケーブル（図示せず）を介して電気的に接続されたモータードライバー３０２を介して制御装置２０により制御される。

シャフト１９は、第２アーム１３の先端部（基台１１と反対側の端部）に連結されている。この場合、シャフト１９は、第２アーム１３に対して、そのシャフト１９の軸線（中心軸）の方向に沿って移動可能に設置されている。また、シャフト１９の軸線は、回転軸Ｏ１、Ｏ２と平行であるが、これに限定されるものではない。また、シャフト１９の移動は、第３駆動源４０７の駆動によりなされる。また、第３駆動源４０７の駆動は、第３駆動源４０７とケーブル（図示せず）を介して電気的に接続されたモータードライバー（図示せず）を介して制御装置２０により制御される。なお、シャフト１９の軸線は、回転軸Ｏ１、Ｏ２と非平行であってもよい。

なお、第１駆動源４０１、第２駆動源４０２と同様に、第３駆動源４０７には、第３角度センサー（図示せず）が設けられている。第３駆動源４０７および第３角度センサーは、それぞれ、第２アーム１３内に収納されており、制御装置２０と電気的に接続されている。
また、第２アーム１３内には、第２アーム１３に対してシャフト１９を移動可能に支持する移動支持機構（図示せず）が設けられている。この移動支持機構は、第３駆動源４０７の駆動力をシャフト１９に伝達し、第２アーム１３に対してシャフト１９を移動させるものである。

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

図１４に示すように、第１駆動源制御部２０１は、減算器５１７と、位置制御部５２７と、減算器５３７と、角速度制御部５４７と、回転角度算出部５５７と、角速度算出部５６７と、加減算器６２７と、変換部５８７と、補正値算出部５９７と、加算器６０７と、切替部６３７、６４７とを有している。なお、切替部６３７は、後述する角速度ωＡ１と角速度ωＡ２とのいずれか一方を選択し、加減算器６２７に出力するものである。また、切替部６４７は、後述する角速度ωＡ２ｍを加減算器６２７に出力する場合と出力しない場合とを切り替えるものである。なお、この第１駆動源制御部２０１は、第１実施形態の第２駆動源制御部２０２と同様である。
第２駆動源制御部２０２は、減算器５１８と、位置制御部５２８と、減算器５３８と、角速度制御部５４８と、回転角度算出部５５８と、角速度算出部５６８と、加減算器６１８と、変換部５８８と、補正値算出部５９８と、加算器６０８とを有している。なお、この第２駆動源制御部２０２は、第１実施形態の第３駆動源制御部２０３と同様である。

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

このロボット１Ｂでは、第１駆動源制御部２０１は、後述する第１駆動源４０１のフィードバック制御において、前記角度検出手段（姿勢検出手段）の検出結果、すなわち、検出された第２アーム１３の角度θに基づいて、第１アーム１２の振動を抑制するための第１駆動源４０１の作動の制御に用いる慣性センサーを選択する。すなわち、第１駆動源制御部２０１は、検出された第２アーム１３の角度θに基づいて、第１慣性センサー３１と第２慣性センサー３２とのいずれか一方を選択し、その選択した慣性センサーの検出結果を用いて、第１駆動源４０１の作動を制御する。

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

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

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

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

次に、第１駆動源制御部２０１における角速度フィードバック値ωｆｂについて説明する。
まず、第１駆動源制御部２０１は、第２アーム１３の角度θに応じて、加減算器６２７に、後述する角速度ωＡ２、角速度ωＡ１ｍおよび角速度ωＡ２ｍが入力されるように切替部６３７、６４７を切り替える場合と、後述する角速度ωＡ１および角速度ωＡ１ｍが入力されるように切替部６３７、６４７を切り替える場合とを選択する。

すなわち、角度θが第１の閾値θ１以上、第１の閾値θ１よりも大きい第２の閾値θ２以下の場合は、第２アーム１３が第１アーム１２に対して伸びた状態または少し曲がった状態（以下、単に「伸長状態」、「伸長姿勢」とも言う）であり、この場合は、第１駆動源制御部２０１は、使用する慣性センサーとして第２慣性センサー３２を選択する。そして、第１駆動源制御部２０１は、加減算器６２７に角速度ωＡ２、角速度ωＡ１ｍおよび角速度ωＡ２ｍが入力されるように、切替部６３７、６４７を切り替える。なお、第１の閾値θ１は、第１実施形態と同様である。

また、角度θが第１の閾値θ１未満または第２の閾値θ２よりも大きい場合は、第２アーム１３が第１アーム１２に対して大きく曲がった状態（以下、単に「折り畳み状態」、「折り畳み姿勢」とも言う）であり、この場合は、第１駆動源制御部２０１は、使用する慣性センサーとして第１慣性センサー３１を選択する。そして、第１駆動源制御部２０１は、加減算器６２７に角速度ωＡ１および角速度ωＡ１ｍが入力されるように、切替部６３７、６４７を切り替える。なお、第２の閾値θ２は、第１実施形態と同様である。
これにより、振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。

まずは、第１アーム１２の角度θが第１の閾値θ１未満または第２の閾値θ２よりも大きい場合について説明する。
この場合は、角速度算出部５６７では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１駆動源４０１の角速度ωｍ１が算出され、その角速度ωｍ１は、加算器６０７に出力される。

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

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

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

次に、第２アーム１３の角度θが第１の閾値θ１以上、第２の閾値θ２未満の場合について説明する。
この場合は、角速度算出部５６７では、第１角度センサー４１１から入力されるパルス信号の周波数に基づいて、第１駆動源４０１の角速度ωｍ１が算出され、その角速度ωｍ１は、加算器６０７に出力される。

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

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

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

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

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

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

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

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

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

また、第２慣性センサー３２により、第２アーム１３の回転軸Ｏ２の回りの角速度が検出される。そして、その第２慣性センサー３２の検出信号、すなわち、第２慣性センサー３２により検出された第２アーム１３の回転軸Ｏ１の回りの角速度ωＡ２は、加減算器６１８に出力される。
また、前述した第１慣性センサー３１の検出信号、すなわち、第１慣性センサー３１により検出された第１アーム１２回転軸Ｏ１の回りの角速度ωＡ１は、加減算器６１８に出力される。

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

補正値算出部５９８は、角速度ωｍ２ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第２補正成分）Ｋａ・ωｍ２ｓを求め、その補正値Ｋａ・ωｍ２ｓを加算器６０８に出力する。なお、この第２駆動源制御部２０２におけるゲインＫａと、第１駆動源制御部２０１におけるゲインＫａとは、同一でもよく、また、異なっていてもよい。
加算器６０８には、角速度ωｍ２が入力され、また、補正値Ｋａ・ωｍ２ｓが入力される。加算器６０８は、角速度ωｍ２と補正値Ｋａ・ωｍ２ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３８に出力する。以降の動作は、前述した通りである。
このロボット１Ｂによれば、前述した第１実施形態と同様の効果が得られる。

＜第４実施形態＞
以下、第４実施形態について、前述した第２、第３実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
第４実施形態のロボット１では、第1駆動源制御部２０１は、第２アーム１３の角度θに加えて、ロボット１の先端部、すなわち、シャフト１９の取付部１９１の下端部（先端部）における先端負荷質量を加味して、アーム１２の振動を抑制するための第１駆動源４０１の作動の制御に用いる慣性センサーを選択する。すなわち、第１駆動源制御部２０１は、先端負荷質量を加味して、第１慣性センサー３１と第２慣性センサー３２とのいずれか一方を選択し、その選択した慣性センサーの検出結果を用いて、第１駆動源４０１の作動を制御する。

具体的には、本実施形態では、第１駆動源制御部２０１は、第１駆動源４０１のフィードバック制御において、先端負荷質量に基づいて、第１の閾値θ１、第２の閾値θ２（θ２＞θ１）をそれぞれ調整する。この場合、先端負荷質量が大きいほど、第１の閾値θ１を大きく設定し、第２の閾値θ２を小さく設定する。
なお、先端負荷質量とは、第２実施形態と同様、シャフト１９の取付部１９１に装着される機能部（エンドエフェクタ）と、その機能部が把持する対象物との合計の質量である。
このロボット１によれば、前述した第２、第３実施形態と同様の効果が得られる。

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

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

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

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

すなわち、前記第１実施形態では、リストが２本のアームを有しているので、ロボットのアームの本数は、６本であるが、本発明では、これに限定されず、ロボットのアームの本数は、２本、３本、４本、５本または７本以上でもよい。
同様に、前記第２実施形態では、ロボットのアームの本数は、２本であるが、本発明では、これに限定されず、ロボットのアームの本数は、３本以上でもよい。

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

また、前記実施形態では、姿勢検出手段として、現在の第１アームの軸線と第２アームの軸線とのなす角θを検出する角度検出手段、現在の第２アームの軸線と第３アームの軸線とのなす角θを検出する角度検出手段を用いたが、本発明では、これに限定されない。姿勢検出手段としては、その他、例えば、駆動源の回転角度の目標値（指令値）を取得する手段を含むもの等が挙げられる。この姿勢検出手段は、例えば、次の制御周期における駆動源の回転角度の目標値と、現在の角度θとに基づいて、次の制御周期における角度θを求めるものである。この場合は、次の制御周期における角度θに基づいて、使用する慣性センサーを選択する。

また、例えば、アームの先端部の現在の位置、次の制御周期におけるアームの先端部の位置の指令値、ロボットの最も先端側のアームであるリストの先端部の現在の位置、次の制御周期におけるリストの先端部の位置の指令値、シャフトの下端部（先端部）の現在の位置、次の制御周期におけるシャフトの下端部（先端部）の位置の指令値等に基づいて、使用する慣性センサーを選択する。

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

Claims (8)

1. 第１回転軸を軸中心として回動する第１アームと、前記第１回転軸とは異なる方向の第２回転軸を軸中心として回動する第２アームと、前記第２回転軸とは平行な方向の第３回転軸を軸中心として回動する第３アームと、を含むアーム連結体と、
   前記第２アームを回動させる第２駆動源と、
   前記アーム連結体に設けられた複数の慣性センサーと、を備え、
   前記複数の慣性センサーは、前記第２アームに設置された第２慣性センサーと、前記第３アームに設置された第３慣性センサーと、を含み、
   前記第２駆動源は、前記アーム連結体の姿勢に基づいて、前記第２慣性センサーからの出力または前記第３慣性センサーからの出力を用いて制御されることを特徴とするロボット。
2. 前記第２駆動源は、前記第２アームを基準として第３アームの姿勢に基づいて、前記第２慣性センサーからの出力または前記第３慣性センサーからの出力を用いて制御されることを特徴とする請求項１に記載のロボット。
3. 前記第２駆動源は、前記第２アームの軸線と前記第３アームの軸線とのなす角θが第１の閾値以上、前記第１の閾値よりも大きい第２の閾値以下では、前記第３慣性センサーからの出力を用いて制御され、前記角θが前記第２の閾値よりも大きいまたは前記第１の閾値未満では、前記第２慣性センサーからの出力を用いて制御されることを特徴とする請求項２に記載のロボット。
4. 前記第１アームを回動させる第１駆動源を備え、
   前記複数の慣性センサーは、前記第１アームに設置された第１慣性センサーを含み、
   前記第１駆動源は、記第１慣性センサーからの出力を用いて制御されることを特徴とする請求項１ないし３のいずれか一項に記載のロボット。
5. 前記第１の閾値は、６０°以上、１５０°以下の範囲内とし、前記第２の閾値は、２１０°以上、３００°以下の範囲内とする請求項３に記載のロボット。
6. 前記第２駆動源は、前記アーム連結体に設けられるエンドエフェクタの質量または前記エンドエフェクタと前記エンドエフェクタが把持する物との質量に基づいて、前記第２慣性センサーからの出力または前記第３慣性センサーからの出力を用いて制御されることを特徴とする請求項１ないし５のいずれか一項に記載のロボット。
7. 前記第３アームを回動させる第３駆動源を備え、
   前記第３駆動源は、前記第２慣性センサーからの出力および前記第３慣性センサーからの出力を用いて制御されることを特徴とする請求項１ないし６のいずれか一項に記載のロボット。
8. 複数の慣性センサーは、ジャイロセンサーであることを特徴とする請求項１ないし７のいずれか一項に記載のロボット。

Images