(57)【要約】  【課題】  車両の姿勢を推定するための測定手段が３つである場合にも、それらのうちの故障している測定手段を特定する。  【解決手段】  車両Ｃの同種の姿勢をそれぞれ測定するための３つ以上のセンサ１０と、センサ１０の測定値に基づいて車両Ｃの姿勢を算出する姿勢算出部２０と、３つ以上のセンサ１０でそれぞれ測定された姿勢の差分を算出する差分算出部３０と、差分に基づいて、３つ以上のセンサ１０のうちの、他の複数のセンサ１０の測定値から算出された姿勢に対して差分を有する姿勢が算出されたセンサ１０を故障センサ１０´として特定する故障検知部５０を備えている。【選択図】  図１
2019196945.tif 000002
【特許請求の範囲】
【請求項１】
車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段（１０、２０）と、
前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）と、
前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を故障測定手段として特定する故障特定手段（５０）と、
を備えた、故障検知システム。
【請求項２】
前記故障特定手段は、累積された前記差分に基づいて前記故障測定手段を特定する、請求項１に記載の故障検知システム。
【請求項３】
前記故障測定手段が測定する前記姿勢を、前記差分に従って補正する補正手段をさらに備えた、請求項１又は２に記載の故障検知システム。
【請求項４】
前記３つ以上の測定手段の少なくとも一つは、前記車両に固定されたカメラと、前記カメラの撮影によって得られた画像に基づいて前記姿勢を測定する画像処理手段とを備えた、請求項１〜３のいずれかに記載の故障検知システム。
【請求項５】
前記画像処理手段は、前記画像に含まれる鉛直方向に沿う構造物を検出して、当該構造物の前記画像内における向きに基づいて、前記姿勢として車両のロール角を算出する、請求項４に記載の故障検知システム。
【請求項６】
前記カメラが前記故障測定手段として特定されたときに、当該カメラの撮影によって得られた画像を、前記差分に基づいて補正する画像補正手段（７０）をさらに備えた、請求項４又は５に記載の故障検知システム。
【請求項７】
前記３つ以上の測定手段は、前記車両の同種の姿勢として、同軸周りの角速度を測定する、請求項１〜５のいずれかに記載の故障検知システム。
【請求項８】
前記３つ以上の測定手段は、前記車両の同種の姿勢として、前記車両のピッチ角、ヨー角、ロール角のいずれかを測定する、請求項１〜５のいずれかに記載の故障検知システム。
【請求項９】
車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段（１０、２０）と、
前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）と、
前記３つ以上の測定手段でそれぞれ測定された姿勢及び前記差分算出手段で算出された差分に基づいて、前記車両の姿勢を推定する姿勢推定手段（６０）と、
を備えた車両姿勢推定システム。
【請求項１０】
前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を故障測定手段として特定する故障特定手段（５０）をさらに備え、
前記姿勢推定手段は、前記故障測定手段が測定した姿勢を補正する、
請求項９に記載の車両姿勢推定システム。
【請求項１１】
前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を故障測定手段として特定する故障特定手段（５０）をさらに備え、
前記姿勢推定手段は、前記故障測定手段が測定した姿勢を無視して前記車両の姿勢を推定する、
請求項９に記載の車両姿勢推定システム。
【請求項１２】
演算処理装置において実行されることで、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段でそれぞれ測定された姿勢から故障した前記測定手段を検知する故障検知プログラムであって、前記演算処理装置を、
前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）、及び
前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を特定する故障特定手段（５０）、
として機能させる、故障検知プログラム。
【請求項１３】
演算処理装置において実行されることで、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段でそれぞれ測定された姿勢から車両の姿勢を推定する車両姿勢推定プログラムであって、前記演算処理装置を、
前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）、及び
前記３つ以上の測定手段でそれぞれ測定された姿勢及び前記差分に基づいて、前記車両の姿勢を推定する姿勢推定手段（６０）、
として機能させる、車両姿勢推定プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測定手段の故障を検知する故障検知システム及びプログラム、並びに測定手段を用いて車両の姿勢を推定する車両姿勢推定システム及びプログラムに関する。
【背景技術】
【０００２】
車両において、自動運転、運転アシスト、車両制御を行うために、車両の姿勢（例えば、ロール角、ピッチ角、ヨー角）を推定する必要がある。このために、車両には車両の姿勢に応じた物理量を測定してその測定値を出力するセンサが設けられる。例えば、車両の姿勢を測定するにジャイロセンサが用いられ、ジャイロセンサの測定値に基づいて車両の姿勢が推定される。
【０００３】
このようなセンサに故障が生じた場合には車両の正確な姿勢を推定することができなくなるため、センサに故障が生じた場合にはそれを検知する必要がある。特許文献１には、車両の姿勢推定のために車両に４つのジャイロセンサを取り付けて、それらで取得された角速度をそれぞれ比較することで、ジャイロセンサの故障を検知する方法が開示されている。具体的には、特許文献１の方法では、正四角錐（ピラミッド形）の支持台の４つの斜面にそれぞれジャイロセンサを設置することで、いずれかのジャイロセンサの故障を検知して、正しい角速度又は角度を測定する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００５−２２１２８４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、特許文献１に記載の方法によれば、３つのジャイロセンサだけでは、一部のジャイロセンサに故障が生じた場合にその方向の角速度を測定することができない（特許文献１の００２９段落参照）。
【０００６】
そこで、本発明は、車両の姿勢を推定するための測定手段が３つである場合にも、それらのうちの故障している測定手段を特定できる故障検知システム及びプログラム、並びに３つの測定手段のうちのいずれかが故障していても車両の姿勢を推定できる車両姿勢推定システム及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記目的を達成するために、本発明の第１の態様は、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段（１０、２０）と、前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）と、前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を故障測定手段として特定する故障特定手段（５０）とを備えた故障検知システムである。
【０００８】
本発明の第２の態様は、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段（１０、２０）と、前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）と、前記３つ以上の測定手段でそれぞれ測定された姿勢及び前記差分算出手段で算出された差分に基づいて、前記車両の姿勢を推定する姿勢推定手段（６０）とを備えた車両姿勢推定システムである。
【０００９】
本発明の第３の態様は、演算処理装置において実行されることで、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段でそれぞれ測定された姿勢から故障した前記測定手段を検知する故障検知プログラムであって、前記演算処理装置を、前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）、及び前記差分に基づいて、前記３つ以上の測定手段のうちの、他の複数の前記測定手段で測定された姿勢に対して差分を有する姿勢を測定した前記測定手段を特定する故障特定手段（５０）として機能させる故障検知プログラムである。
【００１０】
本発明の第４の態様は、演算処理装置において実行されることで、車両の同種の姿勢をそれぞれ測定する３つ以上の測定手段でそれぞれ測定された姿勢から車両の姿勢を推定する車両姿勢推定プログラムであって、前記演算処理装置を、前記３つ以上の測定手段でそれぞれ測定された前記姿勢の差分を算出する差分算出手段（３０）、及び前記３つ以上の測定手段でそれぞれ測定された姿勢及び前記差分に基づいて、前記車両の姿勢を推定する姿勢推定手段（６０）として機能させる車両姿勢推定プログラムである。
【図面の簡単な説明】
【００１１】
【図１】図１は、本発明の実施の形態の車両姿勢推定システムの構成を示すブロック図である。
【図２】図２は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平の場合）を示す図である。
【図３】図３は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平から傾いている場合）を示す図である。
【図４】図４は、本発明の実施の形態の路面が傾いている場合の撮影画像の例を示す図である。
【図５】図５は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平から傾いており、かつ、カメラがずれている場合）を示す図である。
【図６】図６は、本発明の実施の形態の図５の場合の撮影画像の例を示す図である。
【図７】図７は、本発明の実施の形態の観測姿勢の差分（軸ずれがない場合）を説明する図である。
【図８】図８は、本発明の実施の形態の観測姿勢の差分（軸ずれがある場合）を説明する図である。
【図９】図９は、本発明の実施の形態の車両姿勢推定のフロー図である。
【発明を実施するための形態】
【００１２】
以下、図面を参照して本発明の実施形態を説明する。なお、以下に説明する実施形態は、本発明を実施する場合の一例を示すものであって、本発明を以下に説明する具体的構成に限定するものではない。本発明の実施にあたっては、実施の形態に応じた具体的構成が適宜採用されてよい。
【００１３】
図１は、本発明の実施の形態の車両姿勢推定システムの構成を示すブロック図である。車両姿勢推定システム１００は、車両において構成され、ＩＭＵ（Inertial Measurement Unit：慣性計測ユニット）１１、ジャイロセンサ１２、及びカメラ１３（以下、総称して「センサ１０」という。）を備えている。これらのセンサ１０は、いずれも車両の姿勢を表す物理量を計測して、当該物理量を表す情報を出力するものである。
【００１４】
ＩＭＵ１１は、角速度と加速度を検出するためのセンサ群を１つにまとめたセンサである。具体的には、ＩＭＵ１１は、３つのジャイロセンサと３つの加速度センサを含み、測定値として車両の３軸周り（即ち、ロール、ピッチ、ヨー）の角速度及び３軸方向（即ち、前後方向、上下方向、左右方向）の加速度を出力する。ＩＭＵ１１は、さらにジャイロセンサや加速度センサのバイアス誤差を補正するために、地磁気センサや傾斜センサ、ＧＰＳを備えていてもよい。
【００１５】
ジャイロセンサ１２は、ＩＭＵ１１とは別体として設けられた単体のセンサである。ジャイロセンサ１２の軸は車両の前後方向を向いており、ジャイロセンサ１２は測定値としてロール角の角速度を出力する。
【００１６】
カメラ１３は、光軸が車両の前後方向を向き、かつ、垂直軸が車両の上下方向と一致するように、車両に固定される。カメラ１３は、車両前方の画像を撮影する。カメラ１３は、測定値として、撮影によって得られた画像（以下、「撮影画像」という。）を出力する。
【００１７】
車両姿勢推定システム１００は、さらに、姿勢算出部２０と、差分算出部３０と、保持部４０と、故障検知部５０と、姿勢推定部６０と、画像補正部７０とを備えている。姿勢算出部２０、差分算出部３０、故障検知部５０、姿勢推定部６０、及び画像補正部７０は、プロセッサ等の演算処理装置が所定のプログラムを実行することで実現されて、以下に説明する動作を実行する。また、保持部４０は、演算処理装置からアクセス可能なメモリ等の記憶装置を含んで構成される。
【００１８】
なお、姿勢算出部２０、差分算出部３０、故障検知部５０、姿勢推定部６０、及び画像補正部７０は、１つのプロセッサで実現されてもよいし、複数のプロセッサで実現されてもよい。また、保持部４０も、複数の記憶装置からなっていてもよい。
【００１９】
また、車両姿勢推定システム１００において、センサ１０と、姿勢算出部２０と、差分算出部３０と、保持部４０と、故障検知部５０とで故障検知システム１０１が構成される。後述するように、故障検知部５０は、故障検知システム１０１の出力として故障センサ１０´を特定する情報及びその故障センサ１０´の軸ずれ角εの情報を出力し、姿勢推定部６０はそれらの情報を用いて車両の姿勢を推定し、画像補正部７０はそれらの情報を用いて画像を補正するが、故障検知システム１０１の出力の用途はこれらに限られない。
【００２０】
姿勢算出部２０は、各センサ１０から出力される情報に基づいて、車両の姿勢を算出する。具体的には、姿勢算出部２０は、ＩＭＵ１１から車両のピッチ角、ロール角、ヨー角の角速度、及び前後方向、左右方向、上下方向の加速度を取得して、これらに基づいて、車両のピッチ角、ロール角、ヨー角を算出する。姿勢推定部２０は、例えばロール角については、ＩＭＵ１１から取得したロール角の角速度を積分することで、現在の車両のロール角を算出する。
【００２１】
また、姿勢算出部２０は、ジャイロセンサ１２から出力されるロール角の角速度に基づいて、車両のロール角を算出する。具体的には、姿勢算出部２０は、ジャイロセンサ１２から車両のロール角の角速度を取得して、これを積分することで現在の車両のロール角を算出する。さらに、姿勢算出部２０は、カメラ１３から出力される画像に基づいて、車両のロール角を算出する。
【００２２】
図２は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平の場合）を示す図である。カメラ１３は、上述のように、その上下方向（即ち、垂直軸１３２の方向）が車両Ｃの上下方向に一致するように、車両Ｃに取り付けられる。また、カメラ１３の光軸は車両Ｃの前後方向を向いている。
【００２３】
図２に示すように、カメラ１３の視野１３１は長方形であり、その上辺及び下辺はいずれも現実空間の水平方向と平行であり、その左辺及び右辺は現実空間の鉛直方向（即ち、重力方向）Ｒ１と平行である。したがって、車両Ｃが傾きのない水平な路面Ｒを走行している場合には、撮影画像において、左右方向は現実空間の水平方向と一致しており、上下方向は現実空間の鉛直方向と一致している。
【００２４】
図３は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平から傾いている場合）を示す図である。図３に示すように、路面Ｒが水平から角度θだけ傾いていると、カメラ１３の視野１３１及び垂直軸１３２は、鉛直軸Ｒ１に対して角度θだけ傾くことになる。しかしながら、構造物Ｓは、路面Ｒが傾いていたとしても、鉛直軸Ｒ１に沿って建てられている。姿勢算出部３０は、このことを利用して撮影画像から車両の姿勢（即ち、ロール角）を算出する。
【００２５】
図４は、本発明の実施の形態の路面が傾いている場合の撮影画像の例を示す図である。図４に示すように、路面Ｒが角度θだけ傾いている場合には、撮影画像において、鉛直方向に延びる部分を有する構造部Ｓにおける当該部分は角度θだけ傾いて映ることになる。このように鉛直方向に延びる部分を有する建造物Ｓとしては、例えばビル、ポール等がある。
【００２６】
姿勢算出部２０は、撮影画像から車両のロール角を求めるために画像処理部としての機能を有する。画像処理部は、まず、撮影画像に対して画像処理を行うことで、画像のエッジを示すエッジ画像を生成する。画像処理部は、さらに、エッジ画像から所定の長さ以上の直線状のエッジを抽出し、その方向を求める。
【００２７】
直線状のエッジの抽出方法としては、例えば、Ｈｏｕｇｈ変換（例えば、Ballard, D.H. 1981. Generalizing the Hough transform to detect arbitrary patterns. Pattern Recognition, 13(2):111-122.を参照）、ＬＳＤ（Line Segment Detector）アルゴリズム（例えば、von Gioi, R.G., Jakubowicz, J., Morel, J.M., Randall, G.: LSD: a Line Segment Detector. Image Processing On Line (2012)を参照）を用いることができる。また、検出能力の向上のために、エッジ検出と組み合わせる方法を用いてもよい（例えば、Canny, J., A Computational Approach To Edge Detection, IEEE Trans. Pattern Analysis and Machine Intelligence, 8(6):679-698, 1986.を参照）。さらに、直線状のエッジの抽出の前処理として、建物の画像領域を検出することで、精度をさらに向上させることができる。この場合に、建物の検出には、ニューラルネットワークなどを用いた機械学習による方法を用いることができる。
【００２８】
姿勢算出部２０は、抽出したエッジの方向（即ち、現実空間の鉛直方向）が撮影画像の上下方向に対してなす角度を車両のロール角として算出する。
【００２９】
図５は、本発明の実施の形態の車両とカメラの視野及び垂直軸との関係（路面が水平から傾いており、かつ、カメラがずれている場合）を示す図である。図５のケースでは、路面Ｒが角度θで傾いており、カメラ１３の垂直軸１３２が、本来の向きから車両Ｃの前後方向の軸周り（即ち、カメラ１３の光軸周り）に角度εだけずれている。
【００３０】
図６は、本発明の実施の形態の図５の場合の撮影画像の例を示す図である。図４に示すように、本来であれば、路面Ｒが角度θで傾いているので、構造物Ｓは、撮影画像内において角度θで傾いているはずであるが、図６の場合にはさらにカメラ１３が角度εで傾いているので、撮影画像内における構造物Ｓの傾きはθ＋εとなる。
【００３１】
以上のように、姿勢算出部２０は、ＩＭＵ１１の測定値に基づいて車両の姿勢（即ち、ロール角）を算出し、ジャイロセンサ１２の測定値に基づいて車両の姿勢（即ち、ロール角）を算出し、カメラ１３の測定値（即ち、撮影画像）に基づいて車両の姿勢（即ち、ロール角）を算出する。
【００３２】
このように、ＩＭＵ１１と、当該ＩＭＵ１１からの情報に基づいて車両の姿勢を算出する姿勢算出部２０とによって、車両の姿勢を測定する１つの測定手段が構成され、ジャイロセンサ１２と、当該ジャイロセンサ１２からの情報に基づいて車両の姿勢を算出する姿勢算出部２０とによって、車両の姿勢を測定する他の測定手段が構成され、カメラ１３と、当該カメラ１３から入力される情報に基づいて車両の姿勢を算出する姿勢算出部２０とによって、車両の姿勢を測定するさらに他の測定手段が構成される。
【００３３】
これらの３つの測定手段は、いずれも車両の同種の姿勢として同じ軸周りの車両の角度を測定するものである。具体的には、本実施の形態の場合には、３つの測定手段は、それぞれ、車両の同種の姿勢として、車両Ｃの前後方向に延びる軸周りの回転角、即ち、ロール角を測定する。
【００３４】
なお、本実施の形態では、１つの姿勢算出部２０が３つのセンサ１０のすべてについて、センサ１０の測定値に基づく姿勢の算出を行ったが、これに代えて、センサ１０ごとに、センサ１０の測定値に基づいて車両の姿勢を算出する手段を設けてもよい。あるいは、それぞれのセンサ１０が上記の姿勢算出部２０の機能を備え、車両の姿勢を出力してもよい。この場合にはセンサ１０で測定手段が構成される。
【００３５】
以上のように、センサ１０と姿勢算出部２０からなる測定手段は、車両の同種の姿勢として、ＩＭＵ１１からの情報に基づく姿勢（即ち、ロール角）、ジャイロセンサ１２からの情報に基づく姿勢（即ち、ロール角）、及びカメラ１３からの情報に基づく姿勢（即ち、ロール角）をそれぞれ測定する。以下では、姿勢算出部２０においてセンサ１０の測定値から算出される姿勢を「観測姿勢」という。センサ１０の軸がずれている場合には、観測姿勢は当該軸ずれ角を含んで算出される。
【００３６】
ＩＭＵ１１の測定値から算出される観測姿勢をθ１、ジャイロセンサ１２の測定値から算出される観測姿勢をθ２、カメラ１３の撮影画像から算出される観測姿勢をθ３とすると、これらは、真値θに加えて、センサ１０の観測ノイズΔθ及び軸ずれ角εを含んでいる可能性があり、一般的には以下の式（１）〜（３）で表現される。
θ１＝θ＋Δθ１＋ε１    …（１）
θ２＝θ＋Δθ２＋ε２    …（２）
θ３＝θ＋Δθ３＋ε３    …（３）
【００３７】
姿勢算出部２０は、算出した観測姿勢θ１〜θ３を差分算出部３０及び保持部４０に出力する。
【００３８】
差分算出部３０は、姿勢算出部２０で得られた３つの観測姿勢のそれぞれの差分を算出する。具体的には、差分算出部３０は、ＩＭＵ１１からの情報に基づく観測姿勢θ１とジャイロセンサ１２からの情報に基づく観測姿勢θ２との差分（θ１−θ２）を算出し、ジャイロセンサ１２からの情報に基づく観測姿勢θ２とカメラ１３からの情報に基づく観測姿勢θ３との差分（θ２−θ３）を算出し、カメラ１３からの情報に基づく観測姿勢θ３とＩＭＵ１１からの情報に基づく観測姿勢θ１との差分（θ３−θ１）を算出する。
【００３９】
差分算出部３０は、算出した３つの差分をその時刻情報とともに保持部４０に出力する。保持部４０は、３つの差分とその時刻情報とを関連付けて記憶する。
【００４０】
故障検知部５０は、保持部４０に記憶された差分を読み出して、故障が生じているセンサ１０を故障センサ１０´として特定する。具体的には、故障検知部５０は、他の２つのセンサ１０から算出された姿勢と異なる姿勢が算出された１つのセンサ１０を故障センサ１０´として特定する。
【００４１】
故障検知部５０は、一時刻に得られた観測姿勢（即ち、瞬時値）の差分のみではなく、一定の時間幅で得られた複数の観測姿勢の差分の平均値（即ち、時間平均）をとることにより、３つの観測姿勢のそれぞれの差分を算出する。すなわち、差分算出部３０は、所定の時間内の差分を統計的に処理することで、観測ノイズの影響を小さくして軸ずれの影響による差分を求める。故障検知部５０は、最新の差分を含み、最新の差分から過去に所定の時間だけ遡った時点までの複数の差分の平均を時間平均として算出する。
【００４２】
いずれのセンサ１０にも軸ずれが生じておらず、軸調が完全な場合には、式（１）〜（３）の軸ずれ角ε１、ε２、ε３はいずれも０になる。また、式（１）〜（３）の観測ノイズΔθ１、Δθ２、Δθ３は、時間に独立であるので、これらの時間平均は０とみなすことができる。
【００４３】
いま、軸ずれがない（即ち、ε１＝０、ε２＝０、ε３＝０）場合には、差分（θ１−θ２）（＝θ＋Δθ１＋ε１−（θ＋Δθ２＋ε２））は、時間平均をとると、Δθ１−Δθ２の成分は０になるので、θ−θ＝０となる。差分（θ２−θ３）及び差分（θ３−θ１）についても同様に、軸ずれがない場合には、時間平均をとると０になる。
【００４４】
図７は、本発明の実施の形態の観測姿勢の差分（軸ずれがない場合）を説明する図である。図７（ａ）は、ＩＭＵ１１とジャイロセンサ１２との間の観測姿勢の差分（θ１−θ２）を示しており、図７（ｂ）は、ジャイロセンサ１２とカメラ１３との間の観測姿勢の差分（θ２−θ３）を示しており、図７（ｃ）は、カメラ１３とＩＭＵ１１との間の観測姿勢の差分（θ３−θ１）を示している。図７の例では、いずれのセンサ１０にも軸ずれが生じていない（即ち、ε１＝０、ε２＝０、ε３＝０）場合を示している。
【００４５】
図７に示すように、一時刻の差分（即ち、瞬時値）を見ると、各センサ１０にランダムに生じる観測ノイズのため必ずしも差分は０にはなっていないが、時間平均をとると、軸ずれがない場合には、ランダムに生じる観測ノイズが互いに相殺されて、差分は平均値としては０になる。
【００４６】
一方、軸ずれが生じている場合には、各差分の時間平均をとると、観測ノイズが互いに相殺されて軸ずれの影響εが残ることになる。例えば、ＩＭＵ１１に軸ずれε１が生じているとすると、差分（θ１−θ２）の時間平均はε１となり、差分（θ２−θ３）の時間平均は０となり、差分（θ３−θ１）の時間平均は−ε１となる。このように、差分（θ１−θ２）の時間平均はε１であり、かつ、差分（θ３−θ１）の時間平均は−ε１であることから、θ１にε１のずれが生じていることが分かる。
【００４７】
図８は、本発明の実施の形態の観測姿勢の差分（軸ずれがある場合）を説明する図である。図７に例と同様に、図８（ａ）は、ＩＭＵ１１とジャイロセンサ１２との間の観測姿勢の差分（θ１−θ２）を示しており、図８（ｂ）は、ジャイロセンサ１２とカメラ１３との間の観測姿勢の差分（θ２−θ３）を示しており、図８（ｃ）は、カメラ１３とＩＭＵ１１との間の姿勢の観測差分（θ３−θ１）を示している。図８の例では、ジャイロセンサ１２に軸ずれが生じている場合を示している。
【００４８】
ジャイロセンサ１２に軸ずれが生じると、図８（ａ）に示すＩＭＵ１１とジャイロセンサ１２との間の観測姿勢の差分は０にはならず、図８（ｂ）に示すジャイロセンサ１２とカメラ１３との間の観測姿勢の差分も０にはならない。このことから、ジャイロセンサ１２に軸ずれが生じていることが分かる。
【００４９】
故障検知部５０は、特定した故障センサ１０´及びその軸ずれ角εの情報を姿勢推定部６０及び画像補正部７０に出力する。姿勢推定部６０は、姿勢算出部２０で算出された観測姿勢（即ち、ロール角）θ１、θ２、θ３と、故障センサ１０´及びその軸ずれ角εの情報とに基づいて、車両の姿勢の真値θを推定する。このとき、姿勢推定部６０は、保持部４０に保持されている一定時間幅の複数の観測姿勢θ１、θ２、θ３を読み出して、それぞれの時間平均をとって、各センサ１０の観測姿勢θ１、θ２、θ３とする。
【００５０】
姿勢推定部６０は、いずれのセンサ１０にも軸ずれがない場合には、観測姿勢θ１、θ２、θ３のいずれか、あるいはそれらの平均を真値θであると推定する。姿勢推定部６０は、いずれかのセンサ１０に軸ずれεがある場合には、軸ずれεを打ち消すようにその故障センサ１０´の観測姿勢を補正して、補正された後の観測姿勢を用いて真値θを推定する。あるいは、姿勢推定部６０は、いずれかのセンサ１０に軸ずれがある場合には、当該故障センサ１０´の観測姿勢を無視して、軸ずれが生じていないセンサ１０の観測姿勢のみを用いて姿勢の真値θを推定してもよい。
【００５１】
画像補正部７０は、故障検知部５０から、カメラ１３が故障センサ１０´として特定された旨の情報を受けた場合には、撮影画像をカメラ１３の軸ずれ角ε３に基づいて補正する。この軸ずれ角ε３は、故障検知部５０において姿勢の差分（θ２−θ３）（＝−ε３）及び／又は姿勢の差分（θ３−θ１）（＝ε３）から求められたものである。
【００５２】
図９は、本発明の実施の形態の車両姿勢推定のフロー図である。まず、各センサ１０がセンシングを行って測定値を取得する（ステップＳ９１）。姿勢算出部２０は、各センサ１０の測定値に基づいて車両の観測姿勢を算出して保持部４０に保存する（ステップＳ９２）。差分算出部３０は、同一時刻の各観測姿勢どうしの差分を算出して保持部４０に保存する（ステップＳ９３）。
【００５３】
故障検知部５０は、各差分に基づいて故障センサ１０´を検知し、その軸ずれ角εを求める（ステップＳ９４）。故障検知部５０は、故障センサ１０´がない場合には（ステップＳ９５にてＮＯ）、保持部４０に保持された各観測姿勢の時間平均を取って姿勢の真値を推定する（ステップＳ９６）。故障検知部５０は、故障センサ１０´がある場合には（ステップＳ９５にてＹＥＳ）、故障センサ１０´の観測姿勢を補正した上で（ステップＳ９７）、姿勢の真値を推定する（ステップＳ９６）。
【００５４】
以上説明したように、本発明の実施の形態の故障検知システムによれば、車両の同種の姿勢を測定する少なくとも３つの測定手段を備えていればセンサの故障を検知することができ、本実施の形態の車両姿勢推定システムによれば、車両の同種の姿勢を測定する少なくとも３つの測定手段を備えていれば、いずれかのセンサが故障していた場合にも車両の姿勢を正しく推定できる。
【００５５】
なお、上記の実施の形態では、姿勢算出部２０は、ＩＭＵ１１、ジャイロセンサ１２、カメラ１３の測定値に基づいて、車両の同種の姿勢として車両のロール角を算出したが、カメラ１３の光軸が車両の左右方向を向いている場合には、撮影画像に基づいて車両のピッチ角を測定することができる。この場合には、さらに２つ以上ピッチ角を算出できるセンサ１０を備えることで、車両の同種の姿勢として車両のピッチ角を測定できる３つ以上の測定手段を構成することができる。
【００５６】
また、３つ以上の測定手段にて測定される同種の姿勢において、一部の測定手段について固定（即ち、既知）のバイアスないしオフセットが存在していてもよい。この場合には、姿勢算出部２０及び／又は差分算出部３０において、バイアスないしオフセットを考慮することで、軸ずれがない場合には差分の時間平均を０にすることができ、本発明を適用可能である。
【００５７】
また、上記の実施の形態では、センサ１０の一つをカメラ１３にしたが、本発明はこれに限られず、３つ以上のジャイロセンサで３つ以上の測定手段を構成してもよく、２つ以上のカメラで３つ以上の測定手段を構成してもよい。
【００５８】
また、上記の実施の形態では、各時刻の差分を求めて得られた複数の差分の時間平均をとったが、これに代えて、差分算出部３０は、各センサ１０について姿勢の時間平均をとった後にそれらの差分を求めてもよい。また、差分ないし姿勢の時間平均をとる代わりに、差分ないし姿勢の累積値を用いて統計的に処理してもよい。
【符号の説明】
【００５９】
１１  ＩＭＵ、１２  ジャイロセンサ、１３  カメラ、２０  姿勢算出部、
３０  差分算出部、４０  保持部、５０  故障検知部、６０  姿勢推定部、
７０  画像補正部、１３１  視野、１３２  垂直軸
【図１】
2019196945.tif 000003
【図２】
2019196945.tif 000004
【図３】
2019196945.tif 000005
【図４】
2019196945.tif 000006
【図５】
2019196945.tif 000007
【図６】
2019196945.tif 000008
【図７】
2019196945.tif 000009
【図８】
2019196945.tif 000010
【図９】
2019196945.tif 000011