3Dプリンターでロボット作ってみる 多脚ロボット編11

something-make.hatenablog.com

このロボットの3Dデータは下のリンクのものを使用しています

www.thingiverse.com

ハードウェアが形になったのでソフトウェアをつくっていきます。

とりあえずの目標は、「スイッチを入れたらロボットが前進する」という動作ができることにします。

ロボットの歩かせ方で参考にしたのはこのサイト

makezine.jp

とりあえずこのサイトを参考にする場合に必要なのが運動学と逆運動学。ロボット系の書籍には大体書いてありますが、要は手足の先の位置と関節の角度の関係を表す方法です。

こういう図を見たことがある人も多いと思います。

関節が二つあるロボットの腕、又は脚をモデルにしたもので、

 $x=l_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)$

 $y=l_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)$

という数式で表すことができます。これをこのまま逆にしてxとyを入力すると関節の角度が出てくる方程式を作ってもいいのですが、結構ややこしいので今回は簡略化します。

右端が足先、左が脚の根元として、この問題は脚の根元から足先までの距離Lを求め、距離Lを実現するための角度θ1を求める問題になります。

足先までの距離Lは

 $L=\sqrt{x^2+y^2+z^2}$

で座標から簡単に求めることができます。

ここでLをl1,l2で表すと

 $L=\sqrt{{\left(l_1+l_2\cos {\theta }_2\right)}^2+{\left(l_2\sin {\theta }_2\right)}^2}$

 $=\sqrt{l_1^2+2l_1{l}_2\cos {\theta }_2+l_2^2{\cos }^2{\theta }_2+l_2^2{\sin }^2{\theta }_2}$

sinとcosが消えて

$L=\sqrt{l_1^2+l_2^2+2l_1{l}_2\cos {\theta }_2}$

これを変形して

 $\cos {\theta }_2=\frac{L^2-l_1^2-l_2^2}{2l_1{l}_2}$

足先の位置と脚の長さがあれば、角度を表せることがわかりました。あとはこれを逆三角関数に入力すれば角度を求めることができます。

ここまでやってさて実装してみようとArduinoIDEを開いたのですが、重大な問題に気付きました。Arduinoにはアークコサインが実装されていない。

Arduino 日本語リファレンス

通常の三角関数はあるのですが、逆三角関数が無いんですね。

無いものは作るしかありません。

cos関数は実装されているので数値を比較することで段々目的の角度に近づけるアルゴリズムをわざわざ実装してつくりました。

Acos 0.5 = 60deg
top 0.00 , buttom 180.00
top 0.00 , buttom 90.00
top 45.00 , buttom 90.00
top 45.00 , buttom 67.50
top 56.25 , buttom 67.50
top 56.25 , buttom 61.88
top 59.06 , buttom 61.88
top 59.06 , buttom 60.47
top 59.77 , buttom 60.47
top 59.77 , buttom 60.12
top 59.94 , buttom 60.12
top 59.94 , buttom 60.03
top 59.99 , buttom 60.03
top 59.99 , buttom 60.01
top 60.00 , buttom 60.01
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
top 60.00 , buttom 60.00
60.00deg

結構計算を無駄にやっている感じがしますが、実際に答えを出力してくれるようになったので問題ありません。

この関数ができた後、ふと数学は専用のヘッダーファイルとかがあるのでは？という考えにようやく至り、検索してみたした。

Arduino - H

ありましたアークタンジェント

これがあるならアークサインもアークコサインも出せますね。

結局ほぼ丸一日棒に振ってしまいました。