倒立振子の制作に挑戦してみた！第1回:マイコンで姿勢推定

倒立振子の制作にチャレンジしてみようと思います。

倒立振子を作るうえで必要なものを考えたときに、

①姿勢を検出するセンサ
②前後に動くためのモータ
③センサの値からモータの制御量を決めるマイコン

の３つがあればとりあえず動くものがつくれそうな気がします。

①と③は加速度ジャイロセンサを内蔵したマイコン製品であるM5Stackシリーズを使えばよさそうです。

②についてはDCモータやステッピングモータなどいろいろな選択肢がありますが、連続回転サーボなるものが使いやすそうだったので、今回はこれを使ってみることにします。

倒立振子をしっかり立たせるためには、筐体についてもしっかりしたものを作る必要があるみたいです。そこで今回は3Dプリンターで各パーツをがっちり固定できるような治具を作ることにします。

今回はM5StickC2Plusを使うことにしました。選定理由としては、①内臓バッテリーで駆動可能、②６軸センサ内蔵、③小さくて軽い、などが挙げられます。

第１回のメインの内容になります。

今回使用するマイコンには,xyzの三軸方向について加速度と角速度を測定できる6軸センサが搭載されています。このセンサから得られる加速度・角速度からマイコンの姿勢（角度）を推定する手法はいろいろありますが、今回はMadgwickフィルタを利用して姿勢を計算することにしました。

Madgwickフィルタの詳細は私もあまり把握していないのですが、Arduinoライブラリを使えばフィルタの動作原理を知らなくても姿勢推定ができるみたいです。

というわけで、m5stickとMadgwickフィルタで姿勢推定を行うプログラムを書きました。↓

6軸センサの値を取得するためにM5Unifiedライブラリを使っています。

また、出力値をシリアルプロッタ上で見るためにVScodeの拡張機能であるTelePlotを使用しました（PlatformIOにはシリアルプロッタが無いので...）。

プログラムの動作としては、要所要所に記載したコメントの通りなのですが、

①M5unifiedライブラリを使って6軸センサの値を取得
②センサの値(x,y,z軸方向の加速度・角速度)をmadgwickフィルタライブラリに渡す
③madgwickフィルタでロール・ピッチ・ヨー角を求めてTelePlot上でモニタする

という流れになります。

動作確認のために、M5stickを各軸方向に回転させながら、姿勢角の出力結果を見てみました。

ロール角・ピッチ角ともに、-90~90度の間でしっかり測定ができていることが確認できました。

便利なライブラリのおかげで、簡単に姿勢推定機能を実装することができました。

次回は筐体の制作に取り組んでみようと思います。

天下一反省会！