足を逆運動学で制御 -シリアルサーボと距離を詰めたい俺5-
お父ちゃん
2022.1.10
0
0
前回はシリアルサーボ (KRS3301)を用いて製作した足で足踏み動作を実現しました。。
ここでは歩行の実現に向けて、足を逆運動学による座標指定制御を目指しましたので報告いたします。
以前、逆運動学でロボットアームを制御した経験が大変役に立ちました。
目次
足の座標
足の座標は以下のように定義しました。
前方を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()は以下のようにしました。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
float L1=38,L2=53,L3=53,L4=60;//unit:mm void ik(float x,float y,float z){ float phi,ld; ld=sqrt(x*x + (z-L1-L4)*(z-L1-L4)); phi=atan2(x,(z-L1-L4)); th0 = asin(y /(2 * (L1 + L2))); th1 = phi + acos(ld /(2 * L2)); th2 = acos((ld*ld - L2*L2 - L3*L3)/(2*L2*L3)); thd0 = th0 * 180.0 / PI; thd1 = th1 * 180.0 / PI; thd2 = th2 * 180.0 / PI; } |
動作
逆運動学で各種動作を確認しました。
制御マイコンESP32にスマホアプリBlynkを用いてBLE通信で座標を指定します。
重心移動
Y軸の移動距離を指定 (y: ±50mm)
逆運動学で重心移動#Blynk pic.twitter.com/AuAptBppWg
— HomeMadeGarbage (@H0meMadeGarbage) January 9, 2022
屈伸
Z軸の移動距離を指定 (z: 0~30mm)
逆運動学で屈伸
高さ移動量を指定#Blynk pic.twitter.com/YCFOw4H5s7— HomeMadeGarbage (@H0meMadeGarbage) January 9, 2022
足移動
X軸とZ軸の座標を指定 (x: ±35mm, z: 0~30mm)
逆運動学で足移動#Blynk pic.twitter.com/WXeUHqS3UX
— HomeMadeGarbage (@H0meMadeGarbage) January 9, 2022
おわりに
ここでは製作した足を逆運動学で制御してみました。
各足を任意の座標にほぼ動かせるようになったので、次回はいよいよ歩行に挑戦です。
頑張ります！
