ArduinoのMadgwickライブラリの使い方

 

 ちょこちょこ要望があるのでAruduinoにあるMadgwickライブラリの使い方を簡単ですが紹介します。

 

スポンサーリンク
スポンサーリンク

Madgwickライブラリの概要

  

 Madgwickライブラリは6軸センサー(加速度、ジャイロ)や9軸センサー(加速度、ジャイロ、地磁気)からの取得値から、Madgwickフィルターというフィルターを通して姿勢角(roll、pitch、yaw)を算出するライブラリです。

 

 フィルターの中身(数学的理解)は私にとっては非常に難解でいまいち消化不足です。おおざっぱには、「ジャイロ値からの算出姿勢」を「他のセンサー値」から補正してより正確な姿勢角を推定するというものです・・・。がライブラリ使うだけなら割と簡単です。

 

 ライブラリが2016年ごろにVrUPされてますね。以前のライブラリではサンプリング周波数が512Hzになっていて、ライブラリの外からは変更できなかったです。そのため、「通信速度」や「ジャイロ値に係数」掛けて調整してました。

 

 現在(2016年ごろ~現在2018/5)のライブラリではサンプリング周波数も設定できるため、変な調整はいらなさそうです。(たまに更新見ないとだめですね。)

 

Madgwickライブラリの使い方

 

 6軸センサーでの使い方です。9軸センサー(地磁気)は持ってなく、試したことないのでここではあまり触れません。

 

 はじめにライブラリ取り込んでインスタンス化します。

#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

でsetup()関数内で、

  MadgwickFilter.begin(100); //100Hz

 センサーのサンプリング周波数を設定します。引数は「Hz」で記述。

 

 これで準備完了です。

スポンサーリンク

 ここからはloop()内に記述する内容です。

 

 姿勢角を計算させる関数です。

MadgwickFilter.updateIMU(float gx, float gy, float gz, float ax, float ay, float az);

 姿勢角をアップデート(計算)します。引数は、各軸のジャイロ値、加速値です。

 

 ジャイロ値は[deg/s]。加速度値は、フィルターの中で正規化してますので「センサー生値」でも「G[kg/m・s^2]」でもどちらでもいいはずです。

 

 ジャイロ値、加速度値は、別途MPU6050などセンサーから取得しておきます。(MPU6050の使い方はこちら

 

 9軸センサーの場合は

MadgwickFilter.update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);

 となります。地磁気センサー分の引数が増えるだけです。

 

 アップデートした後に、姿勢角(roll、pitch、yaw)を取得します。

  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW   = MadgwickFilter.getYaw();

 それぞれ軸ごとに関数が準備されてます。これでフィルター通した姿勢角が取得できます。戻り値の単位は「度」です。戻り値を「ラジアン」単位にする場合は、*****.getRollRadians()関数を使います。

 

 制御ループ(サンプリング周波数)を最初に設定した周波数でまわせば、フィルター通過後の姿勢角が取得できます。ただ補正ゲインはライブラリの外からは変更できないので中身をいじる必要があります。ちなみにライブラリの中で設定されている係数は「0.1」です。(と思います・・。)

 

確認動画

 (使い廻しの動画ですみません)一番右側がmadwickフィルターを通した姿勢角です。(左から順に「加速度センサーのみ」、「ジャイロのみ」、「簡易相補フィルター」で姿勢角を算出したものです。)Arduino+mpu6050で取得した値をProcessingで可視化してます。

 

 ライブラリ使うだけで難解なMadgwickフィルターが、割と簡単に使えるので便利です。

 

コメント

  1. かけだし より:

    初めまして「ArduinoのMadgwickライブラリの使い方」の記事を拝見しました。大学で現在mpu6050を用いた姿勢制御について学んでいる者ですがヨー軸のドリフトで躓いておりMadgwick
    filterを用いても思うように姿勢を出せない状態です。大変恐縮ですが下記のメールアドレス宛に本記事のスケッチを添付していただくことは可能でしょうか?

    • imo より:

      かけだしさん、初めまして。サイト拝見下さり有難う御座います。
      さてご質問の件、z軸(yaw)の角度ですが、6軸センサー(ジャイロ、加速度)からでは算出は非常に困難です。
      比較的短い時間の相対的な角度変化であれば算出可能ですが、絶対角度を出すのは恐らくできないかと・・。
      別途、地磁気センサーなどで向き(角度)を確認・補正する必要があります。

  2. クラビーア より:

    初めまして
    私はMPU9250でドローン製作のためarduinoとつなげて姿勢の角度を出そうとしている
    素人です。このページが大変詳しくよみやすく重宝させていただいております。
    さて、今回Madgwickを使って6軸でロール、ピッチ、ヨーの角度を
    出してシリアルプロッタで確認しておりました。
    ところが、回転角度に対して反応はするのですが、示す値が約2倍のようでした。
    つまりセンサを水平(昆虫で表現すると地面を這う状態)にするとロール、ピッチは0ですが、
    垂直に近づけると(昆虫で表現すると木に登る状態)180度になる状況です。
    ロールもピッチも同じように2倍で出力されています。
    もしも、何か原因等で考えられることがございましたら教えていただけませんでしょうか。

    • imo より:

      クラビーアさん、サイト拝見下さり有難う御座います。
      情報が少ないので何とも言えませんが、一様にずれた値が出力されるのであれば、
      サンプリング周波数あたりの設定を一度疑ってみてはいかがでしょうか。

      • 匿名 より:

        コメントいただきありがとうございます。
        MadgwickFilter.beginを200Hzにすると値的にそこそこ近づいてきました。
        見よう見まねでやっており、ありがとうございました。

  3. シドウ より:

    初めまして
    私はMPU6050での姿勢制御を目指しているのですが、
    Madgwickフィルタを通してもドリフトが発生してしまいます。
    フィルタに渡す値に対して閾値などは設けられていますでしょうか?
    それともセンサやプログラムの問題でしょうか。
    かなり前の記事に対してですがよければ返信のほどお待ちしています。

    • imo より:

      シドウさん、サイト拝見下さり有難う御座います。
      フィルタを通す際には特に閾値等は設けてません。
      ドリフト発生はz軸廻り(YAW回転)でしょうか?
      MPU6050のみではz軸廻り(YAW回転)のドリフト補正は不可能ですので注意が必要です。

      • シドウ より:

        返信ありがとうございます。
        返事が遅れて申し訳ありません。
        Yawだけでなくpitch,rollにも及んでいる状態です。
        別のmpu6050でも同等の状態なのでプログラムを確認してみようかと思います。