JP2010271215A - 状態推定装置、方法、プログラム及びその記録媒体 - Google Patents

Abstract

【課題】未知の外乱がある環境で、対象物の位置等の状態（位置）の推定を高精度で行う。
【解決手段】状態パーティクルを用いて状態の推定を行うモンテカルロ・ローカリゼーション法を応用して、さらに変位量パーティクルを用いる。変位量パーティクルを用いることにより、対象物に与えられる外乱を考慮して対象物の変位量を推定することが可能となり、対象物の状態の推定を高精度に行うことができる。また、誘拐度Ｅを定義して、誘拐度Ｅが低い場合には、移動誤差を拡大することで状態パーティクルの存在範囲を広げる処理を行う。
【選択図】図１

Description

この発明は、対象物の位置等の状態を計測及び推定する技術に関する。

自律移動ロボットの位置を計測する技術として、ＧＰＳ方式、無線ＬＡＮ方式による位置計測技術が知られている。しかし、これらの位置計測技術の計測精度は、自律移動ロボットを制御する上で不十分であることがある。

そこで、これらの位置計測技術により求まった計測値を用いて、より精度が高い位置を推定するために、フィルタ技術が導入されている（非特許文献１）。これらのフィルタ技術のほとんどがベイズフィルタの応用によるものである。ベイズフィルタを用いて高精度の位置の推定を行うためには、自動移動ロボットの高精度な移動モデルが必要である。

上田隆一、新井民夫、浅沼和範、梅田和昇、大隅久、「パーティクルフィルタを利用した自己位置推定に生じる致命的な推定誤りからの回復法」、日本ロボット学会誌Vol. 23、No. 4、pp.466〜473、2005

非特許文献１に記載された技術では、環境中において、時間的な観点で継続的に存在する風などの未知の外乱がある環境では対象物の位置等の状態（位置）の推定を高精度で行うことができないという課題があった。

上記の課題を解決するために、状態パーティクルのみならず、変位量パーティクルを用いる。

変位量パーティクルを用いることにより、対象物に与えられる外乱を考慮して対象物の変位量を推定することが可能となり、対象物の状態の推定を高精度に行うことができる。

状態推定装置の機能ブロック図の例。 変位量計算部の機能ブロック図の例。 変位量重み計算部の機能ブロック図の例。 状態推定方法の流れ図の例。 ステップＳ１の流れ図の例。 ステップＳ２の流れ図の例。 ステップＳ３の流れ図の例。 ステップＳ６の流れ図の例。 ステップＳ７の流れ図の例。 自律移動ロボットを例示する図。（ａ）は正面図であり、（ｂ）は側面図。

［対象物］
状態の推定の対象となる対象物は、例えば図１０に例示する自律飛行船等の自律移動ロボットである。この自律飛行船の重心位置は低く設定されているので姿勢傾斜に対する復元力は大きく、自律飛行船の姿勢角は方位角以外は０に維持されるとする。以下、対象物が自律移動ロボットである場合を例に挙げて説明する。なお、状態の推定の対象となる対象物は何でもよく、自律移動ロボットに限られない。

図１０の自律移動ロボットのゴンドラ部には、５つの無線ＬＡＮ方式の位置計測センサ６１，６２，６３，６４，６５が取り付けられている。これらの位置計測センサ６１，６２，６３，６４，６５は、自律移動ロボットの水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））を中心として水平面内に点対象に配置されている。位置計測センサ６１，６２，６３，６４，６５は、水平面内重心位置（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ））から、それぞれ方位角にしてΦｓ１，Φｓ２，Φｓ３，Φｓ４，Φｓ５の方向に距離Ｒｓ１，Ｒｓ２，Ｒｓ３，Ｒｓ４，Ｒｓ５だけ離れた位置に固定されているものとする。各位置計測センサ６１，６２，６３，６４，６５は、水平面内位置（ＸＹ）及び高度（Ｚ）を計測することができる。

時刻ｔにおける位置計測センサ６１，６２，６３，６４，６５の計測値をそれぞれＺ１（ｔ）、Ｚ２（ｔ）、Ｚ３（ｔ）、Ｚ４（ｔ）、Ｚ５（ｔ）とする。

Ｚ１（ｔ）＝（Ｚｘ１（ｔ），Ｚｙ１（ｔ），Ｚｚ１（ｔ））
Ｚ２（ｔ）＝（Ｚｘ２（ｔ），Ｚｙ２（ｔ），Ｚｚ２（ｔ））
Ｚ３（ｔ）＝（Ｚｘ３（ｔ），Ｚｙ３（ｔ），Ｚｚ３（ｔ））
Ｚ４（ｔ）＝（Ｚｘ４（ｔ），Ｚｙ４（ｔ），Ｚｚ４（ｔ））
Ｚ５（ｔ）＝（Ｚｘ５（ｔ），Ｚｙ５（ｔ），Ｚｚ５（ｔ））

位置計測センサ６１，６２，６３，６４，６５の真の位置をそれぞれ（Ｘｓ１（ｔ），Ｙｓ１（ｔ），Ｚｓ１（ｔ））、（Ｘｓ２（ｔ），Ｙｓ２（ｔ），Ｚｓ２（ｔ））、（Ｘｓ３（ｔ），Ｙｓ３（ｔ），Ｚｓ３（ｔ））、（Ｘｓ４（ｔ），Ｙｓ４（ｔ），Ｚｓ４（ｔ））、（Ｘｓ５（ｔ），Ｙｓ５（ｔ），Ｚｓ５（ｔ））とし、自律移動ロボットの方位角をΦｇ（ｔ）とし、ｉ＝１，…，５とすると、
Ｘｓｉ（ｔ）＝Ｘｇ（ｔ）＋Ｒｓｉ・ｃｏｓ（Φｇ（ｔ）＋Φｓｉ）
Ｙｓｉ（ｔ）＝Ｙｇ（ｔ）＋Ｒｓｉ・ｓｉｎ（Φｇ（ｔ）＋Φｓｉ）
Ｚｓｉ（ｔ）＝Ｚｇ（ｔ）
という関係が成り立つ。この関係を利用して、位置計測センサ６１，６２，６３，６４，６５の計測値から、自律移動ロボットの方位角を求めることができる。もちろん、これらの位置計測センサとは別に、直接方位角を計測するジャイロセンサ等の角度センサを用いても構わない。

対象物の推定の対象となる状態は、例えば水平面内位置、高度及び方位角（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））である。

［ベイズフィルタ］
以下、ベイズフィルタについて簡単に説明をする。ベイズフィルタの詳細については、参考文献１を参照のこと。

〔参考文献１〕スラン・セバスチャン（著）、バーガード・ウルフラム（著）、フォックス・ディーター（著）、上田隆一（訳）、「確率ロボティクス」、毎日コミュニケーションズ、2007/10/25
ベイズフィルタは、以下の（１）予測、（２）計測更新を各時刻ｔにおいて全ての確率変数（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））について繰り返すアルゴリズムである。

（１）予測
bel0(Xg(t),Yg(t),Zg(t),Φg(t))=∫p(Xg(t),Yg(t),Zg(t),Φg(t)|u(t),Xg(t-1),Yg(t-1),Zg(t-1),Φg(t-1))・bel(Xg(t-1),Yg(t-1),Zg(t-1),Φg(t-1))dXg(t-1)Yg(t-1)Zg(t-1)Φg(t-1)
（２）計測更新
bel(Xg(t),Yg(t),Zg(t),Φg(t))=ηp(Z1(t),Z2(t),Z3(t),Z4(t),Z5(t)|Xg(t),Yg(t),Zg(t),Φg(t))・bel0(Xg(t),Yg(t),Zg(t),Φg(t))
ｂｅｌ（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））は、自律移動ロボットの位置が（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））である確率分布である。時刻ｔのｂｅｌ（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））を求めるために、まず、（１）予測のステップにおいて、一時刻前のｂｅｌ（Ｘｇ（ｔ−１），Ｙｇ（ｔ−１），Ｚｇ（ｔ−１），Φｇ（ｔ−１））に、移動モデルにより推定した自律移動ロボットの状態遷移確率ｐ（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ）｜ｕ（ｔ），Ｘｇ（ｔ−１），Ｙｇ（ｔ−１），Ｚｇ（ｔ−１），Φｇ（ｔ−１））をかけて、時刻ｔのｂｅｌ（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））の推定値ｂｅｌ０（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））を求める。ｕ（ｔ）は時刻ｔでの自律移動ロボットの制御入力である。次に、（２）計測更新のステップにおいて、位置計測センサの計測値（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ））を用いて、推定値ｂｅｌ０（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））の補正を行い、時刻ｔのｂｅｌ（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））を求める。具体的には、自律移動ロボットの状態が（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））である場合に位置計測センサにより計測値（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ））が得られる確率ｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））を用いて、ｂｅｌ０の補正を行っている。なお、ηは、正の定数である。

［モンテカルロ・ローカリゼーション］
ベイズフィルタは、数学的な概念を記したものに過ぎないので、計算機でこれを実装するためには、別の表現が必要である。例えば、ベイズフィルタの実装形態として、パーティクルフィルタを用いたモンテカルロ・ローカリゼーション（ＭＬ）が知られている。以下、パーティクルフィルタ及びモンテカルロ・ローカリゼーションについて簡単に説明する。詳細については、参考文献１を参照のこと。

パーティクルフィルタは、例えば以下のアルゴリズムで記述される。
各状態パーティクルＳｉ（ｔ）（ｉ＝１，…，Ｍ）は、状態Ｌｉ（ｔ）＝（Ｘｇｉ（ｔ），Ｙｇｉ（ｔ），Ｚｇｉ（ｔ），Φｇｉ（ｔ））と、その状態Ｌｉ（ｔ）を取る可能性を示す指標である重みＷｉ（ｔ）との組により構成され、Ｓｉ（ｔ）＝（Ｌｉ（ｔ），Ｗｉ（ｔ））と表現される。Ｍは、状態パーティクルの数であり、一般にＭが大きい程計算の近似精度は高まる。

以下の（１）から（４）の計算により、時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）（ｉ＝１，…，Ｍ）から、時刻ｔの状態パーティクルＳｉ（ｔ）（ｉ＝１，…，Ｍ）を生成する。この（１）から（４）の計算を各時刻ｔごとに行う。

（１）時刻ｔ−１の複数の状態パーティクルＳｉ（ｔ−１）（ｉ＝１，…，Ｍ）から重みＷｉ（ｔ−１）の値に比例した確率に従って１つの状態パーティクルを選択してＳｋ（ｔ−１）とする。これを、ｋ＝１からｋ＝ＭまでＭ回繰り返す。すなわち、重みＷｉ（ｔ−１）の値に比例した確率に従って、時刻ｔ−１のＭ個の状態パーティクルＳｋ（ｔ−１）（ｋ＝１，…，Ｍ）を選択する。

重みＷｉ（ｔ−１）の値に比例した確率に従って状態パーティクルを選択することにより、時刻ｔ−１における自律移動ロボットの状態Ｌｉ（ｔ−１）における存在確率を考慮している。

（２）ｐ（Ｌ（ｔ）｜ｕ（ｔ−１），Ｌｋ（ｔ−１））の値に比例した確率に従って状態遷移先の１つの状態Ｌｋ（ｔ）を選択する。状態Ｌｋ（ｔ）の一時的な重みＷ’ｋ（ｔ）（一時重みＷ’ｋ（ｔ）とも言う。）を１／Ｍとする。これを、ｋ＝１からｋ＝ＭまでＭ回繰り返す。ｐは状態遷移確率である。ｐは自律移動ロボットの運動モデルを用いて計算される。

（３）以下の総和αを計算する。
α＝Σ_ｋ＝１ ^Ｍｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）
ｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））は、自律移動ロボットの状態がＬｋ（ｔ）である場合に、位置計測センサの計測値が（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ））である確率である。
一時重みＷ’ｋ（ｔ）は例えば１／Ｍである。

（４）以下の式のようにαを用いて重みを補正して、状態Ｌｋ（ｔ）の重みＷｋ（ｔ）とする。これにより、状態パーティクルＳｋ（ｔ）＝（Ｌｋ（ｔ），Ｗｋ（ｔ））が得られる。これを、ｋ＝１からｋ＝ＭまでＭ回繰り返す。これは、Ｗｋ（ｔ）の総和が１になるようにするための正規化処理である。

Ｗｋ（ｔ）＝ｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）／α
時刻ｔにおける自律移動ロボットの状態は、状態Ｌｉ（ｔ）（ｉ＝１，…，Ｍ）を重みＷｉ（ｔ）で重み付き加算することにより推定することができる。具体的には、状態Ｌｉ（ｔ）を構成するＸｇｉ（ｔ），Ｙｇｉ（ｔ），Ｚｇｉ（ｔ），Φｇｉ（ｔ）のそれぞれをＷｉ（ｔ）で重み付き加算することにより自律移動ロボットの状態（Ｘｇ（ｔ），Ｙｇ（ｔ），Ｚｇ（ｔ），Φｇ（ｔ））を推定することができる。

Ｘｇ（ｔ）＝Σ_ｉ＝１ ^ＭＷｉ（ｔ）Ｘｇｉ（ｔ）
Ｙｇ（ｔ）＝Σ_ｉ＝１ ^ＭＷｉ（ｔ）Ｙｇｉ（ｔ）
Ｚｇ（ｔ）＝Σ_ｉ＝１ ^ＭＷｉ（ｔ）Ｚｇｉ（ｔ）
Φｇ（ｔ）＝Σ_ｉ＝１ ^ＭＷｉ（ｔ）Φｇｉ（ｔ）

しかし、モンテカルロ・ローカリゼーションでは、自律移動ロボットの運動モデルが現実の運動を正確に反映していない場合や、自律移動ロボットに様々な外乱（突風、急な流れ等）が与えられて推定された状態とセンサによる計測値との間に大きな乖離が発生した場合に、フィルタの動作そのものが破綻してしまうという問題がある。これは誘拐問題と言われるものである。例えば、上記ステップ（３）のｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））の値が全ての状態パーティクルＳｋ（ｔ）について０になってしまうと、重みＷｉ（ｔ）の値がすべて０になってしまい状態パーティクルＳｋ（ｔ）が意味をなさないものになってしまう。

すなわち、モンテカルロ・ローカリゼーションでは、多くの状態パーティクルＳｋ（ｔ）についてのｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））の値が０になるのを防がなければならない。そのためには、自律移動ロボットの高精度な運動モデルの構築が必要であるが、未知の外乱がある環境における複雑かつ非線形な力学特性を持つ自律移動ロボットの運動モデルの構築は極めて難しい。そこで、この発明では、自律移動ロボットの状態のみならず、状態の変位量についてもパーティクルを用いて推定する。

［実施形態］
図１に、状態推定装置の機能ブロックを例示する。図４に、状態推定方法の流れ図を例示する。

＜ステップＳ１＞
状態パーティクル記憶部１には、各時刻ｔごとにＭ個の状態パーティクルＳｉ（ｔ）（ｉ＝１，…，Ｍ）が記憶されている。状態パーティクルＳｉ（ｔ）は、対象物の状態Ｌｉ（ｔ）＝（Ｘｇｉ（ｔ），Ｙｇｉ（ｔ），Ｚｇｉ（ｔ），Φｇｉ（ｔ））と、その状態Ｌｉ（ｔ）を取る可能性を示す指標である重みＷｉ（ｔ）との組により構成され、Ｓｉ（ｔ）＝（Ｌｉ（ｔ），Ｗｉ（ｔ））と表現される。

状態パーティクル選択部３は、時刻ｔ−１の状態パーティクルから重みの値に比例した確率に従ってＭ個の状態パーティクルＳｋ（ｔ−１）（ｋ＝１，…，Ｍ）を選択する（ステップＳ１）。ステップＳ１は、例えば以下のステップＳ１１からステップＳ１４で構成される。

制御部１５が変数ｋをｋ＝１と初期化する（ステップＳ１１）。

状態パーティクル選択部３は、時刻ｔ−１の状態パーティクルから重みの値に比例した確率に従って１つの状態パーティクルを選択してＳｋ（ｔ−１）とする（ステップＳ１２）。選択された状態パーティクルＳｋ（ｔ−１）は、状態変位部５に送られる。

例えば、Ｍ＝３であり（通常Ｍはもっと大きな数である。）、Ｓ１（ｔ−１）＝（Ｌ１（ｔ−１），１／２）、Ｓ２（ｔ−１）＝（Ｌ２（ｔ−１），１／３）、Ｓ３（ｔ−１）＝（Ｌ３（ｔ−１），１／６）であるとする。この場合、状態パーティクル選択部３は、１／２の確率で状態パーティクルＳ１（ｔ−１）を選択し、１／３の確率で状態パーティクルＳ２（ｔ−１）を選択し、１／６の確率で状態パーティクルＳ３（ｔ−１）を選択して、Ｓｋ（ｔ−１）とする。

制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ１３）。ｋ＝Ｍであれば、ステップＳ１を終えてステップＳ２に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ１４）、ステップＳ１２に戻る。ステップＳ１２において、状態パーティクル選択部３は、時刻ｔ−１の状態パーティクルから重みの値に比例した確率に従って１つの状態パーティクルを再度選択してＳｋ（ｔ−１）とする。

状態パーティクル選択部３は、異なるｋに対して毎回同じ状態パーティクルの集合から重みの値に比例した確率に従って１つの状態パーティクルを選択するため、重みが大きな状態パーティクルが複数選択されることもある。例えば、上記の例では、重みが最大である状態パーティクルＳ１（ｔ−１）が複数選択される可能性が高い。

＜ステップＳ２＞
変位量パーティクル記憶部２には、各時刻ｔごとにＭ個の変位量パーティクルＳＦｉ（ｔ）（ｉ＝１，…，Ｍ）が記憶されている。変位量パーティクルＳＦｉ（ｔ）は、対象物の状態の変位量Ｆｉ（ｔ）とその変位量Ｆｉ（ｔ）を取る可能性を示す指標である重みＷＦｉ（ｔ）との組により構成され、ＳＦｉ（ｔ）＝（Ｆｉ（ｔ），ＷＦｉ（ｔ））と表現される。Ｆｉ（ｔ）＝（Ｆｉｘ（ｔ），Ｆｉｙ（ｔ），Ｆｉｚ（ｔ），ＦｉΦ（ｔ））である。

変位量パーティクル選択部４は、時刻ｔ−１の変位量パーティクルから重みの値に比例した確率に従ってＭ個の変位量パーティクルＳＦｋ（ｔ−１）（ｋ＝１，…，Ｍ）を選択する（ステップＳ２）。ステップＳ２は、例えば以下のステップＳ２１からステップＳ２４で構成される。

制御部１５が変数ｋをｋ＝１と初期化する（ステップＳ２１）。

変位量パーティクル選択部４は、時刻ｔ−１の変位量パーティクルから重みの値に比例した確率に従って１つの変位量パーティクルを選択してＳＦｋ（ｔ−１）とする（ステップＳ２２）。選択された変位量パーティクルＳＦｋ（ｔ−１）は、状態変位部５に送られる。

例えば、Ｍ＝３であり（通常Ｍはもっと大きな数である。）、ＳＦ１（ｔ−１）＝（Ｆ１（ｔ−１），１／２）、ＳＦ２（ｔ−１）＝（Ｆ２（ｔ−１），１／３）、ＳＦ３（ｔ−１）＝（Ｆ３（ｔ−１），１／６）であるとする。この場合、変位量パーティクル選択部４は、１／２の確率で変位量パーティクルＳＦ１（ｔ−１）を選択し、１／３の確率で変位量パーティクルＳＦ２（ｔ−１）を選択し、１／６の確率で変位量パーティクルＳＦ３（ｔ−１）を選択して、ＳＦｋ（ｔ−１）とする。

制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ２３）。ｋ＝Ｍであれば、ステップＳ２を終えてステップＳ３に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ２４）、ステップＳ２２に戻る。ステップＳ２２において、変位量パーティクル選択部４は、時刻ｔ−１の変位量パーティクルから重みの値に比例した確率に従って１つの変位量パーティクルを再度選択してＳＦｋ（ｔ−１）とする。

変位量パーティクル選択部４は、異なるｋに対して毎回同じ変位量パーティクルの集合から重みの値に比例した確率に従って１つの変位量パーティクルを選択するため、重みが大きな変位量パーティクルが複数選択されることもある。例えば、上記の例では、重みが最大である変位量パーティクルＳＦ１（ｔ−１）が複数選択される可能性が高い。

＜ステップＳ３＞
状態変位部５は、各状態パーティクルＳｋ（ｔ−１）の状態Ｌｋ（ｔ−１）を変位量Ｆｋ（ｔ−１）に応じて変位させた状態を含む領域から、その変位させた状態に近い状態ほど高い確率で選択されるように時刻ｔの状態Ｌｋ（ｔ）を選択する（ステップＳ３）。選択された状態Ｌｋ（ｔ）は、総和計算部７に送られる。また、状態Ｌｋ（ｔ）は、状態パーティクル記憶部１に送られて、後述するＷｋ（ｔ）と共に状態パーティクルＳｋ（ｔ）＝（Ｌｋ（ｔ），Ｗｋ（ｔ））として状態パーティクル記憶部１に記憶される。ステップＳ３は、例えば以下のステップＳ３１からステップＳ３４で構成される。

制御部１５が変数ｋをｋ＝１と初期化する（ステップＳ３１）。

確率関数計算部５１は、確率関数Ｐ（Ｌｋ（ｔ）｜Ｌｋ（ｔ−１），Ｆｋ（ｔ−１））を計算する（ステップＳ３２）。

確率関数Ｐ（Ｌｋ（ｔ）｜Ｌｋ（ｔ−１），Ｆｋ（ｔ−１））は正規分布を使用して、以下のように定義することができる。ここで、Ｌｋ（ｔ）＝（Ｘｇｋ（ｔ），Ｙｇｋ（ｔ），Ｚｇｋ（ｔ），Φｇｋ（ｔ））とし、Ｆｋ（ｔ−１）＝（Ｆｘｋ（ｔ−１），Ｆｙｋ（ｔ−１），Ｆｚｋ（ｔ−１），ＦΦｋ（ｔ−１））とする。

P(Lk(t)|Lk(t-1),Fk(t-1))=p(Xgk(t)|Xgk(t-1),Fxk(t-1))・p(Ygk(t)|Ygk(t-1),Fyk(t-1))・p(Zgk(t)|Zgk(t-1),Fzk(t-1))・p(Φgk(t)|Φgk(t-1),FΦk(t-1))
p(Xgk(t)|Xgk(t-1),Fxk(t-1))=(1/((2π)^1/2・σx))・exp(-Rx/2σx²)
p(Ygk(t)|Ygk(t-1),Fyk(t-1))=(1/((2π)^1/2・σy))・exp(-Ry/2σy²)
p(Zgk(t)|Zgk(t-1),Fzk(t-1))=(1/((2π)^1/2・σz))・exp(-Rz/2σz²)
p(Φgk(t)|Φgk(t-1),FΦk(t-1))=(1/((2π)^1/2・σΦ))・exp(-RΦ/2σΦ²)
Rx=(Xgk(t)+(Xgk(t-1)+Fxk(t-1)))・(Xgk(t)-(Xgk(t-1)+Fxk(t-1)))
Ry=(Ygk(t)+(Ygk(t-1)+Fyk(t-1)))・(Ygk(t)-(Ygk(t-1)+Fyk(t-1)))
Rz=(Zgk(t)+(Zgk(t-1)+Fzk(t-1)))・(Zgk(t)-(Zgk(t-1)+Fzk(t-1)))
RΦ=(Φgk(t)+(Φgk(t-1)+FΦk(t-1)))・(Φgk(t)-(Φgk(t-1)+FΦk(t-1)))

ここで、σｘ、σｙ、σｚ、σΦは、一時間ステップの間にどれだけ変位量が変化し得るかを表す指標となっている。ここでは、自律移動ロボットが変位量Ｆｋ（ｔ−１）で変位していると推定しているので、その変位量Ｆｋ（ｔ−１）だけ変位した自律移動ロボットの状態と、実際に位置計測センサで計測された自律移動ロボットの状態がどの程度異なり得るかを各σｘ、σｙ、σｚ、σΦで表している。各σｘ、σｙ、σｚ、σΦの値が大きければそれだけ変位量のずれが大きい可能性が高く、逆にそれらの値が小さければそれだけ変位量のずれが小さい可能性が高いことを示している。各σｘ、σｙ、σｚ、σΦは、自律移動ロボットの加速度の大きさを反映するものであるから、
Ｖｘ（ｔ＋１）＝ａｘＶｘ（ｔ）＋ｂｘＶｏｌｔｘ^２
Ｖｙ（ｔ＋１）＝ａｙＶｙ（ｔ）＋ｂｙＶｏｌｔｙ^２
Ｖｚ（ｔ＋１）＝ａｚＶｚ（ｔ）＋ｂｚＶｏｌｔｚ^２
ＶΦ（ｔ＋１）＝ａΦＶΦ（ｔ）＋ｂΦＶｏｌｔΦ^２
という関係を利用して、簡単な運動モデルを用いて以下のように計算される。

σx=cos(Φ(t-1)+VΦ(t-1))・Ax+sin(Φ(t-1)+VΦ(t-1))・Ay
σy=-sin(Φ(t-1)+VΦ(t-1))・Ax+cos(Φ(t-1)+VΦ(t-1))・Ay
Ax=KxMx(Vx(t)-Vx(t-1))=KxMx(axVx(t-1)+bxVoltx²-Vx(t-1))=KxMx((ax-1)Vx(t-1)+bxVoltx²)
Ay=KyMy(Vy(t)-Vy(t-1))=KyMy(ayVy(t-1)+byVolty²-Vy(t-1))=KyMy((ay-1)Vy(t-1)+byVolty²)
σz=KzMz(Vz(t)-Vz(t-1))=KzMz(azVz(t-1)+bzVoltz²-Vz(t-1))=KzMz((az-1)Vz(t-1)+bzVoltz²)
σΦ=KΦMΦ(VΦ(t)-VΦ(t-1))=KΦMΦ(aΦVΦ(t-1)+bΦVoltΦ²-VΦ(t-1))=KΦMΦ((aΦ-1)VΦ(t-1)+bΦVoltΦ²)
Ｍｘ、Ｍｙ、Ｍｚ、ＭΦは入力が０のときに最小値を取る広義単調増加関数である。ここで、広義単調増加関数とは、ｘ_１≦ｘ_２ならば、ｆ（ｘ_１）≦ｆ（ｘ_２）となる関数ｆのことである。Ｋｘ、Ｋｙ、Ｋｚ、ＫΦは予め定められた定数であり、例えば１である。

Ｖｘ（ｔ），Ｖｙ（ｔ），Ｖｚ（ｔ），ＶΦ（ｔ）はそれぞれ自律移動ロボットの機体座標における前進方向の速さ、横方向の速さ、上下方向の速さ、旋回速度である。ａｘ，ｂｘ，ａｙ，ｂｙ，ａｚ，ｂｚ，ａΦ，ｂΦは、適宜定められる運動モデルの安定微係数である。Ｖｏｌｔｘ，Ｖｏｌｔｙ，Ｖｏｌｔｚ，ＶｏｌｔΦはそれぞれ前進方向、横方向、上下方向、旋回方向に力を発生するアクチュエータに与えられる電圧値である。アクチュエータが例えばＤＣモータにプロペラを装備したものである場合、それが発生する力とモーメントは、電圧値の二乗の次元を持つので、運動モデル内では二乗して使用されている。もちろん、電圧値の代わりに、直接アクチュエータの発生する力を使用して運動モデルを構築してもよい。自律移動ロボットに横方向アクチュエータが装備されていないならば、横方向の運動モデルは扱わずに、横方向の移動速度はすべて風外乱によるものであると仮定してもよい。安定微係数は、大まかな値を設定しておけばよい。

状態変位部５は、確率関数Ｐ（Ｌｋ（ｔ）｜Ｌｋ（ｔ−１），Ｆｋ（ｔ−１））の値に比例した確率に従って、Ｌｋ（ｔ）を選択する（ステップＳ３３）。

制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ３４）。ｋ＝Ｍであれば、ステップＳ３を終えてステップＳ４に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ３５）、ステップＳ３２に戻る。

＜ステップＳ４＞
総和計算部７は、自律移動ロボットが状態Ｌｋ（ｔ）である場合に計測値Ｚ（ｔ）が計測される確率Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））に予め定められた一時重みＷ’ｋ（ｔ）をかけた値の和α（ｔ）＝Σ_ｋ＝１ ^ＭＰ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を計算する（ステップＳ４）。

一時重みＷ’ｋ（ｔ）は例えば１／Ｍである。

Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））は、例えば状態Ｌｋ（ｔ）＝（Ｘｇｋ（ｔ），Ｙｇｋ（ｔ），Ｚｇｋ（ｔ），Φｇｋ（ｔ））における位置計測センサ６ｉ（ｉ＝１，…，５）の位置（Ｘｋｓｉ（ｔ），Ｙｋｓｉ（ｔ），Ｚｋｓｉ（ｔ））と、位置計測センサ６ｉの実際の計測値（Ｚｘｉ（ｔ），Ｚｙｉ（ｔ），Ｚｚｉ（ｔ））との距離が小さいほどＰ（Ｚ（ｔ）｜Ｌｋ（ｔ））が大きくなるように定められる。

Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））＝Ｐ（Ｚ１（ｔ），Ｚ２（ｔ），Ｚ３（ｔ），Ｚ４（ｔ），Ｚ５（ｔ）｜Ｌｋ（ｔ））の計算は、自律移動ロボットが状態Ｌｋ（ｔ）である場合に位置計測センサ６ｉにおいて計測値Ｚｉ（ｔ）が計測される確率Ｐ（Ｚｉ（ｔ）｜Ｌｋ（ｔ））を用いて、例えば以下のように行う。

Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））＝Π_ｉ＝１ ^５Ｐ（Ｚｉ（ｔ）｜Ｌｋ（ｔ））
ここで、状態Ｌｋ（ｔ）において、Ｐ（Ｚｉ（ｔ）｜Ｌｋ（ｔ））は、正規分布を用いて以下のように計算することができる。σｓは位置計測センサ６１，…，６５の計測精度に応じて決定される。計測精度が高いほどσｓは小さくなる。

Ｐ（Ｚｉ（ｔ）｜Ｌｋ（ｔ））＝（１／（（２π）^１／２・σｓ））・ｅｘｐ（−ｒ／２σｓ^２）
ｒ＝（Ｚｘｉ（ｔ）−Ｘｋｓｉ（ｔ））^２＋（Ｚｙｉ（ｔ）−Ｙｋｓｉ（ｔ））^２＋（Ｚｚｉ（ｔ）−Ｚｋｓｉ（ｔ））^２
なお、状態Ｌｋ（ｔ）＝（Ｘｇｋ（ｔ），Ｙｇｋ（ｔ），Ｚｇｋ（ｔ），Φｇｋ（ｔ））における位置計測センサ６ｉ（ｉ＝１，…，５）の位置（Ｘｋｓｉ（ｔ），Ｙｋｓｉ（ｔ），Ｚｋｓｉ（ｔ））は、Ｌｋ（ｔ）から次のようにして求められる。

Ｘｋｓｉ（ｔ）＝Ｘｇｋ（ｔ）＋Ｒｓｉ・ｃｏｓ（Φｇｋ（ｔ）＋Φｓｉ）
Ｙｋｓｉ（ｔ）＝Ｙｇｋ（ｔ）＋Ｒｓｉ・ｓｉｎ（Φｇｋ（ｔ）＋Φｓｉ）
Ｚｋｓｉ（ｔ）＝Ｚｇｋ（ｔ）

＜ステップＳ５＞
状態重み計算部８は、Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を和α（ｔ）で割った値を計算して、時刻ｔの状態Ｌｋ（ｔ）に対する重みＷｋ（ｔ）とする（ステップＳ５）。

生成された重みＷｋ（ｔ）は、状態パーティクル記憶部１に送られて、状態Ｌｋ（ｔ）と共に状態パーティクルＳｋ（ｔ）＝（Ｌｋ（ｔ），Ｗｋ（ｔ））として状態パーティクル記憶部１に記憶される。

＜ステップＳ６＞
変位量計算部９は、生成された時刻ｔの状態パーティクルＳｉ（ｔ）の状態Ｌｉ（ｔ）と、状態パーティクル記憶部１に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）の状態Ｌｉ（ｔ−１）と、変位量パーティクル記憶部２に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の変位量Ｆｉ（ｔ−１）のうち少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の変位量Ｆｉ（ｔ）を計算する。計算された変位量Ｆｉ（ｔ）は、変位量パーティクル記憶部２に送られて、後述するＷＦｉ（ｔ）と共に変位量パーティクルＳＦｉ（ｔ）＝（Ｆｉ（ｔ），ＷＦｉ（ｔ））として変位量パーティクル記憶部２に記憶される。

変位量計算部９は、図２に例示するように、状態パーティクル選択部９１、変位量パーティクル選択部９２及び合成部９３を含む。変位量計算部９は、例えば図７に例示するステップＳ６１からステップＳ６７により変位量Ｆｉ（ｔ）を計算する。

制御部１５が変数ｋをｋ＝１と初期化する（ステップＳ６１）。

状態パーティクル選択部９１は、時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）から重みの値に比例した確率に従って１つの状態パーティクルを選択してＳｋ（ｔ−１）とする（ステップＳ６２）。選択された状態パーティクルＳｋ（ｔ−１）は、合成部９３及び変位量重み計算部１０に送られる。

また、状態パーティクル選択部９１は、時刻ｔの状態パーティクルＳｉ（ｔ）から重みの値に比例した確率に従って１つの状態パーティクルを選択してＳｋ（ｔ）とする（ステップＳ６３）。選択された状態パーティクルＳｋ（ｔ）は、合成部９３及び変位量重み計算部１０に送られる。

変位量パーティクル選択部９２は、時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）から重みの値に比例した確率に従って１つの変位量パーティクルを選択してＳＦｋ（ｔ−１）とする（ステップＳ６４）。選択された変位量パーティクルＳＦｋ（ｔ−１）は、合成部９３及び変位量重み計算部１０に送られる。

合成部９３は、Ｓｋ（ｔ）の状態Ｌｋ（ｔ）からＳｋ（ｔ−１）の状態Ｌｋ（ｔ−１）を減算したＬｋ（ｔ）−Ｌｋ（ｔ−１）と、ＳＦｋ（ｔ−１）の変位量Ｆｋ（ｔ−１）とを合成して、時刻ｔの変位量パーティクルＳＦｋ（ｔ）の変位量Ｆｋ（ｔ）を計算する（ステップＳ６５）。例えば、以下の式に従って変位量Ｆｋ（ｔ）を計算する。Ａは、ローパスフィルタの係数であり、０＜Ａ≦１である。

Ｆｋ（ｔ）＝Ａ＊（Ｌｋ（ｔ）−Ｌｋ（ｔ−１））＋（１−Ａ）＊Ｆｋ（ｔ−１）
制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ６６）。ｋ＝Ｍであれば、ステップＳ６を終えてステップＳ７に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ６７）、ステップＳ６２に戻る。

＜ステップＳ７＞
変位量重み計算部１０は、生成された時刻ｔの状態パーティクルＳｉ（ｔ）の重みＷｉ（ｔ）と、状態パーティクル記憶部１に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）の重みＷｉ（ｔ−１）と、変位量パーティクル記憶部２に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の重みＷＦｉ（ｔ−１）の少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の重みＷＦｉ（ｔ）を計算する（ステップＳ７）。生成された重みＷＦｉ（ｔ）は、変位量パーティクル記憶部２に送られて、Ｆｉ（ｔ）と共に変位量パーティクルＳＦｉ（ｔ）＝（Ｆｉ（ｔ），ＷＦｉ（ｔ））として変位量パーティクル記憶部２に記憶される。

変位量重み計算部１０は、図３に例示するように、合成部１０１及び正規化部１０２を含む。ステップＳ７は、変位量重み計算部１０は、例えば図８に例示するステップＳ６１からステップＳ６７により変位量Ｆｉ（ｔ）を計算する。

制御部１５が変数ｋをｋ＝１と初期化する（ステップＳ７１）。

合成部１０１は、Ｓｋ（ｔ−１）の重みＷｉ（ｔ−１）と、Ｓｋ（ｔ）の重みＷｉ（ｔ）と、ＳＦｋ（ｔ−１）の重みＷＦｉ（ｔ−１）とを広義単調増加関数Ｆ（Ｗｉ（ｔ−１），Ｗｉ（ｔ），ＷＦｉ（ｔ−１））に入力した値を計算して、仮重みＷＦｉ’（ｔ）とする（ステップ）。例えば、関数Ｆは以下のように表される。

Ｆ（Ｗｉ（ｔ−１），Ｗｉ（ｔ），ＷＦｉ（ｔ−１））＝Ｗｉ（ｔ−１）＊Ｗｉ（ｔ）＊ＷＦｉ（ｔ−１）

制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ７３）。ｋ＝Ｍであれば、ステップＳ７５に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ７４）、ステップＳ７２に戻る。

正規化部１０２は、αｆ＝Σ_ｋ＝１ ^ＭＷＦｋ’（ｔ）を計算する（ステップＳ７５）。

制御部１５は、変数ｋをｋ＝１と初期化する（ステップＳ７６）。

正規化部１０２は、ＷＦｋ（ｔ）＝ＷＦｋ’（ｔ）／αｆで定義される重みＷＦｋ（ｔ）を計算する（ステップＳ７７）。

制御部１５は、ｋ＝Ｍかどうか判定する（ステップＳ７８）。ｋ＝Ｍであれば、ステップＳ８に進む。ｋ＝Ｍでなければ、変数ｋを１だけインクリメントしてｋ＝ｋ＋１として（ステップＳ７９）、ステップＳ７７に戻る。

＜ステップＳ８＞
状態推定部１１は、状態Ｌｋ（ｔ）の平均値を求めて、自律移動ロボットの状態の推定値とする。つまり、状態Ｌｋ（ｔ）を重みＷｋ（ｔ）で重み付き加算して自律移動ロボットの状態の推定値を求める（ステップＳ８）。

すなわち、自律移動ロボットの世界座標における時刻ｔのｘ方向の位置Ｘｇ（ｔ）、ｙ方向の位置Ｙｇ（ｔ）、ｚ方向の位置Ｚｇ（ｔ）、方位角Φｇ（ｔ）は、次のようにして求めることができる。

Ｘｇ（ｔ）＝Σ_ｋ＝１ ^ＭＷｋ（ｔ）Ｘｇｋ（ｔ）
Ｙｇ（ｔ）＝Σ_ｋ＝１ ^ＭＷｋ（ｔ）Ｙｇｋ（ｔ）
Ｚｇ（ｔ）＝Σ_ｋ＝１ ^ＭＷｋ（ｔ）Ｚｇｋ（ｔ）
Φｇ（ｔ）＝Σ_ｋ＝１ ^ＭＷｋ（ｔ）Φｇｋ（ｔ）

＜ステップＳ９＞
変位量推定部１４は、変位量Ｆｋ（ｔ）＝（Ｆｉｘ（ｔ），Ｆｉｙ（ｔ），Ｆｉｚ（ｔ），ＦｉΦ（ｔ））の平均値を求めて、自律移動ロボットの状態の推定値とする。つまり、変位量Ｆｋ（ｔ）を重みＷＦｋ（ｔ）で重み付き加算して自律移動ロボットの変位量の推定値を求める（ステップＳ９）。

すなわち、自律移動ロボットの世界座標における時刻ｔのｘ方向の変位量Ｆｘ（ｔ）、ｙ方向の変位量Ｆｙ（ｔ）、ｚ方向の変位量Ｆｚ（ｔ），旋回量ＦΦ（ｔ）は、次のようにして求めることができる。

Ｆｘ（ｔ）＝Σ_ｋ＝１ ^ＭＷＦｋ（ｔ）Ｆｋｘ（ｔ）
Ｆｙ（ｔ）＝Σ_ｋ＝１ ^ＭＷＦｋ（ｔ）Ｆｋｙ（ｔ）
Ｆｚ（ｔ）＝Σ_ｋ＝１ ^ＭＷＦｋ（ｔ）Ｆｋｚ（ｔ）
ＦΦ（ｔ）＝Σ_ｋ＝１ ^ＭＷＦｋ（ｔ）ＦｋΦ（ｔ）

これらは世界座標における変位量であるから、これらを機体座標値に変換し、変換後の各値を使用して、最小二乗法等の手法で、運動モデルの安定微係数及びＶｘ（ｔ），Ｖｙ（ｔ），Ｖｚ（ｔ），ＶΦ（ｔ）を更新する。更新されたＶｘ（ｔ），Ｖｙ（ｔ），Ｖｚ（ｔ），ＶΦ（ｔ）は、状態変位部５がステップＳ３において、Ｐ（Ｌｋ（ｔ）｜Ｌｋ（ｔ−１），Ｆｋ（ｔ−１））を計算するために用いられる。

このように、変位量パーティクルを用いることにより、対象物に与えられる外乱を考慮して対象物の変位量を推定することが可能となり、対象物の状態の推定を高精度に行うことができる。

なお、状態パーティクルＳｉ（ｔ）の初期値Ｓｉ（０）は、最初に設定したＭ個のパーティクルを含む領域の中に位置計測センサ６１，６２，６３，６４，６５の計測値が含まれているように設定することが望ましい。例えば、以下のように設定される。

時刻ｔ＝０における位置計測センサ６１，６２，６３，６４，６５の計測値の平均値（Ｘｇ，Ｙｇ，Ｚｇ）を中心とし（点対象に位置センサを配置しているので、平均を取るだけで重心位置がでる。）、位置計測センサ６１，６２，６３，６４，６５の計測誤差をＥｓとしたときに、その平均値から距離Ｅｓ以内の点をランダムにＭ個取ってくる。

方位角については、範囲が０から３６０度で決まっているので、この範囲内でランダムに設定する。または、計測値の平均値（Ｘｇ，Ｙｇ，Ｚｇ）に自律移動ロボットの重心があると仮定し、各位置計測センサ６１，６２，６３，６４，６５の計測値に基づいて、方位角を推定し、その推定された方位角±Ｄ（Ｄは予め定められた角度）の範囲内でランダムに設定する。計測値に基づく方位角の推定は、自律移動ロボットの重心と各位置計測センサ６１，６２，６３，６４，６５の相対的な位置関係はわかっているので、各位置計測センサ６１，６２，６３，６４，６５の実際の計測値と、自律移動ロボットの重心を（Ｘｇ，Ｙｇ，Ｚｇ）とした場合の各位置計測センサ６１，６２，６３，６４，６５の位置との差が最小になるような方位角を計算する等が考えられる。

［変形例等］
誘拐問題の発生をより効果的に防ぐために、以下のように誘拐度Ｅを計算して、誘拐問題が発生しているかどうかを判断してもよい。

誘拐度計算部１２（図１）は、ｑを自然数として、α（ｔ），α（ｔ−１），…，α（ｔ−ｑ）についての広義単調増加関数Ｅに、α（ｔ），α（ｔ−１），…，α（ｔ−ｑ）を入力した値Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））を計算して、誘拐度とする（ステップＳ１０１、図４）。計算された誘拐度Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））は、誘拐判定部１３に送られる。

例えば、関数Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））＝α（ｔ），α（ｔ−１），…，α（ｔ−ｑ）である。割引率γ（０＜γ＜１）を用いて、関数Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））＝α（ｔ）＋Σ_ｑγ^ｑα（ｔ−１）としてもよい。

誘拐判定部１３は、誘拐度Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））と所定の閾値αｔｈとを比較する（ステップＳ１０２）。

誘拐度Ｅが閾値αｔｈよりも小さい場合には誘拐問題が発生している可能性が高いとして、状態変位部５に、先に時刻ｔの状態Ｌｋ（ｔ）を選択したよりも上記変位させた状態に遠い状態が高い確率で選択されるように、時刻ｔの状態Ｌｋ（ｔ）を再度選択させる。例えば、分散σｘ，σｙ，σｚ，σΦを大きくするために、定数Ｋｘ、Ｋｙ、Ｋｚ、ＫΦを大きな値に更新して（ステップＳ１０３）、確率関数Ｐ（Ｌｋ（ｔ）｜Ｌｋ（ｔ−１），Ｆｋ（ｔ−１））に基づいて状態Ｌｋ（ｔ）を再度選択させればよい。

このように、自律移動ロボットの変位量の想定値を大きくすることにより、自律移動ロボットのアクチュエータが与え得るよりも大きな加速度が風等の外乱によって自律移動ロボットに与えられた場合でも、誘拐問題を発生させずに状態の推定が可能となる。

α（ｔ）を基準として誘拐問題が発生しているかどうかを判断すると、実際には誘拐問題が発生していないにも関わらず、センサの計測値に一時的に大きな誤差が生じただけで、誘拐問題が発生していると判断してしまう可能性がある。α（ｔ）ではなく、過去のαを考慮した誘拐度Ｅを基準にして誘拐問題が発生しているかどうかを判断することにより、このような可能性を小さくすることができる。

変位量計算部９は、Ａを０から１の数として、Ｆｉ（ｔ）＝Ａ・Ｌｉ（ｔ）−（１−Ａ）Ｌｉ（ｔ−１）（ｉ＝１，…，Ｍ）により変位量パーティクルＳＦｉ（ｔ）を生成してもよい。この場合、変位量重み計算部１０は、重みＷＦｉ（ｔ）（ｉ＝１，…，Ｍ）を、例えばＷＦｉ（ｔ）＝Ｗｉ（ｔ）とする。

状態推定装置は、コンピュータによって実現することができる。この場合、この装置が有すべき機能の処理内容はプログラムによって記述される。そして、このプログラムをコンピュータで実行することにより、これ装置における各処理機能が、コンピュータ上で実現される。

この処理内容を記述した情報生成プログラムは、コンピュータで読み取り可能な記録媒体に記録しておくことができる。また、この形態では、コンピュータ上で所定のプログラムを実行させることにより、これらの装置を構成することとしたが、これらの処理内容の少なくとも一部をハードウェア的に実現することとしてもよい。

この発明は、上述の実施形態に限定されるものではなく、本発明の趣旨を逸脱しない範囲で適宜変更が可能である。

１ 状態パーティクル記憶部
２ 変位量パーティクル記憶部
３ 状態パーティクル選択部
４ 変位量パーティクル選択部
５ 状態変位部
５１ 確率関数計算部
６１，６２，６３，６４，６５ 位置計測センサ
７ 総和計算部
８ 状態重み計算部
９ 変位量計算部
９１ 状態パーティクル選択部
９２ 変位量パーティクル選択部
９３ 合成部
１０ 変位量重み計算部
１０１ 合成部
１０２ 正規化部
１１ 状態推定部
１２ 誘拐度計算部
１３ 誘拐判定部
１４ 変位量推定部
１５ 制御部

Claims (5)

1. 対象物の状態Ｌｉ（ｔ）とその状態Ｌｉ（ｔ）を取る可能性を示す指標である重みＷｉ（ｔ）との組を状態パーティクルＳｉ（ｔ）とし、
   対象物の状態の変位量Ｆｉ（ｔ）とその変位量Ｆｉ（ｔ）を取る可能性を示す指標である重みＷＦｉ（ｔ）との組を変位量パーティクルＳＦｉ（ｔ）とし、
   各時刻の複数の状態パーティクルを記憶する状態パーティクル記憶部と、
   各時刻の複数の変位量パーティクルを記憶する変位量パーティクル記憶部と、
   上記状態パーティクル記憶部に記憶された時刻ｔ−１の複数の状態パーティクルから重みの値に比例した確率に従って時刻ｔ−１のＭ個の状態パーティクルＳｋ（ｔ−１）（ｋ＝１，２，…，Ｍ）を選択する状態パーティクル選択部と、
   上記変位量パーティクル記憶部に記憶された時刻ｔ−１の複数の変位量パーティクルから重みの値に比例した確率に従って時刻ｔ−１のＭ個の変位量パーティクルＳＦｋ（ｔ−１）（ｋ＝１，２，…，Ｍ）を選択する変位量パーティクル選択部と、
   各状態パーティクルＳｋ（ｔ−１）の状態Ｌｋ（ｔ−１）を変位量Ｆｋ（ｔ−１）に応じて変位させた状態を含む領域から、その変位させた状態に近い状態ほど高い確率で選択されるように時刻ｔの状態Ｌｋ（ｔ）を選択する状態変位部と、
   対象物が状態Ｌｋ（ｔ）である場合に計測値Ｚ（ｔ）が計測される確率Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））に予め定められた一時重みＷ’ｋ（ｔ）をかけた値の和α（ｔ）＝Σ_ｋ＝１ ^ＭＰ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を計算する総和計算部と、
   上記Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を上記和α（ｔ）で割った値を計算して、上記時刻ｔの状態Ｌｋ（ｔ）に対する重みＷｋ（ｔ）とする状態重み計算部と、
   上記生成された時刻ｔの状態パーティクルＳｉ（ｔ）の状態Ｌｉ（ｔ）と、上記状態パーティクル記憶部に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）のＬｉ（ｔ−１）と、上記変位量パーティクル記憶部に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の変位量Ｆｉ（ｔ−１）の少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の変位量Ｆｉ（ｔ）を計算する変位量計算部と、
   上記生成された時刻ｔの状態パーティクルＳｉ（ｔ）の重みＷｉ（ｔ）と、上記状態パーティクル記憶部に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）の重みＷｉ（ｔ−１）と、上記変位量パーティクル記憶部に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の重みＷＦｉ（ｔ−１）の少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の重みＷＦｉ（ｔ）を計算する変位量重み計算部と、
   複数の状態Ｌｋ（ｔ）を重みＷｋ（ｔ）で重み付き加算して対象物の状態の推定値を求める状態推定部と、
   を含む状態推定装置。
2. 請求項１に記載された状態推定装置において、
   ｑを自然数として、α（ｔ），α（ｔ−１），…，α（ｔ−ｑ）についての広義単調増加関数Ｅに、α（ｔ），α（ｔ−１），…，α（ｔ−ｑ）を入力した出力値Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））を計算する誘拐度計算部を更に含み、
   誘拐度Ｅ（α（ｔ），α（ｔ−１），…，α（ｔ−ｑ））が予め定められた値よりも小さい場合には、上記状態変位部は、先に時刻ｔの状態Ｌｋ（ｔ）を選択したよりも上記変位させた状態に遠い状態が高い確率で選択されるように、時刻ｔの状態Ｌｋ（ｔ）を再度選択する、
   ことを特徴とする状態推定装置。
3. 対象物の状態Ｌｉ（ｔ）とその状態Ｌｉ（ｔ）を取る可能性を示す指標である重みＷｉ（ｔ）との組を状態パーティクルＳｉ（ｔ）とし、
   対象物の状態の変位量Ｆｉ（ｔ）とその変位量Ｆｉ（ｔ）を取る可能性を示す指標である重みＷＦｉ（ｔ）との組を変位量パーティクルＳＦｉ（ｔ）とし、
   状態パーティクル記憶部には、各時刻の複数の状態パーティクルが記憶され、
   変位量パーティクル記憶部には、各時刻の複数の変位量パーティクルが記憶されており、
   状態パーティクル選択部が、上記状態パーティクル記憶部に記憶された時刻ｔ−１の複数の状態パーティクルから重みの値に比例した確率に従って時刻ｔ−１のＭ個の状態パーティクルＳｋ（ｔ−１）（ｋ＝１，２，…，Ｍ）を選択する状態パーティクル選択ステップと、
   変位量パーティクル選択部が、上記変位量パーティクル記憶部に記憶された時刻ｔ−１の複数の変位量パーティクルから重みの値に比例した確率に従って時刻ｔ−１のＭ個の変位量パーティクルＳＦｋ（ｔ−１）（ｋ＝１，２，…，Ｍ）を選択する変位量パーティクル選択ステップと、
   状態変位部が、各状態パーティクルＳｋ（ｔ−１）の状態Ｌｋ（ｔ−１）を変位量Ｆｋ（ｔ−１）に応じて変位させた状態を含む領域から、その変位させた状態に近い状態ほど高い確率で選択されるように時刻ｔの状態Ｌｋ（ｔ）を選択する状態変位ステップと、
   総和計算部が、対象物が状態Ｌｋ（ｔ）である場合に計測値Ｚ（ｔ）が計測される確率Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））に予め定められた一時重みＷ’ｋ（ｔ）をかけた値の和α（ｔ）＝Σ_ｋ＝１ ^ＭＰ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を計算する総和計算ステップと、
   状態重み計算部が、上記Ｐ（Ｚ（ｔ）｜Ｌｋ（ｔ））Ｗ’ｋ（ｔ）を上記和α（ｔ）で割った値を計算して、上記時刻ｔの状態Ｌｋ（ｔ）に対する重みＷｋ（ｔ）とする状態重み計算ステップと、
   変位量計算部が、上記生成された時刻ｔの状態パーティクルＳｉ（ｔ）の状態Ｌｉ（ｔ）と、上記状態パーティクル記憶部に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）のＬｉ（ｔ−１）と、上記変位量パーティクル記憶部に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の変位量Ｆｉ（ｔ−１）の少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の変位量Ｆｉ（ｔ）を計算する変位量計算ステップと、
   変位量重み計算部が、上記生成された時刻ｔの状態パーティクルＳｉ（ｔ）の重みＷｉ（ｔ）と、上記状態パーティクル記憶部に記憶された時刻ｔ−１の状態パーティクルＳｉ（ｔ−１）の重みＷｉ（ｔ−１）と、上記変位量パーティクル記憶部に記憶された時刻ｔ−１の変位量パーティクルＳＦｉ（ｔ−１）の重みＷＦｉ（ｔ−１）の少なくとも１つを用いて、時刻ｔの変位量パーティクルＳＦｉ（ｔ）の重みＷＦｉ（ｔ）を計算する変位量重み計算ステップと、
   状態推定部が、複数の状態Ｌｋ（ｔ）を重みＷｋ（ｔ）で重み付き加算して対象物の状態の推定値を求める状態推定ステップと、
   を含む状態推定方法。
4. 請求項１又は２に記載された状態推定装置の各部としてコンピュータを機能させるための状態推定プログラム。
5. 請求項４に記載された状態推定プログラムを記録したコンピュータ読み取り可能な記録媒体。

Images