計測自動制御学会東北支部 第 243 回研究集会 (2008.6.20)
資料番号 243-4

装着型センサによる２リンクアームの３次元動作推定
3D trajectory estimation of 2 link arm using body mountedsensors○小田英晴 ∗ ，佐川貢一 ∗○ Hideharu Koda∗ , Koichi Sagawa∗*弘前大学*Hirosaki University
キーワード : 装着型センサ (Body-mounted-sensor), 3 次元 (Three-dimention)，推定 (Estimation)，
加速度 (Acceleration)，角速度 (Angular velocity)，積分 (Integration)

連絡先 : 〒 036-8561

青森県弘前市文京町 3 番地弘前大学大学院 理工学研究科 知能機械システム工学専攻

小田 英晴，Tel.: (0172)39-3691， Fax.: (0172)39-3691， E-mail: h08gs512@stu.hirosaki-u.ac.jp

1.

緒言

イロで構成される装着型センサを，任意の方向で取り付けても，腕の動作推定が可能な3次元位置計

現在，スポーツ時の身体動作を計測する場合には，市販のビデオカメラでは計測が困難な高速動作が対象となるために，ハイスピードカメラやモーションキャプチャシステムを使う方法などが一般的である．これらの装置は，身体の高速動作を高精度で計測することが可能であるが，高額である
こと，計測範囲が限定されることなどの問題があ

測方法を提案する．特にここでは，腕を模擬した2 リンクアームの前腕と上腕それぞれにセンサを装着し，投球を模擬した動作を行ったときの，腕の 3 次元位置と肘の角度を計測する．推定した腕の姿勢は，光学式モーションキャプチャシステムによって得られた姿勢と比較し，提案した方法の有効性を確認する．

る．また，カメラによる画像解析法の他にも，ジャイロを使用して身体運動を計測する方法が研究されている．この方法では，腕や足の角度変化を計2.

原理測することが可能であるが 1)，並進運動の計測は

2.1

腕の 3 次元位置と方向の推定困難である．一方，加速度センサを併用した装着型センサを用いることで，身体の回転，並進運動を含んだ高速動作を計測する方法が提案されている 2) ．しかし，センサの取り付け方向を指定するなどの実験条件があり，簡便な計測の妨げとなっている．そこで本報告では，加速度センサとジャ本報告では，上腕と前腕に装着型センサ(以下3Dセンサ，Fig.1) を取り付け，腕動作時の加速度および角速度を計測し，加速度の 2 階積分により並進運動を，角速度の積分により回転運動を推定する．
このとき，センサ座標系 Σ0 上で計測された加速度

a0 および角速度 ω 0 は，以下の式で固定座標系 Σ 上–1–

ω02
E2

ω01

Wrist

E1
Elbow
ShoulderΣ0Fig. 1

Fig. 2

Body mounted sensor

2.2の加速度 a および角速度 ω へと変換される．

a = Ea0(1)= Eω 0h= i j(2)ωEki

Derivation of sensors relation

上腕側 3D センサと前腕側 3D センサの関係

ここでは，Fig.2 のように座標系を定義する．Σ0は固定座標系，E1 ，E2 はそれぞれ上腕側 3D セン(3)サ，前腕側 3D センサの FM である．Σ0 上で計測さここで E はセンサの姿勢を表すフレームマトリクれた角速度を，ω01 ，ω02 ，センサ座標系で計測さス(以下FM)であり，i，j ，k はΣ0 座標系の基準単位れた角速度を ω1 ，ω2 とすると，ベクトルである．E は ω を 1 サンプルごとに積分することによって更新する．次に腕の初期姿勢の推定方法について述べる．3D センサの初期姿勢 E は動作前に数秒間静止し，計測された重力加速度を3)利用して推定する ．しかし，重力加速度のみでは

ω01= E 1 ω1(4)ω02= E 2 ω2(5)と表される．いま，肘関節を固定し，肩関節中心の回転運動を行うと，E を決定することはできない．そこで E の i 軸が固= ω02(6)決定する．また，上腕に取り付けた 3D センサの姿= E 1 ω1(7)勢 E1 と，前腕に取り付けた 3D センサの姿勢 E2 と= E 2 ω2(8)ω01

定座標系のx軸方向を向いていると仮定して，E をの位置関係は未知である．腕全体の正しい初期姿
勢を求めるためには，E1 と E2 の関係，すなわち，となる．ここで E1 からみた E2 の姿勢を E12 とすると，E1 ，E2 それぞれからみた肘関節伸展軸の方向，さらに E1 ，E2 と腕の方向の関係を知る必要がある．
そこで次の 3 つの動きを計測することでこれらの関係を推定する．

ω1= E12 ω2(9)となる．以上より E12 は次式のように推定される．

E12–2–= ω1 ω2T (ω2 ω2T )−1(10)

zz

Wrist
E2θE1E1Shoulderjω2Elbow

Σ0e0 =E1e1 =E2 e2Derivation of rotate axis of elbow

kyyi

e1x

Fig. 3

Etmp2ke2ijx

Fig. 4

Derivation of initial position

y2.3

肘関節伸展軸の推定

前腕のみ伸展，屈曲させた時の座標系を Fig.3 のe1e1yように定義する．e0 は固定座標系の肘関節伸展軸θe2yであり，e1 ，e2 はそれぞれ E1，E2 からみた肘関節

e2伸展軸である．いま，肩と肘の位置を固定し，肘の伸展，屈曲運動をしたとき，e2 は，

e2=

ω2||ω2 ||e1xze2xx(11)Fig. 5

Relation of rotate axis when view each FMと表され，(10) 式を利用すると e1 は，

e1= E12 e2(12)回転する．そのときの回転角 θ は，e1と導出される．腕全体の姿勢を決定するためには，

e2e1 と e2 が重なるようにすればよい．

==





e1x
e2x

e1y
e2y

e1z
e2z



(13)

(14)

より，

2.4

腕の初期姿勢導出θ

−1= tan

動作開始前の腕の初期姿勢を決定するため，動作前の重力加速度を利用して，上腕に取り付けた

3D センサの姿勢 E1 と，前腕に取り付けた 3D センe1x · e2y − e1y · e2xe1x · e2x + e1y · e2y(15)となる．これにより正しい前腕側センサの姿勢 E2は，サの姿勢 Etmp2 を求める．このとき，どちらの FME2= E zθ Etmp2(16)の i 軸も固定座標系の x 軸上にあるとして算出する(Fig.4)．次に E1 からみた肘関節伸展軸 e1 と，E2 からみた肘関節伸展軸 e2 の方向が一致するように垂と求めることができる (Fig.4 右)．E zθ は，z 軸周りに θ 回転させる回転マトリクスである．

直軸まわりにEtmp2 回転させる．具体的には，Fig.5に示すように，e1 ，e2 を xy 平面に投影して，ベク2.5

トルの方向が一致するように e2 を z 軸周りに θ だけ

FM と腕の関係

ここでは，それぞれの FM から見て腕がどちらの方向を向いているかを推定する．3D センサを取り付け方向を考慮せずに腕に装着した場合， FM–3–

ShoulderMarkerBody mounted sensorE1
Elbowg

L1

Upper arm
Forearm

E2

Wrist

Σ0Fig. 7L2

Configuration of experimental setup

でき，アルゴリズム検証を容易にできる．3Dセンサ(Fig.1)は加速度センサ(Analog Devices, ADXL320,Fig. 6193)およびジャイロ(Murata, ENC-03M:MicroStone,Derivation of relation of FM and links

MG3-01Ab) で構成されている．ADXL320 は検出と腕の方向の関係が未知である．そこで，Fig.6 の範囲が ±5[G]，ADXL193 は ±120[G]，ENC-03M はように腕を真下に向けて下ろして静止していると±300[deg/s]，MG3-01Ab は ±4000[deg] である．検きの重力加速度から， FM と腕の方向の関係を求出範囲が広いセンサは，動きの小さい区間では出める．それぞれの FM で計測された重力加速度を力電圧に対するノイズ成分の影響が大きくなって

g1 ，g2 とすると，腕の方向を表す単位ベクトル L1，しまうため，2種類のセンサを併用することでこれL2 は，を防いでいる．各センサから出力される電圧デー

g1||g1 ||g2
L2 = −||g2 ||

L1 = −

タは，AD コンバータカード ( CONTEC，ADA16-(17)(18)

さらに，上腕，前腕の長さをそれぞれ l1，l2 とする32/2(CB)F) )を使用して，ノート型PC( Panasonic,
CF-03 Let’s note ) でサンプリング周波数 1000[Hz]でサンプリングした．3DセンサはFig.8に示すように，腕に対して 20[deg] 回転させた姿勢で取り付けと，腕を表すベクトルは，る．同時に，光学式モーションキャプチャシステム

L01 = E1 L1 l1(19)(Vicon460, Vicon Motion Systems) で動作を計測す

L02 = E2 L2 l2(20)るために，前腕および上腕の姿勢，センサの位置を計測するための反射マーカ 6 個を 2 リンクアームと求めることができる．に取り付ける．光学式モーションキャプチャシステ

3.

実験

3.1

実験方法

ムは，物体に取り付けられた反射マーカの位置を複数のカメラでとらえることで，精密な3次元位置情報を得ることができるシステムである．Vicon

今回，動作を計測するために上腕と前腕の動きを再現できる 2 リンクアームを製作した (Fig.7)．2 リ
ンクアームは，600[mm] の木材 2 本を連結させ，肘関節を模している．このアームを使うことによって，のサンプリング周波数は最大 120[Hz] であるので，計測後に 1000[Hz] に再サンプリングして比較する．
実験で行う動作は，今後の応用を考え，野球の投球動作を模擬したものとした．

センサの取り付け位置や角度を明確にすることが–4–

パラメータは，
jji20[deg]kik20[deg]E12e2
L1 = L 2Upper arm

Fig. 8Forearmとなるはずである．これに対して推定結果は，

10.05 0.03
E12 = −0.05
10.01

Angle of sensors mounting

3.2

評価方法

3.2.1

並進運動


1 0 0
= 0 1 0
0 0 1
=0.34 0.94 0=
0.94 −0.34 0


並進運動については上腕側のセンサに取り付けたマーカの3次元座標と，加速度の積分によって得

e2=

L1

=

L2

=0.03 −0.011

0.42 0.90 0.07

0.92 −0.39 0
0.92 −0.39 0.05

た 3D センサの 3 次元座標を比較する．3D センサとであり，推定結果から算出したセンサ取り付け角

マーカの 3 次元軌道の x 座標，y 座標，z 座標それぞ度は，上腕側センサが 23.1[deg]，前腕側センサがれの時間変化について相関係数を算出し，評価す23.0[deg] であった．どちらのセンサも 3[deg] ほどのる．その際，積分誤差を軽減するために，初期位ずれと，十分高い精度での推定ができたといえる．

置と移動後の位置を測定し，センサから計算した

今回は，これらのパラメータの導出に必要な動作

3 次元座標を測定した最終位置に修正する．

について詳細な検証は行っていないが，腕の動かし方，速度，時間などを変化させることで精度が3.2.2

回転運動変わる可能性があるため，精度向上のためにそれ回転運動は，3D センサと Vicon それぞれから上らについても検証したいと考える．

腕，前腕を表すベクトルを算出し，上腕ベクトル同士がなす角，前腕ベクトル同士がなす角を算出

4.2

3 次元動作推定し，評価する．さらにそれらのベクトルから肘関

Fig.9に，3Dセンサによる腕の推定軌道と，Vicon

節角度を算出し，3D センサ，Vicon それぞれの角による実測軌道を示す．初期の姿勢から，−x 方向度を比較する．に腕を引いた後，+x方向へ投球するような動作を推定できていることがわかる．この動作を，並進4.

結果・考察

運動と回転運動に分けて評価する．まず，並進運

4.1

初期姿勢推定用パラメータ動について比較する．Fig.10 は，加速度の 2 階積分による上腕側 3D センサの軌道と，Vicon によってここでは初期 FM 推定に必要な E12 ，e2 ，L1 ，L2の検証を行う．Fig.8 のように取り付けた場合，各計測されたマーカの軌道の一例である．Fig.10 の各座標の軌道について，時間変化を示したものがFig.11 であり，R は相関係数である．各座標の R をみると，すべての軸において 0.98 以上となっており，非常に高い相関があることが示されている．ま–5–

Pitching direction

3Dsensor
ViconElbow

10.2z[m]z[m]0.500.1
End0Start-0.1−0.5End−11.5

3D sensor
Vicon

10.5Start

0-0.2
1−10−0.5−1 1x[m]-0.200.50.2

0y[m]

x[m]

Fig. 9 Trajectry of upper arm and forearm measured 3D sensor and Vicon

0.4
-0.5

y[m]

Fig. 10 Trajectry of upper arm mounted sensor
measured 3D sensor and Vicon
1x R =0.99

3Dsensor
Vicon

0Table 1 Correlation coefficient of coordinate between 3D sensor and Vicon

Coordinate[m]−10.5y R =0.98

0x coordinate

y coordinate

z coordinate

10.99

0.981.00

20.990.96

1.000.5

30.97

0.990.98

0Ave.

0.98

0.97

0.99

−0.5

−0.50z R =1.000.5

11.5
Time[s]22.5

た Table 1 に，同様の実験を 3 回行ったときの相関

Fig. 11係数とその平均値を示す．表からも，軌道が高い

of time variation(R:correlation coefficient)3

Coordinate of upper arm mounted sensor

相関関係にあることが示されており，高精度で推定ができていることがわかる． 次に回転運動についてみると，相関係数が 1.00 と高いことや，初期いて比較する．Fig.12 上は，3D センサ，Vicon それ

角度，最終角度ともに小さい誤差で推定できていぞれから算出した上腕ベクトルがなす角と，前腕ることから，動作中の姿勢更新が高精度でできてベクトルがなす角を示したものであり，差がゼロおり，肘関節の伸展・屈曲動作を推定できているであれば両者は一致していることを表す． Fig.12といえる．また Table 2 に，実験 3 回分の角度の差下は，肘角度の比較である．J は角度の差の 2 乗平の 2 乗平均平方根，相関係数とその平均値を示す．

均平方根，R は相関係数を表している．上腕，前どの試行でもほぼ同様の結果が得られ，肘関節角腕ベクトルがなす角をみると，J が 10[deg] 以下と度の推定精度が高いことが示された．このことかいう高い推定精度である．また，前腕の初期姿勢ら，さらなる精度向上がを目指すためには，初期が6[deg]ほどずれている．このずれは，ほか数回の姿勢に関する検証が必要になると考えられる．

実験の結果にも見られたことから，上腕と前腕の関係や FM と回転軸の関係を導出の際の誤差が影響していると考えられる．また，肘関節角度につ–6–参考文献
Error[deg]1510ForearmJ =6.49

Upper arm

J =8.211) 柊 幸伸，加藤 宗規，佐藤 仁，丸山 仁司: 3 軸
角速度センサを用いた動作分析の試み，理学療法科学，20-2，93-98 (2005)

Upper arm
Forearm52) 塚本 利昭，近藤 和泉，及川 友和，佐川 貢一:
装着型センサによる投球動作分析，青森県スポー
ツ医学研究会誌，14-5，13-16 (2005)

Elbow angle[deg]03) 佐川 貢一，森山 靖子，塚本 利昭，近藤 和泉:
装着型センサによる投球動作計測-画像解析システ
ムとの比較-，IIP 情報・知能・精密機器部門講演会
講演論文集，2005-2504，337-340 (2005)

150
100
3D sensor
Vicon

R =1.00
50
000.5

11.5
Time[s]22.5

3Fig. 12 Coordinate of Upper arm mounted
sensor of time variation(J:root mean square，R:correlation coefficient)

Table 2 Error of arm position between 3D sensor and Vicon, and Correlation coefficient of elbow
angle between 3D sensor and Vicon
Forearm Upper arm Elbow angle
16.49

8.21

1.00

2

6.65

4.8

1.00

3

5.58

7.39

1.00

Ave.

6.24

6.8

1.00

5.

結言

本報告では3Dセンサを任意の姿勢に取り付けた場合の，2 リンクアームの 3 次元動作推定を実現するアルゴリズムの構築を試みた．実験の結果，並進運動，肘関節角度の変化は非常に高い相関が見られ，高精度で推定ができていた．前腕，上腕の姿勢は小さい誤差で推定ができていた．初期姿勢の推定は可能だが，ある程度の誤差がみられた．
これらの結果から，装着型センサによって，腕の動作推定が可能であることが確認された．今後は推定精度を向上させ，実際に投球動作を行ったときの，腕の動作推定を行う予定である．

–7–