Table of Contents
何を学習するの?
- ArduinodでGY-521(3軸加速度、角速度センサ)の測定値からGY-521の傾斜角度を計算する方法を学習します。
- Arduinodを使ってGY-521(3軸加速度、角速度センサ)の信号を取得し表示させる方法とGY-521に内蔵されているMPU-6050のフィルター設定などを学習しました。傾斜治具にGY-521を取り付けて、加速度と角速度を測定しGY-521の傾斜角度を計算します。
GY-521の仕様や操作方法の記事は?
- GY-521を傾斜運動させる治具を作成しました。この治具にGY-521を取り付けてセンサからの出力を計測します。
GY-521を傾斜運動させる治具の記事は?
- GY-521はArduino UNO R3のスタータキットに含まれていました。
Arduino UNO R3とGY-521を購入するには?
- Amazon — [Arduino用のUNO R3 最終版スタータキット] — Arduino UNO R3とGY-521が含まれています
GY-521を傾斜治具に取り付けて加速度と角速度から傾斜角度を計算します
- GY-521を作成した傾斜治具に取り付けて傾斜運動させます。測定した加速度と角速度を使って傾斜角度を計算します。
- 測定のために連続回転サーボモーター(SG90-HV)を一定速度で回転させます。電源を抜くときに回転しているのは嫌なので、10分回転して、10秒止まるる動作を繰り返します。
- loop()中のwriteMicroseconds()関数の設定値を例えば、1600、1650、1700などに変えて回転数を変化させます。
//連続回転サーボモーター(SG90-HV)を一定速度で回転させる
#include <Servo.h>
Servo myservo;
void setup(){
myservo.attach(9);
myservo.writeMicroseconds(1500);// move speed to 0
delay(5000);
}
void loop(){
myservo.writeMicroseconds(1650);// move speed to setting speed
delay(600000UL);
myservo.writeMicroseconds(1500);// move speed to 0
delay(10000);
}GY-521の加速度から傾斜角度を計算します
- GY-521の加速度計で重力加速度が計測できます。重力加速度はGY-521の傾きによってX軸、Y軸、Z軸でそれぞれ計測されます。
- X軸を回転軸としてGY-521を回転する場合は、GY-521のY軸とZ軸の測定値が変化します。
- DLPF=6で出力しています
// GY-521の加速度の計測値を表示する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t AcX,AcY,AcZ;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Serial.print(AcX);Serial.print(",");
Serial.print(AcY); Serial.print(",");
Serial.println(AcZ);
}- GY-521がX軸を中心に傾くと、重力加速度はZ軸にgz=gcos(θ)、Y軸にgy=gsin(θ)に分かれて計測されます。X軸は回転の中心なので加速度の変化はありません。
- [GY-521を傾斜運動させる治具]を使って、GY-521を傾斜運動させています。傾斜運動の角度が小さいため、Z軸の変動は大きくありません。Y軸の計測値に加速度の変化が現れています。
- DLPF=6で出力しています
// GY-521の加速度の測定値を表示する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t AcX,AcY,AcZ;
double GY521Angle;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
GY521Angle = atan2(AcZ,AcY)*180/3.1415;
Serial.println(GY521Angle);
}- 傾斜角を計算するには計測した、AcY(Y軸方向の加速度)、AcZ(Z軸方向の加速度)の値を使います。
- 計算した角度をシリアルプロッタで出力しました。(縦軸は傾斜角度、横軸は時間です。)
- 傾斜角は、Z軸方向を0度として、84度から94度程度まで振れています。[GY-521を傾斜運動させる治具]は+/-5度程度の傾斜を想定していたので妥当な数値です。
- 傾斜の平均は90度からズレています。[GY-521を傾斜運動させる治具]のカムの高さの調整が必要です。やや手間がかかります。(-_-;)
- 回転動作をすることは、回転方向に加速度が発生していることになります。この加速度により計算した傾斜角に誤差が発生すると言われています。
GY-521の角速度から傾斜角度を計算します
- GY-521 は角速度計を内蔵しています。角速度と経過時間が解れば、その積が角度になります。
- DLPF=6で出力しています
// GY-521の角速度の計測値を表示する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t GyX,GyY,GyZ;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x43); // starting with register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print(GyX); Serial.print(",");
Serial.print(GyY); Serial.print(",");
Serial.println(GyZ);
}- 静止状態のGY-521からの角速度の出力は’0’であるはずですが、実際に測定するとオフセットが発生しています。角度は測定値と時間の積で求めるので、オフセットを差し引いて計算しないと正しい角度が計算できません。オフセットの値と変動を測定します。
- GY-521を静止状態にして、角速度を1000回測定し平均を値を計測します。
- DLPF=6で出力しています
// GY-521の角速度計のオフセットを測定する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t GyX,GyY,GyZ;
double GyX0,GyY0,GyZ0;
int i;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
}
void loop(){
i=0;
GyX0=0;
GyY0=0;
GyZ0=0;
while (i<1000){
Wire.beginTransmission(MPU_addr);
Wire.write(0x43); // starting with register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX0=GyX0+GyX;
GyY0=GyY0+GyY;
GyZ0=GyZ0+GyZ;
i=i+1;
}
Serial.print(GyX0/1000); Serial.print(",");
Serial.print(GyY0/1000); Serial.print(",");
Serial.println(GyZ0/1000);
}- 角速度のオフセットをX軸、Y軸、Z軸の順番でシリアルモニタに表示させています。
- 測定した角速度のオフセットの値を実測値から引いて、原点’0’中心で角速度がどのように変動しているか計測します。
- 計測した結果から、X軸のオフセット、614、Y軸のオフセット、-197、Z軸のオフセット、-236とします。
- DLPF=6で出力しています
// GY-521の角速度の計測値をオフセットを引いて表示する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t GyX,GyY,GyZ;
int16_t GyXOffset,GyYOffset,GyZOffset;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
//角速度のオフセットを設定する
GyXOffset = -614;
GyYOffset = 197;
GyZOffset = 236;
}
void loop(){
Wire.beginTransmission(MPU_addr);
Wire.write(0x43); // starting with register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=GyX+GyXOffset;
GyY=GyY+GyYOffset;
GyZ=GyZ+GyZOffset;
Serial.print(GyX); Serial.print(",");
Serial.print(GyY); Serial.print(",");
Serial.println(GyZ);
}- 角速度の計測値は、’0’中心にまとまっていますが、バラツキは+/-5以上あります。角速度のスケールはフルレンジで、+/- 250°の設定になっており(デフォルトの設定) 測定値を角度に換算するには、131で割ります。例えば計測値が6.55の時に、角度は0.05°です。
- それぞれの角速度測定値の平均値が’0’であれば問題ないですが、長時間測定すると平均値が’0’から離れて変動します。角度を角速度と時間の積で計算する場合は、角速度の初期値からの変動に起因する真値からのずれが生じ、ドリフトと呼ばれているようです。
- GY-521の角速度から角度を計算するとドリフトを含めてどのような出力になるか計測します。
DLPF=6で出力しています
// GY-521の角速度の計測値をつかってX軸の角度を計算する
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t GyX,GyY,GyZ;
double GyXOffset,GyYOffset,GyZOffset;
// Timers
unsigned long timer = 0;
float timeStep = 0.01;
// Pitch, Roll and Yaw values
float pitch = 0;
float roll = 0;
float yaw = 0;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(9600);
//Wakeup MPU-6050
Wire.beginTransmission(MPU_addr);
Wire.write(WR_MGMT_1_addr); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
//MPU-6050のレジスタ(27)を'0b00011000'に設定し測定レンジを設定する
MPU6050_WriteRegister_Byte (MPU_addr, 27 ,0b00000000);
//角速度のオフセットを設定する
GyXOffset = -614;
GyYOffset = 197;
GyZOffset = 236;
}
void loop(){
timer = millis();
Wire.beginTransmission(MPU_addr);
Wire.write(0x43); // starting with register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=GyX+GyXOffset;
GyY=GyY+GyYOffset;
GyZ=GyZ+GyZOffset;
// Calculate Roll
roll = roll + GyX * timeStep;
//角度に変換して表示する
Serial.println(roll/131);
// Wait to full timeStep period
delay((timeStep * 1000) - (millis() - timer));
}- X軸の角速度から角度を計算しています。測定間隔は、10msecとしています。
- 角度は、SIN波のように変動しており、幅は10度程度です。[GY-521を傾斜運動させる治具]は+/-5度程度の傾斜を想定していたので妥当な数値です。
- 角度の計算値が右に傾いています。ドリフトが発生しているのが解ります。
傾斜角度をカルマンフィルタを使って計算します
- GY-521の加速度と角速度の計測値からを角速度を計算しました。角速度から角度を計算するとドリフトが発生し、加速度から角度を計算すると誤差が生じています。
- インターネットを検索すると、角度を計算する方法として、加速度、角速度、また得られた角度を変数としてカルマンフィルターや相補フィルタを使う方法等の記事が多く掲載されています。
- Arduino IDEのライブラリ管理から検索すると、カルマンフィルタのライブラリが何種類か見つかります。理論はともかく(-_-;)、既存のライブラリを使って角度を計算させてみます。
- ライブラリのコメントに加速度計、ジャイロスコープの記載があったので、Kalman Filter Libraryを選びました。
- Kalman Filter Libraryをインストールするとスケッチの例も一緒にインストールされます。
- Arduino IDEの[ファイル] -> [スケッチ例] -> [Kalman Filter Library] -> [MPU6050]を選ぶと、Arduino IDEにスケッチが読み込まれます。Arduino UNO R3にGY-521が接続されていれば、このスケッチを書き込むとシリアルモニタ、シリアルプロッタで測定値が確認できます。
- スケッチの通信速度が115200bpsとなっています。手動でArduino IDEの通信速度を115200bpsにします。
- [GY-521を傾斜運動させる治具]にGY-521を取り付けて傾斜運動させながら計測します。
- X軸、Y軸共に4個の計算値が出力されています。
| roll, pitch | 加速度から算出した、roll(横揺れ)およびpitch(縦揺れ) |
| gyroXangle, gyroYangle | 角速度から算出したX角度およびY角度 |
| compAngleX, compAngleY | 補完フィルタを使って算出したX角度およびY角度 |
| kalAngleX, kalAngleY | カルマンフィルタを使って算出したX角度およびY角度 |
Serial.print(roll);
Serial.print(gyroXangle);
Serial.print(compAngleX);
Serial.print(kalAngleX);
Serial.print(pitch);
Serial.print(gyroYangle);
Serial.print(compAngleY);
Serial.print(kalAngleY); - シリアルモニタの出力
- シリアルプロッタの出力
- MPU6050.ino
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
#include <Wire.h>
#include <Kalman.h> // Source: https://github.com/TKJElectronics/KalmanFilter
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
double gyroXangle, gyroYangle; // Angle calculate using the gyro only
double compAngleX, compAngleY; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Make calibration routine
void setup() {
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
/* Update all the values */
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// Source: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf eq. 25 and eq. 26
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// It is then converted from radians to degrees
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
Serial.print(roll); Serial.print("\t");
Serial.print(gyroXangle); Serial.print("\t");
Serial.print(compAngleX); Serial.print("\t");
Serial.print(kalAngleX); Serial.print("\t");
Serial.print("\t");
Serial.print(pitch); Serial.print("\t");
Serial.print(gyroYangle); Serial.print("\t");
Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleY); Serial.print("\t");
#if 0 // Set to 1 to print the temperature
Serial.print("\t");
double temperature = (double)tempRaw / 340.0 + 36.53;
Serial.print(temperature); Serial.print("\t");
#endif
Serial.print("\r\n");
delay(2);
}- I2C.ino
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
This software may be distributed and modified under the terms of the GNU
General Public License version 2 (GPL2) as published by the Free Software
Foundation and appearing in the file GPL2.TXT included in the packaging of
this file. Please note that GPL2 Section 2[b] requires that all works based
on this software must also be made publicly available under the terms of
the GPL2 ("Copyleft").
Contact information
-------------------
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB
const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication
uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {
return i2cWrite(registerAddress, &data, 1, sendStop); // Returns 0 on success
}
uint8_t i2cWrite(uint8_t registerAddress, uint8_t *data, uint8_t length, bool sendStop) {
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data, length);
uint8_t rcode = Wire.endTransmission(sendStop); // Returns 0 on success
if (rcode) {
Serial.print(F("i2cWrite failed: "));
Serial.println(rcode);
}
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
uint8_t i2cRead(uint8_t registerAddress, uint8_t *data, uint8_t nbytes) {
uint32_t timeOutTimer;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
uint8_t rcode = Wire.endTransmission(false); // Don't release the bus
if (rcode) {
Serial.print(F("i2cRead failed: "));
Serial.println(rcode);
return rcode; // See: http://arduino.cc/en/Reference/WireEndTransmission
}
Wire.requestFrom(IMUAddress, nbytes, (uint8_t)true); // Send a repeated start and then release the bus after reading
for (uint8_t i = 0; i < nbytes; i++) {
if (Wire.available())
data[i] = Wire.read();
else {
timeOutTimer = micros();
while (((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());
if (Wire.available())
data[i] = Wire.read();
else {
Serial.println(F("i2cRead timeout"));
return 5; // This error value is not already taken by endTransmission
}
}
}
return 0; // Success
}- スケッチ例(MPU6050.ino, I2C.ino)の出力から、gyroXangle(角速度から角度を計算)とkalAngleX(カルマンフィルター補正)のみ表示させ比較すると、カルマンフィルタによりドリフトが補正されていることが解ります。
- カルマンフィルターはArduinoで使うのであればライブラリがあるので難しくはない感じです。カルマンフィルタを理解するにはもっと努力が必要ですが (-_-;)
傾斜角度を相補フィルタを使って計算します
- 相補フィルタも角度計算に良く使われているようです。カルマンフィルタよりもスケッチの中に組み込みやすそうでしたので実際にスケッチを書いて、どのような結果になるかを確認します。ピッチ軸の角度計算を以下の条件で行います。
- 角速度 Gyro = (GyX,GyY,GyZ)を測定する
- 加速度 Acc = (AcX,AcY,AcZ)を測定する
- 角速度によるピッチ角度変化計算値 Δθg = Gyx × Δt (Δtは測定時間間隔)
- 加速度によるピッチ角度計算値 θa = atan2(AcX, AcZ)
- 相補フィルタによるピッチ角計算値 θ(n+1) = k×(θ(n)+Δθg) + (1-k)×θa
// 傾斜角度を相補フィルタで角速度の測定値と加速度の測定値を使って計算します
#include<Wire.h>
const int MPU_addr=0x68; // I2C address of the MPU-6050
const int WR_MGMT_1_addr=0x6B; // MPU-6050 PWR_MGMT_1 register
int ReadReg=0x1C; // Register in MPU-6050
uint8_t ReadRegValue; // Register Value
int16_t GyX,GyY,GyZ;
double GyXOffset,GyYOffset,GyZOffset;
int16_t AcX,AcY,AcZ;
double GY521Angle;
// Timers
unsigned long timer = 0;
float timeStep = 0.01;
// Pitch, Roll and Yaw values
float Pitch = 90;
float Roll = 0;
float Yaw = 0;
float DeltaPitch =0;
float NewPitch =90;
float RawPitch = 90;
// Furntion to write a byte to a register
uint8_t MPU6050_WriteRegister_Byte (int MPUAdd, int Reg, uint8_t Value){
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // Register for writing
Wire.write(Value); // Write value
Wire.endTransmission(true);
}
// Furntion to read a byte from a register
uint8_t MPU6050_ReadRegister_Byte (int MPUAdd, int Reg){
uint8_t Value;
Wire.beginTransmission(MPUAdd);
Wire.write(Reg); // starting with register ReadReg
Wire.endTransmission(true);
Wire.beginTransmission(MPUAdd);
Wire.requestFrom(MPUAdd,1,true); // request a total of 1 register
Value=Wire.read(); // Value of ReadReg
Wire.endTransmission(true);
return Value;
}
void setup(){
Wire.begin();
Serial.begin(38400);
//Wakeup MPU-6050
MPU6050_WriteRegister_Byte (MPU_addr,WR_MGMT_1_addr,0);
//MPU-6050のレジスタ(26)を'6'に設定しDLPFの設定を変更する
MPU6050_WriteRegister_Byte (MPU_addr, 26 ,6);
//MPU-6050のレジスタ(27)を'0b00011000'に設定し測定レンジを設定する
MPU6050_WriteRegister_Byte (MPU_addr, 27 ,0b00000000);
//角速度のオフセットを設定する
GyXOffset = -614;
GyYOffset = 197;
GyZOffset = 236;
}
void loop(){
timer = millis();
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
GY521Angle = atan2(AcZ,AcY)*180/3.1415;
Wire.beginTransmission(MPU_addr);
Wire.write(0x43); // starting with register 0x43 (GYRO_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
GyX=GyX+GyXOffset;
GyY=GyY+GyYOffset;
GyZ=GyZ+GyZOffset;
// Calculate Pitch
DeltaPitch = GyX * timeStep/131;
RawPitch = RawPitch + DeltaPitch;
NewPitch = 0.995*(Pitch + DeltaPitch) + 0.005 * GY521Angle;
//角速度からの計算値と相補フィルタの出力を表示する
Serial.print(RawPitch);
Serial.print(",");
Serial.println(NewPitch);
Pitch = NewPitch;
// Wait to full timeStep period
delay((timeStep * 1000) - (millis() - timer));
}
- 相補フィルタの補正値もドリフトが発生していません。GY-521を使った電子工作を作る場合は、スケッチへの導入はカルマンフィルタよりも簡単ではないかと感じます。
- カルマンフィルタや相補フィルタの動作を学習するのに[Kalman filter vs Complementary filter]に比較の記事がありました。参考にさせて頂きました。
ご質問、誤植の指摘などありましたら。「問い合わせ 」のページからお願いします。
