JP5892663B2 - 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体 - Google Patents

Description

本発明は、移動体の自己位置を推定する技術に関し、特に、未知環境下で動作する移動体の自己位置を推定する技術に関するものである。

未知環境内をロボットが移動するためには、環境を計算機上にモデリングする必要がある。未知環境をモデリングする手法として、Simultaneous Localization and Mapping（ＳＬＡＭ）が知られている（例えば、非特許文献１）。これは、未知環境下での相対的自己位置推定と地図生成を同時に行うことによって環境のモデリングを行う。ＳＬＡＭでは周囲の環境は静的であるという仮定に基づくものが多く、動的環境下では、マッチング誤差が発生してしまうという問題があった。

そのため、作業領域を人と共有するような実環境下でロボットを運用する場合、人の往来など動的な環境変化に対応する必要がある。動的環境下で利用可能なＳＬＡＭとして、外れ値を考慮したランドマークに基づくアプローチが提案されている（例えば、非特許文献２）。

しかしながら、非特許文献２の手法には十分な数のランドマークが観察される必要性や、動的障害物による隠れの問題がある。

また、センシング対象の形状（非特許文献３）や、フレーム間の最近傍点距離（非特許文献４）によって、静止物体と移動物体とを区別し、マッチングを行う手法も提案されている。

しかしながら、非特許文献３、４の手法は、測定精度が移動物体かどうかを判定する判別器の性能に強く依存するという問題がある。

また、レーザレンジファインダで計測される２次元の位置情報のように、複数の計測点により構成された位置情報同士のマッチングを行う手法として、Ｌ２ノルムを最小化する手法であるIterative Closest Point(ICP)も知られている（非特許文献５）。

図２３は、ＩＣＰを用いた場合のマッチングの誤差を示した図である。図２３においては、ロボットの移動面を示す２次元の座標空間に、ロボットに搭載された位置センサにより計測された計測点と、移動体の周囲に存在する物体の実際の位置とが重ね合わせてプロットされている。そして、図２３の円の点線で囲んだ領域に計測点と実際の位置との誤差が生じている。

このように、非特許文献５に示す手法では、図２３に示すように、動的な環境下において多くのマッチング誤差が発生してしまう。

S.Thrunet al. : "Simultaneous Mapping and Localization with Sparse ExtendedInformation Filters: Theory and Initial Results", Algorithmic Foundations ofRobotics V, Vol.7, pp.363-380, 2003. Denis F Wolf, Gaurav s Sukhatme : "Mobile Robot Simultaneous Localization and Mapping inDynamic Environments" A. Ess, B. Leibe, K. Schindler, L. vanGool : "Moving ObstacleDetection in Highly Dynamic Scenes", ICRA09, 2009. 識名拓, 油田信一: "多く通行人がいる廊下環境での移動ロボットによる地図作成", ROBOMEC2010，1A2-D29, June 14-16, 2010. Sebastian Thrun , Wolfram Burgard , Dieter Fox : "Probabilistic robotics", The MIT Press,2005.

本発明の目的は、移動物体が存在する未知環境下において移動体の自己位置を精度良く推定することができる技術を提供することである。

本発明の一局面による自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、前記位置情報取得部により第１時刻で取得された第１位置情報を、前記第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの前記第１位置情報と前記第２位置情報との一致度を算出する一致度算出部と、前記第１位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部とを備え、前記一致度算出部は、前記第１位置情報を構成するある計測点に対し、一定距離内に前記第２位置情報を構成するある計測点が存在すれば、前記一致度に所定のポイントを付与し、付与したポイントの合計値を前記一致度として算出する。

また、本発明の別の一局面による移動体は、上記の自己位置推定装置と、前記位置情報を取得する位置センサとを備えている。また、本発明の更に別の一局面による自己位置推定方法及び自己位置推定プログラムは上記の自己位置推定装置と同じ特徴を備えている。

本発明の実施の形態による自己位置推定装置が適用された移動体のブロック図である。 本発明の実施の形態による自己位置推定装置の動作を示すフローチャートである。 図２のＳ２に示す探索処理の詳細を示すフローチャートである。 評価値の算出処理の詳細を示すフローチャートである。 （Ａ）は評価値の算出手法を説明する概念図である。（Ｂ）は第１位置情報を構成する計測点ａ_１〜ａ_３が回転移動される様子を示している。 評価値としてＬ２ノルムを採用した場合において、動的環境下で時刻ｔ−１の位置情報と時刻ｔの位置情報とにおいてマッチング誤差が生じる理由を示した図である。 （Ａ）はＳＩＣＫのＬＳＭ１００の外観図である。（Ｂ）は本発明の実施の形態による移動体の外観図である。 位置センサの仕様を示した表である。 探索候補が生成される処理を説明する図である。 ハッシュテーブルを示した概念図である。 （Ａ）は実験場所を示した図であり、（Ｂ）はこの実験場所に人物が行き来する動的環境を示した図である。 実験場所に人物が行き来していない静的環境下で生成した周辺空間の２次元マップを示している。 評価値として、Ｌ２ノルム、Ｍ−ｅｓｔｉｍａｔｏｒ、及びＬ０ノルムを用いた場合の実験結果を示した表である。 Ｌ２ノルムを採用した場合に作成した２次元マップである。 Ｍ−ｅｓｔｉｍａｔｏｒを採用した場合に作成した２次元マップである。 Ｌ０ノルムを採用した場合に作成した２次元マップである。 Ｂｒｕｔｅ−ｆｏｒｃｅ、ｋｄ−ｔｒｅｅ、ＬＳＨを用いて計算時間を比較する実験を行った場合の実験結果をまとめた表である。 静的環境下でＲＢＰＦ−ＳＬＡＭを用いて生成した２次元マップである。 動的環境下で移動体を移動させながら計測した位置情報を利用し、Ｌ０ノルムを用いた評価値Ｅで位置情報同士のマッチングを行って生成した２次元マップである。 Ｌ２ノルムを用いた評価値で位置情報同士のマッチングを行って生成した２次元マップである。 ＲＢＰＦ−ＳＬＡＭを用いて生成した２次元マップである。 本実施の形態による自己位置推定装置を用いて生成された２次元マップである。 ＩＣＰを用いた場合のマッチングの誤差を示した図である。

以下、本発明の実施の形態による自己位置推定装置について説明する。図１は、本発明の実施の形態による自己位置推定装置が適用された移動体のブロック図である。自己位置推定装置は、位置情報取得部１１、一致度算出部１２、推定部１３、マップ生成部１４、及びオドメトリ算出部１５を備えている。そして、自己位置推定装置は、位置センサ１６により取得された位置情報に基づき、移動体の自己位置を推定する。移動体は、自己位置推定装置、位置センサ１６、及び回転量センサ１７を備えている。

本実施の形態では、自己位置推定装置は例えばＣＰＵ、ＲＯＭ、ＲＡＭ、及びハードディスク等を備えるコンピュータにより構成されている。そして、位置情報取得部１１、一致度算出部１２、推定部１３、マップ生成部１４、及びオドメトリ算出部１５は、ハードディスクに格納された自己位置推定プログラムを例えばＣＰＵが実行することで実現される。なお、自己位置推定プログラムは、ＤＶＤ−ＲＯＭ等のコンピュータ読み取り可能な記録媒体に記録されてユーザに提供される。ユーザはこの記録媒体をコンピュータにインストールすることで、コンピュータを自己位置推定装置として機能させる。

位置情報取得部１１は、位置センサ１６により計測された移動体の周辺空間に存在する各物体の位置情報を一定の周期で時系列に取得する。ここで、位置情報は、物体が存在する箇所を示す計測点から構成されている。本実施の形態では、位置センサ１６として、レーザレンジファインダが採用されている。そのため、計測点は、位置センサ１６の位置を原点とし、位置センサ１６の正面方向を基準方向とする２次元のローカル座標空間において、その基準方向からの角度と、原点からの距離とを含む２次元の極座標データにより表される。以下の説明では、ローカル座標空間は、基準方向にｙ軸が設定され、原点を通り、かつ、基準方向に直交する方向にｘ軸が設定されているものとする。

一致度算出部１２は、位置情報取得部１１により第１時刻で取得された第１位置情報を、第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの第１位置情報と第２位置情報との一致度を算出する。本実施の形態では、第１時刻を時刻ｔとし、第２時刻を時刻ｔ−１として説明するが、これに限定されず、第１時刻を時刻ｔ−１とし、第２時刻を時刻ｔとしてもよい。また、時刻ｔとは、位置センサが時系列に取得する位置情報の取得タイミングを示す。また、必要に応じて、位置情報の取得タイミングをフレームと記述する。

また、本実施の形態では、時刻ｔの位置情報と時刻ｔ−１の位置情報との一致の度合いを評価する評価値を用いて一致度を表す。具体的には、評価値Ｅは、式（１）により表される。

Ｅ（Ｒ，Ｔ）＝Σ_ｉ＝１ ^ｎｆ（Ｒａ_ｉ＋Ｔ，｛ｂ_ｊ｝） （１）
ｆ（ａ_ｉ，｛ｂ_ｊ｝）＝０（∋ｊ，｜ａ_ｉ−ｂ_ｊ｜≦ε） or 1 (otherwise)

但し、ａ_ｉは時刻ｔの位置情報の各計測点を表し、ｂ_ｊは時刻ｔ−１の位置情報の各計測点を表している。また、Ｒは時刻ｔ−１を基準としたときの時刻ｔの各計測点の回転移動量を示し、Ｔは時刻ｔ−１を基準としたときの時刻ｔの各計測点の平行移動量を示す。ｉは時刻ｔの位置情報の計測点を特定するためのインデックスであり、ｊは時刻ｔ−１の位置情報の計測点を特定するためのインデックスでる。εはレーザレンジファインダの精度を考慮した距離である。ｎは計測点の個数を示している。

式（１）は、計測点ａ_ｉをＲ回転移動させ、かつ、Ｔ平行移動させたときにおいて、計測点ａ_ｉが計測点ｂ_ｊを中心として距離ε以内に存在しなかった場合、評価値Ｅに１のペナルティが課されると解釈することができる。したがって、式（１）では評価値Ｅが小さいほど時刻ｔ−１の位置情報と時刻ｔの位置情報との一致度が高いことを示す。

なお、式（１）において、ｆ（ａ_ｉ，｛ｂ_ｊ｝）＝１ （∋ｊ，｜ａ_ｉ−ｂ_ｊ｜≦ε） ｏｒ ０ （otherwise）としてもよい。この場合、計測点ａ_ｉが計測点ｂ_ｊを中心として距離ε以内に存在する場合、評価値Ｅに１ポイントが付与され、一致度が高いほど評価値Ｅが大きくなる。

式（１）の計算では、明示的に対応点を決定する必要はなく、単に、計測点ｂ_ｊを中心として、距離ε内に計測点ａ_ｉが存在するか否かを判定すればよい。つまり、式（１）ではＬ０ノルムを用いて評価値Ｅが定義されている。このようにして、計測点ｂ_ｊに対して距離ε内に計測点ａ_ｉが存在するか否かを探索する手法はｒ近傍探索と呼ばれている。

なお、ノルムとは距離を意味し、Ｌ０ノルムの他、Ｌ２ノルム等が存在する。Ｌ２ノルムは距離の２乗和であり、従来より広く用いられている。

図５（Ａ）は、評価値Ｅの算出手法を説明する概念図である。図５（Ａ）に示すように、時刻ｔ−１の位置情報を構成するある計測点ｂに対し、時刻ｔの位置情報を構成するある計測点ａ_２は距離ε内に位置している。よって、式（１）はｆ（ｘ）＝０となり、評価値Ｅにはペナルティが課されない。一方、時刻ｔの位置情報を構成する別の計測点ａ_１は計測点ｂに対し、距離ε内に位置していない。よって、式（１）はｆ（ｘ）＝１となり、評価値Ｅには１のペナルティが課される。

図５（Ｂ）は、時刻ｔの位置情報を構成する計測点ａ_１〜ａ_３が回転移動される様子を示している。図５（Ｃ）は、回転移動された計測点ａ_１〜ａ_３と計測点ｂ_１〜ｂ_３とにおいて評価値Ｅが算出される様子を示した図である。

図５（Ｃ）では計測点ｂ_１，ｂ_２から距離ε内に計測点ａ_１，ａ_２が位置しているが、計測点ｂ_３から距離ε内に計測点ａ_３が存在していない。そのため、評価値Ｅは１となる。

図１に戻り、推定部１３は、時刻ｔの位置情報の平行移動量Ｔ及び回転移動量Ｒを変更させて、一致度を最大にする平行移動量Ｔ及び回転移動量Ｒを探索し、自己位置を推定する。つまり、推定部１３は、式（１）に示す評価値Ｅを最小にする回転移動量Ｒ及び平行移動量Ｔを探索する。このような探索を行い、その探索結果に基づいてマップを作成し、自己位置を推定する手法はＳＬＡＭと呼ばれている。

具体的には、推定部１３は、時刻ｔ−１における位置情報において、時刻ｔにおける位置情報のローカル座標空間を平行移動量Ｔで平行移動し、かつ、回転移動量Ｒで回転させ、時刻ｔ−１の位置情報と時刻ｔの位置情報との評価値Ｅを算出する処理を繰り返し、評価値Ｅを最小にする平行移動量Ｔ及び回転移動量Ｒを探索する。

そして、推定部１３は、評価値Ｅを最小にする平行移動量Ｔを時刻ｔ−１から時刻ｔまでの移動体の移動量として求め、時刻ｔ−１における移動体の位置に移動量を加えた値を時刻ｔにおける移動体の位置として求める。

また、推定部１３は、評価値Ｅを最小にする回転移動量Ｒを時刻ｔ−１から時刻ｔまでの移動体の向きの変化量として求め、時刻ｔ−１における移動体の向きに変化量を加えた値を時刻ｔにおける移動体の向きとして求める。

ここで、時刻ｔにおける移動体の向きとしては、例えば、移動体の周辺空間を示す２次元のグローバル座標空間において、所定の基準方向に対する、時刻ｔにおける移動体の基準方向の角度を採用することができる。また、時刻ｔにおける移動体の位置としては、例えばグローバル座標空間における移動体の位置を採用することができる。

また、回転移動量Ｒとしては、例えば、時刻ｔ−１における移動体の向きに対する時刻ｔにおける移動体の向きの回転を示す２×２の行列を採用することができる。また、平行移動量Ｔとしては、例えば、時刻ｔ−１から時刻ｔまでの移動体の移動量を示すｘ成分とｙ成分との値を示す採用することができる。

図６は、評価値ＥとしてＬ２ノルムを採用した場合において、動的環境下で時刻ｔ−１の位置情報と時刻ｔの位置情報とにおいてマッチング誤差が生じる理由を示した図である。時刻ｔにおいて、移動物体４１は例えば人間のような移動する物体である。静止物体４２は例えば壁のような静止している物体である。時刻ｔ及び時刻ｔ−１の位置情報において、静止物体４２のみが存在していれば、評価値Ｅの最小値を求めると、時刻ｔ−１における静止物体４２を時刻ｔにおける静止物体４２に一致させることができ、誤差は生じない。

しかしながら、移動物体４１が存在すると、移動物体４１は時々刻々移動するため、時刻ｔにおける移動物体４１及び静止物体４２の位置関係と、時刻ｔ−１における移動物体４１及び静止物体４２の位置関係とは異なってしまう。

よって、評価値Ｅの最小値を求めたとしても、評価値ＥとしてＬ２ノルムを用いた場合は、移動物体４１の影響により、時刻ｔにおける静止物体４２に対して、時刻ｔ−１における静止物体４２を一致させることができない。その結果、静止物体４２においてマッチング誤差が生じてしまうのである。

このように動的環境下ではマッチング誤差が発生するが、本発明者らはＬ０ノルムを用いて評価値Ｅを算出すると、マッチング誤差を低減させることができることを見出した。そこで、本実施の形態では、評価値ＥをＬ０ノルムを用いて算出している。

ノルムとは距離を示し、一般的によく用いられるのは、距離の２乗和であるＬ２ノルムである。Ｌ２ノルムを用いて最小の評価値Ｅを探索する場合、最適解を求める際に非線形最小二乗法を用いることができる。一方、Ｌ０ノルムを用いて最小の評価値Ｅを求める処理は、離散最適化問題に分類されるため、ＮＰ困難である。そのため、Ｌ０ノルムを採用した場合、時刻ｔに計測した位置情報の平行移動量Ｔ及び回転移動量Ｒを総当たり的に変化させて最小の評価値Ｅを探索する必要がある。

図１に戻り、マップ生成部１４は、推定部１３により推定された時刻ｔでの移動体の位置及び向きをグローバル座標空間にプロットし、かつ、プロットした位置及び向きを基準とし、時刻ｔでの位置情報をプロットすることで、周辺空間の２次元マップを生成する。

位置センサ１６は、レーザレンジファインダにより構成され、一定周期で時系列に位置情報を取得し、位置情報取得部１１に供給する。レーザレンジファインダとしては、ＳＩＣＫのＬＭＳ１００を採用することができる。図７（Ａ）はＳＩＣＫのＬＳＭ１００の外観図である。図７（Ｂ）は本発明の実施の形態による移動体の外観図である。図７（Ｂ）に示すように、位置センサ１６は移動体の前側に取り付けられている。

図８は、位置センサ１６の仕様を示した表である。図８に示すように位置センサ１６は、画角が２７０度であり、解像度が０．５度であり、測定可能な範囲が２０ｍであり、周波数が３０Ｈｚであり、取得できる計測点の個数が５４０個である。

図１に戻り、オドメトリ算出部１５は、移動体の車輪に取り付けられた回転量センサ１７から供給さされる計測データにしたがって移動体のオドメトリを算出し、推定部１３に供給する。ここで、オドメトリは回転量センサ１７からの計測データに従って推定される時刻ｔ−１を基準としたときの時刻ｔでの移動体の自己位置である。本実施の形態では、オドメトリ算出部１５は、位置センサ１６と同じ周期で時系列にオドメトリを算出する。

回転量センサ１７は、移動体の左右一対の車輪のそれぞれに取り付けられ、移動体の車輪の回転量を示す計測データをオドメトリ算出部１５に供給する。ここで、回転量センサ１７は、位置センサ１６と同じ周期で計測データを取得する。

図７（Ｂ）に示すように移動体は、前方に設けられた左右一対の車輪９１，９１、後方に設けられた左右一対の車輪９２，９２を備えている。そして、一対の回転量センサ１７は、それぞれ、車輪９２，９２に取り付けられ、車輪９２，９２の回転量を計測する。

その他、移動体は図７（Ｂ）に示すように、車輪９１，９２の上部に設けられたフレーム９３、フレーム９３の底面９３１の表側に載置されたコンピュータ９４、底面９３１の裏側に取り付けられたモータ９５等を備えている。位置センサ１６は、フレーム９３の上部に設けられたバー９３２に取り付けられている。

コンピュータ９４は、上記の位置情報取得部１１〜オドメトリ算出部１５の機能を実現する。また、コンピュータ９４は、移動体の駆動制御プログラムが実装され、移動体の駆動制御を司る。例えば、コンピュータ９４は、マップ生成部１４により生成された２次元マップを参照し、物体との衝突を回避しながら、所定のゴールに向けて移動するようにモータ９５を駆動させる。

モータ９５は例えば、車輪９２，９２に取り付けられた一対のモータにより構成され、コンピュータ９４からの信号に基づいて駆動する。左右一対のモータ９５は、コンピュータ９４からの信号にしたがって、車輪９２，９２の回転量を変えることで移動体を旋回させる。

次に、図１に示す自己位置推定装置の動作について説明する。図２は、本発明の実施の形態による自己位置推定装置の動作を示すフローチャートである。まず、位置情報取得部１１は時刻ｔにおける位置情報を取得する（Ｓ１）。

次に、推定部１３は、時刻ｔにおける位置情報の平行移動量Ｔと回転移動量Ｒとを変化させて複数の探索候補を生成し、各探索候補の評価値Ｅを一致度算出部１２に算出させ、評価値Ｅを最小にする探索候補を探索する（Ｓ２）。

次に、推定部１３は、Ｓ２で探索した探索候補の回転移動量Ｒと平行移動量Ｔとから移動体の自己位置を推定する（Ｓ３）。次に、マップ生成部１４は、推定部１３により推定された自己位置を２次元の座標空間にプロットし、移動体の周辺空間の２次元マップを生成する（Ｓ４）。Ｓ４の処理が終了すると、処理がＳ１に戻され、Ｓ１〜Ｓ４の処理が繰り返される。

図３は、図２のＳ２に示す探索処理の詳細を示すフローチャートである。まず、推定部１３は、変数ｋに０を代入し、変数ｋを初期化する（Ｓ２１）。

次に、推定部１３は、ｎ１（ｎ１は２以上の整数）個の探索候補を生成する。具体的には、推定部１３は、オドメトリ算出部１５により推定された移動体の自己位置から移動体の位置の事前確率を求め、この事前確率にしたがって、ランダムにｎ１パターンの回転移動量Ｒ及び平行移動量Ｔを決定し、決定したｎ１パターンの回転移動量Ｒと平行移動量Ｔとを用いて時刻ｔの位置情報を時刻ｔ−１の位置情報に対してずらし、ｎ１個の探索候補を生成する（Ｓ２２）。

ここで、事前確率としては、オドメトリ算出部１５が推定した自己位置に近い位置ほど探索候補が多くなるような確率を採用することができる。そして、この事前確率にしたがった抽選処理によってｎ１個の探索候補が生成される。

図９は探索候補が生成される処理を説明する図である。図９では、（Ａ）〜（Ｄ）に向けて処理が進む。図９の各点は時刻ｔの位置情報の回転移動量Ｒと平行移動量Ｔとの要素をまとめたものを点の位置として現してある。図９（Ａ）では、Ｓ２２の処理によってｎ１個の探索候補が生成されている。

次に、一致度算出部１２は、ｎ１個の探索候補のそれぞれにつき、式（１）を用いて時刻ｔの位置情報との評価値Ｅを算出する（Ｓ２３）。

次に、推定部１３は、変数ｋが定数Ｋ未満の場合（Ｓ２４でＮＯ）、ｎ１個の探索候補の評価値Ｅからｎ１個の探索候補のそれぞれの重み値ｗｊを算出する（Ｓ２５）。ここで、重み値ｗｊは式（２）によって規定される。

ｗｊ＝ｅｘｐ（（ｍ−Ｅ（Ｒ，Ｔ））／ｋ） （２）
ここで、ｍ、ｋは定数である。ｍとしては例えば想定される評価値Ｅの最大値を採用することができる。ｊは探索候補を特定するインデックスである。式（２）に示すように評価値Ｅが小さくなるほど、重み値ｗｊは大きくなる。

次に、推定部１３は、重み値ｗｊに基づき、ｎ１個の探索候補の中からｎ２（ｎ２＜ｎ１）個の探索候補を抽出する（Ｓ２６）。この場合、推定部１３は、重み値ｗｊの値が大きいほど抽選確率が高くなるような抽選処理を行い、ｎ２個の探索候補を抽出してもよいし、重み値ｗｊが大きい順にｎ２個の探索候補を抽出してもよい。

Ｓ２６の処理により、図９（Ａ）に示すｎ１個の探索候補から図９（Ｂ）に示すようにｎ２個の探索候補が抽出される。図９（Ｂ）の例では、ｎ２＝３と設定されているため、３個の探索候補が抽出されていることが分かる。

図３に戻り、Ｓ２７において、推定部１３は、ｎ２個の探索候補のそれぞれにつき、ｎ３個の探索候補を生成する。この場合、推定部１３は、ｎ２個の探索候補の原点を基準とし、（σｘ，σｙ，σθ）の正規分布にしたがって、ｎ３パターンの回転移動量Ｒ及び平行移動量Ｔを決定し、ｎ３個の探索候補を生成すればよい。

例えば、ｎ２＝３の場合、３個の探索候補の原点を基準としてｎ３個の探索候補が生成される。この場合、ｎ２個の探索候補の重み値ｗｊの値に応じてｎ３個の個数を変えても良い。例えば、ｎ２＝３の場合の探索候補をＺ１〜Ｚ３とし、探索候補Ｚ１〜Ｚ３の重み値がｗ１〜ｗ３であったとする。但し、ｗ１＞ｗ２＞ｗ３である。この場合、探索候補Ｚ１〜Ｚ３から生成される探索候補の個数をそれぞれｎ３（１）〜ｎ３（３）とすると、重み値ｗ１に応じて、ｎ３（１）〜ｎ３（３）の値を決定し、探索候補を生成すればよい。なお、ｎ３（１）＞ｎ３（２）＞ｎ３（３）である。

図９（Ｃ）に示すように、Ｓ２７の処理により、ｎ２個の探索候補を基準としてｎ３個の探索候補が生成され、合計、ｎ２×ｎ３個の探索候補が生成されていることが分かる。

次に、推定部１３は変数ｋを１インクリメントし（Ｓ２８）、処理をＳ２３に戻す。Ｓ２４において、変数ｋが定数Ｋ以上になった場合（Ｓ２３でＮＯ）、推定部１３は、評価値Ｅが最小の探索候補を特定する（Ｓ２９）。

なお、２ループ目以降のＳ２３において、Ｓ２７で算出された全探索候補に対して評価値Ｅが算出される。このように、推定部１３は、Ｓ２３〜２８の処理をＫ回繰り返し、評価値Ｅが最小の探索候補を特定する。そして、探索候補を生成するにあたり、解となる可能性が高い回転移動量Ｒ及び平行移動量Ｔを持つ探索候補を重点的に生成する重点的サンプリングを行う。そのため、解となる探索候補を効率良く求めることが可能となる。

評価値Ｅを最小化する解を求める手法として、ベイズ推定を用いる手法が知られている。この手法は、Ｌ２ノルムを用いて評価値Ｅを算出する手法において、位置推定の信頼性が移動物体の影響によって一時的に乏しくなることを防止し、位置推定の頑強性を高めるために用いられる。

本実施の形態では、Ｓ２６に示すように、ｎ１個からｎ２個に探索候補を絞り、以降の処理が行われている。このように、本実施形態ではベイズ推定ではなく、maximuma-posteriori（ＭＡＰ）推定のように、解を絞り込んで次のステップに処理を進める手法を採用している。そのため、解の正確性に懸念が残るが、Ｌ０ノルムは移動する物体に対して頑健であるため、ＭＡＰ推定を適用しても問題はない。その結果、探索候補の個数の増大を抑制し、最小の評価値Ｅを探索する処理を高速化することができる。

なお、図３において、Ｓ２２が第１処理に相当し、Ｓ２５，Ｓ２６が第２処理に相当し、Ｓ２７が第３処理に相当し、Ｓ２４，Ｓ２９が第４処理に相当する。

次に、図３のＳ２３に示す評価値の算出処理の詳細について説明する。ベクトルデータをハッシュ値に変換するアルゴリズムとして、ＬＳＨ(Locality Sensitive Hashing）が知られている（Alexandr Andoni, Mayur Datar, Nicole Immorlica, Piotr Indyk, and Vahab Mirrokni : “Locality-Sensitive Hashing Scheme Based on p-Stable Distributions” , Nearest Neighbor Methods in Learning and Vision, MIT Press, 2006.）。

上記のようにＬ０ノルムを用いた評価値Ｅにおいて、最小の評価値Ｅを探索する処理は最適化が困難であり、総当り法で計算されることが多い。したがって、探索範囲及び解像度によっては計算量が爆発的に増大してしまう。そのため、評価値Ｅの算出手法としては、可能な限り高速な手法を採用することが好ましい。

Ｌ０ノルムを用いた評価値Ｅにおいて最小の評価値Ｅを探索する場合、ｒ近傍を求める際の計算の負荷が大きい。ｒ近傍を求める単純なアルゴリズム（例えば、Ｂｒｕｔｅｆｏｒｃｅ）の計算量はＯ（ｎ）であるが、ｋｄ−ｔｒｅｅやボロノイ図を用いれば、計算量をＯ（ｌｏｇｎ）まで減らすことができる。

しかしながら、ＬＳＨは、近似的ではあるがこれらの手法よりも更に高速であることが知られている。そこで、本実施の形態では、ＬＳＨを採用している。ＬＳＨは、データの距離が近いほど、高い確率で同じハッシュ値をとる性質を有する。同じハッシュ値に分類されたデータのみが近傍点の候補であるとみなすことで、計算量を大幅に削減することができる。

図４は、評価値Ｅの算出処理の詳細を示すフローチャートである。まず、一致度算出部１２は、時刻ｔ−１の位置情報から複数のハッシュテーブルを生成する（Ｓ４１）。ここで、ハッシュテーブルは、時刻ｔ−１の位置情報の各計測点を所定のハッシュ関数を用いて格子空間に投影し、格子空間の各位置における計測点の存在の有無を示すテーブルである。格子空間は、時刻ｔ−１における位置情報のローカル座標空間を距離εに基づき、格子状に区切ることで生成される空間である。ここで、ハッシュ関数ｈは式（３）により表される。

ｈ（ｂ）＝＜（ｐ・ｂ＋ｑ）／ε＞ （３）
但し、＜＞は（ｐ・ｂ＋ｑ）／ε以下の最大の整数を示す。ｐは１を平均とする正規分布により決定される。ｑは一様分布により０からεの範囲でランダムに決定される。ｂは計測点を示している。

本実施の形態では、以下に示す２つのハッシュ関数ｈｘ，ｈｙを採用する。

ｈｘ（ｂ）＝＜（ｐ・ｂｘ＋ｑｘ）／ε＞ （３−１）
ｈｙ（ｂ）＝＜（ｐ・ｂｙ＋ｑｙ）／ε＞ （３−２）
なお、ｑｘ，ｑｙは、ｑのｘ，ｙ成分であり、一様分布により０からεの範囲でランダムに決定される。ｂｘ，ｂｙは計測点ｂのｘ，ｙ成分である。

計測点ｂはｘ軸及びｙ軸で規定されるローカル座標空間に位置している。したがって、一致度算出部１２は、式（３−１）に示すように、ｈｘ（ｂ）を用いて計測点ｂの格子空間におけるｘ座標の値を求め、式（３−２）に示すように、ｈｙ（ｂ）を用いて計測点ｂの格子空間におけるｙ座標の値を求める。

図１０はハッシュテーブルを示した概念図である。図１０（Ａ）は時刻ｔ−１の位置情報のローカル座標空間に設定された格子空間を示している。

図１０（Ｂ）は、図１０（Ａ）に示す格子空間に基づいて生成されたハッシュテーブルＴ１〜Ｔ３を示している。図１０（Ａ）において、計測点ｂは式（３−１），（３−２）の＜＞の演算が施される前の位置にプロットされている。図１０（Ａ）において、例えば１行１列目の格子には計測点ｂが存在している。よって、この計測点ｂに＜＞の演算を施してハッシュ値ｈｘ（ｂ），ｈｙ（ｂ）を求めると、図１０（Ｂ）に示すように、計測点ｂはハッシュテーブルＴ１の１行１列目のビンに位置することになる。よって、ハッシュテーブルＴ１の１行１列目のビンには計測点ｂの存在を示す、例えば１のラベルが設定される。一方、図１０（Ａ）において、例えば１行３列目の格子には計測点ｂが存在していない。よって、図１０（Ｂ）に示すように、ハッシュテーブルＴ１の１行３列目のビンには計測点ｂの非存在を示す例えば０のラベルが設定される。

このようにして、計測点ｂが式（３−１）、（３−２）を用いて格子空間に投影され、ハッシュテーブルＴ１が生成される。

ここで、一致度算出部１２は、式（３−１）、（３−２）に示すハッシュ関数を複数種類用意して、複数のハッシュテーブルを生成する。図１０（Ｂ）の例では、３種のハッシュ関数ｈ１（＝（ｈ１ｘ（ｂ），ｈ１ｙ（ｂ）））〜ｈ３（＝（ｈ３ｘ（ｂ），ｈ３ｙ（ｂ）））を用いて３個のハッシュテーブルＴ１〜Ｔ３が生成されている。

この場合、一致度算出部１２は、式（３−１）、（３−２）に示す、ｐ，ｑｘ，ｑｙの値を異なる値に設定することで複数種類のハッシュ関数を生成すればよい。

このように、複数のハッシュテーブルＴ１〜Ｔ３を採用することで、計測点ａが計測点ｂの距離εの範囲内にあるにも関わらず、たまたまハッシュテーブルにはハッシュ値が存在しなかったというような、探索ミスを防止することができる。

なお、上記では、（３−１）、（３−２）で示すハッシュ関数を用いたが、（３−１’）、（３−２’）で示すハッシュ関数を採用してもよい。

ｈｘ（ｂ）＝＜（ｂｘ＋ｑｘ）／ｐε＞ （３−１’）
ｈｙ（ｂ）＝＜（ｂｙ＋ｑｙ）／ｐε＞ （３−２’）

（３−１）、（３−２）との違いは、ｐが分母に移され、εに乗じられている点である。これにより、ローカル座標空間に設定される格子の長さが変わり、ハッシュテーブルにより多くの多様性を持たせることができる。

図４に戻り、Ｓ４２において、一致度算出部１２は、時刻ｔの位置情報の各計測点ａをハッシュ関数ｈ１〜ｈ３を用いて投影空間に投影し、計測点ａのハッシュ値ｈ１（ａ）〜ｈ３（ａ）を求める。

次に、一致度算出部１２は、求めたハッシュ値ｈ１（ａ）〜ｈ３（ａ）のいずれかが対応するハッシュテーブルＴ１〜Ｔ３に存在すれば、評価値Ｅにペナルティを課さずに評価値Ｅを算出する（Ｓ４３）。

例えば、ハッシュ値ｈ１（ａ）で示されるハッシュテーブルＴ１のビンに１のラベルが設定されていれば、ハッシュ値ｈ２（ａ），ｈ３（ａ）で示されるハッシュテーブルＴ２，Ｔ３のビンに０のラベルが設定されていても、計測点ａから距離ε内に計測点ｂが位置するとみなされ、評価値Ｅにペナルティが課されない。

一致度算出部１２は、このような処理を各計測点ａに対して行い、付与したペナルティの合計値を評価値Ｅとして算出する。このように時刻ｔ−１の計測点ｂに基づいて予めハッシュテーブルを生成しておくことで、ハッシュテーブルの参照にかかる計算量をＯ（１）にすることができる。

なお、上記実施の形態では、位置情報は２次元としたが、これに限定されず３次元であってもよい。この場合、位置センサ１６として、例えば深度センサを採用すればよい。そして、周辺空間を３次元のグローバル座標空間で表し、推定した移動体の自己位置プロットしていき、３次元マップを採用してもよい。

＜実験例＞
次に、本発明の実施の形態による自己位置推定装置が適用された移動体の実験について説明する。

＜実験環境＞
本実験では、移動体として、車椅子型移動ロボットEMC-230を採用し、これに図８に示す仕様のレーザレンジファインダ（ＬＲＦ）を取りつけ、センシングを行った。ＬＲＦがセンシングした情報は極座標で表現され、角度方向に等間隔である。センサからの距離が増大すると点の密度が疎になるため、セグメンテーションを行い、セグメント内の点の間隔がε以下になるよう補間した。

実験場所として、奈良先端科学技術大学院大学情報棟１階を用いた。動的環境をＬＲＦで計測し、時刻ｔ，時刻ｔ−１の位置情報のマッチングを行った。この実験において、移動体は静止した状態で動的環境の計測を行った。動的環境は幅１０ｍ程の廊下で奥行き２０ｍの範囲で、２５人前後の歩行者が移動していた。

図１１（Ａ）は実験場所を示した図であり、図１１（Ｂ）はこの実験場所に人物が行き来する動的環境を示した図である。

図１２は、実験場所に人物が行き来していない静的環境下で生成した周辺空間の２次元マップを示している。この２次元マップはＲＢＰＦ−ＳＬＡＭを用いて生成されたものであり真値とみなせる。

＜実験結果＞
この実験では最小の評価値Ｅを探索する際の探索範囲を、ｘ軸方向、ｙ軸方向にそれぞれプラスマイナス ２０［ｃｍ］とし、角度を プラスマイナス０．３［ｒａｄ］と設定した。そして、この探索範囲において、１．０［ｃｍ］、０．０１［ｒａｄ］（＝約０．５７［ｄｅｇ］）刻みで総当たり法で、回転移動量Ｒ、平行移動量Ｔを変化させて、評価値Ｅの最小値を探索した。

そして、評価値ＥとしてＬ０ノルムを採用した場合、Ｌ２ノルムを採用した場合、Ｍ−ｅｓｔｉｍａｔｏｒを採用した場合のマッチングの位置推定精度の比較を行った。この際、Ｍ−ｅｓｔｉｍａｔｏｒにはＢｉｗｅｉｇｈｔ法に基づき、距離ｄに対し、以下の評価関数ρ（ｄ）を用いた。

ρ（ｄ）＝Ｂ^２／２ （ｄ≧Ｂ）
ｏｒ
（Ｂ^２／２）（１−（１−（ｄ／Ｂ）^２）^３） （ｄ＜Ｂ）

ここで、Ｂは任意の残差値であり、０．０１［ｍ］とした。図１３は、評価値Ｅとして、Ｌ２ノルム、Ｍ−ｅｓｔｉｍａｔｏｒ、及びＬ０ノルムを用いた場合の実験結果を示した表である。

Ｌ２ノルムでは最大１４［ｃｍ］のマッチング誤差が発生しており、平均二乗誤差（ＲＭＳＥ）も１０［ｃｍ］ほどあった。一方、Ｌ０ノルムではマッチング誤差は最大１［ｃｍ］であり、ＬＲＦの計測誤差内に収まっていることがわかる。

次に、ＬＲＦにより２００フレーム分の位置情報を測定し、Ｌ２ノルム、Ｍ−ｅｓｔｉｍａｔｏｒ、及びＬ０ノルムをそれぞれ用いて隣接するフレーム同士の位置情報のマッチングを行い、２次元マップを生成した。

図１４は、Ｌ２ノルムを採用した場合に作成した２次元マップである。図１５は、Ｍ−ｅｓｔｉｍａｔｏｒを採用した場合に作成した２次元マップである。図１６は、Ｌ０ノルムを採用した場合に作成した２次元マップである。

図１４に示すようにＬ２ノルムを採用した場合、マッチング誤差により、移動体が移動していると誤認識され、２次元マップの生成に失敗していることが分かる。

図１５に示すように、Ｍ−ｅｓｔｉｍａｔｏｒを採用した場合、大きなマッチング誤差はなかったが、累積したマッチング誤差により、移動体が移動していると誤認識され、２次元マップの生成に失敗している。一方、図１６に示すように、Ｌ０ノルムを採用した場合、フレーム間のマッチング誤差がほとんど発生せず、自己位置推定装置は移動体が静止している状態を計測できている。

この実験結果から分かるようにＬ０ノルムを採用した場合、他の手法を採用した場合に比べ、動的環境下において、高い頑健性を有していることが分かる。

次に、本実施の形態による自己位置推定装置の優位性を検証するため、ＬＳＨを採用した場合、Ｂｒｕｔｅ−ｆｏｒｃｅを採用した場合、及びｋｄ−ｔｒｅｅを採用した場合の計算時間の比較を行った。なお、評価値ＥとしてはＬ０ノルムを採用した。探索範囲は、上記の値と同じである。合計２４０００通りの評価値Ｅを計算し、総計算時間（Average calculation time）、１秒あたりに計算できるＬ０ノルムの数（Frequency of calculation aec.）、及びｒ近傍点が発見できた計測点の平均個数（Average match points）を求めた。

図１７は、Ｂｒｕｔｅ−ｆｏｒｃｅ、ｋｄ−ｔｒｅｅ、ＬＳＨを用いて計算時間を比較する実験を行った場合の実験結果をまとめた表である。

図１７に示すように、Ｂｒｕｔｅ−ｆｏｒｃｅ、ｋｄ−ｔｒｅｅに比べ、ＬＳＨを用いた場合、総計算時間が短いことがわかる。しかしながら、Ｂｒｕｔｅ−ｆｏｒｃｅ及びｋｄ−ｔｒｅｅに比べ、ＬＳＨはｒ近傍点が発見できた計測点の平均個数（Average match points）が１５個ほど少ない。これは、ＬＳＨによる近傍点探索が近似であることが原因である。

この一致点数の差の影響を、複数フレームのマッチングにより２次元マップを生成することで調べる。図１８は静的環境下でＲＢＰＦ−ＳＬＡＭを用いて生成した２次元マップである。この２次元マップを真値と見なす。

図１９は、動的環境下で移動体を移動させながら計測した位置情報を利用し、Ｌ０ノルムを用いた評価値Ｅで位置情報同士のマッチングを行って生成した２次元マップである。この２次元マップ内に存在する点は移動する人の軌跡を示している。

図２０は、Ｌ２ノルムを用いた評価値Ｅで位置情報同士のマッチングを行って生成した２次元マップである。両２次元マップのエッジの形状を比較すると、Ｌ０ノルムの方が動的環境下でも比較的正確に２次元マップが生成されていることが分かる。

更に、図２１に示すような大規模な２次元マップを生成した。図２１は、ＲＢＰＦ−ＳＬＡＭを用いて生成した２次元マップである。この２次元マップを真値と見なす。

Ｌ０ノルムでは、フレーム間のマッチングしか行っていないため、微少なマッチング誤差が累積し、図２２に示すように地図が歪んでしまう。図２２は、Ｌ０ノルムを用いて生成された２次元マップである。この２次元マップは、図２１と同じ環境下で生成されたものである。図２２に現れた歪みは、例えばループクロージング等の手法を適用すれば改善することができる。

上記の自己位置推定装置及び移動体の技術的特徴は以下のようにまとめられる。

（１）上記の自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、前記位置情報取得部により第１時刻で取得された第１位置情報を、前記第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの前記第１位置情報と前記第２位置情報との一致度を算出する一致度算出部と、前記第１位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部とを備え、前記一致度算出部は、前記第１位置情報を構成するある計測点に対し、一定距離内に前記第２位置情報を構成するある計測点が存在すれば、前記一致度に所定のポイントを付与し、付与したポイントの合計値を前記一致度として算出する。

この構成によれば、第２位置情報に対して第１位置情報がずらされて一致度を最大とする平行移動量及び回転移動量が探索される。そして、一致度は、第１位置情報を構成するある計測点に対し、一定距離内に第２位置情報のある計測点が位置すれば所定のポイントが付与される。つまり、本構成では、静止物体のみならず移動物体を含む動的環境下においても頑強なＬ０ノルムを用いて一致度が算出されている。そのため、移動物体を含む未知環境下において、移動体の自己位置を正確に推定することができる。

（２）前記一致度算出部は、前記周辺空間を前記一定距離に基づいて格子状に区切ることで格子空間を生成し、前記第２位置情報の各計測点を所定のハッシュ関数を用いて前記格子空間に投影し、前記格子空間の各位置における前記計測点の存在の有無を示すハッシュテーブルを生成し、前記第１位置情報の各計測点を前記ハッシュ関数を用いて前記格子空間に投影し、ある計測点の前記格子空間における位置が前記ハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与することが好ましい。

この構成によれば、第２位置情報の各計測点が所定のハッシュ関数を用いて格子空間に投影され、各計測点の存在の有無を示すハッシュテーブルが生成される。そして、第１位置情報の各計測点もハッシュ関数を用いて格子空間に投影され、投影された計測点の位置がハッシュテーブルに存在すれば、第１位置情報を構成するある計測点の近傍に第２位置情報を構成するある計測点が存在するとみなされ、一致度に所定のポイントが付与される。このように、本構成では、第１、第２位置情報の計測点のハッシュ値同士を比較することで一致度が算出されているため、一致度を最大とする平行移動量及び回転移動量を高速に求めることができる。

（３）前記一致度算出部は、種類の異なる複数のハッシュ関数を用いて前記ハッシュテーブルを複数生成し、前記第１位置情報のある計測点を各ハッシュ関数を用いて前記格子空間に投影し、いずれかの位置が対応するハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与することが好ましい。

この構成によれば、複数のハッシュテーブルが採用されているため、第１位置情報のある計測点が第２位置情報のある計測点に対し、距離εの範囲内にあるにも関わらず、たまたまハッシュテーブルにはハッシュ値が存在しなかったというような、探索ミスを防止することができる。

（４）前記推定部は、前記平行移動量及び前記回転移動量をランダムに変化させて前記第１位置情報からｎ１（ｎ１は正の整数）個の探索候補を生成する第１処理と、各探索候補の前記一致度に基づき、各探索候補の重み値を算出し、算出した重み値に基づいて、ｎ２（ｎ２＜ｎ１）個の探索候補を抽出する第２処理と、抽出したｎ２個の探索候補のそれぞれについて、所定の確率分布に基づいて前記平行移動量及び前記回転移動量を変化させてｎ３（ｎ３は正の整数）個の探索候補を生成する第３処理と、前記ｎ３個の探索候補のそれぞれに対し、前記重み値を算出し、算出した重み値が最大となる探索候補を探索解として求める第４処理とを備えることが好ましい。

この構成によれば、解となる可能性の高い位置に重点的に第１位置情報がずらされて、複数の探索候補が生成されて第２位置情報との一致度が求められている。そのため、一致度を最大にする探索候補を効率良く算出することができる。

（５）前記第４処理は、前記第２、第３処理を所定回数繰り返し、前記重み値が最大となる探索候補を前記探索解として求めることが好ましい。

この構成によれば、第２、第３処理が繰り返されるため、一致度を最大にする探索候補をより正確に算出することができる。

（６）前記移動体のオドメトリを算出するオドメトリ算出部を更に備え、前記第１処理は、前記オドメトリから得られる前記移動体の位置の事前確率に基づき、前記平行移動量及び前記回転移動量をランダムに変化させてｎ１個の探索候補を生成することが好ましい。

この構成によれば、第１処理において、解となる可能性の高い位置に重点的に第１位置情報をずらして探索候補を生成することができ、探索精度をより高めることができる。

（７）上記の移動体は、（１）〜（６）のいずれかの移動体と、前記位置情報を取得する位置センサとを備える。

この構成によれば、未知環境下において自己位置を精度良く推定しながら、移動することができる移動体を提供することができる。

Claims (9)

1. 移動体の自己位置を推定する自己位置推定装置であって、
   前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、
   前記位置情報取得部により第１時刻で取得された第１位置情報を、前記第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの前記第１位置情報と前記第２位置情報との一致度を算出する一致度算出部と、
   前記第１位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部とを備え、
   前記一致度算出部は、前記第１位置情報を構成するある計測点に対し、一定距離内に前記第２位置情報を構成するある計測点が存在すれば、前記一致度に所定のポイントを付与し、付与したポイントの合計値を前記一致度として算出する自己位置推定装置。
2. 前記一致度算出部は、前記周辺空間を前記一定距離に基づいて格子状に区切ることで格子空間を生成し、前記第２位置情報の各計測点を所定のハッシュ関数を用いて前記格子空間に投影し、前記格子空間の各位置における前記計測点の存在の有無を示すハッシュテーブルを生成し、前記第１位置情報の各計測点を前記ハッシュ関数を用いて前記格子空間に投影し、ある計測点の前記格子空間における位置が前記ハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与する請求項１記載の自己位置推定装置。
3. 前記一致度算出部は、種類の異なる複数のハッシュ関数を用いて前記ハッシュテーブルを複数生成し、前記第１位置情報のある計測点を各ハッシュ関数を用いて前記格子空間に投影し、いずれかの位置が対応するハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与する請求項２記載の自己位置推定装置。
4. 前記推定部は、
   前記平行移動量及び前記回転移動量をランダムに変化させて前記第１位置情報からｎ１（ｎ１は正の整数）個の探索候補を生成する第１処理と、
   各探索候補の前記一致度に基づき、各探索候補の重み値を算出し、算出した重み値に基づいて、ｎ２（ｎ２＜ｎ１）個の探索候補を抽出する第２処理と、
   抽出したｎ２個の探索候補のそれぞれについて、所定の確率分布に基づいて前記平行移動量及び前記回転移動量を変化させてｎ３（ｎ３は正の整数）個の探索候補を生成する第３処理と、
   前記ｎ３個の探索候補のそれぞれに対し、前記重み値を算出し、算出した重み値が最大となる探索候補を探索解として求める第４処理とを備える請求項１〜３のいずれかに記載の自己位置推定装置。
5. 前記第４処理は、前記第２、第３処理を所定回数繰り返し、前記重み値が最大となる探索候補を前記探索解として求める請求項４記載の自己位置推定装置。
6. 前記移動体のオドメトリを算出するオドメトリ算出部を更に備え、
   前記第１処理は、前記オドメトリから得られる前記移動体の位置の事前確率に基づき、前記平行移動量及び前記回転移動量をランダムに変化させてｎ１個の探索候補を生成する請求項４又は５記載の自己位置推定装置。
7. 請求項１〜６のいずれかに記載の自己位置推定装置と、
   前記位置情報を取得する位置センサとを備える移動体。
8. 移動体の自己位置を推定する自己位置推定方法であって、
   コンピュータが、前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得ステップと、
   コンピュータが、前記位置情報取得ステップにより第１時刻で取得された第１位置情報を、前記第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの前記第１位置情報と前記第２位置情報との一致度を算出する一致度算出ステップと、
   コンピュータが、前記第１位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定ステップとを備え、
   前記一致度算出ステップは、前記第１位置情報のある計測点から一定距離内に前記第２位置情報のある計測点が存在すれば、前記一致度に所定のポイントを付与し、前記ポイントの加算値を前記一致度として算出する自己位置推定方法。
9. 移動体の自己位置を推定する自己位置推定プログラムであって、
   前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、
   前記位置情報取得部により第１時刻で取得された第１位置情報を、前記第１時刻と時系列的に前又は後の時刻である第２時刻で取得された第２位置情報に対して平行移動及び回転移動させたときの前記第１位置情報と前記第２位置情報との一致度を算出する一致度算出部と、
   前記第１位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部としてコンピュータを機能させ、
   前記一致度算出部は、前記第１位置情報のある計測点から一定距離内に前記第２位置情報のある計測点が存在すれば、前記一致度に所定のポイントを付与し、前記ポイントの加算値を前記一致度として算出する自己位置推定プログラム。

Images