移動物予測装置及びプログラム
【課題】移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができるようにする。
【解決手段】領域分割部４２によって、走路区分の領域及び静止物の領域に応じて領域を分割した領域分割マップを生成する。分岐ノード生成部４４によって、領域境界上に分岐ノードを生成する。移動物生成部４６によって、移動物の位置分布を、速度分布と共に記録する。領域分割マップにおいて移動物の位置分布に近傍に分岐ノードが存在する場合、移動物分割部５２によって、分岐ノードの分岐すべき方向の数だけ、移動物の位置分布を生成すると共に、分岐ノードが存在する境界で隣接する領域の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、移動物の位置分布に対して、分岐割合に応じた重み付けを行うと共に、対応する速度分布の各々を、各分岐すべき方向に応じて変更する。位置更新部４８によって、移動物の位置分布を更新する。
【解決手段】領域分割部４２によって、走路区分の領域及び静止物の領域に応じて領域を分割した領域分割マップを生成する。分岐ノード生成部４４によって、領域境界上に分岐ノードを生成する。移動物生成部４６によって、移動物の位置分布を、速度分布と共に記録する。領域分割マップにおいて移動物の位置分布に近傍に分岐ノードが存在する場合、移動物分割部５２によって、分岐ノードの分岐すべき方向の数だけ、移動物の位置分布を生成すると共に、分岐ノードが存在する境界で隣接する領域の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、移動物の位置分布に対して、分岐割合に応じた重み付けを行うと共に、対応する速度分布の各々を、各分岐すべき方向に応じて変更する。位置更新部４８によって、移動物の位置分布を更新する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動物予測装置及びプログラムに係り、特に、移動物の将来の位置を予測する移動物予測装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、自車走行状態と移動障害物の関係から、緊急回避すべき状態かどうかを判定し、周囲道路構造も含めた状態評価関数（例えば、リスクのポテンシャル）に対する最適制御軌道を算出して、車両制御を行う車両用支援制御装置が知られている（特許文献１）。
【０００３】
また、自車の走行制御計画の安全性を評価するために、周辺車両が将来時刻にとる存在位置分布予測を用いて、複数の自車の走行制御計画候補に対する安全性の評価を行い、最も安全な制御計画を選択する走行制御計画評価装置が知られている（特許文献２）。
【０００４】
また、一次主体と１つ又はそれ以上の非一次主体との間の衝突の警報を容易にするシステムであって、一次主体の状態に基づいて、トリガ条件が満たされるかどうかを判断するように構成されたトリガ機構と、予備的評価機構とを含み、トリガ条件が満たされる場合に、予備的評価機構は、トリガ条件に関連する１つ又はそれ以上の衝突シナリオを生成し、衝突シナリオにおける衝突の予備確率を評価し、衝突シナリオにおける衝突の予備確率に基づいて、衝突シナリオにおける衝突の改善した確率を評価するように特化評価機構を作動させるように構成されることを特徴とするシステムが知られている（特許文献３）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００７−２５３７７０号公報
【特許文献２】特開２００８−１１７０８２号公報
【特許文献３】特開２００８−３１０８０３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記の特許文献１〜３に記載の技術のように、他者との衝突危険度を推定する際には移動物の将来位置予測が重要となる。移動物の将来位置予測には想定すべき挙動・制約の組み合わせが多く、移動物が置かれている環境によっては複雑な位置分布形状になることが考えられるため、計算量が増大してしまう、という問題がある。
【０００７】
本発明は、上記の問題点を解決するためになされたもので、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる移動物予測装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記の目的を達成するために本発明に係る移動物予測装置は、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段と、前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段と、前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段と、前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段と、前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段と、前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段と、前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段とを含んで構成されている。
【０００９】
また、本発明に係るプログラムは、コンピュータを、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段、前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段、前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段、前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段、前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段、前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段、及び前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段として機能させるためのプログラムである。
【００１０】
本発明によれば、移動物検出手段によって、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する。領域検出手段によって、検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する。
【００１１】
そして、マップ生成手段によって、領域検出手段によって検出された走路区分の領域及び静止物の領域に応じて、検出対象範囲の領域を分割した領域分割マップを生成する。分岐ノード生成手段によって、領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する。移動物記録手段によって、領域分割マップにおいて、移動物検出手段によって検出された移動物の位置の確率分布を、移動状態の確率分布と共に記録する。
【００１２】
そして、分岐変更手段によって、領域分割マップにおいて移動物の位置の確率分布の中心との距離が所定値以下となる分岐ノードが存在する場合、分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、分岐すべき方向の数だけ生成された移動物の位置の確率分布に対して、分岐割合に応じた重み付けを行うと共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する。
【００１３】
そして、予測手段によって、移動物の位置の確率分布及び移動状態の確率分布に基づいて、領域分割マップ上の移動物の将来の位置の確率分布を予測する。
【００１４】
このように、分岐ノードの分岐すべき方向の数だけ、移動物の位置の確率分布を生成すると共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置の確率分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる。
【００１５】
本発明に係る分岐変更手段は、前回予測された移動の位置の確率分布及び移動状態の確率分布に基づいて、移動物の位置の確率分布の中心との距離が所定値以下となる分岐ノードが存在する場合、分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、各分岐すべき方向の分岐割合を算出し、分岐すべき方向の数だけ生成された移動物の位置の確率分布に対して、分岐割合に応じた重み付けを行うと共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更することを繰り返し行い、予測手段は、分岐変更手段によって変更された移動物の位置の確率分布及び移動状態の確率分布に基づいて、領域分割マップ上の移動物の将来の位置の確率分布を繰り返し予測するようにすることができる。
【００１６】
本発明に係る移動物予測装置は、算出対象の移動物の位置の確率分布と、算出対象の移動物以外の移動物の位置の確率分布とに基づいて、算出対象の移動物と算出対象の移動物以外の移動物との衝突危険度を算出する危険度算出手段を更に含むようにすることができる。これによって、複雑な形状となる移動物の将来の位置分布を用いて、他の移動物との衝突危険度を精度良く算出することができる。
【００１７】
本発明において、移動物の位置の確率分布及び移動状態の確率分布の各々を、正規分布で表し、生成された移動物の位置の複数の確率分布を、混合正規分布で表すようにすることができる。
【００１８】
本発明に係る領域検出手段は、検出対象範囲から、静止物の領域及び高さを検出し、静止物の領域の進入可能性を、検出された該静止物の高さに基づいて定めるようにすることができる。
【００１９】
また、本発明に係る移動物検出手段は、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布と共に、移動物の種類を検出し、走路区分の領域の進入可能性を、移動物の種類毎に定めるようにすることができる。
【００２０】
上記の移動物予測装置は、自装置を搭載した移動物から見て、検出対象範囲に存在する物体の位置を検出する物体位置検出手段と、自装置を搭載した移動物の運動を推定する運動推定手段と、前回更新された地図情報を、運動推定手段によって推定された自装置を搭載した移動物の現在の運動に基づいて、現在の自装置を搭載した移動物から見た地図情報に繰り返し更新する地図更新手段と、地図更新手段によって地図情報が更新される毎に、更新された地図情報に、物体位置検出手段によって検出された現在の物体の位置に対応するブロックに、静止物の存在を記録すると共に、自装置を搭載した移動物から検出された現在の物体の位置までの間に対応する各ブロックにおける静止物の存在の記録を減少させる静止物記録手段と、を更に含み、領域検出手段は、地図情報に基づいて、静止物の領域を検出するようにすることができる。
【発明の効果】
【００２１】
以上説明したように、本発明の移動物予測装置及びプログラムによれば、分岐ノードの分岐すべき方向の数だけ、移動物の位置の確率分布を生成すると共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置の確率分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる、という効果が得られる。
【図面の簡単な説明】
【００２２】
【図１】本発明の第１の実施の形態に係る衝突危険度判定装置を示すブロック図である。
【図２】（Ａ）走行環境の例を示すイメージ図、（Ｂ）初期状態における局所地図情報を示すイメージ図、及び（Ｃ）レーザレーダによる検出結果が記録された局所地図情報を示すイメージ図である。
【図３】領域分割マップの例を示すイメージ図である。
【図４】領域の種類毎に与えられる進入可能性の例を示す図である。
【図５】領域分割マップに分岐ノードを記録した様子を示すイメージ図である。
【図６】分岐ノードの分岐方向を示す図である。
【図７】（Ａ）位置分布の例を示すイメージ図、及び（Ｂ）位置分布が更新される様子を示すイメージ図である。
【図８】（Ａ）速度分布の例を示すイメージ図、（Ｂ）分岐方向ベクトルと速度ベクトルとを示す図、及び（Ｃ）速度分布を変更する様子を示すイメージ図である。
【図９】複数の分岐方向から分岐すべき方向を選択する様子を示す図である。
【図１０】分岐ノードによって位置分布が分割された後に更新された位置分布を示すイメージ図である。
【図１１】本発明の第１の実施の形態に係る衝突危険度判定装置における局所地図生成処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第１の実施の形態に係る衝突危険度判定装置における危険判定処理ルーチンの内容を示すフローチャートである。
【図１３】本発明の第１の実施の形態に係る衝突危険度判定装置における分岐ノード生成処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第２の実施の形態に係る衝突危険度判定装置を示すブロック図である。
【発明を実施するための形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載された衝突危険度判定装置に、本発明を適用した場合を例に説明する。
【００２４】
図１に示すように、第１の実施の形態に係る衝突危険度判定装置１０は、検出対象範囲である自車両の前方に対してレーザを１次元（水平方向）に走査しながら照射し、レーザの反射によりレーザが照射された物体の２次元位置を検出するレーザレーダ１２と、自車両の運動状態を検出する運動センサ１４と、自車両の前方を撮影するカメラ１８と、自車両の位置を検出するＧＰＳ装置２０と、これらの検出結果に基づいて、現在の自車両から見た前方の静止物の存在を記録した局所地図情報を生成すると共に、移動物の将来の位置及び移動状態を予測し、衝突する危険性があると判定された場合に表示装置２４に警告メッセージを表示させるように制御するコンピュータ２２とを備えている。
【００２５】
ここで、移動物とは、自ら移動する能力を持つか否かに関わらず、移動状態である物体である。例えば、自動車、自動二輪車、自転車、歩行者などである。
【００２６】
レーザレーダ１２は、車両前方に設置され、装置を基準とする車両前方に存在する物体までの距離を検出する装置であり、出力するレーザを水平方向に走査することで、レーザの反射により自車両前方に存在する複数の物体表面上の複数の点の位置を検出することができる。レーザレーダ１２による検出結果は、自車両前方に存在する物体表面のある点の位置を表す２次元座標の集合である。レーザレーダ１２による検出処理は一定サイクルで実行され、レーザレーダ１２は、各時点での自車両前方に存在する物体表面の複数の点の２次元位置を示すデータをコンピュータ２２に出力する。なお、レーザレーダ１２は、物体位置検出手段の一例である。
【００２７】
運動センサ１４は、自車両の速度を計測する車速センサ、ヨーレートを計測するジャイロセンサ、又は自車両の加速度を計測する加速度センサで構成されている。
【００２８】
カメラ１８は、小型のＣＣＤカメラ又はＣＭＯＳカメラで構成され、車両の前方を撮影するように車両のフロントウィンドウ上部等に取り付けられている。カメラ１８で撮影された前方の道路状況等の画像データは、コンピュータ２２に入力される。
【００２９】
コンピュータ２２は、ＣＰＵ、後述する局所地図生成処理ルーチン及び危険度判定処理ルーチンの各々を実行するためのプログラムを記憶したＲＯＭ、データ等を記憶するＲＡＭ、及びこれらを接続するバスを含んで構成されている。このコンピュータ２２をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、図１に示すように、レーザレーダ１２により検出された物体の２次元位置を取得する物体位置取得部３０と、運動センサ１４により検出された車速、ヨーレート、又は加速度に基づいて、自車両の運動を推定する自車運動推定部３２と、自車両の運動に基づいて、一時刻前までの局所地図情報を現在の自車両から見た座標系の局所地図情報に更新する局所地図更新部３４と、検出された現在の物体の２次元位置に基づいて、更新された局所地図情報における静止物の存在確率を変更する存在確率変更部３６と、を備えている。なお、存在確率変更部３６は、静止物記録手段の一例である。
【００３０】
自車運動推定部３２は、局所地図情報が前回更新されてから現在までの間における、運動センサ１４により検出された車速、ヨーレート、又は加速度に基づいて、局所地図情報が前回更新されてから現在までの自車両の運動を推定する。
【００３１】
局所地図更新部３４は、一時刻前までに更新された局所地図情報を、推定された自車両の運動に従って、現在の自車から見た座標系へと変換することで、現在の自車両から見た前方領域を表わす局所地図情報を生成する。また、局所地図更新部３４は、局所地図情報の更新を繰り返し行う。局所地図情報は、図２（Ｂ）に示すように、自車両前方の一定領域内を、一定の大きさの格子（ブロック）によって分割した複数のブロックで表される地図である。各ブロックにはその位置に静止物が存在する確率が記録され、初期状態では、図２（Ｂ）に示すように、存在確率として初期値である０.５（中間値）が記録されている。
【００３２】
存在確率変更部３６は、局所地図更新部３４により局所地図情報が更新される毎に、物体位置取得部３０によって取得された現在の物体の２次元位置それぞれに基づいて、更新された局所地図情報において、その物体の２次元位置に対応するブロックの存在確率を増加させ、自車両からその物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させる。このように存在確率を変更することにより、一定期間以上、対応する位置で物体が検出されたブロックの存在確率は高くなり、物体が一時的にしか検出されないブロックの存在確率は低くなる。図２（Ａ）に示すような走行環境である場合、図２（Ｃ）に示すように、例えば、静止物が確実に存在する位置のブロックには存在確率として１（最大値）が記録され、静止物が存在しない位置のブロックには存在確率として０（最小値）が記録される。また、他の物体によって隠されているなどの原因によって、その位置の情報が得られていないブロック（死角領域のブロック）には、存在確率として初期値である０.５（中間値）が記録される。
【００３３】
この結果、多くの移動物が存在する走行環境においても、比較的長時間同じ位置に観測される路側物などの静止物のみを安定して検出可能である。また、現在までレーザレーダ１２により観測できていない部分（不可視領域）も、存在確率が初期値（０.５）のままのブロックとして検出可能である。また、レーザレーダ１２の計測点がほとんど得られない、自車両の位置から見て静止物が存在する領域より遠方側の領域は、初期値から変化しないので、死角領域とみなすことができる。
【００３４】
また、コンピュータ２２は、電子地図を記憶した地図データベース３８と、カメラ１８によって撮影された前方画像、ＧＰＳ装置２０によって検出された自車両の位置、及び電子地図に基づいて、自車両周辺の移動物及び走行環境の状況を検出する環境検出部４０と、局所地図情報及び走行環境状況の検出結果に基づいて、静止物の領域及び各種の走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する領域分割部４２と、領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成する分岐ノード生成部４４と、物体位置取得部３０によって取得された物体の２次元位置の時系列データ、局所地図情報、及び運動センサ１４の検出結果に基づいて、自車両も含む移動物の位置分布及び運動状態分布を表わす移動物データを生成して領域分割マップ上に記録する移動物生成部４６と、移動物の位置分布及び運動状態分布に基づいて、将来の位置分布を予測して、領域分割マップ上に記録された移動物データを更新する位置更新部４８と、移動物データの位置分布の近傍に分岐ノードが存在するか否かを判定する分岐判定部５０と、分岐ノードが存在する場合に、位置分布を複数に分割すると共に、分割された位置分布の各々に対応する運動状態分布を変更する移動物分割部５２と、移動物データの位置分布に基づいて、自車両と他の移動物とが衝突する危険性があるか否かを判定し、判定結果を表示装置２４に表示させる危険判定部５４とを備えている。
【００３５】
位置更新部４８、分岐判定部５０、移動物分割部５２、及び危険判定部５４による一連の処理は、繰り返し実行される。なお、環境検出部４０及び領域分割部４２は、領域検出手段の一例である。領域分割部４２は、マップ生成手段の一例である。分岐判定部５０及び移動物分割部５２は、分岐変更手段の一例であり、位置更新部４８は、予測手段の一例である。危険判定部５４は、危険度算出手段の一例である。
【００３６】
地図データベース３８に記憶されている電子地図には、道路形状や標識表示、建造物に関する情報が格納されている。
【００３７】
環境検出部４０は、ＧＰＳ装置２０によって検出された自車位置に基づいて、地図データベース３８の電子地図から、自車両周辺の道路形状や標識表示、建造物に関する情報を検出する。また、環境検出部４０は、カメラ１８によって撮影された前方画像から、学習型のパターン認識技術（例えば、ＳＶＭ）によって、前方に存在する移動物及び移動物の種類（例えば、歩行者、二輪車、自動車等）を検出すると共に、前方の道路における各種の走路区分の種別（車線、歩道、横断歩道、信号、一時停止線など）及び領域を検出する。また、カメラ１８によって撮影された前方画像から、静止物の高さを検出する。
【００３８】
領域分割部４２は、局所地図情報において、静止物（例えば、ガードレール、植込み、建物、駐停車車両など）が存在する領域を検出し、検出された静止物の領域と、環境検出部４０によって検出された各走行領域の領域とに基づいて、図３に示すように、自車周辺領域を走路区分及び静止物により領域分割した領域分割マップを生成する。また、各分割領域に対して、図４に示すように、走路区分の種類及び静止物に対して予め定めた侵入可能性を表す数値（例えば０〜１までの確率値）を付与する。また、静止物の領域に対しては、環境検出部４０により検出された静止物の高さに基づき、侵入可能性を表す数値を付与してもよい。例えば、高さｈが、０＜ｈ＜１．０ｍである場合には、１．０−ｈを、進入可能性として付与し、ｈ＞１．０ｍである場合には、０を進入可能性として付与する。また、走路区分の種類に応じた進入可能性については、移動物の種類ごとに変えた侵入可能性を表す数値が付与される。また、信号などにより侵入可能性が時間的に変化する場合には、交通環境検出時の状況に従うようにしてもよい。
【００３９】
分岐ノード生成部４４は、以下に説明するように、領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成する。分岐ノードは、移動物の種類毎に生成され、ここでは、歩行者用の分岐ノードを生成する場合を例に説明する。車両用の分岐ノードについても同様にして求めることができる。
【００４０】
まず、領域分割マップにおいて、図５に示すように、隣接する分割領域との境界線上の中点付近に分岐ノードを付与する。ある領域に関して、領域境界線上の任意の点から反時計回りに領域境界線上で探索点を移動させ、右側隣接領域の切り替わりを確認しながら分岐ノードを生成する。なお、ここでの例では、隣接領域境界線上の中点付近にただ一つの分岐ノードを生成しているが、隣接領域境界線の長さに応じて分岐ノードの個数と間隔を変えてもよい。
【００４１】
次に、各分岐ノードに対して、図６に示すように、分岐ノードが存在する領域境界に隣接する領域に移動物の種類毎に付与された進入可能性に基づいて、分岐し得る方向、および移動物の種類毎の分岐割合（分岐可能性）を付与する。
【００４２】
ここでは、分岐しうる方向として、各分岐ノード位置おける領域境界線の単位法線ベクトルと単位接線ベクトルの４方向を与える。単位接線ベクトルは、分岐ノード位置を求めた際に、分岐ノード位置周辺での傾きを算出することで求める。あるいは、領域境界が曲線の方程式としてｆ（ｘ，ｙ）＝０の形で与えられている場合には、従来既知の手法により、数学的に解析して曲線の法線ベクトルを１つ求める。いずれかの方法で求めた法線ベクトルまたは接線ベクトルを回転することにより、残りの３方向のベクトルを求める。
【００４３】
また、移動物の種類の各々について、分割割合は、全ての分割し得る方向に対してその合計が１．０になるように以下のように与える。
【００４４】
まず、各分岐方向に対して、当該移動物の種類に対応する分割割合の初期値として次のような値を与える。分岐方向が法線方向である場合には、領域境界から分岐方向側にある領域に付与された、当該移動物の種類に対応する侵入可能性を示す値を与え、分岐方向が接線方向である場合には、領域境界で隣接する領域に付与された、当該移動物の種類に対応する侵入可能性を示す値のうち、大きい方の値を与える。
【００４５】
そして、移動物の種類毎に、各方向に与えられた初期値の合計値で正規化し、合計が１．０となるようにする。
【００４６】
例えば、上記図６に示すように、分岐しうる４方向に対して、分岐割合ａ１〜ａ４を与える場合には、まず、初期値として、ａ１ ＝ ０．８、ａ２ ＝ ｍａｘ（１．０，０．８）＝１．０、ａ３＝１．０、ａ４＝ｍａｘ（１．０，０．８）＝１．０を与える。そして、全方向で正規化することにより、ａ１＝０．８／（０．８＋１．０＋１．０＋１．０）≒０．２１１、ａ２＝ａ３＝ａ４＝１．０／（０．８＋１．０＋１．０＋１．０）≒０．２６３を与える。
【００４７】
移動物生成部４６は、連続する局所地図情報の時間差分をとることによって、移動物と静止物とを識別し、局所地図情報上において移動物が存在する領域を特定する。また、移動物生成部４６、特定された移動物が存在する領域と、環境検出部４０で移動物の種類が検出された領域とを統合することによって、移動物に対して、移動物の種類を与えると共に、ＩＤを与える。
【００４８】
また、移動物生成部４６は、物体位置取得部３０によって取得された物体の２次元位置の時系列データに基づいて、図７（Ａ）及び図８（Ａ）に示すように、移動物の位置および運動状態（例えば速度）を、自車中心ｘ−ｙ座標空間での２次元正規分布（平均ベクトルと分散行列）で表現し、移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。また、移動物生成部４６は、運動センサ１４によって検出された車速に基づいて、自車両の速度分布を生成し、自車両の位置分布及び速度分布を表わす移動物データを生成し、領域分割マップ上の自車位置に記録する。
【００４９】
移動物の位置分布は、以下の（１）式によって表される。
【００５０】
【数１】
【００５１】
ただし、μｐｏｓは、位置平均ベクトルであり、以下の（２）式によって表され、Σｐｏｓは、位置共分散行列であり、以下の（３）式によって表される。
【００５２】
【数２】
【００５３】
また、速度分布は、以下の（４）式によって表される。
【００５４】
【数３】
【００５５】
ただし、μｖｅｌは、速度平均ベクトルであり、以下の（５）式によって表され、Σｖｅｌは、速度共分散行列であり、以下の（６）式によって表される。
【００５６】
【数４】
【００５７】
位置更新部４８は、図７（Ｂ）に示すように、移動物の位置分布及び速度分布を用いて、Δｔ秒後の移動物の位置分布を線形予測によって求める。
【００５８】
更新後の位置分布の位置平均ベクトルμｖｅｌ’及び位置共分散行列Σｖｅｌ’は、以下の（７）式、（８）式で表される。
【００５９】
【数５】
【００６０】
分岐判定部５０は、更新後の移動物の位置分布の中心（平均）と各分岐ノード位置のマハラノビス距離を、以下の（９）式に従って算出し、予め定めた閾値Ｔｄｉｓｔ以下の距離を持つ分岐ノードが存在する場合、近傍に分岐ノードが存在すると判定する。
【００６１】
【数６】
【００６２】
また、分岐判定部５０は、近傍に分岐ノードが存在すると判定した場合、予め定めた閾値Ｔｄｉｓｔ以下の距離を持つ分岐ノードのうち、最小のマハラノビス距離を持つ分岐ノードを選択する。
【００６３】
移動物分割部５２は、近傍に分岐ノードが存在すると判定された移動物について、以下に説明するように、分岐ノード情報に従って移動物の位置分布を分割して複数の位置分布を生成し、分岐方向に応じて分割後の速度分布を変更する。
【００６４】
まず、移動物の平均速度ベクトルの単位ベクトルと分岐方向ベクトルとの内積値が、予め定めたＡ以上となる分岐方向ベクトルを、分岐すべき方向として選択する。
【００６５】
例えば、ｉ番目の分岐方向ベクトルｄｉを、以下の（１０）式で表し、移動物の平均速度ベクトルの単位ベクトルｅｖｅｌを、以下の（１１）式で表わすと、以下の（１２）式を満たす分岐方向ベクトルｄｉが選択される。
【００６６】
【数７】
【００６７】
上記（１２）式により、図９に示すように、後戻りとならない分岐方向が選択される。
【００６８】
次に、以下の（１３）式に示すように、選択した分岐方向ベクトルが持つ分割割合ａｉ’を正規化するように変更する。これによって、分岐すべき方向として選択した分岐方向ベクトルが持つ分割割合の合計が１．０になる。
【００６９】
【数８】
【００７０】
次に、選択された各分岐方向が持つ分割割合ａｉ’に従い、移動物の位置分布を分割して重み付けを行い、加算することで、分割後の位置分布を求める。このとき、分割された位置分布の各々に対して、移動物の種類を示すラベルとＩＤとを複製する。分割後の位置分布は、以下の（１４）式で表される。
【００７１】
【数９】
【００７２】
また、分割された移動物の位置分布に対応する速度分布として、元の速度分布を分岐方向に回転したものを与える。回転行列Ｒを以下の（１５）式で表わすと、回転後の速度平均ベクトルμｖｅｌ’及び位置共分散行列Σｖｅｌ’は、以下の（１６）式、（１７）式で表される。
【００７３】
【数１０】
【００７４】
例えば、図８（Ｂ）に示すように、ｉ番目の分岐方向への回転角度をθとする（反時計回りが正）と、元の速度分布をθ分だけ回転して、図８（Ｃ）に示すような速度分布が得られる。これにより、平均速度の大きさ／分散を維持して、速度方向だけが変わる。すなわち、元の運動量を保ったまま方向だけが、分岐方向を向くように変更される。
【００７５】
なお、同一分岐ノードで再度分割しないように、ここで利用した分岐ノードを記憶しておいてもよい。
【００７６】
上記のように、分割領域が持つ、移動物の種類に対応する進入可能性に基づく分割割合に応じて、移動物の位置分布が分割され、分岐方向に応じて分割後の速度分布が変更された場合には、位置更新部４８における次回の処理において、図１０に示すように、分割された位置分布が、分岐方向に応じて移動するように更新される。
【００７７】
危険判定部５４は、自車両の位置分布と、他の移動物の位置分布とを用いて、危険度として衝突確率を算出する。衝突確率は、同一将来時刻における同一位置に存在する確率（同時確率）であり、以下のようにして求めることができる。
【００７８】
同時確率（衝突確率）は、自車と他の移動物の位置分布を掛け合わせたものを、衝突範囲に渡って積分した値として求めることができる（一般にこの積分値は数値積分によって求める）。本実施例では、積分範囲として、領域分割マップの全体を用いる。
【００７９】
移動物の分割及び移動の結果、自車両および他の移動物の位置分布の各々は、複数個の重み付き分布の和で表されており、求める積分値は、自車両および他の移動物の分割された個々の位置分布の全ての組合せの積分値の和として求める。
【００８０】
例えば、歩行者の位置分布を以下の（１８）式で表わす。
【００８１】
【数１１】
【００８２】
ただし、ａｉは、分割されたｉ番目の分布に対する正規化された分割割合であり、Ｐｅｄｉ（ｘ）は、分割されたｉ番目の分布である。
【００８３】
また、自車両の位置分布を、以下の（１９）式で表わす。
【００８４】
【数１２】
【００８５】
ただし、ｂｊは、分割されたｊ番目の分布に対する正規化された分割割合であり、Ｅｇｏｊ（ｘ）は、分割されたｊ番目の分布である。
【００８６】
このとき、衝突確率は、以下の（２０）式に従って算出される。
【００８７】
【数１３】
【００８８】
ただし、Ｄは、積分範囲（２次元領域）であり、領域分割マップの全領域である。
【００８９】
また、危険判定部５４は、算出した衝突確率が、閾値以上であるか否かを判定し、衝突確率が閾値以上となる移動体が存在する場合には、当該移動物の将来予測位置及び警告情報等を、表示装置２４に表示させて、ドライバに注意喚起を行う。例えば、フロントガラスに当該移動物の将来予測位置及び警告情報等を重畳表示する。
【００９０】
位置更新部４８、分岐判定部５０、移動物分割部５２、危険判定部５４による一連の処理は、予め設定された予測時間分だけ繰り返し実行され、各予測ステップにおいて、各移動物について、近傍に分岐ノードが存在する場合、近傍に存在する分岐ノードに応じて、位置分布の分割及び速度分布の変更が行われて、各移動物の位置分布が移動する。また、各予測ステップにおいて、自車両と各移動物との衝突確率が算出され、衝突確率が閾値以上となった場合には、その予測ステップにおける当該移動物の将来予測位置、及び危険度情報等が表示装置２４によって表示される。
【００９１】
次に、本実施の形態に係る衝突危険度判定装置１０の作用について説明する。
【００９２】
まず、レーザレーダ１２によって、レーザが自車両の前方を水平方向に走査されて、走査方向に並んだレーザ照射位置としての物体の２次元位置の各々までの距離が計測され、自車両の前方に存在する物体の２次元位置が検出される。レーザレーダ１２によって検出される２次元位置は、レーザを走査する毎に得られる。
【００９３】
そして、コンピュータ２２によって、図１１に示す局所地図生成処理ルーチンが実行される。
【００９４】
まず、ステップ１００において、レーザレーダ１２から、前方に存在する物体の２次元位置（走査方向に並んだ各２次元位置までの計測距離）を示すデータを取得し、ステップ１０２において、一時刻前から現在までの間における、運動センサ１４により検出された車速、ヨーレート、又は加速度を取得し、取得した車速、ヨーレート、又は加速度に基づいて、一時刻前から現在までの自車両の運動を推定する。
【００９５】
そして、ステップ１０４において、後述するステップ１０６で前回更新された局所地図情報を、上記ステップ１０２で推定された自車両の運動に応じて、現在の自車両から見た座標系で表わされる局所地図情報に更新する。
【００９６】
次のステップ１０６では、上記ステップ１００で取得した自車両前方に存在する物体の２次元位置に基づいて、上記ステップ１０４で更新された局所地図情報において、自車両前方に存在する物体の２次元位置に対応するブロックの存在確率を増加させると共に、自車両から物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させることにより現在物体が存在しない位置を地図上に記録する。そして、上記ステップ１００へ戻る。
【００９７】
上述したように、局所地図生成処理ルーチンが繰り返し実行されることにより、現在の自車両から見た静止物の位置が随時推定される。
【００９８】
なお、生成される局所地図情報は、上記局所地図生成処理ルーチンが所定回数繰り返されたときに有効とされるようにしてもよい。
【００９９】
また、コンピュータ２２によって、図１２に示す危険判定処理ルーチンが実行される。まず、ステップ１２０において、カメラ１８により撮影された前方画像及びＧＰＳ装置２０によって検出された自車両位置を取得する。次のステップ１２２では、上記ステップ１２０で取得した前方画像、自車両位置、及び地図データベース３８の電子地図に基づいて、自車両周辺の走行環境の状況を検出して、各種の走路区分の領域を検出すると共に、周辺の移動物、移動物の運動状態、移動物の種類、及び静止物の高さを検出する。
【０１００】
そして、ステップ１２４において、上述した局所地図生成処理ルーチンにより得られる現在の局所地図情報を取得する。次のステップ１２５で、上記ステップ１２４で取得した局所地図情報に基づいて、静止物の領域を検出し、ステップ１２６において、上記ステップ１２２で検出した各種走路区分の領域及び上記ステップ１２５で検出した静止物の領域に基づいて、静止物の領域及び走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する。
【０１０１】
そして、ステップ１２８において、上記ステップ１２６で生成した領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成し、ステップ１３０で、レーザレーダ１２から取得された物体の２次元位置の時系列データ、上記ステップ１２４で取得した局所地図情報、及び運動センサ１４の検出結果に基づいて、自車両も含む移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。
【０１０２】
次のステップ１３２では、予測ステップを識別する変数ｎに、初期値１を設定する。ステップ１３４では、上記ステップ１３０又は後述するステップ１３８で得られた位置分布及び速度分布に基づいて、自車両も含む各移動物について、予測ステップｎにおける移動物の位置分布を予測して、各移動物の位置分布を更新する。
【０１０３】
そして、ステップ１３６において、自車両も含む各移動物について、上記ステップ１３４で更新された位置分布の中心との距離が閾値以下となる分岐ノードが存在するか否かを判定する。該当する分岐ノードが存在しない場合には、後述するステップ１４０へ移行する。一方、該当する分岐ノードが存在する場合には、ステップ１３８において、当該分岐ノードの近傍に存在する移動物について、分岐ノードに基づいて位置分布を分割すると共に、対応する速度分布を変更する。
【０１０４】
そして、ステップ１４０において、自車両の位置分布と、他の移動物の各々の位置分布とに基づいて、衝突危険度として、他の移動物との衝突確率を各々算出する。ステップ１４２では、上記ステップ１４０で算出された衝突確率が、閾値以上となる移動物が存在するか否かを判定する。該当する移動物が存在しない場合には、ステップ１４６へ移行する。一方、該当する移動物が存在する場合には、ステップ１４４において、該当する移動物の将来予測位置及び警告情報を表示装置２４に表示させる。
【０１０５】
ステップ１４６では、予測ステップを示す変数ｎが、将来予測時間に対応するステップ数Ｎに到達したか否かを判定する。変数ｎが、定数Ｎに到達していない場合には、ステップ１４８において、変数ｎをインクリメントして、上記ステップ１３４へ戻り、上記ステップ１３４以降の処理を繰り返す。一方、変数ｎが、定数Ｎに到達した場合には、危険判定処理ルーチンを終了する。
【０１０６】
上記ステップ１２８の処理は、図１３に示す分岐ノード生成処理ルーチンによって実現される。分岐ノード生成処理ルーチンは、領域分割マップの分割領域毎に実行される。
【０１０７】
まず、ステップ１６０において、処理対象の分割領域の境界線上の任意の点を探索点として設定すると共に、反時計回り方向に対して、当該探索点の右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。そして、ステップ１６２において、境界線上を反時計回りに探索点をΔｄだけ進める。次のステップ１６４では、現在の探索点の右側に隣接する分割領域ｉｄと記憶された分割領域ｉｄとに基づいて、右側に隣接する分割領域が切り替わったか否かを判定する（ａ！＝ｒａ＿ｉｄ？）。右側に隣接する分割領域が切り替わっていない場合には、上記ステップ１６２へ戻るが、一方、右側に隣接する分割領域が切り替わった場合には、ステップ１６６において、現在の探索点（ｐｘ，ｐｙ）を、開始点として記憶する（ｓｘ＝ｐｘ、ｓｙ＝ｐｙ）と共に、右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。また、累積距離Ｄを初期値すると共に（Ｄ＝０）、分岐ノード生成フラグを初期化する（ｆｌａｇ＝ｆａｌｓｅ）。
【０１０８】
次のステップ１６８では、境界線上を反時計回りに探索点をΔｄだけ進めると共に、累積距離を求める（Ｄ＝Ｄ＋Δｄ）。そして、ステップ１７０において、現在の探索点から−Δｄの範囲内に既に分岐ノードが存在するか否かを判定する。分岐ノードが存在する場合には、分岐ノード生成フラグをオンにして（ｆｌａｇ＝ｔｒｕｅ）、ステップ１７４へ移行する。一方、分岐ノードが存在しない場合には、そのままステップ１７４へ移行する。
【０１０９】
ステップ１７４では、現在の探索点の右側に隣接する分割領域ｉｄと記憶された分割領域ｉｄとに基づいて、右側に隣接する分割領域が切り替わったか否かを判定する（ａ！＝ｒａ＿ｉｄ？）。右側に隣接する分割領域が切り替わっていない場合には、上記ステップ１６８へ戻るが、一方、右側に隣接する分割領域が切り替わった場合には、ステップ１７６において、分岐ノード生成フラグに基づいて、既に分岐ノードが存在するか否かを判定する（ｆｌａｇ＝＝ｔｒｕｅ？）。分岐ノード生成フラグがオフである場合には、分岐ノードが存在しないと判断し、ステップ１７８において、累積距離Ｄの１／２分だけ戻った位置を分岐ノード位置として分岐ノードを生成すると共に、領域境界及び領域境界で隣接する分割領域の各々の進入可能性に基づいて、分岐し得る方向および分岐割合を求めて分岐ノードに与えて、ステップ１８０へ移行する。
【０１１０】
一方、上記ステップ１７６において、既に分岐ノードが存在する場合には、そのままステップ１８０へ移行する。
【０１１１】
ステップ１８０では、右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。また、累積距離Ｄを初期値すると共に（Ｄ＝０）、分岐ノード生成フラグを初期化する（ｆｌａｇ＝ｆａｌｓｅ）。そして、ステップ１８２において、探索点が、開始点を越えたか否かを判定する。現在の探索点から−Δｄの範囲内に開始点（ｓｘ、ｓｙ）が存在する場合には、開始点を越えたと判断して、分岐ノード生成処理ルーチンを終了するが、一方、現在の探索点から−Δｄの範囲内に開始点（ｓｘ、ｓｙ）が存在しない場合には、開始点を越えていないと判断して、上記ステップ１６８へ戻り、上記ステップ１６８以降の処理を繰り返す。
【０１１２】
以上説明したように、第１の実施の形態に係る衝突危険度判定装置によれば、分割領域の境界上に生成された分岐ノードの分岐すべき方向の数だけ、移動物の位置分布を生成すると共に、対応する速度分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる。複雑な形状となる移動物の将来の位置分布を用いて、他の移動物との衝突危険度を精度良く算出することができる。
【０１１３】
また、移動物の将来位置分布を求める際に混合正規分布表現を用いることにより、繰り返し計算を伴わず位置分布を求めることができるため、複数経路が考えられる移動物の将来位置推定において、モンテカルロ法に代表されるような繰り返しを伴うランダムサンプリング手法に比べ、処理速度を向上させることができる。
【０１１４】
また、移動物の将来位置分布を求める際に混合正規分布表現を用いることにより、単一の確率分布を用いるよりも複雑な形状の位置分布を推定することができるため、単純な線形予測によるものに比べ、移動物の位置推定精度が向上する。
【０１１５】
他者との衝突危険度を推定する際には、これら移動物の将来位置予測が重要な要素となる。しかしながら、移動物の将来位置の予測には想定すべき挙動・制約の組み合わせが多く、将来位置分布を効率的に求めることは困難である。そこで、従来技術では、モンテカルロ法に代表されるランダムサンプリングの繰り返し演算により分布を求めており、甚大な計算量の増加により、実時間処理に問題があった。また、ランダムサンプリングを行わない手法では、正規分布などの確率分布を用い線形予測により将来位置予測を求めている。この手法では、横断歩道などでの歩行者分岐の可能性に対して対応できず、位置予測の精度が悪くなってしまう。本実施の形態では、他者の将来位置予測にあたり、交通環境の物理的制約・交通規則の制約に基づいた分岐ノードの情報を生成した。また、移動物の位置分布にパラメトリックな分布を用いることで計算コストを抑えつつ、歩道、車道、横断歩道などの異なる通行帯境界に付与した分岐ノードの移動物の分岐方向・分岐割合の情報に基づきパラメトリックな分布を分割することにより、環境に応じた複雑な位置分布形状となる将来位置予測を効率的かつ高精度に行うことができる。
【０１１６】
なお、上記の第１の実施の形態では、運動センサの検出結果を用いて、自車両の運動を推定する場合を例に説明したが、これに限定されるものではなく、レーザレーダの検出結果の時間差分によって、自車両の運動を推定してもよい。また、例えば、ＧＰＳ装置による自車位置の検出結果を用いて自車両の運動を推定するようにしてもよい。また、レーザレーダ、運動センサ、及びＧＰＳ装置の検出結果を組み合わせて、自車両の運動を推定するようにしてもよい。
【０１１７】
次に、第２の実施の形態に係る衝突危険度判定装置について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１１８】
第２の実施の形態では、路側の装置において、移動物の将来分布を予測して、衝突する危険性を判定している点が、第１の実施の形態と主に異なっている。
【０１１９】
第２の実施の形態に係る衝突危険度判定装置は、路側に固定して設置されており、例えば、主要交差点に設置されている。図１４に示すように、衝突危険度判定装置２１０は、判定対象範囲である自装置の前方に対してレーザを１次元（水平方向）に走査しながら照射し、レーザの反射によりレーザが照射された物体の２次元位置を検出するレーザレーダ１２と、自装置の前方を撮影するカメラ１８と、自装置の位置を検出するＧＰＳ装置２０と、これらの検出結果に基づいて、現在の自装置から見た前方の静止物の存在を記録した局所地図情報を生成すると共に、移動物の将来の位置及び移動状態を予測し、予測結果に応じて衝突する危険性があると判定された場合に、警告メッセージを表示させる表示装置を搭載した車両において警告メッセージを表示させる指令を通信装置２２４に送信させるコンピュータ２２２とを備えている。
【０１２０】
コンピュータ２２２は、物体位置取得部３０と、検出された現在の物体の２次元位置に基づいて、自装置から見た座標系の局所地図情報における静止物の存在確率を変更する存在確率変更部２３６と、を備えている。
【０１２１】
また、コンピュータ２２２は、地図データベース３８と、自装置周辺の移動物及び走行環境の状況を検出する環境検出部４０と、領域分割部４２と、分岐ノード生成部４４と、移動物生成部４６と、位置更新部４８と、分岐判定部５０と、移動物分割部５２と、移動物データの位置分布に基づいて、各移動物について、他の移動物と衝突する危険性を判定し、表示装置を搭載した車両において判定結果を表示させるように指令を送信させる危険判定部２５４とを備えている。
【０１２２】
次に、第２の実施の形態に係る局所地図情報処理ルーチンでは、レーザレーダ１２から、前方に存在する物体の２次元位置を示すデータを取得し、次に、取得した自装置前方に存在する物体の２次元位置に基づいて、局所地図情報において、自装置前方に存在する物体の２次元位置に対応するブロックの存在確率を増加させると共に、自装置から物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させることにより現在物体が存在しない位置を地図上に記録する。そして、最初の処理へ戻る。
【０１２３】
上述したように、局所地図生成処理ルーチンが繰り返し実行されることにより、現在の自装置から見た静止物の位置が随時推定される。
【０１２４】
また、第２の実施の形態に係る危険判定処理ルーチンでは、まず、カメラ１８により撮影された前方画像及びＧＰＳ装置２０によって検出された自装置位置を取得する。次に、上記で取得した前方画像、自装置位置、及び地図データベース３８の電子地図に基づいて、自装置周辺の走行環境の状況を検出して、各種の走路区分の領域を検出すると共に、周辺の移動物、移動物の運動状態、移動物の種類、及び静止物の高さを検出する。
【０１２５】
そして、上述した局所地図生成処理ルーチンにより得られる現在の局所地図情報を取得する。次に、取得した局所地図情報に基づいて、静止物の領域を検出し、上記で検出した各種走路区分の領域及び上記で検出した静止物の領域に基づいて、静止物の領域及び走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する。
【０１２６】
そして、上記で生成した領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成し、レーザレーダ１２から取得された物体の２次元位置の時系列データ、及び上記で取得した局所地図情報に基づいて、領域分割マップ上に、各移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。
【０１２７】
次に、予測ステップを識別する変数ｎに、初期値１を設定し、上記で得られた、又は変更された位置分布及び速度分布に基づいて、各移動物について、予測ステップｎにおける移動物の位置分布を予測して、各移動物の位置分布を更新する。
【０１２８】
そして、各移動物について、上記で更新された位置分布の中心との距離が閾値以下となる分岐ノードが存在するか否かを判定し、該当する分岐ノードが存在する場合には、当該分岐ノードの近傍に存在する移動物について、位置分布を分割すると共に、対応する速度分布を変更する。
【０１２９】
そして、各移動物を判定対象として、判定対象の移動物の位置分布と、他の移動物の各々の位置分布とに基づいて、衝突危険度として、他の移動物との衝突確率を各々算出する。次に、上記で算出された衝突確率が、閾値以上となる移動物が存在するか否かを判定する。該当する移動物が存在する場合には、判定対象の移動物に搭載された表示装置に、該当する移動物の将来予測位置及び警告情報を表示させる指令を、通信装置２２４により当該判定対象の移動物へ送信する。
【０１３０】
予測ステップを示す変数ｎが、将来予測時間に対応するステップ数Ｎに到達するまで、上記の処理を繰り返し、変数ｎが、定数Ｎに到達した場合には、危険判定処理ルーチンを終了する。
【０１３１】
以上説明したように、第２の実施の形態に係る衝突危険度判定装置によれば、路側に設置された装置において、複雑な形状となる移動物の将来の位置分布を用いて、周辺の判定対象の移動物と他の移動物との衝突危険度を精度良く算出することができる。
【０１３２】
なお、上記の第１の実施の形態〜第２の実施の形態では、レーザレーダによりレーザを前方に走査して物体の位置を検出する場合を例に説明したが、これに限定されるものではなく、ミリ波などの電磁波を前方に走査して物体の位置を検出するようにしてもよい。
【０１３３】
また、レーザレーダによって自車両又は自装置の前方の物体の位置を検出する場合を例に説明したが、これに限定されるものではなく、例えば、ステレオカメラによって撮影された前方画像から、自車両又は自装置の前方の物体の位置を検出するようにしてもよい。
【０１３４】
また、領域分割マップの各分割領域に、進入可能性を付与し、付与された進入可能性に基づいて、分岐ノードの分割割合を求める場合を例に説明したが、これに限定されるものではなく、領域の種類毎に進入可能性を定めたテーブルを用意しておき、領域分割マップの各分割領域の種類と、上記のテーブルとに基づいて、分岐ノードの分割割合を求めるようにしてもよい。
【０１３５】
また、カメラによって撮影された前方画像から、走路区分の種類及び領域を検出する場合を例に説明したが、これに限定されるものではなく、走路区分の種類及び領域を示す情報を含む電子地図情報と、ＧＳＰによって検出した自車位置又は自装置の位置とに基づいて、走路区分の種類及び領域を検出するようにしてもよい。また、前方画像からの検出結果と組み合わせて、走路区分の種類及び領域を検出するようにしてもよい。
【０１３６】
また、事故シミュレータにおいて、本発明の技術を適用するようにしてもよい。例えば、進入可能性及び分岐ノードが与えられた領域分割マップにおいて、複数の移動物データを生成して記録し、複数の移動物データの位置分布を、分割ノードに応じて分割すると共に速度分布を変更し、位置分布を更新していくことで、移動物による事故状況をシミュレーションするようにしてもよい。
【０１３７】
本発明のプログラムを、記憶媒体に格納して提供することも可能である。
【符号の説明】
【０１３８】
１０、２１０ 衝突危険度判定装置
１２ レーザレーダ
１４ 運動センサ
１８ カメラ
２０ ＧＰＳ装置
２２、２２２ コンピュータ
３２ 自車運動推定部
３４ 局所地図更新部
３６、２３６ 存在確率変更部
３８ 地図データベース
４０ 環境検出部
４２ 領域分割部
４４ 分岐ノード生成部
４６ 移動物生成部
４８ 位置更新部
５０ 分岐判定部
５２ 移動物分割部
５４、２５４ 危険判定部
【技術分野】
【０００１】
本発明は、移動物予測装置及びプログラムに係り、特に、移動物の将来の位置を予測する移動物予測装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、自車走行状態と移動障害物の関係から、緊急回避すべき状態かどうかを判定し、周囲道路構造も含めた状態評価関数（例えば、リスクのポテンシャル）に対する最適制御軌道を算出して、車両制御を行う車両用支援制御装置が知られている（特許文献１）。
【０００３】
また、自車の走行制御計画の安全性を評価するために、周辺車両が将来時刻にとる存在位置分布予測を用いて、複数の自車の走行制御計画候補に対する安全性の評価を行い、最も安全な制御計画を選択する走行制御計画評価装置が知られている（特許文献２）。
【０００４】
また、一次主体と１つ又はそれ以上の非一次主体との間の衝突の警報を容易にするシステムであって、一次主体の状態に基づいて、トリガ条件が満たされるかどうかを判断するように構成されたトリガ機構と、予備的評価機構とを含み、トリガ条件が満たされる場合に、予備的評価機構は、トリガ条件に関連する１つ又はそれ以上の衝突シナリオを生成し、衝突シナリオにおける衝突の予備確率を評価し、衝突シナリオにおける衝突の予備確率に基づいて、衝突シナリオにおける衝突の改善した確率を評価するように特化評価機構を作動させるように構成されることを特徴とするシステムが知られている（特許文献３）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００７−２５３７７０号公報
【特許文献２】特開２００８−１１７０８２号公報
【特許文献３】特開２００８−３１０８０３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
上記の特許文献１〜３に記載の技術のように、他者との衝突危険度を推定する際には移動物の将来位置予測が重要となる。移動物の将来位置予測には想定すべき挙動・制約の組み合わせが多く、移動物が置かれている環境によっては複雑な位置分布形状になることが考えられるため、計算量が増大してしまう、という問題がある。
【０００７】
本発明は、上記の問題点を解決するためになされたもので、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる移動物予測装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記の目的を達成するために本発明に係る移動物予測装置は、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段と、前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段と、前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段と、前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段と、前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段と、前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段と、前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段とを含んで構成されている。
【０００９】
また、本発明に係るプログラムは、コンピュータを、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段、前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段、前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段、前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段、前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段、前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段、及び前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段として機能させるためのプログラムである。
【００１０】
本発明によれば、移動物検出手段によって、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する。領域検出手段によって、検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する。
【００１１】
そして、マップ生成手段によって、領域検出手段によって検出された走路区分の領域及び静止物の領域に応じて、検出対象範囲の領域を分割した領域分割マップを生成する。分岐ノード生成手段によって、領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する。移動物記録手段によって、領域分割マップにおいて、移動物検出手段によって検出された移動物の位置の確率分布を、移動状態の確率分布と共に記録する。
【００１２】
そして、分岐変更手段によって、領域分割マップにおいて移動物の位置の確率分布の中心との距離が所定値以下となる分岐ノードが存在する場合、分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、分岐すべき方向の数だけ生成された移動物の位置の確率分布に対して、分岐割合に応じた重み付けを行うと共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する。
【００１３】
そして、予測手段によって、移動物の位置の確率分布及び移動状態の確率分布に基づいて、領域分割マップ上の移動物の将来の位置の確率分布を予測する。
【００１４】
このように、分岐ノードの分岐すべき方向の数だけ、移動物の位置の確率分布を生成すると共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置の確率分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる。
【００１５】
本発明に係る分岐変更手段は、前回予測された移動の位置の確率分布及び移動状態の確率分布に基づいて、移動物の位置の確率分布の中心との距離が所定値以下となる分岐ノードが存在する場合、分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、各分岐すべき方向の分岐割合を算出し、分岐すべき方向の数だけ生成された移動物の位置の確率分布に対して、分岐割合に応じた重み付けを行うと共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更することを繰り返し行い、予測手段は、分岐変更手段によって変更された移動物の位置の確率分布及び移動状態の確率分布に基づいて、領域分割マップ上の移動物の将来の位置の確率分布を繰り返し予測するようにすることができる。
【００１６】
本発明に係る移動物予測装置は、算出対象の移動物の位置の確率分布と、算出対象の移動物以外の移動物の位置の確率分布とに基づいて、算出対象の移動物と算出対象の移動物以外の移動物との衝突危険度を算出する危険度算出手段を更に含むようにすることができる。これによって、複雑な形状となる移動物の将来の位置分布を用いて、他の移動物との衝突危険度を精度良く算出することができる。
【００１７】
本発明において、移動物の位置の確率分布及び移動状態の確率分布の各々を、正規分布で表し、生成された移動物の位置の複数の確率分布を、混合正規分布で表すようにすることができる。
【００１８】
本発明に係る領域検出手段は、検出対象範囲から、静止物の領域及び高さを検出し、静止物の領域の進入可能性を、検出された該静止物の高さに基づいて定めるようにすることができる。
【００１９】
また、本発明に係る移動物検出手段は、検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布と共に、移動物の種類を検出し、走路区分の領域の進入可能性を、移動物の種類毎に定めるようにすることができる。
【００２０】
上記の移動物予測装置は、自装置を搭載した移動物から見て、検出対象範囲に存在する物体の位置を検出する物体位置検出手段と、自装置を搭載した移動物の運動を推定する運動推定手段と、前回更新された地図情報を、運動推定手段によって推定された自装置を搭載した移動物の現在の運動に基づいて、現在の自装置を搭載した移動物から見た地図情報に繰り返し更新する地図更新手段と、地図更新手段によって地図情報が更新される毎に、更新された地図情報に、物体位置検出手段によって検出された現在の物体の位置に対応するブロックに、静止物の存在を記録すると共に、自装置を搭載した移動物から検出された現在の物体の位置までの間に対応する各ブロックにおける静止物の存在の記録を減少させる静止物記録手段と、を更に含み、領域検出手段は、地図情報に基づいて、静止物の領域を検出するようにすることができる。
【発明の効果】
【００２１】
以上説明したように、本発明の移動物予測装置及びプログラムによれば、分岐ノードの分岐すべき方向の数だけ、移動物の位置の確率分布を生成すると共に、対応する移動状態の確率分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置の確率分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる、という効果が得られる。
【図面の簡単な説明】
【００２２】
【図１】本発明の第１の実施の形態に係る衝突危険度判定装置を示すブロック図である。
【図２】（Ａ）走行環境の例を示すイメージ図、（Ｂ）初期状態における局所地図情報を示すイメージ図、及び（Ｃ）レーザレーダによる検出結果が記録された局所地図情報を示すイメージ図である。
【図３】領域分割マップの例を示すイメージ図である。
【図４】領域の種類毎に与えられる進入可能性の例を示す図である。
【図５】領域分割マップに分岐ノードを記録した様子を示すイメージ図である。
【図６】分岐ノードの分岐方向を示す図である。
【図７】（Ａ）位置分布の例を示すイメージ図、及び（Ｂ）位置分布が更新される様子を示すイメージ図である。
【図８】（Ａ）速度分布の例を示すイメージ図、（Ｂ）分岐方向ベクトルと速度ベクトルとを示す図、及び（Ｃ）速度分布を変更する様子を示すイメージ図である。
【図９】複数の分岐方向から分岐すべき方向を選択する様子を示す図である。
【図１０】分岐ノードによって位置分布が分割された後に更新された位置分布を示すイメージ図である。
【図１１】本発明の第１の実施の形態に係る衝突危険度判定装置における局所地図生成処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第１の実施の形態に係る衝突危険度判定装置における危険判定処理ルーチンの内容を示すフローチャートである。
【図１３】本発明の第１の実施の形態に係る衝突危険度判定装置における分岐ノード生成処理ルーチンの内容を示すフローチャートである。
【図１４】本発明の第２の実施の形態に係る衝突危険度判定装置を示すブロック図である。
【発明を実施するための形態】
【００２３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。なお、本実施の形態では、車両に搭載された衝突危険度判定装置に、本発明を適用した場合を例に説明する。
【００２４】
図１に示すように、第１の実施の形態に係る衝突危険度判定装置１０は、検出対象範囲である自車両の前方に対してレーザを１次元（水平方向）に走査しながら照射し、レーザの反射によりレーザが照射された物体の２次元位置を検出するレーザレーダ１２と、自車両の運動状態を検出する運動センサ１４と、自車両の前方を撮影するカメラ１８と、自車両の位置を検出するＧＰＳ装置２０と、これらの検出結果に基づいて、現在の自車両から見た前方の静止物の存在を記録した局所地図情報を生成すると共に、移動物の将来の位置及び移動状態を予測し、衝突する危険性があると判定された場合に表示装置２４に警告メッセージを表示させるように制御するコンピュータ２２とを備えている。
【００２５】
ここで、移動物とは、自ら移動する能力を持つか否かに関わらず、移動状態である物体である。例えば、自動車、自動二輪車、自転車、歩行者などである。
【００２６】
レーザレーダ１２は、車両前方に設置され、装置を基準とする車両前方に存在する物体までの距離を検出する装置であり、出力するレーザを水平方向に走査することで、レーザの反射により自車両前方に存在する複数の物体表面上の複数の点の位置を検出することができる。レーザレーダ１２による検出結果は、自車両前方に存在する物体表面のある点の位置を表す２次元座標の集合である。レーザレーダ１２による検出処理は一定サイクルで実行され、レーザレーダ１２は、各時点での自車両前方に存在する物体表面の複数の点の２次元位置を示すデータをコンピュータ２２に出力する。なお、レーザレーダ１２は、物体位置検出手段の一例である。
【００２７】
運動センサ１４は、自車両の速度を計測する車速センサ、ヨーレートを計測するジャイロセンサ、又は自車両の加速度を計測する加速度センサで構成されている。
【００２８】
カメラ１８は、小型のＣＣＤカメラ又はＣＭＯＳカメラで構成され、車両の前方を撮影するように車両のフロントウィンドウ上部等に取り付けられている。カメラ１８で撮影された前方の道路状況等の画像データは、コンピュータ２２に入力される。
【００２９】
コンピュータ２２は、ＣＰＵ、後述する局所地図生成処理ルーチン及び危険度判定処理ルーチンの各々を実行するためのプログラムを記憶したＲＯＭ、データ等を記憶するＲＡＭ、及びこれらを接続するバスを含んで構成されている。このコンピュータ２２をハードウエアとソフトウエアとに基づいて定まる機能実現手段毎に分割した機能ブロックで説明すると、図１に示すように、レーザレーダ１２により検出された物体の２次元位置を取得する物体位置取得部３０と、運動センサ１４により検出された車速、ヨーレート、又は加速度に基づいて、自車両の運動を推定する自車運動推定部３２と、自車両の運動に基づいて、一時刻前までの局所地図情報を現在の自車両から見た座標系の局所地図情報に更新する局所地図更新部３４と、検出された現在の物体の２次元位置に基づいて、更新された局所地図情報における静止物の存在確率を変更する存在確率変更部３６と、を備えている。なお、存在確率変更部３６は、静止物記録手段の一例である。
【００３０】
自車運動推定部３２は、局所地図情報が前回更新されてから現在までの間における、運動センサ１４により検出された車速、ヨーレート、又は加速度に基づいて、局所地図情報が前回更新されてから現在までの自車両の運動を推定する。
【００３１】
局所地図更新部３４は、一時刻前までに更新された局所地図情報を、推定された自車両の運動に従って、現在の自車から見た座標系へと変換することで、現在の自車両から見た前方領域を表わす局所地図情報を生成する。また、局所地図更新部３４は、局所地図情報の更新を繰り返し行う。局所地図情報は、図２（Ｂ）に示すように、自車両前方の一定領域内を、一定の大きさの格子（ブロック）によって分割した複数のブロックで表される地図である。各ブロックにはその位置に静止物が存在する確率が記録され、初期状態では、図２（Ｂ）に示すように、存在確率として初期値である０.５（中間値）が記録されている。
【００３２】
存在確率変更部３６は、局所地図更新部３４により局所地図情報が更新される毎に、物体位置取得部３０によって取得された現在の物体の２次元位置それぞれに基づいて、更新された局所地図情報において、その物体の２次元位置に対応するブロックの存在確率を増加させ、自車両からその物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させる。このように存在確率を変更することにより、一定期間以上、対応する位置で物体が検出されたブロックの存在確率は高くなり、物体が一時的にしか検出されないブロックの存在確率は低くなる。図２（Ａ）に示すような走行環境である場合、図２（Ｃ）に示すように、例えば、静止物が確実に存在する位置のブロックには存在確率として１（最大値）が記録され、静止物が存在しない位置のブロックには存在確率として０（最小値）が記録される。また、他の物体によって隠されているなどの原因によって、その位置の情報が得られていないブロック（死角領域のブロック）には、存在確率として初期値である０.５（中間値）が記録される。
【００３３】
この結果、多くの移動物が存在する走行環境においても、比較的長時間同じ位置に観測される路側物などの静止物のみを安定して検出可能である。また、現在までレーザレーダ１２により観測できていない部分（不可視領域）も、存在確率が初期値（０.５）のままのブロックとして検出可能である。また、レーザレーダ１２の計測点がほとんど得られない、自車両の位置から見て静止物が存在する領域より遠方側の領域は、初期値から変化しないので、死角領域とみなすことができる。
【００３４】
また、コンピュータ２２は、電子地図を記憶した地図データベース３８と、カメラ１８によって撮影された前方画像、ＧＰＳ装置２０によって検出された自車両の位置、及び電子地図に基づいて、自車両周辺の移動物及び走行環境の状況を検出する環境検出部４０と、局所地図情報及び走行環境状況の検出結果に基づいて、静止物の領域及び各種の走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する領域分割部４２と、領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成する分岐ノード生成部４４と、物体位置取得部３０によって取得された物体の２次元位置の時系列データ、局所地図情報、及び運動センサ１４の検出結果に基づいて、自車両も含む移動物の位置分布及び運動状態分布を表わす移動物データを生成して領域分割マップ上に記録する移動物生成部４６と、移動物の位置分布及び運動状態分布に基づいて、将来の位置分布を予測して、領域分割マップ上に記録された移動物データを更新する位置更新部４８と、移動物データの位置分布の近傍に分岐ノードが存在するか否かを判定する分岐判定部５０と、分岐ノードが存在する場合に、位置分布を複数に分割すると共に、分割された位置分布の各々に対応する運動状態分布を変更する移動物分割部５２と、移動物データの位置分布に基づいて、自車両と他の移動物とが衝突する危険性があるか否かを判定し、判定結果を表示装置２４に表示させる危険判定部５４とを備えている。
【００３５】
位置更新部４８、分岐判定部５０、移動物分割部５２、及び危険判定部５４による一連の処理は、繰り返し実行される。なお、環境検出部４０及び領域分割部４２は、領域検出手段の一例である。領域分割部４２は、マップ生成手段の一例である。分岐判定部５０及び移動物分割部５２は、分岐変更手段の一例であり、位置更新部４８は、予測手段の一例である。危険判定部５４は、危険度算出手段の一例である。
【００３６】
地図データベース３８に記憶されている電子地図には、道路形状や標識表示、建造物に関する情報が格納されている。
【００３７】
環境検出部４０は、ＧＰＳ装置２０によって検出された自車位置に基づいて、地図データベース３８の電子地図から、自車両周辺の道路形状や標識表示、建造物に関する情報を検出する。また、環境検出部４０は、カメラ１８によって撮影された前方画像から、学習型のパターン認識技術（例えば、ＳＶＭ）によって、前方に存在する移動物及び移動物の種類（例えば、歩行者、二輪車、自動車等）を検出すると共に、前方の道路における各種の走路区分の種別（車線、歩道、横断歩道、信号、一時停止線など）及び領域を検出する。また、カメラ１８によって撮影された前方画像から、静止物の高さを検出する。
【００３８】
領域分割部４２は、局所地図情報において、静止物（例えば、ガードレール、植込み、建物、駐停車車両など）が存在する領域を検出し、検出された静止物の領域と、環境検出部４０によって検出された各走行領域の領域とに基づいて、図３に示すように、自車周辺領域を走路区分及び静止物により領域分割した領域分割マップを生成する。また、各分割領域に対して、図４に示すように、走路区分の種類及び静止物に対して予め定めた侵入可能性を表す数値（例えば０〜１までの確率値）を付与する。また、静止物の領域に対しては、環境検出部４０により検出された静止物の高さに基づき、侵入可能性を表す数値を付与してもよい。例えば、高さｈが、０＜ｈ＜１．０ｍである場合には、１．０−ｈを、進入可能性として付与し、ｈ＞１．０ｍである場合には、０を進入可能性として付与する。また、走路区分の種類に応じた進入可能性については、移動物の種類ごとに変えた侵入可能性を表す数値が付与される。また、信号などにより侵入可能性が時間的に変化する場合には、交通環境検出時の状況に従うようにしてもよい。
【００３９】
分岐ノード生成部４４は、以下に説明するように、領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成する。分岐ノードは、移動物の種類毎に生成され、ここでは、歩行者用の分岐ノードを生成する場合を例に説明する。車両用の分岐ノードについても同様にして求めることができる。
【００４０】
まず、領域分割マップにおいて、図５に示すように、隣接する分割領域との境界線上の中点付近に分岐ノードを付与する。ある領域に関して、領域境界線上の任意の点から反時計回りに領域境界線上で探索点を移動させ、右側隣接領域の切り替わりを確認しながら分岐ノードを生成する。なお、ここでの例では、隣接領域境界線上の中点付近にただ一つの分岐ノードを生成しているが、隣接領域境界線の長さに応じて分岐ノードの個数と間隔を変えてもよい。
【００４１】
次に、各分岐ノードに対して、図６に示すように、分岐ノードが存在する領域境界に隣接する領域に移動物の種類毎に付与された進入可能性に基づいて、分岐し得る方向、および移動物の種類毎の分岐割合（分岐可能性）を付与する。
【００４２】
ここでは、分岐しうる方向として、各分岐ノード位置おける領域境界線の単位法線ベクトルと単位接線ベクトルの４方向を与える。単位接線ベクトルは、分岐ノード位置を求めた際に、分岐ノード位置周辺での傾きを算出することで求める。あるいは、領域境界が曲線の方程式としてｆ（ｘ，ｙ）＝０の形で与えられている場合には、従来既知の手法により、数学的に解析して曲線の法線ベクトルを１つ求める。いずれかの方法で求めた法線ベクトルまたは接線ベクトルを回転することにより、残りの３方向のベクトルを求める。
【００４３】
また、移動物の種類の各々について、分割割合は、全ての分割し得る方向に対してその合計が１．０になるように以下のように与える。
【００４４】
まず、各分岐方向に対して、当該移動物の種類に対応する分割割合の初期値として次のような値を与える。分岐方向が法線方向である場合には、領域境界から分岐方向側にある領域に付与された、当該移動物の種類に対応する侵入可能性を示す値を与え、分岐方向が接線方向である場合には、領域境界で隣接する領域に付与された、当該移動物の種類に対応する侵入可能性を示す値のうち、大きい方の値を与える。
【００４５】
そして、移動物の種類毎に、各方向に与えられた初期値の合計値で正規化し、合計が１．０となるようにする。
【００４６】
例えば、上記図６に示すように、分岐しうる４方向に対して、分岐割合ａ１〜ａ４を与える場合には、まず、初期値として、ａ１ ＝ ０．８、ａ２ ＝ ｍａｘ（１．０，０．８）＝１．０、ａ３＝１．０、ａ４＝ｍａｘ（１．０，０．８）＝１．０を与える。そして、全方向で正規化することにより、ａ１＝０．８／（０．８＋１．０＋１．０＋１．０）≒０．２１１、ａ２＝ａ３＝ａ４＝１．０／（０．８＋１．０＋１．０＋１．０）≒０．２６３を与える。
【００４７】
移動物生成部４６は、連続する局所地図情報の時間差分をとることによって、移動物と静止物とを識別し、局所地図情報上において移動物が存在する領域を特定する。また、移動物生成部４６、特定された移動物が存在する領域と、環境検出部４０で移動物の種類が検出された領域とを統合することによって、移動物に対して、移動物の種類を与えると共に、ＩＤを与える。
【００４８】
また、移動物生成部４６は、物体位置取得部３０によって取得された物体の２次元位置の時系列データに基づいて、図７（Ａ）及び図８（Ａ）に示すように、移動物の位置および運動状態（例えば速度）を、自車中心ｘ−ｙ座標空間での２次元正規分布（平均ベクトルと分散行列）で表現し、移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。また、移動物生成部４６は、運動センサ１４によって検出された車速に基づいて、自車両の速度分布を生成し、自車両の位置分布及び速度分布を表わす移動物データを生成し、領域分割マップ上の自車位置に記録する。
【００４９】
移動物の位置分布は、以下の（１）式によって表される。
【００５０】
【数１】
【００５１】
ただし、μｐｏｓは、位置平均ベクトルであり、以下の（２）式によって表され、Σｐｏｓは、位置共分散行列であり、以下の（３）式によって表される。
【００５２】
【数２】
【００５３】
また、速度分布は、以下の（４）式によって表される。
【００５４】
【数３】
【００５５】
ただし、μｖｅｌは、速度平均ベクトルであり、以下の（５）式によって表され、Σｖｅｌは、速度共分散行列であり、以下の（６）式によって表される。
【００５６】
【数４】
【００５７】
位置更新部４８は、図７（Ｂ）に示すように、移動物の位置分布及び速度分布を用いて、Δｔ秒後の移動物の位置分布を線形予測によって求める。
【００５８】
更新後の位置分布の位置平均ベクトルμｖｅｌ’及び位置共分散行列Σｖｅｌ’は、以下の（７）式、（８）式で表される。
【００５９】
【数５】
【００６０】
分岐判定部５０は、更新後の移動物の位置分布の中心（平均）と各分岐ノード位置のマハラノビス距離を、以下の（９）式に従って算出し、予め定めた閾値Ｔｄｉｓｔ以下の距離を持つ分岐ノードが存在する場合、近傍に分岐ノードが存在すると判定する。
【００６１】
【数６】
【００６２】
また、分岐判定部５０は、近傍に分岐ノードが存在すると判定した場合、予め定めた閾値Ｔｄｉｓｔ以下の距離を持つ分岐ノードのうち、最小のマハラノビス距離を持つ分岐ノードを選択する。
【００６３】
移動物分割部５２は、近傍に分岐ノードが存在すると判定された移動物について、以下に説明するように、分岐ノード情報に従って移動物の位置分布を分割して複数の位置分布を生成し、分岐方向に応じて分割後の速度分布を変更する。
【００６４】
まず、移動物の平均速度ベクトルの単位ベクトルと分岐方向ベクトルとの内積値が、予め定めたＡ以上となる分岐方向ベクトルを、分岐すべき方向として選択する。
【００６５】
例えば、ｉ番目の分岐方向ベクトルｄｉを、以下の（１０）式で表し、移動物の平均速度ベクトルの単位ベクトルｅｖｅｌを、以下の（１１）式で表わすと、以下の（１２）式を満たす分岐方向ベクトルｄｉが選択される。
【００６６】
【数７】
【００６７】
上記（１２）式により、図９に示すように、後戻りとならない分岐方向が選択される。
【００６８】
次に、以下の（１３）式に示すように、選択した分岐方向ベクトルが持つ分割割合ａｉ’を正規化するように変更する。これによって、分岐すべき方向として選択した分岐方向ベクトルが持つ分割割合の合計が１．０になる。
【００６９】
【数８】
【００７０】
次に、選択された各分岐方向が持つ分割割合ａｉ’に従い、移動物の位置分布を分割して重み付けを行い、加算することで、分割後の位置分布を求める。このとき、分割された位置分布の各々に対して、移動物の種類を示すラベルとＩＤとを複製する。分割後の位置分布は、以下の（１４）式で表される。
【００７１】
【数９】
【００７２】
また、分割された移動物の位置分布に対応する速度分布として、元の速度分布を分岐方向に回転したものを与える。回転行列Ｒを以下の（１５）式で表わすと、回転後の速度平均ベクトルμｖｅｌ’及び位置共分散行列Σｖｅｌ’は、以下の（１６）式、（１７）式で表される。
【００７３】
【数１０】
【００７４】
例えば、図８（Ｂ）に示すように、ｉ番目の分岐方向への回転角度をθとする（反時計回りが正）と、元の速度分布をθ分だけ回転して、図８（Ｃ）に示すような速度分布が得られる。これにより、平均速度の大きさ／分散を維持して、速度方向だけが変わる。すなわち、元の運動量を保ったまま方向だけが、分岐方向を向くように変更される。
【００７５】
なお、同一分岐ノードで再度分割しないように、ここで利用した分岐ノードを記憶しておいてもよい。
【００７６】
上記のように、分割領域が持つ、移動物の種類に対応する進入可能性に基づく分割割合に応じて、移動物の位置分布が分割され、分岐方向に応じて分割後の速度分布が変更された場合には、位置更新部４８における次回の処理において、図１０に示すように、分割された位置分布が、分岐方向に応じて移動するように更新される。
【００７７】
危険判定部５４は、自車両の位置分布と、他の移動物の位置分布とを用いて、危険度として衝突確率を算出する。衝突確率は、同一将来時刻における同一位置に存在する確率（同時確率）であり、以下のようにして求めることができる。
【００７８】
同時確率（衝突確率）は、自車と他の移動物の位置分布を掛け合わせたものを、衝突範囲に渡って積分した値として求めることができる（一般にこの積分値は数値積分によって求める）。本実施例では、積分範囲として、領域分割マップの全体を用いる。
【００７９】
移動物の分割及び移動の結果、自車両および他の移動物の位置分布の各々は、複数個の重み付き分布の和で表されており、求める積分値は、自車両および他の移動物の分割された個々の位置分布の全ての組合せの積分値の和として求める。
【００８０】
例えば、歩行者の位置分布を以下の（１８）式で表わす。
【００８１】
【数１１】
【００８２】
ただし、ａｉは、分割されたｉ番目の分布に対する正規化された分割割合であり、Ｐｅｄｉ（ｘ）は、分割されたｉ番目の分布である。
【００８３】
また、自車両の位置分布を、以下の（１９）式で表わす。
【００８４】
【数１２】
【００８５】
ただし、ｂｊは、分割されたｊ番目の分布に対する正規化された分割割合であり、Ｅｇｏｊ（ｘ）は、分割されたｊ番目の分布である。
【００８６】
このとき、衝突確率は、以下の（２０）式に従って算出される。
【００８７】
【数１３】
【００８８】
ただし、Ｄは、積分範囲（２次元領域）であり、領域分割マップの全領域である。
【００８９】
また、危険判定部５４は、算出した衝突確率が、閾値以上であるか否かを判定し、衝突確率が閾値以上となる移動体が存在する場合には、当該移動物の将来予測位置及び警告情報等を、表示装置２４に表示させて、ドライバに注意喚起を行う。例えば、フロントガラスに当該移動物の将来予測位置及び警告情報等を重畳表示する。
【００９０】
位置更新部４８、分岐判定部５０、移動物分割部５２、危険判定部５４による一連の処理は、予め設定された予測時間分だけ繰り返し実行され、各予測ステップにおいて、各移動物について、近傍に分岐ノードが存在する場合、近傍に存在する分岐ノードに応じて、位置分布の分割及び速度分布の変更が行われて、各移動物の位置分布が移動する。また、各予測ステップにおいて、自車両と各移動物との衝突確率が算出され、衝突確率が閾値以上となった場合には、その予測ステップにおける当該移動物の将来予測位置、及び危険度情報等が表示装置２４によって表示される。
【００９１】
次に、本実施の形態に係る衝突危険度判定装置１０の作用について説明する。
【００９２】
まず、レーザレーダ１２によって、レーザが自車両の前方を水平方向に走査されて、走査方向に並んだレーザ照射位置としての物体の２次元位置の各々までの距離が計測され、自車両の前方に存在する物体の２次元位置が検出される。レーザレーダ１２によって検出される２次元位置は、レーザを走査する毎に得られる。
【００９３】
そして、コンピュータ２２によって、図１１に示す局所地図生成処理ルーチンが実行される。
【００９４】
まず、ステップ１００において、レーザレーダ１２から、前方に存在する物体の２次元位置（走査方向に並んだ各２次元位置までの計測距離）を示すデータを取得し、ステップ１０２において、一時刻前から現在までの間における、運動センサ１４により検出された車速、ヨーレート、又は加速度を取得し、取得した車速、ヨーレート、又は加速度に基づいて、一時刻前から現在までの自車両の運動を推定する。
【００９５】
そして、ステップ１０４において、後述するステップ１０６で前回更新された局所地図情報を、上記ステップ１０２で推定された自車両の運動に応じて、現在の自車両から見た座標系で表わされる局所地図情報に更新する。
【００９６】
次のステップ１０６では、上記ステップ１００で取得した自車両前方に存在する物体の２次元位置に基づいて、上記ステップ１０４で更新された局所地図情報において、自車両前方に存在する物体の２次元位置に対応するブロックの存在確率を増加させると共に、自車両から物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させることにより現在物体が存在しない位置を地図上に記録する。そして、上記ステップ１００へ戻る。
【００９７】
上述したように、局所地図生成処理ルーチンが繰り返し実行されることにより、現在の自車両から見た静止物の位置が随時推定される。
【００９８】
なお、生成される局所地図情報は、上記局所地図生成処理ルーチンが所定回数繰り返されたときに有効とされるようにしてもよい。
【００９９】
また、コンピュータ２２によって、図１２に示す危険判定処理ルーチンが実行される。まず、ステップ１２０において、カメラ１８により撮影された前方画像及びＧＰＳ装置２０によって検出された自車両位置を取得する。次のステップ１２２では、上記ステップ１２０で取得した前方画像、自車両位置、及び地図データベース３８の電子地図に基づいて、自車両周辺の走行環境の状況を検出して、各種の走路区分の領域を検出すると共に、周辺の移動物、移動物の運動状態、移動物の種類、及び静止物の高さを検出する。
【０１００】
そして、ステップ１２４において、上述した局所地図生成処理ルーチンにより得られる現在の局所地図情報を取得する。次のステップ１２５で、上記ステップ１２４で取得した局所地図情報に基づいて、静止物の領域を検出し、ステップ１２６において、上記ステップ１２２で検出した各種走路区分の領域及び上記ステップ１２５で検出した静止物の領域に基づいて、静止物の領域及び走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する。
【０１０１】
そして、ステップ１２８において、上記ステップ１２６で生成した領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成し、ステップ１３０で、レーザレーダ１２から取得された物体の２次元位置の時系列データ、上記ステップ１２４で取得した局所地図情報、及び運動センサ１４の検出結果に基づいて、自車両も含む移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。
【０１０２】
次のステップ１３２では、予測ステップを識別する変数ｎに、初期値１を設定する。ステップ１３４では、上記ステップ１３０又は後述するステップ１３８で得られた位置分布及び速度分布に基づいて、自車両も含む各移動物について、予測ステップｎにおける移動物の位置分布を予測して、各移動物の位置分布を更新する。
【０１０３】
そして、ステップ１３６において、自車両も含む各移動物について、上記ステップ１３４で更新された位置分布の中心との距離が閾値以下となる分岐ノードが存在するか否かを判定する。該当する分岐ノードが存在しない場合には、後述するステップ１４０へ移行する。一方、該当する分岐ノードが存在する場合には、ステップ１３８において、当該分岐ノードの近傍に存在する移動物について、分岐ノードに基づいて位置分布を分割すると共に、対応する速度分布を変更する。
【０１０４】
そして、ステップ１４０において、自車両の位置分布と、他の移動物の各々の位置分布とに基づいて、衝突危険度として、他の移動物との衝突確率を各々算出する。ステップ１４２では、上記ステップ１４０で算出された衝突確率が、閾値以上となる移動物が存在するか否かを判定する。該当する移動物が存在しない場合には、ステップ１４６へ移行する。一方、該当する移動物が存在する場合には、ステップ１４４において、該当する移動物の将来予測位置及び警告情報を表示装置２４に表示させる。
【０１０５】
ステップ１４６では、予測ステップを示す変数ｎが、将来予測時間に対応するステップ数Ｎに到達したか否かを判定する。変数ｎが、定数Ｎに到達していない場合には、ステップ１４８において、変数ｎをインクリメントして、上記ステップ１３４へ戻り、上記ステップ１３４以降の処理を繰り返す。一方、変数ｎが、定数Ｎに到達した場合には、危険判定処理ルーチンを終了する。
【０１０６】
上記ステップ１２８の処理は、図１３に示す分岐ノード生成処理ルーチンによって実現される。分岐ノード生成処理ルーチンは、領域分割マップの分割領域毎に実行される。
【０１０７】
まず、ステップ１６０において、処理対象の分割領域の境界線上の任意の点を探索点として設定すると共に、反時計回り方向に対して、当該探索点の右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。そして、ステップ１６２において、境界線上を反時計回りに探索点をΔｄだけ進める。次のステップ１６４では、現在の探索点の右側に隣接する分割領域ｉｄと記憶された分割領域ｉｄとに基づいて、右側に隣接する分割領域が切り替わったか否かを判定する（ａ！＝ｒａ＿ｉｄ？）。右側に隣接する分割領域が切り替わっていない場合には、上記ステップ１６２へ戻るが、一方、右側に隣接する分割領域が切り替わった場合には、ステップ１６６において、現在の探索点（ｐｘ，ｐｙ）を、開始点として記憶する（ｓｘ＝ｐｘ、ｓｙ＝ｐｙ）と共に、右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。また、累積距離Ｄを初期値すると共に（Ｄ＝０）、分岐ノード生成フラグを初期化する（ｆｌａｇ＝ｆａｌｓｅ）。
【０１０８】
次のステップ１６８では、境界線上を反時計回りに探索点をΔｄだけ進めると共に、累積距離を求める（Ｄ＝Ｄ＋Δｄ）。そして、ステップ１７０において、現在の探索点から−Δｄの範囲内に既に分岐ノードが存在するか否かを判定する。分岐ノードが存在する場合には、分岐ノード生成フラグをオンにして（ｆｌａｇ＝ｔｒｕｅ）、ステップ１７４へ移行する。一方、分岐ノードが存在しない場合には、そのままステップ１７４へ移行する。
【０１０９】
ステップ１７４では、現在の探索点の右側に隣接する分割領域ｉｄと記憶された分割領域ｉｄとに基づいて、右側に隣接する分割領域が切り替わったか否かを判定する（ａ！＝ｒａ＿ｉｄ？）。右側に隣接する分割領域が切り替わっていない場合には、上記ステップ１６８へ戻るが、一方、右側に隣接する分割領域が切り替わった場合には、ステップ１７６において、分岐ノード生成フラグに基づいて、既に分岐ノードが存在するか否かを判定する（ｆｌａｇ＝＝ｔｒｕｅ？）。分岐ノード生成フラグがオフである場合には、分岐ノードが存在しないと判断し、ステップ１７８において、累積距離Ｄの１／２分だけ戻った位置を分岐ノード位置として分岐ノードを生成すると共に、領域境界及び領域境界で隣接する分割領域の各々の進入可能性に基づいて、分岐し得る方向および分岐割合を求めて分岐ノードに与えて、ステップ１８０へ移行する。
【０１１０】
一方、上記ステップ１７６において、既に分岐ノードが存在する場合には、そのままステップ１８０へ移行する。
【０１１１】
ステップ１８０では、右側に隣接する分割領域ｉｄをメモリに記憶する（ａ＝ｒａ＿ｉｄ）。また、累積距離Ｄを初期値すると共に（Ｄ＝０）、分岐ノード生成フラグを初期化する（ｆｌａｇ＝ｆａｌｓｅ）。そして、ステップ１８２において、探索点が、開始点を越えたか否かを判定する。現在の探索点から−Δｄの範囲内に開始点（ｓｘ、ｓｙ）が存在する場合には、開始点を越えたと判断して、分岐ノード生成処理ルーチンを終了するが、一方、現在の探索点から−Δｄの範囲内に開始点（ｓｘ、ｓｙ）が存在しない場合には、開始点を越えていないと判断して、上記ステップ１６８へ戻り、上記ステップ１６８以降の処理を繰り返す。
【０１１２】
以上説明したように、第１の実施の形態に係る衝突危険度判定装置によれば、分割領域の境界上に生成された分岐ノードの分岐すべき方向の数だけ、移動物の位置分布を生成すると共に、対応する速度分布の各々を、各分岐すべき方向に応じて変更し、移動物の将来の位置分布を予測するようにすることにより、移動物の将来の位置分布形状が複雑な形状であっても、計算量の増大を抑制することができる。複雑な形状となる移動物の将来の位置分布を用いて、他の移動物との衝突危険度を精度良く算出することができる。
【０１１３】
また、移動物の将来位置分布を求める際に混合正規分布表現を用いることにより、繰り返し計算を伴わず位置分布を求めることができるため、複数経路が考えられる移動物の将来位置推定において、モンテカルロ法に代表されるような繰り返しを伴うランダムサンプリング手法に比べ、処理速度を向上させることができる。
【０１１４】
また、移動物の将来位置分布を求める際に混合正規分布表現を用いることにより、単一の確率分布を用いるよりも複雑な形状の位置分布を推定することができるため、単純な線形予測によるものに比べ、移動物の位置推定精度が向上する。
【０１１５】
他者との衝突危険度を推定する際には、これら移動物の将来位置予測が重要な要素となる。しかしながら、移動物の将来位置の予測には想定すべき挙動・制約の組み合わせが多く、将来位置分布を効率的に求めることは困難である。そこで、従来技術では、モンテカルロ法に代表されるランダムサンプリングの繰り返し演算により分布を求めており、甚大な計算量の増加により、実時間処理に問題があった。また、ランダムサンプリングを行わない手法では、正規分布などの確率分布を用い線形予測により将来位置予測を求めている。この手法では、横断歩道などでの歩行者分岐の可能性に対して対応できず、位置予測の精度が悪くなってしまう。本実施の形態では、他者の将来位置予測にあたり、交通環境の物理的制約・交通規則の制約に基づいた分岐ノードの情報を生成した。また、移動物の位置分布にパラメトリックな分布を用いることで計算コストを抑えつつ、歩道、車道、横断歩道などの異なる通行帯境界に付与した分岐ノードの移動物の分岐方向・分岐割合の情報に基づきパラメトリックな分布を分割することにより、環境に応じた複雑な位置分布形状となる将来位置予測を効率的かつ高精度に行うことができる。
【０１１６】
なお、上記の第１の実施の形態では、運動センサの検出結果を用いて、自車両の運動を推定する場合を例に説明したが、これに限定されるものではなく、レーザレーダの検出結果の時間差分によって、自車両の運動を推定してもよい。また、例えば、ＧＰＳ装置による自車位置の検出結果を用いて自車両の運動を推定するようにしてもよい。また、レーザレーダ、運動センサ、及びＧＰＳ装置の検出結果を組み合わせて、自車両の運動を推定するようにしてもよい。
【０１１７】
次に、第２の実施の形態に係る衝突危険度判定装置について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１１８】
第２の実施の形態では、路側の装置において、移動物の将来分布を予測して、衝突する危険性を判定している点が、第１の実施の形態と主に異なっている。
【０１１９】
第２の実施の形態に係る衝突危険度判定装置は、路側に固定して設置されており、例えば、主要交差点に設置されている。図１４に示すように、衝突危険度判定装置２１０は、判定対象範囲である自装置の前方に対してレーザを１次元（水平方向）に走査しながら照射し、レーザの反射によりレーザが照射された物体の２次元位置を検出するレーザレーダ１２と、自装置の前方を撮影するカメラ１８と、自装置の位置を検出するＧＰＳ装置２０と、これらの検出結果に基づいて、現在の自装置から見た前方の静止物の存在を記録した局所地図情報を生成すると共に、移動物の将来の位置及び移動状態を予測し、予測結果に応じて衝突する危険性があると判定された場合に、警告メッセージを表示させる表示装置を搭載した車両において警告メッセージを表示させる指令を通信装置２２４に送信させるコンピュータ２２２とを備えている。
【０１２０】
コンピュータ２２２は、物体位置取得部３０と、検出された現在の物体の２次元位置に基づいて、自装置から見た座標系の局所地図情報における静止物の存在確率を変更する存在確率変更部２３６と、を備えている。
【０１２１】
また、コンピュータ２２２は、地図データベース３８と、自装置周辺の移動物及び走行環境の状況を検出する環境検出部４０と、領域分割部４２と、分岐ノード生成部４４と、移動物生成部４６と、位置更新部４８と、分岐判定部５０と、移動物分割部５２と、移動物データの位置分布に基づいて、各移動物について、他の移動物と衝突する危険性を判定し、表示装置を搭載した車両において判定結果を表示させるように指令を送信させる危険判定部２５４とを備えている。
【０１２２】
次に、第２の実施の形態に係る局所地図情報処理ルーチンでは、レーザレーダ１２から、前方に存在する物体の２次元位置を示すデータを取得し、次に、取得した自装置前方に存在する物体の２次元位置に基づいて、局所地図情報において、自装置前方に存在する物体の２次元位置に対応するブロックの存在確率を増加させると共に、自装置から物体の２次元位置までの直線上に存在する各ブロックの存在確率を減少させることにより現在物体が存在しない位置を地図上に記録する。そして、最初の処理へ戻る。
【０１２３】
上述したように、局所地図生成処理ルーチンが繰り返し実行されることにより、現在の自装置から見た静止物の位置が随時推定される。
【０１２４】
また、第２の実施の形態に係る危険判定処理ルーチンでは、まず、カメラ１８により撮影された前方画像及びＧＰＳ装置２０によって検出された自装置位置を取得する。次に、上記で取得した前方画像、自装置位置、及び地図データベース３８の電子地図に基づいて、自装置周辺の走行環境の状況を検出して、各種の走路区分の領域を検出すると共に、周辺の移動物、移動物の運動状態、移動物の種類、及び静止物の高さを検出する。
【０１２５】
そして、上述した局所地図生成処理ルーチンにより得られる現在の局所地図情報を取得する。次に、取得した局所地図情報に基づいて、静止物の領域を検出し、上記で検出した各種走路区分の領域及び上記で検出した静止物の領域に基づいて、静止物の領域及び走路区分の領域に分割した領域分割マップを生成し、各分割領域に進入可能性を付与する。
【０１２６】
そして、上記で生成した領域分割マップにおいて隣接する分割領域の境界上に分岐ノードを生成し、レーザレーダ１２から取得された物体の２次元位置の時系列データ、及び上記で取得した局所地図情報に基づいて、領域分割マップ上に、各移動物の位置分布及び速度分布を表わす移動物データを生成して、領域分割マップ上に記録する。
【０１２７】
次に、予測ステップを識別する変数ｎに、初期値１を設定し、上記で得られた、又は変更された位置分布及び速度分布に基づいて、各移動物について、予測ステップｎにおける移動物の位置分布を予測して、各移動物の位置分布を更新する。
【０１２８】
そして、各移動物について、上記で更新された位置分布の中心との距離が閾値以下となる分岐ノードが存在するか否かを判定し、該当する分岐ノードが存在する場合には、当該分岐ノードの近傍に存在する移動物について、位置分布を分割すると共に、対応する速度分布を変更する。
【０１２９】
そして、各移動物を判定対象として、判定対象の移動物の位置分布と、他の移動物の各々の位置分布とに基づいて、衝突危険度として、他の移動物との衝突確率を各々算出する。次に、上記で算出された衝突確率が、閾値以上となる移動物が存在するか否かを判定する。該当する移動物が存在する場合には、判定対象の移動物に搭載された表示装置に、該当する移動物の将来予測位置及び警告情報を表示させる指令を、通信装置２２４により当該判定対象の移動物へ送信する。
【０１３０】
予測ステップを示す変数ｎが、将来予測時間に対応するステップ数Ｎに到達するまで、上記の処理を繰り返し、変数ｎが、定数Ｎに到達した場合には、危険判定処理ルーチンを終了する。
【０１３１】
以上説明したように、第２の実施の形態に係る衝突危険度判定装置によれば、路側に設置された装置において、複雑な形状となる移動物の将来の位置分布を用いて、周辺の判定対象の移動物と他の移動物との衝突危険度を精度良く算出することができる。
【０１３２】
なお、上記の第１の実施の形態〜第２の実施の形態では、レーザレーダによりレーザを前方に走査して物体の位置を検出する場合を例に説明したが、これに限定されるものではなく、ミリ波などの電磁波を前方に走査して物体の位置を検出するようにしてもよい。
【０１３３】
また、レーザレーダによって自車両又は自装置の前方の物体の位置を検出する場合を例に説明したが、これに限定されるものではなく、例えば、ステレオカメラによって撮影された前方画像から、自車両又は自装置の前方の物体の位置を検出するようにしてもよい。
【０１３４】
また、領域分割マップの各分割領域に、進入可能性を付与し、付与された進入可能性に基づいて、分岐ノードの分割割合を求める場合を例に説明したが、これに限定されるものではなく、領域の種類毎に進入可能性を定めたテーブルを用意しておき、領域分割マップの各分割領域の種類と、上記のテーブルとに基づいて、分岐ノードの分割割合を求めるようにしてもよい。
【０１３５】
また、カメラによって撮影された前方画像から、走路区分の種類及び領域を検出する場合を例に説明したが、これに限定されるものではなく、走路区分の種類及び領域を示す情報を含む電子地図情報と、ＧＳＰによって検出した自車位置又は自装置の位置とに基づいて、走路区分の種類及び領域を検出するようにしてもよい。また、前方画像からの検出結果と組み合わせて、走路区分の種類及び領域を検出するようにしてもよい。
【０１３６】
また、事故シミュレータにおいて、本発明の技術を適用するようにしてもよい。例えば、進入可能性及び分岐ノードが与えられた領域分割マップにおいて、複数の移動物データを生成して記録し、複数の移動物データの位置分布を、分割ノードに応じて分割すると共に速度分布を変更し、位置分布を更新していくことで、移動物による事故状況をシミュレーションするようにしてもよい。
【０１３７】
本発明のプログラムを、記憶媒体に格納して提供することも可能である。
【符号の説明】
【０１３８】
１０、２１０ 衝突危険度判定装置
１２ レーザレーダ
１４ 運動センサ
１８ カメラ
２０ ＧＰＳ装置
２２、２２２ コンピュータ
３２ 自車運動推定部
３４ 局所地図更新部
３６、２３６ 存在確率変更部
３８ 地図データベース
４０ 環境検出部
４２ 領域分割部
４４ 分岐ノード生成部
４６ 移動物生成部
４８ 位置更新部
５０ 分岐判定部
５２ 移動物分割部
５４、２５４ 危険判定部
【特許請求の範囲】
【請求項１】
検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段と、
前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段と、
前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段と、
前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段と、
前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段と、
前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段と、
前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段と、
を含む移動物予測装置。
【請求項２】
前記分岐変更手段は、前回予測された前記移動の位置の確率分布及び移動状態の確率分布に基づいて、前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更することを繰り返し行い、
前記予測手段は、前記分岐変更手段によって変更された前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を繰り返し予測する請求項１記載の移動物予測装置。
【請求項３】
算出対象の移動物の位置の確率分布と、前記算出対象の移動物以外の移動物の位置の確率分布とに基づいて、前記算出対象の移動物と前記算出対象の移動物以外の移動物との衝突危険度を算出する危険度算出手段を更に含む請求項１又は２記載の移動物予測装置。
【請求項４】
前記移動物の位置の確率分布及び移動状態の確率分布の各々を、正規分布で表し、
前記生成された前記移動物の位置の複数の確率分布を、混合正規分布で表した請求項１〜請求項３の何れか１項記載の移動物予測装置。
【請求項５】
前記領域検出手段は、前記検出対象範囲から、前記静止物の領域及び高さを検出し、
前記静止物の領域の進入可能性を、前記検出された該静止物の高さに基づいて定めた請求項１〜請求項４の何れか１項記載の移動物予測装置。
【請求項６】
前記移動物検出手段は、前記検出対象範囲から、前記移動物の位置の確率分布及び移動状態の確率分布と共に、前記移動物の種類を検出し、
前記走路区分の領域の進入可能性を、前記移動物の種類毎に定めた請求項１〜請求項５の何れか１項記載の移動物予測装置。
【請求項７】
自装置を搭載した移動物から見て、前記検出対象範囲に存在する物体の位置を検出する物体位置検出手段と、
前記自装置を搭載した移動物の運動を推定する運動推定手段と、
前回更新された前記地図情報を、前記運動推定手段によって推定された前記自装置を搭載した移動物の現在の運動に基づいて、現在の前記自装置を搭載した移動物から見た前記地図情報に繰り返し更新する地図更新手段と、
前記地図更新手段によって前記地図情報が更新される毎に、前記更新された地図情報に、前記物体位置検出手段によって検出された現在の物体の位置に対応するブロックに、前記静止物の存在を記録すると共に、前記自装置を搭載した移動物から前記検出された現在の物体の位置までの間に対応する各ブロックにおける前記静止物の存在の記録を減少させる静止物記録手段と、を更に含み、
前記領域検出手段は、前記地図情報に基づいて、前記静止物の領域を検出する請求項１〜請求項６の何れか１項記載の移動物予測装置。
【請求項８】
コンピュータを、
検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段、
前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段、
前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段、
前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段、
前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段、
前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段、及び
前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段
として機能させるためのプログラム。
【請求項１】
検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段と、
前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段と、
前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段と、
前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段と、
前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段と、
前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段と、
前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段と、
を含む移動物予測装置。
【請求項２】
前記分岐変更手段は、前回予測された前記移動の位置の確率分布及び移動状態の確率分布に基づいて、前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更することを繰り返し行い、
前記予測手段は、前記分岐変更手段によって変更された前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を繰り返し予測する請求項１記載の移動物予測装置。
【請求項３】
算出対象の移動物の位置の確率分布と、前記算出対象の移動物以外の移動物の位置の確率分布とに基づいて、前記算出対象の移動物と前記算出対象の移動物以外の移動物との衝突危険度を算出する危険度算出手段を更に含む請求項１又は２記載の移動物予測装置。
【請求項４】
前記移動物の位置の確率分布及び移動状態の確率分布の各々を、正規分布で表し、
前記生成された前記移動物の位置の複数の確率分布を、混合正規分布で表した請求項１〜請求項３の何れか１項記載の移動物予測装置。
【請求項５】
前記領域検出手段は、前記検出対象範囲から、前記静止物の領域及び高さを検出し、
前記静止物の領域の進入可能性を、前記検出された該静止物の高さに基づいて定めた請求項１〜請求項４の何れか１項記載の移動物予測装置。
【請求項６】
前記移動物検出手段は、前記検出対象範囲から、前記移動物の位置の確率分布及び移動状態の確率分布と共に、前記移動物の種類を検出し、
前記走路区分の領域の進入可能性を、前記移動物の種類毎に定めた請求項１〜請求項５の何れか１項記載の移動物予測装置。
【請求項７】
自装置を搭載した移動物から見て、前記検出対象範囲に存在する物体の位置を検出する物体位置検出手段と、
前記自装置を搭載した移動物の運動を推定する運動推定手段と、
前回更新された前記地図情報を、前記運動推定手段によって推定された前記自装置を搭載した移動物の現在の運動に基づいて、現在の前記自装置を搭載した移動物から見た前記地図情報に繰り返し更新する地図更新手段と、
前記地図更新手段によって前記地図情報が更新される毎に、前記更新された地図情報に、前記物体位置検出手段によって検出された現在の物体の位置に対応するブロックに、前記静止物の存在を記録すると共に、前記自装置を搭載した移動物から前記検出された現在の物体の位置までの間に対応する各ブロックにおける前記静止物の存在の記録を減少させる静止物記録手段と、を更に含み、
前記領域検出手段は、前記地図情報に基づいて、前記静止物の領域を検出する請求項１〜請求項６の何れか１項記載の移動物予測装置。
【請求項８】
コンピュータを、
検出対象範囲から、移動物の位置の確率分布及び移動状態の確率分布を検出する移動物検出手段、
前記検出対象範囲から、複数種類の走路区分の領域及び静止物の領域を検出する領域検出手段、
前記領域検出手段によって検出された前記走路区分の領域及び前記静止物の領域に応じて、前記検出対象範囲の領域を分割した領域分割マップを生成するマップ生成手段、
前記領域分割マップにおいて、隣接する異なる領域間の境界上に、複数の分岐方向を含む分岐ノードを生成する分岐ノード生成手段、
前記領域分割マップにおいて、前記移動物検出手段によって検出された前記移動物の位置の確率分布を、前記移動状態の確率分布と共に記録する移動物記録手段、
前記領域分割マップにおいて前記移動物の位置の確率分布の中心との距離が所定値以下となる前記分岐ノードが存在する場合、前記分岐ノードの分岐方向のうち分岐すべき方向の数だけ、該移動物の位置の確率分布を生成すると共に、前記分岐ノードが存在する境界で隣接する領域の各々について定められた移動物の進入可能性に基づいて、各分岐すべき方向の分岐割合を算出し、前記分岐すべき方向の数だけ生成された前記移動物の位置の確率分布に対して、前記分岐割合に応じた重み付けを行うと共に、対応する前記移動状態の確率分布の各々を、各分岐すべき方向に応じて変更する分岐変更手段、及び
前記移動物の位置の確率分布及び移動状態の確率分布に基づいて、前記領域分割マップ上の前記移動物の将来の位置の確率分布を予測する予測手段
として機能させるためのプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１２−３７９８０（Ｐ２０１２−３７９８０Ａ）
【公開日】平成２４年２月２３日（２０１２．２．２３）
【国際特許分類】
【出願番号】特願２０１０−１７５６４３（Ｐ２０１０−１７５６４３）
【出願日】平成２２年８月４日（２０１０．８．４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２４年２月２３日（２０１２．２．２３）
【国際特許分類】
【出願日】平成２２年８月４日（２０１０．８．４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]