JP4389046B2 - 状態ベクトル推定方法および自律型移動体 - Google Patents

Description

本発明は、状態ベクトル推定方法および状態ベクトル推定方法により制御される自律型移動体に関し、より特定的には、状態ベクトルの要素としての自己位置を同定することが可能な方法および自己位置を同定することが可能な自律型移動体に関する。

自律移動型ロボットにおいて、自己が環境に対してどのような状態にあるかを知ることは、ロボットが意志決定をする上で、重要な情報である。このような情報を推定する手法に関して多くの研究が行なわれている（たとえば、非特許文献１を参照）。

近年、上記のような問題に対する対処として主流となっているのは、環境を確率モデルとして定式化し、ベイズ推定法を利用して状態を推定する手法である（たとえば、非特許文献２を参照）。特に、パーティクルフィルタによるモンテカルロ位置同定法（Monte Calro Localization：ＭＣＬ）がよく知られている（たとえば、非特許文献３を参照）。
J.Borenstein et al.: Navigating Mobile Robots : Systems and Techniques. A.K. Peters, Ltd., 1996. C.F.Olson : IEEE Trans. On Robotics and Automation, 16(1), 55-66, 2000. S. Thrun et al. : Artificial Intelligence, 128(1-2), 99-141, 2001.

上述したＭＣＬは、パーティクルと呼ばれるサンプル集合を用いて状態を推定する手法であり、ロバスト性が高く、有効な手法の１つである。

その一方で、ＭＣＬでは、精度の高い推定を行なうためには多数のサンプルが必要となり、サンプル数の増加とともに計算量が増加してしまうという問題がある。

本発明は、上記のような問題を解決するためになされたものであって、その目的は、精度の高い状態ベクトルの推定を行ないつつ、計算量を削減することが可能な状態ベクトル推定方法および状態ベクトル推定方法により制御される自律型移動体を提供することである。

この発明の他の目的は、精度の高い自己位置の同定を行ないつつ、計算量を削減することが可能な自己位置の同定方法およびこの同定方法を用いた自律型移動体を提供することである。

このような目的を達成するために、本発明の状態ベクトル推定方法は、対象とする物理システムの状態ベクトルが制御部からの複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から状態ベクトルを推定するための状態ベクトル推定方法であって、事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成するステップと、現タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を状態遷移モデルに基づいて算出する予測分布算出ステップと、次タイムステップにおいて観測センサにより観測量を観測する観測ステップと、観測モデル、算出された予測分布ならびに次タイムステップで観測された観測量に基づいて、次タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出ステップと、事後分布に基づいて、状態ベクトルの期待推定量を算出する推定量算出ステップと、予測分布算出ステップから推定量算出ステップまでを、第１のガウス関数分布成分を第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返すステップとを備える。

好ましくは、制御部は、第１および第２のガウス関数成分を格納するための記憶装置を有し、状態ベクトル推定方法は、事後分布を算出した後に、事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を第２のガウス関数成分として、記憶装置に残すステップをさらに備える。

好ましくは、状態ベクトルは、制御部が搭載される自律型移動体の自己位置である。
この発明の他の局面に従うと、自律型移動体であって、行動命令に従って、自律型移動体を駆動するための駆動手段と、自律型移動体の環境の観測量を獲得するための観測センサと、駆動手段に対して複数の行動命令を出力する制御部とを備え、制御部は、自律型移動体の状態ベクトルが複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から状態ベクトルを推定して、自律型移動体の動作を制御し、制御部は、事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成する観測モデル生成手段と、現タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を状態遷移モデルに基づいて算出する予測分布算出手段と、次タイムステップにおいて観測センサにより観測量を獲得する観測処理手段と、観測モデル、算出された予測分布ならびに次タイムステップで観測された観測量に基づいて、次タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出手段と、事後分布に基づいて、状態ベクトルの期待推定量を算出する推定量算出手段と、予測分布算出手段、観測処理手段、事後分布算出手段および推定量算出手段による処理を、第１のガウス関数分布成分を第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返す反復手段とを備える。

好ましくは、制御部は、第１および第２のガウス関数成分を格納するための記憶装置と、事後分布を算出した後に、事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を第２のガウス関数成分として、記憶装置に残す選別手段とをさらに含む。

好ましくは、状態ベクトルは、自律型移動体の自己位置である。

以下、図面を参照して本発明の実施の形態について説明する。
（本発明の概要）
以下では、本発明の状態ベクトル推定方法が適用される制御対象の例としては自立型移動体で説明を行なう。そして、自立型移動体である自律型移動ロボットの例として、サイバーローデント（Cyber Rodent：ＣＲ）を取り上げ、このＣＲの自己位置同定問題を例として、本発明に係る状態ベクトル推定方法を適用した自己位置同定方法を説明する。

図１は、このようなＣＲ１００の外観を示す図である。ＣＲ１００は、ネズミ型移動ロボットであり、外界の画像を観測するためのカメラ２０２と、移動のための車輪２１２を備える。ＣＲ１００は、所定の移動可能領域（以下、「フィールド」と呼ぶ）中を自律的に動き回ることができるものとする。

簡単さのために、ＣＲ１００の行動は、以下の２種類のマクロ行動命令によって制御されているものとする。１つは回転することなく２５［ｃｍ］前進するための行動命令であり、命令Forwardで表される。もう１つは重心位置を変化させることなく反時計回り方向にπ/6[radian] 回転するための行動命令であり、命令Rotationで表される。実際には、より複雑な複数の移動命令のセット（移動量を指定した直進移動、移動量を指定した後退移動、回転量を指定した回転運動等）により、ＣＲ１００の動作を制御することが可能である。

後に詳しく説明するように、ＣＲ１００には６つの赤外線（Infrared Radiation：IR）近接センサ搭載されており、これらのセンサを通してフィールド中に設置されている壁や障害物が近接しているかどうかを検知することができる。しかしながら、以下の説明では、ＣＲ１００の位置や進行方向を明示的に伝える天井カメラのようなセンサは、ＣＲ１００は直接利用できないものとする。その代わりに、ＣＲ１００は、各フィールド上の位置と進行方向が与えられたときに、どのようなＩＲセンサ値が得られるのかという対応関係を表すキャリブレーション用データは事前に収集できるものとする。

以上のような条件の下で、時々刻々と与えられるセンサ情報を利用して、本発明の自己位置の同定方法では、ＣＲ１００がフィールド中のどの位置でどの進行方向を向いているのかを逐次推定する。

（自己位置の同定方法の原理）
以下では、本発明の自己位置の同定方法の具体的なアルゴリズムの説明を行なう前提として、自己位置の同定方法の原理について概略説明する。

１−１．環境の確率モデル
まず、以下のように、２種類の確率分布を使ってＣＲ１００の動作環境をモデル化する。

図２は、ＣＲ１００が自律的に動き回ることができるフィールドの一例を示す図である。

図２で示されるように、フィールドの水平方向および垂直方向から計ったＣＲ１００の位置座標を、それぞれ、Ｐ_hおよびＰ_vで表し、水平方向の軸から計ったＣＲ１００の進行方向をｑ∈（-π，π]で表すものとする。そして、ＣＲ１００ の位置と進行方向から状態ベクトルｘをｘ≡（Ｐ_h, Ｐ_v, cos q, sin q)^Tによって構成する。ここで、記号“T”は転置を表している。

ＣＲ１００ の行動するフィールドがスリップや摩擦などによって生じるノイズを全く含まない理想環境であることを仮定すると、状態ベクトルｘは以下の方程式にしたがって遷移する。

ここで、（ｔ）は時間ステップを指し示すインデクスである。Ａ（ｔ−１）は行動命令に依存した４×４次行列であり、以下で与えられる。

しかしながら、実環境では様々なノイズとなり得る情報源が存在する。そこで、状態遷移を撹乱させるノイズ源を４次元ベクトルξ（ｔ）で表し、これが偏りのない白色ガウスノイズにしたがうことを仮定すれば、実環境における状態遷移方程式は以下で与えられる。

ここで、Ｎp(ｘ;μ,Σ)はｐ次元平均ベクトルμとｐ×ｐ次共分散行列Σをパラメータとする状態ｘのp次元ガウス確率密度関数であり、以下で定義される。

なお、Ｑは白色ガウスノイズｖ(t)の共分散を表す４×４次のある正定値行列である。この状態遷移方程式から、前時刻ｔ−１における状態ベクトルｘ（ｔ−１）が与えられたときの現時刻ｔにおける状態ベクトルｘ（ｔ）の確率密度関数は以下のように導くことができる。

上記式（１）は状態遷移の特性を表す確率分布であるため、以下では「状態遷移モデル」と呼ぶ。

次にセンサの特性を表すためのモデルを構築する。
事前に用意されたセンサキャリブレーション用データセットをＤ≡｛（ｘ（ｋ），ｙ（ｋ））；ｋ＝１，…，Ｋ｝とする。ここで、ｙは状態ｘの時に観測されたセンサ値を６次元ベクトルと表したものであり、ｋはデータ順を表すインデクスである。

そして、データセットＤの入出力特性は以下で定義される確率的正規化動径基底関数（normalized Radial Basis Function; nRBF）ネットワークに変換される。

ここで、ｖ_mおよびＳ_mは、状態空間を滑らかに分割するための設計パラメータであり、Φm（ｘ）は、確率分布Ｐ（ｙ｜ｘ）を表現するために、ｍ番目のガウス分布成分Ｎ_６（ｙ；ｗ_m，Ｒ_m）が果たすべき寄与度に対応する。言い換えると、ｖ_mは状態空間中の離散的格子点に対応し、Ｓ_mは、ユークリッド空間における最隣接格子点の二乗距離に対応する値を対角要素に代入した対角行列として設定される。ｖ_mは、４次元の状態空間を一様に覆い尽くすように設定し、その平均間隔となるようにＳ_mを設定することが望ましいが、具体的な設定方法は後で述べる。６次元ベクトルｗ_mおよび６×６次正定値対角行列Ｒ_mは、それぞれ、ＣＲ１００が状態ｖ_m近傍に存在するとき得られるセンサ値の平均と分散を表すパラメータである。最尤推定法に基づけば、ｗ_mとＲ_mは、データセットＤとゲイティング関数の集合｛Φm（・）|ｍ＝１，…，Ｍ｝から以下によって自動的に決定することができる。

式（２）は観測されるセンサ値の特性を表す確率分布であるため、以下では「観測モデル」と呼ぶ。

なお、ここでは、ＣＲ１００の自己位置推定問題に特化した状態遷移モデルや観測モデルの構築方法について述べたが、これらが、それぞれ、（１）式および（２）式で表現できるような自然現象や制御対象であれば、以下のアルゴリズムは任意のものに適用できる。

１−２．混合カルマンフィルタによる逐次状態推定
状態遷移モデル（１）式および観測モデル（２）式が構築されると、各時刻において直接観測できない状態ベクトルは逐次ベイズ推定法によって推定することができる。

今、時刻ｔまでの観測情報（センサ）の履歴Ｙ₁ ^tが与えられたときの、状態ベクトルｘ（ｔ）の条件付き確率密度関数が、以下の式（３）ようにＮ（ｔ）個のガウス関数成分からなる混合ガウス分布によって与えられているものとする。

ここで、式（３）において、ｎ番目のガウス分布成分の寄与度を表す混合比と呼ばれるパラメータは、以下のように表されている。

さらに、この混合比は、以下のような条件を満たす変数である。

また、式（３）における他の変数の意味は以下のとおりである。

この時、式（１）と式（３）から、観測情報の履歴Ｙ₁ ^tが与えられたときの、次時刻の状態ベクトルｘ（ｔ＋１）の予測分布は以下の式（４）で解析的に求めることができる。

ここで、以下の関係が成り立つ。

式（４）が計算された後、新たな情報ｙ（ｔ＋１）が観測されると、与えられた観測情報の履歴Ｙ₁ ^t+1に対する状態ベクトルx(t + 1) の事後分布は以下の式（５）で表現することができる。

特に、任意の状態ベクトルｘに対して、以下のような近似がなりたつように、パラメータ｛（ｖ_m，Ｓ_m）|ｍ＝１，…，Ｍ｝が設定されているものとする。

このような近似が成り立つとき、式（５）は以下の式（６）ように近似することができる。

ここで、以下の関係が成り立つ。

また、インデクスｎ´はｎ´＝ｎＭ＋ｍ（ｎ＝０，１，…，Ｎ（ｔ）−１；ｍ＝１，…Ｍ）が成り立つような整数である。式（５）と式（６）がいずれも混合ガウス分布のクラスに属するので、初期状態ｘ（１）に対する事前分布ｐ（ｘ（ｔ））を混合ガウス分布として与えれば、式（４）と式（６）によって構成される逐次更新式を使って、以下の事後分布ＰＴＤを得ることができる。

この逐次計算法は、各ガウス関数成分のパラメータがカルマンフィルタアルゴリズムとによって推定されるので混合カルマンフィルタと呼ぶ。

［発明の実施の形態］
図３は、本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットＣＲ１００の構成の一例を示す機能ブロック図である。

図３を参照して、ＣＲ１００は、ＣＲ１００が追跡する目標対象物を映像として認識するための画像を撮影するカメラ２０２と、ＣＲ１００と周囲の障害物、たとえば、壁との間の距離を測定するための赤外線近接センサ２０４．１〜２０４．６と、バッテリ２２０と、バッテリ２２０から供給される電力に基づいて、ＣＲ１００の各部に供給する電力を生成する電源回路部２３０と、制御ボード（以下、「制御部」と呼ぶ）３００と、制御部３００からの制御信号によりそれぞれ駆動される右駆動モータ部２１３および左駆動モータ部２１４（以下、両者を総称する場合は「駆動モータ部」と呼ぶ）と、駆動モータ部２１３および２１４によりそれぞれトルクを与えられてＣＲ１００を移動させるための車輪２１１および２１２とを備える。

制御部３００では、カメラ２０２を介して入力されたデジタル画像データは、一旦、インタフェース３１０を経由して、記憶装置３０４内の画像データバッファ部に格納される。演算部３０２は、たとえば、ＣＰＵ（Central Processing Unit）であって、画像データバッファ部中のデジタル画像データを、必要に応じて、解析する。

また、演算部３０２は、センサ２０４．１〜２０４．６からの信号に基づいて、ＣＲ１００と周囲の障害物との間の距離を認識し、後に説明するような自己位置の同定処理を行う。さらに、演算部３０２は、駆動モータを制御するための制御信号、たとえば、駆動モータ部２１３および２１４に与える駆動トルクに対応した制御信号を計算して、制御系インタフェース３１２を介して、駆動モータ部２１３および２１４に与える。

なお、記憶装置３０４は、たとえば、ＲＯＭ（Read Only Memory)およびＲＡＭ（Random Access Memory）を含んでおり、ＲＯＭにはＣＲ１００の制御プログラムが記憶されており、ＲＡＭは、この制御プログラムの動作のための一時記憶メモリとして動作する。あるいは、ＲＯＭには、制御プログラムを走らせるためのＯＳ（Operating System）が記憶されており、制御プログラムは、ＲＡＭに読み込まれて実行されることとしてもよい。この場合は、ＲＡＭに読み込む制御プログラムを格納するための記憶装置（たとえば、ハードディスクや不揮発性メモリ）が、記憶装置３０４に含まれていてもよい。

本発明の自己位置同定方法を実行するための主要部は、制御部３００を構成するコンピュータハードウェアと、演算部３０２により実行されるソフトウェアとにより構成される。一般的にこうしたソフトウェアはメモリカード等の記憶媒体に格納されて流通させることも可能で、この場合は、ドライブ装置により記憶媒体から読取られて記憶装置３０４に一旦格納される。図３に示すようなコンピュータのハードウェア自体およびその動作原理は一般的なものである。したがって、本発明の最も本質的な部分は、記憶装置３０４に記憶されるソフトウェアである。

ここで、センサ２０４．１は、赤外線光測距方式のセンサであって、壁に当たった赤外線の反射量を測定する。センサ２０４．１が受光した光量をＡＤコンバータ（図示せず）でデジタル値に変換し、制御部３００が読み取る。制御部３００は、反射が多いときは壁近くにいる、反射が少ないときは壁から離れた位置にいると判断する。

より詳しくは、センサ２０４．１は、発光素子ＰＬと受光素子ＰＤで構成される。発光素子ＰＬには、たとえば、赤外線発光ダイオード（ＬＥＤ）が用いられる。受光素子ＰＤには、たとえば、フォトトランジスタやフォトダイオードなどが用いられる。センサ２０４．１では、たとえば、規定周期的にＬＥＤを一瞬だけ光らせ、受光側に投光側の同期と同期した反射光のみを取出すハイパスフィルタを設けることにより、周りからの外乱光の影響を受けずに距離の測定を行うことができる。なお、他のセンサ２０４．２〜２０４．６も、同様の構成を有する。また、センサ２０４．１〜２０４．６としては、ＣＲ１００と周囲の障害物との間の距離を測定できるものであれば、他の方式のセンサであってもよい。

図４は、図３に示したＣＲ１００の上部から見た距離測定の方向（センサの方向）を示す概念図である。

図４に示すように、センサ２０４．１は、ＣＲ１００の真正面をセンシング方向とし、センサ２０４．２および２０４．３は、センサ２０４．１のセンシング方向からそれぞれ左右に３０°ずつずれた方向をセンシング方向とする。センサ２０４．４および２０４．５は、ＣＲ１００のそれぞれ左側面および右側面方向をセンシング方向とし、センサ２０４．６は、ＣＲ１００の後方をセンシング方向とする。

［自己位置同定のための推定手法］
上述した自己位置の同定方法を実際に制御プログラムとして実装するにあたっては、以下のような工夫が必要である。

すなわち、式（４）と式（６）によって構成される逐次更新式の一番の問題点は、時間経過とともに事後分布を表現するためのガウス関数成分の数が指数的に増大していくことである。例えば、初期状態ｘ（１）の事前分布ｐ（ｘ（１））が単一成分からなるガウス関数によって与えられていたとしても、上記事後分布ＰＴＤは、Ｍ^t個のガウス関数成分からなる混合ガウス分布となる。

この困難さを解消するために、以下のような発見的解決法を導入する。今、制御部３００の記憶装置３００がワーキングエリアとしてＮmax個のガウス関数成分を保持できるだけのメモリ空間を有しているものとし、事後分布ＰＴＤを近似する場合を考える。この時、式（６）を計算した後、各ガウス関数成分が持つ以下の式で表される混合比の降順にｍｉｎ（ＭＮ（ｔ），Ｎmax）個のガウス関数成分のみを保持し、他の成分はメモリ空間から削除する。

削除されるガウス関数成分が持つ混合比の和は最大でも１／Ｎmaxとなり、Ｎmaxが十分に大きければ１／Ｎmaxは、ほぼ０となる。したがって、削除されるガウス関数成分は、事後分布ＰＴＤの近似にほとんど役立たないために、このような発見論的方法が多くの実問題に対して有効に働く。

また、ＣＲ１００の問題の場合、観測情報の履歴Ｙ₁ ^tから良い行動選択を推論する際には、現在の状態ｘ（ｔ）を式（６）のように確率分布として表現するよりも一つの値として表現するほうが容易に扱うことができる。この場合には、事後分布（６）の下での以下の式で表現されるｘ（ｔ）の期待値によって、ｘ（ｔ）の期待推定値を表現することができる。

（自己位置同定の手順）
（初期設定）
図５〜７は、以上の説明した方法により、ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示すフローチャートである。

まず、図５を参照して、自己位置同定処理の初期処理として、ユーザからの指示によりキャリブレーション用に収集するデータ数Ｋを設定する（Ｓ１００）。

続いて、ＣＲ１００がフィールド上を移動し、キャリブレーション用データＤを収集する（Ｓ１０２）。このとき、特に限定されないが、たとえば、ユーザの指示により、フィールド内で位置が既知の地点にＣＲ１００を移動させ、そのときの方向の情報をユーザからＣＲ１００に与えることとしてもよい。

さらに、確率的ｎＲＢＦネットワークを構成する成分数Ｍを設定するとともに、設計パラメータ（ｖ_m、Ｓ_m）の設定を実行する（Ｓ１０４）。成分数Ｍの設定は、たとえば、ユーザが実験的に事前に求めた値とすることができるし、設計パラメータについても、ユーザがフィールドの大きさにより事前に設定するものとする。

確率的ｎＲＢＦネットワークの残りのパラメータ｛（ｗ_m、Ｒ_m）｜ｍ＝１，…，Ｍ｝は、上述したように、以下の式にしたがって決定する。

さらに、ユーザは、ＣＲ１００が保持するガウス関数成分の数Ｎｍａｘをメモリ空間を超えない自然数に設定する（Ｓ１０８）。

初期状態ｘ（１）に対する事前確率分布ｐ（ｘ（１））を以下のようにして設定する（Ｓ１１０）。

なお、ここで「任意の」とは、「ユーザによって、条件を満たす限り、各変数の初期値としては、任意の値、または、任意の値の集合を設定することができること」を意味する。

以上の初期設定の後、自己位置の同定処理が行なわれる（Ｓ１１２）。
（自己位置の同定）
次に、図６および図７を参照して、図５のステップＳ１１２の処理について、さらに詳しく説明する。

図６を参照して、まず、演算部３０２により、タイムステップの変数ｔが１に設定される（Ｓ２００）。

次に、演算部３０２は、変数ｔが所定の値Ｔ以下である場合は（Ｓ２０２）、次に、変数ｔの値が２以上であるか否かを判定する（Ｓ２０４）。

ステップＳ２０４において、ｔ＝１であって変数ｔの値が２未満である場合は、処理をステップ処理をステップＳ２０８に移行する。一方、ステップＳ２０４において、変数ｔの値が２以上である場合は、演算部３０２は、以下の式に従って、パラメータの更新を行なう（Ｓ２０６）。

これにより、以下のような予測分布が得られる。

続いて、ステップＳ２０８では、演算部３０２は、センサ情報ｙ（ｔ）の観測を行ないセンサからの測定情報を受け取る（Ｓ２０８）。

変数ｎ′の値を０とし（Ｓ２１０）、続いて、変数ｎおよびｍの値を１とする（Ｓ２１２）。

さらに、演算部３０２は、変数ｎと変数Ｎ（ｔ―１）の値とを比較し（Ｓ２１４）、変数ｎが変数Ｎ（ｔ―１）以下であれば、処理はステップＳ２１６に移行し、変数ｎが変数Ｎ（ｔ―１）の値を超えていれば、処理は、ステップＳ２３０に移行する。

ステップＳ２１６においては、演算部３０２は、変数ｍと所定の成分数Ｍとを比較し、変数ｍが変数Ｍ以下であれば、処理はステップＳ２２０に移行し、変数ｍが変数Ｍの値を超えていれば、変数ｍを１に再設定し、変数ｎを１だけインクリメントして（Ｓ２１８）、処理は、ステップＳ２１４に移行する。

ステップＳ２２０では、変数ｎ′の値が１だけインクリメントされ、さらに、パラメータの値が以下の式により更新される（Ｓ２２２）。

さらに、変数ｍの値が１だけインクリメントされて（Ｓ２２４）、処理は、ステップＳ２１６に移行する。

図７を参照して、演算部３０２は、以下の通りパラメータ集合を混合比に関して降順のソートを行なう（Ｓ２３０）。

さらに、変数Ｎ（ｔ）の値を以下のようにして更新する（Ｓ２３２）。

次に、演算部３０２は、以下のように混合比の大きな順序でパラメータ集合の番号の付け替えを行なう。

すなわち、演算部３０２は、まず、変数ｎを１に設定した後（Ｓ２３３）、変数ｎと定数Ｎ（ｔ）とを比較し（Ｓ２３４）、変数ｎが定数Ｎ（ｔ）以下である場合、以下の式に従い、パラメータ集合の番号を付け替える。

その後、変数ｎの値を１だけインクリメントして（Ｓ２３８）、処理はステップＳ２３４に復帰する。

ステップＳ２３４で、変数ｎが定数Ｎ（ｔ）を超える場合、このような番号の付け替えが終了して、以下の事後分布ＰＴＤが得られる（Ｓ２４０）。

さらに、以下の式により、状態ベクトルｘ（ｔ）の期待推定量が得られる（Ｓ２４２）。

タイムステップの変数ｔが１だけインクリメントされて、処理がステップＳ２０２に復帰する。ステップＳ２０２において、変数ｔが所定の定数Ｔを超えていれば、自己位置の同定処理が終了する。

このような自己位置の同定が行えれば、ＣＲ１００の演算部３０２は、たとえば、同定された自己位置を利用して、ＣＲ１００の移動動作を制御することができる。
［計算機シミュレーション結果］
実環境を計算機上で模倣したサイバーローデントシミュレータ（Cyber Rodent Simulator）を使って、上述したような自己位置の推定法のプログラムを実装し、その性能を評価した結果について、以下に説明する。

まず、各試行においてＣＲ１００の初期位置および初期進行方向をランダムにセットした。そして、２つの行動命令ForwardとRotationが等確率でランダムに選択されるものとした。ただし、壁や障害物に衝突しそうな場合には必ずRotationが選択されるものとした。選択された行動命令が実行されると、ＣＲ１００は遷移後の（未知の）状態でセンサ情報を観測し、自己位置同定アルゴリズムにしたがって現在の状態を推定した。この行動選択と状態推定の手続きを１０００時間ステップまで交互に繰り返し、１回の試行が終わるものとした。

確率的ｎＲＢＦネットワークを構成するガウス関数成分の数はＭ＝１７２８とし、各パラメータは以下のようにセットした。

事後分布を近似するためにＣＲ１００が保持できるガウス関数成分の最大数はＮmax＝４０に設定した。

初期状態ｘ（１）に関する事前知識は全くないので、その事前分布はほぼ無情報分布となるように以下の混合ガウス分布で与えられるものとした。

状態遷移の際に生じるノイズの共分散行列は、Ｑ＝diag（１，１，０．０１，０．０１）で与えられるものとした。

また、本発明の自己位置の同定方法の有効性を示すために、３００個のランダムサンプル（パーティクル）を使ったＭＣＬ法もシミュレータに実装し、２つの手法の性能を比較した。

以下では、本発明の自己位置同定法を混合カルマンフィルタ法（Mixture Kalman Filter based method; ＭＫＬ法）と呼ぶ。

図８は、ＭＫＬ法とＭＣＬ法での位置推定誤差の時間経過を示した図である。図８では、位置に関する推定誤差ε（ｔ）を以下の式で評価し、その時間経過を示している。

ここで、推定誤差ε（ｔ）の式において、以下の変数は、推定された状態ベクトルｘ（ｔ）のｉ番目の要素を表している。

図９は、ＭＫＬ法とＭＣＬ法での進行方向に関する推定誤差の時間経過を示した図である。図９では、進行方向に関する推定誤差δ（ｔ）を以下の式で評価し、その時間経過を示している。

ここで、以下の関係が成り立つ。

ＣＲ１００に搭載されている赤外線近接センサは、たとえば、そこから２５［ｃｍ］以内にある障害物や壁を検知できるものとした。

そこで、図１０は、ε（ｔ）＜２５［ｃｍ］を満たす試行の割合を自己位置推定の成功率として評価し、その時間経過を示した図である。

推定をはじめたばかりの初期段階では、ＭＫＬ法はＭＣＬ法に比べて推定精度が劣っているものの、時間が経過することにしたがってその差はすぐに縮まってくる。

そして、５００時間ステップ以降では、ＭＣＬ法に比べて、ＭＫＬ法の推定精度は良い平均性能を示すようになり、かつ、推定分散も小さくなることが分かる。

また、インテル社製ＣＰＵ（Pentium（登録商標）４ クロック周波数：２.２ＧＨｚ，キャッシュメモリ:５１２ＫＢ)、メインメモリ:５１２ＭＢのコンピュータで計算した場合、１時間ステップあたりに要する計算時間はＭＣＬ法が、約１．１秒要するのに比べて、ＭＫＬ法は０．８秒となった。

これらの結果は、本発明のＭＫＬ法が従来のＭＣＬ法に比べて効率が良く、推定精度も良い計算アルゴリズムであることを示している。

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。

ＣＲ１００の外観を示す図である。 ＣＲ１００が自律的に動き回ることができるフィールドの一例を示す図である。 本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットＣＲ１００の構成の一例を示す機能ブロック図である。 図３に示したＣＲ１００の上部から見た距離測定の方向（センサの方向）を示す概念図である。 ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第１のフローチャートである。 ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第２のフローチャートである。 ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第３のフローチャートである。 ＭＫＬ法とＭＣＬ法での位置推定誤差の時間経過を示した図である。 ＭＫＬ法とＭＣＬ法での進行方向に関する推定誤差の時間経過を示した図である。 自己位置推定の成功率の時間経過を示した図である。

符号の説明

１００ サイバーローデント、２０２ カメラ、２０４．１〜２０４．６ 赤外線近接センサ、２１１ 右車輪、２１２ 左車輪、２１３ 右駆動モータ部、２１４左駆動モータ部、２２０ バッテリ、２３０ 電源回路部、３００ 制御部、３０２ 演算部、３０４ 記憶装置、３１０，３１２ インタフェース。

Claims (6)

1. 対象とする物理システムの状態ベクトルが制御部からの複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から前記状態ベクトルを推定するための状態ベクトル推定方法であって、
   事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成するステップと、
   現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出ステップと、
   次タイムステップにおいて観測センサにより前記観測量を観測する観測ステップと、
   前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出ステップと、
   前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出ステップと、
   前記予測分布算出ステップから前記推定量算出ステップまでを、前記第１のガウス関数分布成分を前記第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返すステップとを備える、状態ベクトル推定方法。
2. 前記制御部は、前記第１および第２のガウス関数成分を格納するための記憶装置を有し、
   前記事後分布を算出した後に、前記事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を前記第２のガウス関数成分として、前記記憶装置に残すステップをさらに備える、請求項１記載の状態ベクトル推定方法。
3. 前記状態ベクトルは、前記制御部が搭載される自律型移動体の自己位置である、請求項１記載の状態推定方法。
4. 自律型移動体であって、
   行動命令に従って、前記自律型移動体を駆動するための駆動手段と、
   前記自律型移動体の環境の観測量を獲得するための観測センサと、
   前記駆動手段に対して複数の前記行動命令を出力する制御部とを備え、前記制御部は、前記自律型移動体の状態ベクトルが前記複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される前記観測量から前記状態ベクトルを推定して、前記自律型移動体の動作を制御し、
   前記制御部は、
   事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成する観測モデル生成手段と、
   現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出手段と、
   次タイムステップにおいて前記観測センサにより前記観測量を獲得する観測処理手段と、
   前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出手段と、
   前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出手段と、
   前記予測分布算出手段、観測処理手段、事後分布算出手段および前記推定量算出手段による処理を、前記第１のガウス関数分布成分を前記第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返す反復手段とを備える、自律型移動体。
5. 前記制御部は、
   前記第１および第２のガウス関数成分を格納するための記憶装置と、
   前記事後分布を算出した後に、前記事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を前記第２のガウス関数成分として、前記記憶装置に残す選別手段とをさらに含む、請求項４記載の自律型移動体。
6. 前記状態ベクトルは、前記自律型移動体の自己位置である、請求項４記載の自律型移動体。