ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御

Home > 電子工作 > ロボット犬『Mini Pupperミニぷぱ』逆運動学で足制御
すき 0
うんこ 0

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

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

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

 

 

PCA9685検証

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

 

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

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

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

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

 

ESP32とPCA9685

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

 

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

 

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

 

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

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

 

逆運動学で足を制御

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

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

 

腿の長さ$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軸)位置を指定し、サーボ角を導出して制御します。

 

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

 

おわりに

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

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

実に楽しみです!

次の記事

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

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

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