図1は、本発明の第1実施形態である位置決めロボット20を示すブロック図である。図2は、各多関節アーム30A〜30Cによってワーク40を協働把持した状態を示す斜視図である。図2に示すように、位置決めロボット20は、複数の多関節アーム30A〜30Cによって協働してワーク40を把持し、協働把持したワーク40を把持位置から目標位置まで移動させて位置決めすることができる。たとえば位置決めロボット20によってワーク40を目標位置に位置決めした状態で、他の加工ロボット50が加工を行う。このように、本実施形態では、位置決めロボット40は、任意の位置にワーク40を協働把持する治具用途のロボットとなる。位置決めロボット20は、複数の多関節アーム30A〜30Cと、複数の多関節アーム30A〜30C毎に設けられるサブコントローラ31A〜31Cと、ホストコントローラ32とを含んで構成される。
位置決めロボット20は、本実施の形態では、3つの多関節アーム30A〜30Cを有し、それぞれ同一に構成される。したがって1つの多関節アーム30Aについて説明し、残余の多関節アーム30B,30Cについての説明を省略する。各多関節アーム30A〜30Cは、6自由度を有する。多関節アーム30Aは、直列に並ぶ6つのアーム体21〜26と、隣接する2つのアーム体を回転自在に連結する6つの関節部Jt1〜Jt6と、ハンド部27と、基台28とを含む。
各アーム体21〜26は、それぞれ連結されて直列状に延びるアーム構成体29を構成する。ハンド部27は、アーム構成体29のうちで直列方向一端部のアーム体26に接続され、把持対象物であるワーク40を把持可能に形成される。ハンド部27は、ワーク40を把持した状態でワーク40と一体となる、言換えるとワーク40を剛体結合する構成であればよい。たとえばハンド部27は、2つの挟持体でワーク40を挟んでワーク40を保持する構成であってもよく、吸着体によってワーク40を吸着する構成であってもよい。
また基台28は、アーム構成体29のうちで直列方向他端部のアーム体21に接続され、壁または床などの予め定められる固定位置に固定される。各関節部Jt1〜Jt6によって連結されるアーム体が相対変位することによって、基台28に対してハンド部27の位置および姿勢を位置合わせすることができる。
各関節部Jt1〜Jt6は、同軸関節部と傾斜関節部とのうちのいずれかである。同軸関節部は、隣接する2つの連結対象物を同軸に連結し、一方の連結対象物を他方の連結対象物に対して、それらの各連結対象物の軸線と同軸な回転軸線まわりに回転自在に連結する。傾斜関節部は、隣接する2つの連結対象物を連結し、一方の連結対象物を、他方の連結対象物の軸線に対して傾斜する回転軸線まわりに円錐回転自在に連結する。本実施の形態では、連結対象物は、アーム体21〜26および基台28のいずれかによって実現される。また各アーム体21〜26は、略円筒状に形成され、各アーム体の中心を通過する軸線が設定される。
各関節部Jt1〜Jt6は、基台28から第1〜第6関節部Jt1〜Jt6の順で並ぶ。第1関節部Jt1および第6関節部Jt6は、同軸関節部であり、第2関節部Jt2〜第5関節部Jt5は、傾斜関節部である。また本発明において、用語「回転」とは、回転軸線まわりの360以上の回転だけでなく、軸線まわりに360度以下の角度で角変位する場合も含むものとする。
第1関節部Jt1は、基台28の直列方向一端部と、第1アーム体21の直列方向他端部とを連結する。第1アーム体21は、第1関節部Jt1によって、基台28に対して第1アーム体21の軸線と同軸の第1回転軸線A1まわりに回転自在となる。第2関節部Jt2は、第1アーム体21の直列方向一端部と、第2アーム体22の直列方向他端部とを連結する。第2アーム体22は、第2関節部Jt2によって、第1アーム体21の軸線に対して所定の角度、本実施の形態では45度で傾斜する第2回転軸線A2まわりに回転自在となる。
第3関節部Jt3は、第2アーム体22の直列方向一端部と、第3アーム体23の直列方向他端部とを連結する。第3アーム体23は、第3関節部Jt3によって、第2アーム体22の軸線に対して所定の角度、本実施の形態では45度で傾斜する第3回転軸線A3まわりに回転自在となる。第4関節部Jt4は、第3アーム体23の直列方向一端部と、第4アーム体24の直列方向他端部とを連結する。第4アーム体24は、第4関節部Jt4によって、第3アーム体23の軸線に対して所定の角度、本実施の形態では45度で傾斜する第4回転軸線A4まわりに回転自在となる。
第5関節部Jt5は、第4アーム体24の直列方向一端部と、第5アーム体25の直列方向他端部とを連結する。第5アーム体25は、第5関節部Jt5によって、第4アーム体24の軸線に対して所定の角度、本実施の形態では45度で傾斜する第5回転軸線A5まわりに回転自在となる。第6関節部Jt6は、第5アーム体25の直列方向一端部と、第6アーム体26の直列方向他端部とを連結する。第6アーム体26は、第6関節部Jt6によって、第5アーム体25の軸線と同軸の第6回転軸線A6まわりに回転自在となる。
このように各関節部Jt1〜Jt6は、隣接するアーム体を互いに回転可能に連結する。また各アーム体21〜26が回転することで、アーム構成体29は、図1に示すように、アーム体21〜26のそれぞれの軸線が同軸に配置されて一直線状に延びる状態に変形可能に構成される。各アーム体21〜26の軸線が一直線状に延びた状態では、第2回転軸線A2と第3回転軸線A3とは平行であり、第4回転軸線A4と第5回転軸線A5とは平行であり、第3回転軸線A3と第4回転軸線A4とは垂直である。
各多関節アーム30A〜30Cは、各アーム体21〜26をそれぞれ相対回転させることで、アーム構成体29を蛇のように変形させ、ハンド部27の位置および姿勢を位置決め可能である。各多関節アーム30A〜30Cのいずれかには、関節部によって連結される2つのアーム体を、関節部に設定される回転軸線まわりに回転駆動する駆動手段となるサーボモータが設けられる。
多関節アーム30Aが有する関節部Jt1〜Jt6は、駆動関節部と、非駆動関節部とのいずれかとなる。駆動関節部は、サーボモータが設けられる関節部であって、サーボモータによって連結する2つのアーム体を相対回転駆動可能な関節部である。駆動関節部は、ワーク移動にあたってワークの自由度を拘束する数以上に設けられる。また非駆動関節部は、サーボモータおよびサーボモータに付随する減速器が設けられない関節部であって、隣接する2つのアーム体の相対変位を許容する状態を維持する。言換えると、非駆動関節部は、連結する2つのアーム体が受動的に相対変位するが、能動的に相対変位させることができない。
本実施の形態では、第1関節部Jt1と、第2関節部Jt2と、第4関節部Jt4とが駆動関節部となり、第3関節部Jt3と、第5関節部Jt5と、第6関節部Jt6とが非駆動関節部となる。図1には、駆動関節部を黒丸記号で示し、非駆動関節を白丸記号で示す。
図3は、多関節アーム30Aのうちで、第1〜第4関節部Jt1〜Jt4を示す断面図である。駆動関節部である第1関節部Jt1、第2関節部Jt2および第4関節部Jt4には、サーボモータ41のほかに、ブレーキ42と、エンコーダ43と、減速器55とがそれぞれ設けられる。ブレーキ42は、駆動関節部によって連結される2つのアーム体の相対変位を阻止する締結状態と、相対変位を許容する開放状態とに切換え可能な変位阻止手段となる。またエンコーダ43は、隣接するアーム体の相対変位位置を検出する検出手段となる。減速器55は、サーボモータ41から与えられる動力を予め定める減速比で減速してアーム体に伝達して、アーム体を相対変位させる。
また非駆動関節部である第3関節部Jt3は、それぞれブレーキ42とエンコーダ43とが設けられ、サーボモータ41および減速器55が除かれる。図3には、図示していないが、残余の非駆動関節部である第5関節部Jt5および第6関節部Jt6も、第3関節部Jt3と同様にそれぞれブレーキ42とエンコーダ43とが設けられ、サーボモータ41および減速器55が除かれる。
各多関節アーム30A〜30Cが有する駆動関節部の総数は、6つ以上が好ましい。本実施の形態では、駆動関節部は、各多関節アーム30A〜30Cにそれぞれ3つ、合計9つ設けられる。言換えると、非駆動関節もまた、各多関節アーム30A〜30Cにそれぞれ3つ、合計9つ設けられる。
各駆動関節部は、各関節部のうちで、連結する2つのアーム体の相対変位をそれぞれ固定することで、協働把持するワーク40を介して、各多関節アーム30A〜30Cが互いに支えられて、各多関節アーム30A〜30Cの変形が阻止される複数の関節部に選ばれる。言換えると、各駆動関節部は、各関節部のうちで、ワーク40を協働して把持した場合に、連結する2つのアーム体の相対変位をそれぞれ固定することで、各多関節アーム30A〜30Cがワーク40の変位を拘束する関節部に選択される。
第5関節部Jt5および第6関節部Jt6を非駆動関節部とした場合に、第1多関節アーム30Aのみでワーク40を把持した状態では、ワーク40が第6回転軸線A6まわりに回転自在となるとともに、把持したワーク40が第5回転軸線A5まわりに回転自在となる。これに対して図2に示すように、複数の多関節アーム30A〜30Cによってワーク40を協働把持した状態では、各多関節アーム30A〜30Cのハンド部27ごとに回転可能な方向が異なることで、各多関節アーム30A〜30Cが互いに支えられ、第1〜第3多関節アーム30A〜30Cに設定される第5回転軸線A5および第6回転軸線A6まわりにワーク40がそれぞれ回転することが阻止される。このように直列方向一端部側の1または複数の関節部Jt5,Jt6を非駆動関節部とした場合であっても、複数の多関節アーム30A〜30Cが協働してワーク40を把持することで、ワーク40の変位を阻止することができる。
本実施の形態では、3つの多関節アーム30A〜30Cの基台28が、水平な床面にそれぞれ配置され、各基台28を結ぶ線分によって三角形が形成されるように配置される。これによって第1〜第3多関節アーム30A〜30Cの第6回転軸線A6が全て同軸となることを防ぐことができ、ワーク40の変位をより確実に阻止することができる。
第3関節部Jt3を非駆動関節部とした場合も同様であって、ワーク40を協働把持することによって各多関節アーム30A〜30Cの第3アーム体23の位置がそれぞれ決定された状態では、第2アーム体22の回転可能な方向が異なることで、各多関節アーム30A〜30Cが互いに支えられ、第1〜第3多関節アーム30A〜30Cにそれぞれ設定される第3回転軸線A3まわりに第3アーム体23がそれぞれ回転することが阻止される。
このように駆動関節部は、ワーク40を協働把持したうえで、各駆動関節部にそれぞれ設けられるアーム体の相対変位を固定することで、各多関節アーム30A〜30Cが変形することが阻止される関節部に選択される。言換えると、各駆動関節部にそれぞれ設けられるアーム体の相対変位を固定した状態で、それぞれの多関節アーム30に設けられる各非駆動関節部によって許容されるワーク40の移動可能領域が、一点で交わるように設定される。
図4は、ワーク40を協働把持した多関節アームの非自立条件を示す図である。本実施の形態の位置決めロボット20は、以下に示す非自立状態を回避する条件、言換えると自立条件を満足するように、非駆動関節部および移動経路などが適切に設定される。また図4には、2つの多関節アーム30D,30Eを用いた場合について説明する。非自立状態では、ワーク40を位置決めすべき角変位量で各駆動関節部を回転させたとしても、ワーク40の移動が許容されて、ワーク40が角変位自在となる状態を意味する。
具体的には、図4(1)に示すように、一方の多関節アーム30Dと他方の多関節アーム30Eに設けられる同軸関節部61が、非駆動関節部としてそれぞれ採用される場合、それらの各多関節アーム30D,30Eの非駆動同軸関節部60の回転軸線61が同軸に延びる場合に、2つの多関節アーム30D,30Eがその回転軸線61まわりに角変位自在となる。
本実施の形態では、図2に示すように、各多関節アーム30A〜30Cのうちで、非駆動同軸関節部となる各第6関節部Jt6の第6回転軸線A6が鉛直方向に延びた状態で、ワーク40を協働把持する。これによって、各第6回転軸線A6が同軸となることを防いで、多関節アーム30A〜30Cの変形を防ぐことができる。また各ハンド部27が一直線状に並ばない状態で、ワーク40を協働把持するので、仮に各第6回転軸線A6が水平に延びる場合であっても、各第6回転軸線A6が同軸となることを防いで、各多関節アーム30A〜30Cの変形を防ぐことができる。
また図4(2)に示すように、2つの多関節アーム30D,30Eに設けられる垂直関節部62が、非駆動関節部としてそれぞれ採用される場合もまた、それらの各多関節アーム30D,30Eの非駆動垂直関節部62の回転軸線63が、同軸に延びる場合に、各多関節アーム30D,30Eがその回転軸線61まわりに角変位自在となる。ここで、垂直関節部62は、2つのアーム体を連結し、一方のアーム体の軸線に垂直な回転軸線まわりに、他方のアーム体を相対回転自在に連結する関節部である。
また他の例として、一方の多関節アームの垂直関節部62の回転軸線63と、他方の多関節アームの同軸関節部60の回転軸線61とが同軸となる場合、各多関節アームがその回転軸線61,63まわりに角変位自在となる。本実施の形態では、各多関節アーム30A〜30Cが非駆動垂直関節部62を有しない構成であるので、多関節アーム30A〜30Cの変形を防ぐことができる。
また図4(3)に示すように、2つの多関節アーム30D,30Eに設けられる傾斜関節部64のいずれかが、非駆動関節部としてそれぞれ採用される場合、以下の(1)〜(4)の全ての条件を満足することで、各多関節アーム30D,30Eが変形可能となる。(1)それぞれ各多関節アーム30D,30Eに、各非駆動傾斜関節部64a〜64dが2つずつ設けられ、それらの各非駆動傾斜関節部64a〜64dの回転軸線が平行に延びる。(2)一方の多関節アーム30Dに設けられる2つの非駆動傾斜関節部64a,64bの距離L1と、他方の多関節アーム30Eに設けられる2つの非駆動傾斜関節部64c,64dの距離L2とが等しい。(3)一方の多関節アーム30Dに設けられる2つの非駆動傾斜関節部64a,64bを結ぶ直線と、他方の多関節アーム30Eに設けられる2つの非駆動傾斜関節部64c,64dを結ぶ直線とが平行となる。(4)2つの多関節アーム30D,30Eに設けられる直列方向一方のそれぞれの非駆動傾斜関節部64a,64cを結ぶ直線と、2つの多関節アーム30D,30Eに設けられる直列方向他方のそれぞれの非駆動傾斜関節部64b,64dを結ぶ直線とが平行となる。
本実施の形態では、各多関節アーム30A〜30Cには、2つの非駆動傾斜関節部Jt3,Jt5がそれぞれ設けられるものの、それらの回転軸線A3〜A5は、平行に位置することがない。また第1多関節アーム30Aの第3関節部Jt3と、第2多関節アーム30Bの第3関節部Jt3とを結ぶ直線と、第1多関節アーム30Aの第3関節部Jt3と、第3多関節アーム30Cの第3関節部Jt3とを結ぶ直線とが一致しないので、各多関節アーム30A〜30Cの変形を防ぐことができる。このように、位置決めロボットは、複数の多関節アームを用いてワークを協働把持した場合に、各ワークの移動を阻止して各多関節アームが自立状態を満足する自立条件を満足する。自立条件については、図10を用いて詳細に後述する。
ホストコントローラ32は、記憶部と、演算部と、通信部と、入力部とを含んで構成される。演算部は、記憶部に記憶される演算プログラムを実行し、予め入力または記憶されるワークの移動計画に基づいて、ワーク40が移動するワーク移動経路を演算するとともに、各多関節アーム30A〜30Cのハンド部27が移動するハンド移動経路を演算する。通信部は、多関節アーム30A〜30C毎に演算したハンド移動経路を示す信号を、多関節アーム30A〜30C毎に設けられるサブコントローラ31A〜31Cに与える。また入力部は、操作者などからワークの移動計画が入力される。記憶部は、ROM(Read
Only Memory)などの記憶回路によって実現される。また演算部は、CPU(Central
Processing Unit)などの演算処理回路によって実現される。また入力部は、キーボードおよびスイッチなどであってもよく、各ロボットの位置をティーチングするティーチングペンダントを介して、ワーク40の移動計画が入力されてもよい。
サブコントローラ31A〜31Cは、記憶部と、演算部と、通信部と、入力部と、出力部とを含んで構成される。演算部は、記憶部に記憶される演算プログラムを実行し、通信部を介してホストコントローラ32から与えられるハンド移動経路を示す信号に応答する。演算部は、入力部を介してエンコーダ43から与えられるアーム体の相対角度と、通信部から与えられるハンド移動経路とに基づいて、ハンド部27が移動経路に沿って移動するような各サーボモータ41の動作量およびブレーキ42の切換えタイミングを演算する。具体的には、演算部は、ハンド部の移動経路に基づいて各サーボモータ41の動作量をそれぞれ演算する逆変換演算処理と、逆変換演算結果の各サーボモータ41の動作量で各モータ41を制御するサーボ処理とを行う。記憶部は、ROM(Read Only Memory)などの記憶回路によって実現される。また演算部は、CPU(Central Processing Unit)などの演算処理回路によって実現される。
出力部は、演算部が演算した演算結果が与えられて、演算結果に応答して、サーボモータ41を動作する動力を与えるとともに、ブレーキ42の状態を切換える信号を与える。たとえば出力部は、増幅回路であるサーボアンプによって実現される。サーボアンプは、演算部で演算されたサーボモータ41の動作量に応じた電流を、対応する各サーボモータ41にそれぞれ与える。これによってサーボモータ41は、演算手段46によって演算された動作量でアーム体を相対回転させる。またサーボアンプは、演算部で演算されたブレーキ42の状態に応じた指令をブレーキ42に与える。ブレーキ42は、アーム体の変位を阻止する締結指令が与えられるとアーム体の相対変位を阻止し、アーム体の変位を許容する開放指令が与えられるとアーム体の相対変位を許容する。
各サブコントローラ31A〜31Cは、ホストコントローラ32の指令に従って、それぞれ多関節アーム30A〜30Cを制御する。これによって各ハンド部27を予め定めるハンド移動経路に沿って移動させることができ、各ハンド部27に協働把持した1つのワーク40を、予め定める目標位置および目標姿勢にワーク40を位置合わせすることができる。
図5は、サブコントローラ31Aによる多関節アーム30Aのワーク移動制御手順を示すフローチャートである。各サブコントローラ31A〜31Cは、同様の動作を行うので、1つのサブコントローラ31Aについて説明し、残余のサブコントローラ31B,31Cの説明については省略する。サブコントローラ31Aの演算部は、記憶部に記憶されるワーク移動制御プログラムを読出し、その制御プログラムを実行することによって、サーボモータ41およびブレーキ42を制御する。
まずステップa0で、各多関節アーム30A〜30Cの各ハンド部27によってワーク40を協働把持した状態で、ホストコントローラ32からワーク移動指令が与えられると、演算部は、多関節アーム30Aのワーク移動制御動作を開始し、ステップa1に進む。
ステップa1では、演算部は、出力部を介して、各非駆動関節部に設けられるブレーキ42にそれぞれ開放指令を与える。開放指令が与えられたブレーキ42は、対応するアーム体の相対変位を許容する。演算部は、各非駆動関節部が連結するアーム体の相対変位を許容させると、ステップa2に進む。
ステップa2では、演算部は、通信部を介してホストコントローラ32から与えられるハンド移動位置および姿勢を示す信号に応答して、ハンド移動位置にハンド部27を移動させるような、各アーム体の相対変位位置を演算する。次に演算結果に応じて、各駆動関節部に接続されるサーボモータ41をそれぞれサーボ制御して、各アーム体を相対回転させ、ステップa3に進む。
ステップa3では、演算部は、ハンド部27が最終的に移動すべき終了位置に達したか否かを判断する。たとえばホストコントローラ32から移動終了信号が与えられると、演算部は、ハンド部27が終了位置に達したことを判断する。ハンド部27が終了位置に達した場合、理想的な状態ではワーク40は、予め定める目標位置に位置決めされる。
演算部は、ハンド部27が終了位置に達していないと判断すると、ホストコントローラ32から、次のハンド移動位置を示す信号が与えられるまで待機する。次のハンド移動位置を示す信号が与えられると、ステップa2に戻り、ステップa2とステップa3とを繰返す。ステップa3において、演算部は、ハンド部27が終了位置に達したことを判断すると、ステップa4に進む。
ステップa4では、演算部は、各非駆動関節部および各駆動関節部に設けられるブレーキ42にそれぞれ締結指令を与える。締結指令が与えられたブレーキ42は、対応するアーム体の相対変位を阻止する。このように演算部は、各関節部が連結するアーム体の相対変位を阻止させると、ステップa5に進む。ステップa5では、演算部は、多関節アーム30のワーク移動制御動作を終了する。
各サブコントローラ31A〜31Cは、上述したステップa0〜ステップa5の動作をそれぞれ行う。ホストコントローラ32は、協働把持されるワーク40を移動させた場合に、各ハンド部27がそれぞれ順次位置するであろうハンド移動位置を示す信号を、それぞれ対応する各サブコントローラ31A〜31Cに順次与える。
上述したように各駆動関節部にそれぞれ設けられるアーム体の相対変位を固定した状態では、協働把持されるワークを介して各多関節アーム30が互いに支えられ、それぞれの多関節アーム30の変形が阻止される。したがって駆動関節部に連結されるアーム体を、ハンド部27がハンド移動位置に移動したときに位置する相対角度位置に変位させると、非駆動関節部に連結されるアーム体は、駆動関節部に連結されるアーム体の移動に追従して、受動的に相対変位する。
このようにして駆動関節部および非駆動関節部によって連結されるアーム体がそれぞれ相対変位することで、ハンド部27の把持位置のずれ、アーム体の機械的誤差およびサーボ遅れなどの各種の誤差が存在しない理想的な状態では、協働把持するワークを目標位置に位置決めすることができる。
またワーク移動にあたって各種の誤差が存在する現実的な状態では、サーボモータ41によって相対変位されるアーム体は、理想的な状態に比べて、各種の誤差に起因してずれた位置に位置決めされることになる。理想的な状態に比べて2つのアーム体がずれたとしても、非駆動関節部に連結される2つのアーム体は、相対変位が自在であるので、ずれた状態で2つのアーム体を連結することができる。これによって2つのアーム体がずれた状態で無理に連結されることが抑えられ、ワーク40および各アーム体21〜26に生じる変形力を抑えることができる。
図6は、ホストコントローラ32における各サブコントローラ31A〜31Cの制御手順を示すフローチャートである。ホストコントローラ32の演算部は、記憶部に記憶される制御プログラムを読出し、その制御プログラムに従って各サブコントローラ31に動作指令を与える。
まずステップb0で、準備工程として、ホストコントローラ32は、入力部を介して、ワーク40を移動させるために必要な移動プログラムが入力される。移動プログラムは、たとえばワーク40が通過する複数の教示位置、前記教示位置を結ぶ移動経路の種類を示す情報、ワーク40の移動速度および加速度、待機時間などである。入力される移動プログラムは、記憶部に記憶される。各多関節アーム30A〜30Cによって協働把持されるワーク40を移動可能な状態となって、操作者などからワーク40の移動指令が与えられると、ステップb1に進み、各サブコントローラ31A〜31Cへの指令動作を開始する。
ステップb1では、記憶部に記憶される移動プログラムを読出し、ステップb2に進む。ステップb2では、演算部は、読出した移動プログラムに示される命令を解読して、ワーク40の単位時間あたりに変化するワークの位置および姿勢を演算し、ステップb3に進む。ステップb3では、演算部は、ステップb2で演算した演算結果に基づいて、単位時間あたりに変化する各ハンド部27の位置および姿勢をそれぞれ演算し、ステップb4に進む。
ステップb4では、演算部は、ワーク移動指令をサブコントローラ31A〜31Cに送信する。次に、ステップb3で演算した各ハンド部27の移動位置および姿勢を示す信号を、対応するサブコントローラ31A〜31Cに順次送信し、ステップb5に進む。ステップb5では、演算部は、最後のハンド移動位置および姿勢を示す移動終了信号を送信した後、各ハンド部27が終了位置に達したことを示す終了信号を各サブコントローラ31A〜31Cに送信し、ステップb6に進む。ステップb6では、演算部は、各サブコントローラ31A〜31Cへの指令動作を終了する。
本実施の形態では、ワーク40の移動プログラムが記憶部に記憶されたが、注目する1つのハンド部27の移動プログラムが記憶部に記憶されていてもよい。この場合、ステップb2で、注目する1つのハンド部27の移動位置および姿勢を順次演算し、ステップb3で、各ハンド部27のワーク把持位置の関係に基づいて、残余のハンド部27の移動位置および姿勢を順次演算してもよい。このように本実施の形態では、ハンド部27の位置および姿勢の指令値を計算するホストコントローラ32と、各ハンド部27の移動位置および姿勢に基づいて逆変換演算を行う各サブコントローラ31A〜31Cとに分けることによって、1つのコントローラにかかる演算負荷を分担することができ、各多関節アームを円滑に動作させることができる。
以上のように本実施の形態の位置決めロボット20は、1つのワーク40を複数の多関節アーム30A〜30Cによって協働把持させた状態で、ワーク40を搬送および位置決めするロボットである。複数の多関節アーム30A〜30Cによってワーク40を協働把持することで、1つの多関節アームによってワーク40を把持する場合に比べて、各多関節アーム30A〜30Cを小形化することができる。また複数の多関節アーム30A〜30Cの各ハンド部27で、ワーク40を協働把持することで、1つの多関節アームによってワークを把持する場合に比べて、安定してワークを把持することができる。
本実施の形態に従えば、複数の多関節アーム30A〜30Cが、非駆動関節部をそれぞれ有している。非駆動関節部を設けることで、各種の誤差が発生して2つのアーム体がずれたとしても、2つのアーム体がずれた状態で無理に連結されることが抑えられ、ワーク40および各アーム体に生じる変形力を抑えることができ、ワーク40および位置決めロボット20の破損を防ぐことができる。たとえばハンド部27のワークに対する把持位置がばらつく場合であっても、ワーク40などに変形力が与えられることを抑えて、ワーク40などが損傷することを防ぐことができる。
またたとえば各ハンド部27が予め定められる移動経路を正確に通過しない場合であっても、ワーク40などに生じる変形力を抑えることができる。したがってサブコントローラ31が、逆変換演算過程において、解析解ではなく近似解を求め、近似解に基づいて各アーム体の変位位置を決定した場合であっても、協働把持したワーク40を移動させることができる。言換えると、逆変換演算における近似解によってアーム体の変位位置を求めることができるので、本実施の形態のように多関節アーム30A〜30Cのような、ハンド側または基台側の3つの関節部の回転軸線が一点で交わらずに、逆変換演算における解析的の導出が困難である場合でも、ワーク40を円滑に移動させることができる。
また非駆動関節部には、サーボモータ41および減速器55などの駆動手段を不必要とすることができるので、各関節部のすべてが駆動関節部である場合に比べて、位置決めロボットの構造を簡単化することができるとともに、製造コストを低下させることができる。また本実施の形態では、各多関節アーム30A〜30Cに1つ以上の非駆動関節部が設けられることで、各多関節アーム30A〜30Cに与えられる変形力を抑えることができ、各多関節アーム30A〜30Cの破損をより確実に防ぐことができる。ここで、非駆動関節部は、各多関節アーム30がそれぞれ有していてもよく、いずれか1つの多関節アーム30が有していてもよい。また1つの多関節アームが有する非駆動関節部は、協働把持したワーク40を位置決め可能な範囲で、任意の数に設定可能である。
また本実施の形態では、ワーク移動後に各サブコントローラ31が、ブレーキ42を締結状態に切換える。これによって非駆動関節部に連結される2つのアーム体の相対変位を阻止することができ、非駆動関節部を用いたことによる位置決めロボットの剛性不足を補うことができる。このようにワーク40を目標位置および目標姿勢に位置決めした後では、位置決めロボット30の剛性を高めて、ワーク40をより安定して支持することができる。たとえばワーク40を支持した状態でワーク40に加工が成される場合、加工時にワークがずれることを防いで、好適な加工を行うことができる。
また多関節アーム30A〜30Cにそれぞれ非駆動関節部が設けられる場合には、図4(1)〜図4(3)に示すように、非駆動関節部および駆動関節部の選択態様によっては、ワーク40の協働把持が不安定となるおそれが生じる。これに対して、本実施の形態では、各駆動関節部は、連結する2つのアーム体の相対変位をそれぞれ固定した状態で、協働把持するワーク40を介して、各多関節アームが互いに支えられることで各多関節アームの変形が阻止される関節部に選ばれる。ワーク協働把持時において、ワーク40を安定して把持することができ、多関節アーム30A〜30Cにそれぞれ非駆動関節部が設けられても、ワーク40の位置決めを確実に行うことができる。また位置決めロボット30の各関節部が、傾斜関節部と同軸関節部とのいずれかによって実現されることで、ワーク40を移動したほとんどの場合で、協働把持したワーク40が不所望に変位することが防がれ、ワークの位置および姿勢を一定に保つことができる。また本実施の形態では、多関節アーム30A〜30Cが3つ設けられ、それらの基台28が一直線状に並ばないように配置されるので、多関節アーム30が2つ設けられる場合に比べて、協働把持したワーク40が不所望に変位することをさらに防ぐことができる。
また各多関節アーム30A〜30Cが6自由度を有し、駆動関節部が6つ以上設けられることで、ワーク40の位置および姿勢を位置合わせすることができ、ワーク40を予め定める目標位置および目標姿勢に位置決めすることができ、利便性を向上することができる。また非駆動関節部が6つ以上設けられることで、目標位置および目標姿勢に対して、ワーク40が任意の位置および任意の姿勢にずれた状態で把持することができる。
また駆動関節部は、駆動関節部として選択可能な複数の関節部のうちから、基台28側の関節部に設定される。これによってサーボモータ41を多関節アームの根元側に配置させることができ、多関節アーム30のハンド側形状をさらに小形化することができる。ここで、駆動関節部として選択可能な関節部とは、協働保持したワークを任意位置に移動可能とするために最低限必要とする関節部である。
また各多関節アーム30A〜30Cに設けられる関節部が全て駆動関節部である比較例の位置決めロボットの場合には、18つの駆動関節部で6自由度を制御することになり、各多関節アームの同期を厳密に行って、協調制御を行う必要がある。これに対して、本実施の形態では、各多関節アーム30A〜30Cに設けられる9つの駆動関節部で6自由度を制御することになるので、比較例の位置決めロボットに比べて、各多関節アームの同期を厳密に行う必要がなく、協調制御を簡単にすることができる。
図7は、サブコントローラ31Aによるハンド部27の位置決め制御動作を示すフローチャートである。本実施の形態では、各多関節アーム30A〜30Cは、サブコントローラ31によって駆動関節部に設けられるサーボモータ41が制御されることによって、独立してハンド部27を任意の位置に位置決め可能に構成される単独駆動多関節アームとなる。
各多関節アーム30は、ハンド部27を、ワーク把持位置に移動した状態で、各ハンド部27によってワーク40が協働把持された状態で、図5に示すステップa0に進む。多関節アーム30は、図7に示す動作によって、人力または他のロボットの力を借りずに単独でワークを把持位置に移動させる。
各サブコントローラ31A〜31Cは、同様の動作を行うので、第1サブコントローラ31Aについて説明し、残余のサブコントローラ31B,31Cの説明については省略する。第1サブコントローラ31Aの演算部は、記憶部に記憶されるハンド部位置決め制御プログラムを読出し、その制御プログラムを実行することによって、サーボモータ41およびブレーキ42を制御する。
まずステップc0で、サブコントローラ31が、各駆動関節部にそれぞれ連結される各アーム体を角変位可能な状態で、ホストコントローラ32などからハンド部位置決め指令が与えられると、演算部は、ハンド部27の位置決め制御動作を開始し、ステップc1に進む。
ステップc1では、演算部は、出力部を介して、1つの注目する非駆動関節部に設けられるブレーキ42に開放指令を与える。また残余の関節部に設けられるブレーキ42に締結指令を与える。開放指令が与えられたブレーキ42は、対応するアーム体の相対変位の阻止を解除する。また締結指令が与えられたブレーキ42は、対応するアーム体の相対変位を阻止する。このように演算部は、各関節部に設けられるブレーキ42に指令を与えると、ステップc2に進む。
ステップc2では、演算部は、出力部を介して、1または複数の駆動関節部に設けられるサーボモータ41に駆動指令を与える。これによって注目する非駆動関節部が連結するアーム体に与えられる重力の付与方向を変化させ、ステップc3に進む。非駆動関節部が連結するアーム体は、変位が許容された状態であるので、重力の付与方向変化に応じて角変位する。本実施の形態では、非駆動関節部に連結されるアーム体が重力によって予め定める設定相対角度に移動するように、駆動関節部に連結されるアーム体をサーボモータ42によって相対移動させる。この場合、非駆動関節部に連結されるアーム体が重力によって角変位して、設定相対角度に達した状態で停止する。
ステップc3では、演算部は、入力部を介して、注目する非駆動関節部に設けられるエンコーダ42の検出結果に基づいて、非駆動関節部が連結するアーム体が設定相対角度に達したことを判断すると、ステップc4に進む。ステップc4では、演算部は、出力部を介して、注目する非駆動関節部に設けられるブレーキ42に締結指令を与え、ステップc5に進む。これによって注目する非駆動関節部に連結されるアーム体は、設定相対角度に達した状態で角変位が阻止される。
ステップc5では、演算部は、全ての非駆動関節部に連結されるアーム体を、それぞれ個別に設定される設定相対角度で角変位を阻止したか否かを判断する。設定相対角度で角変位を阻止していない非駆動関節部に連結されるアーム体が存在する判断すると、その非駆動関節部を注目する非駆動関節部として再設定し、ステップc2に戻る。また全ての非駆動関節部に連結されるアーム体の角変位を設定相対角度としたことを判断すると、ステップc6に進む。
ステップc6では、演算部は、駆動関節部に設けられるサーボモータ41に駆動指令を与え、駆動関節部に連結されるアーム体を、設定相対角度に角変位させる。角変位させると、駆動関節部に設けられるブレーキ42に締結指令を与え、ステップc7に進む。これによってそれぞれ予め定められる角度位置に、各アーム体の相対角度を位置決めすることができる。ステップc7では、演算部は、ハンド部の位置決め制御動作を終了する。
このように本実施の形態では、非駆動関節部に連結されるアーム体を、外力によって相対変位させた状態で、非駆動関節部に連結されるアーム体が予め定める設定角度に到達すると、アーム体の変位を阻止する。これによってサーボモータ41が設けられない非駆動関節部であっても、連結する2つのアームを設定角度に位置決めすることができる。各多関節アーム30A〜30Cは、人力または他の装置の力を借りることなく単体で、ハンド部27をワーク把持位置に移動させることができ、利便性をさらに向上することができる。このような単独動作多関節アームは、少なくとも最も基台28寄りの1または複数の関節部が駆動関節部として設定されることが好ましい。これによって重力によって、ハンド側の残余の関節部を変位させることができる。また単独動作多関節アームは、単体で動作させることを考えると、重力が作用し難い関節部、図1における第1関節部Jt1や第6関節部Jt6などの垂直関節部は、非駆動関節部としないほうが好ましい。また多関節アームが直線状に変形した状態で、他の非駆動関節部と平行な関節部、たとえば第2関節部Jt2に対する第3関節部Jt3は、非駆動関節部としないほうが好ましい。
また各多関節アーム30A〜30Cを、単体で動作させて、各ハンド部27をワーク把持位置に移動させて、ワーク40を協働把持させた後では、図5に示すように各サブコントローラ31A〜31Cが、各多関節アーム30A〜30Cにそれぞれ設けられるサーボモータ41を制御することで、非駆動関節部のブレーキ42を開放した状態で、協働把持するワーク40を任意の位置に移動させることができる。
また本実施の形態では、非駆動関節部に連結されるアーム体に生じる重力の方向を変化させることによって、非駆動関節部に連結されるアーム体を角変位させたが、重力以外の外力、たとえばサーボモータを回転させて、慣性力をアーム体に与えることによって、非駆動関節部に連結されるアーム体を角変位させてもよい。また位置決めロボットがワークを支持固定する治具ロボットとして用いられることで、高速の位置決めが要求されることが少なく、多関節アームのハンド部を設定角度に位置決めするのに時間を要したとしても、治具ロボットとして好適に用いることができる。
また本実施の形態では、サーボモータ41を回転させて、非駆動関節部に連結されるアーム体を設定角度に固定したが、これに限定されない。たとえばステップc2のサーボモータ41の動作に換えて手動または他のロボットによって外力をハンド部27に与えて、残余のステップc1、c3〜c7の動作を行ってハンド部27を把持位置に案内してもよい。またサーボモータ41の動作量を規定することで、非駆動関節部に連結されるアーム体の角度が定量的に決定される場合には、エンコーダ43が設けられなくてもよい。また仮にハンド部27の把持位置が少々ずれても、上述したように非駆動関節部が設けられることで、把持位置の移動ずれを吸収することができる。
図8は、本発明の第2実施形態である位置決めロボット120を示すブロック図である。また図9は、位置決めロボット120に協働把持されるワーク140を示す図である。第2実施形態である位置決めロボット120は、第1実施形態の位置決めロボット20と対応した構成を示す。対応する構成については、第1実施形態の位置決めロボット20に示される構成の参照符号に100を付加した参照符号を付する。第2実施形態の位置決めロボット120は、関節部の種類が異なるほかは、第1実施形態の位置決めロボット120と同様の構成を示し、同様の構成については説明を省略する。
位置決めロボット120を用いて、駆動関節部の位置決めに誤差が生じている場合についての計算結果を以下に示す。位置決めロボット120は、3つの多関節アーム130A〜130Cを有し、それぞれ同一に構成される。各多関節アーム130A〜130Cは、直列に並ぶ6つのアーム体121〜126と、隣接する2つのアーム体を回転自在に連結する6つの関節部Jt2〜Jt7と、ハンド部127と、基台128とを含む。各アーム体121〜126は、それぞれ連結されて直列状に延びるアーム構成体129を構成する。
第2関節部Jt2は、駆動傾斜関節部となり、基台128に固定される第1アーム体121と第2アーム体122とを連結する。また第3関節部Jt3は、非駆動傾斜関節部となり、第2アーム体122と第3アーム体122とを連結する。また第4関節部Jt4は、駆動傾斜関節部となり、第3アーム体123と第4アーム体124とを連結する。また第5関節部Jt5は、非駆動同軸関節部となり、第4アーム体124と第5アーム体125とを連結する。また第6関節部Jt6は、非駆動傾斜関節部となり、第5アーム体125と第6アーム体126とを連結する。第7関節部Jt7は、非駆動同軸関節部となり、第6アーム体126とハンド部127とを連結する。
各アーム体121〜126の軸線が一直線状に配置される状態では、第2関節部Jt2と第3関節部Jt3との回転軸線A2,A3が平行に延び、第3関節部Jt3と第4関節部Jt4との回転軸線A3,A4が垂直に延びる。また各傾斜関節部Jt2,Jt3,Jt4,Jt6は、連結するアーム体の軸線から45で傾斜する回転軸線A2,A3,A4,A6を有する。また本実施の形態では、逆変換演算が行いやすいように、第5関節部Jt5と、第6関節部Jt6と、第7関節部Jt7との回転軸線A5,A6,A7が、第6関節部Jt6に設定される3軸交点Pwで交差する。
図9に示すように、本実施の形態では、ワーク140は、正三角形板状に形成される。各ハンド部127は、ワーク140の3つの頂点位置をそれぞれ把持するとともに、ワーク140の三角形平面に対してそれぞれ垂直にワーク140を把持すると想定する。
各多関節アーム130A〜130Cの基台128は、予め定めるワールド座標系のX軸とY軸とを含む平面上に配置され、ワールド座標系のZ軸まわりにワールド原点P0を一周する円周上に等間隔にそれぞれ配置される。各多関節アーム130A〜130Cは、アーム構成体129の軸線が一直線状に延びるように変形すると、その軸線がワールド座標系のZ軸に平行に延びる。
各多関節アーム130A〜130Cの基台128には、ベース座標系が設定される。ベース座標系は、一直線状に延びるように変形したアーム構成体129の軸線と、ワールド座標系のX軸とY軸とを含む平面とが交差する点がベース原点PBとして設定される。ワールド座標系のZ軸と、ベース座標系のZ軸とは、互いに平行に延びる。またワールド座標系のX軸とY軸とを含む平面と、ベース座標系のX軸とY軸とを含む平面とは、同一平面内に位置する。
ワールド座標系に対する、ワーク140の位置および姿勢が指定されると、ワールド座標系における各多関節アーム130A〜130Cの三軸交点Pwを決定することができる。またワールド座標系とベース座標系とで座標系変換を行うことで、個々の多関節アーム130A〜130Cに設定されるベース座標系での三軸交点Pwの3次元位置を定めることができる。
第k多関節アームのベース座標系における3軸交点Pwαkの3次元位置(Pwxαk,Pwyαk,Pwzαk)Tと、第k多関節アームの第2関節部Jt2k〜第4関節部Jt4kの角度q2k〜q4kとには、次の(1)式に示す関係を有する
ここで、添え字kは、第k多関節アームを示す記号である。たとえばPwxα1は、ベース座標系における、第1多関節アーム130Aの3軸交点PwのX座標位置を示す。またq21は、第1多関節アーム130Aの第2関節部Jt2の角度を示す。ここで、アーム構成体129の軸線が一直線状に延びた状態を基準状態とすると、q2kは、基準状態において第1アーム体121に対して第2アーム体122が相対角変位する角度である。またq3は、基準状態において第2アーム体122に対して第3アーム体123が相対角変位する角度である。またq4は、基準状態において第3アーム体123に対して第4アーム体124が相対角変位する角度である。このようにアーム体の相対角度を、関節部の角度と称する場合がある。
またL1は、ベース原点から第2関節部Jt2までの距離であり、L2は、第2関節部Jt2から第3関節部Jt3までの距離である。またL3は、第3関節部Jt3から第4関節部Jt4までの距離であり、L4は、第4関節部Jt4から第6関節部Jt6までの距離である。
ワーク140の3次元位置および姿勢が決定されると、ベース座標系における各多関節アームの3軸交点Pwを求めることができ、求められた3軸交点Pwを(1)式に代入することによって、各多関節アームの第2〜第4関節部の角度を求めることができる。本実施の形態では、L1=280mm、L2=290mm、L4=290mm、L3=160mm、L5=180mm、Lb=408mm、Lh=433mmである。ここでLbは、ワールド原点P0から各基台128まで距離であり、Lhは、ワーク140の一辺の長さ(=2・Lh)の半分の値である。また、把持位置で協働把持されるワーク140の中心位置の、ワールド座標系における3次元位置(Pgx,Pgy,Pgz)Tが、(0,0,910)T[mm]であり、ワーク140の3次元姿勢(PgRx,PgRy,PgRz)が(0,0,46.225)[deg]であるとする。この場合、各多関節アームの第2関節部の角度q21,q22,q23はそれぞれ60度となる。また第3関節部の角度q31,q32,q33はそれぞれ−60度となる。また第4関節部の角度q41,q42,q43はそれぞれ120となる。
第k多関節アームのワールド座標系における3軸交点Pwβkの3次元位置(Pwxβk,Pwyβk,Pwzβk)Tは、以下の(2)式に示す関係を有する。
ここで第1多関節アーム、すなわちk=1ではθ=0である。第2多関節アーム、すなわちk=2ではθ=120である。また第3多関節アーム、すなわちk=3ではθ=240である。また(Pwxαk,Pwyαk,Pwzαk)Tは、第k多関節アームにおける第2〜第4関節部の角度q2k〜q4kを(1)式に代入して求めた、ベース座標系における第k多関節アームの3軸交点Pwαの3次元位置であり、第k多関節アームにおける第2〜第4関節部の角度q2k〜q4kを変数として含む関数である。
また第1多関節アーム130Aに設定される各ハンド部27がワーク140を協働把持することで、上述する(2)式は、次の(3)式の関係を満たすように条件が拘束される。
ここで、(a)式のうちの左項をC3とし、(b)式のうちの左項をC1とし、(c)式のうちの左項をC2とし、次の(4)式および(5)式に示す各項を定義する。
ワーク140が剛体であると仮定すると、いずれの多関節アームが変形しても、ハンド部の間の距離が不変である。このことから、次の(6)式が成立する。
(6)式について、各多関節アームに関して、非駆動関節部の角度の変化量Δq3kと、駆動関節部の角度の変化量Δq2k,Δq4kとに分離することで、次の(7)式に変形することができる。
(7)式から、駆動関節部の位置決めに誤差があった場合の、非駆動関節の誤差の吸収量を計算することができる。上述した各多関節ロボットの形状(L1〜L4)等と、ワーク協働把持状態での各関節部の角度q2k,q3k,q4kとを代入して、Jlを求めると、次の(8)式が求められる。
たとえば駆動関節部の角度q2k,q4kのうちで、第1多関節アームの第2関節部Jt2の角度q21が指令値に対して1度ずれて移動し、残余の駆動関節部の角度q22,q23,q41,q42,q43が指令値に厳密に移動した場合、非駆動関節の角度(q31,q32,q32)Tは、移動すべき角度から、(−1.14923,0.255486、−0.265846)T[deg]だけ誤差が生じて位置に動く。したがって第1多関節アームの第3関節部の角度q31は、移動すべき角度(=−60度)に対して、上述した誤差角度(=−1.14923度)分ずれる。
このとき、ワーク140の中心位置の、ワールド座標系における3次元位置(Pgx,Pgy,Pgz)Tは、指令値に対して(0.650269,1.67466,−0.852597)T[mm]だけずれており、ワーク140の3次元姿勢がZ軸に垂直な平面に対して、0.537118[deg]だけ傾く。
ワークの中心位置は、各多関節アームの3軸交点Pwβkの3次元位置(Pwxβk,Pwyβk,Pwzβk)Tの重心位置Pwβ0(=(Pwβ1+Pwβ2+Pwβ3)/3)を求める。次に、各多関節アームの3軸交点Pwβkについて、水平面に対する方向ベクトルを求め、重心位置Pwβ0に対して、前記方向ベクトルの方向にL5を加算することによって、ワーク140の中心位置を求めることができる。
以上のように、に第2実施形態の位置決めロボット120は、各多関節アーム130A〜130Cの第3関節部Jt3が各非駆動関節部としてそれぞれ設定されることで、駆動関節部の角度に誤差が生じたとしても、その誤差に追従して、非駆動関節部の角度が変化する。これによって(6)式を満足することができ、各アーム体およびワークが変形することなく、ワーク140を協働把持させることができ、ワーク140およびアーム体が損傷することを回避することができる。
また駆動関節部の角度ずれに追従して非駆動関節部の角度が変化することについて説明したが、サーボ誤差、ワークの把持誤差、アーム体およびワークの形状誤差、各多関節アームの配置誤差などが生じる場合であっても同様であって、誤差に追従して非駆動関節部の角度が変化する。また非駆動関節部による誤差の吸収について、第2実施形態の位置決めロボット120を用いて説明したが、図1に示す第1実施形態の位置決めロボット20によっても同様に、非駆動関節部によって誤差を吸収することができる。
また第2実施形態の位置決めロボット120は、第1実施形態と同様の効果を得ることができる。さらに位置決めロボット120は、各多関節アーム130A〜130Cに設けられる6つの駆動関節部で、6自由度を制御することになり、協調制御の必要がなくなる。これによって各多関節アーム130A〜130Cの同期を厳密に一致する必要が防がれ、ワークの移動をさらに容易に行うことができる。また第5〜第7関節部の回転軸線A5〜A7が3軸交点Pwで交差するので、逆変換演算を容易に行うことができる。
図10は、位置決めロボット120の自立条件を説明するための図である。図10に示すように、6つの関節部をそれぞれ有する3つの多関節アーム130A〜130Cを備える位置決めロボット120の自立条件を以下に求める。各多関節アーム130A〜130Cの関節部の総数は18個であり、そのうち駆動関節部は6個であり、非駆動関節部が12個であるとする。
ワーク140の位置姿勢を表わす行列をワーク行列Wとする。ワーク行列Wは、ワーク140の3次元位置(Wx,Wy,Wz)と、ワーク140の3次元姿勢(WRx,WRy,WRz)とを含む6つの変数を有する1行6列(Wx,Wy,Wz,WRx,WRy,WRz)Tの行列である。また3つの多関節アーム130A〜130Cのうち、第1多関節アーム130Aの各関節部の角度をそれぞれ表わす行列を第1アーム角度行列Q1とする。第kアーム行列Qkは、第K多関節アーム130A〜130Cの各関節部Jt1k〜Jt6kのそれぞれの角度q1k〜q6kを含む6つの変数を有する1行6列(q1k,q2k,q3k,q4k,q5k,q6k)Tの行列である。この場合、以下の関係式を満足する。
W=F1・Q1 …(9)
W=F2・Q2 …(10)
W=F3・Q3 …(11)
ここでF1〜F3は、各多関節アームの関節部の種類およびアームの長さによって求められる6行6列の行列式であって、順変換式に基づいて求められる変換行列である。
また第1多関節アーム130Aと、第2多関節アーム130Bおよび第3多関節アーム130Cは、それぞれハンド部127によって剛体結合していることから、次式の拘束条件におけるC2とC3とは、ゼロとなる。
C2=F1・Q1−F2・Q2 …(12)
C3=F1・Q1−F3・Q3 …(13)
またワーク行列Wに関して、ワーク140の位置姿勢の微小変化量を表わす行列をdWとする。また第kアーム角度行列Qkに関して、第k多関節アームの各角度の微小変化量を表わす行列をdQkとする。また(10)式で表わされるC2およびC3の微小変化をdC2,dC3とする。この場合、以下のヤコビ行列J1,J2,J3を定義する。各ヤコビ行列J1〜J3は、()内の各要素を変数とする関数である。
dW=J1(dQ1) …(14)
dC2=J2(dQ1,dQ2) …(15)
dC3=J3(dQ1,dQ3) …(16)
第1〜第3アーム角度行列Q1〜Q3の計18個の変数q11〜q61,q12〜q62,q13〜q63のうちから、6個を駆動関節部の角度を示す変数として選び、それらを要素とする1行6列の行列をQaとする。また残りの12個を非駆動関節部の角度を示す変数として選び、それらを要素とする1行12列の行列をQpとする。またQaの微小変化を示す行列をdQaとし、Qpの微小変化を示す行列をdQpとする。そして(14)(15)(16)を連立して変形すれば、以下の式を得ることができる。
dW=Ja・dQa …(17)
dQp=Jp・dQa …(18)
ここで、Jaは、6行6列の行列であり、Jpは、12行6列の行列である。
駆動関節部を位置決めしているにもかかわらず、ワーク140が動いてしまう状態、いわゆる自立できない状態とは、(17)式において、dQaがゼロにもかかわらず、dWがゼロでない値を持つことに相当する。この現象は、ヤコビ行列Jaの逆行列Ja−1がゼロとなるときに生じる。このように逆ヤコビ行列Ja−1の行列式がゼロになる状態は、ワークが「思案点」と呼ばれる位置に到達することを意味する。
ヤコビ行列Jaの行列式がゼロになる特異点に達した状態が、開ループリンク系、閉ループリンク系のいずれにも存在するのに対し、思案点に達する状態は、非駆動関節部を有する閉ループリンク系のみ存在し得る。また、特異点は、制御上、到達しても、そこから容易に脱出できるのに対し、思案点は、到達してしまうと、もとの形態に戻れないこともあり、ワークが思案点に到達することを避けるべきである。このことは、本実施の形態の位置決めロボット全般にいえることである。
したがって全18個の関節から、6つの駆動関節部を選ぶ選び方としては、(17)式の逆ヤコビ行列Ja−1の行列式が、全空間、目的とするワーク移動空間、またはワークの移動経路上で、ゼロにならない駆動関節部に選択すればよい。このことはn個の駆動関節とm個の非駆動関節とを有する多関節ロボットがl個ある場合も同様である。ここで、n,m,lは、任意の整数である。また傾斜関節部を有する多関節ロボット以外、たとえば垂直多関節アームを有する位置決めロボットであっても、同様であって逆ヤコビ行列Ja−1の行列式がゼロとならないことが、各アームを自立させてワークの変形を阻止する自立条件となる。
ホストコントローラは、(17)式に示す逆ヤコビ行列Ja−1の行列式を演算可能な関数式を記憶しており、駆動関節部から与えられる各変位量に基づいて、逆ヤコビ行列Ja−1の行列式を随時演算してもよい。ティーチング時などに、各多関節アーム130A〜130Cによって、協働把持したワーク140をテスト移動させるときに、逆ヤコビ行列Ja−1の行列式が予め定めるゼロ近傍の値に達すると、出力手段によってワーク140が思案点に近づいていることを示す情報を報知して、サーボコントローラにロボットの動作停止指令を与えてもよい。これによって各多関節アームが自立できない状態となることを防ぐことができる。
また出力手段によって、逆ヤコビ行列Ja−1の行列式の絶対値が大きくなる関節部の角度回転方向を表示させてもよい。この場合、指定される駆動関節部を現在位置からプラス方向およびマイナス方向に移動させて、逆ヤコビ行列Ja−1の行列式の絶対値が大きくなる回転方向を出力させる。これによってワーク140が思案点から遠ざかることを作業者が容易に判断することができ、利便性を向上することができる。
このようにホストコンピュータまたはサブコンピュータが、予め定めるプログラムを実行することによって、思案点を回避するように多関節アームを制御する制御装置となり、前記逆ヤコビ行列Ja−1の行列式の絶対値を計算する計算部と、前記逆ヤコビ行列Ja−1の行列式の絶対値が予め定める設定値以下であるか否かを判定する判定部と、判定部において前記逆ヤコビ行列Ja−1の行列式の絶対値が前記設定値よりも小さいと判定されると、前記逆ヤコビ行列Ja−1の行列式の絶対値が前記設定値よりも大きくなるように駆動関節部の角度を角変位させる制御部を含むコンピュータも本発明に含まれる。またワークを移動させるにあたって、計算部による計算段階、判定部による判定段階および制御部による制御段階を繰返して、思案点を回避するように駆動関節部の角度を制御することも本発明に含む。
図11は、本発明の第3実施形態である位置決めロボット220を示す斜視図である。第3実施形態である位置決めロボット220は、第1実施形態の位置決めロボット20と対応した構成を示す。対応する構成については、第1実施形態の位置決めロボット20に示される構成の参照符号に、200を付加した参照符号を付する。第3実施形態の位置決めロボット220は、各多関節アームが6自由度を有する垂直多関節型に形成されるほかは、第1実施形態の位置決めロボット120と同様の構成を示し、同様の構成については説明を省略する。
各多関節アーム230A〜230Cは、基台228からハンド部227に向かってアーム体221〜226の直列方向に進むにつれて、第1〜第6関節部Jt1〜Jt6が順に並んで設けられる。第1関節部Jt1は同軸関節部となり、第2関節部Jt2は垂直関節部となり、第3関節部Jt3は、垂直関節部となる。また第4関節部Jt4は同軸関節部となり、第5関節部Jt5は垂直関節部となり、第6関節部Jt6は、同軸関節部となる。
本実施の形態では、第1〜第3関節部Jt1〜Jt3が駆動関節部となり、第4〜第6関節部が非駆動関節部となる。また各多関節アーム230A〜230Cの基台228をそれぞれ結ぶ線分によって三角形が形成される。駆動関節部は、連結する2つのアーム体の相対変位をそれぞれ固定することで、協働把持するワーク240を介して、各多関節アーム230A〜230Cが互いに支えられることで各多関節アーム230A〜230Cの変形が阻止される関節部に選ばれる。
各多関節アーム230A〜230Cの第3関節部Jt3と第5関節部Jt5とが、それぞれ平行に延びた状態では、それぞれが非駆動関節部であるとワーク240が垂直方向に移動可能となってしまう。また各多関節アーム230A〜230Cの第1関節部Jt1と第6関節部Jt6とが、それぞれ平行に延びた状態では、それぞれが非駆動関節部であるとワーク240が水平方向に移動可能となってしまう。これに対して、本実施の形態では、上述したように、第3関節部Jt3と第5関節部Jt5とが駆動関節部となることによって、ワーク240が不所望に変位することを防ぐことができる。
このような第3実施形態の位置決めロボット220であっても、第1および第2実施形態の位置決めロボット20,120と同様の効果を得ることができる。すなわち協働把持したワーク240を移動させるにあたって、非駆動関節部が設けられることで、誤差に起因して発生するワーク240および位置決めロボット220に与えられる変形力を抑えることができ、ワーク240および位置決めロボット220の破損を防ぐことができる。このような第3実施形態の位置決めロボット220であっても、上述した自立条件を満足するように、駆動関節部が選択されるとともに、ワーク240の移動可能領域または移動経路が決定されることが好ましい。
以上のような各本実施の形態は本発明の例示に過ぎず、発明の範囲内において構成を変更することができる。たとえば本発明は、複数の多関節アームによってワーク40を協働把持し、多関節アームの少なくとも1つの関節部が非駆動関節であれば適用可能である。たとえば全ての関節部が駆動関節部となる多関節アームと、全ての関節部が非駆動関節部となる多関節アームとによってワークを協働把持する位置決めロボットも本発明に含まれる。また多関節アームが有する非駆動関節部は、任意の数に設定可能である。
また3つ以上の多関節アームによってワークを協働把持することが好ましいが、2つの多関節アームによってワークを協働把持する位置決めロボットもまた本発明に含まれる。また異なる種類の多関節ロボットによってワークを協働把持してもよい。また非駆動関節部にブレーキ41およびエンコーダ42が設けられない構成も本発明に含まれる。また本実施の形態では、サブコントローラ32とホストコントローラ31を用いたが、それらの動作を行う1つのコントローラによって、各サーボモータ41が制御されてもよい。