水平多関節ロボット及び水平多関節ロボットの制御方法
【課題】制振制御に用いられる角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線に要求される耐久性を低くした水平多関節ロボット及び水平多関節ロボットの制御方法を提供する。
【解決手段】ロボットは、基台に連結される第１水平アームと、第１水平アームを介して前記基台に連結される第２水平アーム１５と、各アームを回転させる第１及び第２モーター１３，１６と、各モーターの回転角度及び回転速度を算出するための第１及び第２エンコーダー１３Ｅ，１６Ｅとを有している。第１モーター制御部４３は、角速度センサー３０が検出したセンサー角速度ωＡ２から第１及び第２エンコーダー１３Ｅ，１６Ｅに基づく第１及び第２角速度ωＡ１ｍ，ωＡ２ｍを減算し、演算結果である振動角速度ωＡ１ｓに基づく振動速度Ｖ１ｓと第１回転速度Ｖ１ｆｂとの加算した速度計測値が速度指令Ｖｃとなるように第１モーター１３を制御する。
【解決手段】ロボットは、基台に連結される第１水平アームと、第１水平アームを介して前記基台に連結される第２水平アーム１５と、各アームを回転させる第１及び第２モーター１３，１６と、各モーターの回転角度及び回転速度を算出するための第１及び第２エンコーダー１３Ｅ，１６Ｅとを有している。第１モーター制御部４３は、角速度センサー３０が検出したセンサー角速度ωＡ２から第１及び第２エンコーダー１３Ｅ，１６Ｅに基づく第１及び第２角速度ωＡ１ｍ，ωＡ２ｍを減算し、演算結果である振動角速度ωＡ１ｓに基づく振動速度Ｖ１ｓと第１回転速度Ｖ１ｆｂとの加算した速度計測値が速度指令Ｖｃとなるように第１モーター１３を制御する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、角速度センサーを備えた水平多関節ロボット、及び水平多関節ロボットの制御方法に関する。
【背景技術】
【０００２】
従来から、特許文献１に記載のように、アームの角速度を検出する角速度センサーを用いて該アームを制振制御する水平多関節ロボットが知られている。特許文献１に記載の水平多関節ロボットの基台には、基台に対して回転可能に第１アームが連結され、また、該第１アームの先端には、第１アームに対して回転可能に第２アームが連結されている。
【０００３】
そして、上記第１アームが第１駆動源の駆動力を受けて回転する際には、第１アームの回転角度が、第１駆動源の回転角を検出する第１角度センサーによって検出され、また、基体に対する第１アームの角速度が、第１アームに搭載された第１角速度センサーによって検出される。この際、第１駆動源の駆動態様を制御するロボットコントローラでは、第１角度センサー及び第１角速度センサーが検出したデータに基づいて、該検出されるデータが所定の値となるように、第１駆動源に対するフィードバック制御が実行される。
【０００４】
また、上記第２アームが第２駆動源の駆動力を受けて回転する際には、第２アームの回転角度が、第２駆動源の回転角を検出する第２角度センサーによって検出され、また、基体に対する第２アームの角速度が、第２アームに搭載された第２角速度センサーによって検出される。この際、上述したロボットコントローラでは、第１駆動源に対する制御の態様と同様に、第２角度センサー及び第２角速度センサーが検出したデータに基づき、該検出されるデータが所定の値となるように、第２駆動源に対するフィードバック制御が実行される。これにより、第１及び第２アームの制振制御が行われている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−２４２７９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで、上述した各角速度センサーを駆動するためには、角速度センサーに電源を供給する配線や角速度センサーの検出信号を送信する配線など、角速度センサーとコントローラーとの間に各種の電気配線を接続することが必要になる。こうした電気配線は、角速度センサーの他、駆動源にも必要とされるものであって、通常、中空の基体を通して外部のコントローラーに接続されている。そのため、上述した水平多関節ロボットのように、第１アーム及び第２アームに各別に角速度センサーが配置されている場合には、角速度センサーの数量と同じ数量の電気配線が必要になる。その結果、角速度センサー用の電気配線を引き回す作業が煩雑なものになる。
【０００７】
一方、第１アームに生じた振動は、通常、第１アームに連結された他のアームによって増幅されて、その後、水平多関節ロボットのエンドエフェクターに伝わる。そのため、上述した第２角度センサー及び第２角速度センサーを割愛するとともに、第１角速度センサーの検出結果に基づく第１駆動源の制振制御のみを行うことによって、エンドエフェクターの振動を抑えることも可能ではある。しかし、第１アームが基体に対して描く軌道には、通常、他のアームが基体に対して描く軌道と比較して、小さい曲率の軌道が多く含まれる。そのため、第１アームから引き出される電気配線に対しては、他のアームから引き出
される電気配線と比較して、折り曲げられる機会が多くなり、また折り曲げ箇所における曲率が小さくもなる。それゆえに、第１角速度センサーに接続される電気配線には、第２角速度センサーに接続される電気配線よりも高い耐久性が求められるため、結局のところ、こうした電気配線の配置に関わる制約や同電気配線を引き回すことの煩雑さが依然として残ることになる。
【０００８】
本発明は、上記実情を鑑みてなされたものであり、その目的は、制振制御に用いられる角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線に要求される耐久性を低くした水平多関節ロボット及び水平多関節ロボットの制御方法を提供することにある。
【課題を解決するための手段】
【０００９】
本発明の水平多関節ロボットは、少なくとも関節を介して基台に連結される基端を有して第１モーターの駆動により回転する第１アームと、前記第１モーターの回転速度を計測する第１速度計測部と、前記第１速度計測部の計測値に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する制御部とを備える水平多関節ロボットであって、前記第１アームの先端に連結されて第２モーターの駆動により回転する第２アームと、前記第２モーターの回転速度を計測する第２速度計測部と、前記第１モーターの回転速度から前記第１アームの角速度を算出し、且つ前記第２モーターの回転速度から前記第２アームの角速度を算出する角速度算出部と、前記第２アームに配置されて角速度を検出する角速度センサーとを備え、前記制御部は、前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記第１速度計測部の計測値と前記振動速度との加算値を前記速度計測値とする。
【００１０】
上述した発明の水平多関節ロボットに関し、角速度センサーによって検出される角速度は、対応するモーターの回転速度に基づく各アームの角速度と振動に基づく各アームの角速度とを含む角速度である。そのため、対応するモーターの回転速度に基づくアームの角速度をアームごとに求め、その求めた角速度を角速度センサーが検出した角速度から減算することによって、振動によるアームの角速度である振動角速度を求めることが可能である。上記構成の水平多関節ロボットでは、第１及び第２速度計測部の計測値に基づいて第１及び第２アームの角速度が算出され、これらの角速度が角速度センサーの検出値から減算されることで振動角速度が算出される。そして、該振動角速度に応じた第１モーターの回転速度である振動速度と第１速度計測部の計測値との加算値である速度計測値が、速度指令値となるように第１モーターが制御される。すなわち、アームの振動の分をも加味した第１モーターの回転速度が速度指令値となるように第１モーターが制御されることから、第１アームの振動を抑えることができる。
【００１１】
すなわち、第１アームに角速度センサーを配置する必要がなくなることから、配置される角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線の数を低減することができる。その結果、角速度センサーに関わる電気配線を引き回す作業が煩雑になることを抑えることができる。そのうえ、角速度センサーが第２アームに配置されていることから、第１アームに配置される角速度センサーに接続される電気配線に比べて、折り曲げられる機会が少なくなるとともに折り曲げ箇所における曲率も大きくなる。その結果、角速度センサーに接続される電気配線に要求される耐久性を低くすることもできる。
【００１２】
この水平多関節ロボットは、前記第１モーターの回転角度を検出する第１位置検出部を備え、前記制御部は、前記第１位置検出部の検出値と位置指令値との偏差から前記速度指令値を算出する。
【００１３】
この水平多関節ロボットによれば、速度指令値が第１位置検出部の検出値と位置指令値との偏差であることから、第１アームの振動を抑えつつ、該第１アームの位置を位置指令値が示す位置に制御することができる。
【００１４】
この水平多関節ロボットは、前記第２モーターの回転角度を検出する第２位置検出部を備え、前記第１速度計測部は、前記第１位置検出部の検出値から前記第１モーターの回転角度を算出し、前記第２速度計測部は、前記第２位置検出部の検出値から前記第２モーターの回転角度を算出する。
【００１５】
この水平多関節ロボットのように、第１速度計測部は、第１位置検出部の検出値から第１モーターの回転角度を算出することができる。また、第２速度計測部は、第２位置検出部の検出値から第２モーターの回転速度を算出することができる。
【００１６】
この水平多関節ロボットにおいて、前記第１アームは、関節を介して基台に連結されたアームである。
この水平多関節ロボットによれば、基台に連結されているアームを制振制御することができる。
【００１７】
本発明の水平多関節ロボットの制御方法は、少なくとも関節を介して基台に連結される基端を有する第１アームを回転させる第１モーターの回転速度を計測し、その計測された回転速度に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する水平多関節ロボットの制御方法であって、前記第１アームの先端に連結された第２アームを回転させる第２モーターの回転速度を計測するステップと、前記計測された第１モーターの回転速度から前記第１アームの角速度を算出するステップと、前記計測された第２モーターの回転速度から前記第２アームの角速度を算出するステップと、前記第２アームに配置された角速度センサーの検出値を取得するステップとを備え、前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記計測された第１モーターの回転速度と前記振動速度との加算値を前記速度計測値とする。
【００１８】
本発明の水平多関節ロボットの制御方法によれば、第２アームに配置された角速度センサーの検出値に基づいて第１アームを制振制御することができることから、配置される角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線の数を低減することができる。その結果、角速度センサーに関わる電気配線を引き回す作業が煩雑になることを抑えることができる。そのうえ、角速度センサーが第２アームに配置されていることから、第１アームに配置される角速度センサーに接続される電気配線に比べて、折り曲げられる機会が少なくなるとともに折り曲げ箇所における曲率も大きくなる。その結果、角速度センサーに接続される電気配線に要求される耐久性を低くすることもできる。
【図面の簡単な説明】
【００１９】
【図１】本発明の一実施の形態にかかるロボットの正面構造を示す正面図。
【図２】ロボットの電気的構成を示す電気ブロック回路図。
【図３】第１モーター制御部の機能的な構成を示す機能ブロック図。
【発明を実施するための形態】
【００２０】
以下、本発明にかかる水平多関節ロボットの一実施の形態について、図１〜図３を参照して説明する。
図１に示されるように、ロボット１０における基台１１の上端部には、鉛直方向に沿う
軸心Ｃ１を中心にして、基台１１に対して回転する第１アームとしての第１水平アーム１２の基端部が連結されている。基台１１内には、第１水平アーム１２を回転させる第１モーター１３と、該第１モーターの回転軸１３ａに連結され、出力軸１４ａが第１水平アーム１２に連結固定された第１減速機１４とが設置されている。そして、第１モーター１３の駆動力が第１減速機１４を介して第１水平アーム１２に伝達されると、第１水平アーム１２は、基台１１に対して水平面内で回転する。また、第１モーター１３には、該第１モーター１３の回転量に応じたパルス信号を出力する第１位置検出部としての第１エンコーダー１３Ｅが設けられている。
【００２１】
第１水平アーム１２の先端部には、鉛直方向に沿う軸心Ｃ２を中心にして第１水平アーム１２に対して回転する第２アームとしての第２水平アーム１５が連結されている。第２水平アーム１５を構成するアーム本体１５ａの内部には、第２水平アーム１５を回転させる第２モーター１６と、該第２モーター１６の回転軸１６ａに連結されて出力軸１７ａが第１水平アーム１２に連結固定された第２減速機１７とが設置されている。そして、第２モーター１６の駆動力が第２減速機１７を介して第２水平アーム１５に伝達されると、第２水平アーム１５は、軸心Ｃ２を中心にして第１水平アーム１２に対して水平面内で回転する。また、第２モーター１６には、該第２モーター１６の回転量に応じたパルス信号を出力する第２位置検出部としての第２エンコーダー１６Ｅが設けられている。こうしたアーム本体１５ａの上側は、その全体がアームカバー１８によって覆われている。
【００２２】
第２水平アーム１５の先端部には、アーム本体１５ａとアームカバー１８とを貫通し、第２水平アーム１５に対して変位するスプラインシャフト１９が設けられている。スプラインシャフト１９は、第２水平アーム１５の先端部に配置されたスプラインナット１９Ｓに嵌め合わされるように挿通され、第２水平アーム１５に対して回転可能に、かつ、上下方向に移動可能に支持されている。
【００２３】
第２水平アーム１５内には、回転モーター２０が設置され、回転モーター２０の駆動力は、図示しないベルトを介してスプラインナット１９Ｓに伝達される。回転モーター２０がスプラインナット１９Ｓを正逆回転すると、スプラインシャフト１９は鉛直方向に沿う軸心Ｃ３を中心にして正逆回転する。回転モーター２０には、該回転モーター２０の回転量に応じたパルス信号を出力する第３エンコーダー２０Ｅが設けられている。
【００２４】
第２水平アーム１５内には、ボールねじナット１９Ｂを正逆回転する昇降モーター２１が設置されている。昇降モーター２１がボールねじナット１９Ｂを正逆回転すると、スプラインシャフト１９は鉛直方向に昇降する。昇降モーター２１には、該昇降モーター２１の回転量に応じたパルス信号を出力する第４エンコーダー２１Ｅが設けられている。スプラインシャフト１９の下端に連結された作業部２５には、例えば被搬送物を把持するものや被加工物を加工するもの等の取り付けが可能になっている。
【００２５】
また、第２水平アーム１５内には、基台１１に対する第２水平アーム１５の角速度を測定する角速度センサー３０が配設されている。角速度センサー３０は、本実施形態では、水晶型振動子を用いた振動型のジャイロスコープを採用している。第２水平アーム１５における上側には、他端が基台１１に連結された配管部材として可撓性を有する配線ダクト３３の一端が連結されている。第２モーター１６や第２エンコーダー１６Ｅ、回転モーター２０、昇降モーター２１等、第２水平アーム１５内に設置されている各機器に接続される電気配線は、配線ダクト３３内を通じて第２水平アーム１５内から基台１１内まで引き回される。
【００２６】
そして、第２水平アーム１５内から基台１１内まで引き回された各電気配線は、基台１１内でまとめられることによって、上記第１モーター１３及び第１エンコーダー１３Ｅに
接続される電気配線とともに、基台１１の外部に設置されロボット１０を統括制御する制御装置４０まで引き回される。制御装置４０は、角速度センサー３０等から入力される各種信号に基づいて第１水平アーム１２の角速度を演算し、該第２水平アーム１５の振動が抑制されるように第１モーター１３を制御する。
【００２７】
次に、制御装置４０の電気的構成について図２を参照して説明する。
図２に示されるように、制御装置４０の位置指令生成部４１は、ロボット１０が行う処理の内容に基づいて作業部２５の目標位置を演算し、該目標位置に作業部２５を移動させるための軌道を生成する。また、位置指令生成部４１は、該軌道に沿って作業部２５が移動するように、各モーター１３，１６，２０，２１の回転角度を所定の制御周期ごとに演算し、該演算の結果である目標回転角度を第１位置指令Ｐｃ、第２位置指令Ｐｃ２、第３位置指令Ｐｃ３、第４位置指令Ｐｃ４としてモーター制御部４２に出力する。
【００２８】
モーター制御部４２は、第１モーター制御部４３、第２モーター制御部４４、回転モーター制御部４５、及び昇降モーター制御部４６によって構成されている。
第１モーター制御部４３には、位置指令値である第１位置指令Ｐｃの他、第１エンコーダー１３Ｅ、第２エンコーダー１６Ｅ、及び角速度センサー３０の各々から検出信号が入力される。第１モーター制御部４３は、第１エンコーダー１３Ｅの検出信号から算出される回転角度が第１位置指令Ｐｃになるように、各検出信号を用いたフィードバック制御によって第１モーター１３を駆動する。
【００２９】
第２モーター制御部４４には、第２位置指令Ｐｃ２の他、第２エンコーダー１６Ｅから検出信号が入力される。第２モーター制御部４４は、第２エンコーダー１６Ｅの検出信号から算出される回転角度が第２位置指令Ｐｃ２になるように、これらを用いたフィードバック制御によって第２モーター１６を駆動する。
【００３０】
回転モーター制御部４５には、第３位置指令Ｐｃ３の他、第３エンコーダー２０Ｅから検出信号が入力される。回転モーター制御部４５は、第３エンコーダー２０Ｅの検出信号から算出される回転角度が第３位置指令Ｐｃ３になるように、これらを用いたフィードバック制御によって回転モーター２０を駆動する。
【００３１】
昇降モーター制御部４６には、第４位置指令Ｐｃ４の他、第４エンコーダー２１Ｅから検出信号が入力される。昇降モーター制御部４６は、第４エンコーダー２１Ｅの検出信号から算出される回転角度が第４位置指令Ｐｃ４になるように、これらを用いたフィードバック制御によって昇降モーター２１を駆動する。
【００３２】
次に、第１モーター制御部４３の構成について図３を参照して説明する。
図３に示されるように、第１モーター制御部４３の第１減算器５１には、位置指令生成部４１から第１位置指令Ｐｃが入力され、また、回転角度算出部５２から位置フィードバック値Ｐｆｂが入力される。回転角度算出部５２では、第１エンコーダー１３Ｅから入力されるパルス数がカウントされるとともに、カウント値に応じた第１モーター１３の回転角度が位置フィードバック値Ｐｆｂとして第１減算器５１に出力される。第１減算器５１は、これら第１位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差を位置制御部５３に出力する。
【００３３】
位置制御部５３は、第１減算器５１から入力された偏差に予め定められた係数である位置比例ゲインＫｐを乗算することによって、該偏差に応じた第１モーター１３の回転速度を演算する。位置制御部５３は、上記第１モーター１３の回転速度である速度指令値を示す信号を速度指令Ｖｃとして第２減算器５４に出力する。
【００３４】
第２減算器５４には、上記速度指令Ｖｃの他、回転速度演算部５５から速度計測値としての速度フィードバック値Ｖｆｂが入力される。第２減算器５４は、これら速度指令Ｖｃと速度フィードバック値Ｖｆｂとの偏差を速度制御部６３に出力する。
【００３５】
速度制御部６３は、第２減算器５４から入力された偏差に予め定められた係数である速度比例ゲインＫｖ等を用いた所定の演算処理を行うことで、該偏差に応じた第１モーター１３のトルク指令Ｔｃをトルク制御部６４に出力する。トルク制御部６４は、トルク指令Ｔｃに応じた電流を生成して第１モーター１３に供給する。
【００３６】
次に、回転速度演算部５５の構成について詳しく説明する。
回転速度演算部５５は、第１回転速度算出部５６、第１角速度算出部５７、加減算器５８、第２回転速度算出部５９、第２角速度算出部６０、振動速度算出部６１、加算器６２によって構成されている。
【００３７】
第１速度計測部としての第１回転速度算出部５６では、第１エンコーダー１３Ｅからパルス信号が入力され、該パルス信号の周波数に基づいて、第１モーター１３の回転速度である第１回転速度Ｖ１ｆｂが算出される。そして、第１回転速度算出部５６は、この第１回転速度Ｖ１ｆｂを第１角速度算出部５７と加算器６２とに出力する。
【００３８】
角速度算出部を構成する第１角速度算出部５７では、第１減速機１４の減速比Ｎ１と第１回転速度Ｖ１ｆｂとが乗算されることで、第１水平アーム１２の角速度である第１角速度ωＡ１ｍが第１モーター１３の回転速度に基づいて算出される。そして、第１角速度算出部５７は、この第１角速度ωＡ１ｍを加減算器５８に出力する。
【００３９】
角速度算出部を構成する第２速度計測部としての第２回転速度算出部５９では、第２エンコーダー１６Ｅからパルス信号が入力され、該パルス信号の周波数に基づいて第２モーター１６の第２回転速度Ｖ２ｆｂが算出される。そして、第２回転速度算出部５９は、この第２回転速度Ｖ２ｆｂを第２角速度算出部６０に出力する。
【００４０】
第２角速度算出部６０では、第２減速機１７の減速比Ｎ２と第２回転速度Ｖ２ｆｂとが乗算されることで、第２水平アーム１５の角速度である第２角速度ωＡ２ｍが第２モーター１６の回転速度に基づいて算出される。そして、第２角速度算出部６０は、この第２角速度ωＡ２ｍを加減算器５８に出力する。
【００４１】
加減算器５８には、第１角速度ωＡ１ｍ、第２角速度ωＡ２ｍに加えて、角速度センサー３０の検出信号であるセンサー角速度ωＡ２が入力される。ここで、上述した構成からなるロボット１０では、角速度センサー３０の配置された第２水平アーム１５が、下記（ａ）（ｂ）（ｃ）の角加速度が合成された角加速度で回転している。
（ａ）第１モーター１３の回転速度に応じた第１角速度ωＡ１ｍ
（ｂ）第２モーター１６の回転速度に応じた第２角速度ωＡ２ｍ
（ｃ）第１水平アーム１２を介して第２水平アーム１５に伝わる振動に基づく振動角速度ωＡ１ｓ
それゆえに、角速度センサー３０が出力するセンサー角速度ωＡ２には、第１角速度ωＡ１ｍ、第２角速度ωＡ２ｍ、及び振動角速度ωＡ１ｓが含まれている。加減算器５８では、上記センサー角速度ωＡ２から上記（ａ）第１角速度ωＡ１ｍと上記（ｂ）第２角速度ωＡ２ｍとが減算される。そして、加減算器５８は、この減算結果である振動角速度ωＡ１ｓを振動速度算出部６１に出力する。
【００４２】
振動速度算出部６１では、振動角速度ωＡ１ｓに対して所定の比例ゲインＫｇｐが乗算されることで、振動角速度ωＡ１ｓを相殺するような第１モーター１３の回転速度が振動
速度Ｖ１ｓとして算出される。そして、振動速度算出部６１は、この算出結果である振動速度Ｖ１ｓを加算器６２に出力する。
【００４３】
加算器６２では、第１回転速度Ｖ１ｆｂと振動速度Ｖ１ｓとが加算される。加算器６２は、その加算結果である補正回転速度Ｖ１ａを速度フィードバック値Ｖｆｂとして第２減算器５４に出力する。
【００４４】
次に、上述した構成からなるロボット１０の作用のうち、主に第１モーター制御部４３による第１モーター１３の制御の態様について説明する。
位置指令生成部４１から第１モーター制御部４３に第１位置指令Ｐｃが入力されると、第１位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差が位置制御部５３に出力される。次いで、該偏差に応じた速度指令Ｖｃが位置制御部５３から出力されて、速度指令Ｖｃと速度フィードバック値Ｖｆｂとの偏差が速度制御部６３に出力される。
【００４５】
この際、速度フィードバック値Ｖｆｂが第１回転速度Ｖ１ｆｂと振動速度Ｖ１ｓとの加算値であるため、速度制御部６３に入力される偏差は、こうした振動速度Ｖ１ｓ分が除かれた値、すなわち振動速度Ｖ１ｓを相殺するような値となっている。そして、速度制御部６３から上記偏差に応じたトルク指令Ｔｃが出力され、次いで、トルク制御部６４から該トルク指令Ｔｃに応じた電流が第１モーター１３に供給される。
【００４６】
こうした構成によれば、例えば、第１水平アーム１２が振動によって速度指令Ｖｃよりも大きい角速度で回転している場合、補正回転速度Ｖ１ａが第１回転速度Ｖ１ｆｂよりも振動速度Ｖ１ｓの分だけ小さくなる。こうした速度偏差に基づくトルク指令Ｔｃは、第１水平アーム１２を第１位置指令Ｐｃが示す位置へと移動させつつ、且つ、上記振動の分だけ第１モーター１３の回転速度を抑えるトルク指令である。それゆえに、振動を打ち消すようなトルク指令で第１モーター１３が駆動される結果、第１水平アーム１２が制振されることとなる。
【００４７】
また例えば、第１水平アーム１２が振動によって速度指令Ｖｃよりも小さい角速度で回転している場合、補正回転速度Ｖ１ａが第１回転速度Ｖ１ｆｂよりも振動速度Ｖ１ｓの分だけ大きくなる。こうした速度偏差に基づくトルク指令Ｔｃは、第１水平アーム１２を第１位置指令Ｐｃが示す位置へと移動させつつ、且つ、振動の分だけ第１モーター１３の回転速度を大きくするトルク指令である。それゆえに、これもまた振動を打ち消すようなトルク指令で第１モーター１３が駆動される結果、第１水平アーム１２が制振されることとなる。
【００４８】
以上説明したように、本実施の形態に係るロボット１０によれば、以下に列挙する効果を得ることができる。
（１）上記実施形態によれば、第２水平アーム１５に配置された角速度センサー３０の検出信号であるセンサー角速度ωＡ２に基づいて、第１水平アーム１２を制振制御することができる。すなわち、第１水平アーム１２に角速度センサー３０を配置する必要がなくなることから、第１及び第２アームのそれぞれに角速度センサーが配置されている構成に比べて、角速度センサーに接続される電気配線の数を低減することができるとともに、電気配線の引き回し作業が煩雑になることを抑えることができる。
【００４９】
（２）そのうえ、角速度センサー３０が第２水平アーム１５に配置されていることから、第１水平アーム１２に配置される角速度センサーに比べて、該角速度センサー３０に接続される電気配線に関し、折り曲げられる機会を少なくなるとともに折り曲げ箇所における曲率も大きくすることができる。その結果、角速度センサー３０に接続される電気配線に要求される耐久性を低くすることができる。
【００５０】
（３）上記実施形態によれば、速度指令Ｖｃが回転角度算出部５２からの位置フィードバック値Ｐｆｂと第１位置指令Ｐｃとの偏差に基づく値であることから、第１水平アーム１２の振動を抑えつつ、該第１水平アーム１２の位置を第１位置指令Ｐｃが示す位置に制御することができる。
【００５１】
なお、上記実施の形態は、以下のように適宜変更して実施することも可能である。
・上記実施形態において、第１モーター１３の補正回転速度Ｖ１ａを取得するうえでは、第１回転速度算出部５６及び第１角速度算出部５７が割愛される構成であってもよい。こうした構成における加減算器５８では、第１水平アーム１２に関し、角速度センサー３０からのセンサー角速度ωＡ２と第２モーター１６による回転の第２角速度ωＡ２ｍとの差分が演算結果として振動速度算出部６１に出力されることになる。そして、振動速度算出部６１において、所定の比例ゲインＫｇｐが乗算されることによって補正回転速度Ｖ１ａが算出されることになる。その結果、上記（１）〜（３）に記載した効果を得ることができるだけでなく、第１モーター制御部４３の構成を簡素化することができる。
【００５２】
・上記実施形態のロボット１０は、基台１１に連結された第１水平アーム１２と、第１水平アーム１２を介して基台１１に連結される第２水平アーム１５とを有している。これに限らず、ロボットは、例えば、第１及び第２水平アーム１２，１５を介して基台に連結される第３水平アームを有していてもよい。また例えば、基台１１に連結された第３水平アームに第１水平アーム１２が連結されていてもよい。
【００５３】
・上記実施形態において第１回転速度算出部５６は、第１エンコーダー１３Ｅから入力されるパルス信号の周波数に基づいて第１モーター１３の回転速度を検出した。これに限らず、第１モーター１３の回転角度を算出するうえでは、例えば、速度センサーを別途配置して該速度センサーの検出値から第１モーター１３の回転速度を算出してもよい。なお、同様のことが第２回転速度算出部５９にもいえる。
【００５４】
・上記実施形態のロボット１０では、位置制御部５３において、位置指令生成部４１からの第１位置指令Ｐｃと回転角度算出部５２が算出した位置フィードバック値Ｐｆｂとの偏差に応じた速度指令Ｖｃが算出されている。これに限らず、例えば、位置指令生成部４１が作業部２５の目標位置に応じて制御周期ごとの速度指令Ｖｃを予め演算し、その速度指令Ｖｃを第２減算器５４に出力することによって、第１減算器５１、回転角度算出部５２、位置制御部５３が割愛される構成であってもよい。
【００５５】
・上記実施形態の第１モーター制御部４３には、第１エンコーダー１３Ｅから入力される信号に基づいて第１モーター１３の第１回転速度Ｖ１ｆｂを取得し、該第１回転速度Ｖ１ｆｂを第１角速度算出部５７に出力する第１回転速度算出部５６が設けられている。
【００５６】
これに限らず、第１モーター制御部４３の外部から第１角速度算出部５７に第１回転速度Ｖ１ｆｂを入力可能に、あるいは、第１角速度算出部５７が第１エンコーダー１３Ｅからの信号によって第１回転速度Ｖ１ｆｂを取得可能に構成することで第１回転速度算出部５６を割愛してもよい。なお、同様のことが第２回転速度算出部５９及び第２角速度算出部６０にもいえる。
【符号の説明】
【００５７】
Ｃ１，Ｃ２，Ｃ３…軸心、Ｋｐ…位置比例ゲイン、Ｋｖ…速度比例ゲイン、Ｋｇｐ…比例ゲイン、Ｎ１，Ｎ２…減速比、Ｔｃ…トルク指令、Ｖｃ…速度指令、Ｐｃ…第１位置指令、Ｐｃ２…第２位置指令、Ｐｃ３…第３位置指令、Ｐｃ４…第４位置指令、Ｐｆｂ…位置フィードバック値、ωＡ１ｍ…第１角速度、ωＡ１ｓ…振動角速度、ωＡ２…センサー
角速度、ωＡ２ｍ…第２角速度、Ｖ１ａ…補正回転速度、Ｖ１ｓ…振動速度、Ｖｆｂ…速度フィードバック値、Ｖ１ｆｂ…第１回転速度、Ｖ２ｆｂ…第２回転速度、１０…ロボット、１１…基台、１２…第１水平アーム、１３…第１モーター、１３ａ…回転軸、１３Ｅ…第１エンコーダー、１４…第１減速機、１４ａ…出力軸、１５…第２水平アーム、１５ａ…アーム本体、１６…第２モーター、１６ａ…回転軸、１６Ｅ…第２エンコーダー、１７…第２減速機、１７ａ…出力軸、１８…アームカバー、１９…スプラインシャフト、１９Ｂ…ボールねじナット、１９Ｓ…スプラインナット、２０…回転モーター、２０Ｅ…第３エンコーダー、２１…昇降モーター、２１Ｅ…第４エンコーダー、２５…作業部、３０…角速度センサー、３３…配線ダクト、４０…制御装置、４１…位置指令生成部、４２…モーター制御部、４３…第１モーター制御部、４４…第２モーター制御部、４５…回転モーター制御部、４６…昇降モーター制御部、５１…第１減算器、５２…回転角度算出部、５３…位置制御部、５４…第２減算器、５５…回転速度演算部、５６…第１回転速度算出部、５７…第１角速度算出部、５８…加減算器、５９…第２回転速度算出部、６０…第２角速度算出部、６１…振動速度算出部、６２…加算器、６３…速度制御部、６４…トルク制御部。
【技術分野】
【０００１】
この発明は、角速度センサーを備えた水平多関節ロボット、及び水平多関節ロボットの制御方法に関する。
【背景技術】
【０００２】
従来から、特許文献１に記載のように、アームの角速度を検出する角速度センサーを用いて該アームを制振制御する水平多関節ロボットが知られている。特許文献１に記載の水平多関節ロボットの基台には、基台に対して回転可能に第１アームが連結され、また、該第１アームの先端には、第１アームに対して回転可能に第２アームが連結されている。
【０００３】
そして、上記第１アームが第１駆動源の駆動力を受けて回転する際には、第１アームの回転角度が、第１駆動源の回転角を検出する第１角度センサーによって検出され、また、基体に対する第１アームの角速度が、第１アームに搭載された第１角速度センサーによって検出される。この際、第１駆動源の駆動態様を制御するロボットコントローラでは、第１角度センサー及び第１角速度センサーが検出したデータに基づいて、該検出されるデータが所定の値となるように、第１駆動源に対するフィードバック制御が実行される。
【０００４】
また、上記第２アームが第２駆動源の駆動力を受けて回転する際には、第２アームの回転角度が、第２駆動源の回転角を検出する第２角度センサーによって検出され、また、基体に対する第２アームの角速度が、第２アームに搭載された第２角速度センサーによって検出される。この際、上述したロボットコントローラでは、第１駆動源に対する制御の態様と同様に、第２角度センサー及び第２角速度センサーが検出したデータに基づき、該検出されるデータが所定の値となるように、第２駆動源に対するフィードバック制御が実行される。これにより、第１及び第２アームの制振制御が行われている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−２４２７９４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
ところで、上述した各角速度センサーを駆動するためには、角速度センサーに電源を供給する配線や角速度センサーの検出信号を送信する配線など、角速度センサーとコントローラーとの間に各種の電気配線を接続することが必要になる。こうした電気配線は、角速度センサーの他、駆動源にも必要とされるものであって、通常、中空の基体を通して外部のコントローラーに接続されている。そのため、上述した水平多関節ロボットのように、第１アーム及び第２アームに各別に角速度センサーが配置されている場合には、角速度センサーの数量と同じ数量の電気配線が必要になる。その結果、角速度センサー用の電気配線を引き回す作業が煩雑なものになる。
【０００７】
一方、第１アームに生じた振動は、通常、第１アームに連結された他のアームによって増幅されて、その後、水平多関節ロボットのエンドエフェクターに伝わる。そのため、上述した第２角度センサー及び第２角速度センサーを割愛するとともに、第１角速度センサーの検出結果に基づく第１駆動源の制振制御のみを行うことによって、エンドエフェクターの振動を抑えることも可能ではある。しかし、第１アームが基体に対して描く軌道には、通常、他のアームが基体に対して描く軌道と比較して、小さい曲率の軌道が多く含まれる。そのため、第１アームから引き出される電気配線に対しては、他のアームから引き出
される電気配線と比較して、折り曲げられる機会が多くなり、また折り曲げ箇所における曲率が小さくもなる。それゆえに、第１角速度センサーに接続される電気配線には、第２角速度センサーに接続される電気配線よりも高い耐久性が求められるため、結局のところ、こうした電気配線の配置に関わる制約や同電気配線を引き回すことの煩雑さが依然として残ることになる。
【０００８】
本発明は、上記実情を鑑みてなされたものであり、その目的は、制振制御に用いられる角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線に要求される耐久性を低くした水平多関節ロボット及び水平多関節ロボットの制御方法を提供することにある。
【課題を解決するための手段】
【０００９】
本発明の水平多関節ロボットは、少なくとも関節を介して基台に連結される基端を有して第１モーターの駆動により回転する第１アームと、前記第１モーターの回転速度を計測する第１速度計測部と、前記第１速度計測部の計測値に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する制御部とを備える水平多関節ロボットであって、前記第１アームの先端に連結されて第２モーターの駆動により回転する第２アームと、前記第２モーターの回転速度を計測する第２速度計測部と、前記第１モーターの回転速度から前記第１アームの角速度を算出し、且つ前記第２モーターの回転速度から前記第２アームの角速度を算出する角速度算出部と、前記第２アームに配置されて角速度を検出する角速度センサーとを備え、前記制御部は、前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記第１速度計測部の計測値と前記振動速度との加算値を前記速度計測値とする。
【００１０】
上述した発明の水平多関節ロボットに関し、角速度センサーによって検出される角速度は、対応するモーターの回転速度に基づく各アームの角速度と振動に基づく各アームの角速度とを含む角速度である。そのため、対応するモーターの回転速度に基づくアームの角速度をアームごとに求め、その求めた角速度を角速度センサーが検出した角速度から減算することによって、振動によるアームの角速度である振動角速度を求めることが可能である。上記構成の水平多関節ロボットでは、第１及び第２速度計測部の計測値に基づいて第１及び第２アームの角速度が算出され、これらの角速度が角速度センサーの検出値から減算されることで振動角速度が算出される。そして、該振動角速度に応じた第１モーターの回転速度である振動速度と第１速度計測部の計測値との加算値である速度計測値が、速度指令値となるように第１モーターが制御される。すなわち、アームの振動の分をも加味した第１モーターの回転速度が速度指令値となるように第１モーターが制御されることから、第１アームの振動を抑えることができる。
【００１１】
すなわち、第１アームに角速度センサーを配置する必要がなくなることから、配置される角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線の数を低減することができる。その結果、角速度センサーに関わる電気配線を引き回す作業が煩雑になることを抑えることができる。そのうえ、角速度センサーが第２アームに配置されていることから、第１アームに配置される角速度センサーに接続される電気配線に比べて、折り曲げられる機会が少なくなるとともに折り曲げ箇所における曲率も大きくなる。その結果、角速度センサーに接続される電気配線に要求される耐久性を低くすることもできる。
【００１２】
この水平多関節ロボットは、前記第１モーターの回転角度を検出する第１位置検出部を備え、前記制御部は、前記第１位置検出部の検出値と位置指令値との偏差から前記速度指令値を算出する。
【００１３】
この水平多関節ロボットによれば、速度指令値が第１位置検出部の検出値と位置指令値との偏差であることから、第１アームの振動を抑えつつ、該第１アームの位置を位置指令値が示す位置に制御することができる。
【００１４】
この水平多関節ロボットは、前記第２モーターの回転角度を検出する第２位置検出部を備え、前記第１速度計測部は、前記第１位置検出部の検出値から前記第１モーターの回転角度を算出し、前記第２速度計測部は、前記第２位置検出部の検出値から前記第２モーターの回転角度を算出する。
【００１５】
この水平多関節ロボットのように、第１速度計測部は、第１位置検出部の検出値から第１モーターの回転角度を算出することができる。また、第２速度計測部は、第２位置検出部の検出値から第２モーターの回転速度を算出することができる。
【００１６】
この水平多関節ロボットにおいて、前記第１アームは、関節を介して基台に連結されたアームである。
この水平多関節ロボットによれば、基台に連結されているアームを制振制御することができる。
【００１７】
本発明の水平多関節ロボットの制御方法は、少なくとも関節を介して基台に連結される基端を有する第１アームを回転させる第１モーターの回転速度を計測し、その計測された回転速度に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する水平多関節ロボットの制御方法であって、前記第１アームの先端に連結された第２アームを回転させる第２モーターの回転速度を計測するステップと、前記計測された第１モーターの回転速度から前記第１アームの角速度を算出するステップと、前記計測された第２モーターの回転速度から前記第２アームの角速度を算出するステップと、前記第２アームに配置された角速度センサーの検出値を取得するステップとを備え、前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記計測された第１モーターの回転速度と前記振動速度との加算値を前記速度計測値とする。
【００１８】
本発明の水平多関節ロボットの制御方法によれば、第２アームに配置された角速度センサーの検出値に基づいて第１アームを制振制御することができることから、配置される角速度センサーの数を低減するとともに該角速度センサーに接続される電気配線の数を低減することができる。その結果、角速度センサーに関わる電気配線を引き回す作業が煩雑になることを抑えることができる。そのうえ、角速度センサーが第２アームに配置されていることから、第１アームに配置される角速度センサーに接続される電気配線に比べて、折り曲げられる機会が少なくなるとともに折り曲げ箇所における曲率も大きくなる。その結果、角速度センサーに接続される電気配線に要求される耐久性を低くすることもできる。
【図面の簡単な説明】
【００１９】
【図１】本発明の一実施の形態にかかるロボットの正面構造を示す正面図。
【図２】ロボットの電気的構成を示す電気ブロック回路図。
【図３】第１モーター制御部の機能的な構成を示す機能ブロック図。
【発明を実施するための形態】
【００２０】
以下、本発明にかかる水平多関節ロボットの一実施の形態について、図１〜図３を参照して説明する。
図１に示されるように、ロボット１０における基台１１の上端部には、鉛直方向に沿う
軸心Ｃ１を中心にして、基台１１に対して回転する第１アームとしての第１水平アーム１２の基端部が連結されている。基台１１内には、第１水平アーム１２を回転させる第１モーター１３と、該第１モーターの回転軸１３ａに連結され、出力軸１４ａが第１水平アーム１２に連結固定された第１減速機１４とが設置されている。そして、第１モーター１３の駆動力が第１減速機１４を介して第１水平アーム１２に伝達されると、第１水平アーム１２は、基台１１に対して水平面内で回転する。また、第１モーター１３には、該第１モーター１３の回転量に応じたパルス信号を出力する第１位置検出部としての第１エンコーダー１３Ｅが設けられている。
【００２１】
第１水平アーム１２の先端部には、鉛直方向に沿う軸心Ｃ２を中心にして第１水平アーム１２に対して回転する第２アームとしての第２水平アーム１５が連結されている。第２水平アーム１５を構成するアーム本体１５ａの内部には、第２水平アーム１５を回転させる第２モーター１６と、該第２モーター１６の回転軸１６ａに連結されて出力軸１７ａが第１水平アーム１２に連結固定された第２減速機１７とが設置されている。そして、第２モーター１６の駆動力が第２減速機１７を介して第２水平アーム１５に伝達されると、第２水平アーム１５は、軸心Ｃ２を中心にして第１水平アーム１２に対して水平面内で回転する。また、第２モーター１６には、該第２モーター１６の回転量に応じたパルス信号を出力する第２位置検出部としての第２エンコーダー１６Ｅが設けられている。こうしたアーム本体１５ａの上側は、その全体がアームカバー１８によって覆われている。
【００２２】
第２水平アーム１５の先端部には、アーム本体１５ａとアームカバー１８とを貫通し、第２水平アーム１５に対して変位するスプラインシャフト１９が設けられている。スプラインシャフト１９は、第２水平アーム１５の先端部に配置されたスプラインナット１９Ｓに嵌め合わされるように挿通され、第２水平アーム１５に対して回転可能に、かつ、上下方向に移動可能に支持されている。
【００２３】
第２水平アーム１５内には、回転モーター２０が設置され、回転モーター２０の駆動力は、図示しないベルトを介してスプラインナット１９Ｓに伝達される。回転モーター２０がスプラインナット１９Ｓを正逆回転すると、スプラインシャフト１９は鉛直方向に沿う軸心Ｃ３を中心にして正逆回転する。回転モーター２０には、該回転モーター２０の回転量に応じたパルス信号を出力する第３エンコーダー２０Ｅが設けられている。
【００２４】
第２水平アーム１５内には、ボールねじナット１９Ｂを正逆回転する昇降モーター２１が設置されている。昇降モーター２１がボールねじナット１９Ｂを正逆回転すると、スプラインシャフト１９は鉛直方向に昇降する。昇降モーター２１には、該昇降モーター２１の回転量に応じたパルス信号を出力する第４エンコーダー２１Ｅが設けられている。スプラインシャフト１９の下端に連結された作業部２５には、例えば被搬送物を把持するものや被加工物を加工するもの等の取り付けが可能になっている。
【００２５】
また、第２水平アーム１５内には、基台１１に対する第２水平アーム１５の角速度を測定する角速度センサー３０が配設されている。角速度センサー３０は、本実施形態では、水晶型振動子を用いた振動型のジャイロスコープを採用している。第２水平アーム１５における上側には、他端が基台１１に連結された配管部材として可撓性を有する配線ダクト３３の一端が連結されている。第２モーター１６や第２エンコーダー１６Ｅ、回転モーター２０、昇降モーター２１等、第２水平アーム１５内に設置されている各機器に接続される電気配線は、配線ダクト３３内を通じて第２水平アーム１５内から基台１１内まで引き回される。
【００２６】
そして、第２水平アーム１５内から基台１１内まで引き回された各電気配線は、基台１１内でまとめられることによって、上記第１モーター１３及び第１エンコーダー１３Ｅに
接続される電気配線とともに、基台１１の外部に設置されロボット１０を統括制御する制御装置４０まで引き回される。制御装置４０は、角速度センサー３０等から入力される各種信号に基づいて第１水平アーム１２の角速度を演算し、該第２水平アーム１５の振動が抑制されるように第１モーター１３を制御する。
【００２７】
次に、制御装置４０の電気的構成について図２を参照して説明する。
図２に示されるように、制御装置４０の位置指令生成部４１は、ロボット１０が行う処理の内容に基づいて作業部２５の目標位置を演算し、該目標位置に作業部２５を移動させるための軌道を生成する。また、位置指令生成部４１は、該軌道に沿って作業部２５が移動するように、各モーター１３，１６，２０，２１の回転角度を所定の制御周期ごとに演算し、該演算の結果である目標回転角度を第１位置指令Ｐｃ、第２位置指令Ｐｃ２、第３位置指令Ｐｃ３、第４位置指令Ｐｃ４としてモーター制御部４２に出力する。
【００２８】
モーター制御部４２は、第１モーター制御部４３、第２モーター制御部４４、回転モーター制御部４５、及び昇降モーター制御部４６によって構成されている。
第１モーター制御部４３には、位置指令値である第１位置指令Ｐｃの他、第１エンコーダー１３Ｅ、第２エンコーダー１６Ｅ、及び角速度センサー３０の各々から検出信号が入力される。第１モーター制御部４３は、第１エンコーダー１３Ｅの検出信号から算出される回転角度が第１位置指令Ｐｃになるように、各検出信号を用いたフィードバック制御によって第１モーター１３を駆動する。
【００２９】
第２モーター制御部４４には、第２位置指令Ｐｃ２の他、第２エンコーダー１６Ｅから検出信号が入力される。第２モーター制御部４４は、第２エンコーダー１６Ｅの検出信号から算出される回転角度が第２位置指令Ｐｃ２になるように、これらを用いたフィードバック制御によって第２モーター１６を駆動する。
【００３０】
回転モーター制御部４５には、第３位置指令Ｐｃ３の他、第３エンコーダー２０Ｅから検出信号が入力される。回転モーター制御部４５は、第３エンコーダー２０Ｅの検出信号から算出される回転角度が第３位置指令Ｐｃ３になるように、これらを用いたフィードバック制御によって回転モーター２０を駆動する。
【００３１】
昇降モーター制御部４６には、第４位置指令Ｐｃ４の他、第４エンコーダー２１Ｅから検出信号が入力される。昇降モーター制御部４６は、第４エンコーダー２１Ｅの検出信号から算出される回転角度が第４位置指令Ｐｃ４になるように、これらを用いたフィードバック制御によって昇降モーター２１を駆動する。
【００３２】
次に、第１モーター制御部４３の構成について図３を参照して説明する。
図３に示されるように、第１モーター制御部４３の第１減算器５１には、位置指令生成部４１から第１位置指令Ｐｃが入力され、また、回転角度算出部５２から位置フィードバック値Ｐｆｂが入力される。回転角度算出部５２では、第１エンコーダー１３Ｅから入力されるパルス数がカウントされるとともに、カウント値に応じた第１モーター１３の回転角度が位置フィードバック値Ｐｆｂとして第１減算器５１に出力される。第１減算器５１は、これら第１位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差を位置制御部５３に出力する。
【００３３】
位置制御部５３は、第１減算器５１から入力された偏差に予め定められた係数である位置比例ゲインＫｐを乗算することによって、該偏差に応じた第１モーター１３の回転速度を演算する。位置制御部５３は、上記第１モーター１３の回転速度である速度指令値を示す信号を速度指令Ｖｃとして第２減算器５４に出力する。
【００３４】
第２減算器５４には、上記速度指令Ｖｃの他、回転速度演算部５５から速度計測値としての速度フィードバック値Ｖｆｂが入力される。第２減算器５４は、これら速度指令Ｖｃと速度フィードバック値Ｖｆｂとの偏差を速度制御部６３に出力する。
【００３５】
速度制御部６３は、第２減算器５４から入力された偏差に予め定められた係数である速度比例ゲインＫｖ等を用いた所定の演算処理を行うことで、該偏差に応じた第１モーター１３のトルク指令Ｔｃをトルク制御部６４に出力する。トルク制御部６４は、トルク指令Ｔｃに応じた電流を生成して第１モーター１３に供給する。
【００３６】
次に、回転速度演算部５５の構成について詳しく説明する。
回転速度演算部５５は、第１回転速度算出部５６、第１角速度算出部５７、加減算器５８、第２回転速度算出部５９、第２角速度算出部６０、振動速度算出部６１、加算器６２によって構成されている。
【００３７】
第１速度計測部としての第１回転速度算出部５６では、第１エンコーダー１３Ｅからパルス信号が入力され、該パルス信号の周波数に基づいて、第１モーター１３の回転速度である第１回転速度Ｖ１ｆｂが算出される。そして、第１回転速度算出部５６は、この第１回転速度Ｖ１ｆｂを第１角速度算出部５７と加算器６２とに出力する。
【００３８】
角速度算出部を構成する第１角速度算出部５７では、第１減速機１４の減速比Ｎ１と第１回転速度Ｖ１ｆｂとが乗算されることで、第１水平アーム１２の角速度である第１角速度ωＡ１ｍが第１モーター１３の回転速度に基づいて算出される。そして、第１角速度算出部５７は、この第１角速度ωＡ１ｍを加減算器５８に出力する。
【００３９】
角速度算出部を構成する第２速度計測部としての第２回転速度算出部５９では、第２エンコーダー１６Ｅからパルス信号が入力され、該パルス信号の周波数に基づいて第２モーター１６の第２回転速度Ｖ２ｆｂが算出される。そして、第２回転速度算出部５９は、この第２回転速度Ｖ２ｆｂを第２角速度算出部６０に出力する。
【００４０】
第２角速度算出部６０では、第２減速機１７の減速比Ｎ２と第２回転速度Ｖ２ｆｂとが乗算されることで、第２水平アーム１５の角速度である第２角速度ωＡ２ｍが第２モーター１６の回転速度に基づいて算出される。そして、第２角速度算出部６０は、この第２角速度ωＡ２ｍを加減算器５８に出力する。
【００４１】
加減算器５８には、第１角速度ωＡ１ｍ、第２角速度ωＡ２ｍに加えて、角速度センサー３０の検出信号であるセンサー角速度ωＡ２が入力される。ここで、上述した構成からなるロボット１０では、角速度センサー３０の配置された第２水平アーム１５が、下記（ａ）（ｂ）（ｃ）の角加速度が合成された角加速度で回転している。
（ａ）第１モーター１３の回転速度に応じた第１角速度ωＡ１ｍ
（ｂ）第２モーター１６の回転速度に応じた第２角速度ωＡ２ｍ
（ｃ）第１水平アーム１２を介して第２水平アーム１５に伝わる振動に基づく振動角速度ωＡ１ｓ
それゆえに、角速度センサー３０が出力するセンサー角速度ωＡ２には、第１角速度ωＡ１ｍ、第２角速度ωＡ２ｍ、及び振動角速度ωＡ１ｓが含まれている。加減算器５８では、上記センサー角速度ωＡ２から上記（ａ）第１角速度ωＡ１ｍと上記（ｂ）第２角速度ωＡ２ｍとが減算される。そして、加減算器５８は、この減算結果である振動角速度ωＡ１ｓを振動速度算出部６１に出力する。
【００４２】
振動速度算出部６１では、振動角速度ωＡ１ｓに対して所定の比例ゲインＫｇｐが乗算されることで、振動角速度ωＡ１ｓを相殺するような第１モーター１３の回転速度が振動
速度Ｖ１ｓとして算出される。そして、振動速度算出部６１は、この算出結果である振動速度Ｖ１ｓを加算器６２に出力する。
【００４３】
加算器６２では、第１回転速度Ｖ１ｆｂと振動速度Ｖ１ｓとが加算される。加算器６２は、その加算結果である補正回転速度Ｖ１ａを速度フィードバック値Ｖｆｂとして第２減算器５４に出力する。
【００４４】
次に、上述した構成からなるロボット１０の作用のうち、主に第１モーター制御部４３による第１モーター１３の制御の態様について説明する。
位置指令生成部４１から第１モーター制御部４３に第１位置指令Ｐｃが入力されると、第１位置指令Ｐｃと位置フィードバック値Ｐｆｂとの偏差が位置制御部５３に出力される。次いで、該偏差に応じた速度指令Ｖｃが位置制御部５３から出力されて、速度指令Ｖｃと速度フィードバック値Ｖｆｂとの偏差が速度制御部６３に出力される。
【００４５】
この際、速度フィードバック値Ｖｆｂが第１回転速度Ｖ１ｆｂと振動速度Ｖ１ｓとの加算値であるため、速度制御部６３に入力される偏差は、こうした振動速度Ｖ１ｓ分が除かれた値、すなわち振動速度Ｖ１ｓを相殺するような値となっている。そして、速度制御部６３から上記偏差に応じたトルク指令Ｔｃが出力され、次いで、トルク制御部６４から該トルク指令Ｔｃに応じた電流が第１モーター１３に供給される。
【００４６】
こうした構成によれば、例えば、第１水平アーム１２が振動によって速度指令Ｖｃよりも大きい角速度で回転している場合、補正回転速度Ｖ１ａが第１回転速度Ｖ１ｆｂよりも振動速度Ｖ１ｓの分だけ小さくなる。こうした速度偏差に基づくトルク指令Ｔｃは、第１水平アーム１２を第１位置指令Ｐｃが示す位置へと移動させつつ、且つ、上記振動の分だけ第１モーター１３の回転速度を抑えるトルク指令である。それゆえに、振動を打ち消すようなトルク指令で第１モーター１３が駆動される結果、第１水平アーム１２が制振されることとなる。
【００４７】
また例えば、第１水平アーム１２が振動によって速度指令Ｖｃよりも小さい角速度で回転している場合、補正回転速度Ｖ１ａが第１回転速度Ｖ１ｆｂよりも振動速度Ｖ１ｓの分だけ大きくなる。こうした速度偏差に基づくトルク指令Ｔｃは、第１水平アーム１２を第１位置指令Ｐｃが示す位置へと移動させつつ、且つ、振動の分だけ第１モーター１３の回転速度を大きくするトルク指令である。それゆえに、これもまた振動を打ち消すようなトルク指令で第１モーター１３が駆動される結果、第１水平アーム１２が制振されることとなる。
【００４８】
以上説明したように、本実施の形態に係るロボット１０によれば、以下に列挙する効果を得ることができる。
（１）上記実施形態によれば、第２水平アーム１５に配置された角速度センサー３０の検出信号であるセンサー角速度ωＡ２に基づいて、第１水平アーム１２を制振制御することができる。すなわち、第１水平アーム１２に角速度センサー３０を配置する必要がなくなることから、第１及び第２アームのそれぞれに角速度センサーが配置されている構成に比べて、角速度センサーに接続される電気配線の数を低減することができるとともに、電気配線の引き回し作業が煩雑になることを抑えることができる。
【００４９】
（２）そのうえ、角速度センサー３０が第２水平アーム１５に配置されていることから、第１水平アーム１２に配置される角速度センサーに比べて、該角速度センサー３０に接続される電気配線に関し、折り曲げられる機会を少なくなるとともに折り曲げ箇所における曲率も大きくすることができる。その結果、角速度センサー３０に接続される電気配線に要求される耐久性を低くすることができる。
【００５０】
（３）上記実施形態によれば、速度指令Ｖｃが回転角度算出部５２からの位置フィードバック値Ｐｆｂと第１位置指令Ｐｃとの偏差に基づく値であることから、第１水平アーム１２の振動を抑えつつ、該第１水平アーム１２の位置を第１位置指令Ｐｃが示す位置に制御することができる。
【００５１】
なお、上記実施の形態は、以下のように適宜変更して実施することも可能である。
・上記実施形態において、第１モーター１３の補正回転速度Ｖ１ａを取得するうえでは、第１回転速度算出部５６及び第１角速度算出部５７が割愛される構成であってもよい。こうした構成における加減算器５８では、第１水平アーム１２に関し、角速度センサー３０からのセンサー角速度ωＡ２と第２モーター１６による回転の第２角速度ωＡ２ｍとの差分が演算結果として振動速度算出部６１に出力されることになる。そして、振動速度算出部６１において、所定の比例ゲインＫｇｐが乗算されることによって補正回転速度Ｖ１ａが算出されることになる。その結果、上記（１）〜（３）に記載した効果を得ることができるだけでなく、第１モーター制御部４３の構成を簡素化することができる。
【００５２】
・上記実施形態のロボット１０は、基台１１に連結された第１水平アーム１２と、第１水平アーム１２を介して基台１１に連結される第２水平アーム１５とを有している。これに限らず、ロボットは、例えば、第１及び第２水平アーム１２，１５を介して基台に連結される第３水平アームを有していてもよい。また例えば、基台１１に連結された第３水平アームに第１水平アーム１２が連結されていてもよい。
【００５３】
・上記実施形態において第１回転速度算出部５６は、第１エンコーダー１３Ｅから入力されるパルス信号の周波数に基づいて第１モーター１３の回転速度を検出した。これに限らず、第１モーター１３の回転角度を算出するうえでは、例えば、速度センサーを別途配置して該速度センサーの検出値から第１モーター１３の回転速度を算出してもよい。なお、同様のことが第２回転速度算出部５９にもいえる。
【００５４】
・上記実施形態のロボット１０では、位置制御部５３において、位置指令生成部４１からの第１位置指令Ｐｃと回転角度算出部５２が算出した位置フィードバック値Ｐｆｂとの偏差に応じた速度指令Ｖｃが算出されている。これに限らず、例えば、位置指令生成部４１が作業部２５の目標位置に応じて制御周期ごとの速度指令Ｖｃを予め演算し、その速度指令Ｖｃを第２減算器５４に出力することによって、第１減算器５１、回転角度算出部５２、位置制御部５３が割愛される構成であってもよい。
【００５５】
・上記実施形態の第１モーター制御部４３には、第１エンコーダー１３Ｅから入力される信号に基づいて第１モーター１３の第１回転速度Ｖ１ｆｂを取得し、該第１回転速度Ｖ１ｆｂを第１角速度算出部５７に出力する第１回転速度算出部５６が設けられている。
【００５６】
これに限らず、第１モーター制御部４３の外部から第１角速度算出部５７に第１回転速度Ｖ１ｆｂを入力可能に、あるいは、第１角速度算出部５７が第１エンコーダー１３Ｅからの信号によって第１回転速度Ｖ１ｆｂを取得可能に構成することで第１回転速度算出部５６を割愛してもよい。なお、同様のことが第２回転速度算出部５９及び第２角速度算出部６０にもいえる。
【符号の説明】
【００５７】
Ｃ１，Ｃ２，Ｃ３…軸心、Ｋｐ…位置比例ゲイン、Ｋｖ…速度比例ゲイン、Ｋｇｐ…比例ゲイン、Ｎ１，Ｎ２…減速比、Ｔｃ…トルク指令、Ｖｃ…速度指令、Ｐｃ…第１位置指令、Ｐｃ２…第２位置指令、Ｐｃ３…第３位置指令、Ｐｃ４…第４位置指令、Ｐｆｂ…位置フィードバック値、ωＡ１ｍ…第１角速度、ωＡ１ｓ…振動角速度、ωＡ２…センサー
角速度、ωＡ２ｍ…第２角速度、Ｖ１ａ…補正回転速度、Ｖ１ｓ…振動速度、Ｖｆｂ…速度フィードバック値、Ｖ１ｆｂ…第１回転速度、Ｖ２ｆｂ…第２回転速度、１０…ロボット、１１…基台、１２…第１水平アーム、１３…第１モーター、１３ａ…回転軸、１３Ｅ…第１エンコーダー、１４…第１減速機、１４ａ…出力軸、１５…第２水平アーム、１５ａ…アーム本体、１６…第２モーター、１６ａ…回転軸、１６Ｅ…第２エンコーダー、１７…第２減速機、１７ａ…出力軸、１８…アームカバー、１９…スプラインシャフト、１９Ｂ…ボールねじナット、１９Ｓ…スプラインナット、２０…回転モーター、２０Ｅ…第３エンコーダー、２１…昇降モーター、２１Ｅ…第４エンコーダー、２５…作業部、３０…角速度センサー、３３…配線ダクト、４０…制御装置、４１…位置指令生成部、４２…モーター制御部、４３…第１モーター制御部、４４…第２モーター制御部、４５…回転モーター制御部、４６…昇降モーター制御部、５１…第１減算器、５２…回転角度算出部、５３…位置制御部、５４…第２減算器、５５…回転速度演算部、５６…第１回転速度算出部、５７…第１角速度算出部、５８…加減算器、５９…第２回転速度算出部、６０…第２角速度算出部、６１…振動速度算出部、６２…加算器、６３…速度制御部、６４…トルク制御部。
【特許請求の範囲】
【請求項１】
少なくとも関節を介して基台に連結される基端を有して第１モーターの駆動により回転する第１アームと、前記第１モーターの回転速度を計測する第１速度計測部と、前記第１速度計測部の計測値に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する制御部とを備える水平多関節ロボットであって、
前記第１アームの先端に連結されて第２モーターの駆動により回転する第２アームと、
前記第２モーターの回転速度を計測する第２速度計測部と、
前記第１モーターの回転速度から前記第１アームの角速度を算出し、且つ前記第２モーターの回転速度から前記第２アームの角速度を算出する角速度算出部と、
前記第２アームに配置されて角速度を検出する角速度センサーとを備え、
前記制御部は、
前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記第１速度計測部の計測値と前記振動速度との加算値を前記速度計測値とすることを特徴とする水平多関節ロボット。
【請求項２】
前記第１モーターの回転角度を検出する第１位置検出部を備え、
前記制御部は、
前記第１位置検出部の検出値と位置指令値との偏差から前記速度指令値を算出する
請求項１に記載の水平多関節ロボット。
【請求項３】
前記第２モーターの回転角度を検出する第２位置検出部を備え、
前記第１速度計測部は、前記第１位置検出部の検出値から前記第１モーターの回転速度を算出し、
前記第２速度計測部は、前記第２位置検出部の検出値から前記第２モーターの回転速度を算出する
請求項２に記載の水平多関節ロボット。
【請求項４】
前記第１アームは、関節を介して基台に連結されたアームである
請求項１〜３のいずれか一項に記載の水平多関節ロボット。
【請求項５】
少なくとも関節を介して基台に連結される基端を有する第１アームを回転させる第１モーターの回転速度を計測し、その計測された回転速度に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する水平多関節ロボットの制御方法であって、
前記第１アームの先端に連結された第２アームを回転させる第２モーターの回転速度を計測するステップと、
前記計測された第１モーターの回転速度から前記第１アームの角速度を算出するステップと、
前記計測された第２モーターの回転速度から前記第２アームの角速度を算出するステップと、
前記第２アームに配置された角速度センサーの検出値を取得するステップとを備え、
前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記計測された第１モーターの回転速度と前記振動速度との加算値を前記速度計測値とする
ことを特徴とする水平多関節ロボットの制御方法。
【請求項１】
少なくとも関節を介して基台に連結される基端を有して第１モーターの駆動により回転する第１アームと、前記第１モーターの回転速度を計測する第１速度計測部と、前記第１速度計測部の計測値に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する制御部とを備える水平多関節ロボットであって、
前記第１アームの先端に連結されて第２モーターの駆動により回転する第２アームと、
前記第２モーターの回転速度を計測する第２速度計測部と、
前記第１モーターの回転速度から前記第１アームの角速度を算出し、且つ前記第２モーターの回転速度から前記第２アームの角速度を算出する角速度算出部と、
前記第２アームに配置されて角速度を検出する角速度センサーとを備え、
前記制御部は、
前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記第１速度計測部の計測値と前記振動速度との加算値を前記速度計測値とすることを特徴とする水平多関節ロボット。
【請求項２】
前記第１モーターの回転角度を検出する第１位置検出部を備え、
前記制御部は、
前記第１位置検出部の検出値と位置指令値との偏差から前記速度指令値を算出する
請求項１に記載の水平多関節ロボット。
【請求項３】
前記第２モーターの回転角度を検出する第２位置検出部を備え、
前記第１速度計測部は、前記第１位置検出部の検出値から前記第１モーターの回転速度を算出し、
前記第２速度計測部は、前記第２位置検出部の検出値から前記第２モーターの回転速度を算出する
請求項２に記載の水平多関節ロボット。
【請求項４】
前記第１アームは、関節を介して基台に連結されたアームである
請求項１〜３のいずれか一項に記載の水平多関節ロボット。
【請求項５】
少なくとも関節を介して基台に連結される基端を有する第１アームを回転させる第１モーターの回転速度を計測し、その計測された回転速度に基づく速度計測値が速度指令値となるように前記第１モーターをフィードバック制御する水平多関節ロボットの制御方法であって、
前記第１アームの先端に連結された第２アームを回転させる第２モーターの回転速度を計測するステップと、
前記計測された第１モーターの回転速度から前記第１アームの角速度を算出するステップと、
前記計測された第２モーターの回転速度から前記第２アームの角速度を算出するステップと、
前記第２アームに配置された角速度センサーの検出値を取得するステップとを備え、
前記角速度センサーの検出値から前記第１アームの角速度と前記第２アームの角速度とを減算して該減算の結果である振動角速度から該振動角速度に応じた前記第１モーターの回転速度である振動速度を算出し、前記計測された第１モーターの回転速度と前記振動速度との加算値を前記速度計測値とする
ことを特徴とする水平多関節ロボットの制御方法。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１２−１７１０５２（Ｐ２０１２−１７１０５２Ａ）
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願番号】特願２０１１−３５８６０（Ｐ２０１１−３５８６０）
【出願日】平成２３年２月２２日（２０１１．２．２２）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年９月１０日（２０１２．９．１０）
【国際特許分類】
【出願日】平成２３年２月２２日（２０１１．２．２２）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]