【簡単 ・安価】Arduinoと3軸加速度・3軸ジャイロセンサ(MPU6050)を使用して姿勢角の制御を行ってみる

ロボットを作成したいと考えたとき、姿勢角の制御が非常に重要になります。例えば、ドローンを水平に維持するためには姿勢角を制御する必要があり、加速度センサ、ジャイロセンサが使用されています。また、カメラの手振れ補正などにも使われています。今回使用するMPU6050は非常に安価で簡単に使用できるため、今回はこの使用方法について紹介いたします。

今回はMPU6050を使用して、使用方法と現状の角度の測定方法を紹介します。

MPU6050の使用方法について紹介

使用するもの

今回使用するものはこちらになります。

ArduinoのスタートキットとMPU6050を使用すれば、姿勢制御の概要がわかるので、下記のものだけで作ることができます。

スタートキットがあるとあまりなじみのないセンサやいろんな基礎知識が学べるのでおすすめです。

加速度センサによる角度測定

姿勢を保つためには角度測定が必要になります。MPU6050が傾いた際に重力加速度Gが作用します。角度θはセンサにかかるx軸、Z軸の加速度Gx, Yzより下記式で算出できます。

ただしこの方法であると、運動加速度が無視できない場合、正確な角度測定ができなくなります。

3軸の加速度センサからこのように角度測定が可能となります。

ジャイロセンサによる角度測定

またジャイロセンサにより角度測定を行うこともできます。ジャイロセンサは角加速度センサのため、加速度を積分することで角度を計算することができます。ただし、ドリフト成分を含むため、数値が徐々に変化し、絶対的な角度測定ができません。

そのため、①と②を組み合わせて相補フィルタを用いることで正確な測定を行うことができます。今回はこの2つのセンサを用いて測定を行います。

MPU6050を使用したプログラム内容について

MPU6050を使用したプログラムがこちらになります。

こちらでは、MPU6050とモータを使用して、ロボットのPID制御を行い、倒立するロボットを作っています。balancing robotというか倒立振子するロボットになっています。このようにMPU6050を使用して簡単に角度を制御し姿勢制御するロボットを作ることができます。

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"
MPU6050 accelgyro;

const float Targetdeg=0;
const float Kp=20;
const float Ki=10;
const float Kd=0;
float P, I, D, preP;

int16_t ax, ay, az;
int16_t gx, gy, gz;

// L298N motor GPIO Setting
extern int gpLb =  10; // Left 1
extern int gpLf = 11; // Left 2
extern int gpRb = 12; // Right 1
extern int gpRf = 13; // Right 2
extern int ENA1 = 3;  //left
extern int ENA2 = 5; //right
uint8_t duty; //モータのコントロール値

void setup() {
  Wire.begin();
  Serial.begin(115200);
  
// モータ設定
  pinMode(gpLb, OUTPUT); //Left Backward
  pinMode(gpLf, OUTPUT); //Left Forward
  pinMode(gpRb, OUTPUT); //Right Forward
  pinMode(gpRf, OUTPUT); //Right Backward
  pinMode(ENA1, OUTPUT);
  pinMode(ENA2, OUTPUT);
  //initialize
  digitalWrite(gpLb, LOW);
  digitalWrite(gpLf, LOW);
  digitalWrite(gpRb, LOW);
  digitalWrite(gpRf, LOW);
  digitalWrite(ENA1, LOW);
  digitalWrite(ENA2, LOW);

  // 初期化
  accelgyro.initialize();
  delay(10);

  // 計測範囲を2000 deg/secに設定、16.4 LSB/deg/s
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  delay(10);
  
  accelgyro.CalibrateGyro();
  accelgyro.CalibrateAccel();
}

void loop() {

  // 時間を計っとく
  static float timer = 0;

  // 各軸加速度と角速度を取得
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  //X、Y軸まわりの角度を計算
  float degY = atan2(ax, az) * RAD_TO_DEG;
  float degX = atan2(ay, az) * RAD_TO_DEG;

  // Z軸まわりの角度を計算
  static float degZ = 0;
  static int last_time = micros();
  float omega = float(gz)/16.4; //omega[deg/s] = gz[LSB] / 16.4[LSB/deg/s] LSB：最下位ビット
  int current_time = micros();  //time[us]
  float dt = current_time - last_time;
  // 角度は角速度の積分
  degZ += omega *dt/1000000.0;  //degZ[deg] = omega[deg/s] * dt[us] / 1000000[us/s]
  last_time = current_time;

//  for(int i=0;i<1000;i++){
//    degY=degY/1000;}
    
  P=degY-Targetdeg;
  I+=P*dt;
  D=(P-preP)/dt;
  preP=P;
  
  duty=Kp*P+Kd*D+Ki*I;

   if (degY>0){
     digitalWrite(gpLb,HIGH);
     digitalWrite(gpRb,LOW);
     digitalWrite(gpLf,LOW);
     digitalWrite(gpRf,HIGH);
     analogWrite(ENA1,duty);
     analogWrite(ENA2,duty);
   }else{
     digitalWrite(gpLb,LOW);
     digitalWrite(gpRb,HIGH);
     digitalWrite(gpLf,HIGH);
     digitalWrite(gpRf,LOW);
     analogWrite(ENA1,duty);
     analogWrite(ENA2,duty);
   }

//  Serial.print(timer+=dt/1000000); Serial.print(", ");
  Serial.print(degX); Serial.print(", ");
  Serial.print(degY);Serial.print(", ");
  Serial.print(degZ);Serial.print(",");
  Serial.println(duty);
}

https://www.youtube.com/shorts/vE2m0uL1TjE