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軸センサー(ジャイロ、加速度)からでは算出は非常に困難です。
      比較的短い時間の相対的な角度変化であれば算出可能ですが、絶対角度を出すのは恐らくできないかと・・。
      別途、地磁気センサーなどで向き(角度)を確認・補正する必要があります。