JP3253444B2

JP3253444B2 - ３次元物体位置姿勢推定法

Info

Publication number: JP3253444B2
Application number: JP03433394A
Authority: JP
Inventors: 弘一田中; 直樹武川
Original assignee: Nippon Telegraph and Telephone Corp
Current assignee: Nippon Telegraph and Telephone Corp
Priority date: 1994-03-04
Filing date: 1994-03-04
Publication date: 2002-02-04
Anticipated expiration: 2017-02-04
Also published as: JPH07244737A

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、レンジセンサを用いて
３次元物体の位置姿勢を推定する物体位置姿勢推定法に
関するものである。

【０００２】

【従来の技術】従来、機械部品や工具などの定型の３次
元物体の位置姿勢をレンジデータをもとに決定する手法
として、測定対象の３次元物体の重心位置、重心まわり
のモーメントを計算するものが知られている。すなわ
ち、この手法は、まず計測物体のレンジデータから重心
を求める。この重心を、計算機内部にデータベースとし
て記述されている物体モデルから計算される重心と比較
することにより、物体の３次元空間内での位置（ｘ，
ｙ，ｚ）を決定する。つづいて重心まわりのモーメント
を計測物体、物体モデルそれぞれにおいて計算し、それ
らを比較することにより物体姿勢（θ，φ，α）を決定
する。但し、（ｘ，ｙ，ｚ）は空間に定めた直交座標系
に対しての座標、（θ，φ，α）は、物体の姿勢を表す
パラメータで、球座標（θ，φ）が物体方向を表し、
（α）がその回りでの回転をあらわす。

【０００３】

【発明が解決しようとする課題】上述した従来の方法
は、重心まわりの回転モーメントを用いて姿勢を決定す
るものである。そのため回転モーメントの表現において
方向の特徴がでやすい対象物体、即ちアスペクト比が著
しく異なる形状の物体には適用できるが、それ以外の物
体に対して精度よく姿勢を決定することが難しかった。
また、従来法はレンジセンサに対して物体が見えている
方向を固定としているために、環境変動による物体の３
次元的な姿勢の変化には対応できなかった。

【０００４】本発明は、このような問題点を解決するた
めに為されたものであり、その目的は、３次元的な姿勢
の変動がある場合やアスペクト比が１に近いような形状
の物体でもレンジデータをもとに安定かつ効率的に物体
の位置姿勢の決定を行うことができる、３次元物体位置
姿勢推定法を提供することにある。また、１計測方向か
らのみでは、物体の特徴がつかめないようなセンサと物
体の配置の状況に対しても安定な位置姿勢推定結果を与
えることができる３次元物体位置姿勢推定法を提供する
ことにある。

【０００５】

【課題を解決するための手段】上記の目的を達成するた
め、本発明の３次元物体位置姿勢推定法は、計測された
３次元物体のレンジデータをもとに該３次元物体の物体
位置と物体姿勢を決定する方法において、３次元物体の
計測レンジデータと物体モデルとのマッチングにより該
３次元物体の観測方向の候補を生成する第１の手順と、
第１の手順の結果である観測方向の候補に対してその方
向回りの回転角を推定する第２の手順と、第２の手順の
結果に基づいて物体姿勢の候補を表す回転行列を決定す
る第３の手順と、第３の手順の結果に基づいて各物体姿
勢の候補に対して物体位置を表す３つのパラメータを決
定する第４の手順と、これらの候補の中で評価のために
定義された測度が最高の値をとる候補により物体位置の
３つのパラメータ及び物体姿勢の回転行列を決定する第
５の手順と、を有することを特徴とする。

【０００６】ここで、第１の手順としては、計測レンジ
データから３次元物体のレンジデータヒストグラムを作
成し、物体モデルに基づき事前に作成しておいた多方向
からのレンジデータヒストグラムと前記３次元物体のレ
ンジデータヒストグラムの相関をとり、相関値の高い方
から上位候補を生成するものが好適である。

【０００７】また、第２の手順としては、観測方向回り
の回転角を、事前に作成しておいた物体モデルの角度方
向重心マップと計測レンジデータから求めた角度方向重
心マップとの相関をとり、最大の相関値を与える回転角
により決定するものが好適である。

【０００８】また、第３の手順としては、第１の手順と
第２の手順と座標系間の幾何学的関係式に基づいて各候
補毎に物体姿勢を表す回転行列を決定するものが好適で
ある。

【０００９】また、第３の手順が各候補毎に物体姿勢を
表す回転行列を決定するに際し、上記好適な３次元物体
位置姿勢推定法における第１の手順の相関値と第２の手
順の相関値の積により姿勢候補の確からしさを決定し、
第５の手順としては、前記姿勢候補の確からしさを測度
として物体位置の３つのパラメータ及び物体姿勢の回転
行列を決定するものが好適である。

【００１０】また、第４の手順としては、物体位置を計
測レンジデータの３次元重心と物体姿勢候補の３次元重
心との差から推定するものが好適である。

【００１１】さらに、第１、第２、第３、第４の手順を
レンジセンサの２計測方向に対して行なうことにより、
物体の位置姿勢の候補をそれぞれ作成し、第５の手順が
前記レンジセンサの２計測方向に対しての位置姿勢の候
補間の全てのペアーに対してそれらが幾何学的に両立で
きるかの評価をその評価のために定義された両立性を表
す測度で行ない、この両立性を表す測度の値により物体
位置の３つのパラメータ及び物体姿勢の回転行列を決定
するものが好適である。

【００１２】ここで、第５の手順としては、レンジセン
サの２計測方向に対しての位置姿勢の候補間の全てのペ
アーに対してそれらが幾何学的に両立できるかの評価を
その評価のために定義された両立性を表す測度で行な
い、この両立性を表す測度の値と請求項５記載の３次元
物体位置姿勢推定法における第３の手順で求めた姿勢候
補の確からしさの値との積を計算し、該積を測度として
最高の値をとる物体姿勢の候補のペアーを求め、そのペ
アーから物体位置の３つのパラメータ及び物体姿勢の回
転行列を決定するものが好適である。

【００１３】また、第１、第２、第３、第４の手順につ
いてレンジセンサの計測方向を３以上として物体の位置
姿勢の候補をそれぞれ作成し、第５の手順が前記レンジ
センサの３以上の計測方向に対しての位置姿勢の候補間
の全ての組に対してそれらが幾何学的に両立できるかの
評価をその評価のために定義された両立性を表す測度で
行ない、この両立性を表す測度の値を用いるものが好適
である。

【００１４】

【作用】物体の位置姿勢を決定するためには、物体の３
次元空間での位置３自由度と物体の姿勢３自由度、合計
６自由度を決定する必要がある。しかしながら、物体モ
デルと計測データのマッチングをするためにこの位置姿
勢を表す６自由度空間を直接に探索することは、計算コ
ストおよび計算に要するメモリーの観点からして得策で
ない。本発明では、この６自由度の決定問題を、レンジ
データが捉える観測方向（θ，φ）の２自由度、観測方
向まわりでの回転角（α）の１自由度、３次元位置
（ｘ，ｙ，ｚ）の３自由度決定問題に分解することによ
り計算コストおよび計算に要するメモリーの削減を可能
とする。また、本発明は、２以上の計測方向のレンジデ
ータに対して両立する最適な位置姿勢を推定することに
より、１方向観測レンジデータのみでは安定に推定でき
ないような状況、すなわち、ノイズによるデータの汚れ
がある場合や、位置姿勢を推定するには物体特徴が得ら
れにくいセンサと物体の配置の場合においても安定な位
置姿勢推定結果を与える。

【００１５】

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

【００１６】まず、本発明の原理について説明する。図
１から図５まではそのための説明図であって、図１は計
測センサ座標系と解座標系の関係を示し、図２（ａ），
（ｂ），（ｃ）は３次元データ獲得の概念を示し、図３
は解座標系と前解座標系の関係を示し、４は前解座標系
とモデルセンサ座標系の関係を示し、図５は球面テセレ
ーション法を用いた物体方向サンプリング法の概念を示
している。

【００１７】ワールド座標系（Ｏ→：Ｘ→，Ｙ→，Ｚ
→）内に置かれたモデル物体Ｍｏｄｅｌに対して、観測
物体Ｏｂｊｅｃｔの位置姿勢推定は、図１にあるよう
に、以下の関係にある回転行列（回転マトリックス）Ｒ
と並進ベクトルＴを見つける問題である。

【００１８】

【数１】

【００１９】ただし、文中のＯ→，Ｘ→，Ｙ→，Ｚ→等
の文字・記号は下記の文字・記号を表すものとする（以
下、同じ）。

【００２０】

【数２】

【００２１】本発明では、ノイズの影響にロバストな位
置姿勢の推定を行なうために、図２（ａ）に示すように
２つのレンジセンサを用いて異なる２方向から図２
（ｂ）のような物体を計測し、これらセンサから獲得さ
れる図２（ｃ）のようなレンジデータの両方に両立する
位置姿勢を推定する。

【００２２】まず、座標系と回転行列、並進ベクトルの
間の関係を説明し、位置姿勢候補の生成手続きについて
説明を行なう。図１にあるように計測物体を計測センサ
座標系（ｏ→：ｘ→，ｙ→，ｚ→）_objから見た時と同
じ見え方をするモデル物体を見る座標系を解座標系と呼
び（ｏ→：ｘ→，ｙ→，ｚ→）_solで、図３にあるよう
にＲ^-1Ｔだけ原点を並行移動した座標系を前解座標系と
呼び（ｏ→：ｘ→，ｙ→，ｚ→）_sol′で、そして図４
のように前解座標系（ｏ→：ｘ→，ｙ→，ｚ→）_sol′
と同じｚ軸方向をもつ、モデルのデータベースを作成し
た座標系をモデルセンサ座標系と呼び（ｏ→：ｘ→，ｙ
→，ｚ→）_iで表す。これら座標系間の幾何学的な関係
を用いることにより、回転行列Ｒは、

【００２３】

【数３】

【００２４】で表される。ここで、（２）式中の文字・
記号の意味と各項の定義は以下の通りである。

【００２５】

【数４】

【００２６】また、並進ベクトルＴは、モデルセンサ座
標系（ｏ→：ｘ→，ｙ→，ｚ→）_iで事前に計算された
重心座標（Ｇｘ_i，Ｇｙ_i，Ｇｚ_i）を前解座標系（ｏ
→：ｘ→，ｙ→，ｚ→）_sol′で表示し直したものを
（Ｇｘ_sol′，Ｇｙ_sol′，Ｇｚ_sol′）とし、計測セン
サ座標系（ｏ→：ｘ→，ｙ→，ｚ→）_objで計算された
重心座標（Ｇｘ_obj，Ｇｙ_obj，Ｇｚ_obj）とすれば、

【００２７】

【数５】

【００２８】と求められる。計測センサ座標系は既知で
あるので、レンジデータより抽出される特徴量をもとに
モデルセンサ座標系および前解座標系を求めることが、
位置姿勢を推定するうえで必要である。

【００２９】そのために、物体モデルに対して事前に図
５にあるようにモデル物体を囲む量子化された全方向か
らレンジデータのヒストグラムを計算しておく。このヒ
ストグラムを多重方向ヒストグラムと呼び以下ではｍ−
ヒストグラムと書く。また同様に全方向に対して、その
方向と直角な平面上でのレンジデータの重心の列である
角度方向重心マップ及び３次元重心を計算しておき、ｍ
−ヒストグラムとともにデータベースとして保持する。
このデータベースを用いての物体位置姿勢候補の生成の
手続きは以下のようになる。

【００３０】１）ｍ−ヒストグラムと計測データのヒス
トグラムの相関をとり、その相関値の高い方からソーテ
ィングを行ない上位ｎ量子化方向を決定する。

【００３１】２）上記１）で生成されたモデルセンサ座
標系における上位各方向に対して角度方向重心マップと
計測データの角度方向重心マップとの相関から一番高い
相関値をあたえる回転角度を求め、前解座標系が求ま
る。これにより回転行列Ｒ_i（１≦ｉ≦ｎ）が決定され
る。また、ヒストグラム相関の時の相関値と角度方向重
心マップの時の相関値の積で各候補の姿勢の確からしさ
ｐ_i（１≦ｉ≦ｎ）を定義する。

【００３２】３）上記２）の各物体姿勢に対してデータ
ベースにあるモデルの３次元重心と計測物体の３次元重
心の差から並進ベクトルＴ_i（１≦ｉ≦ｎ）を決定す
る。

【００３３】具体的には、それぞれのセンサに対して、
ｎ個の位置姿勢の候補｛（Ｒ，Ｔ）_k1，（Ｒ，Ｔ）_k2，
…，（Ｒ，Ｔ）_kn｝（ｋ＝１，２）及び各候補の確から
しさ｛ｐ_k1，ｐ_k2，…，ｐ_kn｝（ｋ＝１，２）を生成す
る。こうして生成された位置姿勢候補をもとに、計測物
体の位置姿勢の推定は、２つの位置姿勢ｐｏｓｅ_1i＝
（Ｒ，Ｔ）_1i、ｐｏｓｅ_2j＝（Ｒ，Ｔ）_2jの両立の測度
をｐｏｓｅ＿ｍａｔｃｈ（ｐｏｓｅ_1i，ｐｏｓｅ_2j）で
表したとき、（５）式で定義される測度ｍｅａｓｕｒｅ
（ｐｏｓｅ_1i，ｐｏｓｅ_2j）を最大にするｐｏｓｅ_1i∈
｛（Ｒ，Ｔ）₁₁，（Ｒ，Ｔ）₁₂，…，（Ｒ，Ｔ）_1n｝
と、ｐｏｓｅ_2j∈｛（Ｒ，Ｔ）₁₁，（Ｒ，Ｔ）₁₂，…，
（Ｒ，Ｔ）_1n｝の組み合わせ（ｐｏｓｅ_1i，ｐｏｓ
ｅ_2j）を求め、その平均を計測物体の位置姿勢とするこ
とにより行なう。

【００３４】

【数６】

【００３５】次に、本発明の実施例について処理の流れ
を説明する。図６はその基本的な処理の流れ図である。
先ず、処理はオフライン処理とオンライン処理にわかれ
る。

【００３６】オフライン処理では、物体のモデルをもと
にモデルを囲む球上の各方向に対して、ｍ−ヒストグラ
ム、角度方向重心マップ、３次元重心を計算し、各方向
に対してのこれら３種類の特徴量をデータベースとして
保存する。実際にレンジセンサを用いて対象物を計測し
てデータベースを生成する方法として、ＣＡＤモデルな
どの対象を記述するモデルに対して仮想的なレンジセン
サを計算機上でシミュレーションする方法がある。ここ
では、後者について説明する。図１２は本実施例におけ
る球面テセレーション法を用いた物体方向サンプリング
の説明図、図１３は本実施例におけるヒストグラムの構
成の説明図、本実施例における角度方向重心マップの構
成の説明図である。

【００３７】はじめに、観測方向をサンプリングするた
めに球面テセレーション法を使う。図１２にあるように
球面は正２０面体からはじめて、個々の面である３角形
を４分割していくことにより任意の細かさでほぼ均等に
分割することができる。ここでは、３段階の分割を行な
い全方向を１２８０方向にサンプリングする。こうし
て、サンプリングされた１２８０方向に対してその方向
（θ_i，φ_i）から計測したとき獲得されるレンジデータ
（ｘ_i，_l、ｙ_i，_l、ｚ_i，_l）（１≦ｉ≦１２８０，ｌは
レンジデータ標本点を表す）を計算機シミュレーション
により生成する。こうして得られたレンジデータからｍ
−ヒストグラムｈ_i（１≦ｉ≦１２８０）を以下の式に
従い構成する。

【００３８】

【数７】

【００３９】図１３にあるようにヒストグラムは量子化
された方向ｉをｚ軸とし、ｚ_minとｚ_maxの間の距離をＬ
個の区間に等分し個々の区間に含まれるレンジデータの
個数をカウントしたものである。次に、各方向に対して
３次元重心を以下の式に従い計算する。

【００４０】

【数８】

【００４１】ここで、Ｎ_iはモデルのレンジデータ標本
数である。ｚ軸に垂直は平面上での重心（Ｇｘ_i，Ｇ
ｙ_i）回りの物体形状を表現するために、図１４にある
ように角度方向をＭ個の区間に等分し各区間にたいして
動径方向の重心を計算し、角度方向重心マップｆ_i（１
≦ｉ≦１２８０）を作成する。角度方向重心マップは以
下のように定義される。

【００４２】

【数９】

【００４３】ここで、Ｎ_i，_mは、方向ｍにおけるデータ
個数

【００４４】

【数１０】

【００４５】γ_i，_l、α_i，_lは、レンジデータの極座標
表示、

【００４６】

【数１１】

【００４７】で定義され、（ｘ_i，_l、ｙ_i，_l）は量子化
方向（θ_i，φ_i）のモデルレンジデータ列のｘ−ｙ成分
である。以上がオフライン処理である。

【００４８】次に、オンライン処理について説明する。
オンライン処理は、基本的に図６に示すように、以下の
第１から第５までの手順からなる。すなわち、第１の手
順において、３次元物体の計測レンジデータと物体モデ
ルとのマッチングにより３次元物体の観測方向の候補を
生成し、第２の手順において、第１の手順の結果である
観測方向の候補に対してその方向回りの回転角を推定
し、第３の手順において、第２の手順の結果に基づいて
物体姿勢の候補を表す回転行列を決定し、第４の手順に
おいて、第３の手順の結果に基づいて各物体姿勢の候補
に対して物体位置を表す３つのパラメータを決定し、第
５の手順において、これらの候補の中で評価のために定
義された測度が最高の値をとる候補により物体位置の３
つのパラメータ及び物体姿勢の回転行列を決定し、３次
元物体の位置姿勢を推定する。図７から図１１までは上
記の各手順のより詳しい処理の流れ図である。

【００４９】まず、２つのレンジセンサで異なる方向か
ら観測物体を計測することによりレンジデータ
（ｘ_obj，_l、ｙ_obj，_l、ｚ_obj，_l）（ｋ＝１，２、ｋは
２つのレンジセンサを表す）を獲得する。獲得されたレ
ンジデータをもとに、第１の手順ではヒストグラムを、
第２の手順では重心、角度方向重心マップを計算する。
これらの量はオフラインでは１２８０方向に対してそれ
ぞれ計算されたが、このオンライン処理では実際のレン
ジセンサ方向のみの計算であることを注意しておく。２
つのセンサから得られるレンジデータそれぞれに対し
て、以下の処理により、観測物体の位置姿勢候補の生成
を行う。以下では、レンジセンサのラベルであるｋを省
略する。

【００５０】第１の手順では図７に示すように、先ず、
レンジデータのヒストグラムｈ_objを（６）式のように
計算する。こうして計算された、ヒストグラムｈ_objと
オフラインで計算していた多重方向ヒストグラムｈ_iと
の間の相関を次式の相関関数ｈ＿ｍａｔｃｈにより評価
する。これは観測方向ｉと奥行き方向ｚに対して一致す
る候補を求めるものである。

【００５１】

【数１２】

【００５２】ここで、ｈ_objとｈ_iは以下の意味で正規化
しておく。

【００５３】

【数１３】

【００５４】各方向ｉ毎にｓｈｉｆｔを変数としてｈ＿
ｍａｔｃｈ（ｉ，ｓｈｉｆｔ）の値がが最大になるｓｈ
ｉｆｔを求め、そのときの相関値をＣ₁（ｉ）とする。
この関数Ｃ₁（ｉ）を値が高い順にソーティングを行な
い量子化された観測方向の上位ｎ候補を生成する。

【００５５】次に、第２の手順では、図８に示すように
３次元重心を次式により計算して求める。

【００５６】

【数１４】

【００５７】ここで、Ｎ_objは、観測レンジデータ標本
数である。この３次元重心の計算結果を用い、角度方向
重心マップを次式で計算を行なう。

【００５８】

【数１５】

【００５９】ここで、Ｎ_obj，_mは、方向ｍにおけるデー
タ個数

【００６０】

【数１６】

【００６１】γ_obj，_l、α_obj，_lは、レンジデータの極
座標表示、

【００６２】

【数１７】

【００６３】で定義され、（ｘ_obj，_l、ｙ_obj，_l）は計
測レンジデータ列のｘ−ｙ成分、（Ｇｘ_obj，Ｇｙ_obj）
は重心を表す。この角度方向マップｆ_objとデータベー
スのある上位ｎ候補の角度方向重心マップｆ_iを

【００６４】

【数１８】

【００６５】で定義される相関関数ｆ＿ｍａｔｃｈで評
価を行ない、各上位ｎ個の量子化方向毎に一番高い相関
値を与える回転角ｍを決定する。この時の相関値をＣ₂
（ｉ）とする。ここでＢは回転方向重心マップの平均値
を０にするために、Ｃは自己相関値が１になるように規
格化するために入れた定数である。

【００６６】次に、第３の手順では、図９に示すように
各方向毎に決定された回転角ｍから各候補に対して前解
座標系を求め、（２）式を使うことにより姿勢を表す回
転行列｛Ｒ₁，Ｒ₂，…，Ｒ_n｝を求める。また、ヒスト
グラム相関の相関値Ｃ₁（ｉ）と角度方向重心マップの
相関値Ｃ₂（ｉ）の積より物体位置姿勢の確からしさｐ_i
を計算する。

【００６７】ｐ_i≡Ｃ₁（ｉ）×Ｃ₂（ｉ）（１５）また、第４の手順では、図１０に示すように物体の３次
元重心（Ｇｘ_obj，Ｇｙ_obj，Ｇｚ_obj）と物体モデルの
３次元重心の差から（４）式を使うことにより並進ベク
トル｛Ｔ₁，Ｔ₂，…，Ｔ_n｝を求める。

【００６８】以上の手続きにより、物体位置姿勢候補
｛（Ｒ，Ｔ）₁，（Ｒ，Ｔ）₂，…，（Ｒ，Ｔ）_n｝とそ
の確からしさ｛ｐ₁，ｐ₂，…，ｐ_n｝の生成が完成す
る。この手続きを２つの計測レンジデータに対して行な
い、物体位置姿勢候補｛（Ｒ，Ｔ）_k1，（Ｒ，Ｔ）_k2，
…，（Ｒ，Ｔ）_kn｝（ｋ＝１，２）とその確からしさ
｛ｐ_k1，ｐ_k2，…，ｐ_kn｝（ｋ＝１，２）を生成する。

【００６９】次に、第５の手順では、図１１に示すよう
にこの物体位置姿勢候補を用いて観測物体の位置姿勢を
推定する。先ず、（５）式で定義される評価式において
ｐｏｓｅ＿ｍａｔｃｈ（ｐｏｓｅ_1i，ｐｏｓｅ_2j）を以
下のように定義する。

【００７０】

【数１９】

【００７１】ここで、ｐｏｓｅ＿ｓｉｇは定数、Ｔｒは
行のトレース、Ｉは単位行列を表す。ｍｅａｓｕｒｅ
（ｐｏｓｅ_1i，ｐｏｓｅ_2j）を最大にするｐｏｓｅ_1i∈
｛（Ｒ，Ｔ）₁₁，（Ｒ，Ｔ）₁₂，…，（Ｒ，Ｔ）_1n｝
と、ｐｏｓｅ_2j∈｛（Ｒ，Ｔ）₁₁，（Ｒ，Ｔ）₁₂，…，
（Ｒ，Ｔ）_2n｝の組み合わせ（ｐｏｓｅ_1i，ｐｏｓ
ｅ_2j）を求める。このｐｏｓｅ_1i＝（Ｒ，Ｔ）_1iとｐｏ
ｓｅ_2j＝（Ｒ，Ｔ）_2jから並進ベクトルＴは平均値を計
算して、

【００７２】

【数２０】

【００７３】により求める。回転行列はある軸回りの回
転で表現でき、この回転軸を方向として回転角の大きさ
とするようなベクトルを考えそれをその回転行列の回転
ベクトルとよぶ。そこで、この回転行列に回転ベクトル
を対応させる関数をＦで、回転ベクトルにその回転行列
を対応させる関数をＦ^-1で定義する。この定義をもと推
定する回転行列Ｒ_1iとＲ_2iの平均である回転行列Ｒは

【００７４】

【数２１】

【００７５】と推定される。このようにして回転行列Ｒ
および並進ベクトルＴを推定し、このことより観測物体
の位置姿勢が推定される。

【００７６】なお、上記の実施例では、レンジセンサに
よる計測方向を２つとした場合について説明したが、レ
ンジセンサによる計測方向を３以上に拡張することは同
様にして容易に実施できることである。ただし、計測方
向が増えれば物体の位置姿勢の推定精度は向上するが、
それだけ処理時間が増大するので、そのトレードオフを
考慮する必要がある。また、上記の実施例では、位置姿
勢の推定において、評価のための測度として、位置姿勢
の確からしさとレンジセンサの２計測方向に対しての位
置姿勢の候補間の全てのペアーに対してそれらが幾何学
的に両立できるかの評価のために定義された両立性を表
す測度との積を用いた例を示したが、推定の精度が少し
低くても良い場合には、そのいずれかを用いても良い。
すなわち、レンジセンサの２計測方向に対しての位置姿
勢の候補間の全てのペアーに対してそれらが幾何学的に
両立できるかの評価のために定義された両立性を表す測
度だけを用いても、第３の手順で求めた姿勢候補の確か
らしさの値だけを測度として用いても良い。

【００７７】

【発明の効果】以上述べたように、本発明は、３次元物
体の位置姿勢の６自由度決定を、観測方向の２自由度、
観測方向まわりの物体回転角１自由度、物体位置の３自
由度の決定に分解して決定を行うことにより、計算コス
トおよび計算に要するメモリーを削減する効果がある。
また、姿勢推定のマッチングにおいてヒストグラムと角
度方向重心マップの相関を用い、従来のモーメント法で
は決定することが難しかったアスペクト比が１に近い物
体に対しても安定した推定値を与える効果がある。

【００７８】また、請求項２の発明によれば、特に、モ
デルデータから生成された各物体方向からのヒストグラ
ムと計算データのヒストグラムの相関をとるために、非
凸物体に対して発生するセルフオクルージョンのある計
測データに対しても適用可能である。

【００７９】また、請求項７の発明によれば、特に、２
方向から観測されたレンジデータに対しての最適な位置
姿勢を推定するので、１観測方向からのみでは位置姿勢
を安定して推定することが難しいセンサと物体の配置の
状況においても適用可能である。

【００８０】また、請求項８の発明によれば、特に、位
置姿勢推定の評価の測度として、２観測方向のレンジデ
ータに対する両立性の評価測度と姿勢候補の確からしさ
の積を用いているので、請求項７の発明に加えて、推定
の精度を一層高めることができる。

【００８１】さらに、請求項９の発明によれば、特に、
３以上の方向から観測されたレンジデータに対しての最
適な位置姿勢を推定するので、処理時間はかかるもの
の、より一層推定の精度を向上させることができる。

【図面の簡単な説明】

【図１】本発明の原理を説明するための計測センサ座標
系と解座標系の説明図

【図２】（ａ），（ｂ），（ｃ）は本発明の原理を説明
するための２つのレンジセンサによる３次元データの獲
得を示す概念図

【図３】本発明の原理を説明するための解座標系と前解
座標系の説明図

【図４】本発明の原理を説明するための前解座標系とモ
デルセンサ座標系の説明図

【図５】本発明の原理を説明するための球面テセレーシ
ョン法を用いた物体方向サンプリング法の概念図

【図６】本発明の一実施例を示す処理流れ図

【図７】上記実施例の第１の手順の詳細を示す処理流れ
図

【図８】上記実施例の第２の手順の詳細を示す処理流れ
図

【図９】上記実施例の第３の手順の詳細を示す処理流れ
図

【図１０】上記実施例の第４の手順の詳細を示す処理流
れ図

【図１１】上記実施例の第５の手順の詳細を示す処理流
れ図

【図１２】上記実施例における球面テセレーション法を
用いた物体方向サンプリングの説明図

【図１３】上記実施例におけるヒストグラムの構成の説
明図

【図１４】上記実施例における角度方向重心マップの構
成の説明図

───────────────────────────────────────────────────── フロントページの続き (56)参考文献特開平３−150402（ＪＰ，Ａ) 特開昭61−129508（ＪＰ，Ａ) 田中弘一、武川直樹，３次元物体同定、位置姿勢推定のための多重方向ヒストグラム法，情報処理学会研究報告ＣＶ −85，日本，情報処理学会，1993年９月30日，Ｖｏｌ．93，Ｎｏ．87，ｐ．25 −32 (58)調査した分野(Int.Cl.⁷，ＤＢ名) G06T 1/00 G06T 7/00 - 7/60 G01B 11/00 - 11/30

Claims

(57)【特許請求の範囲】

【請求項１】計測された３次元物体のレンジデータを
もとに該３次元物体の物体位置と物体姿勢を決定する方
法において、３次元物体の計測レンジデータと物体モデ
ルとのマッチングにより該３次元物体の観測方向の候補
を生成する第１の手順と、第１の手順の結果である観測
方向の候補に対してその方向回りの回転角を推定する第
２の手順と、第２の手順の結果に基づいて物体姿勢の候
補を表す回転行列を決定する第３の手順と、第３の手順
の結果に基づいて各物体姿勢の候補に対して物体位置を
表す３つのパラメータを決定する第４の手順と、これら
の候補の中で評価のために定義された測度が最高の値を
とる候補により物体位置の３つのパラメータ及び物体姿
勢の回転行列を決定する第５の手順と、を有することを
特徴とする３次元物体位置姿勢推定法。
【請求項２】第１の手順が、計測レンジデータから３
次元物体のレンジデータヒストグラムを作成し、物体モ
デルに基づき事前に作成しておいた多方向からのレンジ
データヒストグラムと前記３次元物体のレンジデータヒ
ストグラムの相関をとり、相関値の高い方から上位候補
を生成することを特徴とする請求項１記載の３次元物体
位置姿勢推定法。
【請求項３】第２の手順が、観測方向回りの回転角
を、事前に作成しておいた物体モデルの角度方向重心マ
ップと計測レンジデータから求めた角度方向重心マップ
との相関をとり、最大の相関値を与える回転角により決
定することを特徴とする請求項１記載の３次元物体位置
姿勢推定法。
【請求項４】第３の手順が、第１の手順と第２の手順
と座標系間の幾何学的関係式に基づいて各候補毎に物体
姿勢を表す回転行列を決定することを特徴とする請求項
１記載の３次元物体位置姿勢推定法。
【請求項５】第３の手順が各候補毎に物体姿勢を表す
回転行列を決定するに際し、請求項２記載の３次元物体
位置姿勢推定法における第１の手順の相関値と請求項２
記載の３次元物体位置姿勢推定法における第２の手順の
相関値の積により姿勢候補の確からしさを決定し、第５
の手順が前記姿勢候補の確からしさを測度として物体位
置の３つのパラメータ及び物体姿勢の回転行列を決定す
ることを特徴とする請求項１記載の３次元物体位置姿勢
推定法。
【請求項６】第４の手順が、物体位置を計測レンジデ
ータの３次元重心と物体姿勢候補の３次元重心との差か
ら推定することを特徴とする請求項１記載の３次元物体
位置姿勢推定法。
【請求項７】第１、第２、第３、第４の手順をレンジ
センサの２計測方向に対して行なうことにより、物体の
位置姿勢の候補をそれぞれ作成し、第５の手順が前記レ
ンジセンサの２計測方向に対しての位置姿勢の候補間の
全てのペアーに対してそれらが幾何学的に両立できるか
の評価をその評価のために定義された両立性を表す測度
で行ない、この両立性を表す測度の値により物体位置の
３つのパラメータ及び物体姿勢の回転行列を決定するこ
とを特徴とする請求項１記載の３次元物体位置姿勢推定
法。
【請求項８】第５の手順が、レンジセンサの２計測方
向に対しての位置姿勢の候補間の全てのペアーに対して
それらが幾何学的に両立できるかの評価をその評価のた
めに定義された両立性を表す測度で行ない、この両立性
を表す測度の値と請求項５記載の３次元物体位置姿勢推
定法における第３の手順で求めた姿勢候補の確からしさ
の値との積を計算し、該積を測度として最高の値をとる
物体姿勢の候補のペアーを求め、そのペアーから物体位
置の３つのパラメータ及び物体姿勢の回転行列を決定す
ることを特徴とする請求項７記載の３次元物体位置姿勢
推定法。
【請求項９】第１、第２、第３、第４の手順について
レンジセンサの計測方向を３以上として物体の位置姿勢
の候補をそれぞれ作成し、第５の手順が前記レンジセン
サの３以上の計測方向に対しての位置姿勢の候補間の全
ての組に対してそれらが幾何学的に両立できるかの評価
をその評価のために定義された両立性を表す測度で行な
い、この両立性を表す測度の値を用いることを特徴とす
る請求項７または請求項８に記載の３次元物体位置姿勢
推定法。