第 3 章 系統隔離作業支援に用いるトラッキング 技術の設計技術の設計
3.5 ジャイロ・加速度センサを用いたトラッキング
ジャイロ・加速度センサを用いたトラッキングでは、まずジャイロセンサの値から 姿勢を推定する。次に求められた姿勢を元に、加速度センサの値から重量加速度成分 をキャンセルした後、位置の推定を行う。
3.5.1 ジャイロセンサによる姿勢の変化量の算出
ジャイロセンサから得られる値(ωx, ωy, ωz)は、センサ座標系における各座標軸周り の回転を表す角速度の値である。前回センサから値を得てから経過した微少時間をt とすると、微少時間tの間の回転移動成分Rs(4t)は、式3.35で表される。
Rs(4t)= 0 BB
@
1 0 0
0 cosωxt −sinωxt 0 sinωxt cosωxt
1 CC A
0 BB
@
cosωyt 0 sinωyt
0 1 0
−sinωyt 0 cosωyt 1 CC A
0 BB
@
cosωzt −sinωzt 0 sinωzt cosωzt 0
0 0 1
1 CC
A (3.35)
3.5.2 加速度センサによる位置の変化量の算出
加速度センサから得られる値(ax, ay, az)は、センサ座標系において各座標軸方向へ どれだけ位置が変化したかを加速度で表されたものである。この値には、重力加速度 成分が加わっているため、地面に対して垂直下方向の重力加速度を、加速度センサの 各軸成分に分解して除かなければ、加速度センサの位置の変化に起因する加速度だけ を得ることができない。
まず重力加速度成分を取り除く。時刻tにおける姿勢を表す回転行列Rtを、
Rt =
R11 R12 R13 R21 R22 R23 R31 R32 R33
(3.36)
とする。マーカ座標系における重力加速度ベクトルgmを
gm = (0, g,0) (3.37)
であり、センサ座標系における重力加速度成分gs(gx, gy, gz)は、
(gx, gy, gz) = (gR12, gR22, gR32) (3.38)
となる。よって、時刻t+tのセンサ座標系での各軸方向の加速度成分の大きさを、
at+4t = (ax(t+t), ay(t+t), az(t+t)) (3.39) とすると、重力加速度成分がキャンセルされた加速度成分a0(t+4t)は、
a0(t+4t) = (ax(t+t)−gR12, ay(t+t)−gR22, az(t+t)−gR32) (3.40) となる。これで重力加速度の影響をキャンセルできたため、a0tは、時刻tにおける ユーザの移動によるセンサ座標系での軸方向の加速度を表すことになる。時刻tにおけ るセンサ座標系での速度成分をvtとすると、a0tを縦に書いて離散積分の台形公式を用 いると、
vt = 1
2 ×(v(t`4t)+R`s(4t)a0t)× t (3.41) となる。
これが時間tにおける速度なので、時刻t− tから時刻tの間の平行移動成分Ts(4t) は、離散積分の台形公式を用いて、
Ts(4t) = 1
2 ×(v(t`4t)+vt)× t (3.42)
となる。よって3.5.1で求めたRs(4t)と、本節で求めたTs(4t)を用いて、微少時間 tの間の位置・姿勢の変化を表す行列S(t;4t)を得る。
3.5.3 ジャイロセンサ・加速度センサのドリフト成分を除去するアル
ゴリズム
ジャイロセンサや加速度センサに生じるドリフト成分とは、本来の回転・移動の変化 に無関係な、時間や温度などの環境に起因して起こる出力値の変動である[24]。図3.10 は、ジャイロセンサを正の方向へ約90◦回転させた際の、出力である角速度を表す電 圧値(V)と、その値を離散積分した角度を表す電圧値(V)である。
本来、0秒から1.6秒位までと、3.4秒以降は静止しているため出力は0(V)であり、
積分した角度の値も変化しないはずである。しかし、測定結果ではいずれの時間でも 負の電圧が計測されていて、その結果静止しているはずのジャイロセンサが、少しず つ負の方向へ動いているように見える。このような本来計測する動きと無関係な出力 がドリフトである。先に述べたとおり、ドリフトの原因は時間や環境に起因するため、
図 3.10: ドリフト成分の例
完全に除去することは困難である。このドリフト成分を含むセンサの補正には、拡張 カルマンフィルタを用いた予測が広く用いられている[25][26]。これはドリフト成分を補 正するわけでなく、ノイズとして含んだまま運動予測を行うことで、真値を得る手法 である。
本研究では、ドリフト成分のみを除去するために次の手法を開発した。
図3.11に示すように、同一のセンサを1軸に対し2個用い、それらを運動方向に対 して正負が逆になるように設置する。
図 3.11: センサの取り付け方
2つのセンサからの値は、図3.12の減算装置によって差が取られる。
この時センサの出力は、2個のセンサをA, Bとして式3.43のように書ける。
図 3.12: 減算回路
SA(t) =F(ω(t)) +eA(t)
SB(t) =−F(ω(t)) +eB(t) (3.43) ただし、各成分は、
SA(t) :センサAの出力 SB(t) :センサBの出力
ω(t) :時刻tでのセンサの移動成分(角速度または加速度)
F(ω) :移動ωに伴うセンサの出力成分
eA(t) :センサAのドリフト成分 eB(t) :センサBのドリフト成分
ドリフト成分は一般的に同一環境下で試験された場合の結果は、同一であることが 望ましいとされ、製造元によって一定以内に収まるとされている[24]。よって
eA(t)≈eB(t) (3.44)
減算装置によってこれら2つの出力の差をとると、式3.43、式3.44より
SA−SB = 2F(ω(t)) (3.45)
式3.45はドリフト成分がキャンセルされ、移動に伴うセンサの出力だけが2倍で出力 されることを意味する。以上より、移動に伴うセンサの出力のみを得ることができる。
3.5.4 センサ値補正アルゴリズム
センサ値の補正には、様々な手法が考えられている[25][26]。その補正目的は、主にド リフト成分の除去である。本研究ではドリフト成分を3.5.3項で述べた手法で除去して いる。よってここでは、人工マーカを用いたトラッキングか自然特徴点を用いたトラッ キングが行われ、位置・姿勢を表す行列を得た際に、その行列でセンサによって得ら れた位置・姿勢を表す行列を書き換えることとする。