2軸ロボットアームの順運動学を座標変換できちんと変換する

初めに

このような2軸のロボットアームを考える時に以前順運動学をpythonで実装したときロボットアームの先端の座標を

\begin{bmatrix}
x\\y
\end{bmatrix}
=
\begin{bmatrix}
L_1 \times cos(\theta_1) + L_2 \times cos(\theta_2) \\
L_1 \times sin(\theta_1) + L_2 \times sin(\theta_2)
\end{bmatrix}

\\このときL_1,L_2はアームの長さを表す．\theta_1,\theta_2はアームの角度を表す．

と表していましたが，実際にロボットアームの運動学を解くときには，座標変換を行って求めるものだと思いました．ということで今回は自分で導出してみたいと思います．

事前知識

回転移動\\
\begin{bmatrix}
x'\\y'\\1
\end{bmatrix}
=
\begin{bmatrix} 
cos(\theta) & -sin(\theta) & 0 \\
sin(\theta) & cos(\theta) & 0 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix}
x\\y\\1
\end{bmatrix}
\\平行移動\\
\begin{bmatrix}
x'\\y'\\1
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 & x_1 \\
0 & 1 & y_1 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix}
x'\\y'\\1
\end{bmatrix}
\\x_1,x_2はそれぞれの軸の方向への移動量.
\\x',y'が移動後の座標，x,yが移動前の座標.

行列は掛け算の順序によって結果が変化するのでかける順番が重要です．
平行移動させてから回転移動させたい場合は，
移動後の行列 = 回転行列＊平行行列＊移動前の行列
の順序で行う必要が有ります．
掛け算の順序が逆になる理由は，合成写像の分野を勉強すれば理由が分かると思います．(私は線形代数を学んだことがないので断定はできませんが)

実装

この方が2次元の座標変換をしてくださっていたので答え合わせの意味で確認しました．

アームの長さは平行移動を用いて表しました．以下の図のようにx軸方向にL1,L2だけx軸方向に平行移動させていることが分かります．

以上からアームの先端の座標x',y'は

\begin{bmatrix}
x'\\y'\\1
\end{bmatrix}
=
\begin{bmatrix} 
cos(\theta_1) & -sin(\theta_1) & 0 \\
sin(\theta_1) & cos(\theta_1) & 0 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix}
1 & 0 & L_1 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix} 
cos(\theta_2) & -sin(\theta_2) & 0 \\
sin(\theta_2) & cos(\theta_2) & 0 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix}
1 & 0 & L_2 \\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
\bullet
\begin{bmatrix}
0\\0\\1
\end{bmatrix}
\\移動前の座標は即ち原点なので最後の項の移動前の座標x=0,y=0

これを頑張って計算すると

\begin{bmatrix}
x'\\y'\\1
\end{bmatrix}
=
\begin{bmatrix}
L_1cos\theta_1+L_2(cos\theta_1cos\theta_2-sin\theta_1sin\theta_2)\\
L_1sin\theta_1+L_2(cos\theta_1sin\theta_2+sin\theta_1cos\theta_2) 
\\1
\end{bmatrix}

となります．
以前の実装と計算結果は同じになります．

最後に

実際に実装したものをcolabに上げておきました．

参考