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

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

ただいまの
回答率

88.77%

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

受付中

回答 1

投稿 編集

  • 評価
  • クリップ 2
  • VIEW 4,375

tannu

score 13

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

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

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

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

#include <Wire.h>
#include <Servo.h>
#define SerialPort Serial
#include <SparkFunMPU9250-DMP.h>
MPU9250_DMP imu;
double roll , pitch, yaw;
long int pre_ts=0;

void setup() 
{

    {
  SerialPort.begin(115200);

 if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      SerialPort.println("Unable to communicate with MPU-9250");
      SerialPort.println("Check connections, and try again.");
      SerialPort.println();
      delay(3000);
    }
  }


  imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);


  imu.setGyroFSR(250); // Set gyro to 2000 dps
  // Accel options are +/- 2, 4, 8, or 16 g
  imu.setAccelFSR(2); // Set accel to +/-2g
  imu.setLPF(10); // Set LPF corner frequency to 5Hz
  imu.setSampleRate(10); // Set sample rate to 10Hz
  imu.setCompassSampleRate(50); // Set mag rate to 10Hz
}

  pre_ts=millis();
}

//}
void loop() 
{
    if ( imu.dataReady() )
  {
    imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
    printIMUData(millis()-pre_ts);
    pre_ts=millis();
  }
}

void printIMUData(long int dt)
{  

  float accelX = imu.calcAccel(imu.ax);
  float accelY = imu.calcAccel(imu.ay);
  float accelZ = imu.calcAccel(imu.az);
  float gyroX = imu.calcGyro(imu.gx)/57.3;
  float gyroY = imu.calcGyro(imu.gy)/57.3;
  float gyroZ = imu.calcGyro(imu.gz)/57.3;
  float magX = imu.calcMag(imu.mx);
  float magY = imu.calcMag(imu.my);
  float magZ = imu.calcMag(imu.mz);


   pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ))));
   roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ))));

   // yaw from mag

   float Yh = (magY * cos(roll)) - (magZ * sin(roll));
   float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch));

   yaw =  atan2(Yh, Xh);



   roll = roll*57.3;
   pitch = pitch*57.3;
   yaw = yaw*57.3;

   // -180~180 to 0~360
   roll = roll + 180; 
   pitch = pitch + 180;
   yaw = yaw + 180; 

  Serial.print(pitch);
  Serial.print(',');
  Serial.print(roll);
  Serial.print(',');
  Serial.println(yaw);


}

お聞きしたいこと

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

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

追記

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

#include "freeram.h"

#include "mpu.h"
#include "I2Cdev.h"

int ret;
void setup() {
    Fastwire::setup(400,0);
    Serial.begin(9600);
    ret = mympu_open(200);
    //Serial.print("MPU init: "); Serial.println(ret);
    //Serial.print("Free mem: "); Serial.println(freeRam());

}

unsigned int c = 0; //cumulative number of successful MPU/DMP reads
unsigned int np = 0; //cumulative number of MPU/DMP reads that brought no packet back
unsigned int err_c = 0; //cumulative number of MPU/DMP reads that brought corrupted packet
unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set

void loop() {
    ret = mympu_update();

    switch (ret) {
    case 0: c++; break;
    case 1: np++; return;
    case 2: err_o++; return;
    case 3: err_c++; return; 
    default: 
        Serial.print("READ ERROR!  ");
        Serial.println(ret);
        return;
    }

    if (!(c%25)) {
        // Serial.print(np); Serial.print("  "); Serial.print(err_c); Serial.print(" "); Serial.print(err_o);
        //Serial.print(" Yaw: "); 
        Serial.print(mympu.ypr[0]);
       Serial.print(',');
        //Serial.print(" Pitch: "); 
        Serial.print(mympu.ypr[1]);
       Serial.print(',');
        //Serial.print(" Roll: "); 
        Serial.println(mympu.ypr[2]);


        //Serial.print("\tgy: "); Serial.print(mympu.gyro[0]);
        //Serial.print(" gp: "); Serial.print(mympu.gyro[1]);
        //Serial.print(" gr: "); Serial.println(mympu.gyro[2]);
    }
}
  • 気になる質問をクリップする

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

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

    クリップを取り消します

  • 良い質問の評価を上げる

    以下のような質問は評価を上げましょう

    • 質問内容が明確
    • 自分も答えを知りたい
    • 質問者以外のユーザにも役立つ

    評価が高い質問は、TOPページの「注目」タブのフィードに表示されやすくなります。

    質問の評価を上げたことを取り消します

  • 評価を下げられる数の上限に達しました

    評価を下げることができません

    • 1日5回まで評価を下げられます
    • 1日に1ユーザに対して2回まで評価を下げられます

    質問の評価を下げる

    teratailでは下記のような質問を「具体的に困っていることがない質問」、「サイトポリシーに違反する質問」と定義し、推奨していません。

    • プログラミングに関係のない質問
    • やってほしいことだけを記載した丸投げの質問
    • 問題・課題が含まれていない質問
    • 意図的に内容が抹消された質問
    • 過去に投稿した質問と同じ内容の質問
    • 広告と受け取られるような投稿

    評価が下がると、TOPページの「アクティブ」「注目」タブのフィードに表示されにくくなります。

    質問の評価を下げたことを取り消します

    この機能は開放されていません

    評価を下げる条件を満たしてません

    評価を下げる理由を選択してください

    詳細な説明はこちら

    上記に当てはまらず、質問内容が明確になっていない質問には「情報の追加・修正依頼」機能からコメントをしてください。

    質問の評価を下げる機能の利用条件

    この機能を利用するためには、以下の事項を行う必要があります。

回答 1

0

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

    float Q_angle  =  0.001;
    float Q_gyro   =  0.003;
    float R_angle  =  0.03; 
    float x_angle;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
    float  y, S;
    float K_0, K_1;
    float dt=0.005;
    float kal_deg;

float kalman(float newAngle, float newRate){

        x_angle += dt * (newRate - x_bias);

        P_00 +=  dt * (dt * P_11 - P_01 - P_10 + Q_angle);
        P_01 -=  dt * P_11;
        P_10 -=  dt * P_11;
        P_11 +=  Q_gyro * dt;

        y = newAngle - x_angle;
        S = P_00 + R_angle;
        K_0 = P_00 / S;
        K_1 = P_10 / S;

        x_angle +=  K_0 * y;
        x_bias  +=  K_1 * y;
        P_00 -= K_0 * P_00;
        P_01 -= K_0 * P_01;
        P_10 -= K_1 * P_00;
        P_11 -= K_1 * P_01;

    return x_angle;
}


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

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

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

-追記-

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

投稿

編集

  • 回答の評価を上げる

    以下のような回答は評価を上げましょう

    • 正しい回答
    • わかりやすい回答
    • ためになる回答

    評価が高い回答ほどページの上位に表示されます。

  • 回答の評価を下げる

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

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

    評価を下げる際はその理由を明確に伝え、適切な回答に修正してもらいましょう。

  • 2019/05/03 18:03

    お返事ありがとうございます.
    以前はmpu6050を使用していたのですが,重力方向周りの絶対角を求めたかったためにこちらのセンサを使用しております.
    自分も制御工学などは勉強中なので苦しんでおります,,,

    追記部分の記事を参考にさせていただいたのですが,自分はarduino unoを使用しているためメモリ不足でコンパイル出来ませんでした,,

    DMPについては少し調べてみようと思います.
    参考になるご意見ありがとうございました.

    キャンセル

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

  • ただいまの回答率 88.77%
  • 質問をまとめることで、思考を整理して素早く解決
  • テンプレート機能で、簡単に質問をまとめられる

関連した質問

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