はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
全記事では，干渉物が存在する環境下で，3軸ロボットアームの経路生成を実行した．
(https://qiita.com/haruhiro1020/items/6994a94efaff98d8dae1)
本記事では，同時変換行列を使用した順運動学(ロボットの関節角度からロボットの手先位置を算出する)を説明する．
次記事で，6軸ロボットアームの順運動学を算出するために，本記事で同時変換行列の説明をする．
同時変換行列に関しては，後ほど説明する．
また，以前に2軸ロボットアームと3軸ロボットアームの順運動学を説明した．
(2軸ロボットアームの順運動学：https://qiita.com/haruhiro1020/items/4529e648161beac7754f
3軸ロボットアームの順運動学：https://qiita.com/haruhiro1020/items/27c0cf098056dc792ab9)
本記事では，同時変換行列を使用して，2軸ロボットアームと3軸ロボットアームの順運動学を算出して，以前説明した順運動学と一致することを確認する．
本記事では，drawioという作図ツールを使用している．使用方法は下記を参考とした．
(https://qiita.com/G-awa/items/8fd414700b68b2bcafcc)
また，図の中に数式を記載している．数式を綺麗に記載するために，latexを使用している．drawioにlatexを使用する方法は下記を参考とした．
(・https://qiita.com/takahashi-ry/items/cd085391118396b95537
・https://sogog.hatenablog.com/entry/2022/06/06/232723)
本記事で実装すること
・同時変換行列を使用して，2軸ロボットアーム，3軸ロボットアームの順運動学を算出する．
本記事では実装できないこと (将来実装したい内容)
・6軸ロボットアームの順運動学
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
絶対位置と相対位置
位置には絶対位置と相対位置が存在する．
絶対位置とは下図の中で，ワールド座標系$\sum$から見た点$a$の位置を指す．
相対位置とは下図の中で，関節${i+1}$の座標系$\sum_{i+1}$から見た点$a$の位置(${}^{i+1}\vec a$)の位置を指す(要するにワールド座標系以外からの座標系から見た点$a$の位置)．
ロボットアームでは，絶対位置を算出したい．
関節${i+1}$の座標系$\sum_{i+1}$から見た点$a$の位置を絶対位置に変換する必要がある．
はじめから絶対位置を算出するのは困難なため，はじめに，関節${i}$の座標系$\sum_{i}$から見た点$a$の位置(${}^{i}\vec a$)を算出することを考える．
\displaylines{
{}^{i}\vec a = {}^{i}\vec p_{i+1} + {}^{i}R_{i+1} * {}^{i+1}\vec a \\
{}^{i}\vec a ... 関節{i}から見た点aの位置 (x, y, z) \\
{}^{i}\vec p_{i+1} ... 関節{i}から見た関節{i+1}の位置 (x, y, z) \\
{}^{i}R_{i+1} ... 関節{i}から見た関節{i+1}の回転行列 (3 * 3行列) \\
{}^{i+1}\vec a ... 関節{i+1}から見た点aの位置 (x, y, z) \\
}
次に，関節${i-1}$の座標系$\sum_{i-1}$から見た点$a$の位置(${}^{i-1}\vec a$)を算出することを考える．
\displaylines{
{}^{i-1}\vec a = {}^{i-1}\vec p_{i} + {}^{i-1}R_{i} * {}^{i}\vec a \\
{}^{i-1}\vec a = {}^{i-1}\vec p_{i} + {}^{i-1}R_{i} * ({}^{i}\vec p_{i+1} + {}^{i}R_{i+1} * {}^{i+1}\vec a) \\
{}^{i-1}\vec a ... 関節{i-1}から見た点aの位置 (x, y, z) \\
{}^{i-1}\vec p_{i} ... 関節{i-1}から見た関節{i}の位置 (x, y, z) \\
{}^{i-1}R_{i} ... 関節{i-1}から見た関節{i}の回転行列 (3 * 3行列) \\
{}^{i}\vec a ... 関節{i}から見た点aの位置 (x, y, z) \\
}
上記をワールド座標系$\sum$から見た点$a$の位置(${}^{w}\vec a$)を算出することを考える．
\displaylines{
{}^{w}\vec a = {}^{w}\vec p_{1} + {}^{w}R_{1} * {}^{1}\vec a \\
{}^{w}\vec a = {}^{w}\vec p_{1} + {}^{w}R_{1} * ({}^{1}\vec p_{2} + {}^{1}R_{2} * ( ... ) ) \\
{}^{w}\vec a ... ワールド座標系から見た点aの位置 (x, y, z) \\
{}^{w}\vec p_{1} ... ワールド座標系から見た関節{1}の位置 (x, y, z) \\
{}^{w}R_{1} ... ワールド座標系から見た関節{1}の回転行列 (3 * 3行列) \\
{}^{w}\vec a ... ワールド座標系から見た点aの位置 (x, y, z) \\
}
ワールド座標系$\sum$から見た点$a$の位置(${}^{w}\vec a$)を算出すると，関節の数が増えるほど式が複雑になっていく．
式を簡単にするため，位置を3次元(x, y, z) ($\vec a$)から4次元(x, y, z, 1) ($\tilde a$)に拡張して考える．
関節${i}$の座標系$\sum_{i}$から見た点$a$の位置(${}^{i}\tilde a$)を算出することを考える．
\displaylines{
{}^{i}\vec a = {}^{i}\vec p_{i+1} + {}^{i}R_{i+1} * {}^{i+1}\vec a \\
{}^{i}\tilde a
=
\begin{pmatrix}
{}^{i}\vec a \\
1 \\
\end{pmatrix}
=
\begin{pmatrix}
{}^{i}R_{i+1} & {}^{i}\vec p_{i+1} \\
0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
{}^{i+1}\vec a \\
1 \\
\end{pmatrix} \\
{}^{i}\tilde a = {}^{i}T_{i+1} * {}^{i+1}\tilde a \\
{}^{i}T_{i+1} =
\begin{pmatrix}
{}^{i}R_{i+1} & {}^{i}\vec p_{i+1} \\
0 & 1 \\
\end{pmatrix}\\
{}^{i}\tilde a ... 関節{i}から見た点aの位置 (x, y, z, 1) \\
{}^{i}\vec p_{i+1} ... 関節{i}から見た関節{i+1}の位置 (x, y, z) \\
{}^{i}R_{i+1} ... 関節{i}から見た関節{i+1}の回転行列 (3 * 3行列) \\
{}^{i+1}\vec a ... 関節{i+1}から見た点aの位置 (x, y, z) \\
{}^{i}T_{i+1} ... 関節{i}から見た関節{i+1}の同時変換行列 (4 * 4行列) \\
{}^{i+1}\tilde a ... 関節{i+1}から見た点aの位置 (x, y, z, 1) \\
}
上式のように同時変換行列を定義する．
同時変換行列を使用して，ワールド座標系から点$a$の位置を算出すると，下式のようになる．
\displaylines{
{}^{w}\vec a = {}^{w}\vec p_{1} + {}^{w}R_{1} * ({}^{1}\vec p_{2} + {}^{1}R_{2} * ( ... ) ) \\
{}^{w}\tilde a = {}^{w}T_{1} * {}^{1}T_{2} * ... * {}^{i}T_{i+1} * {}^{i+1}\tilde a \\
{}^{w}\tilde a = {}^{w}T_{i+1} * {}^{i+1}\tilde a \\
{}^{w}T_{i+1} = {}^{w}T_{1} * {}^{1}T_{2} * ... * {}^{i}T_{i+1} \\
{}^{w}\vec a ... ワールド座標系から見た点aの位置 (x, y, z) \\
{}^{w}\vec p_{1} ... ワールド座標系から見た関節{1}の位置 (x, y, z) \\
{}^{w}R_{1} ... ワールド座標系から見た関節{1}の回転行列 (3 * 3行列) \\
{}^{w}\tilde a ... ワールド座標系から見た点aの位置 (x, y, z, 1) \\
{}^{w}T_{1} ... ワールド座標系から見た関節{1}の同時変換行列 (4 * 4行列) \\
{}^{w}T_{i+1} ... ワールド座標系から見た関節{i+1}の同時変換行列 (4 * 4行列) \\
}
同時変換行列$T$を使用することで，上記のようにワールド座標系から点$a$までの絶対位置を算出することが簡単になる．
\displaylines{
{}^{i}T_{i+1} =
\begin{pmatrix}
{}^{i}R_{i+1} & {}^{i}\vec p_{i+1} \\
0 & 1 \\
\end{pmatrix}\\
{}^{i}\vec p_{i+1} ... 関節{i}から見た関節{i+1}の位置 (x, y, z) \\
{}^{i}R_{i+1} ... 関節{i}から見た関節{i+1}の回転行列 (3 * 3行列) \\
}
同時変換行列を使用して，2軸ロボットアームと3軸ロボットアームの順運動学を実施する．
同時変換行列を使用した2軸ロボットアームの順運動学
上記で算出した同時変換行列($T$)を使用して，2軸ロボットアームの順運動学を実施する．
下図のような2軸ロボットアームを考える．
関節$1$はz軸周りに$\theta_{1}$回転し，関節$2$はz軸周りに$\theta_{2}$回転する．
以前に，上図の2軸ロボットアームの順運動学を算出した．算出した順運動学は下記のようになった．
\displaylines{
P
=
\begin{pmatrix}
x \\
y \\
\end{pmatrix}
=
\begin{pmatrix}
l_{1} * \sin(\theta_{1}) + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
l_{1} * \cos(\theta_{1}) + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
\end{pmatrix} ... (1)
}
今回，関節$1$と関節$2$はz軸周りに回転するため，回転行列は下式の通りとなる．
\displaylines{
{}^{w}R_{1}
=
\begin{pmatrix}
\cos(\theta_{1}) & -\sin(\theta_{1}) & 0 \\
\sin(\theta_{1}) & \cos(\theta_{1}) & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
{}^{1}R_{2}
=
\begin{pmatrix}
\cos(\theta_{2}) & -\sin(\theta_{2}) & 0 \\
\sin(\theta_{2}) & \cos(\theta_{2}) & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
{}^{2}R_{e}
=
\begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
}
各座標系の相対位置は下式の通りとなる．
\displaylines{
{}^{w}p_{1}
=
\begin{pmatrix}
0 \\
0 \\
0 \\
\end{pmatrix} \\
{}^{1}p_{2}
=
\begin{pmatrix}
l_{1} \\
0 \\
0 \\
\end{pmatrix} \\
{}^{2}p_{e}
=
\begin{pmatrix}
l_{2} \\
0 \\
0 \\
\end{pmatrix} \\
}
各座標系の同時変換行列を計算してみる．
\displaylines{
{}^{w}T_{1}
=
\begin{pmatrix}
{}^{w}R_{1} & {}^{w}p_{1} \\
0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
\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{pmatrix} \\
{}^{1}T_{2}
=
\begin{pmatrix}
{}^{1}R_{2} & {}^{1}p_{2} \\
0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
\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{pmatrix} \\
}
同時変換行列を使用して，順運動学を計算してみる．
(三角関数の公式を使用している．)
\displaylines{
\tilde P = {}^{w}T_{1} * {}^{1}T_{2} * {}^{2}\tilde p_{e} \\
\tilde P
=
\begin{pmatrix}
\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{pmatrix}
\begin{pmatrix}
\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{pmatrix}
\begin{pmatrix}
l_{2} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\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} + \sin\theta_{1} * \cos\theta_{2} & 0 & l_{1} * \sin\theta_{1} \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{2} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\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 & l_{1} * \sin\theta_{1} \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{2} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
l_{2} * \cos(\theta_{1} + \theta_{2}) + l_{1} * \cos\theta_{1} \\
l_{2} * \sin(\theta_{1} + \theta_{2}) + l_{1} * \sin\theta_{1} \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
x \\
y \\
z \\
1 \\
\end{pmatrix}
=
\begin{pmatrix}
l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
l_{1} * \sin\theta_{1} + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
0 \\
1 \\
\end{pmatrix} ... (2)
}
同時変換行列を使用して算出した順運動学である式(2)と，幾何学で算出した順運動学である式(1)が一致することを確認できた．
次は同時変換行列を使用して，3軸ロボットアームの順運動学を計算する．
同時変換行列を使用した3軸ロボットアームの順運動学
同時変換行列($T$)を使用して，3軸ロボットアームの順運動学を実施する．
下図のような3軸ロボットアームを考える．
関節$1$はz軸周りに$\theta_{1}$回転し，関節$2$は-y軸周りに$\theta_{2}$回転し，関節$3$は-y軸周りに$\theta_{3}$回転する．
以前に，上図の3軸ロボットアームの順運動学を算出した．算出した順運動学は下記のようになった．
\displaylines{
P
=
\begin{pmatrix}
x \\
y \\
z \\
\end{pmatrix}
\begin{pmatrix}
\cos\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
\sin\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
l_{1} + l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) \\
\end{pmatrix} ... (3)
}
今回，関節$1$はz軸周りに回転し，関節$2$と関節$3$は$-y$軸周りに回転するため，回転行列は下式の通りとなる．
\displaylines{
{}^{w}R_{1}
=
\begin{pmatrix}
\cos(\theta_{1}) & -\sin(\theta_{1}) & 0 \\
\sin(\theta_{1}) & \cos(\theta_{1}) & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
{}^{1}R_{2}
=
\begin{pmatrix}
\cos(\theta_{2}) & 0 & -\sin(\theta_{2}) \\
0 & 1 & 0 \\
\sin(\theta_{2}) & 0 & \cos(\theta_{2}) \\
\end{pmatrix} \\
{}^{2}R_{3}
=
\begin{pmatrix}
\cos(\theta_{3}) & 0 & -\sin(\theta_{3}) \\
0 & 1 & 0 \\
\sin(\theta_{3}) & 0 & \cos(\theta_{3}) \\
\end{pmatrix} \\
{}^{3}R_{e}
=
\begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
}
各座標系の相対位置は下式の通りとなる．
\displaylines{
{}^{w}p_{1}
=
\begin{pmatrix}
0 \\
0 \\
0 \\
\end{pmatrix} \\
{}^{1}p_{2}
=
\begin{pmatrix}
0 \\
0 \\
l_{1} \\
\end{pmatrix} \\
{}^{2}p_{3}
=
\begin{pmatrix}
l_{2} \\
0 \\
0 \\
\end{pmatrix} \\
{}^{3}p_{e}
=
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
\end{pmatrix} \\
}
各座標系の同時変換行列を計算してみる．
\displaylines{
{}^{w}T_{1}
=
\begin{pmatrix}
{}^{w}R_{1} & {}^{w}p_{1} \\
0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
\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{pmatrix} \\
{}^{1}T_{2}
=
\begin{pmatrix}
{}^{1}R_{2} & {}^{1}p_{2} \\
0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
\cos(\theta_{2}) & 0 & -\sin(\theta_{2}) & l_{1} \\
0 & 1 & 0 & 0 \\
\sin(\theta_{2}) & 0 & \cos(\theta_{2}) & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix} \\
{}^{2}T_{3}
=
\begin{pmatrix}
{}^{2}R_{3} & {}^{2}p_{3} \\
0 & 1 \\
\end{pmatrix}
=
\begin{pmatrix}
\cos(\theta_{3}) & 0 & -\sin(\theta_{3}) & l_{2} \\
0 & 1 & 0 & 0 \\
\sin(\theta_{3}) & 0 & \cos(\theta_{3}) & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix} \\
}
同時変換行列を使用して，順運動学を計算してみる．
(三角関数の公式を使用している．)
\displaylines{
\tilde P = {}^{w}T_{1} * {}^{1}T_{2} * {}^{2}T_{3} * {}^{3}\tilde p_{e} \\
\tilde P
=
\begin{pmatrix}
\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{pmatrix}
\begin{pmatrix}
\cos(\theta_{2}) & 0 & -\sin(\theta_{2}) & l_{1} \\
0 & 1 & 0 & 0 \\
\sin(\theta_{2}) & 0 & \cos(\theta_{2}) & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
\cos(\theta_{3}) & 0 & -\sin(\theta_{3}) & l_{2} \\
0 & 1 & 0 & 0 \\
\sin(\theta_{3}) & 0 & \cos(\theta_{3}) & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\cos\theta_{1} * \cos\theta_{2} & -\sin\theta_{1} & -\cos\theta_{1} * \sin\theta_{2} & 0 \\
\sin\theta_{1} * \cos\theta_{2} & \cos\theta_{1} & -\sin\theta_{1} * \sin\theta_{2} & 0 \\
\sin\theta_{2} & 0 & \cos\theta_{2} & l_{1} \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
\cos(\theta_{3}) & 0 & -\sin(\theta_{3}) & l_{2} \\
0 & 1 & 0 & 0 \\
\sin(\theta_{3}) & 0 & \cos(\theta_{3}) & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\cos\theta_{1} * \cos\theta_{2} * \cos\theta_{3} - \cos\theta_{1} * \sin\theta_{2} * \sin\theta_{3} & -\sin\theta_{1} & -\cos\theta_{1} * \cos\theta_{2} * \sin\theta_{3} - \cos\theta_{1} * \sin\theta_{2} * \cos\theta_{3} & l_{2} * \cos\theta_{1} * \cos\theta_{2} \\
\sin\theta_{1} * \cos\theta_{2} * \cos\theta_{3} - \sin\theta_{1} * \sin\theta_{2} * \sin\theta_{3} & \cos\theta_{1} & -\sin\theta_{1} * \cos\theta_{2} * \sin\theta_{3} - \sin\theta_{1} * \sin\theta_{2} * \cos\theta_{3} & l_{2} * \sin\theta_{1} * \cos\theta_{2} \\
\sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3} & 0 & -\sin\theta_{2} * \sin\theta_{3} + \cos\theta_{2} * \cos\theta_{3} & l_{2} * \sin\theta_{2} + l_{1} \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\cos\theta_{1} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3}) & -\sin\theta_{1} & -\cos\theta_{1} * (\cos\theta_{2} * \sin\theta_{3} + \sin\theta_{2} * \cos\theta_{3}) & l_{2} * \cos\theta_{1} * \cos\theta_{2} \\
\sin\theta_{1} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3}) & \cos\theta_{1} & -\sin\theta_{1} * (\cos\theta_{2} * \sin\theta_{3} + \sin\theta_{2} * \cos\theta_{3}) & l_{2} * \sin\theta_{1} * \cos\theta_{2} \\
\sin(\theta_{2} + \theta_{3}) & 0 & \cos(\theta_{2} + \theta_{3}) & l_{2} * \sin\theta_{2} + l_{1} \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
\cos\theta_{1} * \cos(\theta_{2} + \theta_{3}) & -\sin\theta_{1} & -\cos\theta_{1} * \sin(\theta_{2} + \theta_{3}) & l_{2} * \cos\theta_{1} * \cos\theta_{2} \\
\sin\theta_{1} * \cos(\theta_{2} + \theta_{3}) & \cos\theta_{1} & -\sin\theta_{1} * \cos(\theta_{2} + \theta_{3}) & l_{2} * \sin\theta_{1} * \cos\theta_{2} \\
\sin(\theta_{2} + \theta_{3}) & 0 & \cos(\theta_{2} + \theta_{3}) & l_{2} * \sin\theta_{2} + l_{1} \\
0 & 0 & 0 & 1 \\
\end{pmatrix}
\begin{pmatrix}
l_{3} \\
0 \\
0 \\
1 \\
\end{pmatrix} \\
\tilde P
=
\begin{pmatrix}
x \\
y \\
z \\
1 \\
\end{pmatrix}
=
\begin{pmatrix}
l_{3} * \cos\theta_{1} * \cos(\theta_{2} + \theta_{3}) + l_{2} * \cos\theta_{1} * \cos\theta_{2} \\
l_{3} * \sin\theta_{1} * \cos(\theta_{2} + \theta_{3}) + l_{2} * \sin\theta_{1} * \cos\theta_{2} \\
l_{3} * \sin(\theta_{2} + \theta_{3}) + l_{2} * \sin\theta_{2} + l_{1} \\
1 \\
\end{pmatrix}
=
\begin{pmatrix}
\cos\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3})) \\
\sin\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3})) \\
l_{1} + l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) \\
1 \\
\end{pmatrix} ... (4)
}
同時変換行列を使用して算出した順運動学である式(3)と，幾何学で算出した順運動学である式(4)が一致することを確認できた．
次回は6軸のロボットアームを同時変換行列により，順運動学を算出する．
おわりに
本記事では，下記内容を実装しました．
・同時変換行列を使用して，2軸ロボットアームと3軸ロボットアームの順運動学
次記事では，下記内容を実装していきます．
・6軸ロボットアームの順運動学