 |
| 座標変換 （１．数学的基礎） |
| ２．応用編 |
| フレームマトリックス法 |
| 方位情報の導入 |
| 角度θｅのノイズ除去 |
| 静止場所の特定方法 |
| 積分誤差の除去方法 |
| 静止点での連続性 |
| 戻る |
|
|
２つの座標系，固定（絶対）座標系と移動（計測するセンサにと共に移動）座標系を考える。
これらの重力加速度と，地磁気の方向を計測して，選手の静止を特定する。
単位ベクトル，ｉ，ｊ，ｋと ，ｘｙ平面とのなす角をそれぞれ，θｘ，θｙ，θｚとする。 重力加速度のｇのｉ，ｊ，ｋ方向成分をａｘ，ａｙ，ａｚとすると，θｘ，θｙ，θｚは次式で表すことができる。 θｘ ＝ sin -1 ａｘ／ｇ （３．１） θｙ ＝ sin -1 ａｙ／ｇ （３．２） θｚ ＝ sin -1 ａｚ／ｇ （３．３） ａｘ ＝ ｇ・sin θｘ ａｙ ＝ ｇ・sin θｙ ａｚ ＝ ｇ・sin θｚ ａｘ２＋ａｙ２＋ａｚ２＝ｇ２・（sin２θｘ＋sin２θｙ＋sin２θｚ）＝ｇ２ sin２θｘ＋sin２θｙ＋sin２θｚ＝１ 静止時のＦＭ Ｅ０＝［ｉ，ｊ，ｋ］は次式のようになる。
ａ ＝ Ｅｎ ａ’ （３．５） ω ＝ Ｅｎ ω’ （３．６） Ｅｎ＝［ｉ ｊ ｋ］ （３．７） i移動中のＥｎは，１サンプリング毎に、ラグビーセンサ（センサー座標）で計測した３軸加速度成分からの一定値のベクトル成分（一定回転と見なせる時間間隔と考える）ω’から，３次元回転行列Ｒω’θを求め，次式で更新する。 Ｅｎ＋１＝Ｒω’θＥｎ （３．８） Ｒω’θは，ベクトルω’の周りにθ回転させる行列である。Δｔはサンプリング周期，ラグビーセンサでは，１０ｍｓｅｃである。 θ＝Δｔ｜ω’｜ （３．９） 従って，静止 ｔ＝０でのΣでは，ａ（０） ＝ Ｅ０・ａ’（０） （３．１０） で，（３．４）式により， ａ（０） ＝ ｇ ｛０ ０ １｝Ｔ （３．１１） となる。 角速度は ω（０） ＝ Ｅ０・ω’（０） （３．１２） で変換されるが，ω’（０）＝０（静止状態）ではあるので、ω（０）＝０と見なす。 次に，Δｔ＝１０ｍｓｅｃ後の計測で、計測された角速度（回転）は，センサシステム系の計測された角速度ω’（１）＝｛ω’ｘ ω’ｙ ω’ｚ｝Ｔ （３．１３） が計測される。これから，回転角度θ＝Δｔ｜ω’｜を求める。 ｜ω｜＝｜ω’｜＝√（ω’Ｔω’） 単位ベクトルを ［λ μ ν］Ｔ ＝ ω’／｜ω’｜ （３．１４） とすると，回転行列Ｒωθは，
となり，センサシステム（ラグビーセンサ）の座標で計測された加速度ａ’（１）から，グランド（固定）座標での加速度ａ（１）を次式から求める。 ａ（１）＝Ｅ１・ａ’（１） ＝ ＲωθＥ０・ａ’（１） また，角速度は，ω（１）＝Ｅ１・ω’（１）＝Ｒωθ・ａ’（１） 次のΔｔ＝１０ｍｓｅｃ（２０ｍｓｅｃ経過）後のラグビーセンサでの計測値より，同時にω’（２）から回転角度θを求めてゼロでなければ，（３．１５）式よりＲωθを求め，変換する。ゼロであれば，Ｒωθ＝Ｅ＝１となり，逐次座標変換をして，グランド（固定）座標での加速度，角速度を求める。 ａ（２）＝Ｅ２・ａ’（２） ＝ Ｒωθ Ｅ１・ａ’（２） （３．１６） ω（２）＝Ｅ２・ω’（２）＝ Ｒωθ Ｅ１・ω’（２） （３．１７） ラグビーセンサのセンサー座標系で，角速度ω’（ｋ）がゼロとなったとき，加速度ａ’（ｋ）の大きさが１ｇに等しくなったときに，静止状態と仮定し，姿勢を計測する。 しかしながら，ω’（ｋ）がゼロなる機会は極めて少ない可能性があるので，ある一定期間Δωが小さく，加速度の大きさも１ｇに近いと言う条件が，静止条件を見つける妥協条件であろう。 ６つの自由度のうち，位置の自由度は，加速度のパターン，すなわち，歩行，走行などの周期的な時系列データ以外のときは，位置の自由度については，静止の姿勢計算は計算せずに，この部分では，静止と次の静止間の角速度を時間積分した回転角で座標変換して，その間の加速度，角速度の計算に専念する。 姿勢計算ができる部分の条件としては，角速度がゼロであることが望ましいが，選手に装着している部分では，必ずしも，立脚条件と外部から見なせる場合（目視観測）でも，角速度の大きさがゼロになっていない計測状態が続く可能性がある。 ある一定時間，積分すれば，その時間積分結果は角速度が変化していても角度の変化は少ない場合がある。 これは，静止していると考えて姿勢計算を実施し，その後の歩行、走行の初期姿勢にするほうが実用的なのである。
|
静止時のＦＭのｉを，Σ座標系でｘ軸方向（北）から角度γを向くように定義する。
方位γを導入すると，
静止時のＦＭ Ｅ０＝［ｉ，ｊ，ｋ］は次式のようになる。
（tanθｘ sinθｘ＋cosθｘ ＝ （sin２θｘ＋cos２θｘ）／cosθｘ＝１／cosθｘ）
γ＝０の時，（３．４）式と一致する。
Ｅ０ ＝ ｛ ｉ ｊ ｋ｝ （３．４Ａ）
となる。 方位情報を使って，グランド座標において，加速度・速度をプロットするとき，また、その方向情報しての移動方向を表している。
姿勢の計測は，方位センサの情報を利用して，ｘ軸方向の絶対方位を付け加える。
方位コンパスを使用する際の注意点としては，”Ｎ（北）極の偏西という事情を考慮する。
その土地柄の地磁気狂いという現象があるために，実は磁石が示すＮは真北ではない。 日本においては磁石はわずかに西よりに偏る。 沖縄は４度，北海道は９度，関東・東海・関西は６度である。 本州本土の大部分は大体６度〜７度，偏西している。 東京では，ほぼ６度であるから，北と算出した方向から方位を東に６度分ずらして座標系を記述する。
選手の移動中のＦＭの誤差は，ジャイロセンサによる角速度積分によって引き起こされるが，方位センサが３軸で計測されると，移動中でも，地磁気の方向からの姿勢を計算することで，ジャイロセンサからの信号を補正することができる。 （つまり、走行中でも，姿勢を算出可能である）
しかしながら，ジャイロセンサの信号から角度算出の精度に，多少のゼロ点ドリフトがあっても，移動が終了し，静止状態（立っている状態）が再び，検出できるので，すなわち，重力加速度および，地磁気の方位から，正確なＦＭを推定できる。 そこで，最初の静止状態から移動状態，そして静止状態へのＦＭが連続的につながるようになる。 この間に発生するジャイロセンサの時間積分結果を，静止状態のところで補正することにする。 この走行中の誤差の蓄積は，次に示す消去方法を採用する。 走行中の誤差の蓄積は，時間積分であるから，小さな加速度のＤＣ成分は，時間と共に大きな値になり，積分した値は，リーズナブルな値とならない。 これを避ける方法は，走行中（ある一定以上のピッチ走行が見られる部分）期間で無いところでは，時間積分を実施しない，あるいは，加速度成分をゼロとするなどの，補正を実施する。
下図にしめしたように，最初の静止状態から，n=N番目に再び，静止状態になったとき，それぞれの姿勢角を求めることができる。 ジャイロセンサ信号からの時間積分して計算された角度変化θと最初と最後の静止状態での角度変化分との差が角速度計算からの誤差であるから，この補正方法を次に示す。
ｎ＝Ｎのとき、最初の静止状態からＥ０から角速度ωの積分により得られた変化角度によるＥＮが得れたところから，最後の静止状態から得られたＥ’Ｎとの差が角度θeとして得られる。
静止状態から誤差を含んだENからE'Nの差異をベクトルωｅの周りにθｅ回転させる等価回転マトリックスと考えて、次式で修正する。
Ｅ’ｎ ＝ ＲωｅθｅＥｎ （３．１８）
ここで，Ｒωｅθｅは，ＥｎからＥ’ｎへの等価マトリックスである。ここで，ｎ＝Ｎのとき，誤差を含んで計算してきた経路の結果のＥｎと静止条件の姿勢Ｅ’ｎからＲωｅθｅを求めると，次のように表される。
Ｒωｅθｅ ＝ Ｅ’ｎＥｎＴ
このＲωｅθｅ の各成分から，ωｅ(λｅ，μｅ，νｅ）とθｅは次式から求める。。
ただし，
である。 途中経過（歩行，走行）の補正は，最初の静止状態の時刻からのサンプリング数をｎ，次の静止状態までのサンプリング総数をＮとすると， ｎサンプリング時の修正されたＦＭは次式のように表される。
Ｅ’（ｎ） ＝ Ｒωｅθｎ Ｅ（ｎ） （３．２４）
θｎ ＝ ｎ θｅ／Ｎ
以上の方法により、誤差の蓄積を排除した結果の座標変換によって，サンプリング毎の加速度，角速度を算出する。
この処理により，グランド（固定）座標から見た加速度を導出することができる。 この表現（座標系）では，横軸を時間軸とすると，Ｘ軸では，上，下→，北，南に，Ｙ軸では，上，下→西，東に対応させたことを表示して，グランド座標に変換した，物理量であることを明確にする。
センサー座標で，静止条件の候補となった時間の近傍には，走行から歩行へ移行し，あるいは，姿勢の変化後を含んだ後に，停止条件が検出されるはずである。 停止条件と見なされた時点からは、姿勢の変化後に，歩行そして走行，または走行へ移行している。 歩行，走行のパターンが発生することなく，再び姿勢の変化が続き，静止点と判断できた静止条件が現れる。
ここでの提案は，歩行，走行パターンが見えていない時間帯では，姿勢の変化あるものの，一定の角速度で回転している，時間帯では，姿勢の計算を実施し、その質点の運動は，停止と見なせる，数値の大きさであり，歩行，走行に結びつけることのできる姿勢を求めることが，走行中の姿勢をより精度良く確定できる重要な，作業となるでしょう。
しかしながら，質点の移動が少ない時間帯でも，姿勢の計測の主役は，角速度である。この角速度（すなわち，姿勢の変化）が大きいと，姿勢を求める重力加速度成分に向心力という慣性力が加わり，姿勢誤差が発生する。 角速度は計測できるが，その回転半径は，身体のおへその下に装着しているとの仮定の下でも，推定することは難しい。 従って，この向心力は，無視できる大きさの角速度のときに、静止姿勢として求めるが， 方位センサーから求めれるグランド座標への結びつきのために，重力成分の殆んどが，Z軸にあるときにする。 すなわち，転倒しての静止状態は，速度としてはゼロと断定できるが，向きを決定することができない姿勢であるから，活用できる静止状態に，2種類あることになる。
積分誤差の除去方法
加速度α＝ａ−ｇがゼロ，角速度ωがゼロのときは，ラグビーセンサを装着した選手（剛体）の速度ｖはゼロと仮定して良いだろう。
α ＝ ０ and ω ＝ ０ → ｖ ＝ ０ （３．２５）
時刻 ｔ のとき，加速度センサによって計測された重力加速度ｇを差し引いた加速度αｍ（ｔ）と真の加速度ａｒ（ｔ）との関係が次の式で表されるとする。
αｍ（ｔ） ＝ ａｒ（ｔ） ＋ ａe （３．２６）
ここで，ａeは計測誤差である。
Ｙ０軸（西と東の方向）での移動距離も，同様に求める。
グランドの静止点位置が計測できる手段としては，ＧＰＳの情報を待つ必要があろうが，相対的な移動距離というものは既に述べた手段で求めることができるから，静止位置を適当なグランド座標に配置することで，その後の移動状態が解析され，明確になるだろう。
（３．２８）静止点では，センサー座標とグランド座標との関係は（３．４A）より、求めると、（３．４）と同じように，上式（３．２８）となるが，簡単のため，下図のように，静止１では，センサー座標とグランド座標のX軸は一致して，静止2でも一致しているとする。
静止１： θx ＝ 0° ，θｙ ＝ -30°，θz ＝ 60° センサー座標で計測された，重力加速度を
α1 ＝ ｇ・［ ０， -1/2， √3/2 ］T とすると， グランド座標では，
（３．２９）同様に，静止２： θx ＝ 0° ，θｙ ＝ -60°，θz ＝ 30° センサー座標で計測された，重力加速度を
α2 ＝ ｇ・［ ０， -√3/2，1/2 ］T とすると， グランド座標では，
（３．３０）2つの変換行列E1とE2の間で，一定の角速度で変換が行われたとすると．その変換行列は，
（３．３１）となる。 ここで，ω（λ，μ，ν）とθは，式（３．１９）から式（３．２３）から，
（３．３２）これから，
（３．３３）求めた数値から，回転は，X軸を中心に−30°回転する回転行列であることが分かる。
静止１から，静止２への回転は上記のように1回の回転で表せることが分かる。 しかしながら，実際には，静止１と静止２の座標の回転は，式（３．１５）の回転行列で，計算される行列で，実行する。 但し，このことは，サンプリング毎に演算が繰り返された結果の回転行列は，上記の式（３．３１）に一致するはずである。すなわち，
EN = Rωnθｎ・EN-1・EN-2・・・E1・E0 （３．３４）
＝ ＲωθE0 （３．３５）
のサンプリング毎に角速度の成分から計算した変換行列のω，θと，上記で計算した，静止１と静止２の変換行列のω’，θ’との差が補正すべき，誤差ベクトルωe，θeであるする。，