【Arduino】MPU-6050を使って姿勢角を算出　（フィルターなし）

前回、MPU-6050を使って角速度と加速度のデータを取得するプログラムを書きました。
shizenkarasuzon.hatenablog.com

今回は、それを使って姿勢角を求めたいと思います。

1.方法
実行環境
実行結果
プログラム（全文）

1.方法

姿勢角には、Pitch（ピッチ）、Roll（ロール）、Yaw（ヨー）の3種類があります。

この記事では、ピッチ角とロール角を求めてみます。

それぞれ、以下の数式で表すことができます。

ロール角 $\phi$ について
$\phi = atan(\dfrac{a_y}{a_z})$

ピッチ角 $\psi$ について
$\psi = atan(\dfrac{-a_x}{a_y sin(\phi) + a_z cos(\phi)})$

$a_x$ 、 $a_x$ 、 $a_x$ はそれぞれ、 $x$ 軸、 $y$ 軸、 $z$ 軸の加速度を表します。

この部分をArduinoのプログラムに実装すると、このようになります。

double roll  = atan2(acc_y, acc_z) * RAD_TO_DEG;
double pitch = atan(-acc_x / sqrt(acc_y * acc_y + acc_z * acc_z)) * RAD_TO_DEG;

RAD_TO_DEGというのは、 $\dfrac{360}{2\pi}$ を表す定数で、弧度法（rad）の数値を度数法（°）に変更にするのに使用しています。

これを実際に埋め込んだプログラム全文は、この記事の一番下に書いています。

実行環境

回路図等は、以下のサイトをご覧ください。
shizenkarasuzon.hatenablog.com

プログラム全文は、この記事の一番下に載せています。

実行結果

f:id:pythonjacascript:20190216170934j:plain
このように、シリアルモニタを通して姿勢角のデータを算出することに成功しました。

Processingで可視化した結果です。
f:id:pythonjacascript:20190216165429j:plain

Processingのプログラムはここを見ながら作ってください。

プログラム（全文）

#include <Wire.h>
#define MPU6050_WHO_AM_I     0x75  // R
#define MPU6050_PWR_MGMT_1   0x6B  // R/W
#define MPU_ADDRESS  0x68


// デバイス初期化時に実行される
void setup() {
  Wire.begin();
  Serial.begin(115200); //115200bps

  Wire.beginTransmission(MPU_ADDRESS);
  Wire.write(MPU6050_PWR_MGMT_1);  //MPU6050_PWR_MGMT_1レジスタの設定
  Wire.write(0x00);
  Wire.endTransmission();
}


void loop() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);
  int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;

  axRaw = Wire.read() << 8 | Wire.read();
  ayRaw = Wire.read() << 8 | Wire.read();
  azRaw = Wire.read() << 8 | Wire.read();
  Temperature = Wire.read() << 8 | Wire.read();
  gxRaw = Wire.read() << 8 | Wire.read();
  gyRaw = Wire.read() << 8 | Wire.read();
  gzRaw = Wire.read() << 8 | Wire.read();

  // 加速度値を分解能で割って加速度(G)に変換する
  float acc_x = axRaw / 16384.0;  //FS_SEL_0 16,384 LSB / g
  float acc_y = ayRaw / 16384.0;
  float acc_z = azRaw / 16384.0;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  float gyro_x = gxRaw / 131.0;  // (度/s)
  float gyro_y = gyRaw / 131.0;
  float gyro_z = gzRaw / 131.0;

  double roll  = atan2(acc_y, acc_z) * RAD_TO_DEG;
  double pitch = atan(-acc_x / sqrt(acc_y * acc_y + acc_z * acc_z)) * RAD_TO_DEG;
  
  Serial.print(roll);  Serial.print(",");
  Serial.print(pitch);  Serial.println("");
}