WO2017175840A1 - 姿勢推定装置及び輸送機器 - Google Patents

Abstract

移動体の姿勢を推定する姿勢推定装置であって、移動体のロール角及びピッチ角を推定すると共に、第一、第二、及び第三角速度検出部、並びに、第一、第二、及び第三加速度検出部のうちの少なくとも一つのオフセット誤差を演算処理によって推定する姿勢推定部を備える。姿勢推定部は、現在の推定動作において、第一、第二、及び第三角速度検出部の検出値、第一、第二、及び第三加速度検出部の検出値、速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値に基づいて、移動体のロール角及びピッチ角と、少なくとも一つのオフセット誤差とを推定する。

Description

姿勢推定装置及び輸送機器

　本発明は、姿勢推定装置、及びこれを備えた輸送機器に関する。

　従来、自動二輪車等の車両のロール角を推定する種々の推定装置が提案されている。例えば、推定装置により推定されたロール角に基づいて灯火器の向きを制御することにより、車両の傾斜に拘らず灯火器で適切な方向に光を照射することができる。

　特許文献１に記載された車両姿勢推定装置では、車両運動の前後加速度、横加速度、上下加速度、ヨー加速度、及びロール角速度の各検出値、前後車体速度の推定値、及びピッチ角速度の推定値に基づいて、ロール角及びピッチ角が推定されている。

特開２００９－７３４６６号公報

　近年、姿勢推定装置の推定精度を向上することが望まれている。

　本発明は、移動体の姿勢推定精度を向上させる姿勢推定装置、及びこれを備えた輸送機器を実現することを目的とする。

　（構成１）
　本発明の一例である構成１は、移動体の姿勢を推定する姿勢推定装置に関する。前記姿勢推定装置は、
　前記移動体の、第一軸の周りでの角速度である第一角速度を検出する第一角速度検出部と、
　前記移動体の、前記第一軸とは異なる方向である第二軸の周りでの角速度である第二角速度を検出する第二角速度検出部と、
　前記移動体の、前記第一軸及び前記第二軸とは異なる第三軸の周りでの角速度である第三角速度を検出する第三角速度検出部と、
　前記移動体の、第一方向に関する加速度である第一加速度を検出する第一加速度検出部と、
　前記移動体の、前記第一方向とは異なる第二方向に関する加速度である第二加速度を検出する第二加速度検出部と、
　前記移動体の、前記第一方向及び前記第二方向とは異なる第三方向に関する加速度である第三加速度を検出する第三加速度検出部と、
　前記移動体の進行方向の移動速度に関する情報を検出する速度情報検出部と、
　前記移動体のロール角及びピッチ角を推定すると共に、前記第一角速度検出部、前記第二角速度検出部、前記第三角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差を演算処理によって推定する姿勢推定部とを備える。
　前記姿勢推定部は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値に基づいて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定することを特徴とする。

　上記の構成１によれば、移動体の、３方向に係る角速度を検出する検出部、及び３方向に係る加速度を検出する検出部を備える。すなわち、姿勢推定装置は６軸に係る値を検出することができる。そして、この検出された値に基づいて、移動体のロール角及びピッチ角に加えて、少なくとも一つの検出部のオフセット誤差が推定される。そして、この推定されたオフセット誤差が考慮された状態で、移動体のロール角及びピッチ角、及びオフセット誤差が推定される。これにより、移動体のロール角及びピッチ角の推定精度が向上する。

　（構成２）
　上記構成１において、前記姿勢推定部は、前記第一角速度検出部、前記第二角速度検出部、及び前記第三角速度検出部のオフセット誤差を推定するものとしても構わない（構成２）。構成２によれば、前回推定された各検出部のオフセット誤差が考慮されて、移動体のロール角及びピッチ角、及び現在のオフセット誤差が推定される。これにより、移動体のロール角及びピッチ角の推定精度が向上する。

　（構成３）
　上記構成１または２において、前記姿勢推定部は、前記移動体の進行方向の移動速度を更に推定する構成であるものとしても構わない。

　（構成４及び構成５）
　上記構成１～３のいずれかにおいて、前記姿勢推定部は、前記第一加速度検出部、前記第二加速度検出部、又は前記第三加速度検出部のオフセット誤差を推定するものとしても構わない（構成４）。構成４の一例として、前記第一加速度検出部が前記移動体の上下方向における加速度を検出する場合において、前記姿勢推定部が前記第一加速度検出部のオフセット誤差を推定するものとしても構わない（構成５）。

　（構成６）
　上記構成１～５のいずれかにおいて、
　前記第一角速度検出部は、前記移動体のロール角速度を検出し、
　前記第二角速度検出部は、前記移動体のヨー加速度を検出し、
　前記第三角速度検出部は、前記移動体のピッチ角速度を検出するものとしても構わない（構成６）。

　また、一例として、
　前記第一加速度検出部は、前記移動体の上下方向における加速度を検出し、
　前記第二加速度検出部は、前記移動体の前後方向における加速度を検出し、
　前記第一加速度検出部は、前記移動体の左右方向における加速度を検出するものとしても構わない。

　（構成７）
　上記構成１～６のいずれかにおいて、前記姿勢推定部は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値の関係を用いて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定するカルマンフィルタを含むものとしても構わない。

　（構成８）
　上記構成７において、前記カルマンフィルタは、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値を要素とする所定の関数の値が定数になる特性方程式を用いて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定するものとしても構わない。

　上記構成８によれば、特性方程式を用いることで、新たに一つの検出部のオフセット誤差を推定することができる。これにより、移動体のロール角及びピッチ各の推定精度が向上する。なお、この特性方程式としては、例えば、前記移動体の、前記第一軸の周りの回転運動方程式から導出された方程式を利用することができる（構成９）。

　（構成１０）
　上記構成１～９のいずれかにおいて、前記姿勢推定装置は、
　前記移動体に備えられた前輪及び後輪の少なくとも一方にかかる荷重を推定する荷重推定部を備え、
　前記荷重推定部は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、並びに、前記姿勢推定部によって推定された前記移動体のロール角、前記移動体のピッチ角、及び前記少なくとも一つのオフセット誤差に基づいて前記荷重を推定するものとしても構わない。

　（構成１１）
　上記構成１０において、前記姿勢推定装置は、
　前記移動体の前輪又は後輪の少なくとも一方に設けられたサスペンションのストローク量を推定するサスペンションストローク量推定部を備え、
　前記荷重推定部は、前記移動体に設けられた前記前輪及び前記後輪の双方にかかる荷重を推定し、
　前記サスペンションストローク量推定部は、前記荷重推定部によって推定された、前記前輪及び前記後輪の双方にかかる荷重の推定値に基づいて、前記サスペンションのストローク量を推定するものとしても構わない。

　（構成１２）
　上記構成１１において、前記姿勢推定装置は、
　前記サスペンションストローク量推定部により推定された、前記サスペンションのストローク量の推定値に基づいて、前記移動体に固定された車体座標系の軸と、前記前輪又は前記後輪が接触している路面上に固定された路面座標系の軸との角度である仰俯角を推定する仰俯角推定部を備えるものとしても構わない。上記構成１２によれば、移動体の仰俯角を推定することができるため、更に精度よく移動体の姿勢が推定される。

　（構成１３）
　上記構成１２において、前記姿勢推定部は、前記仰俯角推定部により推定された前記仰俯角の推定値を考慮して、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定するものとしても構わない。

　（構成１４）
　上記構成１２又は１３において、前記姿勢推定装置は、
　前記仰俯角推定部により推定された前記仰俯角の推定値と、前記姿勢推定部により推定された前記移動体のロール角及びピッチ角の推定値とに基づいて、前記移動体が設置されている路面の縦断勾配を推定する勾配推定部を備えるものとしても構わない。

　上記構成１４によれば、路面の勾配を精度よく推定することができる。

　前記移動体と、上記構成１～１４のいずれかの前記姿勢推定装置とを備えた輸送機器も、本発明の実施形態の１つである。

　本発明によれば、移動体の姿勢推定精度を向上させることができる。

慣性座標系の基底ベクトルｅ_o、車両座標系の基底ベクトルｅ_b、及び路面座標系の基底ベクトルｅ_rの関係を示す図面である。 実施形態に係る姿勢推定装置を備えた車両の模式図である。 姿勢推定装置の構成の一例を模式的に示すブロック図である。 仰俯角の説明をするための図である。 カルマンフィルタの構成の一例を模式的に示すブロック図である。 センサ群の取り付け位置を説明するための図である。 後輪の模式的な断面図である。 センサ群の取り付け位置をベクトルで表した図である。 前後輪の路面接地点にかかる力のベクトルを表した図である。 キャスター角及びリヤアーム角を説明するための図である。 δ_Rとδ_Wの関係の一例を模式的に示したグラフである。 サスペンションの緩衝装置をモデル化した等価回路である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 姿勢推定装置の構成の一例を模式的に示すブロック図である。 実施形態に係る姿勢推定装置を備えた車両の模式図である。

　本発明の実施形態につき、図面を参照して説明する。なお、以下の各図において、図面の寸法比と実際の寸法比は必ずしも一致するものとは限らない。

　〈定義〉
　以下、図面を参照して座標軸の定義を行う。図１は、慣性座標系の基底ベクトルｅ_o、車両座標系の基底ベクトルｅ_b、及び路面座標系の基底ベクトルｅ_rの関係を示す図面である。なお、以下の数式において、ベクトルを太字で表記することがある。いずれの座標系共に、Ｘ軸、Ｙ軸、及びＺ軸を有する。なお、図１では、車両座標系、路面座標系を構成する基準となる車両１００が自動二輪車であるものとして説明をしている。

　慣性座標系ｅ_oは、地球の水平面に固定された座標系であり、Ｚ軸を鉛直上向きすなわち重力方向と定義する。

　車両座標系ｅ_bは、水平路面上に車両が直立している状態において、水平面にＸ軸及びＹ軸が存在し、Ｘ軸が車両の前方向となるように車体に固定された座標系である。車両座標系ｅ_bは、車体のサスペンションが動くことによって、慣性座標系ｅ_oとの間のなす角度が変化する。サスペンションは、車輪（２，３）と車体１の間に設けられた緩衝装置である。

　路面座標系ｅ_rは、Ｙ軸を車両座標系ｅ_bのＹ軸と共通とし、後輪３と路面２００との接地点Ｐ３から、前輪２と路面２００との接地点Ｐ２の向きにＸ軸が一致するように、Ｙ軸の周りに車両座標系ｅ_bを回転させた座標系である。

　本明細書において、「ヨー角」とは、慣性座標系ｅ_oのＺ軸（ｅ_oz）の軸周りの回転角を表し、「ヨー角速度」とは、上記「ヨー角」の時間変化率を表し、「ヨー角加速度」とはヨー角速度の時間変化率を表す。また、本明細書において、「ピッチ角」とは、慣性座標系ｅ_oのＹ（ｅ_oy）軸の軸周りの回転角を表し、「ピッチ角速度」とは、上記「ピッチ角」の時間変化率を表し、「ピッチ角加速度」とはピッチ角速度の時間変化率を表す。また、本明細書において、「ロール角」とは、車両座標系ｅ_bのＸ軸（ｅ_bx）の軸周りの回転角を表し、「ロール角速度」とは、上記「ロール角」の時間変化率を表し、「ロール角加速度」とはロール角速度の時間変化率を表す。

　本明細書において、「上下方向」とは車両座標系ｅ_bのＺ軸（ｅ_bz）方向を指し、「前後方向」とは車両座標系ｅ_bのＸ軸（ｅ_bx）方向を指し、「左右方向」とは車両座標系ｅ_bのＹ軸（ｅ_by）方向を指す。

　ここで、ロール角、ロール角速度、ロール角加速度、ヨー角、ヨー角速度、ヨー角加速度、ピッチ角、ピッチ角速度、ピッチ角加速度、上下加速度、前後加速度、及び左右加速度を、それぞれ以下の符号で記載する。なお、各パラメータを表す符号上の１つのドットは１階時間微分を意味する。

　〈車両〉
　図２は、本実施形態に係る姿勢推定装置を備えた車両の模式図である。図２に示される車両１００は自動二輪車である。

　図２に示すように、車両１００は車体１を備える。車体１の前部には前輪２が取り付けられており、車体１の後部には後輪３が取り付けられている。また、車体１の中央部には、センサ群５が取り付けられている。センサ群５の詳細については、後述する。

　後輪３のホイールには、後輪３の回転速度を検出する後輪速度センサ７が取り付けられている。本実施形態では、後輪速度センサ７が「速度情報検出部」に対応する。

　車体１の前側の上部には、ハンドル１１が左右に揺動可能に設けられている。なお、図１に示す例では、ハンドル１１の近傍にナビゲーションシステム１２が設けられており、車体１の前部にはヘッドライト１４及びヘッドライト駆動装置１５が設けられている。ヘッドライト駆動装置１５はヘッドライト１４の向きを制御する。車体１の後部には、電子制御ユニット２０が設けられている。以下では、電子制御ユニット２０を、適宜「ＥＣＵ２０」と略記する。なお、ＥＣＵ２０やセンサ群５の配置位置は、図２の態様に限られない。

　センサ群５及び後輪速度センサ７の出力信号は、ＥＣＵ２０に与えられる。ＥＣＵ２０は、車体１の各部を制御すると共に、車体１のロール角及びピッチ角を推定し、これらの推定値を、例えばナビゲーションシステム１２やヘッドライト駆動装置１５に出力する。

　本実施形態では、センサ群５、後輪速度センサ７、及びＥＣＵ２０が姿勢推定装置を構成する。

　〈姿勢推定装置の構成〉
　図３は、姿勢推定装置の構成の一例を模式的に示すブロック図である。図３に示す姿勢推定装置１０は、センサ群５、後輪速度センサ７、ローパスフィルタ（３１，３２，３３，３４，３５，３６）、微分器（４１，４２，４３，４５）、及びカルマンフィルタ５０、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０を備える。カルマンフィルタ５０、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０の機能は、例えば、図２に示すＥＣＵ２０がプログラムに従って動作することによって実現される。カルマンフィルタ５０が「姿勢推定部」に対応する。

　センサ群５は、ロール角速度センサ２１、ヨー角速度センサ２２、ピッチ角速度センサ２３、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６を含む。

　ロール角速度センサ２１は、車体１のロール角速度を検出するように車体１に設けられている。ヨー角速度センサ２２は、車体１のヨー角速度を検出するように車体１に設けられている。ピッチ角速度センサ２３は、車体１のピッチ角速度を検出するように車体１に設けられている。本実施形態では、ロール角速度センサ２１が「第一角速度検出部」に対応し、ヨー角速度センサ２２が「第二角速度検出部」に対応し、ピッチ角速度センサ２３が「第三角速度検出部」に対応する。

　上下加速度センサ２４は、車体１の上下加速度を検出するように車体１に設けられている。前後加速度センサ２５は、車体１の前後加速度を検出するように車体１に設けられている。左右加速度センサ２６は、車体１の左右加速度を検出するように車体１に設けられている。本実施形態では、上下加速度センサ２４が「第一加速度検出部」に対応し、前後加速度センサ２５が「第二加速度検出部」に対応し、左右加速度センサ２６が「第三加速度検出部」に対応する。

　なお、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６は、一例として互いに直交する方向における加速度を検出している。これは必須ではなく、上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６は、少なくとも、それぞれ、互いに異なる３つの方向における加速度を検出することができればよい。

　ロール角速度センサ２１の出力信号は、ローパスフィルタ３１を通してロール角速度としてカルマンフィルタ５０及び微分器４１に与えられる。ローパスフィルタ３１は、ロール角速度センサ２１の出力信号のノイズを除去する。微分器４１は、ロール角速度の微分値をロール角加速度としてカルマンフィルタ５０に与える。

　ヨー角速度センサ２２の出力信号は、ローパスフィルタ３２を通してヨー角速度としてカルマンフィルタ５０及び微分器４２に与えられる。ローパスフィルタ３２は、ヨー角速度センサ２２の出力信号のノイズを除去する。微分器４２は、ヨー角速度の微分値をヨー角加速度としてカルマンフィルタ５０に与える。

　ピッチ角速度センサ２３の出力信号は、ローパスフィルタ３３を通してピッチ角速度としてカルマンフィルタ５０及び微分器４３に与えられる。ローパスフィルタ３３は、ピッチ角速度センサ２３の出力信号のノイズを除去する。微分器４３は、ピッチ角速度の微分値をピッチ角加速度としてカルマンフィルタ５０に与える。

　上下加速度センサ２４の出力信号は、ローパスフィルタ３４を通して上下加速度としてカルマンフィルタ５０に与えられる。前後加速度センサ２５の出力信号は、ローパスフィルタ３５を通して前後加速度としてカルマンフィルタ５０に与えられる。左右加速度センサ２６の出力信号は、ローパスフィルタ３６を通して左右加速度としてカルマンフィルタ５０に与えられる。

　ローパスフィルタ（３１，３２，３３，３４，３５，３６）の各々の周波数特性は、対応するセンサ（２１，２２，２３，２４，２５，２６）の出力特性に応じて設定される。より具体的には、センサ（２１，２２，２３，２４，２５，２６）の出力信号に含まれるノイズ信号の周波数特性は、設計段階において予め特定することが可能である。ローパスフィルタ（３１，３２，３３，３４，３５，３６）はそのようなノイズ信号を遮断し、必要とされるセンサ（２１，２２，２３，２４，２５，２６）の検出信号を通過させるように設計され得る。

　後輪速度センサ７の出力信号は、後輪速度としてカルマンフィルタ５０に与えられる。後輪速度は、路面と後輪３のタイヤとの間に滑りが生じないと仮定した場合のタイヤの最外周の回転速度であり、実際には後輪速度センサ７の出力信号およびタイヤのサイズに基づいて算出される。説明を簡略化するために、図３では、後輪速度センサ７から後輪速度を示す信号が出力されるものとして図示している。

　上述したように、カルマンフィルタ５０は、ロール角速度、ロール角加速度、ヨー加速度、ヨー角加速度、ピッチ角速度、ピッチ角加速度、上下加速度、前後加速度、左右加速度、及び後輪速度に関する検出値が入力される。そして、カルマンフィルタ５０は、これらの値に基づいて、ロール角、ピッチ角、前後方向車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット、ピッチ角速度センサオフセット、及び上下加速度センサオフセットを推定して出力する。

　なお、本実施形態の姿勢推定装置１０は、仰俯角推定部８０を備え、仰俯角推定部８０による仰俯角ζの推定値もカルマンフィルタ５０に入力される構成である。

　ロール角速度センサオフセットは、ロール角速度センサ２１のオフセット誤差である。ヨー角速度センサオフセットは、ヨー角速度センサ２２のオフセット誤差である。ピッチ角速度センサオフセットは、ピッチ角速度センサ２３のオフセット誤差である。上下加速度センサオフセットは、上下加速度センサ２４のオフセット誤差である。

　後輪速度センサ７から検出された後輪速度、カルマンフィルタ５０が推定した前後方向車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセット、ピッチ角速度センサオフセット、及び上下加速度センサオフセット、並びに仰俯角推定部８０が推定した仰俯角、及び仰俯角の１階微分値を、以下の符号で表記するものとする。

　図４は、仰俯角ζの説明をするための図面である。仰俯角ζは、車両１００のサスペンションのストローク量が発生しているときの、路面座標系ｅ_rと車両座標系ｅ_bのなす角と定義される。ここでは、仰俯角ζを、車両座標系ｅ_bのＸ軸ｅ_bxと路面座標系のＸ軸ｅ_rxとのなす角度として説明する。

　本実施形態に係るカルマンフィルタ５０では、ロール角速度センサ２１、ヨー角速度センサ２２、ピッチ角速度センサ２３、上下加速度センサ２４、前後加速度センサ２５、左右加速度センサ２６、及び後輪速度センサ７を用いて、後述する式（４３）、（４４）に記載された７つの関係式が導出される。これらの関係式を用いて、ピッチ角θ、ロール角φ、及び前後方向車両速度Ｖ_xという３つのパラメータが推定される。すなわち、関係式の数と推定される出力の数との間に冗長性が存在する。言い換えると、関係式の数が推定する出力の数よりも多い。この冗長性を利用して、カルマンフィルタ５０は、更にロール角速度センサオフセットｂ_ro、ヨー角速度センサオフセットｂ_ya、ピッチ角速度センサオフセットｂ_pi、及び上下加速度センサオフセットｂ_zを推定することが可能となる。更に、図３に示されるように、姿勢推定装置１０が仰俯角推定部８０を備える場合には、この仰俯角ζの推定値を考慮して、各値を推定することが可能である。

　〈カルマンフィルタの構成〉
　図５は、カルマンフィルタ５０の構成の一例を模式的に示すブロック図である。図５に示すカルマンフィルタ５０では、以下に説明する車両１００の運動学モデルが用いられる。

　図５において、カルマンフィルタ５０は、システム方程式演算部５１、観測方程式演算部５２、減算部５３、加算部５４、積分部５５、及びカルマンゲイン演算部５６を含んで構成される。システム方程式演算部５１で演算される対象となる方程式であるシステム方程式は、関数ｆ（ｘ，ｕ）を含む。観測方程式演算部５２で演算される対象となる方程式である観測方程式は、関数ｈ（ｘ，ｕ）を含む。ガルマンゲイン演算部５６は、７次のカルマンゲインＫを含む。カルマンフィルタ５０を構成する各演算部（５１，５２，５３，５４，５５，５６）は、例えば、予め用意されたプログラムをＥＣＵ２０が実行することによって実現される。ただし、各演算部（５１，５２，５３，５４，５５，５６）の一部又は全部を、独立したハードウェアとして実装しても構わない。

　現在の推定動作において、システム方程式に含まれる関数ｆ（ｘ，ｕ）の入力パラメータｕとして、ロール角速度ω_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、及び特性方程式の検出値と、各算出値との差に基づいてカルマンゲインＫが算出される。なお、後述では、この特性方程式の検出値を０として説明するが、時間に応じて変化しない定数であれば０には限られない。

　この運動学モデルのシステム方程式ｆ（ｘ，ｕ）、及び観測方程式ｈ（ｘ，ｕ）を導出することにより、入力パラメータｕ，ｙと出力パラメータｘとの関係式を導くことができる。

　〈システム方程式及び観測方程式の導出〉
　本実施形態では、運動学モデルを簡単化するために以下の点を仮定する。

　（ａ）後輪３と路面との間に回転方向の滑りは生じない。
　（ｂ）後輪３の横滑り速度は０である。
　（ｃ）路面は平坦である。なお、ここでいう「平坦」とは凹凸がなく平面であるという意味であり、傾斜していてもよい。

　これらの仮定（ａ）～（ｃ）に基づいて以下のようにして運動学モデル式を導出する。なお、ヨー角の微分値、ピッチ角の微分値、及びロール角の微分値をそれぞれ以下の符号で記載する。

　まず、一般的なオイラー角と角速度との関係式から、次式が成立する。

　図６はセンサ群５の取り付け位置を説明するための図である。図６において（ａ）に車両１００の左側面を示し、（ｂ）に車両１００の正面を示す。図７は、後輪３の模式的な断面図である。図８はセンサ群５の取り付け位置をベクトルで表した図である。

　図６において、センサ群５の取り付け位置をＰＳとする。取り付け位置ＰＳから後輪３の中心までの水平距離をＬとし、路面２００から取り付け位置ＰＳまでの高さをｈとする。

　以下、慣性座標系の原点Ｏを基準とするセンサ群５の取り付け位置ＰＳの位置ベクトル、慣性座標系の原点Ｏを基準とする後輪３の接地点Ｐ３の位置ベクトル、後輪３の接地点Ｐ３からセンサ群５の取り付け位置ＰＳまでのベクトルを、並びに各ベクトルの２階微分ベクトルをそれぞれ以下の符号で示す。なお、各パラメータを表す符号の上の２つのドットは、２階時間微分を意味する。

　このとき、以下の（２）式が成立する。

　ここで、重力加速度ベクトル、及び取り付け位置ＰＳにおいて検出される加速度ベクトルを、それぞれ以下の符号で表す。この加速度ベクトルＧは、取り付け位置ＰＳに設置されている上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６により検出される。

　加速度ベクトルＧは、以下の（３）式によって得られる。

　以下、上記（３）式の右辺を算出する。図８に示すベクトルρは、次式で表される。

　上述したように、ベクトルｅ_bは車両座標系の基底ベクトルであり、ｅ_bxは車体１の前方向成分、ｅ_byは車体１の左方向成分、ｅ_bzは車体１の上方向成分に対応する。また、（４）式の右辺に示す行列ρは、図６及び図７より、次式で表される。なお、図７内において、後輪３の断面の半径をＲ_crとし、後輪３の半径をＲ_eとしている。また、上述したように、車体１のロール角をφで表している。

　なお、車両１００のサスペンションの動きを考慮すると、厳密にはＬやｈは変化するが、その変化の値は、Ｌやｈの値に対して十分小さいので、Ｌやｈの値は一定であると近似することができる。

　上式（５）からベクトルρの２階微分ベクトルは次式のように求まる。

　上式（６）において、ａ_x、ａ_y及びａ_zは関数である。関数ａ_x，ａ_y，ａ_zは上式（４）及び（５）を計算することにより求めることができる。上式（５）から上式（６）への変形の際に上式（１）を用いることにより、ロール角φの微分値、ヨー角ψの微分値、及びピッチ角θの微分値を消去することができる。

　次に、車両１００の横滑り速度をＶ_yとする。図８の位置ベクトルｒ₀の１階微分ベクトルは、車両速度Ｖ_x及び横滑り速度Ｖ_yを用いて次式のように表される。なお、上述したように、ベクトルｅ_bは車両座標系の基底ベクトルである。なお、本実施形態では、路面２００
の勾配やサスペンションのストロークに伴う仰俯角も推定に考慮するため、車両１００のＺ方向についても速度Ｖ_zが発生する。つまり、図８の位置ベクトルｒ₀の１階微分ベクトルは、Ｖ_x、Ｖ_y、及びＶ_zを用いて次式のように表される。

　仮定（ｂ）からＶ_ｙ＝０とし、上式（７）を１階微分すると、位置ベクトルｒの２階微分ベクトルは次式のように表される。

　上述したように、仰俯角ζは、車両座標系ｅ_bのＸ軸と路面座標系ｅ_rのＸ軸のなす角度に対応する。この定義より、下記式（９）が成立する。

　よって、上記（８）式は、下記式（１０）に書き換えられる。

　次に、重力加速度ベクトルは次式のように表される。なお、下記式（１１）の右辺内のｇは、重力加速度の大きさを表す。

　上式（３）、（６）、（１０）、及び（１１）より、取り付け位置ＰＳにおいて検出される加速度ベクトルＧは、以下の式のように表される。

　ここで、取り付け位置ＰＳで検出される加速度ベクトルＧは、前後加速度センサ２５により検出される前後加速度Ｇ_x、左右加速度センサ２６により検出される左右加速度Ｇ_y、及び上下加速度センサ２４により検出される上下加速度Ｇ_zを用いて次式のように表される。

　よって、上記（１２）及び（１３）式より、前後加速度Ｇ_x、左右加速度Ｇ_y、及び上下加速度Ｇ_zは、次式のように導かれる。

　次に、後輪速度ｖ_rと、車両速度Ｖ_xとの関係を求める。上記の仮定（ａ）から後輪３と路面２００との間に滑りがないので、後輪速度ｖ_rと、車両速度Ｖ_xとの間には次式の関係が成立する。

　上記（１５）式から、次式が求まる。

　上記（１）式、（１４）式、及び（１６）式から次式が求まる。

　ところで、図８に示すように、センサ群５の取り付け位置ＰＳから、車体１のメインボディの重心点ＭＰに向かうベクトルをＷとする。なお、この重心点ＭＰは、車体１のバネ下の重量を含まない重心点とする。このベクトルＷは、以下の式で表される。車体１のバネ下の重量は、例えば、タイヤ（２，３）の重量を含む。

　ここで、車体１の重量はほぼ左右対称に分布されるため、Ｗ_y＝０である。車体１のメインボディの質量をＭとし、慣性モーメントテンソルをＩとすると、基準点Ｏから見た車両１００の角運動量ベクトルＬは、以下のように算出される。なお、以下のベクトル演算式において、「・」は内積を表し、「×」は外積を表す。

　なお、上記（１９）式において、角速度ベクトルω及びテンソルＩは、それぞれ以下の式で定義される。

　なお、上記式（２１）において、円内にクロスが配置されている演算子は「テンソル積」を表している。

　上記（１９）式の両辺を時間微分することで、下記式を得る。

　（５）式から（６）式への変形と同様の変形により、ベクトルＷの２階微分ベクトル、並びに、テンソルＩと角加速度ベクトルωドット（角速度ベクトルωの１階微分ベクトル）の内積は、それぞれ下記式で表される。

　上式（２３）内のｗ_x、ｗ_y及びｗ_z、並びに上式（２４）内のｑ_x、ｑ_y及びｑ_zは関数である。これらの関数は、（５）式から（６）式への変形と同様の変形により求めることができる。

　次に、基準点Ｏから見た車両１００にかかる力のモーメントを計算する。図９に図示されるように、車両１００に作用する力として、重力ベクトル－Ｍｇ、空気抵抗や揚力等からなる空力ベクトルγ、前輪２の路面接地点Ｐ２にかかる力ベクトルＦＦ、及び後輪３の路面接地点Ｐ３にかかる力ベクトルＦＲが挙げられる。前輪２の路面接地点Ｐ２にかかる力ベクトルＦＦ、及び後輪３の路面接地点Ｐ３にかかる力ベクトルＦＲは、それぞれ下記式で表される。

　重力と空力は、車両１００の重心点ＭＰに対して作用するとする。空力ベクトルγは、以下の式のように表される。

　空力は、車両１００の速度Ｖと角速度ωに依存するので、以下の式で表される。

　なお、上記式（２７）では、仮定（ｂ）に基づくＶ_y＝０、及び式（９）を用いて変形を行った。

　図９に示すように、センサ群５の取り付け位置ＰＳから前輪２の路面接地点Ｐ２までのベクトルをσとすると、ベクトルσは以下の式で表される。なお、以下の式において、取り付け位置ＰＳから前輪２の中心までの水平距離をＬ’とする。

　基準点Ｏから見た車両１００にかかる力のモーメントベクトルＮは、下記式で表される。

　ここで、基準点Ｏから見た車両１００の角運動量ベクトルＬの１階微分量は、基準点Ｏから見た車両１００にかかる力のモーメントベクトルＮに一致する。すなわち、下記式が成立する。

　上記（３０）式は、基準点がどこにあっても成立するはずである。すなわち、上記（３０）式は、任意の位置ベクトルｒに対して成立するはずである。よって、上記（２２）式及び（２９）式を恒等式とすることで、以下の２式が成立する。

　式（３１）は、下記式（３３）のように変形できる。

　式（３３）を成分で表すと次式（３４）のようになる。

　上記（１４）式の第１成分の関係式から次の式が導かれる。

　この上記（３５）式を用いて、上記（３４）式は以下のように書き換えられる。

　上記（３２）式は、（３１）式を用いて以下のように変形できる。

　上記（３７）式を成分で表すと下記の式（４０）になる。

　上記（３６）式の第２成分及び第３成分を、（４０）式の第１成分に代入すると、ＦＦ_y、ＦＲ_y、ＦＦ_z、ＦＲ_zを消去することができる。すなわち、下記式（４１）を得る。

　上記（４１）式は、以下のように記載することが可能である。

　ここで、上記（４２）式の関数Ｑ_xの因子は、センサ群５から入力される値、及び姿勢推定装置１０で推定される値の他は、全て車両諸元等から一義的に定まる数である。つまり、上記（４２）式は、時間の経過に拘らず、必ず定数を示す関数Ｑ_xが存在することを意味する。この関数「Ｑ_x＝定数」を満たす方程式が「特性方程式」に対応する。

　ところで、上記（３５）式より、上記（１７）式は、以下のように書き換えることができる。

　また、上記（１４）式、（１６）式、及び（４２）式より、以下の式が導かれる。

　カルマンフィルタ５０は、システム方程式演算部５１において上記（４３）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（４４）式を観測方程式として用いて演算を行う。

　更に、ロール角速度オフセットｂ_ro、ヨー角速度オフセットｂ_ya、ピッチ角速度オフセットｂ_pi、上下加速度オフセットｂ_zが変化する場合でも、これらの値は車両１００の動きに比べて遅い。このため、ロール角速度オフセットｂ_roの微分値、ヨー角速度オフセットｂ_yaの微分値、ピッチ角速度オフセットｂ_piの微分値、及び上下加速度オフセットｂ_zの微分値をいずれも０とみなすことができる。

　また、上記（４３）式、（４４）式において、オフセット誤差を反映させた値に置き換えることで、それぞれ以下の式を得る。

　各オフセット誤差を考慮する場合においては、カルマンフィルタ５０は、システム方程式演算部５１において上記（４５）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（４６）式を観測方程式として用いて演算を行う。これらの式（４５），（４６）では、ロール角速度オフセットｂ_ro、ヨー角速度オフセットｂ_ya、ピッチ角速度オフセットｂ_pi、及び上下加速度オフセットｂ_zが考慮されている。

　カルマンフィルタ５０が、上記（４５）式をシステム方程式とし、上記（４６）式を観測方程式として演算を行うことで、ロール角φ、ピッチ角θ、車両速度Ｖ_x、ロール角速度オフセットｂ_ro、ヨー角速度オフセットｂ_ya、ピッチ角速度オフセットｂ_pi、及び上下加速度オフセットｂ_zを推定することができる。上記（４５）式の右辺が関数ｆ（ｘ，ｕ）に対応し、上記（４６）式の右辺が関数ｈ（ｘ，ｕ）に対応する。

　角速度センサでは、加速度センサに比べてオフセット誤差が発生しやすい。従って、本実施形態では、ロール角速度センサ２１のオフセット誤差（ロール角速度センサオフセットｂ_ro）、ヨー角速度センサ２２のオフセット誤差（ヨー角速度センサオフセットｂ_ya）、及びピッチ角速度センサ２３のオフセット誤差（ピッチ角速度オフセットｂ_pi）が推定される。ロール角速度オフセットｂ_ro、ヨー角速度オフセットｂ_ya、及びピッチ角速度オフセットｂ_piの推定値を次の推定動作の際に用いることにより、ロール角φ及びピッチ角θの推定精度が十分に向上する。

　また、３つの加速度センサ（上下加速度センサ２４、前後加速度センサ２５、及び左右加速度センサ２６）のオフセット誤差を推定することができれば、ロール角φ及びピッチ角θの推定精度がさらに向上することが期待される。しかしながら、３つの加速度センサのオフセット誤差を推定する場合には、可観測性を保つことはできない。そこで、本実施の形態では、次の理由により上下加速度センサ２４のオフセット誤差（上下加速度センサオフセットｂ_z）を推定する。車体１のロール角φが小さい範囲では、車体１の上下加速度はほとんど変化しない。このような上下加速度の検出値が上下加速度センサオフセットｂ_ｚにより変化した場合、ロール角φの推定の際に上下加速度の検出値の変化の影響が大きくなる。従って、上下加速度センサオフセットｂ_zの推定値を次の推定動作の際に用いることにより、ロール角φの小さい範囲での推定精度がさらに向上する。

　ただし、本発明のカルマンフィルタ５０が、どのセンサのオフセット誤差を推定する機能を有しているかについては、任意に選択することが可能である。

　〈荷重推定部の構成〉
　荷重推定部６０は、カルマンフィルタ５０において演算した結果に基づいて、車両１００の前輪２及び後輪３の少なくとも一方にかかる荷重を推定する処理を行う。

　上述した式（３６）及び（４０）を再掲する。

　式（３６）の各成分を、式（４０）の第２成分及び第３成分に代入すると、以下の式（４７）を得る。

　本実施形態において、車両１００は二輪車であるため、Ｌ＋Ｌ'≠０である。このとき、（３６）式の第２成分、第３成分、（４７）式、及び（４８）式から、ＦＦ_y、ＦＲ_y、ＦＦ_z、及びＦＲ_zについて解くことができる。Ｚ方向に係る前輪２の力ＦＦ_zと、Ｚ方向に係る後輪３の力ＦＲ_zは、下記式によって算出される。なお、下記の式においてＪ_F及びＪ_Rはいずれも関数である。

　荷重推定部６０は、カルマンフィルタ５０において演算した結果に基づいて、上記の演算を行うことで、前輪２にかかる力ＦＦと、後輪３にかかる力ＦＲとを算出する。

　〈サスペンションストローク量推定部の構成〉
　サスペンションストローク量推定部７０は、荷重推定部６０で行われた推定結果に基づいて、演算を行い、前輪２又は後輪３の少なくとも一方に設けられたサスペンションのストローク量を推定する。

　キャスター角をε_Fとすると、フロントフォーク１６にかかる力F_fはＦＦ_z cosε_F－ＦＦ_x sinε_Fで算出される。なお、キャスター角とは、前輪３の接地点Ｐ２から鉛直上向きに向かう方向と、フロントフォーク１６とのなす角である。

　リヤ（後輪）サスペンションの緩衝装置のストローク量をδ_R、後輪３の中心位置の変位量をδ_wとする。また、リヤサスペンションの緩衝装置のストローク量がδ_Rのときの、リヤアームの、水平方向からの垂れ角をε_R(δ_R)とする。キャスター角ε_F及びリヤアーム角ε_R(δ_w)を図１０に示す。

　このとき、サスペンションにかかる力Ｆ₁は、下記式で表される。

　なお、上記（５１）式において、κはレバ－比と呼ばれる諸元であり、設計図面から算出できる。図１１にδ_Rとδ_Wの関係の一例を模式的に示す。

　ここで、サスペンションストローク量推定部７０は、以下の推定処理を行う。

　この時、フロントフォークにかかる力F_f、及びサスペンションにかかる力Ｆ₁は、それぞれ以下の式（５４）、（５５）で算出される。

　図１２は、サスペンションの緩衝装置をモデル化した等価回路である。サスペンションは、図１２のようなバネｋ及びダンパーｃの並列モデルにて等価的に表現することが可能である。この等価回路に基づいて、運動方程式を立てると、次の式のようになる。なお、各サスペンションの緩衝装置のストローク量、各緩衝装置のバネ定数、各緩衝装置のダンパー係数、各バネにかけられているプリセット荷重を、以下の表に示す符号で定義する。

　なお、上記式（５６）及び（５７）では、（５４）式で記載されている関数Ｄ_F、及び（５５）式で記載されている関数Ｄ_Rの引数の記載を割愛している。

　上記式（５６）及び（５７）に記載された微分方程式を解くと、以下の式が導出される。

　なお、サスペンションストローク量推定部７０は、上述したように、式（５６）及び（５７）に記載された微分方程式を解くことで、サスペンションの緩衝装置のストローク量を算出するものとしても構わないし、時間の離散化処理を行うことでサスペンションの緩衝装置のストローク量を算出するものとしても構わない。

　更に、サスペンションストローク量推定部７０は、下記式に基づく演算を行うことで、リヤサスペンションの沈下量δ_Wを算出するものとしてもよい。

　〈仰俯角推定部の構成〉
　仰俯角推定部８０は、荷重推定部６０及びサスペンションストローク量推定部７０による推定結果に基づいて仰俯角ζを算出する。

　前輪のサスペンションが沈下量δ_Fだけ沈下し、後輪のサスペンションが沈下量δ_Wだけ沈下した場合、前輪の中心の位置ベクトルＦ_o、後輪の中心の位置ベクトルＲ_oは、それぞれ下記式で表すことができる。

　上記式（５９）及び（６０）より、仰俯角ζは、以下の式により算出される。

　なお、上記式（６１）において、ζ₀は、サスペンションのストローク量が０の場合における、車両座標系の基底ベクトルｅ_bと路面座標系の基底ベクトルｅ_rとのなす角である。

　〈勾配推定部の構成〉
　勾配推定部９０は、仰俯角推定部８０による推定結果、及びカルマンフィルタ５０による推定結果に基づいて路面の縦断勾配ηを算出する。

　路面の縦断勾配ηは、車両１００のピッチ角θ、ロール角φ、及び仰俯角ζによって以下の式で表される。

　よって、路面の縦断勾配ηは、以下の式で算出される。

　〈各パラメータの推定値の算出〉
　以下、本実施形態に係る姿勢推定装置を用いて精度よく車両１００の姿勢が推定できていることを、実施例を参照して説明する。

　図１３Ａ～図１３Ｊは、所定の路面上に車両１００を走行させながら、姿勢推定装置によって車両１００の姿勢を推定した結果を示すグラフである。図１３Ａ～図１３Ｉは、水平面と斜面とを有する路面上を縦横無尽に車両１００が走行した場合を模擬した結果である。また、図１３Ｊは、ほぼ水平面からなるサーキット場において、ライダーが積極的に車両１００から身を乗り出して旋回しながら走行している場合を模擬した結果である。

　図１３Ａは、オフセット誤差を有しないロール角速度センサ２１、ヨー角速度センサ２２、ピッチ角速度センサ２３、上下加速度センサ２４、前後加速度センサ２５、左右加速度センサ２６、及び後輪速度センサ７を取り付け、前記各センサの検出値に基づいて各パラメータの推定値を算出したときの結果である。なお、現実の状態を模擬する目的で、カルマンフィルタ５０に入力される各センサからの検出値には白色ノイズが重畳されている。

　図１３Ａによれば、カルマンフィルタ５０により、ピッチ角θ、ロール角φ、及び前後方向車両速度Ｖ_x共に、真値に近い値が推定できていることが分かる。

　図１３Ｂは、図１３Ａでカルマンフィルタ５０で推定された推定値に基づいて、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０が、それぞれの推定値を算出したときの結果を示すグラフである。図１３Ｂによれば、サスペンションストローク量推定部７０により真値に近いフロントフォーク縮み量が推定されていることが分かる。図１３Ｂによれば、各推定部により真値に近いフロントフォーク縮み量、後輪トラベル量、及び路面勾配が推定できていることが分かる。

　図１３Ｃは、所定のタイミングでピッチ角速度センサ２３の検出値に、所定のオフセット誤差（ここでは１deg／秒）を重畳させたときの、各推定値の結果を示すグラフである。なお、このシミュレーションにおいても、カルマンフィルタ５０に入力される各センサからの検出値には白色ノイズが重畳されている。

　図１３Ｃによれば、ピッチ角速度センサオフセットｂ_piの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。

　図１３Ｄ及び図１３Ｅは、図１３Ｃと同じ状況において、ピッチ角、ロール角、及び車体速度の推定結果の精度を示すグラフである。図１３Ｅは、図１３Ｄに示す結果の表示方法を異ならせたものであり、推定値と真値との差分の絶対値をグラフ化したものである。図１３Ｄ及び図１３Ｅによれば、ピッチ角速度センサ２３にオフセット誤差が発生している場合において、ピッチ角速度センサ２３のオフセットを推定した場合の方が、オフセットの推定を行わなかった場合と比較して、ピッチ角θ、ロール角φ、及び前後方向車両速度Ｖ_xについて、真値に近い値が推定できていることが確認される。

　図１３Ｆ～図１３Ｉは、更に、ピッチ角速度センサ２３以外のセンサに対してもオフセット誤差を発生させた場合の推定結果を示すグラフである。図１３Ｆは、所定の時間経過後にロール角速度センサ２１に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１３Ｇは、所定の時間経過後にヨー角速度センサ２２に対してオフセットを１deg／秒重畳させた場合の各推定値に対応する。図１３Ｈは、所定の時間経過後に上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させた場合の各推定値に対応する。

　また、図１３Ｉは、全てのセンサに対して複合的にオフセットを重畳させた場合の各推定値に対応する。具体的には、４０秒後にロール角速度センサ２１に１deg／秒重畳させ、５０秒後にピッチ角速度センサ２３に１deg／秒重畳させ、６０秒後にヨー角速度センサ２２に１deg／秒重畳させ、７０秒後に上下加速度センサ２４に対してオフセットを１００ｍＧ重畳させた場合の結果である。

　図１３Ｆ～図１３Ｉによれば、いずれの場合においても、各センサオフセットの値を、他のオフセットに干渉させることなく、精度よく推定できていることが確認される。

　また、図１３Ｊに示すように、ライダーが激しい運転をするような状況においても、各角度の値及びオフセットの値を、精度よく推定できていることが確認される。なお、図１３Ｊにおいても、推定値を実線で表し、真値を点線で表している。

　〈作用〉
　本実施の形態に係る姿勢推定装置１０によれば、ピッチ角θ、ロール角φ、及び車両速度Ｖ_xとともにロール角速度センサオフセットｂ_ro、ヨー角速度センサオフセットｂ_ya、ピッチ角速度センサオフセットｂ_pi、及び上下加速度センサオフセットｂ_zが推定され、各推定値が次の推定動作の際に用いられる。

　特に、加速度センサに比べてオフセットが発生しやすい角速度センサ（２１，２２，２３）のオフセット誤差の推定値が次の推定動作に用いられる。それにより、ピッチ角θ、ロール角φ、及び車両速度Ｖ_xの推定精度が十分に向上する。更に、本実施形態に係る姿勢推定装置によれば、上下加速度センサ２４のオフセット誤差の推定値が次の推定動作の際に用いられる。それにより、ロール角φの小さい範囲での推定精度がさらに向上する。

　また、本実施の形態に係る姿勢推定装置１０によれば、後輪速度センサ７により検出される後輪速度ｖ_rの検出値が推定動作に用いられる。それにより、車両１００が小さい半径で低速で旋回する場合でも、ピッチ角θ、ロール角φ、車両速度Ｖ_x、ロール角速度センサオフセットｂ_ro、ヨー角速度センサオフセットｂ_ya、ピッチ角速度センサオフセットｂ_pi、及び上下加速度センサオフセットｂ_zを高い精度で推定することが可能となる。

　このように、姿勢推定装置１０によれば、車体１の姿勢が高い精度で推定される。それにより、例えば、ＥＣＵ２０により推定された車体１の姿勢に関する情報に基づいてナビゲーションシステム１２やヘッドライト駆動装置１５を高精度に制御することが可能となる。また、この推定された車体１の姿勢に関する情報に基づいて、車両１００の駆動力、制動力、サスペンションの制御を行うことができる。

　［別実施形態］
　以下、別実施形態について説明する。

　〈１〉　上述した実施形態では、カルマンフィルタ５０は上下加速度オフセットｂ_zを推定するものとして説明した。しかし、カルマンフィルタ５０は、上下加速度オフセットｂ_zに代えて、前後加速度オフセットｂ_xを推定するものとしても構わない。この場合、カルマンフィルタ５０は、システム方程式演算部５１において上記（４５）式に代えて、以下の（６４）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（４６）式に代えて、以下の（６５）式を観測方程式として用いて演算を行う。

　図１４は、図１３Ａ～図１３Ｊと同様に、所定の路面上に車両１００を走行させながら、姿勢推定装置によって車両１００の姿勢を推定した結果を示すグラフである。図１４は、所定の時間経過後に、前後加速度センサ２５の検出値に、所定のオフセット誤差（ここでは１００ｍＧ）を重畳させたときの、各推定値の結果を示している。図１４においても、図１３Ａ～図１３Ｊと同様に、推定値を実線で表し、真値を点線で表している。

　図１４によれば、前後角速度センサオフセットｂ_xの推定値においては、所定のタイミング経過後に推定値が上昇しており、真値に近い推定ができていることが分かる。また、他のセンサオフセットの推定値は０であり、これも真値に近い推定ができていることが示されている。

　また、同様に、カルマンフィルタ５０は、上下加速度オフセットｂ_zに代えて、左右加速度オフセットｂ_yを推定するものとしても構わない。この場合、カルマンフィルタ５０は、システム方程式演算部５１において上記（４５）式に代えて、以下の（６６）式をシステム方程式として用いて演算を行い、観測方程式演算部５２において上記（４６）式に代えて、以下の（６７）式を観測方程式として用いて演算を行う。

　〈２〉　上記実施形態では、姿勢推定装置１０が、荷重推定部６０、サスペンションストローク量推定部７０、仰俯角推定部８０、及び勾配推定部９０を備えているものとして説明した。しかし、姿勢推定装置１０は、これらの演算機能を有していないものとしても構わない。すなわち、図１５に示すように、姿勢推定装置１０が、センサ群５、後輪速度センサ７、ローパスフィルタ（３１，３２，３３，３４，３５，３６）、微分器（４１，４２，４３，４５）、及びカルマンフィルタ５０を備える構成であっても構わない。

　この場合、図３の構成とは異なり、姿勢推定装置１０は、仰俯角ζを推定する機能を有していないため、カルマンフィルタ５０には、仰俯角ζ及び仰俯角ζの１階微分値は入力されない。よって、カルマンフィルタ５０は、上述した各式において、ζ＝０とした式に置き換えて演算を行うものとすればよい。この構成によれば、車両１００において、サスペンションの動きを考慮せずに車体１の姿勢を推定することができる。

　〈３〉上述した実施形態では、後輪３に設けられた後輪速度センサ７からの情報が後輪速度としてカルマンフィルタ５０に入力されるものとして説明した。しかし、図１６に示すように、前輪２に設けられた、前輪速度センサ８からの情報が、前輪速度としてカルマンフィルタ５０に入力されるものとしても構わない。この場合、上述した式において、ベクトルρとベクトルσを入れ替えて計算すればよい。なお、計算上は、力ベクトルＦＦを後輪３の路面接地点Ｐ３にかかる力と読み替え、力ベクトルＦＲを前輪２の路面接地点Ｐ２にかかる力と読み替えることで、上記と同様の方法によって車体１の姿勢推定が可能で
ある。

　〈４〉　基準点Ｏから見た車両１００にかかる力のモーメントを計算する際に、空力γ＝０と近似しても構わない。この場合、上記（３３）式においてγの項が存在しなくなるが、（３３）式から（３４）式への変形の過程で、（２７）式の関係式を利用してγを他の因子の関数に置換しているため、やはり（３４）式と同じ式が導かれる。

　〈５〉　上記実施形態において、センサ群５は、ロール角速度センサ２１、ヨー角速度センサ２２、及びピッチ角速度センサ２３を備えるものとして説明した。しかし、これらの各角速度センサは、車両１００の上下方向、左右方向、及び前後方向とは異なる３以上の軸周りでの角速度を検出するセンサとしても構わない。この場合、各角速度センサの検出値を、幾何学的方法により、ロール角速度ω_ro、ピッチ角速度ω_pi、及びヨー角速度ω_yaに置換することが可能である。そして、置換された各値に基づいて、姿勢推定装置１０が上述した演算式で規定された演算を行うものとすればよい。

　同様に、上記実施形態において、センサ群５は、上下加速度センサ２４、前後加速度センサ２５、及び左右加速・BR>Xセンサ２６を備えるものとして説明した。しかし、これらの各加速度センサは、車両１００の上下方向、左右方向、前後方向とは異なる３以上の加速度センサとしても構わない。この場合、各加速度センサの検出値を、幾何学的方法により、上下加速度Ｇ_z、前後加速度Ｇ_x、及び左右加速度Ｇ_yに置換することが可能である。そして、置換された各値に基づいて、姿勢推定装置１０が上述した演算式で規定された演算を行うものとすればよい。

　〈６〉　上述した実施形態では、カルマンフィルタ５０の入力パラメータの一つとして、後輪速度ｖ_rが利用されるものとしたが、これに限定されず、車両１００の走行速度（車両速度）を測定することのできる速度センサにより検出される車両速度を、カルマンフィルタ５０の入力パラメータとしてもよい。一例として、ＧＰＳセンサからの情報を用いることができる。

　この場合において、カルマンフィルタ５０で推定された車両速度Ｖxに代えて、センサで検出された車両速度に関する情報が用いられるものとしても構わない。すなわち、カルマンフィルタは車両速度Ｖxを推定しないものとしても構わない。

　〈７〉　上記実施形態において、カルマンフィルタ５０はＥＣＵ２０及びプログラムによって実現されるものとしたが、これに限定されず、カルマンフィルタ５０の一部又は全ての機能が電子回路等のハードウェアにより実現されるものとしても構わない。

　〈８〉　上記実施形態におけるカルマンフィルタ５０に代えて、他の適応フィルタリング手法を用いても構わない。一例として、ＬＭＳ（最小平均二乗）適応フィルタやＨ∞フィルタリング等を用いることができる。

　〈９〉　上記実施形態では、姿勢推定装置１０が自動二輪車に適用されるものとして説明を行ったが、同様のモデリングが可能であれば、自動四輪車、自動三輪車等の他の車両、又は船舶等の種々の輸送機器に適用することができる。

　上記実施形態において、オフセット誤差は、各検出部における検出値と実際の値すなわち真値との差を示す値である。カルマンフィルタには、複数の検出部それぞれの検出値が入力される。カルマンフィルタは、ロール角の推定値及びピッチ角の推定値を出力する。さらに、カルマンフィルタは、入力される複数の検出値のうち少なくとも１つの検出値に対応する検出部におけるオフセット誤差の推定値を、算出する。カルマンフィルタは、現在の推定動作において入力される複数の検出値と、前回の推定動作において算出した推定値とを用いて、現在の推定動作における推定値を算出する構成となっている。

　上記実施形態において、姿勢推定装置は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値を入力とする。姿勢推定装置の出力は、前記移動体のロール角及びピッチ角の推定値に加えて、前記第一角速度検出部、前記第二角速度検出部、前記第三角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差のうち少なくとも一つを含む。

　姿勢推定部は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値のうち少なくとも１つと、前回の推定動作の推定値とを用いて、現在の中間推定値を算出する。姿勢推定部は、中間推定値を、前回の推定値と観測値との比較結果に基づいて補正する。観測値は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、及び、前記速度情報検出部の検出値のうち少なくとも１つとすることができる。上記例では、姿勢推定部は、前回の推定値と観測値との比較結果を示す値の一例として、カルマンゲインを算出している。上記中間推定値を、前回の推定値と観測値との比較結果を示す値を用いて補正した値が、現在の推定動作の推定値として出力される。

　　　　１　　　：　　　車体
　　　　２　　　：　　　前輪
　　　　３　　　：　　　後輪
　　　　５　　　：　　　センサ群
　　　　７　　　：　　　後輪速度センサ
　　　　８　　　：　　　前輪速度センサ
　　　１０　　　：　　　姿勢推定装置
　　　１１　　　：　　　ハンドル
　　　１２　　　：　　　ナビゲーションシステム
　　　１４　　　：　　　ヘッドライト
　　　１５　　　：　　　ヘッドライト駆動装置
　　　１６　　　：　　　フロントフォーク
　　　２０　　　：　　　電子制御ユニット（ＥＣＵ）
　　　２１　　　：　　　ロール角速度センサ
　　　２２　　　：　　　ヨー角速度センサ
　　　２３　　　：　　　ピッチ角速度センサ
　　　２４　　　：　　　上下加速度センサ
　　　２５　　　：　　　前後加速度センサ
　　　２６　　　：　　　左右加速度センサ
　　　３１，３２，３３，３４，３５，３６　　　：　　　ローパスフィルタ
　　　４１，４２，４３，４５　　　：　　　微分器
　　　５０　　　：　　　カルマンフィルタ
　　　５１　　　：　　　システム方程式演算部
　　　５２　　　：　　　観測方程式演算部
　　　５３　　　：　　　減算部
　　　５４　　　：　　　加算部
　　　５５　　　：　　　積分部
　　　５６　　　：　　　カルマンゲイン演算部
　　　６０　　　：　　　荷重推定部
　　　７０　　　：　　　サスペンションストローク量推定部
　　　８０　　　：　　　仰俯角推定部
　　　９０　　　：　　　勾配推定部
　　１００　　　：　　　車両
　　２００　　　：　　　路面

Claims (15)

1. 　移動体の姿勢を推定する姿勢推定装置であって、
   　前記移動体の、第一軸の周りでの角速度である第一角速度を検出する第一角速度検出部と、
   　前記移動体の、前記第一軸とは異なる方向である第二軸の周りでの角速度である第二角速度を検出する第二角速度検出部と、
   　前記移動体の、前記第一軸及び前記第二軸とは異なる第三軸の周りでの角速度である第三角速度を検出する第三角速度検出部と、
   　前記移動体の、第一方向に関する加速度である第一加速度を検出する第一加速度検出部と、
   　前記移動体の、前記第一方向とは異なる第二方向に関する加速度である第二加速度を検出する第二加速度検出部と、
   　前記移動体の、前記第一方向及び前記第二方向とは異なる第三方向に関する加速度である第三加速度を検出する第三加速度検出部と、
   　前記移動体の進行方向の移動速度に関する情報を検出する速度情報検出部と、
   　前記移動体のロール角及びピッチ角を推定すると共に、前記第一角速度検出部、前記第二角速度検出部、前記第三角速度検出部、前記第一加速度検出部、前記第二加速度検出部、及び前記第三加速度検出部のうちの少なくとも一つのオフセット誤差を演算処理によって推定する姿勢推定部とを備え、
   　前記姿勢推定部は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値に基づいて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定することを特徴とする姿勢推定装置。
2. 　前記姿勢推定部は、前記第一角速度検出部、前記第二角速度検出部、及び前記第三角速度検出部のオフセット誤差を推定することを特徴とする請求項１に記載の姿勢推定装置。
3. 　前記姿勢推定部は、前記移動体の進行方向の移動速度を更に推定することを特徴とする請求項１又は２に記載の姿勢推定装置。
4. 　前記姿勢推定部は、前記第一加速度検出部、前記第二加速度検出部、又は前記第三加速度検出部のオフセット誤差を推定することを特徴とする請求項１～３のいずれか１項に記載の姿勢推定装置。
5. 　前記第一加速度検出部は、前記移動体の上下方向における加速度を検出し、
   　前記姿勢推定部は、前記第一加速度検出部のオフセット誤差を推定することを特徴とする請求項４に記載の姿勢推定装置。
6. 　前記第一角速度検出部は、前記移動体のロール角速度を検出し、
   　前記第二角速度検出部は、前記移動体のヨー加速度を検出し、
   　前記第三角速度検出部は、前記移動体のピッチ角速度を検出することを特徴とする請求項１～５のいずれか１項に記載の姿勢推定装置。
7. 　前記姿勢推定部は、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値の関係を用いて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定するカルマンフィルタを含むことを特徴とする請求項１～６のいずれか１項に記載の姿勢推定装置。
8. 　前記カルマンフィルタは、現在の推定動作において、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるピッチ角の推定値、及び前回の推定動作によるオフセット誤差の推定値を要素とする所定の関数の値が定数になる特性方程式を用いて、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定することを特徴とする請求項７に記載の姿勢推定装置。
9. 　前記特性方程式が、前記移動体の、前記第一軸の周りの回転運動方程式から導出された方程式であることを特徴とする請求項８に記載の姿勢推定装置。
10. 　前記移動体に備えられた前輪及び後輪の少なくとも一方にかかる荷重を推定する荷重推定部を備え、
    　前記荷重推定部は、前記第一角速度検出部の検出値、前記第二角速度検出部の検出値、前記第三角速度検出部の検出値、前記第一加速度検出部の検出値、前記第二加速度検出部の検出値、前記第三加速度検出部の検出値、前記速度情報検出部の検出値、並びに、前記姿勢推定部によって推定された前記移動体のロール角、前記移動体のピッチ角、及び前記少なくとも一つのオフセット誤差に基づいて前記荷重を推定することを特徴とする請求項１～９のいずれか１項に記載の姿勢推定装置。
11. 　前記移動体の前輪又は後輪の少なくとも一方に設けられたサスペンションのストローク量を推定するサスペンションストローク量推定部を備え、
    　前記荷重推定部は、前記移動体に設けられた前記前輪及び前記後輪の双方にかかる荷重を推定し、
    　前記サスペンションストローク量推定部は、前記荷重推定部によって推定された、前記前輪及び前記後輪の双方にかかる荷重の推定値に基づいて、前記サスペンションのストローク量を推定することを特徴とする請求項１０に記載の姿勢推定装置。
12. 　前記サスペンションストローク量推定部により推定された、前記サスペンションのストローク量の推定値に基づいて、前記移動体に固定された車体座標系の軸と、前記前輪又は前記後輪が接触している路面上に固定された路面座標系の軸との角度である仰俯角を推定する仰俯角推定部を備えたことを特徴とする請求項１１に記載の姿勢推定装置。
13. 　前記姿勢推定部は、前記仰俯角推定部により推定された前記仰俯角の推定値を考慮して、前記移動体のロール角及びピッチ角と、前記少なくとも一つのオフセット誤差とを推定することを特徴とする請求項１２に記載の姿勢推定装置。
14. 　前記仰俯角推定部により推定された前記仰俯角の推定値と、前記姿勢推定部により推定された前記移動体のロール角及びピッチ角の推定値とに基づいて、前記移動体が設置されている路面の縦断勾配を推定する勾配推定部を備えたことを特徴とする請求項１２又は１３に記載の姿勢推定装置。
15. 　前記移動体と、
    　請求項１～１４のいずれか１項に記載の姿勢推定装置とを備えたことを特徴とする輸送機器。

Images