(57)【要約】【課題】  精度の高い自己位置の同定を行ないつつ、計算量を削減することが可能な状態ベクトル推定方法により制御される自立型移動体を提供する。【解決手段】  自立型移動体であるサイバーローデント１００においては、赤外線近接センサ２０４．１〜２０４．６からの観測信号に基づいて、制御部３００は、混合カルマンフィルタ法により、次タイムステップまでの観測情報の履歴が与えられた場合の状態ベクトルの事後分布を複数個のガウス関数成分からなる混合ガウス分布として算出し、この算出されたガウス関数成分により予測分布を更新しつつ、演算を繰り返して、事後分布に基づいて、状態ベクトルの期待推定量を算出する。【選択図】        図３
2007052652.tif 000002
【特許請求の範囲】
【請求項１】
対象とする物理システムの状態ベクトルが制御部からの複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される観測量から前記状態ベクトルを推定するための状態ベクトル推定方法であって、
事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成するステップと、
現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出ステップと、
次タイムステップにおいて観測センサにより前記観測量を観測する観測ステップと、
前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出ステップと、
前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出ステップと、
前記予測分布算出ステップから前記推定量算出ステップまでを、前記第１のガウス関数分布成分を前記第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返すステップとを備える、状態ベクトル推定方法。
【請求項２】
前記制御部は、前記第１および第２のガウス関数成分を格納するための記憶装置を有し、
前記事後分布を算出した後に、前記事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を前記第２のガウス関数成分として、前記記憶装置に残すステップをさらに備える、請求項１記載の状態ベクトル推定方法。
【請求項３】
前記状態ベクトルは、前記制御部が搭載される自律型移動体の自己位置である、請求項１記載の状態推定方法。
【請求項４】
自律型移動体であって、
行動命令に従って、前記自律型移動体を駆動するための駆動手段と、
前記自律型移動体の環境の観測量を獲得するための観測センサと、
前記駆動手段に対して複数の前記行動命令を出力する制御部とを備え、前記制御部は、前記自律型移動体の状態ベクトルが前記複数の行動命令と白色ガウスノイズに依存する状態遷移方程式に基づいて遷移するとの状態遷移モデルを用いて、観測される前記観測量から前記状態ベクトルを推定して、前記自律型移動体の動作を制御し、
前記制御部は、
事前に観測された校正用のデータの組の入出力特性を確率的正規化動径基底関数ネットワークに変換して観測モデルを生成する観測モデル生成手段と、
現タイムステップまでの観測情報の履歴が与えられた場合の前記状態ベクトルの条件付き確率密度関数が複数個の第１のガウス関数成分からなる第１の混合ガウス分布によって与えられるとするとき、次タイムステップの状態ベクトルの予測分布を前記状態遷移モデルに基づいて算出する予測分布算出手段と、
次タイムステップにおいて前記観測センサにより前記観測量を獲得する観測処理手段と、
前記観測モデル、前記算出された予測分布ならびに前記次タイムステップで観測された観測量に基づいて、前記次タイムステップまでの前記観測情報の履歴が与えられた場合の前記状態ベクトルの事後分布を複数個の第２のガウス関数成分からなる第２の混合ガウス分布として算出する事後分布算出手段と、
前記事後分布に基づいて、前記状態ベクトルの期待推定量を算出する推定量算出手段と、
前記予測分布算出手段、観測処理手段、事後分布算出手段および前記推定量算出手段による処理を、前記第１のガウス関数分布成分を前記第２のガウス関数成分により置換して、所定の条件が満たされるまで繰り返す反復手段とを備える、自律型移動体。
【請求項５】
前記制御部は、
前記第１および第２のガウス関数成分を格納するための記憶装置と、
前記事後分布を算出した後に、前記事後分布を構成する複数個の第２のガウス関数成分のうち、混合比の上位から所定個を前記第２のガウス関数成分として、前記記憶装置に残す選別手段とをさらに含む、請求項４記載の自律型移動体。
【請求項６】
前記状態ベクトルは、前記自律型移動体の自己位置である、請求項４記載の自律型移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態ベクトル推定方法および状態ベクトル推定方法により制御される自律型移動体に関し、より特定的には、状態ベクトルの要素としての自己位置を同定することが可能な方法および自己位置を同定することが可能な自律型移動体に関する。
【背景技術】
【０００２】
自律移動型ロボットにおいて、自己が環境に対してどのような状態にあるかを知ることは、ロボットが意志決定をする上で、重要な情報である。このような情報を推定する手法に関して多くの研究が行なわれている（たとえば、非特許文献１を参照）。
【０００３】
近年、上記のような問題に対する対処として主流となっているのは、環境を確率モデルとして定式化し、ベイズ推定法を利用して状態を推定する手法である（たとえば、非特許文献２を参照）。特に、パーティクルフィルタによるモンテカルロ位置同定法（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”は転置を表している。
【００２２】
ＣＲ１００ の行動するフィールドがスリップや摩擦などによって生じるノイズを全く含まない理想環境であることを仮定すると、状態ベクトルｘは以下の方程式にしたがって遷移する。
【００２３】
【数１】
2007052652.tif 000003
【００２４】
ここで、（ｔ）は時間ステップを指し示すインデクスである。Ａ（ｔ−１）は行動命令に依存した４×４次行列であり、以下で与えられる。
【００２５】
【数２】
2007052652.tif 000004
【００２６】
しかしながら、実環境では様々なノイズとなり得る情報源が存在する。そこで、状態遷移を撹乱させるノイズ源を４次元ベクトルξ（ｔ）で表し、これが偏りのない白色ガウスノイズにしたがうことを仮定すれば、実環境における状態遷移方程式は以下で与えられる。
【００２７】
【数３】
2007052652.tif 000005
【００２８】
ここで、Ｎp(ｘ;μ,Σ)はｐ次元平均ベクトルμとｐ×ｐ次共分散行列Σをパラメータとする状態ｘのp次元ガウス確率密度関数であり、以下で定義される。
【００２９】
【数４】
2007052652.tif 000006
【００３０】
なお、Ｑは白色ガウスノイズｖ(t)の共分散を表す４×４次のある正定値行列である。この状態遷移方程式から、前時刻ｔ−１における状態ベクトルｘ（ｔ−１）が与えられたときの現時刻ｔにおける状態ベクトルｘ（ｔ）の確率密度関数は以下のように導くことができる。
【００３１】
【数５】
2007052652.tif 000007
【００３２】
上記式（１）は状態遷移の特性を表す確率分布であるため、以下では「状態遷移モデル」と呼ぶ。
【００３３】
次にセンサの特性を表すためのモデルを構築する。
事前に用意されたセンサキャリブレーション用データセットをＤ≡｛（ｘ（ｋ），ｙ（ｋ））；ｋ＝１，…，Ｋ｝とする。ここで、ｙは状態ｘの時に観測されたセンサ値を６次元ベクトルと表したものであり、ｋはデータ順を表すインデクスである。
【００３４】
そして、データセットＤの入出力特性は以下で定義される確率的正規化動径基底関数（normalized Radial Basis Function; nRBF）ネットワークに変換される。
【００３５】
【数６】
2007052652.tif 000008
【００３６】
ここで、ｖmおよびＳmは、状態空間を滑らかに分割するための設計パラメータであり、Φm（ｘ）は、確率分布Ｐ（ｙ｜ｘ）を表現するために、ｍ番目のガウス分布成分Ｎ６（ｙ；ｗm，Ｒm）が果たすべき寄与度に対応する。言い換えると、ｖmは状態空間中の離散的格子点に対応し、Ｓmは、ユークリッド空間における最隣接格子点の二乗距離に対応する値を対角要素に代入した対角行列として設定される。ｖmは、４次元の状態空間を一様に覆い尽くすように設定し、その平均間隔となるようにＳmを設定することが望ましいが、具体的な設定方法は後で述べる。６次元ベクトルｗmおよび６×６次正定値対角行列Ｒmは、それぞれ、ＣＲ１００が状態ｖm近傍に存在するとき得られるセンサ値の平均と分散を表すパラメータである。最尤推定法に基づけば、ｗmとＲmは、データセットＤとゲイティング関数の集合｛Φm（・）|ｍ＝１，…，Ｍ｝から以下によって自動的に決定することができる。
【００３７】
【数７】
2007052652.tif 000009
【００３８】
式（２）は観測されるセンサ値の特性を表す確率分布であるため、以下では「観測モデル」と呼ぶ。
【００３９】
なお、ここでは、ＣＲ１００の自己位置推定問題に特化した状態遷移モデルや観測モデルの構築方法について述べたが、これらが、それぞれ、（１）式および（２）式で表現できるような自然現象や制御対象であれば、以下のアルゴリズムは任意のものに適用できる。
【００４０】
１−２．混合カルマンフィルタによる逐次状態推定
状態遷移モデル（１）式および観測モデル（２）式が構築されると、各時刻において直接観測できない状態ベクトルは逐次ベイズ推定法によって推定することができる。
【００４１】
今、時刻ｔまでの観測情報（センサ）の履歴Ｙ1tが与えられたときの、状態ベクトルｘ（ｔ）の条件付き確率密度関数が、以下の式（３）ようにＮ（ｔ）個のガウス関数成分からなる混合ガウス分布によって与えられているものとする。
【００４２】
【数８】
2007052652.tif 000010
【００４３】
ここで、式（３）において、ｎ番目のガウス分布成分の寄与度を表す混合比と呼ばれるパラメータは、以下のように表されている。
【００４４】
【数９】
2007052652.tif 000011
【００４５】
さらに、この混合比は、以下のような条件を満たす変数である。
【００４６】
【数１０】
2007052652.tif 000012
【００４７】
また、式（３）における他の変数の意味は以下のとおりである。
【００４８】
【数１１】
2007052652.tif 000013
【００４９】
この時、式（１）と式（３）から、観測情報の履歴Ｙ1tが与えられたときの、次時刻の状態ベクトルｘ（ｔ＋１）の予測分布は以下の式（４）で解析的に求めることができる。
【００５０】
【数１２】
2007052652.tif 000014
【００５１】
ここで、以下の関係が成り立つ。
【００５２】
【数１３】
2007052652.tif 000015
【００５３】
式（４）が計算された後、新たな情報ｙ（ｔ＋１）が観測されると、与えられた観測情報の履歴Ｙ1t+1に対する状態ベクトルx(t + 1) の事後分布は以下の式（５）で表現することができる。
【００５４】
【数１４】
2007052652.tif 000016
【００５５】
特に、任意の状態ベクトルｘに対して、以下のような近似がなりたつように、パラメータ｛（ｖm，Ｓm）|ｍ＝１，…，Ｍ｝が設定されているものとする。
【００５６】
【数１５】
2007052652.tif 000017
【００５７】
このような近似が成り立つとき、式（５）は以下の式（６）ように近似することができる。
【００５８】
【数１６】
2007052652.tif 000018
【００５９】
ここで、以下の関係が成り立つ。
【００６０】
【数１７】
2007052652.tif 000019
【００６１】
また、インデクスｎ´はｎ´＝ｎＭ＋ｍ（ｎ＝０，１，…，Ｎ（ｔ）−１；ｍ＝１，…Ｍ）が成り立つような整数である。式（５）と式（６）がいずれも混合ガウス分布のクラスに属するので、初期状態ｘ（１）に対する事前分布ｐ（ｘ（ｔ））を混合ガウス分布として与えれば、式（４）と式（６）によって構成される逐次更新式を使って、以下の事後分布ＰＴＤを得ることができる。
【００６２】
【数１８】
2007052652.tif 000020
【００６３】
この逐次計算法は、各ガウス関数成分のパラメータがカルマンフィルタアルゴリズムとによって推定されるので混合カルマンフィルタと呼ぶ。
【００６４】
［発明の実施の形態］
図３は、本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットＣＲ１００の構成の一例を示す機能ブロック図である。
【００６５】
図３を参照して、ＣＲ１００は、ＣＲ１００が追跡する目標対象物を映像として認識するための画像を撮影するカメラ２０２と、ＣＲ１００と周囲の障害物、たとえば、壁との間の距離を測定するための赤外線近接センサ２０４．１〜２０４．６と、バッテリ２２０と、バッテリ２２０から供給される電力に基づいて、ＣＲ１００の各部に供給する電力を生成する電源回路部２３０と、制御ボード（以下、「制御部」と呼ぶ）３００と、制御部３００からの制御信号によりそれぞれ駆動される右駆動モータ部２１３および左駆動モータ部２１４（以下、両者を総称する場合は「駆動モータ部」と呼ぶ）と、駆動モータ部２１３および２１４によりそれぞれトルクを与えられてＣＲ１００を移動させるための車輪２１１および２１２とを備える。
【００６６】
制御部３００では、カメラ２０２を介して入力されたデジタル画像データは、一旦、インタフェース３１０を経由して、記憶装置３０４内の画像データバッファ部に格納される。演算部３０２は、たとえば、ＣＰＵ（Central Processing Unit）であって、画像データバッファ部中のデジタル画像データを、必要に応じて、解析する。
【００６７】
また、演算部３０２は、センサ２０４．１〜２０４．６からの信号に基づいて、ＣＲ１００と周囲の障害物との間の距離を認識し、後に説明するような自己位置の同定処理を行う。さらに、演算部３０２は、駆動モータを制御するための制御信号、たとえば、駆動モータ部２１３および２１４に与える駆動トルクに対応した制御信号を計算して、制御系インタフェース３１２を介して、駆動モータ部２１３および２１４に与える。
【００６８】
なお、記憶装置３０４は、たとえば、ＲＯＭ（Read Only Memory)およびＲＡＭ（Random Access Memory）を含んでおり、ＲＯＭにはＣＲ１００の制御プログラムが記憶されており、ＲＡＭは、この制御プログラムの動作のための一時記憶メモリとして動作する。あるいは、ＲＯＭには、制御プログラムを走らせるためのＯＳ（Operating System）が記憶されており、制御プログラムは、ＲＡＭに読み込まれて実行されることとしてもよい。この場合は、ＲＡＭに読み込む制御プログラムを格納するための記憶装置（たとえば、ハードディスクや不揮発性メモリ）が、記憶装置３０４に含まれていてもよい。
【００６９】
本発明の自己位置同定方法を実行するための主要部は、制御部３００を構成するコンピュータハードウェアと、演算部３０２により実行されるソフトウェアとにより構成される。一般的にこうしたソフトウェアはメモリカード等の記憶媒体に格納されて流通させることも可能で、この場合は、ドライブ装置により記憶媒体から読取られて記憶装置３０４に一旦格納される。図３に示すようなコンピュータのハードウェア自体およびその動作原理は一般的なものである。したがって、本発明の最も本質的な部分は、記憶装置３０４に記憶されるソフトウェアである。
【００７０】
ここで、センサ２０４．１は、赤外線光測距方式のセンサであって、壁に当たった赤外線の反射量を測定する。センサ２０４．１が受光した光量をＡＤコンバータ（図示せず）でデジタル値に変換し、制御部３００が読み取る。制御部３００は、反射が多いときは壁近くにいる、反射が少ないときは壁から離れた位置にいると判断する。
【００７１】
より詳しくは、センサ２０４．１は、発光素子ＰＬと受光素子ＰＤで構成される。発光素子ＰＬには、たとえば、赤外線発光ダイオード（ＬＥＤ）が用いられる。受光素子ＰＤには、たとえば、フォトトランジスタやフォトダイオードなどが用いられる。センサ２０４．１では、たとえば、規定周期的にＬＥＤを一瞬だけ光らせ、受光側に投光側の同期と同期した反射光のみを取出すハイパスフィルタを設けることにより、周りからの外乱光の影響を受けずに距離の測定を行うことができる。なお、他のセンサ２０４．２〜２０４．６も、同様の構成を有する。また、センサ２０４．１〜２０４．６としては、ＣＲ１００と周囲の障害物との間の距離を測定できるものであれば、他の方式のセンサであってもよい。
【００７２】
図４は、図３に示したＣＲ１００の上部から見た距離測定の方向（センサの方向）を示す概念図である。
【００７３】
図４に示すように、センサ２０４．１は、ＣＲ１００の真正面をセンシング方向とし、センサ２０４．２および２０４．３は、センサ２０４．１のセンシング方向からそれぞれ左右に３０°ずつずれた方向をセンシング方向とする。センサ２０４．４および２０４．５は、ＣＲ１００のそれぞれ左側面および右側面方向をセンシング方向とし、センサ２０４．６は、ＣＲ１００の後方をセンシング方向とする。
【００７４】
［自己位置同定のための推定手法］
上述した自己位置の同定方法を実際に制御プログラムとして実装するにあたっては、以下のような工夫が必要である。
【００７５】
すなわち、式（４）と式（６）によって構成される逐次更新式の一番の問題点は、時間経過とともに事後分布を表現するためのガウス関数成分の数が指数的に増大していくことである。例えば、初期状態ｘ（１）の事前分布ｐ（ｘ（１））が単一成分からなるガウス関数によって与えられていたとしても、上記事後分布ＰＴＤは、Ｍt個のガウス関数成分からなる混合ガウス分布となる。
【００７６】
この困難さを解消するために、以下のような発見的解決法を導入する。今、制御部３００の記憶装置３００がワーキングエリアとしてＮmax個のガウス関数成分を保持できるだけのメモリ空間を有しているものとし、事後分布ＰＴＤを近似する場合を考える。この時、式（６）を計算した後、各ガウス関数成分が持つ以下の式で表される混合比の降順にｍｉｎ（ＭＮ（ｔ），Ｎmax）個のガウス関数成分のみを保持し、他の成分はメモリ空間から削除する。
【００７７】
【数１９】
2007052652.tif 000021
【００７８】
削除されるガウス関数成分が持つ混合比の和は最大でも１／Ｎmaxとなり、Ｎmaxが十分に大きければ１／Ｎmaxは、ほぼ０となる。したがって、削除されるガウス関数成分は、事後分布ＰＴＤの近似にほとんど役立たないために、このような発見論的方法が多くの実問題に対して有効に働く。
【００７９】
また、ＣＲ１００の問題の場合、観測情報の履歴Ｙ1tから良い行動選択を推論する際には、現在の状態ｘ（ｔ）を式（６）のように確率分布として表現するよりも一つの値として表現するほうが容易に扱うことができる。この場合には、事後分布（６）の下での以下の式で表現されるｘ（ｔ）の期待値によって、ｘ（ｔ）の期待推定値を表現することができる。
【００８０】
【数２０】
2007052652.tif 000022
【００８１】
（自己位置同定の手順）
（初期設定）
図５〜７は、以上の説明した方法により、ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示すフローチャートである。
【００８２】
まず、図５を参照して、自己位置同定処理の初期処理として、ユーザからの指示によりキャリブレーション用に収集するデータ数Ｋを設定する（Ｓ１００）。
【００８３】
続いて、ＣＲ１００がフィールド上を移動し、キャリブレーション用データＤを収集する（Ｓ１０２）。このとき、特に限定されないが、たとえば、ユーザの指示により、フィールド内で位置が既知の地点にＣＲ１００を移動させ、そのときの方向の情報をユーザからＣＲ１００に与えることとしてもよい。
【００８４】
さらに、確率的ｎＲＢＦネットワークを構成する成分数Ｍを設定するとともに、設計パラメータ（ｖm、Ｓm）の設定を実行する（Ｓ１０４）。成分数Ｍの設定は、たとえば、ユーザが実験的に事前に求めた値とすることができるし、設計パラメータについても、ユーザがフィールドの大きさにより事前に設定するものとする。
【００８５】
確率的ｎＲＢＦネットワークの残りのパラメータ｛（ｗm、Ｒm）｜ｍ＝１，…，Ｍ｝は、上述したように、以下の式にしたがって決定する。
【００８６】
【数２１】
2007052652.tif 000023
【００８７】
さらに、ユーザは、ＣＲ１００が保持するガウス関数成分の数Ｎｍａｘをメモリ空間を超えない自然数に設定する（Ｓ１０８）。
【００８８】
初期状態ｘ（１）に対する事前確率分布ｐ（ｘ（１））を以下のようにして設定する（Ｓ１１０）。
【００８９】
【数２２】
2007052652.tif 000024
【００９０】
なお、ここで「任意の」とは、「ユーザによって、条件を満たす限り、各変数の初期値としては、任意の値、または、任意の値の集合を設定することができること」を意味する。
【００９１】
以上の初期設定の後、自己位置の同定処理が行なわれる（Ｓ１１２）。
（自己位置の同定）
次に、図６および図７を参照して、図５のステップＳ１１２の処理について、さらに詳しく説明する。
【００９２】
図６を参照して、まず、演算部３０２により、タイムステップの変数ｔが１に設定される（Ｓ２００）。
【００９３】
次に、演算部３０２は、変数ｔが所定の値Ｔ以下である場合は（Ｓ２０２）、次に、変数ｔの値が２以上であるか否かを判定する（Ｓ２０４）。
【００９４】
ステップＳ２０４において、ｔ＝１であって変数ｔの値が２未満である場合は、処理をステップ処理をステップＳ２０８に移行する。一方、ステップＳ２０４において、変数ｔの値が２以上である場合は、演算部３０２は、以下の式に従って、パラメータの更新を行なう（Ｓ２０６）。
【００９５】
【数２３】
2007052652.tif 000025
【００９６】
これにより、以下のような予測分布が得られる。
【００９７】
【数２４】
2007052652.tif 000026
【００９８】
続いて、ステップＳ２０８では、演算部３０２は、センサ情報ｙ（ｔ）の観測を行ないセンサからの測定情報を受け取る（Ｓ２０８）。
【００９９】
変数ｎ′の値を０とし（Ｓ２１０）、続いて、変数ｎおよびｍの値を１とする（Ｓ２１２）。
【０１００】
さらに、演算部３０２は、変数ｎと変数Ｎ（ｔ―１）の値とを比較し（Ｓ２１４）、変数ｎが変数Ｎ（ｔ―１）以下であれば、処理はステップＳ２１６に移行し、変数ｎが変数Ｎ（ｔ―１）の値を超えていれば、処理は、ステップＳ２３０に移行する。
【０１０１】
ステップＳ２１６においては、演算部３０２は、変数ｍと所定の成分数Ｍとを比較し、変数ｍが変数Ｍ以下であれば、処理はステップＳ２２０に移行し、変数ｍが変数Ｍの値を超えていれば、変数ｍを１に再設定し、変数ｎを１だけインクリメントして（Ｓ２１８）、処理は、ステップＳ２１４に移行する。
【０１０２】
ステップＳ２２０では、変数ｎ′の値が１だけインクリメントされ、さらに、パラメータの値が以下の式により更新される（Ｓ２２２）。
【０１０３】
【数２５】
2007052652.tif 000027
【０１０４】
さらに、変数ｍの値が１だけインクリメントされて（Ｓ２２４）、処理は、ステップＳ２１６に移行する。
【０１０５】
図７を参照して、演算部３０２は、以下の通りパラメータ集合を混合比に関して降順のソートを行なう（Ｓ２３０）。
【０１０６】
【数２６】
2007052652.tif 000028
【０１０７】
さらに、変数Ｎ（ｔ）の値を以下のようにして更新する（Ｓ２３２）。
【０１０８】
【数２７】
2007052652.tif 000029
【０１０９】
次に、演算部３０２は、以下のように混合比の大きな順序でパラメータ集合の番号の付け替えを行なう。
【０１１０】
すなわち、演算部３０２は、まず、変数ｎを１に設定した後（Ｓ２３３）、変数ｎと定数Ｎ（ｔ）とを比較し（Ｓ２３４）、変数ｎが定数Ｎ（ｔ）以下である場合、以下の式に従い、パラメータ集合の番号を付け替える。
【０１１１】
【数２８】
2007052652.tif 000030
【０１１２】
その後、変数ｎの値を１だけインクリメントして（Ｓ２３８）、処理はステップＳ２３４に復帰する。
【０１１３】
ステップＳ２３４で、変数ｎが定数Ｎ（ｔ）を超える場合、このような番号の付け替えが終了して、以下の事後分布ＰＴＤが得られる（Ｓ２４０）。
【０１１４】
【数２９】
2007052652.tif 000031
【０１１５】
さらに、以下の式により、状態ベクトルｘ（ｔ）の期待推定量が得られる（Ｓ２４２）。
【０１１６】
【数３０】
2007052652.tif 000032
【０１１７】
タイムステップの変数ｔが１だけインクリメントされて、処理がステップＳ２０２に復帰する。ステップＳ２０２において、変数ｔが所定の定数Ｔを超えていれば、自己位置の同定処理が終了する。
【０１１８】
このような自己位置の同定が行えれば、ＣＲ１００の演算部３０２は、たとえば、同定された自己位置を利用して、ＣＲ１００の移動動作を制御することができる。
［計算機シミュレーション結果］
実環境を計算機上で模倣したサイバーローデントシミュレータ（Cyber Rodent Simulator）を使って、上述したような自己位置の推定法のプログラムを実装し、その性能を評価した結果について、以下に説明する。
【０１１９】
まず、各試行においてＣＲ１００の初期位置および初期進行方向をランダムにセットした。そして、２つの行動命令ForwardとRotationが等確率でランダムに選択されるものとした。ただし、壁や障害物に衝突しそうな場合には必ずRotationが選択されるものとした。選択された行動命令が実行されると、ＣＲ１００は遷移後の（未知の）状態でセンサ情報を観測し、自己位置同定アルゴリズムにしたがって現在の状態を推定した。この行動選択と状態推定の手続きを１０００時間ステップまで交互に繰り返し、１回の試行が終わるものとした。
【０１２０】
確率的ｎＲＢＦネットワークを構成するガウス関数成分の数はＭ＝１７２８とし、各パラメータは以下のようにセットした。
【０１２１】
【数３１】
2007052652.tif 000033
【０１２２】
事後分布を近似するためにＣＲ１００が保持できるガウス関数成分の最大数はＮmax＝４０に設定した。
【０１２３】
初期状態ｘ（１）に関する事前知識は全くないので、その事前分布はほぼ無情報分布となるように以下の混合ガウス分布で与えられるものとした。
【０１２４】
【数３２】
2007052652.tif 000034
【０１２５】
状態遷移の際に生じるノイズの共分散行列は、Ｑ＝diag（１，１，０．０１，０．０１）で与えられるものとした。
【０１２６】
また、本発明の自己位置の同定方法の有効性を示すために、３００個のランダムサンプル（パーティクル）を使ったＭＣＬ法もシミュレータに実装し、２つの手法の性能を比較した。
【０１２７】
以下では、本発明の自己位置同定法を混合カルマンフィルタ法（Mixture Kalman Filter based method; ＭＫＬ法）と呼ぶ。
【０１２８】
図８は、ＭＫＬ法とＭＣＬ法での位置推定誤差の時間経過を示した図である。図８では、位置に関する推定誤差ε（ｔ）を以下の式で評価し、その時間経過を示している。
【０１２９】
【数３３】
2007052652.tif 000035
【０１３０】
ここで、推定誤差ε（ｔ）の式において、以下の変数は、推定された状態ベクトルｘ（ｔ）のｉ番目の要素を表している。
【０１３１】
【数３４】
2007052652.tif 000036
【０１３２】
図９は、ＭＫＬ法とＭＣＬ法での進行方向に関する推定誤差の時間経過を示した図である。図９では、進行方向に関する推定誤差δ（ｔ）を以下の式で評価し、その時間経過を示している。
【０１３３】
【数３５】
2007052652.tif 000037
【０１３４】
ここで、以下の関係が成り立つ。
【０１３５】
【数３６】
2007052652.tif 000038
【０１３６】
ＣＲ１００に搭載されている赤外線近接センサは、たとえば、そこから２５［ｃｍ］以内にある障害物や壁を検知できるものとした。
【０１３７】
そこで、図１０は、ε（ｔ）＜２５［ｃｍ］を満たす試行の割合を自己位置推定の成功率として評価し、その時間経過を示した図である。
【０１３８】
推定をはじめたばかりの初期段階では、ＭＫＬ法はＭＣＬ法に比べて推定精度が劣っているものの、時間が経過することにしたがってその差はすぐに縮まってくる。
【０１３９】
そして、５００時間ステップ以降では、ＭＣＬ法に比べて、ＭＫＬ法の推定精度は良い平均性能を示すようになり、かつ、推定分散も小さくなることが分かる。
【０１４０】
また、インテル社製ＣＰＵ（Pentium（登録商標）４ クロック周波数：２.２ＧＨｚ，キャッシュメモリ:５１２ＫＢ)、メインメモリ:５１２ＭＢのコンピュータで計算した場合、１時間ステップあたりに要する計算時間はＭＣＬ法が、約１．１秒要するのに比べて、ＭＫＬ法は０．８秒となった。
【０１４１】
これらの結果は、本発明のＭＫＬ法が従来のＭＣＬ法に比べて効率が良く、推定精度も良い計算アルゴリズムであることを示している。
【０１４２】
今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は上記した説明ではなくて特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。
【図面の簡単な説明】
【０１４３】
【図１】ＣＲ１００の外観を示す図である。
【図２】ＣＲ１００が自律的に動き回ることができるフィールドの一例を示す図である。
【図３】本発明の自己位置同定方法を用いた制御装置が適用される自律型移動ロボットＣＲ１００の構成の一例を示す機能ブロック図である。
【図４】図３に示したＣＲ１００の上部から見た距離測定の方向（センサの方向）を示す概念図である。
【図５】ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第１のフローチャートである。
【図６】ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第２のフローチャートである。
【図７】ＣＲ１００の自己位置の同定処理を実行させるための計算手続きを示す第３のフローチャートである。
【図８】ＭＫＬ法とＭＣＬ法での位置推定誤差の時間経過を示した図である。
【図９】ＭＫＬ法とＭＣＬ法での進行方向に関する推定誤差の時間経過を示した図である。
【図１０】自己位置推定の成功率の時間経過を示した図である。
【符号の説明】
【０１４４】
１００  サイバーローデント、２０２  カメラ、２０４．１〜２０４．６  赤外線近接センサ、２１１  右車輪、２１２  左車輪、２１３  右駆動モータ部、２１４左駆動モータ部、２２０  バッテリ、２３０  電源回路部、３００  制御部、３０２  演算部、３０４  記憶装置、３１０，３１２  インタフェース。
【図３】
2007052652.tif 000039
【図５】
2007052652.tif 000040
【図６】
2007052652.tif 000041
【図７】
2007052652.tif 000042
【図１】
2007052652.tif 000043
【図２】
2007052652.tif 000044
【図４】
2007052652.tif 000045
【図８】
2007052652.tif 000046
【図９】
2007052652.tif 000047
【図１０】
2007052652.tif 000048