前回はESP32で４本の足の制御を実現し４足歩行を学びました。
ここでは更に深堀します。
ロボット犬『Mini Pupperミニぷぱ』がやってきた
電源コードレス化
前回は5V ACアダプタから電源供給していましたが、ミニぷぱのバッテリを使用してコードレス化を目指しました。
ミニぷぱには2セルの7.4V LiPoバッテリが搭載されています。
構成
LiPoバッテリからPCA9685モジュールのサーボ電源 (V+) と5V DCDCコンバータに給電し、
PCA9685モジュールのロジック電源 (VCC) とESP32にはDCDCの5V出力で給電します。
相変わらずのブレッドボード。。
部品
- ESP32 開発ボード
[bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
- PCA9685モジュール
[amazonjs asin=”B07SLRG5J1″ locale=”JP” title=”VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換”]
- 5V DCDCコンバータ
[bc url=”https://akizukidenshi.com/catalog/g/gM-13536/”]
動作
電源コードがなくなったので、これでどこにでも行けます。
本当に散歩してきました。かわいい。
毎日したい
歩行検討
これまでは足の移動を座標指定しサーボが最速で動ける速度で直線的に動かして
遅延時間を設けて動作周期を変えていました。
ここでは足を細かく時間分割して正弦波形で動かすようにしてみました。
正弦波で足を動かすことによって足の方向転換時の加速度を低減を狙っています。
足踏み
足の高さを正弦波形で変えます。
線型的に足を移動して遅延で周期を設けた場合と正弦波形での移動では
正弦波のほうが機体のブレが小さくなりました。
歩行
方向時の足の位置やロール角も正弦波形で動かしてみました。
動作周期が短い場合にはさほど効果を感じませんでしたが
周期を長くするとマイルドな足移動が効いてきました。
正弦波形で動かしているというよりは時間分割で細かく座標していることが大きく効いたようです。
しかし、無駄な加速度は機体のブレに繋がるので正弦波でオールオッケーということにいたします。
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用のサーボ制御関数 [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]);
}
サーボ角度指定の際に初期位置を補正するoffset[]も加算しています。
残り３本の足についても同様に関数を用意して制御します。
足移動
ここでは足移動の際に正弦波形で細かく座標移動を行いました。
時間分割による座標変化のコードは以下の書籍のサンプルコードを参考にしました。
[amazonjs asin=”B07CXW1LVD” locale=”JP” title=”ROBO-ONEにチャレンジ！ 二足歩行ロボット自作ガイド”]
書籍のサンプルコードは以下でダウンロードできます。
_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));
}
}
periodで周期の1/4時間を指定して足の高さを正弦波で上下しています。
それぞれの動作も同様に正弦波で移動させています。
おわりに
コントローラにESP32を採用し、足を自由自在に制御できるようになったため
４足歩行についての理解が非常に深まりました。
本当に素晴らしいキットをいただいたことを幸せに思います。
ラズパイによる正規の制御の学習進めたいと思います！
あー楽しいね！
次の記事
ロボット犬『Mini Pupperミニぷぱ』を ATOM Matrix で堪能
