MPU-6050を使った慣性航法 (1)
Arduino Nano互換機とMPU-6050を使って、ロボットの位置や姿勢を計算する、推測航法を試してみます。
といっても、卓上を５分程度動くロボットの制御が目的であり、太平洋を横断したり、宇宙ロケットを作りたいわけではないので、地球は丸いとか、自転や公転とか、カルマンフィルタとか、そういう要素を考慮しない、シンプルな推測航法です。
計算は、Arduino Nano互換機で行なうことにします。
MPU-6050の制御には、Jeff Rowberg氏のMPU-6050用ライブラリを使いますが、単純な加速度と角速度のみを取得し、割り込みも使いません。
加速度と角速度のみを取得して、シリアルに出力するプログラム
まず、MPU-6050の素性を知るために、基本的な加速度と角速度のみを取得して、シリアルに出力するプログラムを作ってみます。
■インクルードファイル
「MPU6050」は「I2Cdev」を使うので、「I2Cdev.h」をインクルードします。
今回はDMP(MotionApps)を使わないので、「MPU6050.h」をインクルードします。
■Full-Scale RangeとSensitivity Scale Factorの選択
MPU-6050の加速度センサと角速度センサは、検出範囲（Full-Scale Range）を４段階から選択できます。
加速度センサの検出範囲は「±2 / ±4 / ±8 / ±16 (g)」の４段階、角速度センサの検出範囲は「±250 / ±500 / ±1000 / ±2000 (°/s)」の４段階です。検出範囲が広いほど感度（Sensitivity Scale Factor）が低下します。
Jeff Rowberg氏のライブラリでは、加速度センサの設定は「MPU6050_ACCEL_FS_2」「MPU6050_ACCEL_FS_4」「MPU6050_ACCEL_FS_8」「MPU6050_ACCEL_FS_16」、角速度センサの設定は「MPU6050_GYRO_FS_250」「MPU6050_GYRO_FS_500」「MPU6050_GYRO_FS_1000」「MPU6050_GYRO_FS_2000」で選択できます。
ここでは、プログラムで扱いやすいように、加速度の検出範囲を「ACCEL_FSR」、感度を「ACCEL_SSF」、角速度の検出範囲を「GYRO_FSR」、感度を「GYRO_SSF」で定義しました。
加速度計のフルスケールレンジを設定するには、「MPU6050」クラスの「setFullScaleAccelRange」メソッドを使います。
角速度計のフルスケールレンジを設定するには、「MPU6050」クラスの「setFullScaleGyroRange」メソッドを使います。
■加速度/角速度のrawデータを読み込む
加速度と角速度の値は、int16_t型の変数で受け取ります。
実際に値を取得するには、「MPU6050」クラスの「getMotion6」メソッドを使います。
加速度センサの検討
プログラムを実行して取得したデータをExcelでグラフにしてみました。
Z軸を上にした状態からはじめて、Z軸を下にした状態、Y軸を上にした状態、Y軸を下にした状態、X軸を上にした状態、X軸を下にした状態へと手で回転させています。
加速度
重力の影響は、上向きの値として検出されます。加速度センサを「±2g」の検出範囲で使っているので、16384で1gです。
加速度の大きさ
加速度の大きさ（拡大）
角速度センサの検討
次に、角速度センサの特性をみてみます。
角速度
角速度の初期状態
コード全体
全体のコードは、以下の通りです。
シリアルの通信速度を「38400」に設定しています。
といっても、卓上を５分程度動くロボットの制御が目的であり、太平洋を横断したり、宇宙ロケットを作りたいわけではないので、地球は丸いとか、自転や公転とか、カルマンフィルタとか、そういう要素を考慮しない、シンプルな推測航法です。
計算は、Arduino Nano互換機で行なうことにします。
MPU-6050の制御には、Jeff Rowberg氏のMPU-6050用ライブラリを使いますが、単純な加速度と角速度のみを取得し、割り込みも使いません。
加速度と角速度のみを取得して、シリアルに出力するプログラム
まず、MPU-6050の素性を知るために、基本的な加速度と角速度のみを取得して、シリアルに出力するプログラムを作ってみます。
■インクルードファイル
#include "I2Cdev.h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif今回はDMP(MotionApps)を使わないので、「MPU6050.h」をインクルードします。
■Full-Scale RangeとSensitivity Scale Factorの選択
//■加速度 Full-Scale Range
#define ACCEL_FSR MPU6050_ACCEL_FS_2
//#define ACCEL_FSR MPU6050_ACCEL_FS_4
//#define ACCEL_FSR MPU6050_ACCEL_FS_8
//#define ACCEL_FSR MPU6050_ACCEL_FS_16
//■加速度 Sensitivity Scale Factor
#define ACCEL_SSF 16384.0
//#define ACCEL_SSF 8192.0
//#define ACCEL_SSF 4096.0
//#define ACCEL_SSF 2048.0
//■角速度 Full-Scale Range
#define GYRO_FSR MPU6050_GYRO_FS_250
//#define GYRO_FSR MPU6050_GYRO_FS_500
//#define GYRO_FSR MPU6050_GYRO_FS_1000
//#define GYRO_FSR MPU6050_GYRO_FS_2000
//■角速度 Sensitivity Scale Factor
#define GYRO_SSF 131.0
//#define GYRO_SSF 65.5
//#define GYRO_SSF 32.8
//#define GYRO_SSF 16.4加速度センサの検出範囲は「±2 / ±4 / ±8 / ±16 (g)」の４段階、角速度センサの検出範囲は「±250 / ±500 / ±1000 / ±2000 (°/s)」の４段階です。検出範囲が広いほど感度（Sensitivity Scale Factor）が低下します。
Jeff Rowberg氏のライブラリでは、加速度センサの設定は「MPU6050_ACCEL_FS_2」「MPU6050_ACCEL_FS_4」「MPU6050_ACCEL_FS_8」「MPU6050_ACCEL_FS_16」、角速度センサの設定は「MPU6050_GYRO_FS_250」「MPU6050_GYRO_FS_500」「MPU6050_GYRO_FS_1000」「MPU6050_GYRO_FS_2000」で選択できます。
ここでは、プログラムで扱いやすいように、加速度の検出範囲を「ACCEL_FSR」、感度を「ACCEL_SSF」、角速度の検出範囲を「GYRO_FSR」、感度を「GYRO_SSF」で定義しました。
//■加速度計のフルスケールレンジを設定
mpu.setFullScaleAccelRange(ACCEL_FSR);
//■角速度計のフルスケールレンジを設定
mpu.setFullScaleGyroRange(GYRO_FSR);角速度計のフルスケールレンジを設定するには、「MPU6050」クラスの「setFullScaleGyroRange」メソッドを使います。
■加速度/角速度のrawデータを読み込む
int16_t mpu_accel_x, mpu_accel_y, mpu_accel_z; //加速度(raw)
int16_t mpu_gyro_x, mpu_gyro_y, mpu_gyro_z; //角速度(raw) mpu.getMotion6(&mpu_accel_x, &mpu_accel_y, &mpu_accel_z,
&mpu_gyro_x, &mpu_gyro_y, &mpu_gyro_z);加速度センサの検討
プログラムを実行して取得したデータをExcelでグラフにしてみました。
Z軸を上にした状態からはじめて、Z軸を下にした状態、Y軸を上にした状態、Y軸を下にした状態、X軸を上にした状態、X軸を下にした状態へと手で回転させています。
重力の影響は、上向きの値として検出されます。加速度センサを「±2g」の検出範囲で使っているので、16384で1gです。
角速度センサの検討
次に、角速度センサの特性をみてみます。
コード全体
全体のコードは、以下の通りです。
シリアルの通信速度を「38400」に設定しています。
//MPU6050からRAWデータを取得
//■インクルードファイル
#include "I2Cdev.h"
#include "MPU6050.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
//■加速度 Full-Scale Range
#define ACCEL_FSR MPU6050_ACCEL_FS_2
//#define ACCEL_FSR MPU6050_ACCEL_FS_4
//#define ACCEL_FSR MPU6050_ACCEL_FS_8
//#define ACCEL_FSR MPU6050_ACCEL_FS_16
//■加速度 Sensitivity Scale Factor
#define ACCEL_SSF 16384.0
//#define ACCEL_SSF 8192.0
//#define ACCEL_SSF 4096.0
//#define ACCEL_SSF 2048.0
//■角速度 Full-Scale Range
#define GYRO_FSR MPU6050_GYRO_FS_250
//#define GYRO_FSR MPU6050_GYRO_FS_500
//#define GYRO_FSR MPU6050_GYRO_FS_1000
//#define GYRO_FSR MPU6050_GYRO_FS_2000
//■角速度 Sensitivity Scale Factor
#define GYRO_SSF 131.0
//#define GYRO_SSF 65.5
//#define GYRO_SSF 32.8
//#define GYRO_SSF 16.4
//■「MPU6050」クラスのオブジェクト作成
MPU6050 mpu;
//MPU6050 mpu(0x69);
//■機体座標系
int16_t mpu_accel_x, mpu_accel_y, mpu_accel_z; //加速度(raw)
int16_t mpu_gyro_x, mpu_gyro_y, mpu_gyro_z; //角速度(raw)
void setup() {
//■I2C処理開始
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
//■シリアル通信を開始
Serial.begin(38400);
while (!Serial);
//■MPU-6050デバイスの初期化
mpu.initialize();
//■MPU-6050デバイスの接続を確認
if(mpu.testConnection() != true) {
Serial.println(F("ERROR testConnection"));
return; // 接続失敗
}
//■加速度計のフルスケールレンジを設定
mpu.setFullScaleAccelRange(ACCEL_FSR);
//■角速度計のフルスケールレンジを設定
mpu.setFullScaleGyroRange(GYRO_FSR);
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
void loop() {
//■加速度/角速度のrawデータを読み込む
mpu.getMotion6(&mpu_accel_x, &mpu_accel_y, &mpu_accel_z,
&mpu_gyro_x, &mpu_gyro_y, &mpu_gyro_z);
Serial.print(micros()); Serial.print(",");
Serial.print(mpu_accel_x); Serial.print(",");
Serial.print(mpu_accel_y); Serial.print(",");
Serial.print(mpu_accel_z); Serial.print(",");
Serial.print(mpu_gyro_x); Serial.print(",");
Serial.print(mpu_gyro_y); Serial.print(",");
Serial.println(mpu_gyro_z);
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
コメント