JP5746695B2 - 車両の進行路推定装置 - Google Patents

Description

本発明は、車両の進行路推定装置に関し、特に車両の前方の静止物を検知してその位置情報から、車両の進行路を推定することに関する。

特許文献１は、車両の進行路推定装置を開示する。この進行路推定装置では、ヨーレートセンサを用いることなく、検出した障害物に対する自車両の走行状態を表すデータから自車両の進行路を推定する。

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

特許文献１に記載の進行路推定装置では、検出した障害物に対する自車両の走行状態を表すデータを得るために、ヨーレートセンサに代わって舵角センサ等の様々なセンサの出力値を利用する必要がある。また、各センサの出力値の誤差の重畳により自車両の進行路の推定精度が低くなる恐れがある。

そこで、本発明は、この従来技術の問題を軽減あるいは解消すること、すなわち、ヨーレートセンサを用いることなく、言い換えれば車両の前方の物体を検知する物体検知手段のみを用いて、自車両の進行路の推定精度を向上させることを目的とする。

本発明は、車両の進行路推定装置を提供する。その進行路推定装置は、車両に搭載され、該車両の前方の物体を検知する物体検知手段と、検知された物体が静止物であるかを判定する静止物判定手段と、車両の位置を原点とする２次元（ＸＹ）座標上に投影された静止物の時系列の位置データから当該静止物の軌跡を示す近似直線を算出する手段と、２次元（ＸＹ）座標上において、静止物の時系列の位置データにおける中点Ｍ（Ｘ_Ｍ、Ｙ_Ｍ）を通り近似直線に直交する直交直線を算出する手段と、直交直線とＸ軸との交点Ｘｒ（Ｒ、０）から車両の旋回半径（Ｒ）を算出する手段と、を備える。

本発明によれば、１つの静止物に対しての時系列の位置データに着目するため、静止物が１つ以上存在する環境であれば、路面状況等によらず、物体検知手段のみを使用して、精度良く自車両の進行路を推定することが可能となる。

本発明の一形態によると、複数の静止物が存在する場合に各静止物について得られる旋回半径から車両のヨーレートを算出し、算出されたヨーレートの平均値から車両の進行路を推定する手段をさらに備える。

本発明の一形態によれば、複数の静止物が検出できるときは、算出されるヨーレート（旋回半径）の平均を取ることで、万が一異常な値を推定したときにその異常値をなましてその影響を小さくすることが可能となる。

本発明の一実施例に従う、車両の進行路推定装置の構成を示すブロック図である。 本発明の一実施例に従う、物体検知手段等の取り付け位置を説明するための図である。 本発明の一実施例に従う、制御ユニットにおける処理フローを示す図である。 本発明の一実施例に従う、検出した物体候補の位置情報の概念図である。 本発明の一実施例に従う、検出した静止物の履歴（位置変化）を示す図である。 本発明の一実施例に従う、ヨーレートの平均値算出に至るフローを示すブロック図である。

図面を参照しながら本発明の実施の形態を説明する。図１は、本発明の一実施例に従う、車両の進行路推定装置の構成を示すブロック図である。進行路推定装置は、レーダ１０と、レーダ１０によって取得された物体情報（位置、距離等）に基づいて車両の進行路を推定するための制御ユニット１２と、制御ユニット１２からの制御信号に基づいて音または音声で警報を発生するスピーカ１４と、レーダ１０により取得された物体情報から運転者に車両周辺の物体を認識させるための表示を行う表示装置１６とを備える。

レーダ１０は、例えばスキャン式レーザレーダが該当し、各方向にスキャンするビームの物体からの反射光を受光することにより、検出点として各方向における物体（候補）の位置（領域）とそれまでの距離を検出する。なお、レーダ１０は、他の種類のレーダ（例えばミリ波レーダ等）であってもよい。また、レーダ１０の代わりに、赤外線カメラ等のカメラを用いてもよい。さらに、ナビゲーション装置を備える車両においては、スピーカ１４および表示装置１６として、ナビゲーション装置が備える該当機能を利用してもよい。

図１の制御ユニット１２は、その構成（機能）としてブロック１２１〜１２６で示される機能を有する。すなわち、制御ユニット１２は、レーダ１０からの検知信号を受けて車両の前方の物体を検知する物体検知手段１２１と、検知された物体が静止物であるかを判定する静止物判定手段１２２と、車両の位置を原点とするＸＹ座標上に投影された静止物の時系列の位置データから当該静止物の軌跡を示す近似直線を算出する手段１２３と、ＸＹ座標上において、静止物の時系列の位置データにおける中点Ｍ（Ｘ_Ｍ、Ｙ_Ｍ）を通り近似直線に直交する直交直線を算出する手段１２４と、直交直線とＸ軸との交点Ｘｒ（Ｒ、０）から車両の旋回半径Ｒを算出する手段１２５として機能する。

制御ユニット１２は、さらに、複数の静止物が存在する場合に各静止物について得られる旋回半径から車両のヨーレートを算出し、算出されたヨーレートの平均値から車両の進行路を推定する手段１２６としても機能する。

制御ユニット１２は、さらに、自車両の速度（車速）を検出する車速センサ、ブレーキセンサ、ヨーレート（旋回方向への回転角の変化速度）を検出するヨーレートセンサ等からの検出信号を受けて必要な処理をおこなう機能を有する。その処理には、車両の進行路の推定結果、各種センサ値等に基づき車両の制動制御をおこなう制御信号を生成することも含まれる。

各ブロックの機能は、制御ユニット１２が有するコンピュータ（ＣＰＵ）によって実現される。なお、制御ユニット１２の構成は、ナビゲーション装置の中に組み込んでもよい。

制御ユニット１２は、ハードウエア構成として、例えば、入力アナログ信号をデジタル信号に変換するＡ／Ｄ変換回路、デジタル化した画像信号を記憶する画像メモリ、各種演算処理を行う中央演算処理装置（ＣＰＵ）、ＣＰＵが演算に際してデータを記憶するのに使用するＲＡＭ、ＣＰＵが実行するプログラムおよび用いるデータ（テーブル、マップを含む）を記憶するＲＯＭ、スピーカ１４に対する駆動信号および表示装置１６に対する表示信号などを出力する出力回路を備えている。

図２は、本発明の一実施例に従う、図１に示したレーダ１０等の取り付け位置を説明するための図である。レーダ１０は、例えば図２に示すように車両２０の前部バンパー部上、車幅方向の中心部に配置される。図２の符号１６ａは、表示装置１６としてヘッドアップディスプレイ（以下「ＨＵＤ」という）を用いた場合の例を示している。ＨＵＤ１６ａは、図に示すように、車両２０のフロントウインドシールドの運転者の前方視界を妨げない位置に表示画面が表示されるように設けられる。

図３は、本発明の一実施例に従う、制御ユニット１２によって実行される処理フローである。この処理フローは、制御ユニット１２のＣＰＵがメモリに格納している処理プログラムを呼び出して、所定の時間間隔で実行される。

ステップＳ１０において、レーダ１０からの信号を受けて車両前方の物体の位置を特定する。その特定には任意の方法を用いることができるが、例えば以下のようにおこなう。

レーダ１０としてスキャン式レーザレーダを使用して、その検出点群の幅から物体、例えば障害物候補の位置を特定する。具体的には、スキャン式レーザレーダでは、各方向にスキャンするビームの物体からの反射光を受光することにより、検出点として各方向における物体までの距離を測定する。レーザレーダの位置を原点としてレーダの正面方向をＹ軸、横方向をＸ軸とする２次元座標系（図２参照）にこれらの検出点の位置をプロットして検出点の集合を得る。これらの検出点の集合から、各検出点の相互の間隔が所定値以下のものを検出点群としてグループ化し、グループ化した検出点群のうち、広がり幅が所定値以下のものを所定の物体候補（例えば障害物候補）とみなしてその位置を特定する。検出した物体候補の位置情報は順次時系列な情報としてメモリに格納される。

図４は、検出した物体候補の位置情報の概念図である。図４では、ＸＹ座標で表わされる、車両２０の進行方向２１でのレーザレーダで検知可能な領域２２において、１〜８の合計８つの物体２３が検出されている。８つの物体２３には、静止物とそれ以外の物体の双方が含まれ得る。

ステップＳ１１において、検出した物体が静止物体であるか否かを判定する。その判定は、例えば自車両と検出物体との相対速度、すなわち自車速度と検出物体の単位時間当たりの移動量との差分が所定値以下であるか否かでおこなう。あるいは、検出物体の移動ベクトルを求めてその大きさが所定値以下であるか否かで判定してもよい。この判定がＹｅｓの場合は次のステップＳ１２に進み、Ｎｏの場合は処理を終了する。

ステップＳ１２において、静止物の距離（位置）が時系列データとして正しく得られているか否かを判定する。図５は検出した静止物の履歴（位置変化）を示す図である。図５では、ＸＹ座標上に円弧Ｓに沿った静止物の履歴としてｎ個の位置データ（ｘ１、ｙ１）〜（ｘｎ、ｙｎ）が示されている。なお、図５の詳細については後述する。ステップＳ１２の判定は、例えば位置データのＹ座標値を用いて、次の式（１）の関係が成り立つか否かでおこなう。（１）式の判定をおこなうことにより、静止物の位置（距離）が車両の進行にともなって車両に近く（短く）なっているかを検知することができる。

ｙ１＞ｙ２＞ｙ３・・・・＞ｙｎ （１）

この判定がＹｅｓの場合は次のステップＳ１３に進み、Ｎｏの場合は処理を終了する。

ステップＳ１３において、検知された各静止物の時系列の位置データが所定数以上であるか否かを判定する。所定数は、使用するレーダ１０の精度および目標とするヨーレート誤差に基づき予め車両の走行テストにより決定しておく。所定数は例えば３０である。この判定がＹｅｓの場合は次のステップＳ１４に進み、Ｎｏの場合はステップＳ１０に戻ってデータの取得を繰り返す。

ステップＳ１４において、ヨーレートセンサにより検出されたヨーレートの変化量（ｄｅｇ／ｓ）が所定値以内であるか否かを判定する。車両がカーブ入り口やワインディング路など、車両の軌跡が不安定で、ヨーレート変化量が大きい状態にある場合に推定処理を止めるためである。この判定がＹｅｓの場合は次のステップＳ１５に進み、Ｎｏの場合は処理を終了する。

ステップＳ１５において、２次元（ＸＹ）座標上の各静止物の履歴（時系列の位置データ）から近似直線を算出する。図５を参照して説明すると、ＸＹ座標上に円弧Ｓに沿った静止物の履歴として得られたｎ個の位置データ（ｘ１、ｙ１）〜（ｘｎ、ｙｎ）のそれぞれのＸ座標値との差が最小になるように、次式（２）で表わされる１次関数を近似直線Ａとして算出する。

Ｙ ＝ ａＸ ＋ ｂ （２）

なお、近似直線Ａの算出にあたっては最小２乗法等の他の近似関数を求める方法を利用してもよい。

ステップＳ１６において、近似直線Ａに直交する直交直線を算出する。最初に位置データ（ｘ１、ｙ１）〜（ｘｎ、ｙｎ）の中点を求める。具体的には、図５において、例えば位置データの最遠座標（ｘ１、ｙ１）と最近座標（ｘｎ、ｙｎ）を直線で結び、その直線の中点の座標Ｍ（Ｘ_Ｍ、Ｙ_Ｍ）を求める。次に、中点の座標Ｍ（Ｘ_Ｍ、Ｙ_Ｍ）を通り、近似直線Ａに直交する直交直線Ｂを算出する。図５では、直交直線Ｂは次式（３）で表わされる１次関数として得られる。
Ｙ ＝ −（１／ａ）Ｘ ＋ ｃ （３）

ステップＳ１７において、自車両の旋回半径Ｒを算出する。具体的には、ステップＳ１６において得られた直交直線ＢとＸ軸との交点Ｘｒ（Ｒ、０）を求める。その交点ＸｒのＸ座標値Ｒが車両２０の位置を原点（０、０）にした際の旋回半径Ｒ、言い換えれば進行路の曲率半径Ｒとなる。（３）式にＸ＝Ｒを代入し、先の中点の座標Ｍ（Ｘ_Ｍ、Ｙ_Ｍ）を用いると、旋回半径Ｒは次式（４）で表わすことができる。この旋回半径Ｒから自車両の進行軌跡Ｔを推定することができる。
Ｒ＝ａ・ｃ
＝Ｘ_Ｍ＋ａＹ_Ｍ （４）
なお、複数の静止物が検知されている場合は、各静止物の軌跡から対応する旋回半径Ｒが算出される。このように、本発明の一実施形態によれば、基本的にレーダからの検知情報のみに基づき自車両の旋回半径Ｒ、進行軌跡Ｔを推定（算出）することができる。

ステップＳ１８において、検知された複数の静止物についてのデータ、すなわち旋回半径Ｒが並行して得られているか否かを判定する。この判定がＮｏの場合は次のステップＳ１９に進み、Ｙｅｓの場合はステップＳ２０に進む。

ステップＳ１９において、得られた旋回半径Ｒをヨーレートに変換する。具体的には、次式（５）を用いてヨーレートγを算出する。Ｖは自車速度である。

γ ＝ Ｖ ／ Ｒ （５）

ちなみに単位は、Ｒ（ｍ）、Ｖ（ｍ／ｓ）の場合γ（ｒａｄ／ｓ）となるので、γ（ｄｅｇ／ｓ）を得るためには（５）式から得られるγ（ｒａｄ／ｓ）に１８０／πを乗算する。

ステップＳ２０において、ヨーレートの平均値を算出する。図６は、ヨーレートの平均値算出に至るフローをブロックで表わした図である。図６では、図４の８つの物体２３を検知した場合の例に対応している。レーダ１０からの信号を受けて８つの物体２３のデータ（位置情報）を取得する（ブロック３１）。各データから静止物と判定された物体に対応する旋回半径Ｒを算出する（ブロック３２）。各旋回半径Ｒを自車速度Ｖ（ブロック３７）を用いてヨーレートγに変換する（ブロック３３）。以上の内容の詳細は、図３のステップＳ１０〜Ｓ１８において説明した通りである。

ブロック３４において、得られたヨーレートγの平均値を算出する。このように、本発明の一形態によれば、複数の静止物が検出できるときは、ヨーレートの平均を取ることで、万が一異常な値を推定したときにその異常値をなましてその影響を小さくすることが可能となる。

さらに、ヨーレートセンサからのヨーレート検出値が利用できる場合は、その検出値を取得する（ブロック３８）。そして、ローパスフィルタ（ブロック３９）を通した後のヨーレート検出値とヨーレートの平均値との差分を算出する（ブロック３５）。この差分値からヨーレートセンサのドリフト量を得ることができる（ブロック３６）。このドリフト量は、ヨーレートセンサの出力値の補正に用いることができる。このように、本発明の一実施形態によれば、旋回半径Ｒおよびヨーレートγを推定すると同時に、その推定値を用いてヨーレートセンサの検出値の補正をもおこなうことができる。

図３に戻って、ステップＳ２１において、レーダ１０による物体検知が継続しているか否かを判定する。この判定がＹｅｓの場合はステップＳ１０に戻り所定の時間間隔での一連の処理を繰り返し、Ｎｏの場合は処理を終了する。

以上、本発明の実施の形態について説明したが、本発明はこのような実施形態に限定されることはなく、本発明の趣旨を逸脱しない範囲において改変して用いることができる。例えば、上述した実施形態では、レーダ１０を車両の前部パンパー部上に配置しているが、その場所に限らず、例えばフロントグリルの内側に配置し、レーダを照射する範囲をレーダ透過フィルムなどで覆うように構成してもよい。

１０ レーダ
１２ 制御ユニット
１４ スピーカ
１６ 表示装置
１６ａ ＨＵＤ
２０ 車両
２２ レーダ検知領域
２３ 検知物体（静止物）

Claims (4)

1. 車両の前方の物体を検知する検知手段および制御ユニットを備え、車両に搭載され、該車両の進行路を計算する進行路推定装置であって、該制御ユニットは、
   前記検知手段から順次入力を受け取り、該車両の前方の物体を、該車両の位置を原点とする２次元座標上の時系列の位置データとして検出する検出手段と、
   前記検出された前記時系列の位置データに基づいて前記車両の前方の物体が静止物であるか否かを判定する判定手段と、
   静止物と判定されたなら、前記車両の位置を原点とする２次元座標上に投影された静止物の時系列の位置データから当該静止物の軌跡を示す近似直線を算出する手段と、
   前記２次元座標上において、前記静止物の時系列の位置データにおける中点を通り前記近似直線に直交する直交直線を算出する手段と、
   前記直交直線とＸ軸との交点から前記車両の旋回半径を算出する手段と、
   から構成されている、車両の進行路推定装置。
2. 前記制御ユニットは、さらに、複数の静止物が存在する場合に各静止物について得られる前記旋回半径から前記車両のヨーレートを算出し、算出されたヨーレートの平均値から前記車両の進行路を推定する、よう構成されている、請求項１に記載の車両の進行路推定装置。
3. 前記制御ユニットは、さらに、複数の静止物が存在する場合に、各静止物について得られる前記旋回半径と、前記車両の速度とから各静止物に対する前記車両のヨーレートを算出し、
   得られた前記各静止物に対する前記車両のヨーレートの平均値を算出する、
   よう構成されている、請求項１に記載の車両の進行路推定装置。
4. 前記制御ユニットは、さらに、前記車両のヨーレートセンサが検出したヨーレート値と、前記ヨーレートの平均値との差分を算出し、得られた当該差分値を当該ヨーレートセンサの検出値を補正するために利用する、よう構成されている、請求項３に記載の車両の進行路推定装置。

Images