ぱたへね

はてなダイアリーはrustの色分けができないのでこっちに来た

Modern Robotics: Mechanics, Planning, and ControlのForward Kinematics その1

教科書から分かったところだけをまとめています。

教科書はこの本です。今回はForward Kinematics。

www.amazon.co.jp

教科書p116の図1より。

f:id:natsutan:20211211235538p:plain
Figure 4.1

それぞれのθから手の先の(x,y)を計算するのが、Forward Kinematics です。

図から頑張って追えばこのような関係が導き出されます。

x=L_1\cos\theta_1+L_2\cos(\theta_1+\theta_2)+L_3 \cos(\theta_1+\theta_2+\theta_3)
y=L_1\sin\theta_1+L_2\sin(\theta_1+\theta_2)+L_3 \sin(\theta_1+\theta_2+\theta_3)
\phi=\theta_1+\theta_2+\theta_3

これをもっとシステマティックに求めたい。

フレームを変換する行列Tを使うと。

T_{04} = T_{01}T_{12}T_{23}T_{34}

それぞれの行列はこうなります。今までと同じく左上の3x3が座標軸の回転、右側の3つが並行移動です。

T_{01} =  \begin{bmatrix} \cos \theta_1 & -\sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{12} =  \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & L_1 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{23} =  \begin{bmatrix} \cos \theta_3 & -\sin \theta_3 & 0 & L_2 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 
T_{34} =  \begin{bmatrix} 1 & -0 & 0 & L3 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

全部計算する根性が無いので、最初の2つだけ計算してみました。

T_{01}T_{12} =  \begin{bmatrix} \cos \theta_1 & -\sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix}   \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & L_1 \\ \sin \theta_2 & \cos \theta_2 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} =  
 \begin{bmatrix} \cos \theta_1 \cos \theta_2 - \sin \theta_1 \sin \theta_2 & -\cos \theta_1 \sin \theta_2 + \sin \theta_1 \cos \theta_2 & 0 & L_1 \cos \theta_1 \\ \sin \theta_1 \cos \theta_2 - \cos \theta_1 \sin \theta_2 & -\sin \theta_1 \sin \theta_2 + \cos \theta_1 \cos \theta_2  & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

受検で勉強したsinとcosの加算公式(sin(x+y) = sin(x)cos(y)+cos(x)sin(y))を使うとこうなります。

T_{01}T_{12} =  \begin{bmatrix} \cos (\theta_1 + \theta_2) & -\sin (\theta_1 + \theta_2) & 0 & L_1 \cos \theta_1 \\ \sin (\theta_1 + \theta_2) & \cos (\theta_1 + \theta_2) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1  \end{bmatrix} 

上手く合成されてますね。