足を逆運動学で制御 -シリアルサーボと距離を詰めたい俺5-

Home > 電子工作 > 足を逆運動学で制御 -シリアルサーボと距離を詰めたい俺5-
すき 0
うんこ 0

前回はシリアルサーボ (KRS3301)を用いて製作した足で足踏み動作を実現しました。。

シリアルサーボで足踏み -シリアルサーボと距離を詰めたい俺4-

ここでは歩行の実現に向けて、足を逆運動学による座標指定制御を目指しましたので報告いたします。

以前、逆運動学でロボットアームを制御した経験が大変役に立ちました。

逆運動学 で制御 ーロボットアーム自作への道2ー

 

 

足の座標

足の座標は以下のように定義しました。
前方をX軸、右足側をY軸、高さ方向をZ軸とします。

 

足の長さは以下の通りです。

 

逆運動学

足先の座標 (x, y, z)から各サーボの角度を導出します。

Y軸方向

Y軸方向へは足の一番上と一番下のサーボを制御して移動させます(重心移動)。
一番上と一番下のサーボの角度$θ_0$を同一にすることで足底は天板と常に平行を保つことができます。

移動量$y$から角度$θ_0$は以下で導出できます。
ここでは膝曲げによる足長の変化は加味していません。
(おそらくここ近似しても今後の歩行には大きく影響しないはず)

$$θ_0 = \sin^{-1} \left(\frac{y}{L1+L2+L3+L1} \right)$$

X-Z方向

足を上げて(Z軸)、前後(X軸)するためのサーボ角度は以下で導出されます。

サーボの角度$θ_1$は余弦定理を用いて導出します。ちなみにL2 = L3。

${L_3}^2 = {L_2}^2 + {l_d}^2 – 2{L_2}{l_d}\cos{θ_1}’  $

${θ_1}’ = \cos^{-1} \left(\frac{l_d}{2{L_2}} \right)$

$$θ_1 = φ + {θ_1}’  = φ +\cos^{-1} \left(\frac{l_d}{2{L_2}} \right) $$

$$      但し、φ = \tan^{-1} \left(\frac{x}{z – L_1 – L_4} \right)$$

$$       l_d = \sqrt{x^2 + \left(z – L_1 – L_4\right)^2}$$

 

膝のサーボの角度$θ_2$も余弦定理を用いて導出します。

${l_d}^2 = {L_2}^2 + {L_3}^2 – 2{L_2}{L_3}\cos{θ_2}’ = {L_2}^2 + {L_3}^2 – 2{L_2}{L_3}\cos\left({\pi-θ_2}\right) = {L_2}^2 + {L_3}^2 – 2{L_2}{L_3}\cos{θ_2}$

$$θ_2 = \cos^{-1} \left(\frac{{l_d}^2-{L_2}^2 – {L_3}^2}{2{L_2}{L_3}} \right)$$

 

足首のサーボの角度$θ_3$は足を平行にするために

$$θ_3 =θ_2-θ_1$$

 

プログラム

逆運動学によるサーボを制御するプログラムは以下の書籍のサンプルコードを参考にしました。

サンプルコード ダウンロード先: https://www.ohmsha.co.jp/book/9784274222115/

 
サンプルコードを製作した足用に調整して使用しました。
逆運動学の関数ik()は以下のようにしました。

 

動作

逆運動学で各種動作を確認しました。

制御マイコンESP32にスマホアプリBlynkを用いてBLE通信で座標を指定します。

重心移動

Y軸の移動距離を指定 (y: ±50mm)

屈伸

Z軸の移動距離を指定 (z: 0~30mm)

足移動

X軸とZ軸の座標を指定 (x: ±35mm, z: 0~30mm)

 

おわりに

ここでは製作した足を逆運動学で制御してみました。

各足を任意の座標にほぼ動かせるようになったので、次回はいよいよ歩行に挑戦です。

頑張ります!

次の記事

歩行検討1 -シリアルサーボと距離を詰めたい俺6-

コメントはこちらから

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

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