JP6922640B2

JP6922640B2 - 角速度導出装置および角速度導出方法

Info

Publication number: JP6922640B2
Application number: JP2017199792A
Authority: JP
Inventors: 近藤　高広; 高広近藤; 秀徳福嶋
Original assignee: JVCKenwood Corp
Current assignee: JVCKenwood Corp
Priority date: 2017-10-13
Filing date: 2017-10-13
Publication date: 2021-08-18
Anticipated expiration: 2037-10-13
Also published as: JP2019074377A

Description

本発明は、角速度導出技術に関し、特に３軸のジャイロセンサの出力値をもとに角速度を導出する角速度導出装置および角速度導出方法に関する。

車載用ナビゲーション装置における自動車の現在位置を検出する手段は、距離センサと角度センサを用いた自律（推測）航法と、衛星からの電波を受信するＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）受信機を使用する衛星航法とに分けられる。前者において、車両が傾斜するとジャイロセンサの感度が水平軸に対して低下する。そのため、３軸の加速度センサと速度センサの出力をもとに坂道の傾斜角、つまりピッチ角を導出し、ピッチ角でジャイロセンサの出力を補正する（例えば、特許文献１参照）。

特開平９−４２９７９号公報

１軸のジャイロセンサを使用する場合、ジャイロセンサのロールおよびピッチによって生じる感度誤差により、位置の検出精度が悪化する場合もある。これを解決するために３軸のジャイロセンサを使用することが有効である。ジャイロセンサに基づいて計算した角速度を積分すると、絶対姿勢角を算出することができるため、センサの傾きを考慮したヨー角を導出できる。しかし、各軸の角速度に含まれる誤差も絶対姿勢角に積分されるため、３軸のジャイロセンサを使用する場合に、角速度の導出精度の悪化を抑制することが望まれる。

本発明はこうした状況に鑑みてなされたものであり、その目的は、３軸のジャイロセンサを使用する場合に角速度の導出精度の悪化を抑制する技術を提供することにある。

上記課題を解決するために、本発明のある態様の角速度導出装置は、移動体に搭載可能な角速度導出装置であって、３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換する第１変換部と、第１変換部において変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新する更新部と、更新部により更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換する第２変換部と、第２変換部において変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出する角速度導出部と、角速度導出部において導出した角速度を出力する出力部とを備える。出力部は、角速度導出部において導出した角速度の絶対値の積分値がしきい値以上である場合、角速度導出部において導出した角速度の代わりに、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力する。

本発明の別の態様もまた、角速度導出装置である。この装置は、移動体に搭載可能な角速度導出装置であって、３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換する第１変換部と、第１変換部において変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新する更新部と、更新部により更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換する第２変換部と、第２変換部において変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出する角速度導出部と、角速度導出部において導出した角速度を出力する出力部と、温度を取得する取得部とを備える。出力部は、取得部において取得した温度の変化がしきい値以上である場合、角速度導出部において導出した角速度の代わりに、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力する。

本発明のさらに別の態様は、角速度導出方法である。この方法は、移動体に搭載可能な角速度導出装置における角速度導出方法であって、３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換するステップと、変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新するステップと、更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換するステップと、変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出するステップと、導出した角速度を出力するステップとを備える。出力するステップは、導出した角速度の絶対値の積分値がしきい値以上である場合、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力する。

本発明のさらに別の態様もまた、角速度導出方法である。この方法は、移動体に搭載可能な角速度導出装置における角速度導出方法であって、３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換するステップと、変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新するステップと、更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換するステップと、変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出するステップと、導出した角速度を出力するステップと、温度を取得するステップとを備える。出力するステップは、取得した温度の変化がしきい値以上である場合、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力する。

なお、以上の構成要素の任意の組合せ、本発明の表現を方法、装置、システム、記録媒体、コンピュータプログラムなどの間で変換したものもまた、本発明の態様として有効である。

本発明によれば、３軸のジャイロセンサを使用する場合に角速度の導出精度の悪化を抑制できる。

実施例１に係る角速度導出装置の構成を示す図である。実施例１に係る座標系を示す図である。図１の角速度導出装置による導出期間の調節手順を示すフローチャートである。実施例２に係る角速度導出装置の構成を示す図である。図４のピッチ角導出部における処理の概要を示す図である。図４の角速度導出装置による切替手順を示すフローチャートである。実施例３に係る角速度導出装置の構成を示す図である。図７の角速度導出装置による切替手順を示すフローチャートである。実施例４に係る角速度導出装置の構成を示す図である。図９の角速度導出装置による切替手順を示すフローチャートである。

（実施例１）
本発明を具体的に説明する前に、まず概要を述べる。本実施例は、車両等に搭載され、３軸のジャイロセンサを使用して角速度を導出する角速度導出装置に関する。角速度導出装置は、３軸の加速度センサから導出した初期姿勢を３軸のジャイロセンサの出力値によって更新し、更新された姿勢と前回の姿勢との差分から角速度を導出する。姿勢角は、３軸のジャイロセンサを車両等に取り付ける際に生じる傾きや、坂あるいはバンク等の車両状態による傾きを合成したものであり、最終的に地表水平面を基準とした上下／前後／左右の３軸に対して求められる。３軸のジャイロセンサの出力値である角速度は、センサ自体を基準とした座標系で表されており、地表水平面を基準とした座標系で表されていない。そのため、３軸のジャイロセンサから出力された角速度を、地表水平面を基準とした座標系に座標変換する必要がある。

そのため、３軸の加速度センサからの出力値をもとに初期姿勢を導出するとともに、３軸のジャイロセンサから出力された角速度によって初期姿勢を更新していくことによって、姿勢が導出される。ここで、姿勢の表現方法には、オイラー角、方向余弦行列、クォータニオンの３種類がある。初期姿勢と姿勢にはオイラー角が使用され、更新処理にはクォータニオンが使用される。このような３軸の加速度センサからの出力値をもとにした姿勢角の導出では、初期姿勢の導出と、姿勢の更新において、絶対姿勢角の誤差が蓄積するおそれがあり、このような誤差の蓄積によってヨー角速度の導出精度が低下する。本実施例では、初期姿勢の導出における誤差を抑制し、それによって姿勢角および角速度の導出精度の悪化を抑制することを目的とする。

車両が停止している場合、車両の並進加速度および向心加速度が０になるので初期姿勢を精度よく導出できるが、高速道路高架、橋等の人工的建造物の振動、乗降に伴う車両の振動の影響を受けた場合において、その加速度成分により初期姿勢の導出精度が悪化する。そのため、本実施例に係る角速度導出装置は、車両の速度がしきい値よりも低い場合に、姿勢の変化の程度に応じて初期姿勢の導出期間を調節する。

図１は、実施例１に係る角速度導出装置１００の構成を示す。角速度導出装置１００は、３軸加速度センサ１０、初期姿勢導出部１２、第１変換部１４、３軸ジャイロセンサ１６、更新部１８、第２変換部２０、角速度導出部２２、出力部２４、速度センサ２６、制御部２８を含む。また、制御部２８は、略停止判定部５０、分散値取得部５２、正規化部５４、調節部５６を含む。角速度導出装置１００は、図示しない車両に搭載可能である。

３軸加速度センサ１０は、３軸のそれぞれに対応した加速度を測定する。ここでは、３軸を説明するために図２を使用する。図２は、実施例１に係る座標系を示す。３軸として、ｘ軸、ｙ軸、ｚ軸からなる直交座標系が規定される。ｘ軸は車両２００の進行方向を向き、ｚ軸は車両２００の床面から下方への法線方向を向き、ｙ軸はｘ軸およびｚ軸に垂直方向を向く。３軸加速度センサ１０は、測定によってｘ軸方向の加速度ａ_ｘ、ｙ軸方向の加速度ａ_ｙ、ｘ軸方向の加速度ａ_ｚを取得する。図１に戻る。３軸加速度センサ１０は、これらを３軸加速度センサ１０の出力値として、初期姿勢導出部１２に出力する。

初期姿勢導出部１２は、３軸加速度センサ１０の出力値を入力する。初期姿勢導出部１２は、３軸加速度センサ１０の出力値をもとにオイラー角表現の初期姿勢を導出する。オイラー角表現では、ｘ軸まわりの回転角をロール角φと呼び、ｙ軸まわりの回転角をピッチ角θ、ｚ軸まわりの回転角をヨー角ψと呼ぶ。初期姿勢のφとθは次のように示される。

なお、ψの初期値は、任意の値でよく、例えば、「０」に設定される。

第１変換部１４は、初期姿勢導出部１２において導出したオイラー角表現の初期姿勢を方向余弦行列による初期姿勢に変換してから、方向余弦行列による初期姿勢をクォータニオンによる初期姿勢に変換する。オイラー角表現の初期姿勢は、方向余弦行列Ｅを用いて次のように変換される。

また、方向余弦行列Ｅの各成分は次のように示される。

クォータニオンは、回転方向の単位ベクトルと回転角の４成分で定義される。方向余弦行列Ｅによる初期姿勢は、クォータニオンによる初期姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４に次のように変換される。

第１変換部１４は、クォータニオンによる初期姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を更新部１８に出力する。

３軸ジャイロセンサ１６は、角速度ベクトルω＝［ｐｑｒ］^Ｔを出力値として順次出力する。各成分ｐ、ｑ、ｒは図２のように示される。３軸ジャイロセンサ１６は、出力値を更新部１８に順次出力する。

更新部１８は、次のクォータニオンによる姿勢の微分方程式を解く。

初期の段階において、更新部１８は、第１変換部１４において変換したクォータニオンによる初期姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を初期値として微分方程式の右辺に代入し、そのタイミングにおける角速度ベクトルの各成分ｐ、ｑ、ｒも微分方程式の右辺に代入する。微分方程式を解くことによって、更新部１８は、更新したクォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を導出する。

これに続いて、更新部１８は、導出したクォータニオンによる姿勢ｑ１、ｑ２、ｑ３、ｑ４を微分方程式の右辺に代入し、新たな角速度ベクトルの各成分ｐ、ｑ、ｒも微分方程式の右辺に代入する。微分方程式を解くことによって、更新部１８は、クォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を再度導出する。つまり、更新部１８は、３軸ジャイロセンサ１６の出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を更新する。更新部１８は、更新したクォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を第２変換部２０に出力する。

第２変換部２０は、更新部１８において更新したクォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４を方向余弦行列Ｅによる姿勢に変換してから、方向余弦行列Ｅによる姿勢をオイラー角表現の姿勢に変換する。更新したクォータニオンによる姿勢ｑ_１、ｑ_２、ｑ_３、ｑ_４は、方向余弦行列Ｅによる姿勢に次のように変換される。

方向余弦行列Ｅは、オイラー角表現の姿勢に次のように変換される。

第２変換部２０は、オイラー角表現の姿勢のうち、ヨー角ψを角速度導出部２２に出力する。

角速度導出部２２は、第２変換部２０において変換したオイラー角表現の姿勢のうち、ヨー角ψを入力する。角速度導出部２２は、ヨー角ψの時間変化ｔ［ｓｅｃ］をもとに角速度を導出する。例えば、着目する時刻ｎのヨー角をψ_ｎと示し、時刻ｎ−１のヨー角をψ_ｎ−１と示す場合、角速度は、（ψ_ｎ−ψ_ｎ−１）／ｔによって導出される。出力部２４は、角速度導出部２２において導出した角速度を出力する。

速度センサ２６は、ドライブシャフトの回転に対応して回転するスピードメータケーブルの中間に設置され、ドライブシャフトの回転に伴った速度パルス信号を出力する。また、速度センサ２６は、車両の移動に伴って出力される速度パルス信号を所定の期間ごとに計数することによって、周期的にパルス数を検出する。このパルス数は車両２００の速度に比例するので、速度センサ２６は、車両２００の速度を測定するといえる。速度センサ２６は、測定した速度を制御部２８に出力する。なお、車両２００の速度は、速度センサ２６ではなく、ＧＮＳＳ（ＧｌｏｂａｌＮａｖｉｇａｔｉｏｎＳａｔｅｌｌｉｔｅＳｙｓｔｅｍ）によって取得されてもよい。

制御部２８の略停止判定部５０は、速度センサ２６から速度の情報を入力する。略停止判定部５０は、速度の情報が所定期間にわたってしきい値よりも低い場合、例えば、「０」である場合に、車両２００が略停止状態であること判定する。略停止判定部５０は、車両２００が略停止状態であること判定した場合に、これを分散値取得部５２に通知する。ここで、しきい値は、車両２００がほとんど停止していると判断できる場合を特定可能な値に設定される。車両２００がほとんど停止していると判断できる場合は、例えば、速度センサ２６がパルスを所定期間検出できない場合に相当する。

分散値取得部５２は、略停止判定部５０から通知を入力した場合、３軸ジャイロセンサ１６の出力値の分散値Ｖａｒ＿ｇｙｒｏを取得する。分散値を計算する期間は、例えば１０秒間、サンプリング周波数は、例えば１０［Ｈｚ］であり、これによって車両２００の姿勢の短周期的な変化が検出される。分散値取得部５２は、分散値Ｖａｒ＿ｇｙｒｏを正規化部５４に出力する。

正規化部５４は、分散値取得部５２から分散値Ｖａｒ＿ｇｙｒｏを入力する。正規化部５４は、予め実験により測定された３軸ジャイロセンサ１６の出力値の分散ＴｙｐＶａｒ＿ｇｙｒｏを保持する。正規化部５４は、分散値Ｖａｒ＿ｇｙｒｏを分散ＴｙｐＶａｒ＿ｇｙｒｏで除算し、正規化する。正規化部５４は、（Ｖａｒ＿ｇｙｒｏ／ＴｙｐＶａｒ＿ｇｙｒｏ）を調節部５６に出力する。

調節部５６は、正規化部５４から（Ｖａｒ＿ｇｙｒｏ／ＴｙｐＶａｒ＿ｇｙｒｏ）を入力する。調節部５６は、予め実験により決定される姿勢角導出基準期間Ｔｅｒｍ＿ｄｅｆａｕｌｔを保持する。これには、温度が急激に変化する期間より小さく、かつ、ホワイトノイズに影響される期間より大きい期間を設定、例えば、３秒が設定される。調節部５６は、姿勢角導出基準期間Ｔｅｒｍ＿ｄｅｆａｕｌｔに（Ｖａｒ＿ｇｙｒｏ／ＴｙｐＶａｒ＿ｇｙｒｏ）を乗算する。乗算結果が導出期間に相当する。つまり、３軸ジャイロセンサ１６の出力値の分散値が大きくなると、初期姿勢導出部１２における導出期間が長くなるように、調節部５６は導出期間を調節する。ここで、（Ｖａｒ＿ｇｙｒｏ／ＴｙｐＶａｒ＿ｇｙｒｏ）が１以下の場合、導出期間はＴｅｒｍ＿ｄｅｆａｕｌｔにされる。また、導出期間が１０秒間を超える場合、導出期間は１０秒間とされる。調節部５６は、導出期間を初期姿勢導出部１２に設定する。初期姿勢導出部１２は、設定された導出期間にわたって初期姿勢を平均化する。

これは、略停止状態において、正規化した角速度の分散値を使用して導出期間を決定することに相当する。角速度の分散値が大きい場合には、車両２００の姿勢が短周期的に変化して、３軸加速度センサ１０の出力値にはノイズ成分が入り込む。そのため、導出期間を長くすることによってノイズ成分の影響が低減される。

なお、分散値取得部５２、正規化部５４、調節部５６は、次の処理を実行してもよい。分散値取得部５２は、略停止判定部５０から通知を入力した場合、３軸加速度センサ１０の出力値の分散値Ｖａｒ＿ａｃｃｌを取得する。分散値を計算する期間は、例えば１０秒間、サンプリング周波数は、例えば１０［Ｈｚ］であり、これによって車両２００の姿勢の変化が検出される。分散値取得部５２は、分散値Ｖａｒ＿ａｃｃｌを正規化部５４に出力する。

正規化部５４は、分散値取得部５２から分散値Ｖａｒ＿ａｃｃｌを入力する。正規化部５４は、予め実験により測定された３軸加速度センサ１０の出力値の分散ＴｙｐＶａｒ＿ａｃｃｌを保持する。正規化部５４は、分散値Ｖａｒ＿ａｃｃｌを分散ＴｙｐＶａｒ＿ａｃｃｌで除算し、正規化する。正規化部５４は、（Ｖａｒ＿ａｃｃｌ／ＴｙｐＶａｒ＿ａｃｃｌ）を調節部５６に出力する。

調節部５６は、正規化部５４から（Ｖａｒ＿ａｃｃｌ／ＴｙｐＶａｒ＿ａｃｃｌ）を入力する。調節部５６は、予め実験により決定される姿勢角導出基準期間Ｔｅｒｍ＿ｄｅｆａｕｌｔを保持する。これには、温度が急激に変化する期間より小さく、かつ、ホワイトノイズに影響される期間より大きい期間を設定、例えば、３秒が設定される。調節部５６は、姿勢角導出基準期間Ｔｅｒｍ＿ｄｅｆａｕｌｔに（Ｖａｒ＿ａｃｃｌ／ＴｙｐＶａｒ＿ａｃｃｌ）を乗算する。乗算結果が導出期間に相当する。つまり、３軸加速度センサ１０の出力値の分散値が大きくなると、初期姿勢導出部１２における導出期間が長くなるように、調節部５６は導出期間を調節する。ここで、（Ｖａｒ＿ａｃｃｌ／ＴｙｐＶａｒ＿ａｃｃｌ）が１以下の場合、導出期間はＴｅｒｍ＿ｄｅｆａｕｌｔにされる。また、導出期間が１０秒間を超える場合、導出期間は１０秒間とされる。調節部５６は、導出期間を初期姿勢導出部１２に設定する。初期姿勢導出部１２は、設定された導出期間にわたって初期姿勢を平均化する。これは、略停止状態において、正規化した加速度の分散値を使用して導出期間を決定することに相当する。加速度の分散値が大きい場合には、車両２００の姿勢が変化している。そのため、導出期間を長くすることによって精度が向上する。

この構成は、ハードウエア的には、任意のコンピュータのＣＰＵ、メモリ、その他のＬＳＩで実現でき、ソフトウエア的にはメモリにロードされたプログラムなどによって実現されるが、ここではそれらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックがハードウエアのみ、ソフトウエアのみ、またはそれらの組合せによっていろいろな形で実現できることは、当業者には理解されるところである。

以上の構成による角速度導出装置１００の動作を説明する。図３は、角速度導出装置１００による導出期間の調節手順を示すフローチャートである。速度がしきい値より小さい場合（Ｓ１０のＹ）、調節部５６は、３軸ジャイロセンサ１６の出力値の分散値、あるいは３軸加速度センサ１０の出力値の分散値をもとに導出期間を調節する（Ｓ１２）。速度がしきい値より小さくない場合（Ｓ１０のＮ）、ステップ１２はスキップされる。

本発明の実施例によれば、車両２００の速度がしきい値よりも低い場合に、３軸加速度センサ１０の出力値の分散値をもとに、初期姿勢の導出期間を調節するので、車両２００の振動の影響を抑制できる。また、車両２００の速度がしきい値よりも低い場合に、３軸ジャイロセンサ１６の出力値の分散値をもとに、初期姿勢の導出期間を調節するので、車両２００の振動の影響を抑制できる。また、車両２００の振動の影響が抑制されるので、３軸ジャイロセンサ１６を使用する場合に姿勢の導出精度の悪化を抑制できる。また、３軸ジャイロセンサ１６を使用する場合に姿勢の導出精度の悪化が抑制されるので、３軸ジャイロセンサ１６を使用する場合に角速度の導出精度の悪化を抑制できる。

また、車両２００の速度がしきい値よりも低い場合に、３軸ジャイロセンサ１６の出力値の分散値が大きくなると導出期間を長くするので、ノイズ成分の影響を低減できる。また、ノイズ成分の影響が低減されるので、姿勢の導出精度の悪化を抑制できる。また、車両２００の速度がしきい値よりも低い場合に、３軸加速度センサ１０の出力値の分散値が大きくなると導出期間を長くするので、車両２００の姿勢が変化していても、姿勢の導出精度の悪化を抑制できる。

（実施例２）
次に、実施例２を説明する。実施例２は、実施例１と同様に、３軸のジャイロセンサを使用して角速度を導出する角速度導出装置に関する。本実施例では、姿勢の更新によって蓄積される姿勢角の誤差、およびそれによって生ずるヨー角速度の導出精度の悪化を抑制することを目的とする。３軸の加速度センサの出力値から計算された初期姿勢を３軸ジャイロセンサの出力値によって更新する場合、時間の経過とともに、計算に使用する３軸のジャイロセンサのオフセットおよび感度の誤差が姿勢に蓄積される。これにより、姿勢の精度が悪化するため、その差分から計算される角速度の精度も悪化する。一方、１軸のジャイロセンサの出力値から角速度を導出する場合、車両の姿勢の変化による検出軸の傾きの影響を受け、精度が悪化する。

本実施例に係る角速度導出装置では、角速度を導出するための２種類の処理について、精度の高い方の処理を選択して実行することによって、角速度の導出精度を高める。具体的には、３軸の加速度センサの出力値から初期姿勢を導出したタイミングを起点として、３軸のジャイロセンサの出力値を使用して導出した角速度の絶対値の積分値が導出される。角速度導出装置は、積分値がしきい値以上になった場合、１軸のジャイロセンサの出力値から導出した角速度を出力する。ここでは、これまでとの差異を中心に説明する。

図４は、実施例２に係る角速度導出装置１００の構成を示す。角速度導出装置１００は、３軸加速度センサ１０、初期姿勢導出部１２、第１変換部１４、３軸ジャイロセンサ１６、更新部１８、第２変換部２０、角速度導出部２２、出力部２４、速度センサ２６、ピッチ角導出部３０、１軸角速度導出部３２を含む。

ピッチ角導出部３０は、３軸加速度センサ１０の出力値を入力するとともに、速度センサ２６の出力値を入力する。ピッチ角導出部３０は、これらをもとに車両２００のピッチ角θを導出する。図５は、ピッチ角導出部３０における処理の概要を示す。斜面を走行している車両２００において、これまでと同様にｘ軸、ｙ軸、ｚ軸が定義されるので、ｘ軸方向の加速度ａ_ｘ、ｙ軸方向の加速度ａ_ｙ、ｘ軸方向の加速度ａ_ｚが３軸加速度センサ１０によって取得される。また、速度センサ２６の出力値である車両２００の速度は、ｖの方向を示す。ピッチ角導出部３０は、速度ｖを微分することによって、加速度ａを取得する。この加速度ａも速度ｖと同一の方向を有する。重力加速度をｇと示す場合、図５における関係は次のように示される。

この連立方程式を解くことによって、図５における斜面の傾きθが導出され、斜面の傾きθがピッチ角θに相当する。ピッチ角導出部３０は、ピッチ角θを１軸角速度導出部３２に出力する。

１軸角速度導出部３２は、３軸ジャイロセンサ１６の出力値のうち、１軸の出力値を取得する。また、１軸角速度導出部３２は、ピッチ角導出部３０からピッチ角θを入力する。１軸角速度導出部３２は、次のように角速度ωを導出する。

ここで、Ｖｏｕｔは、１軸の出力値であり、Ｖｏｆｆｓｅｔは、３軸ジャイロセンサ１６のオフセット値、Ｓ（ｍＶ／ｄｅｇ／ｓｅｃ）は、３軸ジャイロセンサ１６の感度係数である。１軸角速度導出部３２は、角速度ωを出力部２４に出力する。

出力部２４は、角速度導出部２２において導出した角速度（以下、「３軸角速度」ということもある）を入力する。また、出力部２４は、１軸角速度導出部３２において導出した角速度（以下、「１軸角速度」ということもある）を入力する。出力部２４は、初期姿勢導出部１２において初期姿勢が導出されてから、３軸角速度の絶対値の積分値の計算を開始する。出力部２４は、積分値がしきい値より小さい場合、３軸角速度を出力する。一方、出力部２４は、積分値がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力する。なお、出力部２４は、初期姿勢導出部１２において新たな初期姿勢が導出されると、積分値をリセットする。

以上の構成による角速度導出装置１００の動作を説明する。図６は、角速度導出装置１００による切替手順を示すフローチャートである。出力部２４は、３軸角速度の絶対値の積分値を導出する（Ｓ３０）。積分値がしきい値以上である場合（Ｓ３２のＹ）、出力部２４は１軸角速度を出力する（Ｓ３４）。積分値がしきい値以上でない場合（Ｓ３２のＮ）、出力部２４は３軸角速度を出力する（Ｓ３６）。

本発明の実施例によれば、３軸角速度の絶対値の積分値をしきい値と比較することで、姿勢の更新過程において積分される誤差の増加を検出できる。また、３軸角速度の絶対値の積分値がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力するので、姿勢の誤差の影響を受けた３軸加速度の出力を回避できる。また、姿勢の誤差の影響を受けた３軸角速度の出力が回避されるので、出力される角速度の導出精度の悪化を抑制できる。また、１軸角速度が出力されるので、姿勢に蓄積された絶対姿勢角の誤差がもたらす、ヨー軸角速度への誤差を抑制できる。

（実施例３）
次に、実施例３を説明する。実施例３は、これまでと同様に、３軸のジャイロセンサを使用して角速度を導出する角速度導出装置に関する。また、実施例３は、実施例２と同様に、姿勢の更新によって蓄積される姿勢角の誤差、およびそれによって生ずるヨー角速度の導出精度の悪化を抑制することを目的とする。本実施例に係る角速度導出装置でも、角速度を導出するための２種類の処理について、精度の高い方の処理を選択して実行することによって、角速度の導出精度を高める。具体的には、３軸の加速度センサの出力値から初期姿勢を導出した後、温度の変化が導出される。角速度導出装置は、温度の変化がしきい値以上になった場合、１軸のジャイロセンサの出力値から導出した角速度を出力する。ここでは、これまでとの差異を中心に説明する。

図７は、実施例３に係る角速度導出装置１００の構成を示す。角速度導出装置１００は、３軸加速度センサ１０、初期姿勢導出部１２、第１変換部１４、３軸ジャイロセンサ１６、更新部１８、第２変換部２０、角速度導出部２２、出力部２４、速度センサ２６、ピッチ角導出部３０、１軸角速度導出部３２、温度センサ３４、取得部３６を含む。

温度センサ３４は、例えば、半導体温度センサ等に相当し、３軸ジャイロセンサ１６の内部、あるいは近傍に設置される。つまり、温度センサ３４は、３軸ジャイロセンサ１６の温度またはその周囲の温度を検出する。これは、３軸ジャイロセンサ１６が設置されているセンサ設置部の環境温度が検出されることに相当する。検出された温度は、例えば、温度に比例した０Ｖ〜５Ｖのアナログ信号として出力される。取得部３６は、温度センサ３４からのアナログ信号を入力する。これは温度を取得することに相当する。取得部３６は、ＡＤ（ＡｎａｌｏｇｔｏＤｉｇｉｔａｌ）変換装置を備え、温度センサ３４の電圧値に対して、例えば、サンプリング間隔１００ｍｓｅｃでのＡＤ変換を実行する。取得部３６は、ＡＤ変換の結果のデジタル信号を温度として出力部２４に出力する。

出力部２４は、取得部３６からの温度を入力する。また、出力部２４は、角速度導出部２２からの３軸角速度を入力するとともに、１軸角速度導出部３２からの１軸角速度も入力する。出力部２４は、初期姿勢導出部１２において初期姿勢が導出された後、温度の変化を導出する。出力部２４は、温度の変化がしきい値より小さい場合、３軸角速度を出力する。一方、出力部２４は、温度の変化がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力する。

以上の構成による角速度導出装置１００の動作を説明する。図８は、角速度導出装置１００による切替手順を示すフローチャートである。取得部３６は温度を取得する（Ｓ５０）。温度の変化がしきい値以上である場合（Ｓ５２のＹ）、出力部２４は１軸角速度を出力する（Ｓ５４）。温度の変化がしきい値以上でない場合（Ｓ５２のＮ）、出力部２４は３軸角速度を出力する（Ｓ５６）。

本発明の実施例によれば、温度をしきい値と比較するので、環境の変化によるジャイロセンサの誤差の増加を検出できる。また、温度がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力するので、誤差の影響を受けた３軸角速度の出力を回避できる。また、誤差の影響を受けた３軸角速度の出力が回避されるので、出力される角速度の導出精度の悪化を抑制できる。また、１軸角速度が出力されるので、環境の変化による誤差の増加の影響を抑制できる。

（実施例４）
次に、実施例４を説明する。実施例４は、これまでと同様に、３軸のジャイロセンサを使用して角速度を導出する角速度導出装置に関する。また、実施例４は、実施例２、３と同様に、姿勢の更新によって蓄積される姿勢角の誤差、およびそれによって生ずるヨー角速度の導出精度の悪化を抑制することを目的とする。本実施例に係る角速度導出装置でも、角速度を導出するための２種類の処理について、精度の高い方の処理を選択して実行することによって、角速度の導出精度を高める。具体的には、３軸のジャイロセンサの出力値によって更新された姿勢のピッチ角と、３軸の加速度センサと速度センサから算出したピッチ角との差分が導出される。角速度導出装置は、差分がしきい値以上になった場合、１軸のジャイロセンサの出力値から導出した角速度を出力する。ここでは、これまでとの差異を中心に説明する。

図９は、実施例４に係る角速度導出装置１００の構成を示す。角速度導出装置１００は、３軸加速度センサ１０、初期姿勢導出部１２、第１変換部１４、３軸ジャイロセンサ１６、更新部１８、第２変換部２０、角速度導出部２２、出力部２４、速度センサ２６、ピッチ角導出部３０、１軸角速度導出部３２を含む。

ピッチ角導出部３０は、ピッチ角θを１軸角速度導出部３２に出力するとともに、出力部２４にも出力する。第２変換部２０は、オイラー角表現の姿勢のうち、ピッチ角θを出力部２４に出力する。出力部２４は、第２変換部２０からのピッチ角θ（以下、「第１ピッチ角」ともいう）を入力するとともに、ピッチ角導出部３０からのピッチ角θ（以下、「第２ピッチ角」ともいう）を入力する。出力部２４は、初期姿勢導出部１２において初期姿勢が導出された後、第１ピッチ角と第２ピッチ角との差異を導出する。出力部２４は、差異がしきい値より小さい場合、３軸角速度を出力する。一方、出力部２４は、差異がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力する。

以上の構成による角速度導出装置１００の動作を説明する。図１０は、角速度導出装置１００による切替手順を示すフローチャートである。第２変換部２０は、３軸ジャイロセンサ１６の出力値と３軸加速度センサ１０の出力値とをもとに第１ピッチ角を導出する（Ｓ７０）。ピッチ角導出部３０は、３軸加速度センサ１０の出力値と速度センサ２６の出力値とをもとに第２ピッチ角を導出する（Ｓ７２）。第１ピッチ角と第２ピッチ角との差異がしきい値以上である場合（Ｓ７４のＹ）、出力部２４は１軸角速度を出力する（Ｓ７６）。第１ピッチ角と第２ピッチ角との差異がしきい値以上でない場合（Ｓ７４のＮ）、出力部２４は３軸角速度を出力する（Ｓ７８）。

本発明の実施例によれば、３軸ジャイロセンサ１６と３軸加速度センサ１０とからの出力値とをもとに導出した第１ピッチ角と、３軸加速度センサ１０と速度センサ２６とからの出力値とをもとに導出した第２ピッチ角との差異をしきい値と比較するので、絶対姿勢角の誤差を検出できる。また、差異がしきい値以上である場合、３軸角速度の代わりに、１軸角速度を出力するので、誤差の影響を受けた３軸加速度の出力を回避できる。また、誤差の影響を受けた３軸角速度の出力が回避されるので、出力される角速度の導出精度の悪化を抑制できる。また、１軸角速度が出力されるので、絶対姿勢角の誤差の影響を抑制できる。

以上、本発明について実施例をもとに説明した。この実施例は例示であり、それらの各構成要素や各処理プロセスの組合せにいろいろな変形例が可能なこと、またそうした変形例も本発明の範囲にあることは当業者に理解されるところである。

本実施例１から４の任意の組合せも有効である。本変形例によれば、本実施例１から４を任意に組み合わせた効果を得ることができる。

１０３軸加速度センサ、１２初期姿勢導出部、１４第１変換部、１６３軸ジャイロセンサ、１８更新部、２０第２変換部、２２角速度導出部、２４出力部、２６速度センサ、２８制御部、３０ピッチ角導出部、３２１軸角速度導出部、３４温度センサ、３６取得部、５０略停止判定部、５２分散値取得部、５４正規化部、５６調節部、１００角速度導出装置、２００車両。

Claims

移動体に搭載可能な角速度導出装置であって、
３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換する第１変換部と、
前記第１変換部において変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新する更新部と、
前記更新部により更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換する第２変換部と、
前記第２変換部において変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出する角速度導出部と、
前記角速度導出部において導出した角速度を出力する出力部とを備え、
前記出力部は、前記角速度導出部において導出した角速度の絶対値の積分値がしきい値以上である場合、前記角速度導出部において導出した角速度の代わりに、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力することを特徴とする角速度導出装置。
移動体に搭載可能な角速度導出装置であって、
３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換する第１変換部と、
前記第１変換部において変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新する更新部と、
前記更新部により更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換する第２変換部と、
前記第２変換部において変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出する角速度導出部と、
前記角速度導出部において導出した角速度を出力する出力部と、
温度を取得する取得部とを備え、
前記出力部は、前記取得部において取得した温度の変化がしきい値以上である場合、前記角速度導出部において導出した角速度の代わりに、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力することを特徴とする角速度導出装置。
移動体に搭載可能な角速度導出装置における角速度導出方法であって、
３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換するステップと、
変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新するステップと、
更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換するステップと、
変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出するステップと、
導出した角速度を出力するステップとを備え、
前記出力するステップは、導出した角速度の絶対値の積分値がしきい値以上である場合、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力することを特徴とする角速度導出方法。
移動体に搭載可能な角速度導出装置における角速度導出方法であって、
３軸の加速度センサの出力値をもとに導出したオイラー角表現の初期姿勢をクォータニオンによる初期姿勢に変換するステップと、
変換したクォータニオンによる初期姿勢を初期値として、３軸のジャイロセンサの出力値を順次代入しながら、クォータニオンによる姿勢の微分方程式を繰り返し解くことによって、クォータニオンによる姿勢を更新するステップと、
更新したクォータニオンによる姿勢をオイラー角表現の姿勢に変換するステップと、
変換したオイラー角表現の姿勢の時間変化をもとに角速度を導出するステップと、
導出した角速度を出力するステップと、
温度を取得するステップとを備え、
前記出力するステップは、取得した温度の変化がしきい値以上である場合、１軸のジャイロセンサの出力値をもとに導出したヨー角速度を出力することを特徴とする角速度導出方法。