SpresenseマルチIMU Add-onボードのラッパーライブラリを作ってみた

初めに

最近話題の高精度マルチIMU「SpresenseマルチIMU Add-onボード」が発売されました。こちらは、皆さんが続々と触って報告しているのですが、いかんせん、今のままだとArduinoIDEから使いにくかったので、ライブラリを作ってみました。

この記事は、適宜更新することにした。

これらの経緯の詳細は、ブログの方に書いていますので、そちらを参照してみてください。

マルチIMU製品情報

製品の詳細な情報は、こちらになります。

SpresenseマルチIMU Add-onボード

使い方の公式ドキュメントはこちらになります。

マルチIMU Add-onボード チュートリアル

その他使用している周辺ボード

メインボード

拡張ボード

ライブラリ化

ただ、初期化部分が煩雑だったり、読み出しの待ち合わせやブロック読み出しができなかったりで、
ちょっとラッパーが欲しくなり作成しました。

これで、コア間への移植とかが楽になったかなということで、SubCoreでのセンシングと、MainCoreでのSDカードなどのマルチコア化ができるようになり、1920Hzのデータも問題なく記録できるようになりました。

で、取得したバイナリを表示するツールも用意して、確認してみました。
https://github.com/Interested-In-Spresense/SpresenseIMU/blob/master/tool/imu_viewer.py

これを動かすとこんな感じ。

1.145461,37.83,0.00,-0.00,0.00,-0.03,-1.96,9.60
1.145974,37.78,0.00,-0.00,-0.00,-0.03,-1.95,9.60
1.146468,37.83,-0.00,0.00,-0.00,-0.03,-1.94,9.63
1.146977,37.84,-0.00,0.00,-0.00,-0.03,-1.94,9.66
1.147478,37.83,-0.00,0.00,-0.00,-0.04,-1.96,9.67
1.147977,37.87,-0.00,0.00,-0.00,-0.04,-1.97,9.66
1.148485,37.83,0.00,-0.00,-0.00,-0.02,-1.98,9.64
1.149020,37.83,0.00,-0.00,0.00,-0.02,-1.97,9.62
1.149511,37.82,0.00,-0.00,0.00,-0.01,-1.96,9.60
1.149978,37.83,0.00,-0.00,0.00,-0.02,-1.94,9.60
1.150494,37.83,0.00,0.00,-0.00,-0.02,-1.93,9.61
1.151099,37.83,-0.00,0.00,-0.00,-0.03,-1.94,9.63

タイムスタンプもそれっぽいし、ファイルのつなぎ目も正しく連続でした。
いいね。

MadickAHRS を使った姿勢推定（2025/12/20更新）

公式からAHRSのサンプルが追加されているのをみて、Arduinoでも簡単に追加できるな～と思い、追加しました。ライブラリは、Arduinoライブラリとして入手可能なこちらを使います。

Arduinoのライブラリとして登録しているものでは、マルチIMUでは正しい姿勢推定ができなかったので、以下の2点を修正しています。

• Quatanionを取得できるようにした
• 周波数ではなく、タイムスタンプのΔ値を使って計算できるようにした。

これらの修正がないと、精度よく推定ができません。以下の注意点がそのポイントです。

姿勢推定時のマルチIMUの注意点

• 周波数が指定したものより大きくずれている
• 毎フレームのタイムスタンプは割と揺らいでいる
• 最初にジャイロバイアスを除去するためのキャリブレーションが必要

これらの課題を避けるために MadgwickAHRSの修正を行っています。
※この課題に対する紆余曲折は、元ネタのブログを参照してください。

これを使った姿勢推定のライブラリは、こちら

さらに、この結果を見やすくするため、SpresenseのUSBシリアルでつないで、高速転送させ、

PC上のProcessingでそれを描画するようなサンプルも用意しました。

Processingのコードは、Quatanionの結果とオイラー角の結果のグラフを書くものと3Dモデルに姿勢を反映させるものの2種類あります。

自己位置の推定（2025/12/20更新）

姿勢が精度よく推定できるようになれば、自己位置も推定できるだろうということで、次に自己位置を推定するサンプルを作成しました。ボードを手で持って動かした軌跡を可視化するサンプルになります。

非常に精度がいいとはいえ、ドリフトはしなわけではなく、それに対して、角速度を積分して得られた姿勢から加速度を加味して積分し速度を出し、それを積分して位置をだす。となると、角速度の3乗で誤差が載るので、ある程度移動したら、ZUPTで補正する必要がありそうです。

自己位置推定時のマルチIMUの注意点

• 姿勢推定はMadwickなどのフィルタを使わずセンサー値をそのまま積分する
• 重力加速度のキャリブレーション、初期姿勢の正確な推定が大事
• 動かし方次第だが、できるだけ高周波でセンシングした方が良い
• ZUPT判定のヒステリシスを大きくとると判定までの時間でドリフトしてしまう
• キャリブレーション時のノイズを極力避ける（テーブルに触れるのですらNG）

これらの注意点を注意しながらサンプルコードを作成しました。
1サンプルごとにコア間通信をしてしまうと逆にオーバーヘッドが大きすぎるため、
一定数単位（80サンプル）で、データを送信するようにしています。また、MainCoreへの位置情報は、最終位置が伝わればよいので、一定数単位（80サンプル）に1回のみ送信し、PCへもその頻度での更新を行っています。

同時に、センサーのサンプリングレートは960にしています。それ以上が推奨かなと思っています。

こちらもPC上に軌跡を描けるように、USBの高速シリアルで結果を送って、Processingで描画しています。

この結果は、こちらになります。

これである程度の位置推定もできるようになりました！

自己位置の推定（2026/2/27更新）

どうしても性能が出ない場合の解析用に、解析ツールも追加してみました。
実際に組み込む前提で、Mainボードのデバッグ出力のみ利用するようにしています。

Spresenseのコードは、以下のようになっています。

1. 開始して少し（3秒）待つ。（温度等安定させるため）
2. ジャイロバイアスと重力加速度のキャリブレーション（3秒）待つ。（動かさないように）
3. 5秒待つ。（その間に計測環境を開始）
4. 1920Hzで3秒間データを取得。
5. その後、ゆっくりデバッグ出力。（ターミナルで取得したデータをファイルに保存）

このようにして出たデータを、DC 成分の可視化をする場合は、このツールを呼び出し、

ファイルを指定すると、こんな感じで可視化できます。

同じく、周波数特性を解析するツールは、

このツールから、ファイルを指定すると、こんな感じで可視化できます。

元ネタ（ブログ）

こちらの記事は、こちらのブログからの記事です。
紆余曲折や課題などをより詳細に書いています。