JP6111562B2 - ロボット - Google Patents

Description

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

基台と複数以上の回動可能なアームとを備え、３次元空間で自在に作業をおこなう多軸ロボットであって、アームの回動やアームに加わる外乱等によってアームに振動が生じ易いという問題に対して、この振動の減衰を目的として、最も先端側のアームに設けたＸ軸、Ｙ軸、Ｚ軸の３軸方向の加速度を検知する加速度センサーによる振動抑制制御技術を備えたロボットが知られている（例えば、特許文献１参照）。また、基台と複数以上のアームとを備え、これらのアームの回転軸が互いに平行である多軸ロボットであって、アームごとに角速度センサーを設け、これらの角速度センサーが検出する成分も含めて演算することにより制御されるロボットが知られている（例えば、特許文献２参照）。

特開平１０−１０００８５号公報 特開２００５−２４２７９４号公報

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

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

特許文献２のロボットでは、アームの回転軸が互いに平行であり、角速度センサーが検出する成分の方向は互いに同じであるので、異なる回転成分が混在する演算および制御方法は考慮されていない。従って、この技術をアームの回転軸が互いに異なる多軸ロボットに採用しても振動抑制能力は満足できない。
本発明の目的は、複数のアームを備え、これらのアームの回転軸が互いに異なる多軸ロボットにおいて、容易かつ確実に、振動を抑制することができるロボットを提供することにある。

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

これにより、容易かつ確実に、ロボットの振動を抑制することができる。
すなわち、膨大な演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、第１アームと第２アームにそれぞれ慣性センサーを設置するとともに、第１アームの回動の回転軸と第２アームの回動の回転軸とを互いに直交させたので、それぞれのアームの角速度が互いに混在しない単純な回転成分として検出できる。よってこれらを用いた演算による制御を行うので、より容易に、精度良く、確実に、ロボットの振動を抑制することができる。

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

これにより、より確実にロボットの振動を抑制することができる。また、第１アームの回動の回転軸と第２アームの回動の回転軸とを互いに直交させたので、それぞれのアームの角速度が互いに混在しない単純な回転成分として検出でき、これらの混在無き回転成分にそれぞれ、フィードバックゲインを乗算するので、それぞれが高い精度で補正できる。よってこれらを用いた演算による制御を行うので、より容易に、精度良く、確実に、ロボットの振動を抑制することができる。

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

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

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

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

これにより、より確実にロボットの振動を抑制することができる。また、第１アームの回動の回転軸と第２アームの回動の回転軸とを互いに直交させ、且つ第２アームの回動の回転軸と第３アームの回動の回転軸とを互いに平行にしたので、それぞれのアームの角速度が互いに混在しない単純な回転成分として検出でき、これらの混在無き回転成分にそれぞれ、フィードバックゲインを乗算するので、それぞれが高い精度で補正できる。よってこれらを用いた演算による制御を行うので、より容易に、精度良く、確実に、ロボットの振動を抑制することができる。

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

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

そして、特に、第２駆動源制御手段は、角速度ωＡ３、ωＡ２ｍおよびωＡ３ｍによって第２駆動源の作動を制御する、すなわち、第２アームを回動させる第２駆動源に対して、第２アームよりも大きな振動が生じる第３アームに設置されている第３慣性センサーの検出結果を用いて、振動を抑える制御を行うので、振動を抑制する効果を高めることができる。

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

これにより、より確実にロボットの振動を抑制することができる。また、第１アームの回動の回転軸と第２アームの回動の回転軸とを互いに直交させ、且つ第２アームの回動の回転軸と第３アームの回動の回転軸とを互いに平行にしたので、それぞれのアームの角速度が互いに混在しない単純な回転成分として検出でき、これらの混在無き回転成分にそれぞれ、フィードバックゲインを乗算するので、それぞれが高い精度で補正できる。よってこれらを用いた演算による制御を行うので、より容易に、精度良く、確実に、ロボットの振動を抑制することができる。特に、第２駆動源制御手段では、第３慣性センサーが検出した成分を含む値にフィードバックゲインを乗算するので、振動を抑制する効果を一層高めることができる。

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

これにより、容易かつ確実に、ロボットの振動を抑制することができる。
すなわち、膨大な演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。
また、各アームに慣性センサーを設置し、各アームに対してその振動を抑える制御を行うので、より確実に、ロボットの振動を抑制することができる。
そして、特に、第２駆動源制御手段では、前記角速度ωＡ３と前記角速度ωＡ２ｍと前記角速度ωＡ３ｍとから導かれる値、または前記角速度ωＡ２と前記角速度ωＡ２ｍとから導かれる値、のいずれか一方を、角度検出手段の検出結果に基づいて選択してフィードバックして、振動を抑制する効果と制御の安定性との両立を図ることができる。

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

（適用例９）
本発明のロボットにおいて、前記アーム角が第１の閾値以上、該第１の閾値よりも大きい第２の閾値以下では、前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックし、前記アーム角が前記第１の閾値未満または前記第２の閾値よりも大きい場合は、前記角速度ωＡ２から前記角速度ωＡ２ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックする前記第２駆動源制御手段を備えることが好ましい。

これにより、より確実に、振動を抑制する効果を高めることと、制御の安定性との両立を図ることができる。
すなわち、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値以上、第２の閾値以下の場合（伸長姿勢）は、第２アームの軸線と第３アームの軸線とのなす角θが第１の閾値未満または第２の閾値よりも大きい場合（折り畳み姿勢）に比べて、ロボットの制御が安定しているが、ロボットの慣性モーメントが大きく、振動が大きいので、前記角速度ωＡ３から前記角速度ωＡ２ｍおよび前記角速度ωＡ３ｍを減算した値に対してフィードバックゲインを乗算した前記第２補正成分により前記第２角速度指令をフィードバックすることにより、振動を抑制する効果を高めることができる。

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

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

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

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

（適用例１４）
本発明のロボットは、基台と、前記基台に連結され、第１回転軸を軸中心として回動する第１アームと、前記第１回転軸と直交する第２回転軸を軸中心として回動する第２アームと、前記第１アームの角速度を検出する第１慣性センサーと、前記第１アームの駆動源の回転角度を検出する第１角度センサーと、前記第２アームの角速度を検出する第２慣性センサーと、前記第２アームの駆動源の回転角度を検出する第２角度センサーと、前記第１角度センサーの検出結果から導かれる角速度と前記第１慣性センサーから検出される角速度とをフィードバックする前記第１アームの駆動源の制御手段と、前記第２角度センサーの検出結果から導かれる角速度と前記第２慣性センサーから検出される角速度とをフィードバックする前記第２アームの駆動源の制御手段と、を備えることを特徴とする。
これにより、３次元空間を自在に作業できる多関節ロボットでありながら、容易かつ確実に、駆動による振動を抑制するロボットを提供することができる。

本発明のロボットでは、互いに直交な回転軸と、または平行な回転軸と、が混在するようにアームを連結するので、ひとつの慣性センサーからの情報は少なくともひとつの座標軸の情報のみでよい。従って、例えばヤコビ変換等の座標軸変換が不要であり、簡便な演算で制御部へフィードバックできる。
すなわち、膨大な情報に基づいて制御部へフィードバックする為の演算が不要であり、これにより、ロボットの制御における応答速度を速くすることができる。また、特異点の存在する演算が不要であるので、確実に、ロボットの制御を行うことができ、振動を抑制することができる。なお、第１アームの駆動源の制御手段と第２アームの駆動源の制御手段は演算上の回路は互いに独立しているが、同じＩＣ回路上に構築することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

＜第３実施形態＞
図１４は、本発明のロボットの第３実施形態の主要部を示すブロック図である。
以下、第３実施形態について、前述した第２実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１４に示すように、第３実施形態のロボット１では、制御装置２０の第２駆動源制御部２０２が第２実施形態と異なっている。以下、第２駆動源制御部２０２について説明する。

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

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

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

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

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

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

また、前述した第３駆動源制御部２０３の角速度算出部５６９からは、第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが加減算器６２９に出力される。
また、前述した第３慣性センサー３３の検出信号、すなわち、第３慣性センサー３３により検出された第３アーム１４の回転軸Ｏ２の回りの角速度ωＡ３は、加減算器６２９と加減算器６１１０に出力される。

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

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

このロボット１によれば、前述した第２実施形態と同様の効果が得られる。
そして、このロボット１では、第２アーム１３を駆動する第２駆動源４０２に対して、第２アーム１３よりも大きな振動が生じる先端側の第３アーム１４に設置されている第３慣性センサー３３の検出結果を用いて、振動を抑える制御を行うので、ロボット１の振動を抑制する効果を高めることができる。

詳述すると、第２駆動源制御部２０２は、角速度ωＡ３、ωＡ２ｍおよびωＡ３ｍによって第２駆動源４０２の作動を制御する、すなわち、第２アーム１３を回動させる第２駆動源４０２に対して、第２アーム１３よりも大きな振動が生じる第３アーム１３に設置されている第３慣性センサー３３の検出結果を用いて、振動を抑える制御を行うので、振動を抑制する効果を高めることができる。

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

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

また、図１６に示すように、第４実施形態のロボット１では、制御装置２０の第２駆動源制御部２０２が第２実施形態および第３実施形態と異なっている。以下、第２駆動源制御部２０２について説明する。
図１６に示すように、第２駆動源制御部２０２は、切替部６３、６４を有している。切替部６３は、角速度ωＡ２と角速度ωＡ３とのいずれか一方を選択し、加減算器６２１１に出力するものである。また、切替部６４は、角速度ωＡ３ｍを加減算器６２１１に出力する場合と出力しない場合とを切り替えるものである。

このロボット１では、第２駆動源制御部２０２は、前述したように第３角度センサー４１３を用いて角度θを検出し、その検出した角度θに応じて、加減算器６２１１に、角速度ωＡ３、角速度ωＡ２ｍおよび角速度ωＡ３ｍが入力されるように切替部６３、６４を切り替える場合と、角速度ωＡ２および角速度ωＡ２ｍが入力されるように切替部６３、６４を切り替える場合とを選択する。

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

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

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

一方、第３アーム１４が折り畳み姿勢の場合は、伸長姿勢の場合に比べて、ロボット１の慣性モーメントが小さく、振動が小さいが、制御が不安定になりがちで、ロボット１の振動し易いので、第３アーム１４よりも小さな振動が生じる第３アーム１４の基端側の第２アーム１３に設置されている第２慣性センサー３２の検出結果を用いて、第２駆動源４０２を前記第２実施形態と同様に制御することにより、ロボット１の振動を防止し、制御を安定させることができる。

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

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

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

そして、角度θが第１の閾値以上、第２の閾値以下の場合は、前述した第３駆動源制御部２０３の角速度算出部５６１１から加減算器６２１１に、第３アーム１４の回転軸Ｏ３の回りの角速度ωＡ３ｍが入力される。また、前述した第３慣性センサー３３の検出信号、すなわち、第３慣性センサー３３により検出された第３アーム１４の回転軸Ｏ２の回りの角速度ωＡ３が加減算器６２１１に入力される。

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

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

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

このロボット１によれば、前述した第２、３実施形態と同様の効果が得られる。
そして、このロボット１では、第２アーム１３の軸線１３１と第３アーム１４の軸線１４１とのなす角θに応じて適切な制御を行うことができる。
なお、本実施形態では、第３駆動源制御部２０３の構成は、第２、第３実施形態の第３駆動源制御部２０３と同様であるが、これに限らず、例えば、第１実施形態の第３駆動源制御部２０３と同様であってもよい。

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

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

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

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

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

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

Claims (7)

1. 基台と、
   前記基台に連結され第１回転軸を軸中心として回動する第１アームと、前記第１回転軸の軸方向と異なる軸方向である第２回転軸を軸中心として回動する第２アームと、前記第２回転軸の軸方向と平行な軸方向である第３回転軸を軸中心として回動する第３アームと、前記第３回転軸の軸方向と異なる軸方向である第４回転軸を軸中心として回動する第４アームと、前記第４回転軸の軸方向と異なる軸方向である第５回転軸を軸中心として回動する第５アームと、前記第５回転軸の軸方向と異なる軸方向である第６回転軸を軸中心として回動する第６アームと、を含むアーム連結体と、
   前記第１アーム、前記第２アームおよび前記第３アームのうち少なくとも一つに設けられた慣性センサーと、を備え、
   前記アーム連結体は、前記慣性センサーからの出力に基づいて制御され、
   前記第６アームには、前記アーム連結体の振動を抑制するための慣性センサーが設けられていない、
   ことを特徴とするロボット。
2. 前記第１アームを駆動する第１駆動源を備え、
   前記慣性センサーは、前記第１アームに設けられた第１慣性センサーを含み、
   前記第１駆動源は、前記第１慣性センサーからの出力に基づいて制御される、
   ことを特徴とする請求項１に記載のロボット。
3. 前記第２アームを駆動する第２駆動源と、
   前記慣性センサーは、前記第２アームに設けられた第２慣性センサーを含み、
   前記第２駆動源は、前記第２慣性センサーからの出力に基づいて制御される、
   ことを特徴とする請求項１または２に記載のロボット。
4. 前記第３アームを駆動する第３駆動源と、
   前記慣性センサーは、前記第３アームに設けられた第３慣性センサーを含み、
   前記第３駆動源は、前記第３慣性センサーからの出力に基づいて制御される、
   ことを特徴とする請求項１ないし３のいずれか一項に記載のロボット。
5. 前記第１慣性センサーは、前記第１アームの先端部に設けられている、
   ことを特徴とする請求項２に記載のロボット。
6. 前記第１回転軸は、前記基台の設置面の法線と一致する、
   ことを特徴とする請求項１ないし５のいずれか一項に記載のロボット。
7. 前記慣性センサーは、ジャイロセンサーである、
   ことを特徴とする請求項１ないし６のいずれか一項に記載のロボット。

Images