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

Description

【０００１】
本発明は、傾動可能な物体の追跡及び制御に関する。
【０００２】
４元数を用いて物品の空間内での方位を表すことが知られている。４元数表記法は一般に、広く採用されているオイラーデータ表示法よりも計算上の利用効率が高い。さらに、４元数表記法は、オイラー表記法を用いた場合に生じる場合のある特異性の影響を受けない。次の米国特許、即ち、米国特許第５，８７５，９９３号、同第５，２１２，４８０号、同第４，７９７，８３６号、同第４，７４２，３５６号及び同第４，７３７，７９４号は、４元数を用いて空間中の物体の方位を制御し、決定し且つ（或いは）表示する技術を開示している。
【０００３】
４元数の概略的説明
４元数は、１８４３年にサー・ウィリアム・ローアン・ハミルトンによって最初に案出された４つの要素（元）の多元数である。４元数は、スカラー部分と複素数ベクトル部分とから成っている。ベクトル部分は、３つの互いに直交する複素数の単位ベクトルｉ，ｊ，ｋにより割り当てられた方向である３つの実数成分の順序付けられた３つ組（ベクトル）から成っている。一般的な４元数Ｑの一例を以下に示す。
【０００４】
【数１】

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

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

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

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

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

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

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

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

【００２３】
【数１０】

（なお、上記についてはＥ．カルタン著『スピノルの理論』を参照のこと）。
【００２４】
パウリのスピン行列及び単位４元数係数の定義を用いると、単位４元数を以下のように書き表すことができる。
【００２５】
【数１１】

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

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

【００２９】
この４元数の形式と上述した単位複素数の指数関数形式の類似性に着目する。この形式は、ハミルトンが最初に探究した「位相」の３次元変化を示唆している。半角が現れるのは、Ｑが両側変換であるということによって説明がつく。かくして、左辺と右辺の係数、Ｑ、Ｑ^＊は各々、所望の空間位相シフトの半分に寄与する。
【００３０】
４元数についての従来のハミルトン形式ではなくパウリのスピン行列形式を用いるのが一層都合のよい場合が多い。例えば、以下に示すような内積を作ることによりベクトルを行列として表すことができる。
【００３１】
【数１４】

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

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

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

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

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

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

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

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

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

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

【００５３】
変換行列Ｒ_ＥＢは直交行列なので、その転置行列は逆変換Ｒ_ＢＥである。したがって、４元数を用いると任意の角運動にわたり物体の姿勢を追跡することができる。
【００５４】
比較目的のため、オイラー角速度を計算する式を以下に示す。関連の回転行列Ｒも又、オイラー角速度の関数として示される。
【００５５】
【数２５】

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

【００５８】
公知のように、オイラー法は主として三角関数を利用しているので極めて多くの計算を必要とする。加うるに、オイラー速度式は、ピッチ角のセカントで決まり、その結果、ピッチが±９０゜に近づくと、特異性が生じることになる。これとは対照的に、４元数を利用する方法は、三角関数を含んでおらず、乗算及び加算を利用しているに過ぎない。その結果、４元数法は、計算効率が極めて高い。上述したように、物体の姿勢を、物体座標系中に定められた２つのベクトル及び２つの外部基準点が与えられると、４元数の形態で一義的に表すことができる。しかしながら、特に地球上の物体の場合、陸上に位置しているにせよ、或いは水域の表面上又は表面下にあるにせよ何れにせよ、２つの外部基準点を確立することは困難であると共に（或いは）費用がかかる場合がある。
【００５９】
本発明者は、４元数表記法をどのように実用的に変形すれば変形４元数形態を得るのに外部基準点を１つしか必要としないようにすることができるか、そしてかかる変形４元数形態を自由に傾動できる物体に実用的に適用できるが、これについて方角（yaw）が変形４元数を採用したシステムにより拘束され、又は制御の必要がないようにするにはどのようにすればよいかを認識することが最も重要なことであると考えている。
【００６０】
【発明の目的及び概要】
本発明の目的は、傾動可能な物体の追跡及び（又は）制御具合を向上させることにある。
【００６１】
本発明の別の目的は、従来システムよりも必要とする計算の少ない追跡及び制御システムを提供することにある。
【００６２】
本発明の別の目的は、外部基準点を１つだけ利用するシステムの４元数処理法を提供することにある。
【００６３】
本発明の更に別の目的は、物体の向きについての信頼度の高い高帯域幅追跡を可能にすることにある。
【００６４】
本発明の一特徴は、チルト検出手段及び角速度検出手段を有する傾動可能な物体の姿勢を推定する方法にある。この方法は、角速度検出手段から角速度情報を出力する段階と、出力された角速度情報を変換して統合し、それにより第１の４元数位置情報を生じさせて第１の４元数位置情報が地球座標系において水平軸回りの回転を表すよう拘束されるようにする段階と、チルト検出手段からチルト情報を出力する段階と、出力されたチルト情報を処理して第２の４元数位置情報を生じさせ、第２の４元数位置情報が地球座標系において水平軸回りの回転を表すよう拘束されるようにする段階と、第１の４元数位置情報を第２の４元数位置情報と比較して誤差情報を生じさせる段階と、誤差情報を利用して角速度検出手段のドリフトを補償する段階とを有する。
【００６５】
本発明の別の特徴は、チルト検出手段及び角速度検出手段を有する傾動可能な物体の姿勢を推定する方法にある。この方法は、角速度検出手段から角速度情報を出力する段階と、出力された角速度情報から、スカラー量及び２成分ベクトルの形態をした修正状態の４元数速度情報を生じさせる段階と、修正４元数速度情報を統合して第１の修正４元数姿勢情報を生じさせる段階とを有し、修正４元数速度情報を生じさせる段階は、第１の修正４元数姿勢情報がｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊの形態のものであるように拘束されており、上式において、ｉ，ｊは、互いに且つ第３の虚数単位ベクトルｋについて以下の関係を満足する虚数単位ベクトルであり、
ｉ^２＝ｊ^２＝ｋ^２＝ｉｊｋ＝−１， ｉｊ＝−ｊｉ＝ｋ，
ｊｋ＝−ｋｊ＝ｉ， ｋｉ＝−ｉｋ＝ｊ；
上記方法は、チルト検出手段からチルト情報を出力する段階と、出力されたチルト情報から、形態ｅ＝ｅ_０＋ｅ_１ｉ＋ｅ_２ｊである第２の修正４元数位置情報を生成する段階と、第１の修正４元数位置情報を第２の修正４元数位置情報と比較して修正４元数位置誤差情報を生成する段階と、修正４元数位置誤差情報を角速度誤差信号に変換する段階と、角速度誤差信号を利用して出力された角速度情報の誤差を補償する段階とを更に有する。
【００６６】
本発明に従って提供される方法では、純流体センサ等から出力されるチルトに関する情報が、高帯域幅角速度センサ、例えば、１組のジャイロ中のドリフト又は他のずれを補償するのに用いられる。チルトに関する基準データを通じて得られる誤差の補償により、補正された高帯域幅角速度データを積分してずれ誤差を累積しないリアルタイムの姿勢追跡推定値を得ることができる。方角の角速度情報（ヨーレート情報）を破棄すると、変形（３成分）４元数表示が得られ、これは、チルトに関する情報から導き出され、単一の重力基準点に基づく類似の変形４元数速度情報と比較される。誤差に関する補償及びフィルタリングは、４元数表記法を用いて行われ、計算上の効率が高くなる。
【００６７】
本発明の上記特徴及び利点並びに他の特徴及び利点は、以下の説明及び図面に記載されていると共に（或いは）これらから自明である。
【００６８】
【好ましい実施形態の詳細な説明】
図１は、本発明を適用できる一般化された傾動可能な物体１０の概略側面図である。
【００６９】
図２は、この物体１０の平面図である。物体１０は、水平状態からの傾きを追跡し、表示すると共に（或いは）制御しようとする物体であれば任意のものであってよい。物体１０の例としては、ロボット、傾動可能なモータ制御式車椅子、沖合掘削用プラットフォーム、海上船舶又はボート或いは潜水艦、傾動可能な鉄道機関車又は電車、遊園地で乗る乗客エンクロージャ又は乗り物、飛行シミュレータ、又は化学反応又は他の製造法で用いられる物質を収容し選択的に注ぎ出すのに用いられる容器が挙げられる。物体１０は、自動車であってもよい。物体１０と関連して制御システム１２が設けられ、この制御システムは、１以上のチルト検出装置１４及び１以上の角速度検出装置１６からの入力を受け取る。チルト検出装置１４は、従来型純流体チルトセンサ及び（又は）加速度計であるのがよい。１以上のチルトセンサが設けられることが意図されている。角速度センサ１６を、従来型ジャイロ又は他の公知の角速度検出装置によって構成するのがよい。１以上の角速度センサが設けられることが意図されている。
【００７０】
説明を明確にするための座標系を、図１及び図２示すように定めるのがよい。水平前向きの方向は、矢印２０によって示されるような正のｘ軸であると考えられる。正のｙ軸は、矢印２２で示すように右に向いていて且つｘ軸に垂直な水平方向であると考えられる（図２）。正のｚ軸は、矢印２４で示すように垂直方向に真っ直ぐ下に向いていると考えられる（図１）。チルトは、垂直からの角度のずれと定義され、±１８０゜に制限された範囲のものである。チルト又は傾斜の方向は、ｘ軸及びｙ軸で表される。ｘ軸回りの回転は、ロール又は横揺れであると考えられ、正のロールは、右側へ傾いている。ピッチ又は縦揺れは、ｙ軸回りの回転として定義され、正のピッチは、後方へのチルトである。ヨーは、ｚ（垂直）軸回りの回転として定義され、正のヨーは、右側への回転として定義される。これらの定義は右手の法則に準拠していることは注目されよう。ピッチ及びロールに関する上記定義は、オイラーのピッチ及びロールについての定義と類似しているが、異なる点は、オイラー角度が、特定の順序で適用され、中間座標系を生じさせることにある。本明細書で説明するチルトに基づくシステムでは、垂直軸回りの回転は存在せず、或いは無視できると仮定する。例えば、物体１０の方角は、固定されてもよく、或いは、チルト追跡及び制御には無関係であってもよく、或いは、オペレータとしての人間による制御を受けてもよく、或いは、チルトに関する情報を考慮に入れない又は考慮に入れる必要のないシステムによって制御されるものであってもよい。
【００７１】
図３は、図１に示す制御システム１２のコンポーネントをブロック図の形態で示している図である。制御システム１２は、インタフェース４０を有し、このインタフェースを介して制御システム１２はチルトセンサ１４及びジャイロ１６から出力された信号を受け取る。これら信号は本発明に従って、姿勢／回転推定ブロック４２によって処理される。ジャイロ及びセンサから出力された信号に基づいて、ブロック４２は、以下に説明する手法で物体の姿勢及び（又は）回転角速度の推定結果を生じさせる。ブロック４２によって得られた姿勢及び（又は）回転情報に基づいて、ブロック４４は、アクチュエータ４６を制御して物体１０の姿勢を制御するための制御信号を生じさせる。アクチュエータとしては、モータ、ソレノイド、可浸チャンバ、或いは上述の種類の物体の姿勢を制御するのに用いられる従来型の機械式、電気機械式、油圧式又は空気圧式装置が挙げられる。ブロック４２，４４の少なくとも一部を実行するための電子ハードウェアを、１以上の従来型マイクロプロセッサで構成してもよい。加うるに、制御信号発生ブロック４４は好ましくは、アクチュエータ４６のための適当な駆動信号を生じさせる駆動回路を更に有する。
【００７２】
制御システム１２は、物体１０に搭載されるものとして図１に示されているが、制御システム１２の少なくとも或る部分を物理的に物体１０から分離してもよいことは理解されるべきである。例えば、ブロック４２，４４のうち幾分か又は全てを実行する処理回路は、物体１０から見て遠隔に位置して遠隔測定法によりチルトセンサ１４及びジャイロ１６からの出力を受け取り、制御信号を適当なワイヤレス通信チャンネルによって物体１０に送り戻すことができる。
【００７３】
姿勢及び回転の推定
姿勢／回転推定ブロック４２の作用について以下に説明する。
【００７４】
チルトセンサ、例えば、従来型純流体センサは、比較的ドリフトの無いものであるのがよく、したがって、重力ベクトルについて信頼性の高い指示を与えることができる。かくして、静的システムの姿勢をかかるセンサを用いて重力に対して判定することができる。しかしながら、動的システムでは、この種のセンサは、角加速度及び振動加速度の影響を受けるので、これらを低域フィルタにかけて重力以外の加速度の影響を減衰させる必要がある。その結果得られたデータは、フィルタの帯域幅によって決定される有限期間にわたり、重力の平均方向を提供する。しかしながら、応答時間が早いことを必要とするシステムでは、重力センサだけでは適当ではない。
【００７５】
他方、ジャイロ又は他の角速度センサは、加速度による悪影響を受けることなく、高帯域幅及び高速応答をもたらすことができる。かかるセンサによって検出された角速度は、システムの姿勢を直接示すものではないが、角加速度に関するデータを統合すると位置に関する情報を得ることができる。しかしながら、速度センサは、統合の際に相当大きな誤差を生じさせる場合のあるドリフトの影響を受けやすい。ブロック４２によって行われる推定法は、ジャイロ１６及びチルトセンサ１４によりそれぞれ提供される高帯域幅情報と低帯域幅情報を組み合わせ、モデルベース（モデルを利用した）推定技術を用いて正確な位置及び回転データを生じさせる。
【００７６】
モデルベース推定器では、真の位置の推定値を、センサデータとシステムダイナミックスの内部モデルを比較することにより生じさせる。測定データと予想データの誤差を用いて推定値を連続的に精緻なものにする。この誤差が推定値に影響を及ぼす度合いは、適当な状態誤差を推定器に送り戻す重み付けされた行列Ｈによって求められる。かかる推定器に関する状態変数式が以下に示されている。
【００７７】
【数２７】

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

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

【００８３】
上式において、ω（ｓ）は、ｓ−領域においてｄθ／ｄｔの測定値である。したがって、この推定技術は、低帯域幅位置θ_０と高帯域幅速度ωを組み合わせるものである。低周波数では、推定位置は、上述したように低帯域幅測定値θ_０に近づく。高周波数では、推定位置は、高帯域幅速度推定値ωの積分に近づき、この高帯域幅速度推定値は、高帯域幅位置θと等価である。これに類似した結果が、推定速度について得られる。かくして、この種の推定器は、低帯域幅チルトデータ及び高帯域幅角速度データから角度位置及び角速度の推定値を得るのに好適である。利得ｋ_１，ｋ_２は、適当なフィルタ帯域幅及び安定性が得られるように選択される。式（３４）中においてｋ_２＝０且つω（ｓ）＝ｓθ（ｓ）とすれば、式（３３）に示す同一の１次位置推定値が得られることは注目されるべきである。
【００８４】
４元数を用いると任意の角度運動を通じてシステムの姿勢を追跡できることができることは知られている。さらに、４元数は、計算上効果的な表記法であると共にオイラー角度の受ける特異性には影響されない。また、高帯域幅センサ及び低帯域幅センサからのデータを、推定器を用いて組み合わせると、全帯域幅にわたって正確なデータが得られることも又知られている。しかしながら、４元数は、直接的な物理的測定には向いておらず、これにより、チルトセンサデータからの絶対４元数基準のずれが複雑になる。かかる基準が無ければ、推定器は、ジャイロドリフトが積分された４元数を改竄し、最終的にシステムがその角度位置の追跡を失うようにするのを防止することが不可能になる。
【００８５】
上述の説明から、従来の４元数は、一義的に回転を表すには２つの回転ベクトル及び２つの外部基準点を必要とすることが思い起こされよう。しかしながら、チルトセンサだけでは、地球の中心に対する単一の基準だけが利用できるに過ぎない。本発明は、４つの従来要素のうち０でないのは３つに過ぎず、４元数形式において一義的に姿勢（tilt）を表すのに基準を１つしか必要としない変形４元数表記法を提供することにより、チルト利用システムに関するこの問題を解決する。この解決策の前提条件は、システムがその方角についての知識を必要としないのでヨー（垂直軸回りの回転）の変化を追跡する必要はないということにある。
【００８６】
上述の展開に基づき、単一の基準点からの４元数係数を計算するには、各座標系中の基準ベクトルが回転軸線に垂直であることが必要である。重力をその基準とするチルト利用システムの場合、これは、回転軸線が常時水平のままであるということが必要であることに等しい。回転軸線ｎが常に水平であれば、この回転軸のｚ成分は同じ０でなければならない。単位４元数成分の定義を参照すると、これは、４元数のｑ_３成分も同じく０でなければならないことを意味している。したがって、条件ｑ_３＝０は拘束条件となる。かくして、結果的に得られる４元数は、変形されていて、有意の成分を３つしか含んでいない。
【００８７】
また、拘束条件ｑ_３＝０は、４元数速度式（式（２８）又は式（２９））を積分している間、満足されなければならない。この条件は、条件ｄｑ_３／ｄｔ＝０も又保たれている限り、積分操作中維持できる。式（２８）及び式（２９）から、これは、以下の条件のうちの１つが成立しなければならないということを意味している。
【００８８】
【数３０】

【００８９】
【数３１】

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

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

【００９４】
上式において、ｎ＝［ｎ_１ ｎ_２］^Ｔであり、ｅ_３＝ｎ_３＝０である。
【００９５】
ω_Ｅ３及びｅ_３を除去した後、変形４元数速度式は次のようになる。
【００９６】
【数３４】

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

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

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

【０１０３】
この結果を用いると変形４元数速度を積分すると、変形４元数が得られる。しかしながら、この式（４３）は、理想的な角速度ベクトルωを前提としていて、ジャイロからの実際の角速度データのドリフトを補償することはできない。一般に、ドリフトについて補正を行うには、ずれをヨージャイロデータから差し引かなければならない。次に、上述の式中のωに代えてω’＝ω−ω_{ｄｒｉｆｔ}の形式の補正角速度が用いられる。ジャイロドリフトが時間の関数であるということにより事態は一層複雑になる。したがって、ドリフト補正項を連続的にアップデートしなければならない。これには、上述の２次推定技術の利用が必要である。
【０１０４】
推定器の目的は、４元数速度の積分により得られた４元数をチルトデータから得られた４元数と比較することにある。定常偏差がジャイロのドリフトだけに起因しているものと仮定すると、この偏差又は誤差の大きさを用いるとジャイロドリフト補正項を加減することができる。推定器利得は、定常偏差が時間の経過につれて０になるように選択される。推定器利得値の選択は、当業者の通常の知識の範囲内にあり、したがって、これ以上の説明は不要であろう。
【０１０５】
補正項を正しく調整するために、４元数誤差を変換して角速度誤差に戻す必要がある。これは、以下に示すような地球座標系中の角速度について解かれた元の４元数速度式を用いて達成される。
【０１０６】
【数３８】

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

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

【０１１１】
代入後、結果的に得られる地球座標系中の角度誤差ベクトルは、Δθ_Ｅ’＝［Δθ_Ｅ１ Δθ_Ｅ２ ω_Ｅ３］^Ｔによって与えられる。ジャイロは物体座標系中に位置しているので、地球座標系中の角度誤差をΔθ＝Ｒ_ＢＥΔθ_Ｅ’を用いて物体座標系に変換しなければならない。次に、結果的に得られる角度誤差を推定器によって用いると、ジャイロドリフト補正項を得ることができる。
【０１１２】
次に、チルトデータを変形４元数に変換する方法について説明する。４元数成分を得るには、回転軸に垂直であって、θ／２の角度だけ離れた２つの単位ベクトルの内積と外積を作るのがよいことを示した。かかる２つのベクトルを地球の中心（即ち、チルトセンサによって測定される重力の方向）のところに外部基準点を用いることにより作ることができる。初期重力ベクトルをＧ_ｉとし、最終重力ベクトルをＧ_ｆで示すものとする。（初期重力ベクトルＧ_ｉに相当するデータを初期化中に得ることができ、次の使用のために保存する。「最終」重力ベクトルＧ_ｆは、推定器が動作しているときにアップデートされることになろう。）
【０１１３】
これら２つのベクトルを以下に示すように正規化する。
【０１１４】
【数４１】

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

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

【０１１９】
作図により、ＡとＢの外積は、各座標系中の重力ベクトルに垂直なベクトルを作る。その結果として、このベクトルも又水平でなければならない。これは、４元数のｚ成分が所望通り同じ０であるということを意味している。したがって、４元数のうちの最初の３つの成分だけが微々たるものであるという訳ではなく、変形４元数の形式を満足させる。
【０１２０】
上述の推定法の全体が図４のブロック図に示されている。
【０１２１】
推定技術は、２つのループ、即ちｋ_１ループ及びｋ_２ループに分割される。ｋ_１ループは、チルトセンサからのデータに低帯域幅フィルタをかけ、推定４元数速度を調整するのに用いられる誤差の大きさを決定する。ｋ_２ループは、ジャイロデータについてドリフト補正を行う。ジャイロは物体座標系中に位置しているので、ｋ_２ループは座標変換の物体側に位置していなければならない。ｋ_２利得は、ジャイロデータ中のドリフトを補正するのに用いられる誤差の大きさを決定する。次に、重み付けされた誤差を積算してループが時間の経過につれて０の定常誤差を達成するようにする。換言すると、４元数誤差が０になると、ｋ_２積分器の出力は変化を停止し、ドリフト補正項は一定のままになる。
【０１２２】
図４において、ｋ_２ループ中の加算ブロック５０は、重み付けされた誤差信号（これは、ドリフト補正信号であると考えることもできる）をジャイロ１６から出力された角速度情報に適用し、それにより補正された角速度情報が得られる。
【０１２３】
ブロック５２において、補正された角速度情報を地球座標系に変換して、変形４元数に変換する。ブロック５２中の標識Ｅ／２によって示されているように、このブロックに適用される計算は、上述の式（４３）及び式（４２）に一致する。結果的に得られた推定速度情報（これは、上述の変形４元数形態をなしている）は、加算ブロック５４への入力として提供される。ブロック５４では、ｋ_１ループによって得られた補正を推定４元数速度情報に適用しそれにより推定域の変化を表すデータが得られる。このデータ（これは、推定位置差信号ということができる）をブロック５６で積算すると変形４元数推定位置データが得られる。ブロック５８では、ブロック５６から出力された変形４元数推定位置データ及びブロック５２から出力された推定変形４元数速度情報が、オイラー角度に変換されて制御信号発生ブロック４４（図３）に出力される。この変換は、以下の数式を用いると容易に達成される。（以下の式は三角関数を用いているが、計算上の複雑さは、最小限に抑えられているのでルックアップテーブルを用いて容易に実行することができる。）
【０１２４】
【数４４】

【０１２５】
また、チルト利用システムの運動範囲が制限されている場合、小角度近似法を用いてこれら数式を単純化できる。
【０１２６】
制御信号活性器４４は、オイラー信号ではなく４元数信号を出力するタイプのものであれば、ブロック５８を省いてもよく、更に推定４元数位置及び速度に関する情報（本発明に従って得られる変形形態の状態にある）を直接制御信号発生器に送ってもよい。
【０１２７】
何れの場合においても、変形４元数形態の推定位置情報は、加算ブロック６０に入力として与えられ、この加算ブロックにおいて、これはブロック６２のところで変形４元数形態に変換されたチルトセンサ１４から出力された現在のチルト情報と比較される。（ブロック６２で得られる信号は、変形４元数基準位置信号と呼ぶことができる。）ブロック６２のところで行われる変換は、式（４６）乃至式（４８）と関連して、上述した手順に一致している。加算ブロックから出力された結果としての誤差信号をブロック６４のところで利得ファクタｋ_１によって重み付けし、次に、上述の補正信号としてブロック５４に送る。ブロック６０からの出力（これは、誤差位置信号と考えることができる）も又、ブロック６６への入力として得られる。式（４４）に従って、ブロック６６は、推定４元数とチルト４元数の誤差をチルト角度誤差に変換する。ブロック６６から出力される結果としての信号の無効ヨー成分を地球座標系ヨー成分で置き換え、この地球座標系ヨー成分は、ブロック５２の処理から得ることができ、結果的に得られる角度誤差ベクトルをブロック６８のところで物体座標系に変換する。次に、ブロック６８の出力をブロック７０のところで積分してｋ_２利得で重み付けし、結果的に得られる信号を上述のドリフト補正信号として加算ブロック５０に送る。
【０１２８】
ブロック５２，６６，６８における処理に必要な係数は、図４の符号７２のところに示すように積分ブロック５６から出力された変形４元数位置情報から得られることは注目されるべきである。
【０１２９】
当業者には理解されるように、図４にブロック図の形態で示された方法は、１以上の適当にプログラムされたコンピュータ処理装置（例えば、１以上のマイクロプロセッサ）によって有利に実施でき、かかるコンピュータ処理装置は、制御システム１２の一部をなしている。
【０１３０】
本発明に従って得られる姿勢及び回転推定器では、回転センサによって得られる高帯域幅情報及びチルトセンサからの低帯域幅情報は、従来の４つの要素ではなく３つの要素を持つ新規な変形４元数表記法で組み合わされる。結果的に得られる推定４元数は、設計対象の帯域幅全体にわたって有効であり、時間の経過につれてドリフトすることはない。実行されるべきコンピュータ処理を、単純な算術、例えば、乗算、加算及び平方根を用いて効率的に取り扱うことができる。変形４元数推定器は、傾動状態が追跡されると共に（或いは）制御されるべき広範なシステムに適用できる。
【０１３１】
本発明の上記説明は、例示であって、本発明を限定するものではない。当業者であれば、上述の実施形態の種々の設計変更又は改造を行うことができ、これらは本発明の精神及び範囲から逸脱することなく想到できる。
【図面の簡単な説明】
【図１】 本発明が適用される傾動可能な物体の概略側面図である。
【図２】 図１の傾動可能な物体の平面図である。
【図３】 図１の傾動可能な物体の制御システムのブロック図である。
【図４】 図３の制御システムの本発明に従って得られる姿勢及び回転推定部分のブロック図である。

Claims (11)

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

Images