JP2008002992A - 姿勢角検出装置と姿勢角検出方法 - Google Patents

Abstract

【課題】加速度センサからの出力値を補正することで移動体の移動中の姿勢角を精度良く検出する姿勢角検出装置及びその方法を提供する。
【解決手段】移動体７に加わる加速度を測定する加速度センサ２と、移動体７のヨーレートを測定するヨーレートセンサ３と、移動体７の速度を測定する速度センサ４と、速度から実加速度を計算し、ヨーレートと速度とから遠心力を計算し、実加速度と遠心力との合力である移動成分加速度を計算する移動成分加速度計算手段５と、移動成分加速度で加速度を補正して得られる重力加速度から姿勢角を計算する姿勢角計算手段６と、からなることを特徴とする。
【選択図】図１

Description

本発明は、移動体の姿勢角検出装置とその方法に関する。

車両などの移動体自身の位置や速度を知る装置（航法装置）の一種に、地上の設備の助けを必要としない慣性航法装置（ＩＮＳ）がある。このＩＮＳに使用される慣性センサは、速度の変化を検出する加速度センサと姿勢の変化を検出するジャイロセンサとからなる。ＩＮＳは、慣性センサを用いて出発点からの速度や姿勢の変化を、計算機で積み上げていくことにより、自分の位置や速度を知ることができる。

通常、移動体の姿勢状態を特定するために、移動体の前後（軸）方向をｘ（ロール）軸、左右（軸）方向をｙ（ピッチ）軸、上下（軸）方向をｚ（ヨー）軸とする直交３軸の座標系を用いる。移動体が水平な地面に置かれたとき、移動体のヨー軸は地球の鉛直軸（Ｚ軸）方向にほぼ向けられ、ロール軸及びピッチ軸は地球の局地水平面（Ｘ−Ｙ平面）にほぼ平行になるとする。そして、ロール軸が局地水平面Ｘ軸から傾いている角度をピッチ角、ピッチ軸が局地水平面Ｙ軸から傾いている角度をロール角、Ｘ−Ｙ平面上でロール軸又はピッチ軸がずれている角度をヨー角、の３角度で表す。この３角度を測定するために、３軸ジャイロセンサが用いられる。

例えば特許文献１の発明は、３軸ジャイロセンサと加速度センサとの出力値にそれぞれ重み付けを行い、姿勢角を計算している。ジャイロセンサの出力結果は、姿勢角検出のために積分される。しかし、ジャイロセンサは時間の経過とともに誤差を同じ方向に出力する現象（ドリフト）があり、誤差を含んだ値を積分すれば、誤差が積み上げられることとなる。その上、ジャイロセンサは高価なため、低コストで機能を満たすためには、加速度センサで重力を検出し姿勢角を計算する方法がある。特許文献２の発明は、２つの加速度センサからの出力結果を基に移動体の姿勢角を検出している。

しかし、加速度センサの出力は、移動体の移動に伴う加速度を含んでいるため、特許文献２に示されている発明では、正確な姿勢角を検出することが難しい。
特開平８−１７８６８７号公報 特開２００３−１３９５３６号公報

本発明は、上記課題に鑑みてなされたもので、加速度センサからの出力値を補正することで移動体の移動中の姿勢角を精度良く検出する姿勢角検出装置及びその方法を提供することを目的とする。

本発明者は、移動体に加わる加速度からのみ姿勢角を計算したのでは、移動体の移動中の姿勢角が正確に検出されにくいことから、移動体の移動に係る加速度の分力（遠心力など）が移動体の速度や角速度から求められることに着目し、本発明を完成させた。

本発明の姿勢角検出装置は、移動体に加わる加速度を測定する加速度センサと、該移動体のヨーレートを測定するヨーレートセンサと、該移動体の速度を測定する速度センサと、該速度から実加速度を計算し、該ヨーレートと該速度とから遠心力を計算し、該実加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算手段と、該移動成分加速度で該加速度を補正して得られる重力加速度から姿勢角を計算する姿勢角計算手段と、を有することを特徴とする。

本発明の姿勢角検出装置では、加速度センサが移動体に加わる加速度をロール軸方向、ピッチ軸方向、ヨー軸方向の３方向について測定し、ヨーレートセンサが移動体のヨー軸方向の回転に対するヨーレート（角速度）を測定する。そして、移動体の速度から移動（運動）に関する加速度（運動量）を計算によって求め実加速度とする。同様に、移動体に係る遠心力を移動体の速度及びヨーレートを用いて計算する。実加速度と遠心力とは、移動体の移動によって生じる加速度（移動成分加速度）であり、加速度センサで得られた加速度に含まれている。そこで、加速度から移動成分加速度を取り除くことで、移動体の移動に関する運動量が除去されて重力加速度が得られる。そして、重力加速度を用いて姿勢角が求められる。

本発明の姿勢角検出装置では、移動体の直交３軸の加速度と移動体の速度とを検出することで、ジャイロセンサはヨー角を検出するための１つを用いれば、移動体の姿勢角を求めることができる。そのため、高価なジャイロセンサの数を減らすことができる。しかし、ジャイロセンサの個数を１つに限定せず、直交３軸で用いかつ加速度と速度とから移動体の姿勢角を検出することで、より正確な検出結果を得ることができる。また、速度センサ及び加速度センサの出力結果をそのまま姿勢角の検出に用いるのではなく、移動体の移動に伴う加速度（運動量）を計算し、加速度センサの加速度から取り除くことで、正確な姿勢角を計算することができる。

本発明の姿勢角検出装置で用いられる姿勢角計算手段は、移動成分加速度を加速度から除去した重力加速度を計算する重力加速度計算手段と、重力加速度から移動体のロール角を計算するロール角計算手段と、重力加速度から移動体のピッチ角を計算するピッチ角計算手段とからなることが好ましい。移動体の姿勢角は、ロール角とピッチ角とで表す。重力加速度計算手段では、各方向の重力加速度が求められるため、ピッチ軸方向の重力加速度からロール角、ロール角軸方向の重力加速度からピッチ角を計算することができる。この重力分力を用いて移動体の姿勢角をロール角とピッチ角とについて求めることができるため、より正確な姿勢角を検出することができる。

本発明の姿勢角検出装置で用いられる移動成分加速度計算手段は、速度を時間で微分して実加速度を計算して移動成分加速度の前後方向の分力とする前後軸加速度計算手段と、ヨーレートと速度とから遠心力を計算して移動成分加速度の左右方向の分力とする左右軸加速度計算手段と、移動体の上下方向における遠心力の分力を計算して移動成分加速度の上下方向の分力とする上下軸加速度計算手段とからなることが好ましい。移動成分加速度は、移動体の移動（運動）に関する運動量であり、移動体の直交３軸に対応した分力を表す。よって、移動体の直交３軸ごとに求め移動体に係る加速度から取り除くことで、移動体の姿勢角を検出するために、より正確な重力加速度を求めることができる。

本発明の姿勢角検出装置で用いられる上下軸加速度計算手段における遠心力の分力の計算は、姿勢角計算手段により計算したロール角又はピッチ角に基づいて行い、姿勢角計算手段は、計算した遠心力の分力に基づきロール角及びピッチ角のうちの少なくとも一方を計算することが好ましい。上下軸加速度計算手段において、遠心力の分力の計算をロール角又はピッチ角に基づいて行い、それをピッチ角及びロール角の計算に反映させることで移動体の姿勢角がより正確に求めることができる。

本発明の姿勢角検出方法は、移動体に加わる加速度と、該移動体のヨーレートと、該移動体の速度と、を測定する測定ステップと、該速度から実加速度を計算し、該ヨーレートと該速度とから遠心力を計算しし、該実加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算ステップと、該移動成分加速度で該加速度を補正して得られた重力加速度から姿勢角を計算する姿勢角計算ステップと、を有することを特徴とする。

本発明の姿勢角検出方法は、加速度センサが移動体に加わる加速度をロール軸方向、ピッチ軸方向、ヨー軸方向の３方向について測定し、ヨーレートセンサが移動体のヨー軸方向の回転に対するヨーレートを測定する。そして、移動体の速度から移動に関する加速度を計算によって求め実加速度とする。同様に、移動体に係る遠心力を移動体の速度を用いて計算する。実加速度と遠心力とは、移動体の移動によって生じる加速度（移動成分加速度）であり、加速度センサで得られた加速度に含まれている。そこで、加速度から移動成分加速度を取り除くことで、移動体の移動に関する運動量が除去されて重力加速度が得られる。そして、重力加速度を用いて姿勢角が求められる。

本発明の姿勢角検出方法では、移動体の速度及び直交３軸の加速度を検出することで、移動体の姿勢角を求めることができるため、ヨー角を検出するジャイロセンサが１つのみで高価なジャイロセンサの数を減らすことができる。また、速度センサ及び加速度センサの出力結果をそのまま姿勢角の検出に用いるのではなく、移動体の移動に伴う加速度を計算し、加速度センサの加速度から取り除くことで、正確な姿勢角を計算することができる。

本発明の姿勢角検出方法で用いられる姿勢角計算ステップは、移動成分加速度を加速度から除去した重力加速度を計算する重力加速度計算ステップと、重力加速度から移動体のロール角を計算するロール角計算ステップと、重力加速度から移動体のピッチ角を計算するピッチ角計算ステップとからなることが好ましい。移動体の姿勢角は、ロール角とピッチ角とで表す。重力加速度計算手段では、各方向の重力加速度が求められるため、ピッチ軸方向の重力加速度からロール角、ロール角軸方向の重力加速度からピッチ角を計算することができる。この重力分力を用いて移動体の姿勢角をロール角とピッチ角とについて求めることができるため、より正確な姿勢角を検出することができる。

本発明の姿勢角検出方法で用いられる移動成分加速度計算ステップは、速度を時間で微分して実加速度を計算して移動成分加速度の前後方向の分力とする前後軸加速度計算ステップと、ヨーレートと速度とから遠心力を計算して移動成分加速度の左右方向の分力とする左右軸加速度計算ステップと、移動体の上下方向における遠心力の分力を計算して移動成分加速度の上下方向の分力とする上下軸加速度計算ステップとからなることが好ましい。移動成分加速度は、移動体の移動（運動）に関する運動量であり、移動体の直交３軸に対応した分力を表す。よって、移動体の直交３軸ごとに求め移動体に係る加速度から取り除くことで、移動体の姿勢角を検出するために、より正確な重力加速度を求めることができる。

本発明の姿勢角検出方法で用いられる重力加速度計算ステップは重力加速度を複数の分力に分けて計算し、ロール角計算ステップ及びピッチ角形算ステップのうちの一方で計算したロール角又はピッチ角に基づいて複数の分力のうちの一部を計算するステップであり、ロール角計算ステップ及びピッチ角計算ステップのうちの一方は複数の分力のうちの残部に基づいてロール角又はピッチ角を計算し、ロール角計算ステップ及びピッチ角計算ステップのうちの他方は複数の分力のうちの一部乃至全部に基づいてロール角又はピッチ角を計算するステップであることが好ましい。本発明の姿勢角検出方法では、ロール角及びピッチ角を計算するために重力加速度計算ステップで計算する重力加速度の複数の分力を、ロール角又はピッチ角に基づいて計算する。これにより、ロール角及びピッチ角の計算に反映されるため、移動体の姿勢角をより正確に求めることができる。

本発明の姿勢角検出装置及びその方法によれば、高価なジャイロセンサを直交３軸分使用するのではなく１つでも機能するので、低コストで移動体の姿勢角を検出することができる。また、ジャイロセンサの出力は角速度（ヨーレート）を用い、ジャイロセンサのドラフトによる誤差を含んだ値を計算に用いることがないため、正確な姿勢角を検出することができる。

そして、加速度センサの出力値をそのまま計算に用いるのではなく、移動体の運動に関する加速度を取り除いた重力加速度を求めているため、移動体が移動していても正確に姿勢角を検出することができる。

以下、実施例を用いて本発明を具体的に説明する。

（実施例）
図１は、本実施例の姿勢角検出装置の構成図である。

姿勢角検出装置１は、加速度センサ２と、ヨーレートセンサ３と、速度センサ４と、移動加速度成分計算手段５、姿勢角計算手段６とからなる。移動加速度成分計算手段５、姿勢角計算手段６は、コンピュータ上のロジックとして実現される。

姿勢角検出装置１が取り付けられる移動体７は、図２に示されるように、移動体７を水平な地面に置いたときに、移動体７の上下軸（ヨー軸）８１が地球の鉛直軸（Ｚ軸）９１方向にほぼ向けられ、移動体７の前後軸（ロール軸）８２及び左右軸（ピッチ軸）８３が地球の水平面（Ｘ−Ｙ平面）９４にほぼ平行になるようにする。移動体７のロール軸８２が局地水平面Ｘ軸９２から傾いている角度をピッチ角７１、ピッチ軸８３が局地水平面Ｙ軸９３から傾いている角度をロール角７２、Ｘ−Ｙ平面上でロール軸８２又はピッチ軸８３がずれている角度をヨー角７３として、移動体７の姿勢角を３角度で表す。

加速度センサ２は、ロール軸８２、ピッチ軸８３及びヨー軸８１の直交する３軸の加速度をそれぞれ測定するＡｘ２１、Ａｙ２２、Ａｘ２３からなる。加速度センサ２は、移動体の加速度を測定し移動加速度成分計算手段５及び姿勢角計算手段６に出力する。加速度センサ２は、直交３軸のロール軸８２が移動体７の前後方向、ピッチ軸８３が移動体７の左右方向、ヨー軸８１が移動体７の上下方向に一致するように設置され、移動体７の回転と同時に回転する。

ヨーレートセンサ３は、移動体７の上下軸であるヨー軸８１の角速度（ヨーレート）を測定し移動加速度成分計算手段５に出力するジャイロセンサである。速度センサ４は、移動体７の速度を測定し移動加速度成分計算手段５に出力する手段である。移動体が車両の場合は、車輪速を測定する。

移動成分加速度計算手段５は、ヨーレートセンサ３及び速度センサ４から入力されたヨーレート及び速度を元に、移動体７の移動に係る加速度を直交３軸の各軸毎に計算する手段であり、前後軸加速度計算手段５１と、左右軸加速度計算手段５２と、上下軸加速度計算手段５３とからなる。移動成分加速度計算手段５は計算結果を姿勢角計算手段６に出力する。

前後軸加速度計算手段５１は、移動体７の移動に係る前後方向の加速度を計算する手段である。左右軸加速度計算手段５２は、移動体７の移動に係る左右方向の加速度を計算する手段である。上下軸加速度計算手段５３は、移動体７の移動に係る上下方向の加速度を計算する手段である。

姿勢角計算手段６は、重力加速度計算手段６１とロール角計算手段６２とピッチ角計算手段６３とからなる。重力加速度計算手段６１は、加速度センサ２より入力される加速度から移動成分加速度計算手段５より入力される移動成分加速度を用いて補正した重力加速度を計算する手段である。ロール角計算手段６２は、重力加速度計算手段６１から入力される重力加速度を元にロール角７２を計算する手段である。ピッチ角計算手段６３は、重力加速度計算手段６１から入力される重力加速度を元にピッチ角７３を計算する手段である。

本実施例の姿勢角検出装置１で用いられる姿勢角検出方法（以下、姿勢角検出ルーチン）をフローチャートで示したものが図３である。本ルーチンは、姿勢角検出装置１が起動後、所定期間中に所定時間毎に実行されるものである。

本ルーチンの処理開始後、加速度センサ２が移動体７に加わる加速度を測定し、ヨーレートセンサ３がヨーレート（ω）を測定し、速度センサ４が速度（Ｖｗ）を測定し、移動成分加速度計算手段５及び姿勢角計算手段６へ出力を行う（測定ステップＳ１）。加速度センサ２は、Ａｘ２１でロール軸方向の加速度（Ａｘ＿ｓｅｎｓｏｒ）が測定され、Ａｙ２２でピッチ軸方向の加速度（Ａｙ＿ｓｅｎｓｏｒ）が測定され、Ａｚ２３でヨー軸方向の加速度（Ａｚ＿ｓｅｎｓｏｒ）が測定する。

次に、移動成分加速度計算手段５は、ヨーレート及び速度より移動体７に係る移動（運動）に関する加速度（移動成分加速度）の計算を行う（移動成分加速度計算ステップ）。

そして、姿勢角計算手段６は、加速度センサ２で測定した加速度を移動成分加速度計算手段５から入力された移動成分加速度で補正して求めた重力加速度で、ロール角（Ｒｏｌｌ）７２、ピッチ角（Ｐｉｔｃｈ）７１の計算を行う（姿勢角計算ステップ）。

移動成分加速度計算手段５は、前後軸加速度計算手段５１が、速度（Ｖｗ）を時間で微分して実加速度を計算し移動成分加速度の前後方向の分力（Ａｘ＿ｃｏｒ）とする前後軸加速度計算ステップＳ２１を持つ。そして、左右軸加速度計算手段５２がヨーレート（ω）と速度（Ｖｗ）とから遠心力を計算し移動成分加速度の左右方向の分力（Ａｙ＿ｃｏｒ）とする左右軸加速度計算ステップＳ２２を持ち、上下軸加速度計算手段５３が移動体７の上下方向における遠心力の分力を計算して移動成分加速度の上下方向の分力（Ａｚ＿ｃｏｒ）とする上下軸加速度計算ステップＳ２３を持つ。

遠心力（Ａｙ＿ｃｏｒ）は、
Ａｙ＿ｃｏｒ＝ 速度（Ｖｗ）× ヨーレート（ω）・・・（１）
で求められる。

移動成分加速度の上下方向の分力（Ａｚ＿ｃｏｒ）は、姿勢角計算手段６の姿勢角計算ステップによって計算されるＲｏｌｌを元に計算される。

姿勢角計算手段６は、重力加速度計算手段６１が加速度を移動成分加速度で補正した重力加速度を計算する重力加速度計算ステップＳ３１と、重力加速度から移動体７のＲｏｌｌを計算するロール角計算ステップＳ３２と、重力加速度から移動体７のＰｉｔｃｈを計算するピッチ角計算ステップＳ３３とがある。

Ｓ３２において、移動体７が傾く方向により、Ａｙ＿ｓｅｎｓｏｒは、図４に示される順バンクと図５に示される逆バンクとで異なる数式で計算する。
順バンクの場合：
Ａｙ＿ｓｅｎｓｏｒ ＝ Ａｙ＿ｃｏｒ−ｇ×ｓｉｎ（Ｒｏｌｌ）・・・（２）
ｇ＝その地域での重力加速度
逆バンクの場合：
Ａｙ＿ｓｅｎｓｏｒ ＝ Ａｙ＿ｃｏｒ＋ｇ×ｓｉｎ（Ｒｏｌｌ）・・・（３）
ｇ＝その地域での重力加速度

式（２）及び式（３）の ｇ・ｓｉｎ（Ｒｏｌｌ）が、移動成分加速度の左右方向の分力である。式（２）及び式（３）を左右方向の分力を求めるように移項した場合、移動体７がどちらに傾いているかで、測定されたＡｙ＿ｓｅｎｓｏｒに遠心力が加わっているのか引かれているのか違ってくる。そこで、図６に示されるように、Ａｙ＿ｓｅｎｓｏｒの大きさと遠心力の大きさとを比べて、遠心力が小さいと順バンクであり、遠心力が大きいと逆バンクである。順バンクの時に１を逆バンクの時に−１をバンクフラグ（Ｂ＿ｆｌａｇ）の値とする。また、移動体７の旋回についても左旋回と右旋回とでＡｙ＿ｓｅｎｓｏｒと遠心力との力の作用する向きが異なるため、図６に示されるように、ωによって判断し、右旋回の時に１を左旋回の時に−１を方向フラグ（ＲＬ＿ｆｌａｇ）の値とする。ｇの重力加速度は定数であるため、左右方向の分力が求めることができれば、Ｒｏｌｌが計算できる。
Ｒｏｌｌ＝ｓｉｎ^−１（（｜Ａｙ＿ｓｅｎｓｏｒ｜−｜Ｖｗ×ω｜）／ｇ）×
ＲＬ＿ｆｌａｇ×Ｂ＿ｆｌａｇ ・・・（４）

次に、Ｓ３３において、Ｐｉｔｃｈが加速度（Ａｘ＿ｓｅｎｓｏｒ、Ａｙ＿ｓｅｎｓｏｒ、Ａｚ＿ｓｅｎｓｏｒ）を移動成分加速度の直交３軸の各分力（Ａｘ＿ｃｏｒ，Ａｙ＿ｃｏｒ，Ａｚ＿ｃｏｒ）により補正した重力加速度から計算される。
Ｐｉｔｃｈ ＝ｔａｎ^−１（（Ａｘ＿ｓｅｎｓｏｒ−Ａｘ＿ｃｏｒ）／
（（Ａｙ＿ｓｅｎｓｏｒ−Ａｙ＿ｃｏｒ）^２＋
（Ａｚ＿ｓｅｎｓｏｒ−Ａｚ＿ｃｏｒ）^２））^１／２）・・・（５）

移動成分加速度の直交３軸の各分力（Ａｘ＿ｃｏｒ，Ａｙ＿ｃｏｒ，Ａｚ＿ｃｏｒ）は、移動成分加速度計算手段５のＳ２で求められている。

本実施例の姿勢角検出装置１及びその方法によれば、加速度センサと速度センサとを用いることで高価なジャイロセンサを直交３軸すべてに使用することなく、低コストで移動体の姿勢角を検出することができる。また、ジャイロセンサのドラフトに伴う誤差を含んだ値を計算に用いることもないため、正確な姿勢角を検出することができる。

そして、加速度センサの出力値をそのまま計算に用いるのではなく、移動体の運動に関する加速度を取り除いた上で重力加速度を求めているため、移動体が移動していても正確に姿勢角を検出することができる。

以上、本発明の好適な実施例について説明したが、本発明は上記実施例に限定されているものではない。例えば、移動体の速度は移動体が車輪を有する自動車などであれば車輪速を測定し、Ｐｉｔｃｈを求めるときは、移動成分加速度の上下方向の分力の求め方について、ピッチ角と速度とにより計算した移動体の加速度を追加することで、より高精度の姿勢角を検出することができる。また、移動体が車輪を有しない航空機などであれば、移動体の速度としてプロペラの回転速度を測定することが考えられる。

本実施例の姿勢角検出装置の構成図である。 本実施例の姿勢角検出装置及び方法で用いられる絶対軸と相対軸とを示す図である。 本実施例の姿勢角検出方法のフローチャートである。 本実施例の移動体の順バンク状態を示す図である。 本実施例の移動体の逆バンク状態を示す図である。 本実施例の姿勢角検出方法で用いられるバンクと旋回とを判定するフローチャートである。

符号の説明

１：姿勢角検出装置
２：加速度センサ
３：ヨーレートセンサ
４：速度センサ
５：移動加速度成分計算手段
６：姿勢角計算手段
７：移動体
５１：前後軸加速度計算手段
５２：左右軸加速度計算手段
５３：上下軸加速度計算手段
６１：重力加速度計算手段
６２：ロール角計算手段
６３：ピッチ角計算手段
７１：ピッチ角
７２：ロール角
７３：ヨー角
８１：移動体７の上下軸（ヨー軸）
８２：移動体７の前後軸（ロール軸）
８３：移動体７の左右軸（ピッチ軸）
９１：地球の鉛直軸（Ｚ軸）
９２：局地水平面Ｘ軸
９３：局地水平面Ｙ軸
９４：地球の水平面（Ｘ−Ｙ平面）

Claims (8)

1. 移動体に加わる加速度を測定する加速度センサと、
   該移動体のヨーレートを測定するヨーレートセンサと、
   該移動体の速度を測定する速度センサと、
   該速度から実加速度を計算し、該ヨーレートと該速度とから遠心力を計算し、該実加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算手段と、
   該移動成分加速度で該加速度を補正して得られる重力加速度から姿勢角を計算する姿勢角計算手段と、
   からなることを特徴とする姿勢角検出装置。
2. 前記姿勢角計算手段は、前記移動成分加速度を前記加速度から除去した前記重力加速度を計算する重力加速度計算手段と、該重力加速度から該移動体のロール角を計算するロール角計算手段と、該重力加速度から該移動体のピッチ角を計算するピッチ角計算手段とからなる請求項１に記載の姿勢角検出装置。
3. 前記移動成分加速度計算手段は、前記速度を時間で微分して前記実加速度を計算して前記移動成分加速度の前後軸方向の分力とする前後軸加速度計算手段と、前記ヨーレートと該速度とから遠心力を計算して該移動成分加速度の左右軸方向の分力とする左右軸加速度計算手段と、該移動体の上下軸方向における該遠心力の分力を計算して該移動成分加速度の上下軸方向の分力とする上下軸加速度計算手段とからなる請求項１又は２に記載の姿勢角検出装置。
4. 前記上下軸加速度計算手段における前記遠心力の分力の計算は、前記姿勢角計算手段により計算した前記ロール角又は前記ピッチ角に基づいて行い、
   該姿勢角計算手段は、計算した該遠心力の分力に基づき該ロール角及び該ピッチ角のうちの少なくとも一方を計算する請求項３に記載の姿勢角検出装置。
5. 移動体に加わる加速度と、該移動体のヨーレートと、該移動体の速度と、を測定する測定ステップと、
   該速度から実加速度を計算し、該ヨーレートと該速度とから遠心力を計算しし、該実加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算ステップと、
   該移動成分加速度で該加速度を補正して得られた重力加速度から姿勢角を計算する姿勢角計算ステップと、
   を有することを特徴とする姿勢角検出方法。
6. 前記姿勢角計算ステップは、前記移動成分加速度を前記加速度から除去した前記重力加速度を計算する重力加速度計算ステップと、該重力加速度から該移動体のロール角を計算するロール角計算ステップと、該重力加速度から該移動体のピッチ角を計算するピッチ角計算ステップとからなる請求項４に記載の姿勢角検出方法。
7. 前記移動成分加速度計算ステップは、前記速度を時間で微分して前記実加速度を計算して前記移動成分加速度の前後軸方向の分力とする前後軸加速度計算ステップと、前記ヨーレートと該速度とから遠心力を計算して該移動成分加速度の左右軸方向の分力とする左右軸加速度計算ステップと、該移動体の上下軸方向における該遠心力の分力を計算して該移動成分加速度の上下軸方向の分力とする上下軸加速度計算ステップとからなる請求項４又は５に記載の姿勢角検出方法。
8. 前記重力加速度計算ステップは前記重力加速度を複数の分力に分けて計算し、前記ロール角計算ステップ及び前記ピッチ角形算ステップのうちの一方で計算した前記ロール角又は前記ピッチ角に基づいて該複数の分力のうちの一部を計算するステップであり、
   該ロール角計算ステップ及び該ピッチ角計算ステップのうちの一方は該複数の分力のうちの残部に基づいて該ロール角又は該ピッチ角を計算し、該ロール角計算ステップ及び該ピッチ角計算ステップのうちの他方は該複数の分力のうちの一部乃至全部に基づいて該ロール角又は該ピッチ角を計算するステップである請求項６に記載の姿勢角検出方法。

Images