JPWO2015156097A1 - 衝突防止装置 - Google Patents

Abstract

自車（５０）の周囲の他車（６０）を観測する目標観測センサ（１）と、観測結果を基に他車（６０）を追尾する目標追尾部（３）と、追尾結果を基に他車（６０）の移動予測範囲を算出する目標運動予測部（５）と、他車（６０）の移動予測範囲の重なりから他車（６０）同士の衝突可能性を推定する目標間衝突可能性推定部（６）と、他車（６０）同士の衝突可能性がある場合に、衝突を回避するよう移動予測範囲を再算出する目標運動再予測部（７）と、自車（５０）の運動を観測する自機運動センサ（８）と、観測結果を基に自車（５０）の移動予測範囲を算出する自機運動予測部（１０）と、他車（６０）の最終的な移動予測範囲と自車（５０）の移動予測範囲との重なりから、自車（５０）と他車（６０）との衝突可能性を推定する自機衝突可能性推定部（１１）とを備えた。

Description

この発明は、自機が搭載された移動体の周囲に存在する目標の運動を予測して、当該移動体との衝突を回避する衝突防止装置に関するものである。

従来から、移動体（自動車、艦船、航空機等）に搭載されたレーダ、カメラ等のセンサを用いて周囲の目標（自動車、艦船、航空機等）を観測し、当該移動体と目標との衝突を避けるように円滑な運行を支援するシステムが知られている。
このセンサにより得られた情報から運行を支援する技術については多くの論文、特許文献等で挙げられており、それらを実現する装置及び方法については様々な提案がなされている。

代表的な例として、自動車の衝突防止技術がある。この技術では、自車（移動体）に搭載されたレーダ及び光学センサで他車（目標）を観測し、自車に対する距離及び接近速度が閾値に達すると判明した場合に衝突の可能性ありと判定し、警告又は自車の走行自体を制御している。

また、特許文献１に開示された技術では、自車と他車の現時刻の運動推定結果を外挿して、未来のある時刻における自車と他車の予測範囲を計算し、それの重なりの有無により衝突の可能性を判定している。例えば図１５では、自車５０と他車６０について４サンプリング時刻分の予測範囲を計算している。この例では、いずれのサンプリング時刻においても、自車５０の予測範囲と他車６０の予測範囲が重ならないため、「衝突の可能性はない」と判定する。

特開２０００−２７６６９６号公報 特開平８−２７１６１７号公報

しかしながら、特許文献１に開示された技術は、現時点での他車６０の運動を外挿することにより、他車６０の未来での位置を予測するものである。そのため、図１６に示すように、他車６０が突然、自車５０の前方に割込んだ場合、他車６０の運動予測（図１６に示す破線）と実際の運動（図１６に示す実線）との不一致が大きくなる。結果、衝突の予測が困難となるという課題がある。

ここで、割込みの発生原因として、ある車両の前方車が遅く、車線を維持すると衝突の可能性があるためという場合がある。一例を図１７に示す。この例では、右車線で自車５０が走行し、左車線で他車６０ａ，６０ｂが走行し、前方の他車６０ａの速度が他車６０ｂの速度に比べて極めて小さいとする。この場合、他車６０ｂは、他車６０ａとの衝突を回避するため、自車５０が走行する車線に割込む可能性がある。よって、複数の車両（図１７の例では他車６０ａ，６０ｂ）の運動を検出することで、上記割込みを予測することができるものと考えられる。

この発明は、上記のような課題を解決するためになされたもので、複数の目標の運動を検出して運動予測の精度を向上することができる衝突防止装置を提供することを目的としている。

この発明に係る衝突防止装置は、自機が搭載された移動体の周囲に存在する目標を観測する目標観測センサと、目標観測センサによる観測結果に基づいて、目標を追尾する目標追尾部と、目標追尾部による追尾結果に基づいて、目標の移動予測範囲を算出する目標運動予測部と、目標が複数存在する場合に、目標運動予測部により算出された目標の移動予測範囲の重なりから、当該目標同士の衝突可能性を推定する目標間衝突可能性推定部と、目標間衝突可能性推定部により目標同士の衝突可能性があると推定された場合に、衝突を回避するような当該目標の移動予測範囲を再算出する目標運動再予測部と、移動体の運動を観測する自機運動センサと、自機運動センサによる観測結果に基づいて、移動体の移動予測範囲を算出する自機運動予測部と、目標運動予測部又は目標運動再予測部により算出された目標同士の衝突可能性のない当該目標の移動予測範囲と、自機運動予測部により算出された移動体の移動予測範囲との重なりから、当該移動体と当該目標との衝突可能性を推定する自機衝突可能性推定部とを備えたものである。

この発明によれば、上記のように構成したので、複数の目標の運動を検出して運動予測の精度を向上することができる。

この発明の実施の形態１に係る衝突防止装置の構成を示すブロック図である。 この発明の実施の形態１に係る衝突防止装置の処理手順を示すフローチャートである。 この発明の実施の形態１に係る衝突防止装置による車両の運動モデルを示す図である。 他車同士の衝突可能性がある場合を示す図である。 この発明の実施の形態１に係る衝突防止装置の再予測による該当他車の運動モデル選択を示す図である。 他車同士の相対運動例を示す図である。 この発明の実施の形態１に係る衝突防止装置により再予測された予測経路例を示す図である。 この発明の実施の形態２に係る衝突防止装置の構成を示すブロック図である。 この発明の実施の形態２に係る衝突防止装置の処理手順を表すフローチャートである。 他車の車線変更開始時刻が異なる場合を示す図である。 他車がとり得る車線移動予測範囲の例を示す図である。 この発明の実施の形態２に係る信頼度比較部の処理手順を表すフローチャートである。 この発明の実施の形態２に係る信頼度比較部により算出される信頼度の一例を示す図である。 この発明の実施の形態３に係る衝突防止装置の構成を示すブロック図である。 従来の衝突防止装置による自車と他車の移動予測範囲例を示す図である。 従来の衝突防止装置による他車の移動予測範囲と、割込み発生時の実際の運動との差異を示す図である。 他車の割込みの要因となる状況例を示す図である。

以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態１．
図１はこの発明の実施の形態１に係る衝突防止装置の構成を示す図である。以下の各実施の形態では、本発明の衝突防止装置を自動車に適用した場合について説明する。
衝突防止装置は、自車（移動体）５０に搭載され、自車５０とその周囲に存在する他車（目標）６０の運動を予測して、自車５０と他車６０との衝突を回避するものである。この衝突防止装置は、図１に示すように、目標観測センサ１、目標観測値データ記憶部２、目標追尾部３、目標追尾データ記憶部４、目標運動予測部５、目標間衝突可能性推定部６、目標運動再予測部７、自機運動センサ８、自機運動データ記憶部９、自機運動予測部１０、自機衝突可能性推定部１１、制動判断部１２、自動ブレーキ部１３及び警告発生部１４から構成されている。

目標観測センサ１は、衝突防止装置が搭載された自車５０の周囲に存在する他車６０を観測するものである。この目標観測センサ１では、周囲に存在する他車６０の観測を定期的に実施し、その観測結果から他車６０の位置を算出する。この目標観測センサ１による観測結果を示す情報（目標観測値データ）は目標観測値データ記憶部２に出力される。

目標観測値データ記憶部２は、目標観測センサ１からの目標観測値データを記憶するものである。この目標観測値データ記憶部２は、ＨＤＤ、ＤＶＤ、メモリ等によって構成される。

目標追尾部３は、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、他車６０を追尾するものである。この目標追尾部３では、目標観測センサ１により得られた他車６０の位置を時系列処理することで、より正確な他車６０の位置及び速度を含む運動諸元を推定する。この目標追尾部３による追尾結果を示す情報（目標追尾データ）は目標追尾データ記憶部４に出力される。

目標追尾データ記憶部４は、目標追尾部３からの目標追尾データを記憶するものである。この目標追尾データ記憶部４は、ＨＤＤ、ＤＶＤ、メモリ等によって構成される。

目標運動予測部５は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲を算出するものである。この目標運動予測部５により算出された他車６０の移動予測範囲を示す情報は目標間衝突可能性推定部６に出力される。

目標間衝突可能性推定部６は、他車６０が複数存在する場合に、目標運動予測部５により算出された他車６０の移動予測範囲の重なりから、他車６０同士の衝突可能性を推定するものである。この目標間衝突可能性推定部６による他車６０同士の衝突可能性を示す情報は目標運動再予測部７に出力される。

目標運動再予測部７は、目標間衝突可能性推定部６により他車６０同士の衝突可能性があると推定された場合に、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、その衝突を回避するような他車６０の移動予測範囲を再算出するものである。この目標運動再予測部７により再算出された他車６０の移動予測範囲を示す情報は自機衝突可能性推定部１１に出力される。
なお、目標間衝突可能性推定部６により他車６０同士の衝突可能性がないと推定された場合、又は他車６０が複数存在しない場合には、目標運動予測部５により算出された他車６０の移動予測範囲を示す情報がそのまま自機衝突可能性推定部１１に出力される。

自機運動センサ８は、自車５０の運動を観測するものである。この自機運動センサ８は、例えば自車走行計等で構成され、自車５０の位置及び速度を含む運動諸元を観測する。この自機運動センサ８による観測結果を示す情報（自機運動データ）は自機運動データ記憶部９に出力される。

自機運動データ記憶部９は、自機運動センサ８からの自機運動データを記憶するものである。この自機運動データ記憶部９は、ＨＤＤ、ＤＶＤ、メモリ等によって構成される。

自機運動予測部１０は、自機運動データ記憶部９に記憶された自機運動データに基づいて、自車５０の移動予測範囲を算出するものである。この自機運動予測部１０により算出された自車５０の移動予測範囲を示す情報は自機衝突可能性推定部１１に出力される。

自機衝突可能性推定部１１は、目標運動予測部５又は目標運動再予測部７により算出された他車６０同士の衝突可能性のない他車６０の移動予測範囲と、自機運動予測部１０により算出された自車５０の移動予測範囲との重なりから、自車５０と他車６０との衝突可能性を推定するものである。この自機衝突可能性推定部１１により推定された自車５０と他車６０との衝突可能性を示す情報は制動判断部１２に出力される。

制動判断部１２は、自機衝突可能性推定部１１による推定結果に基づいて、自車５０の制動の要否を判断するものである。この制動判断部１２による判断結果を示す情報は自動ブレーキ部１３及び警告発生部１４に出力される。

自動ブレーキ部１３は、制動判断部１２により自車５０の制動を要すると判断された場合に、自動制動による自車５０の減速機能を動作させるものである。
警告発生部１４は、制動判断部１２により自車５０の制動を要すると判断された場合に、自車５０の運転者に対して現時点でブレーキ操作が必要である旨のメッセージの提示や音声の出力を行うものである。

なお、目標追尾部３、目標運動予測部５、目標間衝突可能性推定部６、目標運動再予測部７、自機運動予測部１０、自機衝突可能性推定部１１、制動判断部１２、自動ブレーキ部１３及び警告発生部１４は、ソフトウェアに基づくＣＰＵを用いたプログラム処理によって実行される。

次に、上記のように構成された衝突防止装置の処理の流れについて、図２〜７を参照しながら説明する。
衝突防止装置の処理では、図２に示すように、まず、目標観測センサ１は、自車５０の周囲に存在する他車６０の観測を定期的に実施し、その観測結果から他車６０の位置を算出する（ステップＳＴ２０１、目標観測ステップ）。この目標観測センサ１による観測結果を示す情報（目標観測値データ）は目標観測値データ記憶部２に出力されて記憶される。

次いで、目標追尾部３は、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、他車６０の位置を時系列処理（追尾処理）することで、より正確な他車６０の位置、及び速度を推定する（ステップＳＴ２０２、目標追尾ステップ）。すなわち、目標観測センサ１により得られた静的な他車６０の位置に対して、速度情報が加わることで動的な情報となり、より正確な他車６０の位置を得ることができる。以下に、目標追尾部３の処理の詳細について説明する。

目標追尾部３では、まず、目標観測センサ１から最新の観測時刻での観測結果（観測値）を示す情報を読込む。次に、既存航跡のゲートを算出する。次に、読込んだ観測値がそのゲート内にあるかどうかを調べ、当該観測値がどの航跡と相関可能であるかを決定する。ここで、航跡は、ｘｙの２次元空間における他車６０の位置及び速度から成る下式（１）の４個の要素を持ったベクトルを推定諸元とする。

そして、最新観測時刻ｋにおける他車６０の予測ベクトルＸ_ｋハット（−）は下式（２）で計算される。

ここで、自動車の運動を複数仮定する。図３に、他車６０の運動として、５つの運動モデル（（ａ）等速モデル（自車線維持）、（ｂ）加速モデル（自車線維持）、（ｃ）減速モデル（自車線維持）、（ｄ）右車線移動モデル（進路変更モデル）、（ｅ）左車線移動モデル（進路変更モデル）を仮定した場合を示す。本実施の形態では、自動車が上記５つの運動モデルのうちいずれかの運動を行うものとする。
なお、運動モデルとして、「減速又は加速しながら右車線又は左車線へ移動するモデル」を加えてもよい。

また、上式（２）の推移行列Φ_ｋ−１は、各運動モデルに応じた行列として設定される。例えば等速モデルの場合には、推移行列Φ_ｋ−１は下式（３）で表される。

ここで、Ｔはサンプリング間隔（目標観測センサ１による直前の観測時刻から現時刻までの経過時間）である。また、Ｘ_ｋ−１ハット（＋）は１観測時間前の平滑ベクトルである。

また、予測誤差共分散行列Ｐ_ｋ（−）は下式（４）で計算される。

ここで、Ｐ_ｋ−１（＋）は１観測時刻前の平滑誤差共分散行列である。また、Ｑ_ｋ−１は駆動雑音共分散行列であり、下式（５）で表される。

ここで、ｑは事前に設定するパワースペクトル密度のパラメータであり、Ｉ_２×２は２行２列の単位行列である。

そして、残差共分散行列Ｓ_ｋを下式（６）に従って算出する。

ここで、Ｈ_ｋは観測行列、Ｒ_ｋは目標観測センサ１の観測誤差共分散行列、Γ_２（ｋ）は観測雑音の変換行列である。観測値のベクトルが距離及び方位角の極座標上の値で得られる場合、各々は下式（７）〜（９）で表される。

ここで、０_２×２は２行２列の全要素が０の行列である。また、σ_Ｒは目標観測センサ１の距離観測誤差標準偏差であり、σ_Ｂｙは方位角観測誤差標準偏差である。

そして、上記の残差共分散行列Ｓ_ｋを用いて、目標観測センサ１からの観測値ｚ_ｋのゲート内外判定を行う。この観測値ｚ_ｋは、距離及び方位角で表された観測情報を直交座標上の位置に変換して得られるベクトルである。ゲート内外判定は、以下の不等式（１０）の成否により決まる。

ここで、ｚ_ｋ（−）は予測観測値であり、下式（１１）により計算できる。

また、ｄはχ平方検定における有意水準によって定まる境界値のパラメータである。

前サンプリング時刻における運動モデルの信頼度をβ_{ｋ−１，ｂ}（＋）（ｂ∈{（ａ），（ｂ），（ｃ），（ｄ），（ｅ）}）とすると、現サンプリング時刻における予測モデル信頼度β_ｋ，ａ（−）（ａ∈{（ａ），（ｂ），（ｃ），（ｄ），（ｅ）}）は、下式（１２）で計算される。

ここで、ｐ_ｋ，ａｂは運動モデルが（ｂ）から（ａ）に推移する確率を示す推移確率パラメータである。この推移確率パラメータは（ｂ）と（ａ）の全ての組み合わせによる推移について定める行列として表現できる。

この推移確率パラメータｐ_ｋ，ａｂは、通常では例えば下式（１５）のように設定する。

この例では、対角成分が０．６、非対角成分が０．１となっている。これは、現在の運動モデルを維持する確率が０．６であり、異種の運動モデル間を遷移する確率が任意に移行の組み合わせについて０．１であることを示している。これにより、他車６０が自車線を維持するような安定した状況では、例えば「他車６０の今後の運動は、等速８０％、加速１０％、右車線移動１０％、他０％」なる結果が得られ、等速モデルを選択して移動予測範囲を設定する。
なお、追尾に要求される正確さの度合いに応じて、上記の運動モデルのフィルタは重み付け混合を行う。

そして、上式（１０）のゲート内外判定によってゲート内と判定された観測値を用いて航跡の最新時刻における運動諸元推定値を算出する。ただし、複数の既存航跡が存在し、特定の観測値が複数の航跡のゲート内に入った場合は、観測値と既存航跡とを１対１対応させる相関決定処理が必要となる。特に本発明を自動車に適用した場合、自車５０の周囲には複数の他車６０が存在する場合が多いので、この相関の問題が重要となる。この相関を複数の仮説を生成しながら決定する方式はこれまで幾つか提案されている（例えば特許文献２参照）。

そして、相関決定により観測値が既存航跡に割り当てられると、これらの航跡の観測時刻における平滑諸元を計算し、更新航跡についてはさらに相関結果に対応する航跡の尤度を計算する。平滑ベクトルは下式（１４）で計算される。

ここで、Ｋ_ｋはフィルタゲインであり、下式（１５）により計算できる。

また、平滑誤差共分散行列は下式（１６）で計算される。

また、航跡の尤度（尤もらしさ）は、観測値の確率分布が予測位置（２次元の位置）を中心としたガウス分布となることを仮定して下式（１７）から計算する。

観測値を反映したモデル信頼度は下式（１８）の通りである。

ここで、ν_ｋ，ａは観測値を基に算出した運動モデル（ａ）に対する尤度であり、下式（１９）で表される。

以上により、追尾航跡毎に、他車６０の位置及び速度の選択した運動モデルの推定値、及び上記の推定誤差共分散行列が推定される。これらの情報（目標追尾データ）は目標追尾データ記憶部４に出力されて記憶され、目標運動予測部５で操舵の可否を判定するために用いられる。

次いで、目標運動予測部５は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲を算出する（ステップＳＴ２０３、目標運動予測ステップ）。ここで、目標運動予測部５は、他車６０の位置及び速度の推定値、及びそれらの推定誤差共分散行列より、未来での移動予測範囲を算出する。予測時刻は、現時刻から右折完了までの一定のサンプリング間隔に従って設定する離散的な複数の時刻である。これらのサンプリング時刻は、後述する自機運動予測ステップにおいて設定するサンプリング時刻と完全に一致させる。
この目標運動予測部５により算出された他車６０の移動予測範囲を示す情報は目標間衝突可能性推定部６に出力される。

次いで、目標間衝突可能性推定部６は、他車６０が複数存在する場合に、目標運動予測部５により算出された他車６０の移動予測範囲の重なりから、他車６０同士の衝突可能性を推定する（ステップＳＴ２０４、目標間衝突可能性推定ステップ）。この際、自車５０が観測する周囲の他車６０の全ての組み合わせについて衝突可能性を推定する。例えば、自車５０の周囲環境が図４に示す状況の場合（３台の他車６０ａ〜６０ｃが存在する場合）、他車６０ａと他車６０ｂの組み合わせ、他車６０ｂと他車６０ｃの組み合わせ、他車６０ａと他車６０ｃの組み合わせの計３通りの組み合わせについて衝突可能性を推定する。

衝突可能性の推定は、下式（２０）の成否により行う。そして、この式（２０）が成立する場合には、その組み合わせにおいて衝突の可能性があると推定する。

ここで、右辺のｔｈｒｅｓｈｏｌｄ_ｃは事前に設定する閾値パラメータである。左辺は、組み合わせにおける２つの他車６０が同一位置となる確率であり、下式（２１）に示すように自車５０と他車６０の特定の位置における同時存在確率密度の位置空間全体に渡る積分である。

また、式（２０）に対して下式（２１）のような数値計算を行うことで、近似することができる。

なお、ｘ_{ｐ，１，ｋ}（−）とＰ_{ｐ，１，ｋ}（−）は一方の他車６０の予測の中心とその共分散行列であり、ｘ_{ｐ，２，ｋ}（−）とＰ_{ｐ，２，ｋ}（−）は他方の他車６０の予測の中心とその共分散行列である。

これは、従来技術における自車５０と他車６０との間での予測楕円の重なりによる衝突可能性推定を、他車６０同士の衝突可能性推定にそのまま適用することで、その目的を実現している。図４の例では自車５０の隣接車線で他車６０ａ，６０ｂがあり、後方の他車６０ｂの方が現在速く走行しているとする。ここで、図中の楕円の中心は移動予測位置であり、その広がりは予測の誤差共分散行列の標準偏差分である。この場合、予測楕円の重なりにより、他車６０ｂは他車６０ａに衝突すると推定される。
この目標間衝突可能性推定部６による他車６０同士の衝突可能性を示す情報は目標運動再予測部７に出力される。

次いで、目標運動再予測部７は、目標間衝突可能性推定部６により他車６０同士の衝突可能性があると推定された場合に、その衝突を回避するような他車６０の移動予測範囲を再算出する（ステップＳＴ２０５、目標運動再予測ステップ）。以下では、衝突を回避する行動は運転者の目視を含めて相手をより把握し易い後方の車両（図４の例では他車６０ｂ）が取るものとし、その移動予測範囲を修正する場合について示す。

この移動予測範囲の修正では、衝突が起きないような運動モデルの選択により再予測を行う。この再予測は、他車６０との衝突可能性の有無によって運動モデル間の推移確率を調整し、衝突が発生する運動モデルを排除することにより実現する。

図４の例に対して、衝突を回避するための行動を反映して、図５に示すように、他車６０ｂの運動モデル選択方針を以下の通りとする。すなわち、（ａ）等速モデル（自車線維持）は他車６０ａと衝突するので不選択とする。また、（ｂ）加速モデル（自車線維持）は他車６０ａと衝突するので不選択とする。また、（ｃ）減速モデル（自車線維持）は候補として選択する。（ｄ）右車線移動モデルは候補として選択する。（ｅ）左車線移動モデルは道路がないので不選択とする。
結果として、他車６０ｂに対して選択可能な運動モデルは、図５のように減速モデルと右車線移動モデルのいずれかとなる。

上記のような運動モデル選択を計算するための手段の一つとして、推移確率パラメータｐ_ｋ，ａｂの制御がある。通常の運動モデル信頼度計算では、「等速８０％、加速１０％、右車線移動１０％、他０％」なる運動モデルの選択結果が得られるところを「減速４０％、右車線移動６０％、他は０％」としたい。そこで、モデル推移確率パラメータを通常から下式（２２）のように変更する。

右辺の第１項が（ｃ）減速モデルと（ｄ）右車線移動モデルのいずれかに推移するように配分するための行列である。ここでは（ｃ）減速モデルと（ｄ）右車線移動モデルの配分をα：β＝０．３：０．６としている。

この（ｃ）減速モデルと（ｄ）右車線移動モデルの配分パラメータ（上式のαとβ）の決定方法を述べる。各車線において、他車６０ｂの前方の最も近い車両（左車線：他車６０ａ、右車線：他車６０ｃ）との距離が「相対速度に対してどれだけ余裕があるか」を目安とする。一例としては下式（２３）のように相対距離と相対速度の比率で決める。

ここで、Ｖ_１は他車６０ａの速度、Ｖ_２は他車６０ｂの速度、Ｖ_３は他車６０ｃの速度、Ｒ_１２は他車６０ａと他車６０ｂとの間の距離、Ｒ_２３は他車６０ｃと他車６０ｂとの間の距離である（図６参照）。

なお上記では、各車線における自動車とその前方車との相対距離及び相対速度により配分パラメータを算出したが、各車線における自動車の前方車、後方車との相対距離及び相対速度により算出するという方法も有り得る。また、相対距離と相対速度の比でなく、単純に相対距離の比を配分パラメータとしてもよい。

また上記では、他車６０が衝突を回避するように行う運動の選択において、その他車６０の周囲の空間の空き状況に応じて運動に対する重み付け（配分パラメータの算出）を行う場合を示した。しかしながら、これに限るものではなく、例えば、他車６０が自車線に留まり直進運動を優先するように運動に対する重み付けを行うようにしてもよい。この際、他車６０が自車線に留まる確率測度をある事前に固定されたパラメータの値γ以上として、下式（２４）から配分パラメータを算出する。

以上の処理による他車６０の移動予測範囲の再算出結果例を図７に示す。この目標運動再予測部７により再算出された他車６０の移動予測範囲を示す情報は自機衝突可能性推定部１１に出力される。なお、目標間衝突可能性推定部６により他車６０同士の衝突可能性がないと推定された場合、又は他車６０が複数存在しない場合には、目標運動予測部５により算出された他車６０の移動予測範囲を示す情報がそのまま自機衝突可能性推定部１１に出力される。

一方、自機運動センサ８は、自車５０の位置及び速度を含む運動諸元を観測する（ステップＳＴ２０６、自機運動観測ステップ）。この自機運動センサ８による観測結果を示す情報（自機運動データ）は自機運動データ記憶部９に出力されて記憶される。

次いで、自機運動予測部１０は、自機運動データ記憶部９に記憶された自機運動データに基づいて、自車５０の未来での位置及び速度を予測し、その予測誤差共分散行列より自車５０の移動予測範囲を算出する（ステップＳＴ２０７、自機運動予測ステップ）。なお、予測時刻は、現時刻から一定のサンプリング間隔に従って設定する離散的な複数の時刻である。以下では、未来予測時刻のサンプリング時刻の番号をｋとする。

ここで、各サンプリング時刻における自車５０の運動は、ｘｙ平面の２次元空間における自車５０の位置及び速度から成る下式（２５）の４個の要素を持ったベクトルとする。

また、現時刻における自車５０の運動推定値は、位置については自車センサあるいはＧＰＳ等のセンサから得た自己運動情報とする。自車５０の未来予測位置は下式（２６）で算出する。

ここで、Φ_ｕｆ，ｋは自車５０の等速度運動を前提として下式（２７）で表される。

ここで、Ｔ_ｐは未来予測処理におけるサンプリング間隔である

また、未来予測の誤差共分散行列Ｐ_ｕｆ，ｋ（−）を下式（２８）で算出する。

ここで、Ｐ_ｕｆ，ｋ（＋）は現時刻の自車５０の運動諸元の推定誤差共分散行列であり、最新の平滑誤差共分散行列を外挿して算出する。また、Ｑ_ｕｆ，ｋは駆動雑音共分散行列であり、下式（２９）で表される。

以上の処理による自車５０の移動予測範囲の算出結果例を図７に示す。この自機運動予測部１０により算出された自車５０の移動予測範囲を示す情報は自機衝突可能性推定部１１に出力される。

次いで、自機衝突可能性推定部１１は、目標運動予測部５又は目標運動再予測部７により算出された他車６０同士の衝突可能性のない他車６０の移動予測範囲と、自機運動予測部１０により算出された自車５０の移動予測範囲との重なりから、自車５０と他車６０との衝突可能性を推定する（ステップＳＴ２０８、自機衝突可能性推定ステップ）。

具体的には、上記衝突可能性の推定は下式（３０）の成否により行う。そして、下式（３０）が成立する場合は自車５０と他車６０が衝突する可能性があると推定する。

ここで、右辺のｔｈｒｅｓｈｏｌｄ_Ｍは事前に設定する閾値パラメータである。左辺は自車５０と他車６０が同一位置となる確率であり、下式（３１）に示すように自車５０と他車６０の特定の位置における同時存在確率密度の位置空間全体に渡る積分である。これを数値計算により近似することもできる。

この自機衝突可能性推定部１１により推定された自車５０と他車６０との衝突可能性を示す情報は制動判断部１２に出力される。

次いで、制動判断部１２は、自機衝突可能性推定部１１による推定結果に基づいて、自車５０の制動の要否を判断する（ステップＳＴ２０９、制動判断ステップ）。そして、自機衝突可能性推定部１１が自車５０と他車６０との衝突可能性があると推定して制動判断部１２が自車５０の制動を要する判断した場合には、自動ブレーキ部１３を通じて自動制動による自車５０の減速機能を動作させるたり、警告発生部１４を通じて自車５０の運転者に現時点でブレーキ操作が必要である旨のメッセージの提示や音声の出力を行う。
以上の処理を定期的に繰り返すことにより、自車５０の衝突防止対策が常時可能となる。

以上のように、この実施の形態１によれば、自車５０の周囲に存在する他車６０を観測して、他車６０同士の衝突可能性を推定し、衝突可能性がある場合には当該衝突を回避するような他車６０の運動を再予測するように構成したので、複数の他車６０の運動を検出して運動予測の精度を向上することができる。その結果、複数の他車６０の相対運動によって他車６０による自車５０の車線への割込みが生じる可能性をより早く検出し、対策を早期に取らせるための衝突防止装置を得ることができる。

実施の形態２．
実施の形態１では、他車６０の車線移動モデル（進路変更モデル）を一定数とする場合を示した。それに対し、実施の形態２では、他車６０間の位置及び速度に応じて、車線変更の開始時刻を１つ以上設定し、車線移動モデルの数を可変とした場合について示す。
図８はこの発明の実施の形態２に係る衝突防止装置の構成を示す図である。この図８に示す実施の形態２に係る衝突防止装置は、図１に示す実施の形態１に係る衝突防止装置から目標運動予測部５、目標間衝突可能性推定部６及び目標運動再予測部７を取除いて、目標運動予測部１５を追加したものである。その他の構成は同様であり、同一の符号を付して異なる部分についてのみ説明を行う。

なお、実施の形態１における目標追尾部３では、他車６０に対して複数の運動モデルを仮定して他車６０の追尾を行うものとした。それに対し、実施の形態２における目標追尾部３では、運動モデルとして等速モデル（自車線維持）を仮定するものとする。

目標運動予測部１５は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０同士の位置及び速度から運動モデルを設定して当該他車６０の移動予測範囲及び当該移動予測範囲の信頼度を算出するものである。ここで、信頼度とは、他車６０による移動予測範囲への移動が起こり得る可能性を示す指標である。また以下では、目標運動予測部１５は、算出した信頼度から、各移動予測範囲での相対的な信頼度を示す指標である相対信頼度を算出し、移動予測範囲と相対信頼度をリスト化する。また、目標運動予測部１５は、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、不要な移動予測範囲を示す情報の削除も行う。この目標運動予測部１５は、等速予測部１５１、減速予測部１５２、進路変更開始時刻設定部１５３、複数の進路変更予測部１５４（１５４−１〜１５４−Ｎ）、進路変更予測記憶部１５５及び信頼度比較部１５６を有している。

等速予測部１５１は、他車６０の運動モデルとして等速モデル（自車線維持）を仮定し、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲（等速移動予測範囲）を算出するものである。この等速予測部１５１により算出された他車６０の移動予測範囲を示す情報は信頼度比較部１５６に出力される。

減速予測部１５２は、他車６０の運動モデルとして減速モデル（自車線維持）を仮定し、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲（減速移動予測範囲）を算出するものである。この減速予測部１５２により算出された他車６０の移動予測範囲を示す情報は信頼度比較部１５６に出力される。

進路変更開始時刻設定部１５３は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０間の位置及び速度に応じて、他車６０の車線移動モデルでの車線変更開始時刻（進路変更開始時刻）を１つ以上設定するものである。この進路変更開始時刻設定部１５３により設定された車線変更開始時刻を示す情報はそれぞれ対応する進路変更予測部１５４に出力される。

進路変更予測部１５４は、他車６０の運動モデルとして車線移動モデルを仮定し、進路変更開始時刻設定部１５３により設定された対応する車線変更開始時刻に従い、他車６０の移動予測範囲（車線移動予測範囲）を算出するものである。この進路変更予測部１５４により算出された他車６０の移動予測範囲を示す情報は進路変更予測記憶部１５５に出力される。

進路変更予測記憶部１５５は、各進路変更予測部１５４により算出された他車６０の移動予測範囲を示す情報を記憶するものである。この進路変更予測記憶部１５５は、ＨＤＤ、ＤＶＤ、メモリ等によって構成される。

信頼度比較部１５６は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、各予測部１５１，１５２，１５４により算出された他車６０の各移動予測範囲に対して信頼度を算出し、相対信頼度を算出してリスト化するものである。また、信頼度比較部１５６は、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、進路変更予測記憶部１５５内の不要な移動予測範囲を示す情報の削除も行う。この信頼度比較部１５６により得られた他車６０の各移動予測範囲とその相対信頼度を示すリストは自機衝突可能性推定部１１ｂに出力される。

自機衝突可能性推定部１１ｂは、目標運動予測部１５により得られたリストに示された他車６０の移動予測範囲と、自機運動予測部１０により算出された自車５０の移動予測範囲との重なり、及び当該リストに示された他車６０の移動予測範囲の信頼度（相対信頼度）から、自車５０と他車６０との衝突可能性を推定する。この自機衝突可能性推定部１１ｂにより推定された自車５０と他車６０との衝突可能性を示す情報は制動判断部１２に出力される。

次に、上記のように構成された衝突防止装置の処理の流れについて、図９〜１３を参照しながら説明する。
実施の形態２に係る衝突防止装置の処理では、図９に示すように、まず、目標観測センサ１は、自車５０の周囲に存在する他車６０の観測を定期的に実施し、その観測結果から他車６０の位置を算出する（ステップＳＴ９０１、目標観測ステップ）。この目標観測センサ１による観測結果を示す情報（目標観測値データ）は目標観測値データ記憶部２に出力されて記憶される。

次いで、目標追尾部３は、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、他車６０の位置を時系列処理（追尾処理）することで、より正確な他車６０の位置、及び速度を推定する（ステップＳＴ９０２、目標追尾ステップ）。すなわち、目標観測センサ１により得られた静的な他車６０の位置に対して、速度情報が加わることで動的な情報となり、より正確な他車６０の位置を得ることができる。以下に、目標追尾部３の処理の詳細について説明する。

目標追尾部３では、まず、目標観測センサ１から最新の観測時刻での観測結果（観測値）を示す情報を読込む。次に、既存航跡のゲートを算出する。次に、読込んだ観測値がそのゲート内にあるかどうかを調べ、当該観測値がどの航跡と相関可能であるかを決定する。ここで、航跡は、ｘｙの２次元空間における他車６０の位置及び速度から成る上式（１）の４個の要素を持ったベクトルを推定諸元とする。

そして、最新観測時刻ｋにおける他車６０の予測ベクトルＸ_ｋ（−）ハットは上式（２）で計算される。
また、上式（２）の推移行列Φ_ｋ−１は、実施の形態２では他車６０の運動モデルとして等速モデルを仮定するため、上式（３）で表される。

また、予測誤差共分散行列Ｐ_ｋ（−）は上式（４）で計算され、駆動雑音共分散行列Ｑ_ｋ−１は上式（５）で表される。
そして、残差共分散行列Ｓ_ｋを上式（６）に従って算出する。なお、観測値のベクトルが距離及び方位角の極座標上の値で得られる場合、式（６）における観測行列Ｈ_ｋ，観測雑音の変換行列Γ_２（ｋ）及び目標観測センサ１の観測誤差共分散行列Ｒ_ｋはそれぞれ上式（７）〜（９）で表される。

そして、上記の残差共分散行列Ｓ_ｋを用いて、目標観測センサ１からの観測値ｚ_ｋのゲート内外判定を行う。この観測値ｚ_ｋは、距離及び方位角で表された観測情報を直交座標上の位置に変換して得られるベクトルである。ゲート内外判定は、上記の不等式（１０）の成否により決まる。なお、不等式（１０）における予測観測値ｚ_ｋ（−）は上式（１１）により計算できる。

そして、不等式（１０）のゲート内外判定によってゲート内と判定された観測値を用いて航跡の最新時刻における運動諸元推定値を算出する。ただし、複数の既存航跡が存在し、特定の観測値が複数の航跡のゲート内に入った場合は、観測値と既存航跡とを１対１対応させる相関決定処理が必要となる。特に本発明を自動車に適用した場合、自車５０の周囲には複数の他車６０が存在する場合が多いので、この相関の問題が重要となる。この相関を複数の仮説を生成しながら決定する方式はこれまで幾つか提案されている（例えば特許文献２参照）。

そして、相関決定により観測値が既存航跡に割り当てられると、これらの航跡を観測値によって更新し、平滑ベクトルを計算する。平滑ベクトルは上式（１４）で計算される。なお、式（１４）におけるフィルタゲインＫ_ｋは上式（１５）により計算できる。
また、平滑誤差共分散行列は上式（１６）で計算される。

以上により、追尾航跡毎に、他車６０の位置及び速度の推定値、及び上記の推定誤差共分散行列が推定される。これらの情報（目標追尾データ）は目標追尾データ記憶部４に出力されて記憶され、目標運動予測部１５にて用いられる。

次いで、等速予測部１５１は、他車６０の運動モデルとして等速モデル（自車線維持）を仮定し、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲（等速移動予測範囲）を算出する（ステップＳＴ９０３、等速予測ステップ）。すなわち、等速予測部１５１は、他車６０が現時刻における推定速度を維持するとの仮定の下、現時刻から最大予測時間Ｓ秒後までの各サンプリング時刻における位置を推定する。以下に、等速予測部１５１の処理の詳細について説明する。

等速予測部１５１では、まず、下式（３２）より、現時刻ｋからｍ秒後の他車６０の等速移動予測範囲の予測ベクトルｘ^{（ＣｏｎｓｔＶｅｌ）} _ｋ，ｍ（―）を計算する。

ここで、Φ^{（ＣｏｎｓｔＶｅｌ）} _{ｋ，ｍ−１}は下式（３３）で表される。

ここで、Ｔ_ｐは現時刻から未来におけるサンプリング間隔である。
なお、等速移動予測範囲の予測ベクトルの初期値は下式（３４）で表される。

また、等速移動予測範囲の予測誤差共分散行列は下式（３５）で計算される。

ここで、Ｑ_{ｋ，ｍ−１}は駆動雑音共分散行列であり、下式（３６）で表される。

ここで、ｑは事前に設定するパワースペクトル密度のパラメータであり、Ｉ_２×２は２行２列の単位行列である。
なお、等速移動予測範囲の予測誤差共分散行列の初期値は下式（３７）で表される。

以上により、追尾航跡毎に、他車６０が等速直進運動した場合のＳ秒後までの移動予測範囲、及び予測誤差共分散行列が算出される。これらの情報は信頼度比較部１５６に出力され、他車６０が等速直進運動する可能性の算出に用いられる。

次いで、減速予測部１５２は、他車６０の運動モデルとして減速モデル（自車線維持）を仮定し、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０の移動予測範囲（減速移動予測範囲）を算出する（ステップＳＴ９０４、減速予測ステップ）。すなわち、減速予測部１５２は、他車６０が現時刻より減速して移動するとの仮定の下に、現時刻から最大予測時間Ｓ秒後までの各サンプリング時刻における位置を推定する。以下に、減速予測部１５２の処理の詳細について説明する。

減速予測部１５２では、まず、下式（３８）より、現時刻ｋからｍ秒後の他車６０の減速移動予測範囲の予測ベクトルｘ^{（Ｂｒａｋｅ）} _ｋ，ｍ（―）を計算する。

ここで、Φ^{（Ｂｒａｋｅ）} _{ｋ，ｍ−１}及びａ^{（Ｂｒａｋｅ）} _{ｋ，ｍ−１}は下式（３９），（４０）で表される。

ここで、Ｔ_ｐは現時刻から未来におけるサンプリング間隔であり、αは事前に設定する減速の加速度を表す負のスカラーである。
また、減速移動予測範囲の予測ベクトルの初期値は下式（４１）で表される。

また、減速移動予測範囲の予測誤差共分散行列は下式（４２）で計算される。

ここで、Ｑ_{ｋ，ｍ−１}は駆動雑音共分散行列であり、下式（４３）で表される。

ここで、ｑは事前に設定するパワースペクトル密度のパラメータであり、Ｉ_２×２は２行２列の単位行列である。
また、減速移動予測範囲の予測誤差共分散行列の初期値は下式（４４）で表される。

以上により、追尾航跡毎に、他車６０が減速した場合のＳ秒後までの移動予測範囲、及び予測誤差共分散行列が算出される。これらの情報は信頼度比較部１５６に出力され、他車６０が減速する可能性の算出に用いられる。

次いで、進路変更開始時刻設定部１５３は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０間の位置及び速度に応じて、他車６０の車線移動モデルでの車線変更開始時刻を１つ以上設定する（ステップＳＴ９０５、進路変更開始時刻設定ステップ）。以下に、進路変更開始時刻設定部１５３の処理の詳細について説明する。ここでは、図１０を例に、他車６０ｂの車線変更開始時刻を、前方に位置する他車６０ａとの位置関係及び相対速度により設定する場合について示す。なお図１０（ａ）では、他車６０ｂが右車線の状況を見計らった上で右車線移動を行う場合を示し、図１０（ｂ）では、他車６０ｂが他車６０ａとの衝突回避を優先し、急に右車線移動を行う場合を示している。

他車６０ｂが車線移動する際に選択可能な車線変更開始時刻を設定する手段の一つとして、ここでは他車６０ｂと他車６０ａとの衝突予測時間（ＴＴＣ：Ｔｉｍｅ Ｔｏ Ｃｏｌｌｉｓｉｏｎ）を用いる方法を記述する。他車６０ｂと他車６０ａとのＴＴＣは下式（４５）で表される。

ここで、Ｒ_１２は他車６０ｂと他車６０ａとの車間であり、Ｖ_２は他車６０ｂの速度であり、Ｖ_１は他車６０ａの速度である（図６参照）。

そして、上式（４５）で定義したＴＴＣを基に、例えば以下のように、他車６０ｂの車線変更開始時刻を設定する。まず、０秒＜ＴＴＣ＜１秒であった場合には、車線変更開始時刻を０秒後に設定する（第１の条件）。また、１秒≦ＴＴＣ＜２秒であった場合には、車線変更開始時刻を０秒後、０．５秒後に設定する（第２の条件）。また、２秒≦ＴＴＣ＜３秒であった場合には、車線変更開始時刻を０秒後、０．５秒後、１秒後、１．５秒後に設定する（第３の条件）。また、３秒≦ＴＴＣであった場合には、車線変更開始時刻を０秒後、０．５秒後、１秒後、１．５秒後、２秒後、２．５秒後に設定する（第４の条件）。また、ＴＴＣ≦０秒であった場合には、車線変更開始時刻を０秒後、０．５秒後、１秒後、１．５秒後、２秒後、２．５秒後に設定する（第５の条件）。
この条件による車線変更開始時刻の設定の一例を図１１に示す。この図１１では、ＴＴＣが２秒以上３秒未満の場合であり、車線変更開始時刻が０秒〜１．５秒の４通り設定され、それによって４通りの車線移動予測範囲が生成されることを表している。

上記設定の特徴は、他車６０ｂが他車６０ａと衝突するまでの時間に余裕があるほど、車線変更開始時刻の個数を増やしていることにある。例えば第１の条件のように、現時刻の速度を維持した場合に１秒以内に他車６０ｂと他車６０ａが衝突する状況では、他車６０ｂは車線移動を開始するまでの時間の余裕がほぼ無いとして、即座に車線変更を開始する運動モデルの予測のみで十分と考えられる。一方、第４，５の条件のように他車６０ｂと他車６０ａの車間に余裕がある場合は、他車６０ｂは様々な時間に車線移動を開始し得るため、車線変更開始時刻を多数設定する。

なお上記では、車線変更開始時刻を現時刻からの経過秒数で定義したが、これを他車６０ｂと他車６０ａの相対距離の比によって定義する方法も有り得る。例えば、車線変更開始時刻Ｔ_ｎ ^{(Ｓｔｅｅｒ)}を、ある一定距離Ｒ₁₂ｎ／Ｎだけ走行するまでの時間として下式（４６）のように表す。

ここで、ｎは１以上Ｎ以下の整数とし、Ｎは下式（４７）で計算する。

また上記では、車線変更開始時刻の個数をＴＴＣに基づいて決定したが、これを他車６０ｂと他車６０ａとの相対距離Ｒ_１２の大きさによって算出する方法も有り得る。

以上により、他車６０ｂが選択可能な車線変更開始時刻が１つ以上設定される。この進路変更開始時刻設定部１５３により設定されたＮ個の車線変更開始時刻を示す情報はそれぞれ対応する進路変更予測部１５４に出力される。なお以下では、この処理により設定された車線変更開始時刻の個数をＮと記述する。

次いで、進路変更予測部１５４は、他車６０の運動モデルとして車線移動モデルを仮定し、進路変更開始時刻設定部１５３により設定された対応する車線変更開始時刻に従い、他車６０の移動予測範囲（車線移動予測範囲）を算出する（ステップＳＴ９０６、車線移動予測ステップ）。すなわち、進路変更予測部１５４は、現時刻からの経過時間Ｔ_ｎ ^{(Ｓｔｅｅｒ)}で他車６０ｂが車線移動を開始する場合の、現時刻から最大予測時間Ｓ秒後までの各サンプリング時刻における位置を推定する。なお、ｎは１以上Ｎ以下の整数とする。以下に、進路変更予測部１５４の処理の詳細について説明する。ここでは、現時刻からの経過時間Ｔ_ｎ ^{(Ｓｔｅｅｒ)}から車線変更を開始する軌道の例として、Ｔ_ｎ ^{(Ｓｔｅｅｒ)}までは等速直進運動し、その後は一定速度で車線変更する場合を説明する。

進路変更予測部１５４は、まず、下式（４８）より、現時刻ｋからｍ秒後の他車６０ｂの車線移動予測範囲の予測ベクトルｘ^{（Ｓｔｅｅｒ，ｎ）} _ｋ，ｍ（―）を計算する。

ここで、Φ^{（Ｓｔｅｅｒ，ｎ）} _{ｋ，ｍ−１}は下式（４９）で表される。

ここでΦ^{（ＣｏｎｓｔＶｅｌ）} _{ｋ，ｍ−１}は等速モデルを表す行列であり、Φ^{（Ｓｔｅｅｒ）} _{ｋ，ｍ−１}は時刻ｋ＋ｍより車線移動を開始する車線移動モデルに応じて設定される行列である。
また、車線移動予測範囲上の予測ベクトルの初期値は下式（５０）で表される。

また、車線移動予測範囲の予測誤差共分散行列は下式（５１）で計算される。

ここで、Ｑ_{ｋ，ｍ−１}は駆動雑音共分散行列であり、下式（５２）で表される。

ここで、ｑは事前に設定するパワースペクトル密度のパラメータであり、Ｉ_２×２は２行２列の単位行列である。
また、車線移動予測範囲の予測誤差共分散行列の初期値は下式（５３）で表される。

上記では、車線変更開始時刻まで他車６０ｂが等速直進運動することを前提として移動予測範囲を算出した。しかしながら、これに限るものではなく、予め設定した等加速度で運動した後に車線変更を開始する運動モデルも有り得る。また、車線変更するまでの速度及び加速度を、他車６０（図１０における他車６０ｂ）とその周辺車両（図１０における他車６０ａ、他車６０ｃ、自車５０）との相対距離及び相対速度に応じて変える方法も有り得る。

以上により、追尾航跡毎に、他車６０ｂが時刻Ｔ_ｎ ^{(Ｓｔｅｅｒ)}で車線変更した場合のＳ秒後までの移動予測範囲及び予測誤差共分散行列が算出される。これらの情報は進路変更予測記憶部１５５に出力されて記憶される。車線移動予測範囲を示す情報は全部でＮ個保存される。

次いで、信頼度比較部１５６は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、各予測部１５１，１５２，１５４により算出された他車６０の各移動予測範囲に対して信頼度を算出し、相対信頼度を算出してリスト化する。また、目標観測値データ記憶部２に記憶された目標観測値データに基づいて、進路変更予測記憶部１５５内の不要な移動予測範囲を示す情報を削除する（ステップＳＴ９０７、信頼度比較ステップ）。以下に、図１２を参照しながら、信頼度比較部１５６の処理の詳細について説明する。

信頼度比較部１５６の処理では、図１２に示すように、まず、等速予測部１５１により算出された等速移動予測範囲（現時刻からＳ秒後までのサンプリング時刻毎の等速移動予測範囲、及び予測誤差共分散行列）に対し、信頼度を算出する（ステップＳＴ１２０１）。

ここで、移動予測範囲の信頼度は、他車６０ｂが移動予測範囲に沿って運動する可能性を表す指標である。また、移動予測範囲及び予測誤差共分散行列から信頼度を算出する処理は、等速移動予測範囲の場合（ステップＳＴ１２０１）と、減速移動予測範囲の場合（ステップＳＴ１２０２）と、車線移動予測範囲の場合（ステップＳＴ１２０６）において同一でなければならない。

等速移動予測範囲の信頼度は例えば下式（５４）で表される。

ここで、Ｍは２つの移動予測範囲が同一位置となる確率であり、下式（５５）より表される。

また、ｘ^（ｌ） _ｋ，ｍは他車６０の周辺車両ｌの現時刻ｋからｍ秒後の移動予測範囲であり、下式（５６），（５７）より計算する。

ここで、ｘ^（ｌ） _ｋ（＋）は現時刻における他車６０の周辺車両ｌの平滑ベクトルとする。
また、Ｐ^（ｌ） _ｋ，ｍは他車６０の周辺車両ｌの現時刻ｋからｍ秒後の予測誤差共分散行列であり、下式（５８），（５９）より計算する。

ここで、Ｐ^（ｌ） _ｋ（＋）は現時刻における他車６０の周辺車両ｌの平滑誤差共分散行列である。

上式（５４）の信頼度は、等速で移動する周辺車両の移動予測範囲と、他車６０の移動予測範囲が最も接近したときの間隔を表す。したがって、この信頼度の定義は、他車６０が周辺車両から離れた移動予測範囲を選択する可能性が高いとの考え方を前提とする。例えば図１３では、自車５０に近い第１の移動予測範囲１３０１及び他車６０ａに近い第３の移動予測範囲１３０３は選ばれにくいとして信頼度が低くなり、それらの中間の時刻で車線変更を開始する第２の移動予測範囲１３０２の信頼度が高くなる。

次いで、減速予測部１５２により算出された減速移動予測範囲（現時刻からＳ秒後までのサンプリング時刻毎の減速移動予測範囲、及び予測誤差共分散行列）に対し、信頼度を算出する（ステップＳＴ１２０２）。
ここで、等速移動予測範囲の信頼度を上式（５４）とした場合、減速移動予測範囲の信頼度も同様に下式（６０）で表される。

次いで、進路変更予測記憶部１５５から、現時刻において未選択の車線移動予測範囲を１つ選択する（ステップＳＴ１２０３）。ここでは、選択された車線移動予測範囲Ａの予測ベクトルをＸ^{（Ｓｔｅｅｒ，Ａ）} _ｋ’，ｍ、車線移動予測範囲Ａの予測誤差共分散行列をＰ^{（Ｓｔｅｅｒ，Ａ）} _ｋ’，ｍとする。なお、進路変更予測記憶部１５５に記憶された車線移動予測範囲は過去に生成されたものも含まれるため、ｋ’は現時刻又は過去の時刻を表す。

次いで、選択した車線移動予測範囲Ａの現時刻での予測位置と現時刻の他車６０の観測値から、車線移動予測範囲Ａの尤もらしさを表す指標（尤度）を、観測値の確率分布が予測位置を中心としたガウス分布となると仮定した下式（６１）から算出する（ステップＳＴ１２０４）。

次いで、算出した尤度が以下の不等式（６２）を満たすか否かを判断する（ステップＳＴ１２０５）。

ここで、Ｔｈｒｓｈｏｌｄ_ｇは事前に設定する閾値パラメータである。

このステップＳＴ１２０５において、尤度が不等式（６２）を満たすと判断した場合には、車線移動予測範囲Ａは他車６０ｂがとり得る運動であるとして、当該車線移動予測範囲Ａの信頼度を算出する（ステップＳＴ１２０６）。
ここで、等速移動予測範囲の信頼度を上式（５４）とした場合、車線移動予測範囲Ａの信頼度も同様に下式（６３）で表される。

一方、ステップＳＴ１２０５において、尤度が不等式（６２）を満たさないと判断した場合には、車線移動予測範囲Ａは現在の他車６０の位置と大きく異なるとして、当該車線移動予測範囲Ａを示す情報を進路変更予測記憶部１５５から削除する（ステップＳＴ１２０７）。

次いで、進路変更予測記憶部１５５に記憶された全ての車線移動予測範囲を選択したかを判断する（ステップＳＴ１２０８）。このステップＳＴ１２０８において、進路変更予測記憶部１５５内に未選択の車線移動予測範囲を示す情報がある場合には、シーケンスはステップＳＴ１２０３に戻り、上記処理を繰り返す。

一方、ステップＳＴ１２０８において、進路変更予測記憶部１５５に記憶された全ての車線移動予測範囲を選択したと判断した場合には、各移動予測範囲の信頼度を相対信頼度に変換する（ステップＳＴ１２０９）。なお、相対信頼度は、ステップＳＴ１２０１，１２０２，１２０６で算出された各移動予測範囲の信頼度を比較した結果を表す指標であり、例えば下式（６４）より算出する。

ここで、ｂ_p，kは他車６０ｂのいずれかの移動予測範囲の信頼度であり、Ｂ_ｋはステップＳＴ１２０１，１２０２，１２０６で算出された信頼度の総和とする。
例えば、ある車線移動予測範囲Ａの相対信頼度が０．６であった場合、「他車６０ｂは６０％の可能性で車線移動予測範囲Ａの運動をする」と予測する。

また、「他車６０ｂは車線を維持する運動を優先して選択する」との考えに基づき、車線移動予測範囲の相対信頼度のみを低く設定してもよい。

以上のステップＳＴ１２０１〜ＳＴ１２０９により、現時刻ｋにおいて他車６０ｂがとり得る移動予測範囲と、それらが相対的にどの程度の可能性で実現するかを表した相対信頼度とを一覧表示したリストが得られる。このリストは自機衝突可能性推定部１１ｂに出力され、自車５０との衝突可能性の判定に用いられる。

また、上記では信頼度を他車６０ｂと周辺車両との相対距離に基づいて算出したが、これを「他車６０ｂは、衝突したときに被害が大きくなると思われる車両に接近する軌道は避ける」との考えに基づき、他車６０ｂと周辺車両との相対速度から信頼度を算出してもよい。また、「他車６０ｂは緩やかな車線変更ほど選択しやすい」との考えに基づき、他車６０ｂの車線変更する際の角度が緩やかな移動予測範囲ほど信頼度が高くなるようにしてもよい。

再び図９に示す衝突防止装置の全体動作に戻り、自機運動センサ８は、自車５０の位置及び速度を含む運動諸元を観測する（ステップＳＴ９０８、自機運動ステップ）。この処理は実施の形態１と同様である。この自機運動センサ８による観測結果を示す情報（自機運動データ）は自機運動データ記憶部９に出力されて記憶される。

次いで、自機運動予測部１０は、自機運動データ記憶部９に記憶された自機運動データに基づいて、自車５０の未来での位置及び速度を予測し、その予測誤差共分散行列より自車５０の移動予測範囲を算出する（ステップＳＴ９０９、自機運動予測ステップ）。この処理は実施の形態１と同様である。この自機運動予測部１０により算出された自車５０の移動予測範囲を示す情報は自機衝突可能性推定部１１ｂに出力される。

次いで、自機衝突可能性推定部１１ｂは、目標運動予測部１５により得られたリストに示された他車６０の移動予測範囲と、自機運動予測部１０により算出された自車５０の移動予測範囲との重なり、及び当該リストに示された他車６０の移動予測範囲の信頼度（相対信頼度）から、自車５０と他車６０との衝突可能性を推定する（ステップＳＴ９１０、自機衝突可能性推定ステップ）。

具体的には、上記衝突可能性の推定は下式（６５）の成否により行う。そして、下式（６５）が成立する場合は自車５０と他車６０が衝突する可能性があると推定する。

ここで、ｘ_ｐ，ｋ及びＰ_ｐ，ｋは相対信頼度β_ｐ，ｋに対応する他車６０ｂの移動予測範囲及び予測誤差共分散行列である。また、ｔｈｒｅｓｈｏｌｄ_Ｍは事前に設定する閾値パラメータである。
この自機衝突可能性推定部１１ｂにより推定された自車５０と他車６０との衝突可能性を示す情報は制動判断部１２に出力される。

次いで、制動判断部１２は、自機衝突可能性推定部１１ｂによる推定結果に基づいて、自車５０の制動の要否を判断する（ステップＳＴ９１１、制御判断ステップ）。そして、自機衝突可能性推定部１１ｂが自車５０と他車６０との衝突可能性があると推定して制動判断部１２が自車５０の制動を要する判断した場合には、自動ブレーキ部１３を通じて自動制動による自車５０の減速機能を動作させたり、警告発生部１４を通じて自車５０の運転者に現時点でブレーキ操作が必要である旨のメッセージの提示や音声の出力を行う。
以上の処理を定期的に繰り返すことにより、自車５０の衝突防止対策が常時可能となる。

以上のように、この実施の形態２によれば、他車６０間の位置及び速度から、車線移動モデルの数を可変とするように構成したので、例えば図１０に示すような並走車の車線変更の開始が遅れる場合を考慮した予測が可能となり、実施の形態１に対し、自車５０との衝突可能性の判断精度がさらに向上する。また、実施の形態２では、他車６０間の位置及び速度に応じて、車線移動モデルの数を可変としたので、実現可能性が低い移動予測範囲の算出処理が省略され、他車６０の算出処理における演算負荷を削減することができる。

実施の形態３．
実施の形態２では、他車６０の周囲の空間の空き状況に応じて、車線変更の開始時刻を１つ以上設定し、車線移動モデル（進路変更モデル）の数を可変とする場合について示した。それに対し、実施の形態３では、他車６０の周囲の空間の空き状況に応じて、減速モデルのパラメータ及び車線移動モデルのパラメータをそれぞれ１つ以上設定する場合について示す。
図１４はこの発明の実施の形態３に係る衝突防止装置の構成を示す図である。この図１４に示す実施の形態３に係る衝突防止装置は、図８に示す実施の形態２に係る衝突防止装置に減速パラメータ設定部１５７を追加し、減速予測部１５２を複数設け、進路変更開始時刻設定部１５３を進路変更パラメータ設定部１５８に変更したものである。その他の構成は同様であり、同一の符号を付して異なる部分についてのみ説明を行う。

減速パラメータ設定部１５７は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０同士の位置及び速度から、減速モデルのパラメータを１つ以上設定するものである。この減速パラメータ設定部１５７により設定されたパラメータを示す情報はそれぞれ対応する減速予測部１５２に出力される。
また、減速予測部１５２は、減速パラメータ設定部１５７により設定された対応するパラメータを用いて減速モデルを仮定し、他車６０の移動予測範囲（減速移動予測範囲）を算出する。

進路変更パラメータ設定部１５８は、目標追尾データ記憶部４に記憶された目標追尾データに基づいて、他車６０同士の位置及び速度から、車線移動モデルのパラメータを１つ以上設定するものである。この進路変更パラメータ設定部１５８により設定されたパラメータを示す情報はそれぞれ対応する進路変更予測部１５４に出力される。
また、進路変更予測部１５４は、進路変更パラメータ設定部１５８により設定された対応するパラメータを用いて車線移動モデルを仮定し、他車６０の移動予測範囲（車線移動予測範囲）を算出する。

ここで、減速パラメータ設定部１５７により設定される減速モデルのパラメータは、例えば他車６０の加速度である。
また、進路変更パラメータ設定部１５８により設定される車線移動モデルのパラメータは、例えば、現時刻から何秒後に車線変更を開始するかを示す車線変更開始時刻、車線に対してどの程度の角度で車線変更するかを示す車線変更角度（進路変更角度）、車線変更を開始する前にどの程度加速又は減速するかを示す車線変更前加速度、車線変更中にどの程度加速又は減速するかを示す車線変更中加速度、車線変更を終えた後にどの程度加速又は減速するかを示す車線変更後加速度が挙げられる。

以上のように、この実施の形態３によれば、他車６０間の位置及び速度から、他車６０の周囲の空間の空き状況に応じて、減速モデルのパラメータ及び車線移動モデルのパラメータを１つ以上設定するように構成したので、例えば他車６０が前方車の速度に応じて減速する運動を予測することができ、実施の形態１に対して、自車５０との衝突可能性の判断精度がさらに向上する。また、他車６０が車線変更先の車間に応じて車線変更角度、車線変更前後の加速度を調整する運動を予測することができ、さらに自車５０との衝突可能性の判断精度が向上する。

なお上記では、本発明に係る衝突防止装置を自動車に適用し、自車５０とその周囲に存在する他車６０との衝突を回避する場合を例に説明を行った。しかしながら、これに限るものではなく、本発明に係る衝突防止装置をその他の移動体（艦船、航空機等）に適用し、その周囲に存在する目標（艦船、航空機等）との衝突を回避するようにしてもよく、同様の効果を得ることができる。

また、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

この発明に係る衝突防止装置は、複数の目標の運動を検出して運動予測の精度を向上することができ、自機が搭載された移動体の周囲に存在する目標の運動を予測して、当該移動体との衝突を回避する衝突防止装置等に用いるのに適している。

１ 目標観測センサ、２ 目標観測値データ記憶部、３ 目標追尾部、４ 目標追尾データ記憶部、５ 目標運動予測部、６ 目標間衝突可能性推定部、７ 目標運動再予測部、８ 自機運動センサ、９ 自機運動データ記憶部、１０ 自機運動予測部、１１，１１ｂ 自機衝突可能性推定部、１２ 制動判断部、１３ 自動ブレーキ部、１４ 警告発生部、１５ 目標運動予測部、５０ 自車、６０，６０ａ〜６０ｃ 他車、１５１ 等速予測部、１５２ 減速予測部、１５３ 進路変更開始時刻設定部、１５４ 進路変更予測部、１５５ 進路変更予測記憶部、１５６ 信頼度比較部、１５７ 減速パラメータ設定部、１５８ 進路変更パラメータ設定部。

この発明に係る衝突防止装置は、自機が搭載された移動体の周囲に存在する目標を観測する目標観測センサと、目標観測センサによる観測結果に基づいて、予め設定した複数の運動モデルのいずれかを仮定し、運動モデルを運動モデルの信頼度で重み付けしながら目標を追尾する目標追尾部と、目標追尾部による追尾結果に基づいて、目標の移動予測範囲を算出する目標運動予測部と、目標が複数存在する場合に、目標運動予測部により算出された目標の移動予測範囲の重なりから、当該目標同士の衝突可能性を推定する目標間衝突可能性推定部と、目標間衝突可能性推定部により目標同士の衝突可能性があると推定された場合に、衝突を回避する運動モデルを優先するように重みを変更し、目標が衝突を回避する行動を取る前に当該目標の移動予測範囲を再算出する目標運動再予測部と、移動体の運動を観測する自機運動センサと、自機運動センサによる観測結果に基づいて、移動体の移動予測範囲を算出する自機運動予測部と、目標運動予測部又は目標運動再予測部により算出された目標同士の衝突可能性のない当該目標の移動予測範囲と、自機運動予測部により算出された移動体の移動予測範囲との重なりから、当該移動体と当該目標との衝突可能性を推定する自機衝突可能性推定部とを備えたものである。

Claims (7)

1. 自機が搭載された移動体の周囲に存在する目標を観測する目標観測センサと、
   前記目標観測センサによる観測結果に基づいて、前記目標を追尾する目標追尾部と、
   前記目標追尾部による追尾結果に基づいて、前記目標の移動予測範囲を算出する目標運動予測部と、
   前記目標が複数存在する場合に、前記目標運動予測部により算出された前記目標の移動予測範囲の重なりから、当該目標同士の衝突可能性を推定する目標間衝突可能性推定部と、
   前記目標間衝突可能性推定部により前記目標同士の衝突可能性があると推定された場合に、衝突を回避するような当該目標の移動予測範囲を再算出する目標運動再予測部と、
   前記移動体の運動を観測する自機運動センサと、
   前記自機運動センサによる観測結果に基づいて、前記移動体の移動予測範囲を算出する自機運動予測部と、
   前記目標運動予測部又は前記目標運動再予測部により算出された前記目標同士の衝突可能性のない当該目標の移動予測範囲と、前記自機運動予測部により算出された前記移動体の移動予測範囲との重なりから、当該移動体と当該目標との衝突可能性を推定する自機衝突可能性推定部と
   を備えた衝突防止装置。
2. 前記目標運動再予測部は、前記目標が衝突を回避するように行う運動の選択において、当該目標の周囲の空間の空き状況に応じて当該運動に対する重み付けを行う
   ことを特徴とする請求項１記載の衝突防止装置。
3. 前記目標運動再予測部は、前記目標が衝突を回避するように行う運動の選択において、当該目標が直進運動を優先するように当該運動に対する重み付けを行う
   ことを特徴とする請求項１記載の衝突防止装置。
4. 自機が搭載された移動体の周囲に存在する目標を観測する目標観測センサと、
   前記目標観測センサによる観測結果に基づいて、前記目標を追尾する目標追尾部と、
   前記目標追尾部による追尾結果に基づいて、前記目標同士の位置及び速度から運動モデルを設定して当該目標の移動予測範囲及び当該移動予測範囲の信頼度を算出する目標運動予測部と、
   前記移動体の運動を観測する自機運動センサと、
   前記自機運動センサによる観測結果に基づいて、前記移動体の移動予測範囲を算出する自機運動予測部と、
   前記目標運動予測部により算出された前記目標の移動予測範囲と、前記自機運動予測部により算出された前記移動体の移動予測範囲との重なり、及び当該目標の移動予測範囲の信頼度から、当該移動体と当該目標との衝突可能性を推定する自機衝突可能性推定部と
   を備えた衝突防止装置。
5. 前記目標運動予測部は、前記目標の周囲の空間の空き状況に応じて、前記目標の運動モデルである進路変更モデルでの進路変更の開始時刻を１つ以上設定する
   ことを特徴とする請求項４記載の衝突防止装置。
6. 前記目標運動予測部は、前記目標の周囲の空間の空き状況に応じて、前記目標の運動モデルである減速モデルでの加速度を１つ以上設定する
   ことを特徴とする請求項４記載の衝突防止装置。
7. 前記目標運動予測部は、前記目標の周囲の空間の空き状況に応じて、前記目標の運動モデルである進路変更モデルでの加速度及び進路変更角度を１つ以上設定する
   ことを特徴とする請求項４記載の衝突防止装置。

Images