JP5393432B2 - ロール角推定装置および輸送機器 - Google Patents

Description

本発明は、ロール角推定装置およびそれを備えた輸送機器に関する。

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

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

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

しかしながら、従来の推定装置では、高い精度で車両のロール角を推定することができない。それにより、例えば、車両が傾斜していないにもかかわらず、ヘッドライトの向きが傾くことがある。

本発明の目的は、移動体のロール角を高い精度で推定することが可能なロール角推定装置およびそれを備えた輸送機器を提供することである。

（１）第１の発明に係るロール角推定装置は、移動体のロール角を推定するロール角推定装置であって、少なくとも２つの方向に沿った第１および第２の軸の周りでの第１および第２の角速度をそれぞれ検出する第１および第２の角速度検出器と、少なくとも３つの方向における第１、第２および第３の加速度をそれぞれ検出する第１、第２および第３の加速度検出器と、移動体の進行方向の移動速度に関する情報を検出する速度情報検出器と、移動体のロール角を推定するとともに第１および第２の角速度検出器ならびに第１、第２および第３の加速度検出器のうち少なくとも１つのオフセット誤差を推定するように構成された推定部とを備え、推定部は、現在の推定動作において、第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、速度情報検出器の検出値、前回の推定動作によるロール角の推定値ならびに前回の推定動作によるオフセット誤差の推定値に基づいて、移動体のロール角および少なくとも１つのオフセット誤差を推定するものである。

そのロール角推定装置においては、少なくとも２つの方向に沿った第１および第２の軸の周りでの第１および第２の角速度がそれぞれ第１および第２の角速度検出器により検出される。また、少なくとも３つの方向における第１、第２および第３の加速度がそれぞれ第１、第２および第３の加速度検出器により検出される。さらに、移動体の進行方向の移動速度に関する情報が速度情報検出器により検出される。そして、推定部により移動体のロール角が推定されるとともに第１および第２の角速度検出器ならびに第１、第２および第３の加速度検出器のうち少なくとも１つの検出器による検出値のオフセット誤差が推定される。

この場合、推定部は、現在の推定動作において第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、速度情報検出器の検出値、前回の推定動作によるロール角の推定値ならびに前回の推定動作によるオフセット誤差の推定値に基づいて、移動体のロール角および少なくとも１つのオフセット誤差を推定する。

このように、ロール角とともにオフセット誤差が推定され、オフセット誤差の推定値が次の推定動作の際に用いられる。それにより、第１および第２の角速度検出器ならびに第１、第２および第３の加速度検出器のうち少なくとも１つのオフセット誤差によるロール角の推定精度の低下が補償される。その結果、ロール角を高い精度で推定することが可能となる。

（２）第１および第２の角速度検出器は、互いに異なる方向に沿った第１および第２の軸の周りでの第１および第２の角速度をそれぞれ検出し、第１、第２および第３の加速度検出器は、互いに異なる第１、第２および第３の方向における第１、第２および第３の加速度をそれぞれ検出してもよい。

この場合、互いに異なる方向に沿った第１および第２の軸の周りでの第１および第２の角速度がそれぞれ第１および第２の角速度検出器により検出される。また、互いに異なる第１、第２および第３の方向における第１、第２および第３の加速度がそれぞれ第１、第２および第３の加速度検出器により検出される。

（３）推定部は、少なくとも１つのオフセット誤差として、第１および第２の角速度検出器の少なくとも一方のオフセット誤差を推定してもよい。

角速度検出器では、加速度検出器に比べてオフセット誤差が発生しやすい。したがって、第１および第２の角速度検出器の少なくとも一方のオフセット誤差の推定値を次の推定動作の際に用いることにより、ロール角の推定精度が十分に向上する。

（４）推定部は、少なくとも１つのオフセット誤差として、第１および第２の角速度検出器のオフセット誤差を推定してもよい。

この場合、第１および第２の角速度検出器のオフセット誤差の推定値を次の推定動作の際に用いることにより、ロール角の推定精度がより向上する。

（５）推定部は、少なくとも１つのオフセット誤差として、第１、第２および第３の加速度検出器の少なくとも１つのオフセット誤差をさらに推定してもよい。

この場合、第１および第２の角速度検出器の少なくとも一方のオフセット誤差の推定値に加えて第１、第２および第３の加速度検出器の少なくとも１つのオフセット誤差の推定値を次の推定動作の際に用いることにより、ロール角の推定精度がさらに向上する。

（６）第１の加速度は移動体の上下方向における加速度であり、推定部は、第１の加速度検出器のオフセット誤差を推定してもよい。

移動体のロール角が小さい範囲では、移動体の上下方向の加速度はほとんど変化しない。このような上下方向の加速度の検出値が第１の加速度検出器のオフセット誤差により変化した場合、ロール角の推定の際に上下方向の加速度の検出値の変化の影響が大きくなる。したがって、第１の加速度検出器のオフセット誤差の推定値を次の推定動作の際に用いることにより、ロール角の小さい範囲での推定精度がさらに向上する。

（７）移動体は、前輪および後輪を有し、速度情報検出器は、情報として後輪の回転速度を検出する後輪回転速度検出器を含み、推定部は、移動体の移動速度をさらに推定するように構成され、現在の推定動作において第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、後輪回転速度検出器の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるオフセット誤差の推定値ならびに前回の推定動作による移動速度の推定値に基づいて、移動体のロール角、少なくとも１つのオフセット誤差および移動体の移動速度を推定してもよい。

移動体の移動速度と後輪の回転速度と移動体のロール角との間には一定の関係がある。そのため、後輪の回転速度の検出値およびロール角の推定値に基づいて移動体の移動速度を推定することができる。したがって、現在の推定動作において第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、後輪の回転速度の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるオフセット誤差の推定値ならびに前回の推定動作による移動速度の推定値に基づいて、移動体のロール角、少なくとも１つのオフセット誤差および移動体の移動速度を推定することができる。

移動体が小さい半径で低速で旋回する場合には、後輪の回転速度は前輪の回転速度に比べて移動体の移動速度に近い。したがって、後輪の回転速度の検出値を用いることにより、移動体のロール角、少なくとも１つのオフセット誤差および移動体の移動速度を高い精度で推定することが可能となる。

（８）移動体は、前輪および後輪を有し、速度情報検出器は、前輪の回転速度を検出する前輪回転速度検出器と、前輪回転速度検出器の検出値から後輪の回転速度を情報として推定する後輪回転速度推定部とを含み、推定部は、移動体の移動速度をさらに推定するように構成され、現在の推定動作において第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、後輪回転速度推定部の推定値、前回の推定動作によるロール角の推定値、前回の推定動作によるオフセット誤差の推定値ならびに前回の推定動作による移動速度の推定値に基づいて、移動体のロール角、少なくとも１つのオフセット誤差および移動体の移動速度を推定してもよい。

移動体が小さい半径で低速で旋回する場合には、前輪の旋回半径は後輪の旋回半径に比べて大きくなる。それにより、前輪回転速度検出器により検出される前輪の回転速度は後輪の回転速度に比べて高くなる。そこで、前輪回転速度検出器の検出値から後輪回転速度推定部により後輪の回転速度が推定される。それにより、前輪回転速度検出器が設けられた場合にも、後輪回転速度検出器が設けられた場合と同様に、移動体のロール角、少なくとも１つのオフセット誤差および移動体の移動速度を高い精度で推定することが可能となる。

（９）推定部は、現在の推定動作において第１および第２の角速度検出器の検出値、第１、第２および第３の加速度検出器の検出値、速度情報検出器の検出値、前回の推定動作によるロール角の推定値ならびに前回の推定動作によるオフセット誤差の推定値の関係を用いて移動体のロール角および少なくとも１つのオフセット誤差を推定するカルマンフィルタを含んでもよい。

この場合、カルマンフィルタのアルゴリズムを用いることにより推定部を容易に実現することができる。

（１０）第２の発明に係る輸送機器は、移動可能に構成された移動体と、移動体のロール角を推定する第１の発明に係るロール角推定装置と、ロール角推定装置により推定されたロール角を用いた処理を行う処理部とを備えたものである。

その輸送機器においては、第１の発明に係るロール角推定装置により移動体のロール角が高い精度で推定される。それにより、処理部によるロール角を用いた処理が高い精度で行われる。

本発明によれば、移動体のロール角を高い精度で推定することが可能となる。

本発明の一実施の形態に係るロール角推定装置を備えた車両の模式図である。 本発明の一実施の形態に係るロール角推定装置の構成を示すブロック図である。 カルマンフィルタの概念を示す図である。 センサ群の取り付け位置を説明するための図である。 後輪の断面図である。 センサ群の取り付け位置をベクトルで表した図である。 ロール角推定装置による各パラメータの推定値の時間遷移を示す図である。 各パラメータの推定値の時間遷移を示す図である。 本発明の他の実施の形態に係るロール角推定装置を備えた車両の模式図である。 車両の旋回時の前輪および後輪の旋回半径を示す図である。 前輪速度と後輪速度との関係の計算結果を示す図である。

以下、本発明の実施の形態に係るロール角推定装置を車両に適用した例を説明する。

（１）車両の構成
図１は本発明の一実施の形態に係るロール角推定装置を備えた車両の模式図である。図１の車両１００は自動二輪車である。

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

後輪３のホイールに後輪３の回転速度を検出する後輪速度センサ７が取り付けられる。

車体１の前側の上部には、ハンドル１１が左右に揺動可能に設けられる。ハンドル１１の近傍にナビゲーションシステム１２が設けられる。また、車体１の前部にはヘッドライト１４およびヘッドライト駆動装置１５が設けられる。ヘッドライト駆動装置１５はヘッドライト１４の向きを制御する。車体１の後部には、電子制御ユニット（以下、ＥＣＵと略記する）２０が設けられる。

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

本実施の形態では、センサ群５、後輪速度センサ７およびＥＣＵ２０がロール角推定装置を構成する。

（２）ロール角推定装置の構成
図２は本発明の一実施の形態に係るロール角推定装置の構成を示すブロック図である。

図２のロール角推定装置１０は、センサ群５、後輪速度センサ７、ローパスフィルタ６１〜６５、微分器７１，７２およびカルマンフィルタ８０により構成される。カルマンフィルタ８０の機能は、図１のＥＣＵ２０およびプログラムにより実現される。

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

ロール角速度センサ５１は、車体１のロール角速度を検出するように車体１に設けられる。ロール角速度は、車両１００の前後軸周りの角速度である。ヨー角速度センサ５２は、車体１のヨー角速度を検出するように車体１に設けられる。ヨー角速度は、車両１００の上下軸周りの角速度である。

上下加速度センサ５３は、車体１の上下加速度を検出するように車体１に設けられる。上下加速度は、車体１の上下方向の加速度である。前後加速度センサ５４は、車体１の前後加速度を検出するように車体１に設けられる。前後加速度は、車体１の前後方向の加速度である。左右加速度センサ５５は、車体１の左右加速度を検出するように車体１に設けられる。左右加速度は、車体１の左右方向の加速度である。

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

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

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

ここで、ロール角速度、ロール角加速度、ヨー角速度、ヨー角加速度、上下加速度、前後加速度、左右加速度および後輪速度を下表の符号で表す。なお、各パラメータを表す符号の上の１つのドットは１階微分を意味する。

カルマンフィルタ８０は、上記のパラメータに基づいてロール角、車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセットおよび上下加速度センサオフセットを推定して出力する。

ここで、車両１００の進行方向に平行な鉛直面を前後方向鉛直面と呼び、前後方向鉛直面に垂直な鉛直面を左右鉛直面と呼ぶ。ロール角は、左右鉛直面内で重力方向に対する車体１の傾斜角度である。車両速度は、車体１の進行方向の速度である。

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

車両速度、ロール角速度センサオフセット、ヨー角速度センサオフセットおよび上下加速度センサオフセットを下表の符号で表す。

本実施の形態に係るロール角推定装置１０では、ロール角速度センサ５１、ヨー角速度センサ５２、上下加速度センサ５３、前後加速度センサ５４、左右加速度センサ５５および後輪速度センサ７を用いて後述する式（１７），（１８）の５つの関係式が導出される。これらの関係式を用いてロール角φおよび車両速度Ｖ_ｘという２つのパラメータが推定される。すなわち、関係式の数と推定される出力の数との間に冗長性が存在する。言い換えると、関係式の数が推定する出力の数よりも多い。この冗長性を利用してロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚを推定することが可能となる。

（３）カルマンフィルタ８０の構成
図３はカルマンフィルタ８０の概念を示す図である。図３のカルマンフィルタ８０では、以下に説明する車両１００の運動学モデルが用いられる。

図３において、カルマンフィルタ８０は、システム方程式８ａ、加算機能８ｂ、積分機能８ｃ、観測方程式８ｄ、減算機能８ｅおよびカルマンゲイン８ｆにより構成される。システム方程式８ａは関数ｆ（ｘ，ｕ）を含み、観測方程式８ｄは関数ｈ（ｘ）を含み、カルマンゲイン８ｆは５次のカルマンゲインＫを含む。

現在の推定動作において、システム方程式８ａの入力パラメータｕとして、ロール角速度ω_ｒの検出値、ロール角加速度（ロール角速度ω_ｒの微分値）の検出値、ヨー角速度ω_ｙの検出値、ヨー角加速度（ヨー角速度ω_ｙの微分値）の検出値および前後加速度Ｇ_ｘの検出値が与えられる。また、システム方程式８ａの入力パラメータｘとして、前回の推定動作によるロール角φの推定値、車両速度Ｖ_ｘの推定値、ロール角速度センサオフセットｂ_ｒの推定値、ヨー角速度センサオフセットｂ_ｙの推定値および上下加速度センサオフセットｂ_ｚの推定値が与えられる。システム方程式８ａの出力は、ロール角φの微分値、車両速度Ｖ_ｘの微分値、ロール角速度センサオフセットｂ_ｒの微分値、ヨー角速度センサオフセットｂ_ｙの微分値および上下加速度センサオフセットｂ_ｚの微分値である。

ロール角φの微分値、車両速度Ｖ_ｘの微分値、ロール角速度センサオフセットｂ_ｒの微分値、ヨー角速度センサオフセットｂ_ｙの微分値および上下加速度センサオフセットｂ_ｚの微分値に前回の推定動作により得られた５次のカルマンゲインＫが加算される。

カルマンゲインＫが加算されたロール角φの微分値、車両速度Ｖ_ｘの微分値、ロール角速度センサオフセットｂ_ｒの微分値、ヨー角速度センサオフセットｂ_ｙの微分値および上下加速度センサオフセットｂ_ｚの微分値が積分されることにより、現在の推定動作によるロール角φの推定値、車両速度Ｖ_ｘの推定値、ロール角速度センサオフセットｂ_ｒの推定値、ヨー角速度センサオフセットｂ_ｙの推定値および上下加速度センサオフセットｂ_ｚの推定値が得られる。

また、観測方程式８ｄの入力パラメータｘとして、ロール角φの推定値、車両速度Ｖ_ｘの推定値、ロール角速度センサオフセットｂ_ｒの推定値、ヨー角速度センサオフセットｂ_ｙの推定値および上下加速度センサオフセットｂ_ｚの推定値が与えられる。観測方程式８ｄから上下加速度Ｇ_ｚの算出値、左右加速度Ｇ_ｙの算出値および後輪速度ｖ_ｒの算出値が得られる。

また、カルマンフィルタ８０には、入力パラメータｙとして上下加速度Ｇ_ｚの検出値、左右加速度Ｇ_ｙの検出値および後輪速度ｖ_ｒの検出値が与えられる。上下加速度Ｇ_ｚ、左右加速度Ｇ_ｙおよび後輪速度ｖ_ｒの検出値と算出値との差に基づいてカルマンゲインＫが算出される。

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

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

（ａ）車両１００はピッチングしない。

（ｂ）後輪３と路面との間に回転方向の滑りは生じない。

（ｃ）後輪３の横滑り速度は０である。

（ｄ）路面は平坦でかつ傾斜していない。

これらの仮定（ａ）〜（ｄ）に基づいて以下のようにして運動学モデル式を導出する。

ロール角、ロール角の微分値、ピッチ角、ピッチ角の微分値、ヨー角、ヨー角の微分値、ロール角速度、ヨー角速度およびピッチ角速度を下表の符号で表す。

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

上記の仮定（ａ）からピッチ角およびその微分値は０となる。したがって、上式（１）は次式のようになる。

上式（２）の２行目よりピッチ角速度ω_ｐを削除することができる。それにより、次式が求まる。

図４はセンサ群５の取り付け位置を説明するための図である。図４の左側に車両１００の左側面を示し、右側に車両１００の正面を示す。図５は後輪３の断面図である。図６はセンサ群５の取り付け位置をベクトルで表した図である。

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

図５においては、後輪３は路面ＲＤに対してロール角φ傾斜している。上記の仮定（ｄ）より路面ＲＤは傾斜していない。後輪３のタイヤＴＲと路面ＲＤとの接地点をＰとする。また、後輪３のタイヤＴＲの断面の半径をＲ_ｃｒとし、後輪３の半径をＲ_ｅとする。

図６において、慣性座標系の原点Ｏを基準とするセンサ群５の取り付け位置ＰＳの位置ベクトルをｒｖとし、慣性座標系の原点Ｏを基準とする接地点Ｐの位置ベクトルをｒｖ_０とし、接地点Ｐからセンサ群５の取り付け位置ＰＳまでのベクトルをρｖとする。この場合、ｒｖ＝ｒｖ_０＋ρｖが成り立つ。

ここで、位置ベクトルｒｖの２階微分ベクトル、位置ベクトルｒｖ_０の２階微分ベクトル、ベクトルρｖの２階微分ベクトルおよび重力加速度ベクトルを下表の符号で表す。なお、各パラメータを表す符号の上の２つのドットは２階微分を意味する。

ここで、取り付け位置ＰＳの上下加速度センサ５３、前後加速度センサ５４および左右加速度センサ５５により検出される加速度ベクトルをＧｖとする。加速度ベクトルをＧｖは、次式のように、位置ベクトルｒｖの２階微分ベクトルに重力加速度ベクトルを加算することにより得られる。

以下、上式（４）の右辺を算出する。図６のベクトルρｖは、次式で表される。

上式（５）において、［ｅ］＝［ｅ_１，ｅ_２，ｅ_３］である。ｅ_１、ｅ_２およびｅ_３は、車体１に固定された基底ベクトルであり、ｅ_１は車体１の前方向の基底ベクトルであり、ｅ_２は車体１の左方向の基底ベクトルであり、ｅ_３は車体１の垂直上方向の基底ベクトルである。また、ρは行列である。上式（５）の行列ρは、図４および図５より次式で表される。

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

上式（７）において、ａ_ｘ、ａ_ｙおよびａ_ｚは関数である。関数ａ_ｘ，ａ_ｙ，ａ_ｚは上式（５）および（６）を計算することにより求めることができる。上式（６）から上式（７）への変形の際に上式（３）を用いることによりロール角φの微分値およびヨー角ψの微分値を消去することができる。

次に、車両１００の横滑り速度をＶ_ｙとする。図６の位置ベクトルｒｖ_０の１階微分ベクトルは、車両速度Ｖ_ｘおよび横滑り速度をＶ_ｙを用いて次式のように表される。

上式（８）において、［ｅ０］＝［ｅ０_１，ｅ０_２，ｅ０_３］である。ｅ０_１、ｅ０_２およびｅ０_３は、慣性座標系の基底ベクトル［ｅ］＝［ｅ_１，ｅ_２，ｅ_３］をヨー角ψのみ回転させることにより得られる基底ベクトルである。

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

最後に、重力加速度ベクトルは次式のように表される。

上式（１０）においては、ｇは重力加速度の大きさである。

上式（４），（７），（９），（１０）から取り付け位置ＰＳにおいて検出される加速度ベクトルＧｖは次式のように表される。

ここで、取り付け位置ＰＳで検出される加速度ベクトルＧｖは、前後加速度センサ５４により検出される前後加速度Ｇ_ｘ、左右加速度センサ５５により検出される左右加速度Ｇ_ｙ、および上下加速度センサ５３により検出される上下加速度Ｇ_ｚを用いて次式のように表される。

Ｇｖ＝［ｅ］［Ｇ_ｘ，Ｇ_ｙ，Ｇ_ｚ］
したがって、上式（１１）より、前後加速度Ｇ_ｘ 、左右加速度Ｇ_ｙ、および上下加速度Ｇ_ｚは次式のようになる。

次に、後輪速度ｖ_ｒと車両速度Ｖ_ｘとの関係を求める。上記の仮定（ｂ）から後輪３と路面との間に滑りがないので、図５から後輪速度ｖ_ｒと車両速度Ｖ_ｘとの間には次式の関係が成り立つ。

上式（１３）から次式が求まる。

上式（３），（１２），（１４）から次式が求まる。

上式（１５）をシステム方程式として用い、上式（１６）を観測方程式として用いることにより、拡張カルマンフィルタを適用することが可能となる。

さらに、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚが変化する場合でも、それらの値の変化は車両１００の動きに比べて遅い。そのため、ロール角速度センサオフセットｂ_ｒの微分値、ヨー角速度センサオフセットｂ_ｙの微分値および上下加速度センサオフセットｂ_ｚの微分値を０とみなすことができる。

また、上式（１５），（１６）のロール角速度ω_ｒ、ヨー角速度ω_ｙおよび上下加速度Ｇ_ｚをそれぞれω_ｒ−ｂ_ｒ、ω_ｙ−ｂ_ｙおよびＧ_ｚ−ｂ_ｚで置き換えることにより次式を導出することができる。

上式（１７），（１８）では、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚが考慮されている。

上式（１７）をシステム方程式とし、上式（１８）を観測方程式として、拡張カルマンフィルタを適用することにより、ロール角φ、車両速度Ｖ_ｘ、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚを推定することができる。

ここで、上式（１７）の右辺が図３の関数ｆ（ｘ，ｕ）に相当し、上式（１８）の右辺が図３の関数ｈ（ｘ）に相当する。

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

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

（５）各パラメータの推定値の算出
図７および図８はロール角速度センサ５１がオフセット誤差を有する場合および有しない場合における各パラメータの推定値の時間遷移を示す図である。

ここでは、車両１００にオフセット誤差を有しないロール角速度センサ５１、ヨー角速度センサ５２、上下加速度センサ５３、前後加速度センサ５４、左右加速度センサ５５および後輪速度センサ７を取り付け、車両１００の実際の走行中に得られたロール角速度センサ５１、ヨー角速度センサ５２、上下加速度センサ５３、前後加速度センサ５４、左右加速度センサ５５および後輪速度センサ７の検出値に基づいて各パラメータの推定値を算出した。また、ロール角速度センサ５１の検出値に３ｄｅｇ／ｓのオフセット誤差を付加し、各パラメータの推定値を算出した。図７はロール角φおよび車両速度Ｖ_ｘの推定値を示し、図８はロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚの推定値を示す。

符号Ｌ０は、ロール角速度センサ５１の検出値にオフセット誤差が付加されない場合における各パラメータの推定値を示し、Ｌｏｆｆは、ロール角速度センサ５１の検出値にオフセット誤差が付加された場合における各パラメータの推定値を示す。

図７に示すように、ロール角速度センサ５１の検出値にオフセット誤差が付加された場合におけるロール角φの推定値は、短時間でロール角速度センサ５１の検出値にオフセット誤差が付加されない場合におけるロール角φの推定値に一致している。また、ロール角速度センサ５１の検出値にオフセット誤差が付加された場合における車両速度Ｖ_ｘの推定値は、ロール角速度センサ５１の検出値にオフセット誤差が付加されない場合における車両速度Ｖ_ｘの推定値にほぼ一致している。

図８に示すように、ロール角速度センサ５１検出値にオフセット誤差が付加された場合におけるロール角速度センサオフセットｂ_ｒの推定値とロール角速度センサ５１の検出値にオフセット誤差が付加されない場合におけるロール角速度センサオフセットｂ _ｒ の推定値との差はオフセット誤差の値（３ｄｅｇ／ｓ）に等しくなる。

なお、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚの推定値は、ロール角速度センサ５１の検出値にオフセット誤差が付加されない場合にも付加された場合にもほぼ０となった。

このように、本実施の形態に係るロール角推定装置１０によれば、ロール角速度センサ５１がオフセット誤差を有する場合でも、ロール角φを高精度で推定することができることがわかる。

（６）実施の形態の効果
本実施の形態に係るロール角推定装置１０によれば、ロール角φおよび車両速度Ｖ_ｘとともにロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚが推定され、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚの推定値が次の推定動作の際に用いられる。それにより、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚによるロール角φの推定精度の低下が補償される。その結果、ロール角φを高い精度で推定することが可能となる。

また、加速度センサに比べてオフセットが発生しやすいロール角速度センサ５１およびヨー角速度センサ５２のオフセット誤差の推定値が次の推定動作に用いられる。それにより、ロール角φの推定精度が十分に向上する。

さらに、上下加速度センサ５３のオフセット誤差の推定値が次の推定動作の際に用いられる。それにより、ロール角φの小さい範囲での推定精度がさらに向上する。

また、後輪速度センサ７により検出される後輪速度ｖ_ｒの検出値が推定動作に用いられる。それにより、車両１００が小さい半径で低速で旋回する場合でも、車体１のロール角φ、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙ、上下加速度センサオフセットｂ_ｚおよび車両速度Ｖ_ｘを高い精度で推定することが可能となる。

このように、ロール角推定装置１０により車体１のロール角φが高い精度で推定される。それにより、ＥＣＵ２０により推定されたロール角φに基づいてナビゲーションシステム１２およびヘッドライト駆動装置１５が正確に動作する。

（７）他の実施の形態
（ａ）上記実施の形態に係るロール角推定装置１０では、後輪３のホイールに後輪３の回転速度を検出する後輪速度センサ７が取り付けられるが、図９に示すように、後輪速度センサ７の代わりに前輪２のホイールに前輪２の回転速度を検出する前輪速度センサ６が取り付けられてもよい。図９は本発明の他の実施の形態に係るロール角推定装置を備えた車両の模式図である。すなわち、上記実施の形態では、後輪速度センサ７により検出される後輪速度ｖ_ｒが用いられるが、後輪速度センサ７により検出される後輪速度ｖ_ｒの代わりに、図９に示される前輪速度センサ６により検出される前輪速度から後輪速度ｖ_ｒが推定されてもよい。

車両１００が小さい半径で低速で旋回する場合には、前輪２の旋回半径は後輪３の旋回半径に比べて大きくなる。それにより、前輪速度センサ６により検出される前輪２の回転速度は後輪３の回転速度に比べて高くなる。そこで、前輪速度センサ６により検出される前輪２の回転速度からＥＣＵ２０により後輪速度ｖ_ｒが推定される。

図１０は車両１００の旋回時の前輪２および後輪３の旋回半径を示す図である。上記の仮定（ｃ）より後輪３の横滑り速度は０であるため、前輪２の旋回半径ｒ_ｆと後輪３の旋回半径ｒ_ｒとの間には次式の関係が成り立つ。

ここで、Ｌａは車両１００のホイールベース長である。上式（１９）より次式が得られる。

前輪２の接地点の速度Ｖ_ｆと後輪３の接地点の速度Ｖ_ｒとの比は前輪２の旋回半径ｒ_ｆと後輪３の旋回半径ｒ_ｒと等しくなる。したがって、次式が成り立つ。

さらに、車両１００が定常旋回していると仮定すると、後輪３の旋回半径ｒ_ｒは後輪３の接地点の速度Ｖ_ｒおよびロール角φを用いて次式で表される。

上式（２２）において、ｇは重力加速度の大きさである。上式（２２）を上式（２１）に代入すると、次式が導き出される。

上式（２３）から後輪３の接地点の速度Ｖ_ｒは次式のように表される。

上式（２４）は次式の条件が満たされる場合にのみ解を持つ。

車両１００の旋回半径が小さい場合には、上式（２５）の条件が満たされない場合が生じる。そのような場合にも解が存在するように、上式（２４）を次式のように修正する。

上式（２６）におけるロール角φとしては前回の推定動作におけるロール角φの推定値を用いる。

上式（２６）から前輪２の接地点の速度Ｖ_ｆと後輪３の接地点の速度Ｖ_ｒとの関係が求められる。したがって、前輪速度センサ６により検出される前輪２の回転速度（以下、前輪速度ｖ_ｆと呼ぶ）から後輪速度ｖ_ｒを算出することができる。

図１１は前輪速度ｖ_ｆと後輪速度ｖ_ｒとの関係の計算結果を示す図である。図１１の横軸は後輪速度ｖ_ｒを表し、縦軸は前輪速度ｖ_ｆを表す。

図１１に示すように、前輪速度センサ６により検出される前輪速度ｖ_ｆから後輪速度ｖ_ｒを推定することができる。したがって、前輪速度センサ６のみが設けられる場合にも、後輪速度センサ７が設けられた場合と同様に、車体１のロール角φ、車両速度Ｖ_ｘ、ロール角速度センサオフセットｂ_ｒ、ヨー角速度センサオフセットｂ_ｙおよび上下加速度センサオフセットｂ_ｚを高い精度で推定することができる。

（ｂ）上記実施の形態におけるロール角速度センサ５１およびヨー角速度センサ５２の代わりに車両１００の前後方向および上下方向とは異なる２以上の軸の周りでの角速度を検出する２以上の角速度センサを用いてもよい。この場合、幾何学的方法により２以上の角速度センサの検出値をロール角速度ω_ｒおよびヨー角速度ω_ｙに変換することが可能である。それにより、上式（１７），（１８）を用いることができる。

（ｃ）上記実施の形態におけるロール角速度センサ５１およびヨー角速度センサ５２に加えて、車両１００の左右方向の軸の周りでの角速度を検出する角速度センサを用いてもよい。この場合、ロール角φの推定精度がさらに向上する。

（ｄ）上記実施の形態における上下加速度センサ５３、前後加速度センサ５４および左右加速度センサ５５の代わりに車両１００の上下方向、前後方向および左右方向とは異なる３以上の方向の加速度を検出する３以上の加速度センサを用いてもよい。この場合、幾何学的方法により３以上の加速度センサの検出値を上下加速度Ｇ_ｚ、前後加速度Ｇ_ｘおよび左右加速度Ｇ_ｙに変換することが可能である。それにより、上式（１７），（１８）を用いることができる。

（ｅ）上記実施の形態では、カルマンフィルタ８０によりロール角速度センサオフセットｂ_ｒおよびヨー角速度センサオフセットｂ_ｙが推定されるが、これに限定されず、ロール角速度センサオフセットｂ_ｒのみが推定されてもよく、ヨー角速度センサオフセットｂ_ｙのみが推定されてもよい。ロール角速度センサオフセットｂ_ｒのみを推定する場合、その他のセンサオフセットの値をゼロとして計算を行えばよい。同様にヨー角速度センサオフセットｂ_ｙのみを推定する場合、その他のセンサオフセットの値をゼロとして計算を行えばよい。

（ｆ）上記実施の形態では、上下加速度センサオフセットｂ_ｚが推定されるが、これに限定されず、前後加速度センサ５４のオフセット誤差が推定されてもよく、または左右加速度センサ５５のオフセット誤差が推定されてもよい。

（ｇ）上記実施の形態では、カルマンフィルタ８０の入力パラメータの１つとして後輪速度ｖ_ｒが用いられるが、これに限定されず、車両１００の走行速度（車両速度）を測定可能な速度センサにより検出される車両速度をカルマンフィルタ８０の入力パラメータの１つとして用いてもよい。

（ｈ）上記実施の形態では、カルマンフィルタ８０により車両速度Ｖ_ｘが推定されるが、例えば、速度センサにより車両速度が検出される場合には、カルマンフィルタ８０により車両速度Ｖ_ｘが推定されなくてもよい。

（ｉ）上記実施の形態では、カルマンフィルタ８０がＥＣＵ２０およびプログラムにより実現されるが、これに限定されず、カルマンフィルタ８０の一部またはすべての機能が電子回路等のハードウエアにより実現されてもよい。

（ｊ）上記実施の形態におけるカルマンフィルタ８０の代わりに、他の適応フィルタリング手法を用いてもよい。例えば、ＬＭＳ（最小平均二乗）適応フィルタまたはＨ∞フィルタリング等を用いてもよい。

（ｋ）上記実施の形態では、ロール角推定装置１０が自動二輪車に適用されるが、これに限定されず、ロール角推定装置１０は自動四輪車もしくは自動三輪車等の他の車両または船舶等の種々の輸送機器に適用することができる。

（ｌ）上記実施の形態では、ロール角推定装置１０により推定されたロール角φがナビゲーションシステム１２およびヘッドライト駆動装置１５に用いられるが、これに限定されず、ロール角推定装置１０により推定されたロール角φは、輸送機器の他の制御等の種々の処理に用いることができる。

（８）請求項の各構成要素と実施の形態の各要素との対応
以下、請求項の各構成要素と実施の形態の各要素との対応の例について説明するが、本発明は下記の例に限定されない。

上記実施の形態では、車両１００の前後方向の軸が第１の軸の例であり、車両１００の上下方向の軸が第２の軸の例であり、ロール角速度ω_ｒが第１の角速度の例であり、ヨー角速度ω_ｙが第２の角速度の例であり、ロール角速度センサ５１が第１の角速度検出器の例であり、ヨー角速度センサ５２が第２の角速度検出器の例である。

また、車両１００の上下方向が第１の方向の例であり、車両１００の前後方向が第２の方向の例であり、車両１００の左右方向が第３の方向の例であり、上下加速度Ｇ_ｚが第１の加速度の例であり、前後加速度Ｇ_ｘが第２の加速度の例であり、左右加速度Ｇ_ｙが第３の加速度の例であり、上下加速度センサ５３が第１の加速度検出器の例であり、前後加速度センサ５４が第２の加速度検出器の例であり、左右加速度センサ５５が第３の加速度検出器の例である。

さらに、後輪速度ｖ_ｒまたは前輪速度ｖ_ｆが移動速度に関する情報の例であり、後輪速度センサ７または前輪速度センサ６が速度情報検出器の例であり、ＥＣＵ２０が推定部の例である。

また、車両１００または車体１が移動体の例であり、前輪２が前輪の例であり、後輪３が後輪の例であり、後輪速度センサ７が後輪回転速度推定部の例であり、前輪速度センサ６が前輪回転速度検出器の例であり、ＥＣＵ２０が後輪回転速度推定部の例である。

また、ローパスフィルタ６３，６４，６５が第１、第２および第３のローパスフィルタの例であり、ＥＣＵ２０およびプログラムにより実現されるカルマンフィルタ８０がカルマンフィルタの例である。ナビゲーションシステム１２またはヘッドライト駆動装置１５が処理部の例である。

請求項の各構成要素として、請求項に記載されている構成または機能を有する他の種々の要素を用いることもできる。

本発明は、輸送機器等のロール角を推定するために利用することができる。

１ 車体
２ 前輪
３ 後輪
５ センサ群
６ 前輪速度センサ
７，９ 後輪速度センサ
８ａ システム方程式
８ｂ 加算機能
８ｃ 積分機能
８ｄ 観測方程式
８ｅ 減算機能
８ｆ カルマンゲイン
１０ ロール角推定装置
１１ ハンドル
１２ ナビゲーションシステム
１４ ヘッドライト
１５ ヘッドライト駆動装置
２０ ＥＣＵ
５１ ロール角速度センサ
５２ ヨー角速度センサ
５３ 上下加速度センサ
５４ 前後加速度センサ
５５ 左右加速度センサ
６１〜６５ ローパスフィルタ
７１，７２ 微分器
８０ カルマンフィルタ
１００ 車両
ｂ_ｒ ロール角速度センサオフセット
ｂ_ｙ ヨー角速度センサオフセット
ｂ_ｚ 上下加速度センサオフセット
Ｇ_ｘ 前後加速度
Ｇ_ｙ 左右加速度
Ｇ_ｚ 上下加速度
ｒ_ｆ，ｒ_ｒ 旋回半径
ｕ 入力パラメータ
ｖ_ｆ 前輪速度
ｖ_ｒ 後輪速度
Ｖ_ｆ，Ｖ_ｒ 速度
Ｖ_ｘ 車両速度
ω_ｒ ロール角速度
ω_ｙ ヨー角速度
φ ロール角

Claims (10)

1. 移動体のロール角を推定するロール角推定装置であって、
   互いに異なる少なくとも２つの方向に沿った第１および第２の軸の周りでの第１および第２の角速度をそれぞれ検出する第１および第２の角速度検出器と、
   互いに異なる少なくとも３つの方向における第１、第２および第３の加速度をそれぞれ検出する第１、第２および第３の加速度検出器と、
   前記移動体の進行方向の移動速度に関する情報を検出する速度情報検出器と、
   前記移動体のロール角を推定するとともに前記第１および第２の角速度検出器ならびに前記第１、第２および第３の加速度検出器のうち少なくとも１つのオフセット誤差を推定するように構成された推定部とを備え、
   前記推定部は、現在の推定動作において前記第１および第２の角速度検出器の検出値、前記第１、第２および第３の加速度検出器の検出値、前記速度情報検出器の検出値、前回の推定動作によるロール角の推定値ならびに前回の推定動作によるオフセット誤差の推定値に基づいて、前記移動体のロール角および前記少なくとも１つのオフセット誤差を推定する、ロール角推定装置。
2. 前記第１および第２の角速度検出器は、互いに異なる２つの方向に沿った第１および第２の軸の周りでの第１および第２の角速度をそれぞれ検出し、
   前記第１、第２および第３の加速度検出器は、互いに異なる３つの方向における第１、第２および第３の加速度をそれぞれ検出する、請求項１記載のロール角推定装置。
3. 前記推定部は、前記少なくとも１つのオフセット誤差として、前記第１および第２の角速度検出器の少なくとも一方のオフセット誤差を推定する、請求項１または２記載のロール角推定装置。
4. 前記推定部は、前記少なくとも１つのオフセット誤差として、前記第１および第２の角速度検出器のオフセット誤差を推定する、請求項１〜３のいずれかに記載のロール角推定装置。
5. 前記推定部は、前記少なくとも１つのオフセット誤差として、前記第１、第２および第３の加速度検出器の少なくとも１つのオフセット誤差をさらに推定する、請求項４記載のロール角推定装置。
6. 前記第１の加速度は前記移動体の上下方向における加速度であり、
   前記推定部は、前記第１の加速度検出器のオフセット誤差を推定する、請求項５記載のロール角推定装置。
7. 前記移動体は、前輪および後輪を有し、
   前記速度情報検出器は、前記情報として前記後輪の回転速度を検出する後輪回転速度検出器を含み、
   前記推定部は、前記移動体の移動速度をさらに推定するように構成され、現在の推定動作において前記第１および第２の角速度検出器の検出値、前記第１、第２および第３の加速度検出器の検出値、前記後輪速度検出器の検出値、前回の推定動作によるロール角の推定値、前回の推定動作によるオフセット誤差の推定値ならびに前回の推定動作による移動速度の推定値に基づいて、前記移動体のロール角、前記少なくとも１つのオフセット誤差および前記移動体の移動速度を推定する、請求項１〜６のいずれかに記載のロール角推定装置。
8. 前記移動体は、前輪および後輪を有し、 前記速度情報検出器は、
   前記前輪の回転速度を検出する前輪回転速度検出器と、
   前記前輪回転速度検出器の検出値から前記後輪の回転速度を前記情報として推定する後輪回転速度推定部とを含み、
   前記推定部は、前記移動体の移動速度をさらに推定するように構成され、現在の推定動作において前記第１および第２の角速度検出器の検出値、前記第１、第２および第３の加速度検出器の検出値、前記後輪回転速度推定部の推定値、前回の推定動作によるロール角の推定値、前回の推定動作によるオフセット誤差の推定値ならびに前回の推定動作による移動速度の推定値に基づいて、前記移動体のロール角、前記少なくとも１つのオフセット誤差および前記移動体の移動速度を推定する、請求項１〜６のいずれかに記載のロール角推定装置。
9. 前記推定部は、現在の推定動作において前記第１および第２の角速度検出器の検出値、前記第１、第２および第３の加速度検出器の検出値、前記速度情報検出器の検出値、前回の推定動作によるロール角の推定値および前回の推定動作によるオフセット誤差の推定値の関係を用いて前記移動体のロール角および前記少なくとも１つのオフセット誤差を推定するカルマンフィルタを含む、請求項１〜８のいずれかに記載のロール角推定装置。
10. 移動可能に構成された移動体と、
    前記移動体のロール角を推定する請求項１〜９のいずれかに記載のロール角推定装置と、
    前記ロール角推定装置により推定されたロール角を用いた処理を行う処理部とを備えた、輸送機器。

Images