JP6147446B1 - ソフト制約及びペナルティ機能を使用した慣性センサの初期化 - Google Patents

Abstract

センサが休止状態において又は周知の場所若しくは向きにおいて開始することを必要とすることなしに、開始向き及び速度を推定する慣性センサ用の初期化方法。初期化は、初期化期間中においてほぼ守られると予想されるソフト制約の組としてエンコードされた動きのパターンを使用している。ソフト制約からの算出された動きの軌跡の逸脱を計測するため、ペナルティ計測基準が定義される。慣性センサ用の動きの微分方程式が、変数としての初期状態によって解かれ、ペナルティ計測基準を極小化する初期状態が、センサの実際の初期状態の推定値として使用される。特定用途用のソフト制約及びペナルティ計測基準は、その用途について予想される動きのパターンのタイプに基づいて選択される。例示的なケースは、初期化の際に相対的にほとんど運動がない用途と、初期化の際にほぼ周期的な動きを有する用途とを含む。

Description

１つ又は複数の実施形態は、動きのキャプチャと、ユーザー又は装置と結合されうる動きのキャプチャと関連したセンサの初期化とに関する。更に詳しくは、但し、限定を伴うことなしに、１つ又は複数の実施形態は、慣性センサ装置の初期状態を推定するためのソフト制約及びペナルティ機能を使用した慣性センサの初期化を可能にする。初期化は、一般に、初期化時間インターバル中においてこの装置からキャプチャされたデータと、このインターバル中におけるこの装置の動きに対するソフト制約の組とに基づいて、ある時点において実行される。

慣性センサは、一般に、動きの追跡及びナビゲーションのために使用されている。これらは、外部入力又は基準に対するニーズを伴うことなしに、物体の動きを推定するために使用することができるという利点を提供している。これらのシステムは、通常、線形の力を計測するための加速度計と、回転の動きを計測するためのジャイロスコープとを含む。

このようなシステムは、船舶及び航空機用のナビゲーション支援装置として、多年にわたって使用されている。更に最近では、廉価なＭＥＭＳセンサが広く入手可能となったことに伴って、慣性センサは、スマートフォンなどの消費者製品に益々組み込まれるようになっている。

純粋な慣性センサは、均一な線形運動を検出することができず、これは、基本的に、相対原理の結果である。また、これらは、絶対位置又は向きを判定することもできない。事実上、慣性センサは、位置及び向きの変化の追跡に使用することができる。動きの追跡又はナビゲーションの装置として、これらは、既知の開始状態から、且つ慣性センサによって検出された変化から新しい位置及び向きを推定するため、推測航法技法に依存している。

従って、慣性センサを使用した用途は、慣性データを介して位置及び向きの変化を計測する前に、センサの初期状態を判定するため、なんらかの方法を使用しなければならない。既存のシステムは、一般に、この初期化のために、２つの方式のうちの１つ又は両方を使用している。

慣性センサの初期化の１つの既存の方式は、センサを既知の位置及び向きに配置し、且つ動きの追跡を開始する前に、センサが、なんらかの期間にわたって（線形又は回転の動きを伴わない）休止状態にあることを保証するというものである。この結果、センサの初期位置及び向きを動きの追跡アルゴリズムに対して直接的に提供することが可能であり、且つ休止状態の初期状態を反映させるため、初期線速度及び角速度をゼロに設定することができる。この方式は極めて単純であるが、いくつかの状況においては適用できない場合がある。ときには初期位置又は初期向きが判明しない場合がある。いくつかの用途においては、動きの追跡を開始する前に休止状態の期間を確保することが実現可能ではない場合がある。

動きの追跡を開始する前にセンサが休止状態となることを要する既存のシステムは、ときには、加速度計を重力ベクトルのセンサとして使用することにより、垂直線との関係におけるセンサの初期傾斜の推定値を取得することができる。この技法は、センサが休止状態にあり、且つ無視可能な振動が存在している際にのみ適用可能であり、さもなければ、加速度計データは、真の加速度及び重力の組合せを反映することになる。

慣性センサの初期化の第２の既存の方式は、慣性センサデータをその他のセンサデータ供給によって補完するというものである。例えば、ＧＰＳシステムは、しばしば、絶対位置情報を有するデータ供給を提供するため、慣性センサとの関連において使用されている。絶対向き情報を有するデータ供給を提供するため、慣性センサとの関連において磁石を使用してもよい。これらの慣性センサと非慣性センサとを組み合わせたタイプのハイブリッドシステムは、一般に、ナビゲーションに使用されている。例えば、ＧＰＳシステムは、主ナビゲーションツールとして使用されてもよく、この場合に、ＧＰＳ信号が一時的に利用できないケースにおいては、慣性センサが、ＧＰＳ更新とバックアップとの間における外挿を提供している。

ハイブリッドシステムの欠点は、付加的なセンサハードウェア及びソフトウェアの費用及び複雑さである。具体的には、ＧＰＳ（又は携帯電話信号を使用したものなどの類似の場所検出システム）の場合には、ＧＰＳレシーバをセンサハードウェアに統合しなければならない。これは、実際的ではない場合があり、又は屋内などの特定の環境においてはまったく機能しない場合もある。

まとめると、慣性センサを初期化する既知のシステムは、一般に、センサを休止状態及び既知の状態に配置することに、又は慣性センサを更なるセンサのデータ供給によって補完することに依存している。いずれの方式も、特定の用途において制限及び欠点を有している。従って、相対的に一般的な状態において、且つ更なるセンサハードウェアに対するニーズを伴うことなしに使用されうる慣性センサの初期化方法に対するニーズが存在している。

本発明の実施形態は、慣性センサ装置の初期状態を、初期化時間インターバル中においてこの慣性センサ装置からキャプチャされたデータと、このインターバル中におけるこの慣性センサの動きに対するソフト制約の組とに基づいて、ある時点において推定するための方法に関する。これらの制約は、ソフト制約と呼称されてもよく、その理由は、いくつかの実施形態において、これらの制約は、初期化時間インターバル中における装置の動きについてほぼ守られると予想されるからである。これらのソフト制約からのわずかな逸脱が予想されうるが、大規模な逸脱は発生しないと予想されうる。本発明の異なる実施形態は、これらの異なる実施形態を使用する様々な用途において予想される代表的な動きのパターンを潜在的に反映した異なるソフト制約を使用しうる。

本発明の１つ又は複数の実施形態は、慣性センサが、初期化時間インターバルの全体を通じて休止状態にあること又は特定の向きにあることを必要としなくてもよい。更なるいくつかの実施形態は、慣性センサからキャプチャされたデータが、ＧＰＳ読取りなどのその他の非慣性データによって補完されることを必要としなくてもよい。従って、本発明の実施形態は、初期化の際に予想される動きのパターンをソフト制約の組として使用することにより、広範な条件下において、且つ更なるセンサハードウェアを伴うことなしに、慣性センサの初期状態を推定するために使用することができる。例えば、本発明の１つ又は複数の実施形態は、専用の初期化手順のために装置を休止状態において維持することを要することが実際的ではない連続使用状態にある装置の初期向き又は初期速度を推定するために使用されうる。このようなユースケースは、例えば、装置の向き又は速度の初期化のために、ゲームの最中に装置のユーザーを妨害することが実際的ではない統合型の慣性センサを有するスポーツ装置に対して適用されうるであろう。

本発明の実施形態は、ソフト制約の組からの慣性センサ装置の実際の動きの軌跡の逸脱を定量化するため、１つ又は複数のペナルティ計測基準を使用してもよい。このような実施形態を使用する用途においては、ソフト制約自体と同様に、本発明の１つ又は複数の実施形態によって使用されるペナルティ計測基準も、予想される動きのパターンに基づいて選択されうる。

１つ又は複数の実施形態においては、慣性センサの初期状態の推定値は、ペナルティ計測基準を極小化することによって算出されうる。事実上、この手順は、このような実施形態のために定義されたソフト制約に近接した（位置及び速度軌跡を含みうる）動きの軌跡を生成する初期状態を見出している。既存の技術における通常の慣性による動きの追跡用途においては、慣性センサの初期状態が判明しており、且つセンサの動きの軌跡は、慣性センサ用の周知の動きの微分方程式を（分析的に又は数値的に）積分することによって算出されている。本発明の１つ又は複数の実施形態において使用される方式は、ある意味においては、通常の動きの追跡方式とは逆であり、ソフト制約からの動きの軌跡の逸脱を極小化することによって見出されるべき変数として、初期状態を取り扱ってもよい。

ペナルティ計測基準を極小化する初期状態の判定は、関数極小化のための既知の技法の組のいずれかによって実行されうる。これらは、直接的な分析的技法（ときには単純な関数形態の場合に使用されうる）と、関数最適化のための様々な数値的方法とを含む。また、本発明の実施形態は、初期状態の不正確ではあるが有用である推定値を取得するため、近似的な極小化方法を適用してもよい。このような近似的な方法は、例えば、初期化計算が、相対的に小さい演算能力を有する組込み型のプロセッサ上において実行される場合、又は近似的な推定値の取得が緊急に必要とされている場合に使用されうるであろう。

本発明の１つ又は複数の実施形態においては、慣性センサの動きに対するソフト制約は、センサの位置が初期化の期間中において相対的にほとんど変化せず、且つセンサの初期速度が相対的に小さいという制約を含んでもよい。これらの実施形態は、初期化期間にまたがる慣性センサの位置の正味の変化から構成された単純なペナルティ計測基準を使用してもよい。このタイプの単純なペナルティ計測基準は、例えば、プレーヤーが、アドレスの際に、自らの（統合型又は装着型の慣性センサを含む）ゴルフクラブを運動させているが、クラブヘッドが、このアドレス期間中にボールから非常に遠く離れた状態では絶対に運動せず、且つアドレスの際に非常に高速では絶対に運動しないゴルフスイング分析用途における１つ又は複数の実施形態で使用されうるであろう。

１つ又は複数の実施形態においては、センサの初期向きは、動きの微分方程式を向きの式と位置及び速度の式とに分離する技法を使用することにより、直接的に算出することができる。この微分方程式の分離は、初期向きの推定値を付与するために分析的に極小化されうる正味の位置の変化の式をもたらす（ペナルティ計測基準）。この分析的な解決策は、反復的な数値的極小化方式のために時間又は演算リソースを提供することができない用途において利益を提供しうる。

本発明の１つ又は複数の実施形態においては、慣性センサの動きに対するソフト制約は、動きが初期化インターバル中において近似的に周期性を有することを通知する１つ又は複数の制約を含んでもよい。１つ又は複数の実施形態におけるペナルティ計測基準は、単一の選択された動きの期間にまたがる正味の位置の変化及び正味の速度の変化を含んでもよい。このような制約及びペナルティ計測基準は、例えば、打者が、投球を待っている間、自らのバットを準周期的な前後の動きにおいて又は反復的な円形の動きにおいて運動させている野球のスイング分析用途における１つ又は複数の実施形態で使用されうるであろう。

本発明の１つ又は複数の実施形態においては、ペナルティ計測基準を極小化する前に、動きの周期の推定値が予備的な処理ステップにおいて算出されてもよい。１つ又は複数の実施形態は、動きの優位周波数（及び従って優位周期）を識別するため、初期化の際の慣性センサデータに対する離散フーリエ変換（ＤＦＴ）を使用した周期推定ステップを含んでもよい。予備的な周期推定ステップの使用は、観察される動きから周期が動的に算出されうるという利益を提供する。従って、これは、ソフト制約（特に、周期）を特定のイベントについて観察される特定の動きのパターンに対して調節する１つ又は複数の実施形態によって使用される適応型の方法であってもよい。

期間にまたがる正味の位置の変化又は正味の速度の変化を含むペナルティ計測基準を使用する１つ又は複数の実施形態においては、微分方程式の分離が、この初期化期間中における速度及び位置の正味の変化の式を得るために適用されてもよい。１つ又は複数の実施形態においては、これらの式は、順番に分析的に解くことが可能であり、最初に正味の速度の変化を極小化するために初期向きを算出することが可能であり、且つ次いで正味の位置の変化を極小化するために初期速度を算出することができる。

本発明の１つ又は複数の実施形態においては、慣性センサの初期向き及び初期速度の推定値を判定する際に、対象の活動を含むキャプチャ時間インターバルにわたって慣性センサからサンプルデータを収集するステップを含む方法が利用されてもよく、この場合に、サンプルデータは、加速度計データと、レートジャイロデータとを含む。加速度計データ及びレートジャイロデータは、それぞれ、時系列の三次元ベクトルであり、いくつかの実施形態においては、慣性センサは、ジャイロ又は加速度計の３つ未満の軸に関するデータを提供しうるが、このデータは、常に、三次元ベクトル内に埋め込むことができる。任意の時点における慣性センサの向きは、三次元空間の直交変換であり、これは、３×３行列又は単位四元数などの様々な形態において表されうる。任意の地点及び時点における慣性センサの速度及び位置は、それぞれ、三次元ベクトルである。初期化時間インターバルは、キャプチャ時間インターバル内において選択される。ソフト制約の組は、初期化インターバル中における慣性センサの位置及び速度軌跡に関して定義される。ソフト制約のそれぞれは、位置ベクトル及び速度ベクトルの可能な軌跡の組を定義している。本発明の１つ又は複数の実施形態においては、ソフト制約は、初期化インターバル中における対象の活動の位置及び速度軌跡の代表的なパターンの近似を表している。ペナルティ計測基準の組は、ソフト制約からの慣性センサの算出された位置及び速度軌跡の逸脱を定量化するために定義されている。初期化インターバルの開始における慣性センサの初期向き及び初期速度の推定値は、初期向き及び初期速度の推定値が、サンプルのジャイロ及び加速度計データを初期化インターバルにわたって向き、速度、及び位置に積分するために初期状態として使用される際に、ペナルティ計測基準を極小化するために算出される。１つ又は複数の実施形態においては、積分は、向き、速度、及び位置の時間導関数を、互いに且つ計測された加速度計及びジャイロデータに関係付ける慣性センサの結合された動きの微分方程式を使用している。１つ又は複数の実施形態においては、向きの時間導関数が、向き×ジャイロベクトルとの間のクロス積を表す歪対称行列に等しい微分方程式が使用されている。速度ベクトルの時間導関数が、向き×加速度計ベクトルに等しく、この量が、重力ベクトルに加算される第２の微分方程式が使用されている。重力ベクトル項は、慣性センサの加速度計が、真の加速度ではなく、自由落下との関係における見かけの動きを計測するという事実を反映している。位置ベクトルの時間導関数が速度ベクトルに等しい第３の微分方程式が使用されている。

本発明の１つ又は複数の実施形態においては、ソフト制約は、初期化インターバルの開始と終了との間における位置の変化がゼロであるという要件を含み、これは、慣性センサが、事実上、初期化インターバルにわたって、同一の場所において又は空間の限られた領域内において留まると予想されることを反映している。１つ又は複数の実施形態においては、ペナルティ計測基準は、初期化インターバルの終了と初期化インターバルの開始との間における位置の差の絶対値として定義される計測基準を含む。この計測基準は、慣性センサのゼロの正味の運動というソフト制約からの位置軌跡の逸脱を計測する。１つ又は複数の実施形態においては、慣性センサの初期速度の推定値がゼロに設定されており、且つセンサの初期向きを推定するためにペナルティ計測基準の極小化が使用されている。

本発明の１つ又は複数の実施形態においては、ソフト制約は、慣性センサの位置及び速度が、初期化インターバル中において周期性を有するという要件を含む。１つ又は複数の実施形態においては、初期化インターバル中におけるレートジャイロデータの時系列の対応した軸上における離散フーリエ変換を使用してジャイロのそれぞれの軸ごとに周波数ドメイン信号を算出することにより、初期インターバル中における動きの優位周期の推定値が取得されている。それぞれの軸上における変換された周波数ドメイン信号のピークの大きさが算出され、且つすべての軸にまたがる最大ピークの大きさが取得される。この最大ピークの大きさが発生する周波数が、初期化インターバル中における動きの優位周波数として使用される。動きの優位周期は、優位周波数の逆数として算出される。１つ又は複数の実施形態においては、ペナルティ計測基準は、１つの動きの周期にわたる速度の変化の絶対値である速度ペナルティ計測基準と、１つの動きの周期にわたる位置の変化の絶対値である位置ペナルティ計測基準とを含む。長さが動きの優位周期に等しい状態で、初期化インターバル内の指定された期間が選択される。１つ又は複数の実施形態においては、最初に指定された期間にわたる速度ペナルティ計測基準を極小化することにより、センサの初期向きの推定値が算出され、且つ次いで予め算出された初期向きの推定値を使用して、指定された期間にわたる位置ペナルティ計測基準を極小化することにより、センサの初期速度の推定値が算出されている。

本開示を通じて伝達される概念の上述の及びその他の態様、特徴、及び利点については、以下の添付図面との関連において提示される以下のその更に具体的な説明から更に明らかとなろう。

慣性センサ装置の初期向きの推定値を生成するための方法における主要なステップを示す本発明の一実施形態のフローチャートを示す。 慣性センサからキャプチャされたデータ及び初期化インターバルの選択を示す図１のステップのうちの２つのステップの詳細図を示す。 位置及び速度に対するソフト制約の定義及びこれらのソフト制約からの逸脱用のペナルティ計測基準の定義を示す図１のステップのうちの更に２つのステップの詳細図を示す。 動きの微分方程式を積分すると共に結果的に得られる位置及び速度に対してペナルティ計測基準を適用することによって初期状態を算出する図１のフローチャートの最後のステップの一実施形態の詳細図を示す。 ペナルティ計測基準が初期インターバルにわたる位置の変化から構成されると共に初期速度推定値がゼロに設定される方法の一実施形態を示す。 動きの式から初期向きを抽出することによって最適な初期向きを算出するための特定の技法を使用する方法の一実施形態を示す。 初期向きの推定値の直接的な解を取得する方法の一実施形態を示す図６の技法を更に示す。 初期化インターバル中において周期的な運動を必要とするというソフト制約を位置及び速度に適用すると共に離散フーリエ変換を使用してこの周期的な動きの周期を推定する方法の一実施形態のフローチャートを示す。 周期性制約と、この周期性制約からの逸脱用のペナルティ計測基準の定義とを示す図８のステップのうちの２つのステップの詳細図を示す。 初期化インターバル中における優位周期を算出するための離散フーリエ変換の使用法を示す図８のステップのうちの更に２つのステップの詳細図を示す。 最初の最適な初期向きの推定値の算出と、これに後続する最適な初期速度の推定値の算出とを示す図８の最後の２つのステップの一実施形態の詳細図を示す。

以下、慣性センサの初期向き及び初期速度の推定値を判定する方法について説明することとする。以下の例示用の説明においては、本明細書の全体を通じて記述されている概念の更に十分な理解を提供するため、多数の特定の詳細事項が記述されている。但し、当業者には、本明細書において記述されている概念の実施形態は、本明細書において記述されている特定の詳細事項のすべての態様を組み込むことなく実施されうることが明らかとなろう。その他の例においては、開示を不明瞭にしないように、当業者には周知の特定の態様の詳細な記述が省略されている。読者は、本発明の概念の例が本開示の全体を通じて記述されているが、請求項及び任意の均等物の範囲の全体が本発明を定義していることに留意されたい。

図１は、慣性センサの初期向き及び初期速度を判定するための方法の一実施形態のフローチャートを示している。１０１において、サンプルデータが、慣性センサからキャプチャされている。このサンプルデータは、キャプチャ時間インターバルにまたがる計測値の１つ又は複数の時系列から構成されており、この場合に、このキャプチャ時間インターバルは、対象の活動を含んでいる。１０２において、このキャプチャ時間インターバル内において、初期化インターバルが選択されている。この初期化インターバルは、初期向き及び初期速度の推定算出のために使用されることになる。１０３において、別個に、慣性センサの位置及び速度に対するソフト制約の組が定義されている。１０４において、ソフト制約からの算出された位置及び速度の逸脱を計測するペナルティ計測基準が定義されている。１０５において、ペナルティ計測基準を初期化インターバルにまたがって極小化することにより、初期向き及び初期速度の推定値を算出するため、初期化インターバルからのサンプルデータ及びペナルティ計測基準が使用されている。

図２は、１０１におけるサンプルデータの収集ステップ及び１０２における初期化インターバルの選択ステップの詳細図を示している。１０１におけるサンプルデータの収集ステップにおいては、慣性センサデータが、開始時点２０３（ｔ_ｉ）及び終了時点２０４（ｔ_ｆ）を有するキャプチャ時間インターバル［ｔ_ｉ，ｔ_ｆ］にまたがって収集されている。一実施形態においては、慣性センサは、３軸加速度計と、３軸レートジャイロとを含む。その他の実施形態においては、例えば、磁力計からのデータ又はＧＰＳレシーバからの場所データなどを含む異なるデータが利用可能であってもよい。３軸加速度計及び３軸レートジャイロを有する一実施形態においては、センサからキャプチャされたサンプルデータは、

と表記された時系列の加速度計データ２０１と、

と表記された時系列のレートジャイロデータ２０２とを含む。

及び

のボールド体の表記は、これらが、（通常は、３次元空間における）ベクトル量であることを示しているが、図示を目的として、これらの値は、曲線２０１及び２０２において１次元のものとして示されている。加速度計データの表記

は、通常の加速度計が、真の加速度ではなく、特定の力を計測しているという事実を反映している。レートジャイロデータの表記

は、通常のレートジャイロが角速度を計測していることを示している。

図２において、初期化インターバル２０５は、キャプチャインターバル［ｔ_ｉ，ｔ_ｆ］内において選択されている。初期化インターバル［ｔ_０，ｔ_ｎ］⊂［ｔ_ｉ，ｔ_ｆ］は、開始点２０６（ｔ_０）及び終了点２０７（ｔ_ｎ）を有する。初期化インターバルは、対象のイベントの特性に応じた様々な基準に基づいて、キャプチャインターバル内において選択されうる。例えば、１つ又は複数の実施形態においては、初期化インターバルは、キャプチャインターバルの開始における固定された秒数から構成されてもよい。その他の実施形態においては、初期化インターバルは、相対的に低調な動き又は活動を通知するシグネチャについてキャプチャインターバルをサーチすることにより、動的に判定されてもよい。初期化インターバル２０５が選択されたら、初期化インターバルにわたる加速度計データ２０８及びジャイロデータ２０９が、後述する初期向き及び初期速度の推定のために使用される。

図３は、ソフト制約を定義するステップ１０３及びペナルティ計測基準を定義するステップ１０４の詳細図を示している。

と表記された慣性センサの位置軌跡３０１及び

と表記された慣性センサの速度軌跡３０５は、対象の活動において代表的な特定のパターンにほぼ準拠すると予想されている。一般に、慣性センサの位置及び速度は、３次元空間におけるベクトル量であるが、図示を目的として、これらは、図３においては、１次元の値として示されている。位置及び速度について予想される特定のパターンは、対象の特定の１つ又は複数の活動に依存することになる。一般に、位置及び速度に対するソフト制約Ｃ_ｉは、初期化インターバルにわたる位置及び速度の可能な軌跡のサブセット

として表すことができる。本発明の一実施形態は、これらのソフト制約の組｛Ｃ_ｉ｝を含んでもよい。図３は、例示的な制約３０２及び３０６を示している。これらは、例示を目的とした例に過ぎず、その他の実施形態は、その他のタイプ及び数のソフト制約を含むことになる。Ｃ_Ｒと表記されたソフト制約３０２は、位置軌跡３０１に対する制約である。この制約は、位置

が、対象の時間インターバルの全体を通じて、上限３０３及び下限３０４内に留まることを必要としている。Ｃ_Ｖと表記されたソフト制約３０６は、速度軌跡３０５に対する制約である。制約３０６は、軌跡の終了点においてのみ、適用されるものであり、この終了点において、速度がＣ_Ｖによって定義された限度内に位置することを必要としている。これらの例示的なソフト制約３０２及び３０６は、可能なソフト制約のタイプの単純な例を示すものであり、その他の実施形態は、位置、速度、又はこれらの両方に対する更に複雑な制約を含んでもよい。

図３の１０４において、１０３において定義されたソフト制約からの位置及び速度軌跡の逸脱を定量化するため、ペナルティ計測基準が定義されている。

と表記された計測又は算出された位置３０７は、ソフト制約３０２を充足してはいない。ｍ_Ｒと表記されたペナルティ計測基準３０８は、ソフト制約からの

の逸脱を計測する。例示を目的として、ペナルティ計測基準ｍ_Ｒは、ソフト制約によって課された限度からの位置の最大絶対逸脱として定義されている。その他の実施形態は、異なるペナルティ計測基準を定義してもよく、例えば、一実施形態は、図３に示されている最大逸脱ではなく、ソフト制約の限度からの位置の平均絶対逸脱を使用しうるであろう。速度ペナルティ計測基準の図は、位置ペナルティ計測基準の図に類似している。

と表記された計測又は算出された速度３０９は、ソフト制約３０６を充足してはいない。ｍ_Ｖと表記されたペナルティ計測基準３１０は、ソフト制約からの

の逸脱を計測する。例示を目的として、ペナルティ計測基準ｍ_Ｖは、ソフト制約によって課された限度からの速度の絶対逸脱として定義されている。例示的な速度ソフト制約３０６は、時間インターバルの終了点においてのみ適用されることから、速度ペナルティ計測基準３１０も、同様に、この終了点においてのみ、制約からの逸脱を計測する。位置ペナルティ計測基準と同様に、この速度ペナルティ計測基準も一例に過ぎない。その他の実施形態は、異なるペナルティ計測基準を定義してもよく、例えば、一実施形態は、図３に示されている逸脱の絶対値ではなく、ソフト制約の限度からの速度の絶対逸脱の二乗を使用してもよいであろう。

図４は、１０５において、ペナルティ計測基準を極小化するため、初期向き及び初期速度を算出するステップの詳細図を示している。

と表記された加速度計データ２０８及び

と表記されたレートジャイロデータ２０９は、計算手順に対する入力である。これらの軌跡は、初期化インターバル［ｔ_０，ｔ_ｎ］にわたって利用可能である。これらの入力が付与され、且つセンサの向き（Ｑ_０）及び速度

に関する初期状態４０２が付与された場合に、位置軌跡３０７

及び速度軌跡３０９

を取得するために微分方程式４０１を解くことができる。微分方程式４０１は、向きＱ（ｔ）、速度

及び位置

の時間導関数を観察された計測値

及び

に対して関係付ける同時式の組である。これらの微分方程式は、当業者には周知であり、これらは、ストラップダウン慣性センサを使用した慣性ナビゲーション用の式の一形態である。４０１における表記

は、

によって定義された線形演算子Ｓを示しており、ここで、×は、ベクトルのクロス積である。

１０５の計算において、初期状態４０２は、推定対象の未知の量として取り扱われている。これらの初期状態のそれぞれの推定値は、位置及び速度の軌跡３０７及び３０９の特定のペアをもたらす。次いで、これらの算出された軌跡に対して、ペナルティ計測基準３０８及び３１０が適用される。これらのペナルティ計測基準は、ソフト制約からの算出された軌跡の逸脱の集合的な尺度を生成する。本発明の異なる実施形態は、ペナルティ計測基準の加算、又は最初に１つの計測基準の極小化及びそれに後続するその他の計測基準の極小化などの、ペナルティ計測基準の組をソフト制約の逸脱の集合的な尺度に組み合わせるための様々な技法を利用してもよい。４０３において、観察されたペナルティ計測基準３０８及び３１０を極小化するために初期状態が調節されている。この調節プロセスは、ペナルティ計測基準を極小化する初期状態を求めるため、図４に示されているように反復的なものであってもよい。ペナルティ計測基準を極小化する初期状態を見出すため、目的関数の極小化のための任意の既知の技法が使用されてもよい。このような技法は、当業者には周知である。選択された極小化技法を適用した後に、１０５における計算により、初期向きの推定値用の

及び初期速度推定値用の

と表記された初期状態４０４の推定値がもたらされる。

図５は、初期化インターバル中における慣性センサの最小運動の予想を反映した位置軌跡用の特定のソフト制約を有する本発明の一実施形態を示している。このタイプのソフト制約は、例えば、人物又は装置が、活動の開始の前に、相対的に静的である活動に対して適用されうるであろう。図５は、図４の手順が、このタイプの実施形態に対して変更及び専用化される方式を示している。図４と同様に、加速度計データ２０８及びジャイロデータ２０９が、初期状態４０２と共に、微分方程式４０１の積分への入力であり、これにより、位置軌跡３０７が得られる。図５に示されている実施形態においては、位置ペナルティ計測基準５０２は、初期化インターバルにまたがる位置の正味の変化の大きさ

として定義されている。このペナルティ計測基準は、位置軌跡が、初期化インターバル中において、どれほど近接した状態において、センサのゼロの正味の運動というソフト制約に準拠しているかを計測する。その他の実施形態は、この逸脱を計測するため、開始位置から初期化インターバルにわたる位置の最大変化などの異なるペナルティ計測基準を使用してもよいであろう。図５に示されている実施形態においては、初期速度推定値を

に設定することにより、更なる単純化５０１が初期状態に対して適用されている。この単純化は、この場合にも、初期化インターバル中における相対的にわずかな動きの予想を反映している。その他の実施形態は、初期速度に関する異なる仮定を利用してもよいであろう。従って、初期化ステップ５０３は、位置ペナルティ計測基準５０２

を極小化し、これにより、

と表記された初期向きの推定値５０４をもたらすために初期向きＱ_０のみを調節している。

図６は、初期向き及び初期速度の入力４０２及びセンサデータ２０８及び２０９を使用することにより、微分方程式４０１を積分するため、本発明の１つ又は複数の実施形態において適用されうる技法を示している。この技法においては、最初に微分方程式６０１が別個に解かれている。この式

は、６０３において、補助変数Ｐを定義することによって解かれており、ここで、Ｑ＝Ｑ_０Ｐである。この置換は、初期条件Ｐ（０）＝Ｉを有する式

（ここで、Ｉは、単位行列である）をもたらす。この式は、初期向きＱ_０に対する依存性を伴うことなしに、Ｐ（ｔ）について直接的に解くことができる（大部分のケースにおいて、Ｐ（ｔ）は、数値積分によって算出されることになり、その理由は、閉形式解が存在しない可能性が高いからである）。第２に、入力６０４として、６０３において算出されたＰ（ｔ）の値を使用することにより、残りの微分方程式６０２を解くことができる。これらの２つの式は、６０５において積分され、これにより、以下の位置及び速度の式がもたらされる。

及び

１つ又は複数の実施形態においては、ペナルティ計測基準を極小化する初期状態４０２を算出するためにこれらの式が使用されている。

図７は、特定の位置ペナルティ計測基準５０２

と共に６０５の式を使用してペナルティ計測基準を極小化する初期向きＱ_０を直接的に算出するため、１つ又は複数の実施形態において使用される技法を示している。

という単純化５０１を使用することにより、且つ７０１において補助変数

を定義することにより、式

が得られ、ここで、

である。
この式は、初期向きの値

と共に、７０２において図式化されている。Ｑ_０は、直交変換であることから、可能な値

の範囲は、半径

を有する球体上に位置しなければならない。この球体の２次元の図が、円７０３として示されている。

から

への回転が、７０４において示されており、すべてのケースにおいて、

は、７０３上に位置している。従って、

を極小化するためには、

が、

の方向を指し示し、これにより、７０５において、最適な値７０６及び最小の

をもたらすはずであることが幾何学的に明らかである。この技法は、

を中心とした任意の回転までの最適な

を提供するものに過ぎず、これは、純粋な慣性センサが、方位を検知することができないことを反映している。本発明のその他の実施形態においては、初期方位を推定するため、磁力計データなどのその他のセンサデータが同様に使用されてもよい。

図８は、慣性センサの動きが、初期化インターバル中においてほぼ周期性を有することを要するソフト制約を使用した本発明の一実施形態におけるフローチャートを示している。ソフト制約の定義１０３は、初期化インターバル中において慣性センサの速度及び位置用の周期性制約を定義するステップ８０５を含む。周期性ソフト制約に対応した状態において、ペナルティ計測基準の定義１０４は、８０６において、ペナルティ計測基準として、単一の周期にわたる位置の変化及び速度の変化を計測する計測基準を含む。１０２において、初期化インターバルが選択されており、且つ８０１において、このインターバル中における動きの周期の推定値が算出されている。この周期の計算は、８０２において、離散フーリエ変換を介して時系列のセンサジャイロデータを周波数ドメインに変換し、且つ８０３において、変換されたデータから優位周波数を選択することによって実行される。８０４において、周期に等しい長さの初期化インターバル内における特定の時間インターバルが選択されている。ペナルティ計測基準８０６は、最初に８０７において、速度ペナルティ計測基準を極小化するために初期向き推定値が算出され、且つ次いで８０８において、位置ペナルティ計測基準を極小化するために初期速度推定値が算出されるという２つの段階において、１０５において極小化されている。本発明のその他の実施形態は、これらの技法のサブセットを利用してもよく、例えば、同一の速度及び位置ペナルティ計測基準を使用することもできるが、動きの周期を推定するために、フーリエ変換の代わりに時間ドメイン技法を使用することもできよう。

図９は、周期性ソフト制約の定義ステップ８０５及び位置及び速度ペナルティ計測基準の定義ステップ８０６の詳細図を提供している。８０５において、ソフト制約は、（

と表記された）位置９０１及び（

と表記された）速度９０３が、（Ｔと表記された）なんらかの期間９０３を伴う周期性を有することを必要としている。これらのソフト制約は、周期的な動きを必要とするのみであり、位置及び速度曲線の周期又は形状を規定してはいない。８０６において、算出された位置９０４及び算出された速度９０６は、正確な周期性を有してはいない。位置ペナルティ計測基準９０５は、１つの周期にわたる位置の変化の絶対値

として、正確な周期性からの逸脱を計測する。同様に、速度ペナルティ計測基準９０７も、１つの周期にわたる速度の変化の絶対値

として、正確な周期性からの逸脱を計測する。

図１０は、動きの周期の計算ステップ８０１の詳細図を提供している。ジャイロのｘ軸のω_ｘの時系列１００１と、ω_ｙ及びω_ｚの類似の時系列とを含む初期化インターバル中における時間ドメインのレートジャイロデータ

が、離散フーリエ変換を使用して周波数ドメインに変換されている。｜Ω_ｘ｜と表記された変換されたω_ｘの大きさ１００２が、そのピーク値についてスキャニングされている。ピーク値１００３が周波数１００４において発生している。これらの軸上におけるピーク値と、これらのピーク値が発生する周波数とを見出すため、大きさ｜Ω_ｙ｜及び｜Ω_ｚ｜の類似のスキャニングが実行されている。１００５において、３つの軸にまたがる最大のピークの大きさが選択されており（例示を目的として、ｚ軸上において示されている）、且つこのピークが発生するｆ_ｍａｘと表記された周波数１００６が、初期化の際の動きの全体的な優位周波数として使用されている。次いで、１００７において、優位周期が、Ｔ＝１／ｆ_ｍａｘとして算出されている。

図１１は、８０７における初期向きの推定値の計算ステップ及び８０８における初期速度の推定値の計算ステップの詳細図を提供している。［ｔ_１，ｔ_１＋Ｔ］と表記された８０４において見出された長さＴのインターバル１１０１が計算のために使用されている。ペナルティ計測基準９０５及び９０７が順番に極小化されている。上述の及び図６において示されている技法を使用することにより、式１１０２が取得され、これにより、速度ペナルティ計測基準について、次式

が得られる。図７に示されている幾何学的な技法を使用することにより、又は数値的最適化用の任意のその他の周知の技法を使用することにより、この式を極小化することにより、初期向きの推定値

を得ることができる。初期向きの推定値１１０３が、８０８における後続の計算において使用されている。この場合にも、式１１０４を得るため、図６に示されている積分技法が使用され、これにより、次式

が得られる。ここで、この式は、単一の未知の

にのみ依存しており、その理由は、

が予め算出されているからである。ここで、数値的最適化用の任意の周知の既知の技法を使用することにより、この

の式を極小化することにより、初期速度の推定値

を得ることができる。

以下、本発明の好ましい実施形態を項分け記載する。

実施形態１
慣性センサの初期向きＱ_０及び初期速度

の推定値を判定する方法において、
対象の活動を含むキャプチャ時間インターバル［ｔ_ｉ，ｔ_ｆ］にわたって慣性センサからサンプルデータを収集するステップであって、前記サンプルデータは、加速度データ

と、レートジャイロデータ

とを含む、ステップと、
前記キャプチャ時間インターバル内において初期化インターバル［ｔ_０，ｔ_ｎ］⊂［ｔ_ｉ，ｔ_ｆ］を選択するステップと、
前記初期化インターバル中における前記慣性センサの位置

及び速度

に対するソフト制約｛Ｃ_ｉ｝を定義するステップであって、それぞれ

であり、前記ソフト制約は、前記初期化インターバル中における前記対象の活動の位置及び速度軌跡の代表的なパターンの近似を表している、ステップと、
前記ソフト制約｛Ｃ_ｉ｝からの前記位置

及び前記速度

の算出された軌跡の逸脱を定量化するためのペナルティ計測基準

を定義するステップと、
前記初期向き及び初期速度が、結合された微分方程式

を介して、前記初期化インターバルにわたって、前記サンプルデータを向き、位置、及び速度に積分するために初期状態として使用される際に、前記ペナルティ計測基準を極小化するため、前記初期化インターバルの開始における前記慣性センサの前記初期向きＱ_０＝Ｑ（ｔ_０）及び初期速度

の推定値を算出するステップと
を含むことを特徴とする、方法。

実施形態２
前記ソフト制約のうちに、前記初期化インターバルの開始と終了との間における位置の変化がゼロである、即ち、

であるという要件を含めるステップと、
前記初期速度の推定値をゼロに設定する、即ち、

に設定するステップと、
前記ペナルティ計測基準のうちに、前記初期化インターバルの前記終了と前記初期化インターバルの前記開始との間における位置の差の絶対値を含める、即ち、

とするステップと
を更に含むことを特徴とする、実施形態１に記載の方法。

実施形態３
ソフト制約のうちに、前記慣性センサの前記位置及び速度が、前記初期化インターバル中において周期性を有するという要件を含めるステップと、
前記初期化インターバル中におけるレートジャイロデータの時系列の対応する軸に対して離散フーリエ変換を適用することにより、前記レートジャイロのそれぞれの軸ごとに周波数ドメイン信号Ω_ｘ（ｆ）＝ＤＦＴ（ω_ｘ（ｔ））、Ω_ｙ（ｆ）＝ＤＦＴ（ω_ｙ（ｔ））、Ω_ｚ（ｆ）＝ＤＦＴ（ω_ｚ（ｔ））を算出するステップと、
優位周波数ｆ_ｍａｘを３つの軸にまたがる最大の変換の大きさを有する周波数ｆ_ｍａｘ＝ａｒｇｍａｘ（ｍａｘ（｜Ω_ｘ（ｆ）｜，｜Ω_ｙ（ｆ）｜，｜Ω_ｚ（ｆ）｜））として選択し、且つ前記優位周波数から、Ｔ＝１／ｆ_ｍａｘとして優位周期Ｔを算出するステップと、
前記ペナルティ計測基準のうちに、動きの１つの周期にわたる前記速度の変化の絶対値である速度ペナルティ計測基準

と、動きの１つの周期にわたる前記位置の変化の絶対値である位置ペナルティ計測基準

とを含めるステップと、
前記初期速度及び初期向きの推定値の計算のために、前記初期化インターバル内において指定された期間を選択する、即ち、［ｔ_１，ｔ_１＋Ｔ］⊂［ｔ_０，ｔ_ｎ］とするステップと、
前記指定された期間にわたる前記速度ペナルティ計測基準を極小化することにより、前記初期向きの推定値を算出するステップと、
前記初期向きの推定値を使用して、前記指定された期間にわたる前記位置ペナルティ計測基準を極小化することにより、前記初期速度の推定値を算出するステップと
を更に含むことを特徴とする、実施形態１に記載の方法。

Claims (3)

1. 慣性センサの初期向きＱ_０及び初期速度
   

   

   の推定値を判定する方法において、
   対象の活動を含むキャプチャ時間インターバル［ｔ_ｉ，ｔ_ｆ］にわたって慣性センサからサンプルデータを収集するステップであって、前記サンプルデータは、加速度データ
   

   

   と、レートジャイロデータ
   

   

   とを含む、ステップと、
   前記キャプチャ時間インターバル内において初期化インターバル［ｔ_０，ｔ_ｎ］⊂［ｔ_ｉ，ｔ_ｆ］を選択するステップと、
   前記初期化インターバル中における前記慣性センサの位置
   

   

   及び速度
   

   

   に対するソフト制約｛Ｃ_ｉ｝を定義するステップであって、それぞれ
   

   

   であり、前記ソフト制約は、前記初期化インターバル中における前記対象の活動の位置及び速度軌跡の代表的なパターンの近似を表している、ステップと、
   前記ソフト制約｛Ｃ_ｉ｝からの前記位置
   

   

   及び前記速度
   

   

   の算出された軌跡の逸脱を定量化するためのペナルティ計測基準
   

   

   を定義するステップと、
   前記初期向き及び初期速度が、結合された微分方程式
   

   

   を介して、前記初期化インターバルにわたって、前記サンプルデータを向き、位置、及び速度に積分するために初期状態として使用される際に、前記ペナルティ計測基準を極小化するため、前記初期化インターバルの開始における前記慣性センサの前記初期向きＱ_０＝Ｑ（ｔ_０）及び初期速度
   

   

   の推定値を算出するステップと
   を含むことを特徴とする、方法。
2. 前記ソフト制約のうちに、前記初期化インターバルの開始と終了との間における位置の変化がゼロである、即ち、
   

   

   であるという要件を含めるステップと、
   前記初期速度の推定値をゼロに設定する、即ち、
   

   

   に設定するステップと、
   前記ペナルティ計測基準のうちに、前記初期化インターバルの前記終了と前記初期化インターバルの前記開始との間における位置の差の絶対値を含める、即ち、
   

   

   とするステップと
   を更に含むことを特徴とする、請求項１に記載の方法。
3. ソフト制約のうちに、前記慣性センサの前記位置及び速度が、前記初期化インターバル中において周期性を有するという要件を含めるステップと、
   前記初期化インターバル中におけるレートジャイロデータの時系列の対応する軸に対して離散フーリエ変換を適用することにより、前記レートジャイロのそれぞれの軸ごとに周波数ドメイン信号Ω_ｘ（ｆ）＝ＤＦＴ（ω_ｘ（ｔ））、Ω_ｙ（ｆ）＝ＤＦＴ（ω_ｙ（ｔ））、Ω_ｚ（ｆ）＝ＤＦＴ（ω_ｚ（ｔ））を算出するステップと、
   優位周波数ｆ_ｍａｘを３つの軸にまたがる最大の変換の大きさを有する周波数ｆ_ｍａｘ＝ａｒｇｍａｘ（ｍａｘ（｜Ω_ｘ（ｆ）｜，｜Ω_ｙ（ｆ）｜，｜Ω_ｚ（ｆ）｜））として選択し、且つ前記優位周波数から、Ｔ＝１／ｆ_ｍａｘとして優位周期Ｔを算出するステップと、
   前記ペナルティ計測基準のうちに、動きの１つの周期にわたる前記速度の変化の絶対値である速度ペナルティ計測基準
   

   

   と、動きの１つの周期にわたる前記位置の変化の絶対値である位置ペナルティ計測基準
   

   

   とを含めるステップと、
   前記初期速度及び初期向きの推定値の計算のために、前記初期化インターバル内において指定された期間を選択する、即ち、［ｔ_１，ｔ_１＋Ｔ］⊂［ｔ_０，ｔ_ｎ］とするステップと、
   前記指定された期間にわたる前記速度ペナルティ計測基準を極小化することにより、前記初期向きの推定値を算出するステップと、
   前記初期向きの推定値を使用して、前記指定された期間にわたる前記位置ペナルティ計測基準を極小化することにより、前記初期速度の推定値を算出するステップと
   を更に含むことを特徴とする、請求項１に記載の方法。

Images