JP6776882B2 - 運動解析装置、方法及びプログラム - Google Patents

Description

本発明は、移動体の運動を解析するための運動解析装置、方法及びプログラムに関する。

従来より、角速度センサ、加速度センサ及び地磁気センサを含む慣性センサを移動体に取り付け、当該慣性センサの出力値に基づいて移動体の挙動を推定する技術が知られている（特許文献１等）。また、カメラにより移動体の運動を撮影し、得られた画像データに基づいて移動体の挙動を推定することも行われている（特許文献２，３等）。

特開２０１１−２２７０１７号公報 特開２０１３−２１５３４９号公報 特開２００４−３１３４７９号公報

しかしながら、特許文献１のように慣性センサの出力値に基づいて解析を行う場合、当該出力値には慣性センサのドリフト成分等による誤差が含まれるため、解析の精度が低下することがある。例えば、慣性センサの出力値である加速度のデータを積分して速度や位置を求めるような場合、この積分により誤差が蓄積し得る。また、移動体が高速で運動する場合には、特に角速度センサの誤差が大きくなり、移動体の姿勢の推定が困難となり得る。また、回転運動が大きい場合には特に、加速度センサの出力値に含まれる重力加速度の影響を正しく評価することが困難となり得る。

一方、特許文献２，３のように画像データに基づいて解析を行う場合にも、しばしば精度の面で問題が生じる。例えば、画像処理により移動体の位置を推定し、当該位置のデータを微分することで速度や加速度を求めるような場合、位置の推定における誤差が僅かであっても、微分により誤差が増大し得る。

本発明は、高精度に移動体の運動を解析することができる運動解析装置、方法及びプログラムを提供することを目的とする。

本発明の第１観点に係る運動解析装置は、移動体の運動を解析するための運動解析装置であって、第１取得部と、第２取得部と、導出部とを備える。前記第１取得部は、センサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する。前記センサユニットは、前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含む。前記第２取得部は、第1距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する。前記導出部は、前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する。

本発明の第２観点に係る運動解析装置は、第１観点に係る運動解析装置であって、前記導出部は、前記画像データ及び前記センサデータを用いて定義される関数を含む目的関数を最小化又は最大化する最適解として、前記移動体の姿勢を導出する。

本発明の第３観点に係る運動解析装置は、第２観点に係る運動解析装置であって、前記関数は、前記画像データから導出される前記移動体の向きを用いて定義される。

本発明の第４観点に係る運動解析装置は、第２又は第３観点に係る運動解析装置であって、前記関数は、前記地磁気データ及び加速度データの少なくとも一方を用いて定義される。

本発明の第５観点に係る運動解析装置は、第２から第４観点のいずれかに係る運動解析装置であって、前記導出部は、前記移動体の初期の姿勢及び前記時系列の角速度データを用いて、前記移動体の仮の姿勢を算出する。前記関数は、前記仮の姿勢を用いて定義される。

本発明の第６観点に係る運動解析装置は、第５観点に係る運動解析装置であって、前記導出部は、前記画像データから導出される初期の前記移動体の向き及び初期の前記加速度データ、又は、前記角速度データを用いて、前記移動体の初期の姿勢として、前記移動体の静止時の姿勢を導出する。

本発明の第７観点に係る運動解析装置は、第１から第６観点のいずれかに係る運動解析装置であって、前記導出部は、前記移動体の姿勢及び前記移動体の姿勢を微分フィルタによりフィルタリングした微分データに基づいて、前記移動体の角速度を導出する。

本発明の第８観点に係る運動解析装置は、第１から第７観点のいずれかに係る運動解析装置であって、前記導出部は、前記加速度データ用いて、前記画像データから導出される前記移動体の仮の位置及び当該仮の位置を微分した仮の速度の少なくとも一方をスムージングすることにより、前記移動体の位置及び速度の少なくとも一方を導出する。

本発明の第９観点に係る運動解析装置は、第８観点に係る運動解析装置であって、前記導出部は、前記移動体の速度を微分フィルタによりフィルタリングすることにより、前記移動体の加速度を導出する。

本発明の第１０観点に係る運動解析装置は、第１から第９観点のいずれかに係る運動解析装置であって、前記導出部は、前記画像データから導出される前記移動体の向きの時系列データと、前記センサデータから導出される前記移動体の向きの時系列データとの一致度が最も高くなるように、前記時系列の画像データと前記時系列のセンサデータとの時刻合わせを行う。

本発明の第１１観点に係る運動解析装置は、第１観点から第１０観点のいずれかに係る運動解析装置であって、第３取得部をさらに備える。前記第３取得部は、第２距離画像センサにより前記第１距離画像センサとは異なる方向から前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する。前記導出部は、前記センサデータと、前記第２取得部及び前記第３取得部により取得された前記画像データとに基づいて、前記移動体の三次元挙動を導出する。

本発明の第１２観点に係る運動解析装置は、第１観点から第１１観点のいずれかに係る運動解析装置であって、前記導出部は、前記画像データに基づいて第１時刻における前記移動体の姿勢である瞬時姿勢を導出し、前記移動体の姿勢が前記第１時刻において前記瞬時姿勢に一致するように、前記第１時刻と異なる第２時刻と前記第１時刻との間の前記移動体の姿勢を補間する。

本発明の第１３観点に係る運動解析方法は、移動体の運動を解析するための運動解析方法であって、以下の（１）〜（３）のステップを含む。
（１）センサユニットを用いて、時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得するステップ。前記センサユニットは、前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含む。
（２）距離画像センサを用いて、前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得するステップ。
（３）前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出するステップ。

本発明の第１４観点に係る運動解析プログラムは、移動体の運動を解析するための運動解析プログラムであって、以下の（１）〜（３）のステップをコンピュータに実行させる。
（１）センサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得するステップ。前記センサユニットは、前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含む。
（２）距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得するステップ。
（３）前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出するステップ。

本発明によれば、移動体の運動が、（１）時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータ、及び、（２）時系列の深度画像及び二次元画像を含む時系列の画像データに基づいて導出される。その結果、センサデータに含まれる誤差は、画像データに基づいて軽減される。或いは、画像データに含まれる誤差は、センサデータに基づいて軽減される。よって、高精度に移動体の運動を解析することができる。

本発明の第１実施形態に係る運動解析装置を含む運動解析システムの全体構成を示す図。 図１の運動解析システムの機能ブロック図。 ゴルフクラブのグリップ近傍を基準とするｘｙｚ局所座標系を説明する図。 （Ａ）アドレス状態を示す図。（Ｂ）トップ状態を示す図。（Ｃ）インパクト状態を示す図。（Ｄ）フィニッシュ状態を示す図。 第１実施形態に係る運動解析方法の流れを示すフローチャート。 第１実施形態に係る時刻合わせ処理の流れを示すフローチャート。 二次元のシャフトの向きを説明する図。 時刻合わせ処理の原理を説明する概念図。 アドレス期間の姿勢の導出処理の流れを示すフローチャート。 アドレスからインパクト付近までの姿勢の導出処理の流れを示すフローチャート。 グリップの位置及び速度の導出処理の流れを示すフローチャート。 グリップの角速度及び角加速度の導出処理の流れを示すフローチャート。 グリップの加速度の導出処理の流れを示すフローチャート。 実施例１及び参考例１を示すグラフ。 実施例２及び参考例２を示すグラフ。 実施例３及び参考例３を示すグラフ（グリップのＸ軸方向の位置）。 実施例３及び参考例３を示すグラフ（グリップのＹ軸方向の位置）。 実施例３及び参考例３を示すグラフ（グリップのＺ軸方向の位置）。 実施例４及び参考例４を示すグラフ（グリップのＸ軸方向の位置）。 実施例４及び参考例４を示すグラフ（グリップのＹ軸方向の位置）。 実施例４及び参考例４を示すグラフ（グリップのＺ軸方向の位置）。 本発明の第２実施形態に係る運動解析装置を含む運動解析システムの全体構成を示す図。 図１８の運動解析システムの機能ブロック図。 第２実施形態に係る運動解析方法の流れを示すフローチャート。 第２実施形態に係る時刻合わせ処理の流れを示すフローチャート。 ダウンスイング期間の姿勢の補正処理の流れを示すフローチャート。 実施例５を示すグラフ。 変形例に係る初期時刻の姿勢の導出処理の流れを示すフローチャート。

以下、図面を参照しつつ、本発明に係る運動解析装置、方法及びプログラムをゴルフスイングの解析に利用した場合の幾つかの実施形態について説明する。

＜１．第１実施形態＞
＜１−１．運動解析システムの概要＞
図１及び図２に、本発明の第１実施形態に係る運動解析装置１を含む運動解析システム１００の全体構成図を示す。運動解析システム１００は、ゴルファー７によるゴルフクラブ５のスイング動作の解析に適するように構成されている。運動解析装置１は、ゴルフクラブ５に取り付けられた慣性センサユニット４から出力される時系列の角速度データ、加速度データ及び地磁気データ（以下、センサデータということがある）に加えて、スイング動作を撮影した時系列の画像データ（すなわち、動画データ）に基づいて、スイング動作を解析する。以上の撮影は、距離画像センサ２により行われる。運動解析装置１は、慣性センサユニット４及び距離画像センサ２とともに運動解析システム１００を構成する。運動解析装置１による解析の結果は、ゴルファー７に適したゴルフクラブ５のフィッティングや、ゴルファー７のフォームの改善、ゴルフ用品の開発等、様々な用途で利用される。

本実施形態では、主として、スイング動作中に移動するゴルフクラブ５のグリップ５１及びシャフト５２の三次元挙動が解析される。より具体的には、スイング動作中のグリップ５１の位置（三次元座標）、速度、加速度、角速度及び角加速度とともに、シャフト５２の姿勢が導出される。

＜１−２．各部の詳細＞
以下、運動解析システム１００の各部の詳細について説明する。

＜１−２−１．距離画像センサ＞
距離画像センサ２は、ゴルファー７がゴルフクラブ５を試打する様子を二次元画像として撮影するとともに、被写体までの距離を測定する測距機能を有するカメラである。従って、距離画像センサ２は、時系列の二次元画像とともに、時系列の深度画像を出力することができる。なお、ここでいう二次元画像とは、撮影空間の像をカメラの光軸に直交する平面内へ投影した画像である。また、深度画像とは、カメラの光軸方向の被写体の奥行きのデータを、二次元画像と略同じ撮像範囲内の画素に割り当てた画像である。

本実施形態で使用される距離画像センサ２は、二次元画像を赤外線画像（以下、ＩＲ画像という）として撮影する。また、深度画像は、赤外線を用いたタイムオブフライト方式やドットパターン投影方式等の方法により得られる。従って、図１に示すように、距離画像センサ２は、赤外線を前方に向けて発光するＩＲ発光部２１と、ＩＲ発光部２１から照射され、被写体に反射して戻ってきた赤外線を受光するＩＲ受光部２２とを有する。ＩＲ受光部２２は、光学系及び撮像素子等を有するカメラである。ドットパターン投影方式では、ＩＲ発光部２１から照射された赤外線のドットパターンをＩＲ受光部２２で読み取り、距離画像センサ２内部での画像処理によりドットパターンを検出し、これに基づいて奥行きが計算される。本実施形態では、ＩＲ発光部２１及びＩＲ受光部２２は、同じ筐体２０内に収容され、筐体２０の前方に配置されている。また、本実施形態に係る距離画像センサ２では、奥行きを計測可能な有効距離が定められており、深度画像には、奥行きの値が得られない画素が含まれ得る。

距離画像センサ２には、距離画像センサ２の動作全体を制御するＣＰＵ２３の他、撮影された画像データを少なくとも一時的に記憶するメモリ２４が内蔵されている。距離画像センサ２の動作を制御する制御プログラムは、メモリ２４内に格納されている。また、距離画像センサ２には、通信部２５も内蔵されており、当該通信部２５は、撮影された画像データを、有線又は無線の通信線１７を介して、運動解析装置１等の外部のデバイスへと出力することができる。本実施形態では、ＣＰＵ２３及びメモリ２４も、ＩＲ発光部２１及びＩＲ受光部２２とともに、筐体２０内に収納されている。なお、運動解析装置１への画像データの受け渡しは、必ずしも通信部２５を介して行う必要はない。例えば、メモリ２４が着脱式であれば、これを筐体２０内から取り外し、運動解析装置１のリーダー（後述する通信部１５に対応）に挿入する等して、運動解析装置１で画像データを読み出すことができる。

本実施形態では、以上のとおり、距離画像センサ２により赤外線撮影が行われ、撮影されたＩＲ画像に基づいて、グリップ５１の軌道が追跡される。従って、この追跡が容易となるように、グリップ５１（より正確には、グリップ端５１ａ）には、赤外線を効率的に反射する反射シートがマーカーとして貼付される。また、シャフト５２にも、同様の赤外線の反射シートがマーカーとして貼付される。本実施形態では、距離画像センサ２は、ゴルファー７を正面側から撮影すべく、ゴルファー７の前方に設置される。

＜１−２−２．慣性センサユニット＞
慣性センサユニット４は、図１及び図３に示すとおり、ゴルフクラブ５のグリップ端５１ａに取り付けられており、グリップ端５１ａの挙動を計測する。本実施形態に係る慣性センサユニット４は、着脱自在に構成されており、任意のゴルフクラブ５に取り付けることができる。なお、ゴルフクラブ５は、一般的なゴルフクラブであり、シャフト５２と、シャフト５２の一端に設けられたヘッド５３と、シャフト５２の他端に設けられたグリップ５１とから構成される。慣性センサユニット４は、スイング動作の妨げとならないよう、小型且つ軽量に構成されている。図２に示すように、本実施形態に係る慣性センサユニット４には、加速度センサ４１、角速度センサ４２及び地磁気センサ４３が搭載されている。また、慣性センサユニット４には、これらのセンサ４１〜４３から出力されるセンサデータを運動解析装置１等の外部のデバイスに送信するための通信装置４０も搭載されている。なお、本実施形態では、通信装置４０は、スイング動作の妨げにならないように無線式であるが、ケーブルを介して有線式に運動解析装置１に接続するようにしてもよい。

加速度センサ４１、角速度センサ４２及び地磁気センサ４３はそれぞれ、ｘｙｚ局所座標系における加速度、角速度及び地磁気を計測する。より具体的には、加速度センサ４１は、ｘ軸、ｙ軸及びｚ軸方向の加速度ａ_x，ａ_y，ａ_zを計測する。角速度センサ４２は、ｘ軸、ｙ軸及びｚ軸周りの角速度ω_x，ω_y，ω_zを計測する。地磁気センサ４３は、ｘ軸、ｙ軸及びｚ軸方向の地磁気ｍ_x，ｍ_y，ｍ_zを計測する。これらのセンサデータは、所定のサンプリング周期Δｔの時系列データとして取得される。なお、ｘｙｚ局所座標系は、図３に示すとおりに定義される３軸直交座標系である。すなわち、ｚ軸は、シャフト５２の延びる方向に一致し、ヘッド５３からグリップ５１に向かう方向が、ｚ軸正方向である。ｙ軸は、ゴルフクラブ５のアドレス時の飛球方向にできる限り沿うように、すなわち、フェース−バック方向に概ね沿うように配向される。ｘ軸は、ｙ軸及びｚ軸に直交するように、すなわち、トゥ−ヒール方向に概ね沿うように配向され、ヒール側からトゥ側に向かう方向がｘ軸正方向である。

本実施形態では、加速度センサ４１、角速度センサ４２及び地磁気センサ４３からのセンサデータは、通信装置４０を介してリアルタイムに運動解析装置１に送信される。しかしながら、例えば、慣性センサユニット４内の記憶装置にセンサデータを格納しておき、スイング動作の終了後に当該記憶装置からセンサデータを取り出して、運動解析装置１に受け渡すようにしてもよい。

＜１−２−３．運動解析装置＞
運動解析装置１は、ハードウェアとしては汎用のパーソナルコンピュータであり、例えば、タブレットコンピュータ、スマートフォン、ノート型コンピュータ、デスクトップ型コンピュータとして実現される。図２に示すとおり、運動解析装置１は、ＣＤ−ＲＯＭ等のコンピュータで読み取り可能な記録媒体３０から運動解析プログラム３を汎用のコンピュータにインストールすることにより製造される。運動解析プログラム３は、慣性センサユニット４から送られてくるセンサデータ及び距離画像センサ２から送られてくる画像データに基づいて、ゴルフスイングを解析するためのソフトウェアであり、運動解析装置１に後述する動作を実行させる。

運動解析装置１は、表示部１１、入力部１２、記憶部１３、制御部１４及び通信部１５を備える。これらの部１１〜１５は、互いにバス線１６を介して接続されており、相互に通信可能である。表示部１１は、液晶ディスプレイ等で構成することができ、ゴルフスイングの解析の結果等をユーザに対し表示する。なお、ここでいうユーザとは、ゴルファー７自身やそのインストラクター、ゴルフ用品の開発者等、ゴルフスイングの解析の結果を必要とする者の総称である。入力部１２は、マウス、キーボード、タッチパネル等で構成することができ、運動解析装置１に対するユーザからの操作を受け付ける。

記憶部１３は、ハードディスク等で構成することができる。記憶部１３内には、運動解析プログラム３が格納されている他、慣性センサユニット４から送られてくるセンサデータが保存される。また、記憶部１３内には、距離画像センサ２から送られてくる画像データも保存される。制御部１４は、ＣＰＵ、ＲＯＭおよびＲＡＭ等から構成することができる。制御部１４は、記憶部１３内の運動解析プログラム３を読み出して実行することにより、仮想的に第１取得部１４ａ、第２取得部１４ｂ、導出部１４ｃ及び表示制御部１４ｄとして動作する。各部１４ａ〜１４ｄの動作の詳細については、後述する。通信部１５は、慣性センサユニット４や距離画像センサ２等の外部のデバイスから通信線１７を介してデータを受信する通信インターフェースとして機能する。

＜１−３．運動解析方法＞
以下、図５を参照しつつ、ゴルフスイングを対象とする運動解析方法について説明する。この運動解析方法は、第１データ収集処理（Ｓ１）、第２データ収集処理（Ｓ２）及び運動解析処理（Ｓ３〜Ｓ９）により実現される。第１データ収集処理は、慣性センサユニット４から出力されるセンサデータを収集する処理であり、第２データ収集処理は、距離画像センサ２から出力される画像データを収集する処理である。運動解析処理は、当該センサデータ及び当該画像データに基づいて、運動解析装置１によりゴルフクラブ５の三次元挙動を解析する処理である。以下、これらの処理について、順に説明する。

＜１−３−１．第１データ収集処理＞
第１データ収集処理では、ゴルファー７により、上述の慣性センサユニット４付きゴルフクラブ５がスイングされる。このとき、慣性センサユニット４により、ゴルフスイング中の加速度ａ_x，ａ_y，ａ_z、角速度ω_x，ω_y，ω_z及び地磁気ｍ_x，ｍ_y，ｍ_zのセンサデータが検出される。また、これらのセンサデータは、慣性センサユニット４の通信装置４０を介して運動解析装置１に送信される。一方、運動解析装置１側では、第１取得部１４ａが通信部１５を介してこれを受信し、記憶部１３内に格納する。本実施形態では、少なくともアドレスの少し前（以下、初期時刻ｔ₁という）からフィニッシュまでの時系列のセンサデータが収集される。

なお、ゴルフクラブのスイング動作は、一般に、アドレス、トップ、インパクト、フィニッシュの順に進む。アドレスとは、図４（Ａ）に示すとおり、ゴルフクラブ５のヘッド５３をボール近くに配置した静止状態を意味し、トップとは、図４（Ｂ）に示すとおり、アドレスからゴルフクラブ５をテイクバックし、最もヘッド５３が振り上げられた状態を意味する。インパクトとは、図４（Ｃ）に示すとおり、トップからゴルフクラブ５が振り下ろされ、ヘッド５３がボールと衝突した瞬間の状態を意味し、フィニッシュとは、図４（Ｄ）に示すとおり、インパクト後、ゴルフクラブ５を前方へ振り抜いた状態を意味する。

＜１−３−２．第２データ収集処理＞
第２データ収集処理では、ゴルフクラブ５のスイング動作が距離画像センサ２により撮影される。すなわち、距離画像センサ２により、ゴルフスイングを捉えたＩＲ画像及び深度画像を含む画像データが検出される。第２データ収集処理は、第１データ収集処理と並行して、第１データ収集処理と同じスイング動作を対象として行われる。検出された画像データは、距離画像センサ２の通信部２５を介して運動解析装置１に送信される。一方、運動解析装置１側では、第２取得部１４ｂが通信部１５を介してこれを受信し、記憶部１３内に格納する。本実施形態では、少なくともアドレスの少し前の初期時刻ｔ₁からフィニッシュまでの時系列の画像データが収集される。

＜１−３−３．運動解析処理＞
運動解析処理の大まかな流れは、以下のとおりである。すなわち、まず、第１データ収集処理により取得された時系列のセンサデータと、第２データ収集処理により取得された時系列の画像データとの時刻合わせが行われる（Ｓ３）。言い換えると、導出部１４ｃが、センサデータと画像データとを同期させる。

続くステップＳ４では、導出部１４ｃが、初期時刻ｔ₁からアドレスまでの期間（以下、アドレス期間という）のシャフト５２の姿勢を導出する。このとき、本実施形態では、ラグランジュの未定乗数法を用いた最適化が実行される。なお、多くのゴルファー７は、アドレス期間にゴルフクラブ５を軽く前後にスイングさせるワッグル動作を行う。しかしながら、アドレス以降のスイング動作時に比べると、アドレス期間において、ゴルフクラブ５、特にグリップ５１は殆ど運動しない。このため、ステップＳ４では、静止している移動体及びほぼ静止している移動体の運動の解析に適したアルゴリズムが用いられる。

続くステップＳ５では、導出部１４ｃが、アドレスからインパクト付近までのシャフト５２の姿勢を導出する。従って、ステップＳ５では、静止している移動体及びほぼ静止している移動体の運動のみならず、高速で運動する移動体の解析にも適したアルゴリズムが用いられる。本実施形態では、ここでも、ラグランジュの未定乗数法を用いた最適化が実行される。

続くステップＳ６では、導出部１４ｃが、アドレスからインパクト付近までのグリップ５１の位置及び速度を導出する。ここでは、本実施形態では、カルマンフィルタを用いる等したスムージングが実行される。

続くステップＳ７では、導出部１４ｃが、アドレスからインパクト付近までのグリップ５１の角速度及び角加速度を導出する。ここでは、本実施形態では、疑似微分フィルタを用いたフィルタリングが実行される。

続くステップＳ８では、導出部１４ｃが、アドレスからインパクト付近までのグリップ５１の加速度を導出する。本実施形態では、ここでも、疑似微分フィルタを用いたフィルタリングが実行される。

続くステップＳ９では、表示制御部１４ｄが、ステップＳ４〜Ｓ８で導出された三次元挙動を示す情報を表示部１１上に表示する。このとき、例えば、数値情報、文字情報として表示することもできるし、グラフやコンピュータグラフィックス等を用いて図式的に表示することもできる。

＜１−３−３−１．時刻合わせ（Ｓ３）＞
以下、図６を参照しつつ、上記ステップＳ３における、時系列のセンサデータと時系列の画像データとの時刻合わせ処理について説明する。

まず、ステップＳ１１において、導出部１４ｃは、記憶部１３内のセンサデータを参照し、アドレス、トップ及びインパクトの時刻を導出する。これらの時刻の導出方法としては、様々なものが公知であるため、ここでは詳細な説明を省略する。

続くステップＳ１２では、導出部１４ｃは、記憶部１３内のセンサデータに基づいて、アドレスからインパクト付近までの各時刻における姿勢行列Ｎを導出する。今、姿勢行列Ｎを以下の式で表す。姿勢行列Ｎは、ｘｙｚ局所座標系の値をＸＹＺ慣性座標系の値に変換するための行列である。

ＸＹＺ慣性座標系は、図１に示すとおりに定義される３軸直交座標系である。すなわち、Ｚ軸は、鉛直下方から上方に向かう方向であり、Ｘ軸は、ゴルファー７の背から腹に向かう方向であり、Ｙ軸は、地平面に平行でボールの打球地点から目標地点に向かう方向である。

姿勢行列Ｎの９つの成分の意味は、以下のとおりである。
成分ａ：慣性座標系のＸ軸と、局所座標系のｘ軸とのなす角度の余弦
成分ｂ：慣性座標系のＹ軸と、局所座標系のｘ軸とのなす角度の余弦
成分ｃ：慣性座標系のＺ軸と、局所座標系のｘ軸とのなす角度の余弦
成分ｄ：慣性座標系のＸ軸と、局所座標系のｙ軸とのなす角度の余弦
成分ｅ：慣性座標系のＹ軸と、局所座標系のｙ軸とのなす角度の余弦
成分ｆ：慣性座標系のＺ軸と、局所座標系のｙ軸とのなす角度の余弦
成分ｇ：慣性座標系のＸ軸と、局所座標系のｚ軸とのなす角度の余弦
成分ｈ：慣性座標系のＹ軸と、局所座標系のｚ軸とのなす角度の余弦
成分ｉ：慣性座標系のＺ軸と、局所座標系のｚ軸とのなす角度の余弦
ここで、ベクトル（ａ，ｂ，ｃ）は、ｘ軸方向の単位ベクトルを表し、ベクトル（ｄ，ｅ，ｆ）は、ｙ軸方向の単位ベクトルを表し、ベクトル（ｇ，ｈ，ｉ）は、ｚ軸方向の単位ベクトルを表している。

アドレス時においては、ゴルフクラブ５は静止しているため、加速度センサ４２は、Ｚ軸方向の重力加速度のみを検出することになる。この事実から、アドレス時のｃ，ｆ，ｉは、下式に従って算出することができる。

また、残りのａ，ｂ，ｄ，ｅ，ｇ，ｈは、ｃ，ｆ，ｉを説明変数とし、ａ，ｂ，ｄ，ｅ，ｇ，ｈのそれぞれを目的変数とする予め定められた重回帰式より算出することができる。この重回帰式は、多数のａ〜ｉの真値のデータを重回帰分析することにより決定することができる。或いは、姿勢行列は、下式のとおり、オイラー変換行列としても表すことができる。従って、姿勢行列の９つの成分のうち３つの値が分かれば、φ，θ，ψを特定できるため、残りの成分を算出することができる。なお、姿勢行列の算出方法としては様々考えられ、いずれの方法を用いてもよいが、ここで説明した方法は、例えば、特開２０１３−５６０７４号公報においても開示されている。

導出部１４ｃは、アドレス時の姿勢行列Ｎの導出後、アドレス時の姿勢行列Ｎを初期値として、姿勢行列Ｎをサンプリング周期Δｔ間隔で時々刻々更新してゆく。具体的に説明すると、まず、姿勢行列Ｎは、オイラーパラメータｑ＝（ｑ₀，ｑ₁，ｑ₂，ｑ₃）を用いて、以下の式で表される。

従って、ある時刻でのａ〜ｉが既知の場合、当該時刻でのオイラーパラメータｑ＝（ｑ₀，ｑ₁，ｑ₂，ｑ₃）は、以下の式に従って算出することができる。

今、アドレス時の姿勢行列Ｎを規定するａ〜ｉの値は既知である。よって、以上の式に従って、まず、アドレス時のオイラーパラメータｑが算出される。

そして、時刻ｔから微小時刻経過後のオイラーパラメータｑ_t+1は、時刻ｔにおけるオイラーパラメータｑ_tを用いて以下の式で表される。

また、オイラーパラメータｑの時間変化を表す１階微分方程式は、以下の式で表される。

数６，７の式を用いれば、角速度データに基づいて、時刻ｔのオイラーパラメータｑを順次、次の時刻ｔ＋Δｔのオイラーパラメータｑへと更新することができる。ここでは、アドレスからインパクトまでのオイラーパラメータｑが算出される。そして、アドレスからインパクトまでのオイラーパラメータｑを数４の式に順次代入することにより、アドレスからインパクトまでの姿勢行列Ｎが算出される。

なお、慣性センサユニットから出力されるセンサデータに基づいて、オイラーパラメータｑや姿勢行列Ｎを算出する方法としては、ここで説明した方法に限らず、様々な方法が知られている。例えば、「根田康美，藤橋幸太，尾曲邦之，藤原謙，松永三郎"超小型衛星 Cute-1.7 シリーズにおける姿勢決定制御システムについて"第 16 回スペース・エンジニアリング・コンファレンス，東京，2007 年 1 月 25 日」に開示されている技術を利用することもできる。

続くステップＳ１３では、導出部１４ｃは、記憶部１３内の画像データに基づいて、アドレスからインパクト付近までの各時刻における時系列のシャフト５２の向きθ_s（図７参照）を導出する。具体的には、本実施形態では、ＩＲ画像を画像処理することにより、直線状のシャフト５２の像を検出する。そして、このシャフト５２の像が延びる方向とＺ方向との為す角度を、シャフト５２の向きθ_sとして導出する。

続くステップＳ１４では、導出部１４ｃは、ステップＳ１２の時系列の姿勢行列Ｎに基づいて、アドレスからインパクト付近までの各時刻における時系列のシャフト５２の向きθ_sを導出する。具体的には、θ_sは、以下の式に従って算出することができる。

次に、導出部１４ｃは、このセンサデータ由来の時系列のシャフト５２の向きθ_sと、ステップＳ１３の時系列のシャフト５２の向きθ_sとの一致度が最も高くなるように、両時系列データを位置合わせする。例えば、導出部１４ｃは、図８に示すように、両時系列データを時間軸に沿って前後に移動させながら、両時系列データの波形が最も一致する点を決定する。そして、そのときのタイミングを基準として、センサデータの時間軸と、画像データの時間軸とを相対的に時刻合わせさせる。

なお、本実施形態では、二次元のシャフト５２の向きθ_sに基づいて時刻合わせが行われたが、三次元のシャフト５２の向きθ_sに基づいて時刻合わせを行うこともできる。すなわち、姿勢行列Ｎからは、三次元的なシャフト５２の向き（ｇ，ｈ，ｉ）を導出することが可能であるし、深度画像を用いれば、画像データからも三次元的なシャフト５２の向きを導出することが可能である。従って、センサデータ及び画像データのそれぞれを由来とする三次元的なシャフト５２の向きの時系列データを導出し、これらの時系列データの波形が三次元的に最も一致するタイミングを決定すればよい。

＜１−３−３−２．アドレス期間の姿勢の導出（Ｓ４）＞
以下、図９を参照しつつ、アドレス期間の姿勢の導出処理の流れを説明する。ここで、オイラーパラメータｑは、上記のとおり、姿勢行列Ｎとの間で相互に変換可能である。また、上記のとおり、姿勢行列Ｎに含まれる行ベクトルは、慣性座標系内での局所座標系の三軸の向きを表している。従って、オイラーパラメータｑ及び姿勢行列Ｎの少なくとも一方が分かれば、慣性座標系内での局所座標系の三軸の向き、つまりはｚ軸に平行なシャフト５２の向き（ｇ，ｈ，ｉ）が分かる。そのため、以下では、シャフト５２の姿勢として、オイラーパラメータｑ及び姿勢行列Ｎが導出される。

まず、ステップＳ２１では、導出部１４ｃは、アドレス期間に含まれる各時刻の姿勢ｑ，Ｎを導出する。なお、以下のステップＳ２６では、ラグランジュの未定乗数法により、姿勢ｑが最適化される。この意味で、ステップＳ２１で導出される姿勢ｑ，Ｎは、仮の姿勢と言える。

具体的には、導出部１４ｃは、アドレス期間に含まれる最初の時刻である初期時刻ｔ₁のオイラーパラメータｑを算出する。具体的には、導出部１４ｃは、初期時刻ｔ₁のＩＲ画像及び深度画像からシャフト５２の向きを三次元的に導出する。すなわち、画像処理によりＩＲ画像からシャフト５２の像を検出し、当該像に含まれる各点のＹ座標及びＺ座標を導出する。そして、深度画像からこれらの点における深度情報を抽出し、Ｘ座標に変換する。その後、これらの三次元座標から三次元のシャフト５２の向き（ｇ，ｈ，ｉ）が導出される。また、導出部１４ｃは、初期時刻ｔ₁の加速度ａ_x，ａ_y，ａ_zはほぼ重力のみを表しているという事実に基づいて、初期時刻ｔ₁の加速度データ（ａ_x，ａ_y，ａ_z）から三次元の重力の向きを導出する。そして、導出部１４ｃは、シャフト５２の向き（ｇ，ｈ，ｉ）と、重力の向きの外積を導出し、これをｙ軸の方向（ｄ，ｅ，ｆ）とする。さらに、三次元のシャフト５２の向き（ｇ，ｈ，ｉ）と、ｙ軸の方向（ｄ，ｅ，ｆ）との外積を導出し、これをｘ軸の方向（ａ，ｂ，ｃ）とする。これにより、初期時刻ｔ₁の姿勢行列Ｎが導出される。また、導出部１４ｃは、数５の式に従って、初期時刻ｔ₁の姿勢行列Ｎを初期時刻ｔ₁のオイラーパラメータｑに変換する。

また、ステップＳ２１では、導出部１４ｃは、初期時刻ｔ₁の姿勢行列Ｎを初期値として、姿勢行列Ｎをサンプリング周期Δｔ間隔で時々刻々更新してゆく。具体的な方法は、上述のステップＳ１２と同様であるため、ここでは詳細な説明を省略する。

続くステップＳ２２では、導出部１４ｃは、初期時刻ｔ₁の局所座標系での地磁気ｍ^B＝（ｍ^B _x，ｍ^B _y，ｍ^B _z）＝（ｍ_x，ｍ_y，ｍ_z）を、慣性座標系での地磁気ｍ^I＝（ｍ^I _x，ｍ^I _y，ｍ^I _z）に変換する。なお、ｍ^I，ｍ^Bに限らず、右肩にＢを付けたときは、局所座標系のベクトルであることを表し、右肩にＩを付けた場合には、慣性座標系のベクトルであることを表す。ｍ^Iは、時刻によらず一定とすることができ、オイラーパラメータｑを用いて、初期時刻ｔ₁のｍ^Bから下式に従って算出することができる。なお、以下のｑには、ステップＳ２１で導出された初期時刻ｔ₁のオイラーパラメータｑを代入する。
ただし、
である。

地磁気ｍ^Iは、別の方法によっても導出可能である。例えば、磁気センサ４３とは異なる磁気センサを、慣性座標系の原点において、当該磁気センサの三軸を慣性座標系の三軸に一致させた状態で配置する。この場合、当該磁気センサの出力値は、慣性座標系での地磁気ｍ^Iを表すことになる。

続くステップＳ２３では、導出部１４ｃは、アドレス期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、時刻ｔ_iでの姿勢ｑ_iと、時刻ｔ_jにおける姿勢ｑ_jとの差である、ｑ_BjBiを導出する。このとき、ｔ_j，ｔ_iの先後は問わない。姿勢の差ｑ_BjBiは、下式に従って算出することができる。なお、下式に代入されるオイラーパラメータｑ_i，ｑ_jは、ステップＳ２１で算出されたオイラーパラメータｑである。

姿勢の差ｑ_BjBiは、別の方法によっても導出可能である。例えば、数１１の式に代入すべきアドレス期間に含まれる各時刻のオイラーパラメータｑを、ステップＳ２１で算出したものではなく、以下の方法で算出することができる。すなわち、上記ステップＳ２１では、初期時刻ｔ₁より後の各時刻のオイラーパラメータｑは、角速度データを用いて初期時刻ｔ₁のオイラーパラメータｑを順次更新してゆくステップＳ１２と同様の方法により算出された。しかしながら、初期時刻ｔ₁より後の各時刻の姿勢行列Ｎについても、初期時刻ｔ₁の場合と同様に、画像データ及び加速度データを用いて外積を計算する方法により算出してもよい。

あるいは、アドレス期間における姿勢の差ｑ_BjBiは、単位クォータニオン（１，０，０，０）と近似することができる。これは、アドレス期間においては、グリップ５１が静止しているか又は殆ど動いておらず、姿勢の差が殆どないためである。

続くステップＳ２４では、導出部１４ｃは、アドレス期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、以下に定義される行列Ａ_kjを導出する。Ａ_kjは、後のステップＳ２６の最適化処理の中で、目的関数に代入される。

以下、Ａ_kj及び上式中の右辺の各記号ついて説明する。まず、時刻ｔ_i，ｔ_jにおける慣性座標系での三次元のシャフト５２の向きを表すベクトルをそれぞれｂ^I _i、ｂ^I _jとし、時刻ｔ_i，ｔ_jにおける局所座標系での三次元のシャフト５２の向きを表すベクトルをそれぞれｂ^B _i，ｂ^B _jとする。なお、ｂ^B _i，ｂ^B _jは、時刻によらず一定値［０ ０ −１］^Tとすることができ（上付きＴは転置を表す）、従って、ｂ^B _i，ｂ^B _jはいずれも、単にｂ^Bと表記することができる。このとき、下式が成り立つ。

また、数１１より、下式が導かれる。

ここで、数１３，１４の式を変形すると、以式が成り立つ。
ただし、
である。数１５の式を変形すると、下式のようになる。

数１７の式からは、ｑ_iに関して線形の関係が成り立っていることが分かる。この式を整理すると、以下の関係式が導かれる。

数１８の関係式は、画像データやセンサデータに誤差がなければ、常に成り立つ恒等式である。数１８の関係式は、ステップＳ２６の最適化に用いられる目的関数を定義するのに用いられる。

ここで、数１２の式に戻ると、ｂ^I _jx，ｂ^I _jy，ｂ^I _jzは、時刻ｔ_jにおける慣性座標系での三次元のシャフト５２の向きを表すｂ^I _jのＸ，Ｙ，Ｚ軸成分を意味する。また、ｂ_jix，ｂ_jiy，ｂ_jizは、数１６により定義されるｂ_jiのＸ，Ｙ，Ｚ軸成分を意味する。なお、記号のハットは、正規化されていることを意味する。

ステップＳ２４では、導出部１４ｃは、アドレス期間のＩＲ画像及び深度画像に基づいて、アドレス期間の各時刻における三次元のシャフト５２の向きｂ^I _jを導出する。そして、Ａ_kjを導出すべく、その三軸成分であるｂ^I _jx，ｂ^I _jy，ｂ^I _jzの値を、数１２の式に代入する。また、シャフト５２の向きを表すｂ^B＝（ｂ_jx，ｂ_jy，ｂ_jz）＝［０ ０ −１］^Tと、ステップＳ２３のｑ_BjBiを数１６の式に代入することにより、ｂ_jiを導出し、さらにこれを数１２に代入する。

続くステップＳ２５では、導出部１４ｃは、アドレス期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、以下に定義される行列Ａ_mjを導出する。Ａ_mjは、Ａ_kjと同様に、後のステップＳ２６の最適化処理の中で目的関数に代入される。

ステップＳ２４の中で説明した場合と同様に、下式が導かれる。この関係式は、ステップＳ２６の最適化に用いられる目的関数を定義するのに用いられる。

ここで、数１９の式に戻ると、ｍ^I _jx，ｍ^I _jy，ｍ^I _jzは、時刻ｔ_jにおける慣性座標系での地磁気を表すｍ^I _jのＸ，Ｙ，Ｚ軸成分を意味する。また、ｍ_jix，ｍ_jiy，ｍ_jizは、下式により定義されるｍ_jiのＸ，Ｙ，Ｚ軸成分を意味する。

ステップＳ２５では、導出部１４ｃは、数１９の右辺において、ｍ^I _jとしてステップＳ２２で導出された地磁気のデータを代入する。また、時刻ｔ_jにおける地磁気のセンサデータであるｍ^B _j＝（ｍ_jx，ｍ_jy，ｍ_jz）と、ステップＳ２３のｑ_BjBiを数２１の式に代入することにより、ｍ_jiを導出し、さらにこれを数１９の右辺に代入する。

続くステップＳ２６では、ラグランジュの未定乗数法により、アドレス期間の各時刻ｔ_iにおける姿勢ｑ_iが最適化される。具体的には、各時刻ｔ_iにおいて以下の目的関数Ｊ₁を定義する。
ただし、
である。

ここで、数１８，２０から明らかなとおり、Ｆ_k，Ｆ_mは、画像データやセンサデータに誤差がなければ０になるべき関数である。また、（１−ｑ_i ^Tｑ_i）は、拘束条件である。そこで、導出部１４ｃは、ステップＳ２４，Ｓ２５で計算したＡ_kj，Ａ_mjを用いて、目的関数Ｊ₁を最小化するようなオイラーパラメータｑ_iの最適解を導出する。また、導出部１４ｃは、各時刻ｔ_iにおける最適化後のオイラーパラメータｑ_iを数４の式に従って、姿勢行列Ｎに変換する。

なお、ｗ_k，ｗ_mは、画像データ、地磁気データのいずれをどれだけ信頼するかに応じて、設定される重み係数である。

また、本実施形態では、目的関数Ｊ₁の最小化は、以下のとおり行われる。すなわち、目的関数Ｊ₁を最小化するためには、その微分値（偏微分値）がゼロになること、言い換えると、下式が成立することが必要条件となる。ただし、Ｉ₄は、４×４の単位行列である。

上式は、下式の行列の固有値がλとなり、ｑ_iが固有ベクトルになることを表している。

従って、導出部１４ｃは、ステップＳ２４，Ｓ２５の結果から、数２５の行列を計算し、当該行列の最小の固有値に対応する大きさ１の固有ベクトルを導出して、これをｑ_iとする。

＜１−３−３−３．アドレスからインパクト付近までの姿勢の導出（Ｓ５）＞
以下、図１０を参照しつつ、アドレスからインパクト付近までの姿勢の導出処理の流れを説明する。本処理では、アドレス期間における姿勢の導出処理と類似のアルゴリズムが用いられる。両処理の主な相違点は、最適化のための目的関数Ｊ₂に、上記関数Ｆ_k，Ｆ_mに加え、画像データ由来の二次元のシャフト５２の向きに基づく関数Ｆ_nが含まれる点にある。以下、詳細に説明する。

ステップＳ３１では、導出部１４ｃは、アドレスからインパクト付近までの各時刻の仮の姿勢ｑ，Ｎを導出する。具体的には、導出部１４ｃは、角速度ω_x，ω_y，ω_zのデータを用いて、数６，７の式に従って、最適化後のアドレス時のオイラーパラメータｑを初期値としてオイラーパラメータｑをサンプリング周期Δｔ間隔で時々刻々更新してゆく。その後、数５の式に従って、各時刻の姿勢ｑを、姿勢行列Ｎに換算する。

続くステップＳ３２では、導出部１４ｃは、アドレス時の局所座標系での地磁気ｍ^B＝（ｍ^B _x，ｍ^B _y，ｍ^B _z）＝（ｍ_x，ｍ_y，ｍ_z）を、慣性座標系での地磁気ｍ^I＝（ｍ^I _x，ｍ^I _y，ｍ^I _z）に変換する。具体的な変換方法は、ステップＳ２２と同様である。ただし、ここで変換に用いられるオイラーパラメータｑは、ステップＳ３１で算出されたオイラーパラメータｑである。

続くステップＳ３３では、導出部１４ｃは、アドレスからインパクト付近までの期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、時刻ｔ_iでの姿勢ｑ_iと、時刻ｔ_jにおける姿勢ｑ_jとの差である、ｑ_BjBiを導出する。このとき、ｔ_j，ｔ_iの先後は問わない。姿勢の差ｑ_BjBiは、数１１の式に従って算出することができる。なお、ここで数１１の式に代入されるオイラーパラメータｑ_i，ｑ_jは、ステップＳ３１で算出されたオイラーパラメータｑである。

続くステップＳ３４では、導出部１４ｃは、アドレスからインパクト付近までの期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、数１２のとおり定義される行列Ａ_kjを導出する。Ａ_kjは、後のステップＳ３７の最適化処理の中で、目的関数に代入される。

ステップＳ３４では、導出部１４ｃは、アドレスからインパクト付近までの期間のＩＲ画像及び深度画像に基づいて、当該期間の各時刻における三次元のシャフト５２の向きｂ^I _jを導出する。そして、Ａ_kjを導出すべく、その三軸成分であるｂ^I _jx，ｂ^I _jy，ｂ^I _jzの値を、数１２の式に代入する。また、シャフト５２の向きを表すｂ^B＝（ｂ_jx，ｂ_jy，ｂ_jz）＝［０ ０ −１］^Tと、ステップＳ３３のｑ_BjBiを数１６の式に代入することにより、ｂ_jiを導出し、さらにこれを数１２に代入する。

続くステップＳ３５では、導出部１４ｃは、アドレスからインパクト付近までの期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、数１９のとおり定義される行列Ａ_mjを導出する。Ａ_mjは、後のステップＳ３７の最適化処理の中で、目的関数に代入される。

ステップＳ３５では、導出部１４ｃは、数１９の右辺において、ｍ^I _jとしてステップＳ３２で導出された地磁気のデータを代入する。また、時刻ｔ_jにおける地磁気のセンサデータであるｍ^B _j＝（ｍ_jx，ｍ_jy，ｍ_jz）と、ステップＳ３３のｑ_BjBiを数２１の式に代入することにより、ｍ_jiを導出し、さらにこれを数１９の右辺に代入する。

続くステップＳ３６では、導出部１４ｃは、アドレスからインパクト付近までの期間に含まれる任意の時刻ｔ_i，ｔ_jの組み合わせに対し、以下に定義される行列Ａ_njを導出する。Ａ_njは、後のステップＳ３７の最適化処理の中で、目的関数に代入される。

以下、Ａ_nj及び上式中の右辺の各記号ついて説明する。まず、ここでは、ＹＺ平面内における二次元のシャフト５２の向きを考える。ここで、ＸＹＺ座標系における三次元座標をＹＺ平面に射影するための射影行列をＰ_YZとする。このとき、時刻ｔ_iにおけるＹＺ平面内における二次元のシャフトの向きｂ_Siは、Ｐ_YZｂ^I _iと表される。ｂ_Siは、オイラーパラメータｑ_iを用いて、以下のとおり表すことができる。ただし、ｕ＝[１ ０ ０]^Tである。

数２７を変形すると、以下のとおりとなる。
さらに、数１４の式から、数２８を変形する。

さらに、数２９の両辺の右からｑ_BjBiを掛ける。
さらに、数１３，１６から、以下のように変形される。

数３１の式からは、ｑ_iに関して線形の関係が成り立っていることが分かる。この式を整理すると、以下の関係式が導かれる。

数３２の関係式は、画像データやセンサデータに誤差がなければ、常に成り立つ恒等式である。数３２の関係式は、ステップＳ３７の最適化に用いられる目的関数を定義するのに用いられる。

ここで、数２６の式に戻ると、ｂ_Sjx，ｂ_Sjy，ｂ_Sjzは、時刻ｔ_jにおけるＹＺ平面内における二次元のシャフトの向きを表すｂ_SjのＸ，Ｙ，Ｚ軸成分を意味する。従って、Ｘ成分であるｂ_Sjx＝０である。

ステップＳ３６では、導出部１４ｃは、Ａ_njを導出すべく、ｂ^B＝［０ ０ −１］^Tを数１３を用いて慣性座標系の値（ｂ^I _jx，ｂ^I _jy，ｂ^I _jz）に変換し、これを数２６の式に代入する。また、シャフト５２の向きを表すｂ^B＝（ｂ_jx，ｂ_jy，ｂ_jz）＝［０ ０ −１］^Tと、ステップＳ３３のｑ_BjBiを数１６の式に代入することにより、ｂ_jiを導出し、さらにこれを数２６に代入する。さらに、導出部１４ｃは、ＩＲ画像から導出されるｂ_Sjを、数２６の式に代入する。

続くステップＳ３７では、ラグランジュの未定乗数法により、アドレスからインパクト付近までの各時刻ｔ_iにおける姿勢ｑ_iが最適化される。具体的には、各時刻ｔ_iにおいて以下の目的関数Ｊ₂を定義する。
ただし、
である。

ここで、数３２から明らかなとおり、Ｆ_nは、Ｆ_k，Ｆ_mと同じく、画像データやセンサデータに誤差がなければ０になるべき関数である。また、（１−ｑ_i ^Tｑ_i）は、拘束条件である。そこで、導出部１４ｃは、ステップＳ３４〜Ｓ３６で計算したＡ_kj，Ａ_mj，Ａ_njを用いて、目的関数Ｊ₂を最小化するようなオイラーパラメータｑ_iの最適解を導出する。また、導出部１４ｃは、各時刻ｔ_iにおける最適化後のオイラーパラメータｑ_iを数４の式に従って、姿勢行列Ｎに変換する。

なお、ｗ_k，ｗ_m，ｗ_nは、深度画像のデータ、地磁気データ、ＩＲ画像のデータのいずれをどれだけ信頼するかに応じて、設定される重み係数である。

また、本実施形態では、目的関数Ｊ₂の最小化は、以下のとおり行われる。すなわち、目的関数Ｊ₂を最小化するためには、その微分値（偏微分値）がゼロになること、言い換えると、下式が成立することが必要条件となる。

上式は、下式の行列の固有値がλとなり、ｑ_iが固有ベクトルになることを表している。

従って、導出部１４ｃは、ステップＳ３３〜Ｓ３６の結果から、数３６の行列を計算し、当該行列の最小の固有値に対応する大きさ１の固有ベクトルを導出して、これをｑ_iとする。

＜１−３−３−４．グリップの位置及び速度の導出（Ｓ６）＞
以下、図１１を参照しつつ、アドレスからインパクト付近までのグリップ５１の位置及び速度の導出処理の流れを説明する。

グリップ５１の位置は、画像データにより計測される。しかしながら、距離画像センサ２のサンプリング周期が粗い場合や、グリップ５１が画像上に写らない場合等には、位置の測定が困難になることがある。そこで、ここでは、カルマンフィルタを用いる等して、画像データ由来のグリップ５１の位置が加速度データによりスムージングされる。

まず、ステップＳ４１では、導出部１４ｃは、下式に従って、アドレスからインパクト付近までの各時刻ｔ_iにおける局所座標系の加速度ａ^B _i＝（ａ_x，ａ_y，ａ_z）を慣性座標系の加速度に変換し、さらに重力加速度の成分を除去する。これにより、慣性座標系でのグリップ５１の加速度ａ^I _iが導出される。なお、下式のオイラーパラメータｑ_iには、ステップＳ３７で導出された値を代入することができる。Ｇは、重力加速度の大きさである。

続くステップＳ４２，Ｓ４３では、導出部１４ｃは、ステップＳ４１の加速度ａ^I _iを用いて、ＩＲ画像及び深度画像から導出されるグリップ５１の三次元の位置ｐをスムージングすることにより、誤差の除去された位置ｐ及び速度ｐ'（推定値）を導出する。この意味で、ＩＲ画像及び深度画像から導出されるグリップ５１の三次元の位置ｐは、それぞれ仮の位置ということができる。

グリップ５１の位置ｐ及び速度ｐ'を推定するためのシステムは、以下のようにモデル化することができる。ただし、ｘは、求めたい位置ｐ及び速度ｐ'、ｙは、ＩＲ画像及び深度画像から導出される位置ｐ、ｕは、加速度データ、ｗは、プロセス雑音、ｖは、観測雑音を表す。添え字のｋは、時系列のデータ番号を表し、ｋ＝１，２，・・・，Ｎ（Ｎは、データ個数）である。なお、ステップＳ４２，Ｓ４３の説明において登場する符号／記号は、特に断らない限り、これまでの説明及びこれよりも後の説明に登場する符号／記号とは独立して定義されているものとする。
ただし、
とする。

また、プロセス雑音ｗ_kの平均値を０、分散をＱ_kとし、観測雑音ｖ_kの平均値を０、分散をＲ_kとする。このＱ_k及びＲ_kは、スムージングをかける程度を決定する因子となり、Ｑ_k／Ｒ_kが大きければあまりスムージングが行われず、距離画像センサ２の計測結果の比重が大きくなる。一方、Ｑ_k／Ｒ_kが小さければ、スムージングが強くなり、加速度センサ４１の計測結果の比重が大きくなる。

今、時間軸に沿って前向きにフィルタリングを行う場合の推定値をｘ⁺ _fk、誤差共分散をＰ⁺ _fkとする。導出部１４ｃは、ステップＳ４２において、ｋ＝０のときの初期値を以下のとおり導出する。なお、Ｅは、期待値を表す。

その後、導出部１４ｃは、この初期値を以下の式に従って、更新してゆく。

なお、ｘ⁺ _fkは、時刻ｋの状態の時刻ｋ時点での推定値を表し、ｘ^- _fkは、時刻ｋの状態の時刻（ｋ−１）時点での推定値を表す。誤差共分散についても同様に、Ｐ⁺ _fkは、時刻ｋの状態の時刻ｋ時点での推定値を表し、Ｐ^- _fkは、時刻ｋの状態の時刻（ｋ−１）時点での推定値を表す。また、ｘ^- _fk、Ｐ^- _fkは、以下のとおり算出することができる。
である。

以上により、過去及び現在の加速度データ及び画像データから誤差の除去されたｘ＝（ｐ，ｐ'）が推定される。なお、本実施形態では、画像データのサンプリング周期は、慣性センサユニット４の出力値である加速度データのサンプリング周期ΔＴよりも大きい。そのため、ステップＳ４２では、画像データを、線形補完等を適宜行って、加速度データのサンプリング周期ΔＴに合わせたデータセットにしておく前処理が行われる。

続くステップＳ４３では、導出部１４ｃは、時間軸に沿って後向きにフィルタリングを行うことで、未来の加速度データ及び画像データを用いて、グリップ５１の位置ｐ及び速度ｐ'をさらに更新する。

具体的には、ｋ＝Ｎ−１，・・・，１，０に対して、以下の式に従ってスムージングを行うことにより、ｘ_kを導出する。なお、初期値となるｘ_N＝ｘ⁺ _fNである。

以上により、過去及び現在の計測結果のみならず、未来の計測結果を用いて、高精度にグリップ５１の位置ｐ及び速度ｐ'を導出することができる。

＜１−３−３−５．グリップの角速度及び角加速度の導出（Ｓ７）＞
以下、図１２を参照しつつ、アドレスからインパクト付近までのグリップ５１の角速度及び角加速度の導出処理の流れを説明する。

まず、ステップＳ５１において、導出部１４ｃは、ステップＳ５で導出された姿勢ｑ_iに疑似微分フィルタを掛けることにより、アドレスからインパクト付近までの各時刻ｔ_iの姿勢の微分ｑ_i'を導出する。なお、疑似微分フィルタは、以下の式で表される。

ただし、本実施形態では、数４４の式を双１次変換して離散化した以下の式を用いて、時間微分が実行される。

続くステップＳ５２では、導出部１４ｃは、ステップＳ５で導出された姿勢ｑ_iと、ステップＳ５１で導出された姿勢の微分ｑ_i'とを用いて、アドレスからインパクト付近までの各時刻ｔ_iの角速度ω_iを導出する。角速度ω_iは、下式に従って、導出される。

続くステップＳ５３では、導出部１４ｃは、ステップＳ５２で導出した角速度ω_iに疑似微分フィルタを掛けることにより、アドレスからインパクト付近までの各時刻ｔ_iの角加速度ω_i'を算出する。疑似微分フィルタは、数４５と同じである。

＜１−３−３−６．グリップの加速度の導出（Ｓ８）＞
以下、図１３を参照しつつ、アドレスからインパクト付近までのグリップ５１の加速度の導出処理の流れを説明する。

まず、ステップＳ６１において、導出部１４ｃは、ステップＳ６で導出されたスムージング後のグリップ５１の速度ｐ'に疑似微分フィルタを掛けることにより、アドレスからインパクト付近までの各時刻のグリップ５１の加速度ｐ''を導出する。疑似微分フィルタは、数４５と同じである。

続くステップＳ６２において、導出部１４ｃは、ステップＳ５で導出されたオイラーパラメータｑ_iを用いて、以下の式に従って、アドレスからインパクト付近までの各時刻におけるステップＳ６１の慣性座標系での加速度ｐ''＝ａ^I _i、を、局所座標系の加速度ａ^B _iに変換する。

＜１−４．評価＞
以下、本発明の実施例について説明するが、本発明は、以下の実施例に限定されない。

＜１−４−１＞
実施例１として、ステップＳ４の最適化の効果を確認すべく、上記実施形態に係るステップＳ１〜Ｓ４を実行して得られた最適化された姿勢行列Ｎを用いて、局所座標系のシャフトの向き[０，０，１]^Tを全体座標系でのシャフトの向きに変換した。そして、このシャフトの向きのベクトルと、モーションキャプチャーシステムで計測した全体座標系でのシャフトの向きを表すベクトル（真値）との為す角度を、角度誤差として算出した。この結果を、図１４に実線で示す。なお、ここでいうモーションキャプチャーとは、例えば、特開2011−183090号公報及び特開2011−030761号公報に記載されるような、多数の赤外線カメラを使って高精度で動きを計測するシステムである。また、参考例１として、ステップＳ１〜Ｓ３及びステップＳ２１を実行して姿勢行列Ｎを導出し、この姿勢行列Ｎを用いて、局所座標系のシャフトの向き[０，０，１]^Tを全体座標系でのシャフトの向きに変換した。そして、このシャフトの向きのベクトルと、モーションキャプチャーシステムで計測した全体座標系でのシャフトの向きを表すベクトル（真値）との為す角度を、角度誤差として算出した。この結果を、図１４に点線で示す。同図からは、ステップＳ４の最適化処理を経ることにより、真値との誤差が小さくなっていることが分かる。

＜１−４−２＞
実施例２として、ステップＳ５の最適化の効果を確認すべく、上記実施形態に係るステップＳ１〜Ｓ５を実行して得られた最適化された姿勢行列Ｎを用いて、局所座標系のシャフトの向き[０，０，１]^Tを全体座標系でのシャフトの向きに変換した。そして、このシャフトの向きのベクトルと、モーションキャプチャーシステムで計測した全体座標系でのシャフトの向きを表すベクトル（真値）との為す角度を、角度誤差として算出した。この結果を、図１５に実線で示す。また、参考例２として、ステップＳ１〜Ｓ４及びステップＳ３１を実行して姿勢行列Ｎを導出し、この姿勢行列Ｎを用いて、局所座標系のシャフトの向き[０，０，１]^Tを全体座標系でのシャフトの向きに変換した。そして、このシャフトの向きのベクトルと、モーションキャプチャーシステムで計測した全体座標系でのシャフトの向きを表すベクトル（真値）との為す角度を、角度誤差として算出した。この結果を、図１５に点線で示す。同図からは、ステップＳ５の最適化処理を経ることにより、真値との誤差が小さくなっていることが分かる。

＜１−４−３＞
実施例３として、ステップＳ６のスムージングの効果を確認すべく、上記実施形態に係るステップＳ１〜Ｓ６を実行して、グリップの位置ｐを算出した。そして、この位置ｐと真値の位置ｐとの差（位置誤差）を算出した結果を、図１６Ａ〜図１６Ｃに実線で示す。なお、真値は、モーションキャプチャーで計測した値とした。また、参考例３として、ステップＳ１により得られた同じセンサデータから、ステップＳ１１，Ｓ１２と同様の方法で導出された姿勢行列Ｎを用いて絶対座標系の加速度データを算出し、ここから重力成分を除去した後、これを２回積分することによりグリップの位置ｐを算出した。そして、この位置ｐと真値の位置ｐとの差（位置誤差）を算出した結果を、図１６Ａ〜図１６Ｃに点線で示す。同図からは、ステップＳ６のスムージングを経ることにより、真値との誤差が小さくなっていることが分かる。

＜１−４−４＞
実施例４として、ステップＳ４３のスムージングの効果を確認すべく、上記実施形態に係るステップＳ１〜Ｓ６を実行して、グリップの位置ｐを算出した。そして、この位置ｐと真値の位置ｐとの差（位置誤差）を算出した結果を、図１７Ａ〜図１７Ｃに点線で示す。なお、真値は、モーションキャプチャーで計測した値とした。また、参考例４として、ステップＳ１〜Ｓ５，Ｓ４１，Ｓ４２を実行して、グリップの位置ｐを算出した。そして、この位置ｐと真値の位置ｐとの差（位置誤差）を算出した結果を、図１７Ａ〜図１７Ｃに実線で示す。同図からは、ステップＳ４３のスムージングを経ることにより、真値との誤差が小さくなっていることが分かる。

＜２．第２実施形態＞
図１８及び図１９に、本発明の第２実施形態に係る運動解析装置１０１を含む運動解析システム２００の全体構成図を示す。これらの図を図１及び図２と比較すれば明らかなように、第２実施形態の運動解析システム２００には、第１実施形態の運動解析システム１００に対し、２台目の距離画像センサ６が追加されている。２台目の距離画像センサ６は、図１９に示すとおり、１台目の距離画像センサ２と同様の構成を有している。距離画像センサ６は、距離画像センサ２とは異なる方向から撮影すべく、具体的には、ゴルファー７を右側から撮影すべく、ゴルファー７の右側に設置される。距離画像センサ６も、通信線１７を介して運動解析装置１０１に接続される。なお、運動解析装置１０１は、複数台のコンピュータから構成されていてもよく、例えば、距離画像センサ２，６が異なるコンピュータに接続されていてもよい。

第２実施形態の運動解析装置１０１と、第１実施形態の運動解析装置１とは、ハードウェア構成は同じである。しかしながら、後述するとおり、一部ソフトウェア処理が異なるため、異なる運動解析プログラム１０３がインストールされている。それにより、制御部１４は、第１及び第２取得部１４ａ，１４ｂとして動作する他、第３取得部１４ｅとしても動作し、導出部１４ｃに代えて、導出部１１４ｃとして動作する。以下では、第１及び第２実施形態間で共通の要素には共通の符号を付し、適宜説明を省略しつつ、主として両実施形態の相違点について説明する。

なお、本明細書では様々な記号が登場し、同じ記号が異なる意味で、或いは異なる記号が同じ意味で用いられる場合があるが、最新の定義が優先されるものとする。

＜２−１．運動解析方法＞
図２０を参照しつつ、第２実施形態に係る運動解析方法について説明する。図５及び図２０を比較すれば明らかなとおり、第１及び第２実施形態に係る運動解析方法は類似しているが、ステップＳ２，Ｓ３，Ｓ６が、ステップＳ１０２，Ｓ１０３，Ｓ１０６に改良されている点で異なる。また、第２実施形態では、ステップＳ５の後に、ステップＳ５で導出されたシャフト５２の姿勢を補正するステップＳ１０５が挿入されている。以下、相違点に係るステップＳ１０２，Ｓ１０３，Ｓ１０５，Ｓ１０６について順に説明する。

＜２−１−１．第２データ収集処理（Ｓ１０２）＞
第２実施形態では、第２データ収集処理として、ゴルフクラブ５のスイング動作が２台の距離画像センサ２，６により撮影される。すなわち、距離画像センサ２だけでなく、同時に距離画像センサ６も、ゴルフスイングを捉えた時系列のＩＲ画像及び深度画像を含む画像データを検出する。距離画像センサ６により検出された画像データは、距離画像センサ２により検出された画像データと同様に、距離画像センサ６の通信部２５を介して運動解析装置１０１に送信される。一方、運動解析装置１０１側では、第３取得部１４ｅが通信部１５を介してこれを受信し、記憶部１３内に格納する。本実施形態では、少なくともアドレスの少し前の初期時刻ｔ₁からフィニッシュまでの時系列の画像データが収集される。

＜２−１−２．時刻合わせ（Ｓ１０３）＞
本実施形態では、２台の距離画像センサ２，６にそれぞれ由来する２系列の画像データが取得される。そのため、第１実施形態では、画像データとセンサデータとの時刻合わせのみが行われたが、本実施形態では、ステップＳ１０３として、これら２系列の画像データの時刻合わせも行われる。

具体的には、ステップＳ１０３は、図２１に示すフローチャートに従って進行する。同図に示されるとおり、ステップＳ１０３では、導出部１１４ｃにより、ステップＳ３と同じくステップＳ１１〜Ｓ１４が実行される。これにより、センサデータと、距離画像センサ２により正面から撮影された画像データ（以下、正面画像データという）との同期が取られる。続いて、ステップＳ１１１,Ｓ１１２において、正面画像データと、距離画像センサ６により右側から撮影された画像データ（以下、右側画像データという）との同期が取られる。まず、ステップＳ１１１では、導出部１１４ｃが、正面画像データを画像処理することにより、アドレスからインパクト付近までの各時刻におけるグリップ端５１ａの三次元座標を導出する。同様に、導出部１１４ｃが、右側画像データを画像処理することにより、アドレスからインパクト付近までの各時刻におけるグリップ端５１ａの三次元座標を導出する。

続くステップＳ１１２では、導出部１１４ｃは、正面画像データ由来の時系列のグリップ端５１ａの三次元座標と、右側画像データ由来の時系列のグリップ端５１ａの三次元座標との一致度が最も高くなるように、両時系列データを位置合わせする。つまり、ステップＳ１４と同様に、両時系列データを時間軸に沿って前後に移動させながら、両時系列データの波形が最も一致する点を決定する。そして、そのときのタイミングを基準として、正面画像データの時間軸と、右側画像データの時間軸とを相対的に時刻合わせさせる。

以上により、時刻合わせは終了する。なお、以上の処理により、正面画像データを介して、右側画像データとセンサデータも時刻合わせされる。

＜２−１−３．ダウンスイング期間の姿勢の補正（Ｓ１０５）＞
図１５を参照されたい。同図は、上述したとおり、ステップＳ１〜Ｓ５により導出された姿勢行列Ｎに基づくシャフト５２の向きの角度誤差を表している。同図から分かるように、この誤差は、インパクト（図中、０秒に対応する）の少し前までは効果的に低減されているが、インパクト直前においてはやや大きい。これは、ダウンスイング期間に概ね相当するインパクト直前においてはゴルフクラブ５が高速に運動し、角速度センサ４２（ジャイロセンサ）のバイアス成分が大きくなるためと考えられる。

ステップＳ１０５は、このような誤差が比較的大きくなるインパクト直前の期間、言い換えると、（インパクト−Ｔ₀秒）からインパクトまでの期間（以下、ダウンスイング期間という）におけるシャフト５２の姿勢を補正するステップである。Ｔ₀は、例えば、０．１秒〜０．５秒と設定することができ、０．２秒〜０．３秒がより好ましい。

ステップＳ１０５は、図２２に示すとおり進行する。まず、ステップＳ１２１では、導出部１１４ｃは、画像データに基づいてインパクト時の姿勢ｑ（オイラーパラメータ）を導出する。より具体的には、導出部１１４ｃは、右側画像データに含まれるインパクト時の深度画像を画像処理することにより、シャフト５２の向きを表す三次元ベクトルを導出するとともに、右側画像データに含まれるインパクト時のＩＲ画像を画像処理することにより、シャフト５２の向きを表す二次元ベクトルを導出する。同様に、導出部１１４ｃは、正面画像データに含まれるインパクト時の深度画像を画像処理することにより、シャフト５２の向きを表す三次元ベクトルを導出するとともに、正面画像データに含まれるインパクト時のＩＲ画像を画像処理することにより、シャフト５２の向きを表す二次元ベクトルを導出する。さらに、導出部１１４ｃは、右側画像データ及び正面画像データに含まれるインパクト時の対のＩＲ画像を画像処理することにより、シャフト５２の向きを表す三次元ベクトルを導出する。このとき、ＤＬＴ法等を用いて、２枚のＩＲ画像に由来する２方向からのシャフト５２の像の２次元座標に基づいて、シャフト５２の像の３次元座標が導出される。

導出部１１４ｃは、以上の５つのシャフト５２の向きと、未知の姿勢ｑとの誤差の大きさを表わす目的関数を定義する。そして、当該目的関数を最適化することにより、当該誤差を最小にする姿勢ｑを導出する。なお、本実施形態では、シャフト５２の向きを表す上記５つのベクトルを全て用いて目的関数が定義されるが、例えば、これらのうちの２つ以上のベクトルを任意に組み合わせて、目的関数を定義することもできる。

続くステップＳ１２２では、導出部１１４ｃは、ステップＳ１２１で導出されたインパクト時の姿勢ｑ（以下、ｑ_IBsと表記する）に基づいて、ステップＳ５で導出されたインパクト時の姿勢ｑ（以下、ｑ_IBbと表記する）を補正する。ｑ_IBsは、Ｂ_s系の慣性系（ＸＹＺ慣性座標系に一致する。以下同じ）に対する姿勢を表すオイラーパラメータであり、ｑ_IBbは、Ｂ_b系の慣性系に対する姿勢を表すオイラーパラメータである。Ｂ_b系とは、ステップＳ５で得られる局所座標系であり、Ｂ_s系とは、ステップＳ１２１で得られる局所座標系である。

ここで、Ｂ_s系のＢ_b系に対する姿勢を表すオイラーパラメータをｑ_BbBsとおく。このとき、ｑ_BbBsは、以下のとおり表される。

姿勢誤差は、Ｂ_s系又はＢ_b系におけるシャフト軸と、Ｂ系におけるシャフト軸とをそれぞれ慣性系に変換したときの軸どうしの為す角度で評価される。なお、Ｂ系とは、局所座標系（真値）である。姿勢誤差をシャフト軸の向きの違いで評価するために、ｑ_BbBsを次のようにシャフト軸を一致させる回転(シャフト軸の向きの違い)と、シャフト軸周りの回転とに分解して考える。ｑ_sがシャフト軸を一致させる回転に対応し、ｑψがシャフト軸周りの回転に対応する。

ｓとは、−ｚ軸の向きを表す単位ベクトルであり、ｓ＝（０，０,−１）である。ｑ_sは、ｘｙ平面内の単位ベクトルα＝（α_x，α_y,０）の周りにＢ_b系のｓ軸を角度φだけ回転させる操作を表す。ｑψは、ｓ軸の周りに角度ψだけ回転させる操作を表す。従って、姿勢誤差は、角度φを用いて評価される。また、αとｓは直交するため、α・ｓ＝０となる。数４９の式を展開すると、以下のとおりとなる。

上式の右辺第１項〜第３項は、以下のとおり変形される。
以上より、オイラーパラメータｑのベクトル部をＶ（ｑ）と表し、スカラー部をＳ（ｑ）と表すとき、以下のとおりとなる。

数５２の第１式を第２式で割ることにより、ψを表すことができる。

ステップＳ１２２では、導出部１１４ｃは、数４８の式にステップＳ１２１で導出されたインパクト時の姿勢ｑ_IBs、及びステップＳ５で導出されたインパクト時の姿勢ｑ_IBbを代入することにより、インパクト時のｑ_BbBsを導出する。続いて、導出部１１４ｃは、このｑ_BbBsを数５３の式に代入することにより、インパクト時のψを導出する。さらに、導出部１１４ｃは、数４９の第１式を変形した以下の式に、このｑ_BbBsとψとを代入することにより、インパクト時のｑ_sを導出する。

さらに、導出部１１４ｃは、ステップＳ５で導出されたインパクト時の姿勢ｑ_IBbと、インパクト時のｑ_sとを下式に代入することにより、インパクト時の姿勢ｑ_IBbを補正したｑ_IBb’を導出する。数５５の式による変換により、Ｂ_b系の姿勢誤差とＢ_s系の姿勢誤差とが等しくなる。

続くステップＳ１２３，Ｓ１２４では、導出部１１４ｃは、ダウンスイング期間の始端の時刻（すなわち、インパクトのＴ₀前）と終端の時刻（すなわち、インパクトの時刻）との間の姿勢ｑ_IBbを補間する。このとき、インパクトの時刻における姿勢ｑ_IBbが、ステップＳ１２２で導出された補正後の姿勢ｑ_IBb’に一致するようにする。具体的には、まず、ｑ_IBbの微分を考える。
ここで、ω_Bmは、角速度センサ４２の検出値である。ω_biasは、ω_Bmのバイアス成分である。ステップＳ１２３では、このバイアス成分ω_biasが導出される。なお、数５６では、バイアス成分が考慮されているという意味で、ｑ_IBbにハットの記号が付されている。

具体的には、導出部１１４ｃは、ダウンスイング期間に含まれる各時刻において、ｑ_IBb及びω_Bmを数５６の右辺に代入する。このとき代入されるｑ_IBbは、ダウンスイング期間の始端の時刻においては、ステップＳ５で算出された値である。それ以降の時刻においては、直前の時刻において数５６の式から算出されたｑ_IBbの微分と、数６の式とに基づいて更新されるクォータニオンが代入される。そして、これらの全ての時刻の代入値を加算する。言い換えると、ダウンスイング期間を積分期間として、数５６の式が積分される。この加算値すなわち積分値は、ω_biasを未知数として含む式であり、インパクト時の姿勢ｑ_IBbを表している。従って、導出部１１４ｃは、この積分値（式）を、インパクト時の姿勢ｑ_IBbとして数４８の式に代入して、ｑ_BbBsの値（式）を導出する。さらに、このｑ_BbBsの値（式）を用いて、数５３,５４の式からｑ_sの値（式）を導出する。そして、このｑ_sの値（式）が、ステップＳ１２２で先に導出したｑ_sの値と等しくなるように、ω_biasを決定する。ここで、ｑ_sのｚ成分は０となるため、満たすべき終端条件は２成分である。ω_biasは３つの成分を有するので、３つの成分を２つの終端条件を満たすように計算する。このとき、ニュートン法等を用いて効率的に、ω_biasの３つの成分を探索することが好ましい。

次に、Ｓ１２４では、ステップＳ１２３で導出されたバイアス成分ω_biasに基づいて、ダウンスイング期間における姿勢ｑ_IBbを導出し直す。具体的には、ステップＳ５で導出されたダウンスイング期間の始端の時刻における姿勢ｑ_IBbを、数６，７の式を用いて時々刻々更新してゆく。このとき、更新に用いられる角速度は、バイアス成分を加味した角速度（ω_Bm＋ω_bias）とされる。

なお、本実施形態では、ステップＳ１２４において、バイアス成分を加味した角速度（ω_Bm＋ω_bias）を算出するに当たり、各時刻におけるバイアス成分ω_biasが以下の式で補正される。Ｔは、ダウンスイング期間の長さであり、ｔは、ダウンスイング期間の始端からの経過時間である。ω_biasfは、インパクト時のバイアス成分である。
これにより、３段落前の説明で導出されたバイアス成分ω_biasは、インパクトの時刻のバイアス成分ω_biasfとされ、ダウンスイング期間の始端の時刻のバイアス成分は、０とされる。また、ダウンスイング期間中にバイアス成分が徐々に大きくなるように設定される。これにより、ダウンスイング期間とそれ以外の期間との境界（始期の時刻とその直前の時刻との境界）で角速度が急激に変化することを避け、角速度を連続的に変化させることができる。

また、バイアス成分の補正方法としては、以下の方法も考えられ、以下の補正方法は、以上の補正方法と組み合わせて使用することもできるし、単独で使用することもできる。すなわち、バイアス成分は、高速域では大きくなり、低速域では小さくなる。従って、ω_Bmは下式のとおり補正することができる。
ω_Bm’＝ＴＨ＋（ω_Bm−ＴＨ）×ｋ （ω_Bm＞ＴＨの場合）
ω_Bm’＝−ＴＨ＋（ω_Bm＋ＴＨ）×ｋ （ω_Bm＜ＴＨの場合）

ここで、ω_Bm’は、補正後の角速度（バイアス成分を加味した角速度）であり、ＴＨは閾値であり、ｋは、補正係数である。以上により、ステップＳ１０５の補正処理が終了する。

＜２−１−４．グリップの位置及び速度の導出（Ｓ１０６）＞
グリップの位置及び速度の導出処理（ステップＳ１０６）は、ステップＳ６の処理と以下の点を除き同様である。すなわち、ステップＳ１０６では、正面画像データのみならず、側面画像データに基づいて、グリップ５１の位置及び速度が導出される。具体的には、数３９の式が以下のとおり変更される。ｂは、加速度センサのバイアス成分を意味している。

＜２−２．評価＞
以下、本発明の実施例について説明するが、本発明は、以下の実施例に限定されない。

実施例５として、ステップＳ１０５の最適化の効果を確認すべく、第２実施形態に係るステップＳ１〜Ｓ１０５を実行して得られた最適化された姿勢ｑを用いて、局所座標系のシャフトの向き[０，０，１]^Tを全体座標系でのシャフトの向きに変換した。そして、このシャフトの向きのベクトルと、モーションキャプチャーシステムで計測した全体座標系でのシャフトの向きを表すベクトル（真値）との為す角度を、角度誤差として算出した。この結果を、図２３に一点鎖線で示す。なお、図２３には、図１５の実施例２及び参考例２も示している。同図からは、ステップＳ１０５の補正処理を経ることにより、インパクト直前の期間においても、真値との誤差が小さくなっていることが分かる。

＜３．変形例＞
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、その趣旨を逸脱しない限りにおいて、種々の変更が可能である。例えば、以下の変更が可能である。

＜３−１＞
上記ステップＳ２６の最適化処理では、地磁気データ由来の関数Ｆ_mと、画像データ由来の関数Ｆ_kを含む目的関数Ｊ₁が最小化された。しかしながら、目的関数Ｊ₁は、関数Ｆ_m,Ｆ_kの一方を含まないように定義することもできる。同様に、上記ステップＳ３７の最適化処理では、Ｆ_n，Ｆ_m，Ｆ_kを含む目的関数Ｊ₂が最小化されたが、目的関数Ｊ₂は、関数Ｆ_n，Ｆ_m，Ｆ_kの少なくとも１つを含まないように定義することもできる。また、地磁気データ由来の関数Ｆ_mを、加速度データ由来の関数として定義することもできる。この場合、例えば、加速度センサ４１により計測されたａ_x，ａ_y，ａ_zから重力成分を抽出する。そして、地磁気ｍ_x，ｍ_y，ｍ_zのデータを、それぞれ抽出された重力の向きのデータに置き換えることにより、関数Ｆ_mを同様に算出することができる。

＜３−２＞
上記実施形態では、距離画像センサ２によりＩＲ画像が撮影されたが、ＩＲ画像に代えてカラー画像を撮影し、これを運動の解析に用いることもできる。この場合、距離画像センサ２には、可視光を受光する可視光受光部（例えば、ＲＧＢカメラ）を搭載すればよい。

＜３−３＞
上記実施形態に係る運動解析装置、方法及びプログラムは、ゴルフスイングの運動を解析するのに適するように構成されていたが、同様のアルゴリズムは、任意の運動を解析するのに用いることができる。

＜３−４＞
上記ステップＳ２１では、初期時刻ｔ₁の姿勢（オイラーパラメータｑ及び姿勢行列Ｎ）が、初期時刻ｔ₁の画像データ、及び、初期時刻ｔ₁の加速度データから特定される重力の向きに基づいて導出された。しかしながら、初期時刻ｔ₁の姿勢は、角速度データから導出することもできる。図２４のフローチャートは、その一例の処理の流れを示している。

まず、ステップＳ７１において、導出部１４ｃは、初期時刻ｔ₁のオイラーパラメータｑを［０，０，０，１］とし、当該オイラーパラメータｑを、初期時刻ｔ₁からインパクト付近までの角速度ω_x，ω_y，ω_zのデータを用いて、数６，７の式に従って時々刻々更新してゆく。その後、数５の式に従って、各時刻の姿勢ｑを、姿勢行列Ｎに換算する。続いて、導出部１４ｃは、ステップＳ７２において、ステップＳ２３と同様に、時刻ｔ_i，ｔ_jにおける姿勢の差ｑ_BjBiを導出する。また、ステップＳ７３において、ステップＳ２４と同様に、行列Ａ_kjを導出する。その後、ステップＳ７４において、ステップＳ２６と同様に、ラグランジュの未定乗数法により、ステップＳ７１で導出された姿勢ｑ_iが最適化される。具体的には、以下の評価関数Ｊ₃を最小化するように最適化が行われる。

この最適化の方法も、ステップＳ２６と同様であり、評価関数Ｊ₃が最小化されるためには、目的関数Ｊ₃の微分に関し、数６０が成立する必要がある。そして、この式は、数６１の行列の固有値がλとなり、ｑ_iが固有ベクトルになることを表しているから、導出部１４ｃは、数６１の行列を計算し、当該行列の最小の固有値に対応する大きさ１の固有ベクトルを導出して、これをｑ_iとする。導出部１４ｃは、このｑ_iを少なくとも初期時刻ｔ₁について導出し、さらに姿勢行列Ｎに変換する。これにより、初期時刻ｔ₁における姿勢が導出される。

＜３−５＞
第１及び第２実施形態におけるアドレスからインパクト付近までの姿勢の導出処理（ステップＳ５）において、ステップＳ３２〜Ｓ３７を省略することもできる。この場合、ステップＳ６以降の処理においては、ステップＳ３７の結果に代えて、ステップＳ３１で導出された仮の姿勢を用いることができる。

１，１０１ 運動解析装置
２ 距離画像センサ
６ 距離画像センサ
３ 運動解析プログラム
４ 慣性センサユニット
４１ 加速度センサ
４２ 角速度センサ
４３ 地磁気センサ
５ ゴルフクラブ
７ ゴルファー
１４ａ 第１取得部
１４ｂ 第２取得部
１４ｅ 第３取得部
１４ｃ，１１４ｃ 導出部
５１ グリップ
５２ シャフト
１００，２００ 運動解析システム

Claims (13)

1. 移動体の運動を解析するための運動解析装置であって、
   前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する第１取得部と、
   第１距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第２取得部と、
   前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する導出部と
   を備え、
   前記導出部は、前記画像データ及び前記センサデータを用いて定義される関数を含む目的関数を最小化又は最大化する最適解として、前記移動体の姿勢を導出する、
   運動解析装置。
2. 前記関数は、前記画像データから導出される前記移動体の向きを用いて定義される、
   請求項１に記載の運動解析装置。
3. 前記関数は、前記地磁気データ及び加速度データの少なくとも一方を用いて定義される、
   請求項１又は２に記載の運動解析装置。
4. 前記導出部は、前記移動体の初期の姿勢及び前記時系列の角速度データを用いて、前記移動体の仮の姿勢を算出し、
   前記関数は、前記仮の姿勢を用いて定義される、
   請求項１から３のいずれかに記載の運動解析装置。
5. 前記導出部は、前記画像データから導出される初期の前記移動体の向き及び初期の前記加速度データ、又は、前記角速度データを用いて、前記移動体の初期の姿勢として、前記移動体の静止時の姿勢を導出する、
   請求項４に記載の運動解析装置。
6. 移動体の運動を解析するための運動解析装置であって、
   前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する第１取得部と、
   第１距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第２取得部と、
   前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する導出部と
   を備え、
   前記導出部は、前記移動体の姿勢及び前記移動体の姿勢を微分フィルタによりフィルタリングした微分データに基づいて、前記移動体の角速度を導出する、
   運動解析装置。
7. 移動体の運動を解析するための運動解析装置であって、
   前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する第１取得部と、
   第１距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第２取得部と、
   前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する導出部と
   を備え、
   前記導出部は、前記加速度データを用いて、前記画像データから導出される前記移動体の仮の位置及び当該仮の位置を微分した仮の速度の少なくとも一方をスムージングすることにより、前記移動体の位置及び速度の少なくとも一方を導出する、
   運動解析装置。
8. 前記導出部は、前記移動体の速度を微分フィルタによりフィルタリングすることにより、前記移動体の加速度を導出する、
   請求項７に記載の運動解析装置。
9. 移動体の運動を解析するための運動解析装置であって、
   前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する第１取得部と、
   第１距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第２取得部と、
   前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する導出部と
   を備え、
   前記導出部は、前記画像データから導出される前記移動体の向きの時系列データと、前記センサデータから導出される前記移動体の向きの時系列データとの一致度が最も高くなるように、前記時系列の画像データと前記時系列のセンサデータとの時刻合わせを行う、
   運動解析装置。
10. 移動体の運動を解析するための運動解析装置であって、
    前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得する第１取得部と、
    第１距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第２取得部と、
    前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出する導出部と
    を備え、
    前記導出部は、前記画像データに基づいて第１時刻における前記移動体の姿勢である瞬時姿勢を導出し、前記移動体の姿勢が前記第１時刻において前記瞬時姿勢に一致するように、前記第１時刻と異なる第２時刻と前記第１時刻との間の前記移動体の姿勢を補間する、
    運動解析装置。
11. 第２距離画像センサにより前記第１距離画像センサとは異なる方向から前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得する第３取得部
    をさらに備え、
    前記導出部は、前記センサデータと、前記第２取得部及び前記第３取得部により取得された前記画像データとに基づいて、前記移動体の三次元挙動を導出する、
    請求項１から１０のいずれかに記載の運動解析装置。
12. 移動体の運動を解析するための運動解析方法であって、
    前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットを用いて、時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得するステップと、
    距離画像センサを用いて、前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得するステップと、
    前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出するステップと、
    を備え、
    前記導出するステップでは、前記画像データから導出される前記移動体の向きの時系列データと、前記センサデータから導出される前記移動体の向きの時系列データとの一致度が最も高くなるように、前記時系列の画像データと前記時系列のセンサデータとの時刻合わせを行う、
    運動解析方法。
13. 移動体の運動を解析するための運動解析プログラムであって、
    前記移動体に取り付けられ、角速度センサ、加速度センサ及び地磁気センサの少なくとも１つを含むセンサユニットから出力される時系列の角速度データ、加速度データ及び地磁気データの少なくとも１つを含む時系列のセンサデータを取得するステップと、
    距離画像センサにより前記移動体の運動を撮影した時系列の深度画像及び二次元画像を含む時系列の画像データを取得するステップと、
    前記センサデータ及び前記画像データに基づいて、前記移動体の三次元挙動を導出するステップと、
    をコンピュータに実行させ、
    前記導出するステップでは、前記画像データから導出される前記移動体の向きの時系列データと、前記センサデータから導出される前記移動体の向きの時系列データとの一致度が最も高くなるように、前記時系列の画像データと前記時系列のセンサデータとの時刻合わせを行う、
    運動解析プログラム。