Madgwickフィルタで姿勢の演算を行う

環境

WIndows11
開発環境： VScode + PlatformIO + Docker
動作環境： WSL2(Ubuntu 22.04) + ROS2(Humble)

前回までのあらすじ

9軸センサLSM9DS1の値を読み取り、その値をROSのトピックに乗せて配信し、そのトピックを購読した結果、センサーの値がわたっていることが確認できた。

今回の話

Madgwickフィルタを使って、センサの値から現在の姿勢を演算し、トピック配信を試みる

Madgwickフィルタ

6軸あるいは9軸センサの値をもとにXYZ軸方向の傾きを計算するライブラリ
軽量でArduino系のマイコンでも動作する。

1. LSM9DS1 ライブラリを組み込む

秋月のLSM9DS1でMadgwickを使う - Qiita

ライブラリの詳細は以下のサイトへ

PlatformIO Registry

使い方をとりあえず簡単に・・・

1. ライブラリの組み込み

lib_depsにMadgwickを指定

2.セットアップ関係

インクルードファイル

Madgwickオブジェクトの宣言

setup()内でbegin()をコールしておく
100Hzなので10ms間隔で演算を行うようにする

10ms周期で発生するROSのタイマーの中で処理する
1. センサを読み取り
2. madgwick.filter()のコール
3. とりあえずVector3型のトピックに乗せて配信

2. 実行

ラズパイピコにソフトを書き込み、エージェントを起動すると・・・
ちなみにセンサを上向きにおいて動作させている

Madgwickフィルタで計算されたXYZ軸方向の値(double(8byte)×3)が送信されている

別windowを開き、トピックを購読してみる

x : -3.75度、y : -2.98度…
やはりずれがあるなぁ・・・

Y軸方向を垂直においてみる

-62度？
X軸方向もなんか大きく動いている…

ジャイロセンサのドリフト問題を解消しないといけないか？