推定装置、推定方法、および推定プログラム
【課題】移動体が特定の領域に位置する場合の推定精度の向上を図ること。
【解決手段】推定装置は、粒子フィルタを用いたシミュレーションにより、直前の移動体の状態を示す粒子群から、現在の移動体の状態を示す粒子群ｐｓを推定する。次に、推定装置は、推定された現在の移動体の状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域の状態を示す心配粒子群ａｓを生成する。そして、推定装置は、推定された粒子群ｐｓと生成された心配粒子群ａｓとの各粒子の尤度に基づいて、粒子群ｐｓをリサンプリングする。これにより、推定装置は、移動体の真の位置が危険領域の近傍領域にある場合に、移動体の位置の推定精度の向上を図ることができる。
【解決手段】推定装置は、粒子フィルタを用いたシミュレーションにより、直前の移動体の状態を示す粒子群から、現在の移動体の状態を示す粒子群ｐｓを推定する。次に、推定装置は、推定された現在の移動体の状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域の状態を示す心配粒子群ａｓを生成する。そして、推定装置は、推定された粒子群ｐｓと生成された心配粒子群ａｓとの各粒子の尤度に基づいて、粒子群ｐｓをリサンプリングする。これにより、推定装置は、移動体の真の位置が危険領域の近傍領域にある場合に、移動体の位置の推定精度の向上を図ることができる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、推定装置、推定方法、および推定プログラムに関する。
【背景技術】
【０００２】
従来、粒子フィルタ（パーティクルフィルタ：ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ）を用いた移動体の状態推定の技術がある。粒子フィルタとは、移動体の状態の確率分布を移動体の或る状態を示す粒子（パーティクル）の集合により近似する方法である。
【０００３】
粒子フィルタを用いた移動体の状態推定の技術では、シミュレーションにより、過去の移動体の状態を示す粒子群から、現在の移動体の状態を示す粒子群が推定される。そして、推定された現在の移動体の状態を示す粒子群の各粒子の尤もらしさ（尤度）に基づいて、粒子群のリサンプリングが行われる。粒子群のリサンプリングでは、シミュレーションにより推定された粒子群の中から、現在の移動体の状態を示す粒子群となる粒子が選択し直され、現在の移動体の状態が推定し直される。
【０００４】
リサンプリングの技術としては、移動体が有するレーザレーダの測定値に基づいて粒子フィルタを用いて推定された自己位置を補正する技術がある（例えば、下記特許文献１参照）。また、移動体が赤外線照射装置から受信した赤外線照射装置に固有のＩＤ信号により、粒子フィルタを用いて推定された自己位置を補正する技術がある（例えば、下記特許文献２参照）。
【０００５】
また、移動体の状態の推定に用いる粒子の数を動的に変化させる技術がある（例えば、下記特許文献３参照）。また、幾何マップからグリッドマップを生成し、生成したグリッドマップから物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成し、生成したボロノイグラフ上で物体との衝突確率と経路長とから経路探索を行う技術がある（例えば、下記特許文献４参照）。また、複数センサからの情報を統合して、自己位置を推定する技術がある（例えば、下記特許文献５参照）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２２３５０４号公報
【特許文献２】特開２００９−１７６０３１号公報
【特許文献３】特開２０１０−２２４７５５号公報
【特許文献４】特開２００５−３２１９６号公報
【特許文献５】特開２００８−３３６９６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した従来技術では、粒子フィルタを用いたシミュレーションにより推定された現在の移動体の状態を示す粒子群に、移動体の真の状態を示す粒子が含まれない場合がある。例えば、シミュレーションによる計算誤差が、シミュレーションが行われるごとに蓄積されていった場合である。この場合、リサンプリングにおいて、推定された現在の移動体の状態を示す粒子群の中から現在の移動体の状態を示す尤もらしい粒子群を選択し直しても、選択し直した粒子群には移動体の真の状態を示す粒子が含まれない。結果として、上述した従来技術では、選択し直した粒子群が示す移動体の状態から移動体の位置を誤って推定してしまい、移動体が進入すべきではないと設定された領域に進入してしまう場合がある。
【０００８】
本発明は、上述した従来技術による問題点を解消するため、移動体が特定の領域に位置する場合の推定精度の向上を図ることができる推定装置、推定方法、および推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するため、本発明の一側面によれば、粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、移動体の状態を示す第１の粒子群を現状態から次状態に更新し、更新される都度、移動体と空間内の物体との距離を測定し、更新される都度、空間内の特定の領域の状態を示す第２の粒子群を生成し、測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、第１および第２の粒子群の各粒子の尤度を算出し、算出された各粒子の尤度に基づいて、空間内での移動体の位置を特定する推定装置、推定方法、および推定プログラムが提案される。
【発明の効果】
【００１０】
本発明の一側面によれば、移動体が特定の領域に位置する場合の推定精度の向上を図ることができるという効果を奏する。
【図面の簡単な説明】
【００１１】
【図１】図１は、実施の形態にかかる推定装置による移動体の位置推定の内容を示す説明図である。
【図２】図２は、実施の形態にかかる推定装置のハードウェア構成例を示すブロック図である。
【図３】図３は、推定装置２００が有する物体テーブルの記憶内容の一例を示す説明図である。
【図４】図４は、推定装置２００が有する危険領域テーブルの記憶内容の一例を示す説明図である。
【図５】図５は、推定装置２００が有する粒子テーブルの記憶内容の一例を示す説明図である。
【図６】図６は、推定装置２００が有する心配粒子テーブルの記憶内容の一例を示す説明図である。
【図７】図７は、推定装置２００が有するセンサ情報テーブルの記憶内容の一例を示す説明図である。
【図８】図８は、推定装置２００が有する尤度テーブルの記憶内容の一例を示す説明図である。
【図９】図９は、推定装置２００の機能的構成を示すブロック図である。
【図１０】図１０は、実施例１にかかる推定装置２００の動作例１を示す説明図（その１）である。
【図１１】図１１は、実施例１にかかる推定装置２００の動作例１を示す説明図（その２）である。
【図１２】図１２は、実施例１にかかる推定装置２００の動作例１を示す説明図（その３）である。
【図１３】図１３は、実施例１の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。
【図１４】図１４は、実施例１にかかる推定装置２００の動作例２を示す説明図である。
【図１５】図１５は、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１６】図１６は、実施例１にかかる推定装置２００の動作例３を示す説明図である。
【図１７】図１７は、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１８】図１８は、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１９】図１９は、実施例２にかかる推定装置２００による前方車両の位置推定の内容を示す説明図である。
【図２０】図２０は、可変の危険領域Ｄの例を示す説明図である。
【図２１】図２１は、実施例２の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。
【図２２】図２２は、実施例２にかかる推定装置２００の動作例２を示す説明図である。
【図２３】図２３は、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。
【図２４】図２４は、実施例２にかかる推定装置２００の動作例３を示す説明図である。
【図２５】図２５は、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。
【図２６】図２６は、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）の処理内容を示すフローチャートである。
【発明を実施するための形態】
【００１２】
以下に添付図面を参照して、この発明にかかる推定装置、推定方法、および推定プログラムの実施の形態を詳細に説明する。
【００１３】
（実施の形態にかかる推定装置による移動体の位置推定の内容）
まず、本実施の形態にかかる推定装置による移動体の位置推定の内容について説明する。
【００１４】
図１は、実施の形態にかかる推定装置による移動体の位置推定の内容を示す説明図である。図１の例は、移動体が推定装置自体である場合での移動体の位置推定の例である。具体的には、推定装置として、自律移動ロボットＲ（以下では、「ロボットＲ」という）を採用した場合について説明する。
【００１５】
ロボットＲは、ロボットＲの移動する領域内に存在する危険領域Ｄおよび障害物Ｏの位置情報を有する。また、ロボットＲは、ロボットＲの移動する領域内でのロボットＲの或る状態を示す粒子ｐの集合である粒子群ｐｓを用いたシミュレーションにより、ロボットＲの状態を推定する。そして、ロボットＲは、危険領域Ｄおよび障害物Ｏの位置情報と、推定された状態と、に基づいて、ロボットＲの移動する領域内で危険領域Ｄを回避しつつ移動を行う。ロボットＲの状態とは、例えば、ロボットＲの位置、速度、および進行方向である。
【００１６】
図１の（Ａ）において、（１）ロボットＲは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」でのロボットＲの状態を示す粒子群ｐｓから現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓを推定する。そして、ロボットＲは、推定された現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓにより、現在時刻「ｔ」での自己位置を位置Ｓ１であると推定する。
【００１７】
しかしながら、ロボットＲの真の自己位置は危険領域Ｄの近傍領域にある位置Ｓ２であり、ロボットＲは自己位置を誤って推定している。なお、ロボットＲが自己位置を誤って推定する原因としては、例えば、人間によってロボットＲが強制移動させられること（いわゆる誘拐ロボット問題）が挙げられる。また、ロボットＲが自己位置を誤って推定する原因としては、例えば、ロボットＲが移動に用いる車輪の空回りやロボットＲが障害物に衝突したときの姿勢の変化が挙げられる。
【００１８】
また、ロボットＲが自己位置を誤って推定する原因としては、例えば、障害物Ｏの位置情報と実際の障害物Ｏの位置との誤差や、シミュレーションによる計算誤差が、シミュレーションごとに蓄積していくことが挙げられる。また、ロボットＲが自己位置を誤って推定する原因としては、例えば、粒子群ｐｓの粒子ｐの個数が少ないため、移動体の状態の確率分布の近似が不十分であることが挙げられる。
【００１９】
図１の（Ｂ）において、（２）ロボットＲは、現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓに加えて、心配粒子群ａｓを生成する。心配粒子群ａｓとは、危険領域Ｄの位置情報に基づいて危険領域Ｄの近傍領域でのロボットＲの或る状態を示す心配粒子ａの集合である。
【００２０】
（３）次に、ロボットＲは、距離センサを用いて、ロボットＲの位置から障害物Ｏまでの距離を測定する。
【００２１】
（４）そして、ロボットＲは、障害物Ｏの位置と、粒子群ｐｓと心配粒子群ａｓとの各粒子が示すロボットＲの位置と、に基づいて、ロボットＲから障害物Ｏまでの距離を算出する。ロボットＲは、測定したロボットＲの位置から障害物Ｏまでの距離、および算出した各粒子が示すロボットＲの位置から障害物Ｏまでの距離に基づいて、現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓのリサンプリングを行う。
【００２２】
リサンプリングとは、粒子群ｐｓと心配粒子群ａｓとの各粒子の中から、粒子が示す状態の尤度が高い粒子を優先して粒子群ｐｓの粒子ｐとして選択し、粒子群ｐｓを選択し直すことである。このとき、同一の粒子を複数回選択してもよい。そして、選択し直された粒子群ｐｓの示す状態が、現在時刻「ｔ」でのロボットＲの状態として推定し直される。
【００２３】
ここでは、粒子群ｐｓの各粒子ｐが示すロボットＲの位置から障害物Ｏまでの距離は、測定したロボットＲの位置から障害物Ｏまでの距離より短くなっている。例えば、粒子群ｐｓの粒子ｐ１が示すロボットＲの位置から障害物Ｏまでの距離ｄｐは、ロボットＲの位置から障害物Ｏまでの測定距離ｄｒより短くなっている。このため、ロボットＲは、「粒子群ｐｓの各粒子ｐの示すロボットＲの位置が実際のロボットＲの位置とはかけ離れている」として、尤度が低いと判定する。
【００２４】
一方、（Ｂ）に示した心配粒子群ａｓの各粒子ａの位置から障害物Ｏまでの距離は、測定したロボットＲの位置から障害物Ｏまでの距離とほぼ同一である。例えば、心配粒子群ａｓの粒子ａ１の位置から障害物Ｏまでの距離ｄａは、ロボットＲの位置から障害物Ｏまでの測定距離ｄｒとほぼ同一である。このため、ロボットＲは、「心配粒子群ａｓの各粒子ａの示すロボットＲの位置が実際のロボットＲの位置に近い」として、尤度が高いと判定する。
【００２５】
そして、ロボットＲは、粒子群ｐｓの各粒子ｐより尤度が高い心配粒子群ａｓの各粒子ａを優先して、粒子群ｐｓの粒子ｐとして選択し直す。そして、ロボットＲは、選択し直された粒子群ｐｓにより、現在時刻「ｔ」でのロボットＲの状態を推定し直す。
【００２６】
図１の（Ｃ）において、（５）ロボットＲは、リサンプリング結果から推定し直された現在時刻「ｔ」でのロボットＲの状態から、現在時刻「ｔ」での自己位置を位置Ｓ２であると推定する。そして、ロボットＲは、危険領域Ｄの位置情報に基づいて、推定された自己位置である位置Ｓ２が危険領域Ｄの近傍領域に含まれると判定して、危険領域Ｄから離れるように移動することで、危険領域Ｄへの進入を回避する。
【００２７】
このように、ロボットＲは、真の自己位置が危険領域Ｄの近傍領域にある場合、尤度の高い心配粒子ａが粒子群ｐｓの粒子ｐとして選択されるため、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【００２８】
（推定装置のハードウェア構成例）
次に、図２を用いて、推定装置のハードウェア構成例について説明する。
【００２９】
図２は、実施の形態にかかる推定装置のハードウェア構成例を示すブロック図である。図２において、推定装置２００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０１と、ＲＯＭ（Ｒｅａｄ‐Ｏｎｌｙ Ｍｅｍｏｒｙ）２０２と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２０３と、センサ２０４と、出力装置２０５と、駆動装置２０６と、を備えている。また、各構成部はバス２１０によってそれぞれ接続されている。
【００３０】
ここで、ＣＰＵ２０１は、推定装置２００の全体の制御を司る。ＲＯＭ２０２は、ブートプログラムなどのプログラムを記憶している。ＲＡＭ２０３は、ＣＰＵ２０１のワークエリアとして使用される。
【００３１】
センサ２０４は、電磁波の送受信を行い、受信した電磁波を電気信号に変換し、ＲＡＭ２０３に保持する。例えば、センサ２０４として、赤外線受光素子などの距離センサを採用できる。また、センサ２０４は、光学的な映像を電気信号に変換し、ＲＡＭ２０３に保持する。例えば、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）、ＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ−Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）センサなどの光学的な映像を電気信号に変換する光電変換素子を採用できる。
【００３２】
出力装置２０５は、データの出力を行う。出力装置２０５としては、例えば、ディスプレイ、スピーカ等を用いることができる。駆動装置２０６は、ＣＰＵ２０１の制御により推定装置２００を駆動させる。駆動装置２０６としては、例えば、モータ、エンジン、ブレーキ等を用いることができる。推定装置２００としては、例えば、自己位置を推定しながら移動する自律移動ロボットＲや、周辺車両の位置を推定しながら移動する車両が挙げられる。
【００３３】
（物体テーブルの記憶内容）
次に、図３を用いて、推定装置２００が有する物体テーブルの記憶内容の一例について説明する。物体テーブルとは、移動体が移動する領域内の物体の形状や位置を記憶するテーブルである。物体としては、例えば、図１に示した障害物Ｏや壁が挙げられる。物体テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００３４】
図３は、推定装置２００が有する物体テーブルの記憶内容の一例を示す説明図である。図３に示すように、物体テーブル３００は、物体ＩＤ項目のそれぞれに対応付けて、形状項目と、位置項目と、を有し、物体ごとにレコードを構成する。
【００３５】
物体ＩＤ項目には、移動体が移動する領域内の物体を特定する識別子が記憶されている。形状項目には、物体ＩＤ項目の内容により特定される物体の形状が記憶されている。具体的には、例えば、形状項目には、物体の形状の種別フラグと、当該種別の形状を特定する値と、が記憶される。より具体的には、例えば、形状項目には、物体の形状が直方体であることを示す種別フラグと、直方体の奥行きと、直方体の幅と、直方体の高さと、が記憶される。また、より具体的には、例えば、形状項目には、物体の形状が円柱であることを示す種別フラグと、円柱底面の中心の位置と、円柱底面の半径と、円柱の高さと、が記憶されてもよい。
【００３６】
位置項目には、物体ＩＤ項目の内容により特定される物体の位置が記憶されている。具体的には、例えば、位置項目には、物体の位置を示す座標値（例えば、３次元空間でのｘ座標値とｙ座標値とｚ座標値）が記憶される。また、位置項目には、物体の姿勢（例えば、３次元空間でのｘ軸に対する角度とｙ軸に対する角度とｚ軸に対する角度）が記憶されていてもよい。
【００３７】
（危険領域テーブルの記憶内容）
次に、図４を用いて、推定装置２００が有する危険領域テーブルの記憶内容の一例について説明する。危険領域テーブルとは、移動体が進入すべきではないと設定された領域（危険領域Ｄ）の近傍領域を記憶するテーブルである。危険領域テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００３８】
図４は、推定装置２００が有する危険領域テーブルの記憶内容の一例を示す説明図である。図４に示すように、危険領域テーブル４００は、領域ＩＤ項目のそれぞれに対応付けて、近傍領域項目と、心配粒子ＩＤリスト項目と、を有し、危険領域Ｄごとにレコードを構成する。
【００３９】
領域ＩＤ項目には、移動体が進入すべきではないと設定された領域（危険領域Ｄ）を特定する識別子が記憶されている。近傍領域項目には、領域ＩＤ項目の内容により特定される危険領域Ｄの近傍領域の範囲を特定する情報が記憶されている。具体的には、例えば、近傍領域項目には、近傍領域が矩形であれば、辺を共有しない２頂点の座標値が記憶される。心配粒子ＩＤリスト項目には、移動体の或る状態を示す心配粒子ａを特定する識別子が記憶されている。
【００４０】
（粒子テーブルの記憶内容）
次に、図５を用いて、推定装置２００が有する粒子テーブルの記憶内容の一例について説明する。粒子テーブルとは、推定された移動体の位置を示す粒子ｐを記憶するテーブルである。粒子テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００４１】
図５は、推定装置２００が有する粒子テーブルの記憶内容の一例を示す説明図である。図５に示すように、粒子テーブル５００は、粒子ＩＤ項目のそれぞれに対応付けて、変数項目（図５では変数１項目〜変数ｋ項目）を有し、粒子ｐごとにレコードを構成する。
【００４２】
粒子ＩＤ項目には、移動体が取り得る或る状態を示す粒子ｐを特定する識別子が記憶されている。変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐが示す状態を特定する変数の値が記憶されている。変数項目は、図５に示すように複数あってもよいし、１つであってもよい。
【００４３】
例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、３次元空間でのｘ座標値とｙ座標値とｚ座標値が記憶される。
【００４４】
また、例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの速度が記憶されている。なお、速度の単位は、例えば、「ｍ／ｓ（ｍｅｔｅｒ／ｓｅｃｏｎｄ）」である。また、例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を０°としたときの粒子ｐの進行方向の角度が記憶される。進行方向の単位は、例えば、「°」である。
【００４５】
（心配粒子テーブルの記憶内容）
次に、図６を用いて、推定装置２００が有する心配粒子テーブルの記憶内容の一例について説明する。心配粒子テーブルとは、危険領域Ｄの近傍領域の状態を示す心配粒子ａを記憶するテーブルである。心配粒子テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００４６】
図６は、推定装置２００が有する心配粒子テーブルの記憶内容の一例を示す説明図である。図６に示すように、心配粒子テーブル６００は、心配粒子ＩＤ項目のそれぞれに対応付けて、変数項目（図６では変数１項目〜変数ｋ項目）を有し、心配粒子ａごとにレコードを構成する。
【００４７】
心配粒子ＩＤ項目には、危険領域Ｄの近傍領域で移動体が取り得る或る状態を示す心配粒子ａを特定する識別子が記憶されている。変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａが示す状態を特定する変数の値が記憶されている。変数項目は、図６に示すように複数あってもよいし、１つであってもよい。
【００４８】
例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、３次元空間でのｘ座標値とｙ座標値とｚ座標値が記憶される。
【００４９】
また、例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの速度が記憶されている。なお、速度の単位は、例えば、「ｍ／ｓ（ｍｅｔｅｒ／ｓｅｃｏｎｄ）」である。また、例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を０°としたときの心配粒子ａの進行方向の角度が記憶される。
【００５０】
（センサ情報テーブルの記憶内容）
次に、図７を用いて、推定装置２００が有するセンサ情報テーブルの記憶内容の一例について説明する。センサ情報テーブルとは、推定装置２００が有するセンサの測定値を記憶するテーブルである。センサ情報テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００５１】
図７は、推定装置２００が有するセンサ情報テーブルの記憶内容の一例を示す説明図である。図７に示すように、センサ情報テーブル７００は、時刻項目のそれぞれに対応付けて、測定値項目（図７では、測定値１項目〜測定値Ｍ項目）を有し、センサ２０４による測定を行った時刻ごとにレコードを構成する。
【００５２】
時刻項目には、センサ２０４による測定を行った時刻が記憶されている。測定値項目には、時刻項目の時刻で測定されたセンサ２０４の値が記憶されている。測定値項目は、図７に示すように複数あってもよいし、１つであってもよい。例えば、移動体の前方、後方、左方、および右方のそれぞれの方向に存在する障害物Ｏまでの距離を記憶する測定値項目があってもよい。
【００５３】
具体的には、例えば、測定値項目には、距離センサによって測定された移動体から障害物Ｏまでの距離が記憶される。測定値項目の単位は、例えば、「ｍ（ｍｅｔｅｒ）」である。
【００５４】
（尤度テーブルの記憶内容）
次に、図８を用いて、推定装置２００が有する尤度テーブルの記憶内容の一例について説明する。尤度テーブルとは、推定装置２００により算出された粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度を記憶するテーブルである。尤度テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００５５】
図８は、推定装置２００が有する尤度テーブルの記憶内容の一例を示す説明図である。図８に示すように、尤度テーブル８００は、粒子ＩＤ項目のそれぞれに対応付けて、尤度項目を有し、粒子ｐごとにレコードを構成する。また、尤度テーブル８００は、移動体の状態を推定するごとにレコードを更新する。
【００５６】
粒子ＩＤ項目には、粒子群ｐｓと心配粒子群ａｓとの各粒子を特定する識別子が記憶されている。尤度項目には、粒子ＩＤ項目の内容から特定される粒子が示す移動体の状態の尤もらしさを示す尤度が記憶されている。
【００５７】
（推定装置２００の機能的構成例）
次に、図９を用いて、推定装置２００の機能的構成例について説明する。
【００５８】
図９は、推定装置２００の機能的構成を示すブロック図である。推定装置２００は、更新部９０１と、生成部９０３と、測定部９０２と、算出部９０４と、判断部９０５と、設定部９０６と、特定部９０７と、判定部９０８と、制御部９０９と、を含む構成である。
【００５９】
更新部９０１は、粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、移動体の状態を示す第１の粒子群を現状態から次状態に更新する。ここで、空間とは、移動体の移動する領域である。例えば、空間とは、移動体がロボットＲであれば、ロボットＲの移動する部屋や建物であり、移動体が車両であれば、道路である。状態方程式とは、過去の移動体の状態を入力すると移動体の現在の状態を決定する方程式である。
【００６０】
例えば、状態方程式とは、運動方程式であって、時刻「ｔ−１」での移動体の位置と速度を入力すると、時刻「ｔ」での移動体の位置と速度を決定する方程式である。また、状態方程式とは、後述する式（２）や式（５）である。移動体の状態を示す第１の粒子群とは、移動体の状態の確率分布を近似する粒子群ｐｓである。粒子群ｐｓの１粒子は、移動体の取り得る或る状態を示す。
【００６１】
具体的には、例えば、更新部９０１は、粒子テーブル５００に記憶されている直前の時刻での移動体の状態を示す粒子群ｐｓの各粒子ｐの状態を状態方程式に入力して、現在時刻での各粒子ｐの状態を算出する。更新部９０１は、算出された現在時刻での各粒子ｐの状態により、粒子テーブル５００を更新する。
【００６２】
これにより、更新部９０１は、移動体の過去の状態から移動体の現在の状態を推定することができる。なお、更新部９０１は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００６３】
測定部９０２は、更新部９０１によって更新される都度、移動体と空間内の物体との距離を測定する。ここで、移動体と空間内の物体との距離とは、例えば、移動体が推定装置２００自体である場合には、推定装置２００から空間内の物体（障害物Ｏや壁）までの距離である。また、移動体と空間内の物体との距離とは、例えば、移動体と推定装置２００とが異なる場合は、移動体から空間内の物体（推定装置２００自体）までの距離である。具体的には、例えば、測定部９０２は、更新部９０１によって更新される都度、センサ２０４により、移動体と空間内の物体との距離を測定する。
【００６４】
これにより、測定部９０２は、算出部９０４により各粒子の尤度の算出に使用される移動体から空間内の物体までの距離を得ることができる。なお、測定部９０２は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、または、センサ２０４により、その機能を実現する。
【００６５】
生成部９０３は、更新部９０１によって更新される都度、空間内の特定の領域の状態を示す第２の粒子群を生成する。ここで、特定の領域とは、予め設定された移動体が進入すべきではない禁止領域の近傍領域である。禁止領域とは、例えば上述した危険領域Ｄである。また、近傍領域とは、禁止領域の周囲の領域である。禁止領域や近傍領域の範囲は、任意に設定される。第２の粒子群とは、危険領域Ｄの近傍領域での移動体の状態を示す粒子群であり、例えば、上述した心配粒子群ａｓである。具体的には、例えば、生成部９０３は、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出し、心配粒子群ａｓを生成する。
【００６６】
これにより、生成部９０３は、危険領域Ｄの近傍領域に心配粒子ａを生成して、特定部９０７による移動体の位置の特定において、移動体の真の位置が危険領域Ｄの近傍領域にあれば、移動体の位置を危険領域Ｄの近傍領域にあると特定できるようにすることができる。生成部９０３は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００６７】
算出部９０４は、測定部９０２によって測定された距離と、更新部９０１による更新されたときの第１の粒子群と、生成部９０３によって生成された第２の粒子群と、に基づいて、第１および第２の粒子群の各粒子の尤度を算出する。ここで、尤度とは、粒子が示す移動体の状態の尤もらしさを示す値である。尤度としては、例えば、測定により得られた実際の移動体の状態での測定値を期待値とした正規分布において、各粒子の示す移動体の状態から予測される測定値となる確率を採用することができる。
【００６８】
具体的には、例えば、算出部９０４は、更新部９０１によって更新されたときの粒子群ｐｓの各粒子ｐが示す移動体の状態から、粒子群ｐｓの各粒子ｐが示す移動体の位置から空間内の物体までの距離を算出する。次に、算出部９０４は、生成部９０３によって生成された心配粒子群ａｓの各心配粒子ａが示す移動体の状態から、心配粒子群ａｓの各粒子ａが示す移動体の位置から空間内の物体までの距離を算出する。
【００６９】
そして、算出部９０４は、測定部９０２によって測定された移動体から空間内の物体までの距離を期待値とする正規分布において、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す移動体の位置から空間内の物体までの距離となる確率を算出する。算出部９０４は、算出された確率を各粒子の尤度として、尤度テーブル８００を更新する。
【００７０】
ここで、算出部９０４は、測定部９０２によって距離を測定された物体が複数ある場合は、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す移動体の位置からそれぞれの物体までの距離となる確率を算出する。そして、算出部９０４は、物体ごとに算出された確率の総乗を各粒子の尤度として、尤度テーブル８００を更新する。
【００７１】
これにより、算出部９０４は、粒子群ｐｓの各粒子ｐの尤度と心配粒子群ａｓの各粒子ａの尤度を算出することができる。なお、算出部９０４は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００７２】
判断部９０５は、算出部９０４によって前回算出された尤度がしきい値以上であるか否かを判断する。ここで、しきい値は、推定装置２００の利用者によって予め設定された値である。上述した正規分布における確率を尤度に採用した場合、しきい値としては、例えば、「期待値±標準偏差の値が測定値となる確率」を採用することができる。具体的には、例えば、判断部９０５は、算出部９０４によって前回算出された粒子群ｐｓの各粒子ｐの尤度がしきい値以上であるか否かを判断し、尤度がしきい値以上の粒子ｐがあれば、時刻と尤度がしきい値以上の粒子ｐの状態とをＲＡＭ２０３などの記憶領域に記憶する。
【００７３】
また、判断部９０５は、算出部９０４によって前回算出された粒子群ｐｓの各粒子ｐの尤度の総和がしきい値以上であるか否かを判断し、尤度の総和がしきい値以上であれば、時刻と各粒子ｐの状態とをＲＡＭ２０３などの記憶領域に記憶してもよい。これにより、判断部９０５は、尤度がしきい値以上であり、推定の精度が高いと解される移動体の状態を記憶しておくことができる。
【００７４】
この場合、生成部９０３は、判断部９０５によって前回算出された尤度がしきい値以上であると判断された場合には、第２の粒子群の生成を停止する。具体的には、例えば、生成部９０３は、判断部９０５によって前回の移動体の状態を示す粒子群ｐｓのいずれかの粒子ｐの尤度がしきい値以上であると判断された場合には、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出さない。前回算出された粒子群ｐｓの各粒子ｐの尤度の総和の場合も同様である。
【００７５】
これにより、生成部９０３は、移動体の状態の推定の精度が高い場合は、心配粒子ａを生成せずに、処理を低減することができる。また、生成部９０３は、移動体の状態の推定の精度が低い場合には、心配粒子ａを生成することで、移動体の真の位置が危険領域Ｄの近傍領域にあるときは移動体の位置を危険領域Ｄの近傍領域にあると特定部９０７が特定できるようにすることができる。
【００７６】
なお、判断部９０５は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００７７】
設定部９０６は、判断部９０５によってしきい値以上であると判断された尤度が得られたときの移動体の空間内の位置に基づいて、移動体の移動範囲を設定する。具体的には、例えば、設定部９０６は、判断部９０５によってしきい値以上であると判断された尤度が得られたときの移動体の推定位置と、現在時刻までの移動体が移動した速度と、に基づいて、現在時刻までに移動体が移動可能な範囲を算出する。そして、設定部９０６は、算出した移動体が移動可能な範囲をＲＡＭ２０３などの記憶領域に記憶する。これにより、設定部９０６は、移動体が移動しうる範囲を設定することができる。
【００７８】
この場合、生成部９０３は、近傍領域のうち、設定部９０６によって設定された移動範囲と重複する領域に第２の粒子群を生成する。生成部９０３は、危険領域テーブル４００の中で、近傍領域項目の情報から特定される近傍領域の範囲と、設定部９０６によって設定された移動体の移動範囲と、が重複するレコードを特定する。そして、生成部９０３は、特定したレコードの心配粒子ＩＤリストを参照して、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出すことになる。
【００７９】
これにより、生成部９０３は、移動体の移動範囲と重複しない近傍領域には心配粒子ａを生成せずに、処理を低減することができる。また、生成部９０３は、移動体の移動範囲と重複する近傍領域には、心配粒子ａを生成することで、特定部９０７による移動体の位置の特定において、移動体の位置を危険領域Ｄの近傍領域にあると特定できるようにすることができる。
【００８０】
なお、設定部９０６は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８１】
特定部９０７は、算出部９０４によって算出された各粒子の尤度に基づいて、空間内での移動体の位置を特定する。具体的には、例えば、特定部９０７は、算出部９０４によって算出された各粒子の尤度に比例した確率に従って、各粒子の中から粒子群ｐｓの粒子ｐを選択し直す。
【００８２】
より具体的には、例えば、特定部９０７は、［０，１］区間上で、粒子ごとに、当該粒子の尤度に比例する区間長になるように分割された区間を対応付ける。そして、特定部９０７は、［０，１］の一様乱数を発生させて、乱数の値が入る区間に対応する粒子を、粒子群ｐｓの粒子ｐとして選択し直す。ここで、粒子群ｐｓの粒子ｐとして、１粒子を複数回選択してもよい。そして、特定部９０７は、リサンプリングされた粒子群ｐｓが示す移動体の状態から、現在の移動体の位置を特定する。
【００８３】
これにより、特定部９０７は、移動体の位置を特定することができる。なお、特定部９０７は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８４】
判定部９０８は、特定部９０７によって特定された移動体の位置が特定の領域にあるか否かを判定する。具体的には、例えば、判定部９０８は、特定部９０７によって特定された移動体の位置が、危険領域テーブル４００の近傍領域項目の内容から特定される近傍領域の範囲に含まれているか否かを判定し、判定結果をＲＡＭ２０３などの記憶領域に記憶する。
【００８５】
これにより、判定部９０８は、移動体が危険領域Ｄの近傍領域にあるか否かを判定することができる。なお、判定部９０８は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８６】
制御部９０９は、判定部９０８によって特定の領域に位置すると判定された場合、特定の領域から離れる方向へ移動体を移動するように制御する。具体的には、例えば、制御部９０９は、判定部９０８によって危険領域Ｄの近傍領域に移動体が位置すると判定された場合、危険領域テーブル４００を参照して危険領域Ｄから離れる方向へ移動体を移動するように、駆動装置２０６を制御する。
【００８７】
より具体的には、例えば、制御部９０９は、危険領域テーブル４００の近傍領域項目の内容から特定される近傍領域の範囲と、粒子群ｐｓが示す移動体の位置と進行方向とから、移動体の進行方向に危険領域Ｄがあるか否かを判定する。そして、制御部９０９は、移動体の進行方向に危険領域Ｄがあれば、移動体の進行方向が逆方向になるように、駆動装置２０６を制御する。
【００８８】
これにより、制御部９０９は、移動体の危険領域Ｄへの進入を低減することができる。なお、判断部９０５は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８９】
（実施例１）
次に、実施例１について説明する。実施例１は、移動体が推定装置２００自体である場合での移動体の位置推定の例である。具体的には、推定装置２００（移動体）として、自律移動ロボットＲ（以下では、「ロボットＲ」という）を採用した場合について説明する。
【００９０】
（実施例１にかかる推定装置２００の動作例１）
まず、図１０〜図１２を用いて、実施例１にかかる推定装置２００の動作例１について説明する。なお、ロボットＲは、物体テーブル３００および危険領域テーブル４００を有する。また、ロボットＲは、ロボットＲの移動する領域内でのロボットＲの或る状態を示す粒子ｐの集合である粒子群ｐｓを用いたシミュレーションにより、ロボットＲの状態を推定する。そして、ロボットＲは、物体テーブル３００および危険領域テーブル４００と、推定された状態と、に基づいて、危険領域Ｄおよび障害物Ｏが存在する領域内での自己位置を推定し、危険領域Ｄを回避しつつ移動を行う。
【００９１】
図１０〜図１２は、実施例１にかかる推定装置２００の動作例１を示す説明図である。図１０の（Ａ）において、ロボットＲは、時刻「ｔ−１」での移動体の状態ｘt-1を示す粒子群ｐｓから、時刻「ｔ−１」での自己位置を位置Ｓ３であると推定している。なお、移動体の状態ｘtは、下記式（１）で表される。ここで、ｘは移動体の位置であり、ｖ（ｘ）は移動体の速度である。
【００９２】
【数１】
【００９３】
図１０の（Ｂ）において、ロボットＲは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」での移動体の状態ｘt-1を示す粒子群ｐｓから、現在時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓを推定する。
【００９４】
（１）例えば、ロボットＲは、粒子群ｐｓの粒子ｐ１が示す状態ｘt-1(1)を、下記式（２）の状態方程式に入力して現在の粒子の状態ｘt(1)を算出する。ここで、ｉは便宜上付与された粒子の番号である。ηt(i)はシステムノイズである。ΔＴは、タイムステップであり、「（ｔ）−（ｔ−１）」である。ａt(i)は移動体の加速度である。
【００９５】
【数２】
【００９６】
（２）次に、ロボットＲは、粒子群ｐｓの他の粒子ｐについても、同様に現在の粒子ｐの状態ｘtを算出していく。これにより、ロボットＲは、時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓを推定することができる。
【００９７】
次に、図１１において、ロボットＲは、図１０で推定された現在時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓと、危険領域Ｄの近傍領域に生成した危険領域Ｄの近傍領域での移動体の状態ｘtを示す心配粒子群ａｓと、の各粒子の尤度を算出する。図１１の例では、１個の粒子ｐ１と１個の心配粒子ａ１との各粒子の尤度の算出を例に挙げる。
【００９８】
（１）まず、推定装置２００は、物体テーブル３００を参照して、粒子ｐ１と心配粒子ａ１との各粒子が示す位置からの空間内の物体までの距離を算出する。例えば、推定装置２００は、空間内の物体までの距離として、粒子が示す位置からの前方の壁までの距離と右方の壁までの距離と左方の壁までの距離と後方の壁までの距離を算出する。
【００９９】
より具体的には、例えば、推定装置２００は、粒子ａ１が示す位置からの前方の壁までの距離ｄａ＿ｆと右方の壁までの距離ｄａ＿ｒと左方の壁までの距離ｄａ＿ｌと後方の壁までの距離ｄａ＿ｂを算出する。また、推定装置２００は、粒子ｐ１が示す位置からの前方の壁までの距離ｄｐ＿ｆと右方の壁までの距離ｄｐ＿ｒと左方の壁までの距離ｄｐ＿ｌと後方の壁までの距離ｄｐ＿ｂを算出する。
【０１００】
（２）次に、推定装置２００は、センサ２０４により、実際の自己位置からの物体までの距離を測定する。例えば、推定装置２００は、４つのセンサ２０４により、推定装置２００の前方の壁までの距離ｄｒ＿ｆと右方の壁までの距離ｄｒ＿ｒと左方の壁までの距離ｄｒ＿ｌと後方との壁までの距離ｄｒ＿ｂを算出する。なお、ここでは、推定装置２００は、４つのセンサ２０４により各壁までの距離を測定したが、これに限らない。例えば、推定装置２００は、１つのセンサ２０４の測定方向を変化させて、各壁までの距離を測定してもよい。
【０１０１】
（３）そして、推定装置２００は、粒子ｐ１が示す位置からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ（尤度）を算出する。例えば、推定装置２００は、測定された各壁までの距離を期待値とした正規分布において、粒子ｐ１が示す位置からの各壁までの距離になる確率の総乗ｐ（ｙt｜ｘt|t-1(i)）を、下記式（３）により算出する。ここで、σjは標準偏差である。ｙjtはセンサ２０４により測定された壁までの距離である。ｙjt(i)は算出された壁までの距離である。ｉは粒子ｐの番号である。ｊはセンサ２０４の番号である。
【０１０２】
【数３】
【０１０３】
そして、推定装置２００は、算出した確率の総乗を、粒子ｐ１の尤度とする。なお、図１１では、粒子ｐ１が示す位置からの各壁までの距離が真の自己位置から各壁までの距離と差が大きいため、粒子ｐ１の尤度は低くなる。
【０１０４】
（４）推定装置２００は、上記（３）と同様に心配粒子ａ１からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ（尤度）を算出する。例えば、推定装置２００は、測定された各壁までの距離を期待値とした正規分布において、心配粒子ａ１からの各壁までの距離になる確率の総乗を、式（３）により算出する。そして、推定装置２００は、算出した確率の総乗を、心配粒子ａ１の尤度とする。なお、図１１では、心配粒子ａ１からの各壁までの距離と、真の自己位置からの各壁までの距離との差が小さいため、心配粒子ａ１の尤度は高くなる。
【０１０５】
そして、図１２において、ロボットＲは、図１１のようにして算出した粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、現在の自己位置を示す粒子群ｐｓをリサンプリングして、現在の自己位置を特定する。
【０１０６】
具体的には、リサンプリングでは、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に比例した確率に従って、各粒子の中から粒子群ｐｓの粒子ｐを選択し直す。このとき、同一の粒子を複数回選択してもよい。より具体的には、例えば、ロボットＲは、各粒子の尤度に比例するように粒子ごとに分割した［０，１］区間上で、［０，１］の一様乱数を発生させて、乱数の値が入る区間に対応する粒子（粒子ｐまたは心配粒子ａ）を、粒子群ｐｓの粒子ｐとして選択する。
【０１０７】
図１２に示すように、ロボットＲの真の自己位置が、図１０で推定された粒子群ｐｓが示す位置Ｓ４の近傍領域にあれば、粒子群ｐｓの各粒子ｐの尤度が高くなり、心配粒子群ａｓの各粒子ａの尤度が低くなる。そのため、ロボットＲは、リサンプリングにより、図１０で推定された粒子群ｐｓの各粒子ｐを心配粒子群ａｓの各粒子ａより優先して、粒子群ｐｓの粒子ｐとして選択し直し、粒子群ｐｓを推定し直す。そして、ロボットＲは、推定し直された粒子群ｐｓが示す位置Ｓ４を自己位置であると推定することができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域ではないと判定して、通常動作を実行することができる。
【０１０８】
一方、ロボットＲの真の自己位置が、図１０で推定された粒子群ｐｓが示す位置Ｓ４の近傍領域ではなく、危険領域Ｄの近傍領域にあれば、粒子群ｐｓの各粒子ｐの尤度が低くなり、心配粒子群ａｓの各粒子ａの尤度が高くなる。そのため、ロボットＲは、リサンプリングにより、心配粒子群ａｓの各粒子ａを粒子群ｐｓの各粒子ｐより優先して、粒子群ｐｓの粒子ｐとして選択し直し、粒子群ｐｓを推定し直す。そして、ロボットＲは、推定し直された粒子群ｐｓが示す位置Ｓ５を自己位置であると推定することができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると判定して、危険領域Ｄから離れる方向へ移動することができる。
【０１０９】
このように、ロボットＲは、心配粒子群ａｓを用いてリサンプリングを行うことにより、心配粒子群ａｓの各粒子ａの尤度が高い場合は、推定し直す粒子群ｐｓの粒子ｐとして心配粒子ａを選択する。ここで、ロボットＲは、真の自己位置が危険領域Ｄの近傍領域にある場合、尤度の高い心配粒子ａが粒子群ｐｓの粒子ｐとして選択されることにより、自己位置の推定精度の向上を図ることができる。
【０１１０】
一方、ロボットＲは、心配粒子群ａｓを用いてリサンプリングを行っても、真の自己位置が危険領域Ｄの近傍領域でなければ、尤度の低い心配粒子ａは粒子群ｐｓの粒子ｐとして選択されないため、自己位置を危険領域Ｄの近傍領域にあると誤って推定することはない。また、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１１１】
（実施例１にかかる推定装置２００の動作例１における推定装置２００の動作制御処理）
次に、図１３を用いて、実施例１にかかる推定装置２００の動作例１における推定装置２００の動作制御処理の処理内容について説明する。
【０１１２】
図１３は、実施例１の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。まず、ＣＰＵ２０１は、初期化処理を行う（ステップＳ１３０１）。なお、初期化処理では、推定装置２００は、物体テーブル３００から空間内の物体の情報を読み出し、危険領域テーブル４００から空間内の危険領域Ｄの情報を読み出し、空間内に均等に粒子ｐを生成する。
【０１１３】
次に、推定装置２００は、粒子フィルタを用いたシミュレーションにより、移動体の直前の状態を示す粒子群ｐｓから、移動体の現在の状態を示す粒子群ｐｓを推定する（ステップＳ１３０２）。なお、ステップＳ１３０２の処理は、図１０に示した推定装置２００の処理に対応する。
【０１１４】
そして、推定装置２００は、センサ２０４により、自装置から障害物Ｏまでの距離を測定する（ステップＳ１３０３）。次に、ＣＰＵ２０１は、心配粒子生成処理を実行する（ステップＳ１３０４）。
【０１１５】
心配粒子生成処理において、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する。
【０１１６】
そして、推定装置２００は、ステップＳ１３０２で推定された粒子群ｐｓとステップＳ１３０４で生成された心配粒子群ａｓとの各粒子の尤度を算出する（ステップＳ１３０５）。なお、ステップＳ１３０３〜ステップＳ１３０５の処理は、図１１に示した推定装置２００の処理に対応する。
【０１１７】
次に、推定装置２００は、ステップＳ１３０５で算出した各粒子の尤度から、ステップＳ１３０２で生成された粒子群ｐｓのリサンプリングを行い、移動体の現在の状態を特定する（ステップＳ１３０６）。そして、推定装置２００は、特定した移動体の現在の状態に応じて動作を決定する（ステップＳ１３０７）。なお、ステップＳ１３０６およびステップＳ１３０７の処理は、図１２に示した推定装置２００の処理に対応する。
【０１１８】
次に、推定装置２００は、動作終了か否かを判定する（ステップＳ１３０８）。なお、例えば、動作終了とは、ロボットＲが目的地に到着したこと、現在時刻が所定の時刻であることが挙げられる。ここで、動作終了していない場合（ステップＳ１３０８：Ｎｏ）、ＣＰＵ２０１は、ステップＳ１３０２に戻る。
【０１１９】
一方、動作終了している場合（ステップＳ１３０８：Ｙｅｓ）、推定装置２００は、動作制御処理を終了する。これにより、推定装置２００（移動体）は、自己位置を推定し、危険領域Ｄを回避しつつ移動を行うことができる。
【０１２０】
（実施例１にかかる推定装置２００の動作例２）
次に、図１４を用いて、実施例１にかかる推定装置２００の動作例２について説明する。
【０１２１】
図１４は、実施例１にかかる推定装置２００の動作例２を示す説明図である。動作例１ではロボットＲの推定した自己位置の精度に関わらず心配粒子ａを生成していたが、動作例２では、ロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しない。また、動作例２では、ロボットＲにより推定された自己位置の精度が低いときには心配粒子ａを生成する。
【０１２２】
これにより、ロボットＲは、現在の自己位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。一方、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１２３】
図１４の（Ａ）に示すように、ロボットＲにより時刻「ｔ−１」で推定された粒子群ｐｓの尤度の総和がしきい値以上であるため、時刻「ｔ−１」でロボットＲにより推定された自己位置は精度が高いものとする。この場合、ロボットＲは、危険領域Ｄの近傍領域に心配粒子ａを生成せずとも、次の時刻「ｔ」の自己位置を推定できる。
【０１２４】
図１４の（Ｂ）に示すように、（１）ロボットＲは、次の時刻「ｔ」になると、（Ａ）に示した時刻「ｔ−１」で推定された粒子群ｐｓから、時刻「ｔ」でのロボットＲの自己位置を示す粒子群ｐｓを推定する。（２）ここで、ロボットＲは、心配粒子群ａｓを生成しない。
【０１２５】
（３）次に、ロボットＲは、センサ２０４により、真の自己位置から障害物Ｏまでの距離を測定する。（４）そして、ロボットＲは、障害物Ｏまでの測定距離に基づいて、粒子群ｐｓのリサンプリングを行う。
【０１２６】
図１４の（Ｃ）に示すように、（Ｂ）でリサンプリングされた粒子群ｐｓの示す自己位置は、位置Ｓ６になっている。このように、尤度の総和がしきい値以上であった粒子群ｐｓに基づいて推定された現在の自己位置を示す粒子群ｐｓは精度が高いため、ロボットＲは、正しく自己位置を推定することができている。（５）そして、ロボットＲは、危険領域テーブル４００を参照して、自己位置である位置Ｓ６が危険領域Ｄの近傍領域ではないと判定して、通常動作を行う。
【０１２７】
このように、ロボットＲは、ロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しないことで、処理量を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１２８】
（実施例１の動作例２におけるステップＳ１３０４の心配粒子生成処理）
次に、図１５を用いて、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）について説明する。なお、動作例２における推定装置２００の動作制御処理の処理内容は、心配粒子生成処理（ステップＳ１３０４）を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０１２９】
図１５は、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、現在の自己位置の精度が高いか否かを判定する（ステップＳ１５０１）。具体的には、例えば、推定装置２００は、前回の自己位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ１５０１：Ｙｅｓ）、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１３０】
一方、精度が低い場合（ステップＳ１５０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１５０２）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１３１】
これにより、推定装置２００は、現在の自己位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１３２】
（実施例１にかかる推定装置２００の動作例３）
次に、図１６を用いて、実施例１にかかる推定装置２００の動作例３について説明する。
【０１３３】
図１６は、実施例１にかかる推定装置２００の動作例３を示す説明図である。動作例１ではロボットＲが危険領域Ｄに進入する可能性があるか否かに関わらず心配粒子ａを生成していたが、動作例３ではロボットＲが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１３４】
これにより、ロボットＲは、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、ロボットＲは、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１３５】
図１６に示すように、ロボットＲは、推定された粒子群ｐｓの尤度の総和がしきい値以上であり、推定された自己位置の精度が高かった時刻「ｔ−６」がある場合、時刻「ｔ−６」で推定された自己位置から現在時刻「ｔ」までに移動可能な範囲を算出する。
【０１３６】
具体的には、ロボットＲは、時刻「ｔ−６」から現在時刻「ｔ」までの各時刻でのロボットＲの速度から、ロボットＲの時刻「ｔ−６」から現在時刻「ｔ」まででの最大移動距離を算出する。そして、ロボットＲは、時刻「ｔ−６」で推定された自己位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
【０１３７】
この場合、ロボットＲは、算出された移動可能な範囲に重複する危険領域Ｄには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Ｄには進入する可能性がない。そのため、ロボットＲは、移動可能な範囲に重複する危険領域Ｄの近傍領域には心配粒子ａを生成し、移動可能な範囲に重複しない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１３８】
（１）例えば、ロボットＲは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０１３９】
（２）また、ロボットＲは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０１４０】
（３）一方、ロボットＲは、算出した移動可能な範囲と重複していない危険領域Ｄの近傍領域には、心配粒子ａを生成しない。
【０１４１】
これにより、ロボットＲは、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、ロボットＲは、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１４２】
（実施例１の動作例３における推定装置２００の動作制御処理）
次に、実施例１の動作例３における推定装置２００の動作制御処理の処理内容について説明する。
【０１４３】
動作例３における推定装置２００の動作制御処理の処理内容は、ステップＳ１３０４の心配粒子生成処理を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０１４４】
（実施例１の動作例３におけるステップＳ１３０４の心配粒子生成処理）
次に、図１７を用いて、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）について説明する。
【０１４５】
図１７は、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ１７０１）。
【０１４６】
ここで、粒子ｐがなかった場合（ステップＳ１７０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１７０２）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１４７】
一方、粒子ｐがあった場合（ステップＳ１７０１：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ１７０３）。次に、推定装置２００は、特定した時刻から現在時刻までのロボットＲの状態から、現在のロボットＲの移動可能範囲を算出する（ステップＳ１７０４）。
【０１４８】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１７０５）。次に、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１４９】
これにより、推定装置２００は、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１５０】
（実施例１にかかる推定装置２００の動作例４）
次に、実施例１にかかる推定装置２００の動作例４について説明する。動作例４は、動作例２および動作例３を組み合わせた場合の動作例である。動作例４ではロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しない。また、動作例４ではロボットＲが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１５１】
これにより、推定装置２００は、推定された自己位置の精度が高いときには心配粒子ａを生成しないため、処理を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
【０１５２】
また、推定装置２００は、自装置が進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しないため、処理を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１５３】
（実施例１にかかる推定装置２００の動作例４におけるステップＳ１３０４の心配粒子生成処理）
動作例４における推定装置２００の動作制御処理の処理内容は、心配粒子生成処理（ステップＳ１３０４）を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。したがって、図１８を用いて、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）について説明する。
【０１５４】
図１８は、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、現在の自己位置の精度が高いか否かを判定する（ステップＳ１８０１）。具体的には、例えば、推定装置２００は、前回の自己位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ１８０１：Ｙｅｓ）、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５５】
一方、精度が低い場合（ステップＳ１８０１：Ｎｏ）、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ１８０２）。ここで、粒子ｐがなかった場合（ステップＳ１８０２：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１８０３）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５６】
一方、粒子ｐがあった場合（ステップＳ１８０２：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ１８０４）。次に、推定装置２００は、特定した時刻から現在時刻までのロボットＲの状態から、現在のロボットＲの移動可能範囲を算出する（ステップＳ１８０５）。
【０１５７】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１８０６）。次に、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５８】
これにより、推定装置２００は、推定された自己位置の精度が高いときには心配粒子ａを生成しないため、処理を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
【０１５９】
また、推定装置２００は、自装置が進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しないため、処理を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１６０】
以上説明したように、実施例１にかかる推定装置２００によれば、推定された移動体（推定装置２００自体）の状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子群ａｓが生成される。次に、推定装置２００によれば、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置２００によれば、リサンプリングにより選択し直された粒子群ｐｓから、移動体の状態が推定し直される。
【０１６１】
このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、真の自己位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６２】
また、推定装置２００によれば、推定された移動体の状態の精度が高いときには心配粒子ａを生成しないことにより、処理量を低減することができる。また、推定装置２００によれば、現在の自己位置の精度が低い場合には心配粒子ａが生成される。このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、真の自己位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６３】
また、推定装置２００によれば、移動体が移動可能な範囲と重複しない危険領域Ｄの近傍領域は、移動体が進入する可能性がない危険領域Ｄの近傍領域であるため、心配粒子ａを生成しないことで、処理量を低減することができる。また、推定装置２００によれば、移動体が移動可能な範囲と重複する危険領域Ｄの近傍領域は、移動体が進入する可能性があるため、心配粒子ａを生成が生成される。このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に真の自己位置がある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６４】
また、推定装置２００によれば、推定された移動体の位置が危険領域Ｄの近傍領域にある場合、移動体が危険領域Ｄから離れるようにするため、移動体が危険領域Ｄに進入することを回避することができる。
【０１６５】
（実施例２）
次に、実施例２について説明する。実施例１は、移動体が推定装置２００自体である場合での移動体の位置推定の例であったが、実施例２は、移動体が推定装置２００とは異なる他の装置である場合での移動体の位置推定の例である。例えば、実施例２では推定装置２００として車両を採用し、移動体として推定装置２００の前方の車両を採用した場合について説明する。このとき、車両（推定装置２００）は、前方の車両（移動体）の位置を推定し、自車の周囲（危険領域Ｄ）に前方の車両が接近することを回避する。
【０１６６】
まず、図１９を用いて、実施例２にかかる推定装置２００による前方車両の位置推定の内容について説明する。
【０１６７】
図１９は、実施例２にかかる推定装置２００による前方車両の位置推定の内容を示す説明図である。図１９の例では、車両Ｃは、危険領域Ｄの位置情報を有し、前方車両Ｆの位置を推定して、前方車両Ｆが危険領域Ｄの近傍領域に進入しないように移動を行う。
【０１６８】
図１９の（Ａ）において、（１）車両Ｃは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」での前方車両Ｆの状態を示す粒子群ｐｓから現在時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓを推定する。前方車両Ｆの状態Ａtは下記式（４）で表される。
【０１６９】
ここで、Ｗtは、前方車両Ｆの横幅である。ｘtは、前方車両Ｆの横方向の中心座標である。Ｚtは、車両Ｃから前方車両Ｆまでの距離である。ｖｘtは、車両Ｃに対する前方車両Ｆの横方向の相対速度である。ｖｚtは、車両Ｃに対する前方車両Ｆの距離方向の相対速度である。
【０１７０】
【数４】
【０１７１】
具体的には、車両Ｃは、粒子群ｐｓの各粒子ｐが示す前方車両の状態Ａt-1を、下記式（５）および式（６）の状態方程式に入力して現在の各粒子ｐの状態Ａtを算出する。ΔＴは前方車両Ｆの状態の推定を実行する間隔を示すタイムステップであり、「（ｔ）−（ｔ−１）」である。
【０１７２】
【数５】
【０１７３】
【数６】
【０１７４】
これにより、車両Ｃは、時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓを推定することができる。ここでは、車両Ｃは、推定された現在時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓにより、現在時刻「ｔ」での前方車両Ｆの位置を位置Ｓ７であると推定する。
【０１７５】
しかしながら、真の前方車両Ｆの位置は危険領域Ｄの近傍である位置Ｓ８であり、車両Ｃは前方車両Ｆの位置を誤って推定している。
【０１７６】
図１９の（Ｂ）において、（２）車両Ｃは、推定された現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓに加えて、危険領域Ｄの位置情報に基づいて危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子群ａｓを生成する。
【０１７７】
（３）次に、車両Ｃは、センサ２０４により、前方車両Ｆの位置までの距離ｄｒを測定する。また、車両Ｃは、センサ２０４により、前方車両Ｆの左端の座標値と右端の座標値とを測定する。
【０１７８】
（４）そして、車両Ｃは、下記式（７）および式（８）により、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を算出する。ここで、前方車両Ｆまでの距離とは、例えば、粒子ｐ１が示す前方車両Ｆの位置までの距離ｄｐや粒子ａ１が示す前方車両Ｆの位置までの距離ｄａである。
【０１７９】
【数７】
【０１８０】
【数８】
【０１８１】
車両Ｃは、算出された各粒子が示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と比較したときの尤度を算出する。そして、車両Ｃは、算出した尤度に基づいて、現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓのリサンプリングを行う。
【０１８２】
例えば、車両Ｃは、測定された前方車両Ｆの状態を示す値を期待値とした多変量正規分布において、粒子ｐが示す前方車両Ｆの状態を示す値になる確率ｐ（ｙt｜Ａt|t-1(i)）を、下記式（９）により算出する。ここで、ｙtはセンサ２０４により測定された前方車両Ｆの状態を示す値であり、例えば、前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値である。ｙt(i)は算出された前方車両Ｆの状態を示す値であり、前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値である。ρは次元数であり、ここでは３である。ｉは粒子の番号である。Σは分散共分散行列である。
【０１８３】
【数９】
【０１８４】
そして、推定装置２００は、算出した確率を、粒子ｐの尤度とする。なお、図１９では、粒子ｐが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値が真の前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と差が大きいため、粒子ｐの尤度は低くなる。
【０１８５】
車両Ｃは、同様に心配粒子ａが示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と比較したときの尤度を算出する。
【０１８６】
例えば、推定装置２００は、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を期待値とした多変量正規分布において、心配粒子ａが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値になる確率を、式（９）により算出する。
【０１８７】
そして、推定装置２００は、算出した確率を、心配粒子ａの尤度とする。なお、図１９では、心配粒子ａが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値が、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と差が小さいため、心配粒子ａの尤度は高くなる。
【０１８８】
図１９の（Ｃ）において、車両Ｃは、粒子群ｐｓのリサンプリングにおいて、粒子群ｐｓの各粒子ｐより尤度の高い心配粒子群ａｓの各粒子ａを優先して、粒子群ｐｓの粒子ｐとして選択し直す。
【０１８９】
（５）車両Ｃは、リサンプリング結果に基づいて、現在時刻「ｔ」での前方車両Ｆの位置を位置Ｓ８であると推定する。そして、車両Ｃは、危険領域テーブル４００の近傍領域項目の内容から特定される危険領域Ｄの近傍領域の範囲に、前方車両Ｆの位置である位置Ｓ８が含まれていると判定して、前方車両Ｆが危険領域Ｄに進入することを回避するために、車両Ｃの速度を遅くする。
【０１９０】
このように、車両Ｃは、粒子フィルタを用いたシミュレーションによって推定された現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓが危険領域Ｄの近傍領域に存在しない場合であっても、前方車両Ｆの位置が危険領域Ｄの近傍領域にあることを推定できる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０１９１】
図１９では、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、車両Ｃの速度を遅くしたが、これに限らない。例えば、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、出力装置２０５により車両Ｃの運転者に警告を通知するようにしてもよい。
【０１９２】
また、図１９では、車両Ｃは、前方車両Ｆの位置を推定したが、これに限らない。例えば、車両Ｃは、後方車両の位置を推定してもよい。また、図１９では、車両Ｃは、１台の前方車両Ｆの位置を推定したが、これに限らない。例えば、車両Ｃは、複数の車線がある場合、それぞれの車線を走行中の前方車両Ｆの位置を推定してもよい。
【０１９３】
また、図１９では、危険領域Ｄは固定された領域であったが、これに限らない。例えば、危険領域Ｄは車両Ｃに対する前方車両Ｆの相対速度によって変化する可変の領域であってもよい。
【０１９４】
図２０は、可変の危険領域Ｄの例を示す説明図である。例えば、車両Ｃに対する前方車両Ｆの相対速度が正であるときは、車両Ｃと前方車両Ｆの車間距離は時間の経過により大きくなるので、今現在の車間距離がある程度小さくても衝突の可能性は低い。一方で、車両Ｃに対する前方車両Ｆの相対速度が負であり、相対速度の絶対値が大きいほど、時間の経過により車間距離はより短くなるので、衝突を回避するにはより大きな車間距離が必要となる。そのため、図２０に示すように、車両Ｃに対する前方車両Ｆの相対速度が負で絶対値が大きくなるにつれて、より車両Ｃから遠い領域が危険領域Ｄとして設定されるようにしてもよい。
【０１９５】
（実施例２の動作例１における推定装置２００の動作制御処理）
次に、図２１を用いて、実施例２の動作例１における推定装置２００の動作制御処理の処理内容について説明する。
【０１９６】
図２１は、実施例２の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。まず、推定装置２００は、初期化処理を行う（ステップＳ２１０１）。なお、初期化処理では、推定装置２００は、物体テーブル３００から空間内の物体の情報を読み出し、危険領域テーブル４００から空間内の危険領域Ｄの情報を読み出し、空間内に均等に粒子ｐを生成する。
【０１９７】
次に、推定装置２００は、粒子フィルタを用いたシミュレーションにより、前方車両Ｆの直前の状態を示す粒子群ｐｓから、前方車両Ｆの現在の状態を示す粒子群ｐｓを推定する（ステップＳ２１０２）。なお、ステップＳ２１０２の処理は、図１９の（Ａ）に示した推定装置２００の処理に対応する。
【０１９８】
そして、推定装置２００は、センサ２０４により、自装置から前方車両Ｆまでの距離を測定する（ステップＳ２１０３）。次に、推定装置２００は、心配粒子生成処理を実行する（ステップＳ２１０４）。
【０１９９】
心配粒子生成処理において、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する。
【０２００】
そして、推定装置２００は、ステップＳ２１０２で推定された粒子群ｐｓとステップＳ２１０４で生成された心配粒子群ａｓとの各粒子の尤度を算出する（ステップＳ２１０５）。なお、ステップＳ２１０３〜ステップＳ２１０５の処理は、図１９の（Ｂ）に示した推定装置２００の処理に対応する。
【０２０１】
次に、推定装置２００は、ステップＳ２１０５で算出した各粒子の尤度から、ステップＳ２１０２で生成された粒子群ｐｓのリサンプリングを行い、前方車両Ｆの現在の状態を特定する（ステップＳ２１０６）。そして、推定装置２００は、特定した前方車両Ｆの現在の状態に応じて動作を決定する（ステップＳ２１０７）。なお、ステップＳ２１０６およびステップＳ２１０７の処理は、図１９の（Ｃ）に示した推定装置２００の処理に対応する。
【０２０２】
次に、推定装置２００は、動作終了か否かを判定する（ステップＳ２１０８）。ここで、動作終了していない場合（ステップＳ２１０８：Ｎｏ）、推定装置２００は、ステップＳ２１０２に戻る。
【０２０３】
一方、動作終了している場合（ステップＳ２１０８：Ｙｅｓ）、推定装置２００は、動作制御処理を終了する。これにより、推定装置２００は、前方車両Ｆの状態を推定し、前方車両Ｆが危険領域Ｄに進入することを回避しつつ移動を行うことができる。
【０２０４】
（実施例２にかかる推定装置２００の動作例２）
次に、図２２を用いて、実施例２にかかる推定装置２００の動作例２について説明する。
【０２０５】
図２２は、実施例２にかかる推定装置２００の動作例２を示す説明図である。動作例１では車両Ｃの推定した前方車両Ｆの位置の精度に関わらず心配粒子ａを生成していたが、動作例２では、車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しない。また、動作例２では、車両Ｃにより推定された前方車両Ｆの位置の精度が低いときには心配粒子ａを生成する。
【０２０６】
これにより、車両Ｃは、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。一方、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２０７】
図２２の（Ａ）に示すように、車両Ｃにより時刻「ｔ−１」で推定された粒子群ｐｓの尤度の総和がしきい値以上であるため、時刻「ｔ−１」で車両Ｃにより推定された前方車両Ｆの位置は精度が高いものとする。この場合、車両Ｃは、危険領域Ｄの近傍領域に心配粒子ａを生成せずとも、次の時刻「ｔ」の前方車両Ｆの位置を推定できる。
【０２０８】
図２２の（Ｂ）に示すように、（１）車両Ｃは、次の時刻「ｔ」になると、（Ａ）に示した時刻「ｔ−１」で推定された粒子群ｐｓから、時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓを推定する。（２）ここで、車両Ｃは、心配粒子ａを生成しない。
【０２０９】
（３）次に、車両Ｃは、センサ２０４により、真の前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を測定する。（４）そして、車両Ｃは、測定した前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値に基づいて、粒子群ｐｓのリサンプリングを行う。
【０２１０】
図２２の（Ｃ）に示すように、（Ｂ）でリサンプリングされた粒子群ｐｓの示す前方車両Ｆの位置は、真の前方車両Ｆの位置と同じ位置になっている。このように、尤度の総和がしきい値以上であった粒子群ｐｓに基づいて推定された現在の前方車両Ｆの位置を示す粒子群ｐｓは精度が高いため、車両Ｃは、正しく前方車両Ｆの位置を推定することができている。（５）そして、車両Ｃは、危険領域テーブル４００を参照して、前方車両Ｆの位置が危険領域Ｄの近傍領域ではないと判定して、通常動作を行う。
【０２１１】
このように、車両Ｃは、車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しないことで、処理量を低減することができる。また、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２１２】
（実施例２の動作例２における推定装置２００の動作制御処理）
次に、実施例２の動作例２における推定装置２００の動作制御処理の処理内容について説明する。
【０２１３】
動作例２における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２１４】
（実施例２の動作例２におけるステップＳ２１０４の心配粒子生成処理）
次に、図２３を用いて、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２１５】
図２３は、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、現在の前方車両Ｆの位置の精度が高いか否かを判定する（ステップＳ２３０１）。具体的には、推定装置２００は、前回の前方車両Ｆの位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の前方車両Ｆの位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ２３０１：Ｙｅｓ）、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２１６】
一方、精度が低い場合（ステップＳ２３０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２３０２）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２１７】
これにより、推定装置２００は、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２１８】
（実施例２にかかる推定装置２００の動作例３）
次に、図２４を用いて、実施例２にかかる推定装置２００の動作例３について説明する。
【０２１９】
図２４は、実施例２にかかる推定装置２００の動作例３を示す説明図である。動作例１では前方車両Ｆが危険領域Ｄに進入する可能性があるか否かに関わらず心配粒子ａを生成していたが、動作例３では前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０２２０】
これにより、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周囲）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２２１】
図２４に示すように、車両Ｃは、推定された粒子群ｐｓの尤度の総和がしきい値以上であり、推定された前方車両Ｆの位置の精度が高かった時刻「ｔ−６」がある場合、時刻「ｔ−６」で推定された前方車両Ｆの位置から現在時刻「ｔ」までに移動可能な範囲を算出する。
【０２２２】
具体的には、車両Ｃは、時刻「ｔ−６」から現在時刻「ｔ」までの各時刻での車両Ｃに対する前方車両Ｆの相対速度から、前方車両Ｆの時刻「ｔ−６」から現在時刻「ｔ」まででの最大移動距離を算出する。そして、車両Ｃは、時刻「ｔ−６」で推定された前方車両Ｆの位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
【０２２３】
この場合、前方車両Ｆは、算出された移動可能な範囲に重複する危険領域Ｄには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Ｄには進入する可能性がない。
【０２２４】
（１）車両Ｃは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０２２５】
（２）また、車両Ｃは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０２２６】
（３）一方、車両Ｃは、算出した移動可能な範囲と重複していない危険領域Ｄの近傍領域には、心配粒子ａを生成しない。
【０２２７】
これにより、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周囲）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２２８】
（実施例２の動作例３における推定装置２００の動作制御処理）
次に、実施例２の動作例３における推定装置２００の動作制御処理の処理内容について説明する。
【０２２９】
動作例３における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２３０】
（実施例２の動作例３におけるステップＳ２１０４の心配粒子生成処理）
次に、図２５を用いて、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２３１】
図２５は、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ２５０１）。
【０２３２】
ここで、粒子ｐがなかった場合（ステップＳ２５０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２５０２）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２３３】
一方、粒子ｐがあった場合（ステップＳ２５０１：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ２５０３）。次に、推定装置２００は、特定した時刻から現在時刻までの前方車両Ｆの状態から、現在の前方車両Ｆの移動可能範囲を算出する（ステップＳ２５０４）。
【０２３４】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２５０５）。次に、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２３５】
これにより、推定装置２００は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２３６】
（実施例２にかかる推定装置２００の動作例４）
次に、実施例２にかかる推定装置２００の動作例４について説明する。動作例４は、動作例２および動作例３を組み合わせた場合の動作である。動作例４では車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しない。また、動作例４では前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０２３７】
これにより、車両Ｃは、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２３８】
また、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２３９】
（実施例２の動作例４における推定装置２００の動作制御処理）
次に、実施例２の動作例４における推定装置２００の動作制御処理の処理内容について説明する。
【０２４０】
動作例４における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２４１】
（実施例２の動作例４におけるステップＳ２１０４の心配粒子生成処理）
次に、図２６を用いて、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２４２】
図２６は、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）の処理内容を示すフローチャートである。動作例４における心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、現在の前方車両Ｆの位置の精度が高いか否かを判定する（ステップＳ２６０１）。具体的には、推定装置２００は、前回の前方車両Ｆの位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の前方車両Ｆの位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ２６０１：Ｙｅｓ）、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４３】
一方、精度が低い場合（ステップＳ２６０１：Ｎｏ）、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ２６０２）。ここで、粒子ｐがなかった場合（ステップＳ２６０２：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２６０３）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４４】
一方、粒子ｐがあった場合（ステップＳ２６０２：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ２６０４）。次に、推定装置２００は、特定した時刻から現在時刻までの前方車両Ｆの状態から、現在の前方車両Ｆの移動可能範囲を算出する（ステップＳ２６０５）。
【０２４５】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２６０６）。次に、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４６】
これにより、推定装置２００は、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２４７】
また、推定装置２００は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２４８】
以上説明したように、実施例２にかかる推定装置２００によれば、前方車両Ｆの状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子群ａｓが生成される。次に、推定装置２００によれば、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置２００によれば、リサンプリングにより選択し直された粒子群ｐｓから、前方車両Ｆの状態が推定し直される。
【０２４９】
このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５０】
また、推定装置２００によれば、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００によれば、現在の前方車両Ｆの位置の精度が低い場合には、心配粒子ａが生成される。このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５１】
また、推定装置２００によれば、前方車両Ｆが移動可能な範囲と重複しない危険領域Ｄの近傍領域は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域であるため、心配粒子ａを生成しないことで、処理量を低減することができる。また、推定装置２００によれば、前方車両Ｆが移動可能な範囲と重複する危険領域Ｄの近傍領域は、前方車両Ｆが進入する可能性があるため、心配粒子ａが生成される。このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、リサンプリングにより選択し直された粒子群ｐｓから、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５２】
また、推定装置２００によれば、推定された前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆが危険領域Ｄから離れるようにするため、前方車両Ｆが危険領域Ｄに進入することを回避することができる。
【０２５３】
なお、本実施の形態で説明した推定方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。本推定プログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また本推定プログラムは、インターネット等のネットワークを介して配布してもよい。
【０２５４】
上述した実施の形態に関し、さらに以下の付記を開示する。
【０２５５】
（付記１）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新する更新手段と、
前記更新手段によって更新される都度、前記移動体と前記空間内の物体との距離を測定する測定手段と、
前記更新手段によって更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成する生成手段と、
前記測定手段によって測定された距離と、前記更新手段による更新されたときの第１の粒子群と、前記生成手段によって生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出する算出手段と、
前記算出手段によって算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する特定手段と、
を備えることを特徴とする推定装置。
【０２５６】
（付記２）前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段を備え、
前記生成手段は、
前記判断手段によって前記しきい値以上であると判断された場合には、前記第２の粒子群の生成を停止することを特徴とする付記１に記載の推定装置。
【０２５７】
（付記３）前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段と、
前記判断手段によって前記しきい値以上であると判断された尤度が得られたときの前記移動体の前記空間内の位置に基づいて、前記移動体の移動範囲を設定する設定手段と、を備え、
前記生成手段は、
前記近傍領域のうち、前記設定手段によって設定された移動範囲と重複する領域に前記第２の粒子群を生成することを特徴とする付記１または２に記載の推定装置。
【０２５８】
（付記４）前記特定手段によって特定された前記移動体の位置が前記特定の領域であるか否かを判定する判定手段と、
前記判定手段によって前記特定の領域に位置すると判定された場合、前記特定の領域から離れる方向へ前記移動体が移動するように制御する制御手段と、
を備えることを特徴とする付記１〜３のいずれか一つに記載の推定装置。
【０２５９】
（付記５）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータが実行することを特徴とする推定方法。
【０２６０】
（付記６）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータが実行することを特徴とする推定方法。
【０２６１】
（付記７）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータに実行させることを特徴とする推定プログラム。
【０２６２】
（付記８）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータに実行させることを特徴とする推定プログラム。
【符号の説明】
【０２６３】
２００ 推定装置
Ｒ ロボット
Ｃ 車両
９０１ 更新部
９０２ 測定部
９０３ 生成部
９０４ 算出部
９０５ 判断部
９０６ 設定部
９０７ 特定部
９０８ 判定部
９０９ 制御部
【技術分野】
【０００１】
本発明は、推定装置、推定方法、および推定プログラムに関する。
【背景技術】
【０００２】
従来、粒子フィルタ（パーティクルフィルタ：ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ）を用いた移動体の状態推定の技術がある。粒子フィルタとは、移動体の状態の確率分布を移動体の或る状態を示す粒子（パーティクル）の集合により近似する方法である。
【０００３】
粒子フィルタを用いた移動体の状態推定の技術では、シミュレーションにより、過去の移動体の状態を示す粒子群から、現在の移動体の状態を示す粒子群が推定される。そして、推定された現在の移動体の状態を示す粒子群の各粒子の尤もらしさ（尤度）に基づいて、粒子群のリサンプリングが行われる。粒子群のリサンプリングでは、シミュレーションにより推定された粒子群の中から、現在の移動体の状態を示す粒子群となる粒子が選択し直され、現在の移動体の状態が推定し直される。
【０００４】
リサンプリングの技術としては、移動体が有するレーザレーダの測定値に基づいて粒子フィルタを用いて推定された自己位置を補正する技術がある（例えば、下記特許文献１参照）。また、移動体が赤外線照射装置から受信した赤外線照射装置に固有のＩＤ信号により、粒子フィルタを用いて推定された自己位置を補正する技術がある（例えば、下記特許文献２参照）。
【０００５】
また、移動体の状態の推定に用いる粒子の数を動的に変化させる技術がある（例えば、下記特許文献３参照）。また、幾何マップからグリッドマップを生成し、生成したグリッドマップから物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成し、生成したボロノイグラフ上で物体との衝突確率と経路長とから経路探索を行う技術がある（例えば、下記特許文献４参照）。また、複数センサからの情報を統合して、自己位置を推定する技術がある（例えば、下記特許文献５参照）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−２２３５０４号公報
【特許文献２】特開２００９−１７６０３１号公報
【特許文献３】特開２０１０−２２４７５５号公報
【特許文献４】特開２００５−３２１９６号公報
【特許文献５】特開２００８−３３６９６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した従来技術では、粒子フィルタを用いたシミュレーションにより推定された現在の移動体の状態を示す粒子群に、移動体の真の状態を示す粒子が含まれない場合がある。例えば、シミュレーションによる計算誤差が、シミュレーションが行われるごとに蓄積されていった場合である。この場合、リサンプリングにおいて、推定された現在の移動体の状態を示す粒子群の中から現在の移動体の状態を示す尤もらしい粒子群を選択し直しても、選択し直した粒子群には移動体の真の状態を示す粒子が含まれない。結果として、上述した従来技術では、選択し直した粒子群が示す移動体の状態から移動体の位置を誤って推定してしまい、移動体が進入すべきではないと設定された領域に進入してしまう場合がある。
【０００８】
本発明は、上述した従来技術による問題点を解消するため、移動体が特定の領域に位置する場合の推定精度の向上を図ることができる推定装置、推定方法、および推定プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するため、本発明の一側面によれば、粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、移動体の状態を示す第１の粒子群を現状態から次状態に更新し、更新される都度、移動体と空間内の物体との距離を測定し、更新される都度、空間内の特定の領域の状態を示す第２の粒子群を生成し、測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、第１および第２の粒子群の各粒子の尤度を算出し、算出された各粒子の尤度に基づいて、空間内での移動体の位置を特定する推定装置、推定方法、および推定プログラムが提案される。
【発明の効果】
【００１０】
本発明の一側面によれば、移動体が特定の領域に位置する場合の推定精度の向上を図ることができるという効果を奏する。
【図面の簡単な説明】
【００１１】
【図１】図１は、実施の形態にかかる推定装置による移動体の位置推定の内容を示す説明図である。
【図２】図２は、実施の形態にかかる推定装置のハードウェア構成例を示すブロック図である。
【図３】図３は、推定装置２００が有する物体テーブルの記憶内容の一例を示す説明図である。
【図４】図４は、推定装置２００が有する危険領域テーブルの記憶内容の一例を示す説明図である。
【図５】図５は、推定装置２００が有する粒子テーブルの記憶内容の一例を示す説明図である。
【図６】図６は、推定装置２００が有する心配粒子テーブルの記憶内容の一例を示す説明図である。
【図７】図７は、推定装置２００が有するセンサ情報テーブルの記憶内容の一例を示す説明図である。
【図８】図８は、推定装置２００が有する尤度テーブルの記憶内容の一例を示す説明図である。
【図９】図９は、推定装置２００の機能的構成を示すブロック図である。
【図１０】図１０は、実施例１にかかる推定装置２００の動作例１を示す説明図（その１）である。
【図１１】図１１は、実施例１にかかる推定装置２００の動作例１を示す説明図（その２）である。
【図１２】図１２は、実施例１にかかる推定装置２００の動作例１を示す説明図（その３）である。
【図１３】図１３は、実施例１の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。
【図１４】図１４は、実施例１にかかる推定装置２００の動作例２を示す説明図である。
【図１５】図１５は、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１６】図１６は、実施例１にかかる推定装置２００の動作例３を示す説明図である。
【図１７】図１７は、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１８】図１８は、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。
【図１９】図１９は、実施例２にかかる推定装置２００による前方車両の位置推定の内容を示す説明図である。
【図２０】図２０は、可変の危険領域Ｄの例を示す説明図である。
【図２１】図２１は、実施例２の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。
【図２２】図２２は、実施例２にかかる推定装置２００の動作例２を示す説明図である。
【図２３】図２３は、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。
【図２４】図２４は、実施例２にかかる推定装置２００の動作例３を示す説明図である。
【図２５】図２５は、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。
【図２６】図２６は、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）の処理内容を示すフローチャートである。
【発明を実施するための形態】
【００１２】
以下に添付図面を参照して、この発明にかかる推定装置、推定方法、および推定プログラムの実施の形態を詳細に説明する。
【００１３】
（実施の形態にかかる推定装置による移動体の位置推定の内容）
まず、本実施の形態にかかる推定装置による移動体の位置推定の内容について説明する。
【００１４】
図１は、実施の形態にかかる推定装置による移動体の位置推定の内容を示す説明図である。図１の例は、移動体が推定装置自体である場合での移動体の位置推定の例である。具体的には、推定装置として、自律移動ロボットＲ（以下では、「ロボットＲ」という）を採用した場合について説明する。
【００１５】
ロボットＲは、ロボットＲの移動する領域内に存在する危険領域Ｄおよび障害物Ｏの位置情報を有する。また、ロボットＲは、ロボットＲの移動する領域内でのロボットＲの或る状態を示す粒子ｐの集合である粒子群ｐｓを用いたシミュレーションにより、ロボットＲの状態を推定する。そして、ロボットＲは、危険領域Ｄおよび障害物Ｏの位置情報と、推定された状態と、に基づいて、ロボットＲの移動する領域内で危険領域Ｄを回避しつつ移動を行う。ロボットＲの状態とは、例えば、ロボットＲの位置、速度、および進行方向である。
【００１６】
図１の（Ａ）において、（１）ロボットＲは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」でのロボットＲの状態を示す粒子群ｐｓから現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓを推定する。そして、ロボットＲは、推定された現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓにより、現在時刻「ｔ」での自己位置を位置Ｓ１であると推定する。
【００１７】
しかしながら、ロボットＲの真の自己位置は危険領域Ｄの近傍領域にある位置Ｓ２であり、ロボットＲは自己位置を誤って推定している。なお、ロボットＲが自己位置を誤って推定する原因としては、例えば、人間によってロボットＲが強制移動させられること（いわゆる誘拐ロボット問題）が挙げられる。また、ロボットＲが自己位置を誤って推定する原因としては、例えば、ロボットＲが移動に用いる車輪の空回りやロボットＲが障害物に衝突したときの姿勢の変化が挙げられる。
【００１８】
また、ロボットＲが自己位置を誤って推定する原因としては、例えば、障害物Ｏの位置情報と実際の障害物Ｏの位置との誤差や、シミュレーションによる計算誤差が、シミュレーションごとに蓄積していくことが挙げられる。また、ロボットＲが自己位置を誤って推定する原因としては、例えば、粒子群ｐｓの粒子ｐの個数が少ないため、移動体の状態の確率分布の近似が不十分であることが挙げられる。
【００１９】
図１の（Ｂ）において、（２）ロボットＲは、現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓに加えて、心配粒子群ａｓを生成する。心配粒子群ａｓとは、危険領域Ｄの位置情報に基づいて危険領域Ｄの近傍領域でのロボットＲの或る状態を示す心配粒子ａの集合である。
【００２０】
（３）次に、ロボットＲは、距離センサを用いて、ロボットＲの位置から障害物Ｏまでの距離を測定する。
【００２１】
（４）そして、ロボットＲは、障害物Ｏの位置と、粒子群ｐｓと心配粒子群ａｓとの各粒子が示すロボットＲの位置と、に基づいて、ロボットＲから障害物Ｏまでの距離を算出する。ロボットＲは、測定したロボットＲの位置から障害物Ｏまでの距離、および算出した各粒子が示すロボットＲの位置から障害物Ｏまでの距離に基づいて、現在時刻「ｔ」でのロボットＲの状態を示す粒子群ｐｓのリサンプリングを行う。
【００２２】
リサンプリングとは、粒子群ｐｓと心配粒子群ａｓとの各粒子の中から、粒子が示す状態の尤度が高い粒子を優先して粒子群ｐｓの粒子ｐとして選択し、粒子群ｐｓを選択し直すことである。このとき、同一の粒子を複数回選択してもよい。そして、選択し直された粒子群ｐｓの示す状態が、現在時刻「ｔ」でのロボットＲの状態として推定し直される。
【００２３】
ここでは、粒子群ｐｓの各粒子ｐが示すロボットＲの位置から障害物Ｏまでの距離は、測定したロボットＲの位置から障害物Ｏまでの距離より短くなっている。例えば、粒子群ｐｓの粒子ｐ１が示すロボットＲの位置から障害物Ｏまでの距離ｄｐは、ロボットＲの位置から障害物Ｏまでの測定距離ｄｒより短くなっている。このため、ロボットＲは、「粒子群ｐｓの各粒子ｐの示すロボットＲの位置が実際のロボットＲの位置とはかけ離れている」として、尤度が低いと判定する。
【００２４】
一方、（Ｂ）に示した心配粒子群ａｓの各粒子ａの位置から障害物Ｏまでの距離は、測定したロボットＲの位置から障害物Ｏまでの距離とほぼ同一である。例えば、心配粒子群ａｓの粒子ａ１の位置から障害物Ｏまでの距離ｄａは、ロボットＲの位置から障害物Ｏまでの測定距離ｄｒとほぼ同一である。このため、ロボットＲは、「心配粒子群ａｓの各粒子ａの示すロボットＲの位置が実際のロボットＲの位置に近い」として、尤度が高いと判定する。
【００２５】
そして、ロボットＲは、粒子群ｐｓの各粒子ｐより尤度が高い心配粒子群ａｓの各粒子ａを優先して、粒子群ｐｓの粒子ｐとして選択し直す。そして、ロボットＲは、選択し直された粒子群ｐｓにより、現在時刻「ｔ」でのロボットＲの状態を推定し直す。
【００２６】
図１の（Ｃ）において、（５）ロボットＲは、リサンプリング結果から推定し直された現在時刻「ｔ」でのロボットＲの状態から、現在時刻「ｔ」での自己位置を位置Ｓ２であると推定する。そして、ロボットＲは、危険領域Ｄの位置情報に基づいて、推定された自己位置である位置Ｓ２が危険領域Ｄの近傍領域に含まれると判定して、危険領域Ｄから離れるように移動することで、危険領域Ｄへの進入を回避する。
【００２７】
このように、ロボットＲは、真の自己位置が危険領域Ｄの近傍領域にある場合、尤度の高い心配粒子ａが粒子群ｐｓの粒子ｐとして選択されるため、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【００２８】
（推定装置のハードウェア構成例）
次に、図２を用いて、推定装置のハードウェア構成例について説明する。
【００２９】
図２は、実施の形態にかかる推定装置のハードウェア構成例を示すブロック図である。図２において、推定装置２００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０１と、ＲＯＭ（Ｒｅａｄ‐Ｏｎｌｙ Ｍｅｍｏｒｙ）２０２と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２０３と、センサ２０４と、出力装置２０５と、駆動装置２０６と、を備えている。また、各構成部はバス２１０によってそれぞれ接続されている。
【００３０】
ここで、ＣＰＵ２０１は、推定装置２００の全体の制御を司る。ＲＯＭ２０２は、ブートプログラムなどのプログラムを記憶している。ＲＡＭ２０３は、ＣＰＵ２０１のワークエリアとして使用される。
【００３１】
センサ２０４は、電磁波の送受信を行い、受信した電磁波を電気信号に変換し、ＲＡＭ２０３に保持する。例えば、センサ２０４として、赤外線受光素子などの距離センサを採用できる。また、センサ２０４は、光学的な映像を電気信号に変換し、ＲＡＭ２０３に保持する。例えば、ＣＣＤ（Ｃｈａｒｇｅ Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）、ＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ−Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）センサなどの光学的な映像を電気信号に変換する光電変換素子を採用できる。
【００３２】
出力装置２０５は、データの出力を行う。出力装置２０５としては、例えば、ディスプレイ、スピーカ等を用いることができる。駆動装置２０６は、ＣＰＵ２０１の制御により推定装置２００を駆動させる。駆動装置２０６としては、例えば、モータ、エンジン、ブレーキ等を用いることができる。推定装置２００としては、例えば、自己位置を推定しながら移動する自律移動ロボットＲや、周辺車両の位置を推定しながら移動する車両が挙げられる。
【００３３】
（物体テーブルの記憶内容）
次に、図３を用いて、推定装置２００が有する物体テーブルの記憶内容の一例について説明する。物体テーブルとは、移動体が移動する領域内の物体の形状や位置を記憶するテーブルである。物体としては、例えば、図１に示した障害物Ｏや壁が挙げられる。物体テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００３４】
図３は、推定装置２００が有する物体テーブルの記憶内容の一例を示す説明図である。図３に示すように、物体テーブル３００は、物体ＩＤ項目のそれぞれに対応付けて、形状項目と、位置項目と、を有し、物体ごとにレコードを構成する。
【００３５】
物体ＩＤ項目には、移動体が移動する領域内の物体を特定する識別子が記憶されている。形状項目には、物体ＩＤ項目の内容により特定される物体の形状が記憶されている。具体的には、例えば、形状項目には、物体の形状の種別フラグと、当該種別の形状を特定する値と、が記憶される。より具体的には、例えば、形状項目には、物体の形状が直方体であることを示す種別フラグと、直方体の奥行きと、直方体の幅と、直方体の高さと、が記憶される。また、より具体的には、例えば、形状項目には、物体の形状が円柱であることを示す種別フラグと、円柱底面の中心の位置と、円柱底面の半径と、円柱の高さと、が記憶されてもよい。
【００３６】
位置項目には、物体ＩＤ項目の内容により特定される物体の位置が記憶されている。具体的には、例えば、位置項目には、物体の位置を示す座標値（例えば、３次元空間でのｘ座標値とｙ座標値とｚ座標値）が記憶される。また、位置項目には、物体の姿勢（例えば、３次元空間でのｘ軸に対する角度とｙ軸に対する角度とｚ軸に対する角度）が記憶されていてもよい。
【００３７】
（危険領域テーブルの記憶内容）
次に、図４を用いて、推定装置２００が有する危険領域テーブルの記憶内容の一例について説明する。危険領域テーブルとは、移動体が進入すべきではないと設定された領域（危険領域Ｄ）の近傍領域を記憶するテーブルである。危険領域テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００３８】
図４は、推定装置２００が有する危険領域テーブルの記憶内容の一例を示す説明図である。図４に示すように、危険領域テーブル４００は、領域ＩＤ項目のそれぞれに対応付けて、近傍領域項目と、心配粒子ＩＤリスト項目と、を有し、危険領域Ｄごとにレコードを構成する。
【００３９】
領域ＩＤ項目には、移動体が進入すべきではないと設定された領域（危険領域Ｄ）を特定する識別子が記憶されている。近傍領域項目には、領域ＩＤ項目の内容により特定される危険領域Ｄの近傍領域の範囲を特定する情報が記憶されている。具体的には、例えば、近傍領域項目には、近傍領域が矩形であれば、辺を共有しない２頂点の座標値が記憶される。心配粒子ＩＤリスト項目には、移動体の或る状態を示す心配粒子ａを特定する識別子が記憶されている。
【００４０】
（粒子テーブルの記憶内容）
次に、図５を用いて、推定装置２００が有する粒子テーブルの記憶内容の一例について説明する。粒子テーブルとは、推定された移動体の位置を示す粒子ｐを記憶するテーブルである。粒子テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００４１】
図５は、推定装置２００が有する粒子テーブルの記憶内容の一例を示す説明図である。図５に示すように、粒子テーブル５００は、粒子ＩＤ項目のそれぞれに対応付けて、変数項目（図５では変数１項目〜変数ｋ項目）を有し、粒子ｐごとにレコードを構成する。
【００４２】
粒子ＩＤ項目には、移動体が取り得る或る状態を示す粒子ｐを特定する識別子が記憶されている。変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐが示す状態を特定する変数の値が記憶されている。変数項目は、図５に示すように複数あってもよいし、１つであってもよい。
【００４３】
例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、３次元空間でのｘ座標値とｙ座標値とｚ座標値が記憶される。
【００４４】
また、例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの速度が記憶されている。なお、速度の単位は、例えば、「ｍ／ｓ（ｍｅｔｅｒ／ｓｅｃｏｎｄ）」である。また、例えば、変数項目には、粒子ＩＤ項目の内容により特定される粒子ｐの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を０°としたときの粒子ｐの進行方向の角度が記憶される。進行方向の単位は、例えば、「°」である。
【００４５】
（心配粒子テーブルの記憶内容）
次に、図６を用いて、推定装置２００が有する心配粒子テーブルの記憶内容の一例について説明する。心配粒子テーブルとは、危険領域Ｄの近傍領域の状態を示す心配粒子ａを記憶するテーブルである。心配粒子テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００４６】
図６は、推定装置２００が有する心配粒子テーブルの記憶内容の一例を示す説明図である。図６に示すように、心配粒子テーブル６００は、心配粒子ＩＤ項目のそれぞれに対応付けて、変数項目（図６では変数１項目〜変数ｋ項目）を有し、心配粒子ａごとにレコードを構成する。
【００４７】
心配粒子ＩＤ項目には、危険領域Ｄの近傍領域で移動体が取り得る或る状態を示す心配粒子ａを特定する識別子が記憶されている。変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａが示す状態を特定する変数の値が記憶されている。変数項目は、図６に示すように複数あってもよいし、１つであってもよい。
【００４８】
例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、３次元空間でのｘ座標値とｙ座標値とｚ座標値が記憶される。
【００４９】
また、例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの速度が記憶されている。なお、速度の単位は、例えば、「ｍ／ｓ（ｍｅｔｅｒ／ｓｅｃｏｎｄ）」である。また、例えば、変数項目には、心配粒子ＩＤ項目の内容により特定される心配粒子ａの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を０°としたときの心配粒子ａの進行方向の角度が記憶される。
【００５０】
（センサ情報テーブルの記憶内容）
次に、図７を用いて、推定装置２００が有するセンサ情報テーブルの記憶内容の一例について説明する。センサ情報テーブルとは、推定装置２００が有するセンサの測定値を記憶するテーブルである。センサ情報テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００５１】
図７は、推定装置２００が有するセンサ情報テーブルの記憶内容の一例を示す説明図である。図７に示すように、センサ情報テーブル７００は、時刻項目のそれぞれに対応付けて、測定値項目（図７では、測定値１項目〜測定値Ｍ項目）を有し、センサ２０４による測定を行った時刻ごとにレコードを構成する。
【００５２】
時刻項目には、センサ２０４による測定を行った時刻が記憶されている。測定値項目には、時刻項目の時刻で測定されたセンサ２０４の値が記憶されている。測定値項目は、図７に示すように複数あってもよいし、１つであってもよい。例えば、移動体の前方、後方、左方、および右方のそれぞれの方向に存在する障害物Ｏまでの距離を記憶する測定値項目があってもよい。
【００５３】
具体的には、例えば、測定値項目には、距離センサによって測定された移動体から障害物Ｏまでの距離が記憶される。測定値項目の単位は、例えば、「ｍ（ｍｅｔｅｒ）」である。
【００５４】
（尤度テーブルの記憶内容）
次に、図８を用いて、推定装置２００が有する尤度テーブルの記憶内容の一例について説明する。尤度テーブルとは、推定装置２００により算出された粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度を記憶するテーブルである。尤度テーブルは、例えば、図２に示したＲＡＭ２０３により実現される。
【００５５】
図８は、推定装置２００が有する尤度テーブルの記憶内容の一例を示す説明図である。図８に示すように、尤度テーブル８００は、粒子ＩＤ項目のそれぞれに対応付けて、尤度項目を有し、粒子ｐごとにレコードを構成する。また、尤度テーブル８００は、移動体の状態を推定するごとにレコードを更新する。
【００５６】
粒子ＩＤ項目には、粒子群ｐｓと心配粒子群ａｓとの各粒子を特定する識別子が記憶されている。尤度項目には、粒子ＩＤ項目の内容から特定される粒子が示す移動体の状態の尤もらしさを示す尤度が記憶されている。
【００５７】
（推定装置２００の機能的構成例）
次に、図９を用いて、推定装置２００の機能的構成例について説明する。
【００５８】
図９は、推定装置２００の機能的構成を示すブロック図である。推定装置２００は、更新部９０１と、生成部９０３と、測定部９０２と、算出部９０４と、判断部９０５と、設定部９０６と、特定部９０７と、判定部９０８と、制御部９０９と、を含む構成である。
【００５９】
更新部９０１は、粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、移動体の状態を示す第１の粒子群を現状態から次状態に更新する。ここで、空間とは、移動体の移動する領域である。例えば、空間とは、移動体がロボットＲであれば、ロボットＲの移動する部屋や建物であり、移動体が車両であれば、道路である。状態方程式とは、過去の移動体の状態を入力すると移動体の現在の状態を決定する方程式である。
【００６０】
例えば、状態方程式とは、運動方程式であって、時刻「ｔ−１」での移動体の位置と速度を入力すると、時刻「ｔ」での移動体の位置と速度を決定する方程式である。また、状態方程式とは、後述する式（２）や式（５）である。移動体の状態を示す第１の粒子群とは、移動体の状態の確率分布を近似する粒子群ｐｓである。粒子群ｐｓの１粒子は、移動体の取り得る或る状態を示す。
【００６１】
具体的には、例えば、更新部９０１は、粒子テーブル５００に記憶されている直前の時刻での移動体の状態を示す粒子群ｐｓの各粒子ｐの状態を状態方程式に入力して、現在時刻での各粒子ｐの状態を算出する。更新部９０１は、算出された現在時刻での各粒子ｐの状態により、粒子テーブル５００を更新する。
【００６２】
これにより、更新部９０１は、移動体の過去の状態から移動体の現在の状態を推定することができる。なお、更新部９０１は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００６３】
測定部９０２は、更新部９０１によって更新される都度、移動体と空間内の物体との距離を測定する。ここで、移動体と空間内の物体との距離とは、例えば、移動体が推定装置２００自体である場合には、推定装置２００から空間内の物体（障害物Ｏや壁）までの距離である。また、移動体と空間内の物体との距離とは、例えば、移動体と推定装置２００とが異なる場合は、移動体から空間内の物体（推定装置２００自体）までの距離である。具体的には、例えば、測定部９０２は、更新部９０１によって更新される都度、センサ２０４により、移動体と空間内の物体との距離を測定する。
【００６４】
これにより、測定部９０２は、算出部９０４により各粒子の尤度の算出に使用される移動体から空間内の物体までの距離を得ることができる。なお、測定部９０２は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、または、センサ２０４により、その機能を実現する。
【００６５】
生成部９０３は、更新部９０１によって更新される都度、空間内の特定の領域の状態を示す第２の粒子群を生成する。ここで、特定の領域とは、予め設定された移動体が進入すべきではない禁止領域の近傍領域である。禁止領域とは、例えば上述した危険領域Ｄである。また、近傍領域とは、禁止領域の周囲の領域である。禁止領域や近傍領域の範囲は、任意に設定される。第２の粒子群とは、危険領域Ｄの近傍領域での移動体の状態を示す粒子群であり、例えば、上述した心配粒子群ａｓである。具体的には、例えば、生成部９０３は、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出し、心配粒子群ａｓを生成する。
【００６６】
これにより、生成部９０３は、危険領域Ｄの近傍領域に心配粒子ａを生成して、特定部９０７による移動体の位置の特定において、移動体の真の位置が危険領域Ｄの近傍領域にあれば、移動体の位置を危険領域Ｄの近傍領域にあると特定できるようにすることができる。生成部９０３は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００６７】
算出部９０４は、測定部９０２によって測定された距離と、更新部９０１による更新されたときの第１の粒子群と、生成部９０３によって生成された第２の粒子群と、に基づいて、第１および第２の粒子群の各粒子の尤度を算出する。ここで、尤度とは、粒子が示す移動体の状態の尤もらしさを示す値である。尤度としては、例えば、測定により得られた実際の移動体の状態での測定値を期待値とした正規分布において、各粒子の示す移動体の状態から予測される測定値となる確率を採用することができる。
【００６８】
具体的には、例えば、算出部９０４は、更新部９０１によって更新されたときの粒子群ｐｓの各粒子ｐが示す移動体の状態から、粒子群ｐｓの各粒子ｐが示す移動体の位置から空間内の物体までの距離を算出する。次に、算出部９０４は、生成部９０３によって生成された心配粒子群ａｓの各心配粒子ａが示す移動体の状態から、心配粒子群ａｓの各粒子ａが示す移動体の位置から空間内の物体までの距離を算出する。
【００６９】
そして、算出部９０４は、測定部９０２によって測定された移動体から空間内の物体までの距離を期待値とする正規分布において、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す移動体の位置から空間内の物体までの距離となる確率を算出する。算出部９０４は、算出された確率を各粒子の尤度として、尤度テーブル８００を更新する。
【００７０】
ここで、算出部９０４は、測定部９０２によって距離を測定された物体が複数ある場合は、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す移動体の位置からそれぞれの物体までの距離となる確率を算出する。そして、算出部９０４は、物体ごとに算出された確率の総乗を各粒子の尤度として、尤度テーブル８００を更新する。
【００７１】
これにより、算出部９０４は、粒子群ｐｓの各粒子ｐの尤度と心配粒子群ａｓの各粒子ａの尤度を算出することができる。なお、算出部９０４は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００７２】
判断部９０５は、算出部９０４によって前回算出された尤度がしきい値以上であるか否かを判断する。ここで、しきい値は、推定装置２００の利用者によって予め設定された値である。上述した正規分布における確率を尤度に採用した場合、しきい値としては、例えば、「期待値±標準偏差の値が測定値となる確率」を採用することができる。具体的には、例えば、判断部９０５は、算出部９０４によって前回算出された粒子群ｐｓの各粒子ｐの尤度がしきい値以上であるか否かを判断し、尤度がしきい値以上の粒子ｐがあれば、時刻と尤度がしきい値以上の粒子ｐの状態とをＲＡＭ２０３などの記憶領域に記憶する。
【００７３】
また、判断部９０５は、算出部９０４によって前回算出された粒子群ｐｓの各粒子ｐの尤度の総和がしきい値以上であるか否かを判断し、尤度の総和がしきい値以上であれば、時刻と各粒子ｐの状態とをＲＡＭ２０３などの記憶領域に記憶してもよい。これにより、判断部９０５は、尤度がしきい値以上であり、推定の精度が高いと解される移動体の状態を記憶しておくことができる。
【００７４】
この場合、生成部９０３は、判断部９０５によって前回算出された尤度がしきい値以上であると判断された場合には、第２の粒子群の生成を停止する。具体的には、例えば、生成部９０３は、判断部９０５によって前回の移動体の状態を示す粒子群ｐｓのいずれかの粒子ｐの尤度がしきい値以上であると判断された場合には、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出さない。前回算出された粒子群ｐｓの各粒子ｐの尤度の総和の場合も同様である。
【００７５】
これにより、生成部９０３は、移動体の状態の推定の精度が高い場合は、心配粒子ａを生成せずに、処理を低減することができる。また、生成部９０３は、移動体の状態の推定の精度が低い場合には、心配粒子ａを生成することで、移動体の真の位置が危険領域Ｄの近傍領域にあるときは移動体の位置を危険領域Ｄの近傍領域にあると特定部９０７が特定できるようにすることができる。
【００７６】
なお、判断部９０５は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００７７】
設定部９０６は、判断部９０５によってしきい値以上であると判断された尤度が得られたときの移動体の空間内の位置に基づいて、移動体の移動範囲を設定する。具体的には、例えば、設定部９０６は、判断部９０５によってしきい値以上であると判断された尤度が得られたときの移動体の推定位置と、現在時刻までの移動体が移動した速度と、に基づいて、現在時刻までに移動体が移動可能な範囲を算出する。そして、設定部９０６は、算出した移動体が移動可能な範囲をＲＡＭ２０３などの記憶領域に記憶する。これにより、設定部９０６は、移動体が移動しうる範囲を設定することができる。
【００７８】
この場合、生成部９０３は、近傍領域のうち、設定部９０６によって設定された移動範囲と重複する領域に第２の粒子群を生成する。生成部９０３は、危険領域テーブル４００の中で、近傍領域項目の情報から特定される近傍領域の範囲と、設定部９０６によって設定された移動体の移動範囲と、が重複するレコードを特定する。そして、生成部９０３は、特定したレコードの心配粒子ＩＤリストを参照して、心配粒子テーブル６００から心配粒子ａが示す移動体の状態（例えば、移動体の位置、速度、および進行方向）を読み出すことになる。
【００７９】
これにより、生成部９０３は、移動体の移動範囲と重複しない近傍領域には心配粒子ａを生成せずに、処理を低減することができる。また、生成部９０３は、移動体の移動範囲と重複する近傍領域には、心配粒子ａを生成することで、特定部９０７による移動体の位置の特定において、移動体の位置を危険領域Ｄの近傍領域にあると特定できるようにすることができる。
【００８０】
なお、設定部９０６は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８１】
特定部９０７は、算出部９０４によって算出された各粒子の尤度に基づいて、空間内での移動体の位置を特定する。具体的には、例えば、特定部９０７は、算出部９０４によって算出された各粒子の尤度に比例した確率に従って、各粒子の中から粒子群ｐｓの粒子ｐを選択し直す。
【００８２】
より具体的には、例えば、特定部９０７は、［０，１］区間上で、粒子ごとに、当該粒子の尤度に比例する区間長になるように分割された区間を対応付ける。そして、特定部９０７は、［０，１］の一様乱数を発生させて、乱数の値が入る区間に対応する粒子を、粒子群ｐｓの粒子ｐとして選択し直す。ここで、粒子群ｐｓの粒子ｐとして、１粒子を複数回選択してもよい。そして、特定部９０７は、リサンプリングされた粒子群ｐｓが示す移動体の状態から、現在の移動体の位置を特定する。
【００８３】
これにより、特定部９０７は、移動体の位置を特定することができる。なお、特定部９０７は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８４】
判定部９０８は、特定部９０７によって特定された移動体の位置が特定の領域にあるか否かを判定する。具体的には、例えば、判定部９０８は、特定部９０７によって特定された移動体の位置が、危険領域テーブル４００の近傍領域項目の内容から特定される近傍領域の範囲に含まれているか否かを判定し、判定結果をＲＡＭ２０３などの記憶領域に記憶する。
【００８５】
これにより、判定部９０８は、移動体が危険領域Ｄの近傍領域にあるか否かを判定することができる。なお、判定部９０８は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８６】
制御部９０９は、判定部９０８によって特定の領域に位置すると判定された場合、特定の領域から離れる方向へ移動体を移動するように制御する。具体的には、例えば、制御部９０９は、判定部９０８によって危険領域Ｄの近傍領域に移動体が位置すると判定された場合、危険領域テーブル４００を参照して危険領域Ｄから離れる方向へ移動体を移動するように、駆動装置２０６を制御する。
【００８７】
より具体的には、例えば、制御部９０９は、危険領域テーブル４００の近傍領域項目の内容から特定される近傍領域の範囲と、粒子群ｐｓが示す移動体の位置と進行方向とから、移動体の進行方向に危険領域Ｄがあるか否かを判定する。そして、制御部９０９は、移動体の進行方向に危険領域Ｄがあれば、移動体の進行方向が逆方向になるように、駆動装置２０６を制御する。
【００８８】
これにより、制御部９０９は、移動体の危険領域Ｄへの進入を低減することができる。なお、判断部９０５は、具体的には、例えば、図２に示したＲＯＭ２０２、ＲＡＭ２０３などの記憶装置に記憶されたプログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００８９】
（実施例１）
次に、実施例１について説明する。実施例１は、移動体が推定装置２００自体である場合での移動体の位置推定の例である。具体的には、推定装置２００（移動体）として、自律移動ロボットＲ（以下では、「ロボットＲ」という）を採用した場合について説明する。
【００９０】
（実施例１にかかる推定装置２００の動作例１）
まず、図１０〜図１２を用いて、実施例１にかかる推定装置２００の動作例１について説明する。なお、ロボットＲは、物体テーブル３００および危険領域テーブル４００を有する。また、ロボットＲは、ロボットＲの移動する領域内でのロボットＲの或る状態を示す粒子ｐの集合である粒子群ｐｓを用いたシミュレーションにより、ロボットＲの状態を推定する。そして、ロボットＲは、物体テーブル３００および危険領域テーブル４００と、推定された状態と、に基づいて、危険領域Ｄおよび障害物Ｏが存在する領域内での自己位置を推定し、危険領域Ｄを回避しつつ移動を行う。
【００９１】
図１０〜図１２は、実施例１にかかる推定装置２００の動作例１を示す説明図である。図１０の（Ａ）において、ロボットＲは、時刻「ｔ−１」での移動体の状態ｘt-1を示す粒子群ｐｓから、時刻「ｔ−１」での自己位置を位置Ｓ３であると推定している。なお、移動体の状態ｘtは、下記式（１）で表される。ここで、ｘは移動体の位置であり、ｖ（ｘ）は移動体の速度である。
【００９２】
【数１】
【００９３】
図１０の（Ｂ）において、ロボットＲは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」での移動体の状態ｘt-1を示す粒子群ｐｓから、現在時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓを推定する。
【００９４】
（１）例えば、ロボットＲは、粒子群ｐｓの粒子ｐ１が示す状態ｘt-1(1)を、下記式（２）の状態方程式に入力して現在の粒子の状態ｘt(1)を算出する。ここで、ｉは便宜上付与された粒子の番号である。ηt(i)はシステムノイズである。ΔＴは、タイムステップであり、「（ｔ）−（ｔ−１）」である。ａt(i)は移動体の加速度である。
【００９５】
【数２】
【００９６】
（２）次に、ロボットＲは、粒子群ｐｓの他の粒子ｐについても、同様に現在の粒子ｐの状態ｘtを算出していく。これにより、ロボットＲは、時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓを推定することができる。
【００９７】
次に、図１１において、ロボットＲは、図１０で推定された現在時刻「ｔ」での移動体の状態ｘtを示す粒子群ｐｓと、危険領域Ｄの近傍領域に生成した危険領域Ｄの近傍領域での移動体の状態ｘtを示す心配粒子群ａｓと、の各粒子の尤度を算出する。図１１の例では、１個の粒子ｐ１と１個の心配粒子ａ１との各粒子の尤度の算出を例に挙げる。
【００９８】
（１）まず、推定装置２００は、物体テーブル３００を参照して、粒子ｐ１と心配粒子ａ１との各粒子が示す位置からの空間内の物体までの距離を算出する。例えば、推定装置２００は、空間内の物体までの距離として、粒子が示す位置からの前方の壁までの距離と右方の壁までの距離と左方の壁までの距離と後方の壁までの距離を算出する。
【００９９】
より具体的には、例えば、推定装置２００は、粒子ａ１が示す位置からの前方の壁までの距離ｄａ＿ｆと右方の壁までの距離ｄａ＿ｒと左方の壁までの距離ｄａ＿ｌと後方の壁までの距離ｄａ＿ｂを算出する。また、推定装置２００は、粒子ｐ１が示す位置からの前方の壁までの距離ｄｐ＿ｆと右方の壁までの距離ｄｐ＿ｒと左方の壁までの距離ｄｐ＿ｌと後方の壁までの距離ｄｐ＿ｂを算出する。
【０１００】
（２）次に、推定装置２００は、センサ２０４により、実際の自己位置からの物体までの距離を測定する。例えば、推定装置２００は、４つのセンサ２０４により、推定装置２００の前方の壁までの距離ｄｒ＿ｆと右方の壁までの距離ｄｒ＿ｒと左方の壁までの距離ｄｒ＿ｌと後方との壁までの距離ｄｒ＿ｂを算出する。なお、ここでは、推定装置２００は、４つのセンサ２０４により各壁までの距離を測定したが、これに限らない。例えば、推定装置２００は、１つのセンサ２０４の測定方向を変化させて、各壁までの距離を測定してもよい。
【０１０１】
（３）そして、推定装置２００は、粒子ｐ１が示す位置からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ（尤度）を算出する。例えば、推定装置２００は、測定された各壁までの距離を期待値とした正規分布において、粒子ｐ１が示す位置からの各壁までの距離になる確率の総乗ｐ（ｙt｜ｘt|t-1(i)）を、下記式（３）により算出する。ここで、σjは標準偏差である。ｙjtはセンサ２０４により測定された壁までの距離である。ｙjt(i)は算出された壁までの距離である。ｉは粒子ｐの番号である。ｊはセンサ２０４の番号である。
【０１０２】
【数３】
【０１０３】
そして、推定装置２００は、算出した確率の総乗を、粒子ｐ１の尤度とする。なお、図１１では、粒子ｐ１が示す位置からの各壁までの距離が真の自己位置から各壁までの距離と差が大きいため、粒子ｐ１の尤度は低くなる。
【０１０４】
（４）推定装置２００は、上記（３）と同様に心配粒子ａ１からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ（尤度）を算出する。例えば、推定装置２００は、測定された各壁までの距離を期待値とした正規分布において、心配粒子ａ１からの各壁までの距離になる確率の総乗を、式（３）により算出する。そして、推定装置２００は、算出した確率の総乗を、心配粒子ａ１の尤度とする。なお、図１１では、心配粒子ａ１からの各壁までの距離と、真の自己位置からの各壁までの距離との差が小さいため、心配粒子ａ１の尤度は高くなる。
【０１０５】
そして、図１２において、ロボットＲは、図１１のようにして算出した粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、現在の自己位置を示す粒子群ｐｓをリサンプリングして、現在の自己位置を特定する。
【０１０６】
具体的には、リサンプリングでは、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に比例した確率に従って、各粒子の中から粒子群ｐｓの粒子ｐを選択し直す。このとき、同一の粒子を複数回選択してもよい。より具体的には、例えば、ロボットＲは、各粒子の尤度に比例するように粒子ごとに分割した［０，１］区間上で、［０，１］の一様乱数を発生させて、乱数の値が入る区間に対応する粒子（粒子ｐまたは心配粒子ａ）を、粒子群ｐｓの粒子ｐとして選択する。
【０１０７】
図１２に示すように、ロボットＲの真の自己位置が、図１０で推定された粒子群ｐｓが示す位置Ｓ４の近傍領域にあれば、粒子群ｐｓの各粒子ｐの尤度が高くなり、心配粒子群ａｓの各粒子ａの尤度が低くなる。そのため、ロボットＲは、リサンプリングにより、図１０で推定された粒子群ｐｓの各粒子ｐを心配粒子群ａｓの各粒子ａより優先して、粒子群ｐｓの粒子ｐとして選択し直し、粒子群ｐｓを推定し直す。そして、ロボットＲは、推定し直された粒子群ｐｓが示す位置Ｓ４を自己位置であると推定することができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域ではないと判定して、通常動作を実行することができる。
【０１０８】
一方、ロボットＲの真の自己位置が、図１０で推定された粒子群ｐｓが示す位置Ｓ４の近傍領域ではなく、危険領域Ｄの近傍領域にあれば、粒子群ｐｓの各粒子ｐの尤度が低くなり、心配粒子群ａｓの各粒子ａの尤度が高くなる。そのため、ロボットＲは、リサンプリングにより、心配粒子群ａｓの各粒子ａを粒子群ｐｓの各粒子ｐより優先して、粒子群ｐｓの粒子ｐとして選択し直し、粒子群ｐｓを推定し直す。そして、ロボットＲは、推定し直された粒子群ｐｓが示す位置Ｓ５を自己位置であると推定することができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると判定して、危険領域Ｄから離れる方向へ移動することができる。
【０１０９】
このように、ロボットＲは、心配粒子群ａｓを用いてリサンプリングを行うことにより、心配粒子群ａｓの各粒子ａの尤度が高い場合は、推定し直す粒子群ｐｓの粒子ｐとして心配粒子ａを選択する。ここで、ロボットＲは、真の自己位置が危険領域Ｄの近傍領域にある場合、尤度の高い心配粒子ａが粒子群ｐｓの粒子ｐとして選択されることにより、自己位置の推定精度の向上を図ることができる。
【０１１０】
一方、ロボットＲは、心配粒子群ａｓを用いてリサンプリングを行っても、真の自己位置が危険領域Ｄの近傍領域でなければ、尤度の低い心配粒子ａは粒子群ｐｓの粒子ｐとして選択されないため、自己位置を危険領域Ｄの近傍領域にあると誤って推定することはない。また、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１１１】
（実施例１にかかる推定装置２００の動作例１における推定装置２００の動作制御処理）
次に、図１３を用いて、実施例１にかかる推定装置２００の動作例１における推定装置２００の動作制御処理の処理内容について説明する。
【０１１２】
図１３は、実施例１の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。まず、ＣＰＵ２０１は、初期化処理を行う（ステップＳ１３０１）。なお、初期化処理では、推定装置２００は、物体テーブル３００から空間内の物体の情報を読み出し、危険領域テーブル４００から空間内の危険領域Ｄの情報を読み出し、空間内に均等に粒子ｐを生成する。
【０１１３】
次に、推定装置２００は、粒子フィルタを用いたシミュレーションにより、移動体の直前の状態を示す粒子群ｐｓから、移動体の現在の状態を示す粒子群ｐｓを推定する（ステップＳ１３０２）。なお、ステップＳ１３０２の処理は、図１０に示した推定装置２００の処理に対応する。
【０１１４】
そして、推定装置２００は、センサ２０４により、自装置から障害物Ｏまでの距離を測定する（ステップＳ１３０３）。次に、ＣＰＵ２０１は、心配粒子生成処理を実行する（ステップＳ１３０４）。
【０１１５】
心配粒子生成処理において、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する。
【０１１６】
そして、推定装置２００は、ステップＳ１３０２で推定された粒子群ｐｓとステップＳ１３０４で生成された心配粒子群ａｓとの各粒子の尤度を算出する（ステップＳ１３０５）。なお、ステップＳ１３０３〜ステップＳ１３０５の処理は、図１１に示した推定装置２００の処理に対応する。
【０１１７】
次に、推定装置２００は、ステップＳ１３０５で算出した各粒子の尤度から、ステップＳ１３０２で生成された粒子群ｐｓのリサンプリングを行い、移動体の現在の状態を特定する（ステップＳ１３０６）。そして、推定装置２００は、特定した移動体の現在の状態に応じて動作を決定する（ステップＳ１３０７）。なお、ステップＳ１３０６およびステップＳ１３０７の処理は、図１２に示した推定装置２００の処理に対応する。
【０１１８】
次に、推定装置２００は、動作終了か否かを判定する（ステップＳ１３０８）。なお、例えば、動作終了とは、ロボットＲが目的地に到着したこと、現在時刻が所定の時刻であることが挙げられる。ここで、動作終了していない場合（ステップＳ１３０８：Ｎｏ）、ＣＰＵ２０１は、ステップＳ１３０２に戻る。
【０１１９】
一方、動作終了している場合（ステップＳ１３０８：Ｙｅｓ）、推定装置２００は、動作制御処理を終了する。これにより、推定装置２００（移動体）は、自己位置を推定し、危険領域Ｄを回避しつつ移動を行うことができる。
【０１２０】
（実施例１にかかる推定装置２００の動作例２）
次に、図１４を用いて、実施例１にかかる推定装置２００の動作例２について説明する。
【０１２１】
図１４は、実施例１にかかる推定装置２００の動作例２を示す説明図である。動作例１ではロボットＲの推定した自己位置の精度に関わらず心配粒子ａを生成していたが、動作例２では、ロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しない。また、動作例２では、ロボットＲにより推定された自己位置の精度が低いときには心配粒子ａを生成する。
【０１２２】
これにより、ロボットＲは、現在の自己位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。一方、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１２３】
図１４の（Ａ）に示すように、ロボットＲにより時刻「ｔ−１」で推定された粒子群ｐｓの尤度の総和がしきい値以上であるため、時刻「ｔ−１」でロボットＲにより推定された自己位置は精度が高いものとする。この場合、ロボットＲは、危険領域Ｄの近傍領域に心配粒子ａを生成せずとも、次の時刻「ｔ」の自己位置を推定できる。
【０１２４】
図１４の（Ｂ）に示すように、（１）ロボットＲは、次の時刻「ｔ」になると、（Ａ）に示した時刻「ｔ−１」で推定された粒子群ｐｓから、時刻「ｔ」でのロボットＲの自己位置を示す粒子群ｐｓを推定する。（２）ここで、ロボットＲは、心配粒子群ａｓを生成しない。
【０１２５】
（３）次に、ロボットＲは、センサ２０４により、真の自己位置から障害物Ｏまでの距離を測定する。（４）そして、ロボットＲは、障害物Ｏまでの測定距離に基づいて、粒子群ｐｓのリサンプリングを行う。
【０１２６】
図１４の（Ｃ）に示すように、（Ｂ）でリサンプリングされた粒子群ｐｓの示す自己位置は、位置Ｓ６になっている。このように、尤度の総和がしきい値以上であった粒子群ｐｓに基づいて推定された現在の自己位置を示す粒子群ｐｓは精度が高いため、ロボットＲは、正しく自己位置を推定することができている。（５）そして、ロボットＲは、危険領域テーブル４００を参照して、自己位置である位置Ｓ６が危険領域Ｄの近傍領域ではないと判定して、通常動作を行う。
【０１２７】
このように、ロボットＲは、ロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しないことで、処理量を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１２８】
（実施例１の動作例２におけるステップＳ１３０４の心配粒子生成処理）
次に、図１５を用いて、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）について説明する。なお、動作例２における推定装置２００の動作制御処理の処理内容は、心配粒子生成処理（ステップＳ１３０４）を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０１２９】
図１５は、実施例１の動作例２における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、現在の自己位置の精度が高いか否かを判定する（ステップＳ１５０１）。具体的には、例えば、推定装置２００は、前回の自己位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ１５０１：Ｙｅｓ）、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１３０】
一方、精度が低い場合（ステップＳ１５０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１５０２）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１３１】
これにより、推定装置２００は、現在の自己位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１３２】
（実施例１にかかる推定装置２００の動作例３）
次に、図１６を用いて、実施例１にかかる推定装置２００の動作例３について説明する。
【０１３３】
図１６は、実施例１にかかる推定装置２００の動作例３を示す説明図である。動作例１ではロボットＲが危険領域Ｄに進入する可能性があるか否かに関わらず心配粒子ａを生成していたが、動作例３ではロボットＲが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１３４】
これにより、ロボットＲは、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、ロボットＲは、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１３５】
図１６に示すように、ロボットＲは、推定された粒子群ｐｓの尤度の総和がしきい値以上であり、推定された自己位置の精度が高かった時刻「ｔ−６」がある場合、時刻「ｔ−６」で推定された自己位置から現在時刻「ｔ」までに移動可能な範囲を算出する。
【０１３６】
具体的には、ロボットＲは、時刻「ｔ−６」から現在時刻「ｔ」までの各時刻でのロボットＲの速度から、ロボットＲの時刻「ｔ−６」から現在時刻「ｔ」まででの最大移動距離を算出する。そして、ロボットＲは、時刻「ｔ−６」で推定された自己位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
【０１３７】
この場合、ロボットＲは、算出された移動可能な範囲に重複する危険領域Ｄには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Ｄには進入する可能性がない。そのため、ロボットＲは、移動可能な範囲に重複する危険領域Ｄの近傍領域には心配粒子ａを生成し、移動可能な範囲に重複しない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１３８】
（１）例えば、ロボットＲは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０１３９】
（２）また、ロボットＲは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０１４０】
（３）一方、ロボットＲは、算出した移動可能な範囲と重複していない危険領域Ｄの近傍領域には、心配粒子ａを生成しない。
【０１４１】
これにより、ロボットＲは、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、ロボットＲは、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットＲは、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１４２】
（実施例１の動作例３における推定装置２００の動作制御処理）
次に、実施例１の動作例３における推定装置２００の動作制御処理の処理内容について説明する。
【０１４３】
動作例３における推定装置２００の動作制御処理の処理内容は、ステップＳ１３０４の心配粒子生成処理を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０１４４】
（実施例１の動作例３におけるステップＳ１３０４の心配粒子生成処理）
次に、図１７を用いて、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）について説明する。
【０１４５】
図１７は、実施例１の動作例３における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ１７０１）。
【０１４６】
ここで、粒子ｐがなかった場合（ステップＳ１７０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１７０２）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１４７】
一方、粒子ｐがあった場合（ステップＳ１７０１：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ１７０３）。次に、推定装置２００は、特定した時刻から現在時刻までのロボットＲの状態から、現在のロボットＲの移動可能範囲を算出する（ステップＳ１７０４）。
【０１４８】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１７０５）。次に、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１４９】
これにより、推定装置２００は、進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１５０】
（実施例１にかかる推定装置２００の動作例４）
次に、実施例１にかかる推定装置２００の動作例４について説明する。動作例４は、動作例２および動作例３を組み合わせた場合の動作例である。動作例４ではロボットＲにより推定された自己位置の精度が高いときには心配粒子ａを生成しない。また、動作例４ではロボットＲが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０１５１】
これにより、推定装置２００は、推定された自己位置の精度が高いときには心配粒子ａを生成しないため、処理を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
【０１５２】
また、推定装置２００は、自装置が進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しないため、処理を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１５３】
（実施例１にかかる推定装置２００の動作例４におけるステップＳ１３０４の心配粒子生成処理）
動作例４における推定装置２００の動作制御処理の処理内容は、心配粒子生成処理（ステップＳ１３０４）を除き、図１３に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。したがって、図１８を用いて、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）について説明する。
【０１５４】
図１８は、実施例１の動作例４における心配粒子生成処理（ステップＳ１３０４）の処理内容を示すフローチャートである。心配粒子生成処理（ステップＳ１３０４）において、推定装置２００は、現在の自己位置の精度が高いか否かを判定する（ステップＳ１８０１）。具体的には、例えば、推定装置２００は、前回の自己位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ１８０１：Ｙｅｓ）、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５５】
一方、精度が低い場合（ステップＳ１８０１：Ｎｏ）、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ１８０２）。ここで、粒子ｐがなかった場合（ステップＳ１８０２：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１８０３）。そして、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５６】
一方、粒子ｐがあった場合（ステップＳ１８０２：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ１８０４）。次に、推定装置２００は、特定した時刻から現在時刻までのロボットＲの状態から、現在のロボットＲの移動可能範囲を算出する（ステップＳ１８０５）。
【０１５７】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ１８０６）。次に、推定装置２００は、図１３のステップＳ１３０５に移行する。
【０１５８】
これにより、推定装置２００は、推定された自己位置の精度が高いときには心配粒子ａを生成しないため、処理を低減することができる。また、ロボットＲは、現在の自己位置の精度が低い場合には心配粒子ａを生成することで、自己位置が危険領域Ｄの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
【０１５９】
また、推定装置２００は、自装置が進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しないため、処理を低減することができる。また、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、進入する可能性がある危険領域Ｄの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置２００は、自己位置が危険領域Ｄの近傍領域にあると推定された場合には、危険領域Ｄから離れるように移動して、危険領域Ｄに進入することを回避することができる。
【０１６０】
以上説明したように、実施例１にかかる推定装置２００によれば、推定された移動体（推定装置２００自体）の状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子群ａｓが生成される。次に、推定装置２００によれば、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置２００によれば、リサンプリングにより選択し直された粒子群ｐｓから、移動体の状態が推定し直される。
【０１６１】
このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、真の自己位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６２】
また、推定装置２００によれば、推定された移動体の状態の精度が高いときには心配粒子ａを生成しないことにより、処理量を低減することができる。また、推定装置２００によれば、現在の自己位置の精度が低い場合には心配粒子ａが生成される。このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、真の自己位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６３】
また、推定装置２００によれば、移動体が移動可能な範囲と重複しない危険領域Ｄの近傍領域は、移動体が進入する可能性がない危険領域Ｄの近傍領域であるため、心配粒子ａを生成しないことで、処理量を低減することができる。また、推定装置２００によれば、移動体が移動可能な範囲と重複する危険領域Ｄの近傍領域は、移動体が進入する可能性があるため、心配粒子ａを生成が生成される。このとき、推定装置２００の真の自己位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での移動体の状態を示す心配粒子ａが粒子群ｐｓの粒子ｐとして優先的に選択される。そのため、推定装置２００は、進入する可能性がある危険領域Ｄの近傍領域に真の自己位置がある場合、リサンプリングにより選択し直された粒子群ｐｓによる自己位置の推定精度の向上を図ることができる。
【０１６４】
また、推定装置２００によれば、推定された移動体の位置が危険領域Ｄの近傍領域にある場合、移動体が危険領域Ｄから離れるようにするため、移動体が危険領域Ｄに進入することを回避することができる。
【０１６５】
（実施例２）
次に、実施例２について説明する。実施例１は、移動体が推定装置２００自体である場合での移動体の位置推定の例であったが、実施例２は、移動体が推定装置２００とは異なる他の装置である場合での移動体の位置推定の例である。例えば、実施例２では推定装置２００として車両を採用し、移動体として推定装置２００の前方の車両を採用した場合について説明する。このとき、車両（推定装置２００）は、前方の車両（移動体）の位置を推定し、自車の周囲（危険領域Ｄ）に前方の車両が接近することを回避する。
【０１６６】
まず、図１９を用いて、実施例２にかかる推定装置２００による前方車両の位置推定の内容について説明する。
【０１６７】
図１９は、実施例２にかかる推定装置２００による前方車両の位置推定の内容を示す説明図である。図１９の例では、車両Ｃは、危険領域Ｄの位置情報を有し、前方車両Ｆの位置を推定して、前方車両Ｆが危険領域Ｄの近傍領域に進入しないように移動を行う。
【０１６８】
図１９の（Ａ）において、（１）車両Ｃは、粒子フィルタを用いたシミュレーションにより、過去の時刻「ｔ−１」での前方車両Ｆの状態を示す粒子群ｐｓから現在時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓを推定する。前方車両Ｆの状態Ａtは下記式（４）で表される。
【０１６９】
ここで、Ｗtは、前方車両Ｆの横幅である。ｘtは、前方車両Ｆの横方向の中心座標である。Ｚtは、車両Ｃから前方車両Ｆまでの距離である。ｖｘtは、車両Ｃに対する前方車両Ｆの横方向の相対速度である。ｖｚtは、車両Ｃに対する前方車両Ｆの距離方向の相対速度である。
【０１７０】
【数４】
【０１７１】
具体的には、車両Ｃは、粒子群ｐｓの各粒子ｐが示す前方車両の状態Ａt-1を、下記式（５）および式（６）の状態方程式に入力して現在の各粒子ｐの状態Ａtを算出する。ΔＴは前方車両Ｆの状態の推定を実行する間隔を示すタイムステップであり、「（ｔ）−（ｔ−１）」である。
【０１７２】
【数５】
【０１７３】
【数６】
【０１７４】
これにより、車両Ｃは、時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓを推定することができる。ここでは、車両Ｃは、推定された現在時刻「ｔ」での前方車両Ｆの状態を示す粒子群ｐｓにより、現在時刻「ｔ」での前方車両Ｆの位置を位置Ｓ７であると推定する。
【０１７５】
しかしながら、真の前方車両Ｆの位置は危険領域Ｄの近傍である位置Ｓ８であり、車両Ｃは前方車両Ｆの位置を誤って推定している。
【０１７６】
図１９の（Ｂ）において、（２）車両Ｃは、推定された現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓに加えて、危険領域Ｄの位置情報に基づいて危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子群ａｓを生成する。
【０１７７】
（３）次に、車両Ｃは、センサ２０４により、前方車両Ｆの位置までの距離ｄｒを測定する。また、車両Ｃは、センサ２０４により、前方車両Ｆの左端の座標値と右端の座標値とを測定する。
【０１７８】
（４）そして、車両Ｃは、下記式（７）および式（８）により、粒子群ｐｓと心配粒子群ａｓとの各粒子が示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を算出する。ここで、前方車両Ｆまでの距離とは、例えば、粒子ｐ１が示す前方車両Ｆの位置までの距離ｄｐや粒子ａ１が示す前方車両Ｆの位置までの距離ｄａである。
【０１７９】
【数７】
【０１８０】
【数８】
【０１８１】
車両Ｃは、算出された各粒子が示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と比較したときの尤度を算出する。そして、車両Ｃは、算出した尤度に基づいて、現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓのリサンプリングを行う。
【０１８２】
例えば、車両Ｃは、測定された前方車両Ｆの状態を示す値を期待値とした多変量正規分布において、粒子ｐが示す前方車両Ｆの状態を示す値になる確率ｐ（ｙt｜Ａt|t-1(i)）を、下記式（９）により算出する。ここで、ｙtはセンサ２０４により測定された前方車両Ｆの状態を示す値であり、例えば、前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値である。ｙt(i)は算出された前方車両Ｆの状態を示す値であり、前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値である。ρは次元数であり、ここでは３である。ｉは粒子の番号である。Σは分散共分散行列である。
【０１８３】
【数９】
【０１８４】
そして、推定装置２００は、算出した確率を、粒子ｐの尤度とする。なお、図１９では、粒子ｐが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値が真の前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と差が大きいため、粒子ｐの尤度は低くなる。
【０１８５】
車両Ｃは、同様に心配粒子ａが示す前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と比較したときの尤度を算出する。
【０１８６】
例えば、推定装置２００は、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を期待値とした多変量正規分布において、心配粒子ａが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値になる確率を、式（９）により算出する。
【０１８７】
そして、推定装置２００は、算出した確率を、心配粒子ａの尤度とする。なお、図１９では、心配粒子ａが示す位置までの距離および前方車両Ｆの左端の座標値と右端の座標値が、測定された前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値と差が小さいため、心配粒子ａの尤度は高くなる。
【０１８８】
図１９の（Ｃ）において、車両Ｃは、粒子群ｐｓのリサンプリングにおいて、粒子群ｐｓの各粒子ｐより尤度の高い心配粒子群ａｓの各粒子ａを優先して、粒子群ｐｓの粒子ｐとして選択し直す。
【０１８９】
（５）車両Ｃは、リサンプリング結果に基づいて、現在時刻「ｔ」での前方車両Ｆの位置を位置Ｓ８であると推定する。そして、車両Ｃは、危険領域テーブル４００の近傍領域項目の内容から特定される危険領域Ｄの近傍領域の範囲に、前方車両Ｆの位置である位置Ｓ８が含まれていると判定して、前方車両Ｆが危険領域Ｄに進入することを回避するために、車両Ｃの速度を遅くする。
【０１９０】
このように、車両Ｃは、粒子フィルタを用いたシミュレーションによって推定された現在時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓが危険領域Ｄの近傍領域に存在しない場合であっても、前方車両Ｆの位置が危険領域Ｄの近傍領域にあることを推定できる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０１９１】
図１９では、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、車両Ｃの速度を遅くしたが、これに限らない。例えば、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、出力装置２０５により車両Ｃの運転者に警告を通知するようにしてもよい。
【０１９２】
また、図１９では、車両Ｃは、前方車両Ｆの位置を推定したが、これに限らない。例えば、車両Ｃは、後方車両の位置を推定してもよい。また、図１９では、車両Ｃは、１台の前方車両Ｆの位置を推定したが、これに限らない。例えば、車両Ｃは、複数の車線がある場合、それぞれの車線を走行中の前方車両Ｆの位置を推定してもよい。
【０１９３】
また、図１９では、危険領域Ｄは固定された領域であったが、これに限らない。例えば、危険領域Ｄは車両Ｃに対する前方車両Ｆの相対速度によって変化する可変の領域であってもよい。
【０１９４】
図２０は、可変の危険領域Ｄの例を示す説明図である。例えば、車両Ｃに対する前方車両Ｆの相対速度が正であるときは、車両Ｃと前方車両Ｆの車間距離は時間の経過により大きくなるので、今現在の車間距離がある程度小さくても衝突の可能性は低い。一方で、車両Ｃに対する前方車両Ｆの相対速度が負であり、相対速度の絶対値が大きいほど、時間の経過により車間距離はより短くなるので、衝突を回避するにはより大きな車間距離が必要となる。そのため、図２０に示すように、車両Ｃに対する前方車両Ｆの相対速度が負で絶対値が大きくなるにつれて、より車両Ｃから遠い領域が危険領域Ｄとして設定されるようにしてもよい。
【０１９５】
（実施例２の動作例１における推定装置２００の動作制御処理）
次に、図２１を用いて、実施例２の動作例１における推定装置２００の動作制御処理の処理内容について説明する。
【０１９６】
図２１は、実施例２の動作例１における推定装置２００の動作制御処理の処理内容を示すフローチャートである。まず、推定装置２００は、初期化処理を行う（ステップＳ２１０１）。なお、初期化処理では、推定装置２００は、物体テーブル３００から空間内の物体の情報を読み出し、危険領域テーブル４００から空間内の危険領域Ｄの情報を読み出し、空間内に均等に粒子ｐを生成する。
【０１９７】
次に、推定装置２００は、粒子フィルタを用いたシミュレーションにより、前方車両Ｆの直前の状態を示す粒子群ｐｓから、前方車両Ｆの現在の状態を示す粒子群ｐｓを推定する（ステップＳ２１０２）。なお、ステップＳ２１０２の処理は、図１９の（Ａ）に示した推定装置２００の処理に対応する。
【０１９８】
そして、推定装置２００は、センサ２０４により、自装置から前方車両Ｆまでの距離を測定する（ステップＳ２１０３）。次に、推定装置２００は、心配粒子生成処理を実行する（ステップＳ２１０４）。
【０１９９】
心配粒子生成処理において、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する。
【０２００】
そして、推定装置２００は、ステップＳ２１０２で推定された粒子群ｐｓとステップＳ２１０４で生成された心配粒子群ａｓとの各粒子の尤度を算出する（ステップＳ２１０５）。なお、ステップＳ２１０３〜ステップＳ２１０５の処理は、図１９の（Ｂ）に示した推定装置２００の処理に対応する。
【０２０１】
次に、推定装置２００は、ステップＳ２１０５で算出した各粒子の尤度から、ステップＳ２１０２で生成された粒子群ｐｓのリサンプリングを行い、前方車両Ｆの現在の状態を特定する（ステップＳ２１０６）。そして、推定装置２００は、特定した前方車両Ｆの現在の状態に応じて動作を決定する（ステップＳ２１０７）。なお、ステップＳ２１０６およびステップＳ２１０７の処理は、図１９の（Ｃ）に示した推定装置２００の処理に対応する。
【０２０２】
次に、推定装置２００は、動作終了か否かを判定する（ステップＳ２１０８）。ここで、動作終了していない場合（ステップＳ２１０８：Ｎｏ）、推定装置２００は、ステップＳ２１０２に戻る。
【０２０３】
一方、動作終了している場合（ステップＳ２１０８：Ｙｅｓ）、推定装置２００は、動作制御処理を終了する。これにより、推定装置２００は、前方車両Ｆの状態を推定し、前方車両Ｆが危険領域Ｄに進入することを回避しつつ移動を行うことができる。
【０２０４】
（実施例２にかかる推定装置２００の動作例２）
次に、図２２を用いて、実施例２にかかる推定装置２００の動作例２について説明する。
【０２０５】
図２２は、実施例２にかかる推定装置２００の動作例２を示す説明図である。動作例１では車両Ｃの推定した前方車両Ｆの位置の精度に関わらず心配粒子ａを生成していたが、動作例２では、車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しない。また、動作例２では、車両Ｃにより推定された前方車両Ｆの位置の精度が低いときには心配粒子ａを生成する。
【０２０６】
これにより、車両Ｃは、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。一方、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２０７】
図２２の（Ａ）に示すように、車両Ｃにより時刻「ｔ−１」で推定された粒子群ｐｓの尤度の総和がしきい値以上であるため、時刻「ｔ−１」で車両Ｃにより推定された前方車両Ｆの位置は精度が高いものとする。この場合、車両Ｃは、危険領域Ｄの近傍領域に心配粒子ａを生成せずとも、次の時刻「ｔ」の前方車両Ｆの位置を推定できる。
【０２０８】
図２２の（Ｂ）に示すように、（１）車両Ｃは、次の時刻「ｔ」になると、（Ａ）に示した時刻「ｔ−１」で推定された粒子群ｐｓから、時刻「ｔ」での前方車両Ｆの位置を示す粒子群ｐｓを推定する。（２）ここで、車両Ｃは、心配粒子ａを生成しない。
【０２０９】
（３）次に、車両Ｃは、センサ２０４により、真の前方車両Ｆの位置までの距離および前方車両Ｆの左端の座標値と右端の座標値を測定する。（４）そして、車両Ｃは、測定した前方車両Ｆまでの距離および前方車両Ｆの左端の座標値と右端の座標値に基づいて、粒子群ｐｓのリサンプリングを行う。
【０２１０】
図２２の（Ｃ）に示すように、（Ｂ）でリサンプリングされた粒子群ｐｓの示す前方車両Ｆの位置は、真の前方車両Ｆの位置と同じ位置になっている。このように、尤度の総和がしきい値以上であった粒子群ｐｓに基づいて推定された現在の前方車両Ｆの位置を示す粒子群ｐｓは精度が高いため、車両Ｃは、正しく前方車両Ｆの位置を推定することができている。（５）そして、車両Ｃは、危険領域テーブル４００を参照して、前方車両Ｆの位置が危険領域Ｄの近傍領域ではないと判定して、通常動作を行う。
【０２１１】
このように、車両Ｃは、車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しないことで、処理量を低減することができる。また、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周辺）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２１２】
（実施例２の動作例２における推定装置２００の動作制御処理）
次に、実施例２の動作例２における推定装置２００の動作制御処理の処理内容について説明する。
【０２１３】
動作例２における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２１４】
（実施例２の動作例２におけるステップＳ２１０４の心配粒子生成処理）
次に、図２３を用いて、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２１５】
図２３は、実施例２の動作例２における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、現在の前方車両Ｆの位置の精度が高いか否かを判定する（ステップＳ２３０１）。具体的には、推定装置２００は、前回の前方車両Ｆの位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の前方車両Ｆの位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ２３０１：Ｙｅｓ）、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２１６】
一方、精度が低い場合（ステップＳ２３０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２３０２）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２１７】
これにより、推定装置２００は、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２１８】
（実施例２にかかる推定装置２００の動作例３）
次に、図２４を用いて、実施例２にかかる推定装置２００の動作例３について説明する。
【０２１９】
図２４は、実施例２にかかる推定装置２００の動作例３を示す説明図である。動作例１では前方車両Ｆが危険領域Ｄに進入する可能性があるか否かに関わらず心配粒子ａを生成していたが、動作例３では前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０２２０】
これにより、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周囲）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２２１】
図２４に示すように、車両Ｃは、推定された粒子群ｐｓの尤度の総和がしきい値以上であり、推定された前方車両Ｆの位置の精度が高かった時刻「ｔ−６」がある場合、時刻「ｔ−６」で推定された前方車両Ｆの位置から現在時刻「ｔ」までに移動可能な範囲を算出する。
【０２２２】
具体的には、車両Ｃは、時刻「ｔ−６」から現在時刻「ｔ」までの各時刻での車両Ｃに対する前方車両Ｆの相対速度から、前方車両Ｆの時刻「ｔ−６」から現在時刻「ｔ」まででの最大移動距離を算出する。そして、車両Ｃは、時刻「ｔ−６」で推定された前方車両Ｆの位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
【０２２３】
この場合、前方車両Ｆは、算出された移動可能な範囲に重複する危険領域Ｄには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Ｄには進入する可能性がない。
【０２２４】
（１）車両Ｃは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０２２５】
（２）また、車両Ｃは、算出した移動可能な範囲と重複している危険領域Ｄの近傍領域には、心配粒子ａを生成する。
【０２２６】
（３）一方、車両Ｃは、算出した移動可能な範囲と重複していない危険領域Ｄの近傍領域には、心配粒子ａを生成しない。
【０２２７】
これにより、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄ（車両Ｃの周囲）の近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２２８】
（実施例２の動作例３における推定装置２００の動作制御処理）
次に、実施例２の動作例３における推定装置２００の動作制御処理の処理内容について説明する。
【０２２９】
動作例３における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２３０】
（実施例２の動作例３におけるステップＳ２１０４の心配粒子生成処理）
次に、図２５を用いて、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２３１】
図２５は、実施例２の動作例３における心配粒子生成処理（ステップＳ２１０４）を示すフローチャートである。心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ２５０１）。
【０２３２】
ここで、粒子ｐがなかった場合（ステップＳ２５０１：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２５０２）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２３３】
一方、粒子ｐがあった場合（ステップＳ２５０１：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ２５０３）。次に、推定装置２００は、特定した時刻から現在時刻までの前方車両Ｆの状態から、現在の前方車両Ｆの移動可能範囲を算出する（ステップＳ２５０４）。
【０２３４】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２５０５）。次に、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２３５】
これにより、推定装置２００は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２３６】
（実施例２にかかる推定装置２００の動作例４）
次に、実施例２にかかる推定装置２００の動作例４について説明する。動作例４は、動作例２および動作例３を組み合わせた場合の動作である。動作例４では車両Ｃにより推定された前方車両Ｆの位置の精度が高いときには心配粒子ａを生成しない。また、動作例４では前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域には心配粒子ａを生成しない。
【０２３７】
これにより、車両Ｃは、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２３８】
また、車両Ｃは、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、車両Ｃは、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、車両Ｃは、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２３９】
（実施例２の動作例４における推定装置２００の動作制御処理）
次に、実施例２の動作例４における推定装置２００の動作制御処理の処理内容について説明する。
【０２４０】
動作例４における推定装置２００の動作制御処理の処理内容は、ステップＳ２１０４の心配粒子生成処理を除き、図２１に示した動作例１における推定装置２００の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
【０２４１】
（実施例２の動作例４におけるステップＳ２１０４の心配粒子生成処理）
次に、図２６を用いて、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）について説明する。
【０２４２】
図２６は、実施例２の動作例４における心配粒子生成処理（ステップＳ２１０４）の処理内容を示すフローチャートである。動作例４における心配粒子生成処理（ステップＳ２１０４）において、推定装置２００は、現在の前方車両Ｆの位置の精度が高いか否かを判定する（ステップＳ２６０１）。具体的には、推定装置２００は、前回の前方車両Ｆの位置を示す粒子群ｐｓの総尤度がしきい値以上であった場合に、現在の前方車両Ｆの位置の精度が高いと判定する。ここで、精度が高い場合（ステップＳ２６０１：Ｙｅｓ）、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４３】
一方、精度が低い場合（ステップＳ２６０１：Ｎｏ）、推定装置２００は、一定時間以内に尤度の高い粒子ｐがあったか否かを判定する（ステップＳ２６０２）。ここで、粒子ｐがなかった場合（ステップＳ２６０２：Ｎｏ）、推定装置２００は、危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２６０３）。そして、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４４】
一方、粒子ｐがあった場合（ステップＳ２６０２：Ｙｅｓ）、推定装置２００は、最も尤度が高い粒子ｐがあった時刻を特定する（ステップＳ２６０４）。次に、推定装置２００は、特定した時刻から現在時刻までの前方車両Ｆの状態から、現在の前方車両Ｆの移動可能範囲を算出する（ステップＳ２６０５）。
【０２４５】
そして、推定装置２００は、算出した移動可能範囲と危険領域Ｄの近傍領域とが重複している危険領域テーブル４００のレコードの心配粒子ＩＤリスト項目の内容から心配粒子ＩＤを特定して、心配粒子テーブル６００に基づいて、特定した心配粒子ＩＤの心配粒子ａを生成する（ステップＳ２６０６）。次に、推定装置２００は、図２１のステップＳ２１０５に移行する。
【０２４６】
これにより、推定装置２００は、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、現在の前方車両Ｆの位置の精度が低い場合には心配粒子ａを生成することで、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２４７】
また、推定装置２００は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域に心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００は、前方車両Ｆが進入する可能性がある危険領域Ｄの近傍領域に心配粒子ａを生成することにより、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。結果として、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば、前方車両Ｆが危険領域Ｄから離れるように移動することで、前方車両Ｆが危険領域Ｄに進入することを回避できる。
【０２４８】
以上説明したように、実施例２にかかる推定装置２００によれば、前方車両Ｆの状態を示す粒子群ｐｓに加えて、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子群ａｓが生成される。次に、推定装置２００によれば、粒子群ｐｓと心配粒子群ａｓとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置２００によれば、リサンプリングにより選択し直された粒子群ｐｓから、前方車両Ｆの状態が推定し直される。
【０２４９】
このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５０】
また、推定装置２００によれば、現在の前方車両Ｆの位置の精度が高い場合には、心配粒子ａを生成せずに処理量を低減することができる。また、推定装置２００によれば、現在の前方車両Ｆの位置の精度が低い場合には、心配粒子ａが生成される。このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、リサンプリングにより選択し直された粒子群ｐｓによる前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５１】
また、推定装置２００によれば、前方車両Ｆが移動可能な範囲と重複しない危険領域Ｄの近傍領域は、前方車両Ｆが進入する可能性がない危険領域Ｄの近傍領域であるため、心配粒子ａを生成しないことで、処理量を低減することができる。また、推定装置２００によれば、前方車両Ｆが移動可能な範囲と重複する危険領域Ｄの近傍領域は、前方車両Ｆが進入する可能性があるため、心配粒子ａが生成される。このとき、真の前方車両Ｆの位置が危険領域Ｄの近傍領域にあれば心配粒子ａの尤度が高くなり、リサンプリングによって、危険領域Ｄの近傍領域での前方車両Ｆの状態を示す心配粒子ａが、粒子群ｐｓの粒子ｐとして選択される。そのため、推定装置２００は、リサンプリングにより選択し直された粒子群ｐｓから、前方車両Ｆの位置が進入する可能性がある危険領域Ｄの近傍領域にある場合、前方車両Ｆの位置の推定精度の向上を図ることができる。
【０２５２】
また、推定装置２００によれば、推定された前方車両Ｆの位置が危険領域Ｄの近傍領域にある場合、前方車両Ｆが危険領域Ｄから離れるようにするため、前方車両Ｆが危険領域Ｄに進入することを回避することができる。
【０２５３】
なお、本実施の形態で説明した推定方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。本推定プログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また本推定プログラムは、インターネット等のネットワークを介して配布してもよい。
【０２５４】
上述した実施の形態に関し、さらに以下の付記を開示する。
【０２５５】
（付記１）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新する更新手段と、
前記更新手段によって更新される都度、前記移動体と前記空間内の物体との距離を測定する測定手段と、
前記更新手段によって更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成する生成手段と、
前記測定手段によって測定された距離と、前記更新手段による更新されたときの第１の粒子群と、前記生成手段によって生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出する算出手段と、
前記算出手段によって算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する特定手段と、
を備えることを特徴とする推定装置。
【０２５６】
（付記２）前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段を備え、
前記生成手段は、
前記判断手段によって前記しきい値以上であると判断された場合には、前記第２の粒子群の生成を停止することを特徴とする付記１に記載の推定装置。
【０２５７】
（付記３）前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段と、
前記判断手段によって前記しきい値以上であると判断された尤度が得られたときの前記移動体の前記空間内の位置に基づいて、前記移動体の移動範囲を設定する設定手段と、を備え、
前記生成手段は、
前記近傍領域のうち、前記設定手段によって設定された移動範囲と重複する領域に前記第２の粒子群を生成することを特徴とする付記１または２に記載の推定装置。
【０２５８】
（付記４）前記特定手段によって特定された前記移動体の位置が前記特定の領域であるか否かを判定する判定手段と、
前記判定手段によって前記特定の領域に位置すると判定された場合、前記特定の領域から離れる方向へ前記移動体が移動するように制御する制御手段と、
を備えることを特徴とする付記１〜３のいずれか一つに記載の推定装置。
【０２５９】
（付記５）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータが実行することを特徴とする推定方法。
【０２６０】
（付記６）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータが実行することを特徴とする推定方法。
【０２６１】
（付記７）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータに実行させることを特徴とする推定プログラム。
【０２６２】
（付記８）粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータに実行させることを特徴とする推定プログラム。
【符号の説明】
【０２６３】
２００ 推定装置
Ｒ ロボット
Ｃ 車両
９０１ 更新部
９０２ 測定部
９０３ 生成部
９０４ 算出部
９０５ 判断部
９０６ 設定部
９０７ 特定部
９０８ 判定部
９０９ 制御部
【特許請求の範囲】
【請求項１】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新する更新手段と、
前記更新手段によって更新される都度、前記移動体と前記空間内の物体との距離を測定する測定手段と、
前記更新手段によって更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成する生成手段と、
前記測定手段によって測定された距離と、前記更新手段により更新されたときの第１の粒子群と、前記生成手段によって生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出する算出手段と、
前記算出手段によって算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する特定手段と、
を備えることを特徴とする推定装置。
【請求項２】
前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段を備え、
前記生成手段は、
前記判断手段によって前記しきい値以上であると判断された場合には、前記第２の粒子群の生成を停止することを特徴とする請求項１に記載の推定装置。
【請求項３】
前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段と、
前記判断手段によって前記しきい値以上であると判断された尤度が得られたときの前記移動体の前記空間内の位置に基づいて、前記移動体の移動範囲を設定する設定手段と、を備え、
前記生成手段は、
前記特定の領域のうち、前記設定手段によって設定された移動範囲と重複する領域に前記第２の粒子群を生成することを特徴とする請求項１または２に記載の推定装置。
【請求項４】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータが実行することを特徴とする推定方法。
【請求項５】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータが実行することを特徴とする推定方法。
【請求項６】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータに実行させることを特徴とする推定プログラム。
【請求項７】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータに実行させることを特徴とする推定プログラム。
【請求項１】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新する更新手段と、
前記更新手段によって更新される都度、前記移動体と前記空間内の物体との距離を測定する測定手段と、
前記更新手段によって更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成する生成手段と、
前記測定手段によって測定された距離と、前記更新手段により更新されたときの第１の粒子群と、前記生成手段によって生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出する算出手段と、
前記算出手段によって算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する特定手段と、
を備えることを特徴とする推定装置。
【請求項２】
前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段を備え、
前記生成手段は、
前記判断手段によって前記しきい値以上であると判断された場合には、前記第２の粒子群の生成を停止することを特徴とする請求項１に記載の推定装置。
【請求項３】
前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段と、
前記判断手段によって前記しきい値以上であると判断された尤度が得られたときの前記移動体の前記空間内の位置に基づいて、前記移動体の移動範囲を設定する設定手段と、を備え、
前記生成手段は、
前記特定の領域のうち、前記設定手段によって設定された移動範囲と重複する領域に前記第２の粒子群を生成することを特徴とする請求項１または２に記載の推定装置。
【請求項４】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータが実行することを特徴とする推定方法。
【請求項５】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータが実行することを特徴とする推定方法。
【請求項６】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータに実行させることを特徴とする推定プログラム。
【請求項７】
粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第１の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第２の粒子群を生成し、
測定された距離と、更新されたときの第１の粒子群と、生成された第２の粒子群と、に基づいて、前記第１および第２の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータに実行させることを特徴とする推定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【公開番号】特開２０１３−１０１５７３（Ｐ２０１３−１０１５７３Ａ）
【公開日】平成２５年５月２３日（２０１３．５．２３）
【国際特許分類】
【出願番号】特願２０１１−２４５９８０（Ｐ２０１１−２４５９８０）
【出願日】平成２３年１１月９日（２０１１．１１．９）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２５年５月２３日（２０１３．５．２３）
【国際特許分類】
【出願日】平成２３年１１月９日（２０１１．１１．９）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]