JP2014115119A

JP2014115119A - 物体検出装置

Info

Publication number: JP2014115119A
Application number: JP2012267622A
Authority: JP
Inventors: Kiyomi Eimiya; 清美永宮; Hiroshi Sato; 洋佐藤
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2012-12-06
Filing date: 2012-12-06
Publication date: 2014-06-26

Abstract

【課題】ターゲットからの反射波が干渉しても、ターゲットの追尾精度を向上させることができる物体検出装置を提供する。
【解決手段】検出した物体の位置を追尾フィルタに入力して物体を追尾し車両周辺の物体を検出する物体検出装置であって、各物体の位置を検出する位置検出手段と、前記追尾フィルタが算出した任意の２つの物体の追尾位置と当該物体検出装置との距離の差を算出する距離差算出手段と、前記差が第１閾値以内の場合、２つの物体の前記追尾位置の履歴から求めた速度に基づき２つの物体が同一物体であるか否かを判定し、同一物体でないと判定された２つの物体の前記位置を補正して前記追尾フィルタに入力する位置補正手段と、を有することを特徴とする。
【選択図】図１

Description

本発明は車両の周辺の物体の位置を検出する物体検出装置に関する。

運転席からの死角をレーダで監視することで、隣接した走行レーンを走行する後側方の他車両や並走車両の存在を運転者に知らせる物体検出装置が知られている。例えば、運転者がウィンカスイッチをＯＮにした際、物体検出装置は後側方に障害物があることを検出すると、警報音を吹鳴するなどして運転者に注意喚起する。

レーダにより検出されるターゲットの位置（検波点）は若干、ばらつくことが知れており、従来から、検波点を追尾フィルタに入力して追尾点を生成し、追尾点によりターゲットを追尾している。そして、この追尾結果に基づき警報等を発している（例えば、特許文献１参照。）。特許文献１には、今回のサンプリング時刻における観測結果を示す位置Ｒ1と前回サンプリング時刻の情報に基づき予測された推定位置Ｐ0とから、今回のサンプリング時刻の観測対象物体の位置である確定位置Ｐ1を算出する車載型周辺監視装置が開示されている。

特開２００３−２１７０９９号公報

しかしながら、レーダの検出範囲内に複数のターゲットが存在している場合（例えば、接近して存在する場合）、複数のターゲットからの反射波により干渉が生じ、検波点の位置のばらつきが大きくなる。この結果、追尾点の近傍に検波点がなくなり、追尾フィルタが追尾点を更新するための検波点が得られない場合がある。

図１５（ａ）は、レーダの検波点の検出結果を示す図の一例である。レーダ位置（自車位置）が座標の原点に示され、相対的な検波点の位置が"×"でプロットされている。自車量の幅方向をＸ軸（左方向を負、右方向を正）、車軸に垂直な方向をＹ軸（前方を正、後方を負）としている。

図１５（ａ）は、ガードレールの内側（車線内）を自転車が走行している状況の検波点の検出結果を示している。図１５（ａ）の検出結果は時間の経過を含んでいるので、自転車は移動している。レーダの検出範囲内に複数のターゲットが存在する場合、検波点はレーダ位置を中心に円周方向にばらついてしまう。自車両の真後ろ付近にまで検波点がばらつく場合があることが分かる。

図１５（ｂ）は、検波点のばらつきを模式的に示す図の一例である。黒丸の追尾点が互いに直線で接続されており、その周囲に検波点（三角で示す）が検出されている。図では２つの追尾点が追尾されている。レーダ位置に近い追尾点ほど時間的に新しく、時間と共に２つのターゲットが互いに接近している。

追尾点から所定距離内に検波点が検出される場合、該検波点を用いて次のタイミングの追尾点を決定できる。しかし、図の例では、検波点が、レーダの反射波の干渉により、レーダ位置を中心とする円の円周方向に大きくばらついたため、最後の追尾点から所定距離内に検出されなくなっている。また、それまで追尾点が２つだったのに検波点が１つになる場合もある。

検波点のばらつきにより、検波点が追尾点の近くに存在しなくなった場合、物体検出装置はターゲットの運動モデルを用いて推定位置を算出することができる。しかし、運動モデルから求められるターゲットの動きは、ターゲットの実際の動きとずれている場合があるため、ターゲットの位置の検出精度が低下するおそれがある。

本発明は、上記課題に鑑み、ターゲットからの反射波が干渉しても、ターゲットの追尾精度を向上させることができる物体検出装置を提供することを目的とする。

本発明は、検出した物体の位置を追尾フィルタに入力して物体を追尾し車両周辺の物体を検出する物体検出装置であって、各物体の位置を検出する位置検出手段と、前記追尾フィルタが算出した任意の２つの物体の追尾位置と当該物体検出装置との距離の差を算出する距離差算出手段と、前記差が第１閾値以内の場合、２つの物体の前記追尾位置の履歴から求めた速度に基づき２つの物体が同一物体であるか否かを判定し、同一物体でないと判定された２つの物体の前記位置を補正して前記追尾フィルタに入力する位置補正手段と、を有することを特徴とする。

ターゲットからの反射波が干渉しても、ターゲットの追尾精度を向上させることができる物体検出装置を提供することができる。

本実施形態の物体検出装置の特徴を概略的に説明する図の一例である。レーダ装置を含む車載システムの構成例を示す図である。レーダ装置の概略構成図の一例である。物体検出装置の機能ブロック図の一例である。送信信号、受信信号、及び、ビート信号の周波数を模式的に説明する図の一例である。フーリエ変換の結果の一例を示す図である。方位の決定について説明する図の一例である。追尾部がカルマンフィルタにより追尾点を追尾する手順を示すフローチャート図の一例である（従来技術）。本実施形態の追尾部の処理手順を示すフローチャート図の一例である（干渉判定１）。レーダ位置からの距離Ｒと検波パワーの関係を説明する図の一例である。追尾部の処理手順を示すフローチャート図の一例である（干渉判定２）。レーダ位置からの距離Ｒと検波パワーの関係を説明する図の一例である。追尾部の処理手順を示すフローチャート図の一例である（干渉判定３）。ダミー検波点の作成方法を説明する図の一例である。レーダの検波点の検出結果等を示す図の一例である。

以下、本発明を実施するための形態について図面を参照しながら説明する。しかしながら、本発明の技術的範囲が、本実施の形態に限定されるものではない。

図１は、本実施形態の物体検出装置の特徴を概略的に説明する図の一例である。物体検出装置は、レーダがターゲットに反射して得られる反射波を解析して、周期的に検波点の位置を測定している。また、検波点が測定される毎に、例えば追尾フィルタに検波点を適用して追尾点（図の黒丸）を算出する。

図１では、３つの追尾点Ａ〜Ｃが算出されている。ここでは追尾点Ａが自転車、追尾点Ｂ，Ｃが同一の他車両からの異なる反射点であるとする。このように複数のターゲットが近接した場合、干渉により検波点の方位方向（レーダ位置から見た角度）のばらつきが大きくなってしまう。このため、本実施形態の物体検出装置は以下の処理を行う。
（１）追尾点のレーダ位置からの距離Ｒを追尾点同士で比較して、他方の追尾点が閾値ΔＲ内にある場合、２つの追尾点同士が干渉すると判定する。この判定は、２つの追尾点の全組み合わせについて行う。例えば、追尾点ＡとＢ、追尾点ＡとＣ、追尾点ＢとＣの組み合わせで行う。追尾点Ａに対し追尾点Ｂは閾値Ｒth内にあるので、追尾点ＡとＢは干渉すると判定される。追尾点Ｂに対し追尾点Ｃは閾値Ｒth内にあるので、追尾点ＢとＣは干渉すると判定される。
（２）同一のターゲットからの反射波で算出された２つの追尾点はそのまま追尾してもやがて１つの追尾点に収束するので、干渉すると判定する必要性が低い。このため、同一のターゲットから生じる２つの追尾点を特定する。このため、物体検出装置は、追尾点のｙ方向（自車両の進行方向）の速度を監視し、２つの追尾点の速度が近ければ同一ターゲットの２つの追尾点であると判定する。図１の例では、追尾点Ａ，Ｂと追尾点Ｂ、Ｃのうち、追尾点Ｂ，Ｃは同一ターゲットの追尾点なので速度が近く、同一ターゲットの２つの追尾点であると判定される。なお、ｙ方向の速度に限るのは、検波点はｘ方向（車幅方向）に大きくばらつくので、ｘ方向の速度では同一ターゲットか否かの判定が困難なためである。

以上の処理で、干渉する可能性がある追尾点はＡ，Ｂだけになる。（１）（２）の処理により、干渉判定を高精度に行うことができる。そして、干渉すると判定された追尾点については、後述するようにダミー検波点を作成するなどして（追尾点とペアリングする検波点を変える）追尾を継続すれば、ターゲットの位置を精度よく追尾することが可能になる。

〔構成例〕
図２は、レーダ装置を含む車載システム３００の構成例を示す図である。このレーダ装置１００が、特許請求の範囲の物体検出装置に相当する。レーダ装置１００と運転支援ＥＣＵ（Electronic Control Unit）２００がＣＡＮ（Controller Area Network）などの車載ネットワークを介して接続されている。レーダ装置１００は、後述するように、ターゲット（ターゲットを物標という場合がある）までの距離、相対速度、及び、方位（以下、これらをまとめて物標情報という場合がある）を周期的に運転支援ＥＣＵ２００に送信している。方位と距離から自車両に対するターゲットの相対的な位置が求められる。また、ターゲットは、レーダを反射するものであれば物標情報のターゲットとなりうるため、立体物の他、マンホールや路面、壁などの平面物がターゲットの場合もある。

本実施形態のレーダ装置１００の電波の送受信部は、樹脂など電波を通過させる素材で作成されている車両の後方バンパの左端コーナーと右端コーナーの内側にそれぞれ配置されている。レーダの送信方向の中心は、車軸と平行な方向に対し４５〜７０度程度となるように配置されている。照射角度（レーダの検出範囲）は例えば９０〜１２０度など、設計できる。仰角はほぼゼロ（路面に平行）である。

なお、並走走行車両を検出するレーダ装置１００の他に、自車両の前方にレーダ装置１００が取り付けられている場合がある。本実施形態の追尾方法はターゲットの検出範囲（レーダ装置のレーダの照射範囲）に関係なく適用可能である。

運転支援ＥＣＵ２００は物標情報に基づきＢＳＭ（Blind Spot Monitoring−System）やＣＴＷ（Cross Traffic Warning）など各種の運転支援を提供する。運転支援ＥＣＵ２００には、ウィンカスイッチ２０１、車輪速センサ２０２、操舵角センサ２０３、及び、作動デバイス２０４が接続されている。この他、ヨーレートセンサなど車載される一般的なセンサを有している。ウィンカスイッチ２０１は、ウィンカレバーの操作方向を検出する。車輪速センサ２０２は、各輪に配置されたロータの回転を、車体側のセンサが磁束変化などから取り出し、車輪パルスとして検出する。単位時間の車輪パルスの数から回転速度が求められ、さらにタイヤの径を考慮することで車速を求めることができる。操舵角センサ２０３は、ステアリングシャフトの回転角度を検出するセンサである。検出原理には様々なものがあるが、例えば、ステアリングシャフト側にＳ極とＮ極の磁性体を配置しておき、ステアリングシャフトの周囲をリング状に囲み、リング側で磁性の変化を検出することで回転角度を検出する。

作動デバイス２０４は、ターゲットとの異常接近を回避するための運転支援に用いられる各種の車載装置である。作動デバイス２０４として、例えば、警報ブザー、音声出力装置、及び、ステアリングモータがある。警報ブザーは、メータパネルのブザーを吹鳴することで車線変更するとターゲットと異常接近するおそれがあることを注意喚起し、音声出力装置はメッセージ（例えば「後側方に他車両が存在します」など）をスピーカから出力する。ステアリングモータは、後側方に他車両が存在する場合に、運転者がステアリングホイールを操舵すると、ステアリングシャフトに回転トルクを与え、操舵方向と逆方向に反力を生じさせる。運転者としては操舵にそれまでより大きな操舵力が必要になるので、運転者に並走走行車両の存在を注意喚起することができる。

図３は、レーダ装置１００の概略構成図の一例を示す。レーダ装置１００は、複数のアンテナと接続された基板１１上にＶＣＯ（電圧制御発振器）１６、ＡＳＩＣ１７及びマイコン１８を有するように構成される。図示する構成図は一例であって、ＡＳＩＣ１７の機能をマイコン１８で実装することやマイコン１８の機能をＡＳＩＣ１７で実装してもよい。また、図示する以外にＤＳＰを備えていてもよい。

アンテナには送信アンテナ１２と複数の受信アンテナ１４-1〜１４-nがある。送信アンテナ１２は、送信回路１３により所定の周波数に変調されたレーダ波を送信する。後述するように、送信波の周波数は、三角波状に増減するように制御され、上昇区間と下降区間のそれぞれでビート周波数が計測される。なお、このようなレーダをＦＭＣＷ（Frequency-modulated continuous-wave）と呼ぶが、本実施形態ではレーダの照射方向を短時間毎に切り替えるパルスレーダを用いてもよい。

送信回路１３は、ＶＣＯ１６が生成した例えばミリ波帯の高周波信号を電圧に変換して送信信号として送信アンテナ１２に供給する。また、送信回路１３は、ＶＣＯ１６が生成した送信信号（高周波信号）をｎ個の受信アンテナ１４-1〜１４-nの受信回路１５-1〜１５-nに分配する分配回路を有している。

受信アンテナは図ではｎ個であるが、方位を検出するためには最低２つの受信アンテナがあれば足りる。受信アンテナ１４-1〜１４-nは受信回路１５-1〜１５-nを含め、それぞれ同じ構成を有している。受信アンテナ１４-1〜１４-nはターゲットで反射した反射波を受信して受信回路１５-1〜１５-nに出力する。受信回路１５-1〜１５-nは、受信アンテナ１４-1〜１４-nから取得した受信信号と、送信回路１３から取得した送信信号をミキサで混合し、増幅した後、不要な周波数成分を除去して、基板１１側に出力する。混合された信号は、送信信号と受信信号の周波数の差により生じるビート信号である。ビート信号の周波数をビート周波数という。

各受信回路１５-1〜１５-nはビート信号を生成して基板１１側に送出する。各受信アンテナ１４-1〜１４-n及び受信回路１５-1〜１５-nをチャネルという場合がある。なお、図では省略しているが、各受信回路１５-1〜１５-nと基板１１とはスイッチを介して接続されており、同時には１つの受信回路１５-1〜１５-nがビート信号を生成するようになっている。

基板１１側のＶＣＯ１６はマイコン１８により制御され、上昇区間と下降区間とを有する三角波により高周波信号が変調される。また、ＡＳＩＣ１７は、各受信回路１５-1〜１５-nに対応して設けられたＡ／Ｄ変換回路２１-1〜２１-n、Ａ／Ｄ変換回路２１-1〜２１-nの後段に設けられたバッファ２２-1〜２２-n、及び、ＦＦＴ処理部２３を有している。

Ａ／Ｄ変換回路２１-1〜２１-nはビート信号をデジタルデータに変換し、チャネル毎に後段のバッファ２２-1〜２２-nに記憶する。各バッファ２２-1〜２２-nは、上昇区間バッファと下降区間バッファに分かれている。上昇区間で受信アンテナ１４-1〜１４-nが受信したビート信号は上昇部バッファに記憶され、下降区間で受信アンテナ１４-1〜１４-nが受信したビート信号は下降部バッファに記憶される。

ＦＦＴ処理部２３は、各チャネル毎にビート信号にＦＦＴ処理を施す。ＦＦＴ処理によりビート信号は、周波数と電力の関係のデータに変換される。受信アンテナ１４-1〜１４-nが受信する電波には反射波以外の成分も含まれているが、ＦＦＴ処理により、電力がピークの周波数をビート周波数と推定できる。

マイコン１８は、ＣＰＵ、ＲＯＭ、ＲＡＭ、入出力インタフェース、及び、その他の一般的な回路を備えた情報処理装置である。マイコン１８は後述する本実施形態の特徴的な処理を行う。

図４は、本実施形態の物体検出装置の機能ブロック図の一例を示す。物体検出装置は、検波部３１、追尾部３２、及び、警報部３３を有している。検波部３１は、図３の主にアンテナからＡＳＩＣまでが対応し、追尾部３２は主にマイコンが対応し、警報部３３は図２の作動デバイスが対応する。

検波部３１は、ビート信号にＦＦＴ（Fast Fourier Transform）を施しターゲットまでの距離、相対速度、及び、方位を算出する。追尾部３２は、例えば、カルマンフィルタを用いて検波点の追尾を行い、ターゲット位置を高精度に推定する。追尾部３２は、干渉判定部３４、同一物体特定部３５、及び、ダミー検波点作成部３６を有している。これらについては干渉判定１にて説明する。なお図４（ｂ）は干渉判定２で、図４（ｃ）は干渉判定３で使用される機能を示す。

警報部３３は、後側方車両や並走装甲車両の存在を運転者に注意喚起する。以下、検波部３１、追尾部３２、及び、警報部３３について順番に説明する。

＜検波部による距離、相対速度、方位の算出＞
レーダ装置１００は、ＦＦＴ処理部２３が処理した処理結果により距離と相対速度を算出し、ＦＦＴ処理で得られたピーク周波数のビート信号の位相により方位を算出する。
図５は、送信信号、受信信号、及び、ビート信号の周波数を模式的に説明する図の一例である。図５（ａ）は自車両とターゲットの相対速度がゼロの場合を、図５（ｂ）は自車両とターゲットの路面に対する速度が異なる場合をそれぞれ示す。

送信信号Ｓは、周波数の上昇と下降を繰り返す。周波数の変動量をΔＦ、中心周波数をｆ_０、１／ｆｍを上昇区間と下降区間の繰り返し周期、とする。上昇区間において周波数は増大しており、下降区間において周波数は減少する。また、送信アンテナ１２が送信信号Ｓを送信してから受信アンテナ１４-1〜１４-nが受信信号Ｒを受信するまでには、ターゲットまでの間の距離を電波が光の速さＣで往復する時間が必要になる。よって、相対速度がゼロの場合、送信信号と受信信号の周波数には、ターゲットとの距離及びｆｍに応じた差が生じる。この差がビート周波数ｆｂであり、相対速度がゼロの場合、上昇区間のビート周波数ｆｂ１と下降区間のビート周波数ｆｂ２とは等しい。

相対速度がゼロでない場合、送信信号Ｓの周波数は、ターゲットで反射する際に、相対速度に応じてドップラシフトされるため、受信信号Ｒの周波数はターゲットとの距離及びｆｍによる変化分以上に変化する（又は変化が低減される）。ドップラ周波数をｆｄとする。自車両がターゲットに接近している場合、上昇区間では、ドップラ周波数ｆｄだけ受信信号の周波数がシフトする（大きくなる）ので、受信信号と送信信号の周波数の差は小さくなり、下降区間では、受信信号と送信信号の周波数の差は大きくなる。したがって、上昇区間のビート周波数ｆｂ１と下降区間のビート周波数ｆｂ２とは等しくならない。

相対速度がゼロの場合のビート周波数をｆｒとすると、ビート周波数ｆｂ１、ｆｂ２は以下のように表すことができる。
ｆｂ１＝ｆｒ−ｆｄ
ｆｂ２＝ｆｒ＋ｆｄ
これを変形すると下式が得られる。
ｆｒ＝（ｆｂ１＋ｆｂ２）／２
ｆｄ＝（ｆｂ２−ｆｂ１）／２
目標物の距離Ｒと相対速度Ｖは下式から求めることができる。
Ｒ＝（Ｃ／（４・ΔＦ・ｆｍ））・ｆｒ
Ｖ＝（Ｃ／（２・ｆ_０））・ｆｄ
図６は、フーリエ変換の結果の一例を示す図である。図６（ａ）はアンテナ１の上昇部、図６（ｂ）はアンテナ１の下降部、図６（ｃ）はアンテナ２の上昇部、図６（ｄ）はアンテナ２の下降部、のフーリエ変換の結果をそれぞれ示す。なお、周波数ｆｂ１、ｆｂ２のかっこ内の数字はターゲットの違いを示している。各周波数成分の振幅が電力〔ｄB〕としてアンテナ毎に算出される。また、アンテナ毎に上昇部と下降部のフーリエ変換が行われる。図６ではピークが２つあるのでターゲットが２つ検出されている。閾値以上のピークの周波数がｆｂ１、ｆｂ２である。これらからｆｒ、ｆｄを求めると距離Ｒ、相対速度Ｖが得られる。
図７は、方位の決定について説明する図の一例である。図７（ａ）はターゲットがレーダ装置１００の正面に存在する場合の受信信号を模式的に示す図である。ターゲットが正面に存在する場合、ターゲットと２つの受信アンテナ１４-1、１４-2の間に経路差がほとんど生じないので、受信アンテナ１４-1と受信アンテナ１４-2の受信信号の位相は同位相となる。なお、レーダ装置１００はビート信号を処理対象とするが、受信信号が同位相ならビート信号も同位相になる。

図７（ｂ）はターゲットがレーダ装置１００の正面に対し、角度θの方位に存在する場合の経路差を示している。ターゲットと受信アンテナ１、２の間に経路差ｘが生じる。アンテナ間の距離をｄとした場合、経路差ｘはｄ・sinθである。２つのアンテナが受信するビート信号の位相差Δφと方位θの間には以下の関係がある。λは受信信号の波長である。
Δφ＝２π×（ｄ・sinθ／λ）
したがって、位相差Δφが求められれば、λ、ｄは固定値なので、下式から方位θを求めることができる。
θ＝ａｒｃｓｉｎ（λ・Δφ／（２・π・ｄ））
位相差Δφは、ＦＦＴ処理部２３が受信チャネル毎に行うフーリエ変換の結果から求められる。位相差Δφが２つのアンテナのアンテナパターンが検出する振幅の振幅比となるようにアンテナパターンを用いる測角方式をモノパルス方式という。具体的には両アンテナパターンの振幅比が方位θとなるように、異なるアンテナパターン（和パターンと差パターン）のアンテナを設計しておく。予め、振幅比とθの対応をテーブルにしておけば、２つのアンテナの振幅比から方位θを求めることができる。

このような方位の求め方をモノパルス方式という。本実施形態ではモノパルス方式による方位の決定について説明したが、方位の求め方はＤＢＦ（Digital Beam Forming)処理、ＭＵＳＩＣ（Multiple Signal Classification）解析、Ｃａｐｏｎ解析など知られており、モノパルス方式に限定するものではない。

＜追尾部によるターゲットの位置の推定＞
本実施形態では一例としてカルマンフィルタによりターゲットの位置を追尾する。位置を適当な二次元座標で示すため、追尾部３２は距離Ｒと方位θから検波点の位置を（ｘ、ｙ）で表す。カルマンフィルタは、最適制御のために状態変数ｘ（ベクトル）の最も確からしい推定値を求める手法である。カルマンフィルタでは式（１）の状態方程式と式（２）の観測方程式により系の状態を記述する。

ｘは状態変数のベクトルであり、本実施形態はｘ方向の位置ｘとｘ方向の速度ｖ_ｘ（ｘの一次微分）、及び、ｙ方向の位置ｙとｙ方向の速度ｖ_ｙ（ｙの一次微分）の４つの要素を有している。

ｚは観測値を示すベクトルであり、本実施形態では距離Ｒと方位から求められた位置（ｘ、ｙ）を要素とする。ｗ_ｔはプロセスのノイズ、ｖ_ｔは観測値のノイズである。カルマンフィルタではノイズはガウス分布に従うと仮定されており、ノイズの平均値はゼロであり、プロセスのノイズの標準偏差をＳ、観測値のノイズの標準偏差をＱとする。

Ｆは時刻ｔにおける系の状態と時刻ｔ＋１の系の状態を関係づける行列である。Ｈは、状態変数ｘと観測値ｚとを結びつける行列である。

微小時間内のターゲットの運動モデルを等速直線運動と仮定すると、Ｆ，Ｈを次のように求めることができる。τは検波点のサンプリング時間であり、τの間に速度ｖ_ｘ、ｖ_ｙで進んだ位置が時刻ｔ＋１の位置（ｘ、ｙ）となる。なお、Ｆ，Ｈは一例であり、例えばＨで速度を考慮するなど適宜、設計できる。

カルマンフィルタでは、２つの状態の推定値がある。１つは事前推定と呼ばれる推定値

であり、時刻ｔにおける観測値が得られる前に推定された時刻ｔの状態である（以下、事前推定又は事前推定の値という）。もう１つは、事後推定

であり、時刻ｔにおける観測値から推定された時刻ｔの状態である（以下、事後推定又は事後推定の値という）。

事前推定の値と観測値の誤差の共分散行列をＰｔ、事後推定の値と観測値の誤差の共分散行列をＰ´ｔ、と定義する。

事前推定の値と事後推定の値は、観測値が含む誤差がそれほど大きくないと仮定すると、大きくは異なっていないと考えられる。そこで、カルマンフィルタでは、カルマンゲインと呼ばれる係数（行列）を用いて、事後推定の値を、事前推定の値と観測値から推定する式を以下のように記述する。

式（５）は、事後推定の値と事前推定の値との差が、事前推定の値からＨにより予測される値と観測値ｚ_ｔとの差にカルマンゲインＫを乗じた値と等しいことを意味している。よって、右辺第２項を第１項の事前推定の値に加算することで、事後推定の値を修正することができることを意味している。カルマンゲインＫは、右辺第２項の補正係数であり、どの程度、事後推定の値を修正するかを決定する。

カルマンゲインＫは、事後推定の値と観測値の誤差の共分散行列Ｐ´ｔが最小となるように求められる。過程は省略するが、これによりカルマンゲインが求められ、共分散行列Ｐ´ｔが以下のように求められる。なお、Iは単位行列である。

図８は、追尾部３２がカルマンフィルタにより追尾点を追尾する手順を示すフローチャート図の一例である（従来技術）。図８の処理は新たにターゲットが検出される毎に、各ターゲットが検出されている間、対し繰り返し実行される。これにより、ターゲットが追尾される。

追尾部３２には予め、事前推定の値の初期値と、事前推定の誤差の共分散行列Ｐｔの初期値とが与えられている（Ｓ１００）。初期値はいずれもゼロとすればよい。

追尾部３２は、まず、式（６）を用いてカルマンゲインＫを計算する（Ｓ２００）。

次に、追尾部３２は、事前推定の値を更新しておく（Ｓ３００）。これにより、次回のステップＳ５００で事後推定の値の計算が可能になる。なお、初期状態では事後推定の値が得られていないのでガードレールや自転車など想定される物標の観測値（定数）を与えてやる。

次に、観測値ｚ_ｔが検出されると（Ｓ４００のＹｅｓ）、追尾部３２は式（５）により、カルマンゲインＫ、事前推定の値、及び、観測値ｚ_ｔを用いて、事後推定の値を更新する（Ｓ５００）。観測値ｚ_ｔは、追尾点から所定距離内のものが探索される。または、観測値ｚ_ｔは、最新の事前推定の値を用いてもよい。この事後推定の値が最も新しい追尾点であり、次回の観測値（検波点）は新しい追尾点、又は、最新の事前推定の値を中心に探索される。一定の範囲内に検波点がない場合、カルマンフィルタによる追尾でなく運動モデルにより追尾される。

次に、追尾部３２は、式（７）を用いて、事後推定の誤差の共分散行列Ｐ´ｔを更新する（Ｓ６００）。共分散行列Ｐ´ｔを更新することで、事前推定の誤差の共分散行列Ｐｔの更新が可能となる。

追尾部３２は、事前推定の誤差の共分散行列Ｐｔを更新する（Ｓ７００）。これにより、次回のステップＳ２００でカルマンゲインＫの計算が可能になる。
このように、次回の観測値ｚ_ｔに対しては、Ｓ７００で更新された事前推定の誤差の共分散行列Ｐｔによりカルマンゲインが算出され（Ｓ２００）、このカルマンゲインとＳ３００で更新された事前推定の値により事後推定の値が算出される（Ｓ５００）。

なお、本実施形態ではカルマンフィルタにより説明したが、αβフィルタ、αβγフィルタ、又は、粒子フィルタなど、物体追跡に好適なフィルタであれば同様に用いることができる。

＜警報部によるターゲットの存在の注意喚起＞
追尾部３２が検波点（観測値ｚ_ｔ）から最も確からしいターゲットの位置を推定するので、警報部３３は検波点そのものを用いて注意喚起するよりも、正確な注意喚起が可能になる。

警報部３３は、例えば、自車両から所定距離内の警報エリアにターゲット（追尾点）が侵入した場合、警告ランプを点灯してターゲットに存在を報知する。そして、さらに運転者がウィンカをＯＮしたタイミングで警報音を吹鳴することでターゲットと異常接近するおそれがあることを運転者に注意喚起する。

〔本実施形態の干渉判定１〕
図９は、本実施形態の追尾部の処理手順を示すフローチャート図の一例である。図９の処理は、図８の処理で追尾点が算出された後に実行される。追尾部はそれまでのレーダ走査で複数の追尾点を追尾している。ここでは図１のように追尾点Ａ〜Ｃが追尾されているものとする。

＜Ｓ１−１０＞
干渉判定部３４は、追尾点のレーダからの距離Ｒを追尾点同士で比較し、２つの追尾点の距離Ｒの差が閾値Ｒth以内にあるか否かを判定する。干渉判定部３４は、全ての追尾点から２つの組み合わせを全て取り出す。追尾点Ａ〜Ｃの場合、追尾点Ａ，Ｂ、追尾点Ｂ，Ｃ、追尾点Ａ，Ｃ、の３つの組み合わせがある。

干渉判定部３４はレーダ位置から追尾点Ａまでの距離Ｒ_Ａと、追尾点Ｂまでの距離Ｒ_Ｂとの差の絶対値が閾値Ｒth以内か否かを判定する。追尾点Ｂ，Ｃ及び追尾点Ａ，Ｃについても同様に判定する。ここでは、追尾点Ａ，Ｂ、追尾点Ｂ，Ｃが閾値Ｒth以内にあると判定され、干渉するおそれが高いと判定される。

＜Ｓ１−２０＞
次に、同一物体特定部３５は、追尾点Ａ，Ｂ、追尾点Ｂ、Ｃが同一ターゲットか否かを判定する。同一ターゲットの場合、追尾点が１つに収束するのでそのまま追尾してもターゲットの位置の検出精度が低下するおそれが低いためである。

具体的には追尾点Ａ，Ｂ、追尾点Ｂ，Ｃのｙ方向の速度が同一とみなせるか否かを判定する。ｙ方向の速度は周期的に算出される追尾点のｙ座標の差を周期で除することで求められる。同一ターゲットでもｙ方向の速度は完全には一致しないので、例えば速度差が数％〜１０％程度に収まる２つの追尾点は同一ターゲットであると判定する。図１の例では、追尾点Ｂ，Ｃは同一ターゲットであると判定される。したがって、追尾部は追尾点Ａ，Ｂに対してのみ干渉を抑制する処理を行う。

＜Ｓ１−３０＞
追尾部は追尾点Ａ，Ｂに対し干渉を抑制する処理を行う。この処理については後述するが、例えば、ダミー検波点作成部３６がダミー検波点を作成し、追尾部３２において追尾点とペアリングされる検波点をダミー検波点に切り替える。

したがって、以上説明したように、追尾方法１では追尾点同士の距離とｙ方向の速度から干渉の有無を適切に判断できる。干渉すると判定されれば、ダミー検波点により干渉を抑制することができる。

〔本実施形態の干渉判定２〕
干渉判定２では、ターゲット同士が接近して干渉する際、検波パワー（ビート周波数の電力）が時間に対し大きく変動することを利用して、２つの追尾点が干渉するか否かをさらに高精度に判定する。

図１０（ａ）はレーダ位置からの距離Ｒと検波パワーの関係を説明する図の一例である。図６にて説明したように周波数と検波パワーの関係が分かっており、周波数と距離Ｒの関係も分かっている。したがって、距離Ｒからｆｂ１，ｆｂ２を求めれば距離と検波パワーの関係が得られる。図１に示したように、追尾点Ｃが最もレーダ位置に近く、次に追尾点Ｂ、その次に追尾点Ａが存在するので、検波パワーのピークもこの順番に得られる。なお、最も近くのピーク（Ｃよりも近いピーク）は距離Ｒ_Ｃ、Ｒ_Ｂと共にターゲットの車両のピークである。

したがって、距離Ｒ_Ｃ、Ｒ_Ｂ、Ｒ_Ａに検波パワーのピークが得られるが、図１０（ａ）のピーク付近に上下方向の矢印線で示すようにピークは常に一定ではなく時間的に変動するものである。そして、この変動量が、追尾点同士が干渉することで大きくなることが実験的に明らかになった。

図１０（ｂ）は検波パワーのピークの時間的な変動を示す図の一例である。図１０（ａ）の距離Ｒ_Ｂと距離Ｒ_Ａの検波パワーのピークを時間情報と共に記録しておき、時間に対し検波パワーのピークをプロットすることで、図１０（ｂ）のグラフが得られる。図１０（ｂ）の矢印の範囲で示す時間帯に、静止している車両の横を自転車がすり抜けている。この時間帯の検波パワーのピークを見ると、それまで比較的安定していた検波パワーのピークが大きく変動していることが分かる。したがって、検波パワーのピークを記録しておきその変動量を閾値と比較することでターゲット同士が干渉していることを推定できる。

図１１は、本実施形態の追尾部の処理手順を示すフローチャート図の一例である。図１１のＳ２−１０〜Ｓ２−３０は、図９のＳ１−１０を置き換える処理である。ステップＳ２−４０、Ｓ２−５０については図９のＳ１−２０、Ｓ１−３０と同様なので省略する。

＜Ｓ２−１０＞
干渉判定部３４は、追尾点のレーダ位置からの距離Ｒを追尾点同士で比較し、２つの追尾点の距離Ｒの差ΔＲを算出する。Ｓ１−１０と同様に、干渉判定部３４は、全ての追尾点から２つの組み合わせを全て取り出す。追尾点Ａ〜Ｃの場合、追尾点Ａ，Ｂ、追尾点Ｂ，Ｃ、追尾点Ａ，Ｃ、の３つの組み合わせがある。干渉判定部３４はレーダ位置から追尾点Ａまでの距離Ｒ_Ａと、追尾点Ｂまでの距離Ｒ_Ｂとの差ΔＲを算出する。追尾点Ｂ，Ｃ及び追尾点Ａ，Ｃについても同様にΔＲを算出する。

＜Ｓ２−２０＞
パワー変動履歴監視部３７は、追尾点の更新に用いた検波点の過去の検波パワーを用いて、一定時間毎の検波パワーの変動幅を算出する。変動幅は一定時間毎に検波パワーのピークの平均を求めてもよいし、一定時間ごとのピークの変動幅で直前に記憶した変動幅を更新するようにして最新の変動幅を常に保持するようにしてもよい。

＜Ｓ２−３０＞
干渉判定部３４は、ΔＲが閾値Ｒth以下、かつ、変動幅が閾値（例えば、ピーク値の５０〜１００％）以上の場合、追尾点が干渉すると判定する。ΔＲは２つの追尾点から求められるが、変動幅は各追尾点で求められるので、いずれか一方の追尾点の変動幅が閾値以上なら干渉すると判定する。

したがって、干渉判定２では追尾点同士の距離と検波パワーのピークの変動から、干渉の有無をより精度よく判断でき、干渉を抑制することができる。

〔本実施形態の干渉判定３〕
干渉判定３では、検波パワーから大物体であることを判定して、閾値Ｒthを可変（大きくする）にすることで、より早期に干渉の始まりを検出する
図１２はレーダ位置からの距離Ｒと検波パワーの関係を説明する図の一例である。検波パワーがある閾値（ターゲットの大きさ判定のための閾値）より大きい範囲が広い場合は、追尾点の検波パワーが広がりを持っていると推定できる。したがって、複数の追尾点が同一ターゲット（トラックなどの大物体）の可能性がある。

反射波の干渉による検波点のばらつきは実ターゲットが接近した時に始まる。追尾点は大物体の端点を追尾しているとは限らないので、ターゲットの大きさを考慮して閾値Ｒthを可変にすれば、干渉の始まりを検出できる可能性がある。図１２の例では、検波パワーが閾値以上の範囲内にある追尾点Ｃ，Ｂは同一ターゲットと推定できる。これから追尾点Ｂと近い追尾点Ａを検出し追尾点Ａ，Ｂ間の距離が、ターゲットの大きさを考慮した閾値Ｒth以内なら干渉する可能性が高い。

図１３は、本実施形態の追尾部の処理手順を示すフローチャート図の一例である。図１３のＳ３−１０〜Ｓ３−５０は、図９のＳ１−１０、Ｓ１−２０を置き換える処理である。図１３のＳ３−６０については図９ぼＳ１−３０と同様なので省略する。

＜Ｓ３−１０＞
距離範囲決定部３２１は、1周期前の追尾点付近において、検波パワーが閾値以上の距離範囲ｄを決定する。図１２の例ではｄ１とｄ２の２つの距離範囲が決定される。

＜Ｓ３−２０＞
追尾位置グループ化部３２２は、同一の距離範囲ｄ内の追尾点をグループ化して抽出する。追尾点は1つ以上であるが、大物体では２つ以上見つかる場合が多い。図１２の例では、距離範囲ｄ１の追尾点Ｂ，Ｃ、及び、距離範囲ｄ２の追尾点Ａが抽出される。

＜Ｓ３−３０＞
Ｓ３−３０〜Ｓ３−５０は各距離範囲毎に実行される。
閾値変更部３２３は、各距離範囲ｄの大きさに応じて、それぞれの距離範囲間の閾値Ｒthを算出する。距離範囲ｄが長いなら大物体と推定されるが、検波パワーが閾値未満となって再度、閾値を超えた位置にある追尾点は別のターゲットであると考えられる。そして、距離範囲ｄに存在する追尾点と別の距離範囲ｄの追尾点とは、距離範囲ｄが長いほど干渉する可能性があると考えられるので、距離範囲ｄに応じて距離範囲間の閾値Ｒthを大きくする。具体的には、例えば距離範囲ｄ１に比例して距離範囲ｄ１と比較される追尾点までの閾値Ｒthを大きくし，距離範囲ｄ２に比例して距離範囲ｄ２と比較される追尾点までの閾値Ｒthを決定する。なお、閾値Ｒthは例えば距離範囲ｄの３割〜８割程度とする。こうすることで干渉する可能性がある追尾点を検出しやすくなる。

＜Ｓ３−４０＞
追尾点抽出部３２４は各距離範囲でレーダ位置からの距離が近い追尾点同士を抽出する。追尾点Ｃ，Ｂの場合、追尾点Ｃのレーダ位置からの距離Ｒ_Ｃが近い追尾点、及び、追尾点Ｂのレーダ位置からの距離Ｒ_Ｂが近い追尾点を抽出する。追尾点Ｂのレーダ位置からの距離Ｒ_Ｂは、追尾点Ａのレーダ位置からの距離Ｒ_Ａと近いので、追尾点同士として（Ｂ，Ａ）が抽出される。

＜Ｓ３−５０＞
干渉判定部３４は、追尾点同士のレーダ位置からの距離の差がＳ３−３０で求めた閾値Ｒth以内の場合、追尾点同士が干渉すると判定する。つまり、追尾点同士として（Ｂ，Ａ）が抽出された場合、追尾点Ａのレーダ位置から距離Ｒ_Ａ、追尾点Ｂのレーダ位置から距離Ｒ_Ｂの差がＲth以内であれば、干渉すると判定される。

このように、大物体が存在する場合、閾値Ｒthを大きくすることで干渉の始まりを推定することができる。

〔Ｓ１−３０の干渉を抑制する処理〕
干渉するおそれがある場合に、追尾部はダミー検波点を作成することで、検波点が方位方向にばらついても追尾点をフィルタで追尾することを可能にする。

図１４は、ダミー検波点の作成方法を説明する図の一例である。図１４（ａ）に示すように、物体検出装置は、レーダがターゲットに反射して得られる反射波を受信し解析して、周期的に検波点の位置（図の三角）を測定する。また、検波点が測定される毎に、例えばカルマンフィルタに検波点を適用して追尾点（図の丸）を算出する。

干渉すると判定された２つの追尾点は検波点の位置がずれてしまうので、本実施形態の物体検出装置は以下のようにダミー検波点を作成し、ダミー検波点をカルマンフィルタに適用して追尾点の算出を継続する。

図１４（ｂ）はダミー検波点の作成方法を説明する図の一例である。ダミー検波点作成部３６は、ダミー検波点を作成する際、検波点がレーダ位置に対し円周方向にばらつくことを考慮する。円周方向にばらつくことは、ｘ座標の位置ずれは大きいがｙ座標は比較的、ターゲットの位置を反映していることを意味する。そこで、追尾点の過去のｘ座標を利用してダミー検波点のｘ座標（＝Ｘ´）を求め、Ｘ´と距離Ｒを利用してダミー検波点のｙ座標（＝Ｙ´）を求める。

追尾点Ａのダミー検波点Ａ´（Ｘ´、Ｙ´）は例えば、以下のようにして求める。
Ｘ´：追尾点の過去のＸ値の平均や直近値
Ｙ´：レーダ位置を中心に検波点までの距離Ｒの円の円周と、ｘ座標＝Ｘ´の直線の交点をＹ´とする。

Ｙ´＝√（Ｒ^２−Ｘ´^２）
すなわち、先にＸ´を定め、そのＹ座標であるＹ´を、検波点までの距離Ｒから求める。こうすることで、検波点のうちばらつきの大きいｘ座標は追尾点から推定し、検波点が分布する円周上の点を利用してｙ座標を求めることができる。このように推定されたダミー検波点をカルマンフィルタに適用して追尾点を算出するので、運動モデルを用いるよりも追尾点の精度を維持することができる。なお、Ｘ´には追尾点の直前のｘ座標を用いてもよい。

また、ダミー検波点を作成しなくてもよい。レーダの干渉が検波点に影響を及ぼすおそれがあっても、反射強度が大きいターゲットの付近に、検波点は密集しやすいことが予想される（図１５ではガードレールの近くに検波点が密集している。）。

したがって、検波点にレーダの干渉が影響するおそれがあっても、追尾点の近くに検波点があればその検波点の誤差が小さいと推定することができる。そこで、この場合、ダミー検波点を作成せずにカルマンフィルタを更新することができる。検波点を用いることで、ダミー検波点を作成するよりも、処理負荷の増大を抑制し、追尾点の精度を向上させることができる。

１１検波部
１２追尾部
１３警報部
２１一覧作成部
２３ダミー検波点作成部
１００レーダ装置

Claims

検出した物体の位置を追尾フィルタに入力して物体を追尾し車両周辺の物体を検出する物体検出装置であって、
各物体の位置を検出する位置検出手段と、
前記追尾フィルタが算出した任意の２つの物体の追尾位置と当該物体検出装置との距離の差を算出する距離差算出手段と、
前記差が第１閾値以内の場合、２つの物体の前記追尾位置の履歴から求めた速度に基づき２つの物体が同一物体であるか否かを判定し、同一物体でないと判定された２つの物体の前記位置を補正して前記追尾フィルタに入力する位置補正手段と、
を有することを特徴とする物体検出装置。
前記位置検出手段は、照射した電波が物体から反射する反射波を解析して前記距離を検出し、
物体毎に前記反射波の電波強度のピーク値を時系列に監視する電波強度監視手段を有し、
前記追尾位置の前記差が前記第１閾値以内であり、かつ、前記追尾位置に対応する２つの物体の少なくとも一方の前記ピーク値の変動幅が第２閾値以上であり、前記速度に基づき２つの物体が同一物体でないと判定された場合、
前記位置補正手段は、２つの物体の前記位置を補正して前記追尾フィルタに入力する、
ことを特徴とする請求項１記載の物体検出装置。
前記位置検出手段は、照射した電波が物体から反射する反射波を解析して前記距離を検出し、
前記距離に対し前記反射波の電波強度を打点した際に前記電波強度が第３閾値以上になる距離範囲を決定する距離範囲決定手段と、
各距離範囲に前記距離が含まれる１つ以上の前記追尾位置をグループ化する追尾位置グループ化手段と、
前記距離範囲の長さに応じて前記第１閾値を大きくする閾値変更手段と、
グループ内の前記追尾位置の前記距離との差が第４閾値内の前記距離を有する、他のグループの前記追尾位置を抽出する追尾位置抽出手段と、を有し、
グループ内の前記追尾位置の前記距離と、前記追尾位置抽出手段が抽出した他のグループの前記追尾位置の前記距離との差が、前記閾値変更手段が変更した前記第１閾値以内の場合に、前記位置補正手段は前記距離の差が変更後の第１閾値内の２つの追尾位置に対応する物体の前記位置を補正して前記追尾フィルタに入力する、
ことを特徴とする請求項１記載の物体検出装置。
前記速度は、当該物体検出装置が車両に固定された場合に車両の進行方向と同じ方向の速度である、ことを特徴とする請求項１記載の物体検出装置。
前記追尾フィルタは、
前記追尾位置と物体の移動規則から物体の予測位置を予測する位置予測手段と、
前記位置検出手段が検出した物体の前記位置、及び、前記位置予測手段が予測した予測位置に基づき、前記追尾位置を更新する追尾点更新手段と、を有し、
前記位置補正手段は、前記位置検出手段が検出した物体の距離情報を用いて、前記位置検出手段が検出した物体の位置を補正し、
前記追尾点更新手段は、前記位置補正手段が補正した物体の位置、及び、前記位置予測手段が予測した予測位置に基づき、前記追尾位置を更新する、
ことを特徴とする請求項１〜４いずれか１項記載の物体検出装置。