ArduinoでIMU-3000(ジャイロ)を使用して角速度と旋回角を計測する

森下功啓製作所 ONLINE

[2014/2/7追記] 誤字を修正しました。

1. はじめに

図1.1に示すIMU-3000はストロベリーリナックスよりたったの4,725円で購入可能な3軸角速度(ジャイロ)センサです。 感度調整は4段階可能で、ハイエンドクラスにしては激安だと思います。

実はGPSロボットカーコンテスト2011で初めて角速度センサ(ジャイロセンサ)を使ったのですが、散財した甲斐あって上手くロボットを誘導することができました。 せっかくなので本ページではIMU-3000の基本的な使い方を公開しようと思います。 なお、ArduinoからIMU-3000をコントロールするスケッチはhttp://www.hobbytronics.co.uk/arduino-adxl345-imu3000からダウンロード可能です。


IMU-3000の写真
図1.1 IMU-3000を搭載したArduino
ちなみにシールドにはMaple(Olimexino)用の物を使いました。安いし、配線は気が利いているし言うことなし。

2. 角速度の取得方法

IMU-3000のインターフェイスはI2Cのみですので、マイコンとはI2Cポートと接続する必要があります。 Arduinoならば既にAPIが公開されていますので計測は簡単です。

[蛇足]

ちなみに、GPSロボットカーコンテスト2011ではメインMCUのH8マイコンに直接接続して制御する時間的余裕がなかったため、Arduinoで角速度を計測させていました。 ArduinoからはPWM波形として観測結果を出力させ、H8マイコン側はインプットキャプチャを利用してデューティ比を観測することで角速度を得ていました。 来年からはセンサ周りはArduinoに処理を任せて、H8マイコン(ARMになるかも)からはI2Cで観測データを得たいと考えています。 云わば、ハードウェアのオブジェクト化(モジュール化)です。 ソフトウェアI2CをArduinoが扱えればいいのですが…利用できない場合はちょっと別の方法を考える必要がありそうです。


3. スケッチ(プログラムソースコード)

時刻管理を利用して回転角度を出力するサンプルスケッチを図3.1に示します。 ソースコードのダウンロードは下記のリンクより可能です。 サンプルコードでは、10 ms毎にタイマ割り込みを利用してz軸周りの角速度の計測を行い、同時に旋回角を推定しています。

[注意]下記のスケッチにあるコメントには6軸となっていますが、これは誤りです。 IMU-3000は3軸加速度角速度センサです。 スケッチまで手が回りませんので、ダウンロード後に修正してください。。。


プログラムのダウンロード
リリース日 プログラムパッケージ 更新内容
2011/11/27 IMU3000_test.zip 初公開です。

[2012/12/19追記]
Arduino IDE 0022にて動作を確認しています。
SCAとSCLというポートがArduinoとセンサ側の両方にあるので、同じ名前同士のポートを接続すればOKです。 ・・・ソースコード内のIOポートマップにはI2CはA0とA1に接続するよう書いてありますけど、これってA4とA5じゃないかな? 現物を人に貸したから思い出せないや・・・。
/**********************************************************************************
 * [Original message]
 * IMU Fusion Board - ADXL345 & IMU3000
 * Example Arduino Sketch to read the Gyro and Accelerometer Data
 * 
 * Written by www.hobbytronics.co.uk 
 * See the latest version at www.hobbytronics.co.uk/arduino-adxl345-imu3000
 * 08-Apr-2011
 * <--- これ以降はコードの改変情報 --->
 * [開発者(改造なんだけどね)]
 *  森下功啓
 * [概要]
 *  IMU-3000という6軸センサ(3軸加速度+3軸角速度)を用いて、Z軸周りの回転角度を計測します。
 *  なお、本プログラムでは分解能と短時間での精度を重視したために時間が経つと回転角度がドリフトします。
 *  これは分解能を落とすことでバイアスを無視すれば回避できます。
 * [開発プラットフォーム]
 *  Arduino pro ATmega328 3.3V版
 * [I/Oポートマップ]
 *  D0  : シリアルRx
 *  D1  : シリアルTx
 *  D2  : ディップスイッチの状況確認用 入力
 *  D3  : NC
 *  D4  : NC
 *  D5  : NC
 *  D6  : NC
 *  D7  : NC
 *  D8  : NC
 *  D9  : NC
 *  D10 : NC
 *  D11 : NC
 *  D12 : NC
 *  D13 : NC
 *  A0  : I2C 
 *  A1  : I2C 
 *  A2  : NC
 *  A3  : NC
 *  A4  : NC
 *  A5  : NC
 * [履歴]
 *  2011/11/26 本月始めに作ったロボットカー用のソースコードをちょっと改造して、角度計測が完結するようにした。
 **********************************************************************************/
/****************** ヘッダのインクルード ****************************************/
#include <MsTimer2.h>
#include <Wire.h>
/****************** 定数宣言 ****************************************/
#define GYRO                0x68   // gyro I2C address
#define REG_GYRO_X          0x1D   // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL               0x53   // Accel I2c Address
#define ADXL345_POWER_CTL   0x2D
#define DIP_UART_ENABLE_PIN 2      // ディップスイッチのポート番号
#define INTERVAL            10     // 観測インターバル[ms]
#define INT_MAX             32767  // 角速度観測値の最大値。分解能が16bitなので、 int型の最大値でもある。
#define ANGLE_RATE_MAX      500    // 角速度の最大[deg/sec](角速度センサの初期設定に依存する)
#define BUFF_SIZE           128    // 角速度のバッファサイズ
/****************** グローバル変数 ****************************************/
byte    buffer[12];                // Array to store ADC values 
int     gyroBuff[BUFF_SIZE];       // 計測値の平均を出すために、をこの配列に格納する。オフセットを取るのに使用する。
int     wp        = 0;             // 上のバッファへの書き込みアドレス
double  gyro_bias = 0.0;           // 角速度計測値のバイアス値
int     backup_gyro_z = 0;         // 角速度計測値のバックアップ(台形公式用)
double  angle     = 0.0;           // 角度[deg]
boolean gyroGo    = false;         // 角速度の計測許可フラグ
/****************** 関数 ****************************************/
/*-------------------------------------------------------
 タイマー割り込み
 ---------------------------------------------------------*/
void clock(void){
  gyroGo = true;                              // 角速度の計測を指示
  return;
}
/*-------------------------------------------------------
 Write a value to address register on device
 ---------------------------------------------------------*/
void writeTo(int device, byte address, byte val) {
  Wire.beginTransmission(device);       // start transmission to device 
  Wire.send(address);                   // send register address
  Wire.send(val);                       // send value to write
  Wire.endTransmission();               // end transmission
}
/*-------------------------------------------------------
 初期化
 ---------------------------------------------------------*/
void setup()
{
  Serial.begin(57600);                   // シリアル通信速度の設定56700bps
  pinMode(DIP_UART_ENABLE_PIN, INPUT);    // ディップスイッチ用の入力設定

  // ジャイロ関係の初期設定
  Wire.begin();
  // Set Gyro settings
  // Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s 
  writeTo(GYRO, 0x16, 0x0B);       
  //set accel register data address
  writeTo(GYRO, 0x18, 0x32);     
  // set accel i2c slave address
  writeTo(GYRO, 0x14, ACCEL);     
  // Set passthrough mode to Accel so we can turn it on
  writeTo(GYRO, 0x3D, 0x08);     
  // set accel power control to 'measure'
  writeTo(ACCEL, ADXL345_POWER_CTL, 8);     
  //cancel pass through to accel, gyro will now read accel for us   
  writeTo(GYRO, 0x3D, 0x28);    

  // タイマーの割り込み設定
  MsTimer2::set(INTERVAL, clock);      // 割り込みのタイマーを設定(割り込みを利用したソフトウェア割り込みのように見える処理)
  MsTimer2::start();                   // 割り込みのタイマーを開始
}
/*-------------------------------------------------------
 ループ
 ---------------------------------------------------------*/
void loop()
{
  // 'r'を受信すればデフォルト値のリセット
  int k;
  long sum = 0l;
  if((k = Serial.read()) == 'r'){
    for(k = 0; k < BUFF_SIZE; k++)sum += (long)gyroBuff[k];
    gyro_bias = (double)sum / (double)BUFF_SIZE;  // バイアス値をセット
    angle = 0.0;                                  // 角度の積分値をリセット
  }

  // ジャイロ関係の処理
  // Read the Gyro X, Y and Z and Accel X, Y and Z all through the gyro
  // First set the register start address for X on Gyro 
  if(gyroGo){                                      // Wireは割り込みを使っているので、タイマー割り込みの中では使用できない
    gyroGo = false;                                // フラグクリア
    Wire.beginTransmission(GYRO);                  // ここから、センサーから値を読み取る処理
    Wire.send(REG_GYRO_X);                         // Register Address GYRO_XOUT_H
    Wire.endTransmission();
    // New read the 12 data bytes
    Wire.beginTransmission(GYRO);
    Wire.requestFrom(GYRO,12);                     // Read 12 bytes
    int i = 0;
    while(Wire.available() && i < 12)
    {
      buffer[i] = Wire.receive();
      i++;
    }
    Wire.endTransmission();
    //Combine bytes into integers
    // Gyro format is MSB first
    int gyro_z = buffer[4] << 8 | buffer[5];        // Z軸周りの角速度を取得
    gyroBuff[wp] = gyro_z;                          // バッファに退避
    wp = (wp + 1) % BUFF_SIZE;                      // 書き込みアドレスを更新
    // 角度の計算 台形公式を用いた積分を行っている。台形公式を用いた方が誤差の積算が遅い。
    // ちなみに、センサーの物理特性としてヒステリシスが避けられないのでバイアスは避けられない。
    // 定常バイアス値をdouble型にすると、オフセットを取った直後の静止時における安定性は高いものの一度でも動き出すと意味がなくなる。
    // 実質的には、バイアスはint型で全く構わない。途中演算をlong型で行って割り算からdouble型にするとgood。
    angle += (double)((long)ANGLE_RATE_MAX * (long)INTERVAL) * ((double)backup_gyro_z + (double)gyro_z - 2.0 * gyro_bias) / (double)INT_MAX / (double)1000 / 2.0;
    backup_gyro_z = gyro_z;                         // 台形公式のためにバックアップ
    // シリアル出力関係
    if(digitalRead(DIP_UART_ENABLE_PIN) == LOW){    // ディップスイッチをチェック。もしLOWなら出力する。
      Serial.print(gyro_z);                        // 生の角速度センサの出力値を出力
      Serial.print("\t");                          // タブを出力
      Serial.println(angle);                       // 現時点での角度を出力
    }
  }
}

図3.1 サンプルスケッチ


4. 実験と角速度から旋回角を求める方法

実験の様子をYoutubeにアップしてみました。 動画の後半では旋回角を求めるアルゴリズムについて解説しています。


図4.1 実験の様子と計算方法

6. 終わりに

マイコンで旋回角を求める方法が非常に簡単であることが分かって頂けたかと思います。 来年のロボットカーコンテストがより活発になることを祈ります(笑)。

inserted by FC2 system