(57)【特許請求の範囲】
【請求項１】
走行時の車両の挙動を示すピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度とを検出可能な慣性計測装置と、
前記３軸の角速度を用いた姿勢角の変化率に関する運動方程式と、前記３軸の角速度を用いた前記３軸の加速度に関する運動方程式とに基づき、前記車両の運動パターンに応じて、前記慣性計測装置が検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定する誤差推定部と、
を含むセンサ誤差補正装置。
【請求項２】
前記車両の運動パターンは前記車両のヨーレートであり、
前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定し、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する請求項1に記載のセンサ誤差補正装置。
【請求項３】
前後速度を検出する車速検出部と、
外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、
前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレート及びピッチレートが、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定する請求項２に記載のセンサ誤差補正装置。
【請求項４】
前後速度を検出する車速検出部と、
外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、
前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度、横加速度及び上下加速度が、前記車両のロールレート及びピッチレートと、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する請求項２又は３に記載のセンサ誤差補正装置。
【請求項５】
前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレートが、前記慣性計測装置が検出した横加速度の微分値から前記補正部により補正された前後速度と前記補正されたヨーレートの微分値との積を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したロールレートの誤差を推定する請求項３又は４に記載のセンサ誤差補正装置。
【請求項６】
前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のピッチレートが、前記慣性計測装置が検出した前後加速度の微分値から前記補正部により補正された前後速度の２階微分値を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したピッチレートの誤差を推定する請求項３又は４に記載のセンサ誤差補正装置。
【請求項７】
前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度が、ロールレートと重力加速度との積から、前記慣性計測装置が検出した横加速度の微分値を減算し、前記補正部により補正されたヨーレートの微分値と前記補正された前後速度との積を加算し、前記補正されたヨーレートと前記補正された前後速度の微分値との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した前後加速度の誤差を推定し、横加速度が、ピッチレートと重力加速度との積から、前記慣性計測装置が検出した前後加速度の微分値を減算し、前記補正された前後速度の２階微分値を加算し、前記補正されたヨーレートの二乗と前記補正された前後速度との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した横加速度の誤差を推定し、上下加速度が、重力加速度からロールレートと前記補正された前後速度との積を減算して得た値で表されることに基づき前記慣性計測装置が検出した上下加速度の誤差を推定する請求項４に記載のセンサ誤差補正装置。
【請求項８】
前記誤差推定部は、
前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したロールレートの誤差及びピッチレートの誤差を含む状態量の予測値を算出し、ロールレートの誤差及びピッチレートの誤差を仮定した、前記慣性計測装置が検出したロールレート及びピッチレートの観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第１事前推定部と、
前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第１事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第１事前推定部によって算出した前記状態量の予測値を補正する第１状態推定部と、
前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を含む状態量の予測値を算出し、前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を仮定した、前記慣性計測装置が検出した前後加速度、横加速度、及び上下加速度の観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第２事前推定部と、
前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第２事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第２事前推定部によって算出した前記状態量の予測値を補正する第２状態推定部と、
を含む請求項１に記載のセンサ誤差補正装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、センサ誤差補正装置に係り、特に、自車両の位置推定に用いるセンサの誤差を修正するセンサ誤差補正装置に関する。
【背景技術】
【０００２】
車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御には必要不可欠である。
【０００３】
車両の位置推定においては、ＧＰＳ（Global Positioning System）装置等により、現在位置を直接推定するのみならず、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能なＩＭＵ（Inertial Measurement Unit：慣性計測装置）、車速センサ及び操舵角センサ等の車両自身の状態を検知する各種センサで検出した自車両の移動の蓄積に基づいて自車両の位置を推定する場合がある。
【０００４】
ＩＭＵは、いわゆるゼロ点誤差であるゼロ点バイアスが補正されないと、角速度及び加速度を正確に検出することができない。航空機の分野では、ＧＰＳによって検出した位置情報により、ＩＭＵのゼロ点バイアスを補正する技術が存在する。しかしながら、遮蔽物が存在しない空中を飛翔する航空機と異なり、地上を走行する車両は、大きな建造物の近く又はトンネル等のＧＰＳ衛星からの電波が遮蔽される場所を通過する場合があり、かかる場合にはＩＭＵのゼロ点バイアスの補正が困難となる。また、ＧＰＳによってＩＭＵのゼロ点バイアスを精度よく補正するには、事前にＧＰＳアンテナ及びＩＭＵの各々のキャリブレーションが精度よく行われていることを要する。
【０００５】
特許文献１には、車速センサ及びヨーレートセンサの補正をＧＰＳで検出したドップラー速度で補正する速度推定装置の発明が開示されている。
【０００６】
特許文献２には、ＧＰＳとＩＭＵとの出力差より、各々の誤差量を推定し、自車両の位置および自車両の方位の推定精度を向上させる慣性航法の方法の発明が開示されている。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０１３−１１３７８９号公報
【特許文献２】特開平６−３１７４２８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
しかしながら、特許文献１に記載の発明は、使用可能な場合がＧＰＳ衛星からの電波が受信可能な地理的条件下に限定される上に、ヨーレート及び車速の推定が主であり、慣性センサのピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の５つの情報は補正できないという問題があった。
【０００９】
特許文献２に記載の発明は、ＩＭＵの誤差推定を高精度で行うには、高精度なＧＰＳ情報が必要だが、現在はそのようなサービスは少ないという問題があった。また、使用するＩＭＵは、航空機や船舶に用いられる高精度ＩＭＵが想定されており、ＭＥＭＳ（Micro Electro Mechanical Systems）によって量産されたＩＭＵでは同じアルゴリズムを用いても同等の性能が得られず、位置の推定値が不安定になるという問題があった。
【００１０】
本発明は、上記問題に鑑みてなされたものであり、ＩＭＵが検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定可能なセンサ誤差補正装置を実現することを目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために、請求項1に記載のセンサ誤差補正装置は、走行時の車両の挙動を示すピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度とを検出可能な慣性計測装置と、前記３軸の角速度を用いた姿勢角の変化率に関する運動方程式と、前記３軸の角速度を用いた前記３軸の加速度に関する運動方程式とに基づき、前記車両の運動パターンに応じて、前記慣性計測装置が検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定する誤差推定部と、を含んでいる。
【００１２】
また、請求項２に記載のセンサ誤差補正装置は、前記車両の運動パターンは前記車両のヨーレートであり、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定し、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。
【００１３】
また、請求項３に記載のセンサ誤差補正装置では、前記誤差補正部は、前後速度を検出する車速検出部と、外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレート及びピッチレートが、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定する。
【００１４】
また、請求項４に記載のセンサ誤差補正装置では、前後速度を検出する車速検出部と、外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度、横加速度及び上下加速度が、前記車両のロールレート及びピッチレートと、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。
【００１５】
また、請求項５に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレートが、前記慣性計測装置が検出した横加速度の微分値から前記補正部により補正された前後速度と前記補正されたヨーレートの微分値との積を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したロールレートの誤差を推定する。
【００１６】
また、請求項６に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のピッチレートが、前記慣性計測装置が検出した前後加速度の微分値から前記補正部により補正された前後速度の２階微分値を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したピッチレートの誤差を推定する。
【００１７】
また、請求項７に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度が、ロールレートと重力加速度との積から、前記慣性計測装置が検出した横加速度の微分値を減算し、前記補正部により補正されたヨーレートの微分値と前記補正された前後速度との積を加算し、前記補正されたヨーレートと前記補正された前後速度の微分値との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した前後加速度の誤差を推定し、横加速度が、ピッチレートと重力加速度との積から、前記慣性計測装置が検出した前後加速度の微分値を減算し、前記補正された前後速度の２階微分値を加算し、前記補正されたヨーレートの二乗と前記補正された前後速度との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した横加速度の誤差を推定し、上下加速度が、重力加速度からロールレートと前記補正された前後速度との積を減算して得た値で表されることに基づき前記慣性計測装置が検出した上下加速度の誤差を推定する。
【００１８】
また、請求項８に記載のセンサ誤差補正装置では、前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したロールレートの誤差及びピッチレートの誤差を含む状態量の予測値を算出し、ロールレートの誤差及びピッチレートの誤差を仮定した、前記慣性計測装置が検出したロールレート及びピッチレートの観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第１事前推定部と、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第１事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第１事前推定部によって算出した前記状態量の予測値を補正する第１状態推定部と、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を含む状態量の予測値を算出し、前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を仮定した、前記慣性計測装置が検出した前後加速度、横加速度、及び上下加速度の観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第２事前推定部と、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第２事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第２事前推定部によって算出した前記状態量の予測値を補正する第２状態推定部と、を含んでいる。
【発明の効果】
【００１９】
本発明によれば、ＩＭＵが検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定可能なセンサ誤差補正装置を実現できるという効果を奏する。
【図面の簡単な説明】
【００２０】
【図１】本発明の実施の形態に係るセンサ誤差補正装置の一例を示したブロック図である。
【図２】本発明の実施の形態における座標系を示した概略図である。
【図３】本発明の実施の形態における変数の一例を示した説明図である。
【図４】本発明乗実施の形態に係る演算装置の一例を示した概略図である。
【図５】本発明の実施の形態に係る演算装置の第１推定部の機能ブロック図の一例である。
【図６】事前推定部の機能ブロック図の一例である。
【図７】フィルタリング部の機能ブロック図の一例である。
【発明を実施するための形態】
【００２１】
以下、図面を参照して本発明の実施の形態を詳細に説明する。図１に示すように、本実施の形態に係るセンサ誤差補正装置１０は、後述する演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、車両が備えたセンシング装置及びＧＰＳ等によって取得した車両周辺の情報から車両の現在位置及び現在のヨー角（方位角）を算出する位置計測装置２０と、位置計測装置２０が算出した車両の現在位置及び現在のヨー角、車速センサ２４が検出した車両前後速度、ＩＭＵ２６が検出した車両の方位角の角速度及び加速度、並びに操舵角センサが検出した車両の操舵角が入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいて車両位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、演算装置１４で演算された車両の位置等を表示するＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。
【００２２】
本実施の形態に係る位置計測装置２０のセンシング装置は、一例として、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナーのいずれかである。車載カメラ等の撮像装置を車載のセンシング装置とした場合は、一例として、当該撮像装置で取得した車両周辺の画像情報を解析して道路の白線等を検出する。ＬＩＤＡＲを車載のセンシング装置とした場合は、一例として、車両周辺に照射したパルス状のレーザの散乱光から道路の白線等を検出する。ソナーを車載のセンシング装置とした場合は、一例として、アスファルトの路面とペイントされた白線との、超音波の反射率の差を利用して、当該白線を識別する。また、位置計測装置２０はＧＰＳにより、車両２００の緯度及び経度の各々の方向の速度情報を含む測位情報を取得可能に構成されている。
【００２３】
続いて、車両２００の挙動に係る座標系を図２に示したように定義する。地球座標系２０４は地球平面を基準として重力加速度方向とｚeとが平行で、ｙeが北方向を向いている座標系である。路面座標系２０６は、ｚrが車両２００の重心を通り路面に垂直な方向に向き、ｘrは車両進行方向に向いている座標系である。車体座標系２０８は車体バネ上に固定された座標系で、ｚvは車体鉛直上方向、ｘvは車体進行方向を向いている。従って、車両２００の前後方向は、車体座標系２０８のｘ軸に平行な方向となる。本実施の形態では、車体座標系２０８の基準点を車両２００の重心ではなく、車両２００の後輪の車軸の車幅方向の中心とする。
【００２４】
また、オイラー姿勢角であるロール角φ、ピッチ角θ及びヨー角ψは、地球座標系２０４に対して、図２に示したように定義される。例えば、ロール角φはｘ軸まわりの回転角であり、ピッチ角θは、y軸まわりの回転角であり、ヨー角ψは、z軸まわりの回転角である。また、ロール角φ、ピッチ角θ及びヨー角ψの各々は、右ネジの方向（図２では、各々の矢印方向）の回転で正の値を示す。本実施の形態では、便宜上、後述するヨー角偏差は基準座標系を路面座標系２０６とし、さらに、本来は地球座標系２０４に対して定義されるオイラー姿勢角を、車体座標系２０８に対して、ロール角φv、ピッチ角θv及びヨー角ψvと定義する。以後、単に、ロール角φ、ピッチ角θ及びヨー角ψと記した場合は、基本的に、車体座標系２０８に対して定義された姿勢角であるとする。
【００２５】
図３は、本実施の形態における変数の一例を示した説明図である。本実施の形態では、車両２００の前後速度Ｕ、車両２００の横速度Ｖ及び車両２００の上下速度Ｗの各々を定義する。Ｕはｘ軸、Ｖはｙ軸及びＷはｚ軸に各々平行する。
【００２６】
また、車両２００のロール角φ、ピッチ角θ、ヨー角ψに対応するＩＭＵ２６の出力値は、角速度であるロールレートＰ、ピッチレートＱ、ヨーレートＲと定義する。
【００２７】
従来はＩＭＵ２６、ジャイロセンサ等の精度が不十分であったこともあり、ヨーレートＲの推定にも車両運動モデルを活用していた。しかし近年、安価なＩＭＵ２６に用いられるＭＥＭＳジャイロの精度が向上していると共に、車速センサ及び操舵角センサ等の複数センサが車両２００に搭載されることで特にヨーレートＲについては補正が容易になった。
【００２８】
本実施の形態では、車両運動モデルのヨーレートは使用せず、ＩＭＵ２６が検出したヨーレートＲの値を後述するように補正して使用する。
【００２９】
図４は、演算装置１４の一例を示した概略図である。本実施の形態に係る演算装置１４は、ＧＰＳの測位情報又はセンシング装置が検出した車両２０の位置情報によりＩＭＵ２６が検出したヨーレートＲ及び車速センサ２４が検出した車両２００の前後速度Ｕを補正するＧＰＳ補正部４２と、ＩＭＵ２６が検出したロールレートＰ及びピッチレートＱの各々の誤差を推定する第１推定部４４と、ＩＭＵ２６が検出した車両２００の進行方向（ｘ軸方向）の前後加速度、車両２００の横方向（ｙ軸方向）の横加速度及び車両２００の上下方向の上下加速度の各々の誤差を推定する第２推定部４６と、を含む。演算装置１４は、車両２００の位置推定の演算を行うが、本実施の形態では、位置推定の演算に用いるＩＭＵ２６の検出値のゼロ点誤差を推定する場合に特化した説明を行い、位置推定の演算について詳細な説明は省略する。
【００３０】
ヨーレートＲは、鉛直軸であるｚ軸周りの回転運動の角速度であるから、ｘ軸及びｙ軸を含む平面座標系での回転運動の角速度である。また、車両２００の前後速度Ｕも、ヨーレートＲと同様にｘ軸及びｙ軸を含む平面座標系での運動に係る変化量である。ＧＰＳを用いると、平面座標系での運動による車両２００の位置の変化を精度よく検出できるので、ＧＰＳ補正部４２では、ＧＰＳによって得た測位情報に基づいて車速センサ２４が検出した前後速度Ｕ及びＩＭＵ２６が検出したヨーレートＲを補正する。衛星からの電波が遮蔽される等によりＧＰＳによる測位情報を得られない場合、ＧＰＳ補正部４２では、車載カメラ等の撮像装置、ＬＩＤＡＲ（Light Detection and Ranging）及びソナー等のセンシング装置によって得た車両２００の位置情報に基づいて車速センサ２４が検出した前後速度Ｕ及びＩＭＵ２６が検出したヨーレートＲを補正する。
【００３１】
第１推定部４４では、状態方程式ｆ（ｘ）を用いてＩＭＵ２６が検出したピッチレート、ロールレートの各々の誤差を推定する。第２推定部４６では、第１推定部４４とは別の状態方程式ｆ（ｘ）を用いてＩＭＵ２６が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する。状態方程式ｆ（ｘ）を用いた第１推定部４４及び第２推定部４６での処理は、後述する。
【００３２】
続いて、姿勢角推定部４０における処理について説明する。本実施の形態では、下記の式（１）により、車両２００の姿勢角の変化率である姿勢角の角速度を定義する。式（１）は、ピッチレートＰv、ロールレートＱv及びヨーレートＲvの３軸の角速度に係る方程式である。
0006981459.tif 000002
【００３３】
さらに上記式（１）及び重力加速度ｇとに基づくと、ＩＭＵ２６による各軸の加速度は下記の式で表される。なお、下記の式（２）は、ロールレートＰv、ピッチレートＱv、ヨーレートＲv、前後速度Ｕv、横速度Ｖv及び上下速度Ｗvの各変数を含む、前後加速度Ａx、横加速度Ａy及び上下加速度Ａzの３軸の加速度に係る方程式である。上記の式（１）と下記の式（２）とを用いることにより、前後加速度Ａx、横加速度Ａy、上下加速度Ａz、ピッチレートＰv、ロールレートＱv及びヨーレートＲvに係る６自由度の車両の平面運動を記述できる。
0006981459.tif 000003
【００３４】
上記の式（１）、（２）が成立するという条件の下、ＩＭＵ２６の出力のゼロ点誤差の推定を行う。ただし、車両２００が常に路面上を走行することを鑑みると、式（２）において、横速度Ｖv及び上下速度Ｗvの各々の長時間での平均値は０になると仮定できる。従って、横速度Ｖv及び上下速度Ｗv、並びに横速度Ｖvの微分値及び上下速度Ｗvの微分値を各々０とみなすと、上記の式（２）は、下記の式（３）のようになる。
0006981459.tif 000004
【００３５】
また、車両姿勢角であるロール角φv及びピッチ角θvの各々が十分に小さいと仮定すると、sinφv≒φv、cosφv≒１、sinθv≒θv、cosθv≒１とみなすことが可能なので、式（１）、（３）は、下記の式（４）のように縮退される。
0006981459.tif 000005
【００３６】
前述のように、車両２００の運動パターンであるヨーレートＲvは、ＧＰＳ補正部４２で補正されるので、補正済みの既知情報として扱える。また、ＩＭＵ２６による前後加速度Ａxの微分値、及び横加速度Ａyの微分値の各々はゼロ点誤差を含まず既知情報として扱えるので、上記の式（４）から、下記の式（５）、（６）、（７）が導かれる。式（５）、（６）、（７）において、左辺は既知な変数のみを含み、右辺は未知の変数を含む。
0006981459.tif 000006
【００３７】
上記の式（５）、（６）、（７）において、未知の変数はロールレートＰv、ピッチレートＱv、前後加速度Ａx、横加速度Ａy、上下加速度Ａzの５つである。３つの線形方程式に対して未知の変数が５つ存在するので、式（５）、（６）、（７）から一意に５つの変数を算出することはできない。従って、車両２００の走行条件によって推定する変数を変化させる。例えば、ヨーレートＲvが十分に小さい場合を想定し、Ｒv≒０とすると、上記の式（５）、（６）、（７）は、下記の式（８）、（９）のようになる。
0006981459.tif 000007
【００３８】
式（８）、（９）では未知の変数は角速度であるロールレートＰv及びピッチレートＱvのみになるので、ロールレートＰv及びピッチレートＱvの各々を推定することができ、ロールレートＰｓ及びピッチレートＱｓの各々のゼロ点誤差を推定することが可能になる。角速度のゼロ点誤差の推定が完了すると、式（５）、（６）、（７）における未知の変数は加速度のみになるので、ヨーレートＲvが大きい走行区間を想定して、前後加速度Ａx、横加速度Ａy、上下加速度Ａzの各々の値を算出する。
【００３９】
以上の計算プロセスをまとめると下記のようになる。まず、ヨーレートＲv≒０の走行条件において、ロールレートＰv及びピッチレートＱvの各々を下記の式（１０）を用いて推定し、推定されたロールレートＰv及びピッチレートＱvと、ＩＭＵ２６が検出したピッチレートＰｓ、ロールレートＱｓとの差分を、ロールレートＰｓ及びピッチレートＱｓの各々のゼロ点誤差として推定する。式（１０）は、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの車両２００の前後速度Ｕvと、ゼロ点誤差を補正済みのヨーレートＲvとで表される。より具体的には、ロールレートＰvは、ＩＭＵ２６が検出した横加速度Ａyの微分値から補正済みの前後速度Ｕvとゼロ点誤差を補正済みのヨーレートＲvの微分値との積を減算して得た値と重力加速度ｇとの商で表されることを示す。また、ピッチレートＱvは、ＩＭＵ２６が検出した前後加速度Ａxの微分値から補正済みの前後速度Ｕvの２階微分値を減算して得た値と重力加速度ｇとの商で表される。
0006981459.tif 000008
【００４０】
次いで、ヨーレートＲvが大きい区間で前後加速度Ａx、横加速度Ａy、上下加速度Ａzの各々の値を下記の式（１１）を用いて算出し、推定された前後加速度Ａx、横加速度Ａy、上下加速度Ａzと、ＩＭＵ２６が検出した前後加速度Ａｓ、横加速度Ａｓ、上下加速度Ａｓとの差分を、前後加速度Ａｓ、横加速度Ａｓ、上下加速度Ａｓの各々のゼロ点誤差として推定する。式（１０）は、上記の式（１０）等に基づいて補正されたロールレートＰv及びピッチレートＱvと、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの前後速度Ｕvと、ゼロ点誤差を補正済みのヨーレートＲvとで表される。
0006981459.tif 000009
【００４１】
上記の式（１１）は、より具体的には、前後加速度Ａxは、誤差を補正したロールレートＰvと重力加速度ｇとの積から、ＩＭＵ２６が検出した横加速度Ａyの微分値を減算し、誤差を補正したヨーレートＲvの微分値と補正済みの前後速度Ｕvとの積を加算し、誤差を補正したヨーレートＲvと補正済みの前後速度Ｕvの微分値との積を加算して得た値と誤差を補正したヨーレートＲvとの商で表されることを示している。また、上記の式（１１）は、横加速度Ａyは、誤差を補正したピッチレートＱvと重力加速度ｇとの積から、補正済みの前後加速度Ａxの微分値を減算し、補正済みの前後速度Ｕvの２階微分値を加算し、誤差を補正したヨーレートＲvの二乗と補正済みの前後速度Ｕvとの積を加算して得た値と誤差を補正したヨーレートＲvとの商で表されることを示している。また、上記の式（１１）は、上下加速度Ａzは、重力加速度ｇから誤差を補正したロールレートＲvと補正済みの前後速度Ｕvとの積を減算して得た値で表されることを示している。
【００４２】
式（１０）を用いた処理は第１推定部４４で、式（１１）を用いた処理は第２推定部で、各々実行される処理に相当する。
変形例では、状態量である変数ｘと状態方程式ｆ（ｘ）とに基づき、非線形方程式を扱うことが可能なアンセンティッドカルマンフィルタ（以下、「ＵＫＦ」と略記）を用いた演算により、上記のような方程式の縮退を行わずに、ＩＭＵ２６のゼロ点誤差の推定を行う。
【００４３】
以下に、変形例における、第１推定部４４で行われるＵＫＦを用いた角速度誤差推定について説明する。ＵＫＦを用いる場合も、式（１０）を用いる場合と同様に、第１推定部４４は、ヨーレートＲvが十分に小さい走行条件において、ＩＭＵ２６が検出したロールレートＰv及びピッチレートＱvのゼロ点誤差を推定する。ＵＫＦを用いた角速度誤差推定に係る状態変数ｘgを下記の式（１２）のように定義する。下記の式（１２）中のε1はロールレートＰvのゼロ点誤差、ε2はピッチレートＱvのゼロ点誤差である。
0006981459.tif 000010
【００４４】
上記の状態量ｘgに対して、ｄｘ/ｄｔ＝ｆg（ｘg）の関係が成り立つ状態方程式ｆg（ｘg）を下記の式（１３）、（１４）、（１５）のように立式する。式（１４）、（１５）は、上述の式（１）に含まれる状態方程式である。
0006981459.tif 000011
【００４５】
また、各状態量ｘgに対するシステムノイズＱgを下記のように定義する。本実施の形態では、Ｕvの微分、Ｐv、Ｑv、Ｒv、ε1、ε2については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。
0006981459.tif 000012
【００４６】
次に観測量を下記のように観測行列として定義する。式（１６）の右辺のＵsは車速センサ２４が検出した車速の値であり、Ｐｓ、Ｑs、Ｒsの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａx、Ａyの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。
0006981459.tif 000013
【００４７】
本実施の形態では、ＩＭＵ２６が検出したヨーレートＲsはＧＰＳ等によってゼロ点誤差が補正可能なので、ロールレートＰｓとピッチレートＱsとにゼロ点誤差が存在すると仮定する。従って、観測値であるロールレートＰｓとピッチレートＱsとに対する観測方程式ｈg（ｘg）は、下記の式（１７）、（１８）のようになる。前述のように、ε1はロールレートＰｓのゼロ点誤差、ε2はピッチレートＱsのゼロ点誤差である。
0006981459.tif 000014
【００４８】
ただし、各観測量に対する観測ノイズＲgを下記のように定義する。
0006981459.tif 000015
【００４９】
ＵＫＦでは、式（１３）、（１４）、（１５）で示した状態方程式ｆg（ｘg）の各々を離散化して使用する。従って、関数ｆg（ｘg）の入出力関係を、時間ｔ＝ｋ、ｋ−１に対して、ｘg（ｋ）＝ｆg（ｘg（ｋ−１））となる形で使用する。
【００５０】
図５は、本実施の形態に係る演算装置１４の第１推定部４４の機能ブロック図の一例である。図５に示したように、第１推定部４４は、状態方程式ｆg（ｘg）と観測方程式ｈg（ｘg）とを用いて、ＩＭＵ２６の検出値を推定する事前推定部１０２と、事前推定部１０２が出力した事前推定値を、ＩＭＵ２６の検出値を用いて補正するフィルタリング部１０４と、を含む。
【００５１】
カルマンフィルタは線形・非線形を含め種々の手法が提案されている。本実施の形態では、前述のように状態方程式ｆg（ｘg）が非線形である事を踏まえ、非線形カルマンフィルタを活用する例を示す。中でも状態方程式ｆg（ｘg）についての線形化を要さず、かつ計算負荷が比較的小さいＵＫＦを一例として採用する。状態方程式ｆg（ｘg）が線形の場合は、線形カルマンフィルタを用いてもよいし、ＵＫＦ以外の非線形カルマンフィルタを用いてもよい。
【００５２】
図６は、事前推定部１０２の機能ブロック図の一例である。図６に示したように、事前推定部１０２は、第１アンセンティッド変換部１０２Ａと第２アンセンティッド変換部１０２Ｂとを含む。
【００５３】
第１アンセンティッド変換部１０２Ａは、状態方程式ｆg（ｘg）に基づいて状態量を更新する1回目のアンセンティッド変換(Unscented transfer)を行って、ｘgの平均及びｘgの共分散行列を出力する。
【００５４】
第２アンセンティッド変換部１０２Ｂは、第１アンセンティッド変換部１０２Ａが出力した状態量ｘgの平均及び状態量ｘgの共分散行列を用いて、観測方程式ｈg（ｘg）に従って、対応する観測量ｙgに変換する２回目のアンセンティッド変換を行う。
【００５５】
なお、アンセンティッド変換の目的は、ある非線形関数ｙ＝ｆ（ｘ）による変換において、下記の観測量ｙの平均及び共分散行列を精度よく求めることにある。
0006981459.tif 000016
【００５６】
従って、本実施の形態は、平均値と標準偏差とに対応する2n+1個のサンプル(シグマポイント）を用いて、確率密度関数を近似することを特徴とする。
【００５７】
図６に示したように、第１アンセンティッド変換部１０２Ａは、シグマポイント重み係数部１０２Ａ１と、関数変換部１０２Ａ２と、Ｕ変換部１０２Ａ３と、を含む。また、第２アンセンティッド変換部１０２Ｂは、シグマポイント重み係数部１０２Ｂ１と、関数変換部１０２Ｂ２と、Ｕ変換部１０２Ｂ３と、を含む。
【００５８】
第１アンセンティッド変換部１０２Ａのシグマポイント重み係数部１０２Ａ１及び第２アンセンティッド変換部１０２Ｂのシグマポイント重み係数部１０２Ｂ１では、シグマポイントＸi：ｉ＝０、１、２、…２ｎが、下記のように選択される。
0006981459.tif 000017
【００５９】
ただし、スケーリングファクタκは、κ≧０となるように選択する。また、シグマポイントに対する重みは下記のように定義する。
0006981459.tif 000018
【００６０】
第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２における、非線形関数ｆ（ｘ）による各シグマポイントの変換は下記のようになる。下記は、第１アンセンティッド変換部１０２Ａの関数変換部１０２Ａ２での状態方程式ｆg（ｘ）による変換だが、第２アンセンティッド変換部１０２Ｂの関数変換部１０２Ｂ２での観測方程式ｈg（ｘ）を用いた変換では、観測値が得られる。
0006981459.tif 000019
【００６１】
第１アンセンティッド変換部１０２ＡのＵ変換部１０２Ａ３では、関数ｆg（ｘg）によって変換された値と、前述の重み係数とを用いて、下記のように状態量ｘgの平均値と状態量ｘgの共分散行列を算出する。なお、下記式中のＱgはシステムノイズである。
0006981459.tif 000020
【００６２】
第２アンセンティッド変換部１０２ＢのＵ変換部１０２Ｂ３では、下記の計算を行う。
0006981459.tif 000021
【００６３】
図７は、フィルタリング部１０４の機能ブロック図の一例である。フィルタリング部１０４では、Ｕ変換部１０２Ｂ３で計算された、状態量の事前予測値に対応する観測値と、実際に観測された観測値との差を比較し、状態量の予測値を補正する処理を行う。
【００６４】
状態量の予測値を実際に観測された値でフィードバックする処理はカルマンゲイン(Kalman Gain)と呼ばれ、次式で計算される。なお、次式のＲgは観測ノイズである。
0006981459.tif 000022
【００６５】
次に、このカルマンゲインを用いて、状態量の事前予測値を補正する処理を次のように行う。
0006981459.tif 000023
【００６６】
以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、ロールレートＰv及びピッチレートＱvの推定値を算出することにより、ＩＭＵ２６が検出したロールレートＰｓのゼロ点誤差ε1及びピッチレートＱsのゼロ点誤差ε2を推定することができる。
【００６７】
ＩＭＵ２６が検出したロールレートＰｓのゼロ点誤差ε1及びピッチレートＱsのゼロ点誤差ε2を推定する場合、フィルタリング部１０４が出力したｘg（ｋ）、Ｐxg（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘg（ｋ）、Ｐxg（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘg（ｋ）、Ｐxg（ｋ）を前回値として事前推定部１０２に入力する。
【００６８】
第１推定部４４は、ヨーレートＲvが十分に小さい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出したロールレートＰｓのゼロ点誤差ε1及びピッチレートＱsのゼロ点誤差ε2を推定する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、ロールレートＰｓのゼロ点誤差ε1及びピッチレートＱsのゼロ点誤差ε2を精度よく推定できる。
【００６９】
続いて、第２推定部４６におけるＵＫＦを用いた加速度誤差推定について説明する。ＵＫＦを用いる場合も、式(１１)を用いる場合と同様に、第２推定部４６は、車両２００が旋回中等のヨーレートＲvが比較的大きい走行条件において、ＩＭＵ２６が検出した前後加速度Ａx及び横加速度Ａyのゼロ点誤差を推定する。
【００７０】
ＵＫＦを用いた角速度誤差推定に係る状態変数ｘaを下記の式（１９）のように定義する。下記の式（１９）中のε3は前後加速度Ａxのゼロ点誤差、ε4は横加速度Ａyのゼロ点誤差、ε5は上下加速度Ａzのゼロ点誤差である。
0006981459.tif 000024
【００７１】
上記の状態量ｘaに対して、ｄｘ/ｄｔ＝ｆa（ｘa）の関係が成り立つ状態方程式ｆa（ｘa）を下記の式（２０）、（２１）、（２２）のように立式する。式（２１）、（２２）は、上述の式（１）に含まれる状態方程式である。
0006981459.tif 000025
【００７２】
また、各状態量ｘaに対するシステムノイズＱaを下記のように定義する。本実施の形態では、Ｕvの微分、Ｐv、Ｑv、Ｒv、ε3、ε4、ε5については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。
0006981459.tif 000026
【００７３】
次に観測量を下記のように観測行列として定義する。式（２３）の右辺のＵsは車速センサ２４が検出した車速の値であり、Ｐｓ、Ｑs、Ｒsの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａx、Ａyの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。
0006981459.tif 000027
【００７４】
観測値である前後加速度Ａx、横加速度Ａy、上下加速度Ａzに対する観測方程式ｈa（ｘa）は、下記の式（２４）、（２５）、（２６）のようになる。前述のように、ε3は前後加速度Ａxのゼロ点誤差、ε4は横加速度Ａyのゼロ点誤差、ε5は上下加速度Ａzのゼロ点誤差である。
0006981459.tif 000028
【００７５】
ただし、各観測量に対する観測ノイズＲaを下記のように定義する。
0006981459.tif 000029
【００７６】
以上の状態方程式である式（２０）、（２１）、（２２）、観測方程式である式（２４）、（２５）、（２６）を用いて、上述の角速度誤差推定の場合と同様にＵＫＦにより加速度誤差推定を行う。
第２推定部４６は、ヨーレートＲvが比較的大きい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出した前後加速度Ａxのゼロ点誤差ε３、横加速度Ａyのゼロ点誤差ε４、上下加速度Ａzのゼロ点誤差ε５を推定する。ＵＫＦによる演算は、上述の角速度誤差推定の場合と同様なので、詳細な説明は省略する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、角速度の誤差及び加速度の誤差を精度よく推定できる。
【００７７】
以上説明したように、本実施の形態に係るセンサ誤差補正装置によれば、ロールレートＰｓのゼロ点誤差ε1、ピッチレートＱsのゼロ点誤差ε2、前後加速度Ａxのゼロ点誤差ε3、横加速度Ａyのゼロ点誤差ε4及び上下加速度Ａzのゼロ点誤差ε5を精度よく推定できる。ゼロ点誤差を補正したロールレートＰv、ピッチレートＱv、ヨーレートＲv、前後加速度Ａx、横加速度Ａy及び上下加速度Ａzを用いることにより、車両２００の位置推定の精度を向上させることが可能となる。
【００７８】
また、本実施の形態に係るセンサ誤差補正装置１０は、非線形モデルであるＵＫＦを用いるので、ＩＭＵ２６が検出した値の直接微分又は近似を用いることを要しない。従って、本実施の形態に係るセンサ誤差補正装置１０は、ＩＭＵ２６が検出した角速度及び加速度のゼロ点誤差を高精度で推定することができる。
【符号の説明】
【００７９】
１０   センサ誤差補正装置
１２   入力装置
１４   演算装置
１６   表示装置
１８   記憶装置
２０   位置計測装置
２４   車速センサ
４０   姿勢角推定部
４２   ＧＰＳ補正部
４４   第１推定部
４６   第２推定部
１０２ 事前推定部
１０４ フィルタリング部
２００ 車両
【図１】
0006981459.tif 000030
【図２】
0006981459.tif 000031
【図３】
0006981459.tif 000032
【図４】
0006981459.tif 000033
【図５】
0006981459.tif 000034
【図６】
0006981459.tif 000035
【図７】
0006981459.tif 000036