ArduinoでIMU-3000（ジャイロ）を使用して角速度と旋回角を計測する |
図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のインターフェイスはI2Cのみですので、マイコンとはI2Cポートと接続する必要があります。 Arduinoならば既にAPIが公開されていますので計測は簡単です。
[蛇足]ちなみに、GPSロボットカーコンテスト2011ではメインMCUのH8マイコンに直接接続して制御する時間的余裕がなかったため、Arduinoで角速度を計測させていました。 ArduinoからはPWM波形として観測結果を出力させ、H8マイコン側はインプットキャプチャを利用してデューティ比を観測することで角速度を得ていました。 来年からはセンサ周りはArduinoに処理を任せて、H8マイコン（ARMになるかも）からはI2Cで観測データを得たいと考えています。 云わば、ハードウェアのオブジェクト化（モジュール化）です。 ソフトウェアI2CをArduinoが扱えればいいのですが…利用できない場合はちょっと別の方法を考える必要がありそうです。
時刻管理を利用して回転角度を出力するサンプルスケッチを図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 サンプルスケッチ
実験の様子をYoutubeにアップしてみました。 動画の後半では旋回角を求めるアルゴリズムについて解説しています。
マイコンで旋回角を求める方法が非常に簡単であることが分かって頂けたかと思います。 来年のロボットカーコンテストがより活発になることを祈ります（笑）。