JP6604175B2

JP6604175B2 - ピッチ角速度補正値算出装置、姿勢角算出装置およびピッチ角速度補正値算出方法

Info

Publication number: JP6604175B2
Application number: JP2015235916A
Authority: JP
Inventors: 秀徳福嶋
Original assignee: JVCKenwood Corp
Current assignee: JVCKenwood Corp
Priority date: 2015-12-02
Filing date: 2015-12-02
Publication date: 2019-11-13
Anticipated expiration: 2035-12-02
Also published as: US10429207B2; US20170160100A1; JP2017102030A

Description

本発明は、ピッチ角速度補正値算出装置、姿勢角算出装置およびピッチ角速度補正値算出方法に関する。

例えば車両を含む物体には、物体の姿勢角を算出するレートジャイロが搭載されている。このレートジャイロは、例えば温度や湿度の変化によって、オフセット電圧（以下、「オフセット」という）を生じる。そこで、レートジャイロのオフセットを正確に補正する車載用ジャイロのオフセット補正装置が知られている（例えば、特許文献１参照）。この技術は、ピッチレートジャイロにより検出された角速度のうち、ヨーレートジャイロが検出した角速度が所定の角速度以下となっている期間における角速度の所定時間の平均値を求め、この平均値をピッチレートジャイロのオフセットとして補正を行う。

特開２０１１−１９１２４３号公報

特許文献１に記載の技術では、ヨーレートの影響を受けない期間におけるピッチレートの角速度を用いてオフセットを算出し、ピッチレートジャイロのオフセットを補正する。ところが、例えば車両が坂道を走行する場合、ピッチレートジャイロには、車両の上下変動による成分が重畳される。このため、特許文献１に記載の技術は、ピッチレートジャイロのオフセット補正の精度について改善の余地がある。

本発明は、上記に鑑みてなされたものであって、ピッチレートジャイロのオフセット補正の精度を向上することを目的とする。

上述した課題を解決し、目的を達成するために、本発明に係るピッチ角速度補正値算出装置は、角速度検出部で検出した物体の角速度であるピッチ角速度と、前記ピッチ角速度の補正値とに基づいて、物体の姿勢角のピッチ角を算出するピッチ角算出部と、前記ピッチ角速度の補正値を用いて前記ピッチ角算出部で算出した前記ピッチ角、および、速度検出部で検出した前記物体の速度と加速度検出部で検出した前記物体の加速度とに基づいて傾斜角算出部で算出したその時の前記物体の傾斜角と、前記補正値を用いて過去に前記ピッチ角算出部で算出した前記ピッチ角、および、前記傾斜角算出部で算出したその時の前記傾斜角とに基づいて前記補正値を修正する補正値算出部とを有することを特徴とする。

本発明に係る姿勢角算出装置は、上記のピッチ角速度補正値算出装置と、前記角速度検出部は、さらにヨー角速度とロール角速度とを検出し、前記角速度検出部で検出した前記ヨー角速度と前記ピッチ角速度と前記ロール角速度と、前記補正値算出部で算出した前記補正値とに基づいて、前記ピッチ角算出部で前記ピッチ角を補正して算出する補正ピッチ角算出部とを有することを特徴とする。

本発明に係るピッチ角速度補正値算出方法は、角速度検出ステップで検出した物体の角速度であるピッチ角速度と、前記ピッチ角速度の補正値とに基づいて、物体の姿勢角のピッチ角を算出するピッチ角算出ステップと、前記ピッチ角速度の補正値を用いて前記ピッチ角算出ステップで算出した前記ピッチ角、および、速度検出ステップで検出した前記物体の速度と加速度検出ステップで検出した前記物体の加速度とに基づいて傾斜角算出ステップで算出したその時の前記物体の傾斜角と、前記補正値を用いて過去に前記ピッチ角算出ステップで算出した前記ピッチ角、および、前記傾斜角算出ステップで算出したその時の前記傾斜角とに基づいて前記補正値を修正する補正値算出ステップとを有することを特徴とする。

本発明によれば、ピッチレートジャイロのオフセット補正の精度を向上することができるという効果を奏する。

図１は、実施形態に係るピッチ角速度補正値算出部を有する姿勢角算出装置の構成例を示す図である。図２は、ピッチ角速度補正値算出部を有する姿勢角算出装置を搭載した車両の傾斜角を示す図である。図３は、ピッチ角速度補正値算出部を有する姿勢角算出装置を搭載した車両の姿勢角を示す図である。図４は、ピッチ角速度補正値算出処理を含む姿勢角算出処理の流れの例を示すフローチャートである。

以下に添付図面を参照して、本発明に係るピッチ角速度補正値算出装置、姿勢角算出装置およびピッチ角速度補正値算出方法の実施形態を詳細に説明する。なお、以下の実施形態により本発明が限定されるものではない。

図１は、実施形態に係るピッチ角速度補正値算出部（ピッチ角速度補正値算出装置）を有する姿勢角算出装置の構成例を示す図である。姿勢角算出装置１００は、物体に配置されている。本実施形態では、物体の一例として車両について説明する。姿勢角算出装置１００は、車両の角速度に基づいて、車両の姿勢角を補正して算出する。姿勢角算出装置１００は、ピッチ角速度の補正値を算出するピッチ角速度補正値算出部１１０と、算出したピッチ角速度の補正値に基づいてピッチ角を補正して算出する補正ピッチ角算出部１２０と、ロール角を算出するロール角算出部１３０と、ヨー角を算出するヨー角算出部１４０とを有する。

まず、姿勢角算出装置１００が配置される車両について説明する。なお、本実施形態では、姿勢角算出装置１００を車両に配置した場合として説明するが、配置する対象は車両に限定されず、種々の機器に配置することができる。車両には、図１に示すように、検出ユニット１が配置されている。検出ユニット１は、例えば、車両の速度と、車両の加速度と、車両の角速度と、車両の傾斜角と、車両の姿勢角とを検出する。検出ユニット１は、車両のそれぞれの位置に配置された、車速センサ（速度検出部）１１と、３軸加速度センサ（加速度検出部）１２と、３軸ジャイロセンサ（角速度検出部）１３と、記憶装置１４と、姿勢角算出装置１００とを一つの機能として実装する制御装置２０とを有する。

車速センサ１１は、車両の速度を検出する。車速センサ１１は、車両の駆動軸又はタイヤに配置されている。車速センサ１１は、駆動軸又はタイヤの回転に応じたパルス信号を検出する。車速センサ１１は、検出したパルス信号を速度パルス信号として制御装置２０の速度算出部２１に出力する。

３軸加速度センサ１２は、車両の加速度を検出する。より詳しくは、３軸加速度センサ１２は、車両の前後方向に生じる加速度と、車両の左右方向に生じる加速度と、車両の鉛直方向に生じる加速度とを検出する。３軸加速度センサ１２は、検出した車両の加速度を加速度出力信号として制御装置２０の傾斜角算出部２２に出力する。

３軸ジャイロセンサ１３は、車両の角速度を検出する。より詳しくは、３軸ジャイロセンサ１３は、ロールレートジャイロと、ピッチレートジャイロと、ヨーレートジャイロとを有する。ロールレートジャイロは、車両の前後方向を軸とした回転の角速度であるロール角速度を検出する。ピッチレートジャイロは、車両の左右方向を軸とした回転の角速度であるピッチ角速度を検出する。ヨーレートジャイロは、車両の鉛直方向を軸とした回転の角速度であるヨー角速度を検出する。３軸ジャイロセンサ１３は、ロールレートジャイロで検出したロール角速度と、ピッチレートジャイロで検出したピッチ角速度と、ヨーレートジャイロで検出したヨー角速度とを角速度信号として制御装置２０の姿勢角算出装置１００に出力する。

記憶装置１４は、制御装置２０における各種処理に要するデータおよび各種処理結果を記憶する。記憶装置１４は、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリ（Flash Memory）等の半導体メモリ素子、又は、ハードディスク、光ディスク等の記憶装置である。記憶装置１４は、例えば、ピッチ角速度補正値算出部１１０の補正値算出部１１２で算出したピッチ角速度のオフセット補正値（以下、「補正値」という）を記憶する。記憶装置１４は、例えば、ピッチ角速度の補正値に対応して、傾斜角算出部２２で算出したその時の車両の傾斜角を記憶する。

制御装置２０は、例えば、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）などで構成された演算処理装置である。制御装置２０は、記憶装置１４に記憶されているプログラムをメモリにロードして、プログラムに含まれる命令を実行する。制御装置２０は、速度算出部２１と、傾斜角算出部２２と、ピッチ角速度補正値算出部１１０と補正ピッチ角算出部１２０とロール角算出部１３０とヨー角算出部１４０とを含む姿勢角算出装置１００とを有する。

速度算出部２１は、速度パルス信号に基づいて車両の速度を算出する。より詳しくは、速度算出部２１は、車速センサ１１から出力された速度パルス信号に、距離へ変換する変換係数（単位：ｍ／ｐｕｌｓｅ）を乗算して、速度を算出する。速度算出部２１は、算出した車両の速度を傾斜角算出部２２に出力する。

傾斜角算出部２２は、速度算出部２１で算出した速度と、３軸加速度センサ１２で検出した加速度とに基づいて、車両の傾斜角αを算出する。

図２を参照しながら、傾斜角算出部２２における傾斜角αの算出方法について説明する。図２は、ピッチ角速度補正値算出部を有する姿勢角算出装置を搭載した車両の傾斜角を示す図である。図２において、車両は、傾斜した道路面Ｒ上を走行している。車両の水平面に対する傾斜角をαとする。このとき、車両には、重力加速度Ｇの斜面方向成分ＧＡが作用する。速度算出部２１で算出した速度を微分して得られる加速度を第一加速度Ａ１とし、３軸加速度センサ１２で検出した加速度を第二加速度Ａ２とする。第二加速度Ａ２は、第一加速度Ａ１と重力加速度Ｇの斜面方向成分ＧＡとのベクトル和で表される。ベクトルＧＡの大きさは、ベクトルＡ１とベクトルＡ２との絶対値の差で表される。これらより、傾斜角αは、以下のように表される。

傾斜角算出部２２は、算出した傾斜角αをピッチ角速度補正値算出部１１０の補正値算出部１１２と外部装置に出力する。

図１に戻って、姿勢角算出装置１００は、ピッチ角速度補正値算出部１１０と、補正ピッチ角算出部１２０とロール角算出部１３０とヨー角算出部１４０とを有する。姿勢角算出装置１００は、ピッチ角速度補正値算出部１１０でピッチ角速度の補正値を算出し、補正ピッチ角算出部１２０で算出した補正値に基づいてピッチ角を補正して算出する。姿勢角算出装置１００は、ロール角算出部１３０でロール角を算出し、ヨー角算出部１４０でヨー角を算出する。

ピッチ角速度補正値算出部１１０は、３軸ジャイロセンサ１３で検出したピッチ角速度の補正値を算出する。ピッチ角速度の補正値は、ピッチレートジャイロのオフセットを補正するための補正値である。ピッチ角速度補正値算出部１１０は、車両の姿勢角のうち少なくともピッチ角を算出するピッチ角算出部１１１と、ピッチ角速度の補正値を算出する補正値算出部１１２とを有する。

ピッチ角算出部１１１は、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、ピッチ角速度の補正値とに基づいて、車両の姿勢角のうち少なくともピッチ角を算出する。本実施形態では、ピッチ角算出部１１１は、ピッチ角を算出する。

図３を参照しながら、ピッチ角算出部１１１における姿勢角の算出方法について説明する。図３は、ピッチ角速度補正値算出部を有する姿勢角算出装置を搭載した車両の姿勢角を示す図である。本実施形態では、慣性座標をｚ軸，ｙ軸，ｘ軸の順でψ，θ，φ回転させたときの物体座標となる姿勢角を、オイラー角（φ，θ，ψ）で定義する。なお、ｘ軸，ｙ軸，ｚ軸回りの回転角は、それぞれロール角φ、ピッチ角θ、ヨー角ψである。

慣性座標系から物体座標系への方向余弦行列Ｅは、オイラー角（φ，θ，ψ）を用いて以下のように表される。

ここで、四元数（クオータニオン）ｑ_１、ｑ_２、ｑ_３、ｑ_４は、回転方向の単位ベクトルｎと、回転角度θを用いて以下のように表される。

数式２と数式３とより、方向余弦行列Ｅは、四元数ｑ_１、ｑ_２、ｑ_３、ｑ_４を用いて以下のように表される。

次に、方向余弦行列Ｅと、角速度ωとの関係を説明する。まず、方向余弦行列Ｅの時間微分は、以下のように表される。

このとき、角速度ωと方向余弦行列Ｅとは、以下のように表される。

角速度ωから生成された歪対称行列は、以下のように表される。

数式６と数式７とより、方向余弦行列Ｅの時間微分は、角速度ωを用いて以下のように表される。

数式４、数式８より、角速度ω_１〜ω_３には、以下の関係が成り立つ。

数式９より、角速度ωは、行列では以下のように表される。

数式９より、角速度ωは、ベクトルでは以下のように表される。

次に、四元数ｑ_１、ｑ_２、ｑ_３、ｑ_４の微分方程式は、以下のように表される。

ピッチ角算出部１１１は、上記のようにして得られた数式１２の微分方程式について、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度とを用いて、ルンゲクッタ法により、解を求める。このとき、ピッチ角速度には、３軸ジャイロセンサ１３で検出したピッチ角速度をピッチ角速度の補正値でオフセットした値を使用する。このようにしてピッチ角算出部１１１は、微分方程式を解いて、ピッチ角を算出する。ピッチ角算出部１１１は、算出したピッチ角を補正値算出部１１２に出力する。

補正値算出部１１２は、ピッチ角算出部１１１で算出したピッチ角およびその時の傾斜角と、前回算出したピッチ角およびその時の傾斜角とに基づいて、ピッチ角速度の補正値を算出する。より詳しくは、補正値算出部１１２は、ピッチ角速度の補正値（現在の補正値）を用いてピッチ角算出部１１１で算出したピッチ角、および、速度算出部２１で算出した車両の速度と３軸加速度センサ１２で検出した車両の加速度とに基づいて傾斜角算出部２２で算出した現在の車両の傾斜角と、同じ補正値で過去に算出されて記憶装置１４に記憶されたピッチ角速度の補正値を用いてピッチ角算出部１１１で算出したピッチ角、および、傾斜角算出部２２で算出した、前回ピッチ角速度の補正値を算出した時の傾斜角とに基づいて、ピッチ角速度の補正値を修正する。言い換えると、補正値算出部１１２は、ピッチ角算出部１１１で算出したピッチ角と、傾斜角算出部２２で算出したその時の傾斜角αとの差分について、現在の値と、同じピッチ角速度の補正値で算出したときの値（過去の値）とを比較し、その差分量に基づいて、ピッチ角速度の補正値を修正する。具体的には例えば、補正値算出部１１２は、ピッチ角算出部１１１で算出したピッチ角と、傾斜角算出部２２で算出したその時の傾斜角αとの差分について、現在の値と過去の値との差分量がゼロとなるように、ピッチ角速度の補正値を修正してもよい。補正値算出部１１２は、算出したピッチ角速度の修正した補正値をピッチ角速度の補正値として補正ピッチ角算出部１２０に出力する。補正値算出部１１２は、算出したピッチ角速度の修正した補正値をピッチ角速度の補正値として記憶装置１４に記憶する。これにより、補正値算出部１１２で算出したピッチ角速度の補正値が記憶装置１４に記憶されている。補正値算出部１１２は、ピッチ角速度の補正値と対応する、傾斜角算出部２２で算出した傾斜角αを記憶装置１４に記憶する。

補正ピッチ角算出部１２０は、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、補正値算出部１１２で算出したピッチ角速度の補正値とに基づいて、車両の姿勢角のうち少なくともピッチ角を補正して算出する。本実施形態では、補正ピッチ角算出部１２０は、ピッチ角を補正して算出する。補正ピッチ角算出部１２０は、算出したピッチ角を外部装置に出力する。

ロール角算出部１３０は、ロール角を算出する。ロール角算出部１３０は、算出したロール角を外部装置に出力する。

ヨー角算出部１４０は、ヨー角を算出する。ヨー角算出部１４０は、算出したヨー角を外部装置に出力する。

次に、図４を用いて、ピッチ角速度補正値算出処理を含む姿勢角算出処理の流れについて説明する。図４は、ピッチ角速度補正値算出処理を含む姿勢角算出処理の流れの例を示すフローチャートである。

制御装置２０は、車速センサ１１から速度パルス信号が入力されると、車両の速度を算出する（ステップＳ１１）。より詳しくは、制御装置２０は、速度算出部２１で、車速センサ１１で検出した速度パルス信号に基づいて車両の速度を算出し、傾斜角算出部２２に出力する。

制御装置２０は、傾斜角算出部２２で車両の傾斜角を算出する（ステップＳ１２）。より詳しくは、制御装置２０は、傾斜角算出部２２で、速度算出部２１で算出した速度と、３軸加速度センサ１２で検出した加速度とに基づいて、車両の傾斜角を算出する。

制御装置２０は、ピッチ角速度の補正値の補正のタイミングであるか否かを判定する（ステップＳ１３）。より詳しくは、制御装置２０は、例えば、車両の始動時である場合、又は、前回補正値算出部１１２でピッチ角速度の補正値を算出してから所定時間以上が経過している場合は、ピッチ角速度の補正値の補正のタイミングである（ステップＳ１３でＹｅｓ）と判定して、ステップＳ１４に進む。制御装置２０は、例えば、車両の始動時ではない場合（車両の継続走行時である場合）、又は、前回補正値算出部１１２でピッチ角速度の補正値を算出してから所定時間以上が経過していない場合には、ピッチ角速度の補正値の補正のタイミングではない（ステップＳ１３でＮｏ）と判定して、ステップＳ１６に進む。

制御装置２０は、ピッチ角速度とピッチ角速度の補正値とに基づいてピッチ角を算出する（ステップＳ１４）。より詳しくは、制御装置２０は、ピッチ角算出部１１１で、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、補正値算出部１１２で算出したピッチ角速度の補正値とに基づいて、車両のピッチ角を算出する。

制御装置２０は、算出したピッチ角およびその時の傾斜角と、前回算出したピッチ角およびその時の傾斜角とに基づいて、ピッチ角速度の補正値を算出する（ステップＳ１５）。より詳しくは、制御装置２０は、補正値算出部１１２で、ピッチ角算出部１１１が算出したピッチ角と、傾斜角算出部２２が算出したその時の傾斜角αとの差分について、現在の値と過去の値とを比較し、その差分量に基づいて、ピッチ角速度の補正値を修正する。具体的には、制御装置２０は、ピッチ角算出部１１１で算出したピッチ角と、傾斜角算出部２２で算出したその時の傾斜角αとの差分について、現在の値と過去の値との差分量がゼロとなるように、ピッチ角速度の補正値を修正する。制御装置２０は、算出したピッチ角速度の修正した補正値をピッチ角速度の補正値として補正ピッチ角算出部１２０に出力する。制御装置２０は、ピッチ角速度の修正した補正値を記憶装置１４に記憶する。制御装置２０は、ピッチ角速度の修正した補正値と対応する、傾斜角算出部２２で算出した傾斜角を記憶装置１４に記憶する。

制御装置２０は、ピッチ角速度とピッチ角速度の補正値とに基づいてピッチ角を補正して算出する（ステップＳ１６）。より詳しくは、制御装置２０は、補正ピッチ角算出部１２０で、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、補正値算出部１１２で算出したピッチ角速度の補正値とに基づいて、車両のピッチ角を算出する。本実施形態では、図４に図示していないが、ステップＳ１６の前後のステップ、又は、ステップＳ１６において、制御装置２０は、ロール角算出部１３０でロール角を算出し、ヨー角算出部１４０でヨー角を算出してもよい。

このようにして、姿勢角算出装置１００は、ピッチ角速度の補正値の補正のタイミングである場合、ピッチ角算出部１１１で算出したピッチ角と、傾斜角算出部２２で算出したその時の傾斜角αとの差分について、現在の値と過去の値とを比較し、その差分量に基づいて、ピッチ角速度の補正値を算出する。姿勢角算出装置１００は、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、補正値算出部１１２で算出したピッチ角速度の補正値とに基づいて、車両の姿勢角のうち少なくともピッチ角を補正して算出する。

上述したように、姿勢角算出装置１００は、ピッチ角速度の補正値の補正のタイミングである場合、ピッチ角算出部１１１で算出したピッチ角と、傾斜角算出部２２で算出したその時の傾斜角αとの差分について、現在の値と過去の値とを比較し、その差分量に基づいて、ピッチ角速度の補正値を算出することができる。これにより、姿勢角算出装置１００は、例えば車両が坂道を走行する場合においても、３軸ジャイロセンサ１３のピッチレートジャイロのオフセット補正を高精度に行なうことができる。言い換えると、姿勢角算出装置１００は、３軸ジャイロセンサ１３のピッチレートジャイロのオフセット補正の精度を向上することができる。

姿勢角算出装置１００は、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、補正値算出部１１２で算出したピッチ角速度の補正値とに基づいて、車両の姿勢角のうち少なくともピッチ角を補正して算出することができる。これにより、姿勢角算出装置１００は、姿勢角を高精度に算出することができる。姿勢角算出装置１００は、例えば坂道を走行する車両の姿勢角を高精度に算出することができる。

さて、これまで本発明に係るピッチ角速度補正値算出部１１０を有する姿勢角算出装置１００について説明したが、上述した実施形態以外にも種々の異なる形態にて実施されてよいものである。

図示したピッチ角速度補正値算出部１１０と姿勢角算出装置１００との各構成要素は、機能概念的なものであり、必ずしも物理的に図示の如く構成されていなくてもよい。すなわち、各装置の具体的形態は、図示のものに限られず、各装置の処理負担や使用状況などに応じて、その全部又は一部を任意の単位で機能的又は物理的に分散又は統合してもよい。例えば、ピッチ角速度補正値算出部１１０のピッチ角算出部１１１と補正値算出部１１２とによる処理は、一つの処理部で一連の処理として行なわれるようにしても良い。

ピッチ角速度補正値算出部１１０と姿勢角算出装置１００との構成は、例えば、制御装置２０の一つの機能として実現される。ピッチ角速度補正値算出部１１０と姿勢角算出装置１００との構成は、制御装置２０の他の機能と同様に、例えば、ソフトウェアとして、メモリにロードされたプログラム等によって実現される。上記実施形態では、これらのハードウェア又はソフトウェアの連携によって実現される機能ブロックとして説明した。すなわち、これらの機能ブロックについては、ハードウェアのみ、ソフトウェアのみ、又は、それらの組み合わせによって種々の形で実現できる。

ピッチ角算出部１１１と補正ピッチ角算出部１２０とは、独立した処理部として説明したが、例えばピッチ角算出部１１１と補正ピッチ角算出部１２０とは、一つの処理部として実現されてもよい。これは、ピッチ角算出部１１１と補正ピッチ角算出部１２０とは、いずれも、３軸ジャイロセンサ１３で検出したヨー角速度とピッチ角速度とロール角速度と、ピッチ角速度の補正値とに基づいて、車両の姿勢角のうち少なくともピッチ角を補正して算出するものであり、その処理が共通しているためである。

１検出ユニット
１１車速センサ（速度検出部）
１２３軸加速度センサ（加速度検出部）
１３３軸ジャイロセンサ（角速度検出部）
１４記憶装置
２０制御装置
２１速度算出部
２２傾斜角算出部
１００姿勢角算出装置
１１０ピッチ角速度補正値算出部（ピッチ角速度補正値算出装置）
１１１ピッチ角算出部
１１２補正値算出部
１２０補正ピッチ角算出部
１３０ロール角算出部
１４０ヨー角算出部

Claims

角速度検出部で検出した物体の角速度であるピッチ角速度と、前記ピッチ角速度の補正値とに基づいて、物体の姿勢角のピッチ角を算出するピッチ角算出部と、
前記ピッチ角速度の補正値を用いて前記ピッチ角算出部で算出した前記ピッチ角、および、速度検出部で検出した前記物体の速度と加速度検出部で検出した前記物体の加速度とに基づいて傾斜角算出部で算出したその時の前記物体の傾斜角と、前記補正値を用いて過去に前記ピッチ角算出部で算出した前記ピッチ角、および、前記傾斜角算出部で算出したその時の前記傾斜角とに基づいて前記補正値を修正する補正値算出部と
を有することを特徴とするピッチ角速度補正値算出装置。
前記補正値算出部は、前記ピッチ角算出部で算出した前記ピッチ角と、前記傾斜角算出部で算出したその時の前記傾斜角との差分について、現在の値と、前記補正値で算出したときの過去の値とを比較し、その差分量に基づいて、前記補正値を修正することを特徴とする請求項１に記載のピッチ角速度補正値算出装置。
補正値算出部は、前記差分量がゼロとなるように、前記補正値を修正することを特徴とする請求項２に記載のピッチ角速度補正値算出装置。
前記角速度検出部は、３軸ジャイロセンサを有することを特徴とする請求項１から３のいずれか一項に記載のピッチ角速度補正値算出装置。
前記加速度検出部は、３軸加速度センサを有することを特徴とする請求項１から４のいずれか一項に記載のピッチ角速度補正値算出装置。
請求項１から５のいずれか一項に記載のピッチ角速度補正値算出装置と、
前記角速度検出部は、さらにヨー角速度とロール角速度とを検出し、前記角速度検出部で検出した前記ヨー角速度と前記ピッチ角速度と前記ロール角速度と、前記補正値算出部で算出した前記補正値とに基づいて、前記ピッチ角算出部で前記ピッチ角を補正して算出する補正ピッチ角算出部と
を有することを特徴とする姿勢角算出装置。
角速度検出ステップで検出した物体の角速度であるピッチ角速度と、前記ピッチ角速度の補正値とに基づいて、物体の姿勢角のピッチ角を算出するピッチ角算出ステップと、
前記ピッチ角速度の補正値を用いて前記ピッチ角算出ステップで算出した前記ピッチ角、および、速度検出ステップで検出した前記物体の速度と加速度検出ステップで検出した前記物体の加速度とに基づいて傾斜角算出ステップで算出したその時の前記物体の傾斜角と、前記補正値を用いて過去に前記ピッチ角算出ステップで算出した前記ピッチ角、および、前記傾斜角算出ステップで算出したその時の前記傾斜角とに基づいて前記補正値を修正する補正値算出ステップと
を有することを特徴とするピッチ角速度補正値算出方法。