Madgwickフィルタで姿勢角推定

Madgwickフィルタとは?

  • マドウィックフィルタと読む
  • 3次元の姿勢角推定に用いる
  • 加速度、ジャイロ、(地磁気) でセンサフュージョンする
  • 相補フィルタより精度が良く、カルマンフィルタより計算が軽い
  • 計算にはクォータニオンを用いる

Arduinoで使えるMadgwickフィルタライブラリ

下記のライブラリが利用できる。
github.com

  • 入力: 6軸センサ(加速度+ジャイロ) または 9軸センサ(加速度+ジャイロ+地磁気)
  • 出力: 姿勢角(ロール、ピッチ、ヨーのオイラー角表現)

f:id:licheng:20200326200938p:plain

地磁気を入力しない場合、鉛直軸の回転はジャイロのドリフトを補正できない。重力加速度による角度推定が鉛直軸回転には利用できないためである。これは相補フィルタやカルマンフィルタも同様である。

使い方

【宣言】

#include <MadgwickAHRS.h>
Madgwick madgwick;

【初期化】

  madgwick.begin(100); //100Hz (10msec周期)

【計算】

    // 6軸センサの場合 (ジャイロは[deg/s]、加速度は生値でも[G]でも[m/s^2]でも可)
    madgwick.updateIMU(gx, gy, gz, ax, ay, az);
    // 9軸センサの場合 (地磁気は生値でも[uT]でも可)
    madgwick.update(gx, gy, gz, ax, ay, az, mx, my, mz);

【姿勢角(オイラー角表現)の取得】

    // [deg]で取得
    float roll  = madgwick.getRoll();
    float pitch = madgwick.getPitch();
    float yaw   = madgwick.getYaw();
    // [rad]で取得
    float roll  = madgwick.getRollRadians();
    float pitch = madgwick.getPitchRadians();
    float yaw   = madgwick.getYawRadians();
注意事項

このライブラリでは、内部計算に用いているクォータニオンがprivate変数になっている。しかし、オイラー角への変換には逆三角関数を用いるので計算量が多いし、その後の計算にもクォータニオンのまま扱ったほうが有利なことも多い。そこで、クォータニオンをpublic変数に変更しておく。具体的には MadgwickAHRS.h の当該箇所を下記のように修正する。

public:
    float q0;
    float q1;
    float q2;
    float q3;

6軸センサBMI160

BMI160はBOSCH社製の6軸センサである。詳しくは下記の記事を参照。
lipoyang.hatenablog.com

Arduino用ライブラリも公開されている。 (非公式)
github.com

実験プログラム

Arduino系ボードのGR-CITRUSに6軸センサBMI160をSPI接続してMadgwickフィルタによる姿勢推定の実験プログラムを作成した。

f:id:licheng:20200326205542j:plain:w480

結線
GR-CITRUS BMI160
GND GND
3.3V 3V3
13 SPI CK SCL/SCx
12 SPI MI SAO/SDO
11 SPI MO SDA/SDx
10 SPI SS CS
スケッチ

こちらからダウンロードできます。