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

Abstract

【課題】アームの移動速度を十分に抑制することのできるロボットの制御装置及び制御方法を提供する。
【解決手段】複数の回転部１３〜１８と、回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット１０、の制御装置３０であって、動作周期毎に各サーボモータを駆動させる角速度を算出する角速度算出手段と、各回転部に設定された監視部の速度を算出する速度算出手段と、速度算出手段により算出された各監視部の速度が基準速度以下となるように、各サーボモータの角速度を低下させる角速度低下手段と、角速度低下手段により低下させられた各サーボモータの角速度により、各サーボモータを駆動させる駆動手段と、を備える。
【選択図】 図１

Description

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

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

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

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

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

第１の手段は、複数の回転部と、前記回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット、の制御装置であって、動作周期毎に各サーボモータを駆動させる角速度を算出する角速度算出手段と、各回転部に設定された監視部の速度を算出する速度算出手段と、前記速度算出手段により算出された各監視部の前記速度が基準速度以下となるように、各サーボモータの前記角速度を低下させる角速度低下手段と、前記角速度低下手段により低下させられた各サーボモータの前記角速度により、各サーボモータを駆動させる駆動手段と、を備えることを特徴とする。

上記構成によれば、ロボットのアームは複数の回転部を含んでおり、回転部は関節により互いに回転可能に連結されている。そして、動作周期毎に各サーボモータを駆動させる角速度が算出され、その角速度で各サーボモータが駆動されることにより各回転部が駆動される。

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

この点、各回転部に設定された監視部の速度が算出される。そして、算出された各監視部の速度が基準速度以下となるように、各サーボモータの角速度が低下させられる。例えば、各回転部を回転させる際に回転中心となる関節から最も離れた部分が、各回転部の監視部として設定される。そして、低下させられた各サーボモータの角速度により、各サーボモータが駆動される。その結果、ロボットの制御点のみならず、各回転部に設定された監視部の速度を基準速度以下にすることができ、アームの移動速度を十分に抑制することができる。

第２の手段では、前記動作周期後の各サーボモータの角度を算出する角度算出手段を備え、前記速度算出手段は、各サーボモータの現在の角度、前記角度算出手段により算出された各サーボモータの前記動作周期後の角度、及び各回転部の大きさに基づいて、各回転部に設定された監視部の速度を算出する。

上記構成によれば、動作周期後の各サーボモータの角度が算出される。そして、各サーボモータの現在の角度、算出された各サーボモータの動作周期後の角度、及び各回転部の大きさに基づいて、各回転部に設定された監視部の速度を算出することができる。

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

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

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

第５の手段では、前記角速度低下手段は、前記速度算出手段により算出された各監視部の前記速度のうち最大の速度と前記基準速度との比の値に基づいて、各サーボモータの前記角速度を低下させる。

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

第６の手段では、前記角速度低下手段は、前記角速度算出手段により算出された各サーボモータの前記角速度を前記比の値で割ることにより、各サーボモータの前記角速度を低下させる。

上記構成によれば、算出された各サーボモータの角速度を上記比の値で割ることにより、各サーボモータの角速度が低下させられるため、各サーボモータの角速度を適切かつ容易に低下させることができる。

第７の手段では、仮想の動作周期後の各サーボモータの仮想の角度を算出する仮想角度算出手段と、各サーボモータの現在の角度、前記仮想角度算出手段により算出された各サーボモータの前記仮想の動作周期後の仮想の角度、及び各回転部の大きさに基づいて、各回転部に設定された前記監視部の仮想の速度を算出する仮想速度算出手段と、を備え、前記角速度低下手段は、前記仮想速度算出手段により算出された各監視部の前記仮想の速度のうち最大の速度が前記基準速度よりも高い場合に、前記仮想の動作周期を前記比の値で割って前記仮想の動作周期を短縮し、前記最大の速度が判定速度よりも低い場合に、前記仮想角度算出手段により算出された各サーボモータの前記仮想の角度まで、前記動作周期後に各サーボモータが駆動されるように各サーボモータの前記角速度を算出する。

上記構成によれば、仮想の動作周期後の各サーボモータの仮想の角度が算出される。そして、各サーボモータの現在の角度、算出された各サーボモータの仮想の動作周期後の仮想の角度、及び各回転部の大きさに基づいて、各回転部に設定された監視部の仮想の速度を算出することができる。

ここで、算出された各監視部の仮想の速度のうち最大の速度が基準速度よりも高い場合に、仮想の動作周期を上記比の値で割って仮想の動作周期が短縮される。このため、短縮された仮想の動作周期後の各サーボモータの仮想の角度が再度算出される。そして、この仮想の角度に基づいて再度算出された各監視部の仮想の速度のうち最大の速度が基準速度よりも低くなった場合に、各サーボモータの仮想の角度まで、動作周期後に各サーボモータが駆動されるように各サーボモータの角速度が算出される。したがって、動作周期後に各サーボモータが駆動される角度が小さくなり、各サーボモータの角速度を実質的に低下させることができる。しかも、仮想の動作周期を短縮することにより、全てのサーボモータの角速度を一括して低下させることができる。なお、仮想の動作周期が動作周期から短縮されていない場合は、各サーボモータの角速度が維持される。

また、背景技術のように動作目標位置を修正する構成にあっては、動作軌道が曲線である場合に軌道が変化するおそれがある。これに対して、上記のように仮想の動作周期を短縮する構成によれば、動作軌道を維持することができる。なお、判定速度は、基準速度と同一の速度であってもよいし、基準速度よりも若干高い速度等であってもよい。

第８の手段では、前記角速度低下手段により、前記仮想の動作周期を前記比の値で割って前記仮想の動作周期を短縮することが所定回数よりも多く繰り返された場合に、前記駆動手段による各サーボモータの駆動を禁止する駆動禁止手段を備える。

ロボットの制御装置においては、動作周期内に各サーボモータの角速度等を算出して指令を行う必要がある。この点、上記構成によれば、仮想の動作周期を上記比の値で割って仮想の動作周期を短縮することが所定回数よりも多く繰り返された場合に、各サーボモータの駆動が禁止される。このため、動作周期内に各サーボモータの角速度等を算出することができない場合は、サーボモータの駆動を禁止することができる。

第９の手段では、前記動作周期後の各サーボモータの角度を算出する角度算出手段を備え、前記角速度低下手段は、前記速度算出手段により算出された各監視部の前記速度のうち最大の速度が前記基準速度よりも高い場合に、前記動作周期に前記比の値を掛けて前記動作周期を延長して、前記角度算出手段により算出された各サーボモータの前記角度まで、延長された前記動作周期後に各サーボモータが駆動されるように、各サーボモータの前記角速度を低下させる。

上記構成によれば、算出された各監視部の速度のうち最大の速度が基準速度よりも高い場合に、動作周期に上記比の値を掛けて動作周期が延長される。そして、算出された各サーボモータの角度まで、延長された動作周期後に各サーボモータが駆動されるように、各サーボモータの角速度が低下させられる。このため、算出された各サーボモータの角度まで動作周期を延長して駆動することにより、全てのサーボモータの角速度を一括して低下させることができる。

また、背景技術のように動作目標位置を修正する構成にあっては、動作軌道が曲線である場合に軌道が変化するおそれがある。これに対して、上記のように動作周期を延長する構成によれば、動作軌道を維持することができる。

第１０の手段は、複数の回転部と、前記回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット、の制御方法であって、動作周期毎に各サーボモータを駆動させる角速度を算出する角速度算出工程と、各回転部に設定された監視部の速度を算出する速度算出工程と、前記速度算出工程により算出された各監視部の前記速度が基準速度よりも低くなるように、各サーボモータの前記角速度を低下させる角速度低下工程と、前記角速度低下工程により低下させられた各サーボモータの前記角速度により、各サーボモータを駆動させる駆動工程と、を備えることを特徴とする。

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

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

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

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

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

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

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

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

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

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

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

コントローラ３０（制御装置）は、ＣＰＵ、ＲＯＭ、ＲＡＭ、駆動回路、位置検出回路等を備えている。ＲＯＭは、ロボット１０のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、各エンコーダの検出信号がそれぞれ入力される。位置検出回路は、各エンコーダの検出信号に基づいて、各関節に設けられたサーボモータの回転角度を検出する。ＣＰＵは、予め設定された動作プログラム（プログラム）を実行することにより、位置検出回路から入力される位置情報に基づいて、ロボット１０のアームにおける各関節の回転角度（アームの姿勢）を目標回転角度（目標姿勢）にフィードバック制御する。本実施形態では、コントローラ３０は、ロボット１０のティーチング時（手動操作時）において、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御を実行する。基準速度は、ＪＩＳやＩＳＯ等の規格により、例えば２５０ｍｍ／ｓに規定されている。

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

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

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

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

まず、仮想動作周期Ｔｖを動作周期Ｔｒとする（Ｓ１１）。すなわち、仮想動作周期Ｔｖの初期値として動作周期Ｔｒを設定する。

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

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

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

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

続いて、各監視部の仮想動作周期Ｔｖ後の仮想位置Ｐｖｉを算出する（Ｓ１６）。詳しくは、Ｓ１３の処理と同様にして、各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋ及び各回転部の大きさに基づいて、各監視部の仮想動作周期Ｔｖ後の仮想位置Ｐｖｉを算出する。

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

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

続いて、仮想動作周期Ｔｖを比の値αで割った値を、新たな仮想動作周期Ｔｖとする（Ｓ２１）。すなわち、現在の仮想動作周期Ｔｖ（初期値は動作周期Ｔｒ）よりも、仮想動作周期Ｔｖを短縮する。そして、短縮された仮想動作周期Ｔｖを用いて、Ｓ１５の処理から再度実行する。

一方、Ｓ１９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも大きくないと判定した場合（Ｓ１９：ＮＯ）、各サーボモータの角速度ωｋを仮想動作周期Ｔｖに基づいて算出する（Ｓ２２）。具体的には、ωｋ←ωｋ×Ｔｖ／Ｔｒの式により、角速度ωｋを算出する。すなわち、各サーボモータの仮想角度θｖｋまで、動作周期Ｔｒ後に各サーボモータが駆動されるように各サーボモータの角速度ωｋを算出する。なお、仮想動作周期Ｔｖが動作周期Ｔｒから短縮されていない場合は、角速度ωｋが維持される。

続いて、算出された各サーボモータの角速度ωｋにより、各サーボモータを駆動させる（Ｓ２３）。そして、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ１４の処理が角速度算出手段としての処理（角速度算出工程）に相当し、Ｓ１５の処理が仮想角度算出手段（角度算出手段）としての処理に相当し、Ｓ１３〜Ｓ１７の処理が仮想速度算出手段（速度算出手段）としての処理（速度算出工程）に相当し、Ｓ２０，Ｓ２１，Ｓ２２の処理が角速度低下手段としての処理（角速度低下工程）に相当し、Ｓ２３の処理が駆動手段としての処理（駆動工程）に相当する。

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

・各回転部に設定された監視部（点Ｃ１〜Ｃ５）の速度Ｖｉが算出される。そして、算出された各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように、各サーボモータの角速度ωｋが低下させられる。低下させられた各サーボモータの角速度ωｋにより、各サーボモータが駆動される。その結果、ロボット１０の制御点のみならず、各回転部に設定された監視部の速度Ｖｉを基準速度Ｖｌｍ以下にすることができ、アームの移動速度を十分に抑制することができる。

・仮想動作周期Ｔｖ（動作周期Ｔｒ）後の各サーボモータの仮想角度θｖｋ（角度θａｋ）が算出される。そして、各サーボモータの現在の角度θｋ、算出された各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋ、及び各回転部の大きさに基づいて、各回転部に設定された監視部の速度Ｖｉを算出することができる。具体的には、各サーボモータの現在の角度θｋ及び各回転部の大きさに基づいて、各監視部の現在の位置Ｐｉを算出し、各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋ及び各回転部の大きさに基づいて、各監視部の仮想動作周期Ｔｖ（動作周期Ｔｒ）後の仮想位置Ｐｖｉ（位置Ｐａｉ）を算出し、各監視部の現在の位置Ｐｉと仮想動作周期Ｔｖ後の仮想位置Ｐｖｉとの距離を動作周期Ｔｒで割って速度Ｖｉを算出することができる。

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

・算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αに基づいて、各サーボモータの角速度ωｋが低下させられる。このため、速度が最大となる監視部の速度Ｖｉを基準速度Ｖｌｍ以下とするように、各サーボモータの角速度ωｋを適切に低下させることができる。

・算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも高い場合に、仮想動作周期Ｔｖを比の値αで割って仮想動作周期Ｔｖが短縮される。このため、短縮された仮想動作周期Ｔｖ後の各サーボモータの仮想角度θｖｋが再度算出される。そして、この仮想角度θｖｋに基づいて再度算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも低くなった場合に、各サーボモータの仮想角度θｖｋまで、動作周期Ｔｒ後に各サーボモータが駆動されるように各サーボモータの角速度ωｋが低下させられる。したがって、動作周期Ｔｒ後に各サーボモータが駆動される角度θａｋが小さくなり、各サーボモータの角速度ωｋを実質的に低下させることができる。しかも、仮想動作周期Ｔｖを短縮することにより、全てのサーボモータの角速度ωｋを一括して低下させることができる。

・背景技術のように動作目標位置を修正する構成にあっては、動作軌道が曲線である場合に軌道が変化するおそれがある。これに対して、上記のように仮想動作周期Ｔｖを短縮する構成によれば、動作軌道を維持することができる。

（第２実施形態）
第１実施形態ではロボット１０にＰＴＰ（Point To Point）動作をさせたが、第２実施形態ではロボット１０にＣＰ（Continuous Path）動作をさせる。以下、第１実施形態との相違点を中心に説明する。

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

まず、仮想動作周期Ｔｖを動作周期Ｔｒとする（Ｓ３１）。すなわち、仮想動作周期Ｔｖの初期値として動作周期Ｔｒを設定する。

続いて、アームの制御点（ハンド部１８の中心点１８ａ）の現在の位置及び姿勢ＰＰを検出する（Ｓ３２）。ティーチングした軌道に沿って制御点を移動させるＣＰ動作においては、現在の位置及び姿勢ＰＰが算出されている。

続いて、各サーボモータの現在の角度θｋを検出し（Ｓ３３）、各監視部の現在の位置Ｐｉを算出する（Ｓ３４）。Ｓ３３，Ｓ３４の処理は、図３のＳ１２，Ｓ１３の処理と同一である。

続いて、アームの制御点の位置及び姿勢の速度ＰＶを算出する（Ｓ３５）。具体的には、ティーチング時において教示された制御点の軌道に基づいて、制御点の目標位置及び目標姿勢が算出されている。そこで、制御点の目標位置及び目標姿勢に基づいて、制御点の位置及び姿勢の速度ＰＶを算出する。

続いて、仮想動作周期Ｔｖ後の制御点の仮想の位置及び姿勢ＰＰｖを算出する（Ｓ３６）。詳しくは、ＰＰｖ＝ＰＰ＋ＰＶ×Ｔｖの式により、仮想の位置及び姿勢ＰＰｖを算出する。

続いて、各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋを算出する（Ｓ３７）。
詳しくは、仮想動作周期Ｔｖ後の制御点の仮想の位置及び姿勢ＰＰｖに基づいて、各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋを算出する。

続いて、各監視部の仮想動作周期Ｔｖ後の仮想位置Ｐｖｉを算出し（Ｓ３８）、各監視部の速度Ｖｉを算出し（Ｓ３９）、各監視部の速度Ｖｉのうち最大の速度Ｖｉである最大速度Ｖｍｘを算出し（Ｓ４０）、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定する（Ｓ４１）。Ｓ３８〜Ｓ４１の処理は、図３のＳ１６〜Ｓ１９の処理と同一である。

Ｓ４１の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いと判定した場合（Ｓ４１：ＹＥＳ）、最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αを算出し（Ｓ４２）、仮想動作周期Ｔｖを比の値αで割った値を、新たな仮想動作周期Ｔｖとする（Ｓ４３）。Ｓ４２，Ｓ４３の処理は、図３のＳ２０，Ｓ２１の処理と同一である。そして、短縮された仮想動作周期Ｔｖを用いて、Ｓ３６の処理から再度実行する。

一方、Ｓ４１の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも大きくないと判定した場合（Ｓ４１：ＮＯ）、各サーボモータの現在の角度θｋから仮想角度θｖｋまで駆動する角速度ωｋを算出する（Ｓ４４）。詳しくは、ωｋ←（θｖｋ−θｋ）／Ｔｒの式により、角速度ωｋを算出する。すなわち、各サーボモータの仮想角度θｖｋまで、動作周期Ｔｒ後に各サーボモータが駆動されるように、各サーボモータの角速度ωｋを算出する。

続いて、算出された各サーボモータの角速度ωｋにより、各サーボモータを駆動させる（Ｓ４５）。そして、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ３７の処理が仮想角度算出手段（角度算出手段）としての処理に相当し、Ｓ３４〜Ｓ３９の処理が仮想速度算出手段（速度算出手段）としての処理（速度算出工程）に相当し、Ｓ４２，Ｓ４３，Ｓ４４の処理が角速度低下手段としての処理（角速度低下工程）に相当し、Ｓ４５の処理が駆動手段としての処理（駆動工程）に相当する。

以上詳述した本実施形態は、以下の利点を有する。ここでは、第１実施形態と相違する利点のみを述べる。

・仮想動作周期Ｔｖ（動作周期Ｔｒ）後の制御点の仮想の位置及び姿勢ＰＰｖに基づいて、各サーボモータの仮想動作周期Ｔｖ後の仮想角度θｖｋ（角度θａｋ）を算出することができる。

・算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも高い場合に、仮想動作周期Ｔｖを比の値αで割って仮想動作周期Ｔｖが短縮される。このため、短縮された仮想動作周期Ｔｖを用いて、制御点の仮想の位置及び姿勢ＰＰｖを再度算出することができる。したがって、ＰＴＰ動作及びＣＰ動作に関わらず、アームの移動速度を十分に抑制することができる。

なお、第１実施形態及び第２実施形態を、以下のように変更して実施することもできる。

・コントローラ３０（駆動禁止手段）は、仮想動作周期Ｔｖを比の値αで割って仮想動作周期Ｔｖを短縮すること（Ｓ２１，Ｓ４３）が所定回数よりも多く繰り返された場合に、各サーボモータの駆動を禁止してもよい。ロボット１０のコントローラ３０（制御装置）においては、動作周期Ｔｒ内に各サーボモータの角速度ωｋ等を算出して指令を行う必要がある。この点、上記構成によれば、動作周期Ｔｒ内に各サーボモータの角速度ωｋ等を算出することができない場合は、サーボモータの駆動を禁止することができる。なお、上記の所定回数として、例えば５回前後の回数を設定することができる。

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

・図３のＳ２１及び図５のＳ４３において、仮想動作周期Ｔｖを比の値αで割って仮想動作周期Ｔｖを短縮したが、仮想動作周期Ｔｖを比の値αよりも若干大きい値で割って仮想動作周期Ｔｖを短縮してもよい。この場合も、アームの速度抑制制御を迅速に終了することができる。

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

（第３実施形態）
第１実施形態及び第２実施形態では、仮想動作周期Ｔｖを短縮することで各サーボモータの角速度ωｋを低下させたが、第３実施形態では動作周期Ｔｒを延長することで各サーボモータの角速度ωｋを低下させる。以下、第１実施形態との相違点を中心に説明する。

図６は、ロボット１０のアームの移動速度を基準速度以下に抑制する速度抑制制御の処理手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期Ｔｒ毎に繰り返し実行される。なお、本実施形態では、動作周期Ｔｒが可変となっている。

まず、修正動作周期Ｔａを動作周期Ｔｒとする（Ｓ５１）。すなわち、修正動作周期Ｔａの初期値として修正前の動作周期Ｔｒを設定する。

続いて、各サーボモータの現在の角度θｋを検出し（Ｓ５２）、各監視部の現在の位置Ｐｉを算出し（Ｓ５３）、各サーボモータの角速度ωｋを算出する（Ｓ５４）。Ｓ５２〜Ｓ５４の処理は、図３のＳ１２〜Ｓ１４の処理と同一である。

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

続いて、各監視部の動作周期Ｔｒ後の位置Ｐａｉを算出する（Ｓ５６）。詳しくは、図３のＳ１３の処理と同様にして、各サーボモータの動作周期Ｔｒ後の角度θａｋ及び各回転部の大きさに基づいて、各監視部の動作周期Ｔｒ後の位置Ｐａｉを算出する。

続いて、各監視部の速度Ｖｉを算出する（Ｓ５７）。詳しくは、各監視部の現在の位置Ｐｉと動作周期Ｔｒ後の位置Ｐａｉとの距離を、動作周期Ｔｒで割って速度Ｖｉを算出する。

続いて、各監視部の速度Ｖｉのうち最大の速度Ｖｉである最大速度Ｖｍｘを算出し（Ｓ５８）、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いか否か判定する（Ｓ５９）。Ｓ５８，Ｓ５９の処理は、図３のＳ１８，Ｓ１９の処理と同一である。

Ｓ５９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも高いと判定した場合（Ｓ５９：ＹＥＳ）、最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αを算出し（Ｓ６０）、動作周期Ｔｒに比の値αを掛けた値を修正動作周期Ｔａとする（Ｓ６１）。そして、動作周期Ｔｒよりも延長された修正動作周期Ｔａを用いて、Ｓ６２の処理を実行する。

一方、Ｓ５９の判定において、最大速度Ｖｍｘが基準速度Ｖｌｍよりも大きくないと判定した場合も（Ｓ５９：ＮＯ）、Ｓ６２の処理を実行する。各サーボモータの角速度ωｋを修正動作周期Ｔａに基づいて算出する（Ｓ６２）。具体的には、ωｋ←ωｋ×Ｔｒ／Ｔａの式により、角速度ωｋを算出する。すなわち、各サーボモータの角度θａｋまで、修正動作周期Ｔａ後に各サーボモータが駆動されるように各サーボモータの角速度ωｋを算出する。なお、修正動作周期Ｔａが動作周期Ｔｒから延長されていない場合は、角速度ωｋが維持される。

続いて、算出された各サーボモータの角速度ωｋにより、修正動作周期Ｔａで各サーボモータを駆動させる（Ｓ６３）。そして、この一連の処理を一旦終了する（ＥＮＤ）。

なお、Ｓ５４の処理が角速度算出手段としての処理（角速度算出工程）に相当し、Ｓ５５の処理が角度算出手段としての処理に相当し、Ｓ５３〜Ｓ５７の処理が速度算出手段としての処理（速度算出工程）に相当し、Ｓ６０，Ｓ６１，Ｓ６２の処理が角速度低下手段としての処理（角速度低下工程）に相当し、Ｓ６３の処理が駆動手段としての処理（駆動工程）に相当する。

以上詳述した本実施形態は、以下の利点を有する。ここでは、第１実施形態と相違する利点のみを述べる。

・動作周期Ｔｒ後の各サーボモータの角度θａｋが算出される。そして、各サーボモータの現在の角度θｋ、算出された各サーボモータの動作周期Ｔｒ後の角度θａｋ、及び各回転部の大きさに基づいて、各回転部に設定された監視部の速度Ｖｉを算出することができる。具体的には、各サーボモータの現在の角度θｋ及び各回転部の大きさに基づいて、各監視部の現在の位置Ｐｉを算出し、各サーボモータの動作周期Ｔｒ後の角度θａｋ及び各回転部の大きさに基づいて、各監視部の動作周期Ｔｒ後の位置Ｐａｉを算出し、各監視部の現在の位置Ｐｉと動作周期Ｔｒ後の位置Ｐａｉとの距離を動作周期Ｔｒで割って速度Ｖｉを算出することができる。

・算出された各監視部の速度Ｖｉのうち最大速度Ｖｍｘが基準速度Ｖｌｍよりも高い場合に、動作周期Ｔｒに比の値αを掛けた修正動作周期Ｔａに動作周期が延長される。そして、算出された各サーボモータの角度θａｋまで、修正動作周期Ｔａ後に各サーボモータが駆動されるように、各サーボモータの角速度ωｋが低下させられる。このため、算出された各サーボモータの角度θａｋまで動作周期を延長して駆動することにより、全てのサーボモータの角速度ωｋを一括して低下させることができる。

・実質的に、算出された各サーボモータの角速度ωｋを比の値αで割ることにより、各サーボモータの角速度ωｋが低下させられるため、各サーボモータの角速度ωｋを適切かつ容易に低下させることができる。

・背景技術のように動作目標位置を修正する構成にあっては、動作軌道が曲線である場合に軌道が変化するおそれがある。これに対して、上記のように動作周期を延長する構成によれば、動作軌道を維持することができる。

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

・図６のＳ６１において、動作周期Ｔｒに比の値αを掛けて動作周期Ｔｒを延長したが、動作周期Ｔｒに比の値αよりも若干大きい値を掛けて動作周期Ｔｒを延長してもよい。また、基準速度Ｖｌｍとして、ＪＩＳやＩＳＯ等の規格により規定された２５０ｍｍ／ｓを用いたが、それよりも若干低い速度、例えば２３０ｍｍ／ｓを基準速度Ｖｌｍとして用いてもよい。これらの場合、アームの移動速度を、確実かつ容易に２５０ｍｍ／ｓよりも低下させることができる。

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

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

Claims (10)

1. 複数の回転部と、前記回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット、の制御装置であって、
   動作周期毎に各サーボモータを駆動させる角速度を算出する角速度算出手段と、
   各回転部に設定された監視部の速度を算出する速度算出手段と、
   前記速度算出手段により算出された各監視部の前記速度が基準速度以下となるように、各サーボモータの前記角速度を低下させる角速度低下手段と、
   前記角速度低下手段により低下させられた各サーボモータの前記角速度により、各サーボモータを駆動させる駆動手段と、
   を備えることを特徴とするロボットの制御装置。
2. 前記動作周期後の各サーボモータの角度を算出する角度算出手段を備え、
   前記速度算出手段は、各サーボモータの現在の角度、前記角度算出手段により算出された各サーボモータの前記動作周期後の角度、及び各回転部の大きさに基づいて、各回転部に設定された監視部の速度を算出する請求項１に記載のロボットの制御装置。
3. 前記速度算出手段は、各サーボモータの現在の角度及び各回転部の大きさに基づいて、各監視部の現在の位置を算出し、前記角度算出手段により算出された各サーボモータの前記動作周期後の角度及び各回転部の大きさに基づいて、各監視部の前記動作周期後の位置を算出し、各監視部の前記現在の位置と前記動作周期後の位置との距離を前記動作周期で割って前記速度を算出する請求項２に記載のロボットの制御装置。
4. 各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、各回転部の前記監視部として設定する請求項１〜３のいずれか１項に記載のロボットの制御装置。
5. 前記角速度低下手段は、前記速度算出手段により算出された各監視部の前記速度のうち最大の速度と前記基準速度との比の値に基づいて、各サーボモータの前記角速度を低下させる請求項１〜４のいずれか１項に記載のロボットの制御装置。
6. 前記角速度低下手段は、前記角速度算出手段により算出された各サーボモータの前記角速度を前記比の値で割ることにより、各サーボモータの前記角速度を低下させる請求項５に記載のロボットの制御装置。
7. 仮想の動作周期後の各サーボモータの仮想の角度を算出する仮想角度算出手段と、
   各サーボモータの現在の角度、前記仮想角度算出手段により算出された各サーボモータの前記仮想の動作周期後の仮想の角度、及び各回転部の大きさに基づいて、各回転部に設定された前記監視部の仮想の速度を算出する仮想速度算出手段と、を備え、
   前記角速度低下手段は、前記仮想速度算出手段により算出された各監視部の前記仮想の速度のうち最大の速度が前記基準速度よりも高い場合に、前記仮想の動作周期を前記比の値で割って前記仮想の動作周期を短縮し、前記最大の速度が判定速度よりも低い場合に、前記仮想角度算出手段により算出された各サーボモータの前記仮想の角度まで、前記動作周期後に各サーボモータが駆動されるように各サーボモータの前記角速度を算出する請求項５又は６に記載のロボットの制御装置。
8. 前記角速度低下手段により、前記仮想の動作周期を前記比の値で割って前記仮想の動作周期を短縮することが所定回数よりも多く繰り返された場合に、前記駆動手段による各サーボモータの駆動を禁止する駆動禁止手段を備える請求項７に記載のロボットの制御装置。
9. 前記動作周期後の各サーボモータの角度を算出する角度算出手段を備え、
   前記角速度低下手段は、前記速度算出手段により算出された各監視部の前記速度のうち最大の速度が前記基準速度よりも高い場合に、前記動作周期に前記比の値を掛けて前記動作周期を延長して、前記角度算出手段により算出された各サーボモータの前記角度まで、延長された前記動作周期後に各サーボモータが駆動されるように、各サーボモータの前記角速度を低下させる請求項５又は６に記載のロボットの制御装置。
10. 複数の回転部と、前記回転部を互いに回転可能に連結する関節と、各回転部を駆動するサーボモータと、を含むアームを備えるロボット、の制御方法であって、
    動作周期毎に各サーボモータを駆動させる角速度を算出する角速度算出工程と、
    各回転部に設定された監視部の速度を算出する速度算出工程と、
    前記速度算出工程により算出された各監視部の前記速度が基準速度よりも低くなるように、各サーボモータの前記角速度を低下させる角速度低下工程と、
    前記角速度低下工程により低下させられた各サーボモータの前記角速度により、各サーボモータを駆動させる駆動工程と、
    を備えることを特徴とするロボットの制御方法。

Images