JP6448564B2 - データ処理装置、データ処理方法及びデータ処理プログラム - Google Patents

Description

本発明は、自律センサと補強センサとを複合利用して移動体の位置及び姿勢を標定するデータ処理装置、データ処理方法及びデータ処理プログラムに関する。

位置姿勢標定装置では、慣性装置のような自律センサと、ＧＰＳ（Ｇｌｏｂａｌ・Ｐｏｓｉｔｉｏｎｉｎｇ・Ｓｙｓｔｅｍ）のような補強センサとを複合利用している。このような位置姿勢標定装置では、演算処理と結果出力をリアルタイム化することが望まれている。リアルタイム化とは、演算結果のタイムタグと演算結果を出力した時刻とにずれがないようにすることである。従来、アルゴリズムの開発・評価を実施する作業はログデータの後処理において実施していた。

通常、自律センサと補強センサとには遅れ時間が存在する。よって、リアルタイム化の際に、位置姿勢標定処理のターゲット時刻をリアルタイムとしてしまうと、データが利用不可である上に、各センサデータの更新周期のタイミングもまちまちであることから、何らかの工夫によりデータのハンドリングを行わなければならない（特許文献１参照）。

特開２００７−１５５５８１号公報

従来のリアルタイム化では、時刻を遡った処理やその分先に進める（もどす）処理といった煩雑な処理を実装する必要があるという課題があった。
本発明は、簡単な構成でリアルタイム化を実現することができるデータ処理装置を提供することを目的とする。

本発明に係るデータ処理装置は、移動体に搭載された複数のセンサにより取得された複数のセンサデータに基づいて、前記移動体の状態を移動体情報として算出するデータ処理装置において、
前記複数のセンサデータが前記移動体情報の算出に用いられる情報に処理された複数のセンサ情報であって、前記複数のセンサデータの各センサデータの処理が完了した処理完了時刻が各センサ情報に付与された複数のセンサ情報を記憶する記憶部と、
前記複数のセンサ情報のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、前記移動体情報を算出する対象の時刻であるターゲット時刻を設定する時刻設定部と、
前記ターゲット時刻の以前で、かつ、前記ターゲット時刻に最も近い処理完了時刻が付与された前記複数のセンサ情報の各センサ情報を用いて、前記移動体情報を算出する算出部とを備えた。

本発明に係るデータ処理装置によれば、時刻設定部が、複数のセンサ情報のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報、すなわち最も遅れ時間の大きいセンサ情報の処理完了時刻に基づいて、ターゲット時刻を設定し、算出部が、ターゲット時刻の直前の処理完了時刻、すなわちターゲット時刻より前の直近の処理完了時刻が付与された複数のセンサ情報の各センサ情報に基づいて移動体情報を算出するので、簡単な構成で、最もリアルタイムに近く、かつ、高精度の移動体情報を算出することができるという効果を奏する。

実施の形態１に係るデータ処理装置１００の構成について説明する。 実施の形態１に係るデータ処理装置１００のデータ処理方法５１０及びデータ処理プログラム５２０のデータ処理Ｓ１００を示すフロー図である。 実施の形態１に係る時刻設定処理Ｓ１２０を示すフロー図である。 実施の形態１に係る時刻設定処理Ｓ１２０を示す模式図である。 実施の形態１に係る算出処理Ｓ１３０を示すフロー図である。 実施の形態１の変形例に係るデータ処理装置１００の構成について説明する。 実施の形態２に係るデータ処理装置１００ａの構成を示す図である。 実施の形態２に係るデータ処理装置１００ａによるデータ処理Ｓ１００ａを示すフロー図である。 実施の形態２に係る補間処理Ｓ１４０を示すフロー図である。 実施の形態２に係る補間処理Ｓ１４０を示す模式図である。 実施の形態３に係るデータ処理装置１００ｂの構成を示す図である。 実施の形態３に係るフォーマット情報３００の一例を示す図である。 実施の形態３の効果を示す模式図である。

実施の形態１．
＊＊＊構成の説明＊＊＊
図１を用いて、本実施の形態に係るデータ処理装置１００の構成について説明する。
本実施の形態において、データ処理装置１００は、コンピュータである。データ処理装置１００は、プロセッサ９１０、記憶装置９２０、入力インタフェース９３０、出力インタフェース９４０といったハードウェアを備える。記憶装置９２０は、メモリ９２１と補助記憶装置９２２とを含む。

また、データ処理装置１００は、機能構成として、時刻設定部１２０と、算出部１３０と、センサデータ処理部１４０と、記憶部１５０とを備える。センサデータ処理部１４０は、自律センサデータ処理部１４１、補強センサデータＡ処理部１４２、補強センサデータＢ処理部１４３を備える。以下の説明では、データ処理装置１００における時刻設定部１２０と、算出部１３０と、センサデータ処理部１４０との機能を、データ処理装置１００の「部」の機能という。データ処理装置１００の「部」の機能は、ソフトウェアで実現される。

また、記憶部１５０は、メモリ９２１により実現される。記憶部１５０には、複数のセンサ情報３２０が記憶される。複数のセンサ情報３２０の各センサ情報は、自律センサ情報１１、補強センサＡ情報１２、補強センサＢ情報１３が記憶される。本実施の形態では、記憶部１５０はメモリ９２１により実現されているが、メモリ９２１及び補助記憶装置９２２により実現されていてもよい。記憶部１５０の実現方法は任意である。

プロセッサ９１０は、信号線を介して他のハードウェアと接続され、これら他のハードウェアを制御する。
プロセッサ９１０は、プロセッシングを行うＩＣ（Ｉｎｔｅｇｒａｔｅｄ・Ｃｉｒｃｕｉｔ）である。プロセッサ９１０は、ＣＰＵ（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ）である。

メモリ９２１は、具体的には、ＲＡＭ（Ｒａｎｄｏｍ・Ａｃｃｅｓｓ・Ｍｅｍｏｒｙ）である。補助記憶装置９２２は、具体的には、ＲＯＭ（Ｒｅａｄ・Ｏｎｌｙ・Ｍｅｍｏｒｙ）、フラッシュメモリ、又は、ＨＤＤ（Ｈａｒｄ・Ｄｉｓｋ・Ｄｒｉｖｅ）である。

入力インタフェース９３０は、複数のセンサ２００に接続されるポートである。本実施の形態では、複数のセンサ２００は自律センサ２０１、補強センサＡ２０２、補強センサＢ２０３である。入力インタフェース９３０は、複数のセンサ２００の各々で計測されたセンサデータ３１０をデータ処理装置１００に取り込む。入力インタフェース９３０により取り込まれたセンサデータ３１０はメモリ９２１に記憶される。入力インタフェース９３０は、マウス、キーボード、タッチパネルといった入力装置と接続されるポートであってもよい。入力インタフェース９３０は、具体的には、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ・Ｓｅｒｉａｌ・Ｂｕｓ）端子、シリアルインタフェース（ＲＳ−２３２Ｃ）などである。なお、入力インタフェース９３０は、ＬＡＮ（Ｌｏｃａｌ・Ａｒｅａ・Ｎｅｔｗｏｒｋ）と接続されるポートであってもよい。

出力インタフェース９４０は、ディスプレイといった表示機器のケーブルが接続されるポートである。出力インタフェース９４０は、具体的には、ＵＳＢ端子、ＨＤＭＩ（登録商標）（Ｈｉｇｈ・Ｄｅｆｉｎｉｔｉｏｎ・Ｍｕｌｔｉｍｅｄｉａ・Ｉｎｔｅｒｆａｃｅ）端子、シリアルインタフェース（ＲＳ−２３２Ｃ）などである。ディスプレイは、具体的には、ＬＣＤ（Ｌｉｑｕｉｄ・Ｃｒｙｓｔａｌ・Ｄｉｓｐｌａｙ）である。

補助記憶装置９２２には、「部」の機能を実現するプログラムが記憶されている。「部」の機能を実現するプログラムをデータ処理プログラム５２０ともいう。このプログラムは、メモリ９２１にロードされ、プロセッサ９１０に読み込まれ、プロセッサ９１０によって実行される。また、補助記憶装置９２２には、ＯＳ（Ｏｐｅｒａｔｉｎｇ・Ｓｙｓｔｅｍ）も記憶されている。ＯＳの少なくとも一部がメモリ９２１にロードされ、プロセッサ９１０はＯＳを実行しながら、「部」の機能を実現するプログラムを実行する。

データ処理装置１００は、１つのプロセッサ９１０のみを備えていてもよいし、複数のプロセッサ９１０を備えていてもよい。複数のプロセッサ９１０が「部」の機能を実現するプログラムを連携して実行してもよい。

「部」の処理の結果を示す情報、データ、信号値、及び、変数値は、補助記憶装置９２２、メモリ９２１、又は、プロセッサ９１０内のレジスタ又はキャッシュメモリに記憶される。
なお、図１において、各部と記憶部１５０とを結ぶ矢印は、各部が処理の結果を記憶部１５０に記憶すること、あるいは、各部が記憶部１５０から情報を読み出すことを表している。また、各部を結ぶ矢印は、制御あるいはデータの流れを表している。

「部」の機能を実現するプログラムは、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ブルーレイ（登録商標）ディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）といった可搬記録媒体に記憶されてもよい。
なお、データ処理プログラムプロダクトと称されるものは、「部」として説明している機能を実現するプログラムが記録された記憶媒体及び記憶装置であり、見た目の形式に関わらず、コンピュータ読み取り可能なプログラムをロードしているものである。

＊＊＊動作の説明＊＊＊
図２は、本実施の形態に係るデータ処理装置１００のデータ処理方法５１０及びデータ処理プログラム５２０のデータ処理Ｓ１００を示すフロー図である。
データ処理装置１００は、移動体に搭載された複数のセンサ２００により取得された複数のセンサデータ３１０に基づいて、移動体の状態を移動体情報１３１として算出する。
移動体の状態を表す移動体情報１３１とは、具体的には、移動体の位置及び姿勢である。移動体情報１３１を算出するとは移動体の位置及び姿勢を標定することであり、データ処理装置１００は移動体の位置及び姿勢を標定する位置姿勢標定装置ともいう。また、データ処理方法５１０を位置姿勢標定方法、データ処理プログラム５２０を位置姿勢標定プログラムともいう。

記憶処理Ｓ１１０において、記憶部１５０は、複数のセンサデータ３１０が移動体情報１３１の算出に用いられる情報に処理された複数のセンサ情報３２０を記憶する。
ここで、複数のセンサデータ３１０とは複数のセンサ２００の各々により取得されるセンサデータである。具体的には、自律センサ２０１により取得される自律センサデータ３１１、補強センサＡ２０２により取得される補強センサデータＡ３１２、補強センサＢ２０３により取得される補強センサデータＢ３１３である。
また、複数のセンサ情報３２０とは、複数のセンサデータ３１０の各々が、各センサデータに対応するセンサデータ処理部１４０により、移動体情報１３１の算出に用いられる情報に処理されたものである。具体的には、自律センサデータ処理部１４１により利用可能に処理された自律センサ情報１１、補強センサデータＡ処理部１４２により利用可能に処理された補強センサＡ情報１２、補強センサデータＢ処理部１４３により利用可能に処理された補強センサＢ情報１３である。

複数のセンサ情報３２０には、複数のセンサデータの各センサデータの処理が完了した時刻である処理完了時刻が各センサ情報に付与されている。処理完了時刻は、複数のセンサデータ３１０の各センサデータが利用可能となった時刻である。すなわち、複数のセンサ情報３２０では、複数のセンサデータ３１０の各センサデータが利用可能となった時刻を表す処理完了時刻が各センサ情報に付与されている。また、各センサ情報には複数のセンサデータ３１０の各々が複数のセンサ２００の各々により取得された取得時刻も付与されている。

なお、センサデータ処理部１４０は、対応するセンサデータが入力されるとメモリ９２１にバッファリングした上で、センサデータを利用可能となるように順に処理し、センサ情報として記憶部１５０に記憶する。具体的には、自律センサデータ３１０が入力されると、対応する自律センサデータ処理部１４１がメモリ９２１にバッファリングし、バッファリングした自律センサデータ３１０を利用可能となるように順に処理し、自律センサ情報１１として記憶部１５０に記憶する。他のセンサデータ処理部１４０についても同様である。

時刻設定処理Ｓ１２０において、時刻設定部１２０は、複数のセンサ情報３２０のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、移動体情報１３１を算出する対象の時刻であるターゲット時刻１２１を設定する。
時刻設定処理Ｓ１２０については後で詳しく説明する。

算出処理Ｓ１３０において、算出部１３０は、ターゲット時刻１２１の以前の処理完了時刻が付与され、かつ、使用されずにバッファリングされている複数のセンサ情報３２０の各センサ情報を用いて、移動体情報１３１を算出する。なお、ターゲット時刻１２１の以前の処理完了時刻が付与され、かつ、使用されずにバッファリングされているセンサ情報には、ターゲット時刻１２１の以前で、かつ、ターゲット時刻１２１に最も近い処理完了時刻が付与されたセンサ情報が含まれる。すなわち、算出部１３０は、ターゲット時刻１２１の以前で、かつ、ターゲット時刻１２１に最も近い処理完了時刻が付与された複数のセンサ情報３２０の各センサ情報を用いて、移動体情報１３１を算出する。
算出処理Ｓ１３０については後で詳しく説明する。

＜時刻設定処理Ｓ１２０＞
図３は、本実施の形態に係る時刻設定処理Ｓ１２０を示すフロー図である。
図４は、本実施の形態に係る時刻設定処理Ｓ１２０を示す模式図である。
図３及び図４を用いて、本実施の形態に係る時刻設定処理Ｓ１２０について説明する。

図４は、時間の流れ、データ処理装置１００でのリアルタイムにおけるセンサ情報の利用可能状況、及び実際にデータ処理Ｓ１００で使用しているセンサ情報を示している。白抜きのセンサ情報は移動体情報１３１の算出に利用できない利用不可のセンサ情報であり、ハッチングされているセンサ情報は利用可能なデータである。黒枠のセンサ情報は利用可能なセンサ情報のうち使用しているセンサ情報を示している。また、時間の流れにおける目盛りは、データ処理装置１００が処理を実行するタイミングを示している。
図４に示すように、センサ情報のリアルタイム性は、自律センサ情報１１、補強センサＢ情報１３、補強センサＡ情報１２の順に悪くなる。

ステップＳ１２１において、時刻設定部１２０は、各センサ情報について、最新のセンサ情報の処理完了時刻を取得する。図４に示すように、自律センサ情報１１についての最新の処理完了時刻はＴ１である。また、補強センサＡ情報１２についての最新の処理完了時刻はＴ２である。また、補強センサＢ情報１３についての最新の処理完了時刻はＴ３である。

ステップ１２２において、時刻設定部１２０は、最新の処理完了時刻と現在時刻Ｔｒとの差を算出する。最新の処理完了時刻と現在時刻Ｔｒとの差は、センサ情報の遅れ時間を示している。時刻設定部１２０は、最新の処理完了時刻と現在時刻Ｔｒとの差が最も大きいセンサ情報を判定する。図４に示すように、最新の処理完了時刻と現在時刻Ｔｒとの差が最も大きいセンサ情報は、補強センサＡ情報１２である。補強センサＡ情報１２は、複数のセンサ情報３２０のうちリアルタイム性が最も悪いセンサ情報である。

ステップ１２３において、時刻設定部１２０は、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、移動体情報１３１を算出する対象の時刻であるターゲット時刻１２１を設定する。図４に示すように、ターゲット時刻１２１をＴｔとし、現在時刻をＴｒとする。時刻設定部１２０は、補強センサＡ情報１２の処理完了時刻Ｔ２に基づいて、ターゲット時刻Ｔｔを設定する。時刻設定部１２０は、処理完了時刻Ｔ２より現在時刻Ｔｒに近く、かつ、処理完了時刻Ｔ２に最も近い実行タイミングをターゲット時刻Ｔｔとして設定する。すなわち、時刻設定部１２０は、処理完了時刻Ｔ２より以降の直近の実行タイミングをターゲット時刻Ｔｔとして設定する。なお、時刻設定部１２０は、処理完了時刻Ｔ２そのものをターゲット時刻Ｔｔとして設定しても構わない。

図５は、本実施の形態に係る算出処理Ｓ１３０を示すフロー図である。
ステップＳ１３１において、算出部１３０は、ターゲット時刻Ｔｔの以前の処理完了時刻が付与され、かつ、使用されずにバッファリングされている複数のセンサ情報３２０の各センサ情報を用いて、移動体情報１３１を算出する。処理完了時刻がターゲット時刻Ｔｔと等しい場合は、その処理完了時刻のセンサ情報を用いてもよい。
なお、図４では、ターゲット時刻Ｔｔの以前の処理完了時刻が付与され、かつ、使用されずにバッファリングされているセンサ情報は、ターゲット時刻Ｔｔの以前で、かつ、ターゲット時刻Ｔｔに最も近い処理完了時刻が付与されたセンサ情報のみである場合を示している。
図４に示すように、自律センサ情報１１において、ターゲット時刻Ｔｔ以前で、かつ、ターゲット時刻Ｔｔに最も近い処理完了時刻はＴ１ｂである。また、補強センサＡ情報１２において、ターゲット時刻Ｔｔ以前で、かつ、ターゲット時刻Ｔｔに最も近い処理完了時刻はＴ２ｂ、すなわちＴ２である。また、補強センサＢ情報１３において、ターゲット時刻Ｔｔ以前で、かつ、ターゲット時刻Ｔｔに最も近い処理完了時刻はＴ３ｂである。

よって、算出部１３０は、処理完了時刻Ｔ１ｂが付与された自律センサ情報１１ｂと、処理完了時刻Ｔ２ｂが付与された補強センサＡ情報１２ｂと、処理完了時刻Ｔ３ｂが付与された補強センサＢ情報１３ｂとに基づいて、移動体情報１３１を算出する。具体的には、算出部１３０は、自律センサ情報１１ｂから前の自律センサ情報１１と、補強センサＡ情報１２ｂから前の補強センサＡ情報１２と、補強センサＢ情報１３ｂから前の補強センサＢ情報１３とを用いて、移動体の位置及び姿勢を標定し、移動体情報１３１とする。
算出部１３０は、出力インタフェース９４０を介して移動体情報１３１を出力する。

以上で、本実施の形態に係るデータ処理Ｓ１００の説明を終わる。

＊＊＊他の構成＊＊＊
本実施の形態では、データ処理装置１００は、入力インタフェース９３０を介して複数のセンサデータ３１０を取得し、出力インタフェース９４０をして移動体情報１３１を出力する構成であった。しかし、データ処理装置１００が通信装置を備え、通信装置を介して複数のセンサデータ３１０を受信してもよい。また、データ処理装置１００は、通信装置を介して移動体情報１３１を送信してもよい。この場合、通信装置はレシーバとトランスミッタとを備える。具体的には、通信装置は通信チップまたはＮＩＣ（Ｎｅｔｗｏｒｋ・Ｉｎｔｅｒｆａｃｅ・Ｃａｒｄ）である。通信装置はデータを通信する通信部として機能する。レシーバはデータを受信する受信部として機能し、トランスミッタはデータを送信する送信部として機能する。

また、本実施の形態では、データ処理装置１００の「部」の機能がソフトウェアで実現されるが、変形例として、データ処理装置１００の「部」の機能がハードウェアで実現されてもよい。
図６を用いて、本実施の形態の変形例に係るデータ処理装置１００の構成について説明する。
図６に示すように、データ処理装置１００は、処理回路９０９、入力インタフェース９３０、出力インタフェース９４０といったハードウェアを備える。

処理回路９０９は、前述した「部」の機能及び記憶部１５０を実現する専用の電子回路である。処理回路９０９は、具体的には、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ロジックＩＣ、ＧＡ（Ｇａｔｅ・Ａｒｒａｙ）、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ・Ｓｐｅｃｉｆｉｃ・Ｉｎｔｅｇｒａｔｅｄ・Ｃｉｒｃｕｉｔ）、又は、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ・Ｇａｔｅ・Ａｒｒａｙ）である。

「部」の機能及び記憶部１５０は、１つの処理回路９０９で実現されてもよいし、複数の処理回路９０９に分散して実現されてもよい。

別の変形例として、データ処理装置１００の機能がソフトウェアとハードウェアとの組合せで実現されてもよい。すなわち、データ処理装置１００の一部の機能が専用のハードウェアで実現され、残りの機能がソフトウェアで実現されてもよい。

プロセッサ９１０、記憶装置９２０、及び、処理回路９０９を、総称して「プロセッシングサーキットリ」という。つまり、データ処理装置１００の構成が図１及び図４のいずれに示した構成であっても、「部」の機能及び記憶部１５０は、プロセッシングサーキットリにより実現される。

「部」を「工程」又は「手順」又は「処理」に読み替えてもよい。また、「部」の機能をファームウェアで実現してもよい。

＊＊＊本実施の形態の効果の説明＊＊＊
センサデータのリアルタイム性、すなわちセンシングしてからこのセンサデータが位置姿勢標定装置であるデータ処理装置で利用可能となるまでの遅れ時間の少なさは、各センサによるデータ処理時間や通信等による伝送時間により、センサ毎にまちまちである。
これらリアルタイム性の異なるセンサデータを複合利用してリアルタイム演算処理を行うためには、プログラムの実装において、時刻が前後して利用可能となる各センサデータに対応するため、処理する時刻を前に遡らせたり逆に先に進めたりするなど、適切にハンドリングすることが必要となり、処理が複雑化する。
本実施の形態に係るデータ処理装置によれば、利用可能となったセンサ情報を一度バッファリングし、処理した後にその時刻より古いタイムタグのセンサ情報が利用可能となることのない、可能な限りリアルタイムに近い時刻をデータ処理のターゲット時刻として設定し、処理を実施する。このようにすることで、データ処理の後に、その時刻より古いタイムタグを持ったセンサ情報が利用可能となることがなく、時刻を遡った処理やその分先に進める（もどす）といった煩雑な処理は発生しない。
よって、本実施の形態に係るデータ処理装置によれば、処理が複雑化することを避けることができ、かつ、リアルタイム性が高く精度のよい移動体情報、すなわち位置姿勢標定結果を得ることができる。

実施の形態２．
＊＊＊構成の説明＊＊＊
本実施の形態では、主に、実施の形態１との差異について説明する。
図７は、本実施の形態に係るデータ処理装置１００ａの構成を示す図である。
実施の形態１では、最もリアルタイム性の低いセンサ情報が利用可能となるのを待って、データ処理を実施するので、待つ分だけ処理結果である移動体情報１３１のタイムタグのリアルタイム性が損なわれてしまう。カーナビのような地図上での自己位置の表示といった、あまりリアルタイム性が必要とされない用途であれば、これで十分であるが、車両の運転制御のフィードバックとして使用する場合は、リアルタイム性を確保する必要がある。
本実施の形態では、リアルタイム性の遅いセンサ情報が利用可能となることを待って処理することによる遅れを補間することができるデータ処理装置１００ａについて説明する。データ処理装置１００ａは、リアルタイム性の高い自律センサ情報１１を用いて、自律航法によりリアルタイム直前まで処理する。
データ処理装置１００ａにおいて、実施の形態１で説明した構成と同様の構成についてはその説明を省略する。

図７に示すように、データ処理装置１００ａは、実施の形態１で説明したデータ処理装置１００の構成に加えて、補間部１６０を備える。すなわち、実施の形態１で説明した「部」の機能には、補間部１６０の機能が加わる。その他の構成については、実施の形態１と同様である。

＊＊＊動作の説明＊＊＊
図８は、本実施の形態に係るデータ処理装置１００ａによるデータ処理Ｓ１００ａを示すフロー図である。
図８に示すように、データ処理Ｓ１００ａは、実施の形態１で説明したデータ処理Ｓ１００に加えて、補間処理Ｓ１４０を備える。
補間処理Ｓ１４０において、補間部１６０は、最新の処理完了時刻と現在時刻との差が最も小さいセンサ情報である自律センサ情報１１を用いて、ターゲット時刻Ｔｔより後の移動体情報１３１を自律航法により算出する。

図９は、本実施の形態に係る補間処理Ｓ１４０を示すフロー図である。
図１０は、本実施の形態に係る補間処理Ｓ１４０を示す模式図である。
ステップＳ１４１において、補間部１６０は、ターゲット時刻Ｔｔから後の移動体情報１３１を、自律センサ情報１１を用いて自律航法により算出する。
図１０に示すように、自律センサ情報１１はターゲット時刻Ｔｔから後の時刻Ｔｓまでしか利用可能となっていない。そこで、図１０の実線の曲線矢印に示すように、ターゲット時刻Ｔｔから時刻Ｔｓまで、移動体情報１３１を自律航法で算出する。このように、ターゲット時刻Ｔｔから後の移動体情報１３１を自律航法により算出する場合でも、自律センサデータが利用可能となった時刻Ｔｓまでしか算出できない可能性がある。
その場合、補間部１６０は、移動体情報１３１の自律航法による算出が完了した時刻Ｔｓから現在時刻Ｔｒまでの移動体情報１３１を外挿法により算出する。

ステップＳ１４２において、補間部１６０は、移動体情報１３１を現在時刻Ｔｒまで算出したか否かを判定する。現在時刻Ｔｒまで算出したと判定した場合、処理を終了する。
現在時刻Ｔｒまで算出されていないと判定した場合、ステップＳ１４３に進む。

ステップＳ１４３において、補間部１６０は、移動体情報１３１の算出が完了した時刻Ｔｓから現在時刻Ｔｒまでの移動体情報１３１を外挿法により算出する。具体的には、補間部１６０は、積分により時刻Ｔｓから現在時刻Ｔｒまでの位置及び姿勢の予測値を算出する。なお、図１０に示すように、補間部１６０は、外挿法を用いて、現在時刻Ｔｒより未来の時刻まで、移動体情報を予測しても構わない。

＊＊＊本実施の形態の効果の説明＊＊＊
本実施の形態に係るデータ処理装置１００ａでは、リアルタイム性の遅い補強センサデータが利用可能となることを待って処理することにより発生した遅れを、リアルタイム性の高い自律センサデータを用いて、自律航法によりリアルタイム直前まで処理する。さらに、自律センサデータもまだ利用可能ではないリアルタイム、すなわちデータ出力時刻のデータ処理については外挿計算を実施することで補う。よって、本実施の形態に係るデータ処理装置１００ａでは、処理結果である移動体情報のリアルタイム性をより高めることができる。

実施の形態３．
＊＊＊構成の説明＊＊＊
本実施の形態では、主に、実施の形態１，２との差異について説明する。
図１１は、本実施の形態に係るデータ処理装置１００ｂの構成を示す図である。
実施の形態１，２では、補強センサを追加する場合、その都度、追加する補強センサに応じたセンサデータ処理部を付け加える改修をする必要がある。この場合、ＧＰＳを使用できない地下において、磁気マーカーのような補強センサを使用するといった補強センサの最適選択をすることが難しい。また、新たに使用可能となった補強センサが出るたびに、対応するセンサデータ処理部を追加するためのプログラム改修が発生する。
本実施の形態では、補強センサからデータ処理装置への入力データの標準化を行い、プログラムの改修なしで簡単に新たな補強センサを追加することができる構成を有するデータ処理装置１００ｂについて説明する。
データ処理装置１００ｂにおいて、実施の形態１，２で説明した構成と同様の構成についてはその説明を省略する。

図１１は実施の形態２で説明した図７に対応する。図１１に示すように、データ処理装置１００ｂにおいて、実施の形態２で説明したデータ処理装置１００ａと異なる点はセンサデータ処理部１４０ｂである。本実施の形態に係るセンサデータ処理部１４０ｂは、実施の形態２で説明した各センサデータに対応するセンサデータ処理部、すなわち、自律センサデータ処理部１４１、補強センサデータＡ処理部１４２、補強センサデータＢ処理部１４３を備えていない。本実施の形態に係るセンサデータ処理部１４０ｂは、標準処理部１４６を備える。
すなわち、実施の形態２で説明した「部」の機能には、自律センサデータ処理部１４１、補強センサデータＡ処理部１４２、補強センサデータＢ処理部１４３に替わり、標準処理部１４６の機能が加わる。その他の構成については、実施の形態２と同様である。

＊＊＊動作の説明＊＊＊
本実施の形態では、複数のセンサ２００の各センサは、移動体情報１３１の算出に用いられるフォーマット情報３００を含むセンサデータを出力するものとする。すなわち、自律センサデータ３１１ｂ、補強センサデータＡ３１２ｂ、補強センサデータＢ３１３ｂには、各々フォーマット情報３００が含まれる。

図１２は、本実施の形態に係るフォーマット情報３００の一例を示す図である。
上述したように、追加が想定される補強センサには、インフラを必要とするものも少なくなく、今後新たな補強センサのシステムが構築されることも考えられる。そこで、フォーマット情報３００は、データ処理装置１００ｂが補強センサデータを使用するうえで必要となる情報がすべて含まれるように、補強センサからデータ処理装置１００ｂに入力されるデータを標準化したものである。
必要となる情報の一例は、時刻、観測データ、観測誤差、観測方程式、観測物パラメータである。時刻は観測データをセンシングした時刻、観測データはセンシングデータである。また、観測誤差はセンシングデータの誤差分散である。観測方程式はデータ処理装置、すなわち位置姿勢認識装置の状態量と観測データとの関係式である。観測物パラメータは、データ処理装置が観測値を予測するのに必要となる、被センシング物のパラメータである。観測物パラメータの具体例としては、被センシング物の位置情報等がある。

記憶処理Ｓ１１０において、標準処理部１４６は、各センサデータにフォーマット情報３００が含まれる複数のセンサデータ３１０ｂを取得する。標準処理部１４６は、複数のセンサデータ３１０ｂの各センサデータに含まれるフォーマット情報３００を各センサ情報として複数のセンサ情報３２０を記憶部１５０に記憶させる。
その他の処理は、実施の形態１，２で説明したものと同様である。

＊＊＊本実施の形態の効果の説明＊＊＊
位置姿勢標定装置であるデータ処理装置は、慣性装置のような自律センサデータによる自律航法をベースとしているため、これだけではセンサ誤差により精度が劣化する。そこで、位置姿勢の絶対的な観測情報を持つ補強センサのデータを使用することで、劣化した精度を補正する必要がある。上述の通り、補強センサデータのリアルタイム性はセンサ毎まちまちであるが、本実施の形態に係るデータ処理装置によれば、補強センサを増やしても処理が煩雑となることがないため、多くの補強センサを複合利用することで、位置姿勢標定装置の処理のロバスト性を向上することができる。

図１３は、本実施の形態の効果を示す模式図である。図１３に示すように、レーザー、カメラ、磁気マーカー、ＲＦＩＤ（Ｒａｄｉｏ・Ｆｒｅｑｕｅｎｃｙ・Ｉｄｅｎｔｉｆｉｃａｔｉｏｎ）といった補強センサに加え、新たにセンサ１、センサ２、・・・、センサ５等を加える場合でも、プログラムを改修することなく追加することができる。
本実施の形態に係るデータ処理装置は、入力された補強センサデータを時刻順に並べ替えた後にバッファリングする。そして、データ処理装置は、ターゲット時刻前後に補強センサデータがある場合、同じくバッファリングされている観測方程式やパラメータをコールして、自律航法を観測更新して精度維持ができるようにする。これにより、用途や使用場所に応じた複数補強センサを選択することができると共に、データ処理装置に接続するだけで、ロバスト性の高い移動体情報、すなわち位置姿勢標定結果を容易に得ることが可能となる。また、通常は後処理にて開発・評価を実施する位置姿勢標定プログラムを、インタフェースを変更するだけで簡単にリアルタイム処理化することが可能であり、リアルタイム実装後の確認も後処理との比較により容易に確認することができる。

上記の実施の形態では、「部」の各々が独立した機能ブロックとしてデータ処理装置を構成している。しかし、上記のような構成でなくてもよく、データ処理装置の構成は任意である。データ処理装置の機能ブロックは、上記の実施の形態で説明した機能を実現することができれば、任意である。これらの機能ブロックを、他のどのような組み合わせ、あるいは任意のブロック構成で、データ処理装置を構成しても構わない。
また、データ処理装置は、１つの装置でなく、複数の装置から構成されたシステムでもよい。

また、データ処理装置は、移動体に搭載され、計測しながらリアルタイムあるいは準リアルタイムに移動体情報を確認することができるものである。しかし、移動体に搭載されていなくてもよい。また、データ処理装置を、移動体と通信装置を介して接続された遠隔地のセンターに搭載し、リアルタイムあるいは準リアルタイムに移動体情報を算出する構成でもよい。

実施の形態１から３について説明したが、これらの実施の形態のうち、複数の部分を組み合わせて実施しても構わない。あるいは、これらの実施の形態のうち、１つの部分を実施しても構わない。その他、これらの実施の形態を、全体としてあるいは部分的に、どのように組み合わせて実施しても構わない。
なお、上記の実施の形態は、本質的に好ましい例示であって、本発明、その適用物及び用途の範囲を制限することを意図するものではなく、必要に応じて種々の変更が可能である。

１１ 自律センサ情報、１２ 補強センサＡ情報、１３ 補強センサＢ情報、１００，１００ａ，１００ｂ データ処理装置、１２０ 時刻設定部、１２１，Ｔｔ ターゲット時刻、１３０ 算出部、１３１ 移動体情報、１４０，１４０ｂ センサデータ処理部、１４１ 自律センサデータ処理部、１４２ 補強センサデータＡ処理部、１４３ 補強センサデータＢ処理部、１４６ 標準処理部、１５０ 記憶部、１６０ 補間部、２００ 複数のセンサ、２０１ 自律センサ、２０２ 補強センサＡ、２０３ 補強センサＢ、３００ フォーマット情報、３１０，３１０ｂ センサデータ、３１１，３１１ｂ 自律センサデータ、３１２，３１２ｂ 補強センサデータＡ、３１３，３１３ｂ 補強センサデータＢ、３２０ 複数のセンサ情報、３２１ 自律センサ情報、３２２ 補強センサＡ情報、３２３ 補強センサＢ情報、５１０ データ処理方法、５２０ データ処理プログラム、９０９ 処理回路、９１０ プロセッサ、９２０ 記憶装置、９２１ メモリ、９２２ 補助記憶装置、９３０ 入力インタフェース、９４０ 出力インタフェース、Ｓ１００，Ｓ１００ａ データ処理、Ｓ１１０ 記憶処理、Ｓ１２０ 時刻設定処理、Ｓ１３０ 算出処理、Ｓ１４０ 補間処理、Ｔｒ 現在時刻。

Claims (6)

1. 移動体に搭載された複数のセンサにより取得された複数のセンサデータに基づいて、前記移動体の状態を移動体情報として算出するデータ処理装置において、
   前記複数のセンサデータが前記移動体情報の算出に用いられる情報に処理された複数のセンサ情報であって、前記複数のセンサデータの各センサデータの処理が完了した処理完了時刻が各センサ情報に付与された複数のセンサ情報を記憶する記憶部と、
   前記複数のセンサ情報のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、前記移動体情報を算出する対象の時刻であるターゲット時刻を設定する時刻設定部と、
   前記ターゲット時刻の以前で、かつ、前記ターゲット時刻に最も近い処理完了時刻が付与された前記複数のセンサ情報の各センサ情報を用いて、前記移動体情報を算出する算出部と
   を備えたデータ処理装置。
2. 前記データ処理装置は、さらに、
   最新の処理完了時刻と現在時刻との差が最も小さいセンサ情報である自律センサ情報を用いて、前記ターゲット時刻より後の前記移動体情報を算出する補間部を備えた請求項１に記載のデータ処理装置。
3. 前記補間部は、
   前記移動体情報の算出が完了した時刻から現在時刻までの前記移動体情報を外挿法により算出する請求項２に記載のデータ処理装置。
4. 前記データ処理装置は、さらに、
   前記移動体情報の算出に用いられるフォーマット情報を各センサデータが含む前記複数のセンサデータを取得し、前記複数のセンサデータの各センサデータに含まれる前記フォーマット情報を各センサ情報として前記複数のセンサ情報を前記記憶部に記憶させる標準処理部を備えた請求項１から３のいずれか１項に記載のデータ処理装置。
5. 移動体に搭載された複数のセンサにより取得された複数のセンサデータに基づいて、前記移動体の状態を移動体情報として算出するデータ処理装置のデータ処理方法において、
   記憶部が、前記複数のセンサデータが前記移動体情報の算出に用いられる情報に処理された複数のセンサ情報であって、前記複数のセンサデータの各センサデータの処理が完了した処理完了時刻が各センサ情報に付与された複数のセンサ情報を記憶し、
   時刻設定部が、前記複数のセンサ情報のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、前記移動体情報を算出する対象の時刻であるターゲット時刻を設定し、
   算出部が、前記ターゲット時刻の直前の処理完了時刻が付与された前記複数のセンサ情報の各センサ情報を用いて、前記移動体情報を算出するデータ処理方法。
6. 移動体に搭載された複数のセンサにより取得された複数のセンサデータに基づいて、前記移動体の状態を移動体情報として算出するデータ処理装置のデータ処理プログラムにおいて、
   前記複数のセンサデータが前記移動体情報の算出に用いられる情報に処理された複数のセンサ情報であって、前記複数のセンサデータの各センサデータの処理が完了した処理完了時刻が各センサ情報に付与された複数のセンサ情報を記憶する記憶処理と、
   前記複数のセンサ情報のうち、最新の処理完了時刻と現在時刻との差が最も大きいセンサ情報の処理完了時刻に基づいて、前記移動体情報を算出する対象の時刻であるターゲット時刻を設定する時刻設定処理と、
   前記ターゲット時刻の直前の処理完了時刻が付与された前記複数のセンサ情報の各センサ情報を用いて、前記移動体情報を算出する算出処理とをコンピュータに実行させるデータ処理プログラム。

Images