三自由度ロボットアーム

説明

根本の第一関節がyaw角方向に回る軸で、中間の関節と先端の関節はpitch角方向に回る軸という三自由度ロボットアームについての運動学モデル等をまとめました

間違っているかもしれないし、もっと簡略化できるかもしれないとは思ったのですが、面倒だったのでやめました

※ 図中の記号は$\alpha\Rightarrow\xi \qquad \beta\Rightarrow\eta$であることに注意 根本の関節から先端の関節の順で、 各関節間の長さを$l_{1}$,$l_{2}$,$l_{3}$とし、 各関節の角度を$\theta_{1}$,$\theta_{2}$,$\theta_{3}$とし、 各変数を $\xi = \sqrt{x^2 + y^2} \qquad \eta = z - l_{1} \qquad$ $a = l_{2}cos\theta_{2} + c \qquad b = l_{2}sin\theta_{2} + d \qquad c = l_{3}cos(\theta_{2} + \theta_{3}) \qquad d = l_{3}sin(\theta_{2} + \theta_{3})$ と定義すると、結果は下のようになるはずです

位置における順運動学モデル

$$\Large \begin{aligned} x &= acos\theta_{1} \\ y &= asin\theta_{1} \\ z &= b + l_{1} \end{aligned}$$

位置における逆運動学モデル

$$\large \begin{aligned} \theta_{1} &= tan^{-1}\left(\frac{y}{x}\right) \\ \theta_{2} &= tan^{-1}\left(\frac{\eta}{\xi}\right) - tan^{-1}\left(\frac{l_{3}sin\theta_{3}}{l_{2} + l_{3}cos\theta_{3}}\right) \\ \theta_{3} &= \pm cos^{-1}\left(\frac{\xi^2 + \eta^2 - l_{2}^2 - l_{3}^2}{2l_{2}l_{3}}\right) \end{aligned}$$

ヤコビ行列による速度の順運動学

$$\Large \left(\begin{array}{ccc} \frac{dx}{dt} \\ \frac{dy}{dt} \\ \frac{dz}{dt} \end{array}\right) = \left(\begin{array}{ccc} -asin\theta_{1} & -bcos\theta_{1} & -dcos\theta_{1} \\ acos\theta_{1} & -bsin\theta_{1} & -dsin\theta_{1} \\ 0 & a & c \end{array}\right) \left(\begin{array}{ccc} \frac{d\theta_{1}}{dt} \\ \frac{d\theta_{2}}{dt} \\ \frac{d\theta_{3}}{dt} \end{array}\right)$$

逆ヤコビ行列による速度の逆運動学

$$\Large \left(\begin{array}{ccc} \frac{d\theta_{1}}{dt} \\ \frac{d\theta_{2}}{dt} \\ \frac{d\theta_{3}}{dt} \end{array}\right) = \left(\begin{array}{ccc} -\frac{sin\theta_{1}}{a} & \frac{cos\theta_{1}}{a} & 0 \\ \frac{ccos\theta_{1}}{ad-bc} & \frac{csin\theta_{1}}{ad-bc} & \frac{d}{ad-bc} \\ -\frac{acos\theta_{1}}{ad-bc} & -\frac{aain\theta_{1}}{ad-bc} & -\frac{b}{ad-bc} \end{array}\right) \left(\begin{array}{ccc} \frac{dx}{dt} \\ \frac{dy}{dt} \\ \frac{dz}{dt} \end{array}\right)$$

参考文献

http://www.ecei.tohoku.ac.jp/hariyama/lecture/robot/robot01.pdf