(57)【要約】【課題】撮像装置が取得した画像及び車速から推定した適正ヨーレートに基づいて車両のヨーレートセンサが検出した検出ヨーレートの誤差を算出することにより、検出ヨーレートを正確に補正可能なヨーレート補正装置を実現する。【解決手段】車両周辺の画像情報を取得する撮像装置２２と、撮像装置２２が取得した画像情報から、目標経路と車両との横位置偏差、車両のヨー角偏差及び目標経路の曲率を算出する画像情報処理部２０と、目標経路に対する車両の横滑りを含む平面運動の関係式に、横位置偏差、ヨー角偏差、曲率、及び車速センサ２４から得られる車両前後速度を含む実測値を適用することによって推定した適正ヨーレートに基づいて、ヨーレートセンサ２６が検出した検出ヨーレートの誤差を算出する演算装置１４と、を含んでいる。【選択図】図１
2020098116.tif 000002
【特許請求の範囲】
【請求項１】
車両周辺の画像情報を取得する撮像装置と、
前記撮像装置が取得した画像情報から、目標経路と車両との横位置偏差、前記目標経路と車両のヨー角偏差、及び前記目標経路の曲率を算出する画像情報処理部と、
前記目標経路上の目標点の移動と、横滑りを含む前記車両の運動との関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車速センサから得られる車両前後速度を含む実測値から、前記車両の適正ヨーレートを推定し、前記車両の適正ヨーレートに基づいて、前記車両が備えるヨーレートセンサが検出した検出ヨーレートの誤差を算出する演算部と、
を含むヨーレート補正装置。
【請求項２】
前記関係を表す式は、前記目標点の移動速度と前記曲率との積である前記目標点の接線方向の変化率から、前記ヨー角偏差の変化量を減算した結果と、前記適正ヨーレートとが等しい関係を表す式を含む請求項１記載のヨーレート補正装置。
【請求項３】
前記関係を表す式は、前記車両前後速度から、前記適正ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果と、前記目標点の移動速度とが等しい関係を表す式を含む請求項１又は請求項２記載のヨーレート補正装置。
【請求項４】
前記演算部は、前記関係を表す式に基づいて、前回推定された、前記車両のヨーレートを含む前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、
予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車両前後速度と、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正することにより、前記車両の適正ヨーレートを推定する更新部と、
を含む請求項１〜請求項３の何れか１項記載のヨーレート補正装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ヨーレート補正装置に係り、特に、走行路と横滑りを含む車両の平面運動の関係に基づき、車両のヨーレートセンサが検出したヨーレートを補正するヨーレート補正装置に関する。
【背景技術】
【０００２】
車両は、路面に対して前後運動、横運動及びヨー運動の３軸方向の運動を行っている。ヨー運動の速度を示すヨーレートは車両の進行方向を把握する際に重要であり、車両の横滑り防止装置又は自動ブレーキシステム等では、車両に設けられたヨーレートセンサによって検出される。
【０００３】
ヨーレートセンサは加速度センサの一種であるが、温度によって検出値が影響を受けやすい。例えば、環境温度が変化した結果、ヨーレートセンサのゼロ点がゼロからずれる場合があった。
【０００４】
従って、ヨーレートセンサを用いる際には、ヨーレートセンサで検出したヨーレートの値を補正することが必要となる。一例として、車両の操舵角が所定の値以下の場合に車両が直進していると仮定して、ヨーレートセンサのゼロ点を補正するという方法があるが、操舵角センサのゼロ点が正確に調整されていない場合は、ヨーレートセンサのゼロ点の補正が正確にできない。また、路面環境によっては、操舵角センサによって検出した操舵角がゼロを示していても、実際には車両が旋回している場合があるので、補正の精度が良好でないという問題があった。
【０００５】
また、ＧＰＳ（Global Positioning System）によって車両の挙動を把握してヨーレートセンサが検出したヨーレートを補正する方法がある。しかしながら、かかる方法は、車両がトンネル内又はビル街等のＧＰＳ衛星からの電波の受信が困難な場所を走行している場合に、ヨーレートの補正ができないという問題があった。
【０００６】
特許文献１には、車載カメラ等の撮像装置で取得した車両周囲の画像情報から抽出した路面の消失点の変化量に基づいてヨーレートセンサが検出したヨーレートの値を補正するヨーレート補正装置の発明が開示されている。
【０００７】
特許文献２には、車載カメラ等の撮像装置で取得した車両周囲の画像情報から得られた道路情報と、車速センサが検出した車速とを用いてヨーレートセンサのオフセット誤差及びスケールファクタを補正する計測装置の発明が開示されている。
【０００８】
特許文献３には、車載カメラ等の撮像装置で取得した車両周囲の画像情報から道路のレーン中央に対する車両の横位置を検出し、検出した横位置に基づいてヨーレートを算出するヨーレート検出装置の発明が開示されている。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２０１１−１６９７２８号公報
【特許文献２】特開２０１５−１９０９２０号公報
【特許文献３】特開２０１４−２１５２３２号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかしながら、特許文献１に記載の発明は、交差点での右左折又は走行時のレーンチェンジ等の車両の旋回半径が極端に小さくなる場合は、消失点の移動距離が大きくなり、その結果、ヨーレート補正の精度が悪化するおそれがあった。
【００１１】
特許文献２に記載の発明は、車線と車両の進行方向との差分が小さいという仮定の下でヨーレートを補正するため、交差点での右左折又は走行時のレーンチェンジ等の車両の旋回半径が極端に小さくなる場合は、車線と車両の進行方向との差分が大きくなり、当該補正に誤差が生じるおそれがあった。
【００１２】
特許文献３に記載の発明は、データベースに登録された道路曲率と車速との積に基づいてヨーレートを算出しているので、当該データベースにアクセスできない場合にはヨーレートの補正ができなくなる。また、交差点での右左折又は走行時のレーンチェンジ等の車両がデータベース上の道路曲率と異なる方向に走行している場合は、ヨーレート補正に誤差が生じるおそれがあった。
【００１３】
本発明は、上記問題に鑑みてなされたものであり、撮像装置が取得した画像及び車速から推定した適正ヨーレートに基づいて車両のヨーレートセンサが検出した検出ヨーレートの誤差を算出することにより、検出ヨーレートを正確に補正可能なヨーレート補正装置を実現することを目的とする。
【課題を解決するための手段】
【００１４】
上記目的を達成するために、請求項１に記載のヨーレート補正装置は、車両周辺の画像情報を取得する撮像装置と、前記撮像装置が取得した画像情報から、目標経路と車両との横位置偏差、前記目標経路と車両のヨー角偏差、及び前記目標経路の曲率を算出する画像情報処理部と、前記目標経路上の目標点の移動と、横滑りを含む前記車両の運動との関係を表す式を用いて、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車速センサから得られる車両前後速度を含む実測値から、前記車両の適正ヨーレートを推定し、前記車両の適正ヨーレートに基づいて、前記車両が備えるヨーレートセンサが検出した検出ヨーレートの誤差を算出する演算部と、を含んでいる。
【００１５】
また、請求項２に記載のヨーレート補正装置は、請求項１に記載のヨーレート補正装置において、前記関係を表す式は、前記目標点の移動速度と前記曲率との積である前記目標点の接線方向の変化率から、前記ヨー角偏差の変化量を減算した結果と、前記適正ヨーレートとが等しい関係を表す式を含む。
【００１６】
また、請求項３に記載のヨーレート補正装置は、請求項１又は２記載のヨーレート補正装置において、前記関係を表す式は、前記車両前後速度から、前記適正ヨーレートと前記横位置偏差との積を減算したものを、前記ヨー角偏差を用いて補正した結果と、前記目標点の移動速度とが等しい関係を表す式を含む。
【００１７】
請求項４に記載のヨーレート補正装置は、請求項１〜請求項３の何れか１項記載のヨーレート補正装置において、前記演算部は、前記関係を表す式に基づいて、前回推定された、前記車両のヨーレートを含む前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、予め定められた観測方程式を用いて算出される、前記予測された次時刻の前記車両の状態量に対応する、前記横位置偏差、前記ヨー角偏差、前記曲率、及び車両前後速度と、それぞれの前記実測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を補正することにより、前記車両の適正ヨーレートを推定する更新部と、を含む。
【発明の効果】
【００１８】
本発明によれば、撮像装置が取得した画像及び車速から推定した適正ヨーレートに基づいて車両のヨーレートセンサが検出した検出ヨーレートの誤差を算出することにより、検出ヨーレートを正確に補正可能なヨーレート補正装置を実現することができるという効果を奏する。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施の形態に係るヨーレート補正装置の構成の一例を示すブロック図である。
【図２】本発明の実施の形態における座標系を示した概略図である。
【図３】撮像装置の位置、車両の重心、横位置偏差及びヨー角偏差の関係の一例を示した説明図である。
【図４】横滑りを含む車両の運動と目標経路との関係を示した説明図である。
【図５】本発明の実施の形態に係るヨーレート補正装置の入出力の概略を示した説明図である。
【図６】本発明の実施の形態に係るヨーレート補正装置の演算装置の機能ブロック図の一例である。
【図７】事前推定部の機能ブロック図の一例である。
【図８】フィルタリング部の機能ブロック図の一例である。
【図９】交差点での右左折時のように、横位置偏差とヨー角偏差とが大きい場合の車両の旋回の一例を示した概略図である。
【発明を実施するための形態】
【００２０】
以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係るヨーレート補正装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報から目標経路と車両との横位置偏差、車両のヨー角偏差及び目標経路の曲率を算出する画像情報処理部２０と、画像情報処理部２０が算出した横位置偏差、ヨー角偏差、曲率、車速センサ２４が検出した車両前後速度及びヨーレートセンサ２６が検出したヨーレートが入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両のヨーレートの演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算されたヨーレート等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。
【００２１】
続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚeとが平行で、ｙeが北方向を向いている座標系である。路面座標系２０６は、ｚrが車両２００の重心を通り路面に垂直な方向に向き、ｘrは車両進行方向に向いている座標系である。車両座標系２０８は車体バネ上に固定された座標系で、ｚvは車体鉛直上方向、ｘvは車体進行方向を向いている。
【００２２】
本実施の形態は、撮像装置２２で取得した画像情報を用いて車両２００の横速度を算出する。本実施の形態では横速度の算出に際して、図３にあるように、目標点Ｐtにおける目標経路２２０である走行車線の曲率ρに加え、センサ位置Ｐsを基準とした目標点Ｐtに対する横位置偏差ｅ1とヨー角偏差ｅ2とを想定する。なお、センサ位置Ｐ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を定義する。
【００２５】
図５は、本実施の形態に係るヨーレート補正装置１０の入出力の概略を示した説明図である。ヨーレート補正装置１０を構成する演算装置１４には、画像情報処理部２０から横位置偏差ｅ1とヨー角偏差ｅ2と曲率ρとが入力される。また、車速センサ２４からは車両２００の前後速度Ｕr（以下、「車両前後速度Ｕr」と略記）が、ヨーレートセンサ２６からはヨーレートセンサ２６が検出した検出ヨーレートＲsが、各々、演算装置１４に入力される。
【００２６】
演算装置１４は、後述する演算処理によって、横位置偏差ｅ1、ヨー角偏差ｅ2、曲率ρ及び車両前後速度Ｕrから適正ヨーレートＲrを推定して出力すると共に、適正ヨーレートＲrと検出ヨーレートＲsとの差異から算出したヨーレートセンサ誤差Ｒerrを出力する。又は、適正ヨーレートＲr（若しくはヨーレートセンサ誤差Ｒerr）に基づいて、検出ヨーレートＲsを補正する。
【００２７】
路面座標系２０６から見た目標位置Ｐtの偏差ベクトルをＰe=[ｘe ｙe ]とすると、当該偏差ベクトルＰeの微分は下記の２式で表される。
2020098116.tif 000003
【００２８】
車載カメラ等の撮像装置２２が取得した車両２００周辺の画像情報を、画像情報処理部２０において既知の手法によって画像解析することにより、目標点Ｐtにおける走行車線の曲率ρに加え、センサ位置Ｐsを基準とした目標点Ｐtに対する横位置偏差ｅ1とヨー角偏差ｅ2とが抽出される。撮像装置２２はセンサの一種であるから、当該撮像装置２２で取得した画像情報から抽出した曲率ρ、横位置偏差ｅ1及びヨー角偏差ｅ2をセンサで取得されるセンサ情報とする。さらに、車速センサ２４で検出する車両前後速度Ｕrをセンサ情報として、上記式を変形すると、下記の３式が得られる。
2020098116.tif 000004
【００２９】
さらに車体の横速度Ｖrと適正ヨーレートＲrとについて上記３式を整理すると、下記の式（１）、（２）、（３）が得られる。
2020098116.tif 000005
【００３０】
上記式（１）は、車両２００の横速度Ｖrは、車両前後速度Ｕrの目標経路２２０に対する垂直成分、言い換えれば、車両前後速度Ｕrの車両座標系２０８のｙ座標成分であるＵtsinｅ2から、目標経路との横位置偏差ｅ1の変化量（ｅ1の微分値）と、車両２００のヨー運動に伴う車両重心位置Ｐrに対する撮像装置２２の横速度Ｒrｌsを引いたものと一致することを示している。なお、上記式（１）は、目標点の移動速度の、車両前後方向に対する垂直成分から、横位置偏差の変化量と、車両のヨー運動に伴う車両重心位置に対する撮像装置の横速度とを減算した結果と、車両の横速度とが等しい関係を表す式の一例である。
【００３１】
上記式（２）は、車両２００の適正ヨーレートＲrは、目標点Ｐtの移動速度Ｕtと曲率ρとの積であり、目標経路２２０上の目標点Ｐtの接線方向の変化率ρＵtから、接線方向と車両進行方向とのヨー角偏差ｅ2の変化量（ｅ2の微分値）を引いたものと一致することを示している。なお、上記式（２）は、目標点の移動速度と曲率との積から、ヨー角偏差の変化量を減算した結果と、車両のヨーレートとが等しい関係を表す式の一例である。
【００３２】
上記式（３）は、目標経路２２０上の目標点Ｐtの移動速度Ｕtは、車両前後速度Ｕrから車両２００の適正ヨーレートＲrと横位置偏差ｅ1を乗算したＲrｅ1を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差ｅ2の余弦cosｅ2で除算することで求められることを示している。なお、上記式（３）は、車両前後速度から、車両ヨーレートと横位置偏差との積を減算したものを、ヨー角偏差を用いて補正した結果と、目標点の移動速度とが等しい関係を表す式の一例である。
【００３３】
上記式（１）〜（３）を用いて、撮像装置２２及び車速センサ２４によって取得されたデータに対して最もよくあてはまる未知変数を推定することにより、適正ヨーレートＲrの推定が可能になる。
【００３４】
上記式（１）〜（３）はいずれも運動の近似をしておらず、非線形な形を取っているが、上記式（１）〜（３）を直接用いて非線形カルマンフィルタに適用することにより、車両２００が目標経路２２０から大きく逸脱している場合でも正確な推定が可能となる。また、状態変数に関して局所線形化する事により、拡張カルマンフィルタを適用してもよい。
【００３５】
本実施の形態では、目標経路２２０と車両２００の運動との関係を用いて、状態方程式ｆ（ｘ）を定義し、演算装置１４における事前推定とフィルタリングのステップとで活用する。事前推定及びフィルタリングのステップの詳細については、後述する。
【００３６】
まず、撮像装置２２より得られる目標経路２２０に対する横位置偏差ｅ1、ヨー角偏差ｅ2及び目標経路２２０の曲率ρの情報を用いて、車両２００の適正ヨーレートＲrを推定する場合の状態方程式ｆ（ｘ）は、以下のように定義する。
【００３７】
かかる場合の状態方程式ｆ（ｘ）は、下記のような、時間ｔにおける状態量ｘ（ｔ）に対して、下記の式（４）の関係が成り立つ。
【００３８】
2020098116.tif 000006
2020098116.tif 000007
【００３９】
上記の状態量ｘ（ｔ）に対して、上記式（４）の関係が成り立つ状態方程式ｆ（ｘ）を下記のように立式する。
2020098116.tif 000008
【００４０】
式（５）、（６）は、目標経路２２０と車両２００との運動の関係を示した微分方程式である。
【００４１】
また、本実施の形態では、各状態に対するシステムノイズを下記のように定義する。
2020098116.tif 000009
【００４２】
次に観測量を下記のように定義する。
2020098116.tif 000010
【００４３】
ｙ=ｈ（ｘ）を満たす観測方程式ｈ（ｘ）は、下記の各式のように、状態量（右辺）と観測量（左辺）との対応を定義したものである。
2020098116.tif 000011
【００４４】
ただし、各観測量に対する観測ノイズを下記のように定義する。
2020098116.tif 000012
【００４５】
カルマンフィルタでは、式（５）、（６）を含む状態方程式ｆ（ｘ）の各々を離散化して使用する。従って、関数ｆ（ｘ）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘ（ｋ）＝ｆ（ｘ（ｋ−１））となる形で使用する。
【００４６】
図６は、本実施の形態に係るヨーレート補正装置１０の演算装置１４の機能ブロック図の一例である。図６に示したように、演算装置１４は、状態方程式ｆ（ｘ）を用いて、目標経路２２０である走行車線と横滑りを含む車両２００の平面運動との関係に基づく状態の推定を行うと共に、観測方程式ｈ（ｘ）を用いて、対応するセンサ情報を推定する事前推定部１０２と、事前推定部１０２が出力した事前推定値を、センサ情報を用いて補正するフィルタリング部１０４と、を含む。
【００４７】
カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆ（ｘ）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆ（ｘ）についての線形化を要さず、かつ計算負荷が比較的小さいアンセンティッドカルマンフィルタを一例として採用する。状態方程式ｆ（ｘ）が線形の場合は、線形カルマンフィルタを用いてもよいし、アンセンティッドカルマンフィルタ以外の非線形カルマンフィルタを用いてもよい。
【００４８】
図７は、事前推定部１０２の機能ブロック図の一例である。図７に示したように、事前推定部１０２は、第１アンセンティッド変換部１０２Ａと第２アンセンティッド変換部１０２Ｂとを含む。
【００４９】
第１アンセンティッド変換部１０２Ａは、状態方程式ｆ（ｘ）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘの平均及びｘの共分散行列を出力する。
【００５０】
第２アンセンティッド変換部１０２Ｂは、第１アンセンティッド変換部１０２Ａが出力した状態量ｘの平均及び状態量ｘの共分散行列を用いて、観測方程式ｈ（ｘ）に従って、対応する観測量に変換する２回目のアンセンティッド変換を行う。
【００５１】
なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。
2020098116.tif 000013
【００５２】
従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。
【００５３】
図７に示したように、第１アンセンティッド変換部１０２Ａは、シグマポイント重み係数部１０２Ａ１と、関数変換部１０２Ａ２と、Ｕ変換部１０２Ａ３と、を含む。また、第２アンセンティッド変換部１０２Ｂは、シグマポイント重み係数部１０２Ｂ１と、関数変換部１０２Ｂ２と、Ｕ変換部１０２Ｂ３と、を含む。
【００５４】
第１アンセンティッド変換部１０２Ａのシグマポイント重み係数部１０２Ａ１及び第２アンセンティッド変換部１０２Ｂのシグマポイント重み係数部１０２Ｂ１では、シグマポイントＸi：ｉ＝０、１、２、…２ｎが、下記のように選択される。
2020098116.tif 000014
【００５５】
ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。
2020098116.tif 000015
【００５６】
第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２での状態方程式ｆ（ｘ）による変換だが、第２アンセンティッド変換部１０２Ｂの関数変換部１０２Ｂ２での観測方程式ｈ（ｘ）を用いた変換では、観測値が得られる。
2020098116.tif 000016
【００５７】
第１アンセンティッド変換部１０２ＡのＵ変換部１０２Ａ３では、関数ｆ（ｘ）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘの平均値と状態量ｘの共分散行列を算出する。なお、下記式中のＱnはシステムノイズである。
2020098116.tif 000017
【００５８】
第２アンセンティッド変換部１０２ＢのＵ変換部１０２Ｂ３では、下記の計算を行う。
2020098116.tif 000018
【００５９】
図８は、フィルタリング部１０４の機能ブロック図の一例である。フィルタリング部１０４では、Ｕ変換部１０２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。
【００６０】
状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲnは観測ノイズである。
2020098116.tif 000019
【００６１】
次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。
2020098116.tif 000020
【００６２】
以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、車両２００の適正ヨーレートＲrを推定する。
【００６３】
車両２００の適正ヨーレートＲrを推定する場合、フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘ（ｋ）、Ｐx（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ（ｋ）、Ｐx（ｋ）を前回値として事前推定部１０２に入力する。
【００６４】
この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことによって推定した車両２００の適正ヨーレートＲrと、ヨーレートセンサ２６が検出した検出ヨーレートＲsとの差分により、検出ヨーレートＲsの誤差を正確に算出できる。また、検出ヨーレートＲsの誤差を用いて、ヨーレートセンサ２６が検出した検出ヨーレートＲsを補正するようにしてもよい。
【００６５】
ヨーレートセンサ２６が検出した検出ヨーレートＲsの補正は、前述のように、車両２００の操舵角が所定の値以下の場合に車両２００が直進していると仮定して、ヨーレートセンサ２６のゼロ点を補正するという方法、又はＧＰＳによって車両２００の挙動を把握してヨーレートセンサ２６検出したヨーレートを補正する方法がある。しかしながら、前者の方法では、操舵角センサのゼロ点が正確に調整されていない場合は、ヨーレートセンサ２６のゼロ点の補正が正確にできない。さらに、路面環境によっては、操舵角センサによって検出した操舵角がゼロを示していても、実際には車両２００が旋回している場合があるので、補正の精度が良好でないという問題があった。
【００６６】
また、後者の方法では、車両２００がトンネル内又はビル街等のＧＰＳ衛星からの電波の受信が困難な場所を走行している場合に、ヨーレートの補正ができないという問題があった。
【００６７】
その他、撮像装置２２で取得した画像情報から認識した車線の道路波形に基づいて車両２００のヨーレートを推定する方法も考えられるが、当該方法は、車線と車両２００の進行方向が概ね同じであると仮定している。従って、図９に示したように、交差点での右左折時のように、センサ位置Ｐsを基準とした目標点Ｐtに対する横位置偏差ｅ1とヨー角偏差ｅ2とが大きい場合は、ヨーレートの推定に誤差が生じるおそれがあった。特に交差点等での低速旋回時には、車両２００がぐるぐると回転しているような挙動を示すヨーレートが推定されるターンテーブル現象が生じ得る。
【００６８】
本実施の形態に係るヨーレート補正装置１０は、式（２）が示すように、車両２００の適正ヨーレートＲrを、目標点Ｐtの移動速度Ｕtと曲率ρとの積であり、目標経路２２０上の目標点Ｐtの接線方向の変化率ρＵtから、接線方向と車両進行方向とのヨー角偏差ｅ2の変化量（ｅ2の微分値）を引いたものと一致することに基づいて推定している。
【００６９】
また、本実施の形態に係るヨーレート補正装置１０は、式（３）が示すように、目標経路２２０上の目標点Ｐtの移動速度Ｕtを、車両前後速度Ｕrから車両２００の適正ヨーレートＲrと横位置偏差ｅ1を乗算したＲrｅ1を減算することで旋回半径の補正を行い、さらにそれをヨー角偏差ｅ2の余弦cosｅ2で除算することで推定している。
【００７０】
以上説明したように、本実施の形態に係るヨーレート補正装置１０は、既存の状態方程式に依存せず、かつ車両２００の運動について近似をしていないので、車両２００が目標経路２２０から大きく逸脱した場合でも撮像装置が取得した画像及び車速から適正ヨーレートＲrの推定が可能となる。そして、推定した適正ヨーレートＲrに基づいて、ヨーレートセンサ２６が検出した検出ヨーレートＲsを正確に補正することができる。
【符号の説明】
【００７１】
１０   ヨーレート補正装置
１２   入力装置
１４   演算装置
１６   表示装置
１８   記憶装置
２０   画像情報処理部
２２   撮像装置
２４   車速センサ
２６   ヨーレートセンサ
１０２ 事前推定部
１０２Ａ      第１アンセンティッド変換部
１０２Ａ１    シグマポイント重み係数部
１０２Ａ２    関数変換部
１０２Ａ３    Ｕ変換部
１０２Ｂ      第２アンセンティッド変換部
１０２Ｂ１    シグマポイント重み係数部
１０２Ｂ２    関数変換部
１０２Ｂ３    Ｕ変換部
１０４ フィルタリング部
２００ 車両
２０４ 地球座標系
２０６ 路面座標系
２０８ 車両座標系
２２０ 目標経路
【図１】
2020098116.tif 000021
【図２】
2020098116.tif 000022
【図３】
2020098116.tif 000023
【図４】
2020098116.tif 000024
【図５】
2020098116.tif 000025
【図６】
2020098116.tif 000026
【図７】
2020098116.tif 000027
【図８】
2020098116.tif 000028
【図９】
2020098116.tif 000029