JP5589324B2

JP5589324B2 - 新規なセンサフュージョン手法を用いた、移動体の状態推定のための装置、方法、およびプログラム

Info

Publication number: JP5589324B2
Application number: JP2009197531A
Authority: JP
Inventors: 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2009-08-28
Filing date: 2009-08-28
Publication date: 2014-09-17
Anticipated expiration: 2029-08-28
Also published as: JP2011047836A

Description

本発明は、センサフュージョン手法を用いた移動体の状態推定に関する。

移動体（自律ロボットを含むロボットなど）の状態推定においては、さまざまな種類のセンサを用いて測定をし、地図（環境地図）を参照することで、移動体の適切な移動を実現している。

また、自律的に移動する移動体では、その移動に際し自身の状態を推定する（尤度関数を設計する）技術を必要とする。そうした状態推定のために使われる時系列フィルタ（Gaussianフィルタ）としては、Kalman（カルマン）フィルタ、拡張Kalmanフィルタ、またはMonte Carlo（モンテカルロ）フィルタ（パーティクルフィルタ）といったものが知られている。

そのような移動体の状態推定系を実現するための技術としては、以下に挙げる先行技術文献ほかが知られている。

特開２００８−２４９６８８号公報特開２００２−０３２７７３号公報特開２００５−３４５５２７号公報

Stevens-Navarro, E., Vivekanandan, V., and Wong, V. W. S., "Dual and mixture Monte Carlo Localization algorithms for mobile wireless sensor networks", Wireless Communications and Networking Conference, 2007.WCNC 2007. IEEE, 11-15 March 2007, pp. 4024-4028 Sebastian Thrun, Wolfram Burgard, and Dieter Fox, "Probabilistic Robotics", The MIT Press, 2006

しかし従来の状態推定系では、事前に想定した環境に適用する単一なセンサと単一な地図しか持たなかった。このためそうした状態推定系は、設計時の想定外の環境への適用性が低く、破綻をきたしやすいという問題があった。

さまざまな異なる環境にも適用できる状態推定系、およびそうした状態推定系を搭載する移動体が、当該技術分野において希求されている。

本発明の実施形態では、系の状態を推定するための装置であって、
(a) ひとつ以上の第一のセンサと、
(b) ひとつ以上の第二のセンサと、
(c) 前記ひとつ以上の第一のセンサのいずれかから各々が出力を受けて記憶手段に格納する、複数の下位モジュールと
を含み、
前記複数の下位モジュールの各々が有する記憶手段は、
前記複数の下位モジュールの各々に対応する地図と、
前記系の状態を推定するための情報であり、前記ひとつ以上の第二のセンサのいずれかの出力から生成され、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新をされる、第一のデータセットと
を有し、
また前記装置はさらに、
(d) 前記複数の下位モジュールの各々から出力された前記第一のデータセットを受けとり、前記複数の下位モジュールの各々に前記第一のデータセットの改善を行わせ、前記第一のデータセットを統合することで、前記系の状態を推定して前記複数の下位モジュールの各々へフィードバックする、上位モジュール
を含んだ装置が提供される。

また、そうした効果が得られる方法とプログラムも、本発明の或る実施形態において提供される。

本発明の実施形態にかかる装置、方法、もしくはプログラムにより、分散的に状態推定処理を行うことができ、さまざまな異なる環境に適用できる状態推定系を得ることができる。さらには、そうした状態推定系を搭載する移動体も得ることができる。こうした状態推定系では、各センサを互いに補うことができるため、処理（測位など）の安定性を向上できる。

実施形態にかかる状態推定系を描いたブロック図である。実施形態にかかるサブモジュールの一例をより詳しく描いた模式図である。実施形態にかかるパーティクルセットを用いた状態推定ユニットの、縦割式の連携に関する態様を説明するための模式図である。実施形態にかかるパーティクルセットを用いた状態推定ユニットの、自己評価および他者評価に関する態様を説明するための模式図である。補助的パーティクルセットを用いたノーマルパーティクルセットのリサンプリング方法にかかる実施形態を説明する図である。オドメトリを共有する別の実施形態を説明する模式図である。状態推定系を含んだ移動体についての実施形態を描いた模式図である。二次元平面測距データの例を示す図である。自然的画像ランドマークに関するデータの例を示す図である。或る実施形態にかかる、複数の移動体の状態を推定するための装置を示す模式図である。別の実施形態にかかる、複数の移動体の状態を推定するための装置を示す模式図である。実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。

以下、本発明の実施形態を図面に基づいて詳細に説明するが、これは本発明を何ら限定するものではない。
〔定義〕
本明細書において使用する術語についてこれより解説を行う。

「移動体」（"vehicle"）とは例えば、駆動装置を具えた移動可能なロボット（自律ロボットなど）のことを含むが、これには限定はされない。例えば実施形態にかかる状態推定系を、人間が操縦する移動機械系（車輌など）に搭載したものを移動体と称することもまた可能である。

「自律ロボット」（"autonomous robot"）とは、人間の介入を（初期設定やメンテナンスを除き）基本的に必要とすることなく自律的に稼動できるロボットのことを指す。自律ロボットは例えば、或る環境内を移動するにあたり、実施形態にかかる状態推定系を使いつつ、かつ人間からの介入を要さないという動作（"自律航法"）を実施可能である。とはいえ、自律ロボットに対して、人間の介入を行うことができないというわけではないことに留意されたい。

「環境」（"environment"）とは、移動体がその中を測定し移動することになる世界のことを指す。実施形態では、当該技術分野にて公知なあらゆる種類の環境を想定している。実施形態にて想定される環境としては例えば、整然とした環境（廊下など）、整備されていない雑然とした環境（研究室内など）、もしくは或る測距センサが計測範囲内に対象物を捉えにくい環境（屋外の広場など）といったものがあるが、これらに限定はされない。

「地図」（"map"）もしくは「環境地図」（"environmental map"）とは、状態推定を行う対象である系（移動体など）が関与する環境についてのデータの集合体のことを指す。こうした地図としては、location-based（位置情報ベース）な地図とfeature-based（特徴ベース）な地図との二種類が含まれる。location-basedな地図はvolumetric mapともいい、環境内のすべての点についてのラベルを有している。つまり、location-basedな地図では、物体（障害物や壁など）が（座標上の）或る点に存在するかどうかについての情報のみならず、何もない箇所（free space）が"在る"という情報も有している。一方、feature-basedな地図は、環境の形状を指定する地図といえる。つまりこれは、目標物（ランドマークともいう）の位置（極座標表示など）を有する地図である。

「センサ」（"sensor"）とは、当該技術分野において公知である任意の種類のセンサのことを指す。そうしたセンサは、当該技術分野では、内界センサと外界センサとに大別される。センサの例としては、姿勢センサ、タコメータ、ロータリーエンコーダ、レートジャイロ、歩幅計、加速度計、位置センサ、変位センサ、加速度センサ、測距（距離）センサ、方位センサ、触覚センサ、力覚センサ、視覚（ビジョン）センサ、ステレオビジョンセンサ、近接覚センサ、超音波センサ、赤外線センサ、傾斜角センサ、地磁気センサ、レーザーレンジファインダー（距離計）、２Ｄエリアスキャンセンサ、３Ｄエリアスキャンセンサなどが含まれるが、これらに限定はされない。

「内界センサ」（"internal sensor"）とは、ロボットなどの系の内部の状態を知るためのセンサのことを指す。

「外界センサ」（"external sensor"）とは、ロボットなどの系の外部の状況を計測し、系の行動のための判断材料とするためのセンサのことをいう。なお、上述した内界センサと外界センサの区別は排他的なものとはかぎらず、同種のセンサを双方に用いることもまた可能である。例えば、視覚センサを内界センサにも外界センサにも使ってもよい。

「センサノード」（"sensor node"）とは、上述したセンサを含んだ装置のことをいう。センサノードは例えば、センサへの入出力を司るユニットや、情報（データ）処理ユニットや、無線・有線通信ユニットなどを有してもよい。またセンサノードは、実施形態にかかる装置測位装置を有することも、実施形態にかかる装置測位方法もしくはプログラムを実行することも可能である。

「インテリジェント・センサノード」（"intelligent sensor node"）とは、上述したセンサノードの一種であり、入力された情報に基づいて何らかの判断を経た処理を行うことができるものをいう。例えば実施形態にかかるインテリジェント・センサノードは、ローカル的な情報処理モジュールを含むことができる。こうしたインテリジェント・センサノードは、他のセンサノードからの情報を踏まえて、状態推定などの作業を行い、別のノードへとその結果を出力することが可能である。例えば後述する図１のサブモジュール 120, 130 は、インテリジェント・センサノードと見做すことができる。

「センサフュージョン」（センサ統合； "sensor fusion"）とは、複数のセンサの出力を統合的に処理して用いる手法のことをいう。そうしたセンサフュージョンには、複数個の同じ種類のセンサを用いてもよいし、あるいは、異なる種類のセンサをそれぞれ任意の個数だけ用いてもかまわない。本明細書において "統合（処理）" といったとき、それはセンサフュージョン（の一部）と同義であると捉えてもらってかまわない。

「（ユニット間の連携が）横断式である」とは、或る処理系統において、特定の階層に並列に存在している複数のユニットを "横方向に" またがって用いることで、何らかの処理を行うことを指す。

「（ユニット間の連携が）縦割式である」とは、或る処理系統において、上位の階層に存在しているユニットと下位の階層に存在しているユニットとを"縦に"またがって用いることで、何らかの処理を行うことを指す。なお横断式の連携と縦割式の連携は排他的な関係にはなく、両者を組み合わせることが可能であることに留意されたい。また、こうした「縦」「横」という表現はあくまで比喩的なものであって、物理空間的な縦横に直接関係するわけではない旨は、本明細書を通読すれば読者にはおのずと明らかとなろう。

「測距データ」とは、測距センサから得られる距離についてのデータのことをいう。測距データは例えば、測定装置からレンジポイントまでのベクトルである。なお「レンジポイント」とは、測距計が使うビーム（レーザーレンジファインダーが用いるレーザービームなど）が、障害物（壁や天井など）に当った点のことを指す。

「ビジョンデータ」とは、視覚（ビジョン）センサから得られる画像データのことをいう。

「オドメトリ」（"odometer"）とは、移動体の持つ内界センサ（駆動装置に付随するセンサなど；オドメータともいう）から得られるデータ（"オドメトリデータ"）に基づいて、移動体の姿勢と位置を求める手法を指す。オドメトリデータとしては例えば、移動体の移動方向、移動距離、もしくは移動速度、またはそれらの組み合わせ、に関するデータが含まれる。オドメトリの一例として、車輪を二つ持つ移動体の場合、左右の車輪の回転速度から移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求める手法がある。その他、任意の内界センサを使って移動体の並進速度と角速度を求め、それらを積分して移動体の位置と姿勢を求めてもよい。なお「オドメトリモデル」とは、オドメトリデータの集合体のことをいう。また、オドメトリモデルを略して単に「オドメトリ」と称する場合もある。

「姿勢」とは、その系（移動体など）のとっている体勢のことをいう。例えば姿勢といえるパラメータとしては、ロール角、ピッチ角、ヨー角などがある。

「（系の）状態」（"state"）とは、環境を特徴づける変数のことであり、例えば環境中の系の位置や移動速度のことをいう。

「状態推定（法）」（"state estimation"）とは、系の状態を推定するための手法のことをいう。状態推定にあたっては例えば、Kalmanフィルタ、拡張Kalmanフィルタ、相補フィルタ、またはパーティクルフィルタといった時系列フィルタを使用可能である。また本明細書では、「測位」という語を、「状態推定」という語と同義に使用することがある。

「信念」（"belief"）とは、時刻tにて得られた計測データz_tを条件とした、状態x_tの事後確率のことをいう。信念 bel(x_t) = p(x_t | z_t) である。

「パーティクル」（"particle"）とは、或る状態推定系の属する位置や方位などを推定する仮説に関する情報（データ）のことを指す。言い換えれば、パーティクルとは事後確率分布（posterior distribution）の標本である。こうしたパーティクルのことを単に「仮説」や「粒子」と呼ぶこともある。例えばパーティクルには、位置推定対象物の状態（姿勢や速度など）を表すデータを含めることができる。またパーティクルとしては例えば、位置推定対象物の位置ベクトルと、位置推定対象物の速度ベクトルと、そのパーティクルが持つ重み（または評価値）と、を含んだデータを使用できるが、これに限定はされない。

「パーティクルセット（パーティクル集合）」（"particle set"）とは、任意の個数のパーティクルの組のことをいう。実施形態では、或るモジュールに属するパーティクルをまとめて、パーティクルセットとして扱うことができる。そうしたパーティクルセットに含まれるパーティクルの個数は、パーティクルを処理する系の能力や機能、要求される自己位置推定の精度などに応じて任意に設定できる。例えば或る実施形態では、1個以上、10個以上、10²個以上、10³個以上、10⁴個、もしくは10⁵個以上のパーティクルを含んだパーティクルセットを設定できるが、これらに限定はされない。また実施形態では、パーティクルセットとして、ノーマルパーティクルセットおよび補助的パーティクルセットの二種類を設定可能である。それらの意義については後述する。

「パーティクルフィルタ」（"particle filter"）とは、時系列フィルタの一種であって、上述したパーティクルを用いて系の状態推定を行うために使用できるものである。またパーティクルフィルタはBayesフィルタの一種でもある。パーティクルフィルタを実現するアルゴリズムとしては例えば、パーティクルの尤度を重みとして、その重みに基づいたリサンプリング（復元抽出）を行い、パーティクルを予測に基づいて移動させ、現在の観測結果とパーティクルの状態から尤度を計算する、といったものがある。

「パーティクルの淘汰（新陳代謝／更新）」または「パーティクルのリサンプリング」とは、何らかの基準に満たないパーティクルを処理の対象から外し（捨て）、新たなパーティクルを得ることをいう。ここではわかりやすく説明するため、パーティクルセットにパーティクルが五個だけある例を引いてみる。そしてパーティクルの評価値が順に0.5、0.5、0.9、0.7、0.4だったとする（あくまで簡単な例であることに留意されたい）。すると平均値（mean）は0.6となる。仮に淘汰する条件が平均値未満であることだとすれば、このパーティクルセットのうちでは二個のパーティクルだけを残すように淘汰が行われることになる。もちろんこの淘汰条件は一例であり、中央値を使った条件や複数種の評価値を使う条件についても想定されている。そして、パーティクルセットのうち特定のもの（重みの大きなものなど）だけを残してパーティクルを淘汰したうえで、残ったパーティクルをその重みに基づいて新たなパーティクルを "播く" 。例えば、重みが大きいパーティクルほど、その重みに比例して多くの新たなパーティクルの元となることができる。直近の例での評価値が重みであるとすれば、評価値が0.9のパーティクルに基づいて三個の新たなパーティクルを、評価値が0.7のパーティクルに基づいて二個の新たなパーティクルを、それぞれリサンプリングできる（もちろんこの手法は一例に過ぎない）。こうした新たなパーティクルの取得数は、簡単な例でいえばテーブルに評価値と個数の対として定めておくこともできる。このように新たなパーティクルを作成する際には、元となったパーティクルをそのままコピーしてもよいし、何らかの変更を加えたうえで作成してもかまわない。

「パーティクルの良さ」とは、そのパーティクルが推定している系の状態と、その系の "現実" の状態（観測結果など）とのマッチングの度合の高さのことをいう。そうした度合は当該技術分野において公知なさまざまな手法を使って算出可能である。後述する実施例では、そうした手法を例示してある。無論、そうした系の "現実" が、人間が同じ系に対して知覚できる現実とは異なっている場合もあるのは、当業者には理解できることだろう。当然のことながら、状態推定系が複数存在すれば、その認識する "現実" はそれぞれ異なってくるであろう。

「自己評価」（"self evaluation"）とは、或る系（モジュールなど）の状態推定を、その系自身の持つデータを用いて評価することをいう。

「他者評価」（"other evaluation"）とは、或る系の状態推定を、それ以外の系が持つデータを用いて評価することをいう。例えば後述する図４に示す例では、ノーマルパーティクルセットが自己評価を、補助的パーティクルセットが他者評価を、それぞれ受けることができるようになっている。

「残差」（"residual"）とは、或るデータと別の或るデータの差分のことをいう。例えば残差とは尤度の残差（尤度残差）のことを指す。あるいは、観測されたデータと、モデルから推定されたデータとの差を、残差ということもある。

「破綻」（"failure"）とは、ここでは状態推定系が自己位置を（一時的であれ）推定できなくなることをいう。例えば、近距離しか計測できない測距センサだけを搭載した自律ロボットが広場に進出したとする。そして測距センサの測定可能距離・角度内に測定できる対象物が何も無くなってしまったとする。すると、そのロボット（の状態推定系）は "破綻" することがある。このような破綻をきたしたロボットは、望ましくない方向に誤って移動することがあり、事故の原因となりうる。なおこの場合、 "誤った" のはあくまでも外部の系（ユーザーとしての人間など）から観測してわかることであり、自律ロボットの持つ状態推定系が自身が "誤った" と判断できるとはかぎらない。実施形態を用いることで、そうした破綻の抑制という効果を得ることが可能である。

〔状態推定系に関する実施形態の例示〕
ここより、本発明の実施形態を実施可能な程度に詳細に説明してゆく。

図１は、状態推定ユニット 100 を描いたブロック図である。

状態推定ユニット 100 は、統合処理モジュール 110 と、サブモジュール#1 120 と、サブモジュール#2 130 とを含む。統合処理モジュール 110 は、（上述した） "縦割式" で考えた際の "上位" のモジュールにあたる。一方、サブモジュール 120, 130 は "下位" のモジュールにあたる。くりかえしになるが、こういった上位や下位であるという物言いはあくまで比喩的なものに過ぎず、物理空間的な位置関係を限定するものではないことに留意されたい。

サブモジュール#1 120 は、センサ#1 122 および地図#1 128 を含む。

サブモジュール#2 130 は、センサ#2 132 および地図#2 138 を含む。

統合処理モジュール 110 は例えば処理モジュールであり、サブモジュール 120, 130 は例えば測位モジュールであるが、その機能はこれには限定はされない。

センサ#1 122 およびセンサ#2 132 としては、上述した任意の種類のセンサを用いることができる。センサ#1 122 とセンサ#2 132 とは異なる種類のセンサであってもよいし、同じ種類のセンサであってもかまわない。

地図#1 128 と地図#2 138 とは、状態推定をする対象である系がおかれる環境についてのデータである。地図 128, 138 は記憶ユニット（図１には示していない）に保存できる。そうした記憶ユニットは、サブモジュールごとにそれぞれ物理的に独立したものであってもよいし、あるいはひとつの記憶媒体を分割したうえでサブモジュールごとに使用するものであってもよい。こうした地図としては、上述したlocation-basedな地図やfeature-basedな地図を使用できる。

サブモジュール 120, 130 は、統合処理モジュール 110 へとセンサと地図に基づいて出力（後述するノーマルパーティクルセットなど）を行う。統合処理モジュール 110 は、受けとった出力を統合し、さらに別のユニット（図１には図示していない移動体の動作モジュールなど）へと渡すことができる。また統合処理モジュール 110 は、受けとった出力を評価し、不適切であると判断した場合には、その不適切な出力をしたサブモジュールへと改善指示を出すことができる。

なお図１では簡単のため、各サブモジュールにつきひとつずつセンサが具わっていると記載してはあるが、これはあくまで一例であることに留意されたい。或るサブモジュールは複数のセンサを有してもよい。例えば或るサブモジュールが、複数種類のセンサをひとつずつ有していて、センサの出力結果に応じて使うセンサを選ぶようにしてもよい。あるいは或るサブモジュールが、同じ種類のセンサを複数個有し、それらの出力を平均して用いてもかまわない。

また、図１では各サブモジュールにつきひとつずつ地図が具わっているとしてあるが、これもあくまで一例である。複数のサブモジュールがひとつの地図を共有してもよいし、あるいは、ひとつのサブモジュールが複数の地図を有していてもかまわない。

また、図１ではわかりやすさを優先し、サブモジュールが二つだけの場合を描いてあるが、これもあくまで一例であることに留意されたい。実施形態では、さらに多くのサブモジュールを搭載することも当然に想定されている。あるいは別の手法として、統合処理モジュールが複数存在し、その統合処理モジュールを "下位" として束ねているさらに "上位" （"超上位"）のモジュールが存在してもよい。つまり状態推定ユニット 100 の階層数も一例に過ぎず、さらに多くの階層を設けることもできるということである。

以上述べた記法・画法上の留意点については、以降の図においても矛盾を生じないかぎりは同様に考慮されたい。

なおプログラムとして実施形態を実施する場合には、状態推定系（例えば状態推定ユニット 100 ）を構成する要素の一部を、記憶ユニット（後述する外部記憶装置 1208 など）に保存されたデータに基づいたものとできる。それらのデータは、バッファ（後述するメモリ 1202 など）に読み出したうえで、実行ユニット（後述するＣＰＵ 1200 など）によって実行できる。

以下に掲げる例では、系の状態推定に際してパーティクルフィルタを使用しているが、その他の時系列フィルタを用いてもよい。例えば、ヒストグラムフィルタを用いてもよい。

図２は、サブモジュールの一例をより詳しく描いた模式図である。

サブモジュール 120a （図１のサブモジュール 120 に対応）は、センサ#1 122a と、地図#1 128a と、パーティクルフィルタ 200 とを含む。なおここで地図#1 128a およびパーティクルフィルタ 200 は、記憶ユニット 290 に格納されている。こうした記憶ユニット 290 は、例えば後述する外部記憶装置 1208 に対応する。

パーティクルフィルタ 200 には、ノーマルパーティクルセット 210 と、補助的パーティクルセット 220 と、オドメトリ（オドメトリモデル） 230 とが含まれる。

ノーマルパーティクルセット 210 はサブモジュール 120a にとって、主要（プライマリー）なパーティクルセットであると位置づけられる。ノーマルパーティクルセット 210 は、オドメトリモデル 230 から後述するように生成できる。ノーマルパーティクルセット 210 の初期値は、（例えばユーザーが初期値を入力するなどして）任意に設定できる。例えば、オドメトリモデル 230 が移動体の駆動系（図２には示していない）に付随する内界センサであったとする。この場合、移動体の速度や方位などを含んだオドメトリモデル 230 が、その内界センサの出力に基づいて、単位時間ごとに更新されることになる。そして、オドメトリモデル 230 に対し、移動体の状態推定データとしてのノーマルパーティクルセット 210 のリサンプリングが時間的にくりかえされ、ノーマルパーティクルセット 210 の更新が行われてゆく。さらに、こうしたノーマルパーティクルセット 210 に対し、センサ#1 122a の出力と地図#1 128a を用いた評価（自己評価）を行ってもよい。

一方、補助的パーティクルセット 220 はサブモジュール 120a にとって、副次的（セカンダリー）なパーティクルセットであると位置づけられる。補助的パーティクルセット 220 は、センサ#1 122a の出力および地図#1 128a から生成できる。補助的パーティクルセット 220 の初期値も任意に設定可能である。補助的パーティクルセット 220 も、生成源に基づくリサンプリングをくりかえして更新していってもよい。あるいは、補助的パーティクルセット 220 を淘汰せず、生成源からその都度すべて新たに生成してもかまわない。

なお図２では、わかりやすさを優先してパーティクルセットをサブモジュールごとに二種類だけ記載してあるが、この数もまた一例に過ぎないことに留意されたい。三種類以上のパーティクルセットを使用する実施形態もまた、本発明に包摂されうるということが、本明細書を通読すれば理解できるであろう。

このようにサブモジュールが複数種のパーティクルセットを持つことで、好ましいセンサフュージョンが可能となる。それゆえに、状態推定の精度の向上（オドメトリモデルとセンサによる測位結果とのずれの適切な補正など）という効果が得られる。適切なセンサフュージョンが可能となると、特定のセンサが持つ弱点を別のセンサで補うという効果を得ることもできる。例えば、状態推定系が置かれる環境が乱雑な場合、２Ｄエリアスキャンセンサは適切な測定結果を得にくいことがある。そのようなとき、乱雑な環境でも有効なデータを得やすい３Ｄエリアスキャンセンサやビジョンセンサとのセンサフュージョンができれば、享受できる利益は非常に大きいといえる。

図３は、ノーマルパーティクルセットおよび補助的パーティクルセットを用いた状態推定ユニット 300 でのセンサフュージョンを説明するための模式図である。なおくりかえしになるが、この図に示しているサブモジュール、地図、センサ、パーティクルセットの個数はあくまで一例に過ぎず、そのほかの個数を設定した実施形態もまた想定されている。

状態推定ユニット 300 は、統合処理モジュール 310 と、サブモジュール#1 320 と、サブモジュール#2 330 とを含んでいる。

サブモジュール#1 320 は、センサ#1 322 と、改善指示受入ユニット 324 と、地図#1 328 と、パーティクルフィルタ 340 とを含む。

サブモジュール#2 330 は、センサ#2 332 と、改善指示受入ユニット 334 と、地図#2 338 と、パーティクルフィルタ 350 とを含む。

パーティクルフィルタ 340, 350 はそれぞれ、ノーマルパーティクルセット 342, 352 および補助的パーティクルセット 344, 354 を含む。

ノーマルパーティクルセット 342, 352 は例えば、オドメトリモデル（図３には示していない）から生成もしくは更新できる。

補助的パーティクルセット 344, 354 は例えば、センサ 322, 332 の出力および地図 328, 338 から生成もしくは更新できる。

サブモジュール 320, 330 からはそれぞれ、ノーマルパーティクルセット 342, 352 を統合処理モジュール 310 へと出力できる。

統合処理モジュール 310 は、縦割式評価ユニット 360a, 360b と、改善指示提出ユニット 364a, 364b を含む。なお図３では、縦割式評価ユニットと改善指示提出ユニットがサブモジュールの個数に対応して二つずつあるように描いてあるが、これはあくまで一例に過ぎない。これらのユニットが二個以外の個数であっても、同様の効果を得るように状態推定ユニット 300 を構築できる。

縦割式評価ユニット 360a, 360b は、受けとったノーマルパーティクルセット 342, 352 に対してそれぞれ、良さ評価を行う（ステップ S370, S380 ）。

こうした良さ評価としては、例えば以下に述べるアルゴリズム1を用いることができる。

｛アルゴリズム1：パーティクルセットの評価｝
パーティクルの個数をN、iを1≦i≦Nの自然数として、第i番目のパーティクルの状態をs_iと表す。また、第i番目のパーティクルの評価値（重み）をw_iとする。評価値の最大値をw_max、平均値をw_meanとする。このとき、
である。また、評価値の最大値についての閾値をTH_max、評価値の平均値についての閾値をTH_meanとおく。また、パーティクルセットについての評価値をvで表す。
IF
w_max < TH_max or w_mean < TH_mean
THEN
v = 0
ELSE
v = 1;

ここに述べたアルゴリズム1はあくまで一例である。例えば評価値について、最大値と平均値以外の何らかの統計的な値を使ってもよい。あるいは、評価値の最大値と評価値の平均値といった二種以上の統計的な値を組み合わせて、どちらも閾値以上となればv = 1となるという手法を採ってもかまわない。また、vに設定する値を、0と1以外の任意の値に定めてもかまわない。そのほか当該技術分野にて知られた任意の手法を使うことができる。

こうしたアルゴリズム1は記憶ユニット（後述する外部記憶装置 1208 など）に格納することができる。格納されたアルゴリズム（を表現するデータ）を、バッファ（後述するメモリ 1202 など）に読み出したうえで、実行ユニット（後述するＣＰＵ 1200 など）が実行することで、パーティクルセットの評価を実現できる。例えば、パーティクルの評価値を示すデータと所定の閾値を記憶ユニットからバッファに読み出し、実行ユニットが評価値の最大値もしくは平均値を算出したうえで、閾値との比較を行う。その後実行ユニットは、比較結果に基づいて変数vに値を格納し、記憶ユニットに保存する。このようにすることで、アルゴリズム1をハードウェア的に実施することが可能である。

また、地図 328, 338 と、パーティクルフィルタ 340, 350 は、各サブモジュール内の記憶ユニット（図３には示していない；外部記憶装置 1208 などに対応する）に保存される。

なお、各パーティクルの評価値は、当該技術分野にて周知の任意の手法により設定することが可能である。後述するように、サブモジュール自身に対応するセンサの出力と地図を使って、パーティクルを評価可能である。あくまで一例として、パーティクルをランドマークを使って評価する場合、ビジョンセンサから得られたビジョンデータから算出できるそのセンサからランドマークへの距離と角度をまず算定する（観測値）。そして、パーティクルが定める座標を地図上にあてはめ、ランドマークまでの距離と角度を地図上で算定する（計算値）。そして、得られた観測値と計算値の差を、（最小二乗法などを使って）算出して、その算出結果に基づいて評価値を決定可能である。

以下にパーティクルの評価値についての具体的な例示を行うが、本発明の実施形態はこれには限定はされない。

パーティクルセットをX_tとおくと、
X_t := x_t ^[1], x_t ^[2], ... , x_t ^[M] (1)
と表せる。ここでMはパーティクルの個数である。おのおののパーティクル x_t ^[m](1≦m≦M) は、時刻tでの状態に関するひとつの仮説である。

或る状態のパーティクル（仮説）x_tがパーティクルセットX_tに含まれる尤度（likelihood）は、理想的にはBayesフィルタの事後信念（posterior belief） bel(x_t) に比例する。すなわち、パーティクルx_tは
x_t ^[m] 〜 p(x_t| z_1:t,u_1:t) (2)
となるように選ばれるべきである。ここでp(A | B)はBayesの条件付き確率（conditional probability）であり、或る事象Bが起こるという条件下での別の事象Aの起こる確率である。

時刻tのパーティクルx_t ^[m]は、以下のようにサンプリング（採取）できる。
x_t ^[m] 〜 p(x_t| u_t,x_t-1^[m]) (3)

パーティクルx_t ^[m]に対する評価値（重み）w_t ^[m]は、以下のように計算できる。
w_t ^[m] = p(z_t | x_t ^[m]) (4)
ここでz_tは時刻tでの計測（measurement）データであり、例えば上述したオドメトリモデルに相当する。

つまり、w_t ^[m]により重みづけられたパーティクルからなるパーティクルセットは、Bayesフィルタの事後信念 bel(x_t) の近似表現となる。

図３の説明に戻る。それからステップ S372, S382 ではそれぞれ、ノーマルパーティクルセット 342, 352 が良かったかどうかを、縦割式評価ユニット 360a, 360b が判定する。こうした判定としては例えば、上述したパーティクルセット評価値が、所定の閾値を超えたかどうかを算定することが挙げられる。

なお、図３では良さ評価を統合処理モジュール 310 （の持つ縦割式評価ユニット）のみが行っているように描かれているが、これはあくまで一例である。サブモジュールがインテリジェント・センサノードである場合には、サブモジュール自身が自らのノーマルパーティクルセットの良さ評価を行い、その結果を統合処理モジュール 310 （の持つ縦割式評価ユニット）へと送信できる。この場合には、縦割式評価ユニットは実際には各サブモジュールの自己評価を総括する役割を担うことになるといえるだろう。

そしてノーマルパーティクルセットが「良い」と判断された場合、フローはステップ S390 へ進み、統合処理が行われる。例えば二組のノーマルパーティクルセットを統合する場合、単純に和をとったり、平均をとったり、もしくは何らかの重みをつけて平均をとったりといった任意の手法を採ることができる。

一方、ノーマルパーティクルセットが「良い」と判断されなかった場合、フローはそれぞれステップ S374, S384 へ進み、そのパーティクルセットは破棄される。そして統合処理モジュール 310 に含まれる改善指示提出ユニット 364a, 364b が、改善指示を対象となるサブモジュールへと送信する。

サブモジュール 320, 330 がそれぞれ持つ改善指示受入ユニット 324, 334 は、改善指示を受けとると、ノーマルパーティクルセット 342, 352 の改善を行うようパーティクルフィルタ 340, 350 にはたらきかける。例えばサブモジュールは、補助的パーティクルセット 344, 354 を使ってノーマルパーティクルセット 342, 352 を改善するようにしてもよいし、あるいは、図３には示されていない別のパーティクルセットを使って改善を行ってもよい。

図３の例（サブモジュールの個数が2）では、ノーマルパーティクルセット 342, 352 のうちいずれか一方だけが「良い」と判断された場合、統合処理は実際には行われず、「良い」と判断されたパーティクルセットがそのまま統合結果として得られる。あるいは、ノーマルパーティクルセット 342, 352 のいずれも「良い」と判断されなかった場合には、統合処理モジュール310 が今回得た出力は使わないようにできる。例えば、その使わなかった出力の代わりに、統合処理モジュール310 が、前回得た出力を用いるようにしてもよい。あるいは別の手法として、非常用として設定された何らかの規定の値を使うようにしてもよい。

なお当然のことながら、サブモジュールが3個以上あった場合にも、図３と同様の処理を行うことが可能であるし、その手法も自明であるといえよう。

統合処理モジュール 310 が統合処理した結果は例えば、移動体の駆動系にフィードバックすることが可能である。このようにすることで、移動体がより適切な移動をすることができるという効果が得られる。

図４は、ノーマルパーティクルセットおよび補助的パーティクルセットを用いた状態推定ユニット 400 での自己評価および他者評価を説明するための模式図である。

状態推定ユニット 400 は、統合処理モジュール 410 と、サブモジュール#1 420 と、サブモジュール#2 430 とを含んでいる。

はじめに、図４では、統合処理モジュール 410 が、サブモジュール 420, 430 からのノーマルパーティクルを出力として受けることについては、記載を省略していることに留意されたい（図４に示す実施例は、自己評価と他者評価に焦点をあわせた態様と考えてもらってもよい）。

統合処理モジュール 410 は、他者評価ユニット#1 412 と他者評価ユニット#2 414 とを含む。これらの他者評価ユニットは分けて描いてあるが、ひとつのユニットであってもかまわない。

サブモジュール#1 420 は、センサ#1 422 と、他者評価受入ユニット 426 と、地図#1 428 と、パーティクルフィルタ 440 とを含む。

サブモジュール#2 430 は、センサ#2 432 と、他者評価受入ユニット 436 と、地図#2 438 と、パーティクルフィルタ 450 とを含む。

パーティクルフィルタ 440 には、ノーマルパーティクルセット 442 と、補助的パーティクルセット 444 と、オドメトリモデル 446 とが含まれる。
パーティクルフィルタ 450 には、ノーマルパーティクルセット 452 と、補助的パーティクルセット 454 と、オドメトリモデル 456 とが含まれる。

図４の例では、ノーマルパーティクルセット 442, 452 がそれぞれ、オドメトリモデル 446, 456 から（例えば式(3)のようにして）生成される。ノーマルパーティクルセット 442, 452 に対してそれぞれ、センサ 422, 432 の出力および地図 428, 438 に基づいて自己評価を施せる。自己評価のためのアルゴリズムとしては、上述したアルゴリズム1などを使用可能である。そうした自己評価の結果は、（図４には明示されていないが）統合処理モジュール 410 に与えることができる。

またサブモジュール 420, 430 は、ノーマルパーティクルセット 442, 452 を更新してゆくことができる。この際、ノーマルパーティクルセットを、単位時間ごとに、単位時間前に得られたノーマルパーティクルセット自身を基にして、そのサブモジュールに対応したオドメトリモデルを用いて更新してゆくことで得ることができる。

また、（図４には明示されていないが）補助的パーティクルセット 444, 454 はそれぞれ、センサ 422, 432 の出力および地図 428, 438 から上述するように生成できる。この際、補助的パーティクルセットを、単位時間ごとに、センサ 422, 432 の出力および地図 428, 438 に基づき、かつ単位時間前に得られた補助的パーティクルセット自身を使用することなく、新たに生成するようにしてもよい。

あるいは別の手法として、ノーマルパーティクルセットを、単位時間前に得られたノーマルパーティクルセット自身を使用することなく、かつ単位時間ごとにサブモジュールに対応したオドメトリモデルを用いて、毎時新規に生成するようにしてもよい。また別の手法として、補助的パーティクルセットを、単位時間ごとに、単位時間前に得られた補助的パーティクルセット自身を基にして、そのサブモジュールに対応したセンサの出力および地図に基づいて更新してゆくことで得ることも可能である。

さて、他者評価ユニット#1 412 は、サブモジュール#1 420 の持つセンサ#1 422 の出力と地図#1 428 に基づき、別のサブモジュールで（サブモジュール#2 430 など）の持つ補助的パーティクルセットを評価するものである。これを他者評価と称す。他者評価も、自己評価と同じく、比較対象するデータ同士のマッチング度合が閾値を超えているかどうかを観て行うことができる。図４の例（サブモジュールの個数が2）では、他者評価ユニット#1 412 が、サブモジュール#2 430 （の持つ他者評価受入ユニット 436 ）へと他者評価結果の出力を行っているが、これにはあくまで一例である。さらに多くのサブモジュールが存在する場合、他者評価ユニットは、複数個のサブモジュールの持つ他者評価受入ユニットへと他者評価結果を出力可能である。

なお、図４では簡単のため省略してあるが、統合処理モジュール 410 （の持つ他者評価ユニット）は、サブモジュールから補助的パーティクルセットを受けとり、他者評価に使用できる。

他者評価ユニット#1 412 と同様に、他者評価ユニット#2 414 もまた、サブモジュール#2 430 の持つセンサ#2 432 の出力と地図#2 438 に基づき、他者評価受入ユニット 426 を介して、サブモジュール#1 420 の持つ補助的パーティクルセット 444 を評価できる。

このような "横断的" な他者評価を行うことで、補助的パーティクルセットの精度を確認できる。すなわちそれは、その補助的パーティクルセットを有するサブモジュールに関するセンサ（外界センサ）が適切に機能しているかを測ることにもなる。すると後述するようにノーマルパーティクルセットを、補助的パーティクルセットを用いてリサンプリングする場合に、不適切な補助的パーティクルセットの使用を回避できるという効果が得られる。

図５は、或るサブモジュール（サブモジュール 320, 420 など）における、補助的パーティクルセットを用いたノーマルパーティクルセットのリサンプリング（復元抽出）方法を説明する図である。

まず、ステップ S500 では、下位モジュール（サブモジュール#1 320 など）内の改善指示を受ける部分（改善指示受入ユニット 324 など）が、上位モジュール（統合処理モジュール 310 など）からの改善指示を待ち受ける。なおそうした改善指示は例えば、下位モジュールの持つノーマルパーティクルセットの良さを（上位モジュールが持つ縦割式評価ユニットなどによる）評価に基づいて発動できる。

そしてステップ S510 にて、下位モジュールが（例えば単位時間ごとに）改善指示の有無を確認する。改善指示があった場合には、フローはステップ S520 に進む。改善指示がなかった場合には、フローはステップ S530 に進む。つまりステップ S520 に進むということは、ノーマルパーティクルセットの精度が悪く、そのサブモジュールのオドメトリの誤差が大きい可能性があるということである。しかしそうした場合、そのサブモジュールの持つセンサの出力にのみ頼ってノーマルパーティクルセットを修正しようとすると、問題が起きることがある。そのセンサの測定範囲などによっては、適切な（状態推定系の外部の別の系から観た "現実" に近い）測定結果が得られているとはかぎらないからである。そこで或る実施形態では、他者評価を施した（すなわち、他のセンサの出力を踏まえた）補助的パーティクルセットを併用することで、ノーマルパーティクルセットをより好ましく修正できる（リサンプリングできる）という効果が得られる。

さてステップ S520 では、上位モジュールが、ノーマルパーティクルセットを改善するにあたり、補助的パーティクルセットからどれだけの個数のパーティクルを吸収させるかを決定する。いま、ノーマルパーティクルセットと補助的パーティクルセットが同数Nのパーティクルを持つと仮定する。そして、リサンプリングを行うに際し、ノーマルパーティクルセットからN₁個のパーティクルを、補助的パーティクルセットからN₂個のパーティクルを、それぞれ供出させる（ステップ S522, S524 ）。この例では1≦N₂≦N₁≦NかつN₁ + N₂ = Nであるが、N₁≦N₂であってもよい。こうしたN₁, N₂ の値は例えば、上位モジュールの下した自己評価と他者評価の結果を反映させたものとできる。あるいは別の手法として、つねにN₁とN₂との比を一定にしてもよい。なお当然のことながら、他の実施形態では、ノーマルパーティクルセットと補助的パーティクルセットが異なる数のパーティクルを持つようにしてもよい。

以下にパーティクルのリサンプリングについての具体的な例示を行うが、本発明の実施形態はこれには限定はされない。

上述したようにパーティクルセットX_t中のパーティクルx_t ^[m]に対し評価値w_t ^[m]を定めたのち、そのパーティクルセットから、w_t[i]に比例した数のパーティクルx_t[i]を引き出して（1≦i≦M）、パーティクル個数合計がM個の新たなパーティクルセットを得る。リサンプリング前のX_tでは信念 bel'(x_t) に従ってパーティクルが分布していた。それに対し、リサンプリング後のX(t)では、事後信念 bel(x_t) = ηp(z_t | x_t ^[m])bel'(x_t) に従ってパーティクルが分布することになる。なおここでηはBayes則の正規化記号（normalizer in Bayes rule variable）であり、 p(x | y) = ηp(y | x)p(x) である。

さてステップ S526 では、下位モジュールが、ステップ S522, S524 でそれぞれ供出されたパーティクルを基にして、N個のパーティクルを持つ新たなノーマルパーティクルセットをリサンプリングする。そしてステップ S540 に進み、こうして得られた新たなノーマルパーティクルセットを、この単位時間におけるノーマルパーティクルとして（下位モジュールが持つ記憶ユニットに）格納する。

一方、ステップ S510 からステップ S530 へと進んだ場合には、補助的パーティクルセットは使わず、ノーマルパーティクルセットを使って、新たなノーマルパーティクルセットをリサンプリングする（ステップ S532 ）。そしてステップ S540 にて得られた新たなノーマルパーティクルセットを、この単位時間におけるノーマルパーティクルとして格納する。

なおここに述べたパーティクルフィルタは公知の技術であり、その実装例は上述した非特許文献２ほかに開示されている。

図６は、オドメトリを共有する状態推定ユニット 600 を説明するための模式図である。

状態推定ユニット 600 は、状態推定ユニット 400 と概ね同様であるが、オドメトリモデルをサブモジュールごとに持たないところだけが異なる。同様の箇所については、図４と図６では参照番号下二桁が等しい要素についてはほぼ同様であるため、紙幅の都合上説明をくりかえすことはしない。

状態推定ユニット 600 は、（状態推定ユニット 400 とは異なり）記憶ユニット 660 を有する。記憶ユニット 660 は、オドメトリモデル 666 を含む。

図６では、記憶ユニット 660 が統合処理モジュール 610 とサブモジュール 620, 630 の外部に存在するように描いてあるが、これはあくまで一例である。例えば記憶ユニット 660 が、いずれかのサブモジュールの中に搭載されるようにしてもよいし、あるいは、統合処理モジュール 610 中に搭載されるようにしてもかまわない。

図６では、ノーマルパーティクルセット 642, 652 がともに、オドメトリモデル 666 から生成される。このような構造をとることで、オドメトリモデルを構築するデータ量の節減という効果を得ることができる。

〔移動体に関する実施形態の例示〕
図７は、状態推定系 707 を含んだ、移動体 700 を描いた模式図である。
移動体 700 はさらに、走行系 770 も含む。走行系 770 は、例えば図２のオドメトリモデル 230 に相当しうる。走行系 770 は、状態推定系 707 へと移動体の状態（位置・姿勢など）についての情報を送る。そして状態推定系 707 からのフィードバックを走行系 770 が受けて、移動制御機構 780 を介して調節を行う。移動制御機構 780 は、移動機構 790 を適切に制御し、移動体の適切な移動を実現できる。

以下さらに詳しく各要素を説明してゆく。
状態推定系 707 には、統合処理モジュール 710 と、外界センサ#1 720 と、外界センサ#2 730 と、サブモジュール#1 740 と、サブモジュール#2 750 と、サブモジュール#3 760 とが含まれる。

図７の例では、外界センサ#1 720 としてレーザーレンジファインダを、外界センサ#2 730 としてステレオビジョンを設定してある。そして、外界センサ#1 720 からは二次元平面測距データ 722 が出力される。また、外界センサ#2 730 からは三次元平面測距データ 732 および自然的画像ランドマーク（に関するデータ） 734 （環境中の障害物などに関するデータ）が出力される。

二次元平面測距データ 722 の一例を、図８に示してある。例えば外界センサ#1 720 がレーザーレンジファインダである場合、センサから発せられたレーザービームが環境中の障害物（壁や天井など）にヒットし、レンジポイントの座標を得ることができる。

以下に掲げるのは、三次元座標であるデータの例である。

ｘｙｚ
0.813691, -3.412359, 0.302000
0.875252, -3.412136, 0.302000
0.940315, -3.426451, 0.302000
1.001578, -3.420039, 0.302000
1.063323, -3.414471, 0.302000
1.125890, -3.410684, 0.302000
1.192166, -3.418247, 0.302000

図８の例では、こうして得られたレンジデータのx座標とy座標を床に投影して、二次元的なデータとしている。

また、図８では、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い直線が移動体の向きを示している。

また、自然的画像ランドマークに関するデータ 734 の例を、図９に示してある。この例では外界センサ#2 730 がステレオビジョンであるため、左上パネルと右上パネルにそれぞれ基準画像と参照画像が表示されている。これらの画像は、左下パネルの平面地図に対応したものである。

左上パネルと右上パネルには、実線でビジョンデータが、破線でセンサがスキャンを行ったラインが、それぞれ示してある。また図９の左下パネルでは、移動体（自律ロボットなど）の位置を丸で示してあり、丸についた短い矢印が移動体の向きもしくは進行方向を示している。

なおこれらのデータ 722, 732, 734 はあくまで一例であることには留意されたい。
また、図７の例では、ひとつの外界センサから複数のサブモジュールへ別々の出力を行っていることにも留意されたい。実施形態では、或る外界センサが任意の個数のサブモジュールへの出力を行うことを想定している。

状態推定系 707 の持つ三個のサブモジュール 740, 750, 760 はそれぞれ、他人評価受入ユニット 746, 756, 766 および地図 748, 758, 768 を有する。なお、図７に記載されているように地図 748, 758, 768 は対応するサブモジュールの外部の記憶ユニットに保存されていてもよいが、サブモジュールの内部の記憶ユニット（外部記憶装置 1208 など）に保存することも当然可能である。あるいは別の手法として、地図がひとつもしくは二つであって、複数のサブモジュールがその地図を共有していてもよい。また当然のことながら、状態推定系 707 が三個よりも少ないサブモジュールを有してもよいし、または三個よりも多いサブモジュールを有してもかまわない。

サブモジュール 740, 750, 760 はそれぞれ、データ 722, 732, 734 を受けて補助的パーティクルセット（図７には示していない）を生成することができる。

またサブモジュール 740, 750, 760 はそれぞれ、走行系 770 （の自己状態記憶メモリユニット#1 776 ）からの出力を受けて、ノーマルパーティクルセット（図７には示していない）を生成できる。

一方、走行系 770 には、内界センサ 772 と、自己状態推定ユニット 774 と、自己状態記憶メモリユニット#1 776 と、自己状態記憶メモリユニット#2 778 とが含まれる。

内界センサ 772 は、移動機構 790 などを測定し、その結果となるデータは自己状態推定ユニット 774 へと送られる。

自己状態推定ユニット 774 では、内界センサ 772 の出力を基にして、移動体 700 （もしくは、走行系 770 ）の状態推定を行う。自己状態推定ユニット 774 は例えば、地図（図７に示していないものでもよいし、あるいは、地図 748, 758, 768 を使用してもよい）を参照して、移動体 700 （もしくは、走行系 770 ）の位置を推測することができる。あるいは、移動体 700 （もしくは、走行系 770 ）の速度や、向いている方位を推測することも可能である。

自己状態推定ユニット 774 の出力は、自己状態記憶メモリユニット#1 776 と自己状態記憶メモリユニット#2 778 とへ送られる。このとき、双方へ同じ内容のデータを送ってもよいし、もしくは異なる重みをつけて送信してもよい。同じ内容のデータを用いる場合には、初期の時点（すなわち、移動体 700 の稼動直後）では、自己状態記憶メモリユニット#1 776 と自己状態記憶メモリユニット#2 778 とは同じデータを持つことになる。その後、自己状態記憶メモリユニット#1 776 からの出力が状態推定系 707 へ送られてやがて自己状態記憶メモリユニット#2 778 へとフィードバックされると、内界センサ 772 の出力結果を補ったより精度の高い状態推定が可能になるという効果を得ることができる。

次に状態推定系 707 内の詳細な説明を行う。自己状態記憶メモリユニット#1 776 からの出力は、サブモジュール 740, 750, 760 が受ける。上述したようにサブモジュール 740, 750, 760 はそれぞれ、受けとったその出力に基づいて、ノーマルパーティクルセットを生成する。そうして生成されたノーマルパーティクルセットには、例えば統合処理ユニット 710 が持つ縦割式評価ユニット（図７には示していない）により、そのサブモジュールに対応する外界センサの出力と地図とに基づいた評価を施すことができる。

また、統合処理モジュール 710 が含む他者評価ユニット（例えば図４の他者評価ユニット#1 412 に対応）は、サブモジュールの持つ補助的パーティクルセットを、評価することができる。

ノーマルパーティクルセットと補助的パーティクルセットに施される処理の態様については、図５に関する説明で述べた態様と同様に行うことが可能である。
統合処理モジュール 710 は、サブモジュール 740, 750, 760 からの出力を上述した "縦割式" の評価などで適切に取捨選択してから統合できる。そして統合処理モジュール 710 は、移動体 700 の状態を修正するデータを、自己状態記憶メモリユニット#2 778 へとフィードバックできる。

そして、自己状態記憶メモリユニット#2 778 は、移動制御機構 780 を介して、移動機構 790 を適切に制御可能である。

こうして図７に示した移動体では、このように二種の外界センサのセンサフュージョン（ "横断的" なセンサフュージョン）を実現できるという効果を享受できる。

なお、ここでは実施形態の各種データについて、理解しやすいよう簡単な例を開示してはいるが、この例に限らず、データを対応付けて管理可能なXML形式やツリー構造形式やテーブル形式などの他の管理形式を採用してもよい。

図１０は、或る実施形態にかかる、複数の移動体（図７の移動体 700 など）の状態を推定するための装置 1000 を示す模式図である。ここでは一例として、二つの移動体がある場合を示してある。

装置 1000 には、上位統合処理モジュール 1010 と、移動体#1 1020 と、移動体#2 1030 とが含まれる。移動体 1020, 1030 は、例えば図７の移動体 700 と同様であるが、上位統合処理モジュール 1010 へと状態推定結果を提供する機能を有しているところが異なっている。移動体1020, 1030 はそれぞれ、センサ 1022, 1032 と、複数の下位モジュール 1024, 1026, 1034, 1036 と、統合処理モジュール 1028, 1038 とを含んでいる。上位統合処理モジュールは "上位" の "上位" であるので、 "超上位モジュール" であると見做してもよい。

上位統合処理モジュール 1010 は、移動体 1020, 1030 へそれぞれ、上述したような "縦割式" の評価に基づく改善指示を出すことが可能である。また、移動体同士に対して "横断式" の評価を施すことも同様に可能であるといえる。

つまり図１０の例は、図１のサブモジュールが独立した移動体である場合とみなすこともできる。

図１１は、別の実施形態にかかる、複数の移動体（図７の移動体 700 など）の状態を推定するための装置 1100 を示す模式図である。ここでも一例として、二つの移動体がある場合を示してある。

装置 1100 には、上位統合処理モジュール 1110 と、移動体#1 1120 と、移動体#2 1130 とが含まれる。移動体 1120, 1130 は、図１０の移動体 1020, 1030 と同様であるが、センサを内部に有しておらず、外部のセンサ 1140 を共有しているところが異なっている。なお、別の実施形態においては、特定の移動体だけがセンサ 1140 を包摂し、他の移動体がそのセンサ 1140 を用いるといったことも想定されている。

上位統合処理モジュール 1110 は、移動体 1120, 1130 へそれぞれ、上述したような "縦割式" の評価に基づく改善指示を出すことが可能である。また、移動体同士に対して "横断式" の評価を施すことも同様に可能であるといえる。

つまり図１１の例も、図１のサブモジュールが独立した移動体である場合の変形例とみなすことができる。

当然のことながら、装置 1000, 1100 の "階層" の数やモジュールの数や移動体の数は一例に過ぎず、他の実施形態ではここに例示した以外の個数を使用できることに留意されたい。

〔ハードウェア〕
図１２は、実施形態にかかるプログラムを実行することができるハードウェア構造を示す概要図である。ここでは、ＣＰＵ 1200 、メモリ 1202 、入力装置 1204 、出力装置 1206 、外部記憶装置 1208 、媒体駆動装置 1210 、可搬記録媒体 1212 、ネットワーク接続装置 1214 が、バス 1216 によって接続されている。

またこうしたハードウェア構造は、サブモジュールごとに構築してもよいし、移動体ごとに構築してもかまわないことに留意されたい。

ＣＰＵ（中央処理ユニット） 1200 としては、任意の種類のプロセッサを使用でき、例えばMPUや組み込み用途ＣＰＵを使用できる。ＣＰＵ 1200 は、実施形態にかかるプログラムをバッファ（メモリ 1202 など）に読み出したうえで実行できる。

メモリ 1202 としては、ダイナミックランダムアクセスメモリ（DRAM）が含まれるがこれには限定はされない。そうしたDRAMとしては、SDRAM、SLDRAM、RDRAM、および他のDRAMなどがある。メモリ 1202 にはまた、フラッシュメモリなどの不揮発性メモリを含めてもよい。

入力装置 1204 としては、キーボード、マウス、トラックボール、タッチパネル、ゲームコントローラー、もしくは音声認識装置といった、ユーザーが情報を入力しできる任意の装置が含まれる。

出力装置 1206 としては、２Ｄディスプレイ、スピーカー、３Ｄディスプレイといった、情報を出力してユーザーへ伝達できる任意の装置が含まれる。

外部記憶装置 1208 としては、同様に特定の用途に適するひとつ以上の記憶媒体を使用でき、例えば、ハードディスクドライブ、ＳＳＤ（Solid State Drive）を使用できる。

媒体駆動装置 1210 には、任意の種類の可搬記録媒体 1212 を駆動できる任意のドライブが含まれる。可搬記録媒体 1212 としては、フレキシブルディスケット、コンパクトディスク（ＣＤ）、デジタルビデオディスク（ＤＶＤ）、などといったものが含まれる。

ネットワーク接続装置 1214 には、ネットワークに接続可能な任意の装置が含まれる。ネットワーク接続装置1214 としては、イーサネット（登録商標）アダプタ、有線ＬＡＮ（Local Area Network）ルータ、無線LANルータなどが含まれる。

実施形態にかかるプログラムは、外部記憶装置 1208 もしくは媒体駆動装置 1210 によって駆動される可搬記録媒体 1212 に格納することができる。また、ハードウェアは、ネットワーク接続装置 914 を介してネットワークと接続することができ、入力装置 1204 および出力装置 1206 を通じて実施形態にかかるプログラムを入出力することが可能である。また、サーバが持つ記憶装置に実施形態にかかるプログラムを保存し、クライアントからそのプログラムを実行させることで、実施形態にかかる効果を得ることも可能である。そうしたサーバ-クライアントネットワークは、例えばＰ２Ｐ（peer to peer）ネットワークであってもかまわない。

100 状態推定ユニット
110 統合処理モジュール
120 サブモジュール#1
122 センサ#1
128 地図#1
130 サブモジュール#2
132 センサ#2
138 地図#2
200 パーティクルフィルタ
210 ノーマルパーティクルセット
220 補助的パーティクルセット
230 オドメトリモデル
290 記憶ユニット
300 状態推定ユニット
310 統合処理モジュール
320 サブモジュール#1
322 センサ#1
324 改善指示受入ユニット
328 地図#1
330 サブモジュール#2
332 センサ#2
334 改善指示受入ユニット
338 地図#2
340 パーティクルフィルタ
342 ノーマルパーティクルセット
344 補助的パーティクルセット
350 パーティクルフィルタ
352 ノーマルパーティクルセット
354 補助的パーティクルセット
360a 縦割式評価ユニット
360b 縦割式評価ユニット
364a 改善指示提出ユニット
364b 改善指示提出ユニット
400 状態推定ユニット
410 統合処理モジュール
420 サブモジュール#1
422 センサ#1
426 他者評価受入ユニット
428 地図#1
430 サブモジュール#2
432 センサ#2
434 他者評価受入ユニット
438 地図#2
440 パーティクルフィルタ
442 ノーマルパーティクルセット
444 補助的パーティクルセット
446 オドメトリモデル
450 パーティクルフィルタ
452 ノーマルパーティクルセット
454 補助的パーティクルセット
456 オドメトリモデル
600 状態推定ユニット
610 統合処理モジュール
620 サブモジュール#1
622 センサ#1
626 他者評価受入ユニット
628 地図#1
630 サブモジュール#2
632 センサ#2
634 他者評価受入ユニット
638 地図#2
640 パーティクルフィルタ
642 ノーマルパーティクルセット
644 補助的パーティクルセット
650 パーティクルフィルタ
652 ノーマルパーティクルセット
654 補助的パーティクルセット
660 記憶ユニット
666 オドメトリモデル
700 移動体
707 状態推定系
710 統合処理モジュール
720 外界センサ#1
730 外界センサ#2
740 サブモジュール#1
746 他者評価受入ユニット
748 地図
750 サブモジュール#2
756 他者評価受入ユニット
758 地図
760 サブモジュール#3
766 他者評価受入ユニット
768 地図
770 走行系
772 内界センサ
774 自己状態推定ユニット
776 自己状態記憶メモリユニット#1
778 自己状態記憶メモリユニット#2
780 移動制御機構
790 移動機構
1000 装置
1010 上位統合処理モジュール
1020 移動体#1
1022 センサ#1
1024 下位モジュール#1a
1026 下位モジュール#1b
1028 統合処理モジュール#1
1030 移動体#2
1032 センサ#2
1034 下位モジュール#2a
1036 下位モジュール#2b
1038 統合処理モジュール#2
1100 装置
1110 上位統合処理モジュール
1120 移動体#1
1124 下位モジュール#1a
1126 下位モジュール#1b
1128 統合処理モジュール#1
1130 移動体#2
1134 下位モジュール#2a
1136 下位モジュール#2b
1138 統合処理モジュール#2
1140 センサ
1200 ＣＰＵ
1202 メモリ
1204 入力装置
1206 出力装置
1208 外部記憶装置
1210 媒体駆動装置
1212 可搬記録媒体
1214 ネットワーク接続装置
1216 バス

Claims

系の状態を推定するための装置であって、
(a) ひとつ以上の第一のセンサと、
(b) ひとつ以上の第二のセンサと、
(c) 前記ひとつ以上の第一のセンサのいずれかから各々が出力を受けて記憶手段に格納する、複数の下位モジュールと
を含み、
前記複数の下位モジュールの各々が有する記憶手段は、
前記複数の下位モジュールの各々に対応する地図と、
前記系の状態を推定するための情報であり、前記ひとつ以上の第二のセンサのいずれかの出力から生成され、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新をされる、第一のデータセットと
を有し、
また前記装置はさらに、
(d) 前記複数の下位モジュールの各々から出力された前記第一のデータセットを受けとり、前記複数の下位モジュールの各々に前記第一のデータセットの改善を行わせ、前記第一のデータセットを統合することで、前記系の状態を推定して前記複数の下位モジュールの各々へフィードバックする、上位モジュール
を含む
ことを特徴とする、装置。
前記上位モジュールが、
前記複数の下位モジュールの各々に関する前記第一のデータセットを統合するにあたり、前記第一のデータセットの良さを定量化し、定量化された前記第一のデータセットの前記良さを第一の閾値と比較することで、前記第一のデータセットが良いか否かを評価する、縦割式評価手段
を含み、
前記複数の下位モジュールの各々に関する前記第一のデータセットのうち、前記縦割式評価手段によって良いとされた第一のデータセットのみを、前記上位モジュールが統合する
ことを特徴とする、請求項１記載の装置。
前記複数の下位モジュールの各々が有する前記記憶手段がさらに、
前記系の状態を推定するための情報であり、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とから生成される、第二のデータセット
を有し、
前記上位モジュールがさらに、
前記縦割式評価手段により良いとされなかった第一のデータセットに関する下位モジュールに対して改善指示を与える、改善指示手段
を含み、
前記改善指示手段から改善指示を受けた下位モジュールが、前記第二のデータセットを用いて、前記第一のデータセットの更新を行う
ことを特徴とする、請求項２記載の装置。
さらに
(e) 前記系を移動させるための、移動手段
を含み、
前記ひとつ以上の第二のセンサが、前記移動手段の出力を測定し、
前記上位モジュールからの前記移動手段へのフィードバックにより、前記移動手段による移動が調節される
ことを特徴とする、請求項１記載の装置。
請求項１〜４のいずれか一項に記載の装置を搭載して状態推定を行うことを特徴とする、移動体。
系の状態を推定するための方法であって、
地図を記憶手段に格納するステップと、
ひとつ以上の第一のセンサからの出力を、前記記憶手段に格納するステップと、
ひとつ以上の第二のセンサのいずれかの出力から、データセットを生成するステップと、
前記データセットを、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新するステップと、
更新された前記データセットの良さを評価し、該評価結果に応じて前記データセットを改善するステップと、
更新された前記データセットを統合することで、前記系の状態を推定するステップと
を含む、方法。
系の状態を推定する方法をコンピュータに実行させるためのプログラムであって、
前記方法が、
地図を記憶手段に格納するステップと、
ひとつ以上の第一のセンサからの出力を、前記記憶手段に格納するステップと、
ひとつ以上の第二のセンサのいずれかの出力から、データセットを生成するステップと、
前記データセットを、前記ひとつ以上の第一のセンサのいずれかの出力と、前記地図とに基づいて更新するステップと、
更新された前記データセットの良さを評価し、該評価結果に応じて前記データセットを改善するステップと、
更新された前記データセットを統合することで、前記系の状態を推定するステップと
を含む
ことを特徴とする、プログラム。