(57)【要約】 【課題】  どのような姿勢でも精度を下げないアルゴリズムを有し、このアルゴリズムによって理想的な姿勢角度検出装置を得る。【解決手段】  ３個のジャイロスコープと、該ジャイロスコープの角速度に応じた出力に基づいて単位時間に移動した角度を２種類のオイラー角で演算する運動角演算装置と、２軸の加速度を検出する加速度センサと、２軸の地磁気を検出する地磁気センサと、前記加速度センサ及び地磁気センサの出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を２種類のオイラー角で演算する静止角演算装置と、演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、姿勢角を２種類のオイラー角で演算する姿勢角演算装置と、２種類のオイラー角から誤差が小さく使用するべきオイラー角を選択し、もう一方のオイラー角に変換するオイラー角変換演算装置とから構成される姿勢角度検出装置。【選択図】  図１
2005331265.tif 000002
【特許請求の範囲】
【請求項１】
水平面内で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸に直交する軸をＺ軸とし、これらの固定されたＸ軸、Ｙ軸、Ｚ軸からなる座標軸を基準座標とし、姿勢角度検出装置とともに移動するＸｓ軸、Ｙｓ軸、Ｚｓ軸からなる座標軸をセンサ座標とするとき、Ｘｓ軸、Ｙｓ軸及びＺｓ軸の回りの角速度を検出する３個のジャイロスコープと、該ジャイロスコープの角速度に応じた出力に基づいて単位時間に移動した角度を２種類のオイラー角で演算する運動角演算装置と、Ｘｓ−Ｙｓ平面で互いに直交する２軸の加速度を検出するように配置された加速度センサと、Ｘｓ−Ｙｓ平面で互いに直交する２軸の地磁気を検出するように配置される地磁気センサと、前記加速度センサ及び地磁気センサの出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を２種類のオイラー角で演算する静止角演算装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、運動角演算装置の演算結果と静止角演算装置の演算結果から出力するべき姿勢角を２種類のオイラー角で演算する姿勢角演算装置と、２種類のオイラー角から誤差が小さく使用するべきオイラー角を選択し、もう一方のオイラー角に変換するオイラー角変換演算装置とから構成されることを特徴とする姿勢角度検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の姿勢や、３Ｄゲームパッドや、姿勢制御・バーチャルリアリティー等に使用されるヘッドマウントディスプレーでの頭の姿勢角度を検出するトラッカー等、３次元空間中で測定対象物の姿勢角度（３軸の回転角度）を検出するのに特に適した姿勢角検出装置に関する。
【背景技術】
【０００２】
３次元の姿勢角度を表現する方法として、オイラー角がしばしば用いられてきた。オイラー角は、３つの軸の回転させる順番を定めたもので、その順番によってＺ−Ｙ−Ｘオイラー角、Ｚ−Ｙ−Ｚオイラー角など全部で１２通りの順番が存在する。姿勢角度検出装置に使われるオイラー角で最も一般的なものが、Ｚ−Ｙ−Ｘオイラー角である。Ｚ軸まわりの角度がヨー角α、Ｙ軸まわりの角度がピッチ角β、Ｘ軸まわりの角度がロール角γとして表される。
【０００３】
特許文献１には、Ｚ−Ｙ−Ｘオイラー角で姿勢角度を演算して出力する姿勢角検出装置について記載されている。  
【０００４】
しかしながら、上記の方法を用いる場合、姿勢角度検出装置の姿勢がオイラー角のもつ特異点になったときに誤差が大きくなるという問題があった。それぞれのオイラー角と特異点の代表例を表１に示す。
【０００５】
【表１】
2005331265.tif 000003
【０００６】
Ｚ−Ｙ−Ｘオイラー角を例にすると、Ｘ軸が垂直のとき、すなわちピッチ角βが±９０度のときに特異点となり、ヨー角αとロール角γは不定となってしまう。ピッチ角βがちょうど９０度にならなくても、ピッチ角βが９０度に近くなると、例えば６０度以上では精度が悪くなる。例えば、Ｚ−Ｙ−Ｘオイラー角で（１８０，８９，１８０）という姿勢と（０，８９，０）という姿勢はどちらも特異点から１度ずれただけの姿勢で、Ｚ−Ｘ−Ｙオイラー角では両者の差は２度しかないが、Ｚ−Ｙ−Ｘオイラー角ではヨー角α、ロール角γともに１８０度もの差になってしまっている。
【０００７】
特許文献１では、運動角、静止角という２種類の仮の姿勢角を演算してから、それらを組み合わせて最終的な出力角度である姿勢角を演算している。このときに運動角、静止角が上記の（１８０，８９，１８０）、（０，８９，０）という姿勢であるとすると最終の姿勢角も誤差の大きな角度になってしまう。
【０００８】
【特許文献１】特開平１１−２１１４７４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００９】
本発明の目的は、従来の姿勢角度検出装置の問題点の解消を図り、どのような姿勢においても精度を下げないアルゴリズムを有し、このアルゴリズムによって理想的な姿勢角度検出装置を提供することである。
【課題を解決するための手段】
【００１０】
本発明の姿勢角度検出装置は、どのような姿勢においても精度を下げないアルゴリズムを有する姿勢角度検出装置とする。即ち、本発明は、水平面内で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸に直交する軸をＺ軸とし、これらの固定されたＸ軸、Ｙ軸、Ｚ軸からなる座標軸を基準座標とし、姿勢角度検出装置とともに移動するＸｓ軸、Ｙｓ軸、Ｚｓ軸からなる座標軸をセンサ座標とするとき、Ｘｓ軸、Ｙｓ軸及びＺｓ軸の回りの角速度を検出する３個のジャイロスコープと、該ジャイロスコープの角速度に応じた出力に基づいて単位時間に移動した角度を２種類のオイラー角で演算する運動角演算装置と、Ｘｓ−Ｙｓ平面で互いに直交する２軸の加速度を検出するように配置された加速度センサと、Ｘｓ−Ｙｓ平面で互いに直交する２軸の地磁気を検出するように配置される地磁気センサと、前記加速度センサ及び地磁気センサの出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を２種類のオイラー角で演算する静止角演算装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、運動角演算装置の演算結果と静止角演算装置の演算結果から出力するべき姿勢角を２種類のオイラー角で演算する姿勢角演算装置と、２種類のオイラー角から誤差が小さく使用するべきオイラー角を選択し、もう一方のオイラー角に変換するオイラー角変換演算装置とから構成される姿勢角度検出装置である。
【発明の効果】
【００１１】
本発明によれば、特異点の異なる２種類のオイラー角を組み合わせて演算することによって、どのような姿勢においても精度の良い出力が得られるアルゴリズムによって高精度の姿勢角度検出装置が実現できる。
【発明を実施するための最良の形態】
【００１２】
以下、本発明の実施の形態による姿勢角度検出装置について図面を参照して説明する。なお、Ｚ−Ｙ−Ｘオイラー角の出力はヨー角α、ピッチ角β、ロール角γ、（α，β，γ）で表し、Ｚ−Ｘ−Ｙオイラー角の出力はヨー角φ、ロール角θ、ピッチ角ψ、（φ，θ，ψ）で表すこととする。
【００１３】
図１は、本発明における姿勢角度検出装置の一例の構成図である。図１に示すように、本発明の姿勢角度検出装置におけるジャイロ、加速度センサ、磁気センサの配置を示す。図１に示すセンサ座標系において、互いに直交する３軸（Ｘｓ軸、Ｙｓ軸およびＺｓ軸）の回りの角速度を検出するための第１のジャイロ１１、第２のジャイロ１２および第３のジャイロ１３は、Ｘｓ軸、Ｙｓ軸およびＺｓ軸に平行に、すなわち互いに直交して、配置されている。第１の加速度センサ１４、第２の加速度センサ１５および第３の加速度センサ１６は、互いに直交する３軸、Ｘｓ軸、Ｙｓ軸およびＺｓ軸に平行に配置されている。同様に、第１の磁気センサ１７、第２の磁気センサ１８および第３の磁気センサ１９は、互いに直交する３軸、Ｘｓ軸、Ｙｓ軸およびＺｓ軸に平行に配置されている。
【００１４】
図２は、本発明における姿勢角度検出装置の全体構成に関する説明図である。図２に示すように、ジャイロ２２ａ，２２ｂ，２２ｃ、加速度センサ２３ａ，２３ｂ，２３ｃおよび磁気センサ２４ａ，２４ｂ，２４ｃからは、それぞれ角速度（ωｘ，ωｙ，ωｚ）、加速度（Ａｘ，Ａｙ，Ａｚ）、磁気（Ｍｘ，Ｍｙ，Ｍｚ）が出力される。運動角演算装置２６ａは、ジャイロ２２の出力から運動角（Δα，Δβ，Δγ）および（Δφ，Δθ，Δψ）を演算し出力する。静止角演算装置２６ｂは、加速度センサ２３および磁気センサ２４の出力から静止角（α’，β’，γ’）および（φ’，θ’，ψ’）を演算し出力する。判別装置２７は、運動角および静止角から静止角が正しいかどうかを判別し、姿勢角演算装置２６ｃに出力する。姿勢角演算装置２６ｃは判別装置からの判断を元に、運動角と静止角を組み合わせて仮の姿勢角（α’’，β’’，γ’’）および（φ’’，θ’’，ψ’’）を出力する。オイラー角変換演算装置２８は、仮の姿勢角から出力するべき最終的な姿勢角を演算し、オイラー角（α，β，γ）を出力する。
【００１５】
次に、運動角演算装置の内容を説明する。姿勢角度検出装置に加わる角速度に応じた各ジャイロの出力（ωｘ，ωｙ，ωｚ）は、座標変換を行って、演算する単位時間で１単位時間過去の出力α（ｎ−１）、β（ｎ−１）、γ（ｎ−１）、φ（ｎ−１）、θ（ｎ−１）およびψ（ｎ−１）とから、次式の如く、基準座標系における現在の移動角度（運動角）（Δα，Δβ，Δγ）および（Δφ，Δθ，Δψ）に変換される。
【００１６】
Δα（ｎ）＝ωｙ・ｓｉｎγ（ｎ−１）／ｃｏｓβ（ｎ−１）＋ωｚ・ｃｏｓγ（ｎ−１）／ｃｏｓβ（ｎ−１）、
Δβ（ｎ）＝ωｙ・ｃｏｓγ（ｎ−１）−ωｚ・ｓｉｎγ（ｎ−１）、
Δγ（ｎ）＝ωｘ＋ωｙ・ｔａｎβ（ｎ−１）・ｓｉｎγ（ｎ−１）＋ωｚ・ｔａｎβ（ｎ−１）・ｃｏｓγ（ｎ−１）、
Δφ（ｎ）＝−ωｘ・ｓｉｎψ（ｎ−１）／ｃｏｓθ（ｎ−１）＋ωｚ・ｃｏｓψ（ｎ−１）／ｃｏｓθ（ｎ−１）、
Δθ（ｎ）＝ωｘ・ｃｏｓψ（ｎ−１）＋ωｚ・ｓｉｎψ（ｎ−１）、
Δψ（ｎ）＝ωｘ・ｔａｎθ（ｎ−１）・ｓｉｎψ（ｎ−１）＋ωｙ−ωｚ・ｔａｎθ（ｎ−１）・ｃｏｓψ（ｎ−１）
【００１７】
次に、静止角演算装置の説明を行う。図１に示す加速度センサ１４、１５および１６は、それぞれセンサ座標系での重力加速度のＸｓ、ＹｓおよびＺｓ軸の成分分力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）を検出するように配置されている。また、図１に示す磁気センサ１７、１８および１９は、それぞれＸｓ、ＹｓおよびＺｓ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）を検出するように配置されている。
【００１８】
加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）は、下式により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のロール角γ’（ｎ）、θ’（ｎ）および仮のピッチ角β’（ｎ）、ψ’（ｎ）を算出する。
【００１９】
β’（ｎ）＝ｓｉｎ-1Ａｘ（ｎ）
γ’（ｎ）＝ｔａｎ-1［−Ａｙ（ｎ）／−Ａｚ（ｎ）］
θ’（ｎ）＝−ｓｉｎ-1Ａｙ（ｎ）
ψ’（ｎ）＝ｔａｎ -1［Ａｘ（ｎ）／−Ａｚ（ｎ）］
【００２０】
基準座標系での仮のヨー角α’（ｎ）を求めるために、基準座標系での地磁気成分ＨＸ１、ＨＹ１を地磁気センサの出力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）と仮のピッチ角β’（ｎ）、仮のロール角γ’（ｎ）を用いて次式のごとく座標変換を行う。
【００２１】
ＨＸ１（ｎ）＝Ｍｘ・ｃｏｓβ’（ｎ）＋［Ｍｙ・ｓｉｎγ’（ｎ）＋Ｍｚ・ｃｏｓγ’（ｎ）］・ｓｉｎβ’（ｎ）
ＨＹ１（ｎ）＝Ｍｙ・ｃｏｓγ’（ｎ）−Ｍｚ・ｓｉｎγ’（ｎ）
【００２２】
基準座標系での仮のヨー角α’（ｎ）は、
α’（ｎ）＝−ｔａｎ-1［ＨＹ１（ｎ）／ＨＸ１（ｎ）］−α（０）
で表される。ただし、α（０）は原点時の方位角である。
【００２３】
同様に、基準座標系での仮のヨー角φ’（ｎ）を求めるために、基準座標系での地磁気成分ＨＸ２、ＨＹ２を地磁気センサの出力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）と仮のピッチ角ψ’（ｎ）、仮のロール角θ’（ｎ）を用いて次式のごとく座標変換を行う。
ＨＸ２（ｎ）＝Ｍｘ・ｃｏｓψ’（ｎ）＋Ｍｚ・ｓｉｎψ’（ｎ）
ＨＹ２（ｎ）＝Ｍｙ・ｃｏｓθ’（ｎ）＋［Ｍｘ・ｓｉｎψ’（ｎ）−Ｍｚ・ｃｏｓψ’（ｎ）］・ｓｉｎθ’（ｎ）
【００２４】
基準座標系での仮のヨー角φ’（ｎ）は
φ’（ｎ）＝−ｔａｎ-1［ＨＹ２（ｎ）／ＨＸ２（ｎ）］−α（０）
で表される。
【００２５】
次に、判別装置を説明する。加速度センサの出力Ａｘ、ＡｙおよびＡｚは重力加速度のＸｓ、ＹｓおよびＺｓ軸方向成分と運動加速度のＸｓ、ＹｓおよびＺｓ軸方向成分との合成ベクトルとなるために、運動加速度がある場合には静止角演算装置によって算出された仮のピッチ角β’などは正しいピッチ角にはならない。また仮のヨー角α’などについても磁場環境によって正しいヨー角にならない場合がある。判別装置は静止角演算装置の出力であるα’（ｎ）、β’（ｎ）、γ’（ｎ）、φ’（ｎ）、θ’（ｎ）、ψ’（ｎ）が正しいかどうかを判別するものである。
運動加速度が加わっていない状態ではＡｘ、ＡｙおよびＡｚの合成ベクトルは１Ｇとなるので微小定数εを用いて、
｜√（Ａｘ2＋Ａｙ2＋Ａｚ2）−１｜＜ε
が成立するならば、それぞれα’、β’、γ’、φ’、θ’、ψ’は正しいと判断する。
【００２６】
次に、上記判別装置の判別結果に応じて、運動角演算装置の出力と静止角演算装置の出力を演算して最終的な出力を得る姿勢角演算装置で行われる演算方法について説明する。
判別装置においてα’、β’、γ’、φ’、θ’、ψ’が正しいと判断された場合には、
α’’（ｎ）＝α（ｎ−１）＋Δα−ｋ［α（ｎ−１）＋Δα−α’（ｎ）］
β’’（ｎ）＝β（ｎ−１）＋Δβ−ｋ［β（ｎ−１）＋Δβ−β’（ｎ）］
γ’’（ｎ）＝γ（ｎ−１）＋Δγ−ｋ［γ（ｎ−１）＋Δγ−γ’（ｎ）］
φ’’（ｎ）＝φ（ｎ−１）＋Δφ−ｋ［φ（ｎ−１）＋Δφ−φ’（ｎ）］
θ’’（ｎ）＝θ（ｎ−１）＋Δθ−ｋ［θ（ｎ−１）＋Δθ−θ’（ｎ）］
ψ’’（ｎ）＝ψ（ｎ−１）＋Δψ−ｋ［ψ（ｎ−１）＋Δψ−ψ’（ｎ）］
により姿勢角度を演算する。
【００２７】
判別装置においてα’、β’、γ’、φ’、θ’、ψ’が誤っていると判断された場合には、
α’’（ｎ）＝α（ｎ−１）＋Δα
β’’（ｎ）＝β（ｎ−１）＋Δβ
γ’’（ｎ）＝γ（ｎ−１）＋Δγ
φ’’（ｎ）＝φ（ｎ−１）＋Δφ
θ’’（ｎ）＝θ（ｎ−１）＋Δθ
ψ’’（ｎ）＝ψ（ｎ−１）＋Δψ
により姿勢角度を演算する。ただし、ｋは補正係数で０＜ｋ＜１である。
【００２８】
最後に、オイラー角変換演算装置について説明する。姿勢角演算装置からの出力は、Ｚ−Ｙ−Ｘオイラー角（α’’，β’’，γ’’）とＺ−Ｘ−Ｙオイラー角（φ’’，θ’’，ψ’’）である。｜β’’｜＞６０度ではＺ−Ｙ−Ｘオイラー角の方が精度が悪く、｜θ’’｜＞６０度ではＺ−Ｘ−Ｙオイラー角の方が精度が悪いので、｜θ’’｜＞６０度のときにはＺ−Ｙ−Ｘオイラー角（α’’，β’’，γ’’）を、｜θ’’｜≦６０度のときにはＺ−Ｘ−Ｙオイラー角（φ’’，θ’’，ψ’’）を採用する。
【００２９】
Ｚ−Ｙ−Ｘオイラー角（α’’，β’’，γ’’）が採用されたときには、そのまま最終的な出力となる（α，β，γ）として出力し（α＝α’’，β＝β’’，γ＝γ’’）、さらに（φ，θ，ψ）に変換する。
【００３０】
Ｚ−Ｘ−Ｙオイラー角（φ’’，θ’’，ψ’’）が採用されたときには、それが（φ，θ，ψ）となり（φ＝φ’’，θ＝θ’’，ψ＝ψ’’）、さらに最終的な出力となる（α，β，γ）に変換して出力する。
【００３１】
以下に、Ｚ−Ｙ−Ｘオイラー角（α’’，β’’，γ’’）からＺ−Ｘ−Ｙオイラー角（φ，θ，ψ）への変換方法について説明する。センサ座標軸上の２点、Ｐ（１，０，０）、Ｑ（０，１，０）が基準座標軸上でどのように表されるかを図３、図４で説明する。Ｚ−Ｙ−Ｘオイラー角（α’’，β’’，γ’’）が採用されたときにはＰ点、Ｑ点を図３のごとく回転させ基準座標軸上での位置を計算する。数１は、センサ座標軸上のＰ、ＱをＺ−Ｙ−Ｘオイラー角を用いて基準座標で表すための計算式である。
【００３２】
【数１】
2005331265.tif 000004
【００３３】
次に、図４のようにＱ点からＸＹ平面への垂線とＸＹ平面の交点であるＱ’点を考え、ヨー角φとロール角θを次式により計算する。
φ＝ｔａｎ-1（−Ｑｘ／Ｑｙ）
θ＝ｔａｎ-1（Ｑｚ／√(Ｑｘ2＋Ｑｙ2)）
【００３４】
次に、図４のようにＰ点からＸ軸をＺ軸回りにφだけ回転させたＸ’軸への垂線とＸ’軸との交点であるＰ’（Ｐｘ’，Ｐｙ’，０）を求める。線分ＰＰ’とＸ’軸が直交することから、Ｐｘ’，Ｐｙ’を計算すると、
Ｐｘ’＝Ｐｘ ｃｏｓ2φ＋Ｐｙ ｓｉｎφ ｃｏｓφ
Ｐｙ’＝Ｐｘ ｓｉｎφ ｃｏｓφ＋Ｐｙ ｓｉｎ2φ
となる。
【００３５】
従って、ψはＰ’点から、
ψ＝±ｃｏｓ-1√(Ｐｘ’2＋Ｐｙ’2)
と求められる。また、符号については、Ｐｚ＞０のときは負、Ｐｚ＜０のときは正になる。
【００３６】
また、以下に、Ｚ−Ｘ−Ｙオイラー角（φ’’，θ’’，ψ’’）からＺ−Ｙ−Ｘオイラー角（α，β，γ）への変換方法について説明する。センサ座標軸上の２点、Ｐ（１，０，０）、Ｑ（０，１，０）が、基準座標軸上でどのように表されるかを図５、図６で説明する。Ｚ−Ｘ−Ｙオイラー角（φ’’，θ’’，ψ’’）が採用されたときにはＰ点、Ｑ点を図５のように回転させ基準座標軸上での位置を計算する。数２は、センサ座標軸上のＰ、ＱをＺ−Ｘ−Ｙオイラー角を用いて基準座標で表すための計算式である。
【００３７】
【数２】
2005331265.tif 000005
【００３８】
次に、図６のようにＰ点からＸＹ平面への垂線とＸＹ平面の交点であるＰ’点を考え、ヨー角αとピッチ角βを次式により計算する。
α＝ｔａｎ-1（Ｐｙ／Ｐｘ）
β＝ｔａｎ-1（−Ｐｚ／√(Ｐｘ2＋Ｐｙ2)）
【００３９】
次に、図６のようにＱ点からＹ軸をＺ軸回りにαだけ回転させたＹ’軸への垂線とＹ’軸との交点であるＱ’（Ｑｘ’，Ｑｙ’，０）を求める。線分ＱＱ’とＹ’軸が直交することからＱｘ’，Ｑｙ’を計算すると
Ｑｘ’＝Ｑｘｓｉｎ2α−Ｑｙ ｓｉｎα ｃｏｓα
Ｑｙ’＝−Ｑｘｓｉｎα ｃｏｓα＋Ｑｙ ｃｏｓ2α
となる。従って、γはＱ’点から、
γ＝±ｃｏｓ-1√(Ｑｘ’2＋Ｑｙ’2)
と求められる。また、符号については、Ｑｚ＞０のときは正、Ｑｚ＜０のときは負になる
【図面の簡単な説明】
【００４０】
【図１】本発明における姿勢角度検出装置の一例の構成図。
【図２】本発明における姿勢角度検出装置の全体構成に関する説明図。
【図３】Ｐ点、Ｑ点をＺ−Ｙ−Ｘオイラー角で回転させる説明図。
【図４】Ｐ点、Ｑ点からＺ−Ｘ−Ｙオイラー角を求める説明図。
【図５】Ｐ点、Ｑ点をＺ−Ｘ−Ｙオイラー角で回転させる説明図。
【図６】Ｐ点、Ｑ点からＺ−Ｙ−Ｘオイラー角を求める説明図。
【符号の説明】
【００４１】
１１    第１のジャイロ
１２    第２のジャイロ
１３    第３のジャイロ
１４    第１の加速度センサ
１５    第２の加速度センサ
１６    第３の加速度センサ
１７    第１の磁気センサ
１８    第２の磁気センサ
１９    第３の磁気センサ
２２ａ，２２ｂ，２２ｃ    ジャイロ
２３ａ，２３ｂ，２３ｃ    加速度センサ
２４ａ，２４ｂ，２４ｃ    磁気センサ
２６ａ    運動角演算装置
２６ｂ    静止角演算装置
２６ｃ    姿勢角演算装置
２７    判別装置
２８    オイラー角変換演算装置
２９    出力
【図１】
2005331265.tif 000006
【図２】
2005331265.tif 000007
【図３】
2005331265.tif 000008
【図４】
2005331265.tif 000009
【図５】
2005331265.tif 000010
【図６】
2005331265.tif 000011