二軸ジンバル

二軸ジンバルを作ってみます。「"ジンバル" とは何ぞや？」って方はまず Wikipedia のジンバルの項をご覧ください。

購入

ジンバル用のフレーム (元はカメラマウンタらしい) は Amazon で購入しました (購入時価格: ¥458)。組み立て用のビスやナットも付属します。

SG90 Servo SG90サーボ用ミニ2軸オリジナル FPV 特殊なナイロン PTZ（NOサーボ）カメラマウント A838

これは単なるフレームなので、サーボ x2 とジャイロセンサーが別途必要です。

組み立て

パーツはたった四つしかないので組み立て自体はそう難しくないでしょう。サーボの取り付けには、SG90 に付属しているサーボホーンを加工してやる必要があります。

フレームに収まるようにサーボホーンを加工してやります。

ちゃんと組み上がるとこのようになります。

サーボーンの固定には一番短いタッピングビスと、SG90 に付属の皿ネジを、サーボの固定には 6mm のビスを、土台の組み合わせ部には長いタッピングビスを使って固定してあります。短いタッピングビスはあと2・3本あってもよかった気がします。他にもナット＆ボルト等のネジが付属しますが使用しませんでした。

角度調整を行う必要があるので、最初からキッチリ組み立てない方がいいかと思います。可動範囲の確認はサーボのサンプルスケッチ Sweep.ino を実行するといいでしょう。

MPU-6050 用のケーブル

二軸ジンバルを作るためには、フレームの土台と MPU-6050 の取り付け位置を決定しなくてはなりませんが、センサーを物理的に動かす関係上、ブレッドボード用のジャンパーケーブルで接続すると接触不良になったりノイズ (電気的な) が乗るので、コネクタを圧着したリボンケーブルを作りました。

Arduino とサーボの接続

上側のサーボをデジタル 10 ピンへ、下側のサーボをデジタル 9 ピンへ接続しています。

カルマンフィルター

Chapter 21 (Option): Servo Motor で書いたように、センサーの生データは扱いにくいので、カルマンフィルターを通します。

KalmanFilter (GitHub)

ライブラリのインストールは [スケッチ | ライブラリを使用 | ライブラリをインストール] で行います。二軸ジンバルの元となるサンプルスケッチは、

Example-Sketch-for-IMU-including-Kalman-filter (GitHub)

に含まれる MPU6050.ino です。このスケッチでカルマンフィルターを通した二軸データが取得できるので、これとサーボのスケッチを組み合わせれば二軸ジンバルができます。

#include <Wire.h>
#include <Servo.h>
#include "Kalman.h"

Kalman kalmanX;
Kalman kalmanY;

double accX, accY, accZ;
double gyroX, gyroY, gyroZ;

double gyroXangle, gyroYangle;
double kalAngleX, kalAngleY;

uint32_t timer;
uint8_t i2cData[14];

Servo myservo_x;
Servo myservo_y;

int val_x;
int val_y;

void setup() {
  myservo_x.attach(9);
  myservo_y.attach(10);

  Wire.begin();
  TWBR = ((F_CPU / 400000L) - 16) / 2;

  i2cData[0] = 7;
  i2cData[1] = 0x00;
  i2cData[2] = 0x00;
  i2cData[3] = 0x00;
  while (i2cWrite(0x19, i2cData, 4, false));
  while (i2cWrite(0x6B, 0x01, true));

  while (i2cRead(0x75, i2cData, 1));
  if (i2cData[0] != 0x68) {
    while (1);
  }

  delay(100);

  while (i2cRead(0x3B, i2cData, 6));
  accX = (i2cData[0] << 8) | i2cData[1];
  accY = (i2cData[2] << 8) | i2cData[3];
  accZ = (i2cData[4] << 8) | i2cData[5];

  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

  kalmanX.setAngle(roll);
  kalmanY.setAngle(pitch);
  gyroXangle = roll;
  gyroYangle = pitch;

  timer = micros();
}

void loop() {
  while (i2cRead(0x3B, i2cData, 14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  gyroX = (i2cData[8] << 8) | i2cData[9];
  gyroY = (i2cData[10] << 8) | i2cData[11];
  gyroZ = (i2cData[12] << 8) | i2cData[13];

  double dt = (double)(micros() - timer) / 1000000;
  timer = micros();

  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;

  double gyroXrate = gyroX / 131.0;
  double gyroYrate = gyroY / 131.0;

  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
    kalAngleX = roll;
    gyroXangle = roll;
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt);

  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate;
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);

  gyroXangle += gyroXrate * dt;
  gyroYangle += gyroYrate * dt;

  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;

  val_x = kalAngleX + 90;            
  val_x = map(val_x, 0, 180, 0, 180);
  myservo_x.write(val_x);            

  val_y = kalAngleY + 90;            
  val_y = map(val_y, 0, 180, 0, 180);
  myservo_y.write(180 - val_y);        

  delay(2);
}

サンプルの丸パクリなソースコードですが、こんな感じになります。

動作確認

二軸ジンバルを実際に試してみた動画がこちらになります。

Arduino に電源を入れると軍曹さんが常に水平を保ちます。

ここにある情報が役に立って、「調べる手間が省けたからオマイに飯でもおごってやるよ」というハートウォーミングな方がいらっしゃいましたら、下のボタンからどうぞ。