JP4289619B2 - 多関節ロボットのツール位置補正方法 - Google Patents

Description

本発明は、視覚センサによって姿勢と位置座標とを検出したワークの近傍にツールを移動させる多関節ロボットに関し、更に詳しくは、ツールの移動位置精度を向上させるツール位置補正方法に関する。

ツールを交換して様々な作業に使用することのできる汎用性を備えた産業用ロボットとして、多関節ロボットが知られている。この多関節ロボットには、アームを主に水平方向で移動させる水平多関節ロボット（スカラーロボット）や、アームを主に垂直方向で移動させる垂直多関節ロボット（多軸ロボット）等がある。多軸ロボットは、ツールを任意の姿勢にすることができ、スカラーロボットに比べ、より高い汎用性を備えている。

多関節ロボットに所定の動作を行なわせるために、撮像カメラと画像認識装置とからなる視覚センサでワークの位置や姿勢を検出し、この検出位置にロボットハンド等のツールを移動させて、ワークをハンドリングする手法が一般に用いられている。視覚センサで検出した位置にツールを精度よく移動させるには、視覚センサの検出位置座標系と、多関節ロボットのツール位置座標系とを高精度に対応させなければならない。この二つの座標系を対応させる作業は、一般にキャリブレーションと呼ばれており、従来から種々のキャリブレーション方法が用いられている。

例えば、特許文献１に記載されているロボットと視覚センサとのキャリブレーション方法は、ツールとしてロボットハンドを使用し、四隅に円形の指標が描かれたキャリブレーションプレートをツール位置座標系と平行になるようにロボットハンドに持たせ、このキャリブレーションプレートの円形の指標を視覚センサで検出させ、各指標間の実際のピッチと画像認識処理によって得た指標間のピッチとから、視覚センサの検出位置座標系とロボットのツール位置座標系とのＸ座標及びＹ座標、及び回転方向の相関を求めている。
特開平０８−０７１９７２号公報

ツールの可動範囲が広い多軸ロボットでは、ワークの姿勢を制御せずに供給し、このワークの姿勢に合せて多軸ロボットのツールの向きを変化させて、ワークのハンドリングを行なう手法が多用されている。しかし、多軸ロボットは、ツールの回転に伴いロボット自身に位置ズレが発生する。この位置ズレ量は、最大で 数ｍｍ程度にもなるため、従来のキャリブレーション係数による座標変換だけではツールを正確な位置に移動させることができず、ワークのハンドリングに失敗することがあった。

本発明は、上記問題点を解決するためのもので、多関節ロボットのツールの位置ズレ量を補正して移動位置精度を向上させることを目的とする。

上記課題を解決するために、本発明の多関節ロボットのツール位置補正方法は、ツールを視覚センサの検出範囲内の任意の補正基準位置に移動させ、基準作業姿勢をとらせるステップと、視覚センサによってツールまたはツールに保持されたワークの検出位置座標を検出し、ツール位置座標に変換するステップと、ツールを補正基準位置を中心に視覚センサの撮像面に直交する方向で３６０°／Ｎ，（Ｎ＞＝２）ずつ回転させ、各回転位置でツールまたはワークの検出位置座標を視覚センサで検出し、ツール位置座標に変換するステップと、補正基準位置のツール位置座標と各回転位置のツール位置座標との誤差量を算出し、この誤差量を補正データとして記憶するステップとを設けた。

また、視覚センサによって検出したワークの検出位置座標にツールを移動させる際には、ワークの姿勢に対応するツールの回転角度を算出するステップと、この回転角度に対応した補正データを読み出し、または補正データを算出するステップと、検出位置座標から変換されたワークのツール位置座標に補正データを加え、補正位置座標を算出するステップと、この補正位置座標にツールを移動させるステップとを設けた。

更に、ツールの移動位置を補正する補正データの算出は、検出位置座標系とツール位置座標系とのキャリブレーション後に行なうようにした。

本発明の多関節ロボットのツール位置補正方法によれば、ツールの回転時の軸ブレや、検出位置座標系とツール位置座標系との位置ズレ等に起因して発生するツールの移動位置の位置ズレ量を補正することができ、ワークの姿勢に応じて極めて高い精度でツールをワークの位置に移動させることができる。

図１（Ａ），（Ｂ）及び図２は、本発明のキャリブレーション方法が用いられる６軸ロボット２と、視覚センサ３が組み込まれた直進フィーダ４との構成を示す外観斜視図及び側面図である。視覚センサ３は、直進フィーダ４によって供給されたワーク５を撮像して、自身が備える検出位置座標系上でワーク５の検出位置座標及び姿勢を検出する。６軸ロボット２は、視覚センサ３によって検出されたワーク５の検出位置座標及び姿勢に基づいて、自身が備えるツール位置座標系上で先端に取り付けられたツール、例えばロボットハンド２１を移動させてワーク５をハンドリングする。

６軸ロボット２は、サーボモータによって回動される関節を６個備えたアームを有する多軸ロボットであり、先端のツールを取り替えることによって様々な作業に対応することができる汎用性を備えている。また、その関節数の多さと各関節の可動範囲の広さからツールの移動範囲が広く、かつ任意の姿勢をとらせることができる。

６軸ロボット２は、設置に使用される略円柱形状のベース１０と、このベース１０の上部に第１関節１１を介して回動自在に組み付けられた旋回ボディ１２と、この旋回ボディ１２に第２関節１３を介して回動自在に取り付けられた第１アーム１４と、この第１アーム１４に第３関節１５を介して回動自在に取り付けられ、第４関節１６によって先端が回動自在とされた第２アーム１７と、この第２アーム１７に第５関節１８を介して回動自在に取り付けられ、先端に取り付けられたツールを回動させる第６関節１９を備えた手首２０とから構成されている。

図３に示すように、６軸ロボット２の手首２０には、様々な種類のツールを取り付けることができるアーム側フランジ２０ａが設けられている。このアーム側フランジ２０ａには、フランジ面２０ｂの中央Ｏ_fを基準位置とし、フランジ面２０ｂに対して垂直なＺ_f軸と、このＺ_f軸に対して直交するＸ_f軸及びＹ_f軸とからなるロボットフランジ座標系が設けられている。

６軸ロボット２にツールとして取り付けられるロボットハンド２１は、ロボットハンド２１を保持する本体部２２と、この本体部２２に設けられ、手首２０のアーム側フランジ２０ａに取り付けられるツール側フランジ２３とを備えている。ロボットハンド２１は、先端が互いに近接する挟持位置と離反する開放位置との間で揺動自在とされた２本の指部２１ａと、これらの指部２１ａを揺動自在に支持する支持部２１ｂとからなり、直進フィーダ４上を搬送されてきたワーク５を２本の指部２１ａで挟み込んでハンドリングする。本体部２２内には、ロボットハンド２１の指部２１ａを揺動させる駆動機構が組み込まれている。このロボットハンド２１の指部２１ａの先端近傍には、ロボットフランジ座標系上で任意に設定されたハンド位置設定点Ｈが設けられている。

６軸ロボット２は、ツール位置座標系上に設定した任意の点にハンド位置設定点Ｈを一致させるように各関節を制御する。また、ハンド位置設定点Ｈを中心にヨー軸、ピッチ軸、ロー軸の三軸の回りでロボットハンド２１の姿勢を変化させることができる。そのため、ツール位置座標系上の任意の点に対して、異なる方向からロボットハンド２１をアクセスさせることができ、様々な姿勢のワーク５を、そのワーク５に対して特定の方向から、そのワーク５の組み立てに適した姿勢でハンドリングすることができる。

従来よりロボットハンドによるワークのハンドリングでは、ワークを特定の姿勢、例えば次工程での組み立てに適した姿勢を保ったままハンドリングしている。ワークを特定の姿勢に保つには、ロボットハンドによってグリップされるワークのグリップ位置や、ワークとロボットハンドとがなす角度であるグリップ角度を一定にするとよい。そのため、従来のワークハンドリングでは、パーツフィーダ等を用いてワークを予め整列させてグリップ位置を一定の方向に向け、このグリップ位置を特定のグリップ角度のロボットハンドでグリップしていた。

しかし、上記従来のワークハンドリングでは、パーツフィーダの使用によってコストの上昇や作業効率の低下という悪影響を生じていた。また、異なる種類のワークをハンドリングする際には、そのワークに対応した別のパーツフィーダを用意しなくてはならなかった。そこで本実施形態では、ロボットハンド２１の移動及び姿勢の自由度が高い６軸ロボット２を使用することにより、直進フィーダ４によって様々な姿勢で搬送されてきたワーク５のグリップ位置を特定のグリップ角度でグリップしてハンドリングするようにした。これにより、ワーク５の姿勢を揃えるパーツフィーダを省略することができる。

なお、ワーク５は、例えば図４（Ａ）に示すように矩形状をしており、先端５ａ側に貫通穴からなる円形の指標５ｂが設けられている。このワーク５のグリップ位置Ｇは、ワーク５の長手方向において指標５ｂから寸法Ｗの位置で、ワーク５の下面に配置されている。ワーク５をロボットハンド２１でハンドリングする際には、グリップ位置Ｇにハンド位置設定点Ｈが一致するようにロボットハンド２１を移動させ、ワーク５の姿勢、すなわち検出位置座標系のＹ軸に対するワーク５の回転角度θ₁に合せてロボットハンド２１を回転させ、ワーク５の後端５ｃ側からワーク５の両側面をロボットハンド２１で挟み込む。また、同図（Ｂ）に示すように、実際のワークの取り出し及び組み立てに適したグリップ角度θ₂となるようにワーク５がハンドリングされる。

６軸ロボット２は、例えば、ロボットコントローラ２４によって制御される。ロボットコントローラ２４は、６軸ロボット２を制御する制御プログラム等が記録されたメモリと、各プログラムにしたがって各種演算処理を行なうＣＰＵと、６軸ロボット２や制御コンピュータ２５との接続に用いられるＩ／Ｏポート等を備えている。このロボットコントローラ２４は、制御コンピュータ２５によって制御される。

直進フィーダ４は、略箱形状のベース２８と、このベース２８の上に組み付けられて振動してワーク５を搬送する略板形状のトラフ２９と、トラフ２９のワーク搬送面２９ａに形成された開口２９ｂに嵌め込まれ、６軸ロボット２によってワーク５をハンドリングする際の取出しステージ３０を形成する透明なガラス板３１と、ベース２８内に組み込まれトラフ２９の一端側から振動を与える振動ユニット３２と、トラフ２９の他端側を振動可能に支持する支持部材３３とから構成されている。ベース２８の上面でトラフ２９の開口２９ｂに対面する位置には、開口３６が形成されている。

図５に示すように、直進フィーダ４のベース２８内で開口３６に対面する位置には、取出しステージ３０上のワーク５の検出位置座標及び姿勢を下方から検出する視覚センサ３と、ワーク５を下方から照明する照明ランプ３５とが組み込まれている。視覚センサ３は、ＣＣＤやＣＭＯＳ等のイメージエリアセンサと撮像光学系とが組み込まれた撮像カメラ３９と、この撮像カメラ３９による撮像によって生成された画像データを画像処理によってベクトルデータ化して、ワーク５の検出座標位置と回転角度θ₁とを特定する画像認識装置４０とから構成されている。撮像カメラ３９の撮像光軸ｖは、ツール位置座標系のＺ軸にほぼ平行に設定されている。この視覚センサ３も制御コンピュータ２５によって制御される。

照明ランプ３５は、制御コンピュータ２５によって制御されたランプドライバ４１によって点灯され、ガラス板３１越しにワーク５を照明する。ベース２８の開口３６とトラフ２９との間には、外光や塵芥等の進入を防止するために、例えば蛇腹状のカバー４２が取り付けられている。なお、ワーク５の照明は、取出しステージ３０の上方から行なってもよい。また、カバー４２を使用せずに、赤色光とシャープカットフィルタとを使用しても、外光の影響を効果的に防止することができる。

制御コンピュータ２５は、例えば、ＣＰＵ，ＨＤＤ，メモリ等を備えたパーソナルコンピュータ（ＰＣ）からなり、視覚センサ３，ロボットコントローラ２４，振動ユニット３２，ランプドライバ４１等を制御する。また、視覚センサ３の検出位置座標系（ピクセル）と、６軸ロボット２のツール位置座標系（ｍｍ）との間の座標変換を行なうプログラムや、検出位置座標系とツール位置座標系とを対応させるキャリブレーションプログラム等を格納している。

上記６軸ロボット２及び視覚センサ３と、直進フィーダ４は、例えば次のように動作する。直進フィーダ４上には、図示しないボウルフィーダ等によってワーク５が１個ずつ投入される。直進フィーダ４のトラフ２９上に投入されたワーク５は、その姿勢が整列されていないため様々な姿勢となる。直進フィーダ４は、振動ユニット３２によってトラフ２９を振動させて、ワーク５を取出しステージ３０上に向けて搬送する。

取出しステージ３０上に搬送されたワーク５は、ガラス板３１越しに下方から照明ランプ３５によって照明され、撮像カメラ３９によって撮像される。撮像カメラ３９によって生成された画像データは、画像認識装置４０の画像処理によりベクトルデータ化され、ワーク５の回転角度θ₁と、指標５ｂの検出位置座標とが検出される。この検出結果は、制御コンピュータ２５に入力される。

図６は、ツール位置座標系（Ｘ，Ｙ）と検出位置座標系（ｘ，ｙ）との関係を示している。２点鎖線で示す四角形Ｆは、撮像カメラ３９による撮像範囲を示しており、直進フィーダ４の取出しステージ３０よりも僅かに大きな面積を有している。例えば、検出位置座標系（ｘ，ｙ）の基準位置Ｏ_sのツール位置座標が（Ｘ₀，Ｙ₀）であるときに、取出しステージ３０上の検出位置座標（ｘ_a，ｙ_a）にあるワーク５の指標５ｂは、ツール位置座標（Ｘ_a，Ｙ_a）に位置することになる。

制御コンピュータ２５は、下記の座標変換式（１），（２）を使用して、視覚センサ３によって検出された指標５ｂの検出位置座標（ｘ_a，ｙ_a）から、ツール位置座標（Ｘ_a，Ｙ_a）を算出し、これをロボットコントローラ２４に入力する。ロボットコントローラ２４は、指標５ｂのツール位置座標（Ｘ_a，Ｙ_a）とワーク５の回転角度θ₁とから、ワーク５をハンドリングするのに適したグリップ位置Ｇのツール位置座標（Ｘ_a’，Ｙ_a’）を算出する。このグリップ位置Ｇのツール位置座標（Ｘ_a’，Ｙ_a’）は、図４（Ａ）に示すように、指標５ｂのツール位置座標（Ｘ_a，Ｙ_a）に対し、寸法Ｗとワーク５の回転角度θ₁とから得られるオフセット量（ΔＸ，ΔＹ）を加算することで算出される。なお、座標変換式（１），（２）のキャリブレーション係数α₁，β₁，α₂，β₂及び検出位置座標系の基準位置Ｏ_sのツール位置座標（Ｘ₀，Ｙ₀）は、座標変換精度に直接影響するパラメータである。
Ｘ＝α₁ｘ＋β₁ｙ＋Ｘ₀・・・式（１）
Ｙ＝α₂ｘ＋β₂ｙ＋Ｙ₀・・・式（２）

ロボットコントローラ２４は、６軸ロボット２の各関節のサーボモータを動作させて、ロボットハンド２１のハンド位置設定点Ｈをワーク５のグリップ位置Ｇのツール位置座標（Ｘ_a’，Ｙ_a’）に移動させ、かつ回転角度θ₁に合せてロボットハンド２１を回転させて、後端５ｃ側からワーク５の両側面を挟み込む。このときのワーク５とロボットハンド２１との間のグリップ角度θ₂は、実際のワーク５の取り出し及び組み立てに適した角度となる。

上述したように、視覚センサ３によって検出したワーク５の指標５ｂの検出位置座標（Ｘ_a，Ｙ_a）に基づいて、グリップ位置Ｇのツール位置座標（Ｘ_a’，Ｙ_a’）にロボットハンド２１のハンド位置設定点Ｈを正確に移動させるには、６軸ロボット２のツール位置座標系（Ｘ，Ｙ）と視覚センサ３の検出位置座標系（ｘ，ｙ）とを精度よく対応させなくてはならない。以下では、図１２のフローチャートを参照して、ツール位置座標系と検出位置座標系とのキャリブレーションについて説明する。

まず、ロボットハンド２１によって、グリップ位置Ｇとハンド位置設定点Ｈとが合致するように、後端５ｃ側からグリップ角度θ₂でワーク５を把持する。ワーク５をハンドリングしたロボットハンド２１を取出しステージ３０上で移動させ、図７に示すように、予め設定されているツール位置座標系（Ｘ，Ｙ）上の第１キャリブレーション位置Ｃ₁のツール位置座標（Ｐ₁，Ｑ₁）に、ハンド位置設定点Ｈを合致させる。なお、ロボットハンド２１を取出しステージ３０上で移動させる際には、把持したワーク５が取出しステージ３０と干渉しないように、ワーク５と取出しステージ３０とが僅かな隙間をもつような高さにロボットハンド２１が移動される。

次に、視覚センサ３によって、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄における指標５ｂの検出位置座標（ｋ_i,j，ｌ_i,j）（ｉ：キャリブレーション位置番号（１〜４）、ｊ：回転位置番号（１〜Ｎ））が検出される。キャリブレーション位置番号ｉは、最初にｉ＝１にセットされ、各キャリブレーション位置での指標５ｂの検出位置座標の検出終了とともにカウントアップされる。回転位置番号ｊは、最初にｊ＝１にセットされ、ロボットハンド２１が回転されるごとにカウントアップされる。また、ロボットハンド２１が次のキャリブレーション位置に移動したときに、回転位置番号ｊはリセットされる。

図８に示すように、視覚センサ３によって第１キャリブレーション位置Ｃ₁における指標５ｂの検出位置座標（ｋ_1,1，ｌ_1,1）を検出する。次に、ロボットハンド２１を、ハンド位置設定点Ｈを通り、撮像カメラ３９の撮像光軸とほぼ平行なツール位置座標系のＺ軸ｕを中心に反時計方向に３６０°／Ｎ（Ｎ＞＝２）、例えばＮ＝１８として２０°回転させ、視覚センサ３によって指標５ｂの検出位置座標（ｋ_1,2，ｌ_1,2）を検出する。同様に、ロボットハンド２１を３６０°／Ｎ（Ｎ＞＝２）ずつ回転させ、各回転時の指標５ｂの検出位置座標（ｋ_1,3，ｌ_1,3）〜（ｋ_1,18，ｌ_1,18）を検出する。

第１キャリブレーション位置Ｃ₁で所定の回転回数Ｎの検出位置座標の検出を終えると、ロボットハンド２１はハンド位置設定点Ｈを第２キャリブレーション位置Ｃ₂〜第４キャリブレーション位置Ｃ₄へと順に移動し、同様の手順で各キャリブレーション位置における指標５ｂの検出位置座標（ｋ_i,j，ｌ_i,j）を検出する。

全キャリブレーション位置において指標５ｂの検出位置座標を検出し終えたところで、各キャリブレーション位置ごとに下記の式（７）を用いて検出位置座標の平均値（ｘ_i，ｙ_i）を求める。求めた検出位置座標の平均値（ｘ_i，ｙ_i）は、第１キャリブレーション位置Ｃ₁のツール位置座標に対応したものとなる。

従来のキャリブレーションでは、ロボットハンドを１８０°（３６０°／Ｎ，Ｎ＝２）回転させ、（ｋ_i,1，ｌ_i,1），（ｋ_i,2，ｌ_i,2）の２個のデータの平均値を検出位置座標（ｘｉ，ｙｉ）としていたため、絶対精度のない６軸ロボットでは、予め設定したキャリブレーション位置のツール位置座標と検出位置座標を精度よく対応させることができなかった。しかし、本実施形態の手法では、キャリブレーション作業に多大な時間がかからない程度にロボットハンド２１の回転数Ｎを大きくして検出位置座標の平均値を算出することで、６軸ロボット２の絶対精度の影響を最小限に抑え、より正確なキャリブレーションを行なうことができる。

以上により求められた第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄における指標５ｂの検出位置座標（ｘ₁，ｙ₁），（ｘ₂，ｙ₂），（ｘ₃，ｙ₃），（ｘ₄，ｙ₄）と、仮想した適当なキャリブレーション係数α₁，β₁，α₂，β₂とを用いて前述の式（１）、（２）の計算を行なうと、その変換された第１〜第４座標変換位置Ｄ₁〜Ｄ₄のツール位置座標（Ｘ₁，Ｙ₁），（Ｘ₂，Ｙ₂），（Ｘ₃，Ｙ₃），（Ｘ₄，Ｙ₄）のなす四角形は、図９に示すように、予め設定した第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のなす四角形とほぼ間違いなくかけ離れたものとなる。

また、キャリブレーション係数α₁，α₂，β₁，β₂及び検出位置座標系の基準位置（Ｘ₀，Ｙ₀）を変化させ、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のなす四角形に、第１〜第４座標変換位置Ｄ₁〜Ｄ₄のなす四角形を一致させようとしても、第１〜第４座標変換位置Ｄ₁〜Ｄ₄が測定値より求めた値であるので、完全に一致させることは不可能である。

上記第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄の四角形と、第１〜第４座標変換位置Ｄ₁〜Ｄ₄の四角形との不一致を解決するために、本出願人は、各キャリブレーション位置Ｃ₁〜Ｃ₄と各座標変換位置Ｄ₁〜Ｄ₄との間の各誤差量を小さくして誤差方向を合せること、特に、対角線上の点同士の誤差ベクトル（誤差量及び誤差方向）を等しくすることによって、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のなす四角形に近似した第１〜第４座標変換位置Ｄ₁〜Ｄ₄の四角形を形成できることに着目した。

本実施形態では、図１０に示すように、第１座標変換位置Ｄ₁と第４座標変換位置Ｄ₄との誤差ベクトルＬ₁とＬ₄、及び第２座標変換位置Ｄ₂と第３座標変換位置Ｄ₃との誤差ベクトルＬ₂とＬ₃がそれぞれ同一になるようなキャリブレーション係数α₁，β₁，α₂，β₂を算出することにより、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のなす四角形に最も近似した第１〜第４座標変換位置Ｄ₁〜Ｄ₄のなす四角形を作り出した。対角線上の点同士の誤差ベクトルが同一である場合には、以下の式（３）〜（６）が成り立つ。
Ｘ₁−Ｘ₃＝Ｘ₄−Ｘ₂・・・（式）（３）
（Ｘ₁−Ｐ₁）−（Ｘ₂−Ｐ₂）＝（Ｘ₄−Ｐ₂）−（Ｘ₃−Ｐ₁）・・・式（４）
Ｙ₁−Ｙ₂＝Ｙ₄−Ｙ₃・・・（式）（５）
（Ｙ₁−Ｑ₁）−（Ｙ₃−Ｑ₂）＝（Ｙ₄−Ｑ₂）−（Ｙ₂−Ｑ₁）・・・式（６）

上記式（３）と（４）は、α₁，β₁の最適値条件であり、また、式（５），（６）は、α₂，β₂の最適値条件となるため、式（３）と式（４）、及び式（５）と式（６）とをそれぞれ連立させ、式（１），（２）を代入して展開することにより、下記式（８）〜（１５）を得ることができる。これにより、検出位置座標（ｘ₁，ｙ₁），（ｘ₂，ｙ₂），（ｘ₃，ｙ₃），（ｘ₄，ｙ₄）と、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のツール位置座標（Ｐ₁，Ｑ₁），（Ｐ₂，Ｑ₁），（Ｐ₁，Ｑ₂），（Ｐ₂，Ｑ₂）とからキャリブレーション係数α₁，β₁，α₂，β₂を直接算出することができる。
Ｓ₁＝ｘ₁−ｘ₂＋ｘ₃−ｘ₄・・・式（８）
Ｓ₂＝ｘ₁＋ｘ₂−ｘ₃−ｘ₄・・・式（９）
Ｔ₁＝ｙ₁−ｙ₂＋ｙ₃−ｙ₄・・・式（１０）
Ｔ₂＝ｙ₁＋ｙ₂−ｙ₃−ｙ₄・・・式（１１）
α₁＝２Ｔ₂（Ｐ₁−Ｐ₂）／（Ｓ₁Ｔ₂−Ｓ₂Ｔ₁）・・・式（１２）
β₁＝−２Ｓ₂（Ｐ₁−Ｐ₂）／（Ｓ₁Ｔ₂−Ｓ₂Ｔ₁）・・・式（１３）
α₂＝−２Ｔ₁（Ｑ₁−Ｑ₂）／（Ｓ₁Ｔ₂−Ｓ₂Ｔ₁）・・・式（１４）
β₂＝２Ｓ₁（Ｑ₁−Ｑ₂）／（Ｓ₁Ｔ₂−Ｓ₂Ｔ₁）・・・式（１５）

以上の式で算出されたキャリブレーション係数α₁，β₁，α₂，β₂を用いて、次に検出位置座標系の基準位置Ｏ_sのツール位置座標（Ｘ₀，Ｙ₀）を求める。まず、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のツール位置座標（Ｐ₁，Ｑ₁），（Ｐ₂，Ｑ₁），（Ｐ₁，Ｑ₂），（Ｐ₂，Ｑ₂）がなす四角形の重心位置Ｃ_g（Ｘ_g，Ｙ_g）と、指標５ｂの検出位置座標（ｘ₁，ｙ₁），（ｘ₂，ｙ₂），（ｘ₃，ｙ₃），（ｘ₄，ｙ₄）がなす四角形の重心位置Ｅ_g（ｘ_g，ｙ_g）を式（１６），（１７）より求める。
Ｃ_g＝（（Ｐ₁＋Ｐ₂）／２，（Ｑ₁＋Ｑ₂）／２）・・・式（１６）
Ｅ_g＝（（ｘ₁＋ｘ₂＋ｘ₃＋ｘ₄）／４，（ｙ₁＋ｙ₂＋ｙ₃＋ｙ₄）／４）・・・式（１７）

検出位置座標系において、重心位置Ｅ_g（ｘ_g，ｙ_g）から見た検出位置座標系の基準位置Ｏ_sのベクトルは、（−ｘ_g，−ｙ_g）であり、このベクトルをキャリブレーション係数α₁，β₁，α₂，β₂を用いてツール位置座標系に変換すると、（―α₁・ｘ_g−β₁・ｙ_g，−α₂・ｘ_g−β₂・ｙ_g）となる。ここで、各キャリブレーション位置からなる四角形の重心位置Ｃ_g（Ｘ_g，Ｙ_g）は、検出位置座標系における重心位置Ｅ_g（ｘ_g，ｙ_g）と対応していることから、（Ｘ_g，Ｙ_g）に（―α₁・ｘ_g−β₁・ｙ_g，−α₂・ｘ_g−β₂・ｙ_g）を加えることで式（１８）により、Ｏ_s（Ｘ₀，Ｙ₀）が求められる。
Ｏ_s＝（Ｘ_g−α₁ｘ_g−β₁ｙ_g，Ｙ_g−α₂ｘ_g−β₂ｙ_g）・・・式（１８）

以上より算出されたキャリブレーション係数α₁，β₁，α₂，β₂及び検出位置座標系の基準位置Ｏ_sのツール位置座標（Ｘ₀，Ｙ₀）を使用して、検出位置座標系とツール位置座標系との間で座標変換を行なうと、図１１に示すように、対角の誤差ベクトルＬ₁とＬ₄及びＬ₂とＬ₃はそれぞれ一致し、更にはＬ₁とＬ₄の二つのベクトルはＬ₂とＬ₃の二つのベクトルと１８０°反転した向きのベクトルであり、距離も全て同一で極めて小さいベクトルとなる。つまりは、第１〜第４キャリブレーション位置Ｃ₁〜Ｃ₄のなす四角形と第１〜第４座標変換位置Ｄ₁〜Ｄ₄のなす四角形とが最も近い状態となり、６軸ロボット２のロボットハンド２１のハンド位置設定点Ｈを、視覚センサ３によって検出されたワーク５のグリップ位置Ｇに正確に移動させることができる。

以上のキャリブレーションによって求められたキャリブレーション係数α₁，β₁，α₂，β₂及び検出位置座標系の基準位置Ｏ_sのツール位置座標（Ｘ₀，Ｙ₀）は、制御コンピュータ２５のＨＤＤ内に記憶され、視覚センサ３の検出位置座標系から６軸ロボット２のツール位置座標系への座標変換を行なう際に読み出されて、座標変換式の演算に使用される。

図１３に示すように、上記キャリブレーション後には、視覚センサ３によって取出しステージ３０上のワーク５の回転角度θ₁と指標５ｂの検出位置座標とを検出し、座標変換式（１），（２）によって指標５ｂのツール位置座標を算出する。この指標５ｂのツール位置座標に、指標５ｂとグリップ位置Ｇとの間の距離Ｗと回転角度θ₁とによって得られるオフセット量（ΔＸ，ΔＹ）を加算して、ワーク５をハンドリングする際に用いられるグリップ位置Ｇのツール位置座標を算出する。ロボットハンド２１は、このグリップ位置Ｇにハンド位置設定点Ｈが合致するように移動し、ワーク５をハンドリングする。

しかし、図示するように、キャリブレーション後であっても、ワーク５のグリップ位置Ｇと、このグリップ位置Ｇに向けて移動したロボットハンド２１のハンド位置設定点Ｈとが一致しないことがある。それは、ロボットハンド２１の移動には、図８に示すロボットハンド２１の回転時の軸ブレと、キャリブレーションで求められた検出位置座標系の基準位置Ｏ_s（Ｘ₀，Ｙ₀）の真の位置からのズレ量とが影響するためである。グリップ位置Ｇとハンド位置設定点Ｈとに位置ズレが生じると、ワーク５のハンドリングミスが発生してしまう。

そこで、本発明では、キャリブレーションの終了後に、各姿勢のワーク５を取り出す際のグリップ位置Ｇとハンド位置設定点Ｈとの位置ズレ量を算出し、この算出した位置ズレ量をロボットハンド２１を移動させる際の補正データとして用いることにした。これにより、キャリブレーションだけを実施した場合よりも、より正確な位置にロボットハンド２１を移動することができる。以下、図１４のフローチャートを参照しながら、本発明のツール位置補正方法について説明する。

本発明のツール位置補正は、前述のキャリブレーションと同様に制御コンピュータ２５の制御によって行なわれる。制御コンピュータ２５は、ロボットコントローラ２４を制御してロボットハンド２１にワーク５をハンドリングさせ、図１５に示すように、取出しステージ３０上の任意の位置、例えば中央近傍にハンド位置設定点Ｈが合致するようにロボットハンド２１を移動させる。このハンド位置設定点Ｈが移動されたツール位置座標系上の任意の位置を以下では補正基準位置ｍ₁とし、この補正基準位置ｍ₁のツール位置座標を（Ｘ_h1，Ｙ_h1）とする。

補正基準位置ｍ₁に移動されたロボットハンド２１は、基準作業姿勢をとる。基準作業姿勢とは、図４（Ａ），（Ｂ）に示すように、取出しステージ３０上のワーク５の後端５ｃ側をグリップ位置Ｇとハンド位置設定点Ｈとが合致するようにハンドリングし、かつ実際のワーク５の取り出し及び組み立てに適したグリップ角度θ₂で傾斜させた姿勢である。

図１６に示すように、ロボットハンド２１は、補正基準位置ｍ₁を中心として３６０°／Ｎずつ間欠に回転され、視覚センサ３によって、各回転位置における指標５ｂの検出位置座標（ｋ_j，ｌ_j）（ｊ：回転位置番号（１〜Ｎ））が検出される。回転位置番号ｊは、ロボットハンド２１が回転されるごとにカウントアップされる。

図１７に示すように、制御コンピュータ２５は、補正基準位置ｍ₁に移動させたロボットハンド２１の姿勢を基準回転位置とし、回転位置番号ｊとしてｊ＝１をセットする。次いで、基準回転位置（ｊ＝１）における指標５ｂの検出位置座標（ｋ₁，ｌ₁）を視覚センサ３で検出し、座標変換式（１），（２）を用いてツール位置座標（Ｋ₁，Ｌ₁）を算出する。また、下記の式（１９）を用いて、指標５ｂのツール位置座標（Ｋ₁，Ｌ₁）から、補正基準位置ｍ₁のツール位置座標（Ｘ_h1，Ｙ_h1）までの距離を求め、これを基準オフセット量（ΔＸ₁，ΔＹ₁）とする。この基準回転位置（ｊ＝１）における、グリップ位置Ｇとハンド位置設定点Ｈとの補正量（Ｘ_rj，Ｙ_rj）は、Ｘ_r1＝０，Ｙ_r1＝０となる。
（ΔＸ₁，ΔＹ₁）＝（Ｘ_h1−Ｋ₁，Ｙ_h1−Ｌ₁）・・・式（１９）

次にロボットハンド２１は、取出しステージ３０に対して基準作業姿勢を保ちながら、補正基準位置ｍ₁のツール位置座標を通り、視覚センサ３の光軸ｖにほぼ平行なＺ軸ｕを中心に、３６０°／Ｎ、例えばＮ＝１８として２０°回転する。図１６に示すように、このロボットハンド２１の回転により、ハンド位置設定点Ｈは補正基準位置ｍ₁からずれてしまう。視覚センサ３は、回転位置番号（ｊ＝２）における指標５ｂの検出位置座標（ｋ₂，ｌ₂）を検出し、制御コンピュータ２５は、座標変換式（１），（２）を用いてツール位置座標（Ｋ₂，Ｌ₂）に変換する。

制御コンピュータ２５は、下記式（２０）〜（２２）を用いて、基準オフセット量（ΔＸ₁，ΔＹ₁）を２０°回転させて回転位置番号（ｂ＝２）に対応させた新たなオフセット量（ΔＸ₂，ΔＹ₂）を算出する。そして、式（２３）に示すように、オフセット量（ΔＸ₂，ΔＹ₂）をツール位置座標（Ｋ₂，Ｌ₂）に加算することにより、補正基準位置ｍ₁を回転位置番号（ｊ＝２）に対応させた補正基準位置ｍ₂のツール位置座標（Ｘ_h2，Ｙ_h2）を算出する。
ΔＸ_j＝ΔＸ₁・ｃｏｓθ_j−ΔＹ₁ｓｉｎθ_j・・・式（２０）
ΔＹ_j＝ΔＸ₁・ｓｉｎθ_j＋ΔＹ₁ｃｏｓθ_j・・・式（２１）
θ_j＝ｊ・（３６０°／Ｎ）・・・（式２２）
（Ｘｈ_j，Ｙｈ_j）＝（Ｋ_j＋ΔＸ_j，Ｌ_j＋ΔＹ_j）・・・式（２３）

式（２４）に示すように、この補正基準位置ｍ₂から補正基準位置ｍ₁のツール位置座標（Ｘ_h1，Ｙ_h1）を減算することにより、補正基準位置ｍ₁とｍ₂との間の距離、すなわち回転位置番号（ｂ＝２）の補正量（Ｘ_r2，Ｙ_r2）を算出することができる。
（Ｘ_rj，Ｙ_rj）＝（（Ｋ_j＋ΔＸ_j）−Ｘ_h1，（Ｌ_j＋ΔＹ_j）−Ｙ_h1）・・・式（２４）

上記補正量（ベクトル）は、実際のワーク５を取り出す際のロボットハンド２１の位置ズレ量と同値である。また、この位置ズレ量は、ロボットハンド２１の回転軸のブレだけでなく、検出位置座標系の基準位置Ｏ_s（Ｘ₀，Ｙ₀）の真の位置からのズレ量が影響したものとなる。以降、ロボットハンド２１が３６０°回転するまで各回転位置における指標５ｂの検出位置座標（ｋ_j，ｌ_j）を検出し、各回転位置に対応した補正基準位置ｍ_jと補正量（Ｘ_rj，Ｙ_rj）とを算出する。

以上のように求められたワーク５の各回転位置での補正量は、ロボットハンド２１の各回転位置番号に対応づけられ、図１８に示す補正テーブル５０を構成する補正データとして制御コンピュータ２５に記憶される。この補正テーブル５０が生成された６軸ロボット２により、直進フィーダ４上のワーク５をハンドリングする際には、図１９のフローチャートに示す手順でロボットハンド２１の移動位置が補正される。視覚センサ３は、取出しステージ３０上に搬送されてきたワーク５の指標５ｂの検出位置座標と回転角度θ₁とを検出し、制御コンピュータ２５に入力する。

制御コンピュータ２５は、入力された指標５ｂの検出位置座標を座標変換式（１），（２）を用いてツール位置座標に変換し、この指標５ｂのツール位置座標からグリップ位置Ｇのツール位置座標（Ｘ_k，Ｙ_k）を算出する。次に、ワーク５の姿勢から、ワーク５をハンドリングする際のロボットハンド２１の回転角度を算出し、この回転角度に対応した補正データを補正テーブル５０から読み出す。なお、ロボットハンド２１の回転角度が補正テーブル５０の回転位置番号の角度と異なっている場合には、近似する２点の回転位置番号の補正データから比例補完によって補正データを算出する。

例えば、ロボットハンド２１の回転角度が６０°である場合は、補正テーブル５０の回転位置番号４が対応する。制御コンピュータ２５は、補正テーブル５０からＸ_rj＝１．６，Ｙ_rj＝０．２の補正データを読み出し、グリップ位置Ｇのツール位置座標（Ｘ_k，Ｙ_k）から減算して補正位置座標を算出する。この補正位置座標はロボットコントローラ２４に入力され、ハンド位置設定点Ｈが補正位置座標に合致するようにロボットハンド２１が移動される。

このように、ロボットハンド２１と検出位置座標系との位置ズレ量に基づいて得た補正データにより、ハンド位置設定点Ｈの移動位置を補正するようにしたので、取出しステージ３０上でロボットハンド２１を任意の角度に回転させても、ワーク５のグリップ位置Ｇとハンド位置設定点Ｈとの位置ズレ量を非常に小さくすることができる。

なお、このツール位置補正では、取出しステージ３０上の任意の１点だけでロボットハンド２１の位置ズレ量を測定し、補正データを作成するようにした。図２０（Ｂ）〜（Ｆ）は、同図（Ａ）に示す取出しステージ３０上の任意の５か所の位置５５〜５９でロボットハンド２１を回転させた際に生じる、ハンド位置設定点Ｈとグリップ位置Ｇとの位置ズレ量を示したグラフである。これらのグラフから分かるように、ロボットハンド２１の可動範囲に比べて比較的狭い取出しステージ３０上では、どの場所でロボットハンド２１を回転させてもその位置ズレの軌跡はほぼ同じとなる。そのため、取出しステージ３０上の任意の１点でロボットハンド２１を回転させ、その位置ズレ量から補正データを求めれば、図２１（Ａ）〜（Ｆ）に示すように、取出しステージ３０上の全域でロボットハンド２１の位置ズレを補正することができる。なお、複数の位置で位置ズレ量を計測して平均値を求めれば、より高精度な補正データを得ることができる。

なお、キャリブレーションにおいて画像の歪みの影響を更に抑えたい場合には、キャリブレーションで求めた検出位置座標系の基準位置Ｏ_s（Ｘ₀，Ｙ₀）を次のキャリブレーション位置の重心とし、再びキャリブレーションをすることで精度の高いキャリブレーション係数α₁，β₁，α₂，β₂及び検出位置座標計の基準位置Ｏ_s（Ｘ₀，Ｙ₀）を算出することができる。

また、上記実施形態では、キャリブレーション後に再び６軸ロボット２を動作させて補正データを算出したが、キャリブレーションで取得した指標５ｂの検出位置座標を用いて補正データを得ることもできる。更に、ツールとしてロボットハンドを用いたが、その他の種類のツールであっても同様にキャリブレーション及びツール位置補正を行なうことができる。また、ワークの指標を検出することによってキャリブレーションやツール位置補正を行なったが、ロボットハンド等のツール自体の一部を検出してキャリブレーションを行なってもよい。

更に、ワークを４点のキャリブレーション位置で検出してキャリブレーションを行なったが、４点以上のキャリブレーション位置を使用してキャリブレーションを行なってもよい。更に、ロボットハンドを２０°ずつ回転させて検出位置座標の平均値を求めたり、ツールの補正データを計測したが、２以上の回転位置の検出位置座標があれば平均値や補正データを算出することができるので、回転角度は２０°に限定されるものではない。もちろん、より高精度なキャリブレーション、位置補正を行ないたい場合には、ロボットハンドの回転回数を増やすとよい。

また、６軸ロボットを例に説明したが、異なる関節数を有する多軸ロボットや、スカラーロボット等のツール位置補正にも利用することができる。更に、２次元平面上でワークの位置を検出する視覚センサと６軸ロボットとについて説明したが、複数の撮像カメラを用いてワークの三次元位置を検出する三次元視覚センサと多関節ロボットとのツール位置補正にも応用することができる。

６軸ロボットと直進フィーダとの外観斜視図である。 ６軸ロボットと直進フィーダとの側面図及び要部断面図である。 ６軸ロボットのアーム先端のロボットフランジ座標系を示す斜視図である。 ロボットハンドに保持されたワークの形状を示す平面図及び側面図である。 ロボットハンドに保持されたワークと取出しステージと撮像カメラとを示す斜視図である。 ツール位置座標系と検出位置座標系との関係を示すグラフである。 ツール位置座標系と検出位置座標系と各キャリブレーション位置との関係を示すグラフである。 各キャリブレーション位置で行なわれるワークの検出位置座標の検出方法の説明図である。 各キャリブレーション位置と適当なキャリブレーション係数で算出された座標変換位置との関係を示すグラフである。 各キャリブレーション位置と本発明によって得たキャリブレーション係数で算出された座標変換位置との関係を示すグラフである。 各キャリブレーション位置と、本発明によって得たキャリブレーション係数及び検出位置座標系の基準位置とを用いて算出された座標変換位置との関係を示すグラフである。 本発明のキャリブレーション方法の手順を示すフローチャートである。 ワークのグリップ位置とロボットハンドのハンド位置設定点との位置ズレ状態を示す説明図である。 ロボットハンドの移動位置の補正データの算出手順を示すフローチャートである。 補正基準位置に移動されたロボットハンドの状態を示す斜視図である。 補正データの算出時のロボットハンドの回転軌跡を示す説明図である。 ロボットハンドの回転によるハンド位置設定点のズレと、補正量とを示す説明図である。 ロボットハンドの移動位置の補正データを含む補正テーブルを示す表である。 補正データによるロボットハンドの移動位置の補正手順を示すフローチャートである。 取出しステージ上の任意の位置でのロボットハンドの位置ズレ量を示すグラフである。 取出しステージ上の任意の位置での補正されたロボットハンドの位置ズレ量を示すグラフである。

符号の説明

２ ロボットハンド
３ 視覚センサ
４ 直進フィーダ
５ ワーク
２１ ロボットハンド
２４ ロボットコントローラ
２５ 制御コンピュータ
３０ 取出しステージ
５０ 補正テーブル
Ｃ₁〜Ｃ₄ 第１〜第４キャリブレーション位置
Ｄ₁〜Ｄ₄ 第１〜第４座標変換位置
Ｇ グリップ位置
Ｈ ハンド位置設定点
Ｌ₁〜Ｌ₄ 誤差ベクトル

Claims (5)

1. ワークの姿勢と位置座標とを視覚センサによって検出し、この検出した位置座標を視覚センサの検出位置座標系から多関節ロボットのツール位置座標系に変換し、変換して得たワークのツール位置座標にワークの姿勢に対応した方向からツールを移動させ、基準作業姿勢をとった該ツールによりワークに作業を行なう多関節ロボットにおいて、
   前記ツールを視覚センサの検出範囲内の任意の補正基準位置に移動させ、基準作業姿勢をとらせるステップと、
   視覚センサによって、ツールまたはツールに保持されたワークの検出位置座標を検出し、ツール位置座標に変換するステップと、
   ツールを補正基準位置を中心に視覚センサの撮像面に直交する方向で３６０°／Ｎ，（Ｎ＞＝２）ずつ回転させ、各回転位置でツールまたはワークの検出位置座標を視覚センサで検出し、ツール位置座標に変換するステップと、
   補正基準位置のツール位置座標と各回転位置のツール位置座標との誤差量をそれぞれ算出し、これらの誤差量を補正データとして記憶するステップとを備えたことを特徴とする多関節ロボットのツール位置補正方法。
2. ワークの姿勢と位置座標とを視覚センサによって検出し、この検出した位置座標を視覚センサの検出位置座標系から多関節ロボットのツール位置座標系に変換し、変換して得たワークのツール位置座標にワークの姿勢に対応した方向からツールを移動させ、基準作業姿勢をとった該ツールによりワークに作業を行なう多関節ロボットにおいて、
   前記視覚センサの検出範囲内に設定した複数の補正基準位置のそれぞれで、
   前記ツールを前記補正基準位置の１つに移動させ、基準作業姿勢をとらせるステップと、
   視覚センサによって、ツールまたはツールに保持されたワークの検出位置座標を検出し、ツール位置座標に変換するステップと、
   ツールを補正基準位置を中心に視覚センサの撮像面に直交する方向で３６０°／Ｎ，（Ｎ＞＝２）ずつ回転させ、各回転位置でツールまたはワークの検出位置座標を視覚センサで検出し、ツール位置座標に変換するステップと、
   補正基準位置のツール位置座標と各回転位置のツール位置座標との誤差量をそれぞれ算出するステップと、を実行させ、
   前記回転位置ごとに前記各補正基準位置で算出された誤差量の平均値を算出し、これらの平均値を補正データとして記憶することを特徴とする多関節ロボットのツール位置補正方法。
3. 前記視覚センサによって検出したワークの検出位置座標にツールを移動させる際に、
   ワークの姿勢に対応するツールの回転角度を算出するステップと、
   この回転角度に対応した補正データを読み出し、または補正データを算出するステップと、
   検出位置座標から変換されたワークのツール位置座標に補正データを加え、補正位置座標を算出するステップと、
   前記補正位置座標にツールを移動させるステップとを備えたことを特徴とする請求項１または２記載の多関節ロボットのツール位置補正方法。
4. 前記多関節ロボットは、多軸ロボットであることを特徴とする請求項１〜３いずれか記載の多関節ロボットのツール位置補正方法。
5. 前記補正データの算出は、検出位置座標系とツール位置座標系とのキャリブレーションの終了後に行なうことを特徴とする請求項１〜４いずれか記載の多関節ロボットのツール位置補正方法。

Images