以下に添付図面を参照して、この発明にかかる推定装置、推定方法、および推定プログラムの実施の形態を詳細に説明する。
(実施の形態にかかる推定装置による移動体の位置推定の内容)
まず、本実施の形態にかかる推定装置による移動体の位置推定の内容について説明する。
図1は、実施の形態にかかる推定装置による移動体の位置推定の内容を示す説明図である。図1の例は、移動体が推定装置自体である場合での移動体の位置推定の例である。具体的には、推定装置として、自律移動ロボットR(以下では、「ロボットR」という)を採用した場合について説明する。
ロボットRは、ロボットRの移動する領域内に存在する危険領域Dおよび障害物Oの位置情報を有する。また、ロボットRは、ロボットRの移動する領域内でのロボットRの或る状態を示す粒子pの集合である粒子群psを用いたシミュレーションにより、ロボットRの状態を推定する。そして、ロボットRは、危険領域Dおよび障害物Oの位置情報と、推定された状態と、に基づいて、ロボットRの移動する領域内で危険領域Dを回避しつつ移動を行う。ロボットRの状態とは、例えば、ロボットRの位置、速度、および進行方向である。
図1の(A)において、(1)ロボットRは、粒子フィルタを用いたシミュレーションにより、過去の時刻「t−1」でのロボットRの状態を示す粒子群psから現在時刻「t」でのロボットRの状態を示す粒子群psを推定する。そして、ロボットRは、推定された現在時刻「t」でのロボットRの状態を示す粒子群psにより、現在時刻「t」での自己位置を位置S1であると推定する。
しかしながら、ロボットRの真の自己位置は危険領域Dの近傍領域にある位置S2であり、ロボットRは自己位置を誤って推定している。なお、ロボットRが自己位置を誤って推定する原因としては、例えば、人間によってロボットRが強制移動させられること(いわゆる誘拐ロボット問題)が挙げられる。また、ロボットRが自己位置を誤って推定する原因としては、例えば、ロボットRが移動に用いる車輪の空回りやロボットRが障害物に衝突したときの姿勢の変化が挙げられる。
また、ロボットRが自己位置を誤って推定する原因としては、例えば、障害物Oの位置情報と実際の障害物Oの位置との誤差や、シミュレーションによる計算誤差が、シミュレーションごとに蓄積していくことが挙げられる。また、ロボットRが自己位置を誤って推定する原因としては、例えば、粒子群psの粒子pの個数が少ないため、移動体の状態の確率分布の近似が不十分であることが挙げられる。
図1の(B)において、(2)ロボットRは、現在時刻「t」でのロボットRの状態を示す粒子群psに加えて、心配粒子群asを生成する。心配粒子群asとは、危険領域Dの位置情報に基づいて危険領域Dの近傍領域でのロボットRの或る状態を示す心配粒子aの集合である。
(3)次に、ロボットRは、距離センサを用いて、ロボットRの位置から障害物Oまでの距離を測定する。
(4)そして、ロボットRは、障害物Oの位置と、粒子群psと心配粒子群asとの各粒子が示すロボットRの位置と、に基づいて、ロボットRから障害物Oまでの距離を算出する。ロボットRは、測定したロボットRの位置から障害物Oまでの距離、および算出した各粒子が示すロボットRの位置から障害物Oまでの距離に基づいて、現在時刻「t」でのロボットRの状態を示す粒子群psのリサンプリングを行う。
リサンプリングとは、粒子群psと心配粒子群asとの各粒子の中から、粒子が示す状態の尤度が高い粒子を優先して粒子群psの粒子pとして選択し、粒子群psを選択し直すことである。このとき、同一の粒子を複数回選択してもよい。そして、選択し直された粒子群psの示す状態が、現在時刻「t」でのロボットRの状態として推定し直される。
ここでは、粒子群psの各粒子pが示すロボットRの位置から障害物Oまでの距離は、測定したロボットRの位置から障害物Oまでの距離より短くなっている。例えば、粒子群psの粒子p1が示すロボットRの位置から障害物Oまでの距離dpは、ロボットRの位置から障害物Oまでの測定距離drより短くなっている。このため、ロボットRは、「粒子群psの各粒子pの示すロボットRの位置が実際のロボットRの位置とはかけ離れている」として、尤度が低いと判定する。
一方、(B)に示した心配粒子群asの各粒子aの位置から障害物Oまでの距離は、測定したロボットRの位置から障害物Oまでの距離とほぼ同一である。例えば、心配粒子群asの粒子a1の位置から障害物Oまでの距離daは、ロボットRの位置から障害物Oまでの測定距離drとほぼ同一である。このため、ロボットRは、「心配粒子群asの各粒子aの示すロボットRの位置が実際のロボットRの位置に近い」として、尤度が高いと判定する。
そして、ロボットRは、粒子群psの各粒子pより尤度が高い心配粒子群asの各粒子aを優先して、粒子群psの粒子pとして選択し直す。そして、ロボットRは、選択し直された粒子群psにより、現在時刻「t」でのロボットRの状態を推定し直す。
図1の(C)において、(5)ロボットRは、リサンプリング結果から推定し直された現在時刻「t」でのロボットRの状態から、現在時刻「t」での自己位置を位置S2であると推定する。そして、ロボットRは、危険領域Dの位置情報に基づいて、推定された自己位置である位置S2が危険領域Dの近傍領域に含まれると判定して、危険領域Dから離れるように移動することで、危険領域Dへの進入を回避する。
このように、ロボットRは、真の自己位置が危険領域Dの近傍領域にある場合、尤度の高い心配粒子aが粒子群psの粒子pとして選択されるため、自己位置の推定精度の向上を図ることができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(推定装置のハードウェア構成例)
次に、図2を用いて、推定装置のハードウェア構成例について説明する。
図2は、実施の形態にかかる推定装置のハードウェア構成例を示すブロック図である。図2において、推定装置200は、CPU(Central Processing Unit)201と、ROM(Read‐Only Memory)202と、RAM(Random Access Memory)203と、センサ204と、出力装置205と、駆動装置206と、を備えている。また、各構成部はバス210によってそれぞれ接続されている。
ここで、CPU201は、推定装置200の全体の制御を司る。ROM202は、ブートプログラムなどのプログラムを記憶している。RAM203は、CPU201のワークエリアとして使用される。
センサ204は、電磁波の送受信を行い、受信した電磁波を電気信号に変換し、RAM203に保持する。例えば、センサ204として、赤外線受光素子などの距離センサを採用できる。また、センサ204は、光学的な映像を電気信号に変換し、RAM203に保持する。例えば、CCD(Charge Coupled Device)、CMOS(Complementary Metal−Oxide Semiconductor)センサなどの光学的な映像を電気信号に変換する光電変換素子を採用できる。
出力装置205は、データの出力を行う。出力装置205としては、例えば、ディスプレイ、スピーカ等を用いることができる。駆動装置206は、CPU201の制御により推定装置200を駆動させる。駆動装置206としては、例えば、モータ、エンジン、ブレーキ等を用いることができる。推定装置200としては、例えば、自己位置を推定しながら移動する自律移動ロボットRや、周辺車両の位置を推定しながら移動する車両が挙げられる。
(物体テーブルの記憶内容)
次に、図3を用いて、推定装置200が有する物体テーブルの記憶内容の一例について説明する。物体テーブルとは、移動体が移動する領域内の物体の形状や位置を記憶するテーブルである。物体としては、例えば、図1に示した障害物Oや壁が挙げられる。物体テーブルは、例えば、図2に示したRAM203により実現される。
図3は、推定装置200が有する物体テーブルの記憶内容の一例を示す説明図である。図3に示すように、物体テーブル300は、物体ID項目のそれぞれに対応付けて、形状項目と、位置項目と、を有し、物体ごとにレコードを構成する。
物体ID項目には、移動体が移動する領域内の物体を特定する識別子が記憶されている。形状項目には、物体ID項目の内容により特定される物体の形状が記憶されている。具体的には、例えば、形状項目には、物体の形状の種別フラグと、当該種別の形状を特定する値と、が記憶される。より具体的には、例えば、形状項目には、物体の形状が直方体であることを示す種別フラグと、直方体の奥行きと、直方体の幅と、直方体の高さと、が記憶される。また、より具体的には、例えば、形状項目には、物体の形状が円柱であることを示す種別フラグと、円柱底面の中心の位置と、円柱底面の半径と、円柱の高さと、が記憶されてもよい。
位置項目には、物体ID項目の内容により特定される物体の位置が記憶されている。具体的には、例えば、位置項目には、物体の位置を示す座標値(例えば、3次元空間でのx座標値とy座標値とz座標値)が記憶される。また、位置項目には、物体の姿勢(例えば、3次元空間でのx軸に対する角度とy軸に対する角度とz軸に対する角度)が記憶されていてもよい。
(危険領域テーブルの記憶内容)
次に、図4を用いて、推定装置200が有する危険領域テーブルの記憶内容の一例について説明する。危険領域テーブルとは、移動体が進入すべきではないと設定された領域(危険領域D)の近傍領域を記憶するテーブルである。危険領域テーブルは、例えば、図2に示したRAM203により実現される。
図4は、推定装置200が有する危険領域テーブルの記憶内容の一例を示す説明図である。図4に示すように、危険領域テーブル400は、領域ID項目のそれぞれに対応付けて、近傍領域項目と、心配粒子IDリスト項目と、を有し、危険領域Dごとにレコードを構成する。
領域ID項目には、移動体が進入すべきではないと設定された領域(危険領域D)を特定する識別子が記憶されている。近傍領域項目には、領域ID項目の内容により特定される危険領域Dの近傍領域の範囲を特定する情報が記憶されている。具体的には、例えば、近傍領域項目には、近傍領域が矩形であれば、辺を共有しない2頂点の座標値が記憶される。心配粒子IDリスト項目には、移動体の或る状態を示す心配粒子aを特定する識別子が記憶されている。
(粒子テーブルの記憶内容)
次に、図5を用いて、推定装置200が有する粒子テーブルの記憶内容の一例について説明する。粒子テーブルとは、推定された移動体の位置を示す粒子pを記憶するテーブルである。粒子テーブルは、例えば、図2に示したRAM203により実現される。
図5は、推定装置200が有する粒子テーブルの記憶内容の一例を示す説明図である。図5に示すように、粒子テーブル500は、粒子ID項目のそれぞれに対応付けて、変数項目(図5では変数1項目〜変数k項目)を有し、粒子pごとにレコードを構成する。
粒子ID項目には、移動体が取り得る或る状態を示す粒子pを特定する識別子が記憶されている。変数項目には、粒子ID項目の内容により特定される粒子pが示す状態を特定する変数の値が記憶されている。変数項目は、図5に示すように複数あってもよいし、1つであってもよい。
例えば、変数項目には、粒子ID項目の内容により特定される粒子pの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、3次元空間でのx座標値とy座標値とz座標値が記憶される。
また、例えば、変数項目には、粒子ID項目の内容により特定される粒子pの速度が記憶されている。なお、速度の単位は、例えば、「m/s(meter/second)」である。また、例えば、変数項目には、粒子ID項目の内容により特定される粒子pの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を0°としたときの粒子pの進行方向の角度が記憶される。進行方向の単位は、例えば、「°」である。
(心配粒子テーブルの記憶内容)
次に、図6を用いて、推定装置200が有する心配粒子テーブルの記憶内容の一例について説明する。心配粒子テーブルとは、危険領域Dの近傍領域の状態を示す心配粒子aを記憶するテーブルである。心配粒子テーブルは、例えば、図2に示したRAM203により実現される。
図6は、推定装置200が有する心配粒子テーブルの記憶内容の一例を示す説明図である。図6に示すように、心配粒子テーブル600は、心配粒子ID項目のそれぞれに対応付けて、変数項目(図6では変数1項目〜変数k項目)を有し、心配粒子aごとにレコードを構成する。
心配粒子ID項目には、危険領域Dの近傍領域で移動体が取り得る或る状態を示す心配粒子aを特定する識別子が記憶されている。変数項目には、心配粒子ID項目の内容により特定される心配粒子aが示す状態を特定する変数の値が記憶されている。変数項目は、図6に示すように複数あってもよいし、1つであってもよい。
例えば、変数項目には、心配粒子ID項目の内容により特定される心配粒子aの位置を示す座標値が記憶されている。具体的には、例えば、変数項目には、3次元空間でのx座標値とy座標値とz座標値が記憶される。
また、例えば、変数項目には、心配粒子ID項目の内容により特定される心配粒子aの速度が記憶されている。なお、速度の単位は、例えば、「m/s(meter/second)」である。また、例えば、変数項目には、心配粒子ID項目の内容により特定される心配粒子aの進行方向が記憶されている。具体的には、例えば、変数項目には、移動体の移動する領域において、北を0°としたときの心配粒子aの進行方向の角度が記憶される。
(センサ情報テーブルの記憶内容)
次に、図7を用いて、推定装置200が有するセンサ情報テーブルの記憶内容の一例について説明する。センサ情報テーブルとは、推定装置200が有するセンサの測定値を記憶するテーブルである。センサ情報テーブルは、例えば、図2に示したRAM203により実現される。
図7は、推定装置200が有するセンサ情報テーブルの記憶内容の一例を示す説明図である。図7に示すように、センサ情報テーブル700は、時刻項目のそれぞれに対応付けて、測定値項目(図7では、測定値1項目〜測定値M項目)を有し、センサ204による測定を行った時刻ごとにレコードを構成する。
時刻項目には、センサ204による測定を行った時刻が記憶されている。測定値項目には、時刻項目の時刻で測定されたセンサ204の値が記憶されている。測定値項目は、図7に示すように複数あってもよいし、1つであってもよい。例えば、移動体の前方、後方、左方、および右方のそれぞれの方向に存在する障害物Oまでの距離を記憶する測定値項目があってもよい。
具体的には、例えば、測定値項目には、距離センサによって測定された移動体から障害物Oまでの距離が記憶される。測定値項目の単位は、例えば、「m(meter)」である。
(尤度テーブルの記憶内容)
次に、図8を用いて、推定装置200が有する尤度テーブルの記憶内容の一例について説明する。尤度テーブルとは、推定装置200により算出された粒子群psと心配粒子群asとの各粒子の尤度を記憶するテーブルである。尤度テーブルは、例えば、図2に示したRAM203により実現される。
図8は、推定装置200が有する尤度テーブルの記憶内容の一例を示す説明図である。図8に示すように、尤度テーブル800は、粒子ID項目のそれぞれに対応付けて、尤度項目を有し、粒子pごとにレコードを構成する。また、尤度テーブル800は、移動体の状態を推定するごとにレコードを更新する。
粒子ID項目には、粒子群psと心配粒子群asとの各粒子を特定する識別子が記憶されている。尤度項目には、粒子ID項目の内容から特定される粒子が示す移動体の状態の尤もらしさを示す尤度が記憶されている。
(推定装置200の機能的構成例)
次に、図9を用いて、推定装置200の機能的構成例について説明する。
図9は、推定装置200の機能的構成を示すブロック図である。推定装置200は、更新部901と、生成部903と、測定部902と、算出部904と、判断部905と、設定部906と、特定部907と、判定部908と、制御部909と、を含む構成である。
更新部901は、粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、移動体の状態を示す第1の粒子群を現状態から次状態に更新する。ここで、空間とは、移動体の移動する領域である。例えば、空間とは、移動体がロボットRであれば、ロボットRの移動する部屋や建物であり、移動体が車両であれば、道路である。状態方程式とは、過去の移動体の状態を入力すると移動体の現在の状態を決定する方程式である。
例えば、状態方程式とは、運動方程式であって、時刻「t−1」での移動体の位置と速度を入力すると、時刻「t」での移動体の位置と速度を決定する方程式である。また、状態方程式とは、後述する式(2)や式(5)である。移動体の状態を示す第1の粒子群とは、移動体の状態の確率分布を近似する粒子群psである。粒子群psの1粒子は、移動体の取り得る或る状態を示す。
具体的には、例えば、更新部901は、粒子テーブル500に記憶されている直前の時刻での移動体の状態を示す粒子群psの各粒子pの状態を状態方程式に入力して、現在時刻での各粒子pの状態を算出する。更新部901は、算出された現在時刻での各粒子pの状態により、粒子テーブル500を更新する。
これにより、更新部901は、移動体の過去の状態から移動体の現在の状態を推定することができる。なお、更新部901は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
測定部902は、更新部901によって更新される都度、移動体と空間内の物体との距離を測定する。ここで、移動体と空間内の物体との距離とは、例えば、移動体が推定装置200自体である場合には、推定装置200から空間内の物体(障害物Oや壁)までの距離である。また、移動体と空間内の物体との距離とは、例えば、移動体と推定装置200とが異なる場合は、移動体から空間内の物体(推定装置200自体)までの距離である。具体的には、例えば、測定部902は、更新部901によって更新される都度、センサ204により、移動体と空間内の物体との距離を測定する。
これにより、測定部902は、算出部904により各粒子の尤度の算出に使用される移動体から空間内の物体までの距離を得ることができる。なお、測定部902は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、または、センサ204により、その機能を実現する。
生成部903は、更新部901によって更新される都度、空間内の特定の領域の状態を示す第2の粒子群を生成する。ここで、特定の領域とは、予め設定された移動体が進入すべきではない禁止領域の近傍領域である。禁止領域とは、例えば上述した危険領域Dである。また、近傍領域とは、禁止領域の周囲の領域である。禁止領域や近傍領域の範囲は、任意に設定される。第2の粒子群とは、危険領域Dの近傍領域での移動体の状態を示す粒子群であり、例えば、上述した心配粒子群asである。具体的には、例えば、生成部903は、心配粒子テーブル600から心配粒子aが示す移動体の状態(例えば、移動体の位置、速度、および進行方向)を読み出し、心配粒子群asを生成する。
これにより、生成部903は、危険領域Dの近傍領域に心配粒子aを生成して、特定部907による移動体の位置の特定において、移動体の真の位置が危険領域Dの近傍領域にあれば、移動体の位置を危険領域Dの近傍領域にあると特定できるようにすることができる。生成部903は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
算出部904は、測定部902によって測定された距離と、更新部901による更新されたときの第1の粒子群と、生成部903によって生成された第2の粒子群と、に基づいて、第1および第2の粒子群の各粒子の尤度を算出する。ここで、尤度とは、粒子が示す移動体の状態の尤もらしさを示す値である。尤度としては、例えば、測定により得られた実際の移動体の状態での測定値を期待値とした正規分布において、各粒子の示す移動体の状態から予測される測定値となる確率を採用することができる。
具体的には、例えば、算出部904は、更新部901によって更新されたときの粒子群psの各粒子pが示す移動体の状態から、粒子群psの各粒子pが示す移動体の位置から空間内の物体までの距離を算出する。次に、算出部904は、生成部903によって生成された心配粒子群asの各心配粒子aが示す移動体の状態から、心配粒子群asの各粒子aが示す移動体の位置から空間内の物体までの距離を算出する。
そして、算出部904は、測定部902によって測定された移動体から空間内の物体までの距離を期待値とする正規分布において、粒子群psと心配粒子群asとの各粒子が示す移動体の位置から空間内の物体までの距離となる確率を算出する。算出部904は、算出された確率を各粒子の尤度として、尤度テーブル800を更新する。
ここで、算出部904は、測定部902によって距離を測定された物体が複数ある場合は、粒子群psと心配粒子群asとの各粒子が示す移動体の位置からそれぞれの物体までの距離となる確率を算出する。そして、算出部904は、物体ごとに算出された確率の総乗を各粒子の尤度として、尤度テーブル800を更新する。
これにより、算出部904は、粒子群psの各粒子pの尤度と心配粒子群asの各粒子aの尤度を算出することができる。なお、算出部904は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
判断部905は、算出部904によって前回算出された尤度がしきい値以上であるか否かを判断する。ここで、しきい値は、推定装置200の利用者によって予め設定された値である。上述した正規分布における確率を尤度に採用した場合、しきい値としては、例えば、「期待値±標準偏差の値が測定値となる確率」を採用することができる。具体的には、例えば、判断部905は、算出部904によって前回算出された粒子群psの各粒子pの尤度がしきい値以上であるか否かを判断し、尤度がしきい値以上の粒子pがあれば、時刻と尤度がしきい値以上の粒子pの状態とをRAM203などの記憶領域に記憶する。
また、判断部905は、算出部904によって前回算出された粒子群psの各粒子pの尤度の総和がしきい値以上であるか否かを判断し、尤度の総和がしきい値以上であれば、時刻と各粒子pの状態とをRAM203などの記憶領域に記憶してもよい。これにより、判断部905は、尤度がしきい値以上であり、推定の精度が高いと解される移動体の状態を記憶しておくことができる。
この場合、生成部903は、判断部905によって前回算出された尤度がしきい値以上であると判断された場合には、第2の粒子群の生成を停止する。具体的には、例えば、生成部903は、判断部905によって前回の移動体の状態を示す粒子群psのいずれかの粒子pの尤度がしきい値以上であると判断された場合には、心配粒子テーブル600から心配粒子aが示す移動体の状態(例えば、移動体の位置、速度、および進行方向)を読み出さない。前回算出された粒子群psの各粒子pの尤度の総和の場合も同様である。
これにより、生成部903は、移動体の状態の推定の精度が高い場合は、心配粒子aを生成せずに、処理を低減することができる。また、生成部903は、移動体の状態の推定の精度が低い場合には、心配粒子aを生成することで、移動体の真の位置が危険領域Dの近傍領域にあるときは移動体の位置を危険領域Dの近傍領域にあると特定部907が特定できるようにすることができる。
なお、判断部905は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
設定部906は、判断部905によってしきい値以上であると判断された尤度が得られたときの移動体の空間内の位置に基づいて、移動体の移動範囲を設定する。具体的には、例えば、設定部906は、判断部905によってしきい値以上であると判断された尤度が得られたときの移動体の推定位置と、現在時刻までの移動体が移動した速度と、に基づいて、現在時刻までに移動体が移動可能な範囲を算出する。そして、設定部906は、算出した移動体が移動可能な範囲をRAM203などの記憶領域に記憶する。これにより、設定部906は、移動体が移動しうる範囲を設定することができる。
この場合、生成部903は、近傍領域のうち、設定部906によって設定された移動範囲と重複する領域に第2の粒子群を生成する。生成部903は、危険領域テーブル400の中で、近傍領域項目の情報から特定される近傍領域の範囲と、設定部906によって設定された移動体の移動範囲と、が重複するレコードを特定する。そして、生成部903は、特定したレコードの心配粒子IDリストを参照して、心配粒子テーブル600から心配粒子aが示す移動体の状態(例えば、移動体の位置、速度、および進行方向)を読み出すことになる。
これにより、生成部903は、移動体の移動範囲と重複しない近傍領域には心配粒子aを生成せずに、処理を低減することができる。また、生成部903は、移動体の移動範囲と重複する近傍領域には、心配粒子aを生成することで、特定部907による移動体の位置の特定において、移動体の位置を危険領域Dの近傍領域にあると特定できるようにすることができる。
なお、設定部906は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
特定部907は、算出部904によって算出された各粒子の尤度に基づいて、空間内での移動体の位置を特定する。具体的には、例えば、特定部907は、算出部904によって算出された各粒子の尤度に比例した確率に従って、各粒子の中から粒子群psの粒子pを選択し直す。
より具体的には、例えば、特定部907は、[0,1]区間上で、粒子ごとに、当該粒子の尤度に比例する区間長になるように分割された区間を対応付ける。そして、特定部907は、[0,1]の一様乱数を発生させて、乱数の値が入る区間に対応する粒子を、粒子群psの粒子pとして選択し直す。ここで、粒子群psの粒子pとして、1粒子を複数回選択してもよい。そして、特定部907は、リサンプリングされた粒子群psが示す移動体の状態から、現在の移動体の位置を特定する。
これにより、特定部907は、移動体の位置を特定することができる。なお、特定部907は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
判定部908は、特定部907によって特定された移動体の位置が特定の領域にあるか否かを判定する。具体的には、例えば、判定部908は、特定部907によって特定された移動体の位置が、危険領域テーブル400の近傍領域項目の内容から特定される近傍領域の範囲に含まれているか否かを判定し、判定結果をRAM203などの記憶領域に記憶する。
これにより、判定部908は、移動体が危険領域Dの近傍領域にあるか否かを判定することができる。なお、判定部908は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
制御部909は、判定部908によって特定の領域に位置すると判定された場合、特定の領域から離れる方向へ移動体を移動するように制御する。具体的には、例えば、制御部909は、判定部908によって危険領域Dの近傍領域に移動体が位置すると判定された場合、危険領域テーブル400を参照して危険領域Dから離れる方向へ移動体を移動するように、駆動装置206を制御する。
より具体的には、例えば、制御部909は、危険領域テーブル400の近傍領域項目の内容から特定される近傍領域の範囲と、粒子群psが示す移動体の位置と進行方向とから、移動体の進行方向に危険領域Dがあるか否かを判定する。そして、制御部909は、移動体の進行方向に危険領域Dがあれば、移動体の進行方向が逆方向になるように、駆動装置206を制御する。
これにより、制御部909は、移動体の危険領域Dへの進入を低減することができる。なお、判断部905は、具体的には、例えば、図2に示したROM202、RAM203などの記憶装置に記憶されたプログラムをCPU201に実行させることにより、その機能を実現する。
(実施例1)
次に、実施例1について説明する。実施例1は、移動体が推定装置200自体である場合での移動体の位置推定の例である。具体的には、推定装置200(移動体)として、自律移動ロボットR(以下では、「ロボットR」という)を採用した場合について説明する。
(実施例1にかかる推定装置200の動作例1)
まず、図10〜図12を用いて、実施例1にかかる推定装置200の動作例1について説明する。なお、ロボットRは、物体テーブル300および危険領域テーブル400を有する。また、ロボットRは、ロボットRの移動する領域内でのロボットRの或る状態を示す粒子pの集合である粒子群psを用いたシミュレーションにより、ロボットRの状態を推定する。そして、ロボットRは、物体テーブル300および危険領域テーブル400と、推定された状態と、に基づいて、危険領域Dおよび障害物Oが存在する領域内での自己位置を推定し、危険領域Dを回避しつつ移動を行う。
図10〜図12は、実施例1にかかる推定装置200の動作例1を示す説明図である。図10の(A)において、ロボットRは、時刻「t−1」での移動体の状態xt-1を示す粒子群psから、時刻「t−1」での自己位置を位置S3であると推定している。なお、移動体の状態xtは、下記式(1)で表される。ここで、xは移動体の位置であり、v(x)は移動体の速度である。
図10の(B)において、ロボットRは、粒子フィルタを用いたシミュレーションにより、過去の時刻「t−1」での移動体の状態xt-1を示す粒子群psから、現在時刻「t」での移動体の状態xtを示す粒子群psを推定する。
(1)例えば、ロボットRは、粒子群psの粒子p1が示す状態xt-1 (1)を、下記式(2)の状態方程式に入力して現在の粒子の状態xt (1)を算出する。ここで、iは便宜上付与された粒子の番号である。ηt (i)はシステムノイズである。ΔTは、タイムステップであり、「(t)−(t−1)」である。at (i)は移動体の加速度である。
(2)次に、ロボットRは、粒子群psの他の粒子pについても、同様に現在の粒子pの状態xtを算出していく。これにより、ロボットRは、時刻「t」での移動体の状態xtを示す粒子群psを推定することができる。
次に、図11において、ロボットRは、図10で推定された現在時刻「t」での移動体の状態xtを示す粒子群psと、危険領域Dの近傍領域に生成した危険領域Dの近傍領域での移動体の状態xtを示す心配粒子群asと、の各粒子の尤度を算出する。図11の例では、1個の粒子p1と1個の心配粒子a1との各粒子の尤度の算出を例に挙げる。
(1)まず、推定装置200は、物体テーブル300を参照して、粒子p1と心配粒子a1との各粒子が示す位置からの空間内の物体までの距離を算出する。例えば、推定装置200は、空間内の物体までの距離として、粒子が示す位置からの前方の壁までの距離と右方の壁までの距離と左方の壁までの距離と後方の壁までの距離を算出する。
より具体的には、例えば、推定装置200は、心配粒子a1が示す位置からの前方の壁までの距離da_fと右方の壁までの距離da_rと左方の壁までの距離da_lと後方の壁までの距離da_bを算出する。また、推定装置200は、粒子p1が示す位置からの前方の壁までの距離dp_fと右方の壁までの距離dp_rと左方の壁までの距離dp_lと後方の壁までの距離dp_bを算出する。
(2)次に、推定装置200は、センサ204により、実際の自己位置からの物体までの距離を測定する。例えば、推定装置200は、4つのセンサ204により、推定装置200の前方の壁までの距離dr_fと右方の壁までの距離dr_rと左方の壁までの距離dr_lと後方との壁までの距離dr_bを算出する。なお、ここでは、推定装置200は、4つのセンサ204により各壁までの距離を測定したが、これに限らない。例えば、推定装置200は、1つのセンサ204の測定方向を変化させて、各壁までの距離を測定してもよい。
(3)そして、推定装置200は、粒子p1が示す位置からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ(尤度)を算出する。例えば、推定装置200は、測定された各壁までの距離を期待値とした正規分布において、粒子p1が示す位置からの各壁までの距離になる確率の総乗p(yt|xt|t-1 (i))を、下記式(3)により算出する。ここで、σjは標準偏差である。yjtはセンサ204により測定された壁までの距離である。yjt (i)は算出された壁までの距離である。iは粒子pの番号である。jはセンサ204の番号である。
そして、推定装置200は、算出した確率の総乗を、粒子p1の尤度とする。なお、図11では、粒子p1が示す位置からの各壁までの距離が真の自己位置から各壁までの距離と差が大きいため、粒子p1の尤度は低くなる。
(4)推定装置200は、上記(3)と同様に心配粒子a1からの各壁までの距離を、真の自己位置からの各壁までの距離と比較したときの尤もらしさ(尤度)を算出する。例えば、推定装置200は、測定された各壁までの距離を期待値とした正規分布において、心配粒子a1からの各壁までの距離になる確率の総乗を、式(3)により算出する。そして、推定装置200は、算出した確率の総乗を、心配粒子a1の尤度とする。なお、図11では、心配粒子a1からの各壁までの距離と、真の自己位置からの各壁までの距離との差が小さいため、心配粒子a1の尤度は高くなる。
そして、図12において、ロボットRは、図11のようにして算出した粒子群psと心配粒子群asとの各粒子の尤度に基づいて、現在の自己位置を示す粒子群psをリサンプリングして、現在の自己位置を特定する。
具体的には、リサンプリングでは、粒子群psと心配粒子群asとの各粒子の尤度に比例した確率に従って、各粒子の中から粒子群psの粒子pを選択し直す。このとき、同一の粒子を複数回選択してもよい。より具体的には、例えば、ロボットRは、各粒子の尤度に比例するように粒子ごとに分割した[0,1]区間上で、[0,1]の一様乱数を発生させて、乱数の値が入る区間に対応する粒子(粒子pまたは心配粒子a)を、粒子群psの粒子pとして選択する。
図12に示すように、ロボットRの真の自己位置が、図10で推定された粒子群psが示す位置S4の近傍領域にあれば、粒子群psの各粒子pの尤度が高くなり、心配粒子群asの各粒子aの尤度が低くなる。そのため、ロボットRは、リサンプリングにより、図10で推定された粒子群psの各粒子pを心配粒子群asの各粒子aより優先して、粒子群psの粒子pとして選択し直し、粒子群psを推定し直す。そして、ロボットRは、推定し直された粒子群psが示す位置S4を自己位置であると推定することができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域ではないと判定して、通常動作を実行することができる。
一方、ロボットRの真の自己位置が、図10で推定された粒子群psが示す位置S4の近傍領域ではなく、危険領域Dの近傍領域にあれば、粒子群psの各粒子pの尤度が低くなり、心配粒子群asの各粒子aの尤度が高くなる。そのため、ロボットRは、リサンプリングにより、心配粒子群asの各粒子aを粒子群psの各粒子pより優先して、粒子群psの粒子pとして選択し直し、粒子群psを推定し直す。そして、ロボットRは、推定し直された粒子群psが示す位置S5を自己位置であると推定することができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると判定して、危険領域Dから離れる方向へ移動することができる。
このように、ロボットRは、心配粒子群asを用いてリサンプリングを行うことにより、心配粒子群asの各粒子aの尤度が高い場合は、推定し直す粒子群psの粒子pとして心配粒子aを選択する。ここで、ロボットRは、真の自己位置が危険領域Dの近傍領域にある場合、尤度の高い心配粒子aが粒子群psの粒子pとして選択されることにより、自己位置の推定精度の向上を図ることができる。
一方、ロボットRは、心配粒子群asを用いてリサンプリングを行っても、真の自己位置が危険領域Dの近傍領域でなければ、尤度の低い心配粒子aは粒子群psの粒子pとして選択されないため、自己位置を危険領域Dの近傍領域にあると誤って推定することはない。また、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1にかかる推定装置200の動作例1における推定装置200の動作制御処理)
次に、図13を用いて、実施例1にかかる推定装置200の動作例1における推定装置200の動作制御処理の処理内容について説明する。
図13は、実施例1の動作例1における推定装置200の動作制御処理の処理内容を示すフローチャートである。まず、CPU201は、初期化処理を行う(ステップS1301)。なお、初期化処理では、推定装置200は、物体テーブル300から空間内の物体の情報を読み出し、危険領域テーブル400から空間内の危険領域Dの情報を読み出し、空間内に均等に粒子pを生成する。
次に、推定装置200は、粒子フィルタを用いたシミュレーションにより、移動体の直前の状態を示す粒子群psから、移動体の現在の状態を示す粒子群psを推定する(ステップS1302)。なお、ステップS1302の処理は、図10に示した推定装置200の処理に対応する。
そして、推定装置200は、センサ204により、自装置から障害物Oまでの距離を測定する(ステップS1303)。次に、CPU201は、心配粒子生成処理を実行する(ステップS1304)。
心配粒子生成処理において、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する。
そして、推定装置200は、ステップS1302で推定された粒子群psとステップS1304で生成された心配粒子群asとの各粒子の尤度を算出する(ステップS1305)。なお、ステップS1303〜ステップS1305の処理は、図11に示した推定装置200の処理に対応する。
次に、推定装置200は、ステップS1305で算出した各粒子の尤度から、ステップS1302で生成された粒子群psのリサンプリングを行い、移動体の現在の状態を特定する(ステップS1306)。そして、推定装置200は、特定した移動体の現在の状態に応じて動作を決定する(ステップS1307)。なお、ステップS1306およびステップS1307の処理は、図12に示した推定装置200の処理に対応する。
次に、推定装置200は、動作終了か否かを判定する(ステップS1308)。なお、例えば、動作終了とは、ロボットRが目的地に到着したこと、現在時刻が所定の時刻であることが挙げられる。ここで、動作終了していない場合(ステップS1308:No)、CPU201は、ステップS1302に戻る。
一方、動作終了している場合(ステップS1308:Yes)、推定装置200は、動作制御処理を終了する。これにより、推定装置200(移動体)は、自己位置を推定し、危険領域Dを回避しつつ移動を行うことができる。
(実施例1にかかる推定装置200の動作例2)
次に、図14を用いて、実施例1にかかる推定装置200の動作例2について説明する。
図14は、実施例1にかかる推定装置200の動作例2を示す説明図である。動作例1ではロボットRの推定した自己位置の精度に関わらず心配粒子aを生成していたが、動作例2では、ロボットRにより推定された自己位置の精度が高いときには心配粒子aを生成しない。また、動作例2では、ロボットRにより推定された自己位置の精度が低いときには心配粒子aを生成する。
これにより、ロボットRは、現在の自己位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。一方、ロボットRは、現在の自己位置の精度が低い場合には心配粒子aを生成することで、自己位置が危険領域Dの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
図14の(A)に示すように、ロボットRにより時刻「t−1」で推定された粒子群psの尤度の総和がしきい値以上であるため、時刻「t−1」でロボットRにより推定された自己位置は精度が高いものとする。この場合、ロボットRは、危険領域Dの近傍領域に心配粒子aを生成せずとも、次の時刻「t」の自己位置を推定できる。
図14の(B)に示すように、(1)ロボットRは、次の時刻「t」になると、(A)に示した時刻「t−1」で推定された粒子群psから、時刻「t」でのロボットRの自己位置を示す粒子群psを推定する。(2)ここで、ロボットRは、心配粒子群asを生成しない。
(3)次に、ロボットRは、センサ204により、真の自己位置から障害物Oまでの距離を測定する。(4)そして、ロボットRは、障害物Oまでの測定距離に基づいて、粒子群psのリサンプリングを行う。
図14の(C)に示すように、(B)でリサンプリングされた粒子群psの示す自己位置は、位置S6になっている。このように、尤度の総和がしきい値以上であった粒子群psに基づいて推定された現在の自己位置を示す粒子群psは精度が高いため、ロボットRは、正しく自己位置を推定することができている。(5)そして、ロボットRは、危険領域テーブル400を参照して、自己位置である位置S6が危険領域Dの近傍領域ではないと判定して、通常動作を行う。
このように、ロボットRは、ロボットRにより推定された自己位置の精度が高いときには心配粒子aを生成しないことで、処理量を低減することができる。また、ロボットRは、現在の自己位置の精度が低い場合には心配粒子aを生成することで、自己位置が危険領域Dの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1の動作例2におけるステップS1304の心配粒子生成処理)
次に、図15を用いて、実施例1の動作例2における心配粒子生成処理(ステップS1304)について説明する。なお、動作例2における推定装置200の動作制御処理の処理内容は、心配粒子生成処理(ステップS1304)を除き、図13に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
図15は、実施例1の動作例2における心配粒子生成処理(ステップS1304)の処理内容を示すフローチャートである。心配粒子生成処理(ステップS1304)において、推定装置200は、現在の自己位置の精度が高いか否かを判定する(ステップS1501)。具体的には、例えば、推定装置200は、前回の自己位置を示す粒子群psの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合(ステップS1501:Yes)、推定装置200は、図13のステップS1305に移行する。
一方、精度が低い場合(ステップS1501:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS1502)。そして、推定装置200は、図13のステップS1305に移行する。
これにより、推定装置200は、現在の自己位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、現在の自己位置の精度が低い場合には心配粒子aを生成することで、自己位置が危険領域Dの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置200は、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1にかかる推定装置200の動作例3)
次に、図16を用いて、実施例1にかかる推定装置200の動作例3について説明する。
図16は、実施例1にかかる推定装置200の動作例3を示す説明図である。動作例1ではロボットRが危険領域Dに進入する可能性があるか否かに関わらず心配粒子aを生成していたが、動作例3ではロボットRが進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しない。
これにより、ロボットRは、進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、ロボットRは、進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、進入する可能性がある危険領域Dの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
図16に示すように、ロボットRは、推定された粒子群psの尤度の総和がしきい値以上であり、推定された自己位置の精度が高かった時刻「t−6」がある場合、時刻「t−6」で推定された自己位置から現在時刻「t」までに移動可能な範囲を算出する。
具体的には、ロボットRは、時刻「t−6」から現在時刻「t」までの各時刻でのロボットRの速度から、ロボットRの時刻「t−6」から現在時刻「t」まででの最大移動距離を算出する。そして、ロボットRは、時刻「t−6」で推定された自己位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
この場合、ロボットRは、算出された移動可能な範囲に重複する危険領域Dには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Dには進入する可能性がない。そのため、ロボットRは、移動可能な範囲に重複する危険領域Dの近傍領域には心配粒子aを生成し、移動可能な範囲に重複しない危険領域Dの近傍領域には心配粒子aを生成しない。
(1)例えば、ロボットRは、算出した移動可能な範囲と重複している危険領域Dの近傍領域には、心配粒子aを生成する。
(2)また、ロボットRは、算出した移動可能な範囲と重複している危険領域Dの近傍領域には、心配粒子aを生成する。
(3)一方、ロボットRは、算出した移動可能な範囲と重複していない危険領域Dの近傍領域には、心配粒子aを生成しない。
これにより、ロボットRは、進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、ロボットRは、進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、進入する可能性がある危険領域Dの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、ロボットRは、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1の動作例3における推定装置200の動作制御処理)
次に、実施例1の動作例3における推定装置200の動作制御処理の処理内容について説明する。
動作例3における推定装置200の動作制御処理の処理内容は、ステップS1304の心配粒子生成処理を除き、図13に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
(実施例1の動作例3におけるステップS1304の心配粒子生成処理)
次に、図17を用いて、実施例1の動作例3における心配粒子生成処理(ステップS1304)について説明する。
図17は、実施例1の動作例3における心配粒子生成処理(ステップS1304)の処理内容を示すフローチャートである。心配粒子生成処理(ステップS1304)において、推定装置200は、一定時間以内に尤度の高い粒子pがあったか否かを判定する(ステップS1701)。
ここで、粒子pがなかった場合(ステップS1701:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS1702)。そして、推定装置200は、図13のステップS1305に移行する。
一方、粒子pがあった場合(ステップS1701:Yes)、推定装置200は、最も尤度が高い粒子pがあった時刻を特定する(ステップS1703)。次に、推定装置200は、特定した時刻から現在時刻までのロボットRの状態から、現在のロボットRの移動可能範囲を算出する(ステップS1704)。
そして、推定装置200は、算出した移動可能範囲と危険領域Dの近傍領域とが重複している危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS1705)。次に、推定装置200は、図13のステップS1305に移行する。
これにより、推定装置200は、進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、進入する可能性がある危険領域Dの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置200は、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1にかかる推定装置200の動作例4)
次に、実施例1にかかる推定装置200の動作例4について説明する。動作例4は、動作例2および動作例3を組み合わせた場合の動作例である。動作例4ではロボットRにより推定された自己位置の精度が高いときには心配粒子aを生成しない。また、動作例4ではロボットRが進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しない。
これにより、推定装置200は、推定された自己位置の精度が高いときには心配粒子aを生成しないため、処理を低減することができる。また、ロボットRは、現在の自己位置の精度が低い場合には心配粒子aを生成することで、自己位置が危険領域Dの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
また、推定装置200は、自装置が進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しないため、処理を低減することができる。また、推定装置200は、進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、進入する可能性がある危険領域Dの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置200は、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
(実施例1にかかる推定装置200の動作例4におけるステップS1304の心配粒子生成処理)
動作例4における推定装置200の動作制御処理の処理内容は、心配粒子生成処理(ステップS1304)を除き、図13に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。したがって、図18を用いて、実施例1の動作例4における心配粒子生成処理(ステップS1304)について説明する。
図18は、実施例1の動作例4における心配粒子生成処理(ステップS1304)の処理内容を示すフローチャートである。心配粒子生成処理(ステップS1304)において、推定装置200は、現在の自己位置の精度が高いか否かを判定する(ステップS1801)。具体的には、例えば、推定装置200は、前回の自己位置を示す粒子群psの総尤度がしきい値以上であった場合に、現在の自己位置の精度が高いと判定する。ここで、精度が高い場合(ステップS1801:Yes)、推定装置200は、図13のステップS1305に移行する。
一方、精度が低い場合(ステップS1801:No)、推定装置200は、一定時間以内に尤度の高い粒子pがあったか否かを判定する(ステップS1802)。ここで、粒子pがなかった場合(ステップS1802:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS1803)。そして、推定装置200は、図13のステップS1305に移行する。
一方、粒子pがあった場合(ステップS1802:Yes)、推定装置200は、最も尤度が高い粒子pがあった時刻を特定する(ステップS1804)。次に、推定装置200は、特定した時刻から現在時刻までのロボットRの状態から、現在のロボットRの移動可能範囲を算出する(ステップS1805)。
そして、推定装置200は、算出した移動可能範囲と危険領域Dの近傍領域とが重複している危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS1806)。次に、推定装置200は、図13のステップS1305に移行する。
これにより、推定装置200は、推定された自己位置の精度が高いときには心配粒子aを生成しないため、処理を低減することができる。また、ロボットRは、現在の自己位置の精度が低い場合には心配粒子aを生成することで、自己位置が危険領域Dの近傍領域にある場合、自己位置の推定精度の向上を図ることができる。
また、推定装置200は、自装置が進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しないため、処理を低減することができる。また、推定装置200は、進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、進入する可能性がある危険領域Dの近傍領域に自己位置がある場合、自己位置の推定精度の向上を図ることができる。結果として、推定装置200は、自己位置が危険領域Dの近傍領域にあると推定された場合には、危険領域Dから離れるように移動して、危険領域Dに進入することを回避することができる。
以上説明したように、実施例1にかかる推定装置200によれば、推定された移動体(推定装置200自体)の状態を示す粒子群psに加えて、危険領域Dの近傍領域での移動体の状態を示す心配粒子群asが生成される。次に、推定装置200によれば、粒子群psと心配粒子群asとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置200によれば、リサンプリングにより選択し直された粒子群psから、移動体の状態が推定し直される。
このとき、推定装置200の真の自己位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での移動体の状態を示す心配粒子aが粒子群psの粒子pとして優先的に選択される。そのため、推定装置200は、真の自己位置が危険領域Dの近傍領域にある場合、リサンプリングにより選択し直された粒子群psによる自己位置の推定精度の向上を図ることができる。
また、推定装置200によれば、推定された移動体の状態の精度が高いときには心配粒子aを生成しないことにより、処理量を低減することができる。また、推定装置200によれば、現在の自己位置の精度が低い場合には心配粒子aが生成される。このとき、推定装置200の真の自己位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での移動体の状態を示す心配粒子aが粒子群psの粒子pとして優先的に選択される。そのため、推定装置200は、真の自己位置が危険領域Dの近傍領域にある場合、リサンプリングにより選択し直された粒子群psによる自己位置の推定精度の向上を図ることができる。
また、推定装置200によれば、移動体が移動可能な範囲と重複しない危険領域Dの近傍領域は、移動体が進入する可能性がない危険領域Dの近傍領域であるため、心配粒子aを生成しないことで、処理量を低減することができる。また、推定装置200によれば、移動体が移動可能な範囲と重複する危険領域Dの近傍領域は、移動体が進入する可能性があるため、心配粒子aを生成が生成される。このとき、推定装置200の真の自己位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での移動体の状態を示す心配粒子aが粒子群psの粒子pとして優先的に選択される。そのため、推定装置200は、進入する可能性がある危険領域Dの近傍領域に真の自己位置がある場合、リサンプリングにより選択し直された粒子群psによる自己位置の推定精度の向上を図ることができる。
また、推定装置200によれば、推定された移動体の位置が危険領域Dの近傍領域にある場合、移動体が危険領域Dから離れるようにするため、移動体が危険領域Dに進入することを回避することができる。
(実施例2)
次に、実施例2について説明する。実施例1は、移動体が推定装置200自体である場合での移動体の位置推定の例であったが、実施例2は、移動体が推定装置200とは異なる他の装置である場合での移動体の位置推定の例である。例えば、実施例2では推定装置200として車両を採用し、移動体として推定装置200の前方の車両を採用した場合について説明する。このとき、車両(推定装置200)は、前方の車両(移動体)の位置を推定し、自車の周囲(危険領域D)に前方の車両が接近することを回避する。
まず、図19を用いて、実施例2にかかる推定装置200による前方車両の位置推定の内容について説明する。
図19は、実施例2にかかる推定装置200による前方車両の位置推定の内容を示す説明図である。図19の例では、車両Cは、危険領域Dの位置情報を有し、前方車両Fの位置を推定して、前方車両Fが危険領域Dの近傍領域に進入しないように移動を行う。
図19の(A)において、(1)車両Cは、粒子フィルタを用いたシミュレーションにより、過去の時刻「t−1」での前方車両Fの状態を示す粒子群psから現在時刻「t」での前方車両Fの状態を示す粒子群psを推定する。前方車両Fの状態Atは下記式(4)で表される。
ここで、Wtは、前方車両Fの横幅である。X tは、前方車両Fの横方向の中心座標である。Ztは、車両Cから前方車両Fまでの距離である。vxtは、車両Cに対する前方車両Fの横方向の相対速度である。vztは、車両Cに対する前方車両Fの距離方向の相対速度である。
具体的には、車両Cは、粒子群psの各粒子pが示す前方車両の状態At-1を、下記式(5)および式(6)の状態方程式に入力して現在の各粒子pの状態Atを算出する。ΔTは前方車両Fの状態の推定を実行する間隔を示すタイムステップであり、「(t)−(t−1)」である。
これにより、車両Cは、時刻「t」での前方車両Fの状態を示す粒子群psを推定することができる。ここでは、車両Cは、推定された現在時刻「t」での前方車両Fの状態を示す粒子群psにより、現在時刻「t」での前方車両Fの位置を位置S7であると推定する。
しかしながら、真の前方車両Fの位置は危険領域Dの近傍である位置S8であり、車両Cは前方車両Fの位置を誤って推定している。
図19の(B)において、(2)車両Cは、推定された現在時刻「t」での前方車両Fの位置を示す粒子群psに加えて、危険領域Dの位置情報に基づいて危険領域Dの近傍領域での前方車両Fの状態を示す心配粒子群asを生成する。
(3)次に、車両Cは、センサ204により、前方車両Fの位置までの距離drを測定する。また、車両Cは、センサ204により、前方車両Fの左端の座標値と右端の座標値とを測定する。
(4)そして、車両Cは、下記式(7)および式(8)により、粒子群psと心配粒子群asとの各粒子が示す前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値を算出する。ここで、前方車両Fまでの距離とは、例えば、粒子p1が示す前方車両Fの位置までの距離dpや心配粒子a1が示す前方車両Fの位置までの距離daである。
車両Cは、算出された各粒子が示す前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値を、測定された前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値と比較したときの尤度を算出する。そして、車両Cは、算出した尤度に基づいて、現在時刻「t」での前方車両Fの位置を示す粒子群psのリサンプリングを行う。
例えば、車両Cは、測定された前方車両Fの状態を示す値を期待値とした多変量正規分布において、粒子pが示す前方車両Fの状態を示す値になる確率p(yt|At|t-1 (i))を、下記式(9)により算出する。ここで、ytはセンサ204により測定された前方車両Fの状態を示す値であり、例えば、前方車両Fまでの距離および前方車両Fの左端の座標値と右端の座標値である。yt (i)は算出された前方車両Fの状態を示す値であり、前方車両Fまでの距離および前方車両Fの左端の座標値と右端の座標値である。ρは次元数であり、ここでは3である。iは粒子の番号である。Σは分散共分散行列である。
そして、推定装置200は、算出した確率を、粒子pの尤度とする。なお、図19では、粒子pが示す位置までの距離および前方車両Fの左端の座標値と右端の座標値が真の前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値と差が大きいため、粒子pの尤度は低くなる。
車両Cは、同様に心配粒子aが示す前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値を、測定された前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値と比較したときの尤度を算出する。
例えば、推定装置200は、測定された前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値を期待値とした多変量正規分布において、心配粒子aが示す位置までの距離および前方車両Fの左端の座標値と右端の座標値になる確率を、式(9)により算出する。
そして、推定装置200は、算出した確率を、心配粒子aの尤度とする。なお、図19では、心配粒子aが示す位置までの距離および前方車両Fの左端の座標値と右端の座標値が、測定された前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値と差が小さいため、心配粒子aの尤度は高くなる。
図19の(C)において、車両Cは、粒子群psのリサンプリングにおいて、粒子群psの各粒子pより尤度の高い心配粒子群asの各粒子aを優先して、粒子群psの粒子pとして選択し直す。
(5)車両Cは、リサンプリング結果に基づいて、現在時刻「t」での前方車両Fの位置を位置S8であると推定する。そして、車両Cは、危険領域テーブル400の近傍領域項目の内容から特定される危険領域Dの近傍領域の範囲に、前方車両Fの位置である位置S8が含まれていると判定して、前方車両Fが危険領域Dに進入することを回避するために、車両Cの速度を遅くする。
このように、車両Cは、粒子フィルタを用いたシミュレーションによって推定された現在時刻「t」での前方車両Fの位置を示す粒子群psが危険領域Dの近傍領域に存在しない場合であっても、前方車両Fの位置が危険領域Dの近傍領域にあることを推定できる。結果として、車両Cは、前方車両Fの位置が危険領域D(車両Cの周辺)の近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
図19では、車両Cは、前方車両Fの位置が危険領域Dの近傍領域にある場合、車両Cの速度を遅くしたが、これに限らない。例えば、車両Cは、前方車両Fの位置が危険領域Dの近傍領域にある場合、出力装置205により車両Cの運転者に警告を通知するようにしてもよい。
また、図19では、車両Cは、前方車両Fの位置を推定したが、これに限らない。例えば、車両Cは、後方車両の位置を推定してもよい。また、図19では、車両Cは、1台の前方車両Fの位置を推定したが、これに限らない。例えば、車両Cは、複数の車線がある場合、それぞれの車線を走行中の前方車両Fの位置を推定してもよい。
また、図19では、危険領域Dは固定された領域であったが、これに限らない。例えば、危険領域Dは車両Cに対する前方車両Fの相対速度によって変化する可変の領域であってもよい。
図20は、可変の危険領域Dの例を示す説明図である。例えば、車両Cに対する前方車両Fの相対速度が正であるときは、車両Cと前方車両Fの車間距離は時間の経過により大きくなるので、今現在の車間距離がある程度小さくても衝突の可能性は低い。一方で、車両Cに対する前方車両Fの相対速度が負であり、相対速度の絶対値が大きいほど、時間の経過により車間距離はより短くなるので、衝突を回避するにはより大きな車間距離が必要となる。そのため、図20に示すように、車両Cに対する前方車両Fの相対速度が負で絶対値が大きくなるにつれて、より車両Cから遠い領域が危険領域Dとして設定されるようにしてもよい。
(実施例2の動作例1における推定装置200の動作制御処理)
次に、図21を用いて、実施例2の動作例1における推定装置200の動作制御処理の処理内容について説明する。
図21は、実施例2の動作例1における推定装置200の動作制御処理の処理内容を示すフローチャートである。まず、推定装置200は、初期化処理を行う(ステップS2101)。なお、初期化処理では、推定装置200は、物体テーブル300から空間内の物体の情報を読み出し、危険領域テーブル400から空間内の危険領域Dの情報を読み出し、空間内に均等に粒子pを生成する。
次に、推定装置200は、粒子フィルタを用いたシミュレーションにより、前方車両Fの直前の状態を示す粒子群psから、前方車両Fの現在の状態を示す粒子群psを推定する(ステップS2102)。なお、ステップS2102の処理は、図19の(A)に示した推定装置200の処理に対応する。
そして、推定装置200は、センサ204により、自装置から前方車両Fまでの距離を測定する(ステップS2103)。次に、推定装置200は、心配粒子生成処理を実行する(ステップS2104)。
心配粒子生成処理において、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する。
そして、推定装置200は、ステップS2102で推定された粒子群psとステップS2104で生成された心配粒子群asとの各粒子の尤度を算出する(ステップS2105)。なお、ステップS2103〜ステップS2105の処理は、図19の(B)に示した推定装置200の処理に対応する。
次に、推定装置200は、ステップS2105で算出した各粒子の尤度から、ステップS2102で生成された粒子群psのリサンプリングを行い、前方車両Fの現在の状態を特定する(ステップS2106)。そして、推定装置200は、特定した前方車両Fの現在の状態に応じて動作を決定する(ステップS2107)。なお、ステップS2106およびステップS2107の処理は、図19の(C)に示した推定装置200の処理に対応する。
次に、推定装置200は、動作終了か否かを判定する(ステップS2108)。ここで、動作終了していない場合(ステップS2108:No)、推定装置200は、ステップS2102に戻る。
一方、動作終了している場合(ステップS2108:Yes)、推定装置200は、動作制御処理を終了する。これにより、推定装置200は、前方車両Fの状態を推定し、前方車両Fが危険領域Dに進入することを回避しつつ移動を行うことができる。
(実施例2にかかる推定装置200の動作例2)
次に、図22を用いて、実施例2にかかる推定装置200の動作例2について説明する。
図22は、実施例2にかかる推定装置200の動作例2を示す説明図である。動作例1では車両Cの推定した前方車両Fの位置の精度に関わらず心配粒子aを生成していたが、動作例2では、車両Cにより推定された前方車両Fの位置の精度が高いときには心配粒子aを生成しない。また、動作例2では、車両Cにより推定された前方車両Fの位置の精度が低いときには心配粒子aを生成する。
これにより、車両Cは、現在の前方車両Fの位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。一方、車両Cは、現在の前方車両Fの位置の精度が低い場合には心配粒子aを生成することで、前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、車両Cは、前方車両Fの位置が危険領域D(車両Cの周辺)の近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
図22の(A)に示すように、車両Cにより時刻「t−1」で推定された粒子群psの尤度の総和がしきい値以上であるため、時刻「t−1」で車両Cにより推定された前方車両Fの位置は精度が高いものとする。この場合、車両Cは、危険領域Dの近傍領域に心配粒子aを生成せずとも、次の時刻「t」の前方車両Fの位置を推定できる。
図22の(B)に示すように、(1)車両Cは、次の時刻「t」になると、(A)に示した時刻「t−1」で推定された粒子群psから、時刻「t」での前方車両Fの位置を示す粒子群psを推定する。(2)ここで、車両Cは、心配粒子aを生成しない。
(3)次に、車両Cは、センサ204により、真の前方車両Fの位置までの距離および前方車両Fの左端の座標値と右端の座標値を測定する。(4)そして、車両Cは、測定した前方車両Fまでの距離および前方車両Fの左端の座標値と右端の座標値に基づいて、粒子群psのリサンプリングを行う。
図22の(C)に示すように、(B)でリサンプリングされた粒子群psの示す前方車両Fの位置は、真の前方車両Fの位置と同じ位置になっている。このように、尤度の総和がしきい値以上であった粒子群psに基づいて推定された現在の前方車両Fの位置を示す粒子群psは精度が高いため、車両Cは、正しく前方車両Fの位置を推定することができている。(5)そして、車両Cは、危険領域テーブル400を参照して、前方車両Fの位置が危険領域Dの近傍領域ではないと判定して、通常動作を行う。
このように、車両Cは、車両Cにより推定された前方車両Fの位置の精度が高いときには心配粒子aを生成しないことで、処理量を低減することができる。また、車両Cは、現在の前方車両Fの位置の精度が低い場合には心配粒子aを生成することで、前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、車両Cは、前方車両Fの位置が危険領域D(車両Cの周辺)の近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
(実施例2の動作例2における推定装置200の動作制御処理)
次に、実施例2の動作例2における推定装置200の動作制御処理の処理内容について説明する。
動作例2における推定装置200の動作制御処理の処理内容は、ステップS2104の心配粒子生成処理を除き、図21に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
(実施例2の動作例2におけるステップS2104の心配粒子生成処理)
次に、図23を用いて、実施例2の動作例2における心配粒子生成処理(ステップS2104)について説明する。
図23は、実施例2の動作例2における心配粒子生成処理(ステップS2104)を示すフローチャートである。心配粒子生成処理(ステップS2104)において、推定装置200は、現在の前方車両Fの位置の精度が高いか否かを判定する(ステップS2301)。具体的には、推定装置200は、前回の前方車両Fの位置を示す粒子群psの総尤度がしきい値以上であった場合に、現在の前方車両Fの位置の精度が高いと判定する。ここで、精度が高い場合(ステップS2301:Yes)、推定装置200は、図21のステップS2105に移行する。
一方、精度が低い場合(ステップS2301:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS2302)。そして、推定装置200は、図21のステップS2105に移行する。
これにより、推定装置200は、現在の前方車両Fの位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、現在の前方車両Fの位置の精度が低い場合には心配粒子aを生成することで、前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、推定装置200は、前方車両Fの位置が危険領域Dの近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
(実施例2にかかる推定装置200の動作例3)
次に、図24を用いて、実施例2にかかる推定装置200の動作例3について説明する。
図24は、実施例2にかかる推定装置200の動作例3を示す説明図である。動作例1では前方車両Fが危険領域Dに進入する可能性があるか否かに関わらず心配粒子aを生成していたが、動作例3では前方車両Fが進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しない。
これにより、車両Cは、前方車両Fが進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、車両Cは、前方車両Fが進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、推定装置200は、前方車両Fの位置が危険領域D(車両Cの周囲)の近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
図24に示すように、車両Cは、推定された粒子群psの尤度の総和がしきい値以上であり、推定された前方車両Fの位置の精度が高かった時刻「t−6」がある場合、時刻「t−6」で推定された前方車両Fの位置から現在時刻「t」までに移動可能な範囲を算出する。
具体的には、車両Cは、時刻「t−6」から現在時刻「t」までの各時刻での車両Cに対する前方車両Fの相対速度から、前方車両Fの時刻「t−6」から現在時刻「t」まででの最大移動距離を算出する。そして、車両Cは、時刻「t−6」で推定された前方車両Fの位置を中心として算出した最大移動距離を半径とする円を、移動可能な範囲として算出する。
この場合、前方車両Fは、算出された移動可能な範囲に重複する危険領域Dには進入する可能性があるが、算出された移動可能な範囲に重複しない危険領域Dには進入する可能性がない。
(1)車両Cは、算出した移動可能な範囲と重複している危険領域Dの近傍領域には、心配粒子aを生成する。
(2)また、車両Cは、算出した移動可能な範囲と重複している危険領域Dの近傍領域には、心配粒子aを生成する。
(3)一方、車両Cは、算出した移動可能な範囲と重複していない危険領域Dの近傍領域には、心配粒子aを生成しない。
これにより、車両Cは、前方車両Fが進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、車両Cは、前方車両Fが進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、推定装置200は、前方車両Fの位置が危険領域D(車両Cの周囲)の近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
(実施例2の動作例3における推定装置200の動作制御処理)
次に、実施例2の動作例3における推定装置200の動作制御処理の処理内容について説明する。
動作例3における推定装置200の動作制御処理の処理内容は、ステップS2104の心配粒子生成処理を除き、図21に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
(実施例2の動作例3におけるステップS2104の心配粒子生成処理)
次に、図25を用いて、実施例2の動作例3における心配粒子生成処理(ステップS2104)について説明する。
図25は、実施例2の動作例3における心配粒子生成処理(ステップS2104)を示すフローチャートである。心配粒子生成処理(ステップS2104)において、推定装置200は、一定時間以内に尤度の高い粒子pがあったか否かを判定する(ステップS2501)。
ここで、粒子pがなかった場合(ステップS2501:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS2502)。そして、推定装置200は、図21のステップS2105に移行する。
一方、粒子pがあった場合(ステップS2501:Yes)、推定装置200は、最も尤度が高い粒子pがあった時刻を特定する(ステップS2503)。次に、推定装置200は、特定した時刻から現在時刻までの前方車両Fの状態から、現在の前方車両Fの移動可能範囲を算出する(ステップS2504)。
そして、推定装置200は、算出した移動可能範囲と危険領域Dの近傍領域とが重複している危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS2505)。次に、推定装置200は、図21のステップS2105に移行する。
これにより、推定装置200は、前方車両Fが進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、前方車両Fが進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、推定装置200は、前方車両Fの位置が危険領域Dの近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
(実施例2にかかる推定装置200の動作例4)
次に、実施例2にかかる推定装置200の動作例4について説明する。動作例4は、動作例2および動作例3を組み合わせた場合の動作である。動作例4では車両Cにより推定された前方車両Fの位置の精度が高いときには心配粒子aを生成しない。また、動作例4では前方車両Fが進入する可能性がない危険領域Dの近傍領域には心配粒子aを生成しない。
これにより、車両Cは、現在の前方車両Fの位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。また、車両Cは、現在の前方車両Fの位置の精度が低い場合には心配粒子aを生成することで、前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。
また、車両Cは、前方車両Fが進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、車両Cは、前方車両Fが進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、車両Cは、前方車両Fの位置が危険領域Dの近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
(実施例2の動作例4における推定装置200の動作制御処理)
次に、実施例2の動作例4における推定装置200の動作制御処理の処理内容について説明する。
動作例4における推定装置200の動作制御処理の処理内容は、ステップS2104の心配粒子生成処理を除き、図21に示した動作例1における推定装置200の動作制御処理の処理内容と同様であるため、ここでは説明を省略する。
(実施例2の動作例4におけるステップS2104の心配粒子生成処理)
次に、図26を用いて、実施例2の動作例4における心配粒子生成処理(ステップS2104)について説明する。
図26は、実施例2の動作例4における心配粒子生成処理(ステップS2104)の処理内容を示すフローチャートである。動作例4における心配粒子生成処理(ステップS2104)において、推定装置200は、現在の前方車両Fの位置の精度が高いか否かを判定する(ステップS2601)。具体的には、推定装置200は、前回の前方車両Fの位置を示す粒子群psの総尤度がしきい値以上であった場合に、現在の前方車両Fの位置の精度が高いと判定する。ここで、精度が高い場合(ステップS2601:Yes)、推定装置200は、図21のステップS2105に移行する。
一方、精度が低い場合(ステップS2601:No)、推定装置200は、一定時間以内に尤度の高い粒子pがあったか否かを判定する(ステップS2602)。ここで、粒子pがなかった場合(ステップS2602:No)、推定装置200は、危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS2603)。そして、推定装置200は、図21のステップS2105に移行する。
一方、粒子pがあった場合(ステップS2602:Yes)、推定装置200は、最も尤度が高い粒子pがあった時刻を特定する(ステップS2604)。次に、推定装置200は、特定した時刻から現在時刻までの前方車両Fの状態から、現在の前方車両Fの移動可能範囲を算出する(ステップS2605)。
そして、推定装置200は、算出した移動可能範囲と危険領域Dの近傍領域とが重複している危険領域テーブル400のレコードの心配粒子IDリスト項目の内容から心配粒子IDを特定して、心配粒子テーブル600に基づいて、特定した心配粒子IDの心配粒子aを生成する(ステップS2606)。次に、推定装置200は、図21のステップS2105に移行する。
これにより、推定装置200は、現在の前方車両Fの位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、現在の前方車両Fの位置の精度が低い場合には心配粒子aを生成することで、前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。
また、推定装置200は、前方車両Fが進入する可能性がない危険領域Dの近傍領域に心配粒子aを生成せずに処理量を低減することができる。また、推定装置200は、前方車両Fが進入する可能性がある危険領域Dの近傍領域に心配粒子aを生成することにより、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。結果として、推定装置200は、前方車両Fの位置が危険領域Dの近傍領域にあれば、前方車両Fが危険領域Dから離れるように移動することで、前方車両Fが危険領域Dに進入することを回避できる。
以上説明したように、実施例2にかかる推定装置200によれば、前方車両Fの状態を示す粒子群psに加えて、危険領域Dの近傍領域での前方車両Fの状態を示す心配粒子群asが生成される。次に、推定装置200によれば、粒子群psと心配粒子群asとの各粒子の尤度に基づいて、リサンプリングが行われる。そして、推定装置200によれば、リサンプリングにより選択し直された粒子群psから、前方車両Fの状態が推定し直される。
このとき、真の前方車両Fの位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での前方車両Fの状態を示す心配粒子aが、粒子群psの粒子pとして選択される。そのため、推定装置200は、真の前方車両Fの位置が危険領域Dの近傍領域にある場合、リサンプリングにより選択し直された粒子群psによる前方車両Fの位置の推定精度の向上を図ることができる。
また、推定装置200によれば、現在の前方車両Fの位置の精度が高い場合には、心配粒子aを生成せずに処理量を低減することができる。また、推定装置200によれば、現在の前方車両Fの位置の精度が低い場合には、心配粒子aが生成される。このとき、真の前方車両Fの位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での前方車両Fの状態を示す心配粒子aが、粒子群psの粒子pとして選択される。そのため、推定装置200は、前方車両Fの位置が危険領域Dの近傍領域にある場合、リサンプリングにより選択し直された粒子群psによる前方車両Fの位置の推定精度の向上を図ることができる。
また、推定装置200によれば、前方車両Fが移動可能な範囲と重複しない危険領域Dの近傍領域は、前方車両Fが進入する可能性がない危険領域Dの近傍領域であるため、心配粒子aを生成しないことで、処理量を低減することができる。また、推定装置200によれば、前方車両Fが移動可能な範囲と重複する危険領域Dの近傍領域は、前方車両Fが進入する可能性があるため、心配粒子aが生成される。このとき、真の前方車両Fの位置が危険領域Dの近傍領域にあれば心配粒子aの尤度が高くなり、リサンプリングによって、危険領域Dの近傍領域での前方車両Fの状態を示す心配粒子aが、粒子群psの粒子pとして選択される。そのため、推定装置200は、リサンプリングにより選択し直された粒子群psから、前方車両Fの位置が進入する可能性がある危険領域Dの近傍領域にある場合、前方車両Fの位置の推定精度の向上を図ることができる。
また、推定装置200によれば、推定された前方車両Fの位置が危険領域Dの近傍領域にある場合、前方車両Fが危険領域Dから離れるようにするため、前方車両Fが危険領域Dに進入することを回避することができる。
なお、本実施の形態で説明した推定方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。本推定プログラムは、ハードディスク、フレキシブルディスク、CD−ROM、MO、DVD等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。また本推定プログラムは、インターネット等のネットワークを介して配布してもよい。
上述した実施の形態に関し、さらに以下の付記を開示する。
(付記1)粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第1の粒子群を現状態から次状態に更新する更新手段と、
前記更新手段によって更新される都度、前記移動体と前記空間内の物体との距離を測定する測定手段と、
前記更新手段によって更新される都度、前記空間内の特定の領域の状態を示す第2の粒子群を生成する生成手段と、
前記測定手段によって測定された距離と、前記更新手段による更新されたときの第1の粒子群と、前記生成手段によって生成された第2の粒子群と、に基づいて、前記第1および第2の粒子群の各粒子の尤度を算出する算出手段と、
前記算出手段によって算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する特定手段と、
を備えることを特徴とする推定装置。
(付記2)前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段を備え、
前記生成手段は、
前記判断手段によって前記しきい値以上であると判断された場合には、前記第2の粒子群の生成を停止することを特徴とする付記1に記載の推定装置。
(付記3)前記算出手段によって前回算出された尤度がしきい値以上であるか否かを判断する判断手段と、
前記判断手段によって前記しきい値以上であると判断された尤度が得られたときの前記移動体の前記空間内の位置に基づいて、前記移動体の移動範囲を設定する設定手段と、を備え、
前記生成手段は、
前記特定の領域のうち、前記設定手段によって設定された移動範囲と重複する領域に前記第2の粒子群を生成することを特徴とする付記1または2に記載の推定装置。
(付記4)前記特定手段によって特定された前記移動体の位置が前記特定の領域であるか否かを判定する判定手段と、
前記判定手段によって前記特定の領域に位置すると判定された場合、前記特定の領域から離れる方向へ前記移動体が移動するように制御する制御手段と、
を備えることを特徴とする付記1〜3のいずれか一つに記載の推定装置。
(付記5)粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第1の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第2の粒子群を生成し、
測定された距離と、更新されたときの第1の粒子群と、生成された第2の粒子群と、に基づいて、前記第1および第2の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータが実行することを特徴とする推定方法。
(付記6)粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第1の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第2の粒子群を生成し、
測定された距離と、更新されたときの第1の粒子群と、生成された第2の粒子群と、に基づいて、前記第1および第2の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータが実行することを特徴とする推定方法。
(付記7)粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第1の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第2の粒子群を生成し、
測定された距離と、更新されたときの第1の粒子群と、生成された第2の粒子群と、に基づいて、前記第1および第2の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記移動体内のコンピュータに実行させることを特徴とする推定プログラム。
(付記8)粒子フィルタを用いた空間内での移動体の状態方程式に基づくシミュレーションにより、前記移動体の状態を示す第1の粒子群を現状態から次状態に更新し、
更新される都度、前記移動体と前記空間内の物体との距離を測定し、
更新される都度、前記空間内の特定の領域の状態を示す第2の粒子群を生成し、
測定された距離と、更新されたときの第1の粒子群と、生成された第2の粒子群と、に基づいて、前記第1および第2の粒子群の各粒子の尤度を算出し、
算出された前記各粒子の尤度に基づいて、前記空間内での前記移動体の位置を特定する、
処理を前記物体内のコンピュータに実行させることを特徴とする推定プログラム。