【SPRESENSE】BMI160用のセンサーフュージョン・フィルターを検討する(1) [ロボット]
しばらく Python で遊んでいましたが、倒立振子も忘れたわけではありません。今回は、BMI160用のフィルターを検討したいと思います。
今回、試してみるフィルターは次の3つです。
・ Complementary Filter
・ Kalman Filter
・ Madgwick Filter
それぞれに対して簡単に紹介します。
■ Complementary Filter
Complementary Filter は、ジャイロデータの積分値と加速度データで割り出したロールとピッチをある比(0.93:0.02)で足したものを出力とするものです。非常に単純で、応答速度が早いですが、ノイズも大きいのが欠点です。英語ですがこちらの記事が参考になります。
Reading a IMU Without Kalman: The Complementary Filter
■ Kalman Filter
Kalman Filter は、もう説明が必要のないくらい有名なフィルターです。Arduino用のライブラリがありますので、こちらをダウンロードして使いました。
Kalman Filter Library
カルマンフィルターの詳細を知りたい方はこちらの記事が参考になります。
カルマンフィルタの考え方
■ Madgwick Filter
Madgwick Filter は、少しマイナーですがカルマンフィルター同様に自己位置推定によく使われるフィルターです。カルマンフィルターより応答が早いのが特長ですが、収束が遅いと言われています。Madgwick Filter も Arduino ライブラリがあるのでこちらを使用しました。
Madgwick
Madgwick フィルターの詳細を知りたい方はこちらの記事が参考になると思います。
Madgwick Filterを読んでみた
■ Madgwick Filter を改造する
使用した Madgwick Filter はデータのサンプリング周波数が固定値でしか与えられません。時間保証ができるのであればよいのですが、SPRESENSEはOS上で走っているので若干のジッターは避けられません。ですので、サンプリング間隔を計算の度に与えられる関数を追加しました。
■ SPRESENSE用スケッチを書く
それぞれのフィルターがどのように振る舞うか調べるために次のスケッチを書いてみました。
■ 出力データを確認する
ロール方向、ピッチ方向にそれぞれ筐体を振ってみてデータを収集しました。
それぞれのデータはこちらです。
どのフィルタもジャイロのドリフトはキャンセルしてくれているのは分かりますが、グラフだけではどれが良いのかピンと来ませんね。もう少しうまく評価できる方法を考えてみたいと思います。🤨
(-ω-;)
今回、試してみるフィルターは次の3つです。
・ Complementary Filter
・ Kalman Filter
・ Madgwick Filter
それぞれに対して簡単に紹介します。
■ Complementary Filter
Complementary Filter は、ジャイロデータの積分値と加速度データで割り出したロールとピッチをある比(0.93:0.02)で足したものを出力とするものです。非常に単純で、応答速度が早いですが、ノイズも大きいのが欠点です。英語ですがこちらの記事が参考になります。
Reading a IMU Without Kalman: The Complementary Filter
■ Kalman Filter
Kalman Filter は、もう説明が必要のないくらい有名なフィルターです。Arduino用のライブラリがありますので、こちらをダウンロードして使いました。
Kalman Filter Library
カルマンフィルターの詳細を知りたい方はこちらの記事が参考になります。
カルマンフィルタの考え方
■ Madgwick Filter
Madgwick Filter は、少しマイナーですがカルマンフィルター同様に自己位置推定によく使われるフィルターです。カルマンフィルターより応答が早いのが特長ですが、収束が遅いと言われています。Madgwick Filter も Arduino ライブラリがあるのでこちらを使用しました。
Madgwick
Madgwick フィルターの詳細を知りたい方はこちらの記事が参考になると思います。
Madgwick Filterを読んでみた
■ Madgwick Filter を改造する
使用した Madgwick Filter はデータのサンプリング周波数が固定値でしか与えられません。時間保証ができるのであればよいのですが、SPRESENSEはOS上で走っているので若干のジッターは避けられません。ですので、サンプリング間隔を計算の度に与えられる関数を追加しました。
// MadgwickAHRS.h に追加 void updateIMU2(float gx, float gy, float gz, float ax, float ay, float az, double dt);
// MadgwickAHRS.cpp に追加 void Madgwick::updateIMU2( float gx, float gy, float gz, float ax, float ay, float az, double dt) { invSampleFreq = 1.0f / (1.0f / dt); updateIMU(gx, gy, gz, ax, ay, az); }
■ SPRESENSE用スケッチを書く
それぞれのフィルターがどのように振る舞うか調べるために次のスケッチを書いてみました。
#include <Wire.h> #include <math.h> #include <BMI160Gen.h> #include <Kalman.h> #include <MadgwickAHRS.h> #define PRINT_ROLL // #define PRINT_PITCH #define BAUDRATE 115200 #define SENSE_RATE 100 #define GYRO_RANGE 250 #define ACCL_RANGE 2 #define deg_to_rad(a) (a/180*M_PI) #define rad_to_deg(a) (a/M_PI*180) Kalman kalmanRoll; Kalman kalmanPitch; Madgwick madgwick; float convertRawGyro(int gRaw) { // ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s) float lsb_omega = float(0x7FFF) / GYRO_RANGE; return gRaw / lsb_omega; // deg/sec } float convertRawAccel(int aRaw) { // ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g float lsb_g = float(0x7FFF) / ACCL_RANGE; return aRaw / lsb_g; } static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0; static float comp_roll = 0, comp_pitch = 0; static unsigned long last_mills = 0; void print_roll_pitch() { // read raw accl measurements from device int rawXAcc, rawYAcc, rawZAcc; // x, y, z BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc); float accX = convertRawAccel(rawXAcc); float accY = convertRawAccel(rawYAcc); float accZ = convertRawAccel(rawZAcc); float rad_a_roll = atan2(accY, accZ); float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ)); float accl_roll = rad_to_deg(rad_a_roll); float accl_pitch = rad_to_deg(rad_a_pitch); // read raw gyro measurements from device int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw BMI160.readGyro(rawRoll, rawPitch, rawYaw); float omega_roll = convertRawGyro(rawRoll); float omega_pitch = convertRawGyro(rawPitch); float omega_yaw = convertRawGyro(rawYaw); unsigned long cur_mills = micros(); unsigned long duration = cur_mills - last_mills; last_mills = cur_mills; double dt = duration / 1000000.0; // us->s if (dt > 0.1) return; // Gyro data gyro_roll += omega_roll * dt; // (ms->s) omega x time = degree gyro_pitch += omega_pitch * dt; gyro_yaw += omega_yaw * dt; // Complementary filter data comp_roll = 0.93 * (comp_roll + omega_roll * dt) + 0.07 * accl_roll; comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch; // Kalman filter data float kalm_roll = kalmanRoll.getAngle(accl_roll, omega_roll, dt); float kalm_pitch = kalmanPitch.getAngle(accl_pitch, omega_pitch, dt); // Madgwick filter data madgwick.updateIMU2(omega_roll, omega_pitch, omega_yaw, accX, accY, accZ, dt); float madw_roll = madgwick.getRoll(); float madw_pitch = madgwick.getPitch(); float madw_yaw = madgwick.getYaw(); #ifdef PRINT_ROLL Serial.print(cur_mills); // sampling time Serial.print(","); Serial.print(accl_roll); // roll data calculated by the accelerometer Serial.print(","); Serial.print(gyro_roll); // roll data calculated by the gyroscope Serial.print(","); Serial.print(comp_roll); // roll data calculated by the complementary filter Serial.print(","); Serial.print(kalm_roll); // roll data calculated by the kalman filter Serial.print(","); Serial.print(madw_roll); // roll data calculated by the madgwick filter #endif #ifdef PRINT_PITCH Serial.print(cur_mills); // sampling time Serial.print(","); Serial.print(accl_pitch); // pitch data calculated by the acclerometer Serial.print(","); Serial.print(gyro_pitch); // pitch data calculated by the gyroscope Serial.print(","); Serial.print(comp_pitch); // pitch data calculated by the complemtary filter Serial.print(","); Serial.print(kalm_pitch); // pitch data calculated by the kalman fitler Serial.print(","); Serial.print(madw_pitch); // pitch data calculated by the madgwick filter #endif Serial.println(); } void setup() { Serial.begin(115200); BMI160.begin(BMI160GenClass::I2C_MODE); madgwick.begin(SENSE_RATE); BMI160.setGyroRate(SENSE_RATE); BMI160.setAccelerometerRate(SENSE_RATE); BMI160.setGyroRange(GYRO_RANGE); BMI160.setAccelerometerRange(ACCL_RANGE); BMI160.autoCalibrateGyroOffset(); BMI160.autoCalibrateAccelerometerOffset(X_AXIS, 0); BMI160.autoCalibrateAccelerometerOffset(Y_AXIS, 0); BMI160.autoCalibrateAccelerometerOffset(Z_AXIS, 1); delay(100); /* Set kalman and gyro starting angle */ int rawXAcc, rawYAcc, rawZAcc; BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc); float accX = convertRawAccel(rawXAcc); float accY = convertRawAccel(rawYAcc); float accZ = convertRawAccel(rawZAcc); float roll = rad_to_deg(atan(accY / accZ)); float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ))); // Set starting angle kalmanRoll.setAngle(roll); kalmanPitch.setAngle(pitch); gyro_roll = comp_roll = roll; gyro_pitch = gyro_pitch = pitch; } void loop() { print_roll_pitch(); }
■ 出力データを確認する
ロール方向、ピッチ方向にそれぞれ筐体を振ってみてデータを収集しました。
それぞれのデータはこちらです。
どのフィルタもジャイロのドリフトはキャンセルしてくれているのは分かりますが、グラフだけではどれが良いのかピンと来ませんね。もう少しうまく評価できる方法を考えてみたいと思います。🤨
(-ω-;)
SONY SPRESENSE メインボード CXD5602PWBMAIN1
- 出版社/メーカー: スプレッセンス(Spresense)
- メディア: Tools & Hardware
SODIAL 6軸レートジャイロ BMI160 6自由度重力加速度センサー モジュールIIC SPIブロード
- 出版社/メーカー: SODIAL
- メディア: その他
あのね、このページ重すぎて見るのに数十分かかります。
どんな物をページに入れているのかもしれませんが見るのが無理です。
by ma2ma2 (2020-06-08 00:33)
軽くなりました(^^)
by ma2ma2 (2020-06-08 14:55)
ご指摘ありがとうございます。いろいろと試行錯誤しているので、もうしばらく醜い状況が続くと思いますので、ご勘弁ください。
原因は、Googleさんの code-prettify でした。ここのサービスがすでに引っ越ししたようで、いつの間にかアーカイブを参照していました。私のローカル環境ではキャッシュされていたようです。今は新しい本家を参照しているのですが、いつ引っ越しになるかわからないので少し試行錯誤しています。(´・ω・`)
by ys_taro (2020-06-08 15:52)
結局、google code-prettify のコードを自分の github で運用し、そこを参照するようにしました。これで改造も自由だし、ひとまず安心。
^^
by ys_taro (2020-06-08 18:55)