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

Description

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

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

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

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

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

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

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

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

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

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

さらに上記構成では、各監視部の速度を基準速度以下とすべく、各サーボモータ（全てのサーボモータ）の角度の変化量が一律に低下される。このため、ＰＴＰ制御において制御点の位置及び姿勢（軌道）が制限されることなく、角速度が低下されるのみであるため、目標の位置及び姿勢から制御点がずれることなく制御することができる。

第２の手段では、前記変化量算出手段は、前記各サーボモータの動作開始タイミングを互いに一致させるとともに、前記各サーボモータの動作終了タイミングを互いに一致させるように、前記変化量を算出する。

上記構成によれば、各サーボモータの動作開始タイミングを互いに一致させるとともに、各サーボモータの動作終了タイミングを互いに一致させるように、各サーボモータの角度の変化量が算出される。そして、上記第１の手段を前提としているため、各監視部の速度を基準速度以下とすべく、全てのサーボモータの角度の変化量が一律に低下される。したがって、各サーボモータの角速度を低下させたとしても、各サーボモータの動作開始タイミング及び動作終了タイミングが互いに一致した状態を維持することができる。

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

上記構成では、算出された各監視部の速度のうち最大の速度と基準速度との比の値に基づいて、各サーボモータの角度の変化量が低下させられる。このため、速度が最大となる監視部の速度を基準速度以下とするように各サーボモータを駆動させる角速度を適切に低下させることができる。なお、最大の速度と基準速度との比の値とは、最大の速度を基準速度で割った値である（比の値＝最大の速度／基準速度）。

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

上記構成では、各サーボモータの角度の変化量を容易かつ適切に低下させることができる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

続いて、各サーボモータの角速の変化量Δθｋを算出する（Ｓ１３）。具体的には、ティーチング時において、ＴＣＰが通過する点として教示された教示点に基づいて、各サーボモータの目標角度が算出されている。そして、例えば図４に示すように、その目標角度まで各サーボモータを駆動する際の角速度ωｋのパターンが設定されている。各サーボモータの動作開始タイミングを互いに一致させるとともに、各サーボモータの動作終了タイミングを互いに一致させるように、角速度ωｋのパターンは設定されている。そこで、設定された角速度ωｋのパターンに基づいて、各サーボモータの現在の角速度ωｋを算出する。そして、角速度ωｋに動作周期Ｔｒを掛けることにより、各サーボモータの角速の動作周期Ｔｒにおける変化量Δθｋを算出する（Δθｋ＝ωｋ×Ｔｒ）。その結果、各サーボモータの動作開始タイミングを互いに一致させるとともに、各サーボモータの動作終了タイミングを互いに一致させるように、変化量Δθｋが算出される。なお、ｋは、第１軸線Ｊ１〜第６軸線Ｊ６にそれぞれ対応する１〜６の数字である。

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

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

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

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

続いて、各サーボモータの角度の動作周期Ｔｒにおける変化量Δθｋを比の値αで割った値を、新たな変化量Δθｋとする（Ｓ２１）。すなわち、各サーボモータの変化量Δθｋ（全てのサーボモータの変化量）を、比の値αに基づいて一律に低下させる。なお、Δθｋ＝ωｋ×Ｔｒであるため、各サーボモータの角速度ωｋを比の値αに基づいて一律に低下させると考えることもできる。そして、新たに算出された変化量Δθｋを用いて、Ｓ１４の処理から再度実行する。

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

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

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

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

・各監視部の速度Ｖｉを基準速度Ｖｌｍ以下とすべく、各サーボモータの角度の動作周期Ｔｒにおける変化量Δθｋ（全てのサーボモータの角度の変化量）が一律に低下される。このため、ＰＴＰ制御においてＴＣＰの位置及び姿勢（軌道）が制限されることなく、角速度ωｋが低下されるのみであるため、目標の位置及び姿勢からＴＣＰがずれることなく制御することができる。これに対して、特許文献１に記載のように、動作周期後の目標位置を修正して角速度を低下させる場合は、ＰＴＰ制御においてＴＣＰの位置及び姿勢（軌道）が制限されることとなる。このため、ＴＣＰを目標の位置及び姿勢に到達させるためには、何らかの補正処理が必要となる。その結果、動作周期後の目標位置を修正する処理と補正処理とが相反し、無限ループに陥るおそれもある。

・各サーボモータの動作開始タイミングを互いに一致させるとともに、各サーボモータの動作終了タイミングを互いに一致させるように、各サーボモータの角度の変化量Δθｋが算出される。そして、各監視部の速度Ｖｉを基準速度以下とすべく、各サーボモータの角度の変化量Δθｋが一律に低下される。したがって、各サーボモータの角速度ωｋを低下させたとしても、各サーボモータの動作開始タイミング及び動作終了タイミングが互いに一致した状態を維持することができる。

・最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αで角度の変化量Δθｋを割ることにより、変化量Δθｋが低下させられる。このため、各サーボモータの角度の変化量Δθｋを容易かつ適切に低下させることができる。

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

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

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

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

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

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

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

Claims (7)

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

Images