JP3998741B2

JP3998741B2 - Robot movement control method

Info

Publication number: JP3998741B2
Application number: JP19855296A
Authority: JP
Inventors: 淳渡辺; 哲朗加藤; 哲也小坂; 浩次西; 英俊組谷
Original assignee: FANUC Corp
Current assignee: FANUC Corp
Priority date: 1995-07-12
Filing date: 1996-07-10
Publication date: 2007-10-31
Anticipated expiration: 2016-07-10
Also published as: JPH0976185A

Description

【０００１】
【発明の属する技術分野】
本発明はロボットの移動制御方法に関し、更に詳しく言えば、視覚センサを用いて取得されたデータに基づいてロボットを目的の位置に移動させるための移動制御方法に関する。本発明の移動制御方法は、特にジョグ送り形式の移動に有利に適用される。
【０００２】
【従来の技術】
従来より知られているロボットの移動制御方法の一つにジョグ送りがある。これは、オペレータが教示操作盤のジョグ送りボタンを操作してロボットを目的とする位置へ動かすようにした方法である。ジョグ送りは、例えばアームの先端に取り付けられたハンドでワークを持ち上げて所定の場所に積み上げる作業を行うというような作業に利用される。
【０００３】
その場合、オペレータは、目視によってワークとハンドとの位置関係を確認しながら複数個のジョグ送りボタンの選択的操作によってロボットを徐々に移動させ、ＴＣＰ（ツール・センタ・ポイント）を目標とするアプローチ点に到達させていた。そして、このアプローチ動作が完了したら、ハンド操作用のボタンを操作してワークをつかむようにしていた。
【０００４】
【発明が解決しようとする課題】
しかし、ロボットをジョグ送りによって正確なアプローチ位置に到達させるには多くの時間と高度の熟練を要することが一般的である。即ち、ジョグ送りで移動可能なロボットの移動方向は限られており（例えば、ＸＹＺ各座標軸方向に沿った並進と座標軸回り回転並びに各軸送りの場合のＪ１軸〜Ｊ６軸の移動）、単純なジョグ送りボタン操作で一気に正確なアプローチを完了させるというようなことは不可能といって良かった。
【０００５】
本発明はこのような状況に鑑みてなされたものであり、目標位置への自律的なアプローチを可能にして、作業効率を向上させることのできるロボットの移動制御方法を提供することを目的とする。
【０００６】
【課題を解決するための手段】
本発明は、上記課題を解決するために、ロボットが自律的なアプローチ動作を存在位置にばらつきのある対象物に対して行えるようなロボットの移動制御方法を提供するものである。本発明の方法では、カメラ手段及び画像処理手段を備えた視覚センサとロボット制御装置とを含む制御手段が、利用される。
【０００７】
本発明の方法の諸段階には、対象物に対するアプローチ完了状態を表現するアプローチ完了状態表現データを制御手段に記憶させるアプローチ完了状態教示段階と、制御手段によってロボットにアプローチ動作を行わせるアプローチ動作実行段階とが含まれ、更に、アプローチ動作実行段階には、該アプローチ動作がアプローチ完了状態表現データに基づいて自律的に行なわれる自律アプローチ動作実行段階が含まれる。
【０００８】
教示段階でアプローチ完了状態表現データは、ロボットの手先と対象物の相対的な位置関係を特定するものであり、自律アプローチ動作実行段階におけるロボットの移動目標位置は、制御手段内のソフトウェア処理によって、アプローチ完了状態表現データによって特定される対象物−ロボット手先間の相対的位置関係が実現される位置として定められる。
【０００９】
アプローチ完了状態教示段階で記憶されるアプローチ完了状態表現データは、対象物を代表する物体をカメラ手段によって撮影することによって取得された画像に基づいて制御手段内で作成することが出来る。
一つの好ましい形態において、アプローチ完了状態表現データは、画像上における対象物代表物体の特徴部と重心位置を表わすデータとを含むように教示される。
【００１０】
また、別の好ましい形態においては。対象物及び対象物代表物体の表面上に前記視覚センサによって認識可能なマーク座標系が設置され、アプローチ完了状態表現データは、画像上におけるマーク座標系の位置・姿勢を表わすデータとを含むように教示される。
【００１１】
自律アプローチ動作実行段階に先だって、予備的なアプローチ動作を行なうためにジョグ送りを行なうようにしても良い。その際、ジョグ送り段階から自律アプローチ動作実行段階への移行は、制御手段によって自動的に行なわれることが好ましい。
【００１２】
また、自律アプローチ動作実行段階におけるソフトウェア処理は、当該時点において視覚センサによって認識されるロボットの手先と対象物の相対的な位置関係を表わすデータと、教示されたアプローチ完了状態表現データとを比較する処理と、比較結果に基づいてロボットの各軸を移動制御するための処理と、アプローチ完了状態の完了／未完了を判定する処理を、アプローチ完了状態の完了が判定されるまで逐次的に繰り返すものとすることが出来る。
【００１３】
本発明の最も重要な特徴は、アプローチ完了状態表現データがロボット手先（即ち、それに固定されたカメラ）と目標物の相対的な位置関係を記述するデータの形態で予め入力されており、従って、アプローチ動作時に、このアプローチ完了状態表現データに対応するカメラと目標物の相対的な位置関係を実現するような移動制御を行なうことで、自律的なアプローチ動作が実現される点にある。
【００１４】
この特徴により、オペレータに負担をかけることなくアプローチ完了状態が任意のワークに対して効率的に達成される。また、ジョグ送りから自律アプローチ動作への自動移行を行なえば、ロボットの至便利性を更に向上させることが出来る。
【００１５】
【発明の実施の形態】
図１は本発明のロボット移動制御方法をハンドリングロボットの例を用いて概念的に説明する図である。符号１ａはアプローチ完了状態入力手段を表わし、アプローチ対象を代表するワーク３に対するロボット２のアプローチ動作が完了した時のワーク３とハンド２ａの相対的な位置関係（姿勢を含む。）を表現するためのデータ（以下、「アプローチ完了状態表現データ」と言う。）を予めメモリ４に入力しておく機能を有している。なお、ハンド２ａの位置がロボット手先位置（例えばＴＣＰまたは最終アーム先端のフランジ上に設定された座標系の原点）で代表され得ることは言うまでもない。
【００１６】
実際の作業時にあたっては、オペレータがジョグ送りボタン５（一般には、アプローチ動作マニュアル指令手段）を操作し、軸移動制御手段６にロボット２の各軸を制御させ、アプローチ動作を実行させる。アプローチ動作のための移動制御は、アプローチ完了状態入力手段１ａによって入力されたアプローチ完了状態表現データと、メモリ４にワーク３とハンド２ａ（ロボット２の手先）の相対的な位置関係を認識する相対位置認識手段１ｂによって取得される相対位置認識データとに基づいて実行される。
【００１７】
アプローチ動作は、ワーク３とハンド２ａ（ロボット２の手先）の相対的な位置関係が、アプローチ完了状態表現データの表わすものと一致した時点で完了する。なお、実際の作業時に対象とされる個々のワーク３は、正確な位置決めがなされていなくて良いことに注意。また、以下に述べるように、アプローチ完了状態入力手段１ａと相対位置認識手段１ｂは２次元視覚センサを利用する形で具体化される。
【００１８】
図２および図３は本実施形態のロボットの移動制御方法を実施するためのロボット制御システムの要部構成図である。ロボット１０はハンドリングロボットあるいは他種のロボットであり、アーム１１の先端には、ツール（エンドエフェクタ）１２が取り付けられている。ここでは、ツール１２としてハンドが示されている。ロボット１０は、ロボット制御装置３０によって動作制御され、ワーク１４を把持して搬送する。なお、ツール１２の種類及びアプローチ後の作業内容はアプリケーションによることは言うまでもない（例えば、ツール１２が加工ツールであれば、加工を行なう）。
【００１９】
また、ロボット１０のアーム１１には、ツール１２と一体に移動できるようにカメラ１３が取り付けられている。カメラ１３は、画像処理装置２０と接続されており、その撮影画像を画像処理装置２０に送る。
【００２０】
画像処理装置２０は、ＣＰＵ、フレームメモリ、画像処理プロセッサ、プログラムメモリ、データメモリ等を備えた周知の構成と機能を有するもので、カメラ１３からの撮影画像の記憶・処理を行い、その結果得られるデータをロボット制御装置３０に送る。ここでは、画像処理方式に関連して２通りの形態を採用する。
〔方式Ａ〕図２のように、カメラ１３からの撮影画像を輪郭処理してワーク画像の重心位置を求める。また、ワーク１４の特徴部１４ａ等を抽出して、重心と特徴部との位置関係等のデータを求める。このデータを、代表ワークに対するアプローチが実現されている状態で得られた画像に基づいて作成すれば、それがアプローチ完了状態表現データとして利用出来る。
〔方式Ｂ〕図３のように、座標系（マーク座標系１５ａ）を定義できるようなマーク１５をワーク１４に予め用意しておき、カメラ１３による撮影画像の処理により、カメラ座標系１３ａに関するマーク座標系１５ａの相対的位置姿勢のデータを求める。方式Ａの場合と同じく、このデータを、代表ワークに対するアプローチが実現されている状態で得られた画像に基づいて作成すれば、それがアプローチ完了状態表現データとして利用出来る。
【００２１】
ロボット１０のアプローチ動作の指示は、ロボット制御装置３０に接続された教示操作盤４０によって行われる。教示操作盤４０は、後述するように、液晶ディスプレイや各種のキーを備えている。また、オペレータによって持ち運びできる程度の大きさであることが好ましい。ロボット制御装置３０は、方式Ａあるいは方式Ｂに従った画像処理を行なう画像処理装置２０および教示操作盤４０からのマニュアル指令入力に従って、アプローチ完了状態表現データが表わすアプローチ完了状態を実現すべく移動制御を行う。
【００２２】
図４はロボット制御装置３０のハードウェアの構成を示す要部ブロック図である。ロボット制御装置３０には、バス３９に結合されたプロセッサボード３１が装備され、このプロセッサボード３１には、プロセッサ３１ａ、ＲＯＭ３１ｂ、ＲＡＭ３１ｃが設けられている。プロセッサ３１ａは、ＲＯＭ３１ｂに格納されたシステムプログラムに従って、ロボット制御装置３０全体を制御する。ＲＡＭ３１ｃには、画像処理装置から送られるアプローチ完了状態表現データが格納される。
【００２３】
ディジタルサーボ制御回路３２はバス３９に結合され、プロセッサボード３１からの指令によって、サーボアンプ３３を経由して、サーボモータ５１，５２，５３，５４，５５および５６を駆動する。これらのサーボモータはロボット１０の本体機構部に内蔵され、ロボット１０の各軸を駆動する。
【００２４】
バス３９には更にシリアルポート３４が結合されており、液晶ディスプレイ付きの教示操作盤４０、その他のＲＳ２３２Ｃ機器５８と接続されている。ＲＳ２３２Ｃ機器５８には、画像処理装置２０も含まれる。教示操作盤４０はオペレータにより持ち運びができる程度の大きさと重さを有し、そのパネル上にはアプローチ動作マニュアル指令入力手段として使用されるジョグ送りボタン等が設けられている。また、シリアルポートには、オプションでＣＲＴ３６ａが接続可能であり、ＣＲＴ３６ａの位置から作業状態等が確認できる。
【００２５】
ディジタルＩ／Ｏ３５には、操作パネル３６ｂが接続されており、ロボット制御装置３０の位置でロボット１０の動作指令を行うことができる。また、ディジタルＩ／Ｏ３５には、ツール１２の把持駆動部が接続されており、その動作指令を行うことができる。アナログＩ／Ｏ３７には、例えばツール１２としてレーザ溶接用の加工ヘッドが使用された場合に、その溶接電源装置が接続され、このアナログＩ／Ｏ３７を介して溶接電圧の指令が行われる。また、大容量メモリ３８には、ティーチングデータ等が格納される。
【００２６】
図５は教示操作盤４０のパネル面の概略構成を示す図である。表示画面４１は例えば液晶画面であり、移動指令プログラムの詳細データ等が切替え表示される。ファンクションキー４２は、表示画面４１の下端部に表示されるメニューを選択するキーである。教示操作盤有効スイッチ４３は、教示操作盤４０の操作が有効か無効かを切り換えるスイッチである。
【００２７】
非常停止ボタン４４は、ロボット１０の動作を非常停止させるボタンである。カーソルキー４５は、表示画面４１上で表示されるカーソルの移動を行うキーである。テンキー部４６には、数字キーやその他のキーが設けられており、数値および文字の入力、削除等を行うことができる。
【００２８】
一連のジョグ送りボタン４７（Ｊ１〜Ｊ６）は、従来方式のジョグ送りを行なう通常モードでは、並進／回転方向及び＋−方向を指定して移動指令の入力を行うボタンであるが、本実施形態（自律アプローチモード）では、後述するように、アプローチ動作マニュアル指令入力手段として使用する。
【００２９】
次に、本実施形態におけるアプローチ動作の概要について説明する。
図６および図７は、前述の方式Ａ及び方式Ｂを採用した各々の場合について、ロボットの動作を説明する図である。ここでは、ツール１２はハンドであり、ハンド１２によってロボットの動作範囲内に順次供給されるワーク１４を把持して所定位置に配置されたパレット（図示省略）まで搬送するパレタイジング作業を考える。順次供給されるワーク１４は、正確に位置決めされておらず、供給位置には相当のバラツキがあって良い。
【００３０】
［１］準備
１．作業開始に先だって、アプローチ完了状態データの取得作業を行なう。この作業は、代表ワーク１４を適当な位置（実際の作業時の供給位置を代表すると思われる範囲内であれば十分である。）に置き、通常モード（従来方式）のジョグ操作によってロボットを移動させ、アプローチ完了状態を実現させる。この作業には正確を期す必要があるが、このような作業はワークの種別が変わらない限り１回きりで良いことは本発明の重要な特徴である（従来方式では、ワーク毎にジョグ送りのやり直しが必要）。
【００３１】
２．代表ワーク１４に対してアプローチ完了状態を実現したら、ロボット制御装置３０を介してカメラ１３による撮影を行なわせ、アプローチ完了状態表現データを作成するための画像を取得する。
３．取得された画像は画像処理装置２０内で画像処理され、前述の方式Ａ，Ｂに対応して次のようなデータが作成され、アプローチ完了状態表現データとしてＲＡＭ３１ｃに格納される。
【００３２】
〔方式Ａ〕；代表ワーク１４の重心Ｐｇおよび特徴部１４ａの位置関係、ＴＣＰのアプローチ達成時の位置Ｐｃ＝Ｐａと重心Ｐｇとの間の距離Ｌ１等の位置姿勢データ。なお、ここで「重心」とは実際には画像として認識された代表ワーク１４の重心、即ち「画像重心」のことである。
〔方式Ｂ〕カメラ座標系を基準としたマーク座標系１５ａの相対的な位置姿勢を表わすデータ（詳細は後述）。なお、方式Ｂを採用する場合には、実際の作業で扱われる全ワークに対して、ワーク上の同じ位置に同じ姿勢で代表ワーク１４に用意されたのと同じマーク座標系１５ａが用意されていることが前提となる事に注意する必要がある。
【００３３】
［２］実際の作業
１．アプローチ完了表現データ入力後、新しく供給されたワーク１４に対して実際の作業を開始する。オペレータは、ジョグ送りボタン４７（図５）を押してロボットの各軸を移動制御し、ツール１２の位置を代表するＴＣＰを開始点（Ｐｃ＝Ｐｓ）からワーク１４に向けてジョグ送りで移動させる。この間、カメラ１３と画像処理装置２０を含む２次元視覚センサも動作している。
【００３４】
２．ツール１２がワーク１４に接近してカメラ１３の視野にワーク１４が入ると、画像処理装置２０がそれを感知し、ロボット制御装置３０は通常モードから自律アプローチモードに切り換わり、自律アプローチ動作が開始される。なお、最初からカメラ１３の視野にワーク１４が入っている場合には、通常モードから自律アプローチモードへの切り換えはジョグボタン押下直後に起る。
３．自律アプローチ動作は、アプローチ完了状態の実現をロボット制御装置３０が確認しない限り、ジョグ送りボタン４７の押下によって何度でも繰り返し続行される。アプローチ動作の最終的な移動目標位置は、予め入力されているアプローチ完了状態表現データの表わしている位置である。
４．アプローチ動作開始後に、Ａ方式あるいはＢ方式で作成されたアプローチ完了状態表現データと等価なデータが得られるような位置にロボットのＴＣＰが到達した時点で、アプローチ動作が完了する。アプローチが完了すると、ロボットは停止するとともに、ロボット制御装置３０は自律アプローチモードから通常モードに自律的に復帰する。
５．停止後、オペレータは、必要に応じてハンド操作とジョグ送り等を組み合わせてワーク１４を把持し、所定位置へ搬送し、ワーク１４を解放する。
以上の手順を次回以降の各ワーク１４に対しても繰り返すことで多数のワーク１４に対するパレタイジング作業を効率的に行なうことが出来る。
［３］処理の概略
図８はこのようなロボット移動制御のための教示操作盤の操作手順と処理の大要を示すフローチャートである。各ステップの処理を簡単に記すと次のようになる。
〔Ｓ１〕ジョグ送りボタン４７が押されてジョグ送りの指令がなされたか否かを判断し、なされればステップＳ２に進み、なされなければステップＳ１を繰り返す。
〔Ｓ２〕ジョグ送りボタン４７で指定された移動内容（方向、軸等）に応じて、所定の軸を制御して、ロボット１０のジョグ送り移動を開始する。例えば、Ｊ６軸＋方向の移動が指定されていれば、Ｊ６軸を＋方向へ進める移動指令を作成してサーボに渡す。
〔Ｓ３〕ジョグ送りボタン４７がオフにされたか否かを判断し、オフにされればステップＳ８に進み、されなければステップＳ４に進む。
〔Ｓ４〕画像処理装置２０から、カメラ１３の視野内にワーク１４が入ったことを知らせる信号の受信の有無を確認し、受信していればステップＳ５に進み、受信していなければステップＳ３に戻る。
〔Ｓ５〕アプローチ動作の処理を開始する。具体的な処理内容の例については、Ａ方式、Ｂ方式に分けて後述する。
〔Ｓ６〕ジョグ送りボタン４７がオフにされたか否かを判断し、オフにされていればステップＳ８に進み、されていなければステップＳ７に進む。
〔Ｓ７〕アプローチが完了したか否かを判断し、完了していればステップＳ８に進み、完了しなければステップＳ６に戻る。なお、アプローチ完了の判断方式についても、Ａ方式、Ｂ方式の具体例の中で述べる。
〔Ｓ８〕ロボット１０の各軸の動作を停止させる。
【００３５】
［４］方式Ａにおける自律アプローチ動作時の処理
本発明を現実のアプリケーションに適用する場合、図９に示したように、ロボットの手先に搭載されたカメラ１３の光軸方向Ｃを固定した条件で自律アプローチのためのロボット動作を行えるようにすることが実際的である。そこで、ここでは最も現実的なアプリケーションとして、図１０に示したような事例を想定する。
図１０に示したように、ワーク１４は一つのワーク供給面ＰＬ１上に供給される。ワーク供給面ＰＬ１は、ワーク座標系Σw のＺ軸に垂直であるが、ワーク供給面ＰＬ１上のワーク１４での正確な位置決めはなされていない。ワーク１４の特徴部１４ａはワーク上面１４ｂ上にあり、ロボットはカメラ１３の光軸Ｃを常時面ＰＬ１及びワーク上面１４ｂに垂直な方向を向けた状態でアプローチ動作を行なう。
【００３６】
ツール座標系Σt のＺt 軸は、ハンド１２の軸方向及び光軸Ｃと平行になるように設定されているものとする。従って、自律アプローチ動作は、ツール座標系Σt のＺt 軸とワーク座標系のＺ軸の平行関係を維持しながら、ロボットのＴＣＰをアプローチ開始位置Ｐｓからアプローチ完了位置Ｐａまで移動させる過程となる。ＴＣＰをアプローチ完了位置Ｐａに接近させ、到達させるにはカメラ１３と画像処理装置２０による視覚センサ機能が利用される。以下、その処理のアルゴリズムについて説明する。なお、以下の説明において、記号＜＞はベクトルを表わすために使用する。
【００３７】
１．概要
先ず、図１１から図１２に続くフロー説明図は、アプローチ動作の諸段階を仮想的な画面を適宜援用して説明するもので、流れを８つの段階ＡＰ１〜ＡＰ８に分け、図１１にはＡＰ１〜ＡＰ３を、図１２にＡＰ４〜ＡＰ８を示した。
（ＡＰ１）アプローチ開始後で且つアプローチ未完了の状態。この状態においてカメラ１３で捉えられている画像を、アプローチ完了状態を表わす画像と比較する形で一般的に描示した。以下の説明において、各記号の意味は次の通りである。
×；アプローチ完了状態における画面上のワーク重心Ｐｇの位置で、前述［１］の準備作業により、画像処理装置２０内のメモリにカメラ座標系上のデータが記憶されている。
△；アプローチ完了状態における画面上の特徴部１４ａの位置で、重心位置のデータと同様、前述［１］の準備作業により、画像処理装置２０内のメモリにカメラ座標系上のデータが記憶されている。
【００３８】
□；アプローチ開始後の任意の状態でカメラ１３によって捉えられている画像における重心Ｐｇの位置で、アプローチ動作中に画像処理装置２０内で行なわれる画像処理によって随時把握される。
【００３９】
○；アプローチ開始後の任意の状態でカメラ１３によって捉えられている画像における特徴部１４ａの位置で、重心位置と同様、アプローチ動作中に画像処理装置２０内で行なわれる画像処理によって随時把握される。
【００４０】
（ＡＰ２）アプローチ開始後、先ず、光軸Ｃ回りのロボットの姿勢をアプローチ完了状態のそれと比較するために、画面上で見た重心位置と特徴部位置の関係でチェックする。具体的には、×→△の方向を表わす単位ベクトル＜ｄ0 ＞と□→○を表わす単位ベクトル＜ｄ1 ＞が平行であるか否かを判断する。
（ＡＰ３）ＡＰ２の判断がイエス（平行）でない限り、ＡＰ３へ進みロボットを所定量光軸Ｃ回りで動かし、＜ｄ0 ＞と＜ｄ1 ＞の関係を調整する。調整後、再びＡＰ２へ戻り、調整未了であれば再度ＡＰ３へ進む。ＡＰ２，ＡＰ３はＡＰ２の判断がイエス（平行）となるまで繰り返す。
【００４１】
（ＡＰ４）ＡＰ２の判断がイエス（平行）となったら、ＡＰ４へ進む。ＡＰ４では、ロボットの位置をアプローチ完了状態のそれと比較するために、画面上で見た特徴部位置をチェックする。具体的には、○の位置と△の位置が一致しているか否かを判断する。
（ＡＰ５）ＡＰ４の判断がイエス（一致）でない限り、ＡＰ５へ進んでカメラ１３を並進移動させるようなロボット位置調整を行なう。移動方向は、画面上の△の位置（記憶データで表現される不動位置）が、○の位置（ロボットが移動すると動く）へ接近する方向＜ｅ＞とする。この調整によって、＜ｄ0 ＞と＜ｄ1 ＞の平行関係が悪化するのが一般的であるから、ＡＰ２へ戻り、＜ｄ0 ＞と＜ｄ1 ＞の平行関係を再度チェックする。平行関係が崩れていたら、ＡＰ３へ進む。上述した通り、光軸Ｃ回りの姿勢調整（ＡＰ３）とチェック（ＡＰ２）はＡＰ２の判断がイエス（平行）となるまで繰り返される。
【００４２】
（ＡＰ６）ＡＰ４の判断がイエス（一致）となると、ＡＰ６に示したような画面上における○の位置と△の位置関係が実現されることになる。
（ＡＰ７）ＡＰ６の状態で、カメラ１３の３つの視線方向を表わす方向ベクトル、＜ａ0 ＞，＜ｂ0 ＞，＜ｂ1 ＞を計算する。＜ａ0 ＞はカメラ１３が△を見る視線方向を表わす単位ベクトルであり、ＡＰ６の状態では同時に○を見る視線方向を表わす単位ベクトルでもある（画面上の位置が同じであることに注意）。一方、＜ｂ0 ＞，＜ｂ1 ＞は各々カメラ１３が×と□を見る視線方向を表わす単位ベクトルである。そして、ベクトル＜ｂ0 ＞と＜ｂ1 ＞の一致／不一致をチェックする。
【００４３】
（ＡＰ８）ＡＰ７で＜ｂ0 ＞と＜ｂ1 ＞が一致していない限り、ＡＰ８へ進みロボットの位置調整を行なう。この時の移動は、カメラ１３が△及び○を見る視線方向に沿った前進／後退に限定し、内積＜ａ0 ＞・＜ｂ0 ＞が内積＜ａ0 ＞・＜ｂ1 ＞と一致するまで行なう。一般のケースでは、この調整により＜ｄ0 ＞と＜ｄ1 ＞の平行関係が崩れる。
【００４４】
そこで、調整完了後（両内積一致後）、再び（ＡＰ１）へ戻り、上述したサイクルを繰り返す。このようにして、アプローチ完了状態へ向かって漸近し、実質的に「△と○が一致且つ×と□が一致」が実現された状態で迎えたＡＰ７でイエスの判断が出される。このようにして、アプローチ完了状態データの入力時の状態（即ち、アプローチ完了状態）が再現されたら、自律アプローチの処理を終了する。
【００４５】
２．アルゴリズムの説明
次に、上記説明した概略フローに即したアルゴリズムについて、図１３〜図１５のフローチャート並びに関連説明用の図１６〜図２０を参照図に加えて説明する。各図のフローチャートと上記概要の説明との対応関係は次の通りである。
図１３（ステップＨ１〜Ｈ７）；ＡＰ１〜ＡＰ３に対応
図１４（ステップＨ８〜Ｈ１９）；ＡＰ４〜ＡＰ５に対応
図１５（ステップＨ２０〜Ｈ２９）；ＡＰ６〜ＡＰ８に対応
〔Ｈ１〕ロボットの手先（最終アーム先端のフランジ上に設定された座標系の原点またはＴＣＰ）の位置・姿勢を表わす４行４列の行列Ｔを計算し、行列Ｔを記憶したレジスタ値を更新する。
〔Ｈ２〕＜ａ1 ＞，＜ｂ1 ＞を画像処理で求める。図１６に示したように、＜ａ1 ＞はカメラ１３が○（現在の特徴部１４ａの画像上位置）を見る視線方向を表わす単位ベクトルであり、＜ｂ1 ＞はカメラ１３が□（現在の重心の画像上位置）を見る視線方向を表わす単位ベクトルである。また、ベクトル＜ｚ＞は、図１７に示したように、カメラ１３のレンズ中心に相当した位置Ｏc に原点を設定されたカメラ座標系のｚ軸方向の単位ベクトルである。カメラ座標系のｚ軸方向は、光軸Ｃ方向と一致している。
【００４６】
〔Ｈ３〕ベクトル＜ｚ＞，＜ａ1 ＞，＜ｂ1 ＞を用いて、□から○を向いた単位ベクトル＜ｄ1 ＞（図１１左下部参照）を計算する。
〔Ｈ４〕ベクトル＜ｄ0 ＞とベクトル＜ｄ1 ＞が平行であるか否かを判断する。イエスであれば、ステップＨ８へ進み、ノーであればステップＨ５へ進む。
【００４７】
〔Ｈ５〕＜ｄ0 ＞と＜ｄ1 ＞の平行化に必要な回転調整量を表わす行列Ｒを計算する。
〔Ｈ６〕ステップＨ５で求めた行列ＲとステップＨ１で計算済みの行列Ｔを用いて、ロボットの移動目標位置・姿勢を表わす行列Ｔを計算する。
〔Ｈ７〕ステップＨ６で求めた行列Ｔの表わす位置・姿勢へ、ロボットを移動させる。
【００４８】
〔Ｈ８〕ベクトル＜ａ0 ＞とベクトル＜ａ1 ＞が一致しているか否かを判断する。イエスであれば、ステップＨ２０へ進み、ノーであればステップＨ９へ進む。
〔Ｈ９〕ロボットの並進移動の刻み量ｌを設定する。ｌは適当な大きさの固定値ｌ0 （例えば２ｃｍ）と｜＜ａ0 ＞×＜ａ1 ＞｜の積を計算して設定される。
〔Ｈ１０〕ベクトル＜ｚ＞，＜ａ0 ＞，＜ａ1 ＞を用いて、△から○を向いた単位ベクトル＜ｅ＞（図１８上部参照）を計算する。
〔Ｈ１１〕ロボットを＜ｅ＞方向にｌだけ並進移動させた位置・姿勢を表わす行列Ｔを計算する。部分行列Ｉは図１８に併記したように、３行３列の単位行列である。また、行列Ｃはカメラ座標系Ｏc −ｘｙｚ（図１７参照）の位置・姿勢をカメラ手先（最終アーム先端のフランジ上に設定された座標系またはＴＣＰ）から見たカメラ座標系の位置・姿勢を表わす行列である。行列Ｃを表わすデータは、公知の適当なカメラキャリブレーションによってロボット制御装置内に格納されている。
【００４９】
〔Ｈ１２〕ステップＨ１１で求めた行列Ｔの表わす位置・姿勢へ、ロボットを移動させる。
〔Ｈ１３〕移動後の状態で、ベクトル＜ａ1 ＞を求める。
〔Ｈ１４〕ベクトル＜ａ0 ＞とベクトル＜ａ1 ＞が一致しているか否かを判断する。イエスであれば、ステップＨ１へ戻り、ノーであればステップＨ１５へ進む。
〔Ｈ１５〕移動後の状態において△から○を向いた単位ベクトル＜ｆ＞（図１８下部参照）を計算する。
【００５０】
〔Ｈ１６〕ステップＨ１０で求めたベクトル＜ｅ＞とステップＨ１５で求めた＜ｆ＞との内積の符号をチェックする。もし、負であればロボット移動によって○が△をオーバランしたことになる（画像上では△の位置は記憶データによって固定されているので、○が△に近づくように変化する）。オーバラン時（判断イエス時）には、ステップＨ１７へ進む。判断ノー時には、ステップＨ１８へ進む。
【００５１】
〔Ｈ１７〕刻み移動量ｌを半減させる。
〔Ｈ１８〕ステップＨ１５で求めた＜ｆ＞を改めて＜ｅ＞の値とする。
〔Ｈ１９〕ロボットの手先の位置・姿勢を表わす４行４列の行列Ｔを計算し直して、ステップＨ１１へ戻る。
【００５２】
〔Ｈ２０〕ベクトル＜ｂ0 ＞とベクトル＜ｂ1 ＞が一致しているか否かを判断する。イエスであれば、図２０に示したようにアプローチ完了状態が実現されたことを意味するから、処理を終了する。ノーであればステップＨ２１へ進む。
【００５３】
〔Ｈ２１〕ロボットの並進移動の刻み量ｌを再設定する。ｌは適当な大きさの固定値ｌ0'（例えば１ｃｍ）と＜ａ0 ＞と＜ｂ1 ＞−＜ｂ0 ＞の内積を計算して設定される。
〔Ｈ２２〕ロボットを＜ａ0 ＞方向にｌだけ並進移動させた位置・姿勢を表わす行列Ｔを計算する。
【００５４】
〔Ｈ２３〕ステップＨ２２で求めた行列Ｔの表わす位置・姿勢へ、ロボットを移動させる。この移動は、図１９に示したように、画像上で□が×に近づくように変化するような前進後退移動である（×はアプローチ完了状態記憶データで固定されていることに注意）。
〔Ｈ２４〕移動後の状態における＜ｂ1 ＞を画像処理で求める。
〔Ｈ２５〕ステップＨ２４で計算された＜ｂ1 ＞を用いて、＜ａ0 ＞と＜ｂ1 ＞−＜ｂ0 ＞の内積ｍを計算する。
【００５５】
〔Ｈ２６〕ｍ＝０か否かをチェックする。イエスであれば、ステップＨ１へ戻る。ノーであれば、ステップＨ２７へ進む。
〔Ｈ２７〕ステップＨ２５で求めたｍと刻み調整量ｌの積の符号をチェックする。もし、負であればロボット移動によって□が×をオーバランしたことになる。オーバラン時（判断イエス時）には、ステップＨ２８へ進む。判断ノー時には、ステップＨ２９へ進む。
〔Ｈ２８〕刻み移動量ｌを半減させ、且つ、その符号を反転させる。
〔Ｈ２９〕ロボットの手先の位置・姿勢を表わす４行４列の行列Ｔを計算し直して、ステップＨ２２へ戻る。
【００５６】
以上述べたアルゴリズムを用いた処理サイクルを繰り返すことで、ロボットはアプローチ完了状態に漸近し、やがて図２０に示したアプローチ完了状態が達成される。ステップＨ２０でイエスの判断出力が出されて、処理は終わる。
【００５７】
［５］方式Ｂにおける自律アプローチ動作時の処理
方式Ａを適用したアプリケーションではカメラの光軸方向一定という条件の下での自律アプローチ動作を説明したが、以下に説明する方式Ｂのアプリケーションでは、このような条件を外してより一般的な事例を考える。
本アプリケーショでは、図２２に示したように、マーク座標系１５ａを１つづつワーク面上の定位置に設置したワーク１４がワーク供給ＰＬ２上に正確な位置決めなしに供給される。そして、任意のアプローチ開始位置Ｐｓ（カメラ光軸方向傾斜可；但し、マーク座標系１５ａは視野内にある）から自律アプローチ動作を開始して、アプローチ完了位置Ｐａ（カメラ光軸方向傾斜可）への移動を達成するための処理のアルゴリズムを説明する。
【００５８】
なお、ここではロボット乃至ツールの位置は最終アーム先端のフランジに設定された座標系（単にフランジとも言う。）で代表させる。また、記号＜＞は方式Ａの説明の場合と同じくベクトルを表わすために使用するが、個別の意味は＜＞内の文字が方式Ａの説明と重複していても、必ずしも同じ意味とは限らない（例えば＜ｄ₀ ＞など）。行列の表記についても同様である。
【００５９】
１．概要
自律アプローチ動作に必要なアルゴリズムの大要は、図２１に記した通りである。フランジの現在位置（アプローチ動作中の各時点における位置）Ｔ0 、カメラ座標系とマーク座標系の現在相対位置（アプローチ動作中の各時点における相対位置）Ｍ0 と、アプローチ完了状態におけるフランジ位置Ｔg 、アプローチ完了状態におけるカメラ座標系とマーク座標系の相対位置Ｍg との幾何学的な関係は、図２２中に記されているようなものである。なお、Ｍg は予め計算して記憶され、Ｃはキャリブレーションで教示される。これは方式Ａの場合と同様である。
【００６０】
これらＴ0 ，Ｍ0 ，Ｔg ，Ｍg の間の関係を規定する基本方程式は下記のようになる。
Ｔ0 ＣＭ0 ＝Ｔg ＣＭg ・・・（１）
従って、これをＴ_g について解いた次式（２）がロボットの直交座標系上での移動目標位置を表わす基本式となる。
Ｔg ＝Ｔ0 ＣＭ0 Ｍg^-1 Ｃ^-1 ・・・（２）
このＴ_g を最終的な移動目標点とする位置指令をサーボに与えれば、ロボットをアプローチ完了状態へ向けて自律的に移動させることが出来る。故に、自律アプローチ動作のアルゴリズムを定めることは、（２）式の右辺を具体的に求める問題に帰着する。
【００６１】
（２）式の右辺中、Ｔ₀ はロボットの現在位置データを表わしており、Ｃのデータは適当なキャリブレーションで獲得され得る。そこで、Ｍ0 及びＭg の求め方から説明する。
【００６２】
２．Ｍ0 及びＭg の方程式
Ｍ0 ，Ｍg は、いずれもカメラ座標系から見たマーク座標系１５ａの位置・姿勢（現在及びアプローチ完了時）を表わしている。本実施形態で用いるマーク座標系１５ａは、図２３に示されている。マーク座標系１５ａは間隔ａを以て格子状に配列された５個の円形ドットＤ0 ，Ｄ1 ，Ｄ-1，Ｄ2 ，Ｄ-2で構成されている。
【００６３】
図示した通り、ドット間隔ａは正の一定値とする。
ａ＞０・・・（３）
各ドットの中心位置は、Ｄ0 （０，０，０），Ｄ1 （ａ，０，０），Ｄ-1（−ａ，０，０），Ｄ2 （ａ，ａ，０），Ｄ-2（−ａ，ａ，０）である。なお、３次元直交座標系を表現出来るものであれば、他のパターンでマーク座標系１５ａを構成しても良い。
【００６４】
Ｍ0 とＭg は、３次元直交座標系間の位置・姿勢の関係を表現する４×４の同次変換行列であり、次式（４）のように置くことが出来る。
【００６５】
【数１】

ここで、ＲM は回転を表わす３×３行列、＜ｌM ＞は位置を表わす３×１行列（ベクトル）である。以下、Ｍ₀ とＭ_g が満たす方程式として、ＲM と＜ｌM ＞が満たす方程式を求めることを考える。先ず、次式（５），（６）でベクトル＜ｅ1 ＞，＜ｅ2 ＞のように定義する。
【００６６】
【数２】

マーク座標系１５ａに関して視覚センサから得られる情報は、各ドットＤ0 ，Ｄ1 ，Ｄ-1，Ｄ2 ，Ｄ-2の中心がカメラ座標系の原点から見える方向（視線方向）である。これらの方向は、図２４に示したように、５個の単位ベクトル＜ｄ0 ＞，＜ｄ1 ＞，＜ｄ-1 ＞，＜ｄ2 ＞，＜ｄ-2 ＞で表わすことが出来る。
【００６７】
これらベクトルの内積について、次式（７）で定義されるδijを導入する。
δij＝＜ｄi＞・＜ｄj＞（i,j=-2,-1,0,1,2 ）・・・（７）
ここで、＜ｄ0 ＞，＜ｄ1 ＞，＜ｄ-1 ＞，＜ｄ2 ＞，＜ｄ-2 ＞の長さはすべて１であるから、δijの絶対値は１を越えることはない。
｜δij｜≦１（等号成立の必要十分条件はｉ＝ｊ）・・・（８）
また、カメラ座標系原点から各ドット中心までの距離は不明であるが、これをｔi （i=-2,-1,0,1,2 ）と置く。当然これらは正の値をとる。
【００６８】
ｔi ＞０（i=-2,-1,0,1,2 ）・・・（９）
すると、次の方程式（10）〜（12）が得られる。
ｔ0 ＜ｄ0 ＞＝＜ｌM ＞・・・（10）
ｔ1 ＜ｄ1 ＞＝＜ｌM ＞＋ａＲM ＜ｅ1 ＞・・・（11ａ）
ｔ-1＜ｄ-1＞＝＜ｌM ＞−ａＲM ＜ｅ1 ＞・・・（11ｂ）
ｔ2 ＜ｄ2 ＞＝ｔ1 ＜ｄ1 ＞＋ａＲM ＜ｅ2 ＞・・・（12ａ）
ｔ-2＜ｄ-2＞＝ｔ-1＜ｄ-1＞＋ａＲM ＜ｅ2 ＞・・・（12ｂ）
従って、ｔi を求めながらこれらの方程式をＲM と＜ｌM ＞に関して解いて行けば、Ｍ0 ，Ｍg を定めることが出来る。具体的な解法については、４．Ｍ0 及びＭg の求め方の項で説明することにし、カメラ座標系の位置・姿勢を表わすＣについて簡単に触れておく。
【００６９】
３．Ｃの方程式
Ｃはフランジから見たカメラ座標系の位置・姿勢を表現する行列であり、そのデータはカメラ１３のキャリブレーションによって得られる。キャリブレーションについては種々の手法が公知になっており、それらを利用することが出来るが、マーク座標系１５ａを用いてもカメラ１３のキャリブレーションを行なうことも出来る。ここでは、その要点のみを簡単に記しておく。
【００７０】
図２２中に符号ＣＢ１〜ＣＢ３で示したような異なる３点において、カメラ１３による撮影を各々実行し、画像処理を行なってマーク座標系１５ａとカメラ座標系の原点の間の相対位置に関するデータＰi （i=1,2,3 ）を得る。それらと各撮影時のロボット位置のデータＴi （i=1,2,3 ）を組み合わせると、次の３組の方程式が得られる。なお、Ｐi （i=1,2,3 ）を求める要領はＭ0 やＭg と同様である。
Ｔ1 ＣＰ1 ＝Ｔ2 ＣＰ2 ＝Ｔ3 ＣＰ3 ・・・（13）
３点ＣＢ１〜ＣＢ３のロボットの姿勢が互いに異なる条件下では、この方程式をＣに関して解くことが可能である。この条件を（14）とする。
条件：Ｔ1 ，Ｔ2 ，Ｔ3 の各姿勢成分は互いに異なる・・・（14）
求められたＣのデータは、ロボット制御装置あるいは画像処理装置に記憶される。なお、方程式の解法については、５．Ｃの求め方で補足説明を行なう。
【００７１】
４．Ｍ0 及びＭg の求め方
方程式（10）〜（12）を解いてＲM ，＜ｌM ＞を求めるにあたって、先ずｔi （i=-2,-1,0,1,2 ）を求めることを考える。そのために、次式（15）の置き換えを行う。
【００７２】
【数３】

そして、これを用いて方程式（10）〜（12）を次式（16）〜（18）のように変形する。
＜ｒM ＞＝ｔ0 ＲM^-1 ＜ｄ0 ＞＝＜ｌM ＞・・・（16）
＜ｒM ＞＋ａ＜ｅ1 ＞＝ｔ1 ＲM^-1 ＜ｄ1 ＞・・・（17）
＜ｒM ＞−ａ＜ｅ1 ＞＝ｔ-1ＲM^-1 ＜ｄ-1＞・・・（18）
ここで、ＲM^-1 が回転を表わす行列であることに注意すると、次式（19）が成立する。

この（19）式と、（５）〜（８）式及び（16）〜（18）式を用いると、次の関係式（20）〜（23）が得られる。
【００７３】
‖＜ｒM ＞‖² ＝ｔ0² ・・・（20）
‖＜ｒM ＞‖² ＋２ａｒMx＋ａ² ＝ｔ1² ・・・（21ａ）
‖＜ｒM ＞‖² −２ａｒMx＋ａ² ＝ｔ-1² ・・・（21ｂ）
‖＜ｒM ＞‖² ＋ａｒMx＝ｔ0 ｔ1 δ0,1 ・・・（22ａ）
‖＜ｒM ＞‖² −ａｒMx＝ｔ0 ｔ-1δ0,-1 ・・・（22ａ）
δ0,1²＜１，δ0,-1² ＜１・・・（23）
これらの式から条件（３），（９）と式（22ａ），（22ｂ）から、‖＜ｒM ＞‖² ＋ａｒMxとａδ0,1、‖＜ｒM ＞‖² −ａｒMxとａδ0,-1 とが各々同符号になる事に注意して整理すれば、次式（24ａ）〜（25ｂ）が得られる。
【００７４】
【数４】

式（24ａ），（24ｂ）は更に変形すると、次式（26），（27）が得られる。 ‖＜ｒM ＞‖² ＝（ａ²β+²）／（１＋β-²）・・・（26）
ｒMx＝（ａβ+ β- ）／（１＋β-²）・・・（27）
但し、β+ ，β- は次式（28ａ），（28ｂ）で定義されるものとする。
β+ ＝（α1 ＋α-1）／２・・・（28ａ）
β- ＝（α1 −α-1）／２・・・（28ｂ）
そして、式（20）（21ａ），（21ｂ）に式（26）（27）を代入して条件（３），（９）を用いれば、ｔ0 ，ｔ1 ，及びｔ-1が次式（29）（30ａ），（30ｂ）で求まる。
【００７５】
【数５】

これらを用いて更に、ｔ2 ，ｔ-2，＜ｌM ＞及びＲM が次のように求められる。
【００７６】
【数６】

５．Ｃの求め方
前述の条件（14）の下で、前述の方程式（13）を解く。方程式（13）を変形すると、次の（31），（32）が得られる。
Ｃ^-1Ｔ1^-1Ｔ3 Ｃ＝Ｐ1 Ｐ3^-1 ・・・（31）
Ｃ^-1Ｔ2^-1Ｔ3 Ｃ＝Ｐ2 Ｐ3^-1 ・・・（32）
ここで次式（33）〜（37）に従った置き換えを行なう。
【００７７】
【数７】

すると、方程式（31），（32）は次の式（38）〜（41）に分解される。
ＲC^-1 Ｒ1 ＲC ＝Ｒ2 ・・・（38）
ＲC^-1 Ｒ3 ＲC ＝Ｒ4 ・・・（39）
（Ｉ−Ｒ1 ）＜ｌC ＞＝＜ｌ1 ＞−ＲC ＜ｌ2 ＞・・・（40）
（Ｉ−Ｒ3 ）＜ｌC ＞＝＜ｌ3 ＞−ＲC ＜ｌ4 ＞・・・（41）
従って、ＲC 及び＜ｌC ＞を求めることが目標となる。ここで、方程式（38），（39）の形から、回転行列間の相似Ｒ1 〜Ｒ2 ，Ｒ3 〜Ｒ4 及びそれらの相似関係を表現する相似変換行列がともにＲC であることが判る。
そこで、Ｒi （i=1,2,3,4 ）の回転方向を表わすベクトルを＜ｖi ＞（i=1,2,3,4 ）として、次式（42），（43）が得られる。ここで、＜ｖi ＞（i=1,2,3,4 ）は、条件（14）と定義式（34），（36）から一意的に定めることが出来る。
＜ｖ1 ＞＝ＲC ＜ｖ2 ＞・・・（42）
＜ｖ3 ＞＝ＲC ＜ｖ4 ＞・・・（43）
更に、次式（44）の関係を用いれば、ＲC を求めることが出来る。
（＜ｖ1 ＞，＜ｖ3 ＞，＜ｖ1 ＞×＜ｖ3 ＞）
＝ＲC （＜ｖ2 ＞，＜ｖ4 ＞，＜ｖ2 ＞×＜ｖ4 ＞）・・・（44）
方程式（40），（41）に関しては、次式（45）の形にひとまとめにして、最小
２乗法を適用すれば＜ｌC ＞が求められる。
【００７８】
【数８】

６．方式Ｂによる移動制御の処理フローの概略
最後に、１，〜５．の説明事項を前提として、方式Ｂにおける準備作業と自律アプローチ動作のための処理フローの概略を説明する。先ず、準備作業について補足すれば、次のようになる。
（１）作業開始に先だって、上記３で説明したように、３つの位置ＣＢ１〜ＣＢ３にロボットを順次移動させる。
（２）各位置ＣＢ１〜ＣＢ３で、代表ワーク１４上のマーク座標系１５ａの画像を取得し、画像処理装置２０内の画像処理によって各ドット中心の視線方向を表わすデータを取得する。
（３）上記５．で説明したアルゴリズムに従った処理を画像処理装置２０内で行い、カメラ座標系Ｃのデータ（ＲC ，＜ｌC ＞のデータ）を算出・記憶する。
【００７９】
（４）カメラキャリブレーションが完了したら、ロボットを図２２に示した位置Ｐａに移動させ、代表ワーク１４に対するアプローチ完了状態を現出させる。
【００８０】
（５）各位置ＣＢ１〜ＣＢ３で、マーク座標系１５ａの画像を取得し、画像処理装置２０内の画像処理によって各ドット中心の視線方向を表わすデータを取得する。
（６）上記１．２．４．で説明したアルゴリズムに従った処理を画像処理装置２０内で行い、アプローチ完了状態表現データ（Ｍg のデータ）を算出・記憶する。
【００８１】
自律アプローチ動作は、図２５のフローチャートに示したように、次の諸ステップを含む処理サイクルによって実行される。
〔Ｋ１〕＜ｄ0 ＞，＜ｄ1 ＞，＜ｄ-1＞，＜ｄ2 ＞，＜ｄ-2＞を画像処理で求め、Ｍ0 を計算する。
〔Ｋ２〕記憶されているＭg のデータとステップＫ１で求めた＜ｄ0 ＞，＜ｄ1 ＞，＜ｄ-1＞，＜ｄ2 ＞，＜ｄ-2＞から計算されるＭ0 とを比較し、両者の一致／不一致を判定する。一致していればアプローチ完了状態を意味するから、ステップＫ６へ進む。不一致であれば、アプローチ完了状態が達成されていないことを意味するから、ステップＫ１へ戻る。Ｍg とＭ0 の一致度の評価には種々のアルゴリズムが利用出来る。例えば、下記の式（46）で判定指標Δを算出し、Δ＜ε（εは十分小なる正値）であればアプローチ完了、Δ≧εであればアプローチ未完了とすれば良い。
【００８２】
【数９】

〔Ｋ３〕ロボットの手先（フランジ上に設定された座標系の原点）の位置・姿勢を表わす４行４列の行列Ｔ0 を計算・記憶する。
〔Ｋ４〕準備作業で取得したデータ（Ｃ，Ｍg のデータ）と上記説明したアルゴリズムを用いて、（２）の右辺を計算し、直交移動目標位置Ｔg を求める。
【００８３】
〔Ｋ５〕直交移動目標位置Ｔg へ向かうための移動指令を作成してサーボへ渡し、ロボットを移動させる。
【００８４】
もし、システムが無誤差の完璧なものであれば、ステップＫ１〜Ｋ５の１サイクルでアプローチ完了状態に至る筈であるが、実際には視覚センサの誤差、計算の誤差等のために、アプローチ完了状態は、逐次近似的に達成される。
〔Ｋ６〕ロボットを停止させて処理を終了する。
【００８５】
以上Ａ方式、Ｂ方式の実施形態についての説明から理解されるように。本発明に従えば、予め入力されたアプローチ完了状態の位置および姿勢となるように自律的にロボットのアプローチ動作が行なわれるので、作業効率が向上する。
【００８６】
なお、本実施形態では、ハンド１２によりワーク１４を搬送する例を示したが、溶接その他の作業に本発明を適用することができることは言うまでもない。
【００８７】
また、アプローチ完了状態表現データの入力方法として、本実施形態ではカメラ１３の撮影画像を使用したが、それまでの作業経験等から予めそれが判っている場合には、教示操作４０のキー操作によって入力するようにしても良い。
【００８８】
さらに、本実施形態では、カメラ１３の視野内にワーク１４が入った時を自律的にアプローチ動作を開始するタイミングとしたが、ワーク１４とツール１２との距離等、他のパラメータに基づいてアプローチ動作を開始するタイミングを定めても良い。
【００８９】
【発明の効果】
本発明においては、アプローチ完了状態表現データがロボット手先（即ち、それに固定されたカメラ）と目標物の相対的な位置関係を記述するデータの形態で予め入力されているので、アプローチ動作時に、このアプローチ完了状態表現データに対応するカメラと目標物の相対的な位置関係を実現するような移動制御を行なえば、自律的なアプローチ動作によってオペレータに負担をかけることなくアプローチ完了状態が任意のワークに対して達成される。
【００９０】
従って、多数の非位置決めワークに対する各種ロボット作業の効率化を容易に実現出来る。また、ジョグ送りから自律アプローチ動作への自動移行を可能としたので、ロボットの至便性の更なる向上が期待出来る。
【図面の簡単な説明】
【図１】本発明のロボット移動制御方法をハンドリングロボットの例を用いて概念的に説明する図である。
【図２】本実施形態のロボットの移動制御方法を方式Ａに従って実行するするためのロボット制御システムの要部構成図である。
【図３】本実施形態のロボットの移動制御方法を方式Ｂに従って実行するするためのロボット制御システムの要部構成図である。
【図４】本実施形態で使用されるロボット制御装置のハードウェアの構成を示す要部ブロック図である。
【図５】本実施形態で使用される教示操作盤のパネル面の概略構成を示す図である。
【図６】方式Ａを採用した場合について、ロボットの動作を説明する図である。
【図７】方式Ｂを採用した場合について、ロボットの動作を説明する図である。
【図８】実施形態におけるロボット移動制御のための教示操作盤の操作手順と処理の大要を示すフローチャートである。
【図９】方式Ａを適用するアプリケーションにおいてカメラ光軸方向の条件について説明する図である。
【図１０】方式Ａを適用するアプリケーションの概略を説明する図である。
【図１１】方式Ａを適用するアプリケーションにおけるアプローチ動作の処理について、概略を説明するためのフロー説明図の前半部分である。
【図１２】方式Ａを適用するアプリケーションにおけるアプローチ動作の処理について、概略を説明するためのフロー説明図の後半部分である。
【図１３】図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ１〜Ｈ７）である。
【図１４】図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ８〜Ｈ１９）である。
【図１５】図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ２０〜Ｈ２９）である。
【図１６】図１３〜図１５に示したフローチャートの関連説明図で、ベクトル＜ａ0 ＞，＜ｂ0 ＞，＜ａ1 ＞，＜ｂ1 ＞を表わしている。
【図１７】図１３〜図１５に示したフローチャートの関連説明図で、カメラ座標系とベクトル＜ｚ＞を表わしている。
【図１８】図１３〜図１５に示したフローチャートの関連説明図で、ロボット移動前後のベクトル＜ｅ＞と＜ｆ＞の画面上での関係を表わしている。
【図１９】図１３〜図１５に示したフローチャートの関連説明図で、ロボットの前進後退移動によって、画面上で□が×に接近する様子が示されている。
【図２０】図１３〜図１５に示したフローチャートの関連説明図で、アプローチ完了状態が達成された時点における画面が描かれている。
【図２１】Ｂ方式の実施形態について、自律アプローチ動作に必要なアルゴリズムの大要を説明する図である。
【図２２】Ｂ方式の実施形態について、自律アプローチ動作を説明する図である。
【図２３】Ｂ方式の実施形態で使用されるマーク座標系について説明する図である。
【図２４】Ｂ方式の実施形態において、カメラがマーク座標系を見る視線方向を記述するベクトルについて説明する図である。
【図２５】Ｂ方式の実施形態における自律アプローチ動作の処理の概要を記したフローチャートである。
【符号の説明】
１ａアプローチ完了状態入力手段
１ｂ相対位置認識手段
２，１０ロボット
２ａツール（ハンド）
３，１４ワーク
４メモリ
５ジョグ送りボタン
６軸移動制御手段
１１アーム
１２ツール
１３カメラ
１３ａカメラ座標系
１４ワーク
１４ａ特徴部
１５マーク
１５ａマーク座標系
２０画像処理装置
３０ロボット制御装置
３１プロセッサボード
３１ａプロセッサ
３１ｃＲＡＭ
３１ｂＲＯＭ
３５ディジタルＩ／Ｏ
３６ａＣＲＴ
３６ｂ操作パネル
３７アナログＩ／Ｏ
３９バス
４０教示操作盤
４１表示画面
４２ファンクションキー
４４非常停止ボタン
４５カーソルキー
４６テンキー部
４７（Ｊ１〜Ｊ６）ジョグ送りボタン
５１〜５６サーボモータ
５８ＲＳ２３２Ｃ機器
ＣＢ１〜３カメラのキャリブレーションを行なう位置
Ｄ0 ，Ｄ1 ，Ｄ-1 ，Ｄ2 ，Ｄ-2 マーク座標系のドット[0001]
BACKGROUND OF THE INVENTION
The present invention provides robot movement control. Method More specifically, movement control for moving the robot to a target position based on data acquired using a visual sensor Method About. Movement control of the present invention Method Is particularly advantageously applied to jog feed type movements.
[0002]
[Prior art]
Traditionally known robot movement control Method One of them is jog feed. This is because the operator operates the jog feed button on the teaching operation panel to move the robot to the target position. Method It is. The jog feed is used for work such as lifting the work with a hand attached to the tip of the arm and stacking it on a predetermined place.
[0003]
In that case, the operator gradually moves the robot by selective operation of a plurality of jog feed buttons while visually confirming the positional relationship between the work and the hand, and approaches with TCP (Tool Center Point) as a target. The point was reached. When this approach operation is completed, the hand operation button is operated to grab the workpiece.
[0004]
[Problems to be solved by the invention]
However, in general, it takes a lot of time and a high degree of skill to reach a precise approach position by jog feed. That is, the moving direction of the robot that can be moved by jog feed is limited (for example, translation along the coordinate axis directions of XYZ, rotation around the coordinate axis, and movement of the J1 axis to J6 axis in the case of each axis feed), and simple It was good to say that it was impossible to complete an accurate approach at once by operating the jog feed button.
[0005]
The present invention has been made in view of such a situation, and enables robotic movement control that enables an autonomous approach to a target position and improves work efficiency. Method The purpose is to provide.
[0006]
[Means for Solving the Problems]
In order to solve the above-described problems, the present invention enables a robot to perform an autonomous approach operation. For objects with different positions Robot movement control that can be done Method Is to provide. Of the present invention Method Then, control means including a visual sensor provided with camera means and image processing means and a robot control device is used.
[0007]
Of the present invention Method The steps include an approach completion state teaching stage for storing approach completion state expression data representing an approach completion state for an object in the control means, and an approach action execution stage for causing the robot to perform an approach action by the control means. Further, the approach action execution stage includes an autonomous approach action execution stage in which the approach action is autonomously performed based on the approach completion state expression data.
[0008]
The approach completion state expression data in the teaching stage specifies the relative positional relationship between the hand of the robot and the object, and the movement target position of the robot in the autonomous approach operation execution stage is determined by software processing in the control means. It is determined as a position where the relative positional relationship between the object specified by the approach completion state expression data and the robot hand is realized.
[0009]
The approach completion state expression data stored in the approach completion state teaching stage can be created in the control means based on an image obtained by photographing an object representing the object by the camera means.
In one preferred form, the approach completion state expression data is taught to include data representing a feature portion of the object representative object and the position of the center of gravity on the image.
[0010]
In another preferred embodiment. A mark coordinate system recognizable by the visual sensor is installed on the surface of the object and the object representative object, and the approach completion state expression data includes data representing the position / orientation of the mark coordinate system on the image. Be taught.
[0011]
Prior to the autonomous approach operation execution stage, jog feed may be performed in order to perform a preliminary approach operation. At that time, it is preferable that the transition from the jog feed stage to the autonomous approach operation execution stage is automatically performed by the control means.
[0012]
In addition, the software processing in the autonomous approach motion execution stage compares data representing the relative positional relationship between the hand of the robot and the object recognized by the visual sensor at that time, and the taught approach completion state expression data. The process, the process for controlling the movement of each axis of the robot based on the comparison result, and the process for determining completion / non-completion of the approach completion state are sequentially repeated until the completion of the approach completion state is determined. It can be.
[0013]
The most important feature of the present invention is that approach completion state representation data is pre-input in the form of data describing the relative positional relationship between the robot hand (ie, the camera fixed thereto) and the target, An autonomous approach operation is realized by performing movement control that realizes the relative positional relationship between the camera and the target corresponding to the approach completion state expression data during the approach operation.
[0014]
With this feature, the approach completion state can be efficiently achieved for any workpiece without burdening the operator. Moreover, if automatic transition from jog feed to autonomous approach operation is performed, the convenience of the robot can be further improved.
[0015]
DETAILED DESCRIPTION OF THE INVENTION
FIG. 1 shows a robot movement control according to the present invention. Method It is a figure which illustrates notionally using the example of a handling robot. Reference numeral 1a represents an approach completion state input means for expressing the relative positional relationship (including posture) of the work 3 and the hand 2a when the approach operation of the robot 2 with respect to the work 3 representing the approach target is completed. (Hereinafter referred to as “approach completion state expression data”) is previously input to the memory 4. Needless to say, the position of the hand 2a can be represented by the position of the robot hand (for example, the origin of the coordinate system set on the flange of the TCP or the end of the final arm).
[0016]
In actual work, the operator operates the jog feed button 5 (generally, approach operation manual command means) to cause the axis movement control means 6 to control each axis of the robot 2 and execute the approach operation. The movement control for the approach operation includes the approach completion state expression data input by the approach completion state input means 1a and the relative recognition of the relative positional relationship between the work 3 and the hand 2a (the hand of the robot 2) in the memory 4. This is executed based on the relative position recognition data acquired by the position recognition means 1b.
[0017]
The approach operation is completed when the relative positional relationship between the workpiece 3 and the hand 2a (the hand of the robot 2) matches that represented by the approach completion state expression data. Note that the individual workpieces 3 that are targeted during actual work do not have to be accurately positioned. Further, as will be described below, the approach completion state input unit 1a and the relative position recognition unit 1b are embodied using a two-dimensional visual sensor.
[0018]
2 and 3 show the movement control of the robot of this embodiment. Implement the method It is a principal part block diagram of the robot control system for this. The robot 10 is a handling robot or another type of robot, and a tool (end effector) 12 is attached to the tip of the arm 11. Here, a hand is shown as the tool 12. The robot 10 is operation-controlled by the robot control device 30 and grips and conveys the workpiece 14. Needless to say, the type of the tool 12 and the work content after the approach depend on the application (for example, if the tool 12 is a machining tool, machining is performed).
[0019]
In addition, a camera 13 is attached to the arm 11 of the robot 10 so as to move together with the tool 12. The camera 13 is connected to the image processing apparatus 20 and sends the captured image to the image processing apparatus 20.
[0020]
The image processing apparatus 20 has a well-known configuration and function including a CPU, a frame memory, an image processing processor, a program memory, a data memory, and the like. The image processing apparatus 20 stores and processes a photographed image from the camera 13, and obtains the result. Data to be transmitted to the robot controller 30. Here, two forms are adopted in relation to the image processing method.
[Method A] As shown in FIG. 2, the captured image from the camera 13 is contoured to obtain the position of the center of gravity of the work image. Further, the feature 14a and the like of the workpiece 14 are extracted to obtain data such as the positional relationship between the center of gravity and the feature. If this data is created based on an image obtained in a state where the approach to the representative work is realized, it can be used as approach completion state expression data.
[Method B] As shown in FIG. 3, a mark 15 that can define a coordinate system (mark coordinate system 15 a) is prepared in advance on the work 14, and a mark related to the camera coordinate system 13 a is obtained by processing a captured image by the camera 13. Data on the relative position and orientation of the coordinate system 15a is obtained. As in the case of method A, if this data is created based on an image obtained in a state where the approach to the representative work is realized, it can be used as approach completion state expression data.
[0021]
The instruction of the approach operation of the robot 10 is performed by the teaching operation panel 40 connected to the robot control device 30. As will be described later, the teaching operation panel 40 includes a liquid crystal display and various keys. Moreover, it is preferable that it is a size which can be carried by an operator. The robot control device 30 performs movement control to realize the approach completion state represented by the approach completion state expression data in accordance with the manual command input from the image processing device 20 and the teaching operation panel 40 that perform image processing according to the method A or the method B. I do.
[0022]
FIG. 4 is a principal block diagram showing a hardware configuration of the robot control device 30. The robot controller 30 is provided with a processor board 31 coupled to a bus 39, and the processor board 31 is provided with a processor 31a, a ROM 31b, and a RAM 31c. The processor 31a controls the entire robot control device 30 according to the system program stored in the ROM 31b. The RAM 31c stores approach completion state expression data sent from the image processing apparatus.
[0023]
The digital servo control circuit 32 is coupled to the bus 39 and drives the servo motors 51, 52, 53, 54, 55 and 56 via the servo amplifier 33 according to a command from the processor board 31. These servomotors are built in the main body mechanism of the robot 10 and drive each axis of the robot 10.
[0024]
A serial port 34 is further coupled to the bus 39 and is connected to a teaching operation panel 40 with a liquid crystal display and other RS232C devices 58. The RS232C device 58 also includes the image processing device 20. The teaching operation panel 40 is large and heavy enough to be carried by an operator, and a jog feed button or the like used as approach operation manual command input means is provided on the panel. In addition, a CRT 36a can be optionally connected to the serial port, and the working state and the like can be confirmed from the position of the CRT 36a.
[0025]
An operation panel 36 b is connected to the digital I / O 35, and an operation command for the robot 10 can be issued at the position of the robot control device 30. Further, the digital I / O 35 is connected to a grip driving unit of the tool 12 and can issue an operation command thereof. For example, when a laser welding machining head is used as the tool 12, the analog power I / O 37 is connected to a welding power supply device, and a welding voltage is commanded through the analog I / O 37. The large capacity memory 38 stores teaching data and the like.
[0026]
FIG. 5 is a diagram showing a schematic configuration of the panel surface of the teaching operation panel 40. The display screen 41 is, for example, a liquid crystal screen, and the detailed data of the movement command program is switched and displayed. The function key 42 is a key for selecting a menu displayed at the lower end of the display screen 41. The teaching operation panel valid switch 43 is a switch for switching whether the operation of the teaching operation panel 40 is valid or invalid.
[0027]
The emergency stop button 44 is a button for emergency stopping the operation of the robot 10. The cursor key 45 is a key for moving the cursor displayed on the display screen 41. The numeric keypad 46 is provided with numeric keys and other keys, and can input and delete numerical values and characters.
[0028]
A series of jog feed buttons 47 (J1 to J6) are buttons for inputting a movement command by designating the translation / rotation direction and the + − direction in the normal mode in which the conventional jog feed is performed. In the (autonomous approach mode), as described later, it is used as an approach operation manual command input means.
[0029]
Next, an outline of the approach operation in the present embodiment will be described.
6 and 7 are diagrams for explaining the operation of the robot in each case where the above-described method A and method B are employed. Here, the tool 12 is a hand, and let us consider a palletizing operation in which the hand 12 grips the workpiece 14 sequentially supplied within the operation range of the robot and conveys it to a pallet (not shown) arranged at a predetermined position. The sequentially supplied workpieces 14 are not accurately positioned, and the supply positions may vary considerably.
[0030]
[1] Preparation
1. Prior to the start of work, an approach completion state data acquisition work is performed. In this work, the representative work 14 is placed in an appropriate position (it is sufficient if it is within a range that is considered to represent the supply position in actual work), and the robot is moved by a jog operation in the normal mode (conventional method). To achieve the approach completion state. This work needs to be accurate, but it is an important feature of the present invention that such work can be done only once as long as the type of work does not change (in the conventional method, jog feed is performed for each work). You need to start over).
[0031]
2. When the approach completion state is realized with respect to the representative work 14, the camera 13 is photographed via the robot control device 30 to obtain an image for creating approach completion state expression data.
3. The acquired image is subjected to image processing in the image processing apparatus 20, and the following data is created corresponding to the above-described methods A and B, and stored in the RAM 31c as approach completion state expression data.
[0032]
[Method A]: Position and orientation data such as the positional relationship between the center of gravity Pg of the representative work 14 and the characteristic portion 14a, the distance L1 between the position Pc = Pa and the center of gravity Pg when the TCP approach is achieved. Here, the “center of gravity” is the center of gravity of the representative workpiece 14 that is actually recognized as an image, that is, the “image center of gravity”.
[Method B] Data representing the relative position and orientation of the mark coordinate system 15a with reference to the camera coordinate system (details will be described later). When the method B is adopted, the same mark coordinate system 15a as that prepared for the representative workpiece 14 is prepared at the same position on the workpiece with the same posture for all workpieces handled in actual work. It should be noted that it is a prerequisite.
[0033]
[2] Actual work
1. After the approach completion expression data is input, actual work is started on the newly supplied workpiece 14. The operator presses the jog feed button 47 (FIG. 5) to move and control each axis of the robot, and moves the TCP representing the position of the tool 12 from the start point (Pc = Ps) toward the work 14 by jog feed. During this time, the two-dimensional visual sensor including the camera 13 and the image processing device 20 is also operating.
[0034]
2. When the tool 12 approaches the work 14 and the work 14 enters the field of view of the camera 13, the image processing device 20 detects this, and the robot control device 30 switches from the normal mode to the autonomous approach mode, and the autonomous approach operation starts. Is done. When the work 14 is in the field of view of the camera 13 from the beginning, switching from the normal mode to the autonomous approach mode occurs immediately after the jog button is pressed.
3. The autonomous approach operation is continued repeatedly as many times as the jog feed button 47 is pressed, unless the robot control device 30 confirms the realization of the approach completion state. The final movement target position of the approach operation is a position represented by approach completion state expression data inputted in advance.
4). After the approach operation starts, the approach operation is completed when the TCP of the robot reaches a position where data equivalent to the approach completion state expression data created by the A method or the B method can be obtained. When the approach is completed, the robot stops and the robot controller 30 autonomously returns from the autonomous approach mode to the normal mode.
5). After the stop, the operator grips the work 14 by combining hand operation and jog feed as necessary, conveys the work 14 to a predetermined position, and releases the work 14.
By repeating the above procedure for each workpiece 14 after the next time, palletizing work for a large number of workpieces 14 can be performed efficiently.
[3] Outline of processing
FIG. 8 is a flowchart showing an outline of the operation procedure and processing of the teaching operation panel for such robot movement control. The process of each step is briefly described as follows.
[S1] It is determined whether or not the jog feed button 47 has been pressed and a jog feed command has been issued. If yes, the process proceeds to step S2, and if not, step S1 is repeated.
[S2] A predetermined axis is controlled in accordance with the movement content (direction, axis, etc.) designated by the jog feed button 47, and the jog feed movement of the robot 10 is started. For example, if movement in the J6 axis + direction is designated, a movement command for moving the J6 axis in the + direction is created and passed to the servo.
[S3] It is determined whether or not the jog feed button 47 is turned off. If turned off, the process proceeds to step S8, and if not, the process proceeds to step S4.
[S4] The image processing apparatus 20 confirms whether or not a signal indicating that the workpiece 14 has entered the field of view of the camera 13 is received. If received, the process proceeds to step S5. If not received, the process proceeds to step S3. Return.
[S5] The approach operation process is started. Specific examples of processing contents will be described later separately for the A method and the B method.
[S6] It is determined whether or not the jog feed button 47 is turned off. If it is turned off, the process proceeds to step S8, and if not, the process proceeds to step S7.
[S7] It is determined whether or not the approach has been completed. If completed, the process proceeds to step S8, and if not completed, the process returns to step S6. The approach completion determination method will also be described in specific examples of the A method and the B method.
[S8] The operation of each axis of the robot 10 is stopped.
[0035]
[4] Processing during autonomous approach operation in method A
When the present invention is applied to an actual application, as shown in FIG. 9, the robot operation for the autonomous approach can be performed under the condition that the optical axis direction C of the camera 13 mounted on the hand of the robot is fixed. That is practical. Therefore, here, a case as shown in FIG. 10 is assumed as the most realistic application.
As shown in FIG. 10, the work 14 is supplied onto one work supply surface PL1. The workpiece supply plane PL1 is perpendicular to the Z axis of the workpiece coordinate system Σw, but is not accurately positioned on the workpiece 14 on the workpiece supply plane PL1. The characteristic portion 14a of the workpiece 14 is on the workpiece upper surface 14b, and the robot performs an approach operation with the optical axis C of the camera 13 always oriented in a direction perpendicular to the surface PL1 and the workpiece upper surface 14b.
[0036]
It is assumed that the Zt axis of the tool coordinate system Σt is set to be parallel to the axial direction of the hand 12 and the optical axis C. Therefore, the autonomous approach operation is a process of moving the TCP of the robot from the approach start position Ps to the approach completion position Pa while maintaining the parallel relationship between the Zt axis of the tool coordinate system Σt and the Z axis of the work coordinate system. The visual sensor function of the camera 13 and the image processing device 20 is used to make TCP approach and reach the approach completion position Pa. The processing algorithm will be described below. In the following description, the symbol <> is used to represent a vector.
[0037]
1. Overview
First, the flow explanatory diagrams following FIG. 11 to FIG. 12 explain the steps of the approach operation by appropriately using virtual screens. The flow is divided into eight steps AP1 to AP8, and FIG. -AP3 and AP4-AP8 are shown in FIG.
(AP1) The state after the start of the approach and the incomplete approach. In this state, the image captured by the camera 13 is generally depicted in a form compared with an image representing the approach completion state. In the following description, the meaning of each symbol is as follows.
X: At the position of the workpiece center of gravity Pg on the screen in the approach completion state, the data on the camera coordinate system is stored in the memory in the image processing apparatus 20 by the preparatory work described in [1] above.
Δ: At the position of the feature portion 14a on the screen in the approach completion state, the data on the camera coordinate system is stored in the memory in the image processing apparatus 20 by the preparatory work described in [1] as with the data of the center of gravity. Yes.
[0038]
□; The position of the center of gravity Pg in the image captured by the camera 13 in an arbitrary state after the start of the approach is grasped as needed by image processing performed in the image processing apparatus 20 during the approach operation.
[0039]
◯: The position of the characteristic portion 14a in the image captured by the camera 13 in an arbitrary state after the start of the approach, as well as the position of the center of gravity, and is grasped as needed by image processing performed in the image processing apparatus 20 during the approach operation. .
[0040]
(AP2) After starting the approach, first, in order to compare the posture of the robot around the optical axis C with that in the approach completed state, a check is made on the relationship between the position of the center of gravity viewed on the screen and the position of the characteristic portion. Specifically, it is determined whether or not the unit vector <d0> representing the direction of x → Δ and the unit vector <d1> representing □ → o are parallel.
(AP3) Unless the determination of AP2 is yes (parallel), proceed to AP3 and move the robot around the optical axis C by a predetermined amount to adjust the relationship between <d0> and <d1>. After the adjustment, the process returns to AP2, and if the adjustment is not completed, the process proceeds to AP3 again. AP2 and AP3 repeat until the determination of AP2 becomes yes (parallel).
[0041]
(AP4) If the determination of AP2 is yes (parallel), proceed to AP4. In AP4, in order to compare the position of the robot with that in the approach completion state, the position of the characteristic portion viewed on the screen is checked. Specifically, it is determined whether or not the position of ◯ matches the position of △.
(AP5) Unless the determination of AP4 is yes (coincidence), the robot proceeds to AP5 and performs robot position adjustment to translate the camera 13. The moving direction is a direction <e> in which the position of Δ on the screen (the immobile position expressed by stored data) approaches the position of ○ (moves when the robot moves). Since this adjustment generally deteriorates the parallel relationship between <d0> and <d1>, the process returns to AP2, and the parallel relationship between <d0> and <d1> is checked again. If the parallel relationship is broken, proceed to AP3. As described above, posture adjustment around the optical axis C (AP3) and check (AP2) are repeated until the determination of AP2 becomes yes (parallel).
[0042]
(AP6) When the determination of AP4 is yes (match), the positional relationship between the position of ◯ and the position of Δ on the screen as shown in AP6 is realized.
(AP7) In the state of AP6, direction vectors representing the three line-of-sight directions of the camera 13, <a0>, <b0>, <b1> are calculated. <A0> is a unit vector representing the line-of-sight direction in which the camera 13 looks at Δ, and is also a unit vector representing the line-of-sight direction in which the camera 13 looks at ○ simultaneously (note that the position on the screen is the same). On the other hand, <b0> and <b1> are unit vectors representing the line-of-sight directions where the camera 13 looks at x and □, respectively. Then, the coincidence / mismatch of the vectors <b0> and <b1> is checked.
[0043]
(AP8) Unless <b0> and <b1> match in AP7, the process proceeds to AP8 and the position of the robot is adjusted. The movement at this time is limited to advancing / retreating along the line-of-sight direction in which the camera 13 looks at Δ and ○, and is performed until the inner product <a0> · <b0> matches the inner product <a0> · <b1>. In a general case, this adjustment breaks the parallel relationship between <d0> and <d1>.
[0044]
Therefore, after the adjustment is completed (after matching the inner products), the process returns to (AP1) again, and the above-described cycle is repeated. Thus, asymptotically toward the approach completion state, an affirmative determination is made at the AP 7 which has been reached in a state where “Δ and ○ match and x and □ match” is substantially realized. When the state at the time of approach completion state data input (that is, the approach completion state) is reproduced in this way, the autonomous approach processing is terminated.
[0045]
2. Algorithm description
Next, an algorithm according to the schematic flow described above will be described in addition to the flowcharts of FIGS. 13 to 15 and FIGS. 16 to 20 for related explanation in addition to the reference diagrams. The correspondence between the flowchart of each figure and the description of the outline is as follows.
FIG. 13 (Steps H1 to H7); corresponding to AP1 to AP3
FIG. 14 (steps H8 to H19); corresponding to AP4 to AP5
FIG. 15 (steps H20 to H29); corresponding to AP6 to AP8
[H1] Calculates a 4-by-4 matrix T representing the position and orientation of the robot's hand (the origin of the coordinate system or TCP set on the flange at the end of the final arm), and updates the register value storing the matrix T To do.
[H2] <a1> and <b1> are obtained by image processing. As shown in FIG. 16, <a1> is a unit vector representing the line-of-sight direction in which the camera 13 sees a circle (current position on the feature 14a), and <b1> is a □ (current center of gravity). This is a unit vector representing the line-of-sight direction of viewing the image). The vector <z> is a unit vector in the z-axis direction of the camera coordinate system in which the origin is set at a position Oc corresponding to the lens center of the camera 13, as shown in FIG. The z-axis direction of the camera coordinate system coincides with the optical axis C direction.
[0046]
[H3] Using the vectors <z>, <a1>, and <b1>, a unit vector <d1> (see the lower left part of FIG. 11) facing □ from □ is calculated.
[H4] It is determined whether the vector <d0> and the vector <d1> are parallel. If yes, then go to Step H8, if no, go to Step H5.
[0047]
[H5] A matrix R representing the amount of rotation adjustment required for parallelization of <d0> and <d1> is calculated.
[H6] A matrix T representing the movement target position / orientation of the robot is calculated using the matrix R obtained in step H5 and the matrix T calculated in step H1.
[H7] The robot is moved to the position / posture represented by the matrix T obtained in step H6.
[0048]
[H8] It is determined whether the vector <a0> matches the vector <a1>. If yes, then go to step H20, otherwise go to step H9.
[H9] A step l of the translational movement of the robot is set. l is set by calculating a product of a fixed value l0 (for example, 2 cm) having an appropriate size and | <a0> × <a1> |.
[H10] Using the vectors <z>, <a0>, and <a1>, a unit vector <e> (see the upper part of FIG. 18) facing from the triangle to the circle is calculated.
[H11] A matrix T representing the position / posture obtained by translating the robot by l in the <e> direction is calculated. The partial matrix I is a 3 × 3 unit matrix as shown in FIG. The matrix C represents the position / orientation of the camera coordinate system when the position / orientation of the camera coordinate system Oc-xyz (see FIG. 17) is viewed from the camera hand (the coordinate system or TCP set on the flange at the end of the final arm). It is a matrix that represents. Data representing the matrix C is stored in the robot controller by a known appropriate camera calibration.
[0049]
[H12] The robot is moved to the position / posture represented by the matrix T obtained in step H11.
[H13] The vector <a1> is obtained in the state after the movement.
[H14] It is determined whether the vector <a0> matches the vector <a1>. If yes, the process returns to step H1, and if no, the process proceeds to step H15.
[H15] A unit vector <f> (see the lower part of FIG. 18) from Δ to O in the state after movement is calculated.
[0050]
[H16] The sign of the inner product of the vector <e> obtained in step H10 and <f> obtained in step H15 is checked. If it is negative, it means that ○ has overrun Δ due to the robot movement (the position of Δ is fixed by the stored data on the image, so that ○ changes so as to approach Δ). At the time of overrun (when judgment is YES), the routine proceeds to step H17. If the determination is no, the process proceeds to step H18.
[0051]
[H17] The moving amount l in steps is halved.
[H18] <f> obtained in step H15 is again set as the value of <e>.
[H19] Recalculate a 4 × 4 matrix T representing the position / posture of the hand of the robot, and return to Step H11.
[0052]
[H20] It is determined whether the vector <b0> matches the vector <b1>. If yes, it means that the approach completion state has been realized as shown in FIG. 20, and the process ends. If no, the process proceeds to step H21.
[0053]
[H21] The robot increment l is reset. l is set by calculating an inner product of a fixed value l0 '(for example, 1 cm) having an appropriate size and <a0> and <b1>-<b0>.
[H22] A matrix T representing the position / posture obtained by translating the robot by l in the <a0> direction is calculated.
[0054]
[H23] The robot is moved to the position / posture represented by the matrix T obtained in step H22. As shown in FIG. 19, this movement is a forward / backward movement that changes so that □ approaches x on the image (note that x is fixed by approach completion state storage data).
[H24] <b1> in the state after movement is obtained by image processing.
[H25] The inner product m of <a0> and <b1>-<b0> is calculated using <b1> calculated in step H24.
[0055]
[H26] It is checked whether m = 0. If yes, return to step H1. If no, go to step H27.
[H27] The sign of the product of m and the step adjustment amount l obtained in step H25 is checked. If it is negative, it means that □ overruns x due to robot movement. At the time of overrun (when judgment is YES), the routine proceeds to step H28. If the determination is no, the process proceeds to step H29.
[H28] The step movement amount l is halved and the sign is reversed.
[H29] The matrix T of 4 rows and 4 columns representing the position and orientation of the hand of the robot is recalculated, and the process returns to step H22.
[0056]
By repeating the processing cycle using the algorithm described above, the robot gradually approaches the approach completion state, and eventually the approach completion state shown in FIG. 20 is achieved. In step H20, a yes determination output is issued, and the process ends.
[0057]
[5] Processing during autonomous approach operation in method B
In the application using method A, the autonomous approach operation was explained under the condition that the optical axis direction of the camera is constant, but in the method B application described below, this condition is removed and a more general case is shown. Think.
In this application, as shown in FIG. 22, the workpieces 14 in which the mark coordinate systems 15a are installed one by one at a fixed position on the workpiece surface are supplied to the workpiece supply PL2 without accurate positioning. Then, an autonomous approach operation is started from an arbitrary approach start position Ps (can be tilted in the camera optical axis direction; however, the mark coordinate system 15a is in the field of view) to an approach completion position Pa (can be tilted in the camera optical axis direction). A processing algorithm for achieving the movement will be described.
[0058]
Here, the position of the robot or tool is represented by a coordinate system (also simply referred to as a flange) set on the flange at the end of the final arm. The symbol <> is used to represent a vector as in the description of method A, but the individual meanings are not necessarily the same even if the characters in <> overlap with the description of method A. Not (eg <d ₀ > Etc.). The same applies to the matrix notation.
[0059]
1. Overview
The outline of the algorithm necessary for the autonomous approach operation is as shown in FIG. The current position of the flange (position at each time point during the approach operation) T0, the current relative position between the camera coordinate system and the mark coordinate system (relative position at each time point during the approach operation) M0, the flange position Tg when the approach is completed, the approach The geometric relationship between the camera coordinate system and the relative position Mg of the mark coordinate system in the completed state is as shown in FIG. Note that Mg is calculated and stored in advance, and C is taught by calibration. This is the same as in the case of method A.
[0060]
The basic equations defining the relationship between T0, M0, Tg, and Mg are as follows.
T0 CM0 = Tg CMg (1)
Therefore, this is T _g The following equation (2) solved for is a basic equation representing the movement target position on the orthogonal coordinate system of the robot.
Tg = T0 CM0 Mg ^-1 C ^-1 ... (2)
This T _g Is given to the servo as a final movement target point, the robot can be moved autonomously toward the approach completion state. Therefore, defining the algorithm of the autonomous approach operation results in a problem of specifically obtaining the right side of the equation (2).
[0061]
(2) T on the right side of the equation ₀ Represents the current position data of the robot, and the data of C can be obtained by appropriate calibration. Therefore, the method for obtaining M0 and Mg will be described.
[0062]
2. M0 and Mg equations
M0 and Mg both represent the position and orientation of the mark coordinate system 15a as viewed from the camera coordinate system (current and when the approach is completed). The mark coordinate system 15a used in this embodiment is shown in FIG. The mark coordinate system 15a is composed of five circular dots D0, D1, D-1, D2, and D-2 arranged in a grid pattern with an interval a.
[0063]
As shown in the figure, the dot interval a is a positive constant value.
a> 0 (3)
The center position of each dot is D0 (0,0,0), D1 (a, 0,0), D-1 (-a, 0,0), D2 (a, a, 0), D-2 ( -A, a, 0). Note that the mark coordinate system 15a may be configured with other patterns as long as the three-dimensional orthogonal coordinate system can be expressed.
[0064]
M0 and Mg are 4 × 4 homogeneous transformation matrices representing the position / posture relationship between the three-dimensional orthogonal coordinate systems, and can be placed as in the following equation (4).
[0065]
[Expression 1]

Here, RM is a 3 × 3 matrix representing rotation, and <lM> is a 3 × 1 matrix (vector) representing position. Hereinafter, M ₀ And M _g Let us consider obtaining an equation satisfying RM and <IM> as an equation satisfying the above. First, the following equations (5) and (6) are defined as vectors <e1> and <e2>.
[0066]
[Expression 2]

The information obtained from the visual sensor regarding the mark coordinate system 15a is the direction in which the center of each dot D0, D1, D-1, D2, and D-2 can be seen from the origin of the camera coordinate system (line-of-sight direction). These directions can be expressed by five unit vectors <d0>, <d1>, <d-1>, <d2>, <d-2> as shown in FIG.
[0067]
For the inner product of these vectors, δij defined by the following equation (7) is introduced.
δij = <di>. <dj> (i, j = -2, -1,0,1,2) (7)
Here, since the lengths of <d0>, <d1>, <d-1>, <d2>, and <d-2> are all 1, the absolute value of δij does not exceed 1.
| Δij | ≦ 1 (i = j is a necessary and sufficient condition for establishing the equal sign) (8)
Further, although the distance from the origin of the camera coordinate system to the center of each dot is unknown, this is set as ti (i = -2, -1,0,1,2). Of course, these take positive values.
[0068]
ti> 0 (i = -2, -1,0,1,2) (9)
Then, the following equations (10) to (12) are obtained.
t0 <d0> = <1M> (10)
t1 <d1> = <1M> + aRM <e1> (11a)
t-1 <d-1> = <1M> -aRM <e1> (11b)
t2 <d2> = t1 <d1> + aRM <e2> (12a)
t-2 <d-2> = t-1 <d-1> + aRM <e2> (12b)
Therefore, M0 and Mg can be determined by solving these equations with respect to RM and <LM> while obtaining ti. For specific solutions, see 4. This will be explained in the section on how to obtain M0 and Mg, and C representing the position / orientation of the camera coordinate system will be briefly described.
[0069]
3. C equation
C is a matrix expressing the position / posture of the camera coordinate system viewed from the flange, and the data is obtained by calibration of the camera 13. Various methods for calibration are known and can be used. However, the camera 13 can also be calibrated using the mark coordinate system 15a. Here, only the main points are briefly described.
[0070]
Data Pi relating to the relative position between the mark coordinate system 15a and the origin of the camera coordinate system is obtained by executing imaging by the camera 13 at three different points as indicated by reference numerals CB1 to CB3 in FIG. (I = 1,2,3) is obtained. Combining these with the robot position data Ti (i = 1, 2, 3) at the time of each photographing gives the following three sets of equations. The procedure for obtaining Pi (i = 1, 2, 3) is the same as that for M0 and Mg.
T1 CP1 = T2 CP2 = T3 CP3 (13)
It is possible to solve this equation for C under conditions where the robot postures of the three points CB1 to CB3 are different from each other. This condition is (14).
Condition: T1, T2, and T3 posture components are different from each other (14)
The obtained C data is stored in the robot control device or the image processing device. For the solution of equations, see 5. Supplementary explanation will be given in the manner of obtaining C.
[0071]
4). How to find M0 and Mg
In solving equations (10) to (12) to obtain RM, <IM>, consider first obtaining ti (i = -2, -1,0,1,2). For this purpose, the following equation (15) is replaced.
[0072]
[Equation 3]

And using this, equations (10) to (12) are transformed into the following equations (16) to (18).
<RM> = t0 RM ^-1 <D0> = <1M> (16)
<RM> + a <e1> = t1 RM ^-1 <D1> (17)
<RM> -a <e1> = t-1RM ^-1 <D-1> (18)
Where RM ^-1 Note that is a matrix representing rotation, the following equation (19) holds.

Using the equation (19), the equations (5) to (8), and the equations (16) to (18), the following relational equations (20) to (23) are obtained.
[0073]
‖ <RM> ‖ ² = T0 ² ... (20)
‖ <RM> ‖ ² + 2arMx + a ² = T1 ² ... (21a)
‖ <RM> ‖ ² -2arMx + a ² = T-1 ² ... (21b)
‖ <RM> ‖ ² + ArMx = t0 t1 δ0,1 (22a)
‖ <RM> ‖ ² -ArMx = t0 t-1δ0, -1 (22a)
δ0,1 ² <1, δ0, -1 ² <1 (23)
From these expressions, from the conditions (3), (9) and the expressions (22a), (22b), ‖ <rM> ‖ ² + ArMx and aδ0,1, ‖ <rM> ‖ ² The following equations (24a) to (25b) can be obtained by taking care that -arMx and aδ0, -1 have the same sign.
[0074]
[Expression 4]

When the expressions (24a) and (24b) are further modified, the following expressions (26) and (27) are obtained. ‖ <RM> ‖ ² = (A ² β + ² ) / (1 + β- ² (26)
rMx = (aβ + β −) / (1 + β− ² (27)
However, β + and β− are defined by the following equations (28a) and (28b).
β + = (α1 + α-1) / 2 (28a)
β-= (α1−α-1) / 2 (28b)
If the conditions (3) and (9) are used by substituting the expressions (26) and (27) into the expressions (20), (21a), and (21b), t0, t1, and t-1 are expressed by the following expressions (29) ) (30a) and (30b).
[0075]
[Equation 5]

Using these, t2, t-2, <1M> and RM are obtained as follows.
[0076]
[Formula 6]

5). How to find C
Solve the above equation (13) under the above condition (14). When equation (13) is transformed, the following (31) and (32) are obtained.
C ^-1 T1 ^-1 T3 C = P1 P3 ^-1 ... (31)
C ^-1 T2 ^-1 T3 C = P2 P3 ^-1 ... (32)
Here, replacement is performed according to the following equations (33) to (37).
[0077]
[Expression 7]

Then, equations (31) and (32) are decomposed into the following equations (38) to (41).
RC ^-1 R1 RC = R2 (38)
RC ^-1 R3 RC = R4 (39)
(I-R1) <lC> = <l1> -RC <l2> (40)
(I-R3) <lC> = <l3> -RC <l4> (41)
Therefore, the goal is to find RC and <LC>. Here, from the forms of equations (38) and (39), it can be seen that the similarity R1 to R2 and R3 to R4 between the rotation matrices and the similarity transformation matrix expressing their similarity are both Rc.
Therefore, the following equations (42) and (43) are obtained by setting the vector representing the rotation direction of Ri (i = 1,2,3,4) as <vi> (i = 1,2,3,4). Here, <vi> (i = 1, 2, 3, 4) can be uniquely determined from the condition (14) and the definition expressions (34) and (36).
<V1> = RC <v2> (42)
<V3> = RC <v4> (43)
Further, if the relationship of the following equation (44) is used, RC can be obtained.
(<V1>, <v3>, <v1> × <v3>)
= RC (<v2>, <v4>, <v2> × <v4>) (44)
For equations (40) and (41), group them together in the form of
If the square method is applied, <lC> is obtained.
[0078]
[Equation 8]

6). Outline of processing flow of movement control by method B
Finally, 1-5. The outline of the processing flow for the preparatory work and the autonomous approach operation in the method B will be described on the premise of the explanation items. First, the supplementary work will be supplemented as follows.
(1) Prior to the start of work, as described in 3 above, the robot is sequentially moved to the three positions CB1 to CB3.
(2) At each position CB1 to CB3, an image of the mark coordinate system 15a on the representative workpiece 14 is acquired, and data representing the line-of-sight direction of each dot center is acquired by image processing in the image processing apparatus 20.
(3) 5. The processing according to the algorithm described in the above is performed in the image processing apparatus 20, and data of the camera coordinate system C (data of RC, <LC>) is calculated and stored.
[0079]
(4) When the camera calibration is completed, the robot is moved to the position Pa shown in FIG. 22, and the approach completion state for the representative workpiece 14 is displayed.
[0080]
(5) At each position CB1 to CB3, an image of the mark coordinate system 15a is acquired, and data representing the line-of-sight direction of each dot center is acquired by image processing in the image processing apparatus 20.
(6) The above 1.2.4. The processing according to the algorithm described in the above is performed in the image processing apparatus 20, and approach completion state expression data (Mg data) is calculated and stored.
[0081]
As shown in the flowchart of FIG. 25, the autonomous approach operation is executed by a processing cycle including the following steps.
[K1] <d0>, <d1>, <d-1>, <d2>, <d-2> are obtained by image processing, and M0 is calculated.
[K2] The stored Mg data is compared with M0 calculated from <d0>, <d1>, <d-1>, <d2>, <d-2> obtained in step K1. Match / mismatch is determined. If they match, it means that the approach is complete, and the process proceeds to step K6. If they do not match, it means that the approach completion state has not been achieved, and the process returns to step K1. Various algorithms can be used to evaluate the degree of coincidence between Mg and M0. For example, the determination index Δ is calculated by the following equation (46). If Δ <ε (ε is a sufficiently small positive value), the approach is completed, and if Δ ≧ ε, the approach is not completed.
[0082]
[Equation 9]

[K3] A 4-by-4 matrix T0 representing the position and orientation of the robot's hand (the origin of the coordinate system set on the flange) is calculated and stored.
[K4] The right side of (2) is calculated using the data (C, Mg data) obtained in the preparation work and the algorithm described above to obtain the orthogonal movement target position Tg.
[0083]
[K5] A movement command for moving to the orthogonal movement target position Tg is created and transferred to the servo to move the robot.
[0084]
If the system is perfect without error, the approach will be completed in one cycle of steps K1 to K5, but in reality, the approach is completed due to visual sensor errors, calculation errors, etc. The state is achieved in successive approximations.
[K6] The robot is stopped and the process is terminated.
[0085]
As can be understood from the description of the embodiments of the A method and the B method. According to the present invention, the robot's approach operation is autonomously performed so as to obtain the position and posture of the approach completion state input in advance, so that work efficiency is improved.
[0086]
In the present embodiment, an example in which the workpiece 14 is conveyed by the hand 12 has been shown, but it goes without saying that the present invention can be applied to welding and other operations.
[0087]
Further, in this embodiment, the captured image of the camera 13 is used as an approach completion state expression data input method. However, if it is known in advance from the work experience so far, etc., it is determined by the key operation of the teaching operation 40. You may make it input.
[0088]
Furthermore, in the present embodiment, the time when the work 14 enters the field of view of the camera 13 is set as the timing for starting the approach operation autonomously, but the approach is based on other parameters such as the distance between the work 14 and the tool 12. You may determine the timing which starts operation | movement.
[0089]
【The invention's effect】
In the present invention, the approach completion state expression data is input in advance in the form of data that describes the relative positional relationship between the robot hand (that is, the camera fixed thereto) and the target. If movement control that realizes the relative positional relationship between the camera and the target corresponding to the approach completion state expression data is performed, the approach completion state can be changed to an arbitrary work without burdening the operator by an autonomous approach operation. Is achieved.
[0090]
Therefore, it is possible to easily realize the efficiency of various robot operations for a large number of non-positioning workpieces. In addition, since automatic transition from jog feed to autonomous approach operation is possible, further improvement in the convenience of the robot can be expected.
[Brief description of the drawings]
FIG. 1 Robot movement control of the present invention Method It is a figure which illustrates notionally using the example of a handling robot.
FIG. 2 shows movement control of the robot according to the present embodiment. Method FIG. 2 is a configuration diagram of a main part of a robot control system for executing the process according to method A.
FIG. 3 is a movement control of the robot according to the embodiment. Method It is a principal part block diagram of the robot control system for performing this according to the system B.
FIG. 4 is a principal block diagram showing a hardware configuration of a robot control apparatus used in the present embodiment.
FIG. 5 is a diagram showing a schematic configuration of a panel surface of a teaching operation panel used in the present embodiment.
FIG. 6 is a diagram for explaining the operation of the robot when the method A is adopted.
FIG. 7 is a diagram for explaining the operation of the robot when the method B is adopted.
[Fig. 8] Fruit It is a flowchart which shows the outline | summary of the operation procedure of a teaching operation panel for the robot movement control in embodiment, and a process.
FIG. 9 is a diagram illustrating conditions in the camera optical axis direction in an application to which method A is applied.
FIG. 10 is a diagram illustrating an outline of an application to which a method A is applied.
FIG. 11 is a first half of a flow explanatory diagram for explaining an outline of processing of an approach operation in an application to which a method A is applied.
FIG. 12 is the latter half of the flow explanatory diagram for explaining the outline of the approach operation processing in the application to which the method A is applied.
13 is a part of a flowchart (steps H1 to H7) for explaining an algorithm in accordance with the schematic flow shown in FIGS. 11 and 12. FIG.
FIG. 14 is a part of a flowchart (steps H8 to H19) for explaining an algorithm in accordance with the schematic flow shown in FIGS. 11 and 12;
15 is a part of a flowchart (steps H20 to H29) for explaining an algorithm in accordance with the schematic flow shown in FIGS. 11 and 12. FIG.
16 is a related explanatory diagram of the flowcharts shown in FIGS. 13 to 15 and represents vectors <a0>, <b0>, <a1>, <b1>. FIG.
FIG. 17 is a related explanatory diagram of the flowcharts shown in FIGS. 13 to 15 and shows a camera coordinate system and a vector <z>.
FIG. 18 is a related explanatory diagram of the flowcharts shown in FIGS. 13 to 15 and shows the relationship on the screen between the vectors <e> and <f> before and after the robot moves.
FIG. 19 is a related explanatory diagram of the flowcharts shown in FIGS. 13 to 15 and shows a state in which □ approaches x on the screen as the robot moves forward and backward.
FIG. 20 is a related explanatory diagram of the flowcharts shown in FIGS. 13 to 15 and shows a screen when the approach completion state is achieved.
FIG. 21 is a diagram illustrating an outline of an algorithm necessary for an autonomous approach operation in the embodiment of the B method.
FIG. 22 is a diagram for explaining an autonomous approach operation in the embodiment of the B method.
FIG. 23 is a diagram illustrating a mark coordinate system used in an embodiment of a B method.
FIG. 24 is a diagram for describing a vector describing a line-of-sight direction when a camera looks at a mark coordinate system in an embodiment of a B system.
FIG. 25 is a flowchart showing an outline of processing of an autonomous approach operation in the embodiment of the B method.
[Explanation of symbols]
1a Approach completion state input means
1b Relative position recognition means
2,10 robot
2a Tool (hand)
3,14 work
4 memory
5 Jog feed button
6 axis movement control means
11 Arm
12 tools
13 Camera
13a Camera coordinate system
14 Work
14a Features
15 mark
15a Mark coordinate system
20 Image processing device
30 Robot controller
31 Processor board
31a processor
31c RAM
31b ROM
35 Digital I / O
36a CRT
36b Operation panel
37 Analog I / O
39 Bus
40 Teaching operation panel
41 Display screen
42 function keys
44 Emergency stop button
45 cursor keys
46 Numeric keypad
47 (J1-J6) Jog feed button
51-56 Servo motor
58 RS232C equipment
CB1-3 Positions for camera calibration
D0, D1, D-1, D2, D-2 Mark coordinate system dots

Claims

In a robot movement control method for causing a robot to perform an approach operation with respect to an object having a variation in an existing position using a control means including a visual sensor provided with a camera means and an image processing means and a robot control device,
And approaches completion status teachings step of storing the approach completion state representation data representing approaches completion status for Target thereof to said control means,
An approach operation execution stage for causing the robot to perform an approach operation by the control means,
The approach action execution stage includes an autonomous approach action execution stage in which the approach action is autonomously performed based on the approach completion state expression data,
The approach completion status representation data is for specifying the relative positional relationship between the hand and the Target of the robot,
Movement target position of the robot in the autonomous approach operation execution phase, by software processing within the control unit is defined as the position at which the relative positional relationship specified by the approach completion state representation data is achieved, the method.

The approach completion state representation data stored approach completion state teaching phase is created an object that represents the Target material in said control means based on an image obtained by photographing by the camera means, wherein The robot movement control method according to Item 1.

The approach completion state expression data stored in the approach completion state teaching step includes data representing a feature portion and a gravity center position of the object representative object on an image. Robot movement control method .

Mark coordinate system can be recognized by the visual sensor on the surface of the Target object and the object representative object is installed,
The robot according to claim 1 or 2, wherein the approach completion state expression data stored in the approach completion state teaching stage includes data representing a position / attitude of the mark coordinate system on an image. Movement control method .

The approach operation execution stage includes a jog feed stage prior to the autonomous approach operation execution stage,
The robot movement control method according to any one of claims 1 to 4, wherein a transition from the jog feed stage to the autonomous approach operation execution stage is automatically performed by the control means.

Software process in the autonomous approach operation execution phase, the data representing the relative positional relationship between the robot hand and Target was recognized by the visual sensor in the time, and approaches completion state representation data the taught , A process for controlling movement of each axis of the robot based on the comparison result, and a process for determining completion / non-completion of the approach completion state are sequentially performed until the completion of the approach completion state is determined. The robot movement control method according to claim 1, wherein the robot movement control method repeats automatically.