Inverse Kinematics on python
(
【概要】 Python_IK_2Dは2次元での2リンク、2軸の逆運動学の計算プログラムです。 Python_IK_3Dは3次元での2リンク、3軸の逆運動学の計算プログラムです。 Python_IK_3DとservoTest.inoは互いに連携して、3つのサーボを用いたロボットアームを目標座標に動作させるプログラムです。
【使用方法】 それぞれArmの長さ、目標座標を入力すると解が出力されます。
【計算】 以下の図のような計算をしています。(以下の図は二次元です) 前提条件として、Arm1 = Arm2、赤色の線がElbow Downの解、青色の線がElbow Upの解となっています。 また、原点(0,0)からTargetまでの距離をdistanceとします。
(Ⅰ)Target(x₁,y₁)からdistanceを求め、さらにtanθ4を求めます。
(Ⅱ)薄い赤色の直角三角形に着目すると、辺の長さからcosθ2が求められます。
(Ⅲ)求めたtanθ3、cosθ2から、Pythonのmathモジュール内の逆三角関数(atan、acos)でθ2とθ3を求めることができます。
(Ⅳ)あとは [Elbow Up] θ1 = θ3 + θ4 , θ2 = -2 * θ3
[Elbow Down] θ1 = θ4 - θ3 , θ2 = 2 * θ3 このように求めて終了です。
(注)このプログラムにおいて目標座標のy座標が負の時は一度yを正にしてから第一象限で計算し、最終的に符号を逆にして出力しています。 またこのときElbow UpとElbow Downは逆になります。
また、Python_IK_3DとservoTest.inoを使用しロボットアームを動かすプログラムにおいて回路図は以下のようにしました。
(注)電源が3Vになっていますが実際は5Vの電源装置を使用しています。
