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

Description

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

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

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

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

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

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

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

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

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

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

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

ＣＰ制御において、制御点を目標まで動作させる際に動作軌道として、直線動作軌道を設定して、２つの動作点間を直線で補間することがある。

この点、第２の手段では、前記変化量算出手段は、前記制御点を目標まで動作させる際に前記動作軌道として直線動作軌道を設定して前記変化量を算出し、前記低下手段は、前記直線動作軌道上において、前記変化量算出手段により算出された前記変化量を低下させる。

上記構成によれば、制御点を目標まで動作させる際に動作軌道として直線動作軌道が設定されて、制御点の位置及び姿勢の変化量が算出され、直線動作軌道上において、算出された変化量が低下させられる。このため、各サーボモータの角速度を低下させたとしても、制御点が直線動作軌道から外れることを抑制することができる。

また、ＣＰ制御において、制御点を目標まで動作させる際に動作軌道として、曲線動作軌道を設定して、３つの動作点間を曲線で補間することがある。

この点、第３の手段では、前記変化量算出手段は、前記制御点を目標まで動作させる際に前記動作軌道として曲線動作軌道を設定して前記変化量を算出し、前記低下手段は、前記曲線動作軌道上において、前記変化量算出手段により算出された前記変化量を低下させる。

上記構成によれば、制御点を目標まで動作させる際に動作軌道として曲線動作軌道が設定されて、制御点の位置及び姿勢の変化量が算出され、曲線動作軌道上において、算出された変化量が低下させられる。このため、各サーボモータの角速度を低下させたとしても、制御点が曲線動作軌道から外れることを抑制することができる。

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

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

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

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

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

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

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

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

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

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

ロボット、コントローラ、及びティーチングペンダントの概要を示す図。 ロボットの特定姿勢を示す正面図。 アームの速度抑制制御の処理手順を示すフローチャート。 円弧動作軌道を示す模式図。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

また、ＣＰ制御では、ＴＣＰが特異点を通過する際にロボット１０の姿勢が急激に変化することがあり、この場合も上記点Ｃ２や上記点Ｃ３の移動速度が基準速度よりも高くなるおそれがある。さらに、ＴＣＰを動作軌道上で移動させる際に、関節の可動範囲を超えることで関節が急激に逆回転することがあり、この場合も上記点Ｃ２や上記点Ｃ３の移動速度が基準速度よりも高くなるおそれがある。

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

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

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

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

続いて、ＴＣＰの位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを算出する（Ｓ１３）。具体的には、ティーチング時において、ＴＣＰが通過する点として教示された教示点に基づいて、直線動作軌道を設定する。そして、直線動作軌道上における位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを算出する。

続いて、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を算出する（Ｓ１４）。詳しくは、ＴＣＰの現在の位置及び姿勢Ｐ１に、ＴＣＰの位置及び姿勢の変化量ΔＰを加算することにより、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を算出する（Ｐ２＝Ｐ１＋ΔＰ）。

続いて、各サーボモータの動作周期Ｔｒ後の角度θｋ２を算出する（Ｓ１５）。詳しくは、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を逆変換することで、各サーボモータの動作周期Ｔｒ後の角度θｋ２を算出する。

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

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

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

続いて、ＴＣＰの位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを比の値αで割った値を、新たな変化量ΔＰとする（Ｓ２１）。すなわち、比の値αに基づいて、変化量ΔＰを低下させる。ここでは、設定した直線動作軌道上において、ＴＣＰの位置及び姿勢の変化量ΔＰを低下させる。ＴＣＰの姿勢の変化量については、直線動作軌道上においてＴＣＰの位置の変化量を低下させる比率と同一の比率で低下させる。なお、ΔＰ＝Ｖ×Ｔｒであるため、ＴＣＰの位置及び姿勢の変化速度Ｖを比の値αに基づいて低下させると考えることもできる。そして、新たに算出された変化量ΔＰを用いて、Ｓ１４の処理から再度実行する。

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

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

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

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

・ＴＣＰの位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを低下させることで、全てのサーボモータの角速度を一括して低下させるため、全てのサーボモータの角速度を容易に低下させることもできる。したがって、各サーボモータの角速度を低下させる処理を簡素化することができる。

・ＴＣＰを目標まで動作させる際に動作軌道として直線動作軌道が設定されて、ＴＣＰの位置及び姿勢の変化量ΔＰが算出され、直線動作軌道上において、算出された変化量ΔＰが低下させられる。このため、各サーボモータの角速度を低下させたとしても、ＴＣＰが直線動作軌道から外れることを抑制することができる。

・最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値で位置及び姿勢の変化量ΔＰを割ることにより、位置及び姿勢の変化量ΔＰが低下させられる。このため、ＴＣＰの位置及び姿勢の変化量ΔＰを容易かつ適切に低下させることができる。

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

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

・ＣＰ制御において、ＴＣＰを目標まで動作させる際に動作軌道として、円弧動作軌道（曲線動作軌道）を設定して、３つの動作点間を円弧（曲線）で補間してもよい。図４に示すように、ＴＣＰの現在の位置及び姿勢Ｐ１から、点Ｏを中心とする円弧動作軌道により、動作周期Ｔｒ後に位置及び姿勢Ｐ２まで移動させる場合を例に説明する。ここでは、上記実施形態との相違点について説明する。

この場合、図３のＳ１３では、ＴＣＰの位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを算出する（Ｓ１３）。具体的には、ティーチング時において、ＴＣＰが通過する点として教示された教示点に基づいて、円弧動作軌道が設定される。そして、円弧動作軌道上における位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰが算出される。

また、図３のＳ２１では、ＴＣＰの位置及び姿勢の動作周期Ｔｒにおける変化量ΔＰを比の値αで割った値を、新たな変化量ΔＰとする（Ｓ２１）。すなわち、比の値αに基づいて、変化量ΔＰを低下させる。ここでは、設定した円弧動作軌道上において、ＴＣＰの位置及び姿勢の変化量ΔＰを低下させる。図４に示すように、位置及び姿勢Ｐ１のうちの位置Ｐ１ｐは、基準線ｚと線分ＯＰ１とのなす角度θ１を変数とする関数ｆにより、Ｐ１ｐ＝ｆ（θ１）で表される。同様にして、位置及び姿勢Ｐ２のうちの位置Ｐ２ｐ＝ｆ（θ２）で表される。ここで、Ｐ２ｐ＝ｆ（θ１＋Δθ）であるため、位置の変化量ΔＰｐ＝ｆ（θ１＋Δθ）−ｆ（θ１）で表される。このため、位置の変化量ΔＰｐを低下させるためには、角度の変化量Δθを低下させればよい。また、ＴＣＰの姿勢の変化量については、円弧動作軌道上においてＴＣＰの位置の変化量ΔＰｐを低下させる比率と同一の比率で低下させる。

そして、図３のＳ２２では、ＴＣＰの現在の位置及び姿勢Ｐ１が、ＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２まで動作周期Ｔｒ後に制御されるように各サーボモータを駆動する（Ｓ２２）。その際に、位置及び姿勢Ｐ１と位置及び姿勢Ｐ２との間は、円弧動作軌道により補間する。Ｓ２１の処理を経由している場合、Ｓ２１の処理で新たに算出された位置及び姿勢の変化量ΔＰ／αを現在の位置及び姿勢Ｐ１に加算した位置及び姿勢Ｐ３まで、動作周期Ｔｒ後に制御されるように各サーボモータを駆動する。ここで、位置及び姿勢Ｐ３のうちの位置Ｐ３ｐ＝ｆ（θ１＋Δθ／α）で表される（α＝Ｖｍｘ／Ｖｌｍ）。その他の処理は、上記実施形態と同様である。

上記構成によれば、ＴＣＰを目標まで動作させる際に動作軌道として円弧動作軌道（曲線動作軌道）が設定されて、ＴＣＰの位置及び姿勢の変化量ΔＰが算出され、円弧動作軌道上において、算出された変化量ΔＰが低下させられる。このため、各サーボモータの角速度を低下させたとしても、ＴＣＰが円弧動作軌道から外れることを抑制することができる。なお、曲線動作軌道として、円弧動作軌道に限らず、スプライン動作軌道（自由曲線動作軌道）等を採用することもできる。その場合も、上記制御と同様にして、媒介変数（角度θ等）の変化量（Δθ等）を低下させればよい。

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

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

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

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

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

Claims (8)

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

Images