JP4650752B2 - 自己位置同定方法と装置および三次元形状の計測方法と装置 - Google Patents
自己位置同定方法と装置および三次元形状の計測方法と装置 Download PDFInfo
- Publication number
- JP4650752B2 JP4650752B2 JP2007550243A JP2007550243A JP4650752B2 JP 4650752 B2 JP4650752 B2 JP 4650752B2 JP 2007550243 A JP2007550243 A JP 2007550243A JP 2007550243 A JP2007550243 A JP 2007550243A JP 4650752 B2 JP4650752 B2 JP 4650752B2
- Authority
- JP
- Japan
- Prior art keywords
- voxel
- self
- measurement
- error distribution
- dimensional shape
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/77—Determining position or orientation of objects or cameras using statistical methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Probability & Statistics with Applications (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar, Positioning & Navigation (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Description
発明の技術分野
本発明は、外界の三次元情報を取り入れて、自己位置を同定する自己位置同定方法と装置および三次元形状の計測方法と装置に関する。
本発明は、外界の三次元情報を取り入れて、自己位置を同定する自己位置同定方法と装置および三次元形状の計測方法と装置に関する。
自己位置を直接同定する手段として、ＧＰＳ、ＤＧＰＳ、磁気マーカ、磁気方式、無線方式、画像マーカ方式、光学式・電磁式マーカ方式等が知られている。しかしこれらの手段は、安定した位置同定が可能であるというメリットはあるが、インフラが必要であり、位置検出が可能な範囲が限られ、設置のための手間を必要とする等の問題点がある。
これに対して、インフラを用いずにセンサにより周囲環境を計測して獲得した環境情報と、新たに獲得した計測データを比較しながら自己位置を算出する自己位置同定手段が種々提案されている。
この目的に適した三次元センサとして、超音波センサ、画像センサ、距離センサが挙がられる。しかし、超音波センサは、計測範囲が狭く精度が低い問題点があり、画像センサは照明条件に影響を受けやすい、という問題点がある。
この目的に適した三次元センサとして、超音波センサ、画像センサ、距離センサが挙がられる。しかし、超音波センサは、計測範囲が狭く精度が低い問題点があり、画像センサは照明条件に影響を受けやすい、という問題点がある。
これらに対し、距離センサは超音波センサに比べて広範囲の距離データを精度良く計測でき、かつ、画像処理のように照明の変化に影響を受けることがないため、外乱光の影響を受けずに安定した計測を行うことができる利点がある。
距離センサを用いた位置同定の手段は、非特許文献１、特許文献１、等に提案されている。
非特許文献１の手段は、三次元スキャン式レーザセンサを用いて周囲環境計測により獲得した環境モデルと、逐次得られる計測データをＩＣＰアルゴリズムにより比較しながら位置合わせを行うものである。
ＩＣＰアルゴリズムは、対応点が既知でない場合の位置合わせ手段の１つであり、先の計測データに対するその後の計測データの最も近傍の点を求め、回転・並進を行いながらその距離の総和が最小になる状態を一致状態となるように解を求めるものである（非特許文献２）。
ＩＣＰアルゴリズムは、対応点が既知でない場合の位置合わせ手段の１つであり、先の計測データに対するその後の計測データの最も近傍の点を求め、回転・並進を行いながらその距離の総和が最小になる状態を一致状態となるように解を求めるものである（非特許文献２）。
特許文献１の「自己位置同定装置」は、特に無軌道でかつ外界にランドマーク等の設備を設けないものであり、あらかじめ移動体を目的とする軌道に沿って手動で動作させる（ティーチング）ことで、外環境の地図を自動的に記憶させるものである。
この特許文献１では、図１に示すように、平面内を移動する移動体１０１の、該平面内における周囲外環境物体１０２との位置関係を、離散化したデータとしてメモリ上に保持する外環境地図を用いる。前記データは前記移動体上に固定された点を原点１０３とし、所定の間隔に離散化した前記移動体からの方向データ１０４と、前記方向における所定の間隔に離散化した前記周囲外環境物体までの距離データ１０５との組からなる。
この特許文献１では、図１に示すように、平面内を移動する移動体１０１の、該平面内における周囲外環境物体１０２との位置関係を、離散化したデータとしてメモリ上に保持する外環境地図を用いる。前記データは前記移動体上に固定された点を原点１０３とし、所定の間隔に離散化した前記移動体からの方向データ１０４と、前記方向における所定の間隔に離散化した前記周囲外環境物体までの距離データ１０５との組からなる。
また、本発明に関連する技術として、非特許文献３が開示されている。
Ａｎｄｒｅａｓ Ｎｕｃｈｔｅｒ，ｅｔ ａｌ．，"６Ｄ ＳＬＡＭ ｗｉｔｈ ａｎ Ａｐｐｌｉｃａｔｉｏｎ ｉｎ Ａｕｔｏｎｏｍｏｕｓ Ｍｉｎｅ Ｍａｐｐｉｎｇ"，ＩＥＥＥ ２００４ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ （ＩＣＲＡ ’０４）， Ａｐｒｉｌ ２００４
Ｐａｕｌ Ｊ．Ｂｅｓｌ，"Ａ Ｍｅｔｈｏｄ ｆｏｒ Ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ３−Ｄ Ｓｈａｐｅｓ"，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｆ Ｐａｔｔｅｒｎ Ａｎａｌｙｓｉｓ ａｎｄ Ｍｅｃｈａｎｉｃａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ， Ｖｏｌ．１４， Ｎｏ．２， Ｆｅｂｒｕａｒｙ １９９２
関本清英、他「三次元レーザレーダの開発」、石川島播磨技報Ｖｏｌ．４３ Ｎｏ．４（２００３−７）
三次元レーザレーダのような距離センサを用いる場合、計測される三次元形状上の被計測点は、横方向及び垂直方向に離散した点群となる。この点群の間隔は、計測点からの距離が例えば５０ｍの場合、被計測点の間隔は、例えば横方向で約３１５ｍｍ、垂直方向で約５２５ｍｍに達する。
また、複数の計測位置から静止している三次元形状を計測する場合、三次元レーザレーダのような距離センサでは、計測位置毎に、被計測点の位置は通常異なる。
さらに、このような距離センサは、一般に測定距離に例えば約２０ｃｍ前後の誤差を有する。
従って、三次元レーザレーダのような距離センサを用いる場合、以下の制約条件Ａ〜Ｃがある。
条件Ａ：計測データに点数が少ない（例えば、１フレーム＝１６６×５０点）
条件Ｂ：計測データに誤差を含む（例えば、測定距離に約２０ｃｍ前後）
条件Ｃ：計測データは同じ計測点を計るとは限らない。
すなわち、得られる距離データは横方向及び垂直方向に離散した点群であり、計測毎に位置が相違するため対応点がなく、測定距離に比較的大きな誤差を含んでいる。
さらに、このような距離センサは、一般に測定距離に例えば約２０ｃｍ前後の誤差を有する。
従って、三次元レーザレーダのような距離センサを用いる場合、以下の制約条件Ａ〜Ｃがある。
条件Ａ：計測データに点数が少ない（例えば、１フレーム＝１６６×５０点）
条件Ｂ：計測データに誤差を含む（例えば、測定距離に約２０ｃｍ前後）
条件Ｃ：計測データは同じ計測点を計るとは限らない。
すなわち、得られる距離データは横方向及び垂直方向に離散した点群であり、計測毎に位置が相違するため対応点がなく、測定距離に比較的大きな誤差を含んでいる。
このような距離データを上述した「ＩＣＰアルゴリズム」で処理する場合、以下の問題点がある。
（１）誤差の蓄積
ＩＣＰアルゴリズムは、２つの距離データの重ね合わせ手段であり、先のデータとその後のデータとの比較を繰り返し、その差分を積分しても、対応点がほとんどないため、誤差が蓄積してしまう。
（２）計算量が多い
ＩＣＰアルゴリズムは、繰り返し計算であるため、計算量が膨大となる。すなわち、ＩＣＰアルゴリズムは、計測データの各データ点に対応するモデルデータを探索する必要があるため、モデルデータ点数および計測データ点数が増加すると計算量が増大する。具体的には、モデルデータの点数をＭ、計測データの点数をＮとした場合、例えば全探索時の計算オーダは、Ｏ（Ｍ×Ｎ）となる。
（３）計測点が少ない場合を扱えない
ＩＣＰアルゴリズムは、密な距離データを対象としているため、離散した点群であり、空間的に疎である場合は、誤った結果に収束してしまう。
（１）誤差の蓄積
ＩＣＰアルゴリズムは、２つの距離データの重ね合わせ手段であり、先のデータとその後のデータとの比較を繰り返し、その差分を積分しても、対応点がほとんどないため、誤差が蓄積してしまう。
（２）計算量が多い
ＩＣＰアルゴリズムは、繰り返し計算であるため、計算量が膨大となる。すなわち、ＩＣＰアルゴリズムは、計測データの各データ点に対応するモデルデータを探索する必要があるため、モデルデータ点数および計測データ点数が増加すると計算量が増大する。具体的には、モデルデータの点数をＭ、計測データの点数をＮとした場合、例えば全探索時の計算オーダは、Ｏ（Ｍ×Ｎ）となる。
（３）計測点が少ない場合を扱えない
ＩＣＰアルゴリズムは、密な距離データを対象としているため、離散した点群であり、空間的に疎である場合は、誤った結果に収束してしまう。
そのため、このような距離センサを用いた位置同定には、以下の要件を満たす必要がある。
（１）メモリ効率の良い周囲環境のためのデータ構造
逐次得られる計測データを全て保存する方法では無尽蔵にメモリを必要とする。よって、周囲環境を計測した結果を効率良く格納するデータ構造が必要である。
（２）計測データの点数不足および誤差に対する安定性
計測データの点数が少ない場合、および、誤差を含む場合であっても、できるだけ位置同定精度が低下しない必要がある。
（３）位置同定のための計算の効率化
周囲環境を計測して獲得した環境情報と計測データの比較により、自己位置同定を行うが、この比較処理に多くの計算を必要とする。
（１）メモリ効率の良い周囲環境のためのデータ構造
逐次得られる計測データを全て保存する方法では無尽蔵にメモリを必要とする。よって、周囲環境を計測した結果を効率良く格納するデータ構造が必要である。
（２）計測データの点数不足および誤差に対する安定性
計測データの点数が少ない場合、および、誤差を含む場合であっても、できるだけ位置同定精度が低下しない必要がある。
（３）位置同定のための計算の効率化
周囲環境を計測して獲得した環境情報と計測データの比較により、自己位置同定を行うが、この比較処理に多くの計算を必要とする。
本発明は上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、メモリ効率の良い周囲環境のためのデータ構造を有し、計測データの点数不足および誤差に対する安定性があり、位置同定のための計算が効率的にでき、これにより、外界の三次元情報が空間的に離散した点群であり、計測毎に位置が相違して対応点がなく、比較的大きな誤差を含んでいる場合でも、誤差の蓄積がなく、少ない計算量で、正確な位置合わせができる自己位置同定方法と装置および三次元形状の計測方法と装置を提供することにある。
本発明によれば、外界の三次元形状を取り入れて自己位置を同定する自己位置同定方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップを実施する、ことを特徴とする自己位置同定方法が提供される。なお、更に前記自己位置を出力装置に出力する出力ステップを実施してもよい。
また、本発明によれば、外界の三次元形状の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置における前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力する出力ステップを有する、ことを特徴とする三次元形状の計測方法が提供される。
なお、上記自己位置同定方法または三次元形状の計測方法において、上記距離に関する評価値は、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップを実施する、ことを特徴とする自己位置同定方法が提供される。なお、更に前記自己位置を出力装置に出力する出力ステップを実施してもよい。
また、本発明によれば、外界の三次元形状の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置における前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力する出力ステップを有する、ことを特徴とする三次元形状の計測方法が提供される。
なお、上記自己位置同定方法または三次元形状の計測方法において、上記距離に関する評価値は、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
上記自己位置同定方法または三次元形状の計測方法は、以下の内容を含んでもよい。
本発明の好ましい実施形態によれば、前記自己位置同定ステップにおいて、先の計測位置における位置及び姿勢から新たな計測位置の６自由度位置を同定する。
また、前記マッチングステップにおいて、ボクセルの内部に代表点とその誤差分布に加え、ボクセル内に物体の存在確率を表す確率値を設定し、記憶する。
また、前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、位置合わせする粗合わせステップを有する。
なお、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する上記評価値は、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
また、代表点を有するボクセル間の距離に関する上記評価値は、当該距離の総和の代わりに、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
なお、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する上記評価値は、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
また、代表点を有するボクセル間の距離に関する上記評価値は、当該距離の総和の代わりに、当該距離の総和の代わりに、当該距離の平均値、当該距離の２乗の総和、または、当該距離の最大値であってもよく、他の適切な評価値であってもよい。
または、前記マッチングステップにおいて、ボクセルの内部に物体の存在確率を表す確率値を設定し、記憶する手段を有する場合、前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする粗合わせステップを有する。
なお、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する上記評価値は、当該確率値の総和の代わりに、当該確率値の平均値、または、当該確率値の２乗の総和、または、当該確率値の最小値であってもよく、他の適切な評価値であってもよい。
また、近接するボクセルが持つ確率値の差に関する上記評価値は、当該確率値の差の総和の代わりに、当該確率値の差の平均値、当該確率値の差の２乗の総和、または、当該確率値の差の最大値であってもよく、他の適切な評価値であってもよい。
なお、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する上記評価値は、当該確率値の総和の代わりに、当該確率値の平均値、または、当該確率値の２乗の総和、または、当該確率値の最小値であってもよく、他の適切な評価値であってもよい。
また、近接するボクセルが持つ確率値の差に関する上記評価値は、当該確率値の差の総和の代わりに、当該確率値の差の平均値、当該確率値の差の２乗の総和、または、当該確率値の差の最大値であってもよく、他の適切な評価値であってもよい。
また、前記データ入力ステップの後に、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度値を利用し、照合する範囲を限定する探索範囲限定ステップを有する。
また、前記精密合わせステップにおいて、誤差分布が交差する場合を同一計測点とし、その場合の距離値に分布の一致度から求めた重みを掛け合わせて誤差分布間の距離を算出する。
また、前記モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
また、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。
また、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。
また、自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。ボクセルの分割には、例えば八分木やＫ−Ｄ木を用いる。
また、自己位置の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正し、自己位置と誤差分布を出力する。
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。ボクセルの分割には、例えば八分木やＫ−Ｄ木を用いる。
また、自己位置の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正し、自己位置と誤差分布を出力する。
前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
また、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。
本発明の別の実施形態によると、前記精密合わせステップにおいて、前記近接する誤差分布間の距離に基づく評価値が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値（例えば、当該一致度の総乗）が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする。
前記一致度に関する評価値の算出式が次の［数１］で表わされ、
この式において、計測点ｊと環境モデル上の代表点ｉとが対応付けられているとし、当該計測点ｊなる計測データが得られる確率をＥＭ（ｉ、ｊ）としており、ω（ｊ）は、環境モデルの中に計測点ｊと対応付けられる代表点が存在する場合は１、それ以外の場合は０としている。
前記出力ステップにおいて、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する。
前記出力ステップにおいて、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない。
前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力ステップにおいて、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。
前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。
また、本発明によれば、外界の三次元形状を取り入れて自己位置を同定する自己位置同定装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
自己位置を出力装置に出力するデータ伝達装置とを備え、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定する、ことを特徴とする自己位置同定装置が提供される。
また、本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置が提供される。
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
自己位置を出力装置に出力するデータ伝達装置とを備え、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定する、ことを特徴とする自己位置同定装置が提供される。
また、本発明によれば、三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置が提供される。
前記自己位置同定装置または三次元形状の計測装置は、以下の内容を含んでもよい。
前記マッチング装置は、ボクセルの内部に代表点とその誤差分布に加え、ボクセル内に物体の存在確率を表す確率値を設定し、記憶する。
前記自己位置同定装置または三次元形状の計測装置は、前記位置合わせ（精密合わせステップ）の前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、位置合わせをする粗合わせステップを行う。
前記自己位置同定装置または三次元形状の計測装置は、前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする粗合わせステップを行う。
前記自己位置同定装置または三次元形状の計測装置は、前記データ入力ステップの後に、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度値を利用し、照合する範囲を限定する。
前記自己位置同定装置または三次元形状の計測装置は、先の計測位置における位置及び姿勢から新たな計測位置の６自由度位置を同定する。
前記自己位置同定装置または三次元形状の計測装置は、誤差分布が交差する場合を同一計測点とし、その場合の距離値に分布の一致度から求めた重みを掛け合わせて誤差分布間の距離を算出する。
前記モデル構築装置は、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
前記自己位置同定装置または三次元形状の計測装置は、前記自己位置の同定（自己位置同定ステップ）の後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。
該モデル更新装置は、
新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する。
前記自己位置同定装置または三次元形状の計測装置は、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。
該モデル更新装置は、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する。
前記自己位置同定装置または三次元形状の計測装置は、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
前記自己位置同定装置または三次元形状の計測装置は、前記自己位置同定ステップにおいて、自己位置の同定とともに、自己位置の誤差分布を同定し、前記出力ステップの前に、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正し、前記データ伝達装置は、自己位置と誤差分布を出力装置に出力する。
前記モデル更新装置は、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する。
前記自己位置同定装置または三次元形状の計測装置は、前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新装置を有し、
該モデル更新装置は、新たに入力された被計測点の座標値と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。
該モデル更新装置は、新たに入力された被計測点の座標値と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。
前記自己位置同定装置または三次元形状の計測装置は、前記精密合わせステップにおいて、前記近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値（例えば、当該一致度の総乗）が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする。この場合、前記一致度に関する評価値の算出式が上記［数１］の式で表わされる。
前記データ伝達装置は、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する。
前記データ伝達装置は、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない。
前記データ入力装置は、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得する。
前記マッチング装置の後に、前記環境モデルを更新するモデル更新装置を有し、該モデル更新装置において、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力装置は、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。
前記データ伝達装置は、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する。
発明の効果
上記本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセルに分割し、各ボクセル位置を記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
従って、本発明のデータ構造により、複数視点の計測データを固定サイズに統合できる。
上記本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセルに分割し、各ボクセル位置を記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
従って、本発明のデータ構造により、複数視点の計測データを固定サイズに統合できる。
また、ボクセルの内部に物体の存在確率を表す確率値を設定し、保存することによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで容易に判断できるので、検索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
また、粗合わせステップにおいて、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
また、先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする場合には、物体が存在しない情報も加味して位置合わせするので、精度の向上が図れる。
次いで、精密合わせステップにおいて、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせするので、誤差分布同士の精密な位置合わせを短時間にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
また、本発明のデータ構造は、ボクセル構造を拡張した構造をしているため、データサイズは点群に比べて小さく済ませることができる。すなわち、本発明で提案するデータ構造はボクセル内に１つの代表点を保存するため、計測点に対応するモデル点を探索する計算オーダを１とすることができるため、全体での計算オーダをＯ（Ｎ）に減少することができる。
従って、周囲環境と計測データの位置合わせ（ＩＣＰアルゴリズム）を行う際に探索対象となるデータ量が減少することから、計算を効率化できる。
従って、周囲環境と計測データの位置合わせ（ＩＣＰアルゴリズム）を行う際に探索対象となるデータ量が減少することから、計算を効率化できる。
さらに、従来のＩＣＰアルゴリズムは疎データに対して誤った結果を出力するが、本発明の環境モデルは、ボクセル内に代表点と誤差分布を持っているため、疎データに対応した位置合わせが可能である。
また、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正することによって、より自己位置の精度を向上させることができる。
従って、本発明の方法と装置によれば、誤差を含むデータを正確な情報に補正する機能を有すると共に、長時間の計測に対して高精度に収束する。なおかつ、位置同定処理は、ボクセルの各ボクセルに対応する代表点とその誤差分布を計測点で更新する処理であるため計算量が小さく、演算は周囲ボクセルへの影響を与えずボクセル内で閉じている。よって、高速な処理が可能である。また、計測データはボクセルに逐次統合可能であり、その結果得られた周囲環境情報のメモリサイズは固定サイズを上回ることはない。
さらに、前記モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定するので、より真値に近い形状を得ることができる。
特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含むデータであってもカルマンフィルタの効果により真値に収束した高精度な形状が得られる。
特に、カルマンフィルタを用いたモデル更新ステップを繰り返すことで、誤差を含むデータであってもカルマンフィルタの効果により真値に収束した高精度な形状が得られる。
また、前記精密合わせステップにおいて、前記近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値（例えば、当該一致度の総乗）が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせすることで、環境モデル、計測データの双方の誤差を考慮して位置合わせすることができる。
また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する際に、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさ出力装置に出力するため、計測装置を使用する際に、信頼性の低い計測値をアプリケーションの内容に応じて使用者が取捨選択することができるようになる。
また、前記出力ステップにおいて、前記ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しないようにすることによって、計測装置を使用する際に、そもそも信頼性の高い計測値のみを扱うことができるようになるため、扱うデータ量を削減することや、信頼性の向上につながる。
また、前記データ入力ステップにおいて、距離センサを用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得し、特に異なる方向から距離データを得ることによって、分布形状が異なる誤差分布の距離データを統合することが可能となり、精度を向上することができる。
また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合のみ、前記出力ステップにおいて、当該ボクセルの代表点の位置、誤差分布およびボクセルの位置の少なくともいずれかを三次元形状の計測値として出力装置に出力することによって、距離センサから新たに得られた被計測点によって影響を受けたボクセル内の代表点等の値が出力される。このため、使用者は従来と同様の運用を想定しながら、距離センサで得られる元々の計測値があたかもより精度の高い計測値に置き換わったように、利用することができるようになる。このように、より精度の高い三次元形状計測が可能になる。
また、前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力することによって、従来の距離センサの計測値の分解能が粗い場合でも、あたかも精度良くかつ分解能が高い距離センサのように利用することが可能となる。このように、より精度の高い三次元形状計測が可能になる。
本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から明らかになろう。
本発明のその他の目的及び有利な特徴は、添付図面を参照した以下の説明から明らかになろう。
以下本発明の好ましい実施形態について、図面を参照して説明する。なお、各図において、共通する部分には同一の符号を付し、重複した説明を省略する。
図２は、本発明の自己位置同定装置の全体構成図である。この図において、本発明の自己位置同定装置は、６自由度同定手段であり、距離センサ、オドメータ、カメラ、ＧＰＳ、姿勢センサから外界の三次元情報を取り入れて、自己位置を同定する装置である。なお、本発明において、自己位置とは、自己位置同定装置自体の外界における６自由度の位置と姿勢を意味する。
なお、本発明において、距離センサ以外のオドメータ、カメラ、ＧＰＳ、姿勢センサは不可欠ではなく、必要に応じてオプションとして用いる。以下、距離センサを用いた例を説明する。
なお、本発明において、距離センサ以外のオドメータ、カメラ、ＧＰＳ、姿勢センサは不可欠ではなく、必要に応じてオプションとして用いる。以下、距離センサを用いた例を説明する。
図３は、距離センサの一例としての三次元レーザレーダの構成図である。三次元レーザレーダは、例えば非特許文献３に開示されている。
この図に示すように、三次元レーザレーダ１０は、レーダヘッド１２と制御器２０から構成される。レーザダイオード１３から発振されたパルスレーザ光１は、投光レンズ１４で平行光２に整形され、ミラー１８ａ，１８ｂと回転・揺動するポリゴンミラー１５で二次元方向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ光３は、ポリゴンミラー１５を介して受光レンズ１６で集光され、光検出器１７で電気信号に変換される。
この図に示すように、三次元レーザレーダ１０は、レーダヘッド１２と制御器２０から構成される。レーザダイオード１３から発振されたパルスレーザ光１は、投光レンズ１４で平行光２に整形され、ミラー１８ａ，１８ｂと回転・揺動するポリゴンミラー１５で二次元方向に走査され、測定対象物に照射される。測定対象物から反射されたパルスレーザ光３は、ポリゴンミラー１５を介して受光レンズ１６で集光され、光検出器１７で電気信号に変換される。
制御器２０内の時間間隔カウンタ２１は、レーザダイオード１３のパルス発振タイミングと同期したスタートパルス４と、光検出器１７から出力されたストップパルス５の時間間隔を計測する。信号処理ボード２２は、反射光が検出された時点の時間間隔ｔ、ポリゴンミラーの回転角度θ、揺動角度φを極座標データ（ｒ，θ，φ）として出力する。
ｒは計測位置（レーダヘッド設置位置）を原点とする距離であり、ｒ＝ｃ×ｔ／２ の式で求められる。ここでｃは光速である。
判定処理ユニット２３は、信号処理ボードからの極座標データを、レーダヘッド設置位置を原点とした三次元空間データ（ｘ，ｙ，ｚ）へ変換して、検出処理を行うようになっている。なおこの図で２４はドライブユニットである。
ｒは計測位置（レーダヘッド設置位置）を原点とする距離であり、ｒ＝ｃ×ｔ／２ の式で求められる。ここでｃは光速である。
判定処理ユニット２３は、信号処理ボードからの極座標データを、レーダヘッド設置位置を原点とした三次元空間データ（ｘ，ｙ，ｚ）へ変換して、検出処理を行うようになっている。なおこの図で２４はドライブユニットである。
上述した三次元レーザレーダ１０の計測範囲は、例えば、水平画角６０°、垂直画角３０°、最大測定距離５０ｍである。また、位置検出精度は、例えば約２０ｃｍである。
また、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示する場合、１フレームの計測点数を、横方向１６６点、スキャン方向５０点とすると、１フレームに１６６×５０＝８３００点が表示される。この場合にフレームレートは、例えば約２フレーム／秒である。
また、計測データを各画素に対して奥行き方向の距離値を持った距離画像で表示する場合、１フレームの計測点数を、横方向１６６点、スキャン方向５０点とすると、１フレームに１６６×５０＝８３００点が表示される。この場合にフレームレートは、例えば約２フレーム／秒である。
この三次元レーザレーダ１０で計測される三次元形状上の被計測点は、横方向にΔθ×ｒ、垂直方向にΔφ×ｒ、互いに離散した点群となる。例えば、Δθ＝６０／１６６×π／１８０＝６．３×１０−３ラジアン、Δφ＝３０／５０×π／１８０＝１０．５×１０−３ラジアン、ｒ＝５０ｍの場合、最も近接する場合でも、被計測点の間隔は、横方向で約３１５ｍｍ、垂直方向で約５２５ｍｍとなる。
本発明では、距離センサとして、例えば、上述した三次元レーザレーダ１０を用いる。しかし、距離センサはこれに限定されず、視差を利用した距離センサ、その他の周知の距離センサを用いることができる。
図４Ａ，図４Ｂは、距離センサで計測された極座標データと誤差の関係を示す図である。
図４Ａに示すように、任意の計測位置を原点とする極座標値（ｒ，θ，φ）を計測結果として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存在する。
この誤差分布は、誤差分布のｒｓ，θｓ，φｓでの存在確率をＰ（ｒｓ，θｓ，φｓ）とした場合、誤差分布は計測の軸ｒ，θ，φ方向に正規分布しているとし、例えば式（１）で表すことができる。ここで、ｒ，θ，φはセンサからの計測値、σｒ，σθ，σφは は標準偏差、Ａは規格化定数である。
図４Ｂに示すように、誤差分布は、通常ｒ方向に長い切頭円錐形（左図）に内包される分布であるが、遠方においてａとｂの差は小さい。従って、この誤差分布を直方体に包含される楕円体として安全サイドに近似することができる。
図４Ａに示すように、任意の計測位置を原点とする極座標値（ｒ，θ，φ）を計測結果として計測する。距離センサによる計測結果には、図に示すような誤差分布が通常存在する。
この誤差分布は、誤差分布のｒｓ，θｓ，φｓでの存在確率をＰ（ｒｓ，θｓ，φｓ）とした場合、誤差分布は計測の軸ｒ，θ，φ方向に正規分布しているとし、例えば式（１）で表すことができる。ここで、ｒ，θ，φはセンサからの計測値、σｒ，σθ，σφは は標準偏差、Ａは規格化定数である。
図４Ｂに示すように、誤差分布は、通常ｒ方向に長い切頭円錐形（左図）に内包される分布であるが、遠方においてａとｂの差は小さい。従って、この誤差分布を直方体に包含される楕円体として安全サイドに近似することができる。
図５は、本発明の方法を実行するための装置構成図である。この図に示すように、この装置は、データ入力装置３２、外部記憶装置３３、内部記憶装置３４、中央処理装置３５および出力装置３６を備える。
データ入力装置３２は、上述した距離センサを有し、三次元形状上の座標値を記憶装置に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。なお、データ入力装置３２は、キーボード等の通常の入力手段も有するのがよい。
外部記憶装置３３は、ハードディスク、フロッピー（登録商標）ディスク、磁気テープ、コンパクトディスク等である。外部記憶装置３３は、環境モデルのサイズが大きく後述する内部記憶装置３４に入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の全体を保持できない場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。
内部記憶装置３４は、例えばＲＡＭ，ＲＯＭ等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。
中央処理装置３５（ＣＰＵ）は、モデル構築装置、マッチング装置、粗合わせと精密合わせの位置合わせ装置、モデル更新装置、データ伝達装置として機能し、演算や入出力等を集中的に処理し、内部記憶装置３４と共に、プログラムを実行する。モデル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後述のマッチングステップを行う装置であり、位置合わせ装置は、後述の粗合わステップと精密合わせステップを行う装置であり、モデル更新装置は、後述のモデル更新ステップを行う装置であり、データ伝達装置は、出力装置３６へデータを出力する装置である。
出力装置３６は、例えば表示装置、プリンタ、外部装置等であり、内部記憶装置３４および外部記憶装置３３の少なくともいずれかに記憶したデータ及びプログラムの実行結果を出力するようになっている。外部装置とのインターフェイスは、ＬＡＮ、ＵＳＢ、ＩＥＥＥ１３９４等であり、入力された三次元形状上の座標値に対して該当するボクセル内の代表点、誤差分布、ボクセル位置などを付加した結果や、環境モデル全体または環境モデル一部を要求に応じて出力する。
外部記憶装置３３は、ハードディスク、フロッピー（登録商標）ディスク、磁気テープ、コンパクトディスク等である。外部記憶装置３３は、環境モデルのサイズが大きく後述する内部記憶装置３４に入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の全体を保持できない場合には、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を記憶し、かつ本発明の方法を実行するためのプログラムを記憶する。
内部記憶装置３４は、例えばＲＡＭ，ＲＯＭ等であり、環境モデルの一部範囲または全体範囲に対する、入力された三次元形状上の座標値、ボクセル位置、及び代表点とその誤差分布の一部または全体を保管し、かつ演算情報を保管する。
中央処理装置３５（ＣＰＵ）は、モデル構築装置、マッチング装置、粗合わせと精密合わせの位置合わせ装置、モデル更新装置、データ伝達装置として機能し、演算や入出力等を集中的に処理し、内部記憶装置３４と共に、プログラムを実行する。モデル構築装置は、後述のモデル構築ステップを行う装置であり、マッチング装置は、後述のマッチングステップを行う装置であり、位置合わせ装置は、後述の粗合わステップと精密合わせステップを行う装置であり、モデル更新装置は、後述のモデル更新ステップを行う装置であり、データ伝達装置は、出力装置３６へデータを出力する装置である。
出力装置３６は、例えば表示装置、プリンタ、外部装置等であり、内部記憶装置３４および外部記憶装置３３の少なくともいずれかに記憶したデータ及びプログラムの実行結果を出力するようになっている。外部装置とのインターフェイスは、ＬＡＮ、ＵＳＢ、ＩＥＥＥ１３９４等であり、入力された三次元形状上の座標値に対して該当するボクセル内の代表点、誤差分布、ボクセル位置などを付加した結果や、環境モデル全体または環境モデル一部を要求に応じて出力する。
上述した本発明の装置は、上述した距離センサと通常のＰＣ（コンピュータ）を組み合わせたものでもよく、或いは、全体を一体にした装置であってもよい。また、自走可能な装置内に一体的に組み込んでもよい。
図６は、本発明の方法を示すフローチャートである。
本発明の方法は、外界の三次元形状を取り入れて自己位置を同定する自己位置同定方法、および三次元形状の計測方法であり、データ入力ステップＳ１、データ補正ステップＳ２、探索範囲限定ステップＳ３、モデル構築ステップＳ４、マッチングステップＳ５、自己位置同定ステップＳ７、Ｓ１０、粗合わせステップＳ８、精密合わせステップＳ９、モデル更新ステップＳ１１及び出力ステップＳ１２を有する。
なお、これら一連の処理のうち、Ｓ１、Ｓ２、Ｓ３，Ｓ５〜Ｓ１２は、計測データが得られる毎に実施し、Ｓ４は初めて計測データが得られたときにだけ実施する。
本発明の方法は、外界の三次元形状を取り入れて自己位置を同定する自己位置同定方法、および三次元形状の計測方法であり、データ入力ステップＳ１、データ補正ステップＳ２、探索範囲限定ステップＳ３、モデル構築ステップＳ４、マッチングステップＳ５、自己位置同定ステップＳ７、Ｓ１０、粗合わせステップＳ８、精密合わせステップＳ９、モデル更新ステップＳ１１及び出力ステップＳ１２を有する。
なお、これら一連の処理のうち、Ｓ１、Ｓ２、Ｓ３，Ｓ５〜Ｓ１２は、計測データが得られる毎に実施し、Ｓ４は初めて計測データが得られたときにだけ実施する。
データ入力ステップＳ１では、新たな計測位置において、距離センサを用いて、三次元形状上の座標値をコンピュータの記憶装置に入力する。また、例えばゴニオメータ、オドメータ等を併用して、距離センサの位置・姿勢や移動距離も入力するのがよい。
なおこのデータ入力ステップＳ１において、三次元レーザレーダ１０を用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得するのがよい。
なおこのデータ入力ステップＳ１において、三次元レーザレーダ１０を用いて、三次元形状上の座標値を任意の計測位置を原点とする距離データとして原点を移動しながら順次取得するのがよい。
距離センサとして三次元レーザレーダ１０を用いた場合、三次元形状上の座標値は、任意の計測位置を原点とする距離データであり、極座標値（ｒ，θ，φ）で表される。また、各座標値の誤差分布は、極座標値（ｒ，θ，φ）から演算で求めるか、予め別の入力手段（例えばキーボード）で入力する。
データ補正ステップＳ２では、距離データの補正処理を行い、距離データの精度を向上させる。また、極座標データとオドメータのデータから、任意の固定位置を原点とした三次元空間データ（ｘ，ｙ，ｚ）へ変換してもよい。
距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、周囲の点から孤立して存在する点であり、計測データは複数の近接する点で構成されることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計測データが含む誤差分布を考慮して、複数回の計測を統計処理（例えば平均値等）することで、距離の補正を行う。
さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを行うのがよい。
距離データの補正処理では、孤立点の除去、統計的処理、等を行う。孤立点は、周囲の点から孤立して存在する点であり、計測データは複数の近接する点で構成されることから、孤立点は誤計測と仮定して除去することができる。統計的処理は、計測データが含む誤差分布を考慮して、複数回の計測を統計処理（例えば平均値等）することで、距離の補正を行う。
さらに、対象とする三次元形状が、直線近似又は平面近似できる場合にはこれらを行うのがよい。
探索範囲限定ステップＳ３では、距離センサの探索範囲を限定する。
探索範囲を限定せずに環境モデルに対する計測データのマッチング処理を行うと、複数の解（被計測点）が得られる可能性がある。そこで、（１）現在のセンサ位置を過去のセンサ位置の変化から推定し、センサ位置推定結果の近傍を探索する、（２）オドメータを用いてセンサ位置を推定し、探索範囲を限定する、（３）距離データのうち、距離値だけでなく、反射強度値も利用して探索結果を絞り込む、等を実施する。
探索範囲を限定せずに環境モデルに対する計測データのマッチング処理を行うと、複数の解（被計測点）が得られる可能性がある。そこで、（１）現在のセンサ位置を過去のセンサ位置の変化から推定し、センサ位置推定結果の近傍を探索する、（２）オドメータを用いてセンサ位置を推定し、探索範囲を限定する、（３）距離データのうち、距離値だけでなく、反射強度値も利用して探索結果を絞り込む、等を実施する。
図７は、ボクセルの分割に八分木を用いた場合のモデル構築ステップの模式図である。
モデル構築ステップＳ４では、この図に示すように、三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセル６に分割し、各ボクセル位置を記憶する環境モデルを構築する。
ボクセル６の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体でもよい。
また、ボクセル６の各辺の長さは、最大のボクセル６を必要最小限の分解能に相当する大きさに設定するのがよい。以下、最大のボクセル６をレベル１のボクセルと呼ぶ。
また、単一のボクセル内に複数の被計測点が存在する場合には、単一のボクセル内に単一の被計測点のみが存在するように、例えば八分木を選んだ場合は、ボクセルを更に八分割して階層的に複数のボクセルに分割する。以下、最大のボクセル６の八分割を１回実施した空間領域をレベル２のボクセル、ｋ回実施した空間領域をレベルｋ＋１のボクセルと呼ぶ。
モデル構築ステップＳ４では、この図に示すように、三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセル６に分割し、各ボクセル位置を記憶する環境モデルを構築する。
ボクセル６の形状は、各辺の長さが等しい立方体でも、各辺の長さが異なる直方体でもよい。
また、ボクセル６の各辺の長さは、最大のボクセル６を必要最小限の分解能に相当する大きさに設定するのがよい。以下、最大のボクセル６をレベル１のボクセルと呼ぶ。
また、単一のボクセル内に複数の被計測点が存在する場合には、単一のボクセル内に単一の被計測点のみが存在するように、例えば八分木を選んだ場合は、ボクセルを更に八分割して階層的に複数のボクセルに分割する。以下、最大のボクセル６の八分割を１回実施した空間領域をレベル２のボクセル、ｋ回実施した空間領域をレベルｋ＋１のボクセルと呼ぶ。
図８は、構築された環境モデルの模式図である。
マッチングステップＳ５では、この図に示すように、三次元形状上の座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶する。末端のボクセルは計測値の代表点を１つだけ持つことができる。各ボクセルが計測値の代表点とその誤差分布を持つことで、物体の形状を表す。また、物体の存在確率を表す確率値をボクセルに持たせることもできる。
マッチングステップＳ５では、この図に示すように、三次元形状上の座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶する。末端のボクセルは計測値の代表点を１つだけ持つことができる。各ボクセルが計測値の代表点とその誤差分布を持つことで、物体の形状を表す。また、物体の存在確率を表す確率値をボクセルに持たせることもできる。
マッチングステップＳ５において、代表点の絶対位置は次の［数３］の式（２）で与えられる。ここで、（ｘ，ｙ，ｚ）は代表点のボクセルでの相対座標、Ｓｘ，Ｓｙ，Ｓｚはレベル１でのボクセルの一辺の大きさ、ｎｘ（ｋ），ｎｙ（ｋ），ｎｚ（ｋ）はレベルｋでのボクセルの番地、Ｌは求める代表点が存在するレベルである。
図９Ａ，図９Ｂは、本発明におけるボクセルデータのデータ構造を示す図である。
この図において、図９Ａは、各ボクセルデータのメモリレイアウト例である。この図において、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。
図９Ｂは、レベル２（１，１，０）のボクセルが代表点を持つ場合の例を示している。なおこの図において、ｎｕｌｌは空集合を表す。
この図において、図９Ａは、各ボクセルデータのメモリレイアウト例である。この図において、矢印はデータへのリンクを表し、値としてはデータへのポインタを保持する。
図９Ｂは、レベル２（１，１，０）のボクセルが代表点を持つ場合の例を示している。なおこの図において、ｎｕｌｌは空集合を表す。
上述したデータ構造の環境モデルは、以下の特徴を有する。
（１）内容：空間を小直方体で分割して各ボクセルに計測点の代表点と誤差分布を保持する。
（２）精度：ボクセル毎に持つ計測点の代表値相当である。
（３）存在：物体の存在の有無を表現できる。
（４）データ量：ボクセルの個数に比例してメモリを必要とするが、サイズ固定である。
（５）点群からの変換：適しており、計算量は少ない。
（６）アクセス速度：シンプルな構造をしているため、要素へのアクセスが高速である。
（１）内容：空間を小直方体で分割して各ボクセルに計測点の代表点と誤差分布を保持する。
（２）精度：ボクセル毎に持つ計測点の代表値相当である。
（３）存在：物体の存在の有無を表現できる。
（４）データ量：ボクセルの個数に比例してメモリを必要とするが、サイズ固定である。
（５）点群からの変換：適しており、計算量は少ない。
（６）アクセス速度：シンプルな構造をしているため、要素へのアクセスが高速である。
またこの特徴から、上述した環境モデルは、以下の効果Ａ〜Ｃをすべて満たしている。
効果Ａ：誤差を考慮した表現が可能である。
効果Ｂ：必要なメモリ量と計算量が一定量以下である。
効果Ｃ：物体の存在だけでなく、物体が存在しないことを表せる。
効果Ａ：誤差を考慮した表現が可能である。
効果Ｂ：必要なメモリ量と計算量が一定量以下である。
効果Ｃ：物体の存在だけでなく、物体が存在しないことを表せる。
図６において、マッチングステップＳ５の後に、ステップＳ６で先の計測位置おける同一の三次元形状のデータの有無をチェックする。このチェックで、先の計測位置おける同一の三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定する。
この同定は、逐次移動する移動体の初期位置において、既知の６自由度位置（例えばグローバル座標系の原点）で実施するのが好ましい。また、この同定において、新たな計測位置の位置（３自由度）と姿勢（３自由度）を含む６自由度位置を同定するのがよい。
この同定は、逐次移動する移動体の初期位置において、既知の６自由度位置（例えばグローバル座標系の原点）で実施するのが好ましい。また、この同定において、新たな計測位置の位置（３自由度）と姿勢（３自由度）を含む６自由度位置を同定するのがよい。
また、ステップＳ６のチェックで、先の計測位置おける同一の三次元形状のデータが存在する場合には、図６において、粗合わせステップＳ８と精密合わせステップＳ９を、マッチングステップＳ５の後に実施する。
図１０は、粗合わせステップＳ８と精密合わせステップＳ９のデータ処理フロー図であり、図１１は粗合わせステップＳ８の模式図、図１２は精密合わせステップＳ９の模式図である。
図１０は、粗合わせステップＳ８と精密合わせステップＳ９のデータ処理フロー図であり、図１１は粗合わせステップＳ８の模式図、図１２は精密合わせステップＳ９の模式図である。
図１０において、粗合わせステップＳ８では、図１１に示すように、先の計測位置に対する環境モデルに対し、
新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、
もしくは、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする。
新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、
もしくは、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする。
粗合わせステップＳ６における位置合わせは、環境モデルと計測データをボクセル空間上で表現すること、もしくは環境モデルはボクセル空間上で、計測データは代表点と誤差分布表現することで実施する。現在の計測データが位置（ｘ，ｙ，ｚ）、姿勢（θ，φ，ψ）での計測であったとして、計測データをワールド座標に変換して環境モデルとの一致度を算出する。
一致度の算出には、例えば最短距離法を用いることができる。最短距離法を用いた場合のボクセル間の距離は、２つのボクセル空間をｘ（１）、ｘ（２）、ボクセルの総数Ｉを、ボクセルの値ｘｉ （ｎ）とすると、［数４］の式（３）で定義できる。
計測データの最適な位置・姿勢は、位置（ｘ，ｙ，ｚ）、姿勢（θ，φ，ψ）を変化させることによってεを最小にする最小自乗法によって算出できる。
一致度の算出には、例えば最短距離法を用いることができる。最短距離法を用いた場合のボクセル間の距離は、２つのボクセル空間をｘ（１）、ｘ（２）、ボクセルの総数Ｉを、ボクセルの値ｘｉ （ｎ）とすると、［数４］の式（３）で定義できる。
計測データの最適な位置・姿勢は、位置（ｘ，ｙ，ｚ）、姿勢（θ，φ，ψ）を変化させることによってεを最小にする最小自乗法によって算出できる。
また、一致度として、例えば環境モデルと計測データの両ボクセルにおいて、近接する両ボクセルの持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）を用いることができる。この場合は一致度を最小にするように、計測データの最適な位置・姿勢を変化させる。
また、環境モデルはボクセル空間上で、計測データは代表値と誤差分布表現した場合には、計測データの代表値、および誤差分布が近接する環境モデルのボクセルの確率値に関する評価値（例えば、当該確率値の総和）を用いることができる。この場合は一致度を最大にするように、計測データの最適な位置・姿勢を変化させる。
また、環境モデルはボクセル空間上で、計測データは代表値と誤差分布表現した場合には、計測データの代表値、および誤差分布が近接する環境モデルのボクセルの確率値に関する評価値（例えば、当該確率値の総和）を用いることができる。この場合は一致度を最大にするように、計測データの最適な位置・姿勢を変化させる。
図１０において、精密合わせステップＳ９では、図１２に示すように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせする。
精密合わせステップＳ７における環境モデルと計測データの精密合わせ位置合わせには、点群と点群の位置合わせが可能なＩＣＰアルゴリズムに誤差分布を考慮した手法を利用する。位置合わせの初期値には、粗い位置合わせにより得られた位置・姿勢を利用する。
ＩＣＰアルゴリズムに利用する誤差分布間の距離の算出には、例えば誤差分布が交差する場合を同一計測点と考え、その場合の距離値に分布の一致度から求めた重みを掛け合わせて算出する。分布の一致には例えばマハラノビス距離のような距離尺度を利用できる。
この場合の環境モデルと計測データの距離は、環境モデルデータをｐＭｉ、環境モデルデータの誤差分布をΣＭｉ、計測データをＰＤｉ、計測データの誤差分布をΣＤｉ、誤差分布の合成関数をｗ、計測データに対応する環境モデルデータの個数をＮとすると、［数５］の式（４）で定義できる。ここで、Ｔは転置を表す。
計測データの最適な位置・姿勢は、計測データを計測した位置（ｘ，ｙ，ｚ） 、姿勢（θ，φ，ψ）を変化させてＰＤｉを移動することによりεを最小にする最小自乗法によって算出できる。
また、自己位置・姿勢の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正する。
精密合わせステップＳ７における環境モデルと計測データの精密合わせ位置合わせには、点群と点群の位置合わせが可能なＩＣＰアルゴリズムに誤差分布を考慮した手法を利用する。位置合わせの初期値には、粗い位置合わせにより得られた位置・姿勢を利用する。
ＩＣＰアルゴリズムに利用する誤差分布間の距離の算出には、例えば誤差分布が交差する場合を同一計測点と考え、その場合の距離値に分布の一致度から求めた重みを掛け合わせて算出する。分布の一致には例えばマハラノビス距離のような距離尺度を利用できる。
この場合の環境モデルと計測データの距離は、環境モデルデータをｐＭｉ、環境モデルデータの誤差分布をΣＭｉ、計測データをＰＤｉ、計測データの誤差分布をΣＤｉ、誤差分布の合成関数をｗ、計測データに対応する環境モデルデータの個数をＮとすると、［数５］の式（４）で定義できる。ここで、Ｔは転置を表す。
計測データの最適な位置・姿勢は、計測データを計測した位置（ｘ，ｙ，ｚ） 、姿勢（θ，φ，ψ）を変化させてＰＤｉを移動することによりεを最小にする最小自乗法によって算出できる。
また、自己位置・姿勢の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正する。
さらに図６において、モデル更新ステップＳ１１は、自己位置同定ステップＳ１０の後に実施し、モデル構築ステップＳ４で構築した環境モデルを更新する。
図１３は、モデル更新ステップＳ１１におけるデータ処理フロー図である。この図に示すように、ステップＳＴ１で新たに入力された被計測点の座標値に対応するボクセルを探索し、ステップＳＴ２で該当するボクセル内に代表点がない（ボクセルが空である）場合には、ステップＳＴ３で新たに入力された被計測点の座標値と誤差分布を代表点の座標値と誤差分布として設定（新規に登録）する。
また、このステップＳＴ３において、新しい計測位置（原点）と被計測点の間には、原理的に物体が存在しないはずである。従って新しい計測位置（原点）と被計測点の間に位置するボクセル内の代表点と誤差分布を再設定、もしくは消去する。
また、このステップＳＴ３において、新しい計測位置（原点）と被計測点の間には、原理的に物体が存在しないはずである。従って新しい計測位置（原点）と被計測点の間に位置するボクセル内の代表点と誤差分布を再設定、もしくは消去する。
図１４は、該当するボクセル内に既に設定した代表点がある場合の模式図である。
図１３のステップＳＴ２で該当するボクセル内に既に設定した代表点がある場合には、ステップＳＴ４で新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較する（すなわち異なる点か同一点かを判断する）。
この比較で、誤差分布が互いに重複する場合（図１４の（Ａ））には、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定する（すなわち誤差分布を合成する）。
またこの比較で、誤差分布が互いに重複しない場合（図１４の（Ｂ））には、ステップＳＴ６、ＳＴ７で単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割し新規に登録する。
分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度には例えば、マハラノビス距離のような距離尺度を利用できる。また、２つの誤差分布に基づき、両者が同一点を表しているかを統計的検定によって判定してもよい。
図１３のステップＳＴ２で該当するボクセル内に既に設定した代表点がある場合には、ステップＳＴ４で新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較する（すなわち異なる点か同一点かを判断する）。
この比較で、誤差分布が互いに重複する場合（図１４の（Ａ））には、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定する（すなわち誤差分布を合成する）。
またこの比較で、誤差分布が互いに重複しない場合（図１４の（Ｂ））には、ステップＳＴ６、ＳＴ７で単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割し新規に登録する。
分割と合成の基準は、例えば誤差分布の一致度から判断する。誤差分布の一致度には例えば、マハラノビス距離のような距離尺度を利用できる。また、２つの誤差分布に基づき、両者が同一点を表しているかを統計的検定によって判定してもよい。
ステップＳＴ５で両誤差分布から新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動したとき（即ち、ステップＳＴ８で、Ｙｅｓ）、ステップＳＴ２へ戻り、上述の処理を繰り返す。
なお、図１５は、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動する場合を示している。
なお、図１５は、ステップＳＴ５で両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな誤差分布の中心を再設定した結果、新たな代表点が他のボクセル内へ移動する場合を示している。
ボクセル内に物体の存在確率を表す確率値を設定する場合には、モデル更新ステップＳ１１において、ボクセル内の代表点と誤差分布の新規登録、もしくは再設定、もしくは消去、もしくは分割後に新規登録の処理に応じて、ボクセル内の確率値も統計処理によって、新規登録、もしくは再設定、もしくは消去、もしくは分割後に新規登録を行う。
図１６は、誤差分布が互いに重複する場合（図１４の（Ａ））の別の模式図である。ステップＳＴ５において、２つの代表点と誤差分布を合成して新たな代表点を誤差分布を設定する手段として、カルマンフィルタを用いることができる。例えば、二次元の場合に、この図に示すように、２つの代表点をそれぞれｘ（１），ｘ’（２）、２つの誤差分布をΣ（１）、Σ’（２）とし、これを合成した代表点をｘ（２）、誤差分布をΣ（２）とすると、代表点ｘ（２）と誤差分布Σ（２）を算出する模式図は図１６のようになる。
図６において、出力ステップＳ１２では、前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置３６に出力する。出力装置３６が表示装置（例えばＣＲＴ）の場合、三次元画像上に自己位置を立体表示するのが好ましい。また、自己位置のデータを別の装置（例えば制御装置、コンピュータ）に転送してもよく、プリンタで出力してもよい。
また、出力ステップＳ１２において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するとともに、該計測値の信頼性または精度を示す指標（例えば、数値）を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置３６に出力してもよい。さらに、出力ステップＳ１２において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するときに、該ボクセルの内部の誤差分布の大きさ（広がり）が所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値（即ち、このボクセルの代表点の位置）を出力装置３６に出力しないようにしてもよい。
また、出力ステップＳ１２において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するとともに、該計測値の信頼性または精度を示す指標（例えば、数値）を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置３６に出力してもよい。さらに、出力ステップＳ１２において、ボクセルの代表点の位置を三次元形状の計測値として出力装置３６に出力するときに、該ボクセルの内部の誤差分布の大きさ（広がり）が所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値（即ち、このボクセルの代表点の位置）を出力装置３６に出力しないようにしてもよい。
また、前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力ステップにおいて、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力しても良い。
また、出力ステップＳ６において、距離センサの位置・姿勢が得られる場合は、その位置から見える範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力してもよい。距離センサの位置から見える範囲とは、距離センサの位置から距離センサが位置計測可能な範囲であり、距離センサの位置から距離センサが位置計測可能な角度範囲（視野）と、距離センサの位置から距離センサが位置計測可能な距離範囲とを含んでよい。
図６に示した処理の手順は、新たな計測位置において、新しい計測データが得られる度に、処理を繰り返し行い、内部記憶装置３４および外部記憶装置３３の少なくともいずれかに結果を格納する。処理を高速化するためには、内部記憶装置３４に容量が許す限り結果を格納することが好ましい。
上述した本発明の方法と装置によれば、三次元形状の存在する空間領域を、複数のボクセル６に分割し、各ボクセル位置を内部記憶装置３４および外部記憶装置３３の少なくともいずれかに記憶するので、計測対象物が大きい場合であっても、データ量をボクセル数に比例する小さいデータサイズに抑えることができる。
また、座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
従って、本発明のデータ構造により、複数視点の計測データを固定サイズに統合できる。
また、座標値に対応するボクセル６の内部に代表点７とその誤差分布８を設定し記憶するので、ボクセルの分解能以上の情報を表現することができる。
従って、本発明のデータ構造により、複数視点の計測データを固定サイズに統合できる。
また、ボクセルの内部に物体の存在確率を表す確率値を設定し、保存することによって、誤差分布が代表点の属するボクセルよりも広がっている場合においても、各ボクセルにおける物体の存在有無を代表点が属するボクセルを見つけ、その誤差分布から再計算させることなく、当該ボクセルの確率値だけで容易に判断できるので、検索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
また、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度を利用し、照合する範囲を制限するので、検索時間を抑えることができる。
また、粗合わせステップＳ６において、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値（例えば、当該距離の総和）が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値（例えば、当該距離の総和）が最小になるように、
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
もしくは新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値（例えば、当該確率値の総和）が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせするので、誤差の蓄積を防ぎながら、短時間に代表点を有するボクセル同士の位置合わせができる。
また、先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値（例えば、当該確率値の差の総和）が最小になるように、位置合わせする場合には、物体が存在しない情報も加味して位置合わせするので、精度の向上が図れる。
次いで、精密合わせステップＳ７において、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値（例えば、当該距離の総和）が最小になるように位置合わせするので、誤差分布同士の精密な位置合わせを短時間にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
従って、本発明による複数視点からのデータの統合処理により、誤差の蓄積を防ぎながら、高精度な形状取得を可能にできる。
また、本発明のデータ構造は、ボクセル構造を拡張した構造をしているため、データサイズは点群に比べて小さく済ませることができる。従って、周囲環境と計測データの位置合わせ（ＩＣＰアルゴリズム）を行う際に探索対象となるデータ量が減少することから、計算を効率化できる。
さらに、従来のＩＣＰアルゴリズムは疎データに対して誤った結果を出力するが、本発明の環境モデルは、ボクセル内に代表点と誤差分布を持っているため、疎データに対応した位置合わせが可能である。
また、モデル構築ステップＳ４において、最大のボクセル９を必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル９内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割するので、データ量を小さいデータサイズに抑えると同時に、分割後のボクセルと代表点を用いて解像度を更に高めることができる。
特に、三次元形状上の複数の座標値を複数の計測位置を原点とする距離データとして取得し、該距離データの座標値を、前記代表点の座標値とし、距離データの座標値の計測誤差を代表点の誤差分布とすることにより、正確な座標値と誤差分布を用いて複数回の計測を統計的に統合することができ、一層の精度向上が可能となる。
図１７は、複数の計測位置を原点とする距離データを統合することによって、代表点の誤差分布が縮小し、代表点の精度が向上する様子を示している。このように異なる計測位置（即ち、距離センサである三次元計測器の位置）を原点として得られた距離データは誤差分布の向きも異なるので、これらの距離データを環境モデルを介して逐次統合することによって、代表点の誤差分布が縮小し、代表点の位置精度が向上する。なお、図１７において、三次元計測後の図はコップの２次元断面を表わした模式図であり、三次元計測後の図の破線はコップの実際表面を表わしている。
また、自己位置の同定とともに、自己位置の誤差分布を同定し、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正することによって、一層の精度向上が可能となる。
また、原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定、もしくは消去することにより、誤った計測データの影響を除去することができる。
また、新たに入力された被計測点の座標値に対応するボクセルを探索し、該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定することにより、代表点の座標値と誤差分布を容易に設定できる。
更に、前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に八分割して階層的に複数のボクセルに分割する、ことにより、誤差の蓄積を回避しながら高精度な形状に収束させることができる。
従って、本発明の方法と装置によれば、誤差を含むデータを正確な情報に補正する機能を有すると共に、長時間の計測に対して高精度に収束する。なおかつ、位置同定処理は、ボクセルの各ボクセルに対応する代表点とその誤差分布を計測点で更新する処理であるため計算量が小さく、演算は周囲ボクセルへの影響を与えずボクセル内で閉じている。よって、高速な処理が可能である。また、計測データはボクセルに逐次統合可能であり、その結果得られた周囲環境情報のメモリサイズは固定サイズを上回ることはない。
カルマンフィルタを用いたモデル更新ステップについて、詳しく説明する。
カルマンフィルタを用いたモデル更新ステップの場合には、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する。
各モデル点群の位置m（ｉ）を状態量とし、距離センサの計測点の位置を基に、モデルを次の［数６］で表現する。なお、本実施例では、m（ｉ）は、ボクセル内部の代表点である（以下、同様）。
［数６］において、
Ｌ（ｊ）は、距離センサによる計測位置である。例えば、Ｌ（ｊ）は、距離センサのセンサ座標系において三次元ＬＲＦ（レーザレンジファインダ）の計測点ｊ（ｊ＝１，．．．，Ｎ）の位置Ｌ（ｊ）＝（ｘＬ（ｊ），ｙＬ（ｊ），ｚＬ（ｊ））ｔである。ここで、ｔは転置行列を示す（以下、同様）。
ｈｍ（Ｒｒ，ｔｒ，ｍ（ｉ））は、Ｌ（ｊ）に対する観測系モデルである。
Ｒｒは、距離センサを搭載した移動体（例えば移動ロボット）のワールド座標系に対する姿勢を表す回転行列Ｒｒ＝Ｒ（θｘ，θｙ，θｚ）である。なお、θｘ，θｙ，θｚは、それぞれｘ軸、ｙ軸、ｚ軸周りの回転角を示す（以下、同様）。
ｔｒは、上記移動体のワールド座標系に対する位置を表す並進ベクトルｔｒ＝（ｘ，ｙ，ｚ）である。
ｖＬ（ｉ）は、距離センサの計測値Ｌ（ｊ）に加わる観測ノイズである。
Ｒｓは、センサ座標系の移動体座標系に対する回転行列Ｒｓ＝Ｒ（θｘ，θｙ，θｚ）である。
ｔｓは、センサ座標系の移動体座標系に対する位置を表す併進ベクトルｔｓ＝（ｘ，ｙ，ｚ）である。
Ｌ（ｊ）は、距離センサによる計測位置である。例えば、Ｌ（ｊ）は、距離センサのセンサ座標系において三次元ＬＲＦ（レーザレンジファインダ）の計測点ｊ（ｊ＝１，．．．，Ｎ）の位置Ｌ（ｊ）＝（ｘＬ（ｊ），ｙＬ（ｊ），ｚＬ（ｊ））ｔである。ここで、ｔは転置行列を示す（以下、同様）。
ｈｍ（Ｒｒ，ｔｒ，ｍ（ｉ））は、Ｌ（ｊ）に対する観測系モデルである。
Ｒｒは、距離センサを搭載した移動体（例えば移動ロボット）のワールド座標系に対する姿勢を表す回転行列Ｒｒ＝Ｒ（θｘ，θｙ，θｚ）である。なお、θｘ，θｙ，θｚは、それぞれｘ軸、ｙ軸、ｚ軸周りの回転角を示す（以下、同様）。
ｔｒは、上記移動体のワールド座標系に対する位置を表す並進ベクトルｔｒ＝（ｘ，ｙ，ｚ）である。
ｖＬ（ｉ）は、距離センサの計測値Ｌ（ｊ）に加わる観測ノイズである。
Ｒｓは、センサ座標系の移動体座標系に対する回転行列Ｒｓ＝Ｒ（θｘ，θｙ，θｚ）である。
ｔｓは、センサ座標系の移動体座標系に対する位置を表す併進ベクトルｔｓ＝（ｘ，ｙ，ｚ）である。
測定対象物は静止しているものであり、測定対象物の位置ｔｒ、姿勢Ｒｒを環境モデルに対して固定する。
距離センサによる計測点群と、環境モデル点群上の点ｉ（即ち、代表点）を対応づける。この対応付けが行われたモデル点群上の点iに対して次式（４）により更新を行う。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点ｍ（i）に対してのみ次の数［７］により更新を行ってよい。
［数７］において、
添え字ｋは、離散時刻ｋでの値であることを表す。
ｍｋ（ｉ）について、ｍ’ｋ（ｉ）はｍｋ（ｉ）の更新値（事後推定値）を示し、ｍｋ，ｋ−１（ｉ）はｍ’ｋ−１（ｉ）に基づいたｍｋ（ｉ）の予測値（事前推定値）を示す。なお、環境（測定対象物）は静止しているので、ｍｋ，ｋ-１（ｉ）＝ｍ’ｋ-１（ｉ）である。
Σmk（ｉ）は、ボクセル内部の代表点ｍｋ（ｉ）の誤差共分散行列(即ち、上述の誤差分布)である。また、Σｍｋ（ｉ）について、 Σ’ｍｋ（ｉ）はΣｍｋ（ｉ）の更新値（事後推定値）を示し、Σｍｋ，ｋ−１（ｉ）はΣ’ｍｋ−１（ｉ）に基づいたΣｍｋ（ｉ）の予測値（事前推定値）を示す。センサ座標系において三次元LRFの計測点j（j=１,…,Ｎ）の位置をＬ（ｊ）で表し、その誤差共分散行列をΣＬ（ｊ）で表す。ここでＮは、三次元LRFで得られた計測点の総数である。三次元LRFの誤差モデルとして計測距離に関係ない一定の正規分布を仮定する。センサ座標系のｘ軸方向にレーザを照射する場合の誤差共分散行列をΣＳとする。レーザの照射方向に応じて誤差分布も姿勢を変える。ΣＬ（ｊ）は、基準の方向に対するレーザ照射方向を回転行列ＲＬ（ｊ）を用いてΣＬ（ｊ）＝ＲＬ（ｊ）ΣＳＲＬ ｔ（ｊ）と表される。計測点jのワールド座標系における位置ｚ（ｊ）、およびその誤差共分散行列Σｚ（ｊ）は、それぞれｚ（ｊ）＝Ｒｒ（ＲｓＬ（ｊ）＋ｔｓ）＋ｔｒ、Σｚ（ｊ）＝ＲｒＲｓΣＬ（ｊ）Ｒｓ ｔＲｒ ｔと表すことができる。
Ｋｍｋ（ｉ） は、 ｍｋ（ｉ）に対するカルマンゲインである。
ｈｍｋ（Ｒｒｋ，ｔｒｋ，ｍｋ，ｋ−１（ｉ））は、Ｌｋ（ｊ）、ｉ＝ｐｋ（ｊ）に対する観測系モデルである。ｉ＝ｐｋ（ｊ）は、計測点ｊに対応付けられた環境地図（即ち、環境モデル）上の点である。
Ｈｍｋは、Ｌｋ（ｊ）、ｉ＝ｐｋ（ｊ）に対する観測系モデルのヤコビアン行列であり、次の［数８］で表わされる。
添え字ｋは、離散時刻ｋでの値であることを表す。
ｍｋ（ｉ）について、ｍ’ｋ（ｉ）はｍｋ（ｉ）の更新値（事後推定値）を示し、ｍｋ，ｋ−１（ｉ）はｍ’ｋ−１（ｉ）に基づいたｍｋ（ｉ）の予測値（事前推定値）を示す。なお、環境（測定対象物）は静止しているので、ｍｋ，ｋ-１（ｉ）＝ｍ’ｋ-１（ｉ）である。
Σmk（ｉ）は、ボクセル内部の代表点ｍｋ（ｉ）の誤差共分散行列(即ち、上述の誤差分布)である。また、Σｍｋ（ｉ）について、 Σ’ｍｋ（ｉ）はΣｍｋ（ｉ）の更新値（事後推定値）を示し、Σｍｋ，ｋ−１（ｉ）はΣ’ｍｋ−１（ｉ）に基づいたΣｍｋ（ｉ）の予測値（事前推定値）を示す。センサ座標系において三次元LRFの計測点j（j=１,…,Ｎ）の位置をＬ（ｊ）で表し、その誤差共分散行列をΣＬ（ｊ）で表す。ここでＮは、三次元LRFで得られた計測点の総数である。三次元LRFの誤差モデルとして計測距離に関係ない一定の正規分布を仮定する。センサ座標系のｘ軸方向にレーザを照射する場合の誤差共分散行列をΣＳとする。レーザの照射方向に応じて誤差分布も姿勢を変える。ΣＬ（ｊ）は、基準の方向に対するレーザ照射方向を回転行列ＲＬ（ｊ）を用いてΣＬ（ｊ）＝ＲＬ（ｊ）ΣＳＲＬ ｔ（ｊ）と表される。計測点jのワールド座標系における位置ｚ（ｊ）、およびその誤差共分散行列Σｚ（ｊ）は、それぞれｚ（ｊ）＝Ｒｒ（ＲｓＬ（ｊ）＋ｔｓ）＋ｔｒ、Σｚ（ｊ）＝ＲｒＲｓΣＬ（ｊ）Ｒｓ ｔＲｒ ｔと表すことができる。
Ｋｍｋ（ｉ） は、 ｍｋ（ｉ）に対するカルマンゲインである。
ｈｍｋ（Ｒｒｋ，ｔｒｋ，ｍｋ，ｋ−１（ｉ））は、Ｌｋ（ｊ）、ｉ＝ｐｋ（ｊ）に対する観測系モデルである。ｉ＝ｐｋ（ｊ）は、計測点ｊに対応付けられた環境地図（即ち、環境モデル）上の点である。
Ｈｍｋは、Ｌｋ（ｊ）、ｉ＝ｐｋ（ｊ）に対する観測系モデルのヤコビアン行列であり、次の［数８］で表わされる。
カルマンフィルタの更新過程によって、環境地図のモデル点群の各点（ボクセルの代表点）の位置と誤差共分散行列の更新値ｍ’ｋ（ｉ）、Σ’mk（ｉ）が得られた段階で、環境モデルの更新を以下の手順で行う。
（１）これら更新値ｍ’ｋ（ｉ）、Σ’mk（ｉ）を、新たな代表点、誤差分布として再設定する。
（２）上述（１）の結果、代表点の位置が別のボクセル内に移動した場合、移動先のボクセルが代表点を保持していないときは、移動後の代表点とその誤差共分散行列を移動先のボクセルに保持させ、移動元のボクセルからは代表点等を取り除く。移動先のボクセルが既に代表点を保持しているときには、2つの代表点において、これらの両誤差分布が重複するかを判断する（上述のＳＴ４における判断と同様）。その後の処理は、図１３のＳＴ４以降の処理と同じであってよい。
（３）モデル点群上の代表点ｍ（i）と対応付けが行われなかった距離センサによる計測点について、当該計測点が含まれるボクセルが代表点を持たない場合は、計測点とその誤差分布をそのボクセルの代表点と誤差分布として追加し保持する。もし、ボクセル内に既に代表点が存在する場合には、ボクセル内にある対応付けが行われなかった他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボクセルに含まれるように、ボクセルを分割した上で分割後のボクセルに代表点等を継承させる。
（１）これら更新値ｍ’ｋ（ｉ）、Σ’mk（ｉ）を、新たな代表点、誤差分布として再設定する。
（２）上述（１）の結果、代表点の位置が別のボクセル内に移動した場合、移動先のボクセルが代表点を保持していないときは、移動後の代表点とその誤差共分散行列を移動先のボクセルに保持させ、移動元のボクセルからは代表点等を取り除く。移動先のボクセルが既に代表点を保持しているときには、2つの代表点において、これらの両誤差分布が重複するかを判断する（上述のＳＴ４における判断と同様）。その後の処理は、図１３のＳＴ４以降の処理と同じであってよい。
（３）モデル点群上の代表点ｍ（i）と対応付けが行われなかった距離センサによる計測点について、当該計測点が含まれるボクセルが代表点を持たない場合は、計測点とその誤差分布をそのボクセルの代表点と誤差分布として追加し保持する。もし、ボクセル内に既に代表点が存在する場合には、ボクセル内にある対応付けが行われなかった他の複数の計測点を含め、既存の代表点と各計測点とが全て異なるボクセルに含まれるように、ボクセルを分割した上で分割後のボクセルに代表点等を継承させる。
上述のカルマンフィルタを用いたモデル更新ステップを繰り返すことで、序々にボクセル内の誤差共分散行列（即ち、誤差分布）の範囲が小さくなるとともに、ボクセルが分割され易くなる。ボクセルが分割されることによって、初期ボクセルのサイズ以下の変化も表現することが可能となる。
図１８は、カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。図１９は図１８の一部拡大図である。これら図において、初期のボクセルの1辺の長さを100cmとし、再分割数を6分割まで許している。対象が存在している領域では、ボクセルの再分割を繰り返した結果、計測対象を精度良く表現している。対象が存在しない領域ではボクセルの再分割は行われず、必要十分なデータ量で環境を表現できることがわかる。また、各ボクセル内の代表点の誤差分布も小さく、環境地図を高精度で表現できている。このように、誤差を含むデータであってもカルマンフィルタの効果により、真値に収束した結果が得られる。さらに、この方法では計測データ数を増加させることによって標準偏差が小さくなり、精度のさらなる向上が期待できる。
図１８は、カルマンフィルタを用いたモデル更新ステップにより得られた結果を示す。図１９は図１８の一部拡大図である。これら図において、初期のボクセルの1辺の長さを100cmとし、再分割数を6分割まで許している。対象が存在している領域では、ボクセルの再分割を繰り返した結果、計測対象を精度良く表現している。対象が存在しない領域ではボクセルの再分割は行われず、必要十分なデータ量で環境を表現できることがわかる。また、各ボクセル内の代表点の誤差分布も小さく、環境地図を高精度で表現できている。このように、誤差を含むデータであってもカルマンフィルタの効果により、真値に収束した結果が得られる。さらに、この方法では計測データ数を増加させることによって標準偏差が小さくなり、精度のさらなる向上が期待できる。
また、測定対象物の位置・姿勢は固定しているため、更新を測定対象物の位置・姿勢と独立して行うことができる。なお、距離センサによる計測点群と対応付けが行われたモデル点群上の代表点ｍ（i）に対してのみ、上述のカルマンフィルタによる更新を行うことで、大幅な計算コストの削減が可能になる。
前記精密合わせステップにおいて、前記近接する誤差分布間の距離の総和が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値（例えば、当該一致度の総乗）が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせしてもよい。
この場合について詳しく説明する。
この場合について詳しく説明する。
環境地図（環境モデル）であるモデル点群、およびセンサの計測点群双方に誤差モデルを考慮しているため、前記一致度に関する評価値（例えば、当該一致度の総乗）の算出式（以下、評価関数と言う）にも双方の誤差を取り入れることが可能である。本実施例の場合、単に最も近い点を対応付けするのではなく、尤度の概念を取り入れることによって、評価関数に環境地図上の誤差分布、および計測点の誤差分布を加味し、現時点での環境地図において、各計測点の出現確率が最大となる場合に評価関数も最大となるように定める。
具体的には、計測点jに対応付けられた環境地図上の点ｉ＝ｐ（ｊ）の位置が、平均値（代表点）ｍ（ｉ）、誤差共分散行列Σｍ（ｉ）の正規分布に従うと仮定した上で、三次元LRFで計測した結果、Ｌ（ｊ）なる計測データが得られる確率値Ｐｒ（Ｌ（ｊ）｜ｍ（ｉ），Σｍ（ｉ））を点iと点jとの評価関数ＥＭ（ｉ，ｊ）とし、その総乗が最大となるように次の［数９］で評価関数を定める。
具体的には、計測点jに対応付けられた環境地図上の点ｉ＝ｐ（ｊ）の位置が、平均値（代表点）ｍ（ｉ）、誤差共分散行列Σｍ（ｉ）の正規分布に従うと仮定した上で、三次元LRFで計測した結果、Ｌ（ｊ）なる計測データが得られる確率値Ｐｒ（Ｌ（ｊ）｜ｍ（ｉ），Σｍ（ｉ））を点iと点jとの評価関数ＥＭ（ｉ，ｊ）とし、その総乗が最大となるように次の［数９］で評価関数を定める。
ただし、ω（ｊ）は、モデル点群の中に計測点ｊと対応付けられる点が存在する場合は１、それ以外の場合は０とする。
ここで、Ｐｒ（Ｌ（ｊ）｜ｑ）を環境地図の点がｑの位置にある場合にＬ（ｊ）なる計測データが得られる確率値を表すものとし、Ｐｒ（ｑ｜ｍ（ｉ），Σｍ（ｉ））を平均値ｍ（ｉ）、誤差共分散行列Σｍ（ｉ）の正規分布に従うと仮定した上で環境地図の点がｑの位置にある確率値を表すものとすると、数［１０］が成り立つ。
ここで、Ｐｒ（Ｌ（ｊ）｜ｑ）を環境地図の点がｑの位置にある場合にＬ（ｊ）なる計測データが得られる確率値を表すものとし、Ｐｒ（ｑ｜ｍ（ｉ），Σｍ（ｉ））を平均値ｍ（ｉ）、誤差共分散行列Σｍ（ｉ）の正規分布に従うと仮定した上で環境地図の点がｑの位置にある確率値を表すものとすると、数［１０］が成り立つ。
Ｐｒ（ｑ｜ｍ（ｉ），Σｍ（ｉ））は正規分布を仮定すると、次の［数１１］となる。
一方、Ｐｒ（Ｌ（ｊ）｜ｑ）は、Ｌ（ｊ）をｚ（ｊ）で置き換えて、次の［数１２］で近似することができる。
ここで、ｚｋ（ｊ）は、距離センサを搭載した移動体、例えば移動ロボットの位置ｔｒ、姿勢Ｒｒに依存している。実際は、三次元LRFのセンサ座標系の中心から見たｑの向きと計測点Ｌ（ｊ）の向きとは図２０に示すように異なるため、誤差共分散行列Σｚ（ｊ）もｑの向きに合わせて回転変換する必要があるが、対応付けられた環境地図上の点iから大きく離れたところにあるｑの存在確率は低いため、十分な精度で近似できると考えられる。よって、Ｐｒ（Ｌ（ｊ）｜ｍ（ｉ），Σｍ（ｉ））は、次の［数１３］で表すことができる。
簡単な計算によって、次の［数１４］を得る。
ただし、α（ｊ），β（ｊ）は次の［数１５］で表すことができる。
従って、モデル点群の点iと計測点群の点jとの対応付けの一致度を表す評価関数ＥＭ（ｉ，ｊ）は、平均値ｍ（ｉ）、誤差共分散行列Σｍ（ｉ）＋Σｚ（ｊ）の正規分布において、ｚ（ｊ）が得られる確率値に近似できる。この評価関数を用いることによって、環境地図、計測データ双方の誤差を考慮した対応付けが可能となる。
計測点と環境地図（即ち、環境モデル）との対応付けについて補足説明をする。上記実施例では、誤差分布を考慮した統計的な評価関数を用いるため、評価関数の値を求めなければ対応点を定めることができない。そこで、環境地図上のモデル点群の中で対応付けする候補を予め絞り込み、その候補の中から評価関数の値を基に対応点を求める。具体的には、以下のように定めることができる。
（１）対象とする計測点jの誤差共分散行列ΣＬ（ｊ）の範囲（例えば標準偏差の3倍の範囲）と交わる最上位のボクセルとそのボクセルに隣接している最上位のボクセルを求め、下層のボクセルも含めこれらのボクセル内に存在する代表点を対応点の候補とする。ボクセルが階層構造となっているため、この候補点の探索には計算コストはほとんどかからない。このとき、候補となる代表点がない場合には、対応点がないものとみなす。隣接するボクセルも候補に加える理由は、ボクセル内の代表点の位置によっては、誤差共分散行列の範囲が隣接するボクセルまではみ出すことがあるからである。
（２）候補となるボクセルの代表点iと誤差共分散行列を用いて、評価関数ＥＭ（ｉ，ｊ）の値を求める。
（３）評価関数ＥＭ（ｉ，ｊ）の値が最も大きい代表点iを対応点とする。ただし、評価関数の値がある閾値未満の場合には、対応点がないものとみなす。
本実施例では、対応付けの評価関数ＥＭ（ｉ，ｊ）として、尤度に基づいた式を採用しており、対応点の有無に関して統計的に明確な判断尺度があるため、対応点が存在しないと考えられる場合においても無理に対応付けを行うようなことはない。なお、対応点がない場合には、対象となる計測点はこれまで未計測の部分に相当する点であると解釈し、環境地図に追加する。
（１）対象とする計測点jの誤差共分散行列ΣＬ（ｊ）の範囲（例えば標準偏差の3倍の範囲）と交わる最上位のボクセルとそのボクセルに隣接している最上位のボクセルを求め、下層のボクセルも含めこれらのボクセル内に存在する代表点を対応点の候補とする。ボクセルが階層構造となっているため、この候補点の探索には計算コストはほとんどかからない。このとき、候補となる代表点がない場合には、対応点がないものとみなす。隣接するボクセルも候補に加える理由は、ボクセル内の代表点の位置によっては、誤差共分散行列の範囲が隣接するボクセルまではみ出すことがあるからである。
（２）候補となるボクセルの代表点iと誤差共分散行列を用いて、評価関数ＥＭ（ｉ，ｊ）の値を求める。
（３）評価関数ＥＭ（ｉ，ｊ）の値が最も大きい代表点iを対応点とする。ただし、評価関数の値がある閾値未満の場合には、対応点がないものとみなす。
本実施例では、対応付けの評価関数ＥＭ（ｉ，ｊ）として、尤度に基づいた式を採用しており、対応点の有無に関して統計的に明確な判断尺度があるため、対応点が存在しないと考えられる場合においても無理に対応付けを行うようなことはない。なお、対応点がない場合には、対象となる計測点はこれまで未計測の部分に相当する点であると解釈し、環境地図に追加する。
実施形態として、外界の三次元情報を取り入れて、自己の三次元位置を同定する自己位置同定方法と装置、および三次元形状の計測方法と装置について説明したが、二次元形状を三次元形状の特別な場合として見ることにより、外界の二次元情報を取り入れて、自己の二次元位置を同定する自己位置同定方法と装置、および二次元形状の計測方法と装置の形態としても実施できる。
また、上記出力ステップにおいて、前記ボクセル位置、代表点、および誤差分布のすべてを出力しなくてもよく、例えば、これらすべてが無くても三次元形状が把握できる場合や、これらのうち１つ又は２つが必要な場合などにおいては、前記ボクセル位置、代表点、および誤差分布の少なくともいずれかを出力装置に出力してもよい。
また、上記出力ステップにおいて、前記ボクセル位置、代表点、および誤差分布のすべてを出力しなくてもよく、例えば、これらすべてが無くても三次元形状が把握できる場合や、これらのうち１つ又は２つが必要な場合などにおいては、前記ボクセル位置、代表点、および誤差分布の少なくともいずれかを出力装置に出力してもよい。
なお、本発明は、上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。
Claims (31)
- 外界の三次元形状を取り入れて自己位置を同定する自己位置同定方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離の総和が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
更に前記自己位置を出力装置に出力する出力ステップを実施する、ことを特徴とする自己位置同定方法。 - 前記マッチングステップにおいて、ボクセルの内部に代表点とその誤差分布に加え、ボクセル内に物体の存在確率を表す確率値を設定し、記憶する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離の総和が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離の総和が最小になるように、位置合わせする粗合わせステップを有する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値の総和が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差の総和が最小になるように、位置合わせする粗合わせステップを有する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記データ入力ステップの後に、現在の計測位置を過去の計測位置の変化から推定し、もしくは現在の計測位置を取得可能なセンサにより取得し、もしくは計測データの距離値だけでなく反射強度値を利用し、照合する範囲を限定する探索範囲限定ステップを有する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記自己位置同定ステップにおいて、先の計測位置における位置及び姿勢から新たな計測位置の６自由度位置を同定する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記精密合わせステップにおいて、誤差分布が交差する場合を同一計測点とし、その場合の距離値に分布の一致度から求めた重みを掛け合わせて誤差分布間の距離を算出する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記モデル構築ステップにおいて、最大のボクセルを必要最小限の分解能に相当する大きさに設定し、かつ単一のボクセル内に複数の被計測点が存在する場合に、単一のボクセル内に単一の被計測点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
原点と被計測点の間に物体が存在しないものとして、その間に位置するボクセル内の代表点と誤差分布を再設定もしくは消去する、ことを特徴とする請求項１に記載の自己位置同定方法。 - 前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項１に記載の自己位置同定方法。 - 前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１に記載の自己位置同定方法。 - 前記自己位置同定ステップにおいて、自己位置の同定とともに、自己位置の誤差分布を同定し、前記出力ステップの前に、現在の自己位置と誤差分布および同定した自己位置と誤差分布とからカルマンフィルタによって、自己位置と誤差分布を補正し、出力ステップにおいて、自己位置と誤差分布を出力する、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記モデル更新ステップにおいて、前記新たに取得した誤差分布と前記既に設定したボクセル内の誤差分布とを比較し、誤差分布が互いに重複する場合に、両誤差分布から新たな誤差分布と新たな代表点を再設定した結果、新たな代表点が他のボクセル内へ移動したとき、
該他のボクセル内に代表点がない場合に、該新たな誤差分布と該新たな代表点を該他のボクセルの内部に設定し、
該他のボクセル内に既に設定した代表点がある場合に、該新たな誤差分布と既に設定した該他のボクセル内の誤差分布とを比較し、（Ａ）誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、（Ｂ）誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項１に記載の自己位置同定方法。 - 前記自己位置同定ステップの後に、前記環境モデルを更新するモデル更新ステップを有し、
該モデル更新ステップにおいて、新たに入力された被計測点の座標値およびその誤差分布と、既に設定したボクセル内の代表点およびその誤差分布とから、カルマンフィルタにより新たな代表点と誤差分布を取得して再設定する、ことを特徴とする請求項１に記載の自己位置同定方法。 - 前記精密合わせステップにおいて、前記近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする代わりに、前記近接する誤差分布間に基づく最尤推定値によって定めた一致度に関する評価値が最大となるように、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させて位置合わせする、ことを特徴とする請求項１に記載の自己位置同定方法。
- 前記一致度に関する評価値の算出式が次の［数１６］で表わされ、
- 外界の三次元形状を取り入れて自己位置を同定する自己位置同定方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施する、ことを特徴とする自己位置同定方法。 - 前記自己位置を出力装置に出力する出力ステップを実施する、ことを特徴とする請求項１７に記載の自己位置同定方法。
- 前記マッチングステップにおいて、ボクセルの内部に代表点とその誤差分布に加え、ボクセル内に物体の存在確率を表す確率値を設定し、記憶する、ことを特徴とする請求項１７に記載の自己位置同定方法。
- 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセル間との距離に関する評価値が最小になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、代表点を有するボクセル間の距離に関する評価値が最小になるように、位置合わせする粗合わせステップを有する、ことを特徴とする請求項１７に記載の自己位置同定方法。
- 前記精密合わせステップの前に、先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、計測データおよび誤差分布と近接する代表点を有するボクセルの確率値に関する評価値が最大になるように、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接するボクセルが持つ確率値の差に関する評価値が最小になるように、位置合わせする粗合わせステップを有する、ことを特徴とする請求項１７に記載の自己位置同定方法。
- 外界の三次元形状の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測方法であって、
新たな計測位置において、三次元形状上の座標値をコンピュータに入力するデータ入力ステップと、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築ステップと、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチングステップとを実施し、
先の計測位置における前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測データおよび誤差分布を回転及び並進させ、もしくは新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせする精密合わせステップと、
前記精密合わせステップにおける回転量及び並進量から自己位置を同定する自己位置同定ステップとを実施し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力する出力ステップを有する、ことを特徴とする三次元形状の計測方法。 - 前記出力ステップにおいて、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するとともに、該計測値の信頼性または精度を示す指標を、該ボクセルの内部の誤差分布の大きさに基づいて、出力装置に出力する、ことを特徴とする請求項２２に記載の三次元形状の計測方法。
- 前記出力ステップにおいて、前記ボクセルの位置、代表点の位置および誤差分布の位置の少なくともいずれかを三次元形状の計測値として出力装置に出力するときに、該ボクセルの内部の誤差分布の大きさが所定の基準値よりも大きい場合に、該計測値の信頼性または精度が所定の基準よりも低いとして、該ボクセルの前記計測値を出力装置に出力しない、ことを特徴とする請求項２２に記載の三次元形状の計測方法。
- 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
該ボクセル内に代表点がない場合に、前記座標値と誤差分布を代表点の座標値と誤差分布として設定する、ことを特徴とする請求項２２に記載の三次元形状の計測方法。 - 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、
前記ボクセル内に既に設定した代表点がある場合に、新たに取得した誤差分布と既に設定したボクセル内の誤差分布とを比較し、
誤差分布が互いに重複する場合に、両誤差分布から、または、両誤差分布とボクセル内に既に設定した代表点と新たに入力された被計測点の座標値から、新たな誤差分布と新たな代表点を再設定し、
誤差分布が互いに重複しない場合に、単一のボクセル内に単一の代表点のみが存在するように、該ボクセルを更に分割して階層的に複数のボクセルに分割する、ことを特徴とする請求項２２に記載の三次元形状の計測方法。 - 前記マッチングステップの後に、前記環境モデルを更新するモデル更新ステップを有し、該モデル更新ステップにおいて、新たに入力された被計測点の座標値に対応するボクセルを探索し、前記ボクセル内の代表点および誤差分布の少なくともいずれかが新たに設定される、または再設定される、または該ボクセルを更に分割して階層的に複数のボクセルに分割される場合、前記出力ステップにおいて、当該ボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する、ことを特徴とする請求項２２に記載の三次元形状の計測方法。
- 前記出力ステップにおいて、距離センサの位置から距離センサが位置計測可能な範囲の環境モデル内のボクセルの代表点の位置を三次元形状の計測値として出力装置に出力する、ことを特徴とする請求項２２に記載の三次元形状の計測方法。
- 外界の三次元形状を取り入れて自己位置を同定する自己位置同定装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
自己位置を出力装置に出力する出力装置とを備え、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離の総和が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定する、ことを特徴とする自己位置同定装置。 - 外界の三次元形状を取り入れて自己位置を同定する自己位置同定装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
自己位置を出力装置に出力する出力装置とを備え、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定する、ことを特徴とする自己位置同定装置。 - 三次元形状上の被計測点の座標値から三次元形状を復元し、三次元形状データを出力する三次元形状の計測装置であって、
三次元形状上の座標値をコンピュータに入力するデータ入力装置と、
前記三次元形状の存在する空間領域を、境界表面が互いに直交する直方体からなる複数のボクセルに分割し、各ボクセル位置を記憶する環境モデルを構築するモデル構築装置と、
前記座標値に対応するボクセルの内部に代表点とその誤差分布を設定し記憶するマッチング装置と、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在しない場合には、新たな計測位置を自己位置と同定し、
新たな計測位置において、先の計測位置おける前記三次元形状のデータが存在する場合には、
先の計測位置に対する環境モデルに対し、新たな計測位置に対する環境モデルを回転及び並進させ、近接する誤差分布間の距離に関する評価値が最小になるように位置合わせし、
前記位置合わせにおける回転量及び並進量から自己位置を同定し、
更に前記自己位置、前記自己位置に基づいた前記ボクセル位置、代表点、および、誤差分布の少なくともいずれかを出力装置に出力するデータ伝達装置とを備える、ことを特徴とする三次元形状の計測装置。
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2005363320 | 2005-12-16 | ||
| JP2005363320 | 2005-12-16 | ||
| PCT/JP2006/325051 WO2007069726A1 (ja) | 2005-12-16 | 2006-12-15 | 自己位置同定方法と装置および三次元形状の計測方法と装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPWO2007069726A1 JPWO2007069726A1 (ja) | 2009-05-28 |
| JP4650752B2 true JP4650752B2 (ja) | 2011-03-16 |
Family
ID=38163028
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2007550243A Active JP4650752B2 (ja) | 2005-12-16 | 2006-12-15 | 自己位置同定方法と装置および三次元形状の計測方法と装置 |
Country Status (5)
| Country | Link |
|---|---|
| US (1) | US8121399B2 (ja) |
| JP (1) | JP4650752B2 (ja) |
| CN (1) | CN101331379B (ja) |
| DE (1) | DE112006003363B4 (ja) |
| WO (1) | WO2007069726A1 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104501811A (zh) * | 2014-11-24 | 2015-04-08 | 杭州申昊科技股份有限公司 | 一种基于环境直线特征的地图匹配方法 |
Families Citing this family (55)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4455417B2 (ja) * | 2005-06-13 | 2010-04-21 | 株式会社東芝 | 移動ロボット、プログラム及びロボット制御方法 |
| JP4650750B2 (ja) * | 2005-12-16 | 2011-03-16 | 株式会社Ｉｈｉ | 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置 |
| DE112006003380T5 (de) * | 2005-12-16 | 2008-10-16 | Ihi Corporation | Verfahren und Vorrichtung für die Positionsabgleichung von dreidimensionalen Formdaten |
| JP4675811B2 (ja) * | 2006-03-29 | 2011-04-27 | 株式会社東芝 | 位置検出装置、自律移動装置、位置検出方法および位置検出プログラム |
| JP5380792B2 (ja) * | 2007-06-15 | 2014-01-08 | 株式会社Ｉｈｉ | 物体認識方法および装置 |
| JP5246468B2 (ja) * | 2007-11-21 | 2013-07-24 | 株式会社Ｉｈｉ | バリまたは欠損認識方法と装置およびバリ取り方法と装置 |
| JP5342556B2 (ja) | 2008-07-10 | 2013-11-13 | 三菱電機株式会社 | 隊列走行支援装置 |
| US8855819B2 (en) * | 2008-10-09 | 2014-10-07 | Samsung Electronics Co., Ltd. | Method and apparatus for simultaneous localization and mapping of robot |
| KR20100081881A (ko) * | 2009-01-07 | 2010-07-15 | 삼성전자주식회사 | 데이터 매칭 장치, 데이터 매칭 방법, 및 이를 이용한 이동로봇 |
| TWI409717B (zh) * | 2009-06-22 | 2013-09-21 | Chunghwa Picture Tubes Ltd | 適用於電腦產品與影像顯示裝置的影像轉換方法 |
| US8224071B2 (en) * | 2009-06-30 | 2012-07-17 | Mitsubishi Electric Research Laboratories, Inc. | Method for registering 3D points with 3D planes |
| JP5614118B2 (ja) * | 2010-06-15 | 2014-10-29 | 富士通株式会社 | ランドマーク検知方法、ロボット及びプログラム |
| DE102010017630B4 (de) * | 2010-06-29 | 2016-06-02 | Leica Microsystems Cms Gmbh | Verfahren und Einrichtung zur lichtmikroskopischen Abbildung einer Probenstruktur |
| WO2012086029A1 (ja) * | 2010-12-22 | 2012-06-28 | 株式会社日立製作所 | 自律移動システム |
| US20120173185A1 (en) * | 2010-12-30 | 2012-07-05 | Caterpillar Inc. | Systems and methods for evaluating range sensor calibration data |
| EP2477000A1 (de) * | 2011-01-14 | 2012-07-18 | Leica Geosystems AG | Vermessungsgerät mit einer automatischen Darstellungswechsel-Funktionalität |
| CN103717995B (zh) * | 2011-08-29 | 2016-05-11 | 株式会社日立制作所 | 监视装置、监视系统及监视方法 |
| KR102123196B1 (ko) * | 2011-09-13 | 2020-06-16 | 오에스아이 옵토일렉트로닉스 | 개선된 레이저 레인지파인더 센서 |
| US8868344B2 (en) * | 2011-09-22 | 2014-10-21 | Honeywell International Inc. | Systems and methods for combining a priori data with sensor data |
| CN102661742B (zh) * | 2012-05-30 | 2014-04-02 | 北京信息科技大学 | 基于曲率特征加权质心点约束的自适应标志点布局方法 |
| US9816809B2 (en) | 2012-07-04 | 2017-11-14 | Creaform Inc. | 3-D scanning and positioning system |
| EP2875314B1 (en) * | 2012-07-18 | 2017-08-16 | Creaform Inc. | System and method for 3d scanning of the surface geometry of an object |
| JP6192088B2 (ja) * | 2013-02-20 | 2017-09-06 | 国立大学法人九州工業大学 | 物体検出方法および物体検出装置 |
| DE102013211126A1 (de) * | 2013-06-14 | 2014-12-18 | Robert Bosch Gmbh | Verfahren zum Modellieren eines Umfelds eines Fahrzeugs |
| US8996207B2 (en) * | 2013-06-24 | 2015-03-31 | Honeywell International Inc. | Systems and methods for autonomous landing using a three dimensional evidence grid |
| US9664784B2 (en) * | 2013-12-04 | 2017-05-30 | Trimble Inc. | System and methods for data point detection and spatial modeling |
| WO2015085483A1 (en) | 2013-12-10 | 2015-06-18 | SZ DJI Technology Co., Ltd. | Sensor fusion |
| WO2016033795A1 (en) | 2014-09-05 | 2016-03-10 | SZ DJI Technology Co., Ltd. | Velocity control for an unmanned aerial vehicle |
| EP3428766B1 (en) | 2014-09-05 | 2021-04-07 | SZ DJI Technology Co., Ltd. | Multi-sensor environmental mapping |
| EP3008535B1 (en) | 2014-09-05 | 2018-05-16 | SZ DJI Technology Co., Ltd. | Context-based flight mode selection |
| CN104501794A (zh) * | 2014-11-24 | 2015-04-08 | 杭州申昊科技股份有限公司 | 一种基于环境直线特征的地图创建方法 |
| CN104503449A (zh) * | 2014-11-24 | 2015-04-08 | 杭州申昊科技股份有限公司 | 一种基于环境直线特征的定位方法 |
| JP6354556B2 (ja) * | 2014-12-10 | 2018-07-11 | 株式会社デンソー | 位置推定装置、位置推定方法、位置推定プログラム |
| US9519061B2 (en) * | 2014-12-26 | 2016-12-13 | Here Global B.V. | Geometric fingerprinting for localization of a device |
| JP6601613B2 (ja) * | 2015-06-08 | 2019-11-06 | 株式会社パスコ | 位置推定方法、位置推定装置及び位置推定プログラム |
| JP6528641B2 (ja) * | 2015-10-26 | 2019-06-12 | トヨタ自動車株式会社 | 自己位置推定方法 |
| FR3046848B1 (fr) * | 2016-01-14 | 2018-01-05 | Donecle | Procede et systeme de determination de la position d'un engin mobile |
| JP6331114B2 (ja) * | 2016-03-30 | 2018-05-30 | パナソニックＩｐマネジメント株式会社 | 位置推定装置、位置推定方法及び制御プログラム |
| WO2018027259A1 (en) | 2016-08-08 | 2018-02-15 | Bionics Institute | Systems and methods for monitoring neural activity |
| JP6501420B2 (ja) | 2017-01-31 | 2019-04-17 | 株式会社トヨタマップマスター | プローブ情報処理装置、プローブ情報処理方法、コンピュータプログラム及びコンピュータプログラムを記録した記録媒体 |
| JP6819441B2 (ja) * | 2017-04-24 | 2021-01-27 | 日産自動車株式会社 | 物標位置推定方法及び物標位置推定装置 |
| CN111200967B (zh) | 2017-05-22 | 2023-06-06 | 深部脑刺激技术有限公司 | 用于监测神经活动的系统和方法 |
| EP3633407A4 (en) * | 2017-05-24 | 2020-06-10 | Sony Corporation | INFORMATION PROCESSING APPARATUS, INFORMATION PROCESSING METHOD, AND PROGRAM |
| WO2018221458A1 (ja) * | 2017-05-31 | 2018-12-06 | パイオニア株式会社 | 更新装置、制御方法、プログラム及び記憶媒体 |
| CN117607841A (zh) * | 2017-08-11 | 2024-02-27 | 祖克斯有限公司 | 车辆传感器的校准和定位 |
| JP7051366B2 (ja) * | 2017-10-18 | 2022-04-11 | 株式会社東芝 | 情報処理装置、学習済モデル、情報処理方法、およびプログラム |
| WO2019188704A1 (ja) * | 2018-03-29 | 2019-10-03 | パイオニア株式会社 | 自己位置推定装置、自己位置推定方法、プログラム及び記憶媒体 |
| EP3584662B1 (en) * | 2018-06-19 | 2022-04-13 | Panasonic Intellectual Property Management Co., Ltd. | Mobile robot |
| CN108915470B (zh) * | 2018-06-26 | 2024-04-30 | 深圳拓邦股份有限公司 | 无轨电动门偏移检测装置及无轨电动门 |
| CN112955713A (zh) * | 2018-10-29 | 2021-06-11 | 大疆科技股份有限公司 | 用于在可移动物体环境中进行实时地图构建的技术 |
| WO2020157844A1 (ja) * | 2019-01-30 | 2020-08-06 | 三菱電機株式会社 | 計測装置、計測方法及び計測プログラム |
| US11113873B1 (en) * | 2019-09-27 | 2021-09-07 | Zoox, Inc. | Modeling articulated objects |
| CN112684728B (zh) * | 2019-10-17 | 2021-09-28 | 广东原点智能技术有限公司 | 一种基于激光slam的机器人码垛控制方法 |
| KR102461980B1 (ko) * | 2020-11-30 | 2022-11-03 | (주)심스리얼리티 | 3차원 지도의 생성 방법 |
| JP7444136B2 (ja) | 2021-06-08 | 2024-03-06 | 株式会社デンソー | 物体認識システム、物体認識方法、物体認識プログラム |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63186308A (ja) * | 1987-01-28 | 1988-08-01 | Hitachi Ltd | 移動体の誘導方法、および装置 |
| JPH0618221A (ja) * | 1991-03-11 | 1994-01-25 | Agency Of Ind Science & Technol | 多視点距離データの統合方法 |
| JPH09229648A (ja) * | 1996-02-21 | 1997-09-05 | Canon Inc | 画像情報入出力装置及び画像情報入出力方法 |
| JPH09231370A (ja) * | 1996-02-21 | 1997-09-05 | Canon Inc | 画像情報入力装置 |
| JP2000113193A (ja) * | 1998-10-08 | 2000-04-21 | Minolta Co Ltd | 多視点３次元データの合成方法および記録媒体 |
| JP2001022939A (ja) * | 1999-07-12 | 2001-01-26 | Nippon Telegr & Teleph Corp <Ntt> | ３次元情報抽出方法、装置、および３次元情報抽出プログラムを記録した記録媒体 |
| JP2001236522A (ja) * | 1999-12-17 | 2001-08-31 | Canon Inc | 画像処理装置 |
| JP2003296755A (ja) * | 2001-11-27 | 2003-10-17 | Samsung Electronics Co Ltd | 深さイメージに基づく３次元物体を表現するためのノード構造 |
| JP2004005373A (ja) * | 2001-11-27 | 2004-01-08 | Samsung Electronics Co Ltd | 深さイメージに基づく３次元物体を表現するためのノード構造 |
| JP2004521423A (ja) * | 2001-03-12 | 2004-07-15 | コーニンクレッカ フィリップス エレクトロニクス エヌ ヴィ | ８分木を用いた多数の画像からの３次元表現の生成 |
| JP2005037379A (ja) * | 2003-06-30 | 2005-02-10 | Sanyo Electric Co Ltd | 三次元モデリング方法と装置 |
| WO2007069721A1 (ja) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置 |
| WO2007069724A1 (ja) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | 三次元形状データの位置合わせ方法と装置 |
Family Cites Families (23)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2625345A1 (fr) | 1987-12-24 | 1989-06-30 | Thomson Cgr | Procede de visualisation en trois dimensions d'objets codes numeriquement sous forme arborescente et dispositif de mise en oeuvre |
| JPH03170345A (ja) | 1989-11-28 | 1991-07-23 | Asahi Glass Co Ltd | サーマルヘッドの抵抗体オーバーコートガラス組成物 |
| JP3170345B2 (ja) | 1992-05-13 | 2001-05-28 | 日本電信電話株式会社 | ３次元情報抽出方法 |
| JPH06223201A (ja) | 1993-01-22 | 1994-08-12 | Matsushita Electric Ind Co Ltd | 並列画像生成装置 |
| US5724493A (en) | 1994-12-13 | 1998-03-03 | Nippon Telegraph & Telephone Corporation | Method and apparatus for extracting 3D information of feature points |
| JPH0981788A (ja) | 1995-09-12 | 1997-03-28 | Toshiba Corp | 環境モデル入力装置 |
| US5689629A (en) | 1995-12-12 | 1997-11-18 | The Regents Of The University Of California | Iterative optimizing quantization method for reconstructing three-dimensional images from a limited number of views |
| US6064942A (en) | 1997-05-30 | 2000-05-16 | Rockwell Collins, Inc. | Enhanced precision forward observation system and method |
| JPH1196374A (ja) | 1997-07-23 | 1999-04-09 | Sanyo Electric Co Ltd | ３次元モデリング装置、３次元モデリング方法および３次元モデリングプログラムを記録した媒体 |
| JP3813343B2 (ja) | 1997-09-09 | 2006-08-23 | 三洋電機株式会社 | ３次元モデリング装置 |
| US6026189A (en) | 1997-11-13 | 2000-02-15 | National Research Council Of Canada | Method of recognizing objects within two-dimensional and three-dimensional images |
| US7477768B2 (en) | 1999-06-29 | 2009-01-13 | The Research Foundation Of State University Of New York | System and method for performing a three-dimensional virtual examination of objects, such as internal organs |
| US6476803B1 (en) | 2000-01-06 | 2002-11-05 | Microsoft Corporation | Object modeling system and process employing noise elimination and robust surface extraction techniques |
| US6914601B2 (en) | 2001-06-12 | 2005-07-05 | Minolta Co., Ltd. | Method, apparatus, and computer program for generating three-dimensional shape data or volume data |
| JP2003015739A (ja) | 2001-07-02 | 2003-01-17 | Yaskawa Electric Corp | 外環境地図、並びに自己位置同定装置および誘導制御装置 |
| JP2003065736A (ja) | 2001-08-24 | 2003-03-05 | Sanyo Electric Co Ltd | ３次元モデリング装置 |
| KR100812506B1 (ko) * | 2002-05-31 | 2008-03-11 | 후지쯔 가부시끼가이샤 | 원격 조작 로봇 및 로봇 자기 위치 추정 방법 |
| EP1519318A4 (en) * | 2002-06-28 | 2008-11-19 | Fujitsu Ltd | PROGRAM AND METHOD FOR COMPARING THREE-DIMENSIONAL IMAGE AND APPARATUS THEREFOR |
| US7317456B1 (en) | 2002-12-02 | 2008-01-08 | Ngrain (Canada) Corporation | Method and apparatus for transforming point cloud data to volumetric data |
| US7843512B2 (en) | 2004-03-31 | 2010-11-30 | Honeywell International Inc. | Identifying key video frames |
| US20060017720A1 (en) | 2004-07-15 | 2006-01-26 | Li You F | System and method for 3D measurement and surface reconstruction |
| JP5314244B2 (ja) | 2004-10-27 | 2013-10-16 | 富山化学工業株式会社 | 新規な含窒素複素環化合物およびその塩 |
| US7653235B2 (en) | 2005-10-27 | 2010-01-26 | Honeywell International Inc. | Surface anomaly detection system and method |
-
2006
- 2006-12-15 CN CN2006800474622A patent/CN101331379B/zh active Active
- 2006-12-15 US US12/096,830 patent/US8121399B2/en not_active Expired - Fee Related
- 2006-12-15 WO PCT/JP2006/325051 patent/WO2007069726A1/ja active Application Filing
- 2006-12-15 JP JP2007550243A patent/JP4650752B2/ja active Active
- 2006-12-15 DE DE112006003363.8T patent/DE112006003363B4/de active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS63186308A (ja) * | 1987-01-28 | 1988-08-01 | Hitachi Ltd | 移動体の誘導方法、および装置 |
| JPH0618221A (ja) * | 1991-03-11 | 1994-01-25 | Agency Of Ind Science & Technol | 多視点距離データの統合方法 |
| JPH09229648A (ja) * | 1996-02-21 | 1997-09-05 | Canon Inc | 画像情報入出力装置及び画像情報入出力方法 |
| JPH09231370A (ja) * | 1996-02-21 | 1997-09-05 | Canon Inc | 画像情報入力装置 |
| JP2000113193A (ja) * | 1998-10-08 | 2000-04-21 | Minolta Co Ltd | 多視点３次元データの合成方法および記録媒体 |
| JP2001022939A (ja) * | 1999-07-12 | 2001-01-26 | Nippon Telegr & Teleph Corp <Ntt> | ３次元情報抽出方法、装置、および３次元情報抽出プログラムを記録した記録媒体 |
| JP2001236522A (ja) * | 1999-12-17 | 2001-08-31 | Canon Inc | 画像処理装置 |
| JP2004521423A (ja) * | 2001-03-12 | 2004-07-15 | コーニンクレッカ フィリップス エレクトロニクス エヌ ヴィ | ８分木を用いた多数の画像からの３次元表現の生成 |
| JP2003296755A (ja) * | 2001-11-27 | 2003-10-17 | Samsung Electronics Co Ltd | 深さイメージに基づく３次元物体を表現するためのノード構造 |
| JP2004005373A (ja) * | 2001-11-27 | 2004-01-08 | Samsung Electronics Co Ltd | 深さイメージに基づく３次元物体を表現するためのノード構造 |
| JP2005037379A (ja) * | 2003-06-30 | 2005-02-10 | Sanyo Electric Co Ltd | 三次元モデリング方法と装置 |
| WO2007069721A1 (ja) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置 |
| WO2007069724A1 (ja) * | 2005-12-16 | 2007-06-21 | Ihi Corporation | 三次元形状データの位置合わせ方法と装置 |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104501811A (zh) * | 2014-11-24 | 2015-04-08 | 杭州申昊科技股份有限公司 | 一种基于环境直线特征的地图匹配方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| US20090167761A1 (en) | 2009-07-02 |
| JPWO2007069726A1 (ja) | 2009-05-28 |
| DE112006003363B4 (de) | 2016-05-04 |
| WO2007069726A1 (ja) | 2007-06-21 |
| DE112006003363T5 (de) | 2009-01-02 |
| CN101331379B (zh) | 2012-04-11 |
| CN101331379A (zh) | 2008-12-24 |
| US8121399B2 (en) | 2012-02-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4650752B2 (ja) | 自己位置同定方法と装置および三次元形状の計測方法と装置 | |
| JP4650751B2 (ja) | 三次元形状データの位置合わせ方法と装置 | |
| JP4650750B2 (ja) | 三次元形状データの記憶・表示方法と装置および三次元形状の計測方法と装置 | |
| JP5380792B2 (ja) | 物体認識方法および装置 | |
| EP3280977B1 (en) | Method and device for real-time mapping and localization | |
| CN109270545B (zh) | 一种定位真值校验方法、装置、设备及存储介质 | |
| Alsadik et al. | The simultaneous localization and mapping (SLAM)-An overview | |
| JP5627325B2 (ja) | 位置姿勢計測装置、位置姿勢計測方法、およびプログラム | |
| US11568559B2 (en) | Localization of a surveying instrument | |
| Yang et al. | On solving mirror reflection in lidar sensing | |
| EP4109137A1 (en) | Capturing environmental scans using automated transporter robot | |
| US20220057518A1 (en) | Capturing environmental scans using sensor fusion | |
| JP5246468B2 (ja) | バリまたは欠損認識方法と装置およびバリ取り方法と装置 | |
| Kaijaluoto | Precise indoor localization for mobile laser scanner | |
| US20240004080A1 (en) | Capturing environmental scans using landmarks based on semantic features | |
| US20230316567A1 (en) | Localization of a surveying instrument | |
| KR102130687B1 (ko) | 다중 센서 플랫폼 간 정보 융합을 위한 시스템 | |
| US20220406005A1 (en) | Targetless tracking of measurement device during capture of surrounding data | |
| de Haag et al. | Laser‐Based Navigation | |
| Steffen | Visual SLAM from image sequences acquired by unmanned aerial vehicles | |
| Mützel et al. | Geometric features for robust registration of point clouds | |
| Casagrande | Relative pose estimation of a plane on an airfield with automotive-class solid-state LiDAR sensors: Enhancing vehicular localization with point cloud registration | |
| Fossel | Improving Light Detection and Ranging Based Simultaneous Localization and Mapping with Advanced Map Representations | |
| Shokrzadeh | Simultaneous Localization and Mapping for Semi-Sparse Point Clouds | |
| Slattery | Alignment of Centimeter Scale Bathymetry using Six Degrees of Freedom |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20101119 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20101202 |
|
| R151 | Written notification of patent or utility model registration |
Ref document number: 4650752 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R151 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131224 Year of fee payment: 3 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |