"運動体の姿勢角検出装置であって、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から微小時間ts経過後の微小回転角を規定する微小角4元数を演算する手段と、前記微小角4元数とフィードバックされた4元数とから新たな角速度系4元数を演算する手段と、前記加速度から傾斜角を演算する手段と、前記傾斜角から加速度系4元数を演算する手段と、前記角速度系4元数の低域成分を抽出する角速度系低域成分抽出手段と、前記加速度系4元数の低域成分を抽出する加速度系低域成分抽出手段と、前記角速度系4元数の低域成分と前記加速度系4元数の低域成分の差分を誤差として演算する差分演算手段と、前記角速度系4元数から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度系4元数から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度系4元数を前記新たな角速度系4元数を演算する手段にフィードバックするフィードバック手段と、を有し、前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した値を加算して得られた4元数を前記角速度系4元数から減算することで前記誤差を除去し、さらに、前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段を有し、前記運動周波数に応じて前記倍率器、積分器及び差分器の係数が可変設定される運動体の姿勢角検出装置。請求項1記載の装置において、前記角速度4元数低域成分抽出手段は、前記新たな角速度系4元数を演算する手段からの前記新たな角速度系4元数の低域成分を抽出する角速度系ローパスフィルタを有し、前記加速度系4元数低域成分抽出手段は、前記加速度系4元数を演算する手段からの前記加速度系4元数の低域成分を抽出する加速度系ローパスフィルタを有し、前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定されることを特徴とする運動体の姿勢角検出装置。本発明の目的は、高価で高精度な角速度センサを用いることなく、簡易な構成で積分誤差の蓄積を防止し運動体の姿勢角を検出することができる装置を提供することにある。本発明は、運動体の姿勢角検出装置であって、運動体の角速度を検出する角速度センサと、前記運動体の加速度を検出する加速度センサと、前記角速度から微小時間ts経過後の微小回転角を規定する微小角4元数を演算する手段と、前記微小角4元数とフィードバックされた4元数とから新たな角速度系4元数を演算する手段と、前記加速度から傾斜角を演算する手段と、前記傾斜角から加速度系4元数を演算する手段と、前記角速度系4元数の低域成分を抽出する角速度系低域成分抽出手段と、前記加速度系4元数の低域成分を抽出する加速度系低域成分抽出手段と、前記角速度系4元数の低域成分と前記加速度系4元数の低域成分の差分を誤差として演算する差分演算手段と、前記角速度系4元数から前記誤差を除去する誤差除去手段と、前記誤差除去手段で前記誤差が除去された角速度系4元数から姿勢角を演算し前記運動体の姿勢角として出力する出力手段と、前記誤差除去手段で前記誤差が除去された角速度系4元数を前記新たな角速度系4元数を演算する手段にフィードバックするフィードバック手段とを有し、前記誤差除去手段は、倍率器、積分器、及び差分器を有し、前記差分演算手段からの前記誤差を前記倍率器で比例演算し、前記積分器で積分演算し、前記差分器で差分演算した値を加算して得られた4元数を前記角速度系4元数から減算することで前記誤差を除去し、さらに、前記角速度及び加速度から前記運動体特有の運動周波数を検出する手段を有し、前記運動周波数に応じて前記倍率器、積分器及び差分器の係数が可変設定されることを特徴とする。本発明の1つの実施形態では、前記角速度4元数低域成分抽出手段は、前記新たな角速度系4元数を演算する手段からの前記新たな角速度系4元数の低域成分を抽出する角速度系ローパスフィルタを有し、前記加速度系4元数低域成分抽出手段は、前記加速度系4元数を演算する手段からの前記加速度系4元数の低域成分を抽出する加速度系ローパスフィルタを有し、前記角速度系ローパスフィルタ及び前記加速度系ローパスフィルタは、カットオフ周波数及び減衰率が同一に設定される。本発明によれば、高価で大型、重量のあるセンサを用いることなく積分誤差の蓄積を防止して運動体の姿勢角を高精度に検出することができる。以下、図面に基づき実施形態について、運動体としてロボット、姿勢角検出装置としてセンサユニットを例にとり説明する。まず、第1実施形態について説明する。図1に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。ロボットの所定位置に角速度センサ10及び加速度センサ20を有するセンサユニット1が設けられる。角速度センサ10は、センサ座標系センサユニットの座標系における角速度を検出する。各軸回りの角速度をωx、ωy、ωzとする。角速度センサ10は、検出した角速度を微小角行列演算器12に出力する。微小角行列演算器12は、基準座標系XYZから見たセンサ座標系xyzを算出するための座標変換行列を演算する。微小角行列については後述する。微小角行列演算器12は、演算した微小角行列を行列加算演算器14に出力する。行列加算演算器14は、微小角行列演算器12から入力した、角速度センサ10で検出された現在の微小角行列と、出力側からフィードバックされた姿勢角行列とを加算することで積分演算する。行列加算演算器14は、積分した行列を行列姿勢角演算器16に出力する。行列姿勢角演算器16は、入力した積分行列から姿勢角を演算し、角速度姿勢角SYAとしてローパスフィルタLPF18に出力するとともに、減算器32にも出力する。ローパスフィルタ角速度系ローパスフィルタ18は、角速度姿勢角SYAから低域成分を抽出して差分器30に出力する。角速度姿勢角SYAには真の姿勢角姿勢角真値と積分誤差によるドリフト分が含まれており、ローパスフィルタ18は、角速度姿勢角SYAから高域の姿勢角真値を除去し、低域の姿勢角真値とドリフト分を抽出する。一方、加速度センサ20は、センサ座標系xyzの各軸方向の加速度を検出する。検出された加速度をGx、Gy、Gzとする。加速度センサ20は、検出された加速度を傾斜角演算器22に出力する。傾斜角演算器22は、センサ座標系xyzの各軸と基準座標系XYZのZ軸との間の角度を演算する。演算された傾斜角をλx、λy、λzとする。傾斜角演算器22は、演算した傾斜角を加速度行列演算器24に出力する。加速度行列演算器24は、入力した傾斜角を用いて基準座標系から見たセンサ座標系に対する姿勢行列を演算し、行列姿勢角演算器26に出力する。行列姿勢角演算器26は、行列姿勢角演算器16と同様に、入力した行列から姿勢角を演算し、加速度姿勢角SGAとしてローパスフィルタLPF28に出力する。ローパスフィルタ加速度系ローパスフィルタ28は、カットオフ周波数及び減衰率がローパスフィルタ18と同一に設定されたフィルタであり、加速度姿勢角SGAから低域成分を抽出して差分器30に出力する。加速度姿勢角SGAには高域成分に存在する高域の姿勢角偽値と低域成分に存在する低域の姿勢角真値が含まれている。加速度姿勢角には積分誤差によるドリフトは生じない。ローパスフィルタ28は、カットオフ周波数及び減衰率がローパスフィルタ18と同一に設定されたフィルタであるので、低域の姿勢角真値が周波数特性も含めて角速度系と加速度系と同一となり、減算器32による演算で正確に両者が減算により除去され、ドリフト成分のみが出力される。但し、角速度系と加速度系からもたらされる姿勢角相当成分の応答性が予め分かっている場合は、それぞれの特性に合わせてローパスフィルタのカットオフ周波数と減衰率を調整することができる。これは、適応範囲が狭いが、運動特性が予め分かっている場合には有効な方法となり得る。以上のように、差分器30にはローパスフィルタ18から低域の姿勢角真値ドリフト分、ローパスフィルタ28から低域の姿勢角真値が入力される。差分器30は両者の差分を演算し、低域の姿勢角真値ドリフト分-低域の姿勢角真値=ドリフト分を抽出する。差分器30は、差分演算して得られたドリフト分を減算器32に出力する。減算器32は、行列姿勢角演算器16から入力した角速度姿勢角SYAと差分器30から入力したドリフト分との差分を演算し、行列姿勢角演算器16からの角速度姿勢角SYAに含まれるドリフト分を除去する。上記のように、行列姿勢角演算器16からの角速度姿勢角SYAには姿勢角真値高域成分低域成分とドリフト分が含まれており、減算器32は減算により、低域姿勢角真値ドリフト分-ドリフト分=低域姿勢角真値を抽出する。減算器32は、減算して得られた姿勢角真値を出力装置34に出力する。また、減算器32は、姿勢角真値を姿勢角行列演算器36に出力する。姿勢角行列演算器36は、入力した姿勢角から姿勢角行列を演算し、行列加算演算器14にフィードバックする。姿勢角行列演算器36は、行列姿勢角演算器16の逆演算を実行して姿勢角から姿勢角行列を生成する。行列加算演算器14は、フィードバックされた姿勢角行列と、新たに角速度センサ10で検出された角速度に基づいて演算された微小角行列とを加算することで積分演算を実行し、角速度から姿勢角を演算する。出力装置34は、減算器32からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサあるいはホストプロセッサがセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置34は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ10で検出された角速度や加速度センサで検出された加速度を出力装置34に供給してもよく、出力装置34は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置34は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。図中に示さないが、姿勢角行列演算器36で得られた姿勢行列を出力装置34を介してロボットへ出力することも可能である。ロボット内部で行列演算する場合に有効な手法である。以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ20で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢角から除去し、フィードバックしているので、角速度センサ10としては高価で大型、高精度なセンサを用いる必要はない。以下、微小角行列演算器12、行列姿勢角演算器16、傾斜角演算器22及び行列姿勢角演算器26における演算について説明する。まず、姿勢行列について説明する。基準座標系XYZにおけるセンサ座標系の表記法として、離散時間nにおける姿勢行列Tnで表す。姿勢行列Tnは、1式に示すように4×4の要素から構成される。行列Tnの意味として、第一列a,b,c、第二列d,e,f、第三列g,h,iは、それぞれ基準座標系からみたセンサ座標系nのx軸、y軸、z軸の方向ベクトルを表す。第四列は、基準座標系におけるセンサ座標系nの原点位置を表す一般に並進が有る場合はこの第四列に並進量が表される。原点の移動がない場合は、位置の変換を表す第4列の1~3行目の要素を0とおく。図7に示すように、センサ座標系nの原点Oが、基準座標系において0,0,0の位置にあり、x軸ベクトルは基準座標系上のa,b,c、y軸ベクトルはd,e,f、z軸ベクトルはg,h,iの成分を持つ。姿勢角ロールピッチヨー角から姿勢行列Tnを求める手法を以下に説明する。減算器32からの姿勢角に基づき姿勢角行列演算器36で姿勢行列を演算する方法である。姿勢行列Tnを表すために行列による回転変換は、回転軸についての順序を考慮する必要がある。図8に示すように、ロボットで一般的に用いられるロールピッチヨー角を用いる場合は、最初にz軸周りの回転φ、次に回転後のy軸周りの回転θ、最後に回転後のx軸周りの回転Ψの3回の回転が生じたと定義する軸の回転順番が規定されている点に注意。ロールピッチヨー角による変換行列をRPYφ、θ、Ψとする。RPYφ、θ、Ψは回転変換行列を左から右に掛けた行列の積となり、式2で表される。式2は具体的に式3で表される。式3を書き下すと、式4で表記できる。なお、ロールピッチヨー角の代わりに、オイラー角を姿勢角として用いることもできる。オイラー角では、最初にz軸周りの回転φ、次に回転後のy軸周りの回転θ、最後に回転後のz軸周りの回転Ψが起こった時の変換行列をEulerEφ、Eθ、EΨとおき、式5で表される。式5は具体的に式6で表される。式6を書き下すと、式7で表記できる。基準座標系をO-XYZ、初期のセンサ座標系をO0-x0y0z0とおく。基準座標系と時刻t=0時の座標系O0-x0y0z0の関係を、座標変換A0で関係づける。時刻t=tn時の座標系をOn-xnynznとおく。各座標系の原点O,O0,Onは位置の移動がなく同一とする。その後運動体の姿勢変化により、図9に示すように、座標系O-xyzからOn-xnynznへ変化したとき、O-xyzとOn-xnynznはセンサ出力から求まる行列Anで関係づけられる。基準座標系からみたセンサ座標系Tnは、変換Anを右からかけていくことで式8で求める。センサ座標の原点が時間と共に移動する場合は、行列Aの第4列の1~3行目の要素に時間と共に移動した座標が逐次入れられる。行列Aの第4列はセンサ座標系の回転に対しては影響を与えないので、ここでは特に述べない。次に、センサ出力から微小回転行列An行列を導出する方法について説明する。角速度センサ10からの角速度に基づき微小角行列演算器12で微小角行列を演算する方法である。3個の角速度センサは、センサ座標系の各軸に設置されており、図10に示すように、センサx,y,z軸まわりの角速度を計測している。式4において、回転角Δφ,Δθ,ΔΨが十分小さいとき、である。これよりセンサx軸回りの微小回転角ΔΨ、センサy軸回りの微小回転角Δθ、センサz軸回りの微小回転角Δφを用いて式11で表すことができる。式11の結果は行列の各要素が独立に各微小回転角で表せるため、回転の順番に依存しないと近似している。微小角とセンサ出力との間には、微小回転角ΔΨ、Δθ、Δφ、角速度センサ出力ωx,ωy,ωz,サンプリング周期tsから、式12~13の関係がある。サンプリング周期tsが回転運動に対して十分早い周期としているため、サンプリング周期tsの時間内での回転は十分小さく、微小回転角とみなすことができる。このためAn行列は式15で表される。次に、姿勢行列から姿勢角を求める手法について述べる。行列加算演算器14からの姿勢行列積分後の姿勢行列に基づき行列姿勢角演算器16で姿勢角を演算する方法である。また、加速度行列演算器24からの姿勢行列に基づき行列姿勢角演算器26で姿勢角を演算する方法でもある。姿勢行列Tnが式16で表されている。ヨー角φは、姿勢角φの変域は、-π<f≦πである。ピッチ角θは、姿勢角θの変域は、-π/2<θ<π/2である。ロール角Ψは、姿勢角Ψの変域は、-π<Ψ≦πである。オイラー角を用いる場合は、式21~23を使う。Eφは、Eθは、EΨは、次に、行列の正規化について説明する。姿勢行列Tnでは、演算後、姿勢行列の各列が単位ベクトルとならないことがあるため式24の各列ベクトルの大きさが1になるように式25で正規化を行う。後述する図5における正規化直交化器54、56の演算である。ここでp1,p2は式26、27で与えられる。その後、正規化後の各要素をあらためて、と置き直す。さらに、行列の直交化について説明する。姿勢行列Tnでは演算後、姿勢行列の各列が直交した軸とならないことがあるため、式29の各列ベクトルが直交する直交化処理を行うこの場合、z軸を基準としている。z軸、y軸に直交する新しいx’軸を得るため、a’,b’,c’を求める。次に、z軸、x’軸に直交する新しいy’軸を得るために、d’,e’,f’を求める。求めたa’~f’から、直交化した姿勢行列Tnを得る。ここで、atan2について説明する。atan2y,xは、2変数x,yを持つ計算機用関数である。通常使われるatan関数よりも適用範囲が広い。-π<ξ≦πは、x>0、y>0の時x>0、y<0の時となる。同様にして、x<0,y>0の時ξ=πtan1y/xx<0,y<0の時ξ=-πtan1y/xx=0,y>0の時ξ=π/2x=0,y<0の時ξ=-π/2x=0,y=0の時ξ=0となる。次に、傾斜角の演算について説明する。加速度センサ20からの加速度に基づき、傾斜角演算器22で傾斜角を演算する方法である。傾斜角とは、センサx、y、z軸と基準Z軸との間の角度λx、λy、λzである。すなわち、λx:x軸とZ軸の間の角度λy:y軸とZ軸の間の角度λz:z軸とZ軸の間の角度であり、λx、λy、λzの範囲は、0≦λx、λy、λz≦πである。図11に、傾斜角と重力ベクトルを示す。センサ座標に配置された加速度センサから以下のように傾斜角を求める。加速度Gx、Gy、Gzを式40~42用いて正規化し、正規化後の加速度Gx’、Gy’、Gz’を求める。加速度Gx’、Gy’、Gz’から、式43~45を用いて傾斜角λx、λy、λzを求める。次に、傾斜角λx、λy、λzから姿勢行列Tnを求める手法について述べる。傾斜角演算器22からの傾斜角に基づき、加速度行列演算器24で姿勢行列を求める演算である。以上の結果から、姿勢行列Tnを求める。次に、姿勢行列Tnから傾斜角λx、λy、λzを求める手法について述べる。次に第2実施形態について説明する。図2に、本実施形態の構成ブロック図を示す。図1と異なる点は、差分器30はローパスフィルタ18の出力とローパスフィルタ28の出力との差分を演算して倍率器38、積算器40及び差分器42に出力する点である。倍率器38、積算器40、差分器42は、PIDProportionalIntegralDifferential制御を実行する。すなわち、倍率器38は、差分器30からのドリフト分に予め設定された係数を乗じる演算を行い、その値比例項を減算器32に出力する。積算器40は、差分器30からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値積分項を減算器32に出力する。差分器42は、差分器30からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値微分項を減算器32に出力する。各係数は固定でもよいが、後述するように可変でもよい。減算器32は、行列姿勢角演算器16から入力した角速度姿勢角からこれらの項の和比例項積分項微分項を減算し、ドリフト分を除去した運動体の姿勢角として出力装置34及び姿勢角行列演算器36に出力する。減算器32の出力は姿勢角行列演算器36で姿勢行列に変換されてフィードバックされるが、倍率器38がフィードバックの程度を主に決定し、積算器40が長時間ずれ量を蓄積してフィードバック量を決定し、差分器42が短時間変化に対するフィードバック量を決定する。このようなPID制御を用いることにより、短時間特性と長時間特性をともに向上できる。次に第3実施形態について説明する。図3に、本実施形態の構成ブロック図を示す。図2と異なる点は、外部姿勢角44からのZ軸回りの姿勢角、すなわちヨー角を行列姿勢角演算器26に供給する点である。加速度センサ20はx、y、z各軸方向の加速度を検出するが、Gセンサを用いるとZ軸回りの姿勢角であるヨー角φは不明であり、したがってヨー角φは一定値に固定する必要があるが、本実施形態ではヨー角を別途検出して行列姿勢角演算器26に供給することで、ヨー角φも正確に算出できる。具体的には、行列姿勢角演算器26は、上記の演算により姿勢行列Tnから姿勢角としてヨー角φ、ピッチ角θ、ロール角Ψを演算するが、このうちのヨー角φを外部から入力したヨー角φに置換する。これにより、ヨー角φの発散も防止できる。次に第4実施形態について説明する。図4に、本実施形態の構成ブロック図を示す。上記の各実施形態では、行列姿勢角演算器16からの角速度姿勢角をローパスフィルタ18に出力するとともに、行列姿勢角演算器26からの加速度姿勢角をローパスフィルタ28に出力しているが、本実施形態では、行列加算演算器14からの姿勢行列をローパスフィルタALPF46に出力するとともに、加速度行列演算器24からの姿勢行列をローパスフィルタALPF48に出力し、姿勢角段階で低域成分を抽出するのではなく、行列段階で低域成分を抽出する。ローパスフィルタ46、48は、4×4の姿勢行列の各行列成分に適用され、それぞれの低域成分を抽出する。ローパスフィルタ46、48のカットオフ周波数及び減衰率は同一に設定される。ローパスフィルタ角速度系46は、姿勢行列から低域成分を抽出した後、姿勢行列を行列姿勢角演算器50に出力する。行列姿勢角演算器50は、行列姿勢角演算器16と同様に姿勢行列から角速度姿勢角を演算し、差分器30に出力する。ローパスフィルタ46、48は、4×4の姿勢行列の内、回転を表す成分の第一列a,b,c、第二列d,e,f、第三列g,h,i、すなわち3×3の各行列成分に適用され、それぞれの低域成分を抽出することに簡略化することもできる。また、さらに3×3の各行列成分の内の対角成分3個と非対角成分の片側3個の計6個の成分にローパスフィルタを作用させて等価的に行列フィルタを形成させることも可能である。また、ローパスフィルタ加速度系48は、姿勢行列から低域成分を抽出した後、姿勢行列を行列姿勢角演算器26に出力する。図3と同様に、外部の外部姿勢角44からのZ軸回りの姿勢角、すなわちヨー角を加速度系に供給してもよい。但し、図3では外部センサで検出したヨー角φを行列姿勢角演算器26に供給するが、本実施形態では加速度行列演算器24に供給する。これはもちろん、ローパスフィルタ48で姿勢行列の低域成分を抽出するためであり、外部センサで検出されたヨー角φも姿勢行列の成分として含める必要があるからである。次に第5実施形態について説明する。図5に、本実施形態の構成ブロック図を示す。図4と異なる点は、ローパスフィルタ46、48の後段に正規化直交化器54、56を付加した点である。ローパスフィルタ46、48で姿勢行列から低域成分を抽出すると、姿勢行列に歪みが生じ、正規性及び直交性が担保されなくなる。そこで、正規化直交化器54、56で正規化、直交化することで歪みを修正する。正規化及び直交化の方法については上記のとおりである。次に第6実施形態について説明する。図6に、本実施形態の構成ブロック図を示す。図4と異なる点は、角速度センサ10からの角速度、及び加速度センサ20からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器52を設ける点である。運動周波数検知器52は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ46、48に出力する。運動体としてのロボットが2足歩行する場合の、2足歩行に特有の周波数等が一例である。ローパスフィルタ46、48は、運動周波数検知器52から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ46、48はカットオフ周波数を上げるように調整する。また、運動周波数検知器52は、PID制御を実行する倍率器38、積算器40、差分器42それぞれに運動周波数を出力する。倍率器38、積算器40、差分器42は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やPID制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。次に第7実施形態について説明する。図12に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。角速度センサ10は、センサ座標系センサユニットの座標系における角速度を検出する。各軸回りの角速度をωx、ωy、ωzとする。角速度センサ10は、検出した角速度を微小角行列演算器12に出力する。微小角行列演算器12は、基準座標系XYZから見たセンサ座標系xyzを算出するための座標変換行列を演算する。微小角行列については後述する。微小角行列演算器12は、演算した微小角行列を行列加算演算器14に出力する。行列加算演算器14は、微小角行列演算器12から入力した、角速度センサ10で検出された現在の微小角行列と、出力側からフィードバックされた姿勢行列とを加算することで積分演算する。行列加算演算器14は、積分した行列角速度系姿勢行列をローパスフィルタALPF46に出力する。また、行列加算演算器14は、積分した行列を減算器32にも出力する。ローパスフィルタ角速度系ローパスフィルタ46は、角速度系姿勢行列の各成分から低域成分を抽出して差分器30に出力する。角速度系姿勢行列には真の姿勢姿勢真値と積分誤差によるドリフト分が含まれており、ローパスフィルタ46は、角速度系姿勢行列から高域の姿勢真値を除去し、低域の姿勢真値とドリフト分を抽出する。一方、加速度センサ20は、センサ座標系xyzの各軸方向の加速度を検出する。検出された加速度をGx、Gy、Gzとする。加速度センサ20は、検出された加速度を傾斜角演算器22に出力する。傾斜角演算器22は、センサ座標系xyzの各軸と基準座標系XYZのZ軸との間の角度を演算する。演算された傾斜角をλx、λy、λzとする。傾斜角演算器22は、演算した傾斜角を加速度行列演算器24に出力する。加速度行列演算器24は、入力した傾斜角を用いて基準座標系から見たセンサ座標系に対する姿勢行列を演算し、加速度系姿勢行列としてローパスフィルタALPF48に出力する。ローパスフィルタ加速度系ローパスフィルタ48は、カットオフ周波数及び減衰率がローパスフィルタ46と同一に設定されたフィルタであり、加速度系姿勢行列から低域成分を抽出して差分器30に出力する。加速度系姿勢行列には高域に生じるノイズと高域の姿勢真値と低域の姿勢真値が含まれている。加速度系姿勢行列には積分誤差によるドリフトは生じない。ローパスフィルタ48は、カットオフ周波数及び減衰率がローパスフィルタ46と同一に設定されたフィルタであるので、低域の姿勢真値が周波数特性も含めて角速度系と加速度系と同一となり、減算器による減算で正確に両者が減算により除去され、ドリフト成分のみが出力される。但し、角速度系と加速度系からもたらされる姿勢相当成分の応答性が予め分かっている場合は、それぞれの特性に合わせてローパスフィルタのカットオフ周波数と減衰率を調整することができる。これは、適応範囲は狭いが、運動特性が予め分かっている場合には有効な方法となり得る。以上のように、差分器30にはローパスフィルタ46から低域の姿勢真値ドリフト分、ローパスフィルタ48から低域の姿勢真値が入力される。差分器30は両者の差分を演算し、低域の姿勢真値ドリフト分-低域の姿勢真値=ドリフト分を抽出する。差分器30は、差分演算して得られたドリフト分を減算器32に出力する。減算器32は、行列加算演算器14から入力した角速度系姿勢と差分器30から入力したドリフト分との差分行列の差分を演算し、行列加算演算器14からの角速度系姿勢行列に含まれるドリフト分を除去する。上記のように、行列加算演算器14からの角速度系姿勢行列には姿勢真値とドリフト分が含まれており、減算器32は減算により、姿勢真値ドリフト分-ドリフト分=姿勢真値を抽出する。減算器32は、減算して得られた姿勢真値の角速度系姿勢行列を行列姿勢角演算器37に出力する。行列姿勢角演算器37は、姿勢行列から姿勢角を演算し、運動体の姿勢角として出力装置34に出力する。また、減算器32は、ドリフト分が除去された姿勢行列を行列加算演算器14にフィードバックする。行列加算演算器14は、フィードバックされた姿勢行列と、新たに角速度センサ10で検出された角速度に基づいて演算された微小角行列とを加算することで積分演算を実行し、角速度から新たな角速度系姿勢行列を演算する。出力装置34は、行列姿勢角演算器37からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサあるいはホストプロセッサがセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置34は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ10で検出された角速度や加速度センサで検出された加速度を出力装置34に供給してもよく、出力装置34は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置34は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ20で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢から除去しフィードバックしているので、角速度センサ10としては高価で大型、高精度なセンサを用いる必要はない。次に第8実施形態について説明する。図13に、本実施形態の構成ブロック図を示す。図12と異なる点は、差分器30はローパスフィルタ46の出力とローパスフィルタ48の出力との差分を演算して倍率器38、積算器40及び差分器42に出力する点である。倍率器38、積算器40、差分器42は、PID制御を実行する。すなわち、倍率器38は、差分器30からのドリフト分に予め設定された係数を乗じる演算を行い、その値比例項を減算器32に出力する。積算器40は、差分器30からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値積分項を減算器32に出力する。差分器42は、差分器30からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値微分項を減算器32に出力する。乗算演算、積分演算及び差分演算は、ドリフト分の行列の各成分毎に実行する。各係数は固定でもよいが、後述するように可変でもよい。減算器32は、行列加算演算器14から入力した角速度系姿勢行列からこれらの項の和比例項積分項微分項を減算し、ドリフト分を除去した角速度系姿勢行列を算出して行列姿勢角演算器37に出力する。減算器32の出力は行列加算演算器14にフィードバックされるが、倍率器38がフィードバックの程度を主に決定し、積算器40が長時間ずれ量を蓄積してフィードバック量を決定し、差分器42が短時間変化に対するフィードバック量を決定する。このようなPID制御を用いることにより、短時間特性と長時間特性をともに向上できる。また、図13では、外部姿勢角44からのZ軸回りの姿勢角、すなわちヨー角φを加速度行列演算器24に供給している。加速度センサ20はx、y、z各軸方向の加速度を検出するが、Gセンサを用いるとZ軸回りの姿勢角であるヨー角φは不明であり、ヨー角φは一定値例えば0に固定する必要がある。本実施形態ではヨー角φを別途検出して加速度行列演算器24に供給しているため、ヨー角φの発散も防止できる。次に第9実施形態について説明する。図14に、本実施形態の構成ブロック図を示す。図13と異なる点は、ローパスフィルタ46、48の後段に正規化直交化器54、56を付加した点である。ローパスフィルタ46、48で姿勢行列から低域成分を抽出すると、姿勢行列に歪みが生じ、正規性及び直交性が担保されなくなる。そこで、正規化直交化器54、56で正規化、直交化することで歪みを修正する。正規化及び直交化の方法については上記のとおりである。ローパスフィルタ46、48は、4×4の姿勢行列の内、回転を表す成分の第一列a,b,c、第二列d,e,f、第三列g,h,i、すなわち3×3の各行列成分に適用され、それぞれの低域成分を抽出することで簡略化できる。また、さらに3×3の各行列成分の内の対角成分3個と非対角成分の片側3個の計6個の成分にローパスフィルタを作用させて等価的に行列フィルタを形成させることも可能である。次に第10実施形態について説明する。図15に、本実施形態の構成ブロック図を示す。図13と異なる点は、角速度センサ10からの角速度、及び加速度センサ20からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器52を設ける点である。運動周波数検知器52は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ46、48に出力する。運動体としてのロボットが2足歩行する場合の、2足歩行に特有の周波数等が一例である。ローパスフィルタ46、48は、運動周波数検知器52から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ46、48はカットオフ周波数を上げるように調整する。また、運動周波数検知器52は、PID制御を実行する倍率器38、積算器40、差分器42それぞれに運動周波数を出力する。倍率器38、積算器40、差分器42は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やPID制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。次に第11実施形態について説明する。図16に、本実施形態の構成ブロック図を示す。上記の各実施形態では、姿勢行列を用いているが、姿勢行列の代わりに4元数を用いる。角速度センサ10から得られる微小角を用いて微小角4元数を微小角4元数演算器13により生成する。この微小角4元数を用いフィードバックされた4元数に微小回転を作用させ、回転後の4元数を4元数積算演算器15により得る。この積算4元数は減算器32へ出力される。減算器32へ出力されると同じ出力を4元数用ローパスフィルタ63へ入力する。4元数用ローパスフィルタ63は姿勢の低域成分を抽出する。加速度センサ20からの出力は傾斜角演算器22で傾斜角を得た後、加速度4元数演算器25により加速度4元数を得る。この加速度4元数は4元数用ローパスフィルタ67へ入力する。4元数用ローパスフィルタ67は姿勢の低域成分を抽出する。差分器30は、角速度系4元数の低域成分と加速度系4元数の低域成分との差分を演算し、角速度系4元数に含まれるドリフト分を抽出する。差分器30は、ドリフト分を減算器32に出力する。減算器32は、4元数積算演算器15から入力した角速度系4元数と差分器30からのドリフト分の差分を演算し、角速度系4元数からドリフト分を除去する。減算器32は、ドリフト分が除去された角速度系4元数を4元数積算演算器15に出力するとともに4元数姿勢角演算器70に出力する。フィードバックする4元数はドリフト分が除去された4元数であるから、積分誤差の蓄積を防止できる。4元数姿勢角演算器70は、入力した4元数から姿勢角を演算し、出力装置34に出力する。以下、4元数を演算する方法、及び4元数から姿勢角を演算する方法について説明する。まず、4元数について概説する。4元数Quaternionは、2次元の自由度を有する複素数を4次元の自由度に拡張したものである。4元数は、一般に、q=abicjdkと表現される。ここで、a、b、c、dは実数、i、j、kは基底大きさは1である。各基底の積は以下の関係にある。il=li=ijl=lj=jkl=lk=kij=-ji=kjk=-kj=iki=-ik=jまた、2つの4元数q1=a1b1ic1jd1k、q2=a2b2ic2jd2kに対し、和及び差は、q1q2=a1a2b1b2ic1c2jd1d2kq1-q2=a1-a2b1-b2ic1-c2jd1-d2kとなり、積は、q1q2=a1b1ic1jd1ka2b2ic2jd2k=a1a2-b1b2-c1c2-d1d2a1b2b1a2c1d2-d1c2ia1c2-b1d2c1a2d1b2ja1d2b1c2-c1b2d1a2kとなる。虚数単位i、j、kを用いて4元数を以下のように表記する。P=a;xi,yj,zk基準座標系XYZから見たセンサ座標系xyzの関係、すなわち姿勢を4元数Qを用いて、Q=cosη/2;αsinη/2,βsinη/2,γsinη/2のように表す。これは、基準座標系を基準座標系で表された回転軸α、β、γ回りに角度η回転した座標系センサ座標系を示し、センサ座標系の姿勢を表している。次に、角速度センサ出力の4元数による表記について説明する。回転角とセンサ出力との間には、微小回転角ΔΨ、Δθ、Δφ、角速度センサ出力ωx、ωy、ωz、サンプリング周期tsから次の関係がある。ΔΨ=ωxtsΔθ=ωytsΔφ=ωztsこのときの回転変換を4元数で表すと以下のようになる。qx=cosΔΨ/2;sinΔΨ/2i,0j,0kqy=cosΔθ/2;0i,sinΔθ/2j,0kqz=cosΔφ/2;0i、0j,sinΔφ/2kx軸、y軸、z軸回りの回転変換をまとめて表記する。回転角αが十分小さいとき、以下のようになる。qxqyqz=1ΔΨΔθΔφ/8;ΔΨ/2-ΔθΔφ/4i,Δθ/2ΔΨΔφ/4j,Δφ/2-ΔΨΔθ/4k次に、4元数による回転変換の積算について説明する。回転変換q1をq1=cosη1/2;αsinη1/2,βsinη1/2,γsinη1/2とおき、回転変換q2をq2=cosη2/2;αsinη2/2,βsinη2/2,γsinη2/2とおく。まず基準座標系について回転変換q1を行い、次に回転後の座標系について回転変換q2を行ったとする。2つの回転変換による変換q3は、q3=q1q2=cosη1/2;αsinη1/2,βsinη1/2,γsinη1/2cosη2/2;αsinη2/2,βsinη2/2,γsinη2/2である。次に、4元数の正規化について説明する。回転を表す4元数では、演算後、変換qの大きさノルムが1となるように正規化を行う。回転を表す4元数がqで表されているとき、qの大きさを求め、次にqを大きさで割ることで正規化し、q’を得る。q=ε0;ε1,ε2,ε3|q|=ε02ε12ε22ε3205q’=q/|q|である。4元数は直交条件を維持した表記方法なので、さらなる直交化は必要ない。次に、4元数から姿勢角への変換について説明する。4元数で表された変換q=ε0;ε1,ε2,ε3において、a=ε02ε12-ε22-ε32b=2ε1ε2ε0ε3c=2ε1ε3-ε0ε2d=2ε1ε2-ε0ε3e=ε02-ε12ε22-ε32f=2ε2ε3ε0ε1g=2ε1ε3ε0ε2h=2ε2ε3-ε0ε1i=ε02-ε12-ε22ε32とおき、以下の式で姿勢角を求める。ヨー角φ=atan2baピッチ角θ=atan2-c,cosφasinφbロール角Ψ=atan2sinφg-cosφh,-sinφdcosφeである。次に、姿勢角ロール角、ピッチ角、ヨー角から4元数への変換について説明する。姿勢角Ψ,θ,φによるそれぞれの回転変換を4元数で表す。x軸回りの回転をRΨ;i=cosΨ/2isinΨ/2y軸回りの回転をRθ;j=cosθ/2jsinθ/2z軸回りの回転をRφ;k=cosφ/2ksinφ/2これらを掛け合わせると、S=Rφ;kRθ;jRΨ;i=cosφ/2ksinφ/2cosθ/2jsinθ/2cosΨ/2isinΨ/2このとき、4元数で表された変換qをq=ε0;ε1,ε2,ε3と表すと、回転角はSの計算結果における実部、回転軸はSの虚部となる。ε0=cosφ/2cosθ/2cosΨ/2sinφ/2sinθ/2sinΨ/2ε1=-sinφ/2sinθ/2cosΨ/2cosφ/2cosθ/2sinΨ/2ε2=sinφ/2cosθ/2sinΨ/2cosφ/2sinθ/2cosΨ/2ε3=sinφ/2cosθ/2cosΨ/2-cosφ/2sinθ/2sinΨ/2このとき、回転角αは、cosα/2=ε0から得られる。また、回転角はε1,ε2,ε3である。次に第12実施形態について説明する。図17に、本実施形態の構成ブロック図を示す。図16と異なる点は、PID制御を実行するための倍率器38、積算器40及び差分器42を設けた点と、外部姿勢角44で検出したヨー角を加速度4元数演算器25に供給した点である。図13に対応する構成である。倍率器38は、差分器30からのドリフト分4元数に予め設定された係数を乗じる演算を行い、その値比例項を減算器32に出力する。積算器40は、差分器30からのドリフト分4元数を時間積算した値に予め設定された係数を乗じる演算を行い、その値積分項を減算器32に出力する。差分器42は、差分器30からのドリフト分4元数の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値微分項を減算器32に出力する。各係数は固定でもよく可変でもよい。減算器32は、入力した角速度系4元数とこれらの項の和比例項積分項微分項の差分を演算し、ドリフト分を除去した角速度系4元数を算出して4元数姿勢角演算器70に出力する。次に第13実施形態について説明する。図18に、本実施形態の構成ブロック図を示す。図17と異なる点は、ローパスフィルタ63、67の後段に正規化器72、74を設けた点である。図14に対応する構成である。角速度系4元数及び加速度系4元数はそれぞれローパスフィルタ63、67で処理されるが、この処理に伴い歪みが生じ、非正規化され得る。そこで、正規化器72、74で歪みを補正することにより、姿勢角の検出精度を維持できる。次に第14実施形態について説明する。図19に、本実施形態の構成ブロック図を示す。図17と異なる点は、角速度センサ10からの角速度、及び加速度センサ20からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器52を設ける点である。図15に対応する構成である。運動周波数検知器52は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ63、67に出力する。ローパスフィルタ63、67は、運動周波数検知器52から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ63、67はカットオフ周波数を上げるように調整する。また、運動周波数検知器52は、PID制御を実行する倍率器38、積算器40、差分器42それぞれに運動周波数を出力する。倍率器38、積算器40、差分器42は、入力した運動周波数に応じて乗算係数を調整する。このように、運動に特有な周波数に応じて適応的にカットオフ周波数やPID制御の係数を調整することで、応答性、追従性、及び適応性を向上できる。次に第15実施形態について説明する。図20に、本実施形態に係る運動体の姿勢角検出装置の構成ブロック図を示す。図1と異なるのは、ローパスフィルタ18、28でそれぞれsin変換、cos変換、atan2変換を行うSCA変換ことである。角速度センサ10は、センサ座標系センサユニットの座標系における角速度を検出する。各軸回りの角速度をωx、ωy、ωzとする。角速度センサ10は、検出した角速度を微小角行列演算器12に出力する。微小角行列演算器12は、基準座標系XYZから見たセンサ座標系xyzを算出するための座標変換行列を演算する。微小角行列については後述する。微小角行列演算器12は、演算した微小角行列を行列加算演算器14に出力する。行列加算演算器14は、微小角行列演算器12から入力した、角速度センサ10で検出された現在の微小角行列と、出力側からフィードバックされた姿勢角行列とを加算することで積分演算する。行列加算演算器14は、積分した行列を行列姿勢角演算器16に出力する。行列姿勢角演算器16は、入力した積分行列から姿勢角を演算し、角速度姿勢角SYAとしてローパスフィルタLPF18に出力するとともに、減算器32にも出力する。ローパスフィルタ角速度系ローパスフィルタ18は、角速度姿勢角SYAから低域成分を抽出して差分器30に出力する。角速度姿勢角SYAには真の姿勢角姿勢角真値と積分誤差によるドリフト分が含まれており、ローパスフィルタ18は、角速度姿勢角SYAから高域の姿勢角真値を除去し、低域の姿勢角真値とドリフト分を抽出する。一方、加速度センサ20は、センサ座標系xyzの各軸方向の加速度を検出する。検出された加速度をGx、Gy、Gzとする。加速度センサ20は、検出された加速度を傾斜角演算器22に出力する。傾斜角演算器22は、センサ座標系xyzの各軸と基準座標系XYZのZ軸との間の角度を演算する。演算された傾斜角をλx、λy、λzとする。傾斜角演算器22は、演算した傾斜角を加速度行列演算器24に出力する。加速度行列演算器24は、入力した傾斜角を用いて基準座標系から見たセンサ座標系を演算し、行列姿勢角演算器26に出力する。行列姿勢角演算器26は、行列姿勢角演算器16と同様に、入力した行列から姿勢角を演算し、加速度姿勢角SGAとしてローパスフィルタLPF28に出力する。ローパスフィルタ加速度系ローパスフィルタ28は、カットオフ周波数及び減衰率がローパスフィルタ18と同一に設定されたフィルタであり、加速度姿勢角SGAから低域成分を抽出して差分器30に出力する。加速度姿勢角SGAには高域成分に存在する高域の姿勢角偽値と低域成分に存在する低域の姿勢角真値が含まれている。加速度姿勢角には積分誤差によるドリフトは生じない。ローパスフィルタ28は、カットオフ周波数及び減衰率がローパスフィルタ18と同一に設定されたフィルタであるので、低域の姿勢角真値が周波数特性も含めて角速度系と加速度系と同一となり、減算器32による演算で正確に両者が減算により除去され、ドリフト成分のみが出力される。但し、角速度系と加速度系からもたらされる姿勢角相当成分の応答性が予め分かっている場合は、それぞれの特性に合わせてローパスフィルタのカットオフ周波数と減衰率を調整することができる。これは、適応範囲が狭いが、運動特性が予め分かっている場合には有効な方法となり得る。以上のように、差分器30にはローパスフィルタ18から低域の姿勢角真値ドリフト分、ローパスフィルタ28から低域の姿勢角真値が入力される。差分器30は両者の差分を演算し、低域の姿勢角真値ドリフト分-低域の姿勢角真値=ドリフト分を抽出する。差分器30は、差分演算して得られたドリフト分を減算器32に出力する。減算器32は、行列姿勢角演算器16から入力した角速度姿勢角SYAと差分器30から入力したドリフト分との差分を演算し、行列姿勢角演算器16からの角速度姿勢角SYAに含まれるドリフト分を除去する。上記のように、行列姿勢角演算器16からの角速度姿勢角SYAには姿勢角真値高域成分低域成分とドリフト分が含まれており、減算器32は減算により、低域姿勢角真値ドリフト分-ドリフト分=低域姿勢角真値を抽出する。減算器32は、減算して得られた姿勢角真値を出力装置34に出力する。また、減算器32は、姿勢角真値を姿勢角行列演算器36に出力する。姿勢角行列演算器36は、入力した姿勢角から姿勢角行列を演算し、行列加算演算器14にフィードバックする。姿勢角行列演算器36は、行列姿勢角演算器16の逆演算を実行して姿勢角から姿勢角行列を生成する。行列加算演算器14は、フィードバックされた姿勢角行列と、新たに角速度センサ10で検出された角速度に基づいて演算された微小角行列とを加算することで積分演算を実行し、角速度から姿勢角を演算する。出力装置34は、減算器32からの、ドリフト分が除去された姿勢角を外部に出力する。ロボットの場合、メインプロセッサあるいはホストプロセッサがセンサユニットで検出された姿勢角等を入力してロボットの姿勢をフィードバック制御する。出力装置34は、メインプロセッサからのコマンドに従い、所定のタイミングで姿勢角をメインプロセッサに出力する。なお、角速度センサ10で検出された角速度や加速度センサで検出された加速度を出力装置34に供給してもよく、出力装置34は姿勢角に加え、角速度や加速度をメインプロセッサに出力してもよい。いずれを出力するかはメインプロセッサからのコマンドあるいは設定パラメータにより決定される。出力装置34は、あるタイミングでは角速度、加速度及び姿勢角の全てをメインプロセッサに出力し、別のタイミングではこれらのいずれかを選択的に出力する。以上のようにして、ドリフト分が除去された高精度な姿勢角が検出され出力される。加速度センサ20で検出される各軸方向の加速度に基づいてドリフト分を抽出し、このドリフト分を角速度姿勢角から除去しフィードバックしているので、角速度センサ10としては高価で大型、高精度なセンサを用いる必要はない。一方、行列姿勢角演算器16、26は、姿勢行列から姿勢角を算出する際に、関数atan2を用いて演算するため、本来の姿勢角は連続的に変化しているにもかかわらず、演算上は姿勢角が不連続的に変化してしまう事態が生じ得る。これが姿勢角表現における不連続性である。そこで、行列姿勢角演算器16、26からの姿勢角をそのまま用いてローパスフィルタで低域成分を抽出するのではなく、連続的な姿勢角に変換した後にその低域成分を抽出する。図21に、ローパスフィルタ18の構成ブロック図を示す。ローパスフィルタ18は、姿勢角をsin変換、cos変換する変換器及びatan2演算器、あるいはatan演算器を有する。ローパスフィルタ28も同様の構成であるためその説明は省略する。sincos変換器18a、18b、18cは、それぞれ3軸の姿勢角に対してsin変換及びcos変換を施して出力する。例えば、角速度姿勢角がそれぞれS1Ψ、S1θ、S1φであるとすると、sincos変換器18aは、S1Ψに対して、sinS1Ψ及びcosS1Ψを演算して出力する。sin変換及びcos変換された値は、それぞれローパスフィルタ18d、18eに出力される。ローパスフィルタ18d、18eは、sin変換及びcos変換された値からそれぞれ低域成分を抽出し、atan2演算器18jに出力する。atan2演算器18jは、sin変換値及びcos変換値を用いてatan2を演算し、姿勢角S2Ψとして出力する。また、sincos変換器18bは、S1θに対して、sinS1θ及びcosS1θを演算して出力する。sin変換及びcos変換された値は、それぞれローパスフィルタ18f、18gに出力される。ローパスフィルタ18f、18gは、sin変換及びcos変換された値からそれぞれ低域成分を抽出し、atan演算器18kに出力する。atan演算器18kは、sin変換値及びcos変換値を用いてatanを演算し、姿勢角S2θとして出力する。また、sincos変換器18cは、S1φに対して、sinS1φ及びcosS1φを演算して出力する。sin変換及びcos変換された値は、それぞれローパスフィルタ18h、18iに出力される。ローパスフィルタ18h、18iは、sin変換及びcos変換された値からそれぞれ低域成分を抽出し、atan2演算器18mに出力する。atan2演算器18mは、sin変換値及びcos変換値を用いてatan2を演算し、姿勢角S2φとして出力する。このように、sincos変換とatan2演算、あるいはatan演算を組み合わせることで、姿勢角の不連続性によるローパスフィルタの動作不良をなくし、安定して低域成分を抽出できる。sin変換後、cos変換後の値は±1の範囲内で連続で滑らかに変化するので、フィルタ処理において発散や異常な値を示すことがない。フィルタ処理後のsin変換値とフィルタ処理後のcos変換値を用いてatan2演算処理により、姿勢角成分に再変換するので、姿勢角表現範囲を超えるおそれがなく、姿勢角成分のフィルタ処理が実現される。次に第16実施形態について説明する。図22に、本実施形態の構成ブロック図を示す。図20と異なる点は、差分器30はローパスフィルタ18の出力とローパスフィルタ28の出力との差分を演算して倍率器38、積算器40及び差分器42に出力する点である。倍率器38、積算器40、差分器42は、PID制御を実行する。すなわち、倍率器38は、差分器30からのドリフト分に予め設定された係数を乗じる演算を行い、その値比例項を減算器32に出力する。積算器40は、差分器30からのドリフト分を時間積算した値に予め設定された係数を乗じる演算を行い、その値積分項を減算器32に出力する。差分器42は、差分器30からのドリフト分の時間差分を演算した値に予め設定された係数を乗じる演算を行い、その値微分項を減算器32に出力する。各係数は固定でもよいが、後述するように可変でもよい。減算器32は、行列姿勢角演算器16から入力した角速度姿勢角からこれらの項の和比例項積分項微分項を減算し、ドリフト分を除去した運動体の姿勢角として出力装置34及び姿勢角行列演算器36に出力する。減算器32の出力は姿勢角行列演算器36で姿勢行列に変換されてフィードバックされるが、倍率器38がフィードバックの程度を主に決定し、積算器40が長時間ずれ量を蓄積してフィードバック量を決定し、差分器42が短時間変化に対するフィードバック量を決定する。このようなPID制御を用いることにより、より、短時間特性と長時間特性をともに向上できる。次に第17実施形態について説明する。図23に、本実施形態の構成ブロック図を示す。図22と異なる点は、外部姿勢角44からのZ軸回りの姿勢角、すなわちヨー角を行列姿勢角演算器26に供給する点である。加速度センサ20はx、y、z各軸方向の加速度を検出するが、Gセンサを用いるとZ軸回りの姿勢角であるヨー角φは不明であり、したがってヨー角φは一定値に固定する必要があるが、本実施形態ではヨー角を別途検出して行列姿勢角演算器26に供給することで、ヨー角φも正確に算出できる。具体的には、行列姿勢角演算器26は、上記の演算により姿勢行列Tnから姿勢角としてヨー角φ、ピッチ角θ、ロール角Ψを演算するが、このうちのヨー角φを外部から入力したヨー角φに置換する。これにより、ヨー角φの発散も防止できる。また、本実施形態では、差分器30の代わりに差分器31を設けている。この差分器31は、ローパスフィルタ18,28と同様にsincos変換器及びatan2演算器、あるいはatan演算器を有する。図24に、差分器31の構成ブロック図を示す。ローパスフィルタ18からの角速度姿勢角及びローパスフィルタ28からの加速度姿勢角は、ともに差分器31に供給される。3軸それぞれの角速度姿勢角をξn0Ψ、ξn0θ、ξn0φとし、3軸それぞれの加速度姿勢角をξm0Ψ、ξm0θ、ξm0φとする。sincos変換器31aは、ξn0Ψに対してsin変換及びcos変換を行い、それぞれの値をatan2演算器31gに出力する。atan2演算器31gは、sin変換値及びcos変換値を用いてatan2演算を行い、差分器31nに出力する。一方、sincos変換器31bは、ξm0Ψに対してsin変換及びcos変換を行い、それぞれの値をatan2演算器31hに出力する。atan2演算器31hは、sin変換値及びcos変換値を用いてatan2演算を行い、差分器31nに出力する。差分器31nは両者の差分を演算し、ドリフト分を抽出する。差分器31nは、sincos変換器31rにドリフト分を出力する。sincos変換器31r及びatan2演算器31uは同様な演算を行い、ドリフト分としてξd1Ψを出力する。sincos変換器31cは、ξn0θに対してsin変換及びcos変換を行い、それぞれの値をatan演算器31iに出力する。atan演算器31iは、sin変換値及びcos変換値を用いてatan演算を行い、差分器31pに出力する。一方、sincos変換器31dは、ξm0θに対してsin変換及びcos変換を行い、それぞれの値をatan演算器31jに出力する。atan演算器31jは、sin変換値及びcos変換値を用いてatan演算を行い、差分器31pに出力する。差分器31pは両者の差分を演算し、ドリフト分を抽出する。差分器31pは、sincos変換器31sにドリフト分を出力する。sincos変換器31s及びatan演算器31vは同様な演算を行い、ドリフト分としてξd1θを出力する。sincos変換器31eは、ξn0φに対してsin変換及びcos変換を行い、それぞれの値をatan2演算器31kに出力する。atan2演算器31kは、sin変換値及びcos変換値を用いてatan2演算を行い、差分器31qに出力する。一方、sincos変換器31fは、ξm0φに対してsin変換及びcos変換を行い、それぞれの値をatan2演算器31mに出力する。atan2演算器31mは、sin変換値及びcos変換値を用いてatan2演算を行い、差分器31qに出力する。差分器31qは両者の差分を演算し、ドリフト分を抽出する。差分器31qは、sincos変換器31tにドリフト分を出力する。sincos変換器31t及びatan2演算器31wは同様な演算を行い、ドリフト分としてξd1φを出力する。以上のようにして、角速度姿勢角、加速度姿勢角の不連続性による異常計算発生を防止している。また、姿勢角差を演算したときの姿勢角の不連続性による異常計算発生を防止している。また、ドリフト分の姿勢角の不連続性も除去してPID制御の動作不良を防止できる。図25に、本実施形態の処理フローチャートを示す。まず、角速度姿勢角ξn1と加速度姿勢角ξm1に対してそれぞれsincos変換を行い、atan2、あるいはatan演算を行うS101。次に、演算後の値の差分を演算するS102。演算して得られた値ξd0=ξm1-ξn1はドリフト分である。このドリフト分に対して、さらにsincos変換及びatan2演算、あるいはatan演算を行ってξd1を算出するS103。ドリフト分ξd1を演算した後、このドリフト分に対してPID制御を行う。すなわち、比例項、積分項、微分項を、ξd1p=Pξd1ξd1i=Iξd1ξd1d=Dξd1と演算し、これらを加算してξs0=ξd1pξd1iξd1dを算出するS104、S105。その後、角速度姿勢角ξn1とξs0との差分を演算すればよいが、姿勢角はΨ、φがξm=-πとξp=πの間の値、θがξm=-π/2とξp=π/2の間の値として定義されているから、Ψ、φについてはξs0の値が-πとπの間の値を超えると制御が不安定になる。そこで、ξs0がこの範囲を超える場合には、この範囲内の直近の値に置き換える必要が生じる。そこで、まず、ξs0がξm=-πより小さいか否かを判定しS106、ξm=-πより小さい場合には、ξs0をξm=-πに置き換えてξs1とするS108。また、ξs0がξp=πより大きい場合もS107、ξs0をξp=πに置き換えてξs1とするS109。ξs0がξm=-πとξp=πの間である場合には問題ないので、そのままの値をξs1とし、ξn1との差分を演算するS111。一方、θについてはξs0の値がξm=-π/2とξp=π/2の間の値を超えると制御が不安定になる。そこで、ξs0がこの範囲を越える場合には、この範囲内の直近の値に置き換える必要が生じる。そこで、まず、ξs0がξm=-π/2より小さいか否かを判定しS106、ξm=-π/2より小さい場合には、ξs0をξm=-π/2δに置き換えてξs1とするS108。また、ξs0がξp=π/2より大きい場合もS107、ξs0をξp=π/2-δに置き換えてξs1とするS109。ここで、δは実数であり演算で有効な最小数値を表す。ξs0がξm=-π/2とξp=π/2の間である場合には問題ないので、そのままの値をξs1とし、ξn1との差分を演算するS111。但し、ξs0が-π/2となるときは-π/2δ、π/2となるときはπ/2-δとする。以上の処理により、安定な姿勢角を出力できる。このように、本実施形態ではsin変換cos変換atan2変換SCA変換を行ってパラメータの不連続性を解消しているが、以下に姿勢角の不連続性及び差分器31の処理について補足説明する。次に姿勢角の不連続性について説明する。姿勢角はX軸回りのΨ、Y軸回りのθ、Z軸回りのφを用いてΨ、θ、φで表す。それぞれの範囲は、-π<Ψ≦πradあるいは-180<Ψ≦180deg-π/2<θ<π/2radあるいは-90<θ<90deg-π<φ≦πradあるいは-180<φ≦180degである。図26に、Z軸回りに回転したときの回転角度と姿勢角φとの関係を示す。Z軸回転が-720~720degに変化するとき、φは±180degの範囲を振動し180degを超えるときに-180deg付近へ急激に遷移する。これが不連続性の発生メカニズムである。運動体は時間に対して連続に回転図ではZ軸回転で表示しているが、姿勢角φは不連続となる。図27に、具体的にφ=180deg付近で±20degのsin波的な連続回転振動を与えたときの状態を示す。図において、Z軸回転180±20degのsin波的な振動となっており連続的に変化する。しかし、φは±180degの間を不連続に変化する。図28及び図29に、本発明による結果をSCA処理として同時に示す。これによるとSCA処理による波形は図27に示したφと同じ波形を示しており、従来の波形Mφのような歪んだ波形あるいは全く異なった波形となる問題を解決していることが分かる。Ψについても同様である。これにより、高精度で動作範囲の広い姿勢角検出を可能としている。図30に、Y軸回りに回転したときの回転速度とθの関係を示す。Y軸回転が-720~720degに変化するとき、θは±90degの範囲を振動し90degを超えるときに-90deg付近へ急激に遷移する。これが不連続性の発生メカニズムである。運動体は時間に対して連続に回転しているが、姿勢角θは不連続となる。図31に、具体的にθ=90deg付近で±20degのsin波的な連続回転振動を与えたときの状態を示す。Y軸回転は連続的に変化するが、θは±90degの間を不連続に変化する。図32及び図33に、本発明による結果をSCA処理として同時に示す。これによるとSCA処理による波形は図27に示したθと同じ波形を示しており、従来の波形Mθのような歪んだ波形あるいは全く異なった波形となる問題を解決していることが分かる。これにより、高精度で動作範囲の広い姿勢角検出を可能としている。次にSCA差分器について説明する。差分器31は、姿勢角Ψ、θ、φの各成分に対して差分を行う。例えばφに関しては、図26において角速度姿勢角ξnoφが175deg、加速度姿勢角ξmoが-25degとする。差演算するとξno-ξmo=200degとなる。これは、φの定義範囲±180degを超えているので不合理が生じる。このように、演算処理によりφが定義範囲を超えてしまうことが容易に発生する。そこで、差分器31は定義範囲に入れる処理を行う。これがsincosatan2変換SCA変換である。具体的には、ξnoφ-ξmoφのsin値とcos値を得る。そして、atan2演算によりsin値とcos値を用いてξd1φを得る。このとき、ξd1φは定義範囲に入っている。条件次第で200-360=-160degにする方法もある。しかし、±180degを大きく超えた、±360deg、±540deg、±720deg、と考えられるケースが多く、条件次第では極めて多くの、本質的には無限に組合せを記述する必要が生じる。また、条件次第では演算上大きな数値変化が生じ不連続となる。そのため、取扱が非常に困難となる。実際はノイズが重畳した信号を対象とするので、±180deg付近でノイズ重畳したとき、極めて大きい不連続な振動が発生することになる。これは、演算精度との関係で、演算上、不安定になりやすく、精度の高い、安定した出力を得ることができなくなる。これを滑らかに算術的に実行する方法がSCA変換処理である。これは、計算処理過程で定義範囲を超えたデータを定義範囲に入れる処理で、正規化に似た処理である。ところで、ξnoφ自体も途中の演算で定義範囲を超えていることがある。そこで、ξnoφをsinとcosで表現し、atan2を演算することにより、まずξnoφが±180deg内に入るようにする。ξmoφも同様に定義範囲に正規化される。次に、差分器31でξnoφ-ξmoφの演算を行う。この差分器の出力は状態によってはφの定義範囲を超える。そこで、sincosatan2変換により定義範囲に入れ直し、正規化を行う。姿勢角Ψ、θ、φの内、Ψ、φの定義域は-π~πでありatan2処理の出力範囲と同じなのでatan2演算を行う。一方、θの定義域は-π/2~π/2なのでatan2処理の出力範囲の半分となる。そこで、θはatan演算を用いる。atanの出力範囲は-π/2~π/2でθと一致する。atany,x=tan1y,xはx小の領域ではtany/xの分母が小さいため誤差が大きくなる。このため、xがyに対して小さいときにはcot1y,xを用いる。以上、実施形態について説明したが、これに限定されず種々の変更が可能である。例えば、図6の構成では運動周波数検知器52で2足歩行等ロボットの運動に特有の周波数を検出してローパスフィルタ46、48に出力しているが、図1~図3の構成におけるローパスフィルタ18、28に出力し、これらのローパスフィルタ18、28のカットオフ周波数を適応的に可変設定してもよい。また、図1の構成では、行列姿勢角演算器角速度系16からの角速度系姿勢角をローパスフィルタ18に出力し、行列姿勢角演算器加速度系26からの加速度姿勢角をローパスフィルタ28に出力しているが、行列姿勢角演算器16からの角速度系姿勢角及び行列姿勢角演算器26からの加速度姿勢角をともに直接差分器30に出力し、差分器30からの差分を単一のローパスフィルタに入力して低域成分を出力し、この低域成分をドリフト分として減算器32に出力してもよい。これにより、LPFの数を減らせるという利点がある。図2、図3の構成についても同様である。また、図1~図6におけるローパスフィルタとしては、アナログフィルタあるいはデジタルフィルタのいずれを用いてもよい。デジタルフィルタとしては、FIRFinite-durationImpulseResponseフィルタあるいはIIRInfinite-durationImpulseResponseフィルタのいずれでもよい。また、各構成において、出力装置34から姿勢角、角速度、加速度の少なくともいずれかをロボットのメインプロセッサに出力しているが、これに加えて、行列加算演算器14で演算された姿勢行列、加速度行列演算器24で演算された姿勢行列、差分器30で演算されたドリフト分、4元数LPF63、67等で抽出された4元数等を出力装置34に出力し、出力装置34からメインプロセッサに出力してもよい。また、図20、図22、図23の各構成においても、角速度センサ10からの角速度、及び加速度センサ20からの加速度を入力し、運動体であるロボットの運動に特有な周波数を検出する運動周波数検知器52を設けてもよい。運動周波数検知器は、角速度及び加速度から頻繁に現れる周波数成分を抽出し、ローパスフィルタ18、28に出力する。運動体としてのロボットが2足歩行する場合の、2足歩行に特有の周波数等が一例である。ローパスフィルタ18、28は、運動周波数検出器から入力した周波数に応じてそのカットオフ周波数を適応的に調整する。運動周波数が増大するほど、ローパスフィルタ18、28はカットオフ周波数を上げるように調整する。また、運動周波数検出器は、PID制御を実行する倍率器38、積算器40、差分器42それぞれに運動周波数を出力する。倍率器38、積算器40、差分器42は、入力した運動周波数に応じて乗算係数を調整する。以上、実施形態について説明したが、各実施形態の方式はそれぞれ以下のような固有の効果を奏する。すなわち、姿勢角を用いた方式では、出力はロボット、車両、船舶、航空関係で制御等に用いられる姿勢角である。姿勢角そのものを装置内での補正ループの制御対象としているので、実現象に適した安定した出力が得られる。また、適応する計測制御対象により装置内の制御パラメータを最適化する際に、実運動による姿勢角と適合性が良く、効率良く各種パラメータを最適化できる。そのため、最適化に必要とされる時間や労力が少なくて済み、応用に適した特性が得られる。本方式によれば、姿勢角を主体に活用する計測制御対象に適した方式であり、従来不可能あるいは不十分であったロボットの姿勢計測制御や車両の姿勢計測制御が可能となる。また、姿勢行列を用いた方式では、出力はロボット、車両、船舶、航空関係で計測制御等に用いられる姿勢角であるが、内部演算では姿勢行列を補正ループ対象としている。そのため、内部演算において特異点等の回避等の特殊処理を行わないので分かりやすいという利点がある。また、外部に姿勢行列そのものを出力することもできる。従って、姿勢行列を用いて計測制御を行う計測制御対象に適した方式である。姿勢行列は要素数が姿勢角より多くデータ数が多い。そのため通信に要する時間やデータ処理が煩雑になるという欠点がある。しかし、データ数が多い分、特異点の問題を回避できる点や、拡大、縮小、変形座標系への対応が可能であるという自由度の高さをもっている。通信速度が十分速く、計測制御側の演算速度が十分速く、演算に必要なメモリ領域が大きな場合は使いやすいという利点がある。従って、高速通信網を持ち、演算速度の速い高性能な計測制御処理システムに応用する場合、例えば産業用機器、原子力発電所等での高度な計測制御機器、宇宙用機器等において本方式は特に効果を発揮する。また、4元数を用いた方式では、出力はロボット、車両、船舶、航空関係で計測制御等に用いられる姿勢角であるが、内部演算では4元数を補正ループ対象としている。4元数を用いているので演算に必要なデータ量や演算ステップを少なくできるという利点を有する。従って、装置において少ないメモリ、中規模の演算速度をもつプロセッサを用いて本機能を実現できる利点がある。その結果、コストの低減が可能となる。しかしながら、4元数は実空間での振る舞いが分かりにくい表記方法であるので姿勢角に変換して用いている。従って、本方式は計測制御対象の特性が明確であり、同様な運動が多い対象に適した方式であり、汎用ロボットや量産車両、小規模産業用機器の計測制御に適した方式である。"