JP2016147322A - ロボットの制御装置及び制御方法 - Google Patents

Abstract

【課題】アームの移動速度を十分に抑制できるロボットの制御装置及び制御方法を提供する。
【解決手段】コントローラ３０は、複数の回転部１３〜１８と、回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット１０に適用される。コントローラ３０は、アームのＴＣＰの現在の位置及び姿勢を動作周期後の位置及び姿勢まで制御するためのＴＣＰの位置及び姿勢の変化量を算出する手段と、各回転部に設定された各監視部の速度が基準速度以下となるように上記変化量を低下させる手段と、上記低下させられた変化量と、ＴＣＰの現在の位置及び姿勢とに基づいて、ＴＣＰの動作周期後の位置及び姿勢を算出する手段と、算出された動作周期後の位置及び姿勢を逆変換することで得られる各サーボモータの動作周期後の角度まで、各サーボモータの現在の角度が制御されるように、各サーボモータを駆動させる手段とを備える。
【選択図】 図１

Description

本発明は、ロボットの制御装置及び制御方法に関する。

従来、ロボットの手動操作時に、ロボットの制御点の移動速度が基準速度を超えた際に、移動速度が基準速度以下となるように動作目標位置を修正してロボットを動作させるものがある（特許文献１参照）。

特許第３９９４４８７号公報

しかしながら、ロボットの制御点として設定されるアームの先端部の移動速度を、基準速度以下となるように制御したとしても、アームの移動速度を十分に抑制することができない場合があることに本願発明者は着目した。

本発明は、こうした実情に鑑みてなされたものであり、アームの移動速度を十分に抑制することのできるロボットの制御装置及び制御方法を提供することを主たる目的とするものである。

第１の手段は、複数の回転部と、前記回転部を互いに回転可能に連結する関節と、前記各回転部を駆動するサーボモータと、を含むアームを備えるロボットに適用され、前記アームの先端部を制御点として設定し、ＰＴＰ制御により前記制御点の位置及び姿勢を制御するロボットの制御装置であって、前記各サーボモータの動作周期後の角度を算出する角度算出手段と、前記各回転部に設定された監視部の現在の位置を算出する現在監視位置算出手段と、前記角度算出手段により算出された角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出手段と、前記現在監視位置算出手段により算出された前記各監視部の現在の位置と、前記次周期監視位置算出手段により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する速度算出手段と、前記制御点の現在の位置及び姿勢を、前記制御点の動作周期後の位置及び姿勢まで制御するための前記制御点の位置及び姿勢の変化量を算出する変化量算出手段と、前記速度算出手段により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記各監視部の速度が前記基準速度以下となるように、前記変化量算出手段により算出された変化量を低下させる低下手段と、前記低下手段により低下させられた変化量と、前記制御点の現在の位置及び姿勢とに基づいて、前記制御点の動作周期後の位置及び姿勢を算出する位置姿勢算出手段と、前記位置姿勢算出手段により算出された位置及び姿勢を逆変換することで得られる前記各サーボモータの動作周期後の角度で、前記角度算出手段により算出された前記各サーボモータの動作周期後の角度を更新する更新手段と、前記各サーボモータの現在の角度が、前記更新手段によって更新された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動手段と、を備えることを特徴とする。

上記構成では、ロボットのアームは複数の回転部を含んでおり、回転部は関節により互いに回転可能に連結されている。そして、アームの先端部が制御点として設定され、ＰＴＰ制御により、制御点の現在の位置及び姿勢が制御される。

ここで、ロボットの制御点として設定されるアームの先端部の移動速度を、基準速度以下となるように制御したとしても、アーム（ロボット）の姿勢によっては、アームにおける制御点以外の部分の移動速度が基準速度よりも高くなる場合があることに本願発明者は着目した。

そこで上記構成では、各回転部に監視部が設定される。例えば、各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。そして、角度算出手段により各サーボモータの動作周期後の角度が算出され、算出された角度に基づいて、各監視部の動作周期後の位置が算出される。続いて、各監視部の現在の位置、及び各監視部の動作周期後の位置に基づいて、各監視部の速度が算出される。そして、算出された各監視部の速度が基準速度以下となるように、制御点の現在の位置及び姿勢を動作周期後の位置及び姿勢まで制御するための制御点の位置及び姿勢の変化量が低下させられる。

続いて、制御点の低下させられた位置及び姿勢の変化量と、制御点の現在の位置及び姿勢とに基づいて、制御点の動作周期後の位置及び姿勢が新たに算出される。そして、新たに算出された位置及び姿勢を逆変換することで得られる各サーボモータの動作周期後の角度で、角度算出手段により算出された各サーボモータの動作周期後の角度が更新される。そして、各サーボモータの現在の角度が、更新された各サーボモータの角度まで動作周期後に制御されるように、各サーボモータが駆動される。このため、各サーボモータの動作周期あたりに駆動される角度が小さくなり、各サーボモータの駆動される角速度が低下させられる。これにより、ロボットの制御点のみならず、各回転部に設定された監視部の速度を基準速度以下にすることができ、アームの移動速度を十分に抑制することができる。

さらに上記構成では、各監視部の速度を基準速度以下とすべく、全てのサーボモータの角速度を、制御点の位置及び姿勢の変化量を低下させることにより一括して低下させるため、全てのサーボモータの角速度を容易に低下させることもできる。

なお、速度算出手段としては、具体的には、第５の手段のように、前記各監視部の現在の位置と前記各監視部の動作周期後の位置との距離を前記動作周期で割ることにより前記各監視部の速度を算出するといった構成を採用することができる。

また、次周期監視位置算出手段としては、具体的には、第６の手段のように、前記角度算出手段により算出された前記各サーボモータの動作周期後の角度と、前記各回転部の大きさとに基づいて、前記各監視部の動作周期後の位置を算出するといった構成を採用することができる。

さらに、変化量算出手段としては、具体的には、第７の手段のように、前記各サーボモータの動作周期後の角度を順変換することで得られる前記制御点の動作周期後の位置及び姿勢と、前記制御点の現在の位置及び姿勢との差分を前記変化量として算出するといった構成を採用することができる。

第２の手段では、前記低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記変化量を低下させる。

上記構成では、算出された各監視部の速度のうち最大の速度と基準速度との比の値に基づいて、制御点の位置及び姿勢の変化量が低下させられる。上記比の値を用いることにより、基準速度に対する最大の速度の超過分を反映して上記変化量を低下させることができる。なお、最大の速度と基準速度との比の値とは、最大の速度を基準速度で割った値である（比の値＝最大の速度／基準速度）。

第３の手段では、前記低下手段は、前記変化量を前記比の値で割ることにより、前記変化量を低下させる。

上記構成では、制御点の位置及び姿勢の変化量を上記比の値で割るといった簡単な演算を用いるため、上記変化量を容易に低下させることができる。さらに上記構成では、制御点の位置及び姿勢の変化量を上記比の値で割るといった演算を用いるため、上記変化量を一定の法則から定まる比率で低下させることができ、上記変化量の低下分をオペレータが予測しやすくなる。

第４の手段では、前記各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、前記各回転部の前記監視部として設定する。

上記構成によれば、各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。このため、各回転部において、最も速度が高くなる可能性の高い部分を監視部に設定することができ、アームの移動速度を十分に抑制することができる。

第８の手段は、複数の回転部と、前記回転部を互いに回転可能に連結する関節と、前記各回転部を駆動するサーボモータと、を含むアームを備えるロボットに適用され、前記アームの先端部を制御点として設定し、ＰＴＰ制御により前記制御点の位置及び姿勢を制御するロボットの制御方法であって、前記各サーボモータの動作周期後の角度を算出する角度算出工程と、前記各回転部に設定された監視部の現在の位置を算出する現在監視位置算出工程と、前記角度算出工程により算出された角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出工程と、前記現在監視位置算出工程により算出された前記各監視部の現在の位置と、前記次周期監視位置算出工程により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する速度算出工程と、前記制御点の現在の位置及び姿勢を、前記制御点の動作周期後の位置及び姿勢まで制御するための前記制御点の位置及び姿勢の変化量を算出する変化量算出工程と、前記速度算出工程により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記各監視部の速度が前記基準速度以下となるように、前記変化量算出工程により算出された変化量を低下させる低下工程と、前記低下工程により低下させられた変化量と、前記制御点の現在の位置及び姿勢とに基づいて、前記制御点の動作周期後の位置及び姿勢を算出する位置姿勢算出工程と、前記位置姿勢算出工程により算出された位置及び姿勢を逆変換することで得られる前記各サーボモータの動作周期後の角度で、前記角度算出工程により算出された前記各サーボモータの動作周期後の角度を更新する更新工程と、前記各サーボモータの現在の角度が、前記更新工程によって更新された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動工程と、を備えることを特徴とする。

上記工程によれば、第１の手段と同様の作用効果を奏することができる。

ロボット、コントローラ、及びティーチングペンダントの概要を示す図。 ロボットの特定姿勢を示す正面図。 アームの速度抑制制御の処理手順を示すフローチャート。 サーボモータの角速度パターンを示すグラフ。

以下、垂直多関節型ロボットの制御装置に具体化した一実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。

はじめに、ロボット１０の概要を図１に基づいて説明する。

同図に示すように、ロボット１０は、回転部を互いに連結する各関節の回転中心軸線として、第１軸線Ｊ１、第２軸線Ｊ２、第３軸線Ｊ３、第４軸線Ｊ４、第５軸線Ｊ５、及び第６軸線Ｊ６を有する６軸ロボットである。これら各軸線における各部の動作角度は、それぞれサーボモータ等からなる駆動源の駆動、及び減速機等による減速を通じて調整される。サーボモータは、いずれも正逆両方向の回転が可能であり、サーボモータの駆動により原点位置を基準として各回転部が動作（駆動）する。各サーボモータには、その出力軸を制動する電磁ブレーキと、出力軸の回転角度に応じたパルス信号を出力するエンコーダとがそれぞれ設けられている。

ロボット１０は、床に設置されており、第１軸線Ｊ１が鉛直方向へ延びている。ロボット１０において、基台１１は、床等に固定される固定部１２と、その固定部１２の上方に設けられる回転部１３（第１回転部）とを有しており、回転部１３が第１軸線Ｊ１を回転中心として水平方向に回転可能になっている。すなわち、回転部１３は、第１軸線Ｊ１の方向に延びるとともに、固定部１２により第１軸線Ｊ１を中心として回転可能に支持されている。

下アーム１５（第２回転部）が、水平方向に延びる第２軸線Ｊ２を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、下アーム１５は、第１軸線Ｊ１に直交する平面に含まれる第２軸線Ｊ２から離れる方向へ延びるとともに、回転部１３により第２軸線Ｊ２を中心として回転可能に支持されている。下アーム１５は、基本姿勢として鉛直方向に延びる向きに設けられている。

下アーム１５の上端部には、上アーム１６が、水平方向に延びる第３軸線Ｊ３を回転中心として、時計回り方向又は反時計回り方向に回転可能に連結されている。すなわち、上アーム１６は、第２軸線Ｊ２に平行な第３軸線Ｊ３から離れる方向へ延びるとともに、下アーム１５により第３軸線Ｊ３を中心として回転可能に支持されている。上アーム１６は、基本姿勢として水平方向に延びる向きに設けられている。

上アーム１６は、基端側（回転の際に第３軸線Ｊ３を回転中心とする関節側）と先端側とで２つのアーム部に分割されて構成されており、基端側は第１上アーム１６Ａ（第３回転部）、先端側は第２上アーム１６Ｂ（第４回転部）となっている。第２上アーム１６Ｂは、上アーム１６の長手方向に延びる第４軸線Ｊ４を回転中心として、第１上アーム１６Ａに対してねじり方向に回転可能になっている。すなわち、第２上アーム１６Ｂは、第３軸線Ｊ３に直交する平面に含まれる第４軸線Ｊ４の方向に延びるとともに、第１上アーム１６Ａにより第４軸線Ｊ４を中心として回転可能に支持されている。

上アーム１６（詳しくは第２上アーム１６Ｂ）の先端部には、手首部１７（第５回転部）が設けられている。手首部１７は、水平方向に延びる第５軸線Ｊ５を回転中心として、第２上アーム１６Ｂに対して回転可能になっている。すなわち、手首部１７は、第４軸線Ｊ４に直交する第５軸線Ｊ５から離れる方向へ延びるとともに、第２上アーム１６Ｂにより第５軸線Ｊ５を中心として回転可能に支持されている。

手首部１７の先端部には、ワークやツール等を取り付けるためのハンド部１８（第６回転部）が設けられている。ハンド部１８は、その中心線である第６軸線Ｊ６を回転中心として、ねじり方向に回転可能になっている。すなわち、ハンド部１８は、第５軸線Ｊ５に直交する第６軸線Ｊ６の方向に延びるとともに、手首部１７により第６軸線Ｊ６を中心として回転可能に支持されている。以上のように、回転部１３、下アーム１５、上アーム１６、手首部１７、及びハンド部１８によって、ロボット１０のアームが構成されている。

コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、及び位置検出回路等を備えている。ＲＯＭは、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの回転角度を検出する。

ＣＰＵは、予め設定された動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、アーム先端部の制御点の位置及び姿勢を制御する。詳しくは、ＣＰＵは、ＰＴＰ（Point To Point）制御により、ロボット１０のアームにおける各関節の回転角度（アームの姿勢）を目標回転角度（目標姿勢）にフィードフォワード制御する。ＰＴＰ制御では、制御点を目標位置まで動作させる際に制御点の動作軌道（位置及び姿勢）が設定されない。本実施形態では、制御点として、アームのハンド部１８の中心点１８ａであるＴＣＰ（Tool Center Point）が設定されている。また、ＣＰＵは、ＴＣＰの位置及び姿勢に基づいて、この位置及び姿勢を実現するための各関節の角度を逆変換によって算出する機能と、各関節の角度に基づいて、ＴＣＰの位置及び姿勢を順変換によって算出する機能とを有する。

本実施形態では、コントローラ３０は、ロボット１０のティーチング時（手動操作時）において、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御を実行する。基準速度は、ＪＩＳやＩＳＯ等の規格により、例えば２５０ｍｍ／ｓに規定されている。

ティーチングペンダント４０（操作機）は、ＣＰＵ、ＲＯＭ、及びＲＡＭを含むマイクロコンピュータ、各種の手動操作キー、並びにディスプレイ４２等を備えている。ペンダント４０は、コントローラ３０に接続されており、コントローラ３０と通信可能となっている。オペレータ（使用者）は、このペンダント４０を手動操作して、ロボット１０の動作プログラムの作成、修正、登録、各種パラメータの設定を行うことができる。動作プログラムの修正等を行うティーチングでは、作業において制御点であるＴＣＰが通過する教示点を教示する。そして、オペレータは、コントローラ３０を通じて、ティーチングされた動作プログラムに基づきロボット１０を動作させることができる。換言すれば、コントローラ３０は、予め設定された動作プログラム及びペンダント４０の操作に基づいて、ロボット１０のアームの動作を制御する。

ここで、ロボット１０のティーチング時（手動操作時）において、ＴＣＰの移動速度を、基準速度以下となるように制御したとしても、ロボット１０の姿勢によっては、アームにおけるＴＣＰ以外の部分の移動速度が基準速度よりも高くなる場合があることに本願発明者は着目した。例えば、ロボット１０が図２に示す姿勢である場合、回転部１３を回転させると、ＴＣＰ（点Ｃ５）の移動速度は基準速度よりも十分に小さくなる。しかしながら、下アーム１５の先端部（点Ｃ２）及び上アーム１６の一方の端部（点Ｃ３）の移動速度が、基準速度よりも高くなる場合がある。

そこで、各回転部を回転させる際に回転中心となる関節（各回転部の回転中心軸線）から最も離れた部分を、各回転部の監視部（点Ｃ１〜Ｃ５）として設定する。例えば、下アーム１５を回転させる際に回転中心となる関節（回転部１３と下アーム１５との連結部）から最も離れた点Ｃ２を、下アーム１５の監視部として設定する。同様にして、上アーム１６を回転させる際に回転中心となる関節（下アーム１５と上アーム１６との連結部）から最も離れた点Ｃ３，Ｃ４を、上アーム１６の監視部として設定する等を行う。なお、上アーム１６等の回転部に他の部品（パーツ）が取り付けられている場合には、その部品の先端部等を監視部として設定してもよい。そして、全ての監視部の移動速度が基準速度以下となるように、各サーボモータの角速度を抑制する。

図３は、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御の処理手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期Ｔｒ毎に繰り返し実行される。動作周期Ｔｒ（制御周期）は、例えば８ｍｓである。

この一連の処理では、まず、各サーボモータの現在の角度θｋ１を検出する（Ｓ１１）。詳しくは、各サーボモータに設けられたエンコーダの検出信号に基づいて、位置検出回路により各サーボモータの現在の角度θｋ１を検出する。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。

続いて、各サーボモータの現在の角度θｋ１及び各回転部の大きさに基づいて、各監視部の現在の位置Ｐｉ１を算出する（Ｓ１２）。ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。現在の位置Ｐｉ１の算出手法の具体例について説明すると、まず、各回転部の大きさと各監視部の設定された位置とに基づいて、各回転部の回転中心から監視部までの距離を算出する。そして、各サーボモータの現在の角度θｋ１、各回転部の大きさ、及び上記距離を組み合わせることにより、点Ｃ１〜Ｃ５の位置を算出する。

続いて、各サーボモータの角速度ωｋを算出する（Ｓ１３）。具体的には、ティーチング時において、ＴＣＰが通過する点として教示された教示点に基づいて、各サーボモータの目標角度が算出されている。そして、例えば図４に示すように、その目標角度まで各サーボモータを駆動する際の角速度ωｋのパターンが設定されている。そこで、設定された角速度ωｋのパターンに基づいて、各サーボモータの現在の角速度ωｋを算出する。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。

続いて、各サーボモータの動作周期Ｔｒ後の角度θｋ２を算出する（Ｓ１４）。詳しくは、θｋ２＝θｋ１＋ωｋ×Ｔｒの式により、角度θｋ２を算出する。

続いて、各監視部の動作周期Ｔｒ後の位置Ｐｉ２を算出する（Ｓ１５）。ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。ここで、動作周期Ｔｒ後の位置Ｐｉ２は、Ｓ１２の処理と同様にして、各サーボモータの動作周期Ｔｒ後の角度θｋ２及び各回転部の大きさに基づいて算出すればよい。

続いて、各監視部の速度Ｖｉを算出する（Ｓ１６）。詳しくは、各監視部の現在の位置Ｐｉ１と動作周期Ｔｒ後の位置Ｐｉ２との距離を動作周期Ｔｒで割ることにより、速度Ｖｉを算出する。なお、ｉは、点Ｃ１〜Ｃ５にそれぞれ対応する１〜５の数字である。

続いて、各監視部の速度Ｖｉのうち最大の速度である最大速度Ｖｍｘを算出し（Ｓ１７）、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定する（Ｓ１８）。この判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いと判定した場合（Ｓ１８：ＹＥＳ）、最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αを算出する（Ｓ１９）。すなわち、α＝Ｖｍｘ／Ｖｌｍの式により、比の値αを算出する（α＞１）。

続いて、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２から、現在の位置及び姿勢Ｐ１を差し引いた値を、ＴＣＰの動作周期Ｔｒあたりの位置姿勢変化量ΔＰとして算出する（Ｓ２０）。ここでは、各サーボモータの現在の角度θｋ１を順変換することにより、ＴＣＰの現在の位置及び姿勢Ｐ１を算出し、各サーボモータの動作周期Ｔｒ後の角度θｋ２を順変換することにより、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を算出する。

続いて、位置姿勢変化量ΔＰを比の値αで割ることにより、位置姿勢変化量ΔＰを更新する（Ｓ２１）。

続いて、各サーボモータの動作周期Ｔｒ後の角度θｋ２を更新する（Ｓ２２）。詳しくは、まず、ＴＣＰの現在の位置及び姿勢Ｐ１と、更新された位置姿勢変化量ΔＰとの加算値で、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を更新する。そして、更新した位置及び姿勢Ｐ２を逆変換することで得られる各サーボモータの角度で、各サーボモータの動作周期Ｔｒ後の角度θｋ２を更新する。そして、更新された角度θｋ２を用いて、Ｓ１５の処理から再度実行する。

一方、Ｓ１８の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍ以下であると判定した場合（Ｓ１８：ＮＯ）、各サーボモータの角度θｋ２まで、動作周期Ｔｒ後に各サーボモータが駆動されるように各サーボモータを駆動する（Ｓ２３）。ここでは、Ｓ２２の処理を経由している場合、Ｓ２２の処理で更新された角度θｋ２まで、動作周期Ｔｒ後に各サーボモータが駆動されるように各サーボモータを駆動する。そして、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ１２の処理が現在監視位置算出手段としての処理（現在監視位置算出工程）に相当し、Ｓ１４の処理が角度算出手段としての処理（角度算出工程）に相当し、Ｓ１５の処理が次周期監視位置算出手段としての処理（次周期監視位置算出工程）に相当し、Ｓ１６の処理が速度算出手段としての処理（速度算出工程）に相当する。また、Ｓ２０の処理が変化量算出手段としての処理（変化量算出工程）に相当し、Ｓ２１の処理が低下手段としての処理（低下工程）に相当し、Ｓ２２の処理が位置姿勢算出手段，更新手段としての処理（位置姿勢算出工程，更新工程）に相当し、Ｓ２３の処理が駆動手段としての処理（駆動工程）に相当する。

以上詳述した本実施形態は、以下の利点を有する。

・各回転部に設定された監視部（点Ｃ１〜Ｃ５）の現在の位置Ｐｉ１、及び各監視部の動作周期Ｔｒ後の位置Ｐｉ２に基づいて、各監視部の速度Ｖｉが算出される。そして、算出された各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように、ＴＣＰの現在の位置及び姿勢Ｐ１を動作周期Ｔｒ後の位置及び姿勢Ｐ２まで制御するためのＴＣＰの位置姿勢変化量ΔＰが低下させられる。そして、低下させられた位置姿勢変化量ΔＰと、ＴＣＰの現在の位置及び姿勢Ｐ１との加算値が、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢として新たに算出され、新たに算出された位置及び姿勢Ｐ２を逆変換することで得られる各サーボモータの動作周期Ｔｒ後の角度で、各サーボモータの動作周期Ｔｒ後の角度θｋ２が更新される。そして、各サーボモータの現在の角度が、更新された角度θｋ２まで動作周期Ｔｒ後に制御されるように、各サーボモータが駆動される。このため、各サーボモータの動作周期Ｔｒあたりに駆動される角度が小さくなり、各サーボモータの駆動される角速度が低下させられる。これにより、ロボットのＴＣＰのみならず、各回転部に設定された監視部の速度を基準速度Ｖｌｍ以下にすることができ、アームの移動速度を十分に抑制することができる。

また、各監視部の速度Ｖｉを基準速度Ｖｌｍ以下とすべく、全てのサーボモータの角速度を、ＴＣＰの位置姿勢変化量ΔＰを低下させることにより一括して低下させるため、全てのサーボモータの角速度を容易に低下させることもできる。さらに、位置姿勢変化量ΔＰを低下させることで各サーボモータの角速度を低下させるため、例えばＴＣＰの動作軌道が直線である場合、ＴＣＰの動作軌道が当初の動作軌道（具体的には、速度抑制制御を行わない場合の動作軌道）からずれることを回避できる。

・最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αで位置姿勢変化量ΔＰを割ることにより、位置姿勢変化量ΔＰが低下させられる。位置姿勢変化量ΔＰを比の値αで割るといった簡単な演算を用いるため、位置姿勢変化量ΔＰを容易に低下させることができる。さらに、位置姿勢変化量ΔＰを比の値αで割るといった演算を用いるため、位置姿勢変化量ΔＰを一定の法則から定まる比率で低下させることができ、位置姿勢変化量ΔＰの低下分をオペレータが予測しやすくなる。

・各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。このため、各回転部において、最も速度が高くなる可能性の高い部分を監視部に設定することができ、アームの移動速度を十分に抑制することができる。

なお、上記実施形態を以下のように変更して実施することもできる。

・図３のＳ１８において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定したが、最大速度Ｖｍｘが、基準速度Ｖｌｍよりも若干高く設定した判定速度よりも高いか否か判定してもよい。この場合は、アームの速度抑制制御を迅速に終了することができる。

・図３のＳ２１において、位置姿勢変化量ΔＰを比の値αで割ることにより、位置姿勢変化量ΔＰを低下させたが、位置姿勢変化量ΔＰを比の値αよりも若干大きい値で割ることにより、位置姿勢変化量ΔＰを低下させてもよい。この場合も、アームの速度抑制制御を迅速に終了することができる。

・上記実施形態では、基準速度Ｖｌｍとして、ＪＩＳやＩＳＯ等の規格により規定された２５０ｍｍ／ｓを用いたが、それよりも若干低い速度、例えば２３０ｍｍ／ｓを基準速度Ｖｌｍとして用いてもよい。この場合、アームの移動速度を、確実かつ容易に２５０ｍｍ／ｓよりも低下させることができる。

・上記実施形態において、垂直多関節型のロボット１０に代えて、水平多関節型のロボット等を採用することもできる。

１０…ロボット、１３…回転部、１５…下アーム、１６…上アーム、１６Ａ…第１上アーム、１６Ｂ…第２上アーム、１７…手首部、１８…ハンド部、３０…コントローラ。

Claims (8)

1. 複数の回転部と、前記回転部を互いに回転可能に連結する関節と、前記各回転部を駆動するサーボモータと、を含むアームを備えるロボットに適用され、前記アームの先端部を制御点として設定し、ＰＴＰ制御により前記制御点の位置及び姿勢を制御するロボットの制御装置であって、
   前記各サーボモータの動作周期後の角度を算出する角度算出手段と、
   前記各回転部に設定された監視部の現在の位置を算出する現在監視位置算出手段と、
   前記角度算出手段により算出された角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出手段と、
   前記現在監視位置算出手段により算出された前記各監視部の現在の位置と、前記次周期監視位置算出手段により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する速度算出手段と、
   前記制御点の現在の位置及び姿勢を、前記制御点の動作周期後の位置及び姿勢まで制御するための前記制御点の位置及び姿勢の変化量を算出する変化量算出手段と、
   前記速度算出手段により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記各監視部の速度が前記基準速度以下となるように、前記変化量算出手段により算出された変化量を低下させる低下手段と、
   前記低下手段により低下させられた変化量と、前記制御点の現在の位置及び姿勢とに基づいて、前記制御点の動作周期後の位置及び姿勢を算出する位置姿勢算出手段と、
   前記位置姿勢算出手段により算出された位置及び姿勢を逆変換することで得られる前記各サーボモータの動作周期後の角度で、前記角度算出手段により算出された前記各サーボモータの動作周期後の角度を更新する更新手段と、
   前記各サーボモータの現在の角度が、前記更新手段によって更新された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動手段と、を備えることを特徴とするロボットの制御装置。
2. 前記低下手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記変化量を低下させる請求項１に記載のロボットの制御装置。
3. 前記低下手段は、前記変化量を前記比の値で割ることにより、前記変化量を低下させる請求項２に記載のロボットの制御装置。
4. 前記各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、前記各回転部の前記監視部として設定する請求項１〜３のいずれか１項に記載のロボットの制御装置。
5. 前記速度算出手段は、前記各監視部の現在の位置と前記各監視部の動作周期後の位置との距離を前記動作周期で割ることにより前記各監視部の速度を算出する請求項１〜４のいずれか１項に記載のロボットの制御装置。
6. 前記次周期監視位置算出手段は、前記角度算出手段により算出された前記各サーボモータの動作周期後の角度と、前記各回転部の大きさとに基づいて、前記各監視部の動作周期後の位置を算出する請求項１〜５のいずれか１項に記載のロボットの制御装置。
7. 前記変化量算出手段は、前記各サーボモータの動作周期後の角度を順変換することで得られる前記制御点の動作周期後の位置及び姿勢と、前記制御点の現在の位置及び姿勢との差分を前記変化量として算出する請求項１〜６のいずれか１項に記載のロボットの制御装置。
8. 複数の回転部と、前記回転部を互いに回転可能に連結する関節と、前記各回転部を駆動するサーボモータと、を含むアームを備えるロボットに適用され、前記アームの先端部を制御点として設定し、ＰＴＰ制御により前記制御点の位置及び姿勢を制御するロボットの制御方法であって、
   前記各サーボモータの動作周期後の角度を算出する角度算出工程と、
   前記各回転部に設定された監視部の現在の位置を算出する現在監視位置算出工程と、
   前記角度算出工程により算出された角度に基づいて、前記各監視部の動作周期後の位置を算出する次周期監視位置算出工程と、
   前記現在監視位置算出工程により算出された前記各監視部の現在の位置と、前記次周期監視位置算出工程により算出された前記各監視部の動作周期後の位置とに基づいて、前記各監視部の速度を算出する速度算出工程と、
   前記制御点の現在の位置及び姿勢を、前記制御点の動作周期後の位置及び姿勢まで制御するための前記制御点の位置及び姿勢の変化量を算出する変化量算出工程と、
   前記速度算出工程により算出された前記各監視部の速度のうち最大の速度が基準速度よりも高いことを条件として、前記各監視部の速度が前記基準速度以下となるように、前記変化量算出工程により算出された変化量を低下させる低下工程と、
   前記低下工程により低下させられた変化量と、前記制御点の現在の位置及び姿勢とに基づいて、前記制御点の動作周期後の位置及び姿勢を算出する位置姿勢算出工程と、
   前記位置姿勢算出工程により算出された位置及び姿勢を逆変換することで得られる前記各サーボモータの動作周期後の角度で、前記角度算出工程により算出された前記各サーボモータの動作周期後の角度を更新する更新工程と、
   前記各サーボモータの現在の角度が、前記更新工程によって更新された前記各サーボモータの角度まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる駆動工程と、を備えることを特徴とするロボットの制御方法。

Images