(57)【要約】【課題】複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができるようにする。【解決手段】仮説予測部２０６が、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、重複仮説削除部２０７が、当該追跡対象物の複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除する。【選択図】図１
2019168953.tif 000002
【特許請求の範囲】
【請求項１】
センサを用いて、追跡対象物を検出する検出部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、
を含む状態推定装置。
【請求項２】
前記重複仮説削除部は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する
請求項１記載の状態推定装置。
【請求項３】
前記検出部は、センサを用いて、追跡対象物と障害物とを検出し、
前記重複仮説削除部は、前記追跡している追跡対象物の各々について、更に、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を削除する
請求項２記載の状態推定装置。
【請求項４】
センサを用いて、追跡対象物と障害物とを検出する検出部と、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部と、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、
を含む状態推定装置。
【請求項５】
前記追跡している追跡対象物の各々について、前記仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、前記仮説を修正するランドマーク修正部
を更に含む請求項４記載の状態推定装置。
【請求項６】
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する重複仮説削除部
を更に含む請求項４又は５記載の状態推定装置。
【請求項７】
コンピュータを、
センサを用いて、追跡対象物を検出する検出部、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部、及び
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部
として機能させるためのプログラム。
【請求項８】
コンピュータを、
センサを用いて、追跡対象物と障害物とを検出する検出部、
追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部、
前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部、
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部、及び
前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、予め危険シーンのセンサデータを蓄積しておき、現在のセンサデータと照合して危険を報知する技術がある（特許文献１、特許文献２）。また、地図及び通信で取得したセンシングから、ある地点での複数の移動物の予測到達時刻と交通状況に照らして危険を報知する技術がある（特許文献３、特許文献４）。
【０００３】
また、地図と移動物の運動状態から、各移動物の将来移動範囲をリスクポテンシャルで算出し、その重なりによって危険を報知する技術がある（特許文献５〜８）。地図と移動物の運動状態からのリスクポテンシャルを算出する際に、算出手段を動的に切り替える技術がある（特許文献９）。
【０００４】
また、交通参加者の属性ごとの存在しやすさ（しにくさ）を表す地図を用いて、状態推定を行う技術がある（特許文献１０）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００１−３３１９００号公報
【特許文献２】特開２００８−００３７０７号公報
【特許文献３】特開２０１０−０２０６３７号公報
【特許文献４】特開２０１６−０２１１２５号公報
【特許文献５】特表２００９−５０５２６０号公報
【特許文献６】特開２０１２−１６４１５９号公報
【特許文献７】国際公開第１６／０９２５９１号
【特許文献８】特許第５６７３１２７号公報
【特許文献９】特許第５６２０１４７号公報
【特許文献１０】国際公開第２０１２／０３３１７３号
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記特許文献１及び特許文献２の技術は、蓄積されたセンシングデータと、現在直面しているセンシングデータの照合によって危険報知が行われるため、現在直面している場面において、移動物のどのような挙動が危険に至るかを、時系列及び空間的に説明できず、また、蓄積されたセンシングデータに存在しない危険場面に対して報知することができない。
【０００７】
また、上記特許文献３及び特許文献４の技術は、ＧＰＳ等で取得した大局的な位置情報と、地図上の交差点等の位置情報から予測される到達時刻で危険性を判定し、交通状況及び各移動物の位置・速度・走路状態等のセンシングによって重みを付けて精度を向上させるため、交差点内の横断歩道上や複数車線のうちのいずれかといった、局所的な空間上での状態推定と危険判定は、簡易な構成では困難である。極めて精緻な位置推定と高精度の地図及びセンシングが必要となるためである。
【０００８】
また、上記特許文献５〜８の技術は、移動物及び交通環境の局所的かつ相対的な位置関係が精緻に推定できることが前提であり、簡易な構成では困難であって、また、乱数を用いた粒子によるリスクポテンシャルの計算は、繰り返し試行ごとに異なる結果となり、同一場面で異なる状態推定及び危険判定となるため、品質保証が困難である。
【０００９】
また、上記特許文献９の技術は、局所的な移動物の将来予測範囲に占める歩車道の割合等の挙動の変化を促す要因によって、予測手段を切り替えて、リスクポテンシャルの計算コストを削減することを目指しているが、駐車場などの複雑な環境では、常に複雑な予測手段を選択することになるため、リスクポテンシャルの計算コストが削減できない。
【００１０】
また、上記特許文献１０の技術は、交通参加者ごとの存在しやすさや、存在しにくさを表す存在可能性マップという形式での局所地図を生成する必要があり、センシングの誤認識等により属性が不明である交通参加者を取り扱うために、合理的な局所地図を用意することが難しい。
【００１１】
したがって、従来技術では、複雑な交通環境での移動物の状態推定を、多様な可能性の想定を失うことなく、簡易な構成によって少ない計算時間で行うことができない、という問題があった。
【００１２】
本発明は、上記問題点を解決するためになされたもので、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる状態推定装置及びプログラムを提供することを目的とする。
【００１３】
また、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる状態推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１４】
上記の目的を達成するために、第１の発明の状態推定装置は、センサを用いて、追跡対象物を検出する検出部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、を含んで構成されている。
【００１５】
また、第２の発明のプログラムは、コンピュータを、センサを用いて、追跡対象物を検出する検出部、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲に応じて、前記仮説を削除する重複仮説削除部、及び前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の予測結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部として機能させるためのプログラムである。
【００１６】
第１の発明の状態予測装置及び第２の発明のプログラムによれば、検出部が、センサを用いて、追跡対象物を検出し、状態記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説であって、当該追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納し、仮説予測部が、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、当該追跡対象物の複数の仮説を予測する。
【００１７】
そして、重複仮説削除部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除し、状態更新部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説を、当該仮説の予測結果と、当該追跡対象物の検出結果とに基づいて更新する。
【００１８】
このように、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる。
【００１９】
また、第１の発明の状態推定装置の前記重複仮説削除部は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除することができる。
【００２０】
また、第１の発明の状態推定装置の前記検出部は、センサを用いて、追跡対象物と障害物とを検出し、前記重複仮説削除部は、前記追跡している追跡対象物の各々について、更に、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を削除することができる。
【００２１】
また、第３の発明の状態推定装置は、センサを用いて、追跡対象物と障害物とを検出する検出部と、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部と、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部と、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部と、を含んで構成されている。
【００２２】
また、第４の本発明のプログラムは、コンピュータを、センサを用いて、追跡対象物と障害物とを検出する検出部、追跡している追跡対象物の各々について、前記追跡対象物の状態を示す仮説であって、前記追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納した状態記憶部、前記追跡している追跡対象物の各々について、前記センサを用いた検出タイミングでの、前記追跡対象物の複数の仮説を予測する仮説予測部、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は前記仮説の予測結果が持つ範囲と、前記障害物との重複度合いに応じて、前記仮説を修正する近傍仮説修正部、及び前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説を、前記仮説の修正結果と、前記追跡対象物の検出結果とに基づいて更新する状態更新部として機能させるためのプログラムである。
【００２３】
第３の発明の状態予測装置及び第４の発明のプログラムによれば、検出部が、センサを用いて、追跡対象物と障害物とを検出し、状態記憶部が、追跡している追跡対象物の各々について、当該追跡対象物の状態を示す仮説であって、当該追跡対象物が存在する可能性がある範囲を持つ仮説を複数格納し、仮説予測部が、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測する。
【００２４】
そして、近傍仮説修正部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正し、状態更新部が、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説を、当該仮説の修正結果と、当該追跡対象物の検出結果とに基づいて更新する。
【００２５】
このように、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度良く行うことができる。
【００２６】
また、第３の発明の状態推定装置は、前記追跡している追跡対象物の各々について、前記仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、前記仮説を修正するランドマーク修正部を更に含むことができる。
【００２７】
また、第３の発明の状態推定装置は、前記追跡している追跡対象物の各々について、前記追跡対象物の前記複数の仮説から、前記仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、前記仮説を削除する重複仮説削除部を更に含むことができる。
【発明の効果】
【００２８】
以上説明したように、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる、という効果が得られる。
【００２９】
また、本発明の状態推定装置及びプログラムによれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる、という効果が得られる。
【図面の簡単な説明】
【００３０】
【図１】本発明の第１の実施の形態における状態推定システムの概略構成を示すブロック図である。
【図２】本発明の実施の形態に係る交通環境の例を示すイメージ図である。
【図３】本発明の実施の形態に係る交通環境の例を示すイメージ図である。
【図４】本発明の第１の実施の形態に係る仮説予測部がセンサの画角外や、歩行者が出現し得るランドマークにも仮説を生成する場合のイメージ図である。
【図５】本発明の第１の実施の形態に係る仮説を予測した場合のイメージ図である。
【図６】本発明の第１の実施の形態に係る仮説間のＫＬＤを示すイメージ図である。
【図７】本発明の第１の実施の形態に係るＫＬＤが小さい方の仮説を示すイメージ図である。
【図８】本発明の第１の実施の形態における重複仮説削除部が仮説を削除した後のイメージ図である。
【図９】本発明の第１の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第１の実施の形態における重複仮説削除処理ルーチンの内容を示すフローチャートである。
【図１１】本発明の第２の実施の形態における状態推定システムの概略構成を示すブロック図である。
【図１２】本発明の第２の実施の形態に係るおけるユークリッド距離の最も近い２つの追跡器の例を示すイメージ図である。
【図１３】本発明の第２の実施の形態に係る仮説間のＫＬＤを示すイメージ図である。
【図１４】本発明の第２の実施の形態における各仮説の最小のＫＬＤを増大させる修正量を示すイメージ図である。
【図１５】本発明の第２の実施の形態における近傍仮説修正部により得られた修正量に基づいて仮説を修正した場合のイメージ図である。
【図１６】本発明の第２の実施の形態に係るおける追跡器と近いランドマークがある場合を示すイメージ図である。
【図１７】本発明の第２の実施の形態に係るおける仮説とランドマークとのＫＬＤを示すイメージ図である。
【図１８】本発明の第２の実施の形態における仮説とランドマークとのＫＬＤを減少させる修正量を示すイメージ図である。
【図１９】本発明の第２の実施の形態における近傍仮説修正部により得られた修正量と、ランドマーク修正部によって得られた修正量を、合成した修正量を示すイメージ図である。
【図２０】本発明の第２の実施の形態における修正した仮説の状態量を更新した場合を示すフローチャートである。
【図２１】本発明の第２の実施の形態における修正した仮説の共分散を更新した場合を示すフローチャートである。
【図２２】本発明の第２の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。
【図２３】本発明の第２の実施の形態における近傍仮説修正量算出処理ルーチンの内容を示すフローチャートである。
【図２４】本発明の第２の実施の形態におけるランドマーク修正量算出処理ルーチンの内容を示すフローチャートである。
【図２５】本発明の第３の実施の形態における状態推定システムの概略構成を示すブロック図である。
【図２６】本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。
【図２７】本発明の第３の実施の形態における円内に閾値以上の交点を含む仮説を削除した例を示すイメージ図である。
【図２８】本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。
【図２９】本発明の第３の実施の形態における最も近い仮説へのベクトルの反対方向のベクトルを示すイメージ図である。
【図３０】本発明の第３の実施の形態における仮説を中心とする予め定めた半径の円を示すイメージ図である。
【図３１】本発明の第３の実施の形態におけるランドマークから最も近い仮説のランドマークへの方向のベクトルを示すイメージ図である。
【図３２】本発明の第３の実施の形態における状態推定処理ルーチンの内容を示すフローチャートである。
【発明を実施するための形態】
【００３１】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
【００３２】
＜本発明の第１の実施の形態に係る状態推定システムの概要＞
本発明の実施形態では、交通環境上の移動物（車両、歩行者等）を追跡対象物とし、大きさを持つ仮説群で構成した追跡器として表現する。
【００３３】
そして、追跡器内の他の仮説や障害物との重複があれば仮説を除去する。
【００３４】
大きさを持った少数の仮説で追跡器を構成し、解析的な相互作用の計算のみで、乱数を排除し、かつ、網羅性のある状態推定の実現を行う。
【００３５】
追跡器を構成する仮説に大きさを設定し、寄与度の低い仮説を削減することで、計算量を削減することができる。ここで、仮説の大きさは、任意の大きさを定義することができる。本実施形態では、仮説の大きさが、カルマンフィルタにおける予測ステップでの共分散行列で表される場合について説明する。
【００３６】
＜本発明の第１の実施の形態に係る状態推定システムの構成＞
図１に示すように、本発明の第１の実施の形態に係る状態推定システム１０は、センシング装置１００と、状態推定装置２００とを備えて構成される。本実施形態では、交通環境が駐車場であり、追跡対象物が車両と歩行者である場合を例に説明する（図２）。
【００３７】
センシング装置１００は、移動物の位置・速度の推定に必要な情報を計測する。
【００３８】
具体的には、センシング装置１００は、カメラ、レーザーレーダ、電波受信機等のセンサを用いて、車両及び歩行者の位置・速度の推定に有益な情報を計測する。センシング装置１００は、多数台、多数の種類が混在しても良い。
【００３９】
センシング装置１００は、車両及び歩行者を計測できる場所であれば、任意の場所に設置されることができる。
【００４０】
例えば、センシング装置１００は、天井等の移動しない場所に設置されることができる（図２）。この場合、センシング装置１００は、設置位置を適切に選定することで、カメラの撮影範囲やレーザー等が遮蔽される領域を少なくすることができる。
【００４１】
また、例えば、センシング装置１００は、移動物、例えば車両上に設置されることもできる（図３）。以下、本実施形態では、図２の場合を例に説明する。
【００４２】
そして、センシング装置１００は、計測したセンサ情報を、状態推定装置２００の入力部２０１に渡す。ここで、センシング装置１００は、状態推定装置２００と直接接続されていることを要せず、状態推定装置２００に、インターネット等の通信網を介して接続されていてもよい。
【００４３】
状態推定装置２００は、ＣＰＵと、ＲＡＭと、後述する状態量推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。
【００４４】
図１に示すように、状態推定装置２００は、入力部２０１と、障害物特定部２０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部２０６と、重複仮説削除部２０７と、静止障害物地図ＤＢ２０８と、状態更新部２０９と、衝突可能性判定部２１０と、出力部２１１とを備えて構成される。
【００４５】
入力部２０１は、センシング装置１００から、センサ情報の入力を受け付ける。
【００４６】
そして、入力部２０１は、受け付けたセンサ情報を、障害物特定部２０２及び検出部２０４に渡す。
【００４７】
障害物特定部２０２は、センサ情報に基づいて、障害物領域を特定する。
【００４８】
具体的には、障害物特定部２０２は、予め移動物が存在しない状態においてレーザーレーダによるセンサ情報を用いて点群地図を作成し、障害物領域を特定する。
【００４９】
そして、障害物特定部２０２は、特定した障害物領域の情報を遮蔽領域検出部２０３に渡す。
【００５０】
遮蔽領域検出部２０３は、障害物領域の情報に基づいて、遮蔽領域を検出する。
【００５１】
具体的には、遮蔽領域検出部２０３は、例えばレーザーレーダにより計測された点群地図を、障害物特定部２０２で得られた障害物領域との差分により移動物を特定し、幾何学計算によって遮蔽領域を検出する。
【００５２】
そして、遮蔽領域検出部２０３は、検出した遮蔽領域の情報を、仮説予測部２０６に渡す。
【００５３】
検出部２０４は、センサ情報を用いて、観測時刻における歩行者及び車両を検出する。
【００５４】
具体的には、検出部２０４は、センサ情報から、歩行者を検出し、当該歩行者を追跡の対象である追跡対象物として、当該歩行者の位置及び速度を検出すると共に、車両を検出し、当該車両を追跡の対象である追跡対象物として、当該車両の位置および速度を検出する。
【００５５】
例えば、検出部２０４は、カメラ画像を用いた物体認識技術により画像中の人の矩形位置を検出すると共に、車両の矩形位置を検出する。
【００５６】
そして、検出部２０４は、レーザーレーダにより計測された点群をカメラ画像に投影して、矩形内の点群から距離を推定することで、歩行者の位置及び車両の位置を検出する。また、検出部２０４は、前時刻における、矩形内の点群と、現時刻における、矩形内の点群との比較により、歩行者の速度及び移動方向も推定すると共に、車両の速度及び移動方向も推定する。
【００５７】
そして、検出部２０４は、検出した全ての歩行者を追跡している歩行者として、追跡している歩行者の状態（位置、速度、観測時刻）を、仮説予測部２０６に渡すと共に、検出した全ての車両を追跡している車両として、追跡している車両の状態（位置、速度、観測時刻）を、仮説予測部２０６に渡す。
【００５８】
状態記憶部２０５は、追跡している歩行者の各々について、当該歩行者の状態を示す仮説を格納すると共に、追跡している車両の各々について、当該車両の状態を示す仮説を格納する。ここで、歩行者の仮説は、歩行者の存在である正規分布を仮定した仮説（位置、速度、共分散）であり、追跡器に含まれる。また、車両の仮説は、車両の存在である正規分布を仮定した仮説（位置、速度、共分散）であり、追跡器に含まれる。また、仮説にはＩＤや属性を含めてもよい。
【００５９】
具体的には、状態記憶部２０５は、状態更新部２０９により更新された追跡している歩行者の各々について、当該歩行者の状態（位置、速度を含む）、及び最後に観測した時刻を格納すると共に、状態更新部２０９により更新された追跡している車両の各々について、当該車両の状態（位置、速度を含む）、及び最後に観測した時刻を格納する。
【００６０】
仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を予測する。
【００６１】
具体的には、仮説予測部２０６は、全ての追跡器内の仮説に対して、カルマンフィルタの予測ステップ（線形予測／共分散拡大）を設定されたステップ数だけ繰り返すことにより、当該追跡器の将来の移動可能範囲を予測する。
【００６２】
ここでは、追跡器ａについて仮説群
2019168953.tif 000003
が生成されたとものとして、１ステップカルマン予測を用いた場合について説明する。仮説予測部２０６は、仮説ｘ０ａｉ毎に次の時刻の状態を示す仮説群
2019168953.tif 000004
を予測する。
【００６３】
ｉ番目の仮説ｘ０ａｉは、下記式（１）のように表すことができる。
【００６４】
【数１】
2019168953.tif 000005
【００６５】
ここで、（ｘ０ｐｉ）ｘ、（ｘ０ｐｉ）ｙは、それぞれ当該仮説のｘ、ｙ方向の位置を、（ｘ０ｖｉ）ｘ、（ｘ０ｖｉ）ｙは、それぞれ当該仮説のｘ、ｙ方向の速度を表す。
【００６６】
そして、下記式（２）及び（３）を用いて、次の時刻の状態を予測する。図５に、追跡器ａについての予測結果のイメージを示す。
【００６７】
【数２】
2019168953.tif 000006
【００６８】
ここで、
2019168953.tif 000007
は、次の時間への時間幅であり、σｐｘ、σｐｙ、σｖｘ、σｖｙは共分散である。
【００６９】
そして、仮説予測部２０６は、追跡している歩行者の各々についての予測結果を、重複仮説削除部２０７に渡す。
【００７０】
また、仮説予測部２０６は、追跡している車両の各々について、歩行者と同様に、センサを用いたタイミングでの、当該車両の複数の仮説を予測する。
【００７１】
また、仮説予測部２０６は、遮蔽領域の情報を用いて、図４に示すように、センサの画角外や、車両や歩行者が出現し得るランドマークに、車両や歩行者の仮説を新たに生成してもよい。ランドマークから歩行者や車両が交通環境である駐車場に入場する場面が想定されるからである。この場合、センシング装置１００が、駐車場内のランドマークの変化（例えば、エレベータの開閉等）を計測する構成としてもよい。
【００７２】
重複仮説削除部２０７は、追跡している追跡対象物（歩行者又は車両）の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物が存在する範囲との重複度合いに応じて、当該仮説を削除する。
【００７３】
具体的には、重複仮説削除部２０７は、追跡器が２つ以上の仮説で構成されている場合に、以下のように、仮説数の削減を図る。
【００７４】
まず、重複仮説削除部２０７は、予め作成した静止障害物地図ＤＢ２０８の静止障害物地図を用いて、障害物内に仮説の状態が含まれている場合に、当該仮説を削除する。
【００７５】
次に、各予測ステップにおいて各仮説の組み合わせの各々についてＫｕｌｌｂａｃｋ−Ｌｅｉｂｌｅｒ距離（ＫＬＤ）を計算し、さらに仮説間のＫＬＤの平均であるＪｅｎｓｅｎ−Ｓｈａｎｎｏｎ距離（ＪＳＤ）を算出する。
【００７６】
例えば、上記の１ステップカルマンフィルタによる予測結果を用いた場合、ｉ番目の仮説ｘ０ａｉと、ｊ番目の仮説ｘ０ａｊとのＫＬＤは、下記式（４）を用いて求める（図６）。
【００７７】
【数３】
2019168953.tif 000008
【００７８】
次に、重複仮説削除部２０７は、仮説間のＫＬＤの平均を、下記式（５）を用いて求める。
【００７９】
【数４】
2019168953.tif 000009
【００８０】
そして、重複仮説削除部２０７は、ＪＳＤの小さい組み合わせから順に予め設定した値ε以下かどうかを評価し（下記式（６））、設定値ε以下の場合にはＪＳＤを構成するＫＬＤが小さい方の仮説を削除する（図７、図８）。
【００８１】
【数５】
2019168953.tif 000010
【００８２】
削除した仮説ｘ０ａｋに関する評価中のＪＳＤの組み合わせ（ＪＳＤｋｉ、ＪＳＤｉｋ）も評価の対象外とする。なお、ＫＬＤが大きい方の仮説を削除する構成としてもよい。
【００８３】
また、予測された仮説と、特定された障害物領域との組み合わせの各々について、当該仮説の予測結果が持つ範囲と、障害物領域との重複度が閾値以上である場合には、当該仮説を削除する。
【００８４】
そして、重複仮説削除部２０７は、追跡している追跡対象物の各々について、仮説の削除処理を行った後の仮説予測部２０６による予測結果を、状態更新部２０９に渡す。
【００８５】
静止障害物地図ＤＢ２０８は、移動物が存在する地図上の静止している障害物の情報を含む地図である静止障害物地図を格納する。
【００８６】
状態更新部２０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、当該歩行者の検出結果とに基づいて更新する。
【００８７】
また、状態更新部２０９は、追跡している車両の各々について、当該車両の複数の仮説を、仮説の予測結果と、当該車両の検出結果とに基づいて更新する。
【００８８】
具体的には、状態更新部２０９は、重複仮説削除部２０７により仮説の削除処理を行った後の仮説予測部２０６による予測結果と、歩行者の検出結果とに基づいて、追跡器と歩行者の検出結果とを対応付け、追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す歩行者の複数の仮説を更新する。
【００８９】
そして、状態更新部２０９は、追跡している追跡対象物（歩行者又は車両）の各々について、更新した当該追跡対象物の複数の仮説を、状態記憶部２０５及び衝突可能性判定部２１０に渡す。
【００９０】
また、状態更新部２０９は、何れの追跡器とも対応付けられなかった歩行者の検出結果に基づいて、歩行者の複数の仮説を生成し、新たな歩行者の追跡器を生成する。状態更新部２０９は、何れの追跡器とも対応付けられなかった車両の検出結果に基づいて、車両の複数の仮説を生成し、新たな車両の追跡器を生成する。
【００９１】
また、状態更新部３０９は、何れの歩行者の検出結果とも対応付けられなかった歩行者の追跡器のうち、観測時刻から一定時間以上経過している追跡器を削除する。状態更新部３０９は、何れの車両の検出結果とも対応付けられなかった車両の追跡器のうち、観測した時刻から一定時間以上経過している追跡器を状態記憶部２０５から削除する。
【００９２】
衝突可能性判定部２１０は、追跡している歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説と、更新された車両の複数の仮説とに基づいて、当該歩行者と、車両との衝突可能性を判定する。
【００９３】
具体的には、衝突可能性判定部２１０は、追跡器が示す歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説の位置及び速度と、車両の複数の仮説の位置及び速度とから、当該歩行者と車両が衝突する可能性を算出し、算出した可能性が閾値以上であれば、衝突する可能性があると判定する。
【００９４】
なお、衝突可能性判定部２１０は、同様に、追跡している車両の各々について、状態更新部２０９により更新された当該車両の複数の仮説と、更新された他の車両の複数の仮説とに基づいて、当該車両と、他の車両との衝突可能性を判定する。
【００９５】
そして、衝突可能性判定部２１０は、判定結果を出力部２１１に渡す。
【００９６】
出力部２１１は、判定結果を出力する。
【００９７】
＜本発明の第１の実施の形態に係る状態推定装置の作用＞
次に、図９を参照して、本実施形態の状態推定装置２００の状態推定処理ルーチンについて説明する。
【００９８】
まず、ステップＳ１００において、入力部２０１は、センシング装置１００から、センサ情報の入力を受け付ける。
【００９９】
ステップＳ１１０において、障害物特定部２０２は、上記ステップＳ１００により受け付けたセンサ情報に基づいて、障害物領域を特定する。
【０１００】
ステップＳ１２０において、遮蔽領域検出部２０３は、上記ステップＳ１１０により特定した障害物領域の情報に基づいて、遮蔽領域を検出する。
【０１０１】
ステップＳ１３０において、検出部２０４は、上記ステップＳ１００により受け付けたセンサ情報を用いて、歩行者及び車両を検出する。
【０１０２】
ステップＳ１４０において、仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を生成すると共に、追跡している車両の各々について、センサを用いた検出タイミングでの、当該車両の複数の仮説を生成する。
【０１０３】
ステップＳ１５０において、仮説予測部２０６は、追跡している歩行者の各々について、センサを用いた検出タイミングでの、当該歩行者の複数の仮説を予測すると共に、追跡している車両の各々について、センサを用いた検出タイミングでの、当該車両の複数の仮説を予測する。
【０１０４】
ステップＳ１６０において、重複仮説削除部２０７は、追跡している追跡対象物（歩行者、車両）の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除する。また、重複仮説削除部２０７は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、障害物が存在する範囲との重複度合いに応じて、当該仮説を削除する。
【０１０５】
ステップＳ１７０において、状態更新部２０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、歩行者の検出結果とに基づいて更新する。また、状態更新部２０９は、追跡している車両の各々について、当該車両の複数の仮説を、仮説の予測結果と、車両の検出結果とに基づいて更新する。
【０１０６】
ステップＳ１８０において、状態記憶部２０５は、追跡している追跡対象物（歩行者、車両）の各々について、当該追跡対象物の状態を示す仮説を複数格納する。
【０１０７】
ステップＳ１９０において、衝突可能性判定部２１０は、追跡している歩行者の各々について、状態更新部２０９により更新された当該歩行者の複数の仮説と、更新された車両の複数の仮説とに基づいて、当該歩行者と、車両との衝突可能性を判定する。また、衝突可能性判定部２１０は、追跡している車両の各々について、状態更新部２０９により更新された当該車両の複数の仮説と、更新された他の車両の複数の仮説とに基づいて、当該車両と、他の車両との衝突可能性を判定する。
【０１０８】
ステップＳ２００において、出力部２１１は、判定結果を出力する。
【０１０９】
また、上記ステップＳ１６０は、図１０に示す重複仮説削除処理ルーチンによって実現される。
【０１１０】
ステップＳ１６１において、重複仮説削除部２０７は、追跡している歩行者のうち、１番目の歩行者を選択する。
【０１１１】
ステップＳ１６２において、重複仮説削除部２０７は、選択されている歩行者の複数の仮説と、予め作成した静止障害物地図ＤＢ２０８の静止障害物地図を用いて、複数の仮説と障害物との重複度を計算する。
【０１１２】
ステップＳ１６３において、重複仮説削除部２０７は、障害物と重複する仮説があるか否かを判定する。
【０１１３】
重複する仮説が無い場合（ステップＳ１６３のＮＯ）、ステップＳ１６５に進む。
【０１１４】
一方、重複する仮説がある場合（ステップＳ１６３のＹＥＳ）、ステップＳ１６４において、重複仮説削除部２０７は、重複する仮説を削除する。
【０１１５】
ステップＳ１６５において、重複仮説削除部２０７は、複数の仮説の各々について、当該仮説と他の仮説との、仮説同士の重複度を計算する。また、重複仮説削除部２０７は、複数の仮説の各々について、当該仮説が持つ範囲と障害物領域との重複度を計算する。
【０１１６】
ステップＳ１６６において、重複仮説削除部２０７は、計算した重複度に基づいて、重複する仮説があるか否かを判定する。
【０１１７】
重複する仮説が無い場合（ステップＳ１６６のＮＯ），ステップＳ１６８に進む。
【０１１８】
一方、仮説同士で重複する仮説がある場合（ステップＳ１６６のＹＥＳ）、ステップＳ１６７において、仮説同士で重複する仮説の一方を削除する。また、障害物領域と重複する仮説がある場合（ステップＳ１６６のＹＥＳ）、ステップＳ１６７において、障害物領域と重複する仮説を削除する。
【０１１９】
ステップＳ１６８において、重複仮説削除部２０７は、全ての歩行者について処理を行ったか否かを判定する。
【０１２０】
全ての歩行者について処理を行っていない場合（ステップＳ１６８のＮＯ）、ステップＳ１６９において、次の歩行者を選択して、ステップＳ１６２〜Ｓ１６８の処理を繰り返す。
【０１２１】
一方、全ての歩行者について処理を行った場合（ステップＳ１６８のＹＥＳ）、リターンする。
【０１２２】
また、追跡している車両についても、上記図１０に示す重複仮説削除処理ルーチンと同様の処理ルーチンを実行する。
【０１２３】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲に応じて、当該仮説を削除し、当該追跡対象物の複数の仮説を、当該仮説の予測結果と、当該追跡対象物の検出結果とに基づいて更新することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速に行うことができる。
【０１２４】
＜本発明の第２の実施の形態に係る状態推定システムの概要＞
第１の実施形態では、仮説を削除する構成としたが、第２の実施の形態では、他の追跡器との関係や、ランドマーク地図との組み合わせによって多様な状態推定を生成する。すなわち、センサにより得られたセンシング結果から推定された仮説から、将来の状態推定を求め、状態推定に対して、他の追跡器との距離に基づいた修正や、ランドマークへの誘因を考慮した修正を行う。
【０１２５】
また、予め設定した地図により、仮説生成や状態推定を限定することで、より効果を高めることができる。
【０１２６】
＜本発明の第２の実施の形態に係る状態推定システムの構成＞
本発明の第２の実施の形態に係る状態推定システム２０の構成について説明する。なお、第１の実施の形態に係る状態推定システム１０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０１２７】
図１１に示すように、本発明の実施の形態に係る状態推定システム２０は、センシング装置１００と、状態推定装置３００とを備えて構成される。本実施形態では、交通環境が駐車場であり、追跡対象物が歩行者及び車両である場合を例に説明する（図３）。また、センシング装置１００が、車両上に設置される場合を例に説明する（図３）。
【０１２８】
センシング装置１００は、移動物の位置・速度の推定に必要な情報を計測する。
【０１２９】
具体的には、センシング装置１００は、カメラ、レーザーレーダ、電波受信機等のセンサを用いて、車両及び歩行者の位置・速度の推定に有益な情報を計測する。また、センシング装置１００は、センサにより、自車両の運動を表わすセンサ情報を計測する。
【０１３０】
そして、センシング装置１００は、計測したセンサ情報を、状態推定装置２００の入力部２０１に渡す。
【０１３１】
図１１に示すように、状態推定装置３００は、入力部２０１と、障害物特定部３０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部３０６と、予定経路ＤＢ３１２と、近傍仮説修正部３１３と、ランドマーク修正部３１４と、ランドマーク地図ＤＢ３１５と、状態更新部３０９と、衝突可能性判定部３１０と、出力部２１１とを備えて構成される。
【０１３２】
障害物特定部３０２は、センサ情報に基づいて、障害物領域を特定する。
【０１３３】
具体的には、障害物特定部３０２は、センシング装置１００が車両に設置されている場合には、当該車両の運動推定結果を用いて占有格子地図に対する投票を行うことにより、障害物領域を特定する。
【０１３４】
そして、障害物特定部３０２は、特定した障害物領域の情報を遮蔽領域検出部２０３及び近傍仮説修正部３１３に渡す。
【０１３５】
遮蔽領域検出部２０３は、障害物領域の情報に基づいて、遮蔽領域を検出する。
【０１３６】
具体的には、遮蔽領域検出部２０３は、センシング装置１００が車両に設置されている場合には、レーザーレーダにより計測された点群を投票することにより得られる占有格子地図に、カメラ画像による歩行者や車両の検出結果を投影し、移動物による遮蔽と、静止物による遮蔽を分離することにより、遮蔽領域を検出する。
【０１３７】
そして、遮蔽領域検出部２０３は、検出した遮蔽領域の情報を、仮説予測部２０６に渡す。
【０１３８】
仮説予測部３０６は、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、当該追跡対象物の複数の仮説を予測する。
【０１３９】
具体的には、仮説予測部３０６は、全ての追跡器内の仮説に対して、カルマンフィルタの予測ステップ（線形予測／共分散拡大）を設定されたステップ数だけ繰り返すことにより、当該追跡器の将来の移動可能範囲を予測する。このとき、車両の追跡器については、予定経路ＤＢ３１２に格納された当該車両の移動する経路であって、検出結果と同じ座標系で定義されたものである予定経路の範囲に限定して、車両の複数の仮説を予測する。
【０１４０】
そして、仮説予測部３０６は、追跡している追跡対象物の各々についての予測結果を、状態更新部３０９、近傍仮説修正部３１３、及びランドマーク修正部３１４に渡す。また、仮説予測部３０６は、遮蔽領域検出部２０３が検出した遮蔽領域を、近傍仮説修正部３１３に渡す。
【０１４１】
予定経路ＤＢ３１２は、予定経路を格納している。
【０１４２】
近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該追跡対象物の仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該追跡対象物の仮説の修正量を求める。
【０１４３】
具体的には、まず、近傍仮説修正部３１３は、各追跡器同士、及び各追跡器と障害物とのユークリッド距離を算出し、追跡器の各々について、当該追跡器と最も距離が近い他の追跡器又は障害物との組を抽出する。
【０１４４】
例えば、最も近い２つの追跡器の組（追跡器ａと追跡器ｂ）が抽出されたとする（図１２）。
【０１４５】
次に、近傍仮説修正部３１３は、抽出した組において、追跡器ａの複数の仮説と、追跡器ｂの複数の仮説との組み合わせの各々についてＫＬＤを計算し、下記式（７）及び（８）を用いて、最もＫＬＤが短い仮説同士を求める（図１３）。
【０１４６】
【数６】
2019168953.tif 000011
【０１４７】
ここで、ｉ＊は、追跡器ａの仮説のうち、最も追跡器ｂに近い仮説のＩＤであり、ｊ＊は、追跡器ｂの仮説のうち、最も追跡器ａに近い仮説のＩＤである。
【０１４８】
そして、近傍仮説修正部３１３は、最小のＫＬＤを増大させる勾配方向の、すなわち、異なる追跡器内の仮説同士が互いに回避するような各仮説の修正量を求める。近傍仮説修正部３１３は仮説ｘａｉ＊と仮説ｘｂｊ＊とのＫＬＤであるＫＬＤａｉ＊ｂｊ＊及びＫＬＤｂｊ＊ａｉ＊が大きくなるように、下記式（９）及び式（１０）を用いて修正する（図１４）。
【０１４９】
【数７】
2019168953.tif 000012
【０１５０】
ここで、
2019168953.tif 000013
は、仮説ｘａｉの修正量であり、
2019168953.tif 000014
は、仮説ｘｂｊの修正量である。
【０１５１】
なお、当該修正量を用いて仮説を修正すると、図１５に示すように、追跡器ａと追跡器ｂの仮説が回避するように、仮説が修正されることになる。
【０１５２】
また、近傍仮説修正部３１３は、最も距離が近い追跡器と障害物との組を抽出した場合にも、上記と同様に修正量を求める。
【０１５３】
具体的には、追跡器の複数の仮説の各々について、障害物とのＫＬＤを計算し、最もＫＬＤが短い仮説を求め、最小のＫＬＤを増大させる勾配方向の、すなわち、当該追跡器の複数の仮説が回避するような各仮説の修正量を求める。
【０１５４】
そして、近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。
【０１５５】
ランドマーク地図ＤＢ３１５は、歩行者や車両を誘引し得るようなランドマークの交通環境上の位置を格納する。例えば、図３において、歩行者出入口は歩行者を誘引し得るようなランドマークであり、車両出入口は車両を誘引し得るようなランドマークである。
【０１５６】
ランドマーク修正部３１４は、追跡している歩行者の各々について、当該歩行者の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該歩行者の仮説を修正する。
【０１５７】
具体的には、ランドマーク修正部３１４は、ランドマーク地図ＤＢ３１５に格納された歩行者についてのランドマークの位置により、追跡している歩行者の各々について、当該歩行者の仮説を修正する修正量を求める。
【０１５８】
まず、ランドマーク修正部３１４は、近傍仮説修正部３１３と同様に、追跡器とランドマークとの距離が最も短いランドマークを選択する。なお、追跡器とランドマークとの距離が閾値以下のランドマークのうち、最も短いランドマークを選択する構成としてもよい。
【０１５９】
例えば、ランドマークが歩行者出入口の場合（図１６）に、ランドマーク修正部３１４は、追跡器ａとランドマークとの距離が閾値以下のランドマークを選択する。なお、閾値以下のランドマークが複数ある場合には、一番距離が近いランドマークを選んで修正する。
【０１６０】
次に、ランドマーク修正部３１４は、追跡器ａの各仮説を基準として、選択したランドマークｌとのＫＬＤを計算し、下記式（１１）を用いてランドマークｌとのＫＬＤが最も小さい仮説ｘａｉ＊を求める（図１７）。
【０１６１】
【数８】
2019168953.tif 000015
【０１６２】
ここで、ｉ＊は、追跡器ａの仮説のうち、最もランドマークｌに近い仮説のＩＤである。
【０１６３】
そして、ランドマーク修正部３１４は、最小のＫＬＤを減少させる勾配方向の修正量を求める。すなわち、仮説ｘａｉ＊とランドマークｌとのＫＬＤであるＫＬＤｌｊ＊ａｉ＊が小さくなるように、下記式（１２）を用いて仮説ｘａｉ＊の修正量を求める（図１８）。
【０１６４】
【数９】
2019168953.tif 000016
【０１６５】
ここで、
2019168953.tif 000017
は、仮説ｘａｉの修正量である。
【０１６６】
他の追跡器、例えば追跡器ｂの各仮説についても、下記式（１３）及び（１４）を用いて同様に修正する。
【０１６７】
【数１０】
2019168953.tif 000018
【０１６８】
また、ランドマーク修正部３１４は、追跡している車両の各々について、当該車両の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該車両の仮説を修正する。この場合、ランドマーク地図ＤＢ３１５に格納された車両についてのランドマークの位置を用いて、歩行者の場合と同様に仮説を修正する。
【０１６９】
そして、ランドマーク修正部３１４は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。
【０１７０】
状態更新部３０９は、追跡している歩行者の各々について、当該歩行者の複数の仮説を、仮説の予測結果と、仮説の修正量と、歩行者の検出結果とに基づいて更新する。
【０１７１】
具体的には、状態更新部３０９は、歩行者の追跡器の各々についての、近傍仮説修正部３１３により得られた修正量
2019168953.tif 000019
と、ランドマーク修正部３１４によって得られた修正量
2019168953.tif 000020
とを、仮説予測部３０６による当該追跡器の各仮説の予測結果に適用した結果と、歩行者の検出結果とに基づいて、追跡器と歩行者の検出結果とを対応付け、追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す歩行者の複数の仮説を更新する。
【０１７２】
まず、状態更新部３０９は、仮説毎に、近傍仮説修正部３１３により得られた修正量
2019168953.tif 000021
と、ランドマーク修正部３１４によって得られた修正量
2019168953.tif 000022
とを、合成した仮説ｘａｉの修正量
2019168953.tif 000023
を、下記式（１５）を用いて計算する（図１９）。
【０１７３】
【数１１】
2019168953.tif 000024
【０１７４】
なお、修正量を加算合成する場合に、重みηを付けてもよい。仮説毎に重みηに異なる値を設定してもよいし、加算合成する際に重みηを設けてもよい。
【０１７５】
次に、状態更新部３０９は、仮説毎に、仮説予測部３０６により予測された仮説ｘａｉの状態量と共分散を、修正量
2019168953.tif 000025
を用いて修正する（下記式（１６）〜（１８）、図２０、２１）。
【０１７６】
【数１２】
2019168953.tif 000026
【０１７７】
次に、状態更新部３０９は、修正後の歩行者の追跡器の仮説と、歩行者の検出結果とに基づいて、歩行者の追跡器と歩行者の検出結果とを対応付け、歩行者の追跡器の各々について、対応付けされた歩行者の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新する。
【０１７８】
同様に、状態更新部３０９は、車両の追跡器の各々についての、近傍仮説修正部３１３により得られた修正量を、仮説予測部３０６による当該追跡器の各仮説の予測結果に適用した結果と、車両の検出結果とに基づいて、車両の追跡器と車両の検出結果とを対応付け、追跡器の各々について、対応付けされた車両の検出結果に基づいて、当該追跡器の複数の仮説の各々の状態量を更新することにより、当該追跡器が表す車両の複数の仮説を更新する。
【０１７９】
そして、状態更新部３０９は、追跡している追跡対象物の各々について、更新した当該追跡対象物の複数の仮説を、状態記憶部２０５及び衝突可能性判定部２１０に渡す。
【０１８０】
また、状態更新部３０９は、何れの追跡器とも対応付けられなかった歩行者の検出結果に基づいて、歩行者の複数の仮説を生成し、新たな歩行者の追跡器を生成する。状態更新部３０９は、何れの追跡器とも対応付けられなかった車両の検出結果に基づいて、車両の複数の仮説を生成し、新たな車両の追跡器を生成する。
【０１８１】
また、状態更新部３０９は、何れの歩行者の検出結果とも対応付けられなかった歩行者の追跡器のうち、最後に観測した時刻から一定時間以上経過している追跡器を削除する。状態更新部３０９は、何れの車両の検出結果とも対応付けられなかった車両の追跡器のうち、最後に観測した時刻から一定時間以上経過している追跡器を状態記憶部２０５から削除する。
【０１８２】
衝突可能性判定部３１０は、追跡している歩行者の各々について、状態更新部３０９により更新された当該歩行者の複数の仮説と、自車両の運動を表すセンサ情報とに基づいて、自車両と、当該歩行者との衝突可能性を判定する。
【０１８３】
具体的には、衝突可能性判定部３１０は、追跡している歩行者の各々について、状態更新部３０９により更新された当該歩行者の複数の仮説の位置及び速度と、自車両の位置及び速度とから、当該歩行者と自車両とが衝突する可能性を算出し、算出した可能性が閾値以上であれば、衝突する可能性があると判定する。
【０１８４】
また、衝突可能性判定部３１０は、追跡している車両の各々について、歩行者と同様に、状態更新部３０９により更新された当該車両の複数の仮説と、自車両の運動を表すセンサ情報とに基づいて、自車両と、当該車両との衝突可能性を判定する。
【０１８５】
そして、衝突可能性判定部３１０は、判定結果を出力部２１１に渡す。
【０１８６】
＜本発明の第２の実施の形態に係る状態推定装置の作用＞
図２２は、本発明の第２の実施の形態に係る状態推定処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る状態推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０１８７】
ステップＳ３００において、近傍仮説修正部３１３は、追跡している追跡対象物の各々について、当該追跡対象物の仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該追跡対象物の仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該追跡対象物の仮説の修正量を求める。
【０１８８】
ステップＳ３１０において、ランドマーク修正部３１４は、追跡している追跡器の各々について、当該追跡器の仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該追跡器の仮説を修正量を求める。
【０１８９】
ステップＳ３２０において、状態更新部３０９は、上記ステップＳ３００により得られた修正量と、上記ステップＳ３１０により得られた修正量とを合成した仮説の修正量を用いて、上記ステップＳ１５０により予測された仮説の状態と共分散を修正する。
【０１９０】
また、上記ステップＳ３００は、図２３に示す近傍仮説修正量算出処理ルーチンによって実現される。
【０１９１】
ステップＳ３０１において、近傍仮説修正部３１３は、追跡している歩行者の各追跡器同士、及び歩行者の各追跡器と障害物とのユークリッド距離を算出する。
【０１９２】
ステップＳ３０２において、近傍仮説修正部３１３は、最も近い２つの追跡器の組を抽出する。
【０１９３】
ステップＳ３０３において、抽出した組において、追跡器ａの複数の仮説と、追跡器ｂの複数の仮説との組み合わせの各々についてＫＬＤを計算し、最もＫＬＤが短い仮説同士を求め、最小のＫＬＤを増大させる勾配方向の修正量を求め、リターンする。
【０１９４】
また、追跡している車両についても、上記図２３に示す近傍仮説修正量算出処理ルーチンと同様の処理ルーチンを実行する。
【０１９５】
また、上記ステップＳ３１０は、図２４に示すランドマーク修正量算出処理ルーチンによって実現される。
【０１９６】
ステップＳ３１１において、ランドマーク修正部３１４は、ランドマーク地図ＤＢ３１５に格納されたランドマークを取得する。
【０１９７】
ステップＳ３１２において、ランドマーク修正部３１４は、追跡している歩行者のうち、１番目の歩行者を選択する。
【０１９８】
ステップＳ３１３において、ランドマーク修正部３１４は、選択している歩行者とランドマークとの距離が最も短いランドマークを選択する。
【０１９９】
ステップＳ３１４において、ランドマーク修正部３１４は、最小のＫＬＤを減少させる勾配方向の修正量を求める。
【０２００】
ステップＳ３１５において、ランドマーク修正部３１４は、全ての歩行者について処理を行ったか否かを判定する。
【０２０１】
全ての歩行者について処理を行っていない場合（ステップＳ３１５のＮＯ）、ステップＳ３１６において、次の歩行者を選択して、ステップＳ３１３〜Ｓ３１５の処理を繰り返す。
【０２０２】
一方、全ての歩行者について処理を行っている場合（ステップＳ３１５のＹＥＳ）、リターンする。
【０２０３】
また、追跡している車両についても、上記図２４に示すランドマーク修正量算出処理ルーチンと同様の処理ルーチンを実行する。
【０２０４】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正し、当該追跡対象物の複数の仮説を、当該仮説の修正結果と、当該追跡対象物の検出結果とに基づいて更新することにより、複雑な交通環境であっても、多様な移動物の状態推定を精度よく行うことができる。
【０２０５】
＜本発明の第３の実施の形態に係る状態推定システムの概要＞
第１の実施形態では、仮説を削除する構成、第２の実施の形態では、仮説を修正する構成としたが、本実施形態では、これを組み合わせた構成について説明する。削除と修正量の順は任意の構成とすることができるが、本実施形態では、仮説を削除した後に、修正量を算出する。
【０２０６】
また、第１の実施形態及び第２の実施形態では、カルマンフィルタのＫＬＤを用いて仮説の削除・修正を行ったが、本実施形態では、予測後の仮説に一定の大きさを想定することにより、仮説の削除・修正を行う。
【０２０７】
＜本発明の第３の実施の形態に係る状態推定装置の構成＞
本発明の第３の実施の形態に係る状態推定システム３０の構成について説明する。なお、第１の実施の形態に係る状態推定システム１０及び第２の実施の形態に係る状態推定システム２０と同様の構成については、同一の符号を付して詳細な説明は省略する。
【０２０８】
図２５に示すように、本発明の実施の形態に係る状態推定システム３０は、センシング装置１００と、状態推定装置４００とを備えて構成される。本実施形態では、第２の実施形態と同様に、交通環境が駐車場であり、追跡対象物が歩行者及び車両であり、センシング装置１００が、車両上に設置される場合を例に説明する（図３）。
【０２０９】
図２５に示すように、状態推定装置４００は、入力部２０１と、障害物特定部３０２と、遮蔽領域検出部２０３と、検出部２０４と、状態記憶部２０５と、仮説予測部３０６と、予定経路ＤＢ３１２と、重複仮説削除部４０７と、静止障害物地図ＤＢ２０８と、近傍仮説修正部４１３と、ランドマーク修正部４１４と、ランドマーク地図ＤＢ３１５と、状態更新部３０９と、衝突可能性判定部３１０と、出力部２１１とを備えて構成される。
【０２１０】
重複仮説削除部４０７は、追跡している追跡対象物の各々について、当該歩行者の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除する。
【０２１１】
具体的には、重複仮説削除部４０７は、追跡対象物の複数の仮説の各々について、当該仮説が示す位置を中心とする予め定めた半径の円と、同一の追跡対象物の他の仮説が示す位置を中心とする当該半径の円の各々との交点を算出し、当該仮説の円内に存在する交点を数える。
【０２１２】
例えば、図２６のように、中央に位置する仮説は円内に交点が１６個含まれ、他の仮説は円内に１３個の交点が含まれている。
【０２１３】
次に、重複仮説削除部４０７は、円内に閾値以上の交点を含む仮説を削除する。例えば、閾値を１５個とすると、図２６における中央に位置する仮説を削除する（図２７）。
【０２１４】
そして、重複仮説削除部４０７は、追跡している追跡対象物の各々について、仮説の削除処理を行った後の仮説予測部２０６による予測結果を、状態更新部３０９、近傍仮説修正部４１３及びランドマーク修正部４１４に渡す。
【０２１５】
近傍仮説修正部４１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の各々に対し、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説の修正量を求める。
【０２１６】
具体的には、まず、近傍仮説修正部４１３は、各追跡器同士のユークリッド距離を算出し、最も近い２つの追跡器の組（追跡器ａと追跡器ｂ）を抽出する（図１２）。
【０２１７】
次に、近傍仮説修正部４１３は、抽出した追跡器ａ及び追跡器ｂの組における一方の追跡器ａの複数の仮説の各々について、当該仮説が示す位置を中心とする予め定めた半径の円と、他方の追跡器ｂの各仮説が示すが位置を中心とする円の各々との交点を算出し、各円内に存在する交点を数える（図２８）。同様に、追跡器ｂの複数の仮説の各々についても、各円内に存在する交点を数える。
【０２１８】
また、近傍仮説修正部４１３は、一方の追跡器ａの複数の仮説の各々について、他方の追跡器ｂの仮説のうち、最も近い仮説を特定する。例えば、図２８において、追跡器ａの仮説ｘａ２については、追跡器ｂの仮説ｘｂ２が最も近い仮説である。同様に、追跡器ｂの複数の仮説の各々について、追跡器ａの仮説のうち、最も近い仮説を特定する。
【０２１９】
その後、近傍仮説修正部４１３は、一方の追跡器ａの複数の仮説の各々について、最も近い仮説へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する（図２９）。同様に、追跡器ｂの複数の仮説の各々について、最も近い仮説へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する。
【０２２０】
また、近傍仮説修正部４１３は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、当該仮説が示す位置を中心とする予め定めた半径の円と、最も近い障害物領域との交点を算出し、各円内に存在する交点を数える。
【０２２１】
そして、近傍仮説修正部４１３は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、最も近い障害物領域の中心へのベクトルの反対方向のベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する。
【０２２２】
そして、近傍仮説修正部４１３は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。
【０２２３】
ランドマーク修正部４１４は、追跡している追跡対象物の各々について、当該仮説の予測結果と、予め定められたランドマークの位置との距離に応じて、当該仮説を修正する。
【０２２４】
具体的には、まず、ランドマーク修正部４１４は、追跡器の複数の仮説の各々について、当該仮説を中心とする予め定めた半径の円と、ランドマーク地図ＤＢ３１５に格納されたランドマークｌを中心とする予め定めた半径の円との交点を算出し、当該仮説を中心とする円内に存在する交点を数える（図３０）。
【０２２５】
また、ランドマーク修正部４１４は、追跡対象物の追跡器の各々について、当該追跡器の複数の仮説の各々に対し、ランドマークｌの中心へのベクトルに対して、当該仮説の円内に存在する交点の数及び定係数を乗じて、当該仮説の修正量を算出する（図３１）。
【０２２６】
そして、ランドマーク修正部４１４は、追跡している追跡対象物の各々について、当該追跡対象物の複数の仮説の修正量を、状態更新部３０９に渡す。
【０２２７】
＜本発明の第３の実施の形態に係る状態推定装置の作用＞
図３２は、本発明の第３の実施の形態に係る状態推定処理ルーチンを示すフローチャートである。なお、第１の実施の形態に係る状態推定処理ルーチン及び第２の実施の形態に係る状態推定処理ルーチンと同様の処理については、同一の符号を付して詳細な説明は省略する。
【０２２８】
以上説明したように、本発明の実施の形態に係る状態推定装置によれば、追跡している追跡対象物の各々について、センサを用いた検出タイミングでの、追跡対象物の複数の仮説を予測し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、同一の追跡対象物の他の仮説が持つ範囲との重複度合いに応じて、当該仮説を削除し、当該追跡対象物の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の追跡対象物の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説を修正することにより、複雑な交通環境であっても、多様な移動物の状態推定を高速かつ精度よく行うことができる。
【０２２９】
なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。
【０２３０】
例えば、センシング装置１００は、第１の実施形態において、移動しない場所に設置される場合について説明したが、移動物上に設置されてもよい。また、センシング装置１００は、第２、第３の実施形態において、移動物上に設置される場合について説明したが、移動しない場所に設置されてもよい。
【０２３１】
また、第３の実施形態において、仮説を中心とした予め定めた半径の円の交点を用いたが、所定の距離内の仮説やランドマークを算出して、仮説の削除、修正を行ってもよい。
【０２３２】
また、第２の実施形態及び第３の実施形態において、近傍仮説修正部は、追跡している歩行者の各々について、当該歩行者の複数の仮説から、当該仮説の予測結果が持つ範囲と、他の歩行者の仮説が持つ範囲との重複度合い、及び当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて、当該仮説の修正量を求めたが、当該仮説の修正量を、当該仮説の予測結果が持つ範囲と、他の歩行者の仮説が持つ範囲との重複度合い、又は当該仮説の予測結果が持つ範囲と、障害物との重複度合いに応じて求める構成としてもよい。
【０２３３】
また、上記第１の実施形態、第２の実施形態において、第３の実施形態と同様に、予測後の仮説に一定の大きさを想定することにより、仮説の削除・修正を行う方法を適用してもよい。
【０２３４】
また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。
【符号の説明】
【０２３５】
１０、２０、３０     状態推定システム
１００ センシング装置
２００、３００、４００      状態推定装置
２０１ 入力部
２０２、３０２       障害物特定部
２０３ 遮蔽領域検出部
２０４ 検出部
２０５ 状態記憶部
２０６、３０６       仮説予測部
２０７、４０７       重複仮説削除部
２０８ 静止障害物地図ＤＢ
２０９、３０９       状態更新部
２１０ 衝突可能性判定部
２１１ 出力部
３１０ 衝突可能性判定部
３１２ 予定経路ＤＢ
３１３、４１３       近傍仮説修正部
３１４、４１４       ランドマーク修正部
３１５ ランドマーク地図ＤＢ
【図１】
2019168953.tif 000027
【図２】
2019168953.tif 000028
【図３】
2019168953.tif 000029
【図４】
2019168953.tif 000030
【図５】
2019168953.tif 000031
【図６】
2019168953.tif 000032
【図７】
2019168953.tif 000033
【図８】
2019168953.tif 000034
【図９】
2019168953.tif 000035
【図１０】
2019168953.tif 000036
【図１１】
2019168953.tif 000037
【図１２】
2019168953.tif 000038
【図１３】
2019168953.tif 000039
【図１４】
2019168953.tif 000040
【図１５】
2019168953.tif 000041
【図１６】
2019168953.tif 000042
【図１７】
2019168953.tif 000043
【図１８】
2019168953.tif 000044
【図１９】
2019168953.tif 000045
【図２０】
2019168953.tif 000046
【図２１】
2019168953.tif 000047
【図２２】
2019168953.tif 000048
【図２３】
2019168953.tif 000049
【図２４】
2019168953.tif 000050
【図２５】
2019168953.tif 000051
【図２６】
2019168953.tif 000052
【図２７】
2019168953.tif 000053
【図２８】
2019168953.tif 000054
【図２９】
2019168953.tif 000055
【図３０】
2019168953.tif 000056
【図３１】
2019168953.tif 000057
【図３２】
2019168953.tif 000058