HomeMadeGarbage Loading

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

ホーム
電子工作
Raspberry Pi
IOT
音楽
Home > 電子工作 > ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討2

前回はESP32で4本の足の制御を実現し4足歩行を学びました。
ここでは更に深堀します。

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

 

 

電源コードレス化

前回は5V ACアダプタから電源供給していましたが、ミニぷぱのバッテリを使用してコードレス化を目指しました。

ミニぷぱには2セルの7.4V LiPoバッテリが搭載されています。

構成

LiPoバッテリからPCA9685モジュールのサーボ電源 (V+) と5V DCDCコンバータに給電し、
PCA9685モジュールのロジック電源 (VCC) とESP32にはDCDCの5V出力で給電します。

相変わらずのブレッドボード。。

部品

動作

電源コードがなくなったので、これでどこにでも行けます。

 

本当に散歩してきました。かわいい。
毎日したい

 

歩行検討

これまでは足の移動を座標指定しサーボが最速で動ける速度で直線的に動かして
遅延時間を設けて動作周期を変えていました。

ここでは足を細かく時間分割して正弦波形で動かすようにしてみました。
正弦波で足を動かすことによって足の方向転換時の加速度を低減を狙っています。

足踏み

足の高さを正弦波形で変えます。

線型的に足を移動して遅延で周期を設けた場合と正弦波形での移動では
正弦波のほうが機体のブレが小さくなりました。

歩行

方向時の足の位置やロール角も正弦波形で動かしてみました。

動作周期が短い場合にはさほど効果を感じませんでしたが
周期を長くするとマイルドな足移動が効いてきました。

正弦波形で動かしているというよりは時間分割で細かく座標していることが大きく効いたようです。
しかし、無駄な加速度は機体のブレに繋がるので正弦波でオールオッケーということにいたします。

 

Arduinoコード

今回作成したESP32用のArduinoコードを以下で公開いたします。
 https://gist.github.com/homemadegarbage/c31722700b58e631aec13b39db416e0b

ESP32によるミニぷぱの制御は当然 オフィシャルなものではありませんので
実施の際には自身でコードを理解・修正し自己責任で宜しくお願い致します。
過度なサポート要求もお断りさせていただきます。

コードの概要は以下の通りです。

Blynk

スマホのBlynkアプリを用いてBLE通信で歩行動作指示を送ります。

以下のESP32のBLE用サンプルコードを参考にプログラミングしました。
 https://github.com/blynkkk/blynk-library/blob/master/examples/Boards_Bluetooth/ESP32_BLE/ESP32_BLE.ino

 

以下はミニぷば用Blynkスマホアプリ画面

歩行時に上げる足の高さ(WALK HEIGHT)や歩幅(STRIDE)、旋回時のロール角度(TILT)などを指定しています。

ボタンで足踏み(STEP)やジャンプ(JUMP)動作を起動

ジョイスティックで前後(walkAd, walkBack)や旋回(walkL, walkR) 歩行します(ヴァーチャルピン V20使用)。

PCA9685

PCA9685モジュールで12個のサーボを制御します。
ESP32からI2CでPCA9685モジュールにサーボの角度指示をします。

PCA9685用のArduinoライブラリは以下の秋月電子のサイトで公開されているものを使用しました。
 https://akizukidenshi.com/catalog/g/gK-10350/

 
サンプルコードを参考に以下のように12個のサーボを制御しました。

逆運動学

足の移動は以前検証した通り、足のロール角と足の前後位置と上下位置を指定して、
逆運動学によってロール角以外のサーボの角度を算出します。

得られたサーボ角をPCA9685用のサーボ制御関数 [servo_write()]に代入してそれぞれのサーボを動かします。

足の座標とサーボ角度算出の式は以下の通り

 

腿の長さ$L_1$は50mm、下肢の長さ$L_2$は56mmでした。
足のロール角を$θ_0$、腿の角度を$θ_1$、下肢を動かすサーボの角度を$θ_2$とします。

 

ここでは腿のつけ根のロール角$θ_0$と座標x, zを指定して足を制御することにしました。

足を上げて(Z軸)、前後(X軸)するためのサーボ角度は以下で導出されます。

腿のサーボの角度$θ_1$は余弦定理を用いて導出します。

${L_2}^2 = {L_1}^2 + {l_d}^2 – 2{L_1}{l_d}\cos{θ_1}’  $

${θ_1}’ = \cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $

$$θ_1 = φ – {θ_1}’  = φ -\cos^{-1} \left(\frac{{L_1}^2+{l_d}^2 – {L_2}^2}{2{L_1}{l_d}} \right) $$

$$      但し、φ = \tan^{-1} \left(\frac{x}{z’} \right)$$

$$          l_d = \sqrt{x^2 + {z’}^2}$$

$$  高さを足のロール角で補正 z’ = \frac{z}{\cos{θ_0}}$$

 

下肢の角度はサーボの角度$θ_2$と平行になるように$θ_1$とも連動しています。
こういうの平行リンクっていうんでしょうか。
同様に余弦定理を用いて導出します。

${l_d}^2 = {L_1}^2 + {L_2}^2 – 2{L_1}{L_2}\cos\left({\pi-θ_2+θ_1}\right) = {L_1}^2 + {L_2}^2 + 2{L_1}{L_2}\cos\left({θ_2-θ_1}\right) $

$$θ_2 = \cos^{-1} \left(\frac{{l_d}^2-{L_1}^2 – {L_2}^2}{2{L_1}{L_2}} \right) + θ_1$$

以上より右前足の逆運動学の計算関数は以下のようになります。

サーボ角度指定の際に初期位置を補正するoffset[]も加算しています。

残り3本の足についても同様に関数を用意して制御します。

足移動

ここでは足移動の際に正弦波形で細かく座標移動を行いました。

時間分割による座標変化のコードは以下の書籍のサンプルコードを参考にしました。

 
書籍のサンプルコードは以下でダウンロードできます。
_710_humanoide_BNO055_KO_mega_walking.inoを参考にしました。
 https://www.ohmsha.co.jp/book/9784274222115/

例えば、足踏みの場合は以下のようになります。

periodで周期の1/4時間を指定して足の高さを正弦波で上下しています。

それぞれの動作も同様に正弦波で移動させています。

 

おわりに

コントローラにESP32を採用し、足を自由自在に制御できるようになったため
4足歩行についての理解が非常に深まりました。

本当に素晴らしいキットをいただいたことを幸せに思います。

ラズパイによる正規の制御の学習進めたいと思います!

あー楽しいね!

Related Posts

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください