記事一覧

MPU6050 x Arduino で スマホ用デジタル・スタビライザー風(Digital Stabilizer)のものを自作してみる。

RoboTakaoです。ようこそ「極力ローコスト ロボット製作 ブログ」にお越しくださいました。

前回までで3軸加速度センサー&3軸ジャイロスコープで色々テストしてきましたが、今回はスマホ用にデジタル・スタビライザー(Digital Stabilizer)風のものを自作してみたいと思います。

digitalStabi05.png

digitalStabi01.jpg

デジタル・スタビライザーとはビデオカメラ等を手ブレなく撮影するための機材で2軸または3軸のモーターで構成されて振動を吸収するものです。これを簡易的に作ってみようというものです。

今回も前回同様、加速度から算出した角度とジャイロから算出した角度で相補フィルターを使って角度を算出します。

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

主要な構成
3軸加速度&3軸ジャイロスコープ
Arduino Nano
サーボ GWS Micro2BB
セルカ棒の頭(ダイソー)
ラップの芯
サーボブラケット(以前自分で作った二足歩行ロボットの関節)

スマホとの接続はダイソーのセルカ棒を流用しました。
digitalStabi03.jpg

結線

digitalStabi04.png

結果
ラップの棒で作った握り手をグルグル回すと、なんとなく水平をキープしています。
でも画面の中ではブレブレです。なかなか改良の余地があります。
おそらくモーター軸とスマホの重心が一致していないのでモーメントが発生して振動しているのだと思います。
いつか改良してみようと思います。



関連記事
ArduinoとProcessingをFirmataで繋げてみました その2 加速度センサ
加速度センサー MPU6050 x RaspberryPi x python x Processing
ジャイロ MPU6050 x RaspberryPi x python x Processing
MPU6050 x RaspberryPi x python x Processing 相補フィルター Complementary filter

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 angZ = 0.0;
int angXi = 0;
int angZi = 0;
int angXiLast = 0;
int angZiLast = 0;
int aX,aZ;
int angXout, angZout;
float dt=50;

Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

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

myservo2.attach(2,Width_Min,Width_Max);
myservo3.attach(3,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)

// 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.0015) + 0.05*AcXAng;
angZ = 0.95*(angZ + GyZ*dt*0.0015) + 0.05*AcZAng;

angXiLast = angXi;
angZiLast = angZi;

angXi = (int) angX;
angZi = (int) angZ;

aX = angXi - angXiLast;
aZ = angZi - angZiLast;

for (int k=1; k <=10 ; k++){
angXout = aX*k/10 + angXiLast;
angZout = aZ*k/10 + angZiLast;

myservo2.write(95+angXout);
myservo3.write(100+angZout);

delay(dt*0.1);
}
}


それでは今回はこの辺で失礼します!ありがとうございます。

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク