へっぽこ元ロボコニスト

ロボコンに燃え尽きた自分が日々の出来事を書くだけのブログです

2019/10/7(Raspberry PiのROSからpigpioを使ったSPI通信でMPU9250を動かして,Madgwick Filterから姿勢を計算する)

くそ長タイトル

前回はMPU9250の値を取りました

mozyanari.hatenablog.com

MPU9250の値を取るだけでは,重力加速度しか分からないので姿勢を計算していきます
ただ,修論もあるし手軽にROSのパッケージがあるMadgwick Filterを使います

wiki.ros.org

ドローンなら,ラズパイとMPU9250で姿勢が取得出来て,PWMをだしてアンプを動かして飛ばせるのではと思います

環境

前回と同じです Raspberry Pi 3 B
OS:Ubuntu Mate
ROS:Kinetic
9軸センサ:MPU9250

プログラム

github.com pigpioを使ったSPI通信とMPU9250の値の取得は前回の記事を参照してください

センサデータ変換

double acc_x = mpu9250.get_acc_x()/16384.0*9.81;
double gyr_x = mpu9250.get_gyr_x()/130.072*0.01745;

今回は,加速度と角速度だけを使います
imu/data_rawというデータをimu_filter_nodeに与えます
なので,imu/data_rawを自分で作る必要があります

センサデータをimu_filter_nodeが求めている単位に変換して与えます 何度か実験してみた所,
加速度は,m/s2
角加速度は,rad/s2
が必要ということが分かりました
式の意味は前回の記事を参考にしてください

imu/data_rawの作成

sensor_msgs::Imu Imu;
Imu.header.frame_id ="imu";
Imu.header.stamp = ros::Time::now();
Imu.linear_acceleration.x = acc_x;
Imu.linear_acceleration.y = acc_y;
Imu.linear_acceleration.z = acc_z;

Imu.angular_velocity.x = gyr_x;
Imu.angular_velocity.y = gyr_y;
Imu.angular_velocity.z = gyr_z;

もしかしたら,分散の場所に値を入れればMadgwick Filterが上手いこと何か処理してくれるかもしれませんが,ここでは値を入れてpublishするだけです

Madgwick Filterの追加

github.com

ソースにgit cloneで持ってきてコンパイルします
ただ,ラズパイだとフリーズする可能性があります(一度フリーズしました)
なので,makeするときは

catkin_make -j1 -l1

でしましょう

同時に起動するlaunchファイル

github.com

MPU9250から加速度,角加速度を取得するノード,Madgwick Filterノードを同時に起動するlaunchです

この時,パラメータのuse_magをfalhseにします
これで地磁気を使わずに計算します

また,パラメータのreverse_tfはtrueにします これによって,imuリンクからodomリンクを生成します (これがないと,rvizで表示した時に基準となるtfがないために姿勢が変化しているか分かりません)

実験結果

youtu.be

起動するとなぜかodomリンクからyaw軸に90度回転したところから始まります
yaw軸は地磁気がないと補正は難しいようです ですが,十分使える精度だと思います

まとめ

今日はまとめで一日使ってしまった
明日からは研究しよ