MPU-6050を使った慣性航法 (1)
Arduino Nano互換機とMPU-6050を使って、ロボットの位置や姿勢を計算する、推測航法を試してみます。
といっても、卓上を5分程度動くロボットの制御が目的であり、太平洋を横断したり、宇宙ロケットを作りたいわけではないので、地球は丸いとか、自転や公転とか、カルマンフィルタとか、そういう要素を考慮しない、シンプルな推測航法です。
計算は、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)を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」で定義しました。
加速度計のフルスケールレンジを設定するには、「MPU6050」クラスの「setFullScaleAccelRange」メソッドを使います。
角速度計のフルスケールレンジを設定するには、「MPU6050」クラスの「setFullScaleGyroRange」メソッドを使います。
■加速度/角速度のrawデータを読み込む
加速度と角速度の値は、int16_t型の変数で受け取ります。
実際に値を取得するには、「MPU6050」クラスの「getMotion6」メソッドを使います。
加速度センサの検討
プログラムを実行して取得したデータをExcelでグラフにしてみました。
Z軸を上にした状態からはじめて、Z軸を下にした状態、Y軸を上にした状態、Y軸を下にした状態、X軸を上にした状態、X軸を下にした状態へと手で回転させています。

加速度 検出されている加速度は、重力と回転時の加速度などを合成したものです。手で回しているので、大雑把ですが、概ね予想通りの動きを示しています。
重力の影響は、上向きの値として検出されます。加速度センサを「±2g」の検出範囲で使っているので、16384で1gです。

加速度の大きさ 加速度の大きさを計算してみました。理想的には、静止時の加速度は横一列で一致するはずですが、微妙にズレているように見えます。

加速度の大きさ(拡大) 1gを中心に拡大してみます。回すときの加速度と手のブレを考慮しても、向きによって加速度の大きさが明らかに違います。やはり軸ごとにバイアスなどの特性が違いそうです。
角速度センサの検討
次に、角速度センサの特性をみてみます。

角速度 センサの向きを変えるときの回転が検出されています。角速度センサを「±250 (°/s)」の検出範囲で使っているので、131で1°/sです。

角速度の初期状態 最初に回転する前の静止時の角速度だけを取り出すと、バイアスの存在がよくわかります。
コード全体
全体のコードは、以下の通りです。
シリアルの通信速度を「38400」に設定しています。
といっても、卓上を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
今回は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)」の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」クラスの「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);
}
コメント