JP4383230B2

JP4383230B2 - ３次元物体照合装置

Info

Publication number: JP4383230B2
Application number: JP2004108990A
Authority: JP
Inventors: 晴久奥田; 橋本　　学
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2004-04-01
Filing date: 2004-04-01
Publication date: 2009-12-16
Anticipated expiration: 2024-04-01
Also published as: JP2005293350A

Description

本件発明は、３次元物体の計測データを既知モデルを対比することにより位置姿勢推定や物体識別を行う３次元物体照合装置に関し、特に、計測誤差を考慮して高速かつ高精度の位置姿勢推定や物体識別を可能とする３次元物体照合装置に関する。

近年、センサによって計測した３次元データ（レンジデータ又は距離画像）に基づいて物体の位置姿勢推定や物体識別を行う３次元物体照合技術が注目されている。この３次元データを、既知の物体モデルと対比することにより位置姿勢推定や物体識別を行うことができる。３次元物体照合技術には様々なものがある。

例えば、特開平６−１０９４５１号公報（特許文献１）では、計測したレンジデータを物体モデルと照合するために、事前に物体モデルの見え方に関するデータ表（３次元アスペクト表）を生成しておき、レンジデータから抽出した３次元特徴量によって３次元アスペクト表を参照して視方向を決定し、その視方向と既知の視点と物体モデルとから３次元物体の姿勢を推定する。また、特開平１０−１２４６７７号公報（特許文献２）では、計測したレンジデータから平面と円筒面を検出し、この検出結果と基準配置モデルを参照して姿勢候補を選択する。姿勢候補の選択には、一般化ハフ変換等の多数決原理を用いている。しかしながら、これらの３次元物体照合技術では、予め生成したアスペクト表や姿勢候補の中から最もよく合致する姿勢を選択するため、高精度の姿勢推定を行うことは困難である。

一方、Ｂｅｓｌ等の提唱するＩＣＰアルゴリズム（非特許文献１）では、レンジデータから特徴量を抽出する代わりに、ともに点群として表されるレンジデータと物体モデルデータの間で、一方の形状データの各点から他方の形状データの最も近い点までの距離の２乗和を最小化することにより、２つの形状データが最も良く重なるような座標変換を得る。また清水等（非特許文献２）は、ＩＣＰアルゴリズムにおいてセンサの視線方向に生じる計測誤差を考慮して、計測誤差の生じる範囲を誤差の標準偏差σとして規定し、σに応じた適合度を与えることにより計測誤差の影響を考慮した照合を行っている。

特開平６−１０９４５１号公報特開丙１０−１２４６７７号公報 B.J.Besl等、「A method for registration 3-D shapes」、IEEE Transactions on Pattern Analysis and Machine Intelligence、vol.14、no.2、pp.239-256、1992 清水、出口等、「計測誤差を考慮した距離画像からの精密な姿勢推定」、電子情報通信学会論文誌D-II、No12、pp2298-2306、1999年

ＩＣＰアルゴリズムのように点群同士の照合を行えば照合精度を高めることができるが、大量のデータ点数を照合に用いる必要があり、高速の照合が行えないという問題が生じる。また、照合精度を高めるためにはセンサによって生じる計測誤差の影響も考慮する必要があるが、清水等（非特許文献２）の提案する方法では単一の視線方向に生じる計測誤差しか考慮されておらず、物体の形状によっては十分な照合精度を得ることができない。

そこで本件発明は、実用性の高い照合性能を実現するために、センサによる計測誤差の影響を低減しながら、高速かつ高精度の照合を可能とする３次元物体照合装置を提供することを目的とする。

上記目的を達成するために、本件発明に係る３次元物体照合装置は、物体の立体形状を計測して３次元データを得る３次元データ取得手段と、取得した３次元データを蓄積する３次元データ蓄積手段と、取得した３次元データと参照すべき標準３次元データとの照合を行う３次元データ照合手段とを備えた３次元物体照合装置であって、さらに、前記３次元データの各計測点について計測誤差を推定する誤差特性評価手段と、前記３次元データから１以上のデータ領域を切り出し、所定の形状特徴値が閾値よりも大きなデータ領域を初期候補として選定する特徴部分抽出手段と、前記データ領域又は前記データ領域同士の組合せについて推定処理時間と推定照合精度を計算する処理時間・精度評価手段と、前記初期候補から、目標処理時間と目標照合精度を達成するデータ領域又はデータ領域同士の組合せを選定する照合データ領域選定手段とを備え、前記３次元データ照合手段が、前記照合データ領域選定手段の選定したデータ領域又はデータ領域同士の組合せについて標準３次元データとの照合を行うことを特徴とする。

本件発明に係る３次元物体照合装置によれば、照合に適した特徴を持つ１以上のデータ領域を初期候補として選定し、さらに、データ領域中の各計測点の計測誤差から推定した照合精度と、データ領域の大きさ等に基づいて推定した処理時間とをもとに照合に最も適したデータ領域又はデータ領域同士の組合せを選定するため、従来よりも少ないデータ点数で照合を行っても照合精度を低下させることがなく、高速かつ高精度の照合が可能となる。

実施の形態１
以下の実施の形態では、本件発明に係る３次元物体照合装置を位置姿勢推定に応用する場合を例に説明する。図１は、本発明の実施の形態１に係る３次元物体照合装置の概略を示す模式図である。３次元物体照合装置では、センサ等の３次元データ取得手段６によってある視線方向から見た対象物体４の３次元データを取得し、その計測した３次元データを既知の３次元データ（標準３次元データ）と照合することにより、対象物体４の位置姿勢推定を行う。図示するように、センサ６によって取得した３次元データは、メモリ等の３次元データ蓄積手段８に蓄積され、照合データ領域選定手段１０において照合に用いるデータ領域が選定され、３次元データ照合手段１２において標準３次元データとの照合が行われた後、照合結果が出力される。計測した３次元データを表すセンサ座標は任意であるが、例えば、センサ６の視線方向をｚ軸とし、それに直交する平面内にｘ軸とｙ軸を取る直交座標をセンサ座標として用いることができる。位置姿勢の推定精度を高めるために複数の方向から物体４の計測を行うこともあるが、その場合には統一したセンサ座標系に変換して標準３次元データとの照合が行われる。

図２は、図１に示す３次元物体照合装置のより詳細な処理構成を示すブロック図である。図２に示すように、照合データ領域選定手段１０は、下位モジュールとして誤差特性評価手段１４と、特徴部分抽出手段１６と、処理時間・精度評価手段１８を有する。誤差特性評価手段１４では、センサ６と計測点の空間上の位置関係や対象物体４の表面特性（材質、形状）に基づいて各計測点における誤差の推定標準偏差の空間分布（＝誤差特性）を算出する。特徴部分抽出手段１６では、計測データ中の距離変化の大きな点や曲率の大きな点を選定し、照合処理の際に用いるべきデータ領域の初期候補の抽出処理を行う。処理時間・精度評価手段１８では、先に選定されている初期候補のデータ領域の組合せにより達成される処理時間を推定すると共に、そのデータ領域を用いた際の推定精度を先に算出しておいた誤差特性に基づいて評価する。照合データ領域選定手段１０は、これらの下位モジュールの動作を制御して必要な情報を得ることにより、各方向別の目標照合精度や目標の処理時間を実現するのに必要なデータ領域が決定される。ここで選定されたデータ領域を用いて３次元照合を行うことにより、高速かつ高精度の照合が可能となる。また、過去に選定したデータ領域とその照合結果は照合特性蓄積手段２０に蓄積され、照合データ領域選定手段１０における以降の選定処理に活用される。

図３は照合の処理フローを示すフロー図である。以下、このフロー図に従って各構成における処理の詳細について説明する。
（３次元データ取得）
まず、３次元データ取得手段６によって対象物体４の３次元立体形状を表す３次元データが取得される（ステップ３０）。この３次元データ取得手段６としては、各種の３次元距離測定手段を用いることができ、例えば、２つ以上のカメラを用いて三角測量原理により距離分布計測を行うステレオ方式、パターン投光を行い１つ以上のカメラで観測して三角測量原理に基づいて計測を行う構造化光方式、レーダー光の出射から反射光の受光までの時間を計測する飛行時間法などの各種レンジファインダを用いることができる。３次元データ取得手段６によって得られるデータは、一般にある視点方向から物体４を見たときの距離画像であり、部分的な３次元データである。３次元データは、センサ６を動かしながら複数の視点から計測したデータを複合したものであっても良い。

（３次元データ蓄積）
次に、３次元データ取得手段６によって取得した３次元データを３次元データ蓄積手段８に蓄積する（ステップ３２）。３次元データ蓄積手段８は、計測した３次元データを記憶可能なメモリであれば良く、特に種類は問わない。また、３次元データ蓄積手段８には、計測した３次元データの他に、照合すべき対象物体４の標準３次元データも蓄積されている。標準３次元データは、既知の姿勢の物体４を計測することによって得た３次元データでも良いし、ＣＡＤ設計情報などから生成された３次元データであっても良い。

（照合データ領域選定）
次に、照合データ領域選定手段１０が、下位動作モジュールである誤差特性評価手段１４、特徴部分抽出手段１６、処理時間・精度評価手段１８の動作制御を行って総合的な評価を行った上でデータ領域選定を行う（ステップ３４〜４２）。これにより座標の各方向別の目標照合精度や目標の処理時間を実現するのに必要なデータ領域が選定される。以下、３つの下位動作モジュールについて、その動作をフロー順に説明していき、次いでこれらを総合的に評価すると共に動作制御を行う照合データ領域選定手段の動作について述べる。なお、３つの下位動作モジュールは基本的には図３に示すようにシーケンシャルに処理されるが、照合データ領域選定手段１０で適切なデータ領域の選定ができなかった場合には、特徴部分抽出手段１６での閾値パラメータを変更するなどして、目標の位置姿勢推定精度や処理時間を達成するように1回以上動作する構成となっている。

−誤差特性評価
まず、誤差特性評価手段１４では、３次元データ蓄積手段８に蓄積された３次元データの各計測点について誤差特性の評価を行う（ステップ３４）。ここで評価する誤差特性は、各計測点が持つ計測誤差の空間的な分布を評価したものであり、例えばセンサ座標の各座標軸方向における計測誤差の推定標準偏差として表される。ここで評価する誤差特性の種類は特に限定されないが、例えば、センサ６の構造によって決まる誤差特性や、対象物体４の表面反射特性（材質、形状）によって決まる誤差特性などが挙げられる。

センサ６の構造によって決まる誤差特性について図４を参照しながら説明する。図４に示すように、センサ（＝３次元データ取得手段）６として構造化光方式のものを用いた場合、センサ６は主としてパターン投光器２２と受光カメラ部２４から構成され、両者はベースラインと呼ばれる一定距離Ｌ_０だけ離して配置される。ここでは図面正面から見て左側にパターン投光部２２が配置され、右側に受光カメラ部２４が配置された場合を考える。センサ６の構造に由来する計測誤差の大きさは、対象物体４の計測点とセンサ６との空間的な位置関係に依存して変化する。即ち、このセンサ６では、受光カメラ２４のカメラ中心２４ａから計測点に向かって延びる線分をＬ_１とし、パターン投光器２２の投光器中心２２ａから計測点に向かって照射方向に延びる線分をＬ_２とすると、計測点は線分Ｌ_１と線分Ｌ_２の交差点２６又は２６’となり、ベースラインＬ_０と線分Ｌ_１と線分Ｌ_２から構成される三角形から三角測量の原理に基づいて計測点までの距離が算出される。通常、線分Ｌ_１及びＬ_２は空間的な幅を持った線分であるため、その交差点２６又は２６’はある面積をもったひし形領域となり、この領域の広がり分だけ距離の計測誤差が生じることになる。この交差領域の広がりは計測点とセンサ６の空間的な位置関係によって変化する。

例えば、カメラ中心２４ａ及びパターン投光器中心２２ａのいずれに対しても右側または左側に計測点がある場合、図４左側に示すようにＬ_１及びＬ_２の交差角度が鋭角となり、線分Ｌ_１と線分Ｌ_２の交差領域２６は縦長のひし形となる。この計測誤差の誤差ベクトルは、例えば、ひし形の２本の対角線ベクトルによって近似することができるため、特定の方向（この場合は線分Ｌ_１方向）への誤差ベクトルが特に大きくなる。一方、計測点がカメラ中心２４ａと投光器中心２２ａの中間に位置する場合には、線分Ｌ_１と線分Ｌ_２は前述の場合よりも鈍い角度で交わるため、長方形に近いひし形領域２６’となるか、少なくとも前述の場合よりも緩い角度のひし形領域２６’となる。また、ひし形領域２６’は前述の場合のひし形領域２６よりも小さくなる。このためひし形領域２６’の２つの対角線ベクトルによって近似した誤差ベクトルは、前述の場合の誤差ベクトルよりも小さく、また２つの誤差ベクトルの大きさがほぼ等しくなるため特定の方向への依存性も少なくなる。評価した各計測点における誤差ベクトルを、所定の座標系の各座標軸に射影することによって、センサ６の構造の由来する計測誤差の大きさを各座標軸方向ごとに評価することができる。具体的には、２本の対角線ベクトルの座標軸への射影成分を座標軸ごとに足し合わせれば、各方向における計測誤差の大きさを評価することができる。

このセンサ構造に由来する計測誤差は、センサ６の構造に関する情報（上記例では線分Ｌ_１とＬ_２の幅）とセンサ６に対する空間上の位置（センサ空間での座標）から計算することができる。センサ構造に由来する計測誤差をセンサ空間での各座標軸について予め計算しておき、センサによって計測した各計測点の座標に対応付ければ、センサ構造に由来する誤差特性の評価を高速に行うことができる。

次に、対象物体４の表面反射特性（材質、形状）によって決まる誤差特性の評価について説明する。対象物体４の材質や形状は様々であり、それに応じてパターン投光器２２から対象物体４に照射された光が受光カメラ２４方向に反射する反射割合が変化する。この反射割合が小さいほど、受光カメラ２４で感知する計測信号のＳ／Ｎ比が低下するため計測誤差が大きくなる。例えば、対象物体４の計測点が反射率の低い素材である場合には、受光カメラ２４側への帰還光成分が少なくなり、計測信号のＳ／Ｎ比が低下することで計測誤差が大きくなる。また、対象物体４の計測点における面方位が投光器２２に正対するような配置となる場合も、受光カメラ２４側への帰還光成分が少なくなり、Ｓ／Ｎ比が低下することで計測誤差が大きくなる。対象物体４の面方位の影響は、対象物体の表面が鏡面反射性である場合に特に大きい。逆に、対象物体の表面が完全拡散面である場合には対象物体４の面方位の影響は小さくなる。

この対象物体４の表面反射特性（材質、形状）によって決まる計測誤差は、例えば、対象物体４を置いたときに受光カメラ２４で受光した輝度信号から対象物体４を置かないときに受光カメラ２４で受光した輝度信号を減算することによって簡単に評価することができる。即ち、対象物体４を置いたときと置かないときの輝度信号の差は、対象物体４から受光カメラ２４に向かって反射した光の割合に相当するため、これが大きいほど計測信号のＳ／Ｎ比が大きくなり、計測誤差が小さくなる。このようにして評価した計測誤差は、簡易的には等方的に発生すると近似できる。従って、前述のセンサ構造由来の誤差特性に加えて対象物体４の表面反射特性によって決まる誤差特性を考慮する場合、センサ構造に基づく計測誤差のｘ、ｙ、ｚ各成分に対して、表面反射特性から導いた計測誤差に相当する係数を乗算することにより表面反射特性よる計測誤差の影響を加味することができる。

−特徴部分抽出
次に特徴部分抽出手段１６では、計測した３次元データの中から照合に適したデータ領域を抽出する（ステップ３６）。例えば、３次元データから予め決められた標準的なサイズのデータ領域を切り出し、各データ領域について所定の形状特徴値を算出した上で、形状特徴値が閾値を超えるデータ領域を初期候補として選択する。ここで形状特徴値は、照合に適したデータ領域を判断できる指標となるものであれば良い。例えば、あるデータ領域が形状的にユニークであったり、そのデータ領域の濃淡テクスチャがユニークであれば照合に適していると言える。従って、形状特徴値の具体例としては、そのデータ領域における距離データの分散、曲率の分散、曲率（それ自体）、ヒストグラム分布、ヒストグラムの平均や標準偏差、空間周波数（フーリエ変換の係数等）等が挙げられる。

例えば、距離データの分散を形状特徴値とする場合について説明する。一般に計測した３次元データ（距離データ）中で距離変化の大きな領域は、空間的ユニーク性が高いため、位置姿勢推定や物体識別に有効な部分となることが知られている。距離の変化が大きなデータ領域を選定するには、例えば、以下の選定アルゴリズム１ような処理を行えば良い。即ち、Step1では、センサの受光カメラ２４からの視線方向に垂直な面内で等間隔に区切った８方向（２２．５度間隔）を設定し、各方向について距離データの分散値を算出する。そして、Step２において、各方向のデータ分散のうち最小のものが閾値を越えていれば初期候補として選定する。
＜選定アルゴリズム１＞
Step1 ３次元計測データ中、等間隔に区切った8方向について各方向別の距離データ分散値算出
Step2 if(8方向中最も小さな分散値＞閾値) then 選択

次に、曲率の分散を形状特徴値とする場合について説明する。曲率の変化が大きなデータ領域を選定するには、例えば、以下の選定アルゴリズム２のような処理を行えば良い、即ち、Step１では、選定アルゴリズム１と同様に等間隔に区切った８方向について各方向における法線単位ベクトルの分散値を算出する。次に、Step２において、各方向のデータ分散のうち最小のものが閾値を越えていれば初期候補として選定する。
＜選定アルゴリズム２＞
Step1 ３次元計測データ中、等間隔に区切った8方向について、各方向の法線成分の分散値算出
Step2 if (8方向中最も小さな変化値の分散値＞閾値) then 選択

ここでデータ分散の最小値を代表値として閾値と比較しているが、データ分散の代表値の取り方は任意であり、例えばデータ分散の平均値を閾値と比較しても良い。但し、データ分散の最小値を代表値とすれば、照合に不適切な領域の誤選定を防止するのに有効である。例えば、データ分散の最大値を閾値を比較した場合、全体として照合に不適切であるが、ある一方向だけが偶然にデータ分散が大きな領域が誤って選定される場合がある。尚、データ分散を評価する方向は８方向には限定されず、対象物体の形状に合わせて適宜選択すれば良い。評価方向を設定する面も、視線方向に垂直な面には限定されず、例えば対象物体のデータ領域の平均的な法線に垂直な面であっても良い。

−処理時間・精度評価
次に、処理時間・精度評価手段１８において、特徴部分抽出手段１６で選定されたデータ領域の初期候補から各種組合せに対する推定照合精度と推定処理時間算出を行う（ステップ３８）。まず、推定照合精度は、先の誤差特性評価手段１４で評価した誤差特性に基づいて算出できる。例えば、誤差特性評価手段１４によって各計測点における誤差ベクトル、即ち、ｘ、ｙ、ｚ方向の計測誤差の推定標準偏差を見積もっているため、データ領域内の全ての誤差ベクトルを合成すれば、データ領域全体が示す計測誤差をｘ、ｙ、ｚ方向について知ることができる。ここでデータ領域の大きさがそれぞれ異なる場合には、合成した誤差ベクトルをデータ領域内の計測点数で割って平均誤差ベクトルを求めても良い。また、データ領域内の各計測点が持つ誤差ベクトルの分散が、小さいほうが照合精度が良好となる傾向にある。そこで誤差ベクトルの分散を計算して照合精度の指標としても良い。この分散は、誤差ベクトルのｘ、ｙ、ｚ成分の各々について計算しても良い。また、誤差ベクトルの合成と誤差ベクトルの分散の両方を組合せて照合精度の指標を得ても良い。

次に、推定処理時間は、データ領域面積の２乗に比例するので、あらかじめ蓄積しておいた単位点数あたりの照合時間との比例計算により計算できる。なお、処理時間・精度評価手段では、所定のデータ領域の組合せについて評価を行っても良いし、データ領域ごとに評価を行っても良い。

−照合データ領域選定
次に、照合データ領域選定手段１０において、上記に述べた３つの下位動作モジュールである誤差特性評価手段１４、特徴部分抽出手段１６、処理時間・精度評価手段の出力から得られる評価値を基にデータ領域の選定を行い、データ領域の選定が適切にできなかった場合は選定の閾値を変えるなどして特徴部分の抽出からやり直す（ステップ４０、４２）。具体的には、照合データ領域選定手段１０において、特徴部分抽出手段１６で選定されたＭ個の初期候補データ領域から目標の照合精度と処理時間を達成できるデータ領域の組合せを探索する。

例えば、最初に処理時間・精度評価手段１８で得た推定処理時間に基づいて、処理時間を目標時間内に収めるために用いるべきデータ領域数Ｎ（Ｍ≧Ｎ）が決定される。組合せに用いるデータ領域数Ｎが決定されると、処理時間・精度評価手段１８において推定照合精度を得ることができるので、目標照合精度を達成できるかどうかを判断する。ｘ、ｙ、ｚ方向別に目標となる照合精度（位置姿勢推定の許容誤差等）が異なる場合には、各方向ごとに推定照合精度を目標精度と比較することにより、適切なデータ領域の組合せが選定できる。尚、この組合わせを選定する際、組合わせ数が多くなると、検証に時間がかかるため、遺伝アルゴリズムなどの準最適化手法を用いても良い。尚、目標処理時間を達成するデータ領域数Ｎが初期候補数Ｍとほぼ等しい場合（Ｍ≒Ｎ）や、目標とする方向別の精度を達成するために異なる法線方向成分を持つ候補領域を組合せる必要がある場合には、組合せ数が数百個程度に抑えられるのであれば総当り的に組合せを検証しても良い。また、後述する照合特性蓄積手段２０に蓄積されている過去の組合せ例や、あらかじめ与えられている標準的な組合せ例などに応じて、組合せを優先、限定するなどして評価を行うことも可能である。

こうして、目標とする照合精度と処理時間を満たす組合せを見つけることができる。複数の組合せ候補が存在する場合には、そのうち最も照合精度のよいものを選ぶなどして候補を絞り込む。なお、目的の照合精度及び処理時間を満たす組合せが存在しない場合は、処理時間の増加を許容して解を見つけるために照合データ領域選定手段１０における目標処理時間を増加するか、又は、初期候補を増やすために特徴部分抽出手段１６における閾値を変更する等して、再度選定処理を実行する（ステップ４２）。なお、無限ループに陥らないようにするため、一定回数以上の処理が繰り返された場合には、処理を打ち切り、それまでに得られた結果のうち、最適な結果を出力するとともに注意メッセージを出力する。尚、ここでは選択するデータ領域が、複数のデータ領域の組合せである場合を中心に説明したが、十分な照合精度が得られる場合には単一のデータ領域を選定しても良い。

以上の評価結果を用いることで、推定精度及び処理時間見込みを最もよく満たすものを選ぶこともできるし、複数の組合せ候補をリストアップしておき、さらに別の基準と組合せて選定することも可能である。ここで、別の選定基準としては２次元画像処理と組合せて表面のテクスチャ情報も利用して選定を行うことなどが考えられる。例えば、ステップ３６から３８と平行して３次元データの輝度値に対してＳＵＳＡＮオペレータ等の表面テクスチャに基づく特徴選定オペレータを実行し、その結果得られた特徴部分が含まれるようにデータ領域の組合せを選定しても良い。尚、この場合は距離データと輝度データの両方を用いて照合を行う。

（３次元データ照合）
次に、３次元データ照合手段１２において、照合データ領域選定手段１０によって選定されたデータ領域又はデータ領域同士の組合せを３次元データ蓄積手段８に蓄積された標準３次元データと照合する。ここで照合には、３次元データ同士の照合が可能な種々の照合手法を用いることができる。例えば、非特許文献１に記載されたＩＣＰアルゴリズム法、非特許文献２に記載された計測誤差を考慮した照合法、池内克史による「物体認識と認識プログラムの自動生成」（人工知能学会誌、vol.4、No.1、pp.30-42、1989）に紹介された種種の幾何推論システム、特開平６−１０９４５１号に記載されたようなアスペクト姿勢への射影による予め登録した代表姿勢と照合する方法、等を用いることができる。中でもＩＣＰアルゴリズムを用いれば、任意の形状を扱うことができ、しかも高精度の照合が可能となる点で有利である。

ＩＣＰアルゴリズムでは、ともに点群として表現された２つの３次元データ、即ち計測された３次元データと標準３次元データの間で、一方の３次元データのそれぞれの点から他方の３次元データの中で最も近い点までの距離の２乗和を最小化することにより２つの３次元データが最も良く重なるような座標変換を得る。ここで本件発明では、計測された３次元データの全体について照合を行うのではなく、選定されたデータ領域についてのみ照合を行う。このとき照合対象となる標準３次元データからも対応するデータ領域を切り出す必要があるが、対象物体４の微小な移動が予想される場合には、標準３次元データにおいて過去の照合で一致した部分から予想される移動分だけ広げた範囲から複数のデータ領域を切り出し、順次照合すれば良い。対象物体４が完全自由回転をしている等、移動量や方向の予測が困難であるときには、標準３次元データの全体から一致するデータ領域を探索する必要があるため、後述する多重解像度データを用いて照合することが有効である。

（照合特性蓄積、照合結果出力）
こうして最終的な位置姿勢推定結果が得られると、照合に用いたデータ領域や照合結果等の情報が照合特性蓄積手段２０に蓄積された後、照合結果が出力される（ステップ４６、４８）。尚、照合特性蓄積手段２０には、過去の照合結果の他に、照合データ領域選定手段１０が３つの下位動作モジュールを動作制御する際や、各モジュールが評価を行うための各種照合特性を保持する。例えば、評価を行う際の標準領域サイズや標準的な組合せ例に関する情報などが過去の照合結果などから蓄積されており、これを照合データ領域選定手段において優先的な組合せとして利用する。

尚、本実施の形態では、位置姿勢推定の対象となる物体について計測した３次元データから特徴部分であるデータ領域を選定し、既知の標準３次元データと照合する場合について説明したが、３次元照合には基本的に双方向性があるため、この逆の処理を行っても良い。即ち、既知の３次元データから特徴部分であるデータ領域を選定しておき、位置姿勢推定の対象となる物体について計測した３次元データと照合しても良い。この場合は、既知の３次元データが本件発明における「３次元データ」となり、位置姿勢推定の対象となる物体について計測した３次元データが「標準３次元データ」となる。尚、既知の３次元データを計測によって得る場合、構成や処理フローは図２及び図３と同様となる。

実施の形態２
図５は、本発明の実際の形態２に係る３次元物体照合装置を示すブロック図であり、図６は、その処理を示すフロー図である。実施の形態２では、特徴部分抽出手段１４における選定基準及び／又は照合データ領域選定手段１０で用いる選定基準を変更指示するための選定基準変更手段１１を備えており、３次元データを取得、蓄積した後に対象物体４に合わせて選定基準を適宜変更する（ステップ３３）。その他の点は、実施の形態１と同様である。ここで、特徴部分抽出手段１４の選定基準を変更するとは、データ領域選定の閾値の変更のほか、選定アルゴリズムの変更や、切り出すデータ領域の大きさの変更も含まれる。また、照合データ領域選定手段１０で用いる選定基準を変更するとは、目標処理時間や目標照合精度を変えることの他、組合せに用いるデータ領域数の変更、表面テクスチャに基づく選定等の他の選定手法の併用の有無に関する変更なども含まれる。

本実施の形態によれば、対象物体の形状によって選定基準を変更することにより、目標精度、目標処理時間を満たす組合せをより適切に選択することが可能となる。例えば、直方体形状の対象物体に対して、計測視野の関係から複数視点からの計測が行われ、かつ、各計測視点における計測データが角を含まない平面部のみであった場合などは、実施の形態１で述べた選定アルゴリズム１，２では、ユニーク性のある部分が存在しないことになる。しかし、平面３面があれば空間的な位置姿勢拘束条件となりうるので、こうした場合には法線方向が直交する平面３面を照合すべきデータ領域として抽出すればよい。選定基準変更手段１１では、こうした場合に対応して、オペレータが対象物体に応じて特徴部分抽出手段１６における選定閾値（又はアルゴリズム）や照合データ領域選定手段における選定基準を切り替えるように変更指示を与えることができ（ステップ３３）、例えば、対象物体の平面部からあらかじめ決めておいた中央領域を選定するように動作させる。また、照合の際には、通常の照合と異なり、平面の法線方向の位置照合結果のみを有効な結果として用いるなどの動作制御が必要となるため、３次元照合手段１２での照合動作も変更することが好ましい。本実施の形態のように、選定基準変更手段１１を備えたことにより、処理の柔軟性を向上させることができる。

実施の形態３
図７は、本発明の実際の形態３に係る３次元物体照合装置の処理を示すフロー図である。実施の形態３では、実施の形態２においてオペレータが変更していた選定基準を、データ領域の選定結果に応じて選定基準変更手段が自動的に変更するようにする（ステップ４１）。その他の点は、実施の形態２と同様である。

本実施の形態では、照合データ選定手段１０でデータ領域の選定を行った結果、目標精度、目標処理時間を満たす組合せが得られなかった場合に、自動的に特徴部分抽出手段１６における選定基準や照合データ領域選定手段における選定基準を切り替える（ステップ４１）。この変更にあたっては、特徴部分抽出手段１６において、実施の形態１で述べた選定基準１、２では初期候補群が得られない場合に、実施の形態２述べたような平面構成に応じた選定基準に自動的に変更して再度処理を行うように動作が制御する。また、選定基準変更手段１１によって、実施の形態１における表面テクスチャに基づく選定の併用の有無を切替えても良い。

尚、本実施の形態では、照合データ領域選定手段１０の選定結果に基づいてデータ選定基準を変更する場合について説明したが、図８に示すように、３次元データ照合手段１２により照合した後（ステップ４４）、照合結果の適否を判断して（ステップ４５）、照合結果がエラー値である等、適切でない場合には選定基準変更手段１１によって選定基準を変更しても良い（ステップ４１’）。

実施の形態４
図９は、本発明の実際の形態４に係る３次元物体照合装置を示すブロック図であり、図１０は、その処理を示すフロー図である。この実施形態では、図９に示すように、特徴部分抽出手段１６で選定したデータ領域に対して、その後の照合処理計算に用いるデータ点数を削減するためのデータ点数削減手段１９を備えている。例えば図１０に示すように、特徴部分抽出手段によって選定した後（ステップ３６）、選定した初期候補に対してデータ点数の削減を行う（ステップ３７）。これにより、データ点数をすべて用いた場合には目標とする処理時間を実現できない場合にも、データ点数を減らして処理時間が短縮できるため、目標の処理時間の達成が可能となる。その他の点は、実施の形態３と同様である。

データ点数の削減方法としては、近接する点の平均代表点を用いる方法が一般的である。例えば、３次元データが平面状の各点に対する高さ情報のようなサーフェース形式である場合について説明する。データ点数が２×２の領域について１つの平均代表点を与える処理を行った場合、データ点数は１／４となる。照合対象となるデータ領域内の相互の点を総当りで照合するような手法であれば、この２乗、即ち１／１６の計算量で済むため、照合処理を１６倍高速化することが可能である。なお、データ削減のための平均化処理は近傍する２点で行なっても良いし、さらに拡大した領域、例えばデータ点数が４×４の領域やそれ以上の領域であっても良い。その他に、対象となる物体４の形状が比較的滑らかな変化を示す場合には、簡易的な間引き処理によってデータ点数を削減することも可能である。例えば、データ点数が２×２の領域や隣接２点のうちから１点を間引き抽出して用いることも可能である。なお、データ点が３次元空間上の格子点に存在するソリッドデータを扱う場合には、２×２×２領域のように３次元方向に隣接するデータに対して同様に処理すればよい。

ただし、これらの処理では照合点数が減るため、対象物体の形状を表すのに十分なデータ点数が得られない場合には照合精度の低下を生じる可能性がある。そこで、データ削減を行った後（ステップ３７）、処理時間・精度評価手段によって予想される照合精度を算出し（ステップ３８）、目標の照合精度に対して十分であるかどうかを検証する（ステップ４０）。目標の照合精度が得られなかった場合には、選定基準変更手段１１において、削減するデータ点数を変更して処理を繰り返す（ステップ４０〜４２）。これらの処理における照合精度の評価手順は実施の形態１と同様であり、実施の形態１では用いられるデータ点が削減処理される前のものであったのに対し、本実施例では削減処理された後のデータ点である点のみが異なる。本実施の形態によれば、データ点数削減によって高速度処理を実現しつつ、照合精度を維持できるという効果が得られる。

実施の形態５
図１１は、本発明の実際の形態５に係る３次元物体照合装置の処理を示すフロー図である。本実施の形態では、実施の形態４におけるデータ点数削減処理が多重解像度にわたって実行される場合について、さらに「３次元照合手段」における利用方法も含めて説明する。本実施の形態におけるブロック図は、実施の形態４の図９と同じである。

実施の形態４では、データ点数削減手段におけるデータ点数の削減処理を１段階で行う場合について説明したが、本実施の形態ではデータ点数削減を異なるデータ削減率を用いて多段階で行い、多重解像度のデータを得る。例えば、特徴部分抽出手段によってデータ領域の初期候補の選定が終わった後（ステップ３６）、初期候補のあるデータ領域に対して、まずデータ点数が２×２の領域で平均代表点を選定し、次に４×４領域で平均代表点選定し、さらに８×８領域で平均代表点を選定すると（ステップ３７）、それぞれ１/４、１/１６、１／６４のデータ点数に削減した３種類のデータが得られたことになる。データ点数の削減率が高いほど低解像度となるため、上記の処理により多重解像度のデータが得られる。データ点数削減を行った後の多重解像度のデータに対して処理時間・精度評価を行い（ステップ３８）、データ領域の選定を行う（ステップ４０）。

ここで処理時間・精度の算定とそれらを目標照合精度や目標処理時間と比較する処理は各解像度ごとに行う。多重解像度処理の場合、異なる解像度に対して異なる目標照合精度を用いることが好ましい。即ち、低解像度データでの照合においては、低い位置精度で粗く位置推定できればよく、高解像度に移行するほど高い目標位置精度となり、最終的に所望の目標位置精度で位置・姿勢推定ができればよい。従って、本実施の形態においては、各解像度において目標とする照合精度を個別に指定可能とすることが好ましい。

そして３次元照合手段１２において、上記処理によって選択されたデータ領域を用いて照合が行われる（ステップ４４ａ〜４４ｅ）。この際、まず最も低解像度のデータのみを用いた照合が行われ、順次高解像度のデータを用いた照合処理が行われる。例えば、照合するデータの解像度が低解像度から高解像度までＮ段階（Ｎは１から始まる整数）に分かれていたとする。３次元データの照合を開始すると（ステップ４４ａ）、まず初期解像度の段階を最も低いＮ＝１に設定し（ステップ４４ｂ）、次に指定された１段階目の解像度データの照合処理を行い（ステップ４４ｃ）、次に設定された解像度の段階Ｎが最終解像度であるかを判断し（ステップ４４ｄ）、最終解像度でなければ解像度の段階を１つ増加してステップ４４ｃに戻る一方（ステップ４４ｅ）、最終解像度であれば次の照合特性蓄積に進む。

ここで、低解像度で得られた照合結果を、より高解像度での照合の際に利用する。「照合結果」とは、照合位置だけであっても良く、照合度などの照合位置以外の情報を含んでいても良い。例えば、低解像度での照合によって得た照合位置を、次に行う高解像度での照合の照合初期値として用いることが好ましい。これにより、低解像度段階での少ないデータ点数で開始した照合処理結果を、より詳細なデータを持つ高解像度段階に引き継ぐことができ、処理速度の向上と精度維持を容易に両立させることができる。

尚、低解像度で得られた照合結果を、より高解像度での照合に利用する場合、照合位置に加えて照合度を引き継いでも良い。例えば、低解像度において複数の位置を推定した場合、各々の照合位置と照合度を合わせて高解像度での照合に引継ぎ、高解像度では照合度が高い照合位置から優先的に照合を進めることが考えられる。

実施の形態６
図１２は、本発明の実際の形態４に係る３次元物体照合装置を示すブロック図であり、図１３は、その処理を示すフロー図である。実施の形態５では、特徴部分抽出手段１６で選定されたデータ領域に対してデータ点数を削減する場合について説明したが、本実施の形態では、予め多重解像度化された３次元データに対して、特徴部分抽出手段１６でデータ領域の選定を行う場合について説明する。

照合すべき物体の形状によっては、低解像度において選定すべき特徴部分と高解像度において選定すべき特徴部分が異なる場合がある。特に、高い空間周波数成分の形状を持つ物体については、低解像度のデータではその特徴要素が失われるため、実施の形態５で行ったように高解像度データを基に選定した特徴部分については、低解像度データでは特徴が存在しない部分となっている場合がある。そこで本実施の形態においては、３次元データ蓄積手段８に蓄えられた３次元データから多重解像度データ生成手段９において多重解像度データを生成しておき、特徴部分の選定処理を各解像度データに対して行う。

例えば、図１３に示すように、３次元データを蓄積した後（ステップ３２）、多重解像度データ生成手段９による処理を開始する（ステップ３５ａ）。生成する多重解像度データの解像度が低解像度から高解像度までＮ段階（Ｎは１から始まる整数）に分かれているとする。まず初期解像度の段階を最も低いＮ＝１に設定し（ステップ３５ｂ）、次に指定された１段階目の解像度データの生成（データ削減）を行い（ステップ３５ｃ）、生成したデータに対して、誤差特性評価（ステップ３４）、特徴部分抽出（ステップ３６）、処理時間・精度評価（ステップ３８）、照合用データ領域の選定（ステップ４０）を行った後、設定された解像度の段階Ｎが最終解像度であるかを判断し（ステップ３５ｄ）、最終解像度でなければ解像度の段階を１つ増加してステップ３５ｃに戻る一方（ステップ３５
ｅ）、最終解像度であれば次の照合特性蓄積に進む。その後の３次元データ照合手段における照合手順（ステップ４４ａ〜４４ｅ）は、実施の形態５と同様に、低解像度データから高解像度データへと順次照合するとともに、低解像度データにおける照合結果を高解像度データにおける照合初期値として利用する。

ここで解像度毎の誤差精度評価、特徴部分抽出、処理時間・精度評価、及び照合用データ領域の選定は、先の実施の形態と同様にして行うことができる。但し、特徴部分抽出のステップ（ステップ３６）では、解像度によって切り出すデータ領域の大きさ、選定アルゴリズム、閾値等を変更することが好ましい。例えば、解像度が低い場合には広いデータ領域を切り出し、解像度が上がるにしたがって切り出すデータ領域を狭くしていっても良い。特に、最も解像度が低い場合に切り出すデータ領域の大きさを対象物体の全体としておけば、後のデータ照合において有利である。即ち、３次元データ照合は最も低い解像度のデータから開始するが、照合開始時には物体の位置姿勢が予想できていない場合が多いため、照合に用いる３次元データが部分的なデータ領域であると標準３次元データから対応するデータ領域を切り出すことが難しい。そこで最も低い解像度では物体全体をデータ領域とすることにより、標準３次元データの全体と照合すれば良くなり、照合処理が容易となる。解像度が低い場合には、照合位置精度が低い代わりに単位面積あたりの処理時間が短くなるため、物体全体でデータ照合を行っても目標処理時間の達成が可能である。その後順次高い解像度になるに従って、データ領域を小さくしていけば、短い処理時間で高い照合精度を実現できる。以上の処理により、解像度毎に最適な部分を照合可能となり、多くの対象を簡易な手順で高速かつ高精度に照合可能になる。

実施の形態７
図１４は、本発明の実際の形態７に係る３次元物体照合装置を示すブロック図であり、図１５は、その処理を示すフロー図である。本実施の形態では、多重解像度データの生成と照合のみを行い、誤差特性評価や特徴部分抽出は行わない。即ち、３次元データ蓄積手段８に３次元データを蓄積した後（ステップ３２）、多重解像度データ生成手段９において３次元データに対してデータ点数削減を異なるデータ削減率を用いて多段階で行い、多重解像度のデータを得る（ステップ３５）。多重解像度データを得る手順は、実施の形態５で説明したのと同様である。そして３次元照合手段１２において、多重解像度化された３次元データを用いて照合が行われるが（ステップ４４ａ〜４４ｅ）、この際、まず低解像度データのみを用いた照合が行われ、順次高解像度データを用いた照合処理が行われる。この際、低解像度で得られた照合結果が、より高解像度での照合の際に利用される。具体的には、低解像度での照合位置や照合度が高解像度での照合初期値として用いられる。これにより、低解像度段階での少ないデータ点数で開始した照合処理結果を、より詳細なデータを持つ高解像度段階に引き継ぐことができ、処理速度の向上と精度維持を容易に両立させることができるという効果がある。

図１は、本件発明の実施の形態１に係る３次元物体照合装置の概略を示す模式図である。図２は、本件発明の実施の形態１に係る３次元物体照合装置の構成を示すブロック図である。図３は、本件発明の実施の形態１に係る３次元物体照合装置の処理フローを示すフロー図である。図４は、センサ構造に由来する計測誤差を説明する模式図である。図５は、本件発明の実施の形態２に係る３次元物体照合装置の構成を示すブロック図である。図６は、本件発明の実施の形態２に係る３次元物体照合装置の処理フローを示すフロー図である。図７は、本件発明の実施の形態３に係る３次元物体照合装置の処理フローを示すフロー図である。図５は、本件発明の実施の形態３に係る３次元物体照合装置の別の処理フローを示すフロー図。図９は、本件発明の実施の形態４に係る３次元物体照合装置の構成を示すブロック図である。図１０は、本件発明の実施の形態４に係る３次元物体照合装置の処理フローを示すフロー図である。図１１は、本件発明の実施の形態５に係る３次元物体照合装置の処理フローを示すフロー図である。図１２は、本件発明の実施の形態６に係る３次元物体照合装置の構成を示すブロック図である。図１３は、本件発明の実施の形態７に係る３次元物体照合装置の処理フローを示すフロー図である。図１４は、本件発明の実施の形態７に係る３次元物体照合装置の構成を示すブロック図である。図１５は、本件発明の実施の形態６に係る３次元物体照合装置の処理フローを示すフロー図である。

符号の説明

４計測対象物体、６３次元データ取得手段、８３次元データ蓄積手段、１０照合データ領域選定手段、１２３次元データ照合手段、１４誤差特性評価手段、１６特徴部分抽出手段、１８処理時間・精度評価手段。

Claims

物体の立体形状を計測して３次元データを得る３次元データ取得手段と、
取得した３次元データを蓄積する３次元データ蓄積手段と、
取得した３次元データと参照すべき標準３次元データとの照合を行う３次元データ照合手段とを備えた３次元物体照合装置であって、
さらに、前記３次元データの各計測点について計測誤差を推定する誤差特性評価手段と、
前記３次元データから１以上のデータ領域を切り出し、所定の形状特徴値が閾値よりも大きなデータ領域を初期候補として選定する特徴部分抽出手段と、
前記データ領域又は前記データ領域同士の組合せについて推定処理時間と推定照合精度を計算する処理時間・精度評価手段と、
前記初期候補から、目標処理時間と目標照合精度を達成するデータ領域又はデータ領域同士の組合せを選定する照合データ領域選定手段とを備え、
前記３次元データ照合手段が、前記照合データ領域選定手段の選定したデータ領域又はデータ領域同士の組合せについて標準３次元データとの照合を行うことを特徴とする３次元物体照合装置。
前記誤差特性評価手段が、前記３次元データ取得手段に由来する計測誤差を前記３次元取得手段と前記物体の相対位置から推定することを特徴とする請求項１に記載の３次元物体照合装置。
前記３次元データ取得手段が、前記物体に照射した光を受光器によって検出することによって立体形状を計測し、
前記誤差特性評価手段が、前記物体表面で反射して前記受光器に向かう光線の割合に基づいて計測誤差を推定することを特徴とする請求項１又は２に記載の３次元物体照合装置。
過去に選定したデータ領域又はデータ領域同士の組合せを蓄積する照合特性蓄積手段を備え、前記照合データ領域選定手段が、前記照合特性蓄積手段に蓄積された情報に基づいてデータ領域又はデータ領域同士の組合せを選定することを特徴とする請求項１乃至３のいずれか１項に記載の３次元物体照合装置。
前記特徴部分抽出手段で用いる選定基準及び／又は前記照合データ領域選定手段で用いるデータ選定基準を変更する選定基準変更手段を備えたことを特徴とする請求項１乃至４のいずれか１項に記載の３次元物体照合装置。
前記選定基準変更手段が、前記照合データ領域選定手段の選定結果又は前記３次元データ照合手段での照合結果に基づいて自動的に変更動作を行うことを特徴とする請求項５に記載の３次元物体照合装置。
前記３次元データ又はデータ領域から照合すべきデータ点数を削減するデータ点数削減手段を備えたことを特徴とする請求項１乃至６のいずれか１項に記載の３次元物体照合装置。
前記３次元データ又はデータ領域から異なる削減率でデータ点数を削減することにより多重解像度化されたデータを生成する多重解像度データ生成手段を備え、
前記３次元データ照合手段が、前記多重解像度データを低解像度データから高解像度データへと順次照合するとともに、低解像度データにおける照合結果を高解像度データにおける照合初期値として利用することを特徴とする請求項１乃至７に記載の３次元物体照合装置。
前記特徴部分抽出手段で用いる選定基準及び／又は前記照合データ領域選定手段で用いるデータ選定基準を変更する選定基準変更手段を備え、
前記多重解像度データ生成手段が、削減率を変更しながら前記３次元データからデータ削減を繰り返すことにより多重解像度化されたデータを生成し、
前記選定基準変更手段が、各解像度のデータについて、前記特徴部分抽出手段で用いる選定基準及び／又は前記照合データ領域選定手段で用いるデータ選定基準を変更しながら選定を繰り返すことを特徴とする請求項８に記載の３次元物体照合装置。
前記多重解像度データ生成手段が、削減率を変更しながら前記３次元データからデータ削減を繰り返すことにより多重解像度化されたデータを生成し、
前記特徴部分抽出手段が、前記多重解像度化されたデータの解像度に応じて切り出すデータ領域の大きさを変えるとともに、前記解像度が最小であるときには前記３次元データの略全体からデータ領域を切り出すことを特徴とする請求項８又は９に記載の３次元物体照合装置。