(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】2019144900
(43)【公開日】20190829
(54)【発明の名称】状態推定装置及びプログラム
(51)【国際特許分類】
   G06T 7/277 20170101AFI20190802BHJP
【FI】
   !G06T7/277
【審査請求】未請求
【請求項の数】6
【出願形態】OL
【全頁数】20
(21)【出願番号】2018029151
(22)【出願日】20180221
(71)【出願人】
【識別番号】000003609
【氏名又は名称】株式会社豊田中央研究所
【住所又は居所】愛知県長久手市横道41番地の1
(74)【代理人】
【識別番号】100079049
【弁理士】
【氏名又は名称】中島 淳
(74)【代理人】
【識別番号】100084995
【弁理士】
【氏名又は名称】加藤 和詳
(72)【発明者】
【氏名】大濱 吉紘
【住所又は居所】愛知県長久手市横道41番地の1 株式会社豊田中央研究所内
【テーマコード(参考)】
5L096
【Fターム(参考)】
5L096AA06
5L096AA09
5L096BA04
5L096DA01
5L096GA51
5L096GA55
5L096HA05
(57)【要約】
【課題】センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができるようにする。
【解決手段】追跡している追跡対象物の各々について、状態量記憶部120に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報140とに基づいて、境界検出部130が、当該追跡対象物の周辺の可動範囲の境界を検出し、仮説予測部150が、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、仮説更新部200が、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新する。
【選択図】図2
【特許請求の範囲】
【請求項1】
センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部と、
前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部と、
前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部と、
を含む状態推定装置。
【請求項2】
前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界を検出すると共に、前記検出された前記可動範囲の境界から障害物が存在する範囲を検出し、
前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記障害物が存在する範囲と、前記可動範囲との境界に応じて前記仮説の予測結果を修正する
請求項1記載の状態推定装置。
【請求項3】
前記仮説更新部は、
前記仮説の予測結果の何れとも対応してない前記追跡対象物の検出結果が存在する場合に、前記追跡対象物の状態を示す仮説を新たに生成し、
前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記新たに生成された前記追跡対象物の仮説を消去するか、又は前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる存在確率を減ずる請求項2記載の状態推定装置。
【請求項4】
前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界として、路面上の標示境界を検出し、
前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置を中心とする所定の範囲と、前記検出された標示境界とが交差する場合に、前記所定の範囲と前記標示境界との交差状態に応じて前記仮説の予測結果を修正する請求項1〜請求項3の何れか1項記載の状態推定装置。
【請求項5】
前記センシング結果獲得部は、複数センサを用いて、追跡対象物を検出した結果を受け取る請求項1〜請求項4の何れか1項記載の状態推定装置。
【請求項6】
コンピュータを、
センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部、
前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部、
前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部、及び、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、状態推定装置及びプログラムに関する。
【背景技術】
【0002】
従来より、地図と可動物の運動状態から、各可動物の将来移動範囲をリスクポテンシャルで算出し、その重なりによって危険を報知する技術が知られている(特許文献1〜4)。
【0003】
また、地図と可動物の運動状態からのリスクポテンシャルを算出する際に、算出手段を動的に切り替える装置が知られている(特許文献5)。
【0004】
また、交通参加者の属性ごとの存在しやすさ(しにくさ)を表す地図を用いて、移動予測を行う装置が知られている(特許文献6)
【0005】
また、複数のカメラ映像を用いて、車両領域を表すテンプレート位置を抽出し、テンプレート位置情報に対して、多視点的処理と時系列処理を施し、複数のカメラで検出された車両の対応付けを行うことで、走行車両の運動軌道を高精度に計算することができる走行車両検出装置が知られている(特許文献7)。
【先行技術文献】
【特許文献】
【0006】
【特許文献1】特表2009−505260号公報
【特許文献2】特開2012−164159号公報
【特許文献3】国際公開第16/092591号
【特許文献4】特許第5673127号公報
【特許文献5】特許第5620147号公報
【特許文献6】国際公開第2012/033173号
【特許文献7】特開2001−357489号公報
【発明の概要】
【発明が解決しようとする課題】
【0007】
上記特許文献1〜4の技術は、可動物及び交通環境の局所的かつ相対的な位置関係が精緻に推定できることを前提としている。
【0008】
しかしながら、簡易な構成では実現することが困難であり、また、乱数を用いた粒子によるリスクポテンシャルの計算は、繰り返し試行ごとに異なる結果となり、同一場面で異なる移動予測及び危険判定となるため、品質保証も困難となる。
【0009】
また、特許文献5の技術は、局所的な可動物の将来予測範囲に占める歩車道の割合等の挙動の変化を促す要因によって、予測手段を切り替えて、リスクポテンシャルの計算コストを削減することを目指しているが、駐車場などの複雑な環境では、常に複雑な予測手段を選択することになるため、リスクポテンシャルの計算コストが削減できない。
【0010】
また、特許文献6の技術は、交通参加者ごとの存在しやすさ(しにくさ)を表す存在可能性マップという形式での局所地図を生成する必要があり、センシングの誤認識等により属性が不明である交通参加者を取り扱うために、合理的な局所地図を用意することが難しい。
【0011】
また、特許文献7の技術は、ビデオフレームが一定間隔で、複数のカメラ(センサ)間でフレーム同期とれることを前提としている。通信遅延やセンサの仕様で複数のセンサ間でフレーム同期(検出結果送信タイミングの同期)がとれない場合は、一定時間フレームを、結果を統合する装置上で蓄積し、データ補間等の同期処理を行う必要がある。さらにはセンサ数が不定の場合は、同期するセンサが不定ということであるため、同期処理が困難である。
【0012】
特許文献7の問題を解決するべく、複数のセンサ間でフレーム同期をしないアプローチが考えられる(図13)。しかしセンシング結果が確実かつ瞬時に装置に到来することを前提としているため、一時的な通信途絶や誤検出・未検出に対応できない問題がある。さらに、センシング結果から得られる観測量が、障害物の内側などとして明らかに誤っている場合にも、既存の状態量と対応づかない場合に、新規の状態量を生成する問題があった。
【0013】
上記の問題点を解決するべく、状態量を複数個のパーティクルで表現し、歩車道等の標示情報による可動範囲地図を全状態量の観測量としてセンシング結果の到来とは独立に状態量を修正し、また状態量の新規生成の際に反映させるアプローチが考えられる(図14)。
【0014】
しかし、この構成では状態量の可動範囲地図上での適合度(尤度)を評価する際に、原理的に複数のパーティクルで状態量を表現するか、なんらかの近似計算を行う必要があり、多数の状態量を取り扱う際に計算効率が悪化する問題があった。また、尤度の値を標示と交通参加者の属性の全組合せについて定義しなければならない。
【0015】
本発明は、上記問題点を解決するためになされたもので、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる状態推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0016】
上記の目的を達成するために、本発明の状態推定装置は、センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部と、前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部と、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部と、を含んで構成されている。
【0017】
また、本発明のプログラムは、コンピュータを、センサを用いて、追跡対象物を検出した結果を受け取るセンシング結果獲得部、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説を格納した状態量記憶部、前記追跡している追跡対象物の各々について、前記状態量記憶部に格納された仮説と、前記追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、前記追跡対象物の周辺の前記可動範囲の境界を検出する境界検出部、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記境界検出部により検出された前記境界に応じて前記仮説の予測結果を修正する仮説予測部、及び、前記追跡している追跡対象物の各々について、前記追跡対象物の前記仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する仮説更新部として機能させるためのプログラムである。
【0018】
本発明の状態推定装置及びプログラムによれば、センシング結果獲得部が、センサを用いて、追跡対象物を検出した結果を受け取り、状態量記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説を格納し、境界検出部が、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出する。
【0019】
そして、仮説予測部が、追跡している追跡対象物の各々について、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、境界検出部により検出された境界に応じて仮説の予測結果を修正し、仮説更新部が、追跡している追跡対象物の各々について、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新する。
【0020】
このように、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる。
【0021】
また、本発明の状態推定装置は、前記境界検出部は、前記追跡している追跡対象物の各々について、前記追跡対象物の周辺の前記可動範囲の境界を検出すると共に、前記検出された前記可動範囲の境界から障害物が存在する範囲を検出し、前記仮説予測部は、前記追跡している追跡対象物の各々について、前記センサを用いて検出タイミングでの、前記追跡対象物の仮説が示す状態を予測し、前記予測された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記障害物が存在する範囲と、前記可動範囲との境界に応じて前記仮説の予測結果を修正することができる。
【0022】
また、本発明の状態推定装置の前記仮説更新部は、前記仮説の予測結果の何れとも対応してない前記追跡対象物の検出結果が存在する場合に、前記追跡対象物の状態を示す仮説を新たに生成し、前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる位置が、前記障害物が存在する範囲内である場合に、前記新たに生成された前記追跡対象物の仮説を消去するか、又は前記新たに生成された前記追跡対象物の仮説が示す状態に含まれる存在確率を減ずるができる。
【0023】
また、本発明の状態推定装置の前記センシング結果獲得部は、複数センサを用いて、追跡対象物を検出した結果を受け取ることができる。
【発明の効果】
【0024】
以上説明したように、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる、という効果が得られる。
【図面の簡単な説明】
【0025】
【図1】本発明の実施の形態に係る歩行者状態量推定システムを示すブロック図である。
【図2】本発明の実施の形態に係る局地サーバを示すブロック図である。
【図3】本発明の実施の形態に係る衝突判定による状態量を修正する例を示すイメージ図である。
【図4】本発明の実施の形態に係る侵入判定による状態量を修正する例を示すイメージ図である。
【図5】本発明の実施の形態に係る複数の標示境界線の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図6】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図7】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図8】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図9】本発明の実施の形態に係る複数の歩行者の場合における侵入判定による状態量を修正する例を示すイメージ図である。
【図10】本発明の実施の形態に係る状態量を削除する場合のイメージ図である。
【図11】本発明の実施の形態に係る局地サーバにおける歩行者状態量推定処理ルーチンの内容を示すフローチャートである。
【図12】本発明をより複雑な環境に適用した場合の例を示すイメージ図である。
【図13】複数のセンサ間でフレーム同期をしないアプローチのイメージ図である。
【図14】センシング結果の到来とは独立に状態量を修正し、また状態量の新規生成の際に反映させるアプローチのイメージ図である。
【発明を実施するための形態】
【0026】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【0027】
<本発明の実施の形態の概要>
本発明の実施の形態は、複数センサから非同期に到達した情報から生成した観測量と、既に保持している状態量の移動予測との近さに応じて、整合処理(対応付けと状態量の修正)を行い、適切な対応付けがない場合には、観測量周辺に可動範囲を鑑みて状態量を生成する。
【0028】
このとき、移動物の常識的な状態量の移動予測を、障害物や標示境界(歩車道分離の白線等)付近では移動が抑制されるという近似によって簡易な幾何学計算のみで算出する。
【0029】
全ての状態量は存在確率を持ち、観測量の割り当てがない場合に減少し、また各標示境界との位置関係に応じて減少幅が変動し、存在確率が十分に小さくなった際には状態量は消去される。
【0030】
この結果、一時的な通信途絶や誤検出・未検出に対しても、ロバストな統合処理を実現することができる。
【0031】
本発明の実施の形態では、車両に搭載された歩行者の検出器やインフラセンサとしての歩行者の検出器の検出結果を統合して歩行者の状態量を推定する局地サーバに本発明を適用した場合を例に説明する。
【0032】
<歩行者状態量推定システムのシステム構成>
図1に示すように、第1の実施の形態に係る歩行者状態量推定システム1は、局地サーバ10と、基地局50と、複数の車両に搭載された複数の検出器60と、インフラセンサとしての検出器62とを備え、基地局50と局地サーバ10とは、インターネットなどのネットワーク70で接続されており、基地局50と検出器60、62とは、無線通信により接続されている。なお、局地サーバ10は、状態推定装置の一例である。
【0033】
検出器60、62は、カメラやレーダを用いて歩行者を随時検出し、検出する度に、検出結果を、基地局50を介して、局地サーバ10へ送信する。
【0034】
例えば、検出器60は、自車両の前方を撮像するカメラによって撮像された前方の道路画像から、スライディングウィンドウ毎に、画像特徴量(SIFT、FIND、HOGなど)を抽出し、スライディングウィンドウ毎の画像特徴量と、歩行者検出モデル(SVM、AdaBoost)とを用いて、歩行者を検出し、検出された歩行者位置を表す画像座標を求める。また、検出器60は、歩行者位置を表す画像座標を、3次元位置に変換する。この際に、検出された歩行者の高さに応じて誤差分散行列を求める。また、検出器60は、自車両に搭載されたGPSにより計測された自車両の絶対座標と、求められた3次元位置とに基づいて、歩行者の絶対的な3次元位置を求める。
【0035】
上記の歩行者の3次元位置及び誤差分散行列を、検出された歩行者毎に求め、車両Cの検出器60において検出した歩行者の3次元位置のセットY={yi,1,…,yi,m}と、観測誤差分散行列のセットR={ri,1,…,ri,m}とからなる観測情報O={Y,R}を、観測毎に、局地サーバ10へ送信する。
【0036】
複数の検出器60、62は、非同期に、歩行者を検出している。また、複数の検出器60は、各々異なる車両に搭載されているため、局地サーバ10へ検出結果を送信する検出器60、62の数は、不定となる。
【0037】
局地サーバ10は、CPUと、RAMと、後述する歩行者状態量推定処理ルーチンを実行するためのプログラムを記憶したROMとを備え、機能的には次に示すように構成されている。
【0038】
図2に示すように、局地サーバ10は、通信部100と、センシング結果獲得部110と、状態量記憶部120と、境界検出部130と、移動物可動範囲地図140と、仮説予測部150と、衝突判定状態量修正部160と、境界侵入判定状態量修正部170と、仮説整合部180と、仮説消去部190と、仮説更新部200と、障害物境界内側判定部210とを備えている。
【0039】
センシング結果獲得部110は、数不定の複数センサを用いて、歩行者を検出した結果を受け取る。
【0040】
具体的には、センシング結果獲得部110は、検出器60、62の何れかから送信された歩行者の座標のセット及び観測誤差分散行列のセットからなる観測を、通信部100により受信する毎に、歩行者の座標のセット及び観測誤差分散行列のセットからなる観測を取得する。
【0041】
状態量記憶部120は、追跡している歩行者の各々について、当該歩行者の状態を示す仮説を格納する。
【0042】
具体的には、状態量記憶部120は、仮説更新部200により更新された状態量(歩行者の位置、速度を含む)、及び最後に観測した時刻を記憶する。すなわち、歩行者の状態量のセットX={x,…,x}、状態量の分散共分散行列V={v,…,v}、および、最後に観測した時刻のセットT={t,…,t}を記憶している。
【0043】
なお、本実施の形態では、

を、信念空間上の仮説と呼び、その構成要素

を追跡器と呼ぶこととする。
【0044】
移動物可動範囲地図140は、歩行者の可動範囲が記録している。具体的には、障害物や標識表示による歩行者の可動範囲を示す地図であって、障害物か否かのラベル付きの線分集合

で構成される境界地図Uを格納している。ここで、uは障害物を示す線分の集合であり、

は障害物でない線分の集合である。
【0045】
境界地図Uは、例えば遠隔サーバ80から取得され、所定の周期で移動物可動範囲地図140の境界地図Uが更新される。例えば、遠隔サーバ80は、1時間の周期で境界地図Uを局地サーバ10に送信し、局地サーバ10は境界地図Uを受信する度に移動物可動範囲地図140の境界地図Uを更新する。
【0046】
境界検出部130は、追跡している歩行者の各々について、状態量記憶部120に格納された仮説と、境界地図Uとに基づいて、当該歩行者の周辺の可動範囲の境界を検出すると共に、検出された可動範囲の境界から障害物が存在する範囲と、歩行者の周辺の可動範囲の境界としての路面上の標示境界とを検出する。
【0047】
具体的には、境界検出部130は、追跡している歩行者の各々について、状態量記憶部120に記憶された当該歩行者の仮説と、境界地図Uとに基づいて、当該歩行者の仮説の位置の周辺に存在する障害物を示す線分や標示境界(歩車道分離の白線等)である線分を境界として境界地図Uから検出する。
【0048】
そして、検出した境界に含まれる線分のうち、障害物を示す線分の集合uに含まれる線分から、障害物が存在する範囲を検出する。また、検出した境界に含まれる線分のうち、障害物でない線分の集合

に含まれる線分を、路面上の標示境界とする。
【0049】
仮説予測部150は、追跡している歩行者の各々について、センサを用いて検出タイミングでの、当該歩行者の仮説が示す状態を予測する。
【0050】
具体的には、仮説予測部150は、現時刻に合わせて、複数の歩行者それぞれについて、カルマンフィルタの予測ステップにより、次の時刻の歩行者の状態量を予測することを繰り返し、現時刻における歩行者の状態量のセットX={x,…,x}、および、状態量の分散共分散行列V={v,…,v}を求める。例えば、等速予測などにより、次の時刻の歩行者の状態量を予測する。すなわち、仮説予測部150は、仮説

に対して、現時刻の状態に関する予測

を生成する。すなわち、全ての追跡器

について、時間幅

だけ予測を行う。
【0051】
衝突判定状態量修正部160は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、障害物が存在する範囲と、可動範囲との境界に応じて当該仮説の予測結果を修正する。
【0052】
具体的には、衝突判定状態量修正部160は、予測を行った追跡器

と、追跡器の予測前の状態量xと、予測後の状態量

とをそれぞれ境界地図U上に投影して得た点

及び

で定義されるベクトル

が、障害物を示す線分の集合u内のいずれかの線分uと交差するか否かを計算する。
【0053】
交差する場合(図3(a))、その交点をxu,n,kとする。このとき、図3(b)のように、

と修正して、予測後の状態量

を修正する。この場合、修正した予測後の歩行者の位置は、図3(c)のようになる。
【0054】
また、衝突判定状態量修正部160は、図3(d)のように、点xu,n,kを中心として

とuとのなす角ψの倍角だけ

を回転させたベクトルと、

との合成ベクトルの差分

をxの新たな速度成分として、カルマンフィルタの予測ステップを再度行い、予測後の状態量

を修正してもよい。この場合、修正した予測後の歩行者の位置は、図3(e)のようになる。
【0055】
境界侵入判定状態量修正部170は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置を中心とする所定の範囲と、検出された標示境界とが交差する場合に、所定の範囲と標示境界との交差状態に応じて当該仮説の予測結果を修正する。
【0056】
具体的には、境界侵入判定状態量修正部170は、衝突判定状態量修正部160により得られたベクトル

と、障害物でない線分の集合

内のいずれかの線分

が交差するか否かを計算する。なお、状態量に含まれる位置を中心とする一定確率の信頼楕円とは、歩行者の存在する信頼度が一定確率以上となる範囲を表す楕円である。
【0057】
交差する場合(図4(a))、境界侵入判定状態量修正部170は、状態量

と共分散

との固有値解析で得られる、状態量に含まれる位置を中心とする一定確率の信頼楕円の範囲に対して、検出された標示境界の線分

との交差判定を行う。
【0058】
交差判定は2次方程式の判別式で行うことができ、楕円中心から交差する標示境界の線分

への垂線のベクトルαの方向に、予測前の歩行者の速度ベクトルを、その交点と楕円中心のなす角θに比例した修正量αKだけ修正する。ここで、αは予め定めた量を持つベクトルであり、Kは角θに応じて定まる値である。Kは、例えば下記式(1)のように求めることができる。
【0059】
【数1】
【0060】
予測後の歩行者の位置が標示境界線に近い場合、歩行者は基本的に標示境界線を越えずに移動する場合が多いため、移動距離が小さくなるように予測前の歩行者の速度ベクトルを修正し、再度カルマンフィルタの予測ステップを行い、予測後の歩行者の位置を修正する。
【0061】
この場合、角θはπに近い値となるため、式(1)の値は、1に近い値となる。すなわち、修正量αKは、αに近い修正量となるため、歩行者の移動距離を小さくするように、予測前の歩行者の速度ベクトルを修正することができる。
【0062】
例えば、予測後の歩行者の位置が標示境界線を越えている場合(図4(b))、図4(c)のように予測前の歩行者の速度ベクトルを修正する。また、予測後の歩行者の位置が標示境界線を越えていない場合(図4(d))、図4(e)のように予測前の歩行者の速度ベクトルを修正する。
【0063】
一方、予測後の歩行者の位置が標示境界線から遠い場合、歩行者は標示境界線を越えて移動したと考えられるため、予測後の歩行者の位置を修正する必要がない。
【0064】
この場合、角θはπよりも十分小さな値となるため、式(1)の値は、0に近い値となる。すなわち、修正量αKは、0に近い修正量となるため、予測前の歩行者の速度ベクトルを修正しても、予測後の歩行者の位置をほとんど修正しないようにすることができる。
【0065】
また、予測前の歩行者の速度ベクトルでなく、予測後の歩行者の位置を修正する構成としてもよい。また、Kの値が所定の値以下の場合に、予測前の歩行者の速度ベクトルを修正しないようにする構成としてもよい。
【0066】
このように、交点と楕円中心のなす角によって予測後の歩行者の位置の修正を行うことで、極めて大きな速度で境界を越えようとする追跡器を除き、境界を越えないような非線形な予測を行うことができる。
【0067】
この方法は、図5のような対象となる境界線が複数の場合にも適用することができる。
【0068】
例えば、図5のように、2つの標示境界線に対して交点がある場合、交点の各々について、当該交点と楕円中心のなす角θに基づいて、楕円中心から交差する各標示境界の線分への垂線のベクトルの方向に修正量を求め、当該修正量を合成したベクトルを最終的な修正量として、修正することができる。なお、図5(a)は、予測前の状態量と標示境界線の配置、(b)は修正量、(c)は修正量の適用例を示すイメージ図である。
【0069】
また、追跡器同士に適用することで歩行者同士が回避し合うような予測を行うこともできる(図6参照)。この場合、境界を歩行者の位置を中心とする信頼楕円同士に基づいて計算する。
【0070】
例えば、異なる2つの歩行者同士が対面している場合、2つの歩行者の状態量を、信頼楕円の交点間線分で生成した境界線に対して、修正量の大きさを各仮説中心と信頼楕円の交点のなす角に比例させ、修正量の方向を、各仮説中心から境界線への垂線ベクトルの反対方向とする。ここで、図6(a)は、予測前の複数の歩行者の状態量の配置、(b)は修正量、(c)は修正量の適用例を示すイメージ図である。
【0071】
ここで、2つの歩行者同士が、速度の速い状況で対面している場合、図6の場合よりもやや速い程度であれば(図7)には、図6と同様に修正量を算出する。ここで、図7(a)は、予測前の複数の歩行者の状態量の配置、(b)は修正量、(c)は修正量の適用例を示すイメージ図である。
【0072】
一方、図6の場合よりもかなり速い(仮説同士がちょうど近接する)場合(図8)、互いに離れる方向に大きな修正量を算出する。衝突予測ではなく、追跡のための予測を目的としているため、基本的に全ての歩行者は衝突しないように移動している、という前提で予測を行うと仮定して良いので、すれ違うタイミングでの予測として、歩行者同士が大きく回避し合うと予測することができる。
【0073】
なお、図8(a)は、予測前の複数の歩行者の状態量の配置、(b)は修正量、(c)は修正量の適用例を示すイメージ図である。
【0074】
更に、図6の場合よりも極端に速く、仮説同士がすれ違ってしまうような場合(図9)、やや加速する方向に修正量を算出する。すなわち、すれ違うタイミングより後の時刻での予測として、回避し終えた後に歩行者が元の移動方向に維持・修正する動きに対応することができる。ここで、図9(a)は、予測前の複数の歩行者の状態量の配置、(b)は修正量、(c)は修正量の適用例を示すイメージ図である。
【0075】
仮説整合部180は、センシング結果獲得部110により最新の観測情報Oを受け取ると、移動物可動範囲地図140の境界地図Uを用いて、現時刻に対応して、仮説予測部150により追跡している歩行者の各々について予測された仮説と、当該最新の観測情報Oが表す歩行者の各々についての検出結果との対応付けを行う。
【0076】
具体的には、仮説整合部180は、予測された仮説と、境界とに基づいて、観測情報Oに対する、追跡器

の当てはまりのよさを意味する観測尤度

を計算する。
【0077】
そして、仮説整合部180は、観測尤度θi,mに基づいて、現時刻における歩行者の状態量のセットX={x,…,x}と、当該最新の観測情報Oにおいて検出した歩行者の3次元位置のセットY={yi,1,…,yi,m}との対応付けを行う。
【0078】
また、仮説整合部180は、仮説の予測結果の何れとも対応してない歩行者の検出結果が存在する場合に、歩行者の状態を示す仮説を新たに生成する。
【0079】
具体的には、仮説整合部180は、最新の観測情報Oの検出結果との対応付けの結果に基づいて、最新の観測情報Oのうち、追跡器と対応付かなかった歩行者の検出結果から、新たな状態量xを含む追跡器を生成する。
【0080】
なお、新たな状態量xを含む追跡器は、観測された歩行者の3次元位置yと同じ座標、誤差分散行列を持つこととする。
【0081】
仮説更新部200は、追跡している歩行者の各々について、当該歩行者の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。
【0082】
具体的には、センシング結果獲得部110最新の観測情報Oを受け取った場合に、仮説整合部180最新の観測情報Oと対応付けられた歩行者の各々について、対応する3次元位置yi,jを観測値として、その誤差分散行列ri,jと、予測ステップで得られた歩行者の状態量及び分散共分散行列とを用いて、カルマンフィルタのフィルタリングステップにより、状態量を更新すると共に、最後に観測した時刻を更新する。
【0083】
仮説更新部200は、観測情報Oに対する、追跡器

の観測尤度θi,mから、

となる追跡器を特定し、

が予め設定した閾値よりも大きい場合、状態量

と分散共分散行列

とを更新する。
【0084】
また、

が予め設定した閾値以下である場合、

として観測情報Oi,mを3次元空間に投影した位置に、一定の存在確率が設定された新規の追跡器を生成して、

に加える。
【0085】
また、仮説更新部200は、更新または新規作成された追跡器の最終更新時刻

を、現在時刻に設定し、存在確率を

等として、一定値だけ増加させる。
【0086】
また、割り当ての無かった追跡器の存在確率を

等として、一定値だけ減少させる。
【0087】
そして、仮説更新部200は、更新または新規作成された追跡器を、状態量記憶部120に格納する。この時、最後に観測した時刻として、現在時刻を格納する。
【0088】
障害物境界内側判定部210は、新たに生成された歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、新たに生成された当該歩行者の仮説を消去するか、又は新たに生成された歩行者の仮説が示す状態に含まれる存在確率を減ずる。
【0089】
具体的には、障害物境界内側判定部210は、仮説整合部180により新たに生成された追跡器の位置が、障害物が存在する範囲内である場合には、当該追跡器の存在確率を

等として、当該追跡器が削除されるか、又はすぐには存在確率が上昇しないように設定する。
【0090】
ここで、障害物が存在する範囲内であるか否かは、新たに生成した追跡器の状態量

を境界地図Uに直情から投影して得た点

と、境界地図U上の障害物である線分の集合uのうち閉路を形成するものであるuについて、閉路内に

が存在するか否かに基づいて判定する。
【0091】
例えば、閉路を全て3つの線分からなる三角形のみで構成するように境界地図Uが作成されていれば、ある閉路uc,j={uc,j,1,uc,j,2,uc,j,3}を構成する3点について、3つの外積

に関する下記式(2)の不等式が成立している場合に、

は閉路uc,jの内側に存在すると判定することができる。
【0092】
【数2】
【0093】
仮説消去部190は、最後に観測した時刻から一定時間以上経過している歩行者の状態量を含む追跡器を、状態量記憶部120から消去する(図10)。
【0094】
具体的には、仮説消去部190は、存在確率

が十分に小さい追跡器を除去する。
【0095】
局地サーバ10は、上記の一連の処理により更新された状態量のセットXを、歩行者の検出結果の統合結果として出力する。
【0096】
<歩行者状態量推定システム1の動作>
次に、第1の実施の形態に係る歩行者状態量推定システム1の動作について説明する。まず、複数の車両に搭載された複数の検出器60、及びインフラセンサとしての検出器62の各々によって、歩行者が逐次検出され、検出される毎に、検出結果が、基地局50を介して、局地サーバ10に送信されているときに、局地サーバ10において、図11に示す歩行者状態量推定処理ルーチンが実行される。
【0097】
ステップS100において、仮説更新部200は、下記式(3)のように仮説を空集合に初期化する。
【0098】
【数3】
【0099】
ステップS110において、センシング結果獲得部110は、センサを用いて、歩行者を検出した結果である最新の観測情報Oを受け取る。
【0100】
ステップS120において、仮説整合部180は、上記ステップS150で受信した最新の観測情報Oに含まれる歩行者の3次元位置を、センサ座標系から世界座標系に変換する。
【0101】
ステップS130において、境界検出部130は、追跡している歩行者の各々について、状態量記憶部120に格納された仮説と、境界地図Uとに基づいて、当該歩行者の周辺の可動範囲の境界を検出すると共に、検出された可動範囲の境界から障害物が存在する範囲と、歩行者の周辺の可動範囲の境界としての路面上の標示境界とを検出する。
【0102】
ステップS140において、仮説予測部150は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の仮説が示す状態を予測する。
【0103】
ステップS150において、衝突判定状態量修正部160は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、障害物が存在する範囲と、可動範囲との境界に応じて当該仮説の予測結果を修正する。
【0104】
ステップS160において、境界侵入判定状態量修正部170は、追跡している歩行者の各々について、予測された当該歩行者の仮説が示す状態に含まれる位置を中心とする所定の範囲と、検出された標示境界とが交差する場合に、所定の範囲と標示境界との交差状態に応じて当該仮説の予測結果を修正する。
【0105】
ステップS170において、仮説整合部180は、センシング結果獲得部110により最新の観測情報Oを受け取ると、移動物可動範囲地図140の境界地図Uを用いて、現時刻に対応して、仮説予測部150により追跡している歩行者の各々について予測された仮説と、当該最新の観測情報Oが表す歩行者の各々についての検出結果との対応付けを行う。
【0106】
ステップS180において、仮説更新部200は、追跡している歩行者の各々について、当該歩行者の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。また、仮説の予測結果の何れとも対応していない歩行者の検出結果が存在する場合に、歩行者の状態を示す仮説を新たに生成する。
【0107】
ステップS190において、障害物境界内側判定部210は、新たに生成された歩行者の仮説が示す状態に含まれる位置が、障害物が存在する範囲内である場合に、新たに生成された当該歩行者の仮説を消去するか、又は新たに生成された歩行者の仮説が示す状態に含まれる存在確率を減ずる。
【0108】
ステップS200において、仮説消去部190は、最後に観測した時刻から一定時間以上経過している歩行者の状態量を含む追跡器を、状態量記憶部120から消去する。
【0109】
そして、ステップS210において、更新された仮説が示す状態を、歩行者の検出結果の統合結果として出力し、上記ステップS110へ戻る。例えば、歩行者の検出結果の統合結果が、遠隔サーバ80へ送信される。
【0110】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、状態量記憶部に格納された仮説と、当該追跡対象物の可動範囲が記録されている移動物可動範囲情報とに基づいて、当該追跡対象物の周辺の可動範囲の境界を検出し、センサを用いて検出タイミングでの、当該追跡対象物の仮説が示す状態を予測し、検出された境界に応じて仮説の予測結果を修正し、当該追跡対象物の仮説を、仮説の予測結果と、追跡対象物の検出結果とに基づいて更新することにより、センサに一時的な通信途絶や誤検出・未検出が生じた場合でも、移動物の状態量を精度良く求めることができる。
【0111】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【0112】
例えば、上記実施の形態では、カルマン予測を前提としたが、パーティクル予測を行っても、アンセンテッド変換やランダムサンプリングを用いたサンプリングベースの予測を行っても良い。その場合、図12のような、より複雑な環境での予測も可能となる。ここで、図12(a)は、複雑環境下での予測の例、(b)は単なるカルマン予測の場合、(c)は、パーティクルフィルタ等を用いた場合のイメージ図である。
【0113】
また、本実施形態において、歩行者を追跡する場合について説明したが、追跡対象物としては、他の移動物、例えば自転車、動物等を採用することも可能である。
【0114】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【0115】
1 歩行者状態量推定システム
10 局地サーバ
50 基地局
60 検出器
62 検出器
70 ネットワーク
80 遠隔サーバ
100 通信部
110 センシング結果獲得部
120 状態量記憶部
130 境界検出部
140 移動物可動範囲地図
150 仮説予測部
160 衝突判定状態量修正部
170 境界侵入判定状態量修正部
180 仮説整合部
190 仮説消去部
200 仮説更新部
210 障害物境界内側判定部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】