JP2009168624A - 物体速度検出装置 - Google Patents

Abstract

【課題】 対象物の移動速度を高精度に求めることができる物体速度検出装置を提供する。
【解決手段】 物体速度検出装置は、まずミリ波レーダの計測データから得られた相手車の位置情報について、αβフィルタを用いた位置微分を行って、相手車の車速を算出し（手順Ｓ１０４）、更にその車速に対して相手車予測軌道による速度上下限カットフィルタをかけることで、相手車の速度下限値及び速度上限値を決定する（手順Ｓ１０５）。また、物体速度検出装置は、相手車の位置情報に基づいた相手車の車速とは別に、ミリ波レーダの計測データから得られた自車と相手車との相対速度情報に基づいた相手車の速度を算出する（手順Ｓ１０６）。そして、物体速度検出装置は、位置情報から得られた相手車の車速と相対速度情報から得られた相手車の車速とを組み合わせて、最終的な相手車の車速を求める（手順Ｓ１０７）。
【選択図】 図２

Description

本発明は、車両等の対象物の移動速度を検出する物体速度検出装置に関するものである。

従来の物体速度検出装置としては、例えば特許文献１に記載されているように、自車の走行にとって障害となる移動体の位置観測データを入力し、移動体の速度を直線運動モデルに基づいて推定するカルマンフィルタを用いて、当該障害となる移動体の速度を算出するものが知られている。
特開平８−１２２４３２号公報

上記従来技術においては、移動体の位置観測データから移動体の速度を計算することはできる。しかし、短いサンプリング周期が必要とされるＰＣＳ（プリクラッシュセーフティシステム）等に物体速度検出装置が適用される場合には、僅かな位置のずれが速度の算出結果に大きな影響を与えてしまう。カルマンフィルタを用いることで、ある程度の位置のばらつきは抑制されるが、初期値の与え方によっては収束までの時間が長くなってしまうという問題がある。このため、位置情報のみから移動体の速度を算出することには限界がある。

本発明の目的は、対象物の移動速度を高精度に求めることができる物体速度検出装置を提供することである。

本発明は、移動体に搭載され、対象物の移動速度を検出する物体速度検出装置において、対象物の位置情報を取得する位置情報取得手段と、対象物の位置情報に基づいて対象物の移動速度を算出する速度算出手段と、移動体と対象物との相対速度情報を取得する相対速度情報取得手段と、相対速度情報を用いて、対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段とを備えることを特徴とするものである。

このような本発明の物体速度検出装置においては、移動体と対象物との相対速度情報を取得し、この相対速度情報を用いて対象物の移動速度の上限値及び下限値を求めることにより、対象物の位置情報に基づいて得られた対象物の移動速度のばらつきが抑えられるようになる。これにより、対象物の位置情報のみに基づいて対象物の移動速度を求める場合に比べて、移動体に接近する対象物の移動速度を高精度に求めることができる。

好ましくは、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段と、接近方向ベクトルと予め設定された補正角度とに基づいて、対象物の接近方向領域を求める手段とを更に備え、速度制限値算出手段は、対象物の接近方向領域と相対速度情報とに基づいて、対象物の移動速度の上限値及び下限値を算出する。

対象物の位置情報が所定数だけあれば、各位置情報から移動体に対する対象物の接近方向ベクトルを予測することが可能となる。そこで、移動体に対する対象物の接近方向ベクトルと予め設定された補正角度とに基づいて対象物の接近方向領域を求め、この接近方向領域と相対速度情報とに基づいて対象物の移動速度の上限値及び下限値を求めることにより、対象物の接近方向に応じた適切な移動速度の上限値及び下限値を得ることができる。これにより、移動体に接近する対象物の移動速度をより高精度に求めることができる。

また、好ましくは、位置情報取得手段で取得された対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を更に備え、速度制限値算出手段は、対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないときは、予め設定された２つの方向データと相対速度情報とに基づいて、対象物の移動速度の上限値及び下限値を算出する。

対象物の位置情報の有効点数が少なくとも２点あれば、２つの方向データと相対速度情報とに基づいて対象物の移動速度の上限値及び下限値を得ることが可能である。この場合には、移動体に接近する対象物の移動速度を高精度に且つ簡単に求めることができる。

また、本発明は、移動体に搭載され、レーダを用いて対象物の移動速度を検出する物体速度検出装置において、レーダの計測データから対象物の位置情報を取得する位置情報取得手段と、対象物の位置情報に基づいて対象物の第１移動速度を算出する第１速度算出手段と、レーダの計測データから移動体と対象物との相対速度情報を取得する相対速度情報取得手段と、相対速度情報に基づいて対象物の第２移動速度を算出する第２速度算出手段と、第１速度算出手段で算出された第１移動速度と第２速度算出手段で算出された第２移動速度とに基づいて、対象物の移動速度を求める速度決定手段とを備えることを特徴とするものである。

このような本発明の物体速度検出装置においては、対象物の位置情報に基づいて対象物の第１移動速度を算出すると共に、移動体と対象物との相対速度情報に基づいて対象物の第２移動速度を算出し、それらの第１移動速度及び第２移動速度に基づいて対象物の移動速度を求めることにより、両者の算出方法のメリットを享受した移動速度を得ることが可能となる。これにより、対象物の位置情報のみに基づいて対象物の移動速度を求める場合に比べて、移動体に接近する対象物の移動速度を高精度に求めることができる。

好ましくは、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段を更に備え、速度決定手段は、対象物の接近方向ベクトルとレーダの計測半径方向との乖離度に基づいて、第１移動速度及び第２移動速度の重み付けを決定し、対象物の移動速度を求める。

移動体と対象物との相対速度情報に基づいて対象物の第２移動速度を算出する際には、対象物の接近方向がレーダの計測半径方向に近くなるほど算出精度が良くなる。従って、対象物の接近方向ベクトルとレーダの計測半径方向との乖離度に基づいて、第１移動速度及び第２移動速度の重み付けを決定することで、移動体に接近する対象物の移動速度をより高精度に求めることができる。

本発明によれば、対象物の移動速度を高精度に求めることができる。これにより、その後工程で例えば移動体と対象物とが衝突するか否かの判断処理を実施する場合に、衝突判断を高精度に行うことが可能となる。

以下、本発明に係わる物体速度検出装置の好適な実施形態について、図面を参照して詳細に説明する。

図１は、本発明に係わる物体速度検出装置の一実施形態の概略構成を示すブロック図である。同図において、本実施形態の物体速度検出装置１は、例えば車両に搭載されるＰＣＳ（プリクラッシュセーフティシステム）に適用されるものである。

物体速度検出装置１は、ミリ波レーダ２と、車輪速センサ３と、これらのミリ波レーダ２及び車輪速センサ３と接続された速度算出ＥＣＵ（Electronic Control Unit）４とを備えている。

ミリ波レーダ２は、自車に対して接近している相手車にミリ波（例えば76GHz〜77GHz）を当てて、相手車から反射してきたミリ波を受信し、伝搬時間やドップラー効果によって生じる周波数差などから、相手車の位置や自車との相対速度を測定するためのセンサである。車輪速センサ３は、各車輪の回転速度をパルス信号として検出する。

速度算出ＥＣＵ４は、相手車位置演算部５と、相対速度演算部６と、相手車軌道演算部７と、自車速演算部８と、相手車速演算部９とを有している。このような速度算出ＥＣＵ４には、衝突判断ＥＣＵ１０が接続されている。

相手車位置演算部５は、ミリ波レーダ２の計測データから、自車に対する相手車の位置を計算する。相対速度演算部６は、ミリ波レーダ２の計測データから、自車と相手車との相対速度を計算する。相手車軌道演算部７は、相手車の位置情報の履歴から、最小自乗法等を用いて相手車の軌道（接近方向）を計算する。自車速演算部８は、車輪速センサ３の検出値から、自車の車速を計算する。相手車速演算部９は、相手車の位置、自車と相手車との相対速度、相手車の軌道及び自車の車速に基づいて、相手車の車速（走行速度）を求める。

速度算出ＥＣＵ４で得られた各種計算データは、衝突判断ＥＣＵ１０に送られる。そして、衝突判断ＥＣＵ１０は、各種計算データに基づいて、自車と相手車との衝突の可能性を判定する。

図２は、相手車速演算部９により実行される相手車速の計算処理の手順を示すフローチャートである。

同図において、まずミリ波レーダ２からの物標（ここでは自車に接近する相手車）情報を取得する（手順Ｓ１０１）。具体的には、相手車位置演算部５で求めた相手車の位置、相対速度演算部６で求めた自車と相手車との相対速度、相手車軌道演算部７で求めた相手車の軌道についての情報を取得する。

続いて、ミリ波レーダ２からの物標情報の有効点数が２点以上であるかどうかを判断する（手順Ｓ１０２）。ここで、物標情報の有効点数とは、外挿時などを除き、ミリ波レーダ２が安定して捕捉した物標の履歴点数を表す。

物標情報の有効点数が２点以上であるときは、ミリ波レーダ２からの物標情報の有効点数が５点以上であるか、または前回の処理で後述する手順Ｓ１０８，Ｓ１０９を実行したかどうかを判断する（手順Ｓ１０３）。

物標情報の有効点数が所定点数（ここでは５点）以上であるか、または前回の処理で後述する手順Ｓ１０８，Ｓ１０９を実行したときは、相手車位置演算部５で求めた相手車の位置情報について、既存の手法であるαβフィルタを用いた位置微分を行うことにより、ミリ波レーダ２の捕捉点を地上固定座標軸に変換し、位置のばらつきを抑えながら相手車の車速を算出する（手順Ｓ１０４）。

αβフィルタを用いた位置微分を行って相手車の車速を算出する考え方を図３に示す。同図において、まずαβフィルタ式により相手車の予測点を求める。そして、その予測点と捕捉点（ミリ波レーダ情報）との距離を１の割合で考え、その区間を１−α：αの割合で分け、平滑点を求める。この平滑点をその時刻ｋでの相手車の位置とする。

予測点（Ｘ_ｐ（ｋ），Ｙ_ｐ（ｋ））は、以下のように更新される。
Ｘ_ｐ（ｋ）＝Ｘ_ｓ（ｋ−１）＋ＴＶ_ｓｘ（ｋ−１）
Ｙ_ｐ（ｋ）＝Ｙ_ｓ（ｋ−１）＋ＴＶ_ｓｙ（ｋ−１）

平滑点（Ｘ_ｓ（ｋ），Ｙ_ｓ（ｋ））は、以下のように更新される。なお、Ｘ_ｍ（ｊ），Ｙ_ｍ（ｊ）は、捕捉点の値である。
Ｘ_ｓ（ｋ）＝Ｘ_ｐ（ｋ）＋α［Ｘ_ｍ（ｊ）−Ｘ_ｐ（ｋ）］
Ｙ_ｓ（ｋ）＝Ｙ_ｐ（ｋ）＋α［Ｙ_ｍ（ｊ）−Ｙ_ｐ（ｋ）］

但し、初回時に必要なＸ_ｓ（０）、Ｙ_ｓ（０）、Ｖ_ｓｘ（０）、Ｖ_ｓｙ（０）は、以下のように定義する。
Ｘ_ｓ（０）＝Ｘ_ｍ（４）
Ｙ_ｓ（０）＝Ｙ_ｍ（４）
Ｖ_ｓｘ（０）＝Ｖ_ｎｘ
Ｖ_ｓｙ（０）＝Ｖ_ｎｙ

前回算出した平滑速度に、予測点と捕捉点との残差をβの割合分補正することで、今回の平滑速度を求める。そして、これら各方向の平滑速度から、相手車の車速Ｖ_ｎｘｙを算出する。なお、Ｖ_ｓｘ（ｋ），Ｖ_ｓｙ（ｋ）は、時刻ｋでの平滑速度であり、Ｔは、有効点におけるサンプリング間隔である。
Ｖ_ｎｘ＝Ｖ_ｓｘ（ｋ）＝Ｖ_ｓｘ（ｋ−１）＋β［Ｘ_ｍ（ｊ）−Ｘ_ｐ（ｋ）］／Ｔ
Ｖ_ｎｙ＝Ｖ_ｓｙ（ｋ）＝Ｖ_ｓｙ（ｋ−１）＋β［Ｙ_ｍ（ｊ）−Ｙ_ｐ（ｋ）］／Ｔ

ここで、αβフィルタのチューニングは、係数（フィルタ定数）α，βで行う。係数α，βの意味は次の通りである。
α：現在の予測点と捕捉点の重み付き平均に係わる係数
０に近いほど予測点の重みが大きくなり、１に近いほど捕捉点の重みが大きく なる
β：前回の平滑速度を元に捕捉点と予測点の残差に応じた速度増加量を加算するた めの係数
０に近いほど前回の平滑速度を維持し、捕捉点と予測点の残差に応じて平滑速 度が変化する

係数α，βの値は、例えば以下のように決定される。

次いで、手順Ｓ１０４で得られた相手車の推定速度に対して、相手車軌道演算部７で求めた相手車予測軌道による速度上下限カットフィルタをかける（手順Ｓ１０５）。

相手車予測軌道による速度上下限カットフィルタの考え方を図４及び図５に示す。各図において、本手順では、相手車軌道演算部７により得られた相手車Ｂの予測軌道（接近方向ベクトルＭ）と、当該接近方向ベクトルＭに対する一定の補正角度ξ_Ｌ＋，ξ_Ｌ−とを用いて、相手車Ｂの接近方向の範囲（接近方向領域）Ｈを求める。さらに、自車速演算部８で求めた自車Ａの車速（以下、単に自車速という）ｖ_０、相対速度演算部６で求めた自車Ａと相手車Ｂとの相対速度（以下、単に相対速度という）ｖ_ｒ、自車速ｖ_０と相対速度ｖ_ｒとのなす角（見通し角）ξ、相手車Ｂの接近方向ベクトルＭと自車速ｖ_０とのなす角、つまり自車Ａと相手車Ｂとが衝突すると仮定した場合に両者が衝突する直前の角度（衝突直前角）δから、相手車Ｂの速度の上下限値を計算する。

具体的には、相対速度ｖ_ｒの相手車成分ｖ_Ａを相手車Ｂの速度下限値ｖ_ｎｘｙ−と決定する。但し、速度下限値ｖ_ｎｘｙ−は、例えば最低値０ｋｍ／ｈとする。
ｖ_ｎｘｙ−＝ｖ_Ａ＝−ｖ_ｒ−ｖ_Ｉ＝−ｖ_ｒ−ｖ_０ｃｏｓ｜ξ｜

また、相対速度ｖ_ｒの相手車成分ｖ_Ａと衝突直前角δと補正角度ξ_Ｌ＋，ξ_Ｌ−とから、相手車Ｂの速度上限値ｖ_ｎｘｙ＋を決定する。

但し、ゼロ割りを防ぐため、｜δ−｜ξ｜｜＋ξ_Ｌ＋＜π／２もしくは｜δ−｜ξ｜｜＋ξ_Ｌ−＜π／２であり、且つ予測軌道線区分が正式で衝突直前角δが算出されている場合は、上記計算式を用い、これ以外の場合は、速度上限値ｖ_ｎｘｙ＋を例えば１８０ｋｍ／ｈとする。また、速度上限値ｖ_ｎｘｙ＋が例えば１５ｋｍ／ｈ以下の場合は、当該速度上限値ｖ_ｎｘｙ＋を１５ｋｍ／ｈとする。また、補正角度ξ_Ｌ＋は例えば６０°であり、補正角度ξ_Ｌ−は例えば０°である。

このようにして求めた相手車の速度下限値ｖ_ｎｘｙ−及び速度上限値ｖ_ｎｘｙ＋を用いて、相手車Ｂの位置情報に基づいた相手車Ｂの車速Ｖ_ｎｘｙを決定する。
Ｖ_ｎｘｙ＜ｖ_ｎｘｙ−の時、
各方向平滑速度の補正：Ｖ_ｎｘ’＝ｖ_ｎｘｙ−×Ｖ_ｎｘ／Ｖ_ｎｘｙ
Ｖ_ｎｙ’＝ｖ_ｎｘｙ−×Ｖ_ｎｙ／Ｖ_ｎｘｙ
推定速度の更新 ：Ｖ_ｎｘｙ＝ｖ_ｎｘｙ−
Ｖ_ｎｘｙ＞ｖ_ｎｘｙ＋の時、
各方向平滑速度の補正：Ｖ_ｎｘ’＝ｖ_ｎｘｙ＋×Ｖ_ｎｘ／Ｖ_ｎｘｙ
Ｖ_ｎｙ’＝ｖ_ｎｘｙ＋×Ｖ_ｎｙ／Ｖ_ｎｘｙ
推定速度の更新 ：Ｖ_ｎｘｙ＝ｖ_ｎｘｙ＋
ｖ_ｎｘｙ−≦Ｖ_ｎｘｙ≦ｖ_ｎｘｙ＋の時、

のままとする。

次いで、相手車の位置情報に基づいた相手車の車速Ｖ_ｎｘｙとは別に、ドップラー相対速度情報に基づいた相手車の速度ｖ_ｎｒを算出する（手順Ｓ１０６）。

相対速度情報から相手車の車速を求める考え方を図６に示す。同図において、本手順では、相対速度ｖ_ｒ、自車速ｖ_０、見通し角ξ、衝突直前角δから、下記式を用いて相対速度情報に基づいた相手車の速度ｖ_ｎｒを算出する。

但し、ゼロ割りを防ぐため、｜δ−｜ξ｜｜＜π／２であり、且つ衝突直前角δが算出されている場合は、上記計算式を用い、これ以外の場合は、相手車の速度ｖ_ｎｒを算出しないこととする。

次いで、手順Ｓ１０４，Ｓ１０５により位置情報から得られた相手車の車速Ｖ_ｎｘｙと、手順Ｓ１０６により相対速度情報から得られた相手車の車速ｖ_ｎｒとを組み合わせて、最終的な相手車の車速ｖ_ｎを算出し、手順Ｓ１１０に移る（手順Ｓ１０７）。

具体的には、下記式を用いて相手車の車速ｖ_ｎを算出する。
ｖ_ｎ＝（１−Ａ_ｐ）ｖ_ｎｒ＋Ａ_ｐ・Ｖ_ｎｘｙ
Ａ_ｐ（０≦Ａ_ｐ≦１）は、車速混合割合である。Ａ_ｐが大きくなるほど相手車の位置情報から算出した車速の重みが大きくなり、Ａ_ｐが小さくなるほど相対速度情報から算出した車速の重みが大きくなる。

ここで、ミリ波レーダ２の取り付け中心軸上の向き、つまりミリ波計測方向（｜δ−｜ξ｜｜＝０）付近に相手車Ｂの速度ベクトルがある場合には、ミリ波レーダ２により相対速度を精度良く検出することができる。しかし、ミリ波レーダ２の取り付け中心軸上とは異なる向きに相手車Ｂの速度ベクトルがある場合には、中心軸に垂直な速度成分つまりドップラーで見えない速度成分が多くなるため、ミリ波レーダ２による相対速度の検出精度が悪くなる。

そこで、例えば表１に示すようなマップを用いて、｜δ−｜ξ｜｜の値からＡ_ｐ’を求め、その線形補間によりＡ_ｐを求める。但し、車速ｖ_ｎｒが算出されていない場合、Ａ_ｐが１を越える場合には、Ａ_ｐを１とする。

上記の手順Ｓ１０３において、物標情報の有効点数が５点以上でなく２点以上５点未満であり、且つ前回の処理で手順Ｓ１０８，Ｓ１０９を実行していないときは、相手車位置演算部５で求めた相手車の位置情報について、αβフィルタを用いない一般的な位置微分を行って、相手車の車速を算出する（手順Ｓ１０８）。

具体的には、ミリ波レーダ２の捕捉点を地上固定座標軸に変換し、時系列で最も古い捕捉点と最も新しい捕捉点との距離を間隔時間で割り、各方向の速度を求め、各方向の速度から相手車の車速Ｖ_ｎｘｙを算出する。なお、Ｘ_ｍ（ｊ），Ｙ_ｍ（ｊ）は、捕捉点の値であり、Ｔは、有効点におけるサンプリング間隔である。
Ｖ_ｎｘ（ｊ）＝［Ｘ_ｍ（ｊ）−Ｘ_ｍ（１）］／Ｔ
Ｖ_ｎｙ（ｊ）＝［Ｙ_ｍ（ｊ）−Ｙ_ｍ（１）］／Ｔ

次いで、手順Ｓ１０８で得られた相手車の推定速度に対して、予め設定された水平・垂直方向による速度上下限カットフィルタをかけ、手順Ｓ１１０に移る（手順Ｓ１０９）。

水平・垂直方向による速度上下限カットフィルタの考え方を図７及び図８に示す。各図において、垂直方向は、自車の進行方向とし、水平方向は、自車の進行方向に垂直な方向としてある。ただし、水平・垂直方向の決め方は、特にこれに限られない。

本手順では、まず水平方向に対する一定の補正角度ξ_ｈと垂直方向に対する一定の補正角度ξ_ｖとを用いて、相手車Ｂの接近方向の範囲（接近方向領域）Ｈを仮定する。そして、自車速ｖ_０、相対速度ｖ_ｒ、見通し角ξからフィルタ値を算出し、相手車Ｂの速度の上下限値を計算する。

具体的には、相手車Ｂの速度下限値ｖ_ｎｘｙ−は、以下のように決定する。但し、速度下限値ｖ_ｎｘｙ−は、例えば最低値０ｋｍ／ｈとする。
ｖ_ｎｘｙ−＝−ｖ_ｒ−ｖ_Ｉ＝−ｖ_ｒ−ｖ_０ｃｏｓ｜ξ｜＝ｖ_Ａ

また、相手車Ｂの速度上限値ｖ_ｎｘｙ＋は、以下のように決定する。

但し、ゼロ割りを防ぐため、２／π−｜ξ｜＋ξ_ｈ＜π／２もしくは｜ξ｜＋ξ_ｖ＜π／２の場合は、上記計算式を用い、これ以外の場合は、速度上限値ｖ_ｎｘｙ＋を例えば１８０ｋｍ／ｈとする。また、速度上限値ｖ_ｎｘｙ＋が例えば１５ｋｍ／ｈ以下の場合は、当該速度上限値ｖ_ｎｘｙ＋を１５ｋｍ／ｈとする。また、補正角度ξ_ｈは、例えば５°であり、補正角度ξ_ｖは、例えば０°である。

このようにして求めた相手車Ｂの速度下限値ｖ_ｎｘｙ−及び速度上限値ｖ_ｎｘｙ＋を用いて、相手車Ｂの車速Ｖ_ｎｘｙを決定する。
Ｖ_ｎｘｙ＜ｖ_ｎｘｙ−の時、
各方向速度の補正：Ｖ_ｎｘ’＝ｖ_ｎｘｙ−×Ｖ_ｎｘ／Ｖ_ｎｘｙ
Ｖ_ｎｙ’＝ｖ_ｎｘｙ−×Ｖ_ｎｙ／Ｖ_ｎｘｙ
推定速度の更新 ：Ｖ_ｎｘｙ＝ｖ_ｎｘｙ−
Ｖ_ｎｘｙ＞ｖ_ｎｘｙ＋の時、
各方向速度の補正：Ｖ_ｎｘ’＝ｖ_ｎｘｙ＋×Ｖ_ｎｘ／Ｖ_ｎｘｙ
Ｖ_ｎｙ’＝ｖ_ｎｘｙ＋×Ｖ_ｎｙ／Ｖ_ｎｘｙ
推定速度の更新 ：Ｖ_ｎｘｙ＝ｖ_ｎｘｙ＋
ｖ_ｎｘｙ−≦Ｖ_ｎｘｙ≦ｖ_ｎｘｙ＋の時、

のままとする。

そして、その算出した車速Ｖ_ｎｘｙを相手車Ｂの車速ｖ_ｎとする。
ｖ_ｎ＝Ｖ_ｎｘｙ

手順Ｓ１１０では、手順Ｓ１０７または手順Ｓ１０９で得られた相手車の車速ｖ_ｎのデータを衝突判断ＥＣＵ１０に送出する。

また、上記の手順Ｓ１０２において、ミリ波レーダ２からの物標情報の有効点数が２点未満であると判断されたときは、有効点数が極めて少なく、原理的に相手車の車速を求めることはできないので、相手車の車速ｖ_ｎとして無効値データを衝突判断ＥＣＵ１０に送出する（手順Ｓ１１１）。

以上において、ミリ波レーダ２、相手車位置演算部５及び相手車速演算部９の手順Ｓ１０１は、対象物の位置情報を取得する位置情報取得手段を構成する。相手車速演算部９の手順Ｓ１０４，Ｓ１０８は、対象物の位置情報に基づいて対象物の移動速度を算出する速度算出手段を構成する。ミリ波レーダ２、相対速度演算部６及び相手車速演算部９の手順Ｓ１０１は、移動体と対象物との相対速度情報を取得する相対速度情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０５，Ｓ１０９は、相対速度情報を用いて、対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段を構成する。

相手車軌道演算部７は、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段を構成する。相手車速演算部９の手順Ｓ１０１，Ｓ１０５は、接近方向ベクトルと予め設定された補正角度とに基づいて、対象物の接近方向領域を求める手段を構成する。

相手車速演算部９の手順Ｓ１０２，Ｓ１０３は、位置情報取得手段で取得された対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を構成する。

相手車位置演算部５及び相手車速演算部９の手順Ｓ１０１は、レーダ２の計測データから対象物の位置情報を取得する位置情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０４，Ｓ１０５は、対象物の位置情報に基づいて対象物の第１移動速度を算出する第１速度算出手段を構成する。相対速度演算部６及び相手車速演算部９の手順Ｓ１０１は、レーダ２の計測データから移動体と対象物との相対速度情報を取得する相対速度情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０６は、相対速度情報に基づいて対象物の第２移動速度を算出する第２速度算出手段を構成する。相手車速演算部９の手順Ｓ１０７は、第１速度算出手段で算出された第１移動速度と第２速度算出手段で算出された第２移動速度とに基づいて、対象物の移動速度を求める速度決定手段を構成する。

ところで、ミリ波レーダ２から得られる相手車の位置情報を微分することで、相手車の走行速度を算出することはできる。しかし、ＰＣＳにおける演算周期は１６ｍｓと短いため、相手車のわずかな位置情報のずれにより相手車の走行速度の算出結果が大きくばらついてしまう。

これに対し本実施形態においては、ミリ波レーダ２によるミリ波捕捉有効点数が多い場合には、相手車の位置情報の履歴から相手車の接近方向を予測することができることに着目し、まず相手車の位置情報の有効点数が５点以上のときは、αβフィルタを用いた位置微分を行って相手車の車速Ｖ_ｎｘｙを算出した後、相手車の各位置情報の履歴から相手車の接近方向ベクトルを求める。そして、その相手車の接近方向ベクトルと補正角度とを用いて、相手車の接近方向領域を求める。そして、自車に対する相手車の相対速度情報と相手車の接近方向領域とを用いて、相手車の速度上限値及び速度下限値を求め、必要に応じて位置微分により求めた相手車の車速Ｖ_ｎｘｙを補正する。これにより、相手車の位置情報のみから算出した相手車の車速データのばらつきを抑えることができる。

また、相手車の位置情報から得られた相手車の車速Ｖ_ｎｘｙとは別に、自車に対する相手車の相対速度情報から相手車の車速ｖ_ｎｒを求める。ここで、位置情報のみから車速を求める方法では、相手車の接近方向によらず安定して速度を算出することができるが、全体としてばらつきが大きい。一方、相対速度情報のみから車速を求める方法では、相手車の接近方向ベクトルがミリ波レーダ２の計測半径方向（ミリ波計測方向）に近いものに対しては高い精度で算出できるが、相手車の接近方向ベクトルがミリ波レーダ２の計測半径方向から遠くなると算出精度が悪くなってしまう。

本実施形態では、位置情報から得られた相手車の車速Ｖ_ｎｘｙと相対速度情報から得られた相手車の車速ｖ_ｎｒとを組み合わせ、相手車の接近方向ベクトルに対するミリ波レーダ２の計測半径方向の乖離度に応じて両者の混合比率（重み付け）を変えるようにしたので、両者の車速算出方法の長所を活かして、相手車の車速をより高精度に算出することができる。

一方、ミリ波レーダ２によるミリ波捕捉有効点数が少ない場合には、相手車の位置情報の履歴から相手車の接近方向を予測することはできないが、相手車が接近していること自体は分かる。そこで、相手車の位置情報の有効点数が２点以上５点未満のときは、位置微分を行って相手車の車速Ｖ_ｎｘｙを算出した後、水平方向・垂直方向に対する補正角度を用いて相手車の接近方向領域を仮定し、自車に対する相手車の相対速度情報と相手車の接近方向領域とを用いて、相手車の速度上限値及び速度下限値を求め、必要に応じて位置微分により求めた相手車の車速Ｖ_ｎｘｙを補正する。この場合にも、相手車の位置情報のみから算出した相手車の車速データのばらつきを抑えることができる。

以上のように本実施形態によれば、相手車の位置情報と自車に対する相手車の相対速度情報との両方を用いて相手車の走行速度の算出を行うことにより、相手車の走行速度を高精度に検出することができる。その結果、衝突判断ＥＣＵ１０における自車と相手車との衝突判断を精度良く行うことが可能となる。

次に、上述した車速算出ロジックを用いて、相手車の車速を推定する実験を行ったので、その内容について説明する。ここでは、図９に示すように、自車Ａと相手車Ｂがそれぞれ直角方向に４０ｋｍ／ｈで走行するという試験パターンを採用した。

相手車の車速の推定結果を図１０に示す。図１０において、グラフの横軸は、ＴＴＣ（Time to Collision）を示し、グラフの縦軸は、相手車の車速の推定値を示している。また、表中の黒菱形印Ｐは、通常の位置微分（αβフィルタを用いない位置微分）による車速算出結果を表し、表中の黒四角印Ｑは、αβフィルタを用いた位置微分による車速算出結果を表し、表中の白三角印Ｒは、αβフィルタを用いた位置微分によって得られた車速に対して上下限カットフィルタをかけたときの車速算出結果を表し、表中の＊印Ｓは、上記の上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせたときの車速算出結果を表している。また、実線Ｔは、実際の車速（４０ｋｍ／ｈ）を表している。なお、車速推定のためのパラメータ適合値としては、上述した数値をそのまま使用した。

図１０に示す推定結果から明らかなように、αβフィルタを用いた位置微分、上下限カットフィルタ、位置情報から得られた車速と相対速度情報から得られた車速との組み合わせを採用していくに従い、相手車の車速の推定精度がより高くなっている。従って、本発明の有効性が実証されたこととなる。

なお、本発明は、上記実施形態に限定されるものではない。例えば上記実施形態では、ミリ波レーダ２を用いて相手車の位置情報や相対速度情報を取得するようにしたが、使用するレーダとしては、特にミリ波レーダに限られず、レーザレーダ等であっても良い。

また、上記実施形態では、相手車予測軌道による速度上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせて、最終的な相手車の車速を求めるようにしたが、水平・垂直方向による速度上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせて、最終的な相手車の車速を求めることも可能である。

さらに、上記実施形態は、ＰＣＳに適用されたものであるが、本発明の物体速度検出装置は、ＰＣＳ以外のシステムにも適用可能である。また、本発明の物体速度検出装置は、自動車等の車両以外の移動物体の検出にも適用可能である。

本発明に係わる物体速度検出装置の一実施形態の概略構成を示すブロック図である。 図１に示した相手車速演算部により実行される相手車車速の計算処理の手順を示すフローチャートである。 αβフィルタを用いた位置微分を行って相手車の車速を算出する考え方を示す概念図である。 相手車予測軌道による速度上下限カットフィルタの考え方を示す概念図である。 相手車予測軌道による速度上下限カットフィルタの考え方を示す概念図である。 相対速度情報から相手車の車速を求める考え方を示す概念図である。 水平・垂直方向による速度上下限カットフィルタの考え方を示す概念図である。 水平・垂直方向による速度上下限カットフィルタの考え方を示す概念図である。 相手車の車速を推定する試験パターンを示す図である。 図９に示した試験パターンにより相手車の車速を推定した結果を示すグラフである。

符号の説明

１…物体速度検出装置、２…ミリ波レーダ（位置情報取得手段、相対速度情報取得手段）、４…速度算出ＥＣＵ、５…相手車位置演算部（位置情報取得手段）、６…相対速度演算部（相対速度情報取得手段）、７…相手車軌道演算部、８…自車速演算部（速度制限値算出手段、第１速度算出手段、第２速度算出手段）、９…相手車速演算部（位置情報取得手段、速度算出手段、相対速度情報取得手段、速度制限値算出手段、第１速度算出手段、第２速度算出手段、速度決定手段）、Ａ…自車（移動体）、Ｂ…相手車（対象物）。

Claims (5)

1. 移動体に搭載され、対象物の移動速度を検出する物体速度検出装置において、
   前記対象物の位置情報を取得する位置情報取得手段と、
   前記対象物の位置情報に基づいて前記対象物の移動速度を算出する速度算出手段と、
   前記移動体と前記対象物との相対速度情報を取得する相対速度情報取得手段と、
   前記相対速度情報を用いて、前記対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段とを備えることを特徴とする物体速度検出装置。
2. 前記対象物の位置情報に基づいて、前記移動体に対する前記対象物の接近方向ベクトルを求める手段と、
   前記接近方向ベクトルと予め設定された補正角度とに基づいて、前記対象物の接近方向領域を求める手段とを更に備え、
   前記速度制限値算出手段は、前記対象物の接近方向領域と前記相対速度情報とに基づいて、前記対象物の移動速度の上限値及び下限値を算出することを特徴とする請求項１記載の物体速度検出装置。
3. 前記位置情報取得手段で取得された前記対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を更に備え、
   前記速度制限値算出手段は、前記対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないときは、予め設定された２つの方向データと前記相対速度情報とに基づいて、前記対象物の移動速度の上限値及び下限値を算出することを特徴とする請求項１または２記載の物体速度検出装置。
4. 移動体に搭載され、レーダを用いて対象物の移動速度を検出する物体速度検出装置において、
   前記レーダの計測データから前記対象物の位置情報を取得する位置情報取得手段と、
   前記対象物の位置情報に基づいて前記対象物の第１移動速度を算出する第１速度算出手段と、
   前記レーダの計測データから前記移動体と前記対象物との相対速度情報を取得する相対速度情報取得手段と、
   前記相対速度情報に基づいて前記対象物の第２移動速度を算出する第２速度算出手段と、
   前記第１速度算出手段で算出された第１移動速度と前記第２速度算出手段で算出された第２移動速度とに基づいて、前記対象物の移動速度を求める速度決定手段とを備えることを特徴とする物体速度検出装置。
5. 前記対象物の位置情報に基づいて、前記移動体に対する前記対象物の接近方向ベクトルを求める手段を更に備え、
   前記速度決定手段は、前記対象物の接近方向ベクトルと前記レーダの計測半径方向との乖離度に基づいて、前記第１移動速度及び前記第２移動速度の重み付けを決定し、前記対象物の移動速度を求めることを特徴とする請求項４記載の物体速度検出装置。

Images