初めに
本来は、こちらの記事に統一して記載していた記事だったんですが、Advent Calendarに登録できなかったので、別途こちらに記載しました。
こちらの記載も、特に消さずに残しておきます。
使用しているボード
マルチIMUボード
メインボード
拡張ボード
元ネタ（ブログ）
こちらの記事は、こちらのブログからの記事です。
紆余曲折や課題などをより詳細に書いています。
MadickAHRS を使った姿勢推定
公式からAHRSのサンプルが追加されているのをみて、Arduinoでも簡単に追加できるな～と思い、追加しました。ライブラリは、Arduinoライブラリとして入手可能なこちらを使います。
Arduinoのライブラリとして登録しているものでは、マルチIMUでは正しい姿勢推定ができなかったので、以下の2点を修正しています。
- Quatanionを取得できるようにした
- 周波数ではなく、タイムスタンプのΔ値を使って計算できるようにした。
これらの修正がないと、精度よく推定ができません。以下の注意点がそのポイントです。
姿勢推定時のマルチIMUの注意点
- 周波数が指定したものより大きくずれている
- 毎フレームのタイムスタンプは割と揺らいでいる
- 最初にジャイロバイアスを除去するためのキャリブレーションが必要
これらの課題を避けるために MadgwickAHRSの修正を行っています。
※この課題に対する紆余曲折は、元ネタのブログを参照してください。
これを使った姿勢推定のライブラリは、こちら
さらに、この結果を見やすくするため、SpresenseのUSBシリアルでつないで、高速転送させ、
PC上のProcessingでそれを描画するようなサンプルも用意しました。
Processingのコードは、Quatanionの結果とオイラー角の結果のグラフを書くものと3Dモデルに姿勢を反映させるものの2種類あります。
自己位置の推定
姿勢が精度よく推定できるようになれば、自己位置も推定できるだろうということで、次に自己位置を推定するサンプルを作成しました。ボードを手で持って動かした軌跡を可視化するサンプルになります。
非常に精度がいいとはいえ、ドリフトはしなわけではなく、それに対して、角速度を積分して得られた姿勢から加速度を加味して積分し速度を出し、それを積分して位置をだす。となると、角速度の3乗で誤差が載るので、ある程度移動したら、ZUPTで補正する必要がありそうです。
自己位置推定時のマルチIMUの注意点
- 姿勢推定はMadwickなどのフィルタを使わずセンサー値をそのまま積分する
- 重力加速度のキャリブレーション、初期姿勢の正確な推定が大事
- 動かし方次第だが、できるだけ高周波でセンシングした方が良い
- ZUPT判定のヒステリシスを大きくとると判定までの時間でドリフトしてしまう
- キャリブレーション時のノイズを極力避ける（テーブルに触れるのですらNG）
これらの注意点を注意しながらサンプルコードを作成しました。
1サンプルごとにコア間通信をしてしまうと逆にオーバーヘッドが大きすぎるため、
一定数単位（80サンプル）で、データを送信するようにしています。また、MainCoreへの位置情報は、最終位置が伝わればよいので、一定数単位（80サンプル）に1回のみ送信し、PCへもその頻度での更新を行っています。
同時に、センサーのサンプリングレートは960にしています。それ以上が推奨かなと思っています。
こちらもPC上に軌跡を描けるように、USBの高速シリアルで結果を送って、Processingで描画しています。
この結果は、こちらになります。
これである程度の位置推定もできるようになりました！