(57)【要約】【課題】センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができるようにする。【解決手段】センシング結果取得部１０４が、複数のセンサの何れかから、複数の移動物を検出した検出結果及びセンサで得られた検出時刻を受け取り、移動物状態量予測部１０８が、検出結果を受け取る毎に、移動物のそれぞれについて、記憶された状態量を用いて、検出時刻に対応する移動物の状態量を予測し、時間変化量推定部１１２が、記憶された状態量から推定される移動物の時間変化量又はセンサの時間変化量を推定し、観測誤差推定部１１４が、時間変化量に基づいて、複数の移動物それぞれについての移動物の状態量の観測誤差を推定し、移動物状態量統合部１１６が、観測誤差に基づいて、複数の移動物それぞれについて予測された移動物の状態量と、複数の移動物それぞれの検出結果との対応付けを行い、複数の移動物それぞれの状態量を更新する。【選択図】図３
2019061551.tif 000002
【特許請求の範囲】
【請求項１】
少なくとも１つの移動物それぞれの状態量を記憶する移動物状態量記憶部と、
複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻を受け取るセンシング結果獲得部と、
前記センシング結果獲得部によって前記検出結果を受け取る毎に、前記移動物のそれぞれについて、前記移動物状態量記憶部に記憶された状態量を用いて、前記検出時刻に対応する前記移動物の状態量を予測する移動物状態量予測部と、
前記移動物状態量記憶部に記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定する時間変化量推定部と、
前記時間変化量推定部で推定した時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定する観測誤差推定部と、
前記観測誤差推定部によって推定された前記観測誤差に基づいて、前記移動物状態量予測部によって前記複数の移動物それぞれについて予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記移動物状態量記憶部に格納された、前記複数の移動物それぞれの状態量を更新する状態量推定部と、
を含む状態量統合装置。
【請求項２】
前記観測誤差推定部は、前記時間変化量推定部で推定した時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定する請求項１に記載の状態量統合装置。
【請求項３】
前記時間変化量推定部は、前記移動物状態量記憶部に記憶された状態量に基づいて、前記複数の移動物のそれぞれについて、前記移動物の時間変化量を推定し、
前記観測誤差推定部は、前記複数の移動物それぞれについて、前記時間変化量推定部で推定した前記移動物の時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記移動物の状態量の観測誤差を推定する請求項２記載の状態量統合装置。
【請求項４】
前記時間変化量推定部は、前記センサの時間変化量を推定し、
前記観測誤差推定部は、前記複数の移動物それぞれについて、前記時間変化量推定部で推定した前記センサの時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記移動物の状態量の観測誤差を推定する請求項２記載の状態量統合装置。
【請求項５】
前記センシング結果獲得部は、前記複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻、又は前記センサの測位データ及び前記センサで得られた測位時刻を受け取り、
前記複数のセンサのそれぞれの状態量を記憶するセンサ状態量記憶部と、
前記センシング結果獲得部によって前記測位データを受け取る毎に、前記センサ状態量記憶部に記憶された状態量を用いて、前記測位時刻に対応する前記センサの状態量を予測するセンサ状態量予測部と、
前記センサ状態量記憶部に記憶された状態量に基づいて、前記センサの時間変化量を推定するセンサ時間変化量推定部と、
前記センサ時間変化量推定部で推定した前記センサの時間変化量に基づいて、前記センサの状態量の観測誤差を推定するセンサ観測誤差推定部と、
前記センサ観測誤差推定部によって推定された前記観測誤差と、前記センサ状態量予測部によって予測された前記センサの状態量とに基づいて、前記センサの状態量を更新するセンサ状態量推定部と、を更に含む請求項１乃至４の何れか１項記載の状態量統合装置。
【請求項６】
前記センサと通信を行う通信部を更に含み、
前記センサは、車両に搭載される請求項１乃至５の何れか1項記載の状態量統合装置。
【請求項７】
前記センサと通信を行う通信部を更に含み、
前記センサは、道路上に設置され、
前記状態量統合装置は、車両に搭載される請求項１乃至５の何れか1項記載の状態量統合装置。
【請求項８】
少なくとも１つの移動物それぞれの状態量を記憶する移動物状態量記憶部を含むコンピュータを、
複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻を受け取るセンシング結果獲得部、
前記センシング結果獲得部によって前記検出結果を受け取る毎に、前記移動物のそれぞれについて、前記移動物状態量記憶部に記憶された状態量を用いて、前記検出時刻に対応する前記移動物の状態量を予測する移動物状態量予測部、
前記移動物状態量記憶部に記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定する時間変化量推定部、
前記時間変化量推定部で推定した時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定する観測誤差推定部、及び
前記観測誤差推定部によって推定された前記観測誤差に基づいて、前記移動物状態量予測部によって前記複数の移動物それぞれについて予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記移動物状態量記憶部に格納された、前記複数の移動物それぞれの状態量を更新する状態量推定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態量統合装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、複数のカメラ映像を用いて、複数の歩行者が歩いているシーンから隠れ（オクルージョン）ている歩行者の動きを捉えるために、複数カメラの配置・向き情報と複数のカメラ映像から抽出した歩行者の足領域情報を利用して時空間トラッキングをしながら、歩行者を分離・追跡する技術が知られている（非特許文献１）。
【０００３】
また、複数のカメラ映像を用いて、車両領域を表すテンプレート位置を抽出し、テンプレート位置情報に対して、多視点的処理と時系列処理を施し、複数のカメラで検出された車両の対応付けを行うことで、走行車両の運動軌道を高精度に計算することができる走行車両検出装置が知られている（特許文献１）。
【先行技術文献】
【非特許文献】
【０００４】
【非特許文献１】S.M. Khan, et. al., “A multiview approach to tracking people in crowded scenes using a planar homography constraint,” Proceedings of the 9th European conference on Computer Vision, ECCV06, Vol. 4, pp133-146.
【特許文献】
【０００５】
【特許文献１】特開２００１-３５７４８９号広報
【特許文献２】特開２０１２−１４７１８６号広報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
従来技術では、得られたセンサデータの時刻情報をもとにセンシング結果の統合を行っていたが、センシング結果に付与されている時刻情報は各々のセンサにより個別に計測されている値であり、それらの値が同期している保証はない。
【０００７】
また、ランダムに送信される複数センサデータの時刻情報にずれがある場合には、時刻情報のずれ量を計算して補正することができるが（特許文献２）、走行する車両同士など、短期間しか通信のやり取りをしない場合を想定すると、時刻の同期処理を行っている余裕がない場合がある。
【０００８】
この場合、複数のセンサで取得した各データには、センサで計測した時刻の誤差が含まれているため、その時刻の値をそのまま信用してセンシング結果を統合してしまうと、センサの統合が上手くいかず、精度が落ちる、という問題があった。
【０００９】
本発明は、上記問題点を解決するためになされたもので、センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができる状態量統合装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
上記の目的を達成するために、本発明の状態量統合装置は、少なくとも１つの移動物それぞれの状態量を記憶する移動物状態量記憶部と、複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻を受け取るセンシング結果獲得部と、前記センシング結果獲得部によって前記検出結果を受け取る毎に、前記移動物のそれぞれについて、前記移動物状態量記憶部に記憶された状態量を用いて、前記検出時刻に対応する前記移動物の状態量を予測する移動物状態量予測部と、前記移動物状態量記憶部に記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定する時間変化量推定部と、前記時間変化量推定部で推定した時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定する観測誤差推定部と、前記観測誤差推定部によって推定された前記観測誤差に基づいて、前記移動物状態量予測部によって前記複数の移動物それぞれについて予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記移動物状態量記憶部に格納された、前記複数の移動物それぞれの状態量を更新する状態量推定部と、を含んで構成されている。
【００１１】
また、本発明のプログラムは、少なくとも１つの移動物それぞれの状態量を記憶する移動物状態量記憶部を含むコンピュータを、複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻を受け取るセンシング結果獲得部、前記センシング結果獲得部によって前記検出結果を受け取る毎に、前記移動物のそれぞれについて、前記移動物状態量記憶部に記憶された状態量を用いて、前記検出時刻に対応する前記移動物の状態量を予測する移動物状態量予測部、前記移動物状態量記憶部に記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定する時間変化量推定部、前記時間変化量推定部で推定した時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定する観測誤差推定部、及び前記観測誤差推定部によって推定された前記観測誤差に基づいて、前記移動物状態量予測部によって前記複数の移動物それぞれについて予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記移動物状態量記憶部に格納された、前記複数の移動物それぞれの状態量を更新する状態量推定部として機能させるためのプログラムである。
【００１２】
本発明の状態量統合装置及びプログラムによれば、移動物状態量記憶部が、少なくとも１つの移動物それぞれの状態量を記憶し、センシング結果獲得部が、複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻を受け取り、移動物状態量予測部が、前記センシング結果獲得部によって前記検出結果を受け取る毎に、前記移動物のそれぞれについて、前記移動物状態量記憶部に記憶された状態量を用いて、前記検出時刻に対応する前記移動物の状態量を予測する。
【００１３】
そして、時間変化量推定部が、前記移動物状態量記憶部に記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定し、観測誤差推定部が、前記時間変化量推定部で推定した時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定し、状態量推定部が、前記観測誤差推定部によって推定された前記観測誤差に基づいて、前記移動物状態量予測部によって前記複数の移動物それぞれについて予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記移動物状態量記憶部に格納された、前記複数の移動物それぞれの状態量を更新する。
【００１４】
このように、記憶された状態量から推定される前記移動物の時間変化量又は前記センサの時間変化量を推定し、時間変化量に基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定し、予測された前記移動物の状態量と、複数の移動物それぞれの前記検出結果との対応付けを行い、前記複数の移動物それぞれの状態量を更新することにより、センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができる。
【００１５】
また、本発明の状態量統合装置は、前記観測誤差推定部は、前記時間変化量推定部で推定した時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記複数の移動物それぞれについての前記移動物の状態量の観測誤差を推定することができる。
【００１６】
また、本発明の状態量統合装置は、前記時間変化量推定部は、前記移動物状態量記憶部に記憶された状態量に基づいて、前記複数の移動物のそれぞれについて、前記移動物の時間変化量を推定し、前記観測誤差推定部は、前記複数の移動物それぞれについて、前記時間変化量推定部で推定した前記移動物の時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記移動物の状態量の観測誤差を推定することができる。
【００１７】
また、本発明の状態量統合装置は、前記時間変化量推定部は、前記センサの時間変化量を推定し、前記観測誤差推定部は、前記複数の移動物それぞれについて、前記時間変化量推定部で推定した前記センサの時間変化量と、予め定められた時刻ずれの標準偏差とに基づいて、前記移動物の状態量の観測誤差を推定することができる。
【００１８】
また、本発明の状態量統合装置は、前記センシング結果獲得部は、前記複数のセンサの何れかから、複数の移動物を検出した検出結果及び前記センサで得られた検出時刻、又は前記センサの測位データ及び前記センサで得られた測位時刻を受け取り、前記複数のセンサのそれぞれの状態量を記憶するセンサ状態量記憶部と、前記センシング結果獲得部によって前記測位データを受け取る毎に、前記センサ状態量記憶部に記憶された状態量を用いて、前記測位時刻に対応する前記センサの状態量を予測するセンサ状態量予測部と、前記センサ状態量記憶部に記憶された状態量に基づいて、前記センサの時間変化量を推定するセンサ時間変化量推定部と、前記センサ時間変化量推定部で推定した前記センサの時間変化量に基づいて、前記センサの状態量の観測誤差を推定するセンサ観測誤差推定部と、前記センサ観測誤差推定部によって推定された前記観測誤差と、前記センサ状態量予測部によって予測された前記センサの状態量とに基づいて、前記センサの状態量を更新するセンサ状態量推定部と、を更に含むように構成することができる。
【００１９】
また、本発明の状態量統合装置は、前記センサと通信を行う通信部を更に含み、前記センサは、車両に搭載されることができる。
【００２０】
また、本発明の状態量統合装置は、前記センサと通信を行う通信部を更に含み、前記センサは、道路上に設置され、前記状態量統合装置は、車両に搭載されることができる。
【発明の効果】
【００２１】
以上説明したように、本発明の状態予測装置及びプログラムによれば、記憶された状態量から推定される移動物の時間変化量又はセンサの時間変化量を推定し、時間変化量に基づいて、複数の移動物それぞれについての移動物の状態量の観測誤差を推定し、予測された移動物の状態量と、複数の移動物それぞれの検出結果との対応付けを行い、複数の移動物それぞれの状態量を更新することにより、センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができる。
【図面の簡単な説明】
【００２２】
【図１】本発明の第１の実施の形態における状態量統合システムの構成を示すブロック図である。
【図２】本発明の第１の実施の形態における検出器の構成を示すブロック図である。
【図３】本発明の第１の実施の形態における状態量統合装置の概略構成を示すブロック図である。
【図４】本発明の第１の実施の形態における車両の移動による観測誤差分散の増加例を示す図である。
【図５】本発明の第１の実施の形態における状態量統合処理ルーチンの内容を示すフローチャートである。
【図６】本発明の第２の実施の形態における全体概略構成を示す図である。
【図７】本発明の第２の実施の形態における状態量統合装置の概略構成を示すブロック図である。
【発明を実施するための形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２４】
＜本発明の実施の形態の概要＞
本発明の実施の形態は、複数のセンサ間でフレーム同期をしないアプローチをとっている。あるセンサからのセンシング結果を受け取ったタイミングで、歩行者位置の状態量と受け取ったセンシング結果を対応付けて、誤差を計算し、状態量の歩行者位置を補正しながら、歩行者を検出・追従する。この処理を各センサからセンシング結果が状態量統合装置に届く毎に行う。
【００２５】
しかし、本アプローチでは、複数のセンサで取得した各データにはセンサで計測した時刻の誤差が含まれているため、その時刻の値をそのまま信用してセンシング結果を統合してしまうと、特にセンサや対象物が移動している場合にセンサの統合が上手くいかず、精度が落ちてしまう。
【００２６】
この問題を解決するために、想定されるセンサの時刻誤差をもとに、移動しているセンサや対象物の速度から、時刻ずれによる位置の誤差を推定し、その値に基づいて観測に誤差を付与する。
【００２７】
［第１の実施の形態］
本発明の第１の実施の形態では、車両に搭載された歩行者の検出器やインフラセンサとしての歩行者の検出器の検出結果を統合して歩行者の状態量を推定する状態量統合装置に本発明を適用した場合を例に説明する。
【００２８】
＜状態量統合システムのシステム構成＞
図１に示すように、本発明の実施の形態に係る状態量統合装置システム１０は、状態量統合装置１００と、基地局５００と、複数の車両に搭載された複数の検出器６００と、インフラセンサとしての検出器７００とを備え、基地局５００と状態量統合装置１００とは、インターネットなどのネットワーク８００で接続されており、基地局５００と検出器６００、７００とは、無線通信により接続されている。
【００２９】
基地局５００は、検出器６００、７００が在圏している基地局であり、在圏している基地局が複数ある場合には、いずれの基地局を用いても良い。
【００３０】
検出器６００、７００は、カメラやレーダを用いて歩行者を随時検出し、検出する度に、検出結果を、基地局５００を介して、状態量統合装置１００へ送信する。
【００３１】
図２に示すように、検出器６００は、センサ６１０と、計時部６２０と、測位センサ６３０と、通信部６４０とを備えている。
【００３２】
センサ６１０は、自車両の前方を撮像するカメラによって撮像された前方の道路画像から、スライディングウィンドウ毎に、画像特徴量（ＳＩＦＴ，ＦＩＮＤ、ＨＯＧなど）を抽出し、スライディングウィンドウ毎の画像特徴量と、歩行者検出モデル（ＳＶＭ、ＡｄａＢｏｏｓｔ）とを用いて、歩行者を検出し、検出された歩行者位置を表す画像座標を求める。
【００３３】
また、センサ６１０は、歩行者位置を表す画像座標を、３次元位置に変換する。この際に、検出された歩行者の高さに応じて誤差分散行列を求める。センサ６１０は、自車両に搭載されたＧＰＳにより計測された自車両の絶対座標と、求められた３次元位置とに基づいて、歩行者の絶対的な３次元位置を求める。
【００３４】
上記の歩行者の３次元位置および誤差分散行列を、検出された歩行者毎に求め、車両Ｃｉのセンサ６１０において検出した歩行者の３次元位置のセットＹｉ＝｛ｙｉ，１，…ｙｉ，ｍ｝と、観測誤差分散行列のセットＲｉ＝｛ｒｉ，１，…，ｒｉ，ｍ｝とからなる観測Ｏｉ＝｛Ｙｉ，Ｒｉ｝を、観測毎に、通信部６４０へ渡す。
【００３５】
測位センサ６３０は、自車両に搭載されたＧＰＳにより自車両の位置ｐｉ、を求めると共に、その測位分散ｑｉを求める。そして、測位センサ６３０は、自車両の位置ｐｉ、および、測位分散ｑｉを、測位する毎に、通信部６４０へ渡す。
【００３６】
計時部６２０は、時刻を計測している。計時部６２０は、センサ６１０が観測する毎に、当該観測時刻を通信部６４０へ渡す。また、計時部６２０は、測位センサ６３０が、測位する毎に、当該測位時刻を通信部６４０へ渡す。
【００３７】
通信部６４０は、状態量統合装置１００の通信部１０２と通信する。具体的には、通信部６４０は、観測Ｏｉおよび観測時刻を、観測毎に、状態量統合装置１００に送信する。また、通信部６４０は、自車両の位置ｐｉ、測位分散ｑｉ、および、測位時刻を、測位する毎に、状態量統合装置１００に送信する。
【００３８】
また、図２に示すように、検出器７００は、センサ７１０と、計時部７２０と、通信部７３０とを備えている。
【００３９】
センサ７１０は、センサ６１０と同様に、検出した歩行者の３次元位置のセットと、観測誤差分散行列のセットとからなる観測データを、観測毎に、通信部７３０へ送信する。
【００４０】
計時部７２０は、計時部６２０と同様に、センサ７１０が観測する毎に、当該観測時刻を通信部７３０へ渡す。
【００４１】
通信部７３０は、通信部６４０と同様に、観測Ｏｉおよび観測時刻を、観測毎に、状態量統合装置１００に送信する。
【００４２】
複数の検出器６００、７００は、非同期に、歩行者を検出している。
【００４３】
状態量統合装置１００は、ＣＰＵと、ＲＡＭと、後述する状態量統合処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００４４】
図３に示すように、状態量統合装置１００は、通信部１０２と、センシング結果取得部１０４と、移動物状態量記憶部１０６と、移動物状態量予測部１０８と、移動物状態量消去部１１０と、時間変化量推定部１１２と、観測誤差推定部１１４と、移動物状態量統合部１１６と、移動物存在確率更新部１１８と、移動物存在確率記憶部１２０と、移動物状態量更新部１２２と、センサ時間変化量推定部１２４と、センサ状態量記憶部１２６と、センサ状態量予測部１２８と、センサ観測誤差推定部１３０と、センサ状態量更新部１３２とを備えている。
【００４５】
通信部１０２は、検出器６００、７００から、観測データおよび測位データを、受信する。また、通信部１０２は、検出器６００、７００に対し、移動物状態量更新部１２２から取得した歩行者の状態量、および、センサ状態量更新部１３２から取得した車両Ｃｉの状態量を、取得される毎に、送信する。
【００４６】
センシング結果取得部１０４は、検出器６００、７００の何れかから送信された歩行者の座標のセットおよび観測誤差分散行列のセットからなる観測データを、通信部１０２により受信する毎に取得する。
【００４７】
そして、センシング結果取得部１０４は、取得した観測データを時間変化量推定部１１２へ渡す。
【００４８】
また、センシング結果取得部１０４は、検出器６００から送信された自車両の位置ｐｉ、測位分散ｑｉ、および、測位時刻からなる測位データを、通信部１０２により受信する毎に取得する。
【００４９】
そして、センシング結果取得部１０４は、取得した測位データをセンサ時間変化量推定部１２４へ渡す。
【００５０】
移動物状態量記憶部１０６は、観測された複数の歩行者それぞれの、移動物状態量更新部１２２により更新された状態量（歩行者の位置および速度）、状態量の分散共分散行列、および、最後に観測した時刻を記憶する。具体的には、歩行者の状態量のセットＸ＝｛ｘ１，…ｘｎ｝、および、状態量の分散共分散行列Ｖ＝｛ｖ１，…，ｖｎ｝を記憶している。
【００５１】
移動物状態量予測部１０８は、受信した時刻ｔに合わせて、複数の歩行者それぞれについて、カルマンフィルタの予測ステップにより、移動物状態量記憶部１０６に記憶された状態量を用いて次の時刻の歩行者の状態量を予測することを繰り返し、受信した時刻ｔにおける歩行者の状態量のセットＸ＝｛ｘ１，…ｘｎ｝、および、状態量の分散共分散行列Ｖ＝｛ｖ１，…，ｖｎ｝を求める。例えば、等速予測などにより、次の時刻の歩行者の状態量を予測する。
【００５２】
移動物状態量消去部１１０は、最後に観測した時刻から一定時間以上経過している歩行者の存在確率の各々について、最後に観測した時刻からの経過時間に応じた存在確率となるように減衰させて更新する。
【００５３】
また、移動物状態量消去部１１０は、存在確率が一定値以下となった歩行者の状態量を、移動物状態量記憶部１０６から消去する。
【００５４】
時間変化量推定部１１２は、センサ状態量記憶部１２６に記憶されている車両Ｃｉの位置情報および速度情報
2019061551.tif 000003
から、車両Ｃｉの時間変化量
2019061551.tif 000004
を推定する。
【００５５】
具体的には、時間変化量
2019061551.tif 000005
は、位置情報および速度情報
2019061551.tif 000006
を時間微分したものである。
【００５６】
また、時間変化量推定部１１２は、移動物状態量記憶部１０６に記憶されている状態量から、ｌ番目の歩行者の時間変化量
2019061551.tif 000007
を推定する。
【００５７】
具体的には、時間変化量
2019061551.tif 000008
は、ｌ番目の歩行者の状態量を時間微分したものである。
【００５８】
観測誤差推定部１１４は、車両Ｃｉの観測誤差分散行列Ｒｉに対して、下記式（１）のように、時刻誤差分散（時刻誤差に基づく分散）を付与することにより、修正後の観測誤差分散行列Ｒｉ´を求める。
【００５９】
【数１】
2019061551.tif 000009
【００６０】
ここで、
2019061551.tif 000010
は、車両Ｃｉにおける検出器６００の計時部６２０における時刻誤差の標準偏差であり、予め定められた値であり、
2019061551.tif 000011
は、車両の状態量から、歩行者の観測データと同じ空間の観測を出力するモデルである。
【００６１】
また、観測誤差推定部１１４は、車両Ｃｉと歩行者ｌとの組み合わせ毎に、観測誤差分散行列Ｒｉ´に対して、下記式（２）のように、時刻誤差分散（時刻誤差に基づく分散）を付与することにより、車両Ｃｉと歩行者ｌとの組み合わせにおける修正後の観測誤差分散行列Ｒｉ，ｌ´´を求める。
【００６２】
【数２】
2019061551.tif 000012
【００６３】
ここで、
2019061551.tif 000013
は、歩行者の観測モデルである。
【００６４】
図４は、車両の移動による観測誤差分散の増加例を示す図である。図４に示すように、時刻誤差分散を付与することによって、車両の移動方向に対して、観測誤差分散が増加することが分かる。このため、時刻ずれが生じていても、その時刻ずれを考慮した観測誤差分散に修正できることが分かる。
【００６５】
移動物状態量統合部１１６は、センシング結果取得部１０４により最新のデータを受け取る毎に、当該最新のデータの時刻に対応して移動物状態量予測部１０８によって複数の歩行者それぞれについて予測された状態量と、当該最新のデータが表す複数の歩行者それぞれの検出結果との対応付けを行う。
【００６６】
具体的には、当該最新のデータの時刻における歩行者の状態量のセットＸ＝｛ｘ１，…ｘｎ｝と、当該最新のデータにおいて検出した歩行者の３次元位置のセットＹｉ＝｛ｙｉ，１，…ｙｉ，ｍ｝との対応付けを行う。例えば、対応付けられた状態量ｘｌと検出された歩行者の３次元位置ｙｉ，ｋとの組み合わせの観測確率ｐ（ｙｉ，ｋ｜ｘｌ）の積を最大にする対応付けを、ハンガリアン法などの手法により高速に計算する。
【００６７】
対応付けは、例えば、観測確率ｐ（ｙｉ，ｋ｜ｘｌ）を用いて
2019061551.tif 000014
を最大とするｋおよびｌの組み合わせを求めるなどの方法により計算されるが、この観測確率に用いる分散に、時間変化量推定部１１２で得られた修正後の観測誤差分散行列Ｒｉ，ｌ´´を用いる。
【００６８】
なお、状態量ｘｌと検出された歩行者の３次元位置ｙｉ，ｋとの組み合わせの確率ｐ（ｙｉ，ｋ｜ｘｌ）は、状態量ｘｌを中心とし、対応するカルマンフィルタの分散ｖｌと、３次元位置ｙｉ，ｋに対応する観測誤差分散行列
2019061551.tif 000015
を修正した観測誤差分散行列
2019061551.tif 000016
との和を、分散とした多次元正規分布における、３次元位置ｙｉ，ｋの確率に基づいて、確率ｐ（ｙｉ，ｋ｜ｘｌ）を求めればよい。
【００６９】
また、対応付けない確率を設定値として与え、この設定値より小さい確率の組み合わせは、対応付けられないようにする。
【００７０】
移動物存在確率更新部１１８は、移動物状態量統合部１１６による検出結果との対応付けの結果に基づいて、最新のデータの検出結果のうち、状態量と対応付かなかった歩行者の検出結果に対し、当該歩行者の存在確率として、設定値を移動物存在確率記憶部１２０に格納する。設定値は、検出器６００、７００について予め定められた検出精度に応じて定めればよい。
【００７１】
また、移動物存在確率更新部１１８は、移動物存在確率記憶部１２０に記憶されている複数の歩行者それぞれの存在確率のうち、移動物状態量統合部１１６により最新のデータの検出結果と対応付けられた歩行者の存在確率の各々について、検出器６００、７００について予め定められた検出精度に応じて増加させるように更新する。
【００７２】
移動物存在確率記憶部１２０は、移動物状態量記憶部１０６に記憶されている複数の歩行者それぞれの存在確率を記憶する。
【００７３】
移動物状態量更新部１２２は、移動物状態量統合部１１６により最新のデータの検出結果と対応付けられた状態量の各々について、対応する３次元位置ｙｉ，ｊを観測値として、修正後の誤差分散行列
2019061551.tif 000017
、予測ステップで得られた歩行者の状態量および分散共分散行列を用いて、カルマンフィルタのフィルタリングステップにより、状態量を更新すると共に、最後に観測した時刻を更新する。
【００７４】
また、移動物状態量更新部１２２は、移動物状態量統合部１１６による検出結果との対応付けの結果に基づいて、最新のデータの検出結果のうち、状態量と対応付かなかった歩行者の検出結果から、新たな状態量ｘを生成し、最後に観測した時刻として、現在時刻を、移動物状態量記憶部１０６に格納する。なお、新たな状態量ｘは観測された歩行者の３次元位置ｙと同じ座標、当該最新のデータを送信した車両Ｃｉについて求められた誤差分散行列
2019061551.tif 000018
を持つこととする。
【００７５】
また、移動物状態量更新部１２２は、更新した状態量、および、生成した状態量を通信部１０２へ渡す。
【００７６】
センサ時間変化量推定部１２４は、時間変化量推定部１１２と同様に、センサ状態量記憶部１２６に記憶されている車両Ｃｉの位置情報および速度情報
2019061551.tif 000019
から、車両Ｃｉの時間変化量
2019061551.tif 000020
を推定する。
【００７７】
センサ状態量記憶部１２６は、測位した車両Ｃｉの、センサ状態量更新部１３２により更新された状態量（車両の位置情報および速度情報
2019061551.tif 000021
）、および、分散共分散行列を記憶する。
【００７８】
センサ状態量予測部１２８は、受信した時刻ｔに合わせて、車両Ｃｉについて、カルマンフィルタの予測ステップにより、センサ状態量記憶部１２６に記憶された車両Ｃｉの位置情報および速度情報
2019061551.tif 000022
と、分散共分散行列とを用いて、次の時刻の車両の状態量を車両モデルから予測することを繰り返し、受信した時刻ｔにおける車両Ｃｉの状態量、および、その分散共分散行列を求める。
【００７９】
センサ観測誤差推定部１３０は、車両Ｃｉの測位分散ｑｉに対して、下記式（３）のように、時刻誤差分散を付与することにより、修正後の測位分散ｑｉ´を求める。
【００８０】
【数３】
2019061551.tif 000023
【００８１】
ここで、
2019061551.tif 000024
は、車両Ｃｉの測位結果の観測モデルである。
【００８２】
センサ状態量更新部１３２は、センサ状態量予測部１２８により得られた車両Ｃｉの状態量について、車両位置を観測値として、修正後の測位分散ｑｉ´、予測ステップで得られた車両Ｃｉの状態量、および、分散共分散行列を用いて、カルマンフィルタのフィルタリングステップにより、センサ状態記憶部１２６に記憶された車両Ｃｉの状態量を更新する。
【００８３】
そして、センサ状態量更新部１３２は、更新した車両Ｃｉの状態量を、通信部１０２へ渡す。
【００８４】
＜状態量統合装置１００の動作＞
次に、図５を参照して、本実施形態の状態量統合装置１００の状態量統合処理ルーチンについて説明する。
【００８５】
まず、ステップＳ１００において、通信部１０２は、検出器６００、７００からデータを受信する。
【００８６】
ステップＳ１１０において、センシング結果取得部２０４は、受信したデータが観測データか否かの判断をする。
【００８７】
受信したデータが、観測データである場合（ステップＳ１１０のＹＥＳ）、ステップＳ１２０において、移動物状態量予測部１０８は、受信した時刻ｔに合わせて、複数の歩行者それぞれについて、カルマンフィルタの予測ステップにより、移動物状態量記憶部１０６に記憶された状態量を用いて次の時刻の歩行者の状態量を予測することを繰り返し、受信した時刻ｔにおける歩行者の状態量のセットＸ＝｛ｘ１，…ｘｎ｝、および、状態量の分散共分散行列Ｖ＝｛ｖ１，…，ｖｎ｝を求める。
【００８８】
ステップＳ１３０において、移動物状態量消去部１１０は、最後に観測した時刻から一定時間以上経過している歩行者の存在確率の各々について、最後に観測した時刻からの経過時間に応じた存在確率となるように減衰させて更新する。
【００８９】
ステップＳ１４０において、移動物状態量消去部１１０は、存在確率が一定値以下となった歩行者の状態量を、移動物状態量記憶部１０６から消去する。
【００９０】
ステップＳ１５０において、時間変化量推定部１１２は、センサ状態量記憶部１２６に記憶されている車両Ｃｉの位置情報および速度情報
2019061551.tif 000025
から、車両Ｃｉの時間変化量
2019061551.tif 000026
を推定する。
【００９１】
また、時間変化量推定部１１２は、移動物状態量記憶部１０６に記憶されている状態量から、ｌ番目の歩行者の時間変化量
2019061551.tif 000027
を推定する。
【００９２】
ステップＳ１６０において、観測誤差推定部１１４は、車両Ｃｉの観測誤差分散行列Ｒｉに対して、式（１）のように、時刻誤差分散を付与することにより、修正後の観測誤差分散行列Ｒｉ´を求める。
【００９３】
また、観測誤差推定部１１４は、車両Ｃｉと歩行者ｌとの組み合わせ毎に、観測誤差分散行列Ｒｉ´に対して、式（２）のように、時刻誤差分散を付与することにより、車両Ｃｉと歩行者ｌとの組み合わせにおける修正後の観測誤差分散行列Ｒｉ,l´´を求める。
【００９４】
ステップＳ１７０において、移動物状態量統合部１１６は、センシング結果取得部１０４により最新のデータを受け取る毎に、当該最新のデータの時刻に対応して移動物状態量予測部１０８によって複数の歩行者それぞれについて予測された状態量と、当該最新のデータが表す複数の歩行者それぞれの検出結果との対応付けを行う。
【００９５】
ステップＳ１８０において、移動物存在確率更新部１１８は、移動物状態量統合部１１６による検出結果との対応付けの結果に基づいて、最新のデータの検出結果のうち、状態量と対応付かなかった歩行者の検出結果に対し、当該歩行者の存在確率として、設定値を移動物存在確率記憶部１２０に格納する。
【００９６】
また、移動物存在確率更新部１１８は、移動物存在確率記憶部１２０に記憶されている複数の歩行者それぞれの存在確率のうち、移動物状態量統合部１１６により最新のデータの検出結果と対応付けられた歩行者の存在確率の各々について、検出器６００、７００について予め定められた検出精度に応じて増加させるように更新する。
【００９７】
ステップＳ１９０において、移動物状態量更新部１２２は、移動物状態量統合部１１６により最新のデータの検出結果と対応付けられた状態量の各々について、対応する３次元位置ｙｉ，ｊを観測値として、修正後の誤差分散行列
2019061551.tif 000028
、予測ステップで得られた歩行者の状態量および分散共分散行列を用いて、カルマンフィルタのフィルタリングステップにより、状態量を更新すると共に、最後に観測した時刻を更新する。
【００９８】
ステップＳ２００において、移動物状態量更新部１２２は、移動物状態量統合部１１６による検出結果との対応付けの結果に基づいて、最新のデータの検出結果のうち、状態量と対応付かなかった歩行者の検出結果から、新たな状態量ｘを生成し、最後に観測した時刻として、現在時刻を、移動物状態量記憶部１０６に格納する。
【００９９】
受信したデータが、測位データである場合（ステップＳ１１０のＮＯ）、ステップＳ２１０において、センサ状態量予測部１２８は、受信した時刻ｔに合わせて、車両Ｃｉについて、カルマンフィルタの予測ステップにより、センサ状態量記憶部１２６に記憶された車両の位置情報および速度情報
2019061551.tif 000029
と、分散共分散行列とを用いて、次の時刻の車両の状態量を車両モデルから予測することを繰り返し、受信した時刻ｔにおける車両Ｃｉの状態量、および、その分散共分散行列を求める。
【０１００】
ステップＳ２２０において、センサ時間変化量推定部１２４は、センサ状態量記憶部１２６に記憶されている車両Ｃｉの位置情報および速度情報
2019061551.tif 000030
から、車両Ｃｉの時間変化量
2019061551.tif 000031
を推定する。
【０１０１】
ステップＳ２３０において、センサ観測誤差推定部１３０は、車両Ｃｉの測位分散ｑｉに対して、式（３）のように、時刻誤差分散を付与することにより、修正後の測位分散ｑｉ´を求める。
【０１０２】
ステップＳ２４０において、センサ状態量更新部１３２は、センサ状態量予測部１２８により得られた車両Ｃｉの状態量について、車両位置を観測値として、測位分散ｑｉ´、予測ステップで得られた車両Ｃｉの状態量、および、分散共分散行列を用いて、カルマンフィルタのフィルタリングステップにより、センサ状態記憶部１２６に記憶された車両Ｃｉの状態量を更新する。
【０１０３】
ステップＳ２５０において、通信部１０２は、検出器６００、７００に対し、移動物状態量更新部１２２から取得した歩行者の状態量、および、センサ状態量更新部１３２から取得した車両Ｃｉの状態量を、取得される毎に、送信する。
【０１０４】
以上説明したように、第１の実施の形態に係る状態量統合システムによれば、記憶された状態量から推定される移動物の時間変化量又はセンサの時間変化量を推定し、時間変化量に基づいて、複数の移動物それぞれについての移動物の状態量の観測誤差を推定し、予測された移動物の状態量と、複数の移動物それぞれの検出結果との対応付けを行い、複数の移動物それぞれの状態量を更新することにより、センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができる。
【０１０５】
複数のセンサで取得した各データには、センサで計測した時刻の誤差が含まれている。その時刻の値をそのまま信用してセンシング結果を統合してしまうと、特にセンサや対象物が移動している場合にセンサの統合が上手くいかず、精度が落ちるという問題がある。そこで、第１の実施の形態では、想定されるセンサの時刻誤差をもとに、移動しているセンサや対象物の速度から、時刻ずれによる位置の誤差を推定し、その値に基づいて観測に誤差を付与する。これにより、時刻の誤差が含まれていることを考慮してセンシング結果を統合することが可能となるため、ロバスト性が向上し、結果として精度が向上する。
【０１０６】
また、時刻誤差を考慮してセンシング結果を統合することで、これらの時刻誤差による影響を軽減し、精度を向上することが可能である。
【０１０７】
［第２の実施の形態］
＜状態量統合システムのシステム構成＞
本発明の第２の実施の形態では、状態量統合装置および検出器が車両に搭載され、インフラセンサとしての歩行者の検出器が道路上に設置され、状態量統合装置を搭載した自車両の検出器が得た歩行者の検出結果と、他の検出器の検出結果とを統合して歩行者の状態量を推定する状態量統合装置に本発明を適用した場合を例に説明する。
【０１０８】
なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１０９】
図６に示すように、本発明の実施の形態に係る状態量統合装置システム２０は、車両に搭載された状態量統合装置２００および検出器６００と、道路上に設置された検出器７００と、を備え、状態量統合装置２００と、検出器７００とは、無線通信により接続されている。
【０１１０】
検出器７００は、車両と通信可能な範囲に存在する検出器であり、第１の実施の形態の検出器７００と同様の構成である。
【０１１１】
また、車両に搭載された検出器６００は、第１の実施の形態の検出器６００と同様の構成である。なお、車両に搭載された検出器の通信部６４０は、センシング結果取得部２０４と有線接続されていてもよい。
【０１１２】
図７に示すように、状態量統合装置２００は、第１の実施の形態の状態量統合装置１００と、センシング結果取得部２０４を除き、同様の構成である。
【０１１３】
センシング結果取得部２０４は、検出器７００から送信された歩行者の座標のセットおよび観測誤差分散行列のセットからなる観測データを、通信部１０２により受信する毎に、歩行者の座標のセットおよび観測誤差分散行列のセットからなる観測データを取得する。
【０１１４】
また、センシング結果取得部２０４は、自車両の検出器６００から出力された歩行者の座標のセットおよび観測誤差分散行列のセットからなる観測データを取得する。
【０１１５】
そして、センシング結果取得部２０４は、取得した観測データを時間変化量推定部１１２へ渡す。
【０１１６】
また、センシング結果取得部２０４は、自車両の検出器６００から出力された自車両の位置ｐｉ、測位分散ｑｉ、および、測位時刻からなる測位データを取得する。
【０１１７】
そして、センシング結果取得部２０４は、取得した測位データをセンサ時間変化量推定部１２４へ渡す。
【０１１８】
移動物状態量更新部１２２は、更新した状態量、および、生成した状態量を、通信部１０２を介して、他の車両へ送信したり、自車両の運転支援システムなどへ出力する。
【０１１９】
＜状態量統合装置２００の動作＞
状態量統合装置２００の動作は、状態量統合装置１００の動作と同様であるため、説明を省略する。
【０１２０】
以上説明したように、第２の実施の形態に係る状態量統合システムによれば、記憶された状態量から推定される移動物の時間変化量又はセンサの時間変化量を推定し、時間変化量に基づいて、複数の移動物それぞれについての移動物の状態量の観測誤差を推定し、予測された移動物の状態量と、複数の移動物それぞれの検出結果との対応付けを行い、複数の移動物それぞれの状態量を更新することにより、センサ間で異なる誤差を持っている場合でも、精度良くセンシング結果を統合することができる。
【０１２１】
なお、上記の実施の形態では、検出対象となる移動物として歩行者を対象とする場合を例に説明したが、これに限定されるものではなく、例えば車両などの他の移動体を検出対象としてもよい。
【０１２２】
また、上記の実施の形態では、センサの状態の時間変化量を、速度に基づいて検出したが、時間変化する対象は速度に限られるものではない。例えばセンサが回転している場合には、回転による時刻誤差が生じるため、角速度に基づいた時間変化量を求めてもよい。他に、形状の変化、色彩の変化、光量の変化など、時間によりセンサ又は対象物の状態に変化が生ずるものであれば、これらの時間変化量の対象と成りうる。
【０１２３】
また、上記の実施の形態では、カルマンフィルタを用いたが、他のフィルタリング手法、例えば、ベイジアンフィルタなどを用いてもよい。
【０１２４】
また、上記の実施の形態では、インフラセンサには測位センサを搭載しない構成としたが、インフラセンサが移動や回転のような状態変化をする場合に、測位センサが搭載されてもよい。
【０１２５】
また、センサで計測した時刻の誤差を考慮して、観測誤差を修正する場合を例に説明したが、これに限定されるものではなく、センサ間で異なる時刻以外の誤差を考慮して、観測誤差を修正するようにしてもよい。
【０１２６】
なお、本発明のプログラムは、記録媒体に格納して提供することができる。
【符号の説明】
【０１２７】
１０   状態量統合装置システム
２０   状態量統合装置システム
１００ 状態量統合装置
１０２ 通信部
１０４ センシング結果取得部
１０６ 移動物状態量記憶部
１０８ 移動物状態量予測部
１１０ 移動物状態量消去部
１１２ 時間変化量推定部
１１４ 観測誤差推定部
１１６ 移動物状態量統合部
１１８ 移動物存在確率更新部
１２０ 移動物存在確率記憶部
１２２ 移動物状態量更新部
１２４ センサ時間変化量推定部
１２６ センサ状態記憶部
１２６ センサ状態量記憶部
１２８ センサ状態量予測部
１３０ センサ観測誤差推定部
１３２ センサ状態量更新部
２００ 状態量統合装置
２０４ センシング結果取得部
５００ 基地局
６００ 検出器
６１０ センサ
６２０ 計時部
６３０ 測位センサ
６４０ 通信部
７００ 検出器
７１０ センサ
７２０ 計時部
７３０ 通信部
８００ ネットワーク
【図１】
2019061551.tif 000032
【図２】
2019061551.tif 000033
【図３】
2019061551.tif 000034
【図４】
2019061551.tif 000035
【図５】
2019061551.tif 000036
【図６】
2019061551.tif 000037
【図７】
2019061551.tif 000038