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

環境

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

前回までのあらすじ

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

今回の話

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

Madgwickフィルタ

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

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

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

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

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軸方向もなんか大きく動いている…

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

この記事が気に入ったらサポートをしてみませんか?