JP7207851B2 - 制御方法、ロボットシステム、物品の製造方法、プログラム及び記録媒体 - Google Patents

Description

本発明は、ロボットの制御に関する。

工場において部品の移載や組立、加工などの作業を自動化するために、多自由度のロボットを有するロボットシステムが用いられている。多自由度のロボットとしては、例えば垂直多関節型のロボット、水平多関節型のロボット、パラレルリンクのロボット、直交型のロボットなどが用いられる。ロボットをロボットプログラムに従って動作させることにより、目的に応じたさまざまな作業をロボットに行わせることができる。ばらつきのある部品に対して移載や組立、加工などの作業を行う場合、又は生産ラインの立ち上げ時や段取り替え後などに教示を行う場合に、視覚センサを用いてロボットの位置ずれの調整を行う技術がある。例えば特許文献１には、溶接用のロボットの先端にカメラを取り付けて、マークと目標位置姿勢のずれを補正する動作を繰返しロボットに行わせることによって教示を行う技術が提案されている。

特開２０１４－１８４５３０号公報

視覚センサを用いてロボットの姿勢を補正する動作を繰り返し行う繰り返し制御を実施する場合には、繰り返し制御が完了したことを判定するための閾値を事前に定める必要がある。特許文献１では、画像内のマークの並進方向の位置ずれに対する閾値によって、部品に対するロボットの並進距離の収束を判定している。また、画像内に映ったマークの真円度からマークに対するカメラの傾きを補正し、マークの真円度に対する閾値によって、部品に対するロボットの傾き角度の収束を判定している。これらの判定のために用いられる各閾値は、ロボットシステムの作業者が経験的に定めている。ロボットの先端の並進方向の位置決め精度が重要である場合においては、並進距離の閾値を適切に設定していれば、傾き角度の精度は問題にならなかった。しかし近年では、精密組立など精密な作業にもロボットが用いられるようになってきたため、ロボットの先端の傾きも高精度に位置決めを行う必要がある。そのため、マークの真円度を求めてロボット先端の傾きの収束を判定する場合においては、ロボット先端の変位に対してマークの真円度がどの程度変化するかという関係を作業者が正確に把握して、真円度に対する閾値を定める必要があった。この真円度に対する閾値を視覚センサの配置なども考慮して計算するのは手間がかかり、また視覚センサの誤差要因も考慮する必要があり、適切な閾値を定めるには、作業者の作業負荷が大きいものであった。

そこで、本発明は、作業者の作業負荷を低減させ、かつロボットによる生産性が低下することなく、ロボットの位置決めを高精度に行うことを目的とする。

本発明の第１の態様は、ロボット装置に設けられた視覚センサにより特徴点を計測し、前記ロボット装置を制御する制御方法であって、前記特徴点の位置関係に関連する目標座標値を取得し、前記目標座標値とは別に、前記ロボット装置の動作に追従して移動する判定点を取得し、前記特徴点を前記視覚センサにより計測し、前記視覚センサにより計測された際の前記特徴点の座標である計測座標値を取得し、前記目標座標値を前記計測座標値に近づけるように、前記目標座標値と前記判定点とを座標変換し、座標変換される前の前記判定点と座標変換された後の前記判定点との差に応じた第１の値を取得し、前記第１の値が、所定の閾値以下であるか判定し、前記第１の値が前記所定の閾値より大きい場合、前記ロボット装置の姿勢を補正する、ことを特徴とする制御方法である。

また、本発明の第２の態様は、ロボット装置と、前記ロボット装置に支持される視覚センサと、前記ロボット装置と前記視覚センサとを制御する制御部と、を備えるロボットシステムであって、前記制御部が、特徴点の位置関係に関連する目標座標値を取得し、前記目標座標値とは別に、前記ロボット装置の動作に追従して移動する判定点を取得し、前記特徴点を前記視覚センサにより計測し、前記視覚センサにより計測された際の前記特徴点の座標である計測座標値を取得し、前記目標座標値を前記計測座標値に近づけるように、前記目標座標値と前記判定点とを座標変換し、座標変換される前の前記判定点と座標変換された後の前記判定点との差に応じた第１の値を取得し、前記第１の値が、所定の閾値以下であるか判定し、前記第１の値が前記所定の閾値より大きい場合、前記ロボット装置の姿勢を補正する、ことを特徴とするロボットシステムである。

本発明によれば、作業者の作業負荷が低減し、かつロボットによる生産性が低下することなく、ロボットの位置決めを高精度に行うことができる。

第１実施形態に係るロボットシステムを示す説明図である。 （ａ）はワーク保持治具の斜視図である。（ｂ）はワーク保持治具、第１のワーク及び第２のワークの斜視図である。 （ａ）、（ｂ）及び（ｃ）は第１実施形態におけるロボットハンド及びステレオカメラの斜視図である。 第１実施形態に係る教示を行うための事前準備を示すフローチャートである。 （ａ）は校正に用いる校正治具の説明図である。（ｂ）は基準パターン部の平面図である。 ディスプレイに表示される設定画面を示す説明図である。 第１実施形態におけるロボット制御装置及びセンサ制御装置の機能ブロック図である。 第１実施形態における教示作業のフローチャートである。 特徴点の目標座標値と計測座標値との関係を説明するための模式図である。 （ａ）、（ｂ）、（ｃ）及び（ｄ）は図８のステップＳ２０２～Ｓ２０５を説明するための模式図である。 （ａ）、（ｂ）は第１実施形態における判定処理を説明するための模式図である。 （ａ）、（ｂ）は比較例における判定処理を説明するための模式図である。 （ａ）は第２実施形態において組み付け作業に用いられる第１のワーク及び第２のワークの説明図である。また、（ｂ）は第２実施形態におけるロボットの構成を示す模式図である。 第２実施形態において組付作業を行うための事前準備を示すフローチャートである。 第２実施形態において校正に用いる校正治具の説明図である。 第２実施形態における組付作業のフローチャートである。 第３実施形態における演算処理を説明するための模式図である。

以下、本発明を実施するための形態を、図面を参照しながら詳細に説明する。
［第１実施形態］
図１は、第１実施形態に係るロボットシステムを示す説明図である。ロボットシステム１００は、ロボット２００、視覚センサの一例であるステレオカメラ３００、ワーク保持治具４００、ロボット制御装置５００、センサ制御装置６００を備えている。さらに、表示部としてのディスプレイ７００、入力部としてのティーチングペンダント８００及び入力装置８５０を備えている。ロボット２００は、ロボットアーム２０１、及びエンドエフェクタの一例であるロボットハンド２０２を備えている。ロボットアーム２０１は、垂直多関節のロボットアームである。ロボット２００の基端、即ちロボットアーム２０１の基端は、架台１５０に固定されている。

ロボットアーム２０１の先端には、直接、又は不図示の力覚センサを介してロボットハンド２０２が取り付けられている。つまり、ロボット２００の先端がロボットハンド２０２で構成されている。ロボットハンド２０２は、ハンド本体である掌部２１０と、掌部２１０に対して開閉動作が可能な複数、第１実施形態では２本のフィンガ２１１，２１２と、を有し、ワークを把持又は把持解放することができる。即ち、掌部２１０は、ハウジングと、ハウジングの内部に配置され、フィンガ２１１，２１２を駆動する駆動機構と、を有する。

ワーク保持治具４００は、ワークが架台１５０に対して動かないようにワークをチャックする治具であり、架台１５０に固定されている。ワーク保持治具４００は、ロボット２００の先端、即ちロボットハンド２０２の可動範囲内に配置されている。ロボット２００の先端、即ちロボットハンド２０２は、ロボット２００の基端を原点とするロボット座標系Ｏにおいて６自由度の動作が可能である。具体的には、ロボットハンド２０２は、ロボットアーム２０１が動作することにより、ロボット座標系Ｏにおいて互いに直交する３軸の並進方向と、３軸まわりの回転方向の動作が可能である。

ティーチングペンダント８００は、ユーザの操作によってロボット制御装置５００に指令を送る。この指令の入力を受けたロボット制御装置５００は、指令に従ってロボット２００を動作させる。即ち、ユーザは、ティーチングペンダント８００を操作することによってロボットアーム２０１を動作させ、ロボットハンド２０２を任意の方向及び任意の速度でジョグ送りすることができる。また、ユーザは、ティーチングペンダント８００を操作することによって、ロボットハンド２０２のフィンガ２１１，２１２を開閉させることもできる。

ロボット制御装置５００は、主にロボット２００の動作を制御する。センサ制御装置６００は、主にステレオカメラ３００の動作を制御するとともに、画像処理や計測処理等の演算処理を行う。

ロボット制御装置５００はコンピュータであり、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）５０１を備えている。また、ロボット制御装置５００は、内部記憶装置としてＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）５０２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）５０３及びＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）５０４を備えている。また、ロボット制御装置５００は、インターフェース（Ｉ／Ｆ）５０５とバス５０６を備えている。ＣＰＵ５０１、ＲＯＭ５０２、ＲＡＭ５０３、ＨＤＤ５０４及びインターフェース５０５は、互いに通信可能にバス５０６を介して接続されている。

センサ制御装置６００はコンピュータであり、ＣＰＵ６０１を備えている。また、センサ制御装置６００は、内部記憶装置としてＲＯＭ６０２、ＲＡＭ６０３及びＨＤＤ６０４を備えている。また、センサ制御装置６００は、記録ディスクドライブ６０５、インターフェース（Ｉ／Ｆ）６０６、及びバス６０７を備えている。ＣＰＵ６０１、ＲＯＭ６０２、ＲＡＭ６０３、ＨＤＤ６０４、ディスクドライブ６０５及びインターフェース６０６は、互いに通信可能にバス６０７を介して接続されている。

ロボット制御装置５００のインターフェース５０５には、ロボット２００、ワーク保持治具４００及びティーチングペンダント８００が通信線で接続されている。センサ制御装置６００のインターフェース６０６には、ステレオカメラ３００、ディスプレイ７００及び入力装置８５０が通信線で接続されている。また、ロボット制御装置５００のインターフェース５０５とセンサ制御装置６００のインターフェース６０６とが通信線で接続されている。よって、ＣＰＵ５０１とＣＰＵ６０１とは互いに通信可能となっている。第１実施形態では、２つのＣＰＵ５０１，６０１によって制御部として機能する。

ロボット制御装置５００のＲＯＭ５０２には、基本プログラムが格納されている。ＲＡＭ５０３は、ＣＰＵ５０１の演算処理の結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ５０４には、ロボット２００の動作を規定するロボットプログラム５１０が格納されている。ロボットプログラム５１０には、教示点の情報５１１とコマンドの情報５１２とが含まれている。ＣＰＵ５０１は、ロボットプログラム５１０に従ってロボット２００の動作を制御し、ロボット２００にワークの組付作業等の作業を行わせる。また、ＣＰＵ５０１は、教示点の情報５１１の設定、即ち教示作業を行うときには、センサ制御装置６００のＣＰＵ６０１の指令、又はティーチングペンダント８００の指令に従ってロボット２００の動作を制御する。即ち、ＣＰＵ５０１は、ＣＰＵ６０１から入力を受けた情報からロボット２００の取るべき姿勢を求め、求めた姿勢情報に従ってロボット２００を実際に動作させる。また、ＣＰＵ５０１は、ロボット２００を実際に動作させたときの姿勢の情報を教示点の情報５１１として新規作成又は更新が可能である。

センサ制御装置６００のＲＯＭ６０２には、基本プログラムが格納されている。ＲＡＭ６０３は、ＣＰＵ６０１の演算処理結果等、各種データを一時的に記憶する記憶装置である。ＨＤＤ６０４には、教示を行う際のプログラム６１０が格納されている。ディスクドライブ６０５は、ディスク６０８に記録された各種データやプログラム等を読み出すことができる。ＣＰＵ６０１は、ロボット２００の教示を行うときには、プログラム６１０に従って、ステレオカメラ３００の動作を制御し、ロボット２００の動作をロボット制御装置５００を介して制御する。また、ＣＰＵ６０１は、ディスプレイ７００の表示動作を制御する。例えば、ＣＰＵ６０１は、ステレオカメラ３００が撮像した画像データや、画像処理を実行した結果データなどをディスプレイ７００に表示させ、ユーザに情報を提示する。ディスプレイ７００は、三次元画像を表示する表示領域を有しており、特徴点と判定点との相対関係を表示することができる。またディスプレイ７００は、設定画面において複数の判定点候補を表示し、複数の判定点候補のいずれか１つをユーザに選択させることができる。入力装置８５０は、例えばキーボードやマウスであり、ユーザがディスプレイ７００を見ながら操作することより、文字、数値、ポインタ位置などの情報を、センサ制御装置６００に入力することができる。

なお、第１実施形態では、コンピュータ読み取り可能な記録媒体がＨＤＤ６０４であり、ＨＤＤ６０４にプログラム６１０が格納される場合について説明するが、これに限定するものではない。プログラム６１０は、コンピュータ読み取り可能な記録媒体であれば、いかなる記録媒体に記録されていてもよい。例えば、プログラム６１０を供給するための記録媒体としては、ＲＯＭ６０２や、ディスク６０８、不図示の外部記憶装置等を用いてもよい。具体例を挙げて説明すると、記録媒体として、フレキシブルディスク、光ディスク、光磁気ディスク、磁気テープ、ＵＳＢメモリ等の不揮発性メモリ、ＳＳＤ等を用いることができる。

また、第１実施形態では、ロボット制御装置５００とセンサ制御装置６００とがそれぞれ一台のコンピュータで構成され、ロボットシステム１００は合計２台のコンピュータを備えている場合について説明したが、これに限定するものでない。ロボット制御装置５００及びセンサ制御装置６００の双方の機能を１台のコンピュータで実現してもよいし、ロボット制御装置５００及びセンサ制御装置６００の機能を３台以上のコンピュータで分散処理するようにしてもよい。また、センサ制御装置６００はステレオカメラ３００の筐体の外部に配置されているが、筺体に内蔵されたスマートカメラによって同等の機能を実現することも可能である。

図２（ａ）は、ワーク保持治具４００の斜視図である。図２（ｂ）は、ワーク保持治具４００、第１のワークであるワークＷ１及び第２のワークであるワークＷ２の斜視図である。図２（ａ）に示すようにワーク保持治具４００は、ベース部４０１、ベース部４０１に固定されたワーク突当部４０２，４０３、及び治具フィンガ４０４，４０５を有している。ワーク保持治具４００は、ロボットプログラム５１０に従って治具フィンガ４０４，４０５を開閉動作することができる。ワークＷ１をベース部４０１に載置した状態で治具フィンガ４０４，４０５を閉じることにより、ワークＷ１をチャックすることができる。

図２（ｂ）には、ワーク保持治具４００にワークＷ１が保持されている状態が図示されている。第１実施形態では、ワークＷ２をワークＷ１に組み付ける組付作業として、ワークＷ１とワークＷ２との嵌合作業を行う。具体的には、ワークＷ１をワーク保持治具４００に保持させ、ワークＷ２をロボットハンド２０２に把持させてロボットアーム２０１を動作させることにより、嵌合作業を行う。嵌合作業を行う際には、図２（ｂ）に示すように、ワークＷ１の真上に位置する作業開始位置ＰＡにワークＷ１を把持したロボットハンド２０２を移動させ、この作業開始位置ＰＡからワークＷ１を真下に移動させることにより、嵌合作業を行う。したがって、作業開始位置ＰＡにロボットハンド２０２、即ちワークＷ１を正確に位置決めさせるために正確な教示が必要となる。

第１実施形態では、ステレオカメラ３００を用いてロボット２００の教示を行う。ステレオカメラ３００は、ロボット２００に対して着脱可能となっている。具体的に説明すると、ステレオカメラ３００は、ロボットハンド２０２に対して位置決めされた状態でロボットハンド２０２に把持されるように構成されている。

図３（ａ）、図３（ｂ）及び図３（ｃ）は、第１実施形態におけるロボットハンド２０２及びステレオカメラ３００の斜視図である。図３（ａ）及び図３（ｂ）はロボットハンド２０２がステレオカメラ３００を把持解放した状態、図３（ｃ）はロボットハンド２０２がステレオカメラ３００を把持した状態を示している。ステレオカメラ３００は、計測対象物の３次元の位置姿勢を計測可能にステレオ法により計測対象物を撮像するカメラである。ステレオカメラ３００は、センサ制御装置６００からの撮像指令の入力を受けて撮像を行い、取得した画像データをセンサ制御装置６００に送信することができる。ステレオカメラ３００は、第１のカメラであるカメラ３０１と、第２のカメラであるカメラ３０２とを有している。２つのカメラ３０１，３０２は、カメラ筺体３１０の内部に配置されている。カメラ３０１，３０２は、ＣＣＤイメージセンサ又はＣＭＯＳイメージセンサ等の撮像素子を有するデジタルカメラである。カメラ筺体３１０は、不図示のアタッチメント部を介してベース部３１１に固定されている。ベース部３１１には、ロボットハンド２０２との着脱機構として位置決めピン３２１，３２２及びテーパ部３３１，３３２が設けられている。

ロボットハンド２０２の掌部２１０の平面２１０Ａには、ステレオカメラ３００の位置決めピン３２１，３２２に対応する位置に丸穴２２１及び長穴２２２が形成されている。ロボットハンド２０２の掌部２１０の平面２１０Ａ、丸穴２２１及び長穴２２２は、ロボットハンド２０２の機械的基準となっており、フィンガ２１１，２１２はこれらの機械的基準に対して所定の公差範囲内で取り付けられている。具体的には、掌部２１０の平面２１０Ａ、丸穴２２１及び長穴２２２を用いて不図示のフィンガ調整治具を掌部２１０に対して位置決めした状態で、フィンガ２１１，２１２の開閉方向の取り付け位置が不図示のフィンガ調整治具に対して調整されている。フィンガ２１１，２１２の開閉方向の寸法は、不図示の開閉機構に対して厚みの異なるシムを挟んで取り付けることにより調整ができるようになっている。このように、フィンガ２１１，２１２は、掌部２１０を基準に調整されている。ロボットハンド２０２の掌部２１０に対してステレオカメラ３００のベース部３１１を押し当てると、両者の面が突き当たるとともに位置決めピン３２１，３２２と丸穴２２１及び長穴２２２がそれぞれ嵌合する。これにより、ロボットハンド２０２に対してステレオカメラ３００を位置決めすることができる。フィンガ２１１，２１２の内側には、テーパ部２３１，２３２が配置されている。ベース部３１１を掌部２１０に押し当てた状態でフィンガ２１１，２１２を閉じると、ロボットハンド２０２のテーパ部２３１，２３２とステレオカメラ３００のテーパ部３３１，３３２がそれぞれ噛み合う。テーパ部２３１，２３２及びテーパ部３３１，３３２により、ベース部３１１を掌部２１０に押しつける力が発生し、ロボットハンド２０２に対してステレオカメラ３００を安定して固定することができる。このように着脱機構は、ステレオカメラ３００をロボットハンド２０２に対して直接取り付ける構成となっている。そのため、教示作業の際に、ロボットアーム２０１の誤差や、ロボットアーム２０１の先端であるフランジ面から掌部２１０までのロボットハンド２０２の製作誤差の影響を低減して高精度に教示を行うことができる。以上、ステレオカメラ３００は、ロボットハンド２０２に把持されることで、ロボット２００に支持される。

第１実施形態ではワーク保持治具４００は教示作業を行うときの計測対象物である。図２（ａ）に示すワーク保持治具４００には、ステレオカメラ３００によって計測可能な、特徴点の一例である複数のマークＭＫ１～ＭＫ３が付与されている。マークＭＫ１～ＭＫ３は、ワーク保持治具４００の機械的基準、例えばワーク突当部４０２，４０３に対して高精度に位置決めされている。マークＭＫ１～ＭＫ３は、ベース部４０１の上面に、高さ方向に所定の精度範囲内の厚み又は深さで形成されている。マークＭＫ１～ＭＫ３は、特徴点として画像処理で高精度に認識できるように、ベース部４０１に対してコントラストが得られるよう、黒色である。なお、マークＭＫ１～ＭＫ３をベース部４０１に付与する方法は任意であるが、例えばレーザー加工、印刷、エッチング、めっき処理、シール添付などの方法によりマークを付与することができる。また、ワーク保持治具４００に直接的にマークＭＫ１～ＭＫ３を付与する場合について例示しているが、別部材のマーク付き基準プレートをワーク保持治具４００に取り付け、教示時のみ一時的にマークＭＫ１～ＭＫ３を配置するようにしてもよい。

図４は、第１実施形態に係る教示を行うための事前準備を示すフローチャートである。まず、ステレオカメラ３００の校正値を設定する（Ｓ１０１）。ステップＳ１０１では、（１）ステレオカメラ校正と（２）ベース－カメラ間校正の２種類の校正を実施し、そのパラメータをセンサ制御装置６００に設定、即ちＨＤＤ６０４に記憶させる。

（１）ステレオカメラ校正
ステレオカメラ３００によってステレオ画像上の対応点座標から３次元の計測値を得るためのステレオカメラパラメータを校正する。具体的には、カメラ３０１及びカメラ３０２の画像においてピクセル単位で表される画素座標値と、３次元空間における視線ベクトルとの関係を示すカメラ内部パラメータを、カメラ３０１及びカメラ３０２のそれぞれに対して求める。また、カメラ３０１とカメラ３０２の相対的な位置姿勢を表すカメラ外部パラメータも求める。即ち、カメラ外部パラメータとして、カメラ３０１の位置姿勢を代表する第１のセンサ座標系と、カメラ３０２の位置姿勢を代表する第２のセンサ座標系との相対的な位置姿勢を表す座標変換行列を求める。このようなステレオカメラの校正に関してはさまざま手法が公知である。カメラ３０１の画像とカメラ３０２の画像から特徴点を抽出すると、センサ座標系から見たその特徴点の３次元座標値を求めることができる。第１実施形態においては、カメラ３０１が基準カメラであるとして、ステレオ計測の結果は第１のセンサ座標系からみた座標値として表現されるものとする。以下、ステレオカメラ３００の位置姿勢を代表する座標系として、第１のセンサ座標系をセンサ座標系Ｖと表す。

（２）ベース－カメラ間校正
前述したステレオカメラ校正の方法によってセンサ座標系Ｖからみた特徴点の位置は３次元で高精度に計測することが可能であるが、センサ座標系Ｖはカメラ３０１のレンズ主点を原点とする座標系である。そのため、外部からその位置姿勢を直接的に計測することはできない。そこで第１実施形態においては、ベース部３１１に対するセンサ座標系Ｖの位置姿勢を特定するためのベース－カメラ間校正も行い、その校正結果データをセンサ制御装置６００のＨＤＤ６０４に記憶させる。ベース部３１１とセンサ座標系Ｖとの関係を校正するベース－カメラ間校正の方法について具体的に説明する。

図５（ａ）は、校正に用いる校正治具９００の説明図である。校正治具９００は、ベース部９０１、支柱部９０２，９０３、上板部９０４、及び基準パターン部９０５を有する。ベース部９０１と上板部９０４とは、互いに対向するように配置された板状の部材であり、支柱部９０２，９０３で接続されている。ベース部９０１には、掌部２１０の丸穴２２１及び長穴２２２と同様に、ステレオカメラ３００のピン３２１，３２２に対応する位置に穴部９０６，９０７が設けてあり、ステレオカメラ３００を高い再現性で位置決めすることができる。

図５（ｂ）は、基準パターン部９０５の平面図である。上板部９０４の内面には基準パターン部９０５が配置されている。基準パターン部９０５は、板状の部材である。基準パターン部９０５の表面には、エッチング等で高精度に加工されたＫ個のマーク９０８がアレイ状に形成されている。ベース部９０１の上面、穴部９０６，９０７及び基準パターンマーク９０８の各位置は、校正治具９００の製作後に不図示の計測器により計測されており、それぞれの関係が高精度に分かっている。さらにベース部９０１の上面及び穴部９０６，９０７の計測値を基準として、ステレオカメラ３００の取り付け基準位置の位置姿勢に対応する、視覚センサの座標系であるベース座標系Ｂが設定されている。そして、マーク９０８の各点の３次元座標値ｍ［ｉ］（ｉ＝１～Ｋ）はベース座標系Ｂからみた３次元座標値^Ｂｍ［ｉ］（ｉ＝１～Ｋ）に座標変換した上で、不図示の校正装置の演算部に記憶されている。校正治具９００に対してステレオカメラ３００を位置決めした状態で基準パターン部９０５のマーク９０８をステレオ計測することにより、ベース座標系Ｂとセンサ座標系Ｖとの関係を求めることができる。

ベース座標系Ｂとセンサ座標系Ｖとの関係を表す回転行列をＲ_Ｃ、並進ベクトルをｔ_Ｃとする。マーク９０８をステレオ計測して得られるセンサ座標系Ｖからみた座標値^Ｖｍ［ｉ］（ｉ＝１～Ｋ）に対して、各種の誤差がない場合は、以下の式（１）が成り立つ。

実際の計測データを用いる場合には誤差があるため、式（１）の左辺と右辺は厳密には一致しない。したがって、以下の式（２）を解くことによって最小二乗解を得る。

即ち、式（２）を満たすような回転行列Ｒ_Ｃと並進ベクトルｔ_Ｃを最小二乗解として求めればよい。この問題は、点集合同士のマッチング問題として知られており、例えば特異値分解を用いた手法によって解くことができる。これにより、センサ座標系Ｖを基準とする座標値を、ベース座標系Ｂを基準とする座標値に座標変換するためのベース－カメラ間校正値として、回転行列Ｒ_Ｃ及び並進ベクトルｔ_Ｃが得られる。

次に、教示の目標座標値を設定する（Ｓ１０２）。第１実施形態では、図２（ｂ）に示すようにワークＷ２を位置ＰＡに位置決めするための目標座標値を設定する。図６は、ディスプレイ７００に表示される設定画面を示す説明図である。ユーザは、図６に示す設定画面を見ながら、目標座標値を設定することができる。

教示の目標座標値の設定は、ベース部３１１の面及び位置決めピン３２１，３２２に代表されるステレオカメラ３００の機械的基準と、ワーク保持治具４００に付与されたマークＭＫ１～ＭＫ３の設計上の位置関係を入力することによって行う。例えば、ベース座標系Ｂを基準とする設計上のマークＭＫ１～ＭＫ３の３次元の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）を設定する。ここで目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）は、ＸＹＺの３成分を持つベクトルで表される３次元の座標値である。目標座標値^Ｂｐ［ｉ］は、目標座標値のリストをセンサ制御装置６００のＨＤＤ６０４に予め記憶させておき、ユーザがリストの中から選択して設定するようにしてもよいし、ユーザが入力装置８５０を操作して数値を入力することで設定するようにしてもよい。ユーザが入力装置８５０を用いて数値を入力する場合は、入力操作をより簡便に行えるようにするため、マークＭＫ１～ＭＫ３の位置姿勢を代表するマーカー座標系Ｍを導入してもよい。この場合、マーカー座標系Ｍを基準とするマークＭＫ１～ＭＫ３の各点の３次元の座標値^Ｍｐ［ｉ］（ｉ＝１～３）と、ベース座標系Ｂに対するマーカー座標系Ｍの相対的な位置姿勢の設計値を入力する。その回転成分を^ＢＲ_Ｍ，並進成分を^Ｂｔ_Ｍとすると、マークＭＫ１～ＭＫ３の３次元の座標値^Ｂｐ［ｉ］（ｉ＝１～３）は以下の式（３）のように表される。

マーカー座標系Ｍで入力操作を行えるようにすることで、マークＭＫ１～ＭＫ３の形状のみで決まる配置と、マーカー座標系Ｍに対するロボットハンド２０２の位置姿勢を別個に設定できるため、マークＭＫ１～ＭＫ３の点数が多い場合に設定が簡便になる。図６に示す設定画面７１０の例では、ロボットハンド２０２とステレオカメラ３００の３次元ＣＡＤモデルが表示されるとともに、ベース座標系Ｂの位置姿勢が矢印で上書き表示されている。ユーザが入力したマーカー座標系Ｍの位置姿勢情報とマークの座標値に基づいて、ＣＡＤモデル画面上にマーカー座標系Ｍの位置姿勢を表す矢印と、マークＭＫ１～ＭＫ３の各点ｐ１～ｐ３の位置を表す点が重ね書き表示される。これらの入力した設定情報をユーザが直感的に確認することができるので、設定が容易で間違いが起こりにくい。以上、ステップＳ１０２により、センサ制御装置６００のＨＤＤ６０４には、ベース座標系Ｂを基準とするマークＭＫ１～ＭＫ３の目標座標値^Ｂｐ［ｉ］が記憶される。

次に、判定点を設定する（Ｓ１０３）。判定点とは、後述する収束判定の演算に用いる仮想的な点であり、ステレオカメラ３００を用いて計測される特徴点とは異なる点である。判定点は、マークＭＫ１～ＭＫ３に対するステレオカメラ３００の傾きを、求められている精度の範囲内に早く収束させるために、特徴点とは別に追加した点である。ベース座標系Ｂを基準とする判定点の座標値^Ｂｐ_ａｄｄを、センサ制御装置６００のＨＤＤ６０４に記憶させる。判定点は、計算のみで用いる仮想的な点であるので、ステレオカメラ３００の視野領域の外、即ち計測可能範囲の外にあっても構わない。判定点は、任意の点であるが、特徴点であるマークＭＫ１～ＭＫ３よりもステレオカメラ３００又はロボット２００に近い点であるのか好ましい。より好ましい判定点は、フィンガ２１１，２１２と掌部２１０とで囲まれた、ロボットハンドでワークを把持可能な領域（以下、把持領域と呼ぶ）と、ロボットハンドに把持されたワーク外形とを合算した領域内の任意の点である。例えば、ベース座標系Ｂやセンサ座標系Ｖの原点を判定点として用いてもよいし、フィンガ２１１の先端部などの点としてもよい。また、判定点の設定にあたっては、ユーザが入力装置８５０を操作して直接的に数値を入力するようにしてもよいし、ロボットシステム１００におけるマークＭＫ１～ＭＫ３以外の任意の特徴点や座標系の原点を選択肢として提示し、ユーザに選択させてもよい。後者の場合、ベース座標系Ｂの原点やツール座標系Ｔの原点など、ディスプレイ７００の画像を見ながらユーザが選択して設定できるので、判定点を数値入力する前者の場合に比べて設定作業が容易となる。

図６に示す設定画面７１０の例においては、ベース座標系、ツール座標系及びセンサ座標系の原点の値を表示し、ユーザがチェックボックスを有効にすることにより、ユーザが選択することができる。また、設定画面７１０の「ユーザ定義」の欄には、ユーザが任意に座標値を入力できるようになっている。なお、ユーザが数値入力した値に基づいてＣＡＤ画面上に３次元の点の位置を上書き表示してもよいし、逆にＣＡＤ画面からモデル上の面やコーナーなどの特徴点をユーザがクリック等の操作で選択して判定点をＧＵＩ上で設定できるようしてもよい。判定点は、１点又は複数点設定することができるが、第１実施形態では１点とした場合について説明する。以上、ステップＳ１０３により、センサ制御装置６００のＨＤＤ６０４には、ベース座標系Ｂを基準とする判定点の座標値^Ｂｐ_ａｄｄが記憶される。
ここまでは、ユーザが判定点の設定に関与する場合について説明したが、ロボット２００に取り付けるロボットハンド２０２の形状に応じて、ロボットシステム１００が自動で判定点を設定するようにしてもよい。例えば、センサ制御部６００のＨＤＤに、ロボットハンド２０２の種類と判定点とを対応づけるためのテーブルを記憶させておき、取り付けたロボットハンド２０２の情報を取得して自動的に判定点を設定する。この場合、ユーザが判定点を設定する必要がなく、自動で判定点を設定し、ロボットの姿勢を補正する補正動作を実行することができる。

さらに、第１の閾値である閾値δ及び第２の閾値である閾値Δを設定する（Ｓ１０４）。閾値δは、後述する収束の判定に用いる閾値であり、閾値Δは、後述する形状残差の判定に用いる閾値である。ユーザは、自由に閾値δ，Δを設定することができるが、用途に応じた必要精度を勘案して閾値δ，Δを設定すればよい。図６に示す設定画面７１０には、ｍｍ単位の並進量のみで閾値を入力できるようになっており、ユーザが傾き角度を意識する必要がないため、直感的な入力ができるようになっている。以上、ステップＳ１０４により、センサ制御装置６００のＨＤＤ６０４には、閾値δ，Δが記憶される。以上のステップＳ１０１～Ｓ１０４の準備は、オフラインで実行可能な準備作業であり、ロボットシステム１００と接続されていない別のコンピュータなどで設定作業を行ってもよい。

次に、ロボットハンド２０２にステレオカメラ３００を取り付ける（Ｓ１０５）。具体的には、ロボットハンド２０２にステレオカメラ３００を把持させる。具体的な操作としては、例えばロボットアーム２０１をティーチングペンダント８００でジョグ送りしてロボットハンド２０２の掌部２１０を上向きにし、ユーザがステレオカメラ３００を掌部２１０に置く。そして、ユーザがティーチングペンダント８００を操作して、ロボットハンド２０２のフィンガ２１１，２１２を閉じることによってステレオカメラ３００をロボットハンド２０２にチャックさせる。

次に、ロボット２００、具体的にはロボットアーム２０１を、教示を開始する姿勢に動作させる（Ｓ１０６：動作ステップ、動作処理）。教示を開始するロボット２００の姿勢とは、ステレオカメラ３００の視野領域に計測対象物であるワーク保持治具４００が含まれるロボット２００の姿勢である。つまり、ロボット２００、即ちロボットアーム２０１は、ワーク保持治具４００のマークＭＫ１～ＭＫ３をステレオカメラ３００により撮像できる姿勢であれば、どのような姿勢であってもよい。この場合、ＣＰＵ５０１は、ユーザがティーチングペンダント８００を操作したときの指令に従ってロボット２００を動作させてもよいし、予めオフラインティーチングなどで設定された姿勢にロボット２００を動作させてもよい。教示点へロボット２００を動作させた後、マークＭＫ１～ＭＫ３がステレオカメラ３００の視野領域から外れている場合は、ユーザがステレオカメラ３００の画像をディスプレイ７００で確認しながら、ロボットアーム２０１をジョグ送りさせればよい。このステップ１０６により、ステレオカメラ３００の視野領域内に３点のマークＭＫ１～ＭＫ３が含まれることになる。

図７は、第１実施形態におけるロボット制御装置５００及びセンサ制御装置６００の機能ブロック図である。図１に示すロボット制御装置５００のＣＰＵ５０１は、図７に示すロボット制御部５２１として機能し、図１に示すロボット制御装置５００のＨＤＤ５０４は、図７に示す記憶部５２２として機能する。記憶部５２２には、教示点の情報５１１が記憶される。図１に示すセンサ制御装置６００のＣＰＵ６０１は、図７に示すカメラ制御部６１１、計測部６１３、演算部６１５、変換部６２１，演算部６２２、変換部６２３、演算部６２４、判定部６２６、判定部６３２、出力部６４０として機能する。図１に示すセンサ制御装置６００のＨＤＤ６０４は、図７に示す記憶部６１２、記憶部６１４、記憶部６２５、記憶部６２７、記憶部６３１として機能する。記憶部６１２には、校正値が記憶されている。記憶部６１４には、特徴点であるマークＭＫ１～ＭＫ３の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）が記憶されている。記憶部６２５には、判定点の座標値^Ｂｐ_ａｄｄが記憶されている。記憶部６２７には、閾値δが記憶されている。記憶部６３１には、閾値Δが記憶されている。

図８は、第１実施形態における教示作業のフローチャートである。第１実施形態では、組付作業をロボット２００に安定して行わせるために、ワーク保持治具４００に対してロボットハンド２０２を位置決めして、そのときのロボット２００、即ちロボットアーム２０１の姿勢を教示点として記憶させる。まず、計測部６１３は、校正値として、カメラ内部パラメータ、カメラ外部パラメータ、回転行列Ｒ_Ｃ及び並進ベクトルｔ_Ｃを記憶部６１２から取得する。また、演算部６１５、変換部６２１及び演算部６２２は、マークＭＫ１～ＭＫ３の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）を記憶部６１４から取得する。また、変換部６２３及び演算部６２４は、記憶部６２５から判定点の座標値^Ｂｐ_ａｄｄを取得する。また、判定部６２６は、記憶部６２７から閾値δを取得する。また、判定部６３２は、記憶部６３１から閾値Δを取得する。即ち、ＣＰＵ６０１は、マークＭＫ１～ＭＫ３の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）及び判定点の座標値^Ｂｐ_ａｄｄを含む各種パラメータを、ＨＤＤ６０４から読み出すことで取得する（Ｓ２０１：取得ステップ、取得処理）。

次に、計測部６１３は、ステレオカメラ３００を用いた計測によりベース座標系Ｂを基準とするマークＭＫ１～ＭＫ３の位置、即ち計測座標値を求める（Ｓ２０２：計測ステップ、計測処理）。具体的に説明すると、カメラ制御部６１１は、ステレオカメラ３００に対して撮像指令を送信し、ステレオカメラ３００にマークＭＫ１～ＭＫ３を含むワーク保持治具４００を撮像させて、ステレオカメラ３００から画像を取得する。これにより、計測部６１３は、カメラ３０１とカメラ３０２の双方の画像（ステレオ画像）を取得する。計測部６１３は、カメラ制御部６１１から取得したステレオ画像に対して画像処理及びステレオ計算処理を行い、マークＭＫ１～ＭＫ３の計測座標値を求める。

マークＭＫ１～ＭＫ３の位置を計測するための画像処理は、種々の公知手法を用いることができる。例えば、ステレオ画像それぞれの画像全体に対してエッジ抽出処理を行い、抽出したエッジの真円度や外接円半径などの形状特徴量を用いて、マークＭＫ１～ＭＫ３のエッジのみを選択する。その後、マークＭＫ１～ＭＫ３の画像上のエッジに対して楕円フィッティング処理を行い、マークＭＫ１～ＭＫ３の中心の画素座標を求める。円フィッティングでなく楕円フィッティング処理を用いたのは、ステレオカメラ３００と円形のマークＭＫ１～ＭＫ３との相対的な配置によって、投影された際に円が歪んで楕円形状に近くなる可能性があるためである。マークＭＫ１～ＭＫ３の円中心の画素座標上を求めた後、カメラ３０１の画像の画素座標とカメラ３０２の画像の画素座標の対応づけを行い、３次元の座標値の計算を行う。カメラ３０１及びカメラ３０２のカメラ内部パラメータとカメラ外部パラメータを用いることにより、センサ座標系Ｖを基準にマークＭＫ１～ＭＫ３の座標値を算出することができる。この座標値を、^Ｖｑ［ｉ］（ｉ＝１～３）とする。

さらに計測部６１３は、回転行列Ｒ_Ｃ及び並進ベクトルｔ_Ｃを用いて、ステレオカメラ３００を用いて計測したマークＭＫ１～ＭＫ３の座標値^Ｖｑ［ｉ］（ｉ＝１～３）をベース座標系Ｂを基準とする計測座標値^Ｂｑ［ｉ］（ｉ＝１～３）に座標変換する。即ち、計測部６１３は、ステレオカメラ３００を用いた計測の結果として、以下の式（４）を用いて計測座標値^Ｂｑ［ｉ］（ｉ＝１～３）を求める。

次に、判定点の座標値を変換する（演算ステップ、演算処理）。以下、この処理について具体的に説明する。まず、演算部６１５は、目標値と計測値との間のずれ量を表す変換パラメータの計算を行う（Ｓ２０３）。具体的に説明すると、演算部６１５は、ステップＳ２０３において、目標座標値^Ｂｐ［ｉ］と計測座標値^Ｂｑ［ｉ］との間のずれを示す座標変換行列である回転行列Ｒ及び並進ベクトルｔを求める（第１の演算ステップ，第１の演算処理）。図９は、特徴点の目標座標値^Ｂｐ［ｉ］と計測座標値^Ｂｑ［ｉ］との関係を説明するための模式図である。なお、図９において、目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）をｐ１，ｐ２，ｐ３、計測座標値を^Ｂｑ［ｉ］（ｉ＝１～３）をｑ１，ｑ２，ｑ３で表している。演算部６１５は、目標座標値ｐ１，ｐ２，ｐ３がそれぞれ計測座標値ｑ１，ｑ２，ｑ３に重なるように座標変換するための回転行列Ｒ及び並進ベクトルｔを求める。

ベース座標系Ｂが回転行列Ｒ及び並進ベクトルｔの分、移動した後の座標系をＢ’とすると、移動後の目標座標値^Ｂｐ’［ｉ］は以下の式（５）のように表される。

よって、移動後の目標座標値^Ｂｐ’［ｉ］がマークＭＫ１～ＭＫ３の計測座標値^Ｂｑ［ｉ］と一致するようにベース座標系Ｂを動かすためには、以下の式（６）が最小値となるような回転行列Ｒと並進ベクトルｔの最小二乗解を求めればよい。即ち、各マークＭＫ１～ＭＫ３の計測座標値^Ｂｑ［ｉ］には、ステレオカメラ３００やマークＭＫ１～ＭＫ３の個体差に依存する互いに異なる計測誤差が含まれているため、回転行列Ｒと並進ベクトルｔの最小二乗解を求めることになる。ここで、特徴点であるマークの数をＮ、１からＮまでの整数をｉとしている。即ち、演算部６１５は、式（６）が最小値となるように回転行列Ｒ及び並進ベクトルｔを求める。

この演算は、ベース－カメラ間校正で用いた点集合同士のマッチング問題と同様の手法で解くことができる。以上の演算処理により、ベース座標系Ｂのずれを示す回転行列Ｒ及び並進ベクトルｔが求まる。

次に、変換部６２１は、マークＭＫ１～ＭＫ３の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）を回転行列Ｒ及び並進ベクトルｔを用いて座標変換する。即ち、変換部６２１は、以下の式（７）に示す演算を行うことにより、座標変換後の目標座標値^Ｂｐ’［ｉ］（ｉ＝１～３）を求める。具体的には、変換部６２１は、回転行列Ｒと特徴点の目標座標値^Ｂｐ［ｉ］との掛け算を行い、その計算結果と並進ベクトルｔとの足し算を行うことで、座標変換後の目標座標値^Ｂｐ’［ｉ］を求める。

また、変換部６２３は、判定点の座標値^Ｂｐ_ａｄｄを回転行列Ｒ及び並進ベクトルｔを用いて座標変換する。即ち、変換部６２３は、以下の式（８）に示す演算を行うことにより、座標変換後の座標値^Ｂｐ’_ａｄｄを求める。具体的には、変換部６２３は、回転行列Ｒと判定点の座標値^Ｂｐ_ａｄｄとの掛け算を行い、その計算結果と並進ベクトルｔとの足し算を行うことで、座標変換後の座標値^Ｂｐ’_ａｄｄを求める。

以上、変換部６２１，６２３は、ステップＳ２０４において、目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）及び座標値^Ｂｐ_ａｄｄを、回転行列Ｒ及び並進ベクトルｔを用いて座標変換する。

次に、演算部６２２，６２４により座標変換前後の点間の距離を求める（Ｓ２０５：第２の演算ステップ、第２の演算処理）。そして、判定部６２６により収束判定を行う（Ｓ２０６）。具体的に説明すると、演算部６２２は、ステップＳ２０５において、特徴点の座標変換前後の目標座標値の距離を求める。演算部６２４は、ステップＳ２０５において、判定点の座標変換前後の座標値の距離を求める。即ち、演算部６２２は、特徴点の座標変換前の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）と、座標変換後の目標座標値^Ｂｐ’［ｉ］（ｉ＝１～３）との点間の距離ｄ［ｉ］（ｉ＝１～３）を、以下の式（９）を用いて求める。

このように演算部６２２は、ステップＳ２０５において、座標変換前後の点の差分ベクトルに対して、ノルムを演算することにより点間の距離ｄ［ｉ］を求める。

また、演算部６２４は、判定点の座標変換前の座標値ｐ_ａｄｄと座標変換後の座標値ｐ’_ａｄｄとの点間の距離ｄ_ａｄｄを、以下の式（１０）を用いて求める。この距離ｄ_ａｄｄはすなわち、目標座標値と計測座標値のずれ量を判定点の位置に換算した換算ずれ量である。

このように演算部６２４は、ステップＳ２０５において、座標変換前後の点の差分ベクトルに対して、ノルムを演算することにより点間の距離ｄ_ａｄｄを求める。即ち、判定点をベース座標系Ｂの原点以外の点とした場合には、変換部６２３は、判定点の座標値^Ｂｐ_ａｄｄを回転行列Ｒ及び並進ベクトルｔを用いて式（８）に従って座標変換する。そして、演算部６２４は、判定点の座標変換前の座標値ｐ_ａｄｄと座標変換後の座標値ｐ’_ａｄｄとの点間の距離ｄ_ａｄｄを、上記の式（１０）を用いて求める。なお、判定点をベース座標系Ｂの原点とした場合には、演算部６２４は、並進ベクトルｔのノルム｜ｔ｜、即ち長さを求めることにより点間の距離ｄ_ａｄｄを求めればよい。即ち、ｄ_ａｄｄ＝｜ｔ｜である。この場合、変換部６２３の座標変換処理を省略でき、演算負荷が軽減する。

次に、判定部６２６は、ステップＳ２０６において、点間の距離ｄ［ｉ］（ｉ＝１～３），ｄ_ａｄｄと閾値δとを比較することにより、ロボットの姿勢が目標範囲内に収束したか否かを判定する。即ち、判定部６２６は、以下の式（１１）の不等式が成り立つか否かを判定する。つまり、判定部６２６は、式（１１）の不等式で、ロボット２００の姿勢を補正するか否か、即ち補正の要否を判断する（判断ステップ、判断処理）。式（１１）の不等式が成り立たなければ、ロボット２００の姿勢を補正する必要があるということになる。式（１１）の不等式が成り立てば、ロボット２００の姿勢を補正する必要はないということになる。式（１１）には、判定点の変換後の座標値が含まれているので、判定点の変換後の座標値に基づき、補正の要否を判断していることになる。

第１実施形態では、判定部６２６は、特徴点と判定点の全点について、座標変換前後での点間の距離ｄ［ｉ］，ｄ_ａｄｄの全てが、第１の閾値以下、即ち閾値δ以下であるか否かを判定する。点間の距離ｄ［ｉ］，ｄ_ａｄｄの全てが、閾値δ以下の場合は、収束したことを示し、点間の距離ｄ［ｉ］，ｄ_ａｄｄのうち１つでも閾値δを超える場合は、収束していないことを示す。よって、距離ｄ_ａｄｄが閾値δを超える場合は収束していない。

図１０（ａ）、図１０（ｂ）、図１０（ｃ）及び図１０（ｄ）は、図８のステップＳ２０２～Ｓ２０５を説明するための模式図である。なお、図１０（ａ）～図１０（ｄ）において、第１実施形態では３次元の点群に対して処理を実行するが、便宜上、２次元で点群を図示しており、また、３点ある特徴点については２点のみを図示している。またロボットハンド２０２などの機構物は図示せずに座標系および特徴点のみを図示している。図１０（ａ）～図１０（ｄ）中、判定点の座標変換前の座標値ｐ_ａｄｄ、判定点の座標変換後の座標値ｐ’_ａｄｄ、特徴点の座標変換前の目標座標値ｐ１，ｐ２及び特徴点の座標変換後の目標座標値ｐ１’，ｐ２’は×印にて図示している。また、図１０（ａ）～図１０（ｄ）中、特徴点の計測座標値ｑ１，ｑ２は黒丸にて図示している。また、目標座標値ｐ１、目標座標値ｐ２及びベース座標系Ｂの原点を破線でつないでいるのは、これらが一体に動くことを分かりやすくするためである。

図１０（ａ）に示すように、回転行列Ｒ及び並進ベクトルｔを求めた後、図１０（ｂ）に示すように、座標変換後の座標値ｐ’_ａｄｄ，ｐ１’，ｐ２’を求める。そして、座標値ｐ１，ｐ２と座標値ｐ１’，ｐ２’との距離ｄ［１］，ｄ［２］、及び座標値ｐ_ａｄｄと座標値ｐ’_ａｄｄとの距離ｄ_ａｄｄを求める。図１０（ｃ）に示すように、距離ｄ［１］，ｄ［２］，ｄ_ａｄｄのうち１つでも閾値δを超える、即ち座標変換後の点ｐ１’，ｐ２’，ｐ’_ａｄｄを中心とする半径ｒ＝δの球の範囲から座標変換前の点が１つでも超える場合は収束していない。図１０（ｄ）に示すように、距離ｄ［１］，ｄ［２］，ｄ_ａｄｄの全てが閾値δ以下、即ち座標変換後の点ｐ１’，ｐ２’，ｐ’_ａｄｄを中心とする半径ｒ＝δの球の範囲内に座標変換前の点が全て収まっている場合は収束している。

次に、ステップＳ２０６において点間の距離ｄ［１］，ｄ［２］，ｄ_ａｄｄのうち１つでも閾値δを超える場合（Ｓ２０６：Ｎｏ）、即ち収束していない場合、出力部６４０は、判定結果と、回転行列Ｒ及び並進ベクトルｔとをロボット制御部５２１に出力する。ロボット制御部５２１は、回転行列Ｒ及び並進ベクトルｔに基づき、ロボット２００、即ちロボットアーム２０１に取らせる姿勢指令を求め（Ｓ２０７）、姿勢指令に従ってロボット２００、即ちロボットアーム２０１の姿勢を補正する（Ｓ２０８）。ここで、ロボット２００、即ちロボットアーム２０１の姿勢を補正するとは、ロボット座標系Ｏにおけるロボット２００の先端、即ちロボットハンド２０２の位置姿勢を補正することである。

以下、ステップＳ２０７の計算について具体的に説明する。ロボット制御装置５００には、ロボット２００の先端、即ちロボットハンド２０２を原点とするツール座標系Ｔが設定されている。また、ツール座標系Ｔに対するベース座標系Ｂの位置姿勢も予め設定されている。ロボット座標系Ｏに対するツール座標系Ｔの位置姿勢を行列^ＯＨ_Ｔとする。さらに、ツール座標系Ｔに対するベース座標系Ｂの位置姿勢を行列^ＴＨ_Ｂとする。補正後のロボット２００の姿勢を行列^ＯＨ_Ｔ’とすると、行列^ＯＨ_Ｔ’は、以下の式（１２）で記述できる。

行列^ＢＨ_Ｂ’は、ステップＳ２０３において計算した回転行列Ｒ及び並進ベクトルｔを元に計算した、ベース座標系Ｂの現在位置から計測位置への座標変換を表す同次変換行列であり、以下の式（１３）で表される。

即ち、ロボット制御部５２１は、ステップＳ２０７では、式（１２）及び式（１３）を用いて、ロボット２００の先端の位置姿勢を示す行列^ＯＨ_Ｔ’を計算する。次に、ロボット制御部５２１は、行列^ＯＨ_Ｔ’からロボット２００の姿勢指令、即ちロボットアーム２０１の各関節の角度指令を計算する。そして、ロボット制御部５２１は、ステップＳ２０８において、姿勢指令に従ってロボット２００を動作させ、ロボット２００の姿勢を補正する（補正動作ステップ、補正動作処理）。

ロボット２００の姿勢を補正した後は、再びステップＳ２０２～Ｓ２０５の処理を行い、ステップＳ２０６にて閾値δによる収束判定を行う。そして、ステップＳ２０６にて収束していなければ、再びステップＳ２０７、Ｓ２０８、Ｓ２０２～Ｓ２０５の処理を行う。このように、点間の距離ｄ［１］，ｄ［２］，ｄ_ａｄｄのうち全てが閾値δ以下に収束するまで、ステップＳ２０２～Ｓ２０８を繰り返す。つまり、ロボット２００の補正動作を繰り返し実行する。ロボットアーム２０１の動作の誤差やステレオカメラ３００による計測の誤差があるため、１度の補正動作では教示を完了できないことが多い。第１実施形態によれば、繰り返し補正動作を行うことによって、ロボットハンド２０２を高精度に位置決めすることができる。なお、繰り返しの回数に上限を設け、繰り返し回数が所定の回数を超えても収束しない場合はタイムアウトとして終了してもよい。

図１１（ａ）及び図１１（ｂ）は、第１実施形態における判定処理を説明するための模式図である。図１２（ａ）及び図１２（ｂ）は、比較例における判定処理を説明するための模式図である。比較例では判定点を用いずに判定処理を行う。図１１（ａ）と図１２（ａ）、図１１（ｂ）と図１２（ｂ）は、比較のため、それぞれステレオカメラ３００とマークＭＫ１～ＭＫ３との相対位置が同じ場合としている。なお、図１１（ａ）及び図１１（ｂ）においては、判定点がベース座標系Ｂの原点としている場合について図示している。

比較例では、図１２（ａ）に示すように、座標値ｐ１，ｐ２と座標値ｐ１’，ｐ２’との距離ｄ［１］，ｄ［２］が閾値δ以下であり、マークＭＫ１～ＭＫ３に対しステレオカメラ３００が傾いていても、収束した、即ちＯＫと判定される。一方、第１実施形態では、図１１（ａ）に示すように、座標値ｐ１，ｐ２と座標値ｐ１’，ｐ２’との距離ｄ［１］，ｄ［２］が閾値δ以下であっても、判定点の座標値ｐ_ａｄｄと座標値ｐ’_ａｄｄとの距離ｄ_ａｄｄが閾値δを超えている。この場合、マークＭＫ１～ＭＫ３に対しステレオカメラ３００が傾いているため、第１実施形態では収束していない、即ちＮＧと判定される。これにより、ロボット２００の姿勢の補正動作を繰り返すことになる。よって、マークＭＫ１～ＭＫ３に対してロボットハンド２０２を所定の精度範囲内に位置決めすることができる。

比較例において、判定点無しの判定方法でロボットハンド２０２を所定の精度範囲に位置決めするためには、図１２（ｂ）に示すように、閾値δよりも低い閾値δ’を設定する必要がある。この場合、適切な閾値をユーザが計算するのに手間がかかるだけでなく、閾値δよりも厳しい閾値δ’に設定する必要があるため、すぐには収束したとは判定されない。その結果、繰り返し動作の時間が余計にかかることになり、収束が遅くなる。これに対し、図１１（ｂ）に示すように、判定点を追加したことにより、閾値δを比較例のように厳しく設定しなくても、距離ｄ［１］，ｄ［２］，ｄ_ａｄｄが閾値δに早く収束し、次の処理を早く実行することができる。よって、判定点を計算上座標変換させてその位置で収束を判定することにより、マークＭＫ１～ＭＫ３に対してロボットハンド２０２を高精度かつ迅速に位置決めすることができる。

また、第１実施形態では、収束判定に用いる判定点を追加しており、仮想的な点である追加点については、回転行列Ｒ及び並進ベクトルｔを用いて、座標変換前の座標値^Ｂｐ_ａｄｄと座標変換後の座標値^Ｂｐ’_ａｄｄとの点間の距離ｄ_ａｄｄを求めている。各特徴点の計測座標値^Ｂｑ［ｉ］には、ステレオカメラ３００やマークＭＫ１～ＭＫ３の個体差に依存する互いに異なる計測誤差が含まれている。この計測誤差のばらつきが大きい場合、特徴点の目標座標値^Ｂｐ［ｉ］と計測座標値^Ｂｑ［ｉ］との距離を収束判定に用いると、閾値δに収束しない場合がある。したがって、特徴点について、閾値δを用いた判定の精度を高めるためには、判定点と同じく、座標変換前の目標座標値^Ｂｐ［ｉ］と座標変換後の目標座標値^Ｂｐ’［ｉ］との点間の距離を求めるのが好ましい。よって、第１実施形態では、収束判定に用いる指標として、特徴点についても判定点と同じく、座標変換前の目標座標値^Ｂｐ［ｉ］と座標変換後の目標座標値^Ｂｐ’［ｉ］との点間の距離ｄ［ｉ］を求めている。このため、座標変換前の特徴点の目標座標値ｐ［ｉ］及び判定点の座標値ｐ_ａｄｄと、座標変換後の特徴点の目標座標値ｐ’［ｉ］及び判定点の座標値ｐ_ａｄｄ’との関係は、計算上で座標変換を行っているだけであるので、合同となっている。特徴点及び判定点を同じ指標、即ち座標変換前後の距離を用いて判定するので、判定結果がステレオカメラ３００の計測誤差等の外乱要因の影響を受けにくくなる。即ち、特徴点の計測誤差のばらつきが大きくても、判定点と同じく、座標変換前後の点間の距離ｄ［ｉ］で収束判定を行うので、より確実に収束させることができる。

次に、ステップＳ２０６において距離ｄ［ｉ］，ｄ_ａｄｄの全てが閾値以下と判定された場合（Ｓ２０６：Ｙｅｓ）、即ち収束した場合、判定部６３２において、教示結果の信頼性を確認するための形状残差の計算（Ｓ２０９）及び判定（Ｓ２１０）を行う。判定部６３２は、ステレオカメラ３００によって計測したマークＭＫ１～ＭＫ３の計測座標値と目標座標値とに基づき、計測対象物の設計形状と計測形状との間の一致度を表す指標を演算する。具体的には、判定部６３２は、マークＭＫ１～ＭＫ３の計測座標値^Ｂｑ［ｉ］と、マークＭＫ１～ＭＫ３の座標変換後の目標座標値^Ｂｐ’［ｉ］との点間の距離ｅ［ｉ］を、式（１４）を用いて演算する。

ここで、座標変換後の目標座標値^Ｂｐ’［ｉ］は、設計形状の目標値を計測座標値^Ｂｑ［ｉ］に重なるように座標変換した結果となっているので、ｅ［ｉ］の値は、ステレオカメラ３００の全体的な位置ずれ等によっては変化することはない。一方で、何らかの誤差要因により計測座標値^Ｂｑ［ｉ］と目標座標値^Ｂｐ’［ｉ］との点間の距離ｅ［ｉ］の値が大きくなった場合、計測形状が設計形状に対して歪んでいることになる。この性質を利用して、教示の位置決め状態の信頼度を評価することができる。したがって、判定部６３２は、計測座標値^Ｂｑ［ｉ］と目標座標値^Ｂｐ’［ｉ］との距離ｅ［ｉ］が第２の閾値以下、即ち閾値Δ以下であるか否かを判定する。より具体的には、判定部６３２は、式（１５）に示すように複数のｅ［ｉ］のうち誤差が最大の距離ｅ_ｍａｘを抽出し、距離ｅ_ｍａｘが閾値Δ以下であるか否かを判定する。

距離ｅ_ｍａｘが閾値Δ以下の場合（Ｓ２１０：Ｙｅｓ）、即ち全ての距離ｅ［ｉ］が閾値Δ以下の場合は、位置決め結果が信頼できるため、そのときのロボット２００の姿勢を教示点として設定する（Ｓ２１１：設定ステップ、設定処理）。即ち、ロボット制御部５２１は、記憶部５２２に、現在のロボットアーム２０１の姿勢を教示点の情報５１１として記憶させる。このとき、予め記憶部５２２に教示点の情報５１１が記憶されていた場合には上書きすればよい。教示点の情報５１１は、ロボットアーム２０１の関節角度に対応する指令値であってもよいし、関節角度に対応する指令値を順運動学計算により変換して得られる、ロボットアーム２０１の先端の位置姿勢に対応する指令値であってもよい。関節角度は、関節を駆動するモータの回転角度であってもよい。距離ｅ_ｍａｘが閾値Δを超える場合（Ｓ２１０：Ｎｏ）は、教示の位置決め状態が信頼できないため、ロボット制御部５２１は、教示点情報を保存せずにユーザにエラー内容を提示、具体的にはディスプレイ７００にエラー内容を表示させる（Ｓ２１２）。

ステレオカメラ３００への外乱要因や経年劣化などによりステレオカメラ３００の計測の信頼性が低下していた場合や、作業ミスなどが生じる場合がある。仮に、計測の信頼性が低い状態のまま教示点情報を記憶させると、生産のためのワーク組立動作において作業開始位置ＰＡが定常的にずれ、ロボット２００の頻発停止につながる。作業ミスとは、例えばステレオカメラ３００に対する校正値の設定を誤った場合や、ワーク保持治具４００を設置すべき位置に別種のマークつき治具を設置してしまった場合などが挙げられる。また外乱要因や経年劣化としては、例えば照明条件等に起因する計測誤差、熱変形や取扱時に落下等に起因するステレオカメラ３００の歪み、ワーク保持治具４００の歪みによるマークＭＫ１～ＭＫ３の点間の形状ひずみなどが挙げられる。第１実施形態では、ステップＳ２１０～Ｓ２１２の処理により、ステレオカメラ３００の計測の信頼性が低下していた場合や作業ミスなどが生じた場合でも、ロボットアーム２０１の教示点情報が上書きされるのを防止することができる。よって、ロボットシステム１００の信頼性が更に向上する。

以上、第１実施形態によれば、判定点を追加したことにより、所望の精度を達成するための適切な閾値δを、作業者がマークＭＫ１～ＭＫ３に対するロボットハンド２０２の傾き角度の計算などの複雑な演算を意識せずに直感的かつ簡便に設定することができる。このように、作業者による閾値設定の作業負荷を低減することができる。すなわち、ユーザは傾き角度の数値自体を意識することなく、適切な傾き範囲に収まるようにロボット２００を制御することができる。特に判定点を、マークＭＫ１～ＭＫ３よりもロボットハンド２０２やステレオカメラ３００に近い点に設定することで、マークＭＫ１～ＭＫ３に対するロボットハンド２０２の傾きを補正する効果が大きくなり、収束がより早くなる。教示結果に基づいてワークＷ１とワークＷ２との嵌合作業をロボットに行わせる際には、嵌合時のワーク同士の接触箇所における位置精度が重要である。そのため、前述した把持領域とロボットハンドに把持されたワーク外形とを合算した領域内の任意の点に判定点を設けておけば、より好ましい収束判定を行うことができる。

さらに、判定点の座標変換前後の点間の距離で収束を判定することにより、並進成分と角度成分の両方で閾値判定する場合と同等の精度の位置決めを短時間で達成でき、その分、早く教示点の情報５１１を記憶部５２２に記憶させることができる。したがって、教示が早まることで、より早くロボット２００に生産作業を行わせることができ、ロボット２００の作業による生産性が向上する。

また第１実施形態においては、ステレオカメラ３００の校正を、機械的基準に対して行っている。また、ロボットアーム２０１の動作情報を用いて校正や計測を行うと誤差が生じやすいが、第１実施形態では、ロボットアーム２０１の移動によってステレオカメラ３００の位置決めを判定する上でロボットアーム２０１の制御情報を使用していない。即ち、第１実施形態では、ロボットアーム２０１の制御情報に依存せず、ベース部３１１の位置決めピン３２１，３２２を基準に校正した校正値でステレオカメラ３００の計測値を座標変換してずれ状態を評価している。そのため、マークＭＫ１～ＭＫ３に対するベース部３１１の位置決め状態、ひいてはベース部３１１に位置決めピン３２１，３２２を介して結合されているロボットハンド２０２のマークＭＫ１～ＭＫ３に対する位置決め状態を高精度に求めることができる。
なお、第一実施形態においては、掌部２１０側とワーク保持治具４００の間の双方における誤差が所定の範囲に入るように、特徴点と判定点の全点について、座標変換前後での点間の距離ｄ［ｉ］，ｄ_ａｄｄの全てが閾値以下であるか否かを判定した。仮に掌部２１０における位置精度のみが重要であり姿勢の精度は不要であるケースにおいて適用する場合には、判定点に関する座標変換前後の距離ｄ_ａｄｄのみで収束を判定することも可能である。

［第２実施形態］
次に、本発明の第２実施形態に係るロボットシステムについて説明する。第１実施形態では、ロボットの教示方法について説明したが、第２実施形態では、実際に部品の組み立て等の生産作業を行う場合のロボットの制御方法について説明する。図１３（ａ）は、第２実施形態において組み付け作業に用いられる第１のワークＷ１１及び第２のワークＷ１２の説明図である。また、図１３（ｂ）は、第２実施形態におけるロボットの構成を示す模式図である。第２実施形態では、ワークＷ１１に対してワークＷ１２を組付ける作業をロボット２００に行わせる。ロボット２００の先端、即ちロボットハンド２０２の掌部２１０の側面には、ステレオカメラ３００が固定されている。このようにステレオカメラ３００は、ロボットハンド２０２に固定されることで、ロボット２００に支持されている。

ワークＷ１１は、第１の部材Ｗ１０１と第２の部材Ｗ１０２とを不図示のビスなどの接合部で接合して形成されており、第１の部材Ｗ１０１に対する第２の部材Ｗ１０２の位置がビス穴のガタの範囲でバラついている。第１の部材Ｗ１０１の上には、３つピンＷ１０３と、特徴点として、３つの基準穴Ｗ１０４が設けられている。ワークＷ１２には、ワークＷ１１のピンＷ１０３に対応する３つの穴部Ｗ１１１が設けられており、ワークＷ１１との組み付けが可能となっている。これらのワークＷ１１，Ｗ１２に対して、ロボット２００を用いて組み付けを行う。

第２実施形態におけるロボットシステムの構成は、ステレオカメラ３００がロボットハンド２０２に固定されている以外、第１実施形態のロボットシステム１００とほぼ同様である。なお、ロボットハンド２０２は、ワークＷ１２を把持するのに適した平行グリッパとなっている。即ち、ロボットハンド２０２は、第１実施形態と同様、掌部２１０と、掌部２１０に対して開閉移動する一対のフィンガ２１１，２１２を有する。ステレオカメラ３００は、ロボットハンド２０２がワークＷ１２をワークＷ１１に組み付ける直前の位置において、ワークＷ１１の基準穴Ｗ１０４を計測可能な位置に設けられたステレオカメラである。この組み付け直前の位置において、ステレオカメラ３００の視野領域にピンＷ１０３は含まれていないが、基準穴Ｗ１０４は、ステレオカメラ３００の視野領域に含まれている。よって、第２実施形態では、基準穴Ｗ１０４をステレオカメラ３００を用いて計測することで、間接的に位置合わせを実施する。

図１４は、第２実施形態において組付作業を行うための事前準備を示すフローチャートである。以下、図１４のフローチャートに沿って処理内容を説明するが、第１実施形態と共通する部分は適宜省略する。まず、カメラ校正値を設定する（Ｓ３０１）。このステップＳ３０１は、第１実施形態で説明したステップＳ１０１と同様に、ステレオカメラ３００の校正値として、ステレオカメラ校正とベース－カメラ間校正の２種類の校正を事前に実施する。ステレオカメラ校正値に関しては、第１実施形態と同様である。ベース－カメラ間校正は、第１実施形態で説明した校正治具９００とは異なる校正治具を用いて行う。

図１５は、第２実施形態において校正に用いる校正治具９００Ｂの説明図である。校正治具９００Ｂは、ベース部９０１Ｂ、支柱部９０２Ｂ，９０３Ｂ、上板部９０４Ｂ、及び第１実施形態と同様の基準パターン部９０５を有する。ベース部９０１Ｂと上板部９０４Ｂとは、互いに対向するように配置された板状の部材であり、支柱部９０２Ｂ，９０３Ｂで接続されている。ベース－カメラ間校正は、図１５に示すように、ロボットハンド２０２とステレオカメラ３００とを一体にした状態で校正治具９００Ｂに固定して、第１実施形態と同様の基準パターン部９０５を撮像することにより校正を行う。第２実施形態においてはベース座標系Ｂの原点を、ロボットアームに対するロボットハンド２０２の取り付け位置、即ちツール座標系Ｔと同じ位置姿勢に設定している。第１実施形態と同様、校正治具９００Ｂ内においてベース座標系Ｂと基準パターン部９０５のマークの位置関係を計測する。これにより、ベース座標系Ｂに対するセンサ座標系Ｖの相対的な位置姿勢を高精度に求めることができる。ここで、ベース座標系Ｂに対するロボットハンド２０２のフィンガ２１１，２１２の位置は機械的に所定の精度範囲内で調整されている。

次に、目標座標値を設定する（Ｓ３０２）。目標座標値として、組付け直前の位置姿勢におけるツール座標系Ｔ、即ちベース座標系Ｂに対する基準穴１０４の設計上の狙い位置を設定する。例えば、ピン１０３に対して穴部１１１の嵌合が開始する位置の１ｍｍ上空とする目標座標値を設定する。また別の方法としては、ロボットハンド２０２をサンプルワークの実物に対して組み付け直前の位置に位置決めし、ステレオカメラ３００による撮像を行って基準穴１０４の位置を記憶し、目標座標値として用いてもよい。

次に、判定点を設定する（Ｓ３０３）。例えば、判定点として３つの穴部Ｗ１１１の設計上の位置を設定する。即ち、それぞれの判定点を座標値^Ｂｐ_ａｄｄ［ｉ］（ｉ＝１～３）としてＨＤＤ６０４に保存する。

次に、閾値δ，Δを設定する（Ｓ３０４）。この作業は第１実施形態のステップＳ１０４と同様である。閾値δは、穴部Ｗ１１１とピンＷ１０３の嵌合公差やＣ面取りの寸法などを考慮して、例えば０．１ｍｍといった値を設定する。また閾値Δは、ステレオカメラ３００の計測精度、及び基準穴Ｗ１０４の寸法公差を考慮して設定する。例えば、基準穴Ｗ１０４の位置寸法公差が０．０５ｍｍ、ステレオカメラ３００の計測精度が０．０５ｍｍである場合は、合計０．１ｍｍに対してマージンを取り、０．２ｍｍといった値を閾値Δとして設定する。

図１６は、第２実施形態における組付作業のフローチャートである。なお実際の生産においては、順次流れてくるワークに対して組付作業を繰り返し行うが、図１６では１回分の組付作業を示している。

まず、第１実施形態と同様、ＣＰＵ６０１は、特徴点の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）及び判定点の座標値^Ｂｐ_ａｄｄ［ｉ］（ｉ＝１～３）を含む各種パラメータを、ＨＤＤ６０４から読み出すことで取得する（Ｓ４０１：取得ステップ、取得処理）。

次に、ＣＰＵ６０１は、ＣＰＵ５０１に指令を送り、ロボットアーム２０１を不図示の供給部に移動させ、ワークＷ１２をロボットハンド２０２に把持させる。ロボットハンド２０２のフィンガ２１１，２１２によりワークＷ１２が位置決めされ、所定の精度範囲内で把持を行う。

次に、ＣＰＵ６０１は、予め教示された組付開始位置を示す教示点へロボット２００、即ちロボットアーム２０１を動作させる（Ｓ４０２：動作ステップ、動作処理）。組付開始位置を示す教示点とは、ステレオカメラ３００による繰り返し制御を開始するロボット２００の姿勢であって、基準穴Ｗ１０４がステレオカメラ３００の視野領域に入る教示点である。

次に、ＣＰＵ６０１は、ステレオカメラ３００によって基準穴Ｗ１０４の位置計測を行う（Ｓ４０３：計測ステップ、計測処理）。即ち、ステップＳ２０２と同様、ステレオカメラ３００を用いた計測によりベース座標系Ｂを基準とする基準穴Ｗ１０４の計測座標値^Ｂｑ［ｉ］を求める。

次に、ＣＰＵ６０１は、ステップＳ２０３と同様、目標値と計測値との間のずれ量の計算を行う（Ｓ４０４）。即ち、ＣＰＵ６０１は、ステップＳ４０４において、目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）と計測座標値^Ｂｑ［ｉ］（ｉ＝１～３）との間のずれを示す座標変換行列である回転行列Ｒ及び並進ベクトルｔを求める（第１の演算ステップ，第１の演算処理）。

次に、ＣＰＵ６０１は、ステップＳ２０４と同様、特徴点の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）を回転行列Ｒ及び並進ベクトルｔを用いて座標変換して、目標座標値^Ｂｐ’［ｉ］（ｉ＝１～３）を求める（Ｓ４０５）。また、ＣＰＵ６０１は、ステップＳ４０５において、ステップＳ２０４と同様、判定点の座標値^Ｂｐ_ａｄｄ［ｉ］（ｉ＝１～３）を回転行列Ｒ及び並進ベクトルｔを用いて座標変換して、座標値^Ｂｐ’_ａｄｄ［ｉ］（ｉ＝１～３）を求める。

なお、第２実施形態では判定点が複数存在するため、ＣＰＵ６０１は、以下の式（１６）を用いて座標変換を行う。

次に、ＣＰＵ６０１は、第１実施形態のステップＳ２０５と同様、座標変換前後の点間の距離を求める（Ｓ４０６：第２の演算ステップ、第２の演算処理）。具体的には、ＣＰＵ６０１は、第１実施形態と同様の演算で、座標変換前の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）と、座標変換後の目標座標値^Ｂｐ’［ｉ］（ｉ＝１～３）との点間の距離ｄ［ｉ］（ｉ＝１～３）を求める。また、ＣＰＵ６０１は、座標変換前の座標値^Ｂｐ_ａｄｄ［ｉ］（ｉ＝１～３）と、座標変換後の目標座標値^Ｂｐ’_ａｄｄ［ｉ］（ｉ＝１～３）との点間の距離ｄ_ａｄｄ［ｉ］（ｉ＝１～３）を、以下の式（１７）を用いて求める。

次に、ＣＰＵ６０１は、第１実施形態のステップＳ２０６と同様、点間の距離ｄ［ｉ］（ｉ＝１～３），ｄ_ａｄｄ［ｉ］（ｉ＝１～３）と閾値δとを比較することにより、収束したか否かを判定する（Ｓ４０７）。即ち、ＣＰＵ６０１は、以下の式（１８）の不等式が成り立つか否かを判定する。

点間の距離ｄ［ｉ］，ｄ_ａｄｄ［ｉ］の全てが、閾値δ以下の場合は、収束したことを示し、点間の距離ｄ［ｉ］，ｄ_ａｄｄ［ｉ］のうち１つでも閾値δを超える場合は、収束していないことを示す。

ＣＰＵ６０１は、ステップＳ４０７において、点間の距離ｄ［１］，ｄ_ａｄｄ［ｉ］のうち１つでも閾値δを超える場合（Ｓ４０７：Ｎｏ）、判定結果と、回転行列Ｒ及び並進ベクトルｔとをロボット制御装置５００のＣＰＵ５０１に出力する。ＣＰＵ５０１は、第１実施形態のステップＳ２０７と同様、回転行列Ｒ及び並進ベクトルｔに基づき、ロボット２００、即ちロボットアーム２０１に取らせる姿勢指令を求める（Ｓ４０８）。次に、ＣＰＵ５０１は、第１実施形態のステップＳ２０８と同様、姿勢指令に従ってロボット２００、即ちロボットアーム２０１の姿勢を補正する（Ｓ４０９：姿勢補正ステップ、姿勢補正処理）。そして、ステップＳ４０２の処理に戻り、ステップＳ４０７にて収束したと判定されるまでロボット２００の補正動作を繰り返し行う。ＣＰＵ６０１は、収束したと判定した場合、即ち閾値δ以下と判定した場合（Ｓ４０７：Ｙｅｓ）、第１実施形態のステップＳ２０９と同様、形状残差計算を行い（Ｓ４１０）、第１実施形態のステップＳ２１０と同様、閾値Δと比較判定する（Ｓ４１１）。

ＣＰＵ６０１は、ステップＳ４１１において、距離ｅ_ｍａｘが閾値Δ以下と判定した場合は、ロボット２００に予め定められた所定の作業、即ち組付作業を行わせる（Ｓ４１２：作業ステップ、作業処理）。具体的には、ロボットハンド２０２を下降させて、ワークＷ１２をワークＷ１１に組み付ける組み付け作業を行わせる。これにより、穴部１１１のすべてがピン１０３に十分に一致した状態で組み付けを行うことができ、組付時間を短縮することができ、生産性が向上する。

ＣＰＵ６０１は、ステップＳ４１１において、距離ｅ_ｍａｘが閾値Δを超える判定した場合は、ユーザにエラー内容を提示、具体的にはディスプレイ７００にエラー内容を表示させ、ロボット２００を一時停止させる（Ｓ４１３）。これにより、ロボット２００の組付作業の信頼性が向上する。

なお、ワークＷ１１の加工ばらつきが大きいことが事前に分かっている場合などには、エラー停止とせずにワークＷ１１の入れ替え動作を行って、新しいワークＷ１１に対して再度ステップＳ４０２から実行するようにしてもよい。

以上、第２実施形態においては、多点の同時嵌合に対して収束判定のための条件を判定点における距離の閾値δによって設定しており、さらに判定点としてステレオカメラ３００の視野領域外にも設定ができる。このように多点の嵌合を実施するようなケースにおいては特に、従来のように位置と角度によって収束判定を実施すると判定条件の計算が複雑になりがちであるが、第２実施形態によれば、閾値δを簡便に設定できる。さらに、全点において所望の精度範囲に位置決めを完了してからロボット２００に組付作業を行わせるため、高精度で確実性の高い組付作業をロボット２００に行わせることができる。また第１実施形態と同様に、ステレオカメラ３００の校正を機械的基準に対して行っているため、ロボットアーム２０１の動作精度に関係なくロボットハンド２０２の位置決め状態を判定することができる。さらに、ステレオカメラ３００によるずれの計測結果に基づいて繰り返しの収束動作を行い、所定の精度範囲に収束したことを確認してから組付作業を行っている。そのため、ロボットアーム２０１の動作に誤差があってもその誤差の影響が低減され、所定の位置決め精度を満たした状態で組付作業をロボット２００に行わせることができる。また、特徴点においては、第１実施形態と同様に、特徴点の座標変換前後の点間の距離、すなわち合同な点群間での距離を用いた判定方法を採用したことにより、計測誤差等の外乱要因の影響を受けにくく、適切な閾値δを定めることができる。更に、第１実施形態と同様、ステップＳ４１１の処理により、確実性の高い組立作業を実行することができる。
第２実施形態においては、ステップＳ４０７における収束判定する際に、点間の距離ｄ［ｉ］，ｄ_ａｄｄ［ｉ］の全てを閾値δと比較して判定する場合について記載した。しかしワークの嵌合を考えると３つの穴部Ｗ１１１の位置が特に重要であるので、穴部Ｗ１１１の位置に対応した判定点の座標変換前後の距離ｄ_ａｄｄ［ｉ］のみを閾値δと比較して判定しても良い。なお、判定点のみを用いて位置と姿勢の両方を判定したい場合は、本実施形態のように判定点が３点以上あることが好ましい。

［第３実施形態］
次に、本発明の第３実施形態に係るロボットシステムについて説明する。図１７は、第３実施形態における演算処理を説明するための模式図である。なお、第３実施形態におけるロボットシステムの構成は、第１実施形態のロボットシステムと同様であり、説明を省略する。

第３実施形態では、距離の計算が、第１実施形態で説明したステップＳ２０５の距離の計算と異なる。具体的に説明すると、第１実施形態では、ＣＰＵ６０１は、座標変換前の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）と、座標変換後の目標座標値^Ｂｐ’［ｉ］（ｉ＝１～３）との点間の距離ｄ［ｉ］（ｉ＝１～３）を、式（９）を用いて求めていた。第３実施形態では、ＣＰＵ６０１は、ステップＳ２０５において距離ｄ［ｉ］の代わりに、特徴点の目標座標値^Ｂｐ［ｉ］（ｉ＝１～３）と、特徴点の計測座標値^Ｂｑ［ｉ］（ｉ＝１～３）との距離ΔＰ［ｉ］（ｉ＝１～３）を、以下の式（１９）を用いて求める。

即ち、図１７に示すように、目標座標値ｐ１と計測座標値ｑ１との距離ΔＰ［１］及び目標座標値ｐ２と計測座標値ｑ２との距離ΔＰ［２］を求める。

次に、ＣＰＵ６０１は、ステップＳ２０６では、上記の式（１１）の代わりに、以下の式（２０）の不等式が成り立つか否かを判定する。

以上、ステレオカメラ３００による計測の誤差のばらつきが小さい場合には、閾値δを用いた収束判定に、特徴点の目標座標値と計測座標値との距離を用いても、第１実施形態と同様の作用効果を奏する。なお、第３実施形態の距離の計算を、第２実施形態で説明したステップＳ４０６の距離の計算に適用してもよい。

なお、本発明は、以上説明した実施形態に限定されるものではなく、本発明の技術的思想内で多くの変形が可能である。また、実施形態に記載された効果は、本発明から生じる最も好適な効果を列挙したに過ぎず、本発明による効果は、実施形態に記載されたものに限定されない。

第１～第３実施形態では、特徴点を測定して測定座標値を取得する視覚センサとしてステレオカメラ３００を用いた場合について説明したがこれに限定するものではない。例えば視覚センサとして、プロジェクタによるパターン投光法、レーザー切断法、Ｔｉｍｅ－Ｏｆ－Ｆｌｉｇｈｔ法などの方式を用いた視覚センサを用いてもよい。その際、視覚センサは、３次元の位置姿勢の計測を可能とする３次元視覚センサであるのが好ましい。

また、第１実施形態では判定点を１点、第２実施形態では判定点を３点としたが、これらの点数に限定するものではなく、２点であっても４点以上であってもよい。また、第１実施形態では特徴点がマークであり、第２実施形態では特徴点が基準穴である場合について説明したがこれに限定するものではなく、特徴点として識別可能な点であればどのような点であってもよい。また、特徴点の数が３点の場合について説明したが、この点数に限定するものではない。なお、特徴点の点数は３点以上であるのが好ましい。

また、第２実施形態において、ステレオカメラ３００の取り付け位置がロボットハンド２０２の場合について説明したが、これに限定するものではなく、例えばロボットアーム２０１の先端に取り付けてもよく、また、ロボットハンド２０２に内蔵させてもよい。

また第２実施形態においてロボットハンド２０２によってワークＷ１２を再現性よく所定の位置で把持できる場合の例について説明したが、把持位置のばらつきが大きい場合であってもよい。この場合には、把持位置のばらつきも計測して繰り返し制御の目標位置および判定点の位置を補正するような構成としてもよい。たとえば第２実施形態の変形例として、ロボット２００と連動しない位置に別の視覚センサを設け、穴部Ｗ１１１を別の視覚センサで計測して把持位置のずれを計測するようにしてもよい。その場合は、別の視覚センサの計測値に基づいてツール座標系Ｔに対する目標値および判定点の位置を座標変換によって補正することで、把持位置がずれても同様の収束判定を行うことができる。

また、第１～第３実施形態においては、処理のリアルタイム性については限定していないが、ロボット制御装置で低速の繰り返し制御に適用してもよいし、ビジュアルサーボと言われる高速なリアルタイムのビジュアルフィードバック制御に適用してもよい。

本発明は、上述の実施形態の１以上の機能を実現するプログラムを、ネットワーク又は記憶媒体を介してシステム又は装置に供給し、そのシステム又は装置のコンピュータにおける１つ以上のプロセッサーがプログラムを読出し実行する処理でも実現可能である。また、１以上の機能を実現する回路（例えば、ＡＳＩＣ）によっても実現可能である。

１００…ロボットシステム、２００…ロボット、３００…ステレオカメラ（視覚センサ）、４００…ワーク保持治具（計測対象物）、５０１，６０１…ＣＰＵ（制御部）、Ｂ…ベース座標系（視覚センサの座標系）、ＭＫ１～ＭＫ３…マーク（特徴点）

Claims (22)

1. ロボット装置に設けられた視覚センサにより特徴点を計測し、前記ロボット装置を制御する制御方法であって、
   前記特徴点の位置関係に関連する目標座標値を取得し、
   前記目標座標値とは別に、前記ロボット装置の動作に追従して移動する判定点を取得し、
   前記特徴点を前記視覚センサにより計測し、前記視覚センサにより計測された際の前記特徴点の座標である計測座標値を取得し、
   前記目標座標値を前記計測座標値に近づけるように、前記目標座標値と前記判定点とを座標変換し、
   座標変換される前の前記判定点と座標変換された後の前記判定点との差に応じた第１の値を取得し、
   前記第１の値が、所定の閾値以下であるか判定し、
   前記第１の値が前記所定の閾値より大きい場合、前記ロボット装置の姿勢を補正する、
   ことを特徴とする制御方法。
2. 前記ロボット装置はロボットハンドを有するロボットであって、
   前記判定点は、前記ロボットハンドが把持可能な把持領域と、把持されたワーク外形とを合算した領域内の任意の点である、
   ことを特徴とする請求項１に記載の制御方法。
3. 前記判定点は、前記視覚センサの座標系の原点として設定されている、
   ことを特徴とする請求項１または２に記載の制御方法。
4. 座標変換される前の前記目標座標値と座標変換された後の前記目標座標値との差に応じた第２の値を取得し、
   前記第２の値が、第１の閾値以下であるか判定し、
   前記第２の値が前記第１の閾値より大きい場合、前記ロボット装置の姿勢を補正する、
   ことを特徴とする請求項１乃至３の中のいずれか１項に記載の制御方法。
5. 前記目標座標値を前記計測座標値に近づけるように前記目標座標値を座標変換する座標変換パラメータを取得し、
   前記座標変換パラメータを用いて、前記目標座標値と前記判定点とを座標変換する、
   ことを特徴とする請求項１乃至４の中のいずれか１項に記載の制御方法。
6. 前記座標変換パラメータは、前記目標座標値を前記計測座標値に変換する回転行列及び並進ベクトルを含む座標変換行列である、
   ことを特徴とする請求項５に記載の制御方法。
7. 前記視覚センサは、第１及び第２のカメラを有するステレオカメラである、
   ことを特徴とする請求項１乃至６の中のいずれか１項に記載の制御方法。
8. 前記ロボット装置がロボットハンドを備えており、
   前記視覚センサは、前記ロボットハンドに固定されている、
   ことを特徴とする請求項１乃至７の中のいずれか１項に記載の制御方法。
9. 前記ロボット装置がロボットハンドを備えており、
   前記視覚センサは、前記ロボットハンドに、位置決め機構にて位置決めされた状態で把持される、
   ことを特徴とする請求項１乃至７の中のいずれか１項に記載の制御方法。
10. 前記計測座標値と座標変換された後の前記目標座標値との差に応じた第３の値を取得し、
    前記第３の値が、第２の閾値より大きければ、エラーを通知する、
    ことを特徴とする請求項１乃至９の中のいずれか１項に記載の制御方法。
11. 座標変換される前の前記目標座標値と座標変換された後の前記目標座標値との差に応じた第２の値を取得し、
    前記第１の値と前記第２の値のどちらか一方の値が前記所定の閾値より大きい場合、前記座標変換パラメータに基づき、前記ロボット装置の姿勢を補正し、
    前記第１の値及び前記第２の値が、それぞれ前記所定の閾値以下となるまで、前記特徴点を前記視覚センサにより計測し、前記特徴点の計測座標値を取得し、
    前記目標座標値と前記計測座標値とが一致するように、前記目標座標値を座標変換する座標変換パラメータを取得し、
    前記座標変換パラメータにより座標変換される前の前記判定点と前記座標変換パラメータにより座標変換された後の前記判定点との差に応じた第１の差と、
    前記座標変換パラメータにより座標変換される前の前記目標座標値と前記座標変換パラメータにより座標変換された後の前記目標座標値との差に応じた第２の差と、を取得し、
    前記第１の差と、前記第２の差とが、それぞれ前記所定の閾値以下であるか判定し、
    前記第１の差と前記第２の差のどちらか一方の値が前記所定の閾値より大きい場合、前記座標変換パラメータに基づき、前記ロボット装置の姿勢を補正する動作を繰り返す、
    ことを特徴とする請求項５に記載の制御方法。
12. 前記計測座標値と座標変換された後の前記目標座標値との差に応じた第３の値を取得し、
    前記第３の値が、第２の閾値以下である場合、ワークに対する所定の作業を前記ロボット装置に行わせる、
    ことを特徴とする請求項１乃至９の中のいずれか１項に記載の制御方法。
13. 前記計測座標値と座標変換された後の前記目標座標値との差に応じた第３の値を取得し、
    前記第３の値が、第２の閾値以下である場合、その状態の前記ロボット装置の姿勢を教示点に設定する、
    ことを特徴とする請求項１乃至９の中のいずれか１項に記載の制御方法。
14. 請求項１乃至１３の中のいずれか１項に記載の制御方法の各ステップを、コンピュータに実行させるプログラム。
15. 請求項１４に記載のプログラムを記録した、コンピュータにより読み取り可能な記録媒体。
16. ロボット装置と、
    前記ロボット装置に支持される視覚センサと、
    前記ロボット装置と前記視覚センサとを制御する制御部と、
    を備えるロボットシステムであって、
    前記制御部が、
    特徴点の位置関係に関連する目標座標値を取得し、
    前記目標座標値とは別に、前記ロボット装置の動作に追従して移動する判定点を取得し、
    前記特徴点を前記視覚センサにより計測し、前記視覚センサにより計測された際の前記特徴点の座標である計測座標値を取得し、
    前記目標座標値を前記計測座標値に近づけるように、前記目標座標値と前記判定点とを座標変換し、
    座標変換される前の前記判定点と座標変換された後の前記判定点との差に応じた第１の値を取得し、
    前記第１の値が、所定の閾値以下であるか判定し、
    前記第１の値が前記所定の閾値より大きい場合、前記ロボット装置の姿勢を補正する、
    ことを特徴とするロボットシステム。
17. 前記制御部に対するユーザの入力を受け付ける入力部をさらに備え、
    前記制御部は、前記入力部を介して前記判定点の設定情報を取得し、
    ユーザにより設定された前記判定点と、前記特徴点とを用いて、前記ロボット装置を制御する、
    ことを特徴とする請求項１６に記載のロボットシステム。
18. 前記判定点の座標値を設定するための設定画面を表示する表示部をさらに有し、
    前記表示部は、前記設定画面において複数の判定点候補を表示し、複数の判定点候補のいずれか１つをユーザに選択させる、
    ことを特徴とする請求項１６または１７に記載のロボットシステム。
19. 前記表示部は、三次元画像を表示する表示領域を有しており、
    前記表示領域に、前記特徴点と前記判定点との相対関係を表示する、
    ことを特徴とする請求項１８に記載のロボットシステム。
20. 前記表示領域は、前記ロボットおよび前記視覚センサのうち少なくともいずれか一方と、前記特徴点と、前記判定点と、を表示する、
    ことを特徴とする請求項１９に記載のロボットシステム。
21. 前記視覚センサは、第１及び第２のカメラを有するステレオカメラである、
    ことを特徴とする請求項１６乃至２０の中のいずれか１項に記載のロボットシステム。
22. 請求項１６乃至２１の中の何れか１項に記載のロボットシステムを用いて物品の製造を行う、
    ことを特徴とする物品の製造方法。

Images