JP7349978B2 - 異常判定装置、異常判定方法、異常判定プログラム、及び、車両状態推定装置 - Google Patents

Description

本発明は、加速度及び角速度を計測する慣性計測センサであって、特に車両の速度及び姿勢角の推定に用いられる６軸慣性計測センサの異常の有無を判定する異常判定装置、異常判定方法、及び、異常判定プログラムと、その６軸慣性計測センサによる計測値に基づいて車両状態を推定する車両状態推定装置に関する。

車両の挙動を検出する車両の挙動検出装置であって、車両の挙動を検出する挙動検出部と、該挙動検出部の異常を検出するための異常検出部とを有するものが公知である（例えば、特許文献１）。

挙動検出部は６軸慣性センサを備えている。６軸慣性センサは、車両の前後、左右、及び上下方向の加速度と、ロールレート、ピッチレート、及び、ヨーレートとを検出する。

異常検出部は３軸慣性センサと、車輪速センサと、圧力センサとを備えている。３軸慣性センサは、車両の上下及び左右方向の加速度と、ロールレートとを検出する。車輪速センサは少なくとも２つの車輪の車輪速を測定し、異常検出部は測定された車輪速に基づいて前後方向の加速度を検出する。圧力センサは前後輪のブレーキ液圧を測定し、異常検出部は測定されたブレーキ液圧に基づいてヨーレートを検出する。

異常検出部は、挙動検出部において６軸慣性センサによって得られた車両の上下方向の加速度、左右方向の加速度、及び、ロールレートを、それぞれ、３軸慣性センサによって得られた車両の上下方向の加速度、左右方向の加速度、及び、ロールレートと比較する。また、異常検出部は、挙動検出部において６軸慣性センサによって得られた車両の前後方向の加速度及びヨーレートを、車輪速に基づいて検出された前後方向の加速度及びブレーキ液圧に基づいて検出したヨーレートと比較する。異常検出部は、互いに対応する検出値に所定のずれがあった場合には、６軸慣性センサに異常があると判断する。

特許第６６６３００６号公報

車両を自律走行させる自動運転車では、車両の自己位置を正確に特定するため、特に車両前後方向の加速度を検出するセンサの異常を適切に判断する必要がある。特許文献１の異常検出部は、車輪速に基づいて前後方向の加速度を検出するため、車輪がスリップしているときには、車輪速に基づいて検出される前後方向の加速度が真値と乖離し、異常検出部が６軸慣性センサに異常があると判断する虞がある。

本発明は、以上の背景を鑑み、車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートを取得する６軸慣性計測センサの異常を適切に判定することのできる異常判定装置、異常判定方法、異常判定プログラム、及び、車両状態推定装置を提供することを課題とする。

上記課題を解決するために本発明のある態様は、車両（Ｓ）に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサ（５）の異常の有無を判定する異常判定装置であって、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサと、前記６軸慣性計測センサの異常の有無を判定する異常判定部とを備え、前記異常判定部は、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する。

この態様によれば、６軸慣性センサによって取得された車両前後方向の加速度と、３軸慣性センサによって取得された車両前後方向の加速度とを比較することによって、６軸慣性計測センサの異常の有無が判定される。これにより、車輪がスリップしている場合であっても、前後方向の加速度の検出における６軸慣性センサの異常を適切に判定することができる。

上記課題を解決するために本発明のある態様は、車両（Ｓ）に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサ（５）の異常の有無を判定する異常検出方法であって、前記車両に搭載され、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサから、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを取得し、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する。

この態様によれば、６軸慣性センサによって取得された車両前後方向の加速度と、３軸慣性センサによって取得された車両前後方向の加速度とを比較することによって、６軸慣性計測センサの異常の有無が判定される。これにより、車輪速を用いた場合に比べて、車輪がスリップしている場合であっても、前後方向の加速度の検出における６軸慣性センサの異常を適切に判定することができる。

上記課題を解決するために本発明のある態様は、車両（Ｓ）に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサ（５）の異常の有無を判定する異常判定装置であって、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサと、前記６軸慣性計測センサの異常の有無を判定する異常判定部とを備え、前記異常判定部は、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する。

この態様によれば、６軸慣性センサによって取得された車両前後方向の加速度と、３軸慣性センサによって取得された車両前後方向の加速度とを比較することによって、６軸慣性計測センサの異常の有無が判定される。これにより、車輪速を用いた場合に比べて、車輪がスリップしている場合であっても、前後方向の加速度の検出における６軸慣性センサの異常を適切に判定することができる。

上記課題を解決するために本発明のある態様は、車両（Ｓ）に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサ（５）の計測値に基づいて車両状態を推定する車両状態推定装置（４６）であって、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサ（７）と、前記６軸慣性計測センサの異常の有無を判定する異常判定部（３１）とを備え、前記異常判定部は、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する。

この態様によれば、６軸慣性センサによって取得された車両前後方向の加速度と、３軸慣性センサによって取得された車両前後方向の加速度とを比較することによって、６軸慣性計測センサの異常の有無が判定される。これにより、車輪速を用いた場合に比べて、車輪がスリップしている場合であっても、前後方向の加速度の検出における６軸慣性センサの異常を適切に判定することができる。

上記の態様において、好ましくは、前記車両に設けられる車輪の車輪速を測定する車輪速センサ（９）と、前記車輪速センサ、及び、前記６軸慣性計測センサによって計測される前記計測値に基づいて前記車両の姿勢角を算出する車両状態推定部（３１）とを有し、前記異常判定部は、前記車両状態推定部によって算出される前記姿勢角と、前記車輪が滑走することなく走行したときに、前記車輪速を用いて算出される重力の方向に基づく定常姿勢角との差が所定の閾値以上となったときに、前記６軸慣性計測センサ、又は、前記車輪速センサに異常があると判定する。

この態様によれば、積分演算をすることによって取得される姿勢角と、定常姿勢角とが算出されるため、両者の差分を計算することによって、６軸慣性計測センサ及び車輪速センサの異常を検知することができる。

上記の態様において、好ましくは、前記車両状態推定部によって算出されるロール角及び前記定常姿勢角のロール角の差が所定の閾値以上であるか、又は、前記車両状態推定部によって算出されるピッチ角及び前記定常姿勢角のピッチ角の差が所定の閾値以上であるときに、前記６軸慣性計測センサ、又は、前記車輪速センサに異常があると判定する。

この態様によれば、６軸慣性計測センサにおけるロール方向、及び、ピッチ方向の異常や、車輪速センサの異常を検出することができる。

以上の構成によれば、車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートを取得する６軸慣性計測センサの異常を適切に判定することのできる異常判定装置、異常判定方法、異常判定プログラム、及び、車両状態推定装置を提供することができる。

実施形態に係る車両状態推定装置 実施形態に係る車両状態推定装置の機能構成図 異常判定処理のフローチャート （Ａ）車両がスリップすることなく走行（グリップ走行）している場合と、（Ｂ）車両がスリップして走行（スリップ走行）している場合とにおける、横速度の真値、定常車速、過渡車速、及び、車速の推定値（推定車速）を示すグラフ

以下、図面を参照して、本発明に係る異常判定装置、異常判定方法、異常判定プログラム、及び、車両状態推定装置を自律的に走行可能な車両Ｓに適用した実施形態について説明する。

実施形態に係る異常判定装置、異常判定方法、異常判定プログラム、及び、車両状態推定装置は自律的に走行可能な車両Ｓに設けられる。

以下、説明の便宜上、図１に示すように、路面に固定され、水平方向、且つ、互いに直交する二つの方向にそれぞれ延びるＸ軸及びＹ軸と、鉛直上向きのＺ軸とを持つ座標系を固定座標系（慣性座標系ともいう）と記載する。

図２に示すように、車両Ｓは車両Ｓの挙動を制御して自律的に走行させるための車両制御システム３を備えている。車両制御システム３は、２つの慣性計測センサ５、７と、車輪速センサ９と、外界認識装置（不図示）と、操舵・加減速装置１３と、ＨＭＩ１５と、制御装置１７とを含む。

慣性計測センサ５、７はそれぞれ車体に固定されている。慣性計測センサ５、７の一方（以下、主慣性計測センサ５）はいわゆる６軸の慣性計測センサであって、３軸の加速度Ｇ_ｓｅｎｓと、これら３軸の角速度ωとを検出する。

主慣性計測センサ５は加速度Ｇ_ｓｅｎｓを計測する。主慣性計測センサ５は加速度Ｇ_ｓｅｎｓをｘ軸、ｙ軸、及びｚ軸からなる主慣性計測センサ５に固定された（すなわち、車体に固定された）座標系（以下、加速度座標系）を基準として出力する。但し、ｘ軸の正の方向が車体前方、ｙ軸の正の方向が車体左方、ｚ軸の正の方向が車体上方となるように、主慣性計測センサ５は車体に固定されている。すなわち、ｘ軸は車体前後方向に、ｙ軸は車体左右方向に、ｚ軸は車体上下方向となるように設定されている。以下、説明の便宜上、加速度座標系の原点を主慣性計測センサ５の設けられた位置とする。

本実施形態では、主慣性計測センサ５は、車体に保持され、それぞれが検出電極を有する３つの筐体と、それぞれ筐体に収容され、筐体にｘ軸、ｙ軸、及びｚ軸方向に２つずつのばねを介して保持された錘とを有している。主慣性計測センサ５は、錘及び検出電極間の静電容量の変化を検出することによって、主慣性計測センサ５はその錘に加わる慣性力を計測し、その慣性力をおもりの質量で割り、向きを反転させることで、加速度を算出して出力する。但し、主慣性計測センサ５はこの態様には限定されず、例えば、主慣性計測センサ５は、慣性力によって歪みが生じ、その歪みに応じた電圧を発生する圧電素子を用いたものであってもよい。このとき、主慣性計測センサ５は圧電素子に発生する電圧を検出することにより慣性力を取得し、その慣性力に基づいて、加速度を算出して出力するものであってもよい。

主慣性計測センサ５によって出力される加速度Ｇ_ｓｅｎｓのｘ成分、ｙ成分、及び、ｚ成分をそれぞれＧ_{ｓｅｎｓｘ、}Ｇ_{ｓｅｎｓｙ、}Ｇ_{ｓｅｎｓｚ}とすると、固定座標系において車両Ｓに加わる加速度Ｇ（＝ΣＦ／ｍ。ΣＦは重力を含む車両Ｓに加わる力の総和、ｍは車両Ｓの質量を示す）は、以下の式（１）を満たす。

但し、ｉ，ｊ，及びｋはそれぞれ、加速度座標系におけるｘ軸、ｙ軸、及びｚ軸方向の単位ベクトルを示し、Ｇ_ｖは重力加速度を示している。式（１）は、主慣性計測センサ５によって計測される加速度Ｇ_ｓｅｎｓが、車両Ｓに加わる加速度Ｇに重力加速度Ｇｖによる影響を考慮して補正したもの（Ｇ－Ｇ_Ｖ）であることを示している。

重力加速度Ｇｖによる影響を考慮した補正は、以下のように説明される。例えば、車両Ｓに加わる力の総和が０であり、車両Ｓが停止しているときには、式（１）に基づけば、主慣性計測センサ５が出力する加速度Ｇ_ｓｅｎｓは、重力加速度Ｇｖの－１倍に等しい。これは、主慣性計測センサ５が慣性力に基づいて加速度を算出しているためであって、錘に加わる重力を慣性力と区別することができないためである。換言すれば、主慣性計測センサ５が錘に加わる鉛直下向きの重力を筐体（車体）が鉛直上向きに加速していることによる慣性力であると見做して、加速度を出力するためである。重力加速度Ｇｖによる影響を考慮する補正は、主慣性計測センサ５が慣性力に基づいて加速度を算出していることによる影響を考慮したものである。

主慣性計測センサ５によって計測される加速度Ｇ_ｓｅｎｓには、車体の旋回による影響が含まれる。主慣性計測センサ５は角速度ωを、加速度Ｇ_ｓｅｎｓと同様に、加速度座標系を基準として出力する。角速度ωのｘ成分はロールレート、角速度ωのｙ成分はピッチレート、角速度ωのｚ成分はヨーレートに相当する。主慣性計測センサ５は、取得した角速度ωと、加速度Ｇ_ｓｅｎｓとをそれぞれ、制御装置１７に出力する。

慣性計測センサ５、７の他方（以下、副慣性計測センサ７）はいわゆる３軸の慣性計測センサであって、主慣性計測センサ５と同様の原理に基づいて、固定座標系におけるｘ軸方向の加速度Ｇ'_{ｓｅｎｓｘ}と、ｙ軸方向の加速度Ｇ'_{ｓｅｎｓｙ}と、ｚ軸まわりの角速度ω'_ｚとを取得し、それぞれ制御装置１７に出力する。

副慣性計測センサ７は、車両状態の推定以外の目的、例えば横滑り防止装置（ＶＳＡ）等の装置に設けられるものであってもよい。

車輪速センサ９は、車両Ｓに設けられた少なくとも一つの車輪の車輪速を取得する。車輪速センサ９は駆動輪の車輪速を取得してもよい。車輪速センサ９は取得した車輪速を制御装置１７に出力する。

外界認識装置は、車両Ｓの周辺に係る情報を取得する装置であって、例えば、車載カメラや、レーダ、ライダ等を含む。

操舵・加減速装置１３は、車両Ｓの操舵、及び、加減速を行う。操舵・加減速装置１３は、車両Ｓを操舵するステアリング装置、内燃機関やモータ等の車両Ｓに駆動力を付与する駆動装置、及び、車両Ｓを減速させるブレーキ装置を含む。

ＨＭＩ１５は乗員からの入力を受け付けるとともに、乗員に通知を行う装置であって、本実施形態では、タッチパネルによって構成されている。ＨＭＩ１５は乗員から自律走行の開始を指示する入力を受け付けるとともに、車両状態の推定精度が低下したときに、自律走行ができない旨の通知を行う。

制御装置１７は、プロセッサ、ＲＡＭ等を備えたいわゆるマイクロコンピュータであって、２つの慣性計測センサ５、７、車輪速センサ９、外界認識装置、操舵・加減速装置１３、及び、ＨＭＩ１５に接続されている。

制御装置１７は、車両状態を推定する車両状態推定器３１（車両状態推定部）と、主慣性計測センサ５の異常を判定するための異常判定器３３（異常判定部）と、ＨＭＩ１５への入力に基づいて車両Ｓの自律走行のレベルを管理する状態管理部３５と、自車位置を推定する自車位置推定部３７と、車両Ｓを操舵・加減速し、自律走行させる走行制御部３９とを含む。車両状態推定器３１が推定する車両状態には、車速、車速の時間変化率（以下、運動加速度）、及び、姿勢角が含まれる。

ＨＭＩ１５から自律走行の開始を指示する入力があると、異常判定器３３が主慣性計測センサ５の異常の有無を判定する。異常判定器３３が主慣性計測センサ５に異常がないと判定すると、状態管理部３５は自車位置推定部３７に指示を行い、外界認識装置からの信号と、主慣性計測センサ５（６軸慣性計測センサ）の計測値に基づき車両状態推定器３１が推定した車両状態とに基づいて、自車位置推定部３７に自車位置を推定させる。走行制御部３９は、自車位置推定部３７によって推定された自車位置を用いて、操舵・加減速装置１３を制御し、車両Ｓを自律走行させる。

車両制御システム３は、更に、車輪がスリップしている状態（以下、滑走状態）にあるか否かを判定するスリップ判定装置４１（判定部）を備えている。スリップ判定装置４１は、アンチロック・ブレーキシステム４３（ＡＢＳ）、及び、トラクションコントロールシステム４５（ＴＣＳ）を含む。

アンチロック・ブレーキシステム４３（ＡＢＳ）は、急制動によって車輪の回転が止まり（すなわち、車輪がロックし）、車輪がスリップすることを防止する。本実施形態では、アンチロック・ブレーキシステム４３は車輪速及び車速に基づいて、車輪がロックしてスリップしているか否かを判定する。車輪がスリップしていると判定したときには、アンチロック・ブレーキシステム４３は、車輪のスリップが発生したことを示すフラグ信号を制御装置１７に出力し、制動力を緩め、車輪速を回復させた後、ブレーキ圧を保持、上昇を繰り返して、車両Ｓを減速させる。

トラクションコントロールシステム４５（ＴＣＳ）は、発進や急加速における車輪の空転を防止する。本実施形態では、トラクションコントロールシステム４５は、車輪速及び車速を比較することにより、車輪が空転しスリップしていることを判定する。車輪が空転しスリップしていると判定したときには、トラクションコントロールシステム４５は、車輪がスリップしたことを示すフラグ信号を制御装置１７に出力し、駆動輪の回転速度の制御を行う。

車両状態推定装置１は、制御装置１７（より詳細には、車両状態推定器３１）とスリップ判定装置４１とを含み、車速及び姿勢角を含む車両状態を推定する。

以下、車両状態推定器３１について説明する。車両状態推定器３１は、主慣性計測センサ５、車輪速センサ９、及び、スリップ判定装置４１からの信号に基づいて、車速、運動加速度、及び、姿勢角を含む車両状態の推定値を算出し、算出した推定値によって車両状態を更新する。

ここでいう運動加速度ａは、車速ｕ（＝ｕ_ｘｉ＋ｕ_ｙｊ＋ｕ_ｚｋ）とするとき、以下の式（２）で表される。

運動加速度ａは車速の加速度座標系に対する時間変化であって、車速の相対導関数ともよばれる。一方、車速の固定座標系に対する時間変化（ｄｕ／ｄｔ）は固定座標系の加速度Ｇに相当し、車速の絶対導関数ともよばれる。加速度Gと、運動加速度aとは以下の関係式を満たす。

車両状態推定器３１は姿勢角を、オイラー角（又は、タイト・ブライアン角ともいう）によって出力する。すなわち、ロール角φ、ピッチ角θ、ヨー角ψとすると、姿勢角Ωは以下の式（４）によって、表される。

以下、車速の推定値を推定車速ｖ、運動加速度の推定値を推定運動加速度α、姿勢角の推定値を推定姿勢角Θとし、更新前の推定車速をｖ_０、更新前の推定姿勢角をΘ_０として、説明を行う。

車両状態推定器３１（車両状態推定部）は、車両状態を推定するための機能部として、作動判定部５１と、運動加速度計算部５３と、運動加速度補正計算部５５と、運動加速度積分器５７と、姿勢角変化率計算部５９（角速度座標変換部ともいう）と、重力項推定部６１と、角度分解部６３と、姿勢角変化率補正計算部６５と、姿勢角変化率積分器６７とを有する。これらの機能部は、プロセッサによって実行される車両状態推定プログラムによって構成されている。

作動判定部５１は、アンチロック・ブレーキシステム４３が算出したフラグ信号、及び、トラクションコントロールシステム４５からのフラグ信号に基づいて、速度の推定に係るオブザーバゲイン（以下、加速度ゲインＫ）と、姿勢の推定に係るオブザーバゲイン（以下、姿勢角ゲインＫ_ＡＮＧ）とを設定する。作動判定部５１は、決定した加速度ゲインＫ及び姿勢角ゲインＫ_ＡＮＧを、運動加速度補正計算部５５に出力する。

但し、作動判定部５１は、アンチロック・ブレーキシステム４３からのアンチロック・ブレーキシステム４３の作動を示すフラグ信号、及び、トラクションコントロールシステム４５からのトラクションコントロールシステム４５の作動を示すフラグ信号のいずれか一方が入力されているときには、車輪がスリップしていると判定して、それ以外の場合に比べて、加速度ゲインＫ及び姿勢角ゲインＫ_ＡＮＧをそれぞれ小さく設定する。

運動加速度計算部５３には、主慣性計測センサ５によって取得される角速度ω及び加速度Ｇ_ｓｅｎｓと、更新前の推定車速ｖ_０と、更新前の推定姿勢角Θ_０（＝［φ_０，θ_０，ψ_０］^Ｔ）とが入力される。運動加速度計算部５３は、式（５）に基づいて、加速度座標系における重力加速度Ｇ_ｖ（θ_０，φ_０）を算出する。

但し、式（５）において、ｇは、重力加速度の大きさ（～９．８ｍ／ｓ^２）を示す。

その後、運動加速度計算部５３は、式（５）によって算出された重力加速度Ｇ_ｖ（θ_０，φ_０）と、更新前の推定車速ｖ_０と、慣性計測センサによって取得される角速度ω及び加速度Ｇ_ｓｅｎｓとをそれぞれ、式（６）に代入することによって、過渡運動加速度α_ｗを計算する。その後、運動加速度計算部５３は、更新前の推定車速ｖ_０と過渡運動加速度α_ｗとを、運動加速度補正計算部５５に出力する。

式（１）及び式（３）から理解できるように、式（６）は式（１）と式（３）とにより導出される。

また、運動加速度計算部５３は、主慣性計測センサ５によって取得される角速度ωと、更新前の推定車速ｖ_０とを用いて、式（７）により、運搬加速度α_ｃ（向心加速度ともいう）を算出し、重力項推定部６１に出力する。

運動加速度補正計算部５５には車輪速センサ９から車輪速が、運動加速度計算部５３から更新前の推定車速ｖ_０と運動加速度α_wが、作動判定部５１から加速度ゲインＫがそれぞれ入力される。

運動加速度補正計算部５５は車輪速センサ９から入力された車輪速に基づいて、車速の推定値である定常車速ｖ_ｍｅａｓを取得する。定常車速ｖ_ｍｅａｓは、車輪がスリップしていない状況下において、車速と概ね合致する。しかし、車輪がスリップしている状況下においては、定常車速ｖ_ｍｅａｓは車速に合致しない。

運動加速度補正計算部５５は、更に、運動加速度α_w、更新前の推定車速ｖ_０、定常車速ｖ_ｍｅａｓ、及び、加速度ゲインＫを、式（８）に代入することによって、推定運動加速度αを算出して更新する。

式（８）の右辺は、過渡運動加速度α_ｗをフィードバック項（－Ｋ（ｖ_０－ｖ_ｍｅａｓ））によって補正したものに相当する。式（２）を参照すると理解できるように、式（８）は速度に関する微分方程式であって、式（８）は速度に関する、いわゆるオブザーバ方程式に相当する（以下、式（８）を速度オブザーバ方程式という）。換言すれば、運動加速度補正計算部５５は、過渡運動加速度α_ｗをフィードバック項によって補正した速度に関する微分方程式である速度オブザーバ方程式（式（８））を用いて、推定運動加速度αを算出する。

運動加速度補正計算部５５は算出した推定運動加速度αを、重力項推定部６１と、自車位置推定部３７とに出力する。

運動加速度積分器５７には、運動加速度補正計算部５５から推定運動加速度αが入力される。運動加速度積分器５７は、推定運動加速度αを時間積分し、その時間積分した値（∫αｄｔ）によって、推定車速ｖを更新する。

但し、運動加速度積分器５７は、推定運動加速度αを時間積分するときに、単位ベクトルｉ，ｊ，ｋの時間変化を無視し、推定運動加速度αのｘ成分、ｙ成分、及びｚ成分をそれぞれ積分することによって、推定車速ｖを取得してもよい。より具体的には、例えば、運動加速度積分器５７は、更新前の推定車速ｖ_０（算出時のｘ、ｙ、ｚ成分がそれぞれｖ_０ｘ、ｖ_０ｙ、ｖ_０ｚとする）と、推定運動加速度α（算出時のｘ、ｙ、ｚ方向の成分がそれぞれα_ｘ、α_ｙ、α_ｚとする）とを用いて、いわゆるオイラー法を用いて算出してもよい。より具体的には、運動加速度積分器５７は、（ｖ_０ｙ＋α_ｘΔｔ）ｉ＋（ｖ_０ｙ＋α_ｙΔｔ）ｊ＋（ｖ_０ｚ＋α_ｚΔｔ）ｋ（但し、ｉ，ｊ，ｋはそれぞれ、推定車速算出時のｘ、ｙ、ｚ方向の単位ベクトル。Δｔは推定計算の制御周期であって、より詳細には、更新前の推定車速ｖ_０を算出してからの経過時間）を算出し、算出した値を、推定車速ｖとして出力してもよい。

運動加速度積分器５７は、更新した推定車速ｖを運動加速度計算部５３と、走行制御部３９とに出力する。

姿勢角変化率計算部５９には、主慣性計測センサ５によって取得される角速度ωと、更新前の姿勢角の推定値（推定姿勢角Θ_０）とが入力される。姿勢角変化率計算部５９は、角速度ωと更新前の推定姿勢角Θ_０とを式（９）に代入し、姿勢角の時間変化率である姿勢角変化率ΔΘ_Ｔ（θ_０，φ_０，ω）を算出して、姿勢角変化率補正演算部に出力する。

式（９）は、姿勢角変化率ΔΘ_Ｔ（θ_０，φ_０，ω）が角速度ωを座標変換することによって予測される姿勢角変化率に相当することを示している。

重力項推定部６１には、慣性計測センサによって取得される加速度Ｇ_ｓｅｎｓと、運動加速度補正計算部５５によって更新された推定運動加速度αと、運動加速度計算部５３から運搬加速度α_ｃとが入力される。重力項推定部６１は、式（１０）に、加速度Ｇ_ｓｅｎｓと、更新後の推定運動加速度αと、運搬加速度α_ｃとを代入することによって、重力項Ｇ_ｓｔｄを算出し、角度分解部６３に出力する。

式（１０）は、重力項Ｇ_ｓｔｄが、慣性計測センサによって取得される加速度Ｇ_ｓｅｎｓから、推定運動加速度α_０、及び、運搬加速度α_ｃ（＝ω×ｖ_０）を差し引き、重力加速度が慣性力から算出されることを考慮して反転したものに相当していることを示している。

角度分解部６３には、重力項推定部６１から重力項Ｇ_ｓｔｄが入力される。角度分解部６３は、重力項Ｇ_ｓｔｄの成分ｇ_ｅｓｔＸ、ｇ_ｅｓｔＹ、及び、ｇ_ｅｓｔＺを式（１１）に基づいて取得する。

角度分解部６３は、次に、式（１２）に重力項Ｇ_ｓｔｄを代入することによって、定常姿勢角Θ_ｇｒａｖを取得し、姿勢角変化率補正計算部６５に出力する（但し、ｇは～９．８ｍ／ｓ^２）。

但し、式（１２）では、重力項Ｇ_ｓｔｄからヨー角ψを算出せず、ロール角φ、及び、ピッチ角θのみを算出した。式（１１）及び（１２）は、式（５）を参照すると理解できるように、定常姿勢角Θ_ｇｒａｖが推定される重力加速度（すなわち、重力項Ｇ_ｓｔｄ）が鉛直下向きであることを用いて推定される姿勢角であることを示している。

姿勢角変化率補正計算部６５には、角度分解部６３から定常姿勢角Θ_ｇｒａｖが入力され、作動判定部５１から姿勢の推定に係る姿勢角ゲインＫ_ＡＮＧが入力される。姿勢角変化率補正計算部６５は角度分解部６３から入力された定常姿勢角Θ_ｇｒａｖと、姿勢の推定に係る姿勢角ゲインＫ_ＡＮＧと、更新前の推定姿勢角Θ_０とに基づいて、式（１３）を用いて、推定姿勢角の時間変化率ΔΘ（すなわち、推定姿勢角の時間微分）を取得し、姿勢角変化率積分器６７に出力する。

式（１３）の右辺は、姿勢角変化率ΔΘ_Ｔ（θ_０，φ_０，ω）をフィードバック項（－Ｋ_ＡＮＧ（Θ_０－Θ_ｇｒａｖ））によって補正したものに相当する。式（１３）は、姿勢角に関する微分方程式であって、式（１３）は姿勢角に関する、いわゆるオブザーバ方程式に相当する（以下、式（１３）を姿勢角オブザーバ方程式という）。換言すれば、姿勢角変化率補正計算部６５は姿勢角変化率ΔΘ_Ｔ（θ_０，φ_０，ω）をフィードバック項によって補正した姿勢角オブザーバ方程式（式（１３））を用いて、推定姿勢角時間変化率を算出する。

姿勢角変化率補正計算部６５は更に、更新前の推定姿勢角Θ_０と、重力項Ｇ_ｓｔｄに基づいて推定された定常姿勢角Θ_ｇｒａｖとを異常判定器３３に出力する。

姿勢角変化率積分器６７には、推定姿勢角の時間変化率ΔΘが入力される。姿勢角変化率積分器６７は、推定姿勢角の時間変化率ΔΘを時間積分し、その時間積分した値（すなわち、∫ΔΘｄｔ）によって、推定姿勢角Θを更新する。

より具体的には、姿勢角変化率積分器６７は、時間積分した値を、更新前の推定姿勢角Θ_０に更新するまでに要した時間Δｔと推定姿勢角の時間変化率ΔΘとの積を加えることによって算出し、その積分した値（Θ_０＋ΔΘΔｔ）によって、推定姿勢角Θを更新してもよい。

姿勢角変化率積分器６７は、更新した推定姿勢角Θを、運動加速度計算部５３及び走行制御部３９に出力する。

制御装置１７（より詳細には、車両状態推定器３１、及び、異常判定器３３）と、副慣性計測センサ７（３軸慣性計測センサ）と、車輪速センサ９と、スリップ判定装置４１と、によって、主慣性計測センサ５（６軸慣性計測センサ）の計測値に基づいて車速及び姿勢角を含む車両状態を推定する車両状態推定装置４６が構成される。

制御装置（より詳細には、異常判定器３３）と、副慣性計測センサ７（３軸慣性計測センサ）とによって、主慣性計測センサ５（６軸慣性計測センサ）の異常を判定する異常判定装置６８が構成される。

次に、異常判定器３３について説明する。異常判定器３３は、異常判定プログラムを実行することによって、図３に示す異常判定処理を車両Ｓが自律走行している間、常時行い、主慣性計測センサ５の異常の有無を判定する異常判定方法を実施する。以下、図３に沿って、異常判定器３３が行う異常判定処理について説明する。

異常判定器３３は、異常判定処理の最初のステップＳＴ１において、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｘ成分Ｇ_{ｓｅｎｓｘ}と、副慣性計測センサ７によって取得されたｘ方向の加速度Ｇ'_{ｓｅｎｓｘ}とが合致しているか否かを判定する。異常判定器３３は、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｘ成分Ｇ_{ｓｅｎｓｘ}と、副慣性計測センサ７によって取得されたｘ方向の加速度Ｇ'_{ｓｅｎｓｘ}とを相関比較し、合致しているか否かを判定してもよい。また、異常判定器３３は、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｘ成分Ｇ_{ｓｅｎｓｘ}と、副慣性計測センサ７によって取得されたｘ方向の加速度Ｇ'_{ｓｅｎｓｘ}との差が、所定の閾値以下であるときに、両者が合致していると判定してもよい。異常判定器３３は、両者が合致しているときにはステップＳＴ２を、合致していないときには、ステップＳＴ３をそれぞれ実行する。

異常判定器３３は、ステップＳＴ２において、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｙ成分Ｇ_{ｓｅｎｓｙ}と、副慣性計測センサ７によって取得されたｙ方向の加速度Ｇ'_{ｓｅｎｓｙ}とが合致しているか否かを判定する。異常判定器３３は、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｙ成分Ｇ_{ｓｅｎｓｘ}と、副慣性計測センサ７によって取得されたｙ方向の加速度Ｇ'_{ｓｅｎｓｙ}とを相関比較し、合致しているか否かを判定してもよい。異常判定器３３は、主慣性計測センサ５によって取得されたＧ_ｓｅｎｓのｙ成分Ｇ_{ｓｅｎｓｘ}と、副慣性計測センサ７によって取得されたｙ方向の加速度Ｇ'_{ｓｅｎｓｙ}との差が、所定の閾値以下であるときに、両者が合致していると判定してもよい。異常判定器３３は、両者が合致しているときにはステップＳＴ４を、合致していないときにはステップＳＴ３をそれぞれ実行する。

異常判定器３３は、ステップＳＴ３において、主慣性計測センサ５の異常の有無を示すフラグ（センサ異常フラグ）を異常があることを示す値（例えば、１）に設定する。設定が終了すると、異常判定器３３は異常判定処理を終了する。

異常判定器３３は、ステップＳＴ４において、主慣性計測センサ５によって取得された角速度ωのｚ成分ω_ｚと、副慣性計測センサ７によって取得されたｚ方向の角速度ω'_ｚとが合致しているか否かを判定する。異常判定器３３はωｚとω'_ｚとを相関比較し、合致しているか否かを判定してもよい。また、異常判定器３３はωｚとω'_ｚとの差が所定の閾値以下であるときに両者が合致していると判定してもよい。異常判定器３３は合致していると判定したときにはステップＳＴ５を、合致していないと判定したときにはステップＳＴ３をそれぞれ実行する。

異常判定器３３は、ステップＳＴ５において、車両状態推定器３１（より詳細には、姿勢角変化率積分器６７）から出力された更新前の推定姿勢角Θ_０のｘ、ｙ成分と、定常姿勢角Θ_ｇｒａｖのｘ、ｙ成分が合致しているか否かを判定する。異常判定器３３は、対応する各成分の差分を推定誤差とし、その推定誤差がそれぞれ対応する所定の閾値以下であるときに、両者が合致していると判定するとよい。両者が合致していると判定したときには、異常判定器３３はステップＳＴ６を、ｘ、ｙ方向の要素のうちいずれかが対応する閾値よりも大きいときにはステップＳＴ３を、それぞれ実行する。

異常判定器３３は、ステップＳＴ６において、センサ異常フラグを正常である（異常がない）ことを示す値（例えば、０）に設定し、センサ異常フラグを状態管理部３５に出力する。出力が完了すると、異常判定器３３は異常判定処理を終了する。

状態管理部３５は、異常判定器３３から入力されたセンサ異常フラグが、センサが正常であることを示す値であるときには、車両Ｓを自律走行させる。一方、センサ異常フラグが、センサが異常であることを示す値であるときには、車両Ｓの自律走行を禁止する。より具体的には、センサ異常フラグが、センサが異常であることを示す値である場合には、状態管理部３５は、ＨＭＩ１５に車両Ｓの自律走行を希望する旨の入力の受付を行わない。また、車両Ｓの自律走行時に、異常であることを示す値であるセンサ異常フラグが入力された場合には、状態管理部３５はＨＭＩ１５にセンサに異常があることを示す表示を行う。更に、状態管理部３５は、速やかに車両Ｓの自律走行を停止させ、車両Ｓを乗員の運転操作に基づいて走行する手動運転状態へ移行させるとよい。

次に、このように構成した車両状態推定装置４６の動作について説明する。

アンチロック・ブレーキシステム４３から作動を示すフラグ信号、及び、トラクションコントロールシステム４５から作動を示すフラグ信号のいずれもが出力されていないとき（すなわち、車輪がスリップしていないとき）には、作動判定部５１は、加速度ゲインＫ及び姿勢角ゲインＫ_ＡＮＧをいずれかのフラグ信号が出力されているときに比べて十分大きな値に設定する。

車両状態推定器３１は、式（８）に基づいて、推定車速を算出する。式（８）の左辺は運動加速度、すなわち、車速の微分に相当するため、式（８）は車速に関する微分方程式（式（６）。以下、加速度方程式）に、フィードバック項（－Ｋ（ｖ_０－ｖ_ｍｅａｓ））を加えたオブザーバ方程式に相当する。作動判定部５１が加速度ゲインＫを十分大きい値に設定すると、車両状態推定器３１は、推定車速をフィードバック項（－Ｋ（ｖ_０－ｖ_ｍｅａｓ））が零に近づくように値を決定する。すなわち、車両状態推定器３１は推定車速を、定常車速に近い値に決定する。

車両状態推定器３１は、式（１３）に基づいて、推定姿勢角を算出する。式（１３）の左辺は、姿勢角の微分に相当するため、式（１３）は、姿勢角に関する微分方程式（ΔΘ＝ΔΘ_Ｔ。以下、姿勢角方程式）に、フィードバック項（－Ｋ_ＡＮＧ（Θ_０－Θ_ｇｒａｖ））を加えたオブザーバ方程式に相当する。作動判定部５１が姿勢角ゲインＫ_ＡＮＧを十分大きな値に設定すると、車両状態推定器３１は、推定姿勢角をフィードバック項（－Ｋ_ＡＮＧ（Θ_０－Θ_ｇｒａｖ））が零に近づくように値を決定する。すなわち、車両状態推定器３１は推定姿勢角を、定常姿勢角Θ_ｇｒａｖに近い値に決定する。

アンチロック・ブレーキシステム４３から作動を示すフラグ信号、及び、トラクションコントロールシステム４５から作動を示すフラグ信号のいずれか一方が出力されているとき（すなわち、車輪がスリップしているとき）には、作動判定部５１は、加速度ゲインＫ、及び、姿勢角ゲインＫ_ＡＮＧを十分小さな値に設定する。

作動判定部５１が加速度ゲインＫを十分小さい値に設定すると、車両状態推定器３１は、推定車速を、加速度方程式の右辺を数値積分した過渡車速に近い値に決定する。また、作動判定部５１が姿勢角ゲインＫ_ＡＮＧを十分小さい値に設定すると、車両状態推定器３１は、推定姿勢角を、姿勢角方程式の右辺を数値積分した過渡姿勢角に近い値に決定する。

次に、このように構成した車両状態推定器３１の効果について説明する。

車輪がスリップしていない（すなわち、滑走状態にない）ときには、車輪速によって得られる車速は真値に近い。一方、車輪が滑走状態にあるときには、車輪速によって得られる車速は真値から遠く、誤差は大きくなる。

車輪がスリップしていないときには、図４（Ａ）に示すように、推定車速（車速の推定値。二点鎖線）が、車輪速センサ９によって取得された定常車速（破線）に近い値に決定される。一方、加速度方程式の右辺を数値積分した過渡車速（一点鎖線）には、数値積分による誤差が含まれる。よって、推定車速が定常車速に近い値に決定されることによって、過渡車速に近い値に決定される場合に比べて、推定車速が真値（実線）に近くなり、車速の推定精度を高めることができる。

また、車輪がスリップしていないときには、車輪速に基づく車速が真値に近く、姿勢角方程式の右辺を数値積分した過渡姿勢角には数値積分による誤差が含まれるため、車輪速や、慣性計測センサによって取得される加速度等から算出される定常姿勢角が、過渡姿勢角に比べて、姿勢角の真値に近くなる。

車輪がスリップしていないときには、推定姿勢角（姿勢角の推定値）は定常姿勢角に近くなるように決定される。よって、推定姿勢角が、過渡姿勢角に近くなるように決定される場合に比べて、推定精度を高めることができる。

一方、車輪がスリップしている（すなわち、滑走状態にある）ときには、図４（Ｂ）に示すように、推定車速（車速の推定値。二点鎖線）が、加速度方程式の右辺を数値積分した過渡車速（一点鎖線）に近い値となるように決定される。これにより、車速の推定値が、定常車速（破線）に近い値となるように決定される場合に比べて、推定車速が真値（実線）に近くなり、車速の測定精度を高めることができる。

車輪がスリップしているときには、車輪速に基づいて算出される定常姿勢角よりも、過渡姿勢角が真値に近くなる。車輪がスリップしているときには、推定姿勢角が過渡姿勢角に近い値となるように決定されるため、定常姿勢角に近い値となるように決定される場合に比べて、姿勢角の測定精度を高めることができる。

図４（Ａ）に示すように、過渡車速は時間の経過とともに、車速の真値から離れるように変化する。本実施形態では、加速度ゲインＫが十分大きく、車輪がスリップしていないときには、推定車速は、概ね車輪速センサ９によって取得される定常車速に等しくなるように算出され、車輪がスリップし始めると、加速度ゲインＫが変更されて、推定車速は、スリップし始めたときの推定車速を初期値として数値積分することによって得られる過渡車速に近い値に算出される。これにより、過渡車速の算出に用いる数値積分するための積分区間の下限（下端ともいう）をスリップし始める時刻に設定することができる。これにより、積分区間の下限を車両の走行が開始された時刻とした場合に比べて、過渡車速の算出に用いる積分区間を短くすることができるため、スリップ時の推定車速（すなわち、過渡車速）の誤差を低減することができる。

車両Ｓの質量は車両Ｓへの積載量等の使用状態によって変動し、路面と車輪（タイヤ）との間の摩擦係数もまた路面状況に応じて大きく変動する。よって、タイヤのパラメータによって変動するコーナリンングパワー係数などを、不確定性を有するパラメータを用いて表し、車両Ｓの運動方程式を構築すると、使用条件や路面状況を考慮してパラメータを変更する必要があり、車両状態の推定が容易ではない。

本実施形態では、式（８）及び式（１３）に示すように、速度オブザーバ方程式と、姿勢角オブザーバ方程式にそれぞれ質量及び摩擦係数が含まれていない。車両Ｓの質量及び摩擦係数等の不確定性を有するパラメータに依存することなく、車速及び姿勢角を算出することができる。よって、車両Ｓの使用状態や路面状況に依らない車両状態の推定が可能となる。

加速度ゲインＫ、及び、姿勢角ゲインＫ_ＡＮＧを変更することによって、推定車速（推定姿勢角）を、車輪がスリップしていないときには定常車速（定常姿勢角）に近い値に、車輪がスリップしているときには過渡車速（過渡姿勢角）に近い値にそれぞれ決定することができる。また、推定車速（推定姿勢角）を、オブザーバ方程式を積分更新していくこと取得するため、過渡／定常の両面の特性のフュージョンを行うことができ、定常／非定常運動の全域に渡って尤もらしい値に決定することができる。

上記実施形態では、アンチロック・ブレーキシステム４３又はトラクションコントロールシステム４５の作動に基づいて、車輪がスリップするか否かを判定し、加速度ゲインＫ、及び姿勢角ゲインＫ_ＡＮＧを設定する。このように、アンチロック・ブレーキシステム４３又はトラクションコントロールシステム４５の作動を判定に用いることで、車輪が滑走状態にあることを簡便に取得することができる。

次に、異常判定装置６８の動作、及び、効果について説明する。

車輪がスリップしたときには、車輪速に基づく車速は真値とは異なるため、主慣性計測センサ５（６軸慣性計測センサ）によって取得される車両前後方向の加速度と、車輪速に基づいて取得される車両前後方向の加速度とが乖離する場合がある。従って、主慣性計測センサ５による前後方向の加速度と、車輪速センサ９による前後方向の加速度を比較することによって、主慣性計測センサ５の異常を判定することは難しい。

異常判定器３３は、ステップＳＴ１において、主慣性計測センサ５（６軸慣性計測センサ）によって取得された車両前後方向の加速度と、副慣性計測センサ７（３軸慣性計測センサ）によって取得された車両前後方向の加速度とを比較し、両者が合致しない場合に、主慣性計測センサ５（６軸慣性計測センサ）の異常があると判定する。そのため、車輪速センサ９による前後方向の加速度を用いる場合に比べて、車輪がスリップしている場合であっても、前後方向の加速度の検出における主慣性計測センサ５の異常を適切に判定することができる。これにより、車両前方の車両Ｓの移動量が精度よく取得できているかどうか、すなわち、自己位置を適切に評価できているかを適切に判定することができる。

異常判定器３３は、ステップＳＴ２において、主慣性計測センサ５の左右方向の加速度と、副慣性計測センサ７によって取得された車両前後方向の加速度とを比較し、両者が合致しない場合に、主慣性計測センサ５の異常があると判定する。また、異常判定器３３は、ステップＳＴ３において、主慣性計測センサ５によって取得された角速度ωのｚ成分ω_ｚ（すなわち、ヨーレート）と、副慣性計測センサ７によって取得されたｚ方向の角速度ω'_ｚ（すなわち、ヨーレート）が合致しているか否かを判定する。

このように、異常判定器３３は、主慣性計測センサ５によって取得される前後方向の加速度、左右方向の加速度、及び、ヨーレートを副慣性計測センサ７によって取得される値と比較することによって、異常の有無を判定する。これにより、２つのセンサによる相互監視機構を構成することができるとともに、６軸慣性計測センサよりも安価な３軸慣性計測センサを用いることで、２つの６軸慣性計測センサを用いるよりもコストを下げることができる。

また、異常判定器３３は、主慣性計測センサ５によって取得される前後方向の加速度、左右方向の加速度、及び、ヨーレートを副慣性計測センサ７によって取得される値と比較するため、異常が発生すると、自己位置の推定値に誤差が大きくなりやすい前後方向の加速度、左右方向の加速度、及び、ヨーレートの異常をより確実に検知することができる。

異常判定器３３は、ステップＳＴ５において、姿勢角方程式（式（１３））を数値積分することにより得られる姿勢角の推定値（すなわち、推定姿勢角Θ_０）と、定常姿勢角Θ_ｇｒａｖとを算出し、両者の差のｘ成分及びｙ成分を推定誤差とする。６軸慣性計測センサの異常時などにおいて推定誤差が過大となるため、異常判定器３３はこの推定誤差を用いて、６軸慣性計測センサの異常の有無を判定することができる。

より詳細には、異常判定器３３は、ステップＳＴ５において、推定姿勢角Θ_０のｘ成分及び定常姿勢角Θ_ｇｒａｖのｘ成分の差分と、推定姿勢角Θ_０のｙ成分及び定常姿勢角Θ_ｇｒａｖのｙ成分の差分とをそれぞれ算出し、算出された差分がそれぞれ対応する閾値以下であるか否かを判定する。これにより、異常判定器３３は、ステップＳＴ２及びステップＳＴ４において評価されていないロール方向、及び、ピッチ方向の主慣性計測センサ５（６軸慣性計測センサ）の異常を検知することができる。

以上で具体的実施形態の説明を終えるが、本発明は上記実施形態に限定されることなく幅広く変形実施することができる。

上記実施形態では、スリップ判定装置４１は、アンチロック・ブレーキシステム４３と、トラクションコントロールシステム４５とを備えていたが、この態様には、限定されない。例えば、スリップ判定装置４１はビークルスタビリティアシスト装置（ＶＳＡ、横滑り抑制装置）を備え、旋回時に車輪の横滑りなどを抑制するための制御を行ったときに、車輪が滑走状態にあると判定するものであってもよい。また、スリップ判定装置４１は舵角センサを備え、舵角センサによって検出される舵角の時間変化率が所定の閾値以上であるときに、車輪が滑走状態にあると判定するものであってもよい。また、滑走状態には、車輪が滑走状態となり易い状態や、路面状況によっては滑走状態になりえる状態、また、滑走状態に移る直前の状態を含めてもよい。

上記実施形態では、主慣性計測センサ５（６軸慣性計測センサ）の位置における速度を算出していたが、この態様には限定されない。例えば、主慣性計測センサ５によって取得された推定速度を式（１４）によって変換することによって、車両Ｓの所定の位置の速度を推定するように構成してもよい。

但し、式（１４）において、ｖは変換後速度、ｖ_０は変換前速度、ωは主慣性計測センサ５によって検出された角速度であり、Ｌは主慣性計測センサ５が設けられた位置を起点、速度を推定すべき車両の位置を終点とするベクトルを示す。

上記実施形態では、異常判定器３３は、ＳＴ５において、姿勢角方程式による姿勢角の推定値（すなわち、推定姿勢角Θ_０）と、定常姿勢角Θ_ｇｒａｖとの差に基づいて、推定誤差を算出していたがこの態様には、限定されない。例えば、異常判定器３３は、ＳＴ５において、主慣性計測センサ５によって計測されたｚ軸方向（又はｘ軸方向）の加速度や、副慣性計測センサ７によって計測されたｘ軸方向の加速度を予め定められた補正式を用いて補正することによってピッチ方向（ロール角）の角速度の推定値を算出し、その推定値と、主慣性計測センサ５によって計測されたピッチ方向（ロール方向）の角速度とを相関比較する（又は、閾値以下であるかを判定する）ことによって、主慣性計測センサ５の異常を判定してもよい。

５ ：主慣性計測センサ（６軸慣性計測センサ）
７ ：副慣性計測センサ（３軸慣性計測センサ）
９ ：車輪速センサ
３１ ：車両状態推定器（車両状態推定部）
３３ ：異常判定器（異常判定部）
４６ ：車両状態推定装置
６８ ：異常判定装置
Ｓ ：車両

Claims (6)

1. 車両に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサの異常の有無を判定する異常判定装置であって、
   前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサと、
   前記６軸慣性計測センサの異常の有無を判定する異常判定部とを備え、
   前記異常判定部は、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する異常判定装置。
2. 車両に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサの異常の有無を判定する異常判定方法であって、
   前記車両に搭載され、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサから、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを取得し、
   前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する異常判定方法。
3. 車両に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサの異常の有無を判定する異常判定プログラムであって、
   前記車両に搭載され、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサから、前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを取得し、
   前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する異常判定プログラム。
4. 車両に搭載され、前記車両の前後方向の加速度、左右方向の加速度、及び、上下方向の加速度と、ロールレート、ピッチレート、及びヨーレートとを検出する６軸慣性計測センサの計測値に基づいて車両状態を推定する車両状態推定装置であって、
   前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートを検出する３軸慣性計測センサと、
   前記６軸慣性計測センサの異常の有無を判定する異常判定部とを備え、
   前記異常判定部は、前記６軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートをそれぞれ、前記３軸慣性計測センサによって取得された前記前後方向の加速度、前記左右方向の加速度、及び、前記ヨーレートと比較することにより、前記６軸慣性計測センサの異常の有無を判定する車両状態推定装置。
5. 前記車両に設けられる車輪の車輪速を測定する車輪速センサと、前記車輪速センサ、及び、前記６軸慣性計測センサによって計測される前記計測値に基づいて前記車両の姿勢角を算出する車両状態推定部とを有し、
   前記異常判定部は、前記車両状態推定部によって算出される前記姿勢角と、前記車輪が滑走することなく走行したときに、前記車輪速を用いて算出される重力の方向に基づく定常姿勢角との差が所定の閾値以上となったときに、前記６軸慣性計測センサ、又は、前記車輪速センサに異常があると判定する請求項４に記載の車両状態推定装置。
6. 前記車両状態推定部によって算出されるロール角及び前記定常姿勢角のロール角の差が所定の閾値以上であるか、又は、前記車両状態推定部によって算出されるピッチ角及び前記定常姿勢角のピッチ角の差が所定の閾値以上であるときに、前記６軸慣性計測センサ、又は、前記車輪速センサに異常があると判定する請求項５に記載の車両状態推定装置。

Images