本発明は、撮像手段によって撮像された画像を用いて、移動対象物を移動目標位置に移動させる視覚フィードバック装置及び方法に関する。

撮像手段によって撮像された画像を用いて、移動対象物を移動目標位置に誘導させる視覚フィードバックに関する発明が特許文献１に開示されている。 特許文献１には、ロボットハンドが把持したペグをホールに誘導する、いわゆるペグインホールの例が記載されており、特に、カメラで取得したシーンの画像を元に、ペグをホールに移動させる際のペグとホールの位置を正確に検出・追跡する技術について記載されている。

特開２００２−２０８００９号公報 図１１は、特許文献１に開示された装置のシステム構成図である。 図において、２５０１はペグであり、ロボット２５０５に固定されたロボットハンド２５０４に把持されて３次元空間を移動する。 ２５０２はホールであり、ワーク２５０３に貫通した穴である。 ２５０６は左カメラ、２５０７は右カメラであり、それぞれがカメラ台２５０８に固定され、ペグ２５０１とホール２５０２を含むシーン画像２５０９を撮像する。 シーン画像２５０９には、左カメラ２５０６によって撮像される左シーン画像２５１０と、右カメラ２５０７によって撮像される右シーン画像２５１１がある。 ２５１２は左画像処理装置、２５１３は右画像処理装置で、それぞれが予め記憶手段に保存されているテンプレート画像２５１４を用いて、左シーン画像２５１０、右シーン画像２５１１に対してテンプレートマッチングを行い、左シーン画像２５１０、右シーン画像２５１１内のテンプレート画像２５１４の位置を検出する。 テンプレート画像２５１４は、ペグ２５０１先端に対応するペグテンプレート画像２５１５とホール２５０２に対応するホールテンプレート画像２５１６から構成されているため、左画像処理装置２５１２、右画像処理装置２５１３は、左シーン画像２５１０、右シーン画像２５１１内のペグ２５０１先端位置とホール２５０２位置を検出する。 ２５１７は制御装置で、左シーン画像２５１０内のペグ２５０１の位置とホール２５０２の位置、右シーン画像２５１１内のペグ２５０１の位置とホール２５０２の位置から、ペグ２５０１とホール２５０２の３次元位置を演算し、それら３次元位置に基づいてロボット２５０５を動作させる指令を生成し、ロボット２５０５を動作させる。 ペグ２５０１とホール２５０２の３次元位置は、一般に広く知られている三角測量の原理に基づいたステレオ視により求めることができる。 このように、左カメラ２５０６、右カメラ２５０７によって撮像された左シーン画像２５１０、右シーン画像２５１１を用いて、ペグ２５０１先端位置とホール２５０２位置の３次元位置を逐次演算し、その３次元位置に基づいてペグ２５０１を移動させるので、ペグ２５０１をホール２５０２まで移動させることができる。

しかしながら、特許文献１に記載された視覚フィードバック装置では、移動対象物（ペグ）を移動目標（ホール）まで誘導させるにあたり、逐次、テンプレートマッチングにより移動対象物と移動目標を追跡し続ける必要があるため、移動対象物の移動の速度は、テンプレートマッチングによる追跡速度によって制限されるので、あまり大きな速度で移動させることができず、作業効率の低下を招くという問題があった。
また、特許文献１に記載の装置では、撮像手段に固定焦点距離のレンズを装着しているため、移動対象物と移動目標の間の距離に関わらず、撮像される画像の視野が固定されているので、前記画像の１画素当たりの位置分解能が、前記距離に関わらず、常に同じとなる。 一方、移動対象物が移動目標に近付くにつれ、位置の検出精度を高くすることが望まれている。 しかし、位置分解能が一定なので検出精度を高くすることができず、その結果、挿入作業等の成功率が低いという問題があった。
本発明は、このような問題点に鑑みてなされたものであり、撮像手段によって撮像された画像を用いた移動対象物の移動目標への移動を、高速に行い、また、移動対象物位置が移動目標位置に近付くにつれて、位置検出精度を高くして、作業効率を向上させることができる視覚フィードバック装置及び方法を提供することを目的とする。

上記問題を解決するために、請求項１に記載の発明は移動対象物と移動目標を含むシーン画像を撮像する撮像手段を備え、前記シーン画像と予め保存しておいたテンプレート画像を用いて、テンプレートマッチングを行い、前記移動対象物を前記移動目標に誘導する視覚フィードバック装置において、前記移動対象物と前記移動目標位置の距離を演算する距離演算手段を備え、前記距離演算手段で求めた距離が、所定の速度条件距離より大きい場合には、前記テンプレートマッチングを行わないで、前記移動対象物を高速で移動させ、前記距離が前記速度条件距離よりも小さい場合にはテンプレートマッチングを行いながら、前記移動対象物を前記移動目標に誘導するものである。

また、請求項２に記載の発明は前記撮像手段を複数個備えるとともに、前記複数の撮像手段を固定する固定台と、前記シーン画像と前記テンプレート画像を用いてテンプレートマッチングを行い、前記移動対象物の位置と前記移動目標位置を検出する複数のマッチング手段と、前記複数のマッチング手段の出力を受けて、前記移動対象物の３次元位置と前記移動目標位置の３次元位置を演算するステレオ演算手段を備えるものである。

また、請求項３に記載の発明は前記距離演算手段で求めた距離に応じて高速移動指令を生成する高速指令生成手段を備えるものである。

また、請求項４に記載の発明は前記撮像手段に、焦点距離を可変にできるズームレンズと、パン・チルト機構を備えるととともに、前記ズームレンズの焦点距離に応じて、前記テンプレート画像のサイズを変更するテンプレートサイズ変更手段と、前記移動対象物位置と前記移動目標位置の中点が、前記シーン画像の中央に位置するように、前記パン・チルト機構を制御するパン・チルト制御手段と、前記移動対象物位置と前記移動目標位置の各位置が、前記シーン画像の縦方向または横方向の広範囲に位置付けられるように、前記ズームレンズの焦点距離を制御するズーム制御手段とを備えるものである。

また、請求項５に記載の発明は前記撮像手段と前記ズームレンズを複数個備えるとともに、前記複数の撮像手段を固定する固定台と、前記ステレオ演算手段で必要となる各々の前記撮像手段の位置関係を表す行列であって、各々の前記ズームレンズの焦点距離に応じて前記行列を出力できるように構成されたキャリブレーションテーブルとを備えるものである。

また、請求項６に記載の発明は前記テンプレート画像を予め保存する際の前記ズームレンズの焦点距離に対する、前記ズーム制御手段によって変更された前記ズームレンズの焦点距離の比を用いて前記テンプレート画像のサイズを変更するものである。

また、請求項７に記載の発明は撮像手段で移動対象物と移動目標を含むシーン画像を撮像し、前記シーン画像と予め保存しておいたテンプレート画像を用いて、テンプレートマッチングを行い、前記移動対象物を前記移動目標に誘導する視覚フィードバック方法において、前記移動対象物と前記移動目標位置の距離を演算するステップと、前記距離が、所定の速度条件距離より大きい場合に、前記テンプレートマッチングを行わないで、前記移動対象物を高速で移動させるステップと、前記距離が前記速度条件距離よりも小さい場合にはテンプレートマッチングを行いながら、前記移動対象物を前記移動目標に誘導するステップを有するものである。

また、請求項８に記載の発明は異なる複数の方向から複数のシーン画像を撮像するステップと、前記複数のシーン画像についてテンプレートマッチングを行うステップと、
前記テンプレートマッチングの結果に基づいて前記移動対象物の３次元位置と前記移動目標位置の３次元位置を演算するステップを備えるものである。

また、請求項９に記載の発明は前記距離に応じて高速移動指令を生成するものである。

また、請求項１０に記載の発明は前記撮像手段に、焦点距離を可変にできるズームレンズと、パン・チルト機構を備えるととともに、前記ズームレンズの焦点距離に応じて、前記テンプレート画像のサイズを変更するステップと、前記移動対象物位置と前記移動目標位置の中点が、前記シーン画像の中央に位置するように、前記パン・チルト機構を制御するステップと、前記移動対象物位置と前記移動目標位置の各位置が、前記シーン画像の縦方向または横方向の広範囲に位置付けられるように、前記ズームレンズの焦点距離を制御するステップを備えるものである。

また、請求項１１に記載の発明は異なる複数の方向から複数のシーン画像を撮像し、前記複数のシーン画像についてテンプレートマッチングを行い、前記テンプレートマッチングの結果に基づいて前記移動対象物の３次元位置と前記移動目標の３次元位置を演算するステップを備えるものである。

また、請求項１２に記載の発明は前記テンプレート画像のサイズを変更するステップは、前記テンプレート画像を予め保存する際の前記ズームレンズの焦点距離に対する、前記ズーム制御手段によって変更された前記ズームレンズの焦点距離の比を用いて前記テンプレート画像のサイズを変更するこものである。

本発明は、移動対象物と移動目標の間の距離が大きい場合には、テンプレートマッチングを行わないで、前記移動対象物を高速で移動させ、前記距離が小さい場合にはテンプレートマッチングを行なって前記移動対象物を前記移動目標に誘導するので、全体として前記移動対象物を高速かつ高精度に前記移動目標に移動できる効果がある。
また、パン・チルト機構およびズーム制御手段を備えたので、前記移動対象物と前記移動目標をシーン画像の中央に大きくとらえられ、その結果、前記移動対象物と前記移動目標の位置関係を正確に検出できる効果がある。

以下、本発明の実施の形態を図に基づいて説明する。 ここでは、従来の技術の例と同様にペグインホール作業を例に説明する。 ロボット、カメラなどの基本的なハードウェアは従来技術の例と共通するので、図１１に付した符号を引用して詳細な説明を省略する。

本発明の第１の実施例について、図１から図３までを用いて説明する。
図１は本発明の第１の実施例を示す２次元視覚フィードバック装置の機能ブロック図である。 図において、１０１は撮像手段（左カメラ２５０６または右カメラ２５０７に相当）であり、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を含むシーン画像２５０９を撮像する。
１０２はマッチング手段であり、予め保存したテンプレート画像２５１４とシーン画像２５０９を用いてテンプレートマッチングを行い、シーン画像２５０９内の移動対象物と移動目標の位置を検出する。 ここで検出する位置は、一般にシーン画像２５０９の左上を原点とする画像座標系に関するものであり、画像座標系に関する移動対象物位置をＵｐ、移動目標位置をＵｈとする。
１０３は座標変換手段であり、画像座標系に関する移動対象物位置Ｕｐと移動目標位置Ｕｈを、移動機構（ロボット２５０５）が認識できるワールド座標系に変換する。 ワールド座標系に関する移動対象物位置をＷｐ、移動目標位置をＷｈとする。

１０４は指令生成手段であり、ワールド座標系に関する移動対象物位置Ｗｐと移動目標位置Ｗｈから移動機構（ロボット２５０５）を移動させる移動指令を生成する。 移動指令は、通常の位置のフィードバック制御を用いると、ゲインをＫとした時、
Ｐ＝Ｋ（Ｗｈ−Ｗｐ） （１）
で演算することができる。 ここでゲインＫは、０から１までの任意の定数である。
１０５は距離演算手段であり、ワールド座標系に関する移動対象物位置Ｗｐと移動目標位置Ｗｈの距離を演算する。 この距離は、現在の移動対象物（ペグ２５０１）と移動目標（ホール２５０２）の距離であり、現在距離Ｌｗとする。 Ｌｅは終了条件距離であり、移動対象物（ペグ２５０１）の移動を終了させるための条件を表しており、予め設定されている。 Ｌｓは速度条件距離であり、移動対象物（ペグ２５０１）の、次に説明する高速指令生成手段１０６での移動を終了させるための条件を表しており、予め設定されている。 速度条件距離Ｌｓは、終了条件距離Ｌｅより大きな値を設定する。

１０６は高速指令生成手段であり、指令生成手段１０４で生成した移動指令Ｐよりも大きい移動量をもたらす高速移動指令Ｑを生成する。 高速移動指令Ｑは、マッチング手段１０２によるテンプレートマッチングで、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を追跡し続けられることを考慮することなく、高速な移動指令として良い。 これは、高速移動指令Ｑによる移動機構（ロボット２５０５）の動作中は、マッチング手段１０２による処理を実行しないからである。 高速移動指令Ｑは、装置構成時に予め決めた指令としても良いし、移動機構（ロボット２５０５）が持つ最高速度の指令としても良い。 但し、距離演算手段１０５で演算した移動対象物（ペグ２５０１）と移動目標（ホール２５０２）の距離、すなわち、現在距離Ｌｗが速度条件距離Ｌｓより小さくならないような指令とする必要がある。 また、高速移動指令Ｑは、現在距離Ｌｗに応じて生成しても良い。 例えば、現在距離Ｌｗが速度条件距離Ｌｓとなるまでの移動時間をＴとした場合、
（Ｌｗ−Ｌｓ）／Ｔ （２）
を高速移動指令Ｑの速度とする。 もし、移動機構（ロボット２５０５）が持つ最高速度がこの速度より小さければ、移動区間を分割して速度を決めてやれば良い。 このようにすれば、移動時間Ｔを与えれば、現在距離Ｌｗが比較的大きい時には高速で、小さい時には低速（但し移動指令Ｐよりは高速）で移動することができる。

１０７は速度条件比較手段であり、現在距離Ｌｗと速度条件距離Ｌｓを比較し、
Ｌｗ＞Ｌｓ （３）
の場合に、指令切換信号１０８の高速指令生成側の信号を、
Ｌｗ≦Ｌｓ （４）
の場合に、指令切換信号１０８の指令生成側の信号を出力する。

１０９は指令切換手段であり、速度条件比較手段１０７の指令切換信号１０８が高速指令生成側の信号の場合に、指令を高速移動指令Ｑにし、指令生成側の信号の場合に、指令を移動指令Ｐにする。 すなわち、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）との距離（現在距離Ｌｗ）が予め保存した距離（速度条件距離Ｌｓ）より大きければ、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）の追跡を考慮することなく、高速移動指令Ｑによって移動機構（ロボット２５０５）を動作させ、小さければ追跡を考慮した通常の移動指令Ｐによって移動機構（ロボット２５０５）を動作させる。

１１０は終了条件比較手段であり、現在距離Ｌｗと終了条件距離Ｌｅを比較し、
Ｌｗ＞Ｌｅ （５）
の場合に、指令ＯＮ信号１１１を、
Ｌｗ≦Ｌｅ （６）
の場合に、指令ＯＦＦ信号１１２を出力する。

１１３は指令ＯＮ・ＯＦＦ手段であり、終了条件比較手段１１０の出力が指令ＯＮ信号１１１の場合に、高速移動指令Ｑまたは移動指令ＰをＯＮにし、指令ＯＦＦ信号１１２の場合に、高速移動指令Ｑまたは移動指令ＰをＯＦＦにする。 すなわち、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）との距離（現在距離Ｌｗ）が予め保存した距離（終了条件距離Ｌｅ）より大きければまだ離れているものとして高速移動指令Ｑまたは移動指令Ｐを出力し移動機構（ロボット２５０５）を動作させ、小さければ充分に近付いたものとして高速移動指令Ｑまたは移動指令Ｐを切断し移動機構（ロボット２５０５）を停止させる。 これらを繰り返すことにより移動対象物（ペグ２５０１）を移動目標（ホール２５０２）に移動させる。

図２は本発明の第１の実施例を示す２次元視覚フィードバック方法のフローチャートである。
ステップ２０１では、撮像手段１０１により、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を含むシーン画像２５０９を撮像する。 ステップ２０２では、マッチング手段１０２により、予め保存したテンプレート画像２５１４とシーン画像２５０９を用いてテンプレートマッチングを行い、シーン画像２５０９内の移動対象物位置Ｕｐと移動目標位置Ｕｈを検出する。 ステップ２０３では、座標変換手段１０３により、画像座標系に関する移動対象物位置Ｕｐと移動目標位置Ｕｈを、ワールド座標系に関する移動対象物位置Ｗｐと移動目標位置Ｗｈに変換する。 ステップ２０４では、距離演算手段１０５により、ワールド座標系に関する移動対象物位置Ｗｐと移動目標位置Ｗｈの距離（現在距離Ｌｗ）を演算する。

ステップ２０５では、終了条件比較手段１１０と指令ＯＮ・ＯＦＦ手段１１３により、現在距離Ｌｗと終了条件距離Ｌｅを比較し、式（６）を満たす場合に高速移動指令Ｑまたは移動指令ＰをＯＦＦ（終了）にし、式（５）を満たす場合にはステップ２０６に移行する。 ステップ２０６では、速度条件比較手段１０７と指令切換手段１０９により、現在距離Ｌｗと速度条件距離Ｌｓを比較し、式（４）を満たす場合に移動指令Ｐを演算するステップ２０７に移行し、式（３）を満たす場合に高速移動指令Ｑを演算するステップ２０８に移行する。

式（４）を満たす場合は、ステップ２０７で移動指令Ｐを生成し、ステップ２０９で移動指令Ｐに基づいて移動機構（ロボット２５０５）を動作させる。 そして、ステップ２０５で式（６）を満たすまで以上のステップを繰り返し、移動対象物（ペグ２５０１）を移動目標（ホール２５０２）に移動させる。

式（３）を満たす場合のステップ２０８では、高速指令生成手段１０６により、移動指令Ｐよりも大きい移動量をもたらす高速移動指令Ｑを生成する。 その後、ステップ２１０で高速移動指令Ｑに基づいて移動機構（ロボット２５０５）を動作させ、ステップ２０５において式（６）を満たすまで繰り返す。

図３は、本発明の第１の実施例を示すシーン画像の説明図である。 図において、３０１は高速移動状態、３０２は通常移動状態、３０３は終了状態のシーン画像２５０９の例である。 尚、説明を分かり易くするため、ロボットハンド２５０４及びロボット２５０５は図示していない。

まず、高速移動状態３０１では、式（５）を満たすため、ステップ２０５で“Ｙ”となり、指令ＯＮ・ＯＦＦ手段１１３はＯＮとなる。 更に、式（３）を満たすため、ステップ２０６で“Ｙ”となり、指令切換手段１０９は指令を高速移動指令Ｑとする。 つまり、高速移動状態３０１は、移動対象物（ペグ２５０１）の移動目標（ホール２５０２）への移動を、移動対象物位置（ペグ２５０１先端位置）と移動目標位置（ホール２５０２位置）の追跡を行うことなく、高速に行う状態である。

次に、通常移動状態３０２では、式（５）を満たすため、ステップ２０５で“Ｙ”となり、指令ＯＮ・ＯＦＦ手段１１３はＯＮとなる。 更に、式（４）を満たすため、ステップ２０６で“Ｎ”となり、指令切換手段１０９は指令を移動指令Ｐとする。 つまり、通常移動状態３０２は、移動対象物（ペグ２５０１）の移動目標（ホール２５０２）への移動を、移動対象物位置（ペグ２５０１先端位置）と移動目標位置（ホール２５０２位置）の追跡を行いながら、高速移動状態３０１より遅い速度で行う状態である。

更に、終了状態３０３では、式（６）を満たすため、ステップ２０５で“Ｎ”となり、指令ＯＮ・ＯＦＦ手段１１３はＯＦＦとなる。 つまり、終了状態３０３は、移動対象物（ペグ２５０１）の移動目標（ホール２５０２）への移動が終了している状態である。

次に、本発明の第２の実施例について、図４、図５を用いて説明する。
図４は本発明の第２の実施例を示す３次元視覚フィードバック装置の機能ブロック図である。 図１の２次元視覚フィードバック装置と共通する構成要素については同一の符号を付しているので説明を省略する。 図において、４０１は右撮像手段（右カメラ２５０７に相当）であり、４０２は左撮像手段（左カメラ２５０６）であり、共に、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を含むシーン画像（右シーン画像２５１１、左シーン画像２５１０）を撮像する。 ４０３は固定台（カメラ台２５０８）で、右撮像手段４０１と左撮像手段４０２を固定している。 ４０４は右マッチング手段、４０５は左マッチング手段であり、予め保存したテンプレート画像２５１４と右シーン画像２５１１、左シーン画像２５１０を用いてテンプレートマッチングを行い、右シーン画像２５１１、左シーン画像２５１０内の移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を検出する。 ここで検出する位置は、一般に右シーン画像２５１１、左シーン画像２５１０の左上を原点とする画像座標系に関するものであり、画像座標系に関する右撮像手段Ｅ０１の移動対象物位置をＵＲｐ、移動目標位置をＵＲｈ、左撮像手段４０２の移動対象物位置をＵＬｐ、移動目標位置をＵＬｈとする。 ４０６はステレオ演算手段であり、画像座標系に関する右移動対象物位置ＵＲｐ、右移動目標位置ＵＲｈと左移動対象物位置ＵＬｐ、左移動目標位置ＵＬｈから、一般に広く知られている三角測量の原理に基づいたステレオ視により、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）の３次元位置を演算する。 ここで演算する３次元位置は、通常、両撮像手段（４０１と４０２）の中央、またはどちらかの撮像手段に固定されたカメラ座標系に関するものであり、カメラ座標系に関する移動対象物位置をＣｐ、移動目標位置をＣｈとする。 座標変換手段１０３は、移動機構（ロボット２５０５）が認識できるワールド座標系に、カメラ座標系に関する移動対象物位置Ｃｐ、移動目標位置Ｃｈを変換する。 その他のブロックの機能は、図１を用いて説明した２次元視覚フィードバック装置と同じである。

また、図５は本発明の第２の実施例を示す３次元視覚フィードバック方法のフローチャートである。 図２の２次元視覚フィードバック方法と共通するステップには同一の符号を付しているので説明を省略する。 ステップ５０１では、右撮像手段４０１、左撮像手段４０２により、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）を含むシーン画像２５０９を撮像する。 ステップ５０２では、右マッチング手段４０４、左マッチング手段４０５により、予め保存したテンプレート画像２５１４と右シーン画像２５１１、左シーン画像２５１０を用いてテンプレートマッチングを行い、右シーン画像２５１１、左シーン画像２５１０内の右移動対象物位置ＵＲｐ、右移動目標位置ＵＲｈ、左移動対象物位置ＵＬｐ、左移動目標位置ＵＬｈを検出する。 ステップ５０３では、ステレオ演算手段４０６により、画像座標系に関する右移動対象物位置ＵＲｐ、右移動目標位置ＵＲｈと左移動対象物位置ＵＬｐ、左移動目標位置ＵＬｈから、三角測量の原理に基づいたステレオ視により、３次元の、カメラ座標系に関する移動対象物位置Ｃｐ、移動目標位置Ｃｈを演算する。 ステップ２０３では、座標変換手段１０３により、カメラ座標系に関する移動対象物位置Ｃｐ、移動目標位置Ｃｈを、ワールド座標系に関する移動対象物位置Ｗｐと移動目標位置Ｗｈに変換する。 その他のステップの処理は、図２を用いて説明した２次元視覚フィードバック方法と同じである。

次に、本発明の第３の実施例について、図６から図８までを用いて説明する。
図６は本発明の第３の実施例を示す２次元視覚フィードバック装置の機能ブロック図である。 図１に示した２次元視覚フィードバック装置と共通する構成要素には同一の符号を付している。 図において、６０１は撮像手段１０１に装着されるズームレンズであり、焦点距離６０９が可変である。 また、ズームレンズ６０１は焦点距離６０９の値を示す信号を出力できるように構成されている。 ６０２はパン・チルト機構であり、一体化された撮像手段１０１とズームレンズ６０１を上下左右に振ることができる。 ６０３はテンプレートサイズ変更手段であり、テンプレート画像２５１４のサイズを焦点距離６０９に応じて変更してリサイズテンプレート画像６０６を作成する。 今、テンプレート画像２５１４を取得した時の焦点距離６０９をｆ _０ 、現在の焦点距離６０９をｆ _ｉとすると、テンプレート画像２５１４に対するリサイズテンプレート画像のスケールＳを、
Ｓ＝ｆ _ｉ ／ｆ _０ （７）
で演算する。 テンプレートサイズ変更手段６０３は、シーン画像２５０９を撮像した時の焦点距離ｆ _ｉを入力して式（７）によりスケールＳを演算し、このスケールＳをテンプレート画像２５１４に掛けることによって、テンプレート画像２５１４のサイズを拡大・縮小する。 これがリサイズテンプレート画像６０６となる。

マッチング手段１０２では、リサイズテンプレート画像６０６とシーン画像２５０９を用いてテンプレートマッチングを行い、シーン画像２５０９内の移動対象物（ペグ２５０１）位置Ｕｐと移動目標（ホール２５０２）位置Ｕｈを検出する。
６０４はパン・チルト制御手段で、シーン画像２５０９内の移動対象物位置Ｕｐと移動目標位置Ｕｈからそれらの中点を求め、その中点がシーン画像２５０９の中央に位置するようなパン・チルト制御信号６０７を生成する。 すなわち、撮像手段１０１は、常に移動対象物位置Ｕｐと移動目標位置Ｕｈの中央を向いていることになる。
６０５はズーム制御手段であり、シーン画像２５０９内の移動対象物位置Ｕｐと移動目標位置Ｕｈから、それぞれがシーン画像２５０９の縦方向または横方向の広範囲に位置付けられるように、ズームレンズ６０１の焦点距離を制御するズーム制御信号６０８を出力する。
これらパン・チルト制御手段６０４、ズーム制御手段６０５により、パン・チルト機構６０２、ズームレンズ６０１が制御され、撮像手段Ｃ０１によって撮像されるシーン画像２５０９は図７に示すようになる。

図７は、本発明の第３の実施例を示すシーン画像の説明図である。 図において、７０１は第１の移動状態で、ある時点での移動対象物（ペグ２５０１）の移動目標（ホール２５０２）への移動状態を表している。 ７０２は第２の移動状態で、第１の移動状態７０１より移動対象物（ペグ２５０１）が移動目標（ホール２５０２）へ接近している状態を表している。 ７０３は第３の移動状態で、第２の移動状態７０２より更に移動対象物（ペグ２５０１）が移動目標（ホール２５０２）へ接近している状態を表している。 図７に示すように、ズーム制御手段６０５とズームレンズ６０１によりシーン画像２５０９がズームアップされ、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）のそれぞれの位置が、視野の広範囲を占めるようになっていることが分かる。 しかし、例えば、第１の移動状態７０１を考えると、第１の中点７０４が視野の中央にあるため、これをズームアップしただけでは、第１の中点７０４を中心とするシーン画像２５０９しか得られない。 そこで、本実施の形態では、第２の移動状態７０２のようなシーン画像２５０９を得るために、パン・チルト制御手段６０４とパン・チルト機構６０２により、撮像手段１０１の視線を、移動対象物（ペグ２５０１）と移動目標（ホール２５０２）のそれぞれの位置の中点（第２の中点７０５）に向くようにしている。 第３の移動状態７０３の場合も同様、第３の中点７０６に撮像手段Ｃ０１の視線が向くようにパン・チルト機構６０２を制御する。
このように、パン・チルト制御手段６０４、ズーム制御手段６０５により、パン・チルト機構６０２、ズームレンズ６０１が制御される結果、シーン画像２５０９がズームアップされ、移動対象物（ペグ２５０１）が移動目標（ホール２５０２）に近付くにつれ、位置分解能が高くなり、更に、それに応じてテンプレートサイズ変更手段６０３でテンプレート画像２５１４のサイズを変更するので、正しい移動対象物（ペグ２５０１）の位置と移動目標（ホール２５０２）の位置が検出できる。

図８は本発明の第３の実施例を示す２次元視覚フィードバック方法のフローチャートである。 図２の２次元視覚フィードバック方法と共通するステップには同一の符号を付しているので説明を省略する。 ステップ８０１では、テンプレートサイズ変更手段６０３により、焦点距離６０９から式（７）を演算してスケールＳを求め、このスケールＳをテンプレート画像２５１４に掛けることによってサイズを変更してリサイズテンプレート画像６０６を作成する。 ステップ２０２では、リサイズテンプレート画像６０６を用いてテンプレートマッチングを行う。 ステップ８０２では、パン・チルト制御手段６０４とパン・チルト機構６０２により、シーン画像２５０９内の移動対象物位置Ｕｐと移動目標位置Ｕｈからそれらの中点を求め、その中点がシーン画像２５０９の中央に位置するように、撮像手段１０１を駆動する。 ステップ８０３では、ズーム制御手段６０５とズームレンズ６０１により、シーン画像２５０９内の移動対象物位置Ｕｐと移動目標位置Ｕｈから、それぞれがシーン画像２５０９の広範囲に位置付けられるように、ズームレンズ６０１の焦点距離を制御する。

最後に、本発明の第４の実施例について、図９、図１０を用いて説明する。
図９は本発明の第４の実施例を示す３次元視覚フィードバック装置の機能ブロック図である。 図１、図４あるいは図６に示した視覚フィードバック装置と共通する構成要素には同一の符号を付している。 図において、９０１は右撮像手段４０１に装着された右ズームレンズであり、９０２は左撮像手段４０２に装着された左ズームレンズである。 右ズームレンズ９０１、左ズームレンズ９０２は焦点距離（右焦点距離９０８、左焦点距離９０９）が可変であり、右焦点距離９０８、左焦点距離９０９の値を示す信号を出力できるように構成されている。 パン・チルト機構６０２は、固定台４０３（カメラ台２５０８）を上下左右に振ることができる。
９０３は右テンプレートサイズ変更手段、９０４は左テンプレートサイズ変更手段であり、テンプレート画像２５１４のサイズを右焦点距離９０８、左焦点距離９０９に応じて変更して右リサイズテンプレート画像９１２、左リサイズテンプレート画像９１３を作成する。 右テンプレートサイズ変更手段９０３、左テンプレートサイズ変更手段９０４は、右シーン画像２５１１、左シーン画像２５１０を撮像した時の焦点距離ｆ _ｉを入力して式（７）によりスケールＳを演算し、このスケールＳをテンプレート画像２５１４に掛けることによって、テンプレート画像２５１４のサイズを拡大・縮小する。 これが右リサイズテンプレート画像９１２、左リサイズテンプレート画像９１３となる。
右マッチング手段４０４、左マッチング手段４０５では、右リサイズテンプレート画像９１２、左リサイズテンプレート画像９１３と右シーン画像２５１１、左シーン画像２５１０を用いてテンプレートマッチングを行い、右シーン画像２５１１内の移動対象物（ペグ２５０１）ＵＲｐと移動目標（ホール２５０２）ＵＲｈ、左シーン画像２５１０内の移動対象物（ペグ２５０１）位置ＵＬｐと移動目標（ホール２５０２）位置ＵＬｈを検出する。 パン・チルト制御手段６０４は、右シーン画像２５１１内の移動対象物位置ＵＲｐと移動目標位置ＵＲｈ、左シーン画像２５１０内の移動対象物位置ＵＬｐと移動目標位置ＵＬｈから各シーン画像において移動対象物位置と移動目標位置の中点を求め、その中点が各シーン画像の中央に位置するようなパン・チルト制御信号６０７を生成する。 すなわち、右撮像手段４０１、左撮像手段４０２は、常に移動対象物位置と移動目標位置の中央を向いていることになる。
９０５は右ズーム制御手段で、右シーン画像２５１１内の移動対象物位置ＵＲｐと移動目標位置ＵＲｈから、それぞれが右シーン画像２５１１の縦方向または横方向の広範囲に位置付けられるように、右ズームレンズ９０１の焦点距離を制御する右ズーム制御信号９１０を出力する。 ９０６は左ズーム制御手段で、左シーン画像２５１０内の移動対象物位置ＵＬｐと移動目標位置ＵＬｈから、それぞれが左シーン画像２５１０の縦方向または横方向の広範囲に位置付けられるように、左ズームレンズ９０２の焦点距離を制御する左ズーム制御信号９１１を出力する。
９０７はキャリブレーションテーブルであり、ステレオ演算手段４０６の演算で必要となる右撮像手段４０１と左撮像手段４０２の位置関係を表すキャリブレーション行列を、右ズームレンズ９０１、左ズームレンズ９０２の焦点距離（右焦点距離９０８、左焦点距離９０９）に応じて出力できるように構成している。 以下では、このキャリブレーションテーブル９０７について説明する。

ステレオ演算手段４０６は、画像座標系に関する右移動対象物位置ＵＲｐ、右移動目標位置ＵＲｈと左移動対象物位置ＵＬｐ、左移動目標位置ＵＬｈから、一般に広く知られている三角測量の原理に基づいたステレオ視により、カメラ座標系に関する移動対象物位置Ｃｐと移動目標位置Ｃｈの３次元位置を演算する。 この３次元位置の演算には、右撮像手段４０１と左撮像手段４０２に関して予めキャリブレーションを行っておく必要があり、各々の撮像手段の画像座標系に関する位置、例えば、ＵＲｐとＵＬｐと、カメラ座標系に関する３次元位置、この場合Ｃｐとがキャリブレーション行列として関係付けられている。 この関係付けに必要なパラメータとしては、撮像手段（右撮像手段４０１、左撮像手段４０２）の撮像面の大きさや、それぞれの撮像手段の位置関係等があり、その中の重要なパラメータとして、撮像手段（右撮像手段４０１、左撮像手段４０２）の焦点距離（右焦点距離９０８、左焦点距離９０９）がある。 キャリブレーションテーブル９０７は、予め、焦点距離（右焦点距離９０８、左焦点距離９０９）に応じたキャリブレーション行列を保存しておき、焦点距離（右焦点距離９０８、左焦点距離９０９）の入力に応じて必要なキャリブレーション行列を出力するものである。 これによって、ズーム制御手段（右ズーム制御手段９０５、左ズーム制御手段９０６）でズームレンズ（右ズームレンズ９０１、左ズームレンズ９０２）が制御され、焦点距離（右焦点距離９０８、左焦点距離９０９）が変わっても、それに応じたキャリブレーション行列がキャリブレーションテーブル９０７から出力され、ステレオ演算手段４０６では、そのキャリブレーション行列を用いて３次元位置を演算することができる。
尚、本実施の形態では、キャリブレーションテーブル９０７として、予め、焦点距離（右焦点距離９０８、左焦点距離９０９）に応じたキャリブレーション行列を保存しておき、焦点距離（右焦点距離９０８、左焦点距離９０９）の入力に応じて必要なキャリブレーション行列を出力するように構成したが、本発明は、これに限るものではなく、焦点距離（右焦点距離９０８、左焦点距離９０９）に応じてキャリブレーション行列をその都度演算するように構成しても、全く同様の効果を奏することができる。

座標変換手段１０３は、移動機構（ロボット２５０５）が認識できるワールド座標系に、カメラ座標系に関する移動対象物位置Ｃｐ、移動目標位置Ｃｈを変換する。 カメラ座標系は、通常、両撮像手段（４０１と４０２）の中央、またはどちらかの撮像手段に固定されたものであり、パン・チルト機構６０２の動作により、ワールド座標系から見たカメラ座標系の位置・姿勢は変わってしまう。 従って、パン・チルト機構６０２の各回転角度（パン・チルト回転角９１４）を読み取り、パン・チルト回転角９１４からワールド座標系から見たカメラ座標系の位置・姿勢を演算する。 この演算は、一般に広く用いられている座標変換を用いれば容易に行うことができる。 これによって、パン・チルト機構６０２の動作によっても、ワールド座標系から見たカメラ座標系の位置・姿勢をパン・チルト回転角９１４によって演算することができ、カメラ座標系に関する移動対象物位置Ｃｐ、移動目標位置Ｃｈを、ワールド座標系に関する位置に変換することができる。

図１０は本発明の第４の実施例を示す３次元視覚フィードバック方法のフローチャートである。 図８に示したフローチャートと共通するステップには同一の符号を付している。
ステップＡ０１では、右焦点距離９０８、左焦点距離９０９から式（７）を演算してそれぞれのスケールＳを求め、このそれぞれのスケールＳをテンプレート画像２５１４に掛けることによってサイズを変更して右リサイズテンプレート画像９１２、左リサイズテンプレート画像９１３を作成する。
ステップＡ０２では、右ズーム制御手段９０５と右ズームレンズ９０１、左ズーム制御手段９０６と左ズームレンズ９０２により、右シーン画像２５１１、左シーン画像２５１０内の移動対象物位置ＵＲｐ、ＵＬｐと移動目標位置ＵＲｈ、ＵＬｈから、それぞれが右シーン画像２５１１、左シーン画像２５１０内の広範囲に位置付けられるように、右ズームレンズ９０１、左ズームレンズ９０２の焦点距離を制御する。

本発明は、視覚フィードバックによってロボットを高速かつ高精度に動作させる装置及び方法として有用である。

本発明の第１の実施例を示す２次元視覚フィードバック装置の機能ブロック図である。

本発明の第１の実施例を示す２次元視覚フィードバック方法のフローチャートである。

本発明の第１の実施例を示すシーン画像の例である。

本発明の第２の実施例を示す３次元視覚フィードバック装置の機能ブロック図である。

本発明の第２の実施例を示す３次元視覚フィードバック方法のフローチャートである。

本発明の第３の実施例を示す２次元視覚フィードバック装置の機能ブロック図である。

本発明の第３の実施例を示すシーン画像の例である。

本発明の第３の実施例を示す２次元視覚フィードバック方法のフローチャートである。

本発明の第４の実施例を示す３次元視覚フィードバック装置の機能ブロック図である。

本発明の第４の実施例を示す３次元視覚フィードバック方法のフローチャートである。

従来例を示すペグインホールを行う装置の構成図である。

符号の説明

１０１ 撮像手段、１０２ マッチング手段、１０３ 座標変換手段、１０４ 指令生成手段、１０５ 距離演算手段、１０６ 高速指令生成手段、１０７ 速度条件比較手段、１０８ 指令切換信号、１０９ 指令切替手段、１１０ 終了条件比較手段、１１１ 指令ＯＮ信号、１１２ 指令ＯＦＦ信号、１１３ 指令ＯＮ・ＯＦＦ手段、４０１ 右撮像手段、４０２ 左撮像手段、４０３ 固定台、４０４ 右マッチング手段、４０５ 左マッチング手段、４０６ ステレオ演算手段、６０１ ズームレンズ、６０２ パン・チルト機構、６０３ テンプレートサイズ変更手段、６０４ パン・チルト制御手段、６０５ ズーム制御手段、６０６ リサイズテンプレート画像、６０７ パン・チルト制御信号、６０８ ズーム制御信号、６０９ 焦点距離、９０１ 右ズームレンズ、９０２ 左ズームレンズ、９０３ 右テンプレートサイズ変更手段、９０４ 左テンプレートサイズ変更手段、９０５ 右ズーム制御手段、９０６ 左ズーム制御手段、９０７ キャリブレーションテーブル、９０８ 右焦点距離、９０９ 左焦点距離、９１０ 右ズーム制御信号、９１１ 左ズーム制御信号、９１２ 右リサイズテンプレート画像、９１３ 左リサイズテンプレート画像、９１４ パン・チルト回転角、２５０１ ペグ、２５０２ ホール、２５０３ ワーク、２５０４ ロボットハンド、２５０５ ロボット、２５０６ 左カメラ、２５０７ 右カメラ、２５０８ カメラ台、２５０９ シーン画像、２５１０ 左シーン画像、２５１１ 右シーン画像、２５１２ 左画像処理装置、２５１３ 右画像処理装置、２５１４ テンプレート画像、２５１５ ペグテンプレート画像、２５１６ ホールテンプレート画像、２５１７ 制御装置