JP5346217B2 - 多軸ロボット及びその速度制御装置 - Google Patents

Description

本発明は各種作業用のツール部材を取り付けてなる多軸ロボットに関し、より詳しくは、表面部位の移動速度を所定速度以下に規制するよう動作する多軸ロボット及びその速度制御装置に関する。

多軸ロボットは、例えば搬送作業、塗装作業又は溶接作業等の自動化のため、生産分野において広く利用され、その先端部位には各種作業用のツール部材が取り付けられる。多軸ロボットは各回転軸周りの回転を互いに独立して行い得るよう構成され、各回転軸の回転位置の制御によりツール部材を所望位置に所望姿勢で停止させ、各回転軸の角速度の制御によりツール部材を所望経路に沿って所望速度で移動させることができる。

多軸ロボットには多様な動作モード、例えばティーチモード、チェックモード及びリピートモード等がある。ティーチモードは移動開始位置及び移動終了位置を制御装置側に教示するための動作モードであり、このモードではロボットをマニュアル操作により動作させる。チェックモードは、ティーチモードで教示した位置間の移動が適正に行われ得るか否かをチェックするための動作モードであり、これら位置間の移動を予め定めた動作計画に従って自動的に行わせる。リピートモードは、チェックを終了して実用に供する際に使用される動作モードであり、このモードではロボットを自動的且つ継続的に動作させる。

ティーチモード及びチェックモードは、実用に供する前に実行される試験的な動作モードであるので、これら動作モードの実行中には多軸ロボットの表面部位の移動速度を所定の制限移動速度以下に規制することが好ましい。このため、従来、これら動作モードの実行中に多軸ロボットがどのような姿勢になっていてもツール部材の移動速度が制限移動速度以下となるような回転軸の許容角速度を予め設定しておき、これら動作モードでロボットが動作している場合の回転軸の角速度をこの許容角速度以下に規制する制御が行われている（例えば特許文献１参照）。

特開平９−１９３０６０号公報

しかし、このような許容角速度が設定されていると、多軸ロボットの姿勢によっては表面部位の移動速度が制限移動速度を大きく下回ることがある。そのため、動作開始位置及び動作終了位置の教示や、ロボットの自動移動のチェックに要する時間が長くなってしまう。また、例えば長尺のツール部材が取り付けられた場合、ロボットの先端部位の移動速度が制限移動速度以下に規制されていても、ツール部材の端点の移動速度が制限移動速度を超えるおそれがある。

そこで本発明は、多軸ロボットの表面部位の移動速度を適正に規制しつつもその移動速度をなるべく高くすることを目的としている。

本発明はかかる事情に鑑みてなされたものであり、本発明に係る多軸ロボットは、基台に連設された複数の連結部材の先端にツール部材を取り付けてなり、前記基台及び前記複数の連結部材が互いに相対回転可能な垂直型の多軸ロボットであって、前記ツール部材の移動時に各回転軸の角速度を制御する速度制御装置を備え、前記速度制御装置が、前記複数の連結部材及び前記ツール部材において予め設定された複数の表面部位の移動速度のうち最高移動速度が所定の制限移動速度となるような当該回転軸の角速度を求め、前記複数の連結部材は、前記ツール部材が取り付けられる連結部材を含む手首装置を構成すると共にその回転軸が手首軸をなす第１部材群と、前記基台を前記手首装置に接続すると共にその回転軸が主軸をなす第２部材群とを含み、各手首軸の角速度を求めるに際して前記速度制御装置は、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、前記ツール部材の１以上の端点と、当該手首軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該手首軸方向に見たときに当該手首軸から軸直交方向に最も遠位にある表面部位と、を用い、各主軸の角速度を求めるに際して前記速度制御装置は、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、当該主軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該主軸方向に見たときに当該主軸から軸直交方向に最も遠位にある表面部位とを用い、前記ツール部材のいずれの端点をも前記複数の表面部位として用いないことを特徴としている。

このような構成とすることにより、ロボットの移動時に各回転軸の角速度を求めるに際し、複数の表面部位を考慮した上でこれら表面部位の最高移動速度が制限移動速度となるようにしている。そのため、表面部位の移動速度を適正に規制した上でロボットの動作性をなるべく高くすることができる。なお、移動速度が制限移動速度以下となるよう考慮される部位は予め定めた複数の表面部位に限られているため、速度制御装置が実行する処理の内容が煩雑になるのを避けることができる。

前記予め設定された複数の表面部位に、前記ツール部材の中心位置、及び前記ツール部材が取り付けられる連結部材の中心位置が含まれ、前記予め設定された複数の表面部位に、前記ツール部材の１以上の端点が含まれていると、ツール部材として例えば長尺のものや、突出部分を有した特異な形状のものを取り付けた場合においても、このツール部材も含めた表面部位の全てを制限移動速度以下に制限することができる。また、ツール部材が折り畳まれたような姿勢となっているときには、連結部材の中心位置を制限移動速度で移動させることができ、ロボットの動作性が悪くなるのを避けることができる。

予め設定された前記複数の表面部位に、何れかの回転軸周りに回転する部材のうち基台に最も近い部材の表面部位であって当該回転軸方向に見たときに当該回転軸から軸直交方向に最も遠位にある表面部位が含まれていると、ツール部材が当該回転軸周りに回転する部材の表面部位よりも回転軸寄りに位置する場合であっても、この表面部位の移動速度を制限移動速度以下に規制することができる。

前記第２部材群は、前記基台に対して第１回転軸周りに回転可能に連結された第１の連結部材と、前記第１の連結部材に対して第２回転軸周りに回転可能に連結された第２の連結部材と、前記第２の連結部材に対して第３回転軸周りに回転可能に連結された第３の連結部材とを含み、前記第１乃至第３回転軸が前記主軸をなしており、前記第１回転軸の角速度を求めるに際して前記速度制限装置は、前記予め設定された複数の表面部位として、更に前記第２及び第３の連結部材のジョイントの表面部位を用いることが好ましい。第２回転軸の回転角度位置によっては、第２の連結部材が第１回転軸方向に見て第１の連結部材に対して軸直交方向外側に位置する可能性がある。すなわち、第２の連結部材と第３の連結部材のジョイントから第１回転軸までの距離が、ツール部材の端点から第１回転軸までの距離や、第１の連結部材の表面部位から第１回転軸までの距離よりも大きくなる可能性がある。本構成によれば、ジョイントの表面部位が第１回転軸から最遠となる場合においても、このジョイントの表面部位の移動速度を制限移動速度以下に規制することができるようになる。

前記第１の連結部材と、前記ジョイントとにそれぞれ揺動可能に連結されて前記第２の連結部材と平行に並ぶ平行部材を備え、前記第１回転軸の角速度を求めるための複数の表面部位に、前記平行部材と前記ジョイントとの連結部分の表面部位が含まれていてもよい。このような平行部材を備えた場合、第２回転軸の回転角度位置に応じて、ジョイントは、第１回転軸に対して軸直交方向外側に位置することもあれば、軸直交方向内側に位置することもある。本構成によれば、平行部材とジョイントとの連結部分が、ジョイントよりも第１回転軸から軸直交方向外側に位置するようなことがあっても、この連結部分の移動速度を制限移動速度以下に規制することができるようになる。

また、本発明に係る多軸ロボットの速度制御装置は、基台に連設された複数の連結部材の先端にツール部材を取り付けてなり前記基台及び前記複数の連結部材が互いに相対回転可能に構成された垂直型の多軸ロボットに備えられ、前記ツール部材の移動時に各回転軸の角速度を制御する多軸ロボットの速度制御装置であって、前記複数の連結部材及び前記ツール部材の表面部位において予め設定された複数の表面部位の移動速度のうち最高移動速度が所定の制限移動速度となるように当該回転軸の角速度を求め、前記複数の連結部材は、前記ツール部材が取り付けられる連結部材を含む手首装置を構成すると共にその回転軸が手首軸をなす第１部材群と、前記基台を前記手首装置に接続すると共にその回転軸が主軸をなす第２部材群とを含むものであり、各手首軸の角速度を求めるに際しては、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、前記ツール部材の１以上の端点と、当該手首軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該手首軸方向に見たときに当該手首軸から軸直交方向に最も遠位にある表面部位と、を用い、各主軸の角速度を求めるに際しては、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、当該主軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該主軸方向に見たときに当該主軸から軸直交方向に最も遠位にある表面部位とを用い、前記ツール部材のいずれの端点をも前記複数の表面部位として用いないことを特徴としている。

このような構成とすることにより、ロボットの姿勢に関わらず表面部位の最高移動速度が制限移動速度となり、表面部位の移動速度を適正に規制した上でロボットの動作性をなるべく高くすることができる。なお、移動速度が制限移動速度以下となるように考慮される部位は予め定めた複数の表面部位に限られているため、処理内容が煩雑になるのを避けることができる。

本発明によれば、多軸ロボットの表面部位の移動速度を適正に規制しつつもその移動速度をなるべく高くすることができる。

本発明の第１実施形態に係る多軸ロボットの一例として示す６軸垂直型ロボットの構造を示す模式図である。 図１に示すロボットの制御系の構成を示すブロック図である。 図２に示すコントローラによって実行される第６回転軸の角速度を規制するための処理の概念図である。 図２に示すコントローラによって実行される第４及び第５回転軸の角速度を規制するための処理の概念図であり、図４（ａ）が手首装置の模式図、図４（ｂ）が第６回転軸をＺ軸として定義した直交三次元座標系のＸＹ平面を示す図、図４（ｃ）は第５回転軸をＸ軸として定義した直交三次元座標系のＹＺ平面を示す図である。る。 図２に示すコントローラによって実行される第１乃至第３回転軸の角速度を求めるための処理の説明図である。 本発明の第２実施形態に係る多軸ロボットの一例として示す６軸垂直型ロボットの構造を示す模式図である。 図６に示すロボットにおいて実行される第１回転軸の角速度を規制するための処理の説明図であり、図７（ａ）が第３ジョイントの主部が第１回転軸から離れて位置する状態、図７（ｂ）が第３ジョイントの突部が第１回転軸から離れて位置する状態をそれぞれ示している。 本発明の第３実施形態に係る多軸ロボットの一例として示す６軸垂直型ロボットの構造を示す模式図である。

以下、これら図面を参照しながら本発明の実施形態について説明する。

［第１実施形態］
図１に示すロボット１は、その基台２が作業スペース下側の床面に設置される所謂床置き型の産業用ロボットである。基台２には基端アーム４、先端アーム５、第１ハンド６、第２ハンド７及びアタッチメント８がこの順で連設され、アタッチメント８の先端をなすフランジ面８ａに各種作業用のツール９が取り付けられる。ツール９には、例えば搬送作業用の把持部材又は吸着部材、塗装作業用のガン、及び溶接作業用のトーチ等が含まれ、フランジ面８ａには作業内容に応じて適宜選択されたツール９が着脱可能に取り付けられる。ここではツール９がフランジ面８ａに取り付けられると、ツール９はアタッチメント８に対して相対変位しないものとする。

基台２からアタッチメント８までの７つの部材２〜８は互いに相対回転可能に連結されている。つまり、基台２と旋回台３との連結部である第１ジョイントＪＴ１において旋回台３の基台２に対する第１回転軸Ａ１周りの回転が許容され、旋回台３の上端部と基端アーム４の一端部との連結部である第２ジョイントＪＴ２において基端アーム４の旋回台３に対する第２回転軸Ａ２周りの回転が許容され、基端アーム４の他端部と先端アーム５の一端部との連結部である第３ジョイントＪＴ３において先端アーム５の基端アーム４に対する第３回転軸Ａ３周りの回転が許容される。また、先端アーム５の他端部と第１ハンド６の一端部との連結部である第４ジョイントＪＴ４において第１ハンド６の先端アーム５に対する第４回転軸Ａ４周りの回転が許容され、第１ハンド６の他端部と第２ハンド７の一端部との連結部である第５ジョイントＪＴ５において第２ハンド７の第１ハンド６に対する第５回転軸Ａ５周りの回転が許容され、第２ハンド７の他端部とアタッチメント８の端部との連結部である第６ジョイントＪＴ６においてアタッチメント８の第２ハンド７に対する第６回転軸Ａ６周りの回転が許容される。

基台２が床面に適正に設置されると、第１回転軸Ａ１は鉛直に指向し、第２及び第３回転軸Ａ２，Ａ３はそれぞれ水平に指向する。第４回転軸Ａ４は第３回転軸Ａ３と直交方向であって先端アーム５の延在方向に指向し、第５回転軸Ａ５は第４回転軸Ａ４と直交方向に指向し、第６回転軸Ａ６は第５回転軸Ａ５と直交方向に指向している。本実施形態では、後述するように第３回転軸Ａ３が第４回転軸Ａ４と交差し、第４回転軸Ａ４が第５回転軸Ａ５と交差し、第５回転軸Ａ５が第６回転軸Ａ６と交差している場合を例示する。また、第１回転軸Ａ１が第２回転軸Ａ２と捩れの関係にある場合を例示するが、これら両軸Ａ１，Ａ２は互いに交差していてもよい。

第１ハンド６、第２ハンド７及びアタッチメント８はツール９に近い側に設けられる部材であり、これらはツール９の微細な動作を行わせるための構造体１０（以下これを「手首装置」と呼ぶ）をなしている。第１乃至第３回転軸Ａ１〜Ａ３は、ツール９を手首装置１０とともに水平旋回させたり揺動させたりする回転軸であり、ロボット１の所謂主軸をなしている。第４乃至第６回転軸Ａ４〜Ａ６は、手首装置１０に設定される回転軸であり、所謂ＲＢＲ型の手首軸をなしている。

第１乃至第６ジョイントＪＴ１〜ＪＴ６にはそれぞれ第１乃至第６サーボモータＭ１〜Ｍ６が設けられており、各サーボモータＭ１〜Ｍ６が動作することによってこれに対応するジョイントＪＴ１〜ＪＴ６において許容される各回転軸Ａ１〜Ａ６周りの回転が行われる。各サーボモータＭ１〜Ｍ６は互いに独立して動作可能であり、各回転軸Ａ１〜Ａ６周りの回転を互いに独立して行うことによりアタッチメント８に取り付けたツール９を任意の位置及び姿勢に任意の経路に沿って移動させることができる。

図２は図１に示すロボット１の制御系の構成を示すブロック図である。以降ではロボット１の構造的な構成要素には図１の参照符号を付して説明する。図２に示すコントローラ１５は、所定の制御モードの中から現に設定されているモードに基づいて各サーボモータＭ１〜Ｍ６の駆動制御を行い、これによりツール９を適宜移動させる制御を実行する。この制御モードには、例えば、ロボット１の移動終了位置をコントローラ１５に教示して記憶させるため、オペレータがロボット１をマニュアル操作で動作させるティーチモード、ティーチモードで教示された位置間での自動的な移動が適正に行われ得るか否かをチェックするため、動作計画に従ってロボット１を自動的に動作させるチェックモード、チェック終了後にロボット１を実用に供するため、ロボット１を自動的且つ継続的に動作させるリピートモード等が含まれる。

そして、コントローラ１５は、サーボモータＭ１〜Ｍ６の駆動制御を行うための機能ブロックとして、ロボット動作演算部１６及び制御部１７を有している。また、コントローラ１５はティーチペンダント等の外部装置３０と接続可能となっており、オペレータは、外部装置３０を利用することにより、例えばティーチモード及びチェックモードの実行開始指令の入力、ティーチモードの実行中におけるロボット１の動作のマニュアル操作、ティーチモードの実行中における移動開始位置及び移動終了位置の入力などを行うことができる。

ロボット動作演算部１６はＣＰＵ２０、メモリ２１、入出力部２２、通信部２３及びパワーシーケンス部２４を有し、外部装置３０は入出力部２２に接続されることによってコントローラ１５と通信可能となる。制御部１７は通信部２５、ＣＰＵ２６、メモリ２７及びアンプ２８を有しており、通信部２５においてロボット動作演算部１６と通信可能となっている。アンプ２８は各サーボモータＭ１〜Ｍ６に個別に設けられ、各アンプ２８は対応する１つのサーボモータと接続されている。

ティーチモードの実行中、オペレータは外部装置３０において、各回転軸Ａ１〜Ａ６周りの回転を行うモードである「各軸モード」と、ツール９を所定の直交三次元座標系によって定義される軸方向に直進運動させたり各軸周りに回転運動させたりするモードである「直交座標系モード」とを切り替え可能となっている。

「各軸モード」の設定時には、オペレータの操作に応じて、外部装置３０から、ある回転軸周りに所定回転方向に回転させる指令が入出力部２２に入力される。制御部１７のＣＰＵ２６は制御時間（例えば10msec）毎に入出力部２２に入力される指令に基づいて、制御時間が経過する間における当該回転軸周りの目標回転変位量を算出する。この目標回転変位量は、制御時間と予め定められた制限移動速度（例えば250mm/s） 等から求まる。そして、この目標回転変位量に基づいて当該回転軸に対応するサーボモータの動作量の指令値を演算し、これをアンプ２８に出力する。アンプ２８は対応するサーボモータＭ１〜Ｍ６に指令値に応じた電流を供給し、これによりサーボモータが制御時間毎に指令値に準じた動作量だけ動作する。

「直交座標系モード」の設定時には、ツール９の中心位置（所謂ＴＣＰ，図３参照）を原点とした直交三次元座標系（例えば所謂ツール座標系）によって定義される３つの軸各々の向きにツール９を直進運動させることができるとともに、これら３つの軸各々を中心としてツール９を回転運動させることができ、オペレータの操作に応じて外部装置３０から直進運動又は回転運動させる指令が入出力部２２に入力される。ロボット動作部１６のＣＰＵ２０は制御時間毎に入出力部２２に入力される指令に基づいて、ツール９がこの制御時間の経過後にツール９が位置すべき目標位置を算出する。この目標位置は、制御時間と予め定められたツール９の制限移動速度等から求まる移動距離に基づいて算出され、コントローラ１５が演算可能なようにツール座標系において定義された座標データという形で算出される。通信部２３は目標位置の座標データを制御部１７に与える。制御部１７のＣＰＵ２６は目標位置の座標データの逆変換処理を行い、制御時間の経過後にツール９を目標位置まで移動させるために必要となる各回転軸Ａ１〜Ａ６の目標回転位置を算出する。そして、目標回転位置と現在の回転位置との偏差に基づき、各回転軸Ａ１〜Ａ６を制御時間の経過後に目標回転位置まで回転させるために必要となる各サーボモータＭ１〜Ｍ６の動作量の指令値を演算し、これを各アンプ２８に出力する。アンプ２８が対応するサーボモータＭ１〜Ｍ６に指令値に応じた電流を供給することにより、各サーボモータＭ１〜Ｍ６が制御時間毎に指令値に準じた動作量だけ動作し、ツール９が制御時間毎に目標位置まで直進する。

なお、各軸モードでは複数の回転軸周りの回転を同時に行わせることも可能であり、直進モードでは２以上の直進運動や回転運動を同時に行わせることも可能である。このような場合の各サーボモータＭ１〜Ｍ６の駆動制御も、前述したものに準じて実行される。

そして、オペレータは、ロボット１が所望する移動終了位置に達すると、外部装置３０を利用してその位置を移動終了位置として設定することができる。この設定の指令に基づいてＣＰＵ２０はメモリ２１に移動終了位置を記憶させる。ティーチモードではこの一連の動作を繰り返し行うことが可能であり、メモリ２１には１以上の移動終了位置が記憶されることとなる。

ティーチモードが終了してチェックモードを開始するに際して、ＣＰＵ２０は移動終了位置同士の間の移動経路を示す情報と、その移動経路に沿った移動速度の情報とを含む動作計画を生成する。典型的にはこの移動速度の最大値が予め定めた制限移動速度（例えば250mm/s）に設定される。

チェックモードでロボット１を移動させる場合、ロボット動作部１６のＣＰＵ２０は移動経路の補間処理を行い、この移動経路上に制御時間毎の目標位置を設定する。制御部１７のＣＰＵ２６は、ロボット動作部１６より制御時間毎に送られる目標位置の座標データに基づいて前述同様の処理を行い、各サーボモータＭ１〜Ｍ６の動作量の指令値を演算し、これを各アンプ２８に出力する。アンプ２８が対応するサーボモータＭ１〜Ｍ６に指令値に応じた電流を供給することにより、各サーボモータＭ１〜Ｍ６が制御時間毎に指令値に準じた動作量だけ動作し、ツール９が制御時間毎に目標位置まで移動する。

なお、パワーシーケンス部２４は、動作計画と無関係の動作（例えばサーボモータＭ１〜Ｍ６の電源投入直後の始動時の動作や、ロボット１の緊急停止時の動作など）を制御する機能ブロックであり、後述する速度規制の処理はティーチモードや通常のロボット１の動作時だけでなくパワーシーケンス部２４により制御されるロボット１の動作時に実施されてもよい。

コントローラ１５は、これら各モードでロボット１を移動させるに際し、ツール９を含めたロボット１のあらゆる表面部位の移動速度が所定の制限移動速度以下となるように各回転軸Ａ１〜Ａ６周りの角速度を規制する制御を実行する。本実施形態では、この速度規制の処理が煩雑になるのを避けるため、予め定められた複数の表面部位に着目し、これら表面部位の全ての移動速度が制限移動速度以下となるようにしている。その上で、ロボット１がどのような姿勢であっても予め定められた複数の表面部位の一つが制限移動速度で移動するようにし、複数の表面部位の最高移動速度を常に制限移動速度で維持するようにしてロボット１の動作性の向上を図っている。ここで制限移動速度とは、オペレータやコントローラ１５の設計者が例えば安全規格等で定められた所定の上限値（例えば250mm/s 等）を超えない限りにおいて任意に設定可能な速度である。

以下、各回転軸Ａ１〜Ａ６の角速度（すなわち制御時間毎の回転変位量）を規制するための処理について説明する。なお、コントローラ１５の各機能ブロック１６，１７に個別対応してＣＰＵ２０，２６及びメモリ２１，２７が設けられる場合を例示したが、これらＣＰＵ２０，２６により実行される制御を統括して行う単一のＣＰＵに替えたり、これらメモリ２１，２７内の各情報やＣＰＵが実行する制御プログラムを一括して記憶する単一のメモリに替えてもよく、以降では便宜上これら複数のＣＰＵ及びメモリの区別が無いものとして説明する。つまり、この処理を行うために用いる情報や制御プログラムは何れのメモリ２１，２７に格納されてもよく、この処理は何れのＣＰＵ２０，２６によって実行されてもよい。また、以降ではコントローラ１５の構成要素には図２の参照符号を付して説明する。

まず、各軸モードにおける各回転軸Ａ１〜Ａ６の角速度の速度規制の処理について説明する。ここではツール９に近い側である第６回転軸Ａ６から順に説明する。また、６軸ロボットの場合、基台２に設定された所定の位置を原点とする「ベース座標系」、各回転軸Ａ１〜Ａ６上の所定の位置を原点とする「ロボット座標系」、前述したＴＣＰを原点とする「ツール座標系」など、様々な座標系の各々においてロボット１の位置を定義することが可能である。ここでＴＣＰはツール９の所定の位置に設定されるものであり、後述するように本実施形態では先端面の中心位置に設定しているが、その他の任意の位置に設定可能である。なお、第１回転軸Ａ１のロボット座標系を第１のロボット座標系と呼び、以下同様にして各回転軸に付与された序数とそれに対応するロボット座標系の序数とを対応させるものとしている。各座標系は設計データや各回転軸の回転位置に従って互いに変換可能である。そのため、以降の説明に挙げる各座標データを定義する座標系は実施形態の一つを例示したものに過ぎず、その他の座標系において各座標データが定義されていてもよい。

図３は第６回転軸Ａ６の角速度を規制する処理の説明図である。図３ではツール９が仮想的に直方体状である場合を例示しており、このツール９の先端中心に前述したＴＣＰを設定している旨を概念的に示している。

オペレータは、外部装置３０を利用してツール９の形状データを入力し、これをコントローラ１５のメモリに記憶させることができる。この形状データは、例えば図３に示すように、オペレータが任意に設定したツール９の端点の位置を示す座標データによって実現される。この座標データは、ツール９をフランジ面８ａに取り付けた状態としてフランジ面８ａの中心Ｏ６を原点とした第６のロボット座標系で定義することも可能であるし、ツール座標系で定義することも可能である。図３では、直方体状であるとしたツール９の８個の頂点をそれぞれ上記端点ｔ１〜ｔ８として設定し、各端点ｔ１〜ｔ８の座標データを第６のロボット座標系で定義してメモリに記憶させた場合を概念的に例示している。ここでは便宜的に、ツール座標系及び第６のロボット座標系のＸ軸及びＹ軸がフランジ面８ａの面方向を規定しており、ツール座標系のＺ軸と第６のロボット座標系のＺ軸とが一致しているものとする。つまり、第６回転軸Ａ６はこのＺ軸上にあり、ツール座標系と第６のロボット座標系とは、フランジ面８ａの中心Ｏ６とＴＣＰとの間のＺ方向の距離のみに基づいて互いに変換可能であるとする。

ツール９の形状データを記憶させた場合、ＴＣＰと、８つの端点ｔ１〜ｔ８との合計９箇所の表面部位を考慮して第６回転軸Ａ６の角速度（制御時間毎の回転変位量）が求められる。つまり、これら９箇所の表面部位を第６回転軸Ａ６方向に見たときに第６回転軸Ａ６から各表面部位までの軸直交方向の距離がそれぞれ求められる。言い換えると、９個の表面部位がフランジ面８ａと平行な面であるＸＹ平面上に投影され、このＸＹ平面内において点となる第６回転軸Ａ６から９個の表面部位の各々までのＸＹ成分の距離が求められる。そして、求められた９個の距離が互いに比較されて最大値が抽出される。この最大値に対応する表面部位は第６回転軸Ａ６周りの回転が生じたときに最大の移動速度で移動する部位となる。そこでコントローラ１５のＣＰＵは、この表面部位の移動速度が制限移動速度となる第６回転軸Ａ６の制御時間当たりの回転変位量θ６（すなわち角速度）を算出する。そして、この回転変位量θ６に基づいて第６サーボモータＭ６の動作量の指令値を演算する。これにより、第６回転軸Ａ６周りの回転を行わせるに際し、９個の表面部位の全ての移動速度が制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。

図４は第４及び第５回転軸Ａ４，Ａ５の角速度を規制する処理の説明図である。第５回転軸Ａ５の制御時間当たりの回転変位量の算出には、ＴＣＰと、ツール９の各端点と、フランジ面８ａの中心Ｏ６と、第２ハンド７に設定されている「許容最近部位」ｑ５との合計１１個の表面部位が考慮される。

この「許容最近部位」とは、ある回転軸周りの回転が生じたときにその回転軸の回転を許容するジョイントに連結された部材の中から基台２に最も近い部材に着目し、その部材を当該回転軸方向に見たときに回転軸から軸直交方向に最も遠位にある表面部位をいう。ここで着目される部材の姿勢は、当該回転軸の回転位置のみに依存して変化し、それよりもツール９側に設定されている回転軸の回転に応じて変化するものではない。従って、当該部材を当該回転軸方向に見たときには、当該回転軸から軸直交方向に最も遠位にある表面部位は当該回転軸の回転位置に関わらず常に同じ部位であり、本書ではこの部位を許容最近部位として定義している。第５回転軸Ａ５に関して言えば、第５ジョイントＪＴ５によって回転が許容される部材７〜９のうち基台２に最も近い部材（すなわち第５ジョイントＪＴ５に直接的に連結された部材６，７のうちツール９側に位置する部材）は第２ハンド７であるが、この第２ハンド７を第５回転軸Ａ５方向に見たときに第５回転軸Ａ５から軸直交方向に最も遠位にある表面部位ｑ５が許容最近部位として設定される。この部位は、ロボット１の設計段階において各部材の寸法が定まることで予め決定しておくことが可能であり、コントローラ１５のメモリには動作の事前に予めその位置を示す座標データが記憶される。このように許容最近部位を表面部位として考慮すると、ツール部材９が許容最近部位に対して回転軸寄りに位置する姿勢になっていたとしても、上記着目すべき部材の移動速度を制限移動速度以下に規制することが可能となる。なお、ある回転軸周りに回転する部材のうち最も基台側に位置する部材に、角速度の決定に考慮する表面部位を設定する場合を例示したが、その他の部材にこのような表面部位が設定されていてもよい。

この許容最近部位の概念は他の回転軸にも同様にして適用可能である。つまり、第４回転軸Ａ４においては第１ハンド６を第４回転軸Ａ４方向に見たときに第４回転軸Ａ４から最も遠位にある表面部位ｑ４、第３回転軸Ａ３においては先端アーム５を第３回転軸Ａ３方向に見たときに第３回転軸Ａ３から最も遠位にある表面部位ｑ３（図５参照）、第２回転軸Ａ２においては基端アーム３を第２回転軸Ａ２方向に見たときに第２回転軸Ａ２から最も遠位にある表面部位ｑ２（図５参照）、第１回転軸Ａ１においては旋回台３を第１回転軸Ａ１方向に見たときに第１回転軸Ａ１から最も遠位にある表面部位ｑ１（図５参照）がそれぞれ許容最近部位として扱われ得る。

なお、ここでは便宜的に第５及び第６回転軸Ａ５，Ａ６が互いに直交するように設定され、第５回転軸Ａ５は第６のロボット座標系のＸ軸と平行に向くものと仮定する。つまり、第６のロボット座標系が規定するＹＺ平面は第５回転軸Ａ５の直交平面となる。この場合において、第５及び第６回転軸Ａ５，Ａ６の交点Ｏ５を原点として第５のロボット座標系を定義すれば、第６のロボット座標系の原点Ｏ６の座標は、交点Ｏ５との間のＺ方向の距離ｚ５に基づいて、第５のロボット座標系では（０，０，ｚ５）に変換される。

コントローラ１５は、前述した第６回転軸Ａ６と同様、１１個の表面部位を第５回転軸Ａ５方向に見たときにおける第５回転軸Ａ５から各表面部位までの軸直交方向の距離をそれぞれ求める。つまり、１１個の表面部位が第５回転軸Ａ５の直交平面であるＹＺ平面上に投影され、このＹＺ平面内で点となる第５回転軸Ａ５から１１個の表面部位の各々までのＹＺ成分の距離が求められ、求めた１１個の距離が互いに比較されて最大値が抽出され、この最大値に対応する表面部位の移動速度が制限移動速度となる第５回転軸Ａ５の制御時間当たりの回転変位量θ５（すなわち角速度）が算出される。そして、この回転変位量θ５に基づいて第５サーボモータＭ５の動作量の指令値が演算される。これにより、第５回転軸Ａ５周りの回転を行わせるに際し、１１個の表面部位の全ての移動速度が制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。

なお、第５回転軸Ａ５から許容最近部位ｑ５までのＹＺ成分の距離は、第５のロボット座標系で定義される許容最近部位ｑ５の座標データを用いて容易に求めることができる。他方、その他の１０個の表面部位については、第６のロボット座標系又はツール座標系で定義された座標を第５のロボット座標系に変換する必要がある。これら１０個の表面部位のＹＺ成分の距離の導出に関し、ここではツールの端点の１つｔｎに着目して説明する。図４（ａ）に示す端点ｔｎの座標（Ｘｎ，Ｙｎ，Ｚｎ）は第６のロボット座標系で定義されたものとする。図４（ｂ）に示すように、ツール９は第６回転軸Ａ６周り（すなわちＺ軸周り）に回転するため、前述した第６回転軸Ａ６周りの回転変位量θ６に基づいてＸＹ平面内を移動する。コントローラ１５は、第６のロボット座標系において定義された座標（Ｘｎ，Ｙｎ，Ｚｎ）とこの回転変位量θ６とに基づいて、この回転後の端点ｔｎ′の座標（Ｘｎ′，Ｙｎ′，Ｚｎ）を算出する。図４（ａ）に戻り、次いで、第６のロボット座標系と第５のロボット座標系の各原点Ｏ５，Ｏ６間のＺ方向の距離ｚ５を用い、回転後の端点ｔｎ′の座標（Ｘｎ′，Ｙｎ′，Ｚｎ）を、第５のロボット座標系において定義される座標（Ｘｎ′，Ｙｎ′，Ｚｎ＋ｚ５）へと変換する。この座標（Ｘｎ′，Ｙｎ′，Ｚｎ＋ｚ５）に基づいて、端点ｔｎ′の原点Ｏ５からのＹＺ成分の距離ｄｔｎが次式より求められる。

この要領で、第５回転軸Ａ５とは独立して第６回転軸Ａ６周りに回転し得るＴＣＰ、８つの端点ｔ１〜ｔ８、及びフランジ面８ａの中心Ｏ６の１０個の表面部位について、第５回転軸Ａ５上の原点Ｏ５からのＹＺ成分の距離が求められる。

第４回転軸Ａ４の制御時間当たりの回転変位量の算出においても、ＴＣＰ、ツール９の各端点、フランジ面８ａの中心Ｏ６、及び第１ハンド６に設定されている許容最近部位ｑ４との合計１１個の表面部位が考慮される。

なお、ここでは、第４及び第５回転軸Ａ４，Ａ５は互いに交差するよう設定され、第４回転軸Ａ４と第４ジョイントＪＴ４の先端面との交点Ｏ４を原点とする直交三次元座標系が第４のロボット座標系として定義されるものとしている。また、この第４のロボット座標系では、図４（ａ）の記載を基準にして、Ｙ軸上に第４回転軸Ａ４が位置し、ＺＸ平面が第４回転軸Ａ４の直交平面となるものと仮定する。

コントローラ１５は、前述した第５回転軸Ａ５と同様、１１個の表面部位を第４回転軸Ａ４方向に見たときにおける第４回転軸Ａ４から各表面部位までの軸直交方向の距離をそれぞれ求める。つまり、１１個の表面部位が第４回転軸Ａ４の直交平面であるＺＸ平面上に投影され、このＺＸ平面内で点となる第４回転軸Ａ４から１１個の表面部位の各々までのＺＸ成分の距離が求められ、求めた１１個の距離が互いに比較されて最大値が抽出され、この最大値に対応する表面部位の移動速度が制限移動速度となる第４回転軸Ａ４の制御時間当たりの回転変位量（すなわち角速度）が算出される。そして、この回転変位量に基づいて第４サーボモータＭ４の動作量の指令値が演算される。これにより、第４回転軸Ａ４周りの回転を行わせるに際し、１１個の表面部位の全ての移動速度が制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。

なお、第４回転軸Ａ４から許容最近部位ｑ４までのＺＸ成分の距離は、第４のロボット座標系で定義される許容最近部位ｑ４の座標データを用いて容易に求めることができる。他方、その他の１０個の表面部位については、第６のロボット座標系又はツール座標系で定義された座標を第４のロボット座標系に変換する必要がある。これら１０個の表面部位のＺＸ成分の距離の導出に関し、ここではツールの端点の１つｔｍに着目して説明する。図４（ａ）に示す端点ｔｍの座標（Ｘｍ，Ｙｍ，Ｚｍ）も第６のロボット座標系に設定されたものとする。この端点ｔｍについても、まず、第５回転軸Ａ５の回転変位量θ５を算出したときと同様、第６回転軸Ａ６の回転変位量θ６と距離ｚ５を考慮して第５のロボット座標系に定義される座標（Ｘｍ′，Ｙｍ′，Ｚｍ＋ｚ５）へと変換する。次いで、図４（ｃ）に示すように、ツール９は第５回転軸Ａ５周り（すなわちＸ軸周り）に回転するため、前述した第５回転軸Ａ５周りの回転変位量θ５に基づいてＹＺ平面内を移動する。コントローラ１５は、第５のロボット座標系に定義された座標（Ｘｍ′，Ｙｍ′，Ｚｍ＋ｚ５）とこの回転変位量θ５とに基づき、この回転後の端点ｔｍ″の座標（Ｘｍ′，Ｙｍ″，（Ｚｍ＋ｚ５）′）を算出する。図４（ａ）に戻り、次いで、この座標（Ｘｍ′，Ｙｍ″，（Ｚｍ＋ｚ５）′）に基づいて、端点ｔｍ″の原点Ｏ４からのＺＸ成分の距離ｄｔｍが次式より求められる。

この要領で、第４回転軸Ａ４とは独立して第５及び第６回転軸Ａ５，Ａ６周りに回転し得るＴＣＰ、８つの端点ｔ１〜ｔ８及びフランジ面８ａの中心Ｏ６の１０個の表面部位について、第４回転軸Ａ４上の原点Ｏ４からのＺＸ成分の距離が求められる。

このように手首装置１０に設定された第４乃至第６回転軸Ａ４〜Ａ６の各角速度は、ＴＣＰと、ツール９の端点ｔ１〜ｔ８の位置と、当該回転軸に対応する許容最近部位及びフランジ面８ａの中心Ｏ６（第６回転軸Ａ６を除く）とを用いて求められる。他方、所謂ロボット１の主軸をなす第１乃至第３回転軸Ａ１〜Ａ３については、座標変換処理を行う回数や考慮しなければならない他の回転軸の回転変位量が多いため、原則的にＴＣＰと許容最近部位とフランジ面８ａの中心Ｏ６の３個の表面部位を考慮してその角速度を求めるようにしており、これにより処理が煩雑になるのを避けることができる。なお、これら主軸Ａ１〜Ａ３からツール９の端点ｔ１〜ｔ８までの距離は、手首装置１０がどのような姿勢になっていたとしても、ＴＣＰ又はフランジ面８ａの中心Ｏ６までの距離と大して変わらない。このためＴＣＰ及びフランジ面８ａの中心Ｏ６を表面部位として考慮してさえいれば、ツール９の端点ｔ１〜ｔ８を考慮しなくとも、主軸Ａ１〜Ａ３の各々を中心とした回転が行われたときにツール９が制限移動速度を超えて移動するような状況は起こりにくいと考えられ、このように処理を簡略化しても問題となりにくい。なお、第１乃至第３回転軸Ａ１〜Ａ３の角速度を求めるための処理を簡略化するためにツール９の端点ｔ１〜ｔ８を表面部位として考慮しない場合には、各回転軸Ａ１〜Ａ３の回転変位量を求めるために用いる制限移動速度を第４乃至第６回転軸Ａ４〜Ａ６の回転変位量を求めるために用いた制限移動速度よりも小さくなるように設定してもよい。勿論、第１乃至第３回転軸Ａ１〜Ａ３の角速度を求めるためにツール９の端点ｔ１〜ｔ８を表面部位として考慮してもよい。

図５は第１乃至第３回転軸Ａ１〜Ａ３の角速度を規制する処理の説明図である。第３回転軸Ａ３の角速度の算出においては、ＴＣＰ、フランジ面８ａの中心Ｏ６、及び先端アーム５に設定された許容最近部位ｑ３との３個の表面部位が考慮される。そして、前述した第４乃至第６回転軸Ａ４〜Ａ６と同様、３個の表面部位を第３回転軸Ａ３方向に見たときにおける第３回転軸Ａ３から各表面部位までの軸直交方向の距離をそれぞれ求める。つまり、３個の表面部位が第３回転軸Ａ３の直交平面に投影され、この平面内で点となる第３回転軸Ａ３から３個の表面部位の各々までの距離が求められ、求めた３個の距離が互いに比較されて最大値が抽出され、この最大値に対応する表面部位の移動速度が制限移動速度となる第３回転軸Ａ３の制御時間当たりの回転変位量（すなわち角速度）が算出される。そして、この回転変位量に基づいて第３サーボモータＭ３の動作量の指令値が演算される。これにより、第３回転軸Ａ３周りの回転を行わせるに際し、３個の表面部位の全ての移動速度が制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。なお、図５は第３回転軸Ａ３方向に見たロボット１の模式図となっており、ＴＣＰの移動速度が制限移動速度となる場合を例示している。前述したように３個の表面部位は互いに異なる座標系において定義されていたとしても、第３回転軸Ａ３の位置を基準にして互いに関連付け可能となっている。

第２回転軸Ａ２の角速度の算出においては、ＴＣＰ、フランジ面８ａの中心Ｏ６、及び基端アーム４に設定された許容最近部位ｑ２との３個の表面部位が考慮され、第３回転軸Ａ３と同じ要領で第２回転軸Ａ２の制御時間当たりの回転変位量（すなわち角速度）が算出され、この回転変位量に基づいて第２サーボモータＭ２の動作量の指令値が算出される。これにより、第２回転軸Ａ２周りの回転を行わせるに際し、３個の表面部位の全てが制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。なお、図５は第２回転軸Ａ２方向に見たロボット１の模式図となっており、ＴＣＰの移動速度が制限移動速度となる場合を例示している。

第１回転軸Ａ１の角速度の算出においては、ＴＣＰ、フランジ面８ａの中心Ｏ６、及び基端アーム４に設定された許容最近部位ｑ２との３個の表面部位に、更に基端アーム４の他端部（第３ジョイントＪＴ３）の表面部位を加えた合計４個の表面部位が考慮される。第１回転軸Ａ１の制御時間当たりの回転変位量（すなわち）角速度を算出する手順は、考慮する表面部位の個数が１個増えることのみを除き、第２及び第３回転軸Ａ２，Ａ３に関する回転変位量を算出する手順と同じである。このようにして求めた回転変位量に基づいて第１サーボモータＭ１の動作量の指令値を算出することにより、第１回転軸Ａ１周りの回転を行わせるに際して、４個の表面部位の全てが制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。

このように第１回転軸Ａ１に限り、角速度の算出のために考慮する表面部位の中に、第１ジョイントＪＴ１により回転が許容される部材３〜９のうち基台３に最も近い旋回台２だけでなく、その先に連結されている基端アーム４の表面部位が含まれている。これは、基端アーム４が基台２からツール９までの部材の中でも特に長尺のものであって、第１回転軸Ａ１方向に見て旋回台３よりも外側に位置し易く、先端アーム５の他端部（第４ジョイントＪＴ４）が基端アーム４の他端部（第３ジョイントＪＴ３）に対して第１回転軸Ａ１寄りに位置しがちであってＴＣＰ及びフランジ面８ａの中心Ｏ６が基端アーム４の他端部よりも第１回転軸Ａ１寄りに位置しがちであるためである。図５では、ロボット１の動作時において一般に見られる基端アーム４と先端アーム５とが折り畳まれている場合を例示しており、ＴＣＰよりも基端アーム４の他端部（第３ジョイントＪＴ３）の表面部位が第１回転軸Ａ１から遠位にある場合を例示している。

基端アーム４の他端部を第１回転軸Ａ１方向に見たときにおける第１回転軸Ｏ１から該他端部までの距離ｄＪＴ３は、第１回転軸Ａ１から第２回転軸Ａ２までの第１回転軸Ａ１の直交平面内での距離をｄ１、第２回転軸Ａ２の回転位置をθ２，第２回転軸Ａ２と第３回転軸Ａ３（すなわち基端アーム４の各揺動支軸）との間の直線距離をｄ２、第３回転軸Ａ３から基端アーム４の他端部の表面部位までの距離をｄ３とすると、式：ｄＪＴ３＝｜ｄ１＋ｄ２sinθ２｜＋ｄ３ で表される。

ここで、回転位置θ２は基端アーム４が鉛直に延びる状態を０とし、第１回転軸Ａ１に対して遠ざかる方向に基端アーム４が揺動すると正側に増加し、第１回転軸Ａ１に対して近づく方向に基端アーム４が揺動すると負側に増加するものとしている。図５は回転位置θ２が負の値をとる場合を例示しており、このように回転位置θ２が負であっても距離ｄＪＴ３を算出できるように上記式では（ｄ１＋ｄ２sinθ２）の絶対値をとるようにしている。第１回転軸Ａ１の角速度の算出には、このようにして算出された距離ｄＪＴ３が、第１回転軸Ａ１から他の３個の表面部位の各々までの距離と比較されることとなる。

このように、各回転軸Ａ１〜Ａ６における制御時間当たりの回転変位量が、当該回転軸方向に見て当該回転軸からの距離が最大となる表面部位の移動速度が制限移動速度となるようにして決定されている。そのため、ロボット１の表面部位の全てを制限移動速度以下で移動させることができる。しかも、ロボット１の姿勢がどのようなものであっても表面部位の最高速度が制限移動速度となるため、無用にロボット１の動作速度が遅くなることもなく、ロボット１の動作性が向上する。より具体的にいえば、ティーチモードを実行してロボット１の移動停止位置を教示するための作業に要する時間や、チェックモードでのロボット１の動作に要する時間を短縮することができ、ロボット１がリピートモードで動作して実用に供されるまでに要する時間を短縮できる。

また、本実施形態においてはツール９の形状データを考慮して回転軸の角速度を求めている。このため、ガンやトーチのように長尺のツールを取り付けた場合であっても、ガンやトーチの先端を制限移動速度以下で移動させることができる。なお、本実施形態ではツール９の形状データのみをメモリに記憶させることができ、これを考慮して回転軸の角速度を求めるとしたが、ツール９として搬送作業用の把持部材や吸着部材を取り付ける場合、これら部材の端点のみならずこれら部材により搬送されるワークの形状データを入力してもよい。これによりワークの端点を制限移動速度以下で移動させることができるようになる。

次に、直交座標系モードにおける各回転軸Ａ１〜Ａ６の角速度の速度規制の処理について説明する。直交座標系モードにおいては、前述したように、まず、制御時間の経過後にツール９が位置すべき目標位置が算出される。この目標位置は、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の目標位置としている。そのため、目標位置は、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の現在（すなわち制御時間の計時前）の座標と、制限移動速度とを用いた演算により、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の制御時間経過後の目標座標という形で算出される。すなわち、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６が、制御時間が経過する間にここで算出された目標位置まで移動したとすれば、これら表面部位の移動速度が制限移動速度となる。そして、この目標位置を示す目標座標の逆変換処理を行うことにより、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６が制限移動速度で目標座標まで移動するために必要となる各回転軸Ａ１〜Ａ６周りの回転変位量が算出される。

次いで、コントローラ１５のＣＰＵは、各回転軸Ａ１〜Ａ６に関し、求められた回転変位量で回転した場合に、ＴＣＰ及びフランジ面８ａの中心Ｏ６以外に予め設定されている表面部位の制御時間当たりの移動量（すなわち移動速度）を算出する。各回転軸の移動量の算出に考慮する表面部位は、上記の各軸モードにおいて各回転軸の角速度の算出に考慮した表面部位と同じである。つまり、例えば第１回転軸Ａ１においては旋回台３に設定された許容最近部位ｑ１、及び第３ジョイントＪＴ３の２個の表面部位の移動量が求められ、例えば第４回転軸Ａ４においては第１ハンド６に設定された許容最近部位ｑ４、及びツール９の８つの端点ｔ１〜ｔ８の９個の表面部位の移動量が求められ、全て合わせると３０個の移動量が求められることとなる。

そして、これら移動量のうち最大となるものを抽出する。抽出された移動量がＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の移動量であった場合には、当初算出された回転変位量に基づいて各サーボモータＭ１〜Ｍ６の動作量の指令値を算出し、指令値に基づいて各サーボモータＭ１〜Ｍ６を駆動する。これによりＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の移動速度が制限移動速度であって、それ以外の表面部位の移動速度は制限移動速度となる。他方、抽出された移動量がＴＣＰ及び／又はフランジ面８ａの中心Ｏ６以外の表面部位のものであった場合には、その表面部位の制御時間当たりの移動量が制限移動速度と等しくなるような、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６の目標位置を改めて算出する。ここで算出される目標位置は当初算出された目標位置よりも手前側に設定されることとなる。コントローラ１５のＣＰＵは、再算出された目標位置を示す目標座標の逆変換処理を行い、ＴＣＰ及び／又はフランジ面８ａの中心Ｏ６がこの目標座標まで移動するために必要となる各回転軸Ａ１〜Ａ６周りの回転変位量を改めて算出する。そして、再算出された回転変位量に基づいて各サーボモータＭ１〜Ｍ６の動作量の指令値を算出し、指令値に基づいて各サーボモータＭ１〜Ｍ６を駆動する。これにより、最大の移動量になるとして抽出された表面部位の移動速度が制限移動速度であって、ＴＣＰ及びフランジ面８ａの中心Ｏ６を含むその他の表面部位の移動速度が制限移動速度以下となる。

このように、本実施形態によれば、直交座標系モードの設定時においても、ロボット１の表面部位の全てを制限移動速度以下で移動させることができ、ロボット１の姿勢がどのようなものであっても表面部位の最高速度が制限移動速度となる。

［第２実施形態］
図６は本発明の第２実施形態に係る多軸ロボットの一例として示す６軸垂直型のロボット５１の模式図である。本実施形態のロボット５１は第１実施形態に対して構造が相違しており、これに伴い第１及び第２回転軸Ａ１，Ａ２の角速度を求める処理が相違している。その他の点では第１実施形態と共通しており、この共通点については第１実施形態と同じ参照符号を付して重複する詳細説明を省略する。また、ここでは各軸モードにおいて角速度を規制する処理についてのみ説明するが、直進モードにおいても第１実施形態で説明した各軸モードとの関連性に基づき、本実施形態を同様にして適用することができる。

図６に示すロボット５１の旋回台２と第３ジョイントＪＴ３′との間には、基端アーム４と平行に並ぶようにして平行リンク５２が跨って設けられており、平行リンク５２の一端部が旋回台２に対して揺動可能に連結され、平行リンク５２の他端部が第３ジョイントＪＴ３′に揺動可能に連結されている。このため、この第３ジョイントＪＴ３′は、第１実施形態と同様にして基端アーム４と先端アーム５とを相対回転可能に連結して第３回転軸Ａ３が設定されている主部ＪＴ３ａと、主部ＪＴ３ａから第３回転軸Ａ３の軸直交方向に突出して設けられて平行リンク５２の他端部が連結される突部ＪＴ３ｂとを有している。ここでいう「平行」とは、平行リンク５２の２つの揺動支軸Ａ２′，Ａ３′を結ぶ直線と、基端アーム４の２つの揺動支軸Ａ２，Ａ３とを結ぶ直線とが互いに平行であることを意味し、平行リンク５２及び基端アーム４の各々の揺動支軸間での形状は任意のものにすることができる。第２ジョイントＪＴ２において許容される第２回転軸Ａ２周りの回転が行われると、平行リンク５２は揺動支軸Ａ２′を中心にして基端アーム４とともに揺動し、基端アーム４と「平行」に並ぶ状態が維持される。

第２回転軸Ａ２の角速度の算出には、第２回転軸Ａ２方向に見たときにＴＣＰ、フランジ面８ａの中心Ｏ６、基端アーム５及び平行リンク５２に含まれている許容最近部位ｑ２′の３個の表面部位が考慮される。

図７は図６に示すロボット５１において第１回転軸Ａ１の角速度を規制するための処理の説明図である。本実施形態においても、第１回転軸Ａ１の角速度の算出に際し、ＴＣＰ、フランジ面８ａの中心Ｏ６、旋回台２に含まれる許容最近部位ｑ１のみならず、第３ジョイントＪＴ３′の表面部位が考慮される。ここで、平行リンク５２の揺動支軸Ａ２′は第１回転軸Ａ１方向に見て第１回転軸Ａ１と第２回転軸Ａ２との間に位置している。このため、図７（ａ）に示すように、基端アーム４の回転角度が正側であって基端アーム４が延長に延びる状態から第１回転軸Ａ１に対して遠ざかる方向に揺動しているときには、突部ＪＴ３ｂは第３回転軸Ａ３よりも第１回転軸Ａ１寄りに位置するようになり、第３ジョイントＪＴ３′を第１回転軸方向Ａ１に見て第１回転軸Ａ１からの距離が最大となる表面部位は、第１実施形態と同様にして基端アーム４と先端アーム５とが連結されて第３回転軸Ａ３が設定された主部ＪＴ３ａの表面部位となる。

他方、図７（ｂ）に示すように基端アーム４の回転角度θ２′が負側であって基端アーム４が鉛直に延びる状態から第１回転軸Ａ１へと近づく方向に揺動しているときには、突部ＪＴ３ｂは第３回転軸Ａ３よりも第１回転軸Ａ１から離れて位置するようになり、第３ジョイントＪＴ３′を第１回転軸方向Ａ１に見て第１回転軸Ａ１からの距離が最大となる表面部位は、突部ＪＴ３ｂの表面部位となる。

そして、第２回転軸Ａ２が０に近い所定の「臨界回転角度」に位置すると、第１回転軸Ａ１から主部ＪＴ３ａまでの距離と突部ＪＴ３ｂまでの距離とが等しくなる。なお、この臨界回転角度は、平行リンク５２及び基端アームの寸法や、第１乃至第３回転軸Ａ１〜Ａ３及び平行リンク５２の揺動支軸Ａ１′，Ａ２′の間の位置関係に応じて変更される。

コントローラ１５は、第１回転軸Ａ１の角速度を算出するに際し、まず第２回転軸Ａ２の回転角度がこの臨界回転角度に対して正側にあるか負側にあるのかを検知する。検知した回転角度が臨界回転角度に対して正側であったときには、第１回転軸方向Ａ１に見たときの第１回転軸Ａ１からＴＣＰまでの距離、第１回転軸Ａ１からフランジ面８ａの中心Ｏ６までの距離、第１回転軸Ａ１から旋回台３に設定された許容最遠部位ｑ１までの距離、及び第１回転軸Ａ１から第３ジョイントＪＴ３′の主部ＪＴ３ａの表面部位までの距離が求められ、求めた４個の距離が互いに比較されて最大値が抽出され、この最大値に対応する表面部位の移動速度が制限移動速度となる第１回転軸Ａ１の制御時間当たりの回転変位量（すなわち角速度）が算出される。そして、この回転変位量に基づいて第１サーボモータＭ１の動作量の指令値が演算される。これにより、第１回転軸Ａ１周りの回転を行わせるに際し、４個の表面部位の全ての移動速度が制限移動速度以下となってそのうちの１つの移動速度が制限移動速度となる。他方、検知した回転角度が臨界回転角度に対して負側であったときには、第１回転軸Ａ１から第３ジョイントＪＴ３′の主部ＪＴ３ａの表面部位までの距離に替えて第１回転軸Ａ１から第３ジョイントＪＴ３′の突部ＪＴ３ｂまでの距離が求められ、以下同様にして第１回転軸Ａ１の制御時間当たりの回転変位量が算出される。

このように平行リンクを備えた場合には、第２回転軸Ａ２の回転角度に応じて第３ジョイントＪＴ３′の表面部位の中で第１回転軸Ａ１から最も離れた部位が、主部ＪＴ３ａであるのか突部ＪＴ３ｂであるのかを判断し、その判断結果に基づいて第１回転軸Ａ１の角速度を求めるようにしているため、第２回転軸Ａ２の回転角度が負であっても突部ＪＴ３ｂの移動速度を制限移動速度以下に規制することができる。

なお、ここでは平行リンク５２の一端部の揺動支点Ａ２′を第１回転軸Ａ１と第２回転軸Ａ２との間に配置したため、第２回転軸Ａ２の回転角度が負であるときに突部ＪＴ３ｂの表面部位を考慮する必要があったが、第１回転軸Ａ１から揺動支点Ａ２′までの距離が第２回転軸Ａ２までの距離よりも大きくなるときには、第２回転軸Ａ２の回転角度が正となったときに突部ＪＴ３ｂの表面部位を考慮して第１回転軸Ａ１の角速度を求めるように適宜処理内容が変更される。但し、平行リンク５２の一端部の揺動支点Ａ２を本実施形態のように配置することにより、基台２に対して平行リンク５２をコンパクトに配置することができる点で有利である。

［第３実施形態］
図８は本発明の第３実施形態に係る多軸ロボットの一例として示す６軸垂直型のロボット１０１の模式図である。本実施形態は第１及び第２実施形態に対してロボットの据付箇所が相違している。その他の点では上記実施形態とほぼ同一であり、上記各実施形態と同一の構成については同一の符号を付して重複する詳細説明を省略している。また、ここでも各軸モードにおいて角速度を規制する処理についてのみ説明するが、直進モードにおいても第１実施形態で説明した各軸モードとの関連性に基づき、本実施形態を同様にして適用することができる。

図８に示すロボット１０１は、基台２が作業スペース上側の棚面に設置される所謂棚置き型の産業用ロボットである。このため、作業を行うときなどには、旋回台１０３に対して基端アーム４が水平に張り出し、基端アーム４の他端部に対して先端アーム５が下側に延びるような姿勢となる傾向にある。

本実施形態においても、第１回転軸Ａ１の角速度の算出に際しては、ＴＣＰ、フランジ面８ａの中心Ｏ６、及び旋回台３に設定される許容最近部位ｑ１に、第３ジョイントＪＴ３″の表面部位を加えた合計４個の表面部位が用いられる。

第１回転軸Ａ１方向に見たときの第１回転軸Ａ１から第３ジョイントＪＴ３″の表面部位までの距離ｄＪＴ３″は、第２回転軸Ａ２と第３回転軸Ａ３との間の直線距離をｄ２、第２回転軸Ａ２の回転角度をθ２″、第３回転軸Ａ３から第３ジョイントＪＴ３″の表面部位までの距離をｄ３とすると、式：ｄＪＴ３″＝ｄ２cosθ２″＋ｄ３ で表される。

ここで、回転角度θ２″は基端アーム４が水平に延びる状態を０としている。ここでは第２実施形態のように絶対値をとる項を設定していないが、基端アーム４が鉛直に延びる状態を超えて図示する状態と反対側まで揺動するようなことがないため、実用上は絶対値をとる項を設定しなくても問題とならない。また、上記式は、第１回転軸Ａ１方向に見て第２回転軸Ａ２が第１回転軸Ａ１上を通過するよう旋回台１０３を構成した場合を想定しているが、第２回転軸Ａ２と第１回転軸Ａ１とがこのような位置関係にない場合には、適宜第１回転軸Ａ１と第２回転軸Ａ２との位置関係（すなわち距離）をパラメータとする項が上記式に加えられる。

本実施形態のように第１回転軸Ａ１方向に見て第３ジョイントＪＴ３″の表面部位が最遠部位となる傾向にある棚置き型のロボット１０１においては、第１回転軸Ａ１の角速度を求めるために第３ジョイントＪＴ３″の表面部位を考慮するのは特に有効となる。なお、本実施形態においても基端アーム４と平行に並ぶ平行リンクを第２実施形態と同様にして設けてもよい。

これまで本発明の実施形態について説明したが、上記構成は本発明の範囲内で適宜変更可能である。例えば手首装置１０に設定される回転軸として所謂ＲＢＲ型の手首軸を例示したが、所謂ＢＢＲ型の手首軸や３Ｒ型の手首軸であってもよい。また、多軸ロボットとして６軸垂直型の産業用ロボットを例示したが、ロボットの軸数は６つに限られないし、水平型や直角座標型のロボットにも好適に適用することができる。また産業用途に限らず、他の用途で利用されるロボットにも好適に適用することができる。また、本発明に係る速度制御装置として例示したコントローラ１５はロボット１の例えば基台２に取り付けられていてもよいし、ロボット１と遠隔に配置されていて物理的に着脱可能な形態で接続されていてもよい。

本発明はロボットの動作速度を適正に制限しつつもその動作速度をなるべく高くすることができるという優れた作用効果を奏し、表面部位に速度制限をかける必要がある多軸ロボットに利用すると有益である。

Ａ１〜Ａ６ 第１〜第６回転軸
ＪＴ１〜ＪＴ６ 第１〜第６ジョイント
ＪＴ３′，ＪＴ３″ 第３ジョイント
ＪＴ３ａ 主部
ＪＴ３ｂ 突部（連結部分）
Ｍ１〜Ｍ６ 第１〜第６サーボモータ
１，５１，１０１ ロボット
２ 基台
３，１０３ 旋回台（第１の連結部材）
４ 基端アーム（第２の連結部材）
５ 先端アーム（第３の連結部材）
６ 第１ハンド（連結部材）
７ 第２ハンド（連結部材）
８ アタッチメント（連結部材）
９ ツール
１０ 手首装置
１５ コントローラ（速度制御装置）
２１，２５，２８ ＣＰＵ
２２，２６，２８ メモリ
５２ 平行リンク（平行部材）

Claims (4)

1. 基台に連設された複数の連結部材の先端にツール部材を取り付けてなり、前記基台及び前記複数の連結部材が互いに相対回転可能な垂直型の多軸ロボットであって、
   前記ツール部材の移動時に各回転軸の角速度を制御する速度制御装置を備え、
   前記速度制御装置が、前記複数の連結部材及び前記ツール部材において予め設定された複数の表面部位の移動速度のうち最高移動速度が所定の制限移動速度となるように当該回転軸の角速度を求め、
   前記複数の連結部材は、前記ツール部材が取り付けられる連結部材を含む手首装置を構成すると共にその回転軸が手首軸をなす第１部材群と、前記基台を前記手首装置に接続すると共にその回転軸が主軸をなす第２部材群とを含み、
   各手首軸の角速度を求めるに際して前記速度制御装置は、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、前記ツール部材の１以上の端点と、当該手首軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該手首軸方向に見たときに当該手首軸から軸直交方向に最も遠位にある表面部位とを用い、
   各主軸の角速度を求めるに際して前記速度制御装置は、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、当該主軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該主軸方向に見たときに当該主軸から軸直交方向に最も遠位にある表面部位とを用い、前記ツール部材のいずれの端点をも前記複数の表面部位として用いないことを特徴とする多軸ロボット。
2. 前記第２部材群は、前記基台に対して第１回転軸周りに回転可能に連結された第１の連結部材と、前記第１の連結部材に対して第２回転軸周りに回転可能に連結された第２の連結部材と、前記第２の連結部材に対して第３回転軸周りに回転可能に連結された第３の連結部材とを含み、前記第１乃至第３回転軸が前記主軸をなしており、
   前記第１回転軸の角速度を求めるに際して前記速度制限装置は、前記予め設定された表面部位として、更に前記第２及び第３の連結部材のジョイントの表面部位を用いることを特徴とする請求項１に記載の多軸ロボット。
3. 前記第１の連結部材と、前記ジョイントとにそれぞれ揺動可能に連結されて前記第２の連結部材と平行に並ぶ平行部材を備え、
   前記第１回転軸の角速度に対して予め設定された複数の表面部位に、前記平行部材と前記ジョイントとの連結部分の表面部位が含まれていることを特徴とする請求項２に記載の多軸ロボット。
4. 基台に連設された複数の連結部材の先端にツール部材を取り付けてなり前記基台及び前記複数の連結部材が互いに相対回転可能に構成された垂直型の多軸ロボットに備えられ、前記ツール部材の移動時に各回転軸の角速度を制御する多軸ロボットの速度制御装置であって、
   前記複数の連結部材及び前記ツール部材において予め設定された複数の表面部位の移動速度のうち最高移動速度が所定の制限移動速度となるように当該回転軸の角速度を求め、
   前記複数の連結部材は、前記ツール部材が取り付けられる連結部材を含む手首装置を構成すると共にその回転軸が手首軸をなす第１部材群と、前記基台を前記手首装置に接続すると共にその回転軸が主軸をなす第２部材群とを含むものであり、
   各手首軸の角速度を求めるに際しては、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、前記ツール部材の１以上の端点と、当該手首軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該手首軸方向に見たときに当該手首軸から軸直交方向に最も遠位にある表面部位とを用い、
   各主軸の角速度を求めるに際しては、前記複数の表面部位として、前記ツール部材の中心位置と、前記連結部材の中心位置と、当該主軸周りに回転駆動される連結部材のうち前記基台に最も近い部材の表面部位であって当該部材を当該主軸方向に見たときに当該主軸から軸直交方向に最も遠位にある表面部位とを用い、前記ツール部材のいずれの端点をも前記複数の表面部位として用いないことを特徴とする多軸ロボットの速度制御装置。

Images