Genuino101で搭載されている6軸センサー（加速度、角速度）からの角度算出で、Madgwickのフィルタリングアルゴリズムがライブラリで提供されています。ジャイロスコープと加速度センサーから姿勢角を算出しているアルゴリズムのようです。
Madgwickライブラリの内容が公式サイトのチュートリアルでも詳しく説明されていましたので早速試してみました。使い方は割と簡単です。
チュートリアルではProcessingを利用してGenuino101の回転可視化まで解説がありましたが、今回はProcessingでの可視化までは行わず、算出される値がどんなものかを見てみます。Processingによる可視化は別途紹介したいと思います。⇒可視化も試してみました。可視化の記事はこちら
まずはMadgwickのライブラリをインストールする必要がありますので、Arduino IDEの「スケッチ」-「ライブラリをインクルード」-「ライブラリを管理」からMadgwickのライブラリをインストールします。この時点でバージョンは1.1.0でした。
チュートリアルのサンプルスケッチから
#include <CurieIMU.h> #include <MadgwickAHRS.h> Madgwick filter; int factor = 800;
▲IMUとMadgwickのヘッダーファイルを取り込み、filterと呼ばれるMadgwickクラスから関数にアクセスするためのオブジェクトを定義してます。factorは感度になります。数字が低いほど感度が良いです。また通信ボーレートを増やす場合はfactorも増やします。（2018/5 ⊿1：久しぶりにライブラリ確認したらVrUpされていて、サンプリング周波数の設定ができるようになっていました。factorは必要なくなってます。こちらに今のライブラリの使い方を簡単ですが投稿してます。）
CurieIMU.begin(); CurieIMU.autoCalibrateGyroOffset(); CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0); CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0); CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
▲IMUデバイスを開始。ジャイロスコープと加速度センサーのキャリブレーションを行ってます。
CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz); filter.updateIMU(gx / factor, gy / factor, gz / factor, ax, ay, az);
▲6軸センサー値（ジャイロスコープ、加速度）を取得して、Madgwickのフィルタリングアルゴリズム関数を通します。
yaw = filter.getYaw(); roll = filter.getRoll(); pitch = filter.getPitch();
▲最後にyaw/roll/pitchのそれぞれの角度を算出して完了です。
チュートリアルのスケッチを少し微調整して出力してみた結果です▼
▲Genuinoをpitch軸で数回振ってその後静止させたプロットです。出力値はラジアンとなります。Madgwickアルゴリズムの中身は詳しくわかりませんが、非常にスムーズに角度検出しました。
ドリフト補正もされているのでしょうか、水平に戻すと、roll/pitch軸に関しては0値へ収束していきます。ただ暫く動かしていると、yaw軸（z軸）にずれが見られるのと、素早い動きでおかしな動きがありました。
簡単で便利なライブラリです。Bluetooth LEと組み合わせればいろいろできそうです。
▼その他の記事はこちらから▼
・Genuino101の加速度センサーを試してみる
・Genuino101のジャイロスコープを試してみる
・Genuino101、6軸センサーからの角度算出をProcessingで可視化してみる
コメント