ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御
お父ちゃん
2022.2.11
0
0
前回は足を自由に動かすための準備として、ラズパイ4のOS吟味等実施しました。
ここではさらにカスタマイズを進めて足を逆運動学で自由自在に制御できるようになりましたので報告いたします。
目次
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モジュールを使用しました。
いつもの組み合わせなので簡単にサーボを制御できます。
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本に展開して
歩行の学習に入りたいと思います。
実に楽しみです！
