(57)【要約】移動体の姿勢を推定する姿勢推定装置であって、移動体のロール角を推定すると共に、第一、及び第二角速度検出部、並びに、第一、第二、及び第三加速度検出部のうちの少なくとも一つのオフセット誤差を演算処理によって推定する姿勢推定部を備える。姿勢推定部は、対象検出部の仮想オフセット量が、相互に少なくとも２種類以上の異なる値として与えられた複数のカルマンフィルタを含む。複数のカルマンフィルタは、それぞれ、現在の推定動作において、各検出部における検出値、前回の推定動作による推定値、及び仮想オフセット量を用いて、推定値の確からしさを表す尤度を算出する。姿勢推定部は、尤度に基づいて各カルマンフィルタの推定値に対して重み付けをすることで、移動体のロール角を推定する。
2017175844.tif 000002
【特許請求の範囲】
【請求項１】
移動体の姿勢を推定する姿勢推定装置であって、
前記移動体の、第一軸の周りでの角速度である第一角速度を検出する第一角速度検出部と、
前記移動体の、前記第一軸とは異なる方向である第二軸の周りでの角速度である第二角速度を検出する第二角速度検出部と、
前記移動体の、第一方向に関する加速度である第一加速度を検出する第一加速度検出部と、
前記移動体の、前記第一方向とは異なる第二方向に関する加速度である第二加速度を検出する第二加速度検出部と、
前記移動体の、前記第一方向及び前記第二方向とは異なる第三方向に関する加速度である第三加速度を検出する第三加速度検出部と、
前記移動体の進行方向の移動速度に関する情報を検出する速度情報検出部と、
前記移動体のロール角を推定すると共に、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差である、第一オフセット誤差を演算処理によって推定する姿勢推定部とを備え、
前記姿勢推定部は、
前記第一オフセット誤差の仮想オフセット量として、互いに異なる少なくとも２つの値を含む複数の値が、それぞれ入力される複数のカルマンフィルタを含み、
前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、及び前記仮想オフセット量を用いて、前記移動体のロール角の推定値、及び推定結果の確からしさを表す尤度を算出し、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び、前記複数のカルマンフィルタのそれぞれに入力された前記仮想オフセット量に基づいて、前記第一オフセット誤差を推定する姿勢推定装置。
【請求項２】
前記姿勢推定部は、前記複数のカルマンフィルタの各々から得られた前記尤度に基づいて、前記複数のカルマンフィルタの各々に入力された前記仮想オフセット量に重み付けすることで、前記第１オフセット誤差を推定する、請求項１に記載の姿勢推定装置。
【請求項３】
前記姿勢推定部は、
前記複数のカルマンフィルタの各々から得られた前記尤度及び、前記複数のカルマンフィルタの各々から得られた前記移動体のロール角に基づいて、前記移動体のロール角を推定する、請求項１又は２に記載の姿勢推定装置。
【請求項４】
前記複数のカルマンフィルタは、
前記仮想オフセット量として、前記第一オフセット誤差の対象となる検出部である対象検出部のオフセット量が取り得る最大値よりも大きい値が与えられた第一カルマンフィルタと、
前記仮想オフセット量として、前記対象検出部のオフセット量が取り得る最小値よりも小さい値が与えられた第二カルマンフィルタと、
前記仮想オフセット量として、前記対象検出部のオフセット量が取り得る前記最小値と前記最大値の間の値が与えられた第三カルマンフィルタとを含む、請求項１〜３のいずれか１項に記載の姿勢推定装置。
【請求項５】
前記第一オフセット誤差は、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの一つのオフセット誤差である、請求項１〜４のいずれか１項に記載の姿勢推定装置。
【請求項６】
前記複数のカルマンフィルタは、
前記移動体のロール角、及び前記尤度に加えて、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの、前記対象検出部とは別の検出部のオフセット誤差である第二オフセット誤差を推定するものであって、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作による前記第二オフセット誤差の推定値、及び前記仮想オフセット量を用いて、前記移動体のロール角の推定値、前記第二オフセット誤差の推定値、及び前記尤度を算出し、
前記姿勢推定部は、
前記複数のカルマンフィルタの各々から得られた前記尤度及び、前記複数のカルマンフィルタの各々から得られた前記第二オフセット誤差に基づいて、前記第二オフセット誤差を推定する、請求項１〜５のいずれか１項に記載の姿勢推定装置。
【請求項７】
前記第二オフセット誤差は、前記対象検出部とは別の検出部のオフセット誤差であって、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差である、請求項６に記載の姿勢推定装置。
【請求項８】
前記第二オフセット誤差は、前記第一角速度検出部、及び前記第二角速度検出部のオフセット誤差を含む、請求項７に記載の姿勢推定装置。
【請求項９】
前記第二オフセット誤差は、前記対象検出部とは別の検出部のオフセット誤差であって、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの一つのオフセット誤差を含む、請求項８に記載の姿勢推定装置。
【請求項１０】
前記第一角速度検出部は、前記移動体のロール角速度を検出し、
前記第二角速度検出部は、前記移動体のヨー角速度を検出し、
前記第二オフセット誤差は、前記第一角速度検出部のオフセット誤差と、前記第二角速度検出部のオフセット誤差と、前記第一加速度検出部又は前記第二加速度検出部のうちの前記対象検出部とは異なる検出部のオフセット誤差とを含む、請求項９に記載の姿勢推定装置。
【請求項１１】
前記第一加速度検出部は、前記移動体の上下方向における加速度を検出し、
前記第二加速度検出部は、前記移動体の左右方向における加速度を検出し、
前記第三加速度検出部は、前記移動体の前後方向における加速度を検出し、
前記第一オフセット誤差は、前記第一加速度検出部のオフセット誤差、又は前記第二加速度検出部のオフセット誤差である、請求項１〜１０のいずれか１項に記載の姿勢推定装置。
【請求項１２】
前記移動体の、前記第一軸及び前記第二軸とは異なる第三軸の周りでの角速度である第三角速度を検出する第三角速度検出部を含み、
前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、前記仮想オフセット量、及び前回の推定動作による第二オフセット誤差の推定値を用いて、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出し、
前記姿勢推定部は、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタのそれぞれに入力された前記仮想オフセット量に基づいて、前記第一オフセット量を推定し、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタの各々から得られた前記移動体のロール角に基づいて、前記移動体のロール角を推定し、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタの各々から得られた前記移動体のピッチ角に基づいて、前記移動体のピッチ角を推定する請求項６〜１０のいずれか１項に記載の姿勢推定装置。
【請求項１３】
前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、前記仮想オフセット量、及び前回の推定動作による第二オフセット誤差の推定値を要素とする所定の関数の値が定数になる特性方程式を用いて、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出する、請求項１２に記載の姿勢推定装置。
【請求項１４】
前記移動体に備えられた前輪及び後輪の少なくとも一方にかかる荷重を推定する荷重推定部を備え、
前記荷重推定部は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、並びに、前記姿勢推定部によって推定された前記移動体のロール角、前記移動体のピッチ角、及び前記第一オフセット誤差に基づいて前記荷重を推定する、請求項１２又は１３に記載の姿勢推定装置。
【請求項１５】
前記移動体の前輪又は後輪の少なくとも一方に設けられたサスペンションのストローク量を推定するサスペンションストローク量推定部を備え、
前記荷重推定部は、前記移動体に設けられた前記前輪及び前記後輪の双方にかかる荷重を推定し、
前記サスペンションストローク量推定部は、前記荷重推定部によって推定された、前記前輪及び前記後輪の双方にかかる荷重の推定値に基づいて、前記サスペンションのストローク量を推定する、請求項１４に記載の姿勢推定装置。
【請求項１６】
前記サスペンションストローク量推定部により推定された、前記サスペンションのストローク量の推定値に基づいて、前記移動体に固定された車体座標系の軸と、前記前輪又は前記後輪が接触している路面上に固定された路面座標系の軸との角度である仰俯角を推定する仰俯角推定部を備えた、請求項１５に記載の姿勢推定装置。
【請求項１７】
前記複数のカルマンフィルタは、それぞれ、前記仰俯角推定部により推定された前記仰俯角の推定値を考慮して、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出する、請求項１６に記載の姿勢推定装置。
【請求項１８】
前記仰俯角推定部により推定された前記仰俯角の推定値と、前記姿勢推定部により推定された前記移動体のロール角及びピッチ角の推定値とに基づいて、前記移動体が設置されている路面の縦断勾配を推定する勾配推定部を備えたことを特徴とする請求項１６又は１７に記載の姿勢推定装置。
【請求項１９】
前記移動体と、
請求項１〜１８のいずれか１項に記載の姿勢推定装置とを備えた輸送機器。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、姿勢推定装置、及びこれを備えた輸送機器に関する。
【背景技術】
【０００２】
従来、自動二輪車等の車両のロール角を推定する種々の推定装置が提案されている。例えば、推定装置により推定されたロール角に基づいて灯火器の向きを制御することにより、車両の傾斜に拘らず灯火器で適切な方向に光を照射することができる。【０００３】
特許文献１に記載された車両姿勢推定装置では、車両運動の前後加速度、横加速度、上下加速度、ヨー加速度、及びロール角速度の各検出値、前後車体速度の推定値、及びピッチ角速度の推定値に基づいて、ロール角及びピッチ角が推定されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００９−７３４６６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
近年、姿勢推定装置の推定精度を向上することが望まれている。【０００６】
本発明は、移動体の姿勢の推定精度を向上させる姿勢推定装置、及びこれを備えた輸送機器を実現することを目的とする。
【課題を解決するための手段】
【０００７】
（構成１）
本発明の１つの観点における構成１における姿勢推定装置は、移動体の姿勢を推定する姿勢推定装置である。前記姿勢推定装置は、
前記移動体の、第一軸の周りでの角速度である第一角速度を検出する第一角速度検出部と、
前記移動体の、前記第一軸とは異なる方向である第二軸の周りでの角速度である第二角速度を検出する第二角速度検出部と、
前記移動体の、第一方向に関する加速度である第一加速度を検出する第一加速度検出部と、
前記移動体の、前記第一方向とは異なる第二方向に関する加速度である第二加速度を検出する第二加速度検出部と、
前記移動体の、前記第一方向及び前記第二方向とは異なる第三方向に関する加速度である第三加速度を検出する第三加速度検出部と、
前記移動体の進行方向の移動速度に関する情報を検出する速度情報検出部と、
前記移動体のロール角を推定すると共に、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差である、第一オフセット誤差を演算処理によって推定する姿勢推定部とを備える。
前記姿勢推定部は、
前記第一オフセット誤差の仮想オフセット量として、互いに異なる少なくとも２つの値を含む複数の値が、それぞれ入力される複数のカルマンフィルタを含む。
前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、及び前記仮想オフセット量を用いて、前記移動体のロール角の推定値、及び推定結果の確からしさを表す尤度を算出する。
前記姿勢推定部は、前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタのそれぞれに与えられた前記仮想オフセット量に基づいて、前記第一オフセット誤差を推定する。【０００８】
上記の「尤度」は、各カルマンフィルタによって推定された量が、実際の量に対してどの程度確からしいかを示す。この尤度の値が高いカルマンフィルタは、当該カルマンフィルタで推定された量の確からしさが高く、尤度の値が低いカルマンフィルタは、当該カルマンフィルタで推定された量の確からしさが低い。例えば、各カルマンフィルタで推定された量を、各尤度に応じて重み付けを行って演算をすることで、推定の精度を高めることができる。【０００９】
上記の構成１では、各カルマンフィルタは、予め、各検出部のうちのある対象検出部におけるオフセット誤差に関する量を「仮想オフセット量」として与えられている。複数のカルマンフィルタにそれぞれ与えられる複数の仮想オフセット量は、互いに異なっている。そして、対象検出部に「仮想オフセット量」が発生している場合における推定量と尤度が、各カルマンフィルタによって算定される。このとき、各カルマンフィルタで算定された尤度は、対象検出部の実際のオフセット誤差に関し、各カルマンフィルタに与えられた仮想オフセット量にどの程度近いか（遠いか）を示す指標となる。つまり、尤度は、仮想オフセット量が、対象検出部の実際のオフセット誤差にどの程度近いかを示す値となる。例えば、各カルマンフィルタにおける対象検出部に関するオフセット誤差を、各尤度に応じて重み付けした値を用いて、当該対象検出部に関する実際のオフセット誤差の推定値を算出することができる。【００１０】
このことは、姿勢推定部において、各カルマンフィルタで推定される量に加えて、新たに一つの量（ここでは、「第一オフセット誤差」）を推定することができることを意味する。例えば、この推定されたオフセット誤差に基づいて、次の推定処理を行うことで、ロール角の推定精度を、従来よりも向上させることができる。【００１１】
（構成２）
上記構成１において、前記姿勢推定部は、前記複数のカルマンフィルタの各々から得られた前記尤度に基づいて、前記複数のカルマンフィルタの各々に入力された前記仮想オフセット量に重み付けすることで、前記第１オフセット誤差を推定してもよい。例えば、各カルマンフィルタにおける仮想オフセット量に、各カルマンフィルタの尤度に応じた重み付けをした値を用いて、前記第１オフセット誤差の推定値を決定してもよい。このようにして決定された推定値は、複数のカルマンフィルタの仮想オフセット量を尤度に応じて統合したものとなる。【００１２】
（構成３）
上記構成１又は２において、前記姿勢推定部は、前記複数のカルマンフィルタの各々から得られた前記尤度及び、前記複数のカルマンフィルタの各々から得られた前記移動体のロール角に基づいて、前記移動体のロール角の推定値を算出してもよい。例えば、前記複数のカルマンフィルタの各々から得られた前記尤度に基づいて、前記複数のカルマンフィルタの各々から得られた前記移動体のロール角の重み付けをすることで、前記移動体のロール角を推定することができる。すなわち、各カルマンフィルタにおけるロール角の推定値に、各カルマンフィルタの尤度に応じた重み付けをした値を用いて、ロール角の推定値を決定してもよい。このようにして決定された推定値は、複数のカルマンフィルタのロール角の推定値を尤度に応じて統合したものとなる。【００１３】
（構成４）
上記構成１〜３のいずいれかにおいて、姿勢推定部が備える複数のカルマンフィルタの各々は、    前記仮想オフセット量として、前記第一オフセット誤差の対象となる検出部である対象検出部のオフセット量が取り得る最大値よりも大きい値が与えられた第一カルマンフィルタと、
前記仮想オフセット量として、前記対象検出部のオフセット量が取り得る最小値よりも小さい値が与えられた第二カルマンフィルタと、
前記仮想オフセット量として、前記対象検出部のオフセット量が取り得る前記最小値と前記最大値の間の値が与えられた第三カルマンフィルタとを含む構成とすることができる。【００１４】
上記の構成４によれば、対象検出部が最も大きなオフセット誤差を発生させている場合も想定して、第一オフセット誤差を推定することができる。これにより、対象検出部に対応した検出部のオフセット誤差及び尤度の推定精度が高められる。この結果、移動体のロール角の推定精度が高められる。【００１５】
なお、姿勢推定部は、４つ以上のカルマンフィルタを備えるものとしても構わない。このとき、各カルマンフィルタに与えられた仮想オフセット量を異ならせることで、対象検出部に対応した検出部のオフセット誤差及び尤度の推定精度を更に高めることができる。【００１６】
（構成５）
上記構成１又は２において、前記第一オフセット誤差は、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの一つのオフセット誤差とすることができる。【００１７】
（構成６）
上記構成１〜５のいずれかにおいて、前記複数のカルマンフィルタの各々は、前記移動体のロール角、及び前記尤度に加えて、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの、前記対象検出部とは別の検出部のオフセット誤差ある第二オフセット誤差を算出してもよい。この場合、前記カルマンフィルタの各々は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作による前記第二オフセット誤差の推定値、及び前記仮想オフセット量を用いて、前記移動体のロール角の推定値、前記第二オフセット誤差の推定値及び前記尤度を算出することができる。
姿勢推定部は、前記複数のカルマンフィルタの各々から得られた前記尤度及び、前記複数のカルマンフィルタの各々から得られた前記第二オフセット誤差に基づいて、前記第二オフセット誤差を推定するものとしても構わない。【００１８】
上記の構成６によれば、対象検出部とは別の検出部のオフセット誤差（第二オフセット誤差）を、高精度に推定することができる。姿勢推定部は、この第二オフセット誤差も考慮して移動体のロール角を推定することができるため、推定精度が更に高められる。【００１９】
（構成７）
この第二オフセット誤差の対象となる検出部は、一つ以上の検出部とすることができる。すなわち、上記構成６において、前記第二オフセット誤差は、前記対象検出部とは別の検出部のオフセット誤差であって、前記第一角速度検出部、前記第二角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差であるものとすることができる。【００２０】
（構成８）
上記構成７において、前記第二オフセット誤差は、前記第一角速度検出部、及び前記第二角速度検出部のオフセット誤差を含むものとすることができる。【００２１】
（構成９）
上記構成８において、前記第二オフセット誤差は、前記対象検出部とは別の検出部のオフセット誤差であって、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの一つのオフセット誤差を含むものとすることができる。【００２２】
（構成１０）
上記構成９において、
前記第一角速度検出部は、前記移動体のロール角速度を検出し、
前記第二角速度検出部は、前記移動体のヨー角速度を検出し、
前記第二オフセット誤差は、前記第一角速度検出部のオフセット誤差、前記第二角速度検出部のオフセット誤差、及び、前記第一加速度検出部のオフセット誤差又は前記第二加速度検出部のオフセット誤差であって前記第一オフセット誤差とは異なるオフセット誤差であるものとすることができる。【００２３】
（構成１１）
一例として、上記構成１〜８のいずれかにおいて、
前記第一加速度検出部は、前記移動体の上下方向における加速度を検出し、
前記第二加速度検出部は、前記移動体の左右方向における加速度を検出し、
前記第三加速度検出部は、前記移動体の前後方向における加速度を検出し、
前記第一オフセット誤差は、前記第一加速度検出部のオフセット誤差、又は前記第二加速度検出部のオフセット誤差であるものとすることができる。【００２４】
（構成１２）
上記構成６〜１０のいずれかにおいて、前記姿勢推定装置は、
前記移動体の、前記第一軸及び前記第二軸とは異なる第三軸の周りでの角速度である第三角速度を検出する第三角速度検出部を含んでもよい。
前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、前記仮想オフセット量、及び前回の推定動作によるオフセット誤差の推定値を用いて、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出してもよい。姿勢推定部は、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタのそれぞれに入力された前記仮想オフセット量に基づいて、前記第一オフセット量を推定し、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタの各々から得られた前記移動体のロール角に基づいて、前記移動体のロール角を推定し、
前記複数のカルマンフィルタの各々から得られた前記尤度、及び前記複数のカルマンフィルタの各々から得られた前記移動体のピッチ角に基づいて、前記移動体のピッチ角を推定する構成としても構わない。【００２５】
上記の構成１２によれば、移動体の、３方向に係る角速度を検出する検出部、及び３方向に係る加速度を検出する検出部を備える。すなわち、姿勢推定装置は６軸に係る値を検出することができる。そして、この検出された値に基づいて、移動体のロール角及びピッチ角に加えて、少なくとも一つの検出部のオフセット誤差が推定される。そして、この推定されたオフセット誤差が考慮された状態で、移動体のロール角及びピッチ角、及びオフセット誤差が推定される。これにより、移動体のロール角及びピッチ角の推定精度が向上する。【００２６】
（構成１３）
上記構成１２において、前記複数のカルマンフィルタは、それぞれ、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、前記仮想オフセット量、及び前回の推定動作による第二オフセット誤差の推定値を要素とする所定の関数の値が定数になる特性方程式を用いて、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出してもよい。【００２７】
上記構成１３によれば、特性方程式を用いることで、新たに一つの検出部のオフセット誤差を推定することができる。これにより、移動体のロール角及びピッチ角の推定精度が向上する。上記構成１３において、特性方程式としては、例えば、前記移動体の、前記第一軸の周りの回転運動方程式から導出された方程式を利用することができる。【００２８】
（構成１４）
上記構成１２又は１３において、前記姿勢推定装置は、
前記移動体に備えられた前輪及び後輪の少なくとも一方にかかる荷重を推定する荷重推定部を備え、
前記荷重推定部は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、並びに、前記姿勢推定部によって推定された前記移動体のロール角、前記移動体のピッチ角、及び前記第一オフセット誤差に基づいて前記荷重を推定するものとしても構わない。【００２９】
（構成１５）
上記構成１４において、前記姿勢推定装置は、
前記移動体の前輪又は後輪の少なくとも一方に設けられたサスペンションのストローク量を推定するサスペンションストローク量推定部を備え、
前記荷重推定部は、前記移動体に設けられた前記前輪及び前記後輪の双方にかかる荷重を推定し、
前記サスペンションストローク量推定部は、前記荷重推定部によって推定された、前記前輪及び前記後輪の双方にかかる荷重の推定値に基づいて、前記サスペンションのストローク量を推定するものとしても構わない。【００３０】
（構成１６）
上記構成１５において、前記姿勢推定装置は、
前記サスペンションストローク量推定部により推定された、前記サスペンションのストローク量の推定値に基づいて、前記移動体に固定された車体座標系の軸と、前記前輪又は前記後輪が接触している路面上に固定された路面座標系の軸との角度である仰俯角を推定する仰俯角推定部を備えるものとしても構わない。【００３１】
上記構成１６によれば、移動体の仰俯角を推定することができるため、更に精度よく移動体の姿勢が推定される。
（構成１７）
上記構成１６において、前記複数のカルマンフィルタは、前記仰俯角推定部により推定された前記仰俯角の推定値を考慮して、前記移動体のロール角の推定値、前記移動体のピッチ角の推定値、及び前記尤度を算出するものとしても構わない。【００３２】
（構成１８）
上記１６又は１７において、前記姿勢推定装置は、
前記仰俯角推定部により推定された前記仰俯角の推定値と、前記姿勢推定部により推定された前記移動体のロール角及びピッチ角の推定値とに基づいて、前記移動体が設置されている路面の縦断勾配を推定する勾配推定部を備えるものとしても構わない。【００３３】
上記構成によれば、路面の勾配を精度よく推定することができる。【００３４】
（構成１９）
前記移動体と、上記構成１〜１８のいずれかの前記姿勢推定装置とを備えた輸送機器も本発明の実施形態に含まれる。
【発明の効果】
【００３５】
本発明によれば、移動体の姿勢の推定精度を向上させることができる。
【図面の簡単な説明】
【００３６】
【図１】慣性座標系の基底ベクトルｅo、車両座標系の基底ベクトルｅb、及び路面座標系の基底ベクトルｅrの関係を示す図面である。
【図２】実施形態に係る姿勢推定装置を備えた車両の模式図である。
【図３】姿勢推定装置の構成の一例を模式的に示すブロック図である。
【図４】演算部の構成の一例を模式的に示すブロック図である。
【図５】センサ群の取り付け位置を説明するための図である。
【図６】後輪の模式的な断面図である。
【図７】センサ群の取り付け位置をベクトルで表した図である。
【図８】演算部の構成の一例を模式的に示すブロック図である。
【図９Ａ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｂ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｃ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｄ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｅ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｆ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｇ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｈ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｉ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｊ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｋ】各パラメータの推定値の時間遷移を示す図である。
【図９Ｌ】各パラメータの推定値の時間遷移を示す図である。
【図１０】姿勢推定装置の構成の一例を模式的に示すブロック図である。
【図１１】仰俯角の説明をするための図である。
【図１２】演算部の構成の一例を模式的に示すブロック図である。
【図１３】前後輪の路面接地点にかかる力のベクトルを表した図である。
【図１４】キャスター角及びリヤアーム角を説明するための図である。
【図１５】δRとδWの関係の一例を模式的に示したグラフである。
【図１６】サスペンションの緩衝装置をモデル化した等価回路である。
【図１７】姿勢推定装置の構成の一例を模式的に示すブロック図である。
【図１８Ａ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｂ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｃ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｄ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｅ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｆ】各パラメータの推定値の時間遷移を示す図である。
【図１８Ｇ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ａ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｂ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｃ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｄ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｅ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｆ】各パラメータの推定値の時間遷移を示す図である。
【図１９Ｇ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ａ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｂ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｃ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｄ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｅ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｆ】各パラメータの推定値の時間遷移を示す図である。
【図２０Ｇ】各パラメータの推定値の時間遷移を示す図である。
【図２１Ａ】各パラメータの推定値の時間遷移を示す図である。
【図２１Ｂ】各パラメータの推定値の時間遷移を示す図である。
【図２２】実施形態に係る姿勢推定装置を備えた車両の模式図である。
【発明を実施するための形態】
【００３７】
本発明の実施形態につき、図面を参照して説明する。なお、以下の各図において、図面の寸法比と実際の寸法比は必ずしも一致しない。
【００３８】
〈定義〉
以下、図面を参照して座標軸の定義を行う。図１は、慣性座標系の基底ベクトルｅo、車両座標系の基底ベクトルｅb、及び路面座標系の基底ベクトルｅrの関係を示す図面である。なお、以下の数式において、ベクトルを太字で表記することがある。いずれの座標系共に、Ｘ軸、Ｙ軸、及びＺ軸を有する。なお、図１では、車両座標系、路面座標系を構成する基準となる車両１００が自動二輪車であるものとして説明をしている。
【００３９】
【数１】
2017175844.tif 000003
【００４０】
慣性座標系ｅoは、地球の水平面に固定された座標系であり、Ｚ軸を鉛直上向きと定義する。
【００４１】
車両座標系ｅbは、水平路面上に車両が直立している状態において、水平面にＸ軸及びＹ軸が存在し、Ｘ軸が車両の前方向となるように車体に固定された座標系である。車両座標系ｅbは、車体のサスペンションが移動することによって、慣性座標系ｅoとの間のなす角度が変化する。サスペンションは、車輪（２，３）と車体１の間に設けられた緩衝装置である。
【００４２】
路面座標系ｅrは、Ｙ軸を車両座標系ｅbのＹ軸と共通とし、後輪３と路面２００との接地点Ｐ３から、前輪２と路面２００との接地点Ｐ２の向きにＸ軸が一致するように、Ｙ軸の周りに車両座標系ｅbを回転させた座標系である。
【００４３】
本明細書において、「ヨー角」とは、慣性座標系ｅoのＺ軸（ｅoz）の軸周りの回転角を表し、「ヨー角速度」とは、上記「ヨー角」の時間変化率を表し、「ヨー角加速度」とはヨー角速度の時間変化率を表す。また、本明細書において、「ピッチ角」とは、慣性座標系ｅoのＹ（ｅoy）軸の軸周りの回転角を表し、「ピッチ角速度」とは、上記「ピッチ角」の時間変化率を表し、「ピッチ角加速度」とはピッチ角速度の時間変化率を表す。また、本明細書において、「ロール角」とは、車両座標系ｅbのＸ軸（ｅbx）の軸周りの回転角を表し、「ロール角速度」とは、上記「ロール角」の時間変化率を表し、「ロール角加速度」とはロール角速度の時間変化率を表す。
【００４４】
本明細書において、「上下方向」とは車両座標系ｅbのＺ軸（ｅbz）方向を指し、「前後方向」とは車両座標系ｅbのＸ軸（ｅbx）方向を指し、「左右方向」とは車両座標系ｅbのＹ軸（ｅby）方向を指す。
【００４５】
ここで、ロール角、ロール角速度、ロール角加速度、ヨー角、ヨー角速度、ヨー角加速度、ピッチ角、ピッチ角速度、ピッチ角加速度、上下加速度、前後加速度、及び左右加速度を、それぞれ以下の符号で記載する。なお、各パラメータを表す符号上の１つのドットは１階時間微分を意味する。
【００４６】
【表１】
2017175844.tif 000004
【００４７】
［第一実施形態］
本発明の第一実施形態につき、説明する。
【００４８】
〈車両〉
図２は、本実施形態に係る姿勢推定装置を備えた車両の模式図である。図２に示される車両１００は自動二輪車である。
【００４９】
図２に示すように、車両１００は車体１を備える。車体１の前部には前輪２が取り付けられており、車体１の後部には後輪３が取り付けられている。また、車体１の中央部には、センサ群５が取り付けられている。センサ群５の詳細については、後述する。
【００５０】
後輪３のホイールには、後輪３の回転速度を検出する後輪速度センサ７が取り付けられている。本実施形態では、後輪速度センサ７が「速度情報検出部」に対応する。
【００５１】
車体１の前側の上部には、ハンドル１１が左右に揺動可能に設けられている。なお、図１に示す例では、ハンドル１１の近傍にナビゲーションシステム１２が設けられており、車体１の前部にはヘッドライト１４及びヘッドライト駆動装置１５が設けられている。ヘッドライト駆動装置１５はヘッドライト１４の向きを制御する。車体１の後部には、電子制御ユニット２０が設けられている。以下では、電子制御ユニット２０を、適宜「ＥＣＵ２０」と略記する。なお、ＥＣＵ２０やセンサ群５の配置位置は、図２の態様に限られない。
【００５２】
センサ群５及び後輪速度センサ７の出力信号は、ＥＣＵ２０に与えられる。ＥＣＵ２０は、車体１の各部を制御すると共に、車体１のロール角等を推定し、これらの推定値を、例えばナビゲーションシステム１２やヘッドライト駆動装置１５に出力する。
【００５３】
本実施形態では、センサ群５、後輪速度センサ７、及びＥＣＵ２０が姿勢推定装置を構成する。
【００５４】
〈姿勢推定装置の構成〉
図３は、姿勢推定装置の構成の一例を模式的に示すブロック図である。図３に示す姿勢推定装置１０は、センサ群５、後輪速度センサ７、ローパスフィルタ（３１，３２，３４，３５，３６）、微分器（４１，４２）、及び演算部４９を備える。演算部４９の機能は、図２に示すＥＣＵ２０及びプログラムによって実現される。演算部４９が「姿勢推定部」に対応する。
【００５５】
センサ群５は、ロール角速度センサ２１、ヨー角速度センサ２２、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６を含む。
【００５６】
ロール角速度センサ２１は、車体１のロール角速度を検出するように車体１に設けられている。ヨー角速度センサ２２は、車体１のヨー角速度を検出するように車体１に設けられている。本実施形態では、ロール角速度センサ２１が「第一角速度検出部」に対応し、ヨー角速度センサ２２が「第二角速度検出部」に対応する。
【００５７】
上下加速度センサ２４は、車体１の上下方向における加速度を検出するように車体１に設けられている。前後加速度センサ２５は、車体１の前後方向における加速度を検出するように車体１に設けられている。左右加速度センサ２６は、車体１の左右方向における加速度を検出するように車体１に設けられている。本実施形態では、上下加速度センサ２４が「第一加速度検出部」に対応し、前後加速度センサ２５が「第二加速度検出部」に対応し、左右加速度センサ２６が「第三加速度検出部」に対応する。
【００５８】
なお、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６は、一例として、互いに直交する方向における加速度を検出している。これは必須ではなく、少なくとも、それぞれ、互いに異なる３つの方向における加速度を検出することができればよい。
【００５９】
ロール角速度センサ２１の出力信号は、ローパスフィルタ３１を通してロール角速度として演算部４９及び微分器４１に与えられる。ローパスフィルタ３１は、ロール角速度センサ２１の出力信号のノイズを除去する。微分器４１は、ロール角速度の微分値をロール角加速度として演算部４９に与える。
【００６０】
ヨー角速度センサ２２の出力信号は、ローパスフィルタ３２を通してヨー角速度として演算部４９及び微分器４２に与えられる。ローパスフィルタ３２は、ヨー角速度センサ２２の出力信号のノイズを除去する。微分器４２は、ヨー角速度の微分値をヨー角加速度として演算部４９に与える。
【００６１】
上下加速度センサ２４の出力信号は、ローパスフィルタ３４を通して上下加速度として演算部４９に与えられる。前後加速度センサ２５の出力信号は、ローパスフィルタ３５を通して前後加速度として演算部４９に与えられる。左右加速度センサ２６の出力信号は、ローパスフィルタ３６を通して左右加速度として演算部４９に与えられる。
【００６２】
ローパスフィルタ（３１，３２，３４，３５，３６）の各々の周波数特性は、対応するセンサ（２１，２２，２４，２５，２６）の出力特性に応じて設定される。より具体的には、センサ（２１，２２，２４，２５，２６）の出力信号に含まれるノイズ信号の周波数特性は、設計段階において予め特定することが可能である。ローパスフィルタ（３１，３２，３４，３５，３６）はそのようなノイズ信号を遮断し、必要とされるセンサ（２１，２２，２４，２５，２６）の検出信号を通過させるように設計され得る。
【００６３】
後輪速度センサ７の出力信号は、後輪速度として演算部４９に与えられる。後輪速度は、路面と後輪３のタイヤとの間に滑りが生じないと仮定した場合のタイヤの最外周の回転速度であり、実際には後輪速度センサ７の出力信号およびタイヤのサイズに基づいて算出される。説明を簡略化するために、図３では、後輪速度センサ７から後輪速度を示す信号が出力されるものとして図示している。
【００６４】
演算部４９は、ロール角速度、ロール角加速度、ヨー加速度、ヨー角加速度、上下加速度、前後加速度、左右加速度、及び後輪速度に関する検出値が入力される。そして、演算部４９は、これらの値に基づいて、ロール角、前後方向車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット、左右加速度センサオフセット、及び上下加速度センサオフセットを推定して出力する。
【００６５】
ロール角速度センサオフセットは、ロール角速度センサ２１のオフセット誤差である。ヨー角速度センサオフセットは、ヨー角速度センサ２２のオフセット誤差である。左右加速度センサオフセットは、左右加速度センサ２６のオフセット誤差である。上下加速度センサオフセットは、上下加速度センサ２４のオフセット誤差である。
【００６６】
後輪速度センサ７から検出された後輪速度、演算部４９が推定した前後方向車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット、左右加速度センサオフセット、及び上下加速度センサオフセットを、以下の符号で表記するものとする。
【００６７】
【表２】
2017175844.tif 000005
【００６８】
〈カルマンフィルタの構成〉
図４は、演算部４９の構成の一例を模式的に示すブロック図である。図４に示すように、演算部４９は、複数のカルマンフィルタ（50_1、50_2、50_3）と、状態量決定部５０３とを備えている。なお、以下において、カルマンフィルタ（50_1、50_2、50_3）を総称して、「カルマンフィルタ５０」と称することがある。
【００６９】
なお、図４では、演算部４９が備えるカルマンフィルタ５０の数が３つの場合につき示しているが、この数は一例である。演算部４９が備えるカルマンフィルタ５０の数は２つでも構わないし、４つ以上でも構わない。
【００７０】
カルマンフィルタ５０（50_1、50_2、50_3）では、以下に説明する車両１００の運動学モデルが用いられる。
【００７１】
図４において、カルマンフィルタ５０（50_1、50_2、50_3）は、システム方程式演算部５１、観測方程式演算部５２、減算部５３、加算部５４、積分部５５、カルマンゲイン演算部５６、尤度演算部５７、ローパスフィルタ５８、及び仮想オフセット入力部５９を含んで構成される。
【００７２】
システム方程式演算部５１で演算される対象となる方程式であるシステム方程式は、関数ｆ（ｘ，ｕ）を含む。観測方程式演算部５２で演算される対象となる方程式である観測方程式は、関数ｈ（ｘ，ｕ）を含む。ガルマンゲイン演算部５６は、５次のカルマンゲインＫを含む。
【００７３】
仮想オフセット入力部５９は、センサ群５を構成する所定の一のセンサのオフセット値として、カルマンフィルタ５０（50_1、50_2、50_3）ごとに予め定められた値を、観測方程式演算部５２に対して入力する。本実施形態では、仮想オフセット入力部５９は、左右加速度センサ２６のオフセット値ｂyを観測方程式演算部５２に入力する構成とする。このとき、左右加速度センサ２６が「対象検出部」に対応する。
【００７４】
図４では、カルマンフィルタ50_1が備える仮想オフセット入力部５９が、観測方程式演算部５２に対して入力する左右加速度センサ２６のオフセット値をｂy1と表記している。同様に、カルマンフィルタ50_2が備える仮想オフセット入力部５９が、観測方程式演算部５２に対して入力する左右加速度センサ２６のオフセット値をｂy2と表記している。カルマンフィルタ50_3が備える仮想オフセット入力部５９が、観測方程式演算部５２に対して入力する左右加速度センサ２６のオフセット値をｂy3と表記している。
【００７５】
尤度演算部５７は、各カルマンフィルタ５０（50_1、50_2、50_3）が推定した結果の確からしさを表す指標である尤度を演算によって算定する。ローパスフィルタ５８は、尤度演算部５７で算定された値をフィルタリングする演算部である。なお、同様の機能が実現できれば、ローパスフィルタ５８に代えて他の演算部で代用しても構わない。また、ローパスフィルタ５８を備えるか否かは任意である。尤度演算部５７で行われる演算例については後述される。
【００７６】
カルマンフィルタ５０を構成する各演算部（５１，５２，５３，５４，５５，５６，５７，５８）は、例えば、予め用意されたプログラムをＥＣＵ２０が実行することによって実現される。ただし、各演算部（５１，５２，５３，５４，５５，５６，５７，５８）の一部又は全部を、独立したハードウェアとして実装しても構わない。
【００７７】
現在の推定動作において、システム方程式に含まれる関数ｆ（ｘ・BR>Cｕ）の入力パラメータｕとして、ロール角速度ωroの検出値、ロール角加速度（ロール角速度ωroの微分値）の検出値、ヨー角速度ωyaの検出値、ヨー角加速度（ヨー角速度ωyaの微分値）、及び前後加速度Ｇxの検出値が与えられる。
【００７８】
また、システム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｘとして、前回の推定動作による、ロール角φの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、及び上下加速度センサオフセットｂzの推定値が与えられる。
【００７９】
システム方程式演算部５１の出力は、ロール角φの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、及び上下加速度センサオフセットｂzの微分予測値である。
【００８０】
加算部５４では、ロール角φの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、及び上下加速度センサオフセットｂzの微分予測値に対して、前回の推定動作により得られた５次のカルマンゲインＫが加算される。
【００８１】
カルマンゲインＫが加算された、ロール角φの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、及び上下加速度センサオフセットｂzの微分予測値が、積分部５５において積分される。これにより、現在の推定動作による、ロール角φの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、及び上下加速度センサオフセットｂzの推定値が得られる。
【００８２】
観測方程式に含まれる関数ｈ（ｘ，ｕ）の入力パラメータｘとして、ロール角φの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、及び上下加速度センサオフセットｂzの推定値が与えられる。また、上述したように、観測方程式に含まれる関数ｈ（ｘ，ｕ）の入力パラメータｘとして、仮想オフセット入力部５９から入力された左右加速度センサ２６のオフセット値が与えられる。
【００８３】
観測方程式演算部５２の出力は、上下加速度Ｇzの算出値、左右加速度Ｇyの算出値、及び後輪速度ｖrの算出値である。
【００８４】
カルマンフィルタ５０には、入力パラメータｙとして上下加速度Ｇzの検出値、左右加速度Ｇyの検出値、及び後輪速度ｖrの検出値が与えられる。カルマンゲイン演算部５６では、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの検出値と、各算出値との差（以下ではこの差を「観測予測誤差ｅ」と呼ぶことがある。）に基づいてカルマンゲインＫが算出される。
【００８５】
この運動学モデルのシステム方程式ｆ（ｘ，ｕ）、及び観測方程式ｈ（ｘ，ｕ）を導出することにより、入力パラメータｕ，ｙと出力パラメータｘとの関係式を導くことができる。
【００８６】
〈システム方程式及び観測方程式の導出〉
本実施形態では、運動学モデルを簡単化するために以下の点を仮定する。
【００８７】
（ａ）後輪３と路面との間に回転方向の滑りは生じない。
（ｂ）後輪３の横滑り速度は０である。
（ｃ）路面は平坦である。なお、ここでいう「平坦」とは凹凸がなく平面であるという意味であり、傾斜していてもよい。
（ｄ）車両１００はピッチングしない。
【００８８】
これらの仮定（ａ）〜（ｄ）に基づいて以下のようにして運動学モデル式を導出する。なお、ヨー角の微分値、ピッチ角の微分値、及びロール角の微分値をそれぞれ以下の符号で記載する。
【００８９】
【表３】
2017175844.tif 000006
【００９０】
まず、一般的なオイラー角と角速度との関係式から、次式が成立する。
【００９１】
【数２】
2017175844.tif 000007
【００９２】
上記の仮定（ｄ）からピッチ角θ及びその微分値は０になる。従って、上記（１）式は、次のようになる。
【００９３】
【数３】
2017175844.tif 000008
【００９４】
上記（２）式の２行目より、ピッチ角速度ωpiを削除することができる。これにより、次式が求まる。
【００９５】
【数４】
2017175844.tif 000009
【００９６】
図５はセンサ群５の取り付け位置を説明するための図である。図５において（ａ）に車両１００の左側面を示し、（ｂ）に車両１００の正面を示す。図６は、後輪３の模式的な断面図である。図７はセンサ群５の取り付け位置をベクトルで表した図である。
【００９７】
図５において、センサ群５の取り付け位置をＰＳとする。取り付け位置ＰＳから後輪３の中心までの水平距離をＬとし、路面２００から取り付け位置ＰＳまでの高さをｈとする。
【００９８】
以下、慣性座標系の原点Ｏを基準とするセンサ群５の取り付け位置ＰＳの位置ベクトル、慣性座標系の原点Ｏを基準とする後輪３の接地点Ｐ３の位置ベクトル、後輪３の接地点Ｐ３からセンサ群５の取り付け位置ＰＳまでのベクトルを、並びに各ベクトルの２階微分ベクトルをそれぞれ以下の符号で示す。なお、各パラメータを表す符号の上の２つのドットは、２階時間微分を意味する。
【００９９】
【表４】
2017175844.tif 000010
【０１００】
このとき、以下の（４）式が成立する。
【０１０１】
【数５】
2017175844.tif 000011
【０１０２】
ここで、重力加速度ベクトル、及び取り付け位置ＰＳにおいて検出される加速度ベクトルを、それぞれ以下の符号で表す。この加速度ベクトルＧは、取り付け位置ＰＳに設置されている上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６により検出される。
【０１０３】
【表５】
2017175844.tif 000012
【０１０４】
加速度ベクトルＧは、以下の（５）式によって得られる。
【０１０５】
【数６】
2017175844.tif 000013
【０１０６】
以下、上記（５）式の右辺を算出する。図７に示すベクトルρは、次式で表される。
【０１０７】
【数７】
2017175844.tif 000014
【０１０８】
上述したように、ベクトルｅbは車両座標系の基底ベクトルであり、ｅbxは車体１の前方向成分、ｅbyは車体１の左方向成分、ｅbzは車体１の上方向成分に対応する。また、（６）式の右辺に示す行列ρは、図６及び図７より、次式で表される。なお、図７内において、後輪３の断面の半径をＲcrとし、後輪３の半径をＲeとしている。また、上述したように、車体１のロール角をφで表している。
【０１０９】
【数８】
2017175844.tif 000015
【０１１０】
なお、車両１００のサスペンションの動きを考慮すると、厳密にはＬやｈは変化するが、その変化の値は、Ｌやｈの値に対して十分小さいので、Ｌやｈの値は一定であると近似することができる。
【０１１１】
上式（７）からベクトルρの２階微分ベクトルは次式のように求まる。
【０１１２】
【数９】
2017175844.tif 000016
【０１１３】
上式（８）において、ａx、ａy及びａzは関数である。関数ａx，ａy，ａzは上式（６）及び（７）を計算することにより求めることができる。上式（７）から上式（８）への変形の際に上式（１）を用いることにより、ロール角φの微分値、及びヨー角ψの微分値を消去することができる。
【０１１４】
次に、車両１００の横滑り速度をＶyとする。図７の位置ベクトルｒ0の１階微分ベクトルは、車両速度Ｖx及び横滑り速度Ｖyを用いて次式のように表される。なお、ベクトルｅoは慣性座標系の基底ベクトルである。
【０１１５】
【数１０】
2017175844.tif 000017
【０１１６】
仮定（ｂ）からＶｙ＝０とし、上式（９）を１階微分すると、位置ベクトルｒの２階微分ベクトルは次式のように表される。
【０１１７】
【数１１】
2017175844.tif 000018
【０１１８】
次に、重力加速度ベクトルは次式のように表される。なお、下記式（１１）の右辺内のｇは、重力加速度の大きさを表す。
【０１１９】
【数１２】
2017175844.tif 000019
【０１２０】
上式（５）、（８）、（１０）、及び（１１）より、取り付け位置ＰＳにおいて検出される加速度ベクトルＧは、以下の式のように表される。
【０１２１】
【数１３】
2017175844.tif 000020
【０１２２】
ここで、取り付け位置ＰＳで検出される加速度ベクトルＧは、前後加速度センサ２５により検出される前後加速度Ｇx、左右加速度センサ２６により検出される左右加速度Ｇy、及び上下加速度センサ２４により検出される上下加速度Ｇzを用いて次式のように表される。
【０１２３】
【数１４】
2017175844.tif 000021
【０１２４】
よって、上記（１２）及び（１３）式より、前後加速度Ｇx、左右加速度Ｇy、及び上下加速度Ｇzは、次式のように導かれる。
【０１２５】
【数１５】
2017175844.tif 000022
【０１２６】
次に、後輪速度ｖrと、車両速度Ｖxとの関係を求める。上記の仮定（ａ）から後輪３と路面２００との間に滑りがないので、後輪速度ｖrと、車両速度Ｖxとの間には次式の関係が成立する。
【０１２７】
【数１６】
2017175844.tif 000023
【０１２８】
上記（１５）式から、次式が求まる。
【０１２９】
【数１７】
2017175844.tif 000024
【０１３０】
上記（１）式、（１４）式、及び（１６）式から次式が求まる。
【０１３１】
【数１８】
2017175844.tif 000025
【０１３２】
【数１９】
2017175844.tif 000026
【０１３３】
カルマンフィルタ５０は、システム方程式演算部５１において上記（１７）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（１８）式を観測方程式として用いて演算を行う。
【０１３４】
更に、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、ピッチ角速度オフセットｂpi、上下加速度オフセットｂzが変化する場合でも、これらの値は車両１００の動きに比べて遅い。このため、ロール角速度オフセットｂroの微分値、ヨー角速度オフセットｂyaの微分値、ピッチ角速度オフセットｂpiの微分値、及び上下加速度オフセットｂzの微分値をいずれも０とみなすことができる。
【０１３５】
また、上記（１７）式、（１８）式において、オフセット誤差を反映させた値に置き換えることで、それぞれ以下の式を得る。
【０１３６】
【数２０】
2017175844.tif 000027
【０１３７】
【数２１】
2017175844.tif 000028
【０１３８】
各オフセット誤差を考慮する場合においては、カルマンフィルタ５０は、システム方程式演算部５１において上記（１９）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（２０）式を観測方程式として用いて演算を行う。これらの式（１９），（２０）では、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、及び上下加速度オフセットｂzが考慮されている。
【０１３９】
カルマンフィルタ５０が、上記（１９）式をシステム方程式とし、上記（２０）式を観測方程式として演算を行うことで、ロール角φ、車両速度Ｖx、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、及び上下加速度オフセットｂzを推定することができる。上記（１９）式の右辺が関数ｆ（ｘ，ｕ）に対応し、上記（２０）式の右辺が関数ｈ（ｘ，ｕ）に対応する。
【０１４０】
角速度センサでは、加速度センサに比べてオフセット誤差が発生しやすい。従って、本実施形態では、ロール角速度センサ２１のオフセット誤差（ロール角速度センサオフセットｂro）、及びヨー角速度センサ２２のオフセット誤差（ヨー角速度センサオフセットｂya）が推定される。ロール角速度オフセットｂro、及びヨー角速度オフセットｂyaの推定値を次の推定動作の際に用いることにより、ロール角φの推定精度が向上する。
【０１４１】
また、３つの加速度センサ（上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６）のオフセット誤差を推定することができれば、ロール角φの推定精度がさらに向上することが期待される。しかしながら、３つの加速度センサのオフセット誤差を推定する場合には、可観測性を保つことはできない。そこで、本実施の形態では、次の理由により上下加速度センサ２４のオフセット誤差（上下加速度センサオフセットｂz）を推定する。車体１のロール角φが小さい範囲では、車体１の上下加速度はほとんど変化しない。このような上下加速度の検出値が上下加速度センサオフセットｂｚにより変化した場合、ロール角φの推定の際に上下加速度の検出値の変化の影響が大きくなる。従って、上下加速度センサオフセットｂzの推定値を次の推定動作の際に用いることにより、ロール角φの小さい範囲での推定精度がさらに向上する。
【０１４２】
ところで、ロール角φの推定の際には、上下加速度の検出値の変化の影響に次いで、左右加速度の検出値の変化の影響を受けやすい。そこで、本実施形態の演算部４９では、図４を参照して上述したように、複数のカルマンフィルタ５０を用いることで、左右加速度センサオフセットｂyの影響を考慮している。具体的には、各カルマンフィルタ５０（50_1、50_2、50_3）の観測方程式演算部５２に対して、仮想オフセット入力部５９から左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）が入力される。
【０１４３】
本実施形態の観測方程式演算部５２は、上記（２０）式に代えて、下記式を観測方程式とした演算を行う。
【０１４４】
【数２２】
2017175844.tif 000029
【０１４５】
つまり、本実施形態のカルマンフィルタ５０は、システム方程式演算部５１において上記（１９）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（２１）式を観測方程式として用いて演算を行う。
【０１４６】
なお、上記（２１）式の第１行に記載されているｂyの値としては、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９から入力された左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）の値が採用される。
【０１４７】
観測方程式演算部５２は、この仮想オフセット入力部５９から入力された左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）を含む観測方程式に基づいて、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの推定値を算出する。この推定値が、尤度演算部５７に入力され、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさを示す尤度に関する値が算定される。
【０１４８】
ここで、仮想オフセット入力部５９から入力される、左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）の値のうち、最も大きい値については、左右加速度センサ２６が取り得るゼロ点オフセットの最大値よりも大きい値とし、最も小さい値については、左右加速度センサ２６が取り得るゼロ点オフセットの最小値よりも小さい値とするのが好ましい。これにより、観測方程式演算部５２は、左右加速度センサ２６のゼロ点オフセットの値が最も大きく振れた場合を考慮して、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの推定値を算出することができる。
【０１４９】
一例として、左右加速度センサ２６が取り得るゼロ点オフセットの値が±１００ｍＧの範囲で保証されている場合には、カルマンフィルタ50_1の仮想オフセット入力部５９から入力されるオフセット値ｂy1≧１００ｍＧ、カルマンフィルタ50_3の仮想オフセット入力部５９から入力されるオフセット値ｂy3≦−１００ｍＧとするものとしてよい。この場合おいて、カルマンフィルタ50_2の仮想オフセット入力部５９から入力されるオフセット値ｂy2＝０ｍＧとしても構わない。
【０１５０】
なお、上述したように、上述した実施形態では、演算部４９が３つのカルマンフィルタ５０（50_1、50_2、50_3）を備える場合を説明しているが、演算部４９が２つのカルマンフィルタ５０を備える場合や、４つ以上のカルマンフィルタ５０を備える場合においても、同様に左右加速度センサオフセットｂyの値を設定することができる。
【０１５１】
次に、尤度演算部５７が算出する尤度λについて説明する。上述したように、推定値と実際の検出値の差を観測予測誤差ｅで表すと、ｅは、下記式で定義される。
【０１５２】
【数２３】
2017175844.tif 000030
【０１５３】
なお、（２２）式において、ｙは入力パラメータとしてカルマンフィルタ５０から与えられた上下加速度Ｇzの検出値、左右加速度Ｇyの検出値、及び後輪速度ｖrの検出値である。また、ｈ（ｘ）は、観測方程式演算部５２において算出された算出結果である。
【０１５４】
ここで、上下加速度センサ２４が検出する上下加速度Ｇzの検出誤差をδＧz、左右加速度センサ２６が検出する左右加速度Ｇyの検出誤差をδＧy、後輪速度センサ７が検出する後輪速度ｖrの検出誤差をδｖrとすると、これらの検出誤差の分散共分散行列Ｒは下記式で表される。なお、下記式においてＥ（ａ）はａの期待値を表す。
【０１５５】
【数２４】
2017175844.tif 000031
【０１５６】
また、ロール角φの推定誤差をδφ、車両速度Ｖxの推定誤差をδＶx、ロール角速度センサオフセットｂroの推定誤差をδｂro、ヨー角速度センサオフセットｂyaの推定誤差をδｂya、上下加速度センサオフセットｂzの推定誤差をδｂzとすると、これらの検出誤差の分散共分散行列Ｐは下記式で表される。
【０１５７】
【数２５】
2017175844.tif 000032
【０１５８】
各カルマンフィルタ５０（50_1、50_2、50_3）では、上記行列Ｐ及び行列Ｒが逐次算出されている。
【０１５９】
このとき、観測予測誤差ｅに含まれる誤差の分散共分散行列Ｓは、下記式で表される。
【０１６０】
【数２６】
2017175844.tif 000033
【０１６１】
なお、上記式において上付きの添字ｔは、転置行列を表す。すなわちＨtは行列Ｈの転置行列を表す。また、Ｈはｈ（ｘ）のヤコビ行列であり、下記式で規定される行列である。
【０１６２】
【数２７】
2017175844.tif 000034
【０１６３】
尤度演算部５７は、例えば下記式に基づいて尤度λを算出する。この尤度λは、瞬間的な尤度であるため、瞬間尤度と記載することもできる。
【０１６４】
【数２８】
2017175844.tif 000035
【０１６５】
なお、尤度演算部５７は、観測予測誤差ｅ＝ｙ−ｈ（ｘ）に基づいて尤度を算出する構成であればよく、算出方法としては上記（２７）式に限定されない。
【０１６６】
カルマンフィルタ50_1において推定されたｘ（φ、Ｖx、ｂro、ｂya、ｂz）をｘ1とし、カルマンフィルタ50_1が備える尤度演算部５７で算出された尤度λをλ1とする。同様に、カルマンフィルタ50_2において推定されたｘ（φ、Ｖx、ｂro、ｂya、ｂz）をｘ2とし、カルマンフィルタ50_2が備える尤度演算部５７で算出された尤度λをλ2とする。同様に、カルマンフィルタ50_3において推定されたｘ（φ、Ｖx、ｂro、ｂya、ｂz）をｘ3とし、カルマンフィルタ50_3が備える尤度演算部５７で算出された尤度λをλ３とする。このとき、左右加速度センサオフセットｂyは、以下の式で算出される。
【０１６７】
【数２９】
2017175844.tif 000036
【０１６８】
上記（２８）式において、λの上にチルダ（〜）が付されてなる記号は、瞬間尤度λがローパスフィルタ５８によってフィルタリングされた後の値を意味する。なお、必ずしもフィルタリング処理が必要であるわけではない。フィルタリングを行わない場合には、瞬間尤度λの値を採用しても構わない。また、上記（２８）式において、ｎは、演算部４９が備えるカルマンフィルタ５０の数である。本実施形態ではｎ＝３である。
【０１６９】
各カルマンフィルタ５０は、仮想オフセット入力部５９から入力された左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）を考慮して推定処理を行っている。そして、この推定結果の確からしさを示す指標が尤度λ（λ1、λ2、λ3）として求められる。よって、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９に入力された、左右加速度センサオフセットｂyの値を、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさである尤度λ（λ1、λ2、λ3）に応じて重み付けすることによって、左右加速度センサオフセットｂyの推定値を求めることができる。
【０１７０】
また、同様の理屈により、下記式に示すように、各カルマンフィルタ５０において推定されたｘ（φ、Ｖx、ｂro、ｂya、ｂz）を、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさである尤度λ（λ1、λ2、λ3）に応じて重み付けすることによって、左右加速度センサオフセットｂyを考慮した推定結果を得ることができる。
【０１７１】
【数３０】
2017175844.tif 000037
【０１７２】
状態量決定部５０３は、上記式（２８）及び式（２９）に対応した演算を行う。これにより、演算部４９では、ロール角φ、車両速度Ｖx、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、上下加速度オフセットｂzに加えて、左右加速度センサオフセットｂyが推定される。また、各推定値は、左右加速度センサオフセットｂyをも考慮した推定結果であるため、従来よりも更に精度が向上する。
【０１７３】
なお、上記式（２８）、（２９）式では、尤度λの値に応じて比例配分する演算を行っているが、この演算内容は一例である。状態量決定部５０３は、尤度λの値に応じた重み付け処理をしていれば他の演算方法を採用しても構わない。
【０１７４】
〈別構成〉
本実施形態の姿勢推定装置１０は、以下のバリエーションを採用することが可能である。
【０１７５】
（１）  上述した実施形態では、状態量決定部５０３は、尤度λに応じて、各カルマンフィルタ５０に入力された左右加速度センサオフセットｂy、及び各カルマンフィルタで推定された推定値ｘを重み付け処理をした演算をするものとして説明した。しかし、各カルマンフィルタ５０が算出した尤度λに基づいて、最も確からしい一のカルマンフィルタ５０を選択し、当該カルマンフィルタ５０に入力された左右加速度センサオフセットｂyを、左右加速度センサオフセットの推定値とし、当該カルマンフィルタ５０の推定結果を演算部４９の推定結果としても構わない。この場合、状態量決定部５０３の演算処理内容は簡素化される。
【０１７６】
（２）  上述した実施形態では、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、左右加速度センサオフセットｂyを推定するものとした。これに代えて、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び左右加速度センサオフセットｂyを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、上下加速度センサオフセットｂzを推定するものとしても構わない。このとき、上下加速度センサ２４が「対象検出部」に対応する。
【０１７７】
この場合、演算部４９は図４に代えて図８に示すような構成となる。すなわち、仮想オフセット入力部５９は、上下加速度センサ２４のオフセット値ｂzを観測方程式演算部５２に入力する構成である。観測方程式演算部５２では、上記（２１）式を観測方程式として用いて演算が行われる。しかし、上記の実施形態と異なり、上記（２１）式の第２行に記載されているｂzの値として、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９から入力された上下加速度センサオフセットｂz（ｂz1、ｂz2、ｂz3）の値が採用される。観測方程式演算部５２は、この仮想オフセット入力部５９から入力された上下加速度センサオフセットｂz（ｂz1、ｂz2、ｂz3）を含む観測方程式に基づいて、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの推定値を算出する。この推定値が、尤度演算部５７に入力され、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさを示す尤度に関する値が算定される。
【０１７８】
この場合、状態量決定部５０３は、上記（２８）式に代えて、下記式（３０）に基づく演算を行うことで、上下加速度センサオフセットｂzを算出する。また、上記（２９）式によって推定値ｘを求めることで、ロール角φ、車両速度Ｖx、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、上下加速度オフセットｂz、及び左右加速度センサオフセットｂyを考慮した推定値が得られる。
【０１７９】
【数３１】
2017175844.tif 000038
【０１８０】
同様に、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、前後加速度センサオフセットｂxを推定するものとしても構わない。この場合は、前後加速度センサ２５が「対象検出部」に対応する。仮想オフセット入力部５９から入力された前後加速度センサ２５のオフセット値がシステム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｕとして与えられる。
【０１８１】
同様に、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び左右加速度センサオフセットｂyを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、前後加速度センサオフセットｂxを推定するものとしても構わない。この場合も、前後加速度センサ２５が「対象検出部」に対応する。仮想オフセット入力部５９から入力された前後加速度センサ２５のオフセット値がシステム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｕとして与えられる。
【０１８２】
〈各パラメータの推定値の算出〉
以下、本実施形態に係る姿勢推定装置を用いて精度よく車両１００の姿勢が推定できていることを、実施例を参照して説明する。
【０１８３】
図９Ａ〜図９Ｌは、所定の路面上に車両１００を走行させながら、姿勢推定装置によって車両１００の姿勢を推定した結果を示すグラフである。図９Ａ〜図９Ｌは、水平面と斜面とを有する路面上を縦横無尽に車両１００が走行した場合を模擬した結果である。各図は、オフセット誤差を有しないロール角速度センサ２１、ヨー角速度センサ２２、上下加速度センサ２４、前後加速度センサ２５、左右加速度センサ２６、及び後輪速度センサ７を取り付けた状態で、意図的に所定のセンサに対して所定のタイミングでオフセット誤差を生じさせた場合に、前記各センサの検出値に基づいて各パラメータの推定値を算出したときの結果である。なお、現実の状態を模擬する目的で、カルマンフィルタ５０に入力される各センサからの検出値には白色ノイズが重畳されている。
【０１８４】
図９Ａ〜図９Ｆの各図では、図４を参照して説明した本実施形態の演算部４９による推定結果が、実線に対応する。すなわち、図９Ａ〜図９Ｆで想定されている演算部４９は、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、左右加速度センサオフセットｂyを推定する構成である。
【０１８５】
図９Ａ及び図９Ｂは、所定のタイミングで左右加速度センサ２６に対して２５０ｍＧのオフセット誤差を重畳させたときの、各推定値の結果を示すグラフである。図９Ａ及び図９Ｂにおいて、点線は真値に対応し、破線は１つのカルマンフィルタ５０を用いて推定した値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。図４を参照して説明した本実施形態の演算部４９による推定結果が、実線に対応する。
【０１８６】
図９Ａによれば、左右加速度センサオフセットｂyの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。
【０１８７】
また、図９Ｂによれば、破線よりも実線の方が真値に近い推定結果が得られていることが分かる。これにより、複数のカルマンフィルタ５０を並列に構成し、尤度に基づいて追加的にオフセット誤差を推定することで、推定精度が向上することが分かる。
【０１８８】
図９Ｃ〜図９Ｆは、左右加速度センサ２６以外のセンサに対してオフセット誤差を発生させた場合の推定結果を示すグラフである。図９Ｃ〜図９Ｆにおいて、点線は真値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０１８９】
図９Ｃは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図９Ｄは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図９Ｅは、所定の時間経過後に上下加速度センサ２４に対してオフセットを２５０ｍＧ重畳させた場合の各推定値に対応する。
【０１９０】
また、図９Ｆは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、ロール角速度センサ２１にオフセットを１deg／秒重畳させ、ヨー角速度センサ２２にオフセットを１deg／秒重畳させ、上下加速度センサ２４に対してオフセットを２５０ｍＧ重畳させ、左右加速度センサ２６に対してオフセットを２５０ｍＧ重畳さた場合の結果である。
【０１９１】
図９Ｃ〜図９Ｆによれば、オフセットを重畳した直後においては他のセンサオフセットの推定値が一時的に変動している場合も見受けられるものの、時間経過と共に収束していることが見て取れる。よって、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉されることなく、精度よく推定できていることが確認される。
【０１９２】
図９Ｇ〜図９Ｌで想定されている演算部４９は、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、前後加速度センサオフセットｂxを推定する構成である。図９Ｇ〜図９Ｌでは、この演算部４９による推定結果を実線で示している。
【０１９３】
図９Ｇ及び図９Ｈは、所定のタイミングで前後加速度センサ２５に対して１００ｍＧのオフセット誤差を重畳させたときの、各推定値の結果を示すグラフである。図９Ｇ及び図９Ｈにおいて、点線は真値に対応し、破線は１つのカルマンフィルタ５０を用いて推定
した値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０１９４】
図９Ｇによれば、前後加速度センサオフセットｂxの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。
【０１９５】
また、図９Ｈによれば、破線よりも実線の方が真値に近い推定結果が得られていることが分かる。これにより、複数のカルマンフィルタ５０を並列に構成し、尤度に基づいて追加的にオフセット誤差を推定することで、推定精度が向上することが分かる。
【０１９６】
図９Ｉ〜図９Ｌは、前後加速度センサ２６以外のセンサに対してオフセット誤差を発生させた場合の推定結果を示すグラフである。図９Ｉ〜図９Ｌにおいて、点線は真値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０１９７】
図９Ｉは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図９Ｊは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図９Ｋは、所定の時間経過後に上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させた場合の各推定値に対応する。
【０１９８】
また、図９Ｌは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、ロール角速度センサ２１にオフセットを１deg／秒重畳させ、ヨー角速度センサ２２にオフセットを１deg／秒重畳させ、上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させ、前後加速度センサ２５に対してオフセットを１００ｍＧ重畳させた場合の結果である。
【０１９９】
図９Ｉ〜図９Ｌによれば、オフセットを重畳した直後においては他のセンサオフセットの推定値が一時的に変動している場合も見受けられるものの、時間経過と共に収束していることが見て取れる。よって、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉されることなく、精度よく推定できていることが確認される。
【０２００】
［第二実施形態］
本発明の第二実施形態につき、説明する。なお、車体１００については第一実施形態と共通するため、説明を割愛する。
【０２０１】
〈姿勢推定装置の構成〉
図１０は、本実施形態の姿勢推定装置の構成の一例を模式的に示すブロック図である。図１０に示す姿勢推定装置１０は、センサ群５、後輪速度センサ７、ローパスフィルタ（３１，３２，３３，３４，３５，３６）、微分器（４１，４２，４３，４５）、演算部４９、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０を備える。演算部４９、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０の機能は、図２に示すＥＣＵ２０がプログラムに従って動作することによって実現される。演算部４９が「姿勢推定部」に対応する。
【０２０２】
本実施形態において、センサ群５は、ロール角速度センサ２１、ヨー角速度センサ２２、ピッチ角速度センサ２３、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６を含む。すなわち、本実施形態では、第一実施形態と比較して、センサ群５は更にピッチ角速度センサ２３を備える。ピッチ角速度センサ２３は、車体１のピッチ角速度を検出するように車体１に設けられており、「第三角速度検出部」に対応する。
【０２０３】
ピッチ角速度センサ２３の出力信号は、ローパスフィルタ３３を通してピッチ角速度として演算部４９及び微分器４３に与えられる。ローパスフィルタ３３は、ピッチ角速度センサ２３の出力信号のノイズを除去する。微分器４３は、ピッチ角速度の微分値をピッチ角加速度として演算部４９に与える。
【０２０４】
ローパスフィルタ３３の周波数特性は、ピッチ角速度センサ２３の出力特性に応じて設定される。より具体的には、ピッチ角速度センサ２３の出力信号に含まれるノイズ信号の周波数特性は、設計段階において予め特定することが可能である。ローパスフィルタ３３はそのようなノイズ信号を遮断し、ピッチ角速度センサ２３の検出信号を通過させるように設計され得る。
【０２０５】
本実施形態において、演算部４９は、ロール角速度、ロール角加速度、ヨー加速度、ヨー角加速度、ピッチ角速度、ピッチ角加速度、上下加速度、前後加速度、左右加速度、及び後輪速度に関する検出値が入力される。そして、演算部４９は、これらの値に基づいて、ロール角、ピッチ角、前後方向車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット、ピッチ角速度センサオフセット、上下加速度センサオフセット、及び前後加速度センサオフセットを推定して出力する。ピッチ角速度センサオフセットは、ピッチ角速度センサ２３のオフセット誤差である。
【０２０６】
なお、本実施形態の姿勢推定装置１０は、仰俯角推定部８０を備え、仰俯角推定部８０による仰俯角ζの推定値も演算部４９に入力される構成である。
【０２０７】
演算部４９が推定したピッチ角速度センサオフセット、仰俯角推定部８０が推定した仰俯角、及び仰俯角の１階微分値を、以下の符号で表記するものとする。
【０２０８】
【表６】
2017175844.tif 000039
【０２０９】
図１１は、仰俯角ζの説明をするための図面である。仰俯角ζは、車両１００のサスペンションのストローク量が発生しているときの、路面座標系ｅrと車両座標系ｅbのなす角と定義される。ここでは、仰俯角ζを、車両座標系ｅbのＸ軸ｅbxと路面座標系のＸ軸ｅrxとのなす角度として説明する。
【０２１０】
〈カルマンフィルタの構成〉
図１２は、本実施形態における演算部４９の構成の一例を模式的に示すブロック図である。本実施形態でも、図４を参照して上述した第一実施形態と同様に、演算部４９は、複数のカルマンフィルタ（50_1、50_2、50_3）と、状態量決定部５０３とを備えている。図１２では、演算部４９が備えるカルマンフィルタ５０の数が３つの場合につき示しているが、この数は一例である。演算部４９が備えるカルマンフィルタ５０の数は２つでも構わないし、４つ以上でも構わない。
【０２１１】
カルマンフィルタ５０（50_1、50_2、50_3）では、以下に説明する車両１００の運動学モデルが用いられる。図４と同様に、図１２において、カルマンフィルタ５０（50_1、50_2、50_3）は、システム方程式演算部５１、観測方程式演算部５２、減算部５３、加算部５４、積分部５５、カルマンゲイン演算部５６、尤度演算部５７、ローパスフィルタ５８、及び仮想オフセット入力部５９を含んで構成される。
【０２１２】
システム方程式演算部５１で演算される対象となる方程式であるシステム方程式は、関数ｆ（ｘ，ｕ）を含む。観測方程式演算部５２で演算される対象となる方程式である観測方程式は、関数ｈ（ｘ，ｕ）を含む。ガルマンゲイン演算部５６は、７次のカルマンゲインＫを含む。
【０２１３】
仮想オフセット入力部５９は、センサ群５を構成する所定の一のセンサのオフセット値として、カルマンフィルタ５０（50_1、50_2、50_3）ごとに予め定められた値を、システム方程式演算部５１及び観測方程式演算部５２に対して入力する。本実施形態では、仮想オフセット入力部５９は、前後加速度センサ２５のオフセット値ｂxを、システム方程式演算部５１及び観測方程式演算部５２に入力する構成とする。このとき、前後加速度センサ２５が「対象検出部」に対応する。
【０２１４】
図１２では、カルマンフィルタ50_1が備える仮想オフセット入力部５９が、システム方程式演算部５１及び観測方程式演算部５２に対して入力する前後加速度センサ２５のオフセット値をｂx1と表記している。同様に、カルマンフィルタ50_2が備える仮想オフセット入力部５９が、システム方程式演算部５１及び観測方程式演算部５２に対して入力する前後加速度センサ２５のオフセット値をｂx2と表記している。カルマンフィルタ50_3が備える仮想オフセット入力部５９が、システム方程式演算部５１及び観測方程式演算部５２に対して入力する前後加速度センサ２５のオフセット値をｂx3と表記している。
【０２１５】
尤度演算部５７は、各カルマンフィルタ５０（50_1、50_2、50_3）が推定した結果の確からしさを表す指標である尤度を演算によって算定する。ローパスフィルタ５８は、尤度演算部５７で算定された値をフィルタリングする演算部である。なお、同様の機能が実現できれば、ローパスフィルタ５８に代えて他の演算部で代用しても構わない。また、ローパスフィルタ５８を備えるか否かは任意である。尤度演算部５７で行われる演算内容については後述される。
【０２１６】
カルマンフィルタ５０を構成する各演算部（５１，５２，５３，５４，５５，５６，５７，５８）は、例えば、予め用意されたプログラムをＥＣＵ２０が実行することによって実現される。ただし、各演算部（５１，５２，５３，５４，５５，５６，５７，５８）の一部又は全部を、独立したハードウェアとして実装しても構わない。
【０２１７】
本実施形態では、現在の推定動作において、システム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｕとして、ロール角速度ωroの検出値、ロール角加速度（ロール角速度ωroの微分値）の検出値、ヨー角速度ωyaの検出値、ヨー角加速度（ヨー角速度ωyaの微分値）、ピッチ角速度ωpiの検出値、ピッチ角加速度（ピッチ角速度ωpiの微分値）の検出値、及び前後加速度Ｇxの検出値が与えられる。
【０２１８】
また、システム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｘとして、前回の推定動作による、ロール角φの推定値、ピッチ角θの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、ピッチ角速度センサオフセットｂpiの推定値、及び上下加速度センサオフセットｂzの推定値が与えられる。更に、上述したように、システム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｕとして、仮想オフセット入力部５９から入力された前後加速度センサ２５のオフセット値が与えられる。
【０２１９】
システム方程式演算部５１の出力は、ロール角φの微分予測値、ピッチ角θの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、ピッチ角速度センサオフセットｂpiの微分予測値、及び上下加速度センサオフセットｂzの微分予測値である。
【０２２０】
加算部５４では、ロール角φの微分予測値、ピッチ角θの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、ピッチ角速度センサオフセットｂpiの微分予測値、及び上下加速度センサオフセットｂzの微分予測値に対して、前回の推定動作により得られた７次のカルマンゲインＫが加算される。
【０２２１】
カルマンゲインＫが加算された、ロール角φの微分予測値、ピッチ角θの微分予測値、車両速度Ｖxの微分予測値、ロール角速度センサオフセットｂroの微分予測値、ヨー角速度センサオフセットｂyaの微分予測値、ピッチ角速度センサオフセットｂpiの微分予測値、及び上下加速度センサオフセットｂzの微分予測値が、積分部５５において積分される。これにより、現在の推定動作による、ロール角φの推定値、ピッチ角θの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、ピッチ角速度センサオフセットｂpiの推定値、及び上下加速度センサオフセットｂzの推定値が得られる。
【０２２２】
観測方程式に含まれる関数ｈ（ｘ，ｕ）の入力パラメータｘとして、ロール角φの推定値、ピッチ角θの推定値、車両速度Ｖxの推定値、ロール角速度センサオフセットｂroの推定値、ヨー角速度センサオフセットｂyaの推定値、ピッチ角速度センサオフセットｂpiの推定値、及び上下加速度センサオフセットｂzの推定値が与えられる。また、上述したように、観測方程式に含まれる関数ｈ（ｘ，ｕ）の入力パラメータｘとして、仮想オフセット入力部５９から入力された前後加速度センサ２５のオフセット値が与えられる。
【０２２３】
観測方程式演算部５２の出力は、上下加速度Ｇzの算出値、左右加速度Ｇyの算出値、後輪速度ｖrの算出値、及び後述する所定の特性方程式の算出値である。
【０２２４】
カルマンフィルタ５０には、入力パラメータｙとして上下加速度Ｇzの検出値、左右加速度Ｇyの検出値、後輪速度ｖrの検出値、及び特性方程式の検出値としての定数が与えられる。カルマンゲイン演算部５６では、上下加速度Ｇz、左右加速度Ｇy、後輪速度ｖr、及び特性方程式の検出値と、各算出値との差に基づいてカルマンゲインＫが算出される。なお、後述では、この特性方程式の検出値を０として説明するが、時間に応じて変化しない定数であれば０には限られない。
【０２２５】
この運動学モデルのシステム方程式ｆ（ｘ，ｕ）、及び観測方程式ｈ（ｘ，ｕ）を導出することにより、入力パラメータｕ，ｙと出力パラメータｘとの関係式を導くことができる。
【０２２６】
〈システム方程式及び観測方程式の導出〉
本実施形態では、運動学モデルを簡単化するために以下の点を仮定する。
【０２２７】
（ａ）後輪３と路面との間に回転方向の滑りは生じない。
（ｂ）後輪３の横滑り速度は０である。
（ｃ）路面は平坦である。なお、ここでいう「平坦」とは凹凸がなく平面であるという意味であり、傾斜していてもよい。
【０２２８】
これらの仮定（ａ）〜（ｃ）に基づいて以下のようにして運動学モデル式を導出する。すなわち、第一実施形態と比較して、運動学モデル式の導出時に仮定（ｄ）を組み込まない点が異なる。
【０２２９】
上式（７）からベクトルρの２階微分ベクトルは次式のように求まる。なお、上式（８）と比較して、ベクトルρの２階微分ベクトルにピッチ角速度ωpi、及びこの１階微分値を考慮している点が異なる。
【０２３０】
【数３２】
2017175844.tif 000040
【０２３１】
上式（３１）において、ａx、ａy及びａzは関数である。関数ａx，ａy，ａzは上式（６）及び（７）を計算することにより求めることができる。上式（６）から上式（７）への変形の際に上式（１）を用いることにより、ロール角φの微分値、ヨー角ψの微分値、及びピッチ角θの微分値を消去することができる。
【０２３２】
次に、車両１００の横滑り速度をＶyとする。図７の位置ベクトルｒ0の１階微分ベクトルは、車両速度Ｖx及び横滑り速度Ｖyを用いて次式のように表される。なお、上述したように、ベクトルｅbは車両座標系の基底ベクトルである。なお、本実施形態では、路面２００の勾配やサスペンションのストロークに伴う仰俯角も推定に考慮するため、車両１００のＺ方向についても速度Ｖzが発生する。つまり、図７の位置ベクトルｒ0の１階微分ベクトルは、Ｖx、Ｖy、及びＶzを用いて次式のように表される。
【０２３３】
【数３３】
2017175844.tif 000041
【０２３４】
仮定（ｂ）からＶｙ＝０とし、上式（３２）を１階微分すると、位置ベクトルｒの２階微分ベクトルは次式のように表される。なお、本実施形態は、第一実施形態とは異なり、車両１００のピッチングを考慮に入れるため、上式（１０）とは異なる式となる。
【０２３５】
【数３４】
2017175844.tif 000042
【０２３６】
上述したように、仰俯角ζは、車両座標系ｅbのＸ軸と路面座標系ｅrのＸ軸のなす角度に対応する。この定義より、下記式が成立する。
【０２３７】
【数３５】
2017175844.tif 000043
【０２３８】
よって、上記（３３）式は、下記式（３５）に書き換えられる。
【０２３９】
【数３６】
2017175844.tif 000044
【０２４０】
次に、重力加速度ベクトルは次式のように表される。なお、下記式（３６）の右辺内のｇは、重力加速度の大きさを表す。
【０２４１】
【数３７】
2017175844.tif 000045
【０２４２】
上式（５）、（３１）、（３５）、及び（３６）より、取り付け位置ＰＳにおいて検出される加速度ベクトルＧは、以下の式のように表される。
【０２４３】
【数３８】
2017175844.tif 000046
【０２４４】
ここで、取り付け位置ＰＳで検出される加速度ベクトルＧは、前後加速度センサ２５により検出される前後加速度Ｇx、左右加速度センサ２６により検出される左右加速度Ｇy、及び上下加速度センサ２４により検出される上下加速度Ｇzを用いて、上述した（１３）式によって表される。よって、上記（１３）式及び（３７）式より、前後加速度Ｇx、左右加速度Ｇy、及び上下加速度Ｇzは、次式のように導かれる。
【０２４５】
【数３９】
2017175844.tif 000047
【０２４６】
次に、後輪速度ｖrと、車両速度Ｖxとの関係を求める。上記の仮定（ａ）から後輪３と路面２００との間に滑りがないので、後輪速度ｖrと、車両速度Ｖxとの間には次式の関係が成立する。
【０２４７】
【数４０】
2017175844.tif 000048
【０２４８】
上記（３９）式から、次式が求まる。
【０２４９】
【数４１】
2017175844.tif 000049
【０２５０】
上記（１）式、（３８）式、及び（４０）式から次式が求まる。
【０２５１】
【数４２】
2017175844.tif 000050
【０２５２】
ところで、図７に示すように、センサ群５の取り付け位置ＰＳから、車体１のメインボディの重心点ＭＰに向かうベクトルをＷとする。なお、この重心点ＭＰは、車体１のバネ下の重量を含まない重心点とする。車体１のバネ下の重量は、車体１における路面から力の伝達経路においてサスペンションより路面に近い部品の総重量である。サスペンションより路面に近い部品には、例えば、タイヤ（２，３）、ホイール、ブレーキ等が含まれる。上記ベクトルＷは、以下の式で表される。
【０２５３】
【数４３】
2017175844.tif 000051
【０２５４】
ここで、車体１の重量はほぼ左右対称に分布されるため、Ｗy＝０である。車体１のメインボディの質量をＭとし、慣性モーメントテンソルをＩとすると、基準点Ｏから見た車両１００の角運動量ベクトルＬは、以下のように算出される。なお、以下のベクトル演算式において、「・」は内積を表し、「×」は外積を表す。
【０２５５】
【数４４】
2017175844.tif 000052
【０２５６】
なお、上記（４３）式において、角速度ベクトルω及びテンソルＩは、それぞれ以下の式で定義される。
【０２５７】
【数４５】
2017175844.tif 000053
【０２５８】
【数４６】
2017175844.tif 000054
【０２５９】
なお、上記式（４５）において、円内にクロスが配置されている演算子は「テンソル積」を表している。
【０２６０】
上記（４３）式の両辺を時間微分することで、下記式を得る。
【０２６１】
【数４７】
2017175844.tif 000055
【０２６２】
（７）式から（３１）式への変形と同様の変形により、ベクトルＷの２階微分ベクトル、並びに、テンソルＩと角加速度ベクトルωドット（角速度ベクトルωの１階微分ベクトル）の内積は、それぞれ下記式で表される。
【０２６３】
【数４８】
2017175844.tif 000056
【０２６４】
【数４９】
2017175844.tif 000057
【０２６５】
上式（４７）内のｗx、ｗy及びｗz、並びに上式（４８）内のｑx、ｑy及びｑzは関数である。これらの関数は、（７）式から（３１）式への変形と同様の変形により求めることができる。
【０２６６】
次に、基準点Ｏから見た車両１００にかかる力のモーメントを計算する。図１３は、前後輪の路面接地点にかかる力のベクトルを、図７にならって表した図である。図１３に図示されるように、車両１００に作用する力として、重力ベクトル−Ｍｇ、空気抵抗や揚力等からなる空力ベクトルγ、前輪２の路面接地点Ｐ２にかかる力ベクトルＦＦ、及び後輪３の路面接地点Ｐ３にかかる力ベクトルＦＲが挙げられる。前輪２の路面接地点Ｐ２にかかる力ベクトルＦＦ、及び後輪３の路面接地点Ｐ３にかかる力ベクトルＦＲは、それぞれ下記式で表される。
【０２６７】
【数５０】
2017175844.tif 000058
【０２６８】
重力と空力は、車両１００の重心点ＭＰに対して作用するとする。空力ベクトルγは、以下の式のように表される。
【０２６９】
【数５１】
2017175844.tif 000059
【０２７０】
空力は、車両１００の速度Ｖと角速度ωに依存するので、以下の式で表される。
【０２７１】
【数５２】
2017175844.tif 000060
【０２７２】
なお、上記式（５１）では、仮定（ｂ）に基づくＶy＝０、及び式（３４）を用いて変形を行った。
【０２７３】
図１３に示すように、センサ群５の取り付け位置ＰＳから前輪２の路面接地点Ｐ２までのベクトルをσとすると、ベクトルσは以下の式で表される。なお、以下の式において、取り付け位置ＰＳから前輪２の中心までの水平距離をＬ’とする。
【０２７４】
【数５３】
2017175844.tif 000061
【０２７５】
基準点Ｏから見た車両１００にかかる力のモーメントベクトルＮは、下記式で表される。
【０２７６】
【数５４】
2017175844.tif 000062
【０２７７】
ここで、基準点Ｏから見た車両１００の角運動量ベクトルＬの１階微分量は、基準点Ｏから見た車両１００にかかる力のモーメントベクトルＮに一致する。すなわち、下記式が成立する。
【０２７８】
【数５５】
2017175844.tif 000063
【０２７９】
上記（５４）式は、基準点がどこにあっても成立するはずである。すなわち、上記（５４）式は、任意の位置ベクトルｒに対して成立するはずである。よって、上記（４６）式及び（５３）式を恒等式とすることで、以下の２式が成立する。
【０２８０】
【数５６】
2017175844.tif 000064
【０２８１】
【数５７】
2017175844.tif 000065
【０２８２】
式（５５）は、下記式（５７）のように変形できる。
【０２８３】
【数５８】
2017175844.tif 000066
【０２８４】
式（５７）を成分で表すと次式（５８）のようになる。
【０２８５】
【数５９】
2017175844.tif 000067
【０２８６】
上記（３８）式の第１成分の関係式から次の式が導かれる。
【０２８７】
【数６０】
2017175844.tif 000068
【０２８８】
この上記（５９）式を用いて、上記（５８）式は以下のように書き換えられる。
【０２８９】
【数６１】
2017175844.tif 000069
【０２９０】
上記（５６）式は、（５５）式を用いて以下のように変形できる。
【０２９１】
【数６２】
2017175844.tif 000070
【０２９２】
上記（６１）式を成分で表すと下記の式（６４）になる。
【０２９３】
【数６３】
2017175844.tif 000071
【０２９４】
上記（６０）式の第２成分及び第３成分を、（６４）式の第１成分に代入すると、ＦＦy、ＦＲy、ＦＦz、ＦＲzを消去することができる。すなわち、下記式（６５）を得る。
【０２９５】
【数６４】
2017175844.tif 000072
【０２９６】
上記（６５）式は、以下のように記載することが可能である。
【０２９７】
【数６５】
2017175844.tif 000073
【０２９８】
ここで、上記（６６）式の関数Ｑxの因子は、センサ群５から入力される値、及び姿勢推定装置１０で推定される値の他は、全て車両諸元等から一義的に定まる数である。つまり、上記（６６）式は、時間の経過に拘らず、必ず定数を示す関数Ｑxが存在することを意味する。この関数「Ｑx＝定数」を満たす方程式が「特性方程式」に対応する。
【０２９９】
ところで、上記（５９）式より、上記（４１）式は、以下のように書き換えることができる。
【０３００】
【数６６】
2017175844.tif 000074
【０３０１】
また、上記（３８）式、（４０）式、及び（６６）式より、以下の式が導かれる。
【０３０２】
【数６７】
2017175844.tif 000075
【０３０３】
カルマンフィルタ５０（50_1、50_2、50_3）は、システム方程式演算部５１において上記（６７）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（６８）式を観測方程式として用いて演算を行う。
【０３０４】
更に、ロール角速度オフセットｂro、ヨー角速度オフセットｂya、ピッチ角速度オフセットｂpi、上下加速度オフセットｂzが変化する場合でも、これらの値は車両１００の動きに比べて遅い。このため、ロール角速度オフセットｂroの微分値、ヨー角速度オフセットｂyaの微分値、ピッチ角速度オフセットｂpiの微分値、及び上下加速度オフセットｂzの微分値をいずれも０とみなすことができる。
【０３０５】
また、上記（６７）式、（６８）式において、オフセット誤差を反映させた値に置き換えることで、それぞれ以下の式を得る。
【０３０６】
【数６８】
2017175844.tif 000076
【０３０７】
【数６９】
2017175844.tif 000077
【０３０８】
ところで、本実施形態では、図１２を参照して上述したように、各カルマンフィルタ５０（50_1、50_2、50_3）のシステム方程式演算部５１及び観測方程式演算部５２に対して、仮想オフセット入力部５９から前後加速度センサオフセットｂx（ｂx1、ｂx2、ｂx3）が入力される。すなわち、本実施形態のシステム方程式演算部５１は、上記（６９）式に代えて下記（６９ａ）式をシステム方程式とした演算を行い、観測方程式演算部５２は、上記（７０）式に代えて、下記（７０ａ）式を観測方程式とした演算を行う。
【０３０９】
【数７０】
2017175844.tif 000078
【０３１０】
【数７１】
2017175844.tif 000079
【０３１１】
つまり、本実施形態のカルマンフィルタ５０は、システム方程式演算部５１において上記（６９ａ）式をシステム方程式として用いて演算を行い、観測方程式演算部５２におい
て上記（７０ａ）式を観測方程式として用いて演算を行う。
【０３１２】
なお、上記（６９ａ）式、及び（７０ａ）式内に記載されているｂxの値としては、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９から入力された前後加速度センサオフセットｂx（ｂx1、ｂx2、ｂx3）の値が採用される。
【０３１３】
システム方程式演算部５１は、この仮想オフセット入力部５９から入力された前後加速度センサオフセットｂx（ｂx1、ｂx2、ｂx3）を含むシステム方程式に基づいて、ロール角、ピッチ角θ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi、及び上下加速度センサオフセットｂzの推定値を算出する。同様に、観測方程式演算部５２は、この仮想オフセット入力部５９から入力された前後加速度センサオフセットｂx（ｂx1、ｂx2、ｂx3）を含む観測方程式に基づいて、上記システム方程式演算部５１の推定結果も利用して、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの推定値を算出する。これらの推定値が、尤度演算部５７に入力され、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさを示す尤度に関する値が算定される。
【０３１４】
ここで、仮想オフセット入力部５９から入力される、前後加速度センサオフセットｂx（ｂx1、ｂx2、ｂx3）の値のうち、最も大きい値については、前後加速度センサ２５が取り得るゼロ点オフセットの最大値よりも大きい値とし、最も小さい値については、前後加速度センサ２５が取り得るゼロ点オフセットの最小値よりも小さい値とするのが好ましい。これにより、観測方程式演算部５２は、前後加速度センサ２５のゼロ点オフセットの値が最も大きく振れた場合を考慮して、上下加速度Ｇz、左右加速度Ｇy、及び後輪速度ｖrの推定値を算出することができる。
【０３１５】
なお、上述したように、上述した実施形態では、演算部４９が３つのカルマンフィルタ５０（50_1、50_2、50_3）を備える場合を説明しているが、演算部４９が２つのカルマンフィルタ５０を備える場合や、４つ以上のカルマンフィルタ５０を備える場合においても、同様に前後加速度センサオフセットｂxの値を設定することができる。
【０３１６】
次に、尤度演算部５７が算出する尤度λについて説明する。ここで、上下加速度センサ２４が検出する上下加速度Ｇzの検出誤差をδＧz、左右加速度センサ２６が検出する左右加速度Ｇyの検出誤差をδＧy、後輪速度センサ７が検出する後輪速度ｖrの検出誤差をδｖr、ロール軸周り回転運動方程式のモデル式誤差をδＱxとすると、これらの検出誤差の分散共分散行列Ｒは下記式で表される。上述したように、ライダー体重を仮定したり、ライダーを剛体として扱って運動方程式モデルを立てているため、この仮定に基づく誤差が発生する。この誤差がδＱxに対応する。
【０３１７】
【数７２】
2017175844.tif 000080
【０３１８】
また、ロール角φの推定誤差をδφ、ピッチ角θの推定誤差をδθ、車両速度Ｖxの推定誤差をδＶx、ロール角速度センサオフセットｂroの推定誤差をδｂro、ヨー角速度センサオフセットｂyaの推定誤差をδｂya、ピッチ角速度センサオフセットｂpiの推定誤差をδｂpi、上下加速度センサオフセットｂzの推定誤差をδｂzとすると、これらの検出誤差の分散共分散行列Ｐは下記式で表される。
【０３１９】
【数７３】
2017175844.tif 000081
【０３２０】
各カルマンフィルタ５０（50_1、50_2、50_3）では、上記行列Ｐ及び行列Ｒが逐次算出されている。
【０３２１】
観測予測誤差ｅに含まれる誤差の分散共分散行列Ｓは、上述した（２５）式で表される。以下、再掲する。
【０３２２】
【数７４】
2017175844.tif 000082
【０３２３】
式（２５）において、Ｈはｈ（ｘ）のヤコビ行列であり、下記式で規定される行列である。
【０３２４】
【数７５】
2017175844.tif 000083
【０３２５】
尤度演算部５７は、上記（７４）式にて規定される行列Ｈに基づき、例えば第一実施形態と同様に、式（２７）に基づいて尤度λを算出する。状態量決定部５０３は、各カルマンフィルタ５０（50_1、50_2、50_3）から出力された尤度λに基づき、前後加速度センサオフセットｂxの値を重み付けすることで、前後加速度センサオフセットｂxの推定値を求めることができる。同様の理屈により、各カルマンフィルタ５０において推定されたｘ（φ、θ、Ｖx、ｂro、ｂya、ｂz）を、各カルマンフィルタ５０（50_1、50_2、50_3）の推定結果の確からしさである尤度λ（λ1、λ2、λ3）に応じて重み付けすることに・BR>謔チて、前後加速度センサオフセットｂxを考慮した推定結果を得ることができる。
【０３２６】
〈荷重推定部の構成〉
荷重推定部６０は、演算部４９において演算した結果に基づいて、車両１００の前輪２及び後輪３の少なくとも一方にかかる荷重を推定する処理を行う。上述した式（６０）の各成分を、式（６４）の第２成分及び第３成分に代入すると、以下の式を得る。
【０３２７】
【数７６】
2017175844.tif 000084
【０３２８】
本実施形態において、車両１００は二輪車であるため、Ｌ＋Ｌ'≠０である。このとき、（６０）式の第２成分、第３成分、（７５）式、及び（７６）式から、ＦＦy、ＦＲy、ＦＦz、及びＦＲzについて解くことができる。Ｚ方向に係る前輪２の力ＦＦzと、Ｚ方向に係る後輪３の力ＦＲzは、下記式によって算出される。なお、下記の式においてＪF及びＪRはいずれも関数である。
【０３２９】
【数７７】
2017175844.tif 000085
【０３３０】
荷重推定部６０は、演算部４９において演算した結果に基づいて、上記の演算を行うことで、前輪２にかかる力ＦＦと、後輪３にかかる力ＦＲとを算出する。
【０３３１】
〈サスペンションストローク量推定部の構成〉
サスペンションストローク量推定部７０は、荷重推定部６０で行われた推定結果に基づいて、演算を行い、前輪２又は後輪３の少なくとも一方に設けられたサスペンションのストローク量を推定する。
【０３３２】
キャスター角をεFとすると、フロントフォーク１６にかかる力FfはＦＦz cosεF−ＦＦx sinεFで算出される。なお、キャスター角とは、前輪３の接地点Ｐ２から鉛直上向きに向かう方向と、フロントフォーク１６とのなす角である。
【０３３３】
リヤ（後輪）サスペンションの緩衝装置のストローク量をδR、後輪３の中心位置の変位量をδwとする。また、リヤサスペンションの緩衝装置のストローク量がδRのときの、リヤアームの、水平方向からの垂れ角をεR(δR)とする。キャスター角εF及びリヤアーム角εR(δw)を図１４に示す。
【０３３４】
このとき、サスペンションにかかる力Ｆ1は、下記式で表される。
【０３３５】
【数７８】
2017175844.tif 000086
【０３３６】
なお、上記（７９）式において、κはレバ−比と呼ばれる諸元であり、設計図面から算出できる。図１５にδRとδWの関係の一例を模式的に示す。
【０３３７】
ここで、サスペンションストローク量推定部７０は、以下の推定処理を行う。
【０３３８】
【数７９】
2017175844.tif 000087
【０３３９】
この時、フロントフォークにかかる力Ff、及びサスペンションにかかる力Ｆ1は、それぞれ以下の式（８２）、（８３）で算出される。
【０３４０】
【数８０】
2017175844.tif 000088
【０３４１】
図１６は、サスペンションの緩衝装置をモデル化した等価回路である。サスペンションは、図１６のようなバネｋ及びダンパーｃの並列モデルにて等価的に表現することが可能である。この等価回路に基づいて、運動方程式を立てると、次の式のようになる。なお、各サスペンションの緩衝装置のストローク量、各緩衝装置のバネ定数、各緩衝装置のダンパー係数、各バネにかけられているプリセット荷重を、以下の表に示す符号で定義する。
【０３４２】
【表７】
2017175844.tif 000089
【０３４３】
【数８１】
2017175844.tif 000090
【０３４４】
なお、上記式（８４）及び（８５）では、（８２）式で記載されている関数ＤF、及び（８３）式で記載されている関数ＤRの引数の記載を割愛している。
【０３４５】
上記式（８４）及び（８５）に記載された微分方程式を解くと、以下の式が導出される。
【０３４６】
【数８２】
2017175844.tif 000091
【０３４７】
なお、サスペンションストローク量推定部７０は、上述したように、式（８４）及び（８５）に記載された微分方程式を解くことで、サスペンションの緩衝装置のストローク量を算出するものとしても構わないし、時間の離散化処理を行うことでサスペンションの緩衝装置のストローク量を算出するものとしても構わない。
【０３４８】
更に、サスペンションストローク量推定部７０は、下記式に基づく演算を行うことで、リヤサスペンションの沈下量δWを算出するものとしてもよい。
【０３４９】
【数８３】
2017175844.tif 000092
【０３５０】
〈仰俯角推定部の構成〉
仰俯角推定部８０は、荷重推定部６０及びサスペンションストローク量推定部７０による推定結果に基づいて仰俯角ζを算出する。
【０３５１】
前輪のサスペンションが沈下量δFだけ沈下し、後輪のサスペンションが沈下量δWだけ沈下した場合、前輪の中心の位置ベクトルＦo、後輪の中心の位置ベクトルＲoは、それぞれ下記式で表すことができる。
【０３５２】
【数８４】
2017175844.tif 000093
【０３５３】
上記式（８７）及び（８８）より、仰俯角ζは、以下の式により算出される。
【０３５４】
【数８５】
2017175844.tif 000094
【０３５５】
なお、上記式（８９）において、ζ0は、サスペンションのストローク量が０の場合における、車両座標系の基底ベクトルｅbと路面座標系の基底ベクトルｅrとのなす角である。
【０３５６】
〈勾配推定部の構成〉
勾配推定部９０は、仰俯角推定部８０による推定結果、及び演算部４９による推定結果に基づいて路面の縦断勾配ηを算出する。
【０３５７】
路面の縦断勾配ηは、車両１００のピッチ角θ、ロール角φ、及び仰俯角ζによって以下の式で表される。
【０３５８】
【数８６】
2017175844.tif 000095
【０３５９】
よって、路面の縦断勾配ηは、以下の式で算出される。
【０３６０】
【数８７】
2017175844.tif 000096
【０３６１】
〈別構成〉
本実施形態の姿勢推定装置１０は、以下のバリエーションを採用することが可能である。
【０３６２】
（１）  第一実施形態で説明したのと同様に、本実施形態においても、各カルマンフィルタ５０が算出した尤度λに基づいて、最も確からしい一のカルマンフィルタ５０を選択し、当該カルマンフィルタ５０に入力された左右加速度センサオフセットｂyを、左右加速度センサオフセットの推定値とし、当該カルマンフィルタ５０の推定結果を演算部４９の推定結果としても構わない。この場合、状態量決定部５０３の演算処理内容は簡素化される。
【０３６３】
（２）  本実施形態では、各カルマンフィルタ５０において、ロール角φ、ピッチ角θ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、前後加速度センサオフセットｂxを推定するものとした。これに代えて、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi及び前後加速度センサオフセットｂxを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、上下加速度センサオフセットｂzを推定するものとしても構わない。このとき、上下加速度センサ２４が「対象検出部」に対応する。
【０３６４】
この場合、本実施形態の演算部４９が備える各カルマンフィルタ５０は、システム方程式演算部５１において上記（６９ａ）式に代えて、下記（９２）式をシステム方程式として用いて演算を行う。また、各カルマンフィルタ５０は、観測方程式演算部５２において上記（７０ａ）式に代えて、下記（９３）式を観測方程式として用いて演算を行う。
【０３６５】
【数８８】
2017175844.tif 000097
【０３６６】
【数８９】
2017175844.tif 000098
【０３６７】
なお、上記（９３）式内に記載されているｂzの値としては、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９から入力された上下加速度センサオフセットｂz（ｂz1、ｂz2、ｂz3）の値が採用される。
【０３６８】
また、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、左右加速度センサオフセットｂyを推定するものとしても構わない。この場合は、各カルマンフィルタ５０は、システム方程式演算部５１において上記（６９ａ）式に代えて下記（９４）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（７０ａ）式に代えて下記（９５）式を観測方程式として用いて演算を行う。
【０３６９】
【数９０】
2017175844.tif 000099
【０３７０】
【数９１】
2017175844.tif 000100
【０３７１】
同様に、上記（９５）式内に記載されているｂyの値としては、各カルマンフィルタ５０（50_1、50_2、50_3）が備える仮想オフセット入力部５９から入力された左右加速度センサオフセットｂy（ｂy1、ｂy2、ｂy3）の値が採用される。
【０３７２】
（３）  本実施形態では、姿勢推定装置１０が、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０を備えているものとして説明した。しかし、姿勢推定装置１０は、これらの演算機能を有していないものとしても構わない。すなわち、図１７に示すように、姿勢推定装置１０が、センサ群５、後輪速度センサ７、ローパスフィルタ（３１，３２，３３，３４，３５，３６）、微分器（４１，４２，４３）、及び演算部４９を備える構成であっても構わない。
【０３７３】
この場合、図１０の構成とは異なり、姿勢推定装置１０は、仰俯角ζを推定する機能を有していないため、演算部４９には、仰俯角ζ及び仰俯角ζの１階微分値は入力されない。よって、演算部４９は、上述した各式において、ζ＝０とした式に置き換えて演算を行うものとすればよい。この構成によれば、車両１００において、サスペンションの動きを考慮せずに車体１の姿勢を推定することができる。
【０３７４】
〈各パラメータ推定値の算出〉
以下、本実施形態に係る姿勢推定装置を用いて、精度よく車両１００の姿勢が推定できていることを、実施例を参照して説明する。以下の各実施例では、いずれも、第一実施形態と同様、所定の路面上に車両１００を走行させながら、本実施形態係る姿勢推定装置によって車両１００の姿勢を推定した。その際、意図的に所定のセンサに対して所定のタイミングでオフセット誤差を生じさせた。
【０３７５】
（実施例１）
図１８Ａ〜図１８Ｇで想定されている演算部４９は、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi、及び上下加速度センサオフセットｂzを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、前後加速度センサオフセットｂxを推定する構成である。
【０３７６】
図１８Ａ及び図１８Ｂは、所定のタイミングで前後加速度センサ２５に対して１００ｍＧのオフセット誤差を重畳させたときの、各推定値の結果を示すグラフである。図９Ａ及び図９Ｂにおいて、点線は真値に対応し、破線は１つのカルマンフィルタ５０を用いて推定した値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。なお、ここでいう破線のデータは、本実施形態で上述したように、特性方程式を用いることで追加的に一つのセンサオフセットの値を推定した場合のデータである。
【０３７７】
図１８Ａによれば、前後加速度センサオフセットｂxの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。なお、図１８Ａにおいて、破線のデータは尤度を用いた推定処理を行っていないため、前後加速度センサオフセットｂxの推定が行えない構成である。
【０３７８】
また、図１８Ｂによれば、破線よりも実線の方が真値に近い推定結果が得られていることが分かる。これにより、複数のカルマンフィルタ５０を並列に構成し、尤度に基づいて追加的にオフセット誤差を推定することで、推定精度が向上することが分かる。
【０３７９】
図１８Ｃ〜図１８Ｇは、前後加速度センサ２５以外のセンサに対してオフセット誤差を発生させた場合の推定結果を示すグラフである。図１８Ｃ〜図１８Ｇにおいて、点線は真値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０３８０】
図１８Ｃは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１８Ｄは、所定の時間経過後にピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１８Ｅは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１８Ｆは、所定の時間経過後に上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させた場合の各推定値に対応する。
【０３８１】
また、図１８Ｇは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、ロール角速度センサ２１にオフセットを１deg／秒重畳させ、ピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させ、ヨー角速度センサ２２にオフセットを１deg／秒重畳させ、上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させ、前後加速度センサ２５に対してオフセットを１００ｍＧ重畳さた場合の結果である。
【０３８２】
図１８Ｃ〜図１８Ｇによれば、オフセットを重畳した直後においては他のセンサオフセットの推定値が一時的に変動している場合も見受けられるものの、時間経過と共に収束していることが見て取れる。よって、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉されることなく、精度よく推定できていることが確認される。
【０３８３】
（実施例２）
図１９Ａ〜図１９Ｇで想定されている演算部４９は、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi、及び前後加速度センサオフセットｂxを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、上下加速度センサオフセットｂzを推定する構成である。
【０３８４】
図１９Ａ及び図１９Ｂは、所定のタイミングで上下加速度センサ２４に対して１００ｍＧのオフセット誤差を重畳させたときの、各推定値の結果を示すグラフである。図１９Ａ及び図１９Ｂにおいて、点線は真値に対応し、破線は１つのカルマンフィルタ５０を用いて推定した値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。なお、ここでいう破線のデータは、本実施形態で上述したように、特性方程式を用いることで追加的に一つのセンサオフセットの値を推定した場合のデータである。
【０３８５】
図１９Ａによれば、上下加速度センサオフセットｂzの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。なお、図１９Ａにおいて、破線のデータは尤度を用いた推定処理を行っていないため、前後加速度センサオフセットｂzの推定が行えない構成である。
【０３８６】
また、図１９Ｂによれば、破線よりも実線の方が真値に近い推定結果が得られていることが分かる。これにより、複数のカルマンフィルタ５０を並列に構成し、尤度に基づいて追加的にオフセット誤差を推定することで、推定精度が向上することが分かる。
【０３８７】
図１９Ｃ〜図１９Ｇは、上下加速度センサ２４以外のセンサに対してオフセット誤差を発生させた場合の推定結果を示すグラフである。図１９Ｃ〜図１９Ｇにおいて、点線は真値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０３８８】
図１９Ｃは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１９Ｄは、所定の時間経過後にピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１９Ｅは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１９Ｆは、所定の時間経過後に前後加速度センサ２５に対してオフセットを１００ｍＧ重畳させた場合の各推定値に対応する。
【０３８９】
また、図１９Ｇは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、ロール角速度センサ２１にオフセットを１deg／秒重畳させ、ピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させ、ヨー角速度センサ２２にオフセットを１deg／秒重畳させ、上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させ、前後加速度センサ２５に対してオフセットを１００ｍＧ重畳さた場合の結果である。
【０３９０】
図１９Ｃ〜図１９Ｇによれば、オフセットを重畳した直後においては他のセンサオフセットの推定値が一時的に変動している場合も見受けられるものの、時間経過と共に収束していることが見て取れる。よって、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉されることなく、精度よく推定できていることが確認される。
【０３９１】
（実施例３）
図２０Ａ〜図２０Ｇで想定されている演算部４９は、各カルマンフィルタ５０において、ロール角φ、車両速度Ｖx、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂya、ピッチ角速度センサオフセットｂpi、及び前後加速度センサオフセットｂxを推定し、各カルマンフィルタ５０の推定結果に基づいて得られる尤度を考慮して、左右加速度センサオフセットｂyを推定する構成である。
【０３９２】
図２０Ａ及び図２０Ｂは、所定のタイミングで左右加速度センサ２６に対して１００ｍＧのオフセット誤差を重畳させたときの、各推定値の結果を示すグラフである。図２０Ａ及び図２０Ｂにおいて、点線は真値に対応し、破線は１つのカルマンフィルタ５０を用いて推定した値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。なお、ここでいう破線のデータは、本実施形態で上述したように、特性方程式を用いることで追加的に一つのセンサオフセットの値を推定した場合のデータである。
【０３９３】
図２０Ａによれば、左右加速度センサオフセットｂyの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。なお、図２０Ａにおいて、破線のデータは尤度を用いた推定処理を行っていないため、左右加速度センサオフセットｂyの推定が行えない構成である。
【０３９４】
また、図２０Ｂによれば、破線よりも実線の方が真値に近い推定結果が得られていることが分かる。これにより、複数のカルマンフィルタ５０を並列に構成し、尤度に基づいて追加的にオフセット誤差を推定することで、推定精度が向上することが分かる。
【０３９５】
図２０Ｃ〜図２０Ｇは、左右加速度センサ２６以外のセンサに対してオフセット誤差を発生させた場合の推定結果を示すグラフである。図２０Ｃ〜図２０Ｇにおいて、点線は真値に対応し、実線は複数のカルマンフィルタ５０を用いて推定した値に対応する。
【０３９６】
図２０Ｃは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図２０Ｄは、所定の時間経過後にピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図２０Ｅは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図２０Ｆは、所定の時間経過後に前後加速度センサ２５に対してオフセットを１００ｍＧ重畳させた場合の各推定値に対応する。
【０３９７】
また、図２０Ｇは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、ロール角速度センサ２１にオフセットを１deg／秒重畳させ、ピッチ角速度センサ２３に対してオフセットを１deg／秒重畳させ、ヨー角速度センサ２２にオフセットを１deg／秒重畳させ、前後加速度センサ２５に対してオフセットを１００ｍＧ重畳させ、左右加速度センサ２６に対してオフセットを１００ｍＧ重畳さた場合の結果である。
【０３９８】
図２０Ｃ〜図２０Ｇによれば、オフセットを重畳した直後においては他のセンサオフセットの推定値が一時的に変動している場合も見受けられるものの、時間経過と共に収束していることが見て取れる。よって、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉されることなく、精度よく推定できていることが確認される。
【０３９９】
また、実施例１〜３の各結果より、カルマンフィルタ５０内で推定するオフセット量の対象、及び尤度を用いて推定するオフセット量の対象を異ならせた場合でも、同様に精度よく推定が行えていることが確認される。
【０４００】
（実施例４）
図２１Ａ及び図２１Ｂは、第一実施形態の姿勢推定装置と第二実施形態の姿勢推定装置の推定精度を比較したものである。図２１Ａ及び図２１Ｂでは、ロール角速度センサ２１、ピッチ角速度センサ２３、ヨー角速度センサ２２、前後加速度センサ２５、及び上下加速度センサ２４に対してそれぞれオフセットを重畳させた場合の、各推定値の結果を示すグラフである。なお、図２１Ｂは、図２１Ａに示されている結果を、真値からの差分値として表記したグラフである。
【０４０１】
図２１Ａ及び図２１Ｂにおいて、点線は真値に対応する。二点鎖線は、参考例として構成した姿勢推定装置の結果に対応する。参考例は、ロール角速度センサ２１、ヨー角速度センサ２２、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６を有し、一つのカルマンフィルタ５０を備えた姿勢推定装置に対応する。参考例の姿勢推定装置は、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂyo、及び上下加速度センサオフセットｂzの推定を行う構成である。
【０４０２】
図２１Ａ及び図２１Ｂにおいて、破線は第一実施形態の姿勢推定装置の結果に対応する。この姿勢推定装置は、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂyo、ピッチ角速度センサオフセットｂpi、及び上下加速度センサオフセットｂzの推定を行う構成である。
【０４０３】
図２１Ａ及び図２１Ｂにおいて、実線は第二実施形態の姿勢推定装置の結果に対応する。この姿勢推定装置は、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂyo、ピッチ角速度センサオフセットｂpi、上下加速度センサオフセットｂz、及び前後加速度センサオフセットｂxの推定を行う構成である。なお、ここでは、ロール角速度センサオフセットｂro、ヨー角速度センサオフセットｂyo、ピッチ角速度センサオフセットｂpi、及び前後加速度センサオフセットｂxを各カルマンフィルタ５０内で推定し、尤度を用いてを上下加速度センサオフセットｂzの推定を行う構成とした。
【０４０４】
図２１Ａ及び図２１Ｂによれば、推定するオフセットの種類を増やすほど、各推定値が真値に近づいていることが理解される。
【０４０５】
［別実施形態］
別実施形態について説明する。
【０４０６】
〈１〉上述した実施形態では、後輪３に設けられた後輪速度センサ７からの情報が後輪速度として演算部４９に入力されるものとして説明した。しかし、図２２に示すように、前輪２に設けられた、前輪速度センサ８からの情報が、前輪速度として演算部４９に入力されるものとしても構わない。この場合、上述した式において、ベクトルρとベクトルσを入れ替えて計算すればよい。なお、計算上は、力ベクトルＦＦを後輪３の路面接地点Ｐ３にかかる力と読み替え、力ベクトルＦＲを前輪２の路面接地点Ｐ２にかかる力と読み替えることで、上記と同様の方法によって車体１の姿勢推定が可能である。
【０４０７】
〈２〉  基準点Ｏから見た車両１００にかかる力のモーメントを計算する際に、空力γ＝０と近似しても構わない。この場合、上記（５７）式においてγの項が存在しなくなるが、（５７）式から（５８）式への変形の過程で、（５１）式の関係式を利用してγを他の因子の関数に置換しているため、やはり（５８）式と同じ式が導かれる。
【０４０８】
〈３〉  ロール角速度センサ２１、ヨー角速度センサ２２、及びピッチ角速度センサ２３の各角速度センサに代えて、車両１００の上下方向、左右方向、及び前後方向とは異なる３以上の軸周りでの角速度を検出するセンサとしても構わない。この場合、各角速度センサの検出値を、幾何学的方法により、ロール角速度ωro、ピッチ角速度ωpi、及びヨー角速度ωyaに置換することが可能である。そして、置換された各値に基づいて、姿勢推定装置１０が上述した演算式で規定された演算を行うものとすればよい。
【０４０９】
同様に、上記実施形態において、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６の各加速度センサに代えて、車両１００の上下方向、左右方向、前後方向とは異なる３以上の加速度センサとしても構わない。この場合、各加速度センサの検出値を、幾何学的方法により、上下加速度Ｇz、前後加速度Ｇx、及び左右加速度Ｇyに置換することが可能である。そして、置換された各値に基づいて、姿勢推定装置１０が上述した演算式で規定された演算を行うものとすればよい。
【０４１０】
〈４〉  上述した実施形態では、演算部４９の入力パラメータの一つとして、後輪速度ｖrが利用されるものとしたが、これに限定されず、車両１００の走行速度（車両速度）を測定することのできる速度センサにより検出される車両速度を、演算部４９の入力パラメータとしてもよい。一例として、ＧＰＳセンサからの情報を用いることができる。
【０４１１】
この場合において、演算部４９で推定された車両速度Ｖxに代えて、センサで検出された車両速度に関する情報が用いられるものとしても構わない。すなわち、演算部４９は車両速度Ｖxを推定しないものとしても構わない。
【０４１２】
〈５〉  上記実施形態において、演算部４９はＥＣＵ２０及びプログラムによって実現されるものとしたが、これに限定されず、演算部４９の一部又は全ての機能が電子回路等のハードウェアにより実現されるものとしても構わない。
【０４１３】
〈６〉  本発明におけるカルマンフィルタのフィルタリング手法は、上記実施形態におけるカルマンフィルタ５０のフィルタリングに限られない。上記例に代えて、他の適応フィルタリング手法を用いても構わない。一例として、ＬＭＳ（最小平均二乗）適応フィルタやＨ∞フィルタリング等をカルマンフィルタに用いることができる。
【０４１４】
〈９〉  上記実施形態では、姿勢推定装置１０が自動二輪車に適用されるものとして説明を行ったが、同様のモデリングが可能であれば、自動四輪車、自動三輪車等の他の車両、又は船舶等の種々の輸送機器に適用することができる。
【０４１５】
上記実施形態において、オフセット誤差は、各検出部における検出値と実際の値すなわち真値との差を示す値である。各カルマンフィルタには、複数の検出部それぞれの検出値が入力される。さらに、各カルマンフィルタには、仮想オフセット量が入力される。仮想オフセット量は、複数の検出部のうち少なくとも１つの検出部におけるオフセット誤差すなわち第一オフセット誤差の仮定値である。第一オフセット誤差の対象となる検出部が対象検出部である。仮想オフセット量は、対象検出部のオフセット誤差の仮定値である。
【０４１６】
複数のカルマンフィルタそれぞれに入力される複数の仮想オフセット量のうち少なくとも２つの値は互いに異なっている。すなわち、仮想オフセット量は、少なくとも２種類以上の相互に異なる複数の値として複数のカルマンフィルタに与えられる。
【０４１７】
各カルマンフィルタは、ロール角の推定値及び尤度を出力する。また、各カルマンフィルタは、推定値として、複数の検出部の少なくとも１つの検出部であって対象検出部とは異なる検出部のオフセット誤差（すなわち第二オフセット誤差）を出力してもよい。各カルマンフィルタは、現在の推定動作において入力される複数の検出値と、前回の推定動作において算出した推定値とを用いて、現在の推定動作における推定値を算出する構成となっている。
【０４１８】
上記実施形態において、各カルマンフィルタは、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、及び第一オフセット誤差の仮想オフセット量を入力とする。各カルマンフィルタの出力は、前記移動体のロール角の推定値及び尤度を少なくとも含む。
【０４１９】
各カルマンフィルタは、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値のうち少なくとも１つと、前回の推定動作の推定値とを用いて、現在の中間推定値を算出する。各カルマンフィルタは、中間推定値を、前回の推定値と観測値との比較結果に基づいて補正する。観測値は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値のうち少なくとも１つとすることができる。上記例では、各カルマンフィルタは、前回の推定値と観測値との比較結果を示す値の一例として、カルマンゲインを算出している。上記中間推定値を、前回の推定値と観測値との比較結果を示す値を用いて補正した値が、現在の推定動作の推定値として出力される。
【０４２０】
上記実施形態において、カルマンフィルタは、前回の推定値と観測値と異なり度合いを示す値である観測予測誤差に基づいて、尤度を算出している。観測値は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値のうち少なくとも１つとすることができる。
【符号の説明】
【０４２１】
１      ：      車体
２      ：      前輪
３      ：      後輪
５      ：      センサ群
７      ：      後輪速度センサ
８      ：      前輪速度センサ
１０      ：      姿勢推定装置
１１      ：      ハンドル
１２      ：      ナビゲーションシステム
１４      ：      ヘッドライト
１５      ：      ヘッドライト駆動装置
１６      ：      フロントフォーク
２０      ：      電子制御ユニット（ＥＣＵ）
２１      ：      ロール角速度センサ
２２      ：      ヨー角速度センサ
２３      ：      ピッチ角速度センサ
２４      ：      上下加速度センサ
２５      ：      前後加速度センサ
２６      ：      左右加速度センサ
３１，３２，３３，３４，３５，３６      ：      ローパスフィルタ
４１，４２，４３，４５      ：      微分器
４９      ：      演算部
５０      ：      カルマンフィルタ
５１      ：      システム方程式演算部
５２      ：      観測方程式演算部
５３      ：      減算部
５４      ：      加算部
５５      ：      積分部
５６      ：      カルマンゲイン演算部
５７      ：      尤度演算部
５８      ：      ローパスフィルタ
５９      ：      仮想オフセット入力部
６０      ：      荷重推定部
７０      ：      サスペンションストローク量推定部
８０      ：      仰俯角推定部
９０      ：      勾配推定部
１００      ：      車両
２００      ：      路面
【図１】
2017175844.tif 000101
【図２】
2017175844.tif 000102
【図３】
2017175844.tif 000103
【図４】
2017175844.tif 000104
【図５】
2017175844.tif 000105
【図６】
2017175844.tif 000106
【図７】
2017175844.tif 000107
【図８】
2017175844.tif 000108
【図９Ａ】
2017175844.tif 000109
【図９Ｂ】
2017175844.tif 000110
【図９Ｃ】
2017175844.tif 000111
【図９Ｄ】
2017175844.tif 000112
【図９Ｅ】
2017175844.tif 000113
【図９Ｆ】
2017175844.tif 000114
【図９Ｇ】
2017175844.tif 000115
【図９Ｈ】
2017175844.tif 000116
【図９Ｉ】
2017175844.tif 000117
【図９Ｊ】
2017175844.tif 000118
【図９Ｋ】
2017175844.tif 000119
【図９Ｌ】
2017175844.tif 000120
【図１０】
2017175844.tif 000121
【図１１】
2017175844.tif 000122
【図１２】
2017175844.tif 000123
【図１３】
2017175844.tif 000124
【図１４】
2017175844.tif 000125
【図１５】
2017175844.tif 000126
【図１６】
2017175844.tif 000127
【図１７】
2017175844.tif 000128
【図１８Ａ】
2017175844.tif 000129
【図１８Ｂ】
2017175844.tif 000130
【図１８Ｃ】
2017175844.tif 000131
【図１８Ｄ】
2017175844.tif 000132
【図１８Ｅ】
2017175844.tif 000133
【図１８Ｆ】
2017175844.tif 000134
【図１８Ｇ】
2017175844.tif 000135
【図１９Ａ】
2017175844.tif 000136
【図１９Ｂ】
2017175844.tif 000137
【図１９Ｃ】
2017175844.tif 000138
【図１９Ｄ】
2017175844.tif 000139
【図１９Ｅ】
2017175844.tif 000140
【図１９Ｆ】
2017175844.tif 000141
【図１９Ｇ】
2017175844.tif 000142
【図２０Ａ】
2017175844.tif 000143
【図２０Ｂ】
2017175844.tif 000144
【図２０Ｃ】
2017175844.tif 000145
【図２０Ｄ】
2017175844.tif 000146
【図２０Ｅ】
2017175844.tif 000147
【図２０Ｆ】
2017175844.tif 000148
【図２０Ｇ】
2017175844.tif 000149
【図２１Ａ】
2017175844.tif 000150
【図２１Ｂ】
2017175844.tif 000151
【図２２】
2017175844.tif 000152
【国際調査報告】
2017175844.tif 000153
2017175844.tif 000154
2017175844.tif 000155
2017175844.tif 000156