2013/05/04

加速度センサで角度検出してみるの巻

【2014/10/12 記事更新】


前回の記事(http://nekolab.blogspot.jp/2013/05/arduino.html)で、加速度センサ(ADXL345)を動かしてみました。


今回は、センサから得た値の計算をして、センサの傾きを検出してみようと思います。


前回の記事で、加速度センサに傾き検出が~、と書いてましたが、加速度センサは重力加速度も検出するというコメントを頂きました。


と、いうことはですよ、重力がどの向きにかかっているのかを検出することで、センサがどれだけ傾いているかを検出できるということですね。

スマホの画面の向きが変わるやつとかはこれを利用してるのか...知らなかったです。
加速度センサで傾きが分かるとは思わなかったので。



デバイスはArduinoで、前回と大体同じです。


ちょこっとだけプログラムが変わっています。

センサの値を取得するのは前回の記事と変わりません。


このセンサの値ですが、重力加速度がそのまま出力されているわけではありません。

センサに設定してあるスケール(今回は±16g)の範囲で、13bitの分解能(高分解能モードの場合)で出力されます。

出力が1だけ変化したときの重力加速度の変化は、

(16 + 16) / 2^13 = 0.00390635

と、約3.9mgです。


センサ出力の値に0.0039を掛けると、重力加速度に変換できます。

ただしこれは、±16gのスケールと13bitの分解能の場合です。
センサの設定が違えば、この値も変わってきます。



センサの値を重力加速度に変換できたら、X軸の回転角と、Y軸の回転角を求めます。



上図で、OX向きをX軸の回転軸とします。同様にY軸もOYを回転軸とします。

OZは重力加速度で、常に1gの大きさがあります。

また、上図には書いてませんが、角OXZ、角OYZ、角XOYは直角です。


この図で、θxがXの回転軸の角度、θyがYの回転軸の角度です。

θx、θyを求めれば、センサがどれだけ傾いているかがわかります。



上図は、OXZの三角形を抜き出したものです。


これからθxを求めるには、OZとOYの大きさが分かっていれば求められます。

OZは重力加速度なので常に1です。

OYはY軸方向の加速度で、センサの出力から求められます。


よって、θxは、

θx = sin^-1(OY / 1)[rad]

で求められます。


sin^-1はアークサインで、直角三角形の斜辺と求めたい角に接しない辺の長さから、求めたい角の大きさを求める三角関数です。

また、この計算結果は弧度法(ラジアン)で与えられるため注意が必要です。(C言語のmath.hの場合)



同様に、θyについても計算を行うと、もう片方の軸の角度を求めることができます。



さて、これを実際にArduinoで実装してみます。


#include
#include

int acc_address = 0x53;
long acc_x, acc_y, acc_z;
double g_x, g_y, g_z;
double x_rol, y_rol;


void setup()
{
  Serial.begin(9600);
  
  Wire.begin();
  
  Wire.beginTransmission(acc_address);
  Wire.write(0x31);
  Wire.write(0x0B);
  Wire.endTransmission();
  
  Wire.beginTransmission(acc_address);
  Wire.write(0x2d);
  Wire.write(0x08);
  Wire.endTransmission();
  
  delay(1);
}

void loop()
{
  Wire.beginTransmission(acc_address);
  Wire.write(0x32);
  Wire.endTransmission();
  Wire.requestFrom(acc_address, 6);
  if(Wire.available() > 5){
    acc_x = Wire.read();
    acc_x = (Wire.read() << 8) | acc_x;
    acc_y = Wire.read();
    acc_y = (Wire.read() << 8) | acc_y;
    acc_z = Wire.read();
    acc_z = (Wire.read() << 8) | acc_z;
  }
  
  
  g_x = acc_x * 0.0039;
  g_y = acc_y * 0.0039;
  g_z = acc_z * 0.0039;
  
  x_rol = asin(g_y / 1) * (180 / 3.1415);
  y_rol = asin(g_x / 1) * (180 / 3.1415);
  
  Serial.println(g);
  Serial.print("X_rol:");Serial.println(x_rol);
  Serial.print("Y_rol:");Serial.println(y_rol);
  
  delay(100);
}


大体こんな感じです。

シリアル通信で、PCに傾きの値を送信しています。



しかし、この加速度センサは、静止状態では傾きが分かりますが、水平方向に加速度が働くと、正しい値を得られません。

逆に、傾いた状態で、水平方向の加速度を測定することもできません。


じゃあどうするか?

加速度センサ以外のセンサを使って、傾きを検知すればいいのではないか?と思います。


というわけで、ジャイロセンサと加速度センサを使って、傾きと加速度を検出できるようにしたいと思います。


でもまだやってないので、また今度です。


【2014/10/12追加】
加速度センサとジャイロセンサを使って、傾きを検出する方法を書きました。
https://sites.google.com/site/nekolabwiki/electronic/posturecontroll

以下、参考文献、またはサイト
http://www.geology.smu.edu/~dpa-www/robo/balance/inertial.pdf
https://sites.google.com/site/yamagajin/home/filter
http://www.slideshare.net/rogy01/6-32797657

0 件のコメント:

コメントを投稿

記事の感想、意見などはこちらからどうぞ。

人気の投稿