JP5563200B2 - 連接構造の動作をキャプチャするための処理方法 - Google Patents

Description

本発明は、連接構造の動作をキャプチャするための処理方法に関し、具体的には、連接構造の動作を特徴付けることが可能なシミュレートパラメータを得る方法に関する。また、本発明は、本発明によるシミュレートパラメータを得る方法を用いる連接構造の動作を再生成する方法に関する。

本発明の技術分野は、所定の基準座標系における連接構造の動作を記述する一つ以上のパラメータについて、又は動作中の連接構造に固有な、及び／又は動作中の連接構造に対する固有のパラメータの測定値に関連付けられたセンサのシステムに固有な一つ以上のパラメータについて、リアルタイムで、又は他の方法で、測定値を用いて決定することである。

非制限的な例として、本発明の方法は、以下の場合に有利に適用される。

−ロボット工学（その場合センサがロボット上に置かれる）、遠隔操作（その場合センサが人間又は動物上に置かれる）、装置較正等のような産業用途
−例えばユーザとの相互作用を要求するビデオゲーム等のマルチメディア用途
−シミュレーション用途（例えばフライトシミュレーション）
−合成画像の生成及びビデオゲームの編集
−医療用途
−生物力学
いくつかの周知のシステムにより、動く物体の動作に対する全部又は一部パラメータの推定値を得ることができる。例えば、Vicon（登録商標）システム、Optotrak（登録商標）システム、Intersense（登録商標）システム、POLHEMUS（登録商標）システム等が挙げられる。全てのこれらシステム、特にビデオタイプの光学システムは、高い空間定数をユーザに課すのが欠点である。これは、ユーザが獲得量（カメラフィールド、磁場の量、超音波源からの放射量等）の中に留まってしまうからである。

他のシステムは、この欠点がない。例えば、Xsense Technologies（登録商標）は、加速度計と磁気計とジャイロメータとを含む内部装置を用いて任意の空間制限から開放することができる。

また、文献“Implementation and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking”（２００５年４月、スペイン、バルセロナ、2005 IEEE International Conference on Robotics and Automationの議事録；Xiaoping Yun, Conrado Aparicio, Eric R. Bachmann, Robert B. McGhee）は、加速度計と磁気計と３軸ジャイロメータとを含む内部装置を用いるシステムを開示する。内部装置とは別に、システムは、連接本体の形状モデルとセンサモデルとを用いる。センサモデルは、測定される加速度が重力に起因するのみであるとみなされる静的モデルである。また、各センサの方位は、他のセンサによって測定された量で別個に再構成され、使用されるフィルタリングは、ジャイロメータに全て依存する。

ジャイロメータの使用は欠点を呈する。これは、ジャイロメータの使用により、連続的な素早い動作の間にバイアスを生じさせ、結果として時間と共に精度を欠くためである。また、ジャイロメータは、高い電力消費を伴う高価なセンサである。

他のシステムも従来技術から知られている。文献“Real-time Motion Capture for a Human Body using Accelerometers”（Robotica(2001), Volume 19, pp.601-610; Jihong Lee及びInsoo Ha）は、段階的に本体の加速度を計算することのできる、加速度計と動く本体の連接モデルとを原則的に用いるシステムを開示する。この方法は、少なくとも一つのセグメントにおける並進加速度と回転加速度とを認識しなければならない。本発明は上記欠点がない。

このため、本発明は、互いに連接された複数のセグメントからなる連接構造の動作を基準座標系で特徴付けることができる少なくとも一つのシミュレートパラメータを得る方法に関し、少なくとも２つのこれらセグメントは各々、測定センサを備え、上記方法は：
−前記連接構造の動作を記述する推定動作状態パラメータの全部又は一部から前記連接構造の機械的モデルと測定センサモデルとを計算する過程であって、各推定測定データ項目は、センサによって与えられる測定値を示すデータ項目であり、少なくとも一つの推定測定データ項目は、第１測定センサを与えるセグメントの少なくとも一つの推定動作状態パラメータと、前記第１測定センサを与える前記セグメント以外の少なくとも一つのセグメントの少なくとも一つの推定動作状態パラメータとを組合せる項を具備する式の形式で表現される、過程と（これらの項は通常、本明細書で以下“交差項”と称される）、
−少なくとも２つの異なる測定センサに関して、前記測定センサによって与えられる測定値と、前記与えられた測定値を示す前記推定測定データとの差を計算する過程と、
−前記差から発生し、前記項を考慮に入れることができるオブザーバタイプの処理であって、推定動作状態パラメータの少なくとも一つの推定差を与える、包括的な数学的処理過程と、
−前記推定動作状態パラメータと、それに相当する前記推定動作状態パラメータとに関する推定差を加算して、シミュレートパラメータを形成する過程とを含む。

また、本発明は、連接構造の動作を特徴付けることができるシミュレートパラメータから連接構造の動作を再生成する方法に関し、シミュレートパラメータは、本発明の方法によって得られることを特徴とする。

本発明の方法によって解決される一つの課題は、任意の連接構造、例えば動く人体や重力の大きさに比べて大幅な加速のある動作の素早い局面を有する動作を再構成することにある。本発明の方法により有利に、センサが単独で置かれたシステムのセンサに対する動作、又はそのシステムのうち任意の一部に対する動作を、そのセンサが与える測定値から認識できない場合、動作を再生成することができる。

本発明の方法は、高コストな機器であるジャイロメータの使用を省く低コストな方法であり有利である。

本発明の方法は、加速度計及び磁気計を用いるのが好ましい。また、連接構造の必要に応じて選択されたＮ個のセンサを有利に含む。故に、連接構造のセグメント毎に一つのセンサを必要としない。

図１は、本発明の方法を実行するためのシステムを示す。システムは、構造２上に割当てられた一組のセンサ１であってその全て又は一部の動作（例えば人体）に対するキャプチャがシークされるセンサと、コンピュータ３と、ディスプレイ装置４とを含む。重力場ベクトルＧ及び磁場ベクトルＨは、構造２が動作する空間を満たす。センサ１は、コンピュータ３と距離を置いて通信し、その測定データを送信する。測定データは、予め選択されたモデルと包括的な処理方法とを用いてコンピュータ３で処理され、包括的な処理方法は、構造の全て又は一部の動作を特徴付けることができるシミュレートパラメータを決定し、及び／又は構造の全て又は一部の動作を再構成する。センサ１は、例えば加速度計及び磁気計である。

本発明の方法は、全て又は一部のセンサ１によって与えられる測定値と、キャプチャ対象の動作をする構造の機械的モデルと、センサモデルと、センサによって与えられる測定値の包括的な処理とを組合せた使用によって特徴付けられる。

いくつかの異なる機械的構造モデル及び／又はいくつかの異なるセンサモデルは、有利に使用可能である。本発明の好ましい実施形態によると、動的構造モデルと動的センサモデルとで選択が行われる。そのような動的モデルは、物理的現実により近い。それらにより、シークされる未知数をより正確に検出するだけでなく、さらに未知数を検出することができる。動的モデルよりも単純なモデルが、本発明との関連で、特に例えば遅い動作をキャプチャするために使用可能である。

本発明の方法は、観測される構造（人体、動物、ロボット）の状態を再構成することに至る。観測される構造の“状態”は、例えば構造の位置、速度、若しくは加速度、構造の機械的モデルのパラメータ、又は全て又は一部のこれら各種要素の任意の組合せを意味する。本発明の方法により再構成可能なこれら状態は、一般に“動作の状態パラメータ”というタームによって、本明細書で以下に言及される。

本発明の処理方法は、３つの必須要素を組合せた使用に基づく。第１の要素は、構造の機械的モデルによって与えられる。“構造の機械的モデル”は、観測対象の連接構造に対する形状モデル、運動モデル、動的モデル、又はこれら３つのモデルからなる任意の組合せを意味する。機械的モデルにより、モデルの各セグメントの状態を構造の全体的な状態に関連付けることができる（例えば、速度及び加速度が全体的な状態の一部を形成する場合に速度及び加速度の組合せが考えられる）。機械的モデルは、例えば、連接構造を構成するセグメントの長さ、構造の各種構成要素の重量、これら各種要素の慣性特性等のような、既知又は未知である所定数のパラメータからなることができる。一般論として、機械的モデルは、動作に関して得られたパラメータを体の感覚のみに限定するように、構造に固有な特殊性を含む。例えば、人体の場合、肘の関節部分の存在（回転できない）は、優先的にモデルに取込まれる。

第２の要素は、センサモデルである。センサのモデルは、基準座標系でセンサを特徴付ける各種パラメータ（方位、位置、速度、加速度等）に従って、センサの各種測定データを示す関数である。本発明の好ましい実施形態によると、センサは、加速度計及び磁気計である。加速度計に対して、センサモデルは、選択されたパラメータに従って加速度の測定の推定値を結果として与える。磁気計に対して与えられるのは、選択されたパラメータによる磁場の測定の推定値である。

本発明と関連して他の種類のセンサを、加速度計及び磁気計に追加するために、又は全て又は一部のそれらに代えるために使用することができる。センサの選択は、感知対象となる動作の種類に依存する。加速度計、磁気計、ジャイロメータ、力センサ、角度計、及びＧＰＳセンサのうち、少なくとも一つのセンサが選択される（ＧＰＳは、“全地球測位システム”を表す）。

構造の機械的モデル及びセンサモデルは、単一モデルに統合できる。そして、この単一モデルは、各センサが測定する推定値（推定された測定値）を各システムの全体的な状態に関連付ける。

構造の機械的モデルとセンサモデルとの関連付けにより、“交差（cross）”項を測定の推定値の表現で現すことができる。“交差”と称される項は、インデクス“ｉ”のセグメントによって与えられるインデクス“ｉ”のセンサを考える時、インデクス“ｉ”とは異なる“ｊ”である少なくとも一つのセグメントに関するパラメータを含む項である。その時、インデクス“ｉ”のセンサは、交差項で現れるインデクス“ｉ”及び“ｊ”のセグメントの動作に関する情報を与える。

図２は、交差項の影響を示す。図２は、基準座標系Ｒ_０（Ｘ、Ｙ、Ｚ）で動作可能な２つの連接セグメントＳａ及びＳｂを示す。重力場ベクトルＧ及び磁場ベクトルＨは、基準座標系Ｒ_０を満たす。セグメントＳａは、センサＣａ、例えば３軸加速度計を備え、セグメントＳｂは、センサＣｂ、例えば３軸加速度計を備える。基準座標系Ｒ_ａ（Ｘａ、Ｙａ、Ｚａ）は、セグメントＳａにリンクされ、基準座標系Ｒ_ｂ（Ｘｂ、Ｙｂ、Ｚｂ）は、セグメントＳｂにリンクされる。平面Ｙ＝０に固定される連接要素Ｐ１により、セグメントＳａは、平面（Ｘ、Ｙ）で旋回することができ、セグメントＳａ及びＳｂを接続する連接要素Ｐ２により、セグメントＳｂは、セグメントＳａに対して平面（Ｘ、Ｙ）で旋回することができる。角度ａ１は、基準座標系Ｒ_０におけるセグメントＳａの旋回運動を示し、角度ａ２は、基準座標系Ｒ_ａにおけるセグメントＳｂの旋回運動を示す。故に、センサＣｂの測定値が角度ａ２だけでなく角度ａ１によっても影響されることを明らかに現す。センサＣｂの測定値の表現において、角度ａ１、角速度ｄａ１／ｄｔ、及び角加速度ｄ^２ａ１／ｄｔ^２を含む全ての項は、交差項である。

第３の要素は、モデルデータ（構造モデルだけでなくセンサモデル又は単一に統合された構造／センサモデル）から、全ての未知数を同時にシークする処理方法である。この方法は、モデルから計算される測定推定値と実際の測定値とを分離する距離を最小にする。実際の測定値は、予め記録されるか又は継続的に記録されうる。それらは、完全又は不完全な場合がある。処理方法は、それ自体既知であるオブザーバタイプの包括的な数学的方法（global mathematical method）である。本発明の好ましい実施形態によると、処理方法は、拡張カルマンフィルタリングである。また、例えば制約最適化又は非制約最適化方法、粒子フィルタリング、スライディングホリゾン（sliding horizon）オブザーバタイプの方法、又はさらに一般的な任意の線形若しくは非線形オブザーバタイプの方法等、任意のその他包括的なデータ処理方法が使用可能である。包括的なデータ処理方法の使用（データ及び測定データのモデリング）は、上記説明した交差項を考慮するのに必要である。

上記説明した従来技術の方法において、センサモデルは、近似されたモデルであり、例えばジャイロメータ測定値等の追加情報を提供してこのモデルを完成するのに必要である。この補足情報によって達成されるこの近似されたモデルを用いて、センサの方位を決定することができるので、（近似に関する誤差を備えた）センサを与えるセグメントの方位を決定することができる。ここで、センサのみによって測定されたデータの処理は、この場合、不足情報（動作に関する加速度）が他のタイプの測定値によって（加速度計又は磁気計によってではなく）提供されるので、その時に任意の問題を呈しない点が重要である。故に、連接構造とセグメント間に存在する接続タイプとが何であれ、連接チェーンの動作を検出できる。

本発明の場合、モデルに対して任意の近似をすることなく、及び他のタイプの測定値を用いることなく、交差項を明らかにするモデリングが勧められる。交差項は、問題の解決に必要な追加情報を含む。交差項は、いくつかのセンサが存在する時のみ存在し、故に本発明の方法は、分離されたセンサの方位を正確に決定することができない。また、交差項に関するデータは、セグメント間の連接タイプに依存する。

検討すべき所定のシステムの複雑性により、本発明の方法は、“観測性テスト”と称されるテストの改善を提供する。観測性テストは、センサの特性とその構造上の割当形状とが連接動作の正しい再構成をもたらすことが可能か否かを判断することができる。選択されたセンサと割当形状とに対して、機械的モデルとセンサモデルとに依存することにより、このセンサ構成と割当形状とによって得られた情報が、判断対象である動作のパラメータを識別できるか否かが、その時設定される。標準的な数学的方法の一例は、以下に与えられる。

本発明の処理方法は有利に、各セグメント上にセンサを設置する必要がないことを可能にする一方で、本発明の方法により得られた動作の感知が構造内の全セグメントの動作に対する決定をもたらすのに、十分確実なものである。

本発明の方法は、追加的測定値の使用を必要とせず、かつモデルに対して近似をしないので、動作に対する推定値における誤差の出現を有利に回避する。

図３は、本発明の方法を実行するための第１のフロー図を示す。予測モデル５は、最初に設定される。予測モデルは、連続時間ｔ−１及びｔ間で観測される未知数（状態及び／又はパラメータ）に対する変更モデルである。故に、予測モデルは、状態及び／又は状態の値の関数として時間ｔにおけるシステムを定義するパラメータ及び／又は時間ｔ−１におけるパラメータの推定値を与える関数である。システムの状態は、動作の状態パラメータによって時間ｔで定義され、機械的モデルのパラメータ又は上記パラメータの組合せとして先に定義された位置ｐｏｓ（ｔ）、速度ｖｉｔ（ｔ）、加速度ａｃｃ（ｔ）、及びパラメータｐａｒ（ｔ）である。その時、以下の通りである。

[pos(t), vit(t), acc(t), par(t)]=f[pos(t-1), vit(t-1), acc(t-1), par(t-1)]
予測モデル５の出力には、連接構造の動作を記述する、一組の推定動作状態パラメータ

がある。その後、これら推定パラメータは、構造の機械的モデル６に入力される。本発明に対する改良によると、機械的モデル６は、“演繹的構造知識データ”と称されるデータ１２によって補足され、そのデータは、構造（例えば、セグメントの次元及び重量、慣性行列等）を記述するか、又は構造によって実行される全て又は一部の動作（位置及び／又は速度及び／又は加速度情報）を示す。機械的モデル６は、センサモデル７に入力されるセンサに関する方位、位置、速度、及び加速度データを与える。本発明に対するもう一つの改良によると、センサモデル７は、“演繹的センサ知識データ”と称されるデータ１３によって補足され、そのデータは、センサの特性（例えば、感度、補正、ドリフト等）を記述するか、又はセンサによって実行される全て又は一部の動作を示す。センサモデル７は、モデルによって提供される方位及び／又は位置及び／又は速度及び／又は加速度を後者が有する時、センサの測定値を示す推定データを与える。上記説明の通り、機械的モデル及びセンサモデルは、単一のモデルに統合できる。この単一モデルは、図３に８で参照される。センサモデル７によって、又は単一モデル８によって与えられる推定データは、例えば時間ｔにおける磁場測定の推定値

と、時間ｔにおける加速度測定の推定値

とである。その後、これら測定の推定値と、センサによって与えられる実際の測定値とが比較モジュール９で比較される。比較結果は、比較された量の間の差を構成する。その後、この差は、例えば拡張カルマンフィルタリングモジュール、制約最適化又は非制約最適化モジュール、粒子フィルタリングモジュール、スライディングホリゾンオブザーバタイプの処理モジュール等の、数学的なオブザーバタイプの包括的な処理モジュール１０に入力される。モジュール１０は、推定動作状態パラメータの推定差δｐｏｓ（ｔ）、δｖｉｔ（ｔ）、δａｃｃ（ｔ）、δｐａｒ（ｔ）を与え、差は、それらに相当する各種推定パラメータを訂正することを目的とする。その後、加算演算器１１は、推定データ

を第１の一連の入力上で受信し、推定差δｐｏｓ（ｔ）、δｖｉｔ（ｔ）、δａｃｃ（ｔ）、δｐａｒ（ｔ）を第２の一連の入力上で受信し、シークされたシミュレートデータ

を出力として与え、そのシミュレートデータは、構造の動作を再構成することができる。

その時、以下のようになる。

その後、データ

は、予測モデル５に送信され、後続時間において新たな推定パラメータになる。

図４は、第２の実施形態による本発明の方法を実行するためのフロー図を示す。第２の実施形態によると、予測モデルがなく、その後、推定パラメータ

は、機械的モデル６に直接入力される。その後、機械的モデルに補足推定データ１２を小出しに有利に入力することができる。さらに方法は、第１の実施形態で説明した方法と同じである。

本発明の方法による動作キャプチャに対する特定の例は、図５を参照して以下に説明される。図５は、ロトイド（rotoid）接続によって共に接続された２つのセグメントＳ１及びＳ２からなる連接構造（自由度１）を基準座標系Ｒ_ｅｘｔ（Ｘ、Ｙ、Ｚ）で示す。基準座標系Ｒ１（ｘ１、ｙ１、ｚ１）は、セグメントＳ１にリンクされ、基準座標系Ｒ２（ｘ２、ｙ２、ｚ２）は、セグメントＳ２にリンクされる。センサＣ１は、セグメントＳ１に固定され、センサＣ２は、セグメントＳ２に固定される。各センサＣ１、Ｃ２は、３軸加速度計及び３軸磁気計を含む。故に、上記システムは、セグメントＳ１及びＳ２間の連接に対して１の自由度を有し、２つのセグメントＳ１、Ｓ２によって形成される全体の構造に対して回転及び並進で６の自由度を有する。故に、７つの未知数を決定するのが必要である。

構造の動作に対するシミュレーションは、以下のように与えられる。セグメントＳ２は、一定の角加速度で基準座標系Ｒ２の垂直軸ｙ２について回転し、セグメントＳ１は、さらに一定の角加速度で基準座標系Ｒ１の回転軸ｘ１について回転する。システム全体は、加速度ＡＴ_ｚによって定義される並進を実行する。

センサＣに対して、加速度

及び磁場

の測定の推定値（モデルの式）は、以下の式１１によってセンサＣの基準座標系で与えられる。

ここで、
−ベクトルａ_ｃ、ベクトルＧ、及びベクトルＨはそれぞれ、センサの加速度、重力による加速度、及び外部基準座標系Ｒ_ｅｘｔで示される外部磁場であり
−^ＣＲ_ｅｘｔは、センサにリンクされた基準座標系と、基準座標系Ｒｅｘｔとの間の回転行列である。

センサにリンクされた基準座標系と外部基準座標系との間の回転及び並進を示す方法は、連接チェーンの形状モデルの式である。回転^ＣＲ_ｅｘｔは、連接チェーンのパラメータに従って結果的に分解（break down）される。その後、各センサＣ１及びＣ２に対して、以下の式１２で共に収集された以下の測定推定値が得られる。

ここで、項

は、基準座標系Ｒ１における加速度の測定値と磁場の測定値との推定値であり、項

は、基準座標系Ｒ２における加速度の測定値と磁場の測定値との推定値であり、ベクトルθ１及びベクトルθ２はそれぞれ、Ｒ_ｅｘｔの基準座標系におけるセグメントＳ１の回転ベクトルであり、セグメントＳ１に対するセグメントＳ２の回転ベクトルである。

ベクトルθ１は、基準座標系Ｒ_ｅｘｔの軸Ｘ、Ｙ、及びＺ上で３つの回転角θ１_ｘ、θ１_ｙ、θ１_ｚに分解される。

センサＣによって与えられる測定値は、センサの基準座標系Ｒ_ｃで示される。各センサＣ１及びＣ２によって測定される加速度ベクトルａ_ｃ１及びベクトルａ_ｃ２の式は、故に以下の通りである。

ここで、シンボル“Λ”は、“ベクトル積”演算子を示し
−ベクトルＡ_Ｔの項は、全連接構造に関する並進加速度を示し
−項

はそれぞれ、時間に関する量であるベクトルθの１次導関数及び２次導関数を示し
−ベクトル（０_ｉＣ_ｊ）は、基準座標系Ｒｉ（ｉ＝１、２）におけるセンサＣ_ｊの位置ベクトルである。

選択された例において、本発明の方法に必須の要素である交差項は、センサ２の測定値を示すために、変数

と、ベクトルＡ_Ｔとを含む項である。全ての動作のパラメータを同時にシークすることで２つのセンサＣ１及びＣ２を同時に使用して、故にセンサ２から入力される測定値を用いてセグメントＳ１のパラメータに関する情報を回復することができる。

拡張カルマンフィルタを用いてシステムの未知数をシークする。ベクトルＩは、未知数で形成され、前記ベクトルは、３つの基本ベクトル、即ち位置ベクトル、速度ベクトル、及び加速度ベクトルからなる。

位置ベクトルは、空間内の並進位置（セグメントＳ１に対する３の並進自由度Ｔ_ｘ、Ｔ_ｙ、Ｔ_ｚ）と、角度位置（セグメントＳ１に対する３の回転自由度θ_１ｘ、θ_１ｙ、θ_１ｚ及び連接の回転自由度θ_２ｘ）とからなる。

速度ベクトルは、位置ベクトルの時間に関する一次導関数である。故に、速度ベクトルは、並進速度及び角速度を含む。

加速度ベクトルは、位置ベクトルの時間に関する二次導関数である。故に、加速度ベクトルは、並進加速度（式１２の変数Ａ_Ｔｘ、Ａ_Ｔｙ、及びＡ_Ｔｚ）と角加速度とからなる。

このベクトルは、ベクトルＩ＝［ベクトルpos、ベクトルvit、ベクトルacc］形式に置くことができ、ここでベクトルposは、位置ベクトルであり、ベクトルvitは、速度ベクトルであり、ベクトルaccは、加速度ベクトルである。上記式１１及び１２の何れも、並進位置又は並進速度の関数であるモデルを記述しないことに注意すべきである。これら２つの量は、測定値に影響を与えないので、不必要である（また、カルマンフィルタによって観測できない）。しかし、そのように保持することで、システムの状態に関して展開行列（evolution matrix）の記載が容易になるという利点がある。

そして、システムの力学は、以下の式１９のシステムを用いて記載される。

ここで、ｋは現在の繰返しのインデクス、Ｉはシステムの状態、Ａはシステムの状態に関する展開行列、ωは方法のノイズ、ｍは測定値、ｈは測定関数、及びｔは測定ノイズを示す。

上記図３を参照すると、行列Ａを構成するデータは、予測モデル５に入力される。図４を参照すると、行列Ａは、単位行列であり、従ってフロー図には示されない。

ここで、行列Ａは、以下のように定義され、Δｔは、時間ｔ_ｋ＋１から時間ｔ_ｋを分離する期間である。

上記関数ｈにより、各センサの測定の推定値を計算することができ、この関数は以下のように記載される。

線形システム上で作用するカルマンフィルタは、非線形システムへ拡張され、本発明に関して使用される（拡張カルマンフィルタリング）。このため、上記システムの関数ｈのヤコビアンが計算される。その後、拡張カルマンフィルタの式は、全ての利用可能な情報を考慮することにより、測定の推定値と実際の測定値との誤差を最小にする以下の繰返しを求める。

図６、７Ａ〜７Ｄ、８Ａ〜８Ｄ、９Ａ〜９Ｄは、本発明の方法により、図５に示されたシステムに関して得られた結果を示す。

動作

の推定加速度の値と、時間ステップΔｔの値とが以下のようになる（位置及び速度値は、動作の開始時に０である）。

また、ゼロ平均と分散０．０５とのガウスノイズはその後、全ての測定推定値に追加される。

ここで、観測性テストを実行して、センサ及びその割当形状の特性により連接動作の正しい再構成が可能であることを確認することができる。このテストを実行するための一つの数学的方法は、以下に説明される。J_h(pos, vel, acc)を測定関数h(pos, vel, acc)のヤコビアン行列にすると、行列Ｍは、以下のように構成される。

ここでｎは、行列Ａの次元である。

行列Ｍが最大階数の場合、その時センサ及びその割当形状の特性は、連接動作の正しい再構成を可能にするといえる。

センサ１は、加速度計である。図６は、時間ステップΔｔの増加に従い、センサ１によって与えられる重力ベクトルｇの加速度と動作の加速度とからなる、基準座標系Ｒ１におけるベクトル要素の測定値の曲線である。曲線ｇ_ｘは、Ｘ１に平行する加速度要素を示し、曲線ｇ_ｙは、Ｙ１に平行する加速度要素を示し、曲線ｇ_ｚは、Ｚ１に平行する加速度要素である。

各図７Ａ〜７Ｄ、８Ａ〜８Ｄ、及び９Ａ〜９Ｄは、本発明の方法の終わりに与えられた値（与えられたシミュレーション値）と、それに相当する理論値（参照値）との間の、時間を関数とする比較曲線を示す。

図７Ａは、与えられた推定値における時間を関数とする変化（曲線ａ１）と角度θ_２ｘの参照値（曲線ａ２）とを示す。測定値と推定測定値との間のほぼ完全な一致のため、一つの曲線のみが図７Ａに現れる。

図７Ｂは、与えられた推定値における時間を関数とする変化（曲線ｂ１）と角度θ_１ｚの参照値（曲線ｂ２）とを示す。

図７Ｃは、与えられた推定値における時間を関数とする変化（曲線ｃ１）と角度θ_１ｙの参照値（曲線ｃ２）とを示す。測定値と推定測定値との間のほぼ完全な一致のため、一つの曲線のみが図７Ｃに現れる。

図７Ｄは、与えられた推定値における時間を関数とする変化（曲線ｄ１）と、時間を関数とする角度θ_１ｘの参照値（曲線ｄ２）とを示す。

図８Ａは、与えられた推定値における時間を関数とする変化（曲線ｅ１）と、角速度

の参照値（曲線ｅ２）とを示す。

図８Ｂは、与えられた推定値における時間を関数とする変化（曲線ｆ１）と、角速度

の参照値（曲線ｆ２）とを示す。

図８Ｃは、与えられた推定値における時間を関数とする変化（曲線ｇ１）と、角速度

の参照値（曲線ｇ２）とを示す。

図８Ｄは、与えられた推定値における時間を関数とする変化（曲線ｈ１）と、角速度

の参照値（曲線ｈ２）とを示す。

図９Ａは、与えられた推定値における時間を関数とする変化（曲線ｉ１）と、角加速度

の参照値（曲線ｉ２）とを示す。

図９Ｂは、与えられた推定値における時間を関数とする変化（曲線ｊ１）と、角加速度

の参照値（曲線ｊ２）とを示す。

図９Ｃは、与えられた推定値における時間を関数とする変化（曲線ｋ１）と、角加速度

の参照値（曲線ｋ２）とを示す。

図９Ｄは、与えられた推定値における時間を関数とする変化（曲線ｌ１）と、角加速度

の参照値（曲線ｌ２）とを示す。

図６、７Ａ〜７Ｄ、８Ａ〜８Ｄ、及び９Ａ〜９Ｄの全図から明らかなように、本発明の方法により与えられる推定値は、所望の理論値にかなり近接しており、本発明のシミュレーション方法が適切であることを保証する。

類似の方法で、図１０Ａ、１０Ｂ、及び１０Ｃにおいて、時間を関数とする変化は、与えられた推定値に見られ（各曲線ｍ１、ｎ１、ｏ１）、並進加速度ＡＴｘ、ＡＴｙ、及びＡＴｚの参照値に見られる（各曲線ｍ２、ｎ２、ｏ２）。

図１は、本発明の方法を実行するシステムを示す。 図２は、本発明によるセンサを備えた連接本体の一例を示す。 図３は、第１の実施形態による本発明の方法を実行するフロー図を示す。 図４は、第２の実施形態による本発明の方法を実行するフロー図を示す。 図５は、本発明の方法が動作キャプチャを生ずることができるセンサを備えた連接本体の一例を示す。 図６は、図５に示す連接本体によって与えられた加速度測定値を示す。 図７Ａは、図５の例で示した本体の動作をキャプチャする座標及び推定方位角度を示す。 図７Ｂは、図５の例で示した本体の動作をキャプチャする座標及び推定方位角度を示す。 図７Ｃは、図５の例で示した本体の動作をキャプチャする座標及び推定方位角度を示す。 図７Ｄは、図５の例で示した本体の動作をキャプチャする座標及び推定方位角度を示す。 図８Ａは、図５の例で示した本体の動作をキャプチャする座標及び推定角速度を示す。 図８Ｂは、図５の例で示した本体の動作をキャプチャする座標及び推定角速度を示す。 図８Ｃは、図５の例で示した本体の動作をキャプチャする座標及び推定角速度を示す。 図８Ｄは、図５の例で示した本体の動作をキャプチャする座標及び推定角速度を示す。 図９Ａは、図５の例で示した本体の動作をキャプチャする座標及び推定角加速度を示す。 図９Ｂは、図５の例で示した本体の動作をキャプチャする座標及び推定角加速度を示す。 図９Ｃは、図５の例で示した本体の動作をキャプチャする座標及び推定角加速度を示す。 図９Ｄは、図５の例で示した本体の動作をキャプチャする座標及び推定角加速度を示す。 図１０Ａは、図５の例で示した本体の動作をキャプチャする座標及び推定並進を示す。 図１０Ｂは、図５の例で示した本体の動作をキャプチャする座標及び推定並進を示す。 図１０Ｃは、図５の例で示した本体の動作をキャプチャする座標及び推定並進を示す。

Claims (9)

1. 互いに連接される複数のセグメント（Ｓ１、Ｓ２）からなる連接構造の動作を基準座標系（Ｒ_ｅｘｔ）において再構成する方法であって、少なくとも２つの前記セグメントは各々、測定センサ（Ｃ１、Ｃ２）を備え、前記方法は、
   −前記連接構造の状態パラメータを再構成する過程であって、前記状態パラメータ
   

   

   は、前記連接構造の動作を記述する、過程と、
   −時間ｔ−１の状態パラメータの関数として時間ｔの状態パラメータの推定値を与えるために、予測モデルを使用する過程と、
   −各測定センサの測定値の推定値を与えるために、連接構造の機械的モデルと測定センサモデルと時間ｔの状態パラメータの前記推定値とを使用する過程と、
   −測定値の前記推定値と実際の測定データとの間の誤差を最小化する処理方法を使用して、前記状態パラメータの推定差を与える過程と、
   −時間ｔの更新された状態パラメータを与えるために、状態パラメータの前記推定値に状態パラメータの前記推定差を加算する過程と
   を具備し、
   セグメントに関する少なくとも１つの測定センサの測定値の推定値は、交差項を含み、前記交差項は、前記測定センサを含むセグメントとは異なるセグメントの少なくとも１つの状態パラメータを含むことを特徴とする方法。
2. 前記機械的モデルは、前記構造に関する演繹的知識データによって達成されることを特徴とする請求項１に記載の方法。
3. 前記測定センサモデル（７）は、前記センサに関する演繹的知識データ（１３）によって達成されることを特徴とする請求項１または２に記載の方法。
4. 測定値の前記推定値と実際の測定データとの間の誤差を最小化するアルゴリズムは、拡張カルマンフィルタリング、制約最適化、非制約最適化、粒子フィルタリング、又はスライディングホリゾンオブザーバタイプの処理であることを特徴とする請求項１〜３のうち何れか１項に記載の方法。
5. シミュレートされた状態パラメータが得られるように、前記測定センサが、互いに連接される前記セグメント上に割当てられているかを判断する予備的な観測性テストをさらに具備することを特徴とする請求項１〜４のうち何れか１項に記載の方法。
6. 少なくとも一つの測定センサは、加速度計、磁気計、ジャイロメータ、力センサ、角度計、又はＧＰＳセンサから選択されることを特徴とする請求項１〜５のうち何れか１項に記載の方法。
7. 前記連接構造の前記機械的モデルと前記センサモデルとは、動的モデルであることを特徴とする請求項１〜６のうち何れか１項に記載の方法。
8. 前記連接構造の前記機械的モデルと測定センサモデルとは、前記連接構造と前記測定センサとから形成されるシステムの全体的状態にセンサの測定の推定値を関連付ける単一モデルに統合されることを特徴とする請求項１〜７のうち何れか１項に記載の方法。
9. 前記状態パラメータは、各セグメントの位置、速度、および加速度を含むことを特徴とする請求項１〜８のうち何れか１項に記載の方法。

Images