JP2000227309A - ３次元位置姿勢センシング装置 - Google Patents

Abstract

(57)【要約】 【課題】本発明は、遮蔽などの影響を受けずに、安定に
対象物の３次元位置姿勢を推定することができる３次元
位置姿勢センシング装置をを提供する。 【解決手段】本発明によると、撮像装置により撮像され
た、測定対象物に対する３次元位置情報が既知の少なく
とも３個のマーカが写っている画像を入力する画像入力
手段と、上記画像上で上記各マーカに対応する領域を抽
出する領域抽出手段と、上記抽出した領域において、マ
ーカの外観上の特徴から個々のマーカを特定するマーカ
特定手段と、上記特定された各マーカの上記画像上での
位置と、各マーカの測定対象物に対する３次元位置情報
を用いて、上記撮像装置に対する測定対象物の３次元位
置姿勢を演算する位置姿勢演算手段とを有することを特
徴とする３次元位置姿勢センシング装置が提供される。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、３次元位置姿勢セ
ンシング装置に係り、特に、画像撮影装置を利用して対
象物の３次元位置姿勢を推定することにより、対象物の
３次元位置姿勢をセンシングするようにした３次元位置
姿勢センシング装置に関する。

【０００２】

【従来の技術】一般に、相対的な位置が既知な少なくと
も３点のランドマークやマーカを画像撮影により認識
し、対象物と撮影装置との相対的位置姿勢関係を推定す
る問題は、ｎ点問題の一部として考えられる（文献１：
Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ ａｎｄ Ｒ．Ｃ．Ｂｏｌｌ
ｅｓ，“Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓ
ｕｓ：Ａ ｐａｒａｄｉｇｍ ｆｏｒ ｍｏｄｅｌ ｆ
ｉｔｔｉｎｇ ｗｉｔｈａｐｐｌｉｃａｔｉｏｎｓ ｔ
ｏ ｉｍａｇｅ ａｎａｌｙｓｉｓ ａｎｄ ａｕｔｏ
ｍａｔｅｄ ｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕｎ
ｉｃａｔｉｏｎｓｏｆ ｔｈｅ ＡＣＭ，Ｖｏｌ．２
４，Ｎｏ．６，Ｊｕｎｅ １９８１，ｐｐ．３８１−３
９５参照）。

【０００３】この場合、ランドマークあるいはマーカの
個数が３点のみのときには、解が複数個存在することが
知られている。

【０００４】このような問題を解決するために、特殊な
マーカを利用する方法としては、特開平７−９８２０８
号公報に開示されている方法などを挙げることができ
る。

【０００５】すなわち、この特開平７−９８２０８号公
報に開示されている方法は、大円１個と小円１個の位置
関係を利用するものである。

【０００６】また、文献２（Ｗ．Ａ．Ｈｏｆｆ，Ｔ．Ｌ
ｙｏｎ，ａｎｄ Ｋ．Ｎｇｕｙｅｎ，“Ｃｏｍｐｕｔｅ
ｒ Ｖｉｓｉｏｎ−Ｂａｓｅｄ Ｒｅｇｉｓｔｒａｔｉ
ｏｎＴｅｃｈｎｉｑｕｅｓ ｆｏｒ Ａｕｇｍｅｎｔｅ
ｄ Ｒｅａ１１ｔｙ，”Ｐｒｏｃ．ｏｆ Ｉｎｔｅｌｌ
ｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｃｏｍｐｕｔｅｒ
ＶｉｓｉｏｎＸＶ，Ｖｏｌ．２９０４，ｉｎ Ｉｎｔ
ｅｌｌｉｇｅｎｔＳｙｓｔｅｍｓ ａｎｄ Ａｄｖａｎ
ｃｅｄ Ｍａｎｕｆａｃｔｕｒｉｎｇ，ＳＰＩＥ，Ｂｏ
ｓｔｏｎ，Ｍａｓｓａｃｈｕｓｅｔｔｓ，Ｎｏｖ．１９
−２１，ｐｐ．５３８−５４８，１９９６．）では、形
状が同一の複数のマーカを利用して、カメラ画像から３
次元位置姿勢を推定するシステムが開示されている。

【０００７】

【発明が解決しようとする課題】しかるに、上述した特
開平７−９８２０８号公報に開示されているテクニック
では、基本的に、大円１個とその付近に定義された小円
１個のみからマーカが定義されるため、次のような欠点
があった。

【０００８】（１）大円１個と小円１個の画像内での大
きさが小さいときには、測定誤差が大きくなる。

【０００９】（２）大円１個と小円１個が遮蔽や画像処
理上の限界から認識できなかったときには、位置姿勢が
認識できなくなってしまう。

【００１０】また、上述した文献２では、複数のマーカ
が同一のパターン構造で構成されているときには、マー
カ自体を同定することが困難なことが多い。

【００１１】これは、遮蔽などの影響によりマーカ群の
一部が認識できないときには、特に問題となる。

【００１２】また、環境が複雑な場合には、単一カラー
などや白色または黒色だけで構成されるような単純なマ
一カの場合には、マーカと同様なパターンが、環境内に
存在する場合が多く、これらのマーカをマーカでないも
のから差異を明らかにしながら認識することが困難であ
った。

【００１３】そこで、本発明では、以上のような点に鑑
みてなされたもので、（１）遮蔽などの影響により、マ
ーカ群の一部が観察されないときでも、対象物の３次元
位置姿勢を推定することができるようにするとともに、
（２）従来のｎ点問題では、解を確定することができな
かった３個のマーカからだけでも位置姿勢を推定するこ
とができるようにした、３次元位置姿勢センシング装置
を提供することを目的としている。

【００１４】

【課題を解決するための手段】本発明によると、上記課
題を解決するために、（１） 撮像装置により撮像され
た、測定対象物に対する３次元位置情報が既知の少なく
とも３個のマーカが写っている画像を入力する画像入力
手段と、上記画像上で上記各マーカに対応する領域を抽
出する領域抽出手段と、上記抽出した領域において、マ
ーカの外観上の特徴から個々のマーカを特定するマーカ
特定手段と、上記特定された各マーカの上記画像上での
位置と、各マーカの測定対象物に対する３次元位置情報
を用いて、上記撮像装置に対する測定対象物の３次元位
置姿勢を演算する位置姿勢演算手段と、を有することを
特徴とする３次元位置姿勢センシング装置が提供され
る。

【００１５】また、本発明によると、上記課題を解決す
るために、（２） 撮像装置により撮像された、測定対
象物に対する３次元位置情報が既知の少なくとも４個の
マーカが写っている画像を入力する画像入力手段と、上
記画像上で上記各マーカに対応する領域を抽出する領域
抽出手段と、上記抽出した領域において、マーカの外観
上の特徴から個々のマーカを特定するマーカ特定手段
と、上記マーカ特定手段によって特定されたマーカより
３つのマーカを選択するマーカ選択手段と、上記マーカ
選択手段によつて選択された３つのマーカの上記画像上
での位置と、各マーカの測定対象物に対する３次元位置
情報を用いて、上記撮像装置に対する測定対象物の３次
元位置姿勢を演算するためのパラメータ組を複数算出す
るパラメータ組算出手段と、上記マーカ選択手段によっ
て選択されなかつたマーカに対して、上記パラメータ組
算出手段によつて算出されたパラメータ組を適用評価す
ることにより、１つのパラメータ組を選択するパラメー
タ組選択手段と、を有することを特徴とする３次元位置
姿勢センシング装置が提供される。

【００１６】また、本発明によると、上記課題を解決す
るために、（３） 前記パラメータ組選択手段によつて
選択された１つのパラメータ組に関して、各マーカの測
定対象物に対する３次元位置情報を適用して評価するこ
とにより、該パラメータ組を改善するパラメータ改善手
段をさらに含むことを特徴とする（２）記載の３次元位
置姿勢センシング装置が提供される。

【００１７】すなわち、本発明は、具体的には、位置姿
勢を推定すべき対象物あるいは対象物付近に、対象物に
対する相対的位置関係があらかじめ決定している『幾何
学的に固有の特徴を持った固有マーカ（コードマー
カ）』を複数配置し、その複数のマーカを画像撮影装置
により撮影し、画像内から固有マーカ群を抽出するとと
もに、そのマーカの幾何学的特徴を抽出かつそのマーカ
の同定を行い、そのマーカの対象物に対する３次元位置
と画像撮影装置により撮影された画像内での２次元位置
を利用して、画像撮影装置または他の装置を規定する座
標系における対象物の３次元位置姿勢を推定する方式を
与えるものである。

【００１８】

【発明の実施の形態】以下図面を参照して本発明の実施
の形態について説明する。

【００１９】（第１の実施の形態）図１乃至図８を参照
して第１の実施の形態について説明する。

【００２０】図１は、第１の実施の形態による３次元位
置姿勢センシング装置の構成を示すブロック図である。

【００２１】図１に示されているように、３次元位置姿
勢を推定すべき対象物１あるいはその付近には、幾何学
的に固有の特徴を持った複数の固有マーカ２（以下コー
ドマーカと略す）が配置されている。

【００２２】そして、このコードマーカ２を画像撮影装
置３により撮影し、その撮影した画像５がコンピュータ
４内に転送される。

【００２３】ここで、画像撮影装置３は、一般的なＴＶ
カメラなどあるいはディジタルビデオカメラなどでよ
く、画像撮影装置３から画像５を受け取るコンピュータ
４は、通常のコンピュータであっても特殊な画像処理演
算装置であっても構わない。

【００２４】また、画像撮影装置３としてのＴＶカメラ
がアナログ信号を出力する場合には、コンピュータ４内
には、画像５をディジタル化する装置やユニットが含ま
れていてもよい。

【００２５】また、画像撮影装置３がディジタルカメラ
やディジタルビデオカメラなどの場合には、直接コンピ
ュータ４に画像５を転送することにより、コンピュータ
４が画像５をディジタル信号として入力することができ
ればよい。

【００２６】このようにして、第１の実施の形態による
３次元位置姿勢センシング装置では、コンピュータ４が
画像撮影装置３から受け取ったコードマーカ２が撮影さ
れた画像５を受け取り、それがディジタル画像の形に変
換されるとともに、そのディジタル画像をコンピュータ
４で処理することにより、画像５内からコードマーカ２
を認識し、そのコードマーカの画像内位置とあらかじめ
登録されているコードマーカの３次元位置とを利用する
ことにより、対象物１の画像撮影装置３に対する３次元
位置姿勢を推定するものである。

【００２７】なお、本実施の形態では、少なくとも４個
のコードマーカが同定できた場合に関して、対象物の位
置姿勢を推定する方法に関して解説するものとする。

【００２８】そして、少なくとも３個のコードマーカが
同定できた場合に関しては、別の実施の形態で解説する
ものとする。

【００２９】まず、本実施の形態における画像と座標変
換に関する基本的な扱いに関して説明しておく。

【００３０】基本的に、対象物１と画像撮影装置３は固
有の座標系を有しており、画像撮影装置３が撮影した画
像５は、カメラ画像面として定義される。

【００３１】図２は、この画像撮影装置３と、カメラ画
像面と、対象物１が規定するオブジェクト座標系との関
係を表したものである。

【００３２】ここで、対象物１が規定するオブジェクト
座標系はその原点を０_m 、その３次元座標を（ｘ_m ，ｙ
_m ，ｚ_m ）とする。

【００３３】一方、画像撮影装置３が規定するカメラ座
標系は、その原点を０_c 、その３次元座標を（ｘ_c ，ｙ
_c ，ｚ_c ）とする。

【００３４】カメラ画像面はその軸がｕ軸とｖ軸により
構成され、ｕ軸はカメラ座標系のｘ_c と平行に、ｖ軸は
ｙ_c 軸に平行に取られ、カメラ座標系を規定するｚ_c 軸
が画像撮影装置３の光学系の光軸と一致し、その光軸と
カメラ画像面が交わる点（カメラ画像面の中心）が、
（ｕ₀ ，ｖ₀ ）で定義される。

【００３５】画像撮影装置３に相対する対象物１の３次
元位置姿勢を推定する問題は、カメラ座標系に対するオ
ブジェクト座標系の位置姿勢を推定する問題、言い換え
れば、オブジェクト座標系からカメラ座標系への座標変
換パラメータ、またはカメラ座標系からオブジェクト座
標系への座標変換パラメータを算出する問題に帰着され
る。

【００３６】これを数学的に記述すると、斉次変換行列
ｃＨｍまたはｍＨｃを利用して、

【数１】 と書くことができる。

【００３７】ここに、Ｒ＝（ｒ_ｉｊ），Ｒ′＝（ｒ′
_ｉｊ）は３×３の回転行列を表し、ｔ＝（ｔ_ｘ，ｔ_ｙ，
ｔ_ｚ），ｔ′＝（ｔ′_ｘ，ｔ′_ｙ，ｔ′_ｚ）は３次元並
進べクトルを表す。

【００３８】以下で詳細に説明するマーカ群｛Ｍ_ｉ；ｉ
＝１，２，．．．，ｍ｝は、あらかじめオブジェクト座
標系での３次元位置が計測されており、それらを（ｘ_i
^m ，ｙ_i ^m ，ｚ_i ^m ）で表現する。

【００３９】また、その画像内位置を（ｕ_ｉ，ｖ_ｉ）で
記述することとする。

【００４０】すると、画像撮影装置３をピンホールカメ
ラモデルで近似したとき、それらの座標間には、以下の
関係が成立する。

【００４１】

【数２】 ここに（ｕ_ｏ，ｖ_ｏ）は画像内の中心、（α_u ，α_v ）
はｕ方向とｖ方向の拡大率を表し、画像撮影装置３に関
するカメラ内部パラメータであり、カメラキャリブレー
ションにより推定できる値である。

【００４２】図３は、本実施の形態における幾何学的特
徴を持ったコードマーカ２群の一例を表したものであ
る。

【００４３】このコードマーカ２は、円形の形状をして
おり、その大円内には、小円のパターンがコードとして
表されている。

【００４４】この例では、大円の中央部に小円１個があ
り、そのまわりに４個の小円が配置されている。

【００４５】この５個の小円の白黒（またはカラー）コ
ードにより、各マーカに固有のラベルを付与することが
できる。

【００４６】例えば、図３の場合には、コード０からコ
ード１１までの１２個の異なったコードを生成すること
ができる。

【００４７】図４は、本実施の形態における別のコード
パターンを表したものである。

【００４８】このパタ一ンの場合には、大円の中に７個
の小円を配置することで、各種のコードを生成した場合
を表わしている。

【００４９】なお、コードを生成するパターンとして
は、これだけには限らす、例えば、図５に示されるよう
な、同心円上にコードを付加したものでも構わない。

【００５０】ここで、基本的に重要なことは、各マーカ
が幾何学的特徴を有しており、それがマーカにラベルを
付加するに足るコードを生成できることである。

【００５１】また、マーカ自体は円形でなくてもよく、
例えば、正方形でも正多角形でもよい。

【００５２】図６は、本発明において、コンピュータ４
に画像５が入力された後に、対象物１の３次元位置姿勢
を推定するまでの処理手順を示すフローチャートであ
る。

【００５３】以下、各ステップについて簡単に説明す
る。

【００５４】（１）ステップ１：画像５を受け取ったコ
ンピュータ４は、その画像５の中からコードマーカ２に
対応する領域と推定される候補領域を抽出する。

【００５５】（２）ステップ２：コンピュータ４は、上
記ステップ１により抽出された候補領域内を詳細に解祈
し、その中からコードマーカ２のコードに対応する幾何
学的特徴を抽出し、そのコードが認識された場合には、
その領域をマーカ領域として、その画像内位置とコード
を登録する。

【００５６】（３）ステップ３：コンピュータ４は、上
記ステップ２で登録された画像から抽出されたコードマ
ーカ２の画像内２次元位置と対象物１に相対する３次元
位置とを利用することにより、画像撮影装置３に対応す
る対象物１の３次元位置姿勢を算出する。

【００５７】以下、本実施の形態の中心となるステップ
１，２、３について、より詳細に説明する。

【００５８】ステップ１：本実施の形態では、画像撮影
装置３がカラー画像を生成するものとするとともに、コ
ードマーカ２としては、図３で示されるようなコードマ
ーカ（大円と小円の組み合わせ）からなると仮定する。

【００５９】この際、大円の背景色は、ある規定された
色から成り立っており、対象物１内で固有な色であると
仮定する。

【００６０】また、小円内のパターンは、白または黒の
みのパターンから成り立っていると仮定する。

【００６１】マーカの領域は単色から成立しているの
で、その単色に敏感な色フィルタをアルゴリズム内に導
入するものとする。

【００６２】具体的には、画像面（ｕ，ｖ）で定義され
る画像点に関して、カラー画像を構成する３フィルタの
計測値Ｒ（赤）、Ｇ（緑）、Ｂ（青）から、次の式

【数３】 に対応する３個のべクトルを算出し、マーカが取りうる
画像内での色パターンの許容値を、次の式

【数４】 を満たす画像領域を抽出する。

【００６３】ここで、ｉ_ｍｉｎ，ｉ_ｍａｘ，ｒ_ｍｉｎ，
ｒ_ｍａｘ，ｇ_ｍｉｎ，ｇ_ｍａｘなどの値は、あらかじめ
設定しておくものとする。

【００６４】次に、領域内の穴埋めを行うことによりマ
ーカに対応する領域を決定する。

【００６５】ステップ２：次に、抽出された領域がマー
カの像であるか否かを判定する。

【００６６】基本的に、マーカの形状は円形であるの
で、その投影像である画像内での領域は、楕円形で近似
することが可能である。

【００６７】従つて、本ステップ２では、マーカの領域
が楕円で近似することができるか否かを判定する。

【００６８】この方法としては、文献３（Ｋ．Ｒａｈａ
ｒｄｊａ ａｎｄ Ａ．Ｋｏｓａｋａ“Ｖｉｓｉｏｎ−
ｂａｓｅｄ ｂｉｎ−ｐｉｃｋｉｎｇ：Ｒｅｃｏｇｎｉ
ｔｉｏｎ ａｎｄ ｌｏｃａｌｉｚａｔｉｏｎ ｏｆ
ｍｕｌｔｉｐｌｅ ｃｏｍｐｌｅｘ ｏｂｊｅｃｔｓ
ｕｓｉｎｇ ｓｉｍｐｌｅ ｖｉｓｕａｌ ｃｕｅ
ｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ １９９６ ＩＥ
ＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆ
ｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂ
ｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，Ｏｓａｋａ，Ｊａｐ
ａｎ，Ｎｏｖｅｍｂｅｒ １９９６）で示されている方
法により行う。

【００６９】具体的には、（１）マーカの候補領域と考
えられる各領域を含む直方領域を切り出し、マーカ候補
領域内を１、領域外を０とラベル化する。

【００７０】この１でラベル化される領域に対して、穴
埋め処理を行い、内部に存在するラベル０で表される小
領域を除去する。

【００７１】（２）ラベル１で表されるマーカ候補領域
の１次モーメントｑ₀ と２次モーメントＭを算出する。

【００７２】（３）ラベル１で表されるマーカ候補領域
の境界点列の集合をＡ＝｛ｑ｝として、Ａの各点に関し
て、以下の式で表される正規化距離ｄを算出する。

【００７３】

【数５】 （４）ｄの集合Ａに対する平均値μと標準偏差σ_d を算
出する。

【００７４】そして、σ_d がある閾値よりも小さけれ
ば、マー力領域として登録するが、そうでなければマー
カ領域として登録しない。

【００７５】このようにして、楕円であると判定された
場合には、楕円領域内での３値処理により、楕円領域内
で考えられるパターンの抽出を行う。

【００７６】具体的には、図７の（ａ）に示されている
ようなマーカ領域に対して、 （１）上記ステップ１で得られた穴埋めされたマーカ領
域に対して、メディアンフィルタなどを施してノイズ成
分を除去する（図７の（ｂ））。

【００７７】（２）その領域内の濃淡値（明度）の平均
値μ_g と標準偏差σ_g を算出する。

【００７８】（３）領域内の各画素の濃度値ｇに対し
て、ある定められた実数ｔを利用し、 １）ｇ−μ_g ＞ｔσ_g ならば、その画素をラベル１ ２）ｇ−μ_g ＜−ｔσ_g ならば、その画素をラベル−１ ３）１）でも２）でもなければ、その画素をラベル０で
ラベル化する。

【００７９】こうして得られた領域内で、１，−１，０
で表される小領域を抽出する（図７の（ｃ））。

【００８０】（４）１または−１でラベル化された小領
域で、マーカ領域の中央にもつとも近い小領域を抽出す
る。

【００８１】この小領域をセンターパターンと命名し、
そのセンターパターンと領域の１次モーメントｑ₀ と２
次モーメントＭを利用し、センターパターンからその他
のパターンまでの正規化距離と正規化角度（楕円領域を
円領域ヘ変換したときのパターン間角度）を算出する。

【００８２】図７の（ｄ）は、このセンターパターンか
らその他のパターンまでの正規化距離と正規化角度を算
出する様子を表している。

【００８３】これらの正規化距離とパタ−ン間の正規化
角度がある一定の幾何学的性質を保っていたときには、
このマーカ候補領域は、マーカ領域として認識され、そ
の小領域が作るパターンのコードを読みとることによ
り、マーカの同定を行うことができる。

【００８４】図７の（ｄ）では、図３のコードパターン
のコード２として認識されることになる。

【００８５】このようにして同定されたマーカ領域は、
その画像内でのセンターパターンの重心位置が、コード
マーカの位置（ｕ_ｉ，ｖ_ｉ）（ｉ＝１，２，
３，．．．）として登録される。

【００８６】ステップ３：ステップ２で同定されたマー
カの画像内位置（ｕ_ｉ，ｖ_ｉ）（ｉ＝１，２，
３，．．．）とオブジェクト座標系における３次元マー
カ位置（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）が与えられたとき、
如何にして式（３）で表される斉次変換行列cHm 上を算
出するが、このステップ３の課題である。

【００８７】これは、基本的には、上述した文献１
（Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ ａｎｄＲ．Ｃ．Ｂｏｌｌ
ｅｓ，“Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓ
ｕｓ：Ａ ｐａｒａｄｉｇｍ ｆｏｒ ｍｏｄｅｌ ｆ
ｉｔｔｉｎｇ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎｓ
ｔｏ ｉｍａｇｅ ａｎａｌｙｓｉｓ ａｎｄ ａｕｔ
ｏｍａｔｅｄｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕｎ
ｉｃａｔｉｏｎｓ ｏｆｔｈｅ ＡＣＭ，Ｖｏｌ．２
４，Ｎｏ．６，Ｊｕｎｅ １９８１，ｐｐ．３８１−３
９５）で示される方法を以下に示すように変更しながら
行う。

【００８８】すなわち、この文献１で紹介されている方
法では、同定されたマーカ群の中から一直線上にない任
意の３個のマーカを選択し、その３個のマーカを利用し
てカメラ座標系とオブジェクト座標系間の座標変換パラ
メータ解の候補を算出するようにしている。

【００８９】しかるに、その座標変換パラメータとして
は、最大４種類の解が存在することがわかっているの
で、本発明では、その４種類の各解に対して、選択され
なかつたマーカ群を利用して解の検証を行うことによ
り、解の絞り込みを行うとともに、その解を初期値とし
て、すべてのマーカを利用して解の更新を行うようにし
たものである。

【００９０】以下、その方法に関して簡単に説明する。

【００９１】同定されたマーカ群の中から、ある選択基
準に従って画像内で一直線上にない３個のマーカを選択
する。

【００９２】この選択基準としては、 （１）カメラ画像面で３個のマーカを頂点とする３角形
の面積が最大となる３個のマーカを選択する方法 （２）カメラ画像面で３個のマーカを頂点とする３角形
の最大内角が最小である３個のマーカを選択する方法な
どが考えられる。

【００９３】このようにして得られたマーカをＭ_ｉ（ｉ
＝１，２，３）とする。

【００９４】次に、これら３個のマーカＭ_ｉ（そのモデ
ル座標系における３次元位置をＰ_i、（ｘ_i ^m ，
ｙ_i ^m ，ｚ_i ^m ）、画像内位置をＱ_i （ｕ_i ，ｖ_i ）と
する）に関して、図８に示されるような３個の３角形Δ
０_c Ｍ_ｉＭ_ｊ（ｉ，ｊ＝１，２，３；ｉとｊは等しくな
い）を考える。

【００９５】これらの３個の３角形に関して、カメラ画
像系の原点０_c から各マーカＭ_ｉまでの距離をｄ_ｉと
し、マーカＭ_ｉ，Ｍ_ｊとカメラ座標系原点０_c がなす角
度θ_ijとする。

【００９６】また、マーカＭ_ｉ，Ｍ_ｊ間の距離をＲ_ijと
する。

【００９７】このとき距離Ｒ₁₂，Ｒ₂₃，Ｒ₃₁と角度
θ₁₂，θ₂₃，θ₃₁は既知の値となるが、ｄ_１，ｄ_２，ｄ
_３は未知の値となる。

【００９８】逆に言えば、距離ｄ_１，ｄ_２，ｄ_３を算出
することができれば、オブジェクト座標系からカメラ座
標系への座標変換パラメータを算出することができる。

【００９９】以下この点に関して解説する。

【０１００】（１）距離Ｒ₁₂，Ｒ₂₃，Ｒ₃₁の算出方法 Ｒ₁₂は点Ｐ₁ と点Ｐ₂ 間のユークリッド距離として算出
される。

【０１０１】同様に、Ｒ₂₃、Ｒ₃₁はそれぞれ、点Ｐ₂ と
Ｐ₃ 、点Ｐ₃ とＰ₁ 間のユークリッド距離として算出さ
れる。

【０１０２】（２）角度θ₁₂，θ₂₃，θ₃₁の算出方法 マーカＭ_ｉ，Ｍ_ｊとカメラ座標系の原点０_ｃとがなす角
度θ_ijは以下のように算出することができる。まず、

【数６】 で与えられる。

【０１０３】このようにして、３個の角度をその余弦よ
り算出することができる。

【０１０４】（３）距離ｄ_ｉ（ｉ＝１，２，３）の算出 ３角形０ｃＭ_１Ｍ_２，０ｃＭ_２Ｍ_３，０ｃＭ_３Ｍ_１に対
して第２余弦定理を適用すれば、

【数７】 が導かれる。

【０１０５】これらの３つの式において、未知数は
ｄ_１，ｄ_２，ｄ_３の３個であり、制約式も３つであるの
で、理論的には上式を満たす解｛（ｄ_１（ｋ），ｄ
_２（ｋ），ｄ_３（ｋ））：ｋ＝１，２，３，４｝が存在
する。

【０１０６】その解法に関しては、上述した文献１
（Ｍ．Ａ．Ｆｉｓｃｈｌｅｒ ａｎｄＲ．Ｃ．Ｂｏｌｌ
ｅｓ，“Ｒａｎｄｏｍ ｓａｍｐｌｅ ｃｏｎｓｅｎｓ
ｕｓ：Ａ ｐａｒａｄｉｇｍ ｆｏｒ ｍｏｄｅｌ ｆ
ｉｔｔｉｎｇ ｗｉｔｈ ａｐｐｌｉｃａｔｉｏｎｓ
ｔｏ ｉｍａｇｅ ａｎａｌｙｓｉｓ ａｎｄ ａｕｔ
ｏｍａｔｅｄ ｃａｒｔｏｇｒａｐｈｙ，”Ｃｏｍｍｕ
ｎｉｃａｔｉｏｎｓ ｏｆ ｔｈｅ ＡＣＭ，Ｖｏｌ．
２４，Ｎｏ．６，Ｊｕｎｅ １９８１，ｐｐ．３８１−
３９５）で詳しく述べられているように、この方程式に
は最大４個の解が存在することがわかっており、その解
が４次方程式の解として数値解祈的に解くことが可能で
ある。

【０１０７】（４）解（ｄ_１，ｄ_２，ｄ_３）の検証と最
適解の選択 基本的には、最大４個の解の中で１個だけが正しい解を
与えるものである。

【０１０８】上記の解の中で、どの解が正しいかを検証
するのが、このステップである。

【０１０９】各解（ｄ_１，ｄ_２，ｄ_３）について、カメ
ラ座標系Ｃでのマーカ位置（ｘ_i ^c，ｙ_i ^c ，ｚ_i ^c ）
を算出する方法について解説する。

【０１１０】カメラ座標系原点Ｃからマーカまでの距離
がｄ_ｉであり、その画像内位置が（ｕ_i ，ｖ_i ）であ
り、

【数８】 が成立し、

【数９】 と書くことができる。

【０１１１】いま、オブジェクト座標系でのマーカ位置
を（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）とすれば、オブジェクト
座標系０_ｍからカメラ座標系０_ｃへの変換は、

【数１０】 と書かれる。

【０１１２】ここに、Ｒは回転行列を表し、ｔは並進ベ
クトルを表す。

【０１１３】いま、両座標系でのマーカ群の重心ベクト
ルを

【数１１】 が成立し、並進べクトルと回転行列を別々の式で算出す
ることができる。

【０１１４】ｉ＝１，２，３に対して、上記方程式を解
く方法としては、ｑｕａｔｅｒｎｉｏｎ法（四元数法）
がある。

【０１１５】この方法の詳細は文献３（Ｂ．Ｋ．Ｐ．Ｈ
ｏｒｎ，“Ｃｌｏｓｅｄ−ｆｏｒｍｓｏｌｕｔｉｏｎ
ｏｆ ａｂｓｏ１ｕｔｅ ｏｒｉｅｎｔａｔｉｏｎ ｕ
ｓｉｎｇ ｕｎｉｔ ｑｕａｔｅｒｎｉｏｎｓ，”Ｊｏ
ｕｒｎａｌ ｏｆ Ｏｐｔｉｃａｌ Ｓｏｃｉｅｔｙ
ｏｆ Ａｍｅｒｉｃａ Ａ，Ｖ０１．４，Ｎｏ．４，１
９８７，ｐｐ．６２９−６４２．）に述べられているの
で、その詳細はここでは省略する。

【０１１６】このようにＲ，ｔが算出されると、斉次変
換行列ｃＨｍは式（１），（２）により計算することが
できる。

【０１１７】以上のことを４個の解に対して繰り返すこ
とにより、ｃＨｍ（１），ｃＨｍ（２），ｃＨｍ
（３），ｃＨｍ（４）の４個の解を算出することができ
る。

【０１１８】さて、同定されたコードマーカのうち最初
の選択されなかつたコードマーカをＭ_４，
Ｍ_５，．．．，Ｍ_ｍとする。

【０１１９】次に、各斉次変換行列解ｃＨｍ（ｋ）（ｋ
＝１，２，３，４）に対して、解として最もふさわしい
解を、これらＭ_４，Ｍ_５，．．．，Ｍ_ｍを利用して決定
する方法を説明する。

【０１２０】（１）各解ｃＨｍ（ｋ）に対する評画関数
ｄｉｓｔ（ｋ）を最小にするｋを、以下のステップで算
出する。

【０１２１】（２）各解ｃＨｍ（ｋ）（ｋ＝１，２，
３，４）に関して以下の方法により、ｄｉｓｔ（ｋ）を
算出する。

【０１２２】ａ）ｄｉｓｔ（ｋ）：＝０と評価関数を初
期化する。

【０１２３】ｂ）同定されたが最初の３個に選択されな
かったマーカＭ_ｊ（ｊ＝４，５，．．．，ｍ）につい
て、そのオブジェクト座標系における３次元位置（ｘ_j
^m，ｙ_j ^m ，ｚ_j ^m ）を、ｃＨｍ（ｋ）を利用してカメ
ラ画像面に変換する。

【０１２４】その投影画像点を（ｕ_j ´，ｖ_j ´）とす
る。これは、

【数１２】 により算出することができる。

【０１２５】続いて、マーカＭ_ｊの実際に画像内で測定
された２次元位置（ｕ_j ，ｖ_j ）と投影画像点（ｕ
_j ´，ｖ_j ´）との２乗誤差ｅ_j を算出する。

【０１２６】この２乗誤差ｅ_j は、以下のようにして算
出することができる。

【０１２７】 ｅ_j ＝（ｕ_j ´−ｕ_j )² ＋（ｖ_j ´−ｖ_j ）² そして、ｄｉｓｔ（ｋ）は、

【数１３】 で求められる。

【０１２８】（３）以上のようにして求められたｄｉｓ
ｔ（ｋ）が最小となる斉次変換行列の解ｃＨｍ（ｋ）を
選択する。

【０１２９】要約すると、上記のステップで求められる
最適解ｃＨｍ（ｋ）は、コードマーカＭ_１，Ｍ_２，Ｍ_３
から生成される解のうち、他のマーカＭ_４，
Ｍ_５，．．．，Ｍ_ｍが最も支持する解を選択するもので
ある。

【０１３０】（５）解の更新 上記ステップ（４）で選択された解ｃＨｍ（ｋ）は、コ
ードマーカＭ_１，Ｍ_２，Ｍ_３から推定されたものであ
り、他のマーカＭ_４，Ｍ_５，．．．，Ｍ_ｍに対する測定
値を利用したものではない。

【０１３１】そこで、本ステップ（５）では、上記ステ
ップ（４）で算出された解ｃＨｍ（ｋ）を初期推定値ｃ
Ｈｍ⁽⁰⁾ として、すべてのコードマーカＭｉ（ｉ＝１，
２，．．．，ｍ）により、この解の更新を行う。

【０１３２】すなわち、ｃＨｍを角度成分（ｒｏｌｌ
（φ_ｚ）−ｐｉｔｃｈ（φ_ｙ）−ｙａｗ（φ_ｘ））と並
進成分（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ）に展開して、６次元未知変
数ｐ＝（φ_ｘ，φ_ｙ，φ_ｚ；ｔ_ｘ，ｔ_ｙ，ｔ_ｚ）とし、
その初期推定値をｐ⁽⁰⁾ ＝（φ_ｘ ⁽⁰⁾ ，φ_ｙ ⁽⁰⁾ ，φ_ｚ
⁽⁰⁾ ；ｔ_ｘ ⁽⁰⁾ ．ｔ_ｙ ⁽⁰⁾ ，ｔ_ｚ ⁽⁰⁾ ）と定義する。

【０１３３】具体的には、

【数１４】 により定義される。

【０１３４】これをオブジェクト座標系でのマーカ３次
元位置（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）と、そのカメラ画像
面での位置（ｕ_i ，ｖ_i ）の関係を利用しながら、６次
元位置姿勢パラメータｐ＝（φ_ｘ，φ_ｙ，φ_ｚ；ｔ_ｘ，
ｔ_ｙ，ｔ_ｚ）を更新することを考える。

【０１３５】オブジェクト座標系でのマーカ３次元位置
（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）と、そのカメラ画像面での
位置（ｕ_i ，ｖ_i ）の関係は

【数１５】 により与えられる。

【０１３６】この式を整理すると、各マーカＭ_ｉ（ｉ＝
１，２，．．．，ｍ）に関して、

【数１６】 なる２次制約式によって表現され、６次元パラメータの
初期推定値ｐ⁽⁰⁾ ＝（φ_ｘ ⁽⁰⁾ ，φ_ｙ ⁽⁰⁾ ，φ_ｚ ⁽⁰⁾ ；
ｔ_ｘ ⁽⁰⁾ ．ｔ_ｙ ⁽⁰⁾ ，ｔ_ｚ ⁽⁰⁾ ）を用いて、６次元パラ
メータｐ＝（φ_ｘ，φ_ｙ，φ_ｚ；ｔ_ｘ，ｔ_ｙ，ｔ_ｚ）を
推定する問題となる。

【０１３７】この問題はよく知られた非線形方程式問題
であり、多くの文献がその解法を紹介しているので、こ
こではその詳細を述べない。

【０１３８】このようにして６次元パラメータは、すべ
てのマーカの測定値を利用して更新され、オブジェクト
座標系からカメラ座標系への座標変換パラメータが算出
される。

【０１３９】すなわち、対象物１と画像撮影装置３との
間の位置関係を算出することができる。

【０１４０】以上のような第１の実施の形態によれば、
マーカ群の一部が遮蔽されて検出できなかった場合で
も、検出されたマーカだけから対象物１と画像撮影装置
３との間の３次元位置関係を算出することができる。

【０１４１】また、マーカの検出に際しては、マーカ固
有のコードを利用することにより、マーカ同定の信頼性
が従来技術に比べて格段に向上させることができるの
で、より安定な位置姿勢計測を実現することができる。

【０１４２】（第２の実施の形態）次に、第２の実施の
形態について説明する。

【０１４３】上述した第１の実施の形態では、画像撮影
装置３としてカラー画像を生成できるものを仮定し、そ
の色情報を利用することにより、マーカ領域を抽出する
ようにしている。

【０１４４】これに対し、本第２の実施の形態では、カ
ラー画像を用いずにモノクロ画像を撮影する画像撮影装
置３を利用し、マーカが独自に持つ幾何学的構造を画像
内から抽出することにより、マーカ領域を抽出するよう
にしている。

【０１４５】また、本第２の実施の形態では、抽出され
たマーカ領域の領域の大きさの情報を利用してカメラ座
標系からマーカまでの距離の初期推定値を算出すること
により、マーカの数が３個のときでも、安定的に対象物
１と画像撮影装置３間の３次元位置姿勢パラメータを算
出することができるようにしている。

【０１４６】上述した第１の実施の形態のようにカラー
画像を利用する方式では、複雑な環境下で照明光が変化
する場合には、単一色領域を抽出したり、単純な閾値処
理によってマーカに対応する領域を正確に抽出すること
が困難な場合が多い。

【０１４７】本実施の形態で説明する方式では、このよ
うな複雑な環境下でもロバストに３次元位置姿勢推定が
行えるように、マーカが独自に持つ幾何学的関係を画像
内から抽出することを行う。

【０１４８】本実施の形態の基本的な構成は図１で示さ
れた構成と同様であり、その処理のステップは、図６で
示された方法と同様であるが、ステップ１では、画像撮
影装置３がカラー画像の代わりにモノクロ画像をコンピ
ュータ４に送出する。

【０１４９】また、ステップ２では、カラー画像の代わ
りにモノクロ画像からマーカ領域が抽出される点が異な
る。

【０１５０】また、ステップ３においては、マーカ領域
からマーカを同定するのに必要となるコードが検出され
るとともに、マーカ領域自体の大きさの情報が抽出され
る点が異なる。

【０１５１】さらに、ステップ４においては、マーカ領
域の大きさの情報も利用しながら、対象物１と画像撮影
装置３との間の３次元位置姿勢パラメータが算出され
る。

【０１５２】以下、その詳細を、ステップ２、３、４に
ついて解説する。

【０１５３】なお、本実施の形態でも、コードマーカと
しては、図３で示されるような円形マーカを想定して解
説する。

【０１５４】ステップ２：図９は、第２の実施の形態に
おけるステップ２での処理の手順を示すフローチャート
である。

【０１５５】ステップ１１では、画像撮影装置３により
送出されたモノクロ画像が、コンピュータ４内のメモリ
領域に記憶された後、その画像配列Ｉ（ｕ，ｖ）に対し
て、メディアンフィルタなどの平滑化フィルタを適用
し、マーカ領域内に存在する微細なテクスチャや画像に
含まれるノイズ成分を除去する。

【０１５６】続いて、ステップ１２では、平滑化された
画像について領域分割アルゴリズムを適用し、画像を領
域分割する。

【０１５７】この領域分割アルゴリズムとしては、上述
した文献３（Ｋ．Ｒａｈａｒｄｊａａｎｄ Ａ．Ｋｏｓ
ａｋａ“Ｖｉｓｉｏｎ−ｂａｓｅｄ ｂｉｎ−ｐｉｃｋ
ｉｎｇ：Ｒｅｃｏｇｎｉｔｉｏｎ ａｎｄ ｌｏｃａｌ
ｉｚａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｃｏｍｐｌ
ｅｘ ｏｂｊｅｃｔｓ ｕｓｉｎｇ ｓｉｍｐｌｅｖｉ
ｓｕａｌ ｃｕｅｓ，”Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ
１９９６ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏ
ｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉ
ｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ，Ｏ
ｓａｋａ，Ｊａｐａｎ，Ｎｏｖｅｍｂｅｒ １９９６）
に示されているＳｐｅｄｇｅ−ａｎｄ−Ｍｅｄｇｅ方法
や、文献４（Ｔ．Ｐａｖｌｉｄｉｓ ａｎｄ Ｙ．Ｌｉ
ｏｗ，“Ｉｎｔｅｇｒａｔｉｎｇ ｒｅｇｉｏｎ ｇｒ
ｏｗｉｎｇ ａｎｄ ｅｄｇｅ ｄｅｔｅｃｔｉｏ
ｎ，”ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｐ
ａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍａｃｈｉ
ｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ，Ｖｏｌ．１２，Ｎ
ｏ．３，ｐｐ．２２５−２３３，１９９０）に示される
Ｓｐｌｉｔ一ａｎｄ−Ｍｅｒｇｅ方法や、Ｃａｎｎｙエ
ッジ抽出方法で抽出したエッジ成分を連結して領域分割
する方法など、どのような方法でもよい。

【０１５８】次に、ステップ１３では、このようにして
分割される各領域に関して、その領域の幾何学的特徴量
を計算する。

【０１５９】例えば、その領域ｋに対して『面槓Ａ
（ｋ）』、『大きさＬ（ｋ）』、『濃度の平均値ｍ
（ｋ）』、『濃度の標準偏差ｓ（ｋ）』などを算出す
る。

【０１６０】これらの特徴量が、マーカとして取りうる
値として妥当であるか否かを閾値処理により判定する。

【０１６１】具体的には、上記各特徴量が、あらかじめ
定められた範囲内の値であるか否かを見ればよい。

【０１６２】例えば、上記各特徴量が、面積Ａ（ｋ）、
大きさＬ（ｋ）、濃度の平均値ｍ（ｋ）、濃度の標準偏
差ｓ（ｋ）の場合には、

【数１７】 をすべて満たしているか否かを判定条件とすればよい。

【０１６３】ここで、Ａｍｉｎ，Ａｍａｘなどの値は、
対象物１の大きさ、コ一ドマーカ２の大きさ・色、画像
撮影装置３と対象物１の距離の上限と下限などを考慮す
ることにより、あらかじめ設定できるものである。

【０１６４】このようにして分割された領域の中からマ
ーカに対応すると思われる領域の１次候補が選択され
る。

【０１６５】続いて、ステップ１４では、第１次選択で
候補とされた領域に関して、その領域がコードマーカ領
域として妥当であるか否かを詳細に判定する。

【０１６６】この場合、基本的にコードマーカの外形
は、図３に示されるように円形であるので、その画像内
での投影像は楕円で近似することができる。

【０１６７】そこで、第１次選択で候補とされた領域
が、楕円であるか否かを詳細に判定する。

【０１６８】この判定方法については、第１の実施の形
態の場合と同様であるので、ここでは省略する。

【０１６９】これでステップ２は終了する。

【０１７０】ステップ３：このステップ３において、第
１の実施の形態の場合と原画像がカラー画像ではなく、
モノクロ画像であることが異なるが、その処理の方法は
第１の実施形態の場合と同様であるので、ここではその
解説を省略する。

【０１７１】ステップ４：ステップ３で、コードマーカ
が同定できた後、ステップ４へ引き継がれる情報は、各
コードマーカに対して、オブジェクト座標系におけるコ
ードマーカの３次元位置（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）、
カメラ画像面における位置（ｕ_i ，ｖ_i ）、コードマー
カの画像内における楕円近似に基づく長軸の長さｒ_i で
ある。

【０１７２】本ステップでは、コードマーカの画像内に
おける楕円近似の長軸の長さｒ_i を利用して、カメラ座
標系から各コードマーカまでの距離ｄ_ｉの初期推定値を
算出し、その初期推定値を有効に利用することで対象物
１と画像撮影装置３との間の３次元位置関係を計測する
方法を解説する。

【０１７３】（１）画像撮影装置３からマーカ２までの
距離の初期推定値の算出 以上のようにしてマーカが同定できた場合には、そのマ
ーカ２と画像撮影装置３との間の距離の初期推定値を算
出する。

【０１７４】以下この方法を説明する。

【０１７５】図１０に示すように、マーカの中心を
Ｐ_i 、カメラの焦点を０_c とするとともに、カメラ像面
と０_c Ｐ_i の交点をＱ_i とすると、Ｑ_i はマーカ像の中
心であると判断することができる。

【０１７６】また、マーカの３次元モデルを半径Ｒ_i の
球で近似し、マーカの像を楕円で近似するとき、その画
像内の楕円の長軸の長さをｒ_i とする。

【０１７７】

【数１８】 ここに、θ_i はカメラの光軸と０Ｐ_i がなす角度であ
り、ｚ_i はＰ_i のカメラ座標系におけるｚ値である。

【０１７８】そして、ｚ_i とｄ_i の間には、以下の関係
が成立するから、

【数１９】 と表現することができる。

【０１７９】（２）対象物１と画像撮影装置３間の３次
元位置姿勢推定 カメラ座標系０_c でのマーカ位置（ｘ_i ^c ，ｙ_i ^c ，ｚ
_i ^c ）を算出する方法についてまず解説し、続いてオブ
ジェクト座標系とカメラ座標系間の座標変換パラメータ
の算出について解説する。

【０１８０】各マーカ_ｉとマーカ_ｊに対して、その３次
元的距離をＲ_ｉｊ、カメラ視点０_c（焦点位置）と
Ｑ_ｉ，Ｑ_ｊがなす角度をθ_ｉｊとすると、

【数２０】 で与えられる。

【０１８１】各マーカの基準座標系での３次元位置は確
定しているので、マーカ間の距離もあらかじめ確定して
いる。

【０１８２】いま、この距離をａ_ijとすれば、三角形の
余弦定埋から

【数２１】 が成立しなければならない。

【０１８３】従って、前ステップで求められたｄ_ｉの推
定値とｄ_ｉの誤差分散ならびにｃｏｓ（θ_ｉｊ）の誤差
分散を利用することにより、ｄ_ｉの初期値を更新するこ
とができる。

【０１８４】このようにｄ_ｉの初期値を更新する方法と
しては、 （１）ニュートン法を利用する方法 （２）準ニュートン法を利用する方法 （３）カルマンフィルタを利用する方法 など多くの方法が挙げられる。

【０１８５】本実施の形態では（３）のカルマンフィル
タを利用して解を算出する方法について述べる。

【０１８６】上記で説明されている距離ｄ_ｉ（ｉ＝１，
２，．．．，ｎ）を変数とするべクトルｐを定義し、そ
の初期分散行列をＳ、すなわち

【数２２】 で示される繰り返し演算を行うことにより、求めるべき
べクトルｐを更新することができる。

【０１８７】このｐはカメラ座標系の原点からマーカ位
置までの距離に対応する。

【０１８８】この距離が推定できると、第１の実施の形
態で説明した方法と同様にして、オブジェクト座標系か
らカメラ座標系への座標変換パラメータを算出すること
ができる。

【０１８９】以上、第２の実施の形態によれば、原画像
としてカラー画像を利用する必要がないため、より安価
なシステムを構成することができるとともに、最低３個
のコードマーカを使用する場合でも、対象物と画像撮影
装置間の３次元位置関係を計測することができる。

【０１９０】（第３の実施の形態）次に、第３の実施の
形態について説明する。

【０１９１】上記第１及び第２の実施の形態において
は、原画像からマーカ領域と思われる領域を抽出するの
に、原画像と同じサイズの画像を利用するようにしてい
る。

【０１９２】しかるに、本発明では、基本的にマーカに
対応する領域は、円形や多角形などの単純な形状をして
おり、その内部背景は均一な色などで構成されているこ
とから、原画像と同じサイズの画像を処理しなくてもマ
ーカ領域と想定される領域を抽出することが可能であ
る。

【０１９３】そこで、本実施の形態では、原画像をいっ
たん縮小し、縮小された画像に対して、第１または第２
の実施の形態において、マーカ領域を抽出するととも
に、縮小画像内で抽出されたマーカの位置を利用して、
原画像でのマーカ領域の位置を推定するとともに、原画
像サイズでのマーカ領域内で、コードマーカの同定を行
うことにより、原画像サイズでのマーカ領域抽出に関わ
る処理時間を短縮することができる手法を説明するもの
である。

【０１９４】説明の関係上、本実施の形態では、第２の
実施の形態で説明した方法に関して、縮小を生成して処
理を行う場合について説明する。

【０１９５】しかしながら、第１の実施の形態で説明し
た方法への適用も容易であることは言うまでもない。

【０１９６】本実施の形態では、画像の大きさを原画像
から１／１６に縮小した場合について、説明する。

【０１９７】基本的には、図６のステップ１におけるマ
ーカ領域の抽出を縮小画像で行うものである。

【０１９８】サブステップ１：原画像を１／１６に縮小
する（行方向に１／４、列方向に１／４とする）。

【０１９９】いま、原画像の画像配列を（ｉ，ｊ）と
し、縮小画像の画像配列を（ｉｓ，ｊｓ）とすると、ｉ
＝４＊ｉｓ＋０，ｉ＝４＊ｉｓ＋１，ｉ＝４＊ｉｓ＋
２，ｉ＝４＊ｉｓ＋３の各Ｉに対して、かつｊ＝４＊ｊ
ｓ＋０，ｊ＝４＊ｊｓ＋１，ｊ＝４＊ｊｓ＋２，ｊ＝４
＊ｊｓ＋３に対するＪにおける１６画素の平均値を（ｉ
ｓ，ｊｓ）の画素値とすることで、縮小画像を作成す
る。

【０２００】サブステップ２：サブステップ１で作成さ
れた縮小画像に対して、マーカに対応すると予想される
領域を抽出する。

【０２０１】この抽出方法は、第１の実施の形態または
第２の実施の形態で述べられた方法と同様である。

【０２０２】サブステップ３：サブステップ２で抽出さ
れた領域の位置座標（ｉｓ，ｊｓ）を４倍ずつすること
により、原画像におけるマーカ領域を推定する。

【０２０３】以上のような第３の実施の形態で説明した
手法をとることにより、原画像の大きさに比べて１／１
６の大きさの縮小画像を処理することで、マーカと想定
される領域を抽出することができるので、全体の処理を
高速化することができる。

【０２０４】（第４の実施の形態）次に、第４の実施の
形態について説明する。

【０２０５】この第４の実施の形態では、コードマーカ
以外のランドマーク点を利用して、位置姿勢推定の精度
を向上する方法について説明する。

【０２０６】これまでに解説した第１乃至第３の実施の
形態による方法では、コードマーカと呼ばれるマーカの
３次元位置モデルとその画像への投影像の２次元位置と
の関係のみを利用して、コードマーカが規定する対象物
と画像撮影装置間の３次元位置姿勢を推定するようにし
ている。

【０２０７】このような方法は、コードマーカを多数利
用することができる場合、または計測精度があまり必要
ないときにはよい。

【０２０８】しかしながら、高精度の位置姿勢推定が要
求される場合には、より多くのマーカ３次元モデル特徴
と画像内での観測点の対応関係が必要となる。

【０２０９】以下に述べる第４の実施の形態による手法
では、コードマーカ以外のモデル特徴を利用し、該モデ
ル特徴と観測された画像特徴の対応関係を増加させるこ
とにより、より高精度の位置姿勢推定を実現するもので
ある。

【０２１０】以下、このようなモデル特徴のことをラン
ドマークと呼ぶことにし、そのランドマークをどのよう
に画像内から抽出し、かつ抽出されたランドマークを利
用して、どのように対象物１の位置姿勢を推定するかに
ついて、幾つかの実施例に基づいて解説する。

【０２１１】（実施例１）まず、実施例１として、コー
ドマーカ位置以外のモデル特徴として、すでに認識され
た近接するコードマーカの位置を利用してモデル特徴の
画像内位置を利用し、コードマーカ以外のモデル特徴の
位置を推定するとともに、その推定された位置付近の画
像特徴を抽出し、モデル特徴と画像特徴間の対応関係を
利用して、対象物の位置姿勢を推定する手法を説明す
る。

【０２１２】いま、オブジェクト座標系で、コードマー
カ０，１，．．．，ｍ−１が位置べクトルｐ_０，
ｐ_１，．．．，ｐ_ｍ−１で表され、ランドマークｋの位
置べクトルｐ_k が、ｐ_０，ｐ_１，．．，ｐ_ｍ−１で線形
結合で表されるとすると（ｂ_０，ｂ_１，．．．，ｂ
_ｍ−１は定数）、

【数２３】 その画像内での位置ｑ_k ＝（ｕ_k ，ｖ_k ）は、コードマ
ーカの画像内計測値ｑ₀ ＝（ｕ₀ ，ｖ₀ “），ｑ₁ ＝
（ｕ₁ ，ｖ₁ ），ｑ_m-1 ＝（ｕ_m-1 ，ｖ_m-1 ）を利用し
て

【数２４】 で予測することができる。

【０２１３】この予測値を利用し、ｑ_k の近傍で最も確
からしいランドマーク画像内から抽出することになる。

【０２１４】図１１は、画像内からランドマークを抽出
する一例を示しており、ここでは、この図１１にあるよ
うな例についてを考える。

【０２１５】この例では、コードマーカ０，１，２があ
らかじめ抽出されていると仮定し、それら３個のコード
マーカのちようど重心の位置に円形のランドマークが配
置されている場合を考える。

【０２１６】すると、画像内で抽出されたコ一ドマーカ
０，１，２の画像内位置の重心を取り、その重心付近で
ある適当な大きさのウィンドウを設定し、そのウィンド
ウ領域内にある円形の領域を閾値法で抽出することによ
り、ランドマークｋの抽出を行うことができる。

【０２１７】このようにしてランドマークｋに対応する
領域が抽出されると、その領域の重心位置が、ランドマ
ークｋの位置として登録される。

【０２１８】このようにして、コードマーカ群とランド
マーク群が抽出され、画像内での位置が算出されると、
第１の実施の形態で述べたような位置推定方法で対象物
１の位置姿勢を推定することができる。

【０２１９】また、別の方法としては、第２の実施の形
態で説明したように、まず、最初にコードマーカ群を利
用して対象物位置姿勢を推定し、対象物１の初期推定値
を算出したのち、第１の実施の形態の解の更新の方法で
述べたような方法により、ランドマーク群の画像内位置
とオブジェクト座標系における位置を利用することで、
対象物１の位置姿勢の推定値を更新することができる。

【０２２０】（実施例２）ここで述べる実施例２の手法
では、あらかじめ抽出されたコードマーカを利用して、
対象物１の３次元位置姿勢の初期推定値を算出するとと
もに、その初期推定値を利用して、ランドマークの画像
内でのあるべき予測位置の推定値を算出し、その推定位
置付近で、ランドマークを探索する。

【０２２１】そして、ランドマークが同定できた場合に
は、そのランドマークのオブジェクト座標系での３次元
位置と画像内での２次元位置を利用して、対象物１の３
次元位置姿勢の推定値を更新するものである。

【０２２２】以下、その方法について説明する。

【０２２３】いま、画像内からコードマーカ０，
１，．．．，ｍ−１が同定されたと仮定すると、これら
のオブジェク卜座標系での３次元位置座標を画像内で計
測された２次元座標を利用して、対象物１の３次元位置
姿勢を推定することができる。

【０２２４】この方法は、第１あるいは第２の実施の形
態で示した方法と同様である。

【０２２５】いま、この３次元位置姿勢の初期推定値を
ｃＨｍとする。

【０２２６】また、ランドマークｋのオブジェクト座標
系における位置を（ｘｋ，ｙｋ，ｚｋ）とすると、ラン
ドマークｋの画像内における予測値（ｕｋ，ｖｋ）は式
（３），（４）により算出することができる。

【０２２７】この予測値（ｕｋ，ｖｋ）の周辺で、画像
内からランドマークを抽出・同定することができる。

【０２２８】このようにして、各ランドマークを抽出・
同定できた場合には、実施例２で説明したのと同様にし
て、対象物１の３次元位置姿勢の推定値を更新すること
ができる。

【０２２９】以上、述べたような手法を利用すれば、コ
ードマーカ以外のランドマークを利用して、対象物の３
次元位置姿勢を推定することが可能となり、よりロバス
トで正確な位置姿勢推定を行うことがてきる。

【０２３０】また、この手法により、あらかじめ登録し
ておくべきコードマーカの数を減少させることもでき
る。

【０２３１】（第５の実施の形態）この第５の実施の形
態では、画像撮影装置以外の装置と対象物間の３次元位
置姿勢を推定する手法を説明する。

【０２３２】これまでに説明してきた第１乃至４の実施
の形態は、いずれも、対象物１と画像撮影装置３間の位
置関係を計測する例である。

【０２３３】しかるに、より実際的な例の場合として、
図１に示される画像撮影装置３とコンピュータ４は、あ
るシステムにおいて、対象物１の位置姿勢を推定する位
置センサとして利用される場合がある。

【０２３４】この際には、画像撮影装置３とコンピュー
タ４からなる位置センサを含むシステム内に別の装置が
あり、その別の装置が規定する座標系をシステムが基準
とするシステム座標系と考えることが多い。

【０２３５】図１２は、第５の実施の形態で想定される
構成を表したブロック図である。

【０２３６】すなわち、図１２に示すように、コードマ
ーカ１２２が装着された対象物１２１を撮影する画像撮
影装置１２３が画像データをデータ処理装置であるコン
ピュータ１２４に送出し、コンピュータ１２４はその画
像データを解析することにより、対象物１２１が規定す
るオブジェクト座標系から画像撮影装置１２３が規定す
るカメラ座標系への座標変換パラメータを算出する。

【０２３７】一方、コンピュータ１２４は、画像撮影装
置１２３が規定するカメラ座標系から基準座標系を規定
する装置１２５が規定する基準座標系への座標変換パラ
メータをあらかじめ格納している。

【０２３８】その座標変換パラメータを利用して、コン
ピュータ１２４は、オブジェクト座標系から該基準座標
系への座標変換パラメータを算出する。

【０２３９】いま、基準座標系を規定する装置１２５に
対して、画像撮影装置１２３がキャリブレーションでき
ていると仮定する。

【０２４０】より具体的には、画像撮影装置１２３が規
定するカメラ座標系０ｃと基準座標系０ｒの間の関係が
斉次変換行列ｒＨｃ（カメラ座標系から基準座標系への
斉次変換行列）で決定されていると仮定する。

【０２４１】コンピュータ１２４が画像からマーカ群を
同定し、カメラ座標系での対象物１２１の３次元位置姿
勢を推定したとすると、オブジェクト座標系からカメラ
座標系への座標変換パラメータを算出することができ、
それはｃＨｍで表すことができる。

【０２４２】これら２個の斉次変換行列から、オブジェ
クト座標系から基準座標系への斉次変換行列を、以下の
ように算出することができる。

【０２４３】

【数２５】 このことにより、基準座標系における対象物１２１の３
次元位置姿勢を推定することができる。

【０２４４】本第５の実施の形態によりわかるように、
本発明を別の装置のための位置姿勢センサとして利用す
ることも可能である。

【０２４５】（第６の実施の形態）この第６の実施の形
態では、本発明をセンサブローブ（ワイヤレスセンサプ
ローブ）について適用する場合について説明する。

【０２４６】近年、３次元オブジェクトの３次元点を計
測するために、センサプローブが多く開発されるように
なってきている。

【０２４７】その手法としては、Ｆｌａｓｈ Ｐｏｉｎ
ｔのような光学的センサを利用する方法、磁気センサを
利用する方法などがある。

【０２４８】Ｆｌａｓｈ Ｐｏｉｎｔのような発光ダイ
オードを利用する光学的センサの場合には、高精度で計
測できることはあるが、センサプローブと装置間をワイ
ヤで接続する必要があるので、操作性の問題がある。

【０２４９】一方、磁気センサの場合には、ワイヤレス
での計測が可能であるが、金属の器具などが周辺にある
とノイズの影響が大であるという問題点がある。

【０２５０】本実施の形態は、これらの問題点を解決す
べく発明されたワイヤレスでかつ電磁波などの影響を受
けないセンサプローブに適用した場合である。

【０２５１】具体的には、センサプローブ自体に第１の
実施の形態または第２の実施の形態で説明したようなコ
ードマーカを装着することにより、画像撮像装置からセ
ンサプローブまでの位置姿勢を推定し、センサプローブ
が探針する位置を推定するものである。

【０２５２】以下、具体的に、このセンサプローブに適
用した場合について解説する。

【０２５３】図１３は、この第６の実施の形態による構
成を表すブロック図である。

【０２５４】すなわち、図１３に示すように、探針され
るべき物体Ｘ１３９をセンサプローブ１３８で探針す
る。

【０２５５】そのセンサプローブ１３８には、これまで
の実施の形態で説明してきたようなコードマーカ（２，
１２２）が装着されており、そのコードマーカを画像撮
影装置１３３が撮影する。

【０２５６】そして、画像撮影装置１３３で撮影された
画像データは、コンピュータ１３４に送信される。

【０２５７】コンピュータ１３４は、画像撮影装置１３
３から送信される画像データに基づいて画像解析するこ
とにより、まず、画像撮影装置１３３に対するセンサプ
ローブ１３８の位置姿勢パラメータを計測する。

【０２５８】この場合、センサプロープ１３８が基準と
すべき座標系は、コンピュータ１３４に接続された基準
座標系を規定する装置１３６により規定されており、コ
ンピュータ１３４はその基準座標系に合致した探針点の
３次元位置データ１３７を該基準座標系を規定する装置
１３６に送出する。

【０２５９】図１４は、センサプローブ１３８の例を表
したものである。

【０２６０】この図１４において、センサプローブに
は、その先端に探針すべき針がついており、その針の先
端が、第１または第２の実施の形態で述べられたような
オブジェク座標系の原点として定義される。

【０２６１】そして、それを原点としてオブジェクト座
標系を規定するＸm 軸、Ｙm 軸、Ｚm 軸の３軸が定義さ
れる。

【０２６２】また、このセンサプローブには、第１の実
施の形態や第２の実施の形態で解説したような複数のコ
ードマーカが装着されている。

【０２６３】そのコードマーカの位置はオブジェクト座
標系上であらかじめ確定されていると仮定し、その３次
元座標を（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）とする。

【０２６４】このとき画像撮影装置１３３で、このコー
ドマーカが撮影され、その画像がコンピュータ１３４で
解析されることにより、オブジェクト座標系から画像撮
影装置１３３が規定するカメラ座標系への座標変換パラ
メータを算出することができる。

【０２６５】すなわち、オブジェクト座標系から画像撮
影装置１３３が規定するカメラ座標系への斉次変換行列
を算出することができる。すなわち、

【数２６】 で表現されるｃＨｍを算出することができる。

【０２６６】さて、センサプローブ１３８の針の先端が
オブジェクト座標系の原点と一致することから、（ｘ_i
^m ，ｙ_i ^m ，ｚ_i ^m ）＝（０，０，０）を代入すると、
針先端のカメラ座標系における３次元位置を算出するこ
とができる。

【０２６７】その値は並進ベクトルｔそのものの値とな
る。

【０２６８】画像撮影装置１３３がある基準座標系Ｒに
おいて、キャリブレーションされている場合を考える。

【０２６９】すなわち、図１３で示されたように、他の
装置１３６の座標系で画像撮影装置１３３が規定されて
いる場合を考える。

【０２７０】この場合には、画像撮影装置１３３から基
準座標系への斉次変換行列ｒＨｃがあらかじめ較正され
ていると考えることができる。

【０２７１】従って、基準座標系におけるセンサプロー
ブ１３８の先端の３次元座標（ｘ_i ^r ，ｙ_i ^r ，
ｚ_i ^r ）は、

【数２７】 で表されることになる。

【０２７２】これにより、本センサプローブは基準座標
系における探針点の３次元位置を提供することができ
る。

【０２７３】そして、本実施の形態によれば、従来技術
では実現できなかったワイヤレスのセンサプローブを実
現することができることになる。

【０２７４】（第７の実施の形態）この第７の実施の形
態では、ステレオカメラを利用するようにした場合につ
いて説明する。

【０２７５】これまで説明してきた実施の形態は、画像
撮影装置が撮影する画像１枚を使用して対象物と画像撮
影装置間の位置関係を計測する場合についてである。

【０２７６】本実施の形態では、画像撮影装置が複数あ
り、それらと対象物間の位置関係を計測する方法につい
て解説する。

【０２７７】本方式によれば、検出されるべきコードマ
ーカの数が３個の場合でも、安定的に位置姿勢推定が可
能となる。

【０２７８】基本的に、本実施の形態では、画像撮影装
置間のキャリブレーション（画像撮影装置間の相対的位
置が確定していること）があらかじめなされている場合
に関してのみ、解説するものとする。

【０２７９】また、画像撮影装置としては２台の場合の
みを説明するが、２台を越える複数台の画像撮影装置へ
の拡張は容易である。

【０２８０】図１５は、本実施の形態による方式の概念
図を表したブロック図である。

【０２８１】すなわち、図１５に示すように、あらかじ
め相対的位置が確定した複数（但し、この例ではステレ
オカメラを利用する例として左右の２台）の画像撮影装
置２０３，２０４からの撮影画像データがデータ処理装
置であるコンピュータ２０５ヘ送出される。

【０２８２】コンピュータ２０５は、あらかじめオブジ
ェクト座標系における位置が確定しているコードマーカ
（２０２）位置を利用することにより、各画像撮影装置
２０３，２０４と対象物２０１あるいは一部の画像撮影
装置２０３または２０４と対象物２０１との間の３次元
位置関係を計測する。

【０２８３】本実施の形態においては、複数の画像撮影
装置を有するため、複数の画像撮影装置は、全撮影装置
は基準とするセンサ基準座標系に基づいてキャリブレー
ションされていると仮定する。

【０２８４】すわなち、各画像撮影装置ｊに関して、セ
ンサ基準座標系で規定される３次元点（ｘ_i ^s ，
ｙ_i ^s ，ｚ_i ^s ）が画像撮影装置ｊにおいて、画像位置
（ｕ_i ^j ，ｖ_i ^j ）で観測されたとすると、すでにキャ
リブレーションにより確定している斉次変換行列ｊＨｓ
を利用して、

【数２８】 と記述することができる。

【０２８５】ここに、α_M ^j ，α_M ^j ，ｕ₀ ^j ，ｖ₀ ^j
は、画像撮影装置ｊに関するカメラ内部パラメータであ
り、カメラキャリブレーションにより確定した値であ
る。

【０２８６】もし、オブジェクト座標系で規定される３
次元点（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）を考えると、その画
像撮影装置ｊでの位置（ｕ_i ^j ，ｖ_i ^j ）は、以下の式
により記述できる。

【０２８７】

【数２９】 そこで、本実施の形態で解決すべき問題は、上記の式で
表現されているオブジェクト座標系からセンサ基準座標
系への座標変換パラメータｓＨｍを如何にして推定する
かと言う問題である。

【０２８８】図１６は、本実施の形態の処理手順を表し
たフローチャートである。

【０２８９】本方式においては、左右画像を左右の画像
撮影装置から受け取り、各画像内からコードマーカを同
定し、その画像内位置を算出する（ステップＳ１１，Ｓ
１２，Ｓ１３）。

【０２９０】左右画像の両方から同定できたコードマー
カに関して、そのコードマーカのセンサ基準座標系での
３次元位置を左右画像内位置から算出する。

【０２９１】こうしてセンサ基準座標系で算出された３
次元位置座標とオブジェクト座標系で規定された３次元
位置座標とを利用することにより、オブジェクト座標系
からセンサ基準座標系への座標変換パラメータを算出す
る（ステップＳ１４）。

【０２９２】各画像からコードマーカを同定するステッ
プ（Ｓ１１，Ｓ１２，Ｓ１３）までは、第１の実施の形
態や第２の実施の形態の場合と同様であるので、ここで
はステップＳ１４において、左右の画像の両者から同定
されたコードマーカの画像内位置座標から、どのように
してオブジェクト座標系からセンサ基準座標系への座標
変換パラメータを算出するかを説明する。

【０２９３】いま、左画像から得られたコードマーカｉ
の画像内位置座標を（ｕ_i ¹ ，ｖ_i ¹ ）、右画像から得
られたコードマーカｉの画像内位置座標（ｕ_i ² ，ｖ_i
² ）とする。

【０２９４】このとき、センサ基準座標系での３次元推
定値は、以下のようにして算出することができる。

【０２９５】

【数３０】 を定義したのち、（ｕ_i ¹ ，ｖ_i ¹ ）と（ｕ_i ² ，ｖ_i
² ）を正規化し、

【数３１】 を算出する。すると、式（１００）から、

【数３２】 が得られる。ここで

【数３３】 とおけば、

【数３４】 として得られる。

【０２９６】この式により得られた３次元位置姿勢推定
値（ｘ_i ^s ，ｙ_i ^s ，ｚ_i ^s ）とオブジェクト座標系内
での３次元位置（ｘ_i ^m ，ｙ_i ^m ，ｚ_i ^m ）は、以下の
回転行列Ｒと並進べクトルｔにより関連づけられ、第１
の実施の形態または第２の実施の形態で説明したよう
な、ｑｕａｔｅｒｎｉｏｎ法によりその回転行列Ｒと並
進べクトルｔを求めることができる。

【０２９７】また、このステレオ法では、検出されるべ
きコードマーカの数は最低で３個あればよい。

【０２９８】これにより、オブジェクト座標系からセン
サ基準座標系への座標変換パラメータｓＨｍを算出する
ことが可能となる。ここに

【数３５】 が成立する。

【０２９９】本実施の形態によれば、互いにキャリブレ
ーションされた複数の画像撮影装置を利用することによ
り、コードマーカの数が少ない場合でも、安定した位置
姿勢センシング機能を実現することができる。

【０３００】本実施の形態は、単一の画像撮影装置を利
用したに比べて２倍のコードマーカを利用して位置姿勢
を推定することと等価となり、遮蔽などの影響で数少な
いマーカしか検出できない場合や、複雑な環境下で、画
像撮影装置が撮影する画像内にノイズ成分が多く含まれ
る場合には、特に有効な方法となる。

【０３０１】そして、上述したような第１乃至第７の実
施の形態で示した本発明の明細書には、特許請求の範囲
に示した請求項１乃至３以外に、以下のような付記１．
乃至３６．として示すような発明が含まれている。

【０３０２】付記１．撮像装置により撮像された、測定
対象物に対する３次元位置情報が既知の少なくとも３個
のマーカが写っている画像を入力する画像入力手段と、
上記画像上で上記各マーカに対応する領域を抽出する領
域抽出手段と、上記抽出した領域において、マーカの外
観上の特徴から個々のマーカを特定するマーカ特定手段
と、上記推定されたマーカの画像上での大きさから、マ
ーカまでの距離を推定する距離推定手段と、上記推定さ
れた各マーカまでの距離と、このマーカの上記画像上で
の位置と、このマーカの測定対象物に対する３次元位置
情報を用いて、上記撮像装置に対する測定対象物の３次
元位置姿勢を演算する位置姿勢演算手段と、を有するこ
とを特徴とする３次元位置姿勢センシング装置。

【０３０３】付記２．撮像装置により撮像された、測定
対象物に対する３次元位置情報が既知のマーカが写って
いる画像を入力する画像入力手段と、上記入力された画
像を縮小する画像縮小手段と、上記縮小された画像上で
上記各マーカに対応する領域を抽出する領域抽出手段
と、上記抽出された各マーカの上記画像上での位置と、
各マーカの測定対象物に対する３次元位置情報を用い
て、上記撮像装置に対する測定対象物の３次元位置姿勢
を演算する位置姿勢演算手段と、を有することを特徴と
する３次元位置姿勢センシング装置。

【０３０４】付記３．上記測定対象物の特徴部分を画像
より抽出する特徴部分抽出手段と、上記抽出した特徴部
分の画像上の位置より、上記演算された測定対象物の位
置姿勢を補正する手段をさらに含むことを特徴とする付
記１または付記２記載の３次元位置センシング装置。

【０３０５】付記４．画像を解析して、この画像を撮影
した撮影装置に対する測定対象物の位置姿勢を測定する
方法であり、撮像装置により撮像された、測定対象物に
対する３次元位置情報が既知の少なくとも３個のマーカ
が写っている画像を入力する画像入力工程と、上記画像
上で上記各マーカに対応する領域を抽出する領域抽出工
程と、上記抽出した領域において、マーカの外観上の特
徴から個々のマーカを特定するマーカ特定工程と、上記
特定された各マーカの上記画像上での位置と、各マーカ
の測定対象物に対する３次元位置情報を用いて、上記撮
像装置に対する測定対象物の３次元位置姿勢を演算する
位置姿勢演算工程と、を有することを特徴とする３次元
位置姿勢センシング方法。

【０３０６】付記５．画像を解祈して、この画像を撮影
した撮影装置に対する測定対象物の位置姿勢を測定する
方法であり、撮像装置により撮像された、測定対象物に
対する３次元位置情報が既知の少なくとも４個のマーカ
が写つている画像を入力する画像入力工程と、上記画像
上で上記各マーカに対応する領域を抽出する領域抽出工
程と、上記抽出した領域において、マーカの外観上の特
徴から個々のマーカを特定するマーカ特定工程と、上記
マーカ特定工程によつて特定されたマーカより３つのマ
ーカを選択するマーカ選択工程と、上記マーカ選択工程
によつて選択された３つのマーカの上記画像上での位置
と、各マーカの測定対象物に対する３次元位置情報を用
いて、上記撮像装置に対する測定対象物の３次元位置姿
勢を演算するためのパラメータ組を複数算出するパラメ
ータ組算出工程と、上記マーカ選択工程によって選択さ
れなかったマーカに対して、上記パラメータ組算出工程
によつて算出されたパラメータ組を適用評価することに
より、１つのパラメータ組を選択するパラメータ組選択
工程と、を有することを特徴とする３次元位置姿勢セン
シング方法。

【０３０７】付記６．前記パラメータ組選択工程によつ
て選択された１つのパラメータ組に関して、各マーカの
測定対象物に対する３次元位置情報を適用して評価する
ことにより、該パラメータ組を改善するパラメータ改善
工程をさらに含むことを特徴とする付記５記載の３次元
位置姿勢センシング方法。

【０３０８】付記７．画像を解析して、この画像を撮影
した撮影装置に対する測定対象物の位置姿勢を測定する
方法であり、撮像装置により撮像された、測定対象物に
対する３次元位置情報が既知の少なくとも３個のマーカ
が写っている画像を入力する画像入力工程と、上記画像
上で上記各マーカに対応する領域を抽出する領域抽出工
程と、上記抽出した領域において、マーカの外観上の特
徴から個々のマーカを特定するマーカ特定工程と、上記
特定されたマーカの両像上での大きさから、マーカまで
の距離を推定する距離推定工程と、上記推定された各マ
ーカまでの距離と、このマーカの上記画像上での位置
と、このマーカの測定対象物に対する３次元位置情報を
用いて、上記撮像装置に対する測定対象物の３次元位置
姿勢を演算する位置姿勢演算工程と、を有することを特
徴とする３次元位置姿勢センシング方法。

【０３０９】付記８．画像を解析して、この画像を撮影
した撮影装置に対する測定対象物の位置姿勢を測定する
方法であり、撮像装置により撮像された、測定対象物に
対する３次元位置情報が既知のマーカが写っている画像
を入力する画像入力工程と、上記入力された画像を縮小
する画像縮小工程と、上記縮小された画像上で上記各マ
ーカに対応する領域を抽出する領域抽出工程と、上記抽
出された各マーカの上記両像上での位置と、各マーカの
測定対象物に対する３次元位置情報を用いて、上記撮像
装置に対する測定対象物の３次元位置姿勢を演算する位
置姿勢演算工程と、を有することを特徴とする３次元位
置姿勢センシング方法。

【０３１０】付記９．上記測定対象物の特徴部分を画像
より抽出する特徴部分抽出工程と、上記抽出した特徴部
分の画像上の位置より、上記演算された測定対象物の位
置姿勢を補正する工程をさらに含むことを特徴とする付
記７または付記８記載の３次元位置姿勢センシング方
法。

【０３１１】付記１０．コンピュータによって画像を解
析して、この画像を撮影した撮影装置に対する測定対象
物の位置姿勢を測定するための処理プログラムを記録し
た記録媒体であり、上記処理プログラムはコンピュータ
に、撮像装置により撮像された、測定対象物に対する３
次元位置情報が既知の少なくとも３個のマーカが写って
いる画像を人力させ、上記画像上で上記各マーカに対応
する領域を抽出させ、上記抽出した領域において、マー
カの外観上の特徴から個々のマーカを特定させ、上記特
定された各マーカの上記画像上での位置と、各マーカの
測定対象物に対する３次元位置情報を用いて、上記撮像
装置に対する測定対象物の３次元位置姿勢を演算させ
る、ことを特徴とする３次元位置姿勢センシング処理プ
ログラムを記録した記録媒体。

【０３１２】付記１１．コンピュータによって画像を解
析して、この画像を撮影した撮影装置に対する測定対象
物の位置姿勢を測定するための処理プログラムを記録し
た記録媒体であり、上記処理プログラムはコンピュータ
に、撮像装置により撮像された、測定対象物に対する３
次元位置情報が既知の少なくとも４個のマーカが写って
いる両像を入力させ、上記画像上で上記各マーカに対応
する領域を抽出させ、上記抽出した領域において、マー
カの外観上の特徴から個々のマーカを特定させ、上記特
定されたマーカより３つのマーカを選択させ、上記選択
された３つのマーカの上記画像上での位置と、各マーカ
の測定対象物に対する３次元位置情報を用いて、上記撮
像装置に対する測定対象物の３次元位置姿勢を演算する
ためのパラメータ組を複数算出させ、上記選択されなか
ったマーカに対して、上記算出されたパラメータ組を適
用評価することにより、１つのパラメータ組を選択させ
る、ことを特徴とする３次元位置姿勢センシングブログ
ラムを記録した記録媒体。 付記１２．上記処理プログラムは、コンピュータにさら
に、前記選択された１つのパラメータ組に関して、各マ
ーカの測定対象物に対する３次元位置情報を適用して評
価することにより、該パラメータ組を改善させることを
特徴とする付記１１記載の３次元位置姿勢センシング処
理プログラムを記録した記録媒体。

【０３１３】付記１３．コンピュータによって画像を解
析して、この画像を撮影した撮影装置に対する測定対象
物の位置姿勢を測定するための処理プログラムを記録し
た記録媒体であり、上記処理プログラムはコンピュータ
に、撮像装置により撮像された、測定対象物に対する３
次元位置情報が既知の少なくとも３個のマーカが写って
いる画像を入力させ、上記画像上で上記各マーカに対応
する領域を抽出させ、上記抽出した領域において、マー
カの外観上の特徴から個々のマーカを特定させ、上記特
定されたマーカの画像上での大きさから、マーカまでの
距離を推定させ、上記推定された各マーカまでの距離
と、このマーカの上記画像上での位置と、このマーカの
測定対象物に対する３次元位置情報を用いて、上記撮像
装置に対する測定対象物の３次元位置姿勢を演算させ
る、ことを特徴とする３次元位置姿勢センシング処理プ
ログラムを記録した記録媒体。

【０３１４】付記１４．コンピュータによつて画像を解
析して、この画像を撮影した撮影装置に対する測定対象
物の位置姿勢を測定するための処理プログラムを記録し
た記録媒体であり、上記処理プログラムはコンピュータ
に、撮像装置により撮像された、測定対象物に対する３
次元位置情報が既知のマーカが写っている画像を入力さ
せ、上記入力された画像を縮小させ、上記縮小された画
像上で上記各マーカに対応する領域を抽出させ、上記抽
出された各マーカの上記画像上での位置と、各マーカの
測定対象物に対する３次元位置情報を用いて、上記撮像
装置に対する測定対象物の３次元位置姿勢を演算させ
る、ことを特徴とする３次元位置姿勢センシング処理プ
ログラムを記録した記録媒体。

【０３１５】付記１５．上記処理プログラムは、コンピ
ュータにさらに、上記測定対象物の特徴部分を画像より
抽出させ、上記抽出した特徴部分の画像上の位置より、
上記演算された測定対象物の位置姿勢を補正させること
を特徴とする付記１３または付記１４記載の３次元位置
センシング処理プログラムを記録レた記録媒体。

【０３１６】付記１６．表面に識別マークが配されたマ
ーカであり、上記識別マークの外形が円形であることを
特徴とするマーカ。

【０３１７】（効果）この付記１６．のマーカによる
と、画像から識別マークを抽出する場合に、識別マーク
の外形が円形であるので、その抽出処理が容易である。

【０３１８】すなわち、識別マークの外形が四角とか三
角ではマーク部分の向きによっては画像上の図形の形状
が大幅に異なり認識が困難であるのに対し、識別マーク
の外形が円形であるならマーク部分の向きによっても楕
円になるだけであり、画像上の認識処理が容易となる。

【０３１９】付記１７．表面に識別マークが配されたマ
ーカであり、上記識別マークが、外形が円形である背景
部と、この背景部の内部に配設された複数の所定パター
ンを有し、上記複数の所定パターンの着色の組み合わせ
で、マーカを特定可能であることを特徴とするマーカ。

【０３２０】（効果）この付記１７．のマーカによる
と、付記１６．のマーカによる上述の効果に加えて、領
域内の輝度や色度を分析してマーカを特定可能であると
いう効果を有している。

【０３２１】付記１８．表面に識別マークが配されたマ
ーカであり、上記識別マークが外形が円形である背景部
と、この背景部の中心に配された中心印と、上記中心印
を囲む同心円上に等間隔で配された複数の周辺印とを有
し、上記中心印と周辺印の着色の組み合わせで、マーカ
を特定可能であることを特徴とするマーカ。

【０３２２】（効果）この付記１８．のマーカによる
と、付記１６．のマーカによる上述の効果に加えて、領
域内の輝度や色度を分析してマーカを特定可能であると
いう効果を有している。

【０３２３】付記１９．表面に識別マークが配されたマ
ーカであり、上記識別マークが、外形が円形である背景
部と、この背景部の内部に配された半径の異なる複数の
同心円とを有し、上記同心円の間の領域の着色の組み合
わせで、マーカを特定可能であることを特徴とするマー
カ。

【０３２４】付記２０．位置測定に供されるプローブで
あり、測定対象に当接する部位である当接部と、該プロ
ーブを識別する識別マークが表面に配されたマーク部
と、を具備することを特徴とするプローブ。

【０３２５】付記２１．対象物あるいは対象物付近に装
着された３次元位置が既知な複数のマーカを、画像撮影
装置を利用して撮影しマーカの画像内位置を計測するこ
とにより、対象物との相対的３次元位置姿勢を計測する
装置において、複数のマーカに対応する領域を前記マー
カの画像内から抽出する手段と、該マーカに対応する領
域の画像内での幾何学的特徴を算出することにより、個
々の該マーカを同定する手段と、該マーカの３次元位置
に基づいて、対象物と画像撮影装置間の相対的３次元位
置姿勢を推定する手段と、を具備したことを特徴とする
３次元位置姿勢センシング装置。

【０３２６】付記２２．個々のマーカ内にはコード化さ
れたパターンが配されることを特徴とする付記２１記載
の３次元位置姿勢センシング装置。

【０３２７】付記２３．前記マーカ領域の画像内での幾
何学的特徴を算出し該マーカを同定する手段は、画像内
のマーカ領域の大きさを計測し、前記対象物と画像撮影
装置間の相対的３次元位置姿勢を推定する手段は、該マ
ーカ領域の大きさの計測値に基づいて画像撮影装置とマ
ーカ間の距離の初期値を算出し、該初期値に基づいて対
象物と画像撮影装置間の相対的３次元位置姿勢を算出す
ることを特徴とする付記２１記載の３次元位置姿勢セン
シング装置。

【０３２８】付記２４．前記画像撮影装置は他の装置に
装着されており、画像撮影装置と該他の装置間の３次元
位置姿勢関係が既知あるいは別途計測可能てあるとき、
上記対象物と画像撮影装置間の相対的な３次元位置姿勢
を利用することにより、対象物と該他の装置間の３次元
位置姿勢を計測することを特徴とする付記２１記載の３
次元位置姿勢センシング装置。

【０３２９】付記２５．前記相対的距離推定値が実現可
能である範囲をとるマーカのみを利用して推定値を算出
することを特徴とする付記２３載の３次元位置姿勢セン
シング装置。

【０３３０】付記２６．前記推定された相対的距離に推
定誤差分散を算出し、その推定値と誤差分散の両方を考
慮しながら、対象物と画像撮影装置の３次元位置姿勢を
推定することを特徴とする付記２３記載の３次元位置姿
勢センシング装置。

【０３３１】付記２７．前記マーカとしては、円形のマ
ーカであり、そのマーカの画像内での投影像を楕円とし
て近似し、楕円の長軸の長さを利用して、画像撮影装置
からマーカまでの距離を推定することを特徴とする付記
２３記載の３次元位置姿勢センシング装置。

【０３３２】付記２８．前記マーカとしては、円形のカ
ラーマーカを利用することを特徴とする付記２７．記載
の３次元位置姿勢センシング装置。

【０３３３】付記２９．前記マーカの形状として円形を
利用し、マーカ内のコードとして、小円の色の異なるパ
ターンを利用することを特徴とする付記２２記載の３次
元位置姿勢センシング装置。

【０３３４】付記３０．前記マーカとしては、同心円上
に色の異なるパターンを生成することを特徴とする付記
２２記載の３次元位置姿勢センシング装置。

【０３３５】付記３１．前記円形マーカの方向情報を付
加して３次元位置センシングを行うことを特徴とする付
記２７記載の３次元位置姿勢センシング装置。

【０３３６】付記３２．原画像からマーカ領域を抽出す
る際、原画像の縮小画像を作成し、該縮小画像内からマ
ーカに対応すると想定される領域の候補を抽出し、該候
補領域が原画像内で対応する領域を算出したのち、原画
像内の領域でマーカに対応する領域を抽出・認識するこ
とを特徴とする付記２１記載の３次元位置姿勢センシン
グ装置。

【０３３７】付記３３．固有の幾何学的特徴を有するマ
ーカが認識されたのち、そのマーカの２次元または３次
元位置関係を利用して、固有の幾何学的特徴を持たない
別マーカを画像内から抽出し、該別マーカの画像内位置
と３次元位置を利用して、対象物と画像撮影装置間の３
次元位置姿勢パラメータを更新することを特徴とする付
記２１記載の３次元位置姿勢センシング装置。

【０３３８】付記３４．前記画像撮影装置は、複数の画
像を撮影し、該複数の画像を利用することを特徴とする
付記２１記載の３次元位置姿勢センシング装置。

【０３３９】付記３５．マーカ群がセンサプローブの表
面に装着され、該センサプローブを対象物として取り扱
うことで、センサプローブの相対的位置姿勢を推定する
とともに、センサプローブのプローブ先端の位置を計測
することを特徴とする付記２１記載の３次元位置姿勢セ
ンシング装置。

【０３４０】付記３６．マーカ形状が正多角形であるこ
とを特徴とする付記２３記載の３次元位置姿勢センシン
グ装置。

【０３４１】

【発明の効果】本発明は、以上のような方式を採用する
ことにより、従来の方法では困難であった遮蔽などの影
響を受けずに、ロバストに（安定に）対象物の３次元位
置姿勢を推定することができる。

【０３４２】それゆえ、本発明の３次元位置姿勢センシ
ング装置は、画像撮影装置または他の装置を規定する座
標系における対象物の３次元位置を推定することによ
り、ロボットによる対象物の把持や、対象物の検査など
に有効的に利用することができる。

【０３４３】従って、以上説明したように、本発明によ
れば、（１）遮蔽などの影響により、マーカ群の一部が
観察されないときでも、対象物の３次元位置姿勢を推定
することができるようにするとともに、（２）従来のｎ
点問題では、解を確定することができなかった３個のマ
ーカからだけでも位置姿勢を推定することができるよう
にした、３次元位置姿勢センシング装置を提供すること
が可能となる。

【図面の簡単な説明】

【図１】図１は、第１の実施の形態による３次元位置姿
勢センシング装置の構成を示すブロック図である。

【図２】図２は、図１の画像撮影装置３と、カメラ画像
面と、対象物１が規定するオブジェクト座標系との関係
を表した図である。

【図３】図３は、第１の実施の形態における幾何学的特
徴を持ったコードマーカ２群の一例を表した図である。

【図４】図４は、第１の実施の形態における別のコード
パターンを表した図である。

【図５】図５は、第１の実施の形態における別のコード
パターンを表した図である。

【図６】図６は、第１の実施の形態において、対象物１
の３次元位置姿勢を推定するまでの処理手順を表したフ
ローチャートである。

【図７】図７は、第１の実施の形態において、コードパ
ターンの抽出処理過程を表した図である。

【図８】図８は、第１の実施の形態において、得られた
３個のマーカＭｉに関して想定される３個の３角形Δ０
_c ＭｉＭｊを示す図である。

【図９】図９は、第２の実施の形態におけるステップ２
での処理の手順を示すフローチャートである。

【図１０】図１０は、第２実施の形態において、マーカ
の中心をＰ_i 、カメラの焦点を０_c とするとともに、カ
メラ像面と０_c Ｐ_i の交点をＱ_i とすると、Ｑ_i はマー
カ像の中心であると判断することを示した図である。

【図１１】図１１は、第４の実施の形態において、画像
内からランドマークを抽出する一例を示した図である。

【図１２】図１２は、第５の実施の形態で想定される構
成を表したブロック図である。

【図１３】図１３は、第６の実施の形態による構成を表
すブロック図である。

【図１４】図１４は、第６の実施の形態によるセンサプ
ローブ１３８の例を表した図である。

【図１５】図１５は、第７の実施の形態の概念図を表し
たブロック図である。

【図１６】図１６は、第７の実施の形態の処理手順を表
したフローチャートである。

【符号の説明】

１，１２１，２０１，…対象物、 ２，１２２，２０２…マーカ ３，１２３，１３３，２０３，２０４…画像撮影装置、 ４，１２４，１３４，２０５…コンピュータ、 １２５，１３６…基準座標系を規定する装置、 １３８…センサプローブ、 １３９…探針される物体Ｘ。

フロントページの続き (72)発明者 柴▲崎▼ 隆男 東京都渋谷区幡ヶ谷２丁目43番２号 オリ ンパス光学工業株式会社内 (72)発明者 浅野 武夫 東京都渋谷区幡ヶ谷２丁目43番２号 オリ ンパス光学工業株式会社内 (72)発明者 松崎 弘 東京都渋谷区幡ヶ谷２丁目43番２号 オリ ンパス光学工業株式会社内 (72)発明者 古橋 幸人 東京都渋谷区幡ヶ谷２丁目43番２号 オリ ンパス光学工業株式会社内 Ｆターム(参考） 2F065 AA04 AA37 AA54 AA58 DD03 DD04 DD08 FF04 JJ03 JJ16 JJ23 JJ26 LL30 QQ41 QQ42 3F059 DA02 DB03 DB09 FA05 FB12

Claims (3)

【特許請求の範囲】
1. 【請求項１】 撮像装置により撮像された、測定対象物
   に対する３次元位置情報が既知の少なくとも３個のマー
   カが写っている画像を入力する画像入力手段と、 上記画像上で上記各マーカに対応する領域を抽出する領
   域抽出手段と、 上記抽出した領域において、マーカの外観上の特徴から
   個々のマーカを特定するマーカ特定手段と、 上記特定された各マーカの上記画像上での位置と、各マ
   ーカの測定対象物に対する３次元位置情報を用いて、上
   記撮像装置に対する測定対象物の３次元位置姿勢を演算
   する位置姿勢演算手段と、 を有することを特徴とする３次元位置姿勢センシング装
   置。
2. 【請求項２】 撮像装置により撮像された、測定対象物
   に対する３次元位置情報が既知の少なくとも４個のマー
   カが写っている画像を入力する画像入力手段と、 上記画像上で上記各マーカに対応する領域を抽出する領
   域抽出手段と、 上記抽出した領域において、マーカの外観上の特徴から
   個々のマーカを特定するマーカ特定手段と、 上記マーカ特定手段によって特定されたマーカより３つ
   のマーカを選択するマーカ選択手段と、 上記マーカ選択手段によつて選択された３つのマーカの
   上記画像上での位置と、各マーカの測定対象物に対する
   ３次元位置情報を用いて、上記撮像装置に対する測定対
   象物の３次元位置姿勢を演算するためのパラメータ組を
   複数算出するパラメータ組算出手段と、 上記マーカ選択手段によって選択されなかつたマーカに
   対して、上記パラメータ組算出手段によつて算出された
   パラメータ組を適用評価することにより、１つのパラメ
   ータ組を選択するパラメータ組選択手段と、 を有することを特徴とする３次元位置姿勢センシング装
   置。
3. 【請求項３】 前記パラメータ組選択手段によつて選択
   された１つのパラメータ組に関して、各マーカの測定対
   象物に対する３次元位置情報を適用して評価することに
   より、該パラメータ組を改善するパラメータ改善手段を
   さらに含むことを特徴とする請求項２記載の３次元位置
   姿勢センシング装置。