JPWO2017168472A1

JPWO2017168472A1 - 位置推定装置、位置推定方法及び制御プログラム

Info

Publication number: JPWO2017168472A1
Application number: JP2017533039A
Authority: JP
Inventors: 智英石上
Original assignee: Panasonic Intellectual Property Management Co Ltd
Current assignee: Panasonic Intellectual Property Management Co Ltd
Priority date: 2016-03-30
Filing date: 2016-08-24
Publication date: 2018-04-05
Anticipated expiration: 2036-08-24
Also published as: WO2017168472A1; JP6331114B2; US10402998B2; US20180033160A1

Abstract

空間における移動体の位置を推定する位置推定装置（１００）は、移動体の周囲の局所空間における複数の点各々についての、位置と、色とを、測定する測定部（１１０）と、測定部（１１０）による測定結果から、局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び色情報を抽出する抽出部（１２０）と、地図で表される空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び色情報を管理する地図管理部（１３０）と、地図管理部（１３０）が管理する位置情報及び色情報に基づき、抽出部（１２０）が抽出した位置情報及び色情報に応じて、移動体の位置を推定する位置推定部（１４０）とを備える。

Description

本開示は、空間における移動体の位置を推定する位置推定装置に関する。

特許文献１は、平行移動及び回転した地図データをカメラ映像に重畳表示し、その類似度から位置推定を行う技術を開示する。また、非特許文献１は、３次元座標に関して地図データとカメラで測定した測定データとをボクセル（３次元空間を分割してなる直方体の小領域）群で表してボクセルの照合によりカメラ位置を推定する技術を開示する。

特開２０１３−１０９６２４号公報

鄭龍振、外３名、「大規模な三次元環境地図とＲＧＢ−Ｄカメラを用いた移動ロボットの広域位置同定」、日本ロボット学会誌、２０１３年、Ｖｏｌ．３１、Ｎｏ．９、ｐｐ．８９５−９０６

空間（３次元空間）における位置を３次元座標で表す地図データとカメラ映像等の測定データとからカメラ等の移動体の位置推定を行う場合に、非特許文献１の技術のように、ボクセルの位置（３次元座標）を用いることはボクセルの照合に係る計算の負荷の低減（つまり位置推定の迅速化）に有用である。しかし、同じような形状の構造物が連続的に配置されているような環境においては、位置推定を適切に行えない場合もある。

本開示は、空間における移動体の位置を推定する位置推定装置であって、同じような形状の構造物が連続的に配置されているような環境においても位置推定を適切に行い得る可能性を高めた位置推定装置を提供する。また本開示は、その位置推定装置で用いられる位置推定方法、及び、位置推定用の制御プログラムを提供する。

本開示における位置推定装置は、空間における移動体の位置を推定する位置推定装置であって、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定部と、前記測定部による測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出部と、地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理部と、前記地図管理部が管理する前記位置情報及び前記属性情報に基づき、前記抽出部が抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定部とを備える。

また、本開示における位置推定方法は、空間における移動体の位置を推定する位置推定装置において用いられる位置推定方法であって、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定ステップと、前記測定ステップでの測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、前記地図管理ステップで管理する前記位置情報及び前記属性情報に基づき、前記抽出ステップで抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定ステップとを含む。

また、本開示における制御プログラムは、空間における移動体の位置を推定する、マイクロプロセッサを備える位置推定装置に位置推定処理を実行させるための制御プログラムであって、前記位置推定処理は、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性との、測定結果を取得する取得ステップと、前記取得ステップで取得された測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、前記地図管理ステップで管理する前記位置情報及び前記属性情報に基づき、前記抽出ステップで抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定ステップとを含む。

本開示における位置推定装置等によれば、同じような形状の構造物が連続的に配置されているような環境においても、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

図１は、実施の形態１に係る位置推定装置が存在する空間の一例を示す概略図である。図２は、実施の形態１に係る位置推定装置の機能ブロック図である。図３は、空間における測定点のデータ（測定点データ）に基づいてボクセルについてのデータ（ボクセルデータ）を抽出する過程を示す概念図である。図４は、実施の形態１に係る測定点データの一例を示す図である。図５は、実施の形態１に係るボクセルデータの一例を示す図である。図６は、位置推定装置の抽出部によるボクセルデータの抽出処理の一例を示すフローチャートである。図７は、位置推定装置の位置推定部による位置推定処理の一例を示すフローチャートである。図８は、位置推定部による位置推定処理の一部である尤度算定の一例を示すフローチャートである。図９は、位置推定部による位置推定処理に用いる粒子データの一例を示す図である。図１０は、粒子データを用いて測定ボクセルの位置情報に係る測定座標を地図座標に変換する変換演算を示す概念図である。図１１は、位置推定処理における粒子データを用いた予測、観測（尤度算定）及びリサンプリングを示す概念図である。図１２は、ボクセルについての位置情報の近似度算定の一例を示す図である。図１３は、ボクセルについての色情報の近似度算定の一例を示す図である。図１４は、実施の形態１の変形例１における地図ボクセルデータの一例を示す図である。図１５は、実施の形態１の変形例２におけるボクセルデータの一例を示す図である。図１６は、実施の形態１の変形例２におけるボクセルについての色情報の近似度算定の一例を示す図である。図１７は、実施の形態１の変形例３におけるボクセルについての色情報の抽出に用いるサブボクセルを示す図である。図１８は、実施の形態２に係る位置推定装置の機能ブロック図である。図１９は、実施の形態２に係る測定点データの一例を示す図である。図２０は、実施の形態２に係るボクセルデータの一例を示す図である。図２１は、ボクセルについての反射強度情報の近似度算定の一例を示す図である。図２２は、実施の形態３に係る位置推定装置の機能ブロック図である。図２３は、実施の形態３に係る測定点データの一例を示す図である。図２４は、実施の形態３に係るボクセルデータの一例を示す図である。図２５は、ボクセルについての温度情報の近似度算定の一例を示す図である。

同じような形状の構造物が連続的に配置されているような環境においても、移動体の位置を迅速かつ適切に推定し得る可能性を高めるようにすべく、本開示における位置推定装置は、空間における移動体の位置を推定する位置推定装置であって、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定部と、前記測定部による測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出部と、地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理部と、前記地図管理部が管理する前記位置情報及び前記属性情報に基づき、前記抽出部が抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定部とを備える。

これにより、ボクセルについての位置のみならず属性情報（色、温度、反射強度等といった電磁波により検知される属性に基づく情報）を用いて、位置推定を行う。同じような形状の構造物が連続的に配置されているような環境においても、その構造物間で属性は異なり得るので、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

ここで、例えば、前記位置推定部は、前記地図管理部が管理する前記空間に対応する前記各ボクセルについての位置情報と、前記局所空間の３次元座標系を前記空間の３次元座標系に変換するための複数の変換演算各々を用いて変換した、前記抽出部が抽出した前記局所空間に対応する前記各ボクセルについての位置情報との近似度を算定し、前記変換演算毎に、算定した前記近似度が最も高いところの前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報同士の近似度に基づいて尤度を算定し、前記複数の変換演算のうち算定した前記尤度が最も高い変換演算又は算定した前記尤度に応じて加重平均した変換演算に応じて前記移動体の位置の前記推定を行うこととしてもよい。

これにより、位置情報同士の近似度のみならず属性情報同士の近似度に基づいて、測定した局所空間が空間のどの位置に相当する可能性が高いかに関連する尤度を算定するので、適切に移動体の位置を推定し得る。

また、例えば、前記位置推定部は、前記複数の変換演算各々を用いて変換した前記局所空間に対応する前記各ボクセルに係る前記近似度の前記算定に基づく、前記尤度の前記算定及び前記移動体の位置の前記推定を、繰り返し行い、前記尤度の算定結果に基づいて次に前記変換に用いる前記複数の変換演算を定めることとしてもよい。

これにより、推定を繰り返すので、位置推定の精度を高められる可能性がある。

また、例えば、前記位置推定部は、前記地図管理部が管理する前記空間に対応する前記各ボクセルについての位置情報と、前記局所空間の３次元座標系を前記空間の３次元座標系に変換するための複数の変換演算各々を用いて変換した、前記抽出部が抽出した前記局所空間に対応する前記各ボクセルについての位置情報との近似度を算定し、前記変換演算毎に、算定した前記近似度が最も高いところの前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における両ボクセルについての位置情報同士の近似度に基づいて尤度を算定し、前記複数の変換演算のうち算定した尤度が最も高い変換演算又は算定した尤度に応じて加重平均した変換演算に応じて前記移動体の位置の前記推定を行い、前記複数の変換演算各々を用いて変換した前記局所空間に対応する前記各ボクセルに係る前記近似度の前記算定に基づく、前記尤度の前記算定及び前記移動体の位置の前記推定を、繰り返し行い、前記尤度の算定結果に基づいて次に前記変換に用いる前記複数の変換演算を定め、当該繰り返しの初回以外において一定条件下で、前記尤度の前記算定を、前記両ボクセルについての前記位置情報同士の前記近似度に加えて当該両ボクセルについての前記属性情報同士の近似度にも基づいて、行うこととしてもよい。

これにより、位置推定を効率的に行うことができる。

また、例えば、前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルの位置情報を、前記測定部により測定された当該ボクセル内の各点についての位置の平均値を示すように定めることにより、行われ、前記地図管理部により管理される前記各ボクセルについての前記位置情報は、該当のボクセルに対応する前記空間の一部における複数の点についての位置の平均値を示すこととしてもよい。

これにより、測定された位置をボクセル毎に圧縮した位置情報を用いて、ボクセル間での位置情報の比較（近似度算定等）により位置推定がなされるので、測定された位置を全て比較に用いる場合に比べて計算負荷が低減し、迅速な位置推定が可能となる。

また、例えば、前記抽出部が抽出する、前記局所空間に対応する前記各ボクセルについての前記位置情報、又は、前記地図管理部が管理する、前記空間に対応する前記各ボクセルについての前記位置情報は、更に、当該ボクセル内の各点の位置に基づいて定めた分散共分散行列で特定される平面を示し、前記位置推定部は、前記位置情報同士の近似度を、当該位置情報同士の一方が示す位置の平均値と他方が示す平面との距離に応じて特定することにより、前記尤度の前記算定を行うこととしてもよい。

これにより、平面を示す位置情報を用いることで位置推定を、より高精度に行えるようになる可能性がある。

また、例えば、前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての属性に基づくヒストグラムを示すように定めることにより、行われ、前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、当該点からの電磁波の受波で検知される属性に基づくヒストグラムを示し、前記位置推定部は、前記属性情報同士の近似度を、当該属性情報同士の一方が示す前記ヒストグラムと他方が示す前記ヒストグラムとの距離に応じて特定することにより、前記尤度の前記算定を行うこととしてもよい。

これにより、例えば属性情報が単に１次元の値を示す場合と比べて、位置推定を、より高精度に行えるようになる可能性がある。

また、例えば、前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての属性の平均値を示すように定めることにより、行われ、前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、当該点からの電磁波の受波で検知される属性の平均値を示すこととしてもよい。

これにより、測定された属性をボクセル毎に圧縮した属性情報を用いて、ボクセル間での属性情報の比較（近似度算定等）により位置推定がなされるので、測定された位置を全て比較に用いる場合に比べて計算負荷が低減し、迅速な位置推定が可能となる。

また、例えば、前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての位置と属性との相関を示すように定めることにより、行われ、前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、位置と電磁波の受波で検知される属性との相関を示すこととしてもよい。

これにより、ボクセル内の測定点に基づいた局所自己相関を示す属性情報を用いるので、例えば測定点の属性の平均値のみを示す属性情報を用いる場合と比べて、位置推定を、より高精度に行えるようになる可能性がある。

また、例えば、前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内及び当該ボクセルに近接する１以上のボクセル内の、各点についての位置と属性との相関を示すように定めることにより、行われ、前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部及び当該ボクセルに近接する１以上のボクセルに対応する前記空間の一部における、複数の点についての、位置と電磁波の受波で検知される属性との相関を示すこととしてもよい。

これにより、隣接するボクセル群における測定点に基づいた局所自己相関を示す属性情報を用いるので、例えば測定点の属性の平均値のみを示す属性情報を用いる場合と比べて、位置推定を、より高精度に行えるようになる可能性がある。

また、例えば、前記測定部が測定する前記属性は色であり、前記抽出部が抽出する前記属性情報は、色を表す色情報であり、前記地図管理部が管理する前記属性情報は、色を表す色情報であることとしてもよい。

これにより、ボクセルについての位置のみならず色情報を用いて推定を行う。同じような形状の構造物が連続的に配置されているような環境においても、その構造物間で色は異なり得るので、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

また、例えば、前記抽出部が抽出する前記色情報及び前記地図管理部が管理する前記色情報のそれぞれは、特定色成分と当該特定色成分以外の他色成分とを区別可能な情報であり、前記位置推定部は、前記尤度の前記算定の基礎とする、前記両ボクセルについての前記属性情報同士の前記近似度を、当該属性情報同士における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定することとしてもよい。

これにより、年月、季節、時刻等によって態様（外観等）が変化し、色が特有である樹木、空等について、特定色成分を定めておくことで、空間におけるその態様が変化するもの（樹木、空等）の影響で、位置推定の精度が低下することを抑制できる可能性がある。

また、例えば、前記抽出部が抽出する前記色情報及び前記地図管理部が管理する前記色情報のそれぞれは、特定色成分と当該特定色成分以外の他色成分とを区別可能な情報であり、前記位置推定部は、前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が、所定位置条件を満たす場合には、前記局所空間に係る前記ボクセルについての前記属性情報における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定することとしてもよい。

これにより、適切に所定位置条件を定めれば、空間における態様が変化するものの所在する位置を限定できるので、空間におけるその態様が変化するものの影響で、位置推定の精度が低下することを適切に抑制できる可能性がある。

また、例えば、前記地図管理部は、前記地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての平面度を管理し、前記位置推定部は、前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、当該両ボクセルのうち前記空間に係る前記ボクセルについての前記平面度が特定条件を満たす場合には、前記空間に係る前記ボクセルについての前記属性情報における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定することとしてもよい。

これにより、適切に特定条件を定めれば、空間における態様が変化するものを、平面度により限定できるので、空間におけるその態様が変化するものの影響で、位置推定の精度が低下することを適切に抑制できる可能性がある。

また、例えば、前記測定部が測定する前記属性は電磁波の反射強度であり、前記抽出部が抽出する前記属性情報は、反射強度を表し、前記地図管理部が管理する前記属性情報は、反射強度を表すこととしてもよい。

これにより、ボクセルについての位置のみならず反射強度を示す属性情報を用いて推定を行う。同じような形状の構造物が連続的に配置されているような環境においても、その構造物間で電磁波の反射強度は異なり得るので、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

また、例えば、前記位置推定部は、前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が所定位置条件を満たす場合において、前記局所空間に係る前記ボクセルについての前記属性情報が所定強度より高い反射強度を示すときには、当該属性情報が前記所定強度より高くない反射強度を示すときより、当該近似度への影響の度合いを高めるように算定することとしてもよい。

これにより、例えば所定位置条件を路面に近い高さで満たされるように定めれば、例えば反射強度が高い白線等の道路表示に基づいて、位置推定を精度良く行える可能性がある。

また、例えば、前記測定部が測定する前記属性は温度であり、前記抽出部が抽出する前記属性情報は、温度を表し、前記地図管理部が管理する前記属性情報は、温度を表すこととしてもよい。

これにより、ボクセルについての位置のみならず温度を示す属性情報を用いて推定を行う。同じような形状の構造物が連続的に配置されているような環境においても、その構造物間で温度は異なり得るので、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

また、例えば、前記位置推定部は、前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が所定位置条件を満たす場合において、前記局所空間に係る前記ボクセルについての前記属性情報が人間の体温に相当する所定温度範囲内の温度を示すときより、当該属性情報が前記所定温度範囲外の温度を示すときには、当該近似度への影響の度合いを高めるように算定することとしてもよい。

これにより、時刻によって移動する人間による影響で、位置推定の精度が低下することを抑制できる可能性がある。

また、例えば、前記位置推定部は、推定した前記移動体の位置を示す情報を出力することとしてもよい。

これにより、位置推定の結果の利用が容易となり得る。

これにより、同じような形状の構造物が連続的に配置されているような環境においても、その構造物間で属性は異なり得るので、移動体の位置を迅速かつ適切に推定できる可能性が高まる。

これにより、この制御プログラムがインストールされて実行される、マイクロプロセッサを備える位置推定装置は、測定点の属性を含む測定結果を取得して利用することで、移動体の位置を迅速かつ適切に推定し得る。

なお、これらの包括的又は具体的な各種態様には、装置、システム、方法、集積回路、コンピュータプログラム、コンピュータで読み取り可能な記録媒体等の１つ又は複数の組合せが含まれる。

以下、適宜図面を参照しながら、実施の形態を詳細に説明する。但し、必要以上に詳細な説明は省略する場合がある。例えば、既によく知られた事項の詳細説明や実質的に同一の構成に対する重複説明を省略する場合がある。これは、以下の説明が不必要に冗長になるのを避け、当業者の理解を容易にするためである。

なお、発明者は、当業者が本開示を十分に理解するために図面及び以下の説明を提供するのであって、これらによって請求の範囲に記載の主題を限定することを意図するものではない。即ち、以下の具体例を用いた説明で示される数値、形状、構成要素、構成要素の配置及び接続形態、並びに、ステップ（工程）及びステップの順序等は、一例であって、請求の範囲に記載の主題を限定するものではない。以下の実施の形態における構成要素のうち、独立請求項に記載されていない構成要素については、任意に付加可能な構成要素である。また、各図は、模式図であり、必ずしも厳密に図示されたものではない。

（実施の形態１）
実施の形態１では、主に、空間における移動体の位置を推定するために位置の他に色を用いる位置推定方法を実行する位置推定装置１００について説明する。

以下、図面を用いて、実施の形態１を説明する。

［１−１．構成］
図１は、実施の形態１に係る位置推定装置１００の一例を示し、位置推定装置１００が存在する空間の一例を示す。

位置推定装置１００は、同図に示すように、例えば測定装置１０１を取り付けた移動体１０２であり、建物１ａ、建物１ｂ等の構造物その他の物体が存在する空間（３次元空間）を移動する。位置推定装置１００は、空間における移動体１０２の位置を推定する機能を有する。

測定装置１０１は、周囲の局所空間における複数の点（測定点）の位置及び色を測定する装置である。ここで測定点は、測定性能（分解能）等に応じてある程度の大きさを有する場所であり得る。測定点の色は、その点からの電磁波の受波により検知される、測定点の属性の一種であり、例えば、波長３８０ｎｍ〜７８０ｎｍの範囲の可視光（電磁波の一種）により検知される。測定装置１０１は、例えば、３次元スキャナ（３Ｄスキャナ）、ステレオカメラ、或いは、３Ｄ−ＬｉＤＡＲ（Light Detection and Ranging）又は２次元レーザーレンジファインダーとカメラとの組み合わせ等である。

移動体１０２は、例えば、ロボット、航空機、船舶、車両等であり空間内を移動し得る。移動体１０２がロボットの場合は、移動できれば、例えば、車輪を有する車両型のものであっても、二足歩行するもの等であってもよい。ここでは、移動体１０２として、主に、車両型で路面（地表面）を走行し得るロボット、或いは、車両を想定して説明する。

位置推定装置１００は、測定装置１０１と通信可能なコンピュータを備え、コンピュータは、例えば移動体１０２に搭載されている。

位置推定装置１００のコンピュータは、メモリ、プロセッサ（マイクロプロセッサ）、通信インタフェース（Ｉ／Ｆ）等を備える。このコンピュータは、例えば、メモリの他にハードディスク等の記憶媒体を備えていてもよい。メモリは、プログラム及びデータを予め保持しているＲＯＭ、プログラムの実行に際してデータ等の記憶に利用するためのＲＡＭ等であり、例えば不揮発性メモリを含んでいてもよい。メモリには、位置推定方法に係る位置推定処理を実現するための制御プログラムやその制御プログラムで利用するための設定値等が予め格納される。また、メモリ等の記憶媒体には、移動体１０２が移動する空間を３次元座標系で表した地図情報（後述する地図ボクセルデータ等）が格納される。プロセッサは、制御プログラムを実行することにより、通信Ｉ／Ｆ等を制御して各種処理を行う。また、メモリは、制御プログラムのプロセッサによる実行に際して用いられる各値を一時的に格納するためにも用いられる。通信Ｉ／Ｆは、例えば無線通信或いはケーブルを介した通信により測定装置１０１等と通信を行うための通信回路等である。通信Ｉ／Ｆは、通信網を介して移動体１０２と離れた外部装置と通信する機能を有してもよい。外部装置との通信により、例えば、移動体１０２の一定期間内の移動範囲となり得る空間に対応する地図情報を、外部装置から取得（ダウンロード）したり、推定した移動体１０２の位置を外部装置に送信したりすることが可能になる。

以下、上述した位置推定装置１００の機能について詳細に説明する。

図２は、位置推定装置１００の機能ブロック図である。位置推定装置１００は、同図に示すように、測定部１１０、抽出部１２０、地図管理部１３０、及び、位置推定部１４０を有する。以下、各機能部について説明する。

［１−１−１．測定部］
測定部１１０は、移動体１０２の周囲の局所空間における複数の点（つまり測定点）各々についての、位置と色とを測定する機能を有し、測定により測定結果である測定点データを取得する。測定部１１０は、例えば、測定装置１０１、及び、位置推定装置１００のコンピュータにおける通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。

移動体１０２の位置は逐次変化し得るため、測定部１１０は、例えば、ある一定周期毎に、測定装置１０１により、移動体１０２の周囲の局所空間における各測定点（例えば水平方向全周分等の各測定点）についての測定点データ群を繰り返し取得してもよい。

［１−１−２．抽出部］
抽出部１２０は、測定部１１０による測定結果（測定点データ）から、移動体１０２の周囲の局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報（具体的には色を表す色情報）を抽出する機能を有する。抽出部１２０は、位置推定装置１００のコンピュータにおける、制御プログラムを実行するプロセッサ等により実現される。

局所空間を測定した測定点データは、測定部１１０（測定装置１０１等）で定めた３次元座標系における座標で表される位置を含む。抽出部１２０は、測定点群に係る測定点データから、その３次元座標系に沿って局所空間を直方体のボクセル群に分割した各ボクセルについてのデータ（ボクセルデータ）を抽出することになる。ボクセルデータは、その３次元座標系における位置情報を含み、色情報を含む。抽出部１２０により、測定点データから抽出されたボクセルデータを測定ボクセルデータと称し、測定ボクセルデータを有することとなるボクセルを、測定ボクセルと称する。抽出部１２０による測定ボクセルについての測定ボクセルデータ（位置情報及び色情報）の抽出（抽出処理）では、測定ボクセル内に含まれる複数の測定点についての測定点データ群から、情報量の面で圧縮するように、測定ボクセルについての測定ボクセルデータが算定される。この情報量の圧縮は、３次元空間では２次元平面以上に情報量が増加するので測定点同士をそのまま比較するのでは計算量が増加してリアルタイムの位置推定が困難となるという問題に、対応する手段となる。

抽出部１２０による測定ボクセルデータの抽出は、例えば、各測定ボクセルについて、その測定ボクセルの位置情報を、測定部１１０により測定された測定ボクセル内の各点についての位置の平均値を示すように定めることにより、行われる。一例としては、抽出部１２０は、測定ボクセル内に含まれる各測定点についての測定点データにおける位置（３次元座標ｘ，ｙ，ｚで表される位置）を平均化してその測定ボクセルについての測定ボクセルデータにおける位置情報を算定する。また、抽出部１２０による測定ボクセルデータの抽出は、例えば、各測定ボクセルについて、その測定ボクセルについての色情報を、測定部１１０により測定されたその測定ボクセル内の各点についての色の平均値を示すように定めることにより、行われる。一例としては、抽出部１２０は、測定ボクセル内に含まれる各測定点についての測定点データにおける、赤（Ｒ：Red）、緑（Ｇ：Green）、青（Ｂ：Blue）の成分で表される色（Ｒ、Ｇ、Ｂの各成分が８ビットで表されるＲＧＢ値）を平均化してその測定ボクセルについての測定ボクセルデータにおける色情報を算定する。

図３は、空間における測定点の測定点データに基づいて測定ボクセルについての測定ボクセルデータを抽出する過程を示す概念図である。同図では、各測定点データ（測定点データ２０１ａ等）をその測定点の位置と対応付けた配置で表しており、また、測定ボクセルが有する測定ボクセルデータ（測定ボクセルデータ２０３ａ等）をその測定ボクセルデータにおける位置情報が示す位置と対応付けた配置で表している。測定部１１０により、空間の一部の移動体１０２の周囲の局所空間における測定がなされて、建物１ａ及び建物１ｂを含む複数の測定点について測定点データ２０１ａ、２０１ｂ等が得られる。抽出部１２０は、測定された範囲である局所空間を、測定ボクセル２０２ａ、２０２ｂ等といったボクセル（例えば各辺が数メートルの立方体等）群に分割する（つまり、ボクセル群の３次元配列と対応付ける）。測定ボクセル２０２ａは、測定ボクセルデータ２０３ａを有し、測定ボクセル２０２ｂは、測定ボクセルデータ２０３ｂを有する。測定ボクセル２０２ａ内に含まれる測定点に対応する測定点データ２０１ａ、２０１ｂ等は、測定ボクセル２０２ａについての測定ボクセルデータ２０３ａに反映される。

測定点データは、図４に例示するように、位置を３次元座標ｘ，ｙ，ｚで表し、色をＲＧＢ値で表したものである。なお、図４は、一行毎に一測定点についての測定点データを示している。図５は、測定ボクセルデータの一例を示す図であり、一行毎に一測定ボクセルについての測定ボクセルデータを示している。図５に示すような測定ボクセルデータは、メモリ等の記憶媒体に一時的に保持される。図５に示すように、測定ボクセルデータは、局所空間に対応付けた測定ボクセル群の３次元配列における該当の測定ボクセルの配置を示すインデックス（ｉｎｄｅｘ）と、その測定ボクセル内の各測定点データの位置の平均値を示す位置情報と、その測定ボクセル内の各測定点データの色（ＲＧＢ値）の平均値を示す色情報とを含む。例えば、図３の測定ボクセルデータ２０３ａにおける位置情報は、測定ボクセル２０２ａに含まれる各測定点に対応する測定点データ（測定点データ２０１ａ、２０１ｂ等）の位置の平均値を示し、測定ボクセルデータ２０３ａにおける色情報は、測定ボクセル２０２ａに含まれる各測定点に対応する測定点データ（測定点データ２０１ａ、２０１ｂ等）の色の平均値を示す。

［１−１−３．地図管理部］
地図管理部１３０は、地図（つまり空間を表す３次元座標系を定めた地図）で表される空間と対応付けて、３次元状に配列したボクセル群における各ボクセルについてのボクセルデータ、つまり、位置情報及び属性情報（具体的には色を表す色情報）を、管理する機能を有する。地図管理部１３０は、例えば、位置推定装置１００のコンピュータにおけるメモリ等の記憶媒体、通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。地図管理部１３０が管理するボクセルデータを地図ボクセルデータと称し、地図ボクセルデータを有することとなるボクセルを、地図ボクセルと称する。地図管理部１３０は、空間を分割してなる地図ボクセル群における各地図ボクセルについての地図ボクセルデータ（位置情報及び色情報）を含む地図情報を管理している。

地図管理部１３０による地図情報の管理は、地図情報を利用可能な状態にすることであり、具体的には、メモリ等の記憶媒体により地図情報を保持する、或いは、外部装置等から地図情報を取得することである。地図情報は、例えば、予め定めた３次元座標系に従って、空間を測定することで予め生成された情報である。地図情報は、例えば、空間における各測定点の位置及び属性（具体的には色）を測定してその測定結果に基づいて、各地図ボクセルについての地図ボクセルデータ（位置情報及び色情報）を抽出することで生成される。この測定結果に基づく地図ボクセルデータの抽出は、例えば、抽出部１２０が測定点データから測定ボクセルについての測定ボクセルデータを抽出する方法と同様の方法により行われてもよい。本実施の形態では、地図ボクセルデータのデータ構成は、図５に示す測定ボクセルデータと同様であるものとする。即ち、地図ボクセルデータは、地図ボクセルの配置を示すインデックス（ｉｎｄｅｘ）とその地図ボクセルに対応する、空間の一部における各点（つまり各場所）の位置の測定結果の平均値を示す位置情報と、その地図ボクセルに対応する、空間の一部における各点（つまり各場所）についての、その点からの電磁波の受波により検知される属性としての色の測定結果（ＲＧＢ値）の平均値を示す色情報とを含むものとする。

［１−１−４．位置推定部］
位置推定部１４０は、地図管理部１３０が管理する地図情報の地図ボクセルデータ（位置情報、及び、属性情報としての色情報）に基づき、抽出部１２０が抽出した測定ボクセルデータ（位置情報、及び、属性情報としての色情報）に応じて、移動体１０２の位置を推定する機能を有する。位置推定部１４０は、例えば、位置推定装置１００のコンピュータにおけるメモリ、制御プログラムを実行するプロセッサ等により実現される。

位置推定部１４０は、地図管理部１３０が管理する各地図ボクセルについての地図ボクセルデータと抽出部１２０が抽出した各測定ボクセルについての測定ボクセルデータとを照合する。この照合では、位置推定部１４０は、測定ボクセルについての測定ボクセルデータにおける位置情報が、地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報（移動体１０２の周囲の局所空間の測定に用いた３次元座標による位置情報）を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定する。この変換演算を特定することにより、移動体１０２の位置が推定される。例えば、特定された変換演算を測定ボクセルデータの位置情報に対して施すことで、地図の３次元座標系における移動体１０２の位置が推定できる。

この移動体１０２の位置を推定する方法、つまり、変換演算を推定的に特定する方法としては、例えば、ベイズ推定の一手法として知られるパーティクルフィルタを用いて予測、観測、及び、リサンプリングの手順を繰り返す方法（後述）を用いることができる。概ね、位置推定部１４０は、地図管理部１３０が管理する各地図ボクセルについての位置情報と、候補としての複数の変換演算各々を用いて変換した、抽出部１２０が抽出した各測定ボクセルについての位置情報との近似度を算定する。そして、位置推定部１４０は、その変換演算毎に、算定した近似度が最も高いところの地図ボクセル及び測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報（つまり色情報）同士の近似度に基づいて、尤度を算定する。候補としての複数の変換演算のうち算定した尤度が最も高い変換演算、又は、算定した尤度に応じて加重平均した変換演算に応じて、位置推定部１４０は、移動体１０２の位置の推定を行う。ここで、位置を示す位置情報同士の近似度或いは色を示す色情報同士の近似度は、位置或いは色が近似する度合いの高さを示し、同一であれば最高値となる。なお、位置推定部１４０は、複数の変換演算各々を用いて変換した各測定ボクセルに係る近似度の算定に基づく、尤度の算定及び移動体の位置の推定を、繰り返し行い、尤度の算定結果に基づいて次に変換に用いる複数の変換演算を定める。

［１−２．動作］
以下、上述の構成を備える位置推定装置１００の動作（つまり位置推定方法）について説明する。

［１−２−１．抽出部の動作］
図６は、抽出部１２０による測定ボクセルデータの抽出処理の一例を示すフローチャートである。測定ボクセルデータの抽出処理について、図６に即して説明する。

抽出部１２０は、測定ボクセルについてボクセルサイズを決定する（ステップＳ１１）。例えば、ボクセルサイズとして測定に係る３次元座標系（ｘｙｚ３次元座標系）におけるｘ，ｙ，ｚ方向の長さがそれぞれ定められる。これにより、測定ボクセルは、例えば、各辺が１メートルの立方体等と定められることになる。また、抽出部１２０は、測定ボクセルについての測定に係る３次元座標系における測定範囲内の値（ｘ，ｙ，ｚ座標値）を、インデックスに換算するために用いる加算値であるオフセットのｘ，ｙ，ｚ座標を決定する（ステップＳ１２）。なお、測定ボクセルのボクセルサイズは、必ずしも地図ボクセルのボクセルサイズと一致させる必要はない。

続いて、抽出部１２０は、測定部１１０による測定により得られた移動体１０２の周囲の局所空間に対応する各測定点のうちの１つについての測定点データを処理対象として取得する（ステップＳ１３）。そしてその取得した測定点データに対応する測定ボクセル（その測定点データに係る位置を包含する測定ボクセル）を特定してその測定ボクセルのインデックスを計算する（ステップＳ１４）。例えば、測定ボクセルのインデックスは、測定点データの座標値を用いて次の式１〜式３で計算される。このインデックスは、局所空間に３次元状に配列した測定ボクセル群の各測定ボクセルについての３次元配列における配置を示す番号である。

ｘ（ｉｎｄｅｘ）＝（測定点のｘ座標＋オフセットのｘ座標）／ボクセルサイズ（式１）
ｙ（ｉｎｄｅｘ）＝（測定点のｙ座標＋オフセットのｙ座標）／ボクセルサイズ（式２）
ｚ（ｉｎｄｅｘ）＝（測定点のｚ座標＋オフセットのｚ座標）／ボクセルサイズ（式３）

次に抽出部１２０は、ステップＳ１３で取得した測定点データにおける位置を用いて、ステップＳ１４で特定した測定ボクセルについての位置情報を更新する（ステップＳ１５）。また、抽出部１２０は、ステップＳ１３で取得した測定点データにおける色（ＲＧＢ値）を用いて、ステップＳ１４で特定した測定ボクセルについての色情報を更新する（ステップＳ１６）。このステップＳ１５及びステップＳ１６での処理により、メモリ等の記憶媒体に一時的に保持される、図５に示すような測定ボクセルデータ（位置情報及び色情報）が、更新されることになる。そして、未だ処理対象としていない測定点データを順次処理対象として、全ての測定点データを処理対象として処理を終了するまでステップＳ１３〜Ｓ１６での処理を繰り返す（ステップＳ１７）。

なお、測定部１１０により移動体１０２の周囲の局所空間の各測定点（例えば水平方向全周分等の各測定点）についての測定点データ群が得られる毎に、この抽出処理（ステップＳ１１〜Ｓ１７）、或いは、その一部のステップＳ１３〜Ｓ１７での処理が行われるようにしてもよい。このようにある程度の量の測定点データ群が得られてから抽出部１２０が各測定ボクセルの測定ボクセルデータ（位置情報及び色情報）を抽出する場合には、各測定ボクセル内に位置する測定点についての各測定点データにおける位置の平均値を測定ボクセルについての位置情報とし、その各測定点データにおける色（ＲＧＢ値）の平均値を測定ボクセルについての色情報としてもよい。測定ボクセルの位置（例えば測定ボクセルにおける最小位置に当たる一頂点のｘ座標、ｙ座標、ｚ座標）は、インデックスに基づいて次の式４〜式６により特定可能である。

測定ボクセルのｘ座標＝ｘ（ｉｎｄｅｘ） × ボクセルサイズ − オフセットのｘ座標（式４）
測定ボクセルのｙ座標＝ｙ（ｉｎｄｅｘ） × ボクセルサイズ − オフセットのｙ座標（式５）
測定ボクセルのｚ座標＝ｚ（ｉｎｄｅｘ） × ボクセルサイズ − オフセットのｚ座標（式６）

［１−２−２．位置推定部の動作］
位置推定部１４０は、各地図ボクセルについての地図ボクセルデータと各測定ボクセルについての測定ボクセルデータとを照合する。即ち、位置推定部１４０は、移動体１０２の周囲の局所空間を測定して得られた、測定用の３次元座標系で表される測定ボクセルについての測定ボクセルデータにおける位置情報が、どこかに限定を定めた地図用の３次元座標系で表される地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定することで、推定した変換演算に基づいて位置の推定を行う。

このために位置推定部１４０は、パーティクルフィルタを用いて予測、観測、及び、リサンプリングの手順を繰り返す位置推定処理を行う。パーティクルフィルタは、ベイズの定理を用いて、事前確率と尤度計算（尤度算定）から事後確率を予測する手法である。確率は仮説となる粒子とその尤度によって表現される。位置推定部１４０では、測定ボクセルについての測定ボクセルデータにおける位置情報が地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算として尤度の高い変換演算を推定する手段として、粒子を用いる。粒子毎に変換演算が特定される。また、推定された変換演算により、移動体１０２の位置が推定される。

図７は、位置推定部１４０による位置推定処理の一例を示すフローチャートであり、図８は、位置推定処理の一部である尤度算定の一例を示すフローチャートである。以下、図７及び図８に即して、位置推定処理について説明する。

位置推定部１４０は、パーティクルフィルタを用いた位置推定処理として、まず、初期粒子を配置する（ステップＳ２１）。位置推定部１４０は、次に、粒子全体を遷移させる予測の手順（ステップＳ２２）、予測した粒子について尤度を算定する観測の手順（ステップＳ２３）、及び、リサンプリング（粒子についての再標本抽出）の手順（ステップＳ２４）を繰り返す。位置推定部１４０は、位置推定の必要がなくなった等の一定の中断条件が成立すると、処理の繰り返しを終える（ステップＳ２５）。

図９に、位置推定処理に用いる粒子に関する情報である粒子データの一例を示す。位置推定部１４０により、メモリ等の記憶媒体に、図９に示すような粒子データが粒子毎に記憶される。粒子データは、粒子の３次元座標及び回転角度を示す。図９に示すように、記憶媒体における粒子データの記憶領域は、地図の３次元座標系に対応する粒子の３次元座標（ｘ座標、ｙ座標、ｚ座標）と、ｚ軸を回転中心軸とした回転角度θｙａｗと、その粒子の尤度とを格納することになる。ここでは、便宜上、移動体１０２として車両或いは車両型のロボットを想定して説明しているので、回転成分を１次元で表現したが、ｒｏｌｌ、ｐｉｔｃｈ、ｙａｗ等の３次元の成分で表現してもよい。

図１０は、粒子データを用いて測定ボクセルの位置情報に係る測定座標を地図座標に変換する変換演算を示す概念図である。同図に示すように、測定ボクセルデータの位置情報が示す位置である３次元の測定座標Ｘは、粒子の３次元座標（ｘ座標，ｙ座標，ｚ座標）に基づく並進成分Ｔｐと、粒子の回転角度θｙａｗに基づく回転成分Ｒｐとを用いた変換演算により、地図座標系３０１（地図の３次元座標系）における地図座標Ｙに換算される。

上述したステップＳ２１〜Ｓ２４の各手順について、詳細に説明する。

図７に示すステップＳ２１での初期粒子の配置は、粒子の各成分（ｘ座標、ｙ座標、ｚ座標、回転角度θｙａｗ）を、粒子の値が取り得る範囲内となるように乱数により定めたものである初期粒子の生成により実現される。粒子（粒子の各成分）に基づいて、測定ボクセルデータの位置情報を地図座標系３０１に換算するための変換演算を定めるところ、この粒子の値が取り得る範囲は、少なくとも位置推定処理の間において測定ボクセルデータの位置情報が取り得る範囲（つまり地図で表される空間において移動体１０２が移動し得ると想定される範囲）に対応したものとなる。ステップＳ２１では、例えば、１０００個の位置（座標ｘ、座標ｙ、座標ｚ）を乱数で算定して、各位置について５度ずつ異ならせた７２個の回転角度θｙａｗのバリエーションを生成することで、座標ｘ、座標ｙ、座標ｚ及び回転角度θｙａｗを成分とする合計７２０００個の粒子を生成する。

また、移動体１０２の位置についてＧＰＳ（Global Positioning System）受信機等を用いて取得した位置情報や過去の位置情報等の事前位置情報が利用できる場合には、初期粒子を事前位置情報が示す位置の周辺に配置してもよい。

ステップＳ２２での粒子全体を遷移させる予測は、現在時刻を時刻ｔとすると、時刻ｔ−２と時刻ｔ−１との間での粒子の移動（粒子の成分の変位）に基づいて移動ベクトルｍｖを計算し、時刻ｔ−１の粒子全体に移動ベクトルｍｖを加算し、更にガウス分布等で近似した観測ノイズを各粒子に加算して時刻ｔでの粒子を生成することにより実現される。

また、車輪等の回転数や操舵角等のオドメトリ情報が利用できる場合には、移動ベクトルの代わりにそのオドメトリ情報を用いてもよい。

図１１は、ステップＳ２２での予測により、時刻ｔ−１の粒子状態４０２が、移動ベクトルｍｖの加算及び観測ノイズの加算の結果として、時刻ｔにおける粒子状態４０３になる様子を示している。

ステップＳ２３での粒子の尤度を算定する観測について、図８に即して説明する。位置推定部１４０は、尤度算定（観測）において最初に、尤度算定用のデータ（変数）としての尤度合計値をゼロに初期化する（ステップＳ３１）。

次に、位置推定部１４０は、ステップＳ２２で遷移させた各粒子について順次着目して、着目した粒子に応じた変換演算を適用した複数の測定ボクセル各々と地図ボクセルとの照合により、粒子の尤度を算定する（ステップＳ３２〜Ｓ３８）。

まず、尤度算定において未着目の粒子の１つに着目し、また、抽出部１２０で測定ボクセルデータを抽出した複数の測定ボクセルの１つに着目し、その着目した粒子に応じた変換演算（図１０参照）により、その着目した測定ボクセルの測定ボクセルデータにおける位置情報を地図座標系３０１における地図座標へと変換する（ステップＳ３２）。

続いて、位置推定部１４０は、着目中の測定ボクセルの地図座標に変換した測定ボクセルデータにおける位置情報と最も近似度が高い位置情報を含む地図ボクセルデータに係る地図ボクセルを特定する（ステップＳ３３）。位置情報同士の近似度は、両位置情報が示す位置が近い（位置間の距離が短い）ほど高い。ステップＳ３３では、位置推定部１４０は、測定ボクセルについての位置（つまり測定ボクセルデータにおける位置情報が示す３次元座標の平均値）に基づいて、地図管理部１３０が管理する各地図ボクセルデータのうち、地図ボクセルデータにおける位置情報が示す位置が、測定ボクセルについての位置と最も近い、地図ボクセルデータに係る地図ボクセルを特定する。

例えば一例として、位置推定部１４０は、式１〜式３を用いて、ステップＳ３２で変換した地図座標における位置情報の座標値から、地図ボクセルのインデックスを計算し、求める。

次に、位置推定部１４０は、ステップＳ３３で特定した地図ボクセルと着目中の測定ボクセルとの組に係る地図ボクセルデータにおける位置情報と測定ボクセルデータにおける位置情報との近似度を算定して、その近似度を尤度合計値に加算する（ステップＳ３４）。この両ボクセル（特定した地図ボクセルと着目中の測定ボクセル）についての位置情報同士の近似度は、その両位置情報が示す位置（３次元座標）間の距離Ｄに基づいて、距離Ｄが短いほど近似度が高くなるように予め定めた計算式等に基づいて算定し得る。図１２は、地図ボクセル３１１についての位置情報が示す平均位置３１２と測定ボクセル２１１についての位置情報が示す平均位置２１２との間の距離Ｄに基づいて近似度としてｓｐａｃｅ（Ｄ）を算定する近似度算定の一例を示している。位置推定部１４０は、例えば、距離Ｄの間隔の各位置を示す位置情報同士の近似度を、図１２に示す式９０１のように、ｓｐａｃｅ（Ｄ）として算定してもよい。

次に、位置推定部１４０は、ステップＳ３３で特定した地図ボクセルと着目中の測定ボクセルとの組に係る地図ボクセルデータにおける色情報と測定ボクセルデータにおける色情報との近似度を算定して、その近似度を尤度合計値に加算する（ステップＳ３５）。この両ボクセル（特定した地図ボクセルと着目中の測定ボクセル）についての色情報同士の近似度は、例えば、その両色情報が示す色（ＲＧＢ値である色の平均値、つまり平均色）間の、ＲＧＢ３次元座標系における距離Ｃに基づいて、距離Ｃが短いほど近似度が高くなるように予め定めた計算式等に基づいて算定し得る。

図１３は、ＲＧＢ３次元座標系における、地図ボクセルについての色情報が示す平均色３１３と測定ボクセルについての色情報が示す平均色２１３との間の距離Ｃに基づいて近似度としてｃｏｌｏｒ（Ｃ）を算定する近似度算定の一例を示している。位置推定部１４０は、例えば、距離Ｃの間隔の各色（ＲＧＢ値）を示す色情報同士の近似度を、図１３に示す式９０２のように、ｃｏｌｏｒ（Ｃ）として算定してもよい。なお、ステップＳ３４で位置情報に係る近似度を尤度合計値に加算し、ステップＳ３５において色情報に係る近似度を尤度合計値に加算したが、これらは、地図ボクセルと測定ボクセルとの間の位置情報と色情報とに係る相関に基づいて最終的に粒子毎の尤度を算定するための一例に過ぎない。ステップＳ３４及びステップＳ３５で位置情報に係る近似度と色情報に係る近似度とを尤度合計値に加算する代わりに、例えば、位置情報に係る近似度と色情報に係る近似度との乗算結果を尤度合計値に加算することとしてもよいし、例えば、尤度への影響の重みを位置情報と色情報とで異ならせるために、係数を乗じた位置情報に係る近似度と、色情報に係る近似度とを尤度合計値に加算してもよい。

次に、全ての測定ボクセルについて着目してステップＳ３２〜Ｓ３５に係る処理を完了したか否かを判定し（ステップＳ３６）、未完了の場合にはステップＳ３２に戻り、完了した場合には、尤度合計値を、着目中の粒子の尤度として、粒子データにおける尤度の記憶領域（図９参照）に格納する（ステップＳ３７）。

次に、全ての粒子について着目して尤度を求める処理を完了したか否かを判定し（ステップＳ３８）、未完了の場合にはステップＳ３１に戻り、完了した場合には、最大尤度の粒子に応じた変換演算に基づいて、移動体１０２の現在の位置（つまり位置推定結果）を示す情報を出力する（ステップＳ３９）。例えば、変換演算に対して測定ボクセルについての位置情報としてゼロを与えることで位置推定結果が得られる。また、単純に最大尤度の粒子データの成分を現在の位置として出力してもよい。

ステップＳ３２で図１０に示す変換演算を用いる場合において、ステップＳ３９で、位置推定部１４０は、最大尤度の粒子の粒子データが示すその粒子の成分（３次元座標及び回転角度）を、位置推定結果として出力することになる。なお、ステップＳ３９で、位置推定部１４０は、最大尤度の粒子の成分の代わりに、各粒子の成分（３次元座標及び回転角度）についての、粒子の尤度で重み付けした加重平均の結果を、位置推定結果として出力することとしてもよい。各粒子の尤度を、粒子全体の尤度合計値で除算して正規化することで加重平均を求める。ステップＳ３９での位置推定部１４０による位置推定結果（移動体１０２の位置）を示す情報の出力は、例えば移動体１０２に設けられた装置（例えば移動のために車輪の駆動制御を行う制御機構等）へその情報を伝達すること、移動体１０２に搭載された表示装置にその情報を表示すること、或いは、移動体１０２と離れて所在する装置へその情報を送信すること等により行われる。この出力により位置推定結果が利用される。また、各粒子の粒子データにおける尤度（図９参照）は、ステップＳ２４でのリサンプリングに用いられることになる。

次に、図７のステップＳ２４では、ステップＳ２３での粒子の観測（尤度算定）の結果を用いて、粒子の尤度に応じて再度粒子を選ぶ確率を定めてリサンプリングを行う。

リサンプリングについて、粒子を３つ用いた簡単な例で説明する。リサンプリング前の粒子が、（粒子番号，尤度）で表すと（１，０．５）、（２，０．３）、（３，０．２）であった場合において、全粒子の尤度の合計値は、０．５＋０．３＋０．２により１．０である。この場合に、粒子番号１の粒子は、０．５／１．０、つまり５０％の確率で選ばれ、粒子番号２の粒子は、０．３／１．０、つまり３０％の確率で選ばれ、粒子番号３の粒子は、０．２／１．０、つまり２０％の確率で選ばれるように、乱数を用いて粒子を選ぶことで標本抽出を行う。なお、標本抽出で選ぶ粒子の数は一定でもよいし、例えば粒子全体の尤度の分散に応じて、例えば分散が小さいと粒子の数を減らす等、リサンプリング毎に変化させてもよい。

図１１に示す粒子状態４０４における各粒子４ａ〜４ｅは、ステップＳ２３での観測の結果としての尤度を円の大きさで表している。粒子状態４０４で示した、尤度が比較的低い（小さい）粒子４ａ及び粒子４ｅは、ステップＳ２４でのリサンプリングにより再び選択される確率が低く、乱数により結果的に選ばれなかった例を粒子状態４０５で示している。また、粒子状態４０４で示した、尤度が比較的高い（大きい）粒子４ｃは、選択される確率が高く、ステップＳ２４でのリサンプリングで乱数により結果的に３つ選ばれた例を粒子状態４０５で示している。また、粒子状態４０４で示した、尤度が中位の粒子４ｂ及び粒子４ｄは、選択される確率が中位であり、ステップＳ２４でのリサンプリングで乱数により結果的に選ばれた例を粒子状態４０５で示している。時刻ｔにおいてリサンプリングされた粒子は、時刻ｔ＋１においてステップＳ２２での予測により観測ノイズが付加され、例えば粒子状態４０６に示すように粒子５ａ〜５ｅとなる。なお、図１１で時刻ｔ−１、時刻ｔ、時刻ｔ＋１で示した時刻は、例えば、図７のステップＳ２２〜Ｓ２４が繰り返される毎に１進むこととしてもよいし、そのステップＳ２２〜Ｓ２４が一定回数繰り返される毎に１進むこととしてもよい。

また、例えば、測定部１１０により測定点データ群を繰り返し取得する周期と、ステップＳ２２〜Ｓ２４が繰り返される周期との関係により、ステップＳ２３で観測に用いられる測定ボクセルデータは、前回のステップＳ２３での観測で用いられた測定ボクセルデータと同じ内容になり得るし、異なる内容にもなり得る。例えば、測定部１１０により測定点データ群を繰り返し取得する周期と、ステップＳ２２〜Ｓ２４が繰り返される周期とを合わせて、ステップＳ２３で観測に用いられる測定ボクセルデータが、前回のステップＳ２３での観測で用いられた測定ボクセルデータと異なり得るようにしてもよい。

［１−３．効果等］
上述の位置推定装置１００によれば、位置のみならず色を測定し、ボクセルについての位置のみならず色情報を用いて位置推定を行うので、同じような形状の構造物が連続的に配置されているような環境においても、移動体の位置を迅速かつ適切に推定できる可能性が高い。

［１−４．変形例］
以下、上述した位置推定装置１００の一部を変形した変形例について説明する。

［１−４−１．変形例１：位置情報が平面を示す例］
上述の測定ボクセルデータ及び地図ボクセルデータは、いずれも３次元座標（ｘ座標，ｙ座標，ｚ座標）で表される位置（ボクセル内の各点の位置の平均値）を示す位置情報を含むものであり（図５参照）、図８に示すステップＳ３４での位置情報同士の近似度の算定は、例えば、その各位置情報が示す位置（平均位置）同士の間の距離Ｄに基づいて行われ得ることを示した。

変形例として、測定ボクセルデータ及び地図ボクセルデータのうち一方が、ボクセルについての一平面を示す情報であることとしてもよい。この例では、ステップＳ３４での位置情報同士の近似度の算定を、その位置情報同士の一方が示す平均位置（位置の平均値）、つまり点と、他方が示す平面との距離Ｄに基づいて、行い得る。また、ステップＳ３３で、測定ボクセルと最も近い地図ボクセルを特定する場合における位置情報同士の近似度の算定についても同様に、点と平面との距離に基づいて行ってもよい。例えば、測定ボクセルデータは図５に例示するものであり、地図ボクセルデータは図１４に例示するものであってもよい。図１４に示す地図ボクセルデータでは、地図ボクセル内についての各点について測定された３次元座標の平均値と、その分散に基づく３×３（３行３列）の分散共分散行列から算定した第１固有ベクトル及び第２固有ベクトルとを位置情報として含んでいる。第１固有ベクトルと第２固有ベクトルとにより平面は特定できるため、測定ボクセルデータと地図ボクセルデータとの位置情報同士の近似度の算定として、例えば、点と平面との距離Ｄを用いた計算が可能となる。また、第１固有ベクトル及び第２固有ベクトルではなく、３×３（３行３列）の分散共分散行列自体をボクセルデータに位置情報として含ませてもよい。

この変形例では、抽出部１２０が抽出する、局所空間に対応する各測定ボクセルについての測定ボクセルデータにおける位置情報、又は、地図管理部１３０が管理する、空間に対応する各地図ボクセルについての地図ボクセルデータにおける位置情報が、そのボクセル内の各点の位置に基づいて定めた分散共分散行列で特定される平面を示すものである。これにより、位置推定部１４０は、地図ボクセルデータと測定ボクセルデータとの位置情報同士の近似度を、その位置情報同士の一方が示す位置の平均値と他方が示す平面との距離に応じて特定することにより、上述の尤度（つまり粒子及び変換演算の尤度）の算定を行うことができる。

このように、ボクセルデータにおいて平面を示す位置情報を用いることで、位置推定装置１００が位置の推定を、より高精度に行えるようになる可能性がある。

なお、ボクセル内の点群が理想的な平面でない場合も想定し、分散共分散行列から固有値及び固有ベクトルを算出する際に平面度を推定してもよい。例えば、第一、第二固有値が比較的大きく、第三固有値が比較的小さい場合には理想的な平面と推定し、点と平面との距離Ｄを用いてもよいが、第二固有値と第三固有値との差が小さい場合には球体又は立体の可能性があるので、点と点との距離を用いるように切り替えてもよい。

また、地図ボクセルデータと測定ボクセルデータとの位置情報同士の近似度を、点と面との距離に基づいて算定する場合には、平面度に応じて重み付けしてもよい。また、地図ボクセルデータには、分散共分散行列から事前に計算した各ボクセルの平面及び平面度を含ませてもよい。即ち、地図管理部１３０は、地図ボクセルデータとして、平面度を更に管理し得る。

［１−４−２．変形例２：色情報が色のヒストグラムを示す例］
上述の測定ボクセルデータ及び地図ボクセルデータは、いずれもＲＧＢ値で表される色（ボクセル内の各点の色の平均値）を示す色情報を含むものであり（図５参照）、図８に示すステップＳ３５での色情報同士の近似度の算定は、例えば、その各色情報が示す色（平均色）同士の間のＲＧＢ３次元座標系における距離Ｃに基づいて行われ得ることを示した。

ここでは、色情報が、色を複数のカテゴリであるビンに区分した情報（ヒストグラム）であり、色情報同士の近似度を、ヒストグラムの類似度に基づいて算定するようにした変形例を示す。この場合に、ボクセルデータ（測定ボクセルデータ及び地図ボクセルデータ）は、図１５に例示するようなものとなる。図１５の例では、ボクセルデータにおける色情報は、８ビットのＲＧＢ値のＲ成分、Ｇ成分及びＢ成分について６４階調毎のビンに区分した配分（つまり正規化した度数）を示す。

図１６に、色情報の近似度算定の一例を示す。図１６の例えばＤabsolute（ｈ，ｈ’）、ＤEuclidean（ｈ，ｈ’）、或いは、Ｄintersection（ｈ，ｈ’）等のような既知のヒストグラムの比較方法（つまりユークリッド距離を利用する方法、ヒストグラムインターセクション法等）を用いて、測定ボクセルデータにおける色情報のヒストグラムｈと、地図ボクセルデータにおける色情報のヒストグラムｈ’とに基づいて、色情報の近似度を算定できる。なお、測定ボクセルデータにおける色情報のヒストグラムは、Ｒ成分のヒストグラム２１５、Ｇ成分のヒストグラム２１６、及び、Ｂ成分のヒストグラム２１７を結合したものである。また、地図ボクセルデータにおける色情報のヒストグラムは、Ｒ成分のヒストグラム３１５、Ｇ成分のヒストグラム３１６、及び、Ｂ成分のヒストグラム３１７を結合したものである。

この変形例では、抽出部１２０による測定ボクセルデータの抽出は、各測定ボクセルについて、その測定ボクセルについての属性情報（具体的には色情報）を、測定部１１０により測定されたそのボクセル内の各点についての属性（色）に基づくヒストグラムを示すように定めることにより、行われる。そして、地図管理部１３０により管理される各地図ボクセルについての属性情報（具体的には色情報）は、該当の地図ボクセルに対応する空間の一部における複数の点についての、その点からの電磁波の受波で得られる属性（色）に基づくヒストグラムを示す。これにより、位置推定部１４０は、前記属性情報同士（具体的には色情報）の近似度を、その属性情報同士の一方が示すヒストグラムと他方が示すヒストグラムとの距離に応じて特定することにより、上述の尤度（つまり粒子及び変換演算の尤度）の算定を行うことができる。

［１−４−３．変形例３：色情報が局所自己相関を示す例］
ボクセルに係るボクセルデータの色情報が、そのボクセル内の局所的な位置と相関する濃度傾向（つまり局所自己相関）を示す特徴ベクトルで表されるようにした変形例を示す。

図１７は、ボクセルについての色情報の抽出に用いるサブボクセルを示す図である。図１７に示すように、ボクセル２２０を、３×３×３の２７個のサブボクセルに分割する。そして、各サブボクセル内における各点（測定点）の色（ＲＧＢ値）の各成分についての平均値を算出して、例えば０から１までの範囲内の値となるように正規化する。そして、Ｒ、Ｇ、Ｂの各成分についてボクセル２２０の中央のサブボクセル２２１の色の平均値と、そのサブボクセル２２１からそれ以外の各サブボクセル（例えばサブボクセル２２２等）へ向かう方向毎にその方向へのサブボクセルの色の乗算とを計算する。色情報はＲＧＢの３成分あり、サブボクセルの個数は２７個存在するので、３×２７＝８１次元の特徴ベクトルが生成できる。この８１次元の特徴ベクトルを、ボクセル２２０のボクセルデータにおける色情報とする。

この変形例では、抽出部１２０による測定ボクセルデータの抽出は、各測定ボクセルについて、その測定ボクセルについての属性情報（具体的には色情報）を、測定部１１０により測定されたその測定ボクセル内の各点についての位置と属性（色）との相関を示すように定めることにより、行われる。また、地図管理部１３０により管理される各地図ボクセルについての属性情報（具体的には色情報）は、該当の地図ボクセルに対応する空間の一部における複数の点についての、位置と電磁波の受波で得られる属性（色）との相関を示す。

このようにボクセルデータ（測定ボクセルデータ及び地図ボクセルデータ）における色情報を、ボクセル内の色の局所自己相関を示す特徴ベクトルで表す場合においては、図８に示すステップＳ３５での色情報同士の近似度の算定は、例えば、その８１次元の特徴ベクトル同士のユークリッド距離に基づいて、距離が小さいほど近似度が高いように定めた演算（例えば図１２参照）により、行うことができる。

また、上述の変形例とはスケールを変更して（つまり上述の例のサブボクセルをボクセルに置き換えて）、ボクセルに係るボクセルデータの色情報が、そのボクセルを含む局所的なボクセル群（隣接する複数のボクセル）における局所的な位置と相関する濃度勾配（つまり局所自己相関）を示す特徴ベクトルで表されるようにしてもよい。即ち、あるボクセル（ボクセルＡとする）とそのボクセルＡの周囲を囲む隣接する２６個のボクセルとの各々について、ボクセル内における各点（測定点）の色（ＲＧＢ値）の各成分についての平均値を算出して、例えば０から１までの範囲内の値となるように正規化する。そして、Ｒ、Ｇ、Ｂの各成分についてボクセルＡの色の平均値と、そのボクセルＡからそれ以外の各隣接ボクセルへ向かう方向毎にその方向へのボクセルの色の乗算とを特定する。その特定した全ての値を成分とする８１次元の特徴ベクトルを、ボクセルＡのボクセルデータにおける色情報とする。

この例では、抽出部１２０による測定ボクセルデータの抽出は、各測定ボクセルについて、その測定ボクセルについての属性情報（具体的には色情報）を、測定部１１０により測定されたそのボクセル内及びそのボクセルに近接する１以上のボクセル内の、各点についての位置と属性（色）との相関を示すように定めることにより、行われる。また、地図管理部１３０により管理される各地図ボクセルについての属性情報（具体的には色情報）は、該当のボクセルに対応する空間の一部及びそのボクセルに近接する１以上のボクセルに対応する空間の一部における、複数の点についての、位置と電磁波の受波で得られる属性（色）との相関を示す。

このようにボクセルデータ（測定ボクセルデータ及び地図ボクセルデータ）における色情報を、局所的なボクセル群における色の局所自己相関を示す特徴ベクトルで表す場合においても、図８に示すステップＳ３５での色情報同士の近似度の算定は、例えば、その８１次元の特徴ベクトル同士のユークリッド距離に基づいて、距離が小さいほど近似度が高いように定めた演算（例えば図１２参照）により、行うことができる。

ボクセルデータにおいて局所自己相関を示す色情報を用いることで、位置推定装置１００が位置の推定を、より高精度に行えるようになる可能性がある。

［１−４−４．変形例４：大まかな位置推定に色情報を用いない例］
上述の実施の形態では、位置推定部１４０は、ステップＳ２３での観測において常に、ステップＳ３４で測定ボクセルデータ及び地図ボクセルデータにおける位置情報同士の近似度を算定し、更に、ステップＳ３５で測定ボクセルデータ及び地図ボクセルデータにおける属性情報（具体的には色情報）同士の近似度を算定することとした。しかし、大まかな位置推定は、位置情報同士の近似度の算定のみによって粒子の尤度を求め、細かい位置推定においてのみ、位置情報同士の近似度に加えて色情報同士の近似度に基づいて粒子の尤度を求めることとしてもよい。これにより、位置推定を効率的に行うことができる。例えば、ステップＳ２２〜Ｓ２４での予測、観測及びリサンプリングの繰り返しの初回は、位置情報同士の近似度のみによって粒子の尤度を求め、繰り返しの２回目以後において一定条件下で、位置情報同士の近似度のみならず属性情報（具体的には色情報）同士の近似度にも基づいて粒子の尤度を求めることとしてもよい。この一定条件は、例えば、繰り返しの何回目かを定める条件であってもよいし、前回の観測によって算定された尤度が予め定めた閾値を超えたという条件等であってもよい。

［１−４−５．変形例５：特定色を区別する例］
測定ボクセルデータにおける色情報と地図ボクセルデータにおける色情報との（つまり色情報同士の）近似度を算定する場合に、特定色がその近似度の算定結果へ影響する重みを、特定色以外の色とは異ならせるようにする変形例を示す。

この変形例では、抽出部１２０により抽出される測定ボクセルデータにおける色情報は、色を示し、少なくとも特定色を表す特定色成分とその特定色成分以外の他色成分とを区別可能な情報である。また、地図管理部１３０が管理する地図ボクセルデータにおける色情報も、特定色成分と他色成分とを区別可能な情報である。そして、位置推定部１４０は、上述の尤度（つまり粒子及び変換演算の尤度）の算定の基礎とする、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける色情報同士の近似度を、その色情報同士における特定色成分の近似度への影響の度合いと他色成分の近似度への影響の度合いとを異ならせるように算定する。

具体的な一例としては、樹木の色及び木の葉の色に該当する特定色成分（例えばＲＧＢ値の緑（Ｇ）成分、一定の数値範囲の成分等）について、色情報同士の近似度の算定結果への影響を他色成分より低減させるように重み付けする演算により、近似度を算定する例が挙げられる。なお、位置推定部１４０は、特定色成分の重みをゼロにして、つまり特定色成分を除いて、他色成分のみについて色情報同士の近似度を算定することとしてもよい。このように、樹木の色及び木の葉の色を特定色として、色情報同士の近似度の算定結果に与える影響を他色より小さくすることは、樹木や木の葉の態様（外観等）が年月、季節等によって経年変化することで地図ボクセルと測定ボクセルとの適切な照合が妨げられて位置推定の精度が低下することを抑制する効果を、生じ得る。また、樹木の色及び木の葉の色の他に、人間の肌の色等を特定色としてもよい。これは、地図ボクセルには存在しない人間が多数存在する場面で位置推定の精度が低下することを抑制する効果を、生じ得る。

また、更に、位置推定部１４０は、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける色情報同士の近似度を、その両ボクセルのうち測定ボクセルについての位置情報が、所定位置条件を満たす場合には、その測定ボクセルについての色情報における、特定色成分の近似度への影響の度合いと、他色成分の近似度への影響の度合いとを、異ならせるように算定することとしてもよい。この所定位置条件は、例えば、位置のうち鉛直方向の成分（例えばｚ座標）に係る条件である。例えば、所定位置条件を、位置が数メートルより高い場合に満たすという条件とし、特定色成分を青空及び曇り空の色に該当する成分とすると、雲が流れる等により空間における高い位置の部分の態様が時刻によって変化することにより位置推定の精度が低下することを抑制し得る。

また、同じ考え方で、所定位置条件を地面から２ｍまでの高さとし、人間の肌の色等を特定色としてもよい。これは、地図ボクセルには存在しない人間が多数存在する場面で位置推定の精度が低下することを抑制する効果を生じ得る。

また、更に、位置推定部１４０は、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける色情報同士の近似度を、その両ボクセルのうち地図ボクセルについての平面度が特定条件を満たす場合には、その地図ボクセルについての色情報における、特定色成分の近似度への影響の度合いと、他色成分の近似度への影響の度合いとを、異ならせるように算定することとしてもよい。例えば、樹木や木の葉の態様（外観等）が年月、季節等によって経年変化するが、ボクセル化した際の平面度が、位置推定の参考になる平面でなく、球状や立体に近い状態になるので、特定色成分に加え、ボクセル内の平面度に応じて樹木や木の葉を区別してもよい。特定条件として平面度が球状、立体等に相当するものであること等を定めておくことがでる。

（実施の形態２）
実施の形態２では、主に、空間における移動体の位置を推定するために位置の他に、反射強度（電磁波の反射強度）を用いる位置推定方法を実行する位置推定装置１００ａについて説明する。

実施の形態１で示した位置推定装置１００は、電磁波の受波により検知される測定点の属性の一種である色を検知し、位置と色とを用いて位置推定を行ったのに対して、位置推定装置１００ａは、その属性の別の一種である反射強度を検知し、位置と反射強度を用いて位置推定を行う。なお、実施の形態１と同様のものについては本実施形態でも同じ名称を用いる。

［２−１．構成］
位置推定装置１００ａは、図１に示した位置推定装置１００と同様に、例えば測定装置１０１を取り付けた移動体１０２である。ここで特に説明しない点については、位置推定装置１００ａは、実施の形態１の位置推定装置１００と同様である。測定装置１０１は、例えば、レーザーを用いた３Ｄスキャナ等の、周囲の局所空間における複数の点（測定点）の位置、及び、位置推定のために送出した電磁波の反射強度（レーザーの反射強度）を測定する装置である。本実施の形態では測定装置１０１は、実施の形態１とは異なり必ずしも色を測定する機能を有する必要はない。測定点の反射強度は、例えば、波長１０００ｎｍ〜１５５０ｎｍの範囲のレーザー光を測定点に照射したときの反射光（電磁波の一種）の受光強度として測定される。この受光強度が、地面や建物等といった対象物の素材によって変化するのを利用する。

図１８は、位置推定装置１００ａの機能ブロック図である。位置推定装置１００ａは、同図に示すように、測定部１１０ａ、抽出部１２０ａ、地図管理部１３０ａ、及び、位置推定部１４０ａを有する。以下、各機能部について説明する。

測定部１１０ａは、移動体１０２の周囲の局所空間における複数の点（つまり測定点）各々についての、位置と反射強度とを測定する機能を有し、測定により測定結果である測定点データを取得する。測定部１１０ａは、例えば、測定装置１０１、及び、位置推定装置１００ａのコンピュータにおける通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。測定部１１０ａは、反射強度を測定する点で、色を測定する上述の測定部１１０とは測定対象が異なるが、測定対象の差異以外について、ここで特に説明しない点は測定部１１０と同様である。測定部１１０ａにより取得される測定点データは、図１９に例示するように、位置を３次元座標ｘ，ｙ，ｚで表し、反射強度を０〜１の範囲の値で正規化して表したものである。なお、図１９は、一行毎に一測定点についての測定点データを示している。

抽出部１２０ａは、測定部１１０ａによる測定結果（測定点データ）から、移動体１０２の周囲の局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報（具体的には反射強度を表す反射強度情報）を抽出する機能を有する。抽出部１２０ａは、位置推定装置１００ａのコンピュータにおける、制御プログラムを実行するプロセッサ等により実現される。抽出部１２０ａは、測定ボクセルについての測定ボクセルデータとして位置情報及び反射強度情報を抽出する点で、位置情報及び色情報を抽出する上述の抽出部１２０とは取り扱う情報が異なるが、その差異以外について、ここで特に説明しない点は、抽出部１２０と同様である。

抽出部１２０ａによる測定ボクセルデータ（位置情報及び反射強度情報）における反射強度情報の抽出は、例えば、各測定ボクセルについて、その測定ボクセルについての反射強度情報を、測定部１１０ａにより測定されたその測定ボクセル内の各点についての反射強度の平均値を示すように定めることにより、行われる。一例としては、抽出部１２０ａは、測定ボクセル内に含まれる各測定点についての測定点データにおける反射強度（０〜１の範囲の値）を平均化してその測定ボクセルについての測定ボクセルデータにおける反射強度情報を、算定する。図２０は、測定ボクセルデータの一例を示す図であり、一行毎に一測定ボクセルについての測定ボクセルデータを示している。図２０に示すように、測定ボクセルデータは、局所空間に対応付けた測定ボクセル群の３次元配列における該当の測定ボクセルの配置を示すインデックス（ｉｎｄｅｘ）と、その測定ボクセル内の各測定点データの位置の平均値を示す位置情報と、その測定ボクセル内の各測定点データの反射強度の平均値である平均反射強度を示す反射強度情報とを含む。

地図管理部１３０ａは、地図（つまり空間を表す３次元座標系を定めた地図）で表される空間と対応付けて、３次元状に配列した地図ボクセル群における各地図ボクセルについての地図ボクセルデータ、つまり、位置情報及び属性情報（具体的には反射強度を表す反射強度情報）を、管理する機能を有する。地図管理部１３０ａは、例えば、位置推定装置１００ａのコンピュータにおけるメモリ等の記憶媒体、通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。

地図管理部１３０ａは、地図ボクセルデータとして位置情報及び反射強度情報を管理する点で、位置情報及び色情報を管理する上述の地図管理部１３０とは管理する情報が異なるが、その差異以外について、ここで特に説明しない点は、地図管理部１３０と同様である。地図管理部１３０ａは、空間を分割してなる地図ボクセル群における各地図ボクセルについての地図ボクセルデータ（位置情報及び反射強度情報）を含む地図情報を管理している。地図情報は、例えば、空間における各測定点の位置及び属性（具体的には反射強度）を測定してその測定結果に基づいて、各地図ボクセルについての地図ボクセルデータ（位置情報及び反射強度情報）を抽出することで生成される。本実施の形態では、地図ボクセルデータのデータ構成は、図２０に示す測定ボクセルデータと同様であるものとする。即ち、地図ボクセルデータは、地図ボクセルの配置を示すインデックス（ｉｎｄｅｘ）とその地図ボクセルに対応する、空間の一部における各点（つまり各場所）の位置の測定結果の平均値を示す位置情報と、その地図ボクセルに対応する、空間の一部における各点（つまり各場所）についての、その点からの電磁波の受波により検知される属性としての反射強度の測定結果（０〜１の範囲の値）の平均値を示す反射強度情報とを含むものとする。

位置推定部１４０ａは、地図管理部１３０ａが管理する地図情報の地図ボクセルデータ（位置情報、及び、属性情報としての反射強度情報）に基づき、抽出部１２０ａが抽出した測定ボクセルデータ（位置情報、及び、属性情報としての反射強度情報）に応じて、移動体１０２の位置を推定する機能を有する。位置推定部１４０ａは、例えば、位置推定装置１００ａのコンピュータにおけるメモリ、制御プログラムを実行するプロセッサ等により実現される。位置推定部１４０ａは、地図ボクセル及び測定ボクセルに係る位置情報及び反射強度情報を用いて位置推定を行う点で、地図ボクセル及び測定ボクセルに係る位置情報及び色情報を用いて位置推定を行う上述の位置推定部１４０とは取り扱う情報が異なるが、その差異以外について、ここで特に説明しない点は、位置推定部１４０と同様である。

位置推定部１４０ａは、測定ボクセルについての測定ボクセルデータにおける位置情報が、地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定する。この変換演算を特定することにより、移動体１０２の位置が推定される。この移動体１０２の位置を推定する方法、つまり、変換演算を推定的に特定する方法としては、例えば、パーティクルフィルタを用いて予測、観測、及び、リサンプリングの手順を繰り返す方法（図７参照）を用いることができる。位置推定部１４０ａは、地図管理部１３０ａが管理する各地図ボクセルについての位置情報と、候補としての複数の変換演算各々を用いて変換した、抽出部１２０ａが抽出した各測定ボクセルについての位置情報との近似度を算定する。そして、位置推定部１４０ａは、その変換演算毎に、算定した近似度が最も高いところの地図ボクセル及び測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報（つまり反射強度情報）同士の近似度に基づいて、尤度を算定する。候補としての複数の変換演算のうち算定した尤度が最も高い変換演算に応じて、位置推定部１４０ａは、移動体１０２の位置の推定を行う。ここで、位置を示す位置情報同士の近似度或いは反射強度を示す反射強度情報同士の近似度は、位置或いは反射強度が近似する度合いの高さを示し、同一であれば最高値となる。なお、位置推定部１４０ａは、複数の変換演算各々を用いて変換した各測定ボクセルに係る近似度の算定に基づく、尤度の算定及び移動体の位置の推定を、繰り返し行い、尤度の算定結果に基づいて次に変換に用いる複数の変換演算を定める。

［２−２．動作］
以下、上述の構成を備える位置推定装置１００ａの動作（つまり位置推定方法）について説明する。

測定部１１０ａが、移動体１０２の周囲の局所空間における複数の点の位置及び反射強度を測定して測定点データを取得する。

抽出部１２０ａは、上述の抽出部１２０と同様に図６に示すステップＳ１１〜Ｓ１７での処理を行う。但し、ステップＳ１６では、抽出部１２０ａは、色情報の更新の代わりに、ステップＳ１３で取得した測定点データにおける反射強度を用いて、ステップＳ１４で特定した測定ボクセルについての反射強度情報を更新する。これにより、メモリ等の記憶媒体に一時的に保持される、図２０に示すような測定ボクセルデータ（位置情報及び反射強度情報）が、更新される。

位置推定部１４０ａは、測定ボクセルについての測定ボクセルデータにおける位置情報が、地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定することで、推定した変換演算に基づいて位置の推定を行う。このための一手法としてパーティクルフィルタを用いる。即ち、尤度の高い変換演算を推定する手段として、粒子を用いる。粒子毎に変換演算が特定される。

位置推定部１４０ａは、上述の位置推定部１４０と同様に図７及び図８に示すステップＳ２１〜Ｓ２５及びステップＳ３１〜Ｓ３９での処理を行う。但しステップＳ３５では、位置推定部１４０ａは、色情報同士の近似度の代わりに、ステップＳ３３で特定した地図ボクセルと着目中の測定ボクセルとの組に係る地図ボクセルデータにおける反射強度情報と測定ボクセルデータにおける反射強度情報との近似度を算定して、その近似度を尤度合計値に加算する。この両ボクセル（特定した地図ボクセルと着目中の測定ボクセル）についての反射強度情報同士の近似度は、例えば、その両反射強度情報が示す反射強度（０〜１の範囲の値である反射強度の平均値、つまり平均反射強度）間の、差分Ｒに基づいて、差分Ｒが小さいほど近似度が高くなるように予め定めた計算式等に基づいて算定し得る。

図２１は、地図ボクセルについての反射強度情報が示す平均反射強度３３１と測定ボクセルについての反射強度情報が示す平均反射強度２３１との差分Ｒに基づいて近似度としてＲｅｆｌｅｃｔ（Ｒ）を算定する近似度算定の一例を示している。位置推定部１４０ａは、例えば、差分Ｒの各反射強度を示す反射強度情報同士の近似度を、図２１に示す式のように、Ｒｅｆｌｅｃｔ（Ｒ）として算定してもよい。このように、ステップＳ３４で位置情報に係る近似度を尤度合計値に加算し、ステップＳ３５において反射強度情報に係る近似度を尤度合計値に加算したが、これらは、地図ボクセルと測定ボクセルとの間の位置情報と反射強度情報とに係る相関に基づいて最終的に粒子毎の尤度を算定するための一例に過ぎない。ステップＳ３４及びステップＳ３５で位置情報に係る近似度と反射強度情報に係る近似度とを尤度合計値に加算する代わりに、例えば、位置情報に係る近似度と反射強度情報に係る近似度との積を尤度合計値に加算することとしてもよいし、例えば、尤度への影響の重みを位置情報と反射強度情報とで異ならせるために、係数を乗じた位置情報に係る近似度と、反射強度情報に係る近似度とを尤度合計値に加算してもよい。

ステップＳ３９では、位置推定部１４０ａは、最大尤度の粒子の粒子データが示すその粒子の成分（３次元座標及び回転角度）を、位置推定結果として出力することになる。

なお、ステップＳ３９で、位置推定部１４０ａは、最大尤度の粒子の成分の代わりに、各粒子の成分（３次元座標及び回転角度）についての、粒子の尤度で重み付けした加重平均の結果を、位置推定結果として出力することとしてもよい。各粒子の尤度を粒子全体の尤度合計値で除算して正規化することで加重平均を求める。

［２−３．効果等］
上述の位置推定装置１００ａによれば、位置のみならず反射強度を測定し、ボクセルについての位置のみならず反射強度情報を用いて位置推定を行うので、同じような形状の構造物が連続的に配置されているような環境においても、構造物の材質により反射強度が異なるので、移動体の位置を迅速かつ適切に推定できる可能性が高い。

［２−４．変形例］
測定ボクセルデータにおける属性情報としての反射強度情報と地図ボクセルデータにおける属性情報としての反射強度情報との（つまり反射強度情報同士の）近似度を算定する場合に、所定強度より高い反射強度がその近似度の算定結果へ影響する重みを、所定強度より高くない反射強度とは異ならせるようにする変形例を示す。

この変形例では、位置推定部１４０ａは、上述の尤度（つまり粒子及び変換演算の尤度）の算定の基礎とする、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける反射強度情報同士の近似度を、その反射強度情報が所定強度より高い反射強度を示す場合の近似度への影響の度合いと、その反射強度情報が所定強度より高くない反射強度を示す場合の近似度への影響の度合いとを異ならせるように算定する。具体的な一例としては、白又は黄色等の反射シートによる道路表示（路面の白線等）の反射強度より少し低い反射強度を所定強度として定め、所定強度より高い反射強度は所定強度より高くない反射強度に対して近似度に大きく影響するように重み付けする演算により、近似度を算定する例が挙げられる。なお、位置推定部１４０ａは、所定強度より高い反射強度を示す反射強度情報同士以外は、近似度をゼロ或いは略ゼロと算定することとしてもよい。これにより、道路表示に基づく位置推定を精度良く行える可能性がある。

また、更に、位置推定部１４０ａは、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける反射強度情報同士の近似度を、その両ボクセルのうち測定ボクセルについての位置情報が、所定位置条件を満たす場合には、その測定ボクセルについての反射強度情報が所定強度より高い反射強度を示すときの近似度への影響の度合いを、その反射強度情報が所定強度より高くない反射強度を示すときの近似度への影響の度合いより高めるように算定することとしてもよい。この所定位置条件は、例えば、位置のうち鉛直方向の成分（例えばｚ座標）に係る条件である。例えば、所定位置条件を、位置が路面に近い高さの場合に満たすという条件とし、道路表示に基づく位置推定をより適切に行えるようにしてもよい。

（実施の形態３）
実施の形態３では、主に、空間における移動体の位置を推定するために位置の他に、温度を用いる位置推定方法を実行する位置推定装置１００ｂについて説明する。

実施の形態１で示した位置推定装置１００は、電磁波の受波により検知される測定点の属性の一種である色を検知し、位置と色とを用いて位置推定を行ったのに対して、位置推定装置１００ｂは、その属性の別の一種である温度を検知し、位置と温度を用いて位置推定を行う。なお、実施の形態１と同様のものについては本実施形態でも同じ名称を用いる。

［３−１．構成］
位置推定装置１００ｂは、図１に示した位置推定装置１００と同様に、例えば測定装置１０１を取り付けた移動体１０２である。ここで特に説明しない点については、位置推定装置１００ｂは、実施の形態１の位置推定装置１００と同様である。測定装置１０１は、例えば、３Ｄスキャナ及びサーモグラフィー装置の組み合わせ等の、周囲の局所空間における複数の点（測定点）の位置及び温度を測定する装置である。例えば測定装置１０１が３Ｄスキャナ及びサーモグラフィー装置の組み合わせである場合には予め両装置の位置合わせがなされている。本実施の形態では測定装置１０１は、実施の形態１とは異なり必ずしも色を測定する機能を有する必要はない。測定点の温度は、例えば、波長２μｍ〜１４μｍの赤外線（電磁波の一種）を検出することで測定される。この温度が、地面や建物等の対象物の素材により変化するのを利用する。

図２２は、位置推定装置１００ｂの機能ブロック図である。位置推定装置１００ｂは、同図に示すように、測定部１１０ｂ、抽出部１２０ｂ、地図管理部１３０ｂ、及び、位置推定部１４０ｂを有する。以下、各機能部について説明する。

測定部１１０ｂは、移動体１０２の周囲の局所空間における複数の点（つまり測定点）各々についての、位置と温度とを測定する機能を有し、測定により測定結果である測定点データを取得する。測定部１１０ｂは、例えば、測定装置１０１、及び、位置推定装置１００ｂのコンピュータにおける通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。測定部１１０ｂは、温度を測定する点で、色を測定する上述の測定部１１０とは測定対象が異なるが、測定対象の差異以外について、ここで特に説明しない点は測定部１１０と同様である。測定部１１０ｂにより取得される測定点データは、図２３に例示するように、位置を３次元座標ｘ，ｙ，ｚで表し、温度を数値（例えば絶対温度の値）で表したものである。なお、図２３は、一行毎に一測定点についての測定点データを示している。

抽出部１２０ｂは、測定部１１０ｂによる測定結果（測定点データ）から、移動体１０２の周囲の局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報（具体的には温度を表す温度情報）を抽出する機能を有する。抽出部１２０ｂは、位置推定装置１００ｂのコンピュータにおける、制御プログラムを実行するプロセッサ等により実現される。抽出部１２０ｂは、測定ボクセルについての測定ボクセルデータとして位置情報及び温度情報を抽出する点で、位置情報及び色情報を抽出する上述の抽出部１２０とは取り扱う情報が異なるが、その差異以外について、ここで特に説明しない点は、抽出部１２０と同様である。

抽出部１２０ｂによる測定ボクセルデータ（位置情報及び温度情報）における温度情報の抽出は、例えば、各測定ボクセルについて、その測定ボクセルについての温度情報を、測定部１１０ｂにより測定されたその測定ボクセル内の各点についての温度の平均値を示すように定めることにより、行われる。一例としては、抽出部１２０ｂは、測定ボクセル内に含まれる各測定点についての測定点データにおける温度を平均化してその測定ボクセルについての測定ボクセルデータにおける温度情報を、算定する。

図２４は、測定ボクセルデータの一例を示す図であり、一行毎に一測定ボクセルについての測定ボクセルデータを示している。図２４に示すように、測定ボクセルデータは、局所空間に対応付けた測定ボクセル群の３次元配列における該当の測定ボクセルの配置を示すインデックス（ｉｎｄｅｘ）と、その測定ボクセル内の各測定点データの位置の平均値を示す位置情報と、その測定ボクセル内の各測定点データの温度の平均値である平均温度を示す温度情報とを含む。

地図管理部１３０ｂは、地図（つまり空間を表す３次元座標系を定めた地図）で表される空間と対応付けて、３次元状に配列した地図ボクセル群における各地図ボクセルについての地図ボクセルデータ、つまり、位置情報及び属性情報（具体的には温度を表す温度情報）を、管理する機能を有する。地図管理部１３０ｂは、例えば、位置推定装置１００ｂのコンピュータにおけるメモリ等の記憶媒体、通信Ｉ／Ｆ、制御プログラムを実行するプロセッサ等により実現される。地図管理部１３０ｂは、地図ボクセルデータとして位置情報及び温度情報を管理する点で、位置情報及び色情報を管理する上述の地図管理部１３０とは管理する情報が異なるが、その差異以外について、ここで特に説明しない点は、地図管理部１３０と同様である。

地図管理部１３０ｂは、空間を分割してなる地図ボクセル群における各地図ボクセルについての地図ボクセルデータ（位置情報及び温度情報）を含む地図情報を管理している。地図情報は、例えば、空間における各測定点の位置及び属性（具体的には温度）を測定してその測定結果に基づいて、各地図ボクセルについての地図ボクセルデータ（位置情報及び温度情報）を抽出することで生成される。本実施の形態では、地図ボクセルデータのデータ構成は、図２４に示す測定ボクセルデータと同様であるものとする。即ち、地図ボクセルデータは、地図ボクセルの配置を示すインデックス（ｉｎｄｅｘ）とその地図ボクセルに対応する、空間の一部における各点（つまり各場所）の位置の測定結果の平均値を示す位置情報と、その地図ボクセルに対応する、空間の一部における各点（つまり各場所）についての、その点からの電磁波の受波により検知される属性としての温度の測定結果の平均値を示す温度情報とを含むものとする。

位置推定部１４０ｂは、地図管理部１３０ｂが管理する地図情報の地図ボクセルデータ（位置情報、及び、属性情報としての温度情報）に基づき、抽出部１２０ｂが抽出した測定ボクセルデータ（位置情報、及び、属性情報としての温度情報）に応じて、移動体１０２の位置を推定する機能を有する。位置推定部１４０ｂは、例えば、位置推定装置１００ｂのコンピュータにおけるメモリ、制御プログラムを実行するプロセッサ等により実現される。位置推定部１４０ｂは、地図ボクセル及び測定ボクセルに係る位置情報及び温度情報を用いて位置推定を行う点で、地図ボクセル及び測定ボクセルに係る位置情報及び色情報を用いて位置推定を行う上述の位置推定部１４０とは取り扱う情報が異なるが、その差異以外について、ここで特に説明しない点は、位置推定部１４０と同様である。位置推定部１４０ｂは、測定ボクセルについての測定ボクセルデータにおける位置情報が、地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定する。この変換演算を特定することにより、移動体１０２の位置が推定される。この移動体１０２の位置を推定する方法、つまり、変換演算を推定的に特定する方法としては、例えば、パーティクルフィルタを用いて予測、観測、及び、リサンプリングの手順を繰り返す方法（図７参照）を用いることができる。

位置推定部１４０ｂは、地図管理部１３０ｂが管理する各地図ボクセルについての位置情報と、候補としての複数の変換演算各々を用いて変換した、抽出部１２０ｂが抽出した各測定ボクセルについての位置情報との近似度を算定する。そして、位置推定部１４０ｂは、その変換演算毎に、算定した近似度が最も高いところの地図ボクセル及び測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報（つまり温度情報）同士の近似度に基づいて、尤度を算定する。候補としての複数の変換演算のうち算定した尤度が最も高い変換演算に応じて、位置推定部１４０ｂは、移動体１０２の位置の推定を行う。ここで、位置を示す位置情報同士の近似度或いは温度を示す温度情報同士の近似度は、位置或いは温度が近似する度合いの高さを示し、同一であれば最高値となる。なお、位置推定部１４０ｂは、複数の変換演算各々を用いて変換した各測定ボクセルに係る近似度の算定に基づく、尤度の算定及び移動体の位置の推定を、繰り返し行い、尤度の算定結果に基づいて次に変換に用いる複数の変換演算を定める。

［３−２．動作］
以下、上述の構成を備える位置推定装置１００ｂの動作（つまり位置推定方法）について説明する。

測定部１１０ｂが、移動体１０２の周囲の局所空間における複数の点の位置及び温度を測定して測定点データを取得する。

抽出部１２０ｂは、上述の抽出部１２０と同様に図６に示すステップＳ１１〜Ｓ１７での処理を行う。但し、ステップＳ１６では、抽出部１２０ｂは、色情報の更新の代わりに、ステップＳ１３で取得した測定点データにおける温度を用いて、ステップＳ１４で特定した測定ボクセルについての温度情報を更新する。これにより、メモリ等の記憶媒体に一時的に保持される、図２４に示すような測定ボクセルデータ（位置情報及び温度情報）が、更新される。

位置推定部１４０ｂは、測定ボクセルについての測定ボクセルデータにおける位置情報が、地図ボクセルについての地図ボクセルデータにおける位置情報と近似するように、測定ボクセルデータの位置情報を地図に係る３次元座標系に変換するための変換演算を、逐次、推定的に特定することで、推定した変換演算に基づいて位置の推定を行う。このための一手法としてパーティクルフィルタを用いる。即ち、尤度の高い変換演算を推定する手段として、粒子を用いる。粒子毎に変換演算が特定される。位置推定部１４０ｂは、上述の位置推定部１４０と同様に図７及び図８に示すステップＳ２１〜Ｓ２５及びステップＳ３１〜Ｓ３９での処理を行う。但しステップＳ３５では、位置推定部１４０ｂは、色情報同士の近似度の代わりに、ステップＳ３３で特定した地図ボクセルと着目中の測定ボクセルとの組に係る地図ボクセルデータにおける温度情報と測定ボクセルデータにおける温度情報との近似度を算定して、その近似度を尤度合計値に加算する。この両ボクセル（特定した地図ボクセルと着目中の測定ボクセル）についての温度情報同士の近似度は、例えば、その両温度情報が示す温度（つまり平均温度）間の、差分Ｔに基づいて、差分Ｔが小さいほど近似度が高くなるように予め定めた計算式等に基づいて算定し得る。

図２５は、地図ボクセルについての温度情報が示す平均温度３４１と測定ボクセルについての温度情報が示す平均温度２４１との差分Ｔに基づいて近似度としてＴｅｍｐ（Ｔ）を算定する近似度算定の一例を示している。位置推定部１４０ｂは、例えば、差分Ｔの各温度を示す温度情報同士の近似度を、図２５に示す式のように、Ｔｅｍｐ（Ｔ）として算定してもよい。このように、ステップＳ３４で位置情報に係る近似度を尤度合計値に加算し、ステップＳ３５において温度情報に係る近似度を尤度合計値に加算したが、これらは、地図ボクセルと測定ボクセルとの間の位置情報と温度情報とに係る相関に基づいて最終的に粒子毎の尤度を算定するための一例に過ぎない。ステップＳ３４及びステップＳ３５で位置情報に係る近似度と温度情報に係る近似度とを尤度合計値に加算する代わりに、例えば、位置情報に係る近似度と温度情報に係る近似度との積を尤度合計値に加算することとしてもよいし、例えば、尤度への影響の重みを位置情報と温度情報とで異ならせるために、係数を乗じた位置情報に係る近似度と、温度情報に係る近似度とを尤度合計値に加算してもよい。

ステップＳ３９では、位置推定部１４０ｂは、最大尤度の粒子の粒子データが示すその粒子の成分（３次元座標及び回転角度）を、位置推定結果として出力することになる。

なお、ステップＳ３９で、位置推定部１４０ｂは、最大尤度の粒子の成分の代わりに、各粒子の成分（３次元座標及び回転角度）についての、粒子の尤度で重み付けした加重平均の結果を、位置推定結果として出力することとしてもよい。各粒子の尤度を粒子全体の尤度合計値で除算して正規化することで加重平均を求める。

［３−３．効果等］
上述の位置推定装置１００ｂによれば、位置のみならず温度を測定し、ボクセルについての位置のみならず温度情報を用いて位置推定を行うので、同じような形状の構造物が連続的に配置されているような環境においても、構造物の材質により温度が異なるので、移動体の位置を迅速かつ適切に推定できる可能性が高い。

［３−４．変形例］
測定ボクセルデータにおける属性情報としての温度情報と地図ボクセルデータにおける属性情報としての温度情報との（つまり温度情報同士の）近似度を算定する場合に、人間の体温に相当する所定温度範囲内の温度を示すときより、所定温度範囲外の温度を示すときにはその近似度の算定結果へ影響の度合いを高めるようにする変形例を示す。

この変形例では、位置推定部１４０ｂは、上述の尤度（つまり粒子及び変換演算の尤度）の算定の基礎とする、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける温度情報同士の近似度を、その温度情報が人間の体温に相当する温度範囲（例えば絶対温度３０８〜３１３Ｋ）内を示す場合の近似度への影響の度合いと、その温度情報がその温度範囲外を示す場合の近似度への影響の度合いとを異ならせるように算定する。具体的な一例としては、人間の体温に相当する温度範囲内について、その温度範囲外に対して近似度に大きく影響しないように重み付けする演算により、近似度を算定する例が挙げられる。なお、位置推定部１４０ｂは、人間の体温に相当する温度範囲の外を示す温度情報同士以外は、近似度をゼロ或いは略ゼロと算定することとしてもよい。このように人間の体温に相当する温度範囲を区別して、時刻によって移動する人間を測定結果から除くことで、地図ボクセルと測定ボクセルとの適切な照合が妨げられて位置推定の精度が低下することを抑制する効果を、生じ得る。

また、更に、位置推定部１４０ｂは、測定ボクセル及び地図ボクセルに係る測定ボクセルデータ及び地図ボクセルデータにおける温度情報同士の近似度を、その両ボクセルのうち測定ボクセルについての位置情報が、所定位置条件を満たす場合には、その測定ボクセルについての温度情報が人間の体温に相当する温度範囲を示すときの近似度への影響の度合いを、その温度情報がその温度範囲の外を示すときの近似度への影響の度合いより低減するように算定することとしてもよい。この所定位置条件は、例えば、位置のうち鉛直方向の成分（例えばｚ座標）に係る条件である。例えば、所定位置条件を、概ね人間が所在し得る、位置が２メートル以下の高さの場合に満たすという条件としてもよい。これにより、例えば、２メートルより高い場所における、人間の体温に相当する温度範囲の温度の測定結果を、位置推定において有効に利用し得る。

（他の実施の形態）
以上のように、本出願において開示する技術の例示として、実施の形態１〜３を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略等を行った実施の形態にも適用可能である。

上述の実施の形態１の位置推定装置１００に係る変形例１〜４の各々は、実施の形態２或いは実施の形態３に係る位置推定装置１００ａ、１００ｂにも適用し得る。このうち変形例２〜４の適用においては、例えば属性情報としての色情報を反射強度情報或いは温度情報に、属性としての色を反射強度或いは温度に、それぞれ置き換えるとよい。

また、上述の実施の形態では、測定装置１０１が移動体１０２に取り付けられる例を示したが、移動体１０２の周囲に所在するものであれば必ずしも取り付けられなくてもよい。

また、上述の実施の形態では、位置推定装置１００、１００ａ、１００ｂは、測定装置１０１を取り付けた移動体１０２であることとしたがこれは一例に過ぎず、位置推定装置１００、１００ａ、１００ｂは移動体１０２の一部分（或いは移動体１０２内に搭載される装置）と測定装置１０１とで構成されるものとしてもよいし、移動体１０２に付随した測定装置１０１と、移動体１０２から離れた、測定装置１０１から測定結果を受信可能なコンピュータとで構成されていてもよい。

また、上述の実施の形態で示した色は、必ずしも可視光の波長範囲の全域で検知される色である必要はなく、例えばフィルタを用いて一部分に限定した光で検知されるものであってもよい。また、色は、必ずしもカラーである必要はない。

また、上述の実施の形態で示した地図ボクセルを、異なる時刻で複数回作成し、地図ボクセルについての各種情報の時間変化の割合を、地図ボクセルデータに含ませてもよい。例えば、各地図ボクセルについて、位置情報だけでなく、温度や電磁波の反射強度等といった属性情報の時間変化を持たせることで、地面、建物等の区別、或いは、樹木、木の葉といった経年変化する対象物の区別が可能になる。一例として、樹木や木の葉等の経年変化する対象物は、位置情報が時間変化する。また、一例として、アスファルト、コンクリート、芝生の順番で気温変化に対する温度変化が大きい現象を利用できる。また一例として、壁面や道路面に草等の植物が生息する場合に、位置情報はあまり変化しないが、植物の面積に応じて、レーザー等の反射強度が変化する。

また、上述の実施の形態では、測定ボクセルと地図ボクセルとを対応付ける変換演算として適切なものを尤度に基づき推定するために、パーティクルフィルタを用いる例を示したが、これは一手法に過ぎない。例えば、２つの点群の間で、対応する点の組を求めて、その対応する点の距離の最小二乗和に基づいて位置を求めるＩＣＰ（Iterative Closest Point）アルゴリズムを用いてもよい。

また、上述の実施の形態で示した地図情報は、地図ボクセルデータのみで構成されていてもよいし、測定した測定点データ等を含んで構成されていてもよい。

また、位置推定装置１００、１００ａ、１００ｂにおける各機能構成要素（機能ブロック）は、ＩＣ（Integrated Circuit）、ＬＳＩ（Large Scale Integration）等の半導体装置により個別に１チップ化されてもよいし、一部又は全部を含むように１チップ化されてもよい。また、集積回路化の手法はＬＳＩに限るものではなく、専用回路又は汎用プロセッサで実現してもよい。ＬＳＩ製造後に、プログラムすることが可能なＦＰＧＡ（Field Programmable Gate Array）や、ＬＳＩ内部の回路セルの接続や設定を再構成可能なリコンフィギュラブル・プロセッサを利用してもよい。更には、半導体技術の進歩又は派生する別技術によりＬＳＩに置き換わる集積回路化の技術が登場すれば、その技術を用いて機能ブロックの集積化を行ってもよい。バイオ技術の適用等が可能性としてあり得る。

また、上述した各種処理（例えば図６〜８に示す手順等）の全部又は一部は、電子回路等のハードウェアにより実現されても、ソフトウェアを用いて実現されてもよい。なお、ソフトウェアによる処理は、位置推定装置に含まれるプロセッサがメモリに記憶された制御プログラムを実行することにより実現されるものである。また、その制御プログラムを記録媒体に記録して頒布や流通させてもよい。例えば、頒布された制御プログラムを位置推定装置にインストールして、その位置推定装置のプロセッサに実行させることで、その位置推定装置に各種処理（図６〜８に示す手順等）を行わせることが可能となる。この位置推定装置は、例えば測定装置を含まなくてもよく、測定装置による測定結果を取得して位置推定を行うコンピュータであってもよい。

また、上述した実施の形態で示した構成要素及び機能を任意に組み合わせることで実現される形態も本開示の範囲に含まれる。

本開示は、移動体の位置情報を推定する装置において適用可能である。

１ａ、１ｂ建物
４ａ〜４ｅ、５ａ〜５ｅ粒子
１００、１００ａ、１００ｂ位置推定装置
１０１測定装置
１０２移動体
１１０、１１０ａ、１１０ｂ測定部
１２０、１２０ａ、１２０ｂ抽出部
１３０、１３０ａ、１３０ｂ地図管理部
１４０、１４０ａ、１４０ｂ位置推定部
２０１ａ、２０１ｂ測定点データ
２０２ａ、２０２ｂ、２１１、２２０、３１１ボクセル
２０３ａ、２０３ｂボクセルデータ
２１５〜２１７、３１５〜３１７ヒストグラム
２２１、２２２サブボクセル
４０２〜４０６粒子状態

本開示における位置推定装置は、空間における移動体の位置を推定する位置推定装置であって、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定部と、前記測定部による測定結果から、前記局所空間と対応付けて３次元状に配列した測定ボクセル群における各測定ボクセルについての位置情報及び属性情報を抽出する抽出部と、地図で表される前記空間と対応付けて３次元状に配列した地図ボクセル群における各地図ボクセルについての位置情報及び属性情報を管理する地図管理部と、前記地図管理部が管理する前記地図ボクセル及び前記抽出部が抽出した前記測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報同士の近似度に基づいて、前記移動体の位置を推定する位置推定部とを備える。

また、本開示における位置推定方法は、空間における移動体の位置を推定する位置推定装置において用いられる位置推定方法であって、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定ステップと、前記測定ステップでの測定結果から、前記局所空間と対応付けて３次元状に配列した測定ボクセル群における各測定ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、地図で表される前記空間と対応付けて３次元状に配列した地図ボクセル群における各地図ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、前記地図管理ステップで管理する前記地図ボクセル及び前記抽出ステップで抽出した前記測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報同士の近似度に基づいて、前記移動体の位置を推定する位置推定ステップとを含む。

また、本開示における制御プログラムは、空間における移動体の位置を推定する、マイクロプロセッサを備える位置推定装置に位置推定処理を実行させるための制御プログラムであって、前記位置推定処理は、前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性との、測定結果を取得する取得ステップと、前記取得ステップで取得された測定結果から、前記局所空間と対応付けて３次元状に配列した測定ボクセル群における各測定ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、地図で表される前記空間と対応付けて３次元状に配列した地図ボクセル群における各地図ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、前記地図管理ステップで管理する前記地図ボクセル及び前記抽出ステップで抽出した前記測定ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報同士の近似度に基づいて、前記移動体の位置を推定する位置推定ステップとを含む。

Claims

空間における移動体の位置を推定する位置推定装置であって、
前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定部と、
前記測定部による測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出部と、
地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理部と、
前記地図管理部が管理する前記位置情報及び前記属性情報に基づき、前記抽出部が抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定部とを備える
位置推定装置。
前記位置推定部は、
前記地図管理部が管理する前記空間に対応する前記各ボクセルについての位置情報と、前記局所空間の３次元座標系を前記空間の３次元座標系に変換するための複数の変換演算各々を用いて変換した、前記抽出部が抽出した前記局所空間に対応する前記各ボクセルについての位置情報との近似度を算定し、
前記変換演算毎に、算定した前記近似度が最も高いところの前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における両ボクセルについての位置情報同士の近似度及び属性情報同士の近似度に基づいて尤度を算定し、
前記複数の変換演算のうち算定した前記尤度が最も高い変換演算又は算定した前記尤度に応じて加重平均した変換演算に応じて前記移動体の位置の前記推定を行う
請求項１記載の位置推定装置。
前記位置推定部は、前記複数の変換演算各々を用いて変換した前記局所空間に対応する前記各ボクセルに係る前記近似度の前記算定に基づく、前記尤度の前記算定及び前記移動体の位置の前記推定を、繰り返し行い、前記尤度の算定結果に基づいて次に前記変換に用いる前記複数の変換演算を定める
請求項２記載の位置推定装置。
前記位置推定部は、
前記地図管理部が管理する前記空間に対応する前記各ボクセルについての位置情報と、前記局所空間の３次元座標系を前記空間の３次元座標系に変換するための複数の変換演算各々を用いて変換した、前記抽出部が抽出した前記局所空間に対応する前記各ボクセルについての位置情報との近似度を算定し、
前記変換演算毎に、算定した前記近似度が最も高いところの前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における両ボクセルについての位置情報同士の近似度に基づいて尤度を算定し、
前記複数の変換演算のうち算定した尤度が最も高い変換演算又は算定した尤度に応じて加重平均した変換演算に応じて前記移動体の位置の前記推定を行い、
前記複数の変換演算各々を用いて変換した前記局所空間に対応する前記各ボクセルに係る前記近似度の前記算定に基づく、前記尤度の前記算定及び前記移動体の位置の前記推定を、繰り返し行い、前記尤度の算定結果に基づいて次に前記変換に用いる前記複数の変換演算を定め、当該繰り返しの初回以外において一定条件下で、前記尤度の前記算定を、前記両ボクセルについての前記位置情報同士の前記近似度に加えて当該両ボクセルについての前記属性情報同士の近似度にも基づいて、行う
請求項１記載の位置推定装置。
前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルの位置情報を、前記測定部により測定された当該ボクセル内の各点についての位置の平均値を示すように定めることにより、行われ、
前記地図管理部により管理される前記各ボクセルについての前記位置情報は、該当のボクセルに対応する前記空間の一部における複数の点についての位置の平均値を示す
請求項２〜４のいずれか一項に記載の位置推定装置。
前記抽出部が抽出する、前記局所空間に対応する前記各ボクセルについての前記位置情報、又は、前記地図管理部が管理する、前記空間に対応する前記各ボクセルについての前記位置情報は、更に、当該ボクセル内の各点の位置に基づいて定めた分散共分散行列で特定される平面を示し、
前記位置推定部は、前記位置情報同士の近似度を、当該位置情報同士の一方が示す位置の平均値と他方が示す平面との距離に応じて特定することにより、前記尤度の前記算定を行う
請求項５記載の位置推定装置。
前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての属性に基づくヒストグラムを示すように定めることにより、行われ、
前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、当該点からの電磁波の受波で検知される属性に基づくヒストグラムを示し、
前記位置推定部は、前記属性情報同士の近似度を、当該属性情報同士の一方が示す前記ヒストグラムと他方が示す前記ヒストグラムとの距離に応じて特定することにより、前記尤度の前記算定を行う
請求項２〜６のいずれか一項に記載の位置推定装置。
前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての属性の平均値を示すように定めることにより、行われ、
前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、当該点からの電磁波の受波で検知される属性の平均値を示す
請求項２〜６のいずれか一項に記載の位置推定装置。
前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内の各点についての位置と属性との相関を示すように定めることにより、行われ、
前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部における複数の点についての、位置と電磁波の受波で検知される属性との相関を示す
請求項２〜６のいずれか一項に記載の位置推定装置。
前記抽出部による前記抽出は、前記各ボクセルについて、当該ボクセルについての属性情報を、前記測定部により測定された当該ボクセル内及び当該ボクセルに近接する１以上のボクセル内の、各点についての位置と属性との相関を示すように定めることにより、行われ、
前記地図管理部により管理される前記各ボクセルについての前記属性情報は、該当のボクセルに対応する前記空間の一部及び当該ボクセルに近接する１以上のボクセルに対応する前記空間の一部における、複数の点についての、位置と電磁波の受波で検知される属性との相関を示す
請求項２〜６のいずれか一項に記載の位置推定装置。
前記測定部が測定する前記属性は色であり、
前記抽出部が抽出する前記属性情報は、色を表す色情報であり、
前記地図管理部が管理する前記属性情報は、色を表す色情報である
請求項２〜１０のいずれか一項に記載の位置推定装置。
前記抽出部が抽出する前記色情報及び前記地図管理部が管理する前記色情報のそれぞれは、特定色成分と当該特定色成分以外の他色成分とを区別可能な情報であり、
前記位置推定部は、
前記尤度の前記算定の基礎とする、前記両ボクセルについての前記属性情報同士の前記近似度を、
当該属性情報同士における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定する
請求項１１記載の位置推定装置。
前記抽出部が抽出する前記色情報及び前記地図管理部が管理する前記色情報のそれぞれは、特定色成分と当該特定色成分以外の他色成分とを区別可能な情報であり、
前記位置推定部は、
前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、
当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が、所定位置条件を満たす場合には、前記局所空間に係る前記ボクセルについての前記属性情報における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定する
請求項１１記載の位置推定装置。
前記地図管理部は、前記地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての平面度を管理し、
前記位置推定部は、
前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、
当該両ボクセルのうち前記空間に係る前記ボクセルについての前記平面度が特定条件を満たす場合には、前記空間に係る前記ボクセルについての前記属性情報における前記特定色成分の当該近似度への影響の度合いと前記他色成分の当該近似度への影響の度合いとを異ならせるように算定する
請求項１１記載の位置推定装置。
前記測定部が測定する前記属性は電磁波の反射強度であり、
前記抽出部が抽出する前記属性情報は、反射強度を表し、
前記地図管理部が管理する前記属性情報は、反射強度を表す
請求項２〜１０のいずれか一項に記載の位置推定装置。
前記位置推定部は、
前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、
当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が所定位置条件を満たす場合において、前記局所空間に係る前記ボクセルについての前記属性情報が所定強度より高い反射強度を示すときには、当該属性情報が前記所定強度より高くない反射強度を示すときより、当該近似度への影響の度合いを高めるように算定する
請求項１５記載の位置推定装置。
前記測定部が測定する前記属性は温度であり、
前記抽出部が抽出する前記属性情報は、温度を表し、
前記地図管理部が管理する前記属性情報は、温度を表す
請求項２〜１０のいずれか一項に記載の位置推定装置。
前記位置推定部は、
前記尤度の前記算定の基礎とする、前記空間に係る前記ボクセル及び前記局所空間に係る前記ボクセルの各組における前記両ボクセルについての前記属性情報同士の前記近似度を、
当該両ボクセルのうち前記局所空間に係る前記ボクセルについての前記位置情報が所定位置条件を満たす場合において、前記局所空間に係る前記ボクセルについての前記属性情報が人間の体温に相当する所定温度範囲内の温度を示すときより、当該属性情報が前記所定温度範囲外の温度を示すときには、当該近似度への影響の度合いを高めるように算定する
請求項１７記載の位置推定装置。
前記位置推定部は、推定した前記移動体の位置を示す情報を出力する
請求項１〜１８のいずれか一項に記載の位置推定装置。
空間における移動体の位置を推定する位置推定装置において用いられる位置推定方法であって、
前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性とを、測定する測定ステップと、
前記測定ステップでの測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、
地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、
前記地図管理ステップで管理する前記位置情報及び前記属性情報に基づき、前記抽出ステップで抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定ステップとを含む
位置推定方法。
空間における移動体の位置を推定する、マイクロプロセッサを備える位置推定装置に位置推定処理を実行させるための制御プログラムであって、
前記位置推定処理は、
前記移動体の周囲の局所空間における複数の点各々についての、位置と、当該点からの電磁波の受波で検知される属性との、測定結果を取得する取得ステップと、
前記取得ステップで取得された測定結果から、前記局所空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を抽出する抽出ステップと、
地図で表される前記空間と対応付けて３次元状に配列したボクセル群における各ボクセルについての位置情報及び属性情報を管理する地図管理ステップと、
前記地図管理ステップで管理する前記位置情報及び前記属性情報に基づき、前記抽出ステップで抽出した前記位置情報及び前記属性情報に応じて、前記移動体の位置を推定する位置推定ステップとを含む
制御プログラム。