JP6761388B2

JP6761388B2 - 推定装置及びプログラム

Info

Publication number: JP6761388B2
Application number: JP2017162463A
Authority: JP
Inventors: 康平松▲崎▼
Original assignee: KDDI Corp
Current assignee: KDDI Corp
Priority date: 2017-08-25
Filing date: 2017-08-25
Publication date: 2020-09-23
Anticipated expiration: 2037-08-25
Also published as: JP2019040445A

Description

本発明は、自己位置推定処理などにおいて必要となるリファレンス点群（地図点群）とクエリ点群（センサ点群）とのボクセルに基づくマッチングを、ボクセル分割位置の相違が存在しうる状況においても可能とする推定装置及びプログラムに関する。

近年、様々な種類の距離センサ（例えば, LiDAR, Time-of-flight camera, structured light）の商用化に伴い、3D点群(point cloud)が注目を集めている。3D点群は，物体認識や領域のセグメンテーション，自己位置推定，変化検知等のコンピュータビジョンにおける様々な技術領域で利用される。

3D点群を用いた自己位置推定技術は、予め実世界（フィールド）の特定の範囲をセンサで計測して得られた点群（以後、「地図点群」と呼ぶ。）と、ユーザがその場でセンサを用いて取得した点群（以後、「センサ点群」と呼ぶ。）との位置合わせによって実現される。例えば、代表的な自己位置推定方法として，非特許文献１におけるIterative Closest Point (ICP)がある。

ここで、近年の距離センサは1秒間の間に数十万点以上の3D点を計測するため、実世界の広範囲の計測を介して生成される地図点群のデータ量は膨大となる。そのため、要求されるストレージ量の削減及び／又は点群を用いた処理を効率化するために、点群のデータ量の削減が行われることがある。

その方法の一つに、点群のダウンサンプリングがある。この方法は、例えば所与の点群からランダムに指定個数の点を取得することにより、データ量を削減する。しかしながらこの方法は、単純にデータをスパースにするため、データ量を削減するほど、後段の処理の結果に対して悪影響（例えば位置合わせ精度の低下）を与える可能性が高くなる。

また、データ量削減のためのもう一つの方法として、点群のモデル化がある。この方法は、例えば多変量正規分布の様な確率密度モデルを用いて点群をモデル化し、元々のデータをモデルパラメータの集合として表現することによって、データ量を削減する。
3D点群データのモデル化手法の多くは、ボクセルグリッドに依存している。非特許文献２におけるNormal Distribution Transform (NDT)は空間をボクセルで区切った後に、各ボクセル内に位置する点群を3次元多変量正規分布のパラメータ、すなわち平均と分散共分散行列で表現する。そして、モデル化された地図点群とセンサ点群を用いて自己位置推定を実現する。

非特許文献1 P. Besl and N. McKay, "A Method for Registration of 3-D Shapes," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, pp. 239-256, 1992. Magnusson, Martin, Achim Lilienthal, and Tom Duckett. "Scan registration for autonomous mining vehicles using 3D‐NDT." Journal of Field Robotics 24.10 (2007): 803-827.

しかしながら、以上のような従来手法の点群データ量の削減には次のような課題があった。まず、非特許文献２のようなモデル化手法には量子化誤りの問題があった。

すなわち、モデル化の際に連続的な幾何的形状を計測した3D点群をボクセルで区切ることは、データのボクセル量子化誤りに起因する問題をもたらす。データのボクセル量子化誤りを説明するために、模式図を図１に示す。図１において、楕円体はある点群PG1のデータ分布を模式的に表し、直線で形成された立方体V11,V12,V21,V22はボクセルを表している。ここで、図１の[1]及び[2]の間では、点群PG1の座標は変化せず、（すなわち[1]及び[2]では点群PG1データは同一のものが示されており、）ボクセルを形成する基準となる基準点の座標のみが基準点R1及び基準点R2のように変化している。すなわち図１は、同一の点群PG1データを[1]及び[2]において互いに異なるボクセルで量子化する場合の例を示しており、互いに共通の座標系CS1で同一の点群PG1データを与え、[1]ではその原点Oに一致する位置に基準点R1を設けてボクセル量子化し、[2]では当該原点Oから+x方向に一定距離だけずれた位置に基準点R2を設けてボクセル量子化している。

そして、図１の[1]にてボクセル量子化された点群PG1データは2つのボクセルV11,V12の境界にまたがって概ね２等分される形で割り当てられているのに対して、図１の[2]のボクセル量子化では同じ点群PG1データの全てが２つのボクセルV21,V22のうち左側のボクセルV21のみに割り当てられている。結果として図１の[1]及び[2]の間で、同一の点群PG1データをモデル化しているのにも関わらず、そのモデル化の結果（例えば平均，分散共分散行列）は大きく変化してしまい、得られるモデルのパラメータも類似しないものとなってしまう。

なお、図１では模式例として、[1]及び[2]の間でボクセルのグリッド方向は共通だが基準点位置のみが異なる場合を示した。より一般的には、基準点位置及び／又はボクセルのグリッド方向が互いに異なるボクセル量子化において、基準点位置の相違のみに関して示した図１の模式例と同様に量子化誤りが発生することとなる。

以上のようなボクセル量子化誤りを低減する単純な手法としては、細かい解像度のボクセルを使用することが考えられる。すなわち、センサで計測する物体のスケールと比べて、十分に小さいボクセルを使用すれば、ほとんどの点に対してボクセル量子化誤りを回避できるためである。しかしながら、ボクセルの解像度が細かい場合、解像度が粗い場合に比べて、データ量が大きくなる。更に、それに伴って後段側の処理（自己位置推定処理）の計算量およびメモリ使用量も大きくなることが多い。ここで特に、ダウンサンプリングされた点群およびモデル化された点群データであっても、広範囲の計測から得られる点群に対するデータ量は膨大となることを考慮すると、このように単純にボクセル解像度を細かくする手法は好ましくない。

さらに、一般的にデータのダウンサンプル率と後段処理（自己位置推定処理）の精度とはトレードオフの関係にあり、仮に当該トレードオフ関係による後段処理の精度低下を許容してデータにダウンサンプリングを適用した場合であっても、多数の3D点で構成される地図点群のデータ量は依然として大きい。ここで、3D点の数自体が多いこと以外にも、非特許文献２の手法のモデル化によるボクセルごとのパラメータ数が多いことからも、当該データ量は依然として大きいこととなる。

以上のように、従来手法では主としてボクセル分割位置の相違による量子化誤差への対策ができないことにより、自己位置推定処理においてデータ量削減及び推定処理精度向上を同時に達成することが困難であった。

当該課題に鑑み、本発明は自己位置推定処理などにおいて必要となるリファレンス点群（地図点群）とクエリ点群（センサ点群）とのボクセルに基づくマッチングを、ボクセル分割位置の相違が存在しうる状況においても可能とする推定装置を提供することを目的とする。

上記目的を達成するため、本発明は推定装置であって、クエリ点群に対して、サブボクセル単位の相違を含んでボクセル分割位置が互いに異なる複数の分割手法を適用する第一分割部と、前記適用される複数の分割手法ごとに、ボクセル内のクエリ点群をモデル化したクエリ点群モデル化データを得る第一モデル化部と、リファレンス点群に所定のボクセル分割手法を適用し且つボクセル内のクエリ点群をモデル化して得られたリファレンス点群モデル化データと、前記一連の分割手法ごとのクエリ点群モデル化データとを比較し、類似していると判定される分割手法におけるクエリ点群モデル化データを得る比較部と、を備えることを特徴とする。また、コンピュータを前記推定装置として機能させるプログラムであることを特徴とする。

本発明によれば、自己位置推定処理などにおいて必要となるリファレンス点群（地図点群）とクエリ点群（センサ点群）とのボクセルに基づくマッチングが、ボクセル分割位置の相違が存在しうる状況においても可能となる。

データのボクセル量子化誤りを説明するための模式例を示す図である。一実施形態に係る推定装置及びモデル化装置の機能ブロック図である。ボクセル量子化誤差の低減及び初期位置姿勢の誤差の影響除外が実現されることを模式的に説明するための図である。一実施形態に係るモデル化装置及び推定装置の動作のフローチャートである。ボクセルとブロックとの関係の模式例を示す図である。最近傍ブロックに加えてその周囲のブロックも取得した状況の模式例を示す図である。近傍取得部が計算するベクトルを説明するための模式例を示す図である。地図側空間、当初設定の探索空間、更新された探索空間の模式例を示す図である。第一分割部の（手順３−２）の意義を説明するための模式例として、当該手順によって後段側の比較部で結果的に実現されることとなるサーチ処理の模式例を示す図である。

以下、図面を参照して本発明の各実施形態を説明する。なお、以下の説明において「特筆」として説明する各事項は、一定の実施形態において一定の観点から好ましい事項を述べるもの及び／又は説明上の注意喚起のためのものであり、必ずしも本発明の必須事項を述べたものではない。

図２は一実施形態に係る推定装置及びモデル化装置の機能ブロック図である。図示する通り、モデル化装置20は、第二取得部21、第二整列部22、第二分割部23、第二モデル化部24、第二量子化部25、出力部32及び記憶部33を備え、地図点群データに関して当該図示する順番で各データを処理する。また、推定装置10は第一取得部11、第一整列部12、第一分割部13、第一モデル化部14、第一量子化部15、第一生成部16を備え、センサ点群データに関して当該図示する順番で各データを処理する。さらに、推定装置10は第二生成部26、比較部17、推定部18、初期位置姿勢取得部31、近傍取得部34を備える。

以下ではまず、概略説明として、図２の各機能ブロック間のデータ授受の流れを説明する。各機能ブロックが当該データ授受する際の詳細処理内容は後述する。

なお、図２にて例えば第一取得部11及び第二取得部21のように、機能ブロック名が互いに名称先頭部分の「第一」及び「第二」でのみ異なるもの同士は、互いに同一又は対応する処理を異なるデータに対して行うものである。

例えば、第一取得部11はセンサ点群を取得して第一整列部12に出力し、第二取得部21は地図点群を取得して第二整列部22に出力する。

以下、説明の簡略化のために上記のように当該互いに同一又は対応する処理を、括弧「」で括ると共にスラッシュ記号「／」で並記するという記法で説明する。例えば上記の内容は以下の記法で表現される。

「第一取得部11／第二取得部12」は「センサ点群／地図点群」を取得して「第一整列部12／第二整列部22」へ出力する。

当該記法により、当該対応する各部12〜16及び各部22〜26の処理と、図２のその他の各部の処理とは、図２内にも描かれるように次の通りである。

「第一整列部12／第二整列部22」は、「センサ点群／地図点群」を所定方位に向けて整列させた「方位整列されたセンサ点群／方位整列された地図点群」を「第一分割部13／第二分割部23」へ出力する。

「第一分割部13／第二分割部23」は、「方位整列されたセンサ点群／方位整列された地図点群」に対してボクセル分割及びブロック定義を行って得られる「一連のボクセル及びブロックのもとでの方位整列されたセンサ点群／一連のボクセル及びブロックのもとでの方位整列された地図点群」を「第一モデル化部14／第二モデル化部24」へと出力する。ここで、第二分割部23におけるボクセル分割及びブロック定義は1通りのみの「所定の」ものであるのに対し、第一分割部13におけるボクセル分割及びブロック定義は複数通りの「一連の」ものであることを特筆する。さらに、第一分割部13における「一連のボクセル分割及びブロック定義」は、図１で説明したようなボクセル量子化誤差の影響を排除することを意図したものであることを特筆する。具体的な当該影響排除は、第一分割部13以降の機能ブロックによって実現される。

「第一モデル化部14／第二モデル化部24」は、「一連のボクセル及びブロックのもとでの方位整列されたセンサ点群／一連のボクセル及びブロックのもとでの方位整列された地図点群」に対してモデル化を適用して得られる「一連のモデル化センサデータ／モデル化地図データ」を「第一量子化部15／第二量子化部25」へと出力する。

「第一量子化部15／第二量子化部25」は、「一連のモデル化センサデータ／モデル化地図データ」を量子化して得られる「一連の量子化されたモデル化センサデータ／量子化されたモデル化地図データ」を「第一生成部16／出力部32」へ出力する。

出力部32は、量子化されたモデル化地図データを第二分割部23で定義したブロックごとにまとめたデータを記憶部33へと出力し、記憶部33では当該ブロックごとにまとめられたデータを記憶することで近傍取得部34からの参照に供する。ここで、出力部32でこのようにブロックごとにデータをまとめるのは、詳細を後述する推定装置10による自己位置推定処理において、最終結果としての精密な自己位置推定結果を得る前段階のいわゆる「粗い位置合わせ」に相当する結果を、当該ブロック単位で得ることを可能にするためのものであることを特筆する。

初期位置姿勢取得部31は、推定装置10の初期位置姿勢を取得して第一整列部12、近傍取得部34及び推定部18へと出力する。当該取得する初期位置姿勢は上記のいわゆる「粗い位置合わせ」を可能にするためのものであることを特筆する。近傍取得部34は、初期位置姿勢に対応する１つまたは複数のブロックのデータ（量子化モデル化地図データ）を、記憶部33を参照して取得したうえで、当該取得データを近傍の地図データとして第二生成部26へと出力する。近傍取得部34はまた、当該取得した近傍の地図データの位置と初期位置姿勢の位置との相違に相当するベクトルを算出して第一分割部13へと出力する。

「第一生成部16／第二生成部26」は、「第一量子化部15から得られる一連の量子化されたモデル化センサデータ／近傍取得部34から得られる近傍の地図データ」をデータ列表現したものとして「一連のセンサデータ列／地図データ列」を生成して比較部17へと出力する。当該データ列生成は比較部17における比較処理を可能とするための処理であることを特筆する。

比較部17は、上記出力されたデータ列を比較することにより、第一生成部16から得られた一連のセンサデータ列のうち、第二生成部26から得られた地図データ列に最も類似するのがどのセンサデータ列であるかの結果を最類似結果として推定部18へと出力する。推定部18は、当該最類似結果としてのセンサデータ列に対応する位置姿勢を、推定装置10の最終的な位置姿勢推定結果として出力する。

以上、図２の各部の処理概要を各部間でのデータ授受に関して説明した。ここで、比較部17で最類似結果として得られ推定部18で出力される位置姿勢推定結果においては、図１で説明したようなボクセル量子化誤差の影響が低減されており、且つ、初期位置姿勢取得部34で取得する初期位置姿勢の誤差をも見込んだうえで位置姿勢推定結果が得られていることを特筆する。ここで、初期位置姿勢の誤差を当該見込む処理は近傍取得部34において実現される。また、ボクセル量子化誤差を当該低減するためには第一分割部13で得た一連の（すなわち、一般に多数の）ボクセル分割結果に対応するセンサデータを、近傍取得部34で得られる近傍の地図データに対して比較する必要があるが、モデル化データをさらに量子化したうえで互いに同一サイズとなるデータ列の形式で当該比較を行うことにより、多数のセンサデータであっても高速に当該比較処理が可能となることを特筆する。

図３は、上記のボクセル量子化誤差の低減（及び初期位置姿勢の誤差の影響除外）が実現されることを模式的に説明するための図であり、空間分割して得られる3Dボクセルを模式的に2Dによって表現している。[1]には初期位置姿勢に対応する（ボクセル化のうえモデル化された）地図データMDの模式例が示されており、初期位置姿勢が全く誤差を含まず、且つ、ボクセル量子化誤差の影響その他も全く存在しない完全に理想的な状況であれば、推定装置10が取得する（ボクセル化のうえモデル化された）センサデータSDは当該地図データMDに完全一致するはずである。しかしながら、実際得られるセンサデータSDは[1]に示すように、地図データMDの付近にあるとはいえ、初期位置姿勢の誤差及びボクセル量子化誤差（図１で模式例を示したような、地図データとセンサデータにおけるボクセル分割位置のずれ）の影響を受けている。

そこで、推定装置10においては当該影響が存在することを前提に、初期位置姿勢の誤差及びボクセル量子化誤差によってセンサデータSDが本来の地図データMDから変動しうる想定範囲を網羅的に探索することで、本来の地図データMDに最も類似し、従って、高精度な位置姿勢推定結果に該当するものを見出すというアプローチを取る。図３の[2]では初期位置姿勢の誤差における姿勢（回転成分）の誤差を考慮しての探索範囲の模式例が、センサデータSDとは別手法で第一分割部13による分割がなされたセンサデータSD-R1,SD-R2,SD-R3として示されている。図３の[3]ではボクセル量子化誤差を考慮しての探索範囲の模式例が、[2]のセンサデータSD-R3とは別手法で第一分割部13による分割がなされたセンサデータSD-T1,SD-T2,SD-T3として示されている。（なお、ボクセル量子化誤差を考慮した探索範囲は実際には、サブボクセル単位の区切りで設定される。図３の[3]では図示のための便宜上の強調表現として、当該サブボクセル単位よりも大きな区切り変動をもってセンサデータSD-T1,SD-T2,SD-T3が描かれている。）

詳細は後述するように、実際には第一分割部13において、図３の[2]及び[3]のそれぞれの誤差影響を網羅的に組み合わせた一連のボクセル分割がなされる。（図３の例であれば、[2]の3通り×[3]の3通り＝合計9通りの分割がなされる。）また、初期位置姿勢の誤差における位置（並進成分）の誤差の考慮は実際には、第一分割部13による一連のボクセル分割によって考慮されるのではなく、地図データMDやセンサデータSDの範囲設定によって考慮される。すなわち、当該範囲設定は、並進成分の誤差をも見込んだものとして設定される。

また、第一分割部13で行う上記誤差影響を網羅的に組み合わせた一連のボクセル分割のそれぞれは、図３にも模式的に示されるように、様々な位置姿勢の変換パラメータに対応する分割となっている。推定装置10の推定部18から出力される位置姿勢も、比較部17によって最類似結果と判定されたボクセル分割に対応する位置姿勢となる。

図４は、一実施形態に係る図２のモデル化装置20及び推定装置10の動作のフローチャートである。ステップS1では、モデル化装置20において地図点群のモデル化が行われる。当該ステップS1は、モデル化装置20において第二取得部21から出力部32までの処理が行われることで、記憶部33にモデル化された地図データが記憶されるという形で実現される。

ステップS2では、推定装置10がステップS1で得られたモデル化地図内における自身の現在位置姿勢を推定する。当該推定するための入力データとして、第一取得部11において取得される推定装置10周囲のセンサ点群と、初期位置姿勢推定部31において取得される推定装置10の初期位置姿勢とが用いられ、推定装置10の各部の動作によってステップS2が実現される。ステップS2は所定レートのもとでリアルタイムに実施して、推定装置10の位置姿勢をリアルタイムで推定するようにしてもよい。この場合、位置姿勢が推定される各時刻においてステップS2が実行される。

なお、推定装置10は車両やドローンといったような任意の移動体に備わるものとして構成されて、当該移動体の各時刻における位置姿勢を推定するようにしてもよいし、携帯端末などのそれ自体は移動手段を持たない機器に備わるものとして構成されて、別途の移動手段によって移動する当該機器の各時刻の位置姿勢を推定するようにしてもよい。ステップS1においてモデル化する地図点群の範囲は、ステップS2で位置姿勢を推定することが想定される所定範囲とすればよい。

以下、図２の各部の動作の詳細を説明するが、概要説明の際に「第一取得部11／第二取得部12」のように同一又は対応する処理を担うものとして並記して説明した機能部に関しては適宜、当該並記する記法も用いて説明することとする。

＜第一取得部11及び第二取得部12＞
「第一取得部11／第二取得部12」では、「センサ点群／地図点群」を取得する。具体的な取得手法は、前述のLiDARなどの既存手法を利用できる。

＜初期位置姿勢取得部31＞
初期位置姿勢取得部31は、推定装置10の初期位置姿勢を取得する。当該取得される初期位置姿勢は、並進成分としての位置パラメータおよび回転成分としての回転パラメータを含む情報である。具体的には例えばGNSS/INS（全球測位衛星システム、慣性航法システム、あるいはこれらシステムの複合システム）や、一つ前の時刻のセンサ点群を用いた推定結果（すなわち推定部18による推定結果）、デッドレコニング、あるいは手動入力によって取得すればよい。他の方法を用いて取得してもよい。

＜第一整列部12及び第二整列部22＞
「センサ点群／地図点群」を構成している3D点群は一般的に(x,y,z)の直交座標系で表現されるものである。「第一整列部12／第二整列部22」では、与えられた点群（すなわち、「センサ点群／地図点群」）のローカル座標系を、指定された実世界の任意の所定方位と一致する様に変換する。例えば、点群を表現しているローカル座標系のx軸が緯度0°、y軸が経度0°に一致する様に指定しても良い。

ここで、地図点群のモデル化処理を担うモデル化装置20における第二整列部22と、自己位置推定処理を担う推定装置10における第一整列部12と、において同一の方位を指定することを特筆する。また、当該指定される方位を基準として後段側の「第一分割部13／第二分割部23」のボクセル分割が行われることを特筆する。第一分割部13では回転変換を施したうえでのボクセル分割も行うが、当該回転変換の基準となる方位を指定するのが第一整列部12であることを特筆する。

具体的に第一整列部12及び第二整列部22ではそれぞれ、当該方位指定のための変換パラメータを以下のように与えればよい。

地図点群のモデル化処理を担う側の第二整列部22においては、地図点群データを用意する管理者等によって変換パラメータを手動指定で与えても良いし、第二取得部21が地図点群を計測する際にGNSS/INSや地磁気センサ、ジャイロスコープを利用できる場合はそれらを用いても良いし、他の方法でも良い。

一方、自己位置推定処理を担う側の第一整列部12においては、リアルタイム推定の処理速度を確保しようとする場合、手動指定で与えることは難しい。そこで、第一取得部11でGNSS/INSや地磁気センサ、ジャイロスコープが利用可能であればそれらを用いても良いし、連続的に自己位置推定処理を行う場合に、一つ前の時刻における姿勢推定結果を初期位置姿勢として与え、その初期位置姿勢に含まれる回転パラメータを用いても良いし、デッドレコニングを用いても良いし、他の方法でも良い。

＜第二分割部23＞
第二分割部23は第一処理として、任意に設定可能な所定基準点（例えばローカル座標系の原点）を元に、方位指定された地図点群が分布している空間を所定サイズのボクセルで分割する。更に、x,y,z 方向に指定した個数のボクセルを包括する直方体で分割する。当該直方体をブロックと呼称する。この際、ボクセルおよびブロックの各辺は、それぞれローカル座標系の x 軸，y軸，z軸に沿うように整列されていることを特筆する。

ここで、ボクセルのx軸方向、y軸方向、z軸方向の長さをそれぞれvlx，vly，vlzとし、ブロック内に含まれるx軸方向、y軸方向、z軸方向のボクセルの個数をそれぞれvnx，vny，vnzとする。この時、ブロックのx軸方向、y軸方向、z軸方向の長さをぞれぞれ blx，bly，blzとすると，次式の関係が成り立つ；
blx = vlx * vnx (数式 1)
bly = vly * vny (数式 2)
blz = vlz * vnz (数式 3)

図５はボクセルとブロックとの関係の模式例を示す図であり，vlx=vly=vlz=1，vnx=vny=vnz=3の場合の１つのブロック及びこれに含まれるボクセルの例が示されている。図５においては、点線がボクセルを、実線がブロックを表しており、1つのブロックBが27個のボクセルを含むものとして設定される例が描かれている。

また、第二分割部23は第二処理として、ブロック内でのボクセルの位置を特定可能なIDを定義する。

例えば、ブロック内であるボクセルがx軸方向、y軸方向、z軸方向にそれぞれvoxel_idx番目、voxel_idy番目、voxel_idz番目に位置すると定義すると、図５の模式例におけるグレーで示すボクセルVは(voxel_idx, voxel_idy, voxel_idz)=(3,2,1)と表される。あるいは，例えば次の様な式を用いて単一のIDで表現することもできる；
voxel_id= voxel_idx+vnx*voxel_idy+(vnx*vny)*voxel_idz (数式 4)

さらに、第二分割部23は第三処理として、各ブロックの中心座標を計算する。当該計算する中心座標はローカル座標系において各ブロックの位置を与えているものであり、後述する近傍取得部34で近傍位置に該当すると考えられるブロックを取得する際に参照されるものであることを特筆する。

＜第一モデル化部14及び第二モデル化部24＞
「第一モデル化部14／第二モデル化部24」は、ボクセル内に含まれる点群を特定のモデルで表現する。

なお、第一モデル化部14では詳細を後述する第一分割部13で分割される一連の分割手法においてそれぞれボクセル内の点群モデル化を行い、第二モデル化部24では第二分割部24で分割された1通りのみのボクセルにおいてボクセル内の点群モデル化を行うことを特筆する。一連の分割手法によるボクセル分割データに関してそれぞれ処理を行なうことは後段側の第一量子化部15及び第一生成部16においても同様であるので、当該各部15,16の説明の際にはこの点についての重複説明は省略する。

ここで、ボクセル内にN個の点が含まれるものとして、p_i=(x_i, y_i, z_i) (i=1, 2, ..., N)と表記する。一実施形態においては、「第一モデル化部14／第二モデル化部24」によるモデル表現として、以下の(数式5)のように、ボクセル内の点群をその座標平均値（すなわち重心）で表現するようにしてよい。

この際に、ボクセル内に割り当てられた点が0個の場合は、モデルの計算を行うことができないため、モデル化処理をスキップする。あるいは、ボクセル内に割り当てられた点の数が極端に少ない場合は、実際には物体が存在せず、ノイズである可能性があるため、ボクセル内の点の個数の閾値をN_thとし，N< N_thの場合には処理をスキップする様にしても良い。なお、当該スキップされたボクセルに関してはモデルデータが存在しないものとして、後段側の各処理部によって扱われることとなる。

＜第一量子化部15及び第二量子化部25＞
「第一量子化部15／第二量子化部25」は、「第一モデル化部14／第二モデル化部24」で得られたモデルを量子化する。当該モデルが数式5のものである場合、具体的には例えば以下のように量子化すればよい。

ここで、「第一分割部13／第二分割部23」で定義された基準点（ボクセル分割の基準点）の座標をrp=(rpx, rpy, rpz)とする。数式5におけるmeanから当該rpを減算した際の各成分をそれぞれに対応するボクセルの長さvlx, vly, vlzで除算した際の剰余をそれぞれvsx, vsy, vszとする。vsx, vsy, vszの値が負となった場合、それぞれvlx, vly, vlzを加算する。このような計算で得られたvsx, vsy, vszは、ボクセル内での全ての座標が最小となるコーナーを原点として設定した場合のmeanの座標に相当し、当該meanの座標をボクセル内の相対位置として表現したものに相当する。従って、常に0≦vsx＜vlx、0≦vsy＜vly、0≦vsz＜vlzが成立する。

次に、[0, vlx), [0, vly), [0, vlz)の範囲をそれぞれvqx個、vqy個、vqz個の等間隔の範囲に区切る。そして、vsx, vsy, vszをそれぞれvlx，vly，vlzで除算した際の商（一般に0〜1の範囲にある実数）に、vqx，vqy，vqzを乗算した値を整数値に丸めた値を，vlx，vly，vlz が割り当てられるIDとし，それらを mean_idx，mean_idy，mean_idzとして量子化結果を得る；
mean_idx = vsx / vlx * vqx (mean_idx =0, 1,…, vqx) (数式 6)
mean_idy = vsy / vly * vqy (mean_idy =0, 1,…, vqy) (数式 7)
mean_idz = vsz / vlz * vqz (mean_idz =0, 1,…, vqz) (数式 8)

あるいは、例えば次の様な式を用いて単一のIDで表現した量子化結果を得てもよい。
mean_id = mean_idx + vqx * mean_idy + (vqx * vqy) * mean_idz (数式 9)

＜出力部32＞
出力部32は、与えられた地図点群をモデル化した結果（第二量子化部25の出力データ）を、ブロック単位（第二分割部23で定義したブロック単位）でまとめたデータを出力する。各ブロックが含むデータは、ブロックの中心座標、ボクセルごとのボクセルID(voxel_idx, voxel_idy, voxel_idz)、ボクセルごとの平均ID(mean_idx，mean_idy，mean_idz)である。ここで、第二モデル化部42計算部において、モデル計算をスキップしたボクセルの情報は含まないことを特筆する。

前述の通り、出力部32の上記出力したブロック単位のデータは記憶部32に記憶され、近傍取得部34からの参照に供されることとなる。

＜近傍取得部34＞
近傍取得部34は第一処理として、初期位置姿勢取得部31が取得した初期位置姿勢における座標位置と、記憶部33に記憶されている各ブロックの中心座標を比較した際に、最近傍となるブロックの情報を記憶部33から取得する。

なお、最近傍のブロックを取得した後に、それに加えてその周囲のブロックを合わせて取得しても良い。例えば、最近傍ブロックとその上下左右斜め方向に位置する26個のブロックを選んで取得しても良いし、他の選び方でも良い。

ここで、選択されたブロックの集合の大きさは，初期位置姿勢の誤差量を考慮した上で、第一取得部11の点群を取得したセンサの測定範囲内に収まるものであることが望ましい。例えば、センサ側の測定範囲が半径100[m]である場合に、記憶部33に記憶されている地図側データから1000[m]×1000[m]×1000[m]の大きさとなるブロックの集合を選択したとする。この時、地図側とセンサ側で同一サイズの空間内のデータの比較を行うとすると、センサ側のほとんどはデータの存在しない空間となってしまうためである。（なお、具体的な比較は後段側の比較部17においてなされる。）

近傍取得部34では、最近傍ブロックに加えてその周囲のブロックも取得した場合の追加処理として、取得したブロック集合を一つのブロックとみなし、各ブロックの含むボクセルIDを更新する。具体的な更新処理は例えば、当該ブロック集合を1ブロックとみなして扱った場合に第二分割部23が付与するIDと同様のものを更新して付与さればよい。図６は、最近傍のブロックと、xy平面上での周囲8ブロックを取得したシーンを上方から（z軸の正方向）から見たシーンを模式的に表現している。ここでは、四角形はブロックを表しており、各ブロックには区別の為に番号が振られている。ここでは，5番のブロックが初期位置姿勢の最近傍ブロックに相当する。例えばこれらの9つのブロックを一つのブロックとみなして，前述の図５の説明例のように定義されているボクセルID(voxel_idx, voxel_idy)等を更新する場合であれば、以下のように更新すればよい；
・1番のブロック：更新無し
・2番のブロック：全てのvoxel_idxにvnxを加算
・3番のブロック：全てのvoxel_idxに2*vnxを加算
・4番のブロック：全てのvoxel_idyにvnyを加算
・5番のブロック：全てのvoxel_idxにvnxを，全てのvoxel_idyにvnyを加算
・6番のブロック：全てのvoxel_idxに2*vnxを，全てのvoxel_idyにvnyを加算
・7番のブロック：全てのvoxel_idyに2*vnyを加算
・8番のブロック：全てのvoxel_idxにvnxを，全てのvoxel_idyに2*vnyを加算
・9番のブロック：全てのvoxel_idxに2*vnxを，全てのvoxel_idyに2*vnyを加算
なお、この例ではz軸方向にブロックを追加していないが、追加する場合も同様にvoxel_idzを更新することができる。

また、近傍取得部34は第二処理として、図７に模式例を示すような，初期位置姿勢の位置パラメータを始点とし最近傍ブロック中心（MBC_NNと呼ぶ。）を終点とするベクトルdを計算する。図２に示すように、当該計算したベクトルdは第一分割部13で利用される。図７は、地図データ内容の模式例としての十字路を上方（z軸の正方向）から見たシーンを模式的に表現している。黒い実線は当該十字路を構成する道路を、黒い四角形の点は初期位置姿勢の位置IPを表現している。太線ブロックはモデル化された地図データにおける最近傍のブロックBを、黒い円の点はブロックの中心MBC_NNを、黒い点線は当該ブロックBを構成しているボクセルを意味するものとする。

なお、図７における「みなしSBC_NN」とは次の第一分割部13の説明のためのものである。

＜第一分割部13＞
第一分割部13は、（手順１）第一整列部12によって方向が定義されたセンサ点群に様々な姿勢変換を与えた上で、（手順２）初期位置姿勢の位置にベクトルd（前述の通り、近傍取得部34から取得される）を加えた座標をセンサ側の最近傍ブロック中心点（SBC_NNと呼ぶ。）とみなし、（手順３）空間をボクセルで分割する。当該みなされる最近傍ブロック中心点SBC_NNの模式例は図７にも示されている通りである。

この際、点群を実際に変換するのは（手順１）の回転変換のみであり、（手順２）の並進変換は仮想的な変換であることを特筆する。すなわち、（手順２）の並進変換は（手順３）のボクセル分割の基準位置として参照するためのものである。なお、姿勢変換を施すのはセンサ側（第一分割部13）のみであり、対応する機能部としての第二分割部23において、地図側には姿勢変換を施さないことを特筆する。

（手順３）に関して、センサ点群に対するボクセルの解像度は、地図点群を分割したボクセルの解像度と一致させる。すなわち、第一分割部13で分割するボクセルサイズは第二分割部23で地図データに関して分割したボクセルサイズと同一のものとする。また、ボクセルで分割する空間は，近傍取得部34で取得された単一ブロック又はブロック集合（なお、ブロック集合の場合も図６の模式例で説明したようにデータ形式上は単一ブロックと同じ形式に変換されることを特筆する。）と同一サイズの空間（S_mと呼ぶ。）に、任意の探索範囲を加えた範囲（S_sと呼ぶ。）に限定する。

図８は、当該（手順３）における空間S_mや範囲S_sなどの模式例を図７の例に対応するものとして示す図である。図７のブロックBが図８の空間S_mに対応している。

以下、（手順１）及び（手順３）に関して、その意義は図３を参照して概説した通りであるが、どのような考察に基づきどのような具体的処理を行うかを説明する。

＜（手順３）の考察及び具体的処理＞
初期位置姿勢取得部31から得られた初期位置姿勢が誤差を含まない理想的な状況においては、地図データ位置としての最近傍ブロック中心MBC_NNと、（手順２）でこれにみなされたセンサ側の最近傍ブロック中心点SBC_NNとは一致する。従って当該一致する理想的な状況下で、地図側空間S_mと同サイズの空間をセンサ側の実世界データにおいて取得した場合、その空間が含む実世界の領域は、地図側空間S_mのそれと一致する。従って、センサ側データにおいて仮にこの空間を後段側の第一モデル化部14でモデル化した場合、得られるモデルは地図側空間S_mのそれ（第二モデル化部24のモデル化結果）と類似する可能性が高い。

しかしながら、現実的には、初期位置姿勢は誤差を含む。例えばGPSは数mから数十mの誤差を含む可能性があると言われている。従って、例えば初期位置姿勢には最大5mの誤差が含まれると仮定する場合、地図側空間S_mのサイズに±5mを加えた空間を探索しなければ、地図側空間S_mが含んでいる実世界の領域と同じ領域を見つけることはできない。

そのため、想定する初期位置姿勢の最大誤差をE_maxとする場合、地図側空間S_mに対応していると考えられる実世界の同サイズ領域の両端を最大誤差E_maxだけ拡張した空間を探索空間とし、これを探索空間S_sと呼ぶ。最大誤差E_maxは固定値を与えても良いし、例えばGPSの測位精度の様な初期位置姿勢の誤差の大きさに関する情報が取得できる場合は、最大誤差E_maxを各時刻における値として動的に変更しても良い。なお、例えばx軸方向およびy軸方向にのみ拡張した探索空間S_sを設定し、z軸方向には拡張しない、という様にしても良い。

ここでさらに、具体的な探索処理をボクセルサイズ単位の移動ステップ幅で行うようにする便宜上、探索空間S_s内の任意のボクセルのコーナー点に、それぞれvlx，vly，vlzの整数倍を加えた位置（一連の格子点のような位置）を頂点として定義される一連のボクセル直方体を考慮した際に、探索空間S_sを包括する最小の空間を形成しているボクセル直方体を計算し、それを新たな（さらに更新されボクセルグリッド位置に一致するようにされた）探索空間S_vsとする。そして、探索空間S_vsの中で最もx成分，y成分，およびz成分が小さくなる点を、この探索空間をボクセルで分割する際の基準点とし、基準点srpと呼ぶ。なお、基準点srpの設定はこれに限らず、探索空間S_vs内の任意の所定点を設定することができ、更新する前の探索空間S_s内の任意のボクセルのコーナー点をそのまま探索空間S_vsにおける基準点srpとしてもよい。

以上の地図側空間S_m（実世界であるセンサ側空間においては初期位置姿勢に誤差がない仮定で対応する地図側空間S_mに一致すると考えられる領域）、当初設定の探索空間S_s、更新された探索空間S_vsの模式例は図８に示される通りである。ここで、当初の探索空間S_sの境界部分は点線で示すボクセル境界とは必ずしも一致していないのに対し、更新された探索空間S_vsはその境界がボクセル境界（図中、点線で示される）と一致していることを特筆する。

以上の考察から、（手順３）では具体的に、上記の更新された探索空間S_vsにおいて上記設定された基準点srpに基づき、ボクセル分割を行う。当該ボクセル分割は図１で説明したような量子化誤差影響の低減のために、基準点srpをもとにさらにサブボクセル長さで(x,y,z)方向それぞれに移動した一連の位置を基準とした一連のボクセル分割を行うが、その詳細は後述する。

＜（手順１）の考察及び具体的処理＞
初期位置姿勢は、回転パラメータに誤差を含んでいる可能性もある。センサ点群は、初期位置姿勢を元に第一整列部12で回転変換されることで所定方向が設定されている、すなわち(x,y,z)軸が定義されているが、初期位置姿勢の回転パラメータに誤差がある場合、たとえ位置パラメータに誤差が含まれないとしても地図側空間S_mが含んでいる実世界の領域と同じ領域を見つけることは出来ない。なぜならば図３の[1]に模式例を示すように、地図側データとセンサ側データとで実際の方向が異なる形でボクセル分割してしまうと、得られるモデルデータも異なるからである。

従って、図３の[1]のような方向が異なる状況を解消するものを見出すべく、（手順１）では具体的に次のような処理を行なう。

すなわち、（手順１）ではセンサ点群に特定パターンの回転を施した点群を生成し、これらがさらに（手順２）及び（手順３）で処理されるものとする。この時，第一整列部12で回転変換されたセンサ点群をSPC₀とし、（手順１）でr_size通りの変換が施され、生成された点群をそれぞれSPC_i (i=1,2,…,r_size)とする。ここで、センサ点群に回転処理を施す際は、センサ点群のローカル座標系の原点（すなわち、センサ位置）を中心に回転させる、ということを特筆する。当該回転させた後に（手順２）の並進移動が適用される。

＜（手順３）のボクセル分割の詳細＞
（手順３）のボクセル分割は、（手順１）で得られた一連のSPC_i (i=0,1,2,…,r_size) のそれぞれに関して同様に実施されるものであり、具体的には以下の（手順３−１）及び（手順３−２）の通りである。

（手順３−１）
対象となるセンサ点群SPC_iに対して、その基準点srpを基準に、更新された探索空間S_vsをボクセルで分割する。ここでは，ボクセル量子化誤りを軽減するために、（すなわち、低減された結果を後段側の比較部17において見出すことを可能にするために、）ボクセルの解像度よりも細かいサイズのステップ幅による平行移動を基準点srp及び更新された探索空間S_vsに与えた上で（すなわち、探索空間S_vsにおいて当該ステップ幅だけ基準点srpからずれた位置をボクセル分割位置として設定したうえで）、更新された探索空間S_vsをボクセルで分割する。

具体的には、x軸方向，y軸方向，z軸方向へのステップ幅をそれぞれrsx，rsy，rszとおく場合、それぞれでvlx，vly，vlzを除算した際の商の大きさのパターン数rsnx，rsny，rsnzで，更新された探索空間S_vsをボクセルで分割する；
rsnx = vlx / rsx （数式10）
rsny = vly / rsy （数式11）
rsnz = vlz / rsz （数式12）

例えば，vlx=2[m]，rsx=0.2[m]とすると、無変換（ステップ幅の平行移動を与えない場合）が1パターン、rsx刻みのX軸方向への平行移動を与える9パターンの計10パターンで、更新された探索空間S_vsをボクセルで分割する。Y軸方向にも同様のパターンを与えてx軸方向と組み合わせた場合、計100パターンとなる。Z軸方向にも同様の処理を行って組み合わせた場合、計1000パターンとなる。この例の場合、上記のパターンの中に、地図側のボクセル量子化のための基準点との差が0.2[m]以内になるものが存在することとなる。この時のパターン数rsnx*rsny*rsnzを、基準点ステップ数rs_sizeと呼ぶこととする。

（手順３−２）
まず、当該（手順３−２）の目的を述べる。当該目的はすなわち、図９に模式的に示すように、後段側の比較部17において結果的に実現されることとなる処理である、センサ側点群に相当する更新された探索空間S_vs（当該探索空間S_vsは（手順３−１）のボクセル分割の仕方と（手順１）で適用された回転変換の仕方との組み合わせ総数だけそれぞれ存在する）の内部を、地図側空間S_mを移動させながらサーチして、最も一致する箇所を見つけるという処理を、具体的に「移動させながらサーチ」する必要なく可能とするためである。このため（手順３−２）では、3次元ステップ幅として指定される当該サーチの際の移動量(Δx, Δy, Δz)に紐づける形で、探索空間S_vs内の一連の探索対象ブロック（地図側空間S_mと同サイズ）を予め用意しておくという処理を行なう。

すなわち、図９では探索空間S_vs内を地図側空間S_mでサーチする際のサーチ位置の２つの例が位置P1,P2として示されている。位置P1では探索空間S_vs内の部分データとしてブロックB1のデータがあって地図側空間S_mデータとの類似度が求められ、位置P2では探索空間S_vs内の部分データとしてブロックB2のデータがあって地図側空間S_mデータとの類似度が求められることとなる。（手順３−２）ではこのようなブロックB1,B2等を、それぞれ探索空間S_vs内の位置P1,P2等にある旨の情報を紐づけたうえで個別に抽出する処理が行われることとなる。

なお、本発明の一実施形態においては図９に当該模式的に示すように、センサ側の点群データが探索空間（探索「される」空間）S_vsを形成し、地図側点群データに基づく地図側空間S_mは探索「する」側のものであることを特筆する。（一般に広大となりうる地図点群データ全体内からの局所領域としての地図側空間S_mの「探索」は、近傍取得部34において初期位置姿勢を用いて既に完了していることを特筆する。）

特に、詳細を後述する比較部17では地図側データとセンサ側データとで同サイズのデータ列を比較して類似度を求めるという処理を行なうが、センサ側データにおいて当該同サイズとなるデータ列を算出可能となるような同サイズのブロックを、図９に模式的に示すようなそれぞれの移動量に応じたものとして用意しておくことを可能にするのが当該（手順３−２）である。

従って具体的に（手順３−２）では、各ボクセルに、ブロック内でのボクセル位置を示すvoxel_idを、図９で模式的に示したようなサーチ位置の違いを反映したものとして与える。図９も示すように更新された探索空間S_vsは地図側空間S_mとサイズが異なるため、更新された探索空間S_vsを単一のブロックとみなすと、地図側空間S_mの位置関係と一致しないIDが計算されてしまう。そこで、地図側空間S_mと同サイズの仮想的なブロック（図９にてサーチしているブロック）を当てはめ、その仮想的なブロックごとにボクセル位置を示すIDを各ボクセルに与える。（従って、同一ボクセルであっても異なる仮想ブロックから異なる相対的位置としてIDが付与されうる。このことは、仮想的なブロック間でのサーチ範囲の重複に相当する。）こうして、地図側空間S_mが更新された探索空間S_vs内を移動してカバーしうる全ての範囲を網羅する様に、仮想的なブロックにそれぞれvlx，vly，vlz刻み（ボクセルサイズの幅刻み）の平行移動を施し、その仮想的なブロック内のボクセルにvoxel_idを与える。すなわち、平行移動によるサーチはボクセルサイズ単位の移動で行うように設定する。また、各ブロックの中心座標も計算する。

ここで、x軸方向，y軸方向，z軸方向のブロックの位置で指定される仮想的なブロックのパターン数は、図９に示すようなサーチのパターン数に一致することなる。具体的には、各方向に対する更新された探索空間S_vsの大きさから地図側空間S_mの大きさを減算した値を、それぞれvlx，vly，vlzで除算した際の商に1を加えた値となる。例えば、更新された探索空間S_vsが1辺が10[m]の立方体であり、地図側空間S_mが1辺が5[m]の立方体であり、vlx=vly=vlz=1[m]とすると、更新された探索空間S_vsの全ての範囲を網羅するためのx軸方向，y軸方向，z軸方向のブロックの位置のパターン数はそれぞれ6通りとなる。この場合，計216パターンの仮想的なブロックの情報が得られる。この様な合計パターン数を、ボクセルIDシフティング数vs_sizeと呼ぶこととする。

なお、上記におけるx軸方向，y軸方向，z軸方向のブロックの位置の指定は、ベクトルdの位置、すなわち探索空間S_vsの中心ブロック位置（図８にも示したように拡張を施す前の、誤差等のない理想状況での地図側空間S_mのブロック位置）からの差分として指定しておくことで、当該差分を後段側の推定部18による自己位置姿勢推定の際の位置姿勢の補正のための情報（並進パラメータのうちの一部分の情報）として利用できることを特筆する。

＜以上の（手順１）〜（手順３）により第一分割部13の分割処理のまとめ＞
以上のように網羅的に分割する（手順１）〜（手順３）によって、最終的に、第一分割部13では、与えられたセンサ点群に対して、{(r_size + 1) * rs_size * vs_size}通りのブロック分割結果を得ることとなる。また、当該得られた各ブロックに、自身に対応する変換IDを紐づける。当該変換IDは特に、適用された（手順１）及び（手順３）での変換の仕方の詳細が紐付けられたものとなる。

なお、（手順３−１）による変換とはサブボクセル単位での分割位置の相違（ステップ幅）に対応するものであり、無変換の場合の基準点srpからの当該分割位置の相違の分の並進変換として、無変換の場合のボクセルに最小絶対値の移動幅でボクセルを合わせるようにする際の並進変換として紐付けられることを特筆する。従って、当該並進変換の各成分はボクセルの各辺のサイズの1/2以下の大きさで構成されることを特筆する。

ここで、無変換の場合の基準点srpに関しては、図８に示すような探索空間S_vs及びその中心ブロックとしての地図側空間S_m（に対応していると考えられるブロック）で共通のボクセル頂点位置として設定してよい。すなわち、図８に示す状況でボクセル分割する場合がステップ幅による平行移動がゼロの場合（無変換の場合）としてステップ幅を指定することで、上記の並進変換の情報を得るようにしてよい。

＜第一生成部16及び第二生成部26＞
「第一生成部16／第二生成部26」は第一処理として、与えられたブロック内のボクセルIDと平均IDのペアの集合を元に、データ列を生成する。初めに、ブロックが含む可能性のある最大のボクセル数vnを求める。近傍取得部34で最近傍ブロックのみを取得した場合は、次式で求められる；
vn = vnx * vny * vnz （数式13）
近傍取得部34で周囲のブロックと合わせて取得した場合も同様に、それらを一つのブロックとみなした場合の、ブロックが含む可能性のある最大のボクセル数vnを計算する。なお、最大のボクセル数vnに至らないブロックは、ブロックサイズが小さいのではなく、ボクセル内の点が少ない又は存在しないことにより「第一モデル化部14／第二モデル化部24」でモデル化処理がスキップされたボクセルを含むものであることを特筆する。

その後、「第一生成部16／第二生成部26」は第二処理として、大きさvnの1次元配列を生成し、平均IDの範囲外の数値、例えば-1で初期化してから、ブロック内に含まれるボクセルIDに対応する次元に、そのボクセルから計算された平均IDを代入することでデータ列を得る。ここでは、モデルが計算されなかったボクセルに対応する次元には、平均IDが代入されないままとなる。なお、ボクセルIDの設定の仕方より明らかなように、ブロック内に含まれるボクセルIDに対応する次元とはすなわち、ブロック内のボクセル位置に応じた当該1次元配列の要素（当該要素において代入された平均ID）である。２つの異なるブロックの1次元配列同士を参照して、同じ次元（同位置の要素）の平均ID同士が類似していることは、当該２つのブロック内の同じ位置にある点群モデル化データ同士が類似していることを意味している。従って、２つのブロックの1次元配列同士が類似していることは当該２つのブロックに含まれる点群の全体的な配置が類似していることを意味している。当該類似度は後述する比較部17においてスコアとして数値化される。

また、センサ側のデータ列を得た第二生成部26においてはさらに、当該データ列自身に対応する変換ID（第一分割部13で最後に紐づけた変換ID）を紐づける。

＜比較部17＞
比較部17は、第二生成部26から得られる地図側の単一のデータ列と、第一生成部16から得られるセンサ側の{(r_size + 1) * rs_size* vs_size}通りのデータ列群の間で類似度を計算する。ここで、センサ側のデータ列には，それぞれに対応する変換IDが紐づけられていることを特筆する。

具体的に、データ列の次元ごとに、含まれる要素、すなわち平均IDの値が一致するかを判定し、是であればセンサ側のデータ列に紐づけられた変換IDに対してスコアを加算する。ここで、両方の要素が、モデルが計算されなかったボクセルであることを意味する(-1)である場合、スコアを加算する様にしても良いし、加算しない様にしても良い。計算の効率化のため，地図側の要素が(-1)となる次元については，処理をスキップする様にしても良い。

最後に、類似度スコアの大きい順に変換IDをソートする。計算の効率化のために，上位k件の変換IDのみを取得する様にしても良い。

＜推定部18＞
推定部18は、初期位置姿勢取得部31で得られた初期位置姿勢と、比較部17において最大の類似度を得た変換IDと、を入力とし、センサの自己位置姿勢を計算する。すなわち、第一取得部11の取得したセンサデータを計測したセンサの自己位置姿勢を計算する。

当該計算においては、入力された変換IDに対応する位置姿勢変換パラメータ、すなわち並進パラメータおよび回転パラメータを取得し、それらで構成される一つの姿勢変換行列を計算する。ここで、回転パラメータとは当該変換IDに対応する第一分割部13での（手順１）で適用した回転変換に対応するものである。また、並進パラメータとは当該変換IDに対応する第一分割部13での、（手順３−１）の無変換の場合の基準点srpからの分割位置相違分を反映した並進変換（前述した各成分がボクセルの各辺のサイズの1/2以下の大きさで構成されることとなる並進変換）と、（手順３−２）のボクセル単位でのx軸方向，y軸方向，z軸方向の移動量（前述した探索空間S_vsの中心ブロック位置S_mからの移動量）で与えられる並進変換と、を合成した並進変換に対応するものである。そして、その位置姿勢変換行列を初期位置姿勢にかけた結果を、自己位置姿勢として出力する。なお、当該位置姿勢変換行列の乗算は、誤差を含んだ初期位置姿勢を補正することに相当する。また、当該位置姿勢変換行列の算出においては、第一分割部13での手順と同様に、最初に回転変換を施したうえで次に並進変換を施すものとして、当該行列を算出すればよい。

以上、本発明によれば、推定装置10の位置姿勢をボクセルベースのモデル化に基づき高速に求めることができる。さらに、当該高速に求められた結果は、当該ボクセルベースのモデル化の際の量子化誤差の影響や初期位置姿勢の誤差の影響も除外したものとして得ることができる。

以下、本発明における説明上の各種の補足的事項（別実施形態などを含む）を説明する。

（１）「第一モデル化部14／第二モデル化部24」では、上記説明したような平均位置座標mean以外にも、任意の所定量を、ボクセル内の点群をモデル化するものとして採用することができる。モデルは単一種類でもよいし、複数種類を組み合わせたものであってもよい。

例えばモデルの１つとして、標準偏差を用いてもよい。3D点群の標準偏差σは次式で計算することができる；

その後、σを自身のL2ノルムを用いて正規化することによって，各成分の取りうる値の範囲は[0, 1]となる。従って、各成分を等間隔の範囲に区切ることによって，平均と同様の方法で量子化することができる。

また、センサ取得された3D点群がその対応画素における輝度情報を持っている場合、モデルの一つとして輝度平均を用いても良い。センサで取得された輝度値は通常，特定の範囲、例えば[0,255]に正規化されているため、平均と同様の方法で量子化することができる。

また、センサ取得された3D点群がその対応画素における色情報を持っている場合、モデルの一つとして色平均を用いても良い。センサで取得された色情報は通常，RGBの様なチャネルごとに特定の範囲、例えば[0,255]に正規化されているため，平均と同様の方法で量子化することができる。

（２）「第一量子化部15／第二量子化部25」では、「第一モデル化部14／第二モデル化部24」で得たモデル種類に応じた所定の量子化を行うようにすればよい。

（３）「第一生成部16／第二生成部26」において、複数のモデルに基づくIDの組み合わせは、それらを表現する一意のIDで表現することができ、当該IDをデータ列生成の際に生成されるデータ列の要素として利用することで、平均座標meanのみの単一モデルの場合と同様の処理を行なうことが可能となる。すなわち、前述したブロック内に含まれるボクセルIDに対応する次元に、そのボクセルから計算された平均IDを代入する処理において、当該平均IDを複数のモデルに基づくものとして計算し代入するようにすればよい。この際、複数のモデルに基づく多次元構成された平均IDをボクセルIDに対応する次元に代入するようにしてもよい。

（４）また，本発明の一実施形態においては，地図データの持つ量子化されたモデルパラメータを逆量子化することによって連続量を取得し、データ生成処理で利用しても良い。この際、センサ側はモデルパラメータを量子化せず、連続量を用いる。そして、データ列の比較処理では，対応する連続量間の距離（例えば誤差二乗）を非類似度として累積加算し，最も非類似度の小さいデータ列を選択する様にしても良い。

すなわち、本発明は「第一量子化部15／第二量子化部25」をスキップする変形例も可能である。

（５）また、本発明の一実施形態においては，自己位置推定処理における分割処理からデータ列比較処理までの処理を、粗密戦略に従って反復的に行っても良い。すなわち、1度目の処理では第一分割部13で粗い回転や粗いステップ幅を用いて大まかな自己位置推定結果を取得し，それを元に探索空間S_svを縮小する。その後，2度目の処理における第一分割部13で、より細かい回転や細かいステップ幅を用いることによって、高精度な自己位置推定結果を得る、という様にしても良い。これにより、1度の処理で高精度な自己位置推定を行う場合に比べて、総合的な計算量を削減できる可能性がある。

（６）本発明は自己位置推定処理への適用を例として説明してきたが、具体的なセンサ点群に限定されないクエリ点群（ボクセル分割位置が未定のもの）と、地図点群に限定されないリファレンス点群（既に所定のボクセル分割位置でボクセル分割され記憶部33に記憶されているもの）とを対象としての、当該両点群の間でのボクセル分割位置の不定性を吸収したマッチングを実現するというモジュール的な適用も可能である。当該モジュール的な適用を行う場合は、初期位置姿勢に相当する情報などは予め与えておいて、クエリ点群及びリファレンス点群を用意して、以上と同様の処理を行なえばよい。特に、第一分割部13による網羅的な分割処理は、一般のリファレンス点群のボクセル分割に対して不定性を有するクエリ点群のボクセル分割において量子化誤差などを低減したボクセル分割を自動検知する効果を有する。

当該モジュール的に適用する場合は、自己位置推定処理における実施形態を次のように簡略化して適用すればよい。すなわち、第一取得部11の取得する初期位置姿勢に所定の精度（充分と判断される精度）があることが既知で、第一整列部12で整列されたセンサ点群に基づくデータと近傍取得部34で取得された地図点群データとの比較においては、ボクセル分割位置の相違にのみ対処する必要がある場合の簡略化された自己位置推定処理の実施形態に相当する。すなわち、第一分割部13における（手順１）では複数の回転変換の適用を省略して第一整列部12で回転変換された1通りのセンサ点群をSPC₀のみを用い、（手順３）においては（手順３−１）の基準点ステップ数rs_size通りの分割のみを行い、（手順３−２）のボクセルIDシフティング数vs_size通りの分割は省略することで、全体として（手順３−１）の基準点ステップ数rs_size通りのブロック分割を行うようにすればよい。

全く同様に、本発明の具体的な適用対象が自己位置推定であるか否かを問わず、初期位置姿勢の並進成分に所定精度があることが既知の場合であれば、（手順３−２）を省略することによって第一分割部13では（手順１）による(r_size + 1)通りと（手順３−１）による基準点ステップ数rs_size通りによる全体で(r_size + 1)* rs_size通りのブロック分割を行うようにしてよく、初期位置姿勢の回転成分に所定精度があることが既知の場合であれば、（手順１）を省略することによって第一分割部13では（手順３−１）による基準点ステップ数rs_size通りと（手順３−２）のボクセルIDシフティング数vs_size通りによる全体でrs_size* vs_size通りのブロック分割を行うようにしてもよい。

なお、以上のような各観点から簡略化する各実施形態のうち、（手順３−２）のボクセルIDシフティング数vs_size通りの分割を省略する実施形態では、図８で説明した空間Ssや空間Svsへの拡張は不要となることを特筆する。ただし、空間S_m（のセンサ側データに対応するもの）に（手順３−１）の基準点ステップ数rs_size通りの分割を適用するために、空間S_mの外側所定範囲（ボクセル幅以内の所定範囲）は参照することとなる。

（７）推定装置10及びモデル化装置20は一般的な構成のコンピュータとして実現可能である。すなわち、CPU（中央演算装置）、当該CPUにワークエリアを提供する主記憶装置、ハードディスクやSSDその他で構成可能な補助記憶装置、キーボード、マウス、タッチパネルその他といったユーザからの入力を受け取る入力インタフェース、ネットワークに接続して通信を行うための通信インタフェース、表示を行うディスプレイ、カメラ及びこれらを接続するバスを備えるような、一般的なコンピュータによって推定装置10及びモデル化装置20を構成することができる。さらに、図２に示す推定装置10及びモデル化装置20の各部の処理はそれぞれ、当該処理を実行させるプログラムを読み込んで実行するCPUによって実現することができるが、任意の一部の処理を別途の専用回路等（GPUを含む）において実現するようにしてもよい。

10…推定装置、11…第一取得部、12…第一整列部、13…第一分割部、14…第一モデル化部、15…第一量子化部、16…第一生成部、17…比較部、18…推定部、26…第二生成部、31…初期位置姿勢取得部、34…近傍取得部
20…モデル化装置、21…第一取得部、22…第二整列部、23…第二分割部、24…第二モデル化部、25…第二量子化部、32…出力部、33…記憶部

Claims

クエリ点群に対して、サブボクセル単位でボクセル分割位置が互いに異なる複数の分割手法を適用する第一分割部と、
前記適用される複数の分割手法ごとに、ボクセル内のクエリ点群をモデル化したクエリ点群モデル化データを得る第一モデル化部と、
リファレンス点群に所定のボクセル分割手法を適用し且つボクセル内のリファレンス点群をモデル化して得られたリファレンス点群モデル化データと、前記複数の分割手法ごとのクエリ点群モデル化データとを比較し、類似していると判定される分割手法におけるクエリ点群モデル化データを得る比較部と、を備えることを特徴とする推定装置。
前記第一分割部ではさらに、前記クエリ点群に複数の回転変換を施し、当該施された回転変換ごとのクエリ点群に対して、前記サブボクセル単位でボクセル分割位置が互いに異なる複数の分割手法を適用することを特徴とする請求項１に記載の推定装置。
前記リファレンス点群モデル化データは、ボクセルが集合した所定サイズのリファレンスブロックに対して得られており、
前記第一モデル化部は、前記所定サイズよりも大きなサイズでボクセルが集合したクエリブロックに対して前記クエリ点群モデル化データを得ており、
前記第一分割部ではさらに、前記クエリブロック内の所定位置範囲であって前記リファレンスブロックのサイズを占める位置範囲に並進移動変換を施し、当該施された並進移動変換の移動先範囲におけるクエリ点群に対して、前記サブボクセル単位でボクセル位置が互いに異なる複数の分割手法を適用することを特徴とする請求項１または２に記載の推定装置。
さらに、前記クエリ点群モデル化データを量子化して量子化クエリ点群モデル化データを得る第一量子化部を含み、
前記比較部では、リファレンス点群に所定のボクセル分割手法を適用し且つボクセル内のリファレンス点群をモデル化及び量子化して得られた量子化リファレンス点群モデル化データと、前記第一量子化部で得られる前記複数の分割手法ごとの量子化クエリ点群モデル化データとを比較し、類似していると判定される分割手法における量子化クエリ点群モデル化データを得ることを特徴とする請求項１ないし３のいずれかに記載の推定装置。
前記クエリ点群はセンサによって取得されたものであり、
前記リファレンス点群は前記センサが計測する対象となるフィールドにおいて取得されたものであり、
前記センサが前記クエリ点群を取得した際の初期位置姿勢を取得する初期位置姿勢取得部と、
前記比較部によって最も類似していると判定される分割手法におけるクエリ点群モデル化データの当該分割手法に基づいて前記初期位置姿勢を補正したものとして、前記センサが前記クエリ点群を取得した際の前記フィールド内での位置姿勢を推定する推定部をさらに備えることを特徴とする請求項１ないし４のいずれかに記載の推定装置。
前記比較部で前記比較する対象となる前記リファレンス点群モデル化データを、前記フィールド内の一連のリファレンス点群の中から前記初期位置姿勢の近傍に属するものとして取得する近傍取得部をさらに備えることを特徴とする請求項５に記載の推定装置。
前記リファレンス点群モデル化データは、ボクセルが集合した所定サイズのリファレンスブロックに対して前記近傍取得部によって得られており、
前記第一モデル化部は、前記所定サイズよりも大きなサイズでボクセルが集合したクエリブロックに対して前記クエリ点群モデル化データを得ており、
前記第一分割部ではさらに、前記クエリブロック内の所定位置範囲であって前記リファレンスブロックのサイズを占める位置範囲に並進移動変換を施し、当該施された並進移動変換の移動先範囲におけるクエリ点群に対して、前記サブボクセル単位でボクセル位置が互いに異なる複数の分割手法を適用し、
前記推定部では、前記比較部によって最も類似していると判定される分割手法及び並進移動変換におけるクエリ点群モデル化データの当該並進移動変換に基づいて前記初期位置姿勢における位置を補正したものとして、前記センサが前記クエリ点群を取得した際の前記フィールド内での位置姿勢を推定することを特徴とする請求項６に記載の推定装置。
前記第一分割部ではさらに、前記クエリ点群に複数の回転変換を施し、当該施された回転変換ごとのクエリ点群に対して、前記サブボクセル単位でボクセル分割位置が互いに異なる複数の分割手法を適用し、
前記推定部では、前記比較部によって最も類似していると判定される分割手法及び回転変換におけるクエリ点群モデル化データの当該回転変換に基づいて前記初期位置姿勢における姿勢を補正したものとして、前記センサが前記クエリ点群を取得した際の前記フィールド内での位置姿勢を推定することを特徴とする請求項６または７に記載の推定装置。
前記推定部では、前記比較部によって最も類似していると判定される分割手法におけるクエリ点群モデル化データの当該分割手法におけるボクセル分割位置に基づいて前記初期位置姿勢における位置を補正したものとして、前記センサが前記クエリ点群を取得した際の前記フィールド内での位置姿勢を推定することを特徴とする請求項６ないし８のいずれかに記載の推定装置。
前記第一モデル化部では、ボクセル内のクエリ点群をモデル化したクエリ点群モデル化データを、クエリ点群における座標平均、座標標準偏差、点群に対応する画素の輝度平均、点群に対応する画素の色平均の少なくとも１つを含んで構成されるものとして得ることを特徴とする請求項１ないし９のいずれかに記載の推定装置。
前記比較部では、リファレンス点群モデル化データ及びクエリ点群モデル化データを同一サイズのデータ列となしたうえで比較することを特徴とする請求項１ないし１０のいずれかに記載の推定装置。
コンピュータを請求項１ないし１１のいずれかに記載の推定装置として機能させることを特徴とするプログラム。