javaにおける角速度の積分について

Question

Androidに搭載されているジャイロセンサと加速度センサを使って角度を得るプログラムを作成中なのですが、ジャイロセンサから得た角速度を積分して角度を求めることがうまくできません。角速度を配列にして保持することで1つ前の角速度も参照できるようにし、それを用いて台形法を使い積分しようとしています。しかし、全然まともな角度が出力されません…。配列を制御しているiを共用しているせいだとは思うのですが、共用しないとどんどん角速度の更新に比べて角度の更新が遅延してしまうような気がします…。ちなみに、class Transmission extends Threadは、ボタンが押された時に呼び出されるようにしており、ボタンが押されると一定間隔で積分を始めるようにしています。このようにすればいいよ。や、そもそもこのようなやり方では無理だ。などのアドバイスを貰えると嬉しいです。

以下にソースの一部を添付しておきます。

---

int deltat = 500; //500ミリ秒間隔で積分

public void onSensorChanged(SensorEvent event) {
  switch(event.sensor.getType()){
    //角速度に変化があったら
    case Sensor.TYPE_GYROSCOPE :
      //オフセット
      if(offset == 0){
        gyr_x_off = event.values[0];
        gyr_y_off = event.values[1];
        gyr_z_off = event.values[2];

        offset = 1;
      }

      gyr_x[i] = event.values[0]-gyr_x_off;//オフセットを引いたx軸の角速度
      gyr_y[i] = event.values[1]-gyr_y_off;//オフセットを引いたy軸の角速度
      gyr_z = event.values[2]-gyr_z_off;//オフセットを引いたz軸の角速度

      if((0 <= i) && (i < 9)){
        i = i+1;
      }else{
        i = 1;
      }
      break;
  }
}

class Transmission extends Thread {
  public void run() {
    try{
      while(running){
        //角速度を積分で角度に変換
        if((2 <= i) && (i <= 9)){
          ang_gyr_x = ang_gyr_x+((gyr_x[i-1]+gyr_x[i])/2)*(deltat/1000.0);
          ang_gyr_y = ang_gyr_y+((gyr_y[i-1]+gyr_y[i])/2)*(deltat/1000.0);
        }else if(i == 1){
          ang_gyr_x = ang_gyr_x+((gyr_x[9]+gyr_x[i])/2)*(deltat/1000.0);
          ang_gyr_y = ang_gyr_y+((gyr_y[9]+gyr_y[i])/2)*(deltat/1000.0);
        }else if(i == 0){
          //初回は静止していると仮定して加速度からの角度を入れる。
          ang_gyr_x = ang_acc_x;
          ang_gyr_y = ang_acc_y;
        }

        Thread.sleep(deltat); //deltatミリ秒遅延する
      }
    }catch(InterruptedException e){}
  }
}

Answer 1

質問と関係ありませんが、気になったことを。

コードを見ると各軸ごとに角速度を積分しています(そしてZ軸を無視しています)が、
実はこれ、意味のある値は得られません。

センサーは端末に固定されています。
従ってセンサーの値を積分するということは、端末の軸を基準にして回した値を求めることになります。
一方、目的が、「水平を保つ」なので、欲しい値は研究室系に対して、どれだけ回転しているかです。
(もっと言えば、端末のZ軸が、研究室系のZ軸に対して何度(スカラー)傾いているかが欲しい)

さて、各軸を独立して積分した結果からは、姿勢が一意に決まりません。
積分結果が非0でも水平という状態が普通にありえます。

例えば、
X+90 -> Y+90 -> X-90 -> Z-90
(X+90はセンサーX軸回りで90度回転の意)
という回転操作は、各軸の合計が(0,90,-90)ですが、
操作後の端末は操作前の状態に戻ります。
端末を実際に持って回して確かめてください。

したがって、各軸ごとに独立して積分しても、水平を保つのに役に立ちません。

じゃあどうするのかといえば回転行列やクォータニオンを使います。
オススメはクォータニオンです。

Answer 2

比較する配列の前後（例えばgyr_x[1]とgyr_x[2]）の間で実際の計測時間の差と計算時のdeltatの値がずれているために結果がおかしくなっているのではないでしょうか。
gyr_x[1]とgyr_x[2]の間での実際の計測時間は角速度に変化が発生した後、次に角速度に変化が発生するまでの時間になると思います。

角速度の測定をdeltatの間隔に合わせるか、実際の測定間隔を保持し、それを元に計算すれば良いのではないでしょうか。

Comments

[John_syado]

やっぱりそうなんですね…。回答ありがとうございます。