推定装置
【課題】前方車両の形状及び運動状態を正確に推定する。
【解決手段】推定装置は、レーダ波によって、前方車両表面の複数地点の位置座標を計測し（Ｓ１１０）、この複数地点を単一線分及び二線分で近似する（Ｓ１２０，Ｓ１３００）。そして、各線分毎に、求めた線分長さ及び近似誤差に基づき、線分が車両前後面、側面を近似した線分である事前確率を算出する（Ｓ１４００）。一方、過去において推定された前方車両の位置、向き、速さ、進行方向及び角速度の情報を基に、尤度を算出する（Ｓ１５００）。そしてベイズ推定により、既に求めた事前確率と尤度とから、各線分毎に、線分が車両前後面、側面を近似した線分である事後確率を算出し（Ｓ１６００）、前方車両のサイズ、位置及び向きを推定する（Ｓ１７００）。また、これらの情報を基に、アンセンテッドカルマンフィルタにより、車両の速さ、進行方向及び角速度も合わせて推定する（Ｓ１８００）。
【解決手段】推定装置は、レーダ波によって、前方車両表面の複数地点の位置座標を計測し（Ｓ１１０）、この複数地点を単一線分及び二線分で近似する（Ｓ１２０，Ｓ１３００）。そして、各線分毎に、求めた線分長さ及び近似誤差に基づき、線分が車両前後面、側面を近似した線分である事前確率を算出する（Ｓ１４００）。一方、過去において推定された前方車両の位置、向き、速さ、進行方向及び角速度の情報を基に、尤度を算出する（Ｓ１５００）。そしてベイズ推定により、既に求めた事前確率と尤度とから、各線分毎に、線分が車両前後面、側面を近似した線分である事後確率を算出し（Ｓ１６００）、前方車両のサイズ、位置及び向きを推定する（Ｓ１７００）。また、これらの情報を基に、アンセンテッドカルマンフィルタにより、車両の速さ、進行方向及び角速度も合わせて推定する（Ｓ１８００）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、前方に位置する車両の状態を推定する推定装置に関する。
【背景技術】
【０００２】
従来、車両の状態を推定する装置としては、レーダ波を用いて、離れた位置にある車両の状態を推定するレーダ装置が知られており、この種のレーダ装置としては、例えば、直線道路上に設置され、レーダ波としてのミリ波を所定の範囲に向けて周期的に照射するものが知られている（例えば、特許文献１参照）。
【０００３】
このレーダ装置は、レーダ波が車両に反射して戻ってくる反射波を捉えて、反射波の受波結果に従い、車両の状態を推定する。具体的には、車両が直線運動していると仮定して、線形運動モデルに基づき設計されたカルマンフィルタを用いて、車両の速度や加速度を求め、車両の運動状態を予測する。このレーダ装置によれば、道路を走行する車両の現在位置を知ることができると共に、道路上の車両が運動していれば、車両の未来の運動状態を予測することができる。
【０００４】
従って、このレーダ装置を用いれば、観測値と未来の予測値とを用いて、道路上に車両などが停止しているか否かの判断の精度や、車両速度の算出精度等を高めることができ、車両同士の衝突を予測して、これを回避するための処理を実行することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００２−９９９８６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来では、次のような二つの問題があった。
一つ目の問題は、前方に位置する車両の大きさや向きを正確に検出する技術がないため、前方に位置する車両の大きさや向きなどの情報を、衝突回避に必要十分な程度、得ることができないという問題である。車両の形状がわからなければ、正確に衝突を予測することは困難である。このため、従来技術では、レーダ装置の検出結果に基づいて、衝突回避のための車両制御を実行することに関し、精度的な限界があった。
【０００７】
二つ目の問題は、車両の旋回時の運動を予測する技術がないという問題である。つまり、従来技術では、車両の未来の状態を予測するに際し、車両が直線運動していると仮定して、線形運動モデルに基づき、カルマンフィルタにより車両運動を予測するので、車両が旋回運動している場合等には、その旋回運動を正確に捉えて、車両運動を予測することができないといった問題あった。
【０００８】
本発明は、こうした問題に鑑みなされたものであり、車両の状態を精度良く推定することが可能な推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
かかる目的を達成するためになされた請求項１に記載の推定装置は、レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、レーダ波を反射した車両表面の複数地点の位置座標を計測する測位手段を備え、この測位結果に従って、前方に位置する車両の運動状態を推定する推定装置であって、測位手段により計測された車両表面の複数地点の位置座標に基づき、推定手段により、車両の角速度を含む車両の運動状態を、所定の非線形運動モデルに従って推定する構成にされたものである。
【００１０】
従来では、線形運動モデルを用いて車両の運動状態を推定していたため、車両が旋回運動しているときには、車両の運動状態を正確に推定することができないといった問題があったが、本装置では、非線形運動モデルに基づいて、車両の角速度を含む車両の運動状態を推定するので、車両の旋回運動を正確に捉えることができる。従って、この推定装置は、車両の衝突回避に係る車両制御を行う際に、大変役立つ。
【００１１】
尚、推定手段は、上記車両の運動状態として、車両の角速度以外に、車両の位置、車両の向き、車両の速さ、及び、車両の進行方向の内、少なくとも一つを推定する構成にすることができる（請求項２）。このように、推定装置を構成すれば、車両の旋回運動を詳細に知ることができて、衝突回避に係る車両制御を行う際に、大変役立つ。
【００１２】
また、請求項３に記載の推定装置は、推定手段が、非線形運動モデルが採用されたカルマンフィルタを用いて、車両の状態を推定する構成にされたものである。カルマンフィルタを用いて、車両の運動状態を推定するようにすれば、観測誤差の影響を抑えて、高精度に車両の運動状態を推定することができるといった利点がある。
【００１３】
この他、請求項４に記載の推定装置は、カルマンフィルタが、アンセンテッドカルマンフィルタであることを特徴とするものである。アンセンテッドカルマンフィルタは、非線形運動の運動状態を推定するのに大変優れている。従って、アンセンテッドカルマンフィルタを用いて、車両の運動状態を推定するように推定装置を構成すれば、高精度に運動状態を推定することができる。
【００１４】
また、上記推定装置に採用する非線形運動モデルとしては、等速円運動モデルを挙げることができる（請求項５）。等速円運動モデルを採用してカルマンフィルタを設計し、これに基づいて、車両の運動状態を推定するように上記推定装置を構成すれば、簡単な運動モデルで、高精度に、車両の旋回運動の状態を推定することができる。
【００１５】
また、請求項６に記載の推定装置は、前回推定した角速度がゼロである場合には、車両状態の推定に用いる運動モデルを、非線形運動モデルから所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、車両の状態を推定するように構成されたものである。このように推定装置を構成すれば、旋回運動だけでなく、直進運動についての運動状態をも、正確に推定することができ、広範囲の車両運動を、高精度に推定することができる。
【図面の簡単な説明】
【００１６】
【図１】推定装置１の構成を表すブロック図である。
【図２】ＣＰＵ２５ａが実行する車両状態推定処理を表すフローチャートである。
【図３】レーダ波による距離の測定原理及び位置座標の求め方を示した説明図である。
【図４】反射点の分布についての具体例を表した説明図である。
【図５】ＣＰＵ２５ａが実行する二線分算出処理を表すフローチャートである。
【図６】二線分算出処理の具体例を表す図である。
【図７】反射点に対する、単一線分及び二線分による近似の具体例を表す図である。
【図８】ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。
【図９】事前確率算出のための関数データの具体例を表す図である。
【図１０】尤度算出処理のフローチャートを表す図である。
【図１１】尤度算出のための関数データの具体例を表す図である。
【図１２】ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。
【図１３】ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートを表す図である。
【図１４】ＣＰＵ２５ａが実行する選択処理を表すフローチャートを表す図である。
【図１５】カルマンフィルタを用いた演算の手法を表す説明図である。
【図１６】車両の中心点の位置座標の算出方法を示した説明図である。
【図１７】ＣＰＵ２５ａが実行する状態推定処理を表すフローチャートである。
【発明を実施するための形態】
【００１７】
以下、本発明の実施例について図面と共に説明する。図１は、本発明が適用された推定装置１の構成を表すブロック図であり、四輪自動車の前方部に搭載されている。
本実施例の推定装置１は、図１に示すように、レーザ光を発射するレーザダイオード１１と、レーザダイオードから発射されたレーザ光を平行光線にするコリメータレンズ１２と、レーザダイオード１１を駆動させるレーザダイオード駆動回路１３と、レーザダイオード１１から発射されたレーザ光を反射するミラー１５と、ミラー１５を回転させるモータ１７と、レーザ光を集光する集光レンズ１９と、集光レンズ１９によって集光されたレーザ光を受光して電気信号を発生するフォトダイオード２１と、フォトダイオード２１からの電気信号を増幅する増幅器２２と、レーザダイオード駆動信号及び増幅器２２から出力された信号を取得し、レーザダイオード１１から発射されたレーザ光が反射してフォトダイオード２１によって検出されるまでの時間を検出する検出回路２３と、上述したレーザダイオード駆動回路１３に対するレーザダイオード駆動信号及びモータ１７を駆動させるためのモータ駆動信号を出力して、レーザ光の発射及びモータ１７の回転を制御する共に、モータ１７から入力されるモータ回転位置の情報及び検出回路２３から入力されるレーザ光の飛行時間ΔＴの情報に基づき、各種処理を実行する制御部２５と、から構成されている。
【００１８】
レーザダイオード１１は、前方車両を検知するためのレーダ波として、レーザ光を発射するための装置である。レーザ光は指向性が高いので、本装置によれば、高い空間分解能を得ることができる。また、コリメータレンズ１２は、レーザダイオード１１から発射され発散するレーザ光を、平行光線になるように、レーザ光を屈折させるものである。
【００１９】
ミラー１５は、光を反射するための六角形状の反射面を備えるポリゴンミラーとして構成され、モータ１７は、ＣＰＵ２５ａからのモータ駆動信号に従って、ミラー１５を回転させる構成にされている。本実施例の推定装置１は、この構成により、コリメータレンズ１２を通過してきたレーザ光を、任意の方向へ発射することができる。
【００２０】
また、制御部２５は、ＣＰＵ２５ａ、ＲＯＭ２５ｂ及びＲＡＭ２５ｃ等から構成され、ＣＰＵ２５ａにて、ＲＯＭ２５ｂに記憶された各種プログラムを実行することにより、レーザダイオード１１及びモータ１７を制御すると共に、モータ回転位置の情報及びレーザ光の飛行時間ΔＴの情報を基に、ＲＯＭ２５ｂに記憶されたプログラムによって、レーザ光を反射した物体の状態を推定する。
【００２１】
図２は、ＣＰＵ２５ａが主体となって繰り返し実行する車両状態推定処理を表すフローチャートである。本装置では、レーザ光により検知された前方物体を四輪自動車とみなして以降の処理を行う。
【００２２】
当該車両状態推定処理を開始すると、ＣＰＵ２５ａは、レーザ光を発射してレーザ光の反射点を測位し、その位置座標を求める（Ｓ１１０）。具体的には、レーザレーダ駆動回路１３及びモータ１７を駆動して、ミラー１５を回転させながら、レーザダイオード１１から断続的にパルス光を発射させる。このようなレーザレーダ駆動回路１３及びモータ１７の制御により、前方に位置する車両に向けて、水平方向に照射角度を変化させながら、レーザ光を発射する。
【００２３】
前方物体によって反射されたレーザ光は、集光レンズ１９を通り、フォトダイオード２１に集光される。そして、集光されたレーザ光は、フォトダイオード２１によって電気信号に変換され、増幅器２２によって増幅された後、検出回路２３に送られる。
【００２４】
検出回路２３は、ＣＰＵ２５ａから出力されるレーザダイオード駆動回路１３の駆動信号としてのレーザダイオード駆動信号を取得可能に構成されており、この駆動信号の入力タイミングと、反射光の受光タイミングとから、図３（ａ）に示すように、レーザ光の飛行時間ΔＴとして、レーザ光の発光から受光までの時間（遅延時間：ΔＴ）を検出する。
【００２５】
ＣＰＵ２５ａは、パルス光が発射される度、検出回路２３から入力される遅延時間ΔＴに基づき、レーザ光の速さｖを用いて、反射点までの距離ＤをＤ＝ｖ・ΔＴ／２として求める。そして、算出した距離Ｄと、当該距離Ｄが計測される原因となったパルス光の発射角度Φとから、反射点の位置座標（ｘ，ｙ）として、（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求める。尚、ここで、用いるｘｙ座標系は、車両に固定された推定装置１を基準とした相対座標系である。以下、この座標系を、「座標系Ａ」と表現する。
【００２６】
パルス光の発射角度Φは、モータ回転位置から一意に求めることができる。ここでは、図３（ｂ）に示すように、車両の前後方向の軸線上に、推定装置１が設置されているものとして、当該軸線から前方にｙ軸を設定し、ｙ軸を基準とした角度をφとして定義し、反射点の位置座標（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求めている。
【００２７】
この測位によって位置座標（ｘ，ｙ）が得られる反射点の具体例を表したのが図４である。図４（ａ）は前方車両５０が推定装置１に対して傾いている場合の反射点の分布を表す図であり、図４（ｂ）は前方車両５０の後面５０ａが推定装置１に対して正対している場合の反射点の分布を表す図である。尚、車両側面５０ｂまたは前面５０ｃが推定装置１に正対している場合にも、反射点の分布は、図４（ｂ）に示すようになる。
【００２８】
Ｓ１１０にて、前方物体表面の複数地点の位置座標を求めると、ＣＰＵ２５ａは、次に、Ｓ１１０で得られた複数地点の位置座標（各反射点の位置座標）で表される前方物体の輪郭パターンを、最小二乗法を用いて、一本の直線でフィッティングする。そして、近似直線の端点を求めることで、上記前方物体の輪郭パターンを、単一の線分で近似する。以下では、この近似線分を、線分Ｌ１と呼ぶ。また、前方物体の輪郭パターンを、一本の線分で近似すると、得られた線分Ｌ１について、その長さ及び中点の位置座標、及び、近似誤差を求める（Ｓ１２０）。尚、ここでいう近似誤差は、得られた線分Ｌ１と上記輪郭パターンを構成する各反射点との距離の総和である。
【００２９】
但し、Ｓ１１０で、反射点の位置座標として、４点以上の位置座標を求めることができなかった場合、ＣＰＵ２５ａは、Ｓ１２０に移行することなく、当該車両状態推定処理を一旦終了して、再度、Ｓ１１０の処理を実行することにより、測位をやり直す。
【００３０】
一方、Ｓ１２０での処理を終えると、ＣＰＵ２５ａは、Ｌ字型の二線分算出処理を実行する（Ｓ１３００）。即ち、複数の反射点で表される上記前方物体の輪郭パターンを、最小二乗法を用いて、互いに直交するＬ字型の二本の直線で、フィッティングする。
【００３１】
そして、各直線の端点を求めることで、上記前方物体の輪郭パターンを、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンで近似する。そして、上記座標系Ａにて、この近似図形パターンを構成する各線分の長さと、各線分の向き（上記座標系Ａのｘ軸と線分がなす角度）と、各線分の中点の位置座標と、を求める。
【００３２】
尚、図５は、ＣＰＵ２５ａがＳ１３００で実行するＬ字型の二線分算出処理を表すフローチャートである。
この二線分算出処理を開始すると、ＣＰＵ２５ａは、Ｓ１１０で求められた反射点の数をｎ個（ｎ＞３）として、角度φが大きい順に、反射点に番号Ｎを割り当てる（Ｓ１３１０）。具体的に、Ｓ１３１０では、データとして保持する反射点の位置座標に、番号Ｎを関連付ける処理を行う。
【００３３】
ここでの番号Ｎの割り当て方法は、図６（ａ）に示す通りである。即ち、角度φが大きい反射点の順に、番号Ｎを連番でＮ＝１，２，３，…，ｎ−１，ｎのように割り当てる。一番目の点に、Ｎ＝１を割り当て、二番目の点にＮ＝２を割り当てるといった具合である。
【００３４】
また、このようにしてＳ１３１０での処理を終えると、ＣＰＵ２５ａは、変数ｍの値を２（ｍ＝２）に設定する（Ｓ１３２０）。その後、Ｎ＝１からＮ＝ｍまでの番号が割り当てられた反射点をセグメントＳＥＧ１、Ｎ＝ｍ＋１からＮ＝ｎまでの番号が割り当てられた反射点をセグメントＳＥＧ２に設定する（Ｓ１３３０）。
【００３５】
Ｓ１３３０での処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ１に属する反射点の一群を、最小二乗法により、直線で近似して、セグメントＳＥＧ１に属する反射点の一群の近似直線Ｌ２１を算出すると共に、セグメントＳＥＧ１に属する各反射点から、上記近似直線Ｌ２１までの距離を求めて、各反射点から近似直線Ｌ２１までの距離の総和を、近似誤差として求める（Ｓ１３４０）。
【００３６】
図６（ｂ）は、近似直線Ｌ２１と反射点との距離の求め方を示した説明図である。図６（ｂ）では、セグメントＳＥＧ１に属する反射点の分布の具体例と共に、近似直線Ｌ２１を表す。Ｓ１３４０では、例えば、セグメントＳＥＧ１に属する反射点が、図６（ｂ）に示す分布となっている場合、図中、矢印で表した近似直線Ｌ２１と各反射点との距離を求める。そして、その総和をセグメントＳＥＧ１の近似誤差として求める。
【００３７】
また、Ｓ１３４０の処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ２に属する反射点の一群を、最小二乗法により、Ｓ１３４０で求めた直線と直交する直線で近似し、セグメントＳＥＧ２に属する反射点の一群の近似直線Ｌ２２を算出すると共に、セグメントＳＥＧ２に属する各反射点から、算出した近似直線Ｌ２２までの距離を求めて、各反射点から近似直線Ｌ２２までの距離の総和を、近似誤差として求める（Ｓ１３５０）。
【００３８】
図６（ａ）は、ｎ＝１２及びｍ＝６のときの近似直線Ｌ２１，Ｌ２２の求め方を示した説明図である。図６（ａ）に示すように、本実施例では、まず、セグメントＳＥＧ１の反射点である１番目から６番目までの反射点に対して、最小二乗法で近似直線を求める。ここで求められた近似直線がｙ＝ａｘ＋ｂであるとすると、セグメントＳＥＧ２の反射点群に対して近似する直線は、セグメントＳＥＧ１との直交が条件なので、ｙ＝−（１／ａ）ｘ＋ｃとなる。従って、Ｓ１３４０では、パラメータａ，ｂを算出し、Ｓ１３５０では、パラメータｃを算出する。
【００３９】
そして、算出した一対の近似直線Ｌ２１，Ｌ２２の端点の位置座標を求めることで、近似直線を線分化し、上記前方物体の輪郭パターンに対応する近似図形パターンとして、近似直線Ｌ２１，Ｌ２２から抽出した線分を接続してなるＬ字状の図形パターンを求める。そして、この近似図形パターンを構成する各線分の長さ、向き、及び、中点の位置座標を、上記座標系Ａで算出する（Ｓ１３６０）。
【００４０】
尚、端点の位置座標の求め方は、次の通りである。まず、直交する各直線Ｌ２１，２２の端点の位置座標として、直線Ｌ２１，Ｌ２２の交点の位置座標を求める。そして、図６（ｂ）に示すように、Ｎ＝１が割り当てられた反射点から直線Ｌ２１への垂線と直線Ｌ２１との交点の位置座標を、直線Ｌ２１のもう一方の端点の位置座標として求める。同様に、Ｎ＝ｎが割り当てられた反射点から直線Ｌ２２への垂線と直線Ｌ２２との交点の位置座標を、直線Ｌ２２のもう一方の端点の位置座標として求める。このようにして、本実施例では、端点の位置座標を求めて、近似直線Ｌ２１，Ｌ２２を線分化し、これら各線分の長さ、向き、及び、中点の位置座標を算出する（Ｓ１３６０）。
【００４１】
また、Ｓ１３６０での処理を終えると、ｍ＝ｎ−２であるか否かを判断する（Ｓ１３７０）。そして、ｍ＝ｎ−２でないと判断すると（Ｓ１３７０：Ｎ）、Ｓ１３８０に移行して、ｍを１加算し（ｍ←ｍ＋１）、その後、Ｓ１３３０に戻る。
【００４２】
一方、ｍ＝ｎ−２であると判断すると（Ｓ１３７０：Ｙ）、これまでに算出したｎ−３組の一対の線分Ｌ２１，Ｌ２２のなかから、セグメントＳＥＧ１の近似誤差及びセグメントＳＥＧ２の近似誤差の総和が最も小さい一対の線分Ｌ２１，Ｌ２２を選び、選んだ一対の線分Ｌ２１，Ｌ２２からなるＬ字状の図形パターンを、Ｓ１１０で測位された反射点群（前方物体の輪郭パターン）に対するＬ字状の近似図形パターンとして最終決定する（Ｓ１３９０）。その後、当該二線分算出処理を終了する。
【００４３】
このようにして、本実施例では、Ｓ１１０で測位された反射点群にて表される前方物体の輪郭パターンを、車両の一面をモデル化した単一の線分から成る図形パターンと、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンと、で近似し、その近似図形パターン（具体的には、各線分の長さ、向き、及び、中点の位置座標）を求める。
【００４４】
図７は、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）、及び、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）の具体例を表した図である。
【００４５】
具体的に、図７（ａ）左図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ａ）右図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。
【００４６】
また、図７（ｂ）左図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ｂ）右図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。
【００４７】
Ｓ１３００での二線分算出処理を終えると、ＣＰＵ２５ａは、Ｓ１４００に移行して、図８に示す事前確率算出処理を実行する。図８は、ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。
【００４８】
本実施例では、ベイズ推定の手法を用いて、Ｓ１２０，Ｓ１３００で求めた各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である蓋然性、車両側面を近似した線分である蓋然性、及び、側面を近似した線分でも前後面を近似した線分でもない線分（以下、その他の線分）である蓋然性、を求めるが、Ｓ１４００では、これらの蓋然性を求めるに当って、ベイズ推定の入力に用いる、各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である事前確率、車両側面を近似した線分である事前確率、及び、その他の線分である事前確率を、ＲＯＭ２５ｂに記録された関数データ（マップ等）に従って算出する。
【００４９】
従って、図８を用いて事前確率算出処理を説明する前に、ここでは、事前確率算出処理で用いられる関数データの構成を、図９を用いて説明する。図９は、ＲＯＭ２５ｂに記録された関数データの構成を、グラフとして視覚化して表した図である。
【００５０】
具体的に、図９（ａ）は、線分長さを入力とし、入力された長さの線分が車両前後面を近似した線分である確率、入力された上記長さの線分が車両側面を近似した線分である確率、及び、入力された上記長さの線分がその他の線分である確率、を出力とする関数データの構成を、グラフとして視覚化して表した図である。
【００５１】
また、図９（ｂ）は、線分の近似誤差を入力とし、入力された近似誤差を有する線分が車両前後面を近似した線分である確率、入力された上記近似誤差を有する線分が車両側面を近似した線分である確率、及び、入力された上記近似誤差を有する線分がその他の線分である確率、を出力とする関数データの構成をグラフとして視覚化して表した図である。
【００５２】
図９（ａ）に示すグラフは、具体的に、縦軸が確率、横軸が長さのグラフであり、図９（ｂ）に示すグラフは、縦軸が確率、横軸が線分の近似誤差のグラフである。また、図９（ａ）、（ｂ）の夫々は、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の確率の変化を表すものである。
【００５３】
この関数データは、設計者が自由に設計できるものであるが、縦軸が確率であるので、図９（ａ）に示す関数データ及び図９（ｂ）に示す関数データの夫々は、車両前後面を近似した線分である確率、車両側面を近似した線分である確率、及び、その他の線分である確率の和が、一定になるように設計される。但し、ここでは、和を１に設定する必要はない。和を１としなくても、和を一定にしておけば、規格化処理を付加することで、後述するベイズ推定において、和を１とする狭義の確率と同様の演算が可能であるからである。
【００５４】
図９（ａ）に示すように、本実施例においては、車両の形状を考慮して、線分長さが短い場合に、当該線分が車両前後面を近似した線分である確率が高くなり、線分長さが長い場合に、当該線分が車両側面を近似した線分である確率が高くなるように、関数データが設計されている。また、線分長さが通常考えられる車両の全長より長い場合には、その他の線分である確率が高くなるように、関数データが設計されている。
【００５５】
具体的に、線分が車両前後面を近似した線分である確率は、線分長さが、一般的な車両の全幅に対応した長さになったときにピークを迎えるように設定されており、線分が車両側面を近似した線分である確率は、線分長さが、一般的な車両の全長に対応した長さになったときにピークを迎えるように設定されている。
【００５６】
また、図９（ｂ）に示すように、本実施例においては、近似誤差が小さいほど、該当する線分が車両前後面を近似した線分である確率及び車両側面を近似した線分である確率が高くなり、近似誤差が大きいほど、該当する線分がその他の線分である確率が高くなるように、関数データが設計されている。
【００５７】
本実施例のＣＰＵ２５ａは、ＲＯＭ２５ｂから、このような関数データを読み出して、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、線分長さ及び近似誤差に基づき、線分が車両前後面を近似した線分である事前確率、線分が車両側面を近似した線分である事前確率、及び、線分がその他の線分である事前確率を求める。
【００５８】
続いて、ＣＰＵ２５ａがＳ１４００で実行する事前確率算出処理の内容を、図８を用いて説明する。
この事前確率算出処理を開始すると、ＣＰＵ２５ａは、まず以降の計算対象となる線分（以下、「対象線分」と称する。）を、Ｓ１２０で求めた線分Ｌ１に設定する（Ｓ１４０５）。更に、以降の計算対象となる車両部位（以下、「対象部位」と称する。）を「側面」に設定する（Ｓ１４１０）。
【００５９】
そして、図９（ａ）に示す関数データを用いて、対象線分の長さに基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４１５）を導出する（Ｓ１４１５）。次に、図９（ｂ）に示す関数データを用いて、対象線分の近似誤差に基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４２０）を導出する（Ｓ１４２０）。そして、確率Ｐ（１４２５）＝Ｐ（１４１５）×Ｐ（１４２０）を計算する（Ｓ１４２５）。
【００６０】
次に、現在の対象線分について、「前後面」を対象部位とした確率Ｐ（１４２５）を導出したかを判断し（Ｓ１４３０）、現在の対象線分について、まだ、「前後面」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１４３５）、Ｓ１４１５に戻る。
【００６１】
一方、既に「前後面」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした確率Ｐ（１４２５）を導出したか否かを判断し（Ｓ１４４０）、現在の対象線分について、まだ、「その他」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１４４５）、Ｓ１４１５に戻る。尚、対象部位が「その他」である場合には、対象線分がその他の線分である確率を求めることになる。
【００６２】
これに対し、既に「その他」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４４０：Ｙ）、現在の対象線分について、「側面」を対象部位とした確率Ｐ（１４２５）、「前後面」を対象部位とした確率Ｐ（１４２５）、及び、「その他」を対象部位とした確率Ｐ（１４２５）の和が値１となるように、規格化して、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）、及び、対象線分がその他の線分である事前確率Ｐ（１４２５）を求める（Ｓ１４５０）。
【００６３】
即ち、本実施例では、このようにして求めた「前後面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）として後続の処理で取り扱う。同様に、「側面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）として取り扱い、「その他」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分がその他の線分である事前確率Ｐ（１４２５）として取り扱う。
【００６４】
また、Ｓ１４５０での処理を終えると、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４５５）、まだ線分Ｌ２１についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１４６０）、Ｓ１４１０に戻る。
【００６５】
一方、既に線分Ｌ２１の確率Ｐ（１４２５）を求めていれば（Ｓ１４５５：Ｙ）、対象線分として線分Ｌ２２を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４６５）、まだ線分Ｌ２２についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１４７０）、Ｓ１４１０に戻る。
【００６６】
そして、既に線分Ｌ２２の確率Ｐ（１４２５）を求めている場合には（Ｓ１４６５：Ｙ）、当該事前確率算出処理を終える。
以下では、このようにして事前確率算出処理で求めた線分Ｌ１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１前後）と表記し、線分Ｌ１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１その他）と表記する。
【００６７】
また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１前後）と表記し、線分Ｌ２１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２前後）と表記し、線分Ｌ２２がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２その他）と表記する。
【００６８】
また、Ｓ１４００にて上記事前確率算出処理を終えると、ＣＰＵ２５ａは、Ｓ１５００に移行し、図１０に示す尤度算出処理を実行する。図１０は、ＣＰＵ２５ａが実行する尤度算出処理を表すフローチャートである。
【００６９】
本実施例では、この尤度算出処理にて、ベイズ推定の入力に用いる、各事前確率Ｐ（１側），Ｐ（１前後），Ｐ（１その他）Ｐ（２１側），Ｐ（２１前後），Ｐ（２１その他），Ｐ（２２側），Ｐ（２２前後），Ｐ（２２その他）に対応する尤度Ｌ（１側），Ｌ（１前後），Ｌ（１その他），Ｌ（２１側），Ｌ（２１前後），Ｌ（２１その他），Ｌ（２２側），Ｌ（２２前後），Ｌ（２２その他）を求めるが、この際には、ＲＯＭ２５ｂに記録された尤度導出のための関数データを用いる。従って、尤度算出処理を説明するに先立っては、図１１を用いて、ＲＯＭ２５ｂに記録された尤度導出のための関数データの構成について説明する。
【００７０】
尚、図１１は、尤度導出のためにＲＯＭ２５ｂに記録された関数データの構成を、グラフにより視覚化して表した図である。図１１（ａ）〜（ｆ）に示すグラフは全て、縦軸に尤度が設定されたものであり、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の尤度の変化を表すものである。この関数データは、設計者が自由に設計できる。
【００７１】
具体的に、図１１（ａ）は、線分の中点位置座標と車両側面の予測位置（詳細後述）との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ａ）に示すように、この関数データは、上記距離が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７２】
また、図１１（ｂ）は、線分の中点位置座標と車両前後面の予測位置との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｂ）に示すように、この関数データは、上記距離が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。
【００７３】
また、図１１（ｃ）は、線分長さと車両側面長さ（車両全長）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｃ）に示すように、この関数データは、上記差が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７４】
この他、図１１（ｄ）は、線分長さと車両前後面長さ（車両全幅）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。
【００７５】
また、図１１（ｅ）は、線分の向きと車両の向きの予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７６】
また、図１１（ｆ）は、線分の向きと車両の進行方向の予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７７】
続いて、図１０を用いて、ＣＰＵ２５ａがＳ１５００にて実行する尤度算出処理について説明する。この尤度算出処理を開始すると、ＣＰＵ２５ａは、最初に、当該推定装置１に接続された、自車両の運動状態を検出するためのセンサ群からの入力に基づき、前回車両状態推定処理実行時に対する自車両の変位量（δｘ，δｙ，δθ）を求める（Ｓ１５０５）。δｘは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｘ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δｙは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｙ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δθは、今回の座標系Ａのｙ軸が、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａのｙ軸に対し、どれだけ回転した位置にあるかを、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａで評価したものである。
【００７８】
また、この処理を終えると、ＣＰＵ２５ａは、対象線分を線分Ｌ１に設定する（Ｓ１５１０）。更に、対象部位を「側面」に設定する（Ｓ１５１５）。次に、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）を、図１２に示す個別尤度算出処理にて求める（Ｓ３０００）。尚、図１２を用いた個別尤度算出処理の詳細な説明は、後述する。
【００７９】
また、個別尤度算出処理を終えると、ＣＰＵ２５ａは、次に、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５３０）、「前後面」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１５３５）、Ｓ３０００に戻り、この対象部位について個別尤度算出処理を実行する。
【００８０】
一方、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５４０）。
【００８１】
そして、「その他」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１５４５）、Ｓ３０００に戻る。そして、この対象部位について個別尤度算出処理を実行する。尚、対象部位が「その他」である場合には、個別尤度算出処理で、対象線分がその他の線分である尤度Ｌ（３０００）を求めることになる。
【００８２】
また、「その他」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５５５）、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１５６０）、Ｓ１５１５に戻る。
【００８３】
一方、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を既に求めていれば（Ｓ１５５５：Ｙ）、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５６５）。そして、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１５７０）、Ｓ１５１５に戻る。一方、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていれば（Ｓ１５６５：Ｙ）、当該尤度算出処理を終了する。
【００８４】
以下では、このようにして尤度算出処理で求めた線分Ｌ１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１前後）と表記し、線分Ｌ１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（１その他）と表記する。
【００８５】
また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１前後）と表記し、線分Ｌ２１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２前後）と表記し、線分Ｌ２２がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２２その他）と表記する。
【００８６】
続いて、Ｓ３０００でＣＰＵ２５ａが実行する個別尤度算出処理について説明する。図１２は、ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。
個別尤度算出処理を開始すると、ＣＰＵ２５ａは、まず、前サイクル（前回車両状態推定処理実行時）のＳ１８００の処理で求め、Ｓ１９０でＲＡＭ２５ｃに記憶した車両状態の予測値を取得する（Ｓ３０１０）。尚、Ｓ１８００の処理の詳細は後述するが、Ｓ１８００では、アンセンテッドカルマンフィルタを用いて、次サイクルでの車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２を予測する。また、Ｓ１９０では、これらの予測値を、その時の座標系ＡでＲＡＭ２５ｃに記憶する。
【００８７】
従って、Ｓ３０１０では、車両状態の予測値として、上記車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２の情報を、ＲＡＭ２５ｃから読出し、取得する。
【００８８】
このようにして、Ｓ３０１０での処理を終えると、ＣＰＵ２５ａは、次に、予めＳ１５０５で求めた変位量（δｘ，δｙ，δθ）に基づき、前回車両状態推定処理実行時の座標系Ａで表される上記車両状態の予測値を、今回の座標系Ａに射影して、予測値を座標変換する（Ｓ３０１５）。
【００８９】
また、このようにして、Ｓ３０１５での処理を終えると、ＣＰＵ２５ａは、前サイクルで（前回車両状態推定処理実行時に）予測した車両側面の中点の位置座標（ｘ１，ｙ１）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を算出し、算出した距離を入力として、図１１（ａ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０２０）を導出する（Ｓ３０２０）。
【００９０】
また、前サイクルで予測した車両前後面の中点の位置座標（ｘ２，ｙ２）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を計算し、算出した距離を入力として、図１１（ｂ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０３０）を導出する（Ｓ３０３０）。
【００９１】
Ｓ３０３０での処理を終えると、ＣＰＵ２５ａは、前サイクルで予測した車両側面の長さＮ１と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｃ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０４０）を導出する（Ｓ３０４０）。
【００９２】
また、前サイクルで予測した車両前後面の長さＮ２と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｄ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０５０）を導出する（Ｓ３０５０）。
【００９３】
また、前サイクルで予測した車両の向きΘ１と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｅ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０６０）を導出（Ｓ３０６０）し、更に、前サイクルで予測した車両の進行方向Θ２と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｆ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０７０）を導出する（Ｓ３０７０）。
【００９４】
そして、以上の処理を終えると、Ｓ３０２０〜Ｓ３０７０で算出した各尤度を互いに掛け合わせて、最終的な尤度Ｌ（３０００）を算出する（Ｓ３０８０）。
Ｌ（３０００）＝Ｌ（３０２０）×Ｌ（３０３０）×Ｌ（３０４０）
×Ｌ（３０５０）×Ｌ（３０６０）×Ｌ（３０７０）
その後、ＣＰＵ２５ａは、当該個別尤度算出処理を終了する。また、このような個別尤度算出処理を実行して、Ｓ１５００での尤度算出処理を終えると、ＣＰＵ２５ａは、Ｓ１６００に移行し、図１３に示す事後確率算出処理を実行する。尚、図１３は、ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートである。
【００９５】
本実施例では、この事後確率算出処理を実行することにより、Ｓ１４００で求めた事前確率及びＳ１５００で求めた尤度を用いて、ベイズ推定の手法により、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、当該線分が車両側面を近似した線分である蓋然性（事後確率）、当該線分が車両前後面を近似した線分である蓋然性（事後確率）、及び、当該線分がその他の線分である蓋然性（事後確率）を求める。
【００９６】
続いて、この事後確率算出処理の内容について、図１３を用いて詳述する。事後確率算出処理を開始すると、ＣＰＵ２５ａは、まず、対象線分を線分Ｌ１に設定する（Ｓ１６０５）。続いて、対象部位を「側面」に設定する（Ｓ１６１０）。
【００９７】
次に、対象線分が対象部位を近似した線分である事前確率Ｐ（１４２５）と、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）とを用いて、ベイズ推定の式により、対象線分が対象部位を近似した線分である事後確率Ｐ（１６１５）を求める（Ｓ１６１５）。
【００９８】
Ｓ１６１５では、対象線分が線分Ｌ１であり対象部位が「側面」である場合、事後確率Ｐ（１６１５）を次式に従って算出する。尚、ここでは、対象線分が線分Ｌ１であり対象部位が「側面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両側面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１側）と表記する。
【００９９】
【数１】
また、この処理を終えると、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６３０）、対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１６３５）、Ｓ１６１５に戻る。そして、対象線分が車両「前後面」を近似した線分である事後確率Ｐ（１６１５）を求める。
【０１００】
対象線分が線分Ｌ１であり対象部位が「前後面」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「前後面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両前後面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１前後）と表記する。
【０１０１】
【数２】
一方、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６３０：Ｙ）、ＣＰＵ２５ａは、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６４０）、現在の対象線分について、対象線分がその他の線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１６４５）、Ｓ１６１５に戻る。そして、対象線分がその他の線分である事後確率Ｐ（１６１５）を求める。
【０１０２】
対象線分が線分Ｌ１であり対象部位が「その他」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「その他」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１がその他の線分である事後確率Ｐ（１６１５）を、特にＰｏ（１その他）と表記する。
【０１０３】
【数３】
また、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、事後確率Ｐ（１６１５）を算出したか否かを判断し（Ｓ１６５５）、事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１６６０）、Ｓ１６１０に戻る。
【０１０４】
そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２１が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１側）、線分Ｌ２１が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１前後）、線分Ｌ２１がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１その他）を算出する。
【０１０５】
【数４】
また、対象線分として線分Ｌ２１を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６５５：Ｙ）、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したか否かを判断する（Ｓ１６６５）、そして、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１６７０）、Ｓ１６１０に戻る。
【０１０６】
そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２２が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２側）、線分Ｌ２２が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２前後）、線分Ｌ２２がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２その他）を算出する。
【０１０７】
【数５】
また、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６６５：Ｙ）、当該事後確率算出処理を終了する。また、Ｓ１６００で、このようにして事後確率算出処理を終了すると、ＣＰＵ２５ａは、図１４に示す選択処理を実行する（Ｓ１７００）。図１４は、ＣＰＵ２５ａが実行する選択処理を表すフローチャートである。
【０１０８】
選択処理を開始すると、ＣＰＵ２５ａは、Ｓ１６００で求められた事後確率Ｐｏ(１側)、又は、事後確率Ｐｏ(１前後)、又は、事後確率Ｐｏ(２１側)及び事後確率Ｐｏ(２２前後)の両方、又は、事後確率Ｐｏ(２１前後)及び事後確率Ｐｏ(２２側)の両方、が所定の閾値Ｔｈ以上であるか否かを判定する（Ｓ１７１０）。即ち、ここでは、条件式「Ｐｏ（１側）≧Ｔｈ」、条件式「Ｐｏ(１前後)≧Ｔｈ」、条件式「Ｐｏ(２１側)，Ｐｏ(２２前後)≧Ｔｈ」、条件式「Ｐｏ(２１前後)，Ｐｏ(２２側)≧Ｔｈ」の計四つの条件式の内、少なくとも一つが満足されているか否かを判定する。尚、閾値Ｔｈは、０＜Ｔｈ＜１を満足する範囲内で設計者が自由に設定すればよい。
【０１０９】
そして、閾値Ｔｈ以上ではないと判定すると（Ｓ１７１０：Ｎ）、Ｓ１７２０〜Ｓ１７８０の処理を実行することなく、当該選択処理を終了する。
一方、閾値Ｔｈ以上であると判定すると（Ｓ１７１０：Ｙ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１側）が、事後確率Ｐｏ（１側），Ｐｏ(１前後)，Ｐｏ(２１側)，Ｐｏ(２２前後)，Ｐｏ(２１前後)，Ｐｏ（２２側）の集合の中で、最大であるか否かを判断する（Ｓ１７２０）。
【０１１０】
そして、事後確率Ｐｏ（１側）が最大であると判断すると（Ｓ１７２０：Ｙ）、線分Ｌ１が車両側面を近似した線分であると判定して、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７３０）。その後、当該選択処理を終了する。
【０１１１】
一方、事後確率Ｐｏ（１側）が最大ではないと判断すると（Ｓ１７２０：Ｎ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１前後）が上記集合の中で最大であるか否かを判断する（Ｓ１７４０）。そして、事後確率Ｐｏ（１前後）が最大であると判断すると（Ｓ１７４０：Ｙ）、線分Ｌ１が車両前後面を近似した線分であると判定し、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７５０）。その後、当該選択処理を終了する。
【０１１２】
この他、事後確率Ｐｏ（１前後）が最大ではないと判断すると（Ｓ１７４０：Ｎ）、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が上記集合の中で最大であるか否かを判断する（Ｓ１７６０）。
【０１１３】
そして、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が最大であると判断すると（Ｓ１７６０：Ｙ）、線分Ｌ２１が車両側面を近似した線分であり、線分Ｌ２２が車両前後面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７７０）。その後、当該選択処理を終了する。
【０１１４】
一方、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれとも上記集合の中で最大ではないと判断すると（Ｓ１７６０：Ｎ）、ＣＰＵ２５ａは、線分Ｌ２１が車両前後面を近似した線分であり、線分Ｌ２２が車両側面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７８０）。その後、当該選択処理を終了する。
【０１１５】
このようにしてＳ１７００で選択処理を終了すると、ＣＰＵ２５ａは、Ｓ１８００に移行して、図１７に示す状態推定処理を実行する。
本実施例では、この現在過去状態推定処理を実行することにより、Ｓ１１０での測位結果に基づき、現在の車両状態及び次回の車両状態推定処理実行時（即ち、次サイクル）の車両状態を推定する。以下では、この現在過去状態推定処理を、図１７を用いて詳細に説明する前に、その概略について説明する。
【０１１６】
状態推定処理では、まず、Ｓ１７００で実行した選択処理の判定結果と、Ｓ１２０，Ｓ１３００で算出した各線分Ｌ１，Ｌ２１，Ｌ２２の中点の位置座標、向き、及び長さの算出結果とに従い、前方物体を車両とみなして、上記座標系Ａで、その前方車両の中心点の位置座標（ｘ０m，ｙ０m）、車両の向き（車両前後方向軸線の座標系Ａｘ軸とのなす角度）θm、車両の全長Ｌm及び全幅Ｗmを求める。
【０１１７】
そして、これらの観測値ｚ
【０１１８】
【数６】
を、後述する座標系Ｂに置換した後、この座標変換後の観測値ｚを、カルマンフィルタへの入力とし、座標系Ｂでの前方車両の状態量ｘs
【０１１９】
【数７】
を、カルマンフィルタの出力として、得る。ここでｘ０は、前方車両中心点の座標系Ｂｘ軸上の座標値、ｖxは前方車両の速度ｘ軸成分、ｙ０は、前方車両中心点の座標系Ｂｙ軸上の座標値、ｖyは前方車両の速度ｙ軸成分、θは、ｘ軸と車両側面（若しくは車両前後方向軸線）とのなす角度（但し、０≦θ＜πになるように規格化される。）、ωは、角度θの時間微分（つまり角速度）、Ｗは、前方車両全幅、Ｌは、前方車両全長を示す。
【０１２０】
具体的に、本実施例では、カルマンフィルタとして、非線形運動モデルに適用可能なアンセンテッドカルマンフィルタ（ＵＫＦ）を用いて、前方車両の状態量ｘsを求める。図１５は、観測値ｚから状態量ｘsの推定手順を、示した説明図である。
【０１２１】
本実施例の推定装置１は、このアンセンテッドカルマンフィルタとしての機能をＣＰＵ２５ａに実現させるためのプログラムを、ＲＯＭ２５ｂに備え、Ｓ１８００にて、ＣＰＵ２５ａにより、このプログラムを実行することによって、状態量ｘsを求める。
【０１２２】
尚、ここで用いられるアンセンテッドカルマンフィルタは、周知の理論に基づいて構築されるものであり、設計者が設定する運動モデル及び観測モデルによって定まる伝達関数により、入力に対する出力を演算するものである。
【０１２３】
従って、以下では、カルマンフィルタ及びアンセンテッドカルマンフィルタの原理についての説明は省略し、アンセンテッドカルマンフィルタ（ＵＫＦ）の説明として、本実施例の推定装置１の動作を実現するに当って、アンセンテッドカルマンフィルタの設計に必要な運動モデル、観測モデル、及び、アンセンテッドカルマンフィルタに入出力されるパラメータについて説明する。
【０１２４】
図１５に示すように、本実施例のアンセンテッドカルマンフィルタには、最新サイクルｔの測位結果に基づいて算出された上述の観測値ｚ（ｔ）と、前サイクルｔ−１にて観測値ｚ（ｔ−１）に基づきアンセンテッドカルマンフィルタが演算し出力した状態量ｘs（ｔ，ｔ−１）が入力される。そして、この入力に基づいた演算によりアンセンテッドカルマンフィルタからは、現在の車両状態を示す状態量ｘs（ｔ，ｔ）と、次サイクルｔ＋１の車両状態を示す状態量ｘs（ｔ＋１，ｔ）と、が出力される。但し、本実施例では、時刻（サイクル）ｔ2での観測値ｚ（ｔ2）の入力に基づいて求められる時刻（サイクル）ｔ1での車両の状態量ｘsを、特に、ｘs（ｔ1，ｔ2）と表記する。
【０１２５】
このアンセンテッドカルマンフィルタから出力される現在の車両状態を表す状態量ｘ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を求めればよい。
【０１６４】
この他、尤度算出処理では、車両状態の予測値が必要になるが、車両状態の予測値がＲＡＭ２５ｃに記録されていない場合には、尤度算出処理及び事後確率算出処理を実行することなく、事前確率Ｐをそのまま事後確率Ｐｏとして設定して、選択処理に移行すればよい。
【０１６５】
尚、特許請求の範囲に記載の推定手段は、本実施例のＳ１２０からＳ１８６５までの処理にて実現されている。この他、測位手段は、本実施例のＳ１１０の処理にて実現されている。また、本発明は、上記実施例に限定されることなく、種々の態様を採ることができることは言うまでもない。
【符号の説明】
【０１６６】
１…推定装置、１１…レーザダイオード、１２…コリメータレンズ、１３…レーザダイオード駆動回路、１５ミラー…、１７モータ…、１９…集光レンズ、２１…フォトダイオート、２２…増幅器、２３…検出回路、２５…制御部、２５ａ…ＣＰＵ、２５ｂ…ＲＯＭ、２５ｃ…ＲＡＭ、５０…前方車両
【技術分野】
【０００１】
本発明は、前方に位置する車両の状態を推定する推定装置に関する。
【背景技術】
【０００２】
従来、車両の状態を推定する装置としては、レーダ波を用いて、離れた位置にある車両の状態を推定するレーダ装置が知られており、この種のレーダ装置としては、例えば、直線道路上に設置され、レーダ波としてのミリ波を所定の範囲に向けて周期的に照射するものが知られている（例えば、特許文献１参照）。
【０００３】
このレーダ装置は、レーダ波が車両に反射して戻ってくる反射波を捉えて、反射波の受波結果に従い、車両の状態を推定する。具体的には、車両が直線運動していると仮定して、線形運動モデルに基づき設計されたカルマンフィルタを用いて、車両の速度や加速度を求め、車両の運動状態を予測する。このレーダ装置によれば、道路を走行する車両の現在位置を知ることができると共に、道路上の車両が運動していれば、車両の未来の運動状態を予測することができる。
【０００４】
従って、このレーダ装置を用いれば、観測値と未来の予測値とを用いて、道路上に車両などが停止しているか否かの判断の精度や、車両速度の算出精度等を高めることができ、車両同士の衝突を予測して、これを回避するための処理を実行することができる。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００２−９９９８６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、従来では、次のような二つの問題があった。
一つ目の問題は、前方に位置する車両の大きさや向きを正確に検出する技術がないため、前方に位置する車両の大きさや向きなどの情報を、衝突回避に必要十分な程度、得ることができないという問題である。車両の形状がわからなければ、正確に衝突を予測することは困難である。このため、従来技術では、レーダ装置の検出結果に基づいて、衝突回避のための車両制御を実行することに関し、精度的な限界があった。
【０００７】
二つ目の問題は、車両の旋回時の運動を予測する技術がないという問題である。つまり、従来技術では、車両の未来の状態を予測するに際し、車両が直線運動していると仮定して、線形運動モデルに基づき、カルマンフィルタにより車両運動を予測するので、車両が旋回運動している場合等には、その旋回運動を正確に捉えて、車両運動を予測することができないといった問題あった。
【０００８】
本発明は、こうした問題に鑑みなされたものであり、車両の状態を精度良く推定することが可能な推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
かかる目的を達成するためになされた請求項１に記載の推定装置は、レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、レーダ波を反射した車両表面の複数地点の位置座標を計測する測位手段を備え、この測位結果に従って、前方に位置する車両の運動状態を推定する推定装置であって、測位手段により計測された車両表面の複数地点の位置座標に基づき、推定手段により、車両の角速度を含む車両の運動状態を、所定の非線形運動モデルに従って推定する構成にされたものである。
【００１０】
従来では、線形運動モデルを用いて車両の運動状態を推定していたため、車両が旋回運動しているときには、車両の運動状態を正確に推定することができないといった問題があったが、本装置では、非線形運動モデルに基づいて、車両の角速度を含む車両の運動状態を推定するので、車両の旋回運動を正確に捉えることができる。従って、この推定装置は、車両の衝突回避に係る車両制御を行う際に、大変役立つ。
【００１１】
尚、推定手段は、上記車両の運動状態として、車両の角速度以外に、車両の位置、車両の向き、車両の速さ、及び、車両の進行方向の内、少なくとも一つを推定する構成にすることができる（請求項２）。このように、推定装置を構成すれば、車両の旋回運動を詳細に知ることができて、衝突回避に係る車両制御を行う際に、大変役立つ。
【００１２】
また、請求項３に記載の推定装置は、推定手段が、非線形運動モデルが採用されたカルマンフィルタを用いて、車両の状態を推定する構成にされたものである。カルマンフィルタを用いて、車両の運動状態を推定するようにすれば、観測誤差の影響を抑えて、高精度に車両の運動状態を推定することができるといった利点がある。
【００１３】
この他、請求項４に記載の推定装置は、カルマンフィルタが、アンセンテッドカルマンフィルタであることを特徴とするものである。アンセンテッドカルマンフィルタは、非線形運動の運動状態を推定するのに大変優れている。従って、アンセンテッドカルマンフィルタを用いて、車両の運動状態を推定するように推定装置を構成すれば、高精度に運動状態を推定することができる。
【００１４】
また、上記推定装置に採用する非線形運動モデルとしては、等速円運動モデルを挙げることができる（請求項５）。等速円運動モデルを採用してカルマンフィルタを設計し、これに基づいて、車両の運動状態を推定するように上記推定装置を構成すれば、簡単な運動モデルで、高精度に、車両の旋回運動の状態を推定することができる。
【００１５】
また、請求項６に記載の推定装置は、前回推定した角速度がゼロである場合には、車両状態の推定に用いる運動モデルを、非線形運動モデルから所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、車両の状態を推定するように構成されたものである。このように推定装置を構成すれば、旋回運動だけでなく、直進運動についての運動状態をも、正確に推定することができ、広範囲の車両運動を、高精度に推定することができる。
【図面の簡単な説明】
【００１６】
【図１】推定装置１の構成を表すブロック図である。
【図２】ＣＰＵ２５ａが実行する車両状態推定処理を表すフローチャートである。
【図３】レーダ波による距離の測定原理及び位置座標の求め方を示した説明図である。
【図４】反射点の分布についての具体例を表した説明図である。
【図５】ＣＰＵ２５ａが実行する二線分算出処理を表すフローチャートである。
【図６】二線分算出処理の具体例を表す図である。
【図７】反射点に対する、単一線分及び二線分による近似の具体例を表す図である。
【図８】ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。
【図９】事前確率算出のための関数データの具体例を表す図である。
【図１０】尤度算出処理のフローチャートを表す図である。
【図１１】尤度算出のための関数データの具体例を表す図である。
【図１２】ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。
【図１３】ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートを表す図である。
【図１４】ＣＰＵ２５ａが実行する選択処理を表すフローチャートを表す図である。
【図１５】カルマンフィルタを用いた演算の手法を表す説明図である。
【図１６】車両の中心点の位置座標の算出方法を示した説明図である。
【図１７】ＣＰＵ２５ａが実行する状態推定処理を表すフローチャートである。
【発明を実施するための形態】
【００１７】
以下、本発明の実施例について図面と共に説明する。図１は、本発明が適用された推定装置１の構成を表すブロック図であり、四輪自動車の前方部に搭載されている。
本実施例の推定装置１は、図１に示すように、レーザ光を発射するレーザダイオード１１と、レーザダイオードから発射されたレーザ光を平行光線にするコリメータレンズ１２と、レーザダイオード１１を駆動させるレーザダイオード駆動回路１３と、レーザダイオード１１から発射されたレーザ光を反射するミラー１５と、ミラー１５を回転させるモータ１７と、レーザ光を集光する集光レンズ１９と、集光レンズ１９によって集光されたレーザ光を受光して電気信号を発生するフォトダイオード２１と、フォトダイオード２１からの電気信号を増幅する増幅器２２と、レーザダイオード駆動信号及び増幅器２２から出力された信号を取得し、レーザダイオード１１から発射されたレーザ光が反射してフォトダイオード２１によって検出されるまでの時間を検出する検出回路２３と、上述したレーザダイオード駆動回路１３に対するレーザダイオード駆動信号及びモータ１７を駆動させるためのモータ駆動信号を出力して、レーザ光の発射及びモータ１７の回転を制御する共に、モータ１７から入力されるモータ回転位置の情報及び検出回路２３から入力されるレーザ光の飛行時間ΔＴの情報に基づき、各種処理を実行する制御部２５と、から構成されている。
【００１８】
レーザダイオード１１は、前方車両を検知するためのレーダ波として、レーザ光を発射するための装置である。レーザ光は指向性が高いので、本装置によれば、高い空間分解能を得ることができる。また、コリメータレンズ１２は、レーザダイオード１１から発射され発散するレーザ光を、平行光線になるように、レーザ光を屈折させるものである。
【００１９】
ミラー１５は、光を反射するための六角形状の反射面を備えるポリゴンミラーとして構成され、モータ１７は、ＣＰＵ２５ａからのモータ駆動信号に従って、ミラー１５を回転させる構成にされている。本実施例の推定装置１は、この構成により、コリメータレンズ１２を通過してきたレーザ光を、任意の方向へ発射することができる。
【００２０】
また、制御部２５は、ＣＰＵ２５ａ、ＲＯＭ２５ｂ及びＲＡＭ２５ｃ等から構成され、ＣＰＵ２５ａにて、ＲＯＭ２５ｂに記憶された各種プログラムを実行することにより、レーザダイオード１１及びモータ１７を制御すると共に、モータ回転位置の情報及びレーザ光の飛行時間ΔＴの情報を基に、ＲＯＭ２５ｂに記憶されたプログラムによって、レーザ光を反射した物体の状態を推定する。
【００２１】
図２は、ＣＰＵ２５ａが主体となって繰り返し実行する車両状態推定処理を表すフローチャートである。本装置では、レーザ光により検知された前方物体を四輪自動車とみなして以降の処理を行う。
【００２２】
当該車両状態推定処理を開始すると、ＣＰＵ２５ａは、レーザ光を発射してレーザ光の反射点を測位し、その位置座標を求める（Ｓ１１０）。具体的には、レーザレーダ駆動回路１３及びモータ１７を駆動して、ミラー１５を回転させながら、レーザダイオード１１から断続的にパルス光を発射させる。このようなレーザレーダ駆動回路１３及びモータ１７の制御により、前方に位置する車両に向けて、水平方向に照射角度を変化させながら、レーザ光を発射する。
【００２３】
前方物体によって反射されたレーザ光は、集光レンズ１９を通り、フォトダイオード２１に集光される。そして、集光されたレーザ光は、フォトダイオード２１によって電気信号に変換され、増幅器２２によって増幅された後、検出回路２３に送られる。
【００２４】
検出回路２３は、ＣＰＵ２５ａから出力されるレーザダイオード駆動回路１３の駆動信号としてのレーザダイオード駆動信号を取得可能に構成されており、この駆動信号の入力タイミングと、反射光の受光タイミングとから、図３（ａ）に示すように、レーザ光の飛行時間ΔＴとして、レーザ光の発光から受光までの時間（遅延時間：ΔＴ）を検出する。
【００２５】
ＣＰＵ２５ａは、パルス光が発射される度、検出回路２３から入力される遅延時間ΔＴに基づき、レーザ光の速さｖを用いて、反射点までの距離ＤをＤ＝ｖ・ΔＴ／２として求める。そして、算出した距離Ｄと、当該距離Ｄが計測される原因となったパルス光の発射角度Φとから、反射点の位置座標（ｘ，ｙ）として、（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求める。尚、ここで、用いるｘｙ座標系は、車両に固定された推定装置１を基準とした相対座標系である。以下、この座標系を、「座標系Ａ」と表現する。
【００２６】
パルス光の発射角度Φは、モータ回転位置から一意に求めることができる。ここでは、図３（ｂ）に示すように、車両の前後方向の軸線上に、推定装置１が設置されているものとして、当該軸線から前方にｙ軸を設定し、ｙ軸を基準とした角度をφとして定義し、反射点の位置座標（ｘ，ｙ）＝（Ｄｓｉｎφ、Ｄｃｏｓφ）を求めている。
【００２７】
この測位によって位置座標（ｘ，ｙ）が得られる反射点の具体例を表したのが図４である。図４（ａ）は前方車両５０が推定装置１に対して傾いている場合の反射点の分布を表す図であり、図４（ｂ）は前方車両５０の後面５０ａが推定装置１に対して正対している場合の反射点の分布を表す図である。尚、車両側面５０ｂまたは前面５０ｃが推定装置１に正対している場合にも、反射点の分布は、図４（ｂ）に示すようになる。
【００２８】
Ｓ１１０にて、前方物体表面の複数地点の位置座標を求めると、ＣＰＵ２５ａは、次に、Ｓ１１０で得られた複数地点の位置座標（各反射点の位置座標）で表される前方物体の輪郭パターンを、最小二乗法を用いて、一本の直線でフィッティングする。そして、近似直線の端点を求めることで、上記前方物体の輪郭パターンを、単一の線分で近似する。以下では、この近似線分を、線分Ｌ１と呼ぶ。また、前方物体の輪郭パターンを、一本の線分で近似すると、得られた線分Ｌ１について、その長さ及び中点の位置座標、及び、近似誤差を求める（Ｓ１２０）。尚、ここでいう近似誤差は、得られた線分Ｌ１と上記輪郭パターンを構成する各反射点との距離の総和である。
【００２９】
但し、Ｓ１１０で、反射点の位置座標として、４点以上の位置座標を求めることができなかった場合、ＣＰＵ２５ａは、Ｓ１２０に移行することなく、当該車両状態推定処理を一旦終了して、再度、Ｓ１１０の処理を実行することにより、測位をやり直す。
【００３０】
一方、Ｓ１２０での処理を終えると、ＣＰＵ２５ａは、Ｌ字型の二線分算出処理を実行する（Ｓ１３００）。即ち、複数の反射点で表される上記前方物体の輪郭パターンを、最小二乗法を用いて、互いに直交するＬ字型の二本の直線で、フィッティングする。
【００３１】
そして、各直線の端点を求めることで、上記前方物体の輪郭パターンを、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンで近似する。そして、上記座標系Ａにて、この近似図形パターンを構成する各線分の長さと、各線分の向き（上記座標系Ａのｘ軸と線分がなす角度）と、各線分の中点の位置座標と、を求める。
【００３２】
尚、図５は、ＣＰＵ２５ａがＳ１３００で実行するＬ字型の二線分算出処理を表すフローチャートである。
この二線分算出処理を開始すると、ＣＰＵ２５ａは、Ｓ１１０で求められた反射点の数をｎ個（ｎ＞３）として、角度φが大きい順に、反射点に番号Ｎを割り当てる（Ｓ１３１０）。具体的に、Ｓ１３１０では、データとして保持する反射点の位置座標に、番号Ｎを関連付ける処理を行う。
【００３３】
ここでの番号Ｎの割り当て方法は、図６（ａ）に示す通りである。即ち、角度φが大きい反射点の順に、番号Ｎを連番でＮ＝１，２，３，…，ｎ−１，ｎのように割り当てる。一番目の点に、Ｎ＝１を割り当て、二番目の点にＮ＝２を割り当てるといった具合である。
【００３４】
また、このようにしてＳ１３１０での処理を終えると、ＣＰＵ２５ａは、変数ｍの値を２（ｍ＝２）に設定する（Ｓ１３２０）。その後、Ｎ＝１からＮ＝ｍまでの番号が割り当てられた反射点をセグメントＳＥＧ１、Ｎ＝ｍ＋１からＮ＝ｎまでの番号が割り当てられた反射点をセグメントＳＥＧ２に設定する（Ｓ１３３０）。
【００３５】
Ｓ１３３０での処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ１に属する反射点の一群を、最小二乗法により、直線で近似して、セグメントＳＥＧ１に属する反射点の一群の近似直線Ｌ２１を算出すると共に、セグメントＳＥＧ１に属する各反射点から、上記近似直線Ｌ２１までの距離を求めて、各反射点から近似直線Ｌ２１までの距離の総和を、近似誤差として求める（Ｓ１３４０）。
【００３６】
図６（ｂ）は、近似直線Ｌ２１と反射点との距離の求め方を示した説明図である。図６（ｂ）では、セグメントＳＥＧ１に属する反射点の分布の具体例と共に、近似直線Ｌ２１を表す。Ｓ１３４０では、例えば、セグメントＳＥＧ１に属する反射点が、図６（ｂ）に示す分布となっている場合、図中、矢印で表した近似直線Ｌ２１と各反射点との距離を求める。そして、その総和をセグメントＳＥＧ１の近似誤差として求める。
【００３７】
また、Ｓ１３４０の処理を終えると、ＣＰＵ２５ａは、セグメントＳＥＧ２に属する反射点の一群を、最小二乗法により、Ｓ１３４０で求めた直線と直交する直線で近似し、セグメントＳＥＧ２に属する反射点の一群の近似直線Ｌ２２を算出すると共に、セグメントＳＥＧ２に属する各反射点から、算出した近似直線Ｌ２２までの距離を求めて、各反射点から近似直線Ｌ２２までの距離の総和を、近似誤差として求める（Ｓ１３５０）。
【００３８】
図６（ａ）は、ｎ＝１２及びｍ＝６のときの近似直線Ｌ２１，Ｌ２２の求め方を示した説明図である。図６（ａ）に示すように、本実施例では、まず、セグメントＳＥＧ１の反射点である１番目から６番目までの反射点に対して、最小二乗法で近似直線を求める。ここで求められた近似直線がｙ＝ａｘ＋ｂであるとすると、セグメントＳＥＧ２の反射点群に対して近似する直線は、セグメントＳＥＧ１との直交が条件なので、ｙ＝−（１／ａ）ｘ＋ｃとなる。従って、Ｓ１３４０では、パラメータａ，ｂを算出し、Ｓ１３５０では、パラメータｃを算出する。
【００３９】
そして、算出した一対の近似直線Ｌ２１，Ｌ２２の端点の位置座標を求めることで、近似直線を線分化し、上記前方物体の輪郭パターンに対応する近似図形パターンとして、近似直線Ｌ２１，Ｌ２２から抽出した線分を接続してなるＬ字状の図形パターンを求める。そして、この近似図形パターンを構成する各線分の長さ、向き、及び、中点の位置座標を、上記座標系Ａで算出する（Ｓ１３６０）。
【００４０】
尚、端点の位置座標の求め方は、次の通りである。まず、直交する各直線Ｌ２１，２２の端点の位置座標として、直線Ｌ２１，Ｌ２２の交点の位置座標を求める。そして、図６（ｂ）に示すように、Ｎ＝１が割り当てられた反射点から直線Ｌ２１への垂線と直線Ｌ２１との交点の位置座標を、直線Ｌ２１のもう一方の端点の位置座標として求める。同様に、Ｎ＝ｎが割り当てられた反射点から直線Ｌ２２への垂線と直線Ｌ２２との交点の位置座標を、直線Ｌ２２のもう一方の端点の位置座標として求める。このようにして、本実施例では、端点の位置座標を求めて、近似直線Ｌ２１，Ｌ２２を線分化し、これら各線分の長さ、向き、及び、中点の位置座標を算出する（Ｓ１３６０）。
【００４１】
また、Ｓ１３６０での処理を終えると、ｍ＝ｎ−２であるか否かを判断する（Ｓ１３７０）。そして、ｍ＝ｎ−２でないと判断すると（Ｓ１３７０：Ｎ）、Ｓ１３８０に移行して、ｍを１加算し（ｍ←ｍ＋１）、その後、Ｓ１３３０に戻る。
【００４２】
一方、ｍ＝ｎ−２であると判断すると（Ｓ１３７０：Ｙ）、これまでに算出したｎ−３組の一対の線分Ｌ２１，Ｌ２２のなかから、セグメントＳＥＧ１の近似誤差及びセグメントＳＥＧ２の近似誤差の総和が最も小さい一対の線分Ｌ２１，Ｌ２２を選び、選んだ一対の線分Ｌ２１，Ｌ２２からなるＬ字状の図形パターンを、Ｓ１１０で測位された反射点群（前方物体の輪郭パターン）に対するＬ字状の近似図形パターンとして最終決定する（Ｓ１３９０）。その後、当該二線分算出処理を終了する。
【００４３】
このようにして、本実施例では、Ｓ１１０で測位された反射点群にて表される前方物体の輪郭パターンを、車両の一面をモデル化した単一の線分から成る図形パターンと、車両の前後面及び側面をモデル化した互いに直交する一対の線分を端点で接続して成るＬ字状の図形パターンと、で近似し、その近似図形パターン（具体的には、各線分の長さ、向き、及び、中点の位置座標）を求める。
【００４４】
図７は、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）、及び、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）の具体例を表した図である。
【００４５】
具体的に、図７（ａ）左図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ａ）右図は、測位の結果、図４（ａ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。
【００４６】
また、図７（ｂ）左図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１２０で求められる単一線分の近似図形パターン（線分Ｌ１）を表すものであり、図７（ｂ）右図は、測位の結果、図４（ｂ）に示す反射点の分布が得られたときに、Ｓ１３００で求められるＬ字状の近似図形パターン（線分Ｌ２１，Ｌ２２）を表すものである。
【００４７】
Ｓ１３００での二線分算出処理を終えると、ＣＰＵ２５ａは、Ｓ１４００に移行して、図８に示す事前確率算出処理を実行する。図８は、ＣＰＵ２５ａが実行する事前確率算出処理を表すフローチャートである。
【００４８】
本実施例では、ベイズ推定の手法を用いて、Ｓ１２０，Ｓ１３００で求めた各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である蓋然性、車両側面を近似した線分である蓋然性、及び、側面を近似した線分でも前後面を近似した線分でもない線分（以下、その他の線分）である蓋然性、を求めるが、Ｓ１４００では、これらの蓋然性を求めるに当って、ベイズ推定の入力に用いる、各線分Ｌ１、Ｌ２１、Ｌ２２の夫々が、車両前後面を近似した線分である事前確率、車両側面を近似した線分である事前確率、及び、その他の線分である事前確率を、ＲＯＭ２５ｂに記録された関数データ（マップ等）に従って算出する。
【００４９】
従って、図８を用いて事前確率算出処理を説明する前に、ここでは、事前確率算出処理で用いられる関数データの構成を、図９を用いて説明する。図９は、ＲＯＭ２５ｂに記録された関数データの構成を、グラフとして視覚化して表した図である。
【００５０】
具体的に、図９（ａ）は、線分長さを入力とし、入力された長さの線分が車両前後面を近似した線分である確率、入力された上記長さの線分が車両側面を近似した線分である確率、及び、入力された上記長さの線分がその他の線分である確率、を出力とする関数データの構成を、グラフとして視覚化して表した図である。
【００５１】
また、図９（ｂ）は、線分の近似誤差を入力とし、入力された近似誤差を有する線分が車両前後面を近似した線分である確率、入力された上記近似誤差を有する線分が車両側面を近似した線分である確率、及び、入力された上記近似誤差を有する線分がその他の線分である確率、を出力とする関数データの構成をグラフとして視覚化して表した図である。
【００５２】
図９（ａ）に示すグラフは、具体的に、縦軸が確率、横軸が長さのグラフであり、図９（ｂ）に示すグラフは、縦軸が確率、横軸が線分の近似誤差のグラフである。また、図９（ａ）、（ｂ）の夫々は、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の確率の変化を表すものである。
【００５３】
この関数データは、設計者が自由に設計できるものであるが、縦軸が確率であるので、図９（ａ）に示す関数データ及び図９（ｂ）に示す関数データの夫々は、車両前後面を近似した線分である確率、車両側面を近似した線分である確率、及び、その他の線分である確率の和が、一定になるように設計される。但し、ここでは、和を１に設定する必要はない。和を１としなくても、和を一定にしておけば、規格化処理を付加することで、後述するベイズ推定において、和を１とする狭義の確率と同様の演算が可能であるからである。
【００５４】
図９（ａ）に示すように、本実施例においては、車両の形状を考慮して、線分長さが短い場合に、当該線分が車両前後面を近似した線分である確率が高くなり、線分長さが長い場合に、当該線分が車両側面を近似した線分である確率が高くなるように、関数データが設計されている。また、線分長さが通常考えられる車両の全長より長い場合には、その他の線分である確率が高くなるように、関数データが設計されている。
【００５５】
具体的に、線分が車両前後面を近似した線分である確率は、線分長さが、一般的な車両の全幅に対応した長さになったときにピークを迎えるように設定されており、線分が車両側面を近似した線分である確率は、線分長さが、一般的な車両の全長に対応した長さになったときにピークを迎えるように設定されている。
【００５６】
また、図９（ｂ）に示すように、本実施例においては、近似誤差が小さいほど、該当する線分が車両前後面を近似した線分である確率及び車両側面を近似した線分である確率が高くなり、近似誤差が大きいほど、該当する線分がその他の線分である確率が高くなるように、関数データが設計されている。
【００５７】
本実施例のＣＰＵ２５ａは、ＲＯＭ２５ｂから、このような関数データを読み出して、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、線分長さ及び近似誤差に基づき、線分が車両前後面を近似した線分である事前確率、線分が車両側面を近似した線分である事前確率、及び、線分がその他の線分である事前確率を求める。
【００５８】
続いて、ＣＰＵ２５ａがＳ１４００で実行する事前確率算出処理の内容を、図８を用いて説明する。
この事前確率算出処理を開始すると、ＣＰＵ２５ａは、まず以降の計算対象となる線分（以下、「対象線分」と称する。）を、Ｓ１２０で求めた線分Ｌ１に設定する（Ｓ１４０５）。更に、以降の計算対象となる車両部位（以下、「対象部位」と称する。）を「側面」に設定する（Ｓ１４１０）。
【００５９】
そして、図９（ａ）に示す関数データを用いて、対象線分の長さに基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４１５）を導出する（Ｓ１４１５）。次に、図９（ｂ）に示す関数データを用いて、対象線分の近似誤差に基づき、この対象線分が対象部位を近似した線分である確率Ｐ（１４２０）を導出する（Ｓ１４２０）。そして、確率Ｐ（１４２５）＝Ｐ（１４１５）×Ｐ（１４２０）を計算する（Ｓ１４２５）。
【００６０】
次に、現在の対象線分について、「前後面」を対象部位とした確率Ｐ（１４２５）を導出したかを判断し（Ｓ１４３０）、現在の対象線分について、まだ、「前後面」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１４３５）、Ｓ１４１５に戻る。
【００６１】
一方、既に「前後面」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした確率Ｐ（１４２５）を導出したか否かを判断し（Ｓ１４４０）、現在の対象線分について、まだ、「その他」を対象部位とした確率Ｐ（１４２５）を導出していなければ（Ｓ１４４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１４４５）、Ｓ１４１５に戻る。尚、対象部位が「その他」である場合には、対象線分がその他の線分である確率を求めることになる。
【００６２】
これに対し、既に「その他」を対象部位とした確率Ｐ（１４２５）を導出していれば（Ｓ１４４０：Ｙ）、現在の対象線分について、「側面」を対象部位とした確率Ｐ（１４２５）、「前後面」を対象部位とした確率Ｐ（１４２５）、及び、「その他」を対象部位とした確率Ｐ（１４２５）の和が値１となるように、規格化して、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）、及び、対象線分がその他の線分である事前確率Ｐ（１４２５）を求める（Ｓ１４５０）。
【００６３】
即ち、本実施例では、このようにして求めた「前後面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「前後面」を近似した線分である事前確率Ｐ（１４２５）として後続の処理で取り扱う。同様に、「側面」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分が車両「側面」を近似した線分である事前確率Ｐ（１４２５）として取り扱い、「その他」を対象部位とした確率Ｐ（１４２５）の規格化後の値を、対象線分がその他の線分である事前確率Ｐ（１４２５）として取り扱う。
【００６４】
また、Ｓ１４５０での処理を終えると、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４５５）、まだ線分Ｌ２１についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１４６０）、Ｓ１４１０に戻る。
【００６５】
一方、既に線分Ｌ２１の確率Ｐ（１４２５）を求めていれば（Ｓ１４５５：Ｙ）、対象線分として線分Ｌ２２を設定して、確率Ｐ（１４２５）を求めたか否かを判断し（Ｓ１４６５）、まだ線分Ｌ２２についての確率Ｐ（１４２５）を求めていなければ（Ｓ１４６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１４７０）、Ｓ１４１０に戻る。
【００６６】
そして、既に線分Ｌ２２の確率Ｐ（１４２５）を求めている場合には（Ｓ１４６５：Ｙ）、当該事前確率算出処理を終える。
以下では、このようにして事前確率算出処理で求めた線分Ｌ１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１前後）と表記し、線分Ｌ１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（１その他）と表記する。
【００６７】
また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１前後）と表記し、線分Ｌ２１がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２前後）と表記し、線分Ｌ２２がその他の線分である事前確率Ｐ（１４２５）を、事前確率Ｐ（２２その他）と表記する。
【００６８】
また、Ｓ１４００にて上記事前確率算出処理を終えると、ＣＰＵ２５ａは、Ｓ１５００に移行し、図１０に示す尤度算出処理を実行する。図１０は、ＣＰＵ２５ａが実行する尤度算出処理を表すフローチャートである。
【００６９】
本実施例では、この尤度算出処理にて、ベイズ推定の入力に用いる、各事前確率Ｐ（１側），Ｐ（１前後），Ｐ（１その他）Ｐ（２１側），Ｐ（２１前後），Ｐ（２１その他），Ｐ（２２側），Ｐ（２２前後），Ｐ（２２その他）に対応する尤度Ｌ（１側），Ｌ（１前後），Ｌ（１その他），Ｌ（２１側），Ｌ（２１前後），Ｌ（２１その他），Ｌ（２２側），Ｌ（２２前後），Ｌ（２２その他）を求めるが、この際には、ＲＯＭ２５ｂに記録された尤度導出のための関数データを用いる。従って、尤度算出処理を説明するに先立っては、図１１を用いて、ＲＯＭ２５ｂに記録された尤度導出のための関数データの構成について説明する。
【００７０】
尚、図１１は、尤度導出のためにＲＯＭ２５ｂに記録された関数データの構成を、グラフにより視覚化して表した図である。図１１（ａ）〜（ｆ）に示すグラフは全て、縦軸に尤度が設定されたものであり、点線が「側面」、実線が「前後面」、一点鎖線が「その他」の尤度の変化を表すものである。この関数データは、設計者が自由に設計できる。
【００７１】
具体的に、図１１（ａ）は、線分の中点位置座標と車両側面の予測位置（詳細後述）との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ａ）に示すように、この関数データは、上記距離が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７２】
また、図１１（ｂ）は、線分の中点位置座標と車両前後面の予測位置との距離を入力とし、入力された距離に対応する線分が車両前後面を近似した線分である尤度、入力された上記距離に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記距離に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｂ）に示すように、この関数データは、上記距離が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。
【００７３】
また、図１１（ｃ）は、線分長さと車両側面長さ（車両全長）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。図１１（ｃ）に示すように、この関数データは、上記差が小さい線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７４】
この他、図１１（ｄ）は、線分長さと車両前後面長さ（車両全幅）の予測値との差（但し、絶対値）を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が小さい線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計されている。
【００７５】
また、図１１（ｅ）は、線分の向きと車両の向きの予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７６】
また、図１１（ｆ）は、線分の向きと車両の進行方向の予測値との差を入力とし、入力された差に対応する線分が車両前後面を近似した線分である尤度、入力された上記差に対応する線分が車両側面を近似した線分である尤度、及び、入力された上記差に対応する線分がその他の線分である尤度を出力とする関数データの構成を、グラフとして視覚化して表した図である。この関数データは、上記差が９０度に近い線分ほど、車両前後面を近似した線分である尤度が大きくなるように設計され、上記差が０度又は１８０度に近い線分ほど、車両側面を近似した線分である尤度が大きくなるように設計されている。
【００７７】
続いて、図１０を用いて、ＣＰＵ２５ａがＳ１５００にて実行する尤度算出処理について説明する。この尤度算出処理を開始すると、ＣＰＵ２５ａは、最初に、当該推定装置１に接続された、自車両の運動状態を検出するためのセンサ群からの入力に基づき、前回車両状態推定処理実行時に対する自車両の変位量（δｘ，δｙ，δθ）を求める（Ｓ１５０５）。δｘは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｘ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δｙは、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａの原点位置を基準として、前回車両状態推定処理実行時の座標系Ａのｙ方向に、今回の座標系Ａの原点がどれだけ移動した位置にあるかを評価したものであり、δθは、今回の座標系Ａのｙ軸が、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａのｙ軸に対し、どれだけ回転した位置にあるかを、前回車両状態推定処理実行時（Ｓ１５０５実行時）の座標系Ａで評価したものである。
【００７８】
また、この処理を終えると、ＣＰＵ２５ａは、対象線分を線分Ｌ１に設定する（Ｓ１５１０）。更に、対象部位を「側面」に設定する（Ｓ１５１５）。次に、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）を、図１２に示す個別尤度算出処理にて求める（Ｓ３０００）。尚、図１２を用いた個別尤度算出処理の詳細な説明は、後述する。
【００７９】
また、個別尤度算出処理を終えると、ＣＰＵ２５ａは、次に、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５３０）、「前後面」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１５３５）、Ｓ３０００に戻り、この対象部位について個別尤度算出処理を実行する。
【００８０】
一方、現在の対象線分について、「前後面」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５３０：Ｙ）、現在の対象線分について、「その他」を対象部位とした尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５４０）。
【００８１】
そして、「その他」を対象部位とした尤度Ｌ（３０００）を求めていないと判断すると（Ｓ１５４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１５４５）、Ｓ３０００に戻る。そして、この対象部位について個別尤度算出処理を実行する。尚、対象部位が「その他」である場合には、個別尤度算出処理で、対象線分がその他の線分である尤度Ｌ（３０００）を求めることになる。
【００８２】
また、「その他」を対象部位とした尤度Ｌ（３０００）を求めたと判断すると（Ｓ１５４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、尤度Ｌ（３０００）を求めたか否かを判断し（Ｓ１５５５）、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１５６０）、Ｓ１５１５に戻る。
【００８３】
一方、線分Ｌ２１を対象線分として尤度Ｌ（３０００）を既に求めていれば（Ｓ１５５５：Ｙ）、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めたか否かを判断する（Ｓ１５６５）。そして、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていなければ（Ｓ１５６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１５７０）、Ｓ１５１５に戻る。一方、線分Ｌ２２を対象線分として尤度Ｌ（３０００）を求めていれば（Ｓ１５６５：Ｙ）、当該尤度算出処理を終了する。
【００８４】
以下では、このようにして尤度算出処理で求めた線分Ｌ１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１側）と表記する。また、線分Ｌ１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（１前後）と表記し、線分Ｌ１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（１その他）と表記する。
【００８５】
また、線分Ｌ２１，Ｌ２２についても同様に、線分Ｌ２１が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１側）と表記し、線分Ｌ２１が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２１前後）と表記し、線分Ｌ２１がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２１その他）と表記し、線分Ｌ２２が車両側面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２側）と表記し、線分Ｌ２２が車両前後面を近似した線分である尤度Ｌ（３０００）を、尤度Ｌ（２２前後）と表記し、線分Ｌ２２がその他の線分である尤度Ｌ（３０００）を、尤度Ｌ（２２その他）と表記する。
【００８６】
続いて、Ｓ３０００でＣＰＵ２５ａが実行する個別尤度算出処理について説明する。図１２は、ＣＰＵ２５ａが実行する個別尤度算出処理を表すフローチャートである。
個別尤度算出処理を開始すると、ＣＰＵ２５ａは、まず、前サイクル（前回車両状態推定処理実行時）のＳ１８００の処理で求め、Ｓ１９０でＲＡＭ２５ｃに記憶した車両状態の予測値を取得する（Ｓ３０１０）。尚、Ｓ１８００の処理の詳細は後述するが、Ｓ１８００では、アンセンテッドカルマンフィルタを用いて、次サイクルでの車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２を予測する。また、Ｓ１９０では、これらの予測値を、その時の座標系ＡでＲＡＭ２５ｃに記憶する。
【００８７】
従って、Ｓ３０１０では、車両状態の予測値として、上記車両側面の中点の位置座標（ｘ１，ｙ１）、車両前後面の中点の位置座標（ｘ２，ｙ２）、車両側面の長さ＝Ｎ１、車両前後面の長さ＝Ｎ２、車両の向き（角度）＝Θ１、及び、車両の進行方向（角度）＝Θ２の情報を、ＲＡＭ２５ｃから読出し、取得する。
【００８８】
このようにして、Ｓ３０１０での処理を終えると、ＣＰＵ２５ａは、次に、予めＳ１５０５で求めた変位量（δｘ，δｙ，δθ）に基づき、前回車両状態推定処理実行時の座標系Ａで表される上記車両状態の予測値を、今回の座標系Ａに射影して、予測値を座標変換する（Ｓ３０１５）。
【００８９】
また、このようにして、Ｓ３０１５での処理を終えると、ＣＰＵ２５ａは、前サイクルで（前回車両状態推定処理実行時に）予測した車両側面の中点の位置座標（ｘ１，ｙ１）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を算出し、算出した距離を入力として、図１１（ａ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０２０）を導出する（Ｓ３０２０）。
【００９０】
また、前サイクルで予測した車両前後面の中点の位置座標（ｘ２，ｙ２）と、今回Ｓ１２０又はＳ１３００で算出した対象線分の中点の位置座標との距離を計算し、算出した距離を入力として、図１１（ｂ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０３０）を導出する（Ｓ３０３０）。
【００９１】
Ｓ３０３０での処理を終えると、ＣＰＵ２５ａは、前サイクルで予測した車両側面の長さＮ１と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｃ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０４０）を導出する（Ｓ３０４０）。
【００９２】
また、前サイクルで予測した車両前後面の長さＮ２と、対象線分の長さとの差の絶対値を算出し、その差（絶対値）を入力として、図１１（ｄ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０５０）を導出する（Ｓ３０５０）。
【００９３】
また、前サイクルで予測した車両の向きΘ１と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｅ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０６０）を導出（Ｓ３０６０）し、更に、前サイクルで予測した車両の進行方向Θ２と、対象線分の向きとの差（角度）を算出し、その差を入力として、図１１（ｆ）に示す関数データを用いて、対象線分が対象部位を近似した線分である尤度Ｌ（３０７０）を導出する（Ｓ３０７０）。
【００９４】
そして、以上の処理を終えると、Ｓ３０２０〜Ｓ３０７０で算出した各尤度を互いに掛け合わせて、最終的な尤度Ｌ（３０００）を算出する（Ｓ３０８０）。
Ｌ（３０００）＝Ｌ（３０２０）×Ｌ（３０３０）×Ｌ（３０４０）
×Ｌ（３０５０）×Ｌ（３０６０）×Ｌ（３０７０）
その後、ＣＰＵ２５ａは、当該個別尤度算出処理を終了する。また、このような個別尤度算出処理を実行して、Ｓ１５００での尤度算出処理を終えると、ＣＰＵ２５ａは、Ｓ１６００に移行し、図１３に示す事後確率算出処理を実行する。尚、図１３は、ＣＰＵ２５ａが実行する事後確率算出処理を表すフローチャートである。
【００９５】
本実施例では、この事後確率算出処理を実行することにより、Ｓ１４００で求めた事前確率及びＳ１５００で求めた尤度を用いて、ベイズ推定の手法により、各線分Ｌ１，Ｌ２１，Ｌ２２毎に、当該線分が車両側面を近似した線分である蓋然性（事後確率）、当該線分が車両前後面を近似した線分である蓋然性（事後確率）、及び、当該線分がその他の線分である蓋然性（事後確率）を求める。
【００９６】
続いて、この事後確率算出処理の内容について、図１３を用いて詳述する。事後確率算出処理を開始すると、ＣＰＵ２５ａは、まず、対象線分を線分Ｌ１に設定する（Ｓ１６０５）。続いて、対象部位を「側面」に設定する（Ｓ１６１０）。
【００９７】
次に、対象線分が対象部位を近似した線分である事前確率Ｐ（１４２５）と、対象線分が対象部位を近似した線分である尤度Ｌ（３０００）とを用いて、ベイズ推定の式により、対象線分が対象部位を近似した線分である事後確率Ｐ（１６１５）を求める（Ｓ１６１５）。
【００９８】
Ｓ１６１５では、対象線分が線分Ｌ１であり対象部位が「側面」である場合、事後確率Ｐ（１６１５）を次式に従って算出する。尚、ここでは、対象線分が線分Ｌ１であり対象部位が「側面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両側面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１側）と表記する。
【００９９】
【数１】
また、この処理を終えると、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６３０）、対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６３０：Ｎ）、対象部位を「前後面」に変更して（Ｓ１６３５）、Ｓ１６１５に戻る。そして、対象線分が車両「前後面」を近似した線分である事後確率Ｐ（１６１５）を求める。
【０１００】
対象線分が線分Ｌ１であり対象部位が「前後面」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「前後面」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１が車両前後面を近似した線分である事後確率Ｐ（１６１５）を、特にＰｏ（１前後）と表記する。
【０１０１】
【数２】
一方、現在の対象線分について、当該対象線分が車両前後面を近似した線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６３０：Ｙ）、ＣＰＵ２５ａは、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したか否かを判断し（Ｓ１６４０）、現在の対象線分について、対象線分がその他の線分である事後確率Ｐ（１６１５）をまだ算出していなければ（Ｓ１６４０：Ｎ）、対象部位を「その他」に変更して（Ｓ１６４５）、Ｓ１６１５に戻る。そして、対象線分がその他の線分である事後確率Ｐ（１６１５）を求める。
【０１０２】
対象線分が線分Ｌ１であり対象部位が「その他」である場合には、ここで、事後確率Ｐ（１６１５）を次式に従って算出する。尚、本実施例では、対象線分が線分Ｌ１であり対象部位が「その他」である場合の事後確率Ｐ（１６１５）、即ち、線分Ｌ１がその他の線分である事後確率Ｐ（１６１５）を、特にＰｏ（１その他）と表記する。
【０１０３】
【数３】
また、現在の対象線分について、当該対象線分がその他の線分である事後確率Ｐ（１６１５）を既に算出したと判断すると（Ｓ１６４０：Ｙ）、ＣＰＵ２５ａは、対象線分として線分Ｌ２１を設定して、事後確率Ｐ（１６１５）を算出したか否かを判断し（Ｓ１６５５）、事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６５５：Ｎ）、対象線分を線分Ｌ２１に変更して（Ｓ１６６０）、Ｓ１６１０に戻る。
【０１０４】
そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２１が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１側）、線分Ｌ２１が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１前後）、線分Ｌ２１がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２１その他）を算出する。
【０１０５】
【数４】
また、対象線分として線分Ｌ２１を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６５５：Ｙ）、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したか否かを判断する（Ｓ１６６５）、そして、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出していないと判断すると（Ｓ１６６５：Ｎ）、対象線分を線分Ｌ２２に変更して（Ｓ１６７０）、Ｓ１６１０に戻る。
【０１０６】
そして、Ｓ１６１５の処理を上述した手順で繰り返し実行することにより、線分Ｌ２２が車両側面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２側）、線分Ｌ２２が車両前後面を近似した線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２前後）、線分Ｌ２２がその他の線分である事後確率Ｐ（１６１５）＝Ｐｏ（２２その他）を算出する。
【０１０７】
【数５】
また、対象線分として線分Ｌ２２を設定して事後確率Ｐ（１６１５）を算出したと判断すると（Ｓ１６６５：Ｙ）、当該事後確率算出処理を終了する。また、Ｓ１６００で、このようにして事後確率算出処理を終了すると、ＣＰＵ２５ａは、図１４に示す選択処理を実行する（Ｓ１７００）。図１４は、ＣＰＵ２５ａが実行する選択処理を表すフローチャートである。
【０１０８】
選択処理を開始すると、ＣＰＵ２５ａは、Ｓ１６００で求められた事後確率Ｐｏ(１側)、又は、事後確率Ｐｏ(１前後)、又は、事後確率Ｐｏ(２１側)及び事後確率Ｐｏ(２２前後)の両方、又は、事後確率Ｐｏ(２１前後)及び事後確率Ｐｏ(２２側)の両方、が所定の閾値Ｔｈ以上であるか否かを判定する（Ｓ１７１０）。即ち、ここでは、条件式「Ｐｏ（１側）≧Ｔｈ」、条件式「Ｐｏ(１前後)≧Ｔｈ」、条件式「Ｐｏ(２１側)，Ｐｏ(２２前後)≧Ｔｈ」、条件式「Ｐｏ(２１前後)，Ｐｏ(２２側)≧Ｔｈ」の計四つの条件式の内、少なくとも一つが満足されているか否かを判定する。尚、閾値Ｔｈは、０＜Ｔｈ＜１を満足する範囲内で設計者が自由に設定すればよい。
【０１０９】
そして、閾値Ｔｈ以上ではないと判定すると（Ｓ１７１０：Ｎ）、Ｓ１７２０〜Ｓ１７８０の処理を実行することなく、当該選択処理を終了する。
一方、閾値Ｔｈ以上であると判定すると（Ｓ１７１０：Ｙ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１側）が、事後確率Ｐｏ（１側），Ｐｏ(１前後)，Ｐｏ(２１側)，Ｐｏ(２２前後)，Ｐｏ(２１前後)，Ｐｏ（２２側）の集合の中で、最大であるか否かを判断する（Ｓ１７２０）。
【０１１０】
そして、事後確率Ｐｏ（１側）が最大であると判断すると（Ｓ１７２０：Ｙ）、線分Ｌ１が車両側面を近似した線分であると判定して、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７３０）。その後、当該選択処理を終了する。
【０１１１】
一方、事後確率Ｐｏ（１側）が最大ではないと判断すると（Ｓ１７２０：Ｎ）、ＣＰＵ２５ａは、事後確率Ｐｏ（１前後）が上記集合の中で最大であるか否かを判断する（Ｓ１７４０）。そして、事後確率Ｐｏ（１前後）が最大であると判断すると（Ｓ１７４０：Ｙ）、線分Ｌ１が車両前後面を近似した線分であると判定し、当該単一線分からなる近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７５０）。その後、当該選択処理を終了する。
【０１１２】
この他、事後確率Ｐｏ（１前後）が最大ではないと判断すると（Ｓ１７４０：Ｎ）、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が上記集合の中で最大であるか否かを判断する（Ｓ１７６０）。
【０１１３】
そして、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれか一方が最大であると判断すると（Ｓ１７６０：Ｙ）、線分Ｌ２１が車両側面を近似した線分であり、線分Ｌ２２が車両前後面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７７０）。その後、当該選択処理を終了する。
【０１１４】
一方、事後確率Ｐｏ（２１側）及び事後確率Ｐｏ（２２前後）のいずれとも上記集合の中で最大ではないと判断すると（Ｓ１７６０：Ｎ）、ＣＰＵ２５ａは、線分Ｌ２１が車両前後面を近似した線分であり、線分Ｌ２２が車両側面を近似した線分であると判定し、Ｌ字状の近似図形パターンを、前方に位置する車両の輪郭として最も確からしい図形パターンとして選択する（Ｓ１７８０）。その後、当該選択処理を終了する。
【０１１５】
このようにしてＳ１７００で選択処理を終了すると、ＣＰＵ２５ａは、Ｓ１８００に移行して、図１７に示す状態推定処理を実行する。
本実施例では、この現在過去状態推定処理を実行することにより、Ｓ１１０での測位結果に基づき、現在の車両状態及び次回の車両状態推定処理実行時（即ち、次サイクル）の車両状態を推定する。以下では、この現在過去状態推定処理を、図１７を用いて詳細に説明する前に、その概略について説明する。
【０１１６】
状態推定処理では、まず、Ｓ１７００で実行した選択処理の判定結果と、Ｓ１２０，Ｓ１３００で算出した各線分Ｌ１，Ｌ２１，Ｌ２２の中点の位置座標、向き、及び長さの算出結果とに従い、前方物体を車両とみなして、上記座標系Ａで、その前方車両の中心点の位置座標（ｘ０m，ｙ０m）、車両の向き（車両前後方向軸線の座標系Ａｘ軸とのなす角度）θm、車両の全長Ｌm及び全幅Ｗmを求める。
【０１１７】
そして、これらの観測値ｚ
【０１１８】
【数６】
を、後述する座標系Ｂに置換した後、この座標変換後の観測値ｚを、カルマンフィルタへの入力とし、座標系Ｂでの前方車両の状態量ｘs
【０１１９】
【数７】
を、カルマンフィルタの出力として、得る。ここでｘ０は、前方車両中心点の座標系Ｂｘ軸上の座標値、ｖxは前方車両の速度ｘ軸成分、ｙ０は、前方車両中心点の座標系Ｂｙ軸上の座標値、ｖyは前方車両の速度ｙ軸成分、θは、ｘ軸と車両側面（若しくは車両前後方向軸線）とのなす角度（但し、０≦θ＜πになるように規格化される。）、ωは、角度θの時間微分（つまり角速度）、Ｗは、前方車両全幅、Ｌは、前方車両全長を示す。
【０１２０】
具体的に、本実施例では、カルマンフィルタとして、非線形運動モデルに適用可能なアンセンテッドカルマンフィルタ（ＵＫＦ）を用いて、前方車両の状態量ｘsを求める。図１５は、観測値ｚから状態量ｘsの推定手順を、示した説明図である。
【０１２１】
本実施例の推定装置１は、このアンセンテッドカルマンフィルタとしての機能をＣＰＵ２５ａに実現させるためのプログラムを、ＲＯＭ２５ｂに備え、Ｓ１８００にて、ＣＰＵ２５ａにより、このプログラムを実行することによって、状態量ｘsを求める。
【０１２２】
尚、ここで用いられるアンセンテッドカルマンフィルタは、周知の理論に基づいて構築されるものであり、設計者が設定する運動モデル及び観測モデルによって定まる伝達関数により、入力に対する出力を演算するものである。
【０１２３】
従って、以下では、カルマンフィルタ及びアンセンテッドカルマンフィルタの原理についての説明は省略し、アンセンテッドカルマンフィルタ（ＵＫＦ）の説明として、本実施例の推定装置１の動作を実現するに当って、アンセンテッドカルマンフィルタの設計に必要な運動モデル、観測モデル、及び、アンセンテッドカルマンフィルタに入出力されるパラメータについて説明する。
【０１２４】
図１５に示すように、本実施例のアンセンテッドカルマンフィルタには、最新サイクルｔの測位結果に基づいて算出された上述の観測値ｚ（ｔ）と、前サイクルｔ−１にて観測値ｚ（ｔ−１）に基づきアンセンテッドカルマンフィルタが演算し出力した状態量ｘs（ｔ，ｔ−１）が入力される。そして、この入力に基づいた演算によりアンセンテッドカルマンフィルタからは、現在の車両状態を示す状態量ｘs（ｔ，ｔ）と、次サイクルｔ＋１の車両状態を示す状態量ｘs（ｔ＋１，ｔ）と、が出力される。但し、本実施例では、時刻（サイクル）ｔ2での観測値ｚ（ｔ2）の入力に基づいて求められる時刻（サイクル）ｔ1での車両の状態量ｘsを、特に、ｘs（ｔ1，ｔ2）と表記する。
【０１２５】
このアンセンテッドカルマンフィルタから出力される現在の車両状態を表す状態量ｘ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を求めればよい。
【０１６４】
この他、尤度算出処理では、車両状態の予測値が必要になるが、車両状態の予測値がＲＡＭ２５ｃに記録されていない場合には、尤度算出処理及び事後確率算出処理を実行することなく、事前確率Ｐをそのまま事後確率Ｐｏとして設定して、選択処理に移行すればよい。
【０１６５】
尚、特許請求の範囲に記載の推定手段は、本実施例のＳ１２０からＳ１８６５までの処理にて実現されている。この他、測位手段は、本実施例のＳ１１０の処理にて実現されている。また、本発明は、上記実施例に限定されることなく、種々の態様を採ることができることは言うまでもない。
【符号の説明】
【０１６６】
１…推定装置、１１…レーザダイオード、１２…コリメータレンズ、１３…レーザダイオード駆動回路、１５ミラー…、１７モータ…、１９…集光レンズ、２１…フォトダイオート、２２…増幅器、２３…検出回路、２５…制御部、２５ａ…ＣＰＵ、２５ｂ…ＲＯＭ、２５ｃ…ＲＡＭ、５０…前方車両
【特許請求の範囲】
【請求項１】
前方に位置する車両の運動状態を推定する推定装置であって、
レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、前記レーダ波を反射した前記車両表面の複数地点の位置座標を計測する測位手段と、
前記測位手段により計測された前記車両表面の複数地点の位置座標に基づき、前記車両の角速度を含む前記車両の運動状態を所定の非線形運動モデルに従って推定する推定手段と、
を備えることを特徴とする推定装置。
【請求項２】
前記推定手段は、前記車両の運動状態として、前記車両の角速度以外に、前記車両の位置、前記車両の向き、前記車両の速さ、及び、前記車両の進行方向の内、少なくとも一つを推定する構成にされていること
を特徴とする請求項１に記載の推定装置。
【請求項３】
前記推定手段は、前記非線形運動モデルが採用されたカルマンフィルタを用いて、前記車両の状態を推定する構成にされていること
を特徴とする請求項１又は請求項２に記載の推定装置。
【請求項４】
前記カルマンフィルタは、アンセンテッドカルマンフィルタであること
を特徴とする請求項３に記載の推定装置。
【請求項５】
前記非線形運動モデルは、等速円運動モデルであること
を特徴とする請求項３又は請求項４に記載の推定装置。
【請求項６】
前記推定手段は、前回推定した角速度がゼロである場合には、前記車両の状態の推定に用いる運動モデルを、前記非線形運動モデルから、所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、前記車両の状態を推定する構成にされていること
を特徴とする請求項３から請求項５の何れか１項に記載の推定装置。
【請求項１】
前方に位置する車両の運動状態を推定する推定装置であって、
レーダ波を発射し、発射波が前方に位置する車両に反射して戻ってくる反射波を受波して、前記レーダ波を反射した前記車両表面の複数地点の位置座標を計測する測位手段と、
前記測位手段により計測された前記車両表面の複数地点の位置座標に基づき、前記車両の角速度を含む前記車両の運動状態を所定の非線形運動モデルに従って推定する推定手段と、
を備えることを特徴とする推定装置。
【請求項２】
前記推定手段は、前記車両の運動状態として、前記車両の角速度以外に、前記車両の位置、前記車両の向き、前記車両の速さ、及び、前記車両の進行方向の内、少なくとも一つを推定する構成にされていること
を特徴とする請求項１に記載の推定装置。
【請求項３】
前記推定手段は、前記非線形運動モデルが採用されたカルマンフィルタを用いて、前記車両の状態を推定する構成にされていること
を特徴とする請求項１又は請求項２に記載の推定装置。
【請求項４】
前記カルマンフィルタは、アンセンテッドカルマンフィルタであること
を特徴とする請求項３に記載の推定装置。
【請求項５】
前記非線形運動モデルは、等速円運動モデルであること
を特徴とする請求項３又は請求項４に記載の推定装置。
【請求項６】
前記推定手段は、前回推定した角速度がゼロである場合には、前記車両の状態の推定に用いる運動モデルを、前記非線形運動モデルから、所定の線形運動モデルに切り替えて、当該線形運動モデルに従い、前記車両の状態を推定する構成にされていること
を特徴とする請求項３から請求項５の何れか１項に記載の推定装置。
【図１】
【図２】
【図３】
【図５】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１７】
【図４】
【図６】
【図７】
【図１６】
【図２】
【図３】
【図５】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１７】
【図４】
【図６】
【図７】
【図１６】
【公開番号】特開２００９−２８８２５５（Ｐ２００９−２８８２５５Ａ）
【公開日】平成２１年１２月１０日（２００９．１２．１０）
【国際特許分類】
【出願番号】特願２００９−２１０７０３（Ｐ２００９−２１０７０３）
【出願日】平成２１年９月１１日（２００９．９．１１）
【分割の表示】特願２００７−２１０２７５（Ｐ２００７−２１０２７５）の分割
【原出願日】平成１９年８月１０日（２００７．８．１０）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
【公開日】平成２１年１２月１０日（２００９．１２．１０）
【国際特許分類】
【出願日】平成２１年９月１１日（２００９．９．１１）
【分割の表示】特願２００７−２１０２７５（Ｐ２００７−２１０２７５）の分割
【原出願日】平成１９年８月１０日（２００７．８．１０）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
[ Back to top ]