JP4650750B2 - 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置 - Google Patents

Description

発明の背景

発明の技術分野
本発明は、静止している三次元形状を複数の計測位置から計測し、その距離データを統合して復元するための三次元形状データの記憶・表示方法と装置、および三次元形状の計測方法と装置に関する。

関連技術の説明
三次元形状の計測技術が普及し、様々な応用分野、例えば、機械部品・工作物等の形状計測、移動ロボットにおける自己位置の同定、地形・構造物の計測等に三次元形状データが用いられている。

三次元形状データの記憶・表示方法と装置として、点群、距離画像、サーフェス、ボクセル構造等が従来から用いられている。
点群は、センサの計測データである計測点の座標値で形状を表現する。また距離画像は、二次元画像の各画素に対して奥行き方向の距離値を保持することで形状を表現する。サーフェスは、三角形等の張り合わせにより物体の表面形状を表現する。ボクセル構造は、空間を小直方体で分割し、各ボクセル内に形状情報を保持するものである。

三次元形状計測における計測データは、例えば、計測位置を原点とする極座標値（ｒ，θ，φ）で表される。本発明において、このデータを「距離データ」と呼ぶ。

静止している三次元形状を、複数の位置からの距離データを統合して復元する場合、以下の問題点がある。
点群を用いる場合、計測データに比例してデータ量が増加する。従って、計測対象物が大きい場合の統合データ量は膨大となる。そのため、形状表現のためのデータ構造としては現実的ではない。
距離画像を用いる場合、一方向からの計測であるため、遠方に行くほど画素の間隔が広がり精度が低下する。また、一方向からのデータのみしか表すことができないため、複数視点からの計測結果の統合に向いていない。
サーフェスを用いる場合、統合に複雑な処理を必要とするため、高速処理が困難である。

これに対し、ボクセル構造は、データ量が一定、複数視点からのデータ統合が可能、データ統合に複雑な計算を必要としない、という特徴を有し、複数の位置からの距離データを統合して復元するのに最も適している。

さらに、ボクセル構造を改良したものとして、特許文献１，２や非特許文献１が開示されている。
特許文献１の「三次元情報抽出方法」では、Ｖｏｘｅｌ Ｖｏｔｉｎｇと呼ばれる方法を用いて計測結果を各ボクセルに投票することで、複数視点からの三次元形状を統合する。更にこの方法では、必要な解像度が得られないボクセルに対しては、ボクセルを更に八分割して階層的にデータを管理する方法を開示している。
特許文献２の「環境モデル入力装置」では、各ボクセルに確率値を蓄積する手段を提案している。この手段では、確率を与える頻度に依存せずに環境モデルの統合を行い、正確な環境モデルの作成を可能としている。
非特許文献１の手段では、平面を想定しているものの、確率値を保持するボクセルに対して、計測データに含まれる誤差により発生したボクセルの誤った確率値を正しい値に戻す処理を提案している。

また、本発明に関連する技術として、非特許文献２が開示されている。

特許第３１７０３４５号公報、「三次元情報抽出方法」 特開平９−８１７８８号公報、「環境モデル入力装置」

Ｓｅｂａｓｔｉａｎ Ｔｈｒｕｎ，"Ｌｅａｒｎｉｎｇ Ｏｃｃｕｐａｎｃｙ Ｇｒｉｄ Ｍａｐｓ Ｗｉｔｈ Ｆｏｒｗａｒｄ Ｓｅｎｓｏｒ Ｍｏｄｅｌｓ．" 関本清英、他、「三次元レーザレーダの開発」、石川島播磨技報Ｖｏｌ．４３ Ｎｏ．４（２００３−７）

発明が解決しようとする課題
特許文献１のボクセル構造は、正しい形状を得るために、ボクセルの分割を繰り返すが、分割の回数が増えるとデータサイズが増加するという問題が発生する。そのため、分割の深さを固定とするのが一般であり、これにより解像度の限界が存在する。つまり、分割後のボクセルの大きさ以上の解像度を表現できない。また、誤差を含むデータにより誤って発生したボクセルはそのまま存在してしまう。そのため、誤差を含むデータに対応できない。

特許文献２のボクセル構造は、確率値を用いて形状を表現しているが、特許文献１と同様にデータが誤差を含む場合は誤って発生したボクセルはそのまま存在してしまう。そのため、誤差を含むデータに対応できない。

非特許文献１のボクセル構造は、確率を下げる処理を含むため、誤った確率値を保持するボクセルを消去する機能を有する。しかし、一般にはボクセル内に計測物体が存在している領域と存在しない領域の両方が存在するため、複数回の計測を統合しても、正しい形状に収束しないという問題がある。

発明の要約

本発明は上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、静止している三次元形状を、複数の位置からの距離データを統合して復元することができ、かつデータサイズを大幅に増加することなく解像度を高めることができ、誤差を含むデータに対応できる三次元形状データの記憶・表示方法と装置、および三次元形状の計測方法と装置を提供することにある。

本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶・表示方法であって、
三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、
前記ボクセル位置、代表点、および誤差分布を出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶・表示方法が提供される。
また、本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、
三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、
前記代表点、ボクセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状の計測方法が提供される。

前記三次元形状データの記憶・表示方法または三次元形状の計測方法は、以下の内容を含んでもよい。

本発明の好ましい実施形態によれば、前記モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。

また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。

また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。

また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことが好ましい。ボクセルの分割には、例えば八分木やＫ−Ｄ木を用いる。

また、ボクセルの内部に代表点とその誤差分布に加えて、確率値を併せ持つ、ことが好ましい。
また、前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する。
また、距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する、ことが好ましい。

また、前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と該新たな代表点から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。

本発明の好ましい実施形態によれば、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。

前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する。

前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない。

前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力ステップにおいて、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。

前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。

また本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶・表示装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
前記ボクセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状データの記憶・表示装置が提供される。
また本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
前記代表点、ボクセル位置および誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置が提供される。

前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、以下の内容を含んでもよい。

本発明の好ましい実施形態によれば、前記モデル構築装置は、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。

本発明の好ましい実施形態によれば、前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。

本発明の好ましい実施形態によれば、前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。

本発明の好ましい実施形態によれば、前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、から新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。

本発明の好ましい実施形態によれば、前記マッチング装置は、ボクセルの内部に代表点とその誤差分布に加えて、ボクセルの確率値を併せて設定し記憶する。

本発明の好ましい実施形態によれば、前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、前記三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する距離センサを備える。

本発明の好ましい実施形態によれば、前記マッチング装置は、前記距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する。

本発明の好ましい実施形態によれば、前記モデル更新装置は、
前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と該新たな代表点から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。

本発明の好ましい実施形態によれば、前記三次元形状データの記憶・表示装置または三次元形状の計測装置は、前記マッチングステップの後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。

前記データ伝達装置は、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する。

前記データ伝達装置は、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない。

前記マッチング装置の後に、前記環境モデルを更新するモデル更新装置を有し、該モデル更新装置において、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力装置は、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。

前記出力装置は、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。

発明の効果
上記本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセルに分割し、各ボクセル位置を記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。

また、モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割するので、データ量を小さく抑えると同時に、分割後のボクセルと代表点を用いて解像度を更に高めることができる。

特に、三次元形状上の複数の座標値を複数の計測位置を原点とする距離データとして取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる。

また、原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を消去することにより、誤った計測データの影響を取り除くことができる。

また、新たに入力された被計測点の座標値に対応するボクセルを探索し、該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定することにより、代表点の座標値と誤差分布を容易に設定できる。

更に、前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。
特に、誤差分布が互いに重複する場合に、新たな誤差分布と新たな代表点を再設定した結果、代表点が別のボクセルに移動する場合には、既に設定したボクセル内の誤差分布と更に比較することで、単一のボクセル内に単一の代表点のみが存在するように一貫性を保つことができる。

上述したように、本発明の方法と装置によれば、誤差を含む距離データを正確な情報に補正する機能を有すると共に、これを繰り返すことにより、長時間の計測に対して高精度な形状に収束する。なおかつ、本発明の方法は、各ボクセルに対応する代表点とその誤差分布を新たな計測点で更新する処理であるため計算量が小さい。また、演算は周囲のボクセルへの影響を与えずボクセル内で閉じているため、高速処理が可能である。また、計測データは最大のボクセルが必要最小限の分解能を有するボクセル構造に逐次統合可能であり、メモリサイズは固定サイズを大きく上回ることはない。

また、各ボクセルに確率値を持たせることによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで判断できる。

また、センサの位置・姿勢に誤差がある場合でも、この誤差分布を計測データの誤差分布に合成して、計測データの絶対誤差を算出することで、対象物体の位置精度を向上させることができる。

さらに、前記モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定するので、より真値に近い形状を得ることができる。
特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含むデータであってもカルマンフィルタの効果により真値に収束した高精度な形状が得られる。

また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する際に、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさ出力装置に出力するため、計測装置を使用する際に、信頼性の低い計測値をアプリケーションの内容に応じて使用者が取捨選択することができるようになる。

また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しないようにすることによって、計測装置を使用する際に、そもそも信頼性の高い計測値のみを扱うことができるようになるため、扱うデータ量を削減することや、信頼性の向上につながる。

また、前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得し、特に異なる方向から距離データを得ることによって、分布形状が異なる誤差分布の距離データを統合することが可能となり、精度を向上することができる。

また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合のみ、前記出力ステップにおいて、当該ボクセルの代表点の位置、誤差分布およびボクセルの位置の少なくともいずれかを三次元形状の計測値として出力装置に出力することによって、距離センサから新たに得られた被計測点によって影響を受けたボクセル内の代表点等の値が出力される。このため、使用者は従来と同様の運用を想定しながら、距離センサで得られる元々の計測値があたかもより精度の高い計測値に置き換わったように、利用することができるようになる。このように、より精度の高い三次元形状計測が可能になる。

また、前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力することによって、従来の距離センサの計測値の分解能が粗い場合でも、あたかも精度良くかつ分解能が高い距離センサのように利用することが可能となる。このように、より精度の高い三次元形状計測が可能になる。
本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から明らかになろう。

非特許文献２に開示された三次元レーザレーダの構成図である。 距離センサで計測された極座標データと誤差の関係を示す図である。 誤差分布を直方体に包含される楕円体として近似する場合を示している。 本発明の方法を実行するための装置構成図である。 本発明の方法を示すフローチャートである。 モデル構築ステップの模式図である。 構築された環境モデルの模式図である。 本発明におけるボクセルデータのデータ構造を示す図であり、各ボクセルデータのメモリレイアウト例を示している。 本発明におけるボクセルデータのデータ構造を示す図であり、レベル２（１，１，０）のボクセルが代表点を持つ場合の例を示している。 モデル更新ステップにおけるデータ処理フロー図である。 該当するボクセル内に既に設定した代表点がある場合の模式図である。 誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動する場合を示している。 誤差分布が互いに重複する場合の模式図である。 複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示す模式図である。 ボクセルの確率値の算出方法の模式図である。 誤差分布が互いに重複する場合のボクセルの確率値の算出方法の模式図である。 距離センサの位置の誤差分布と計測データの誤差分布を合成し、計測データの絶対誤差を算出する場合の模式図である。 カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。 図１６の一部拡大図である。

好ましい実施例の説明

以下本発明の好ましい実施形態について、図面を参照して説明する。なお、各図において、共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、距離センサの一例としての三次元レーザレーダの構成図である。三次元レーザレーダは、例えば非特許文献２に開示されている。
この図に示すように、三次元レーザレーダ１０は、レーダヘッド１２と制御器２０から構成される。レーザダイオード１３から発振されたパルスレーザ光１は、投光レンズ１４で平行光２に整形され、ミラー１８ａ，１８ｂと回転・揺動するポリゴンミラー１５で二次元方向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ光３は、ポリゴンミラー１５を介して受光レンズ１６で集光され、光検出器１７で電気信号に変換される。

制御器２０内の時間間隔カウンタ２１は、レーザダイオード１３のパルス発振タイミングと同期したスタートパルス４と、光検出器１７から出力されたストップパルス５の時間間隔を計測する。信号処理ボード２２は、反射光が検出された時点の時間間隔ｔ、ポリゴンミラーの回転角度θ、揺動角度φを極座標データ（ｒ，θ，φ）として出力する。
ｒは計測位置（レーダヘッド設置位置）を原点とする距離であり、ｒ＝ｃ×ｔ／２ の式で求められる。ここでｃは光速である。
判定処理ユニット２３は、信号処理ボードからの極座標データを、レーダヘッド設置位置を原点とした三次元空間データ（ｘ，ｙ，ｚ）へ変換して、検出処理を行うようになっている。なおこの図で２４はドライブユニットである。

上述した三次元レーザレーダ１０の計測範囲は、例えば、水平画角６０°、垂直画角３０°、最大測定距離５０ｍである。また、位置検出精度は、例えば約２０ｃｍである。
また、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示する場合、１フレームの計測点数を、横方向１６６点、スキャン方向５０点とすると、１フレームに１６６×５０＝８３００点が表示される。この場合にフレームレートは、例えば約２フレーム／秒である。

この三次元レーザレーダ１０で計測される三次元形状上の被計測点は、横方向にΔθ×ｒ、垂直方向にΔφ×ｒ、互いに離散した点群となる。例えば、Δθ＝６０／１６６×π／１８０＝６．３×１０^−３ラジアン、Δφ＝３０／５０×π／１８０＝１０．５×１０^−３ラジアン、ｒ＝５０ｍの場合、最も近接する場合でも、被計測点の間隔は、横方向で約３１５ｍｍ、垂直方向で約５２５ｍｍとなる。

本発明では、距離センサとして、例えば、上述した三次元レーザレーダ１０を用いる。しかし、距離センサはこれに限定されず、視差を利用した距離センサ、その他の周知の距離センサを用いることができる。

図２Ａ，図２Ｂは、距離センサで計測された極座標データと誤差の関係を示す図である。
図２Ａに示すように、任意の計測位置を原点とする極座標値（ｒ，θ，φ）を計測結果として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存在する。
この誤差分布は、誤差分布のｒ_ｓ，θ_ｓ，φ_ｓでの存在確率をＰ（ｒ_ｓ，θ_ｓ，φ_ｓ）とした場合、誤差分布は計測の軸ｒ，θ，φ方向に正規分布しているとし、例えば式（１）で表すことができる。ここで、ｒ，θ，φはセンサからの計測値、σ_ｒ，σ_θ，σ_φは は標準偏差、Ａは規格化定数である。
図２Ｂに示すように、誤差分布は、通常ｒ方向に長い切頭円錐形（左図）に内包される分布であるが、遠方においてａとｂの差は小さい。従って、この誤差分布を直方体に包含される楕円体として安全サイドに近似することができる。

図３は、本発明の方法を実行するための装置構成図である。この図に示すように、この装置は、データ入力装置３２、外部記憶装置３３、内部記憶装置３４、中央処理装置３５および出力装置３６を備える。

データ入力装置３２は、上述した距離センサを有し、三次元形状上の座標値を記憶装置に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。なお、データ入力装置３２は、キーボード等の通常の入力手段も有するのがよい。
外部記憶装置３３は、ハードディスク、フロッピ（登録商標）ーディスク、磁気テープ、コンパクトディスク等である。外部記憶装置３３は、環境モデルのサイズが大きく後述する内部記憶装置３４に入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の全体を保持できない場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。
内部記憶装置３４は、例えばＲＡＭ，ＲＯＭ等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。
中央処理装置３５（ＣＰＵ）は、モデル構築装置、マッチング装置、モデル更新装置、データ伝達装置として機能し、演算や入出力等を集中的に処理し、内部記憶装置３４と共に、プログラムを実行する。モデル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後述のマッチングステップを行う装置であり、モデル更新装置は、後述のモデル更新ステップを行う装置であり、データ伝達装置は、出力装置３６へデータを出力する装置である。
出力装置３６は、例えば表示装置、プリンタ、外部装置等であり、内部記憶装置３４および外部記憶装置３３の少なくともいずれかに記憶したデータ及びプログラムの実行結果を出力するようになっている。外部装置とのインターフェイスは、ＬＡＮ、ＵＳＢ、ＩＥＥＥ１３９４等であり、入力された三次元形状上の座標値に対して該当するボクセル内の代表点、誤差分布、ボクセル位置などを付加した結果や、環境モデル全体または環境モデル一部を要求に応じて出力する。

上述した本発明の装置は、上述した距離センサと通常のＰＣ（コンピュータ）を組み合わせたものでもよく、或いは、全体を一体にした装置であってもよい。また、自走可能な装置内に一体的に組み込んでもよい。

図４は、本発明の方法を示すフローチャートである。
本発明の方法は、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状の計測方法と装置であり、データ入力ステップＳ１、データ補正ステップＳ２、モデル構築ステップＳ３、マッチングステップＳ４、モデル更新ステップＳ５及び出力ステップＳ６を有する。
なお、これら一連の処理のうち、Ｓ１、Ｓ２、Ｓ４〜Ｓ６は、計測データが得られる毎に実施し、Ｓ３は初めて計測データが得られたときにだけ実施することができる。

データ入力ステップＳ１では、距離センサを用いて、三次元形状上の座標値をコンピュータの記憶装置に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。
なおこのデータ入力ステップＳ１において、三次元レーザレーダ１０を用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得するのがよい。

距離センサとして三次元レーザレーダ１０を用いた場合、三次元形状上の座標値は、任意の計測位置を原点とする距離データであり、極座標値（ｒ，θ，φ）で表される。また、各座標値の誤差分布は、極座標値（ｒ，θ，φ）から演算で求めるか、予め別の入力手段（例えばキーボード）で入力する。

データ補正ステップＳ２では、距離データの補正処理を行い、距離データの精度を向上させる。また、極座標データとオドメータのデータから、任意の固定位置を原点とした三次元空間データ（ｘ，ｙ，ｚ）へ変換してもよい。
距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、周囲の点から孤立して存在する点であり、計測データは複数の近接する点で構成されることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計測データが含む誤差分布を考慮して、複数回の計測を統計処理（例えば平均値等）することで、距離の補正を行う。
さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを行うのがよい。

図５は、ボクセルの分割に八分木を用いた場合のモデル構築ステップの模式図である。
モデル構築ステップＳ３では、この図に示すように、三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセル６に分割し、各ボクセル位置を記憶する環境モデルを構築する。
ボクセル６の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体でもよい。
また、ボクセル６の各辺の長さは、最大のボクセル６を必要最小限の分解能に相当する大きさに設定するのがよい。以下、最大のボクセル６をレベル１のボクセルと呼ぶ。
また、単一のボクセル内に複数の被計測点が存在する場合には、単一のボクセル内に単一の被計測点のみが存在するように、ボクセルを更に八分割して階層的に複数のボクセルに分割する。以下、最大のボクセル６の八分割を１回実施した空間領域をレベル２のボクセル、ｋ回実施した空間領域をレベルｋ＋１のボクセルと呼ぶ。

図６は、構築された環境モデルの模式図である。
マッチングステップＳ４では、この図に示すように、三次元形状上の座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶する。末端のボクセルは計測値の代表点を１つだけ持つことができる。各ボクセルが計測値の代表点とその誤差分布を持つことで、物体の形状を表す。

マッチングステップＳ４において、代表点の絶対位置は式（２）で与えられる。ここで、（ｘ，ｙ，ｚ）は代表点のボクセルでの相対座標、Ｓｘ，Ｓｙ，Ｓｚはレベル１でのボクセルの一辺の大きさ、ｎ_ｘ（ｋ），ｎ_ｙ（ｋ），ｎ_ｚ（ｋ）はレベルｋでのボクセルの番地、Ｌは求める代表点が存在するレベルである。

図７Ａ，図７Ｂは、本発明におけるボクセルデータのデータ構造を示す図である。
この図において、図７Ａは、各ボクセルデータのメモリレイアウト例である。この図において、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。
図７Ｂは、レベル２（１，１，０）のボクセルが代表点を持つ場合の例を示している。なおこの図において、ｎｕｌｌは空集合を表す。

上述したデータ構造の環境モデルは、以下の特徴を有する。
（１）内容：空間を小直方体で分割して各ボクセルに計測点の代表点と誤差分布を保持する。
（２）精度：ボクセル毎に持つ計測点の代表値相当である。
（３）存在：物体の存在の有無を表現できる。
（４）データ量：ボクセルの個数に比例してメモリを必要とするが、サイズ固定である。
（５）点群からの変換：適しており、計算量は少ない。
（６）アクセス速度：シンプルな構造をしているため、要素へのアクセスが高速である。

またこの特徴から、上述した環境モデルは、以下の効果Ａ〜Ｃをすべて満たしている。
効果Ａ：誤差を考慮した表現が可能である。
効果Ｂ：必要なメモリ量と計算量が一定量以下である。
効果Ｃ：物体の存在だけでなく、物体が存在しないことを表せる。

さらに図４において、モデル更新ステップＳ５は、マッチングステップＳ４の後に実施し、モデル構築ステップＳ３で構築した環境モデルを更新する。

図８は、モデル更新ステップＳ５におけるデータ処理フロー図である。この図に示すように、ステップＳＴ１で新たに入力された被計測点の座標値に対応するボクセルを探索し、ステップＳＴ２で該当するボクセル内に代表点がない（ボクセルが空である）場合には、ステップＳＴ３で新たに入力された被計測点の座標値と誤差分布を代表点の座標値と誤差分布として設定（新規に登録）する。
また、このステップＳＴ３において、新しい計測位置（原点）と被計測点の間には、原理的に物体が存在しないはずである。従って新しい計測位置（原点）と被計測点の間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。

図９は、該当するボクセル内に既に設定した代表点がある場合の模式図である。
図８のステップＳＴ２で該当するボクセル内に既に設定した代表点がある場合には、ステップＳＴ４で新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較する（すなわち異なる点か同一点かを判断する）。
この比較で、誤差分布が互いに重複する場合（図９Ａ）には、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定する（すなわち誤差分布を合成する）。
またこの比較で、誤差分布が互いに重複しない場合（図９Ｂ）には、ステップＳＴ６、ＳＴ７で単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割し新規に登録する。
分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度には例えば、マハラノビス距離のような距離尺度や、尤度のような確率値の尺度を利用できる。また、２つの誤差分布に基づき、両者が同一点を表しているかを統計的検定によって判定してもよい。

ステップＳＴ５で両誤差分布から新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動したとき（即ち、ステップＳＴ８で、Ｙｅｓ）、ステップＳＴ２へ戻り、上述の処理を繰り返す。
なお、図１０は、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動する場合を示している。

図１１は、誤差分布が互いに重複する場合（図９Ａ）の別の模式図である。図８のステップＳＴ５において、２つの代表点と誤差分布を合成して新たな代表点と誤差分布を設定する手段として、カルマンフィルタを用いることができる。例えば、二次元の場合に、この図に示すように、２つの代表点をそれぞれｘ（１），ｘ’（２）、２つの誤差分布をΣ（１）、Σ’（２）とし、これを合成した代表点をｘ（２）、誤差分布をΣ（２）とすると、代表点ｘ（２）と誤差分布Σ（２）を算出する模式図は図１１のようになる。

図４において、出力ステップＳ６では、ボクセル位置、及び代表点とその誤差分布を出力装置３６に出力する。出力装置３６が表示装置（例えばＣＲＴ）の場合、三次元画像上に立体表示するのが好ましい。
また、出力ステップＳ６では、ボクセル位置、及び代表点とその誤差分布を別の装置（例えば制御装置、コンピュータ）に転送してもよく、プリンタで出力してもよい。即ち、出力装置３６は、当該別の装置（例えば制御装置、コンピュータ）であってよい。
なお、出力装置３６は、ボクセル位置、及び代表点とその誤差分布を記憶・保持できる適切な記憶装置であってもよい。
また、出力ステップＳ６において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するとともに、該計測値の信頼性または精度を示す指標（例えば、数値）を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置３６に出力してもよい。さらに、出力ステップＳ６において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するときに、該ボクセルの内部の誤差分布の大きさ（広がり）が所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値（即ち、このボクセルの代表点の位置）を出力装置３６に出力しないようにしてもよい。

また、モデル更新ステップＳ５において、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点、および誤差分布が新たに設定されるか、または再設定されるか、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合のみ、出力ステップＳ６において、当該ボクセルの代表点の位置、誤差分布およびボクセルの位置の少なくともいずれかを三次元形状の計測値として出力装置に出力してもよい。

また、出力ステップＳ６において、距離センサの位置・姿勢が得られる場合は、その位置から見える範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力してもよい。距離センサの位置から見える範囲とは、距離センサの位置から距離センサが位置計測可能な範囲であり、距離センサの位置から距離センサが位置計測可能な角度範囲（視野）と、距離センサの位置から距離センサが位置計測可能な距離範囲とを含んでよい。

図４に示した処理の手順は、新たな計測位置において、新しい計測データが得られる度に、処理を繰り返し行い、内部記憶装置３４および外部記憶装置３３の少なくともいずれかに結果を格納する。処理を高速化するためには、内部記憶装置３４に容量が許す限り結果を格納することが好ましい。

上述した本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセル６に分割し、各ボクセル位置を外部記憶装置３３に記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。

また、モデル構築ステップＳ３において、最大のボクセル９を必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル９内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割するので、データ量を小さいデータサイズに抑えると同時に、分割後のボクセルと代表点を用いて解像度を更に高めることができる。

特に、三次元形状上の複数の座標値を複数の計測位置を原点とする距離データとして取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる。
図１２は、複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示している。このように異なる計測位置（即ち、距離センサである三次元計測器の位置）を原点として得られた距離データは誤差分布の向きも異なるので、これらの距離データを環境モデルを介して逐次統合することによって、代表点の誤差分布が縮小し、代表点の位置精度が向上する。なお、図１２において、三次元計測後の図はコップの２次元断面を表わした模式図であり、三次元計測後の図の破線はコップの実際表面を表わしている。

また、原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去することにより、誤った計測データを補正もしくは消去することができる。

また、新たに入力された被計測点の座標値に対応するボクセルを探索し、該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定することにより、代表点の座標値と誤差分布を容易に設定できる。

更に、前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。

各ボクセルの確率値は、代表点と誤差分布から算出する。ボクセルの確率値の算出方法は、例えば、図１３に示すように、誤差分布の全体に対してボクセルに内包される分布の比から求めることができる。
また、模式図を図１４に示すように、２つ以上の誤差分布が交差するような場合は、例えば、それぞれの誤差分布に対する確率値を用いたベイズ推定等により確率値の合成を行う。

距離センサの位置・姿勢の誤差分布が得られる場合は、この誤差分布を計測データの誤差分布に合成して、計測データの絶対誤差を算出することができる。
例えば、距離センサをオドメータ付き台車に搭載して計測を行う場合、一般に、オドメータ情報には台車のスリップ等による誤差が含まれるため、距離センサの位置には誤差が含まれる。図１５は、距離センサの位置の誤差分布と計測データの誤差分布を合成し、計測データの絶対誤差を算出する場合の模式図である。

従って、本発明の方法と装置によれば、誤差を含む距離データを正確な情報に補正する機能を有すると共に、これを繰り返すことにより、長時間の計測に対して高精度な形状に収束する。なおかつ、本発明の方法は、各ボクセル６に対応する代表点７とその誤差分布８を新たな計測点で更新する処理であるため計算量が小さい。また、演算は周囲のボクセルへの影響を与えずボクセル内で閉じているため、高速処理が可能である。また、計測データは最大のボクセルが必要最小限の分解能を有するボクセル構造に逐次統合可能であり、メモリサイズは固定サイズを大きく上回ることはない。

また、各ボクセルに確率値を持たせることによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで判断できる。

また、センサの位置・姿勢に誤差がある場合でも、この誤差分布を計測データの誤差分布に合成して、計測データの絶対誤差を算出することで、対象物体の位置精度を向上させることができる。

カルマンフィルタを用いたモデル更新ステップについて、詳しく説明する。

カルマンフィルタを用いたモデル更新ステップの場合には、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。

各モデル点群の位置m（ｉ）を状態量とし、距離センサの計測点の位置を基に、モデルを次式（３）で表現する。なお、本実施例では、m（ｉ）は、ボクセル内部の代表点である（以下、同様）。

式（３）において、
Ｌ（ｊ）は、距離センサによる計測位置である。例えば、Ｌ（ｊ）は、距離センサのセンサ座標系において三次元ＬＲＦ（レーザレンジファインダ）の計測点ｊ（ｊ＝１，．．．，Ｎ）の位置Ｌ（ｊ）＝（ｘ_Ｌ（ｊ），ｙ_Ｌ（ｊ），ｚ_Ｌ（ｊ））^ｔである。ここで、ｔは転置行列を示す（以下、同様）。
ｈ_ｍ（Ｒ_ｒ，ｔ_ｒ，ｍ（ｉ））は、Ｌ（ｊ）に対する観測系モデルである。
Ｒ_ｒは、距離センサを搭載した移動体（例えば、移動ロボット）のワールド座標系に対する姿勢を表す回転行列Ｒ_ｒ＝Ｒ（θｘ，θｙ，θｚ）である。なお、θｘ，θｙ，θｚは、それぞれｘ軸、ｙ軸、ｚ軸周りの回転角を示す（以下、同様）。
ｔ_ｒは、距離センサを搭載した移動体（例えば、移動ロボット）のワールド座標系に対する位置を表す並進ベクトルｔ_ｒ＝（ｘ，ｙ，ｚ）である。
ｖ_Ｌ（ｉ）は、距離センサの計測値Ｌ（ｊ）に加わる観測ノイズである。
Ｒ_ｓは、センサ座標系の移動体座標系に対する回転行列Ｒｓ＝Ｒ（θｘ，θｙ，θｚ）である。
ｔ_ｓは、センサ座標系の移動体座標系に対する位置を表す併進ベクトルｔ_ｓ＝（ｘ，ｙ，ｚ）である。

測定対象物は静止しているものであり、測定対象物の位置ｔ_ｒ、姿勢Ｒ_ｒを環境モデルに対して固定する。

距離センサによる計測点群と、環境モデル点群上の点ｉ（即ち、代表点）を対応づける。この対応付けが行われたモデル点群上の点iに対して次式（４）により更新を行う。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点ｍ（i）に対してのみ次式（４）により更新を行ってよい。

式（４）において、
添え字ｋは、離散時刻ｋでの値であることを表す。
ｍ_ｋ（ｉ）について、ｍ’_ｋ（ｉ）はｍ_ｋ（ｉ）の更新値（事後推定値）を示し、ｍ_{ｋ，ｋ−１}（ｉ）はｍ’_ｋ−１（ｉ）に基づいたｍ_ｋ（ｉ）の予測値（事前推定値）を示す。なお、環境（測定対象物）は静止しているので、ｍ_{ｋ，ｋ-１}（ｉ）＝ｍ’_ｋ-１（ｉ）である。
Σ_mk（ｉ）は、ボクセル内部の代表点ｍ_ｋ（ｉ）の誤差共分散行列(即ち、上述の誤差分布)である。また、Σ_ｍｋ（ｉ）について、 Σ’_ｍｋ（ｉ）はΣ_ｍｋ（ｉ）の更新値（事後推定値）を示し、Σ_{ｍｋ，ｋ−１}（ｉ）はΣ’_ｍｋ−１（ｉ）に基づいたΣ_ｍｋ（ｉ）の予測値（事前推定値）を示す。センサ座標系において三次元LRFの計測点j（j=１,…,Ｎ）の位置をＬ（ｊ）で表し、その誤差共分散行列をΣ_Ｌ（ｊ）で表す。ここでＮは、三次元LRFで得られた計測点の総数である。三次元LRFの誤差モデルとして計測距離に関係ない一定の正規分布を仮定する。センサ座標系のｘ軸方向にレーザを照射する場合の誤差共分散行列をΣ_Ｓとする。レーザの照射方向に応じて誤差分布も姿勢を変える。Σ_Ｌ（ｊ）は、基準の方向に対するレーザ照射方向を回転行列Ｒ_Ｌ（ｊ）を用いてΣ_Ｌ（ｊ）＝Ｒ_Ｌ（ｊ）Σ_ＳＲ_Ｌ ^ｔ（ｊ）と表される。計測点jのワールド座標系における位置ｚ（ｊ）、およびその誤差共分散行列Σ_ｚ（ｊ）は、それぞれｚ（ｊ）＝Ｒ_ｒ（Ｒ_ｓＬ（ｊ）＋ｔ_ｓ）＋ｔ_ｒ、Σ_ｚ（ｊ）＝Ｒ_ｒＲ_ｓΣ_Ｌ（ｊ）Ｒ_ｓ ^ｔＲ_ｒ ^ｔと表すことができる。
Ｋ_ｍｋ（ｉ） は、 ｍ_ｋ（ｉ）に対するカルマンゲインである。
ｈ_ｍｋ（Ｒ_ｒｋ，ｔ_ｒｋ，ｍ_{ｋ，ｋ−１}（ｉ））は、Ｌ_ｋ（ｊ）、ｉ＝ｐ_ｋ（ｊ）に対する観測系モデルである。ｉ＝ｐ_ｋ（ｊ）は、計測点ｊに対応付けられた環境地図（即ち、環境モデル）上の点である。
Ｈ_ｍｋは、Ｌ_ｋ（ｊ）、ｉ＝ｐ_ｋ（ｊ）に対する観測系モデルのヤコビアン行列であり、次の式（５）で表わされる。

カルマンフィルタの更新過程によって、環境地図のモデル点群の各点（ボクセルの代表点）の位置と誤差共分散行列の更新値ｍ’_ｋ（ｉ）、Σ’_mk（ｉ）が得られた段階で、環境モデルの更新を以下の手順で行う。
（１）これら更新値ｍ’_ｋ（ｉ）、Σ’_mk（ｉ）を、新たな代表点、誤差分布として再設定する。
（２）上述（１）の結果、代表点の位置が別のボクセル内に移動した場合、移動先のボクセルが代表点を保持していないときは、移動後の代表点とその誤差共分散行列を移動先のボクセルに保持させ、移動元のボクセルからは代表点等を取り除く。移動先のボクセルが既に代表点を保持しているときには、2つの代表点において、これらの両誤差分布が重複するかを判断する（上述のＳＴ４における判断と同様）。その後の処理は、図８のＳＴ４以降の処理と同じであってよい。
（３）モデル点群上の代表点ｍ（i）と対応付けが行われなかった距離センサによる計測点について、当該計測点が含まれるボクセルが代表点を持たない場合は、計測点とその誤差分布をそのボクセルの代表点と誤差分布として追加し保持する。もし、ボクセル内に既に代表点が存在する場合には、ボクセル内にある対応付けが行われなかった他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボクセルに含まれるように、ボクセルを分割した上で分割後のボクセルに代表点等を継承させる。

上述のカルマンフィルタを用いたモデル更新ステップを繰り返すことで、序々にボクセル内の誤差共分散行列（即ち、誤差分布）の範囲が小さくなるとともに、ボクセルが分割され易くなる。ボクセルが分割されることによって、初期ボクセルのサイズ以下の変化も表現することが可能となる。
図１６は、カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。図１７は図１６の一部拡大図である。これら図において、初期のボクセルの1辺の長さを100cmとし、再分割数を6分割まで許している。対象が存在している領域では、ボクセルの再分割を繰り返した結果、計測対象を精度良く表現している。対象が存在しない領域ではボクセルの再分割は行われず、必要十分なデータ量で環境を表現できることがわかる。また、各ボクセル内の代表点の誤差分布も小さく、環境地図を高精度で表現できている。このように、誤差を含むデータであってもカルマンフィルタの効果により、真値に収束した結果が得られる。さらに、この方法では計測データ数を増加させることによって標準偏差が小さくなり、精度のさらなる向上が期待できる。

また、測定対象物の位置・姿勢は固定しているため、更新を測定対象物の位置・姿勢と独立して行うことができる。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点ｍ（i）に対してのみ、上述のカルマンフィルタによる更新を行うことで、大幅な計算コストの削減が可能になる。

計測点と環境地図（即ち、環境モデル）との対応付けについて補足説明をする。対応付けは次のように行える。
対象とする計測点jの誤差共分散行列Σ_Ｌ（ｊ）の範囲（例えば標準偏差の3倍の範囲）と交わる最上位のボクセルとそのボクセルに隣接している最上位のボクセルを求め、下層のボクセルも含めこれらのボクセル内に存在する代表点を対応点の候補とする。ボクセルが階層構造となっているため、この候補点の探索には計算コストはほとんどかからない。このとき、候補となる代表点がない場合には、対応点がないものとみなす。隣接するボクセルも候補に加える理由は、ボクセル内の代表点の位置によっては、誤差共分散行列の範囲が隣接するボクセルまではみ出すことがあるからである。

実施形態として、三次元形状データの記憶・表示方法と装置、および三次元形状の計測方法と装置について説明したが、二次元形状を三次元形状の特別な場合として見ることにより、二次元形状データの記憶・表示方法と装置、および二次元形状の計測方法と装置の形態としても実施できる。
また、上記出力ステップにおいて、前記ボクセル位置、代表点、および誤差分布のすべてを出力しなくてもよく、例えば、これらすべてが無くても三次元形状が把握できる場合や、これらのうち１つ又は２つが必要な場合などにおいては、前記ボクセル位置、代表点、および誤差分布の少なくともいずれかを出力装置に出力してもよい。

なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。

Claims (21)

1. 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶・表示方法であって、
   三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
   前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
   前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、
   前記ボクセル位置、代表点、および誤差分布を出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶・表示方法。
2. 前記モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
3. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
   原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
4. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
   該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
5. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
   前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
   誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
   誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
6. ボクセルの内部に代表点とその誤差分布に加えて、確率値を併せ持つ、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
7. 前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
8. 前記距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する、ことを特徴とする請求項７に記載の三次元形状データの記憶・表示方法。
9. 前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
   該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
   該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項５に記載の三次元形状データの記憶・表示方法。
10. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、
    該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する、ことを特徴とする請求項１に記載の三次元形状データの記憶・表示方法。
11. 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶・表示装置であって、
    三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
    前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
    前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
    前記ボクセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状データの記憶・表示装置。
12. 三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、
    三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
    前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
    前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、
    前記代表点、ボクセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状の計測方法。
13. 前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
14. 前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
15. 前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
16. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
    該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
17. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
    前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
    誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
    誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
18. 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力ステップにおいて、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
19. 前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する、ことを特徴とする請求項１２に記載の三次元形状の計測方法。
20. 三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、
    三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
    前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
    前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
    前記代表点、ボクセル位置および誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置。
21. 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶・表示方法であって、
    三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
    前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
    前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、
    前記代表点、ボクセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶・表示方法。

Images