JP6531768B2 - センサ誤差補正装置及び方法 - Google Patents

Description

本発明は、センサ誤差補正装置及び方法に関し、特に、移動体に搭載されたセンサの誤差を補正するセンサ誤差補正装置及び方法の技術分野に関する。

この種の装置として、例えば、複数のＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星各々から発信されたＧＰＳ信号に基づいて得られる自車両の速度ベクトルを所定時間分積分することにより、自車両の軌跡を算出すると共に、自車両の軌跡上の各エポックにおけるＧＰＳ信号から得られるＧＰＳ擬似距離データと、自車両とＧＰＳ衛星との間の距離との差分に基づいて、自車両の軌跡を平行移動する（即ち、補正する）装置が提案されている（特許文献１参照）。

尚、前後車体速度の推定値、横車体速度の推定値、上下加速度の検出値、ロール角速度の検出値、ロール角の前回推定値及びピッチ角の前回推定値に基づいてピッチ角速度を推定し、該推定されたピッチ角速度から、車両運動固有の特徴が拘束条件に加えられたオブザーバを用いて、ロール角及びピッチ角を推定する装置が提案されている（特許文献２参照）。自動二輪車に固有の運動条件を仮定して、カルマンフィルタを用いて、上下加速度、前後加速度、左右加速度及び後輪速度センサ各々の検出値、並びにロール角、車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット及び上下加速度センサオフセット各々の前回推定値に基づいて、ロール角、車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット及び上下加速度センサオフセットを推定する装置が提案されている（特許文献３参照）。

特許第０５６７３０７１号 特許第０５０２９４４２号 特許第０５３９３４３２号

特許文献１に記載の技術では、車両のロール角が考慮されていない。このため、特許文献１に記載の技術では、車両がカント（“バンク”とも言う）のある道路を走行した場合に求められる車両の軌跡に係る方位（即ち、車両の移動方位）が、実際の方位からずれてしまう。ここで、車両の横加速度からロール角を推定することは可能である。しかしながら、横加速度を計測する横加速度センサの誤差を補正することが難しく、ロール角の推定精度を向上させることが難しいという技術的問題点がある。

本発明は、上記問題点に鑑みてなされたものであり、横加速度センサの誤差を適切に補正することができるセンサ誤差補正装置及び方法を提供することを課題とする。

本発明の第１のセンサ誤差補正装置は、上記課題を解決するために、移動体のヨーレートを計測するヨーレートジャイロと、前記移動体の車輪速を計測する車輪速センサと、前記移動体の横加速度を計測する横加速度センサと、複数の情報発信源各々から発信された信号に基づき、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と前記移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記取得された発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定部と、前記計測されたヨーレート、前記計測された車輪速及び前記計測された横加速度に基づいて前記移動体のロール角を推定するロール角推定部と、前記推定されたロール角に基づいて、前記計測されたヨーレートを水平面ヨーレートに変換する変換部と、前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正部と、を備える。

横加速度センサの誤差を補正するに当たり、先ず、横加速度センサに誤差が生じているか否かを明らかにする必要がある。そこで、当該センサ誤差補正装置では、移動体の旋回期間に、（ｉ）発信源情報に基づく速度ベクトルの方位変化量と、（ｉｉ）ヨーレートジャイロ、車輪速センサ及び横加速度センサ各々の計測結果から推定されたロール角に基づいて、ヨーレートから変換された水平面ヨーレートを積算して得られる方位変化量と、が比較される。

移動体が直進している場合、仮に、横加速度センサに誤差が生じていたとしても、水平面ヨーレートを積算して得られる方位変化量にはほとんど影響がない。他方で、移動体が旋回している場合、横加速度センサに誤差が生じていると、その影響が、水平面ヨーレートを積算して得られる方位変化量に顕著に現れる。このため、移動体の旋回期間に、速度ベクトルの方位変化量と水平面ヨーレートを積算して得られる方位変化量とが比較されるのである。

横加速度センサに誤差が生じている場合、横加速度センサの測定結果等に基づいて推定されるロール角にも誤差が生じる。この結果、速度ベクトルの方位変化量と水平面ヨーレートを積算して得られる方位変化量との間にずれが生じる。尚、ヨーレート及び車輪速各々の誤差は、既存の技術により精度良く補正可能であるので、ヨーレート及び車輪速各々の誤差の影響はないものとする。

横加速度センサに誤差が生じている場合、横加速度センサの計測結果（即ち、横加速度）をどの程度補正するかは、速度ベクトルの方位変化量と水平面ヨーレートを積算して得られる方位変化量とのずれ量に基づいて決定される。即ち、ずれ量が比較的大きければ、横加速度は比較的大きく補正され、ずれ量が比較的小さければ、横加速度は比較的小さく補正される。従って、当該センサ誤差補正装置によれば、横加速度センサの誤差を適切に補正することができる。

本発明の第１のセンサ誤差補正装置の一態様では、前記補正部は、複数回の前記旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記複数回の旋回期間における前記速度ベクトルの方位変化量とを比較する。この態様によれば、複数回の旋回期間にわたって夫々積算された水平面ヨーレートを積算して得られる方位変化量と速度ベクトルの方位変化量とが比較されるので、横加速度の補正に対する、例えばノイズの影響を抑制することができる。

本発明の誤差補正方法は、上記課題を解決するために、移動体のヨーレートを計測するヨーレートジャイロと、前記移動体の車輪速を計測する車輪速センサと、前記移動体の横加速度を計測する横加速度センサと、複数の情報発信源各々から発信された信号に基づき、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と前記移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、を備えるセンサ誤差補正装置における誤差補正方法であって、前記取得された発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定工程と、前記計測されたヨーレート、前記計測された車輪速及び前記計測された横加速度に基づいて前記移動体のロール角を推定するロール角推定工程と、前記推定されたロール角に基づいて、前記計測されたヨーレートを水平面ヨーレートに変換する変換工程と、前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正工程と、を備える。

本発明の誤差補正方法によれば、上述した本発明の第１のセンサ誤差補正装置と同様に、横加速度センサの誤差を適切に補正することができる。尚、本発明の誤差補正方法においても、上述した本発明の第１のセンサ誤差補正装置に係る各種態様と同様の各種態様を採ることができる。

本発明の第２のセンサ誤差補正装置は、上記課題を解決するために、複数の情報発信源各々から発信された信号から取得される、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定部と、前記移動体のヨーレート、前記移動体の車輪速及び前記移動体の横加速度に基づいて、前記移動体のロール角を推定するロール角推定部と、前記推定されたロール角に基づいて、前記ヨーレートを水平面ヨーレートに変換する変換部と、前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正部と、を備える。

本発明の第２のセンサ誤差補正装置によれば、上述した本発明の第１のセンサ誤差補正装置と同様に、横加速度センサの誤差を適切に補正することができる。尚、本発明の第２のセンサ誤差補正装置においても、上述した本発明の第１のセンサ誤差補正装置に係る各種態様と同様の各種態様を採ることができる。

本発明の作用及び他の利得は次に説明する実施するための形態から明らかにされる。

実施形態に係るセンサ誤差補正装置の構成を示すブロック図である。 車両の旋回時における方位角の変化及びロール角の影響の一例を示す図である。 実施形態に係る積算区間の概念を示す図である。 実施形態に係る誤差補正処理を示すフローチャートである。 第２変形例に係るセンサ誤差補正装置の構成を示すブロック図である。

本発明のセンサ誤差補正装置に係る実施形態について、図１乃至図４を参照して説明する。以下の実施例では、車両姿勢を推定する姿勢推定技術に適用されたセンサ誤差補正装置を一例として挙げる。

（装置の構成）
実施形態に係るセンサ誤差補正装置の構成について、図１を参照して説明する。図１は、実施形態に係るセンサ誤差補正装置の構成を示すブロック図である。

センサ誤差補正装置１は、本発明に係る「移動体」の一例としての車両（図示せず）に搭載されている。図１において、センサ誤差補正装置１は、演算部１０、ヨーレートジャイロ２１、ＧＰＳ受信部２２、車輪速センサ２３及び横加速度センサ２４を備えて構成されている。尚、ヨーレートジャイロ２１、ＧＰＳ受信部２２、車輪速センサ２３及び横加速度センサ２４は、センサ誤差補正装置１が独自に備えずともよく、車両に搭載された他の装置と共有されていてよい。

演算部１０は、その内部に論理的に実現される処理ブロックとして又は物理的に実現される処理回路として、ヨーレートオフセット推定・補正部１１、絶対方位角推定・補正部１２、ドップラ速度推定部１３、車輪速スケールファクタ推定・補正部１４、ロール角推定部１５、横加速度ゼロ点補正部１６及び車両姿勢・速度ベクトル推定部１７を有する。

（装置の動作）
次の、上述の如く構成されたセンサ誤差補正装置１の動作について説明する。

１．速度ベクトルの算出
ＧＰＳ受信部２２は、ＧＰＳ衛星からの電波を受信する。ＧＰＳ受信部２２は、受信された電波（即ち、ＧＰＳ信号）から、ＧＰＳ衛星の情報（以降、適宜“衛星情報”と称する）として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を発信した時刻、受信信号の強度、周波数等を取得する。ＧＰＳ受信部２２は、電波を受信した全てのＧＰＳ衛星について、衛星情報を取得する。ＧＰＳ受信部２２は、取得した衛星情報を、演算部１０のドップラ速度推定部１３に出力する。

ドップラ速度推定部１３は、複数のＧＰＳ衛星各々の衛星情報に基づいて、自車両（即ち、当該センサ誤差補正装置１が搭載されている車両）の速度ベクトルを算出する。

具体的には、ＧＰＳ衛星が電波を発信した時刻及び自車両（即ち、ＧＰＳ受信部２２）で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データが算出される。ＧＰＳ衛星から発信される信号の既知の周波数及びＧＰＳ衛星の受信信号の周波数に基づいて、ＧＰＳ衛星からの受信信号のドップラ周波数が算出される。ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を発信した時刻に基づいて、ＧＰＳ衛星の位置座標が算出される。ＧＰＳ衛星のドップラ周波数に基づいて、ＧＰＳ衛星に対する自車両の相対速度が算出される。ＧＰＳ衛星の位置座標の時系列データに基づいて、ＧＰＳ衛星の速度ベクトルが算出される。ＧＰＳ擬似距離データに基づいて自車両の位置が算出され、該算出された自車両の位置及びＧＰＳ衛星の位置座標に基づいてＧＰＳ衛星の方向が算出される。

これらの処理が、複数のＧＰＳ衛星各々について行われた後、各ＧＰＳ衛星に対する自車両の相対速度、各ＧＰＳ衛星の速度ベクトル及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星の方向の（即ち、自車両と各ＧＰＳ衛星とを結ぶ直線上での）自車両の速度が算出され、該算出された自車両の速度に基づいて、自車両の速度ベクトルが算出される。尚、自車両の速度ベクトルの算出方法には、既存の技術を適用可能であるので、その詳細についての説明は省略する。

２．ロール角推定
ヨーレートオフセット推定・補正部１１は、ヨーレートジャイロ２１からの出力信号に基づいて、自車両に生じるヨーレートを推定すると共に、ドップラ速度推定部１３により推定された速度ベクトルに基づいて、ヨーレートのオフセットを推定する。ヨーレートオフセット推定・補正部１１は、推定されたオフセットに基づいて、推定されたヨーレートを補正する。尚、ヨーレートの補正方法には、既存の技術を適用可能であるので、その詳細についての説明は省略する。

車輪速スケールファクタ推定・補正部１４は、車輪速センサ２３の出力信号に基づいて、車輪速を推定すると共に、ドップラ速度推定部１３により推定された速度ベクトルに基づいて、車輪速のスケールファクタを推定する。車輪速スケールファクタ推定・補正部１４は、推定されたスケールファクタに基づいて、推定された車輪速を補正する。尚、車輪速の補正方法には、既存の技術を適用可能であるので、その詳細についての説明は省略する。

ロール角推定部１５は、横加速度センサ２４からの出力信号、ヨーレートオフセット推定・補正部１１により求められた補正後のヨーレート、及び車輪速スケールファクタ推定・補正部１４により求められた補正後の車輪速に基づいて、自車両のロール角を推定する。ここで、ロール角を“φ”、ヨーレートを“Ｒ”、車輪速を“Ｖ”、横加速度を“Ｇｙ”、重力加速度を“ｇ”とすると、ロール角φは、“ｇｓｉｎφ＝Ｇｙ−ＲＶ”と表される。

３．横加速度のゼロ点補正
ロール角は、ヨーレート、車輪速及び横加速度に基づいて推定される。ヨーレート及び車輪速については、上述の如く、補正後のヨーレート及び補正後の車輪速がロール角の推定に用いられる。しかしながら、横加速度については、図１に示すように、横加速度センサ２４からの出力信号から直接求められる。仮に横加速度センサ２４に誤差が生じていると、推定されたロール角にも、横加速度センサ２４の誤差に起因する誤差が含まれることとなる。

横加速度ゼロ点補正部１６は、先ず、推定されたロール角に、横加速度センサ２４に起因する誤差が含まれるか否かを、次のように検出する。即ち、ロール角を考慮して水平面に換算した（又は、射影した）ヨーレート（以降、適宜“水平面ヨーレート”と称する）を積算して得られる自車両の軌跡（図２の破線“ヨーレート積算”参照）は、ロール角に誤差が含まれている場合、衛星信号に基づいて求められた自車両の速度ベクトルに基づく軌跡（図２の実線“実際の軌跡”参照）からずれる。このため、水平面ヨーレートを積算して得られる方位変化量と、速度ベクトルの方位変化量とが比較されることにより、ロール角に誤差が含まれるか否かが検出される。

ロール角に誤差が含まれている場合、横加速度ゼロ点補正部１６は、水平面ヨーレートを積算して得られる方位変化量と、速度ベクトルの方位変化量とのずれ量に基づいて、横加速度センサ２４のゼロ点を補正する。本実施形態に係る「ゼロ点の補正」の概念としては、例えば、横加速度センサ２４からの出力信号に基づく横加速度の値を増加又は減少する補正値が、横加速度ゼロ点補正部１６からロール角推定部１５に出力され、ロール角推定部１５において、補正値により横加速度の値が補正されることである。ゼロ点の補正についての詳細な説明は後述する。

４．車両姿勢の推定
ヨーレートオフセット推定・補正部１１は、補正後のヨーレート、及びロール角推定部１５により推定されたロール角に基づいて、水平面ヨーレートを求め、該求められた水平面ヨーレートを、絶対方位角推定・補正部１２に出力する。

絶対方位角推定・補正部１２は、水平面ヨーレート、及びドップラ速度推定部１３により推定された速度ベクトルに基づいて、自車両の水平面における絶対方位を推定する。絶対方位角推定・補正部１２は、ドップラ速度推定部１３により推定された速度ベクトルに基づいて、絶対方位に対するピッチ角の影響を補正する。尚、絶対方位に対するピッチ角の影響の補正方法には、既存の技術を適用可能であるので、その詳細についての説明は省略する。

車両姿勢・速度ベクトル推定部１７は、絶対方位角推定・補正部１２により求められた補正後の絶対方位、及び車輪速スケールファクタ推定・補正部１４により求められた補正後の車輪速に基づいて、自車両の車両姿勢・速度ベクトル（即ち、自車両の絶対方位及び車速）を推定する。

（横加速度のゼロ点補正）
横加速度センサ２４のゼロ点補正について、図２乃至図４を参照して説明を加える。

ここでは、自車両がカントのある道路を旋回する場合を一例として挙げる。尚、カントのない道路であっても、車両の旋回時には、大なり小なり車両がロールするので（ただし、車両の旋回方向とロール角との関係が、図２と異なる場合もある）、以下の説明は、車両がカントのない道路を旋回する場合にも適用可能である。

推定されたロール角に誤差が含まれている場合、上述の如く、水平面ヨーレートを積算して得られる自車両の軌跡（図２の破線“ヨーレート積算”参照）は、衛星信号に基づいて求められた自車両の速度ベクトルに基づく軌跡（図２の実線“実際の軌跡”参照）からずれる。

ロール角の推定に用いられるヨーレート及び車輪速については、ヨーレートオフセット推定・補正部１１及び車輪速スケールファクタ推定・補正部１４により既に補正されている。このため、ロール角の誤差は、横加速度センサ２４からの出力信号に基づく横加速度に起因するものと言える。尚、ヨーレート及び車輪速については、既存の技術でも精度良く補正可能であることが、本願発明者の研究により判明している。

横加速度センサ２４のゼロ点補正は、上述の如く、水平面ヨーレートを積算して得られる方位変化量と、速度ベクトルの方位変化量とのずれ量に基づいて行われる。水平面ヨーレートは、ヨーレートジャイロ２１、車輪速センサ２３及び横加速度センサ２４各々の出力信号から求められるので、各種車載センサを用いるＤＲ（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ：自律航法）にちなんで、水平面ヨーレートを積算して得られる方位変化量を、適宜「ＤＲ方位変化量」と称する。他方、速度ベクトルは、ＧＰＳ衛星の衛星情報から求められるので、速度ベクトルの方位変化量を、適宜「ＧＰＳ方位変化量」と称する。

車両の直進時には、車両の方位がほとんど変化しないため、原理的に、ゼロ点補正を精度良く行うことはできない。そこで、当該センサ誤差補正装置１では、自車両の旋回時に、ＤＲ方位変化量及びＧＰＳ方位変化量が求められる。

ここで、当該センサ誤差補正装置１では、自車両に生じるヨーレートの絶対値が所定閾値より大きい場合に、自車両が旋回していると判定する。従って、水平面ヨーレートが積算される期間は、例えば図３に示すように、自車両に生じるヨーレートの絶対値が所定閾値（図３の点線参照）より大きい期間となる。尚、「所定閾値」は、例えばヨーレートジャイロ２１の誤差やノイズの影響等を考慮して、“０”よりもわずかに大きい値に設定される。

横加速度センサ２４のゼロ点補正に係る誤差補正処理について、図４のフローチャートを参照して説明する。

図４において、横加速度ゼロ点補正部１６は、ヨーレートオフセット推定・補正部１１により求められた補正後のヨーレートに基づいて、ヨーレートが閾値より大きいか否かを判定する（ステップＳ１０１）。この判定において、ヨーレートが閾値より小さいと判定された場合（ステップＳ１０１：Ｎｏ）、横加速度ゼロ点補正部１６は、再びステップＳ１０１の判定を行う（即ち、横加速度ゼロ点補正部１６は、ヨーレートが閾値より大きくなるまで待機状態となる）。

他方、ステップＳ１０１の判定において、ヨーレートが閾値より大きいと判定された場合（ステップＳ１０１：Ｙｅｓ）、横加速度ゼロ点補正部１６は、ドップラ速度推定部１３により推定された速度ベクトルに基づいて、ヨーレートが閾値より大きいと判定された時点（即ち、自車両が旋回を開始した時点）の方位角を記憶する（ステップＳ１０２）。

上記ステップＳ１０２の処理と並行して、横加速度ゼロ点補正部１６は、ヨーレートオフセット推定・補正部１１により求められた補正後のヨーレート、及びロール角推定部１５により推定されたロール角に基づいて水平面ヨーレートを求め、該水平面ヨーレートの積算を開始する（ステップＳ１０３）。ここで、ヨーレートを“Ｒ”、ロール角を“φ”とすると、水平面ヨーレートは下記式により表される。

次に、横加速度ゼロ点補正部１６は、ヨーレートオフセット推定・補正部１１により求められた補正後のヨーレートに基づいて、ヨーレートが閾値より小さいか否かを判定する（ステップＳ１０４）。この判定において、ヨーレートが閾値より大きいと判定された場合（ステップＳ１０４：Ｎｏ）、横加速度ゼロ点補正部１６は、ステップＳ１０３の処理を行う。ヨーレートが閾値より小さいと判定されるまで、ステップＳ１０３の処理が繰り返し行われることにより、自車両の旋回期間中、水平面ヨーレートが積算される。

他方、ステップＳ１０４の判定において、ヨーレートが閾値より小さいと判定された場合（ステップＳ１０４：Ｙｅｓ）、横加速度ゼロ点補正部１６は、ドップラ速度推定部１３により推定された速度ベクトルに基づいて、ヨーレートが閾値より小さいと判定された時点（即ち、自車両が旋回を終了した時点）の方位角を取得する。そして、横加速度ゼロ点補正部１６は、ステップＳ１０２の処理において記憶した方位角と、該取得された方位角とを比較して、自車両の旋回期間におけるＧＰＳ方位変化量を求める。横加速度ゼロ点補正部１６は、更に、積算された水平面ヨーレートからＤＲ方位変化量を求める。（ステップＳ１０５）。

次に、横加速度ゼロ点補正部１６は、ＤＲ方位変化量がＧＰＳ方位変化量より小さいか否かを判定する（ステップＳ１０６）。ここで、ＤＲ方位変化量がＧＰＳ方位変化量より小さい場合、推定されたロール角は実際のロール角よりも小さい。この場合、水平面ヨーレートを積算して得られる自車両の軌跡は、例えば図２（ａ）の点線（ｉ）のように、実際の軌跡よりも、カーブの外側にずれる。他方、ＤＲ方位変化量がＧＰＳ方位変化量より大きい場合、推定されたロール角は実際のロール角よりも大きい。この場合、水平面ヨーレートを積算して得られる自車両の軌跡は、例えば図２（ａ）の点線（ｉｉ）のように、実際の軌跡よりも、カーブの内側にずれる。

ステップＳ１０６の判定において、ＤＲ方位変化量がＧＰＳ方位変化量よりも小さいと判定された場合（ステップＳ１０６：Ｙｅｓ）、横加速度ゼロ点補正部１６は、推定されるロール角が大きくなる方向に、ＤＲ方位変化量とＧＰＳ方位変化量との差分に応じて、横加速度センサ２４のゼロ点を補正する（即ち、推定されるロール角が大きくなる方向に横加速度の値を補正する補正値をロール角推定部１５に出力する）（ステップＳ１０７）。

他方、ステップＳ１０６の判定において、ＤＲ方位変化量がＧＰＳ方位変化量よりも大きいと判定された場合（ステップＳ１０６：Ｎｏ）、横加速度ゼロ点補正部１６は、推定されるロール角が小さくなる方向に、ＤＲ方位変化量とＧＰＳ方位変化量との差分に応じて、横加速度センサ２４のゼロ点を補正する（即ち、推定されるロール角が小さくなる方向に横加速度の値を補正する補正値をロール角推定部１５に出力する）（ステップＳ１０８）。

ステップＳ１０７又はＳ１０８の処理の後、所定時間経過後に、加速度ゼロ点補正部１６は、ステップＳ１０１の処理を行う。横加速度センサ２４のゼロ点は、例えば環境温度等に起因して、時間とともに変化するので、当該誤差補正処理が繰り返し行われることにより、ロール角の推定精度を向上又は維持することができる。

尚、ステップＳ１０１及びＳ１０４の判定において、ヨーレートと閾値とが「等しい」場合には、どちらかの場合に含めて扱えばよい。同様に、ステップＳ１０６の判定において、ＤＲ方位変化量とＧＰＳ方位変化量とが「等しい」場合には、どちらかの場合に含めて扱えばよい、或いは、ステップＳ１０７又はＳ１０８の処理が行われなくてもよい。

（技術的効果）
本実施形態では、ロール角の推定誤差（即ち、横加速度センサ２４の誤差）の影響が顕著に現れるＤＲ方位変化量と、ＧＰＳ方位変化量とが比較されることによって、横加速度センサ２４のゼロ点が補正される（即ち、横加速度センサ２４により計測された横加速度が補正される）。従って、本実施形態によれば、横加速度センサ２４の誤差を適切に補正することができる。

尚、実施形態に係る「ＧＰＳ受信部２２」及び「ドップラ速度推定部１３」は、夫々、本発明に係る「取得手段」及び「速度推定部」の一例である。実施形態に係る「横加速度ゼロ点補正部１６」は、本発明に係る「変換部」及び「補正部」の一例である。

尚、上述の実施形態では、本発明に係る「情報発信源」の一例としてＧＰＳ衛星を挙げたが、ＧＰＳ衛星に加えて又は代えて、例えば擬似衛星（スードライト）やビーコン等が用いられてもよい。

＜第１変形例＞
上述の実施形態では、１回の旋回期間におけるＤＲ方位変化量とＧＰＳ方位変化量との比較結果に基づいて、横加速度センサ２４のゼロ点が補正される。複数回の旋回期間におけるＤＲ方位変化量の積算値と、複数回の旋回期間におけるＧＰＳ方位変化量の積算値との比較結果に基づいて、横加速度センサ２４のゼロ点が補正されてよい。この際、ロール角の正負により場合分けをして、ＤＲ方位変化量及びＧＰＳ方位変化量各々が積算される。

このように構成すれば、例えば車両がカントのない道路を旋回する場合等、水平面ヨーレートに対するロール角の影響が小さい場合であっても、横加速度センサ２４の誤差を適切に補正することができる。加えて、横加速度センサのゼロ点補正に対する、例えばノイズの影響を抑制することができる。

＜第２変形例＞
実施形態に係るセンサ誤差補正装置の第２変形例について、図５を参照して説明する。図５は、第２変形例に係るセンサ誤差補正装置の構成を示すブロック図である。

図５において、センサ誤差補正装置２は、演算部１０ａ、ヨーレートジャイロ２１、ＧＰＳ受信部２２、車輪速センサ２３、横加速度センサ２４及び前後加速度センサ２５を備えて構成されている。

第２変形例では、前後加速度センサ２５からの出力信号に基づいて、自車両のピッチ角が推定される。ここで、ピッチ角を“θ”、車輪速を“Ｖ”、前後加速度を“Ｇｘ”、重力加速度を“ｇ”とすると、ピッチ角θは、“ｇｓｉｎθ＝Ｇｘ−ｄＶ（ｔ）／ｄｔ”と表される。

第２変形例に係る横加速度センサ２４のゼロ点補正は、上述の実施形態と同様であるので、その説明は割愛するが、上述した誤差補正処理は、センサ誤差補正装置２にも適用可能である。

演算部１０ａは、ピッチ角を推定するために、その内部に論理的に実現される処理ブロックとして又は物理的に実現される処理回路として、ピッチ角推定部１８及び前後加速度ゼロ点推定・補正部１９を有する。

ピッチ角推定部１８は、前後加速度センサ２５からの出力信号、ヨーレートオフセット推定・補正部１１により求められた補正後のヨーレート、及び車輪速スケールファクタ推定・補正部１４により求められた補正後の車輪速に基づいて、自車両のピッチ角を推定する。

前後加速度ゼロ点推定・補正部１９は、ピッチ角推定部１８により推定されたピッチ角と、ドップラ速度推定部１３により推定された速度ベクトルに基づくピッチ角とを比較して、前後加速度センサ２５のゼロ点を補正する。尚、前後加速度センサ２５のゼロ点の補正方法には、既存の技術を適用可能であり、また、本発明の趣旨から外れるので、その詳細についての説明は省略する。

ヨーレートオフセット推定・補正部１１は、補正後のヨーレート、ロール角推定部１５により推定されたロール角、及びピッチ角推定部１８により推定されたピッチ角に基づいて、水平面ヨーレートを求め、該求められた水平面ヨーレートを、絶対方位角推定・補正部１２に出力する。

車両姿勢・速度ベクトル推定部１７は、絶対方位角推定・補正部１２により求められた補正後の絶対方位、及び車輪速スケールファクタ推定・補正部１４により求められた補正後の車輪速に基づいて、自車両の車両姿勢・速度ベクトル（即ち、自車両の絶対方位及び車速）を推定する。車両姿勢・速度ベクトル推定部１７は、更に、ピッチ角推定部１８により推定されたピッチ角と、上記車両姿勢・速度ベクトルとから、自車両の３次元の挙動を推定する。

本発明は、上述した実施形態に限られるものではなく、特許請求の範囲及び明細書全体から読み取れる発明の要旨或いは思想に反しない範囲で適宜変更可能であり、そのような変更を伴うセンサ誤差補正装置及び方法もまた本発明の技術的範囲に含まれるものである。

１、２…センサ誤差補正装置、１０、１０ａ…演算部、１１…ヨーレートオフセット推定・補正部、１２…絶対方位角推定・補正部、１３…ドップラ速度推定部、１４…車輪速スケールファクタ推定・補正部、１５…ロール角推定部、１６…横加速度ゼロ点補正部、１７…車両姿勢・速度ベクトル推定部、２１…ヨーレートジャイロ、２２…ＧＰＳ受信部、２３…車輪速センサ、２４…横加速度センサ

Claims (4)

1. 移動体のヨーレートを計測するヨーレートジャイロと、
   前記移動体の車輪速を計測する車輪速センサと、
   前記移動体の横加速度を計測する横加速度センサと、
   複数の情報発信源各々から発信された信号に基づき、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と前記移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、
   前記取得された発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定部と、
   前記計測されたヨーレート、前記計測された車輪速及び前記計測された横加速度に基づいて前記移動体のロール角を推定するロール角推定部と、
   前記推定されたロール角に基づいて、前記計測されたヨーレートを水平面ヨーレートに変換する変換部と、
   前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正部と、
   を備えることを特徴とするセンサ誤差補正装置。
2. 前記補正部は、複数回の前記旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記複数回の旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正することを特徴とする請求項１に記載のセンサ誤差補正装置。
3. 移動体のヨーレートを計測するヨーレートジャイロと、前記移動体の車輪速を計測する車輪速センサと、前記移動体の横加速度を計測する横加速度センサと、複数の情報発信源各々から発信された信号に基づき、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と前記移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報を取得する取得手段と、を備えるセンサ誤差補正装置における誤差補正方法であって、
   前記取得された発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定工程と、
   前記計測されたヨーレート、前記計測された車輪速及び前記計測された横加速度に基づいて前記移動体のロール角を推定するロール角推定工程と、
   前記推定されたロール角に基づいて、前記計測されたヨーレートを水平面ヨーレートに変換する変換工程と、
   前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正工程と、
   を備えることを特徴とする誤差補正方法。
4. 複数の情報発信源各々から発信された信号から取得される、（ｉ）前記情報発信源各々の位置に関する情報、（ｉｉ）前記情報発信源各々と移動体との距離に関する情報、及び（ｉｉｉ）前記情報発信源各々に対する前記移動体の相対速度に関する情報を含む発信源情報に基づいて、前記移動体の速度ベクトルを求める速度推定部と、
   前記移動体のヨーレート、前記移動体の車輪速及び前記移動体の横加速度に基づいて、前記移動体のロール角を推定するロール角推定部と、
   前記推定されたロール角に基づいて、前記ヨーレートを水平面ヨーレートに変換する変換部と、
   前記移動体が旋回を開始してから旋回を終了するまでの旋回期間における前記水平面ヨーレートを積算して得られる方位変化量と、前記旋回期間における前記速度ベクトルの方位変化量とを比較し、前記計測された横加速度を補正する補正部と、
   を備えることを特徴とするセンサ誤差補正装置。