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

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

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

1. 概要

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

主な構成要素 

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

2. システム設計

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

digitalstabilizer Picture 11

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

digitalstabilizer Picture 10

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

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

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

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

基本的な考え

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

4. 構造設計

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

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

5. Arduinoのスケッチ

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

#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);
  }
}

更新履歴

  • 2017.9.2デジタルスタビライザーのページを作りました。NEW 

inserted by FC2 system