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

Abstract

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

Description

明 細 書

姿勢角検出装置と姿勢角検出方法

技術分野

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

背景技術

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

[0003] 通常、移動体の姿勢状態を特定するために、移動体の前後（軸)方向を x(ロール） 軸、左右 (軸)方向 ¾ ピッチ）軸、上下 (軸)方向を ζ (ョ一）軸とする直交 3軸の座標 系を用いる。移動体が水平な地面に置かれたとき、移動体のョー軸は地球の鉛直軸 (Ζ軸）方向にほぼ向けられ、ロール軸及びピッチ軸は地球の局地水平面 (X— Υ平 面）にほぼ平行になるとする。そして、ロール軸が局地水平面 X軸力も傾いている角 度をピッチ角、ピッチ軸が局地水平面 Υ軸力 傾いている角度をロール角、 X— Υ平 面上でロール軸又はピッチ軸がずれている角度をョ一角、の 3角度で姿勢角を表す 。この 3角度を測定するために、 3軸ジャイロセンサが用いられる。

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

[0005] 特許文献 2の発明は、 2つの加速度センサ力 の出力結果を基に移動体の姿勢角 を検出している。しかし、加速度センサの出力は、移動体の移動に伴う加速度を含ん でいるため、特許文献 2に示されている発明では、正確な姿勢角を検出することが難 しい。

特許文献 1：特開平 8— 178687号公報

特許文献 2 :特開 2003— 139536号公報

発明の開示

発明が解決しょうとする課題

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

課題を解決するための手段

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

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

[0009] 本発明の姿勢角検出装置では、加速度センサが移動体に加わる加速度をロール 軸方向、ピッチ軸方向、ョー軸方向の 3方向について測定し、ョーレートセンサが移 動体のョー軸方向の回転に対するョーレート（角速度）を測定する。そして、移動体 の速度から移動 (運動）に関する加速度 (運動量)を計算によって求め実加速度とす る。同様に、移動体に係る遠心力を移動体の速度及びョーレートを用いて計算する。 実加速度と遠心力とは、移動体の移動によって生じる加速度 (移動成分加速度)であ り、加速度センサで得られた加速度に含まれている。そこで、加速度から移動成分加 速度を取り除くことで、移動体の移動に関する運動量が除去されて重力加速度が得 られる。そして、重力加速度を用いて姿勢角が求められる。 [0010] 本発明の姿勢角検出装置では、移動体の直交 3軸の加速度と移動体の速度とを検 出することで、ジャイロセンサはョ一角を検出するための 1つを用いれば、移動体の 姿勢角を求めることができる。そのため、高価なジャイロセンサの数を減らすことがで きる。しかし、ジャイロセンサの個数を 1つに限定せず、直交 3軸で用いかつ加速度と 速度とから移動体の姿勢角を検出することで、より正確な検出結果を得ることができ る。また、速度センサ及び加速度センサの出力結果をそのまま姿勢角の検出に用い るのではなぐ移動体の移動に伴う加速度 (運動量)を計算し、加速度センサの加速 度力も取り除くことで、正確な姿勢角を計算することができる。

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

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

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

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

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

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

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

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

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

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

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

図面の簡単な説明

[0022] [図 1]本実施例の姿勢角検出装置の構成図である。

[図 2]本実施例の姿勢角検出装置及び方法で用いられる絶対軸と相対軸とを示す図 である。

[図 3]本実施例の姿勢角検出方法のフローチャートである。

[図 4]本実施例の移動体の順バンク状態を示す図である。

[図 5]本実施例の移動体の逆バンク状態を示す図である。

[図 6]本実施例の姿勢角検出方法で用いられるバンクと旋回とを判定するフローチヤ ートである。

符号の説明

[0023] 1 :姿勢角検出装置

2 :加速度センサ

3 :ョーレートセンサ

4 :速度センサ

5 :移動加速度成分計算手段

6 :姿勢角計算手段

7 :移動体

51 :前後軸加速度計算手段

52 :左右軸加速度計算手段 53 :：上下軸加速度計算手段

61 : ：重力加速度計算手段

62 : ：ロール角計算手段

63 : ：ピッチ角計算手段

71 : ：ピッチ角

72 : ：ローノレ角

73 : ：ョ一角

81 : ：移動体 7の上下軸 (ョー軸）

82 : ：移動体 7の前後軸（ロール軸)

83 : ：移動体 7の左右軸 (ピッチ軸）

91 : ：地球の鉛直軸 (Z軸）

92 : ：局地水平面 X軸

93 : ：局地水平面 Y軸

94 : ：地球の水平面（X— Y平面）

発明を実施するための最良の形態

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

(実施例）

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

[0025] 姿勢角検出装置 1は、加速度センサ 2と、ョーレートセンサ 3と、速度センサ 4と、移 動加速度成分計算手段 5、姿勢角計算手段 6とからなる。移動加速度成分計算手段 5、姿勢角計算手段 6は、コンピュータ上のロジックとして実現される。

[0026] 姿勢角検出装置 1が取り付けられる移動体 7は、図 2に示されるように、移動体 7を 水平な地面に置 、たときに、移動体 7の上下軸 (ョー軸） 81が地球の鉛直軸 (Z軸） 9 1方向にほぼ向けられ、移動体 7の前後軸（ロール軸） 82及び左右軸 (ピッチ軸） 83 が地球の水平面 (X— Y平面） 94にほぼ平行になるようにする。移動体 7のロール軸 8 2が局地水平面 X軸 92から傾!、て!/、る角度をピッチ角 71、ピッチ軸 83が局地水平面 Y軸 93力ら傾 、て!/、る角度をロール角 72、 X—Y平面上でロール軸 82又はピッチ軸 83がずれている角度をョ一角 73として、移動体 7の姿勢角を 3角度で表す。 [0027] 加速度センサ 2は、ロール軸 82、ピッチ軸 83及びョー軸 81の直交する 3軸の加速 度をそれぞれ測定する Ax21、 Ay22、 Ax23からなる。加速度センサ 2は、移動体の 加速度を測定し、移動加速度成分計算手段 5及び姿勢角計算手段 6に出力する。加 速度センサ 2は、直交 3軸のロール軸 82が移動体 7の前後方向、ピッチ軸 83が移動 体 7の左右方向、ョー軸 81が移動体 7の上下方向に一致するように設置され、移動 体 7の回転と同時に回転する。

[0028] ョーレートセンサ 3は、移動体 7の上下軸であるョー軸 81の角速度（ョーレート）を測 定し移動加速度成分計算手段 5に出力するジャイロセンサである。速度センサ 4は、 移動体 7の速度を測定し移動加速度成分計算手段 5に出力する手段である。移動体 が車両の場合は、車輪速を測定する。

[0029] 移動成分加速度計算手段 5は、ョーレートセンサ 3及び速度センサ 4から入力され たョーレート及び速度を元に、移動体 7の移動に係る加速度を直交 3軸の軸毎に計 算する手段であり、前後軸加速度計算手段 51と、左右軸加速度計算手段 52と、上 下軸加速度計算手段 53とからなる。移動成分加速度計算手段 5は計算結果を姿勢 角計算手段 6に出力する。

[0030] 前後軸加速度計算手段 51は、移動体 7の移動に係る前後方向の加速度を計算す る手段である。左右軸加速度計算手段 52は、移動体 7の移動に係る左右方向の加 速度を計算する手段である。上下軸加速度計算手段 53は、移動体 7の移動に係る 上下方向の加速度を計算する手段である。

[0031] 姿勢角計算手段 6は、重力加速度計算手段 61とロール角計算手段 62とピッチ角 計算手段 63とからなる。重力加速度計算手段 61は、加速度センサ 2より入力される 加速度から移動成分加速度計算手段 5より入力される移動成分加速度を用いて補 正した重力加速度を計算する手段である。ロール角計算手段 62は、重力加速度計 算手段 61から入力される重力加速度を元にロール角 72を計算する手段である。ピッ チ角計算手段 63は、重力加速度計算手段 61から入力される重力加速度を元にピッ チ角 73を計算する手段である。

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

[0033] 本ルーチンの処理開始後、加速度センサ 2が移動体 7にカ卩わる加速度を測定し、ョ 一レートセンサ 3がョーレート ( ω )を測定し、速度センサ 4が速度 (Vw)を測定し、移 動成分加速度計算手段 5及び姿勢角計算手段 6へ出力する (測定ステップ S 1)。加 速度センサ 2は、 Ax21でロール軸方向の加速度（Ax— sensor)が測定され、 Ay22 でピッチ軸方向の加速度 (Ay— sensor)が測定され、 Az23でョー軸方向の加速度（ Az— sensor)が測定する。

[0034] 次に、移動成分加速度計算手段 5は、ョーレート及び速度より移動体 7に係る移動（ 運動）に関する加速度 (移動成分加速度)の計算を行う (移動成分加速度計算ステツ プ)。

[0035] そして、姿勢角計算手段 6は、加速度センサ 2で測定した加速度を移動成分加速 度計算手段 5から入力された移動成分加速度で補正して求めた重力加速度で、ロー ル角（Roll) 72、ピッチ角（Pitch) 71の計算を行う（姿勢角計算ステップ)。

[0036] 移動成分加速度計算手段 5は、前後軸加速度計算手段 51が、速度 (Vw)を時間 で微分して実加速度を計算し、移動成分加速度の前後方向の分力 (Ax— cor)とす る前後軸加速度計算ステップ S21を持つ。そして、左右軸加速度計算手段 52がョー レート（ ω )と速度 (Vw)とから遠心力を計算し、移動成分加速度の左右方向の分力（ Ay— cor)とする左右軸加速度計算ステップ S22を持ち、上下軸加速度計算手段 53 が移動体 7の上下方向における遠心力の分力を計算して移動成分加速度の上下方 向の分力（Az— cor)とする上下軸加速度計算ステップ S23を持つ。

[0037] 遠心力（Ay_cor)は、

式 1

[0038] Ay_cor= 速度（Vw) X ョーレート（ω )

[0039] で求められる。

[0040] 移動成分加速度の上下方向の分力 (Az— cor)は、姿勢角計算手段 6の姿勢角計 算ステップによって計算される Rollを元に計算される。

[0041] 姿勢角計算手段 6は、重力加速度計算手段 61が加速度を移動成分加速度で補正 した重力加速度を計算する重力加速度計算ステップ S31と、重力加速度から移動体 7の Rollを計算するロール角計算ステップ S32と、重力加速度力 移動体 7の Pitch を計算するピッチ角計算ステップ S33とがある。

[0042] S32において、移動体 7が傾く方向により、 Ay— sensorは、図 4に示される順バン クと図 5に示される逆バンクとで異なる数式で計算する。

[0043] 順バンクの場合：

式 2

[0044」 Ay sensor = Ay — cor— g X sin (Roll) g =その地域での重力加速度

[0045] 逆バンクの場合：

式 3

[004り」 Ay sensor = Ay — cor +g X sin (Roll) g =その地域での重力加速度

[0047] 式 2及び式 3の g ' sin (Roll) 1S 移動成分加速度の左右方向の分力である。式 2及 び式 3を左右方向の分力を求めるように移項した場合、移動体 7がどちらに傾いてい るかで、測定された Ay— sensorに遠心力が加わっているのか引かれているのか違 つてくる。そこで、図 6に示されるように、 Ay— sensorの大きさと遠心力の大きさとを比 ベて、遠心力が小さいと順バンクであり、遠心力が大きいと逆バンクである。順バンク の時に 1を逆バンクの時に— 1をバンクフラグ (B— flag)の値とする。また、移動体 7 の旋回についても左旋回と右旋回とで Ay— sensorと遠心力との力の作用する向き が異なるため、図 6に示されるように、 ωによって判断し、右旋回の時に 1を左旋回の 時に— 1を方向フラグ (RL— flag)の値とする。 gの重力加速度は定数であるため、左 右方向の分力が求めることができれば、 Rollが計算できる。

式 4

[0048] Roll= sin^_1 ( ( | Ay— sensor | - | Vw X ω | ) /g) X

RL— f lag X B— f lag

[0049] 次に、 S33において、 Pitchが加速度（Ax sensor, Ay sensor, Az sensor) を移動成分加速度の直交 3軸の各分力（Ax— cor, Ay— cor, Az— cor)により補正 した重力加速度力 計算される。

式 5

[0050] Pitch =tan ( (Ax一 sensor— Ax一 cor) /

( (Ay— sensor— Ay— cor) ² +

(Az― sensor— Az― cor)；) ) )

[0051] 移動成分加速度の直交 3軸の各分力（Ax— cor, Ay— cor, Az— cor)は、移動成 分力卩速度計算手段 5のステップ S21、 S22、 S23で求められている。

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

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

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

産業上の利用可能性

[0055] 本発明の姿勢角検出装置と姿勢角検出方法は、工業、例えば自動車産業、航空 機産業などの分野に使用できる。

Claims

請求の範囲

[1] 移動体に加わる加速度を測定する加速度センサと、

該移動体のョーレートを測定するョーレートセンサと、

該移動体の速度を測定する速度センサと、

該速度力も実加速度を計算し、該ョーレートと該速度とから遠心力を計算し、該実 加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算 手段と、

該移動成分加速度で該加速度を補正して得られる重力加速度から姿勢角を計算 する姿勢角計算手段と、

力 なることを特徴とする姿勢角検出装置。

[2] 前記姿勢角計算手段は、前記移動成分加速度を前記加速度から除去した前記重 力加速度を計算する重力加速度計算手段と、該重力加速度から該移動体のロール 角を計算するロール角計算手段と、該重力加速度から該移動体のピッチ角を計算す るピッチ角計算手段とからなる請求項 1に記載の姿勢角検出装置。

[3] 前記移動成分加速度計算手段は、前記速度を時間で微分して前記実加速度を計 算して前記移動成分加速度の前後軸方向の分力とする前後軸加速度計算手段と、 前記ョーレートと該速度とから遠心力を計算して該移動成分加速度の左右軸方向の 分力とする左右軸加速度計算手段と、該移動体の上下軸方向における該遠心力の 分力を計算して該移動成分加速度の上下軸方向の分力とする上下軸加速度計算手 段とからなる請求項 1又は 2に記載の姿勢角検出装置。

[4] 前記上下軸加速度計算手段における前記遠心力の分力の計算は、前記姿勢角計 算手段により計算した前記ロール角又は前記ピッチ角に基づいて行い、

該姿勢角計算手段は、計算した該遠心力の分力に基づき該ロール角及び該ピッ チ角のうちの少なくとも一方を計算する請求項 3に記載の姿勢角検出装置。

[5] 移動体に加わる加速度と、該移動体のョーレートと、該移動体の速度と、を測定す る測定ステップと、

該速度力も実加速度を計算し、該ョーレートと該速度とから遠心力を計算し、該実 加速度と該遠心力との合力である移動成分加速度を計算する移動成分加速度計算 ステップと、

該移動成分加速度で該加速度を補正して得られた重力加速度から姿勢角を計算 する姿勢角計算ステップと、

を有することを特徴とする姿勢角検出方法。

[6] 前記姿勢角計算ステップは、前記移動成分加速度を前記加速度から除去した前記 重力加速度を計算する重力加速度計算ステップと、該重力加速度力 該移動体の口 一ル角を計算するロール角計算ステップと、該重力加速度力 該移動体のピッチ角 を計算するピッチ角計算ステップとからなる請求項 5に記載の姿勢角検出方法。

[7] 前記移動成分加速度計算ステップは、前記速度を時間で微分して前記実加速度 を計算して前記移動成分加速度の前後軸方向の分力とする前後軸加速度計算ステ ップと、前記ョーレートと該速度とから遠心力を計算して該移動成分加速度の左右軸 方向の分力とする左右軸加速度計算ステップと、該移動体の上下軸方向における該 遠心力の分力を計算して該移動成分加速度の上下軸方向の分力とする上下軸加速 度計算ステップとからなる請求項 5又は 6に記載の姿勢角検出方法。

[8] 前記重力加速度計算ステップは、前記重力加速度を前後軸方向、左右軸方向及 び上下軸方向の分力に分けて計算し、前記ロール角計算ステップ及び前記ピッチ角 計算ステップのうちの一方で計算した前記ロール角又は前記ピッチ角に基づ!、て該 各分力のうちの一部を計算するステップであり、

該ロール角計算ステップ及び該ピッチ角計算ステップのうちの一方は各分力のうち の残部に基づ!、て該ロール角又は該ピッチ角を計算し、該ロール角計算ステップ及 び該ピッチ角計算ステップのうちの他方は各分力のうちの一部乃至全部に基づいて 該ロール角又は該ピッチ角を計算するステップである請求項 7に記載の姿勢角検出 方法。