fc2ブログ

MPU-6050を使った慣性航法 (1)

Arduino Nano互換機とMPU-6050を使って、ロボットの位置や姿勢を計算する、推測航法を試してみます。
といっても、卓上を5分程度動くロボットの制御が目的であり、太平洋を横断したり、宇宙ロケットを作りたいわけではないので、地球は丸いとか、自転や公転とか、カルマンフィルタとか、そういう要素を考慮しない、シンプルな推測航法です。

計算は、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
「MPU6050」は「I2Cdev」を使うので、「I2Cdev.h」をインクルードします。
今回は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
MPU-6050の加速度センサと角速度センサは、検出範囲(Full-Scale Range)を4段階から選択できます。
加速度センサの検出範囲は「±2 / ±4 / ±8 / ±16 (g)」の4段階、角速度センサの検出範囲は「±250 / ±500 / ±1000 / ±2000 (°/s)」の4段階です。検出範囲が広いほど感度(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」クラスの「setFullScaleAccelRange」メソッドを使います。
角速度計のフルスケールレンジを設定するには、「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)
加速度と角速度の値は、int16_t型の変数で受け取ります。
  mpu.getMotion6(&mpu_accel_x, &mpu_accel_y, &mpu_accel_z,
&mpu_gyro_x, &mpu_gyro_y, &mpu_gyro_z);
実際に値を取得するには、「MPU6050」クラスの「getMotion6」メソッドを使います。

加速度センサの検討
プログラムを実行して取得したデータをExcelでグラフにしてみました。

Z軸を上にした状態からはじめて、Z軸を下にした状態、Y軸を上にした状態、Y軸を下にした状態、X軸を上にした状態、X軸を下にした状態へと手で回転させています。
加速度
加速度
検出されている加速度は、重力と回転時の加速度などを合成したものです。手で回しているので、大雑把ですが、概ね予想通りの動きを示しています。
重力の影響は、上向きの値として検出されます。加速度センサを「±2g」の検出範囲で使っているので、16384で1gです。
加速度の大きさ
加速度の大きさ
加速度の大きさを計算してみました。理想的には、静止時の加速度は横一列で一致するはずですが、微妙にズレているように見えます。
加速度の拡大図
加速度の大きさ(拡大)
1gを中心に拡大してみます。回すときの加速度と手のブレを考慮しても、向きによって加速度の大きさが明らかに違います。やはり軸ごとにバイアスなどの特性が違いそうです。

角速度センサの検討
次に、角速度センサの特性をみてみます。
角速度
角速度
センサの向きを変えるときの回転が検出されています。角速度センサを「±250 (°/s)」の検出範囲で使っているので、131で1°/sです。
静止時の角速度
角速度の初期状態
最初に回転する前の静止時の角速度だけを取り出すと、バイアスの存在がよくわかります。

コード全体
全体のコードは、以下の通りです。
シリアルの通信速度を「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);
}

コメント

非公開コメント