0

0

前回はミニぷぱのコントローラにESP32を採用し、足を自由自在に制御できるようになり４足歩行についての理解を深めました。

ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討2

ここではコントローラにIMUを内蔵したATOM Matrixを使用してみましたので報告いたします。

ATOM Matrix

４足歩行について理解が深まりましたので、次は以下を再現してみたいと考えました。

バランス体操 pic.twitter.com/l8dsCx4SqO

— XGO-Mini AI搭載卓上サイズのロボット 初心者から上級者まで楽しめる (@XgoJp) September 21, 2021

恐らくIMUセンサで姿勢を検出して足の高さを制御して、機体を平行に保っているものと思われます。

これまでのESP32を使用した機構にIMUセンサを追加しようとも思ったのですが、
ESP32-PICOとIMUセンサ MPU6886がコンパクトに内蔵されたATOM Matrixを使用することにしました。

コントロール基板

SHISEIGYO-1 専用基板とPCA9685モジュールを用いてコントローラを構築しました。

注意点

ATOM MatrixとPCA9685モジュール間のI2C通信として
Wire.begin(22, 19); //SDA, SCL
を使用しました。

ATOM内蔵の MPU6886は
Wire1.begin(25,21);  (https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.cpp)
でI2C通信しているのでPCA9685モジュールとの干渉は心配ありません。

但し、M5Atomライブラリを用いて
M5.begin(true, true, true);  　(Serial Enable, I2C Enable, Display Enable)
でI2Cを初期化してしまうと
Wire.begin(25, 21, 100000UL);
が実行されPCA9685モジュールと干渉してしまうので
M5.begin(true, false, true);
とする必要があります。

アドレスが違うので同じI2Cピンを用いても良いのですがMPU6886とPCA9685モジュールの既存のライブラリを使用したく、そのライブラリでWre設定がなされてるので通信ピンを分けました。
今後 高速化のためにコアを分けることも見据えております。

動作

ATOM Matrixでも問題なく歩行動作を確認できました。

ATOM Matrix で ぷぱる

ブレッドボードからの卒業
ATOM Matrixを採用したということは。。#MiniPupper #ミニぷぱ #Blynk #M5Stack pic.twitter.com/bZ6ZRV6VSu

— HomeMadeGarbage (@H0meMadeGarbage) February 13, 2022

平行制御

いよいよIMUセンサをもちいて機体を平行に保つ制御を実現させます。

ATOM MatrixのIMUの座標軸は以下のようになっています。

X軸とY軸の姿勢角をセンシングして足の高さを制御します。
それぞれの軸の姿勢角と角速度にはカルマン・フィルタを用いました。
　https://github.com/TKJElectronics/KalmanFilter

1軸制御

まずは1軸 (Y軸)での制御を実現させます。
なかなか制御がうまくいかず。。

要するにこれやりたい
全然制御上手くいってないから
勉強せんといかん#MiniPupper #ミニぷぱ #Blynk #M5Stack pic.twitter.com/ZmwZQYO103

— HomeMadeGarbage (@H0meMadeGarbage) February 13, 2022

いろいろ制御方法を試行錯誤してる際にサーボが暴走しPCA9685モジュールの逆起電力防止PMOS燃えて動かなくなりました。。煙出たから。。。

しょうがないのでPMOS外してショットキーダイオードをぶち込みましたww
あくまで再購入品が届くまでの応急処置です(しかし こういったイレギュラーな状況下での対応にエンジニアの差というものが出ます)。

P制御

センシングしたY軸の角度$θ_Y$から機体を平行にするための足の移動量$h_Y$を以下で算出し制御してみます。

$$h_Y = h_{Y-1} + {Kp}_Y × \sin{θ_Y}$$

$$但し、h_{Y-1}：直前に算出した移動量、　{Kp}_Y：係数$$

姿勢角から高さ変化量を導出して随時足して目標値に収束させます。
sinをとる必要はなさそうですが何となくそうした。

まだ１軸だけど ぷぱ向き制御出来た#MiniPupper #ミニぷぱ #Blynk #M5Stack pic.twitter.com/qKWMSaXJTY

— HomeMadeGarbage (@H0meMadeGarbage) February 13, 2022

地面の傾きに追従して機体を平行に保とうとしますが、まだぎこちないです。

PD制御

さらにY軸の角速度$ω_Y$もフィードバックさせてみます。

$$h_Y =h_{Y-1} + {Kp}_Y × \sin{θ_Y + {Kd}_Y ×ω_Y / 100.0} 　　但し、{Kd}_Y：係数$$

係数${Kd}_Y$が大きい値にならないように100.0で割っています。

角速度もフィードバックしたら
応答性良くなってヌルヌルになったとです。#MiniPupper #ミニぷぱ #Blynk #M5Stack #PD制御 pic.twitter.com/ISVNzqYiPf

— HomeMadeGarbage (@H0meMadeGarbage) February 13, 2022

かなりヌルヌルと追従できるようになしました！

2軸制御

同様にX軸についてもセンシングして足の高さを制御し、2軸での平行追従制御を実現させました。

念願の2軸#MiniPupper #ミニぷぱ #Blynk #M5Stack #PD制御 pic.twitter.com/l62xvaRjcC

— HomeMadeGarbage (@H0meMadeGarbage) February 13, 2022

おわりに

IMUを内蔵したATOM Matrixをコントローラに採用することで、やってみたかったバランス制御が実現できました。

IMUのYaw角センスで旋回制御して常に同じ方向を向くのもやってみたかったのですが、MPU6886のドリフトが大きく実現できませんでした。
違うセンサでいつか実施したいですね。

次の記事

ロボット犬『Mini Pupperミニぷぱ』ATOM Matrix で動作再検討

関連記事

ロボット犬『Mini Pupperミニぷぱ』ATOM Matrix で動作再検討 ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御 ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討1 ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討2 ロボット犬『Mini Pupperミニぷぱ』ラズパイ4 Node-REDで味見 ロボット犬『Mini Pupperミニぷぱ』Pythonでモーション記述 ロボット犬『Mini Pupperミニぷぱ』を BNO055で姿勢制御 ロボット犬『Mini Pupperミニぷぱ』で 書道