JP7056842B2

JP7056842B2 - 状態推定装置及びプログラム

Info

Publication number: JP7056842B2
Application number: JP2018056620A
Authority: JP
Inventors: 吉紘大濱; 新高橋; 真大門; 宏祐北浦
Original assignee: Denso Corp; Toyota Central R&D Labs Inc
Current assignee: Denso Corp; Toyota Central R&D Labs Inc
Priority date: 2018-03-23
Filing date: 2018-03-23
Publication date: 2022-04-19
Anticipated expiration: 2038-03-23
Also published as: JP2019168953A

Description

本発明は、状態推定装置及びプログラムに関する。

従来より、予め危険シーンのセンサデータを蓄積しておき、現在のセンサデータと照合して危険を報知する技術がある（特許文献１、特許文献２）。また、地図及び通信で取得したセンシングから、ある地点での複数の移動物の予測到達時刻と交通状況に照らして危険を報知する技術がある（特許文献３、特許文献４）。

また、地図と移動物の運動状態から、各移動物の将来移動範囲をリスクポテンシャルで算出し、その重なりによって危険を報知する技術がある（特許文献５～８）。地図と移動物の運動状態からのリスクポテンシャルを算出する際に、算出手段を動的に切り替える技術がある（特許文献９）。

また、交通参加者の属性ごとの存在しやすさ（しにくさ）を表す地図を用いて、状態推定を行う技術がある（特許文献１０）。

特開２００１－３３１９００号公報特開２００８－００３７０７号公報特開２０１０－０２０６３７号公報特開２０１６－０２１１２５号公報特表２００９－５０５２６０号公報特開２０１２－１６４１５９号公報国際公開第１６／０９２５９１号特許第５６７３１２７号公報特許第５６２０１４７号公報国際公開第２０１２／０３３１７３号

上記特許文献１及び特許文献２の技術は、蓄積されたセンシングデータと、現在直面しているセンシングデータの照合によって危険報知が行われるため、現在直面している場面において、移動物のどのような挙動が危険に至るかを、時系列及び空間的に説明できず、また、蓄積されたセンシングデータに存在しない危険場面に対して報知することができない。

また、上記特許文献３及び特許文献４の技術は、ＧＰＳ等で取得した大局的な位置情報と、地図上の交差点等の位置情報から予測される到達時刻で危険性を判定し、交通状況及び各移動物の位置・速度・走路状態等のセンシングによって重みを付けて精度を向上させるため、交差点内の横断歩道上や複数車線のうちのいずれかといった、局所的な空間上での状態推定と危険判定は、簡易な構成では困難である。極めて精緻な位置推定と高精度の地図及びセンシングが必要となるためである。

また、上記特許文献５～８の技術は、移動物及び交通環境の局所的かつ相対的な位置関係が精緻に推定できることが前提であり、簡易な構成では困難であって、また、乱数を用いた粒子によるリスクポテンシャルの計算は、繰り返し試行ごとに異なる結果となり、同一場面で異なる状態推定及び危険判定となるため、品質保証が困難である。

また、上記特許文献９の技術は、局所的な移動物の将来予測範囲に占める歩車道の割合等の挙動の変化を促す要因によって、予測手段を切り替えて、リスクポテンシャルの計算コストを削減することを目指しているが、駐車場などの複雑な環境では、常に複雑な予測手段を選択することになるため、リスクポテンシャルの計算コストが削減できない。

また、上記特許文献１０の技術は、交通参加者ごとの存在しやすさや、存在しにくさを表す存在可能性マップという形式での局所地図を生成する必要があり、センシングの誤認識等により属性が不明である交通参加者を取り扱うために、合理的な局所地図を用意することが難しい。

したがって、従来技術では、複雑な交通環境での移動物の状態推定を、多様な可能性の想定を失うことなく、簡易な構成によって少ない計算時間で行うことができない、という問題があった。

本発明は、上記問題点を解決するためになされたもので、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる状態推定装置及びプログラムを提供することを目的とする。

また、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる状態推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために、第１の発明の状態推定装置は、センサを用いて、追跡対象物を検出する検出部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、を含んで構成されている。

第１の発明の状態推定装置によれば、検出部が、センサを用いて、追跡対象物を検出し、状態記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説であって、当該追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納し、仮説予測部が、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、当該追跡対象物の複数の仮説を予測する。

そして、重複仮説削除部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除し、状態更新部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説を、当該仮説の予測結果と、当該追跡対象物の検出結果とに基づいて更新する。

このように、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる。

また、第１の発明の状態推定装置の前記重複仮説削除部は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除することができる。

また、第１の発明の状態推定装置の前記検出部は、センサを用いて、追跡対象物と障害物とを検出し、前記重複仮説削除部は、前記追跡している追跡対象物の各々について、更に、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を削除することができる。
また、第１の発明の状態推定装置は、前記状態更新部により更新された前記複数の仮説に基づいて、前記追跡している追跡対象物同士の衝突可能性を判定する衝突可能性判定部を更に含むことができる。

また、第２の発明の状態推定装置は、センサを用いて、追跡対象物と障害物とを検出する検出部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、を含んで構成されている。

第２の発明の状態推定装置によれば、検出部が、センサを用いて、追跡対象物と障害物とを検出し、状態記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説であって、当該追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納し、仮説予測部が、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測する。

そして、近傍仮説修正部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正し、状態更新部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説を、当該仮説の修正結果と、当該追跡対象物の検出結果とに基づいて更新する。

このように、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度良く行うことができる。

また、第２の発明の状態推定装置は、前記追跡している追跡対象物の各々について、前記仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、前記仮説を修正するランドマーク修正部を更に含むことができる。

また、第２の発明の状態推定装置は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する重複仮説削除部を更に含むことができる。
また、第２の発明の状態推定装置は、前記状態更新部により更新された前記複数の仮説と、前記センサを備える移動物の運動を表すセンサ情報とに基づいて、前記追跡対象物と、前記移動物との衝突可能性を判定する衝突可能性判定部を更に含むことができる。

以上説明したように、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる、という効果が得られる。

また、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる、という効果が得られる。

本発明の第１の実施の形態における状態推定システムの概略構成を示すブロック図である。本発明の実施の形態に係る交通環境の例を示すイメージ図である。本発明の実施の形態に係る交通環境の例を示すイメージ図である。本発明の第１の実施の形態に係る仮説予測部がセンサの画角外や、歩行者が出現し得るランドマークにも仮説を生成する場合のイメージ図である。本発明の第１の実施の形態に係る仮説を予測した場合のイメージ図である。本発明の第１の実施の形態に係る仮説間のＫＬＤを示すイメージ図である。本発明の第１の実施の形態に係るＫＬＤが小さい方の仮説を示すイメージ図である。本発明の第１の実施の形態における重複仮説削除部が仮説を削除した後のイメージ図である。本発明の第１の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態における重複仮説削除処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態における状態推定システムの概略構成を示すブロック図である。本発明の第２の実施の形態に係るおけるユークリッド距離の最も近い２つの追跡器の例を示すイメージ図である。本発明の第２の実施の形態に係る仮説間のＫＬＤを示すイメージ図である。本発明の第２の実施の形態における各仮説の最小のＫＬＤを増大させる修正量を示すイメージ図である。本発明の第２の実施の形態における近傍仮説修正部により得られた修正量に基づいて仮説を修正した場合のイメージ図である。本発明の第２の実施の形態に係るおける追跡器と近いランドマークがある場合を示すイメージ図である。本発明の第２の実施の形態に係るおける仮説とランドマークとのＫＬＤを示すイメージ図である。本発明の第２の実施の形態における仮説とランドマークとのＫＬＤを減少させる修正量を示すイメージ図である。本発明の第２の実施の形態における近傍仮説修正部により得られた修正量と、ランドマーク修正部によって得られた修正量を、合成した修正量を示すイメージ図である。本発明の第２の実施の形態における修正した仮説の状態量を更新した場合を示すフローチャートである。本発明の第２の実施の形態における修正した仮説の共分散を更新した場合を示すフローチャートである。本発明の第２の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態における近傍仮説修正量算出処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態におけるランドマーク修正量算出処理ルーチンの内容を示すフローチャートである。本発明の第３の実施の形態における状態推定システムの概略構成を示すブロック図である。本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。本発明の第３の実施の形態における円内に閾値以上の交点を含む仮説を削除した例を示すイメージ図である。本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。本発明の第３の実施の形態における最も近い仮説へのベクトルの反対方向のベクトルを示すイメージ図である。本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。本発明の第３の実施の形態におけるランドマークから最も近い仮説のランドマークへの方向のベクトルを示すイメージ図である。本発明の第３の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の第１の実施の形態に係る状態推定システムの概要＞
本発明の実施形態では、交通環境上の移動物（車両、歩行者等）を追跡対象物とし、大きさを持つ仮説群で構成した追跡器として表現する。

そして、追跡器内の他の仮説や障害物との重複があれば仮説を除去する。

大きさを持った少数の仮説で追跡器を構成し、解析的な相互作用の計算のみで、乱数を排除し、かつ、網羅性のある状態推定の実現を行う。

追跡器を構成する仮説に大きさを設定し、寄与度の低い仮説を削減することで、計算量を削減することができる。ここで、仮説の大きさは、任意の大きさを定義することができる。本実施形態では、仮説の大きさが、カルマンフィルタにおける予測ステップでの共分散行列で表される場合について説明する。

＜本発明の第１の実施の形態に係る状態推定システムの構成＞
図１に示すように、本発明の第１の実施の形態に係る状態推定システム１０は、センシング装置１００と、状態推定装置２００とを備えて構成される。本実施形態では、交通環境が駐車場であり、追跡対象物が車両と歩行者である場合を例に説明する（図２）。

センシング装置１００は、移動物の位置・速度の推定に必要な情報を計測する。

具体的には、センシング装置１００は、カメラ、レーザーレーダ、電波受信機等のセンサを用いて、車両及び歩行者の位置・速度の推定に有益な情報を計測する。センシング装置１００は、多数台、多数の種類が混在しても良い。

センシング装置１００は、車両及び歩行者を計測できる場所であれば、任意の場所に設置されることができる。

例えば、センシング装置１００は、天井等の移動しない場所に設置されることができる（図２）。この場合、センシング装置１００は、設置位置を適切に選定することで、カメラの撮影範囲やレーザー等が遮蔽される領域を少なくすることができる。

また、例えば、センシング装置１００は、移動物、例えば車両上に設置されることもできる（図３）。以下、本実施形態では、図２の場合を例に説明する。

そして、センシング装置１００は、計測したセンサ情報を、状態推定装置２００の入力部２０１に渡す。ここで、センシング装置１００は、状態推定装置２００と直接接続されていることを要せず、状態推定装置２００に、インターネット等の通信網を介して接続されていてもよい。

状態推定装置２００は、ＣＰＵと、ＲＡＭと、後述する状態量推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

図１に示すように、状態推定装置２００は、入力部２０１と、障害物特定部２０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部２０６と、重複仮説削除部２０７と、静止障害物地図ＤＢ２０８と、状態更新部２０９と、衝突可能性判定部２１０と、出力部２１１とを備えて構成される。

入力部２０１は、センシング装置１００から、センサ情報の入力を受け付ける。

そして、入力部２０１は、受け付けたセンサ情報を、障害物特定部２０２及び検出部２０４に渡す。

障害物特定部２０２は、センサ情報に基づいて、障害物領域を特定する。

具体的には、障害物特定部２０２は、予め移動物が存在しない状態においてレーザーレーダによるセンサ情報を用いて点群地図を作成し、障害物領域を特定する。

そして、障害物特定部２０２は、特定した障害物領域の情報を遮蔽領域検出部２０３に渡す。

遮蔽領域検出部２０３は、障害物領域の情報に基づいて、遮蔽領域を検出する。

具体的には、遮蔽領域検出部２０３は、例えばレーザーレーダにより計測された点群地図を、障害物特定部２０２で得られた障害物領域との差分により移動物を特定し、幾何学計算によって遮蔽領域を検出する。

そして、遮蔽領域検出部２０３は、検出した遮蔽領域の情報を、仮説予測部２０６に渡す。

検出部２０４は、センサ情報を用いて、観測時刻における歩行者及び車両を検出する。

具体的には、検出部２０４は、センサ情報から、歩行者を検出し、当該歩行者を追跡の対象である追跡対象物として、当該歩行者の位置及び速度を検出すると共に、車両を検出し、当該車両を追跡の対象である追跡対象物として、当該車両の位置および速度を検出する。

例えば、検出部２０４は、カメラ画像を用いた物体認識技術により画像中の人の矩形位置を検出すると共に、車両の矩形位置を検出する。

そして、検出部２０４は、レーザーレーダにより計測された点群をカメラ画像に投影して、矩形内の点群から距離を推定することで、歩行者の位置及び車両の位置を検出する。また、検出部２０４は、前時刻における、矩形内の点群と、現時刻における、矩形内の点群との比較により、歩行者の速度及び移動方向も推定すると共に、車両の速度及び移動方向も推定する。

そして、検出部２０４は、検出した全ての歩行者を追跡している歩行者として、追跡している歩行者の状態（位置、速度、観測時刻）を、仮説予測部２０６に渡すと共に、検出した全ての車両を追跡している車両として、追跡している車両の状態（位置、速度、観測時刻）を、仮説予測部２０６に渡す。

状態記憶部２０５は、追跡している歩行者の各々について、当該歩行者の状態を示す仮説を格納すると共に、追跡している車両の各々について、当該車両の状態を示す仮説を格納する。ここで、歩行者の仮説は、歩行者の存在である正規分布を仮定した仮説（位置、速度、共分散）であり、追跡器に含まれる。また、車両の仮説は、車両の存在である正規分布を仮定した仮説（位置、速度、共分散）であり、追跡器に含まれる。また、仮説にはＩＤや属性を含めてもよい。

具体的には、状態記憶部２０５は、状態更新部２０９により更新された追跡している歩行者の各々について、当該歩行者の状態（位置、速度を含む）、及び最後に観測した時刻を格納すると共に、状態更新部２０９により更新された追跡している車両の各々について、当該車両の状態（位置、速度を含む）、及び最後に観測した時刻を格納する。

仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を予測する。

具体的には、仮説予測部２０６は、全ての追跡器内の仮説に対して、カルマンフィルタの予測ステップ（線形予測／共分散拡大）を設定されたステップ数だけ繰り返すことにより、当該追跡器の将来の移動可能範囲を予測する。

ここでは、追跡器ａについて仮説群

が生成されたとものとして、１ステップカルマン予測を用いた場合について説明する。仮説予測部２０６は、仮説ｘ_０ａｉ毎に次の時刻の状態を示す仮説群

を予測する。

ｉ番目の仮説ｘ_０ａｉは、下記式（１）のように表すことができる。

ここで、（ｘ_０ｐｉ）_ｘ、（ｘ_０ｐｉ）_ｙは、それぞれ当該仮説のｘ、ｙ方向の位置を、（ｘ_０ｖｉ）_ｘ、（ｘ_０ｖｉ）_ｙは、それぞれ当該仮説のｘ、ｙ方向の速度を表す。

そして、下記式（２）及び（３）を用いて、次の時刻の状態を予測する。図５に、追跡器ａについての予測結果のイメージを示す。

ここで、

は、次の時間への時間幅であり、σ_ｐｘ、σ_ｐｙ、σ_ｖｘ、σ_ｖｙは共分散である。

そして、仮説予測部２０６は、追跡している歩行者の各々についての予測結果を、重複仮説削除部２０７に渡す。

また、仮説予測部２０６は、追跡している車両の各々について、歩行者と同様に、センサを用いたタイミングでの、当該車両の複数の仮説を予測する。

また、仮説予測部２０６は、遮蔽領域の情報を用いて、図４に示すように、センサの画角外や、車両や歩行者が出現し得るランドマークに、車両や歩行者の仮説を新たに生成してもよい。ランドマークから歩行者や車両が交通環境である駐車場に入場する場面が想定されるからである。この場合、センシング装置１００が、駐車場内のランドマークの変化（例えば、エレベータの開閉等）を計測する構成としてもよい。

重複仮説削除部２０７は、追跡している追跡対象物（歩行者又は車両）の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物が存在する範囲との重複度合いに応じて、当該仮説を削除する。

具体的には、重複仮説削除部２０７は、追跡器が２つ以上の仮説で構成されている場合に、以下のように、仮説数の削減を図る。

まず、重複仮説削除部２０７は、予め作成した静止障害物地図ＤＢ２０８の静止障害物地図を用いて、障害物内に仮説の状態が含まれている場合に、当該仮説を削除する。

次に、各予測ステップにおいて各仮説の組み合わせの各々についてＫｕｌｌｂａｃｋ－Ｌｅｉｂｌｅｒ距離（ＫＬＤ）を計算し、さらに仮説間のＫＬＤの平均であるＪｅｎｓｅｎ－Ｓｈａｎｎｏｎ距離（ＪＳＤ）を算出する。

例えば、上記の１ステップカルマンフィルタによる予測結果を用いた場合、ｉ番目の仮説ｘ_０ａｉと、ｊ番目の仮説ｘ_０ａｊとのＫＬＤは、下記式（４）を用いて求める（図６）。

次に、重複仮説削除部２０７は、仮説間のＫＬＤの平均を、下記式（５）を用いて求める。

そして、重複仮説削除部２０７は、ＪＳＤの小さい組み合わせから順に予め設定した値ε以下かどうかを評価し（下記式（６））、設定値ε以下の場合にはＪＳＤを構成するＫＬＤが小さい方の仮説を削除する（図７、図８）。

削除した仮説ｘ_０ａｋに関する評価中のＪＳＤの組み合わせ（ＪＳＤ_ｋｉ、ＪＳＤ_ｉｋ）も評価の対象外とする。なお、ＫＬＤが大きい方の仮説を削除する構成としてもよい。

また、予測された仮説と、特定された障害物領域との組み合わせの各々について、当該仮説の予測結果が持つ範囲と、障害物領域との重複度が閾値以上である場合には、当該仮説を削除する。

そして、重複仮説削除部２０７は、追跡している追跡対象物の各々について、仮説の削除処理を行った後の仮説予測部２０６による予測結果を、状態更新部２０９に渡す。

静止障害物地図ＤＢ２０８は、移動物が存在する地図上の静止している障害物の情報を含む地図である静止障害物地図を格納する。

状態更新部２０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。

また、状態更新部２０９は、追跡している車両の各々について、当該車両の複数の仮説を、仮説の予測結果と、当該車両の検出結果とに基づいて更新する。

具体的には、状態更新部２０９は、重複仮説削除部２０７により仮説の削除処理を行った後の仮説予測部２０６による予測結果と、歩行者の検出結果とに基づいて、追跡器と歩行者の検出結果とを対応付け、追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す歩行者の複数の仮説を更新する。

そして、状態更新部２０９は、追跡している追跡対象物（歩行者又は車両）の各々について、更新した当該追跡対象物の複数の仮説を、状態記憶部２０５及び衝突可能性判定部２１０に渡す。

また、状態更新部２０９は、何れの追跡器とも対応付けられなかった歩行者の検出結果に基づいて、歩行者の複数の仮説を生成し、新たな歩行者の追跡器を生成する。状態更新部２０９は、何れの追跡器とも対応付けられなかった車両の検出結果に基づいて、車両の複数の仮説を生成し、新たな車両の追跡器を生成する。

また、状態更新部３０９は、何れの歩行者の検出結果とも対応付けられなかった歩行者の追跡器のうち、観測時刻から一定時間以上経過している追跡器を削除する。状態更新部３０９は、何れの車両の検出結果とも対応付けられなかった車両の追跡器のうち、観測した時刻から一定時間以上経過している追跡器を状態記憶部２０５から削除する。

衝突可能性判定部２１０は、追跡している歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説と、更新された車両の複数の仮説とに基づいて、当該歩行者と、車両との衝突可能性を判定する。

具体的には、衝突可能性判定部２１０は、追跡器が示す歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説の位置及び速度と、車両の複数の仮説の位置及び速度とから、当該歩行者と車両が衝突する可能性を算出し、算出した可能性が閾値以上であれば、衝突する可能性があると判定する。

なお、衝突可能性判定部２１０は、同様に、追跡している車両の各々について、状態更新部２０９により更新された当該車両の複数の仮説と、更新された他の車両の複数の仮説とに基づいて、当該車両と、他の車両との衝突可能性を判定する。

そして、衝突可能性判定部２１０は、判定結果を出力部２１１に渡す。

出力部２１１は、判定結果を出力する。

＜本発明の第１の実施の形態に係る状態推定装置の作用＞
次に、図９を参照して、本実施形態の状態推定装置２００の状態推定処理ルーチンについて説明する。

まず、ステップＳ１００において、入力部２０１は、センシング装置１００から、センサ情報の入力を受け付ける。

ステップＳ１１０において、障害物特定部２０２は、上記ステップＳ１００により受け付けたセンサ情報に基づいて、障害物領域を特定する。

ステップＳ１２０において、遮蔽領域検出部２０３は、上記ステップＳ１１０により特定した障害物領域の情報に基づいて、遮蔽領域を検出する。

ステップＳ１３０において、検出部２０４は、上記ステップＳ１００により受け付けたセンサ情報を用いて、歩行者及び車両を検出する。

ステップＳ１４０において、仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を生成すると共に、追跡している車両の各々について、センサを用いた検出タイミングでの、当該車両の複数の仮説を生成する。

ステップＳ１５０において、仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を予測すると共に、追跡している車両の各々について、センサを用いた検出タイミングでの、当該車両の複数の仮説を予測する。

ステップＳ１６０において、重複仮説削除部２０７は、追跡している追跡対象物（歩行者、車両）の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除する。また、重複仮説削除部２０７は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、障害物が存在する範囲との重複度合いに応じて、当該仮説を削除する。

ステップＳ１７０において、状態更新部２０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、歩行者の検出結果とに基づいて更新する。また、状態更新部２０９は、追跡している車両の各々について、当該車両の複数の仮説を、仮説の予測結果と、車両の検出結果とに基づいて更新する。

ステップＳ１８０において、状態記憶部２０５は、追跡している追跡対象物（歩行者、車両）の各々について、当該追跡対象物の状態を示す仮説を複数格納する。

ステップＳ１９０において、衝突可能性判定部２１０は、追跡している歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説と、更新された車両の複数の仮説とに基づいて、当該歩行者と、車両との衝突可能性を判定する。また、衝突可能性判定部２１０は、追跡している車両の各々について、状態更新部２０９により更新された当該車両の複数の仮説と、更新された他の車両の複数の仮説とに基づいて、当該車両と、他の車両との衝突可能性を判定する。

ステップＳ２００において、出力部２１１は、判定結果を出力する。

また、上記ステップＳ１６０は、図１０に示す重複仮説削除処理ルーチンによって実現される。

ステップＳ１６１において、重複仮説削除部２０７は、追跡している歩行者のうち、１番目の歩行者を選択する。

ステップＳ１６２において、重複仮説削除部２０７は、選択されている歩行者の複数の仮説と、予め作成した静止障害物地図ＤＢ２０８の静止障害物地図を用いて、複数の仮説と障害物との重複度を計算する。

ステップＳ１６３において、重複仮説削除部２０７は、障害物と重複する仮説があるか否かを判定する。

重複する仮説が無い場合（ステップＳ１６３のＮＯ）、ステップＳ１６５に進む。

一方、重複する仮説がある場合（ステップＳ１６３のＹＥＳ）、ステップＳ１６４において、重複仮説削除部２０７は、重複する仮説を削除する。

ステップＳ１６５において、重複仮説削除部２０７は、複数の仮説の各々について、当該仮説と他の仮説との、仮説同士の重複度を計算する。また、重複仮説削除部２０７は、複数の仮説の各々について、当該仮説が持つ範囲と障害物領域との重複度を計算する。

ステップＳ１６６において、重複仮説削除部２０７は、計算した重複度に基づいて、重複する仮説があるか否かを判定する。

重複する仮説が無い場合（ステップＳ１６６のＮＯ），ステップＳ１６８に進む。

一方、仮説同士で重複する仮説がある場合（ステップＳ１６６のＹＥＳ）、ステップＳ１６７において、仮説同士で重複する仮説の一方を削除する。また、障害物領域と重複する仮説がある場合（ステップＳ１６６のＹＥＳ）、ステップＳ１６７において、障害物領域と重複する仮説を削除する。

ステップＳ１６８において、重複仮説削除部２０７は、全ての歩行者について処理を行ったか否かを判定する。

全ての歩行者について処理を行っていない場合（ステップＳ１６８のＮＯ）、ステップＳ１６９において、次の歩行者を選択して、ステップＳ１６２～Ｓ１６８の処理を繰り返す。

一方、全ての歩行者について処理を行った場合（ステップＳ１６８のＹＥＳ）、リターンする。

また、追跡している車両についても、上記図１０に示す重複仮説削除処理ルーチンと同様の処理ルーチンを実行する。

以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除し、当該追跡対象物の複数の仮説を、当該仮説の予測結果と、当該追跡対象物の検出結果とに基づいて更新することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる。

＜本発明の第２の実施の形態に係る状態推定システムの概要＞
第１の実施形態では、仮説を削除する構成としたが、第２の実施の形態では、他の追跡器との関係や、ランドマーク地図との組み合わせによって多様な状態推定を生成する。すなわち、センサにより得られたセンシング結果から推定された仮説から、将来の状態推定を求め、状態推定に対して、他の追跡器との距離に基づいた修正や、ランドマークへの誘因を考慮した修正を行う。

また、予め設定した地図により、仮説生成や状態推定を限定することで、より効果を高めることができる。

＜本発明の第２の実施の形態に係る状態推定システムの構成＞
本発明の第２の実施の形態に係る状態推定システム２０の構成について説明する。なお、第１の実施の形態に係る状態推定システム１０と同様の構成については、同一の符号を付して詳細な説明は省略する。

図１１に示すように、本発明の実施の形態に係る状態推定システム２０は、センシング装置１００と、状態推定装置３００とを備えて構成される。本実施形態では、交通環境が駐車場であり、追跡対象物が歩行者及び車両である場合を例に説明する（図３）。また、センシング装置１００が、車両上に設置される場合を例に説明する（図３）。

具体的には、センシング装置１００は、カメラ、レーザーレーダ、電波受信機等のセンサを用いて、車両及び歩行者の位置・速度の推定に有益な情報を計測する。また、センシング装置１００は、センサにより、自車両の運動を表わすセンサ情報を計測する。

そして、センシング装置１００は、計測したセンサ情報を、状態推定装置２００の入力部２０１に渡す。

図１１に示すように、状態推定装置３００は、入力部２０１と、障害物特定部３０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部３０６と、予定経路ＤＢ３１２と、近傍仮説修正部３１３と、ランドマーク修正部３１４と、ランドマーク地図ＤＢ３１５と、状態更新部３０９と、衝突可能性判定部３１０と、出力部２１１とを備えて構成される。

障害物特定部３０２は、センサ情報に基づいて、障害物領域を特定する。

具体的には、障害物特定部３０２は、センシング装置１００が車両に設置されている場合には、当該車両の運動推定結果を用いて占有格子地図に対する投票を行うことにより、障害物領域を特定する。

そして、障害物特定部３０２は、特定した障害物領域の情報を遮蔽領域検出部２０３及び近傍仮説修正部３１３に渡す。

具体的には、遮蔽領域検出部２０３は、センシング装置１００が車両に設置されている場合には、レーザーレーダにより計測された点群を投票することにより得られる占有格子地図に、カメラ画像による歩行者や車両の検出結果を投影し、移動物による遮蔽と、静止物による遮蔽を分離することにより、遮蔽領域を検出する。

仮説予測部３０６は、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、当該追跡対象物の複数の仮説を予測する。

具体的には、仮説予測部３０６は、全ての追跡器内の仮説に対して、カルマンフィルタの予測ステップ（線形予測／共分散拡大）を設定されたステップ数だけ繰り返すことにより、当該追跡器の将来の移動可能範囲を予測する。このとき、車両の追跡器については、予定経路ＤＢ３１２に格納された当該車両の移動する経路であって、検出結果と同じ座標系で定義されたものである予定経路の範囲に限定して、車両の複数の仮説を予測する。

そして、仮説予測部３０６は、追跡している追跡対象物の各々についての予測結果を、状態更新部３０９、近傍仮説修正部３１３、及びランドマーク修正部３１４に渡す。また、仮説予測部３０６は、遮蔽領域検出部２０３が検出した遮蔽領域を、近傍仮説修正部３１３に渡す。

予定経路ＤＢ３１２は、予定経路を格納している。

近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該追跡対象物の仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該追跡対象物の仮説の修正量を求める。

具体的には、まず、近傍仮説修正部３１３は、各追跡器同士、及び各追跡器と障害物とのユークリッド距離を算出し、追跡器の各々について、当該追跡器と最も距離が近い他の追跡器又は障害物との組を抽出する。

例えば、最も近い２つの追跡器の組（追跡器ａと追跡器ｂ）が抽出されたとする（図１２）。

次に、近傍仮説修正部３１３は、抽出した組において、追跡器ａの複数の仮説と、追跡器ｂの複数の仮説との組み合わせの各々についてＫＬＤを計算し、下記式（７）及び（８）を用いて、最もＫＬＤが短い仮説同士を求める（図１３）。

ここで、ｉ^＊は、追跡器ａの仮説のうち、最も追跡器ｂに近い仮説のＩＤであり、ｊ^＊は、追跡器ｂの仮説のうち、最も追跡器ａに近い仮説のＩＤである。

そして、近傍仮説修正部３１３は、最小のＫＬＤを増大させる勾配方向の、すなわち、異なる追跡器内の仮説同士が互いに回避するような各仮説の修正量を求める。近傍仮説修正部３１３は仮説ｘ_ａｉ ^＊と仮説ｘ_ｂｊ ^＊とのＫＬＤであるＫＬＤ_ａｉ ^＊ _ｂｊ ^＊及びＫＬＤ_ｂｊ ^＊ _ａｉ ^＊が大きくなるように、下記式（９）及び式（１０）を用いて修正する（図１４）。

ここで、

は、仮説ｘ_ａｉの修正量であり、

は、仮説ｘ_ｂｊの修正量である。

なお、当該修正量を用いて仮説を修正すると、図１５に示すように、追跡器ａと追跡器ｂの仮説が回避するように、仮説が修正されることになる。

また、近傍仮説修正部３１３は、最も距離が近い追跡器と障害物との組を抽出した場合にも、上記と同様に修正量を求める。

具体的には、追跡器の複数の仮説の各々について、障害物とのＫＬＤを計算し、最もＫＬＤが短い仮説を求め、最小のＫＬＤを増大させる勾配方向の、すなわち、当該追跡器の複数の仮説が回避するような各仮説の修正量を求める。

そして、近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。

ランドマーク地図ＤＢ３１５は、歩行者や車両を誘引し得るようなランドマークの交通環境上の位置を格納する。例えば、図３において、歩行者出入口は歩行者を誘引し得るようなランドマークであり、車両出入口は車両を誘引し得るようなランドマークである。

ランドマーク修正部３１４は、追跡している歩行者の各々について、当該歩行者の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該歩行者の仮説を修正する。

具体的には、ランドマーク修正部３１４は、ランドマーク地図ＤＢ３１５に格納された歩行者についてのランドマークの位置により、追跡している歩行者の各々について、当該歩行者の仮説を修正する修正量を求める。

まず、ランドマーク修正部３１４は、近傍仮説修正部３１３と同様に、追跡器とランドマークとの距離が最も短いランドマークを選択する。なお、追跡器とランドマークとの距離が閾値以下のランドマークのうち、最も短いランドマークを選択する構成としてもよい。

例えば、ランドマークが歩行者出入口の場合（図１６）に、ランドマーク修正部３１４は、追跡器ａとランドマークとの距離が閾値以下のランドマークを選択する。なお、閾値以下のランドマークが複数ある場合には、一番距離が近いランドマークを選んで修正する。

次に、ランドマーク修正部３１４は、追跡器ａの各仮説を基準として、選択したランドマークｌとのＫＬＤを計算し、下記式（１１）を用いてランドマークｌとのＫＬＤが最も小さい仮説ｘ_ａｉ ^＊を求める（図１７）。

ここで、ｉ^＊は、追跡器ａの仮説のうち、最もランドマークｌに近い仮説のＩＤである。

そして、ランドマーク修正部３１４は、最小のＫＬＤを減少させる勾配方向の修正量を求める。すなわち、仮説ｘ_ａｉ ^＊とランドマークｌとのＫＬＤであるＫＬＤ_ｌｊ ^＊ _ａｉ ^＊が小さくなるように、下記式（１２）を用いて仮説ｘ_ａｉ ^＊の修正量を求める（図１８）。

ここで、

は、仮説ｘ_ａｉの修正量である。

他の追跡器、例えば追跡器ｂの各仮説についても、下記式（１３）及び（１４）を用いて同様に修正する。

また、ランドマーク修正部３１４は、追跡している車両の各々について、当該車両の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該車両の仮説を修正する。この場合、ランドマーク地図ＤＢ３１５に格納された車両についてのランドマークの位置を用いて、歩行者の場合と同様に仮説を修正する。

そして、ランドマーク修正部３１４は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。

状態更新部３０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、仮説の修正量と、歩行者の検出結果とに基づいて更新する。

具体的には、状態更新部３０９は、歩行者の追跡器の各々についての、近傍仮説修正部３１３により得られた修正量

と、ランドマーク修正部３１４によって得られた修正量

とを、仮説予測部３０６による当該追跡器の各仮説の予測結果に適用した結果と、歩行者の検出結果とに基づいて、追跡器と歩行者の検出結果とを対応付け、追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す歩行者の複数の仮説を更新する。

まず、状態更新部３０９は、仮説毎に、近傍仮説修正部３１３により得られた修正量

と、ランドマーク修正部３１４によって得られた修正量

とを、合成した仮説ｘ_ａｉの修正量

を、下記式（１５）を用いて計算する（図１９）。

なお、修正量を加算合成する場合に、重みηを付けてもよい。仮説毎に重みηに異なる値を設定してもよいし、加算合成する際に重みηを設けてもよい。

次に、状態更新部３０９は、仮説毎に、仮説予測部３０６により予測された仮説ｘ_ａｉの状態量と共分散を、修正量

を用いて修正する（下記式（１６）～（１８）、図２０、２１）。

次に、状態更新部３０９は、修正後の歩行者の追跡器の仮説と、歩行者の検出結果とに基づいて、歩行者の追跡器と歩行者の検出結果とを対応付け、歩行者の追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新する。

同様に、状態更新部３０９は、車両の追跡器の各々についての、近傍仮説修正部３１３により得られた修正量を、仮説予測部３０６による当該追跡器の各仮説の予測結果に適用した結果と、車両の検出結果とに基づいて、車両の追跡器と車両の検出結果とを対応付け、追跡器の各々について、対応付けされた車両の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す車両の複数の仮説を更新する。

そして、状態更新部３０９は、追跡している追跡対象物の各々について、更新した当該追跡対象物の複数の仮説を、状態記憶部２０５及び衝突可能性判定部２１０に渡す。

また、状態更新部３０９は、何れの追跡器とも対応付けられなかった歩行者の検出結果に基づいて、歩行者の複数の仮説を生成し、新たな歩行者の追跡器を生成する。状態更新部３０９は、何れの追跡器とも対応付けられなかった車両の検出結果に基づいて、車両の複数の仮説を生成し、新たな車両の追跡器を生成する。

また、状態更新部３０９は、何れの歩行者の検出結果とも対応付けられなかった歩行者の追跡器のうち、最後に観測した時刻から一定時間以上経過している追跡器を削除する。状態更新部３０９は、何れの車両の検出結果とも対応付けられなかった車両の追跡器のうち、最後に観測した時刻から一定時間以上経過している追跡器を状態記憶部２０５から削除する。

衝突可能性判定部３１０は、追跡している歩行者の各々について、状態更新部３０９により更新された当該歩行者の複数の仮説と、自車両の運動を表すセンサ情報とに基づいて、自車両と、当該歩行者との衝突可能性を判定する。

具体的には、衝突可能性判定部３１０は、追跡している歩行者の各々について、状態更新部３０９により更新された当該歩行者の複数の仮説の位置及び速度と、自車両の位置及び速度とから、当該歩行者と自車両とが衝突する可能性を算出し、算出した可能性が閾値以上であれば、衝突する可能性があると判定する。

また、衝突可能性判定部３１０は、追跡している車両の各々について、歩行者と同様に、状態更新部３０９により更新された当該車両の複数の仮説と、自車両の運動を表すセンサ情報とに基づいて、自車両と、当該車両との衝突可能性を判定する。

そして、衝突可能性判定部３１０は、判定結果を出力部２１１に渡す。

＜本発明の第２の実施の形態に係る状態推定装置の作用＞
図２２は、本発明の第２の実施の形態に係る状態推定処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る状態推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

ステップＳ３００において、近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該追跡対象物の仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該追跡対象物の仮説の修正量を求める。

ステップＳ３１０において、ランドマーク修正部３１４は、追跡している追跡器の各々について、当該追跡器の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該追跡器の仮説を修正量を求める。

ステップＳ３２０において、状態更新部３０９は、上記ステップＳ３００により得られた修正量と、上記ステップＳ３１０により得られた修正量とを合成した仮説の修正量を用いて、上記ステップＳ１５０により予測された仮説の状態と共分散を修正する。

また、上記ステップＳ３００は、図２３に示す近傍仮説修正量算出処理ルーチンによって実現される。

ステップＳ３０１において、近傍仮説修正部３１３は、追跡している歩行者の各追跡器同士、及び歩行者の各追跡器と障害物とのユークリッド距離を算出する。

ステップＳ３０２において、近傍仮説修正部３１３は、最も近い２つの追跡器の組を抽出する。

ステップＳ３０３において、抽出した組において、追跡器ａの複数の仮説と、追跡器ｂの複数の仮説との組み合わせの各々についてＫＬＤを計算し、最もＫＬＤが短い仮説同士を求め、最小のＫＬＤを増大させる勾配方向の修正量を求め、リターンする。

また、追跡している車両についても、上記図２３に示す近傍仮説修正量算出処理ルーチンと同様の処理ルーチンを実行する。

また、上記ステップＳ３１０は、図２４に示すランドマーク修正量算出処理ルーチンによって実現される。

ステップＳ３１１において、ランドマーク修正部３１４は、ランドマーク地図ＤＢ３１５に格納されたランドマークを取得する。

ステップＳ３１２において、ランドマーク修正部３１４は、追跡している歩行者のうち、１番目の歩行者を選択する。

ステップＳ３１３において、ランドマーク修正部３１４は、選択している歩行者とランドマークとの距離が最も短いランドマークを選択する。

ステップＳ３１４において、ランドマーク修正部３１４は、最小のＫＬＤを減少させる勾配方向の修正量を求める。

ステップＳ３１５において、ランドマーク修正部３１４は、全ての歩行者について処理を行ったか否かを判定する。

全ての歩行者について処理を行っていない場合（ステップＳ３１５のＮＯ）、ステップＳ３１６において、次の歩行者を選択して、ステップＳ３１３～Ｓ３１５の処理を繰り返す。

一方、全ての歩行者について処理を行っている場合（ステップＳ３１５のＹＥＳ）、リターンする。

また、追跡している車両についても、上記図２４に示すランドマーク修正量算出処理ルーチンと同様の処理ルーチンを実行する。

以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正し、当該追跡対象物の複数の仮説を、当該仮説の修正結果と、当該追跡対象物の検出結果とに基づいて更新することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる。

＜本発明の第３の実施の形態に係る状態推定システムの概要＞
第１の実施形態では、仮説を削除する構成、第２の実施の形態では、仮説を修正する構成としたが、本実施形態では、これを組み合わせた構成について説明する。削除と修正量の順は任意の構成とすることができるが、本実施形態では、仮説を削除した後に、修正量を算出する。

また、第１の実施形態及び第２の実施形態では、カルマンフィルタのＫＬＤを用いて仮説の削除・修正を行ったが、本実施形態では、予測後の仮説に一定の大きさを想定することにより、仮説の削除・修正を行う。

＜本発明の第３の実施の形態に係る状態推定装置の構成＞
本発明の第３の実施の形態に係る状態推定システム３０の構成について説明する。なお、第１の実施の形態に係る状態推定システム１０及び第２の実施の形態に係る状態推定システム２０と同様の構成については、同一の符号を付して詳細な説明は省略する。

図２５に示すように、本発明の実施の形態に係る状態推定システム３０は、センシング装置１００と、状態推定装置４００とを備えて構成される。本実施形態では、第２の実施形態と同様に、交通環境が駐車場であり、追跡対象物が歩行者及び車両であり、センシング装置１００が、車両上に設置される場合を例に説明する（図３）。

図２５に示すように、状態推定装置４００は、入力部２０１と、障害物特定部３０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部３０６と、予定経路ＤＢ３１２と、重複仮説削除部４０７と、静止障害物地図ＤＢ２０８と、近傍仮説修正部４１３と、ランドマーク修正部４１４と、ランドマーク地図ＤＢ３１５と、状態更新部３０９と、衝突可能性判定部３１０と、出力部２１１とを備えて構成される。

重複仮説削除部４０７は、追跡している追跡対象物の各々について、当該歩行者の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除する。

具体的には、重複仮説削除部４０７は、追跡対象物の複数の仮説の各々について、当該仮説が示す位置を中心とする予め定めた半径の円と、同一の追跡対象物の他の仮説が示す位置を中心とする当該半径の円の各々との交点を算出し、当該仮説の円内に存在する交点を数える。

例えば、図２６のように、中央に位置する仮説は円内に交点が１６個含まれ、他の仮説は円内に１３個の交点が含まれている。

次に、重複仮説削除部４０７は、円内に閾値以上の交点を含む仮説を削除する。例えば、閾値を１５個とすると、図２６における中央に位置する仮説を削除する（図２７）。

そして、重複仮説削除部４０７は、追跡している追跡対象物の各々について、仮説の削除処理を行った後の仮説予測部２０６による予測結果を、状態更新部３０９、近傍仮説修正部４１３及びランドマーク修正部４１４に渡す。

近傍仮説修正部４１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の各々に対し、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説の修正量を求める。

具体的には、まず、近傍仮説修正部４１３は、各追跡器同士のユークリッド距離を算出し、最も近い２つの追跡器の組（追跡器ａと追跡器ｂ）を抽出する（図１２）。

次に、近傍仮説修正部４１３は、抽出した追跡器ａ及び追跡器ｂの組における一方の追跡器ａの複数の仮説の各々について、当該仮説が示す位置を中心とする予め定めた半径の円と、他方の追跡器ｂの各仮説が示すが位置を中心とする円の各々との交点を算出し、各円内に存在する交点を数える（図２８）。同様に、追跡器ｂの複数の仮説の各々についても、各円内に存在する交点を数える。

また、近傍仮説修正部４１３は、一方の追跡器ａの複数の仮説の各々について、他方の追跡器ｂの仮説のうち、最も近い仮説を特定する。例えば、図２８において、追跡器ａの仮説ｘ_ａ２については、追跡器ｂの仮説ｘ_ｂ２が最も近い仮説である。同様に、追跡器ｂの複数の仮説の各々について、追跡器ａの仮説のうち、最も近い仮説を特定する。

その後、近傍仮説修正部４１３は、一方の追跡器ａの複数の仮説の各々について、最も近い仮説へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する（図２９）。同様に、追跡器ｂの複数の仮説の各々について、最も近い仮説へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する。

また、近傍仮説修正部４１３は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、当該仮説が示す位置を中心とする予め定めた半径の円と、最も近い障害物領域との交点を算出し、各円内に存在する交点を数える。

そして、近傍仮説修正部４１３は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、最も近い障害物領域の中心へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する。

そして、近傍仮説修正部４１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。

ランドマーク修正部４１４は、追跡している追跡対象物の各々について、当該仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該仮説を修正する。

具体的には、まず、ランドマーク修正部４１４は、追跡器の複数の仮説の各々について、当該仮説を中心とする予め定めた半径の円と、ランドマーク地図ＤＢ３１５に格納されたランドマークｌを中心とする予め定めた半径の円との交点を算出し、当該仮説を中心とする円内に存在する交点を数える（図３０）。

また、ランドマーク修正部４１４は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、ランドマークｌの中心へのベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する（図３１）。

そして、ランドマーク修正部４１４は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。

＜本発明の第３の実施の形態に係る状態推定装置の作用＞
図３２は、本発明の第３の実施の形態に係る状態推定処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る状態推定処理ルーチン及び第２の実施の形態に係る状態推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。

以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速かつ精度よく行うことができる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

例えば、センシング装置１００は、第１の実施形態において、移動しない場所に設置される場合について説明したが、移動物上に設置されてもよい。また、センシング装置１００は、第２、第３の実施形態において、移動物上に設置される場合について説明したが、移動しない場所に設置されてもよい。

また、第３の実施形態において、仮説を中心とした予め定めた半径の円の交点を用いたが、所定の距離内の仮説やランドマークを算出して、仮説の削除、修正を行ってもよい。

また、第２の実施形態及び第３の実施形態において、近傍仮説修正部は、追跡している歩行者の各々について、当該歩行者の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の歩行者の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説の修正量を求めたが、当該仮説の修正量を、当該仮説の予測結果が持つ範囲と、他の歩行者の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて求める構成としてもよい。

また、上記第１の実施形態、第２の実施形態において、第３の実施形態と同様に、予測後の仮説に一定の大きさを想定することにより、仮説の削除・修正を行う方法を適用してもよい。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１０、２０、３０状態推定システム
１００センシング装置
２００、３００、４００状態推定装置
２０１入力部
２０２、３０２障害物特定部
２０３遮蔽領域検出部
２０４検出部
２０５状態記憶部
２０６、３０６仮説予測部
２０７、４０７重複仮説削除部
２０８静止障害物地図ＤＢ
２０９、３０９状態更新部
２１０衝突可能性判定部
２１１出力部
３１０衝突可能性判定部
３１２予定経路ＤＢ
３１３、４１３近傍仮説修正部
３１４、４１４ランドマーク修正部
３１５ランドマーク地図ＤＢ

Claims

センサを用いて、追跡対象物を検出する検出部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、
を含み、
前記重複仮説削除部は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する
状態推定装置。
前記検出部は、センサを用いて、追跡対象物と障害物とを検出し、
前記重複仮説削除部は、前記追跡している追跡対象物の各々について、更に、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を削除する
請求項１記載の状態推定装置。
前記状態更新部により更新された前記複数の仮説に基づいて、前記追跡している追跡対象物同士の衝突可能性を判定する衝突可能性判定部、
を更に含む請求項１又は２記載の状態推定装置。
センサを用いて、追跡対象物と障害物とを検出する検出部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、
前記追跡している追跡対象物の各々について、前記仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、前記仮説を修正するランドマーク修正部と、
を含む状態推定装置。
センサを用いて、追跡対象物と障害物とを検出する検出部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する重複仮説削除部と、
を含む状態推定装置。
前記状態更新部により更新された前記複数の仮説と、前記センサを備える移動物の運動を表すセンサ情報とに基づいて、前記追跡対象物と、前記移動物との衝突可能性を判定する衝突可能性判定部、
を更に含む請求項４又は５記載の状態推定装置。