JP7206875B2 - 車両横速度推定装置 - Google Patents

Description

本発明は、車両横速度推定装置に係り、特に、走行路と横滑りを含む車両の平面運動の関係に基づき、車両横速度を推定する車両横速度推定装置に関する。

車両は、路面に対する前後運動、横運動及びヨー運動の３軸方向の運動をする。特に車両のコーナリング時の挙動における重要な要素として、車両横速度がある。車体の横方向の運動が計測可能になる事により、例えば車両の自己位置推定が高精度に行えるようになる。従って、高精度な自己位置推定により、車両の運動を計測する他の各種センサの正確な補正も可能となる。また、車両運動モデルをオンラインで補正する事が可能になり、車両運動モデルによって算出される車両制御パラメータのオンラインチューニングにも活用できる。

横運動はスリップ角センサで検出でき、ヨー運動はヨーレートセンサで検出できる。しかしながら、スリップ角センサは、走行中の車両からレーザ光を路面に照射し、路面で反射されたレーザ光を受信して車両横速度を検出する複雑かつ高価な装置であり、量産車両には車載され難い。従って、量産車両の車両横速度を推定するために、車両運動モデルを用いた推定手法が提案されている。

特許文献１には、ヨーレートセンサ及び車速センサを用いて車両の回転姿勢の変化量を推定し、カメラ画像から抽出した道路に添って延びる基準線に対する車両の回転姿勢の変化量を算出すると共に、所定の基準に対する車両の回転姿勢の変化量との差に基づいて当該基準線の曲がり情報を道路の曲がり情報として演算する走路の曲がり情報検出装置の発明が開示されている。

特許文献２には、車速、ヨーレート及び横加速度に基づいて車両スリップ角を推定する車両のすべり角検出装置の発明が開示されている。

特許文献３には、車載カメラ等の撮像装置で取得した車両周囲の画像情報から車両のヨー角を検出すると共に、車両運動モデルに基づいて舵角センサとヨーレートセンサとから得た情報からヨー角推定値を算出し、当該ヨー角推定値と車載カメラで検出したヨー角とから算出したオフセット誤差で、車両運動モデルに基づいて推定されたスリップ角推定値を補正する車両物理量推定装置の発明が開示されている。

特開平１０－３１８７４２号公報 特開平１１－２４８４５５号公報 特開２０１０－２７４７０１号公報

しかしながら、特許文献１に記載の発明は、ヨー角変化を時系列的な連続性を考慮せずに計算しているので、推定されるヨーレート値にノイズが生じやすく、ヨーレートセンサのオフセット誤差によりヨーレート推定の精度が低下するという問題があった。また、車両横速度を考慮していないので、交差点等で車両横速度が急激に変化する場合には、ヨーレート推定の精度がさらに低下するおそれがある。

特許文献２に記載の発明は、各センサの特性に合わせてローパスフィルタ及びハイパスフィルタを適用しているので、推定値の出力に遅延が生じるおそれがある。加速度センサ及びヨーレートセンサの各々の出力結果には、上記のフィルタでも除去が困難なオフセット誤差が生じるため、横滑り角の推定の精度が低下するという問題があった。

特許文献３に記載の発明は、車両運動モデルをヨーレートで補正することにより、スリップ角推定の精度を向上させているが、入力される操舵角は一般的にオフセット誤差が生じているので、誤差を排除できないという問題があり、さらに用いている車両運動モデルは路面、タイヤ等の影響を受けやすく、環境及び車両諸元に応じた設定を要するという問題があった。

本発明は、上記問題に鑑みてなされたものであり、車両が備えたセンシング装置によって検出した車両周辺の情報、車速センサで検出した車速及びヨーレートセンサで検出したヨーレートから、車両横速度を精度よく推定可能な車両横速度推定装置を実現することを目的とする。

上記目的を達成するために、請求項１に記載の車両横速度推定装置は、車両が備えたセンシング装置によって取得した車両周辺の情報から、目標経路と車両との横位置偏差及び前記目標経路と車両のヨー角偏差を算出する位置計測部と、前記位置計測部で算出した前記横位置偏差及び前記ヨー角偏差により定まる前記目標経路上の目標点の移動と、横滑りを含む前記車両の運動との関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、車速センサから得られる車両前後速度及びヨーレートセンサから得られる車両ヨーレートを含む実測値から、前記車両の車両横速度を推定する演算部と、を含んでいる。

また、請求項２に記載の車両横速度推定装置は、請求項１に記載の車両横速度推定装置において、前記関係を表す式は、前記目標点の移動速度の、車両前後方向に対する垂直成分から、前記横位置偏差の変化量と、前記車両のヨー運動に伴う車両重心位置に対する前記センシング装置の横速度とを減算した結果と、前記車両の車両横速度とが等しい関係を表す式を含んでいる。

また、請求項３に記載の車両横速度推定装置は、請求項２に記載の車両横速度推定装置において、前記関係を表す式は、前記車両前後速度から、前記車両ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果と、前記目標点の移動速度とが等しい関係を表す式を更に含んでいる。

また、請求項４に記載の車両横速度推定装置は、請求項１～３のいずれか１項に記載の車両横速度推定装置において、前記センシング装置は、撮像装置が取得した前記車両周辺の画像情報の解析を行う画像解析装置、ＬＩＤＡＲ及びソナーのいずれかである。

また、請求項５に記載の車両横速度推定装置は、請求項１～請求項４の何れか１項記載の車両横速度推定装置において、前記演算部は、前記関係を表す式に基づいて、前回推定された、前記車両のヨーレート及び車両横速度を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、車両前後速度及び前記車両ヨーレートと、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正する更新部と、を含んでいる。

本発明によれば、車両が備えたセンシング装置によって検出した車両周辺の情報、車速センサで検出した車速及びヨーレートセンサで検出したヨーレートから、車両横速度を精度よく推定可能な車両横速度推定装置を実現することができるという効果を奏する。

本発明の実施の形態に係る車両横速度推定装置の構成の一例を示すブロック図である。 本発明の実施の形態における座標系を示した概略図である。 撮像装置の位置、車両の重心、横位置偏差及びヨー角偏差の関係の一例を示した説明図である。 横滑りを含む車両の運動と目標経路との関係を示した説明図である。 本発明の実施の形態に係る車両横速度推定装置の入出力の概略を示した説明図である。 本発明の実施の形態に係る車両横速度推定装置の演算装置の機能ブロック図の一例である。 事前推定部の機能ブロック図の一例である。 フィルタリング部の機能ブロック図の一例である。 交差点での右左折時のように、横位置偏差とヨー角偏差とが大きい場合の車両の旋回の一例を示した概略図である。 数珠つなぎの渋滞が生じ、車両の進行方向の遠方の情報の取得が困難な場合の概略図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係る車両横速度推定装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、車両が備えたセンシング装置によって取得した車両周辺の情報から目標経路と車両との横位置偏差及び車両のヨー角偏差を算出する位置計測装置２０と、位置計測装置２０が算出した横位置偏差、ヨー角偏差、車速センサ２４が検出した車両前後速度及びヨーレートセンサ２６が検出したヨーレートが入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両横速度の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両横速度等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

本実施の形態に係る車載のセンシング装置は、一例として、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナーのいずれかである。車載カメラ等の撮像装置を車載のセンシング装置とした場合は、一例として、当該撮像装置で取得した車両周辺の画像情報を解析して道路の白線等を検出する。ＬＩＤＡＲを車載のセンシング装置とした場合は、一例として、車両周辺に照射したパルス状のレーザの散乱光から道路の白線等を検出する。ソナーを車載のセンシング装置とした場合は、一例として、アスファルトの路面とペイントされた白線との、超音波の反射率の差を利用して、当該白線を識別する。

続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚ_eとが平行で、ｙ_eが北方向を向いている座標系である。路面座標系２０６は、ｚ_rが車両２００の重心を通り路面に垂直な方向に向き、ｘ_rは車両進行方向に向いている座標系である。車両座標系２０８は車体バネ上に固定された座標系で、ｚ_vは車体鉛直上方向、ｘ_vは車体進行方向を向いている。

本実施の形態は、車載のセンシング装置によって検出した車両周辺の情報を用いて車両２００の車両横速度を算出する。本実施の形態では車両横速度の算出に際して、図３にあるように、目標点Ｐ_tにおける目標経路２２０である走行車線の曲率ρに加え、センサ位置Ｐ_sを基準とした目標点Ｐ_tに対する横位置偏差ｅ₁とヨー角偏差ｅ₂とを想定する。なお、センサ位置Ｐ_sは車両重心Ｐ_rから車両２００の前方方向に距離（車両重心Ｐ_rとセンサ位置Ｐ_sとのオフセット距離）ｌ_sで隔てられた位置にあり、車載カメラ等の撮像装置、ＬＩＤＡＲのセンサ又はソナーが設けられている。図３に示したように、センサ位置Ｐ_sを基準とした車両２００の進行方向の移動量はｘ_Sであり、センサ位置Ｐ_sを基準とした車両２００の横方向の移動量はｙ_sである。

次に、車両横速度を推定する際に用いる、横滑りを含む車両２００の運動と目標経路２２０との関係を図４に示す。一般に車両２００は路面に対して横滑りをしており、車両横速度を有する運動を伴っている。車両２００の挙動に対し、走行車線上の目標位置Ｐ_tは前後方向の速度のみを有し、車両横速度を有さない運動をしている。この特徴を利用し、車両２００の運動と目標位置Ｐ_tの運動との差分を導出する事で、車両横速度を推定するのに必要な条件式を導出する事が可能になる。

本実施の形態では、図３、４のように各変数を設定する。目標経路２２０上の目標位置Ｐ_tは、接線方向の速度成分Ｕ_tのみを持つとして、目標位置Ｐ_tの速度ベクトルをＶ_t=[Ｕ_t ０]^T、目標位置Ｐ_tをＰ_t=[ｘ_t ｙ_t ]^T、地球座標系のx軸方向に対する接線方向のヨー角をψ_tと定義する。さらに車両２００は前後方向のみならず車両横速度を有するとして（車体スリップ角を有する）、路面座標系２０６の原点をＰ_r=[ｘ_r ｙ_r ]^T、ヨー角をψ_r、車両２００の各軸方向の速度成分を図４に示したように、ν_r=[Ｕ_r Ｖ_r ]^Tとする。本実施の形態では、後に車両座標系２０８を基準に計測されたセンサ情報との関係を容易にするため、偏差の基準座標系を路面座標系２０６とし、ヨー角偏差としてψ_e=ψ_t－ψ_rを定義する。

図５は、本実施の形態に係る車両横速度推定装置１０の入出力の概略を示した説明図である。車両横速度推定装置１０を構成する演算装置１４には、位置計測装置２０から横位置偏差ｅ₁とヨー角偏差ｅ₂とが入力される。また、車速センサ２４からは車両２００の前後速度Ｕ_r（以下、「車両前後速度Ｕ_r」と略記）が、ヨーレートセンサ２６からはヨーレートセンサ２６が検出したヨーレートＲ_rが、各々、演算装置１４に入力される。

演算装置１４は、後述する演算処理によって、横位置偏差ｅ₁、ヨー角偏差ｅ₂及び車両前後速度Ｕ_r及びヨーレートＲ_rから車両横速度Ｖ_rを推定して出力する。

路面座標系２０６から見た目標位置Ｐ_tの偏差ベクトルをＰ_e=[ｘ_e ｙ_e ]とすると、当該偏差ベクトルＰ_eの微分は下記の２式で表される。

位置計測装置２０では、車載カメラ等の撮像装置が取得した車両２００周辺の画像情報を既知の手法によって画像解析することにより、又はＬＩＤＡＲ若しくはソナーを用いて取得した車両周辺の情報から、センサ位置Ｐ_sを基準とした目標点Ｐ_tに対する横位置偏差ｅ₁とヨー角偏差ｅ₂とが抽出される。撮像装置、ＬＩＤＡＲ及びソナーはセンサの一種であるから、これらの装置で取得した情報から抽出した横位置偏差ｅ₁及びヨー角偏差ｅ₂をセンサで取得されるセンサ情報とする。さらに、車速センサ２４で検出する車両前後速度Ｕ_r及びヨーレートセンサで検出するヨーレートＲ_rをセンサ情報として、上記式を変形すると、下記の３式が得られる。

さらに車両横速度Ｖ_rと適正ヨーレートＲ_rとについて上記３式を整理すると、下記の式（１）、（２）が得られる。

上記式（１）は、車両横速度Ｖ_rは、車両前後速度Ｕ_rの目標経路２２０に対する垂直成分、言い換えれば、車両前後速度Ｕ_rの車両座標系２０８のｙ座標成分であるＵ_tsinｅ₂から、目標経路との横位置偏差ｅ₁の変化量（ｅ₁の微分値）と、車両２００のヨー運動に伴う車両重心位置Ｐ_rに対する位置計測装置２０のセンサの横速度Ｒ_rｌ_sを引いたものと一致することを示している。なお、上記式（１）は、目標点Ｐ_tの移動速度の、車両前後方向に対する垂直成分から、横位置偏差ｅ₁の変化量と、車両２００のヨー運動に伴う車両重心位置に対する位置計測装置２０のセンサの横速度とを減算した結果と、車両横速度Ｖ_rとが等しい関係を表す式の一例である。

上記式（２）は、目標経路２２０上の目標点Ｐ_tの移動速度Ｕ_tは、車両前後速度Ｕ_rから車両２００のヨーレートＲ_rと横位置偏差ｅ₁を乗算したＲ_rｅ₁を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差ｅ₂の余弦cosｅ₂で除算することで求められることを示している。なお、上記式（２）は、車両前後速度Ｕ_rから、ヨーレートＲ_rと横位置偏差ｅ₁との積を減算したものを、ヨー角偏差ｅ₂を用いて補正した結果と、目標点Ｐ_tの移動速度とが等しい関係を表す式の一例である。

上記式（１）、（２）を用いて、位置計測装置２０、車速センサ２４及びヨーレートセンサ２６によって取得されたデータに対して最もよくあてはまる未知変数を推定することにより、車両横速度Ｖ_rの推定が可能になる。

上記式（１）、（２）はいずれも運動の近似をしておらず、非線形な形を取っているが、上記式（１）、（２）を直接用いて非線形カルマンフィルタに適用することにより、車両２００が目標経路２２０から大きく逸脱している場合でも正確な推定が可能となる。また、状態変数に関して局所線形化する事により、拡張カルマンフィルタを適用してもよい。

本実施の形態では、目標経路２２０と車両２００の運動との関係を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。

まず、位置計測装置２０より得られる目標経路２２０に対する横位置偏差ｅ₁及びヨー角偏差ｅ₂の情報と、車速センサ２４より得られる車両前後速度Ｕ_rと、ヨーレートセンサ２６より得られるヨーレートＲ_rと、を用いて車両横速度Ｖ_rを推定する場合の状態方程式ｆ（ｘ）は、以下のように定義する。

かかる場合の状態方程式ｆ（ｘ）は、下記のような、時間ｔにおける状態量ｘ（ｔ）に対して、下記の式（３）の関係が成り立つ。

上記の状態量ｘ（ｔ）に対して、上記式（３）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。

式（４）は、目標経路２２０と車両２００との運動の関係を示した微分方程式である。
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。

次に観測量を下記のように定義する。

ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。

ただし、各観測量に対する観測ノイズを下記のように定義する。

カルマンフィルタでは、式（４）を含む状態方程式ｆ（ｘ）の各々を離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ－１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ－１））となる形で使用する。

図６は、本実施の形態に係る車両横速度推定装置１０の演算装置１４の機能ブロック図の一例である。図６に示したように、演算装置１４は、状態方程式ｆ（ｘ）を用いて、目標経路２２０である走行車線と横滑りを含む車両２００の平面運動との関係に基づく状態の推定を行うと共に、観測方程式ｈ（ｘ）を用いて、対応するセンサ情報を推定する事前推定部１０２と、事前推定部１０２が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部１０４と、を含む。

カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ（ｘ）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ（ｘ）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆ（ｘ）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。

図７は、事前推定部１０２の機能ブロック図の一例である。図７に示したように、事前推定部１０２は、第１アンセンティッド変換部１０２Ａと第２アンセンティッド変換部１０２Ｂとを含む。

第１アンセンティッド変換部１０２Ａは、状態方程式ｆ（ｘ）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘの平均及びｘの共分散行列を出力する。

第２アンセンティッド変換部１０２Ｂは、第１アンセンティッド変換部１０２Ａが出力した状態量ｘの平均及び状態量ｘの共分散行列を用いて、観測方程式ｈ（ｘ）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。

なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。

従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。

図７に示したように、第１アンセンティッド変換部１０２Ａは、シグマポイント重み係数部１０２Ａ１と、関数変換部１０２Ａ２と、Ｕ変換部１０２Ａ３と、を含む。また、第２アンセンティッド変換部１０２Ｂは、シグマポイント重み係数部１０２Ｂ１と、関数変換部１０２Ｂ２と、Ｕ変換部１０２Ｂ３と、を含む。

第１アンセンティッド変換部１０２Ａのシグマポイント重み係数部１０２Ａ１及び第２アンセンティッド変換部１０２Ｂのシグマポイント重み係数部１０２Ｂ１では、シグマポイントＸ_i：ｉ＝０、１、２、…２ｎが、下記のように選択される。

ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。

第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部１０２Ｂの関数変換部１０２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測値が得られる。

第１アンセンティッド変換部１０２ＡのＵ変換部１０２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの共分散行列を算出する。なお、下記式中のＱ_nはシステムノイズである。

第２アンセンティッド変換部１０２ＢのＵ変換部１０２Ｂ３では、下記の計算を行う。

図８は、フィルタリング部１０４の機能ブロック図の一例である。フィルタリング部１０４では、Ｕ変換部１０２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。

状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲ_nは観測ノイズである。

次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。

以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、車両横速度Ｖ_rを推定する。

車両横速度Ｖ_rを推定する場合、フィルタリング部１０４が出力したｘ（ｋ）、Ｐ_x（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘ（ｋ）、Ｐ_x（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐ_x（ｋ）を前回値として事前推定部１０２に入力する。

この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことによって車両横速度Ｖ_rを推定する。

本実施の形態に係る車両横速度推定装置１０は、式（１）が示すように、車両２００の車両横速度Ｖ_rが、目標点Ｐ_tの移動速度の、車両前後方向に対する垂直成分から、横位置偏差ｅ₁の変化量と、車両２００のヨー運動に伴う車両重心位置に対する位置計測装置２０のセンサの車両横速度とを減算した結果と一致することに基づいて、車両横速度Ｖ_rを推定している。

また、本実施の形態に係る車両横速度推定装置１０は、式（２）が示すように、目標経路２２０上の目標点Ｐ_tの移動速度Ｕ_tを、車両前後速度Ｕ_rから車両２００の適正ヨーレートＲ_rと横位置偏差ｅ₁を乗算したＲ_rｅ₁を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差ｅ₂の余弦cosｅ₂で除算することで推定している。

式（１）、（２）はいずれも運動の近似をしていないので、図９に示したように、横位置偏差ｅ₁及びヨー角偏差ｅ₂が大きくなる交差点での右左折時のような車両２００が目標経路２２０から大きく逸脱した場合でも、車両が備えたセンシング装置によって取得した車両周辺の情報から目標経路２２０と車両２００との横位置偏差ｅ₁と車両のヨー角偏差ｅ₂とを算出する位置計測装置２０、車速センサ２４及びヨーレートセンサ２６によって各々取得されたデータから車両横速度Ｖ_rを精度よく推定することができる。

式（１）、（２）を用いた車両横速度Ｖ_rの推定では、道路の曲率ρは不要で、横位置偏差ｅ₁及びヨー角偏差ｅ₂を用いる。曲率ρを算出する場合、車両２００の進行方向の遠方までの道路線形の情報を要するが、横位置偏差ｅ₁及びヨー角偏差ｅ₂の算出では、車両２００Ａ直下の白線を、ＬＩＤＡＲ等のセンシングで検出することで足りる。

従って、本実施の形態に係る車両横速度推定装置１０によれば、画像の取得に難がある夜間、又は図１０に示したように、数珠つなぎの渋滞が生じ、車両２００Ｂによって車両２００Ａの前方視界が阻害され、遠方の道路線形の情報を取得できない場合でも、車両２００Ａ直下の白線を、ＬＩＤＡＲ等で検出することにより、車両２００Ａの横位置偏差ｅ₁及びヨー角偏差ｅ₂を検出して、車両横速度Ｖ_rを精度よく推定することができる。

以上説明したように、本実施の形態に係る車両横速度推定装置１０は、車両２００が備えたセンシング装置によって検出した車両２００周辺の情報である横位置偏差ｅ₁及びヨー角偏差ｅ₂、車速センサ２４で検出した車両前後速度Ｕ_r及びヨーレートセンサ２６で検出したヨーレートＲ_rから車両横速度Ｖ_rを精度よく推定することができる。

１０ 車両横速度推定装置
１２ 入力装置
１４ 演算装置
１６ 表示装置
１８ 記憶装置
２０ 位置計測部
２２ 撮像装置
２４ 車速センサ
２６ ヨーレートセンサ
１０２ 事前推定部
１０２Ａ 第１アンセンティッド変換部
１０２Ａ１ シグマポイント重み係数部
１０２Ａ２ 関数変換部
１０２Ａ３ Ｕ変換部
１０２Ｂ 第２アンセンティッド変換部
１０２Ｂ１ シグマポイント重み係数部
１０２Ｂ２ 関数変換部
１０２Ｂ３ Ｕ変換部
１０４ フィルタリング部
２００ 車両
２０４ 地球座標系
２０６ 路面座標系
２０８ 車両座標系
２２０ 目標経路

Claims (5)

1. 車両が備えたセンシング装置によって取得した車両周辺の情報から、目標経路と車両との横位置偏差及び前記目標経路と車両のヨー角偏差を算出する位置計測部と、
   前記位置計測部で算出した前記横位置偏差及び前記ヨー角偏差により定まる前記目標経路上の目標点の移動と、横滑りを含む前記車両の運動との関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、車速センサから得られる車両前後速度及びヨーレートセンサから得られる車両ヨーレートを含む実測値から、前記車両の車両横速度を推定する演算部と、
   を含む車両横速度推定装置。
2. 前記関係を表す式は、前記目標点の移動速度の、車両前後方向に対する垂直成分から、前記横位置偏差の変化量と、前記車両のヨー運動に伴う車両重心位置に対する前記センシング装置の横速度とを減算した結果と、前記車両の車両横速度とが等しい関係を表す式を含む請求項１に記載の車両横速度推定装置。
3. 前記関係を表す式は、前記車両前後速度から、前記車両ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果と、前記目標点の移動速度とが等しい関係を表す式を更に含む請求項２に記載の車両横速度推定装置。
4. 前記センシング装置は、撮像装置が取得した前記車両周辺の画像情報の解析を行う画像解析装置、ＬＩＤＡＲ及びソナーのいずれかである請求項１～３のいずれか１項に記載の車両横速度推定装置。
5. 前記演算部は、前記関係を表す式に基づいて、前回推定された、前記車両のヨーレート及び車両横速度を含む前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、
   予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、車両前後速度及び前記車両ヨーレートと、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正する更新部と、
   を含む請求項１～請求項４の何れか１項記載の車両横速度推定装置。

Images