JP2014050935A - ロボット制御装置、ロボットおよびロボットシステム - Google Patents

Abstract

【課題】ロボットの振動を抑制しつつ、容易に、機能を拡張することができるロボット制御装置、ロボットおよびロボットシステムを提供すること。
【解決手段】ロボット制御装置は、角速度センサーから出力される信号および位置センサーから出力される信号を受信する第１受信部と、角速度センサーから出力され、第１受信部で受信した信号から特定周波数帯域の信号を除去する第１フィルタリング処理部とを有する第１チップと、第１フィルタリング処理部から出力される信号を受信し、第１受信部を介して位置センサーから出力される信号を受信する第２受信部と、第２受信部で受信した信号に基づいて、アームの角速度の振動成分を求める演算部と、演算部で求められたアームの角速度の振動成分に基づいて、駆動源の作動を制御する制御部とを有する第２チップとを備える。
【選択図】図１

Description

本発明は、ロボット制御装置、ロボットおよびロボットシステムに関するものである。

従来、基台と、基台に対して回動自在に連結されたアーム軸部と、アーム軸部に対して回動自在に連結された給除材アームと、給除材アームの先端部に設けられたハンド保持機構と、アーム軸部を回動させるモーターと、給除材アームを回動させるモーターとを備えるロボットが知られている（例えば、特許文献１参照）。
各モーターには、それぞれ、その回転角度を検出する角度センサーが設置けられており、給除材アームには、その角速度を検出する角速度センサーが設置されている。そして、アーム軸部を回動させるモーターおよび給除材アームを回動させるモーターは、それぞれ、前記各角度センサー、角速度センサーの検出結果に基づいて、その作動が制御される。

この場合、アーム軸部を回動させるモーターの作動を制御するモーター制御部、給除材アームを回動させるモーターの作動を制御するモーター制御部は、それぞれ、例えば、ＤＳＰ（デジタルシグナルプロセッサー）等の１つの制御用チップで構成されている。制御用チップでは、角速度センサーおよび角度センサーから出力された各信号を受信し、角速度センサーから出力された信号に対して、ノイズを除去するフィルタリング処理等が行われる。

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

しかしながら、特許文献１に記載のロボットでは、１つの制御用チップにおいて、角速度センサーおよび角度センサーから出力された各信号を受信し、各処理を行っているので、その制御用チップに多くの処理が集中してしまうという問題がある。これにより、新たな機能を追加する場合、制御用チップに対して、その新たな機能を実現するための新たな処理を追加することが困難である。
本発明の目的は、ロボットの振動を抑制しつつ、容易に、機能を拡張することができるロボット制御装置、ロボットおよびロボットシステムを提供することにある。

このような目的は、下記の本発明により達成される。
本発明のロボット制御装置は、回動可能なアームと、前記アームを回動させる駆動源と、前記アームに設置された角速度センサーと、前記駆動源の回転角度を検出する位置センサーとを有するロボットの制御を行うロボット制御装置であって、
前記角速度センサーから出力される信号および前記位置センサーから出力される信号をそれぞれ受信する第１受信部と、前記角速度センサーから出力され、前記第１受信部で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理部とを有する第１チップと、
前記第１フィルタリング処理部から出力される信号を受信し、前記第１受信部を介して前記位置センサーから出力される信号を受信する第２受信部と、前記第２受信部で受信した信号に基づいて、前記アームの角速度の振動成分を求める演算部と、前記演算部で求められた前記アームの角速度の振動成分に基づいて、前記駆動源の作動を制御する制御部とを有する第２チップとを備えることを特徴とする。

これにより、ロボットの振動を抑制することができる。
また、第１フィルタリング処理部により、角速度センサーで発生するノイズを除去することができ、より確実に、ロボットの振動を抑制することができる。
また、角速度センサーから出力され、前記特定周波数帯域の上限値よりも高い周波数の信号および下限値よりも低い周波数の信号は、アームの角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。
また、第１チップと第２チップとを設け、かつ、処理時間が比較的長い処理を行う第１フィルタリング処理部を第１チップに設け、その他の演算部および制御部を第２チップに設けることにより、処理の負荷を第１チップと第２チップに適正に分散させることができる。これによって、容易に、機能を拡張することができる。

本発明のロボット制御装置では、前記第１フィルタリング処理部は、時刻ｔ（ｎ）における信号から前記特定周波数帯域の信号を除去する場合、少なくとも前記時刻ｔ（ｎ）における信号と、前記時刻ｔ（ｎ）よりも１つ前の時刻ｔ（ｎ−１）における信号と、前記時刻ｔ（ｎ）よりも２つ前の時刻ｔ（ｎ−２）における信号とを用いて処理を行うよう構成されていることが好ましい。

このような３次以上のフィルタリング処理を行うことにより、カットオフ周波数付近での信号の利得を向上させつつ、特定周波数帯域の信号をより確実に除去することができる。
また、前記フィルタリング処理は、処理時間が比較的長くかかるので、第２チップではなく、第１チップに第１フィルタリング処理部を設けることにより、第２チップにおける処理の負荷を大幅に軽減することができ、処理の負荷を適正に分散させることができる。

本発明のロボット制御装置では、前記第２チップは、前記演算部により前記アームの角速度の振動成分を求めた後、前記振動成分に対応する信号から所定周波数より低い周波数の信号を除去する第２フィルタリング処理部を有することが好ましい。

演算部では、アームの角速度の振動成分を求める際、例えば、角速度センサーのオフセット成分等の影響により、その振動成分に対応する信号としては存在しない周波数のノイズが発生する。このため、その振動成分を求めた後、第２フィルタリング処理部により所定周波数より低い周波数の信号を除去することにより、前記ノイズを除去することができる。これにより、より確実に、ロボットの振動を抑制することができる。

本発明のロボット制御装置では、前記演算部は、前記位置センサーから出力される信号に基づいて前記アームの角速度を求め、前記アームの角速度と、前記角速度センサーから出力される信号から得た前記アームの角速度との差分を求め、前記差分を前記アームの角速度の振動成分とすることが好ましい。
これにより、より確実に、ロボットの振動を抑制することができる。
本発明のロボット制御装置では、前記特定周波数帯域の周波数の幅は、３００Ｈｚ以上、９００Ｈｚ以下の範囲内に設定されることが好ましい。
これにより、角速度センサーで発生するノイズを除去することができ、より確実に、ロボットの振動を抑制することができる。

本発明のロボットは、回動可能なアームと、
前記アームを回動させる駆動源と、
前記アームに設置された角速度センサーと、
前記駆動源の回転角度を検出する位置センサーと、
前記角速度センサーから出力される信号および前記位置センサーから出力される信号をそれぞれ受信する第１受信部と、前記角速度センサーから出力され、前記第１受信部で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理部とを有する第１チップと、
前記第１フィルタリング処理部から出力される信号を受信し、前記第１受信部を介して前記位置センサーから出力される信号を受信する第２受信部と、前記第２受信部で受信した信号に基づいて、前記アームの角速度の振動成分を求める演算部と、前記演算部で求められた前記アームの角速度の振動成分に基づいて、前記駆動源の作動を制御する制御部とを有する第２チップとを備えることを特徴とする。

これにより、ロボットの振動を抑制することができる。
また、第１フィルタリング処理部により、角速度センサーで発生するノイズを除去することができ、より確実に、ロボットの振動を抑制することができる。
また、角速度センサーから出力され、前記特定周波数帯域の上限値よりも高い周波数の信号および下限値よりも低い周波数の信号は、アームの角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。
また、第１チップと第２チップとを設け、かつ、処理時間が比較的長い処理を行う第１フィルタリング処理部を第１チップに設け、その他の演算部および制御部を第２チップに設けることにより、処理の負荷を第１チップと第２チップに適正に分散させることができる。これによって、容易に、機能を拡張することができる。

本発明のロボットでは、前記アーム、前記駆動源、前記角速度センサーおよび前記位置センサーをそれぞれ２つ有し、
さらに基台を有し、
一方の前記アームは、前記基台に対し、第１回転軸を回転中心として回動自在に連結され、他方の前記アームは、一方の前記アームに対し、前記第１回転軸と直交または前記第１回転軸に直交する軸と平行な第２回転軸を回転中心として回動自在に連結されており、
前記第１チップおよび前記第２チップは、２つの前記アームのうちの少なくとも一方の制御用に設けられていることが好ましい。
これにより、前記本発明の効果を有し、２つのアームを有するロボットを提供することができる。

本発明のロボットシステムは、回動可能なアームと、前記アームを回動させる駆動源と、前記アームに設置された角速度センサーと、前記駆動源の回転角度を検出する位置センサーとを有するロボットと、
前記ロボットの制御を行う本発明のロボット制御装置とを備えることを特徴とする。
これにより、ロボットの振動を抑制することができる。
また、第１フィルタリング処理部により、角速度センサーで発生するノイズを除去することができ、より確実に、ロボットの振動を抑制することができる。
また、角速度センサーから出力され、前記特定周波数帯域の上限値よりも高い周波数の信号および下限値よりも低い周波数の信号は、アームの角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。
また、第１チップと第２チップとを設け、かつ、処理時間が比較的長い処理を行う第１フィルタリング処理部を第１チップに設け、その他の演算部および制御部を第２チップに設けることにより、処理の負荷を第１チップと第２チップに適正に分散させることができる。これによって、容易に、機能を拡張することができる。

本発明のロボットシステムでは、前記アーム、前記駆動源、前記角速度センサーおよび前記位置センサーをそれぞれ２つ有し、
さらに基台を有し、
一方の前記アームは、前記基台に対し、第１回転軸を回転中心として回動自在に連結され、他方の前記アームは、一方の前記アームに対し、前記第１回転軸と直交または前記第１回転軸に直交する軸と平行な第２回転軸を回転中心として回動自在に連結されており、
前記第１チップおよび前記第２チップは、２つの前記アームのうちの少なくとも一方の制御用に設けられていることが好ましい。
これにより、前記本発明の効果を有し、２つのアームを有するロボットを備えるロボットシステムを提供することができる。

本発明のロボットシステムの第１実施形態であって、そのロボットを正面側から見た斜視図である。 図１に示すロボットシステムであって、そのロボットを背面側から見た斜視図である。 図１に示すロボットシステムのロボットの概略図である。 図１に示すロボットシステムのロボットの概略図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムのロボット制御装置の第１チップおよび第２チップを示すブロック図である。 図１に示すロボットシステムのロボット制御装置の第１チップおよび第２チップの制御動作を示す図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムの主要部のブロック図である。 図１に示すロボットシステムの主要部のブロック図である。 本発明のロボットシステムの第２実施形態であって、そのロボットを正面側から見た斜視図である。

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

なお、以下では、説明の都合上、図１〜図４中の上側を「上」または「上方」、下側を「下」または「下方」と言う。また、図１〜図４中の基台側を「基端」、その反対側を「先端」と言う。また、図１および図２では、ロボット制御装置２０は、それぞれ、ブロック図で示されている。また、図４では、回転軸Ｏ２、Ｏ３は、それぞれ、誇張して図示されている。また、図４では、角速度センサー３１、３２は、それぞれ、その存在を明確にするため、アーム１２、１３の外部に図示されている。

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

ロボット１は、基台１１と、４本のアーム（リンク）１２、１３、１４、１５と、リスト（リンク）１６と、６つの駆動源４０１、４０２、４０３、４０４、４０５、４０６とを備えている。このロボット１は、基台１１と、アーム１２、１３、１４、１５と、リスト１６とが基端側から先端側に向ってこの順に連結された垂直多関節（６軸）ロボット（ロボット本体）である。垂直多関節ロボットでは、基台１１と、アーム１２〜１５と、リスト１６とを総称して「アーム」と言うこともでき、アーム１２を「第１アーム」、アーム１３を「第２アーム」、アーム１４を「第３アーム」、アーム１５を「第４アーム」、リスト１６を「第５アーム、第６アーム」と分けて言うことができる。なお、本実施形態では、リスト１６は、第５アームと、第６アームとを有している。リスト１６にはエンドエフェクタ等を取り付けることができる。
アーム１２〜１５、リスト１６は、それぞれ、基台１１に対し独立して変位可能に支持されている。このアーム１２〜１５、リスト１６の長さは、それぞれ、特に限定されないが、図示の構成では、第１アーム１２、第２アーム１３、第４アーム１５の長さが、第３アーム１４およびリスト１６よりも長く設定されている。

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

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

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

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

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

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

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

ここで、このロボット１では、第１アーム１２および第２アーム１３の振動を抑制するために、第１アーム１２に第１角速度センサー３１を設置し、その第１角速度センサー３１の検出結果に基づいて第１駆動源４０１の作動を制御し、第２アーム１３に第２角速度センサー３２を設置し、その第２角速度センサー３２の検出結果に基づいて第２駆動源４０２の作動を制御する。これにより、確実に、第１アーム１２および第２アーム１３の振動を抑制することができ、これによって、ロボット１全体の振動を抑制することができる。
なお、ロボット１のアーム１２〜１５、リスト１６の振動の主な原因としては、例えば、減速機のねじれや撓み、アーム１２〜１５、リスト１６の撓み等が挙げられる。

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

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

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

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

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

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

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

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

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

支持リング１６２は、関節１７５を介して、第４アーム１５の駆動機構３ｄに連結されており、当該駆動機構３ｄのモーター４０５Ｍの駆動により、リスト本体１６１ごと回転軸Ｏ５回りに回動する。
アーム本体２の構成材料としては、特に限定されず、例えば、各種金属材料を用いることができ、これらの中でも、特にアルミニウムまたはアルミニウム合金が好ましい。アーム本体２が金型を用いて成形される鋳物である場合、当該アーム本体２の構成材料にアルミニウムまたはアルミニウム合金を用いることにより、金型成形を容易に行なうことができる。

また、基台１１の基台本体１１２、リスト１６のリスト本体１６１、支持リング１６２の構成材料としては、それぞれ、特に限定されず、例えば、前記アーム本体２の構成材料と同様のもの等が挙げられる。なお、リスト１６のリスト本体１６１の構成材料は、ステンレス鋼を用いるのが好ましい。
また、封止手段４の構成材料としては、特に限定されず、例えば、各種樹脂材料、各種金属材料を用いることができる。なお、封止手段４の構成材料として、樹脂材料を用いることにより、軽量化を図ることができる。

次に、図５、図６、図８〜図１３を参照し、ロボット制御装置２０の構成について説明する。
図５、図８〜図１３に示すように、ロボット制御装置２０は、第１駆動源４０１の作動を制御する第１駆動源制御部２０１と、第２駆動源４０２の作動を制御する第２駆動源制御部２０２と、第３駆動源４０３の作動を制御する第３駆動源制御部２０３と、第４駆動源４０４の作動を制御する第４駆動源制御部２０４と、第５駆動源４０５の作動を制御する第５駆動源制御部２０５と、第６駆動源４０６の作動を制御する第６駆動源制御部２０６とを有している。

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

図６に示すように、第１チップ２１１は、第１角速度センサー３１から出力される信号および第１位置センサー４１１から出力される信号をそれぞれ受信する第１受信部２１１１と、第１角速度センサー３１から出力され、第１受信部２１１１で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理部２１１２とを有している。そして、第１駆動源制御部２０１の他の各回路部は、それぞれ、第２チップ２２１に設けられている。

すなわち、第２チップ２２１は、第１フィルタリング処理部２１１２から出力される信号を受信するとともに、第１受信部２１１１を介して第１位置センサー４１１から出力される信号を受信する第２受信部２２１１と、第２受信部２２１１で受信した信号に基づいて、第１アーム１２の角速度の振動成分（振動角速度）を求める演算部２２１２と、演算部２２１２により第１アーム１２の角速度の振動成分を求めた後、その振動成分に対応する信号から所定周波数より低い周波数の信号を除去する第２フィルタリング処理部２２１３と、第２フィルタリング処理部２２１３から出力され、演算部２２１２で求められた第２アーム１３の角速度の振動成分に基づいて第１駆動源４０１の作動を制御する制御部２２１４とを有している。
なお、図６と図８とでは、第１駆動源制御部２０１の各機能部の区分けが異なるので、前記のように異なる名称が付されている。

以下、第１チップ２１１および第２チップ２２１の動作を簡単に説明する。なお、詳細な説明は、第１駆動源制御部２０１の動作として後述する。
まず、第１角速度センサー３１から出力された信号は、その第１角速度センサー３１の近傍に設置された回路部のＡＤ変換部（図示せず）によりＡＤ変換、すなわち、アナログ信号がデジタル信号に変換され、前記回路部の送信部（図示せず）によりロボット制御装置２０の第１駆動源制御部２０１に送信される。

図７に示すように、前記第１角速度センサー３１から出力され、デジタル信号に変換された信号は、第１チップ２１１の第１受信部２１１１で受信され、第１フィルタリング処理部２１１２において、前記信号に対し、特定周波数帯域の信号を除去する第１フィルタリング処理が行われる。なお、この第１フィルタリング処理については、後述する。
そして、第１フィルタリング処理後の信号は、第１チップ２１１の第１フィルタリング処理部２１１２から出力され、第２チップ２２１の第２受信部２２１１で受信される。また、第１位置センサー４１１から出力された信号は、第１チップ２１１の第１受信部２１１１で受信され、その第１受信部２１１１から第２チップ２２１に出力され、第２チップ２２１の第２受信部２２１１で受信される。

演算部２２１２では、第２受信部２２１１で受信した信号に基づいて、第１アーム１２の角速度の振動成分を求め、この振動成分に対応する信号は、第２フィルタリング処理部２２１３に出力される。
そして、第２フィルタリング処理部２２１３において、前記信号に対し、所定周波数より低い周波数の信号を除去する第２フィルタリング処理を行う。なお、この第２フィルタリング処理については、後述する。

第２フィルタリング処理後の信号は、第２フィルタリング処理部２２１３から制御部２２１４に出力され、制御部２２１４において、前記第１アーム１２の角速度の振動成分に基づいて第１駆動源４０１の作動が制御される。この場合、前記第１アーム１２の角速度の振動成分に基づいて後述する角速度フィードバック値が求められ、その角速度フィードバック値をフィードバックすることで、第１駆動源４０１の作動を制御する。

図９に示すように、第２駆動源制御部２０２は、減算器５１２と、位置制御部５２２と、減算器５３２と、角速度制御部５４２と、回転角度算出部５５２と、角速度算出部５６２と、減算器５７２と、変換部５８２と、補正値算出部５９２と、加算器６０２とを有している。
図５に示すように、この第２駆動源制御部２０２は、第１チップ２１２と、第２チップ２２２とで構成されている。なお、第１チップ２１２および第２チップ２２２は、それぞれ、前記第１駆動源制御部２０１の第１チップ２１１および第２チップ２２１と同様であるので、その説明は省略する。

図１０に示すように、第３駆動源制御部２０３は、減算器５１３と、位置制御部５２３と、減算器５３３と、角速度制御部５４３と、回転角度算出部５５３と、角速度算出部５６３とを有している。図５に示すように、この第３駆動源制御部２０３は、第１チップ２１３と、第２チップ２２３とで構成されている。第１チップ２１３は、第３位置センサー４１３から出力される信号を受信する図示しない受信部のみを有し、他の各回路部は、それぞれ、第２チップ２２３に設けられている。

図１１に示すように、第４駆動源制御部２０４は、減算器５１４と、位置制御部５２４と、減算器５３４と、角速度制御部５４４と、回転角度算出部５５４と、角速度算出部５６４とを有している。図５に示すように、この第４駆動源制御部２０４は、第１チップ２１４と、第２チップ２２４とで構成されている。第１チップ２１４は、第４位置センサー４１４から出力される信号を受信する図示しない受信部のみを有し、他の各回路部は、それぞれ、第２チップ２２４に設けられている。

図１２に示すように、第５駆動源制御部２０５は、減算器５１５と、位置制御部５２５と、減算器５３５と、角速度制御部５４５と、回転角度算出部５５５と、角速度算出部５６５とを有している。図５に示すように、この第５駆動源制御部２０５は、第１チップ２１５と、第２チップ２２５とで構成されている。第１チップ２１５は、第５位置センサー４１５から出力される信号を受信する図示しない受信部のみを有し、他の各回路部は、それぞれ、第２チップ２２５に設けられている。

図１３に示すように、第６駆動源制御部２０６は、減算器５１６と、位置制御部５２６と、減算器５３６と、角速度制御部５４６と、回転角度算出部５５６と、角速度算出部５６６とを有している。図５に示すように、この第６駆動源制御部２０６は、第１チップ２１６と、第２チップ２２６とで構成されている。第１チップ２１６は、第６位置センサー４１６から出力される信号を受信する図示しない受信部のみを有し、他の各回路部は、それぞれ、第２チップ２２６に設けられている。
なお、第１チップ２１１〜２１６としては、それぞれ、特に限定されないが、例えば、ＦＰＧＡ（フィールドプログラマブルゲートアレイ）等が挙げられる。
また、第２チップ２２１〜２２６としては、それぞれ、特に限定されないが、例えば、ＤＳＰ（デジタルシグナルプロセッサー）等が挙げられる。

次に、第１フィルタリング処理部２１１２について説明する。
第１フィルタリング処理部２１１２は、第１受信部２１１１で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理を行うものである。
この特定周波数帯域の下限値としては、特に限定されないが、２００Ｈｚ以上、８００Ｈｚ以下の範囲内に設定されることが好ましく、４００Ｈｚ以上、６００Ｈｚ以下の範囲内に設定されることがより好ましい。

また、特定周波数帯域の上限値としては、特に限定されないが、８００Ｈｚ以上、１４００Ｈｚ以下の範囲内に設定されることが好ましく、１０００Ｈｚ以上、１２００Ｈｚ以下の範囲内に設定されることがより好ましい。
また、特定周波数帯域の周波数の幅は、特に限定されないが、３００Ｈｚ以上、９００Ｈｚ以下の範囲内に設定されることが好ましく、５００Ｈｚ以上、７００Ｈｚ以下の範囲内に設定されることがより好ましい。

これにより、第１角速度センサー３１で発生するノイズを除去することができる。
この場合、第１アーム１２回動している際に第１角速度センサー３１で発生するノイズの周波数は、ほぼ決まっているので、前記のように特定周波数帯域を設定することにより、第１角速度センサー３１で発生するノイズを確実に除去することができる。
また、第１角速度センサー３１から出力され、特定周波数帯域の上限値よりも高い周波数および下限値よりも低い周波数の信号は、第１アーム１２の角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。

また、第１フィルタリング処理部２１１２における第１フィルタリング処理としては、前記特定周波数帯域の信号を除去する処理であれば、特に限定されないが、３次以上のフィルタリング処理であることが好ましく、３次のフィルタリング処理であることがより好ましい。なお、１次のフィルタリング処理でもよく、また、２次のフィルタリング処理でもよいことは、言うまでもない。

ここで、特定周波数帯域の信号を除去する１次のフィルタリング処理では、周期的に各時刻ｔが設定されており、時刻ｔ（ｎ）における信号から特定周波数帯域の信号を除去する場合、その時刻ｔ（ｎ）における信号のみを用いて処理を行う。
これに対して、３次のフィルタリング処理では、時刻ｔ（ｎ）における信号から特定周波数帯域の信号を除去する場合、その時刻ｔ（ｎ）における信号と、時刻ｔ（ｎ）よりも１つ前の時刻ｔ（ｎ−１）における信号と、時刻ｔ（ｎ）よりも２つ前の時刻ｔ（ｎ−２）における信号とを用いて処理を行う。

前記ｎは、整数であり、何番目の時刻であるかを示す。なお、４次以上のフィルタリング処理では、さらに、１つずつ、前の時刻の信号が追加され、処理がなされる。
このような３次以上の第１フィルタリング処理を行うことにより、カットオフ周波数付近での信号の利得を向上させつつ、特定周波数帯域の信号をより確実に除去することができる。

また、３次以上の第１フィルタリング処理は、処理時間が比較的長くかかるので、第２チップ２２１ではなく、第１チップ２１１に第１フィルタリング処理部２１１２を設けることにより、第２チップ２２１における処理の負荷を大幅に軽減することができ、処理の負荷を適正に分散させることができる。
なお、第１フィルタリング処理部２１１２としては、例えば、各種のバンドエリミネーションフィルター（帯域阻止フィルター）を用いることができる。

次に、第２フィルタリング処理部２２１３について説明する。
第２フィルタリング処理部２２１３は、演算部２２１２により第１アーム１２の角速度の振動成分を求めた後、その振動成分に対応する信号から所定周波数より低い周波数の信号を除去する第２フィルタリング処理を行うものである。
前記所定周波数としては、特に限定されないが、２Ｈｚ以上、２０Ｈｚ以下の範囲内に設定されることが好ましく、５Ｈｚ以上、１０Ｈｚ以下の範囲内に設定されることがより好ましい。

演算部２２１２では、第１アーム１２の角速度の振動成分を求める際、例えば、第１角速度センサー３１のオフセット成分等の影響により、その振動成分に対応する信号としては存在しない周波数のノイズが発生する。このため、その振動成分を求めた後、第２フィルタリング処理部２２１３により所定周波数より低い周波数の信号を除去することにより、前記ノイズを除去することができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

以上説明したように、このロボットシステム１０では、ロボット１の振動を抑制することができる。
また、第１駆動源制御部２０２の第１フィルタリング処理部２１１２により、第１角速度センサー３１で発生するノイズを除去することができ、同様に、第２駆動源制御部２０２の第１フィルタリング処理部により、第２角速度センサー３２で発生するノイズを除去することができ、より確実に、ロボット１の振動を抑制することができる。
また、第１角速度センサー３１から出力され、特定周波数帯域の上限値よりも高い周波数および下限値よりも低い周波数の信号は、第１アーム１２の角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。

同様に、第２角速度センサー３２から出力され、特定周波数帯域の上限値よりも高い周波数の信号および下限値よりも低い周波数の信号は、第２アーム１３の角速度の振動成分を求める際に用いられ、また、下限値よりも低い周波数の信号は、他の制御に利用することができる。
また、第１駆動源制御部２０１として、第１チップ２１１と第２チップ２２１とを設け、かつ、処理時間が比較的長い処理を行う第１フィルタリング処理部２１１２を第１チップ２１１に設け、その他の演算部２２１２、第２フィルタリング処理部２２１３および制御部２２１４を第２チップ２２１に設けることにより、処理の負荷を第１チップ２１１と第２チップ２２１に適正に分散させることができる。これによって、容易に、機能を拡張することができる。

同様に、第２駆動源制御部２０２として、第１チップ２１２と第２チップ２２２とを設け、かつ、処理時間が比較的長い処理を行う第１フィルタリング処理部を第１チップ２１２に設け、その他の演算部、第２フィルタリング処理部および制御部を第２チップ２２２に設けることにより、処理の負荷を第１チップ２１２と第２チップ２２２に適正に分散させることができる。これによって、容易に、機能を拡張することができる。
また、ロボット１の制御において、複雑で膨大な演算が不要であり、これによって、演算誤差が生じ難く、振動を確実に抑制することができ、また、ロボット１の制御における応答速度を速くすることができる。

＜第２実施形態＞
図１４は、本発明のロボットシステムの第２実施形態であって、そのロボットを正面側から見た斜視図である。図１４では、ロボット制御装置２０は、ブロック図で示されている。
以下、第２実施形態について、前述した第１実施形態との相違点を中心に説明し、同様の事項については、その説明を省略する。

図１４に示すように、第２実施形態のロボットシステム１０では、ロボット制御装置２０は、ロボット１に内蔵されている。ロボット１におけるロボット制御装置２０の位置は、特に限定されないが、図示の構成では、ロボット制御装置２０は、基台１１の基台本体１１２内に設置されている。
このロボットシステム１０によれば、前述した第１実施形態と同様の効果が得られる。
なお、ロボット制御装置２０は、ロボット１の外面、例えば、基台１１の外面に設置されていてもよい。

以上、本発明のロボット制御装置、ロボットおよびロボットシステムを、図示の実施形態に基づいて説明したが、本発明はこれに限定されるものではなく、各部の構成は、同様の機能を有する任意の構成のものに置換することができる。また、本発明に、他の任意の構成物が付加されていてもよい。
なお、各駆動源のモーターとしては、それぞれ、前記サーボモーターの他、例えば、ステッピングモーター等が挙げられる。また、モーターとしてステッピングモーターを用いる場合は、位置センサーとして、例えば、ステッピングモーターへ入力する駆動パルスの数を計測することで、モーターの回転角度を検出するものを用いてもよい。

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

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

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

Claims (9)

1. 回動可能なアームと、前記アームを回動させる駆動源と、前記アームに設置された角速度センサーと、前記駆動源の回転角度を検出する位置センサーとを有するロボットの制御を行うロボット制御装置であって、
   前記角速度センサーから出力される信号および前記位置センサーから出力される信号をそれぞれ受信する第１受信部と、前記角速度センサーから出力され、前記第１受信部で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理部とを有する第１チップと、
   前記第１フィルタリング処理部から出力される信号を受信し、前記第１受信部を介して前記位置センサーから出力される信号を受信する第２受信部と、前記第２受信部で受信した信号に基づいて、前記アームの角速度の振動成分を求める演算部と、前記演算部で求められた前記アームの角速度の振動成分に基づいて、前記駆動源の作動を制御する制御部とを有する第２チップとを備えることを特徴とするロボット制御装置。
2. 前記第１フィルタリング処理部は、時刻ｔ（ｎ）における信号から前記特定周波数帯域の信号を除去する場合、少なくとも前記時刻ｔ（ｎ）における信号と、前記時刻ｔ（ｎ）よりも１つ前の時刻ｔ（ｎ−１）における信号と、前記時刻ｔ（ｎ）よりも２つ前の時刻ｔ（ｎ−２）における信号とを用いて処理を行うよう構成されている請求項１に記載のロボット制御装置。
3. 前記第２チップは、前記演算部により前記アームの角速度の振動成分を求めた後、前記振動成分に対応する信号から所定周波数より低い周波数の信号を除去する第２フィルタリング処理部を有する請求項１または２に記載のロボット制御装置。
4. 前記演算部は、前記位置センサーから出力される信号に基づいて前記アームの角速度を求め、前記アームの角速度と、前記角速度センサーから出力される信号から得た前記アームの角速度との差分を求め、前記差分を前記アームの角速度の振動成分とする請求項１ないし３のいずれかに記載のロボット制御装置。
5. 前記特定周波数帯域の周波数の幅は、３００Ｈｚ以上、９００Ｈｚ以下の範囲内に設定される請求項１ないし４のいずれかに記載のロボット制御装置。
6. 回動可能なアームと、
   前記アームを回動させる駆動源と、
   前記アームに設置された角速度センサーと、
   前記駆動源の回転角度を検出する位置センサーと、
   前記角速度センサーから出力される信号および前記位置センサーから出力される信号をそれぞれ受信する第１受信部と、前記角速度センサーから出力され、前記第１受信部で受信した信号から０よりも大きい周波数の下限値および周波数の上限値を有する特定周波数帯域の信号を除去する第１フィルタリング処理部とを有する第１チップと、
   前記第１フィルタリング処理部から出力される信号を受信し、前記第１受信部を介して前記位置センサーから出力される信号を受信する第２受信部と、前記第２受信部で受信した信号に基づいて、前記アームの角速度の振動成分を求める演算部と、前記演算部で求められた前記アームの角速度の振動成分に基づいて、前記駆動源の作動を制御する制御部とを有する第２チップとを備えることを特徴とするロボット。
7. 前記アーム、前記駆動源、前記角速度センサーおよび前記位置センサーをそれぞれ２つ有し、
   さらに基台を有し、
   一方の前記アームは、前記基台に対し、第１回転軸を回転中心として回動自在に連結され、他方の前記アームは、一方の前記アームに対し、前記第１回転軸と直交または前記第１回転軸に直交する軸と平行な第２回転軸を回転中心として回動自在に連結されており、
   前記第１チップおよび前記第２チップは、２つの前記アームのうちの少なくとも一方の制御用に設けられている請求項６に記載のロボット。
8. 回動可能なアームと、前記アームを回動させる駆動源と、前記アームに設置された角速度センサーと、前記駆動源の回転角度を検出する位置センサーとを有するロボットと、
   前記ロボットの制御を行う請求項１ないし５のいずれかに記載のロボット制御装置とを備えることを特徴とするロボットシステム。
9. 前記アーム、前記駆動源、前記角速度センサーおよび前記位置センサーをそれぞれ２つ有し、
   さらに基台を有し、
   一方の前記アームは、前記基台に対し、第１回転軸を回転中心として回動自在に連結され、他方の前記アームは、一方の前記アームに対し、前記第１回転軸と直交または前記第１回転軸に直交する軸と平行な第２回転軸を回転中心として回動自在に連結されており、
   前記第１チップおよび前記第２チップは、２つの前記アームのうちの少なくとも一方の制御用に設けられている請求項８に記載のロボットシステム。

Images