SimpleFOC で倒立振子4
お父ちゃん
2024.10.26
0
0
ここ最近はSimpleFOCの勉強に凝っていて、先日もベクトル制御による倒立振子を楽しみました。
↑ここで非常に応答の良い倒立振子を実現できたので、更に検証を進めます。
目次
屈伸型倒立振子
SimpleFOCのベクトル制御によって小型で応答性がよいブラシレスモータユニットを手にしたので、前々からいつか作ってみたいと思っていた屈伸型倒立振子を作りたいと思います。
1脚をリンク機構によってサーボ 1個で動かす屈伸型倒立振子は以前にいくつか製作しました。
この屈伸型倒立振子は足が上下にのみ移動可能です。
ここではサーボ 2個ずつで足を構成して上下移動に更に前後移動も加えて豊かに動作する屈伸型倒立振子を目指します。
構成
屈伸型倒立振子の構成は以下のとおり
屈伸の足のためにシリアルサーボ STS3032を追加しました。
- Makerbase esp32デュアルsimpriefocシールド
![](http://ae01.alicdn.com/kf/S9a3b1087ac894b0d9c5a6d6f9b288b4aI.jpg_350x350.jpg")
- IMUセンサ MPU6050
-
I2Cマルチプレクサ TCA9548A
-
磁気エンコーダ AS5600
-
260KVブラシレスモータ
-
3セル 11.1V LiPoバッテリ
- サーボ用 降圧DCDCモジュール
- シリアルサーボ STS3032
機体製作
Makerbase社製コントローラの余剰ピンでシリアルサーボを動かせることを確認しました。
余剰ピンでサーボ動かして…またコイツなんかしようとしてるわ pic.twitter.com/k8mso5I37I
— HomeMadeGarbage (@H0meMadeGarbage) October 23, 2024
ESP32のGPIOを用いてFeetech製シリアルサーボを簡単に駆動できる以下のライブラリを使用しました。
サーボからのデータ受信はできませんがUART信号変換基板を使用せずに直接信号送信が可能です。
降圧DCDCをコントローラ裏に配置・配線してバッテリ電圧 11.1Vを7.4Vにしてサーボに給電します。
3Dプリントしたフレームを5節リンク状に組んで 1脚 2個のサーボで動かします。
あーあ 今 おしめ替えるから。。 pic.twitter.com/v1Bq2NMDB1
— HomeMadeGarbage (@H0meMadeGarbage) October 23, 2024
関節はPLAの滑りのみでざっくばらんにやってる pic.twitter.com/H6pJ6ykPa3
— HomeMadeGarbage (@H0meMadeGarbage) November 2, 2024
足先にモータをつけて配線します。
磁気エンコーダとブラシレスモータを治具で固定
エンコーダのI2Cラインをマルチプレクサに配線
機体完成！
逆運動学
5節リンクによる足での逆運動学は以前にも経験があり、今回もそちらを流用してサーボを動かします。
足の座標 (x, y) からサーボの角度 ($θ_1$, $θ_2$)を導出します。
片足の座標は以下で定義しました。
サーボのアームの長さは$L_1$、下肢の長さは$L_2$、サーボの軸間の長さを$l$とします。
前後のサーボの角度$θ_1$, $θ_2$は以下で導出されます。
$$θ_1=θ_a-θ_b$$
$$θ_2=θ_c-θ_d$$
$θ_a$, $θ_b$, $θ_c$, $θ_d$は三角比や余弦定理より以下で導出できます。
$θ_a = \tan^{-1} \left(\frac{y}{x-l/2} \right)$
${L_2}^2 = {L_1}^2 + {l_a}^2 – 2{L_1}{l_a}\cos{θ_b} $
$θ_b=\cos^{-1} \left(\frac{ {L_1}^2+ {l_a}^2- {L_2}^2}{2{L_1}{l_a}} \right)$
$π-θ_c = \tan^{-1} \left(\frac{y}{x+l/2} \right)$
$θ_c =π- \tan^{-1} \left(\frac{y}{x+l/2} \right)$
${L_2}^2 = {L_1}^2 + {l_b}^2 – 2{L_1}{l_b}\cos{θ_d} $
$θ_d=\cos^{-1} \left(\frac{ {L_1}^2+ {l_b}^2- {L_2}^2}{2{L_1}{l_b}} \right)$
よってサーボの角度$θ_1$, $θ_2$は足先の座標 (x, y)より以下で導出できる。
$$θ_1=\tan^{-1} \left(\frac{y}{x-l/2} \right)-\cos^{-1} \left(\frac{ {L_1}^2+ {l_a}^2- {L_2}^2}{2{L_1}{l_a}} \right)$$
$$θ_2=π- \tan^{-1} \left(\frac{y}{x+l/2} \right)-\cos^{-1} \left(\frac{ {L_1}^2+ {l_b}^2- {L_2}^2}{2{L_1}{l_b}} \right)$$
$$ 但し、l_a = \sqrt{\left(x-l/2\right)^2 + y^2}$$
$$ l_b = \sqrt{\left(x+l/2\right)^2 + y^2}$$
ちなみに今回の屈伸型倒立振子では$L_1 = 50$ mm、$L_2 = 60$ mmです。
動作
逆運動学で足の座標を指定してサーボを動作させつつ倒立を目指します。
足踏み・屈伸
いい感じ
倒立振子 pic.twitter.com/mrWkeWr2OZ
— HomeMadeGarbage (@H0meMadeGarbage) October 24, 2024
ジャンプ
ブラシレスモータの応答が良いのでジャンプ後の倒立も問題なし
機体幅詰
機体幅が広すぎるのでバッテリ位置を変えて、20 mmほど幅詰めしました。
機体姿勢制御
機体のロール軸やピッチ軸によって足を動かして姿勢を制御してみます。
ロール軸制御
機体ロール軸の傾きをフィードバックして左右の足高さをPD制御して機体姿勢を保持させてみました。
いいですね。モータが元気だからもち上げても倒立を維持しつつ姿勢保持してくれて面白い。
ピッチ軸制御
さらに機体ピッチ軸の傾きをフィードバックして足を前後させてみます。
ここにきてやっと1脚にサーボを2個使用する意味が見いだせる時が来ました。
以下の動画冒頭の急停止の時に足を前後させるのカッコよく、いつか自分でもやってみたいと思っていました。
3Dプリント二輪ジャンプロボットhttps://t.co/7gpTyly4Gb#3DPrinted #opensource #wheeled #jumping #robot #DIY #handmade #Kino_Robotics pic.twitter.com/plA2immKXP
— T.Yamazaki (@ZappyZappy7) June 13, 2024
恐らくピッチ軸制御で実現できるはずです。
ほらね。できた 😛 。
足を前後できるご利益ですね。これは嬉しい
あっち行っててば pic.twitter.com/LW6nXqT1fC
— HomeMadeGarbage (@H0meMadeGarbage) October 25, 2024
おわりに
ここではSimpleFOCによる屈伸型倒立振子の製作を実施しました。
1脚にサーボを2個使用するタイプに初めて挑戦し、足の前後動作もできるようになったのでカッコいいピッチ軸制御も実現できました。
当然 起き上がりもできるよ。
起上り pic.twitter.com/pqUR23rC00
— HomeMadeGarbage (@H0meMadeGarbage) October 26, 2024
さて足の前後移動をピッチ軸制御にだけ使用するのは実にもったいないですよね。
この構成での屈伸型倒立振子でやりたいことはまだまだございます。
引き続き実験・調査して報告させていただきますので、その際にはまた是非お越しください。
バイなら
