SPRESENSE と BMI160アドオンボードで傾きを測ってみた！（１）

前回、SPRESENSEでモーターの回転数を測ってみました。今回は、スイッチサイエンスから出ている BMI160アドオンボードを使って、倒立振子の車体の傾きを検出してみたいと思います。

SPRESENSE用3軸加速度・3軸ジャイロ・気圧・温度センサアドオンボード BMP280 BMI160搭載
https://www.switch-science.com/catalog/5258/

このボードはジャイロと加速度センサーが搭載されています。今回はそれぞれのセンサーで出力される値を比較してみたいと思います。加速度センサーとジャイロセンサーの座標軸は次のようになっています。

加速度センサーの座標軸

ジャイロセンサーの座標軸

ジャイロセンサーからは角加速度が出力されますので、角度は時間積分で求まります。

\[
\theta = \int_{0}^{t} \omega(t) \, dt
\]

加速度センサーで角度を出すには、重力を使います。重力加速度は常に下向きになりますので、角度は例えばZ成分とY成分のatan で求まります。プログラミング時は角度とラジアンの単位の違いに注意してください。

あともう一点、注意しないといけないのは、置いた場所が水平とは限らないことです。何もしないと各軸への重力影響が取り除けず正しい角度が算出できません。開始時に各軸の重力加速度を計測をしておき、制御時はその影響を取り除く必要があります。（プログラム中では"Calibration"と呼んでいます）

これらのことを踏まえて、次のようなスケッチを書いてみました。


#include <Wire.h>
#include <math.h>
#include <BMI160Gen.>

#define BAUDRATE  115200
#define SENSE_RATE   200
#define GYRO_RANGE   250
#define ACCL_RANGE     2

#define COUNT_FOR_CALIB 1024

#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)

#define PRINT_ACCL
#define PRINT_GYRO
// #define PRINT_YAW

uint16_t adjust_usec;
static float calib_accel_x = 0, calib_accel_y = 0, calib_accel_z = 0;   

float convertRawGyro(int gRaw) 
{
  // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
  float lsb_omega = float(0x7FFF) / GYRO_RANGE;
  return gRaw / lsb_omega;  // deg/sec
}

float convertRawAccel(int aRaw) 
{
  // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
  float lsb_g = float(0x7FFF) / ACCL_RANGE;
  return aRaw / lsb_g;
}

void print_pitch_roll() 
{

  static uint16_t n = 0;
  static float g_angle_roll = 0, g_angle_pitch = 0, g_angle_yaw = 0;
  static float a_angle_roll = 0, a_angle_pitch = 0, a_angle_yaw = 0;      
 
  static unsigned long last_mills = 0; 
  
  // read raw gyro measurements from device
  int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
  BMI160.readGyro(rawRoll, rawPitch, rawYaw);
  
  // read raw accl measurements from device
  int rawXAcc, rawYAcc, rawZAcc; // x, y, z
  BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);

  unsigned long cur_mills = millis();
  unsigned long duration = cur_mills - last_mills;
  last_mills = cur_mills;
  float fduration = duration / 1000.0; // ms->s
  
  // convert the raw gyro data to degrees/second
  float omega_roll  = convertRawGyro(rawRoll);
  float omega_pitch = convertRawGyro(rawPitch);
  float omega_yaw   = convertRawGyro(rawYaw);
  
  g_angle_roll  += omega_roll  * fduration; // (ms->s) omega x time = degree
  g_angle_pitch += omega_pitch * fduration;
  g_angle_yaw   += omega_yaw   * fduration; // for the reference

  // convert the raw acclerometer data to m/s^2
  float cur_a_x = convertRawAccel(rawXAcc) - calib_accel_x;
  float cur_a_y = convertRawAccel(rawYAcc) - calib_accel_y;
  float cur_a_z = convertRawAccel(rawZAcc) - calib_accel_z;
  cur_a_x = deg_to_rad(cur_a_x);
  cur_a_y = deg_to_rad(cur_a_y);
  cur_a_z = deg_to_rad(cur_a_z);
  a_angle_roll = atan(cur_a_y / cur_a_z);
  a_angle_pitch = atan(-cur_a_x / cur_a_z);
  a_angle_roll = rad_to_deg(a_angle_roll);
  a_angle_pitch = rad_to_deg(a_angle_pitch);
  
   
#ifdef PRINT_GYRO 
  // Serial.print("Duration:");
  // Serial.print(duration);
  // Serial.print(",");
  Serial.print("GYRO_roll:");
  Serial.print(g_angle_roll);
  Serial.print(",");
  Serial.print("GYRO_pitch:");
  Serial.print(g_angle_pitch);
  Serial.print(",");
#ifdef PRINT_YAW
  Serial.print("GYRO_yaw:");
  Serial.print(g_angle_yaw);
  Serial.print(",");
#endif
#endif

  // display tab-separated accel x/y/z values
#ifdef PRINT_ACCL
  Serial.print("ACC_roll:");
  Serial.print(a_angle_roll);
  Serial.print(",");
  Serial.print("ACC_pitch:");
  Serial.print(a_angle_pitch);
#endif

  Serial.println();
  usleep(adjust_usec); // 2msec is the process time for displaying the values.
}


void setup() {
  Serial.begin(BAUDRATE);

  BMI160.begin(BMI160GenClass::I2C_MODE);

  BMI160.setGyroRate(SENSE_RATE);
  BMI160.setAccelerometerRate(SENSE_RATE);
  BMI160.setGyroRange(GYRO_RANGE);
  BMI160.setAccelerometerRange(ACCL_RANGE);

  BMI160.autoCalibrateGyroOffset();
  BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  // Serial.println("Calibrating...");
  unsigned long sampling_rate = 1000 / SENSE_RATE;

  // Calcurate offset
  for (int i = 0; i < COUNT_FOR_CALIB; ++i) {
    int xAcc, yAcc, zAcc;
    BMI160.readAccelerometer(xAcc, yAcc, zAcc);
    calib_accel_x += convertRawAccel(xAcc);
    calib_accel_y += convertRawAccel(yAcc);
    calib_accel_z += convertRawAccel(zAcc)-1.0;
  }
  
  // mean value
  calib_accel_x /= COUNT_FOR_CALIB;
  calib_accel_y /= COUNT_FOR_CALIB;
  calib_accel_z /= COUNT_FOR_CALIB;
  
  calib_accel_z *= 1000; // The offset supports +/-495.3(mg)
  Serial.println("calib_a_x:" + String(calib_accel_x, 3));
  Serial.println("calib_a_y:" + String(calib_accel_y, 3));
  Serial.println("calib_a_z:" + String(calib_accel_z, 3));

  // SENSE_RATE is Hz and 2 millsec is the process time for this sketch
  adjust_usec = (1000/SENSE_RATE - 2) * 1000; 
  digitalWrite(LED0, HIGH);
}

void loop() {
  print_pitch_roll(); 
}

このスケッチで計測した結果がこちらです。

ジャイロの変化は非常に滑らかなのに対し、加速度センサーはノイズが多いです。一方、ジャイロは"gyro_roll"の最後にオフセットがかかってしまっていますが、加速度センサーはその影響はなさそうです。（拡大して見ると"gyro_pitch"もオフセットを持ってしまっているようです）

どちらも一長一短ですね。この２つのセンサーの特長をうまく利用するために、次はフィルター処理を試してみたいと思います。😀
（＾＾）／～

SONY SPRESENSE メインボード CXD5602PWBMAIN1

出版社/メーカー: スプレッセンス(Spresense)
メディア: Tools & Hardware

ROHM センサAdd-onボード SPRESENSE-SENSOR-EVK-701

出版社/メーカー: ローム(ROHM)
メディア: Tools & Hardware

SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサーモジュールIIC SPIブロード

出版社/メーカー: SODIAL
メディア: その他

SPRESENSE と BMI160アドオンボードで傾きを測ってみた！（１）

関連する記事