(57)【要約】【課題】  ワーク搬送テーブルが傾斜した状態にあっても、ロボットの位置・姿勢を制御可能な作業ロボットの駆動制御システムを提供する。【解決手段】  ロボットに対する作業指令プログラムをストアしている第１記憶手段10A、及び回転行列Rzyx (α、β、γ)の内容ならびにワークテーブルの設置及び動作状態に関する信号をパラメータデータとしてストアする第２記憶手段10Bから構成されるデータメモリ10を有し、第１演算手段12は、前記作業指令プログラムを解析し、次のサンプル処理時刻にロボットが移動すべき位置データを、第１指令値として算出する。  第２演算手段16は、回転演算機能16Aと直線演算機能16Bとからなる。第２演算手段では、前記第１演算手段から与えられる第１指令値に対し、回転演算機能または直線演算機能により補正を加え、次のサンプル処理時刻にロボットの加工ヘッドが移動すべき最終の位置データをサーボ系28に与える。【選択図】  図８
2005332001.tif 000002
【特許請求の範囲】
【請求項１】
ワークと、同ワークを所定位置に搭載するワークテーブルと、同ワークテーブルとは別の位置に配置され当該ワークテーブル上のワークに対し加工作業を遂行する加工ヘッドを有する多関節型の作業ロボットと、前記加工ヘッドによる前記ワークに対する加工作業を遂行するよう加工ヘッドの姿勢を制御するロボット制御装置及び前記ワークテーブルの運動に基づく前記ワークの前記作業ロボットに対する相対的変位を検出する第１検出手段とを備えたシステムであって、
前記ロボット制御装置は、
前記ワークが作業ロボットに対して相対的に静止状態にあるとき前記加工作業を遂行せしめるための一連の作業指令手順からなる作業指令プログラムを貯蔵する第１記憶手段と、
同第１記憶手段から読み出された作業指令プログラムに基づいて前記作業ロボットの各駆動機構に対する第１指令値を生成する第１演算手段と、
前記ワークテーブルの移動に基づき前記ワークと作業ロボットとが相対的に移動状態にあるとき前記第１検出手段からの検出信号に基づいて前記第１演算手段から与えられる第１指令値を補正する第２演算手段と、
を備え、
さらに前記第２演算手段は、前記ワークテーブルが回転運動する場合に前記作業ロボットに対する前記ワークテーブルの回転軸周りの回転角変位量α及び前記回転軸のワークテーブル面への傾斜角成分β、γにより定義される回転行列を用いる回転演算機能を遂行すると共に、前記回転角変位量αは前記第１検出手段からの検出信号によりワークテーブルの回転運動中逐次与えられ、前記傾斜角成分は前記ワークテーブルの作業ロボットベース座標系に対する予め測定されたパラメータ値として与えられ、前記制御装置は前記回転行列ならびに前記パラメータ値α及びβ、γを貯蔵する第２記憶手段を備えていることを特徴とする多関節作業ロボットの駆動制御システム。
【請求項２】
請求項１において、前記ワークテーブル回転軸の第１方向及び第２方向への傾斜角成分を検出する第２検出手段を設け、同第２検出手段からの検出信号に基づいて前記第２記憶手段内のパラメータ値β、γを設定するようにしたことを特徴とする多関節作業ロボットの駆動制御システム。
【請求項３】
請求項１または２において、前記ワークテーブルが直線移動する場合であって、前記第２演算手段は、前記ワークテーブルの直線移動に伴う補正量を生成する直線演算機能をさらに有し、前記制御装置には前記ワークテーブルの運動状態に応じて前記２演算手段の前記各演算機能を指定する指定手段を備えたことを特徴とする多関節作業ロボットの駆動制御システム。
【請求項４】
請求項３において、前記指定手段は前記第２演算手段の回転演算機能及び直線演算機能の両方を指定することを特徴とする多関節作業ロボットの駆動制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、多関節型の作業ロボットに係り、特にワークテーブルが作業ロボットに対し相対移動する環境下で、当該ワークテーブル上に搭載されたワークに対し、塗装、描画等の作業（以下加工作業という）を当該作業ロボットの加工ヘッドを介して遂行する場合における作業ロボットの駆動制御システムに関する。
【背景技術】
【０００２】
図９は作業ロボットの加工ヘッドが稜線を倣う動作を行うためのワークピースＷを示す。作業ロボットの座標系（以下、ロボットのベース座標系あるいはワールド座標系という）に対し、ある場所に搭載された前記ワークピースＷ（以下、ワークという）の稜線を倣う動作を行う場合、次のようなロボットプログラムを作成する。
ＰＲＯＧＲＡＭ  ＲＹＯＳＥＮ
ＭＯＶＥＳ  Ｐ１
ＭＯＶＥＳ  Ｐ２
ＭＯＶＥＳ  Ｐ３
ＭＯＶＥＳ  Ｐ４
ＥＮＤ
【０００３】
上記プログラムはあくまでロボットのベース座標系に対し静止・固定されたワークＷへの倣い動作プログラムであるため、仮に当該ワークＷがコンベア等により常時スライドするような場合、従って、ワークＷの位置・姿勢がロボットのベース座標系からみて移動変化する場合、ロボットの先端部即ち、加工ヘッドの先端部はワーク稜線から離れ、倣い動作はできない。
図１０は、コンベアCNVに取付けられたエンコーダENCから、処理周期毎にパルスカウンタを読取り可能なシステムを例示するものであって、同図１０において、コンベアCNVに取付けられたエンコーダENCがE1(pulse)カウントした場合、コンベアベクトル（単位ベクトル）
【０００４】
【数１】
2005332001.tif 000003
とし、エンコーダ1パルスに相当するコンベア移動量をRefとすると、P1点はロボットのベース座標系から見て
【０００５】
【数２】
2005332001.tif 000004
だけ移動したことになる。
するとプログラムによる動作指令位置に上記ベクトルΔＤを加算した位置をサーボ部へ指令することで、見かけ上コンベアCNVに追従しながら、同コンベア上のワークＷに対する倣い動作が可能となる。前記プログラム中の 「MOVES P1」について動作した時の処理の流れをブロックで図１１に示す。
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、図１０、図１１に示した方法はあくまでコンベアべクトルＶcnv即ち、直線移動型のコンベアにしか対応できず、例えば、水平面で回転するテーブル上にワークを搭載して同回転テーブルの回転中に当該ワークに対し種々の加工作業を遂行する場合や、同一テーブル上で他のワークを別のロボットで加工処理する場合及び加工作業ラインの柔軟性のため前記回転テーブルの回転軸がロボットのベース座標に対して傾斜させることが必要な場合、あるいは回転テーブル自体が直線移動コンベア上に搭載されて配置されるといった場合におけるワークへの加工作業には対応できないという問題点があった。
【０００７】
本願発明者は上述した問題点を解決せんとして鋭意検討した結果、ワークが、オイラー角と呼ばれる直交するＸ、Ｙ、Ｚの３つの座標軸の周りで所定角回転した座標系上にある場合、ロボットのベース座標系から見た当該ワーク上の位置を算出するため回転行列を用いることによって解決できることを突き止めた。
【０００８】
従って、本発明の目的は、ワークを搭載したワークテーブルが直線移動、回転運動のいずれの駆動形態であっても、さらにまた前記ワークテーブルの回転軸が傾斜した状態にあっても、ロボットのベース座標系から見て前記ワークと当該ロボットの加工ヘッドとが相対的に静止するように当該ロボットの加工ヘッドの位置・姿勢を制御可能な多関節作業ロボットの駆動制御システムを提供することにある。
【課題を解決するための手段】
【０００９】
前記目的を達成するため、本発明による多関節作業ロボットの駆動制御システムは、
ワークと、同ワークを所定位置に搭載するワークテーブルと、同ワークテーブルとは別の位置に配置され当該ワークテーブル上のワークに対し加工作業を遂行する加工ヘッドを有する多関節型の作業ロボットと、前記加工ヘッドによる前記ワークに対する加工作業を遂行するよう加工ヘッドの姿勢を制御するロボット制御装置及び前記ワークテーブルの運動に基づく前記ワークの前記作業ロボットに対する相対的変位を検出する第１検出手段とを備えたシステムにおいて、
前記ロボット制御装置は、
前記ワークが作業ロボットに対して相対的に静止状態にあるとき前記加工作業を遂行せしめるための一連の作業指令手順からなる作業指令プログラムを貯蔵する第１記憶手段と、
同第１記憶手段から読み出された作業指令プログラムに基づいて前記作業ロボットの各駆動機構に対する第１指令値を生成する第１演算手段と、
前記ワークテーブルの移動による前記ワークと作業ロボットとが相対的に移動状態にあるとき前記第１検出手段からの検出信号に基づいて前記第１演算手段から与えられる第１指令値を補正する第２演算手段と、
を備え、
さらに前記第２演算手段は、前記ワークテーブルが回転運動する場合に前記作業ロボットに対する前記ワークテーブルの回転軸周りの回転角変位量α及び前記回転軸のワークテーブル面への傾斜角成分β、γにより定義される回転行列を用いる回転演算機能を遂行すると共に、前記回転角変位量αは前記第１検出手段からの検出信号によりワークテーブルの回転運動中逐次与えられ、前記傾斜角成分は前記ワークテーブルの作業ロボットベース座標系に対する予め測定されたパラメータ値として与えられ、前記制御装置は前記回転行列ならびに前記パラメータ値α及びβ、γを貯蔵する第２記憶手段を備えて構成される。
その場合、前記ワークテーブル回転軸の第１方向及び第２方向への傾斜角成分を検出する第２検出手段を設け、同第２検出手段からの検出信号に基づいて前記第２記憶手段内のパラメータ値β、γを設定するように構成することができる。
その場合さらに、前記ワークテーブルが直線移動する場合であって、前記第２演算手段は、前記ワークテーブルの直線移動に伴う補正量を生成する直線演算機能をさらに有し、前記制御装置には前記ワークテーブルの運動状態に応じて前記２演算手段の前記演算機能を指定する指定手段を備えて構成することもできる。
【発明の効果】
【００１０】
本発明による多関節作業ロボットの制御システムは、
第１検出手段からの検出信号に基づいて前記第１演算手段から与えられる第１指令値を補正する第２演算手段を有し、さらに前記第２演算手段は、前記ワークテーブルが回転運動する場合に前記作業ロボットに対する前記ワークテーブルの回転軸周りの回転角変位量α及び前記回転軸のワークテーブル面への傾斜角成分β、γにより定義される回転行列を用いる回転演算機能を遂行するようになっているので、ワークテーブル上に搭載されたワークがロボットのベース座標系に対し回転運動した場合及び、さらに当該テーブルの回転軸が傾斜している場合においてもワークとロボットの加工ヘッドを相対的に静止状態にすることができる。
【発明を実施するための最良の形態】
【００１１】
以下図１乃至図９により本発明の実施態様に係る実施例について詳細に説明する。
図１は、回転可能な水平面を有する円弧形状コンベア（以下、コンベアまたはワークテーブルという）ＴＢの部分平面図で、コンベアの動きにロボットを追従制御させるコンベア同期処理の様子を示す。同図１でワークテーブルＴＢ上の所定位置には図１０に示されたワークＷが取付けられている。また、ワークテーブルＴＢの手前側には多関節作業ロボットＲＢＴが配置されている。参照符号ENCはエンコーダであって、ワークテーブルＴＢのＺ軸周りの回転角の変化分を検出する。参照符号ＣＴＲはロボットＲＢＴの制御装置であって、エンコーダＥＮＣからの検出信号によりワークテーブルＴＢの基準位置からみた現在の回転角位置データを常時記憶する。参照符号ＴＲＧはワークテーブルＴＢ上のワークＷとロボットＲＢＴの相対位置に関して同期をとるために、テーブル外周部の所定位置に配置された同期開始トリガセンサであり、レーザ光線等の遮断により同期開始角位置を検出することができ、この検出信号は前記制御装置ＣＴＲへ与えられる。同図１においてはワークテーブルＴＢ上のワークＷがラインＬ０を通過しラインＬ１に達したとき前記トリガ信号が発せられる。
【００１２】
一方、ロボットＲＢＴの加工ヘッドＷＨＤは予め待機位置からラインＬ２に対応するコンベア同期開始位置Ａに位置付けされており、ワークＷがラインＬ２に達したときから前記加工ヘッドＷＨＤによるワークＷへの加工動作例えば、稜線倣い動作が行われる。この稜線倣い動作はラインＬ３に対応するコンベア同期終了位置Ｂまで連続して遂行される。
【００１３】
具体的にこのプロセスを説明すると、前述したように稜線倣い動作のプログラムは、
ＰＲＯＧＲＡＭ  ＲＹＯＳＥＮ
ＭＯＶＥＳ  Ｐ１
ＭＯＶＥＳ  Ｐ２
ＭＯＶＥＳ  Ｐ３
ＭＯＶＥＳ  Ｐ４
ＥＮＤ
である。
【００１４】
上記プログラムによりロボットＲＢＴがワークＷの稜線を倣うプログラム中の 「MOVES P1」については、「プログラム解析」、次いで「Ｐ１への軌道生成」、さらに「分配目標位置算出」という手順を経て各制御軸サーボ部へ指令される。
図２はこの時の分配位置指令値P1[0]、P1[1]、P1[2]の様子を示す。
これら分配位置指令値P1[0]、P1[1]、P1[2]…の全てに対して図３のP1’[0]に示すような座標変換処理を行うことで、見かけ上ワークテーブルＴＢの回転に追従しながら、ワークＷの稜線P０〜Ｐ３の倣い動作が可能となる。
【００１５】
ここで、上記座標変換は、図４に示されるように、制御周期毎にエンコーダＥＮＣのパルスカウンタ値を、角度補正量ｄθ(deg)に換算し、これをもとにワークテーブルＴＢの回転軸（Ｚ軸）周りの補正用の回転行列を求める。この回転行列によりロボットＲＢＴのワールド座標系分配位置指令値P１[0]を座標変換して補正位置指令値 P１’[0]を算出する。この補正位置指令値の算出方法は図５及び下記式３を参照して明らかなように、  回転軸座標系から見た、ロボットＲＢＴのワールド座標系位置指令値ベクトル「O-P1［0］」を座標変換してコンベア同期位置ベクトル「O-P1’ ［0］」を（式４）により算出する。続いてこれをワールド座標系から見た位置ベクトル「P1’ ［0］」を（式５）により変換する。
【００１６】
【数３】
2005332001.tif 000005
なお、上記（式３）において、Ｒｚ（θ）はＺ軸の周りに角度θ回転させたときに対応する回転行列である。
【００１７】
図６は、前記プログラム中の 「MOVES P1」について動作した時の処理の流れをブロックで示す。図１１に示した処理の流れと対比すると、図１１においては、プログラム解析により算出された分配目標位置にエンコーダパルスから換算される直線移動量を加算するのに対し、図６においては、プログラム解析により算出された分配目標位置に対してエンコーダパルスから換算される角度に基づく座標変換を行うことによって各制御軸のサーボ部へ指令値が与えられる。しかもその座標変換において（式３）で示された回転行列を採用している点が相違している。
【００１８】
以上の例では、ワークテーブルＴＢのテーブル面が水平であるとしたが、ワークＷ自体の形状、その加工作業は多様であって、作業ライン構成・配置の柔軟性が要求され、場合によっては当該ワークテーブル面を傾斜させることが要求されることもある。また、ワークＷへの加工作業が非常に高精度を要求されるため当該テーブル面の傾斜が水平面に比べ無視できない程度である場合や、さらに工作機械におけるサドルとその上に搭載されるロータリーテーブルのように、回転するワークテーブルＴＢが直線移動するコンベア上に搭載されるといった構成配置の場合等には前記ワークテーブルＴＢにおける回転軸即ち、Ｚ軸が完全に垂直でないという条件に対処する必要がある。
【００１９】
上述したワークテーブルの回転軸が傾斜している場合にも対応可能なロボットの駆動制御を行うシステムについて図７及び図８により説明する。
図７は、直交座標系（Ｘ、Ｙ、Ｚ）において、Ｘ−Ｙ平面即ち、水平面にあったワークテーブル面がＹ軸の周りでβだけ回転させたときに前記ワークテーブル面が傾斜して傾斜面ＳＬＦとなった状態を示す。この場合、前記Ｚ軸、Ｘ軸も角βだけ回転し、それぞれＺ’軸、Ｘ’軸となる。このときＺ’軸はＸ方向に傾斜している。（第１方向）図示しないが、図７の状態でさらにＸ軸の周りで角γだけ回転させた場合にはＺ軸の傾斜方向はＹ軸方向にも傾斜角γ（第２方向）を有する。
【００２０】
なお、前記ワークテーブルの直交座標系（Ｘ、Ｙ、Ｚ）はロボットのベース座標系とそれぞれ各軸は平行である。図７でＺ、Ｙ、Ｘの各軸の周りでそれぞれα、β、γ回転したときに対応する回転行列Rzyx(α、β、γ)は次の（式６）に示される。ここでＣは余弦cosを、Ｓは正弦sin をあらわす。
【００２１】
【数４】
2005332001.tif 000006
図８は、図１乃至図６で説明した水平のワークテーブル面を持つ場合も含めた作業ロボットの制御を説明するための機能的制御ブロック線図を示す。同図８において、参照符号１０はデータメモリであって、ロボットに対する作業の詳細を記述した作業指令プログラムをストアしている第１記憶手段１０Ａ、及び前記回転行列Rzyx (α、β、γ)の内容ならびにワークテーブルの設置及び動作状態に関する前記エンコーダやトリガセンサ等からの信号をパラメータデータとしてストアする第２記憶手段１０Ｂから構成される。
【００２２】
参照符号１２は第１演算手段であって、前記作業指令プログラムを解析し、次のサンプル処理時刻にロボットの加工ヘッドが移動すべき位置データを、第１指令値として算出する。
参照符号１６は第２演算手段であって、回転演算機能（部）１６Ａと直線演算機能（部）１６Ｂとからなる。参照符号１４は前記回転演算機能（部）１６Ａと直線演算機能（部）１６Ｂのそれぞれを指定する指定手段であって、予めワークテーブルの構成、配置によって定められる。即ち、ワークがワークテーブルにより回転運動する場合は回転演算機能（部）１６Ａが指定され、図１０に示したようにワークがコンベア上で直線移動のみの場合は直線演算機能（部）１６Ｂが指定される。また、前記のように、コンベア上でワークテーブルが回転する場合は回転演算機能（部）１６Ａと直線演算機能（部）１６Ｂの両方が指定される。
【００２３】
第２演算手段では、前記第１演算手段１２から与えられる第１指令値に対し、回転演算機能（部）１６Ａまたは直線演算機能（部）１６Ｂにより補正を加え、次のサンプル処理時刻にロボットの加工ヘッドが移動すべき最終の位置データをサーボ系２８に与える。ここで両方の演算機能が指定されているときは、直線演算機能（部）１６Ｂにより与えられる補正量を回転演算機能（部）１６Ａの結果に対し単に加算することにより達成される。
【００２４】
前記回転演算機能（部）１６Ａでは、前述した（式４）、（式５）と同様な演算が行われる。ただし、回転行列Ｒｚ（θ）に代わってRzyx (α、β、γ)となる。
演算に際し、第２記憶部１０Ｂから回転行列及びそこで使用されるパラメータ値α、β、γがそれらのストアされているメモリ領域から読み取られる。なお、前記ワークテーブルが図１に対応する即ち、Ｚ軸の傾斜がなく、Ｙ軸、Ｘ軸周りの回転が無い場合においては、β、γは共にゼロであって、αをθに対応させると（式６）に示す回転行列の内容は（式３）の内容と一致することは当然である。
【００２５】
図８において、参照符号２２は第１検出手段であって、ワークテーブルの回転角位置の変化量即ち、Ｚ軸の回転角αを検出する。また、ワークテーブルがコンベアのような直線移動の場合は当該コンベア移動方向の移動量になる。
参照符号２４は第２検出手段であって、回転するワークテーブルにおける傾斜角即ち、Ｚ軸のＹ方向及びＸ方向の傾斜角β、γを検出する。通常はこれら傾斜角β、γはそれぞれＹ軸、Ｘ軸の周りの回転量としてロータリーエンコーダ等により予め測定しておくことが可能である。また、水準器によりテーブル面を直接測定することもできる。前者の場合、角αと同様常時β、γを測定することもできる。
参照符号２６はワークテーブル上のワークとロボットとの同期をとるトリガセンサである。
参照符号２０は中央演算処理装置（以下ＣＰＵという）であって、プログラムメモリ１８にストアされている制御プログラムに従ってロボット制御装置内の制御を遂行する。即ち、前記第１演算手段１２及び第２演算手段１６における演算の遂行、各検出手段２２、２４、２６からデータメモリ１０への検出データの取り込み、前記演算の遂行に先行してデータメモリ１０からの作業指令プログラム、回転行列、パラメータ値などのフェッチ動作などを行うようになっている。
【００２６】
以上、本発明の好適実施例について説明したが、本発明の技術思想は上述した実施例に限定されるものではなく、当業者であれば本発明に基づいて種々の変形を実施することができることは当然であり、そのような変形例は本発明の技術思想の範囲に含まれる。
【図面の簡単な説明】
【００２７】
【図１】本発明の１実施例を示すもので、回転可能な水平面を有する円弧形状コンベアの部分平面を示す図である。
【図２】図１における分配位置指令値P1[0]，P1[1]の様子を示す図である。
【図３】本発明の座標変換処理を例示する図である。
【図４】制御周期毎にエンコーダのパルスカウンタ値を、角度補正量ｄθに換算し、これをもとにワークテーブルの回転軸周りの補正用の回転行列を求めるための説明図である。
【図５】本発明の補正位置指令値の算出方法を例示する図である。
【図６】プログラム「MOVES P1」について動作した時の処理の流れを示すブロック図である。
【図７】直交座標系（Ｘ、Ｙ、Ｚ）において、Ｘ−Ｙ平面即ち、水平面にあったワークテーブル面がＹ軸の周りでβだけ回転させたときに前記ワークテーブル面が傾斜して傾斜面ＳＬＦとなった状態を示す図である。
【図８】図１乃至図６で説明した水平のワークテーブル面を持つ場合も含めた作業ロボットの制御を説明するための機能的制御ブロック線図である。
【図９】作業ロボットの加工ヘッドが稜線を倣う動作を行うためのワークピースを示す図である。
【図１０】コンベアに取付けられたエンコーダから、処理周期毎にパルスカウンタを読取り可能なシステムを例示する図である。
【図１１】ワークに対する倣い動作が可能なプログラム中の 「MOVES P1」について動作した時の処理の流れを示すブロックである。
【符号の説明】
【００２８】
１０        データメモリ
１０Ａ      第１記憶手段
１０Ｂ      第２記憶手段
１２        第1演算手段
１４        指定手段
１６        第２演算手段
１６Ａ      回転演算機能（部）
１６Ｂ      直線演算機能（部）
１８        プログラムメモリ
２０        ＣＰＵ
２２        第１検出手段
２４        第２検出手段
２６        トリガ同期センサ
２８        サーボ系
ＥＮＣ      エンコーダ
ＳＬＦ      傾斜面
ＲＢＴ      作業ロボット
ＴＢ        ワークテーブル
ＷＨＤ      ワーク加工ヘッド
【図１】
2005332001.tif 000007
【図２】
2005332001.tif 000008
【図３】
2005332001.tif 000009
【図４】
2005332001.tif 000010
【図５】
2005332001.tif 000011
【図６】
2005332001.tif 000012
【図７】
2005332001.tif 000013
【図８】
2005332001.tif 000014
【図９】
2005332001.tif 000015
【図１０】
2005332001.tif 000016
【図１１】
2005332001.tif 000017