(57)【特許請求の範囲】
【請求項１】
人間により運ばれる物体の時刻ｋにおける空間内での向きを、３空間軸に沿った前記物体の加速度計による全加速度（ｙＡ）、磁力計による磁場（ｙＭ）、およびジャイロメータによる回転速度（ｙＧ）の測定値を用いて推定する方法であって、
Ａ：第１のサブユニットが、操作中に測定時刻ｋにおける測定値（ｙＡ，ｙＭ）を前処理手段により前処理して前記測定値における外乱、すなわち前記物体の固有加速度または地球磁場に加えられた磁場からの外乱の存在を検知して、外乱が検知された時刻ｋにおける推定された無外乱測定値である推定測定値
【数１】
0005861235.tif 000049
を、ステップｋ−１で取得された向きまたは外乱が検知されていない測定値に基づいて算出するステップと、
Ｂ：第２のサブユニットが、ステップＡから得られた、時刻ｋで取得された推定測定値
【数２】
0005861235.tif 000050
および時刻ｋにおける回転速度（ｙＧ）に基づいて、観測装置により時刻ｋにおける向きを推定するステップと
を含む方法。
【請求項２】
時刻ｋにおける空間内での物体の向きの前記推定が、空間内での３軸に沿った前記物体の全加速度（ｙＡ）、磁場（ｙＭ）、および回転速度（ｙＧ）の測定値だけを用いることを特徴とする、請求項１に記載の方法。
【請求項３】
ステップＡが、
Ａ２：前記全加速度および磁場（ｙＡ，ｙＭ）の測定値内での時刻ｋにおける外乱の有無の検知、
Ａ３：時刻ｋにおいて外乱が存在しない場合、時刻ｋで推定された推定測定値
【数３】
0005861235.tif 000051
は時刻ｋにおける測定値に等しく、外乱が存在する場合、時刻ｋで推定された推定測定値
【数４】
0005861235.tif 000052
はステップｋ−１で取得された向きに基づいて計算されること
を含むことを特徴とする、請求項１に記載の方法。
【請求項４】
前記ステップＡ２が、
Ａ２．１：前記全加速度測定値のノルムを重力場（Ｇ０）のノルムと比較するテスト、すなわち時刻ｋにおける前記加速度測定値のノルムと前記重力場（Ｇ０）のノルムとの差の絶対値が所定の閾値（αＡ）を下回る場合は前記加速度外乱がゼロであると仮定し、さもなければ外乱が存在し、各軸上で前記外乱が、時刻ｋにおける前記全加速度測定値と時刻ｋで推定された前記無外乱加速度測定値との差に等しいと仮定することを特徴とするテストと、
Ａ２．２：前記磁場測定値のノルムを地球磁場（Ｈ０）のノルムと比較するテスト、すなわち前記磁場測定値のノルムと前記地球磁場のノルムとの差の絶対値が所定の閾値（αＭ）を下回る場合は前記磁気的外乱がゼロであると仮定し、さもなければ、各軸上で前記磁気的外乱が、時刻ｋにおける前記磁場測定値と、時刻ｋで推定された前記無外乱磁場測定値との差に等しいと仮定することを特徴とするテストと
を含んでいることを特徴とする、請求項３に記載の方法。
【請求項５】
前記ステップＡ２．１において、時刻ｋ−１で推定された外乱
【数５】
0005861235.tif 000053
に対する追加的なテストを実行し、前記全加速度測定値のノルムと時刻ｋにおける前記重力場（Ｇ０）のノルムとの差の絶対値が前記所定の閾値（αＡ）を下回る場合、時刻ｋ−１において推定された前記加速度外乱のノルムが所定の閾値（βＡ）を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける前記加速度外乱が実質的にゼロであると仮定し、および／またはステップＡ２．２において時刻ｋ−１で推定された磁気的外乱
【数６】
0005861235.tif 000054
に対する追加的なテストを実行し、前記磁場測定値のノルムと前記地球磁場（Ｈ０）のノルムとの差の絶対値が前記所定の閾値（αＭ）を下回る場合、時刻ｋ−１において推定された前記磁気的外乱の絶対値が所定の閾値（βＭ）を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける前記磁気的外乱が実質的にゼロであると仮定することを特徴とする、請求項４に記載の方法。
【請求項６】
ステップＡ２の検知の少なくとも１個がユーザにより設定された時間ウインドウ（ＴＡ，ＴＭ）に対して実行されることを特徴とする、請求項３に記載の方法。
【請求項７】
前記固有加速度の検知は、
時刻ｔｋにおいて測定された加速度（ｙＡ，Ｋ）のノルムと地球重力場を表わすベクトル（Ｇ０）のノルムとの差の絶対値が所定の閾値（αＡ）を超える場合に時間ウィンドウ（ＴＡ）内に時刻ｔｋが存在するかのテストを実行し、
このテストの結果が肯定的である場合、時刻ｔｋにおける加速度
【数７】
0005861235.tif 000055
の無外乱測定値をスカラ（−１）、時刻ｔｋ−１における物体の推定された向き
【数８】
0005861235.tif 000056
、ベクトル（Ｇ０）と推定された向き
【数９】
0005861235.tif 000057
の共役
【数１０】
0005861235.tif 000058
の積と推定し、
このテスト結果が否定的である場合、時刻ｔｋにおける加速度
【数１１】
0005861235.tif 000059
の無外乱測定値を時刻ｔｋにおいて測定された加速度（ｙＡ，Ｋ）と推定し、
時刻ｔｋにおける前記固有加速度と推定された
【数１２】
0005861235.tif 000060
がスカラ（−１）、時刻ｔｋ−１における物体の推定された向き
【数１３】
0005861235.tif 000061
、ベクトル（Ｇ０）と推定された向き
【数１４】
0005861235.tif 000062
の共役
【数１５】
0005861235.tif 000063
の積と時刻ｔｋにおいて測定された加速度（ｙＡ，Ｋ）との差に等しい場合に終了する
処理により生成されることを特徴とする、請求項６に記載の方法。
【請求項８】
前記磁気的外乱の検知は、
時刻ｔｋにおいて測定された磁場（ｙＭ，Ｋ）のノルムと地球磁場を表わすベクトル（Ｈ０）のノルムとの差の絶対値が所定の閾値（αＭ）を超える場合に時間ウィンドウ（ＴＭ）内に時刻ｔｋが存在するかのテストを実行し、
このテストの結果が肯定的である場合、時刻ｔｋにおける磁場
【数１６】
0005861235.tif 000064
の無外乱測定値を時刻ｔｋ−１における物体の推定された向き
【数１７】
0005861235.tif 000065
、ベクトル（Ｈ０）と推定された向き
【数１８】
0005861235.tif 000066
の共役
【数１９】
0005861235.tif 000067
の積と推定し、
このテスト結果が否定的である場合、時刻ｔｋにおける磁場
【数２０】
0005861235.tif 000068
の無外乱測定値を時刻ｔｋにおいて測定された磁場（ｙＭ，Ｋ）と推定し、
時刻ｔｋにおける地球磁場に加えられた磁場と推定された
【数２１】
0005861235.tif 000069
が時刻ｔｋ−１における物体の推定された向き
【数２２】
0005861235.tif 000070
、ベクトル（Ｈ０）と推定された向き
【数２３】
0005861235.tif 000071
の共役
【数２４】
0005861235.tif 000072
との積と時刻ｔｋにおいて測定された磁場との差に等しい場合に終了する
処理により実行されることを特徴とする、請求項６に記載の方法。
【請求項９】
前記固有加速度の検知の出力において時刻ｔｋにおける角度
【数２５】
0005861235.tif 000073
がもまた計算され、次いで前記磁気的外乱の検知が、
時間ウィンドウＴＭとして、固有加速度が検知された場合は第１の閾値ＴＭｆａｓｔを設定し、他の場合は第２の閾値ＴＭｓｌｏｗを設定し、
時刻ｔｋにおいて測定された磁場（ｙＭ，Ｋ）のノルムと地球磁場を表わすベクトル（Ｈ０）のノルムとの差の絶対値が所定の閾値（αＭ）を超える場合、または、角度（ｕｋ）と角度変数（ｕ０）との差の絶対値が所定の閾値（αｕ）を超える場合に、時間ウィンドウ（ＴＭ）内に時刻ｔｋが存在するかのテストを実行し、
このテストの結果が肯定的である場合、時刻ｔｋにおける磁場
【数２６】
0005861235.tif 000074
の無外乱測定値を時刻ｔｋ−１における物体の推定された向き
【数２７】
0005861235.tif 000075
、ベクトル（Ｈ０）と推定された向き
【数２８】
0005861235.tif 000076
の共役
【数２９】
0005861235.tif 000077
の積と推定し、
このテスト結果が否定的である場合、時刻ｔｋにおける磁場
【数３０】
0005861235.tif 000078
の無外乱測定値を時刻ｔｋにおいて測定された磁場（ｙＭ，Ｋ）と推定し、
時刻ｔｋにおける地球磁場に加えられた磁場と推定された
【数３１】
0005861235.tif 000079
が時刻ｔｋ−１における物体の推定された向き
【数３２】
0005861235.tif 000080
、ベクトル（Ｈ０）と推定された向き
【数３３】
0005861235.tif 000081
の共役
【数３４】
0005861235.tif 000082
との積と時刻ｔｋにおいて測定された磁場との差に等しい場合に終了する
処理により実行されることを特徴とする、請求項７に記載の方法。
【請求項１０】
前記ステップＢで用いる前記観測装置が拡張カルマンフィルタであることを特徴とする、請求項１〜９のいずれか１項に記載の方法。
【請求項１１】
時刻ｋにおいて推定された無外乱測定値から向きを推定するステップＢが、
時刻ｋ−１において帰納的に推定された状態ベクトル
【数３５】
0005861235.tif 000083
から時刻ｋにおける先験的な状態ベクトル
【数３６】
0005861235.tif 000084
を推定するステップ、
時刻ｋにおける先験的な状態ベクトル
【数３７】
0005861235.tif 000085
の推定から時刻ｋにおける先験的な測定値
【数３８】
0005861235.tif 000086
を推定するステップ、
時刻ｋで推定された前記無外乱測定値と前記先験的に推定された測定値
【数３９】
0005861235.tif 000087
との差を計算することにより、前記拡張カルマンフィルタ（Ｋｋ）のゲインおよびイノベーション（Ｉｋ）を計算するステップ、
前記ゲインおよび前記イノベーションにより時刻ｋにおいて先験的に推定された状態ベクトルを修正することにより、時刻ｋにおける推定された向き
【数４０】
0005861235.tif 000088
を計算するステップ
を含んでいることを特徴とする、請求項１０に記載の方法。
【請求項１２】
前記拡張カルマンフィルタで用いる前記状態ベクトルが角速度および方位四元数の要素を含んでいることを特徴とする、請求項１１に記載の方法。
【請求項１３】
前記拡張カルマンフィルタで用いる前記状態ベクトルが方位四元数の要素だけを含んでいることを特徴とする、請求項１２に記載の方法。
【請求項１４】
加速度測定値（ｙＡ）を与える検知ユニット、磁場（ｙＭ）を測定する検知ユニット、３空間軸に沿って回転速度（ｙＧ）を測定する検知ユニット、および前記検知ユニットから供給された測定値に基づいて時刻ｋにおける向きを推定する処理ユニットを少なくとも含む人間により運ばれる物体の姿勢制御システムであって、
前記加速度（ｙＡ）および磁場（ｙＭ）の測定値を前処理する第１のサブユニット、すなわち前記測定値内における、前記物体の固有加速度または地球磁場に加えられた磁場からの外乱の存在を検知すること、並びに外乱が検知された時刻ｋにおける推定された無外乱測定値または外乱が検知されていない測定値である推定測定値
【数４１】
0005861235.tif 000089
を算出する前処理サブユニットと、
時点ｋにおいて前記第１のサブユニットから取得した推定測定値
【数４２】
0005861235.tif 000090
と前記検知ユニットから取得した時点ｋにおける前記回転速度（ｙＧ）から時点ｋにおける向きを観測装置により推定する第２のサブユニットと
を含む姿勢制御システム。
【請求項１５】
前記第１のサブユニットが、前記加速度測定値内における、固有加速度の存在を検知するモジュール、および前記磁場測定値内における磁気的外乱の存在を検知するモジュールを含んでいることを特徴とする、請求項１４に記載の姿勢制御システム。
【請求項１６】
前記加速度測定値内における固有加速度の存在を検知する前記モジュールおよび前記磁場測定値内における磁気的外乱の存在を検知する前記モジュールが１個以上の時間ウインドウにおいてこれら検知を実行すべく動作可能であることを特徴とする、請求項１５に記載の姿勢制御システム。
【請求項１７】
前記固有加速度および前記磁気的外乱を推定すると共に、前記物体の速度および位置を計算するモジュールを更に含んでいる、請求項１４に記載の姿勢制御システム。
【請求項１８】
前記観測装置が拡張カルマンフィルタであることを特徴とする、請求項１４に記載の姿勢制御システム。
【請求項１９】
空間内での３軸に沿った前記全加速度（ｙＡ）の測定値、前記磁場（ｙＭ）の測定値、および前記回転速度（ｙＧ）の測定値を供給する検知ユニットがＭＥＭＳセンサであることを特徴とする、請求項１４に記載の姿勢制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、固有加速度および磁気的外乱の有無によらず、空間における物体の向きを推定する方法、および前記方法を実装することにより向きの推定を可能にする適当な装置に関する。
【背景技術】
【０００２】
一般に、向きを把握するには、モーションキャプチャ装置と呼ばれる機構の一部をなし、姿勢制御装置とも呼ばれる多くのセンサを用いる。
【０００３】
ＭＥＭＳ（微小電子機械システム）センサを用いて上記制御装置を構築することができ、前記ＭＥＭＳセンサは小型且つ安価である利点を有する。そのようなＭＥＭＳセンサを用いることにより、特に在宅高齢者の監視や機能リハビリを行う生医学的分野、スポーツ運動の分析を行うスポーツ分野、自動車、ロボット工学、仮想現実および３次元アニメーション分野、更により一般的に運動を判定または観測する必要がある様々な応用分野で姿勢制御装置の利用を検討することが可能になる。
【０００４】
しかしこれらのＭＥＭＳセンサには、（例えばナビゲーション分野で利用される）非ＭＥＭＳセンサに比べて相対的にノイズやバイアスの影響を受けやすいという短所がある。
【０００５】
更に、加速度計と磁力計の両方を用いる姿勢制御装置の存在が知られており、これらにより３自由度を有する運動、すなわち地球重力場および地球磁場に比べて固有加速度および磁気的外乱が各々無視できる運動を再構成できる。しかし、この仮定が成り立たない、すなわち固有加速度または磁気的外乱を無視できない場合、運動は６〜９自由度を示す。従って、加速度計と磁力計だけを使用する姿勢制御装置を用いて、動いている物体の向きを推定することは不可能である。このため、多岐にわたるモーションキャプチャの用途に対応すべくこれらの制約を克服することが必須である。
【０００６】
従って、追加的なセンサ、特にレートジャイロ、加速度計、および磁力計を組み合わせて用いることが検討されてきた。これらのセンサから得られる測定値は、二つの部分からなる。すなわち動いている物体の向きに直接関係する情報部分、および使用するセンサに依存する性質を有する外乱部分である。第１に、これらは加速度計から供給された測定値の場合は固有加速度、磁力計から得られた測定値の場合は磁気的外乱、およびレートジャイロの場合はバイアスである。これらの外乱により向きの推定が不正確になる恐れがある。
【０００７】
現在、加速度計、磁力計、およびレートジャイロから供給された測定値から、物体の向きを推定する多くの方法がある。
【０００８】
１個以上の最適化基準を実装する所謂最適化方法があるが、これらは計算時間に関して比較的コストが高い。更に、問題が複雑になった場合、最適化基準を定義することが困難である。
【０００９】
また、ニューラルネットワークを実装する方法もあるが、後者は正確な推定を得るために、特にデータベースの大きさと計算時間に関して学習フェーズを必要とする。
【００１０】
更に、最適化方法およびニューラルネットワークを実装する方法の場合、時間経過に伴う状態遷移の概念を考慮することが困難であるため、これらの方法は脆弱である。
【００１１】
観測装置を実装する方法もあり、これらは上述の方法とは異なり、２個のソースからの情報すなわち、センサから供給された測定値から得られた情報およびトレンドモデルから得られた情報を、リアルタイム実行に匹敵する計算時間を維持しつつ統合することができる。
【００１２】
観測装置を用いる公知の方法は、主としてカルマンフィルタの利用に依存する。この技術の利点は、センサから供給された測定値から得られた情報の品質およびモデルの品質を考慮しつつ、データの統合を行うことができる点である。
【００１３】
当業者によく知られた多くの種類のカルマンフィルタがある。
【００１４】
・拡張カルマンフィルタ（またはＥＫＦ）。これは高速且つ実装が容易であり、モーションキャプチャへの応用の一つが特に文献「Ｑｕａｔｅｒｎｉｏｎ−ｂａｓｅｄ  ｅｘｔｅｎｄｅｄ  Ｋａｌｍａｎ  ｆｉｌｔｅｒ  ｆｏｒ  ｄｅｔｅｒｍｉｎａｔｉｏｎ  ｉｎ  ｔｈｅ  ｏｒｉｅｎｔａｔｉｏｎ  ｂｙ  ｉｎｅｒｔｉａｌ  ａｎｄ  ｍａｇｎｅｔｉｃ  ｓｅｎｓｉｎｇ」，ＳＡＢＡＴＩＮＩ  Ａ．Ｍ．，ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  ｏｎ  Ｂｉｏｍｅｄｉｃａｌ  Ｅｎｇｉｎｅｅｒｉｎｇ，２００６，５３（７）に記述されている。
・強非線形問題専用のＵＫＦカルマンフィルタ（無香料カルマンフィルタ（Ｕｎｓｃｅｎｔｅｄ  Ｋａｌｍａｎ  Ｆｉｌｔｅｒ））。時折、モーションキャプチャの関連で遭遇する問題は弱非線形であるため、向きの推定にはあまり適していない。更に、ＥＫＦフィルタに比べて計算コストが大幅に増大する。従って、ＥＫＦフィルタほど関心が払われない。例えば、文献"Ｐｏｒｔａｂｌｅ  ｏｒｉｅｎｔａｔｉｏｎ  ｅｓｔｉｍａｔｉｏｎ  ｄｅｖｉｃｅ  ｂａｓｅｄ  ｏｎ  ａｃｃｅｌｅｒｏｍｅｔｅｒｓ，ｍａｇｎｅｔｏｍｅｔｅｒｓ  ａｎｄ  ｇｙｒｏｓｃｏｐｅ  ｓｅｎｓｏｒｓ  ｆｏｒ  ｓｅｎｓｏｒ  ｎｅｔｗｏｒｋ"  ｂｙ  ＨＡＲＡＤＡ  Ｔ．，ＵＣＨＩＮＯ  Ｈ．，ＭＯＲＩ  Ｔ．，ＳＡＴＯ  Ｔ．，ＩＥＥＥ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｍｕｌｔｉｓｅｎｓｏｒ  Ｆｕｓｉｏｎ  ａｎｄ  Ｉｎｔｅｇｒａｔｉｏｎ  ｆｏｒ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｓｙｓｔｅｍｓ，２００３に、加速度計と磁力計により絶対方位が統合され、且つ回転速度がＵＫＦフィルタにより統合される携帯用方位推定装置を記述している。
・補完的カルマンフィルタ。この場合、状態自体ではなく状態における誤差を推定することが目的であって、実装が極めて複雑である。
【００１５】
フィルタの選択に加え、フィルタに渡される測定値の品質、特にそれらの値の信頼性が極めて重要である。
【００１６】
実際には、上述のように測定値は、動いている物体の向きに直接関係する情報部分、用いられるセンサに依存する性質を有する外乱部分を含んでいる。これらは加速度計から供給された測定値の場合は固有加速度、磁力計から得られた測定値の場合は磁気的外乱、およびレートジャイロの場合はバイアスである。また、測定ノイズを考慮する必要があるが、前記ノイズは従来フィルタ内で処理されていた。
【００１７】
現在、外乱を処理する多くの方法がある。その一つは、文献「Ｄｅｓｉｇｎ，ｉｍｐｌｅｍｅｎｔａｔｉｏｎ  ａｎｄ  ｅｘｐｅｒｉｍｅｎｔａｌ  ｒｅｓｕｌｔｓ  ｏｆ  ａ  ｑｕａｔｅｒｎｉｏｎ−ｂａｓｅｄ  Ｋａｌｍａｎ  ｆｉｌｔｅｒ  ｆｏｒ  ｈｕｍａｎ  ｂｏｄｙ  ｍｏｔｉｏｎ  ｔｒａｃｋｉｎｇ」，ＹＵＮ  Ｘ．，ＢＡＣＨＭＡＮＮ  Ｅ．Ｒ．ＩＥＥＥ  Ｔｒａｎｓａｃｔｉｏｎｓ  Ｏｎ  Ｒｏｂｏｔｉｃｓ，２００６，２２（６）および「Ａｐｐｌｉｃａｔｉｏｎ  ｏｆ  ＭＩＭＵ／Ｍａｇｎｅｔｏｍｅｔｅｒ  ｉｎｔｅｇｒａｔｅｄ  ｓｙｓｔｅｍ  ｏｎ  ｔｈｅ  ａｔｔｉｔｕｄｅ  ｄｅｔｅｒｍｉｎａｔｉｏｎ  ｏｆ  ｍｉｃｒｏ  ｓａｔｅｌｌｉｔｅ」，ＳＵ  Ｋ．，ＲＥＮ  Ｄ．Ｈ．，ＹＯＵ  Ｚ．，ＺＨＯＵ  Ｑ．，Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｍｅｃｈａｔｒｏｎｉｃｓ  ａｎｄ  Ａｕｔｏｍａｔｉｏｎ上，Ａｕｇｕｓｔ  ２００４年，Ｃｈｅｎｇｄｕ，Ｃｈｉｎａに記述されているように外乱が無視できると考えてセンサから得られた測定値に対してフィルタを提供するものである。従って、実際にセンサの１個に外乱が生じた場合には、フィルタから得られた測定値には誤差があるが、フィルタはそれを正確なものであると見なす。そのため向きの推定は不正確になる。従って、自由度数によらず所望の精度で推定値を得るために外乱を無視することはできない。このように外乱を受けた測定値が観測装置に供給されるため、性能レベルは大幅に低下する。
【００１８】
推定方法は従って、例えば文献「Ｐｏｒｔａｂｌｅ  ｏｒｉｅｎｔａｔｉｏｎ  ｅｓｔｉｍａｔｉｏｎ  ｄｅｖｉｃｅ  ｂａｓｅｄ  ｏｎ  ａｃｃｅｌｅｒｏｍｅｔｅｒｓ，ｍａｇｎｅｔｏｍｅｔｅｒｓ  ａｎｄ  ｇｙｒｏｓｃｏｐｅ  ｓｅｎｓｏｒｓ  ｆｏｒ  ｓｅｎｓｏｒ  ｎｅｔｗｏｒｋ」，ＨＡＲＡＤＡ  Ｔ．、ＵＣＨＩＮＯ  Ｈ．、ＭＯＲＩ  Ｔ．，ＩＥＥＥ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｍｕｌｔｉｓｅｎｓｏｒ  Ｆｕｓｉｏｎ  ａｎｄ  Ｉｎｔｅｇｒａｔｉｏｎ  ｆｏｒ  Ｉｎｔｅｌｌｉｇｅｎｔ  Ｓｙｓｔｅｍｓ，２００３に記述されているように外乱の存在を検知し、且つこれらの測定値の信頼性を更新することにより特定の測定値における偶発的な欠陥を考慮できるようにする。この方法は、測定値における外乱を検知する追加的なステップを提供する。外乱が検知された場合、対応する測定値に対する信用度が最小化される。
【００１９】
従って、外乱を含む測定値が提供する情報は、向きを推定する際に考慮されない。このため向きの推定は他のセンサから供給された測定値だけに依存する。ここで、多くのセンサからの測定値が同時に外乱を示す場合、観測装置はもはや正しい向きの推定値を提供するために十分な情報を有していない。
【００２０】
最後に、文献「Ｉｎｅｒｔｉａｌ  ａｎｄ  ｍａｇｎｅｔｉｃ  ｓｅｎｓｉｎｇ  ｏｆ  ｈｕｍａｎ  ｍｏｔｉｏｎ」，ＲＯＥＴＥＮＢＥＲＧ  Ｄ．，ｄｏｃｔｏｒａｌ  ｔｈｅｓｉｓ，Ｔｗｅｎｔｅ  ｕｎｉｖｅｒｓｉｔｙ，Ｎｅｔｈｅｒｌａｎｄｓ，２００６、および「Ｍｅａｓｕｒｉｎｇ  ｏｒｉｅｎｔａｔｉｏｎ  ｏｆ  ｈｕｍａｎ  ｂｏｄｙ  ｓｅｇｍｅｎｔ  ｕｓｉｎｇ  ｍｉｎｉａｔｕｒｅ  ｇｙｒｏｓｃｏｐｅｓ  ａｎｄ  ａｃｃｅｌｅｒｏｍｅｔｅｒｓ」，ＰｈＤ  Ｔｈｅｓｉｓ，Ｉｎｅｒｔｉａｌ  ｓｅｎｓｉｎｇ  ｏｆ  ｈｕｍａｎ  ｍｏｖｅｍｅｎｔ，ＬＵＩＮＧＥ  Ｈ．Ｊ．ｎ  ２００２ｂに記述さた方法では、観測装置を用いて外乱の存在を検知および推定している。このため、状態ベクトルが拡張され、より現実に近づいた測定モデル内で外乱が生じる。この技術は原理的には、６〜９自由度を有する運動の場合に適しているように見える。しかし、外乱と向きを組み合わせて推定することは、観測可能性が欠如していることに起因して困難である。また、多数のパラメータを設定する必要があるため、実装の複雑さが増大する。
【発明の概要】
【発明が解決しようとする課題】
【００２１】
従って本発明の一つの目的は、固有加速度および磁気的外乱の有無によらず、正確に向きを推定し、既存の方法に比べて簡素化された方法でこれを行う向き推定方法を提供することである。
【課題を解決するための手段】
【００２２】
上述の目的は、加速、磁場、および回転速度の３空間軸に沿った測定値に基づいて向きを推定する方法により実現され、本方法は
・外乱の存在を検知して、無外乱測定値を推定すべくこれらの測定値を前処理するステップと、
・前処理ステップから得られた測定値に基づいて向きを推定するステップと
を含んでいる。
【００２３】
本方法は外乱を無視しない、すなわち推定が誤っていないことを意味する。本方法は常に外乱を推定する。外乱が存在する場合、他の推定方法の場合のように付随する測定値または測定値群を拒絶しない。更に、本方法は外乱を状態ベクトルまたは測定モデルに組み込まないため、モデルが簡素化され、推定が不可能になる状況に陥らない。
【００２４】
従って、２段階の連続するステップで、向きを推定し、恐らくは外乱を推定する。観測装置には従って、加速度計、磁力計、およびレートジャイロから、向きの推定に理想的な条件になるべく近い、すなわち固有加速度、磁気的外乱、およびバイアスが各々存在しない状態での測定値が提供される。
【００２５】
このため、測定値の前処理を実行するための追加情報として、先行する時点で推定された向きを用いる。
【００２６】
本発明による推定方法は従って、センサ測定値から、関心対象である運動によらず、物体の向きを最適な仕方で抽出することを可能にする。本方法は更に、実装が簡単で、ごく少数の設定パラメータだけを含んでいる。
【００２７】
観測装置は拡張カルマンフィルタであることに利点がある。
【００２８】
外乱、特に固有加速度を推定する方法を提供し、積分および二重積分により物体の速度および位置の各々に戻ることが可能になる。
【００２９】
本発明の主題は従って、時刻ｋにおける空間内での物体の向きを、３空間軸に沿った前記物体の全加速度、磁場、および回転速度測定値を用いて推定する方法であって、以下のステップを含んでいる。
Ａ：時点ｋにおける前記測定値を前処理して前記測定値における外乱、すなわち物体の固有加速度、地球磁場に加えられた磁場、および回転速度測定値におけるバイアスを含む群からの外乱の存在を検知して時刻ｋにおける無外乱測定値を推定し、
Ｂ：ステップＡから得られた、時刻ｋで推定された無外乱測定値に基づいて、観測装置により時刻ｋにおける向きを推定する。
【００３０】
ステップＡは以下のステップを含んでいることに利点がある。
Ａ１：回転速度測定値の前処理、
Ａ２：全加速度および磁場の前記測定値内での時刻ｋにおける外乱の有無の検知、
Ａ３：時刻ｋにおいて外乱が存在しない場合、時刻ｋで推定された無外乱測定値は時刻ｋにおける測定値に等しく、外乱が存在する場合、時刻ｋで推定された無外乱測定値は、先行する時点ｋ−１で推定された向きに基づいて計算される。
【００３１】
ステップＡ１は、回転速度測定値から、事前の初期化ステップを実行する間に決定された平均バイアスを減算するものである。この平均バイアスは、回転速度測定値を与える手段を所与の期間停止して、各軸における回転速度測定値の平均を計算することにより得られる。人が装着した姿勢制御装置の場合、この停止は、当人の避け難い震えを除くために当人から制御装置を取り外すことを伴う。
【００３２】
加速および磁場測定値を前処理するＡ２は以下のステップを含んでいてよい。
・ステップＡ２．１、すなわち全加速度測定値のノルムを重力場のノルムと比較するテストからなり、時刻ｋにおける加速度測定値のノルムと重力場のノルムとの差の絶対値が所定の閾値を下回る場合は加速度外乱がゼロであると仮定し、さもなければ外乱が存在し、各軸上で当該外乱が、時刻ｋにおける全加速度測定値と時刻ｋで推定された無外乱加速度測定値との差に等しいと仮定する。
・ステップＡ２．２、すなわち磁場測定値のノルムを地球磁場のノルムと比較するテストからなり、磁場測定値のノルムと地球磁場のノルムとの差の絶対値が所定の閾値を下回る場合は磁気的外乱がゼロであると仮定し、さもなければ、各軸上で磁気的外乱が、時刻ｋにおける磁場測定値と時刻ｋで推定された無外乱磁場測定値との差に等しいと仮定する。
【００３３】
ステップＡ２．１において時刻ｋ−１で推定された外乱に対する追加的なテストを実行し、全加速度測定値のノルムと時刻ｋにおける重力場のノルムとの差の絶対値が所定の閾値を下回る場合、時刻ｋ−１において推定された加速度外乱のノルムが所定の閾値を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける加速度外乱が実質的にゼロであると仮定し、および／またはステップＡ２．２において時刻ｋ−１で推定された磁気的外乱に対する追加的なテストを実行し、磁場測定値のノルムと地球磁場のノルムとの差の絶対値が所定の閾値を下回る場合、時刻ｋ−１において推定された磁気的外乱の絶対値が所定の閾値を下回るか否かを調べる検査を実行し、このテストの結果が肯定的である場合、時刻ｋにおける磁気的外乱が実質的にゼロであると仮定することに利点がある。この追加的なステップにより、本推定方法の精度を向上させることができる。
【００３４】
推定された向きがぶれることがわかっている使用時の好ましくないケースでは、ステップＡ２．１およびＡ２．２のこれらの比較判定法の堅牢さは、状況に応じて異なる限られた時間ウインドウでしか保証されない。比較テストは従って、後述するような時間帯にわたり実行することに利点がある。従って、
【数１】
0005861235.tif 000002
を用いて、推定された無外乱測定値と呼ぶ前処理ステップ終了後に得られた測定値を表記すれば、
・固有加速度の検知は加速度測定値のノルムだけにより実行される。このノルムが持続期間ＴＡのスライディングウインドウの測定値の少なくとも１個に対するＧ０（地球の重力）のノルムと異なる場合、現時点における測定に外乱が存在すると考えられる。
・磁気的外乱の検知も同様に実行される。
○磁気測定値のノルムが、持続期間ＴＭのスライディングウインドウの測定値の少なくとも１個に対するＨ０（地球磁場）のノルムと異なる場合、
○または磁気測定値と無外乱加速度測定値の逆数
【数２】
0005861235.tif 000003
の間の角度がベクトルＧ０とＨ０の間の角度とは異なる場合、
現時点における測定値に磁気的外乱が存在する。
【００３５】
ＴＡは定数パラメータであってよく、一方ＴＭの値は運動の速度に関係していてよい。
【００３６】
同一装置上に置かれてユーザにより起動可能なこの変型例を用いて、時刻ｋ−１における測定値の値を探す必要がなく、従ってぶれが解消される。
【００３７】
ステップＢで用いる観測装置は好適には高速且つ簡便な拡張カルマンフィルタである。
【００３８】
時刻ｋにおいて推定された測定値から向きを推定するステップＢは以下を含んでいる。
・時刻ｋ−１において帰納的に推定された状態ベクトルから、時刻ｋにおける先験的な状態ベクトルの推定
・時刻ｋにおける先験的な状態ベクトルの推定から、測定値の先験的な推定と呼ばれる時刻ｋにおける先験的な測定値の推定、
・時刻ｋで推定された無外乱測定値と先験的に推定された測定値との間の差を計算することによる、拡張カルマンフィルタのゲインおよびイノベーションの計算、
・ゲインおよびイノベーションにより時刻ｋにおいて先験的に推定された状態ベクトルの修正により、時刻ｋにおいて推定された向きの計算、
【００３９】
拡張カルマンフィルタで用いる状態ベクトルは、角速度および方位四元数の要素を含んでいてよい。
【００４０】
拡張カルマンフィルタで用いる状態ベクトルは、方位四元数の要素だけを含んでいることに利点があり、それにより状態および測定モデルの構造を簡素化できる。
【００４１】
本発明の主題はまた、加速度測定値の供給に適した手段、磁場を測定する手段、３空間軸に沿って回転速度を測定する手段であって、運動において物体と連動することを意図された手段、および前記測定手段から供給された測定値に基づいて時刻ｋにおける向きを推定する手段を含む姿勢制御装置であり、前記推定手段は、
・前記加速、磁場、および回転の測定値を前処理する手段であって、前記測定値における外乱の存在を検知し、推定された無外乱加速度計の測定値、推定された無外乱磁気測定値、および非バイアス回転速度の配信に適している前処理手段と、
・前処理手段から供給された測定値から観測装置により時刻ｋにおける向きを推定する手段と
を含んでいる。当該観測装置は、拡張カルマンフィルタであってよい。
【００４２】
本発明による姿勢制御装置はまた、制御を行う間、回転速度測定手段の平均バイアスを計算する手段を含んでいてよい。
【００４３】
前処理手段は、加速度測定値における固有加速度の存在を検知する手段および磁場測定値における磁気的外乱の存在を検知する手段を含んでいる。
【００４４】
本発明による姿勢制御装置はまた、固有加速度を推定すると共に物体の速度位置を計算する手段を含んでいてよい。
【００４５】
３空間軸に沿って全加速度測定値、磁場測定値、および回転速度測定値を供給することに適した手段はＭＥＭＳセンサであることに利点がある。
【００４６】
本発明は、以下の記述および添付の図面からより理解されよう。
【図面の簡単な説明】
【００４７】
【図１】本発明による方法の時刻ｋにおけるフロー図である。
【図２Ａ−２Ｃ】本発明による加速度計、レートジャイロ、および磁力計の各々からの測定値を前処理するステップの詳細なフロー図を示す。
【発明を実施するための形態】
【００４８】
目的は、空間で移動している物体の向き、例えば人の向きを得ることである。このため、３空間軸に沿って全加速度、磁場、および回転速度測定値を供給することに適したセンサを含む姿勢制御装置を用いる。当該センサは、コストを削減し設置面積を制限できるＭＥＭＳセンサであることに利点がある。
【００４９】
加速度測定値の場合、例えば、各軸上で測定値を提供する３軸加速度計または３個の単軸加速度計であってよい。
【００５０】
同様に、磁場測定値の場合、３軸磁力計または３個の単軸の磁力計であってよい。
【００５１】
回転速度測定の場合、例えば、３個の単軸のレートジャイロでまたは２個の２軸レートジャイロであることに利点がある。
【００５２】
３軸は整列配置されていてもされていなくてもよいが、後者の場合、軸間の相対的な向きは既知でなければならない。
【００５３】
以下に記述において、簡便のため、加速度計または加速度計群を加速度計、磁力計または磁力計群を磁力計、レートジャイロまたはレートジャイロ群をレートジャイロと表記する。これらのセンサは、向きを知りたい物体に取り付けられる。
【００５４】
次式によりモデル化する測定値ｙだけが存在する。
【数３】
0005861235.tif 000004
但し、ｙＡ：加速度計から供給される全加速度の３軸測定値、
ｙＭ：磁力計から供給される磁場の３軸測定値、
ｙＧ：レートジャイロから供給される回転速度の３軸測定値、
Ｒ:回転行列、
Ｇ０:地球の重力分野（ベクトル３×１）、
Ｈ０:地球磁場（ベクトル３×１）、
ω:角速度、
ａ:固有加速度、
ｄ:磁気的外乱、
ｂ:レートジャイロバイアス、
ｖＡ：加速度測定値ノイズ、
ｖＭ：磁気測定ノイズ、
ｖＧ：レートジャイロ測定ノイズ
である。
【００５５】
向きはベクトルＧ０およびＨ０のデータにより完全に定義される基準座標に対して推定される。例えば、地心座標はベクトルＧ０（０；０；１）および
【数４】
0005861235.tif 000005
により定義される。
【００５６】
簡潔を旨として、３空間方向における測定値を区別しない。
【００５７】
測定値の数学的定義（Ｉ）から明らかなように、これらの測定値の各々は、向きの推定値を得る情報を含む第１の部分「−ＲＧ０、」、「Ｒ．Ｈ０」、ω、測定値にランダムに現れる場合がありる外乱を表す第２の部分ａ、ｄ、ｂ、および各センサにおける測定ノイズを表す第３の部分ｖＡ、ｖＭ、ＶＧを含んでいる。
【００５８】
本発明の方法において、測定値を収集した後で、向きの推定を行う処理ステップで当該測定値を用いる前に、前処理ステップを実行する。
【００５９】
図１に本発明による方法の一般的なフロー図を示す。
【００６０】
本方法の以下での記述において、時刻ｋにおける向きの推定（ｋは２以上の適当な整数）を一例として取り上げる。
【００６１】
本発明による方法は、姿勢制御装置を初期化するステップ１００、センサから供給された測定値を前処理するステップ２００、および観測装置による第３の処理ステップ３００を含んでいる。各ステップについて以下に詳述する。
【００６２】
本発明による方法の各種ステップについて詳細に述べる前に、観測装置について述べる。測定値処理ステップで用いる観測装置は、簡単、堅牢、且つすばやく実装できる拡張カルマンフィルタであることに利点がある。
【００６３】
当該フィルタは、当業者に広く知られているため詳述しない。状態モデルおよび測定モデルの数式だけを与える。
【００６４】
カルマンフィルタは、状態の時間的および動的傾向を定義する状態モデル、およびセンサ測定値および状態に関連付けるために用いる測定モデルを含んでいる。
【００６５】
第１のモデリングによれば、カルマンフィルタの状態ベクトルは角速度の３個の要素および向きを定義する四元数の４個の要素からなる。
【００６６】
関連付けられた状態および測定モデルは各々次式であってよい。
【数５】
0005861235.tif 000006
但し、ｘ：状態ベクトル
ω：角速度
ｑ：四元数
τ：角速度の進化モデルの時定数：
ｙ：測定値
ｗｘ：モデリングノイズ
【００６７】
簡潔を旨として、４×１次元ベクトル四元数[０，Ｇ０]は３×１次元ベクトルＧ０により識別され、同じことが４×１次元ベクトル四元数[０，Ｈ０］に当てはまり、ベクトルＨ０も３×１次元である。
【００６８】
第２のモデリングによれば、利点として、４次元以下の四元数の要素だけを含む状態ベクトルが用いることができるのに対し、第１のモデリングでは状態ベクトルは７次元である。次いでジャイロ測定値が直接状態モデルに投入され、測定ベクトルは加速度計および磁力計からの測定値だけを含んでいる。
【００６９】
状態モデルおよび測定モデルは次式で表すことができる。
【数６】
0005861235.tif 000007
【００７０】
この第２のモデリングを用いて状態および測定モデルの次元を直接下げるため、これらの構造を簡素化することができる。更に、測定ノイズおよび状態ベクトル推定誤差の設定パラメータ、特にモデリングノイズの共分散行列の要素の数もまた制限されるため、本方法の実装がより容易になる。このようにして得られた推定結果は、第１のモデリングを用いて得られたものと同様の精度である。
【００７１】
ここで本発明によるステップ１００、２００および３００について詳述する。
【００７２】
初期化ステップ１００は、推定したいレートジャイロの平均的外乱を与える。この外乱ｂは実際にはレートジャイロのバイアスであって、２個の限界値の間で変化する。
【００７３】
初期化ステップ（ｋ＝１）の場合、
最初の時点において固有加速度ａ１および磁気的外乱ｄ１が既知であり、その場合初期化ステップは状態ベクトルｘ１の決定を含んでいる。角速度は、最初の時点でゼロと仮定し、四元数は外乱ａ１およびｄ１の修正された加速および磁場測定値を用いる最適化により決定されるか、あるいは、初期状態における向きが既知であり、その場合にはａ１およびｄ１は導くことができる。
【００７４】
ｋ＝１で制御装置を初期化するステップ１００は以下を含んでいる。
・モデリングノイズ、測定ノイズ、および初期状態ベクトル推定誤差に各々関連付けられた共分散行列Ｑ、Ｒ、およびＰ１の設定、
【数７】
0005861235.tif 000008
と表記するバイアスｂの推定値の計算。姿勢制御装置は所定の時間、例えば約１秒間、静止状態に保たれ、各軸でのレートジャイロからの出力値の平均が計算される。その後、前処理ステップ２００において平均バイアス
【数８】
0005861235.tif 000009
の当該推定値がレートジャイロの各々測定値から減算されることにより、バイアスの影響を最小限に抑えて、得られた結果の精度を向上させることができる。
【００７５】
平均バイアス
【数９】
0005861235.tif 000010
の当該推定値は好適には、各々の取得開始時に生起する。静止期間中に当該推定値をリフレッシュすることも提案できる。
【００７６】
図２Ａ〜２Ｃに本発明による方法のステップの詳細を示す。
【００７７】
ステップ２００において、３個のセンサにより得られた測定値は、３個のステップ２１０、２２０、および２３０において前処理される。本方法を用いる状況および望まれる精度に応じて、多くの可能な変型実施形態があり得る。
【００７８】
第１のステップ２１０は、全ての実施形態を通じて同一である。図２Ｂに示すステップ２１０において、レートジャイロからの測定値ｙＧ，ｋが前処理される。上述のように、測定値ｙＧ，ｋの前処理は、真の測定値から平均バイアス
【数１０】
0005861235.tif 000011
を減算することにより得られ、時刻ｋにおけるレートジャイロからの前処理された測定値が出力として得られ、
【数１１】
0005861235.tif 000012
と表記される。
【００７９】
記述に際して、簡潔を旨として、加速度はＧ０（地球磁場）の倍数で与え、磁場をＨ０の倍数として与える。
【００８０】
第一の実施形態において、加速度外乱（ステップ２２０）および磁気的外乱（ステップ２３０）の存在を検知する２個のテストを並行して実行するのに利点がある。ステップ２２０において、時刻ｋにおいて加速度計ｙＡ，ｋにより得られた測定値が前処理される。ステップ２２０は、外乱すなわち固有加速度ａの有無を検知する第１のサブステップ２２０．１、および先行する時点ｋ−１で推定された向きに基づく加速
【数１２】
0005861235.tif 000013
の前処理された測定値を生成する第２のサブステップ２２０．２を含んでいる。
【００８１】
ステップ２２０．１において、固有加速度ａの有無を検知すべく、測定値ｙＡ，ｋのノルムが重力場（注：本方法はＧ０の倍数で機能する）のノルムと比較されるため、比較は１に対して行われる。すなわち、
【数１３】
0005861235.tif 000014
か否か。
【００８２】
利点として、上記テストの結果が肯定的である場合、次のテストが追加される。
【数１４】
0005861235.tif 000015
【００８３】
時刻ｋ−１において推定された固有加速度
【数１５】
0005861235.tif 000016
のノルムとβＡとの比較をうまく用いて、第１のテストが不十分である特定のケースを除外できる。実際には、時刻ｋ−１において固有加速度の値が高い（すなわちβＡより大きい）場合、時刻ｋで固有加速度がβＡを下回ることはなさそうであると仮定する。αＡとβＡは各々、例えば０．０４および０．２に等しい。
【００８４】
この第２のテストは従って、無外乱測定値
【数１６】
0005861235.tif 000017
の推定の精度、従って向きの推定の精度を向上させる。
【００８５】
上記２個のテストの結果が肯定的である場合、時刻ｋにおいて固有加速度
【数１７】
0005861235.tif 000018
がゼロであるとの決定がなされる。次いでステップ２２０．２において、推定された測定値がｙＡ，ｋに等しく、観測装置により直接使用することができる。
【数１８】
0005861235.tif 000019
【００８６】
一方、ステップ２２０．２において、先行する時点で推定された向き
【数１９】
0005861235.tif 000020
により新たな加速度測定値が生成される。
【００８７】
時刻ｋにおいて推定された無外乱加速度測定値は次いで、測定モデルを用いて次式で表される。
【数２０】
0005861235.tif 000021
【００８８】
時刻ｋにおける固有加速度の値
【数２１】
0005861235.tif 000022
もそこから導くことができる（ステップ２２０．３）ため、各々積分および二重積分により物体の速度および位置をそこから導くことが可能になる。
【００８９】
固有加速度は、次式に等しい。
【数２２】
0005861235.tif 000023
【００９０】
ステップ２２０と同様に、ステップ２３０において、時刻ｋで磁力計により得られた測定値ｙＭ，ｋが前処理される。このステップ２３０は、磁気的外乱ｄの有無を検知する第１のサブステップ２３０．１と、先行する時点ｋ−１において推定された向きに基づく磁場
【数２３】
0005861235.tif 000024
の前処理された測定値を生成する第２のサブステップ２３０．２とを含んでいる。
【００９１】
ステップ２３０．１において、磁気的外乱ｄの有無を検知するために、測定値ｙＭ，ｋのノルムを磁場（注：本方法はＨ０の倍数で機能する）のノルムと比較されるため、比較は１に対して行われる。すなわち、
【数２４】
0005861235.tif 000025
か否か。
【００９２】
利点として、磁場の外乱が存在しない旨のテストの結果が肯定的である場合、次のテストが追加される。
【数２５】
0005861235.tif 000026
【００９３】
時刻ｋ−１において推定された磁気的外乱
【数２６】
0005861235.tif 000027
のノルムとβＭとの比較を効果的に用いて、第１のテストが不十分である特定のケースを除外することができる。実際には、時刻ｋ−１において磁気的外乱の値が高い（すなわちβＭより大きい）場合、時刻ｋで磁気的外乱がβＭを下回ることはなさそうであると仮定する。αＭとβＭは各々、例えば０．０４および０．２に等しい。
【００９４】
この第２のテストは従って、無外乱測定値
【数２７】
0005861235.tif 000028
の推定の精度、従って向きの推定の精度を向上させる。
【００９５】
上記２個のテストの結果が肯定的である場合、時刻ｋにおいて磁気的外乱
【数２８】
0005861235.tif 000029
がゼロであるとの決定がなされる。次いでステップ２３０．２において、推定された測定値がｙＭ，ｋに等しく、観測装置により直接使用することができる。
【数２９】
0005861235.tif 000030
【００９６】
一方、ステップ２３０．２において、先行する時点で推定された向き
【数３０】
0005861235.tif 000031
により新たな磁場測定値が生成される。
【００９７】
時刻ｋにおいて推定された無外乱磁気測定値は次いで、測定モデルを用いて次式で表される。
【数３１】
0005861235.tif 000032
【００９８】
時刻ｋにおける磁気的外乱の値もそこから導くことができ（ステップ２３０．３）、次式に等しい。
【数３２】
0005861235.tif 000033
【００９９】
第２の実施形態は特に、向きの推定が大幅にぶれることが知られる場合に適用できる。これらの場合、加速度および磁気的外乱の存在に関するテストが実行される時間ウインドウを定義することに利点がある。
【０１００】
この変型例において、加速度測定値テストのために、時点ｔｋで終了するウインドウに対してステップ２２０における比較が実行される。固有加速度が検知された（ウインドウ[ｔｋ−ＴＡ；ｔｋ）の測定値の少なくとも１個に対してノルムＧ０とαＡの範囲内で異なる加速度測定値のノルム）場合、先行する時点で推定された向きにより無外乱加速度測定値が生成され、一方、無外乱加速度測定値は、前処理フェーズに関係する測定値（センサ測定値）に等しい。センサ測定値から固有加速度の値が計算される。単なる例として示すαＡおよびＴＡの典型的な値は０．２ｇおよび０．４ｓである。
【０１０１】
次いで、固有加速度が存在しないことのテストは次式で表される。
【数３３】
0005861235.tif 000034
【０１０２】
利点として、閾値を超えない場合であっても、第１の変型実施形態と同じ式により固有加速度を系統的に計算することができる。
【数３４】
0005861235.tif 000035
【０１０３】
次いで、または並行して（この第２のオプションは時間を節約できることが利点である）、ステップ２３０における磁気信号の外乱を検知するテストが実行される。磁気的外乱が検知された（ウインドウ[ｔｋ−ＴＭ；ｔｋ]の測定値の少なくとも１個に対してＨ０のノルムとαＭの範囲内で異なる磁気測定値のノルム）場合、先行する時点で推定された向きにより無外乱磁気測定値が生成される。一方、無外乱磁気測定値は前処理フェーズに関係する測定値（センサ測定値）に等しい。次いでセンサ測定値から磁気的外乱の値が計算される。単なる例として示すαＭおよびＴＭの典型的な値は各々０．１ｈおよび０．５ｓである。
【０１０４】
次いで、外乱が存在しないことのテストは次式で表される。
【数３５】
0005861235.tif 000036
【０１０５】
利点として、閾値を超えない場合であっても、第１の変型実施形態と同じ式により磁気的外乱を系統的に計算することができる。
【数３６】
0005861235.tif 000037
【０１０６】
これらの最初の二つの実施形態において、ステップ２０２、２０３が並列に実行されることに利点がある。
【０１０７】
他方、検知の精度を向上させることが必要であって、装置が三角関数を用いるのに充分な計算記憶手段を含んでいる場合、第３の実施形態を用いて検知の精度を向上させることができる。この場合、検知計算を並列に実行することで得られる利点が無くなり、固有加速度の存在に関するテストの後で磁気的外乱の存在に関するテストを実行する方が有利である。ステップ２０２の第１の計算の出力において、
【数３７】
0005861235.tif 000038
も計算され、ｕ０を用いてベクトルＧ０とＨ０の間で測定された角度を表す。このパラメータは初期化ステップ１００において計算することができる。次いで、磁気的外乱の検知に対するテストが以下のように実行される。磁気的外乱が検知される（ウインドウ[ｔｋ−ＴＭ；ｔｋ]の測定値の少なくとも１個に対してＨ０のノルムとαＭの範囲内で異なる磁気測定値のノルムまたはｕ０とαｕの範囲内で異なる角度ｕｋ）場合、先行する時点で推定された向きにより無外乱磁気測定値が生成される。一方、無外乱磁気測定値は前処理フェーズに関係する測定値（センサ測定値）に等しい。次いでセンサ測定値から磁気的外乱の値が計算される。利点として、固有加速度の有無に応じてＴＭの２個の異なる値（各々ＴＭ_ｆａｓｔおよびＴＭ_ｓｌｏｗ）が用いられる。単なる例として示すαＭ、αｕ、ＴＭ_ｆａｓｔ、およびＴＭ_ｓｌｏｗの典型的な値は各々０．１ｈ、１０°、０．５ｓ、および３ｓである。
【０１０８】
次いで、外乱が存在することのテストは次式で表される。
【数３８】
0005861235.tif 000039
【０１０９】
ステップ３００において、前処理された測定値
【数３９】
0005861235.tif 000040
が観測装置により使用される。例えば拡張カルマンフィルタが、その因数分解された形式で用いられる。
【０１１０】
ここで、フィルタにより実行される計算段階について説明する。
【０１１１】
第２のモデリングを用いていると仮定する。しかし、明らかに第１のモデリングを同様に用いることができる。
【０１１２】
ステップ３００は、以下のステップを含んでいる。
ａ）状態ベクトルの先験的な推定、
ｂ）測定値の先験的な推定、
ｃ）カルマンフィルタのゲインＫｋ、およびイノベーションＩｋの計算、
ｄ）先験的に推定された状態の修正。
【０１１３】
ステップａ）〜ｄ）について以下に詳述する。
【０１１４】
ステップａ）において、時刻ｋ−１における状態ベクトルの帰納的な推定値から、時刻ｋにおける状態ベクトルを先験的に推定する。
【０１１５】
先験的な状態ベクトルの推定は次式で与えられる。
【数４０】
0005861235.tif 000041
但し、
【数４１】
0005861235.tif 000042
：時刻ｋ−１における状態ベクトルの先験的な推定値、
【数４２】
0005861235.tif 000043
：時刻ｋにおける状態ベクトルの先験的な推定値、
Ｔｅ：サンプリング間隔。
【０１１６】
ステップｂ）において、測定モデル（ＩＩＩ）を用いて、ステップａ）で推定された状態ベクトルを用いた測定値の推定値を実行する。
【数４３】
0005861235.tif 000044
【０１１７】
ステップｃ）において、ゲインＫｋおよびイノベーションＩｋが計算される。イノベーションは、前処理された測定値から、先験的な推定された測定値を減算することにより得られる。
【０１１８】
次式が得られる。
【数４４】
0005861235.tif 000045
【０１１９】
ステップｄ）において、先験的な推定された状態がゲインおよびイノベーションにより修正される。
【数４５】
0005861235.tif 000046
【０１２０】
この修正は、時刻ｋにおける向き
【数４６】
0005861235.tif 000047
の先験的な推定値を与える。
【０１２１】
利点として、後続するステップｅ）において、推定された四元数が正規化される。
【数４７】
0005861235.tif 000048
これにより、計算の各ステップにおけるぶれを回避することが可能になる。
【０１２２】
本発明による方法は、測定モデルと整合した、無外乱測定値に近い測定値を観測装置に提供する利点をもたらす。従って、向きの推定に対する外乱の影響が大幅に減少する。また、各測定値の信頼性を一定に維持することにより、外乱がある場合であっても各センサから得られる情報を観測装置に永続的に提供することが可能になる。実際、加速度計、磁力計、およびレートジャイロからの測定値を組み合わせて用いることで、推定された向きに対する測定誤差（測定ノイズ、残留外乱、残留レートジャイロバイアス）の影響を減らすことが可能になる。
【０１２３】
本発明による方法は、最大９個の自由度で実行された運動によらず、向きだけでなく、各サンプリング間隔における固有加速度および磁気的外乱の推定を可能にする。本方法は基本的な構成ブロックすなわち数値テスト、解析的計算、拡張カルマンフィルタの使用に依存するため、極めて簡単に実装できる。
【図１】
0005861235.tif 000091
【図２Ａ】
0005861235.tif 000092
【図２Ｂ】
0005861235.tif 000093
【図２Ｃ】
0005861235.tif 000094