センサ情報の統合方法と装置
【課題】誤差と計算量を増大させずに複数のセンサ情報を統合することができる方法と装置を提供する。
【解決手段】新たに受信したセンサ情報Ｙｎを、観測時刻の順に記録し、各センサ情報に対し、第１条件を満たすか否かを判定する（Ｓ３）。第１条件は、「そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること」である。第１条件を満たす場合に、第１条件を満たす各センサ情報６を観測時刻順に用いて、内部状態を予測し、第１推定値Ｘ１を予測した内部状態に更新して記憶し、第２推定値Ｘ２を更新した第１推定値Ｘ１で上書きし、更新に用いたセンサ情報を削除する（Ｓ４，Ｓ５，Ｓ７，Ｓ８）。第１条件を満たさない場合に、新たに受信したセンサ情報Ｙｎを用いて、内部状態を予測し、第２推定値Ｘ２を予測した内部状態に更新して記憶する（Ｓ９）第２推定値Ｘ２を対象物の内部状態として出力する（Ｓ１０）。
【解決手段】新たに受信したセンサ情報Ｙｎを、観測時刻の順に記録し、各センサ情報に対し、第１条件を満たすか否かを判定する（Ｓ３）。第１条件は、「そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること」である。第１条件を満たす場合に、第１条件を満たす各センサ情報６を観測時刻順に用いて、内部状態を予測し、第１推定値Ｘ１を予測した内部状態に更新して記憶し、第２推定値Ｘ２を更新した第１推定値Ｘ１で上書きし、更新に用いたセンサ情報を削除する（Ｓ４，Ｓ５，Ｓ７，Ｓ８）。第１条件を満たさない場合に、新たに受信したセンサ情報Ｙｎを用いて、内部状態を予測し、第２推定値Ｘ２を予測した内部状態に更新して記憶する（Ｓ９）第２推定値Ｘ２を対象物の内部状態として出力する（Ｓ１０）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット、計測器など複数のセンサを使う装置におけるセンサ情報の統合方法と装置に関する。
【背景技術】
【０００２】
対象物をロボットでハンドリングする自動装置や、センサで周囲を計測しながら移動する移動ロボットなどでは、対象物やロボットの位置や速度などを計測する計測手段を複数備えていることが多い。例えば移動ロボットの場合、以下の複数のセンサを搭載していることが一般的である。
（ａ）移動ロボットの車輪の回転量を計測するエンコーダと、（ｂ）ロボットの周囲にあるランドマークの位置を計測するカメラ。
【０００３】
一般的に、複数のセンサ情報を統合する手段として、カルマンフィルタ、情報フィルタ、パーティクルフィルタなどのベイズフィルタが挙げられる。
ベイズフィルタを使った複数センサ情報の統合方法は、以下のようになる。
（１）新たなセンサ情報（どのセンサの結果でも良い）が得られたら、観測時刻における内部状態を予測する予測処理。
（２）予測した内部状態をセンサ情報と比較し修正する更新処理。
ここで内部状態とは、計測対象に関する様々な情報を事前に定義したものである。例えば、ロボットの位置を計測したい場合、ロボットの位置、姿勢、速度、角速度などを内部状態として定義する。
【０００４】
上述の処理（１）（２）によって修正された内部状態は、観測時刻時の値となる。したがって、センサ情報がＹ１，Ｙ２，Ｙ３・・・と得られた場合、内部状態の時刻も、Ｙ１の時刻，Ｙ２の時刻，Ｙ３の時刻・・・のように変化する。
【０００５】
センサが複数ある場合、センサの処理時間やセンサとの通信遅延時間が異なる影響でセンサ情報が観測時刻の順番通りに得られない。例えば、情報を統合処理する装置上で、センサ情報がＹ１，Ｙ２，Ｙ３・・・と得られたとき、Ｙ３の観測時刻がＹ２の観測時刻よりも古い場合がある。
ベイズフィルタの予測処理は、予測時間幅（Ｙ２の時刻−Ｙ１の時刻、・・・）に応じて推定の確度が減少する（推定誤差が増える）。したがって、センサ情報を観測時刻の順番通りに処理しないと、予測時間幅が長くなるため、内部状態の推定誤差が増加してしまう。
ベイズフィルタは、前回のステップでの推定結果を、次のステップの推定処理に使う。したがって、推定誤差が増加すると、以降の推定にも影響する。
【０００６】
上述した問題を解決するために、種々の手段が既に提案されている（例えば、非特許文献１，２、特許文献１）。
【０００７】
非特許文献１の方法は、センサ情報が得られるたびに内部状態を逐次更新する。前回処理したセンサ情報よりも古い時刻のセンサ情報（Ｙ）が得られたとき、Ｙの時刻の内部状態を予測し修正するものである。
特許文献１の手段は、センサごとに計測結果を蓄積し、遅れの大きいセンサについて計測データの履歴から以降の計測値を予測する。予測値と遅れの小さいセンサ情報を統合するものである。
非特許文献２の手段は、一定時間分のセンサ情報を観測時刻順に並べ替えて蓄積し、ステップごとに蓄積したデータを全て処理するものである。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】前山祥一，大矢晃久，油田信一，「移動ロボットのための遡及的現在位置推定法 処理時間を要する外界センサデータの利用」 日本ロボット学会誌，ｖｏｌ．１５，Ｎｏ．７，ｐｐ．１０７５−１０８１，１９９７
【非特許文献２】鏡 慎吾，石川正俊，「通信遅延を考慮したセンサ選択手法」 電子情報通信学会論文誌 Ｖｏｌ．Ｊ８８−Ａ，Ｎｏ．５，ｐｐ．５７７−５８７，２００５
【特許文献】
【０００９】
【特許文献１】特許第３３６７４６１号公報、「移動体姿勢角検出装置」
【発明の概要】
【発明が解決しようとする課題】
【００１０】
非特許文献１の方法は、観測時刻の順番通りに推定処理しないため、統合結果の誤差が大きくなる。この文献では、古い時刻のセンサ情報を最新時刻の推定結果に統合する方法を示している。しかしこの統合方法は、この文献が対象とする移動ロボットに対してのみ有効な方法である。また、統合の際、近似処理をするため、誤差が大きくなる。
【００１１】
特許文献１の手段は、遅延の大きいセンサが分かっていることが前提となる。したがって、各センサの遅延時間が不確定な場合に適用できない。
【００１２】
非特許文献２の方法は、蓄積する時間（許容時間）をあらかじめ設定する必要があり、許容時間を超える遅延のあるセンサ情報は推定に反映されない。また、蓄積したデータを毎回全て処理するため、許容時間に応じて、計算量が増大する。そのため、許容時間に制約がある。
【００１３】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、センサが複数あり、各センサの遅延時間が不確定であり、各センサからのセンサ情報が観測時刻の順番通りに得られない場合であっても、誤差と計算量を増大させずに複数のセンサ情報を統合することができる方法と装置を提供することにある。
【課題を解決するための手段】
【００１４】
本発明によれば、複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合方法であって、
（Ａ）データ記憶部により、新たに受信したセンサ情報を、観測時刻の順に記録し、
（Ｂ）制御部により、データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、予測部により内部状態を予測し、第１推定値記憶部により第１推定値を予測した内部状態に更新して記憶し、第２推定値記憶部により第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、予測部により内部状態を予測し、第２推定値記憶部により第２推定値を予測した内部状態に更新して記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合方法が提供される。
【００１５】
また本発明によれば、複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合装置であって、
受信したセンサ情報をその観測時刻の順に記録するデータ記憶部と、
センサ情報から対象物の内部状態を予測する予測部と、
内部状態を更新する更新部と、
内部状態の第１推定値を記憶する第１推定値記憶部と、
内部状態の第２推定値を記憶する第２推定値記憶部と、
データ記憶部、予測部、更新部、第１推定値記憶部及び第２推定値記憶部を制御する制御部とを備え、
（Ａ）新たに受信したセンサ情報を、観測時刻の順にデータ記憶部に記録し、
（Ｂ）データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、内部状態を予測し、第１推定値を予測した内部状態に更新して第１推定値記憶部に記憶し、第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、内部状態を予測し、第２推定値を予測した内部状態に更新して第２推定値記憶部に記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合装置が提供される。
【発明の効果】
【００１６】
上記本発明の方法と装置によれば、第１条件を満たす場合には、そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されているので、非特許文献２の手段と同様に、第１推定値記憶部の第１推定値は観測時刻の順にセンサ情報を処理した推定結果となる。したがって、第２推定値を第１推定値で上書きして、対象物の内部状態として出力することにより、高い精度で複数のセンサ情報を統合することができる。
【００１７】
第１条件を満たさない場合には、新たに受信したセンサ情報を用いて内部状態を予測し、第２推定値を予測した内部状態に更新して、対象物の内部状態として出力することにより、最新のセンサ情報に基づき複数のセンサ情報を統合することができる。
この場合は、時刻の古いセンサ情報を処理する場合があるため、一時的に精度が低くなることがあるが、この誤差は、第１条件を満たしたときに、精度の高い第１推定値で上書きされることで全て解消され、以降の推定に影響しない。したがって、第１条件を満たさない場合でも、第２推定値の推定結果は、非特許文献１の方法よりも精度が高くなる。
【００１８】
また、蓄積したデータを毎回全て処理するわけではなく、第１条件を満たしたタイミングでのみ処理するため、非特許文献２よりも計算量が少ない。
また、第１条件を満たす場合に処理することで、事前に許容時間を設定することなく、各センサの遅延時間が不確定でも、必ず観測時刻順にセンサ情報を処理できる。
【図面の簡単な説明】
【００１９】
【図１】本発明による統合装置を備えたロボットシステムの構成図である。
【図２】本発明による統合装置を有する計測装置の構成図である。
【図３】本発明による統合装置の構成図である。
【図４】本発明による統合方法を示すフローチャートである。
【図５】３つのセンサによる計測状態を示す説明図である。
【図６】３つのセンサから統合装置への入力順序を示す説明図である。
【図７】３つのセンサで計測した場合の、データ記憶部のセンサ情報の推移を示す説明図である。
【発明を実施するための形態】
【００２０】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２１】
図１は、本発明による統合装置を備えたロボットシステムの構成図である。
この図において、１は移動ロボット、２は移動ロボット１に搭載されたロボット上カメラ、３は外部カメラである。この例では、移動ロボット１の位置、姿勢、速度、角速度などを、ロボット上カメラ２、外部カメラ３又はレーザレーダを使って計測するようになっている。
以下、ロボット上カメラ２、外部カメラ３、レーザレーダ、およびその他の計測手段を総称して「センサ」と呼び、センサで計測された情報を「センサ情報」と呼ぶ。
【００２２】
図２は、本発明による統合装置を有する計測装置の構成図である。
この図において、本発明の統合装置１０は状態推定装置であり、複数のセンサ５からセンサ情報６が入力され、内部状態７の推定値を出力するようになっている。
複数のセンサ５によって得られたセンサ情報６は、逐次、状態推定装置１０に入力される。センサ情報６は、センサ５の計測値や、観測した時刻（観測時刻）、センサデバイスＩＤなどの情報をまとめたデータである。
状態推定装置１０は、入力されたセンサ情報６に基づいて、対象物１（図１の例では移動ロボット１）の位置、姿勢、速度、角速度などの内部状態７を推定する。
この場合、計測のタイミング、計測処理、伝送方法がセンサ５ごとに異なるため、状態推定装置１０に入力されるセンサ情報６は、観測時刻の順ではない。
【００２３】
図３は、本発明による統合装置の構成図である。
本発明による統合装置１０は状態推定装置であり、データ記憶部１２、予測部１４、更新部１６、第１推定値記憶部１７、第２推定値記憶部１８、及び制御部２０を備える。
【００２４】
本発明による統合装置１０は、複数のセンサ５からセンサ情報６をランダムに受信し、複数のセンサ情報６を統合して対象物１の内部状態７を予測し出力する装置である。
【００２５】
データ記憶部１２は、受信したセンサ情報６をその観測時刻の順に記録する。
予測部１４は、センサ情報６から対象物１の内部状態７を予測する。
更新部１６は、内部状態７を更新する。
第１推定値記憶部１７は、内部状態７の第１推定値Ｘ１を記憶する。
第２推定値記憶部１８は、内部状態７の第２推定値Ｘ２を記憶する。
制御部２０は、データ記憶部１２、予測部１４、更新部１６、第１推定値記憶部１７及び第２推定値記憶部１８を制御する。
【００２６】
第１推定値Ｘ１と第２推定値Ｘ２は、センサ情報６に基づいて推定された、対象物１の位置、姿勢、速度、角速度などである。内部状態７の内訳は、統合結果を使う目的に合わせて適宜設定する。
予測部１４と更新部１６には、従来技術で挙げたベイズフィルタの予測処理と更新処理が実装される。
データ記憶部１２は、得られたセンサ情報６を一時的に蓄積する機能を持つ。以下、蓄積されたセンサ情報６のうち、観測時刻の最も古いものをＹｂ、最も新しいものをＹｔとする。
【００２７】
図４は、本発明による統合方法を示すフローチャートである。この図に示すように本発明の統合方法は、Ｓ１〜Ｓ１０の各ステップ（工程）からなる。
【００２８】
新たなセンサ情報Ｙｎが入力されたとき、Ｓ１において、データ記憶部１２の中で観測時刻が最も古いセンサ情報Ｙｂと新たなセンサ情報Ｙｎを比較し、新たなセンサ情報Ｙｎの観測時刻の方が古い場合（ＮＯ）、推定処理を行わず、Ｓ１０で第２推定値記憶部１８に記憶された第２推定値Ｘ２をそのまま出力する。
なおこの手順が発生するのは、後述するＳ３において第２条件又は第３条件が満たされる場合のみである。
【００２９】
Ｓ１において、新たなセンサ情報Ｙｎの観測時刻が最も古いセンサ情報Ｙｂよりも新しい場合（ＹＥＳ）、Ｓ２において新たなセンサ情報Ｙｎをデータ記憶部１２に観測時刻の順に記録する。
なお図３に示したように、新たなセンサ情報Ｙｎの観測時刻が最も新しいとは限らない。
【００３０】
Ｓ３において、データ記憶部１２から観測時刻の古い順にセンサ情報６を取り出し、以下の第１条件、第２条件、第３条件のいずれかを満たすか否かを判定する。
第１条件：そのセンサ情報６の観測時刻と同じかそれよりも新しいセンサ情報６がすべてのセンサ５から受信されていること。
第２条件：そのセンサ情報６の観測時刻が、現在時刻に対して予め設定した設定時間以上古い時刻であること。
第３条件：そのセンサ情報６の観測時刻よりも新しいセンサ情報６が、予め設定した設定数以上記録されていること。
【００３１】
第１条件において、「すべてのセンサ５から受信されている」ことを判定するために、使用するセンサ５を登録したセンサリストを用意し、稼働中にセンサリストを変更してもよい。例えば、計測開始時は、Ａ，Ｂ，Ｃ，Ｄの４つのセンサ５を使い、途中からＡ，Ｂ，Ｃの３つのみに変更してもよい。変更後は，Ａ〜Ｃのセンサ情報が受信されていれば第１条件を満たす。
また、センサリストの異なる複数の統合装置を使用してもよい。例えば、システム全体ではＡ，Ｂ，Ｃ，Ｄの４つのセンサ５があるが、統合装置１はＡ，Ｂ，Ｃのみを判定し、統合装置２はＡ，Ｂ，Ｃ，Ｄで判定するようにしてもよい。
【００３２】
最も古いセンサ情報Ｙｂが第１条件を満たす場合、最も古いセンサ情報Ｙｂよりも観測時刻の古いセンサ情報が今後入力されることはない。したがって、第１条件を満たす最も古いセンサ情報Ｙｂのみ処理すれば、内部状態７の時刻を、過去から未来へ順方向に進めながら予測処理と更新処理ができる。
【００３３】
第１条件のみでは、センサ５の処理時間や通信遅延が大きいと、蓄積されるセンサ情報６が増えて、記憶領域が不足する場合がある。そこで、第２条件と第３条件を適宜組み合わせる。
例えば、第１条件と第２条件のみとの組み合わせ、又は第１条件と第３条件のみとの組み合わせであってもよい。
第２条件と第３条件を満たす最も古いセンサ情報Ｙｂを処理した場合、これ以降に最も古いセンサ情報Ｙｂより古い時刻のセンサ情報が入力されても、推定処理に使われない（Ｓ１→Ｓ１０参照）。
【００３４】
しかし、対象物１の位置と姿勢が時々刻々と変化する場合などでは、新しいセンサ情報６の方が重要である。したがって、第２条件のように、一定時間以上古い時刻のセンサ情報は推定に使わなくても問題にならないことが多い。第２条件の設定時間は、正常状態においてすべてのセンサ５から受信される時間間隔より長く、かつ遅れが許容できる範囲で設定するのがよい。
【００３５】
また、第３条件を満たす最も古いセンサ情報Ｙｂを処理することで、蓄積されているセンサ情報６の数が必ず一定数以下になるので、データ記憶部１２の容量を超えないことを保証できる。第３条件の設定数は、データ記憶部１２の容量を超えない範囲で設定するのがよい。
【００３６】
Ｓ３において、処理する（ＹＥＳ）と判定した場合、Ｓ４において、最も古いセンサ情報Ｙｂを使って、内部状態７の第１推定値Ｘ１について、次のように推定処理する。
まず、現在記録されている第１推定値Ｘ１を使って、最も古いセンサ情報Ｙｂの観測時刻における内部状態７を予測する。次に、最も古いセンサ情報Ｙｂの計測値と予測した内部状態７を比較して内部状態７を更新する。更新後の内部状態７を新たな第１推定値Ｘ１とする。
【００３７】
推定処理（予測処理、更新処理）には、従来技術であるカルマンフィルタ、情報フィルタ、パーティクルフィルタなどのアルゴリズムを用いる。
【００３８】
Ｓ５において、Ｓ４で処理した最も古いセンサ情報Ｙｂをデータ記憶部１２から削除する。Ｓ３〜Ｓ５を繰り返して、処理条件を満たすセンサ情報６を全て推定処理する。
【００３９】
Ｓ３〜Ｓ５で第１推定値Ｘ１を更新しなかった場合（Ｓ６でＮＯ）は、Ｓ９において新たなセンサ情報Ｙｎを使って、第２推定値Ｘ２に対して推定処理し、Ｓ１０で第２推定値Ｘ２を出力する。
【００４０】
Ｓ３〜Ｓ５で第１推定値Ｘ１を更新した場合（Ｓ６でＹＥＳ）は、Ｓ７で第２推定値Ｘ２を第１推定値Ｘ１で上書きし、Ｓ８で残りのセンサ情報６を全て使って、第２推定値Ｘ２について推定処理し、Ｓ１０で第２推定値Ｘ２を出力する。
【００４１】
以下、上記の動作フローの第１条件によって、センサ情報を蓄積し処理する例を示す。
【００４２】
図５は、３つのセンサＡ，Ｂ，Ｃによる計測状態を示す説明図であり、図６は、３つのセンサＡ，Ｂ，Ｃから統合装置１０への入力順序を示す説明図である。
【００４３】
図５において、３つのセンサＡ，Ｂ，Ｃはそれぞれ、センサ情報Ａ１，Ａ２，Ａ３，Ａ４、Ｂ１，Ｂ２，Ｂ３、Ｃ１，Ｃ２を順次出力する。この時、３つのセンサＡ，Ｂ，Ｃの処理時間、通信遅延などによって、状態推定装置１０に入力されるセンサ情報６の順番は図６のようになる。
図６におけるステップ１〜５は、状態推定装置１０における処理の中間段階を示している。
【００４４】
図７は、３つのセンサＡ，Ｂ，Ｃで計測した場合の、データ記憶部１２のセンサ情報６の推移を示す説明図である。
図６の例において、一連の処理によって、データ記憶部１２内のセンサ情報６は、図７のように推移する。
【００４５】
ステップ１までは、センサＣの情報がないため、第１推定値Ｘ１は更新されない。その間、状態推定装置１０は、Ａ１→Ｂ１→Ａ２の順にセンサ情報６を使って、第２推定値Ｘ２を推定処理する。
【００４６】
ステップ２の時点で、センサ情報Ａ１，Ｃ１は第１条件を満たす。したがって、第１推定値Ｘ１をセンサ情報Ａ１→Ｃ１を使って更新し、センサ情報Ａ１，Ｃ１は削除される。第２推定値Ｘ２は第１推定値Ｘ１で上書きされ、残りのデータを全て使って推定処理される。したがって、第２推定値Ｘ２は、センサ情報Ａ１→Ｃ１→Ｂ１→Ａ２と観測時刻順に処理された結果となる。
この結果、データ記憶部１２内のセンサ情報６は、センサ情報Ｂ１，Ａ２のみとなる。
【００４７】
ステップ３までは、第１条件を満たすデータはなく、新たなセンサ情報Ｂ２，Ｂ３のみを用いて第２推定値Ｘ２のみを逐次処理する。この場合、センサ情報Ｂ１，Ａ２を処理しない分、非特許文献２の方法より計算量が少ない。
【００４８】
ステップ４でも、新たなセンサ情報Ａ３のみを用いて第２推定値Ｘ２のみを処理する。この時新たに加わったセンサ情報Ａ３は、センサ情報Ｂ３よりも古い時刻なので、第２推定値Ｘ２の推定精度は一時的に低下する。
【００４９】
ステップ５にて、センサ情報Ｂ１，Ａ２，Ｂ２，Ａ３が第１条件を満たすため、第１推定値Ｘ１を更新する。第２推定値Ｘ２の最終出力は、センサ情報をＡ１→Ｃ１→Ｂ１→Ａ２→Ｂ２→Ａ３→Ｃ２→Ｂ３のように観測時刻順に処理した結果となる。この結果、ステップ４で発生した誤差が解消される。
【００５０】
データ記憶部１２にセンサ情報Ｙｎを記録する際、観測時刻が同一のセンサ情報６がある場合は、計測値の統計値をとって、１つのセンサ情報６に統合することができる。例えば、計測値の平均値をとる、或いは、非特許文献２の方法のように情報フィルタによって統合する。こうすることで、データ記憶部１２に記録するセンサ情報６を減らすことができ、消費メモリや計算量を減らすことができる。
【００５１】
上述した本発明の方法と装置によれば、第１条件を満たす場合には、そのセンサ情報６の観測時刻と同じかそれよりも新しいセンサ情報６がすべてのセンサから受信されているので、非特許文献２の手段と同様に、第１推定値記憶部１７の第１推定値Ｘ１は観測時刻の順にセンサ情報６を処理した推定結果となる。したがって、第２推定値Ｘ２を第１推定値Ｘ１で上書きして、対象物１の内部状態７として出力することにより、高い精度で複数のセンサ情報６を統合することができる。
【００５２】
第１条件を満たさない場合には、新たに受信したセンサ情報６を用いて内部状態７を予測し、第２推定値Ｘ２を予測した内部状態７に更新して、対象物１の内部状態７として出力することにより、最新のセンサ情報６に基づき複数のセンサ情報６を統合することができる。
この場合は、時刻の古いセンサ情報６を処理する場合があるため、一時的に精度が低くなることがあるが、この誤差は、第１条件を満たしたときに、精度の高い第１推定値Ｘ１で上書きされることで全て解消され、以降の推定に影響しない。したがって、第１条件を満たさない場合でも、第２推定値Ｘ２の推定結果は、非特許文献１の方法よりも精度が高くなる。
【００５３】
また、蓄積したデータを毎回全て処理するわけではなく、第１条件を満たしたタイミングでのみ処理するため、非特許文献２よりも計算量が少ない。
また、第１条件を満たす場合に処理することで、事前に許容時間を設定することなく、各センサ５の遅延時間が不確定でも、必ず観測時刻順にセンサ情報６を処理できる。
【００５４】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【符号の説明】
【００５５】
１ 移動ロボット（対象物）、２ ロボット上カメラ、３ 外部カメラ、
５ センサ、６ センサ情報、７ 内部状態、
１０ 統合装置（状態推定装置）、
１２ データ記憶部、１４ 予測部、１６ 更新部、
１７ 第１推定値記憶部、１８ 第２推定値記憶部、２０ 制御部
【技術分野】
【０００１】
本発明は、ロボット、計測器など複数のセンサを使う装置におけるセンサ情報の統合方法と装置に関する。
【背景技術】
【０００２】
対象物をロボットでハンドリングする自動装置や、センサで周囲を計測しながら移動する移動ロボットなどでは、対象物やロボットの位置や速度などを計測する計測手段を複数備えていることが多い。例えば移動ロボットの場合、以下の複数のセンサを搭載していることが一般的である。
（ａ）移動ロボットの車輪の回転量を計測するエンコーダと、（ｂ）ロボットの周囲にあるランドマークの位置を計測するカメラ。
【０００３】
一般的に、複数のセンサ情報を統合する手段として、カルマンフィルタ、情報フィルタ、パーティクルフィルタなどのベイズフィルタが挙げられる。
ベイズフィルタを使った複数センサ情報の統合方法は、以下のようになる。
（１）新たなセンサ情報（どのセンサの結果でも良い）が得られたら、観測時刻における内部状態を予測する予測処理。
（２）予測した内部状態をセンサ情報と比較し修正する更新処理。
ここで内部状態とは、計測対象に関する様々な情報を事前に定義したものである。例えば、ロボットの位置を計測したい場合、ロボットの位置、姿勢、速度、角速度などを内部状態として定義する。
【０００４】
上述の処理（１）（２）によって修正された内部状態は、観測時刻時の値となる。したがって、センサ情報がＹ１，Ｙ２，Ｙ３・・・と得られた場合、内部状態の時刻も、Ｙ１の時刻，Ｙ２の時刻，Ｙ３の時刻・・・のように変化する。
【０００５】
センサが複数ある場合、センサの処理時間やセンサとの通信遅延時間が異なる影響でセンサ情報が観測時刻の順番通りに得られない。例えば、情報を統合処理する装置上で、センサ情報がＹ１，Ｙ２，Ｙ３・・・と得られたとき、Ｙ３の観測時刻がＹ２の観測時刻よりも古い場合がある。
ベイズフィルタの予測処理は、予測時間幅（Ｙ２の時刻−Ｙ１の時刻、・・・）に応じて推定の確度が減少する（推定誤差が増える）。したがって、センサ情報を観測時刻の順番通りに処理しないと、予測時間幅が長くなるため、内部状態の推定誤差が増加してしまう。
ベイズフィルタは、前回のステップでの推定結果を、次のステップの推定処理に使う。したがって、推定誤差が増加すると、以降の推定にも影響する。
【０００６】
上述した問題を解決するために、種々の手段が既に提案されている（例えば、非特許文献１，２、特許文献１）。
【０００７】
非特許文献１の方法は、センサ情報が得られるたびに内部状態を逐次更新する。前回処理したセンサ情報よりも古い時刻のセンサ情報（Ｙ）が得られたとき、Ｙの時刻の内部状態を予測し修正するものである。
特許文献１の手段は、センサごとに計測結果を蓄積し、遅れの大きいセンサについて計測データの履歴から以降の計測値を予測する。予測値と遅れの小さいセンサ情報を統合するものである。
非特許文献２の手段は、一定時間分のセンサ情報を観測時刻順に並べ替えて蓄積し、ステップごとに蓄積したデータを全て処理するものである。
【先行技術文献】
【非特許文献】
【０００８】
【非特許文献１】前山祥一，大矢晃久，油田信一，「移動ロボットのための遡及的現在位置推定法 処理時間を要する外界センサデータの利用」 日本ロボット学会誌，ｖｏｌ．１５，Ｎｏ．７，ｐｐ．１０７５−１０８１，１９９７
【非特許文献２】鏡 慎吾，石川正俊，「通信遅延を考慮したセンサ選択手法」 電子情報通信学会論文誌 Ｖｏｌ．Ｊ８８−Ａ，Ｎｏ．５，ｐｐ．５７７−５８７，２００５
【特許文献】
【０００９】
【特許文献１】特許第３３６７４６１号公報、「移動体姿勢角検出装置」
【発明の概要】
【発明が解決しようとする課題】
【００１０】
非特許文献１の方法は、観測時刻の順番通りに推定処理しないため、統合結果の誤差が大きくなる。この文献では、古い時刻のセンサ情報を最新時刻の推定結果に統合する方法を示している。しかしこの統合方法は、この文献が対象とする移動ロボットに対してのみ有効な方法である。また、統合の際、近似処理をするため、誤差が大きくなる。
【００１１】
特許文献１の手段は、遅延の大きいセンサが分かっていることが前提となる。したがって、各センサの遅延時間が不確定な場合に適用できない。
【００１２】
非特許文献２の方法は、蓄積する時間（許容時間）をあらかじめ設定する必要があり、許容時間を超える遅延のあるセンサ情報は推定に反映されない。また、蓄積したデータを毎回全て処理するため、許容時間に応じて、計算量が増大する。そのため、許容時間に制約がある。
【００１３】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、センサが複数あり、各センサの遅延時間が不確定であり、各センサからのセンサ情報が観測時刻の順番通りに得られない場合であっても、誤差と計算量を増大させずに複数のセンサ情報を統合することができる方法と装置を提供することにある。
【課題を解決するための手段】
【００１４】
本発明によれば、複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合方法であって、
（Ａ）データ記憶部により、新たに受信したセンサ情報を、観測時刻の順に記録し、
（Ｂ）制御部により、データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、予測部により内部状態を予測し、第１推定値記憶部により第１推定値を予測した内部状態に更新して記憶し、第２推定値記憶部により第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、予測部により内部状態を予測し、第２推定値記憶部により第２推定値を予測した内部状態に更新して記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合方法が提供される。
【００１５】
また本発明によれば、複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合装置であって、
受信したセンサ情報をその観測時刻の順に記録するデータ記憶部と、
センサ情報から対象物の内部状態を予測する予測部と、
内部状態を更新する更新部と、
内部状態の第１推定値を記憶する第１推定値記憶部と、
内部状態の第２推定値を記憶する第２推定値記憶部と、
データ記憶部、予測部、更新部、第１推定値記憶部及び第２推定値記憶部を制御する制御部とを備え、
（Ａ）新たに受信したセンサ情報を、観測時刻の順にデータ記憶部に記録し、
（Ｂ）データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、内部状態を予測し、第１推定値を予測した内部状態に更新して第１推定値記憶部に記憶し、第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、内部状態を予測し、第２推定値を予測した内部状態に更新して第２推定値記憶部に記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合装置が提供される。
【発明の効果】
【００１６】
上記本発明の方法と装置によれば、第１条件を満たす場合には、そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されているので、非特許文献２の手段と同様に、第１推定値記憶部の第１推定値は観測時刻の順にセンサ情報を処理した推定結果となる。したがって、第２推定値を第１推定値で上書きして、対象物の内部状態として出力することにより、高い精度で複数のセンサ情報を統合することができる。
【００１７】
第１条件を満たさない場合には、新たに受信したセンサ情報を用いて内部状態を予測し、第２推定値を予測した内部状態に更新して、対象物の内部状態として出力することにより、最新のセンサ情報に基づき複数のセンサ情報を統合することができる。
この場合は、時刻の古いセンサ情報を処理する場合があるため、一時的に精度が低くなることがあるが、この誤差は、第１条件を満たしたときに、精度の高い第１推定値で上書きされることで全て解消され、以降の推定に影響しない。したがって、第１条件を満たさない場合でも、第２推定値の推定結果は、非特許文献１の方法よりも精度が高くなる。
【００１８】
また、蓄積したデータを毎回全て処理するわけではなく、第１条件を満たしたタイミングでのみ処理するため、非特許文献２よりも計算量が少ない。
また、第１条件を満たす場合に処理することで、事前に許容時間を設定することなく、各センサの遅延時間が不確定でも、必ず観測時刻順にセンサ情報を処理できる。
【図面の簡単な説明】
【００１９】
【図１】本発明による統合装置を備えたロボットシステムの構成図である。
【図２】本発明による統合装置を有する計測装置の構成図である。
【図３】本発明による統合装置の構成図である。
【図４】本発明による統合方法を示すフローチャートである。
【図５】３つのセンサによる計測状態を示す説明図である。
【図６】３つのセンサから統合装置への入力順序を示す説明図である。
【図７】３つのセンサで計測した場合の、データ記憶部のセンサ情報の推移を示す説明図である。
【発明を実施するための形態】
【００２０】
以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２１】
図１は、本発明による統合装置を備えたロボットシステムの構成図である。
この図において、１は移動ロボット、２は移動ロボット１に搭載されたロボット上カメラ、３は外部カメラである。この例では、移動ロボット１の位置、姿勢、速度、角速度などを、ロボット上カメラ２、外部カメラ３又はレーザレーダを使って計測するようになっている。
以下、ロボット上カメラ２、外部カメラ３、レーザレーダ、およびその他の計測手段を総称して「センサ」と呼び、センサで計測された情報を「センサ情報」と呼ぶ。
【００２２】
図２は、本発明による統合装置を有する計測装置の構成図である。
この図において、本発明の統合装置１０は状態推定装置であり、複数のセンサ５からセンサ情報６が入力され、内部状態７の推定値を出力するようになっている。
複数のセンサ５によって得られたセンサ情報６は、逐次、状態推定装置１０に入力される。センサ情報６は、センサ５の計測値や、観測した時刻（観測時刻）、センサデバイスＩＤなどの情報をまとめたデータである。
状態推定装置１０は、入力されたセンサ情報６に基づいて、対象物１（図１の例では移動ロボット１）の位置、姿勢、速度、角速度などの内部状態７を推定する。
この場合、計測のタイミング、計測処理、伝送方法がセンサ５ごとに異なるため、状態推定装置１０に入力されるセンサ情報６は、観測時刻の順ではない。
【００２３】
図３は、本発明による統合装置の構成図である。
本発明による統合装置１０は状態推定装置であり、データ記憶部１２、予測部１４、更新部１６、第１推定値記憶部１７、第２推定値記憶部１８、及び制御部２０を備える。
【００２４】
本発明による統合装置１０は、複数のセンサ５からセンサ情報６をランダムに受信し、複数のセンサ情報６を統合して対象物１の内部状態７を予測し出力する装置である。
【００２５】
データ記憶部１２は、受信したセンサ情報６をその観測時刻の順に記録する。
予測部１４は、センサ情報６から対象物１の内部状態７を予測する。
更新部１６は、内部状態７を更新する。
第１推定値記憶部１７は、内部状態７の第１推定値Ｘ１を記憶する。
第２推定値記憶部１８は、内部状態７の第２推定値Ｘ２を記憶する。
制御部２０は、データ記憶部１２、予測部１４、更新部１６、第１推定値記憶部１７及び第２推定値記憶部１８を制御する。
【００２６】
第１推定値Ｘ１と第２推定値Ｘ２は、センサ情報６に基づいて推定された、対象物１の位置、姿勢、速度、角速度などである。内部状態７の内訳は、統合結果を使う目的に合わせて適宜設定する。
予測部１４と更新部１６には、従来技術で挙げたベイズフィルタの予測処理と更新処理が実装される。
データ記憶部１２は、得られたセンサ情報６を一時的に蓄積する機能を持つ。以下、蓄積されたセンサ情報６のうち、観測時刻の最も古いものをＹｂ、最も新しいものをＹｔとする。
【００２７】
図４は、本発明による統合方法を示すフローチャートである。この図に示すように本発明の統合方法は、Ｓ１〜Ｓ１０の各ステップ（工程）からなる。
【００２８】
新たなセンサ情報Ｙｎが入力されたとき、Ｓ１において、データ記憶部１２の中で観測時刻が最も古いセンサ情報Ｙｂと新たなセンサ情報Ｙｎを比較し、新たなセンサ情報Ｙｎの観測時刻の方が古い場合（ＮＯ）、推定処理を行わず、Ｓ１０で第２推定値記憶部１８に記憶された第２推定値Ｘ２をそのまま出力する。
なおこの手順が発生するのは、後述するＳ３において第２条件又は第３条件が満たされる場合のみである。
【００２９】
Ｓ１において、新たなセンサ情報Ｙｎの観測時刻が最も古いセンサ情報Ｙｂよりも新しい場合（ＹＥＳ）、Ｓ２において新たなセンサ情報Ｙｎをデータ記憶部１２に観測時刻の順に記録する。
なお図３に示したように、新たなセンサ情報Ｙｎの観測時刻が最も新しいとは限らない。
【００３０】
Ｓ３において、データ記憶部１２から観測時刻の古い順にセンサ情報６を取り出し、以下の第１条件、第２条件、第３条件のいずれかを満たすか否かを判定する。
第１条件：そのセンサ情報６の観測時刻と同じかそれよりも新しいセンサ情報６がすべてのセンサ５から受信されていること。
第２条件：そのセンサ情報６の観測時刻が、現在時刻に対して予め設定した設定時間以上古い時刻であること。
第３条件：そのセンサ情報６の観測時刻よりも新しいセンサ情報６が、予め設定した設定数以上記録されていること。
【００３１】
第１条件において、「すべてのセンサ５から受信されている」ことを判定するために、使用するセンサ５を登録したセンサリストを用意し、稼働中にセンサリストを変更してもよい。例えば、計測開始時は、Ａ，Ｂ，Ｃ，Ｄの４つのセンサ５を使い、途中からＡ，Ｂ，Ｃの３つのみに変更してもよい。変更後は，Ａ〜Ｃのセンサ情報が受信されていれば第１条件を満たす。
また、センサリストの異なる複数の統合装置を使用してもよい。例えば、システム全体ではＡ，Ｂ，Ｃ，Ｄの４つのセンサ５があるが、統合装置１はＡ，Ｂ，Ｃのみを判定し、統合装置２はＡ，Ｂ，Ｃ，Ｄで判定するようにしてもよい。
【００３２】
最も古いセンサ情報Ｙｂが第１条件を満たす場合、最も古いセンサ情報Ｙｂよりも観測時刻の古いセンサ情報が今後入力されることはない。したがって、第１条件を満たす最も古いセンサ情報Ｙｂのみ処理すれば、内部状態７の時刻を、過去から未来へ順方向に進めながら予測処理と更新処理ができる。
【００３３】
第１条件のみでは、センサ５の処理時間や通信遅延が大きいと、蓄積されるセンサ情報６が増えて、記憶領域が不足する場合がある。そこで、第２条件と第３条件を適宜組み合わせる。
例えば、第１条件と第２条件のみとの組み合わせ、又は第１条件と第３条件のみとの組み合わせであってもよい。
第２条件と第３条件を満たす最も古いセンサ情報Ｙｂを処理した場合、これ以降に最も古いセンサ情報Ｙｂより古い時刻のセンサ情報が入力されても、推定処理に使われない（Ｓ１→Ｓ１０参照）。
【００３４】
しかし、対象物１の位置と姿勢が時々刻々と変化する場合などでは、新しいセンサ情報６の方が重要である。したがって、第２条件のように、一定時間以上古い時刻のセンサ情報は推定に使わなくても問題にならないことが多い。第２条件の設定時間は、正常状態においてすべてのセンサ５から受信される時間間隔より長く、かつ遅れが許容できる範囲で設定するのがよい。
【００３５】
また、第３条件を満たす最も古いセンサ情報Ｙｂを処理することで、蓄積されているセンサ情報６の数が必ず一定数以下になるので、データ記憶部１２の容量を超えないことを保証できる。第３条件の設定数は、データ記憶部１２の容量を超えない範囲で設定するのがよい。
【００３６】
Ｓ３において、処理する（ＹＥＳ）と判定した場合、Ｓ４において、最も古いセンサ情報Ｙｂを使って、内部状態７の第１推定値Ｘ１について、次のように推定処理する。
まず、現在記録されている第１推定値Ｘ１を使って、最も古いセンサ情報Ｙｂの観測時刻における内部状態７を予測する。次に、最も古いセンサ情報Ｙｂの計測値と予測した内部状態７を比較して内部状態７を更新する。更新後の内部状態７を新たな第１推定値Ｘ１とする。
【００３７】
推定処理（予測処理、更新処理）には、従来技術であるカルマンフィルタ、情報フィルタ、パーティクルフィルタなどのアルゴリズムを用いる。
【００３８】
Ｓ５において、Ｓ４で処理した最も古いセンサ情報Ｙｂをデータ記憶部１２から削除する。Ｓ３〜Ｓ５を繰り返して、処理条件を満たすセンサ情報６を全て推定処理する。
【００３９】
Ｓ３〜Ｓ５で第１推定値Ｘ１を更新しなかった場合（Ｓ６でＮＯ）は、Ｓ９において新たなセンサ情報Ｙｎを使って、第２推定値Ｘ２に対して推定処理し、Ｓ１０で第２推定値Ｘ２を出力する。
【００４０】
Ｓ３〜Ｓ５で第１推定値Ｘ１を更新した場合（Ｓ６でＹＥＳ）は、Ｓ７で第２推定値Ｘ２を第１推定値Ｘ１で上書きし、Ｓ８で残りのセンサ情報６を全て使って、第２推定値Ｘ２について推定処理し、Ｓ１０で第２推定値Ｘ２を出力する。
【００４１】
以下、上記の動作フローの第１条件によって、センサ情報を蓄積し処理する例を示す。
【００４２】
図５は、３つのセンサＡ，Ｂ，Ｃによる計測状態を示す説明図であり、図６は、３つのセンサＡ，Ｂ，Ｃから統合装置１０への入力順序を示す説明図である。
【００４３】
図５において、３つのセンサＡ，Ｂ，Ｃはそれぞれ、センサ情報Ａ１，Ａ２，Ａ３，Ａ４、Ｂ１，Ｂ２，Ｂ３、Ｃ１，Ｃ２を順次出力する。この時、３つのセンサＡ，Ｂ，Ｃの処理時間、通信遅延などによって、状態推定装置１０に入力されるセンサ情報６の順番は図６のようになる。
図６におけるステップ１〜５は、状態推定装置１０における処理の中間段階を示している。
【００４４】
図７は、３つのセンサＡ，Ｂ，Ｃで計測した場合の、データ記憶部１２のセンサ情報６の推移を示す説明図である。
図６の例において、一連の処理によって、データ記憶部１２内のセンサ情報６は、図７のように推移する。
【００４５】
ステップ１までは、センサＣの情報がないため、第１推定値Ｘ１は更新されない。その間、状態推定装置１０は、Ａ１→Ｂ１→Ａ２の順にセンサ情報６を使って、第２推定値Ｘ２を推定処理する。
【００４６】
ステップ２の時点で、センサ情報Ａ１，Ｃ１は第１条件を満たす。したがって、第１推定値Ｘ１をセンサ情報Ａ１→Ｃ１を使って更新し、センサ情報Ａ１，Ｃ１は削除される。第２推定値Ｘ２は第１推定値Ｘ１で上書きされ、残りのデータを全て使って推定処理される。したがって、第２推定値Ｘ２は、センサ情報Ａ１→Ｃ１→Ｂ１→Ａ２と観測時刻順に処理された結果となる。
この結果、データ記憶部１２内のセンサ情報６は、センサ情報Ｂ１，Ａ２のみとなる。
【００４７】
ステップ３までは、第１条件を満たすデータはなく、新たなセンサ情報Ｂ２，Ｂ３のみを用いて第２推定値Ｘ２のみを逐次処理する。この場合、センサ情報Ｂ１，Ａ２を処理しない分、非特許文献２の方法より計算量が少ない。
【００４８】
ステップ４でも、新たなセンサ情報Ａ３のみを用いて第２推定値Ｘ２のみを処理する。この時新たに加わったセンサ情報Ａ３は、センサ情報Ｂ３よりも古い時刻なので、第２推定値Ｘ２の推定精度は一時的に低下する。
【００４９】
ステップ５にて、センサ情報Ｂ１，Ａ２，Ｂ２，Ａ３が第１条件を満たすため、第１推定値Ｘ１を更新する。第２推定値Ｘ２の最終出力は、センサ情報をＡ１→Ｃ１→Ｂ１→Ａ２→Ｂ２→Ａ３→Ｃ２→Ｂ３のように観測時刻順に処理した結果となる。この結果、ステップ４で発生した誤差が解消される。
【００５０】
データ記憶部１２にセンサ情報Ｙｎを記録する際、観測時刻が同一のセンサ情報６がある場合は、計測値の統計値をとって、１つのセンサ情報６に統合することができる。例えば、計測値の平均値をとる、或いは、非特許文献２の方法のように情報フィルタによって統合する。こうすることで、データ記憶部１２に記録するセンサ情報６を減らすことができ、消費メモリや計算量を減らすことができる。
【００５１】
上述した本発明の方法と装置によれば、第１条件を満たす場合には、そのセンサ情報６の観測時刻と同じかそれよりも新しいセンサ情報６がすべてのセンサから受信されているので、非特許文献２の手段と同様に、第１推定値記憶部１７の第１推定値Ｘ１は観測時刻の順にセンサ情報６を処理した推定結果となる。したがって、第２推定値Ｘ２を第１推定値Ｘ１で上書きして、対象物１の内部状態７として出力することにより、高い精度で複数のセンサ情報６を統合することができる。
【００５２】
第１条件を満たさない場合には、新たに受信したセンサ情報６を用いて内部状態７を予測し、第２推定値Ｘ２を予測した内部状態７に更新して、対象物１の内部状態７として出力することにより、最新のセンサ情報６に基づき複数のセンサ情報６を統合することができる。
この場合は、時刻の古いセンサ情報６を処理する場合があるため、一時的に精度が低くなることがあるが、この誤差は、第１条件を満たしたときに、精度の高い第１推定値Ｘ１で上書きされることで全て解消され、以降の推定に影響しない。したがって、第１条件を満たさない場合でも、第２推定値Ｘ２の推定結果は、非特許文献１の方法よりも精度が高くなる。
【００５３】
また、蓄積したデータを毎回全て処理するわけではなく、第１条件を満たしたタイミングでのみ処理するため、非特許文献２よりも計算量が少ない。
また、第１条件を満たす場合に処理することで、事前に許容時間を設定することなく、各センサ５の遅延時間が不確定でも、必ず観測時刻順にセンサ情報６を処理できる。
【００５４】
なお、本発明は上述した実施形態に限定されず、特許請求の範囲の記載によって示され、さらに特許請求の範囲の記載と均等の意味および範囲内でのすべての変更を含むものである。
【符号の説明】
【００５５】
１ 移動ロボット（対象物）、２ ロボット上カメラ、３ 外部カメラ、
５ センサ、６ センサ情報、７ 内部状態、
１０ 統合装置（状態推定装置）、
１２ データ記憶部、１４ 予測部、１６ 更新部、
１７ 第１推定値記憶部、１８ 第２推定値記憶部、２０ 制御部
【特許請求の範囲】
【請求項１】
複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合方法であって、
（Ａ）データ記憶部により、新たに受信したセンサ情報を、観測時刻の順に記録し、
（Ｂ）制御部により、データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、予測部により内部状態を予測し、第１推定値記憶部により第１推定値を予測した内部状態に更新して記憶し、第２推定値記憶部により第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、予測部により内部状態を予測し、第２推定値記憶部により第２推定値を予測した内部状態に更新して記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合方法。
【請求項２】
第２条件：そのセンサ情報の観測時刻が、現在時刻に対して遅れが許容できる設定時間以上古い時刻であること、又は
第３条件：そのセンサ情報の観測時刻よりも新しいセンサ情報が、データ記憶部の容量を超えない設定数以上記録されていること、が満たされる場合に第１条件と同様に処理する、ことを特徴とする請求項１に記載のセンサ情報の統合方法。
【請求項３】
使用するセンサを登録した複数のセンサリストを用意し、稼働中にセンサリストを変更し、前記第１条件を満たすか否かを判定する、ことを特徴とする請求項１に記載のセンサ情報の統合方法。
【請求項４】
複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合装置であって、
受信したセンサ情報をその観測時刻の順に記録するデータ記憶部と、
センサ情報から対象物の内部状態を予測する予測部と、
内部状態を更新する更新部と、
内部状態の第１推定値を記憶する第１推定値記憶部と、
内部状態の第２推定値を記憶する第２推定値記憶部と、
データ記憶部、予測部、更新部、第１推定値記憶部及び第２推定値記憶部を制御する制御部とを備え、
（Ａ）新たに受信したセンサ情報を、観測時刻の順にデータ記憶部に記録し、
（Ｂ）データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、内部状態を予測し、第１推定値を予測した内部状態に更新して第１推定値記憶部に記憶し、第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、内部状態を予測し、第２推定値を予測した内部状態に更新して第２推定値記憶部に記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合装置。
【請求項５】
使用するセンサを登録したセンサリストをデータ記憶部に記録する複数の統合装置を使用し、各統合装置により対応するセンサリストに登録されたセンサから前記第１条件を満たすか否かを判定する、ことを特徴とする請求項４に記載のセンサ情報の統合装置。
【請求項１】
複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合方法であって、
（Ａ）データ記憶部により、新たに受信したセンサ情報を、観測時刻の順に記録し、
（Ｂ）制御部により、データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、予測部により内部状態を予測し、第１推定値記憶部により第１推定値を予測した内部状態に更新して記憶し、第２推定値記憶部により第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、予測部により内部状態を予測し、第２推定値記憶部により第２推定値を予測した内部状態に更新して記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合方法。
【請求項２】
第２条件：そのセンサ情報の観測時刻が、現在時刻に対して遅れが許容できる設定時間以上古い時刻であること、又は
第３条件：そのセンサ情報の観測時刻よりも新しいセンサ情報が、データ記憶部の容量を超えない設定数以上記録されていること、が満たされる場合に第１条件と同様に処理する、ことを特徴とする請求項１に記載のセンサ情報の統合方法。
【請求項３】
使用するセンサを登録した複数のセンサリストを用意し、稼働中にセンサリストを変更し、前記第１条件を満たすか否かを判定する、ことを特徴とする請求項１に記載のセンサ情報の統合方法。
【請求項４】
複数のセンサからセンサ情報をランダムに受信し、複数のセンサ情報を統合して対象物の内部状態を予測し出力するセンサ情報の統合装置であって、
受信したセンサ情報をその観測時刻の順に記録するデータ記憶部と、
センサ情報から対象物の内部状態を予測する予測部と、
内部状態を更新する更新部と、
内部状態の第１推定値を記憶する第１推定値記憶部と、
内部状態の第２推定値を記憶する第２推定値記憶部と、
データ記憶部、予測部、更新部、第１推定値記憶部及び第２推定値記憶部を制御する制御部とを備え、
（Ａ）新たに受信したセンサ情報を、観測時刻の順にデータ記憶部に記録し、
（Ｂ）データ記憶部に記録された各センサ情報に対し、第１条件を満たすか否かを判定し、
第１条件：そのセンサ情報の観測時刻と同じかそれよりも新しいセンサ情報がすべてのセンサから受信されていること、
（Ｃ）第１条件を満たす場合に、第１条件を満たす各センサ情報を観測時刻順に用いて、内部状態を予測し、第１推定値を予測した内部状態に更新して第１推定値記憶部に記憶し、第２推定値を更新した第１推定値で上書きし、更新に用いたセンサ情報を削除し、
（Ｄ）第１条件を満たさない場合に、新たに受信したセンサ情報を用いて、内部状態を予測し、第２推定値を予測した内部状態に更新して第２推定値記憶部に記憶し、
（Ｅ）第２推定値記憶部の第２推定値を対象物の内部状態として出力する、ことを特徴とするセンサ情報の統合装置。
【請求項５】
使用するセンサを登録したセンサリストをデータ記憶部に記録する複数の統合装置を使用し、各統合装置により対応するセンサリストに登録されたセンサから前記第１条件を満たすか否かを判定する、ことを特徴とする請求項４に記載のセンサ情報の統合装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２０１３−５０８５２（Ｐ２０１３−５０８５２Ａ）
【公開日】平成２５年３月１４日（２０１３．３．１４）
【国際特許分類】
【出願番号】特願２０１１−１８８４８０（Ｐ２０１１−１８８４８０）
【出願日】平成２３年８月３１日（２０１１．８．３１）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
【公開日】平成２５年３月１４日（２０１３．３．１４）
【国際特許分類】
【出願日】平成２３年８月３１日（２０１１．８．３１）
【出願人】（０００００００９９）株式会社ＩＨＩ (5,014)
【Ｆターム（参考）】
[ Back to top ]