JP2009042181A

JP2009042181A - 推定装置

Info

Publication number: JP2009042181A
Application number: JP2007210275A
Authority: JP
Inventors: Takashi Ogawa; 高志小川
Original assignee: Denso Corp
Current assignee: Denso Corp
Priority date: 2007-08-10
Filing date: 2007-08-10
Publication date: 2009-02-26
Also published as: DE102008037014A1; US20090040095A1; US7920087B2

Abstract

【課題】前方車両の形状及び運動状態を正確に推定する。
【解決手段】推定装置は、レーダ波によって、前方車両表面の複数地点の位置座標を計測し（Ｓ１１０）、この複数地点を単一線分及び二線分で近似する（Ｓ１２０，Ｓ１３００）。そして、各線分毎に、求めた線分長さ及び近似誤差に基づき、線分が車両前後面、側面を近似した線分である事前確率を算出する（Ｓ１４００）。一方、過去において推定された前方車両の位置、向き、速さ、進行方向及び角速度の情報を基に、尤度を算出する（Ｓ１５００）。そしてベイズ推定により、既に求めた事前確率と尤度とから、各線分毎に、線分が車両前後面、側面を近似した線分である事後確率を算出し（Ｓ１６００）、前方車両のサイズ、位置及び向きを推定する（Ｓ１７００）。また、これらの情報を基に、アンセンテッドカルマンフィルタにより、車両の速さ、進行方向及び角速度も合わせて推定する（Ｓ１８００）。
【選択図】図２

Description

本発明は、前方に位置する車両の状態を推定する推定装置に関する。

従来、車両の状態を推定する装置としては、レーダ波を用いて、離れた位置にある車両の状態を推定するレーダ装置が知られており、この種のレーダ装置としては、例えば、直線道路上に設置され、レーダ波としてのミリ波を所定の範囲に向けて周期的に照射するものが知られている（例えば、特許文献１参照）。

このレーダ装置は、レーダ波が車両に反射して戻ってくる反射波を捉えて、反射波の受波結果に従い、車両の状態を推定する。具体的には、車両が直線運動していると仮定して、線形運動モデルに基づき設計されたカルマンフィルタを用いて、車両の速度や加速度を求め、車両の運動状態を予測する。このレーダ装置によれば、道路を走行する車両の現在位置を知ることができると共に、道路上の車両が運動していれば、車両の未来の運動状態を予測することができる。

従って、このレーダ装置を用いれば、観測値と未来の予測値とを用いて、道路上に車両などが停止しているか否かの判断の精度や、車両速度の算出精度等を高めることができ、車両同士の衝突を予測して、これを回避するための処理を実行することができる。
特開２００２−９９９８６号公報

しかしながら、従来では、次のような二つの問題があった。
一つ目の問題は、前方に位置する車両の大きさや向きを正確に検出する技術がないため、前方に位置する車両の大きさや向きなどの情報を、衝突回避に必要十分な程度、得ることができないという問題である。車両の形状がわからなければ、正確に衝突を予測することは困難である。このため、従来技術では、レーダ装置の検出結果に基づいて、衝突回避のための車両制御を実行することに関し、精度的な限界があった。

二つ目の問題は、車両の旋回時の運動を予測する技術がないという問題である。つまり、従来技術では、車両の未来の状態を予測するに際し、車両が直線運動していると仮定して、線形運動モデルに基づき、カルマンフィルタにより車両運動を予測するので、車両が旋回運動している場合等には、その旋回運動を正確に捉えて、車両運動を予測することができないといった問題あった。

本発明は、こうした問題に鑑みなされたものであり、車両の状態を精度良く推定することが可能な推定装置を提供することを目的とする。

かかる目的を達成するためになされた請求項１に記載の推定装置は、前方に位置する車両の状態を推定する推定装置であって、測位手段、解析手段、及び、推定手段を備えるものである。この推定装置においては、まず測位手段が、レーダ波を発射し、このレーダ波が前方物体に反射して戻ってくる反射波を受波して、レーダ波を反射した前方物体表面の複数地点の位置座標を計測する。

次に、解析手段が、車両の輪郭をモデル化した、位置、向き及びサイズが未知の図形パターンで、測位手段により計測された複数地点の位置座標で表される前方物体の輪郭パターンを近似し、図形パターンの位置、向き及びサイズを求める。そして推定手段が、解析手段により求められた図形パターンの位置、向き及びサイズに基づき、前方に位置する車両の状態として、前方車両の位置と、前方車両の向きと、前方車両の全長及び全幅の両方又はいずれか一方と、を推定する。

この推定装置によれば、車両の輪郭をモデル化した図形パターンを用いて、車両の状態（前方車両の位置と、前方車両の向きと、前方車両の全長及び全幅の両方又はいずれか一方と）を推定するため、従来技術よりも、車両の状態を高精度に推定することができる。

従来では、測位結果だけを基にして車両の状態を推定するため、前方車両が走行していると、測位誤差の影響を受けて、車両の状態を、高精度に推定するのが難しかった。
そこで、本発明の推定装置では、測位結果だけを基にするのではなく、一般的な車両の輪郭は、類似していることを利用して、車両（四輪自動車）の形状をモデル化し、測位結果を、そのモデルと照合することで、前方に位置する車両の状態を正確に推定するようにしたのである。従って、この発明によれば、簡単かつ高精度に、前方に位置する車両の状態を推定することができ、衝突回避等の車両制御に大変役立つ。

また、請求項２に記載の推定装置は、車両の一面をモデル化した単一の線分から成る図形パターンと、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンと、を上記車両の輪郭をモデル化した図形パターンとして用いるものである。

この推定装置では、解析手段が、各図形パターン毎に、当該図形パターンで、前方物体の輪郭パターンを近似して、当該図形パターンの位置、向き及びサイズと、当該図形パターンと上記車両の輪郭パターンとの近似誤差と、を求める。また、推定手段は、解析手段により求められた各図形パターンの位置、向き、サイズ及び近似誤差に基づき、前方に位置する車両の位置と、車両の向きと、車両の全長及び全幅の両方又はいずれか一方と、を推定する。

つまり、この推定装置では、車両の輪郭を長方形と考え、その長方形の位置、向き及びサイズを求める。但し、測位手段によって得られる情報は、車両のどの部位を測位した結果を表すものであるのか不明である。換言すると、レーダ波を反射した部位が、車両の前面もしくは後面（以下、「前後面」と表現する。）又は側面だけか、或いは、前後面と側面との両方であるかは、測位時点では、わからない。

そこで、この推定装置では、単一の線分とＬ字型の二線分との二つの図形パターンを用いて、車両の輪郭パターンを近似し、それと共に近似誤差を求める。そして、近似誤差を考慮して、車両の状態を推定する。従って、この推定装置によれば、測位時に、当該推定装置に対する車両の向きがどの向きであっても、高精度に、前方に位置する車両の状態を求めることができる。

また、請求項３に記載の推定装置は、解析手段が、各図形パターンの位置、向き、サイズ及び近似誤差として、各図形パターン毎に、当該図形パターンを構成する各線分の位置、向き及び長さと、各線分と上記車両の輪郭パターンとの近似誤差とを求める構成にされたものである。この推定装置において、推定手段は、解析手段により求められた各図形パターンを構成する線分の位置、向き、長さ、及び近似誤差に基づき、前方に位置する車両の位置と、車両の向きと、車両の全長及び全幅の両方又はいずれか一方とを推定する。

この推定装置のように、各線分毎に、位置、向き、長さ、及び近似誤差を求めるようにすれば、詳細に、車両の輪郭パターンを近似することができて、高精度に、車両の状態を推定することができる。

また、請求項４に記載の推定装置は、推定手段が、蓋然性算出手段と選択手段とを備える構成にされたものである。この推定装置において、蓋然性算出手段は、上述した各図形パターンを構成する計三本の線分それぞれについて、車両のどの部位を近似しているかの蓋然性、つまり、車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を求める。尚、蓋然性を求める際には、解析手段によって求められた該当する線分の位置、向き、長さ及び近似誤差から、所定の計算をし、上記蓋然性を求める。

一方、選択手段は、蓋然性算出手段の算出結果に基づき、前方に位置する車両の輪郭として最も確からしい図形パターンとして、上述した二つの図形パターン、つまり単一の線分またはＬ字型の二線分のどちらかを選択する。

即ち、この推定装置が備える推定手段は、上記選択手段により選択された図形パターンについて、解析手段により求められた位置、向き及びサイズに基づき、前方に位置する車両の状態を推定する。

この推定装置では、上述した手法で、近似した各線分毎に、車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を算出し、このような客観的な指標に基づいて、上述した二つの図形パターンからどちらかを選び、車両の状態を推定するので、精度よく、車両の状態を推定することができる。

また、請求項５に記載の推定装置は、測位手段が、レーダ波を発射して前方物体表面の複数地点の位置座標を計測する処理を、繰返し実行する構成にされ、解析手段及び推定手段が、測位手段と共に繰返し動作する構成にされたものである。

当該推定装置は、更に、予測手段を備え、予測手段にて、推定手段による車両の状態の推定前に、推定手段により過去に推定された車両の状態に基づき、推定手段による次回推定時における車両の状態を、所定の運動モデルに従って推定する。

そして、蓋然性算出手段は、今回計測された複数地点の位置座標に基づき解析手段により求められた各線分の位置、長さ、向き及び近似誤差と、予測手段により推定された車両の状態と、に基づき、各線分毎に、当該線分が車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を求める。

つまり、この推定装置は、観測値から次の観測時の車両状態を予測して、次の観測時に、今回の観測値と、その予測結果とから、車両の状態を推定することで、瞬間的な観測値だけではなく、過去の観測値も加味して、車両の状態を推定する。従って、この推定装置によれば、観測値から車両の状態を予測しない場合と比較して、高精度に、車両の状態を推定することができ、衝突回避等の車両制御に大変役立つ。

また、この推定装置には、外乱で測位に失敗したとしても、上述した予測結果に基づいて前方に位置する車両の状態を推定できるといった利点がある。
また、請求項６に記載の推定装置は、上記各線分について、車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を、ベイズ推定の手法により求めるようにしたものである。

即ち、この推定装置では、蓋然性算出手段が、各線分毎に、車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を、ベイズ推定の手法により求める。

但し、ベイズ推定を用いるためには、予め事前確率とその事前確率に対応する尤度とを求める必要がある。そこで、本装置では、蓋然性算出手段にて、各線分毎に、解析手段によって求められた当該線分の長さ及び近似誤差に基づき、当該線分が、車両前後面を近似した線分である事前確率、及び、車両側面を近似した線分である事前確率を算出する。

また、本装置では、各線分毎に、上記各事前確率に対応する尤度を、解析手段によって求められた当該線分の位置、向き及び長さと、予測手段により推定された車両の状態と、に基づき、蓋然性算出手段にて算出する。

そして、蓋然性算出手段は、各線分毎に、上述した方法で求められた事前確率及び尤度を、ベイズ推定の式に入力し、上記車両前後面を近似した線分である蓋然性として、車両前後面を近似した線分である事後確率を求めると共に、上記車両側面を近似した線分である蓋然性として、車両側面を近似した線分である事後確率を求める。

従って、この推定装置によれば、上記蓋然性として、より確からしい値を導き出すことができ、結果として前方に位置する車両の状態を高精度に推定することができる。
また、請求項７に記載の推定装置は、予測手段が、推定手段により過去に推定された車両の状態に基づき、推定手段による次回推定時における車両の状態を、所定の運動モデルに従って推定すると共に、所定の運動モデルに従って、次回推定時における車両の進行方向を推定する構成にされたものである。

この推定装置において、蓋然性算出手段は、各線分毎に、予測手段により推定された車両の進行方向と、今回計測された複数地点の位置座標に基づき解析手段により求められた当該線分の向きとに基づき、各事前確率に対応する尤度を算出する。

このように、請求項７に記載の推定装置では、車両の進行方向も加味して、車両の状態を推定する。従って、この推定装置によれば、一層正確に、車両の状態を推定することができる。

また、請求項８に記載の推定装置は、レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、レーダ波を反射した車両表面の複数地点の位置座標を計測する測位手段を備え、この測位結果に従って、前方に位置する車両の運動状態を推定する推定装置であって、測位手段により計測された車両表面の複数地点の位置座標に基づき、推定手段により、車両の角速度を含む車両の運動状態を、所定の非線形運動モデルに従って推定する構成にされたものである。

従来では、線形運動モデルを用いて車両の運動状態を推定していたため、車両が旋回運動しているときには、車両の運動状態を正確に推定することができないといった問題があったが、本装置では、非線形運動モデルに基づいて、車両の角速度を含む車両の運動状態を推定するので、車両の旋回運動を正確に捉えることができる。従って、この推定装置は、車両の衝突回避に係る車両制御を行う際に、大変役立つ。

尚、推定手段は、上記車両の運動状態として、車両の角速度以外に、車両の位置、車両の向き、車両の速さ、及び、車両の進行方向の内、少なくとも一つを推定する構成にすることができる（請求項９）。このように、推定装置を構成すれば、車両の旋回運動を詳細に知ることができて、衝突回避に係る車両制御を行う際に、大変役立つ。

また、請求項１０に記載の推定装置は、推定手段が、非線形運動モデルが採用されたカルマンフィルタを用いて、車両の状態を推定する構成にされたものである。カルマンフィルタを用いて、車両の運動状態を推定するようにすれば、観測誤差の影響を抑えて、高精度に車両の運動状態を推定することができるといった利点がある。

この他、請求項１１に記載の推定装置は、カルマンフィルタが、アンセンテッドカルマンフィルタであることを特徴とするものである。アンセンテッドカルマンフィルタは、非線形運動の運動状態を推定するのに大変優れている。従って、アンセンテッドカルマンフィルタを用いて、車両の運動状態を推定するように推定装置を構成すれば、高精度に運動状態を推定することができる。

また、上記推定装置に採用する非線形運動モデルとしては、等速円運動モデルを挙げることができる（請求項１２）。等速円運動モデルを採用してカルマンフィルタを設計し、これに基づいて、車両の運動状態を推定するように上記推定装置を構成すれば、簡単な運動モデルで、高精度に、車両の旋回運動の状態を推定することができる。

また、請求項１５に記載の推定装置は、前回推定した角速度がゼロである場合には、車両状態の推定に用いる運動モデルを、非線形運動モデルから所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、車両の状態を推定するように構成されたものである。このように推定装置を構成すれば、旋回運動だけでなく、直進運動についての運動状態をも、正確に推定することができ、広範囲の車両運動を、高精度に推定することができる。

以下、本発明の実施例について図面と共に説明する。図１は、本発明が適用された推定装置１の構成を表すブロック図であり、四輪自動車の前方部に搭載されている。

本実施例の推定装置１は、図１に示すように、レーザ光を発射するレーザダイオード１１と、レーザダイオードから発射されたレーザ光を平行光線にするコリメータレンズ１２と、レーザダイオード１１を駆動させるレーザダイオード駆動回路１３と、レーザダイオード１１から発射されたレーザ光を反射するミラー１５と、ミラー１５を回転させるモータ１７と、レーザ光を集光する集光レンズ１９と、集光レンズ１９によって集光されたレーザ光を受光して電気信号を発生するフォトダイオード２１と、フォトダイオード２１からの電気信号を増幅する増幅器２２と、レーザダイオード駆動信号及び増幅器２２から出力された信号を取得し、レーザダイオード１１から発射されたレーザ光が反射してフォトダイオード２１によって検出されるまでの時間を検出する検出回路２３と、上述したレーザダイオード駆動回路１３に対するレーザダイオード駆動信号及びモータ１７を駆動させるためのモータ駆動信号を出力して、レーザ光の発射及びモータ１７の回転を制御する共に、モータ１７から入力されるモータ回転位置の情報及び検出回路２３から入力されるレーザ光の飛行時間ΔＴの情報に基づき、各種処理を実行する制御部２５と、から構成され
ている。

レーザダイオード１１は、前方車両を検知するためのレーダ波として、レーザ光を発射するための装置である。レーザ光は指向性が高いので、本装置によれば、高い空間分解能を得ることができる。また、コリメータレンズ１２は、レーザダイオード１１から発射され発散するレーザ光を、平行光線になるように、レーザ光を屈折させるものである。

ミラー１５は、光を反射するための六角形状の反射面を備えるポリゴンミラーとして構成され、モータ１７は、ＣＰＵ２５ａからのモータ駆動信号に従って、ミラー１５を回転させる構成にされている。本実施例の推定装置１は、この構成により、コリメータレンズ１２を通過してきたレーザ光を、任意の方向へ発射することができる。

また、制御部２５は、ＣＰＵ２５ａ、ＲＯＭ２５ｂ及びＲＡＭ２５ｃ等から構成され、ＣＰＵ２５ａにて、ＲＯＭ２５ｂに記憶された各種プログラムを実行することにより、レーザダイオード１１及びモータ１７を制御すると共に、モータ回転位置の情報及びレーザ光の飛行時間ΔＴの情報を基に、ＲＯＭ２５ｂに記憶されたプログラムによって、レーザ
光を反射した物体の状態を推定する。

図２は、ＣＰＵ２５ａが主体となって繰り返し実行する車両状態推定処理を表すフローチャートである。本装置では、レーザ光により検知された前方物体を四輪自動車とみなして以降の処理を行う。

当該車両状態推定処理を開始すると、ＣＰＵ２５ａは、レーザ光を発射してレーザ光の反射点を測位し、その位置座標を求める（Ｓ１１０）。具体的には、レーザレーダ駆動回路１３及びモータ１７を駆動して、ミラー１５を回転させながら、レーザダイオード１１から断続的にパルス光を発射させる。このようなレーザレーダ駆動回路１３及びモータ１７の制御により、前方に位置する車両に向けて、水平方向に照射角度を変化させながら、レーザ光を発射する。

前方物体によって反射されたレーザ光は、集光レンズ１９を通り、フォトダイオード２１に集光される。そして、集光されたレーザ光は、フォトダイオード２１によって電気信号に変換され、増幅器２２によって増幅された後、検出回路２３に送られる。

検出回路２３は、ＣＰＵ２５ａから出力されるレーザダイオード駆動回路１３の駆動信号としてのレーザダイオード駆動信号を取得可能に構成されており、この駆動信号の入力タイミングと、反射光の受光タイミングとから、図３（ａ）に示すように、レーザ光の飛行時間ΔＴとして、レーザ光の発光から受光までの時間（遅延時間：ΔＴ）を検出する。

ＣＰＵ２５ａは、パルス光が発射される度、検出回路２３から入力される遅延時間ΔＴ
に基づき、レーザ光の速さｖを用いて、反射点までの距離ＤをＤ＝ｖ・ΔＴ／２として求
める。そして、算出した距離Ｄと、当該距離Ｄが計測される原因となったパルス光の発射角度Φとから、反射点の位置座標（ｘ，ｙ）として、（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求める。尚、ここで、用いるｘｙ座標系は、車両に固定された推定装置１を基準とした相対座標系である。以下、この座標系を、「座標系Ａ」と表現する。

パルス光の発射角度Φは、モータ回転位置から一意に求めることができる。ここでは、図３（ｂ）に示すように、車両の前後方向の軸線上に、推定装置１が設置されているものとして、当該軸線から前方にｙ軸を設定し、ｙ軸を基準とした角度をφとして定義し、反射点の位置座標（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求めている。

この測位によって位置座標（ｘ，ｙ）が得られる反射点の具体例を表したのが図４である。図４（ａ）は前方車両５０が推定装置１に対して傾いている場合の反射点の分布を表す図であり、図４（ｂ）は前方車両５０の後面５０ａが推定装置１に対して正対している場合の反射点の分布を表す図である。尚、車両側面５０ｂまたは前面５０ｃが推定装置１に正対している場合にも、反射点の分布は、図４（ｂ）に示すようになる。

Ｓ１１０にて、前方物体表面の複数地点の位置座標を求めると、ＣＰＵ２５ａは、次に、Ｓ１１０で得られた複数地点の位置座標（各反射点の位置座標）で表される前方物体の輪郭パターンを、最小二乗法を用いて、一本の直線でフィッティングする。そして、近似直線の端点を求めることで、上記前方物体の輪郭パターンを、単一の線分で近似する。以下では、この近似線分を、線分Ｌ１と呼ぶ。また、前方物体の輪郭パターンを、一本の線分で近似すると、得られた線分Ｌ１について、その長さ及び中点の位置座標、及び、近似誤差を求める（Ｓ１２０）。尚、ここでいう近似誤差は、得られた線分Ｌ１と上記輪郭パターンを構成する各反射点との距離の総和である。

但し、Ｓ１１０で、反射点の位置座標として、４点以上の位置座標を求めることができなかった場合、ＣＰＵ２５ａは、Ｓ１２０に移行することなく、当該車両状態推定処理を一旦終了して、再度、Ｓ１１０の処理を実行することにより、測位をやり直す。

一方、Ｓ１２０での処理を終えると、ＣＰＵ２５ａは、Ｌ字型の二線分算出処理を実行する（Ｓ１３００）。即ち、複数の反射点で表される上記前方物体の輪郭パターンを、最小二乗法を用いて、互いに直交するＬ字型の二本の直線で、フィッティングする。

そして、各直線の端点を求めることで、上記前方物体の輪郭パターンを、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンで近似する。そして、上記座標系Ａにて、この近似図形パターンを構成する各線分の長さと、各線分の向き（上記座標系Ａのｘ軸と線分がなす角度）と、各線分の中点の位置座標と、を求める。

尚、図５は、ＣＰＵ２５ａがＳ１３００で実行するＬ字型の二線分算出処理を表すフローチャートである。
この二線分算出処理を開始すると、ＣＰＵ２５ａは、Ｓ１１０で求められた反射点の数をｎ個（ｎ＞３）として、角度φが大きい順に、反射点に番号Ｎを割り当てる（Ｓ１３１０）。具体的に、Ｓ１３１０では、データとして保持する反射点の位置座標に、番号Ｎを関連付ける処理を行う。

ここでの番号Ｎの割り当て方法は、図６（ａ）に示す通りである。即ち、角度φが大きい反射点の順に、番号Ｎを連番でＮ＝１，２，３，…，ｎ−１，ｎのように割り当てる。一番目の点に、Ｎ＝１を割り当て、二番目の点にＮ＝２を割り当てるといった具合である。

また、このようにしてＳ１３１０での処理を終えると、ＣＰＵ２５ａは、変数ｍの値を２（ｍ＝２）に設定する（Ｓ１３２０）。その後、Ｎ＝１からＮ＝ｍまでの番号が割り当てられた反射点をセグメントＳＥＧ１、Ｎ＝ｍ＋１からＮ＝ｎまでの番号が割り当てられた反射点をセグメントＳＥＧ２に設定する（Ｓ１３３０）。

Ｓ１３３０での処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ１に属する反射点の一群を、最小二乗法により、直線で近似して、セグメントＳＥＧ１に属する反射点の一群の近似直線Ｌ２１を算出すると共に、セグメントＳＥＧ１に属する各反射点から、上記近似直線Ｌ２１までの距離を求めて、各反射点から近似直線Ｌ２１までの距離の総和を、近似誤差として求める（Ｓ１３４０）。

図６（ｂ）は、近似直線Ｌ２１と反射点との距離の求め方を示した説明図である。図６（ｂ）では、セグメントＳＥＧ１に属する反射点の分布の具体例と共に、近似直線Ｌ２１を表す。Ｓ１３４０では、例えば、セグメントＳＥＧ１に属する反射点が、図６（ｂ）に示す分布となっている場合、図中、矢印で表した近似直線Ｌ２１と各反射点との距離を求める。そして、その総和をセグメントＳＥＧ１の近似誤差として求める。

また、Ｓ１３４０の処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ２に属する反射点の一群を、最小二乗法により、Ｓ１３４０で求めた直線と直交する直線で近似し、セグメントＳＥＧ２に属する反射点の一群の近似直線Ｌ２２を算出すると共に、セグメントＳＥＧ２に属する各反射点から、算出した近似直線Ｌ２２までの距離を求めて、各反射点から近似直線Ｌ２２までの距離の総和を、近似誤差として求める（Ｓ１３５０）。

図６（ａ）は、ｎ＝１２及びｍ＝６のときの近似直線Ｌ２１，Ｌ２２の求め方を示した説明図である。図６（ａ）に示すように、本実施例では、まず、セグメントＳＥＧ１の反射点である１番目から６番目までの反射点に対して、最小二乗法で近似直線を求める。ここで求められた近似直線がｙ＝ａｘ＋ｂであるとすると、セグメントＳＥＧ２の反射点群に対して近似する直線は、セグメントＳＥＧ１との直交が条件なので、ｙ＝−（１／ａ）ｘ＋ｃとなる。従って、Ｓ１３４０では、パラメータａ，ｂを算出し、Ｓ１３５０では、パラメータｃを算出する。

そして、算出した一対の近似直線Ｌ２１，Ｌ２２の端点の位置座標を求めることで、近似直線を線分化し、上記前方物体の輪郭パターンに対応する近似図形パターンとして、近似直線Ｌ２１，Ｌ２２から抽出した線分を接続してなるＬ字状の図形パターンを求める。そして、この近似図形パターンを構成する各線分の長さ、向き、及び、中点の位置座標を、上記座標系Ａで算出する（Ｓ１３６０）。

尚、端点の位置座標の求め方は、次の通りである。まず、直交する各直線Ｌ２１，２２の端点の位置座標として、直線Ｌ２１，Ｌ２２の交点の位置座標を求める。そして、図６（ｂ）に示すように、Ｎ＝１が割り当てられた反射点から直線Ｌ２１への垂線と直線Ｌ２１との交点の位置座標を、直線Ｌ２１のもう一方の端点の位置座標として求める。同様に、Ｎ＝ｎが割り当てられた反射点から直線Ｌ２２への垂線と直線Ｌ２２との交点の位置座標を、直線Ｌ２２のもう一方の端点の位置座標として求める。このようにして、本実施例では、端点の位置座標を求めて、近似直線Ｌ２１，Ｌ２２を線分化し、これら各線分の長さ、向き、及び、中点の位置座標を算出する（Ｓ１３６０）。

また、Ｓ１３６０での処理を終えると、ｍ＝ｎ−２であるか否かを判断する（Ｓ１３７０）。そして、ｍ＝ｎ−２でないと判断すると（Ｓ１３７０：Ｎ）、Ｓ１３８０に移行して、ｍを１加算し（ｍ←ｍ＋１）、その後、Ｓ１３３０に戻る。

一方、ｍ＝ｎ−２であると判断すると（Ｓ１３７０：Ｙ）、これまでに算出したｎ−３組の一対の線分Ｌ２１，Ｌ２２のなかから、セグメントＳＥＧ１の近似誤差及びセグメントＳＥＧ２の近似誤差の総和が最も小さい一対の線分Ｌ２１，Ｌ２２を選び、選んだ一対の線分Ｌ２１，Ｌ２２からなるＬ字状の図形パターンを、Ｓ１１０で測位された反射点群（前方物体の輪郭パターン）に対するＬ字状の近似図形パターンとして最終決定する（Ｓ１３９０）。その後、当該二線分算出処理を終了する。

このようにして、本実施例では、Ｓ１１０で測位された反射点群にて表される前方物体の輪郭パターンを、車両の一面をモデル化した単一の線分から成る図形パターンと、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンと、で近似し、その近似図形パターン（具体的には、各線分の長さ、向き、及び、中点の位置座標）を求める。

図７は、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）、及び、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）の具体例を表した図である。

具体的に、図７（ａ）左図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ａ）右図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。

また、図７（ｂ）左図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ｂ）右図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。

Ｓ１３００での二線分算出処理を終えると、ＣＰＵ２５ａは、Ｓ１４００に移行して、図８に示す事前確率算出処理を実行する。図８は、ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。

本実施例では、ベイズ推定の手法を用いて、Ｓ１２０，Ｓ１３００で求めた各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である蓋然性、車両側面を近似した線分である蓋然性、及び、側面を近似した線分でも前後面を近似した線分でもない線分（以下、その他の線分）である蓋然性、を求めるが、Ｓ１４００では、これらの蓋然性を求めるに当って、ベイズ推定の入力に用いる、各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である事前確率、車両側面を近似した線分である事前確率、及び、その他の線分である事前確率を、ＲＯＭ２５ｂに記録された関数データ（マップ等）に従って算出する。

従って、図８を用いて事前確率算出処理を説明する前に、ここでは、事前確率算出処理で用いられる関数データの構成を、図９を用いて説明する。図９は、ＲＯＭ２５ｂに記録された関数データの構成を、グラフとして視覚化して表した図である。

具体的に、図９（ａ）は、線分長さを入力とし、入力された長さの線分が車両前後面を近似した線分である確率、入力された上記長さの線分が車両側面を近似した線分である確率、及び、入力された上記長さの線分がその他の線分である確率、を出力とする関数データの構成を、グラフとして視覚化して表した図である。

また、図９（ｂ）は、線分の近似誤差を入力とし、入力された近似誤差を有する線分が車両前後面を近似した線分である確率、入力された上記近似誤差を有する線分が車両側面を近似した線分である確率、及び、入力された上記近似誤差を有する線分がその他の線分である確率、を出力とする関数データの構成をグラフとして視覚化して表した図である。

図９（ａ）に示すグラフは、具体的に、縦軸が確率、横軸が長さのグラフであり、図９（ｂ）に示すグラフは、縦軸が確率、横軸が線分の近似誤差のグラフである。また、図９（ａ）、（ｂ）の夫々は、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の確率の変化を表すものである。

この関数データは、設計者が自由に設計できるものであるが、縦軸が確率であるので、図９（ａ）に示す関数データ及び図９（ｂ）に示す関数データの夫々は、車両前後面を近似した線分である確率、車両側面を近似した線分である確率、及び、その他の線分である確率の和が、一定になるように設計される。但し、ここでは、和を１に設定する必要はない。和を１としなくても、和を一定にしておけば、規格化処理を付加することで、後述するベイズ推定において、和を１とする狭義の確率と同様の演算が可能であるからである。

図９（ａ）に示すように、本実施例においては、車両の形状を考慮して、線分長さが短い場合に、当該線分が車両前後面を近似した線分である確率が高くなり、線分長さが長い場合に、当該線分が車両側面を近似した線分である確率が高くなるように、関数データが設計されている。また、線分長さが通常考えられる車両の全長より長い場合には、その他の線分である確率が高くなるように、関数データが設計されている。

具体的に、線分が車両前後面を近似した線分である確率は、線分長さが、一般的な車両の全幅に対応した長さになったときにピークを迎えるように設定されており、線分が車両側面を近似した線分である確率は、線分長さが、一般的な車両の全長に対応した長さになったときにピークを迎えるように設定されている。

また、図９（ｂ）に示すように、本実施例においては、近似誤差が小さいほど、該当する線分が車両前後面を近似した線分である確率及び車両側面を近似した線分である確率が高くなり、近似誤差が大きいほど、該当する線分がその他の線分である確率が高くなるように、関数データが設計されている。

本実施例のＣＰＵ２５ａは、ＲＯＭ２５ｂから、このような関数データを読み出して、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、線分長さ及び近似誤差に基づき、線分が車両前後面を近似した線分である事前確率、線分が車両側面を近似した線分である事前確率、及び、線分がその他の線分である事前確率を求める。

続いて、ＣＰＵ２５ａがＳ１４００で実行する事前確率算出処理の内容を、図８を用いて説明する。
この事前確率算出処理を開始すると、ＣＰＵ２５ａは、まず以降の計算対象となる線分（以下、「対象線分」と称する。）を、Ｓ１２０で求めた線分Ｌ１に設定する（Ｓ１４０５）。更に、以降の計算対象となる車両部位（以下、「対象部位」と称する。）を「側面」に設定する（Ｓ１４１０）。

そして、図９（ａ）に示す関数データを用いて、対象線分の長さに基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４１５）を導出する（Ｓ１４１５）。次に、図９（ｂ）に示す関数データを用いて、対象線分の近似誤差に基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４２０）を導出する（Ｓ１４２０）。そして、確率Ｐ（１４２５）＝Ｐ（１４１５）×Ｐ（１４２０）を計算する（Ｓ１４２５）。

次に、現在の対象線分について、「前後面」を対象部位とした確率Ｐ（１４２５）を導出したかを判断し（Ｓ１４３０）、現在の対象線分について、まだ、「前後面」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１４３５）、Ｓ１４１５に戻る。

一方、既に「前後面」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした確率Ｐ（１４２５）を導出したか否かを判断し（Ｓ１４４０）、現在の対象線分について、まだ、「その他」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１４４５）、Ｓ１４１５に戻る。尚、対象部位が「その他」である場合には、対象線分がその他の線分である確率を求めることになる。

これに対し、既に「その他」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４４０：Ｙ）、現在の対象線分について、「側面」を対象部位とした確率Ｐ（１４２５）、「前後面」を対象部位とした確率Ｐ（１４２５）、及び、「その他」を対象部位とした確率Ｐ（１４２５）の和が値１となるように、規格化して、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）、及び、対象線分がその他の線分である事前確率Ｐ（１４２５）を求める（Ｓ１４５０）。

即ち、本実施例では、このようにして求めた「前後面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）として後続の処理で取り扱う。同様に、「側面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）として取り扱い、「その他」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分がその他の線分である事前確率Ｐ（１４２５）として取り扱う。

また、Ｓ１４５０での処理を終えると、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４５５）、まだ線分Ｌ２１についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１４６０）、Ｓ１４１０に戻る。

一方、既に線分Ｌ２１の確率Ｐ（１４２５）を求めていれば（Ｓ１４５５：Ｙ）、対象線分として線分Ｌ２２を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４６５）、まだ線分Ｌ２２についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１４７０）、Ｓ１４１０に戻る。

そして、既に線分Ｌ２２の確率Ｐ（１４２５）を求めている場合には（Ｓ１４６５：Ｙ）、当該事前確率算出処理を終える。
以下では、このようにして事前確率算出処理で求めた線分Ｌ１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１前後）と表記し、線分Ｌ１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１その他）と表記する。

また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１前後）と表記し、線分Ｌ２１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２前後）と表記し、線分Ｌ２２がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２その他）と表記する。

また、Ｓ１４００にて上記事前確率算出処理を終えると、ＣＰＵ２５ａは、Ｓ１５００に移行し、図１０に示す尤度算出処理を実行する。図１０は、ＣＰＵ２５ａが実行する尤度算出処理を表すフローチャートである。

本実施例では、この尤度算出処理にて、ベイズ推定の入力に用いる、各事前確率Ｐ（１側），Ｐ（１前後），Ｐ（１その他）Ｐ（２１側），Ｐ（２１前後），Ｐ（２１その他），Ｐ（２２側），Ｐ（２２前後），Ｐ（２２その他）に対応する尤度Ｌ（１側），Ｌ（１前後），Ｌ（１その他），Ｌ（２１側），Ｌ（２１前後），Ｌ（２１その他），Ｌ（２２側），Ｌ（２２前後），Ｌ（２２その他）を求めるが、この際には、ＲＯＭ２５ｂに記録された尤度導出のための関数データを用いる。従って、尤度算出処理を説明するに先立っては、図１１を用いて、ＲＯＭ２５ｂに記録された尤度導出のための関数データの構成について説明する。

尚、図１１は、尤度導出のためにＲＯＭ２５ｂに記録された関数データの構成を、グラフにより視覚化して表した図である。図１１（ａ）〜（ｆ）に示すグラフは全て、縦軸に尤度が設定されたものであり、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の尤度の変化を表すものである。この関数データは、設計者が自由に設計できる。

具体的に、図１１（ａ）は、線分の中点位置座標と車両側面の予測位置（詳細後述）との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ａ）に示すように、この関数データは、上記距離が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。

また、図１１（ｂ）は、線分の中点位置座標と車両前後面の予測位置との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｂ）に示すように、この関数データは、上記距離が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。

また、図１１（ｃ）は、線分長さと車両側面長さ（車両全長）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｃ）に示すように、この関数データは、上記差が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。

この他、図１１（ｄ）は、線分長さと車両前後面長さ（車両全幅）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。

また、図１１（ｅ）は、線分の向きと車両の向きの予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。

また、図１１（ｆ）は、線分の向きと車両の進行方向の予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。

続いて、図１０を用いて、ＣＰＵ２５ａがＳ１５００にて実行する尤度算出処理について説明する。この尤度算出処理を開始すると、ＣＰＵ２５ａは、最初に、当該推定装置１に接続された、自車両の運動状態を検出するためのセンサ群からの入力に基づき、前回車両状態推定処理実行時に対する自車両の変位量（δｘ，δｙ，δθ）を求める（Ｓ１５０５）。δｘは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｘ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δｙは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｙ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δθは、今回の座標系Ａのｙ軸が、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａのｙ軸に対し、どれだけ回転した位置にあるかを、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａで評価したものである。

また、この処理を終えると、ＣＰＵ２５ａは、対象線分を線分Ｌ１に設定する（Ｓ１５１０）。更に、対象部位を「側面」に設定する（Ｓ１５１５）。次に、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）を、図１２に示す個別尤度算出処理にて求める（Ｓ３０００）。尚、図１２を用いた個別尤度算出処理の詳細な説明は、後述する。

また、個別尤度算出処理を終えると、ＣＰＵ２５ａは、次に、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５３０）、「前後面」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１５３５）、Ｓ３０００に戻り、この対象部位について個別尤度算出処理を実行する。

一方、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５４０）。

そして、「その他」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１５４５）、Ｓ３０００に戻る。そして、この対象部位について個別尤度算出処理を実行する。尚、対象部位が「その他」である場合には、個別尤度算出処理で、対象線分がその他の線分である尤度Ｌ（３０００）を求めることになる。

また、「その他」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５５５）、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１５６０）、Ｓ１５１５に戻る。

一方、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を既に求めていれば（Ｓ１５５５：Ｙ）、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５６５）。そして、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１５７０）、Ｓ１５１５に戻る。一方、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていれば（Ｓ１５６５：Ｙ）、当該尤度算出処理を終了する。

以下では、このようにして尤度算出処理で求めた線分Ｌ１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１前後）と表記し、線分Ｌ１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（１その他）と表記する。

また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１前後）と表記し、線分Ｌ２１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２前後）と表記し、線分Ｌ２２がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２２その他）と表記する。

続いて、Ｓ３０００でＣＰＵ２５ａが実行する個別尤度算出処理について説明する。図１２は、ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。
個別尤度算出処理を開始すると、ＣＰＵ２５ａは、まず、前サイクル（前回車両状態推定処理実行時）のＳ１８００の処理で求め、Ｓ１９０でＲＡＭ２５ｃに記憶した車両状態の予測値を取得する（Ｓ３０１０）。尚、Ｓ１８００の処理の詳細は後述するが、Ｓ１８００では、アンセンテッドカルマンフィルタを用いて、次サイクルでの車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２を予測する。また、Ｓ１９０では、これらの予測値を、その時の座標系ＡでＲＡＭ２５ｃに記憶する。

従って、Ｓ３０１０では、車両状態の予測値として、上記車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２の情報を、ＲＡＭ２５ｃから読出し、取得する。

このようにして、Ｓ３０１０での処理を終えると、ＣＰＵ２５ａは、次に、予めＳ１５０５で求めた変位量（δｘ，δｙ，δθ）に基づき、前回車両状態推定処理実行時の座標系Ａで表される上記車両状態の予測値を、今回の座標系Ａに射影して、予測値を座標変換する（Ｓ３０１５）。

また、このようにして、Ｓ３０１５での処理を終えると、ＣＰＵ２５ａは、前サイクルで（前回車両状態推定処理実行時に）予測した車両側面の中点の位置座標（ｘ１，ｙ１）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を算出し、算出した距離を入力として、図１１（ａ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０２０）を導出する（Ｓ３０２０）。

また、前サイクルで予測した車両前後面の中点の位置座標（ｘ２，ｙ２）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を計算し、算出した距離を入力として、図１１（ｂ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０３０）を導出する（Ｓ３０３０）。

Ｓ３０３０での処理を終えると、ＣＰＵ２５ａは、前サイクルで予測した車両側面の長さＮ１と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｃ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０４０）を導出する（Ｓ３０４０）。

また、前サイクルで予測した車両前後面の長さＮ２と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｄ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０５０）を導出する（Ｓ３０５０）。

また、前サイクルで予測した車両の向きΘ１と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｅ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０６０）を導出（Ｓ３０６０）し、更に、前サイクルで予測した車両の進行方向Θ２と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｆ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０７０）を導出する（Ｓ３０７０）。

そして、以上の処理を終えると、Ｓ３０２０〜Ｓ３０７０で算出した各尤度を互いに掛け合わせて、最終的な尤度Ｌ（３０００）を算出する（Ｓ３０８０）。
Ｌ（３０００）＝Ｌ（３０２０）×Ｌ（３０３０）×Ｌ（３０４０）
×Ｌ（３０５０）×Ｌ（３０６０）×Ｌ（３０７０）
その後、ＣＰＵ２５ａは、当該個別尤度算出処理を終了する。また、このような個別尤度算出処理を実行して、Ｓ１５００での尤度算出処理を終えると、ＣＰＵ２５ａは、Ｓ１６００に移行し、図１３に示す事後確率算出処理を実行する。尚、図１３は、ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートである。

本実施例では、この事後確率算出処理を実行することにより、Ｓ１４００で求めた事前確率及びＳ１５００で求めた尤度を用いて、ベイズ推定の手法により、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、当該線分が車両側面を近似した線分である蓋然性（事後確率）、当該線分が車両前後面を近似した線分である蓋然性（事後確率）、及び、当該線分がその他の線分である蓋然性（事後確率）を求める。

続いて、この事後確率算出処理の内容について、図１３を用いて詳述する。事後確率算出処理を開始すると、ＣＰＵ２５ａは、まず、対象線分を線分Ｌ１に設定する（Ｓ１６０５）。続いて、対象部位を「側面」に設定する（Ｓ１６１０）。

次に、対象線分が対象部位を近似した線分である事前確率Ｐ（１４２５）と、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）とを用いて、ベイズ推定の式により、対象線分が対象部位を近似した線分である事後確率Ｐ（１６１５）を求める（Ｓ１６１５）。

Ｓ１６１５では、対象線分が線分Ｌ１であり対象部位が「側面」である場合、事後確率Ｐ（１６１５）を次式に従って算出する。尚、ここでは、対象線分が線分Ｌ１であり対象部位が「側面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両側面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１側）と表記する。

また、この処理を終えると、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６３０）、対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１６３５）、Ｓ１６１５に戻る。そして、対象線分が車両「前後面」を近似した線分である事後確率Ｐ（１６１５）を求める。

対象線分が線分Ｌ１であり対象部位が「前後面」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「前後面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両前後面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１前後）と表記する。

一方、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６３０：Ｙ）、ＣＰＵ２５ａは、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６４０）、現在の対象線分について、対象線分がその他の線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１６４５）、Ｓ１６１５に戻る。そして、対象線分がその他の線分である事後確率Ｐ（１６１５）を求める。

対象線分が線分Ｌ１であり対象部位が「その他」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「その他」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１がその他の線分である事後確率Ｐ（１６１５）を、特にＰｏ（１その他）と表記する。

また、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、事後確率Ｐ（１６１５）を算出したか否かを判断し（Ｓ１６５５）、事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１６６０）、Ｓ１６１０に戻る。

そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２１が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１側）、線分Ｌ２１が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１前後）、線分Ｌ２１がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１その他）を算出する。

また、対象線分として線分Ｌ２１を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６５５：Ｙ）、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したか否かを判断する（Ｓ１６６５）、そして、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１６７０）、Ｓ１６１０に戻る。

そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２２が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２側）、線分Ｌ２２が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２前後）、線分Ｌ２２がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２その他）を算出する。

また、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６６５：Ｙ）、当該事後確率算出処理を終了する。また、Ｓ１６００で、このようにして事後確率算出処理を終了すると、ＣＰＵ２５ａは、図１４に示す選択処理を実行する（Ｓ１７００）。図１４は、ＣＰＵ２５ａが実行する選択処理を表すフローチャートである。

選択処理を開始すると、ＣＰＵ２５ａは、Ｓ１６００で求められた事後確率Ｐｏ(１側)、又は、事後確率Ｐｏ(１前後)、又は、事後確率Ｐｏ(２１側)及び事後確率Ｐｏ(２２前後)の両方、又は、事後確率Ｐｏ(２１前後)及び事後確率Ｐｏ(２２側)の両方、が所定の閾値Ｔｈ以上であるか否かを判定する（Ｓ１７１０）。即ち、ここでは、条件式「Ｐｏ（１側）≧Ｔｈ」、条件式「Ｐｏ(１前後)≧Ｔｈ」、条件式「Ｐｏ(２１側)，Ｐｏ(２２前後)≧Ｔｈ」、条件式「Ｐｏ(２１前後)，Ｐｏ(２２側)≧Ｔｈ」の計四つの条件式の内、少なくとも一つが満足されているか否かを判定する。尚、閾値Ｔｈは、０＜Ｔｈ＜１を満足する範囲内で設計者が自由に設定すればよい。

そして、閾値Ｔｈ以上ではないと判定すると（Ｓ１７１０：Ｎ）、Ｓ１７２０〜Ｓ１７８０の処理を実行することなく、当該選択処理を終了する。
一方、閾値Ｔｈ以上であると判定すると（Ｓ１７１０：Ｙ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１側）が、事後確率Ｐｏ（１側），Ｐｏ(１前後)，Ｐｏ(２１側)，Ｐｏ(２２前後)，Ｐｏ(２１前後)，Ｐｏ（２２側）の集合の中で、最大であるか否かを判断する（Ｓ１７２０）。

そして、事後確率Ｐｏ（１側）が最大であると判断すると（Ｓ１７２０：Ｙ）、線分Ｌ１が車両側面を近似した線分であると判定して、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７３０）。その後、当該選択処理を終了する。

一方、事後確率Ｐｏ（１側）が最大ではないと判断すると（Ｓ１７２０：Ｎ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１前後）が上記集合の中で最大であるか否かを判断する（Ｓ１７４０）。そして、事後確率Ｐｏ（１前後）が最大であると判断すると（Ｓ１７４０：Ｙ）、線分Ｌ１が車両前後面を近似した線分であると判定し、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７５０）。その後、当該選択処理を終了する。

この他、事後確率Ｐｏ（１前後）が最大ではないと判断すると（Ｓ１７４０：Ｎ）、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が上記集合の中で最大であるか否かを判断する（Ｓ１７６０）。

そして、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が最大であると判断すると（Ｓ１７６０：Ｙ）、線分Ｌ２１が車両側面を近似した線分であり、線分Ｌ２２が車両前後面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７７０）。その後、当該選択処理を終了する。

一方、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれとも上記集合の中で最大ではないと判断すると（Ｓ１７６０：Ｎ）、ＣＰＵ２５ａは、線分Ｌ２１が車両前後面を近似した線分であり、線分Ｌ２２が車両側面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７８０）。その後、当該選択処理を終了する。

このようにしてＳ１７００で選択処理を終了すると、ＣＰＵ２５ａは、Ｓ１８００に移行して、図１７に示す状態推定処理を実行する。
本実施例では、この現在過去状態推定処理を実行することにより、Ｓ１１０での測位結果に基づき、現在の車両状態及び次回の車両状態推定処理実行時（即ち、次サイクル）の車両状態を推定する。以下では、この現在過去状態推定処理を、図１７を用いて詳細に説明する前に、その概略について説明する。

状態推定処理では、まず、Ｓ１７００で実行した選択処理の判定結果と、Ｓ１２０，Ｓ１３００で算出した各線分Ｌ１，Ｌ２１，Ｌ２２の中点の位置座標、向き、及び長さの算出結果とに従い、前方物体を車両とみなして、上記座標系Ａで、その前方車両の中心点の位置座標（ｘ０_m，ｙ０_m）、車両の向き（車両前後方向軸線の座標系Ａｘ軸とのなす角度）θ_m、車両の全長Ｌ_m及び全幅Ｗ_mを求める。

そして、これらの観測値ｚ

を、後述する座標系Ｂに置換した後、この座標変換後の観測値ｚを、カルマンフィルタへの入力とし、座標系Ｂでの前方車両の状態量ｘ_s

を、カルマンフィルタの出力として、得る。ここでｘ０は、前方車両中心点の座標系Ｂｘ軸上の座標値、ｖ_xは前方車両の速度ｘ軸成分、ｙ０は、前方車両中心点の座標系Ｂｙ軸上の座標値、ｖ_yは前方車両の速度ｙ軸成分、θは、ｘ軸と車両側面（若しくは車両前後方向軸線）とのなす角度（但し、０≦θ＜πになるように規格化される。）、ωは、角度θの時間微分（つまり角速度）、Ｗは、前方車両全幅、Ｌは、前方車両全長を示す。

具体的に、本実施例では、カルマンフィルタとして、非線形運動モデルに適用可能なアンセンテッドカルマンフィルタ（ＵＫＦ）を用いて、前方車両の状態量ｘ_sを求める。図１５は、観測値ｚから状態量ｘ_sの推定手順を、示した説明図である。

本実施例の推定装置１は、このアンセンテッドカルマンフィルタとしての機能をＣＰＵ２５ａに実現させるためのプログラムを、ＲＯＭ２５ｂに備え、Ｓ１８００にて、ＣＰＵ２５ａにより、このプログラムを実行することによって、状態量ｘ_sを求める。

尚、ここで用いられるアンセンテッドカルマンフィルタは、周知の理論に基づいて構築されるものであり、設計者が設定する運動モデル及び観測モデルによって定まる伝達関数により、入力に対する出力を演算するものである。

従って、以下では、カルマンフィルタ及びアンセンテッドカルマンフィルタの原理についての説明は省略し、アンセンテッドカルマンフィルタ（ＵＫＦ）の説明として、本実施例の推定装置１の動作を実現するに当って、アンセンテッドカルマンフィルタの設計に必要な運動モデル、観測モデル、及び、アンセンテッドカルマンフィルタに入出力されるパラメータについて説明する。

図１５に示すように、本実施例のアンセンテッドカルマンフィルタには、最新サイクルｔの測位結果に基づいて算出された上述の観測値ｚ（ｔ）と、前サイクルｔ−１にて観測値ｚ（ｔ−１）に基づきアンセンテッドカルマンフィルタが演算し出力した状態量ｘ_s（ｔ，ｔ−１）が入力される。そして、この入力に基づいた演算によりアンセンテッドカルマンフィルタからは、現在の車両状態を示す状態量ｘ_s（ｔ，ｔ）と、次サイクルｔ＋１の車両状態を示す状態量ｘ_s（ｔ＋１，ｔ）と、が出力される。但し、本実施例では、時刻（サイクル）ｔ₂での観測値ｚ（ｔ₂）の入力に基づいて求められる時刻（サイクル）ｔ₁での車両の状態量ｘ_sを、特に、ｘ_s（ｔ₁，ｔ₂）と表記する。

このアンセンテッドカルマンフィルタから出力される現在の車両状態を表す状態量ｘ_s（ｔ，ｔ）は、ＣＰＵ２５ａの動作により、推定装置１の制御部２５に接続された車内ネットワークに送出される。また、現在の車両状態を表す状態量ｘ_s（ｔ，ｔ）及び次サイクルの車両状態を表すｘ_s（ｔ＋１，ｔ）は、ＲＡＭ２５ｃに保存される。そして、次サイクルの車両状態を表すｘ_s（ｔ＋１，ｔ）は、上述したように、次サイクルの状態推定処理実行時に用いられる。この他、ＲＡＭ２５ｃに保存された状態量ｘ_s（ｔ，ｔ）は、次サイクル推定処理実行時に、入力とする観測値ｚ（ｔ）を座標系Ｂに置換する際に用いられる。

尚、アンセンテッドカルマンフィルタで用いられる座標系Ｂは、前サイクルの前方車両の位置及び向きを基準にして設定されるｘｙ座標系であり、原点が、前サイクルｔ−１の状態推定処理にて推定された状態量ｘ_s（ｔ−１，ｔ−１）が示す車両中心点の位置座標（ｘ０，ｙ０）に、設定され、ｙ軸が、前サイクルｔ−１の状態推定処理にて推定された状態量ｘ_s（ｔ−１，ｔ−１）が示す車両の向きに（車両の前後方向軸線に沿って）設定され、ｘ軸が、ｙ軸とは直交するように設定されるｘｙ座標系である。座標系Ａから座標系Ｂへの変換に際しては、時刻ｔ−１からｔまでの自車両の変位量のパラメータが必要になるが、このパラメータについては、Ｓ１５０５で求めたパラメータ（δｘ，δｙ，δθ）を用いる。また、座標変換に際しては、ヘルマート変換を用いる。なお今回は、前方車両５０を基準として座標系を設定したが、任意の位置を基準として構わない。

以上には、アンセンテッドカルマンフィルタの入出力について説明したが、次には、本実施例の推定装置１が備えるアンセンテッドカルマンフィルタの運動モデルについて説明する。本実施例の推定装置１は、後述する非線形運動モデルに基づいて設計されたアンセンテッドカルマンフィルタ（以下、「ＵＫＦ１」と表記する。）と、後述する線形運動モデルに基づいて設計されたアンセンテッドカルマンフィルタ（以下、「ＵＫＦ２」と表記する。）と、を備え、ＵＫＦ１及びＵＫＦ２を切り替えて用いて、車両状態を推定する。

ＵＫＦ１は、非線形運動モデルとしての等速円運動モデルに基づいて設計されたものであり、前方車両が旋回運動していると想定される際に用いられる。具体的に、ＵＫＦ１は、次の運動モデルに基づいて設計され、推定装置１に搭載される。尚、等速円運動を用いる理由は、旋回運動を捉えるのに好適だからである。

上式の修飾記号＊は、繰り返し実行される上記車両状態推定処理の一周期Ｔ_s後の状態を示し、Ｔ_sは、当該繰り返し周期である。尚、ここで用いるパラメータｘ，ｙは、前方車両５０の中心位置であり、アンセンテッドカルマンフィルタへの入力ｘ０_m，ｙ０_mに対応する。

上式に示されるｘ^*、ｙ^*、ｖ_x ^*、ｖ_y ^*、θ^*及びω^*と、ｘ、ｙ、ｖ_x、ｖ_y、θ及びωとの関係は、等速円運動という条件から当然に求められる。また、車両の全幅Ｗ及び全長Ｌは、時間に無関係であると考えて、時間が経っても変化しないと定義する。

但し、この運動モデルでは、分母にωがあるので、ωがゼロである時は計算ができない。そこで、ωがゼロのときは、状態量ｘ_sの推定にＵＫＦ２を用いて、運動モデルを、等速円運動モデルから等速直線運動モデルに変更して、状態量ｘ_sを推定する。

即ち、本実施例のＵＫＦ２は、線形運動モデルとしての等速直線運動モデルに基づいて設計されたものであり、前方車両が直線運動していると想定される際に用いられる。具体的に、ＵＫＦ１は、次の運動モデルに基づいて設計され、推定装置１に搭載される。

また、本実施例のＵＫＦ１及びＵＫＦ２における観測モデルは、ｚ（ｔ）に対して、θ_mを０°≦θ_m＜１８０°になるように規格化したものである。なお、観測モデルは、これに限らず、設計者が自由に設計できる。

本実施例では、このような構成のＵＫＦ１，ＵＫＦ２を用いて、前方車両の状態量Ｘ_sを推定する。
続いて、これらのアンセンテッドカルマンフィルタの入力に用いる観測値ｚ（ｔ）の設定方法について簡単に説明する。尚、この設定処理は、具体的に、状態推定処理のＳ１８０５からＳ１８４２の処理にて実現される。

上述したように、状態推定処理では、ｚ（ｔ）を設定するために、選択処理の判定結果と、Ｓ１２０，Ｓ１３００で算出した各線分Ｌ１，Ｌ２１，Ｌ２２の中点の位置座標、向き、及び長さの算出結果とに従い、前方物体を車両とみなして、前方車両の中心点の位置座標（ｘ０_m，ｙ０_m）、車両の向き（車両前後方向軸線の座標系Ａｘ軸とのなす角度）θ_m、車両の全長Ｌ_m及び全幅Ｗ_mを求めるが、前方車両が、図４（ａ）に示す状態であるか、図４（ｂ）に示す状態にあるかによって測位できる車両の部位が変わるため、当然に、測位結果に従って、前方車両の中心点の位置座標（ｘ０_m，ｙ０_m）を求める際の処理手順が変わる。そこで、ここでは、具体例を挙げて、ｚ（ｔ）の設定方法を説明する。

図１６は、選択処理にて線分Ｌ２１が車両側面を近似した線分であると判定され、線分Ｌ２２が車両前後面を近似した線分であると判定された場合のｚ（ｔ）の導出方法を示した説明図である。

図１６に示すように、この場合には、線分Ｌ２１の長さを、車両の全長Ｌ_mに設定し、線分Ｌ２２の長さを、車両の全幅Ｗ_mに設定する。また、ｘ軸と線分Ｌ２１とがなす角度を車両の向きθ_mに設定する。そして、線分Ｌ２２と接続されない線分Ｌ２１の端点と、線分Ｌ２１と接続されない線分Ｌ２２の端点とを結んでできる線分Ｌ３の中点の位置を、前方車両の中心点とし、その中心点のｘ軸の座標値をｘ_m、そのｙ軸の座標値をｙ_mに設定する。このようにして、ｚ（ｔ）を設定する。

また、選択処理にて線分Ｌ２１が車両側面を近似した線分であると判定され、線分Ｌ２２が車両前後面を近似した線分であると判定された場合には、上述した「線分Ｌ２１が車両側面を近似した線分であると判定され、線分Ｌ２２が車両前後面を近似した線分であると判定された場合」のｚ（ｔ）の設定に関する説明において、線分Ｌ２１を、線分Ｌ２２に読み替え、線分Ｌ２２を線分Ｌ２１に読み替えて解釈される手法で、ｘ_m，ｙ_m，θ_m，Ｗ_m，Ｌ_mを設定する。

一方、選択処理にて、線分Ｌ１が車両側面又は車両前後面を近似した線分であると判定された場合には、Ｓ１８０５で肯定判断して、次のように、ｘ_m，ｙ_m，θ_m，Ｗ_m，Ｌ_mを設定する。続いて、図１７を用いてＳ１８００でＣＰＵ２５ａが実行する状態推定処理について詳細に説明する。

この状態推定処理を開始すると、ＣＰＵ２５ａは、Ｓ１７００において単一線分からなる近似図形パターンが、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択されたか否かを判断し（Ｓ１８０５）、単一線分からなる近似図形パターンが選択されたと判断すると（Ｓ１８０５：Ｙ）、Ｓ１８１０に移行する。一方、Ｓ１７００においてＬ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択している場合には（Ｓ１８０５：Ｎ）、Ｓ１８１０〜Ｓ１８４０の処理を実行することなく、Ｓ１８４２に移行する。

Ｓ１８１０に移行すると、ＣＰＵ２５ａは、現在対象としている前方車両５０について現サイクルより過去のサイクルの選択処理で、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択したことがあるか否かを判断する（Ｓ１８１０）。

そして、Ｌ字状の近似図形パターンを選択したことがあれば（Ｓ１８１０：Ｙ）、今回の選択処理にて、線分Ｌ１が車両前後面を近似した線分であると判定されているか否かを判断する（Ｓ１８１５）。そして、線分Ｌ１が車両前後面を近似した線分であると判定されている場合には（Ｓ１８１５：Ｙ）、上記過去において推定された当該車両についての車両側面の長さを、Ｌ_mに設定し、線分Ｌ１の長さを、Ｗ_mに設定して、これらの値Ｌ_m，Ｗ_mがｚ（ｔ）に用いられるようにする（Ｓ１８２０）。その後、Ｓ１８４２に移行する。

一方、線分Ｌ１が車両側面を近似した線分であると判定されている場合には（Ｓ１８１５：Ｎ）、上記過去において推定された当該車両についての車両前後面の長さを、Ｗ_mに設定し、線分Ｌ１の長さを、Ｌ_mに設定する（Ｓ１８２５）。その後、Ｓ１８４２に移行する。

これに対し、現在対象としている前方車両５０について現サイクルより過去のサイクルの選択処理で、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択したことがなければ（Ｓ１８１０：Ｎ）、Ｓ１８３０に移行し、今回の選択処理にて、線分Ｌ１が車両前後面を近似した線分であると判定されているか否かを判断する（Ｓ１８３０）。

そして、線分Ｌ１が車両前後面を近似した線分であると判定されている場合には（Ｓ１８３０：Ｙ）、線分Ｌ１の長さを、Ｗ_mに設定すると共に、車両側面の長さＬ_mに、所定長さ（例えば、図９（ａ）で側面の確率がピークになる長さ）を設定する（Ｓ１８３５）。その後、Ｓ１８４２に移行する。

また、線分Ｌ１が車両側面を近似した線分であると判定されている場合には（Ｓ１８３０：Ｎ）、線分Ｌ１の長さを、Ｌ_mに設定すると共に、車両前後面の長さＷ_mに、所定長さ（例えば、図９（ａ）で前後面の確率がピークになる長さ）を設定する（Ｓ１８４０）。その後、Ｓ１８４２に移行する。

また、Ｓ１８４２に移行すると、ＣＰＵ２５ａは、アンセンテッドカルマンフィルタに入力するための観測値ｚ（ｔ）を設定する。
具体的に、Ｓ１７００の選択処理で、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択している場合（即ち、選択処理にて線分Ｌ２１が車両側面を近似した線分であると判定し、線分Ｌ２２が車両前後面を近似した線分であると判定した場合、又は、線分Ｌ２１が車両側面を近似した線分であると判定し、線分Ｌ２２が車両前後面を近似した線分であると判定した場合）には、上述した手法で、観測値ｚ（ｔ）を設定する。

一方、Ｓ１７００の選択処理で、単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択した場合（即ち、選択処理にて線分Ｌ１が車両側面又は車両前後面を近似した線分であると判定した場合）には、Ｓ１８２０、Ｓ１８２５、Ｓ１８３５及びＳ１８４０のいずれかにて設定したＷ_m，Ｌ_mの情報と線分Ｌ１の情報とに基づき、前方車両の中心点の位置座標（ｘ０_m，ｙ０_m）を含むｚ（ｔ）を算出し、観測値ｚ（ｔ）を設定する（Ｓ１８４２）。具体的には、線分Ｌ１の中点を基点に、線分Ｌ１と直交方向で座標系Ａ（ｘｙ座標系）におけるｙの値が大きくなる向きに沿って、車両全幅Ｗ_m又は車両全長Ｌ_mの半分の長さを移動した位置を、前方車両の中心点とみなして、前方車両の中心点の位置座標（ｘ０_m，ｙ０_m）を算出する。そして、更に、車両の向きθ_mを求めて、観測値ｚ（ｔ）を設定する。

また、Ｓ１８４２にて座標系Ａでの観測値ｚ（ｔ）を設定すると、この観測値ｚ（ｔ）を上述した座標系Ｂに座標変換する。即ち、座標系Ａの観測値ｚ（ｔ）を、前回の車両状態推定処理によって推定された状態量ｘ_s（ｔ−１，ｔ−１）が示す車両中心点の座標（ｘ０，ｙ０）が原点となる座標系Ｂに射影する（Ｓ１８４５）。

また、前回の車両状態推定処理によって推定された現在の車両状態の予測値である状態量ｘ_s（ｔ，ｔ−１）を、前回の座標系Ｂ（即ち、状態量ｘ_s（ｔ−２，ｔ−２）が示す車両中心点の座標（ｘ０，ｙ０）が原点となる座標系Ｂ）から今回の座標系Ｂ（即ち、状態量ｘ_s（ｔ−１，ｔ−１）が示す車両中心点の座標（ｘ０，ｙ０）が原点となる座標系Ｂ）に座標変換する（Ｓ１８４７）。

次に、前回の車両状態推定処理における推定結果である状態量ｘ_s（ｔ−１，ｔ−１）が示す角速度ωがゼロであるかを判断し（Ｓ１８５０）、角速度ωがゼロであったなら（Ｓ１８５０：Ｙ）、Ｓ１８６５で用いる運度モデルを等速直線運動モデルに（換言すると、用いるカルマンフィルタをＵＫＦ２に）設定し（Ｓ１８６０）、角速度ωがゼロでなかったなら（Ｓ１８５０：Ｎ）、Ｓ１８６５で用いる運動モデルを等速円運動モデルに（換言すると、用いるカルマンフィルタをＵＫＦ１に）設定する（Ｓ１８５５）。その後、
Ｓ１８６５に移行する。

また、Ｓ１８６５では、設定した運動モデルに基づいて設計されたアンセンテッドカルマンフィルタに、座標系Ｂへの変換後の観測値Ｚ（ｔ）及び状態量ｘ_s（ｔ，ｔ−１）を入力して、アンセンテッドカルマンフィルタから現在の車両状態を表す状態量ｘ_s（ｔ，ｔ）及び次サイクルの車両状態を表す状態量ｘ_s（ｔ＋１，ｔ）を得る。

即ち、角速度ωがゼロであると判断した場合（Ｓ１８５０：Ｙ）には、ＵＫＦ２を用いて状態量ｘ_s（ｔ，ｔ）及び状態量ｘ_s（ｔ＋１，ｔ）を算出し、角速度ωがゼロでないと判断した場合（Ｓ１８５０：Ｎ）には、ＵＫＦ１を用いて状態量ｘ_s（ｔ，ｔ）及び状態量ｘ_s（ｔ＋１，ｔ）を算出する。

また、Ｓ１８６５では、算出した座標系Ｂでの状態量ｘ_s（ｔ，ｔ）及び状態量ｘ_s（ｔ＋１，ｔ）をＲＡＭ２５ｃに保存する。その後、当該状態推定処理を終了する。
そして、Ｓ１８００に移行すると、Ｓ１９０に移行し、次回の尤度算出処理で用いる車両状態の予測値を、状態量ｘ_s（ｔ＋１，ｔ）に基づき算出して、同じく、ＲＡＭ２５ｃに保存する。

即ち、状態量ｘ_s（ｔ＋１，ｔ）が示す車両全長Ｌを、パラメータＮ１に設定し、状態量ｘ_s（ｔ＋１，ｔ）が示す車両全幅Ｗを、パラメータＮ２に設定すると共に、状態量ｘ_s（ｔ＋１，ｔ）が示す車両の向きθを、今回の座標系Ａに置換して、座標系Ａでの車両の向きθを、パラメータΘ１に設定し、状態量ｘ_s（ｔ＋１，ｔ）が示す車両の速度ベクトル（ｖ_x，ｖ_y）を、座標系Ａに置換し、その座標系Ａでの速度ベクトル（ｖ_x，ｖ_y）が座標系Ａのｘ軸となす角度を、パラメータΘ２に設定する。

また、状態量ｘ_s（ｔ＋１，ｔ）が示す車両の中心点の位置座標（ｘ０，ｙ０）を、今回の座標系Ａに置換して、次サイクルでの車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）を次のように求める。

即ち、座標系Ａでの車両の中心点（ｘ０，ｙ０）を基点とし、車両の前後方向軸線に沿って、座標系Ａにおけるｙの値が減少する向きに、車両全長Ｌの半分の距離を移動した位置の座標を、車両前後面の中点の位置座標（ｘ２，ｙ２）として求める。また、座標系Ａでの車両の中心点（ｘ０，ｙ０）を基点とし、車両の前後方向軸線とは垂直な直線に沿って、座標系Ａにおけるｙの値が減少する向きに、車両全幅Ｗの半分の距離を移動した位置の座標を、車両側面の中点の位置座標（ｘ１，ｙ１）として求める。但し、どちらの向きに進んでもｙの値が変わらなければ、任意の向きに進めばよい。

そして、これらの値（ｘ１，ｙ１）、（ｘ２，ｙ２）、Ｎ１、Ｎ２、Θ１、及び、Θ２を車両状態の予測値として、ＲＡＭ２５ｃに保存する。そして、最後にＳ１８００で得た前方車両５０の現在の情報であるｘ_s（ｔ，ｔ）を車内ネットワークに出力する（Ｓ２００）。そして、一連の処理を終える。

上述したように、本実施例の車両状態推定処理は、旋回運動に対応できるような構成なので、従来技術に比べて使用範囲が格段に広くなっている。よって、車両に搭載すれば衝突回避のための情報として活用できるし、道路に設置する場合でも、従来ではできなかったカーブ付近での設置も可能となる。

尚、上記実施例では、四点以上の反射点が計測されることが後続の処理を実行する条件であった。しかし、計測した反射点の数が二点又は三点でも、推定装置１が前方車両５０の状態を推定できるように、推定装置１を構成してもよい。その方法を説明すると、二点の場合、ＣＰＵ２５ａは、Ｓ１２０の処理では、最小二乗法ではなく、この二点を結んだ線分の長さ、向き及び中点の位置を求める。そして、ＣＰＵ２５ａは、二線分算出処理（Ｓ１３００）を行わず、事前確率算出処理（Ｓ１４００）から選択処理（Ｓ１７００）までの処理で、二線分算出処理の結果に基づいて行う処理も行わない。つまり、Ｓ１２０で求めた線分が、車両前後面又は側面のどちらを近似しているかのみを選択する。その後の処理は、上述した処理と同様である。

また、三点の場合、ＣＰＵ２５ａは、二線分算出処理に代えて次の処理をする。その処理とは、まず、その三点のうちの二点を結んでできる線分は三本あるので、その三本の線分を算出する。そして、その三本の線分のなかから二本を選ぶことで、二線分を算出する。三本の線分のなかから二本を選ぶ方法は、例えば、二線分が作る角度のうちの劣角が、最も９０度に近い二線分の組み合わせにする方法が考えられる。その後のＳ１４００以降の処理は上述した処理と同様である。

また、Ｓ１７１０でＮｏと判断した場合の後続の処理内容について、上では説明しなかったが、この場合には、車両状態推定処理を中断し、前方物体が車両ではないとして、測位をやり直しても良いし、前回の車両状態推定処理で保存したｘ_s（ｔ，ｔ−１）を、現在の車両状態を表す状態量ｘ_s（ｔ，ｔ）として出力するように構成されてもよい。

また、Ｓ１８４５及びＳ１８４７の処理は、ｘ_s（ｔ，ｔ−１）の値がＲＡＭ２５ｃに保存されていなければ、行うことができない。従って、その場合には、以降の処理を行わずに、観測値ｚ（ｔ）の情報を、ＲＡＭ２５ｃに保存した後に一連の処理を終えるように構成されるとよい。そうすれば次回の車両状態推定処理では、Ｓ１８４５及びＳ１８４７の処理が実行可能になる。尚、アンセンテッドカルマンフィルタによる初回演算処理実行時には、速度ｖ_x，ｖ_y、角速度ωとして所定の初期値を設定すると共に、必要な予測値を観測値で代替して、状態量ｘ_sを求めればよい。

この他、尤度算出処理では、車両状態の予測値が必要になるが、車両状態の予測値がＲＡＭ２５ｃに記録されていない場合には、尤度算出処理及び事後確率算出処理を実行することなく、事前確率Ｐをそのまま事後確率Ｐｏとして設定して、選択処理に移行すればよい。

尚、請求項１から７までに記載の推定手段は、本実施例のＳ１４００からＳ１８４２までの処理にて実現されている。また、請求項８から１５までに記載の推定手段は、本実施例のＳ１２０からＳ１８６５までの処理にて実現されている。この他、測位手段は、本実施例のＳ１１０の処理にて実現され、解析手段は、本実施例のＳ１２０からＳ１３００までの処理にて実現され、蓋然性算出手段は、本実施例のＳ１４００からＳ１６００までの処理にて実現されている。また、選択手段は、本実施例のＳ１７００にて実現され、予測手段は、本実施例のＳ１８４５からＳ１８６５まで及びＳ１９０の処理にて実現されている。最後に、本発明は、上記実施例に限定されることなく、種々の態様を採ることができることは言うまでもない。

推定装置１の構成を表すブロック図である。ＣＰＵ２５ａが実行する車両状態推定処理を表すフローチャートである。レーダ波による距離の測定原理及び位置座標の求め方を示した説明図である。反射点の分布についての具体例を表した説明図である。ＣＰＵ２５ａが実行する二線分算出処理を表すフローチャートである。二線分算出処理の具体例を表す図である。反射点に対する、単一線分及び二線分による近似の具体例を表す図である。ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。事前確率算出のための関数データの具体例を表す図である。尤度算出処理のフローチャートを表す図である。尤度算出のための関数データの具体例を表す図である。ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートを表す図である。ＣＰＵ２５ａが実行する選択処理を表すフローチャートを表す図である。カルマンフィルタを用いた演算の手法を表す説明図である。車両の中心点の位置座標の算出方法を示した説明図である。ＣＰＵ２５ａが実行する状態推定処理を表すフローチャートである。

符号の説明

１…推定装置、１１…レーザダイオード、１２…コリメータレンズ、１３…レーザダイオード駆動回路、１５ミラー…、１７モータ…、１９…集光レンズ、２１…フォトダイオート、２２…増幅器、２３…検出回路、２５…制御部、２５ａ…ＣＰＵ、２５ｂ…ＲＯＭ、２５ｃ…ＲＡＭ、５０…前方車両

Claims

前方に位置する車両の状態を推定する推定装置であって、
レーダ波を発射し、このレーダ波が前方物体に反射して戻ってくる反射波を受波して、前記レーダ波を反射した前記前方物体表面の複数地点の位置座標を計測する測位手段と、
車両の輪郭をモデル化した、位置、向き及びサイズが未知の図形パターンで、前記測位手段により計測された複数地点の位置座標で表される前記前方物体の輪郭パターンを近似し、前記図形パターンの位置、向き及びサイズを求める解析手段と、
前記解析手段により求められた位置、向き及びサイズに基づき、前記前方に位置する車両の状態として、前方に位置する車両の位置と、前記車両の向きと、前記車両の全長及び全幅の両方又はいずれか一方と、を推定する推定手段と、
を備えることを特徴とする推定装置。
前記解析手段は、車両の一面をモデル化した単一の線分から成る図形パターンと、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンと、を前記車両の輪郭をモデル化した図形パターンとして用いて、前記各図形パターン毎に、当該図形パターンで、前記前方物体の輪郭パターンを近似して、当該図形パターンの位置、向き及びサイズと、当該図形パターンと前記輪郭パターンとの近似誤差と、を求め、
前記推定手段は、前記解析手段により求められた前記各図形パターンの位置、向き、サイズ及び近似誤差に基づき、前方に位置する車両の位置と、前記車両の向きと、前記車両の全長及び全幅の両方又はいずれか一方と、を推定する構成にされていること
を特徴とする請求項１記載の推定装置。
前記解析手段は、前記各図形パターンの位置、向き、サイズ及び近似誤差として、前記各図形パターン毎に、当該図形パターンを構成する各線分の位置、向き及び長さと、各線分と前記輪郭パターンとの近似誤差と、を求め、
前記推定手段は、前記前方に位置する車両の状態として、前記解析手段により求められた前記各図形パターンを構成する線分の位置、向き、長さ、及び近似誤差に基づき、前方に位置する車両の位置と、前記車両の向きと、前記車両の全長及び全幅の両方又はいずれか一方と、を推定する構成にされていること
を特徴とする請求項２記載の推定装置。
前記推定手段は、
前記解析手段によって位置、向き、長さ、及び近似誤差が求められた三本の前記線分について、各線分毎に、当該線分についての前記位置、向き、長さ、及び近似誤差に基づき、当該線分が、車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を求める蓋然性算出手段と、
前記前方物体が車両であるとみなして、前記蓋然性算出手段の算出結果に基づき、一組の前記図形パターンの中から、前方に位置する車両の輪郭として最も確からしい図形パターンを選択する選択手段と、
を備え、
前記選択手段により選択された図形パターンについて、前記解析手段により求められた前記位置、向き及びサイズに基づき、前記前方に位置する車両の状態を推定する構成にされていること
を特徴とする請求項３記載の推定装置。
前記測位手段は、レーダ波を発射して前記前方物体表面の複数地点の位置座標を計測する処理を繰返し実行する構成にされ、
前記解析手段及び前記推定手段は、前記測位手段と共に繰返し動作する構成にされ、
当該推定装置は、更に、
前記推定手段による前記車両の状態の推定前に、前記推定手段により過去に推定された前記車両の状態に基づき、前記推定手段による次回推定時における前記車両の状態を、所定の運動モデルに従って推定する予測手段
を備え、
前記蓋然性算出手段は、今回計測された前記複数地点の位置座標に基づき前記解析手段により求められた各線分の位置、長さ、向き及び近似誤差と、前記予測手段により推定された前記車両の状態と、に基づき、前記各線分毎に、当該線分が車両前後面を近似した線分である蓋然性、及び、車両側面を近似した線分である蓋然性を求める構成にされていること
を特徴とする請求項４記載の推定装置。
前記蓋然性算出手段は、
前記車両前後面を近似した線分である蓋然性、及び、前記車両側面を近似した線分である蓋然性を、ベイズ推定の手法により求める構成にされ、
前記各線分毎に、
前記解析手段によって求められた当該線分の長さ及び近似誤差に基づき、当該線分が、車両前後面を近似した線分である事前確率、及び、車両側面を近似した線分である事前確率を算出し、
前記解析手段によって求められた当該線分の位置、向き及び長さと、前記予測手段により推定された前記車両の状態と、に基づき、前記各事前確率に対応する尤度を算出し、
前記車両前後面を近似した線分である蓋然性、及び、前記車両側面を近似した線分である蓋然性として、前記算出した各事前確率及び尤度に基づき、ベイズ推定の手法により、当該線分が、車両前後面を近似した線分である事後確率、及び、車両側面を近似した線分である事後確率を、求める構成にされていること
を特徴とする請求項５記載の推定装置。
前記予測手段は、前記推定手段により過去に推定された前記車両の状態に基づき、前記推定手段による次回推定時における前記車両の状態を、前記所定の運動モデルに従って推定すると共に、前記所定の運動モデルに従って、前記次回推定時における前記車両の進行方向を推定する構成にされ、
前記蓋然性算出手段は、前記各線分毎に、
前記予測手段により推定された前記車両の状態及び前記車両の進行方向と、今回計測された前記複数地点の位置座標に基づき前記解析手段により求められた当該線分の向き、位置及び長さと、に基づき、前記各事前確率に対応する尤度を算出する構成にされていること
を特徴とする請求項６に記載の推定装置。
前方に位置する車両の運動状態を推定する推定装置であって、
レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、前記レーダ波を反射した前記車両表面の複数地点の位置座標を計測する測位手段と、
前記測位手段により計測された前記車両表面の複数地点の位置座標に基づき、前記車両の角速度を含む前記車両の運動状態を所定の非線形運動モデルに従って推定する推定手段と、
を備えることを特徴とする推定装置。
前記推定手段は、前記車両の運動状態として、前記車両の角速度以外に、前記車両の位置、前記車両の向き、前記車両の速さ、及び、前記車両の進行方向の内、少なくとも一つを推定する構成にされていること
を特徴とする請求項８記載の推定装置。
前記推定手段は、前記非線形運動モデルが採用されたカルマンフィルタを用いて、前記車両の状態を推定する構成にされていること
を特徴とする請求項８又は請求項９記載の推定装置。
前記カルマンフィルタは、アンセンテッドカルマンフィルタであること
を特徴とする請求項１０記載の推定装置。
前記非線形運動モデルは、等速円運動モデルであること
を特徴とする請求項１０又は請求項１１に記載の推定装置。
前記推定手段は、前回推定した角速度がゼロである場合には、前記車両の状態の推定に用いる運動モデルを、前記非線形運動モデルから、所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、前記車両の状態を推定する構成にされていること
を特徴とする請求項１０から請求項１２の何れかに記載の推定装置。