質問をすることでしか得られない、回答やアドバイスがある。

15分調べてもわからないことは、質問しよう!

新規登録して質問してみよう
ただいま回答率
86.02%
Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

Q&A

受付中

9軸センサにフィルタを用いてドリフトを抑えたい

tannu
tannu

総合スコア13

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。

1回答

0グッド

2クリップ

10142閲覧

投稿2019/05/02 17:03

編集2019/05/03 10:06

センサのドリフトを抑えたい

Arduinoボードと9軸センサ(MPU9250)を利用してセンサの絶対角度を算出しようとしています.
(角度のみをシリアルモニタに表示させたいです)
ドリフトを抑えようとMadgwickフィルタもしくはカルマンフィルタを使用しようと考えているのですが,プログラミングの経験が浅く上手く適用することができません.
具体的にどのようにプログラムを変更すれば正しく適用できるのか教えていただきたく存じます.

現在使用しているプログラム

現在は以下のプログラムを使用しています.
インターネット上に上がっていたプログラムをほとんど引用して使用しています.

Arduino

1#include <Wire.h> 2#include <Servo.h> 3#define SerialPort Serial 4#include <SparkFunMPU9250-DMP.h> 5MPU9250_DMP imu; 6double roll , pitch, yaw; 7long int pre_ts=0; 8 9void setup() 10{ 11 12 { 13 SerialPort.begin(115200); 14 15 if (imu.begin() != INV_SUCCESS) 16 { 17 while (1) 18 { 19 SerialPort.println("Unable to communicate with MPU-9250"); 20 SerialPort.println("Check connections, and try again."); 21 SerialPort.println(); 22 delay(3000); 23 } 24 } 25 26 27 imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); 28 29 30 imu.setGyroFSR(250); // Set gyro to 2000 dps 31 // Accel options are +/- 2, 4, 8, or 16 g 32 imu.setAccelFSR(2); // Set accel to +/-2g 33 imu.setLPF(10); // Set LPF corner frequency to 5Hz 34 imu.setSampleRate(10); // Set sample rate to 10Hz 35 imu.setCompassSampleRate(50); // Set mag rate to 10Hz 36} 37 38 pre_ts=millis(); 39} 40 41//} 42void loop() 43{ 44 if ( imu.dataReady() ) 45 { 46 imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); 47 printIMUData(millis()-pre_ts); 48 pre_ts=millis(); 49 } 50} 51 52void printIMUData(long int dt) 53{ 54 55 float accelX = imu.calcAccel(imu.ax); 56 float accelY = imu.calcAccel(imu.ay); 57 float accelZ = imu.calcAccel(imu.az); 58 float gyroX = imu.calcGyro(imu.gx)/57.3; 59 float gyroY = imu.calcGyro(imu.gy)/57.3; 60 float gyroZ = imu.calcGyro(imu.gz)/57.3; 61 float magX = imu.calcMag(imu.mx); 62 float magY = imu.calcMag(imu.my); 63 float magZ = imu.calcMag(imu.mz); 64 65 66 pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ)))); 67 roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ)))); 68 69 // yaw from mag 70 71 float Yh = (magY * cos(roll)) - (magZ * sin(roll)); 72 float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch)); 73 74 yaw = atan2(Yh, Xh); 75 76 77 78 roll = roll*57.3; 79 pitch = pitch*57.3; 80 yaw = yaw*57.3; 81 82 // -180~180 to 0~360 83 roll = roll + 180; 84 pitch = pitch + 180; 85 yaw = yaw + 180; 86 87 Serial.print(pitch); 88 Serial.print(','); 89 Serial.print(roll); 90 Serial.print(','); 91 Serial.println(yaw); 92 93 94}

お聞きしたいこと

上記プログラムにどのようにフィルタを適用すればよいのか
もしくは
上記プログラムは関係なくフィルタを適用した上での角度算出ができるプログラム
をお伺いしたいです.

知識不足な部分が多いかと思いますがよろしくお願いします.

追記

DMPを利用した角度を算出するプログラムを見つけることができたのですが,絶対角度で算出することができません.
こちらのコードから絶対角度を算出する方法があれば教えていただきたいです.
(加速度,ジャイロ,地磁気の値を取り出せれば回転行列によって求められるのですが,,,)
以下にそのコードを添付いたします.(必要ないものはコメントアウトしています)
参照サイト

Arduino

1#include "freeram.h" 2 3#include "mpu.h" 4#include "I2Cdev.h" 5 6int ret; 7void setup() { 8 Fastwire::setup(400,0); 9 Serial.begin(9600); 10 ret = mympu_open(200); 11 //Serial.print("MPU init: "); Serial.println(ret); 12 //Serial.print("Free mem: "); Serial.println(freeRam()); 13 14} 15 16unsigned int c = 0; //cumulative number of successful MPU/DMP reads 17unsigned int np = 0; //cumulative number of MPU/DMP reads that brought no packet back 18unsigned int err_c = 0; //cumulative number of MPU/DMP reads that brought corrupted packet 19unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set 20 21void loop() { 22 ret = mympu_update(); 23 24 switch (ret) { 25 case 0: c++; break; 26 case 1: np++; return; 27 case 2: err_o++; return; 28 case 3: err_c++; return; 29 default: 30 Serial.print("READ ERROR! "); 31 Serial.println(ret); 32 return; 33 } 34 35 if (!(c%25)) { 36 // Serial.print(np); Serial.print(" "); Serial.print(err_c); Serial.print(" "); Serial.print(err_o); 37 //Serial.print(" Yaw: "); 38 Serial.print(mympu.ypr[0]); 39   Serial.print(','); 40 //Serial.print(" Pitch: "); 41 Serial.print(mympu.ypr[1]); 42   Serial.print(','); 43 //Serial.print(" Roll: "); 44 Serial.println(mympu.ypr[2]); 45 46 47 //Serial.print("\tgy: "); Serial.print(mympu.gyro[0]); 48 //Serial.print(" gp: "); Serial.print(mympu.gyro[1]); 49 //Serial.print(" gr: "); Serial.println(mympu.gyro[2]); 50 } 51} 52

気になる質問をクリップする

クリップした質問は、後からいつでもマイページで確認できます。

またクリップした質問に回答があった際、通知やメールを受け取ることができます。

下記のような質問は推奨されていません。

  • 質問になっていない投稿
  • スパムや攻撃的な表現を用いた投稿

適切な質問に修正を依頼しましょう。

回答1

0

私もジンバルを2年前に作った時、このカルマンフィルタに直面しました。
私が使っていたのは6軸センサのmpu6050だったので、9軸も同じようにとはいかないと思いますが・・・一様下記のようなコードが落ちていました。

C

1 float Q_angle = 0.001; 2 float Q_gyro = 0.003; 3 float R_angle = 0.03; 4 float x_angle; 5 float x_bias = 0; 6 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; 7 float y, S; 8 float K_0, K_1; 9 float dt=0.005; 10 float kal_deg; 11 12float kalman(float newAngle, float newRate){ 13 14 x_angle += dt * (newRate - x_bias); 15 16 P_00 += dt * (dt * P_11 - P_01 - P_10 + Q_angle); 17 P_01 -= dt * P_11; 18 P_10 -= dt * P_11; 19 P_11 += Q_gyro * dt; 20 21 y = newAngle - x_angle; 22 S = P_00 + R_angle; 23 K_0 = P_00 / S; 24 K_1 = P_10 / S; 25 26 x_angle += K_0 * y; 27 x_bias += K_1 * y; 28 P_00 -= K_0 * P_00; 29 P_01 -= K_0 * P_01; 30 P_10 -= K_1 * P_00; 31 P_11 -= K_1 * P_01; 32 33 return x_angle; 34} 35

newAngleに加速度センサで得られた角度を、newRateには角速度を積分して得られた角度を入れるそうです。dtにはサンプリングタイムですね。他のパラメータ(ゲイン)のQやRなどはmpu6050ではこの値がベストらしいです。

正しいかどうかはカルマンフィルタの理論がうまく理解できてないため、分からないです。(試行錯誤しながら実験した感じではノイズがきちんと除去されていました。ちなみに、カルマンフィルタ自体はノイズに含まれている誤差が平均値0の白色ノイズという前提らしいです。)

私は制御工学を途中で挫折したので全然詳しくありませんし、これが答えになってるかどうかはわかりませんが頑張ってください。

-追記-

mpu9250では自身の姿勢角度を計算してから出力できるようになってるらしいので、DMPライブラリを使用してみてはいかがでしょうか。
こちらの記事を参考にするといいかもしれません。

投稿2019/05/02 18:02

編集2019/05/02 18:17
konataro

総合スコア37

下記のような回答は推奨されていません。

  • 質問の回答になっていない投稿
  • スパムや攻撃的な表現を用いた投稿

このような回答には修正を依頼しましょう。

回答へのコメント

tannu

2019/05/03 09:03

お返事ありがとうございます. 以前はmpu6050を使用していたのですが,重力方向周りの絶対角を求めたかったためにこちらのセンサを使用しております. 自分も制御工学などは勉強中なので苦しんでおります,,, 追記部分の記事を参考にさせていただいたのですが,自分はarduino unoを使用しているためメモリ不足でコンパイル出来ませんでした,, DMPについては少し調べてみようと思います. 参考になるご意見ありがとうございました.

まだベストアンサーが選ばれていません

会員登録して回答してみよう

アカウントをお持ちの方は

15分調べてもわからないことは
teratailで質問しよう!

ただいまの回答率
86.02%

質問をまとめることで
思考を整理して素早く解決

テンプレート機能で
簡単に質問をまとめる

質問する

関連した質問

同じタグがついた質問を見る

Arduino

Arduinoは、AVRマイコン、単純なI/O(入出力)ポートを備えた基板、C言語を元としたArduinoのプログラム言語と、それを実装した統合開発環境から構成されたシステムです。