JP6036476B2 - ロボット - Google Patents

Description

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

基台と複数の回動可能なアームとを備え、３次元空間で自在に作業をおこなう多軸ロボットであって、アームの回動やアームに加わる外乱等によってアームに振動が生じ易いという問題に対して、この振動の減衰を目的として、最も先端側のアームにＸ軸、Ｙ軸、Ｚ軸の３軸方向の加速度を検知する加速度センサーを設け、振動抑制制御機能を有するロボットが知られている。このロボットは、例えば、基台と、基台に連結され、第１回転軸を軸中心として回動する第１アームと、第１アームに連結され、第１回転軸と直交する第２回転軸を軸中心として回動する第２アームと、第２アームに連結され、第２回転軸と直交する第３回転軸を軸中心として回動する第３アームと、第３アームに連結され、第３回転軸と直交する第４回転軸を軸中心として回動し、長手形状をなす第４アームとを有している。

また、基台と複数のアームとを備え、これらのアームの回転軸が互いに平行である多軸ロボットであって、最も先端側のアームやアームごとに角速度センサーや加速度センサー等の慣性センサーを設け、その慣性センサーが検出する成分も含めて演算することにより制御され、これにより振動を抑制するロボットが知られている（例えば、特許文献１参照）。

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

しかしながら、従来のロボットでは、最も基台側のアームである第１アームの第１回転軸と、基台側から４番目のアームである第４アームの長手方向に延びる長手軸とが平行である場合は、さらに先端側のアームや負荷質量のために第４アームの長手方向軸周りのねじり振動が発生し、このねじり振動が第１回転軸と同方向であるために、これらの軸の制御の補正値としてこのねじり振動が含まれてフィードバックされるという課題を見出した。同様に、基台側から２番目のアームである第２アームの第２回転軸と第４アームの長手方向に延びる長手軸とが平行である場合は、第４アームの長手方向軸周りのねじり振動が発生し、このねじり振動が第１回転軸と同方向であるために、これらの軸の制御の補正値としてこのねじり振動が含まれてフィードバックされるという課題を見出した。
本発明の目的は、複数のアームを備える多軸ロボットにおいて、基台側から４番目のアームである第４アームにおけるねじり振動が含まれてもフィードバック制御できるロボットを提供することにある。

本発明に係わる以下の適用例により、複数のアームを備える多軸ロボットにおいて、基台側から４番目のアームである第４アームにおけるねじり振動を含む振動抑制制御におけるフィードバック制御をおこなうロボットを提供できる。
（適用例１）
本発明に係わるロボットは、基台と、
前記基台に連結されて第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結されて第２回転軸を軸中心として回動し、前記第２回転軸を前記第１回転軸と直交する回転軸または前記第１回転軸と直交する軸と平行な回転軸とする第２アームと、
前記第２アームに連結されて第３回転軸を軸中心として回動し、前記第３回転軸を前記第２回転軸と直交する回転軸または前記第２回転軸と直交する軸と平行な回転軸とする第３アームと、
前記第３アームに連結されて第４回転軸を軸中心として回動し、前記第４回転軸を前記第３回転軸と直交する回転軸または前記第３回転軸と直交する軸と平行な回転軸とし且つ長手形状をなす第４アームと、
前記第１アームを回動させる第１駆動源と、
前記第１駆動源の回転角度を検出する第１角度センサーと、
前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
前記第４アームの姿勢を検出する姿勢検出部と、
前記第１駆動源の回転角度から導かれる前記第１アームの前記第１回転軸の角速度と前記第１慣性センサーにより検出された前記第１アームの前記第１回転軸の角速度とから導かれる第１補正成分をフィードバックして前記第１駆動源を制御するとともに、前記姿勢検出部の検出結果に基づいて前記第１補正成分のフィードバックゲインを設定する第１駆動源制御部と、
を備えることを特徴とする。

これにより、ロボットのアームでの振動を抑制することができる。
第１アームの第１回転軸と、第４アームの長手方向に延びる長手軸とが平行であって、さらに先端側のアームや負荷質量のために発生する第４アームの長手軸周りのねじり振動が第１回転軸と同方向であっても、フィードバックゲインの設定を変更することにより、第１補正成分をフィードバックして第１駆動源を制御する振動抑制制御が不安定になることを防止し、安定した振動抑制が可能になる。これにより、アーム剛性不足や大きな負荷質量の場合であってもロボットの性能に大きな影響を与える１次共振周波数の振動を抑制する能力を高く維持することができる。その結果、振動抑制能力の高い高性能のロボットを提供することができる。

（適用例２）
本発明に係わるロボットでは、前記第１駆動源制御部は、前記第４アームの長手方向に延びる長手軸と前記第１回転軸とが平行であるときの前記第１補正成分のフィードバックゲインを、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第１回転軸とが直交するときの前記第１補正成分のフィードバックゲインよりも低く設定することが好ましい。
第４アームの長手軸周りのねじり振動が第１回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害することを低減することができる。

（適用例３）
本発明に係わるロボットでは、前記第４アームの長手方向に延びる長手軸と前記第１回転軸とが平行であるときの前記第１補正成分のフィードバックゲインは、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第１回転軸とが直交するときの前記第１補正成分のフィードバックゲインの０％以上、７０％以下に設定されることが好ましい。
第４アームの長手軸周りのねじり振動が第１回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。

（適用例４）
本発明に係わるロボットでは、前記第２アームを回動させる第２駆動源と、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第２アームの前記第２回転軸の角速度または加速度を検出する第２慣性センサーと、
前記第２駆動源の回転角度から導かれる前記第２アームの前記第２回転軸の角速度と前記第２慣性センサーにより検出された前記第２アームの前記第２回転軸の角速度とから導かれる第２補正成分をフィードバックして前記第２駆動源を制御するとともに、前記姿勢検出部の検出結果に基づいて前記第２補正成分のフィードバックゲインを設定する第２駆動源制御部と、
を備えることが好ましい。
第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。これにより、第２補正成分をフィードバックして第２駆動源を制御する振動抑制制御が不安定になることを防止し、安定した振動抑制が可能になり、これによって、より確実に、ロボットの振動を抑制することができる。

（適用例５）
本発明に係わるロボットでは、前記第２駆動源制御部は、前記第４アームの長手方向に延びる長手軸と前記第２回転軸とが平行であるときの前記第２補正成分のフィードバックゲインを、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記第２補正成分のフィードバックゲインよりも低く設定することが好ましい。
第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。

（適用例６）
本発明に係わるロボットでは、前記第４アームの長手方向に延びる長手軸と前記第２回転軸とが平行であるときの前記第２補正成分のフィードバックゲインは、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記第２補正成分のフィードバックゲインの０％以上、７０％以下に設定されることが好ましい。
第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。これにより、より確実に、ロボットの振動を抑制することができる。

（適用例７）
本発明に係わるロボットでは、前記第２慣性センサーは、前記第４アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。
（適用例８）
本発明に係わるロボットでは、前記第２慣性センサーは、前記第３アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。

（適用例９）
本発明に係わるロボットでは、前記第１慣性センサーは、前記第４アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。
（適用例１０）
本発明に係わるロボットでは、前記第１慣性センサーは、前記第３アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。
（適用例１１）
本発明に係わるロボットでは、前記第１慣性センサーは、前記第１アームに設置され、
前記第２慣性センサーは、前記第２アームに設置されることが好ましい。
これにより、より確実に、ロボットの振動を抑制することができる。

（適用例１２）
本発明に係わるロボットは、基台と、
前記基台に連結されて第１回転軸を軸中心として回動する第１アームと、
前記第１アームに連結されて第２回転軸を軸中心として回動し、前記第２回転軸を前記第１回転軸と直交する回転軸または前記第１回転軸と直交する軸と平行な回転軸とする第２アームと、
前記第２アームに連結されて第３回転軸を軸中心として回動し、前記第３回転軸を前記第２回転軸と直交する回転軸または前記第２回転軸と直交する軸と平行な回転軸とする第３アームと、
前記第３アームに連結されて第４回転軸を軸中心として回動し、前記第４回転軸を前記第３回転軸と直交する回転軸または前記第３回転軸と直交する軸と平行な回転軸とし且つ長手形状をなす第４アームと、
前記第２アームを回動させる第２駆動源と、
前記第２駆動源の回転角度を検出する第２角度センサーと、
前記第２アームの前記第２回転軸の角速度または加速度を検出する第２慣性センサーと、
前記第４アームの姿勢を検出する姿勢検出部と、
前記第２駆動源の回転角度から導かれる前記第２アームの前記第２回転軸の角速度と 前記第２慣性センサーにより検出された前記第２アームの前記第２回転軸の角速度とから導かれる補正成分をフィードバックして前記第２駆動源を制御するとともに、前記姿勢検出部の検出結果に基づいて前記補正成分のフィードバックゲインを設定する第２駆動源制御部と、を備えることを特徴とする。

これにより、ロボットの振動を抑制することができる。
第２アームの第２回転軸と、第４アームの長手方向に延びる長手軸とが平行であり、さらに先端側のアームや負荷質量のために発生する第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、これらの軸の制御の補正値にこのねじり振動が含まれてもフィードバック制御をすることができる。すなわち、補正成分をフィードバックして第２駆動源を制御する振動抑制制御が不安定になることを防止し、安定した振動抑制が可能になる。これにより、アーム剛性不足や大きな負荷質量の場合であってもロボットの性能に大きな影響を与える１次共振周波数の振動を抑制する能力を高く維持することができる。その結果、振動抑制能力の高い高性能のロボットを提供することができる。

（適用例１３）
本発明に係わるロボットの前記駆動源制御部では、前記第４アームの長手方向に延びる長手軸と前記第２回転軸とが平行であるときの前記補正成分のフィードバックゲインを前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記補正成分のフィードバックゲインよりも低く設定することが好ましい。
第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。

（適用例１４）
本発明に係わるロボットでは、前記長手軸と前記第２回転軸とが平行であるときの前記補正成分のフィードバックゲインは、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記補正成分のフィードバックゲインの０％以上、７０％以下に設定されることが好ましい。
第４アームの長手軸周りのねじり振動が第２回転軸と同方向であっても、フィードバックゲインを低く設定することにより、前記ねじり振動が振動制御を阻害する程度を低減することができる。

（適用例１５）
本発明に係わるロボットでは、前記第２慣性センサーは、前記第４アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。
（適用例１６）
本発明のロボットでは、前記第２慣性センサーは、前記第３アームに設置されることが好ましい。
これにより、センサーの数を削減しつつ、ロボットの振動を抑制することができる。

（適用例１７）
前本発明に係わるロボットでは、前記第４アームに連結されて第５回転軸を軸中心として回動し、前記第５回転軸を前記第４回転軸と直交する回転軸または前記第４回転軸と直交する軸と平行な回転軸とする第５アームと、
前記第５アームに連結されて第６回転軸を軸中心として回動し、前記第６回転軸を前記第５回転軸と直交する回転軸または前記第５回転軸と直交する軸と平行な回転軸とする第６アームと、
前記第６アームに連結されて第７回転軸を軸中心として回動し、前記第７回転軸を前記第６回転軸と直交する回転軸または前記第６回転軸と直交する軸と平行な回転軸とする第７アームと、
を有することが好ましい。
これにより、ロボットは、振動を抑制するとともに、より多彩な動作を実行することができる。

本発明のロボットの第１実施形態を模式的に示す正面図である。 図１に示すロボットのアーム連結体の概略図である。 図１に示すロボットのアーム連結体の概略図である。 図１に示すロボットのアーム連結体の側面図である。 図１に示すロボットのアーム連結体の平面図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの主要部のブロック図である。 図１に示すロボットの制御動作を示すフローチャートである。 本発明のロボットの第２実施形態におけるアーム連結体を示す概略図である。

以下、本発明のロボットを添付図面に示す好適な実施形態に基づいて詳細に説明する。
なお、本発明のロボットは、複数のアームを回動自在に連結してなるアーム連結体を１つ以上有するロボットに適用することができるが、下記の実施形態では、代表的に、本発明を、前記アーム連結体を２つ有する双腕ロボット適用した場合について説明する。
＜第１実施形態＞
図１は、本発明のロボットの第１実施形態を模式的に示す正面図である。図２は、図１に示すロボットのアーム連結体の概略図である。図３は、図１に示すロボットのアーム連結体の概略図である。図４は、図１に示すロボットのアーム連結体の側面図である。図５は、図１に示すロボットのアーム連結体の平面図である。図６〜図１３は、それぞれ、図１に示すロボットの主要部のブロック図である。図１４は、図１に示すロボットの制御動作を示すフローチャートである。

なお、以下では、説明の都合上、図１〜図５中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１〜図５中の支持部側を「基端」、その反対側を「先端」と言う。また、図１では、各アーム連結体８は、それぞれ、模式的に示され、そのアームの数、形状、姿勢等は、図２〜図５に図示されている実際のものとは異なっている。また、図４では、第２回転軸Ｏ２は、誇張して図示され、また、図５では、第３回転軸Ｏ３は、誇張して図示されている。また、図２、図４、図５では、慣性センサー３１は、その存在を明確にするため、第４アーム１５の外部に図示されている。また、図４および図５では、アーム連結体８の第１アーム１２〜第４アーム１５までが図示され、第５アーム１６およびリスト１７の図示は、省略されている。

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

ロボット１のロボット本体１０は、胴部７と、胴部７に連結された１対のアーム連結体８と、胴部７を支持する基部１００とを備えている。この場合、各アーム連結体８の支持部（基台）１１が、それぞれ、胴部７に固定されている。
なお、２つのアーム連結体８の構成は同様であるので、以下では、代表的に、一方のアーム連結体８について説明し、他方のアーム連結体８については説明を省略する。

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

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

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

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

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

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

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

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

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

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

また、第４アーム１５内には、慣性センサー３１が設置されている。本実施形態では、慣性センサー３１として、互いに直交する３つの検出軸であるｘ軸、ｙ軸、ｚ軸（いずれも図示せず）のそれぞれの回りの角速度を検出する角速度センサーを用いる。具体的には、例えば、ジャイロセンサー等を用いることができる。
また、慣性センサー３１の姿勢は、特に限定されず、例えば、アーム連結体８の姿勢が、図２（ａ）に示すように、第１回転軸Ｏ１と平行な軸と第３回転軸Ｏ３とが直交し、第１回転軸Ｏ１と第４回転軸Ｏ４とが平行であり、かつ、第１回転軸Ｏ１と第４アーム１５の長手軸１５１とが平行である場合に、慣性センサー３１の３つの検出軸が、それぞれ、第１回転軸Ｏ１、第２回転軸Ｏ２、第３回転軸Ｏ３と平行になるような姿勢でもよく、また、傾斜するような姿勢でもよい。

この慣性センサー３１により、慣性センサー３１の設置個所におけるｘ軸の回りの角速度、ｙ軸の回りの角速度、ｚ軸の回りの角速度をそれぞれ検出する。そして、制御部２０は、その検出結果および慣性センサー３１（第４アーム１５）の姿勢等に基づいて、第１アーム１２の第１回転軸Ｏ１の回りの角速度、第２アーム１３の第２回転軸Ｏ２の回りの角速度、第３アーム１４の第３回転軸Ｏ３の回りの角速度をそれぞれ求める。各角速度を求めるには、それぞれ、例えば、公知の演算を行えばよいので、その説明は省略する。なお、本願の特許請求の範囲では、前記アームの回転軸の回りの角速度をアームの回転軸の角速度と言っている。

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

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

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

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

図７に示すように、第１駆動源制御部２０１は、減算器５１１と、位置制御部５２１と、減算器５３１と、角速度制御部５４１と、回転角度算出部５５１と、角速度算出部５６１と、減算器５７１と、変換部５８１と、補正値算出部５９１と、加算器６０１と、フィードバックゲイン調整部６１１、角速度算出部６２１とを有している。なお、角速度算出部６２１および慣性センサー３２により、第１アーム１２の第１回転軸Ｏ１の回りの角速度を検出する第１角速度検出部が構成される。

図８に示すように、第２駆動源制御部２０２は、減算器５１２と、位置制御部５２２と、減算器５３２と、角速度制御部５４２と、回転角度算出部５５２と、角速度算出部５６２と、減算器５７２と、変換部５８２と、補正値算出部５９２と、加算器６０２と、フィードバックゲイン調整部６１２、角速度算出部６２２を有している。なお、角速度算出部６２２および慣性センサー３１により、第２アーム１３の第２回転軸Ｏ２の回りの角速度を検出する第２角速度検出部が構成される。

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

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

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

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

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

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

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

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

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

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

補正値算出部５９１は、角速度ωｍ１ｓに予め定められた係数であるゲイン（フィードバックゲイン）Ｋａを乗算し、補正値（第１補正成分）Ｋａ・ωｍ１ｓを求め、その補正値Ｋａ・ωｍ１ｓを加算器６０１に出力する。ゲインＫａは、フィードバックゲイン調整部６１１により、第１回転軸Ｏ１に対する第４アーム１５の姿勢に応じて調整される。なお、このゲインＫａの調整については、後で詳述する。
加算器６０１には、角速度ωｍ１が入力され、また、補正値Ｋａ・ωｍ１ｓが入力される。加算器６０１は、角速度ωｍ１と補正値Ｋａ・ωｍ１ｓとの加算値を角速度フィードバック値ωｆｂとして減算器５３１に出力する。なお、以降の動作は、前述した通りである。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

さて、このロボット１では、フィードバックゲイン調整部６１１、６１２により、第１回転軸Ｏ１に対する第４アーム１５の姿勢、第２回転軸Ｏ２に対する第４アーム１５の姿勢に応じて、それぞれ、ゲイン（フィードバックゲイン）Ｋａを調整する。すなわち、ゲインＫａを標準値（通常値）である第１の値と、標準値よりも低い第２の値とのいずれかに設定するよう構成されている。なお、第１回転軸Ｏ１に対する第４アーム１５の姿勢は、フィードバックゲイン調整部６１１が、各駆動源４０１〜４０４に設けられた各角度センサー４１１〜４１４の検出結果に基づいて求める。同様に、第２回転軸Ｏ２に対する第４アーム１５の姿勢は、フィードバックゲイン調整部６１２が、各駆動源４０１〜４０４に設けられた各角度センサー４１１〜４１４の検出結果に基づいて求める。したがって、フィードバックゲイン調整部６１１により、第１回転軸Ｏ１に対する第４アーム１５の姿勢を検出する姿勢検出部が構成される。同様に、フィードバックゲイン調整部６１２により、第２回転軸Ｏ２に対する第４アーム１５の姿勢を検出する姿勢検出部が構成される。

具体的には、第４アーム１５の長手方向に延びる長手軸（中心軸）１５１と第１アーム１２の第１回転軸Ｏ１とが平行であるときまたは平行に近いとき、第４アーム１５の長手方向に延びる長手軸１５１と第２アーム１３の第２回転軸Ｏ２とが平行であるときまたは平行に近いときは、それぞれ、ゲインＫａを標準値（通常値）である第１の値よりも低減する、すなわち、第１の値よりも低い第２の値に設定するよう構成されている。第４アーム１５の長手軸１５１と第１アーム１２の第１回転軸Ｏ１とが平行であるときまたは平行に近いときは、ゲインＫａを第２の値に設定することにより、ロボット１の振動の発振を防止することができる。同様に、第４アーム１５の長手軸１５１と第２アーム１３の第２回転軸Ｏ２とが平行であるときまたは平行に近いときは、ゲインＫａを第２の値に設定することにより、ロボット１の振動の発振を防止することができる。

このゲインＫａの第２の値は、他の姿勢のときのゲインＫａ、すなわち、第１の値の０％以上、７０％以下に設定されることが好ましく、３０％以上、６０％以下に設定されることがより好ましい。これにより、より確実に、ロボット１の振動の発振を防止しつつ、その振動を防止することができる。
なお、第１駆動源４０１の制御においては、前記他の姿勢とは、第４アーム１５の長手軸１５１と第１アーム１２の第１回転軸Ｏ１とが平行、平行に近いとき以外であり、その１例（典型例）としては、第４アーム１５の長手軸１５１と平行であり、第１回転軸Ｏ１と交差し得る第１仮想軸１５２を想定した場合、その第１仮想軸１５２と第１回転軸Ｏ１とが直交するときである。すなわち、前記１例は、第３回転軸Ｏ３と同じ方向から平面視して長手軸１５１と第１回転軸Ｏ１とが直交するときである。

同様に、第２駆動源４０２の制御においては、前記他の姿勢とは、第４アーム１５の長手軸１５１と第２アーム１３の第２回転軸Ｏ２とが平行、平行に近いとき以外であり、その１例（典型例）としては、第４アーム１５の長手軸１５１と平行であり、第２回転軸Ｏ２と交差し得る第２仮想軸１５３を想定した場合、その第２仮想軸１５３と第２回転軸Ｏ２とが直交するときである。すなわち、前記１例は、第３回転軸Ｏ３と同じ方向から平面視して長手軸１５１と第２回転軸Ｏ２とが直交するときである。

また、ゲインＫａの第２の値は、特に限定されず、諸条件に応じて適宜設定されるものであるが、０以上、０．６以下であることが好ましく、０．１以上、０．４以下であることがより好ましい。より確実に、ロボット１の振動の発振を防止しつつ、その振動を防止することができる。
また、ゲインＫａの第１の値は、特に限定されず、諸条件に応じて適宜設定されるものであるが、０．２以上、０．８以下であることが好ましく、０．３以上、０．６以下であることがより好ましい。より確実に、ロボット１の振動の発振を防止しつつ、その振動を防止することができる。

ここで、前述したように、第１駆動源４０１の制御において、第４アーム１５の長手軸１５１と第１アーム１２の第１回転軸Ｏ１とが平行であるときに限らず、平行に近いとき、すなわち、第４アーム１５の長手軸１５１に平行な軸である前記第１仮想軸１５２と第１アーム１２の第１回転軸Ｏ１とのなす角θ１（図２（ｂ）参照）が所定角度以内である場合もゲインＫａを第２の値に設定する。すなわち、第１仮想軸１５２と第１アーム１２の第１回転軸Ｏ１とのなす角θ１がθｍｉｎ以上、θｍａｘ以下である場合もゲインＫａを第２の値に設定する。

θｍａｘは、特に限定されず、諸条件に応じて適宜設定されるものであるが、１０°以上、４５°以下であることが好ましく、１５°以上、３５°以下であることがより好ましい。
またθｍｉｎは、特に限定されず、諸条件に応じて適宜設定されるものであるが、−４５°以上、−１０°以下であることが好ましく、−３５°以上、−１５°以下であることがより好ましい。

これにより、ロボット１の振動の発振をより確実に防止することができる。
なお、θ１は、フィードバックゲイン調整部６１１が、各駆動源４０１〜４０４に設けられた各角度センサー４１１〜４１４の検出結果に基づいて求める。
同様に、前述したように、第２駆動源４０２の制御において、第４アーム１５の長手軸１５１と第２アーム１３の第２回転軸Ｏ２とが平行であるときに限らず、平行に近いとき、すなわち、第４アーム１５の長手軸１５１に平行な軸である前記第２仮想軸１５３と第２アーム１３の第２回転軸Ｏ２とのなす角θ２（図２（ｃ）参照）が所定角度以内である場合もゲインＫａを第２の値に設定する。すなわち、第２仮想軸１５３と第２アーム１３の第２回転軸Ｏ２とのなす角θ２がθｍｉｎ以上、θｍａｘ以下である場合もゲインＫａを第２の値に設定する。
θｍａｘは、特に限定されず、諸条件に応じて適宜設定されるものであるが、１０°以上、４５°以下であることが好ましく、１５°以上、３５°以下であることがより好ましい。

またθｍｉｎは、特に限定されず、諸条件に応じて適宜設定されるものであるが、−４５°以上、−１０°以下であることが好ましく、−３５°以上、−１５°以下であることがより好ましい。
これにより、ロボット１の振動の発振をより確実に防止することができる。
なお、θ２は、フィードバックゲイン調整部６１２が、各駆動源４０１〜４０４に設けられた各角度センサー４１１〜４１４の検出結果に基づいて求める。

次に、ロボット１のゲインＫａを調整する際の制御部２０の制御動作について説明する。
図１４に示すように、まず、第１駆動源４０１の制御、第２駆動源４０２の制御のそれぞれにおけるゲインＫａの切り替えの基準となる角度範囲を設定する（ステップＳ１０１）。本実施形態では、例えば、第１仮想軸１５２と第１アーム１２の第１回転軸Ｏ１とのなす角θ１、第２仮想軸１５３と第２アーム１３の第２回転軸Ｏ２とのなす角θ２について、それぞれ、「−３０°以上、３０°以下」に設定する。

次いで、第１仮想軸１５２と第１アーム１２の第１回転軸Ｏ１とのなす角θ１を求める（ステップＳ１０２）。
次いで、第１仮想軸１５２と第１アーム１２の第１回転軸Ｏ１とのなす角θ１が、−３０°以上、３０°以下であるか否かを判断し（ステップＳ１０２）、θ１が、−３０°以上、３０°以下でない場合は、第１駆動源４０１の制御におけるゲインＫａを第１の値に設定する（ステップＳ１０３）。第１駆動源４０１の制御においては、θ１が−３０°以上、３０°以下でない場合は、ロボット１の振動が発振し難いので、ゲインＫａを第１の値に設定してもロボット１の振動が発振してしまうことがなく、また、ゲインＫａを第１の値に設定することにより、振動をより確実に抑制することができる。

また、ステップＳ１０２において、θ１が、−３０°以上、３０°以下である場合は、第１駆動源４０１の制御におけるゲインＫａを第２の値に設定する（ステップＳ１０４）。θ１が−３０°以上、３０°以下である場合は、ロボット１の振動が発振し易いので、ゲインＫａを第２の値に設定することにより、ロボット１の振動が発振してしまうことを防止しつつ、振動を抑制することができる。

次いで、第２仮想軸１５３と第２アーム１３の第２回転軸Ｏ２とのなす角θ２を求める（ステップＳ１０６）。
次いで、第２仮想軸１５３と第２アーム１３の第２回転軸Ｏ２とのなす角θ２が、−３０°以上、３０°以下であるか否かを判断し（ステップＳ１０５）、θ２が、−３０°以上、３０°以下でない場合は、第２駆動源４０２の制御におけるゲインＫａを第１の値に設定する（ステップＳ１０６）。第２駆動源４０２の制御においては、θ２が−３０°以上、３０°以下でない場合は、ロボット１の振動が発振し難いので、ゲインＫａを第１の値に設定してもロボット１の振動が発振してしまうことがなく、また、ゲインＫａを第１の値に設定することにより、振動をより確実に抑制することができる。

また、ステップＳ１０５において、θ２が、−３０°以上、３０°以下である場合は、第２駆動源４０２の制御におけるゲインＫａを第２の値に設定する（ステップＳ１０７）。θ２が−３０°以上、３０°以下である場合は、ロボット１の振動が発振し易いので、ゲインＫａを第２の値に設定することにより、ロボット１の振動が発振してしまうことを防止しつつ、振動を抑制することができる。

そして、ステップＳ１０６、Ｓ１０７の後、ステップＳ１０２に戻り、再度、ステップＳ１０２以降を実行する。
このようにして、第１駆動源４０１の制御におけるゲインＫａ、第２駆動源４０２の制御におけるゲインＫａを、それぞれ、θ１、θ２に応じて、第１の値と第２の値とのいずれかに設定する。これにより、ロボット１の振動の発振を防止しつつ、振動を抑制することができる。

以上説明したように、このロボット１によれば、容易かつ確実に、ロボット１の振動を抑制することができる。
特に、ロボット１の姿勢、すなわち、第１回転軸Ｏ１、第２回転軸Ｏ２に対する第４アーム１５の姿勢に応じてそれぞれ第１駆動源４０１の制御におけるゲインＫａ、第２駆動源４０２の制御におけるゲインＫａを調整するので、ロボット１の振動が発振してしまうことを防止することができ、より確実に、ロボット１の振動を抑制することができる。

なお、本実施形態では、慣性センサー３１を第４アーム１５に設置しているが、これに限定されず、慣性センサー３１を第３アーム１４に設置してもよい。この場合も、ロボット１の振動が発振してしまうことを防止しつつ、ロボット１の振動を抑制することができる。
また、慣性センサー３１としては、前記３つの検出軸を有する角速度センサーに限定されず、例えば、第３アーム１４または第４アーム１５に、１つの検出軸を有する角速度センサーを３つ設置してもよい。この場合は、各検出軸が互いに直交するように各角速度センサーを設置する。

＜第２実施形態＞
図１５は、本発明のロボットの第２実施形態におけるアーム連結体を示す概略図である。
なお、以下では、説明の都合上、図１５中の支持部側を「基端」、その反対側を「先端」と言う。図１５では、第１慣性センサー３２、第２慣性センサー３３、第３慣性センサー３４は、それぞれ、その存在を明確にするため、対応するアーム１５の外部に図示されている。

以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。
図１５に示すように、第２実施形態のロボット１では、第１アーム１２内に、第１慣性センサー３２が設置され、第２アーム１３内に、第２慣性センサー３３が設置され、第３アーム１４内に、第３慣性センサー３４が設置されている。

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

同様に、第２アーム１３における第２慣性センサー３３の設置位置は、特に限定されないが、第２アーム１３の先端部が好ましい。第２アーム１３の振動は、その先端部において最大になるので、これにより、より確実にロボット１の振動を抑制することができる。
同様に、第３アーム１４における第３慣性センサー３３の設置位置は、特に限定されないが、第３アーム１４の先端部が好ましい。第３アーム１４の振動は、その先端部において最大になるので、これにより、より確実にロボット１の振動を抑制することができる。

前記第１慣性センサー３２により第１アーム１２の第１回転軸Ｏ１の回りの角速度を検出する。また、第２慣性センサー３３により第２アーム１３の第２回転軸Ｏ２の回りの角速度を検出する。また、第３慣性センサー３４により第３アーム１４の角速度を検出する。なお、第３慣性センサー３４により検出される第３アーム１４の角速度には、第１アーム１２の回動による成分を含んでいるので、制御部２０は、第３慣性センサー３４の検出値から第１慣性センサー３２の検出値を減算し、その値を第３アーム１４の第３回転軸Ｏ３の回りの角速度とする。

このロボット１によれば、前述した第１実施形態と同様の効果が得られる。
そして、このロボット１では、制御部２０における演算を簡略化することができる。すなわち、第１回転軸Ｏ１と第２回転軸Ｏ２は直交し、また、第２回転軸Ｏ２と第３回転軸Ｏ３は直交しているので、第１慣性センサー３２により第１アーム１２の第１回転軸Ｏ１の回りの角速度のみが検出され、第２慣性センサー３３により第２アーム１３の第２回転軸Ｏ２の回りの角速度のみが検出される。これによって、特別な演算が不要となり、制御部２０の回路構成を簡素化することができる。

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

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

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

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

また、前記実施形態では、ロボットは、複数のアームを回動自在に連結してなるアーム連結体を２つ有する双腕ロボットであるが、本発明では、これに限定されず、例えば、複数のアームを回動自在に連結してなるアーム連結体を１つ有する単腕ロボット、または、３つ以上有するロボットであってもよい。
また、本発明のロボットは、アーム型ロボット（ロボットアーム）に限定されず、他の形式のロボット、例えば、脚式歩行（走行）ロボット等であってもよい。

１……ロボット（産業用ロボット） １０……ロボット本体 １１……支持部 １２、１３、１４、１５、１６、１８、１９……アーム（リンク） １７……リスト（リンク） １５１……長手軸 １５２、１５３……仮想軸 １７１、１７２、１７３、１７４、１７５、１７６、１７７……関節（ジョイント） ３１、３２、３３、３４……慣性センサー ７……胴部 ８……アーム連結体 ９……マニピュレーター １００……基部 ２０……制御部 １０１……床 ２０１、２０２、２０３、２０４、２０５、２０６、２０７……駆動源制御部 ３０１、３０２、３０３、３０４、３０５、３０６、３０７……モータードライバー ４０１、４０２、４０３、４０４、４０５、４０６、４０６……駆動源 ４０１Ｍ、４０２Ｍ、４０３Ｍ、４０４Ｍ、４０５Ｍ、４０６Ｍ、４０７Ｍ……モーター ４１１、４１２、４１３、４１４、４１５、４１６、４１７……角度センサー ５１１、５１２、５１３、５１４、５１５、５１６、５１７……減算器 ５２１、５２２、５２３、５２４、５２５、５２６、５２７……位置制御部 ５３１、５３２、５３３、５３４、５３５、５３６、５３７……減算器 ５４１、５４２、５４３、５４４、５４５、５４６、５４７……角速度制御部 ５５１、５５２、５５３、５５４、５５５、５５６、５５７……回転角度算出部 ５６１、５６２、５６３、５６４、５６５、５６６、５６７……角速度算出部 ５７１、５７２、５７３……減算器 ５８１、５８２、５８３……変換部 ５９１、５９２、５９３……補正値算出部 ６０１、６０２、６０３……加算器 ６１１、６１２……フードバックゲイン調整部 ６２１、６２２、６２３……角速度算出部 Ｏ１、Ｏ２、Ｏ３、Ｏ４、Ｏ５、Ｏ６、Ｏ７……回転軸

Claims (5)

1. 基台と、
   前記基台に連結されて第１回転軸を軸中心として回動する第１アームと、
   前記第１アームに連結されて第２回転軸を軸中心として回動し、前記第２回転軸を前記第１回転軸と交差する回転軸の軸方向と平行な回転軸とする第２アームと、
   前記第２アームに連結されて第３回転軸を軸中心として回動し、前記第３回転軸を前記第２回転軸と交差する回転軸の軸方向と平行な回転軸とする第３アームと、
   前記第３アームに連結されて第４回転軸を軸中心として回動し、前記第４回転軸を前記第３回転軸と交差する回転軸の軸方向と平行な回転軸とし且つ長手形状を成す第４アームと、
   前記第１アームを回動させる第１駆動源と、
   前記第１駆動源の回転角度を検出する第１角度センサーと、
   前記第１アームの前記第１回転軸の角速度または加速度を検出する第１慣性センサーと、
   前記第４アームの姿勢を検出する姿勢検出部と、
   前記第１駆動源の回転角度から導かれる前記第１アームの前記第１回転軸の角速度と前記第１慣性センサーにより検出された前記第１アームの前記第１回転軸の角速度とから導かれる第１補正成分をフィードバックして前記第１駆動源を制御し、前記姿勢検出部の検出結果に基づいて前記第１補正成分のフィードバックゲインを設定する第１駆動源制御部と、を備え、
   前記第１駆動源制御部は、前記第４アームの長手方向に延びる長手軸と前記第１回転軸とが平行であるときの前記第１補正成分のフィードバックゲインを、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第１回転軸とが直交するときの前記第１補正成分のフィードバックゲインよりも低く設定することを特徴とするロボット。
2. 前記第４アームの長手方向に延びる長手軸と前記第１回転軸とが平行であるときの前記第１補正成分のフィードバックゲインは、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第１回転軸とが直交するときの前記第１補正成分のフィードバックゲインの０％以上、７０％以下に設定される請求項１に記載のロボット。
3. 前記第２アームを回動させる第２駆動源と、
   前記第２駆動源の回転角度を検出する第２角度センサーと、
   前記第２アームの前記第２回転軸の角速度または加速度を検出する第２慣性センサーと、
   前記第２駆動源の回転角度から導かれる前記第２アームの前記第２回転軸の角速度と前記第２慣性センサーにより検出された前記第２アームの前記第２回転軸の角速度とから導かれる第２補正成分をフィードバックして前記第２駆動源を制御し、前記姿勢検出部の検出結果に基づいて前記第２補正成分のフィードバックゲインを設定する第２駆動源制御部と、
   を備える請求項１または２に記載のロボット。
4. 前記第２駆動源制御部は、前記第４アームの長手方向に延びる長手軸と前記第２回転軸とが平行であるときの前記第２補正成分のフィードバックゲインを、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記第２補正成分のフィードバックゲインよりも低く設定する請求項３に記載のロボット。
5. 前記第４アームの長手方向に延びる長手軸と前記第２回転軸とが平行であるときの前記第２補正成分のフィードバックゲインは、前記第３回転軸と同じ方向から平面視して前記長手軸と前記第２回転軸とが直交するときの前記第２補正成分のフィードバックゲインの０％以上、７０％以下に設定される請求項３または４に記載のロボット。

Images