JP2003521697A

JP2003521697A - 変形４元数データ表記法を用いる傾動可能な物体の姿勢推定

Info

Publication number: JP2003521697A
Application number: JP2001556277A
Authority: JP
Inventors: ロー・ジェフリー・ディー
Original assignee: Independence Technology LLC
Current assignee: Independence Technology LLC
Priority date: 2000-02-03
Filing date: 2001-02-01
Publication date: 2003-07-15
Anticipated expiration: 2021-02-01
Also published as: IL151006A; EP1257784B1; CN1270162C; DK1257784T3; AU3472701A; MXPA02007560A; NO20023641L; WO2001057474A1; CA2398704C; RU2253092C2; AU777647B2; NO335201B1; CA2398704A1; PT1257784E; ATE329228T1; US6377906B1; IL151006A0; EP1257784A1; JP4808354B2; WO2001057474A8

Abstract

(57)【要約】ジャイロ及びチルトセンサから出力された信号に基づいて傾動可能な物体の姿勢を追跡して制御する。ジャイロから出力された信号を変換し統合し、それにより推定位置情報をヨー成分が値０に拘束される変形４元数の形態で得る。また、同一形態の変形４元数情報をチルトセンサから出力された信号から生成し、これを利用して推定位置情報中の誤差成分を検出してこれを補正する。ジャイロドリフトも又、チルトセンサの出力に基づいて補正する。

Description

【発明の詳細な説明】

【０００１】本発明は、傾動可能な物体の追跡及び制御に関する。

【０００２】４元数を用いて物品の空間内での方位を表すことが知られている。４元数表記
法は一般に、広く採用されているオイラーデータ表示法よりも計算上の利用効率
が高い。さらに、４元数表記法は、オイラー表記法を用いた場合に生じる場合の
ある特異性の影響を受けない。次の米国特許、即ち、米国特許第５，８７５，９
９３号、同第５，２１２，４８０号、同第４，７９７，８３６号、同第４，７４
２，３５６号及び同第４，７３７，７９４号は、４元数を用いて空間中の物体の
方位を制御し、決定し且つ（或いは）表示する技術を開示している。

【０００３】４元数の概略的説明４元数は、１８４３年にサー・ウィリアム・ローアン・ハミルトンによって最
初に案出された４つの要素（元）の多元数である。４元数は、スカラー部分と複
素数ベクトル部分とから成っている。ベクトル部分は、３つの互いに直交する複
素数の単位ベクトルｉ，ｊ，ｋにより割り当てられた方向である３つの実数成分
の順序付けられた３つ組（ベクトル）から成っている。一般的な４元数Ｑの一例
を以下に示す。

【０００４】

【数１】

【０００５】４元数の加算は、成分を同一方向に足し算することにより行われる。乗算は、
単位基本ベクトルの以下の積に着目することにより行われる。

【０００６】

【数２】

【０００７】４元数は、多元数なので、４元数はベクトル部分の方向が逆である複素行列を
更に有している。一例を以下に示す。

【０００８】

【数３】

【０００９】４元数の２乗を計算するには、４元数と以下に示すようなその複素共役の積を
求めるのがよい。

【００１０】

【数４】

【００１１】単位絶対値（Ｑ^２＝１）を持つ４元数は、特別な意味をもっている。具体的に
説明すると、この演算子は、両側回転演算子として働く。ハミルトンはその研究
努力により、複素数にｅｘｐ（ｉθ）の形式の単位複素数を掛けた場合に複素平
面内に生じる回転効果の３次元拡張法を発展させる４元数を発見したということ
は注目に値する。その結果、ｅｘｐ（ｉθ）の回転効果が得られる。その理由は
、複素数の乗算では、これらのそれぞれの絶対値の乗算及びこれらのそれぞれの
位相の加算が必要だからである。ｅｘｐ（ｉθ）は、単位絶対値を有しているの
で、このｅｘｐ（ｉθ）は、積の位相に影響を及ぼし得るに過ぎない。複素平面
内では、これは、原点回りの角度θの回転として現れる。この効果をベクトルの
回転に一般化しようとする際、ハミルトンは最初は３要素多元数を試行した。ハ
ミルトンは４つの要素が３次元空間中の「位相」変化の理由を説明することが必
要であると気づいたときに初めて首尾良く所望の結果を得た。

【００１２】典型的には、ベクトル回転は、片側回転演算子Ｒを用いて達成され、この演算
子Ｒは、３次元空間内では、実数の３×３直交行列として表すことができる。こ
の変換行列は、以下に示すように左辺の乗算によりベクトルｘをベクトルｘ’に
回転させる。

【００１３】

【数５】

【００１４】両側演算子は、左辺と右辺の両方の乗算を用いて適用する必要がある。４元数
演算子の場合、回転は、０のスカラー部分を備えた（即ち、ベクトルである）所
与の４元数（Χ）に以下に示すような単位４元数及びその共役を事前及び事後乗
算すると達成される。

【００１５】

【数６】

【００１６】その結果得られたベクトルΧ’は、特定の角度だけ一般軸の回りに回転したも
のであり、これらは両方とも単位４元数Ｑで求められる。回転軸が単位ベクトル
ｎで示され、回転角が角度θで表されている場合、単位４元数成分を以下のよう
に書き表すことができる。

【００１７】

【数７】

【００１８】これら成分は、次の正規化条件を満足する。

【００１９】

【数８】

【００２０】このようにして特定された４元数成分は、オイラーパラメータとも呼ばれてい
る。これらパラメータは、回転軸及び回転角度を導き出すのに必要な情報の全て
を含んでいる。単位ベクトルｎによって定められた回転軸は、固有軸とも呼ばれ
ているというのは、これは、固有値λ＝＋１に相当する片側回転行列Ｒの固有ベ
クトルだからである。これが生じる理由は、回転軸は、原点と回転座標系の両方
に共通でなければならず、したがって、回転演算子によっては変化しないもので
なければならないからである。いわゆる固有軸回転は、オイラー角回転と比較し
て一般軸回りの１回転であり、この回転は、３つの互いに別々の回転、即ち、ｚ
軸、ｙ軸、ｘ軸のそれぞれの回りでのヨー、ピッチ（pitch ）、ロール（roll）
を行うことにより同一の変換を達成する。

【００２１】４元数の複素数単位ベクトル（ｉ，ｋ，ｊ）は、以下に示すようなパウリのス
ピン行列に関連付けられる。

【００２２】

【数９】

【００２３】

【数１０】（なお、上記についてはＥ．カルタン著『スピノルの理論』を参照のこと）。

【００２４】パウリのスピン行列及び単位４元数係数の定義を用いると、単位４元数を以下
のように書き表すことができる。

【００２５】

【数１１】

【００２６】上式において、

【数１２】

【００２７】これは又、次の行列指数関数と同値であるように表すことができる。

【００２８】

【数１３】

【００２９】この４元数の形式と上述した単位複素数の指数関数形式の類似性に着目する。
この形式は、ハミルトンが最初に探究した「位相」の３次元変化を示唆している
。半角が現れるのは、Ｑが両側変換であるということによって説明がつく。かく
して、左辺と右辺の係数、Ｑ、Ｑ^＊は各々、所望の空間位相シフトの半分に寄与
する。

【００３０】４元数についての従来のハミルトン形式ではなくパウリのスピン行列形式を用
いるのが一層都合のよい場合が多い。例えば、以下に示すような内積を作ること
によりベクトルを行列として表すことができる。

【００３１】

【数１４】

【００３２】上式により次の行列が得られる。

【００３３】

【数１５】

【００３４】この行列形式は、多くの有用な特徴を備えている。例えば、単位法線ａによっ
て定められた平面を通るベクトルｘの鏡映は、以下に示すようなベクトルの行列
形式を用いて容易に得られることが証明できる。

【００３５】

【数１６】

【００３６】また、任意の回転を２つの鏡映によって得ることができるということも又証明
できる。鏡映平面がθ／２の角度で交差し、交線が単位ベクトルｎで規定される
場合、その結果得られる変換は、任意のベクトルｘを固有軸ｎの回りに角度θだ
け回転させることになる。これは、これら平面の単位法線がベクトルａ，ｂであ
る場合に以下に示される。

【００３７】

【数１７】

【００３８】回転を実行するこの両側演算子は、上述の４元数回転に酷似している。事実、
Ｑ＝ＢＡであることが確認できる。以下の乗算的単位元は、ベクトルの行列形式
の特性から結果として生じる。

【００３９】

【数１８】

【００４０】角度θの回転の場合、単位法線ベクトルａ，ｂは、角度θ／２のところで交わ
らなければならない。したがって、これらの内積と外積はそれぞれ、ａ・ｂ＝ｃ
ｏｓ（θ／２）及びａ×ｂ＝ｎｓｉｎ（θ／２）であり、ここで、ｎは、交線に
平行である。これらの値を式（２６）に代入すると、所望の結果が得られる。

【００４１】

【数１９】

【００４２】この展開の興味ある特徴は、４元数の係数を適当な単位ベクトルの単純な内積
と外積を用いて見出すことができるということにある。特に、２つのベクトルａ
，ｂは、回転軸に垂直であり且つθ／２の角度だけ離れていなければならない。
また、一座標系における２つの任意のベクトルα，βについての知識及び回転座
標系におけるこれらの回転対応ベクトルα’，β’についての知識が与えられる
と、変換の元になっている４元数及び関連の回転行列を一義的に求めることがで
きるということも又証明できる。かくして、原点と２つの外部参照又は基準点と
の間の基本座標系中に定められた２つのベクトルが与えられると、回転座標系か
らの同一の２つの外部基準点の追加の「照準（sighting）」をとり、最終座標系
で計測された基準ベクトルの座標を初期座標系中で計測された座標と比較するこ
とによりシステムの姿勢を一義的に決定することができる。

【００４３】また、４元数速度の直接積分により４元数成分を計算することも考えられる。
このためには、角速度の関数として４元数の導関数についての表現が必要である
。これを式（２１）を微分することにより導き出すことができる。

【００４４】その結果得られる４元数係数の導関数は各々、既存の４元数成分によって重み
付けされた角速度成分の一次結合である。４元数速度及び角速度をベクトル形式
にすると、結果的に以下の行列式が得られる。

【００４５】

【数２０】

【００４６】上式において、ドット（・）付きのｑ_ｉは、４元数速度の成分であり、ω_ｉは、
角速度ω（＝θ）の成分である。

【００４７】物体の向き及び（又は）回転を、２以上の座標系で表すことができる。例えば
、関心のある物体に対する座標系を定め、或いは、座標系を外部の固定された物
体に対して定めることが可能である。或る用途については、座標系を定めるため
に地球を固定された物体としてとることができる。式（２８）では、角速度は、
物体座標系に関係付けられる。角速度が地球座標系に関係付けられた類似の行列
式が式（２９）として以下に示されている。座標系をシフトさせると、４元数行
列において幾つかの符号の変化が生じることは注目されたい。

【００４８】

【数２１】この式（２９）を以下に示すような行列変数を用いて一層簡単な形態で書き表す
ことができる。

【００４９】

【数２２】また、この式（３０）を、Ｑ^ＴＱ＝Ｉであることに着目することによって以下に
示すようにω_Ｅについて解くことができる。

【００５０】

【数２３】

【００５１】４元数速度式を問題の物体の瞬時角速度を用いて積分すると、その結果得られ
る４元数は、座標を初期座標系から回転座標系に変換するのに必要な情報を含ん
でいる。かくして、４元数成分を用いると上述の回転行列Ｒ（式（８））を生じ
させることができる。物体座標を各座標に変換する回転行列は、４元数成分の項
で以下のように示される。

【００５２】

【数２４】

【００５３】変換行列Ｒ_ＥＢは直交行列なので、その転置行列は逆変換Ｒ_ＢＥである。した
がって、４元数を用いると任意の角運動にわたり物体の姿勢を追跡することがで
きる。

【００５４】比較目的のため、オイラー角速度を計算する式を以下に示す。関連の回転行列
Ｒも又、オイラー角速度の関数として示される。

【００５５】

【数２５】

【００５６】上式において、ψ＝ロール、θ＝ピッチ、Ψ＝ヨー、ω＝角速度ベクトル＝［Ｐ
ＱＲ］^Ｔである。

【００５７】なお、次のとおりである。

【数２６】

【００５８】公知のように、オイラー法は主として三角関数を利用しているので極めて多く
の計算を必要とする。加うるに、オイラー速度式は、ピッチ角のセカントで決ま
り、その結果、ピッチが±９０゜に近づくと、特異性が生じることになる。これ
とは対照的に、４元数を利用する方法は、三角関数を含んでおらず、乗算及び加
算を利用しているに過ぎない。その結果、４元数法は、計算効率が極めて高い。
上述したように、物体の姿勢を、物体座標系中に定められた２つのベクトル及び
２つの外部基準点が与えられると、４元数の形態で一義的に表すことができる。
しかしながら、特に地球上の物体の場合、陸上に位置しているにせよ、或いは水
域の表面上又は表面下にあるにせよ何れにせよ、２つの外部基準点を確立するこ
とは困難であると共に（或いは）費用がかかる場合がある。

【００５９】本発明者は、４元数表記法をどのように実用的に変形すれば変形４元数形態を
得るのに外部基準点を１つしか必要としないようにすることができるか、そして
かかる変形４元数形態を自由に傾動できる物体に実用的に適用できるが、これに
ついて方角（yaw）が変形４元数を採用したシステムにより拘束され、又は制御
の必要がないようにするにはどのようにすればよいかを認識することが最も重要
なことであると考えている。

【００６０】

【発明の目的及び概要】

本発明の目的は、傾動可能な物体の追跡及び（又は）制御具合を向上させるこ
とにある。

【００６１】本発明の別の目的は、従来システムよりも必要とする計算の少ない追跡及び制
御システムを提供することにある。

【００６２】本発明の別の目的は、外部基準点を１つだけ利用するシステムの４元数処理法
を提供することにある。

【００６３】本発明の更に別の目的は、物体の向きについての信頼度の高い高帯域幅追跡を
可能にすることにある。

【００６４】本発明の一特徴は、チルト検出手段及び角速度検出手段を有する傾動可能な物
体の姿勢を推定する方法にある。この方法は、角速度検出手段から角速度情報を
出力する段階と、出力された角速度情報を変換して統合し、それにより第１の４
元数位置情報を生じさせて第１の４元数位置情報が地球座標系において水平軸回
りの回転を表すよう拘束されるようにする段階と、チルト検出手段からチルト情
報を出力する段階と、出力されたチルト情報を処理して第２の４元数位置情報を
生じさせ、第２の４元数位置情報が地球座標系において水平軸回りの回転を表す
よう拘束されるようにする段階と、第１の４元数位置情報を第２の４元数位置情
報と比較して誤差情報を生じさせる段階と、誤差情報を利用して角速度検出手段
のドリフトを補償する段階とを有する。

【００６５】本発明の別の特徴は、チルト検出手段及び角速度検出手段を有する傾動可能な
物体の姿勢を推定する方法にある。この方法は、角速度検出手段から角速度情報
を出力する段階と、出力された角速度情報から、スカラー量及び２成分ベクトル
の形態をした修正状態の４元数速度情報を生じさせる段階と、修正４元数速度情
報を統合して第１の修正４元数姿勢情報を生じさせる段階とを有し、修正４元数
速度情報を生じさせる段階は、第１の修正４元数姿勢情報がｅ＝ｅ_０＋ｅ_１ｉ＋
ｅ_２ｊの形態のものであるように拘束されており、上式において、ｉ，ｊは、互
いに且つ第３の虚数単位ベクトルｋについて以下の関係を満足する虚数単位ベク
トルであり、ｉ^２＝ｊ^２＝ｋ^２＝ｉｊｋ＝−１，ｉｊ＝−ｊｉ＝ｋ，ｊｋ＝−ｋｊ＝ｉ，ｋｉ＝−ｉｋ＝ｊ；上記方法は、チルト検出手段からチルト情報を出力する段階と、出力されたチル
ト情報から、形態ｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊである第２の修正４元数位置情報を
生成する段階と、第１の修正４元数位置情報を第２の修正４元数位置情報と比較
して修正４元数位置誤差情報を生成する段階と、修正４元数位置誤差情報を角速
度誤差信号に変換する段階と、角速度誤差信号を利用して出力された角速度情報
の誤差を補償する段階とを更に有する。

【００６６】本発明に従って提供される方法では、純流体センサ等から出力されるチルトに
関する情報が、高帯域幅角速度センサ、例えば、１組のジャイロ中のドリフト又
は他のずれを補償するのに用いられる。チルトに関する基準データを通じて得ら
れる誤差の補償により、補正された高帯域幅角速度データを積分してずれ誤差を
累積しないリアルタイムの姿勢追跡推定値を得ることができる。方角の角速度情
報（ヨーレート情報）を破棄すると、変形（３成分）４元数表示が得られ、これ
は、チルトに関する情報から導き出され、単一の重力基準点に基づく類似の変形
４元数速度情報と比較される。誤差に関する補償及びフィルタリングは、４元数
表記法を用いて行われ、計算上の効率が高くなる。

【００６７】本発明の上記特徴及び利点並びに他の特徴及び利点は、以下の説明及び図面に
記載されていると共に（或いは）これらから自明である。

【００６８】

【好ましい実施形態の詳細な説明】

図１は、本発明を適用できる一般化された傾動可能な物体１０の概略側面図で
ある。

【００６９】図２は、この物体１０の平面図である。物体１０は、水平状態からの傾きを追
跡し、表示すると共に（或いは）制御しようとする物体であれば任意のものであ
ってよい。物体１０の例としては、ロボット、傾動可能なモータ制御式車椅子、
沖合掘削用プラットフォーム、海上船舶又はボート或いは潜水艦、傾動可能な鉄
道機関車又は電車、遊園地で乗る乗客エンクロージャ又は乗り物、飛行シミュレ
ータ、又は化学反応又は他の製造法で用いられる物質を収容し選択的に注ぎ出す
のに用いられる容器が挙げられる。物体１０は、自動車であってもよい。物体１
０と関連して制御システム１２が設けられ、この制御システムは、１以上のチル
ト検出装置１４及び１以上の角速度検出装置１６からの入力を受け取る。チルト
検出装置１４は、従来型純流体チルトセンサ及び（又は）加速度計であるのがよ
い。１以上のチルトセンサが設けられることが意図されている。角速度センサ１
６を、従来型ジャイロ又は他の公知の角速度検出装置によって構成するのがよい
。１以上の角速度センサが設けられることが意図されている。

【００７０】説明を明確にするための座標系を、図１及び図２示すように定めるのがよい。
水平前向きの方向は、矢印２０によって示されるような正のｘ軸であると考えら
れる。正のｙ軸は、矢印２２で示すように右に向いていて且つｘ軸に垂直な水平
方向であると考えられる（図２）。正のｚ軸は、矢印２４で示すように垂直方向
に真っ直ぐ下に向いていると考えられる（図１）。チルトは、垂直からの角度の
ずれと定義され、±１８０゜に制限された範囲のものである。チルト又は傾斜の
方向は、ｘ軸及びｙ軸で表される。ｘ軸回りの回転は、ロール又は横揺れである
と考えられ、正のロールは、右側へ傾いている。ピッチ又は縦揺れは、ｙ軸回り
の回転として定義され、正のピッチは、後方へのチルトである。ヨーは、ｚ（垂
直）軸回りの回転として定義され、正のヨーは、右側への回転として定義される
。これらの定義は右手の法則に準拠していることは注目されよう。ピッチ及びロ
ールに関する上記定義は、オイラーのピッチ及びロールについての定義と類似し
ているが、異なる点は、オイラー角度が、特定の順序で適用され、中間座標系を
生じさせることにある。本明細書で説明するチルトに基づくシステムでは、垂直
軸回りの回転は存在せず、或いは無視できると仮定する。例えば、物体１０の方
角は、固定されてもよく、或いは、チルト追跡及び制御には無関係であってもよ
く、或いは、オペレータとしての人間による制御を受けてもよく、或いは、チル
トに関する情報を考慮に入れない又は考慮に入れる必要のないシステムによって
制御されるものであってもよい。

【００７１】図３は、図１に示す制御システム１２のコンポーネントをブロック図の形態で
示している図である。制御システム１２は、インタフェース４０を有し、このイ
ンタフェースを介して制御システム１２はチルトセンサ１４及びジャイロ１６か
ら出力された信号を受け取る。これら信号は本発明に従って、姿勢／回転推定ブ
ロック４２によって処理される。ジャイロ及びセンサから出力された信号に基づ
いて、ブロック４２は、以下に説明する手法で物体の姿勢及び（又は）回転角速
度の推定結果を生じさせる。ブロック４２によって得られた姿勢及び（又は）回
転情報に基づいて、ブロック４４は、アクチュエータ４６を制御して物体１０の
姿勢を制御するための制御信号を生じさせる。アクチュエータとしては、モータ
、ソレノイド、可浸チャンバ、或いは上述の種類の物体の姿勢を制御するのに用
いられる従来型の機械式、電気機械式、油圧式又は空気圧式装置が挙げられる。
ブロック４２，４４の少なくとも一部を実行するための電子ハードウェアを、１
以上の従来型マイクロプロセッサで構成してもよい。加うるに、制御信号発生ブ
ロック４４は好ましくは、アクチュエータ４６のための適当な駆動信号を生じさ
せる駆動回路を更に有する。

【００７２】制御システム１２は、物体１０に搭載されるものとして図１に示されているが
、制御システム１２の少なくとも或る部分を物理的に物体１０から分離してもよ
いことは理解されるべきである。例えば、ブロック４２，４４のうち幾分か又は
全てを実行する処理回路は、物体１０から見て遠隔に位置して遠隔測定法により
チルトセンサ１４及びジャイロ１６からの出力を受け取り、制御信号を適当なワ
イヤレス通信チャンネルによって物体１０に送り戻すことができる。

【００７３】姿勢及び回転の推定姿勢／回転推定ブロック４２の作用について以下に説明する。

【００７４】チルトセンサ、例えば、従来型純流体センサは、比較的ドリフトの無いもので
あるのがよく、したがって、重力ベクトルについて信頼性の高い指示を与えるこ
とができる。かくして、静的システムの姿勢をかかるセンサを用いて重力に対し
て判定することができる。しかしながら、動的システムでは、この種のセンサは
、角加速度及び振動加速度の影響を受けるので、これらを低域フィルタにかけて
重力以外の加速度の影響を減衰させる必要がある。その結果得られたデータは、
フィルタの帯域幅によって決定される有限期間にわたり、重力の平均方向を提供
する。しかしながら、応答時間が早いことを必要とするシステムでは、重力セン
サだけでは適当ではない。

【００７５】他方、ジャイロ又は他の角速度センサは、加速度による悪影響を受けることな
く、高帯域幅及び高速応答をもたらすことができる。かかるセンサによって検出
された角速度は、システムの姿勢を直接示すものではないが、角加速度に関する
データを統合すると位置に関する情報を得ることができる。しかしながら、速度
センサは、統合の際に相当大きな誤差を生じさせる場合のあるドリフトの影響を
受けやすい。ブロック４２によって行われる推定法は、ジャイロ１６及びチルト
センサ１４によりそれぞれ提供される高帯域幅情報と低帯域幅情報を組み合わせ
、モデルベース（モデルを利用した）推定技術を用いて正確な位置及び回転デー
タを生じさせる。

【００７６】モデルベース推定器では、真の位置の推定値を、センサデータとシステムダイ
ナミックスの内部モデルを比較することにより生じさせる。測定データと予想デ
ータの誤差を用いて推定値を連続的に精緻なものにする。この誤差が推定値に影
響を及ぼす度合いは、適当な状態誤差を推定器に送り戻す重み付けされた行列Ｈ
によって求められる。かかる推定器に関する状態変数式が以下に示されている。

【００７７】

【数２７】上式において、ｘ＝システム状態ベクトル、ｘ_Ｅ＝推定状態ベクトル、ｙ＝シス
テム出力ベクトル、ｙ_Ｅ＝推定出力ベクトル、ｕ＝システム／推定器入力ベクト
ル、Ａ＝システム行列、Ｂ＝入力行列、Ｃ＝出力行列、Ｄ＝入力／出力行列、Ｈ
＝フィードバック行列である。

【００７８】添え字“ｅ”は、推定パラメータを示していることに注目されたい。内部シス
テムモデルは、行列（Ａ，Ｂ，Ｃ，Ｄ）によって特定される。フィードバック行
列Ｈにより、推定器の安定性が決まる。安定推定器は、時間の経過につれて実際
の状態と推定状態が等しくなる（ｘ_ｅ＝ｘ）である平衡状態に達することになろ
う。内部モデルは、簡単な積分器を利用している場合が多い。かかる単純モデル
の有効性は、積分と微分が物理的現象の基礎をなしているということに関連して
いる。例えば、速度は、位置の時間的変化率又は導関数であり、即ち、ｖ＝ｄｘ
／ｄｔである。したがって、パラメータに関し、Ａ＝０，Ｂ＝１，Ｃ＝１，Ｄ＝
０、フィードバック利得Ｈ＝ｋ_１の簡単な１次積分器が与えられると、システム
の測定された静的位置θ_０と測定された動的位置θを組み合わせて推定値θ_ｅが
周波数範囲全体にわたって正確であるようにする推定器を得ることが可能である
。事実、推定値は、２つの測定値の周波数依存性重み付けされた和である。この
結果のラプラス変換を以下に示す。

【００７９】

【数２８】

【００８０】低周波数（ｓ＝０）では、推定位置は低帯域幅位置測定値θ_０に近づき、高周
波数では、推定位置は高い帯域幅位置測定値θに近づくことは注目されたい。か
くして、高帯域幅データと低帯域幅データが組み合わされる。

【００８１】推定器が簡単な２次積分器を利用していればより有用な推定値を得ることがで
きる。この場合、Ｈ＝［ｋ_１ｋ_２］^Ｔという形式のフィードバック行列を用い
ると出力行列Ｃ＝［１０］により低帯域幅測定値θ_０をフィードバックするこ
とができる。その結果得られた位置及び速度の推定値のラプラス変換は以下に示
されている。

【００８２】

【数２９】

【００８３】上式において、ω（ｓ）は、ｓ−領域においてｄθ／ｄｔの測定値である。した
がって、この推定技術は、低帯域幅位置θ_０と高帯域幅速度ωを組み合わせるも
のである。低周波数では、推定位置は、上述したように低帯域幅測定値θ_０に近
づく。高周波数では、推定位置は、高帯域幅速度推定値ωの積分に近づき、この
高帯域幅速度推定値は、高帯域幅位置θと等価である。これに類似した結果が、
推定速度について得られる。かくして、この種の推定器は、低帯域幅チルトデー
タ及び高帯域幅角速度データから角度位置及び角速度の推定値を得るのに好適で
ある。利得ｋ_１，ｋ_２は、適当なフィルタ帯域幅及び安定性が得られるように選
択される。式（３４）中においてｋ_２＝０且つω（ｓ）＝ｓθ（ｓ）とすれば、
式（３３）に示す同一の１次位置推定値が得られることは注目されるべきである
。

【００８４】４元数を用いると任意の角度運動を通じてシステムの姿勢を追跡できることが
できることは知られている。さらに、４元数は、計算上効果的な表記法であると
共にオイラー角度の受ける特異性には影響されない。また、高帯域幅センサ及び
低帯域幅センサからのデータを、推定器を用いて組み合わせると、全帯域幅にわ
たって正確なデータが得られることも又知られている。しかしながら、４元数は
、直接的な物理的測定には向いておらず、これにより、チルトセンサデータから
の絶対４元数基準のずれが複雑になる。かかる基準が無ければ、推定器は、ジャ
イロドリフトが積分された４元数を改竄し、最終的にシステムがその角度位置の
追跡を失うようにするのを防止することが不可能になる。

【００８５】上述の説明から、従来の４元数は、一義的に回転を表すには２つの回転ベクト
ル及び２つの外部基準点を必要とすることが思い起こされよう。しかしながら、
チルトセンサだけでは、地球の中心に対する単一の基準だけが利用できるに過ぎ
ない。本発明は、４つの従来要素のうち０でないのは３つに過ぎず、４元数形式
において一義的に姿勢（tilt）を表すのに基準を１つしか必要としない変形４元
数表記法を提供することにより、チルト利用システムに関するこの問題を解決す
る。この解決策の前提条件は、システムがその方角についての知識を必要としな
いのでヨー（垂直軸回りの回転）の変化を追跡する必要はないということにある
。

【００８６】上述の展開に基づき、単一の基準点からの４元数係数を計算するには、各座標
系中の基準ベクトルが回転軸線に垂直であることが必要である。重力をその基準
とするチルト利用システムの場合、これは、回転軸線が常時水平のままであると
いうことが必要であることに等しい。回転軸線ｎが常に水平であれば、この回転
軸のｚ成分は同じ０でなければならない。単位４元数成分の定義を参照すると、
これは、４元数のｑ_３成分も同じく０でなければならないことを意味している。
したがって、条件ｑ_３＝０は拘束条件となる。かくして、結果的に得られる４元
数は、変形されていて、有意の成分を３つしか含んでいない。

【００８７】また、拘束条件ｑ_３＝０は、４元数速度式（式（２８）又は式（２９））を積
分している間、満足されなければならない。この条件は、条件ｄｑ_３／ｄｔ＝０
も又保たれている限り、積分操作中維持できる。式（２８）及び式（２９）から
、これは、以下の条件のうちの１つが成立しなければならないということを意味
している。

【００８８】

【数３０】

【００８９】

【数３１】

【００９０】これら拘束条件のうちの１つを満足させるため、少なくとも１つのパラメータ
が所望に応じて任意に変化することが必要である。物体座標系では、３つの角速
度成分及び３つの変形４元数成分は全て必要な情報を含んでいる。しかしながら
、地球座標系では、角速度のヨー成分はシステムがチルトを利用しているものな
ので、重要ではないという前提に立っていた。したがって、真のヨー成分に代え
て上記拘束条件を満足する仮想ヨー成分を用いることができる。実際には、これ
は、測定角速度を物体座標系から地球座標系に変換することによって行われる。
次に、結果的に得られる地球ヨーを以下の拘束式を満足させる仮想地球ヨーで置
き換えることができる。

【００９１】

【数３２】

【００９２】この地球ヨー成分を元の４元数速度式（式（２９））に逆代入するとヨーに対
するその明確な依存性を無くすことができる。変形３要素４元数を他の４元数か
ら識別するために、ここでは、変形４元数を“ｑ”ではなく“ｅ”で示すことに
すると、これら成分を次のように書き表すことができる。

【００９３】

【数３３】

【００９４】上式において、ｎ＝［ｎ_１ｎ_２］^Ｔであり、ｅ_３＝ｎ_３＝０である。

【００９５】 ω_Ｅ３及びｅ_３を除去した後、変形４元数速度式は次のようになる。

【００９６】

【数３４】

【００９７】回転行列Ｒ_ＥＢ（式（３２））も又、以下に示すようにω_Ｅ３及びｅ_３を除去
することにより単純化できる。

【００９８】

【数３５】

【００９９】これらの結果を組み合わせ、ω_Ｅ＝Ｒ_ＥＢωに着目すると、地球座標系に関連
付けられた変形４元数速度を以下に示すように物体座標系中の角速度の関数とし
て書き表すことができる。

【０１００】

【数３６】

【０１０１】これを以下に示すように行列形式でより簡潔に表すことができる。

【０１０２】

【数３７】

【０１０３】この結果を用いると変形４元数速度を積分すると、変形４元数が得られる。し
かしながら、この式（４３）は、理想的な角速度ベクトルωを前提としていて、
ジャイロからの実際の角速度データのドリフトを補償することはできない。一般
に、ドリフトについて補正を行うには、ずれをヨージャイロデータから差し引か
なければならない。次に、上述の式中のωに代えてω’＝ω−ω_{ｄｒｉｆｔ}の形
式の補正角速度が用いられる。ジャイロドリフトが時間の関数であるということ
により事態は一層複雑になる。したがって、ドリフト補正項を連続的にアップデ
ートしなければならない。これには、上述の２次推定技術の利用が必要である。

【０１０４】推定器の目的は、４元数速度の積分により得られた４元数をチルトデータから
得られた４元数と比較することにある。定常偏差がジャイロのドリフトだけに起
因しているものと仮定すると、この偏差又は誤差の大きさを用いるとジャイロド
リフト補正項を加減することができる。推定器利得は、定常偏差が時間の経過に
つれて０になるように選択される。推定器利得値の選択は、当業者の通常の知識
の範囲内にあり、したがって、これ以上の説明は不要であろう。

【０１０５】補正項を正しく調整するために、４元数誤差を変換して角速度誤差に戻す必要
がある。これは、以下に示すような地球座標系中の角速度について解かれた元の
４元数速度式を用いて達成される。

【０１０６】

【数３８】

【０１０７】この式を時間に関して積分すると、以下の結果が得られる。

【０１０８】

【数３９】

【０１０９】したがって、上記の式を用いると、推定４元数とチルト４元数の誤差（δｑ＝
ｑ’−ｑ）をチルト角度誤差Δθ_Ｅに変換することができる。この場合、ｑとＱ
の両方においてｑ_３＝０である変形４元数を用いていることが思い起こされる。
しかしながら、このようにして得られる地球ヨー成分は、４元数を仮想地球ヨー
を用いて生じさせたので有効ではない。有効な角度誤差を得るためには、元の地
球ヨーを誤差成分Δθ_Ｅ３に代えて代入する必要がある。地球ヨーについての表
現をω_Ｅ＝Ｒ_ＥＢωからｑ_３＝０として導き出すことができ、以下のように与え
られる。

【０１１０】

【数４０】

【０１１１】代入後、結果的に得られる地球座標系中の角度誤差ベクトルは、Δθ_Ｅ’＝［
Δθ_Ｅ１ Δθ_Ｅ２ ω_Ｅ３］^Ｔによって与えられる。ジャイロは物体座標系中
に位置しているので、地球座標系中の角度誤差をΔθ＝Ｒ_ＢＥΔθ_Ｅ’を用いて
物体座標系に変換しなければならない。次に、結果的に得られる角度誤差を推定
器によって用いると、ジャイロドリフト補正項を得ることができる。

【０１１２】次に、チルトデータを変形４元数に変換する方法について説明する。４元数成
分を得るには、回転軸に垂直であって、θ／２の角度だけ離れた２つの単位ベク
トルの内積と外積を作るのがよいことを示した。かかる２つのベクトルを地球の
中心（即ち、チルトセンサによって測定される重力の方向）のところに外部基準
点を用いることにより作ることができる。初期重力ベクトルをＧ_ｉとし、最終重
力ベクトルをＧ_ｆで示すものとする。（初期重力ベクトルＧ_ｉに相当するデータ
を初期化中に得ることができ、次の使用のために保存する。「最終」重力ベクト
ルＧ_ｆは、推定器が動作しているときにアップデートされることになろう。）

【０１１３】これら２つのベクトルを以下に示すように正規化する。

【０１１４】

【数４１】

【０１１５】これら正規化された重力ベクトルを用いて、所望の単位ベクトルを以下のよう
に作ることができる。

【０１１６】

【数４２】

【０１１７】最終的に、４元数成分を以下に示すように内積及び外積の計算により生じさせ
ることができる。

【０１１８】

【数４３】

【０１１９】作図により、ＡとＢの外積は、各座標系中の重力ベクトルに垂直なベクトルを
作る。その結果として、このベクトルも又水平でなければならない。これは、４
元数のｚ成分が所望通り同じ０であるということを意味している。したがって、
４元数のうちの最初の３つの成分だけが微々たるものであるという訳ではなく、
変形４元数の形式を満足させる。

【０１２０】上述の推定法の全体が図４のブロック図に示されている。

【０１２１】推定技術は、２つのループ、即ちｋ_１ループ及びｋ_２ループに分割される。ｋ _１ループは、チルトセンサからのデータに低帯域幅フィルタをかけ、推定４元数
速度を調整するのに用いられる誤差の大きさを決定する。ｋ_２ループは、ジャイ
ロデータについてドリフト補正を行う。ジャイロは物体座標系中に位置している
ので、ｋ_２ループは座標変換の物体側に位置していなければならない。ｋ_２利得
は、ジャイロデータ中のドリフトを補正するのに用いられる誤差の大きさを決定
する。次に、重み付けされた誤差を積算してループが時間の経過につれて０の定
常誤差を達成するようにする。換言すると、４元数誤差が０になると、ｋ_２積分
器の出力は変化を停止し、ドリフト補正項は一定のままになる。

【０１２２】図４において、ｋ_２ループ中の加算ブロック５０は、重み付けされた誤差信号
（これは、ドリフト補正信号であると考えることもできる）をジャイロ１６から
出力された角速度情報に適用し、それにより補正された角速度情報が得られる。

【０１２３】ブロック５２において、補正された角速度情報を地球座標系に変換して、変形
４元数に変換する。ブロック５２中の標識Ｅ／２によって示されているように、
このブロックに適用される計算は、上述の式（４３）及び式（４２）に一致する
。結果的に得られた推定速度情報（これは、上述の変形４元数形態をなしている
）は、加算ブロック５４への入力として提供される。ブロック５４では、ｋ_１ル
ープによって得られた補正を推定４元数速度情報に適用しそれにより推定域の変
化を表すデータが得られる。このデータ（これは、推定位置差信号ということが
できる）をブロック５６で積算すると変形４元数推定位置データが得られる。ブ
ロック５８では、ブロック５６から出力された変形４元数推定位置データ及びブ
ロック５２から出力された推定変形４元数速度情報が、オイラー角度に変換され
て制御信号発生ブロック４４（図３）に出力される。この変換は、以下の数式を
用いると容易に達成される。（以下の式は三角関数を用いているが、計算上の複
雑さは、最小限に抑えられているのでルックアップテーブルを用いて容易に実行
することができる。）

【０１２４】

【数４４】

【０１２５】また、チルト利用システムの運動範囲が制限されている場合、小角度近似法を
用いてこれら数式を単純化できる。

【０１２６】制御信号活性器４４は、オイラー信号ではなく４元数信号を出力するタイプの
ものであれば、ブロック５８を省いてもよく、更に推定４元数位置及び速度に関
する情報（本発明に従って得られる変形形態の状態にある）を直接制御信号発生
器に送ってもよい。

【０１２７】何れの場合においても、変形４元数形態の推定位置情報は、加算ブロック６０
に入力として与えられ、この加算ブロックにおいて、これはブロック６２のとこ
ろで変形４元数形態に変換されたチルトセンサ１４から出力された現在のチルト
情報と比較される。（ブロック６２で得られる信号は、変形４元数基準位置信号
と呼ぶことができる。）ブロック６２のところで行われる変換は、式（４６）乃
至式（４８）と関連して、上述した手順に一致している。加算ブロックから出力
された結果としての誤差信号をブロック６４のところで利得ファクタｋ_１によっ
て重み付けし、次に、上述の補正信号としてブロック５４に送る。ブロック６０
からの出力（これは、誤差位置信号と考えることができる）も又、ブロック６６
への入力として得られる。式（４４）に従って、ブロック６６は、推定４元数と
チルト４元数の誤差をチルト角度誤差に変換する。ブロック６６から出力される
結果としての信号の無効ヨー成分を地球座標系ヨー成分で置き換え、この地球座
標系ヨー成分は、ブロック５２の処理から得ることができ、結果的に得られる角
度誤差ベクトルをブロック６８のところで物体座標系に変換する。次に、ブロッ
ク６８の出力をブロック７０のところで積分してｋ_２利得で重み付けし、結果的
に得られる信号を上述のドリフト補正信号として加算ブロック５０に送る。

【０１２８】ブロック５２，６６，６８における処理に必要な係数は、図４の符号７２のと
ころに示すように積分ブロック５６から出力された変形４元数位置情報から得ら
れることは注目されるべきである。

【０１２９】当業者には理解されるように、図４にブロック図の形態で示された方法は、１
以上の適当にプログラムされたコンピュータ処理装置（例えば、１以上のマイク
ロプロセッサ）によって有利に実施でき、かかるコンピュータ処理装置は、制御
システム１２の一部をなしている。

【０１３０】本発明に従って得られる姿勢及び回転推定器では、回転センサによって得られ
る高帯域幅情報及びチルトセンサからの低帯域幅情報は、従来の４つの要素では
なく３つの要素を持つ新規な変形４元数表記法で組み合わされる。結果的に得ら
れる推定４元数は、設計対象の帯域幅全体にわたって有効であり、時間の経過に
つれてドリフトすることはない。実行されるべきコンピュータ処理を、単純な算
術、例えば、乗算、加算及び平方根を用いて効率的に取り扱うことができる。変
形４元数推定器は、傾動状態が追跡されると共に（或いは）制御されるべき広範
なシステムに適用できる。

【０１３１】本発明の上記説明は、例示であって、本発明を限定するものではない。当業者
であれば、上述の実施形態の種々の設計変更又は改造を行うことができ、これら
は本発明の精神及び範囲から逸脱することなく想到できる。

【図面の簡単な説明】

【図１】本発明が適用される傾動可能な物体の概略側面図である。

【図２】図１の傾動可能な物体の平面図である。

【図３】図１の傾動可能な物体の制御システムのブロック図である。

【図４】図３の制御システムの本発明に従って得られる姿勢及び回転推定部分のブロッ
ク図である。

───────────────────────────────────────────────────── フロントページの続き (81)指定国ＥＰ(ＡＴ，ＢＥ，ＣＨ，ＣＹ，ＤＥ，ＤＫ，ＥＳ，ＦＩ，ＦＲ，ＧＢ，ＧＲ，ＩＥ，ＩＴ，ＬＵ，ＭＣ，ＮＬ，ＰＴ，ＳＥ，ＴＲ)，ＯＡ(ＢＦ，ＢＪ，ＣＦ，ＣＧ，ＣＩ，ＣＭ，ＧＡ，ＧＮ，ＧＷ，ＭＬ，ＭＲ，ＮＥ，ＳＮ，ＴＤ，ＴＧ)，ＡＰ(ＧＨ，ＧＭ，ＫＥ，ＬＳ，ＭＷ，ＭＺ，ＳＤ，ＳＬ，ＳＺ，ＴＺ，ＵＧ，ＺＷ)，ＥＡ(ＡＭ，ＡＺ，ＢＹ，ＫＧ，ＫＺ，ＭＤ，ＲＵ，ＴＪ，ＴＭ)，ＡＥ，ＡＧ，ＡＬ，ＡＭ，ＡＴ，ＡＵ，ＡＺ，ＢＡ，ＢＢ，ＢＧ，ＢＲ，ＢＹ，ＢＺ，ＣＡ，ＣＨ，ＣＮ，ＣＲ，ＣＵ，ＣＺ，ＤＥ，ＤＫ，ＤＭ，ＤＺ，ＥＥ，ＥＳ，ＦＩ，ＧＢ，ＧＤ，ＧＥ，ＧＨ，ＧＭ，ＨＲ，ＨＵ，ＩＤ，ＩＬ，ＩＮ，ＩＳ，ＪＰ，ＫＥ，ＫＧ，ＫＰ，ＫＲ，ＫＺ，ＬＣ，ＬＫ，ＬＲ，ＬＳ，ＬＴ，ＬＵ，ＬＶ，ＭＡ，ＭＤ，ＭＧ，ＭＫ，ＭＮ，ＭＷ，ＭＸ，ＭＺ，ＮＯ，ＮＺ，ＰＬ，ＰＴ，ＲＯ，ＲＵ，ＳＤ，ＳＥ，ＳＧ，ＳＩ，ＳＫ，ＳＬ，ＴＪ，ＴＭ，ＴＲ，ＴＴ，ＴＺ，ＵＡ，ＵＧ，ＵＺ，ＶＮ，ＹＵ，ＺＡ，ＺＷ (72)発明者ロー・ジェフリー・ディーアメリカ合衆国、35803 アラバマ州、ハンツビル、ハミングバード・ドライブ 701 Ｆターム(参考） 2F029 AA01 AB03 AC05 AC12 AD02 AD08

Claims

【特許請求の範囲】

【請求項１】チルト検出手段及び角速度検出手段を有する傾動可能な物体
の姿勢を推定する方法であって、前記角速度検出手段から角速度情報を出力する
段階と、前記出力された角速度情報を変換して統合し、それにより第１の４元数
位置情報を生じさせて前記第１の４元数位置情報が地球座標系において水平軸回
りの回転を表すよう拘束されるようにする段階と、前記チルト検出手段からチル
ト情報を出力する段階と、前記出力されたチルト情報を処理して第２の４元数位
置情報を生じさせ、前記第２の４元数位置情報が前記地球座標系において水平軸
回りの回転を表すよう拘束されるようにする段階と、前記第１の４元数位置情報
を前記第２の４元数位置情報と比較して誤差情報を生じさせる段階と、前記誤差
情報を利用して前記角速度検出手段のドリフトを補償する段階とを有することを
特徴とする方法。
【請求項２】チルト検出手段及び角速度検出手段を有する傾動可能な物体
の姿勢を推定する方法であって、前記角速度検出手段から角速度情報を出力する
段階と、前記出力された角速度情報から、スカラー量及び２成分ベクトルの形態
をした修正状態の４元数速度情報を生じさせる段階と、前記修正４元数速度情報
を統合して第１の修正４元数姿勢情報を生じさせる段階とを有し、前記修正４元
数速度情報を生じさせる前記段階は、前記第１の修正４元数姿勢情報がｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊの形態のものであるように拘束されており、上式において、ｉ
，ｊは、互いに且つ第３の虚数単位ベクトルｋについて以下の関係を満足する虚
数単位ベクトルであり、ｉ^２＝ｊ^２＝ｋ^２＝ｉｊｋ＝−１，ｉｊ＝−ｊｉ＝ｋ，ｊｋ＝−ｋｊ＝ｉ，ｋｉ＝−ｉｋ＝ｊ；前記方法は、前記チルト検出手段からチルト情報を出力する段階と、前記出力さ
れたチルト情報から、前記形態ｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊである第２の修正４元
数位置情報を生成する段階と、前記第１の修正４元数位置情報を前記第２の修正
４元数位置情報と比較して修正４元数位置誤差情報を生成する段階と、前記修正
４元数位置誤差情報を角速度誤差信号に変換する段階と、前記角速度誤差信号を
利用して前記出力された角速度情報の誤差を補償する段階とを更に有することを
特徴とする方法。
【請求項３】前記第２の修正４元数位置情報に低域フィルタをかける段階
を更に有していることを特徴とする請求項２記載の方法。
【請求項４】前記角速度情報は、第１の座標系で提供され、前記チルト情
報は、前記第１の座標系とは異なる第２の座標系で生成され、前記修正４元数速
度情報を生じさせる前記段階は、前記出力された角速度情報を前記第１の座標系
から前記第２の座標系に変換する段階を含み、前記方法は、前記角速度誤差信号
を前記利用段階の実施前に前記第２の座標系から前記第１の座標系に変換する段
階を更に有していることを特徴とする請求項２記載の方法。
【請求項５】前記第２の修正４元数位置情報は、前記チルト検出手段から
出力され、第１の時機に記憶される第１のチルト情報及び前記チルト検出手段か
ら出力され、前記第１の時機と異なる第２の時機に記録される第２のチルト情報
に基づいて生成されることを特徴とする請求項２記載の方法。
【請求項６】傾動可能な物体の姿勢を推定する方法であって、角速度情報
を物体座標系で生成する段階と、ドリフト補正信号を前記角速度情報に適用して
補正された角速度情報を生じさせる段階と、修正４元数推定位置情報を利用して
前記補正された角速度情報を処理して地球座標系に関連付けられた修正４元数推
定速度情報を生じさせる段階とを有し、前記修正４元数推定速度信号は、スカラ
ー成分と２つのベクトル成分とから成り、前記方法は、前記修正４元数推定速度
情報を重み付けされた位置誤差信号に基づいて調整して推定位置差信号を生じさ
せる段階と、前記推定位置差信号を統合し、それによりスカラー成分と２つのベ
クトル成分とから成る前記修正４元数推定位置情報を生じさせる段階と、チルト
情報を前記地球座標系で生成する段階と、前記チルト情報を利用してスカラー成
分と２つのベクトル成分とから成る修正４元数基準位置情報を生成する段階と、
前記修正４元数基準位置情報を前記修正４元数推定位置情報から差し引いて誤差
位置信号を生じさせる段階と、前記誤差位置信号を重み付けして前記重み付けさ
れた位置誤差信号を生じさせる段階と、前記修正４元数推定位置情報を利用して
前記誤差位置信号をチルト各誤差信号に変換する段階と、角度誤差ベクトルを前
記チルト角度信号のピッチ成分及びロール成分並びに前記修正４元数推定位置情
報を用いて前記補正角速度情報を前記地球座標系に変換することによって生じた
角速度信号のヨー成分から作る段階と、前記修正４元数推定位置情報を利用して
前記角度誤差ベクトルを前記地球座標系から前記物体座標系に変換する段階と、
重み付けされた積分を前記変換された角度誤差ベクトルに適用して前記ドリフト
補正信号を生じさせる段階とを更に有することを特徴とする方法。
【請求項７】前記修正４元数推定速度情報及び前記修正４元数推定位置情
報を変換してオイラー角度出力情報を生成する段階と、前記オイラー角度出力情
報を制御信号発生手段に出力する段階とを更に有することを特徴とする請求項６
記載の方法。
【請求項８】傾動可能な物体の姿勢を推定する装置であって、前記物体に
取り付けられていて、チルト情報を出力するチルト検出手段と、前記物体に取り
付けられていて、角速度情報を出力する角速度検出手段と、前記角速度検出手段
から出力された前記角速度情報から、スカラー量及び２成分ベクトルの形態をし
た修正状態の４元数速度情報を生じさせる手段と、前記修正４元数速度情報を統
合して第１の修正４元数姿勢情報を生じさせる手段とを有し、前記修正４元数速
度情報を生じさせる前記手段は、前記第１の修正４元数姿勢情報がｅ＝ｅ_０＋ｅ _１ｉ＋ｅ_２ｊの形態のものであるように拘束されており、上式において、ｉ，ｊ
は、互いに且つ第３の虚数単位ベクトルｋについて以下の関係を満足する虚数単
位ベクトルであり、ｉ^２＝ｊ^２＝ｋ^２＝ｉｊｋ＝−１，ｉｊ＝−ｊｉ＝ｋ，ｊｋ＝−ｋｊ＝ｉ，ｋｉ＝−ｉｋ＝ｊ；前記装置は、前記チルト検出手段から出力された前記チルト情報から、前記形態
ｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊである第２の修正４元数位置情報を生成する手段と、
前記第１の修正４元数位置情報を前記第２の修正４元数位置情報と比較して修正
４元数位置誤差情報を生成する手段と、前記修正４元数位置誤差情報を角速度誤
差信号に変換する手段と、前記角速度誤差信号に基づいて前記角速度検出手段か
ら出力された前記角速度情報の誤差を補償する手段とを更に有していることを特
徴とする装置。
【請求項９】前記チルト検出手段は、少なくとも１つの純流体チルトセン
サを有することを特徴とする請求項８記載の装置。
【請求項１０】前記角速度検出手段は、少なくとも１つのジャイロを有す
ることを特徴とする請求項９記載の装置。
【請求項１１】前記修正４元数速度情報をオイラー角度速度情報に変換し
、前記第１の修正４元数姿勢情報をオイラー角度位置情報に変換する手段を更に
有していることを特徴とする請求項８記載の装置。