JP3998741B2 - ロボットの移動制御方法 - Google Patents
ロボットの移動制御方法 Download PDFInfo
- Publication number
- JP3998741B2 JP3998741B2 JP19855296A JP19855296A JP3998741B2 JP 3998741 B2 JP3998741 B2 JP 3998741B2 JP 19855296 A JP19855296 A JP 19855296A JP 19855296 A JP19855296 A JP 19855296A JP 3998741 B2 JP3998741 B2 JP 3998741B2
- Authority
- JP
- Japan
- Prior art keywords
- approach
- robot
- completion state
- camera
- coordinate system
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Control Of Position Or Direction (AREA)
- Manipulator (AREA)
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 、カメラ座標系とマーク座標系の現在相対位置（アプローチ動作中の各時点における相対位置）Ｍ0 と、アプローチ完了状態におけるフランジ位置Ｔg 、アプローチ完了状態におけるカメラ座標系とマーク座標系の相対位置Ｍg との幾何学的な関係は、図２２中に記されているようなものである。なお、Ｍg は予め計算して記憶され、Ｃはキャリブレーションで教示される。これは方式Ａの場合と同様である。
【００６０】
これらＴ0 ，Ｍ0 ，Ｔg ，Ｍg の間の関係を規定する基本方程式は下記のようになる。
Ｔ0 ＣＭ0 ＝Ｔg ＣＭg ・・・（１）
従って、これをＴg について解いた次式（２）がロボットの直交座標系上での移動目標位置を表わす基本式となる。
Ｔg ＝Ｔ0 ＣＭ0 Ｍg-1 Ｃ-1 ・・・（２）
このＴg を最終的な移動目標点とする位置指令をサーボに与えれば、ロボットをアプローチ完了状態へ向けて自律的に移動させることが出来る。故に、自律アプローチ動作のアルゴリズムを定めることは、（２）式の右辺を具体的に求める問題に帰着する。
【００６１】
（２）式の右辺中、Ｔ0 はロボットの現在位置データを表わしており、Ｃのデータは適当なキャリブレーションで獲得され得る。そこで、Ｍ0 及びＭg の求め方から説明する。
【００６２】
２．Ｍ0 及びＭg の方程式
Ｍ0 ，Ｍg は、いずれもカメラ座標系から見たマーク座標系１５ａの位置・姿勢（現在及びアプローチ完了時）を表わしている。本実施形態で用いるマーク座標系１５ａは、図２３に示されている。マーク座標系１５ａは間隔ａを以て格子状に配列された５個の円形ドットＤ0 ，Ｄ1 ，Ｄ-1，Ｄ2 ，Ｄ-2で構成されている。
【００６３】
図示した通り、ドット間隔ａは正の一定値とする。
ａ＞０ ・・・（３）
各ドットの中心位置は、Ｄ0 （０，０，０），Ｄ1 （ａ，０，０），Ｄ-1（−ａ，０，０），Ｄ2 （ａ，ａ，０），Ｄ-2（−ａ，ａ，０）である。なお、３次元直交座標系を表現出来るものであれば、他のパターンでマーク座標系１５ａを構成しても良い。
【００６４】
Ｍ0 とＭg は、３次元直交座標系間の位置・姿勢の関係を表現する４×４の同次変換行列であり、次式（４）のように置くことが出来る。
【００６５】
【数１】
ここで、ＲM は回転を表わす３×３行列、＜ｌM ＞は位置を表わす３×１行列（ベクトル）である。以下、Ｍ0 とＭ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 ＞‖2 ＝ｔ02 ・・・（20）
‖＜ｒM ＞‖2 ＋２ａｒMx＋ａ2 ＝ｔ12 ・・・（21ａ）
‖＜ｒM ＞‖2 −２ａｒMx＋ａ2 ＝ｔ-12 ・・・（21ｂ）
‖＜ｒM ＞‖2 ＋ａｒMx＝ｔ0 ｔ1 δ0,1 ・・・（22ａ）
‖＜ｒM ＞‖2 −ａｒMx＝ｔ0 ｔ-1δ0,-1 ・・・（22ａ）
δ0,12＜１，δ0,-12 ＜１ ・・・（23）
これらの式から条件（３），（９）と式（22ａ），（22ｂ）から、‖＜ｒM ＞‖2 ＋ａｒMxとａδ0,1、‖＜ｒM ＞‖2 −ａｒMxとａδ0,-1 とが各々同符号になる事に注意して整理すれば、次式（24ａ）〜（25ｂ）が得られる。
【００７４】
【数４】
式（24ａ），（24ｂ）は更に変形すると、次式（26），（27）が得られる。 ‖＜ｒM ＞‖2 ＝（ａ2β+2）／（１＋β-2） ・・・（26）
ｒMx＝（ａβ+ β- ）／（１＋β-2） ・・・（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 マーク座標系のドット
【発明の属する技術分野】
本発明はロボットの移動制御方法に関し、更に詳しく言えば、視覚センサを用いて取得されたデータに基づいてロボットを目的の位置に移動させるための移動制御方法に関する。本発明の移動制御方法は、特にジョグ送り形式の移動に有利に適用される。
【０００２】
【従来の技術】
従来より知られているロボットの移動制御方法の一つにジョグ送りがある。これは、オペレータが教示操作盤のジョグ送りボタンを操作してロボットを目的とする位置へ動かすようにした方法である。ジョグ送りは、例えばアームの先端に取り付けられたハンドでワークを持ち上げて所定の場所に積み上げる作業を行うというような作業に利用される。
【０００３】
その場合、オペレータは、目視によってワークとハンドとの位置関係を確認しながら複数個のジョグ送りボタンの選択的操作によってロボットを徐々に移動させ、ＴＣＰ（ツール・センタ・ポイント）を目標とするアプローチ点に到達させていた。そして、このアプローチ動作が完了したら、ハンド操作用のボタンを操作してワークをつかむようにしていた。
【０００４】
【発明が解決しようとする課題】
しかし、ロボットをジョグ送りによって正確なアプローチ位置に到達させるには多くの時間と高度の熟練を要することが一般的である。即ち、ジョグ送りで移動可能なロボットの移動方向は限られており（例えば、ＸＹＺ各座標軸方向に沿った並進と座標軸回り回転並びに各軸送りの場合のＪ１軸〜Ｊ６軸の移動）、単純なジョグ送りボタン操作で一気に正確なアプローチを完了させるというようなことは不可能といって良かった。
【０００５】
本発明はこのような状況に鑑みてなされたものであり、目標位置への自律的なアプローチを可能にして、作業効率を向上させることのできるロボットの移動制御方法を提供することを目的とする。
【０００６】
【課題を解決するための手段】
本発明は、上記課題を解決するために、ロボットが自律的なアプローチ動作を存在位置にばらつきのある対象物に対して行えるようなロボットの移動制御方法を提供するものである。本発明の方法では、カメラ手段及び画像処理手段を備えた視覚センサとロボット制御装置とを含む制御手段が、利用される。
【０００７】
本発明の方法の諸段階には、対象物に対するアプローチ完了状態を表現するアプローチ完了状態表現データを制御手段に記憶させるアプローチ完了状態教示段階と、制御手段によってロボットにアプローチ動作を行わせるアプローチ動作実行段階とが含まれ、更に、アプローチ動作実行段階には、該アプローチ動作がアプローチ完了状態表現データに基づいて自律的に行なわれる自律アプローチ動作実行段階が含まれる。
【０００８】
教示段階でアプローチ完了状態表現データは、ロボットの手先と対象物の相対的な位置関係を特定するものであり、自律アプローチ動作実行段階におけるロボットの移動目標位置は、制御手段内のソフトウェア処理によって、アプローチ完了状態表現データによって特定される対象物−ロボット手先間の相対的位置関係が実現される位置として定められる。
【０００９】
アプローチ完了状態教示段階で記憶されるアプローチ完了状態表現データは、対象物を代表する物体をカメラ手段によって撮影することによって取得された画像に基づいて制御手段内で作成することが出来る。
一つの好ましい形態において、アプローチ完了状態表現データは、画像上における対象物代表物体の特徴部と重心位置を表わすデータとを含むように教示される。
【００１０】
また、別の好ましい形態においては。対象物及び対象物代表物体の表面上に前記視覚センサによって認識可能なマーク座標系が設置され、アプローチ完了状態表現データは、画像上におけるマーク座標系の位置・姿勢を表わすデータとを含むように教示される。
【００１１】
自律アプローチ動作実行段階に先だって、予備的なアプローチ動作を行なうためにジョグ送りを行なうようにしても良い。その際、ジョグ送り段階から自律アプローチ動作実行段階への移行は、制御手段によって自動的に行なわれることが好ましい。
【００１２】
また、自律アプローチ動作実行段階におけるソフトウェア処理は、当該時点において視覚センサによって認識されるロボットの手先と対象物の相対的な位置関係を表わすデータと、教示されたアプローチ完了状態表現データとを比較する処理と、比較結果に基づいてロボットの各軸を移動制御するための処理と、アプローチ完了状態の完了／未完了を判定する処理を、アプローチ完了状態の完了が判定されるまで逐次的に繰り返すものとすることが出来る。
【００１３】
本発明の最も重要な特徴は、アプローチ完了状態表現データがロボット手先（即ち、それに固定されたカメラ）と目標物の相対的な位置関係を記述するデータの形態で予め入力されており、従って、アプローチ動作時に、このアプローチ完了状態表現データに対応するカメラと目標物の相対的な位置関係を実現するような移動制御を行なうことで、自律的なアプローチ動作が実現される点にある。
【００１４】
この特徴により、オペレータに負担をかけることなくアプローチ完了状態が任意のワークに対して効率的に達成される。また、ジョグ送りから自律アプローチ動作への自動移行を行なえば、ロボットの至便利性を更に向上させることが出来る。
【００１５】
【発明の実施の形態】
図１は本発明のロボット移動制御方法をハンドリングロボットの例を用いて概念的に説明する図である。符号１ａはアプローチ完了状態入力手段を表わし、アプローチ対象を代表するワーク３に対するロボット２のアプローチ動作が完了した時のワーク３とハンド２ａの相対的な位置関係（姿勢を含む。）を表現するためのデータ（以下、「アプローチ完了状態表現データ」と言う。）を予めメモリ４に入力しておく機能を有している。なお、ハンド２ａの位置がロボット手先位置（例えばＴＣＰまたは最終アーム先端のフランジ上に設定された座標系の原点）で代表され得ることは言うまでもない。
【００１６】
実際の作業時にあたっては、オペレータがジョグ送りボタン５（一般には、アプローチ動作マニュアル指令手段）を操作し、軸移動制御手段６にロボット２の各軸を制御させ、アプローチ動作を実行させる。アプローチ動作のための移動制御は、アプローチ完了状態入力手段１ａによって入力されたアプローチ完了状態表現データと、メモリ４にワーク３とハンド２ａ（ロボット２の手先）の相対的な位置関係を認識する相対位置認識手段１ｂによって取得される相対位置認識データとに基づいて実行される。
【００１７】
アプローチ動作は、ワーク３とハンド２ａ（ロボット２の手先）の相対的な位置関係が、アプローチ完了状態表現データの表わすものと一致した時点で完了する。なお、実際の作業時に対象とされる個々のワーク３は、正確な位置決めがなされていなくて良いことに注意。また、以下に述べるように、アプローチ完了状態入力手段１ａと相対位置認識手段１ｂは２次元視覚センサを利用する形で具体化される。
【００１８】
図２および図３は本実施形態のロボットの移動制御方法を実施するためのロボット制御システムの要部構成図である。ロボット１０はハンドリングロボットあるいは他種のロボットであり、アーム１１の先端には、ツール（エンドエフェクタ）１２が取り付けられている。ここでは、ツール１２としてハンドが示されている。ロボット１０は、ロボット制御装置３０によって動作制御され、ワーク１４を把持して搬送する。なお、ツール１２の種類及びアプローチ後の作業内容はアプリケーションによることは言うまでもない（例えば、ツール１２が加工ツールであれば、加工を行なう）。
【００１９】
また、ロボット１０のアーム１１には、ツール１２と一体に移動できるようにカメラ１３が取り付けられている。カメラ１３は、画像処理装置２０と接続されており、その撮影画像を画像処理装置２０に送る。
【００２０】
画像処理装置２０は、ＣＰＵ、フレームメモリ、画像処理プロセッサ、プログラムメモリ、データメモリ等を備えた周知の構成と機能を有するもので、カメラ１３からの撮影画像の記憶・処理を行い、その結果得られるデータをロボット制御装置３０に送る。ここでは、画像処理方式に関連して２通りの形態を採用する。
〔方式Ａ〕 図２のように、カメラ１３からの撮影画像を輪郭処理してワーク画像の重心位置を求める。また、ワーク１４の特徴部１４ａ等を抽出して、重心と特徴部との位置関係等のデータを求める。このデータを、代表ワークに対するアプローチが実現されている状態で得られた画像に基づいて作成すれば、それがアプローチ完了状態表現データとして利用出来る。
〔方式Ｂ〕 図３のように、座標系（マーク座標系１５ａ）を定義できるようなマーク１５をワーク１４に予め用意しておき、カメラ１３による撮影画像の処理により、カメラ座標系１３ａに関するマーク座標系１５ａの相対的位置姿勢のデータを求める。方式Ａの場合と同じく、このデータを、代表ワークに対するアプローチが実現されている状態で得られた画像に基づいて作成すれば、それがアプローチ完了状態表現データとして利用出来る。
【００２１】
ロボット１０のアプローチ動作の指示は、ロボット制御装置３０に接続された教示操作盤４０によって行われる。教示操作盤４０は、後述するように、液晶ディスプレイや各種のキーを備えている。また、オペレータによって持ち運びできる程度の大きさであることが好ましい。ロボット制御装置３０は、方式Ａあるいは方式Ｂに従った画像処理を行なう画像処理装置２０および教示操作盤４０からのマニュアル指令入力に従って、アプローチ完了状態表現データが表わすアプローチ完了状態を実現すべく移動制御を行う。
【００２２】
図４はロボット制御装置３０のハードウェアの構成を示す要部ブロック図である。ロボット制御装置３０には、バス３９に結合されたプロセッサボード３１が装備され、このプロセッサボード３１には、プロセッサ３１ａ、ＲＯＭ３１ｂ、ＲＡＭ３１ｃが設けられている。プロセッサ３１ａは、ＲＯＭ３１ｂに格納されたシステムプログラムに従って、ロボット制御装置３０全体を制御する。ＲＡＭ３１ｃには、画像処理装置から送られるアプローチ完了状態表現データが格納される。
【００２３】
ディジタルサーボ制御回路３２はバス３９に結合され、プロセッサボード３１からの指令によって、サーボアンプ３３を経由して、サーボモータ５１，５２，５３，５４，５５および５６を駆動する。これらのサーボモータはロボット１０の本体機構部に内蔵され、ロボット１０の各軸を駆動する。
【００２４】
バス３９には更にシリアルポート３４が結合されており、液晶ディスプレイ付きの教示操作盤４０、その他のＲＳ２３２Ｃ機器５８と接続されている。ＲＳ２３２Ｃ機器５８には、画像処理装置２０も含まれる。教示操作盤４０はオペレータにより持ち運びができる程度の大きさと重さを有し、そのパネル上にはアプローチ動作マニュアル指令入力手段として使用されるジョグ送りボタン等が設けられている。また、シリアルポートには、オプションでＣＲＴ３６ａが接続可能であり、ＣＲＴ３６ａの位置から作業状態等が確認できる。
【００２５】
ディジタルＩ／Ｏ３５には、操作パネル３６ｂが接続されており、ロボット制御装置３０の位置でロボット１０の動作指令を行うことができる。また、ディジタルＩ／Ｏ３５には、ツール１２の把持駆動部が接続されており、その動作指令を行うことができる。アナログＩ／Ｏ３７には、例えばツール１２としてレーザ溶接用の加工ヘッドが使用された場合に、その溶接電源装置が接続され、このアナログＩ／Ｏ３７を介して溶接電圧の指令が行われる。また、大容量メモリ３８には、ティーチングデータ等が格納される。
【００２６】
図５は教示操作盤４０のパネル面の概略構成を示す図である。表示画面４１は例えば液晶画面であり、移動指令プログラムの詳細データ等が切替え表示される。ファンクションキー４２は、表示画面４１の下端部に表示されるメニューを選択するキーである。教示操作盤有効スイッチ４３は、教示操作盤４０の操作が有効か無効かを切り換えるスイッチである。
【００２７】
非常停止ボタン４４は、ロボット１０の動作を非常停止させるボタンである。カーソルキー４５は、表示画面４１上で表示されるカーソルの移動を行うキーである。テンキー部４６には、数字キーやその他のキーが設けられており、数値および文字の入力、削除等を行うことができる。
【００２８】
一連のジョグ送りボタン４７（Ｊ１〜Ｊ６）は、従来方式のジョグ送りを行なう通常モードでは、並進／回転方向及び＋−方向を指定して移動指令の入力を行うボタンであるが、本実施形態（自律アプローチモード）では、後述するように、アプローチ動作マニュアル指令入力手段として使用する。
【００２９】
次に、本実施形態におけるアプローチ動作の概要について説明する。
図６および図７は、前述の方式Ａ及び方式Ｂを採用した各々の場合について、ロボットの動作を説明する図である。ここでは、ツール１２はハンドであり、ハンド１２によってロボットの動作範囲内に順次供給されるワーク１４を把持して所定位置に配置されたパレット（図示省略）まで搬送するパレタイジング作業を考える。順次供給されるワーク１４は、正確に位置決めされておらず、供給位置には相当のバラツキがあって良い。
【００３０】
［１］準備
１．作業開始に先だって、アプローチ完了状態データの取得作業を行なう。この作業は、代表ワーク１４を適当な位置（実際の作業時の供給位置を代表すると思われる範囲内であれば十分である。）に置き、通常モード（従来方式）のジョグ操作によってロボットを移動させ、アプローチ完了状態を実現させる。この作業には正確を期す必要があるが、このような作業はワークの種別が変わらない限り１回きりで良いことは本発明の重要な特徴である（従来方式では、ワーク毎にジョグ送りのやり直しが必要）。
【００３１】
２．代表ワーク１４に対してアプローチ完了状態を実現したら、ロボット制御装置３０を介してカメラ１３による撮影を行なわせ、アプローチ完了状態表現データを作成するための画像を取得する。
３．取得された画像は画像処理装置２０内で画像処理され、前述の方式Ａ，Ｂに対応して次のようなデータが作成され、アプローチ完了状態表現データとしてＲＡＭ３１ｃに格納される。
【００３２】
〔方式Ａ〕；代表ワーク１４の重心Ｐｇおよび特徴部１４ａの位置関係、ＴＣＰのアプローチ達成時の位置Ｐｃ＝Ｐａと重心Ｐｇとの間の距離Ｌ１等の位置姿勢データ。なお、ここで「重心」とは実際には画像として認識された代表ワーク１４の重心、即ち「画像重心」のことである。
〔方式Ｂ〕 カメラ座標系を基準としたマーク座標系１５ａの相対的な位置姿勢を表わすデータ（詳細は後述）。なお、方式Ｂを採用する場合には、実際の作業で扱われる全ワークに対して、ワーク上の同じ位置に同じ姿勢で代表ワーク１４に用意されたのと同じマーク座標系１５ａが用意されていることが前提となる事に注意する必要がある。
【００３３】
［２］実際の作業
１．アプローチ完了表現データ入力後、新しく供給されたワーク１４に対して実際の作業を開始する。オペレータは、ジョグ送りボタン４７（図５）を押してロボットの各軸を移動制御し、ツール１２の位置を代表するＴＣＰを開始点（Ｐｃ＝Ｐｓ）からワーク１４に向けてジョグ送りで移動させる。この間、カメラ１３と画像処理装置２０を含む２次元視覚センサも動作している。
【００３４】
２．ツール１２がワーク１４に接近してカメラ１３の視野にワーク１４が入ると、画像処理装置２０がそれを感知し、ロボット制御装置３０は通常モードから自律アプローチモードに切り換わり、自律アプローチ動作が開始される。なお、最初からカメラ１３の視野にワーク１４が入っている場合には、通常モードから自律アプローチモードへの切り換えはジョグボタン押下直後に起る。
３．自律アプローチ動作は、アプローチ完了状態の実現をロボット制御装置３０が確認しない限り、ジョグ送りボタン４７の押下によって何度でも繰り返し続行される。アプローチ動作の最終的な移動目標位置は、予め入力されているアプローチ完了状態表現データの表わしている位置である。
４．アプローチ動作開始後に、Ａ方式あるいはＢ方式で作成されたアプローチ完了状態表現データと等価なデータが得られるような位置にロボットのＴＣＰが到達した時点で、アプローチ動作が完了する。アプローチが完了すると、ロボットは停止するとともに、ロボット制御装置３０は自律アプローチモードから通常モードに自律的に復帰する。
５．停止後、オペレータは、必要に応じてハンド操作とジョグ送り等を組み合わせてワーク１４を把持し、所定位置へ搬送し、ワーク１４を解放する。
以上の手順を次回以降の各ワーク１４に対しても繰り返すことで多数のワーク１４に対するパレタイジング作業を効率的に行なうことが出来る。
［３］処理の概略
図８はこのようなロボット移動制御のための教示操作盤の操作手順と処理の大要を示すフローチャートである。各ステップの処理を簡単に記すと次のようになる。
〔Ｓ１〕ジョグ送りボタン４７が押されてジョグ送りの指令がなされたか否かを判断し、なされればステップＳ２に進み、なされなければステップＳ１を繰り返す。
〔Ｓ２〕ジョグ送りボタン４７で指定された移動内容（方向、軸等）に応じて、所定の軸を制御して、ロボット１０のジョグ送り移動を開始する。例えば、Ｊ６軸＋方向の移動が指定されていれば、Ｊ６軸を＋方向へ進める移動指令を作成してサーボに渡す。
〔Ｓ３〕ジョグ送りボタン４７がオフにされたか否かを判断し、オフにされればステップＳ８に進み、されなければステップＳ４に進む。
〔Ｓ４〕画像処理装置２０から、カメラ１３の視野内にワーク１４が入ったことを知らせる信号の受信の有無を確認し、受信していればステップＳ５に進み、受信していなければステップＳ３に戻る。
〔Ｓ５〕アプローチ動作の処理を開始する。具体的な処理内容の例については、Ａ方式、Ｂ方式に分けて後述する。
〔Ｓ６〕ジョグ送りボタン４７がオフにされたか否かを判断し、オフにされていればステップＳ８に進み、されていなければステップＳ７に進む。
〔Ｓ７〕アプローチが完了したか否かを判断し、完了していればステップＳ８に進み、完了しなければステップＳ６に戻る。なお、アプローチ完了の判断方式についても、Ａ方式、Ｂ方式の具体例の中で述べる。
〔Ｓ８〕ロボット１０の各軸の動作を停止させる。
【００３５】
［４］方式Ａにおける自律アプローチ動作時の処理
本発明を現実のアプリケーションに適用する場合、図９に示したように、ロボットの手先に搭載されたカメラ１３の光軸方向Ｃを固定した条件で自律アプローチのためのロボット動作を行えるようにすることが実際的である。そこで、ここでは最も現実的なアプリケーションとして、図１０に示したような事例を想定する。
図１０に示したように、ワーク１４は一つのワーク供給面ＰＬ１上に供給される。ワーク供給面ＰＬ１は、ワーク座標系Σ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 、カメラ座標系とマーク座標系の現在相対位置（アプローチ動作中の各時点における相対位置）Ｍ0 と、アプローチ完了状態におけるフランジ位置Ｔg 、アプローチ完了状態におけるカメラ座標系とマーク座標系の相対位置Ｍg との幾何学的な関係は、図２２中に記されているようなものである。なお、Ｍg は予め計算して記憶され、Ｃはキャリブレーションで教示される。これは方式Ａの場合と同様である。
【００６０】
これらＴ0 ，Ｍ0 ，Ｔg ，Ｍg の間の関係を規定する基本方程式は下記のようになる。
Ｔ0 ＣＭ0 ＝Ｔg ＣＭg ・・・（１）
従って、これをＴg について解いた次式（２）がロボットの直交座標系上での移動目標位置を表わす基本式となる。
Ｔg ＝Ｔ0 ＣＭ0 Ｍg-1 Ｃ-1 ・・・（２）
このＴg を最終的な移動目標点とする位置指令をサーボに与えれば、ロボットをアプローチ完了状態へ向けて自律的に移動させることが出来る。故に、自律アプローチ動作のアルゴリズムを定めることは、（２）式の右辺を具体的に求める問題に帰着する。
【００６１】
（２）式の右辺中、Ｔ0 はロボットの現在位置データを表わしており、Ｃのデータは適当なキャリブレーションで獲得され得る。そこで、Ｍ0 及びＭg の求め方から説明する。
【００６２】
２．Ｍ0 及びＭg の方程式
Ｍ0 ，Ｍg は、いずれもカメラ座標系から見たマーク座標系１５ａの位置・姿勢（現在及びアプローチ完了時）を表わしている。本実施形態で用いるマーク座標系１５ａは、図２３に示されている。マーク座標系１５ａは間隔ａを以て格子状に配列された５個の円形ドットＤ0 ，Ｄ1 ，Ｄ-1，Ｄ2 ，Ｄ-2で構成されている。
【００６３】
図示した通り、ドット間隔ａは正の一定値とする。
ａ＞０ ・・・（３）
各ドットの中心位置は、Ｄ0 （０，０，０），Ｄ1 （ａ，０，０），Ｄ-1（−ａ，０，０），Ｄ2 （ａ，ａ，０），Ｄ-2（−ａ，ａ，０）である。なお、３次元直交座標系を表現出来るものであれば、他のパターンでマーク座標系１５ａを構成しても良い。
【００６４】
Ｍ0 とＭg は、３次元直交座標系間の位置・姿勢の関係を表現する４×４の同次変換行列であり、次式（４）のように置くことが出来る。
【００６５】
【数１】
【００６６】
【数２】
【００６７】
これらベクトルの内積について、次式（７）で定義されるδ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）の置き換えを行う。
【００７２】
【数３】
＜ｒM ＞＝ｔ0 ＲM-1 ＜ｄ0 ＞＝＜ｌM ＞ ・・・（16）
＜ｒM ＞＋ａ＜ｅ1 ＞＝ｔ1 ＲM-1 ＜ｄ1 ＞ ・・・（17）
＜ｒM ＞−ａ＜ｅ1 ＞＝ｔ-1ＲM-1 ＜ｄ-1＞ ・・・（18）
ここで、ＲM-1 が回転を表わす行列であることに注意すると、次式（19）が成立する。
【００７３】
‖＜ｒM ＞‖2 ＝ｔ02 ・・・（20）
‖＜ｒM ＞‖2 ＋２ａｒMx＋ａ2 ＝ｔ12 ・・・（21ａ）
‖＜ｒM ＞‖2 −２ａｒMx＋ａ2 ＝ｔ-12 ・・・（21ｂ）
‖＜ｒM ＞‖2 ＋ａｒMx＝ｔ0 ｔ1 δ0,1 ・・・（22ａ）
‖＜ｒM ＞‖2 −ａｒMx＝ｔ0 ｔ-1δ0,-1 ・・・（22ａ）
δ0,12＜１，δ0,-12 ＜１ ・・・（23）
これらの式から条件（３），（９）と式（22ａ），（22ｂ）から、‖＜ｒM ＞‖2 ＋ａｒMxとａδ0,1、‖＜ｒM ＞‖2 −ａｒMxとａδ0,-1 とが各々同符号になる事に注意して整理すれば、次式（24ａ）〜（25ｂ）が得られる。
【００７４】
【数４】
ｒMx＝（ａβ+ β- ）／（１＋β-2） ・・・（27）
但し、β+ ，β- は次式（28ａ），（28ｂ）で定義されるものとする。
β+ ＝（α1 ＋α-1）／２ ・・・（28ａ）
β- ＝（α1 −α-1）／２ ・・・（28ｂ）
そして、式（20）（21ａ），（21ｂ）に式（26）（27）を代入して条件（３），（９）を用いれば、ｔ0 ，ｔ1 ，及びｔ-1が次式（29）（30ａ），（30ｂ）で求まる。
【００７５】
【数５】
【００７６】
【数６】
前述の条件（14）の下で、前述の方程式（13）を解く。方程式（13）を変形すると、次の（31），（32）が得られる。
Ｃ-1Ｔ1-1Ｔ3 Ｃ＝Ｐ1 Ｐ3-1 ・・・（31）
Ｃ-1Ｔ2-1Ｔ3 Ｃ＝Ｐ2 Ｐ3-1 ・・・（32）
ここで次式（33）〜（37）に従った置き換えを行なう。
【００７７】
【数７】
Ｒ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）で判定指標Δを算出し、Δ＜ε（εは十分小なる正値）であればアプローチ完了、Δ≧εであればアプローチ未完了とすれば良い。
【００８２】
【数９】
〔Ｋ４〕準備作業で取得したデータ（Ｃ，Ｍg のデータ）と上記説明したアルゴリズムを用いて、（２）の右辺を計算し、直交移動目標位置Ｔg を求める。
【００８３】
〔Ｋ５〕直交移動目標位置Ｔg へ向かうための移動指令を作成してサーボへ渡し、ロボットを移動させる。
【００８４】
もし、システムが無誤差の完璧なものであれば、ステップＫ１〜Ｋ５の１サイクルでアプローチ完了状態に至る筈であるが、実際には視覚センサの誤差、計算の誤差等のために、アプローチ完了状態は、逐次近似的に達成される。
〔Ｋ６〕ロボットを停止させて処理を終了する。
【００８５】
以上Ａ方式、Ｂ方式の実施形態についての説明から理解されるように。本発明に従えば、予め入力されたアプローチ完了状態の位置および姿勢となるように自律的にロボットのアプローチ動作が行なわれるので、作業効率が向上する。
【００８６】
なお、本実施形態では、ハンド１２によりワーク１４を搬送する例を示したが、溶接その他の作業に本発明を適用することができることは言うまでもない。
【００８７】
また、アプローチ完了状態表現データの入力方法として、本実施形態ではカメラ１３の撮影画像を使用したが、それまでの作業経験等から予めそれが判っている場合には、教示操作４０のキー操作によって入力するようにしても良い。
【００８８】
さらに、本実施形態では、カメラ１３の視野内にワーク１４が入った時を自律的にアプローチ動作を開始するタイミングとしたが、ワーク１４とツール１２との距離等、他のパラメータに基づいてアプローチ動作を開始するタイミングを定めても良い。
【００８９】
【発明の効果】
本発明においては、アプローチ完了状態表現データがロボット手先（即ち、それに固定されたカメラ）と目標物の相対的な位置関係を記述するデータの形態で予め入力されているので、アプローチ動作時に、このアプローチ完了状態表現データに対応するカメラと目標物の相対的な位置関係を実現するような移動制御を行なえば、自律的なアプローチ動作によってオペレータに負担をかけることなくアプローチ完了状態が任意のワークに対して達成される。
【００９０】
従って、多数の非位置決めワークに対する各種ロボット作業の効率化を容易に実現出来る。また、ジョグ送りから自律アプローチ動作への自動移行を可能としたので、ロボットの至便性の更なる向上が期待出来る。
【図面の簡単な説明】
【図１】 本発明のロボット移動制御方法をハンドリングロボットの例を用いて概念的に説明する図である。
【図２】 本実施形態のロボットの移動制御方法を方式Ａに従って実行するするためのロボット制御システムの要部構成図である。
【図３】 本実施形態のロボットの移動制御方法を方式Ｂに従って実行するするためのロボット制御システムの要部構成図である。
【図４】 本実施形態で使用されるロボット制御装置のハードウェアの構成を示す要部ブロック図である。
【図５】 本実施形態で使用される教示操作盤のパネル面の概略構成を示す図である。
【図６】 方式Ａを採用した場合について、ロボットの動作を説明する図である。
【図７】 方式Ｂを採用した場合について、ロボットの動作を説明する図である。
【図８】 実施形態におけるロボット移動制御のための教示操作盤の操作手順と処理の大要を示すフローチャートである。
【図９】 方式Ａを適用するアプリケーションにおいてカメラ光軸方向の条件について説明する図である。
【図１０】 方式Ａを適用するアプリケーションの概略を説明する図である。
【図１１】 方式Ａを適用するアプリケーションにおけるアプローチ動作の処理について、概略を説明するためのフロー説明図の前半部分である。
【図１２】 方式Ａを適用するアプリケーションにおけるアプローチ動作の処理について、概略を説明するためのフロー説明図の後半部分である。
【図１３】 図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ１〜Ｈ７）である。
【図１４】 図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ８〜Ｈ１９）である。
【図１５】 図１１及び図１２に示した概略フローに即したアルゴリズムについて説明するフローチャートの一部（ステップＨ２０〜Ｈ２９）である。
【図１６】 図１３〜図１５に示したフローチャートの関連説明図で、ベクトル＜ａ0 ＞，＜ｂ0 ＞，＜ａ1 ＞，＜ｂ1 ＞を表わしている。
【図１７】 図１３〜図１５に示したフローチャートの関連説明図で、カメラ座標系とベクトル＜ｚ＞を表わしている。
【図１８】 図１３〜図１５に示したフローチャートの関連説明図で、ロボット移動前後のベクトル＜ｅ＞と＜ｆ＞の画面上での関係を表わしている。
【図１９】 図１３〜図１５に示したフローチャートの関連説明図で、ロボットの前進後退移動によって、画面上で□が×に接近する様子が示されている。
【図２０】 図１３〜図１５に示したフローチャートの関連説明図で、アプローチ完了状態が達成された時点における画面が描かれている。
【図２１】 Ｂ方式の実施形態について、自律アプローチ動作に必要なアルゴリズムの大要を説明する図である。
【図２２】 Ｂ方式の実施形態について、自律アプローチ動作を説明する図である。
【図２３】 Ｂ方式の実施形態で使用されるマーク座標系について説明する図である。
【図２４】 Ｂ方式の実施形態において、カメラがマーク座標系を見る視線方向を記述するベクトルについて説明する図である。
【図２５】 Ｂ方式の実施形態における自律アプローチ動作の処理の概要を記したフローチャートである。
【符号の説明】
１ａ アプローチ完了状態入力手段
１ｂ 相対位置認識手段
２，１０ ロボット
２ａ ツール（ハンド）
３，１４ ワーク
４ メモリ
５ ジョグ送りボタン
６ 軸移動制御手段
１１ アーム
１２ ツール
１３ カメラ
１３ａ カメラ座標系
１４ ワーク
１４ａ 特徴部
１５ マーク
１５ａ マーク座標系
２０ 画像処理装置
３０ ロボット制御装置
３１ プロセッサボード
３１ａ プロセッサ
３１ｃ ＲＡＭ
３１ｂ ＲＯＭ
３５ ディジタルＩ／Ｏ
３６ａ ＣＲＴ
３６ｂ 操作パネル
３７ アナログＩ／Ｏ
３９ バス
４０ 教示操作盤
４１ 表示画面
４２ ファンクションキー
４４ 非常停止ボタン
４５ カーソルキー
４６ テンキー部
４７（Ｊ１〜Ｊ６） ジョグ送りボタン
５１〜５６ サーボモータ
５８ ＲＳ２３２Ｃ機器
ＣＢ１〜３ カメラのキャリブレーションを行なう位置
Ｄ0 ，Ｄ1 ，Ｄ-1 ，Ｄ2 ，Ｄ-2 マーク座標系のドット
Claims (6)
- カメラ手段及び画像処理手段を備えた視覚センサとロボット制御装置とを含む制御手段を用いて、存在位置にばらつきのある対象物に対するアプローチ動作をロボットに行なわせるためのロボットの移動制御方法において、
対象物に対するアプローチ完了状態を表現するアプローチ完了状態表現データを前記制御手段に記憶させるアプローチ完了状態教示段階と、
前記制御手段によってロボットにアプローチ動作を行わせるアプローチ動作実行段階とを含み、
前記アプローチ動作実行段階には、該アプローチ動作が前記アプローチ完了状態表現データに基づいて自律的に行なわれる自律アプローチ動作実行段階とを含んでおり、
前記アプローチ完了状態表現データは、前記ロボットの手先と対象物の相対的な位置関係を特定するものであり、
前記自律アプローチ動作実行段階におけるロボットの移動目標位置が、前記制御手段内のソフトウェア処理によって、前記アプローチ完了状態表現データによって特定される前記相対的位置関係が実現される位置として定められる、前記方法。 - 前記アプローチ完了状態教示段階で記憶されるアプローチ完了状態表現データが、対象物を代表する物体を前記カメラ手段によって撮影することによって取得された画像に基づいて前記制御手段内で作成される、請求項１に記載されたロボットの移動制御方法。
- 前記アプローチ完了状態教示段階で記憶されるアプローチ完了状態表現データが、画像上における前記対象物代表物体の特徴部と重心位置を表わすデータとを含んでいる、請求項１または請求項２に記載されたロボットの移動制御方法。
- 対象物及び前記対象物代表物体の表面上に前記視覚センサによって認識可能なマーク座標系が設置されており、
前記アプローチ完了状態教示段階で記憶されるアプローチ完了状態表現データが、画像上における前記マーク座標系の位置・姿勢を表わすデータとを含んでいる、請求項１または請求項２に記載された、ロボットの移動制御方法。 - 前記アプローチ動作実行段階には、前記自律アプローチ動作実行段階に先立つジョグ送りの段階が含まれており、
前記ジョグ送り段階から前記自律アプローチ動作実行段階への移行が、前記制御手段によって自動的に行なわれる、請求項１〜請求項４のいずれか１項に記載されたロボットの移動制御方法。 - 前記自律アプローチ動作実行段階におけるソフトウェア処理が、当該時点において前記視覚センサによって認識される前記ロボットの手先と対象物の相対的な位置関係を表わすデータと、前記教示されたアプローチ完了状態表現データとを比較する処理と、前記比較結果に基づいてロボットの各軸を移動制御するための処理と、アプローチ完了状態の完了／未完了を判定する処理を、アプローチ完了状態の完了が判定されるまで逐次的に繰り返すものである、請求項１〜請求項５のいずれか１項に記載されたロボットの移動制御方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP19855296A JP3998741B2 (ja) | 1995-07-12 | 1996-07-10 | ロボットの移動制御方法 |
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP7-175891 | 1995-07-12 | ||
| JP17589195 | 1995-07-12 | ||
| JP19855296A JP3998741B2 (ja) | 1995-07-12 | 1996-07-10 | ロボットの移動制御方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0976185A JPH0976185A (ja) | 1997-03-25 |
| JP3998741B2 true JP3998741B2 (ja) | 2007-10-31 |
Family
ID=26497002
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP19855296A Expired - Fee Related JP3998741B2 (ja) | 1995-07-12 | 1996-07-10 | ロボットの移動制御方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3998741B2 (ja) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006039893A (ja) * | 2004-07-27 | 2006-02-09 | Ishikawajima Harima Heavy Ind Co Ltd | 可搬式自動加工装置 |
| JP5062535B2 (ja) * | 2008-10-20 | 2012-10-31 | 株式会社安川電機 | 産業用ロボットの手持操作器およびロボット制御システム |
| JP5318727B2 (ja) * | 2009-10-30 | 2013-10-16 | 本田技研工業株式会社 | 情報処理方法及び装置並びにプログラム |
| JP6188440B2 (ja) * | 2013-06-17 | 2017-08-30 | キヤノン株式会社 | ロボット装置及びロボット制御方法 |
| JP6424432B2 (ja) * | 2014-01-23 | 2018-11-21 | セイコーエプソン株式会社 | 制御装置、ロボットシステム、ロボット及びロボット制御方法 |
| JP7323993B2 (ja) * | 2017-10-19 | 2023-08-09 | キヤノン株式会社 | 制御装置、ロボットシステム、制御装置の動作方法及びプログラム |
| CN107865753B (zh) * | 2017-11-30 | 2023-05-09 | 湖南妙手机器人有限公司 | 康复机器人 |
| JP6810087B2 (ja) | 2018-03-29 | 2021-01-06 | ファナック株式会社 | 機械学習装置、機械学習装置を用いたロボット制御装置及びロボットビジョンシステム、並びに機械学習方法 |
| JP7205752B2 (ja) * | 2018-11-09 | 2023-01-17 | オムロン株式会社 | ロボット制御装置、ロボット制御方法、及びロボット制御プログラム |
| WO2021048579A1 (ja) * | 2019-09-11 | 2021-03-18 | Dmg森精機株式会社 | システム及び工作機械 |
| CN112793983B (zh) * | 2020-12-30 | 2022-04-19 | 易思维（杭州）科技有限公司 | 适用于卡槽式料架的自动上件方法 |
| CN113084827B (zh) * | 2021-04-01 | 2022-06-14 | 北京飞影科技有限公司 | 一种摄像装置光心位置标定方法及装置 |
| CN116999177B (zh) * | 2023-10-07 | 2024-01-12 | 北京科鹏医疗器械有限公司 | 一种经自然通道内窥镜操作的接触力自主保护算法 |
-
1996
- 1996-07-10 JP JP19855296A patent/JP3998741B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0976185A (ja) | 1997-03-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3998741B2 (ja) | ロボットの移動制御方法 | |
| US11045954B2 (en) | Robot system and method of controlling the same | |
| US11197730B2 (en) | Manipulator system | |
| EP0523889B1 (en) | Apparatus for controlling industrial robot to perform coordinated operation using teaching playback method and method thereof | |
| US11977365B2 (en) | Skill transfer mechanical apparatus | |
| EP0577437A1 (en) | Control apparatus for industrial robot | |
| JP2684359B2 (ja) | ロボットのワーク直交座標系設定装置 | |
| KR20180069031A (ko) | 로봇의 다이렉트 교시방법 | |
| JP2925072B2 (ja) | ロボットの位置教示のための移動制御方式 | |
| US20170259430A1 (en) | Robot device and robot controller | |
| CN109414820B (zh) | 机器人的运转方法、储存部、及机器人*** | |
| CN114905487A (zh) | 示教装置、示教方法以及记录介质 | |
| US11964391B2 (en) | Robot system | |
| US11878423B2 (en) | Robot system | |
| JPH08267381A (ja) | ロボット手動送り制御装置 | |
| JPH1190868A (ja) | ロボット制御装置 | |
| Cao et al. | Novel humanoid dual-arm grinding robot | |
| Wahab et al. | Automation of Pick and Place Operation in Contact Lens Manufacturing | |
| JPH01175608A (ja) | ロボットのティーチングデータ作成方法 | |
| JP3076345B2 (ja) | マスタ・スレーブマニピュレータの制御方法及び装置 | |
| JPH01154206A (ja) | ティーチングボックス | |
| CN116728422A (zh) | 机器人示教控制方法、机器人示教装置以及机器人*** | |
| JPH0643917A (ja) | ロボット群制御方法 | |
| Lu et al. | Fine Hand-Eye Coordination Control of Robot in Parallel Space | |
| CN114434436A (zh) | 一种控制机器人的方法、装置和计算机可读存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20050413 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050419 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050620 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20050726 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20050922 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20060228 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070808 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100817 Year of fee payment: 3 |
|
| LAPS | Cancellation because of no payment of annual fees |