【Arduino & PmodNAV】加速度とジャイロから、角度を算出するプログラム
前回の記事で、PmodNAVというセンサーを使用して、加速度やジャイロを測定するプログラムを公開しました。
shizenkarasuzon.hatenablog.com
そこで、今回はそれらの加速度やジャイロのデータをもとにして、角度（センサーの傾き）を算出するプログラムを書きます。
使用しているセンサーはPmodNAVです。
Pmod NAV [Reference.Digilentinc]
では、スタート。
1.目標
目標➀センサーの傾きを算出する
そのためには、前回の記事の容量で取得した加速度のデータ「ax, ay, az」と、ジャイロセンサのデータ「gx, gy, gz」を使います。
一口に「センサーの傾き」と言っても、三次元での傾きのことなので、値は３つあります。「ピッチ(pitch)」、「ロール (roll)」、「ヨー (yaw)」です。
この記事では、このうち、「ピッチ(pitch)」、「ロール (roll)」の傾きを Kalman filter を用いて取得します。
目標②ピッチとロール軸まわりの傾きをシリアルモニタに出力する。
目標➀で取得した傾きをシリアルモニタに数値の形でリアルタイム表示します。
2.ハードウェア
前回の記事と同じです。次のピンをつないでください。
Arduino UNO の場合：
PmodNAV<---> Arduino
pin 6 3.3V
pin 5 GND
pin 4 A5
pin 2 A4
Arduino Due の場合
PmodNAV<---> Arduino
pin 6 3.3V
pin 5 GND
pin 4 21（SCLピン）
pin 2 20（SDAピン)
3.ソフトウェア
この記事では、「Kalman filter」と呼ばれるフィルターを用いて傾きを算出しています。なので、「Kalman filter」のライブラリーをダウンロードして、Arduino IDEのライブラリに追加する必要があります。
ライブラリ（Kalman filter) のダウンロード先（GitHub)：
https://github.com/TKJElectronics/KalmanFilter
このサイトからZIPでダウンロードして、Arduino IDE で、「ライブラリをインクルード」してください。
では、プログラムです。
機能：
/************************************************************************** * Description: Pmod_NAV * All data (accelerometer, gyroscope, magnetometer) are displayed * * Wiring * Module<----------> Arduino * pin 6 3.3V * pin 5 GND * pin 4 A5 * pin 2 A4 ************************************************************************/ #include <Wire.h> #include <SparkFunLSM9DS1.h> #include <Kalman.h> double accX, accY, accZ; double gyroX, gyroY, gyroZ; //Filter Angle Kalman kalmanX; Kalman kalmanY; 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 double kalDAngleX, kalDAngleY; uint32_t timer; //Velocity double velX, velY, velZ = 0; double comVelX, comVelY, comVelZ = 0; double comAccX,comAccY,comAccZ=0; double gravityX, gravityY, gravityZ=0; double pitch, roll, yaw = 0; const double alpha = 0.8;//lowpassfilter //Log char report[80]; // Déclaration des adresses du module #define LSM9DS1_M 0x1E #define LSM9DS1_AG 0x6B LSM9DS1 imu; // Creation of the object imu #define PRINT_SPEED 100000 static unsigned long lastPrint = 0; void setup(void){ Serial.begin(115200); // initialization of serial communication imu.settings.device.commInterface = IMU_MODE_I2C; // initialization of the module imu.settings.device.mAddress = LSM9DS1_M; imu.settings.device.agAddress = LSM9DS1_AG; if (!imu.begin()){ Serial.println("Probleme de communication avec le LSM9DS1."); while (1); } accX = imu.ax; accY = imu.ay; accZ = imu.az; gyroX = imu.gx; gyroY = imu.gy; gyroZ = imu.gz; double roll = atan2(accY, accZ) * RAD_TO_DEG; double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; kalmanX.setAngle(roll); // Set starting angle kalmanY.setAngle(pitch); gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch; delay(100);//Wait for sensor to stablize timer = micros(); } void printAttitude(void){ Serial.print(pitch); Serial.print(","); Serial.print(roll); Serial.print(","); Serial.println(yaw); delay(2); } void loop(){ if (imu.gyroAvailable()) imu.readGyro(); // acquisition des données du gyroscope if (imu.accelAvailable()) imu.readAccel(); //Acquisition of accelerometer data if (imu.magAvailable()) imu.readMag(); // Acquisition of the magnetometer accX = imu.ax; accY = imu.ay; accZ = imu.az; gyroX = imu.gx; gyroY = imu.gy; gyroZ = imu.gz; double dt = (double)(micros() - timer) / 1000000; // Calculate delta time timer = micros(); /*角度を求める kalman Filter*/ roll = atan2(accY, accZ) * RAD_TO_DEG; pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG; double gyroXrate = gyroX / 131.0; // Convert to deg/s double gyroYrate = gyroY / 131.0; // Convert to deg/s 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); 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; /*速度を求める*/ // 重力加速度を求める gravityX = alpha * gravityX + (1 - alpha) * accX; gravityY = alpha * gravityY + (1 - alpha) * accY; gravityZ = alpha * gravityZ + (1 - alpha) * accZ; // 補正した加速度 comAccX = accX - gravityX; comAccY = accY - gravityY; comAccZ = accZ - gravityZ; if(abs(comAccX) < 100)comAccX = 0; if(abs(comAccY) < 100)comAccY = 0; if(abs(comAccZ) < 100)comAccZ = 0; velX += comAccX*dt; velY += comAccY*dt; velZ += comAccZ*dt; if ((lastPrint + PRINT_SPEED) < micros()) printAttitude(); }
