Atom Matrix、Atom MotionとRustで作る倒立振子

この記事はM5Stack Advent Calendar 2022に参加しています。

はじめに

制御工学の勉強として倒立振子ロボット（セ〇ウェイ的なやつ）を作りたくなりました。
ですが今は疲れていてはんだ付けするほどの気力はありません。
そこで今回は気力のない時でも作れる、心身に優しいお粥のようなロボットを紹介します。

ロボットの制御にはESP32マイコンとIMUなどを搭載した、モータの駆動にはAtom用拡張モジュールAtom Motionを使います。

はんだ付けなしで作れる倒立振子ロボットとしてはAtom Matrixなどと同じM5Stack製品のBALA-C PLUSがあるので、これから作られる方はそちらをお勧めします。

ハードウェア

部品リスト

使う材料は下記の通りです。

• Atom Matrix
  • ESP32マイコン、IMU（加速度・ジャイロセンサ）、LEDマトリックスなどを搭載した開発モジュールです。
• Atom Motion
  • Atom用の拡張モジュールです。DCモータ2台とサーボ4台を制御でき、リチウムイオン電池からの給電機能もついています。
  • Atom MotionにはAtom Liteというマイコン開発モジュールが付属していますが、そちらはIMUがついていないので別途Atom Matrixが必要です。
• ギヤードモータ FEETECH FM90 ×2
  • サーボモータのように見えますが、単なるギア付きDCモータです。制御回路は入っていません。
  • もともとサーボ用の機構なのでバックラッシュ（歯車の遊び）が小さく、制御回路がないため連続回転サーボよりも応答性が良いという、倒立振子にはもってこいのモータです。
• タイヤ FS90-W ×2
• 両面テープ（クッション性のある強力なもの、通常のもの両方）
• Atom Matrixの入っていた箱

組み立て

このようにクッションタイプの両面テープを使って2台のギヤードモータを背中合わせに貼り合わせ、その上にAtom Motionを接合します。

裏面には補強板を貼ります。ちなみにこれはAtom Matrixが入っていた薄いプラスチックの箱を切り、何枚も薄い両面テープで重ねて丈夫にしたものです。

あとはタイヤやAtom Matrix、バッテリなどを取り付けて、モータの線をAtom MotionのDCモータ端子に接続すれば完成です。

ソフトウェア

今回は勉強も兼ねてRustでプログラムを書いてみました（もともとArduino環境で書いてパラメータ調整まで終えてからRustに移植しています）。
動作は単に直立するだけですが、ボタンを押してスタートする機能や、倒れるとモータを停止させる機能を実装しています。
全体は長いのでGitHub上に置いています。

Rustについて

Rustコンパイラ（およびその基盤となっているLLVM）は公式ではESP32の搭載しているXtensaアーキテクチャに対応していないのですが、Espressif社やコミュニティの方により移植・対応が進んでおり、特に問題なく使用できます。
開発環境を構築する際、以前はLLVMを自分でビルドするなど手間が大きかったのですが、今はespupというツールで簡単に構築できるようになっていました。

開発環境に関して、詳しくはThe Rust on ESP Bookをご参照ください。

さらに、今はESP-IDF（Espressif社公式のC/C++用開発環境）をベースとしてメモリ確保やスレッドなどOS環境のようなライブラリ(Rust STD)が整備されており、I2Cなどペリフェラル用のAPIも整備されているため、（Arduinoほど簡単とはいかないものの）C言語と遜色ない開発ができるようになっています。

ただし、センサなど周辺デバイスのライブラリの不足を感じることはあります。
Atom Matrixに搭載されているIMU、MPU6886も当初はRust用ライブラリがありませんでした。
しかし、同じメーカーの製品でレジスタ構成も近いMPU6050用のライブラリがあったため、今回はそれを改造して使用しています。

制御について

制御方式はおそらく、同種のロボットで一般的に使われているであろうPID制御を使っています。
ただし今回は、PID制御の式

$$u(t) = K_p e(t) + K_i \int_0^t e(\tau) d\tau + K_d \frac{de(t)}{dt}$$

の両辺を微分して

$$\frac{du(t)}{dt} = K_p \frac{de(t)}{dt} + K_i e(t) + K_d \frac{d^2 e(t)}{dt^2}$$

として、操作量の微分を出力する「速度型PID制御」という方式を使っています^[1]。

パラメータ調整

倒立振子ロボットで一番大変なのはおそらくパラメータの調整です。
今回は特にスマートな方法は使わず単純に試行錯誤で値を決めましたが、一番効果が大きかったのはIゲインを大きくすることと、目標角度を精密に調整することでした。

結果

https://www.youtube.com/watch?v=MPwQVkEy1xE

一応立つことはできますが、ゆらゆらと移動してしまいます。
車輪の回転数を測定する仕組みがなく、位置制御ができないのも一因でしょうか。
Arduino環境で実装した際には遠隔操作による前進・後退も実装していましたが、応答性が低く不安定だったためRustでは実装していません。

おわりに

今回はAtom MatrixとAtom Motionを使い、簡易的な倒立振子ロボットを製作しました。
現在はステッピングモータを使った新しいハードウェアを開発中です。

脚注

1. 参考文献：速度型PID制御～制御工学の基礎あれこれ～ http://arduinopid.web.fc2.com/N19.html