(57)【要約】【課題】センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができるようにする。【解決手段】追跡している追跡対象物の各々について、状態量記憶部１２０に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報１４０とに基づいて、境界検出部１３０が、当該追跡対象物の周辺の可動範囲の境界を検出し、仮説予測部１５０が、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、仮説更新部２００が、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新する。【選択図】図２
2019144900.tif 000002
【特許請求の範囲】
【請求項１】
センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部と、
前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部と、
前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部と、
を含む状態推定装置。
【請求項２】
前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界を検出すると共に、前記検出された前記可動範囲の境界から障害物が存在する範囲を検出し、
前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記障害物が存在する範囲と、前記可動範囲との境界に応じて前記仮説の予測結果を修正する
請求項１記載の状態推定装置。
【請求項３】
前記仮説更新部は、
前記仮説の予測結果の何れとも対応してない前記追跡対象物の検出結果が存在する場合に、前記追跡対象物の状態を示す仮説を新たに生成し、
前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記新たに生成された前記追跡対象物の仮説を消去するか、又は前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる存在確率を減ずる請求項２記載の状態推定装置。
【請求項４】
前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界として、路面上の標示境界を検出し、
前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置を中心とする所定の範囲と、前記検出された標示境界とが交差する場合に、前記所定の範囲と前記標示境界との交差状態に応じて前記仮説の予測結果を修正する請求項１〜請求項３の何れか１項記載の状態推定装置。
【請求項５】
前記センシング結果獲得部は、複数センサを用いて、追跡対象物を検出した結果を受け取る請求項１〜請求項４の何れか１項記載の状態推定装置。
【請求項６】
コンピュータを、
センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部、
前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部、
前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部、及び、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、地図と可動物の運動状態から、各可動物の将来移動範囲をリスクポテンシャルで算出し、その重なりによって危険を報知する技術が知られている（特許文献１〜４）。
【０００３】
また、地図と可動物の運動状態からのリスクポテンシャルを算出する際に、算出手段を動的に切り替える装置が知られている（特許文献５）。
【０００４】
また、交通参加者の属性ごとの存在しやすさ（しにくさ）を表す地図を用いて、移動予測を行う装置が知られている（特許文献６）
【０００５】
また、複数のカメラ映像を用いて、車両領域を表すテンプレート位置を抽出し、テンプレート位置情報に対して、多視点的処理と時系列処理を施し、複数のカメラで検出された車両の対応付けを行うことで、走行車両の運動軌道を高精度に計算することができる走行車両検出装置が知られている（特許文献７）。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特表２００９−５０５２６０号公報
【特許文献２】特開２０１２−１６４１５９号公報
【特許文献３】国際公開第１６／０９２５９１号
【特許文献４】特許第５６７３１２７号公報
【特許文献５】特許第５６２０１４７号公報
【特許文献６】国際公開第２０１２／０３３１７３号
【特許文献７】特開２００１−３５７４８９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上記特許文献１〜４の技術は、可動物及び交通環境の局所的かつ相対的な位置関係が精緻に推定できることを前提としている。
【０００８】
しかしながら、簡易な構成では実現することが困難であり、また、乱数を用いた粒子によるリスクポテンシャルの計算は、繰り返し試行ごとに異なる結果となり、同一場面で異なる移動予測及び危険判定となるため、品質保証も困難となる。
【０００９】
また、特許文献５の技術は、局所的な可動物の将来予測範囲に占める歩車道の割合等の挙動の変化を促す要因によって、予測手段を切り替えて、リスクポテンシャルの計算コストを削減することを目指しているが、駐車場などの複雑な環境では、常に複雑な予測手段を選択することになるため、リスクポテンシャルの計算コストが削減できない。
【００１０】
また、特許文献６の技術は、交通参加者ごとの存在しやすさ（しにくさ）を表す存在可能性マップという形式での局所地図を生成する必要があり、センシングの誤認識等により属性が不明である交通参加者を取り扱うために、合理的な局所地図を用意することが難しい。
【００１１】
また、特許文献７の技術は、ビデオフレームが一定間隔で、複数のカメラ（センサ）間でフレーム同期とれることを前提としている。通信遅延やセンサの仕様で複数のセンサ間でフレーム同期（検出結果送信タイミングの同期）がとれない場合は、一定時間フレームを、結果を統合する装置上で蓄積し、データ補間等の同期処理を行う必要がある。さらにはセンサ数が不定の場合は、同期するセンサが不定ということであるため、同期処理が困難である。
【００１２】
特許文献７の問題を解決するべく、複数のセンサ間でフレーム同期をしないアプローチが考えられる（図１３）。しかしセンシング結果が確実かつ瞬時に装置に到来することを前提としているため、一時的な通信途絶や誤検出・未検出に対応できない問題がある。さらに、センシング結果から得られる観測量が、障害物の内側などとして明らかに誤っている場合にも、既存の状態量と対応づかない場合に、新規の状態量を生成する問題があった。
【００１３】
上記の問題点を解決するべく、状態量を複数個のパーティクルで表現し、歩車道等の標示情報による可動範囲地図を全状態量の観測量としてセンシング結果の到来とは独立に状態量を修正し、また状態量の新規生成の際に反映させるアプローチが考えられる（図１４）。
【００１４】
しかし、この構成では状態量の可動範囲地図上での適合度（尤度）を評価する際に、原理的に複数のパーティクルで状態量を表現するか、なんらかの近似計算を行う必要があり、多数の状態量を取り扱う際に計算効率が悪化する問題があった。また、尤度の値を標示と交通参加者の属性の全組合せについて定義しなければならない。
【００１５】
本発明は、上記問題点を解決するためになされたもので、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる状態推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１６】
上記の目的を達成するために、本発明の状態推定装置は、センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部と、前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部と、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部と、を含んで構成されている。
【００１７】
また、本発明のプログラムは、コンピュータを、センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部、前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部、及び、前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部として機能させるためのプログラムである。
【００１８】
本発明の状態推定装置及びプログラムによれば、センシング結果獲得部が、センサを用いて、追跡対象物を検出した結果を受け取り、状態量記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説を格納し、境界検出部が、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出する。
【００１９】
そして、仮説予測部が、追跡している追跡対象物の各々について、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、境界検出部により検出された境界に応じて仮説の予測結果を修正し、仮説更新部が、追跡している追跡対象物の各々について、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新する。
【００２０】
このように、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる。
【００２１】
また、本発明の状態推定装置は、前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界を検出すると共に、前記検出された前記可動範囲の境界から障害物が存在する範囲を検出し、前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記障害物が存在する範囲と、前記可動範囲との境界に応じて前記仮説の予測結果を修正することができる。
【００２２】
また、本発明の状態推定装置の前記仮説更新部は、前記仮説の予測結果の何れとも対応してない前記追跡対象物の検出結果が存在する場合に、前記追跡対象物の状態を示す仮説を新たに生成し、前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記新たに生成された前記追跡対象物の仮説を消去するか、又は前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる存在確率を減ずるができる。
【００２３】
また、本発明の状態推定装置の前記センシング結果獲得部は、複数センサを用いて、追跡対象物を検出した結果を受け取ることができる。
【発明の効果】
【００２４】
以上説明したように、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる、という効果が得られる。
【図面の簡単な説明】
【００２５】
【図１】本発明の実施の形態に係る歩行者状態量推定システムを示すブロック図である。
【図２】本発明の実施の形態に係る局地サーバを示すブロック図である。
【図３】本発明の実施の形態に係る衝突判定による状態量を修正する例を示すイメージ図である。
【図４】本発明の実施の形態に係る侵入判定による状態量を修正する例を示すイメージ図である。
【図５】本発明の実施の形態に係る複数の標示境界線の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図６】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図７】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図８】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図９】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図１０】本発明の実施の形態に係る状態量を削除する場合のイメージ図である。
【図１１】本発明の実施の形態に係る局地サーバにおける歩行者状態量推定処理ルーチンの内容を示すフローチャートである。
【図１２】本発明をより複雑な環境に適用した場合の例を示すイメージ図である。
【図１３】複数のセンサ間でフレーム同期をしないアプローチのイメージ図である。
【図１４】センシング結果の到来とは独立に状態量を修正し、また状態量の新規生成の際に反映させるアプローチのイメージ図である。
【発明を実施するための形態】
【００２６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００２７】
＜本発明の実施の形態の概要＞
本発明の実施の形態は、複数センサから非同期に到達した情報から生成した観測量と、既に保持している状態量の移動予測との近さに応じて、整合処理（対応付けと状態量の修正）を行い、適切な対応付けがない場合には、観測量周辺に可動範囲を鑑みて状態量を生成する。
【００２８】
このとき、移動物の常識的な状態量の移動予測を、障害物や標示境界（歩車道分離の白線等）付近では移動が抑制されるという近似によって簡易な幾何学計算のみで算出する。
【００２９】
全ての状態量は存在確率を持ち、観測量の割り当てがない場合に減少し、また各標示境界との位置関係に応じて減少幅が変動し、存在確率が十分に小さくなった際には状態量は消去される。
【００３０】
この結果、一時的な通信途絶や誤検出・未検出に対しても、ロバストな統合処理を実現することができる。
【００３１】
本発明の実施の形態では、車両に搭載された歩行者の検出器やインフラセンサとしての歩行者の検出器の検出結果を統合して歩行者の状態量を推定する局地サーバに本発明を適用した場合を例に説明する。
【００３２】
＜歩行者状態量推定システムのシステム構成＞
図１に示すように、第１の実施の形態に係る歩行者状態量推定システム１は、局地サーバ１０と、基地局５０と、複数の車両に搭載された複数の検出器６０と、インフラセンサとしての検出器６２とを備え、基地局５０と局地サーバ１０とは、インターネットなどのネットワーク７０で接続されており、基地局５０と検出器６０、６２とは、無線通信により接続されている。なお、局地サーバ１０は、状態推定装置の一例である。
【００３３】
検出器６０、６２は、カメラやレーダを用いて歩行者を随時検出し、検出する度に、検出結果を、基地局５０を介して、局地サーバ１０へ送信する。
【００３４】
例えば、検出器６０は、自車両の前方を撮像するカメラによって撮像された前方の道路画像から、スライディングウィンドウ毎に、画像特徴量（ＳＩＦＴ、ＦＩＮＤ、ＨＯＧなど）を抽出し、スライディングウィンドウ毎の画像特徴量と、歩行者検出モデル（ＳＶＭ、ＡｄａＢｏｏｓｔ）とを用いて、歩行者を検出し、検出された歩行者位置を表す画像座標を求める。また、検出器６０は、歩行者位置を表す画像座標を、３次元位置に変換する。この際に、検出された歩行者の高さに応じて誤差分散行列を求める。また、検出器６０は、自車両に搭載されたＧＰＳにより計測された自車両の絶対座標と、求められた３次元位置とに基づいて、歩行者の絶対的な３次元位置を求める。
【００３５】
上記の歩行者の３次元位置及び誤差分散行列を、検出された歩行者毎に求め、車両Ｃｉの検出器６０において検出した歩行者の３次元位置のセットＹｉ＝｛ｙｉ，１，…，ｙｉ，ｍ｝と、観測誤差分散行列のセットＲｉ＝｛ｒｉ，１，…，ｒｉ，ｍ｝とからなる観測情報Ｏｉ＝｛Ｙｉ，Ｒｉ｝を、観測毎に、局地サーバ１０へ送信する。
【００３６】
複数の検出器６０、６２は、非同期に、歩行者を検出している。また、複数の検出器６０は、各々異なる車両に搭載されているため、局地サーバ１０へ検出結果を送信する検出器６０、６２の数は、不定となる。
【００３７】
局地サーバ１０は、ＣＰＵと、ＲＡＭと、後述する歩行者状態量推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００３８】
図２に示すように、局地サーバ１０は、通信部１００と、センシング結果獲得部１１０と、状態量記憶部１２０と、境界検出部１３０と、移動物可動範囲地図１４０と、仮説予測部１５０と、衝突判定状態量修正部１６０と、境界侵入判定状態量修正部１７０と、仮説整合部１８０と、仮説消去部１９０と、仮説更新部２００と、障害物境界内側判定部２１０とを備えている。
【００３９】
センシング結果獲得部１１０は、数不定の複数センサを用いて、歩行者を検出した結果を受け取る。
【００４０】
具体的には、センシング結果獲得部１１０は、検出器６０、６２の何れかから送信された歩行者の座標のセット及び観測誤差分散行列のセットからなる観測を、通信部１００により受信する毎に、歩行者の座標のセット及び観測誤差分散行列のセットからなる観測を取得する。
【００４１】
状態量記憶部１２０は、追跡している歩行者の各々について、当該歩行者の状態を示す仮説を格納する。
【００４２】
具体的には、状態量記憶部１２０は、仮説更新部２００により更新された状態量（歩行者の位置、速度を含む）、及び最後に観測した時刻を記憶する。すなわち、歩行者の状態量のセットＸ＝｛ｘ１，…，ｘｎ｝、状態量の分散共分散行列Ｖ＝｛ｖ１，…，ｖｎ｝、および、最後に観測した時刻のセットＴ＝｛ｔ１，…，ｔｎ｝を記憶している。
【００４３】
なお、本実施の形態では、
2019144900.tif 000003
を、信念空間上の仮説と呼び、その構成要素
2019144900.tif 000004
を追跡器と呼ぶこととする。
【００４４】
移動物可動範囲地図１４０は、歩行者の可動範囲が記録している。具体的には、障害物や標識表示による歩行者の可動範囲を示す地図であって、障害物か否かのラベル付きの線分集合
2019144900.tif 000005
で構成される境界地図Ｕを格納している。ここで、ｕは障害物を示す線分の集合であり、
2019144900.tif 000006
は障害物でない線分の集合である。
【００４５】
境界地図Ｕは、例えば遠隔サーバ８０から取得され、所定の周期で移動物可動範囲地図１４０の境界地図Ｕが更新される。例えば、遠隔サーバ８０は、１時間の周期で境界地図Ｕを局地サーバ１０に送信し、局地サーバ１０は境界地図Ｕを受信する度に移動物可動範囲地図１４０の境界地図Ｕを更新する。
【００４６】
境界検出部１３０は、追跡している歩行者の各々について、状態量記憶部１２０に格納された仮説と、境界地図Ｕとに基づいて、当該歩行者の周辺の可動範囲の境界を検出すると共に、検出された可動範囲の境界から障害物が存在する範囲と、歩行者の周辺の可動範囲の境界としての路面上の標示境界とを検出する。
【００４７】
具体的には、境界検出部１３０は、追跡している歩行者の各々について、状態量記憶部１２０に記憶された当該歩行者の仮説と、境界地図Ｕとに基づいて、当該歩行者の仮説の位置の周辺に存在する障害物を示す線分や標示境界（歩車道分離の白線等）である線分を境界として境界地図Ｕから検出する。
【００４８】
そして、検出した境界に含まれる線分のうち、障害物を示す線分の集合ｕに含まれる線分から、障害物が存在する範囲を検出する。また、検出した境界に含まれる線分のうち、障害物でない線分の集合
2019144900.tif 000007
に含まれる線分を、路面上の標示境界とする。
【００４９】
仮説予測部１５０は、追跡している歩行者の各々について、センサを用いて検出タイミングでの、当該歩行者の仮説が示す状態を予測する。
【００５０】
具体的には、仮説予測部１５０は、現時刻に合わせて、複数の歩行者それぞれについて、カルマンフィルタの予測ステップにより、次の時刻の歩行者の状態量を予測することを繰り返し、現時刻における歩行者の状態量のセットＸ＝｛ｘ１，…，ｘｎ｝、および、状態量の分散共分散行列Ｖ＝｛ｖ１，…，ｖｎ｝を求める。例えば、等速予測などにより、次の時刻の歩行者の状態量を予測する。すなわち、仮説予測部１５０は、仮説
2019144900.tif 000008
に対して、現時刻の状態に関する予測
2019144900.tif 000009
を生成する。すなわち、全ての追跡器
2019144900.tif 000010
について、時間幅
2019144900.tif 000011
だけ予測を行う。
【００５１】
衝突判定状態量修正部１６０は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、障害物が存在する範囲と、可動範囲との境界に応じて当該仮説の予測結果を修正する。
【００５２】
具体的には、衝突判定状態量修正部１６０は、予測を行った追跡器
2019144900.tif 000012
と、追跡器の予測前の状態量ｘｎと、予測後の状態量
2019144900.tif 000013
とをそれぞれ境界地図Ｕ上に投影して得た点
2019144900.tif 000014
及び
2019144900.tif 000015
で定義されるベクトル
2019144900.tif 000016
が、障害物を示す線分の集合ｕ内のいずれかの線分ｕｋと交差するか否かを計算する。
【００５３】
交差する場合（図３（ａ））、その交点をｘｕ，ｎ，ｋとする。このとき、図３（ｂ）のように、
2019144900.tif 000017
と修正して、予測後の状態量
2019144900.tif 000018
を修正する。この場合、修正した予測後の歩行者の位置は、図３（ｃ）のようになる。
【００５４】
また、衝突判定状態量修正部１６０は、図３（ｄ）のように、点ｘｕ，ｎ，ｋを中心として
2019144900.tif 000019
とｕｋとのなす角ψの倍角だけ
2019144900.tif 000020
を回転させたベクトルと、
2019144900.tif 000021
との合成ベクトルの差分
2019144900.tif 000022
をｘｎの新たな速度成分として、カルマンフィルタの予測ステップを再度行い、予測後の状態量
2019144900.tif 000023
を修正してもよい。この場合、修正した予測後の歩行者の位置は、図３（ｅ）のようになる。
【００５５】
境界侵入判定状態量修正部１７０は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置を中心とする所定の範囲と、検出された標示境界とが交差する場合に、所定の範囲と標示境界との交差状態に応じて当該仮説の予測結果を修正する。
【００５６】
具体的には、境界侵入判定状態量修正部１７０は、衝突判定状態量修正部１６０により得られたベクトル
2019144900.tif 000024
と、障害物でない線分の集合
2019144900.tif 000025
内のいずれかの線分
2019144900.tif 000026
が交差するか否かを計算する。なお、状態量に含まれる位置を中心とする一定確率の信頼楕円とは、歩行者の存在する信頼度が一定確率以上となる範囲を表す楕円である。
【００５７】
交差する場合（図４（ａ））、境界侵入判定状態量修正部１７０は、状態量
2019144900.tif 000027
と共分散
2019144900.tif 000028
との固有値解析で得られる、状態量に含まれる位置を中心とする一定確率の信頼楕円の範囲に対して、検出された標示境界の線分
2019144900.tif 000029
との交差判定を行う。
【００５８】
交差判定は２次方程式の判別式で行うことができ、楕円中心から交差する標示境界の線分
2019144900.tif 000030
への垂線のベクトルαの方向に、予測前の歩行者の速度ベクトルを、その交点と楕円中心のなす角θに比例した修正量αＫだけ修正する。ここで、αは予め定めた量を持つベクトルであり、Ｋは角θに応じて定まる値である。Ｋは、例えば下記式（１）のように求めることができる。
【００５９】
【数１】
2019144900.tif 000031
【００６０】
予測後の歩行者の位置が標示境界線に近い場合、歩行者は基本的に標示境界線を越えずに移動する場合が多いため、移動距離が小さくなるように予測前の歩行者の速度ベクトルを修正し、再度カルマンフィルタの予測ステップを行い、予測後の歩行者の位置を修正する。
【００６１】
この場合、角θはπに近い値となるため、式（１）の値は、１に近い値となる。すなわち、修正量αＫは、αに近い修正量となるため、歩行者の移動距離を小さくするように、予測前の歩行者の速度ベクトルを修正することができる。
【００６２】
例えば、予測後の歩行者の位置が標示境界線を越えている場合（図４（ｂ））、図４（ｃ）のように予測前の歩行者の速度ベクトルを修正する。また、予測後の歩行者の位置が標示境界線を越えていない場合（図４（ｄ））、図４（ｅ）のように予測前の歩行者の速度ベクトルを修正する。
【００６３】
一方、予測後の歩行者の位置が標示境界線から遠い場合、歩行者は標示境界線を越えて移動したと考えられるため、予測後の歩行者の位置を修正する必要がない。
【００６４】
この場合、角θはπよりも十分小さな値となるため、式（１）の値は、０に近い値となる。すなわち、修正量αＫは、０に近い修正量となるため、予測前の歩行者の速度ベクトルを修正しても、予測後の歩行者の位置をほとんど修正しないようにすることができる。
【００６５】
また、予測前の歩行者の速度ベクトルでなく、予測後の歩行者の位置を修正する構成としてもよい。また、Ｋの値が所定の値以下の場合に、予測前の歩行者の速度ベクトルを修正しないようにする構成としてもよい。
【００６６】
このように、交点と楕円中心のなす角によって予測後の歩行者の位置の修正を行うことで、極めて大きな速度で境界を越えようとする追跡器を除き、境界を越えないような非線形な予測を行うことができる。
【００６７】
この方法は、図５のような対象となる境界線が複数の場合にも適用することができる。
【００６８】
例えば、図５のように、２つの標示境界線に対して交点がある場合、交点の各々について、当該交点と楕円中心のなす角θに基づいて、楕円中心から交差する各標示境界の線分への垂線のベクトルの方向に修正量を求め、当該修正量を合成したベクトルを最終的な修正量として、修正することができる。なお、図５（ａ）は、予測前の状態量と標示境界線の配置、（ｂ）は修正量、（ｃ）は修正量の適用例を示すイメージ図である。
【００６９】
また、追跡器同士に適用することで歩行者同士が回避し合うような予測を行うこともできる（図６参照）。この場合、境界を歩行者の位置を中心とする信頼楕円同士に基づいて計算する。
【００７０】
例えば、異なる２つの歩行者同士が対面している場合、２つの歩行者の状態量を、信頼楕円の交点間線分で生成した境界線に対して、修正量の大きさを各仮説中心と信頼楕円の交点のなす角に比例させ、修正量の方向を、各仮説中心から境界線への垂線ベクトルの反対方向とする。ここで、図６（ａ）は、予測前の複数の歩行者の状態量の配置、（ｂ）は修正量、（ｃ）は修正量の適用例を示すイメージ図である。
【００７１】
ここで、２つの歩行者同士が、速度の速い状況で対面している場合、図６の場合よりもやや速い程度であれば（図７）には、図６と同様に修正量を算出する。ここで、図７（ａ）は、予測前の複数の歩行者の状態量の配置、（ｂ）は修正量、（ｃ）は修正量の適用例を示すイメージ図である。
【００７２】
一方、図６の場合よりもかなり速い（仮説同士がちょうど近接する）場合（図８）、互いに離れる方向に大きな修正量を算出する。衝突予測ではなく、追跡のための予測を目的としているため、基本的に全ての歩行者は衝突しないように移動している、という前提で予測を行うと仮定して良いので、すれ違うタイミングでの予測として、歩行者同士が大きく回避し合うと予測することができる。
【００７３】
なお、図８（ａ）は、予測前の複数の歩行者の状態量の配置、（ｂ）は修正量、（ｃ）は修正量の適用例を示すイメージ図である。
【００７４】
更に、図６の場合よりも極端に速く、仮説同士がすれ違ってしまうような場合（図９）、やや加速する方向に修正量を算出する。すなわち、すれ違うタイミングより後の時刻での予測として、回避し終えた後に歩行者が元の移動方向に維持・修正する動きに対応することができる。ここで、図９（ａ）は、予測前の複数の歩行者の状態量の配置、（ｂ）は修正量、（ｃ）は修正量の適用例を示すイメージ図である。
【００７５】
仮説整合部１８０は、センシング結果獲得部１１０により最新の観測情報Ｏｉを受け取ると、移動物可動範囲地図１４０の境界地図Ｕを用いて、現時刻に対応して、仮説予測部１５０により追跡している歩行者の各々について予測された仮説と、当該最新の観測情報Ｏｉが表す歩行者の各々についての検出結果との対応付けを行う。
【００７６】
具体的には、仮説整合部１８０は、予測された仮説と、境界とに基づいて、観測情報Ｏｉに対する、追跡器
2019144900.tif 000032
の当てはまりのよさを意味する観測尤度
2019144900.tif 000033
を計算する。
【００７７】
そして、仮説整合部１８０は、観測尤度θi，ｍに基づいて、現時刻における歩行者の状態量のセットＸ＝｛ｘ１，…，ｘｎ｝と、当該最新の観測情報Ｏｉにおいて検出した歩行者の３次元位置のセットＹｉ＝｛ｙｉ，１，…，ｙｉ，ｍ｝との対応付けを行う。
【００７８】
また、仮説整合部１８０は、仮説の予測結果の何れとも対応してない歩行者の検出結果が存在する場合に、歩行者の状態を示す仮説を新たに生成する。
【００７９】
具体的には、仮説整合部１８０は、最新の観測情報Ｏｉの検出結果との対応付けの結果に基づいて、最新の観測情報Ｏｉのうち、追跡器と対応付かなかった歩行者の検出結果から、新たな状態量ｘを含む追跡器を生成する。
【００８０】
なお、新たな状態量ｘを含む追跡器は、観測された歩行者の３次元位置ｙと同じ座標、誤差分散行列を持つこととする。
【００８１】
仮説更新部２００は、追跡している歩行者の各々について、当該歩行者の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。
【００８２】
具体的には、センシング結果獲得部１１０最新の観測情報Ｏｉを受け取った場合に、仮説整合部１８０最新の観測情報Ｏｉと対応付けられた歩行者の各々について、対応する３次元位置ｙｉ，ｊを観測値として、その誤差分散行列ｒｉ，ｊと、予測ステップで得られた歩行者の状態量及び分散共分散行列とを用いて、カルマンフィルタのフィルタリングステップにより、状態量を更新すると共に、最後に観測した時刻を更新する。
【００８３】
仮説更新部２００は、観測情報Ｏｉに対する、追跡器
2019144900.tif 000034
の観測尤度θi，ｍから、
2019144900.tif 000035
となる追跡器を特定し、
2019144900.tif 000036
が予め設定した閾値よりも大きい場合、状態量
2019144900.tif 000037
と分散共分散行列
2019144900.tif 000038
とを更新する。
【００８４】
また、
2019144900.tif 000039
が予め設定した閾値以下である場合、
2019144900.tif 000040
として観測情報Ｏｉ，ｍを３次元空間に投影した位置に、一定の存在確率が設定された新規の追跡器を生成して、
2019144900.tif 000041
に加える。
【００８５】
また、仮説更新部２００は、更新または新規作成された追跡器の最終更新時刻
2019144900.tif 000042
を、現在時刻に設定し、存在確率を
2019144900.tif 000043
等として、一定値だけ増加させる。
【００８６】
また、割り当ての無かった追跡器の存在確率を
2019144900.tif 000044
等として、一定値だけ減少させる。
【００８７】
そして、仮説更新部２００は、更新または新規作成された追跡器を、状態量記憶部１２０に格納する。この時、最後に観測した時刻として、現在時刻を格納する。
【００８８】
障害物境界内側判定部２１０は、新たに生成された歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、新たに生成された当該歩行者の仮説を消去するか、又は新たに生成された歩行者の仮説が示す状態に含まれる存在確率を減ずる。
【００８９】
具体的には、障害物境界内側判定部２１０は、仮説整合部１８０により新たに生成された追跡器の位置が、障害物が存在する範囲内である場合には、当該追跡器の存在確率を
2019144900.tif 000045
等として、当該追跡器が削除されるか、又はすぐには存在確率が上昇しないように設定する。
【００９０】
ここで、障害物が存在する範囲内であるか否かは、新たに生成した追跡器の状態量
2019144900.tif 000046
を境界地図Ｕに直情から投影して得た点
2019144900.tif 000047
と、境界地図Ｕ上の障害物である線分の集合ｕのうち閉路を形成するものであるｕｃについて、閉路内に
2019144900.tif 000048
が存在するか否かに基づいて判定する。
【００９１】
例えば、閉路を全て３つの線分からなる三角形のみで構成するように境界地図Ｕが作成されていれば、ある閉路ｕｃ，ｊ＝｛ｕｃ，ｊ，１，ｕｃ，ｊ，２，ｕｃ，ｊ，３｝を構成する３点について、３つの外積
2019144900.tif 000049
に関する下記式（２）の不等式が成立している場合に、
2019144900.tif 000050
は閉路ｕｃ，ｊの内側に存在すると判定することができる。
【００９２】
【数２】
2019144900.tif 000051
【００９３】
仮説消去部１９０は、最後に観測した時刻から一定時間以上経過している歩行者の状態量を含む追跡器を、状態量記憶部１２０から消去する（図１０）。
【００９４】
具体的には、仮説消去部１９０は、存在確率
2019144900.tif 000052
が十分に小さい追跡器を除去する。
【００９５】
局地サーバ１０は、上記の一連の処理により更新された状態量のセットＸを、歩行者の検出結果の統合結果として出力する。
【００９６】
＜歩行者状態量推定システム１の動作＞
次に、第１の実施の形態に係る歩行者状態量推定システム１の動作について説明する。まず、複数の車両に搭載された複数の検出器６０、及びインフラセンサとしての検出器６２の各々によって、歩行者が逐次検出され、検出される毎に、検出結果が、基地局５０を介して、局地サーバ１０に送信されているときに、局地サーバ１０において、図１１に示す歩行者状態量推定処理ルーチンが実行される。
【００９７】
ステップＳ１００において、仮説更新部２００は、下記式（３）のように仮説を空集合に初期化する。
【００９８】
【数３】
2019144900.tif 000053
【００９９】
ステップＳ１１０において、センシング結果獲得部１１０は、センサを用いて、歩行者を検出した結果である最新の観測情報Ｏｉを受け取る。
【０１００】
ステップＳ１２０において、仮説整合部１８０は、上記ステップＳ１５０で受信した最新の観測情報Ｏｉに含まれる歩行者の３次元位置を、センサ座標系から世界座標系に変換する。
【０１０１】
ステップＳ１３０において、境界検出部１３０は、追跡している歩行者の各々について、状態量記憶部１２０に格納された仮説と、境界地図Ｕとに基づいて、当該歩行者の周辺の可動範囲の境界を検出すると共に、検出された可動範囲の境界から障害物が存在する範囲と、歩行者の周辺の可動範囲の境界としての路面上の標示境界とを検出する。
【０１０２】
ステップＳ１４０において、仮説予測部１５０は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の仮説が示す状態を予測する。
【０１０３】
ステップＳ１５０において、衝突判定状態量修正部１６０は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、障害物が存在する範囲と、可動範囲との境界に応じて当該仮説の予測結果を修正する。
【０１０４】
ステップＳ１６０において、境界侵入判定状態量修正部１７０は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置を中心とする所定の範囲と、検出された標示境界とが交差する場合に、所定の範囲と標示境界との交差状態に応じて当該仮説の予測結果を修正する。
【０１０５】
ステップＳ１７０において、仮説整合部１８０は、センシング結果獲得部１１０により最新の観測情報Ｏｉを受け取ると、移動物可動範囲地図１４０の境界地図Ｕを用いて、現時刻に対応して、仮説予測部１５０により追跡している歩行者の各々について予測された仮説と、当該最新の観測情報Ｏｉが表す歩行者の各々についての検出結果との対応付けを行う。
【０１０６】
ステップＳ１８０において、仮説更新部２００は、追跡している歩行者の各々について、当該歩行者の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。また、仮説の予測結果の何れとも対応していない歩行者の検出結果が存在する場合に、歩行者の状態を示す仮説を新たに生成する。
【０１０７】
ステップＳ１９０において、障害物境界内側判定部２１０は、新たに生成された歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、新たに生成された当該歩行者の仮説を消去するか、又は新たに生成された歩行者の仮説が示す状態に含まれる存在確率を減ずる。
【０１０８】
ステップＳ２００において、仮説消去部１９０は、最後に観測した時刻から一定時間以上経過している歩行者の状態量を含む追跡器を、状態量記憶部１２０から消去する。
【０１０９】
そして、ステップＳ２１０において、更新された仮説が示す状態を、歩行者の検出結果の統合結果として出力し、上記ステップＳ１１０へ戻る。例えば、歩行者の検出結果の統合結果が、遠隔サーバ８０へ送信される。
【０１１０】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる。
【０１１１】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０１１２】
例えば、上記実施の形態では、カルマン予測を前提としたが、パーティクル予測を行っても、アンセンテッド変換やランダムサンプリングを用いたサンプリングベースの予測を行っても良い。その場合、図１２のような、より複雑な環境での予測も可能となる。ここで、図１２（ａ）は、複雑環境下での予測の例、（ｂ）は単なるカルマン予測の場合、（ｃ）は、パーティクルフィルタ等を用いた場合のイメージ図である。
【０１１３】
また、本実施形態において、歩行者を追跡する場合について説明したが、追跡対象物としては、他の移動物、例えば自転車、動物等を採用することも可能である。
【０１１４】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０１１５】
１     歩行者状態量推定システム
１０   局地サーバ
５０   基地局
６０   検出器
６２   検出器
７０   ネットワーク
８０   遠隔サーバ
１００ 通信部
１１０ センシング結果獲得部
１２０ 状態量記憶部
１３０ 境界検出部
１４０ 移動物可動範囲地図
１５０ 仮説予測部
１６０ 衝突判定状態量修正部
１７０ 境界侵入判定状態量修正部
１８０ 仮説整合部
１９０ 仮説消去部
２００ 仮説更新部
２１０ 障害物境界内側判定部
【図１】
2019144900.tif 000054
【図２】
2019144900.tif 000055
【図３】
2019144900.tif 000056
【図４】
2019144900.tif 000057
【図５】
2019144900.tif 000058
【図６】
2019144900.tif 000059
【図７】
2019144900.tif 000060
【図８】
2019144900.tif 000061
【図９】
2019144900.tif 000062
【図１０】
2019144900.tif 000063
【図１１】
2019144900.tif 000064
【図１２】
2019144900.tif 000065
【図１３】
2019144900.tif 000066
【図１４】
2019144900.tif 000067