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

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

電源コードレス化

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

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

構成

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

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

部品

ESP32 開発ボード

ＥＳＰ−ＷＲＯＯＭ−３２Ｄ開発ボード: 無線、高周波関連商品秋月電子通商-電子部品・ネット通販
akizukidenshi.com
PCA9685モジュール
5V DCDCコンバータ

超高効率ＤＣ−ＤＣコンバーター　５Ｖ１Ａ　Ｍ７８ＡＲ０５−１: 電源一般秋月電子通商-電子部品・ネット通販
akizukidenshi.com

動作

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

電源を５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個のサーボを制御しました。

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 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$$

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

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]);
}

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/

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

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));
  }
}

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を採用し、足を自由自在に制御できるようになったため
４足歩行についての理解が非常に深まりました。

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

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

あー楽しいね！

ロボット犬『Mini Pupperミニぷぱ』を ATOM Matrix で堪能

コメントはこちらからコメントをキャンセル

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

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

電源コードレス化

構成

部品

動作

歩行検討

足踏み

歩行

Arduinoコード

Blynk

PCA9685

逆運動学

足移動

おわりに

次の記事

コメントはこちらからコメントをキャンセル

電源コードレス化

構成

部品

動作

歩行検討

足踏み

歩行

Arduinoコード

Blynk

PCA9685

逆運動学

足移動

おわりに

次の記事

関連記事

コメントはこちらから コメントをキャンセル

コメントはこちらからコメントをキャンセル