Arduinoで作る自作デジタルスタビライザー

Arduinoと加速度＆ジャイロでデジタルスタビライザーを作ってみた

 2018.9.2

自作デジタルスタビライザの制作

1. 概要

ここでは３つのサーボとArduino Nanoと加速度＆ジャイロセンサーを使ってデジタリスタビライザーを製作してみたので紹介します。
とりあえず出来上がりはこんな感じで、こんな風に握り手を動かしても先端に取り付けられたスマホが水平を保ってくれるように動きます。
またジョイスティックで方向を変えられるようにしました。
digitalstabilizer Picture 1digitalstabilizer Picture 2

主な構成要素 

・Micro2BB サーボ 3個
・Arduino Nano コントロールボード
・MPU6050 3軸加速度センサー＆３軸ジャイロ　または MPU9250 3軸加速度センサー＆３軸ジャイロ＆３軸磁気センサー
・ジョイススティック
・基板（ユニバーサル基板）
・6Vニッケル水素電池
・3Dプリントしたサーボブラケットとケース

2. システム設計

システムの構成要素は次のようなものになっています。

digitalstabilizer Picture 11

ユニバーサル基板の回路図はこちら

digitalstabilizer Picture 10

3. 加速度＆ジャイロセンサー

今回使用するのはMPU6050というチップの載せた３軸加速度＋３軸ジャイロセンサーです。

Type:GY521
Weight:18.1 g
Size:3 x 2.1 x 0.5 cm Input Voltage:3-5V
Interface: I2C (2 wire, IIC)
Gyro rage：±250 500 1000 2000 °/s
Accelerometer rage:±2±4±8±16g

digitalstabilizer Picture 11

データシートはこちら

MPU-6050 DataSheet
MPU-6050 Register Map

4. 加速度＆ジャイロセンサー＆地磁気センサー

さらにMPU9250 ３軸加速度＋３軸ジャイロセンサー＋３軸地磁気センサーでも動かしてみます。

モジュールモデル：GY-9250
チップ：MPU-9250
コア電圧：DC2.4V～3.6V
　・I/O電圧：1.71V～コア電圧まで
　・インターフェース：I2CまたはSPI
　・サイズ：約13x11mm
　・重さ：0.45g (端子含まず)
加速度
　・測定レンジ ±2 / ±4 / ±8 / ±16g
　・分解能：16ビット
　・出力レート：0.24～4000Hz
ジャイロ
　・測定レンジ ±250 / ±500 / ±1000 / ±2000dps（°/sec）
　・分解能：16ビット
　・出力レート：4～8000Hz
コンパス（AK8963）
　・測定レンジ：±4800μT
　・分解能：14ビット/16ビット

digitalstabilizer Picture 12

取り付けもMPU6050と互換性があります

digitalstabilizer Picture 13

今回は比較的実装が簡単な相補フィルターを使用します。

基本的な考え

角度 = 0.98 x (前回の角度 + ジャイロの値 x サンプリング周期) x 0.02 x 加速度センサーで出した角度

5. 構造設計

構造設計はFusion360で行い、３Dプリンターで出力しました。

digitalstabilizer Picture 3 digitalstabilizer Picture 4 digitalstabilizer Picture 5
digitalstabilizer Picture 6 digitalstabilizer Picture 7

6. Arduinoのスケッチ

Arduinoのスケッチを示します。(良いスケッチとは思えませんが）

MPU6050の場合

#include
#include 

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr;
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
float AcXAng,AcZAng;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700;    //最小パルス幅（ms）
int Width_Max = 2300;   //最大パルス幅（ms）


void setup() 
{ 
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  //LPF設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,Width_Min,Width_Max);
  myservo4.attach(4,Width_Min,Width_Max);

  Serial.begin(9600);

} 

void loop() 
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcXr=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcYr=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZr=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmpr=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyXr=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyYr=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZr=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

// joystic data
  val1 = analogRead(joy_1);
  val2 = analogRead(joy_2);

  if (val1 < 300){
    angjoy1 = angjoy1 - djoy;
  }
  if (val1 > 700){
    angjoy1 = angjoy1 + djoy;
  }
  if (val2 < 300){
    angjoy2 = angjoy2 - djoy;
  }
  if (val2 > 700){
    angjoy2 = angjoy2 + djoy;
  }
  
// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
  AcX = AcXr/16384.0;
  AcY = AcYr/16384.0;
  AcZ = AcZr/16384.0;
  
// temperture
  Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
  GyX = GyXr/131.0;
  GyY = GyYr/131.0;
  GyZ = GyZr/131.0;

  AcXAng = atan(AcZ/AcY)/3.142*180;
  AcZAng = atan(AcX/AcY)/3.142*180;

//相補フィルター
  angX = 0.95*(angX - GyX*dt*0.001*1.5) + 0.05*AcXAng;
  angZ = 0.95*(angZ + GyZ*dt*0.001*1.2) + 0.05*AcZAng;

//鉛直軸
  angY = angY - GyY*dt*0.001;

  angXiLast = angXi;
  angYiLast = angYi;
  angZiLast = angZi;

  angXi = (int) angX + angjoy1;
  angYi = (int) angY - angjoy2;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aY = angYi - angYiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <= dstep ; k++){
    angXout = aX*k/dstep + angXiLast;
    angYout = aY*k/dstep + angYiLast;
    angZout = aZ*k/dstep + angZiLast;

    if (angXout < -40) angXout = -40;
    if (angXout > 20) angXout = 20;
    if (angYout < -80) angYout = -80;
    if (angXout > 80) angYout = 80;
    if (angZout < -70) angZout = -70;
    if (angZout > 70) angZout = 70;

    myservo2.write(ZeroY - angYout);
    myservo3.write(ZeroX - angXout);
    myservo4.write(ZeroZ - angZout);

    delayMicroseconds(dt/dstep*1000);
  }
}

MPU9250の場合

#include
#include 

const int MPU_addr=0x68;  // I2C address of the MPU-6050
const int AK8963_addr=0x0c;  // I2C address of the AK8963
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr,MgXr,MgYr,MgZr;
int16_t Mg[7];
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ,MgXmin,MgYmin,MgZmin,MgXmax,MgYmax,MgZmax,MgXoff,MgYoff,MgZoff;
float AcXAng,AcZAng,AcXAngPi,AcZAngPi,MgYAng;
float Upper,Lower;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700;    //最小パルス幅（ms）
int Width_Max = 2300;   //最大パルス幅（ms）


void setup() 
{ 
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  //LPF設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();

  //AK8963設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x37);  //磁気センサのバイパスモード設定用のアドレス
  Wire.write(0x02);  //bypass mode(磁気センサが使用出来るようになる)
  Wire.endTransmission();
  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x0a);  //磁気センサ設定用のアドレス
  Wire.write(0x16);  //磁気センサの出力周期(100Hz) 0x12:磁気センサの出力周期(8Hz)
  Wire.endTransmission();
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,Width_Min,Width_Max);
  myservo4.attach(4,Width_Min,Width_Max);

  Serial.begin(9600);

} 

void loop() 
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcXr=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcYr=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZr=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmpr=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyXr=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyYr=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZr=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x02);  // starting with register 0x02
  Wire.requestFrom(AK8963_addr,1,true);
  Wire.endTransmission(false);

  //ST1Bit=Wire.read();

  if((Wire.read() & 0x01)){   //読み出し準備ができたか確認
    Wire.beginTransmission(AK8963_addr);
    Wire.write(0x03);  // starting with register 0x02
    Wire.endTransmission(false);
    Wire.requestFrom(AK8963_addr,7,true);  // request a total of 7 registers
  
    MgYr=Wire.read()|(Wire.read()<<8);
    MgXr=Wire.read()|(Wire.read()<<8);
    MgZr=-(Wire.read()|(Wire.read()<<8));
  }

// joystic data
  val1 = analogRead(joy_1);
  val2 = analogRead(joy_2);

  if (val1 < 300){
    angjoy1 = angjoy1 - djoy;
  }
  if (val1 > 700){
    angjoy1 = angjoy1 + djoy;
  }
  if (val2 < 300){
    angjoy2 = angjoy2 - djoy;
  }
  if (val2 > 700){
    angjoy2 = angjoy2 + djoy;
  }
  
// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
  AcX = AcXr/16384.0;
  AcY = AcYr/16384.0;
  AcZ = AcZr/16384.0;
  
// temperture
  Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
  GyX = GyXr/131.0;
  GyY = GyYr/131.0;
  GyZ = GyZr/131.0;
  
  AcZAngPi = atan(AcX/AcY);
  AcZAng = AcZAngPi/3.142*180;
  AcXAngPi = atan(-AcZ/(AcX*sin(AcZAngPi)+AcY*cos(AcZAngPi)));
  AcXAng = AcXAngPi/3.142*180;
  
// Magnetic data
  MgX = MgXr / 32768.0f * 4800.0f;//[uT]に変換
  MgY = MgYr / 32768.0f * 4800.0f;//[uT]に変換
  MgZ = MgZr / 32768.0f * 4800.0f;//[uT]に変換

//Calculation of offset

  if (MgX <= MgXmin) MgXmin = MgX;
  if (MgX >= MgXmax) MgXmax = MgX;
  if (MgY <= MgYmin) MgYmin = MgY;
  if (MgY >= MgYmax) MgYmax = MgY;
  if (MgZ <= MgZmin) MgZmin = MgZ;
  if (MgZ >= MgZmax) MgZmax = MgZ;

  MgXoff = (MgXmax - MgXmin)/2 + MgXmin;
  MgYoff = (MgYmax - MgYmin)/2 + MgYmin;
  MgZoff = (MgZmax - MgZmin)/2 + MgZmin;

  Upper = (MgY - MgYoff)*sin(AcZAngPi) - (MgX - MgXoff)*cos(AcZAngPi);
  Lower = (MgZ - MgZoff)*cos(AcXAngPi) + (MgX - MgXoff)*sin(AcXAngPi)*sin(AcZAngPi) + (MgY - MgYoff)*sin(AcXAngPi)*cos(AcZAngPi);
  
  if(Lower > 0.0){
    if(Upper > 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180;
      }else if(Upper < 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180;
      }
  }else if(Lower < 0.0){
    if(Upper > 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180 + 180;
      }else if(Upper < 0.0){
         MgYAng = atan(Upper/Lower)/3.142*180 - 180;
      }
    }
  
//相補フィルター
  angX = 0.9*(angX - GyX*dt*0.001*1.5) - 0.1*AcXAng;
  angZ = 0.9*(angZ + GyZ*dt*0.001*1.2) + 0.1*AcZAng;
  angY = 0.9*(angY - GyY*dt*0.001) - 0.1*MgYAng;

  angXiLast = angXi;
  angYiLast = angYi;
  angZiLast = angZi;

  angXi = (int) angX + angjoy1;
  angYi = (int) angY - angjoy2;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aY = angYi - angYiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <= dstep ; k++){
    angXout = aX*k/dstep + angXiLast;
    angYout = aY*k/dstep + angYiLast;
    angZout = aZ*k/dstep + angZiLast;

    if (angXout < -40) angXout = -40;
    if (angXout > 20) angXout = 20;
    if (angYout < -80) angYout = -80;
    if (angYout > 80) angYout = 80;
    if (angZout < -70) angZout = -70;
    if (angZout > 70) angZout = 70;

    myservo2.write(ZeroY - angYout);
    myservo3.write(ZeroX - angXout);
    myservo4.write(ZeroZ - angZout);

    delayMicroseconds(dt/dstep*1000);
  }
}

更新履歴

  • 2017.11.14MPU9250を追加しました。NEW 
  • 2017.9.2デジタルスタビライザーのページを作りました。 
Copyright © 2017 robotakao All Rights Reserved.

inserted by FC2 system