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

Abstract

(57)【要約】 【目的】 カメラ等の入力画像に対応する任意形状の物
体の３次元空間内での姿勢を高速かつ高精度に推定する
姿勢推定方法と装置を提供する。 【構成】 カメラの２次元画像の特徴線分および特徴点
を入力画像特徴抽出装置１で求める。画像・物体対応付
け装置３にて、これらと物体モデル記述格納装置２の物
体モデル線分記述及び物体モデル点記述の各対応付けを
行い、その整合性から主指定線分集合、２つの指定点
対、副指定線分集合を選択する。この主指定線分集合と
指定点対から３次元空間での全ての姿勢候補を主指定線
分集合・点対姿勢推定装置４で求める。また、副指定線
分集合から３次元空間での全ての姿勢候補を副指定線分
集合姿勢推定装置５で求める。次に、線分集合姿勢決定
装置６で、物体モデル線分記述中の線分および物体モデ
ル点記述中の点の対応付けと両立しない姿勢候補を逐次
削除し、各剛体中の線分・点集合の姿勢を推定する。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

【０００３】

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

【０００４】一方、四面体配置推定法は、これらの欠点
を除去するため、１枚の入力画像と予め記述格納された
物体モデルのみから３次元姿勢を高速に推定することを
目的として、１枚の画像入力から得られる４点の２次元
座標値と予め格納された物体モデル中の４点の３次元相
対位置を用いて、物体の３次元姿勢を推定する方法であ
るが、物体モデル中の４点以上の点を安定的に抽出でき
ないような物体モデルに対してはこの方法を適用できな
いという欠点があった。

【０００５】また、三線分二点配置推定法は、３本の有
向線分と２点の抽出により３次元姿勢を行える点で、四
面体配置推定法では取り扱えない物体をも対象とできる
が、複数の剛体が回転自由度を持つ関節で結合されて構
成されるような柔構造物体には個々の剛体別に姿勢推定
を行なえない場合には、これらの方法を適用できないと
いう問題があり、また、有向線分に関して３次元方向の
みを用いていたため、線分の通過点あるいは端点等が固
定されている場合については、それらの情報が利用され
ないことに起因して余分の対応付けを必要とするという
問題点があった。

【０００６】また、縦続物体指定線分配置推定法は、単
一または複数の剛体および該剛体を接続するための軸ま
わりの回転自由度を持つ関節により構成される縦続接続
物体の姿勢推定を両端の剛体中の２線分およびその他の
剛体中の１線分および１つの剛体中の２点により実現で
きるが、任意の接続物体の姿勢推定には適用できないと
いう問題があり、また、縦続接続物体であっても前記組
み合わせ以外の線分組み合わせでは物体の姿勢を推定で
きないという問題点があった。

【０００７】本発明は、上記従来技術の問題点を除去す
るためになされたものであり、その目的は、カメラ等の
入力画像に対応する任意形状の物体の３次元空間内での
姿勢を高速かつ高精度に推定する方法及び装置を提供す
ることにある。

【０００８】

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

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

【００１０】

【作用】本発明の物体の姿勢推定方法およびその装置で
は、撮像手段で撮像した物体の２次元画像の特徴線分お
よび特徴点と予め用意してある対象物体の物体モデル線
分記述および物体モデル点記述の特徴との各対応付けを
行い、その整合性から所定の条件で規定する線分集合
（主指定線分集合）および２点（指定点対）および所定
の条件で規定する線分集合（副指定線分集合）を選択
し、主指定線分集合と指定点対から３次元空間で考えう
る全ての姿勢候補を求め、次に、副指定線分集合から物
体の３次元空間で考えうる全ての姿勢候補を求め、これ
ら物体モデル線分記述中の線分および物体モデル点記述
中の点の対応付けと両立しない姿勢候補を逐次削除し、
最終的に各剛体中の線分・点集合の姿勢を推定して、該
物体の３次元姿勢を高精度で推定可能とする。また、２
次元画像入力から得られる点集合および線分集合の像
と、予め格納された物体モデルを構成する剛体中の対応
する点の３次元相対位置および有向線分集合の相対関係
とを用いることにより、関節で接続されたような様々な
形態の物体の３次元姿勢を高速に推定可能としている。

【００１１】

【実施例】以下、図面を用いて本発明の実施例を詳細に
説明する。

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

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

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

【００１５】（１）縦続接続された剛体集合中の両端の
剛体について、剛体モデル線分記述（後述する図４
（ｃ）の２３）の中のそれぞれ２本の線分（但し、単一
の剛体である場合には３本の線分）である。しかしなが
ら、剛体モデル点記述（後述する図５（ｂ）の２５）の
中の点対を線分種別３（線分の両端点が固定されている
線分種別を指す）の線分の代用として用いることができ
る。この場合、剛体モデル線分記述２３には対応する線
分種別３の線分に関する記述を付加するものとする。

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

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

【００１８】副指定線分集合とは、以下に示す（４）の
条件を満たす線分集合のことである。

【００１９】（４）主指定線分集合中の線分を含む剛体
を一端とする、縦続接続された剛体集合中の各剛体につ
いて、剛体モデル線分記述２３中のそれぞれ１本の線
分。ただし、剛体モデル点記述２５中の点対を線分種別
３の線分の代用として用いることができる。この場合、
剛体モデル線分記述２３には対応する線分種別３の線分
に関する記述を付加するものとする。

【００２０】指定点対は次のように定義される。

【００２１】（５）上記（１）項から（３）項で指定さ
れる線分のうち、１本の線分種別３の線分の両端点を指
定点対と呼ぶ。

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

【００２３】画像・物体対応付け装置３は、入力画像特
徴抽出装置１で抽出されたカメラ入力画像の特徴線分お
よび特徴点の特徴と物体モデル記述格納装置２に格納さ
れている剛体モデル線分記述および剛体モデル点記述中
の特徴記述との対応付けを行い、対応付けの取れたもの
の中から前記条件で規定される主指定線分集合、副指定
線分集合および指定点対を選択し、該主指定線分集合、
副指定線分集合および指定点対よりなる主指定線分・副
指定線分・点対対応付け表を生成する。この画像・物体
対応付け装置３は本発明の問題を解決する手段における
２つの選択手段を兼ねている。

【００２４】主指定線分集合・点対姿勢推定装置４で
は、画像・物体対応付け装置３により選択された主指定
線分集合および指定点対について、それの３次元空間内
で考えうるすべての姿勢候補を推定する。これは、主指
定線分集合・指定点対および各関節の回転軸について３
次元回転の候補を求め、次に指定点対の対応付けから１
つの剛体の３次元平行移動を求め、次に隣接する剛体の
３次元平行移動を順次求めることにより、各剛体の３次
元姿勢を推定するという順序で、主物体縦続接続記述中
の各剛体について３次元空間内での姿勢候補をすべて生
成する。

【００２５】副指定線分集合姿勢推定装置５では、画像
・物体対応付け装置３により選択された副指定線分集合
について、それの３次元空間内で考えうるすべての姿勢
候補を推定する。これは、主指定線分集合・点対姿勢推
定装置４で求められた姿勢候補から副指定線分集合およ
び各関節の回転軸について３次元回転の候補を求め、姿
勢候補がすでに求められた剛体に隣接する剛体の３次元
平行移動を順次求めることにより、副物体縦続接続記述
中の各剛体の３次元姿勢を推定するという順序で、各剛
体について３次元空間内での姿勢候補をすべて生成す
る。

【００２６】線分集合姿勢決定装置６では、主指定線分
集合・点対姿勢推定装置４および副指定線分集合姿勢推
定装置５により生成された各剛体の姿勢候補から物体モ
デル記述格納装置２に含まれる各剛体中の線分集合およ
び点集合の像を推定し、これと画像の照合をとることに
より、姿勢候補の中から妥当な姿勢を選択する。各剛体
中の線分集合および点集合の姿勢が推定できれば、対象
とするカメラ被写体物体の姿勢が推定できたことにな
る。

【００２７】図２（ａ）は前記入力画像特徴抽出装置１
に２次元画像を入力するカメラ系を示す。カメラの中心
点を原点Ｏ、光軸をＺ軸とする直交座標系Ｘ−Ｙ−Ｚを
考え、Ｚ＝ｆ（ｆはカメラの焦点距離で既知とする）に
画像面１１を考え、該画像面１１とＺ軸の交点Ｏ’も既
知であるものとする。また、画像は中心投影法により生
成されるものとする。即ち、３次元空間内の点Ｐ（Ｘ，
Ｙ，Ｚ）とその像である点ｐ（ｕ，ｖ）は （ｕ，ｖ）＝（Ｘｆ／Ｚ，Ｙｆ／Ｚ） なる関数をみたすものとする。入力画像特徴抽出装置１
は、画像面１１の画像を入力し、被写体の点特徴（２直
線の交点、色彩、記載された記号など）を記述した図２
（ｂ）に示す特徴点記述１２および、被写体の線分特徴
（色彩、線分の両側領域の色彩、線分の両端の光学的特
徴および幾何的特徴など）を記述した特徴線分記述１３
を抽出する。ここで、特徴点ｐ_i（ｉ＝１，２，…）の
座標は（ｕ_i，ｖ_i）であり、有向線分ｌ_i（ｉ＝１，
２，…）の始点ｐ_i1および終点ｐ_i2の座標（ｕ_ij，
ｖ_ij）（ｊ＝１，２）である。

【００２８】図３は対象とする３次元物体の一例を示
し、図４（ａ），同図（ｂ），同図（ｃ）および図５
（ａ），同図（ｂ）はそれぞれ該物体に対応する物体モ
デル記述格納装置２内の主物体縦続接続記述および副物
体縦続接続記述および剛体モデル線分記述および関節記
述および剛体モデル線分記述の例を示したものである。
即ち、物体モデル記述格納装置２内の主物体縦続接続記
述２１は、対象とする物体２０に含まれる、両端が剛体
であり、かつ、剛体と関節を交互に並べた順列であり、
各関節が前後の剛体を接続することを示している。ま
た、各剛体Ｂ_a0には、その剛体に固有の座標系Ｓ_a0が定
義されている。副物体縦続接続記述２２_-aは、対象とす
る物体２０に含まれる、一端が主物体縦続接続記述中に
含まれる剛体Ｂ_a0であり、かつ、剛体と関節を交互に並
べた順列であり、各関節が前後の剛体を接続することを
示している。また、各剛体Ｂ_abには、その剛体に固有の
座標系Ｓ_abが定義されている。物体モデル記述格納装置
２内の剛体モデル線分記述２３は、対象とする物体２０
に含まれる各剛体Ｂ_ab内の１本以上（ただし、主物体縦
続接続記述の両端では２本以上、または主物体縦続接続
記述が単一剛体の場合には３本以上）の有向線分Ｌ_ab1
〜Ｌ_abnについての、座標系Ｓ_abにおける３次元方向
（ｄｘ_ab1，ｄｙ_ab1，ｄｚ_ab1）〜（ｄｘ_abn，ｄ
ｙ_abn，ｄｚ_abn）とその有向線分を特徴付ける線分およ
び周辺領域の光学的特徴（色彩・符号など）および、線
分種別（０：座標系Ｓ_ab内で位置が３次元空間内で不
定、１：線分の位置が共線位置にある一点（基準点）に
より制約される、２：線分の１端点のみが固定、３：線
分の両端点が固定）および、線分種別１のものについて
は基準点の３次元座標、また線分種別２のものについて
は端点の３次元座標、また線分種別３のものについては
両端点の３次元座標の組み合わせからなる線分属性記述
の集合として与えられる。また、物体モデル記述格納装
置２内の関節記述２４は、対象とする物体２０に含まれ
る各関節Ｊ_abについて、主物体縦続接続記述２１により
示される隣接する剛体Ｂ_a-1,₀，Ｂ_a,₀により、あるいは
副物体縦続接続記述２２により示される隣接する剛体Ｂ
_a,_b-1，Ｂ_a,_bに固有の座標系

【００２９】

【数１】

【００３０】中での回転軸

【００３１】

【数２】

【００３２】（線分種別１の有向線分）の基準点の３次
元座標値

【００３３】

【数３】

【００３４】と３次元方向

【００３５】

【数４】

【００３６】を示す関節記述の集合として与えられる。
また、物体モデル記述格納装置２内の剛体モデル点記述
２５は、対象とする物体２０に含まれる各剛体Ｂ_ab上の
２点以上の点Ｐ_ab1〜Ｐ_abnについての、剛体中で定義さ
れる任意の座標系Ｓ_abにおける３次元座標値（ｘ_ab1，
ｙ_ab1，ｚ_ab1）〜（ｘ_abn，ｙ_abn，ｚ_abn）とその点を
特徴付ける光学的特徴（色彩・符号など）のペアからな
る点属性記述の集合として与えられる。

【００３７】画像・物体対応付け装置３では、入力画像
特徴抽出装置１の特徴点出力および特徴線分出力と物体
モデル記述格納装置２の各線分属性記述および各点属性
記述中の光学的特徴記述の対応付けを行い、対応付けの
とれたものの中から主指定線分集合および副指定線分集
合および指定点対を選択して、図６（ａ），（ｂ），
（ｃ）に例示するような主指定線分対応付け表３１およ
び副指定線分対応付け表３２および指定点対対応付け表
３３を生成する。主指定線分集合および副指定線分集合
および指定点対は、対応付けの取れたものの中からラン
ダムに選択する。この選択した主指定線分集合および副
指定線分集合および指定点対の姿勢を推定した結果、許
容される姿勢が存在しない場合は、対応付けの取れたも
のの中から前記条件で規定される主指定線分集合および
副指定線分集合および指定点対を選択して再び姿勢推定
する。また、主指定線分集合および副指定線分集合およ
び指定点対の選択をランダムにするのではなく、線分種
別３、２あるいは１の線分の優先選択、あるいは点間の
距離、３線分に対応する単位ベクトルのスカラー３種類
などにより定義される評価関数による指定線分集合およ
び指定点対の選択など、バリエーションは各種考えら
れ、これにより対応付けの効率を上げることができる。

【００３８】主指定線分集合・点対姿勢推定装置４は、
画像・物体対応付け装置３の出力として得られる主指定
線分対応付け表３１で示された各指定線分および、指定
点対応付け表３３で示された点対の３次元空間内での考
えうる全ての姿勢候補を推定する。副指定線分集合姿勢
推定装置５は、画像・物体対応付け装置３の出力として
得られる副指定線分対応付け表３２で示された各指定線
分の３次元空間内での考えうる全ての姿勢候補を推定す
る。図６（ａ）の主推定線分対応付け表３１の指定線分
集合Ｌ₀₀₁，Ｌ₀₀₂，Ｌ₁₀₂，Ｌ₂₀₁，Ｌ₂₀₃および、図６
（ｂ）の副指定線分対応付け表３２の指定線分集合Ｌ
₀₁₁，Ｌ₁₁₁および、図６（ｃ）の指定点対応付け表３３
の２点Ｐ₁₀₁，Ｐ₁₀₂を図３の物体２０中に矢印付き太
線、矢印付き二重線および白丸で示す。

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

【００４０】主形状パラメータ算出部４１は、図４の主
物体縦続接続記述２１および剛体モデル線分記述２３な
らびに図５の関節記述２４および剛体モデル点記述２５
ならびに図６の主指定線分対応付け表３１および指定点
対対応付け表３３を入力とし、主縦続剛体回転推定部４
２で用いる各種の形状パラメータを算出する。ここで、
主物体縦続接続記述２１中の最初の剛体を注目剛体
Ｂ₀₀、また最後の剛体を検定剛体Ｂ_t0、また指定点対を
含む剛体を基準剛体Ｂ_b0と呼ぶことにし、主物体縦続接
続記述２１中の第ｉ番目（ｉ＝０，…ｔ）の剛体モデル
をＢ_i0で表す。このとき、主形状パラメータ算出部４１
は、主指定線分対応付け表３１中の各線分のうち剛体Ｂ
_i0中の各線分Ｌ_i0jと、指定点対対応付け表３３中の基
準剛体Ｂ_b0中の２点Ｐ_b0k（ｋ＝１，２）によって決定
される有向線分Ｌ_b0（＝Ｐ_b01Ｐ_b02）について、各線分
Ｌ_i0jの３次元空間中の方位ベクトルに対応する単位ベ
クトルＥ＊_i0jを剛体モデル線分記述２３および剛体モ
デル点記述２５から求め、また剛体Ｂ_i0と隣接する各関
節の回転軸の方位ベクトルＥ＊_i0jを関節記述２４から
求め、これをもとに各剛体中の３つの単位ベクトルＥ＊
_a，Ｅ＊_b，Ｅ＊_cの配置を規定する３つの角度α_ab，α
_ac、α_bc（ただし、Ｅ＊_a・Ｅ＊_b＝ｃｏｓα_ab）、およ
びスカラー３重積γ_abc（＝（Ｅ＊_a×Ｅ＊_b）・Ｅ＊_c）
を求める。ここで、Ｅ＊_00i（ｉ＝１，２）は注目剛体
中の有向線分に対応し、Ｅ_j02は第ｊ番目の剛体中の有
向線分、さらに、Ｅ＊_t0i（ｉ＝２，３）は検定剛体中
の有向線分に対応するものとする。一方、Ｅ＊_j03およ
びＥ＊_j+1,₀,₁は、第ｊ番目の剛体と第（ｊ＋１）番目
の剛体を結ぶ関節の回転軸に対応する単位ベクトルの座
標系Ｓ_j,₀および座標系Ｓ_j+1,₀での表示とする。また、
物体中の線分Ｌ_i0jの像ｌ_i0jについて、原点Ｏから線分
像の始点方向および終点方向の単位ベクトル（画像ベク
トル）ｐ_i0j1、ｐ_i0j2とで形成される平面（解釈平面）
の単位法線ベクトル（解釈ベクトル）Ｉ_i0j＝ｐ_i0j1×
ｐ_i0j2／｜ｐ_i0j1×ｐ_i0j2｜を計算により求める。ま
た、各線分Ｌ_i0jに対して、解釈平面上の存在域を規定
する単位ベクトル（存在域ベクトル）Ｑ_i0jk＝Ｉ_i0j×
ｐ_i0jkを求める。

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

【００４２】Ｅ_i0j・Ｅ_i0j＝１ （ｉ＝０，…ｔ ａｎ
ｄ ｊ＝１，２，３） Ｅ_i0j・Ｅ_i0k＝ｃｏｓα_i0j,_i0k （ｉ＝０，…ｔ ａ
ｎｄ ｊ，ｋ＝１，２，３） （Ｅ_i01×Ｅ_i02）・Ｅ_i03＝γ_i01,_i02,_i03 Ｅ_i03＝Ｅ_i+1,₀,₁ （ｉ＝０，…ｔ−１） Ｉ_00j・Ｅ_00j＝０（ｊ＝１，２） Ｉ_i02・Ｅ_i02＝０（ｉ＝１，…ｔ−１） Ｉ_t0j・Ｅ_t0j＝０（ｊ＝２，３） Ｑ_00jk・Ｅ_00j＞０（ｊ＝１，２ ａｎｄ ｋ＝１，
２） Ｑ_i02k・Ｅ_i02＞０（ｉ＝１，…ｔ−１ ａｎｄ ｋ＝
１，２） Ｑ_t0jk・Ｅ_t0j＞０（ｊ＝２，３ ａｎｄ ｋ＝１，
２） 図８は、上記主縦続剛体回転推定部４２の構成例を示
し、方程式解導出部４２１および主縦続剛体回転候補計
算部４２２からなり、主形状パラメータ算出部４１で計
算されるα_ab、γ_abcおよびＩ_i0jおよびＱ_i0jkを入力と
し、上記連立方程式あるいは同様の連立方程式を満たす
Ｅ_i0jの組をすべて出力する構成を示している。即ち、
方程式解導出部４２１は、与えられた入力（α_ab、γ
_abc、Ｉ_i0jおよびＱ_i0jk）のうちＱ₀₀₁₁以外のＱ_i0jkを
除く情報を用いて上述の連立方程式の上の７式から意味
のある根をすべて求める。これには、例えば、次に示す
１次元探索法により方程式の根を求める方法が用いられ
る。

【００４３】この１次元探索法について以下に説明す
る。ここで、表記を簡単化するため、ベクトルＥを正規
化する演算子Ｕを次式で表す。

【００４４】Ｕ（Ｅ）＝Ｅ／‖Ｅ‖ また、以下に示すＥ_s、Ｅ_tおよびＥ_cにより定められる
ｓ−ｔ−ｃ直交座標系を用いる。

【００４５】Ｅ_c＝Ｕ（Ｉ₀₀₁×Ｉ₀₀₂） Ｅ_s＝Ｕ（Ｕ（Ｉ₀₀₁×Ｅ_c）＋Ｕ（Ｉ₀₀₂×Ｅ_c）） Ｅ_t＝Ｅ_c×Ｅ_s また、この座標系に対応する球面座標表示を （ｓ，ｔ，ｃ）＝（ｒ ｃｏｓηｓｉｎ ξ，ｒ ｓｉ
ｎηｓｉｎξ，ｒ ｃｏｓξ） で示すものとする。

【００４６】ここで、Ｅ₀₀₁とＥ₀₀₂がなす平面の単位法
線ベクトル Ｎ₀＝（ｃｏｓηｓｉｎ ξ，ｓｉｎηｓｉｎξ，ｃｏｓ
ξ） は、ガウス球面上で式（ａ）で表される曲線上に制限さ
れる。

【００４７】

【数５】

【００４８】このとき、Ｅ₀₀₁は次の２式を満たすもの
を選択する（必ず１通り定まる）。

【００４９】Ｅ₀₀₁＝±Ｕ（Ｎ₀×Ｉ₀₀₁） Ｑ₀₀₁₁・Ｅ₀₀₁＞０ （ｂ） 次に、Ｅ₀₀₂が次式で求められる。

【００５０】 Ｅ₀₀₂＝Ｕ（ｃｏｓα₀₀₁,₀₀₂Ｅ₀₀₁＋ｓｉｎα₀₀₁,₀₀₂（Ｎ₀×Ｅ₀₀₁）） （ｃ） ｉ＝０，１，２…ｔに対して、Ｅ_i01およびＥ_i02が既知
のとき、次式でＥ_i03を求めることができる。

【００５１】 Ｅ_i03＝Ｕ（ω_i01Ｅ_i01＋ω_i02Ｅ_i02＋γ_i01,_i02,_i03（Ｅ_i01×Ｅ_i02）） （ｄ） ただし、 ω_i01＝ｃｏｓα_i01,_i03−ｃｏｓα_i02,_i03ｃｏｓ
α_i01,_i02 ω_i02＝ｃｏｓα_i02,_i03−ｃｏｓα_i01,_i03ｃｏｓ
α_i01,_i02 ここで、ｉ≠ｔのとき、Ｅ_i+1,₀,₁＝Ｅ_i03が成立する。
ｉ＝１，２…ｔに対して、Ｅ_i01およびＩ_i02が既知のと
き、次式でＥ_i02を二義的に求めることができる。

【００５２】 Ｅ_i02＝Ｕ（υ_i01Ｅ_i01＋υ_i02Ｉ_i02±υ_i03（Ｅ_i01×Ｉ_i02）） （ｅ） ただし、 υ_i01＝ｃｏｓα_i01,_i02 υ_i02＝−ｃｏｓα_i01,_i02（Ｅ_i01・Ｉ_i02） υ_i03 ²＝１−ｃｏｓ²α_i01,_i02−（Ｅ_i01・Ｉ_i02）² ここで、式（ｂ）から式（ｃ）で求められるベクトルが
連立方程式の解であるか否かは、Ｅ_t03が式（ｆ）を満
たすか否かをチェックすることにより確かめられる。

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

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

【００５５】基準剛体姿勢推定部４３は、主縦続剛体回
転推定部４２の出力として得られ、主縦続剛体回転候補
記憶装置４５に保持されている基準剛体Ｂ_b0の回転候補
の集合および指定点対対応付け表３３で指定されている
２点の３次元位置Ｐ_b0k＝（Ｘ_b0k，Ｙ_b0k，Ｚ_b0k）（ｋ
＝１，２）およびその画像位置ベクトルｐ_bok＝
（ｘ_b0k，ｙ_b0k，ｚ_b0k）および主指定線分対応付け表
３１を入力とし、基準剛体のとりうる姿勢を出力する。

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

【００５７】 ｐ_b0k＝（Ｐ_b0kＲ_b0＋Ｔ_b0）／｜Ｐ_b0kＲ_b0＋Ｔ_b0｜ （ｋ＝１，２） これには、例えば次に示す解法が用いられる。

【００５８】この解法においては、基準剛体Ｂ_b0内の２
点の３次元位置Ｐ_b0k＝（Ｘ_b0k，Ｙ_b0k，Ｚ_b0k）（ｋ＝
１，２）およびその画像位置ベクトルｐ_b0k＝（ｘ_b0k，
ｙ_b0k，ｚ_b0k）が与えられたとき、Ｐ_b0kＲ_b0＝（Ｘ'
_b0k，Ｙ'_b0k，Ｚ'_b0k）と表記すると、平行移動Ｔ_b0の
各要素は次式で与えられる。

【００５９】

【数６】

【００６０】しかしながら、この解法と等価な方法によ
り連立方程式の根を求めるなどのバリエーションも各種
考えることができる。基準剛体姿勢検定部４３２は、回
転マトリクスＲ_b0と上述の連立方程式を満たす平行移動
Ｔ_b0とからなる剛体姿勢（Ｒ_b0，Ｔ_b0）の集合のうち、
主指定線分対応付け表３１中の線分種別１および２およ
び３の線分について矛盾をしない姿勢のみを選択し出力
する。即ち、線分種別１および２および３の該線分Ｌ
_b0iについてそれぞれ以下に示す判定を行い、姿勢を選
択する。線分種別１については、基準点および基準点と
方向ベクトルで定義される任意の１点からなる２点の３
次元位置Ｐ_b0ik（ｋ＝１，２）から算出される推定画像
位置ベクトル ｐ'_b0ik＝（Ｐ_b0ikＲ_b0＋Ｔ_b0）／｜Ｐ_b0ikＲ_b0＋Ｔ_b0
｜ から次式で算出される推定解釈ベクトル Ｉ'_b0i＝ｐ’_b0i1×ｐ'_b0i2／｜ｐ’_b0i1×ｐ'_b0i2｜ と、その線分に対応する画像上の線分上の任意の２点の
画像位置ベクトルｐ_b0ikから求められる線分の解釈ベク
トルＩ_boiの解釈ベクトル一致度Ｉ’_b0i・Ｉ_b0iがいき
値以上である場合、主縦続剛体姿勢候補記憶装置４５に
姿勢候補（Ｒ_b0，Ｔ_b0）の値を格納する。解釈ベクトル
一致度がいき値未満である場合、対応する有効表示ビッ
トを０にする。線分種別２については端点の３次元位置
Ｐ_b0i1および、端点と方向ベクトルで定義される任意の
１点の３次元位置Ｐ_b0i2から算出される前述の解釈ベク
トル一致度および、端点の推定画像位置ベクトル ｐ'_b0i1＝（Ｐ_b0i1Ｒ_b0＋Ｔ_b0）／｜Ｐ_b0i1Ｒ_b0＋Ｔ_b0
｜ と、対応する画像上の画像位置ベクトルｐ_b0i1から求め
られる端点一致度ｐ'_b0i1・ｐ_b0i1がいき値以上である
場合、主縦続剛体姿勢候補記憶装置４５に姿勢候補（Ｒ
_b0，Ｔ_b0）の値を格納する。解釈ベクトル一致度あるい
は端点一致度がいき値未満である場合、対応する有効表
示ビットを０にする。また、線分種別３については両端
の２点の３次元位置Ｐ_b0ik（ｋ＝１，２）から算出され
る前述の解釈ベクトル一致度および、両端点の推定画像
位置ベクトル ｐ'_b0ik＝（Ｐ_b0ikＲ_b0＋Ｔ_b0）／｜Ｐ_b0ikＲ_b0＋Ｔ_b0
｜ と、対応する画像上の画像位置ベクトルｐ_b0ikから求め
られる端点一致度ｐ'_{b0i k}・ｐ_b0ikがいき値以上である
場合、主縦続剛体姿勢候補記憶装置４５に姿勢候補（Ｒ
_b0，Ｔ_b0）の値を格納する。解釈ベクトル一致度あるい
は少なくとも１つの端点一致度がいき値未満である場
合、対応する有効表示ビットを０にする。

【００６１】主縦続剛体姿勢推定部４４は、既に姿勢候
補（Ｒ_i-1,₀，Ｔ_i-1,₀）の集合が求められている剛体Ｂ
_i-1,₀に隣接する剛体Ｂ_i0に関して、以下のような姿勢
候補（Ｒ_i0，Ｔ_i0）を求める。また、隣接する剛体Ｂ
_i-2,₀についても同様の方法で姿勢候補を求める。

【００６２】図１０は、上記主縦続剛体姿勢推定部４４
の構成例を示し、４４１は主縦続剛体平行移動推定部、
４４２は主縦続剛体姿勢検定部である。主縦続剛体平行
移動推定部４４１は、主縦続剛体姿勢候補記憶装置４５
中の有効表示ビットが１である系列の剛体Ｂ_i-1,₀の１
つの姿勢候補（Ｒ_i-1,₀，Ｔ_i-1,₀）およびこれに対応す
る剛体Ｂ_i0の回転に対応する回転マトリクスＲ_i0から、
平行移動Ｔ_i0＝（Δｘ，Δｙ，Δｚ）を２つの剛体を接
続する関節の回転軸の基準点（座標系Ｓ_i-1,₀における
表示Ｐ_i-1,₀,₃,₁、座標系Ｓ_i0における表示Ｐ_i,₀,₁,₁）
に関する次式により求める。

【００６３】 Ｔ_i0＝Ｐ_i-1,₀,₃,₁Ｒ_i-1,₀−Ｐ_i,₀,₁,₁Ｒ_i0＋Ｔ_i-1,₀ なお、これと等価な方法により平行移動Ｔ_i0を求めるな
どのバリエーションも各種考えることができる。主縦続
剛体姿勢検定部４４２は、回転マトリクスＲ_i0と上述の
連立方程式を満たす平行移動Ｔ_i0とからなる剛体姿勢
（Ｒ_i0，Ｔ_i0）の集合のうち、主指定線分対応付け表３
１中の線分種別１および２および３の線分について矛盾
をしない姿勢のみを選択し出力する。即ち、線分種別１
および２および３の該線分Ｌ_i0jについてそれぞれ以下
に示す判定を行い、姿勢を選択する。線分種別１につい
ては、基準点および基準点と方向ベクトルで定義される
任意の１点からなる２点の３次元位置Ｐ_i0jk（ｋ＝１，
２）から算出される推定画像位置ベクトル ｐ'_i0jk＝（Ｐ_i0jkＲ_i0＋Ｔ_i0）／｜Ｐ_i0jkＲ_i0＋Ｔ_i0
｜ から次式で算出される推定解釈ベクトル Ｉ'_i0j＝ｐ’_i0j1×ｐ'_i0j2／｜ｐ’_i0j1×ｐ'_i0j2｜ と、その線分に対応する画像上の線分上の任意の２点の
画像位置ベクトルｐ_i0jkから求められる線分の解釈ベク
トルＩ_i0jの解釈ベクトル一致度Ｉ’_i0j・Ｉ_i0jがいき
値以上である場合、主縦続剛体姿勢候補記憶装置４５に
姿勢候補（Ｒ_i0，Ｔ_i0）の値を格納する。解釈ベクトル
一致度がいき値未満である場合、対応する有効表示ビッ
トを０にする。線分種別２については端点の３次元位置
Ｐ_i0j1および、端点と方向ベクトルで定義される任意の
１点の３次元位置Ｐ_i0j2から算出される前述の解釈ベク
トル一致度および、端点の推定画像位置ベクトル ｐ'_i0j1＝（Ｐ_i0j1Ｒ_i0＋Ｔ_i0）／｜Ｐ_i0j1Ｒ_i0＋Ｔ_i0
｜ と、対応する画像上の画像位置ベクトルｐ_i0j1から求め
られる端点一致度ｐ'_i0j1・ｐ_i0j1がいき値以上である
場合、主縦続剛体姿勢候補記憶装置４５に姿勢候補（Ｒ
_i0，Ｔ_i0）の値を格納する。解釈ベクトル一致度あるい
は端点一致度がいき値未満である場合、対応する有効表
示ビットを０にする。また、線分種別３については両端
の２点の３次元位置Ｐ_i0jk（ｋ＝１，２）から算出され
る前述の解釈ベクトル一致度および、両端点の推定画像
位置ベクトル ｐ'_i0jk＝（Ｐ_i0jkＲ_i0＋Ｔ_i0）／｜Ｐ_i0jkＲ_i0＋Ｔ_i0
｜ と、対応する画像上の画像位置ベクトルｐ_i0jkから求め
られる端点一致度ｐ'_i0jk・ｐ_i0jkがいき値以上である
場合、主縦続剛体姿勢候補記憶装置４５に姿勢候補（Ｒ
_i0，Ｔ_i0）の値を格納する。解釈ベクトル一致度あるい
は少なくとも１つの端点一致度がいき値未満である場
合、対応する有効表示ビットを０にする。

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

【００６５】副指定線分集合姿勢推定装置５は、図１２
に示すように、副形状パラメータ算出部５１、副縦続剛
体回転推定部５２、副縦続剛体姿勢推定部５３、副縦続
剛体姿勢候補記憶装置５４により構成される。

【００６６】副形状パラメータ算出部５１は、副物体縦
続接続記述２２および剛体モデル線分記述２３および関
節記述２４および副指定線分対応付け表３２を入力と
し、副縦続剛体回転推定部５２で用いる各種の形状パラ
メータを算出する。ここで、副物体縦続接続記述２２−
ｉの第ｈ番目（ｈ＝０，…，ｓ）の剛体モデルをＢ_ihで
表し、最後の剛体Ｂ_isを終端剛体と呼び、それ以外の剛
体Ｂ_ih（ｈ＝０，…，ｓ−１）を非終端剛体と呼ぶ。こ
のとき、副形状パラメータ算出部５１は、副指定線分対
応付け表３２−ｉ中の各線分のうち剛体Ｂ_ih中の各線分
Ｌ_ihjについて、各線分Ｌ_ihjの３次元空間中の方位ベク
トルに対応する単位ベクトルＥ＊_ihjを剛体モデル線分
記述２３から求め、また剛体Ｂ_ihと隣接する各関節の回
転軸の方位ベクトルＥ＊_ihjを関節記述２４から求め、
これをもとに各非終端剛体中の３つの単位ベクトルＥ＊
_a、Ｅ＊_b、Ｅ＊_cの配置を規定する３つの角度α_ab、α
_ac、α_bc（ただし、Ｅ＊_a・Ｅ＊_b＝ｃｏｓα_ab）、およ
びスカラー３重積γ_abc（＝（Ｅ＊_a×Ｅ＊_b）・Ｅ＊_c）
を求める。なお終端剛体については２つの単位ベクトル
Ｅ＊_a、Ｅ＊_bの配置を規制する１つの角度α_ab（ただ
し、Ｅ＊_a・Ｅ＊_b＝ｃｏｓα_ab）を求める。ここで、Ｅ
＊_ih2は第ｈ番目の剛体中の有向線分、一方、Ｅ＊_ih3お
よびＥ＊_i,_h+1,₁は、第ｈ番目の剛体と第（ｈ＋１）番
目の剛体を結ぶ関節の回転軸に対応する単位ベクトルの
座標系Ｓ_ihおよび座標系Ｓ_i,_h+1での表示とする。ま
た、物体中の線分Ｌ_ihjの像ｌ_ihjについて、原点Ｏから
線分像の始点方向および終点方向の単位ベクトル（画像
ベクトル）ｐ_ihj1、ｐ_ihj2とで形成される平面（解釈平
面）の単位法線ベクトル（解釈ベクトル）Ｉ_ihj＝ｐ
_ihj1×ｐ_ihj2／｜ｐ_ihj1×ｐ_ihj2｜を計算により求め
る。また、各線分Ｌ_ihjに対して、解釈平面上の存在域
を規定する単位ベクトル（存在域ベクトル）Ｑ_ihjk＝Ｉ
_ihj×ｐ_ihjkを求める。

【００６７】副縦続剛体回転推定部５２では、主縦続剛
体姿勢候補記憶装置４５に記憶されているＥ_iojおよび
副形状パラメータ算出部５１の出力として得られる角度
α_ab、スカラー３重積γ_abc、解釈ベクトルＩ_ihj、存在
域ベクトルＱ_ihjkから、指定線分および関節の回転軸の
取りうる勾配をすべて求める。これは、下記の連立方程
式をすべて満足するベクトルＥ_ihj、即ちＥ＊_ihjに回転
マトリクスＲ_ihを施した実空間での配置（Ｅ_ihj＝Ｅ＊
_ihjＲ_ih）および回転マトリクスＲ_ihをすべて出力する
ことに相当する。

【００６８】Ｅ_ihj・Ｅ_ihj＝１ （ｈ＝０，…ｓ−１
ａｎｄ ｊ＝１，２，３） Ｅ_isj・Ｅ_isj＝１ （ｊ＝１，２） Ｅ_ihj・Ｅ_ihk＝ｃｏｓα_ihj,_ihk （ｉ＝０，…ｓ−１
ａｎｄ ｊ，ｋ＝１，２，３） Ｅ_is1・Ｅ_is2＝ｃｏｓα_is1,_is2 （Ｅ_ih1×Ｅ_ih2）・Ｅ_ih3＝γ_ih1,_ih2,_ih3 Ｅ_ih3＝Ｅ_i,_h+1,₁ （ｈ＝０，…ｓ−１） Ｉ_ih2・Ｅ_ih2＝０ （ｈ＝１，…ｓ） Ｑ_ih2k・Ｅ_ih2＞０（ｈ＝１，…ｓ ａｎｄ ｋ＝１，
２） 図１３は、図１２における副縦続剛体回転推定部５２の
構成例を示し、副方程式解導出部５２１および副縦続剛
体回転候補計算部５２２からなり、主縦続剛体姿勢候補
記憶装置４５に記憶されたＥ_i0jの組と副形状パラメー
タ算出部５１で計算されるα_ab、γ_abcおよびＩ_ihjおよ
びＱ_ihjkを入力とし、上記連立方程式あるいは同様の連
立方程式を満たすＥ_ihjの組をすべて出力する構成を示
している。即ち、副方程式解導出部５２１は、与えられ
た入力（Ｅ_i0j、α_ab、γ_abc、Ｉ_ihjおよびＱ_ihjk）を
用いて上述の連立方程式の上の７式から意味のある根を
すべて求める。これには、例えば、次に示す方法により
根を求めることができる。

【００６９】ここで、ｈ≠ｓのとき、Ｅ_ih1＝Ｅ_i,_h-1,₃
が成立する。ｈ＝１，…，ｓに対して、Ｅ_ih1およびＩ
_ih2が既知のとき、次式でＥ_ih2を二義的に求めることが
できる。

【００７０】 Ｅ_ih2＝Ｕ（υ_ih1Ｅ_ih1＋υ_ih2Ｉ_ih2±υ_ih3（Ｅ_ih1×Ｉ_ih2）） （ｇ） ただし、 υ_ih1＝ｃｏｓα_ih1,_ih2 υ_ih2＝−ｃｏｓα_ih1,_ih2（Ｅ_ih1・Ｉ_ih2） υ_ih3 ²＝１−ｃｏｓ²α_ih1,_ih2−（Ｅ_ih1・Ｉ_ih2）² Ｅ_ih1およびＥ_ih2が既知のとき、次式でＥ_ih3を求める
ことができる。

【００７１】 Ｅ_ih3＝Ｕ（ω_ih1Ｅ_ih1＋ω_ih2Ｅ_ih2＋γ_ih1,_ih2,_ih3（Ｅ_ih1×Ｅ_ih2）） （ｈ） ただし、 ω_ih1＝ｃｏｓα_ih1,_ih3−ｃｏｓα_ih2,_ih3ｃｏｓ
α_ih1,_ih2 ω_ih2＝ｃｏｓα_ih2,_ih3−ｃｏｓα_ih1,_ih3ｃｏｓ
α_ih1,_ih2 式（ｇ）および式（ｈ）で求められるベクトルが連立方
程式の解である。

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

【００７３】副続剛体姿勢推定部５３は、既に姿勢候補
（Ｒ_i,_h-1，Ｔ_i,_h-1）の集合が求められている剛体Ｂ_i,
_h-1に隣接する剛体Ｂ_ihに関して、以下のような姿勢候
補（Ｒ_ih，Ｔ_ih）を求める。また、隣接する剛体Ｂ_i,
_h+1についても同様の方法で姿勢候補を求める。

【００７４】図１４は、図１２における副縦続剛体姿勢
推定部５３の構成例を示し、５３１は副縦続剛体平行移
動推定部、５３２は副縦続剛体姿勢検定部である。副縦
続剛体平行移動推定部５３１は、副縦続剛体姿勢候補記
憶装置５４中の有効表示ビットが１である系列のＢ_i,
_h-1の１つの姿勢候補（Ｒ_i,_h-1，Ｔ_i,_h-1）およびこれ
に対応するＢ_ihの回転に対応する回転マトリクスＲ_ihか
ら、平行移動Ｔ_ih＝（Δｘ，Δｙ，Δｚ）を２つの剛体
を接続する関節の回転軸の基準点（座標系Ｓ_i,_h-1にお
ける表示Ｐ_i,_h-1,₃,₁、座標系Ｓ_i0における表示
Ｐ_i,_h,₁,₁）に関する次式により求める。

【００７５】 Ｔ_ih＝Ｐ_i,_h-1,₃,₁Ｒ_i,_h-1−Ｐ_i,_h,₁,₁Ｒ_ih＋Ｔ_i,_h-1 なお、これと等価な方法により平行移動Ｔ_iｈを求める
などのバリエーションも各種考えることができる。副縦
続剛体姿勢検定部５３２は、Ｒ_ihと上述の連立方程式を
満たすＴ_ihとからなる剛体姿勢（Ｒ_ih，Ｔ_ih）の集合の
うち、副指定線分対応付け表３２中の線分種別１および
２および３の線分について矛盾をしない姿勢のみを選択
し出力する。即ち、線分種別１および２および３の該線
分Ｌ_ihjについてそれぞれ以下に示す判定を行い、姿勢
を選択する。線分種別１については、基準点および基準
点と方向ベクトルで定義される任意の１点からなる２点
の３次元位置Ｐ_ihjk（ｋ＝１，２）から算出される推定
画像位置ベクトル ｐ'_ihjk＝（Ｐ_ihjkＲ_ih＋Ｔ_ih）／｜Ｐ_ihjkＲ_ih＋Ｔ_ih
｜ から次式で算出される推定解釈ベクトル Ｉ'_ihj＝ｐ’_ihj1×ｐ'_ihj2／｜ｐ’_ihj1×ｐ'_ihj2｜ と、その線分に対応する画像上の線分上の任意の２点の
画像位置ベクトルｐ_ｉｈｊｋから求められる線分の解釈
ベクトルＩ_ｉｈｊｉの解釈ベクトル一致度Ｉ’_ihj・Ｉ
_ihjがいき値以上である場合、副縦続剛体姿勢候補記憶
装置５４に姿勢候補（Ｒ_ih，Ｔ_ih）の値を格納する。解
釈ベクトル一致度がいき値未満である場合、対応する有
効表示ビットを０にする。線分種別２については端点の
３次元位置Ｐ_ihj1および、端点と方向ベクトルで定義さ
れる任意の１点の３次元位置Ｐ_ihj2から算出される前述
の解釈ベクトル一致度および、端点の推定画像位置ベク
トル ｐ'_ihj1＝（Ｐ_ihj1Ｒ_ih＋Ｔ_ih）／｜Ｐ_ihj1Ｒ_ih＋Ｔ_ih
｜ と、対応する画像上の画像位置ベクトルｐ_ihj1から求め
られる端点一致度ｐ'_ihj1・ｐ_ihj1がいき値以上である
場合、副縦続剛体姿勢候補記憶装置５４に姿勢候補（Ｒ
_ih，Ｔ_ih）の値を格納する。解釈ベクトル一致度あるい
は端点一致度がいき値未満である場合、対応する有効表
示ビットを０にする。また、線分種別３については両端
の２点の３次元位置Ｐ_ihjk（ｋ＝１，２）から算出され
る前述の解釈ベクトル一致度および、両端点の推定画像
位置ベクトル ｐ'_ihjk＝（Ｐ_ihjkＲ_ih＋Ｔ_ih）／｜Ｐ_ihjkＲ_ih＋Ｔ_ih
｜ と、対応する画像上の画像位置ベクトルｐ_ihjkから求め
られる端点一致度ｐ'_ihjk・ｐ_ihjkがいき値以上である
場合、副縦続剛体姿勢候補記憶装置５４に姿勢候補（Ｒ
_ih，Ｔ_ih）の値を格納する。解釈ベクトル一致度あるい
は少なくとも１つの端点一致度がいき値未満である場
合、対応する有効表示ビットを０にする。

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

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

【００７８】

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

【図面の簡単な説明】

【図１】本発明の一実施例を示すブロック図

【図２】（ａ）は上記実施例のカメラ系を示す図、
（ｂ），（ｃ）は上記実施例の入力画像特徴抽出装置で
抽出される特徴点記述と特徴線分記述の例を示す図

【図３】上記実施例で対象とする３次元物体の一例を示
す図

【図４】（ａ），（ｂ），（ｃ）は上記実施例の物体モ
デル記述格納装置における図３の物体についての主物体
縦続接続記述および副物体縦続記述および剛体モデル線
分記述の例を示す図

【図５】（ａ），（ｂ）は上記実施例の物体モデル記述
格納装置における図３の物体についての関節記述および
剛体モデル点記述の例を示す図

【図６】（ａ），（ｂ），（ｃ）は上記実施例の画像・
物体対応付け装置で生成される主指定線分対応付け表お
よび副指定線分対応付け表および指定点対対応付け表の
例を示す図

【図７】上記実施例における主指定線分集合・点対姿勢
推定装置の構成例を示す図

【図８】図７の構成例における主縦続剛体回転推定部の
構成例を示す図

【図９】図７の構成例における基準剛体回転推定部の構
成例を示す図

【図１０】図７における主縦続剛体姿勢推定部の構成例
を示す図

【図１１】図７における主縦続剛体姿勢候補記憶装置の
実現例を示す図

【図１２】上記実施例における副指定線分集合姿勢推定
装置の構成例を示す図

【図１３】図１２における副縦続剛体回転推定部の構成
例を示す図

【図１４】図１２における副縦続剛体姿勢推定部の構成
例を示す図

【図１５】図１２における副縦続剛体姿勢候補記憶装置
の実現例を示す図

【符号の説明】

１…入力画像特徴抽出装置 ２…物体モデル記述格納装置 ３…画像・物体対応付け装置 ４…主指定線分集合・点対姿勢推定装置 ５…副指定線分集合姿勢推定装置 ６…線分集合姿勢決定装置 ４１…主形状パラメータ算出部 ４２…主縦続剛体回転推定部 ４３…基準剛体姿勢推定部 ４４…主縦続剛体姿勢推定部 ４５…主縦続剛体姿勢候補記憶装置 ５１…副形状パラメータ算出部 ５２…副縦続剛体回転推定部 ５３…副縦続剛体姿勢推定部 ５４…副縦続剛体姿勢候補記憶装置

Claims (2)

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