ロボット犬『Mini Pupperミニぷぱ』ESP32で歩行動作検討2
お父ちゃん
2022.2.12
0
0
前回はESP32で４本の足の制御を実現し４足歩行を学びました。
ここでは更に深堀します。
目次
電源コードレス化
前回は5V ACアダプタから電源供給していましたが、ミニぷぱのバッテリを使用してコードレス化を目指しました。
ミニぷぱには2セルの7.4V LiPoバッテリが搭載されています。
構成
LiPoバッテリからPCA9685モジュールのサーボ電源 (V+) と5V DCDCコンバータに給電し、
PCA9685モジュールのロジック電源 (VCC) とESP32にはDCDCの5V出力で給電します。
相変わらずのブレッドボード。。
部品
- ESP32 開発ボード
- PCA9685モジュール
- 5V DCDCコンバータ
動作
電源コードがなくなったので、これでどこにでも行けます。
電源を５V ACアダプタから ミニぷぱのバッテリに変更
コレで外にお散歩行ける#MiniPupper #ミニぷぱ #Blynk #ESP32 pic.twitter.com/9vGe4jjFWt
— HomeMadeGarbage (@H0meMadeGarbage) February 12, 2022
本当に散歩してきました。かわいい。
毎日したい
たとえ無理とわかっていても、試してみたくなるのが人情というもの pic.twitter.com/23ICu0Y065
— HomeMadeGarbage (@H0meMadeGarbage) February 12, 2022
歩行検討
これまでは足の移動を座標指定しサーボが最速で動ける速度で直線的に動かして
遅延時間を設けて動作周期を変えていました。
ここでは足を細かく時間分割して正弦波形で動かすようにしてみました。
正弦波で足を動かすことによって足の方向転換時の加速度を低減を狙っています。
足踏み
足の高さを正弦波形で変えます。
足踏み
足の高さ ２点指定してリニアに変えたバージョンと
正弦波で変えたバージョン気のせいかもしれないけど正弦波のほう機体のブレが小さい気がする
歩行に応用してみましょうね。#MiniPupper #ミニぷぱ #Blynk #ESP32 pic.twitter.com/i1l5Enlit7
— HomeMadeGarbage (@H0meMadeGarbage) February 12, 2022
線型的に足を移動して遅延で周期を設けた場合と正弦波形での移動では
正弦波のほうが機体のブレが小さくなりました。
歩行
方向時の足の位置やロール角も正弦波形で動かしてみました。
正弦波での細やかな移動制御
周期短いとご利益ないけど
ゆっくり歩行時に優しさが顕著に表れる但しリニアにチャキチャキ動かしたほうが
小型犬ぽさはある#MiniPupper #ミニぷぱ #Blynk #ESP32 pic.twitter.com/imBLEATB7F— HomeMadeGarbage (@H0meMadeGarbage) February 12, 2022
動作周期が短い場合にはさほど効果を感じませんでしたが
周期を長くするとマイルドな足移動が効いてきました。
正弦波形で動かしているというよりは時間分割で細かく座標していることが大きく効いたようです。
しかし、無駄な加速度は機体のブレに繋がるので正弦波でオールオッケーということにいたします。
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個のサーボを制御しました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
PCA9685 pwm = PCA9685(0x40); //PCA9685のアドレス指定（アドレスジャンパ未接続時） #define SERVOMIN 130 //最小パルス幅 (標準的なサーボパルスに設定) #define SERVOMAX 640 //最大パルス幅 (標準的なサーボパルスに設定) void setup() { pwm.begin(); //初期設定 (アドレス0x40用) pwm.setPWMFreq(60); //PWM周期を60Hzに設定 (アドレス0x40用) } void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定 ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（130～640）に変換 pwm.setPWM(ch, 0, ang); } |
逆運動学
足の移動は以前検証した通り、足のロール角と足の前後位置と上下位置を指定して、
逆運動学によってロール角以外のサーボの角度を算出します。
得られたサーボ角を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$$
以上より右前足の逆運動学の計算関数は以下のようになります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 |
void fRIK(float x,float th0,float z){ float phi,ld, th1, th2; float zd = z / cos(th0/180.0 * PI); ld=sqrt(x*x + zd*zd); phi=atan2(x, zd); th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld)); th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1; servo_write(1, th0 + offset[1]); servo_write(2, th1 * 180.0 / PI + offset[2]); servo_write(3, th2 * 180.0 / PI + offset[3]); } |
サーボ角度指定の際に初期位置を補正するoffset[]も加算しています。
残り３本の足についても同様に関数を用意して制御します。
足移動
ここでは足移動の際に正弦波形で細かく座標移動を行いました。
時間分割による座標変化のコードは以下の書籍のサンプルコードを参考にしました。
書籍のサンプルコードは以下でダウンロードできます。
_710_humanoide_BNO055_KO_mega_walking.inoを参考にしました。
https://www.ohmsha.co.jp/book/9784274222115/
例えば、足踏みの場合は以下のようになります。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 |
if(Step){ //足踏み float tim,tt; unsigned long time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(fRx, fRthd0, fRz - walkHeight * sin(tt)); rLIK(rLx, rLthd0, rLz - walkHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); fRIK(fRx, fRthd0, fRz - walkHeight * cos(tt)); rLIK(rLx, rLthd0, rLz - walkHeight * cos(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); rRIK(rRx, rRthd0, rRz - walkHeight * sin(tt)); fLIK(fLx, fLthd0, fLz - walkHeight * sin(tt)); } time_mSt= millis(); tim=0; while(tim<period){ tim=millis()-time_mSt; tt=float(tim * PI / 2 / period); rRIK(rRx, rRthd0, rRz - walkHeight * cos(tt)); fLIK(fLx, fLthd0, fLz - walkHeight * cos(tt)); } } |
periodで周期の1/4時間を指定して足の高さを正弦波で上下しています。
それぞれの動作も同様に正弦波で移動させています。
おわりに
コントローラにESP32を採用し、足を自由自在に制御できるようになったため
４足歩行についての理解が非常に深まりました。
本当に素晴らしいキットをいただいたことを幸せに思います。
ラズパイによる正規の制御の学習進めたいと思います！
あー楽しいね！
