(57)【特許請求の範囲】
【請求項１】
移動体に搭載され、対象物の移動速度を検出する物体速度検出装置において、
前記対象物の位置情報を取得する位置情報取得手段と、
前記対象物の位置情報に基づいて前記対象物の移動速度を算出する速度算出手段と、
前記移動体と前記対象物との相対速度情報を取得する相対速度情報取得手段と、
前記相対速度情報を用いて、前記対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段とを備えることを特徴とする物体速度検出装置。
【請求項２】
前記対象物の位置情報に基づいて、前記移動体に対する前記対象物の接近方向ベクトルを求める手段と、
前記接近方向ベクトルと予め設定された補正角度とに基づいて、前記対象物の接近方向領域を求める手段とを更に備え、
前記速度制限値算出手段は、前記対象物の接近方向領域と前記相対速度情報とに基づいて、前記対象物の移動速度の上限値及び下限値を算出することを特徴とする請求項１記載の物体速度検出装置。
【請求項３】
前記位置情報取得手段で取得された前記対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を更に備え、
前記速度制限値算出手段は、前記対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないときは、予め設定された２つの方向データと前記相対速度情報とに基づいて、前記対象物の移動速度の上限値及び下限値を算出することを特徴とする請求項１または２記載の物体速度検出装置。
【請求項４】
移動体に搭載され、レーダを用いて対象物の移動速度を検出する物体速度検出装置において、
前記レーダの計測データから前記対象物の位置情報を取得する位置情報取得手段と、
前記対象物の位置情報に基づいて前記対象物の第１移動速度を算出する第１速度算出手段と、
前記レーダの計測データから前記移動体と前記対象物との相対速度情報を取得する相対速度情報取得手段と、
前記相対速度情報に基づいて前記対象物の第２移動速度を算出する第２速度算出手段と、
前記第１速度算出手段で算出された第１移動速度と前記第２速度算出手段で算出された第２移動速度とに基づいて、前記対象物の移動速度を求める速度決定手段とを備えることを特徴とする物体速度検出装置。
【請求項５】
前記対象物の位置情報に基づいて、前記移動体に対する前記対象物の接近方向ベクトルを求める手段を更に備え、
前記速度決定手段は、前記対象物の接近方向ベクトルと前記レーダの計測半径方向との乖離度に基づいて、前記第１移動速度及び前記第２移動速度の重み付けを決定し、前記対象物の移動速度を求めることを特徴とする請求項４記載の物体速度検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両等の対象物の移動速度を検出する物体速度検出装置に関するものである。
【背景技術】
【０００２】
従来の物体速度検出装置としては、例えば特許文献１に記載されているように、自車の走行にとって障害となる移動体の位置観測データを入力し、移動体の速度を直線運動モデルに基づいて推定するカルマンフィルタを用いて、当該障害となる移動体の速度を算出するものが知られている。
【特許文献１】特開平８−１２２４３２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
上記従来技術においては、移動体の位置観測データから移動体の速度を計算することはできる。しかし、短いサンプリング周期が必要とされるＰＣＳ（プリクラッシュセーフティシステム）等に物体速度検出装置が適用される場合には、僅かな位置のずれが速度の算出結果に大きな影響を与えてしまう。カルマンフィルタを用いることで、ある程度の位置のばらつきは抑制されるが、初期値の与え方によっては収束までの時間が長くなってしまうという問題がある。このため、位置情報のみから移動体の速度を算出することには限界がある。
【０００４】
本発明の目的は、対象物の移動速度を高精度に求めることができる物体速度検出装置を提供することである。
【課題を解決するための手段】
【０００５】
本発明は、移動体に搭載され、対象物の移動速度を検出する物体速度検出装置において、対象物の位置情報を取得する位置情報取得手段と、対象物の位置情報に基づいて対象物の移動速度を算出する速度算出手段と、移動体と対象物との相対速度情報を取得する相対速度情報取得手段と、相対速度情報を用いて、対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段とを備えることを特徴とするものである。
【０００６】
このような本発明の物体速度検出装置においては、移動体と対象物との相対速度情報を取得し、この相対速度情報を用いて対象物の移動速度の上限値及び下限値を求めることにより、対象物の位置情報に基づいて得られた対象物の移動速度のばらつきが抑えられるようになる。これにより、対象物の位置情報のみに基づいて対象物の移動速度を求める場合に比べて、移動体に接近する対象物の移動速度を高精度に求めることができる。
【０００７】
好ましくは、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段と、接近方向ベクトルと予め設定された補正角度とに基づいて、対象物の接近方向領域を求める手段とを更に備え、速度制限値算出手段は、対象物の接近方向領域と相対速度情報とに基づいて、対象物の移動速度の上限値及び下限値を算出する。
【０００８】
対象物の位置情報が所定数だけあれば、各位置情報から移動体に対する対象物の接近方向ベクトルを予測することが可能となる。そこで、移動体に対する対象物の接近方向ベクトルと予め設定された補正角度とに基づいて対象物の接近方向領域を求め、この接近方向領域と相対速度情報とに基づいて対象物の移動速度の上限値及び下限値を求めることにより、対象物の接近方向に応じた適切な移動速度の上限値及び下限値を得ることができる。これにより、移動体に接近する対象物の移動速度をより高精度に求めることができる。
【０００９】
また、好ましくは、位置情報取得手段で取得された対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を更に備え、速度制限値算出手段は、対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないときは、予め設定された２つの方向データと相対速度情報とに基づいて、対象物の移動速度の上限値及び下限値を算出する。
【００１０】
対象物の位置情報の有効点数が少なくとも２点あれば、２つの方向データと相対速度情報とに基づいて対象物の移動速度の上限値及び下限値を得ることが可能である。この場合には、移動体に接近する対象物の移動速度を高精度に且つ簡単に求めることができる。
【００１１】
また、本発明は、移動体に搭載され、レーダを用いて対象物の移動速度を検出する物体速度検出装置において、レーダの計測データから対象物の位置情報を取得する位置情報取得手段と、対象物の位置情報に基づいて対象物の第１移動速度を算出する第１速度算出手段と、レーダの計測データから移動体と対象物との相対速度情報を取得する相対速度情報取得手段と、相対速度情報に基づいて対象物の第２移動速度を算出する第２速度算出手段と、第１速度算出手段で算出された第１移動速度と第２速度算出手段で算出された第２移動速度とに基づいて、対象物の移動速度を求める速度決定手段とを備えることを特徴とするものである。
【００１２】
このような本発明の物体速度検出装置においては、対象物の位置情報に基づいて対象物の第１移動速度を算出すると共に、移動体と対象物との相対速度情報に基づいて対象物の第２移動速度を算出し、それらの第１移動速度及び第２移動速度に基づいて対象物の移動速度を求めることにより、両者の算出方法のメリットを享受した移動速度を得ることが可能となる。これにより、対象物の位置情報のみに基づいて対象物の移動速度を求める場合に比べて、移動体に接近する対象物の移動速度を高精度に求めることができる。
【００１３】
好ましくは、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段を更に備え、速度決定手段は、対象物の接近方向ベクトルとレーダの計測半径方向との乖離度に基づいて、第１移動速度及び第２移動速度の重み付けを決定し、対象物の移動速度を求める。
【００１４】
移動体と対象物との相対速度情報に基づいて対象物の第２移動速度を算出する際には、対象物の接近方向がレーダの計測半径方向に近くなるほど算出精度が良くなる。従って、対象物の接近方向ベクトルとレーダの計測半径方向との乖離度に基づいて、第１移動速度及び第２移動速度の重み付けを決定することで、移動体に接近する対象物の移動速度をより高精度に求めることができる。
【発明の効果】
【００１５】
本発明によれば、対象物の移動速度を高精度に求めることができる。これにより、その後工程で例えば移動体と対象物とが衝突するか否かの判断処理を実施する場合に、衝突判断を高精度に行うことが可能となる。
【発明を実施するための最良の形態】
【００１６】
以下、本発明に係わる物体速度検出装置の好適な実施形態について、図面を参照して詳細に説明する。
【００１７】
図１は、本発明に係わる物体速度検出装置の一実施形態の概略構成を示すブロック図である。同図において、本実施形態の物体速度検出装置１は、例えば車両に搭載されるＰＣＳ（プリクラッシュセーフティシステム）に適用されるものである。
【００１８】
物体速度検出装置１は、ミリ波レーダ２と、車輪速センサ３と、これらのミリ波レーダ２及び車輪速センサ３と接続された速度算出ＥＣＵ（Electronic Control Unit）４とを備えている。
【００１９】
ミリ波レーダ２は、自車に対して接近している相手車にミリ波（例えば76GHz〜77GHz）を当てて、相手車から反射してきたミリ波を受信し、伝搬時間やドップラー効果によって生じる周波数差などから、相手車の位置や自車との相対速度を測定するためのセンサである。車輪速センサ３は、各車輪の回転速度をパルス信号として検出する。
【００２０】
速度算出ＥＣＵ４は、相手車位置演算部５と、相対速度演算部６と、相手車軌道演算部７と、自車速演算部８と、相手車速演算部９とを有している。このような速度算出ＥＣＵ４には、衝突判断ＥＣＵ１０が接続されている。
【００２１】
相手車位置演算部５は、ミリ波レーダ２の計測データから、自車に対する相手車の位置を計算する。相対速度演算部６は、ミリ波レーダ２の計測データから、自車と相手車との相対速度を計算する。相手車軌道演算部７は、相手車の位置情報の履歴から、最小自乗法等を用いて相手車の軌道（接近方向）を計算する。自車速演算部８は、車輪速センサ３の検出値から、自車の車速を計算する。相手車速演算部９は、相手車の位置、自車と相手車との相対速度、相手車の軌道及び自車の車速に基づいて、相手車の車速（走行速度）を求める。
【００２２】
速度算出ＥＣＵ４で得られた各種計算データは、衝突判断ＥＣＵ１０に送られる。そして、衝突判断ＥＣＵ１０は、各種計算データに基づいて、自車と相手車との衝突の可能性を判定する。
【００２３】
図２は、相手車速演算部９により実行される相手車速の計算処理の手順を示すフローチャートである。
【００２４】
同図において、まずミリ波レーダ２からの物標（ここでは自車に接近する相手車）情報を取得する（手順Ｓ１０１）。具体的には、相手車位置演算部５で求めた相手車の位置、相対速度演算部６で求めた自車と相手車との相対速度、相手車軌道演算部７で求めた相手車の軌道についての情報を取得する。
【００２５】
続いて、ミリ波レーダ２からの物標情報の有効点数が２点以上であるかどうかを判断する（手順Ｓ１０２）。ここで、物標情報の有効点数とは、外挿時などを除き、ミリ波レーダ２が安定して捕捉した物標の履歴点数を表す。
【００２６】
物標情報の有効点数が２点以上であるときは、ミリ波レーダ２からの物標情報の有効点数が５点以上であるか、または前回の処理で後述する手順Ｓ１０８，Ｓ１０９を実行したかどうかを判断する（手順Ｓ１０３）。
【００２７】
物標情報の有効点数が所定点数（ここでは５点）以上であるか、または前回の処理で後述する手順Ｓ１０８，Ｓ１０９を実行したときは、相手車位置演算部５で求めた相手車の位置情報について、既存の手法であるαβフィルタを用いた位置微分を行うことにより、ミリ波レーダ２の捕捉点を地上固定座標軸に変換し、位置のばらつきを抑えながら相手車の車速を算出する（手順Ｓ１０４）。
【００２８】
αβフィルタを用いた位置微分を行って相手車の車速を算出する考え方を図３に示す。同図において、まずαβフィルタ式により相手車の予測点を求める。そして、その予測点と捕捉点（ミリ波レーダ情報）との距離を１の割合で考え、その区間を１−α：αの割合で分け、平滑点を求める。この平滑点をその時刻ｋでの相手車の位置とする。
【００２９】
予測点（Ｘｐ（ｋ），Ｙｐ（ｋ））は、以下のように更新される。
Ｘｐ（ｋ）＝Ｘｓ（ｋ−１）＋ＴＶｓｘ（ｋ−１）
Ｙｐ（ｋ）＝Ｙｓ（ｋ−１）＋ＴＶｓｙ（ｋ−１）
【００３０】
平滑点（Ｘｓ（ｋ），Ｙｓ（ｋ））は、以下のように更新される。なお、Ｘｍ（ｊ），Ｙｍ（ｊ）は、捕捉点の値である。
Ｘｓ（ｋ）＝Ｘｐ（ｋ）＋α［Ｘｍ（ｊ）−Ｘｐ（ｋ）］
Ｙｓ（ｋ）＝Ｙｐ（ｋ）＋α［Ｙｍ（ｊ）−Ｙｐ（ｋ）］
【００３１】
但し、初回時に必要なＸｓ（０）、Ｙｓ（０）、Ｖｓｘ（０）、Ｖｓｙ（０）は、以下のように定義する。
Ｘｓ（０）＝Ｘｍ（４）
Ｙｓ（０）＝Ｙｍ（４）
Ｖｓｘ（０）＝Ｖｎｘ
Ｖｓｙ（０）＝Ｖｎｙ
【００３２】
前回算出した平滑速度に、予測点と捕捉点との残差をβの割合分補正することで、今回の平滑速度を求める。そして、これら各方向の平滑速度から、相手車の車速Ｖｎｘｙを算出する。なお、Ｖｓｘ（ｋ），Ｖｓｙ（ｋ）は、時刻ｋでの平滑速度であり、Ｔは、有効点におけるサンプリング間隔である。
Ｖｎｘ＝Ｖｓｘ（ｋ）＝Ｖｓｘ（ｋ−１）＋β［Ｘｍ（ｊ）−Ｘｐ（ｋ）］／Ｔ
Ｖｎｙ＝Ｖｓｙ（ｋ）＝Ｖｓｙ（ｋ−１）＋β［Ｙｍ（ｊ）−Ｙｐ（ｋ）］／Ｔ
【数１】
0005034966.tif 000002
【００３３】
ここで、αβフィルタのチューニングは、係数（フィルタ定数）α，βで行う。係数α，βの意味は次の通りである。
α：現在の予測点と捕捉点の重み付き平均に係わる係数
０に近いほど予測点の重みが大きくなり、１に近いほど捕捉点の重みが大きく          なる
β：前回の平滑速度を元に捕捉点と予測点の残差に応じた速度増加量を加算するた          めの係数
０に近いほど前回の平滑速度を維持し、捕捉点と予測点の残差に応じて平滑速          度が変化する
【００３４】
係数α，βの値は、例えば以下のように決定される。
【数２】
0005034966.tif 000003
【００３５】
次いで、手順Ｓ１０４で得られた相手車の推定速度に対して、相手車軌道演算部７で求めた相手車予測軌道による速度上下限カットフィルタをかける（手順Ｓ１０５）。
【００３６】
相手車予測軌道による速度上下限カットフィルタの考え方を図４及び図５に示す。各図において、本手順では、相手車軌道演算部７により得られた相手車Ｂの予測軌道（接近方向ベクトルＭ）と、当該接近方向ベクトルＭに対する一定の補正角度ξＬ＋，ξＬ−とを用いて、相手車Ｂの接近方向の範囲（接近方向領域）Ｈを求める。さらに、自車速演算部８で求めた自車Ａの車速（以下、単に自車速という）ｖ０、相対速度演算部６で求めた自車Ａと相手車Ｂとの相対速度（以下、単に相対速度という）ｖｒ、自車速ｖ０と相対速度ｖｒとのなす角（見通し角）ξ、相手車Ｂの接近方向ベクトルＭと自車速ｖ０とのなす角、つまり自車Ａと相手車Ｂとが衝突すると仮定した場合に両者が衝突する直前の角度（衝突直前角）δから、相手車Ｂの速度の上下限値を計算する。
【００３７】
具体的には、相対速度ｖｒの相手車成分ｖＡを相手車Ｂの速度下限値ｖｎｘｙ−と決定する。但し、速度下限値ｖｎｘｙ−は、例えば最低値０ｋｍ／ｈとする。
ｖｎｘｙ−＝ｖＡ＝−ｖｒ−ｖＩ＝−ｖｒ−ｖ０ｃｏｓ｜ξ｜
【００３８】
また、相対速度ｖｒの相手車成分ｖＡと衝突直前角δと補正角度ξＬ＋，ξＬ−とから、相手車Ｂの速度上限値ｖｎｘｙ＋を決定する。
【数３】
0005034966.tif 000004
【００３９】
但し、ゼロ割りを防ぐため、｜δ−｜ξ｜｜＋ξＬ＋＜π／２もしくは｜δ−｜ξ｜｜＋ξＬ−＜π／２であり、且つ予測軌道線区分が正式で衝突直前角δが算出されている場合は、上記計算式を用い、これ以外の場合は、速度上限値ｖｎｘｙ＋を例えば１８０ｋｍ／ｈとする。また、速度上限値ｖｎｘｙ＋が例えば１５ｋｍ／ｈ以下の場合は、当該速度上限値ｖｎｘｙ＋を１５ｋｍ／ｈとする。また、補正角度ξＬ＋は例えば６０°であり、補正角度ξＬ−は例えば０°である。
【００４０】
このようにして求めた相手車の速度下限値ｖｎｘｙ−及び速度上限値ｖｎｘｙ＋を用いて、相手車Ｂの位置情報に基づいた相手車Ｂの車速Ｖｎｘｙを決定する。
Ｖｎｘｙ＜ｖｎｘｙ−の時、
各方向平滑速度の補正：Ｖｎｘ’＝ｖｎｘｙ−×Ｖｎｘ／Ｖｎｘｙ
Ｖｎｙ’＝ｖｎｘｙ−×Ｖｎｙ／Ｖｎｘｙ
推定速度の更新      ：Ｖｎｘｙ＝ｖｎｘｙ−
Ｖｎｘｙ＞ｖｎｘｙ＋の時、
各方向平滑速度の補正：Ｖｎｘ’＝ｖｎｘｙ＋×Ｖｎｘ／Ｖｎｘｙ
Ｖｎｙ’＝ｖｎｘｙ＋×Ｖｎｙ／Ｖｎｘｙ
推定速度の更新      ：Ｖｎｘｙ＝ｖｎｘｙ＋
ｖｎｘｙ−≦Ｖｎｘｙ≦ｖｎｘｙ＋の時、
【数４】
0005034966.tif 000005
のままとする。
【００４１】
次いで、相手車の位置情報に基づいた相手車の車速Ｖｎｘｙとは別に、ドップラー相対速度情報に基づいた相手車の速度ｖｎｒを算出する（手順Ｓ１０６）。
【００４２】
相対速度情報から相手車の車速を求める考え方を図６に示す。同図において、本手順では、相対速度ｖｒ、自車速ｖ０、見通し角ξ、衝突直前角δから、下記式を用いて相対速度情報に基づいた相手車の速度ｖｎｒを算出する。
【数５】
0005034966.tif 000006
【００４３】
但し、ゼロ割りを防ぐため、｜δ−｜ξ｜｜＜π／２であり、且つ衝突直前角δが算出されている場合は、上記計算式を用い、これ以外の場合は、相手車の速度ｖｎｒを算出しないこととする。
【００４４】
次いで、手順Ｓ１０４，Ｓ１０５により位置情報から得られた相手車の車速Ｖｎｘｙと、手順Ｓ１０６により相対速度情報から得られた相手車の車速ｖｎｒとを組み合わせて、最終的な相手車の車速ｖｎを算出し、手順Ｓ１１０に移る（手順Ｓ１０７）。
【００４５】
具体的には、下記式を用いて相手車の車速ｖｎを算出する。
ｖｎ＝（１−Ａｐ）ｖｎｒ＋Ａｐ・Ｖｎｘｙ
Ａｐ（０≦Ａｐ≦１）は、車速混合割合である。Ａｐが大きくなるほど相手車の位置情報から算出した車速の重みが大きくなり、Ａｐが小さくなるほど相対速度情報から算出した車速の重みが大きくなる。
【００４６】
ここで、ミリ波レーダ２の取り付け中心軸上の向き、つまりミリ波計測方向（｜δ−｜ξ｜｜＝０）付近に相手車Ｂの速度ベクトルがある場合には、ミリ波レーダ２により相対速度を精度良く検出することができる。しかし、ミリ波レーダ２の取り付け中心軸上とは異なる向きに相手車Ｂの速度ベクトルがある場合には、中心軸に垂直な速度成分つまりドップラーで見えない速度成分が多くなるため、ミリ波レーダ２による相対速度の検出精度が悪くなる。
【００４７】
そこで、例えば表１に示すようなマップを用いて、｜δ−｜ξ｜｜の値からＡｐ’を求め、その線形補間によりＡｐを求める。但し、車速ｖｎｒが算出されていない場合、Ａｐが１を越える場合には、Ａｐを１とする。
【表１】
0005034966.tif 000007
【００４８】
上記の手順Ｓ１０３において、物標情報の有効点数が５点以上でなく２点以上５点未満であり、且つ前回の処理で手順Ｓ１０８，Ｓ１０９を実行していないときは、相手車位置演算部５で求めた相手車の位置情報について、αβフィルタを用いない一般的な位置微分を行って、相手車の車速を算出する（手順Ｓ１０８）。
【００４９】
具体的には、ミリ波レーダ２の捕捉点を地上固定座標軸に変換し、時系列で最も古い捕捉点と最も新しい捕捉点との距離を間隔時間で割り、各方向の速度を求め、各方向の速度から相手車の車速Ｖｎｘｙを算出する。なお、Ｘｍ（ｊ），Ｙｍ（ｊ）は、捕捉点の値であり、Ｔは、有効点におけるサンプリング間隔である。
Ｖｎｘ（ｊ）＝［Ｘｍ（ｊ）−Ｘｍ（１）］／Ｔ
Ｖｎｙ（ｊ）＝［Ｙｍ（ｊ）−Ｙｍ（１）］／Ｔ
【数６】
0005034966.tif 000008
【００５０】
次いで、手順Ｓ１０８で得られた相手車の推定速度に対して、予め設定された水平・垂直方向による速度上下限カットフィルタをかけ、手順Ｓ１１０に移る（手順Ｓ１０９）。
【００５１】
水平・垂直方向による速度上下限カットフィルタの考え方を図７及び図８に示す。各図において、垂直方向は、自車の進行方向とし、水平方向は、自車の進行方向に垂直な方向としてある。ただし、水平・垂直方向の決め方は、特にこれに限られない。
【００５２】
本手順では、まず水平方向に対する一定の補正角度ξｈと垂直方向に対する一定の補正角度ξｖとを用いて、相手車Ｂの接近方向の範囲（接近方向領域）Ｈを仮定する。そして、自車速ｖ０、相対速度ｖｒ、見通し角ξからフィルタ値を算出し、相手車Ｂの速度の上下限値を計算する。
【００５３】
具体的には、相手車Ｂの速度下限値ｖｎｘｙ−は、以下のように決定する。但し、速度下限値ｖｎｘｙ−は、例えば最低値０ｋｍ／ｈとする。
ｖｎｘｙ−＝−ｖｒ−ｖＩ＝−ｖｒ−ｖ０ｃｏｓ｜ξ｜＝ｖＡ
【００５４】
また、相手車Ｂの速度上限値ｖｎｘｙ＋は、以下のように決定する。
【数７】
0005034966.tif 000009
【００５５】
但し、ゼロ割りを防ぐため、２／π−｜ξ｜＋ξｈ＜π／２もしくは｜ξ｜＋ξｖ＜π／２の場合は、上記計算式を用い、これ以外の場合は、速度上限値ｖｎｘｙ＋を例えば１８０ｋｍ／ｈとする。また、速度上限値ｖｎｘｙ＋が例えば１５ｋｍ／ｈ以下の場合は、当該速度上限値ｖｎｘｙ＋を１５ｋｍ／ｈとする。また、補正角度ξｈは、例えば５°であり、補正角度ξｖは、例えば０°である。
【００５６】
このようにして求めた相手車Ｂの速度下限値ｖｎｘｙ−及び速度上限値ｖｎｘｙ＋を用いて、相手車Ｂの車速Ｖｎｘｙを決定する。
Ｖｎｘｙ＜ｖｎｘｙ−の時、
各方向速度の補正：Ｖｎｘ’＝ｖｎｘｙ−×Ｖｎｘ／Ｖｎｘｙ
Ｖｎｙ’＝ｖｎｘｙ−×Ｖｎｙ／Ｖｎｘｙ
推定速度の更新  ：Ｖｎｘｙ＝ｖｎｘｙ−
Ｖｎｘｙ＞ｖｎｘｙ＋の時、
各方向速度の補正：Ｖｎｘ’＝ｖｎｘｙ＋×Ｖｎｘ／Ｖｎｘｙ
Ｖｎｙ’＝ｖｎｘｙ＋×Ｖｎｙ／Ｖｎｘｙ
推定速度の更新  ：Ｖｎｘｙ＝ｖｎｘｙ＋
ｖｎｘｙ−≦Ｖｎｘｙ≦ｖｎｘｙ＋の時、
【数８】
0005034966.tif 000010
のままとする。
【００５７】
そして、その算出した車速Ｖｎｘｙを相手車Ｂの車速ｖｎとする。
ｖｎ＝Ｖｎｘｙ
【００５８】
手順Ｓ１１０では、手順Ｓ１０７または手順Ｓ１０９で得られた相手車の車速ｖｎのデータを衝突判断ＥＣＵ１０に送出する。
【００５９】
また、上記の手順Ｓ１０２において、ミリ波レーダ２からの物標情報の有効点数が２点未満であると判断されたときは、有効点数が極めて少なく、原理的に相手車の車速を求めることはできないので、相手車の車速ｖｎとして無効値データを衝突判断ＥＣＵ１０に送出する（手順Ｓ１１１）。
【００６０】
以上において、ミリ波レーダ２、相手車位置演算部５及び相手車速演算部９の手順Ｓ１０１は、対象物の位置情報を取得する位置情報取得手段を構成する。相手車速演算部９の手順Ｓ１０４，Ｓ１０８は、対象物の位置情報に基づいて対象物の移動速度を算出する速度算出手段を構成する。ミリ波レーダ２、相対速度演算部６及び相手車速演算部９の手順Ｓ１０１は、移動体と対象物との相対速度情報を取得する相対速度情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０５，Ｓ１０９は、相対速度情報を用いて、対象物の移動速度の上限値及び下限値を算出する速度制限値算出手段を構成する。
【００６１】
相手車軌道演算部７は、対象物の位置情報に基づいて、移動体に対する対象物の接近方向ベクトルを求める手段を構成する。相手車速演算部９の手順Ｓ１０１，Ｓ１０５は、接近方向ベクトルと予め設定された補正角度とに基づいて、対象物の接近方向領域を求める手段を構成する。
【００６２】
相手車速演算部９の手順Ｓ１０２，Ｓ１０３は、位置情報取得手段で取得された対象物の位置情報の有効点数が２点以上であり且つ所定数より少ないかどうかを判断する手段を構成する。
【００６３】
相手車位置演算部５及び相手車速演算部９の手順Ｓ１０１は、レーダ２の計測データから対象物の位置情報を取得する位置情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０４，Ｓ１０５は、対象物の位置情報に基づいて対象物の第１移動速度を算出する第１速度算出手段を構成する。相対速度演算部６及び相手車速演算部９の手順Ｓ１０１は、レーダ２の計測データから移動体と対象物との相対速度情報を取得する相対速度情報取得手段を構成する。自車速演算部８及び相手車速演算部９の手順Ｓ１０６は、相対速度情報に基づいて対象物の第２移動速度を算出する第２速度算出手段を構成する。相手車速演算部９の手順Ｓ１０７は、第１速度算出手段で算出された第１移動速度と第２速度算出手段で算出された第２移動速度とに基づいて、対象物の移動速度を求める速度決定手段を構成する。
【００６４】
ところで、ミリ波レーダ２から得られる相手車の位置情報を微分することで、相手車の走行速度を算出することはできる。しかし、ＰＣＳにおける演算周期は１６ｍｓと短いため、相手車のわずかな位置情報のずれにより相手車の走行速度の算出結果が大きくばらついてしまう。
【００６５】
これに対し本実施形態においては、ミリ波レーダ２によるミリ波捕捉有効点数が多い場合には、相手車の位置情報の履歴から相手車の接近方向を予測することができることに着目し、まず相手車の位置情報の有効点数が５点以上のときは、αβフィルタを用いた位置微分を行って相手車の車速Ｖｎｘｙを算出した後、相手車の各位置情報の履歴から相手車の接近方向ベクトルを求める。そして、その相手車の接近方向ベクトルと補正角度とを用いて、相手車の接近方向領域を求める。そして、自車に対する相手車の相対速度情報と相手車の接近方向領域とを用いて、相手車の速度上限値及び速度下限値を求め、必要に応じて位置微分により求めた相手車の車速Ｖｎｘｙを補正する。これにより、相手車の位置情報のみから算出した相手車の車速データのばらつきを抑えることができる。
【００６６】
また、相手車の位置情報から得られた相手車の車速Ｖｎｘｙとは別に、自車に対する相手車の相対速度情報から相手車の車速ｖｎｒを求める。ここで、位置情報のみから車速を求める方法では、相手車の接近方向によらず安定して速度を算出することができるが、全体としてばらつきが大きい。一方、相対速度情報のみから車速を求める方法では、相手車の接近方向ベクトルがミリ波レーダ２の計測半径方向（ミリ波計測方向）に近いものに対しては高い精度で算出できるが、相手車の接近方向ベクトルがミリ波レーダ２の計測半径方向から遠くなると算出精度が悪くなってしまう。
【００６７】
本実施形態では、位置情報から得られた相手車の車速Ｖｎｘｙと相対速度情報から得られた相手車の車速ｖｎｒとを組み合わせ、相手車の接近方向ベクトルに対するミリ波レーダ２の計測半径方向の乖離度に応じて両者の混合比率（重み付け）を変えるようにしたので、両者の車速算出方法の長所を活かして、相手車の車速をより高精度に算出することができる。
【００６８】
一方、ミリ波レーダ２によるミリ波捕捉有効点数が少ない場合には、相手車の位置情報の履歴から相手車の接近方向を予測することはできないが、相手車が接近していること自体は分かる。そこで、相手車の位置情報の有効点数が２点以上５点未満のときは、位置微分を行って相手車の車速Ｖｎｘｙを算出した後、水平方向・垂直方向に対する補正角度を用いて相手車の接近方向領域を仮定し、自車に対する相手車の相対速度情報と相手車の接近方向領域とを用いて、相手車の速度上限値及び速度下限値を求め、必要に応じて位置微分により求めた相手車の車速Ｖｎｘｙを補正する。この場合にも、相手車の位置情報のみから算出した相手車の車速データのばらつきを抑えることができる。
【００６９】
以上のように本実施形態によれば、相手車の位置情報と自車に対する相手車の相対速度情報との両方を用いて相手車の走行速度の算出を行うことにより、相手車の走行速度を高精度に検出することができる。その結果、衝突判断ＥＣＵ１０における自車と相手車との衝突判断を精度良く行うことが可能となる。
【００７０】
次に、上述した車速算出ロジックを用いて、相手車の車速を推定する実験を行ったので、その内容について説明する。ここでは、図９に示すように、自車Ａと相手車Ｂがそれぞれ直角方向に４０ｋｍ／ｈで走行するという試験パターンを採用した。
【００７１】
相手車の車速の推定結果を図１０に示す。図１０において、グラフの横軸は、ＴＴＣ（Time to Collision）を示し、グラフの縦軸は、相手車の車速の推定値を示している。また、表中の黒菱形印Ｐは、通常の位置微分（αβフィルタを用いない位置微分）による車速算出結果を表し、表中の黒四角印Ｑは、αβフィルタを用いた位置微分による車速算出結果を表し、表中の白三角印Ｒは、αβフィルタを用いた位置微分によって得られた車速に対して上下限カットフィルタをかけたときの車速算出結果を表し、表中の＊印Ｓは、上記の上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせたときの車速算出結果を表している。また、実線Ｔは、実際の車速（４０ｋｍ／ｈ）を表している。なお、車速推定のためのパラメータ適合値としては、上述した数値をそのまま使用した。
【００７２】
図１０に示す推定結果から明らかなように、αβフィルタを用いた位置微分、上下限カットフィルタ、位置情報から得られた車速と相対速度情報から得られた車速との組み合わせを採用していくに従い、相手車の車速の推定精度がより高くなっている。従って、本発明の有効性が実証されたこととなる。
【００７３】
なお、本発明は、上記実施形態に限定されるものではない。例えば上記実施形態では、ミリ波レーダ２を用いて相手車の位置情報や相対速度情報を取得するようにしたが、使用するレーダとしては、特にミリ波レーダに限られず、レーザレーダ等であっても良い。
【００７４】
また、上記実施形態では、相手車予測軌道による速度上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせて、最終的な相手車の車速を求めるようにしたが、水平・垂直方向による速度上下限カットフィルタをかけて得られた車速と相対速度情報から得られた車速とを組み合わせて、最終的な相手車の車速を求めることも可能である。
【００７５】
さらに、上記実施形態は、ＰＣＳに適用されたものであるが、本発明の物体速度検出装置は、ＰＣＳ以外のシステムにも適用可能である。また、本発明の物体速度検出装置は、自動車等の車両以外の移動物体の検出にも適用可能である。
【図面の簡単な説明】
【００７６】
【図１】本発明に係わる物体速度検出装置の一実施形態の概略構成を示すブロック図である。
【図２】図１に示した相手車速演算部により実行される相手車車速の計算処理の手順を示すフローチャートである。
【図３】αβフィルタを用いた位置微分を行って相手車の車速を算出する考え方を示す概念図である。
【図４】相手車予測軌道による速度上下限カットフィルタの考え方を示す概念図である。
【図５】相手車予測軌道による速度上下限カットフィルタの考え方を示す概念図である。
【図６】相対速度情報から相手車の車速を求める考え方を示す概念図である。
【図７】水平・垂直方向による速度上下限カットフィルタの考え方を示す概念図である。
【図８】水平・垂直方向による速度上下限カットフィルタの考え方を示す概念図である。
【図９】相手車の車速を推定する試験パターンを示す図である。
【図１０】図９に示した試験パターンにより相手車の車速を推定した結果を示すグラフである。
【符号の説明】
【００７７】
１…物体速度検出装置、２…ミリ波レーダ（位置情報取得手段、相対速度情報取得手段）、４…速度算出ＥＣＵ、５…相手車位置演算部（位置情報取得手段）、６…相対速度演算部（相対速度情報取得手段）、７…相手車軌道演算部、８…自車速演算部（速度制限値算出手段、第１速度算出手段、第２速度算出手段）、９…相手車速演算部（位置情報取得手段、速度算出手段、相対速度情報取得手段、速度制限値算出手段、第１速度算出手段、第２速度算出手段、速度決定手段）、Ａ…自車（移動体）、Ｂ…相手車（対象物）。
【図１】
0005034966.tif 000011
【図２】
0005034966.tif 000012
【図３】
0005034966.tif 000013
【図４】
0005034966.tif 000014
【図５】
0005034966.tif 000015
【図６】
0005034966.tif 000016
【図７】
0005034966.tif 000017
【図８】
0005034966.tif 000018
【図９】
0005034966.tif 000019
【図１０】
0005034966.tif 000020