Position estimation by using acceleration sensor

加速度センサーを元に端末の位置を推定する

※もともと大学1年次の自由課題のレポートとして書いたものを公開用に書き直しました。誤りを見つけたら指摘して下さると嬉しいです。

加速度を2回積分すれば座標が求まります。Android では加速度センサーから簡単に端末の加速度が調べられるので、それを元に端末の座標を求めようと挑戦してみました。これはその技術資料です。取得できる加速度はかなり小刻みに振動している上に重力加速度も含まれているので、まずローパスフィルタとハイパスフィルタに通します。また、加速度は端末座標系で返ってくるので世界座標系に変換する必要があります。最終的には直線のみで表現した軌跡までなら求められるようになりました。

フィルター

端末から取得できる加速度は細かく振動しています。 端末を静止させて最初に取得した加速度を初期加速度として保持しておき、そのまま静止させて取得できる加速度の初期化速度との差分を調べると次の図のようになります。（Xperia）

ほとんど0～0.08程度の値ですが、たまに4.01, 8.01といった異常に大きい値が返ってきています。このような異常な値を削り、細かく振動する加速度を平滑化するためにはローパスフィルタを、そこから更に瞬間的な加速度を取得するためにはハイパスフィルタをかますべきです。

ローパスフィルタとハイパスフィルタ

SensorEvent event から加速度を取得する時に次のローパスフィルタを挟めば平滑化できます。k は<tex>0 \leq k \leq 1</tex>で大きいほど変化に敏感になります。（必要な変数は予めフィールドに持たせてあるものとします）

// Low Pass Filter
// 「信号 平滑化」でGoogle検索すると詳しい情報がたくさん見つかります。
lowPassX += (event.values[0] - lowPassX) * k; // event.values[0から2] はそれぞれ端末座標系での生の加速度です
lowPassY += (event.values[1] - lowPassY) * k;
lowPassZ += (event.values[2] - lowPassZ) * k;

何も手を加えていないevent.values[2] と k = 0.1 のローパスフィルタをかました場合のそれを比較すると次の図のようになります。

この違いは圧倒的ですね！ そして、次のハイパスフィルタを挟めば瞬間的な加速度の増分が求まります。

// High Pass Filter
rawAx = event.values[0] - lowPassX;
rawAy = event.values[1] - lowPassY;
rawAz = event.values[2] - lowPassZ;

座標系について

Android の加速度センサーの軸はポートレートのとき次の図の左図のように設定されています。これを端末座標系とします。また、自分の目の前に端末のディスプレイが上になるように置いた時の端末座標系の軸の向きを世界座標系とします。

これはSensorListener | Android Developersの"Definition of the coordinate system"をちょうど反転させたような座標系で、公式ドキュメントと食い違っています。

IMPORTANT NOTE: The axis are swapped when the device's screen orientation changes. To access the unswapped values, use indices 3, 4 and 5 in values[].

（筆者訳）要注意事項：端末のオリエンテーション（向き）が変化した時に軸は反転します。反転していない値を取得するためには values[3, 4, 5] を参照して下さい。

と書いてありますが、Xperiaではvalues[3～5]を参照しようとすると ArrayIndexOutOfBoundsException です・・・どうしようか迷いましたが、確かに取得できている値の軸は上の図の通りなので、公式ドキュメントをあまり気にしないことにしました。また、他のNexus One、Galaxy S でも Xperia と同様の現象を確認しました。

端末座標系を世界座標系に変換する

加速度は端末座標系で返ってきます。端末座標系と世界座標系が一致している状態で端末を上下に振ればz軸方向に加速度が変化しますが、一致していなければ（世界座標系から見たら）めちゃくちゃな方向に加速度が変化しているように見えてしまうので、加速度ベクトルを世界座標系に変換する必要があります。

Androidの方位・ピッチ・ロールの挙動

世界座標系でドロイド君が向いている方向を絶対的な「北」とし、このとき方位が0度になるように調整します。これで端末を右に90度傾ければ方位+=90、左に90度傾ければ方位-=90です。

Androidのピッチとロールは次の図のように取得できます。ドロイド君の向きは先程の座標系の定義と合わせてあります。

ここでついうっかり、roll の返す値がおかしいと悪態を突きながら次のようなコードを書きがちです。

// 端末座標系でのz軸方向での加速度が負なら (= ディスプレイが下を向いているなら)
if (rawAz < 0) {
	if (rawRoll > 0) {
		rawRoll = 180 - rawRoll;
	} else {
		rawRoll = -180 - rawRoll;
	}
}

確かに、正確なロールの角度が欲しいならこのコードを書くべきです。 こうすると上の図の roll(improved) のような値を取得できます。しかし、これで次に述べる座標変換を行うとz軸方向に2重にカウントされたかのような値が出てきます（原因を特定出来ず・・・）。デフォルトのロールの奇妙な挙動は、座標変換を簡単にするための工夫のようです。

方位・ピッチ・ロールを用いた座標変換

x軸周りの回転行列は、y軸をz軸の方に傾けるような方向を正として

y軸回りの回転行列は、z軸をx軸の方に傾けるような方向を正として

z軸回りの回転行列は、x軸をy軸の方に傾けるような方向を正として

z軸周りの回転は方位、x軸周りの回転はピッチ、y軸周りの回転はロールに対応しているので、端末座標系から世界座標系への変換行列は

となります。

先ほど求めた rawAx, rawAy, rawAz をこの変換行列に次のように掛けあわせれば完成です。

端末を色々な向きに傾けながら、世界座標系でx軸（青色）、y軸（緑色）、z軸（赤色）の方向にそれぞれ振ってもきちんと世界座標系での加速度が求められている様子

具体的なコードは次のとおりです。

// ピッチ・ロール
double nPitchRad = Math.toRadians(-pitch); // n means negative
double sinNPitch = Math.sin(nPitchRad);
double cosNPitch = Math.cos(nPitchRad);
 
double nRollRad = Math.toRadians(-roll);
double sinNRoll = Math.sin(nRollRad);
double cosNRoll = Math.cos(nRollRad);
 
double bx, by; // 一時退避
 
bx = rawAx * cosNRoll + rawAz * sinNRoll;
by = rawAx * sinNPitch * sinNRoll + rawAy * cosNPitch - rawAz * sinNPitch * cosNRoll;
az = -rawAx * cosNPitch * sinNRoll + rawAy * sinNPitch * cosNRoll + rawAz * cosNPitch * cosNRoll;
 
// 方位
double nAzimuthRad = Math.toRadians(-azimuth);
double sinNAzimuth = Math.sin(nAzimuthRad);
double cosNAzimuth = Math.cos(nAzimuthRad);
 
ax = bx * cosNAzimuth - by * sinNAzimuth;
ay = bx * sinNAzimuth + by * cosNAzimuth;

ついに「端末の向きに関係ない世界座標系での瞬間的な加速度」を求められるようになりました。これは色々な所に流用できます。

加速度を積分する

SensorManager の registerListener の第二引数に SensorManager.SENSOR_DELAY_FASTEST を指定すると、（Xperiaの場合）onSensorChanged は1秒間に100回ほど(10[ms]に1回)のペースで呼ばれます。 呼び出されるごとに nowTime(現在の時刻), oldTime(1回前に呼び出された時の時刻) などを適切に更新して、interval = nowTime - oldTime を用意します。 そして、この interval を利用して加速度のリーマン和を求めれば速度が求まって、さらにこの速度のリーマン和を求めれば座標が求まります。具体的には次のように該当する変数に足していくだけです。（単位がメートルだと分かりにくいのでセンチメートルにしています）

// ax, ay, az が求まった後で
long nowTime = System.currentTimeMillis();
long interval = nowTime - oldTime;
oldTime = nowTime;
 
vx += ax * interval / 10; // [cm/s] にする
vy += ay * interval / 10;
vz += az * interval / 10;
x += vx * interval / 1000; // [cm] にする
y += vy * interval / 1000;
z += vz * interval / 1000;

移動した方向と、他の移動との距離の比のみが推定できている様子

実際に動かしてみれば分かりますが、座標を少しでも正確に求めるためには次のような工夫が必要です。

1. 動かす前に端末を静止させて速度や座標を全てゼロクリアする
2. 素早く・大きく動かして目的の場所まで動かし終わった瞬間に処理・描画を一時停止する（一時停止しないと座標が変な方向にずれ続ける）

こうする事でやっと端末の移動した方向と距離の比が求まります。座標を正確に求めることはできません。 しかし、「端末の移動した方向と距離の比」さえ分かれば端末の軌跡を推定できます。

このような方法を実際に使っているのは独metaio社の Red Bull Augmented Racing Gameでしょう。次のページの動画を見てください。

• Red Bull Augmented Racing Game

「こんな面倒な方法でコースを自作するなら指で実際に描けばいいじゃないか」と思うかもしれませんが技術的には面白いです。 （ソースコードなどを一切見ていないので完全な推測ですが）端末の軌跡を推定するのに加速度を使ってるのは確実でしょう。 そして、恐らくこのアプリでは缶をスキャンするごとに先ほどの工夫の1を繰り返すことによって、累積誤差を除いています（実際はもっといい方法でやってるかもしれません）。

端末が移動した軌跡を推定できている様子。大成功です！

そこで、そのような方法で本当に軌跡を推定できるのか、次の方法で実際に試してみた所、きれいな軌跡を求められるようになりました。

1. 端末を静止させて速度や座標を全てゼロクリアする
2. 直線で、端末を弾くように少し動かす

この1と2を繰り返すと上手くいきます。

問題点

右の図を見れば分かる通り、動かし方に非現実的な制限があるので、直線の軌跡しか取得できません。

具体的な実装方法

次のクラス図のように実装すると分かりやすくなっていいと思います。状態の描画に View を使ったものと SurfaceView を使ったものの2種類がありますが、これは試行錯誤の結果で、View でもテキスト・図形程度なら十分高速に描画できる事が分かりました。詳しくはクラス図内のメモを見てください。

ポイント

1. MainActivity に全部書くのではなくセンサー系の処理を GetSensorActivity に分離する
2. けど MainActivity の onSensorChanged には中心となる処理（加速度取得、フィルター、座標計算）を書く
3. そして、その結果を「状態を表示するためのビュー」が持つキューにプッシュする
4. ビューではキューから結果をポップして描画する

Thanks

• Developer Collaboration ProjectからAndroid端末をお借りました