WO2007069721A1

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

Info

Publication number: WO2007069721A1
Application number: PCT/JP2006/325042
Authority: WO
Inventors: Toshihiro Hayashi; Yukihiro Kawano; Hideo Terada
Original assignee: Ihi Corporation
Priority date: 2005-12-16
Filing date: 2006-12-15
Publication date: 2007-06-21
Also published as: JPWO2007069721A1; JP4650750B2; US8300048B2; CN101331380A; US20090184961A1; DE112006003361T5; CN101331380B

Abstract

　三次元形状上の座標値をコンピュータに入力するデータ入力ステップＳ１と、空間領域を境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップＳ３と、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップＳ４と、環境モデルを更新するモデル更新ステップＳ５と、ボクセル位置、代表点および誤差分布の少なくともいずれかを出力装置に出力する出力ステップＳ６とを有する。

Description

明細書

三次元形状データの記憶'表示方法と装置および三次元形状の計測方法と装置

発明の背景

[0001] 発明の技術分野

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

[0002] 関連技術の説明

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

[0003] 三次元形状データの記憶 ·表示方法と装置として、点群、距離画像、サーフェス、ボタセル構造等が従来力用いられて、る。

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

[0004] 三次元形状計測における計測データは、例えば、計測位置を原点とする極座標値

(r, θ， φ )で表される。本発明において、このデータを「距離データ」と呼ぶ。

[0005] 静止している三次元形状を、複数の位置からの距離データを統合して復元する場合、以下の問題点がある。

点群を用いる場合、計測データに比例してデータ量が増加する。従って、計測対象物が大きい場合の統合データ量は膨大となる。そのため、形状表現のためのデータ構造としては現実的ではな、。

距離画像を用いる場合、一方向力もの計測であるため、遠方に行くほど画素の間隔が広がり精度が低下する。また、一方向からのデータのみしか表すことができないため、複数視点力もの計測結果の統合に向、て、な、。

サーフヱスを用いる場合、統合に複雑な処理を必要とするため、高速処理が困難である。

[0006] これに対し、ボタセル構造は、データ量が一定、複数視点力のデータ統合が可能

、データ統合に複雑な計算を必要としない、という特徴を有し、複数の位置からの距離データを統合して復元するのに最も適してヽる。

[0007] さらに、ボタセル構造を改良したものとして、特許文献 1, 2や非特許文献 1が開示されている。

特許文献 1の「三次元情報抽出方法」では、 Voxel Votingと呼ばれる方法を用いて計測結果を各ボタセルに投票することで、複数視点からの三次元形状を統合する。更にこの方法では、必要な解像度が得られないボタセルに対しては、ボタセルを更に八分割して階層的にデータを管理する方法を開示している。

特許文献 2の「環境モデル入力装置」では、各ボタセルに確率値を蓄積する手段を提案している。この手段では、確率を与える頻度に依存せずに環境モデルの統合を行い、正確な環境モデルの作成を可能としている。

非特許文献 1の手段では、平面を想定しているものの、確率値を保持するボタセルに対して、計測データに含まれる誤差により発生したボタセルの誤った確率値を正し V、値に戻す処理を提案して!/、る。

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

[0009] 特許文献 1 :特許第 3170345号公報、「三次元情報抽出方法」

特許文献 2：特開平 9 81788号公報、「環境モデル入力装置」

[0010] 非特許文献 1： Sebastian Thrun, "Learning Occupancy Grid Maps With

Forward Sensor Models.

非特許文献 2 :関本清英、他、「三次元レーザレーダの開発」、石川島播磨技報 Vol. 43 No. 4 (2003- 7)

発明の開示

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

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

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

発明の要約

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

[0015] 本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶'表示方法であって、

三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモデル構築ステップと、

前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、

前記ボタセル位置、代表点、および誤差分布を出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶'表示方法が提供される。また、本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、

前記代表点、ボタセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状の計測方法が提供される。

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

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

[0018] また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

原点と被計測点の間に物体が存在しな、ものとして、その間に位置するボタセル内の代表点と誤差分布を再設定もしくは消去する。

[0019] また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

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

前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボタセル内の誤差分布とを比較し、

誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことが好ましい。ボタセルの分割には、例えば八分木や K—D木を用いる。

[0021] また、ボタセルの内部に代表点とその誤差分布にカ卩えて、確率値を併せ持つ、ことが好ましい。

また、前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する。

また、距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する、ことが好ま、。

[0022] また、前記モデル更新ステップにお、て、前記新たに取得した誤差分布と前記既に設定したボタセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボタセル内へ移動したとき、

該他のボタセル内に代表点がな!、場合に、該新たな誤差分布と該新たな代表点を該他のボタセルの内部に設定し、

該他のボタセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボタセル内の誤差分布とを比較し、（A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と該新たな代表点から、新たな誤差分布と新たな代表点を再設定し、（B)誤差分布が互いに重複しな、場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボタセルを更に分割して階層的に複数のボタセルに分割する。

[0023] 本発明の好ましい実施形態によれば、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、

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

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

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

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

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

[0028] また本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶 ·表示装置であって、

三次元形状上の座標値をコンピュータに入力するデータ入力装置と、

前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモデル構築装置と、

前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、

前記ボタセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状データの記憶 ·表示装置が提供される。また本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、

前記代表点、ボタセル位置および誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置が提供される。

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

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

[0031] 本発明の好ましい実施形態によれば、前記三次元形状データの記憶'表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、

該モデル更新装置は、

新たに入力された被計測点の座標値に対応するボタセルを探索し、原点と被計測点の間に物体が存在しな、ものとして、その間に位置するボタセル内の代表点と誤差分布を再設定もしくは消去する。

[0032] 本発明の好ましい実施形態によれば、前記三次元形状データの記憶'表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、

該モデル更新装置は、

新たに入力された被計測点の座標値に対応するボタセルを探索し、

該ボクセル内に代表点がな、場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。

[0033] 本発明の好ましい実施形態によれば、前記三次元形状データの記憶'表示装置または三次元形状の計測装置は、前記マッチング装置が前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶した後に、前記環境モデルを更新するモデル更新装置を有し、

該モデル更新装置は、

誤差分布が互いに重複する場合に、両誤差分布、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力された被計測点の座標値から、力新たな誤差分布と新たな代表点を再設定し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する。

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

[0035] 本発明の好ましい実施形態によれば、前記三次元形状データの記憶'表示装置または三次元形状の計測装置は、前記三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する距離センサを備える。 [0036] 本発明の好まヽ実施形態によれば、前記マッチング装置は、前記距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する。

[0037] 本発明の好ましい実施形態によれば、前記モデル更新装置は、

前記新たに取得した誤差分布と前記既に設定したボタセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボタセル内へ移動したとき、

[0038] 本発明の好ましい実施形態によれば、前記三次元形状データの記憶'表示装置または三次元形状の計測装置は、前記マッチングステップの後に、前記環境モデルを更新するモデル更新装置を有し、

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

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

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

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

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

[0043] 発明の効果

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

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

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

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

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

[0048] 更に、前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボタセル内の誤差分布とを比較し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。

特に、誤差分布が互いに重複する場合に、新たな誤差分布と新たな代表点を再設定した結果、代表点が別のボタセルに移動する場合には、既に設定したボタセル内の誤差分布と更に比較することで、単一のボタセル内に単一の代表点のみが存在するように一貫性を保つことができる。

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

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

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

[0052] さらに、前記モデル更新ステップにお、て、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボタセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定するので、より真値に近、形状を得ることができる。

特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含むデータであってもカルマンフィルタの効果により真値に収束した高精度な形状が得られる。

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

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

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

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

[0057] また、前記出力ステップにおいて、距離センサの位置力も距離センサが位置計測可能な範囲の環境モデル内のボタセルの代表点の位置を三次元形状の計測値として出力装置に出力することによって、従来の距離センサの計測値の分解能が粗い場合でも、あた力も精度良くかつ分解能が高い距離センサのように利用することが可能となる。このように、より精度の高い三次元形状計測が可能になる。

本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から明らかになろう。

図面の簡単な説明

[0058] [図 1]非特許文献 2に開示された三次元レーザレーダの構成図である。

[図 2A]距離センサで計測された極座標データと誤差の関係を示す図である。

[図 2B]誤差分布を直方体に包含される楕円体として近似する場合を示している。

[図 3]本発明の方法を実行するための装置構成図である。

[図 4]本発明の方法を示すフローチャートである。

[図 5]モデル構築ステップの模式図である。

[図 6]構築された環境モデルの模式図である。

[図 7A]本発明におけるボタセルデータのデータ構造を示す図であり、各ボタセルデータのメモリレイアウト例を示して、る。

[図 7B]本発明におけるボタセルデータのデータ構造を示す図であり、レベル 2 (1, 1 , 0)のボタセルが代表点を持つ場合の例を示している。 [図 8]モデル更新ステップにおけるデータ処理フロー図である。

[図 9]該当するボタセル内に既に設定した代表点がある場合の模式図である。

[図 10]誤差分布が互いに重複する場合に、両誤差分布カゝら新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボタセル内へ移動する場合を示している。

[図 11]誤差分布が互いに重複する場合の模式図である。

[図 12]複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示す模式図である。

[図 13]ボタセルの確率値の算出方法の模式図である。

[図 14]誤差分布が互、に重複する場合のボタセルの確率値の算出方法の模式図である。

[図 15]距離センサの位置の誤差分布と計測データの誤差分布を合成し、計測データの絶対誤差を算出する場合の模式図である。

[図 16]カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。

[図 17]図 16の一部拡大図である。

好ましい実施例の説明

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

[0060] 図 1は、距離センサの一例としての三次元レーザレーダの構成図である。三次元レ一ザレーダは、例えば非特許文献 2に開示されて、る。

この図に示すように、三次元レーザレーダ 10は、レーダヘッド 12と制御器 20から構成される。レーザダイオード 13から発振されたノルスレーザ光 1は、投光レンズ 14で平行光 2に整形され、ミラー 18a, 18bと回転 '揺動するポリゴンミラー 15で二次元方向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ光 3は、ポリゴンミラー 15を介して受光レンズ 16で集光され、光検出器 17で電気信号に変換される。

[0061] 制御器 20内の時間間隔カウンタ 21は、レーザダイオード 13のパルス発振タイミングと同期したスタートパルス 4と、光検出器 17から出力されたストップパルス 5の時間間隔を計測する。信号処理ボード 22は、反射光が検出された時点の時間間隔 t、ポリゴンミラーの回転角度 Θ、揺動角度 φを極座標データ (r, θ , φ )として出力する。

rは計測位置 (レーダヘッド設置位置)を原点とする距離であり、 r=c X t/2 の式で求められる。ここで cは光速である。

判定処理ユニット 23は、信号処理ボード力もの極座標データを、レーダヘッド設置位置を原点とした三次元空間データ (X, y, z)へ変換して、検出処理を行うようになつて、る。なおこの図で 24はドライブユニットである。

[0062] 上述した三次元レーザレーダ 10の計測範囲は、例えば、水平画角 60° 、垂直画角 30° 、最大測定距離 50mである。また、位置検出精度は、例えば約 20cmであるまた、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示する場合、 1フレームの計測点数を、横方向 166点、スキャン方向 50点とすると、 1フレームに 166 X 50 = 8300点力 S表示される。この場合にフレームレートは、例えば約 2フレーム Z秒である。

[0063] この三次元レーザレーダ 10で計測される三次元形状上の被計測点は、横方向に

Δ Θ X r,垂直方向に Δ φ X r、互いに離散した点群となる。例えば、 Δ Θ =60/16 6 X π /180 = 6. 3 X 10—³ラジアン、 Δ φ = 30/50 X π /180= 10. 5 X 10—³ラジアン、 r= 50mの場合、最も近接する場合でも、被計測点の間隔は、横方向で約 3 15mm,垂直方向で約 525mmとなる。

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

[0065] 図 2A,図 2Bは、距離センサで計測された極座標データと誤差の関係を示す図である。

図 2Aに示すように、任意の計測位置を原点とする極座標値 (r, θ , φ )を計測結果として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存在する。

この誤差分布は、誤差分布の r , θ , φ での存在確率を P (r , θ , φ )とした場合、誤差分布は計測の軸 r, θ , φ方向に正規分布しているとし、例えば式（1)で表すことができる。ここで、 r， θ， φはセンサ力もの計測値、 σ , σ , σ はは標準偏差

r θ φ

、 Aは規格ィ匕定数である。

図 2Bに示すように、誤差分布は、通常 r方向に長い切頭円錐形 (左図）に内包される分布である力遠方において aと bの差は小さい。従って、この誤差分布を直方体に包含される楕円体として安全サイドに近似することができる。

[0066] [数 1]

(θ_χ - θ)² ~

exp exp ( - Φ) (1)

A V 2²σ^σ, ノ 2²σび 2 2a

J 2a ノ

[0067] 図 3は、本発明の方法を実行するための装置構成図である。この図に示すように、この装置は、データ入力装置 32、外部記憶装置 33、内部記憶装置 34、中央処理装置 35および出力装置 36を備える。

[0068] データ入力装置 32は、上述した距離センサを有し、三次元形状上の座標値を記憶装置に入力する。また、例えばゴ-ォメータ、ォドメータ等を併用して、距離センサの位置 ·姿勢や移動距離も入力するのがよい。なお、データ入力装置 32は、キーボード等の通常の入力手段も有するのがよい。

外部記憶装置 33は、ハードディスク、フロッピ (登録商標）一ディスク、磁気テープ、コンパクトディスク等である。外部記憶装置 33は、環境モデルのサイズが大きく後述する内部記憶装置 34に入力された三次元形状上の座標値、ボタセル位置、及び代表点とその誤差分布の全体を保持できなヽ場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボタセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。

内部記憶装置 34は、例えば RAM, ROM等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボタセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。

中央処理装置 35 (CPU)は、モデル構築装置、マッチング装置、モデル更新装置、データ伝達装置として機能し、演算や入出力等を集中的に処理し、内部記憶装置

34と共〖こ、プログラムを実行する。モデル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後述のマッチングステップを行う装置であり、モデル更新装置は、後述のモデル更新ステップを行う装置であり、データ伝達装置は、出力装置 36へデータを出力する装置である。

出力装置 36は、例えば表示装置、プリンタ、外部装置等であり、内部記憶装置 34 および外部記憶装置 33の少なくとも、ずれかに記憶したデータ及びプログラムの実行結果を出力するようになっている。外部装置とのインターフェイスは、 LAN、 USB、 IEEE 1394等であり、入力された三次元形状上の座標値に対して該当するボタセル内の代表点、誤差分布、ボタセル位置などを付加した結果や、環境モデル全体または環境モデル一部を要求に応じて出力する。

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

[0070] 図 4は、本発明の方法を示すフローチャートである。

本発明の方法は、三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状の計測方法と装置であり、データ入力ステップ Sl、データ補正ステツプ S2、モデル構築ステップ S3、マッチングステップ S4、モデル更新ステップ S5 及び出力ステップ S6を有する。

なお、これら一連の処理のうち、 Sl、 S2、 S4〜S6は、計測データが得られる毎に実施し、 S3は初めて計測データが得られたときにだけ実施することができる。

[0071] データ入力ステップ S1では、距離センサを用いて、三次元形状上の座標値をコンピュータの記憶装置に入力する。また、例えばゴ-ォメータ、ォドメータ等を併用して

、距離センサの位置 ·姿勢や移動距離も入力するのがよい。

なおこのデータ入力ステップ S1において、三次元レーザレーダ 10を用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しな力順次取得するのがよい。

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

[0073] データ補正ステップ S2では、距離データの補正処理を行い、距離データの精度を向上させる。また、極座標データとォドメータのデータから、任意の固定位置を原点とした三次元空間データ (X, y, z)へ変換してもよい。

距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、周囲の点力孤立して存在する点であり、計測データは複数の近接する点で構成されることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計測データが含む誤差分布を考慮して、複数回の計測を統計処理 (例えば平均値等）することで、距離の補正を行う。

さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを行うのがよい。

[0074] 図 5は、ボタセルの分割に八分木を用いた場合のモデル構築ステップの模式図である。

モデル構築ステップ S3では、この図に示すように、三次元形状の存在する空間領域を、境界表面が互いに直交する直方体力なる複数のボタセル 6に分割し、各ボタセル位置を記憶する環境モデルを構築する。

ボタセル 6の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体でもよい。

また、ボタセル 6の各辺の長さは、最大のボタセル 6を必要最小限の分解能に相当する大きさに設定するのがよい。以下、最大のボタセル 6をレベル 1のボタセルと呼ぶまた、単一のボタセル内に複数の被計測点が存在する場合には、単一のボタセル内に単一の被計測点のみが存在するように、ボタセルを更に八分割して階層的に複数のボタセルに分割する。以下、最大のボタセル 6の八分割を 1回実施した空間領域をレベル 2のボタセル、 k回実施した空間領域をレベル k+ 1のボタセルと呼ぶ。

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

[0076] マッチングステップ S4にお、て、代表点の絶対位置は式（2)で与えられる。ここで、

(X, y, z)は代表点のボタセルでの相対座標、 Sx, Sy, Szはレベル 1でのボタセルの一辺の大きさ、 n (k) , n (k) , n (k)はレベル kでのボタセルの番地、 Lは求める代表点が存在するレベルである。

[0077] [数 2]

{X ' ， ^Ζ) (k) …②

[0078] 図 7A,図 7Bは、本発明におけるボタセルデータのデータ構造を示す図である。

この図において、図 7Aは、各ボタセルデータのメモリレイアウト例である。この図において、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。図 7Bは、レベル 2 (1, 1, 0)のボタセルが代表点を持つ場合の例を示している。なおこの図において、 nullは空集合を表す。

[0079] 上述したデータ構造の環境モデルは、以下の特徴を有する。

( 1)内容:空間を小直方体で分割して各ボタセルに計測点の代表点と誤差分布を保持する。

(2)精度:ボタセル毎に持つ計測点の代表値相当である。

(3)存在：物体の存在の有無を表現できる。

(4)データ量:ボタセルの個数に比例してメモリを必要とする力サイズ固定である。

(5)点群からの変換：適しており、計算量は少な!/、。

(6)アクセス速度：シンプルな構造をして、るため、要素へのアクセスが高速である。

[0080] またこの特徴から、上述した環境モデルは、以下の効果 A〜Cをすベて満たしている。

効果 A：誤差を考慮した表現が可能である。効果 B：必要なメモリ量と計算量が一定量以下である。

効果 C：物体の存在だけでなく、物体が存在しな!、ことを表せる。

[0081] さらに図 4において、モデル更新ステップ S5は、マッチングステップ S4の後に実施し、モデル構築ステップ S3で構築した環境モデルを更新する。

[0082] 図 8は、モデル更新ステップ S5におけるデータ処理フロー図である。この図に示すように、ステップ ST1で新たに入力された被計測点の座標値に対応するボタセルを探索し、ステップ ST2で該当するボタセル内に代表点がない (ボタセルが空である）場合には、ステップ ST3で新たに入力された被計測点の座標値と誤差分布を代表点の座標値と誤差分布として設定 (新規に登録)する。

また、このステップ ST3において、新しい計測位置 (原点）と被計測点の間には、原理的に物体が存在しないはずである。従って新しい計測位置 (原点）と被計測点の間に位置するボタセル内の代表点と誤差分布を再設定もしくは消去する。

[0083] 図 9は、該当するボタセル内に既に設定した代表点がある場合の模式図である。

図 8のステップ ST2で該当するボタセル内に既に設定した代表点がある場合には、ステップ ST4で新たに取得した誤差分布と既に設定したボタセル内の誤差分布とを比較する (すなわち異なる点か同一点かを判断する)。

この比較で、誤差分布が互いに重複する場合（図 9A)には、ステップ ST5で両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定する (すなわち誤差分布を合成する)。

またこの比較で、誤差分布が互いに重複しない場合（図 9B)には、ステップ ST6、 S T7で単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボタセルに分割し新規に登録する。

分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度には例えば、マハラノビス距離のような距離尺度や、尤度のような確率値の尺度を利用できる。また、 2つの誤差分布に基づき、両者が同一点を表している力を統計的検定によって判定してもよい。

[0084] ステップ ST5で両誤差分布から新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボタセル内へ移動したとき（即ち、ステップ ST8で、 Y es)、ステップ ST2へ戻り、上述の処理を繰り返す。

なお、図 10は、ステップ ST5で両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボタセル内へ移動する場合を示している。

[0085] 図 11は、誤差分布が互いに重複する場合（図 9A)の別の模式図である。図 8のステツプ ST5にお、て、 2つの代表点と誤差分布を合成して新たな代表点と誤差分布を設定する手段として、カルマンフィルタを用いることができる。例えば、二次元の場合に、この図に示すように、 2つの代表点をそれぞれ x (l) , x' (2)、 2つの誤差分布を∑ (1)、∑ ' (2)とし、これを合成した代表点を X (2)、誤差分布を∑ (2)とすると、代表点 X (2)と誤差分布∑ (2)を算出する模式図は図 11のようになる。

[0086] 図 4において、出力ステップ S6では、ボタセル位置、及び代表点とその誤差分布を出力装置 36に出力する。出力装置 36が表示装置 (例えば CRT)の場合、三次元画像上に立体表示するのが好ましい。

また、出力ステップ S6では、ボタセル位置、及び代表点とその誤差分布を別の装置 (例えば制御装置、コンピュータ）に転送してもよぐプリンタで出力してもよい。即ち、出力装置 36は、当該別の装置 (例えば制御装置、コンピュータ)であってよい。なお、出力装置 36は、ボタセル位置、及び代表点とその誤差分布を記憶'保持できる適切な記憶装置であってもよ、。

また、出力ステップ S6において、ボタセルの代表点の位置を三次元形状の計測値として出力装置 36に出力するとともに、該計測値の信頼性または精度を示す指標（例えば、数値)を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置 36 に出力してもよい。さらに、出力ステップ S6において、ボタセルの代表点の位置を三次元形状の計測値として出力装置 36に出力するときに、該ボクセルの内部の誤差分布の大きさ (広がり）が所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値 (即ち、このボタセルの代表点の位置）を出力装置 36に出力しな、ようにしてもょ、。 [0087] また、モデル更新ステップ S5にお、て、新たに入力された被計測点の座標値に対応するボタセルを探索し、前記ボタセル内の代表点、および誤差分布が新たに設定されるカゝ、または再設定されるカゝ、または該ボクセルを更に分割して階層的に複数のボタセルに分割される場合のみ、出力ステップ S6において、当該ボタセルの代表点の位置、誤差分布およびボタセルの位置の少なくとも、ずれかを三次元形状の計測値として出力装置に出力してもよい。

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

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

[0090] 上述した本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボタセル 6に分割し、各ボタセル位置を外部記憶装置 33に記憶するので、計測対象物が大き、場合であっても、データ量をボタセル数に比例する小さ、データサイズに抑えることができる。

また、座標値に対応するボタセル 6の内部に代表点 7とその誤差分布 8を設定し記憶するので、ボタセルの分解能以上の情報を表現することができる。

[0091] また、モデル構築ステップ S3にお、て、最大のボタセル 9を必要最小限の分解能に相当する大きさに設定し、かつ単一のボタセル 9内に複数の被計測点が存在する場合に、単一のボタセル内に単一の被計測点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボタセルに分割するので、データ量を小さいデータサイズに抑えると同時に、分割後のボタセルと代表点を用いて解像度を更に高めることがでさる。 [0092] 特に、三次元形状上の複数の座標値を複数の計測位置を原点とする距離データとして取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる図 12は、複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示している。このように異なる計測位置 (即ち、距離センサである三次元計測器の位置)を原点として得られた距離データは誤差分布の向きも異なるので、これらの距離データを環境モデルを介して逐次統合することによって、代表点の誤差分布が縮小し、代表点の位置精度が向上する。なお、図 12において、三次元計測後の図はコップの 2次元断面を表わした模式図であり、三次元計測後の図の破線はコップの実際表面を表わしている。

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

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

[0095] 更に、前記ボタセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボタセル内の誤差分布とを比較し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボタセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。

[0096] 各ボタセルの確率値は、代表点と誤差分布力も算出する。ボタセルの確率値の算出方法は、例えば、図 13に示すように、誤差分布の全体に対してボタセルに内包される分布の比力も求めることができる。

また、模式図を図 14に示すように、 2つ以上の誤差分布が交差するような場合は、例えば、それぞれの誤差分布に対する確率値を用いたベイズ推定等により確率値の合成を行う。

[0097] 距離センサの位置 ·姿勢の誤差分布が得られる場合は、この誤差分布を計測データの誤差分布に合成して、計測データの絶対誤差を算出することができる。

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

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

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

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

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

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

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

[0104] [数 3]

L(j) = h_m(R_r，t_r，m(i)) + v_L(j)

= R (R_r- mG) - t_r)一 t_s) + v_L( ( し， N) -⁽³⁾

[0105] 式（3)において、

L(j)は、距離センサによる計測位置である。例えば、 L(j)は、距離センサのセンサ座標系において三次元 LRF (レーザレンジファインダ）の計測点; j(j = l, ... , N)の位置 L(j) = (x (j), y (j), z (j))tである。ここで、 tは転置行列を示す (以下、同様）

ししし h (R , t , m(i) )は、 L (j)に対する観測系モデルである。

m r r

Rは、距離センサを搭載した移動体 (例えば、移動ロボット）のワールド座標系に対する姿勢を表す回転行列 R=R(0x, θγ, 0ζ)である。なお、 0χ, θγ, 0ζは、それぞれ X軸、 y軸、 z軸周りの回転角を示す (以下、同様)。

ま、距離センサを搭載した移動体 (例えば、移動ロボット）のワールド座標系に対する位置を表す並進ベクトル t = (X, y, z)である。

v (i)は、距離センサの計測値 L(j)に加わる観測ノイズである。

し

Rは、センサ座標系の移動体座標系に対する回転行列 Rs=R(0x, θγ, 0ζ)で s

ある。

tは、センサ座標系の移動体座標系に対する位置を表す併進ベクトル t = (X, y, z s s

)である。

[0106] 測定対象物は静止しているものであり、測定対象物の位置、姿勢！^を環境モデルに対して固定する。

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

[0108] [数 4]

K_mk(i)=∑_mk (i)H_mk(j) H_mk(j)∑_mk (i)H_mk +∑_Lk(j))- 1

m'k(i) ⁼ ¾k-i(i) +K_mk(i)(L_k(j) - h_mk(R_rk，t_rk，m_k，_k ))） --(4) mk(i) = 2_mkk— i) - K_mk(i)H_mk(j)∑_mkk— i)

[0109] 式（4)において、

添え字 kは、離散時刻 kでの値であることを表す。

m (i)につ、て、 m， (i)は m (i)の更新値 (事後推定値)を示し、 m (i)は m， k k k k, k-1 k-

(i)に基づ!ヽた m (i)の予測値 (事前推定値)を示す。なお、環境 (測定対象物）は

1 k

静止しているので、 m (i) =m，（i)である。

k, k-1 k-1

∑ (i)は、ボタセル内部の代表点 m (i)の誤差共分散行列 (即ち、上述の誤差分 mk k

布)である。また、∑ (i)につ、て、 ∑， (i)は∑ (i)の更新値 (事後推定値)を示 mk mk mk

し、∑ (i)は∑ ' (i)に基づ!/、た∑ (i)の予測値 (事前推定値)を示す。セ mk, k— 1 mk— 1 mk

ンサ座標系にお、て三次元 LRFの計測点 j (j=l ,… ,Ν)の位置を L (j)で表し、その誤差共分散行列を∑ (j) F

しで表す。ここで Nは、三次元 LR で得られた計測点の総数である。三次元 LRFの誤差モデルとして計測距離に関係な、一定の正規分布を仮定する。センサ座標系の X軸方向にレーザを照射する場合の誤差共分散行列を∑ Sとする。レーザの照射方向に応じて誤差分布も姿勢を変える。∑ (j)

しは、基準の方向に対するレーザ照射方向を回転行列 R (j) j)

しを用いて∑ (j)=R (j)∑ R t(

しし S しと表される。計測点 jのワールド座標系における位置 z(j)、およびその誤差共分散行列∑ z ( j)は、それぞれ z(j)=R (RL(j)+t)+t、 ∑ (j)=RR∑ (j)RtRtと表すことがで r s s r z r s L s r

きる。

K (i)は、 m (i)に対するカルマンゲインである。

mk k

(R , t , m (i))は、 L (j)、 i=p (j)に対する観測系モデルである。 i=p (j )は、計測点 jに対応付けられた環境地図（即ち、環境モデル)上の点である。

H は、 L (j)、 i=p (j)に対する観測系モデルのヤコビアン行列であり、次の式（5 mk k k

)で表わされる。

[0110] [数 5]

[0111] カルマンフィルタの更新過程によって、環境地図のモデル点群の各点（ボタセルの代表点)の位置と誤差共分散行列の更新値 m' (i)、∑ ' (i)が得られた段階で、環 k mk

境モデルの更新を以下の手順で行う。

(1)これら更新値 m' (i)、∑ ' (i)を、新たな代表点、誤差分布として再設定する。

k mk

(2)上述（1)の結果、代表点の位置が別のボタセル内に移動した場合、移動先のボクセルが代表点を保持して、な、ときは、移動後の代表点とその誤差共分散行列を移動先のボタセルに保持させ、移動元のボタセルからは代表点等を取り除く。移動先のボタセルが既に代表点を保持しているときには、 2つの代表点において、これらの両誤差分布が重複するかを判断する（上述の ST4における判断と同様)。その後の処理は、図 8の ST4以降の処理と同じであってよい。

(3)モデル点群上の代表点 m (i)と対応付けが行われな力つた距離センサによる計測点について、当該計測点が含まれるボタセルが代表点を持たない場合は、計測点とその誤差分布をそのボタセルの代表点と誤差分布として追加し保持する。もし、ボクセル内に既に代表点が存在する場合には、ボタセル内にある対応付けが行われな力つた他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボタセルに含まれるように、ボタセルを分割した上で分割後のボタセルに代表点等を継承させる。

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

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

[0114] 計測点と環境地図 (即ち、環境モデル)との対応付けについて補足説明をする。対応付けは次のように行える。

対象とする計測点 jの誤差共分散行列∑ (j) 3

しの範囲 (例えば標準偏差の倍の範囲

)と交わる最上位のボタセルとそのボタセルに隣接して、る最上位のボタセルを求め、下層のボタセルも含めこれらのボタセル内に存在する代表点を対応点の候補とする。ボタセルが階層構造となっているため、この候補点の探索には計算コストはほとんどかからない。このとき、候補となる代表点がない場合には、対応点がないものとみなす。隣接するボタセルも候補に加える理由は、ボタセル内の代表点の位置によっては、誤差共分散行列の範囲が隣接するボタセルまではみ出すことがあるからである。

[0115] 実施形態として、三次元形状データの記憶 ·表示方法と装置、および三次元形状の計測方法と装置にっ、て説明したが、二次元形状を三次元形状の特別な場合として見ることにより、二次元形状データの記憶'表示方法と装置、および二次元形状の計測方法と装置の形態としても実施できる。

また、上記出力ステップにおいて、前記ボタセル位置、代表点、および誤差分布のすべてを出力しなくてもよぐ例えば、これらすべてが無くても三次元形状が把握できる場合や、これらのうち 1つ又は 2つが必要な場合などにおいては、前記ボタセル位置、代表点、および誤差分布の少なくともいずれかを出力装置に出力してもよい。なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々〖こ変更することができることは勿論である。

Claims

請求の範囲

[1] 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶 ·表示方法であって、

前記ボタセル位置、代表点、および誤差分布を出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶 ·表示方法。

[2] 前記モデル構築ステップにお、て、最大のボタセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボタセル内に複数の被計測点が存在する場合に、単一のボタセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[3] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

原点と被計測点の間に物体が存在しな、ものとして、その間に位置するボタセル内の代表点と誤差分布を再設定もしくは消去する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[4] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

該ボクセル内に代表点がな、場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[5] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[6] ボタセルの内部に代表点とその誤差分布に加えて、確率値を併せ持つ、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[7] 前記データ入力ステップにお、て、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[8] 前記距離センサの位置と誤差分布が得られる場合に、距離センサの位置の誤差分布と計測データの誤差分布を合成する、ことを特徴とする請求項 7に記載の三次元形状データの記憶 ·表示方法。

[9] 前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボタセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布力新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、

該他のボタセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボタセル内の誤差分布とを比較し、（A)誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボタセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（B)誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことを特徴とする請求項 5に記載の三次元形状データの記憶 ·表示方法。

[10] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、

該モデル更新ステップにお、て、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボタセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する、ことを特徴とする請求項 1に記載の三次元形状データの記憶 ·表示方法。

[11] 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶 ·表示装置であって、

前記ボタセル位置、代表点、および誤差分布を出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状データの記憶 ·表示装置。

[12] 三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、

前記代表点、ボタセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状の計測方法。

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

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

[15] 前記データ入力ステップにお、て、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する、ことを特徴とする請求項 12に記載の三次元形状の計測方法。

[16] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

該ボクセル内に代表点がな、場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項 12に記載の三次元形状の計測方法。

[17] 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボタセルを探索し、

誤差分布が互いに重複しない場合に、単一のボタセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボタセルに分割する、ことを特徴とする請求項 12に記載の三次元形状の計測方法。

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

[19] 前記出力ステップにおいて、距離センサの位置力距離センサが位置計測可能な範囲の環境モデル内のボタセルの代表点の位置を三次元形状の計測値として出力装置に出力する、ことを特徴とする請求項 12に記載の三次元形状の計測方法。

[20] 三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、

前記代表点、ボタセル位置および誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置。

[21] 三次元形状上の被計測点の座標値から三次元形状を復元するための三次元形状データの記憶 ·表示方法であって、

三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボタセルに分割し、各ボタセル位置を記憶する環境モデルを構築するモデル構築ステップと、前記座標値に対応するボタセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップと、

前記代表点、ボタセル位置および誤差分布の少なくともいずれかを出力装置に出力する出力ステップとを有する、ことを特徴とする三次元形状データの記憶 ·表示方法。