WO1993015376A1

WO1993015376A1 - System for recognizing and tracking target mark, and method therefor

Info

Publication number: WO1993015376A1
Application number: PCT/JP1993/000107
Authority: WO
Inventors: Masayoshi Hashima; Fumi Hasegawa; Keiju Okabayashi; Ichiro Watanabe; Shinji Kanda; Naoyuki Sawasaki; Yuichi Murase
Original assignee: Fujitsu Limited
Priority date: 1992-01-30
Filing date: 1993-01-29
Publication date: 1993-08-05
Also published as: US5521843A; US5617335A; JP3242108B2

Description

明細書

タ一ゲットマークの認識 · 追跡システム及び方法

技術分野

本発明はビデオカメラを用いてタ一ゲットマークを認識し、追跡するタ一ゲットマークの認識 · 追跡システム及び方法に関し、特に、ビデオ力メラで撮影された画像の画像処理によつてターゲットマークの位置、姿勢等を検出し、それに基づき、所定位置からのずれ量を検出し、そのずれ量に応じて自己の位置景

姿勢等を制御するターゲットマークの認識 ·追跡システム及び方法に関する。

術背

ロボッ卜に移動物体を自律的に把持させたり、宇宙機などをドッキングさせる場合、その移動物体や宇宙機に設けられたターゲットマークを、ビデオカメラを用いて認識し、追跡することが必要となる。

従来から、対象物体の位置及び姿勢を計測する方法として、対象物体に設けたターゲットマークをビデオカメラを用いて撮像し、その撮像データを画像処理して対象物体の位置及び姿勢を求める手法が知られている。この手法は、上述したように、ロボットハンドが対象物体を把持する場合などで用いられる。その場合、ビデオカメラはロボットに搭載され、ビデオカメラが検出したターゲットマ一クの位置及び姿勢データに応じて口ボットはタ一ゲットマークの追跡を行い、対象物体を把持する, しかし、上記従来の方法では、ターゲットマークの撮像デ一タを画像処理して対象物体の位置及び姿勢を得るまでに時間がかかるため、ロボッ卜へのリアルタイムなフィードバックが不可能であり、対象物体の追跡が困難であった。

また、画像上でパターンマッチングにより目標物を追跡する方法もあるが、 2次元空間での計算になるため、計算量が多く時間を要する。

さらに、対象物体の追跡に関して、対象物体の運動を把握して移動位置を予測する方法もあるが、この場合は、対象物体が規則正しく運動していることが前提となるので、通常の対象物体の移動には単純には適用することができない。

なお、ターゲットマークの認識の前段階として、他の物体が混在したりノィズが多い画像中から目的のマークを抽出することが必要となる。このための従来の手法としては、マークの面積による比較を行つたり、パターンマッチングによる特徵抽出が行われている。

しかし、面積による比較の手法は、目的とするマークと同じ程度の面積を持つ抽出画像を目的のマークと判定するものである力、同程度の大きさの物体が周囲にある場合ゃノィズの多い画像から目的のマークを抽出することは実質的に不可能であり、よつて適用範囲が限定されるという問題点がある。

また、パターンマッチングによる特徵抽出手法では、画像メモリ内をサーチするために多くの時間が必要となり、処理時間が長くかかるという問題点がある。

またさらに、対象物体の三次元空間内での位置、姿勢等を計測する場合に、 3点乃至 4点の位置闋係を示す三角形、又は四角形のターゲットマ一クが使用されている。ターゲットマ一クが対象物体の特定平面に貼り付けられていれば、撮像されたターゲットマークの画像空間での位置関係から物体の位置と姿勢を計測できる。この計測に際しては、ターゲットマークの物体空間から力メラの画像平面に結像された像点の座標に対して斜影幾何学に基づく演算が実行される。物体の位置、或いは姿勢が変化すればターゲットマークの像点間の関係も変化するから. その変化に基づいて対象物体の三次元空間における位置と姿勢についての演算が可能になる。

しかし、従来のタ一ゲットマ一ク.による計測装置では、三角形、又は四角形の画像から抽出された像点の座標値に基づいて位置と姿勢についての演算が実行されるから、ターゲットマ一クのカメラに対する姿勢によつて測定精度にばらつきが生じる ₍ すなわち、画像平面内での各像点から、方向成分が含まれる画像データを得て、ターゲットマークが貼着された対象物体の特定平面を記述する場合には、各像点に対する基準距離が変化して位置と姿勢についての測定精度に安定性が欠けるという問題点があった。

また、従来のタ一ゲツトマ一クによる位置の演算では、タ一ゲットマークの平面が画像平面に対してある角度をなしていることを必要とし、したがって、カメラをターゲットマークに正対させた場合に比べて演算パラメータが著しく増加する。このため、位置の測定においても演算が複雑になって、その分だけ測定の精度が低下するという問題点があつた。

また、 4つの点状のマークを撮影手段で画像化した場合に、画像上ではその点はある面積を有してしまい、画像データにおいて点の位置を正確に特定することができず、したがって、サブピクセル単位の画像上の位置決めができないという題点があった。すなわち、そうした画像データにおける不正確な点の位置を基に、対象物までの距離やその姿勢を算出することになり、対象物までの距離やその姿勢を、精度よく計測することができなかった。発明の開示

本発明はこのような点に鑑みてなされたものであり、本発明の第 1の目的は、ビデオ力メラを用いて对象物体のタ一ゲットマークをリアルタイムで追跡することができるタ一ゲットマ一クの認識 ·追跡システム及び方法を提供することである。

本発明の第 2の目的は、高速、かつ確実に目的のターゲットマークを抽出できるようにしたターゲットマ一クの認識 ·追跡システム及び方法を提供することである。

本発明の第 3の目的は、ターゲットマークとカメラとの位置関係による測定誤差のばらつきをなくしたターゲットマークの認識 ·追跡システム及び方法を提供することである。

本発明の第 4の目的は、対象物までの距離やその姿勢を、精度よく、かつ簡単に計測するようにしたターゲットマ一クの認識 · 追跡システム及び方法を提供することである。

本発明では上記課題を解決するために、

ビデオカメラを用いてターゲットマークを追跡するタ一ゲットマーク追跡システムにおいて、対象物体に設けられ、黒円の ' 中央部に白三角形を立体的にシフトさせて構成したタ一ゲットマークと、タ一ゲットマークを撮像するビデオカメラと、ビデォカメラを搭載し 6 自由度方向に移動可能な移動機構と、ビデォカメラによるターゲットマークの画像データを処理する画像処理手段と、画像処理手段によって得られたターゲットマークの投影ヒストグラム情報からターゲットマ一クのずれ量を検出するずれ量検出手段と、ずれ量に応じて移動機構の移動動作を制御し、ビデオ力メラにタ一ゲットマークの追跡を行わせる移動機構制御手段と、を有することを特徵とするターゲットマ一ク追跡システムが、提供される。

タ一ゲットマークは黒円の中央部に白三角形を立体的にシフ卜させて構成したものであり、対象物体に設けられる。ビデオカメラは、 6 自由度方向に移動可能な移動機構に搭載され、タ —ゲットマークを撮像する。画像処理手段は、ビデオカメラによるターゲットマークの画像データを処理し、タ一ゲットマ一クの各座標軸に対する投影ヒストグラム等を求める。ずれ量検出手段は、その投影ヒストグラム等の情報からターゲットマ一クのずれ量を検出する。移動機構制御手段は、そのずれ量に応じて移動機構の移動動作を制御し、ビデオカメラにターゲットマークの追跡を行わせる。

このように、黒円の中央部に白三角形を立体的にシフ卜させて構成したターゲットマークをビデオカメラを用いて撮像し、その画像データからターゲットマ一クの各座標軸方向のずれ量を求める。このずれ量は、画像データの各軸投影ヒストグラム処理等によつて求められるが、この各軸投影ヒストグラム処理等は一次元領域での計算である。このため、その計算は複雑な計算を必要とせず非常に簡単に行うことができ、計算量も僅かである。したがって、ターゲットマークのずれ量は、高速処理で求められる。その結果、ずれ量に応じた移動機構へのリアルタイムなフィードバックが可能となり、ビデオ力メラによるターゲットマークの追跡がリアルタイムで可能となる。

また、タ一ゲットマークをビデオカメラにより撮影し、その撮影データを処理することで、ビデオカメラをターゲットマ一クに对し常時、所定関係位置に保持するようにするターゲットマーク視覚追跡制御方式において、ビデオカメラから出力されたターゲットマークの画像においてターゲットマークの目標位置 · 姿勢からの変化量を検出する画像変化量検出手段と、ビデォカメラとタ一ゲットマークとの所定闋係位置からの相対的な実変化量を検出する実変化量検出手段と、ビデオカメラとターゲットマークとの所定関係位置を実験的にずらし、そのときに得られる、画像変化量検出手段の検出値と、実変化量検出手段の検出値とから、相互の関連づけをする同定手段と、を有することを特徴とするターゲットマーク視覚追跡制御方式が、提供される。

以上の構成により、同定手段は、ビデオカメラとタ一ゲットマークとの所定関係位置を実験的にずらし、そのときに得られる、画像変化量検出手段の検出値、即ち、画像上のターゲットマークの目標位置 ·姿勢からの変化量と、実変化量検出手段の検出値、即ち、ビデオカメラとターゲットマ一クとの所定関係位置からの相对的な実変化量とから、相互の関連づけをする。

このように、同定をすることにより、ターゲットマークの画像変化量を、ビデオ力メラとターゲットマ一クとの所定関係位置からの相対的な実変化量に高速に変換でき、ターゲットマ一クとビデオカメラとの間の距離が大きく変化した場合においても、応答性能が劣化せず、安定に移動物体の追跡を行える。

また、少なくとも特定な形状の 3角形から成るターゲットマ —クのカメラ撮影画像に基づいて対象物体の力メラ視線軸回りの姿勢を検出するタ一ゲットマークの姿勢検出方法において、ターゲットマークの 3角形の画像の X , Y方向の各投影ヒストグラムを求め、求められた各投影ヒストグラムにおいて、タ一ゲットマークの 3角形の画像の X， Y方向の各重心位置を求め、求められた各投影ヒストグラムにおいて、ヒストグラム値が最大になるときの各最大ヒストグラム値、およびそのときの X , Y軸値を求め、求められた各重心位置、並びに各最大ヒストグラム値および X， Y軸値、さらに、ターゲットマークの幾何的既知データに基づき、ターゲットマークの 3角形の姿勢が、予め分類され設定されている姿勢パターンの中のいずれのパターンに相当するかを判別し、判別されたパターンにおけるターゲットマークの 3角形のカメラ視線軸回りの姿勢を算出することを特徴とするタ一ゲットマークの姿勢検出方法が、提供される。上記構成において、まず、予めターゲットマークの 3角形の姿勢を分類して姿勢パターンを設定しておき、求められたターゲットマークの 3角形の画像の X， Y方向の各重心位置、並びに各最大ヒストグラム値および X， Y軸値、さらに、ターゲットマークの幾何的既知データに基づき、ターゲットマークの 3 角形の姿勢が、予め分類され設定されている姿勢パタ一ンの中のいずれのパターンに相当するかを判別する。そして、判別されたパターンにおけるターゲットマ一クの 3角形のカメラ視線軸回りの姿勢を算出するようにする。これにより、ターゲットマークの口一ル回転量を的確に、且つ簡単に把握できる。また、原画像を 2値化する過程と、 2値化した画像のうちの画素が連結している画像をグループ化する過程と、グル一プ化された画像について X Y投影ヒストグラムを求める過程と、グループ化された画像の X Y投影ヒストグラムの極値の数を計数する過程と、計数した極値の数を、目的とするターゲットマ一クについて予め求めた X Y投影ヒストグラムの極値数と比較して、グループ化された画像が目的とするターゲットマークであるか否か判定する過程とからなるタ一ゲットマークの検出方法が、提供される。

また、ターゲットマークの像に基づいて物体の位置と姿勢を計測する位置、姿勢の計測装置において、物体の特定平面上にあって円とその中心点を含むターゲットマ一クと、ターゲットマークを撮像して円とその中心点の画像を作成する力メラと、ターゲットマークの画像から物体の位置と姿勢の計測に必要な特徵点を抽出する特徵抽出手段と、特徵点に関する射影幾何学濱算によってターゲットマークの物体空間内での位置及び姿勢を演算する演算手段と、を有することを特徴とする位置、姿勢の計測装置が、提供される。

さらに、対象物までの距離および対象物の姿勢を計測する距離姿勢計測装置において、各重心が同一平面上に位置するように対象物に配置され、少なくとも 1つの円の半径が他と異なる 4つの円板状マークと、 4つの円板状マークを撮影する撮影手段と、撮影手段から出力される 4つの円板状マークの画像データに基づき、各円板状マークの重心位置を算出する重心算出手段と、重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、対象物までの距離および対象物の姿勢を演算する演算手段と、を有することを特徴とする距離姿勢計測装置が. 提供される。図面の簡単な説明図 1 はタ一ゲットマーク追跡システムの全体構成を示す図であ O o

図 2 はタ一ゲットマークの斜視図である。

図 3はターゲットマークの上面図である。

図 4はタ一ゲットマークの側面図である。

図 5 はタ一ゲットマ一クの検出手順を示すフローチャートである。

図 6 はターゲットマークの X Y投影ヒストグラ厶を示す図でめ

図 Ί は投影画像の 2値化処理の例を示す図である。

図 8は 2値化処理画像のラベリング処理の例を示す図である _c 図 9はグループ番号 3の物体を分離する例を示す図である。図 1 0 は分離したグループ審号 3の物体の X Y投影ヒストグラムを示す図である。

図 1 1 は他の形態のターゲットマークとその X Y投影ヒストグラムを示す図である。

図 1 2 はまた他の形態のタ一ゲットマ一クとその X Y投影ヒストグラムを示す図である。

図 1 3は位置 · 姿勢を計測するための 4円ターゲットマ一クの例を示す図である。

図 1 4はタ一ゲットマークの X Y方向ずれ量の求め方を説明する図である。図 1 5 はターゲットマ一クの中心位置 P mの求め方についての説明図である。

図 1 6はターゲットマークの Z方向ずれ量の求め方を説明する図である。

図 1 7 はタ一ゲットマ一クを斜めから見た場合の説明図であり、（A ) はカメラの姿勢を、（B ) はターゲットマーク画像をそれぞれ示す。

図 1 8は図 1 7のターゲットマーク画像の X Y投影ヒストグラムを示す図である。

図 1 9はョ一方向ずれ量の求め方を説明する図である。

図 2 0はピッチ方向ずれ量の求め方を説明する図である。

図 2 1 はロール方向に回転したターゲットマークの画像及びその X軸投影ヒストグラムを示す図である。

図 2 2は口一ル方向ずれ量の求め方を説明する図である。

図 2 3はウィンドウ設定方法の説明図である。

図 2 4はターゲットマークの他の例を示す図である。

図 2 5はターゲットマ一クのさらに他の例を示す図である。図 2 6は図 2 5のターゲットマ一クの使用例を示す図である, 図 2 7 は位置ずれ検出および物体の把持を行うためのフ口一チャートを示す図である。

図 2 8はビデオカメラとタ一ゲットマークとの所定位置関係を示す図である。

図 2 9はずれ量実測時のビデオカメラの動きを示す図である < 図 3 0はずれ量の実測位置を示す座標図である。

図 3 1 は z— E z関係を示すグラフである。

図 3 2は E x— D X関係を示すグラフである。図 3 3は z— A x関係を示すグラフである。

図 3 4は画像上のずれ量 E * を実ずれ量 D * に高速に変換する手順を示すフ π—チヤ一トである。

図 3 5 は実ずれ量の各成分を示す座標図である。

図 3 6 は z _ E z関係を示すグラフである。

図 3 7 は E y— D y関係を示すグラフである。

図 3 8は z — A y関係を示すグラフである。

図 3 9 〖ま第 1 の制御実施例を示すフローチャートである。図 4 0 は座標変換を説明する座標図である。

図 4 1 は第 2の制御実施例を示すフローチャートである。図 4 2 は座標変換を説明する座標図である。

図 4 3はターゲットマークの画像において 3角形が円の中心から大きくずれた場合を示す投影ヒストグラムである。

図 4 4はタ一ゲットマークの画像における円の中心の求め方を説明する投影ヒストグラムである。

図 4 5 は 3角形と円とから成るターゲットマークの画像の実測投影ヒストグラムから 3角形のみの画像の投影ヒストグラムを求める方法を説明する図である。

図 4 6 はターゲットマークの 3角形の基準形状と位置を示す図である。

図 4 7 はタ一ゲットマークの 3角形の重心位置とヒストグラム値の最大値およびその位置とを示す図である。

図 4 8は 3角形のロール角を漸増させたときに分けられる姿勢パターンの前半を示す図である。

図 4 9 は 3.角形のロール角を漸増させたときに分けられる姿勢パターンの後半を示す図である。図 5 0 は重心位置やヒストグラム値の最大値およびその位置に応じて分類された姿勢パターンを示す図である。

図 5 1 は投影ヒストグラムデータを得るためのハードウェアの構成を説明する図である。

図 5 2はウィンドウの移動の仕方を説明する図である。

図 5 3は投影ヒストグラ厶データを得るためのハードウエアの実施例の具体的構成図である。

図 5 4は図 5 3に示す各プロックにおける信号波形のタィミングチャートである。

図 5 5は位置、姿勢の計測装置を示すプロック図である。

図 5 6はターゲットマ一クによる計測の原理を示す図である。図 5 7はターゲットマ一クとその画像の闋係を示す図である。図 5 8は円と中心点の画像を示す図である。

図 5 9 は消失点を決定する方法を示す図である。

図 6 0 はターゲットマ一クの姿勢を求める方法を示す図である ₀ - 図 6 1 はタ一ゲットマークの一例を示す図である。

図 6 2は対象物に設けられるターゲットマ一クの第 1の例を示す平面図である。

図 6 3は第 1の距離姿勢渲算の手順を示すフローチャートでめる。

図 6 4はラベリングおよびヒストグラム処理を示す図である。図 6 5は X Y投影ヒストグラム図である。

図 6 6は対象物に設けられるターゲットマークの第 2の例を示す平面図である。

図 6 7は第 2の距離姿勢演算の手順を示すフローチャートであ <0 o

図 6 8は X Y投影ヒストグラム図である。 '

図 6 9 は対象物に設けられるターゲットマークの第 3の例を示す平面図である。

図 7 0 は第 3の距離姿勢演算の手順を示すフローチヤ一トであ Ο ο

図 7 1 は対象物に固定された座標系と力メラに固定された座標系とを示す図である。

発明を実施するための最良の形態以下、本発明の実施例を図面に基づいて説明する。

図 1 は本発明に係るタ一ゲットマーク追跡システムの全体構成を示す図である。図において、ターゲットマ一ク追跡システムは、対象物体 1、ターゲットマーク 1 0、カメラ 2 0、ロボット 3 0、画像処理装置 4 0、位置ずれ演算装置 5 0及び口ボット制御装置 6 0から構成される。

夕—ゲットマ—ク 1 0 は、後述するように、黒円の中央部に白三角形を立体的にシフトさせて構成したものであり、対象物体 1 に設けられる。カメラ 2 0 は、 6 自由度方向に移動可能なロボット 3 0 に搭載され、ターゲットマーク 1 0を撮像する。画像処理装置 4 0 は、カメラ 2 0によるターゲットマーク 1 0 の画像データを処理し、タ一ゲットマーク 1 0の各座標軸に対する投影ヒストグラム等を求める。位置ずれ演算装置 5 0は、例えばワークステ一ションが使用され、画像処理装置 4 0から送られてきた投影ヒストグラム等の情報から目的とするターゲットマーク 1 0を抽出し、その抽出したタ一ゲットマーク 1 0 のずれ量を検出する。その詳細は後述する。ロボット制御装置

6 0は、そのずれ量に応じて口ボット 3 0の移動動作を制御しカメラ 2 0にタ一ゲットマ一ク 1 0の追跡を行わせる。口ボッ卜 3 0は、その追跡動作によつて対象物体 1の把持部 1 Hを把持して所定の作業目的を達成する。次に、上記のターゲットマーク 1 0の構成を図 2から図 4を用いて説明する。

図 2、図 3、及び図 4はターゲットマークの斜視図、上面図及び側面図である。図に示すように、ターゲットマーク 1 0は. 黒円 1 1の中央に設けられた支柱 1 3に白三角形 1 2を載せて構成したものである。白三角形 1 2は、支柱 1 3の高さ分だけシフトしている。黒円 1 1の背後には、詳細は後述するように- 白領域が設けられる。また、支柱 1 3は、黒円 1 1 と同様に黒色に塗られている。なお、支柱 1 3は、对象物体 1の移動態様によってカメラ 2 0の撮影領域に入ってこないことが明らかな場合は、必ずしも黒色である必要はない。このタ一ゲットマ一ク 1 0は、上述したように対象物体 1 に設けられ、そのターゲットマーク 1 0をカメラ 2 0が撮像する。ターゲットマ一ク 1 0の抽出及びターゲットマーク 1 0のずれ量の検出は、その画像データを基にして行われる。以下に、まず図 5〜図 1 3を参照して画像の中からのターゲットマーク 1 0の抽出方法を、さらに図 1 4〜図 5 0を参照してタ一ゲットマーク 1 0のずれ量の検出方法を説明する。

図 5はタ一ゲットマークの検出手順を示すフ口一チャートである。この実施例では、投影画像中から、円の内部に三角形が配置されたターゲットマーク画像を検出するものとする。このターゲットマーク画像の X Y投影ヒストグラム（ヒストグラム）を求めると、図 6 に示すように、その極値（極大値または極小値で、以下、「山」または「谷」とも称する）の数は X、 Yともに山が 2つ、谷が 1つであり、この特徴はターゲットマークが回転しても数が変化しないことである。なお、 X投影ヒストグラムは同じ X座標値を持つ画素の合計値、 Y投影ヒストグラムは同じ Y座標値を持つ画素の合計値である。図 5の中で. Sに続く数字はステップ番号を表す。

〔 S 1 〕まず、原画像をカメラ 2 0で撮影してその原画像デ一タを画像処理装置 4 0 の画像メモリに取り込む。

〔 S 2〕次いで、その画像メモリ内の画像データを 2値化する。この処理の結果、例えば図 7 に示すような 2値化画像が得られる。

〔 S 3〕次に、ラベリング処理によつて、 2値化画像中において同じ値（ 0または 1 ) を持ち、連結している画素の集合からなる各画像に対し、それぞれ異なる番号（ラベル）を付与してグループ化する。図 8はこのラベリング処理結果の例を示すものであり、図 7の 2値化画像はグループ審号 1〜 8の 8個のグループにそれぞれグループ化される。

このようにして求めた各グループ番号 1〜 8の各画像に対してステップ S 5〜S 1 1のターゲットマークの検出処理を行う <

〔 S 4〕まず、グループ審号 nを、 n = l とする。

〔 S 5〕グループ番号 nをグループ数 8と比較し、グループ番号 nがグループ数 8より小さいならば、ステップ S 5へ進み- グループ数 8に達したならば、ステップ S 1 2へ進む。

〔 S 6〕処理対象の画像データ中からグループ審号 nの画像を切り出す。つまり、ラベリング処理によってグループ化された各画像をゥィンドウ処理によりグループ毎に分離してそれぞれの 2値画像を作成する。図 9 は、例えば、目的とするターゲットマ一クであるグループ蕃号 3の画像に対して、このタ一ゲットマーク検出処理が行われた場合を示す。

[ S 7 ] このようにして切り出した画像に対して面積によるフィルタリングを行う。すなわち、計測範囲において目的とするターゲットマークの面積（画素数）がとり得る値の最大値と最小値を予め算出し、その範囲内にグループ番号 nの画像が入つているか否かでフィルタリングをする。これにより、切り出した画像がターゲットマークであるか否かの大雑把な判定を行うことができる。この面積によるフィルタリングで検出対象から除外されなかった場合には、ステップ S 8へ進み、除外された場合にはステップ S 1 1へ進む。

〔 S 8〕次にそのグループ蕃号 nの画像について X Y投影ヒストグラムを求め、その結果をメモリに格納する。図 1 0には、例えば、グループ蕃号 3の画像に対してこの処理が行われた結果を示す。

C S 9 j このようにして求めた X Y投影ヒストグラムにおいて山を検出し、その数をカウントする。なお、山の代わりに、谷であつてもよい。

〔 S 1 0〕この山の数が、図 6のターゲットマークの X Y投影ヒストグラムにおいて予め求めてある山の数と一致するか否かを判定する。図 6のターゲットマークにおいては、山の数が X、 Yともに 2つであるという特徵を有し、グループ審号 3の画像はその条件に一致する。一致するならばステップ S 1 3へ進み、一致しなければステップ S 1 1へ進む。なお、山の代わりに谷である場合は、図 6のターゲットマークにおいては、谷の数が X、 Yともに 1つであるという特徴を有し、グループ審号 3の画像はその条件に一致する。

〔 S 1 1 〕グループ審号を一っィンクリメントする。

〔 S 1 2〕グループ番号 nがグループ数 8に達してしまった場合は、ターゲットマ一クを画像の中から抽出できなかったとして本プログラ厶を終了する。

〔 S 1 3〕切り出した画像が図 6のタ一ゲットマークの画像であると判定して、その画像のグループ番号 η (この実施例では 3 ) を呈示する。

〔 S 1 4〕マーク検出（抽出）に成功したものとして本プログラムを終了する。

なお、上記マーク抽出方法において、検出対象のターゲットマークは図 6の形態のものに限られるものではなく、例えば図 1 1 または図 1 2 に示すようなものであってもよい。かかる形態のマークの場合、 X投影では山は 3つ、谷は 2つ、 Y投影では山は 2つ、谷は 1つとなることを特徴として、マークの検出ができる。

また本実施例では、上述の図 6 と図 1 1 のような異なる 2種類のマークを用いることによって、それが付けられている物体の位置 · 姿勢を計測することもできる。すなわち、図 1 3に示すように、図 6のマークを原点マーク M l とし、図 1 1のマークをサブ原点マーク M 2 とし、それらを結ぶ線とその中点でほぼ直交する線の右左にそれぞれ円 M 3 、 M 4を配し、この 4つの円 M 1 〜M 4で 4円ターゲットマークを構成する。この 4円タ—ゲットマークの位置 · 姿勢を計測することで、その 4円ターゲットマ一クの付された物体の位置♦姿勢を推定するものであるが、その処理方法は以下のようなもので-ある。

① 前述のマーク検出方法により原点マーク M 1を検出し、そのマーク M 1の 2値化画像からその重心位置座標（原点）を求める。

② 同様にしてサブ原点マーク M 2の重心位置座標（サブ原点） ¾ "求 toる。

③ 原点の座標とサブ原点の座標との中点の座標を求める。

④ 重心位置がこの中点の座標に近い物体を 2つ選び、これをターゲットマークの残りの 2つの円 M 3、 M 4 とする。

⑤ この段階では 2円のいずれが M 3であり、いずれが M 4 であるかの特定はできない。そのため、原点の座標とサブ原点の座標とからそれらを結ぶ直線の方程式を導き、 ④で求めた 2 円の座標を直線の方程式に代入することによって、円 M 3、 M 4が、この直線の方程式の上側にあるか、あるいは下側にあるかをそれぞれ判別して 2つの円 M 3、 M 4を特定する。

⑥ 以上により、 4円ターゲットマークの平面上の 4点の座標が求められるので、これの 4点を使って 4点透視問題を解き- その 4円ターゲットマークの位置 ·姿勢を計測する。

以上のターゲットマークの検出（抽出）にあたっては種々の変形形態が可能である。特に、検出対象のターゲットマークは実施例のものに限られるものではなく、 X Y投影ヒストグラムを求めたときにその極値によつて特徵づけられる種々の形態のマークを本手法によって検出できるものである。この極値による特徴抽出は、極大値と極小値を合わせたもので特徴抽出するものであっても、また何れか一方により特徴抽出するものであつてもよい。

以上のように、本ターゲットマークの検出手法によれば、画像中に多くの物体が混在したり、ノィズが多い画像であつたりした場合にも、従来手法によりも確実かつ高速にターゲットマ -クを検出することができる。

つぎに、以上のようにして検出されたターゲットマーク 1 0 におけるずれ量を検出する方法を、図 1 4以降の図を参照して説明する。

図 1 4はタ一ゲットマ一クの X, Y方向ずれ量の求め方を説明する図である。図はタ一ゲットマーク 1 0がカメラ 2 0の画面中心からずれて写っている場合を示す。カメラ 2 0が撮像したタ一ゲットマ一ク 1 0の画像（ターゲットマーク画像） 1 0 Aは、画像処理装置 4 0の画像メモリ 4 1 に取り込まれ、画像座標系 4 2 において認識される。ターゲットマーク画像 1 0 A は、黒円画像 1 1 A及び白三角形画像 1 2 Aから成る。ここで，画像メモリ 4 1 は、 X方向、 Y方向ともに 5 1 2 ピクセルから構成され、画像メモリ 4 1 内の各点の座標位置はピクセルを単位として表示される。

この画像座標系 4 2 において、ターゲットマ一ク 1 0の X方向及び Y方向のずれ量は、タ一ゲットマーク画像 1 O Aの目標位置 P o ( t X , t y ) と、ターゲットマーク画像 1 O Aの中心位置 P m (m x， m y ) との差分として、次式（ 1 ) で表される。

X方向のずれ量 1 = m X - t x

Y方向のずれ量 1 y = m y— t y ( 1 ) 目標位置 P o ( t x , t y ) を画像メモリ 4 1の中心点 P o ( 2 5 6 , 2 5 6 ) とし、タ一ゲットマーク画像 1 0 Aの中心位置 Pm (mx, m y ) を Pm ( 1 5 6, 3 5 6 ) とすれば、 X方向のずれ量 l x二 1 5 6— 2 5 6 =— 1 0 0、 Y方向のずれ量 l y = 3 5 6— 2 5 6 = 1 0 0となる。

実際には、タ一ゲットマ一ク 1 0のずれ修正は、力メラ座標系 2 2で行うので、上記の画像座標系 4 2でのずれ量 l x, 1 yは、次式（ 2 ) によってカメラ座標系 2 2でのずれ量 L X , L yに変換される。

L X = 1 y

L y =- 1 x ( 2 )

上記の例では、 L x = 1 0 0， L y =_ 1 0 0となる。

次に、ターゲットマーク画像 1 O Aの中心位置 Pm (mx, my ) の求め方について説明する。

図 1 5はターゲットマ一クの中心位置 P mの求め方についての説明図である。図において、ターゲットマーク 1 0 Aの中心位置 Pm (mx, my) は、黒円画像 1 1 Aの中心点に相当する。画像処理装置 4 0は、 2値化したターゲットマーク画像 1 0 Aの各画素値「 0」、「 1」を積算して X軸方向の投影ヒストグラム（ヒストグラム） 1 5及び Y軸方向の投影ヒストグラム 1 6を作成し、続いて、その投影ヒストグラム 1 5及び 1 6 から中心位置 Pm (m x , my) の X座標及び Y座標を求める < すなわち、中心位置 Pm (mx, my) の X座標 mxは、 X軸方向の投影ヒストグラム 1 5から得られる両端位置 X b 1及び X b 2を用いて、次式（ 3 ) から求められる。

mx - (X b l +X b 2 ) / 2 ( 3 ) また、中心位置 Pm (m X , my) の Y座標 myは、 Y軸方向の投影ヒストグラム 1 6から得られる両端位置 Y b 1及び Y b 2を用いて次式（ 4 ) から求められる。 ' m y = (Y b l + Y b 2 ) / 2 ( 4 ) このようにして求められた中心位置 P m (m x， m y ) と、予め設定されている目標位置 P o ( t X , t y ) との差分が、上述したように、ターゲットマーク 1 0の X方向及び Y方向のずれ量となる。次にターゲットマーク 1 0の Z方向ずれ量の求め方について説明する。

図 1 6 はターゲットマークの Z方向ずれ量の求め方を説明する図である。ターゲットマ一ク 1 0.の Z方向ずれ量は、ターゲットマーク画像 1 0 Aの面積 M a と目標面積（ 1 0 Bで示す円の面積） T a との差分から求められる。すなわち、画像処理装置 4 0 は、上述した X軸方向の投影ヒストグラム 1 5または Y 軸方向の投影ヒストグラム 1 6のいずれかを全域にわたって積算することによってターゲットマ一ク 1 0 Aの面積 M aを求め, その面積 M a と予め設定されている目標面積 T a との差分から, Z方向のずれ量を求める。例えば、目標面積 T aを 1 0 0 0 0 ピクセル、タ一ゲットマーク画像 1 O Aの面積 M aを 6 0 0 0 ピクセルとすると、 Z方向のずれ量 L zは、 T a— M a = 4 0 0 0 ピクセル（ 1 0 0 0 0 — 6 0 0 0 ) に応じた値をとる。このずれ量 L z は、上記のずれ量 L x， L yと同様に、カメラ座標系 2 2でのずれ量である。

次にカメラ座標系 2 2の姿勢ずれ量の求め方について説明する。

図 1 7 はターゲットマ一クを斜めから見た場合の説明図であり、（A) はカメラの姿勢を、（B) はターゲットマーク画像をそれぞれ示す。カメラ 2 0がターゲットマ一ク 1 0を斜めから撮像する場合、力メラ座標系 2 2はョ一方-向、ピッチ方向、 —ル方向にずれている。なお、ョー方向、ピッチ方向及び σ ール方向のずれとは、カメラ座標系 2 2 (図 1 6参照）での X 軸回り、 Υ軸回り及び Ζ軸回りの回転をいう。ここで、ョ一方向及びピッチ方向のずれによって、カメラ 2 0が図 1 7 ( Α ) に示す姿勢をとつたとすると、そのときのターゲットマ一ク画像 1 0 Αは、図 1 7 ( B ) に示すように、白三角形画像 1 2 A が黒円画像 1 1 Aに対してずれているように見える。このように、白三角形画像 1 2 Aがずれて見えるのは、白三角形 1 2が黒円 1 1から支柱 1 3の分だけシフトしているからである。この白三角形画像 1 2 Aのずれを検出することによって、ョ一方向及びピッチ方向のずれ量が求まる。その求め方について以下に説明する。

図 1 8は図 1 Ί ( B ) のターゲットマーク画像の X , Y投影ヒストグラムを示す図である。図に示すように、 X軸投影ヒストグラム 1 7及び Y軸投影ヒストグラム 1 8では、黒円画像 1 1 Aに対する白三角形画像 1 2 Aのずれに对応して、その山 (ピーク）の位置及び谷（π —ピーク）の位置がずれて現れる _t この X軸投影ヒストグラム 1 7及び Y軸投影ヒストグラム 1 8 からョー方向及びピッチ方向のずれが求まる。

図 1 9 はョ一方向ずれ量の求め方を説明する図である。図に示した X軸投影ヒストグラム 1 7から、先ず、 X b 1、 X b 2 - X P 1、 X p 2を求める。 X b 1及び X b 2は X軸投影ヒストグラム 1 7の両端座標値である。この X b 1 と X b 2の間に黒円画像 1 1 Aが位置している。 X p 1及び X p 2は X軸投影ヒストグラム 1 7の山の座標値である。この X p l と X p 2 との間に白三角形画像 1 2 Aが位置している。ョ一方向ずれ量 Lya を求めるには、この白三角形画像 1 2 Aの部分（X p 1〜X P 2 ) が黒円画像 l l A (X b l ~X b 2 ) のどの部分に位置するかを求めればよい。すなわち、黒円画像 1 1 A (ターゲットマーク画像 1 O A) 全体の中心位置 X b c と白三角形画像 1 2 Aの中心位置 X p c との差を求めればよく、次式（ 5 ) 、 ( 6 ) 及び（ 7 ) を用いて求めることができる。

X p c = (X p l + X p 2 ) / 2 ( 5 )

X b c = (X b l + X b 2 ) / 2 ( 6 )

L aw = X b c - X p c ( 7 ) ここで、 X p c ：白三角形画像 1 2 Aの X軸方向中心位置

X b c ： X軸投影ヒストグラム 1 7の中心位置（黒円画像 1 1 Aの X軸方向中心位置）

L yaw ：ョ一方向ずれ量

図 2 0 はピッチ方向ずれ量の求め方を説明する図である。ピツチ方向ずれ量 L pitch は、ョ—方向ずれ量 Lyaw を求める場合と全く同様にして求めることができる。すなわち、先ず、 Y 軸投影ヒストグラム 1 8から Y b 1及び Y b 2 ( Y軸投影ヒストグラム 1 8の両端座標値）、並びに Y p 1及び Y p 2 (Y軸投影ヒストグラム 1 8の山の座標値）を求め、これらの値を用いて、次式（ 8 ) 、（ 9 ) 及び（ 1 0 ) から求めることができる

Y p c = (Y P 1 + Y P 2 ) / 2 ( 8 )

Y b c = (Y b l + Y b 2 ) / 2 ( 9 )

L pitch = Y b c - Y p c ( 1 0 ) ここで、 Y p c ：白三角形画像 1 2 Aの Y軸方向中心位置

Y b c ： Y軸投影ヒストグラム 1 8の中心位置（黒円画像 1 1 Aの Y軸方向中心位置）

Lpitch ：ピッチ方向ずれ量

次に力メラ座標系 2 2が口一ル方向に回転した場合の口ール方向ずれ量の求め方について説明する。

図 2 1は口ール方向に回転したタ一ゲットマークの画像及びその X軸投影ヒストグラムを示す図である。力メラ座標系 2 2 が口一ル方向に回転すると、ターゲットマーク画像 1 0 A内の白三角形画像 1 2 Aにおいて、図に示すように、その一頂点 1 2 Cが回転し、その一頂点 1 2 Cの位置に応じて、 X軸投影ヒストグラム 1 9の谷の位置も変化する。この点に着目して、口ール方向のずれを図 2 2に示すように、 X軸投影ヒストグラム 1 9から求める。

図 2 2はロール方向ずれ量の求め方を説明する図である。口 —ル方向ずれ量 Lrollを求めるには、先ず、 X軸投影ヒストグラム 1 9から、 X p l、 X p 2及び X l pを求める。 X p 1及び X P 2は X軸投影ヒストグラム 1 9の山の座標値（白三角形画像 1 2 Aの X軸方向両端の座標値）である。 X 1 pは X軸投影ヒストグラム 1 9の谷の座標値（白三角形画像 1 2 Aの一頂点 1 2 Cの X軸方向座標値）である。口—ル方向ずれ量 Lroll は、これらの値を用いて、次式（ 1 1 ) 及び（ 1 2) から求めることができる。

X p c = (X p l +X p 2 ) X2 · ♦ . · ♦ ( i i )

Lroll=X p c -X l p ( 1 2) ここで、 X p c ：白三角形画像 1 2 Aの X軸方向中心位置 L rol 1：ロール方向ずれ量

前述した位置ずれ演算装置 5 0では、このようにして求めたずれ量 L x、 L y、 L z、 Lyaw 、 Lpitch 及び Lroll (これらを代表してしとする）に、次式（ 1 3 ) に示すように、比例ゲイン k ( k X、 k y、 k z、 k yaw 、 k p i tch 及び k ro 11 ) を掛け、ロボット 3 0に対する速度指令値 V ( V X、 V y、 V z、 V yaw 、 V pitch 及び Vroll) を作成する。

V = k · L ( 1 3 )

ここで、 V、 k、 Lはともにベクトルである。

この速度指令値 Vは位置ずれ演算装置 5 0から口ボット制御装置 6 0に送られる。速度指令値 Vは、カメラ 2 0の移動速度 (カメラ座標系 2 2の原点の移動速度）、すなわちカメラ 2 0 を搭載しているロボット 6 0の手先の移動速度を示している。このため、ロボット制御装置 6 0は、その速度指令値 Vから、ロボット制御装置 6 0のサンプリング時間毎の移動量を計算し，その移動量を指令して口ボット 3 0の動作を制御する。口ボット制御装置 6 0のサンプリング時間が、速度指令値 Vが位置ずれ濱算装置 5 0からフィードバックされてくる時間（計測時間）より短いときは、その速度指令値 Vを数サンプリング先の指令値としてその間を補間する。例えば、計測時間がサンプリング時間の 5倍ならば 5サンプリングの動作でその指令位置に移動するように、ロボット 3 0の動作を制御する。

次に、ターゲットマ一ク画像 1 0 Aの画像処理時に行われるウィンドウ設定方法について説明する。

図 2 3はウィンドウ設定方法の説明図である。図では、カメラ 2 0とタ一ゲットマーク 1 0とが垂直でないため、ターゲットマ一ク画像 1 0 Aが偏平に見える場合を示す。ターゲットマーク画像 1 0 Aは、画像処理装置 4 0の画像メモリ 4 1 に取り込まれてマーク認識が行われ、認識されたターゲットマーク画像 1 O Aの回りにウィンドウ 4 4が設定される。

すなわち、マーク認識時に投影ヒストグラム処理によって得られたターゲットマ一ク 1 0 Aの重心位置をゥインドウ 4 4の中心とする。また、ウィンドウ 4 4のサイズ W 1 X及び W 1 y は、同様にマーク認識時に投影ヒストグラム処理によって得られたタ一ゲットマーク 1 0 Aの縦、横のサイズ m 】 X及び m 】 yに、予め設定した比率、例えば 2倍を掛けて得る。この場合、ウィンドウ 4 4の領域を画像メモリ 4 1上に確実に確保するため、予め対象物体 1上でタ一ゲットマーク 1 0の黒円 1 1の周囲に、ターゲットマ一ク 1 0の縦、撗サイズに対して設定比率を掛けた分より少し大きくなるように白領域を設けておく。こうすることにより、画像メモリ 4 1 に常時、白領域 4 3を確保することができる。このため、ターゲットマーク 1 0 とカメラ 2 0 との距離が変化して画像メモリ 4 1 に取り込まれたターゲットマ一ク画像 1 0 Aの大きさが変化しても、ターゲットマ一ク画像 1 0 Aは白領域 4 3からはみだすことなく、タ一ゲットマーク画像 1 O Aの回りにウィンドウ 4 4を設定することができる。

このように、ターゲットマ一ク画像 1 O Aの回りに白領域 4 3を確保し、その白領域 4 3にウィンドウ 4 4を設定することにより、ノイズなどのターゲットマーク画像 1 0 A以外の黒領域を除去することができる。

このウィンドウ設定は、ターゲットマ一ク 1 0 Aの追跡スタート時には、上述したようにマーク認識時に得られた投影ヒストグラム情報を用いて行われるが、その後の追跡時には、カメラ 2 0 による計測時毎に得られる新たな投影ヒストグラム情報を用いて行われる。

図 2 4はターゲットマ一クの他の例を示す図であり、（ A ) は斜視図、（B ) は側面図である。この例でのターゲットマーク 1 0 0 は、黒円 1 1 0の中央に設けられた三角柱 1 3 0の上面に白三角形 1 2 0を描いて構成したものである。三角柱 1 3 0 は、白三角形 1 2 0の部分以外では黒円 1 1 0 と同様に黒色に塗られている。

図 2 5 はターゲットマ一クのさらに他の例を示す図である。この例でのタ一ゲットマーク 1 0 1 は、黒円領域を削って低くし、その低くして形成した黒円領域 1 1 1の底に白三角形 1 2 1を置いたものである。すなわち、白三角形 1 2 1を負の方向にシフトして構成したものである。この例では、黒円領域 1 1 1の凹形状を角状にしたが、他の形状、例えば、お碗型形状とすることもできる。

図 2 6 は図 2 5のタ一ゲットマークの使用例を示す図である _c 上記のターゲットマーク 1 0 1 は、ドッキングなどのように、二つの物体が密着するようなときに用いることができる。例えば、図に示すように、一方の物体 1 Aにターゲットマーク 1 0 1 を設け、他方の物体 1 Bにカメラ 2 0を設け、カメラ 2 0でタ一ゲットマーク 1 0 1を撮像しつつ、物体 1 A及び 1 Bをドッキングさせる。この場合、ターゲットマーク 1 0 1を撮像するカメラ 2 0がタ一ゲットマ一ク 1 0 1へ接近し、両物体 1 A 及び 1 Bが接触しても、カメラ 2 0 はターゲットマーク 1 0 1 を撮影を継続することができる。したがって、その画像データを用いてドッキングを完了させることができる。

図 2 7はターゲットマ一クの追跡と物体の把持とを実行するためのフローチャートを示す図である。図において、 Sに続く数値はステップ審号を示す。このフローチャートの各ステップの内、ステップ S 2 1〜ステップ S 2 5は画像処理装置 3 0側において、ステップ S 2 6〜ステップ S 3 2は位置ずれ演算装置 5 0側においてそれぞれ実行される。

〔 S 2 1〕カメラ 2 0からの撮像データに基づいてマーク認識を行う。 '

C S 2 2 ウィンドウ 4 4の設定を行う。

〔 S 2 3〕原画を取り込む。

〔S 2 4〕 2値化を行う。

〔 S 2 5〕 X軸方向及び Y軸方向の投影ヒストグラム 1 5及び 1 6を求める。

〔 S 2 6〕タ一ゲットマ一ク 1 0の確認を行う。

〔 S 2 ?〕ターゲットマーク 1 0が存在するか否かを判別する _{ 存在する場合は次のステップ S 2 8に進み、そうでなければステツプ S 2 1 に戻る。以上のステップ S 2 1〜 S 2 7は、図 5 に示すマーク検出手順に相当する。

〔 S 2 8〕ターゲットマーク 1 0のずれ量 L X、 L y、 L z、 L yaw 、 L pitch 及び Lrollを求める。

L S 2 9 ] 速度指令値（制御指令値） Vを作成する。

〔 S 3 0〕速度指令値 Vを出力する。

〔 S 3 1〕次回の計測のためのゥインドウ 4 4の位置、サイズを決定する。〔 S 3 2〕ロボット 3 0からのセンサ信号によつて対象物体 1 を把持したか否かを判別する。把持したと判別した場合はそのまま終了し、そうでなければステップ S 2 2に戻る。

以上述べたように、本実施例では、黒円 1 1の中央に白三角形 1 2を立体的にシフトさせて構成したターゲットマ一ク 1 0 をカメラ 2 0を用いて撮像し、その画像データからターゲットマーク 1 0のずれ量 L X、 L y、 L z、 L yaw 、 L p i t ch 及び L ro l lを求める。これらのずれ量 L x等は、画像データの各軸投影ヒストグラム処理から求められるが、この各軸投影ヒストグラム処理は、一次元領域での計算である。このため、その計算は、複雑な I†算を必要とせず非常に簡単に行うことができ、計算量も僅かである。したがって、ターゲットマーク 1 0のずれ量 L x等は、高速処理で求められる。その結果、ずれ量 L x 等に応じたロボット 3 0へのリアルタイムなフィードノ、'ックが可能となり、カメラ 2 0 によるターゲットマ一クの追跡がリァルタイムで可能となる。ロボット 3 0 は、その追跡動作によつて対象物体 1 を把持し所定の作業目的を達成することができる, 上記の説明では、ターゲットマークを黒円と白三角形で構成したが、その色を逆に組み合わせて白円と黒三角形で構成することもできる。

ところで、上記の実施例の装置では、ターゲットマ一クの実移動量（m mなどの絶対的なスケール）と、画像上でのその画像移動量（ピクセル数）との関係が、ターゲットマークとビデォカメラとの間の距離により非線型に変化するために、ターゲットマークとビデオ力メラとの間の距離が大きく変化する場合には、応答性能が劣化し、安定に動作させることが困難になる, すなわち、画像上でのターゲットマークの同一の画像移動量に対し、タ一ゲットマークの実移動量は、タ'一ゲットマークとビデオ力メラとの間の距離が異なることによつて異なってしまう o

こうした問題点を解決して、ターゲットマークとビデオカメラとの間の距離が大きく変化した場合においても、応答性能が劣化せず、安定に移動物体の追跡を行えるようにした他の実施例を、つぎに説明する。

図 2 8は、ビデオカメラ 2 0 とターゲットマ一ク 1 0 との所定の位置関係を示し、（A ) は、ビデオカメラ 2 0 とターゲットマーク 1 0 とが所定の位置関係にあるときの座標位置関係を示す図であり、（B ) は、そのときにビデオ力メラ 2 0で撮影されたターゲットマーク 1 0の画像を示す図である。図中、 0 - X y zはビデオ力メラ 2 0に固定された右手系の座標系を示し、 0— X Y Zはターゲットマ一ク 1 0に固定された右手系の座標系を示す。 z軸と Z軸とが同一直線上に相反する方向を向いて設定され、 X軸と Y軸とが平行、 y軸と X軸とが平行になるように設定される。ビデオ力メラ 2 0 とタ一ゲットマ一ク 1 0 とが所定の位置関係にあるときには、ビデオカメラ 2 0で撮影されたターゲットマーク 1 0の画像は、図 2 8 ( B ) に示すような画像となり、これが目標位置にあるときのターゲットマーク 1 0の画像である。

ところで、ビデオカメラ 2 0 とターゲットマ一ク 1 0 との所定の位置関係（図 2 8に示す状態）がずれた場合、図 1の画像処理装置 4 0および位置ずれ渲算装置 5 0 は、ビデオカメラ 2 0により得られるターゲットマ一ク 1 0の画像をタ一ゲットマ —ク 1 0が目標位置にあつたときの画像と比較することにより各軸方向のずれ量を次のように求める。

• X , Y方向については、画面内でのターゲットマーク 1 0 の白三角板 1 2の中心位置のずれ量として、

• Z方向については、画面内でのタ一ゲットマーク 1 0の黒円板 1 1の面積のずれ量として、

• ピッチ，ョ一方向については、画面内でのターゲットマーク 1 0の黒円板 1 1の中心位置と、白三角板 1 1の中心位置との距離のずれ量として、

• ロール方向については、画面内でのターゲットマ一ク 1 0 の白三角板 1 2の回転（自転）ずれ量として、

それぞれ求める。

こうしたずれ量が、ビデオ力メラ 2 0 とターゲットマーク 1 0 との距離に依存して、実座標系でのずれ量に対し、非線型に変化するため、図 1 のロボット制御装置 6 0では、まず、両者のずれ量の間の関連性を実測によって求めて同定（関連付け）を行う。

すなわち、図 2 9 に示すように、タ一ゲットマーク 1 0を固定し、ロボット 3 0の 6 自由度マニピュレータ 3 0 aの先端を. ターゲットマ一ク 1 0 に固定された座標系（0— X Y Z ) において実験的に所定量動かし、そのときにビデオカメラ 2 0で得られたターゲットマ一ク 1 0の画像を画像処理することにより - ターゲットマ一ク 1 0の目標位置からの画面上でのずれ量を測定し、記録する。なお、ビデオカメラ 2 0を固定し、ターゲットマ一ク 1 0を 6 自由度移動可能なステージに載せ、動かすことにより実測してもよい。図 2 9中の 3 0 bは対象物体を把持するハンドである。

さらに詳しく言えば、ターゲットマ一ク 1 0の画面上でのずれ量、および実座標系でのずれ量の実測は、カメラ座標系（o - X y z ) をターゲット座標系（0— X Y Z) 上で、 x， y , z方向、ロール、ピッチ、ョ一方向（Z軸、 Y軸、 X軸回りの回転）にそれぞれ移動 ' 回転させ、そのときの所定位置からの実移動 · 回転量と、ターゲットマーク 1 0の画像のずれ量（単位はピクセル）を測定することで行う。

具体的には、図 3 0に示すように、まず、ビデオカメラ 2 0 を z軸方向の各点に移動し、 z軸の値（ビデオカメラ 2 0とタ —ゲットマ一ク 1 0との距離）と、そのときのターゲットマ一ク 1 0の画面上での面積 E z (ピクセル値）とを計測する。つぎに、ビデオカメラ 2 0を y， X軸正負方向に数点移動させ、そのときの実移動量 D x, D yと、ターゲットマーク 1 0の画面上でのずれ量 E x， E y (ピクセル値）とを計測する。また、ビデオカメラ 2 0をターゲット座標系（0— XY Z) の X, Y, Z軸回りに数点回転させ、そのときの実移動量 Dyaw , Dpitc h ， Drollと、ターゲットマーク 1 0の画面上でのずれ量 Eya w ， Epitch ， Eroll (ピクセル値）とを計測する。以上の計測を z軸方向の各点において行う。

なお、追跡フィ一ドバック制御を行なっている最中には、ビデォカメラ 2 0とタ一ゲットマーク 1 Qとの所定関係位置からのずれが比較的小さいことから、ビデオ力メラ 2 0を各軸方向にずらせる範囲は、所定関係位置近傍だけで十分である。

以上の測定を行うことで、

- ビデオカメラ 2 0とタ一ゲットマ一ク 1 0との距離 zと、画面上でのタ一ゲットマ一ク 1 0の面積 E z との z — E z関係 • ビデオカメラ 2 0 とタ一ゲットマ一ク 1 '0 との距離 zがー定である場合の各軸方向（ x， y , ロール，ピッチ，ョー）の実ずれ量 D* と、画面上でのずれ量 E* との D* — E* 関係

(*は添字であり、ここでは * = x， y , roll, pitch, yaw) • ビデオカメラ 2 0 とターゲットマーク 1 0 との距離 z と、上記 D* - E* 関係との z — （D* — E* ) 関係

に関するデータを得ることができる。

つぎに、図 1のロボット制御装置 6 0では、 z — E z関係に関するデータを z — E z平面にプロッ卜して、図 3 1 に示す曲線を得る。そして、この曲線を、近似的に 4次関数 z = f (E z ) として数式化する。したがって、この関数 z = f (E z ) を用いれば、画面上でのターゲットマ一ク 1 0の面積 E zから, ビデオ力メラ 2 0 とターゲットマーク 1 0 との距離 zを簡単に算出することができる。なお、ビデオカメラ 2 0がターゲットマーク 1 0 に正対しない場合に生じるターゲットマ一ク 1 0の黒円板 1 1 の画面上での楕円化は、面積 E z に殆ど影響はない _c また、上記 D* — E* 関係に関するデータから、各軸方向 ( X , y , ロール，ピッチ，ョー）の実ずれ量 D* と、画面上でのずれ量 E* との関係は、例えば図 3 2のようにプロットされる。図 3 2 は X軸方向の E x— D x関係を、上記 z — ( D* 一 E* ) 関係に関するデータをも使つて各距離 z毎にプロットしたものである。この図から分かるように、ビデオカメラ 2 0 とタ一ゲットマーク 1 0 との所定関係位置からのずれが比較的小さく、かつ、ビデオカメラ 2 0 とターゲットマーク 1 0 との距離 zが一定である場合、 E x— D x関係は、原点を通る直線で近似することができる。これは他の各軸方向についても同様 t、ある o ·

ところで、上記直線において、その傾きは、距離 Zの値の関数になっているとみることができる。そこで、例えば図 3 2を基に、直線の傾き A xを距離 zに対しプロットすると、図 3 3 の曲線が得られる。図 3 3は X軸方向の z — A X関係を示している。ここで、図 3 3の曲線を、近似的に 2次関数 A x = g

( z ) として数式化する。同様に、他の各軸方向についても、 A * = g * ( z ) ( * = y , roll, pitch, yaw) として数式ィ匕することができる。

したがって、以上の同定の結果から、画像上での目標位置からのタ一ゲットマ一ク 1 0のずれ量 (単位はピクセル）を実ずれ量 D* に高速に変換することができるようになる。

図 3 4は、画像上のずれ量 E* を実ずれ量 D* に高速に変換する手順を示すフローチャートである。図中、 Sに続く数字はステツプ審号を表す。

〔S 4 1〕図 1の画像処理装置 4 0および位置ずれ演算装置 5 0から出力される、画像上でのタ一ゲットマーク 1 0の黒円板 1 1の面積 E zを、予め同定の結果求められた関数 z = f

(E z ) に代入し、ビデオ力メラ 2 0とタ一ゲットマーク 1 0 との距離 zを求める。

〔S 4 2〕ステップ S 4 1で求めた距離 zから、ビデオカメラ 2 0とタ一ゲットマ一ク 1 0とが所定闋係位置にあるときの両者の距離 Z tを減算することで、 z方向の実ずれ量 D zを求のる。

CS 4 3 ステップ S 4 1で求めた距離 Zを、予め同定の結果求められた関数 A* = g * ( z ) ( * = x , y , roll, pitc h， yaw) に代入し、各軸方向の傾き A* を算出する。

〔 S 4 4〕ステップ S 4 3で算出された各軸方向の傾き A* と、図 1の画像処理装置 4 0および位置ずれ演算装置 5 0から出力される、画像上での各軸方向の目標位置 ·姿勢からのずれ量 E* とを、 D* = E* / A * ( * = x , y , roll, pitch, y aw) に代入して、各軸方向の実ずれ量 D* を求める。

以上の処理で求められた実ずれ量 D* は、実座標での絶対的スケール（位置ならば mm、姿勢ならばラジアン等）で表され、図 3 5 に示すように、並進成分 D x， D y , D zは、カメラ座標系 0 — x y zでの y軸、 X軸、 z軸方向の各ずれ量であり、回転成分 Droll, Dpitch ， Dyaw は、ターゲット座標系〇一 X Y Zで記述されたずれ量である。

図 3 1〜 3 3で示した各関係図に対応する具体的なデータを図 3 6 ~ 3 8において示す。図 3 6 は z — E z関係を示す具体的な数値のグラフである。図 3 7 は y軸方向の E y— D y関係を各距離 z毎にプロットしたものである。図 3 8は y軸方向の z— A y関係を示すグラフである。

つぎに、以上のようにして変換された実ずれ量 D* ( * = X , y , roll, pitch, yaw) を用いて、図 1のロボット制御装置 6 0が、ロボット 3 0のマニピュレータ 3 0 aへ送る速度指令値を作成し、それによつてターゲットマークの実時間追跡制御を行うが、その制御手順の 2つの実施例を、図 3 9および図 4 1 に示すフローチャートに従って、以下に説明する。

図 3 9 はその第 1の制御実施例を示すフローチャートである。第 1の制御実施例は、ビデオ力メラ 2 0をタ一ゲット座標系〇 -X Y zの原点〇を中心として回転することで、位置修正を行うものであり、この第 1の制御実施例は、姿勢に関するずれ量が比較的小さい場合に有効な方法であり、厳密さを欠くが処理時間が短いという特徴がある。図中、 Sに続く数字はステップ番号を表す。

〔 S 5 1〕ビデオ力メラ 2 0からターゲット.マーク 1 0の画像を入力する。

〔S 5 2〕画像処理装置 4 0および位置ずれ演算装置 5 0で，画面上でのずれ量 E* ( * = X , y , z， roll, pitch, yaw) を検出する。

CS 5 3 αボット制御装置 6 0で、ずれ量を実ずれ量 D* ( * = X , y， z， roll, pitch, yaw) に変換する。

CS 5 43 さらにロボット制御装匿- 6 0で、ステップ S 5 3 で変換された実ずれ量 D* に、比例ゲイン K* (* = X, y , z , roll, pitch, yaw) を次式（ 1 4 ) のように乗じて、速度指令値 V* (* = x， y , z， roll, pitch, yaw) を算出する。

Vx 0 K_y 0 0 0 0 Dx

Vy x 0 0 0 0 0 Dy

V_z 0 0 Kz 0 0 0 Dz

Vroll 0 0 0 Kroll 0 0 Droll

V Pitch 0 0 0 0 0 Kyaw D pitch

0 0 0 0 K pitch 0 Dyaw ( 1 4 ) の速度指令値 V* は、図 4 0に示すように、回転成分に対し、カメラ座標系 o _ x y zをビデオカメラ 2 0 とターゲットマーク 1 0 との距離 Z tだけ z軸方向にずらした新座標系 o ^R - X ^R y ^R z ^R での速度指令値となるように変換される。これにより、ビデオカメラ 2 0の姿勢のずれ（ピッチ、ョー方向）を近似的にタ一ゲット座標系〇一 X Y Zの原点 0を中心として回転することになる。

〔 S 5 5〕ステップ S 5 4で算出された速度指令値 V* をマニピユレータ 3 0 aへ出力する。

〔 S 5 6〕ステップ S 5 5での速度指令値 V* の出力の結果. ビデオ力メラ 2 0 とターゲットマーク 1 0 とが所定位置関係になり、その結果、ハンド 3 0 bが対象物体 1を把持できたか否かを判別し、把持できないならば、ステップ S 5 1へ戻る。

図 4 1 は第 2の制御実施例を示すフローチャートである。第 2の制御実施例は、ターゲット座標系 0— X Y Zでの実ずれ量 D* をカメラ座標系 0— x y zでの実ずれ量に変換し、これによりビデオカメラ 2 0を位置制御するものである。この第 2の制御実施例は、処理時間が若干かかるが、厳密な制御方法であり、姿勢に関するずれ量の大きさに拘らず有勃な方法である。図中、 Sに続く数字はステップ番号を表す。

[ S 6 1〕ビデオカメラ 2 0からターゲットマ一ク 1 0の画像を入力する。

〔 S 6 2〕画像処理装置 4 0および位置ずれ演算装置 5 0で- 画面上でのずれ量 E* ( * = X , y , z， roll, pitch, yaw) を検出する。

〔 S 6 3〕ロボット制御装置 6 0で、ずれ量 E* を実ずれ量 D X ( * = X , y , z， roll, pitch, yaw )に変換する。〔 S 6 4〕さらにロボット制御装置 6 0で、ステップ S 6 3 で変換された実ずれ量 D* を、カメラ座標系- o - X y zでのずれ量に変換する。この変換の仕方を以下に説明する。

すなわち、ターゲット座標系 0— XYZからみたビデオカメラ 2 0の所定（目標）姿勢を表す回転行列を C d、ビデオカメラ 2 0の現在の姿勢を表す回転行列を C a、ターゲット座標系 0— XY Zでのビデオカメラ 2 0の姿勢のずれ量 Droll, Dpi tch ， Dyaw の回転変換を表す行列を D tとすると、次式（ 1 5 ) が成り立つ。

C a = D t C d * · · ( 1 5 ) これより、次式（ 1 6 ) が求まる。

C d = C a (C d -«D t -'C d) · ♦ · ( 1 6 ) ここで、 D a = C d— 'D t—tC dとすると、 D aは、図 4 2に示すように、現在の力メラ座標系 O ^a — X ^a y ^a Z ^a からみたビデオ力メラ 2 0の姿勢のずれ量の、所定位置の力メラ座標系 0 ^d - X ^d y ^d z ^d への回転変換を表す行列である。したがつて、 D aを算出して力メラ座標系 0 — X y zでの実ずれ量を求める。

〔S 6 5〕ステップ S 6 4で求めたカメラ座標系 0 — X y z での実ずれ量に定数ゲインをかけてビデオ力メラ 2 0の速度指令値を算出する。

〔S 6 6〕ステップ S 6 5で算出された速度指令値をマニピユレータ 3 0 aへ出力する。

〔S 6 7〕ステップ S 6 6での速度指令値の出力の結果、ビデォカメラ 2 0とターゲットマーク 1 0とが所定位置関係になり、その結果、ハンド 3 0 bが対象物体 1を把持できたか否かを判別し、把持できないならば、ステップ S 6 1へ戻る。

以上の第 2の制御実施例では、若干計算量は増えるが、より応答性能のよい追跡制御が実現できる。

以上説明した実施例ではいずれも、 z— E z関係および z— A * 関係を、それぞれ 4次曲線および 2次曲線で近似的に表せると見做し、適当な近似関数 z = f ( E z ) ， A * = g * ( z ) を設定して、それらの関数を使用して画面上のずれ量を実ずれ量に変換することが行われているが、他の方法として、 z - E z関係および z— A * 関係を、それぞれ直線で近似的に表せると見做し、適当な近似直線関数を設定するようにしてもよい。その場合には、変換誤差が大きくなるが、より高速に変換が行われ得る。また、 z— E _Z関係および z— A * 関係を関数に置き換えず、各テーブルとして保持し、これらテーブルによつて変換を行えば、各テ一ブルのための記憶容量が多少増えるが、より高速に変換が行われ得る。

以上の図 2 8〜 4 2 によって示したように、ビデオカメラとターゲットマ一クとの所定関係位置を実験的にずらし、そのときに得られる、画像上でのターゲットマークの目標位置からのずれ量と、ビデオカメラとタ一ゲットマークとの所定関係位置からの実ずれ量とから、相互の関連づけである同定を行い、結果的に、画像上でのターゲットマークの目標位置からのずれ量を基に、ビデオカメラとターゲットマークとの所定関係位置からの実ずれ量を求める。

このように、同定をすることにより、画像上でのターゲットマ一クの目標位置からのずれ量を、ビデオカメラとターゲットマークとの所定関係位置からの実ずれ量に高速に変換でき、ターゲットマークとビデオカメラとの間の距離が大きく変化した場合においても、応答性能が劣化しない、安定な移動物体の追跡を行える。

さらに、上記求められた実ずれ量に、比例ゲインをかけ、さらに回転成分に対し、ビデオカメラとタ一ゲットマークとの距離だけずらせた座標系での速度指令値を算出し、ビデオカメラの位置制御の速度指令値とし、これによつてビデオカメラの位置を制御する。これにより、ビデオカメラとターゲットマークとの所定関係位置からのずれ量が比較的小さい場合に、高速なフィードバックができ、応答性能のよい追跡制御が実現できる _t あるいは、上記求められた実ずれ量からビデオカメラに固定された座標系におけるビデオカメラの姿勢のずれ量を求め、定数ゲインをかけてビデオ力メラの位置制御の速度指令値とし、これによつてビデオカメラの位置を制御する。これにより、ビデォカメラとターゲットマ一クとの所定関係位置からのずれ量に拘らず追跡制御が正確に行われ、処理計算量が若干増えるが- より応答性能のよい追跡制御が実現できる。

以上の実施例では、ターゲットマークのロール方向ずれ量 L ro l l 〔数式（ 1 2 ) 、図 2 2参照〕が僅かであることを前提に考えてきたが.、このロール方向ずれ量 L ro l lが、ターゲットマ —クの 3角形の形状にもよるが、士 9 0度を越えるとどんな形状の 3角形でも、この口ール方向ずれ量 L ro l lを検出できなくなるという問題が発生する。 3角形の形状が鋭角 2等辺 3角形である場合には、もつと僅かな回転でも検出不可能になる。

また、上記実施例では、ロール方向のずれ補正を、ロール方向ずれ量 L ro l lの検出を繰り返し行うことにより処理するため. 補正時間を多く要すという問題や、口ール方向のずれ補正ゲインが対象物体の姿勢により非線型であるため、キヤリブレーションにより補正データを作成しないと安定収束する補償がないという問題がある。

さらに、図 4 3に示すように、ターゲットマーク画像において、 3角形の中心と背後の円の中心との X， Y方向の位置ずれが大きい場合、 3角形の頂点位置で投影ヒストグラムが最大値になるとは限らないことが生じる。したがって、上記実施例のように、例えば X方向の投影ヒストグラムにおける 2つのピーク点 X p l , X p 2の中点を 3角形の中心とするような処理をすると、位置ずれを過少検出してしまうため、ずれ補正に時間がかかることになるという問題がある。また、上記 2つのピーク点 X P 1， X p 2の検出を行う際に、ノィズ等の影響によりローカルピークを本来のピークと見做してしまうという危険性ある。

こうした問題点を解決した、タ一ゲットマークの検出方法の他の実施例を次に説明する。

まず、 3角形と背後の円とから成るターゲットマークの画像の X， Y方向の実測投影ヒストグラムにおいて、図 4 4に示すように、ヒストグラム値がそれぞれ所定値 5 x， <5 yである 2 点をそれぞれ求め、これらの 2点の中点 X c g , Y e gをそれぞれ求める。これら X c g， Y e gをターゲットマークの円の画像の中心位置（X c g， Y c g ) とする。上記の所定値 x， 5 yをノイズレベルよりも少し大きい値に設定することにより、ノィズの影響を受けることなく、ターゲットマ一クの円の画像の中心位置を検出することが可能となる。つぎに、この検出された円の画像の中心位置（X c g， Y c g ) と、この円の既知の幾何的データ（半径等）とから、この円の画像の X, Y方向の投影ヒストグラムを算出する。図 4 5

(B) は、こうして算出された円の画像の X， Y方向の投影ヒストグラムを示し、図 4 5 (A) は、 3角形と背後の円とから成るターゲットマークの画像の X， Y方向の実測投影ヒストグラムを示す。そして、図 4 5 (B ) の円の画像の算出投影ヒストグラムから、図 4 5 (A) の 3角形と背後の円とから成るターゲットマークの画像の実測投影ヒストグラムを減算することにより、図 4 5 (C ) に示す 3角形の画像の X， Y方向の投影ヒストグラムを得る。

こうして得られた 3角形の画像の X, Y方向の投影ヒストグラムを基に、下記のようにして 3角形のロール方向の回転姿勢を検出する。

まず、このターゲットマークの 3角形の形状を、鋭角 2等辺 3角形とし、その鋭角の頂角が 4 5度よりも小さいものと特定する。このターゲットマークの 3角形の画像において、図 4 6 に 2点鎖線で示すような 3角形 1 2 aの姿勢を基準姿勢とし、 3角形 1 2 aの重心 Gに対しロール方向へ回転した 3角形 1 2 bの反時計方向の回転角をとする。なお、 3角形 1 2 aの底角 Eと重心 Gとを結ぶ線が X軸方向となす角度を 0 とし、また、底角 Eと重心 Gとを結ぶ線が 3角形 1 2 aの辺に交差する点を Fとするとき、この線分 E Fの長さを L p (ピクセル数）とする。

ここで、図 4 7 に示すように、 3角形の画像の X， Y方向の投影ヒストグラムを用いて、重心 Gの座標（X g , Y g ) と、ヒストグラム値が最大になる X, Y座標値 Xm， Ym、およびそのときの最大値 H y， H xとを求める。 -

—方、これらの値 X g， Y g , X m, Ym, Η y , Η χの大きさに応じて、 3角形 1 2 bの回転姿勢は次のような姿勢パターンに分類される。

図 4 8， 4 9は回転角を漸増させたときの座標値 Xm， Y mに対する重心 Gの位置関係の推移を示す図であり、 2 4種類のパターンに分けられる。図中、 A， B， a , b, 0は座標値 Xm， Ymに対する重量（面積）バランスを示すための符号であり、 A > a 〉 0および B〉 b > 0 という重量（面積）の大小関係にある。また、矢印は座標値 Xm， Ymに対する重心 G

(黒点で表示）の存在する方向を示している。図中の枠の上にそれぞれ示した〔〕はパターン番号を表す。

各姿勢パターンにおける回転角は次のような式によって表される。

〔 1〕 0 · ♦ · ( 1 7 )

〔 2〕この姿勢パターンは図 4 6 に示すものであり、

θ = Θ。一 arcs in 〔 3 (Ym— Y g ) Z 2 L p〕

- ( 1 8 )

〔 3〕 θ = π/ 2 - θ ο • ( 1 9 )

〔 4〕 θ = π/ 2 - θ ο + arcsin [ 3 ( X g Xm) / 2 L p ] • ♦ ( 2 0 )

〔 5〕 θ = Θ o • ( 2 1 )

〔 6〕 θ = π/ 2 - 0 ο +arcsin [ 3 (X g Xm) / 2 L P〕 - - ( 2 2 )

〔 7〕 θ = π / 2 - ( 2 3 ) 〔 8〕 0 = π / 2 + Θ Q -arcsin C 3 {X g - Xm) 2 L

P〕 • ( 2 4 )

〔 9〕 Θ — π— Θ o · · ( 2 5 ) 〔 1 0〕 θ = π - Θ o + arcsin C 3 ( Y g - Ym) / 2 L p ]

( 2 6 )

〔 1 1 θ = π/ 2 + θ ο · · ( 7 ) C 1 2 θ - Θ o +arcsin C 3 (Y g— Ym) Z 2 L p〕

( 2 8 )

C 1 3 θ = π · ♦ ( 2 9 ) 〔 1 4 θ = π + Θ o -arcsin 〔 3 (Y g— Ym) Z 2 L P〕

( 3 0 )

〔 1 5 θ = > π/ 2 - θ ο · · ( 3 1 ) 〔 1 6 θ = 3 π 2 - θ ο + arcsin C 3 ( X m X g) / 2

L p ( 3 2 ) 〔 1 7 θ = π + Θ · · ( 3 3 ) [ 1 8 θ = π/ 2 - θ ο +arcsin [ 3 ( X m - X g ) / 2

L p〕 ( 3 4 ) 〔 1 9 Θ = ?> π / 2 · ♦ ( 3 5 )

〔 2 0 θ = π/ + θ ο -arcsin C 3 (Xm X g) X 2

L p〕 ( 3 6 ) 〔 2 1 Θ = 2 π - 0 o * · ( 3 7 )

〔 2 2 Θ = 2 π - Θ o +arcsin C 3 (Ym-Y g) / 2 L

P〕 - ( 3 8 )

〔 2 3 θ = 3 π^ 2 + θ ο ' · · ( 3 9 )

[ 2 4 θ = 2 π - θ o + arcsin [ 3 (Ym— Y g) / 2 L p ] • · · ( 4 0 ) 図 5 0は、以上の 2 4のパターンを、値 X g， Y g , Xm， Ym， H y , H xの大きさや大小関係から分類しこものである, なお、この中で太枠で囲んで示す姿勢パターンは、特異的に値 X g , Y g , Xm, Ym， H y， H xによって分類することが不可能となるものである。

前述のように、 3角形の画像の X, Y方向の実測投影ヒストグラムを用いて、重心 Gの座標（X g, Y g ) と、ヒストグラム値が最大になる X， Y座標値 Xm, Ym、およびそのときの最大値 H y， H Xとが求められたときに、これらの値を図 5 0 に示す分類と照合することによって、実測されたターゲットマークの 3角形がどの姿勢パタ一ンであるかを知ることができる _c ,さらに、上記式（ 1 7 ) 〜（ 4 0 ) によって、その回転角が幾らであるかも知ることができる。

ただし、図 5 0において太枠で囲んで示す姿勢パターンに闋しては、 2者のいずれであるか特定できない。そのため、この場合には、カメラをロール方向に僅か回転させて、太枠外の姿勢パターンにし、その後再び、回転角の検出を行うようにする o

こうして検出された回転角を用いて、前述の口ール方向ずれ量 L rollの検出を行うようにする。

なお、 3角形と背後の円とから構成されるターゲットマークにおける 3角形の重心 Gの座標（X g， Y g ) を算出する簡単な方法を、次に示す。

まず、ターゲットマークの画像の実測投影ヒストグラムを基にターゲットマーク画像の重心座標（X a g , Y a g ) を求め、また、既知のターゲットマ一クの円の面積 S cと 3角形の面積 S t とを基に、それらの比 (= S t ZS c ) を求める。なお，前述のようにターゲットマークの円の画像の'中心位置を（X c g, Y c g ) とする。

このとき、次式（ 4 1 ) ， ( 4 2 ) が成り立つ。

a g = ( r - X g + X c g ) / ( 1 + r ) ( 4 1 ) Y a g = ( r - Y g + Ύ c g) / ( 1 + r ) ( 4 ) これらを変形して、

X g = ί ( 1 + r ) X a g - X c g) ] / y ( 4 3 ) Y g = C ( 1 + r ) Y a ^ - Y c g) ] / Ύ ( 4 4 ) を得る。したがって、ターゲットマーク画像の重心座標（X a g, Y a g ) と、ターゲットマークの円の面積 S cと 3角形の面積 S t との比 r (= S t S c ) とから、ターゲットマークの 3角形の重心 Gの座標（X g， Y g ) を簡単に算出することができる。

以上の実施例では、ターゲットマークの鋭角 2等辺 3角形の頂角が、 4 5度より小さい場合の 3角形の π—ル回転を説明したが、この頂角が 4 5度以上で 6 0度より小さい場合の 3角形のロール回転に関しても、図 4 8〜 5 0に示した分類が少し変更になるものの、基本的には同じ発想により回転角が検出可、 &る。

以上のように、特定な形状の 3角形のターゲットマークを使用し、 3角形の画像の X， Y方向の実測投影ヒストグラムを用いて、 3角形の画像の重心 Gの座標（X g， Y g ) と、ヒストグラム値が最大になる X, Y座標値 Xm， Ym、およびそのときの最大値 H y， H Xとを求め、予め分類された姿勢パターンから 3角形の姿勢を回転角として求めるようにする。これにより、広範囲の口一ル回転角の検出が安定して行われ得るようになり、また、ロール回転角のずれ量が正確に検出できるので、位置ずれ補正の補正時間が少なくてすむようになる。また、上記の検出値 X g， Y g， X m， Y m， H y , H xは、画像処理の上では非常に簡単に検出できる値である。したがって、境界線抽出を行い、ターゲットマークの 3角形の各辺の長さを求め. それらの比等から画像上の 3角形の姿勢を特定するような複雑な処理を必要とする公知の方法に比べ、 3角形の姿勢の特定が非常に容易となる。

なお、以上の実施例では、図 5 1 ( A ) に示すように、汎用の画像処理装置を使用している。すなわち、カメラからの 1 フレーム分の画像スキャンデータを、バスを経由して与えられる各コマンドに従い、ストァ処理をしてフレームメモリ 3 0 1 に格納し、次にウインドウ設定処理してフレームメモリ 3 0 2に格納し、次に 2値化処理をしてフレームメモリ 3 0 3に格納し，最後に投影ヒストグラム処理をしてヒストグラムメモリ 3 0 4 に格納する。そして、このヒストグラムメモリ 3 '0 4に格納されたデータが C P Uに読み出され、位置ずれ検出が行われる。

ところで、こうした汎用の画像処理装置を使用した場合、ストァ、ウィンドウ設定、 2値化、投影ヒストグラムの各処理毎に 1 フレーム分のメモリ書き込みおよび読み出しをする必要があり、それらに要する各時間が、バスのアクセス時間を考えると 5 0 m s程度必要となる。そのため、汎用の画像処理装置では、 1 フレーム分の投影ヒストグラ厶データ-を得るには少なくとも 2 0 0 m s は必要であった。しかし、高速で移動するタ一ゲットマークの追跡に対しては、もっと高速な処理が求められる。そこで、高速な処理を可能にした画像処理装置の実施例を下記に示す。 '

図 5 1 ( B ) は投影ヒストグラムデータを得るための処理を高速化した画像処理装置の実施例の原理を説明する図である。すなわち、本実施例では、ストア、ウィンドウ設定、 2値化、投影ヒストグラムの各処理を、フレームメモリを使用することなく同時に行ない、そうして得られた投影ヒストグラムデータをヒストグラムメモリ 3 0 5に格納するようにする。

図 5 3は図 5 1 ( B ) に示した実施例の具体的なブロック図である。また、図 5 4は、図 5 3の各プロックにおける信号波形を示すタイミングチャートであり、これを以下の図 5 3の実施例の説明時に随時参照する。

クランプ部 3 0 8を経て映像信号〔図 5 4 ( A ) 〕がアナ口グのコンパレ一タ 3 1 1へ入力し、また、コンパレータ 3 1 1 へは、バスからの所定のスライスレベルのデータ力 D Aコンパ' ータ 3 1 0でアナ口グ値に変換されて入力される。コンパレ一タ 3 1 1 は、スライスレベルを越えた映像信号により 2値化パルス信号〔図 5 4 ( D ) 〕を出力する。一方、同期分離部 3 0 9では映像信号から垂直同期信号および水平同期信号〔図 5 4

( B ) ] を取り出し、ウィンドウ判断部 3 1 2へ出力する。このウィンドウ判断部において、 X方向のゥィンドゥ判断は水平同期信号の入力で、 1 2 M H zのドットクロック〔図 5 4

( C ) 〕のカウントを開始し、そのカウント値をバス経由で C P Uから設定したゥィンドウ位置を示すデータと照合することにより行う。また、 Y方向のウィンドウ判断は垂直同期信号の入力で、水平同期信号を力ゥントし、 Xの場合と同様にゥィンドウ位置と照合することにより行う。ウィンドウ判断部の出力

〔図 5 4 (E) 〕は、 X側のウィンドウ内信号と Y側のゥィンドウ信号の論理積をとつたものである。

AND回路 3 1 3は、 2値化パルス信号〔図 5 4 (D) 〕とウィンドウ内信号〔図 5 4 (E) 〕との論理積である AND出力〔図 5 4 (F) ] を出力する。この AND出力はウィンドウ内における 2値化信号を示し、ヒストグラム処理部 3 1 6は、この AND出力が「 1」の間だけドットクロック〔図 5 4 ( C ) 〕をカウントすることで、ヒストグラム値を得るようにする。得られた Xおよび Y方向のヒストグラムは、それぞれ X方向の記憶部（M) 3 1 7および Y方向の記憶部（M) 3 1 8に記憶される。 C P Uはバスを介してこれら記憶部 3 1 7， 3 1 8のヒストグラムデータを読み出す。また、 AND出力は表示部 3 1 4を介して画像モニタ 3 1 5へ出力される。

上記のゥィンドウ比較、ストア、 2値化、投影ヒストグラムの各処理は、水平スキャンの間に行われ、また、垂直帰線期間中にヒストグラム値が記憶部 3 1 7， 3 1 8に格納される。すなわち、ウィンドウ比較、ストア、 2値化、投影ヒストグラムの各処理、およびヒストグラム値の格納という一連の処理が、次フレームの画像スキヤンが開始されるまでの間に完了してしまう。

以上のように、汎用の画像処理装置の場合にはトラツキングアルゴリズ厶に不必要なフレー厶メモリへのアクセスを繰り返し、そのため、投影ヒストグラムを 1回求めるのに少なくとも 2 0 0 m sの処理時間を要するのに対し、この実施例の画像処理装置では、フレームメモリを必要とせず、したがって、フレ —ムメモリへのアクセス等が不要となり、 1 フレーム（ 3 3 m s ) の処理時間で済むようになる。すなわち、大幅な高速化を実現でき、高速で移動するタ一ゲットマ一クに対する追跡が可能となる。

また、このハードウェアではラベリング処理ができないので- 図 5 2のよ όにウィンドウ 3 0 7を画面内 3 0 6において高速に移動させ、そのゥインドウ内の投影ヒストグラムを調べることによりターゲットマークのサーチを行う。

以上の実施例では、タ一ゲットマークは 3角形の形状を有していることが必要であつたが、次に、 3角形の形状を持たないターゲットマ一クを認識する実施例について説明する。

図 5 5 は、この実施例の構成を示すブロック図である。この図 5 5において、たとえば口ボット作業に際して、口ボットハンドによって把持される作業対象物（以下、単に物体という） 1 は、三次元空間内で任意の位置と姿勢にある。この物体 1の位置、姿勢を計測する計測装置には、 1合のカメラ 2 0によつて撮像されたターゲットマーク Αの像が力メラインタフェース 3から入力される。本実施例の位置、姿勢の計測装置は、このターゲットマーク Aの画像データに基づいて物体の位置と姿勢を計測するものである。

ターゲットマーク Aは、物体 1の特定平面上にあって円とその中心点とを含んでいる。このタ一ゲットマーク Aはカメラ 2 0で撮像され、円とその中心点との画像が作成される。カメラインタフェース 3は画像メモリ 4、画像処理プロセッサ 5に画像バス 6によって接続されている。ターゲットマーク Aの画像は、例えば画像メモリ 4などに一旦記憶され、画像処理プロセッサ 5 によって物体 1の位置と姿勢の計測に必要な特徴点が抽出される。

C P U 7及び主メモリ 8は、物体 1 の位置及び姿勢を演算する演算手段であって、像空間から抽出された特徴点に関する射影幾何学演算を行うことによって、タ一ゲットマーク Aの物体空間内での位置及び姿勢を算出できる。この演算手段は、コント口一ルバス 9を介して画像処理プロセッサ 5等に接続されている。

図 5 6 は、ターゲットマ一ク Aによる位置、姿勢の計測の原理を示す図である。ターゲットマ一ク Aは半径 rの円とその中心点 Cから構成される。図 5 6 には、このターゲットマーク A から、カメラ 2 0の画像面である像平面 i に投影画像 A i ， C i が透視された状態を示す。ここでは視点 0がカメラ 2 0のレンズ中心であつて、本来なら像平面 i は Z軸方向で負の側に位置するが、物体空間と像空間とを明瞭に対応させるために、正の側に描いている。 f は、カメラ 2 0のレンズの焦点距離である。また、 X , Υ , Zはカメラ 2 0の基準座標であり、 X t ， Y t， Z t はターゲットマーク Aの中心点 Cを原点とする物体空間の基準座標である。

いま、ターゲットマーク Aのカメラ 2 0 に対する姿勢は、 Y t軸上の直線 mの周りに少し回転した姿勢であり、直線 mとターゲットマ一ク Aの円周との交点 S， Tを結ぶ直径方向べクトルが像平面 i に平行になつている。タ一ゲットマ一ク Aの円の像 A i と中心点 Cの像 C i とから、画像処理プ口セッサ 5 は所定の特徴点を抽出して、 C P U 7 によってターゲットマ一ク A の位置と姿勢を計測する。本実施例では、半径が既知である円とその中心点との投影像から、三次元空間上での円の位置と姿勢が一意的に決まることを利用している。

また、本実施例では三次元空間上の円を投影した場合に、物体空間の円の直径をなす直線のうち少なくとも 1つの直線は、像平面と平行になることを利用して、ターゲットマ一ク Aの像平面 i に平行な直径を特定する 2つの点 S， Tを求めて、像平面 i での像点間の距離と円の直径 2 rとの比から、中心点 Cの座標を決定するようにしている。

さらに、本実施例では三次元空間上の円の任意の直径の像から 2個以上の消失点を求めている。この消失点を中心点の像 C i と結ぶ線のいずれもが、それぞれ物体空間での実際の円の直径の方向に一致していることを利用して、タ一ゲットマ一ク A の円を含む平面に対する法線べクトル Z tが求められる。この場合に、ターゲットマ一ク Aの円の半径の値は分からなくても，ターゲットマーク Aを含む平面の方向が決定できる。

以下、以上に説明した原理を更に詳しく説明する。

図 5 ?は、カメラ 2 0の基準座標におけるターゲットマ一ク Aとその像との関係を示す図である。半径 rの円とその中心点とからなるターゲットマーク Aから、円の像 A i と中心点 Cの像点 C i とが得られたとき、像点 C i を通る 2本の直線を考える。この直線と円の像 A i との交点を求めれば、ターゲットマ —ク Aの位置と姿勢が求められる。この理由を、図 5 7に基づいて説明する。

図 5 7には、視点 0と円の像 A i とから決定される斜円錐を, 視点◦とターゲットマーク Aの直径の内の任意の 2点 U， Vとを含む平面で切った断面が示されている。ここで点 U、点 V、及び点 Cのべクトルは、それぞれ次の式（ 4 5 ) 〜（4 7 ) で表すことができる。

(〇U) = s (〇U i ) · · · (4 5 )

(0 V) = t (0 V i ) ♦ · · (4 6 )

(0 C) = u (0 C i ) · · ♦ ( 4 7 ) ここで（〇U) などは点〇を始点、点 U等を終点とするべクトルを示し、 s， t， uはスカラー量である。また、図 5 7において点 U, Vが円周上の点であって、 Cが円の中心点である力、ら、

( U C ) = ( C V ) ♦ · · ( 4 8 )

I U C I = r · · · (4 9 ) が成り立つ。これらの式から分かるように、像平面 i 内での点 U i、点 V i、及び点 C i の座標値が決まれば、それらに対応するスカラー量 s， t， uは決定されるから、点 U、点 V、及び点 Cの座標値が決まる。同様にして、像点 C i を通る別の直線に対応するターゲットマ一ク Aの円周上の点の座標値を決定すれば、ターゲットマ一ク Aの位置と姿勢とが一意的に決定でさる。

図 5 8は、像平面 i に投影されたタ一ゲットマ一ク Aの円と中心点との画像を示す図である。

カメラ 2 0が物体 1のターゲットマーク Aの面に正対している場合を除けば、常にターゲットマ一ク Aの円は像平面 i に楕円の画像 A iを投影する。タ一ゲットマーク Aの円とその中心点 Cを通る直線 mとの交点 S, Tの像点を S i， T i とすれば- 中心点 Cの像点 C i によって 2等分される線分は 1つだけ存在し、か όそれは容易に画像処理プロセッサ 5によつて像点 S i , T i として抽出される。また、像点 C i の座標を（X。， y 0 ) とすれば、中心点 Cの座標値は、円の直径 2 rと像平面 i での像点 S i , T i 間の距離との比 pによって、次のように決定される。

C (p x o , p y ₀ , p f ) · * · ( 5 0 ) こうして演算パラメータを著しく少なくすることができるから、簡単な計算だけで、精度良くターゲットマーク Aの位置を計測できる。

ターゲットマーク Aの姿勢を計測する別の方法について、次に説明する。

図 5 9 は、 1直線上の 4点が像平面 i に投影されたときの消失点を決定する方法を示す図である。 1直線上の 4点 a， b, c , dについて、複比 Rを R ( a， b， c , d ) と表せば、複比は

R ( a, b， c , d ) = ( a c b c ) / ( a dXb d)

- - * ( 5 1 ) と定義され、像平面 i に投影された 4点 a i， b i , c i , d i についての複比 R ( a i , b i ， c i , d i ) と等しくなる _c 複比は、一般に投影によっても保存されるからである。いま、距離の分かっている一直線上の 3点とその像点が与えられているとすれば、それらの複比 Rが互いに等しいという関係を利用して、像平面 i上での消失点を次のようにして決定することができる。

( a c / b c ) / ( a d b d ) = ( a i c i /b i c i ) ( a i d i b i d i ) · * . ( 5 2 ) ここで、点 dを一直線上で無限大にもっていくと、 d i = ( a i 氺 b i c i 氺 a c — b i * a i c i 氺 b e ) / ( b i c i 氺 a c — a i c i * b c ) ■ · ♦ · ( 5 3 ) この d i が像平面 i 上での消失点の座標値である。ただし、 a , bなどはその点の座標値を表し、 a b， a i c i などは 2 点間の距離を表している。

ここで、点 a , b， cの代わりにターゲットマーク Aの点 U , C, Vを与えると、それらの間の距離はいずれも半径値 r として既知の値であるから、任意の直径を構成する円周上の 2点と円の中心点 Cとの像空間内での座標値から消失点を一義的に決定できる。そして、視点〇からこの消失点に至る線分の方向は、ターゲットマ一ク Aの円周上の点 U， Vを結ぶ線分の方向に一致する。もっとも、上の式で距離 a c， b e に対応する値はいずれも r となるから、分子分母で打ち消される結果、ターゲットマーク Aの円の半径値は既知でなくても良い。

こうして、像空間内でターゲットマーク Aの円の直径を構成する円周上の任意の 2点を結ぶ直線の投影画像についての消失点が 2個以上求められると、ターゲットマ一ク Aの円を含む平面内での 2本の直径方向べクトルから、ターゲットマーク Aの法線べクトル Z t (図 5 6参照）を求めることにより、物体 1 の姿勢が決定できる。

つぎに、ターゲットマーク Aの法線べクトル Z tが分かつている場合に、精度良くタ一ゲットマ一ク Aの位置を計測するやり方について説明する。

図 6 0 はタ一ゲットマ一ク Aの画像 A i からターゲットマ一ク Aの姿勢を求める方法を示す図である。像平面 i で中心点 C の像点 C i を通り、線分 S i T i とは異なる 2本以上の直線を想定し、円の投影画像 A i との交点からそれぞれ 2個以上の消失点を求める。これら消失点からは、上述した複比の性質に基づいて特定の消失線が決定される。この消失線は、図 6 0のべクトル Dのような、物体空間のターゲットマーク Aの像平面 ί に平行な直径となる直線 mと平行なべクトルを決定する。これによって、ターゲットマ一ク Aの法線べクトルが決定される。また、図 6 0 において消失線に平行なべクトル Dから線分 S i T i が求まると、像点 C i の座標から、中心点 Cの座標値は，円の直径 2 r と像平面 i での像点 S i， T i の距離との比 p によって、 C ( p X。， p y。， p ΐ ) と決定される。

さらに、ターゲットマ一ク Αの像平面 i で線分 S i T ϊ を抽出する場合、物体空間の軸 Z tの方向、すなわちターゲットマーク Aの法線べクトルが分かっていれば、像平面 ί の法線べクトルとの外積によっても、べクトル Dと同様にターゲットマ一ク Αの像平面 i に平行な直径方向べクトルが求められる。

図 6 1 は、ターゲットマーク Aの一例を示す図である。このターゲットマーク Aは、直交する 2本の直径により 4つの領域に区分された円からなり、その中心点を特定するために、交互に白と黒に塗り分けられている。中心点を特定するために、一定の面積をもつた点で表示すれば、その重心を中心点として柚出する必要が生じる。しかし、重心が必ずしも中心点と一致しないために、従来では特徴点の抽出の段階で一定の誤差が発生していた。

本実施例のターゲットマーク Aのように、 2本の境界線の交点によって中心点を特定することにより、中心点を感度良く抽出できるから、ノィズの影響を受けずに、画像処理プロセッサ 5において特徴点の抽出が安定して行われる。なお、中心点を単に 2本の直径の交点などで特定するだけであっても、中心点の抽出精度の向上が期待できる。

以上説明したように、本実施例ではタ一ゲットマークに物体の姿勢に依存しない円とその中心点とを含むものを使用しているので、安定した精度で位置と姿勢の計測ができる。また、位置と姿勢の計測にあたって、使用するパラメータが少なくてすむために、簡単な計算で正確な計測が可能になる。

つぎに、やはり 3角形のタ一ゲットマ一クを使用しない他の実施例を説明する。

図 6 2 は、対象物に設けられるターゲットマークの第 1の例を示す平面図である。すなわち、ターゲットマークは、平面状の白地 2 3 0 に黒色（ハツチング部）の 4つの円板状のマ一ク 2 3 1 ~ 2 3 4が、互いに各所定の距離だけ離れて配置されたものである。マーク 2 3 1 〜 2 3 4の半径は少なくとも 1つが他と異なっている。こうしたターゲットマークを対象物に設置する。なお、必ずしも、タ一ゲットマークの白地 2 3 0が平面である必要はないが、平面でないときは、 4つの円板状のマーク 2 3 1 〜 2 3 4の各重心位置が平面上に位置するようにする必要がある。

こうしたターゲットマークを、図示はしないが、マ二ュピレ —タの先端に取り付けられた小型 C C Dカメラで撮影し、その画像データを距離姿勢演算装置により処理する。

図 6 3は、この距離姿勢演算装置により行われる第 1の距離姿勢演算の手順を示すフ口一チャートである。このフローチヤ一卜に沿って第 1 の距離姿勢演算の手順を説明する。図中、 S に続く数字はステップ番号を表す。

[ S 7 1 ] カメラからマーク 2 3 1〜 2 3 4に関する画像データを入力する。

CS 7 2 ] 入力した画像データを白黒反転させた上で 2値化する。

〔 S 7 3〕 2値化されたデータを基に、ラベリングを行う。すなわち、図 6 4 ( A) に示すように、例えば、マーク 2 3 1 にラベル 4を、マーク 2 3 2にラベル 6を、マーク 2 3 3にラベル 7を、マーク 2 3 4にラベル 3を付す。そして、図 6 4

(B ) に示すように、同一ラベル蕃号を付された領域における画素（ピクセル）数をカウントする。

C S 7 4 ] マーク選択範囲の中から、これらのカウントされた画素数が一審大きいラベル審号を有するマークを選択する。マーク選択範囲に、最初は、 4つのマーク 2 3 1〜 2 3 4全てが含まれ、したがって、図 6 4 (B) の画素数 8 5 0 0のラベル番号 3を付されたマーク 2 3 4が選択される。なお、このステップで選択されたマ一クは、次回以降の本ステップ実行のときにはマーク選択範囲の中から除かれる。そのため、次回の本ステツプ実行のときには、画素数 5 1 2 0のマーク 2 3 2が選択され、同様に、その次の 3回目の本ステップ実行のときには- 画素数 5 1 1 4のマーク 2 3 3が選択され、 4回目の本ステツプ実行のときには、残ったマーク 2 3 1が選択される。

〔 S 7 5〕図 6 5 に示すように、ステップ S 7 4で選択されたマーク、例えばマーク 2 3 4に対して、 X方向および Y方向の投影ヒストグラム処理を行い、 X方向の投影ヒストグラム 2 3 5および Y方向の投影ヒストグラム 2 3 6を得る。〔 S 7 6〕これらの X方向の投影ヒストグラム 2 3 5および Y方向の投影ヒストグラム 2 3 6の荷重平均 X 0， Y oを求め. これらを、マーク 2 3 4の X方向の重心位置ァドレス、および Y方向の重心位置ァドレスとする。

C S 7 7 ] 4つのマーク 2 3 1〜 2 3 4全てに対し、上記ステツプ S 7 5， S 7 6を実行し終えたならば、ステップ S 7 8 へ進み、未完了であればステップ S 7 4へ戻る。

〔 S 7 8〕算出された 4つのマーク 2 3 1〜 2 3 4の各重心位置を基に、各重心の配置を特定し、 4点透視問題を解き、対象物までの距離および対象物の姿勢を演算する。この 4点透視問題の解き方については、最後に詳しく説明する。なお、 4つの円の半径のうち、少なくとも 1つが他と異なれば、 4つの円の重心の配置は一意に決定できる。

以上のように、対象物に設けた 4つの円板状マーク 2 3 1 ~ 2 3 4を基に、それらの重心位置を求めるようにしたので、 4 つのマーク位置が正確に特定でき、対象物までの距離や対象物の姿勢を、精度よく、かつ簡単に計測することが可能となる。図 6 6 は、対象物に設けられるターゲットマークの第 2の例を示し、（A) はその平面図、（B) は（A) に示される矢印 C, D方向からみたターゲットマ一クの断面図である。すなわち、ターゲットマークは、箱状体 2 4 0から成り、白地の上面板 2 4 6 にマークとなる 2つの円形孔 2 4 1， 2 4 2 (以下、マーク 2 4 1 , 2 4 2 という）が設けられ、底板 2 4 5にはマ —クとなる白く着色された 2つの円板 2 4 3， 2 4 4 (以下、マーク 2 4 3 , 2 4 4 という）が設けられる。マーク 2 4 1， 2 4 2 は互いに半径が異なり、また、マーク 2 4 3， 2 4 4は- マーク 2 4 1 , 2 4 2よりもそれぞれ小さい半径を有するとともに、マーク 2 4 1， 2 4 2の各真下に配置される。箱状体 2 4 0内部は黒色に着色されている。こうしたターゲットマークを対象物に設置する。

こうしたタ一ゲットマ一クを、図示しないが、マニュピレ一タの先端に取り付けられた小型 C C Dカメラで撮影し、その画像データを距離姿勢渲算装置により処理する。

図 6 7 は、この距離姿勢演算装置により行われる第 2の距離姿勢演算の手順を示すフローチャートである。このフローチヤートに沿って第 2の距離姿勢演算の手順を説明する。図中、 S に続く数字はステップ蕃号を表す。

〔S 8 1〕カメラからマーク 2 4 1 ~ 2 4 4に関する画像デ一夕を入力する。

〔 S 8 2〕入力した画像データを白黒反転させた上で 2値化する。 ―

[ S 8 3 ] 2値化されたデータを基にラベリングを行う。そして、同一ラベル番号を付された領域における画素数を力ゥントする。

C S 8 4 ] 大円であるマーク 2 4 1， 2 4 2のうちから、これらの力ゥントされた画素数が大きいラベル審号を有するマークを選択する。なお、このステップの次回の実行のときには、大円であるマーク 2 4 1， 2 4 2のうちから、カウントされた画素数が小さいラベル蕃号を有するマークを選択する。

〔 S 8 5〕図 6 8 ( A) に示すように、ステップ S 8 4で選択されたマーク、例えばマーク 2 4 2、およびこのマーク 2 4 内に位置するマーク 2 4 4に対して、 X方向および Y方向の投影ヒストグラム処理を行い、 X方向の投影ヒストグラム 2 4 7および Y方向の投影ヒストグラム 2 4 8を得る。

[ S 8 6 ] これらの X方向の投影ヒストグラム 2 4 7および Y方向の投影ヒストグラム 2 4 8の分布荷重を求め、これらをそれぞれ A , A yとする。

〔 S 8 7〕 X方向の投影ヒストグラム 2 4 7および Y方向の投影ヒストグラム 2 4 8における各 2つのピークを抽出し、それらの X， Υ座標値を求める。

〔 S 8 8〕ステップ S 8 7で求められたピークの X， Y座標値で囲まれる範囲よりも少し外側の範囲に、図 6 8 ( A) のマーク 2 4 4付近を拡大した図 6 8 (B) に示すように、マスク 2 4 9を設定する。

〔 S 8 9〕図 6 8 ( B ) に示すように、ステップ S 8 8で設定されたマスク 2 4 9内のデータを白黒反転させた上で、 X方向および Y方向の投影ヒストグラム処理を行い、 X方向の投影ヒストグラム 2 5 0および Y方向の投影ヒストグラム 2 5 1 を得る。そして、これらの X方向の投影ヒストグラム 2 5 0および Y方向の投影ヒストグラム 2 5 1 の分布荷重を求め、これらを、それぞれ B X , B y とする。

〔 S 9 0〕ステップ S 8 6で算出した分布荷重 A X， A y、およびステップ S 8 9で算出した分布荷重 B x , B yをそれぞれ合算して、 A x + A y， B x + B yを求め（これらは、大円であるマーク 2 4 2だけの分布荷重となっている）、これらを基にマーク 2 4 2の X方向および Y方向の荷重平均をそれぞれ求めて、マーク 2 4 2だけの X方向の重心位置ァドレス、および Y方向の重心位置ァドレスとする。〔 S 9 1〕ステップ S 8 9で算出した分布荷重 B , B y (これらは、小円であるマーク 2 4 4だけの分布荷重となっている）を基にマーク 2 4 4の X方向および Y方向の荷重平均をそれぞれ求めて、マーク 2 4 4の X方向の重心位置ァドレス、および Y方向の重心位置アドレスとする。なお、これらのマーク 2 4 4の X方向の重心位置ァドレスおよび Y方向の重心位置アドレスは、マーク 2 4 4をカメラ方向へ投影してマーク 2 4 2の平面で得られる投影図形における重心位置のァドレスとなつている。

[ S 9 2 ] 2つのマーク 2 4 1， 2 4 2に対し、上記ステツプ S 8 5〜 S 9 1を実行し終えたならば、ステップ S 9 3へ進み、未完了であればステップ S 8 4へ戻る。

C S 9 3〕算出された 4つのマーク 2 4 1〜 2 4 4の各重心位置を基に、 4点透視問題を解き、対象物までの距離および対象物の姿勢を演算する。この 4点透視問題の解き方については- 最後に詳しく説明する。

以上のように、 4つのマーク 2 4 1〜 2 4 4を、対象物に設けるようにしたので、カメラが円扳状マークの設けられた对象物に正対した場合でも分解能を低下させることなく、対象物までの距離やその姿勢を、精度よく計測できる。

図 6 9 は、対象物に設けられるターゲットマークの第 3の例を示す平面図である。図中、タ一ゲットマークのマーク 2 6 1 〜 2 6 4は、図 6 6に示す第 2実施例のターゲットマークのマーク 2 4 1〜 2 4 4と同一な構成であり、マーク 2 6 1， 2 6 2 , 2 6 5 , 2 6 6は図 6 2に示す第 1実施例のターゲットマークのマーク 2 3 1 ~ 2 3 4 と同一な構成である。こうしたタ —ゲットマークを対象物に設置する。

こうしたタ一ゲットマークを、図示しないが、マニュピレ一タの先端に取り付けられた小型 C C Dカメラで撮影し、その画像データを距離姿勢演算装置により処理する。

図 7 0は、この距離姿勢演算装置により行われる第 3の距離姿勢演算の手順を示すフローチヤ一トである。このフローチヤ一卜に沿って第 3の距離姿勢演算の手順を説明する。図中、 S に続く数字はステップ番号を表す。

[ S 1 0 1 ] カメラからマーク 2 6 1 〜 2 6 6 に関する画像データを入力する。

〔 S 1 0 2〕入力した画像データを白黒反転させた上で 2値化する。

〔 S 1 0 3〕 2値化されたデータを基にラベリングを行う。そして、同一ラベル番号を付された領域における画素数を力ゥントする。

〔 S 1 0 4〕これらのカウントされた画素数が 1審目および 2番目に大きいラベル番号を有するマーク 2つ、即ちマーク 2 6 1 , 2 6 2を選択する。

〔 S 1 0 5〕ステップ S 1 0 4で選択されたマ一ク 2 6 1， 2 6 2、およびこれらのマーク 2 6 1 ， 2 6 2内に位置するマ —ク 2 6 3， 2 6 4に対して、立体マーク検出法、すなわち、図 6 7のステップ S 8 4〜ステップ S 9 2の処理と同一の処理を実行する。

〔 S 1 0 6〕ステップ S 1 0 5の実行により、 4つの重心が検出できたならば、ステップ S 1 0 9へ進み、一方、カメラの視線が円'板状マークに対して大きく傾き、マーク 2 6 3 , 2 6 4がマーク 2 6 1， 2 6 2に隠れて見えなくなり、検出できないならば、ステップ S 1 0 7へ進む。

〔 S 1 0 7〕平面 2 6 0上に配置されたマーク 2 6 1、 2 6 2 , 2 6 5 , 2 6 6を選択する。

〔S 1 0 8〕ステップ S 1 0 7で選択されたマーク 2 6 1、 2 6 2 , 2 6 5 , 2 6 6に対して、図 6 3のステップ S 7 4〜ステップ S 7 7の処理と同一の処理を実行する。

〔 S 1 0 9〕ステップ S 1 0 5で算出された 4つのマーク 2 6 1〜 2 6 4の各重心位置を基に、またはステップ S 1 0 8で算出された 4つのマーク 2 6 1、 2 6 2， 2 6 5 , 2 6 6の各重心位置を基に、 4点透視問題を解き、対象物までの距離および对象物の姿勢を演算する。この 4点透視問題の解き方については、以下に詳しく説明する。

以上のように、通常は、分解能のよいステップ S 1 0 5の立体マーク検出法を適用するが、カメラの視線が円扳状マークに ¾して大きく傾くと、マーク 2 6 3， 2 6 4がマーク 2 6 1 , 2 6 2に隠れて見えなくなり、 2つの重心位置が検出できなくなるので、その場合には、ステップ S 1 0 8の検出法を適用する o

つぎに、 4点透視問題（アルゴリズム）の解き方について説明する。

図 7 1 は、対象物に固定された座標系と力メラに固定された座標系とを示す図である。まず、基準となる座標系をカメラの画像面 2 7 1上に設定する。このとき、原点 0は、光軸 2 7 2 と画像面 2 7 1 とが交わる位置にとり、視線方向（光軸 2 7 2 方向）を Z軸、視線に向かって左を X軸、上方を Y軸とする右手系座標を設定する。また、焦点 2 7 3は、 Z軸の負の方向に距離 f だけ離れた点に存在するとして、モデル化を行う。実際の透視変換マトリクスは、実際のレンズ系を考慮する必要があるが、ここでは、シンプルなモデル化を行なった。

対象物の座標系としては、計測に利用する 4つの特徴点（前述の重心位置）のうちの一点を、対象物座標原点 0 ^R とする。 4点が存在する平面 2 7 4の法線方向を Z ^R 軸とし、 X ^R ， Y ^B 軸に関しては、座標系が右手系を構成するようにとる。このように構成した座標系において、原点〇 ^R から他の 3つの特徴点への 3本のべクトルをそれぞれ、 A ( a l， a 2 , 0 ) , Β ( b 1 , b 2 , 0 ) , C ( c 1 , c 2 , 0 ) とする（各点は X ^R Y ^R 平面内に存在する）。

また、各特徴点 0 ^R , A， B， Cの画像面 2 7 1上への投影点は、〇 1 ， "， β , rであり、原点〇から各投影点までのベタトルを、 0 1 ( o l， o 2， 0 ) ， {a I , a 2 , 0 ) ，（ 1， 2 , 0 ) ，（ 7 1， 2 , 0 ) とする。以上により、各特徴点〇 ^R ， A, B， Cと、画面 2 7 1上の投影された投影点 0 1 ，ひ， β， r との、各座標系に対する表現が可能になる。

つぎに、各特徴点〇 ^R ， A, B, と、画面 2 7 1上の投影された特徴点（投影点） 0 1，《， β， r との間の座標変換について、説明する。

まず、対象物座標原点〇 ^R からのべクトル A, B， Cと、画像面上での特徴点べクトル， β , r とを結びつける変換行列を求める。以後、べクトルは、第 4成分を 1 に規格化した同次座標表示によって表現する。図 Ί 1 に示すように、座標系 0 ^R - X ^R Y^R Z ^R は、座標系〇— X Y Zを: X軸方向に s、 Y軸方向に t、 Z軸方向に uだけ平行移動し、さらにそれを回転させたものと理解できる。よつて、ベクトル A, B, Cとベクトル"， β , とは、平行移動および回転を表す変換行列丁と、透視変換 Pz Pを表す変換行列との積による行列で結びつく。

すなわち、次式（ 5 4 ) が成り立つ。

I = P z P T U · I ^R · · · ( 5 4 ) ここに、 Uは、単位の変換行列であり、また、 I ^R は座標系 0 ^R — X^R Y^R Z ^R で表示されたべクトルであり、 I はそれを X Y面に透視変換したベクトルを座標系〇一 X Y Zで表示したものである。 P z ， P, T, Uは、それぞれ次のような行列でめ.な。 n I , n I 2 Π I 3 s

T = n 2 1 n 22 Π 23 t ( 5 5 ) n 3 1 n 32 Tl 33 u

0 0 0 1

一 1 0 0 0

P = 0 一 1 0 0 ( 5 6 )

0 0 一 1 0

0 0 1 1 0 0 0

z ⁼ 0 1 0 0 ( 5 7 )

0 0 0 0

0 0 0 1

P 0 0 0

u = 0 P 0 0 ( 5 8 )

0 0 P 0

0 0 0 1 ただし、 ρは、 _P = f ZF ( f ：画素数で表される焦点距離. F =メ一トル等の単位で表される焦点距離）で表され、単位長さ当たりの画素数である。

こで、 Tは、座標系〇 ^R - X ^R Y^R Z ^R および座標系 0—

X Y Zの間の平行移動，回転を表し、 Pは焦点距離 f の透視変換、 P z は X Y面への投影変換を表す。なお、座標系〇^R - X ^R Y ^R Z ^R の Χ ^β 軸、 Y.^R 軸、 Z ^R 軸の各単位べクトルを座標系 0 - X Y Zで成分表示すると次のようになる。

η X R = ( n i l, n 2 1 , n a I ) ^T · · · ( 5 9 ) η Y ( n , ₂, n 22, n 3₂) ^r · · · ( 6 0 ) η Z R ( n , 3, n 23, n ₃₃) ^T * * · ( 6 1 ) こで、式（ 5 5 ) 〜（ 5 8 ) を基に Pz P TUを計算すると、結局、次の変換式（ 6 2 ) が得られる。一 pn i i pn I 2 pn i 3 一 s

-pn₂ l -pn₂2 - -pnn₂₂33 -- 11 ( 6 2 0 0 0 0

ただし、 r = 1 Z f とする。

ところで、変換式（ 6 2 ) において、ベタトル I ^R にべクトル A, B, Cを、ベクトル I にベクトル"， β , τを代入するすなわち、

( 6 3， 6 4 6 5 )

( 6 6 , 6 7， 6 8 )

これらのうち、式（ 6 2 ) ( 6 3 ) , ( 6 6 ) より、次式 ( 6 9 ) が得られる。 a 1 al

o l P z P T U al

0 0

1 1 l

2

( 6 9 ) まとめると、次式（ 7 0 ) となる。

… · ( 7 0 ) 式（ 7 0 ) は同時座標表示であるため、次式（ 7 1 ) のよう変形できる。 * · - (7 1 )

で、対象物座標原点 0^R の投影点 01は、つぎのように表すことができる。

0 1 0

o 2 = P, P TU 0

0 0

1 1

1 )

1 ) (7 2)

o 1 = s X ( r u - 1 ) ♦ · · ( 7 3 ) o 2 = t ( r u - 1 ) · · ♦ ( 7 ) 式（ 7 1 ) の両辺を比較し、式（ 7 3 ) ( 7 4 ) を代入し、整理すると、次式（ 7 5 ) ( 7 6 ) となる。

( 1 - 01 ) ( u - f ) Z p = ( f a 1 n ! , + f a 2 n ,₂

- a l a l n ₃.+ a l a 2 n ₃₂) · · · ( 7 5 )

( 2 - o 2 ) ( u - f ) / p = ( f a 1 n _{2 I} + f a 2 n ₂₂

- al a l n ₃ i+ «2 a 2 n ₃₂) · · ♦ ( 7 6 )

ここで、式（ 6 2 ) , ( 6 4 ) , ( 6 7 ) 、および式（ 6 2 ) ( 6 5 ) , ( 6 8 ) についても同様に変形整理すると、以下のようになる。

( 一 o l ) ( u - f ) p = ( f b l n , ,+ f b 2 n _{I 2} - β \ b l n _{3 I}+ 91 b 2 n ₃₂) · · · ( 7 7 )

( β 2 - o 2 ) ( u - f ) / p = ( f b l n ₂₁+ f b 2 n ₂₂ - β 2 b l n _{3 l} + β 2 b 2 n ₃₂) · ♦ · ( 7 8 )

( r 1 - o 1 ) ( u - f ) / p = ( f c 1 n , ,+ f c 2 n ₁₂ - r l c l n ₃₁+ r l c 2 n ₃₂) · · · ( 7 9 )

( T 2 - o 2 ) ( u - f ) Z p = ( f c 1 n ₂₁+ f c 2 n ₂₂ - r 2 c l n _{3 I}+ r 2 c 2 n ₃₂) · · · ( 8 0 ) 以上の式を整理すると、つぎのような線型方程式（ 8 1 ) が得られる。 f al f a2 0 0 - al al 一ひ 1 a2 n I I al - ol

0 0 f al f a2 一 a2 al -a2 a2 n 12 2 - o2 f bl f b2 0 0 一 SI bl 一/ SI b2 n 2i 91 - ol (u-O/p

0 0 f bl f b2 -02 bl - 2 b2 n 22 β2 - o2 f cl f c2 0 0 -rl cl - rl c2 n 3i rl - ol

0 0 f cl f c2 - 72 cl - 72 c2 n 32 r2 - o2

♦ · ♦ ( 8 1 ) ここで、式（ 8 1 ) 中の 6 x 6のマトリクスを計測マトリクス Mとすると、式（ 8 1 ) から、つぎのようになる。

ここで、 n = (n i l, n i, n 1 ) ^T ， n = ( n ₂, n ₂₂， n ₃₂) ^T は、式（ 8 2 ) から求めることができる。また、 s , t , uは、以下のようにして求める。

まず、 n _XRのノルムは、単位べクトルであるので、

(η χ,) ² + (n _2I) ² + (η ₃ι) ² = 1 · ♦ ♦ ( 8 3 ) となり、式（ 8 2 ) から Qが導かれ、以下となる。

(n I I + n 2 I + n 3 1 ) ( P X (u - f ) ) ² =a

( 8 4 ) よって、

P X (u - f ) = Q'^/2 ( 8 5 ) となり、式（ 7 3 ) , ( 7 4 ) , ( 8 5 ) から、

u = p /Q ^1/2 + f · · ( 8 6 ) s = p o 1 / f Q ^1/2 · · ( 8 7 ) t - p o 2 f Q ^1/2 · · ( 8 8 ) となる。

以上により、式（ 8 2 ) ， ( 8 6 ) , ( 8 7 ) ( 8 8 ) 力、ら、座標系〇 ^R - X ^R Y^E Z ^R と座標系〇一 X Y Zとの間の変換式が、すべて決定され、次式（ 8 9 ) で表すことができる。

I -T I ^R ♦ · · ( 8 9 ) ここで、 Tを変換マトリクスと呼び、 Tは次式（ 9 0 ) で与

X»りれる。

ただ'し、 ( Π ΐ 3 η 2 3, η ₃s) ^τ = n xR X ri YRとする。

なお、 X^R は、画素数に変換されているものとする。

以上の定式化を行い、特徴点 0^R A, B, Cの空間配置が決定され、導出した変換マトリクス Tにより、対象物までの距離および対象物の姿勢を表現できる。

なお、このようなマトリクスの表現形式では、特に、操作者に対象物の姿勢を直観的に示すことができないので、ロボットの姿勢表現で一般的と考えられている r o 1 1 一 p i t c h - y a w表現形式に、または、宇宙関係でしばしば使用される 4 元数表現形式に変換するようにする。

上説明したように本実施例では、対象物に設けた 4つの円板状マークを基に、それらの重心位置を求めるようにしたので. 4つのマーク位置が正確に特定でき、したがって、対象物までの距離やその姿勢を、精度よく、かつ簡単に計測することが可

B匕と , つ

また、円板状の 4つのマークからなる立体マークを対象物に設けるようにしたので、撮影手段が円板状マークの設けられた対象物に真っ正面から対向（正対）した場合でも分解能を低下させることなく、対象物までの距離やその姿勢を、精度よく計測できる。

さらに、通常は、立体マークを用いて、分解能を低下させることなく、対象物までの距離やその姿勢を、精度よく計測し、一方、撮影手段の視線が円板状マークに対して大きく傾いて、小円マークが大円マークに隠れて見えなくなったときには、平面状の 4つの円板状クを用いて、立体マークの欠点を補う

Claims

請求の範囲

1 . ビデオ力メラを用いてターゲットマークを追跡するタ一ゲットマーク追跡システムにおいて、

対象物体に設けられ、黒円の中央部に白三角形を立体的にシフトさせて構成したターゲットマークと、

前記ターゲットマークを撮像するビデオ力メラと、

前記ビデオカメラを搭載し 6 自由度方向に移動可能な移動機構と、

前記ビデオ力メラによる前記タ一ゲットマークの画像データを処理する画像処理手段と、

前記画像処理手段によつて得られた前記ターゲットマークの投影ヒストグラム情報から前記ターゲットマークのずれ量を検出するずれ量検出手段と、

前記ずれ量に応じて前記移動機構の移動動作を制御し、前記ビデオカメラに前記ターゲットマークの追跡を行わせる移動機構制御手段と、

を有することを特徵とするターゲットマーク追跡システム。

2 . 前記ずれ量検出手段は、前記ターゲットマークのずれ量をカメラ座標系において認識することを特徴とする請求項 1記載のターゲットマ一ク追跡システム。

3 . 前記画像処理手段は、画像座標系での前記ターゲットマ —クの画像データに対してマーク認識、ウィンドウ、ストア、 2値化、 X軸投影ヒストグラム及び Y軸投影ヒストグラムの各処理を行うことを特徴とする請求項 1記載のターゲットマーク追跡システム。

4 . 前記ずれ量検出手段は、前記 X軸投影ヒストグラム処理及び前記 Y軸投影ヒストグラム処理によって求められた前記ターゲットマークの重心位置と予め設定されている前記ターゲットマークの目標位置との差分から、前記ターゲットマークの X 軸方向及び Υ軸方向のずれ量を検出することを特徵とする請求項 3記載のターゲットマ一ク追跡システム。

5 . 前記ずれ量検出手段は、前記 X軸投影ヒストグラム処理または前記 Υ軸投影ヒストグラム処理によって求められた前記ターゲットマークの面積と予め設定されている前記ターゲットマークの目標面積との差分から、前記ターゲットマークの Ζ軸方向のずれ量を検出することを特徴とする請求項 3記載のターゲットマ一ク追跡システム。

6 . 前記ずれ量検出手段は、前記 X軸投影ヒストグラム処理によって求められたピーク点等から前記ターゲットマークの黒円に対する白三角形のずれ量を求め、前記ずれ量を前記ターゲットマークのョ一方向のずれ量とすることを特徴とする請求項 3記載のターゲットマーク追跡システム。

7 . 前記ずれ量検出手段は、前記 Υ軸投影ヒストグラム処理によつて求められたピーク点等から前記ターゲットマ一クの黒円に対する白三角形のずれ量を求め、前記ずれ量を前記ターゲットマークのピッチ方向のずれとすることを特徵とする請求項 3記載のタ一ゲットマ一ク追跡システム。

8 . 前記ずれ量検出手段は、前記 X軸投影ヒストグラム処理によって求められたピーク点及び口一ピーク点から前記ターゲットマークの口一ル方向のずれ量を求めることを特徴とする請求項 3記載のターゲットマーク追跡システム。

9 . 前記画像処理手段は、前記 X軸投影ヒストグラム処理及び前記 Y軸投影ヒストグラム処理によつて求められた前記タ一ゲットマークの重心位置及びサイズから、前記ゥインドウの位置及びサイズを決定することを特徵とする請求項 3記載のターゲットマーク追跡システム。

1 0 . ビデオ力メラを用いてタ一ゲットマークを撮像し前記ターゲットマークのずれ量を検出するずれ量検出方式において、対象物体に設けられ、黒円の中央部に白三角形を立体的にシフトさせて構成したターゲットマークと、

前記ターゲットマークを撮像するビデオ力メラと、

前記画像処理手段によつて得られた前記ターゲットマークの投影ヒストグラ人情報から前記ターゲットマークのずれ量を検出するずれ量検出手段と、

を有することを特徵とするずれ量検出方式。

1 1 . ターゲットマークをビデオカメラにより撮影し、その撮影データを処理することで、前記ビデオカメラを前記タ一ゲットマークに対し常時、所定闋係位置に保持するようにするターゲットマーク視覚追跡制御方式において、

前記ビデオカメラから出力された前記ターゲットマークの画像において前記ターゲットマークの目標位置 ·姿勢からの変化量を検出する画像変化量検出手段と、

前記ビデオ力メラと前記ターゲットマークとの所定関係位置からの相対的な実変化量を検出する実変化量検出手段と、

前記ビデオカメラと前記ターゲットマークとの所定関係位置を実験的にずらし、そのときに得られる、前記画像変化量検出手段の検出値と、前記実変化量検出手段の検出値とから、相互の関連づけをする同定手段と、

を有することを特徴とするターゲットマーク視覚追跡制御方式。

1 2 . 前記同定手段は、前記画像変化量検出手段の検出値のうちの、画像上での前記ターゲットマ一クの面積を表す検出値と、前記実変化量検出手段の検出値のうちの、前記ビデオカメラと前記ターゲットマークとの間の距離とを関連づけて、前記面積を表す検出値から、前記ビデオ力メラと前記ターゲットマークとの間の距離を導き出す第 1の導出手段を有することを特徴とする請求項 1 1記載のタ—ゲットマーク視覚追跡制御方式 _t

1 3 . 前記同定手段は、前記画像変化量検出手段の各種検出値と、前記実変化量検出手段の検出値のうちの前記ビデオ力メラと前記ターゲットマ一クとの間の距離および前記実変化量検出手段の各種検出値とを関連づけ、前記第 1の導出手段が導き出した前記距離および前記画像変化量検出手段の各種検出値から、前記ビデオカメラと前記ターゲットマ一クとの所定関係位置からの相対的な実変化量を導き出す第 2の導出手段を有することを特徴とする請求項 1 2記載のターゲットマーク視覚追跡制御方式。

1 4 . タ一ゲットマークをビデオカメラにより撮影し、その撮影データを処理することで、前記ビデオカメラを前記ターゲットマークに対し常時、所定関係位置に保持するようにするターゲットマ一ク視覚追跡制御方式において、

前記ビデオカメラから出力された前記ターゲットマークの画像において前記ターゲットマークの目標位置 · 姿勢からの変化量を検出する画像変化量検出手段と、

前記ビデオ力メラと前記ターゲットマークとの所定関係位置からの相対的な実変化量を検出する実変化量検出手段と、前記ビデオ力メラと前記ターゲットマークとの所定関係位置を実験的にずらし、そのときに得られる、前記画像変化量検出手段の検出値と、前記実変化量検出手段の検出値とから、相互の関連づけをし、前記画像変化量検出手段の検出値から、前記ビデオカメラと前記タ一ゲットマ一クとの所定関係位置からの相対的な実変化量を導き出す導出手段と、

前記導出手段が導き出した実変化量に比例ゲインをかけ、さらに回転成分に対し、前記ビデオカメラと前記ターゲットマ一クとの距離だけずらせた座標系での速度指令値を算出し、前記ビデオカメラの位置制御の速度指令値とする制御手段と、を有することを特徴とするターゲットマーク視覚追跡制御方式 o ―

1 5 . ターゲットマークをビデオ力メラにより撮影し、その撮影データを処理することで、前記ビデオカメラを前記ターゲットマークに対し常時、所定関係位置に保持するようにするターゲットマーク視覚追跡制御方式において、

前記ビデオ力メラから出力された前記ターゲットマークの画像において前記ターゲットマークの目標位置 ·姿勢からの変化量を検出する画像変化量検出手段と、

前記ビデオ力メラと前記ターゲットマークとの所定関係位置からの相対的な実変化量を検出する実変化量検出手段と、前記ビデオ力メラと前記ターゲットマークとの所定関係位置を実験的にずらし、そのときに得られる、前記画像変化量検出手段の検出値と、前記実変化量検出手段の検出値とから、相互の関連づけをし、前記画像変化量検出手段の検出値から、前記ビデオカメラと前記ターゲットマークとの所定関係位置からの相対的な実変化量を導き出す導出手段と、

前記導出手段が導き出した実変化量から前記ビデオカメラに固定された座標系における前記ビデオ力メラの姿勢のずれ量を求め、定数ゲインをかけて前記ビデオカメラの位置制御の速度指令値とする制御手段と、

1 6 . 少なくとも特定な形状の 3角形から成るターゲットマークのカメラ撮影画像に基づいて対象物体の力メラ視線軸回りの姿勢を検出するターゲットマークの姿勢検出方法において、前記ターゲットマークの 3角形の画像の X， Y方向の各投影ヒストグラムを求め、

前記求められた各投影ヒストグラムにおいて、前記タ一ゲットマークの 3角形の画像の X , Y方向の各重心位置を求め、前記求められた各投影ヒストグラムにおいて、ヒストグラム値が最大になるときの各最大ヒストグラム値、およびそのときの X， Y軸値を求め、

前記求められた各重心位置、並びに各最大ヒストグラム値および X， Y軸値、さらに、前記ターゲットマ一クの幾何的既知データに基づき、前記ターゲットマークの 3角形の姿勢が、予め分類され設定されている姿勢パターンの中のいずれのパターンに相当するかを判別し、

前記判別されたパターンにおけるターゲットマークの 3角形のカメラ視線軸回りの姿勢を算出することを特徵とするターゲットマークの姿勢検出方法。

1 7 . 前記算出されたタ一ゲットマークの 3角形の力メラ視線軸回りの姿勢を、前記タ一ゲットマークの 3角形のカメラ視線軸回りの基準姿勢と比較して、前記ターゲットマ一クの 3角形のカメラ視線軸回りの姿勢のロール回転角ずれ量を求めることを特徵とする請求項 1 6記載のターゲットマークの姿勢検出方法。

1 8 . 前記予め分類され設定されている姿勢パターンの中に複数の異なる姿勢がある場合には、前記ターゲットマークを撮影するカメラの姿勢を僅かロール回転させてから再び、姿勢検出を行うことを特徵とする請求項 1 6記載のターゲットマークの姿勢検出方法。

1 9 . 前記ターゲットマークは前記 3角形の他に、前記 3角形の背後に配置された円盤とからなり、前記 3角形の画像の X , Y方向の投影ヒストグラムを、前記円盤のみの画像の X， Y方向の算出投影ヒストグラムから、前記タ一ゲットマ一クの画像の X， Y方向の実測投影ヒストグラムをそれぞれ減算して求めることを特徵とする請求項 1 6記載のターゲットマークの姿勢検出方法。

2 0 . 前記円盤のみの画像の X , Y方向の算出投影ヒストグラムは、前記ターゲットマークの画像の X , Y方向の各実測投影ヒストグラムにおいて、前記ヒストグラム値が所定の値となる各 2つの座標値を求め、前記求められた各 2つの座標値の各中心が前記円盤の中心であるとして、既知の前記円盤の幾何的データに基づき算出されることを特徵とする請求項 1 9記載のタ一ゲットマークの姿勢検出方法。

2 1 . 前記 3角形と背後に配置された円盤とからなるターゲットマ一クの画像の X， Y方向の実測投影ヒストグラムの各重、位 irf 求め、

前記円盤の画像上での中心座標を求め、

前記円盤の面積と前記 3角形の面積との比を求め、

前記求められたターゲットマ一クの各重心位置、円盤の中心座標、および面積比に基づき、前記 3角形の重心位置を算出することを特徴とする請求項 1 9記載のターゲットマークの姿勢検出方法。

2 2 . ビデオカメラを用いてターゲットマ一クを追跡するタ —ゲットマ—ク追跡システムにおいて、ターゲットマ一クを撮像するビデオカメラと、

前記ビデオカメラを搭載し、 6 自由度方向に移動可能な移動機構と、

前記ビデオカメラによる前記ターゲットマークの画像データを処理するに際し、ウィンドウ比較、ストア、 2値化、投影ヒストグラムの各処理を 1 フレーム分の画像走査の間に行う画像処理手段と、

前記画像処理手段によって得られた前記ターゲットマークの投影ヒストグラム情報から前記ターゲットマークのずれ量を検出するずれ量検出手段と、

前記ずれ量に応じて前記移動機構の移動動作を制御し、前記ビデオ力メラに前記ターゲットマークの追跡を行わせる移動機構制御手段と、

を有することを特徴とするタ一ゲットマーク追跡システム。

2 3 . 原画像を 2値化する過程と、

該 2値化した画像のうちの画素が連結している画像をグループ化する過程と、

該グループ化された画像について X Y投影ヒストグラ厶を求める過程と、

該グループ化された画像の X Y投影ヒストグラムの極値の数を計数する過程と、

該計数した極値の数を、目的とするターゲットマークについて予め求めた X Y投影ヒストグラムの極値数と比較して、該グループ化された画像が該目的とするターゲットマ一クであるか否か判定する過程と

からなるタ一ゲットマークの検出方法。

2 4 . 極値数の特徵が異なる 2種類以上のマークを所定の位置関係で組み合わせてターゲットマークを構成し、このターゲットマーク中の各マークに対して、前記各過程を実行して各マークを検出してそれらの座標点を計測し、さらに、それらの座標点に対して n点透視問題を解くことで、ターゲットマークの位置 · 姿勢を計測する請求項 2 3記載のターゲットマ一クの検出方法。

2 5 . ターゲットマークの像に基づいて物体の位置と姿勢を計測する位置、姿勢の計測装置において、

前記物体の特定平面上にあって円とその中心点を舍むターゲットマークと、

前記タ一ゲットマークを撮像して前記円とその中心点の画像を作成する力メラと、

前記ターゲットマークの画像から前記物体の位置と姿勢の計測に必要な特徴点を抽出する特徴抽出手段と、

前記特徴点に関する射影幾何学演算によって前記ターゲットマークの物体空間内での位置及び姿勢を演算する演算手段と、を有することを特徵とする位置、姿勢の計測装置。

2 6 . 前記特徴抽出手段は、前記ターゲットマークの円の画像の中から、物体空間の円の直径のうちターゲットマークの像平面に平行な直径を特定する 2つの像点を抽出するものであり . 前記演算手段では、抽出された 2つの像点間の距離、前記ターゲットマークの円の直径の値、及び前記カメラにより撮像された前記ターゲットマ一クの中心点の像空間内での座標値に基づいて、前記物体空間内での前記ターゲットマークの位置を濱算することを特徴とする請求項 2 5記載の位置、姿勢の計測装

2 7 . 前記演算手段は、前記ターゲットマークの円の直径を構成する円周上の任意の 2点とその中心点の像空間内での座標値から、前記ターゲットマークの物体空間内での直径方向べクトルを濱算するものであって、

前記ターゲットマークについての少なくとも 2以上の直径方向べクトルから前記物体空間内での前記ターゲットマ一クの法線べクトルを求めることにより、

前記ターゲットマ一クが貼着された物体の姿勢を決定することを特徴とする請求項 2 5記載の位置、姿勢の計測装置。

2 8 . 前記ターゲットマ一クは、交差する 2本の線分によつて前記円の中心点が表示されていることを特徴とする請求項 2 5記載の位置、姿勢の計測装置。

2 9 . 前記ターゲットマークの円は、 2本の直径によって区分された領域に塗り分けられていることを特徴とする請求項 2 5記載の位置、姿勢の計測装置。

3 0 . 前記特徵抽出手段は、前記ターゲットマークの円の直径のうち、前記ターゲットマークの像平面内で前記ターゲッ卜マークの中心点の像によつて 2等分される直径を特定することによって、前記 2つの像点を抽出することを特徵とする請求項 2 6記載の位置、姿勢の計測装置。

3 1 . 前記演算手段では、さらに前記ターゲットマークの法線べクトルと像平面の法線べクトルとの外積によって、前記タ —ゲットマークの像平面に平行な直径方向べクトルを求めることによって、

前記直径方向べクトルにより特定される前記ターゲットマ一クの円の直径を構成する円周上の 2点の像空間内での 2つの像点間の距離、前記ターゲットマークの円の直径の値、及び前記カメラにより撮像された前記ターゲットマークの中心点の像空間内での座標値に基づいて、前記物体空間内での前記ターゲットマークの位置を演算することを特徴とする請求項 2 7記載の位置、姿勢の計測装置。

3 2 . 対象物までの距離および対象物の姿勢を計測する距離姿勢計測装置において、

各重心が同一平面上に位置するように対象物に配置され、少なくとも 1つの円の半径が他と異なる 4つの円板状マークと、前記 4つの円板状マークを撮影する撮影手段と、

前記撮影手段から出力される 4つの円板状マークの画像データに基づき、各円板状マークの重心位置を算出する重心算出手段と、前記重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、前記対象物までの距離および前記対象物の姿勢を演算する潢算手段と、

を有することを特徴とする距離姿勢計測装置。

3 3 . 前記重心算出手段は、前記撮影手段から出力される 4 つの円板状マークの画像データに基づき、各円板状マークに対し、 2値化、垂直水平投影ヒストグラム作成を行い、そして荷重平均計算による円板状マークの重心位置を算出するように構成したことを特徴とする請求項 3 2記載の距離姿勢計測装置。

3 4 . 対象物までの距離および対象物の姿勢を計測する距離姿勢計測装置において、

互いに半径が異なる第 1および第 2の円板状マークと、前記第 1および第 2の円板状マークが設けられた第 1の平面と異なる第 2の平面上に配置された第 3および第 4の円板状マ —クと、

前記第 1乃至第 4の円板状マークを撮影する撮影手段と、前記撮影手段から出力される第 1乃至第 4の円板状マークの画像データに基づき、前記第 1 および第 2の円板状マークの重心位置を算出するとともに、前記第 3および第 4の円板状マークを前記撮影手段方向へ投影して前記第 1の平面で得られる各投影図形における重心位置を算出する重心算出手段と、

前記重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、前記対象物までの距離および前記対象物の姿勢を演算する演算手段と、

を有することを特徴とする距離姿勢計測装置。

3 5 . 前記第 3および第 4の円板状マ一クは、前記第 1および第 2の円板状マークの各重心を通る平面に垂直で、かつ、前記第 1および第 2の円板状マークの各重心を通る線上にそれぞれ位置するように構成されたことを特徵とする請求項 3 4記載の距離姿勢計測装置。

3 6 . 前記第 3および第 4の円板状マークの各半径は、前記第 1および第 2の円板状マークの各半径より、それぞれ小さく構成されたことを特徵とする請求項 3 5記載の距離姿勢計測装

3 7 . 対象物までの距離および対象物の姿勢を計測する距離姿勢計測装置において、

各重心が同一平面上に位置するように対象物に配置され、少なくとも 1つの円の半径が他と異なる第 1乃至第 4の円扳状マ —クと、

前記第 1乃至第 4の円板状マークの各重心を通る第 1の平面と異なる第 2の平面上に配置された第 5および第 6の円板状マ —クと、

前記第 1乃至第 6の円板状マークを撮影する撮影手段と、前記撮影手段から出力される第 3乃至第 6の円板状マークの画像データに基づき、前記第 3および第 4の円板状マークの重心位置を算出するとともに、前記第 5および第 6の円板状マークを前記撮影手段方向へ投影して前記第 1の平面で得られる各投影図形における重心位置を算出する第 1の重心算出手段と、前記撮影手段から出力される第 1乃至第 4の円扳状マークの画像データに基づき、各円板状マークの重心位置を算出する第 2の重心算出手段と、

前記第 1の重心算出手段または前記第 2の重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、前記対象物までの距離および前記対象物の姿勢を演算する演算手段と、を有することを特徴とする距離姿勢計測装置。

3 8 . 前記演算手段は、前記第 1の重心算出手段が 4つの重心位置を算出しているときは前記第 1の重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、前記対象物までの距離および前記対象物の姿勢を演算し、一方、前記第 1の重心算出手段が 4つの重心位置を算出できないときは前記第 2 の重心算出手段が算出した 4つの重心位置を基に、 4点透視問題を解き、前記対象物までの距離および前記対象物の姿勢を演算することを特徴とする請求項 3 7記載の距離姿勢計測装置。