MadgwickフィルタのライブラリをArduinoで使う

Last updated at 2023-12-27Posted at 2021-09-03

Madgwickフィルタの力で6軸センサによる姿勢推定をよい感じに整える

前回記事

これの続きです。

やること

手元のセンサを傾けると、画面の中のオブジェクトもシンクロして傾くというデモの続きです。
今回はMadgwickフィルタを通してセンサで取得した姿勢を安定させます。
ライブラリにはMadgwickAHRS.hを使います。

まずこのライブラリの名前について。
"Madgwick"の読みかたですが海外の動画などをみるとマジェウィックと読んでいるようです。
その後ろのAHRSが何を意味するかというとAttitude Heading Reference Systemのことで、IMUに地磁気センサを足したもののことらしいです。加速度とジャイロの6軸だけのものはIMU(Inertial Measurement Unit)と呼ぶようです。

準備物・実施環境

Arduino系のもの（今回の狙いはTeensy3.2)
processing4（たぶん3でもよい）
6軸センサ　GY-521/MPU6050（※MPU9250でも動く）
手元の環境はMac

写真左がGY-521/MPU6050、右がMPU9250

参考

下記のサイトを参考にさせていただきました。

接続

GY-521	Arduino
VCC	5V
GND	GND
SCL	SCL(Pin19)
SDA	SDA(Pin18)
XDA	none
AD0	none
INT	none

SCL0はArduinoUno,Teensy3.2共にPin19です。
SDA0はArduinoUno,Teensy3.2共にPin18です。
接続は電源2本、信号2本の計4本で動きます。
MPU9250のブレイクアウトボードを使う場合も同様ですが、今回は6軸分のみ使っています。

プログラム

Arduino系側のスケッチ

Arduino系


#include "MPU6050.h"
MPU6050 accelgyro;
#include <MadgwickAHRS.h>
Madgwick MadgwickFilter;

int16_t ax, ay, az;//加速度　int16_tは2バイトの符号付き整数
int16_t gx, gy, gz;//角速度　同上
float ROLL, PITCH, YAW;

void setup()
{
  Wire.begin();
  Serial.begin(115200);
  accelgyro.initialize();//I2Cデバイスの初期化
  delay(300);
  MadgwickFilter.begin(100);//フィルタのサンプリングを100Hzで
}

void loop()
{
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  MadgwickFilter.updateIMU(gx / 131.0, gy / 131.0, gz / 131.0, ax / 16384.0, ay / 16384.0, az / 16384.0);
  ROLL = MadgwickFilter.getRoll();
  PITCH = MadgwickFilter.getPitch();
  YAW  = MadgwickFilter.getYaw();
  Serial.print(ROLL); Serial.print(",");
  Serial.print(PITCH); Serial.print(",");
  Serial.print(YAW);
  Serial.print("\n");
  delay(10);
}

ax, ay, azは各軸方向の加速度で、出力される+32767〜−32768の値がおよそ +2g〜−2g に対応するようです。よって値を16384.0で割るとgに近似できます。
gx, gy, gzは角速度で，出力される+32767〜−32768の値がおよそ+250°/s〜−250°/s に対応するようです。よって値を131.0で割ると°/sに近似できます。

MadgwickFilterに渡す値の単位は上記で変換したg(m/s2)と角加速度(degree)を入れるようにしています。ROLL、PITCH、YAWの各軸の推定角度はMadgwickよりdegreeで出力されます。
この数値をProcessingに渡します。

実行

Arduino側のプログラムは以上です。実行するとシリアルモニタ上にずらりとデータが表示されるようになります。
ROLL、PITCH、YAW の順で書き出されます。

Processing側のスケッチ

Processing3

import processing.serial.*;
Serial myPort;
printArray(Serial.list());

まずProcessing3の画面で上記のスケッチを実行します。
Processing3のスクリプト画面の下段にあるシリアル通信のコンソール画面がに、シリアルポートのリストが出力されます。
ここで、Arduino系で使っているシリアルポートと同じものの番号を調べて控えておきます。
Arduinoで使用中のポートはArduinoSDKのメニューから「ツール」→「シリアルポート」で確認できます。

Processing4

import processing.serial.*; // シリアルライブラリをインポート

Serial myPort;  //シリアルポートのインスタンス
int available_serialport = 2; // シリアル検索プログラムで調べたシリアルポートの番号に設定数値を変更しておく
String arduinoPort = Serial.list()[available_serialport ]; //シリアルポートの設定
float []data = new float [3]; //シリアルのデータを格納する配列を宣言

void setup() {
  lights();
  size(300, 300, P3D); // キャンバスサイズ
  myPort = new Serial(this, arduinoPort, 115200); // シリアルポートの設定
}

void draw() {
  background(230); //背景グレー
  translate(width / 2, height / 2, 0); // 原点を図形の中心に
  rotateX(radians(-data[1])); //ここではProcessingのX軸としてPITCH軸の値を渡している.センサの初期方向にあわせお好みで調整
  rotateZ(radians(-data[0])); //ここではProcessingのZ軸としてROLL軸の値を渡している.センサの初期方向にあわせお好みで調整

  int size = 10;//図形のサイズ倍率。キャンバスを大きくする時に変更可
  box(20 * size, 1 * size, 15 * size); //GY-521基盤のような直方体を描く
  translate(0, -4 * size, 0);
}

void serialEvent(Serial p) { //シリアルを監視
  String inString = myPort.readStringUntil('\n'); //データがあったら改行のところまで読み込む
  if (inString != null) { //シリアルの文字列データが何か入っていれば
    inString = trim(inString); //シリアル文字列の前後の空白を削除
    data = float(split(inString, ',')); //ピリオドで分割して配列に格納
    println(data);//受信した配列データをprocessingのコンソールに出力
  }
}

実行

前述のプログラムが入ったArduinoをPCにUSB接続した状態で、上記のProcessing4用スケッチを実行すると、画面に板が現れます。
Arduinoに接続したセンサを傾けると、画面の中の板もシンクロして傾きます。
フィルタのおかげで前回のものよりもスムーズに反映されます。

このあと

9軸を使ったMadgwickや6軸によるヨー推定なども試していきたいと思います。