TOP > 国内特許検索 > 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体 > 明細書

明細書 :自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第5892663号 (P5892663)
登録日 平成28年3月4日(2016.3.4)
発行日 平成28年3月23日(2016.3.23)
発明の名称または考案の名称 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、及び移動体
国際特許分類 G01B  11/00        (2006.01)
G05D   1/02        (2006.01)
G01S  17/89        (2006.01)
G01C  21/28        (2006.01)
FI G01B 11/00 A
G05D 1/02 J
G01S 17/89
G01C 21/28
請求項の数または発明の数 9
全頁数 21
出願番号 特願2013-521307 (P2013-521307)
出願日 平成23年12月7日(2011.12.7)
新規性喪失の例外の表示 特許法第30条第1項適用 以下の刊行物及び研究集会にて発表 1. 発行日:平成22年12月23日 刊行物名:第11回公益社団法人計測自動制御学会システムインテグレーション部門 講演会 講演論文集 発行所:公益社団法人計測自動制御学会 2. 開催日:平成23年2月16日 研究集会名:奈良先端科学技術大学院大学 平成22年度博士前期課程修士論文・課題研究発表会 主催者名:国立大学法人奈良先端科学技術大学院大学
国際出願番号 PCT/JP2011/006846
国際公開番号 WO2012/176249
国際公開日 平成24年12月27日(2012.12.27)
優先権出願番号 2011137623
優先日 平成23年6月21日(2011.6.21)
優先権主張国 日本国(JP)
審査請求日 平成26年11月4日(2014.11.4)
特許権者または実用新案権者 【識別番号】504143441
【氏名又は名称】国立大学法人 奈良先端科学技術大学院大学
発明者または考案者 【氏名】日永田 佑介
【氏名】末永 剛
【氏名】竹村 憲太郎
【氏名】高松 淳
【氏名】小笠原 司
個別代理人の代理人 【識別番号】100067828、【弁理士】、【氏名又は名称】小谷 悦司
【識別番号】100115381、【弁理士】、【氏名又は名称】小谷 昌崇
【識別番号】100118049、【弁理士】、【氏名又は名称】西谷 浩治
審査官 【審査官】神谷 健一
参考文献・文献 特開2010-262546(JP,A)
特開2011-048706(JP,A)
特開平08-261719(JP,A)
特開2008-040677(JP,A)
特開2007-164783(JP,A)
米国特許出願公開第2005/0216426(US,A1)
米国特許出願公開第2007/0112700(US,A1)
日永田佑介,L0ノルム最小化を利用した動的な混雑環境下に適用可能なリアルタイムSLAM,奈良先端科学技術大学院大学修士論文,奈良先端科学技術大学院大学,2011年 5月24日,http://library.naist.jp/dspace/handle/10061/6230
調査した分野 G01B 11/00-11/30
G01C 15/00
G01C 21/28
G05D 1/02
G01S 17/89
特許請求の範囲 【請求項1】
移動体の自己位置を推定する自己位置推定装置であって、
前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、
前記位置情報取得部により第1時刻で取得された第1位置情報を、前記第1時刻と時系列的に前又は後の時刻である第2時刻で取得された第2位置情報に対して平行移動及び回転移動させたときの前記第1位置情報と前記第2位置情報との一致度を算出する一致度算出部と、
前記第1位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部とを備え、
前記一致度算出部は、前記第1位置情報を構成するある計測点に対し、一定距離内に前記第2位置情報を構成するある計測点が存在すれば、前記一致度に所定のポイントを付与し、付与したポイントの合計値を前記一致度として算出する自己位置推定装置。
【請求項2】
前記一致度算出部は、前記周辺空間を前記一定距離に基づいて格子状に区切ることで格子空間を生成し、前記第2位置情報の各計測点を所定のハッシュ関数を用いて前記格子空間に投影し、前記格子空間の各位置における前記計測点の存在の有無を示すハッシュテーブルを生成し、前記第1位置情報の各計測点を前記ハッシュ関数を用いて前記格子空間に投影し、ある計測点の前記格子空間における位置が前記ハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与する請求項1記載の自己位置推定装置。
【請求項3】
前記一致度算出部は、種類の異なる複数のハッシュ関数を用いて前記ハッシュテーブルを複数生成し、前記第1位置情報のある計測点を各ハッシュ関数を用いて前記格子空間に投影し、いずれかの位置が対応するハッシュテーブルに存在した場合、前記一致度に前記ポイントを付与する請求項2記載の自己位置推定装置。
【請求項4】
前記推定部は、
前記平行移動量及び前記回転移動量をランダムに変化させて前記第1位置情報からn1(n1は正の整数)個の探索候補を生成する第1処理と、
各探索候補の前記一致度に基づき、各探索候補の重み値を算出し、算出した重み値に基づいて、n2(n2<n1)個の探索候補を抽出する第2処理と、
抽出したn2個の探索候補のそれぞれについて、所定の確率分布に基づいて前記平行移動量及び前記回転移動量を変化させてn3(n3は正の整数)個の探索候補を生成する第3処理と、
前記n3個の探索候補のそれぞれに対し、前記重み値を算出し、算出した重み値が最大となる探索候補を探索解として求める第4処理とを備える請求項1~3のいずれかに記載の自己位置推定装置。
【請求項5】
前記第4処理は、前記第2、第3処理を所定回数繰り返し、前記重み値が最大となる探索候補を前記探索解として求める請求項4記載の自己位置推定装置。
【請求項6】
前記移動体のオドメトリを算出するオドメトリ算出部を更に備え、
前記第1処理は、前記オドメトリから得られる前記移動体の位置の事前確率に基づき、前記平行移動量及び前記回転移動量をランダムに変化させてn1個の探索候補を生成する請求項4又は5記載の自己位置推定装置。
【請求項7】
請求項1~6のいずれかに記載の自己位置推定装置と、
前記位置情報を取得する位置センサとを備える移動体。
【請求項8】
移動体の自己位置を推定する自己位置推定方法であって、
コンピュータが、前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得ステップと、
コンピュータが、前記位置情報取得ステップにより第1時刻で取得された第1位置情報を、前記第1時刻と時系列的に前又は後の時刻である第2時刻で取得された第2位置情報に対して平行移動及び回転移動させたときの前記第1位置情報と前記第2位置情報との一致度を算出する一致度算出ステップと、
コンピュータが、前記第1位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定ステップとを備え、
前記一致度算出ステップは、前記第1位置情報のある計測点から一定距離内に前記第2位置情報のある計測点が存在すれば、前記一致度に所定のポイントを付与し、前記ポイントの加算値を前記一致度として算出する自己位置推定方法。
【請求項9】
移動体の自己位置を推定する自己位置推定プログラムであって、
前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、
前記位置情報取得部により第1時刻で取得された第1位置情報を、前記第1時刻と時系列的に前又は後の時刻である第2時刻で取得された第2位置情報に対して平行移動及び回転移動させたときの前記第1位置情報と前記第2位置情報との一致度を算出する一致度算出部と、
前記第1位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部としてコンピュータを機能させ、
前記一致度算出部は、前記第1位置情報のある計測点から一定距離内に前記第2位置情報のある計測点が存在すれば、前記一致度に所定のポイントを付与し、前記ポイントの加算値を前記一致度として算出する自己位置推定プログラム。
発明の詳細な説明 【技術分野】
【0001】
本発明は、移動体の自己位置を推定する技術に関し、特に、未知環境下で動作する移動体の自己位置を推定する技術に関するものである。
【背景技術】
【0002】
未知環境内をロボットが移動するためには、環境を計算機上にモデリングする必要がある。未知環境をモデリングする手法として、Simultaneous Localization and Mapping(SLAM)が知られている(例えば、非特許文献1)。これは、未知環境下での相対的自己位置推定と地図生成を同時に行うことによって環境のモデリングを行う。SLAMでは周囲の環境は静的であるという仮定に基づくものが多く、動的環境下では、マッチング誤差が発生してしまうという問題があった。
【0003】
そのため、作業領域を人と共有するような実環境下でロボットを運用する場合、人の往来など動的な環境変化に対応する必要がある。動的環境下で利用可能なSLAMとして、外れ値を考慮したランドマークに基づくアプローチが提案されている(例えば、非特許文献2)。
【0004】
しかしながら、非特許文献2の手法には十分な数のランドマークが観察される必要性や、動的障害物による隠れの問題がある。
【0005】
また、センシング対象の形状(非特許文献3)や、フレーム間の最近傍点距離(非特許文献4)によって、静止物体と移動物体とを区別し、マッチングを行う手法も提案されている。
【0006】
しかしながら、非特許文献3、4の手法は、測定精度が移動物体かどうかを判定する判別器の性能に強く依存するという問題がある。
【0007】
また、レーザレンジファインダで計測される2次元の位置情報のように、複数の計測点により構成された位置情報同士のマッチングを行う手法として、L2ノルムを最小化する手法であるIterative Closest Point(ICP)も知られている(非特許文献5)。
【0008】
図23は、ICPを用いた場合のマッチングの誤差を示した図である。図23においては、ロボットの移動面を示す2次元の座標空間に、ロボットに搭載された位置センサにより計測された計測点と、移動体の周囲に存在する物体の実際の位置とが重ね合わせてプロットされている。そして、図23の円の点線で囲んだ領域に計測点と実際の位置との誤差が生じている。
【0009】
このように、非特許文献5に示す手法では、図23に示すように、動的な環境下において多くのマッチング誤差が発生してしまう。
【先行技術文献】
【0010】

【非特許文献1】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.
【非特許文献2】Denis F Wolf, Gaurav s Sukhatme : “Mobile Robot Simultaneous Localization and Mapping inDynamic Environments”
【非特許文献3】A. Ess, B. Leibe, K. Schindler, L. vanGool : “Moving ObstacleDetection in Highly Dynamic Scenes”, ICRA09, 2009.
【非特許文献4】識名拓, 油田信一: “多く通行人がいる廊下環境での移動ロボットによる地図作成”, ROBOMEC2010,1A2-D29, June 14-16, 2010.
【非特許文献5】Sebastian Thrun , Wolfram Burgard , Dieter Fox : “Probabilistic robotics”, The MIT Press,2005.
【発明の概要】
【0011】
本発明の目的は、移動物体が存在する未知環境下において移動体の自己位置を精度良く推定することができる技術を提供することである。
【0012】
本発明の一局面による自己位置推定装置は、移動体の自己位置を推定する自己位置推定装置であって、前記移動体の周辺空間に存在する各物体の位置情報を時系列に取得する位置情報取得部と、前記位置情報取得部により第1時刻で取得された第1位置情報を、前記第1時刻と時系列的に前又は後の時刻である第2時刻で取得された第2位置情報に対して平行移動及び回転移動させたときの前記第1位置情報と前記第2位置情報との一致度を算出する一致度算出部と、前記第1位置情報の平行移動量及び回転移動量を変更させて、前記一致度を最大にする前記平行移動量及び前記回転移動量を探索し、前記自己位置を推定する推定部とを備え、前記一致度算出部は、前記第1位置情報を構成するある計測点に対し、一定距離内に前記第2位置情報を構成するある計測点が存在すれば、前記一致度に所定のポイントを付与し、付与したポイントの合計値を前記一致度として算出する。
【0013】
また、本発明の別の一局面による移動体は、上記の自己位置推定装置と、前記位置情報を取得する位置センサとを備えている。また、本発明の更に別の一局面による自己位置推定方法及び自己位置推定プログラムは上記の自己位置推定装置と同じ特徴を備えている。
【図面の簡単な説明】
【0014】
【図1】本発明の実施の形態による自己位置推定装置が適用された移動体のブロック図である。
【図2】本発明の実施の形態による自己位置推定装置の動作を示すフローチャートである。
【図3】図2のS2に示す探索処理の詳細を示すフローチャートである。
【図4】評価値の算出処理の詳細を示すフローチャートである。
【図5】(A)は評価値の算出手法を説明する概念図である。(B)は第1位置情報を構成する計測点a~aが回転移動される様子を示している。
【図6】評価値としてL2ノルムを採用した場合において、動的環境下で時刻t-1の位置情報と時刻tの位置情報とにおいてマッチング誤差が生じる理由を示した図である。
【図7】(A)はSICKのLSM100の外観図である。(B)は本発明の実施の形態による移動体の外観図である。
【図8】位置センサの仕様を示した表である。
【図9】探索候補が生成される処理を説明する図である。
【図10】ハッシュテーブルを示した概念図である。
【図11】(A)は実験場所を示した図であり、(B)はこの実験場所に人物が行き来する動的環境を示した図である。
【図12】実験場所に人物が行き来していない静的環境下で生成した周辺空間の2次元マップを示している。
【図13】評価値として、L2ノルム、M-estimator、及びL0ノルムを用いた場合の実験結果を示した表である。
【図14】L2ノルムを採用した場合に作成した2次元マップである。
【図15】M-estimatorを採用した場合に作成した2次元マップである。
【図16】L0ノルムを採用した場合に作成した2次元マップである。
【図17】Brute-force、kd-tree、LSHを用いて計算時間を比較する実験を行った場合の実験結果をまとめた表である。
【図18】静的環境下でRBPF-SLAMを用いて生成した2次元マップである。
【図19】動的環境下で移動体を移動させながら計測した位置情報を利用し、L0ノルムを用いた評価値Eで位置情報同士のマッチングを行って生成した2次元マップである。
【図20】L2ノルムを用いた評価値で位置情報同士のマッチングを行って生成した2次元マップである。
【図21】RBPF-SLAMを用いて生成した2次元マップである。
【図22】本実施の形態による自己位置推定装置を用いて生成された2次元マップである。
【図23】ICPを用いた場合のマッチングの誤差を示した図である。
【発明を実施するための形態】
【0015】
以下、本発明の実施の形態による自己位置推定装置について説明する。図1は、本発明の実施の形態による自己位置推定装置が適用された移動体のブロック図である。自己位置推定装置は、位置情報取得部11、一致度算出部12、推定部13、マップ生成部14、及びオドメトリ算出部15を備えている。そして、自己位置推定装置は、位置センサ16により取得された位置情報に基づき、移動体の自己位置を推定する。移動体は、自己位置推定装置、位置センサ16、及び回転量センサ17を備えている。

【0016】
本実施の形態では、自己位置推定装置は例えばCPU、ROM、RAM、及びハードディスク等を備えるコンピュータにより構成されている。そして、位置情報取得部11、一致度算出部12、推定部13、マップ生成部14、及びオドメトリ算出部15は、ハードディスクに格納された自己位置推定プログラムを例えばCPUが実行することで実現される。なお、自己位置推定プログラムは、DVD-ROM等のコンピュータ読み取り可能な記録媒体に記録されてユーザに提供される。ユーザはこの記録媒体をコンピュータにインストールすることで、コンピュータを自己位置推定装置として機能させる。

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

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

【0019】
また、本実施の形態では、時刻tの位置情報と時刻t-1の位置情報との一致の度合いを評価する評価値を用いて一致度を表す。具体的には、評価値Eは、式(1)により表される。

【0020】
E(R,T)=Σi=1f(Ra+T,{b}) (1)
f(a,{b})=0(∋j,|a-b|≦ε) or 1 (otherwise)

【0021】
但し、aは時刻tの位置情報の各計測点を表し、bは時刻t-1の位置情報の各計測点を表している。また、Rは時刻t-1を基準としたときの時刻tの各計測点の回転移動量を示し、Tは時刻t-1を基準としたときの時刻tの各計測点の平行移動量を示す。iは時刻tの位置情報の計測点を特定するためのインデックスであり、jは時刻t-1の位置情報の計測点を特定するためのインデックスでる。εはレーザレンジファインダの精度を考慮した距離である。nは計測点の個数を示している。

【0022】
式(1)は、計測点aをR回転移動させ、かつ、T平行移動させたときにおいて、計測点aが計測点bを中心として距離ε以内に存在しなかった場合、評価値Eに1のペナルティが課されると解釈することができる。したがって、式(1)では評価値Eが小さいほど時刻t-1の位置情報と時刻tの位置情報との一致度が高いことを示す。

【0023】
なお、式(1)において、f(a,{b})=1 (∋j,|a-b|≦ε) or 0 (otherwise)としてもよい。この場合、計測点aが計測点bを中心として距離ε以内に存在する場合、評価値Eに1ポイントが付与され、一致度が高いほど評価値Eが大きくなる。

【0024】
式(1)の計算では、明示的に対応点を決定する必要はなく、単に、計測点bを中心として、距離ε内に計測点aが存在するか否かを判定すればよい。つまり、式(1)ではL0ノルムを用いて評価値Eが定義されている。このようにして、計測点bに対して距離ε内に計測点aが存在するか否かを探索する手法はr近傍探索と呼ばれている。

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

【0026】
図5(A)は、評価値Eの算出手法を説明する概念図である。図5(A)に示すように、時刻t-1の位置情報を構成するある計測点bに対し、時刻tの位置情報を構成するある計測点aは距離ε内に位置している。よって、式(1)はf(x)=0となり、評価値Eにはペナルティが課されない。一方、時刻tの位置情報を構成する別の計測点aは計測点bに対し、距離ε内に位置していない。よって、式(1)はf(x)=1となり、評価値Eには1のペナルティが課される。

【0027】
図5(B)は、時刻tの位置情報を構成する計測点a~aが回転移動される様子を示している。図5(C)は、回転移動された計測点a~aと計測点b~bとにおいて評価値Eが算出される様子を示した図である。

【0028】
図5(C)では計測点b,bから距離ε内に計測点a,aが位置しているが、計測点bから距離ε内に計測点aが存在していない。そのため、評価値Eは1となる。

【0029】
図1に戻り、推定部13は、時刻tの位置情報の平行移動量T及び回転移動量Rを変更させて、一致度を最大にする平行移動量T及び回転移動量Rを探索し、自己位置を推定する。つまり、推定部13は、式(1)に示す評価値Eを最小にする回転移動量R及び平行移動量Tを探索する。このような探索を行い、その探索結果に基づいてマップを作成し、自己位置を推定する手法はSLAMと呼ばれている。

【0030】
具体的には、推定部13は、時刻t-1における位置情報において、時刻tにおける位置情報のローカル座標空間を平行移動量Tで平行移動し、かつ、回転移動量Rで回転させ、時刻t-1の位置情報と時刻tの位置情報との評価値Eを算出する処理を繰り返し、評価値Eを最小にする平行移動量T及び回転移動量Rを探索する。

【0031】
そして、推定部13は、評価値Eを最小にする平行移動量Tを時刻t-1から時刻tまでの移動体の移動量として求め、時刻t-1における移動体の位置に移動量を加えた値を時刻tにおける移動体の位置として求める。

【0032】
また、推定部13は、評価値Eを最小にする回転移動量Rを時刻t-1から時刻tまでの移動体の向きの変化量として求め、時刻t-1における移動体の向きに変化量を加えた値を時刻tにおける移動体の向きとして求める。

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

【0034】
また、回転移動量Rとしては、例えば、時刻t-1における移動体の向きに対する時刻tにおける移動体の向きの回転を示す2×2の行列を採用することができる。また、平行移動量Tとしては、例えば、時刻t-1から時刻tまでの移動体の移動量を示すx成分とy成分との値を示す採用することができる。

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

【0036】
しかしながら、移動物体41が存在すると、移動物体41は時々刻々移動するため、時刻tにおける移動物体41及び静止物体42の位置関係と、時刻t-1における移動物体41及び静止物体42の位置関係とは異なってしまう。

【0037】
よって、評価値Eの最小値を求めたとしても、評価値EとしてL2ノルムを用いた場合は、移動物体41の影響により、時刻tにおける静止物体42に対して、時刻t-1における静止物体42を一致させることができない。その結果、静止物体42においてマッチング誤差が生じてしまうのである。

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

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

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

【0041】
位置センサ16は、レーザレンジファインダにより構成され、一定周期で時系列に位置情報を取得し、位置情報取得部11に供給する。レーザレンジファインダとしては、SICKのLMS100を採用することができる。図7(A)はSICKのLSM100の外観図である。図7(B)は本発明の実施の形態による移動体の外観図である。図7(B)に示すように、位置センサ16は移動体の前側に取り付けられている。

【0042】
図8は、位置センサ16の仕様を示した表である。図8に示すように位置センサ16は、画角が270度であり、解像度が0.5度であり、測定可能な範囲が20mであり、周波数が30Hzであり、取得できる計測点の個数が540個である。

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

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

【0045】
図7(B)に示すように移動体は、前方に設けられた左右一対の車輪91,91、後方に設けられた左右一対の車輪92,92を備えている。そして、一対の回転量センサ17は、それぞれ、車輪92,92に取り付けられ、車輪92,92の回転量を計測する。

【0046】
その他、移動体は図7(B)に示すように、車輪91,92の上部に設けられたフレーム93、フレーム93の底面931の表側に載置されたコンピュータ94、底面931の裏側に取り付けられたモータ95等を備えている。位置センサ16は、フレーム93の上部に設けられたバー932に取り付けられている。

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

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

【0049】
次に、図1に示す自己位置推定装置の動作について説明する。図2は、本発明の実施の形態による自己位置推定装置の動作を示すフローチャートである。まず、位置情報取得部11は時刻tにおける位置情報を取得する(S1)。

【0050】
次に、推定部13は、時刻tにおける位置情報の平行移動量Tと回転移動量Rとを変化させて複数の探索候補を生成し、各探索候補の評価値Eを一致度算出部12に算出させ、評価値Eを最小にする探索候補を探索する(S2)。

【0051】
次に、推定部13は、S2で探索した探索候補の回転移動量Rと平行移動量Tとから移動体の自己位置を推定する(S3)。次に、マップ生成部14は、推定部13により推定された自己位置を2次元の座標空間にプロットし、移動体の周辺空間の2次元マップを生成する(S4)。S4の処理が終了すると、処理がS1に戻され、S1~S4の処理が繰り返される。

【0052】
図3は、図2のS2に示す探索処理の詳細を示すフローチャートである。まず、推定部13は、変数kに0を代入し、変数kを初期化する(S21)。

【0053】
次に、推定部13は、n1(n1は2以上の整数)個の探索候補を生成する。具体的には、推定部13は、オドメトリ算出部15により推定された移動体の自己位置から移動体の位置の事前確率を求め、この事前確率にしたがって、ランダムにn1パターンの回転移動量R及び平行移動量Tを決定し、決定したn1パターンの回転移動量Rと平行移動量Tとを用いて時刻tの位置情報を時刻t-1の位置情報に対してずらし、n1個の探索候補を生成する(S22)。

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

【0055】
図9は探索候補が生成される処理を説明する図である。図9では、(A)~(D)に向けて処理が進む。図9の各点は時刻tの位置情報の回転移動量Rと平行移動量Tとの要素をまとめたものを点の位置として現してある。図9(A)では、S22の処理によってn1個の探索候補が生成されている。

【0056】
次に、一致度算出部12は、n1個の探索候補のそれぞれにつき、式(1)を用いて時刻tの位置情報との評価値Eを算出する(S23)。

【0057】
次に、推定部13は、変数kが定数K未満の場合(S24でNO)、n1個の探索候補の評価値Eからn1個の探索候補のそれぞれの重み値wjを算出する(S25)。ここで、重み値wjは式(2)によって規定される。

【0058】
wj=exp((m-E(R,T))/k) (2)
ここで、m、kは定数である。mとしては例えば想定される評価値Eの最大値を採用することができる。jは探索候補を特定するインデックスである。式(2)に示すように評価値Eが小さくなるほど、重み値wjは大きくなる。

【0059】
次に、推定部13は、重み値wjに基づき、n1個の探索候補の中からn2(n2<n1)個の探索候補を抽出する(S26)。この場合、推定部13は、重み値wjの値が大きいほど抽選確率が高くなるような抽選処理を行い、n2個の探索候補を抽出してもよいし、重み値wjが大きい順にn2個の探索候補を抽出してもよい。

【0060】
S26の処理により、図9(A)に示すn1個の探索候補から図9(B)に示すようにn2個の探索候補が抽出される。図9(B)の例では、n2=3と設定されているため、3個の探索候補が抽出されていることが分かる。

【0061】
図3に戻り、S27において、推定部13は、n2個の探索候補のそれぞれにつき、n3個の探索候補を生成する。この場合、推定部13は、n2個の探索候補の原点を基準とし、(σx,σy,σθ)の正規分布にしたがって、n3パターンの回転移動量R及び平行移動量Tを決定し、n3個の探索候補を生成すればよい。

【0062】
例えば、n2=3の場合、3個の探索候補の原点を基準としてn3個の探索候補が生成される。この場合、n2個の探索候補の重み値wjの値に応じてn3個の個数を変えても良い。例えば、n2=3の場合の探索候補をZ1~Z3とし、探索候補Z1~Z3の重み値がw1~w3であったとする。但し、w1>w2>w3である。この場合、探索候補Z1~Z3から生成される探索候補の個数をそれぞれn3(1)~n3(3)とすると、重み値w1に応じて、n3(1)~n3(3)の値を決定し、探索候補を生成すればよい。なお、n3(1)>n3(2)>n3(3)である。

【0063】
図9(C)に示すように、S27の処理により、n2個の探索候補を基準としてn3個の探索候補が生成され、合計、n2×n3個の探索候補が生成されていることが分かる。

【0064】
次に、推定部13は変数kを1インクリメントし(S28)、処理をS23に戻す。S24において、変数kが定数K以上になった場合(S23でNO)、推定部13は、評価値Eが最小の探索候補を特定する(S29)。

【0065】
なお、2ループ目以降のS23において、S27で算出された全探索候補に対して評価値Eが算出される。このように、推定部13は、S23~28の処理をK回繰り返し、評価値Eが最小の探索候補を特定する。そして、探索候補を生成するにあたり、解となる可能性が高い回転移動量R及び平行移動量Tを持つ探索候補を重点的に生成する重点的サンプリングを行う。そのため、解となる探索候補を効率良く求めることが可能となる。

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

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

【0068】
なお、図3において、S22が第1処理に相当し、S25,S26が第2処理に相当し、S27が第3処理に相当し、S24,S29が第4処理に相当する。

【0069】
次に、図3のS23に示す評価値の算出処理の詳細について説明する。ベクトルデータをハッシュ値に変換するアルゴリズムとして、LSH(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.)。

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

【0071】
L0ノルムを用いた評価値Eにおいて最小の評価値Eを探索する場合、r近傍を求める際の計算の負荷が大きい。r近傍を求める単純なアルゴリズム(例えば、Bruteforce)の計算量はO(n)であるが、kd-treeやボロノイ図を用いれば、計算量をO(logn)まで減らすことができる。

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

【0073】
図4は、評価値Eの算出処理の詳細を示すフローチャートである。まず、一致度算出部12は、時刻t-1の位置情報から複数のハッシュテーブルを生成する(S41)。ここで、ハッシュテーブルは、時刻t-1の位置情報の各計測点を所定のハッシュ関数を用いて格子空間に投影し、格子空間の各位置における計測点の存在の有無を示すテーブルである。格子空間は、時刻t-1における位置情報のローカル座標空間を距離εに基づき、格子状に区切ることで生成される空間である。ここで、ハッシュ関数hは式(3)により表される。

【0074】
h(b)=<(p・b+q)/ε> (3)
但し、<>は(p・b+q)/ε以下の最大の整数を示す。pは1を平均とする正規分布により決定される。qは一様分布により0からεの範囲でランダムに決定される。bは計測点を示している。

【0075】
本実施の形態では、以下に示す2つのハッシュ関数hx,hyを採用する。

【0076】
hx(b)=<(p・bx+qx)/ε> (3-1)
hy(b)=<(p・by+qy)/ε> (3-2)
なお、qx,qyは、qのx,y成分であり、一様分布により0からεの範囲でランダムに決定される。bx,byは計測点bのx,y成分である。

【0077】
計測点bはx軸及びy軸で規定されるローカル座標空間に位置している。したがって、一致度算出部12は、式(3-1)に示すように、hx(b)を用いて計測点bの格子空間におけるx座標の値を求め、式(3-2)に示すように、hy(b)を用いて計測点bの格子空間におけるy座標の値を求める。

【0078】
図10はハッシュテーブルを示した概念図である。図10(A)は時刻t-1の位置情報のローカル座標空間に設定された格子空間を示している。

【0079】
図10(B)は、図10(A)に示す格子空間に基づいて生成されたハッシュテーブルT1~T3を示している。図10(A)において、計測点bは式(3-1),(3-2)の<>の演算が施される前の位置にプロットされている。図10(A)において、例えば1行1列目の格子には計測点bが存在している。よって、この計測点bに<>の演算を施してハッシュ値hx(b),hy(b)を求めると、図10(B)に示すように、計測点bはハッシュテーブルT1の1行1列目のビンに位置することになる。よって、ハッシュテーブルT1の1行1列目のビンには計測点bの存在を示す、例えば1のラベルが設定される。一方、図10(A)において、例えば1行3列目の格子には計測点bが存在していない。よって、図10(B)に示すように、ハッシュテーブルT1の1行3列目のビンには計測点bの非存在を示す例えば0のラベルが設定される。

【0080】
このようにして、計測点bが式(3-1)、(3-2)を用いて格子空間に投影され、ハッシュテーブルT1が生成される。

【0081】
ここで、一致度算出部12は、式(3-1)、(3-2)に示すハッシュ関数を複数種類用意して、複数のハッシュテーブルを生成する。図10(B)の例では、3種のハッシュ関数h1(=(h1x(b),h1y(b)))~h3(=(h3x(b),h3y(b)))を用いて3個のハッシュテーブルT1~T3が生成されている。

【0082】
この場合、一致度算出部12は、式(3-1)、(3-2)に示す、p,qx,qyの値を異なる値に設定することで複数種類のハッシュ関数を生成すればよい。

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

【0084】
なお、上記では、(3-1)、(3-2)で示すハッシュ関数を用いたが、(3-1’)、(3-2’)で示すハッシュ関数を採用してもよい。

【0085】
hx(b)=<(bx+qx)/pε> (3-1’)
hy(b)=<(by+qy)/pε> (3-2’)

【0086】
(3-1)、(3-2)との違いは、pが分母に移され、εに乗じられている点である。これにより、ローカル座標空間に設定される格子の長さが変わり、ハッシュテーブルにより多くの多様性を持たせることができる。

【0087】
図4に戻り、S42において、一致度算出部12は、時刻tの位置情報の各計測点aをハッシュ関数h1~h3を用いて投影空間に投影し、計測点aのハッシュ値h1(a)~h3(a)を求める。

【0088】
次に、一致度算出部12は、求めたハッシュ値h1(a)~h3(a)のいずれかが対応するハッシュテーブルT1~T3に存在すれば、評価値Eにペナルティを課さずに評価値Eを算出する(S43)。

【0089】
例えば、ハッシュ値h1(a)で示されるハッシュテーブルT1のビンに1のラベルが設定されていれば、ハッシュ値h2(a),h3(a)で示されるハッシュテーブルT2,T3のビンに0のラベルが設定されていても、計測点aから距離ε内に計測点bが位置するとみなされ、評価値Eにペナルティが課されない。

【0090】
一致度算出部12は、このような処理を各計測点aに対して行い、付与したペナルティの合計値を評価値Eとして算出する。このように時刻t-1の計測点bに基づいて予めハッシュテーブルを生成しておくことで、ハッシュテーブルの参照にかかる計算量をO(1)にすることができる。

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

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

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

【0094】
実験場所として、奈良先端科学技術大学院大学情報棟1階を用いた。動的環境をLRFで計測し、時刻t,時刻t-1の位置情報のマッチングを行った。この実験において、移動体は静止した状態で動的環境の計測を行った。動的環境は幅10m程の廊下で奥行き20mの範囲で、25人前後の歩行者が移動していた。

【0095】
図11(A)は実験場所を示した図であり、図11(B)はこの実験場所に人物が行き来する動的環境を示した図である。

【0096】
図12は、実験場所に人物が行き来していない静的環境下で生成した周辺空間の2次元マップを示している。この2次元マップはRBPF-SLAMを用いて生成されたものであり真値とみなせる。

【0097】
<実験結果>
この実験では最小の評価値Eを探索する際の探索範囲を、x軸方向、y軸方向にそれぞれプラスマイナス 20[cm]とし、角度を プラスマイナス0.3[rad]と設定した。そして、この探索範囲において、1.0[cm]、0.01[rad](=約0.57[deg])刻みで総当たり法で、回転移動量R、平行移動量Tを変化させて、評価値Eの最小値を探索した。

【0098】
そして、評価値EとしてL0ノルムを採用した場合、L2ノルムを採用した場合、M-estimatorを採用した場合のマッチングの位置推定精度の比較を行った。この際、M-estimatorにはBiweight法に基づき、距離dに対し、以下の評価関数ρ(d)を用いた。

【0099】
ρ(d)=B/2 (d≧B)
or
(B/2)(1-(1-(d/B)) (d<B)

【0100】
ここで、Bは任意の残差値であり、0.01[m]とした。図13は、評価値Eとして、L2ノルム、M-estimator、及びL0ノルムを用いた場合の実験結果を示した表である。

【0101】
L2ノルムでは最大14[cm]のマッチング誤差が発生しており、平均二乗誤差(RMSE)も10[cm]ほどあった。一方、L0ノルムではマッチング誤差は最大1[cm]であり、LRFの計測誤差内に収まっていることがわかる。

【0102】
次に、LRFにより200フレーム分の位置情報を測定し、L2ノルム、M-estimator、及びL0ノルムをそれぞれ用いて隣接するフレーム同士の位置情報のマッチングを行い、2次元マップを生成した。

【0103】
図14は、L2ノルムを採用した場合に作成した2次元マップである。図15は、M-estimatorを採用した場合に作成した2次元マップである。図16は、L0ノルムを採用した場合に作成した2次元マップである。

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

【0105】
図15に示すように、M-estimatorを採用した場合、大きなマッチング誤差はなかったが、累積したマッチング誤差により、移動体が移動していると誤認識され、2次元マップの生成に失敗している。一方、図16に示すように、L0ノルムを採用した場合、フレーム間のマッチング誤差がほとんど発生せず、自己位置推定装置は移動体が静止している状態を計測できている。

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

【0107】
次に、本実施の形態による自己位置推定装置の優位性を検証するため、LSHを採用した場合、Brute-forceを採用した場合、及びkd-treeを採用した場合の計算時間の比較を行った。なお、評価値EとしてはL0ノルムを採用した。探索範囲は、上記の値と同じである。合計24000通りの評価値Eを計算し、総計算時間(Average calculation time)、1秒あたりに計算できるL0ノルムの数(Frequency of calculation aec.)、及びr近傍点が発見できた計測点の平均個数(Average match points)を求めた。

【0108】
図17は、Brute-force、kd-tree、LSHを用いて計算時間を比較する実験を行った場合の実験結果をまとめた表である。

【0109】
図17に示すように、Brute-force、kd-treeに比べ、LSHを用いた場合、総計算時間が短いことがわかる。しかしながら、Brute-force及びkd-treeに比べ、LSHはr近傍点が発見できた計測点の平均個数(Average match points)が15個ほど少ない。これは、LSHによる近傍点探索が近似であることが原因である。

【0110】
この一致点数の差の影響を、複数フレームのマッチングにより2次元マップを生成することで調べる。図18は静的環境下でRBPF-SLAMを用いて生成した2次元マップである。この2次元マップを真値と見なす。

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

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

【0113】
更に、図21に示すような大規模な2次元マップを生成した。図21は、RBPF-SLAMを用いて生成した2次元マップである。この2次元マップを真値と見なす。

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

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

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

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

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

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

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

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

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

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

【0124】
(5)前記第4処理は、前記第2、第3処理を所定回数繰り返し、前記重み値が最大となる探索候補を前記探索解として求めることが好ましい。

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

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

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

【0128】
(7)上記の移動体は、(1)~(6)のいずれかの移動体と、前記位置情報を取得する位置センサとを備える。

【0129】
この構成によれば、未知環境下において自己位置を精度良く推定しながら、移動することができる移動体を提供することができる。
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7
【図9】
8
【図10】
9
【図11】
10
【図12】
11
【図13】
12
【図14】
13
【図15】
14
【図16】
15
【図17】
16
【図18】
17
【図19】
18
【図20】
19
【図21】
20
【図22】
21
【図23】
22