JPH0454409A - 物体の姿勢推定方法およびその装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 〔発明の目的〕 （産業上の利用分野） 本発明は、物体の姿勢推定方法及びその装置に関し、詳
しくは、カメラから２次元画像として入力される３次元
物体について、それの３次元空間内での姿勢を推定する
方法及びその装置１こ関する。

（従来の技術） 従来、２次元画像を用いた三次元情報抽出法としては、
複数のカメラ系を用いるステレ第３次元復元法、制御可
能なスリット光などの光源とカメラ系を用いる能動的距
離測定法、物体上の４点以上の点の画像との対応付けを
用いる四面体配置推定法（特６Ｂ−５８９４０）　、お
よび物体上の３線分以上および２点以上の画像との対応
付けを用いる三線付二点配置推定法（特６．３−２０１
５７７）などが知られている。

（発明が解決しようとする課題） 従来技術のうちステレ第３次元復元法および能動的距離
測定法は、いずれも三角測量に基づくもので、ステレ第
３次元復元法では画像間の点の対応付けに時間がかかり
、また十分な精度が得にくいという欠点があった。一方
、能動的距離測定法では光源などの付帯装置が不可欠で
ある。点の対応付けに時間がかかるという欠点があった
。また、両者に共通の問題点としては、誤った点の対応
付けが起こった場合、これを検出する能力を持ち合わせ
ていないという欠点があった。

一方、四面体配置推定法はこれらの欠点を除去するため
、１枚の入力画像と予め記述格納された物体モデルのみ
から３次元姿勢を高速に推定することを目的として、１
枚の画像入力から得られる４点の２次元座標値と予め格
納された物体モデル中の４点の３次元相対位置を用いて
、物体の３次元姿勢を推定する方法であるが、物体モデ
ル中の４点以上の点を安定的に抽出できないような物体
モデルに対してはこの方法を適用できないという欠点が
あった。また、三線付二点配置推定法は３本の有向線分
と２点の抽出により３次元姿勢を行える点で、四面体配
置推定法では取り扱えない物体をも対象とできるが、複
数の剛体が回転自由度を持つ関節で結合されて構成され
るような柔構造物体において個々の剛体側に姿勢推定を
行なえない場合には、これらの方法を適用できないとい
う問題があり、また有向線分に関して３次元方向のみを
用いていたため、線分の通過点あるいは端点等が固定さ
れている場合には、それらの情報が利用されないことに
起因して余分の対応付けを必要とするという問題点があ
った。

本発明の目的は、上記に鑑みてなされたもので、ソノ目
的とするところは、カメラ入力画像に対応する任意形状
の物体の３次元空間内での姿勢を高速かつ高精度に推定
する方法及び装置を提供することにある。

〔発明の構成〕

（課題を解決するための手段） 上記目的を達成するため、本発明の物体の姿勢推定方法
は、撮像手段で撮像した２次元画像に対応する物体の３
次元姿勢を推定する物体の姿勢推定方法であって、単一
または複数の剛体および該剛体を縦続接続するための軸
周りの回転自由度を有する関節から構成される物体の定
義、該定義により予め指定された物体中の各剛体内の有
向線分について剛体内で任意に定義される座標系での各
有向線分の３次元空間内の方向ベクトル値と線分の特徴
、各関節の回転軸に対応する有向線分について関節によ
って接続される２つの剛体およびそれぞれに固有の前記
座標系での方向ベクトル値および該有向線分と同−ａ線
上にある１点の３次元座標値、および少なくとも１個以
上の剛体中の少なくとも２点以上の点について各剛体に
固有の前記座標系での各点の３次元座標値と特徴を記述
した物体モデル記述を用意し、前記２次元画像の特徴と
物体モデル記述との対応付けを行って、所定の条件で規
定される線分の組合せおよび２点を選択し、該線分の組
合せおよび２点に対応する前記物体モデル記述中の有向
線分および２点の姿勢を推定して、前記２次元画像に対
応する物体の姿勢候補を推定し、この各候補について前
記物体モデル記述中の各有向線分と各点の像を画像と照
合し、物体の姿勢を決定することを要旨とする。

また、本発明の物体の姿勢推定装置は、物体の２次元画
像を撮像する撮像手段と、該撮像手段で撮像した画像か
ら特徴を抽出する抽出手段と、単一または複数の剛体と
該剛体を縦続接続するための軸周りの回転自由度を有す
る関節から構成される対象物体の接続構成、対象物体中
の各剛体内の有向線分について剛体内で任意に定義され
る座標系での各有向線分の３次元空間内の方向ベクトル
値と線分の特徴、各関節の回転軸に対応する有向線分に
ついて関節によって接続される２つの剛体に固有の前記
座標系での方向ベクトル値および該有向線分と同一直線
上にある１点の３次元座標値、および剛体中の少なくと
も２点以上の点について各剛体に固有の前記座標系での
各点の３次元座標値と特徴を記述した物体モデル記述を
格納する格納手段と、前記２次元画像の特徴と物体モデ
ル記述中の特徴記述の対応付けを行って、対応付けのと
れたものの中から所定の条件で規定される線分の組合せ
および２点を選択する選択手段と、前記選択された線分
集合中の線分および２点の３次元空間での考え得るすべ
ての姿勢候補を求める手段と、前記水められた各姿勢候
補について前記物体モデル記述中の各有向線分と各点の
像を画像と照合して物体の姿勢を決定する決定手段とを
有することを要旨とする。

（作用） 本発明の物体の姿勢推定方法およびその装置では、撮像
手段で撮像した物体の２次元画像の特徴線分および特徴
点と予め用意しである対象物体の物体モデル線分記述お
よび物体モデル点記述の特徴との各対応付けを行い、そ
の整合性から所定の条件で規定する線分集合（指定線分
集合）および２点く指定点対）を選択し、この選択した
指定線分集合および指定点対から物体の３次元空間で考
え得るすべての姿勢候補を求め、これから物体モデル線
分記述中の線分および物体モデル点記述中の点の対応付
けと両立しない姿勢候補を逐次削除して最終的に各剛体
中の線分・点集合の姿勢を推定し、対象とする物体の３
次元空間上での姿勢を推定している。

（実施例） 以下、図面を用いて本発明の詳細な説明する。

第１図は本発明の一実施例のブロック図であり、１は人
力画像特徴抽出装置、２は物体モデル記述格納装置、３
は画像・物体対応付は装置、４は指定線分集合・点対姿
勢推定装置、５は線分集合姿勢決定装置を示す。

入力画像特徴抽出装置１は、焦点距離やレンズ中心が既
知である１台のカメラ（図示せず）から対象とする３次
元物体の２次元画像を入力し、次に示す指定線分集合と
指定点対に対する条件で規定する組合せ以上の特徴線分
と少なくとも２点以上の特徴点を抽出する。

指定線分集合および指定点対に対する条件に（１いて説
明する。

指定線分集合とは、以下に示す（１）から（３）の条件
を満たす線分集合のことである。

（１）両端の剛体について、剛体モデル線分記述（後述
する第３図（ｃ）の２２）の中のそれぞれ２本の線分（
但し、物体が単一剛体からなる時には３本の線分）であ
る。しかしながら、剛体モデル点記述（後述する第３図
（ｅ）の２４）の中の点対を後述する線分種別３の線分
として用いることができる。この場合、剛体モデル線分
記述２２には対応する線分種別３の線分に関する記述を
付加するものとする。

（２）両端以外の剛体について、剛体モデル線分記述２
２中のそれぞれ１本の線分、ただし、剛体モデル点記述
２４中の点対を線分種別３の線分の代用として用いるこ
とができる。この場合、剛体モデル線分記述２２には対
応する線分種別３の線分に関する記述を付加するものと
する。

（３）上記の（１）項および（２）項で指定される線分
には、少なくとも１本以上の線分種別３の線分が含まれ
ている。

指定点対は次のように定義される。

（４）上記（１）項から（３）項で指定される線分のう
ち１本の線分種別３の線分の両端点を指定点対と称する
。

物体モデル記述格納装置２には、あらがしめ対象とする
物体を構成する剛体および関節の接続関係を示す物体構
成記述と、その物体中の各剛体内の少なくとも１個以上
（ただし、両端では２個以上単一の剛体である場合には
３個以上）の有向線分について、剛体内で定義される任
意の座標系で得られた３次元方向表示とその線分の特徴
（例えば光学的特徴）および線分と共線位置にある一点
の３次元座標値とを記述した剛体モデル線分記述と、各
関節の回転軸に対応する各１個の有向線分について、関
節により接続される２つの剛体で定義された前述座標系
で得られた３次元方向表示を記述した関節記述と、少な
くとも１個以上の剛体中の（前述の線分との重複を許す
）少なくとも２点以上の点の剛体内で定義された座標系
で得られた３次元座標値とその点の特徴（例えば光学的
特徴）を記述した剛体モデル点記述が格納されている。

画像・物体対応付は装置３は、入力画像特徴抽出装置１
で抽出されたカメラ入力画像の特徴線分および特徴点の
特徴と物体モデル記述格納装置２に格納されている剛体
モデル線分記述および剛体モデル点記述中の特徴記述と
の対応付けを行い、対応付けの取れたものの中から前記
条件で規定される指定線分集合および指定点対を選択し
、該指定線分集合および指定点対よりなる指定線分・点
対対応材は表を生成する。指定線分集合・点対姿勢推定
装置４では、画像・物体対応付は装置３により選択され
た指定線分集合および指定点対について、それの３次元
空間内で考えうるすべての姿勢候補を推定する。これは
、指定線分集合・指定点対および各関節の回転軸につい
て三次元回転の候補を求め、次に指定点対の対応付けか
ら１つの剛体の三次元平行移動を求め、次に隣接する剛
体の三次元平行移動を順次水めることにより、各剛体の
三次元姿勢を推定するという順序で、各剛体について３
次元空間内での姿勢候補をすべて生成する。線分集合姿
勢決定装置５では、指定線分集合・点対姿勢推定装置４
により生成された各剛体の姿勢候補から物体モデル記述
格納装置２に含まれる各剛体中の線分集合および点集合
の像を推定し、これと画像の照合をとることにより、姿
勢候補の中から妥当な姿勢を選択する。各剛体中の線分
集合および点集合の姿勢が推定できれば、対象とするカ
メラ被写体物体の姿勢が推定できたことになる。

第２図（ａ）は前記入力画像特徴抽出装置に２次元画像
を入力するカメラ系を示す。カメラの中心点を原点０１
光軸を２軸とする直交座標系Ｘ−Ｙ−Ｚを考え、Ｚ−ｆ
　（ｆはカメラの焦点距離で既知とする）に画像面１１
を考え、該画像面１１とＺ軸の交点０゛　も既知である
ものとする。また、画像は中心投影法により生成される
ものとする。

即ち、３次元空間内の点Ｐ　（Ｘ、Ｙ、Ｚ）とその像で
ある点ｐ　（ｕ、ｖ）は、 （ｕ　、　　ｖ　）　＝　（Ｘ　ｆ　／　Ｚ　、　Ｙ　
ｆ　／　Ｚ　）なる関係をみたすものとする。入力画像
特徴抽出装置１は、両像面１１の画像を入力し、被写体
の点特徴（２直線の交点、色彩、記載された記号など）
を記述した第２図（ｂ）に示す特徴点記述および、被写
体の線分特徴（色彩、線分の両側領域の色彩、線分の両
端の光学的特徴および幾何的特徴など）を記述した第２
図（ｃ）に示す特徴線分記述を抽出する。ここで、特徴
点ｐ＋（ｉ−１゜２、・・・）の座標は（ｕｌ、ｖｌ）
であり、有向線分１＋　　（ｔ＝１．２．・・・）の始
点ｐ＋１および終点ｐ＋２の座標は（ｕ＋１．ｖｚ）（
ｊ−１，２）である。

第３図（ａ）は対象とする３次元物体２０の一例を示し
、同図（ｂ）、（Ｃ）、（ｄ）および（ｅ）はそれぞれ
該物体に対応する物体モデル記述格納装置２内の物体縦
続接続記述２１、剛体モデル線分記述２２、関節記述２
３および剛体モデル点記述２４の例を示したものである
。即ち、物体モデル記述格納装置２内の物体縦続接続記
述２１は、対象とする物体２０に含まれる両端が剛体で
あり、かつ剛体と関節を交互に並べた順列であり、各関
節が前後の剛体を接続することを示している。また、各
剛体Ｂ、には、その剛体に固有の座標系Ｓ、が定義され
ている。物体モデル記述格納装置２内の剛体モデル線分
記述２２は、対象とする物体２０に含まれる各剛体Ｂ、
内の１本以上（ただし、両端は２本以上、単一の剛体物
体では３本以上）の有向線分り、１〜Ｌ　ａｎについて
の座標系Ｓ、における３次元方向（ｄ　Ｘ　＠１１　ｄ
　Ｙ　５１１ｄ　ｚａ＋）　〜（ｄＸ＊ｓ＋　　ｄｙｓ
ｅ、　　ｄ　Ｚ＊ａ）と、その有向線分を特徴付ける線
分および周辺領域の光学的特徴（色彩・符号など）と、
線分種別０〜３（０は座標系Ｓ、内で位置が３次元空間
内で不定であり、１は線分の位置が共線位置にある一点
（基準点）により制約され、２は線分の１端点のみが固
定であり、３は線分の両端点が固定である）と、線分種
別１のものについては基準点の３次元座標、また線分種
別２のものについては端点の３次元座標、また線分種別
３のものについては両端点の３次元座標との組合せから
なる線分属性記述の集合としてあたえられる。また、物
体モデル記述格納装置２内の関節記述２３は、対象とす
る物体２０に含まれる各関節Ｊ、について物体縦続接続
記述２１により示される隣接する剛体Ｂ＠−１＋Ｂ、に
固有の座標系Ｓ、（ａ’−ａ−１，ａ）中での回転軸Ａ
、（線分種別１の有向線分）の基準点の３次元座標値（
ｘ　ａ・１．ｙ、・１．ｚ、・１）と３次元方向（ｄ　
ｘ　、＋、　　ｄ　ｙ　＠’Ｉ　　ｄ　ｚ　ａｉを示す
関節記述の集合として与えられる。また、物体モデル記
述格納装置２内の剛体モデル点記述２４は、対象とする
物体２０に含まれる各剛体Ｂ、上の２点以上の点Ｐ１〜
Ｐ１．についての剛体中で定義される任意の座標系Ｓ、
における３次元座標値（Ｘ　ａｌ＋　３’　ｓｉｎ　Ｚ
　ｓｌ）　〜（Ｘ＠＠ｌ　３’６＠ｌ　Ｚ　ａｓ）とそ
の点を特徴付ける光学的特徴（色彩・符号など）のベア
からなる魚屑性記述の集合としてあたえられる。

画像・物体対応付は装置３では、入力画像特徴抽出装置
１の特徴点出力および特徴線分出力と物体モデル記述格
納装置２の各線分属性記述および各魚屑性記述中の光学
的特徴記述の対応付けを行い、対応付けのとれたものの
中から指定線分集合および指定点対を選択して指定線分
対応付は表および指定点対対応付は表を生成する。第４
図（ａ）（ｂ）はこの指定線分対応付は表および指定点
対対応付は表の一例を符号３１．３２を付してそれぞれ
示す。指定線分集合および指定点対は対応付けの取れた
ものの中からランダムに選択する。

この選択した指定線分集合および指定点対の姿勢を推定
した結果、許容される姿勢が存在しない場合は、対応付
けの取れたものの中から前記条件で規定される指定線分
集合および指定点対を選択して再び姿勢推定する。また
、指定線分集合および指定点対の選択をランダムにする
のではなく、線分種別３．２あるいは１の線分の優先選
択、あるいは点間の距離、３線分に対応する単位ベクト
ルのスカラー３重積などにより定義される評価関数によ
る指定線分集合および指定点対の選択など、バリエーシ
ョンは各種考えられ、これにより対応付けの効率を上げ
ることができる。

指定線分集合・点対姿勢推定装Ｗ４は、画像・物体対応
付は装置３の出力としてえられる前記指定線分対応付は
表３１で示された各指定線分および、前記指定点対対応
付は表３２で示された点対の３次元空間内での考えうる
全ての姿勢候補を推定する。第４図の指定線分対応付は
表３１の指定線分集合Ｌ　＃ｌ＋　　Ｌ　ａ２＋　　Ｌ
　１２＋　　Ｌ　２２＋　Ｌ２３および指定点対対応付
は表３２の２点Ｐｌ　ｌ＋　ＰＩ　２を第３図（ａ）の
物体２０中に矢印付き太線および白丸で示す。

前記指定線分集合・点対姿勢推定装置４は、第５図に示
すように、形状パラメータ算出部４１、縦続剛体回転推
定部４２、基準剛体姿勢推定部４３、剛体姿勢推定部４
４および剛体姿勢候補記憶装置４５により構成される。

形状パラメータ算出部４１は、前述した第３図に示す物
体縦続接続記述２１、剛体モデル線分記述２２、関節記
述２３および剛体モデル点記述２４および第４図に示す
指定線分対応付は表３１および指定点対対応付は表３２
を入力とし、縦続剛体回転推定部４２で用いる各種の形
状パラメータを算出する。ここで、物体縦続接続記述２
１中の最初の剛体を注目剛体Ｂ。、最後の剛体を検定剛
体Ｂ　＋　Ｓまた指定点対を含む剛体を基準剛体Ｂ。

と呼ぶことにし、物体縦続接続記述２１中の第ｉ番目ｃ
ｉ−ｏ、　　　・・ｔ）の剛体モデルを８１であられす
。このとき、形状パラメータ算出部４ユは、指定線分対
応付は表３１中の各線分のうち剛体Ｂ１中の各線分Ｌｌ
、と、指定点対対応付は表３２中の基準剛体Ｂ、中の２
点Ｐｚｉ　　（ｋ−１，２）によって決定される有向線
分Ｌｚ（−Ｐｚ＋Ｐ、＋２）について各線分Ｌ＋１の３
次元空間中の方位ベクトルに対応する単位ベクトルＥ　
＋　、を剛体モデル線分記述２２および剛体モデル点記
述２４から求め、また剛体Ｂ、と隣接する各関節の回転
軸の方位ベクトルＥ　＋　１を関節記述２３から求め、
これをもとに各剛体中の３つの単位ベクトルＥｅｌＥ、
、Ｅｃの配置を規定する３つの角度α、ｂ。

αｓｅｔ　　αｂｅ（ただし、Ｅ、・Ｅｂ−ｃｏｓαａ
ｈ）、およびスカラー３重積γ、ｂｔ　　（−（Ｅ、　
ｘＥｂ　）・Ｅｔ）を求める。ここで、Ｅ−＋　（ｉ　
−１、２）は注目剛体中の有向線分に対応し、Ｅ、２は
第ｊ番目の剛体中の有向線分、さらにＥｌｌ（ｉ−２，
３）は検定剛体中の有向線分に対応するものとする。

一方、Ｅｌ３およびＥｌｌ１．Ｉは、第ｊ番目の剛体と
第（Ｅｌ１）番目の剛体を結ぶ関節の回転軸に対応する
単位ベクトルの座標系Ｓ、および座標系５１４１での表
示とする。また、物体中の線分りの像ＩＩ、について、
原点Ｏから線分像の始点方向および終点方向への単位ベ
クトル（画像位置ベクトル）　ｐｉｌｌ　＋　　ｐ＋＋
２とで形成される平面（解釈平面）の単位法線ベクトル
（解釈ベクトル）Ｉｚ＝ｐｚｉ　Ｘｐｚｚ　／　ｌ　ｐ
ｉｌｌ　ＸｐＨ２１を計算により求める。また、各線分
Ｌ＋１に対して、解釈平面上の存在域を規定する単位ベ
クトル（存在域ベクトル）　Ｑ　＋Ｂ−１＋＋Ｘ　ｐ　
ｚ＊を求める。

縦続剛体回転推定部４２では、形状パラメータ算出部４
１の出力として得られる角度α、１、スカラー３重積γ
、、３、解釈ベクトル１１、存在域ベクトルＱ　Ｉｌｋ
から指定線分、指定点対および関節の回転軸の取りうる
勾配をすべて求める。これは、下記の連立方程式をすべ
て満足するベクトルＥ１、即ちＥｌ、に回転マトリクス
Ｒ，を施した実空間での配置（Ｅ、、−Ｅ、、Ｒ，）お
よび回転マトリクスＲ，をすべで出力することに相当す
る。

Ｅｌｌ・Ｅ、−１ （ｉ−０，・・・ｔ　　ｊ−１，２，３）ＥＢ”　Ｅ＋
＊−ＣＯＳ　（Ｅｌ６ （ｉ−０，・ｔ　　ｊ、に−１，２，３）（Ｅ　＋ｔＸ
　Ｅ　１２） Ｅ　＋３＝　Ｅ　＋＋＋、　ｌ ｔｏ、争　Ｅ　、、−０ Ｅｌ２・　Ｅｌ２−Ｏ Ｉ　　ｌ）・　Ｅｏ−Ｏ Ｑ　ｅｌｋ　　”　　Ｅ　ａ、＞　Ｑ （ｊ−１，２ Ｑ　Ｂｉ　　”　　Ｅ　＋２＞　Ｑ （ｉ−１，・　φ　争　ｔ−１ Ｑ　＋１ｉ　　”　　Ｅ　＋、＞　０ （ｊ−２，３ｋ−１，２） 第６図は、前記縦続剛体回転推定部４２の構成例を示し
、方程式解導出部４２１および縦続剛体回転候補計算部
４２２からなり、形状パラメータ算出部４１で計算され
るαｓｂ１　　γｍｂｃ１１１１およびＱ、ｋを人力と
し、上記連立方程式あるいは同様の連立方程式を満たす
Ｅｌｌの絹をすべて出力する構成を示している。即ち、
方程式解導出部４２１は、与えられた入力（α、５．γ
、、。＋　　１．およびＱ、、）のうちＱ０１１以外の
Ｑ、、、を除く情報ＩＩＥＩ３１１Ｉ″　γ　−１，１
２，１３（ｉ−０，・　・　・　ｔ　−１） （ｊ−１，２） （ｉ−１，・　Φ　・　ｔ　−１） （ｊ−２，３） ｋ−１，２） ｋ−１，２） を用いて上述の連立方程式の上の５式から意味のある根
をすべて求める。これには、例えば、次に示す１次元探
索法により方程式の根を求める方法が用いられる。

この１次元探索法について以下に説明する。

最初の剛体をＢつ、また最後の剛体Ｂ、とじ、物体縦続
接続記述２１中の第１番目（ｉ−０゜・・ｔ）の剛体モ
デルをＢ、であわらす。このとき、形状パラメータ算出
部４１は、指定線分対応付は表３１中の各線分のうち剛
体Ｂ１中の各線分り、と、指定点対対応付は表３２中の
基準剛体Ｂ、中の２点Ｐ、、、（ｋ−１，２）によって
決定される有向線分Ｌ　＋＋　（−Ｐ　ｚ＋　Ｐ　ｚ□
）について、各線分ＬＩ」の３次元空間中の方位ベクト
ルに対応する単位ベクトルＥ１１を剛体モデル線分記述
２２および剛体モデル点記述２４から求め、また剛体Ｂ
１と隣接する各関節の回転軸の方位ベクトルに対応する
単位ベクトルＥ、を関節記述２３から求め、これをもと
に各剛体中の３つの単位ベクトルＥ、、Ｅ、、Ｅｃの配
置を規定する３つの角度αｍｂ＋　　αａｔｊ　　α１
＋ｃ（ただし−α−ｂ−ＣＯＳ　−’（Ｅ、　　・Ｅ、
）、およびスカラー３重積γ５ｈｃ（−（Ｅ、ＸＥ、）
　　・Ｅ、）を求める。ここで、Ｅ、、（ｉ−１，２）
は注目剛体中の有向線分に対応し、Ｅ、２は第ｊ番目の
剛体中の有向線分、さらにＥ、（ｉ−２，３）は検定剛
体中の有向線分に対応するものとする。一方、Ｅｌ、お
よびＥ、４゜は、第ｊ番目の剛体と第（＋＋１）番目の
剛体を結ぶ関節の回転軸に対応する単位ベクトルの座標
系ＳＩおよび座標系Ｓ　Ｈｈｌでの表示とする。また、
物体中の線分り、、の像について、原点Ｏから線分像の
始点方向および終点方向への単位ベクトル（画像位置ベ
クトル）ｐｚ＋　＋　　ｐ１１２とて形成される平面（
解釈平面）の単位法線ベクトル（解釈ベクトル）　　Ｉ
　ｚ−ｐｚ＋　ＸＩ）１１２　／　ｌ　ｐ＋１＋　Ｘｐ
＋１２１を計算により求める。また、線分Ｌ６．に対し
て、解釈平面上の存在域を規定する単位ベクトル（存在
域ベクトル）Ｑ。ｚ　−Ｉ　ｅ＋Ｘ　ｐ　６ｚを求める
。ここで、βｍｂ＝ｃＯ５−’（Ｉｓ　　　ｉｂ　）と
する。

また、表記を簡単化するため、ベクトルＥを正規化する
演算子Ｕを次式で表す。

さらに、以下に示すＥ、、Ｅ、およびＥｃにより定めら
れる５−ｔ−ｃ直交座標系を用いる。

Ｅ　Ｃ−Ｕ　（Ｉ　ｅ　＋　Ｘ　Ｉ　６２）Ｅ、−Ｕ　
（Ｕ　（Ｉ。、ＸＥ、） ＋Ｕ（１，２ＸＥ。）） Ｅ　＋　−Ｅ　ｃ　ｘＥ　ｓ また、この座標系に対応する球面座標表示を（ｓ、　　
ｔ、　　ｃ）　−（ｒ　　ｃｏｓ　ｙ７　　ｓｉｎξ。

ｒ　　ｓｉｎ　ｒ）　　ｓｉｎξ、　　ｒ　　ｃｏｓξ
）で示すものとする。

ここで、ＥｅｌとＥｃ２がなす平面の単位法線ベクトル
Ｎｏ　−（ｃｏｓ　ｒ）　　ｓｉｎξ、　　ｓｉｎηｓ
ｉｎξ、　　ＣＯＳξ）は、ガウス球面上で式（ａ）で
表される曲線上に制限される。

このとき、Ｅ、、、は次の２式を満たすものを選択する
。（必ず１通りに定まる。） Ｅｏｌ＝±Ｕ　（Ｎ、　ｘ　Ｉｏ、） Ｑ・目　−Ｅ、、＞Ｑ　　　　　　　　　　　　（ｂ）
次に、Ｅ。２が次式で求められる。

Ｂ１１２−Ｕ（ｃｏｓａ６１．ｏ２Ｅｅｌ＋　　ｓｉｎ
　ａ　。ｌ、６２　　（Ｎｏ　Ｘ　Ｅ　、＋）＞（ｃ） ｉ−０，１，２・　・ｔに対して、Ｅｌ＋およびＥ、２
が既知のとき、次式でＢ１３を求めることができる。

Ｅ　１３−　Ｕ　　（ω１１Ｅ　、＋ω１２Ｅ１２＋　
γ　＋＋、　１２．１３　　（Ｅ　ｚＸ　Ｅ　１２））
（ｄ） ただし、 ω　＋＋”＝　　　ＣＯ５Ｕ　　１１，１３ＣＯ５α　
１２，１３ ω　１２””　　　Ｃ０ＥＩ　　α　Ｉ２、　＋３ＣＯ
Ｓ　　α　１１．＋３ ここで、ｉ＃ｔのとき、Ｅｌ＋１ ＣＯ５α　１１．１２ ｗａ　Ｅ　、　３か成立す る。

ｉ−１，２・・・ｔに対して、Ｅｌ、およびＩＪ２が既
知のとき、次式でＥｌ２を二義的に求めることができる
。

Ｅｌ２−Ｕ（υ、Ｅ、１＋υ１２１＋２±ｕ　１３　（
ＥｚＸ　Ｉ　１２））　　　（ｅ）ただし、 ＩＪ　ｌｌ−ＣＯＳ　αＩ＋、　１２ υ１２−−ＣｏＳα目、＋２　　（Ｅｚ・Ｉ　１２）υ
、３２冒Ｉ　　　ＣＯ８（１＋（、＋２’（Ｅ、１・Ｉ
、２）２ ここで、式（ｂ）から式（ｅ）で求められるベクトルが
連立方程式の解であるか否かは、Ｅ、３が式（ｆ）を満
たすか否かをチエツクすることで確かめられる。

Ｉｔ、・Ｅｌ３−０　　　　　　　　　　　　　　（ｆ
）即ち、式（ａ）で表される曲線（１次元）上の各点に
おいて式（ｂ）から式（ｅ）により各ベクトルＥ、を算
出し、これが式（ｆ）を満たすか否かを判定することに
より、結果として、式（ｂ）から式（ｅ）により連立方
程式の解がすべて得られる。なお、式（ｅ）は２つの解
を生成するため、式（ａ）の各点に対して、それぞれ２
′組の解の候補が生成される。

しかしながら、上述した１次元探索法と等価な方法によ
り連立方程式の根を求めるなどのバリエーションも各種
考えることができる。縦続剛体回転候補計算部４２２は
、方程式解導出部４２１の出力として得られる各板に対
応する三次元姿勢について、上述の連立方程式の下の３
式が満足されるか否かを確かめ、満足されたすべての姿
勢について各剛体Ｂ、でＥ　ｚ−ＥｚＲ＋　　（ｊ　−
１、２゜３）を満たす回転マトリクスＲ，を求め、剛体
姿勢候補記憶装置４５の該当する項に出力するとともに
有効表示ビットを１にする。

基準剛体姿勢推定部４３は、縦続剛体回転推定部４２の
出力として得られ、剛体姿勢候補記憶装置４５に保持さ
れている基準剛体Ｂ、の回転候補の集合および前記指定
点対対応付は表３２で指定されている２点の３次元位置
Ｐ　ｂｋ−（Ｘ　ｂｈ、　Ｙ　ｂｋ。

Ｚ、、）（ｋ−１，２）　　その画像位置ベクトルｐｂ
＊−ＣＸｂｋ＋　　Ｙｂ＊＋　　Ｚｂｋ）および前記指
定線分対応付は表３１を入力とし、基準剛体のとりうる
姿勢を出力する。

第７図は、前記基準剛体姿勢推定部４３の構成例を示し
、基準剛体平行移動推定部４３１および基準剛体姿勢検
定部４３２からなる。基準剛体平行移動推定部４３１は
、Ｂ、の１つの回転候補に対応する回転マトリクスＲ５
から対応する平行移動Ｔ　ｂ−（ΔＸ、Δｙ、Δ２）を
次式を満たすように求める。

ｐｂ＊−（ＰｂｈＲｂ　＋Ｔｂ　） ／ｌＰｂ＊Ｒｂ＋Ｔｂｌ　　　（ｋ−１，２）これには
、例えば、次に示す解法が用いられる。

この解法においては、基準剛体Ｂ、内の２点の３次元位
置ＰＴｈ、−（Ｘｂｋ、Ｙｂ、、Ｚ、ｋ）（ｋ−１゜２
）およびその画像位置ベクトルｐ　ｂｈ−（Ｘ　ｂｈ。

Ｙ　ｂｋ＋　　Ｚ　ｂｈ）が与えられたとき、画像マト
リクスＲ５から、対応する平行移動Ｔ、−（ΔＸ、Δｙ
。

Δ２）を次式を満たすように求めればよい。

ｐ　ｂｋｓｗ　　（Ｐ　ｂｋＲ）　　＋Ｔｂ　　）／　
　ｌ　　ＰｂｈＲｂ　　＋Ｔｂ いま、ＰｂｋＲｈ　−（Ｘｂｉ　　、　Ｙｂｋ、　　Ｚ
ｂｋ’　）と表記すると、Ｔ、の各要素は次式で与えら
れる。

Ｚｂ＋Ｘｂ２　　Ｚｂ２Ｘｂ＋ しかしながら、この解法と等価な方法により連立方程式
の根を求めるなどのベリエーションモ各種考えることが
できる。基準剛体姿勢検定部４３２は、Ｒ５と上述の連
立方程式を満たすＴ、とからなる剛体姿勢（Ｒｈ、Ｔｂ
）の集合のうち、指定線分対応付は表３１中の線分種別
１，２および３の線分について矛盾をしない姿勢のみを
選択し出力する。即ち、線分種別１．２および３の該線
分Ｌｈ、についてそれぞれ以下に示す判定を行い、姿勢
を選択する。線分種別１については、基準点および基準
点と方向ベクトルで定義される任意の１点からなる２点
の３次元位置Ｐ　ｂ目（ｋ　−１。

２）から算出される推定画像位置ベクトルｐ“ｂａｈ　
−（Ｐ、ｋＲｈ　＋Ｔｂ　）／　ｌ　Ｐ　ｂ　Ｉｈ　Ｒ
ｂ　＋　Ｔ　ｂから次式で算出される推定解釈ベクトル
１　’　ｂｚ−ｐ’　ｂｚ　×ｐ１１１２／　Ｉ　ｐ　
　ｂ＋＋　Ｘｐ　　ｂｚｚと、その線分に対応する画像
上の線分上の任意の２点の画像位置ベクトルｐ　ｂａｈ
から求められる線分の解釈ベクトル！、の解釈ベクトル
−成度Ｉ’ｂ＋”Ｉｂ＋がいき値以上である場合、剛体
姿勢候補記憶装置４５に姿勢候補（Ｒ，、Ｔ、）の値を
格納する。解釈ベクトル−成度がいき値未満である場合
、対応する有効表示ビットを０にする。

線分種別２については端点の３次元位置Ｐ、１１、端点
と方向ベクトルで定義される任意の１点の３次元位置Ｐ
　ｂ１７から算出される前述の解釈ベクトル−成度およ
び端点の推定画像位置ベクトルｐ’　　ｈｚ　　−（Ｐ
　ｈｘ　　Ｒｂ　　＋　Ｔｂ　　）／　　ｌ　　Ｐ　ｂ
ｘ　　Ｒｂ　　＋Ｔｂと、対応する画像上の画像位置ベ
クトルｐ、１、から求められる端点−成度ｐ゛、１　・
ｐ　ｂｚ１が閾値以上である場合、剛体姿勢候補記憶装
置４５に姿勢候補（Ｒ，、Ｔ、）の値を格納する。解釈
ベクトル−成度あるいは端点−成度が閾値未満である場
合、対応する有効表示ビットを０にする。　また、線分
種別３については両端の２点の３次元位置Ｐ−＋−（ｋ
−１，２）から算出される前述の解釈ベクトル−成度お
よび両端点の推定画像位置ベクトル １）　　、＋　　−（Ｐ　ｂａｈ　Ｒｂ　＋　Ｔｂ　）
／　ｌ　Ｐｂ＋ｈ　Ｒｂ　＋Ｔ。

と、対応する画像上の画像位置ベクトルｐ１．から求め
られる端点−成度ｐ’　ｂａｈ　’　ｐｂ目が閾値以上
である場合、剛体姿勢候補記憶装置４５に姿勢候補（Ｒ
，、Ｔ、）の値を格納する。解釈ベクトル−成度あるい
は少なくとも１つの端点−成度が閾値未満である場合、
対応する有効表示ビットを０にする。

剛体姿勢推定部４４は、既に姿勢候補（Ｒ＋−１゜Ｔ＋
−＋）の集合が求められている剛体Ｂ１−１に隣接する
剛体Ｂ１に関して、以下のように姿勢候補（Ｒ，、Ｔ、
）を求める。また、隣接する剛体Ｂ１−２についても同
様の方法で姿勢候補を求める。

第８図は、剛体姿勢推定部４４の構成例を示し、剛体平
行移動推定部４４１は、剛体姿勢候補記憶装置４５中の
有効表示ビットが１である系列のＢ、−１の１つの姿勢
候補（Ｒ＋−＋　、　Ｔ　＋−１）およびこれに対応す
るＢ１の回転に対応する回転マトリクスＲ３から平行移
動Ｔ　＋　−（ΔＸ、Δｙ、Δ２）を２つの剛体を接続
する関節の回転軸の基準点（座標系Ｓ、−３における表
示Ｐ　ｌ−１，３，１〜座標系Ｓ、における表示Ｐ、＋
、＋）に関する次式により求める。

Ｔ　＋　−Ｐ　ｌ−１，ｉ、　１Ｒ＋−＋Ｐ　１．　Ｈ
，１Ｒ１＋　Ｔ　＋−＋ なお、これと等価な方法により平行移動Ｔ１を求めるな
どのバリエーションも各種考えることができる。剛体姿
勢検定部４４２は、Ｒ＋と上述の連立方程式を満たすＴ
＋とからなる剛体姿勢（ＲＴ、）の集合のうち、指定線
分対応付は表３１中の線分種別１．２および３の線分に
ついて矛盾をしない姿勢のみを選択し出力する。即ち、
線分種別１．２および３の該線分Ｌｌ、についてそれぞ
れ以下に示す判定を行い、姿勢を選択する。線分種別１
については、基準点および′基準点と方向ベクトルで定
義される任意の１点からなる２点の３次元位置ｐ、、、
（ｋ−１，２）から算出される推定画像位置ベクトル ｐ　　＋、に−（Ｐ＋１ｋＲ＋　＋Ｔ、）／　Ｉ　Ｐ＋
ｋＲ＋Ｔ＋ から次式で算出される推定解釈ベクトルＩｌ　　、ｊ″
″ｐ’　　ｚ＋　Ｘｐ’　　１１２／　ｌ　ｐｚ＋　×
１）　　＋１２ と、その線分に対応する画像上の線分上の任意の２点の
画像位置ベクトルｐ、、から求められる線分の解釈ベク
トルＩ＋、の解釈ベクトル−成度Ｉ　’　　１１’　Ｉ
　１１が閾値以上である場合、剛体姿勢候補記憶装置４
５に姿勢候補（Ｒ，、ＴＩ　）の値を格納する。解釈ベ
クトル−政変が閾値未満である場合、対応する有効表示
ビットを０にする。　線分種別２については端点の３次
元位置Ｐ　ＩＩ＋および端点と方向ベクトルで定義され
る任意の１点の３次元位置Ｐ１，２から算出される前述
の解除ベクトル−政変および端点の推定画像位置ベクト
ルｐ’　　ｚ＋　−（Ｐ＋１１　Ｒ＋　＋ＴＩ）／　Ｉ
　Ｐ　＋ｒ＋　Ｒ＋　＋　Ｔ と、対応する画像上の画像位置ベクトルｐ、、１から求
められる端点−政変ｐ’　　＋１＋　　・ｐｌｌｌが閾
値以上である場合、剛体姿勢候補記憶装置４５に姿勢候
補（Ｒ１，７１）の値を格納する。解釈ベクトル−政変
あるいは端点−政変が閾値未満である場合、対応する有
効表示ビットを０にする。　また、線分種別３について
は両端の２点の３次元位置ＰＩＩ＆−（ｋ＝１．２）か
ら算出される前述の解釈ベクトル−政変および両端点の
推定画像位置ベクトル ｐ’　　ｌ、ｋ　ｗａ　（Ｐ、ｋＲ＋　十Ｔ＋　）／　
　ｌ　　Ｐ　ｘ＊　　Ｒ＋　　＋Ｔと、対応する画像上
の画像位置ベクトルｐ６５．から求められる端点−政変
ｐ’　　１＊　”　ｐ＋１ｉが閾値以上である場合、剛
体姿勢候補記憶装置４５に姿勢候補（Ｒ１、ＴＩ　）の
値を格納する。解釈ベクトル−政変あるいは少なくとも
１つの端点−政変が閾値未満である場合、対応する有効
表示ビットを０にする。

第９図は剛体姿勢候補記憶装置４５の実現例を示し、物
体の姿勢候補Ｃ１に対応する欄の集合からなり、有効表
示ビット４５１と各剛体姿勢４５２を記憶するために用
いられる。有効ビット４５１は、縦続剛体回転推定部４
２により初期化され、基準剛体姿勢推定部４３および剛
体姿勢推定部４４により更新される。剛体姿勢推定部４
４の動作が終了した時点で有効ビット４５１が１である
姿勢が有効な姿勢を示す。

線分集合姿勢決定装置５は、指定線分集合・点対姿勢推
定装置４の出力として得られる各剛体の姿勢候補をもと
に、物体モデル記述格納装置２に含まれる各剛体中の線
分集合および点集合の像を推定し、これと画像の照合を
とることにより、姿勢候補の中から妥当な姿勢を選択す
る。解が存在しない場合、あるいは解が複数存在する場
合、その旨を信号によって示す。

〔発明の効果〕

以上説明したように、本発明によれば、撮像手段で撮像
した物体の２次元画像の特徴線分および特徴点と予め用
意しである対象物体の物体モデル線分記述および物体モ
デル点記述の特徴との各対応付けを行い、その整合性か
ら所定の条件で規定する線分集合（指定線分集合）およ
び２点（指定点対）を選択し、この選択した指定線分集
合および指定点対から物体の３次元空間で考え得るすべ
ての姿勢候補を求め、これから物体モデル線分記述中の
線分および物体モデル点記述中の点の対応付けと両立し
ない姿勢候補を逐次削除して最終的に各剛体中の線分・
点集合の姿勢を推定し、対象とする物体の３次元空間上
での姿勢を推定しているので、画像入力から得られる点
集合の２次元座標値および有向線分集合の像と予め格納
された物体モデルを構成する剛体中の対応する点の３次
元相対位置および有向線分集合の相対関係とを用いて、
関節で縦続接続された物体の３次元姿勢を高速に推定す
ることができ、工業用視覚センサまたはロボットの視覚
系に広く適用することができる。

【図面の簡単な説明】

第１図は本発明の一実施例に係わる物体の姿勢推定装置
の構成を示すブロック図、第２図は第１図の物体の姿勢
推定装置に使用される入力画像特徴抽出装置に２次元画
像を入力するカメラ系を示す説明図、第３図は対象とす
る３次元物体の一例および第１図の物体の姿勢推定装置
に使用される物体モデル記述格納装置内の各記述を示す
説明図、第４図は第１図の物体の姿勢推定装置に使用さ
れる画像・物体対応付は装置で生成される指定線分集合
付は表および指定点対対応付は表を示す図、第５図は第
１図の物体の姿勢推定装置に使用される指定線分集合・
点対姿勢推定装置の詳細な構成を示すブロック図、第６
図は第５図の指定線分集合・点対姿勢推定装置に使用さ
れる縦続剛体回転推定部の詳細な構成を示すブロック図
、第７図は第５図の指定線分集合・点対姿勢推定装置に
使用される基準剛体姿勢推定部の詳細な構成を示すブロ
ック図、第８図は第５図の指定線分集合・点対姿勢推定
装置に使用される剛体姿勢推定部の詳細な構成を示すブ
ロック図、第９図は第５図の指定線分集合・点対姿勢推
定装置に使用される合成姿勢候補記憶装置に記憶される
内容を示す説明図である。 １・・・入力画像特徴抽出装置、 ２・・・物体モデル記憶格納装置、 ３・・・画像・物体対応付は装置、 ４・・・指定線分集合・点対姿勢推定装置、５・・・線
分集合姿勢決定装置。

Claims (2)

【特許請求の範囲】
1. （１）撮像手段で撮像した２次元画像に対応する物体の
   ３次元姿勢を推定する物体の姿勢推定方法であって、単
   一または複数の剛体および該剛体を縦続接続するための
   軸周りの回転自由度を有する関節から構成される物体の
   定義、該定義により予め指定された物体中の各剛体内の
   有向線分について剛体内で任意に定義される座標系での
   各有向線分の３次元空間内の方向ベクトル値と線分の特
   徴、各関節の回転軸に対応する有向線分について関節に
   よって接続される２つの剛体およびそれぞれに固有の前
   記座標系での方向ベクトル値および該有向線分と同一直
   線上にある１点の３次元座標値、および少なくとも１個
   以上の剛体中の少なくとも２点以上の点について各剛体
   に固有の前記座標系での各点の２次元座標値と特徴を記
   述した物体モデル記述を用意し、前記２次元画像の特徴
   と物体モデル記述との対応付けを行って、所定の条件で
   規定される線分の組合せおよび２点を選択し、該線分の
   組合せおよび２点に対応する前記物体モデル記述中の有
   向線分および２点の姿勢を推定して、前記２次元画像に
   対応する物体の姿勢候補を推定し、この各候補について
   前記物体モデル記述中の各有向線分と各点の像を画像と
   照合し、物体の姿勢を決定することを特徴とする物体の
   姿勢推定方法。
2. （２）物体の２次元画像を撮像する撮像手段と、該撮像
   手段で撮像した画像から特徴を抽出する抽出手段と、単
   一または複数の剛体と該剛体を縦続接続するための軸周
   りの回転自由度を有する関節から構成される対象物体の
   接続構成、対象物体中の各剛体内の有向線分について剛
   体内で任意に定義される座標系での各有向線分の３次元
   空間内の方向ベクトル値と線分の特徴、各関節の回転軸
   に対応する有向線分について関節によって接続される２
   つの剛体に固有の前記座標系での方向ベクトル値および
   該有向線分と同一直線上にある１点の３次元座標値、お
   よび剛体中の少なくとも２点以上の点について各剛体に
   固有の前記座標系での各点の３次元座標値と特徴を記述
   した物体モデル記述を格納する格納手段と、前記２次元
   画像の特徴と物体モデル記述中の特徴記述の対応付けを
   行って、対応付けのとれたものの中から所定の条件で規
   定される線分の組合せおよび２点を選択する選択手段と
   、前記選択された線分集合中の線分および２点の３次元
   空間での考え得るすべての姿勢候補を求める手段と、前
   記求められた各姿勢候補について前記物体モデル記述中
   の各有向線分と各点の像を画像と照合して物体の姿勢を
   決定する決定手段とを有することを特徴とする物体の姿
   勢推定装置。