特許 6547319 ロボットの制御装置及び制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6547319

(24)【登録日】2019年7月5日

(45)【発行日】2019年7月24日

(54)【発明の名称】ロボットの制御装置及び制御方法

(51)【国際特許分類】

   B25J 9/10 20060101AFI20190711BHJP

【ＦＩ】

   B25J9/10 A

【請求項の数】9

【全頁数】15

(21)【出願番号】特願2015-24030(P2015-24030)

(22)【出願日】2015年2月10日

(65)【公開番号】特開2016-147323(P2016-147323A)

(43)【公開日】2016年8月18日

【審査請求日】2017年10月12日

(73)【特許権者】

【識別番号】501428545

【氏名又は名称】株式会社デンソーウェーブ

(74)【代理人】

【識別番号】100121821

【弁理士】

【氏名又は名称】山田 強

(74)【代理人】

【識別番号】100125575

【弁理士】

【氏名又は名称】松田 洋

(72)【発明者】

【氏名】井手 慧

(72)【発明者】

【氏名】川瀬 大介

(72)【発明者】

【氏名】香川 尚哉

【審査官】 松田 長親

(56)【参考文献】

【文献】 特開２０１０−１６７５１５（ＪＰ，Ａ）

【文献】 国際公開第０２／０６６２１０（ＷＯ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

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

【請求項2】

前記姿勢修正手段は、前記対象修正手段により修正された前記対象監視部の位置に基づいて、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置を実現できないと判定した場合に、前記動作軌道上において前記制御点の動作周期後の位置及び姿勢を修正し、
前記駆動手段は、前記制御点の現在の位置及び姿勢が、前記姿勢修正手段によって修正された前記制御点の位置及び姿勢まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる請求項１に記載のロボットの制御装置。

【請求項3】

前記姿勢修正手段は、前記対象修正手段により修正された前記対象監視部の位置に基づいて、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置を実現できないと判定した場合に、前記動作軌道上において、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置に最も近い実現可能な位置へ、前記制御点の動作周期後の位置を修正する請求項２に記載のロボットの制御装置。

【請求項4】

前記ロボットは、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部を備え、
前記対象修正手段は、前記アームにおいて前記対象監視部よりも前記支持部側に設けられた前記サーボモータの角度の変化量を低下させることにより、前記対象監視部の位置を修正する請求項１〜３のいずれか１項に記載のロボットの制御装置。

【請求項5】

前記対象修正手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記対象監視部の位置を修正する請求項１〜３のいずれか１項に記載のロボットの制御装置。

【請求項6】

前記各回転部を回転させる際に回転中心となる前記関節から最も離れた部分を、前記各回転部の前記監視部として設定する請求項１〜５のいずれか１項に記載のロボットの制御装置。

【請求項7】

前記速度算出手段は、前記各監視部の現在の位置と前記各監視部の動作周期後の位置との距離を前記動作周期で割ることにより前記各監視部の速度を算出する請求項１〜６のいずれか１項に記載のロボットの制御装置。

【請求項8】

前記次周期監視位置算出手段は、前記次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置及び姿勢を逆変換することで得られる前記各サーボモータの動作周期後の角度と、前記各回転部の大きさとに基づいて、前記各監視部の動作周期後の位置を算出する請求項１〜７のいずれか１項に記載のロボットの制御装置。

【請求項9】

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

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特許第３９９４４８７号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

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

【0005】

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

【課題を解決するための手段】

【0006】

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

【0007】

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

【0008】

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

【0009】

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

【0010】

続いて、修正された対象監視部の位置に基づいて、制御点の動作周期後の姿勢が修正される。そして、制御点の現在の位置及び姿勢が、算出された制御点の位置、及び修正された制御点の姿勢まで動作周期後に制御されるように、各サーボモータが駆動される。したがって、制御点の姿勢を修正することで、制御点の動作周期後の位置（制御点の速度）を維持しつつ各監視部の速度を基準速度以下に低下させることができる。特に、ロボットにピックアンドプレイス動作をさせる場合は、制御点（ロボット）の姿勢は重要ではなく、制御点の位置が重要であるため、上記制御が有効である。

【0011】

対象監視部の速度が基準速度以下となるように対象監視部の位置を修正した結果、制御点の動作周期後の位置を実現できなくなる場合がある。この場合には、制御点の姿勢だけでなく、制御点の位置も修正する必要がある。

【0012】

この点、第２の手段では、前記姿勢修正手段は、前記対象修正手段により修正された前記対象監視部の位置に基づいて、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置を実現できないと判定した場合に、前記動作軌道上において前記制御点の動作周期後の位置及び姿勢を修正し、前記駆動手段は、前記制御点の現在の位置及び姿勢が、前記姿勢修正手段によって修正された前記制御点の位置及び姿勢まで前記動作周期後に制御されるように、前記各サーボモータを駆動させる。

【0013】

上記構成によれば、修正された対象監視部の位置に基づいて、算出された制御点の動作周期後の位置を実現できないと判定された場合に、動作軌道上において制御点の動作周期後の位置及び姿勢が修正される。そして、制御点の現在の位置及び姿勢が、修正された制御点の位置及び姿勢まで動作周期後に制御されるように、各サーボモータが駆動される。このため、動作軌道上に制御点の位置を維持しつつ、制御点の位置及び姿勢の制御を継続することができる。

【0014】

第３の手段では、前記姿勢修正手段は、前記対象修正手段により修正された前記対象監視部の位置に基づいて、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置を実現できないと判定した場合に、前記動作軌道上において、次周期位置姿勢算出手段により算出された前記制御点の動作周期後の位置に最も近い実現可能な位置へ、前記制御点の動作周期後の位置を修正する。

【0015】

上記構成によれば、修正された対象監視部の位置に基づいて、算出された制御点の動作周期後の位置を実現できないと判定された場合に、動作軌道上において、算出された制御点の動作周期後の位置に最も近い実現可能な位置へ、制御点の動作周期後の位置が修正される。したがって、制御点の動作周期後に実現可能な位置のうち、算出された制御点の動作周期後の位置に最も近い位置へ制御点を移動させることができ、制御点の速度の低下を最小限に留めることができる。

【0016】

アームの両端部のうちアームの先端部とは反対側を動作可能に支持する支持部を備えるロボットにおいて、対象監視部の速度を基準速度以下にすべく対象監視部の位置を修正するためには、アームにおいて対象監視部よりも支持部側の回転部の速度を低下させればよい。

【0017】

この点、第４の手段では、前記ロボットは、前記アームの両端部のうち前記アームの先端部とは反対側を動作可能に支持する支持部を備え、前記対象修正手段は、前記アームにおいて前記対象監視部よりも前記支持部側に設けられた前記サーボモータの角度の変化量を低下させることにより、前記対象監視部の位置を修正する。

【0018】

上記構成によれば、アームにおいて対象監視部よりも支持部側に設けられたサーボモータの角度の変化量を低下させることにより、対象監視部の位置が修正される。したがって、角度の変化量を低下させるサーボモータの数を抑制しつつ、効率的に対象監視部の速度を低下させることができる。

【0019】

第５の手段では、前記対象修正手段は、前記最大の速度と前記基準速度との比の値に基づいて、前記対象監視部の位置を修正する。

【0020】

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

【0021】

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

【0022】

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

【0023】

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

【0024】

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

【0025】

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

【0026】

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

【図面の簡単な説明】

【0027】

【図1】ロボット、コントローラ、及びティーチングペンダントの概要を示す図。

【図2】ロボットの特定姿勢を示す正面図。

【図3】第１実施形態におけるアームの速度抑制制御の処理手順を示すフローチャート。

【図4】対象監視部の位置修正及びＴＣＰの姿勢修正を示す図。

【図5】対象監視部の位置修正、ＴＣＰの位置及び姿勢修正を示す模式図。

【図6】第２実施形態におけるアームの速度抑制制御の処理手順を示すフローチャート。

【発明を実施するための形態】

【0028】

（第１実施形態）
以下、垂直多関節型ロボットの制御装置に具体化した第１実施形態について、図面を参照しつつ説明する。本実施形態のロボットは、例えば産業用ロボットとして機械組立工場などの組立システムにて用いられる。そして、ロボットは、ワークを取り上げて所定位置に置くピックアンドプレイス動作を実行する。

【0029】

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

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

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

【0035】

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

【0036】

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

【0037】

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

【0038】

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

【0039】

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

【0040】

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

【0041】

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

【0042】

また、ＣＰ制御では、ＴＣＰが特異点を通過する際にロボット１０の姿勢が急激に変化することがあり、この場合も上記点Ｃ２や上記点Ｃ３の移動速度が基準速度よりも高くなるおそれがある。

【0043】

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

【0044】

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

【0045】

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

【0046】

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

【0047】

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

【0048】

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

【0049】

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

【0050】

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

【0051】

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

【0052】

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

【0053】

続いて、比の値αに基づいて対象監視部の動作周期Ｔｒ後の位置を修正する（Ｓ２１）。具体的には、対象監視部の現在位置から動作周期Ｔｒ後の位置までの移動量を低減する。詳しくは、対象監視部の現在位置と動作周期Ｔｒ後の位置との距離を比の値αで割った値を、新たな移動量とする。ここで、対象監視部の速度を基準速度以下にすべく対象監視部の位置を修正する際には、アームにおいて対象監視部よりも固定部１２側の回転部の速度が低下される。その結果、アームにおいて対象監視部よりも固定部１２側に設けられたサーボモータである対象サーボモータの角度θの変化量Δθが低下される。

【0054】

続いて、ＴＣＰの動作周期Ｔｒ後の姿勢を修正する（Ｓ２２）。詳しくは、修正された対象監視部の位置に基づいて、ＴＣＰの動作周期Ｔｒ後の位置を実現するようにＴＣＰの動作周期Ｔｒ後の姿勢を修正する。対象監視部の位置からＴＣＰの動作周期Ｔｒ後の位置を実現するためには、アームにおいて対象監視部よりも先端側の回転部の回転量を変更すればよい。このため、アームにおいて対象監視部よりも先端側に設けられたサーボモータの角度の変化量を変更することにより、ＴＣＰの動作周期Ｔｒ後の位置を実現する。そして、対象監視部の動作周期Ｔｒ後の修正された位置及びＴＣＰの動作周期Ｔｒ後の修正された姿勢により定まる、各サーボモータの動作周期Ｔｒ後の角度θｋ２を用いて、Ｓ１６の処理から再度実行する。

【0055】

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

【0056】

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

【0057】

図４は、対象監視部の位置修正及びＴＣＰの姿勢修正を示す図である。現在の動作周期において、ロボット１０の状態（ＴＣＰの位置及び姿勢）が図４（ａ）に示す状態であったとする。そして、次の動作周期において、図４（ｂ）に示す状態まで移行するように各サーボモータの駆動量が算出された際に、対象監視部の速度が基準速度Ｖｌｍを超える場合がある。この場合、図３に示す速度抑制制御によれば、次の動作周期における対象監視部の位置が、図４（ｃ）に示す位置に修正される。これにより、対象監視部の速度が基準速度Ｖｌｍ以下に低下させられる。そして、修正された対象監視部の位置を起点として、図４（ｂ）に示すＴＣＰの位置を次の動作周期で実現するように、アームにおいて対象監視部よりも先端側の回転部の駆動量が修正される。その結果、ＴＣＰの次の動作周期における位置を維持しつつ、ＴＣＰの次の動作周期における姿勢のみが変更される。

【0058】

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

【0059】

・各回転部に設定された監視部（点Ｃ１〜Ｃ５）の現在の位置Ｐｉ１、及び各監視部の動作周期Ｔｒ後の位置Ｐｉ２に基づいて、各監視部の速度Ｖｉが算出される。そして、算出された各監視部の速度Ｖｉが基準速度Ｖｌｍ以下となるように、最大の速度となる監視部である対象監視部の位置が修正される。そして、修正された対象監視部の位置に基づいて、ＴＣＰの動作周期Ｔｒ後の姿勢が修正される。ＴＣＰの現在の位置及び姿勢Ｐ１が、算出されたＴＣＰの位置、及び修正されたＴＣＰの姿勢まで動作周期Ｔｒ後に制御されるように、各サーボモータが駆動される。したがって、ＴＣＰの姿勢を修正することで、ＴＣＰの動作周期Ｔｒ後の位置（ＴＣＰの速度）を維持しつつ各監視部の速度を基準速度Ｖｌｍ以下に低下させることができる。特に、ロボット１０にピックアンドプレイス動作をさせる場合は、ＴＣＰ（ロボット）の姿勢は重要ではなく、ＴＣＰの位置が重要であるため、上記制御が有効である。

【0060】

・対象監視部の現在位置から動作周期Ｔｒ後の位置までの移動量が低減される。詳しくは、対象監視部の現在位置と動作周期Ｔｒ後の位置との距離を比の値αで割った値が、新たな移動量とされる。その結果、アームにおいて対象監視部よりも固定部１２側に設けられた対象サーボモータの角度θの変化量Δθが低下される。したがって、角度θの変化量Δθを低下させる対象サーボモータの数を抑制しつつ、効率的に対象監視部の速度を低下させることができる。

【0061】

・最大速度Ｖｍｘと基準速度Ｖｌｍとの比の値αに基づいて、対象監視部の位置が修正される。このため、速度が最大となる対象監視部の速度を基準速度Ｖｌｍ以下とするように、対象監視部の速度を適切に低下させることができる。

【0062】

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

【0063】

（第２実施形態）
以下、第２実施形態について、第１実施形態との相違点を中心に説明する。本実施形態では、修正された対象監視部の位置に基づいて、算出されたＴＣＰの動作周期Ｔｒ後の位置を実現できないと判定した場合に、動作軌道上においてＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２を修正する。

【0064】

図５は、対象監視部の位置修正、ＴＣＰの位置及び姿勢修正を示す模式図である。図５（ａ）に示すように、現在の動作周期でのＴＣＰの位置から、次の動作周期でのＴＣＰの位置へ移動させる際に、対象監視部の速度が基準速度Ｖｌｍを超えたとする。この場合、上記第１実施形態によれば、図５（ｂ）に示すように対象監視部の位置が修正される。

【0065】

ここで、対象監視部の速度が基準速度Ｖｌｍ以下となるように対象監視部の位置を修正した結果、ＴＣＰの次の動作周期での位置を実現できなくなる場合がある。詳しくは、修正された対象監視部の位置を起点とした場合に、ＴＣＰの次の動作周期での位置までＴＣＰが届かなくなる場合がある。この場合には、ＴＣＰの姿勢だけでなく、ＴＣＰの位置も修正する必要がある。

【0066】

そこで、本実施形態では、図５（ｃ）に示すように、動作軌道上において実現可能なＴＣＰの位置へ次の動作周期でのＴＣＰの位置を修正する。そして、その修正されたＴＣＰ位置を実現するように、修正された対象監視部の位置を起点としてＴＣＰの姿勢を修正する。

【0067】

図６は、本実施形態の速度抑制制御の処理手順を示すフローチャートである。この一連の処理は、コントローラ３０によって、アームを動作させる動作周期Ｔｒ毎に繰り返し実行される。動作周期Ｔｒ（制御周期）は、例えば８ｍｓである。ここでは、制御点を目標まで動作させる際に動作軌道として、直線動作軌道を設定する場合（直線補間）を例に説明する。なお、図３のフローチャートと同一の処理については、同一のステップ番号を付すことにより説明を省略する。Ｓ１１〜Ｓ２１の処理は、図３のＳ１１〜Ｓ２１の処理と同一である。

【0068】

続いて、修正された対象監視部の位置に基づいて、ＴＣＰの動作周期Ｔｒ後の位置を実現可能であるか否か判定する（Ｓ２１ａ）。この判定において、ＴＣＰの動作周期Ｔｒ後の位置を実現可能であると判定した場合（Ｓ２１ａ：ＹＥＳ）、ＴＣＰの動作周期Ｔｒ後の位置を修正することなく、ＴＣＰの動作周期Ｔｒ後の姿勢を修正する（Ｓ２２）。Ｓ２２の処理は図３のＳ２２の処理と同一であり、この場合は上記第１実施形態と同様の制御となる。

【0069】

一方、Ｓ２１ａの判定において、ＴＣＰの動作周期Ｔｒ後の位置を実現可能でないと判定した場合（Ｓ２１ａ：ＮＯ）、ＴＣＰの動作周期Ｔｒ後の位置を修正する（Ｓ２１ｂ）。詳しくは、ＴＣＰの動作軌道上において、Ｓ１４の処理で算出されたＴＣＰの動作周期Ｔｒ後の位置に最も近い実現可能な位置へ、ＴＣＰの動作周期Ｔｒ後の位置を修正する。例えば、修正された対象監視部の位置を中心として、アームにおいて対象監視部よりも先端側の回転部を最も伸ばした状態でＴＣＰを移動させて描かれる球面と、動作軌道との交点をＴＣＰの動作周期Ｔｒ後の位置とする。その後、修正された対象監視部の位置に基づいて、ＴＣＰの動作周期Ｔｒ後の姿勢を修正する（Ｓ２２）。Ｓ２２の処理は、図３のＳ２２の処理と同一である。なお、Ｓ２１ｂの処理が姿勢修正手段としての処理に相当する。

【0070】

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

【0071】

・修正された対象監視部の位置に基づいて、算出されたＴＣＰの動作周期Ｔｒ後の位置を実現できないと判定された場合に、動作軌道上においてＴＣＰの動作周期Ｔｒ後の位置及び姿勢Ｐ２が修正される。そして、ＴＣＰの現在の位置及び姿勢Ｐ１が、修正された制御点の位置及び姿勢まで動作周期Ｔｒ後に制御されるように、各サーボモータが駆動される。このため、動作軌道上にＴＣＰの位置を維持しつつ、ＴＣＰの位置及び姿勢の制御を継続することができる。

【0072】

・修正された対象監視部の位置に基づいて、算出されたＴＣＰの動作周期Ｔｒ後の位置を実現できないと判定された場合に、動作軌道上において、算出されたＴＣＰの動作周期Ｔｒ後の位置に最も近い実現可能な位置へ、ＴＣＰの動作周期Ｔｒ後の位置が修正される。したがって、ＴＣＰの動作周期Ｔｒ後に実現可能な位置のうち、算出されたＴＣＰの動作周期Ｔｒ後の位置に最も近い位置へＴＣＰを移動させることができ、ＴＣＰの速度の低下を最小限に留めることができる。

【0073】

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

【0074】

・ＣＰ制御において、ＴＣＰを目標まで動作させる際に動作軌道として、円弧動作軌道を設定して、２つの動作点間を円弧で補間してもよい。上記第２実施形態では、設定した円弧動作軌道上において、ＴＣＰの位置の変化量を低下させればよい。そして、位置及び姿勢Ｐ１と修正後の位置及び姿勢との間は、円弧動作軌道により補間する。こうした構成によれば、各サーボモータの角速度を低下させたとしても、ＴＣＰが円弧動作軌道から外れることを抑制することができる。

【0075】

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

【0076】

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

【0077】

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

【0078】

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

【符号の説明】

【0079】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】