0

0

前回は足を自由に動かすための準備として、ラズパイ4のOS吟味等実施しました。

ロボット犬『Mini Pupperミニぷぱ』カスタマイズ下準備

ここではさらにカスタマイズを進めて足を逆運動学で自由自在に制御できるようになりましたので報告いたします。

PCA9685検証

専用基板にはサーボドライバPCA9685が載っており、ラズパイからI2Cでサーボを制御していることが前回わかりました。

PCA9685つこてまんな pic.twitter.com/HHTdtfnXBw

— HomeMadeGarbage (@H0meMadeGarbage) February 9, 2022

そこでラズパイ4に専用基板を載せて、Node-REDからPCA9685を制御してサーボを動かしてみようと考えました。

PCA9685用Node-REDノードは以下を使用
　https://flows.nodered.org/node/node-red-contrib-pca9685

しかし、専用基板からバッテリを接続して起動しましたが、サーボの電源には電圧がフルで共有されず動かすことができませんでした。。。

ロードスイッチかDCDCのイネーブルピンがありそうだな

単純にサーボに電源供給できひん
#MiniPupper #ミニぷぱ pic.twitter.com/2W0A3gpFbQ

— HomeMadeGarbage (@H0meMadeGarbage) February 9, 2022

恐らく、専用基板にロードスイッチかDCDCのイネーブルピンなどの起動制御があるのだと思いますが、
現状 回路図が未公開なのでラズパイによるサーボ制御は一旦保留とします。

ESP32とPCA9685

こうなったら一旦ラズパイ4は忘れて、慣れ親しんだESP32でPCA9685を制御しちゃいましょう。

公式で回路公開されるまで保留だな

次行こ次 pic.twitter.com/aHsfcFJZf9

— HomeMadeGarbage (@H0meMadeGarbage) February 9, 2022

ESP32開発ボードと以下のPCA9685モジュールを使用しました。

VKLSVAN 2個 PCA9685 16チャンネル 12-ビット PWM Servo モーター ドライバー IIC モジュール Arduinoと互換

• ShenZhen HuaRen ChuangYi Science and Technology Ltd .
• 価格￥1,799(2025/07/14 17:19時点)
• 商品ランキング35,121位

いつもの組み合わせなので簡単にサーボを制御できます。
BlynkをつかってBLEでスマホから制御しています。

慣れ親しんだ構成
接しやすぅ#ESP32 #PCA9685 #Blynk #サーボモータ pic.twitter.com/Fc8Fdgoa7U

— HomeMadeGarbage (@H0meMadeGarbage) February 9, 2022

ミニぷぱの右前足のサーボ3個を繋げてみました。

憧れの足 自由自在

#MiniPupper #ミニぷぱ #Blynk pic.twitter.com/o86jkRCiIO

— HomeMadeGarbage (@H0meMadeGarbage) February 9, 2022

それぞれのサーボの角度を指定して自由に動かすことができるようになりました。

逆運動学で足を制御

右前足を位置座標を指定して動かせるようにします。

座標は腿のつけ根を原点として以下で定義します。

腿の長さ$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$$

以上で 足のロール角$θ_0$と足の前後(X軸)位置と上下(Z軸)位置の指定で足のサーボを制御できるようになりました。

動作

実際導出した式で足を動かしてみます。

スマホから前後(X軸)位置と上下(Z軸)位置を指定し、サーボ角を導出して制御します。

逆運動学でガッシガシ#MiniPupper #ミニぷぱ #Blynk #ESP32 pic.twitter.com/GpvaZXOggo

— HomeMadeGarbage (@H0meMadeGarbage) February 10, 2022

ロール角を動かしても前後(X軸)位置と上下(Z軸)位置は保たれています。

ロールしたって
前後位置と高さ保持#MiniPupper #ミニぷぱ #Blynk #ESP32 pic.twitter.com/qq4VrOFCrU

— HomeMadeGarbage (@H0meMadeGarbage) February 10, 2022

おわりに

ラズパイ4と専用基板での制御を一旦諦め、邪道ですがESP32での足制御を検討しました。

逆運動学でやっと自由に動かせるようになったので、コレを4本に展開して
歩行の学習に入りたいと思います。

実に楽しみです！

次の記事

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

関連記事

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