平滑化フィルタ装置および速度検出装置
【課題】暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することが可能な平滑化フィルタ装置およびそれを用いた速度検出装置を提供する。
【解決手段】時系列的に離散的に入力される物理量Ｘのデータを平滑化して出力する平滑化フィルタ装置１は、加算割合Ｐを決定する加算割合決定手段２と、今回入力された物理量Ｘのデータの入力値Ｘin(t)と所定周期前の出力値Ｘout(t-1)とを加算割合Ｐにより重み付け加算して算出した出力値Ｘout(t)を出力する平滑化手段３と、出力値の時間微分ｄＸout(t)を算出して、出力値の時間微分の平滑化値ｄＸs(t)を算出する時間微分平滑化値算出手段４とを備え、加算割合決定手段２は、入力値の時間微分ｄＸin(t)を算出し、算出した入力値の時間微分ｄＸin(t)と時間微分平滑化値算出手段４が算出した出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて加算割合Ｐを決定する。
【解決手段】時系列的に離散的に入力される物理量Ｘのデータを平滑化して出力する平滑化フィルタ装置１は、加算割合Ｐを決定する加算割合決定手段２と、今回入力された物理量Ｘのデータの入力値Ｘin(t)と所定周期前の出力値Ｘout(t-1)とを加算割合Ｐにより重み付け加算して算出した出力値Ｘout(t)を出力する平滑化手段３と、出力値の時間微分ｄＸout(t)を算出して、出力値の時間微分の平滑化値ｄＸs(t)を算出する時間微分平滑化値算出手段４とを備え、加算割合決定手段２は、入力値の時間微分ｄＸin(t)を算出し、算出した入力値の時間微分ｄＸin(t)と時間微分平滑化値算出手段４が算出した出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて加算割合Ｐを決定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、平滑化フィルタ装置および速度検出装置に係り、特に、入力された物理量のデータを平滑化する平滑化フィルタ装置とそれを用いて物体の速度を検出する速度検出装置に関する。
【背景技術】
【０００２】
近年、乗用車や貨物自動車等の車両では、自車両前方を撮像した画像の解析やレーダ装置による計測等により先行車両との距離や相対速度を検出して、自動的に自車両の加減速制御を行う先行車両追従装置や衝突防止装置の開発が進められている（例えば特許文献１、２参照）。
【０００３】
このような制御を確実に行うためには、自車両と先行車両との距離や相対速度等を正確にかつ遅れなく検出することが必要となる。しかし、実際には、例えば相対速度を車速センサを用いて検出したり時系列データとして検出した距離の時間的な差分から検出したりする場合、センサ自体が有する誤差や精度或いは検出した距離の精度や誤検出された結果等のために、図２３に模式的に示すように、時系列データとして検出された相対速度に、その値が大きく上下するいわゆる暴れが生じる。
【０００４】
このように値が上下に大きく暴れる時系列データをそのまま制御に用いることはできないため、このような時系列データの暴れを抑えることを目的として、データをカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタで平滑化することが一般的に行われている（例えば特許文献３参照）。
【特許文献１】特開平１０−６９５９８号公報
【特許文献２】特開２００６−１８８１５５号公報
【特許文献３】特開２００１−２４２２４２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、図２３に例示した暴れを有するデータでは、平滑化フィルタを用いることにより図２４に曲線Ａで示されるようにデータの暴れの振幅の中心付近を通るようなデータに平滑化されることが望まれる。しかしながら、前記のような平滑化フィルタを用いた平滑化では、通常、そのフィルタの特性によりそれぞれ特徴を持った平滑化がなされる。
【０００６】
例えば前記の３種類の平滑化フィルタのうち、移動平均フィルタや時定数フィルタを用いた場合には、通常、理想的な平滑化曲線Ａに比べて時間遅れをもって変化する曲線Ｂ、すなわち図２４の例では時系列データにあわせて減少していく平滑化曲線Ａと比較してΔＴだけ時間的に遅れて減少していく曲線Ｂのようにデータが平滑化される。
【０００７】
曲線Ｂのように時間遅れをもって平滑化された結果に基づいて自車両の自動制御が行われると、例えばこの時系列データが相対速度のデータである場合には、ある時刻における相対速度が平滑化曲線Ａで表される実際の相対速度よりも大きな相対速度であると判断されて大きな制動が自動的に加わり、ドライバに違和感を与えるとともに、車間をつめるために不必要な加速を強いることとなる。
【０００８】
また、この時系列データが自車両と先行車両との距離のデータである場合には、ある時刻における距離が平滑化曲線Ａで表される実際の距離よりも大きな距離であると判断されて、今度は逆に必要な量の制動が加わらないため、ドライバが急ブレーキを踏んだり、最悪の場合には先行車両に追突するという事態を招いてしまう。
【０００９】
一方、図２４の時系列データにカルマンフィルタを適用して平滑化した場合には、通常、理想的な平滑化曲線Ａに比べてデータの増減がより強調された曲線Ｃのようにデータが平滑化される。この場合、曲線Ｃに基づいて自車両の自動制御が行われると、時系列データが距離や相対速度のデータである場合には、その値が実際よりも大きく急激に変化したと認識されて急制動がかかるなどして、やはりドライバに違和感を与えたり、最悪の場合には先行車両に追突するという事態を招いてしまう。
【００１０】
このように、暴れを有するデータを平滑化し、その平滑化したデータに基づいて自動制御等を行うような場合には、従来のカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタをそのまま用いることは必ずしも有効とはいえない。図２４に示した理想的な平滑化曲線Ａのように、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することができる平滑化フィルタの開発が望まれる。
【００１１】
本発明は、このような事情に鑑みてなされたものであり、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することが可能な平滑化フィルタ装置を提供することを目的とする。また、それを用いて物体の速度を的確に検出可能な速度検出装置を提供することをも目的とする。
【課題を解決するための手段】
【００１２】
前記の問題を解決するために、第１の発明は、
時系列的に離散的に入力される物理量のデータを平滑化して出力する平滑化フィルタ装置において、
加算割合を決定する加算割合決定手段と、
今回入力された前記物理量のデータの入力値と所定周期前の出力値とを前記加算割合により重み付け加算して算出した値を平滑化された前記物理量のデータの出力値として出力する平滑化手段と、
前記出力値の時間微分を算出して、前記出力値の時間微分の平滑化値を算出する時間微分平滑化値算出手段と
を備え、
前記加算割合決定手段は、前記入力値の時間微分を算出し、算出した前記入力値の時間微分と前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記加算割合を決定することを特徴とする。
【００１３】
第２の発明は、第１の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記入力値の変化傾向を判定し、判定した前記入力値の変化傾向と前記入力値の時間微分との関係に基づいて前記加算割合を決定することを特徴とする。
【００１４】
第３の発明は、第１の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値を所定の係数で増幅した値と前記入力値の時間微分との差分を算出し、その差分に基づいて前記加算割合を決定することを特徴とする。
【００１５】
第４の発明は、第３の発明の平滑化フィルタ装置において、前記所定の係数は、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅内に収まるように設定されることを特徴とする。
【００１６】
第５の発明は、第３または第４の発明の平滑化フィルタ装置において、前記所定の係数は、正の値とされ、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅の端部付近の値となるように設定されることを特徴とする。
【００１７】
第６の発明は、第３から第５のいずれかの発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記入力値に重み付けする前記加算割合を、前記差分の絶対値が小さいほど大きい値となるように決定することを特徴とする。
【００１８】
第７の発明は、第６の発明の平滑化フィルタ装置において、前記加算割合決定手段は、複数の範囲に分割された前記差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備え、前記テーブルに基づいて前記加算割合を決定することを特徴とする。
【００１９】
第８の発明は、第６の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記差分の絶対値についての単調減少関数を備え、前記単調減少関数に基づいて前記加算割合を決定することを特徴とする。
【００２０】
第９の発明は、第１から第８のいずれかの発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記入力値の入力回数が所定回数に満たない間は、前記加算割合を予め設定された一定値に決定することを特徴とする。
【００２１】
第１０の発明は、第１から第９のいずれかの発明の平滑化フィルタ装置において、前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して現在のサイクリング周期の前記出力値の時間微分の平滑化値を算出することを特徴とする。
【００２２】
第１１の発明は、第１から第１０のいずれかの発明の平滑化フィルタ装置において、前記物理量のデータは、他の物理量を時間微分して算出される前記他の物理量の変化速度のデータであることを特徴とする。
【００２３】
第１２の発明は、第３から第１０のいずれかの発明の平滑化フィルタ装置において、前記物理量のデータは、他の物理量を時間微分して得られる前記他の物理量の変化速度のデータであり、前記所定の係数は、前記他の物理量の値が小さいほど小さい値となるように可変させることを特徴とする。
【００２４】
第１３の発明は、第１から第１０のいずれかの発明の平滑化フィルタ装置を用いて物体の速度を検出する速度検出手装置であって、
前記物理量のデータは前記物体の速度を測定する測定手段から入力される速度のデータであり、平滑化された前記速度を出力することを特徴とする。
【００２５】
第１４の発明は、第１１または第１２の発明の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記他の物理量は前記物体の距離を測定する測定手段から入力される距離であり、前記物理量のデータは前記距離を時間微分して得られる前記物体の速度のデータであり、平滑化された前記速度を出力することを特徴とする。
【００２６】
第１５の発明は、第１３または第１４の発明の速度検出装置において、前記速度は、自車両の車速であることを特徴とする。
【００２７】
第１６の発明は、第１３または第１４の発明の速度検出装置において、前記速度は、自車両と先行車両との相対速度であることを特徴とする。
【００２８】
第１７の発明は、第１６の発明の速度検出装置において、前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する場合には、前記自車両と先行車両との距離が大きいほど前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする。
【００２９】
第１８の発明は、第１５から第１７のいずれかの発明の速度検出装置において、
自車両が高速道路を走行していることを検出する手段を備え、
前記時間微分平滑化値算出手段は、前記手段により自車両が高速道路を走行していることが検出された場合には、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する際に、前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする。
【発明の効果】
【００３０】
第１の発明によれば、平滑化フィルタ装置を、従来の平滑化フィルタのように時間遅れを生じることなく時系列データの暴れの振幅の中心付近の値に的確に平滑化するように構成するためには、データの入力値の変化傾向を的確に捉え、そのデータの変化傾向を平滑化された出力値に的確に反映させることが必要となる。
【００３１】
本発明では、時間微分平滑化値算出手段で平滑化された出力値の時間微分をさらに平滑化しその平滑化値から入力値の全体的な変化傾向を把握する。そして、加算割合決定手段でこの出力値の時間微分の平滑化値から把握される入力値の全体的な変化傾向と同じ方向、すなわち例えば入力値の変化傾向が減少傾向にあれば物理量のデータを減少させる方向の入力値の加算割合が増大するように加算割合を決定する。そして、最後にその加算割合に基づいて平滑化手段で時定数フィルタ処理を行い、入力値を強調して所定周期前の出力値と重み付け加算する。
【００３２】
このようにして物理量のデータの入力値の変化傾向を平滑化される出力値に的確に反映させることが可能となり、平滑化される出力値を入力値の変化傾向にあわせて的確に増減させることが可能となる。そのため、従来の移動平均フィルタや時定数フィルタ等の平滑化フィルタのように時間遅れを発生させることなく、また、カルマンフィルタのように必要以上にデータの増減を強調することなく、時系列データの暴れの振幅の中心付近の値に的確に平滑化された理想的な平滑化曲線を得ることが可能となる。
【００３３】
第２の発明によれば、出力値の時間微分の平滑化値から入力値の変化傾向が増加傾向にあるのか、減少傾向にあるのか、或いはあまり変化しない傾向であるのかを判定し、例えばその傾向の分類に対応して予め加算割合が割り当てられたテーブル等に基づいて加算割合を決定する。そのため、前記発明の効果に加え、加算割合を簡単に求めることが可能となり、演算負荷の低減を図ることが可能となる。
【００３４】
第３の発明によれば、入力値の全体的な変化傾向を表す出力値の時間微分の平滑化値を所定の係数で増幅した値と、入力値の時間微分との差分に基づいて加算割合を決定することで、出力値を算出するための時定数フィルタ処理において入力値を強調する度合を表す加算割合を差分という具体的な数値に基づいて決定することが可能となり、入力値を出力値に的確に反映させて、前記各発明の効果をより的確に発揮させることが可能となる。
【００３５】
第４の発明によれば、出力値の時間微分の平滑化値を増幅させる前記所定の係数は、例えばそれをあまりに大きな値に設定すると、出力値の時間微分の平滑化値自体が有する暴れがより拡大されると同時に、出力値の時間微分の平滑化値を所定の係数で増幅した値と入力値の時間微分の値とが大きく異なる値となり、差分が有する有用性が弱まる。
【００３６】
そのため、その所定の係数で出力値の時間微分の平滑化値を増幅した値が入力値の時間微分の振幅内に収まるように適切に設定されることで、出力値の時間微分の平滑化値自体が有する暴れの拡大を適切な範囲に収め、差分が有する有用性を維持した状態で差分に基づいて加算割合を決定することが可能となり、前記各発明の効果がより的確に発揮される。
【００３７】
第５の発明によれば、正の値の所定の係数により出力値の時間微分の平滑化値を増幅した値が入力値の時間微分の振幅の端部付近の値となるように設定される。この場合、出力値の時間微分の平滑化値を所定の係数で増幅した値と入力値の時間微分の差分の絶対値が小さくなるような入力値は、平滑化される出力値を入力値の変化傾向にあわせて的確に増減させるために強調されるべき入力値である。従って、その差分の絶対値と加算割合との対応づけが容易になり、前記第３の発明や第４の発明の効果がより的確に発揮される。
【００３８】
第６の発明によれば、入力値に重み付けする加算割合を差分の絶対値が小さいほど大きい値となるように決定することで、差分と加算割合を非常に単純な対応づけにより対応づけることが可能となり、前記各発明の効果が的確かつ容易に発揮される。
【００３９】
第７の発明によれば、加算割合決定手段に、複数の範囲に分割された差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備えておき、入力値に重み付けする加算割合を差分の絶対値が小さいほど大きい値となるように加算割合を割り当て、そのテーブルに基づいて加算割合を決定するように構成すれば、前記各発明の効果をより的確にかつより容易に発揮することが可能となる。
【００４０】
第８の発明によれば、加算割合決定手段に、差分の絶対値について単調に減少する関数を備えておき、その単調減少関数に基づいて加算割合を決定するように構成すれば、前記各発明の効果をより的確にかつより容易に発揮することが可能となる。
【００４１】
第９の発明によれば、第１から第８のいずれかの発明の平滑化フィルタ装置において、 平滑化フィルタ装置の起動時などにはメモリに所定周期前の出力値の時間微分の平滑化値等の情報が保存されていなかったり、記憶されていても出力値の時間微分の平滑化値が安定していない場合がある。そのため、前記各発明の効果に加え、入力値の数が所定回数分に満たない間は加算割合を可変させずに予め設定された一定値に決定することで、平滑化フィルタ装置の起動時などに出力値を安定させて適切な値を出力させることが可能となり、装置の信頼性を向上させることが可能となる。
【００４２】
第１０の発明によれば、入力値の変化傾向を表す出力値の時間微分の平滑化値を算出する際に、出力値の時間微分とその所定周期前の出力値の時間微分の平滑化値とを所定の割合で重み付け加算する時定数フィルタ処理により算出することで、出力値の時間微分の平滑化値が、入力値や入力値の時間微分の暴れに追随して暴れることなく、滑らかに推移するようになる。そのため、滑らかに推移する出力値により入力値の変化傾向を的確に把握して加算割合を決定することが可能となり、前記各発明の効果をより的確に発揮することが可能となる。
【００４３】
第１１の発明によれば、平滑化フィルタ装置は、前記各発明により、測定手段で測定され入力された物理量のデータを平滑化して出力することが可能であることは言うまでもないが、例えば測定手段が測定した物理量が時間微分されて形成されたその物理量の変化速度のデータを平滑化して出力するように構成することも可能である。そして、そのように構成された場合にも、前記各発明と同様に物理量の変化速度のデータの暴れの振幅の中心付近の値に的確に平滑化された理想的な平滑化曲線を得ることが可能となり、物理量の変化速度のデータに対して前記各発明の効果が的確に発揮される。
【００４４】
第１２の発明によれば、例えば自車両と先行車両との距離の測定値を時間微分して得られる変化速度である自車両と先行車両との相対速度を平滑化する場合、自車両と先行車両との距離が大きい場合には相対速度の暴れが大きくその時間微分である相対加速度の暴れも大きいが、自車両と先行車両との距離が小さくなるにつれて相対速度の暴れが小さくなり相対加速度の暴れも小さくなることがある。そのような場合、相対加速度の暴れの端部に沿うように設定された所定の係数で増幅された出力値の時間微分の平滑化値が、前記距離が小さくなってくると暴れの端部から離れてしまい、前記差分の有用性が弱まってしまう場合がある。
【００４５】
そのため、そのような場合には、所定の係数を「他の物理量」である自車両と先行車両との距離の値が小さいほど小さい値となるように可変させるように構成すれば、所定の係数で増幅された出力値の時間微分の平滑化値が相対加速度の暴れの端部から離れてしまうことを防止することができ、差分の有用性を維持することが可能となり、前記各発明の効果がより効果的に発揮される。
【００４６】
第１３の発明によれば、第１〜第１０の発明の平滑化フィルタ装置は、自動車やトラック等の車両や鉄道車両、航空機、船舶等の運輸手段或いはロボットや各種機械・機器等のように自ら移動し、他の物体等を移動し、或いは自己の内部部品を移動させることで観念される物体の速度を検出する速度検出装置に適用することができる。
【００４７】
そして、速度検出装置を、物体の速度を測定する測定手段から入力される速度のデータに対して演算処理を行い、平滑化された速度を出力するように構成すれば、もともと暴れを有していた速度のデータを、そのデータの暴れの振幅の中心付近の値に的確に平滑化された速度として出力することが可能となり、速度検出装置により物体の速度を的確に検出することが可能となる。
【００４８】
第１４の発明によれば、第１１または第１２の発明の平滑化フィルタ装置は、自動車やトラック等の車両や鉄道車両、航空機、船舶等の運輸手段或いはロボットや各種機械・機器等のように他の物体との距離を把握し、その把握された距離を時間微分して物体の速度を検出する速度検出装置に適用することができる。
【００４９】
そして、速度検出装置を、他の物理量として測定手段から入力される距離のデータを時間微分して物体の速度を算出し、その速度に対して演算処理を行い、平滑化された速度を出力するように構成すれば、もともと暴れを有していた速度のデータを、そのデータの暴れの振幅の中心付近の値に的確に平滑化された速度として出力することが可能となり、速度検出装置により物体の速度を的確に検出することが可能となる。
【００５０】
第１５の発明によれば、前記の速度検出装置においては、平滑化される速度が自車両の車速である場合にも前記各発明の効果が効果的に発揮される。
【００５１】
第１６の発明によれば、前記の速度検出装置においては、平滑化される速度が自車両と先行車両との相対速度である場合にも前記各発明の効果が効果的に発揮される。
【００５２】
第１７の発明によれば、平滑化される速度が自車両と先行車両との相対速度である場合には、自車両と先行車両との距離が大きい場合に距離が小さい場合に比べて相対加速度が比較的大きく暴れることがあり、前記所定の割合を一定に維持したままであると出力値の時間微分の平滑化値も比較的大きく暴れてしまう場合がある。
【００５３】
そのため、第１７の発明の構成とすれば、所定の割合を一定のままに維持した場合に比べて出力値の時間微分の平滑化値の暴れをより少なくすることが可能となり、出力値の時間微分の平滑化値を滑らかに推移させて、相対速度のデータの変化傾向を的確に把握することが可能となる。そのため、平滑化フィルタ装置の機能が的確に発揮され、それを用いた速度検出装置についての前記各発明の効果がより的確に発揮される。
【００５４】
第１８の発明によれば、自車両が高速道路を走行している場合には、通常の道路を走行している場合に比べて自車両と先行車両との相対速度の増減の程度がさほど大きくなく、相対加速度が比較的安定して推移する傾向がある。従って、速度検出装置に自車両が高速道路を走行していることを検出する手段を備え、第１８の発明の構成とすれば、出力値の時間微分の平滑化値の暴れをより少なくすることが可能となり、出力値の時間微分の平滑化値を滑らかに推移させて、相対速度のデータの変化傾向を的確に把握することが可能となる。そのため、平滑化フィルタ装置の機能が的確に発揮され、それを用いた速度検出装置についての前記各発明の効果がより的確に発揮される。
【発明を実施するための最良の形態】
【００５５】
以下、本発明に係る平滑化フィルタ装置およびそれを用いた速度検出装置の実施の形態について、図面を参照して説明する。
【００５６】
［第１の実施の形態］
まず、平滑化フィルタ装置について説明する。平滑化フィルタ装置１は、図示しないＣＰＵやＲＯＭ、ＲＡＭ等がバスに接続されたマイクロコンピュータより構成されている。
【００５７】
本実施形態に係る平滑化フィルタ装置１は、図１に示すように、主に加算割合決定手段２と、平滑化手段３と、時間微分平滑化値算出手段４と、メモリ５とを備えて構成されている。平滑化フィルタ装置１は、所定のサンプリング周期で時系列的かつ離散的に所定の物理量Ｘのデータが入力値Ｘin(t)としてＩ／Ｏインターフェース６を介して入力されると、以下に説明する演算処理を行った後、その物理量Ｘのデータの平滑化された値を出力値Ｘout(t)として出力するようになっている。
【００５８】
ここで、本実施形態では、変数ｔはサンプリングタイミングを表す変数であり、以下では、ｔは今回すなわち所定の物理量Ｘの時系列データのうち入力値Ｘin(t)が入力されたサンプリングタイミングを表し、ｔ−１は前回すなわち今回のサンプリングタイミングｔの１周期前のサンプリングタイミングを表すものとする。
【００５９】
以下、図２に示すフローチャートに従って平滑化フィルタ装置１の加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４の構成と演算処理について説明する。
【００６０】
まず、装置全体の演算処理の流れを概説すると、平滑化フィルタ装置１は、加算割合決定手段２で物理量Ｘのデータの時間微分に基づいて加算割合Ｐを決定し（ステップＳ２〜Ｓ５）、平滑化手段３で加算割合Ｐを用いて今回の入力値Ｘin(t)と前回の出力値Ｘout(t-1)との重み付け加算により物理量Ｘのデータを平滑化して今回の出力値Ｘout(t)を算出するようになっている（ステップＳ６）。
【００６１】
つまり、平滑化手段３では後述する（３）式に示すように時定数フィルタの手法を用いて物理量Ｘのデータの平滑化が行われるが、その際、従来の時定数フィルタのように時定数すなわち加算割合Ｐを一定の値に固定するのではなく、加算割合Ｐを加算割合決定手段２で今回入力された物理量Ｘのデータの時間微分に基づいて可変させ、各サンプリングタイミングｔで可変させることで、平滑化を適切に行うようになっている。
【００６２】
また、平滑化フィルタ装置１の時間微分平滑化値算出手段４では、次回の加算割合Ｐを決定するために前回の出力値Ｘout(t-1)の時間微分の平滑化値ｄＸs(t-1)を算出するようになっている（ステップＳ７〜Ｓ９）。
【００６３】
以下、詳しく説明する。なお、後述するように、メモリ５には、前回の出力値Ｘout(t-1)と前回の演算処理で算出した出力値の時間微分の平滑化値ｄＸs(t-1)とが保存されている。
【００６４】
加算割合決定手段２は、今回のサンプリングタイミングｔで物理量Ｘのデータの入力値Ｘin(t)が入力されると（ステップＳ１：ＹＥＳ）、メモリ５から前回の出力値Ｘout(t-1)を読み出して、下記（１）式に従って入力値の時間微分ｄＸin(t)を算出するようになっている（ステップＳ２）。ここで、Δｔはサンプリング周期を表す。
ｄＸin(t)＝（Ｘin(t)−Ｘout(t-1)）／Δｔ …（１）
【００６５】
入力される物理量Ｘのデータが例えば図２３に示したような時系列的な暴れを有するデータである場合、算出される入力値の時間微分ｄＸin(t)も図３に示すように時系列的な暴れを有する波形となる。
【００６６】
加算割合決定手段２は、続いて、メモリ５から前回のサンプリングタイミングで算出された出力値の時間微分の平滑化値ｄＸs(t-1)を読み出して、平滑化値ｄＸs(t-1)を所定の係数Ｋで増幅するようになっている（図２のステップＳ３）。ここで、本実施形態では、係数Ｋは正の値とする。
【００６７】
本実施形態では、前記所定の係数Ｋは予め設定されるようになっている。平滑化フィルタ装置１に入力される物理量Ｘのデータの入力値Ｘin(t)の暴れの具合はその物理量Ｘのデータを測定するセンサ等の測定装置により種々であり、平滑化フィルタ装置１が適用される測定装置に応じて適宜設定されることが必要となる。
【００６８】
物理量Ｘのデータとして、ある測定装置から例えば図２３に示したような時系列的な暴れを有するデータが入力され、図３に示すような入力値Ｘin(t)の時間微分ｄＸin(t)が得られるとする。この場合、図４に示すように係数Ｋで出力値Ｘout(t-1)の時間微分の平滑化値ｄＸs(t)を増幅した値ｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の上端および下端の内側に収まるように係数Ｋの値が設定されることが好ましい。
【００６９】
本実施形態では、特に、係数Ｋ倍された平滑化値ｄＸs(t)すなわちｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態になるように係数Ｋの値が設定されている。
【００７０】
加算割合決定手段２は、続いて、下記（２）式に従って前記（１）式で算出した入力値の時間微分ｄＸin(t)と、前回の出力値の時間微分の平滑化値ｄＸs(t-1)を所定の係数Ｋで増幅した値ｄＸs(t-1)×Ｋとの差分Ｄ(t)を算出するようになっている（図２のステップＳ４）。
Ｄ(t)＝ｄＸin(t)−ｄＸs(t-1)×Ｋ …（２）
【００７１】
加算割合決定手段２は、続いて、算出した差分Ｄ(t)に基づいて後述する平滑化手段３での重み付け加算に用いられる加算割合Ｐを決定するようになっている（ステップＳ５）。本実施形態では、加算割合決定手段２は、差分Ｄ(t)の絶対値|Ｄ(t)|が小さいほど加算割合Ｐが大きい値となるように決定するように構成されている。
【００７２】
具体的には、例えば差分Ｄ(t)が加速度のディメンジョンを有する場合、加算割合決定手段２には、図５に示すような差分の絶対値|Ｄ(t)|の複数の範囲に分割して各範囲ごとに加算割合Ｐが割り当てられたテーブルが備えられていれば、算出した差分Ｄ(t)とこのテーブルとに基づいて加算割合Ｐを決定することが可能となる。本実施形態では、加算割合決定手段２はこのようなテーブルを備えている。
【００７３】
なお、図５中、Ｇは標準重力加速度を表す。また、本実施形態のようにテーブルを準備しておく代わりに、例えば差分の絶対値|Ｄ(t)|を変数とする単調減少関数を備えておき、この単調減少関数に基づいて加算割合Ｐを決定するように構成することも可能である。
【００７４】
加算割合決定手段２は、以上のようにして決定した加算割合Ｐを平滑化手段３に送信するようになっている。
【００７５】
平滑化手段３は、加算割合決定手段２で決定された加算割合Ｐを用いて下記（３）式に従って今回の入力値Ｘin(t)と前回の出力値Ｘout(t-1)との重み付け加算により物理量Ｘのデータを平滑化して今回の出力値Ｘout(t)を算出するようになっている（図２のステップＳ６）。
Ｘout(t)＝Ｘout(t-1)×（１−Ｐ）＋Ｘin(t)×Ｐ …（３）
【００７６】
平滑化手段３は、このようにして算出した今回の出力値Ｘout(t)を出力するとともに（ステップＳ６）、時間微分平滑化値算出手段４に送るようになっている。
【００７７】
時間微分平滑化値算出手段４は、次回の加算割合決定手段２における演算処理で用いるために、平滑化手段３で算出された出力値Ｘout(t)に基づいて、前述した出力値の時間微分の平滑化値ｄＸs(t-1)を算出するようになっている。
【００７８】
具体的には、時間微分平滑化値算出手段４は、まず、平滑化手段３から送信されてきた今回の出力値Ｘout(t)とメモリ５から読み出した前回の出力値Ｘout(t-1)とを用いて、下記（４）式に従って出力値の時間微分ｄＸout(t)を算出するようになっている（ステップＳ７）。なお、Δｔは前記（１）式の場合と同様にサンプリング周期を表す。
ｄＸout(t)＝（Ｘout(t)−Ｘout(t-1)）／Δｔ …（４）
【００７９】
時間微分平滑化値算出手段４は、続いて、算出した出力値の時間微分ｄＸout(t)とメモリ５から読み出した前回の出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて、今回の出力値の時間微分の平滑化値ｄＸs(t)を算出するようになっている（ステップＳ８）。
【００８０】
この今回の出力値の時間微分の平滑化値ｄＸs(t)の算出においては、種々の平滑化手法を用いることが可能であり、前述したカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタを用いることができる。
【００８１】
本実施形態では、時間微分平滑化値算出手段４は、今回算出した出力値の時間微分ｄＸout(t)と前回の出力値の時間微分の平滑化値ｄＸs(t-1)とに基づいて、下記（５）式に示される時定数が固定された時定数フィルタで重み付け加算して今回の出力値の時間微分の平滑化値ｄＸs(t)を算出するようになっている。
ｄＸs(t)＝ｄＸs(t-1)×０．９５＋ｄＸout(t)×０．０５ …（５）
【００８２】
時間微分平滑化値算出手段４は、今回の出力値Ｘout(t)と算出した今回の出力値の時間微分の平滑化値ｄＸs(t)とを、次回の加算割合決定手段２における演算処理のために「前回の出力値Ｘout(t-1)」および「前回の出力値の時間微分の平滑化値ｄＸs(t-1)」としてそれぞれメモリ５に上書き保存するようになっている（ステップＳ９）。
【００８３】
なお、上記の本実施形態の平滑化フィルタ装置１の構成から分かるように、平滑化フィルタ装置１の起動時などには、メモリ５に前回の出力値Ｘout(t-1)や前回の出力値の時間微分の平滑化値ｄＸs(t-1)が保存されていなかったり、前回の出力値の時間微分の平滑化値ｄＸs(t-1)が安定しない場合がある。
【００８４】
そのため、本実施形態では、加算割合決定手段２は、入力される物理量Ｘのデータの入力値Ｘin(t)の数が予め設定された所定回数分に満たない間は、加算割合Ｐを例えば０．１等の予め設定された一定値に決定するようになっている。
【００８５】
次に、本実施形態に係る平滑化フィルタ装置１の作用について説明する。
【００８６】
本実施形態に係る平滑化フィルタ装置１では、平滑化手段３で、前記（３）式に示したように時定数フィルタの手法を用いて物理量Ｘのデータの平滑化が行われるが（ステップＳ６）、その際に用いられるフィルタの時定数すなわち加算割合Ｐを加算割合決定手段２で物理量Ｘのデータの時間微分に基づいて可変させる（ステップＳ２〜Ｓ５）。また、時間微分平滑化値算出手段４では次回の演算処理のために出力値の時間微分の平滑化値ｄＸs(t)を算出する（ステップＳ７〜Ｓ９）。
【００８７】
前述したように、移動平均フィルタや時定数フィルタ等の従来の平滑化フィルタでは、図２４に示したようにデータの暴れの振幅の中心付近を通る理想的な平滑化曲線Ａに対して時間遅れΔＴをもって物理量Ｘのデータが平滑化される。そのような時間遅れを解消し、減少傾向や増加傾向を示しているデータを図２４の平滑化曲線Ａのように理想的に平滑化するためには、(i)データの入力値Ｘin(t)が減少傾向にあるのか、増加傾向にあるのか、或いはあまり変化しない傾向にあるのか等の入力値Ｘin(t)の変化傾向を的確に捉え、(ii)そのデータの変化傾向を平滑化された出力値Ｘout(t)に的確に反映させることが必要となる。
【００８８】
本実施形態に係る平滑化フィルタ装置１では、まず、(i)の入力値Ｘin(t)の変化傾向を、前述した時間微分平滑化値算出手段４におけるステップＳ７、Ｓ８の演算処理で装置１から出力された出力値Ｘout(t)の時間微分ｄＸout(t)を算出し、それをさらに平滑化して平滑化値ｄＸs(t-1)を算出して追跡する。
【００８９】
入力値Ｘin(t)の変化傾向を出力値Ｘout(t)の時間微分で判定するのは一見矛盾するようだが、図２３に示したように入力値Ｘin(t)が時系列的な暴れを有する場合、その時間微分ｄＸin(t)も図３、図４に示すようにサンプリングタイミングｔごとにその値が正になったり負になったり時系列的に暴れるため、入力値Ｘin(t)の時間微分ｄＸin(t)を入力値Ｘin(t)の変化傾向の判定にそのまま使用できない。
【００９０】
そこで、本実施形態では、入力値Ｘin(t)の値を平滑化した出力値Ｘout(t)を用い、その時間微分ｄＸout(t)の平滑化値ｄＸs(t)から入力値Ｘin(t)の全体的な変化傾向を判定することとしている。
【００９１】
次に、(ii)の前記のようにして判定した入力値Ｘin(t)の変化傾向を出力値Ｘout(t)に的確に反映させる手法として、本実施形態に係る平滑化フィルタ装置１では、前記入力値の変化傾向すなわちそれを表す出力値の時間微分の平滑化値ｄＸs(t)を加味して前記（３）式の加算割合Ｐの値を可変させる手法が用いられている。
【００９２】
具体的には、(i)で述べた入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)を所定の係数Ｋで増幅し、それと現在の入力値Ｘin(t)の時間的変化を表す時間微分ｄＸin(t)との差分Ｄ(t)を算出し、その差分の絶対値|Ｄ(t)|に応じて加算割合Ｐを決定する。
【００９３】
本実施形態では、特に、図４に示したように平滑化値ｄＸs(t)のＫ倍が入力値の時間微分ｄＸin(t)の暴れの振幅の下端に沿う状態となるように係数Ｋの値が設定されており、|Ｄ(t)|が小さいほど加算割合Ｐが大きくなり、|Ｄ(t)|が大きいほど加算割合Ｐが小さくなるように決定される。
【００９４】
つまり、例えば図４の領域Ｒ２のように、入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)が負の値となり入力値Ｘin(t)が全体的に減少傾向を示している場合、現在の入力値の時間微分ｄＸin(t)がｄＸs(t)×Ｋに近く、入力値の時間微分ｄＸin(t)の暴れの振幅の下端に近く大きく負側に振れているときに入力値Ｘin(t)の加算割合Ｐを大きくして出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくする。
【００９５】
入力値の時間微分ｄＸin(t)が負の値であれば今回の入力値Ｘin(t)は前回の出力値Ｘout(t-1)よりも小さい場合が多いから、この入力値Ｘin(t) の加算割合Ｐを大きくして出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくすることで、従来のように加算割合Ｐを固定した場合よりも大きく今回の出力値Ｘout(t)を前回の出力値Ｘout(t-1)に対して減少させることができる。
【００９６】
つまり、本実施形態では、入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)が負の値となり入力値Ｘin(t)が全体的に減少傾向を示している場合に、入力値Ｘin(t) の加算割合Ｐをより増大させて出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくすることで、出力値Ｘout(t)の減少傾向がより強調されるようになる。そのため、図６に示すように、出力値Ｘout(t)が、加算割合Ｐを固定した場合に平滑化された出力値Ｂよりも減少側に押し下げられて、理想的な平滑化曲線Ａを描くようになる。
【００９７】
一方、例えば図４の領域Ｒ１のように、入力値Ｘin(t)が全体的にあまり変化しない傾向を示している場合、出力値の時間微分の平滑化値ｄＸs(t)は０に近い値であり、ｄＸs(t)×Ｋも０に近い値となる。そのため、現在の入力値の時間微分ｄＸin(t)が０に近いほど差分の絶対値|Ｄ(t)|が小さくなって加算割合Ｐが大きくなる。つまり、前回の出力値Ｘout(t-1)とそれほど値が変わらない今回の入力値Ｘin(t)の出力値Ｘout(t)に対する寄与度が大きくなる。
【００９８】
また、入力値の時間微分ｄＸin(t)が正の方向や負の方向に大きく出ても|Ｄ(t)|が大きく出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度は小さく、その出現頻度も正負ともに同程度であるから、時間微分ｄＸin(t)が正の方向や負の方向に大きく出るような入力値Ｘin(t)の出力値Ｘout(t)に対する影響は小さいものとなる。
【００９９】
そのため、図４の領域Ｒ１のように入力値Ｘin(t)が全体的にあまり変化しない傾向を示している場合には、入力値Ｘin(t)が時系列的に暴れても、出力値Ｘout(t)は暴れが小さく全体的にはあまり変化しない値を取り続けるようになる。
【０１００】
他方、例えば図４の領域Ｒ３のように、入力値Ｘin(t)の全体的な変化傾向が減少傾向からあまり変化しない傾向に変わるような場合、時間微分ｄＸin(t)が正の方向に大きく出る入力値Ｘin(t)の加算割合Ｐの値自体はそれほど大きくない。
【０１０１】
しかし、減少傾向を示していた領域Ｒ２に比べて、前回の出力値Ｘout(t-1)よりも大きな値の入力値Ｘin(t)が入力される頻度が増大するため、前記（３）式に従って次第に出力値Ｘout(t)も減少傾向からあまり変化しない傾向に変わっていき、出力値の時間微分の平滑化値ｄＸs(t)も０に近づいていく。
【０１０２】
このようにして、本実施形態に係る平滑化フィルタ装置１は、入力値Ｘin(t)があまり変化しない傾向にあるときは平滑化された出力値Ｘout(t)もあまり変化しない傾向を示し、入力値Ｘin(t)が減少傾向にあるときは平滑化された出力値Ｘout(t)も減少傾向を示し、しかも入力値Ｘin(t)の減少傾向に理想的に追従して減少する平滑化された出力値Ｘout(t)を出力することが可能となる。
【０１０３】
なお、上記の例では、物理量Ｘのデータの入力値Ｘin(t)の全体的な変化傾向が図２３に示すように減少傾向にある場合について述べたが、例えば、図７に示すように物理量Ｘのデータの入力値Ｘin(t)の全体的な変化傾向が増加傾向にある場合も何ら前記構成を変更することなく同様に出力値Ｘout(t)の理想的な平滑化を行うことができる。
【０１０４】
すなわち、図７に示すように入力値Ｘin(t)の全体的な変化傾向が増加傾向にある場合には、図８に示すように出力値の時間微分の平滑化値ｄＸs(t)も正の値をとるようになる。その際、本実施形態のように、入力値Ｘin(t)が全体的に減少傾向を示し平滑化値ｄＸs(t)が負の値をとるときに図４に示したようにｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態になるように係数Ｋの値が設定されていれば、平滑化値ｄＸs(t)も正の値をとる場合には、図８に示すように、ｄＸs(t)×Ｋは入力値の時間微分ｄＸin(t)の暴れの振幅の上端にほぼ沿う状態となる。
【０１０５】
そして、前記と同様に差分Ｄ(t)を算出し、その絶対値|Ｄ(t)|と加算割合Ｐとの関係を設定すれば、入力値Ｘin(t)が全体的に増加傾向にある場合には、図９に示すように、出力値Ｘout(t)が、加算割合Ｐを固定した場合に平滑化された出力値Ｂよりも増加側に押し上げられて、理想的な平滑化曲線Ａを描くようになる。また、入力値Ｘin(t)があまり変化しない傾向にある場合や、増加傾向にあった入力値Ｘin(t)があまり変化しない傾向に変化する場合も前記減少傾向の場合と同様に説明され、良好に入力値Ｘin(t)を平滑化することができる。
【０１０６】
以上のように、本実施形態に係る平滑化フィルタ装置１によれば、出力値の時間微分ｄＸout(t)を平滑化した平滑化値ｄＸs(t)を算出し、それに基づいて物理量Ｘのデータの入力値Ｘin(t)の変化傾向を的確に捉えることが可能となる。また、それと同時に、例えば平滑化値ｄＸs(t)を所定の係数Ｋで増幅した値と入力値の時間微分ｄＸin(t)との差分に基づいて加算割合Ｐを決定するなどして入力値の時間微分ｄＸin(t)と平滑化値ｄＸs(t)に基づいて出力値Ｘout(t)の算出に用いる加算割合Ｐを決定することで、データの入力値Ｘin(t)の変化傾向を平滑化される出力値Ｘout(t)に的確に反映させることが可能となる。
【０１０７】
そのため、平滑化される出力値Ｘout(t)を入力値Ｘin(t)の変化傾向にあわせて的確に増減させることが可能となり、従来の移動平均フィルタや時定数フィルタ等の平滑化フィルタのように時間遅れを発生させることなく、図２４に示した理想的な平滑化曲線Ａのように、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することが可能となる。
【０１０８】
また、前記所定の係数Ｋを適切に設定したり、差分の絶対値|Ｄ(t)|と加算割合Ｐとの関係を適切に設定すれば、従来のカルマンフィルタのようにデータの増減を必要以上に強調させて出力値Ｘout(t)が入力値Ｘin(t)の変化傾向に的確には符合しない平滑化を行うことを防止し、理想的な平滑化曲線Ａを出力することが可能な平滑化フィルタ装置とすることが可能となる。
【０１０９】
さらに、移動平均フィルタでは、平均をとるための所定のサンプリング回数分の過去の入力値Ｘin(t-1)〜Ｘin(t-n)をメモリ５に保存しておく必要があるが、本実施形態では、メモリ５に保存しておくのは前回の出力値Ｘout(t-1)と平滑化値ｄＸs(t-1)のみであり、メモリ５の使用効率の低減させることができるという副次的な効果もある。
【０１１０】
なお、加算割合Ｐの決定手法、すなわち所定の係数Ｋの設定手法や、その係数Ｋに基づいて算出される差分Ｄ(t)と加算割合Ｐとの関係の設定手法は、本実施形態の手法に限定されない。
【０１１１】
前述の説明から分かるように、極端に言えば、物理量Ｘのデータの入力値Ｘin(t)の変化傾向が増加傾向であるか、減少傾向であるか、或いはあまり変化しない傾向であるかさえ分かれば、それに応じて加算割合Ｐの可変のさせ方を変えて的確な平滑化を行うことが可能となる。
【０１１２】
そのため、例えば、加算割合決定手段２を、時間微分平滑化値算出手段４が前回算出した出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて入力値Ｘin(t)の変化傾向を分類して判定し、例えば図１０に示すようなテーブルに基づいて加算割合Ｐの可変のさせ方を変更して加算割合Ｐを決定するように構成することも可能である。
【０１１３】
また、本実施形態では、入力された物理量Ｘのデータ自体を平滑化する平滑化フィルタ装置１について説明した。この平滑化フィルタ装置１は、例えば自車両の速度を測定する測定手段や平滑化フィルタ装置を搭載した装置と他の物体との相対速度を測定する測定手段から入力された速度や相対速度のデータＸを平滑化して、平滑化された速度や相対速度を出力値Ｘout(t)として出力する。
【０１１４】
しかし、例えば、ある移動可能な装置の移動距離を測定する測定手段から距離Ｙのデータを入力し、入力された距離Ｙのデータの入力値Ｙin(t)を時間微分して速度Ｘin(t)を求め、算出した速度Ｘin(t)を平滑化し、平滑化された速度を出力値Ｘout(t)として出力するような平滑化フィルタ装置として構成することも可能である。
【０１１５】
また、例えば、ある装置と他の物体との距離を測定する測定手段から距離Ｙのデータを入力し、入力された距離Ｙのデータの入力値Ｙin(t)を時間微分して相対速度Ｘin(t)を求め、算出した相対速度Ｘin(t)を平滑化し、平滑化された相対速度を出力値Ｘout(t)として出力するような平滑化フィルタ装置として構成することも可能である。
【０１１６】
つまり、平滑化フィルタ装置を、平滑化して出力する物理量Ｘのデータとは別の物理量Ｙのデータから平滑化の対象とする物理量Ｘのデータを形成して、その物理量Ｘのデータを平滑化するように構成することも可能である。例えば、別の物理量Ｙのデータが入力されると、そのデータＹを時間微分して物理量Ｙの変化速度である物理量のデータＸを算出し、物理量Ｙの変化速度である物理量Ｘのデータを平滑化して出力するように構成することも可能である。
【０１１７】
この場合、図２にフローチャートは図１１に示すフローチャートのように修正される。すなわち、メモリ５には、前回の出力値Ｘout(t-1)や出力値の時間微分の平滑化値ｄＸs(t-1)のほか、前回の入力値Ｙin(t-1)も保存されており、平滑化フィルタ装置の加算割合決定手段２は、ステップ１に代わるステップＳ１０で今回のサンプリングタイミングｔでの物理量Ｙのデータの入力値Ｙin(t)が入力されると（ステップＳ１０：ＹＥＳ）、メモリ５から前回の入力値Ｙin(t-1)を読み出す。
【０１１８】
そして、下記（６）式に従って入力値Ｙin(t)を時間微分して物理量Ｙのデータの変化速度ｄＹin(t)を算出し、その変化速度ｄＹin(t)をそれ以降の処理の対象となる物理量Ｘin(t)とする（ステップＳ１１）。なお、Δｔはサンプリング周期を表す。
Ｘin(t)＝ｄＹin(t)＝（Ｙin(t)−Ｙin(t-1)）／Δｔ …（６）
【０１１９】
Ｘin(t)は、前述した演算処理により加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４で演算処理されて平滑化され（ステップＳ２〜Ｓ９）、平滑化された出力値Ｘout(t)が出力される。
【０１２０】
一方、加算割合決定手段２は、今回入力された物理量Ｙのデータの入力値Ｙin(t)を「前回の入力値Ｙin(t-1)」としてメモリ５に上書き保存し（ステップＳ１２）、次回の入力値の時間微分ｄＹin(t)すなわち物理量Ｘin(t)の算出処理に用いる。
【０１２１】
このように、物理量ＸのデータＸin(t)が他の物理量Ｙを時間微分して算出される他の物理量Ｙの変化速度のデータＹin(t)である場合でも、基本的に、前述した実施形態に他の物理量Ｙを時間微分し、算出した変化速度Ｙin(t)を処理の対象となる物理量Ｘin(t)とするという演算処理が加わるだけで、加算割合決定手段２や平滑化手段３、時間微分平滑化値算出手段４における他の演算処理は上記とまったく同様に行われるから、前述した実施形態の効果は本変形例においてもまったく同様に発揮される。
【０１２２】
なお、上記の例では、物理量ＸのデータＸin(t)が入力されるごとに出力値Ｘout(t)を算出する構成について述べたが、物理量ＸのデータＸin(t)が入力されるサンプリング周期に対して例えば２サンプリング周期ごとに出力値Ｘout(t)を算出するように構成してもよい。
【０１２３】
［第２の実施の形態］
本実施形態では、前記第１の実施形態に係る平滑化フィルタ装置を用いた速度検出装置について説明する。
【０１２４】
本実施形態では、前述した第１の実施形態の変形例を用いて、後述する測定手段により測定された自車両と先行車両との距離を時間微分して自車両と先行車両との相対速度を算出し、平滑化フィルタ装置でその相対速度を平滑化して検出し出力する速度検出装置について述べる。
【０１２５】
本実施形態に係る速度検出装置１０は、図１２に示すように、主に撮像手段１１や変換手段１２、画像処理手段１５等で構成される測定手段１８と、平滑化フィルタ装置２２を含む検出手段１９とで構成されている。
【０１２６】
なお、撮像手段１１から検出手段１９の立体物検出手段２０までの構成は本願出願人により先に提出された特開平５−１１４０９９号公報、特開平５−２６５５４７号公報、特開平６−２６６８２８号公報、特開平１０−２８３４６１号公報、特開平１０−２８３４７７号公報、特開２００６−７２４９５号公報等に詳述されており、詳細な説明はそれらの公報に委ねる。以下、簡単に説明する。
【０１２７】
撮像手段１１は、本実施形態では、互いに同期が取られたＣＣＤやＣＭＯＳセンサ等のイメージセンサがそれぞれ内蔵され例えばルームミラー近傍に車幅方向に所定の間隔をあけて取り付けられた一対のメインカメラ１１ａおよびサブカメラ１１ｂからなるステレオカメラであり、所定のサンプリング周期で車両前方の道路を含む風景を撮像して一対の画像を出力するように構成されている。一対のカメラのうちメインカメラ１１ａは、運転者に近い側のカメラであり、図１３に示すような基準画像Ｔを撮像するようになっている。なお、サブカメラ１１ｂで撮像された画像を比較画像という。
【０１２８】
メインカメラ１１ａとサブカメラ１１ｂから出力された画像データは、変換手段１２であるＡ／Ｄコンバータ１２ａ、１２ｂでアナログ画像から画素ごとに輝度値を有するデジタル画像にそれぞれ変換され、画像補正部１３で、ずれやノイズの除去等の画像補正が行われ、画像データメモリ１４に格納されると同時に検出手段１９に送信されるようになっている。
【０１２９】
画像処理手段１５のイメージプロセッサ１６では、基準画像Ｔと比較画像の各画像データにステレオマッチング処理やフィルタリング処理を施して実空間上の距離に対応する視差ｄｐを算出するようになっている。以下、視差ｄｐが割り当てられた画像を距離画像という。このようにして算出された視差ｄｐの情報すなわち距離画像は、画像処理手段１５の距離データメモリ１７に格納するようになっている。
【０１３０】
なお、視差ｄｐ、距離画像上の点（ｉ，ｊ）と、前記一対のカメラ１１ａ、１１ｂの中央真下の道路面上の点を原点とし、自車両の車幅方向をＸ軸方向、車高方向をＹ軸方向、車長方向をＺ軸方向とした場合の実空間上の点（Ｘ，Ｙ，Ｚ）とは、下記（７）〜（９）式で表される座標変換により一意に対応づけられる。なお、下記各式において、ＣＤは一対のカメラの間隔、ＰＷは１画素当たりの視野角、ＣＨは一対のカメラの取り付け高さ、ＩＶおよびＪＶは自車両正面の無限遠点の距離画像上のｉ座標およびｊ座標、ＤＰは消失点視差を表す。
Ｘ＝ＣＤ／２＋Ｚ×ＰＷ×（ｉ−ＩＶ） …（７）
Ｙ＝ＣＨ＋Ｚ×ＰＷ×（ｊ−ＪＶ） …（８）
Ｚ＝ＣＤ／（ＰＷ×（ｄｐ−ＤＰ）） …（９）
【０１３１】
本実施形態では、以上の撮像手段１１からイメージプロセッサ１６や距離データメモリ１７を含む画像処理手段１５までで、自車両前方の所定領域内に存在する立体物までの自車両からの距離Ｚすなわち前記（９）式により距離Ｚと一意に対応づけられる視差ｄｐを測定する測定手段１８が構成されている。
【０１３２】
なお、本実施形態では、測定手段１８は自車両前方に存在する立体物までの距離Ｚを測定できるものであればよく、本実施形態の他にも、例えば自車両前方にレーザ光や赤外線等を照射してその反射光の情報に基づいて立体物までの距離Ｚを測定するレーダ装置等で構成することも可能であり、検出の手法は特定の手法に限定されない。
【０１３３】
検出手段１９は、図示しないＣＰＵやＲＯＭ、ＲＡＭ、入出力インターフェース等がバスに接続されたマイクロコンピュータより構成されている。また、検出手段１９には、車速センサやヨーレートセンサ、ステアリングホイールの操舵角を測定する操舵角センサ等のセンサ類Ｑが接続されている。なお、ヨーレートセンサの代わりに自車両の車速等からヨーレートを推定する装置等を用いることも可能である。
【０１３４】
検出手段１９は、図１２に示すように、立体物検出手段２０と、先行車両検出手段２１と、前述した平滑化フィルタ装置２２とを備えており、さらに図示しないメモリを備えている。また、検出手段１９の各手段には、センサ類Ｑから必要なデータが入力されるようになっている。
【０１３５】
立体物検出手段２０は、本実施形態では、前述したように特開平１０−２８３４６１号公報等に記載された車外監視装置等をベースに構成されている。詳細な説明はそれらの公報に委ねる。以下、簡単にその構成について説明する。
【０１３６】
立体物検出手段２０は、距離データメモリ１７から前述した距離画像を読み出して、距離画像を所定の画素幅で垂直方向に延びる短冊状の区分に分割する。そして、短冊状の各区分に属する各視差ｄｐを前記（９）式に従ってそれぞれ距離Ｚに変換し、各距離Ｚのうち道路面より上方に存在すると位置付けられる距離に関してヒストグラムを作成して、度数が最大の区間までの距離をその短冊状の区分における立体物までの距離とする。これを全区分について行うようになっている。以下、各区分を代表する距離を距離Ｚという。
【０１３７】
例えば、図１３に示した基準画像Ｔから作成された距離画像に対して前記距離Ｚの算出を行い、算出された各区分ごとの距離Ｚを実空間上にプロットすると、図１４に示すように自車両前方の立体物の自車両ＭＣに面した部分に対応する部分に多少バラツキを持って各点としてプロットされる。
【０１３８】
立体物検出手段２０は、このようにプロットされる各点を図１５に示すように近接する各点の距離や方向性に基づいて互いに隣接する各点をそれぞれグループＧ１〜Ｇ７にグループ化し、図１６に示すようにそれぞれのグループ内の各点が自車両ＭＣの車幅方向すなわちＸ軸方向に略平行に並ぶサブグループには“物体”Ｏ１〜Ｏ３とラベルし、各点が自車両ＭＣの進行方向すなわちＺ軸方向に略平行な並ぶサブグループには“側壁”Ｓ１〜Ｓ４とラベルして分類する。また、同一の立体物の“物体”と“側壁”の交点をコーナー点Ｃとしてラベルするようになっている。
【０１３９】
立体物検出手段２０は、このようにして、［物体Ｏ１、コーナー点Ｃ、側壁Ｓ１］、［側壁Ｓ２］、［物体Ｏ２］、［物体Ｏ３］、［側壁Ｓ３］、［側壁Ｓ４］をそれぞれ立体物として検出するようになっている。立体物検出手段２０は、このようにして検出した立体物の情報や各サブグループの端点の座標等をそれぞれメモリに保存するようになっている。
【０１４０】
先行車両検出手段２１は、自車両の進行路上や自車両が走行している走行レーン上に存在する立体物の中で、自車両に最も近接する立体物を先行車両として検出するようになっている。本実施形態では、先行車両検出手段２１は、自車両の進行路上に先行車両を検出するようになっていて、まず、センサ類Ｑである車速センサやヨーレートセンサ、舵角センサから入力される自車両の挙動すなわち自車両の車速Ｖやヨーレートγ、ステアリングホイールの舵角δ等に基づいて自車両の旋回曲率Ｃuaを算出して、図１７に示すように自車両ＭＣの走行軌跡Ｌestを算出するようになっている。
【０１４１】
旋回曲率Ｃuaは、例えば車速Ｖとヨーレートγとを用いて、
Ｃua＝γ／Ｖ …（１０）
に従って算出される。また、例えば車速Ｖと舵角δとを用いて、
Ｒｅ＝（１＋Ａsf・Ｖ２）・（Ｌwb／δ） …（１１）
Ｃua＝１／Ｒｅ …（１２）
に従って算出することもできる。ここで、Ｒｅは旋回半径、Ａsfは車両のスタビリティファクタ、Ｌwbはホイールベースである。
【０１４２】
先行車両検出手段２１は、このようにして算出した走行軌跡を中心とする自車両の車幅分の領域を自車両の進行路Ｒestとして把握し、その上に存在する立体物のうち自車両ＭＣに最も近い物体Ｏ２を検出するようになっている。
【０１４３】
先行車両検出手段２１は、さらに、前回検出された先行車両の情報をメモリから読み出し、前回検出した先行車両と今回検出した自車両に最も近接する立体物すなわち物体Ｏ２との位置関係や移動速度等に基づいて前回の先行車両と今回検出した立体物とが同一の立体物である確率をそれぞれ算出し、算出された確率が予め設定された閾値以上であれば今回検出した立体物を先行車両とラベル付けして先行車両を検出し、先行車両の情報を今回検出した立体物の情報で更新してメモリに継続登録することで、先行車両の情報を更新しながらそれを追跡するようになっている。
【０１４４】
なお、前述したように、先行車両検出手段２１を、自車両ＭＣが走行している走行レーン上に存在し自車両ＭＣに最も近接する立体物として先行車両を検出するように構成することも可能である。自車両ＭＣの走行レーンを検出するために必要な自車両ＭＣの左右の区画線等の車線の位置を検出するための装置としては、例えば本願出願人により先に提出された特開２００１−９２９７０号公報に記載の車線認識装置等を用いることができる。
【０１４５】
平滑化フィルタ装置２２には、先行車両検出手段２１が検出した先行車両すなわちこの場合は物体Ｏ２と自車両ＭＣとの距離Ｚが入力されるようになっている。この場合の物体Ｏ２の距離Ｚとしては、例えば車幅方向すなわちＸ軸方向に延在する物体Ｏ２の中心位置の自車両ＭＣからのＺ軸方向の距離Ｚが入力される。
【０１４６】
平滑化フィルタ装置２２は、前記第１の実施形態において詳述した演算処理を行い、算出した自車両ＭＣと先行車両Ｏ２との平滑化された相対速度を、最終的に速度検出装置１０により検出された相対速度として装置外に出力するようになっている。具体的には、図１１に示したフローチャートに従って加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４で演算処理を行い、出力値Ｘout(t)である自車両ＭＣと先行車両Ｏ２との相対速度を出力するようになっている。
【０１４７】
なお、この場合、物体Ｏ２と自車両ＭＣとの距離Ｚが第１の実施形態における他の物理量Ｙin(t)に相当し、距離Ｚの時間微分である相対速度ΔＶが他の物理量Ｙin(t)の時間微分ｄＹin(t)すなわちＸin(t)に相当し、出力される平滑化された相対速度ΔＶが出力値Ｘout(t)に相当する。また、本実施形態では、出力値Ｘout(t)の算出に用いられる入力値の時間微分ｄＸin(t)や出力値の時間微分ｄＸout(t)、出力値の時間微分の平滑化値ｄＸs(t)はそれぞれ加速度［ｍ／ｓ２］のディメンジョンを有する。
【０１４８】
次に、本実施形態に係る速度検出装置１０の作用について説明する。
【０１４９】
上記のような本実施形態に係る速度検出装置１０の構成では、例えばＣＣＤカメラ等の撮像手段１１の解像度等の影響で、自車両と先行車両との距離Ｚが実際には連続的に滑らかに変化しているにもかかわらず、あるサンプリングタイミングｔ−１で検出された距離Ｚ＝Ｙin(t-1)と次のサンプリングタイミングｔで検出された距離Ｚ＝Ｙin(t)とが比較的大きく変化したように検出されることが多い。このような場合、その時間微分Ｘin(t)＝ｄＹin(t)である相対速度ΔＶは比較的大きく変化して、相対速度ΔＶのデータが時系列的に暴れを有するデータとなる。
【０１５０】
実際に、本実施形態の構成を備えた速度検出装置１０により自車両と先行車両との距離Ｚ＝Ｙin(t)を測定すると、図１８に示すように比較的滑らかに推移するデータが得られるが、その相対速度ΔＶである時間微分ｄＹin(t)＝Ｘin(t)を前記（６）式に従って算出すると、図１９に示すような比較的暴れの大きい時系列データＸin(t)が得られる。
【０１５１】
さらに、平滑化フィルタ装置２２の加算割合決定手段２で、自車両と先行車両との相対加速度に相当するＸin(t)の時間微分ｄＸin(t)を前記（１）式に従って算出すると、図２０に示すような暴れの大きい時系列データｄＸin(t)が得られる。また、算出される出力値の時間微分の平滑化値ｄＸs(t)のＫ倍は同図に示すようにＸin(t)の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態で推移する。
【０１５２】
平滑化フィルタ装置２２の加算割合決定手段２では、図２１に示すようにこのｄＸin(t)とｄＸs(t-1)×Ｋとの差分Ｄ(t)の絶対値に応じて激しく増減する加算割合Ｐが決定される。そして、このように各サンプリングタイミングｔごとに変動する加算割合Ｐすなわち時定数に基づいて、平滑化フィルタ装置２２の平滑化手段３で前記（３）式で示される時定数フィルタによって相対速度ΔＶに相当し暴れを有する時系列データであるＸin(t)が平滑化され、図２２に示すようなデータの暴れの振幅の中心付近の値に平滑化された出力値Ｘout(t)が出力される。
【０１５３】
なお、図２２において、Ｘcom(t)は、比較例として、前記（３）式の時定数フィルタにおける加算割合Ｐを０．１に固定した従来の時定数フィルタによる演算結果を表したものである。
【０１５４】
以上のように、本実施形態に係る速度検出装置１０によれば、平滑化フィルタ装置２２が前記第１の実施形態における効果を的確に発揮して、自車両と先行車両との距離Ｚから得られた比較的暴れの大きい相対速度ΔＶ＝Ｘin(t)の時系列データを有効に平滑化し、しかも、図２２に示したように、データの暴れの振幅の中心付近の値に的確に平滑化された相対速度ΔＶ＝Ｘout(t)を出力することが可能となる。
【０１５５】
そのため、この速度検出装置１０の出力値Ｘout(t)を例えば先行車両追従装置に適用すれば、先行車両と自車両との適切な相対速度ΔＶに基づいて適切な先行車両追従制御を行うことが可能となり、例えば図２２に示した従来の時定数フィルタの出力値Ｘcom(t)に基づいて制御を行った場合に必要以上の制動が加わってドライバに違和感を与えたりドライバがそれに反発して車間をつめるために不必要な加速を強いることを有効に防止することが可能となる。
【０１５６】
なお、上記の第２の実施形態では、平滑化された自車両と先行車両との相対速度ΔＶを出力するにあたって、自車両と先行車両との距離Ｚから相対速度ΔＶを求める場合について説明した。しかし、本発明はこの場合に限定されず、例えばドップラーレーダ等の測定手段により自車両と先行車両との相対速度ΔＶを直接測定し、測定された相対速度ΔＶを平滑化フィルタ装置２２により平滑化して検出し出力する場合についても適用される。
【０１５７】
その場合、速度検出装置の平滑化フィルタ装置２２における演算処理は、図２に示したフローチャートに従って行われる。そして、図２のフローチャートにおいて入力値Ｘin(t)は自車両と先行車両との相対速度ΔＶであり、出力値Ｘout(t)は平滑化された自車両と先行車両との相対速度ΔＶとなる。
【０１５８】
また、本発明は、前述したような自車両と先行車両との相対速度ΔＶの平滑化に限定されず、例えば自車両の車速Ｖの平滑化に適用することも可能である。自車両の車速Ｖの測定手段としては、例えば本願出願人により先に提出された特開２００４−３１７２０６号公報に記載された自車両の車輪の回転を検出して車速Ｖを算出するパルス検出センサを用いることができる。
【０１５９】
さらに、上記の第２の実施形態では、速度検出装置を自動車やトラック等の車両に搭載する場合について説明したが、本実施形態に係る速度検出装置は、この他にも、例えば鉄道車両や航空機、船舶等の運輸手段や、ロボット、各種機械・機器等のように自ら移動し或いは他の物体等を移動させることで物体の速度を観念でき、その速度を測定できるものであれば、いずれにも適用可能である。
【０１６０】
ところで、上記の第２の実施形態のように自車両と先行車両との距離Ｚから相対速度ΔＶを求める構成とすると、自車両と先行車両との距離Ｚが大きいほど距離画像の１画素あたりの距離差が大きくなり検出された距離Ｚの検出誤差が大きくなる傾向がある。そのため、図１８、図１９に示したように、「他の物理量」である距離Ｚが大きい場合ほど、その時間微分Ｘin(t)であり平滑化の対象となる「物理量のデータ」すなわち自車両と先行車両との相対速度ΔＶは暴れが大きくなり、距離Ｚが小さいほど相対速度ΔＶの暴れが小さくなるという特徴的な傾向を示す。
【０１６１】
その結果、図２０に示したように、相対加速度を表すｄＸin(t)も距離Ｚが小さくなるほど暴れが小さくなり、同図の場合のように係数Ｋを一定とした場合には、距離Ｚが小さくなり相対加速度ｄＸin(t)の暴れの振幅が減少し始めたサンプリングタイミングｔでは、相対加速度ｄＸin(t)の振幅の下端に対してｄＸs(t)×Ｋが若干離れている。
【０１６２】
このように、「他の物理量」である距離Ｚが小さいほど相対加速度ｄＸin(t)の振幅も小さくなる場合には、それに適合させるため、前記所定の係数Ｋを他の物理量の値が小さいほど小さい値となるように可変させることが好ましい。適合の手法としては、上記のように距離Ｚを所定の範囲ごとに区分して区分ごとに係数Ｋの値が設定されたテーブルを用意したり、係数Ｋの値を距離Ｚの関数として算出するように構成することが可能である。
【０１６３】
また、図２０に示したように、距離Ｚが大きい場合に相対加速度ｄＸin(t)が比較的大きく暴れるため、加算割合Ｐを決定するための基準となる出力値の時間微分の平滑化値ｄＸs(t)が、距離Ｚが大きい場合には距離Ｚが小さい場合と比較して比較的大きな暴れを有している場合がある。
【０１６４】
そのため、本実施形態では前記（５）式に示したように出力値の時間微分の平滑化値ｄＸs(t)の算出における時定数を一定としたが、時定数を可変とし、自車両と先行車両との距離Ｚが大きいほど前記（５）式における右辺第１項すなわちｄＸs(t-1)の係数をさらに大きくするように修正し、平滑化値ｄＸs(t)の暴れを少なくするようにして平滑化値ｄＸs(t)を算出するように構成することが好ましい。
【０１６５】
また、別の観点として、自車両が高速道路を走行している場合には、通常の道路を走行している場合に比べて自車両と先行車両との相対速度の増減の程度がさほど大きくなく、相対加速度ｄＸin(t)が比較的安定して推移する傾向がある。このような場合には、前記と同様に、加算割合Ｐを決定するための基準となる出力値の時間微分の平滑化値ｄＸs(t)を算出するための前記（５）式において右辺第１項すなわちｄＸs(t-1)の係数をさらに大きくするように修正して、平滑化値ｄＸs(t)の暴れを少なくするように構成することが可能となる。
【０１６６】
そこで、速度検出装置１０に、自車両が高速道路を走行していることを検出する手段を備えておき、この手段により自車両が高速道路を走行していることが検出された場合には、前記（５）式においてｄＸs(t-1)の係数をさらに大きくするように修正して平滑化値ｄＸs(t)を算出するように構成することが好ましい。
【０１６７】
前記手段としては、例えば、ＧＰＳ（Global Positioning System）受信機等を搭載したナビゲーション装置からの情報に基づいて地図情報から検出したり、或いは自車両の車速が時速８０ｋｍ等の所定の速度以上である状態が所定時間以上継続したことを認識したり、或いは自車両の左右の車線位置の間隔を測定しその間隔が所定間隔以上であることを認識するなどして、自車両が高速道路を走行していることを検出するように構成することが可能である。
【図面の簡単な説明】
【０１６８】
【図１】第１の実施形態に係る平滑化フィルタ装置の構成を示すブロック図である。
【図２】平滑化フィルタ装置における演算処理の手順を示すフローチャートである。
【図３】算出された入力値の時間微分ｄＸin(t)を表すグラフである。
【図４】算出された出力値の時間微分の平滑化値ｄＸs(t)とそのＫ倍された値を表すグラフである。
【図５】差分の絶対値の各範囲に加算割合が割り当てられたテーブルの例を表す図である。
【図６】加算割合を固定した場合の出力値と理想的な平滑化曲線とを表すグラフである。
【図７】増加傾向にある物理量のデータの入力値を表すグラフである。
【図８】図７の入力値に基づく出力値の時間微分の平滑化値ｄＸs(t)とそのＫ倍された値を表すグラフである。
【図９】入力値が増加傾向にある場合の加算割合を固定した場合の出力値と理想的な平滑化曲線とを表すグラフである。
【図１０】入力値の変化傾向と加算割合とを対応させるテーブルの例を表す図である。
【図１１】他の物理量の時間微分を平滑化するための演算処理の手順を示すフローチャートである。
【図１２】第２の実施形態に係る速度検出装置の構成を示すブロック図である。
【図１３】基準画像の一例を示す図である。
【図１４】区分ごとの距離を実空間上にプロットした各点を表す図である。
【図１５】図１４の各点をグループ化した場合の各グループを表す図である。
【図１６】図１５の各グループに基づいて検出された物体や側壁を表す図である。
【図１７】自車両の走行軌跡と進行路を説明する図である。
【図１８】測定された距離の例を表すグラフである。
【図１９】図１８の距離に基づいて算出された相対速度を表すグラフである。
【図２０】図１９の相対速度に基づいて算出された相対加速度に相当するｄＸin(t)および出力値の時間微分の平滑化値ｄＸs(t)のＫ倍を表すグラフである。
【図２１】図２０に基づいて決定された加算割合を表すグラフである。
【図２２】図２１の加算割合に基づいて算出された出力値を表すグラフである。
【図２３】減少傾向にある相対速度の時系列データを表すグラフである。
【図２４】図２３のデータを理想的に平滑化した場合の平滑化曲線と従来の平滑化フィルタにより平滑化した場合の曲線を表すグラフである。
【符号の説明】
【０１６９】
１、２２ 平滑化フィルタ装置
２ 加算割合決定手段
３ 平滑化手段
４ 時間微分平滑化値算出手段
１０ 速度検出装置
１８ 測定手段
Ｄ(t) 差分
ｄＸin(t) 入力値の時間微分
ｄＸout(t) 出力値の時間微分
ｄＸs(t) 出力値の時間微分の平滑化値
Ｋ 係数
ＭＣ 自車両
Ｏ２ 先行車両
Ｐ 加算割合
Δｔ サンプリング周期
Ｖ 車速
ΔＶ 相対速度
Ｘ 物理量
Ｘin(t) 物理量のデータの入力値
Ｘout(t) 物理量のデータの出力値
Ｙ 他の物理量
Ｚ 距離
【技術分野】
【０００１】
本発明は、平滑化フィルタ装置および速度検出装置に係り、特に、入力された物理量のデータを平滑化する平滑化フィルタ装置とそれを用いて物体の速度を検出する速度検出装置に関する。
【背景技術】
【０００２】
近年、乗用車や貨物自動車等の車両では、自車両前方を撮像した画像の解析やレーダ装置による計測等により先行車両との距離や相対速度を検出して、自動的に自車両の加減速制御を行う先行車両追従装置や衝突防止装置の開発が進められている（例えば特許文献１、２参照）。
【０００３】
このような制御を確実に行うためには、自車両と先行車両との距離や相対速度等を正確にかつ遅れなく検出することが必要となる。しかし、実際には、例えば相対速度を車速センサを用いて検出したり時系列データとして検出した距離の時間的な差分から検出したりする場合、センサ自体が有する誤差や精度或いは検出した距離の精度や誤検出された結果等のために、図２３に模式的に示すように、時系列データとして検出された相対速度に、その値が大きく上下するいわゆる暴れが生じる。
【０００４】
このように値が上下に大きく暴れる時系列データをそのまま制御に用いることはできないため、このような時系列データの暴れを抑えることを目的として、データをカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタで平滑化することが一般的に行われている（例えば特許文献３参照）。
【特許文献１】特開平１０−６９５９８号公報
【特許文献２】特開２００６−１８８１５５号公報
【特許文献３】特開２００１−２４２２４２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
ところで、図２３に例示した暴れを有するデータでは、平滑化フィルタを用いることにより図２４に曲線Ａで示されるようにデータの暴れの振幅の中心付近を通るようなデータに平滑化されることが望まれる。しかしながら、前記のような平滑化フィルタを用いた平滑化では、通常、そのフィルタの特性によりそれぞれ特徴を持った平滑化がなされる。
【０００６】
例えば前記の３種類の平滑化フィルタのうち、移動平均フィルタや時定数フィルタを用いた場合には、通常、理想的な平滑化曲線Ａに比べて時間遅れをもって変化する曲線Ｂ、すなわち図２４の例では時系列データにあわせて減少していく平滑化曲線Ａと比較してΔＴだけ時間的に遅れて減少していく曲線Ｂのようにデータが平滑化される。
【０００７】
曲線Ｂのように時間遅れをもって平滑化された結果に基づいて自車両の自動制御が行われると、例えばこの時系列データが相対速度のデータである場合には、ある時刻における相対速度が平滑化曲線Ａで表される実際の相対速度よりも大きな相対速度であると判断されて大きな制動が自動的に加わり、ドライバに違和感を与えるとともに、車間をつめるために不必要な加速を強いることとなる。
【０００８】
また、この時系列データが自車両と先行車両との距離のデータである場合には、ある時刻における距離が平滑化曲線Ａで表される実際の距離よりも大きな距離であると判断されて、今度は逆に必要な量の制動が加わらないため、ドライバが急ブレーキを踏んだり、最悪の場合には先行車両に追突するという事態を招いてしまう。
【０００９】
一方、図２４の時系列データにカルマンフィルタを適用して平滑化した場合には、通常、理想的な平滑化曲線Ａに比べてデータの増減がより強調された曲線Ｃのようにデータが平滑化される。この場合、曲線Ｃに基づいて自車両の自動制御が行われると、時系列データが距離や相対速度のデータである場合には、その値が実際よりも大きく急激に変化したと認識されて急制動がかかるなどして、やはりドライバに違和感を与えたり、最悪の場合には先行車両に追突するという事態を招いてしまう。
【００１０】
このように、暴れを有するデータを平滑化し、その平滑化したデータに基づいて自動制御等を行うような場合には、従来のカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタをそのまま用いることは必ずしも有効とはいえない。図２４に示した理想的な平滑化曲線Ａのように、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することができる平滑化フィルタの開発が望まれる。
【００１１】
本発明は、このような事情に鑑みてなされたものであり、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することが可能な平滑化フィルタ装置を提供することを目的とする。また、それを用いて物体の速度を的確に検出可能な速度検出装置を提供することをも目的とする。
【課題を解決するための手段】
【００１２】
前記の問題を解決するために、第１の発明は、
時系列的に離散的に入力される物理量のデータを平滑化して出力する平滑化フィルタ装置において、
加算割合を決定する加算割合決定手段と、
今回入力された前記物理量のデータの入力値と所定周期前の出力値とを前記加算割合により重み付け加算して算出した値を平滑化された前記物理量のデータの出力値として出力する平滑化手段と、
前記出力値の時間微分を算出して、前記出力値の時間微分の平滑化値を算出する時間微分平滑化値算出手段と
を備え、
前記加算割合決定手段は、前記入力値の時間微分を算出し、算出した前記入力値の時間微分と前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記加算割合を決定することを特徴とする。
【００１３】
第２の発明は、第１の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記入力値の変化傾向を判定し、判定した前記入力値の変化傾向と前記入力値の時間微分との関係に基づいて前記加算割合を決定することを特徴とする。
【００１４】
第３の発明は、第１の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値を所定の係数で増幅した値と前記入力値の時間微分との差分を算出し、その差分に基づいて前記加算割合を決定することを特徴とする。
【００１５】
第４の発明は、第３の発明の平滑化フィルタ装置において、前記所定の係数は、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅内に収まるように設定されることを特徴とする。
【００１６】
第５の発明は、第３または第４の発明の平滑化フィルタ装置において、前記所定の係数は、正の値とされ、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅の端部付近の値となるように設定されることを特徴とする。
【００１７】
第６の発明は、第３から第５のいずれかの発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記入力値に重み付けする前記加算割合を、前記差分の絶対値が小さいほど大きい値となるように決定することを特徴とする。
【００１８】
第７の発明は、第６の発明の平滑化フィルタ装置において、前記加算割合決定手段は、複数の範囲に分割された前記差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備え、前記テーブルに基づいて前記加算割合を決定することを特徴とする。
【００１９】
第８の発明は、第６の発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記差分の絶対値についての単調減少関数を備え、前記単調減少関数に基づいて前記加算割合を決定することを特徴とする。
【００２０】
第９の発明は、第１から第８のいずれかの発明の平滑化フィルタ装置において、前記加算割合決定手段は、前記入力値の入力回数が所定回数に満たない間は、前記加算割合を予め設定された一定値に決定することを特徴とする。
【００２１】
第１０の発明は、第１から第９のいずれかの発明の平滑化フィルタ装置において、前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して現在のサイクリング周期の前記出力値の時間微分の平滑化値を算出することを特徴とする。
【００２２】
第１１の発明は、第１から第１０のいずれかの発明の平滑化フィルタ装置において、前記物理量のデータは、他の物理量を時間微分して算出される前記他の物理量の変化速度のデータであることを特徴とする。
【００２３】
第１２の発明は、第３から第１０のいずれかの発明の平滑化フィルタ装置において、前記物理量のデータは、他の物理量を時間微分して得られる前記他の物理量の変化速度のデータであり、前記所定の係数は、前記他の物理量の値が小さいほど小さい値となるように可変させることを特徴とする。
【００２４】
第１３の発明は、第１から第１０のいずれかの発明の平滑化フィルタ装置を用いて物体の速度を検出する速度検出手装置であって、
前記物理量のデータは前記物体の速度を測定する測定手段から入力される速度のデータであり、平滑化された前記速度を出力することを特徴とする。
【００２５】
第１４の発明は、第１１または第１２の発明の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記他の物理量は前記物体の距離を測定する測定手段から入力される距離であり、前記物理量のデータは前記距離を時間微分して得られる前記物体の速度のデータであり、平滑化された前記速度を出力することを特徴とする。
【００２６】
第１５の発明は、第１３または第１４の発明の速度検出装置において、前記速度は、自車両の車速であることを特徴とする。
【００２７】
第１６の発明は、第１３または第１４の発明の速度検出装置において、前記速度は、自車両と先行車両との相対速度であることを特徴とする。
【００２８】
第１７の発明は、第１６の発明の速度検出装置において、前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する場合には、前記自車両と先行車両との距離が大きいほど前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする。
【００２９】
第１８の発明は、第１５から第１７のいずれかの発明の速度検出装置において、
自車両が高速道路を走行していることを検出する手段を備え、
前記時間微分平滑化値算出手段は、前記手段により自車両が高速道路を走行していることが検出された場合には、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する際に、前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする。
【発明の効果】
【００３０】
第１の発明によれば、平滑化フィルタ装置を、従来の平滑化フィルタのように時間遅れを生じることなく時系列データの暴れの振幅の中心付近の値に的確に平滑化するように構成するためには、データの入力値の変化傾向を的確に捉え、そのデータの変化傾向を平滑化された出力値に的確に反映させることが必要となる。
【００３１】
本発明では、時間微分平滑化値算出手段で平滑化された出力値の時間微分をさらに平滑化しその平滑化値から入力値の全体的な変化傾向を把握する。そして、加算割合決定手段でこの出力値の時間微分の平滑化値から把握される入力値の全体的な変化傾向と同じ方向、すなわち例えば入力値の変化傾向が減少傾向にあれば物理量のデータを減少させる方向の入力値の加算割合が増大するように加算割合を決定する。そして、最後にその加算割合に基づいて平滑化手段で時定数フィルタ処理を行い、入力値を強調して所定周期前の出力値と重み付け加算する。
【００３２】
このようにして物理量のデータの入力値の変化傾向を平滑化される出力値に的確に反映させることが可能となり、平滑化される出力値を入力値の変化傾向にあわせて的確に増減させることが可能となる。そのため、従来の移動平均フィルタや時定数フィルタ等の平滑化フィルタのように時間遅れを発生させることなく、また、カルマンフィルタのように必要以上にデータの増減を強調することなく、時系列データの暴れの振幅の中心付近の値に的確に平滑化された理想的な平滑化曲線を得ることが可能となる。
【００３３】
第２の発明によれば、出力値の時間微分の平滑化値から入力値の変化傾向が増加傾向にあるのか、減少傾向にあるのか、或いはあまり変化しない傾向であるのかを判定し、例えばその傾向の分類に対応して予め加算割合が割り当てられたテーブル等に基づいて加算割合を決定する。そのため、前記発明の効果に加え、加算割合を簡単に求めることが可能となり、演算負荷の低減を図ることが可能となる。
【００３４】
第３の発明によれば、入力値の全体的な変化傾向を表す出力値の時間微分の平滑化値を所定の係数で増幅した値と、入力値の時間微分との差分に基づいて加算割合を決定することで、出力値を算出するための時定数フィルタ処理において入力値を強調する度合を表す加算割合を差分という具体的な数値に基づいて決定することが可能となり、入力値を出力値に的確に反映させて、前記各発明の効果をより的確に発揮させることが可能となる。
【００３５】
第４の発明によれば、出力値の時間微分の平滑化値を増幅させる前記所定の係数は、例えばそれをあまりに大きな値に設定すると、出力値の時間微分の平滑化値自体が有する暴れがより拡大されると同時に、出力値の時間微分の平滑化値を所定の係数で増幅した値と入力値の時間微分の値とが大きく異なる値となり、差分が有する有用性が弱まる。
【００３６】
そのため、その所定の係数で出力値の時間微分の平滑化値を増幅した値が入力値の時間微分の振幅内に収まるように適切に設定されることで、出力値の時間微分の平滑化値自体が有する暴れの拡大を適切な範囲に収め、差分が有する有用性を維持した状態で差分に基づいて加算割合を決定することが可能となり、前記各発明の効果がより的確に発揮される。
【００３７】
第５の発明によれば、正の値の所定の係数により出力値の時間微分の平滑化値を増幅した値が入力値の時間微分の振幅の端部付近の値となるように設定される。この場合、出力値の時間微分の平滑化値を所定の係数で増幅した値と入力値の時間微分の差分の絶対値が小さくなるような入力値は、平滑化される出力値を入力値の変化傾向にあわせて的確に増減させるために強調されるべき入力値である。従って、その差分の絶対値と加算割合との対応づけが容易になり、前記第３の発明や第４の発明の効果がより的確に発揮される。
【００３８】
第６の発明によれば、入力値に重み付けする加算割合を差分の絶対値が小さいほど大きい値となるように決定することで、差分と加算割合を非常に単純な対応づけにより対応づけることが可能となり、前記各発明の効果が的確かつ容易に発揮される。
【００３９】
第７の発明によれば、加算割合決定手段に、複数の範囲に分割された差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備えておき、入力値に重み付けする加算割合を差分の絶対値が小さいほど大きい値となるように加算割合を割り当て、そのテーブルに基づいて加算割合を決定するように構成すれば、前記各発明の効果をより的確にかつより容易に発揮することが可能となる。
【００４０】
第８の発明によれば、加算割合決定手段に、差分の絶対値について単調に減少する関数を備えておき、その単調減少関数に基づいて加算割合を決定するように構成すれば、前記各発明の効果をより的確にかつより容易に発揮することが可能となる。
【００４１】
第９の発明によれば、第１から第８のいずれかの発明の平滑化フィルタ装置において、 平滑化フィルタ装置の起動時などにはメモリに所定周期前の出力値の時間微分の平滑化値等の情報が保存されていなかったり、記憶されていても出力値の時間微分の平滑化値が安定していない場合がある。そのため、前記各発明の効果に加え、入力値の数が所定回数分に満たない間は加算割合を可変させずに予め設定された一定値に決定することで、平滑化フィルタ装置の起動時などに出力値を安定させて適切な値を出力させることが可能となり、装置の信頼性を向上させることが可能となる。
【００４２】
第１０の発明によれば、入力値の変化傾向を表す出力値の時間微分の平滑化値を算出する際に、出力値の時間微分とその所定周期前の出力値の時間微分の平滑化値とを所定の割合で重み付け加算する時定数フィルタ処理により算出することで、出力値の時間微分の平滑化値が、入力値や入力値の時間微分の暴れに追随して暴れることなく、滑らかに推移するようになる。そのため、滑らかに推移する出力値により入力値の変化傾向を的確に把握して加算割合を決定することが可能となり、前記各発明の効果をより的確に発揮することが可能となる。
【００４３】
第１１の発明によれば、平滑化フィルタ装置は、前記各発明により、測定手段で測定され入力された物理量のデータを平滑化して出力することが可能であることは言うまでもないが、例えば測定手段が測定した物理量が時間微分されて形成されたその物理量の変化速度のデータを平滑化して出力するように構成することも可能である。そして、そのように構成された場合にも、前記各発明と同様に物理量の変化速度のデータの暴れの振幅の中心付近の値に的確に平滑化された理想的な平滑化曲線を得ることが可能となり、物理量の変化速度のデータに対して前記各発明の効果が的確に発揮される。
【００４４】
第１２の発明によれば、例えば自車両と先行車両との距離の測定値を時間微分して得られる変化速度である自車両と先行車両との相対速度を平滑化する場合、自車両と先行車両との距離が大きい場合には相対速度の暴れが大きくその時間微分である相対加速度の暴れも大きいが、自車両と先行車両との距離が小さくなるにつれて相対速度の暴れが小さくなり相対加速度の暴れも小さくなることがある。そのような場合、相対加速度の暴れの端部に沿うように設定された所定の係数で増幅された出力値の時間微分の平滑化値が、前記距離が小さくなってくると暴れの端部から離れてしまい、前記差分の有用性が弱まってしまう場合がある。
【００４５】
そのため、そのような場合には、所定の係数を「他の物理量」である自車両と先行車両との距離の値が小さいほど小さい値となるように可変させるように構成すれば、所定の係数で増幅された出力値の時間微分の平滑化値が相対加速度の暴れの端部から離れてしまうことを防止することができ、差分の有用性を維持することが可能となり、前記各発明の効果がより効果的に発揮される。
【００４６】
第１３の発明によれば、第１〜第１０の発明の平滑化フィルタ装置は、自動車やトラック等の車両や鉄道車両、航空機、船舶等の運輸手段或いはロボットや各種機械・機器等のように自ら移動し、他の物体等を移動し、或いは自己の内部部品を移動させることで観念される物体の速度を検出する速度検出装置に適用することができる。
【００４７】
そして、速度検出装置を、物体の速度を測定する測定手段から入力される速度のデータに対して演算処理を行い、平滑化された速度を出力するように構成すれば、もともと暴れを有していた速度のデータを、そのデータの暴れの振幅の中心付近の値に的確に平滑化された速度として出力することが可能となり、速度検出装置により物体の速度を的確に検出することが可能となる。
【００４８】
第１４の発明によれば、第１１または第１２の発明の平滑化フィルタ装置は、自動車やトラック等の車両や鉄道車両、航空機、船舶等の運輸手段或いはロボットや各種機械・機器等のように他の物体との距離を把握し、その把握された距離を時間微分して物体の速度を検出する速度検出装置に適用することができる。
【００４９】
そして、速度検出装置を、他の物理量として測定手段から入力される距離のデータを時間微分して物体の速度を算出し、その速度に対して演算処理を行い、平滑化された速度を出力するように構成すれば、もともと暴れを有していた速度のデータを、そのデータの暴れの振幅の中心付近の値に的確に平滑化された速度として出力することが可能となり、速度検出装置により物体の速度を的確に検出することが可能となる。
【００５０】
第１５の発明によれば、前記の速度検出装置においては、平滑化される速度が自車両の車速である場合にも前記各発明の効果が効果的に発揮される。
【００５１】
第１６の発明によれば、前記の速度検出装置においては、平滑化される速度が自車両と先行車両との相対速度である場合にも前記各発明の効果が効果的に発揮される。
【００５２】
第１７の発明によれば、平滑化される速度が自車両と先行車両との相対速度である場合には、自車両と先行車両との距離が大きい場合に距離が小さい場合に比べて相対加速度が比較的大きく暴れることがあり、前記所定の割合を一定に維持したままであると出力値の時間微分の平滑化値も比較的大きく暴れてしまう場合がある。
【００５３】
そのため、第１７の発明の構成とすれば、所定の割合を一定のままに維持した場合に比べて出力値の時間微分の平滑化値の暴れをより少なくすることが可能となり、出力値の時間微分の平滑化値を滑らかに推移させて、相対速度のデータの変化傾向を的確に把握することが可能となる。そのため、平滑化フィルタ装置の機能が的確に発揮され、それを用いた速度検出装置についての前記各発明の効果がより的確に発揮される。
【００５４】
第１８の発明によれば、自車両が高速道路を走行している場合には、通常の道路を走行している場合に比べて自車両と先行車両との相対速度の増減の程度がさほど大きくなく、相対加速度が比較的安定して推移する傾向がある。従って、速度検出装置に自車両が高速道路を走行していることを検出する手段を備え、第１８の発明の構成とすれば、出力値の時間微分の平滑化値の暴れをより少なくすることが可能となり、出力値の時間微分の平滑化値を滑らかに推移させて、相対速度のデータの変化傾向を的確に把握することが可能となる。そのため、平滑化フィルタ装置の機能が的確に発揮され、それを用いた速度検出装置についての前記各発明の効果がより的確に発揮される。
【発明を実施するための最良の形態】
【００５５】
以下、本発明に係る平滑化フィルタ装置およびそれを用いた速度検出装置の実施の形態について、図面を参照して説明する。
【００５６】
［第１の実施の形態］
まず、平滑化フィルタ装置について説明する。平滑化フィルタ装置１は、図示しないＣＰＵやＲＯＭ、ＲＡＭ等がバスに接続されたマイクロコンピュータより構成されている。
【００５７】
本実施形態に係る平滑化フィルタ装置１は、図１に示すように、主に加算割合決定手段２と、平滑化手段３と、時間微分平滑化値算出手段４と、メモリ５とを備えて構成されている。平滑化フィルタ装置１は、所定のサンプリング周期で時系列的かつ離散的に所定の物理量Ｘのデータが入力値Ｘin(t)としてＩ／Ｏインターフェース６を介して入力されると、以下に説明する演算処理を行った後、その物理量Ｘのデータの平滑化された値を出力値Ｘout(t)として出力するようになっている。
【００５８】
ここで、本実施形態では、変数ｔはサンプリングタイミングを表す変数であり、以下では、ｔは今回すなわち所定の物理量Ｘの時系列データのうち入力値Ｘin(t)が入力されたサンプリングタイミングを表し、ｔ−１は前回すなわち今回のサンプリングタイミングｔの１周期前のサンプリングタイミングを表すものとする。
【００５９】
以下、図２に示すフローチャートに従って平滑化フィルタ装置１の加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４の構成と演算処理について説明する。
【００６０】
まず、装置全体の演算処理の流れを概説すると、平滑化フィルタ装置１は、加算割合決定手段２で物理量Ｘのデータの時間微分に基づいて加算割合Ｐを決定し（ステップＳ２〜Ｓ５）、平滑化手段３で加算割合Ｐを用いて今回の入力値Ｘin(t)と前回の出力値Ｘout(t-1)との重み付け加算により物理量Ｘのデータを平滑化して今回の出力値Ｘout(t)を算出するようになっている（ステップＳ６）。
【００６１】
つまり、平滑化手段３では後述する（３）式に示すように時定数フィルタの手法を用いて物理量Ｘのデータの平滑化が行われるが、その際、従来の時定数フィルタのように時定数すなわち加算割合Ｐを一定の値に固定するのではなく、加算割合Ｐを加算割合決定手段２で今回入力された物理量Ｘのデータの時間微分に基づいて可変させ、各サンプリングタイミングｔで可変させることで、平滑化を適切に行うようになっている。
【００６２】
また、平滑化フィルタ装置１の時間微分平滑化値算出手段４では、次回の加算割合Ｐを決定するために前回の出力値Ｘout(t-1)の時間微分の平滑化値ｄＸs(t-1)を算出するようになっている（ステップＳ７〜Ｓ９）。
【００６３】
以下、詳しく説明する。なお、後述するように、メモリ５には、前回の出力値Ｘout(t-1)と前回の演算処理で算出した出力値の時間微分の平滑化値ｄＸs(t-1)とが保存されている。
【００６４】
加算割合決定手段２は、今回のサンプリングタイミングｔで物理量Ｘのデータの入力値Ｘin(t)が入力されると（ステップＳ１：ＹＥＳ）、メモリ５から前回の出力値Ｘout(t-1)を読み出して、下記（１）式に従って入力値の時間微分ｄＸin(t)を算出するようになっている（ステップＳ２）。ここで、Δｔはサンプリング周期を表す。
ｄＸin(t)＝（Ｘin(t)−Ｘout(t-1)）／Δｔ …（１）
【００６５】
入力される物理量Ｘのデータが例えば図２３に示したような時系列的な暴れを有するデータである場合、算出される入力値の時間微分ｄＸin(t)も図３に示すように時系列的な暴れを有する波形となる。
【００６６】
加算割合決定手段２は、続いて、メモリ５から前回のサンプリングタイミングで算出された出力値の時間微分の平滑化値ｄＸs(t-1)を読み出して、平滑化値ｄＸs(t-1)を所定の係数Ｋで増幅するようになっている（図２のステップＳ３）。ここで、本実施形態では、係数Ｋは正の値とする。
【００６７】
本実施形態では、前記所定の係数Ｋは予め設定されるようになっている。平滑化フィルタ装置１に入力される物理量Ｘのデータの入力値Ｘin(t)の暴れの具合はその物理量Ｘのデータを測定するセンサ等の測定装置により種々であり、平滑化フィルタ装置１が適用される測定装置に応じて適宜設定されることが必要となる。
【００６８】
物理量Ｘのデータとして、ある測定装置から例えば図２３に示したような時系列的な暴れを有するデータが入力され、図３に示すような入力値Ｘin(t)の時間微分ｄＸin(t)が得られるとする。この場合、図４に示すように係数Ｋで出力値Ｘout(t-1)の時間微分の平滑化値ｄＸs(t)を増幅した値ｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の上端および下端の内側に収まるように係数Ｋの値が設定されることが好ましい。
【００６９】
本実施形態では、特に、係数Ｋ倍された平滑化値ｄＸs(t)すなわちｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態になるように係数Ｋの値が設定されている。
【００７０】
加算割合決定手段２は、続いて、下記（２）式に従って前記（１）式で算出した入力値の時間微分ｄＸin(t)と、前回の出力値の時間微分の平滑化値ｄＸs(t-1)を所定の係数Ｋで増幅した値ｄＸs(t-1)×Ｋとの差分Ｄ(t)を算出するようになっている（図２のステップＳ４）。
Ｄ(t)＝ｄＸin(t)−ｄＸs(t-1)×Ｋ …（２）
【００７１】
加算割合決定手段２は、続いて、算出した差分Ｄ(t)に基づいて後述する平滑化手段３での重み付け加算に用いられる加算割合Ｐを決定するようになっている（ステップＳ５）。本実施形態では、加算割合決定手段２は、差分Ｄ(t)の絶対値|Ｄ(t)|が小さいほど加算割合Ｐが大きい値となるように決定するように構成されている。
【００７２】
具体的には、例えば差分Ｄ(t)が加速度のディメンジョンを有する場合、加算割合決定手段２には、図５に示すような差分の絶対値|Ｄ(t)|の複数の範囲に分割して各範囲ごとに加算割合Ｐが割り当てられたテーブルが備えられていれば、算出した差分Ｄ(t)とこのテーブルとに基づいて加算割合Ｐを決定することが可能となる。本実施形態では、加算割合決定手段２はこのようなテーブルを備えている。
【００７３】
なお、図５中、Ｇは標準重力加速度を表す。また、本実施形態のようにテーブルを準備しておく代わりに、例えば差分の絶対値|Ｄ(t)|を変数とする単調減少関数を備えておき、この単調減少関数に基づいて加算割合Ｐを決定するように構成することも可能である。
【００７４】
加算割合決定手段２は、以上のようにして決定した加算割合Ｐを平滑化手段３に送信するようになっている。
【００７５】
平滑化手段３は、加算割合決定手段２で決定された加算割合Ｐを用いて下記（３）式に従って今回の入力値Ｘin(t)と前回の出力値Ｘout(t-1)との重み付け加算により物理量Ｘのデータを平滑化して今回の出力値Ｘout(t)を算出するようになっている（図２のステップＳ６）。
Ｘout(t)＝Ｘout(t-1)×（１−Ｐ）＋Ｘin(t)×Ｐ …（３）
【００７６】
平滑化手段３は、このようにして算出した今回の出力値Ｘout(t)を出力するとともに（ステップＳ６）、時間微分平滑化値算出手段４に送るようになっている。
【００７７】
時間微分平滑化値算出手段４は、次回の加算割合決定手段２における演算処理で用いるために、平滑化手段３で算出された出力値Ｘout(t)に基づいて、前述した出力値の時間微分の平滑化値ｄＸs(t-1)を算出するようになっている。
【００７８】
具体的には、時間微分平滑化値算出手段４は、まず、平滑化手段３から送信されてきた今回の出力値Ｘout(t)とメモリ５から読み出した前回の出力値Ｘout(t-1)とを用いて、下記（４）式に従って出力値の時間微分ｄＸout(t)を算出するようになっている（ステップＳ７）。なお、Δｔは前記（１）式の場合と同様にサンプリング周期を表す。
ｄＸout(t)＝（Ｘout(t)−Ｘout(t-1)）／Δｔ …（４）
【００７９】
時間微分平滑化値算出手段４は、続いて、算出した出力値の時間微分ｄＸout(t)とメモリ５から読み出した前回の出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて、今回の出力値の時間微分の平滑化値ｄＸs(t)を算出するようになっている（ステップＳ８）。
【００８０】
この今回の出力値の時間微分の平滑化値ｄＸs(t)の算出においては、種々の平滑化手法を用いることが可能であり、前述したカルマンフィルタや移動平均フィルタ、時定数フィルタ等の平滑化フィルタを用いることができる。
【００８１】
本実施形態では、時間微分平滑化値算出手段４は、今回算出した出力値の時間微分ｄＸout(t)と前回の出力値の時間微分の平滑化値ｄＸs(t-1)とに基づいて、下記（５）式に示される時定数が固定された時定数フィルタで重み付け加算して今回の出力値の時間微分の平滑化値ｄＸs(t)を算出するようになっている。
ｄＸs(t)＝ｄＸs(t-1)×０．９５＋ｄＸout(t)×０．０５ …（５）
【００８２】
時間微分平滑化値算出手段４は、今回の出力値Ｘout(t)と算出した今回の出力値の時間微分の平滑化値ｄＸs(t)とを、次回の加算割合決定手段２における演算処理のために「前回の出力値Ｘout(t-1)」および「前回の出力値の時間微分の平滑化値ｄＸs(t-1)」としてそれぞれメモリ５に上書き保存するようになっている（ステップＳ９）。
【００８３】
なお、上記の本実施形態の平滑化フィルタ装置１の構成から分かるように、平滑化フィルタ装置１の起動時などには、メモリ５に前回の出力値Ｘout(t-1)や前回の出力値の時間微分の平滑化値ｄＸs(t-1)が保存されていなかったり、前回の出力値の時間微分の平滑化値ｄＸs(t-1)が安定しない場合がある。
【００８４】
そのため、本実施形態では、加算割合決定手段２は、入力される物理量Ｘのデータの入力値Ｘin(t)の数が予め設定された所定回数分に満たない間は、加算割合Ｐを例えば０．１等の予め設定された一定値に決定するようになっている。
【００８５】
次に、本実施形態に係る平滑化フィルタ装置１の作用について説明する。
【００８６】
本実施形態に係る平滑化フィルタ装置１では、平滑化手段３で、前記（３）式に示したように時定数フィルタの手法を用いて物理量Ｘのデータの平滑化が行われるが（ステップＳ６）、その際に用いられるフィルタの時定数すなわち加算割合Ｐを加算割合決定手段２で物理量Ｘのデータの時間微分に基づいて可変させる（ステップＳ２〜Ｓ５）。また、時間微分平滑化値算出手段４では次回の演算処理のために出力値の時間微分の平滑化値ｄＸs(t)を算出する（ステップＳ７〜Ｓ９）。
【００８７】
前述したように、移動平均フィルタや時定数フィルタ等の従来の平滑化フィルタでは、図２４に示したようにデータの暴れの振幅の中心付近を通る理想的な平滑化曲線Ａに対して時間遅れΔＴをもって物理量Ｘのデータが平滑化される。そのような時間遅れを解消し、減少傾向や増加傾向を示しているデータを図２４の平滑化曲線Ａのように理想的に平滑化するためには、(i)データの入力値Ｘin(t)が減少傾向にあるのか、増加傾向にあるのか、或いはあまり変化しない傾向にあるのか等の入力値Ｘin(t)の変化傾向を的確に捉え、(ii)そのデータの変化傾向を平滑化された出力値Ｘout(t)に的確に反映させることが必要となる。
【００８８】
本実施形態に係る平滑化フィルタ装置１では、まず、(i)の入力値Ｘin(t)の変化傾向を、前述した時間微分平滑化値算出手段４におけるステップＳ７、Ｓ８の演算処理で装置１から出力された出力値Ｘout(t)の時間微分ｄＸout(t)を算出し、それをさらに平滑化して平滑化値ｄＸs(t-1)を算出して追跡する。
【００８９】
入力値Ｘin(t)の変化傾向を出力値Ｘout(t)の時間微分で判定するのは一見矛盾するようだが、図２３に示したように入力値Ｘin(t)が時系列的な暴れを有する場合、その時間微分ｄＸin(t)も図３、図４に示すようにサンプリングタイミングｔごとにその値が正になったり負になったり時系列的に暴れるため、入力値Ｘin(t)の時間微分ｄＸin(t)を入力値Ｘin(t)の変化傾向の判定にそのまま使用できない。
【００９０】
そこで、本実施形態では、入力値Ｘin(t)の値を平滑化した出力値Ｘout(t)を用い、その時間微分ｄＸout(t)の平滑化値ｄＸs(t)から入力値Ｘin(t)の全体的な変化傾向を判定することとしている。
【００９１】
次に、(ii)の前記のようにして判定した入力値Ｘin(t)の変化傾向を出力値Ｘout(t)に的確に反映させる手法として、本実施形態に係る平滑化フィルタ装置１では、前記入力値の変化傾向すなわちそれを表す出力値の時間微分の平滑化値ｄＸs(t)を加味して前記（３）式の加算割合Ｐの値を可変させる手法が用いられている。
【００９２】
具体的には、(i)で述べた入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)を所定の係数Ｋで増幅し、それと現在の入力値Ｘin(t)の時間的変化を表す時間微分ｄＸin(t)との差分Ｄ(t)を算出し、その差分の絶対値|Ｄ(t)|に応じて加算割合Ｐを決定する。
【００９３】
本実施形態では、特に、図４に示したように平滑化値ｄＸs(t)のＫ倍が入力値の時間微分ｄＸin(t)の暴れの振幅の下端に沿う状態となるように係数Ｋの値が設定されており、|Ｄ(t)|が小さいほど加算割合Ｐが大きくなり、|Ｄ(t)|が大きいほど加算割合Ｐが小さくなるように決定される。
【００９４】
つまり、例えば図４の領域Ｒ２のように、入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)が負の値となり入力値Ｘin(t)が全体的に減少傾向を示している場合、現在の入力値の時間微分ｄＸin(t)がｄＸs(t)×Ｋに近く、入力値の時間微分ｄＸin(t)の暴れの振幅の下端に近く大きく負側に振れているときに入力値Ｘin(t)の加算割合Ｐを大きくして出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくする。
【００９５】
入力値の時間微分ｄＸin(t)が負の値であれば今回の入力値Ｘin(t)は前回の出力値Ｘout(t-1)よりも小さい場合が多いから、この入力値Ｘin(t) の加算割合Ｐを大きくして出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくすることで、従来のように加算割合Ｐを固定した場合よりも大きく今回の出力値Ｘout(t)を前回の出力値Ｘout(t-1)に対して減少させることができる。
【００９６】
つまり、本実施形態では、入力値Ｘin(t)の全体的な変化傾向を表す平滑化値ｄＸs(t)が負の値となり入力値Ｘin(t)が全体的に減少傾向を示している場合に、入力値Ｘin(t) の加算割合Ｐをより増大させて出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度を大きくすることで、出力値Ｘout(t)の減少傾向がより強調されるようになる。そのため、図６に示すように、出力値Ｘout(t)が、加算割合Ｐを固定した場合に平滑化された出力値Ｂよりも減少側に押し下げられて、理想的な平滑化曲線Ａを描くようになる。
【００９７】
一方、例えば図４の領域Ｒ１のように、入力値Ｘin(t)が全体的にあまり変化しない傾向を示している場合、出力値の時間微分の平滑化値ｄＸs(t)は０に近い値であり、ｄＸs(t)×Ｋも０に近い値となる。そのため、現在の入力値の時間微分ｄＸin(t)が０に近いほど差分の絶対値|Ｄ(t)|が小さくなって加算割合Ｐが大きくなる。つまり、前回の出力値Ｘout(t-1)とそれほど値が変わらない今回の入力値Ｘin(t)の出力値Ｘout(t)に対する寄与度が大きくなる。
【００９８】
また、入力値の時間微分ｄＸin(t)が正の方向や負の方向に大きく出ても|Ｄ(t)|が大きく出力値Ｘout(t)に対する入力値Ｘin(t)の寄与度は小さく、その出現頻度も正負ともに同程度であるから、時間微分ｄＸin(t)が正の方向や負の方向に大きく出るような入力値Ｘin(t)の出力値Ｘout(t)に対する影響は小さいものとなる。
【００９９】
そのため、図４の領域Ｒ１のように入力値Ｘin(t)が全体的にあまり変化しない傾向を示している場合には、入力値Ｘin(t)が時系列的に暴れても、出力値Ｘout(t)は暴れが小さく全体的にはあまり変化しない値を取り続けるようになる。
【０１００】
他方、例えば図４の領域Ｒ３のように、入力値Ｘin(t)の全体的な変化傾向が減少傾向からあまり変化しない傾向に変わるような場合、時間微分ｄＸin(t)が正の方向に大きく出る入力値Ｘin(t)の加算割合Ｐの値自体はそれほど大きくない。
【０１０１】
しかし、減少傾向を示していた領域Ｒ２に比べて、前回の出力値Ｘout(t-1)よりも大きな値の入力値Ｘin(t)が入力される頻度が増大するため、前記（３）式に従って次第に出力値Ｘout(t)も減少傾向からあまり変化しない傾向に変わっていき、出力値の時間微分の平滑化値ｄＸs(t)も０に近づいていく。
【０１０２】
このようにして、本実施形態に係る平滑化フィルタ装置１は、入力値Ｘin(t)があまり変化しない傾向にあるときは平滑化された出力値Ｘout(t)もあまり変化しない傾向を示し、入力値Ｘin(t)が減少傾向にあるときは平滑化された出力値Ｘout(t)も減少傾向を示し、しかも入力値Ｘin(t)の減少傾向に理想的に追従して減少する平滑化された出力値Ｘout(t)を出力することが可能となる。
【０１０３】
なお、上記の例では、物理量Ｘのデータの入力値Ｘin(t)の全体的な変化傾向が図２３に示すように減少傾向にある場合について述べたが、例えば、図７に示すように物理量Ｘのデータの入力値Ｘin(t)の全体的な変化傾向が増加傾向にある場合も何ら前記構成を変更することなく同様に出力値Ｘout(t)の理想的な平滑化を行うことができる。
【０１０４】
すなわち、図７に示すように入力値Ｘin(t)の全体的な変化傾向が増加傾向にある場合には、図８に示すように出力値の時間微分の平滑化値ｄＸs(t)も正の値をとるようになる。その際、本実施形態のように、入力値Ｘin(t)が全体的に減少傾向を示し平滑化値ｄＸs(t)が負の値をとるときに図４に示したようにｄＸs(t)×Ｋが入力値の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態になるように係数Ｋの値が設定されていれば、平滑化値ｄＸs(t)も正の値をとる場合には、図８に示すように、ｄＸs(t)×Ｋは入力値の時間微分ｄＸin(t)の暴れの振幅の上端にほぼ沿う状態となる。
【０１０５】
そして、前記と同様に差分Ｄ(t)を算出し、その絶対値|Ｄ(t)|と加算割合Ｐとの関係を設定すれば、入力値Ｘin(t)が全体的に増加傾向にある場合には、図９に示すように、出力値Ｘout(t)が、加算割合Ｐを固定した場合に平滑化された出力値Ｂよりも増加側に押し上げられて、理想的な平滑化曲線Ａを描くようになる。また、入力値Ｘin(t)があまり変化しない傾向にある場合や、増加傾向にあった入力値Ｘin(t)があまり変化しない傾向に変化する場合も前記減少傾向の場合と同様に説明され、良好に入力値Ｘin(t)を平滑化することができる。
【０１０６】
以上のように、本実施形態に係る平滑化フィルタ装置１によれば、出力値の時間微分ｄＸout(t)を平滑化した平滑化値ｄＸs(t)を算出し、それに基づいて物理量Ｘのデータの入力値Ｘin(t)の変化傾向を的確に捉えることが可能となる。また、それと同時に、例えば平滑化値ｄＸs(t)を所定の係数Ｋで増幅した値と入力値の時間微分ｄＸin(t)との差分に基づいて加算割合Ｐを決定するなどして入力値の時間微分ｄＸin(t)と平滑化値ｄＸs(t)に基づいて出力値Ｘout(t)の算出に用いる加算割合Ｐを決定することで、データの入力値Ｘin(t)の変化傾向を平滑化される出力値Ｘout(t)に的確に反映させることが可能となる。
【０１０７】
そのため、平滑化される出力値Ｘout(t)を入力値Ｘin(t)の変化傾向にあわせて的確に増減させることが可能となり、従来の移動平均フィルタや時定数フィルタ等の平滑化フィルタのように時間遅れを発生させることなく、図２４に示した理想的な平滑化曲線Ａのように、暴れを有する時系列データをデータの暴れの振幅の中心付近の値に的確に平滑化することが可能となる。
【０１０８】
また、前記所定の係数Ｋを適切に設定したり、差分の絶対値|Ｄ(t)|と加算割合Ｐとの関係を適切に設定すれば、従来のカルマンフィルタのようにデータの増減を必要以上に強調させて出力値Ｘout(t)が入力値Ｘin(t)の変化傾向に的確には符合しない平滑化を行うことを防止し、理想的な平滑化曲線Ａを出力することが可能な平滑化フィルタ装置とすることが可能となる。
【０１０９】
さらに、移動平均フィルタでは、平均をとるための所定のサンプリング回数分の過去の入力値Ｘin(t-1)〜Ｘin(t-n)をメモリ５に保存しておく必要があるが、本実施形態では、メモリ５に保存しておくのは前回の出力値Ｘout(t-1)と平滑化値ｄＸs(t-1)のみであり、メモリ５の使用効率の低減させることができるという副次的な効果もある。
【０１１０】
なお、加算割合Ｐの決定手法、すなわち所定の係数Ｋの設定手法や、その係数Ｋに基づいて算出される差分Ｄ(t)と加算割合Ｐとの関係の設定手法は、本実施形態の手法に限定されない。
【０１１１】
前述の説明から分かるように、極端に言えば、物理量Ｘのデータの入力値Ｘin(t)の変化傾向が増加傾向であるか、減少傾向であるか、或いはあまり変化しない傾向であるかさえ分かれば、それに応じて加算割合Ｐの可変のさせ方を変えて的確な平滑化を行うことが可能となる。
【０１１２】
そのため、例えば、加算割合決定手段２を、時間微分平滑化値算出手段４が前回算出した出力値の時間微分の平滑化値ｄＸs(t-1)に基づいて入力値Ｘin(t)の変化傾向を分類して判定し、例えば図１０に示すようなテーブルに基づいて加算割合Ｐの可変のさせ方を変更して加算割合Ｐを決定するように構成することも可能である。
【０１１３】
また、本実施形態では、入力された物理量Ｘのデータ自体を平滑化する平滑化フィルタ装置１について説明した。この平滑化フィルタ装置１は、例えば自車両の速度を測定する測定手段や平滑化フィルタ装置を搭載した装置と他の物体との相対速度を測定する測定手段から入力された速度や相対速度のデータＸを平滑化して、平滑化された速度や相対速度を出力値Ｘout(t)として出力する。
【０１１４】
しかし、例えば、ある移動可能な装置の移動距離を測定する測定手段から距離Ｙのデータを入力し、入力された距離Ｙのデータの入力値Ｙin(t)を時間微分して速度Ｘin(t)を求め、算出した速度Ｘin(t)を平滑化し、平滑化された速度を出力値Ｘout(t)として出力するような平滑化フィルタ装置として構成することも可能である。
【０１１５】
また、例えば、ある装置と他の物体との距離を測定する測定手段から距離Ｙのデータを入力し、入力された距離Ｙのデータの入力値Ｙin(t)を時間微分して相対速度Ｘin(t)を求め、算出した相対速度Ｘin(t)を平滑化し、平滑化された相対速度を出力値Ｘout(t)として出力するような平滑化フィルタ装置として構成することも可能である。
【０１１６】
つまり、平滑化フィルタ装置を、平滑化して出力する物理量Ｘのデータとは別の物理量Ｙのデータから平滑化の対象とする物理量Ｘのデータを形成して、その物理量Ｘのデータを平滑化するように構成することも可能である。例えば、別の物理量Ｙのデータが入力されると、そのデータＹを時間微分して物理量Ｙの変化速度である物理量のデータＸを算出し、物理量Ｙの変化速度である物理量Ｘのデータを平滑化して出力するように構成することも可能である。
【０１１７】
この場合、図２にフローチャートは図１１に示すフローチャートのように修正される。すなわち、メモリ５には、前回の出力値Ｘout(t-1)や出力値の時間微分の平滑化値ｄＸs(t-1)のほか、前回の入力値Ｙin(t-1)も保存されており、平滑化フィルタ装置の加算割合決定手段２は、ステップ１に代わるステップＳ１０で今回のサンプリングタイミングｔでの物理量Ｙのデータの入力値Ｙin(t)が入力されると（ステップＳ１０：ＹＥＳ）、メモリ５から前回の入力値Ｙin(t-1)を読み出す。
【０１１８】
そして、下記（６）式に従って入力値Ｙin(t)を時間微分して物理量Ｙのデータの変化速度ｄＹin(t)を算出し、その変化速度ｄＹin(t)をそれ以降の処理の対象となる物理量Ｘin(t)とする（ステップＳ１１）。なお、Δｔはサンプリング周期を表す。
Ｘin(t)＝ｄＹin(t)＝（Ｙin(t)−Ｙin(t-1)）／Δｔ …（６）
【０１１９】
Ｘin(t)は、前述した演算処理により加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４で演算処理されて平滑化され（ステップＳ２〜Ｓ９）、平滑化された出力値Ｘout(t)が出力される。
【０１２０】
一方、加算割合決定手段２は、今回入力された物理量Ｙのデータの入力値Ｙin(t)を「前回の入力値Ｙin(t-1)」としてメモリ５に上書き保存し（ステップＳ１２）、次回の入力値の時間微分ｄＹin(t)すなわち物理量Ｘin(t)の算出処理に用いる。
【０１２１】
このように、物理量ＸのデータＸin(t)が他の物理量Ｙを時間微分して算出される他の物理量Ｙの変化速度のデータＹin(t)である場合でも、基本的に、前述した実施形態に他の物理量Ｙを時間微分し、算出した変化速度Ｙin(t)を処理の対象となる物理量Ｘin(t)とするという演算処理が加わるだけで、加算割合決定手段２や平滑化手段３、時間微分平滑化値算出手段４における他の演算処理は上記とまったく同様に行われるから、前述した実施形態の効果は本変形例においてもまったく同様に発揮される。
【０１２２】
なお、上記の例では、物理量ＸのデータＸin(t)が入力されるごとに出力値Ｘout(t)を算出する構成について述べたが、物理量ＸのデータＸin(t)が入力されるサンプリング周期に対して例えば２サンプリング周期ごとに出力値Ｘout(t)を算出するように構成してもよい。
【０１２３】
［第２の実施の形態］
本実施形態では、前記第１の実施形態に係る平滑化フィルタ装置を用いた速度検出装置について説明する。
【０１２４】
本実施形態では、前述した第１の実施形態の変形例を用いて、後述する測定手段により測定された自車両と先行車両との距離を時間微分して自車両と先行車両との相対速度を算出し、平滑化フィルタ装置でその相対速度を平滑化して検出し出力する速度検出装置について述べる。
【０１２５】
本実施形態に係る速度検出装置１０は、図１２に示すように、主に撮像手段１１や変換手段１２、画像処理手段１５等で構成される測定手段１８と、平滑化フィルタ装置２２を含む検出手段１９とで構成されている。
【０１２６】
なお、撮像手段１１から検出手段１９の立体物検出手段２０までの構成は本願出願人により先に提出された特開平５−１１４０９９号公報、特開平５−２６５５４７号公報、特開平６−２６６８２８号公報、特開平１０−２８３４６１号公報、特開平１０−２８３４７７号公報、特開２００６−７２４９５号公報等に詳述されており、詳細な説明はそれらの公報に委ねる。以下、簡単に説明する。
【０１２７】
撮像手段１１は、本実施形態では、互いに同期が取られたＣＣＤやＣＭＯＳセンサ等のイメージセンサがそれぞれ内蔵され例えばルームミラー近傍に車幅方向に所定の間隔をあけて取り付けられた一対のメインカメラ１１ａおよびサブカメラ１１ｂからなるステレオカメラであり、所定のサンプリング周期で車両前方の道路を含む風景を撮像して一対の画像を出力するように構成されている。一対のカメラのうちメインカメラ１１ａは、運転者に近い側のカメラであり、図１３に示すような基準画像Ｔを撮像するようになっている。なお、サブカメラ１１ｂで撮像された画像を比較画像という。
【０１２８】
メインカメラ１１ａとサブカメラ１１ｂから出力された画像データは、変換手段１２であるＡ／Ｄコンバータ１２ａ、１２ｂでアナログ画像から画素ごとに輝度値を有するデジタル画像にそれぞれ変換され、画像補正部１３で、ずれやノイズの除去等の画像補正が行われ、画像データメモリ１４に格納されると同時に検出手段１９に送信されるようになっている。
【０１２９】
画像処理手段１５のイメージプロセッサ１６では、基準画像Ｔと比較画像の各画像データにステレオマッチング処理やフィルタリング処理を施して実空間上の距離に対応する視差ｄｐを算出するようになっている。以下、視差ｄｐが割り当てられた画像を距離画像という。このようにして算出された視差ｄｐの情報すなわち距離画像は、画像処理手段１５の距離データメモリ１７に格納するようになっている。
【０１３０】
なお、視差ｄｐ、距離画像上の点（ｉ，ｊ）と、前記一対のカメラ１１ａ、１１ｂの中央真下の道路面上の点を原点とし、自車両の車幅方向をＸ軸方向、車高方向をＹ軸方向、車長方向をＺ軸方向とした場合の実空間上の点（Ｘ，Ｙ，Ｚ）とは、下記（７）〜（９）式で表される座標変換により一意に対応づけられる。なお、下記各式において、ＣＤは一対のカメラの間隔、ＰＷは１画素当たりの視野角、ＣＨは一対のカメラの取り付け高さ、ＩＶおよびＪＶは自車両正面の無限遠点の距離画像上のｉ座標およびｊ座標、ＤＰは消失点視差を表す。
Ｘ＝ＣＤ／２＋Ｚ×ＰＷ×（ｉ−ＩＶ） …（７）
Ｙ＝ＣＨ＋Ｚ×ＰＷ×（ｊ−ＪＶ） …（８）
Ｚ＝ＣＤ／（ＰＷ×（ｄｐ−ＤＰ）） …（９）
【０１３１】
本実施形態では、以上の撮像手段１１からイメージプロセッサ１６や距離データメモリ１７を含む画像処理手段１５までで、自車両前方の所定領域内に存在する立体物までの自車両からの距離Ｚすなわち前記（９）式により距離Ｚと一意に対応づけられる視差ｄｐを測定する測定手段１８が構成されている。
【０１３２】
なお、本実施形態では、測定手段１８は自車両前方に存在する立体物までの距離Ｚを測定できるものであればよく、本実施形態の他にも、例えば自車両前方にレーザ光や赤外線等を照射してその反射光の情報に基づいて立体物までの距離Ｚを測定するレーダ装置等で構成することも可能であり、検出の手法は特定の手法に限定されない。
【０１３３】
検出手段１９は、図示しないＣＰＵやＲＯＭ、ＲＡＭ、入出力インターフェース等がバスに接続されたマイクロコンピュータより構成されている。また、検出手段１９には、車速センサやヨーレートセンサ、ステアリングホイールの操舵角を測定する操舵角センサ等のセンサ類Ｑが接続されている。なお、ヨーレートセンサの代わりに自車両の車速等からヨーレートを推定する装置等を用いることも可能である。
【０１３４】
検出手段１９は、図１２に示すように、立体物検出手段２０と、先行車両検出手段２１と、前述した平滑化フィルタ装置２２とを備えており、さらに図示しないメモリを備えている。また、検出手段１９の各手段には、センサ類Ｑから必要なデータが入力されるようになっている。
【０１３５】
立体物検出手段２０は、本実施形態では、前述したように特開平１０−２８３４６１号公報等に記載された車外監視装置等をベースに構成されている。詳細な説明はそれらの公報に委ねる。以下、簡単にその構成について説明する。
【０１３６】
立体物検出手段２０は、距離データメモリ１７から前述した距離画像を読み出して、距離画像を所定の画素幅で垂直方向に延びる短冊状の区分に分割する。そして、短冊状の各区分に属する各視差ｄｐを前記（９）式に従ってそれぞれ距離Ｚに変換し、各距離Ｚのうち道路面より上方に存在すると位置付けられる距離に関してヒストグラムを作成して、度数が最大の区間までの距離をその短冊状の区分における立体物までの距離とする。これを全区分について行うようになっている。以下、各区分を代表する距離を距離Ｚという。
【０１３７】
例えば、図１３に示した基準画像Ｔから作成された距離画像に対して前記距離Ｚの算出を行い、算出された各区分ごとの距離Ｚを実空間上にプロットすると、図１４に示すように自車両前方の立体物の自車両ＭＣに面した部分に対応する部分に多少バラツキを持って各点としてプロットされる。
【０１３８】
立体物検出手段２０は、このようにプロットされる各点を図１５に示すように近接する各点の距離や方向性に基づいて互いに隣接する各点をそれぞれグループＧ１〜Ｇ７にグループ化し、図１６に示すようにそれぞれのグループ内の各点が自車両ＭＣの車幅方向すなわちＸ軸方向に略平行に並ぶサブグループには“物体”Ｏ１〜Ｏ３とラベルし、各点が自車両ＭＣの進行方向すなわちＺ軸方向に略平行な並ぶサブグループには“側壁”Ｓ１〜Ｓ４とラベルして分類する。また、同一の立体物の“物体”と“側壁”の交点をコーナー点Ｃとしてラベルするようになっている。
【０１３９】
立体物検出手段２０は、このようにして、［物体Ｏ１、コーナー点Ｃ、側壁Ｓ１］、［側壁Ｓ２］、［物体Ｏ２］、［物体Ｏ３］、［側壁Ｓ３］、［側壁Ｓ４］をそれぞれ立体物として検出するようになっている。立体物検出手段２０は、このようにして検出した立体物の情報や各サブグループの端点の座標等をそれぞれメモリに保存するようになっている。
【０１４０】
先行車両検出手段２１は、自車両の進行路上や自車両が走行している走行レーン上に存在する立体物の中で、自車両に最も近接する立体物を先行車両として検出するようになっている。本実施形態では、先行車両検出手段２１は、自車両の進行路上に先行車両を検出するようになっていて、まず、センサ類Ｑである車速センサやヨーレートセンサ、舵角センサから入力される自車両の挙動すなわち自車両の車速Ｖやヨーレートγ、ステアリングホイールの舵角δ等に基づいて自車両の旋回曲率Ｃuaを算出して、図１７に示すように自車両ＭＣの走行軌跡Ｌestを算出するようになっている。
【０１４１】
旋回曲率Ｃuaは、例えば車速Ｖとヨーレートγとを用いて、
Ｃua＝γ／Ｖ …（１０）
に従って算出される。また、例えば車速Ｖと舵角δとを用いて、
Ｒｅ＝（１＋Ａsf・Ｖ２）・（Ｌwb／δ） …（１１）
Ｃua＝１／Ｒｅ …（１２）
に従って算出することもできる。ここで、Ｒｅは旋回半径、Ａsfは車両のスタビリティファクタ、Ｌwbはホイールベースである。
【０１４２】
先行車両検出手段２１は、このようにして算出した走行軌跡を中心とする自車両の車幅分の領域を自車両の進行路Ｒestとして把握し、その上に存在する立体物のうち自車両ＭＣに最も近い物体Ｏ２を検出するようになっている。
【０１４３】
先行車両検出手段２１は、さらに、前回検出された先行車両の情報をメモリから読み出し、前回検出した先行車両と今回検出した自車両に最も近接する立体物すなわち物体Ｏ２との位置関係や移動速度等に基づいて前回の先行車両と今回検出した立体物とが同一の立体物である確率をそれぞれ算出し、算出された確率が予め設定された閾値以上であれば今回検出した立体物を先行車両とラベル付けして先行車両を検出し、先行車両の情報を今回検出した立体物の情報で更新してメモリに継続登録することで、先行車両の情報を更新しながらそれを追跡するようになっている。
【０１４４】
なお、前述したように、先行車両検出手段２１を、自車両ＭＣが走行している走行レーン上に存在し自車両ＭＣに最も近接する立体物として先行車両を検出するように構成することも可能である。自車両ＭＣの走行レーンを検出するために必要な自車両ＭＣの左右の区画線等の車線の位置を検出するための装置としては、例えば本願出願人により先に提出された特開２００１−９２９７０号公報に記載の車線認識装置等を用いることができる。
【０１４５】
平滑化フィルタ装置２２には、先行車両検出手段２１が検出した先行車両すなわちこの場合は物体Ｏ２と自車両ＭＣとの距離Ｚが入力されるようになっている。この場合の物体Ｏ２の距離Ｚとしては、例えば車幅方向すなわちＸ軸方向に延在する物体Ｏ２の中心位置の自車両ＭＣからのＺ軸方向の距離Ｚが入力される。
【０１４６】
平滑化フィルタ装置２２は、前記第１の実施形態において詳述した演算処理を行い、算出した自車両ＭＣと先行車両Ｏ２との平滑化された相対速度を、最終的に速度検出装置１０により検出された相対速度として装置外に出力するようになっている。具体的には、図１１に示したフローチャートに従って加算割合決定手段２、平滑化手段３および時間微分平滑化値算出手段４で演算処理を行い、出力値Ｘout(t)である自車両ＭＣと先行車両Ｏ２との相対速度を出力するようになっている。
【０１４７】
なお、この場合、物体Ｏ２と自車両ＭＣとの距離Ｚが第１の実施形態における他の物理量Ｙin(t)に相当し、距離Ｚの時間微分である相対速度ΔＶが他の物理量Ｙin(t)の時間微分ｄＹin(t)すなわちＸin(t)に相当し、出力される平滑化された相対速度ΔＶが出力値Ｘout(t)に相当する。また、本実施形態では、出力値Ｘout(t)の算出に用いられる入力値の時間微分ｄＸin(t)や出力値の時間微分ｄＸout(t)、出力値の時間微分の平滑化値ｄＸs(t)はそれぞれ加速度［ｍ／ｓ２］のディメンジョンを有する。
【０１４８】
次に、本実施形態に係る速度検出装置１０の作用について説明する。
【０１４９】
上記のような本実施形態に係る速度検出装置１０の構成では、例えばＣＣＤカメラ等の撮像手段１１の解像度等の影響で、自車両と先行車両との距離Ｚが実際には連続的に滑らかに変化しているにもかかわらず、あるサンプリングタイミングｔ−１で検出された距離Ｚ＝Ｙin(t-1)と次のサンプリングタイミングｔで検出された距離Ｚ＝Ｙin(t)とが比較的大きく変化したように検出されることが多い。このような場合、その時間微分Ｘin(t)＝ｄＹin(t)である相対速度ΔＶは比較的大きく変化して、相対速度ΔＶのデータが時系列的に暴れを有するデータとなる。
【０１５０】
実際に、本実施形態の構成を備えた速度検出装置１０により自車両と先行車両との距離Ｚ＝Ｙin(t)を測定すると、図１８に示すように比較的滑らかに推移するデータが得られるが、その相対速度ΔＶである時間微分ｄＹin(t)＝Ｘin(t)を前記（６）式に従って算出すると、図１９に示すような比較的暴れの大きい時系列データＸin(t)が得られる。
【０１５１】
さらに、平滑化フィルタ装置２２の加算割合決定手段２で、自車両と先行車両との相対加速度に相当するＸin(t)の時間微分ｄＸin(t)を前記（１）式に従って算出すると、図２０に示すような暴れの大きい時系列データｄＸin(t)が得られる。また、算出される出力値の時間微分の平滑化値ｄＸs(t)のＫ倍は同図に示すようにＸin(t)の時間微分ｄＸin(t)の暴れの振幅の下端にほぼ沿う状態で推移する。
【０１５２】
平滑化フィルタ装置２２の加算割合決定手段２では、図２１に示すようにこのｄＸin(t)とｄＸs(t-1)×Ｋとの差分Ｄ(t)の絶対値に応じて激しく増減する加算割合Ｐが決定される。そして、このように各サンプリングタイミングｔごとに変動する加算割合Ｐすなわち時定数に基づいて、平滑化フィルタ装置２２の平滑化手段３で前記（３）式で示される時定数フィルタによって相対速度ΔＶに相当し暴れを有する時系列データであるＸin(t)が平滑化され、図２２に示すようなデータの暴れの振幅の中心付近の値に平滑化された出力値Ｘout(t)が出力される。
【０１５３】
なお、図２２において、Ｘcom(t)は、比較例として、前記（３）式の時定数フィルタにおける加算割合Ｐを０．１に固定した従来の時定数フィルタによる演算結果を表したものである。
【０１５４】
以上のように、本実施形態に係る速度検出装置１０によれば、平滑化フィルタ装置２２が前記第１の実施形態における効果を的確に発揮して、自車両と先行車両との距離Ｚから得られた比較的暴れの大きい相対速度ΔＶ＝Ｘin(t)の時系列データを有効に平滑化し、しかも、図２２に示したように、データの暴れの振幅の中心付近の値に的確に平滑化された相対速度ΔＶ＝Ｘout(t)を出力することが可能となる。
【０１５５】
そのため、この速度検出装置１０の出力値Ｘout(t)を例えば先行車両追従装置に適用すれば、先行車両と自車両との適切な相対速度ΔＶに基づいて適切な先行車両追従制御を行うことが可能となり、例えば図２２に示した従来の時定数フィルタの出力値Ｘcom(t)に基づいて制御を行った場合に必要以上の制動が加わってドライバに違和感を与えたりドライバがそれに反発して車間をつめるために不必要な加速を強いることを有効に防止することが可能となる。
【０１５６】
なお、上記の第２の実施形態では、平滑化された自車両と先行車両との相対速度ΔＶを出力するにあたって、自車両と先行車両との距離Ｚから相対速度ΔＶを求める場合について説明した。しかし、本発明はこの場合に限定されず、例えばドップラーレーダ等の測定手段により自車両と先行車両との相対速度ΔＶを直接測定し、測定された相対速度ΔＶを平滑化フィルタ装置２２により平滑化して検出し出力する場合についても適用される。
【０１５７】
その場合、速度検出装置の平滑化フィルタ装置２２における演算処理は、図２に示したフローチャートに従って行われる。そして、図２のフローチャートにおいて入力値Ｘin(t)は自車両と先行車両との相対速度ΔＶであり、出力値Ｘout(t)は平滑化された自車両と先行車両との相対速度ΔＶとなる。
【０１５８】
また、本発明は、前述したような自車両と先行車両との相対速度ΔＶの平滑化に限定されず、例えば自車両の車速Ｖの平滑化に適用することも可能である。自車両の車速Ｖの測定手段としては、例えば本願出願人により先に提出された特開２００４−３１７２０６号公報に記載された自車両の車輪の回転を検出して車速Ｖを算出するパルス検出センサを用いることができる。
【０１５９】
さらに、上記の第２の実施形態では、速度検出装置を自動車やトラック等の車両に搭載する場合について説明したが、本実施形態に係る速度検出装置は、この他にも、例えば鉄道車両や航空機、船舶等の運輸手段や、ロボット、各種機械・機器等のように自ら移動し或いは他の物体等を移動させることで物体の速度を観念でき、その速度を測定できるものであれば、いずれにも適用可能である。
【０１６０】
ところで、上記の第２の実施形態のように自車両と先行車両との距離Ｚから相対速度ΔＶを求める構成とすると、自車両と先行車両との距離Ｚが大きいほど距離画像の１画素あたりの距離差が大きくなり検出された距離Ｚの検出誤差が大きくなる傾向がある。そのため、図１８、図１９に示したように、「他の物理量」である距離Ｚが大きい場合ほど、その時間微分Ｘin(t)であり平滑化の対象となる「物理量のデータ」すなわち自車両と先行車両との相対速度ΔＶは暴れが大きくなり、距離Ｚが小さいほど相対速度ΔＶの暴れが小さくなるという特徴的な傾向を示す。
【０１６１】
その結果、図２０に示したように、相対加速度を表すｄＸin(t)も距離Ｚが小さくなるほど暴れが小さくなり、同図の場合のように係数Ｋを一定とした場合には、距離Ｚが小さくなり相対加速度ｄＸin(t)の暴れの振幅が減少し始めたサンプリングタイミングｔでは、相対加速度ｄＸin(t)の振幅の下端に対してｄＸs(t)×Ｋが若干離れている。
【０１６２】
このように、「他の物理量」である距離Ｚが小さいほど相対加速度ｄＸin(t)の振幅も小さくなる場合には、それに適合させるため、前記所定の係数Ｋを他の物理量の値が小さいほど小さい値となるように可変させることが好ましい。適合の手法としては、上記のように距離Ｚを所定の範囲ごとに区分して区分ごとに係数Ｋの値が設定されたテーブルを用意したり、係数Ｋの値を距離Ｚの関数として算出するように構成することが可能である。
【０１６３】
また、図２０に示したように、距離Ｚが大きい場合に相対加速度ｄＸin(t)が比較的大きく暴れるため、加算割合Ｐを決定するための基準となる出力値の時間微分の平滑化値ｄＸs(t)が、距離Ｚが大きい場合には距離Ｚが小さい場合と比較して比較的大きな暴れを有している場合がある。
【０１６４】
そのため、本実施形態では前記（５）式に示したように出力値の時間微分の平滑化値ｄＸs(t)の算出における時定数を一定としたが、時定数を可変とし、自車両と先行車両との距離Ｚが大きいほど前記（５）式における右辺第１項すなわちｄＸs(t-1)の係数をさらに大きくするように修正し、平滑化値ｄＸs(t)の暴れを少なくするようにして平滑化値ｄＸs(t)を算出するように構成することが好ましい。
【０１６５】
また、別の観点として、自車両が高速道路を走行している場合には、通常の道路を走行している場合に比べて自車両と先行車両との相対速度の増減の程度がさほど大きくなく、相対加速度ｄＸin(t)が比較的安定して推移する傾向がある。このような場合には、前記と同様に、加算割合Ｐを決定するための基準となる出力値の時間微分の平滑化値ｄＸs(t)を算出するための前記（５）式において右辺第１項すなわちｄＸs(t-1)の係数をさらに大きくするように修正して、平滑化値ｄＸs(t)の暴れを少なくするように構成することが可能となる。
【０１６６】
そこで、速度検出装置１０に、自車両が高速道路を走行していることを検出する手段を備えておき、この手段により自車両が高速道路を走行していることが検出された場合には、前記（５）式においてｄＸs(t-1)の係数をさらに大きくするように修正して平滑化値ｄＸs(t)を算出するように構成することが好ましい。
【０１６７】
前記手段としては、例えば、ＧＰＳ（Global Positioning System）受信機等を搭載したナビゲーション装置からの情報に基づいて地図情報から検出したり、或いは自車両の車速が時速８０ｋｍ等の所定の速度以上である状態が所定時間以上継続したことを認識したり、或いは自車両の左右の車線位置の間隔を測定しその間隔が所定間隔以上であることを認識するなどして、自車両が高速道路を走行していることを検出するように構成することが可能である。
【図面の簡単な説明】
【０１６８】
【図１】第１の実施形態に係る平滑化フィルタ装置の構成を示すブロック図である。
【図２】平滑化フィルタ装置における演算処理の手順を示すフローチャートである。
【図３】算出された入力値の時間微分ｄＸin(t)を表すグラフである。
【図４】算出された出力値の時間微分の平滑化値ｄＸs(t)とそのＫ倍された値を表すグラフである。
【図５】差分の絶対値の各範囲に加算割合が割り当てられたテーブルの例を表す図である。
【図６】加算割合を固定した場合の出力値と理想的な平滑化曲線とを表すグラフである。
【図７】増加傾向にある物理量のデータの入力値を表すグラフである。
【図８】図７の入力値に基づく出力値の時間微分の平滑化値ｄＸs(t)とそのＫ倍された値を表すグラフである。
【図９】入力値が増加傾向にある場合の加算割合を固定した場合の出力値と理想的な平滑化曲線とを表すグラフである。
【図１０】入力値の変化傾向と加算割合とを対応させるテーブルの例を表す図である。
【図１１】他の物理量の時間微分を平滑化するための演算処理の手順を示すフローチャートである。
【図１２】第２の実施形態に係る速度検出装置の構成を示すブロック図である。
【図１３】基準画像の一例を示す図である。
【図１４】区分ごとの距離を実空間上にプロットした各点を表す図である。
【図１５】図１４の各点をグループ化した場合の各グループを表す図である。
【図１６】図１５の各グループに基づいて検出された物体や側壁を表す図である。
【図１７】自車両の走行軌跡と進行路を説明する図である。
【図１８】測定された距離の例を表すグラフである。
【図１９】図１８の距離に基づいて算出された相対速度を表すグラフである。
【図２０】図１９の相対速度に基づいて算出された相対加速度に相当するｄＸin(t)および出力値の時間微分の平滑化値ｄＸs(t)のＫ倍を表すグラフである。
【図２１】図２０に基づいて決定された加算割合を表すグラフである。
【図２２】図２１の加算割合に基づいて算出された出力値を表すグラフである。
【図２３】減少傾向にある相対速度の時系列データを表すグラフである。
【図２４】図２３のデータを理想的に平滑化した場合の平滑化曲線と従来の平滑化フィルタにより平滑化した場合の曲線を表すグラフである。
【符号の説明】
【０１６９】
１、２２ 平滑化フィルタ装置
２ 加算割合決定手段
３ 平滑化手段
４ 時間微分平滑化値算出手段
１０ 速度検出装置
１８ 測定手段
Ｄ(t) 差分
ｄＸin(t) 入力値の時間微分
ｄＸout(t) 出力値の時間微分
ｄＸs(t) 出力値の時間微分の平滑化値
Ｋ 係数
ＭＣ 自車両
Ｏ２ 先行車両
Ｐ 加算割合
Δｔ サンプリング周期
Ｖ 車速
ΔＶ 相対速度
Ｘ 物理量
Ｘin(t) 物理量のデータの入力値
Ｘout(t) 物理量のデータの出力値
Ｙ 他の物理量
Ｚ 距離
【特許請求の範囲】
【請求項１】
時系列的に離散的に入力される物理量のデータを平滑化して出力する平滑化フィルタ装置において、
加算割合を決定する加算割合決定手段と、
今回入力された前記物理量のデータの入力値と所定周期前の出力値とを前記加算割合により重み付け加算して算出した値を平滑化された前記物理量のデータの出力値として出力する平滑化手段と、
前記出力値の時間微分を算出して、前記出力値の時間微分の平滑化値を算出する時間微分平滑化値算出手段と
を備え、
前記加算割合決定手段は、前記入力値の時間微分を算出し、算出した前記入力値の時間微分と前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記加算割合を決定することを特徴とする平滑化フィルタ装置。
【請求項２】
前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記入力値の変化傾向を判定し、判定した前記入力値の変化傾向と前記入力値の時間微分との関係に基づいて前記加算割合を決定することを特徴とする請求項１に記載の平滑化フィルタ装置。
【請求項３】
前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値を所定の係数で増幅した値と前記入力値の時間微分との差分を算出し、その差分に基づいて前記加算割合を決定することを特徴とする請求項１に記載の平滑化フィルタ装置。
【請求項４】
前記所定の係数は、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅内に収まるように設定されることを特徴とする請求項３に記載の平滑化フィルタ装置。
【請求項５】
前記所定の係数は、正の値とされ、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅の端部付近の値となるように設定されることを特徴とする請求項３または請求項４に記載の平滑化フィルタ装置。
【請求項６】
前記加算割合決定手段は、前記入力値に重み付けする前記加算割合を、前記差分の絶対値が小さいほど大きい値となるように決定することを特徴とする請求項３から請求項５のいずれか一項に記載の平滑化フィルタ装置。
【請求項７】
前記加算割合決定手段は、複数の範囲に分割された前記差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備え、前記テーブルに基づいて前記加算割合を決定することを特徴とする請求項６に記載の平滑化フィルタ装置。
【請求項８】
前記加算割合決定手段は、前記差分の絶対値についての単調減少関数を備え、前記単調減少関数に基づいて前記加算割合を決定することを特徴とする請求項６に記載の平滑化フィルタ装置。
【請求項９】
前記加算割合決定手段は、前記入力値の入力回数が所定回数に満たない間は、前記加算割合を予め設定された一定値に決定することを特徴とする請求項１から請求項８のいずれか一項に記載の平滑化フィルタ装置。
【請求項１０】
前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して現在のサイクリング周期の前記出力値の時間微分の平滑化値を算出することを特徴とする請求項１から請求項９のいずれか一項に記載の平滑化フィルタ装置。
【請求項１１】
前記物理量のデータは、他の物理量を時間微分して算出される前記他の物理量の変化速度のデータであることを特徴とする請求項１から請求項１０のいずれか一項に記載の平滑化フィルタ装置。
【請求項１２】
前記物理量のデータは、他の物理量を時間微分して得られる前記他の物理量の変化速度のデータであり、
前記所定の係数は、前記他の物理量の値が小さいほど小さい値となるように可変させることを特徴とする請求項３から請求項１０のいずれか一項に記載の平滑化フィルタ装置。
【請求項１３】
請求項１から請求項１０のいずれか一項に記載の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記物理量のデータは前記物体の速度を測定する測定手段から入力される速度のデータであり、平滑化された前記速度を出力することを特徴とする速度検出装置。
【請求項１４】
請求項１１または請求項１２に記載の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記他の物理量は前記物体の距離を測定する測定手段から入力される距離であり、前記物理量のデータは前記距離を時間微分して得られる前記物体の速度のデータであり、平滑化された前記速度を出力することを特徴とする速度検出装置。
【請求項１５】
前記速度は、自車両の車速であることを特徴とする請求項１３または請求項１４に記載の速度検出装置。
【請求項１６】
前記速度は、自車両と先行車両との相対速度であることを特徴とする請求項１３または請求項１４に記載の速度検出装置。
【請求項１７】
前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する場合には、前記自車両と先行車両との距離が大きいほど前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする請求項１６に記載の速度検出装置。
【請求項１８】
自車両が高速道路を走行していることを検出する手段を備え、
前記時間微分平滑化値算出手段は、前記手段により自車両が高速道路を走行していることが検出された場合には、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する際に、前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする請求項１５から請求項１７のいずれか一項に記載の速度検出装置。
【請求項１】
時系列的に離散的に入力される物理量のデータを平滑化して出力する平滑化フィルタ装置において、
加算割合を決定する加算割合決定手段と、
今回入力された前記物理量のデータの入力値と所定周期前の出力値とを前記加算割合により重み付け加算して算出した値を平滑化された前記物理量のデータの出力値として出力する平滑化手段と、
前記出力値の時間微分を算出して、前記出力値の時間微分の平滑化値を算出する時間微分平滑化値算出手段と
を備え、
前記加算割合決定手段は、前記入力値の時間微分を算出し、算出した前記入力値の時間微分と前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記加算割合を決定することを特徴とする平滑化フィルタ装置。
【請求項２】
前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値に基づいて前記入力値の変化傾向を判定し、判定した前記入力値の変化傾向と前記入力値の時間微分との関係に基づいて前記加算割合を決定することを特徴とする請求項１に記載の平滑化フィルタ装置。
【請求項３】
前記加算割合決定手段は、前記時間微分平滑化値算出手段が算出した前記出力値の時間微分の平滑化値を所定の係数で増幅した値と前記入力値の時間微分との差分を算出し、その差分に基づいて前記加算割合を決定することを特徴とする請求項１に記載の平滑化フィルタ装置。
【請求項４】
前記所定の係数は、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅内に収まるように設定されることを特徴とする請求項３に記載の平滑化フィルタ装置。
【請求項５】
前記所定の係数は、正の値とされ、その所定の係数で前記出力値の時間微分の平滑化値を増幅した値が前記入力値の時間微分の振幅の端部付近の値となるように設定されることを特徴とする請求項３または請求項４に記載の平滑化フィルタ装置。
【請求項６】
前記加算割合決定手段は、前記入力値に重み付けする前記加算割合を、前記差分の絶対値が小さいほど大きい値となるように決定することを特徴とする請求項３から請求項５のいずれか一項に記載の平滑化フィルタ装置。
【請求項７】
前記加算割合決定手段は、複数の範囲に分割された前記差分の絶対値の範囲ごとに予め加算割合が割り当てられたテーブルを備え、前記テーブルに基づいて前記加算割合を決定することを特徴とする請求項６に記載の平滑化フィルタ装置。
【請求項８】
前記加算割合決定手段は、前記差分の絶対値についての単調減少関数を備え、前記単調減少関数に基づいて前記加算割合を決定することを特徴とする請求項６に記載の平滑化フィルタ装置。
【請求項９】
前記加算割合決定手段は、前記入力値の入力回数が所定回数に満たない間は、前記加算割合を予め設定された一定値に決定することを特徴とする請求項１から請求項８のいずれか一項に記載の平滑化フィルタ装置。
【請求項１０】
前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して現在のサイクリング周期の前記出力値の時間微分の平滑化値を算出することを特徴とする請求項１から請求項９のいずれか一項に記載の平滑化フィルタ装置。
【請求項１１】
前記物理量のデータは、他の物理量を時間微分して算出される前記他の物理量の変化速度のデータであることを特徴とする請求項１から請求項１０のいずれか一項に記載の平滑化フィルタ装置。
【請求項１２】
前記物理量のデータは、他の物理量を時間微分して得られる前記他の物理量の変化速度のデータであり、
前記所定の係数は、前記他の物理量の値が小さいほど小さい値となるように可変させることを特徴とする請求項３から請求項１０のいずれか一項に記載の平滑化フィルタ装置。
【請求項１３】
請求項１から請求項１０のいずれか一項に記載の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記物理量のデータは前記物体の速度を測定する測定手段から入力される速度のデータであり、平滑化された前記速度を出力することを特徴とする速度検出装置。
【請求項１４】
請求項１１または請求項１２に記載の平滑化フィルタ装置を用いて物体の速度を検出する速度検出装置であって、
前記他の物理量は前記物体の距離を測定する測定手段から入力される距離であり、前記物理量のデータは前記距離を時間微分して得られる前記物体の速度のデータであり、平滑化された前記速度を出力することを特徴とする速度検出装置。
【請求項１５】
前記速度は、自車両の車速であることを特徴とする請求項１３または請求項１４に記載の速度検出装置。
【請求項１６】
前記速度は、自車両と先行車両との相対速度であることを特徴とする請求項１３または請求項１４に記載の速度検出装置。
【請求項１７】
前記時間微分平滑化値算出手段は、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する場合には、前記自車両と先行車両との距離が大きいほど前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする請求項１６に記載の速度検出装置。
【請求項１８】
自車両が高速道路を走行していることを検出する手段を備え、
前記時間微分平滑化値算出手段は、前記手段により自車両が高速道路を走行していることが検出された場合には、前記出力値の時間微分とその所定周期前の前記出力値の時間微分の平滑化値とを所定の割合で重み付け加算して出力値の時間微分の平滑化値を算出する際に、前記所定周期前の前記出力値の時間微分の平滑化値に重み付けする前記所定の割合を大きくすることを特徴とする請求項１５から請求項１７のいずれか一項に記載の速度検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【公開番号】特開２００８−１２８９９８（Ｐ２００８−１２８９９８Ａ）
【公開日】平成２０年６月５日（２００８．６．５）
【国際特許分類】
【出願番号】特願２００６−３１８０４０（Ｐ２００６−３１８０４０）
【出願日】平成１８年１１月２７日（２００６．１１．２７）
【出願人】（０００００５３４８）富士重工業株式会社 (3,010)
【Ｆターム（参考）】
【公開日】平成２０年６月５日（２００８．６．５）
【国際特許分類】
【出願日】平成１８年１１月２７日（２００６．１１．２７）
【出願人】（０００００５３４８）富士重工業株式会社 (3,010)
【Ｆターム（参考）】
[ Back to top ]