(57)【特許請求の範囲】
【請求項１】
対象物の状態を表わす第１状態変数の値を測定し、第２状態変数により定義された状態変数空間において複数のパーティクルのうち少なくとも一部のパーティクルを前回位置の周辺に分散させ、前記複数のパーティクルのそれぞれを複数のモデルのうち各パーティクルに固有の流動的に変化する今回モデルにしたがって今回位置に遷移させた上で、前記第１状態変数の今回測定値に対する前記各パーティクルの尤度を評価し、前記尤度に基づいて前記対象物の今回状態を推定する第１要素と、
前記第１要素により評価された前記尤度が高い前記パーティクルを優先的に生存または増加させる一方、前記第１要素により評価された前記尤度が低い前記パーティクルを優先的に消滅させた後、前記各パーティクルに固有の前記今回モデルを定めるモデル変数の今回値に所定の摂動量を加算又は減算することにより当該モデル変数の次回値を決定し、前記複数のモデルのそれぞれに対応して定義されている複数の区域のうち、前記モデル変数の次回値が属する一の区域に応じて前記各パーティクルに固有の次回モデルを決定する第２要素とを備えていることを特徴とする状態推定装置。
【請求項２】
請求項１記載の状態推定装置において、
前記第２要素が前記モデル変数の前記摂動量が許容範囲に収まるように前記次回値を決定することを特徴とする状態推定装置。
【請求項３】
請求項１記載の状態推定装置において、
前記第１要素が、前記尤度が最高となるパーティクルに固有の今回モデル、または、前記尤度の総和が最高となるパーティクルに共通の今回モデルを、前記対象物の前回の状態遷移モデルとして推定することを特徴とする状態推定装置。
【請求項４】
請求項１記載の状態推定装置において、
前記第１要素が、前記各パーティクルの今回位置の前記尤度を重みとする重み付き平均値を前記対象物の前記第２状態変数の今回値として推定することを特徴とする状態推定装置。
【請求項５】
コンピュータを請求項１記載の状態推定装置として機能させることを特徴とする状態推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、パーティクルフィルタを用いて対象物の状態を推定する装置等に関する。
【背景技術】
【０００２】
パーティクルフィルタを用いた対象物の追跡精度を向上させるため、この対象物が有する複数の運動特性のそれぞれに適合する複数の運動モデルをパーティクルの集合全体に対して用意し、これら複数の運動モデルを適応的に切り替える手法が提案されている（特許文献１参照）。
【特許文献１】特開２００７−３２８７４６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、各時刻において各パーティクルが複数のモデルのそれぞれにしたがって変位させられた上で観測結果に対する尤度が算出され、当該複数のモデルのうち尤度が高いパーティクル群が有する１つのモデルが選択されることにより対象物が追跡されている。すなわち、各時刻においてパーティクルの数にモデルの数を掛け合わせた回数だけ観測結果に対する尤度が算出される。このため、対象物の追跡に要する演算処理量が大きくなってしまい非効率的である。その一方、演算処理量の低減のためにパーティクルの数を減らすと対象物の追跡精度が低下する可能性がある。
【０００４】
そこで、本発明は、対象物の状態を効率的にかつ高精度で推定することができる装置等を提供することを解決課題とする。
【課題を解決するための手段】
【０００５】
本発明の状態推定装置は、対象物の状態を表わす第１状態変数の値を測定し、第２状態変数により定義された状態変数空間において複数のパーティクルのうち少なくとも一部のパーティクルを前回位置の周辺に分散させ、前記複数のパーティクルのそれぞれを複数のモデルのうち各パーティクルに固有の流動的に変化する今回モデルにしたがって今回位置に遷移させた上で、前記第１状態変数の今回測定値に対する前記各パーティクルの尤度を評価し、前記尤度に基づいて前記対象物の今回状態を推定する第１要素と、前記第１要素により評価された前記尤度が高い前記パーティクルを優先的に生存または増加させる一方、前記第１要素により評価された前記尤度が低い前記パーティクルを優先的に消滅させた後、前記各パーティクルに固有の前記今回モデルを定めるモデル変数の今回値に所定の摂動量を加算又は減算することにより当該モデル変数の次回値を決定し、前記複数のモデルのそれぞれに対応して定義されている複数の区域のうち、前記モデル変数の次回値が属する一の区域に応じて前記各パーティクルに固有の次回モデルを決定する第２要素とを備えていることを特徴とする。
【０００６】
第１発明の状態推定装置によれば、対象物の状態推定に際して、第２状態変数により定義される状態変数空間において（１）各パーティクルがその固有の今回モデルにしたがって今回位置に遷移し、（２）第１状態変数の今回測定値に対する各パーティクルの尤度が評価される。このため、（１’）各パーティクルが複数のモデルのそれぞれにしたがって遷移し、（２’）第１状態変数の測定値に対する各パーティクルの尤度が評価される場合よりも、対象物の推定に要する装置の演算処理負荷が軽減される。なお、第１状態変数および第２状態変数は同一であっても異なっていてもよい。第２状態変数が第１状態変数とは異なって測定されない隠れ変数であってもよい。
【０００７】
また、第１状態変数の今回測定値に対する尤度が高いパーティクルほど優先的に生存または増加する一方、当該尤度が低いパーティクルほど優先的に消滅する。このため、対象物がしたがっている蓋然性が高い状態遷移モデル（以下、適宜「高尤度モデル」という。）にしたがって状態変数空間で遷移したパーティクルが徐々に増加傾向となる。さらに、パーティクルに固有の今回モデルを基礎として次回モデルが決定される。このため、高尤度モデルを次回モデルとして状態変数空間で遷移するパーティクルが増加傾向となる。その一方、モデルは流動的に変化するため、今回モデルを基礎としながらもこの今回モデルとは異なるモデルが次回モデルとして決定される余地がある。このため、高尤度モデルを固有のモデルとするパーティクルの増加に歯止めをかけるように、高尤度モデルとは異なるモデル（以下、適宜「低尤度モデル」という。）を固有のモデルとするパーティクルも少数派ながら存在することになる。これにより、対象物がそれまでとは異なるモデルにしたがって状態遷移を開始した場合でも、当該モデルの変化に柔軟に応じて過去の低尤度モデルを将来の高尤度モデルに切り替えながらこの対象物の状態が高精度で推定されうる。すなわち、対象物の状態を効率的にかつ高精度で推定することができる。
【０００９】
さらに、各パーティクルに固有のモデル変数の値を流動的に変動させることにより、各パーティクルに固有のモデルを流動的に変動させることができる。これにより、前記のように対象物がそれまでとは異なるモデルにしたがって状態遷移を開始した場合でも、当該モデルの変化に柔軟に応じて過去の低尤度モデルを将来の高尤度モデルに切り替えながらこの対象物の状態が効率的にかつ高精度で推定されうる。
【００１０】
本発明の状態推定装置において、前記第２要素が前記モデル変数の今回値から次回値への摂動量が許容範囲に収まるように前記次回値を変化させることが好ましい。
【００１１】
当該構成の状態推定装置によれば、モデル変数の値を摂動させることにより前回モデルと異なるモデルが今回モデルとして決定される余地を残しつつも、その値の摂動量を許容範囲に収めることにより生存したパーティクルに固有の前回モデルをある程度は踏襲する形で今回モデルが決定されうる。これにより、対象物が高尤度モデルにしたがって状態遷移している蓋然性が高いという事情を尊重しつつこの対象物の状態が高精度で推定されうる。
【００１２】
本発明の状態推定装置において、前記第１要素が、前記尤度が最高となるパーティクルに固有の今回モデル、または、前記尤度の総和が最高となるパーティクルに共通の今回モデルを、前記対象物の前回の状態遷移モデルとして推定することが好ましい。
【００１３】
当該構成の状態推定装置によれば、尤度が最高となるパーティクルに固有の今回モデルが対象物の前回の状態遷移モデルとして推定される。あるいは、前記のようにパーティクルがその尤度が高いほど増加する傾向にあることに鑑みて、尤度の総和が最高となるパーティクルに共通の今回モデルが、対象物の前回の状態遷移モデルとして推定される。
【００１４】
本発明の状態推定装置において、前記第１要素が、前記各パーティクルの今回位置の前記尤度を重みとする重み付き平均値を前記対象物の前記第２状態変数の今回値として推定することが好ましい。
【００１５】
当該構成の状態推定装置によれば、尤度を重みとするパーティクルの今回位置の重み付き平均値が第２状態変数の今回値として推定される。これにより、対象物が第２状態変数の推定値により定義される状態にあることが推定される、
本発明の状態推定プログラムは、コンピュータを本発明の状態推定装置とし機能させることを特徴とする。
【００１６】
本発明の状態推定プログラムによれば、対象物の状態を効率的にかつ高精度で推定する装置としてコンピュータを機能させることができる。
【発明を実施するための最良の形態】
【００１７】
本発明の状態推定装置の実施形態について図面を用いて説明する。
【００１８】
まず、本発明の状態推定装置の構成について説明する。図１に示されている状態推定装置１は、コンピュータ（ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏ，その他の電子回路等により構成されている。）により構成されている。コンピュータのメモリには、このコンピュータを状態推定装置１として機能させるための状態推定プログラムが格納されている。ＣＰＵによりメモリから状態推定プログラムが読み出され、かつ、このプログラムにしたがった演算処理が実行される。
【００１９】
状態推定装置１は第１要素１１と第２要素１２とを備えている。第１要素１１および第２要素１２のそれぞれは、物理的にはメモリと、このメモリからプログラムを読み出して担当する演算処理を実行する演算処理装置（ＣＰＵ）とにより構成されている。第１要素１１は対象物の状態を表わす第１状態変数値を測定する。第１要素１１は第２状態変数により定義されている状態変数空間において複数のパーティクルのうち少なくとも一部のパーティクルを前回位置の周辺に分散させる。第１要素１１は複数のパーティクルのそれぞれを複数のモデルのうち各パーティクルに固有の流動的に変化する今回モデルにしたがって今回位置に遷移させる。第１要素１１は第１状態変数の今回測定値に対する各パーティクルの尤度を評価し、尤度に基づいて対象物の今回状態を推定する。第２要素１２は第１要素１１により評価された尤度が高いパーティクルを優先的に生存または増加させる一方、第１要素１１により評価された尤度が低いパーティクルを優先的に消滅させる。第２要素１２はその後、各パーティクルに固有の今回モデルを基礎として、各パーティクルに固有の次回モデルを決定する。
【００２０】
前記構成の状態推定装置１の機能について説明する。図３（ａ）に示されているように時刻ｋにおいて人間の動きによりその手にあるボール（対象物）Ｑが動かされ、図３（ｂ）に示されているように時刻ｋ＋１においてボールＱが手から離れかけ、図３（ｃ）に示されているように時刻ｋ＋２においてボールＱが手から離れる状況において、ボールＱの位置（以下「対象物位置」という。）が第２状態変数の値として推定される場合について考える。「ｋ」は状態推定装置１の演算処理サイクルの繰り返し回数を表わしている。ボールＱに重力が作用していない状態で対象物位置が変化するモデルが「第１モデル」として定義され、ボールＱに重力が作用している状態で対象物位置が変化するモデルが「第２モデル」として定義されている。以下、第１モデルが固有モデルとして割り当てられているパーティクルを「第１種パーティクル」といい、第２モデルが固有モデルとして割り当てられているパーティクルを「第２種パーティクル」という。
【００２１】
まず、演算処理サイクルまたは時刻を表わす指数ｋが０にリセットされた上で（図２／Ｓ００１）、ＣＣＤカメラ等の撮像装置により撮像された画像が状態推定装置１に入力され、第１要素１１により当該入力画像に基づいて時刻ｋにおける対象物位置ｘ（ｋ）が第１状態変数として測定される（図２／Ｓ００２）。
【００２２】
また、第２要素１２により複数のパーティクルｙi（ｋ）（ｉ＝１，２，‥）が状態変数空間に分散配置される（図２／Ｓ００４）。なお、初期状態（ｋ＝０）を除き、複数のパーティクルｙi（ｋ）のうち一部は状態変数空間における前回位置の周囲に正規分布またはガウス分布にしたがって分散される。これにより、図４に概念的に示されているように第１種パーティクル（黒丸）および第２種パーティクル（白丸）が状態変数空間に配置される。また、図５に示されているように状態変数の測定値（黒点）の周辺に第１種パーティクル（上向き矢印）および第２種パーティクル（下向き矢印）が分散配置される。状態変数空間は第２状態変数により定義される空間であり、第２状態変数がスカラーである場合には１次元空間として定義される一方、第２状態変数がｎ次元ベクトル（ｎ＝２，３，‥）である場合にはｎ次元空間として定義される。推定対象である第２状態変数としての対象物位置ｘ（ｋ）はスカラーなので、状態変数空間は１次元空間として定義されている。各パーティクルｙi（ｋ）は図６に示されているように流動的に値が変化するモデル変数αi（ｋ）を有している。後述のようにモデル変数αi（ｋ）の変化態様は第２要素１２により調節される。モデル変数αi（ｋ）の値が「第１定義域」に属する場合はパーティクルｙi（ｋ）に「第１モデル」が固有モデルとして割り当てられる一方、モデル変数αi（ｋ）の値が「第２定義域」に属する場合はパーティクルｙi（ｋ）に「第２モデル」が固有モデルとして割り当てられる。なお、一部のパーティクルには第１モデルおよび第２モデルのうち１つのモデルが固有モデルとして選択されうる一方、その他のパーティクルには第１および第３モデル（第２モデルとは異なる。）のうち１つのモデルが固有モデルとして選択されうる等、選択可能なモデルの種類がパーティクルｙi（ｋ）ごとに異なっていてもよい。
【００２３】
さらに、第１要素１１により各パーティクルｙi（ｋ）がその固有モデルにしたがって状態変数空間で遷移させられる（図２／Ｓ００６）。これにより、図４に示されているように第１種パーティクル（黒丸）および第２種パーティクル（白丸）が状態変数空間において遷移させられた上で配置される。
【００２４】
また、第１要素１１により測定された対象物位置ｘ（ｋ）に対する各パーティクルｙi（ｋ）の尤度ｐi（ｋ）＝ｐ（ｙi（ｋ）｜ｘ（ｋ））が評価される（図２／Ｓ００８）。これにより、図４においてパーティクルの径の大小により高低が表現されている尤度ｐi（ｋ）が算出される。
【００２５】
そして、第１要素１１により尤度ｐi（ｋ）を重みとする各パーティクルｙi（ｋ）の重み付き平均値Σiｐi（ｋ）・ｙi（ｋ）／Σiｙi（ｋ）が対象物位置ｘ（ｋ）として推定される（図２／Ｓ０１０）。なお、尤度ｐi（ｋ）が最高となるパーティクルｙi（ｋ）、または、尤度ｐi（ｋ）が高い所定順位以内のパーティクルｙi（ｋ）の平均値もしくは尤度ｙi（ｋ）を重みとする重み付き平均値等が対象物位置ｘ（ｋ）として推定されてもよい。
【００２６】
また、第２要素１２により、時刻ｋにおける対象物位置ｘ（ｋ）に対する各パーティクルの離散的に表現された尤度（または確率密度分布）ｐi（ｋ）に基づき、パーティクルｙi（ｋ）を生存させるかもしくは消滅させるか、または、分裂させるか否かが決定される（図２／Ｓ０１２）。これにより、尤度ｐi（ｋ）が高いパーティクルほど優先的に生存または増加する一方、尤度ｐi（ｋ）が低いパーティクルほど優先的に消滅する。
【００２７】
その後、第２要素１２により、生存しているパーティクルｙi（ｋ）に割り当てられている固有モデル（今回モデル）を基礎として、新たな固有モデル（次回モデル）が決定される（図２／Ｓ０１４）。具体的には、各パーティクルｙi（ｋ）に固有の今回のモデル変数値αi（ｋ）が摂動量δi（ｋ）だけ増やされまたは減らされることにより、次回のモデル変数値αi（ｋ＋１）が決定される。次回のモデル変数値αi（ｋ＋１）が第１定義域に属している場合には「第１モデル」が次回の固有モデルとして決定される。同様に、次回のモデル変数値αi（ｋ＋１）が第２定義域に属している場合には「第２モデル」が次回の固有モデルとして決定される。なお、摂動量δi（ｋ）は許容範囲に収まるように調節されている。
【００２８】
以降、対象物位置ｘ（ｋ）の推定処理が終了しているか否かが判定され（図２／Ｓ０１６）、当該推定処理が終了していないと判定された場合（図２／Ｓ０１６‥ＮＯ）、指数ｋが１だけ増加された上で（図２／Ｓ０１７）、対象物位置ｘ（ｋ）の測定および尤度ｐi（ｋ）の評価等の前記一連の処理が繰り返される（図２／Ｓ００２〜Ｓ０１６参照）。
【００２９】
前記機能を発揮する状態推定装置１によれば、対象物の状態推定に際して、第２状態変数により定義された状態変数空間において（１）各パーティクルがその固有の今回モデルにしたがって今回位置に遷移し、（２）第１状態変数の今回測定値ｘ（ｋ）に対する各パーティクルの尤度ｐi（ｋ）が評価される。このため、（１’）各パーティクルが複数のモデルのそれぞれにしたがって遷移し、（２’）第１状態変数の測定値ｘ（ｋ）に対する各パーティクルの尤度ｐi（ｋ）が評価される場合よりも、対象物の推定に要する状態推定装置１の演算処理負荷が軽減される。
【００３０】
また、尤度ｐi（ｋ）が高いパーティクルほど優先的に生存または増加する一方、尤度が低いパーティクルほど優先的に消滅する（図２／Ｓ０１２参照）。このため、対象物がしたがっている蓋然性が高い状態遷移モデル（高尤度モデル）にしたがって状態変数空間で遷移したパーティクルが徐々に増加傾向となる。たとえば、図４および図５のそれぞれに示されているように高尤度モデルとしての「第１モデル」を有するパーティクル（黒丸または上向矢印）が時刻ｋから時刻ｋ＋１に移行し、時刻ｋ＋１から時刻ｋ＋２に移行するにつれて徐々に増加する。さらに、パーティクルに固有の今回モデルを基礎として次回モデルが決定される（図２／Ｓ０１４参照）。このため、高尤度モデルを次回モデルとして状態変数空間で遷移するパーティクルが増加傾向となる（図５参照）。
【００３１】
その一方、モデルは流動的に変化するため、今回モデルを基礎としながらもこの今回モデルとは異なるモデルが次回モデルとして決定される余地がある（図６参照）。このため、高尤度モデルを固有のモデルとするパーティクルの増加に歯止めをかけるように、高尤度モデルとは異なるモデル（低尤度モデル）を固有のモデルとするパーティクルも少数派ながら存在することになる。たとえば、図４および図５のそれぞれに示されているように高尤度モデルとしての「第１モデル」を有するパーティクル（黒丸または上向矢印）が時刻ｋから時刻ｋ＋１に移行し、時刻ｋ＋１から時刻ｋ＋２に移行するにつれて徐々に増加しているものの、低尤度モデルとしての「第２モデル」を有するパーティクル（白丸または下向矢印）も並存している。これにより、対象物がそれまでとは異なるモデルにしたがって状態遷移を開始した場合でも、当該モデルの変化に柔軟に応じて過去の低尤度モデルを将来の高尤度モデルに切り替えながらこの対象物の状態変数の推定値により表わされる状態が高精度で推定されうる。すなわち、対象物の状態を効率的にかつ高精度で推定することができる。
【００３２】
さらに、モデル変数の今回値αi（ｋ）から次回値αi（ｋ＋１）への摂動量δi（ｋ）が許容範囲に収まるように次回値が調節され、この次回値αi（ｋ＋１）が属する一の区域に応じてパーティクルに固有の次回モデルが決定される（図２／Ｓ０１４，図６参照）。モデル変数値αi（ｋ）を摂動させることにより前回モデルと異なるモデルが今回モデルとして決定される余地を残しつつも、その摂動量δi（ｋ）を許容範囲に収めることにより生存したパーティクルに固有の前回モデルをある程度は踏襲する形で今回モデルが決定されうる。これにより、対象物が高尤度モデルにしたがって状態遷移している蓋然性が高いという事情を尊重しつつこの対象物の状態が高精度で推定されうる。
【００３３】
なお、第１状態変数および第２状態変数としては対象物の位置のほか、速度、加速度またはこれらの組み合わせが推定されてもよい。第１状態変数および第２状態変数は相互に異なっていてもよい。また、第１モデルおよび第２モデルとしてさまざまなモデルが採用されてもよい。たとえば「第１モデル」として対象物に摩擦力や拘束力が作用している状態における対象物の挙動態様を定義するモデルが採用される一方、「第２モデル」として対象物に摩擦力や拘束力が作用していない状態における対象物の挙動態様を定義するモデルが採用されてもよい。
【００３４】
また、第１要素１１が、尤度ｐi（ｋ）が最高となるパーティクルに固有の今回モデル、または、尤度i（ｋ）の総和Σiｐi（ｋ）が最高となるパーティクルに共通の今回モデルを、対象物の前回の状態遷移モデルとして推定してもよい。
【図面の簡単な説明】
【００３５】
【図１】本発明の状態推定装置の構成に関する説明図
【図２】本発明の状態推定装置の機能を示すフローチャート
【図３】対象物の観測に関する説明図
【図４】パーティクルフィルタの生成方法に関する説明図（その１）
【図５】パーティクルフィルタの生成方法に関する説明図（その２）
【図６】モデル変数に関する説明図
【符号の説明】
【００３６】
１‥状態推定装置、１１‥第１要素、１２‥第２要素
【図１】
0005043756.tif 000002
【図２】
0005043756.tif 000003
【図３】
0005043756.tif 000004
【図４】
0005043756.tif 000005
【図５】
0005043756.tif 000006
【図６】
0005043756.tif 000007