JP6981459B2 - センサ誤差補正装置 - Google Patents

Description

本発明は、センサ誤差補正装置に係り、特に、自車両の位置推定に用いるセンサの誤差を修正するセンサ誤差補正装置に関する。

車両の姿勢角及び位置に係る情報は、運転支援技術及び予防安全技術にとって最も重要な情報である。例えば、車体のロール角及びピッチ角は、重力加速度成分を補償する制御入力（操舵角、アクセル及びブレーキ）を決定する際に活用でき、より安定した車両の制御が可能になる。さらに、位置推定は車両の位置制御には必要不可欠である。

車両の位置推定においては、ＧＰＳ（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の３軸の角速度に係る方程式である。

さらに上記式（１）及び重力加速度ｇとに基づくと、ＩＭＵ２６による各軸の加速度は下記の式で表される。なお、下記の式（２）は、ロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_v、前後速度Ｕ_v、横速度Ｖ_v及び上下速度Ｗ_vの各変数を含む、前後加速度Ａ_x、横加速度Ａ_y及び上下加速度Ａ_zの３軸の加速度に係る方程式である。上記の式（１）と下記の式（２）とを用いることにより、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_z、ピッチレートＰ_v、ロールレートＱ_v及びヨーレートＲ_vに係る６自由度の車両の平面運動を記述できる。

上記の式（１）、（２）が成立するという条件の下、ＩＭＵ２６の出力のゼロ点誤差の推定を行う。ただし、車両２００が常に路面上を走行することを鑑みると、式（２）において、横速度Ｖ_v及び上下速度Ｗ_vの各々の長時間での平均値は０になると仮定できる。従って、横速度Ｖ_v及び上下速度Ｗ_v、並びに横速度Ｖ_vの微分値及び上下速度Ｗ_vの微分値を各々０とみなすと、上記の式（２）は、下記の式（３）のようになる。

また、車両姿勢角であるロール角φ_v及びピッチ角θ_vの各々が十分に小さいと仮定すると、sinφ_v≒φ_v、cosφ_v≒１、sinθ_v≒θ_v、cosθ_v≒１とみなすことが可能なので、式（１）、（３）は、下記の式（４）のように縮退される。

前述のように、車両２００の運動パターンであるヨーレートＲ_vは、ＧＰＳ補正部４２で補正されるので、補正済みの既知情報として扱える。また、ＩＭＵ２６による前後加速度Ａ_xの微分値、及び横加速度Ａ_yの微分値の各々はゼロ点誤差を含まず既知情報として扱えるので、上記の式（４）から、下記の式（５）、（６）、（７）が導かれる。式（５）、（６）、（７）において、左辺は既知な変数のみを含み、右辺は未知の変数を含む。

上記の式（５）、（６）、（７）において、未知の変数はロールレートＰ_v、ピッチレートＱ_v、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの５つである。３つの線形方程式に対して未知の変数が５つ存在するので、式（５）、（６）、（７）から一意に５つの変数を算出することはできない。従って、車両２００の走行条件によって推定する変数を変化させる。例えば、ヨーレートＲ_vが十分に小さい場合を想定し、Ｒ_v≒０とすると、上記の式（５）、（６）、（７）は、下記の式（８）、（９）のようになる。

式（８）、（９）では未知の変数は角速度であるロールレートＰ_v及びピッチレートＱ_vのみになるので、ロールレートＰ_v及びピッチレートＱ_vの各々を推定することができ、ロールレートＰ_ｓ及びピッチレートＱ_ｓの各々のゼロ点誤差を推定することが可能になる。角速度のゼロ点誤差の推定が完了すると、式（５）、（６）、（７）における未知の変数は加速度のみになるので、ヨーレートＲ_vが大きい走行区間を想定して、前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの各々の値を算出する。

以上の計算プロセスをまとめると下記のようになる。まず、ヨーレートＲ_v≒０の走行条件において、ロールレートＰ_v及びピッチレートＱ_vの各々を下記の式（１０）を用いて推定し、推定されたロールレートＰ_v及びピッチレートＱ_vと、ＩＭＵ２６が検出したピッチレートＰｓ、ロールレートＱｓとの差分を、ロールレートＰ_ｓ及びピッチレートＱ_ｓの各々のゼロ点誤差として推定する。式（１０）は、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの車両２００の前後速度Ｕ_vと、ゼロ点誤差を補正済みのヨーレートＲ_vとで表される。より具体的には、ロールレートＰ_vは、ＩＭＵ２６が検出した横加速度Ａ_yの微分値から補正済みの前後速度Ｕ_vとゼロ点誤差を補正済みのヨーレートＲ_vの微分値との積を減算して得た値と重力加速度ｇとの商で表されることを示す。また、ピッチレートＱ_vは、ＩＭＵ２６が検出した前後加速度Ａ_xの微分値から補正済みの前後速度Ｕ_vの２階微分値を減算して得た値と重力加速度ｇとの商で表される。

次いで、ヨーレートＲ_vが大きい区間で前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zの各々の値を下記の式（１１）を用いて算出し、推定された前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zと、ＩＭＵ２６が検出した前後加速度Ａ_ｓ、横加速度Ａ_ｓ、上下加速度Ａ_ｓとの差分を、前後加速度Ａ_ｓ、横加速度Ａ_ｓ、上下加速度Ａ_ｓの各々のゼロ点誤差として推定する。式（１０）は、上記の式（１０）等に基づいて補正されたロールレートＰ_v及びピッチレートＱ_vと、加速度センサであるＩＭＵ２６の出力の微分値と、補正済みの前後速度Ｕ_vと、ゼロ点誤差を補正済みのヨーレートＲ_vとで表される。

上記の式（１１）は、より具体的には、前後加速度Ａ_xは、誤差を補正したロールレートＰ_vと重力加速度ｇとの積から、ＩＭＵ２６が検出した横加速度Ａ_yの微分値を減算し、誤差を補正したヨーレートＲ_vの微分値と補正済みの前後速度Ｕ_vとの積を加算し、誤差を補正したヨーレートＲ_vと補正済みの前後速度Ｕ_vの微分値との積を加算して得た値と誤差を補正したヨーレートＲ_vとの商で表されることを示している。また、上記の式（１１）は、横加速度Ａ_yは、誤差を補正したピッチレートＱ_vと重力加速度ｇとの積から、補正済みの前後加速度Ａ_xの微分値を減算し、補正済みの前後速度Ｕ_vの２階微分値を加算し、誤差を補正したヨーレートＲ_vの二乗と補正済みの前後速度Ｕ_vとの積を加算して得た値と誤差を補正したヨーレートＲ_vとの商で表されることを示している。また、上記の式（１１）は、上下加速度Ａ_zは、重力加速度ｇから誤差を補正したロールレートＲ_vと補正済みの前後速度Ｕ_vとの積を減算して得た値で表されることを示している。

式（１０）を用いた処理は第１推定部４４で、式（１１）を用いた処理は第２推定部で、各々実行される処理に相当する。
変形例では、状態量である変数ｘと状態方程式ｆ（ｘ）とに基づき、非線形方程式を扱うことが可能なアンセンティッドカルマンフィルタ（以下、「ＵＫＦ」と略記）を用いた演算により、上記のような方程式の縮退を行わずに、ＩＭＵ２６のゼロ点誤差の推定を行う。

以下に、変形例における、第１推定部４４で行われるＵＫＦを用いた角速度誤差推定について説明する。ＵＫＦを用いる場合も、式（１０）を用いる場合と同様に、第１推定部４４は、ヨーレートＲ_vが十分に小さい走行条件において、ＩＭＵ２６が検出したロールレートＰ_v及びピッチレートＱ_vのゼロ点誤差を推定する。ＵＫＦを用いた角速度誤差推定に係る状態変数ｘ_gを下記の式（１２）のように定義する。下記の式（１２）中のε₁はロールレートＰ_vのゼロ点誤差、ε₂はピッチレートＱ_vのゼロ点誤差である。

上記の状態量ｘ_gに対して、ｄｘ/ｄｔ＝ｆ_g（ｘ_g）の関係が成り立つ状態方程式ｆ_g（ｘ_g）を下記の式（１３）、（１４）、（１５）のように立式する。式（１４）、（１５）は、上述の式（１）に含まれる状態方程式である。

また、各状態量ｘ_gに対するシステムノイズＱ_gを下記のように定義する。本実施の形態では、Ｕ_vの微分、Ｐ_v、Ｑ_v、Ｒ_v、ε₁、ε₂については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。

次に観測量を下記のように観測行列として定義する。式（１６）の右辺のＵ_sは車速センサ２４が検出した車速の値であり、Ｐ_ｓ、Ｑ_s、Ｒ_sの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａ_x、Ａ_yの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。

本実施の形態では、ＩＭＵ２６が検出したヨーレートＲ_sはＧＰＳ等によってゼロ点誤差が補正可能なので、ロールレートＰ_ｓとピッチレートＱ_sとにゼロ点誤差が存在すると仮定する。従って、観測値であるロールレートＰ_ｓとピッチレートＱ_sとに対する観測方程式ｈ_g（ｘ_g）は、下記の式（１７）、（１８）のようになる。前述のように、ε₁はロールレートＰ_ｓのゼロ点誤差、ε₂はピッチレートＱ_sのゼロ点誤差である。

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

ＵＫＦでは、式（１３）、（１４）、（１５）で示した状態方程式ｆ_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に変換する２回目のアンセンティッド変換を行う。

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

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

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

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

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

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

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

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

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

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

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

以上の事前推定部１０２及びフィルタリング部１０４の処理を各タイムステップごとに繰り返すことにより、ロールレートＰ_v及びピッチレートＱ_vの推定値を算出することにより、ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定することができる。

ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定する場合、フィルタリング部１０４が出力したｘ_g（ｋ）、Ｐ_xg（ｋ）を前回値として事前推定部１０２に入力し、上述のように２段階のアンセンティッド変換を行って状態量の事前予測値及び対応する観測値を計算する。計算した状態量の予測値は、フィルタリング部１０４でカルマンゲイン及び実際に観測された最新の観測値を用いて補正される。フィルタリング部１０４が出力したｘ_g（ｋ）、Ｐ_xg（ｋ）は、記憶装置１８に一時的にホールドし、記憶装置１８にホールドしたｘ_g（ｋ）、Ｐ_xg（ｋ）を前回値として事前推定部１０２に入力する。

第１推定部４４は、ヨーレートＲ_vが十分に小さい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出したロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を推定する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、ロールレートＰ_ｓのゼロ点誤差ε₁及びピッチレートＱ_sのゼロ点誤差ε₂を精度よく推定できる。

続いて、第２推定部４６におけるＵＫＦを用いた加速度誤差推定について説明する。ＵＫＦを用いる場合も、式(１１)を用いる場合と同様に、第２推定部４６は、車両２００が旋回中等のヨーレートＲ_vが比較的大きい走行条件において、ＩＭＵ２６が検出した前後加速度Ａ_x及び横加速度Ａ_yのゼロ点誤差を推定する。

ＵＫＦを用いた角速度誤差推定に係る状態変数ｘ_aを下記の式（１９）のように定義する。下記の式（１９）中のε₃は前後加速度Ａ_xのゼロ点誤差、ε₄は横加速度Ａ_yのゼロ点誤差、ε₅は上下加速度Ａ_zのゼロ点誤差である。

上記の状態量ｘ_aに対して、ｄｘ/ｄｔ＝ｆ_a（ｘ_a）の関係が成り立つ状態方程式ｆ_a（ｘ_a）を下記の式（２０）、（２１）、（２２）のように立式する。式（２１）、（２２）は、上述の式（１）に含まれる状態方程式である。

また、各状態量ｘ_aに対するシステムノイズＱ_aを下記のように定義する。本実施の形態では、Ｕ_vの微分、Ｐ_v、Ｑ_v、Ｒ_v、ε₃、ε_4、ε₅については、白色ノイズによって駆動されるランダムウォークモデルとし、1次のマルコフモデルを採用してもよい。

次に観測量を下記のように観測行列として定義する。式（２３）の右辺のＵ_sは車速センサ２４が検出した車速の値であり、Ｐ_ｓ、Ｑ_s、Ｒ_sの各々は角速度センサとして機能するＩＭＵ２６が検出したロールレート、ピッチレート、ヨーレートの各々の値である。また、Ａ_x、Ａ_yの各々は加速度センサとして機能するＩＭＵ２６が検出した前後加速度、横加速度の各々の値である。

観測値である前後加速度Ａ_x、横加速度Ａ_y、上下加速度Ａ_zに対する観測方程式ｈ_a（ｘ_a）は、下記の式（２４）、（２５）、（２６）のようになる。前述のように、ε₃は前後加速度Ａ_xのゼロ点誤差、ε₄は横加速度Ａ_yのゼロ点誤差、ε₅は上下加速度Ａ_zのゼロ点誤差である。

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

以上の状態方程式である式（２０）、（２１）、（２２）、観測方程式である式（２４）、（２５）、（２６）を用いて、上述の角速度誤差推定の場合と同様にＵＫＦにより加速度誤差推定を行う。
第２推定部４６は、ヨーレートＲ_vが比較的大きい走行条件において、この状態量の事前予測値の計算と実際に観測された最新の値での補正を繰り返すことにより、ＩＭＵ２６が検出した前後加速度Ａ_xのゼロ点誤差ε_３、横加速度Ａ_yのゼロ点誤差ε_４、上下加速度Ａ_zのゼロ点誤差ε_５を推定する。ＵＫＦによる演算は、上述の角速度誤差推定の場合と同様なので、詳細な説明は省略する。ＵＫＦを用いることにより、非線形の方程式を近似によって縮退することを要しないので、角速度の誤差及び加速度の誤差を精度よく推定できる。

以上説明したように、本実施の形態に係るセンサ誤差補正装置によれば、ロールレートＰ_ｓのゼロ点誤差ε₁、ピッチレートＱ_sのゼロ点誤差ε₂、前後加速度Ａ_xのゼロ点誤差ε₃、横加速度Ａ_yのゼロ点誤差ε₄及び上下加速度Ａ_zのゼロ点誤差ε₅を精度よく推定できる。ゼロ点誤差を補正したロールレートＰ_v、ピッチレートＱ_v、ヨーレートＲ_v、前後加速度Ａ_x、横加速度Ａ_y及び上下加速度Ａ_zを用いることにより、車両２００の位置推定の精度を向上させることが可能となる。

また、本実施の形態に係るセンサ誤差補正装置１０は、非線形モデルであるＵＫＦを用いるので、ＩＭＵ２６が検出した値の直接微分又は近似を用いることを要しない。従って、本実施の形態に係るセンサ誤差補正装置１０は、ＩＭＵ２６が検出した角速度及び加速度のゼロ点誤差を高精度で推定することができる。

１０ センサ誤差補正装置
１２ 入力装置
１４ 演算装置
１６ 表示装置
１８ 記憶装置
２０ 位置計測装置
２４ 車速センサ
４０ 姿勢角推定部
４２ ＧＰＳ補正部
４４ 第１推定部
４６ 第２推定部
１０２ 事前推定部
１０４ フィルタリング部
２００ 車両

Claims (8)

1. 走行時の車両の挙動を示すピッチレート、ロールレート及びヨーレートの３軸の角速度と、前後加速度、横加速度及び上下加速度の３軸の加速度とを検出可能な慣性計測装置と、
   前記３軸の角速度を用いた姿勢角の変化率に関する運動方程式と、前記３軸の角速度を用いた前記３軸の加速度に関する運動方程式とに基づき、前記車両の運動パターンに応じて、前記慣性計測装置が検出したピッチレート、ロールレート、前後加速度、横加速度及び上下加速度の各々の誤差を推定する誤差推定部と、
   を含むセンサ誤差補正装置。
2. 前記車両の運動パターンは前記車両のヨーレートであり、
   前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定し、前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する請求項1に記載のセンサ誤差補正装置。
3. 前後速度を検出する車速検出部と、
   外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、
   前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレート及びピッチレートが、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき前記慣性計測装置が検出したピッチレート及びロールレートの各々の誤差を推定する請求項２に記載のセンサ誤差補正装置。
4. 前後速度を検出する車速検出部と、
   外部情報に基づいて、前記車速検出部が検出した前後速度の誤差と、前記慣性計測装置が検出したヨーレートの誤差とを補正する補正部と、
   前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度、横加速度及び上下加速度が、前記車両のロールレート及びピッチレートと、前記慣性計測装置が検出した前後加速度及び横加速度の各々の微分値と、前記補正部により補正された前後速度及びヨーレートとで表されることに基づき、前記慣性計測装置が検出した前後加速度、横加速度及び上下加速度の各々の誤差を推定する請求項２又は３に記載のセンサ誤差補正装置。
5. 前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のロールレートが、前記慣性計測装置が検出した横加速度の微分値から前記補正部により補正された前後速度と前記補正されたヨーレートの微分値との積を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したロールレートの誤差を推定する請求項３又は４に記載のセンサ誤差補正装置。
6. 前記誤差推定部は、前記車両のヨーレートが小さい場合に、前記車両のピッチレートが、前記慣性計測装置が検出した前後加速度の微分値から前記補正部により補正された前後速度の２階微分値を減算して得た値と重力加速度との商で表されることに基づき前記慣性計測装置が検出したピッチレートの誤差を推定する請求項３又は４に記載のセンサ誤差補正装置。
7. 前記誤差推定部は、前記車両のヨーレートが大きい場合に、前後加速度が、ロールレートと重力加速度との積から、前記慣性計測装置が検出した横加速度の微分値を減算し、前記補正部により補正されたヨーレートの微分値と前記補正された前後速度との積を加算し、前記補正されたヨーレートと前記補正された前後速度の微分値との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した前後加速度の誤差を推定し、横加速度が、ピッチレートと重力加速度との積から、前記慣性計測装置が検出した前後加速度の微分値を減算し、前記補正された前後速度の２階微分値を加算し、前記補正されたヨーレートの二乗と前記補正された前後速度との積を加算して得た値と前記補正されたヨーレートとの商で表されることに基づき前記慣性計測装置が検出した横加速度の誤差を推定し、上下加速度が、重力加速度からロールレートと前記補正された前後速度との積を減算して得た値で表されることに基づき前記慣性計測装置が検出した上下加速度の誤差を推定する請求項４に記載のセンサ誤差補正装置。
8. 前記誤差推定部は、
   前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出したロールレートの誤差及びピッチレートの誤差を含む状態量の予測値を算出し、ロールレートの誤差及びピッチレートの誤差を仮定した、前記慣性計測装置が検出したロールレート及びピッチレートの観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第１事前推定部と、
   前記車両のヨーレートが小さい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第１事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第１事前推定部によって算出した前記状態量の予測値を補正する第１状態推定部と、
   前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出した前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を含む状態量の予測値を算出し、前後加速度の誤差、横加速度の誤差、及び上下加速度の誤差を仮定した、前記慣性計測装置が検出した前後加速度、横加速度、及び上下加速度の観測値に対する観測方程式を用いて、前記状態量の予測値から、前記慣性計測装置が検出した観測値の予測値を算出する第２事前推定部と、
   前記車両のヨーレートが大きい場合に、前記慣性計測装置が検出して出力した前記観測値と、前記第２事前推定部が算出した前記観測値の予測値との差分に基づいて、前記第２事前推定部によって算出した前記状態量の予測値を補正する第２状態推定部と、
   を含む請求項１に記載のセンサ誤差補正装置。
   
   
   

Images