JP2009507272A

JP2009507272A - 運動体の姿勢角検出装置

Info

Publication number: JP2009507272A
Application number: JP2008524609A
Authority: JP
Inventors: 久義杉原; 裕野々村; 基弘藤吉
Original assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Current assignee: Toyota Motor Corp; Toyota Central R&D Labs Inc
Priority date: 2005-08-01
Filing date: 2006-08-01
Publication date: 2009-02-19
Anticipated expiration: 2026-08-01
Also published as: CN100560305C; CN101232979A; US20100138180A1; JP5167383B2; JP2007041733A; JP4751930B2; US7949487B2; JP2011203257A; WO2007015134A3; WO2007015134A2

Abstract

角速度センサ（１０）で検出された角速度は、微小角行列演算器（１２）、行列加算演算器（１４）で積分され、行列姿勢角演算器（１６）で姿勢角（角速度姿勢角）に復元される。加速度センサ（２０）で検出された加速度は、傾斜角演算器（２２）、加速度行列演算器（２４）で姿勢行列が演算され、行列姿勢角演算器（２６）で姿勢角（加速度姿勢角）に復元される。ローパスフィルタ（１８）、（２８）はそれぞれの低域成分を抽出し、差分器（３０）で両者の差分を演算してドリフト分のみを抽出する。減算器（３２）で角速度姿勢角からドリフト分を除去して出力装置（３４）から出力し、また姿勢角行列演算器（３６）で姿勢行列に変換して行列加算演算器（１４）にフィードバックする。

Description

本発明は運動体の姿勢角検出装置、特にロボット等の運動体の姿勢角を高精度に検出する技術に関する。

ロボット等の移動体の姿勢制御に加速度センサや角速度センサが用いられている。直交する３軸をｘ軸、ｙ軸、ｚ軸とすると、各軸方向の加速度を３個の加速度センサで検出し、各軸回りの角速度を３個の角速度センサで検出する。軸回りの角度、あるいは姿勢角は、角速度センサの出力を時間積分して得られ、ロール角、ピッチ角、ヨー角が算出される。

特開２００４−２６８７３０号公報には、ジャイロセンサから出力される加速度データ及び姿勢データを用いて姿勢制御する技術が開示されている。

しかしながら、角速度センサ（ヨーレートセンサ）のオフセット及びドリフトが大きいと、角速度の積分により姿勢角を求めるため、オフセット、ドリフト値が徐々に蓄積して極めて大きな値となり、時間とともに増加、発散してしまう。これを防止するために、オフセット、ドリフトの小さい角速度センサを用いればよいが、このような角速度センサは大型で重く、かつ高価である。

一方、加速度センサから重力方向を検出し、角速度センサから得られる姿勢角をこの重力方向を用いて適当なタイミングで修正することで積分による誤差の蓄積を補正することも可能であるが、運動体の運動の変化が速く重力以外の加速度が発生する頻度が高い用途に用いることができない。

本発明の目的は、高価で高精度な角速度センサを用いることなく、簡易な構成で積分誤差の蓄積を防止し運動体の姿勢角を検出することができる装置を提供することにある。

本発明は、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から前記運動体の姿勢角を角速度姿勢角として演算する角速度姿勢角演算手段と、前記加速度から運動体の姿勢角を加速度姿勢角として演算する加速度姿勢角演算手段と、前記角速度姿勢角の低域成分を抽出する角速度姿勢角低域成分抽出手段と、前記加速度姿勢角の低域成分を抽出する加速度姿勢角低域成分抽出手段と、前記角速度姿勢角の低域成分と前記加速度姿勢角の低域成分の差分を誤差として演算する差分演算手段と、前記角速度姿勢角から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記角速度姿勢角演算手段にフィードバックするフィードバック手段とを有する。

ここで、前記角速度姿勢角低域成分抽出手段及び前記加速度姿勢角低域成分抽出手段は、前記角速度姿勢角をｓｉｎ変換及びｃｏｓ変換する手段と、前記ｓｉｎ変換及びｃｏｓ変換して得られた値の低域成分を抽出する手段と、低域成分が抽出された値をａｔａｎ２演算、あるいはａｔａｎ演算する手段とを有する。

また、本発明は、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から前記運動体の姿勢角を角速度姿勢角として演算する角速度姿勢角演算手段と、前記加速度から運動体の姿勢角を加速度姿勢角として演算する加速度姿勢角演算手段と、前記角速度姿勢角と前記加速度姿勢角の差分を演算する差分演算手段と、前記差分の低域成分を誤差として抽出する低域成分抽出手段と、前記角速度姿勢角から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記角速度姿勢角演算手段にフィードバックするフィードバック手段と、を有することを特徴とする運動体の姿勢角検出装置である。

また、本発明は、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から微小時間ｔｓ経過後の微小回転角を規定する微小角行列を演算する手段と、前記微小角行列とフィードバックされた姿勢行列とから新たな角速度系姿勢行列を演算する手段と、前記加速度から傾斜角を演算する手段と、前記傾斜角から加速度系姿勢行列を演算する手段と、前記角速度系姿勢行列の低域成分を抽出する角速度系低域成分抽出手段と、前記加速度系姿勢行列の低域成分を抽出する加速度系低域成分抽出手段と、前記角速度系姿勢行列の低域成分と前記加速度系姿勢行列の低域成分の差分を誤差として演算する差分演算手段と、前記角速度系姿勢行列から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度系姿勢行列から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度系姿勢行列を前記新たな角速度系姿勢行列を演算する手段にフィードバックするフィードバック手段とを有する。

また、本発明は、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から微小時間ｔｓ経過後の微小回転角を規定する微小角４元数を演算する手段と、前記微小角４元数とフィードバックされた４元数とから新たな角速度系４元数を演算する手段と、前記加速度から傾斜角を演算する手段と、前記傾斜角から加速度系４元数を演算する手段と、前記角速度系４元数の低域成分を抽出する角速度系低域成分抽出手段と、前記加速度系４元数の低域成分を抽出する加速度系低域成分抽出手段と、前記角速度系４元数の低域成分と前記加速度系４元数の低域成分の差分を誤差として演算する差分演算手段と、前記角速度系４元数から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度系４元数から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度系４元数を前記新たな角速度系４元数を演算する手段にフィードバックするフィードバック手段とを有する。

本発明において、角速度センサで検出された角速度に基づき演算された姿勢角である角速度姿勢角は低域成分抽出手段に供給される。角速度姿勢角には、積分誤差の蓄積によるドリフト分が含まれており、姿勢角の真値を高域成分と低域成分に分けると、（高域の姿勢角真値＋低域の姿勢角真値＋低域のドリフト分）が含まれる。低域成分抽出手段は、これらの成分のうち高域成分を除去し、（低域の姿勢角真値＋低域のドリフト分）を抽出する。一方、加速度センサで検出された加速度に基づき演算された姿勢角である加速度姿勢角も低域成分抽出手段に供給される。加速度姿勢角には、積分誤差の蓄積によるドリフト分はなく、高域のノイズ成分と低域の姿勢角真値が含まれる。低域成分抽出手段は、これらの成分のうち高域成分を除去し、低域の姿勢角真値を抽出する。したがって、これらの低域成分抽出手段の出力を差分演算手段に供給し、（低域の姿勢角真値＋低域のドリフト分）−（低域の姿勢角真値）を演算することで、ドリフト分（誤差）のみを抽出することができる。抽出されたドリフト分を角速度姿勢角から除去することで、高精度に姿勢角を検出できる。また、抽出されたドリフト分を角速度姿勢角から除去してフィードバックすることで、積分誤差の蓄積防止、出力応答性の向上、出力の安定化が実現できる。なお、高域のノイズ分は被測定体の運動に伴う重力以外の加速度成分と振動加速度ノイズ、電気的なノイズ成分からなる。特に被測定体の運動に伴う重力以外の加速度成分によるノイズは、ロボット等の小型で運動量が多い被測定体では大きなノイズ成分となり姿勢角を得るために大きな障害となる。

また、本発明において、本発明において、角速度センサで検出された角速度に基づき演算された角速度系姿勢行列は低域成分抽出手段に供給される。角速度系姿勢行列には、積分誤差の蓄積によるドリフト分が含まれており、姿勢の真値を高域成分と低域成分に分けると、（高域の姿勢真値＋低域の姿勢真値＋低域のドリフト分）が含まれる。低域成分抽出手段は、これらの成分のうち高域成分を除去し、（低域の姿勢真値＋低域のドリフト分）を抽出する。一方、加速度センサで検出された加速度に基づき演算された加速度系姿勢行列も低域成分抽出手段に供給される。加速度系姿勢行列には、積分誤差の蓄積によるドリフト分はなく、高域のノイズ成分と高域の姿勢真値成分と低域の姿勢真値が含まれる。低域成分抽出手段は、これらの成分のうち高域成分を除去し、低域の姿勢真値を抽出する。したがって、これらの低域成分抽出手段の出力を差分演算手段に供給し、（低域の姿勢真値＋低域のドリフト分）−（低域の姿勢真値）を演算することで、ドリフト分（誤差）のみを抽出することができる。抽出されたドリフト分を角速度系姿勢行列から除去することで、誤差のない角速度系姿勢行列が得られ、この角速度系姿勢行列から姿勢角を演算することで、高精度に姿勢角を検出できる。また、抽出されたドリフト分を角速度系姿勢行列から除去してフィードバックすることで、積分誤差の蓄積防止、出力応答性の向上、出力の安定化が実現できる。また、角速度系姿勢行列から４元数を演算するとともに、加速度系姿勢行列から４元数を演算し、４元数の段階で低域成分を抽出してその差分を演算することによっても、同様にドリフト分を抽出することができる。姿勢行列と４元数は実質的に等価であるが、４元数を用いると演算数が削減され処理時間が短縮化される利点がある。

本発明によれば、高価で大型、重量のあるセンサを用いることなく積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出することができる。

以下、図面に基づき本発明の実施形態について、運動体としてロボット、姿勢角検出装置としてセンサユニットを例にとり説明する。

まず、第１実施形態について説明する。図１に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。ロボットの所定位置に角速度センサ１０及び加速度センサ２０を有するセンサユニット１が設けられる。

角速度センサ１０は、センサ座標系（センサユニットの座標系）における角速度を検出する。各軸回りの角速度をωｘ、ωｙ、ωｚとする。角速度センサ１０は、検出した角速度を微小角行列演算器１２に出力する。

微小角行列演算器１２は、基準座標系ＸＹＺから見たセンサ座標系ｘｙｚを算出するための座標変換行列を演算する。微小角行列については後述する。微小角行列演算器１２は、演算した微小角行列を行列加算演算器１４に出力する。

行列加算演算器１４は、微小角行列演算器１２から入力した、角速度センサ１０で検出された現在の微小角行列と、出力側からフィードバックされた姿勢角行列とを加算することで積分演算する。行列加算演算器１４は、積分した行列を行列姿勢角演算器１６に出力する。

行列姿勢角演算器１６は、入力した積分行列から姿勢角を演算し、角速度姿勢角ＳＹＡとしてローパスフィルタＬＰＦ１８に出力するとともに、減算器３２にも出力する。

ローパスフィルタ（角速度系ローパスフィルタ）１８は、角速度姿勢角ＳＹＡから低域成分を抽出して差分器３０に出力する。角速度姿勢角ＳＹＡには真の姿勢角（姿勢角真値）と積分誤差によるドリフト分が含まれており、ローパスフィルタ１８は、角速度姿勢角ＳＹＡから高域の姿勢角真値を除去し、低域の姿勢角真値とドリフト分を抽出する。

一方、加速度センサ２０は、センサ座標系ｘｙｚの各軸方向の加速度を検出する。検出された加速度をＧｘ、Ｇｙ、Ｇｚとする。加速度センサ２０は、検出された加速度を傾斜角演算器２２に出力する。

傾斜角演算器２２は、センサ座標系ｘｙｚの各軸と基準座標系ＸＹＺのＺ軸との間の角度を演算する。演算された傾斜角をλｘ、λｙ、λｚとする。傾斜角演算器２２は、演算した傾斜角を加速度行列演算器２４に出力する。

加速度行列演算器２４は、入力した傾斜角を用いて基準座標系から見たセンサ座標系に対する姿勢行列を演算し、行列姿勢角演算器２６に出力する。

行列姿勢角演算器２６は、行列姿勢角演算器１６と同様に、入力した行列から姿勢角を演算し、加速度姿勢角ＳＧＡとしてローパスフィルタＬＰＦ２８に出力する。

ローパスフィルタ（加速度系ローパスフィルタ）２８は、カットオフ周波数及び減衰率がローパスフィルタ１８と同一に設定されたフィルタであり、加速度姿勢角ＳＧＡから低域成分を抽出して差分器３０に出力する。加速度姿勢角ＳＧＡには高域成分に存在する高域の姿勢角偽値と低域成分に存在する低域の姿勢角真値が含まれている。加速度姿勢角には積分誤差によるドリフトは生じない。ローパスフィルタ２８は、カットオフ周波数及び減衰率がローパスフィルタ１８と同一に設定されたフィルタであるので、低域の姿勢角真値が周波数特性も含めて角速度系と加速度系と同一となり、減算器３２による演算で正確に両者が減算により除去され、ドリフト成分のみが出力される。但し、角速度系と加速度系からもたらされる姿勢角相当成分の応答性が予め分かっている場合は、それぞれの特性に合わせてローパスフィルタのカットオフ周波数と減衰率を調整することができる。これは、適応範囲が狭いが、運動特性が予め分かっている場合には有効な方法となり得る。

以上のように、差分器３０にはローパスフィルタ１８から（低域の姿勢角真値＋ドリフト分）、ローパスフィルタ２８から（低域の姿勢角真値）が入力される。差分器３０は両者の差分を演算し、（低域の姿勢角真値＋ドリフト分）−（低域の姿勢角真値）＝ドリフト分を抽出する。差分器３０は、差分演算して得られたドリフト分を減算器３２に出力する。

減算器３２は、行列姿勢角演算器１６から入力した角速度姿勢角ＳＹＡと差分器３０から入力したドリフト分との差分を演算し、行列姿勢角演算器１６からの角速度姿勢角ＳＹＡに含まれるドリフト分を除去する。上記のように、行列姿勢角演算器１６からの角速度姿勢角ＳＹＡには姿勢角真値（高域成分＋低域成分）とドリフト分が含まれており、減算器３２は減算により、（低域姿勢角真値＋ドリフト分）−（ドリフト分）＝低域姿勢角真値を抽出する。減算器３２は、減算して得られた姿勢角真値を出力装置３４に出力する。また、減算器３２は、姿勢角真値を姿勢角行列演算器３６に出力する。

姿勢角行列演算器３６は、入力した姿勢角から姿勢角行列を演算し、行列加算演算器１４にフィードバックする。姿勢角行列演算器３６は、行列姿勢角演算器１６の逆演算を実行して姿勢角から姿勢角行列を生成する。行列加算演算器１４は、フィードバックされた姿勢角行列と、新たに角速度センサ１０で検出された角速度に基づいて演算された微小角行列とを加算することで積分演算を実行し、角速度から姿勢角を演算する。

出力装置３４は、減算器３２からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサ（あるいはホストプロセッサ）がセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置３４は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ１０で検出された角速度や加速度センサで検出された加速度を出力装置３４に供給してもよく、出力装置３４は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置３４は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。図中に示さないが、姿勢角行列演算器３６で得られた姿勢行列を出力装置３４を介してロボットへ出力することも可能である。ロボット内部で行列演算する場合に有効な手法である。

以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ２０で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢角から除去し、フィードバックしているので、角速度センサ１０としては高価で大型、高精度なセンサを用いる必要はない。

以下、微小角行列演算器１２、行列姿勢角演算器１６、傾斜角演算器２２及び行列姿勢角演算器２６における演算について説明する。

まず、姿勢行列について説明する。基準座標系ＸＹＺにおけるセンサ座標系の表記法として、離散時間ｎにおける姿勢行列Ｔ（ｎ）で表す。姿勢行列Ｔ（ｎ）は、（１）式に示すように４×４の要素から構成される。

行列Ｔ（ｎ）の意味として、第一列（ａ，ｂ，ｃ）、第二列（ｄ，ｅ，ｆ）、第三列（ｇ，ｈ，ｉ）は、それぞれ基準座標系からみたセンサ座標系ｎのｘ軸、ｙ軸、ｚ軸の方向ベクトルを表す。第四列は、基準座標系におけるセンサ座標系ｎの原点位置を表す（一般に並進が有る場合はこの第四列に並進量が表される）。原点の移動がない場合は、位置の変換を表す第４列の１〜３行目の要素を０とおく。図７に示すように、センサ座標系ｎの原点Ｏが、基準座標系において（０，０，０）の位置にあり、ｘ軸ベクトルは基準座標系上の（ａ，ｂ，ｃ）、ｙ軸ベクトルは（ｄ，ｅ，ｆ）、ｚ軸ベクトルは（ｇ，ｈ，ｉ）の成分を持つ。

姿勢角ロール・ピッチ・ヨー角から姿勢行列Ｔ（ｎ）を求める手法を以下に説明する。減算器３２からの姿勢角に基づき姿勢角行列演算器３６で姿勢行列を演算する方法である。姿勢行列Ｔ（ｎ）を表すために行列による回転変換は、回転軸についての順序を考慮する必要がある。図８に示すように、ロボットで一般的に用いられるロール・ピッチ・ヨー角を用いる場合は、最初にｚ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｘ軸周りの回転ψの３回の回転が生じたと定義する（軸の回転順番が規定されている点に注意）。

ロール・ピッチ・ヨー角による変換行列をＲＰＹ（φ、θ、ψ）とする。ＲＰＹ（φ、θ、ψ）は回転変換行列を左から右に掛けた行列の積となり、式（２）で表される。

式（２）は具体的に式（３）で表される。

式（３）を書き下すと、式（４）で表記できる。

なお、ロール・ピッチ・ヨー角の代わりに、オイラー角を姿勢角として用いることもできる。オイラー角では、最初にｚ軸周りの回転φ、次に回転後のｙ軸周りの回転θ、最後に回転後のｚ軸周りの回転ψが起こった時の変換行列をＥｕｌｅｒ（Ｅφ、Ｅθ、Ｅψ）とおき、式（５）で表される。

式（５）は具体的に式（６）で表される。

式（６）を書き下すと、式（７）で表記できる。

基準座標系をＯ−ＸＹＺ、初期のセンサ座標系をＯ₀−ｘ₀ｙ₀ｚ₀とおく。基準座標系と時刻ｔ＝０時の座標系Ｏ₀−ｘ₀ｙ₀ｚ₀の関係を、座標変換Ａ（０）で関係づける。時刻ｔ＝ｔｎ時の座標系をＯn−ｘ_nｙ_nｚ_nとおく。各座標系の原点Ｏ，Ｏ₀，Ｏ_nは位置の移動がなく同一とする。その後運動体の姿勢変化により、図９に示すように、座標系Ｏ_(n-1)−ｘ_(n-1)ｙｚ_(n-1)からＯ_n−ｘ_nｙ_nｚ_nへ変化したとき、Ｏ_(n-1)−ｘ_(n-1)ｙ_(n-1)ｚ_(n-1)とＯ_n−ｘ_nｙ_nｚ_nはセンサ出力から求まる行列Ａ（ｎ）で関係づけられる。基準座標系からみたセンサ座標系Ｔ（ｎ）は、変換Ａ（ｎ）を右からかけていくことで式（８）で求める。センサ座標の原点が時間と共に移動する場合は、行列Ａの第４列の１〜３行目の要素に時間と共に移動した座標が逐次入れられる。行列Ａの第４列はセンサ座標系の回転に対しては影響を与えないので、ここでは特に述べない。

次に、センサ出力から微小回転行列Ａ（ｎ）行列を導出する方法について説明する。角速度センサ１０からの角速度に基づき微小角行列演算器１２で微小角行列を演算する方法である。３個の角速度センサは、センサ座標系の各軸に設置されており、図１０に示すように、センサｘ，ｙ，ｚ軸まわりの角速度を計測している。式（４）において、回転角Δφ，Δθ，Δψが十分小さいとき、

である。これよりセンサｘ軸回りの微小回転角Δψ、センサｙ軸回りの微小回転角Δθ、センサｚ軸回りの微小回転角Δφを用いて式（１１）で表すことができる。式（１１）の結果は行列の各要素が独立に各微小回転角で表せるため、回転の順番に依存しないと近似している。

微小角とセンサ出力との間には、微小回転角Δψ、Δθ、Δφ、角速度センサ出力ωｘ，ωｙ，ωｚ，サンプリング周期ｔｓから、式（１２）〜（１３）の関係がある。サンプリング周期ｔｓが回転運動に対して十分早い周期としているため、サンプリング周期ｔｓの時間内での回転は十分小さく、微小回転角とみなすことができる。

このためＡ（ｎ）行列は式（１５）で表される。

次に、姿勢行列から姿勢角を求める手法について述べる。行列加算演算器１４からの姿勢行列（積分後の姿勢行列）に基づき行列姿勢角演算器１６で姿勢角を演算する方法である。また、加速度行列演算器２４からの姿勢行列に基づき行列姿勢角演算器２６で姿勢角を演算する方法でもある。

姿勢行列Ｔ（ｎ）が式（１６）で表されている。

ヨー角φは、

姿勢角φの変域は、−π＜f≦πである。

ピッチ角θは、

姿勢角θの変域は、−π／２＜θ＜π／２である。

ロール角ψは、

姿勢角ψの変域は、 −π＜ψ≦πである。

オイラー角を用いる場合は、式（２１）〜（２３）を使う。

Ｅφは、

Ｅθは、

Ｅψは、

次に、行列の正規化について説明する。

姿勢行列Ｔ（ｎ）では、演算後、姿勢行列の各列が単位ベクトルとならないことがあるため式（２４）の各列ベクトルの大きさが１になるように式（２５）で正規化を行う。後述する図５における正規化・直交化器５４、５６の演算である。

ここでｐ1，ｐ2は式（２６）、（２７）で与えられる。

その後、正規化後の各要素をあらためて、

と置き直す。

さらに、行列の直交化について説明する。

姿勢行列Ｔ（ｎ）では演算後、姿勢行列の各列が直交した軸とならないことがあるため、式（２９）の各列ベクトルが直交する直交化処理を行う（この場合、ｚ軸を基準としている）。ｚ軸、ｙ軸に直交する新しいｘ’軸を得るため、ａ’，ｂ’，ｃ’を求める。

次に、ｚ軸、ｘ’軸に直交する新しいｙ’軸を得るために、ｄ’，ｅ’，ｆ’を求める。

求めたａ’〜ｆ’から、直交化した姿勢行列Ｔ（ｎ）を得る。

ここで、ａｔａｎ２について説明する。ａｔａｎ２（ｙ，ｘ）は、２変数ｘ，ｙを持つ計算機用関数である。通常使われるａｔａｎ関数よりも適用範囲が広い。

−π＜ξ≦π
は、
ｘ＞０、ｙ＞０の時

ｘ＞０、ｙ＜０の時

となる。同様にして、
ｘ＜０，ｙ＞０の時
ξ＝π＋ｔａｎ^-1（ｙ／ｘ）
ｘ＜０，ｙ＜０の時
ξ＝−π＋ｔａｎ-1（ｙ／ｘ）
ｘ＝０，ｙ＞０の時
ξ＝π／２
ｘ＝０，ｙ＜０の時
ξ＝−π／２
ｘ＝０，ｙ＝０の時
ξ＝０
となる。

次に、傾斜角の演算について説明する。加速度センサ２０からの加速度に基づき、傾斜角演算器２２で傾斜角を演算する方法である。傾斜角とは、センサｘ、ｙ、ｚ軸と基準Ｚ軸との間の角度λｘ、λｙ、λｚである。すなわち、
λｘ：ｘ軸とＺ軸の間の角度
λｙ：ｙ軸とＺ軸の間の角度
λｚ：ｚ軸とＺ軸の間の角度
であり、λｘ、λｙ、λｚの範囲は、０≦（λｘ、λｙ、λｚ）≦πである。図１１に、傾斜角と重力ベクトルを示す。

センサ座標に配置された加速度センサから以下のように傾斜角を求める。加速度Ｇｘ、Ｇｙ、Ｇｚを式（４０）〜（４２）用いて正規化し、正規化後の加速度Ｇｘ’、Ｇｙ’、Ｇｚ’を求める。

加速度Ｇｘ’、Ｇｙ’、Ｇｚ’から、式（４３）〜（４５）を用いて傾斜角λｘ、λｙ、λｚを求める。

次に、傾斜角λｘ、λｙ、λｚから姿勢行列Ｔ（ｎ）を求める手法について述べる。傾斜角演算器２２からの傾斜角に基づき、加速度行列演算器２４で姿勢行列を求める演算である。

以上の結果から、姿勢行列Ｔ（ｎ）を求める。

次に、姿勢行列Ｔ（ｎ）から傾斜角λｘ、λｙ、λｚを求める手法について述べる。

次に第２実施形態について説明する。図２に、本実施形態の構成ブロック図を示す。図１と異なる点は、差分器３０はローパスフィルタ１８の出力とローパスフィルタ２８の出力との差分を演算して倍率器３８、積算器４０及び差分器４２に出力する点である。倍率器３８、積算器４０、差分器４２は、ＰＩＤ(Proportional Integral Differential) 制御を実行する。すなわち、倍率器３８は、差分器３０からのドリフト分に予め設定された係数を乗じる演算を行い、その値（比例項）を減算器３２に出力する。積算器４０は、差分器３０からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値（積分項）を減算器３２に出力する。差分器４２は、差分器３０からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値（微分項）を減算器３２に出力する。各係数は固定でもよいが、後述するように可変でもよい。減算器３２は、行列姿勢角演算器１６から入力した角速度姿勢角からこれらの項の和（比例項＋積分項＋微分項）を減算し、ドリフト分を除去した運動体の姿勢角として出力装置３４及び姿勢角行列演算器３６に出力する。減算器３２の出力は姿勢角行列演算器３６で姿勢行列に変換されてフィードバックされるが、倍率器３８がフィードバックの程度を主に決定し、積算器４０が長時間ずれ量を蓄積してフィードバック量を決定し、差分器４２が短時間変化に対するフィードバック量を決定する。このようなＰＩＤ制御を用いることにより、短時間特性と長時間特性をともに向上できる。

次に第３実施形態について説明する。図３に、本実施形態の構成ブロック図を示す。図２と異なる点は、外部姿勢角４４からのＺ軸回りの姿勢角、すなわちヨー角を行列姿勢角演算器２６に供給する点である、加速度センサ２０はｘ、ｙ、ｚ各軸方向の加速度を検出するが、Ｇセンサを用いるとＺ軸回りの姿勢角であるヨー角φは不明であり、したがってヨー角φは一定値に固定する必要があるが、本実施形態ではヨー角を別途検出して行列姿勢角演算器２６に供給することで、ヨー角φも正確に算出できる。具体的には、行列姿勢角演算器２６は、上記の演算により姿勢行列Ｔ（ｎ）から姿勢角としてヨー角φ、ピッチ角θ、ロール角ψを演算するが、このうちのヨー角φを外部から入力したヨー角φに置換する。これにより、ヨー角φの発散も防止できる。

次に第４実施形態について説明する。図４に、本実施形態の構成ブロック図を示す。上記の各実施形態では、行列姿勢角演算器１６からの角速度姿勢角をローパスフィルタ１８に出力するとともに、行列姿勢角演算器２６からの加速度姿勢角をローパスフィルタ２８に出力しているが、本実施形態では、行列加算演算器１４からの姿勢行列をローパスフィルタ（ＡＬＰＦ）４６に出力するとともに、加速度行列演算器２４からの姿勢行列をローパスフィルタ（ＡＬＰＦ）４８に出力し、姿勢角段階で低域成分を抽出するのではなく、行列段階で低域成分を抽出する。ローパスフィルタ４６、４８は、４×４の姿勢行列の各行列成分に適用され、それぞれの低域成分を抽出する。ローパスフィルタ４６、４８のカットオフ周波数及び減衰率は同一に設定される。ローパスフィルタ（角速度系）４６は、姿勢行列から低域成分を抽出した後、姿勢行列を行列姿勢角演算器５０に出力する。行列姿勢角演算器５０は、行列姿勢角演算器１６と同様に姿勢行列から角速度姿勢角を演算し、差分器３０に出力する。ローパスフィルタ４６、４８は、４×４の姿勢行列の内、回転を表す成分の第一列（ａ，ｂ，ｃ）、第二列（ｄ，ｅ，ｆ）、第三列（ｇ，ｈ，ｉ）、すなわち３×３の各行列成分に適用され、それぞれの低域成分を抽出することに簡略化することもできる。また、さらに３×３の各行列成分の内の対角成分３個と非対角成分の片側３個の計６個の成分にローパスフィルタを作用させて等価的に行列フィルタを形成させることも可能である。

また、ローパスフィルタ（加速度系）４８は、姿勢行列から低域成分を抽出した後、姿勢行列を行列姿勢角演算器２６に出力する。図３と同様に、外部の外部姿勢角４４からのＺ軸回りの姿勢角、すなわちヨー角を加速度系に供給してもよい。但し、図３では外部センサで検出したヨー角φを行列姿勢角演算器２６に供給するが、本実施形態では加速度行列演算器２４に供給する。これはもちろん、ローパスフィルタ４８で姿勢行列の低域成分を抽出するためであり、外部センサで検出されたヨー角φも姿勢行列の成分として含める必要があるからである。

次に第５実施形態について説明する。図５に、本実施形態の構成ブロック図を示す。図４と異なる点は、ローパスフィルタ４６、４８の後段に正規化・直交化器５４、５６を付加した点である。ローパスフィルタ４６、４８で姿勢行列から低域成分を抽出すると、姿勢行列に歪みが生じ、正規性及び直交性が担保されなくなる。そこで、正規化・直交化器５４、５６で正規化、直交化することで歪みを修正する。正規化及び直交化の方法については上記のとおりである。

次に第６実施形態について説明する。

図６に、本実施形態の構成ブロック図を示す。図４と異なる点は、角速度センサ１０からの角速度、及び加速度センサ２０からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器５２を設ける点である。運動周波数検知器５２は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ４６、４８に出力する。運動体としてのロボットが２足歩行する場合の、２足歩行に特有の周波数等が一例である。ローパスフィルタ４６、４８は、運動周波数検知器５２から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ４６、４８はカットオフ周波数を上げるように調整する。また、運動周波数検知器５２は、ＰＩＤ制御を実行する倍率器３８、積算器４０、差分器４２それぞれに運動周波数を出力する。倍率器３８、積算器４０、差分器４２は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やＰＩＤ制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。

次に第７実施形態について説明する。

図１２に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。

行列加算演算器１４は、微小角行列演算器１２から入力した、角速度センサ１０で検出された現在の微小角行列と、出力側からフィードバックされた姿勢行列とを加算することで積分演算する。行列加算演算器１４は、積分した行列（角速度系姿勢行列）をローパスフィルタ（ＡＬＰＦ）４６に出力する。また、行列加算演算器１４は、積分した行列を減算器３２にも出力する。

ローパスフィルタ（角速度系ローパスフィルタ）４６は、角速度系姿勢行列の各成分から低域成分を抽出して差分器３０に出力する。角速度系姿勢行列には真の姿勢（姿勢真値）と積分誤差によるドリフト分が含まれており、ローパスフィルタ４６は、角速度系姿勢行列から高域の姿勢真値を除去し、低域の姿勢真値とドリフト分を抽出する。

加速度行列演算器２４は、入力した傾斜角を用いて基準座標系から見たセンサ座標系に対する姿勢行列を演算し、加速度系姿勢行列としてローパスフィルタ（ＡＬＰＦ）４８に出力する。

ローパスフィルタ（加速度系ローパスフィルタ）４８は、カットオフ周波数及び減衰率がローパスフィルタ４６と同一に設定されたフィルタであり、加速度系姿勢行列から低域成分を抽出して差分器３０に出力する。加速度系姿勢行列には高域に生じるノイズと高域の姿勢真値と低域の姿勢真値が含まれている。加速度系姿勢行列には積分誤差によるドリフトは生じない。ローパスフィルタ４８は、カットオフ周波数及び減衰率がローパスフィルタ４６と同一に設定されたフィルタであるので、低域の姿勢真値が周波数特性も含めて角速度系と加速度系と同一となり、減算器による減算で正確に両者が減算により除去され、ドリフト成分のみが出力される。但し、角速度系と加速度系からもたらされる姿勢相当成分の応答性が予め分かっている場合は、それぞれの特性に合わせてローパスフィルタのカットオフ周波数と減衰率を調整することができる。これは、適応範囲は狭いが、運動特性が予め分かっている場合には有効な方法となり得る。

以上のように、差分器３０にはローパスフィルタ４６から（低域の姿勢真値＋ドリフト分）、ローパスフィルタ４８から（低域の姿勢真値）が入力される。差分器３０は両者の差分を演算し、（低域の姿勢真値＋ドリフト分）−（低域の姿勢真値）＝ドリフト分を抽出する。差分器３０は、差分演算して得られたドリフト分を減算器３２に出力する。

減算器３２は、行列加算演算器１４から入力した角速度系姿勢と差分器３０から入力したドリフト分との差分（行列の差分）を演算し、行列加算演算器１４からの角速度系姿勢行列に含まれるドリフト分を除去する。上記のように、行列加算演算器１４からの角速度系姿勢行列には姿勢真値とドリフト分が含まれており、減算器３２は減算により、（姿勢真値＋ドリフト分）−（ドリフト分）＝姿勢真値を抽出する。減算器３２は、減算して得られた姿勢真値の角速度系姿勢行列を行列姿勢角演算器３７に出力する。行列姿勢角演算器３７は、姿勢行列から姿勢角を演算し、運動体の姿勢角として出力装置３４に出力する。また、減算器３２は、ドリフト分が除去された姿勢行列を行列加算演算器１４にフィードバックする。行列加算演算器１４は、フィードバックされた姿勢行列と、新たに角速度センサ１０で検出された角速度に基づいて演算された微小角行列とを加算することで積分演算を実行し、角速度から新たな角速度系姿勢行列を演算する。

出力装置３４は、行列姿勢角演算器３７からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサ（あるいはホストプロセッサ）がセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置３４は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ１０で検出された角速度や加速度センサで検出された加速度を出力装置３４に供給してもよく、出力装置３４は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置３４は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。

以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ２０で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢から除去しフィードバックしているので、角速度センサ１０としては高価で大型、高精度なセンサを用いる必要はない。

次に第８実施形態について説明する。

図１３に、本実施形態の構成ブロック図を示す。図１２と異なる点は、差分器３０はローパスフィルタ４６の出力とローパスフィルタ４８の出力との差分を演算して倍率器３８、積算器４０及び差分器４２に出力する点である。倍率器３８、積算器４０、差分器４２は、ＰＩＤ制御を実行する。すなわち、倍率器３８は、差分器３０からのドリフト分に予め設定された係数を乗じる演算を行い、その値（比例項）を減算器３２に出力する。積算器４０は、差分器３０からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値（積分項）を減算器３２に出力する。差分器４２は、差分器３０からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値（微分項）を減算器３２に出力する。乗算演算、積分演算及び差分演算は、ドリフト分の行列の各成分毎に実行する。各係数は固定でもよいが、後述するように可変でもよい。減算器３２は、行列加算演算器１４から入力した角速度系姿勢行列からこれらの項の和（比例項＋積分項＋微分項）を減算し、ドリフト分を除去した角速度系姿勢行列を算出して行列姿勢角演算器３７に出力する。減算器３２の出力は行列加算演算器１４にフィードバックされるが、倍率器３８がフィードバックの程度を主に決定し、積算器４０が長時間ずれ量を蓄積してフィードバック量を決定し、差分器４２が短時間変化に対するフィードバック量を決定する。このようなＰＩＤ制御を用いることにより、短時間特性と長時間特性をともに向上できる。

また、図１３では、外部姿勢角４４からのＺ軸回りの姿勢角、すなわちヨー角φを加速度行列演算器２４に供給している。加速度センサ２０はｘ、ｙ、ｚ各軸方向の加速度を検出するが、Ｇセンサを用いるとＺ軸回りの姿勢角であるヨー角φは不明であり、ヨー角φは一定値（例えば０）に固定する必要がある。本実施形態ではヨー角φを別途検出して加速度行列演算器２４に供給しているため、ヨー角φの発散も防止できる。

次に第９実施形態について説明する。

図１４に、本実施形態の構成ブロック図を示す。図１３と異なる点は、ローパスフィルタ４６、４８の後段に正規化・直交化器５４、５６を付加した点である。ローパスフィルタ４６、４８で姿勢行列から低域成分を抽出すると、姿勢行列に歪みが生じ、正規性及び直交性が担保されなくなる。そこで、正規化・直交化器５４、５６で正規化、直交化することで歪みを修正する。正規化及び直交化の方法については上記のとおりである。ローパスフィルタ４６、４８は、４×４の姿勢行列の内、回転を表す成分の第一列（ａ，ｂ，ｃ）、第二列（ｄ，ｅ，ｆ）、第三列（ｇ，ｈ，ｉ）、すなわち３×３の各行列成分に適用され、それぞれの低域成分を抽出することで簡略化できる。また、さらに３×３の各行列成分の内の対角成分３個と非対角成分の片側３個の計６個の成分にローパスフィルタを作用させて等価的に行列フィルタを形成させることも可能である。

次に第１０実施形態について説明する。図１５に、本実施形態の構成ブロック図を示す。図１３と異なる点は、角速度センサ１０からの角速度、及び加速度センサ２０からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器５２を設ける点である。運動周波数検知器５２は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ４６、４８に出力する。運動体としてのロボットが２足歩行する場合の、２足歩行に特有の周波数等が一例である。ローパスフィルタ４６、４８は、運動周波数検知器５２から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ４６、４８はカットオフ周波数を上げるように調整する。また、運動周波数検知器５２は、ＰＩＤ制御を実行する倍率器３８、積算器４０、差分器４２それぞれに運動周波数を出力する。倍率器３８、積算器４０、差分器４２は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やＰＩＤ制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。

次に第１１実施形態について説明する。図１６に、本実施形態の構成ブロック図を示す。上記の各実施形態では、姿勢行列を用いているが、姿勢行列の代わりに４元数を用いる。角速度センサ１０から得られる微小角を用いて微小角４元数を微小角４元数演算器１３により生成する。この微小角４元数を用いフィードバックされた４元数に微小回転を作用させ、回転後の４元数を４元数積算演算器１５により得る。この積算４元数は減算器３２へ出力される。減算器３２へ出力されると同じ出力を４元数用ローパスフィルタ６３へ入力する。４元数用ローパスフィルタ６３は姿勢の低域成分を抽出する。加速度センサ２０からの出力は傾斜角演算器２２で傾斜角を得た後、加速度４元数演算器２５により加速度４元数を得る。この加速度４元数は４元数用ローパスフィルタ６７へ入力する。４元数用ローパスフィルタ６７は姿勢の低域成分を抽出する。差分器３０は、角速度系４元数の低域成分と加速度系４元数の低域成分との差分を演算し、角速度系４元数に含まれるドリフト分を抽出する。差分器３０は、ドリフト分を減算器３２に出力する。

減算器３２は、４元数積算演算器１５から入力した角速度系４元数と差分器３０からのドリフト分の差分を演算し、角速度系４元数からドリフト分を除去する。減算器３２は、ドリフト分が除去された角速度系４元数を４元数積算演算器１５に出力するとともに４元数姿勢角演算器７０に出力する。

フィードバックする４元数はドリフト分が除去された４元数であるから、積分誤差の蓄積を防止できる。４元数姿勢角演算器７０は、入力した４元数から姿勢角を演算し、出力装置３４に出力する。

以下、４元数を演算する方法、及び４元数から姿勢角を演算する方法について説明する。

まず、４元数について概説する。４元数（Ｑｕａｔｅｒｎｉｏｎ）は、２次元の自由度を有する複素数を４次元の自由度に拡張したものである。４元数は、一般に、
ｑ＝ａ＋ｂｉ＋ｃｊ＋ｄｋ
と表現される。ここで、ａ、ｂ、ｃ、ｄは実数、ｉ、ｊ、ｋは基底（大きさは１）である。各基底の積は以下の関係にある。
ｉ・ｌ＝ｌ・ｉ＝ｉ
ｊ・ｌ＝ｌ・ｊ＝ｊ
ｋ・ｌ＝ｌ・ｋ＝ｋ
ｉ・ｊ＝−ｊ・ｉ＝ｋ
ｊ・ｋ＝−ｋ・ｊ＝ｉ
ｋ・ｉ＝−ｉ・ｋ＝ｊ

また、２つの４元数ｑ１＝ａ１＋ｂ１ｉ＋ｃ１ｊ＋ｄ１ｋ、ｑ２＝ａ２＋ｂ２ｉ＋ｃ２ｊ＋ｄ２ｋに対し、和及び差は、
ｑ１＋ｑ２＝ａ１＋ａ２＋（ｂ１＋ｂ２）ｉ＋（ｃ１＋ｃ２）ｊ＋（ｄ１＋ｄ２）ｋ
ｑ１−ｑ２＝ａ１−ａ２＋（ｂ１−ｂ２）ｉ＋（ｃ１−ｃ２）ｊ＋（ｄ１−ｄ２）ｋ
となり、積は、
ｑ１ｑ２＝（ａ１＋ｂ１ｉ＋ｃ１ｊ＋ｄ１ｋ）・（ａ２＋ｂ２ｉ＋ｃ２ｊ＋ｄ２ｋ）＝（ａ１ａ２−ｂ１ｂ２−ｃ１ｃ２−ｄ１ｄ２）＋（ａ１ｂ２＋ｂ１ａ２＋ｃ１ｄ２−ｄ１ｃ２）ｉ＋（ａ１ｃ２−ｂ１ｄ２＋ｃ１ａ２＋ｄ１ｂ２）ｊ＋（ａ１ｄ２＋ｂ１ｃ２−ｃ１ｂ２＋ｄ１ａ２）ｋ
となる。

虚数単位ｉ、ｊ、ｋを用いて４元数を以下のように表記する。
Ｐ＝（ａ；ｘｉ，ｙｊ，ｚｋ）

基準座標系ＸＹＺから見たセンサ座標系ｘｙｚの関係、すなわち姿勢を４元数Ｑを用いて、
Ｑ＝（ｃｏｓη／２；αｓｉｎη／２，βｓｉｎη／２，γｓｉｎη／２）
のように表す。これは、基準座標系を基準座標系で表された回転軸（α、β、γ）回りに角度η回転した座標系（センサ座標系）を示し、センサ座標系の姿勢を表している。

次に、角速度センサ出力の４元数による表記について説明する。回転角とセンサ出力との間には、微小回転角Δψ、Δθ、Δφ、角速度センサ出力ωｘ、ωｙ、ωｚ、サンプリング周期ｔｓから次の関係がある。
ΔΨ＝ωｘ・ｔｓ
Δθ＝ωｙ・ｔｓ
Δφ＝ωｚ・ｔｓ
このときの回転変換を４元数で表すと以下のようになる。
ｑｘ＝（ｃｏｓΔΨ／２；ｓｉｎΔΨ／２ｉ，０ｊ，０ｋ）
ｑｙ＝（ｃｏｓΔθ／２；０ｉ，ｓｉｎΔθ／２ｊ，０ｋ）
ｑｚ＝（ｃｏｓΔφ／２；０ｉ、０ｊ，ｓｉｎΔφ／２ｋ）
ｘ軸、ｙ軸、ｚ軸回りの回転変換をまとめて表記する。回転角αが十分小さいとき、以下のようになる。
ｑｘｑｙｑｚ＝（１＋ΔΨΔθΔφ／８；（ΔΨ／２−ΔθΔφ／４）ｉ，（Δθ／２＋ΔΨΔφ／４）ｊ，（Δφ／２−ΔΨΔθ／４）ｋ）

次に、４元数による回転変換の積算について説明する。回転変換ｑ１を
ｑ１＝（ｃｏｓη１／２；αｓｉｎη１／２，βｓｉｎη１／２，γｓｉｎη１／２）
とおき、回転変換ｑ２を
ｑ２＝（ｃｏｓη２／２；αｓｉｎη２／２，βｓｉｎη２／２，γｓｉｎη２／２）
とおく。まず基準座標系について回転変換ｑ１を行い、次に回転後の座標系について回転変換ｑ２を行ったとする。２つの回転変換による変換ｑ３は、
ｑ３＝ｑ１・ｑ２
＝（ｃｏｓη１／２；αｓｉｎη１／２，βｓｉｎη１／２，γｓｉｎη１／２）・（ｃｏｓη２／２；αｓｉｎη２／２，βｓｉｎη２／２，γｓｉｎη２／２）
である。

次に、４元数の正規化について説明する。回転を表す４元数では、演算後、変換ｑの大きさ（ノルム）が１となるように正規化を行う。回転を表す４元数がｑで表されているとき、ｑの大きさを求め、次にｑを大きさで割ることで正規化し、ｑ’を得る。
ｑ＝（ε０；ε１，ε２，ε３）
｜ｑ｜＝（ε０²＋ε１²＋ε２²＋ε３²）^0.5
ｑ’＝ｑ／｜ｑ｜
である。４元数は直交条件を維持した表記方法なので、さらなる直交化は必要ない。

次に、４元数から姿勢角への変換について説明する。４元数で表された変換ｑ＝（ε０；ε１，ε２，ε３）において、
ａ＝ε０²＋ε１²−ε２²−ε３²
ｂ＝２（ε１ε２＋ε０ε３）
ｃ＝２（ε１ε３−ε０ε２）
ｄ＝２（ε１ε２−ε０ε３）
ｅ＝ε０²−ε１²＋ε２²−ε３²
ｆ＝２（ε２ε３＋ε０ε１）
ｇ＝２（ε１ε３＋ε０ε２）
ｈ＝２（ε２ε３−ε０ε１）
ｉ＝ε０²−ε１²−ε２²＋ε３²
とおき、以下の式で姿勢角を求める。
ヨー角φ＝ａｔａｎ２（ｂ,ａ）
ピッチ角θ＝ａｔａｎ２（−ｃ，ｃｏｓφ・ａ＋ｓｉｎφ・ｂ）
ロール角ψ＝ａｔａｎ２（ｓｉｎφ・ｇ−ｃｏｓφ・ｈ，−ｓｉｎφ・ｄ＋ｃｏｓφ・ｅ）
である。

次に、姿勢角（ロール角、ピッチ角、ヨー角）から４元数への変換について説明する。姿勢角（ψ，θ，φ）によるそれぞれの回転変換を４元数で表す。
ｘ軸回りの回転を
Ｒ（Ψ；ｉ）＝ｃｏｓΨ／２＋ｉｓｉｎΨ／２
ｙ軸回りの回転を
Ｒ（θ；ｊ）＝ｃｏｓθ／２＋ｊｓｉｎθ／２
ｚ軸回りの回転を
Ｒ（φ；ｋ）＝ｃｏｓφ／２＋ｋｓｉｎφ／２
これらを掛け合わせると、
Ｓ＝Ｒ（φ；ｋ）・Ｒ（θ；ｊ）・Ｒ（Ψ；ｉ）
＝（ｃｏｓφ／２＋ｋｓｉｎφ／２）・（ｃｏｓθ／２＋ｊｓｉｎθ／２）・（ｃｏｓΨ／２＋ｉｓｉｎΨ／２）

このとき、４元数で表された変換ｑをｑ＝（ε０；ε１，ε２，ε３）と表すと、回転角はＳの計算結果における実部、回転軸はＳの虚部となる。
ε０＝ｃｏｓφ／２・ｃｏｓθ／２・ｃｏｓΨ／２＋ｓｉｎφ／２・ｓｉｎθ／２・ｓｉｎΨ／２
ε１＝−ｓｉｎφ／２・ｓｉｎθ／２・ｃｏｓΨ／２＋ｃｏｓφ／２・ｃｏｓθ／２・ｓｉｎΨ／２
ε２＝ｓｉｎφ／２・ｃｏｓθ／２・ｓｉｎΨ／２＋ｃｏｓφ／２・ｓｉｎθ／２・ｃｏｓΨ／２
ε３＝ｓｉｎφ／２・ｃｏｓθ／２・ｃｏｓΨ／２−ｃｏｓφ／２・ｓｉｎθ／２・ｓｉｎΨ／２

このとき、回転角αは、ｃｏｓα／２＝ε０から得られる。また、回転角は（ε１，ε２，ε３）である。

次に第１２実施形態について説明する。

図１７に、本実施形態の構成ブロック図を示す。図１６と異なる点は、ＰＩＤ制御を実行するための倍率器３８、積算器４０及び差分器４２を設けた点と、外部姿勢角４４で検出したヨー角を加速度４元数演算器２５に供給した点である。図１３に対応する構成である。倍率器３８は、差分器３０からのドリフト分（４元数）に予め設定された係数を乗じる演算を行い、その値（比例項）を減算器３２に出力する。積算器４０は、差分器３０からのドリフト分（４元数）を時間積算した値に予め設定された係数を乗じる演算を行い、その値（積分項）を減算器３２に出力する。差分器４２は、差分器３０からのドリフト分（４元数）の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値（微分項）を減算器３２に出力する。各係数は固定でもよく可変でもよい。減算器３２は、入力した角速度系４元数とこれらの項の和（比例項＋積分項＋微分項）の差分を演算し、ドリフト分を除去した角速度系４元数を算出して４元数姿勢角演算器７０に出力する。

次に第１３実施形態について説明する。図１８に、本実施形態の構成ブロック図を示す。図１７と異なる点は、ローパスフィルタ６３、６７の後段に正規化器７２、７４を設けた点である。図１４に対応する構成である。角速度系４元数及び加速度系４元数はそれぞれローパスフィルタ６３、６７で処理されるが、この処理に伴い歪みが生じ、非正規化され得る。そこで、正規化器７２、７４で歪みを補正することにより、姿勢角の検出精度を維持できる。

次に第１４実施形態について説明する。図１９に、本実施形態の構成ブロック図を示す。図１７と異なる点は、角速度センサ１０からの角速度、及び加速度センサ２０からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器５２を設ける点である。図１５に対応する構成である。運動周波数検知器５２は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ６３、６７に出力する。ローパスフィルタ６３、６７は、運動周波数検知器５２から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ６３、６７はカットオフ周波数を上げるように調整する。また、運動周波数検知器５２は、ＰＩＤ制御を実行する倍率器３８、積算器４０、差分器４２それぞれに運動周波数を出力する。倍率器３８、積算器４０、差分器４２は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やＰＩＤ制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。

次に第１５実施形態について説明する。図２０に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。図１と異なるのは、ローパスフィルタ１８、２８でそれぞれｓｉｎ変換、ｃｏｓ変換、ａｔａｎ２変換を行う（ＳＣＡ変換）ことである。

加速度行列演算器２４は、入力した傾斜角を用いて基準座標系から見たセンサ座標系を演算し、行列姿勢角演算器２６に出力する。

出力装置３４は、減算器３２からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサ（あるいはホストプロセッサ）がセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置３４は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ１０で検出された角速度や加速度センサで検出された加速度を出力装置３４に供給してもよく、出力装置３４は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置３４は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。

以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ２０で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢角から除去しフィードバックしているので、角速度センサ１０としては高価で大型、高精度なセンサを用いる必要はない。一方、行列姿勢角演算器１６、２６は、姿勢行列から姿勢角を算出する際に、関数ａｔａｎ２を用いて演算するため、本来の姿勢角は連続的に変化しているにもかかわらず、演算上は姿勢角が不連続的に変化してしまう事態が生じ得る。これが姿勢角表現における不連続性である。そこで、行列姿勢角演算器１６、２６からの姿勢角をそのまま用いてローパスフィルタで低域成分を抽出するのではなく、連続的な姿勢角に変換した後にその低域成分を抽出する。

図２１に、ローパスフィルタ１８の構成ブロック図を示す。ローパスフィルタ１８は、姿勢角をｓｉｎ変換、ｃｏｓ変換する変換器及びａｔａｎ２演算器、あるいはａｔａｎ演算器を有する。ローパスフィルタ２８も同様の構成であるためその説明は省略する。

ｓｉｎ・ｃｏｓ変換器１８ａ、１８ｂ、１８ｃは、それぞれ３軸の姿勢角に対してｓｉｎ変換及びｃｏｓ変換を施して出力する。例えば、角速度姿勢角がそれぞれＳ１ψ、Ｓ１θ、Ｓ１φであるとすると、ｓｉｎ・ｃｏｓ変換器１８ａは、Ｓ１ψに対して、ｓｉｎ（Ｓ１ψ）及びｃｏｓ（Ｓ１ψ）を演算して出力する。ｓｉｎ変換及びｃｏｓ変換された値は、それぞれローパスフィルタ１８ｄ、１８ｅに出力される。ローパスフィルタ１８ｄ、１８ｅは、ｓｉｎ変換及びｃｏｓ変換された値からそれぞれ低域成分を抽出し、ａｔａｎ２演算器１８ｊに出力する。ａｔａｎ２演算器１８ｊは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２を演算し、姿勢角Ｓ２ψとして出力する。また、ｓｉｎ・ｃｏｓ変換器１８ｂは、Ｓ１θに対して、ｓｉｎ（Ｓ１θ）及びｃｏｓ（Ｓ１θ）を演算して出力する。ｓｉｎ変換及びｃｏｓ変換された値は、それぞれローパスフィルタ１８ｆ、１８ｇに出力される。ローパスフィルタ１８ｆ、１８ｇは、ｓｉｎ変換及びｃｏｓ変換された値からそれぞれ低域成分を抽出し、ａｔａｎ演算器１８ｋに出力する。ａｔａｎ演算器１８ｋは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎを演算し、姿勢角Ｓ２θとして出力する。また、ｓｉｎ・ｃｏｓ変換器１８ｃは、Ｓ１φに対して、ｓｉｎ（Ｓ１φ）及びｃｏｓ（Ｓ１φ）を演算して出力する。ｓｉｎ変換及びｃｏｓ変換された値は、それぞれローパスフィルタ１８ｈ、１８ｉに出力される。ローパスフィルタ１８ｈ、１８ｉは、ｓｉｎ変換及びｃｏｓ変換された値からそれぞれ低域成分を抽出し、ａｔａｎ２演算器１８ｍに出力する。ａｔａｎ２演算器１８ｍは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２を演算し、姿勢角Ｓ２φとして出力する。

このように、ｓｉｎ・ｃｏｓ変換とａｔａｎ２演算、あるいはａｔａｎ演算を組み合わせることで、姿勢角の不連続性によるローパスフィルタの動作不良をなくし、安定して低域成分を抽出できる。ｓｉｎ変換後、ｃｏｓ変換後の値は±１の範囲内で連続で滑らかに変化するので、フィルタ処理において発散や異常な値を示すことがない。フィルタ処理後のｓｉｎ変換値とフィルタ処理後のｃｏｓ変換値を用いてａｔａｎ２演算処理により、姿勢角成分に再変換するので、姿勢角表現範囲を超えるおそれがなく、姿勢角成分のフィルタ処理が実現される。

次に第１６実施形態について説明する。

図２２に、本実施形態の構成ブロック図を示す。図２０と異なる点は、差分器３０はローパスフィルタ１８の出力とローパスフィルタ２８の出力との差分を演算して倍率器３８、積算器４０及び差分器４２に出力する点である。倍率器３８、積算器４０、差分器４２は、ＰＩＤ制御を実行する。すなわち、倍率器３８は、差分器３０からのドリフト分に予め設定された係数を乗じる演算を行い、その値（比例項）を減算器３２に出力する。積算器４０は、差分器３０からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値（積分項）を減算器３２に出力する。差分器４２は、差分器３０からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値（微分項）を減算器３２に出力する。各係数は固定でもよいが、後述するように可変でもよい。減算器３２は、行列姿勢角演算器１６から入力した角速度姿勢角からこれらの項の和（比例項＋積分項＋微分項）を減算し、ドリフト分を除去した運動体の姿勢角として出力装置３４及び姿勢角行列演算器３６に出力する。減算器３２の出力は姿勢角行列演算器３６で姿勢行列に変換されてフィードバックされるが、倍率器３８がフィードバックの程度を主に決定し、積算器４０が長時間ずれ量を蓄積してフィードバック量を決定し、差分器４２が短時間変化に対するフィードバック量を決定する。このようなＰＩＤ制御を用いることにより、より、短時間特性と長時間特性をともに向上できる。

次に第１７実施形態について説明する。図２３に、本実施形態の構成ブロック図を示す。図２２と異なる点は、外部姿勢角４４からのＺ軸回りの姿勢角、すなわちヨー角を行列姿勢角演算器２６に供給する点である。加速度センサ２０はｘ、ｙ、ｚ各軸方向の加速度を検出するが、Ｇセンサを用いるとＺ軸回りの姿勢角であるヨー角φは不明であり、したがってヨー角φは一定値に固定する必要があるが、本実施形態ではヨー角を別途検出して行列姿勢角演算器２６に供給することで、ヨー角φも正確に算出できる。具体的には、行列姿勢角演算器２６は、上記の演算により姿勢行列Ｔ（ｎ）から姿勢角としてヨー角φ、ピッチ角θ、ロール角ψを演算するが、このうちのヨー角φを外部から入力したヨー角φに置換する。これにより、ヨー角φの発散も防止できる。

また、本実施形態では、差分器３０の代わりに差分器３１を設けている。この差分器３１は、ローパスフィルタ１８，２８と同様にｓｉｎ・ｃｏｓ変換器及びａｔａｎ２演算器、あるいはａｔａｎ演算器を有する。

図２４に、差分器３１の構成ブロック図を示す。ローパスフィルタ１８からの角速度姿勢角及びローパスフィルタ２８からの加速度姿勢角は、ともに差分器３１に供給される。３軸それぞれの角速度姿勢角をξｎ０ψ、ξｎ０θ、ξｎ０φとし、３軸それぞれの加速度姿勢角をξｍ０ψ、ξｍ０θ、ξｍ０φとする。

ｓｉｎ・ｃｏｓ変換器３１ａは、ξｎ０ψに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ２演算器３１ｇに出力する。ａｔａｎ２演算器３１ｇは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２演算を行い、差分器３１ｎに出力する。一方、ｓｉｎ・ｃｏｓ変換器３１ｂは、ξｍ０ψに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ２演算器３１ｈに出力する。ａｔａｎ２演算器３１ｈは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２演算を行い、差分器３１ｎに出力する。差分器３１ｎは両者の差分を演算し、ドリフト分を抽出する。差分器３１ｎは、ｓｉｎ・ｃｏｓ変換器３１ｒにドリフト分を出力する。ｓｉｎ・ｃｏｓ変換器３１ｒ及びａｔａｎ２演算器３１ｕは同様な演算を行い、ドリフト分としてξｄ１ψを出力する。ｓｉｎ・ｃｏｓ変換器３１ｃは、ξｎ０θに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ演算器３１ｉに出力する。ａｔａｎ演算器３１ｉは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ演算を行い、差分器３１ｐに出力する。一方、ｓｉｎ・ｃｏｓ変換器３１ｄは、ξｍ０θに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ演算器３１ｊに出力する。ａｔａｎ演算器３１ｊは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ演算を行い、差分器３１ｐに出力する。差分器３１ｐは両者の差分を演算し、ドリフト分を抽出する。差分器３１ｐは、ｓｉｎ・ｃｏｓ変換器３１ｓにドリフト分を出力する。ｓｉｎ・ｃｏｓ変換器３１ｓ及びａｔａｎ演算器３１ｖは同様な演算を行い、ドリフト分としてξｄ１θを出力する。ｓｉｎ・ｃｏｓ変換器３１ｅは、ξｎ０φに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ２演算器３１ｋに出力する。ａｔａｎ２演算器３１ｋは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２演算を行い、差分器３１ｑに出力する。一方、ｓｉｎ・ｃｏｓ変換器３１ｆは、ξｍ０φに対してｓｉｎ変換及びｃｏｓ変換を行い、それぞれの値をａｔａｎ２演算器３１ｍに出力する。ａｔａｎ２演算器３１ｍは、ｓｉｎ変換値及びｃｏｓ変換値を用いてａｔａｎ２演算を行い、差分器３１ｑに出力する。差分器３１ｑは両者の差分を演算し、ドリフト分を抽出する。差分器３１ｑは、ｓｉｎ・ｃｏｓ変換器３１ｔにドリフト分を出力する。ｓｉｎ・ｃｏｓ変換器３１ｔ及びａｔａｎ２演算器３１ｗは同様な演算を行い、ドリフト分としてξｄ１φを出力する。

以上のようにして、角速度姿勢角、加速度姿勢角の不連続性による異常計算発生を防止している。また、姿勢角差を演算したときの姿勢角の不連続性による異常計算発生を防止している。また、ドリフト分の姿勢角の不連続性も除去してＰＩＤ制御の動作不良を防止できる。

図２５に、本実施形態の処理フローチャートを示す。まず、角速度姿勢角ξｎ１と加速度姿勢角ξｍ１に対してそれぞれｓｉｎ・ｃｏｓ変換を行い、ａｔａｎ２、あるいはａｔａｎ演算を行う（Ｓ１０１）。次に、演算後の値の差分を演算する（Ｓ１０２）。演算して得られた値ξｄ０＝ξｍ１−ξｎ１はドリフト分である。このドリフト分に対して、さらにｓｉｎ・ｃｏｓ変換及びａｔａｎ２演算、あるいはａｔａｎ演算を行ってξｄ１を算出する（Ｓ１０３）。ドリフト分ξｄ１を演算した後、このドリフト分に対してＰＩＤ制御を行う。すなわち、比例項、積分項、微分項を、
ξｄ１ｐ＝Ｐ・ξｄ１
ξｄ１ｉ＝Ｉ・ξｄ１
ξｄ１ｄ＝Ｄ・ξｄ１
と演算し、これらを加算してξｓ０＝ξｄ１ｐ＋ξｄ１ｉ＋ξｄ１ｄを算出する（Ｓ１０４、Ｓ１０５）。

その後、角速度姿勢角ξｎ１とξｓ０との差分を演算すればよいが、姿勢角はψ、φがξｍ＝−πとξｐ＝＋πの間の値、θがξｍ＝−π／２とξｐ＝＋π／２の間の値として定義されているから、ψ、φについてはξｓ０の値が−πと＋πの間の値を超えると制御が不安定になる。そこで、ξｓ０がこの範囲を超える場合には、この範囲内の直近の値に置き換える必要が生じる。そこで、まず、ξｓ０がξｍ＝−πより小さいか否かを判定し（Ｓ１０６）、ξｍ＝−πより小さい場合には、ξｓ０をξｍ＝−πに置き換えてξｓ１とする（Ｓ１０８）。また、ξｓ０がξｐ＝πより大きい場合も（Ｓ１０７）、ξｓ０をξｐ＝πに置き換えてξｓ１とする（Ｓ１０９）。ξｓ０がξｍ＝−πとξｐ＝πの間である場合には問題ないので、そのままの値をξｓ１とし、ξｎ１との差分を演算する（Ｓ１１１）。一方、θについてはξｓ０の値がξｍ＝−π／２とξｐ＝π／２の間の値を超えると制御が不安定になる。そこで、ξｓ０がこの範囲を越える場合には、この範囲内の直近の値に置き換える必要が生じる。そこで、まず、ξｓ０がξｍ＝−π／２より小さいか否かを判定し（Ｓ１０６）、ξｍ＝−π／２より小さい場合には、ξｓ０をξｍ＝−π／２＋δに置き換えてξｓ１とする（Ｓ１０８）。また、ξｓ０がξｐ＝π／２より大きい場合も（Ｓ１０７）、ξｓ０をξｐ＝π／２−δに置き換えてξｓ１とする（Ｓ１０９）。ここで、δは実数であり演算で有効な最小数値を表す。ξｓ０がξｍ＝−π／２とξｐ＝π／２の間である場合には問題ないので、そのままの値をξｓ１とし、ξｎ１との差分を演算する（Ｓ１１１）。但し、ξｓ０が−π／２となるときは−π／２＋δ、π／２となるときはπ／２−δとする。以上の処理により、安定な姿勢角を出力できる。

このように、本実施形態ではｓｉｎ変換・ｃｏｓ変換・ａｔａｎ２変換（ＳＣＡ変換）を行ってパラメータの不連続性を解消しているが、以下に姿勢角の不連続性及び差分器３１の処理について補足説明する。

次に姿勢角の不連続性について説明する。姿勢角はＸ軸回りのψ、Ｙ軸回りのθ、Ｚ軸回りのφを用いて（ψ、θ、φ）で表す。それぞれの範囲は、
−π＜ψ≦π（ｒａｄ）あるいは−１８０＜ψ≦１８０（ｄｅｇ）
−π／２＜θ＜π／２（ｒａｄ）あるいは−９０＜θ＜９０（ｄｅｇ）
−π＜φ≦π（ｒａｄ）あるいは−１８０＜φ≦１８０（ｄｅｇ）
である。

図２６に、Ｚ軸回りに回転したときの回転角度と姿勢角φとの関係を示す。Ｚ軸回転が−７２０〜７２０ｄｅｇに変化するとき、φは±１８０ｄｅｇの範囲を振動し１８０ｄｅｇを超えるときに−１８０ｄｅｇ付近へ急激に遷移する。これが不連続性の発生メカニズムである。運動体は時間に対して連続に回転（図ではＺ軸回転で表示）しているが、姿勢角φは不連続となる。

図２７に、具体的にφ＝１８０ｄｅｇ付近で±２０ｄｅｇのｓｉｎ波的な連続回転振動を与えたときの状態を示す。図において、Ｚ軸回転１８０±２０ｄｅｇのｓｉｎ波的な振動となっており連続的に変化する。しかし、φは±１８０ｄｅｇの間を不連続に変化する。

図２８及び図２９に、本発明による結果をＳＣＡ処理として同時に示す。これによるとＳＣＡ処理による波形は図２７に示したφと同じ波形を示しており、従来の波形Ｍφのような歪んだ波形あるいは全く異なった波形となる問題を解決していることが分かる。ψについても同様である。これにより、高精度で動作範囲の広い姿勢角検出を可能としている。

図３０に、Ｙ軸回りに回転したときの回転速度とθの関係を示す。Ｙ軸回転が−７２０〜７２０ｄｅｇに変化するとき、θは±９０ｄｅｇの範囲を振動し９０ｄｅｇを超えるときに−９０ｄｅｇ付近へ急激に遷移する。これが不連続性の発生メカニズムである。運動体は時間に対して連続に回転しているが、姿勢角θは不連続となる。

図３１に、具体的にθ＝９０ｄｅｇ付近で±２０ｄｅｇのｓｉｎ波的な連続回転振動を与えたときの状態を示す。Ｙ軸回転は連続的に変化するが、θは±９０ｄｅｇの間を不連続に変化する。

図３２及び図３３に、本発明による結果をＳＣＡ処理として同時に示す。これによるとＳＣＡ処理による波形は図２７に示したθと同じ波形を示しており、従来の波形Ｍθのような歪んだ波形あるいは全く異なった波形となる問題を解決していることが分かる。これにより、高精度で動作範囲の広い姿勢角検出を可能としている。

次にＳＣＡ差分器について説明する。差分器３１は、姿勢角（ψ、θ、φ）の各成分に対して差分を行う。例えばφに関しては、図２６において角速度姿勢角ξｎｏφが１７５ｄｅｇ、加速度姿勢角ξｍｏが−２５ｄｅｇとする。差演算するとξｎｏ−ξｍｏ＝２００ｄｅｇとなる。これは、φの定義範囲±１８０ｄｅｇを超えているので不合理が生じる。このように、演算処理によりφが定義範囲を超えてしまうことが容易に発生する。そこで、差分器３１は定義範囲に入れる処理を行う。これがｓｉｎ・ｃｏｓ・ａｔａｎ２変換（ＳＣＡ変換）である。具体的には、ξｎｏφ−ξｍｏφのｓｉｎ値とｃｏｓ値を得る。そして、ａｔａｎ２演算によりｓｉｎ値とｃｏｓ値を用いてξｄ１φを得る。このとき、ξｄ１φは定義範囲に入っている。条件次第で２００−３６０＝−１６０ｄｅｇにする方法もある。しかし、±１８０ｄｅｇを大きく超えた、±３６０ｄｅｇ、±５４０ｄｅｇ、±７２０ｄｅｇ、・・・と考えられるケースが多く、条件次第では極めて多くの、本質的には無限に組合せを記述する必要が生じる。また、条件次第では演算上大きな数値変化が生じ不連続となる。そのため、取扱が非常に困難となる。実際はノイズが重畳した信号を対象とするので、±１８０ｄｅｇ付近でノイズ重畳したとき、極めて大きい不連続な振動が発生することになる。これは、演算精度との関係で、演算上、不安定になりやすく、精度の高い、安定した出力を得ることができなくなる。これを滑らかに算術的に実行する方法がＳＣＡ変換処理である。これは、計算処理過程で定義範囲を超えたデータを定義範囲に入れる処理で、正規化に似た処理である。

ところで、ξｎｏφ自体も途中の演算で定義範囲を超えていることがある。そこで、ξｎｏφをｓｉｎとｃｏｓで表現し、ａｔａｎ２を演算することにより、まずξｎｏφが±１８０ｄｅｇ内に入るようにする。ξｍｏφも同様に定義範囲に正規化される。次に、差分器３１でξｎｏφ−ξｍｏφの演算を行う。この差分器の出力は状態によってはφの定義範囲を超える。そこで、ｓｉｎ・ｃｏｓ・ａｔａｎ２変換により定義範囲に入れ直し、正規化を行う。姿勢角ψ、θ、φの内、ψ、φの定義域は−π〜πでありａｔａｎ２処理の出力範囲と同じなのでａｔａｎ２演算を行う。一方、θの定義域は−π／２〜π／２なのでａｔａｎ２処理の出力範囲の半分となる。そこで、θはａｔａｎ演算を用いる。ａｔａｎの出力範囲は−π／２〜π／２でθと一致する。ａｔａｎ（ｙ，ｘ）＝ｔａｎ^-1（ｙ，ｘ）はｘ小の領域ではｔａｎ（ｙ／ｘ）の分母が小さいため誤差が大きくなる。このため、ｘがｙに対して小さいときにはｃｏｔ^-1（ｙ，ｘ）を用いる。

以上、本発明の実施形態について説明したが、本発明はこれに限定されず種々の変更が可能である。

例えば、図６の構成では運動周波数検知器５２で２足歩行等ロボットの運動に特有の周波数を検出してローパスフィルタ４６、４８に出力しているが、図１〜図３の構成におけるローパスフィルタ１８、２８に出力し、これらのローパスフィルタ１８、２８のカットオフ周波数を適応的に可変設定してもよい。

また、図１の構成では、行列姿勢角演算器（角速度系）１６からの角速度系姿勢角をローパスフィルタ１８に出力し、行列姿勢角演算器（加速度系）２６からの加速度姿勢角をローパスフィルタ２８に出力しているが、行列姿勢角演算器１６からの角速度系姿勢角及び行列姿勢角演算器２６からの加速度姿勢角をともに直接差分器３０に出力し、差分器３０からの差分を単一のローパスフィルタに入力して低域成分を出力し、この低域成分をドリフト分として減算器３２に出力してもよい。これにより、ＬＰＦの数を減らせるという利点がある。図２、図３の構成についても同様である。

また、図１〜図６におけるローパスフィルタとしては、アナログフィルタあるいはデジタルフィルタのいずれを用いてもよい。デジタルフィルタとしては、ＦＩＲ（Ｆｉｎｉｔｅ−ｄｕｒａｔｉｏｎＩｍｐｕｌｓｅＲｅｓｐｏｎｓｅ）フィルタあるいはＩＩＲ（Ｉｎｆｉｎｉｔｅ−ｄｕｒａｔｉｏｎＩｍｐｕｌｓｅＲｅｓｐｏｎｓｅ）フィルタのいずれでもよい。

また、各構成において、出力装置３４から姿勢角、角速度、加速度の少なくともいずれかをロボットのメインプロセッサに出力しているが、これに加えて、行列加算演算器１４で演算された姿勢行列、加速度行列演算器２４で演算された姿勢行列、差分器３０で演算されたドリフト分、４元数ＬＰＦ６３、６７等で抽出された４元数等を出力装置３４に出力し、出力装置３４からメインプロセッサに出力してもよい。

また、図２０、図２２、図２３の各構成においても、角速度センサ１０からの角速度、及び加速度センサ２０からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器５２を設けてもよい。運動周波数検知器は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ１８、２８に出力する。運動体としてのロボットが２足歩行する場合の、２足歩行に特有の周波数等が一例である。ローパスフィルタ１８、２８は、運動周波数検出器から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ１８、２８はカットオフ周波数を上げるように調整する。また、運動周波数検出器は、ＰＩＤ制御を実行する倍率器３８、積算器４０、差分器４２それぞれに運動周波数を出力する。倍率器３８、積算器４０、差分器４２は、入力した運動周波数に応じて乗算係数を調整する。

以上、本発明の実施形態について説明したが、各実施形態の方式はそれぞれ以下のような固有の効果を奏する。すなわち、姿勢角を用いた方式では、出力はロボット、車両、船舶、航空関係で制御等に用いられる姿勢角である。姿勢角そのものを装置内での補正ループの制御対象としているので、実現象に適した安定した出力が得られる。また、適応する計測・制御対象により装置内の制御パラメータを最適化する際に、実運動による姿勢角と適合性が良く、効率良く各種パラメータを最適化できる。そのため、最適化に必要とされる時間や労力が少なくて済み、応用に適した特性が得られる。

本方式によれば、姿勢角を主体に活用する計測・制御対象に適した方式であり、従来不可能あるいは不十分であったロボットの姿勢計測・制御や車両の姿勢計測・制御が可能となる。また、姿勢行列を用いた方式では、出力はロボット、車両、船舶、航空関係で計測・制御等に用いられる姿勢角であるが、内部演算では姿勢行列を補正ループ対象としている。そのため、内部演算において特異点等の回避等の特殊処理を行わないので分かりやすいという利点がある。また、外部に姿勢行列そのものを出力することもできる。従って、姿勢行列を用いて計測・制御を行う計測・制御対象に適した方式である。姿勢行列は要素数が姿勢角より多くデータ数が多い。そのため通信に要する時間やデータ処理が煩雑になるという欠点がある。しかし、データ数が多い分、特異点の問題を回避できる点や、拡大、縮小、変形座標系への対応が可能であるという自由度の高さをもっている。通信速度が十分速く、計測・制御側の演算速度が十分速く、演算に必要なメモリ領域が大きな場合は使いやすいという利点がある。従って、高速通信網を持ち、演算速度の速い高性能な計測・制御処理システムに応用する場合、例えば産業用機器、原子力発電所等での高度な計測・制御機器、宇宙用機器等において本方式は特に効果を発揮する。

また、４元数を用いた方式では、出力はロボット、車両、船舶、航空関係で計測・制御等に用いられる姿勢角であるが、内部演算では４元数を補正ループ対象としている。４元数を用いているので演算に必要なデータ量や演算ステップを少なくできるという利点を有する。従って、装置において少ないメモリ、中規模の演算速度をもつプロセッサを用いて本機能を実現できる利点がある。その結果、コストの低減が可能となる。しかしながら、４元数は実空間での振る舞いが分かりにくい表記方法であるので姿勢角に変換して用いている。従って、本方式は計測・制御対象の特性が明確であり、同様な運動が多い対象に適した方式であり、汎用ロボットや量産車両、小規模産業用機器の計測・制御に適した方式である。

第１実施形態の構成ブロック図である。第２実施形態の構成ブロック図である。第３実施形態の構成ブロック図である。第４実施形態の構成ブロック図である。第５実施形態の構成ブロック図である。第６実施形態の構成ブロック図である。基準座標系（ＸＹＺ）とセンサ座標系（ｘｙｚ）との関係を示す図である。基準座標系における姿勢角（ロール角、ピッチ角、ヨー角）を示す図である。センサ座標系ｎの時間的変化を示す図である。センサ座標系における微小回転角を示す図である。傾斜角を示す図である。第７実施形態の構成ブロック図である。第８実施形態の構成ブロック図である。第９実施形態の構成ブロック図である。第１０実施形態の構成ブロック図である。第１１実施形態の構成ブロック図である。第１２実施形態の構成ブロック図である。第１３実施形態の構成ブロック図である。第１４実施形態の構成ブロック図である。第１５実施形態の構成ブロック図である。図２０におけるローパスフィルタの構成ブロック図である。第１６実施形態の構成ブロック図である。第１７実施形態の構成ブロック図である。図２３における差分器の構成ブロック図である。前記１７実施形態の処理フローチャートである。Ｚ軸回転と姿勢角φとの関係を示すグラフ図である。Ｚ軸回転１８０±２０ｄｅｇにおける姿勢角φを示すグラフ図である。ＬＰＦ（ｆｃ＝中）を通過した回転角、姿勢角φ、ＳＣＡ処理の各信号を示すグラフ図である。ＬＰＦ（ｆｃ＝小）を通過した回転角、姿勢角φ、ＳＣＡ処理の各信号を示すグラフ図である。Ｙ軸回転と姿勢角θとの関係を示すグラフ図である。Ｙ軸回転９０±２０ｄｅｇにおける姿勢角θを示すグラフ図である。ＬＰＦ（ｆｃ＝中）を通過した回転角、姿勢角θ、ＳＣＡ処理の各信号を示すグラフ図である。ＬＰＦ（ｆｃ＝小）を通過した回転角、姿勢角θ、ｓｉｎｃｏｓ変換の各信号を示すグラフ図である。

Claims

運動体の姿勢角検出装置であって、
運動体の角速度を検出する角速度センサと、
前記運動体の加速度を検出する加速度センサと、
前記角速度から前記運動体の姿勢角を角速度姿勢角として演算する角速度姿勢角演算手段と、
前記加速度から運動体の姿勢角を加速度姿勢角として演算する加速度姿勢角演算手段と、
前記角速度姿勢角の低域成分を抽出する角速度姿勢角低域成分抽出手段と、
前記加速度姿勢角の低域成分を抽出する加速度姿勢角低域成分抽出手段と、
前記角速度姿勢角の低域成分と前記加速度姿勢角の低域成分の差分を誤差として演算する差分演算手段と、
前記角速度姿勢角から前記誤差を除去する誤差除去手段と、
前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記運動体の姿勢角として出力する出力手段と、
前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記角速度姿勢角演算手段にフィードバックするフィードバック手段と、
を有する運動体の姿勢角検出装置。
請求項１記載の装置において、
前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した値を加算して得られた値を前記角速度姿勢角から減算することで前記誤差を除去する運動体の姿勢角検出装置。
請求項１、２のいずれかに記載の装置において、さらに、
前記加速度センサとは別個に前記運動体の鉛直軸回りの姿勢角を検出して前記加速度姿勢角演算手段に出力する出力手段を有する運動体の姿勢角検出装置。
請求項１〜３のいずれかに記載の装置において、
前記角速度姿勢角演算手段は、
前記角速度から微小時間Δｔ経過後の微小回転角を規定する微小角行列を演算する手段と、
前記微小角行列と、前記フィードバック手段からフォードバックされた角速度姿勢角に対応する姿勢行列とから新たな角速度系姿勢行列を演算する手段と、
前記新たな角速度系姿勢行列から前記角速度姿勢角を演算する手段と、
を有し、
前記加速度姿勢角演算手段は、
前記加速度から傾斜角を演算する手段と、
前記傾斜角から加速度系姿勢行列を演算する手段と、
前記加速度系姿勢行列から前記加速度姿勢角を演算する手段と、
を有する運動体の姿勢角検出装置。
請求項４記載の装置において、
前記角速度姿勢角低域成分抽出手段は、前記角速度姿勢角を演算する手段からの前記角速度姿勢角の低域成分を抽出する角速度系ローパスフィルタを有し、
前記加速度姿勢角低域成分抽出手段は、前記加速度姿勢角を演算する手段からの前記加速度姿勢角の低域成分を抽出する加速度系ローパスフィルタを有し、
前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定される運動体の姿勢角検出装置。
請求項４記載の装置において、
前記角速度姿勢角低域成分抽出手段は、前記新たな角速度系姿勢行列を演算する手段からの前記新たな角速度系姿勢行列の低域成分を抽出する角速度系ローパスフィルタを有し、
前記加速度姿勢角低域成分抽出手段は、前記加速度系姿勢行列を演算する手段からの前記加速度系姿勢行列の低域成分を抽出する加速度系ローパスフィルタを有し、
前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定される運動体の姿勢角検出装置。
請求項６記載の装置において、さらに、
前記角速度系ローパスフィルタからの角速度系姿勢行列を正規化及び直交化する手段と、
前記加速度系ローパスフィルタからの加速度系姿勢行列を正規化及び直交化する手段
を有する運動体の姿勢角検出装置。
請求項５〜７のいずれかに記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、
前記運動周波数に応じて前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタのカットオフ周波数が可変設定される運動体の姿勢角検出装置。
請求項２記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、前記運動周波数に応じて前記倍率器、積分器及び差分器の係数が可変設定される運動体の姿勢角検出装置。
運動体の姿勢角検出装置であって、
運動体の角速度を検出する角速度センサと、
前記運動体の加速度を検出する加速度センサと、
前記角速度から前記運動体の姿勢角を角速度姿勢角として演算する角速度姿勢角演算手段と、
前記加速度から運動体の姿勢角を加速度姿勢角として演算する加速度姿勢角演算手段と、
前記角速度姿勢角と前記加速度姿勢角の差分を演算する差分演算手段と、
前記差分の低域成分を誤差として抽出する低域成分抽出手段と、
前記角速度姿勢角から前記誤差を除去する誤差除去手段と、
前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記運動体の姿勢角として出力する出力手段と、
前記誤差除去手段で前記誤差が除去された角速度姿勢角を前記角速度姿勢角演算手段にフィードバックするフィードバック手段と、
を有する運動体の姿勢角検出装置。
運動体の姿勢角検出装置であって、
運動体の角速度を検出する角速度センサと、
前記運動体の加速度を検出する加速度センサと、
前記角速度から微小時間ｔｓ経過後の微小回転角を規定する微小角行列を演算する手段と、
前記微小角行列とフィードバックされた姿勢行列とから新たな角速度系姿勢行列を演算する手段と、
前記加速度から傾斜角を演算する手段と、
前記傾斜角から加速度系姿勢行列を演算する手段と、
前記角速度系姿勢行列の低域成分を抽出する角速度系低域成分抽出手段と、
前記加速度系姿勢行列の低域成分を抽出する加速度系低域成分抽出手段と、
前記角速度系姿勢行列の低域成分と前記加速度系姿勢行列の低域成分の差分を誤差として演算する差分演算手段と、
前記角速度系姿勢行列から前記誤差を除去する誤差除去手段と、
前記誤差除去手段で前記誤差が除去された角速度系姿勢行列から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、
前記誤差除去手段で前記誤差が除去された角速度系姿勢行列を前記新たな角速度系姿勢行列を演算する手段にフィードバックするフィードバック手段と、
を有する運動体の姿勢角検出装置。
請求項１１記載の装置において、
前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した行列を加算して得られた行列を前記角速度系姿勢行列から減算することで前記誤差を除去する運動体の姿勢角検出装置。
運動体の姿勢角検出装置であって、
運動体の角速度を検出する角速度センサと、
前記運動体の加速度を検出する加速度センサと、
前記角速度から微小時間ｔｓ経過後の微小回転角を規定する微小角４元数を演算する手段と、
前記微小角４元数とフィードバックされた４元数とから新たな角速度系４元数を演算する手段と、
前記加速度から傾斜角を演算する手段と、
前記傾斜角から加速度系４元数を演算する手段と、
前記角速度系４元数の低域成分を抽出する角速度系低域成分抽出手段と、
前記加速度系４元数の低域成分を抽出する加速度系低域成分抽出手段と、
前記角速度系姿勢行列の低域成分と前記加速度系姿勢行列の低域成分の差分を誤差として演算する差分演算手段と、
前記角速度系４元数から前記誤差を除去する誤差除去手段と、
前記誤差除去手段で前記誤差が除去された角速度系４元数から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、
前記誤差除去手段で前記誤差が除去された角速度系４元数を前記新たな角速度系４元数を演算する手段にフィードバックするフィードバック手段と、
を有する運動体の姿勢角検出装置。
請求項１３記載の装置において、
前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した値を加算して得られた４元数を前記角速度系４元数から減算することで前記誤差を除去する運動体の姿勢角検出装置。
請求項１１〜１４のいずれかに記載の装置において、さらに、
前記加速度センサとは別個に前記運動体の鉛直軸回りの傾斜角を検出して前記加速度系姿勢行列を演算する手段に出力する手段
を有する運動体の姿勢角検出装置。
請求項１１〜１５のいずれかに記載の装置において、
前記角速度系低域成分抽出手段は、角速度系ローパスフィルタを有し、
前記加速度系低域成分抽出手段は、加速度系ローパスフィルタを有し、
前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定される運動体の姿勢角検出装置。
請求項１６記載の装置において、さらに、
前記角速度系ローパスフィルタの出力を正規化する手段と、
前記加速度系ローパスフィルタの出力を正規化する手段
を有する運動体の姿勢角検出装置。
請求項１６、１７のいずれかに記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、前記運動周波数に応じて前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタのカットオフ周波数が可変設定される運動体の姿勢角検出装置。
請求項１２、１４のいずれかに記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、前記運動周波数に応じて前記倍率器、積分器及び差分器の係数が可変設定される運動体の姿勢角検出装置。
請求項１記載の装置において、
前記角速度姿勢角低域成分抽出手段及び前記加速度姿勢角低域成分抽出手段は、
前記角速度姿勢角をｓｉｎ変換及びｃｏｓ変換する手段と、
前記ｓｉｎ変換及びｃｏｓ変換して得られた値の低域成分を抽出する手段と、
低域成分が抽出された値をａｔａｎ２演算、あるいはａｔａｎ演算する手段と、
を有する運動体の姿勢角検出装置。
請求項２０記載の装置において、
前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した値を加算して得られた値を前記角速度姿勢角から減算することで前記誤差を除去する運動体の姿勢角検出装置。
請求項２０、２１のいずれかに記載の装置において、さらに、
前記加速度センサとは別個に前記運動体の鉛直軸回りの傾斜角を検出して前記加速度姿勢角演算手段に出力する手段を有する運動体の姿勢角検出装置。
請求項２０〜２２のいずれかに記載の装置において、
前記角速度姿勢角演算手段は、
前記角速度から微小時間ｔｓ経過後の微小回転角を規定する微小角行列を演算する手段と、
前記微小角行列と、前記フィードバック手段からフィードバックされた角速度姿勢角に対応する姿勢行列とから新たな角速度系姿勢行列を演算する手段と、
前記新たな角速度系姿勢行列から前記角速度姿勢角を演算する手段と、
を有し、
前記加速度姿勢角演算手段は、
前記加速度から傾斜角を演算する手段と、
前記傾斜角から加速度系姿勢行列を演算する手段と、
前記加速度系姿勢行列から前記加速度姿勢角を演算する手段と、
を有する運動体の姿勢角検出装置。
請求項２０〜２３のいずれかに記載の装置において、
前記角速度姿勢角低域成分抽出手段は、前記ｓｉｎ変換及びｃｏｓ変換された値の低域成分を抽出する角速度系ローパスフィルタを有し、
前記加速度姿勢角低域成分抽出手段は、前記ｓｉｎ変換及びｃｏｓ変換された値の低域成分を抽出する加速度系ローパスフィルタを有し、
前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定される運動体の姿勢角検出装置。
請求項２０〜２４のいずれかに記載の装置において、
前記差分演算手段は、
前記角速度姿勢角の低域成分をｓｉｎ変換及びｃｏｓ変換してａｔａｎ２演算、あるいはａｔａｎ演算した値と、前記加速度姿勢角の低域成分をｓｉｎ変換及びｃｏｓ変換してａｔａｎ２演算、あるいはａｔａｎ演算した値の差分を演算する手段を有する運動体の姿勢角検出装置。
請求項２５記載の装置において、さらに、
差分して得られた値をｓｉｎ変換及びｃｏｓ変換してａｔａｎ２演算、あるいはａｔａｎ演算する手段
を有する運動体の姿勢角検出装置。
請求項２０〜２６のいずれかに記載の装置において、
前記角速度姿勢角及び加速度姿勢角のψあるいはφは−πからπまでの間の角度として規定され、
前記誤差除去手段は、前記誤差が−πより小さい場合には−πに置換し、πより大きい場合にはπに置換して前記角速度姿勢角から除去し、
前記角速度姿勢角及び加速度姿勢角のθは−π／２からπ／２までの間の角度として規定され、
前記誤差除去手段は、前記誤差が−π／２より小さい場合には−π／２＋δ（但しδは実数）に置換し、π／２より大きい場合にはπ／２−δに置換して前記角速度姿勢角から除去する運動体の姿勢角検出装置。
請求項２４記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、前記運動周波数に応じて前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタのカットオフ周波数が可変設定される運動体の姿勢角検出装置。
請求項２１記載の装置において、さらに、
前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段
を有し、前記運動周波数に応じて前記倍率器、積分器器及び差分器の係数が可変設定される運動体の姿勢角検出装置。