ロボット工学基礎で用いる行列計算のチップス

海洋ロボコンをやってた人です。

あるタイミングで行列計算をサッと計算したいとき理論式や手順を都度確認したいことがあり、要所事に調べるなら自身が良く使うであろうチップスはまとめておこうと思ったことが本記事の経緯です。

対象者は行列計算の計算式一覧をサッと確認したい方とし、必要に応じてノウハウを追加していきます。

修正点等あればご遠慮なくご指摘ください。

以下箇条書きになりますが、何卒宜しくお願い致します。

1. 回転行列 (Euler angle rotation matrix)

3次元空間における回転行列の計算方法を、特にオイラー角を用いて記載します。

回転行列は、個別の回転行列 $R_{z,\psi}$, $R_{y,\theta}$, $R_{x,\phi}$ を掛け合わせることで求められます。

1.1. 回転行列の定義

回転行列 $\mathbf{R}^n_b$ は、以下のように定義されます：

$$\mathbf{R}^n_b = R_{z,\psi} \cdot R_{y,\theta} \cdot R_{x,\phi}$$

ここで：

• $\psi$: Z軸周りの回転角（ヨー角）
• $\theta$: Y軸周りの回転角（ピッチ角）
• $\phi$: X軸周りの回転角（ロール角）

Z軸周りの回転 ($R_{z,\psi}$)

Z軸を中心に角度 $\psi$ だけ回転する場合、回転行列は次のように表されます：

$$R_{z,\psi} = \begin{bmatrix} c\psi & -s\psi & 0 \\ s\psi & c\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}$$

Y軸周りの回転 ($R_{y,\theta}$)

Y軸を中心に角度 $\theta$ だけ回転する場合、回転行列は次のように表されます：

$$R_{y,\theta} = \begin{bmatrix} c\theta & 0 & s\theta \\ 0 & 1 & 0 \\ -s\theta & 0 & c\theta \end{bmatrix}$$

X軸周りの回転 ($R_{x,\phi}$)

X軸を中心に角度 $\phi$ だけ回転する場合、回転行列は次のように表されます：

$$R_{x,\phi} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & c\phi & -s\phi \\ 0 & s\phi & c\phi \end{bmatrix}$$

1.2. 回転行列計算

回転行列 $\mathbf{R}^n_b$ は、以下の順番で行列積を計算することで求められます：

$$\mathbf{R}^n_b = R_{z,\psi} \cdot R_{y,\theta} \cdot R_{x,\phi}$$

まず、$R_{y,\theta}$ と $R_{x,\phi}$ の積を計算します：

$$R_{y,\theta} \cdot R_{x,\phi} = \begin{bmatrix} c\theta & s\theta s\phi & s\theta c\phi \\ 0 & c\phi & -s\phi \\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix}$$

次に、この結果と $R_{z,\psi}$ を掛けます：

$$R_{z,\psi} \cdot \left( R_{y,\theta} \cdot R_{x,\phi} \right) = \begin{bmatrix} c\psi & -s\psi & 0 \\ s\psi & c\psi & 0 \\ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} c\theta & s\theta s\phi & s\theta c\phi \\ 0 & c\phi & -s\phi \\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix}$$

これを解くと、最終的に次のような回転行列が得られます：

$$\mathbf{R}^n_b = \begin{bmatrix} c\psi c\theta & c\psi s\theta s\phi - s\psi c\phi & c\psi s\theta c\phi + s\psi s\phi \\ s\psi c\theta & s\psi s\theta s\phi + c\psi c\phi & s\psi s\theta c\phi - c\psi s\phi \\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix}$$

2. 同時変換行列（Homogeneous transformation matrix）

2.1. 同時変換行列の定義

同次変換行列は、3次元空間における位置と姿勢を表現する4×4の行列：

$$T = \begin{bmatrix} R_{3\times3} & p_{3\times1} \\ 0_{1\times3} & 1 \end{bmatrix}$$

で表せる。 ここで：

$$R_{3\times3} = \text{回転行列}$$

$$p_{3\times1} = \text{位置ベクトル}$$

である。

2.2. 同時変換行列の計算

$$\begin{bmatrix} x' \\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} R_{11} & R_{12} & R_{13} & p_x \\ R_{21} & R_{22} & R_{23} & p_y \\ R_{31} & R_{32} & R_{33} & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$$

とし、リンク1-リンク3をもつロボットアームを考えると

リンク1（地上からL1まで）：

• z軸周りの回転：$\theta_1$
• z方向の並進：$L_1$

$$T_1 = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 & 0 & 0 \\ \sin\theta_1 & \cos\theta_1 & 0 & 0 \\ 0 & 0 & 1 & L_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

リンク2（L1からL2まで）：

• y軸周りの回転：$\theta_2$
• x方向の並進：$L_2$

$$T_2 = \begin{bmatrix} \cos\theta_2 & 0 & \sin\theta_2 & L_2 \\ 0 & 1 & 0 & 0 \\ -\sin\theta_2 & 0 & \cos\theta_2 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

リンク3（L2からL3まで）：

• y軸周りの回転：$\theta_3$
• x方向の並進：$L_3$

$$T_3 = \begin{bmatrix} \cos\theta_3 & 0 & \sin\theta_3 & L_3 \\ 0 & 1 & 0 & 0 \\ -\sin\theta_3 & 0 & \cos\theta_3 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

ベース座標系からエンドエフェクタまでの変換：

$$T_{total} = T_1T_2T_3$$

となるので、エンドエフェクタの位置ベクトル $p_e$は

$$p_e = \begin{bmatrix} x_e \\ y_e \\ z_e \\ 1 \end{bmatrix} = T_{total}\begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix}$$

と表せる。

SCARA, 6DOF Robot の同時変換行列は下記参照

「Kinematic Modeling of Serial Robotic Manipulators to Automate the Packaging Process of Tuna Cans」

3. 余因子行列 (Cofactor matrix)

$3\times3$の正方行列 $A$の逆行列を求めるために利用する。

3.1. 小行列と余因子の定義

$3\times3$の正方行列

$$A = \begin{bmatrix} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{bmatrix}$$

に対して、行列 $A$ の逆行列 $A^{-1}$ は、余因子行列 $A^\sim$ を用いて次のように計算できます：

$$A^{-1} = \frac{1}{\det(A)} A^\sim$$

$\det(A)$はサラスの定理 (Sarrus's rule)により求められます ($3\times3$行列の場合$\chi$の書き順に沿って対角行列を加減算して求める)。

ここで余因子行列 $A^\sim$ の各要素 $a_{ij}^\sim$ は次のように計算されます：

$$a_{ij}^\sim = (-1)^{i+j} |M_{ji}| (i,j = 1,2,3)$$

ここで、$M_{ji}$ は行列 $A$ の要素 $a_{ji}$ に対応する $j$ 行 $i$ 列の要素を除いた小行列を示す。

3.2. 余因子行列の計算

まず、行列 $A$ の各要素に対応する小行列 $M_{ji}$ とその行列式 $|M_{ji}|$ を示し、それに基づいて余因子行列 $A^\sim$ を求めます。

$i$行$j$列を求めるとき小行列が$j$行$i$列になっていることに注意

余因子 $a_{11}^\sim$ は：

$$a_{11}^\sim = (-1)^{1+1} |M_{11}| = \begin{bmatrix} a_{22} & a_{23} \\ a_{32} & a_{33} \end{bmatrix} = a_{22} a_{33} - a_{23} a_{32}$$

余因子 $a_{12}^\sim$ は：

$$a_{12}^\sim = (-1)^{1+2} |M_{21}| = (-1) \begin{bmatrix} a_{12} & a_{13} \\ a_{32} & a_{33} \end{bmatrix}= -(a_{12} a_{33} - a_{13} a_{32})$$

各要素に対応する余因子を求めた結果、余因子行列 $A^\sim$ は次のようになります：

$$A^\sim = \begin{bmatrix} a_{11}^\sim & a_{12}^\sim & a_{13}^\sim \\ a_{21}^\sim & a_{22}^\sim & a_{23}^\sim \\ a_{31}^\sim & a_{32}^\sim & a_{33}^\sim \end{bmatrix} = \begin{bmatrix} a_{22}a_{33} - a_{23}a_{32} & -(a_{12}a_{33} - a_{13}a_{32}) & a_{12}a_{23} - a_{13}a_{22} \\ -(a_{21}a_{33} - a_{23}a_{31}) & a_{11}a_{33} - a_{13}a_{31} & -(a_{11}a_{23} - a_{13}a_{21}) \\ a_{21}a_{32} - a_{22}a_{31} & -(a_{11}a_{32} - a_{12}a_{31}) & a_{11}a_{22} - a_{12}a_{21} \end{bmatrix}$$

4. 歪対象行列 (Skew-symmetric matrix)

正方行列を$A$とすると歪対象行列(わいたいしょうぎょうれつ) は以下を満たす正方行列になる。

$$A^T = -A$$

ここで、$A^T$ は行列 $A$ の転置を表す。

4.1. det A = 0

証明

1. 行列式の性質より、転置行列の行列式は元の行列と同じ：
   
   $$\text{det}(A^T) = \text{det}(A)$$
2. 定義 $A^T = -A$ より、
   
   $$\text{det}(A^T) = \text{det}(-A)$$
3. 行列のスカラー倍の行列式の性質：
   
   $$\text{det}(-A) = (-1)^n \text{det}(A)$$
4. 奇数次元 $(n)$ の場合、$(-1)^n = -1$ なので、
   
   $$\text{det}(A) = -\text{det}(A)$$
   
   これより、
   
   $$\text{det}(A) = 0$$

4.2. 外積表現

3次元ベクトル $\mathbf{a}$ に対応する歪対称行列$[\mathbf{a}\times]$：

$$[\mathbf{a}\times] = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix}$$

がベクトル $\mathbf{b}$ に作用すると、次を満たす：

$$\mathbf{a} \times \mathbf{b} = [\mathbf{a}\times] \mathbf{b}$$

歪対象行列の主体各成分$a_{ii}=0$である。

ベクトル積を行列形式で計算する際に有用。

例えば回転行列$R$を例にとると

$$\mathbf{\dot{R}} = [\mathbf{\omega}\times] R$$

と表せる．

https://note.com/ss_sports_lab/n/na80cd8198d22

4.3. 定義2: 外積との関連

5. D-H法 (Denavit-Hartenberg conventional technique)

リンクパラメータを用いた座標表現にDH法がある。

5.1. DH法の定義

$$\begin{aligned} & ^{i-1}T_i(\theta_i, d_i, a_i, \alpha_i) = R_z(\theta_i) \cdot T_z(d_i) \cdot T_x(a_i) \cdot R_x(\alpha_i) \\ & ^{i-1}T_i = \begin{bmatrix} c\theta_i & -s\theta_i c\alpha_i & s\theta_i s\alpha_i & a_i c\theta_i \\ s\theta_i & c\theta_i c\alpha_i & -c\theta_i s\alpha_i & a_i s\theta_i \\ 0 & s\alpha_i & c\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}$$

$\theta_i$, $d_i$, $a_i$, $\alpha_i$ はそれぞれ関節角度、リンクオフセット、リンク長、リンクのねじれ角を示す。

$R_a(\cdot)$ と $T_a(\cdot)$ はそれぞれ$a$ベクトルへの回転行列と同時変換行列を示す。

DHパラメータの算出は定義式に従い順にリンク座標系を移動させ定める。

1. Z軸周りの回転 $R_z(\theta_i)$
2. Z軸方向の並進 $T_z(d_i)$
3. X軸方向の並進 $T_x(a_i)$
4. X軸周りの回転 $R_x(\alpha_i)$

5.2. DH法の計算

ここでは「Detection of a human ready for object transfer to facilitate human-robot interaction」を参考にOpenMANIPULATOR-X のDHパラメータを求めます。

• Joint 1 ($T^0_1$)

$$T^0_1 = \begin{bmatrix} \cos\theta_1 & 0 & \sin\theta_1 & 0 \\ \sin\theta_1 & 0 & -\cos\theta_1 & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

• Joint 2 ($T^1_2$)

$$T^1_2 = \begin{bmatrix} \cos\theta_2 & -\sin\theta_2 & 0 & a_2\cos\theta_2 \\ \sin\theta_2 & \cos\theta_2 & 0 & a_2\sin\theta_2 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

• Joint 3 ($T^2_3$)

$$T^2_3 = \begin{bmatrix} \cos\theta_3 & -\sin\theta_3 & 0 & a_3\cos\theta_3 \\ \sin\theta_3 & \cos\theta_3 & 0 & a_3\sin\theta_3 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

• Joint 4 ($T^3_4$)

$$T^3_4 = \begin{bmatrix} \cos\theta_4 & -\sin\theta_4 & 0 & a_4\cos\theta_4 \\ \sin\theta_4 & \cos\theta_4 & 0 & a_4\sin\theta_4 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

以上より、エンドエフェクタの変換行列は以下の式で求まる：

$$T^0_4 = T^0_1 \cdot T^1_2 \cdot T^2_3 \cdot T^3_4$$

このときDH Parameters of OpenMANIPULATOR-X は下記で表される。

Joint	$\theta_i$(deg)	$d_i$(m)	$a_i$(m)	$\alpha_i$(deg)
1	$\theta_1$	0.077	0	90
2	$\theta_2$−$\theta_0$	0	0.130	0
3	$\theta_3$+$\theta_0$	0	0.124	0
4	$\theta_4$	0	0.1216	0

また、OpenMANIPULATOR-Xの位置ベクトルは

$$p_{3\times1} = \begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} f_1(\theta_1, \theta_2, \theta_3, \theta_4) \\ f_2(\theta_1, \theta_2, \theta_3, \theta_4) \\ f_3(\theta_1, \theta_2, \theta_3, \theta_4) \end{bmatrix} = \begin{bmatrix} (cos(θ_1)cos(θ_2))(a_4cos(θ_3)cos(θ_4)+a_3cos(θ_3)-a_4sin(θ_3)sin(θ_4)) + ... \\ (sin(θ_1)cos(θ_2))(a_4cos(θ_3)cos(θ_4)+a_3cos(θ_3)-a_4sin(θ_3)sin(θ_4)) + ... \\ sin(θ_2)(a_4cos(θ_3)cos(θ_4)+a_3cos(θ_3)-a_4sin(θ_3)sin(θ_4)) + ... \end{bmatrix}$$

と定まる。（6. ヤコビ行列で利用のため記載）

6. ヤコビ行列 (Jacobian matrix)

エンドエフェクタに作用する力 $F$ と関節トルク $\tau$ の関係は、ヤコビ行列の転置を用いて以下のように表される：

$$\tau = J(\theta)^T \cdot F$$

ここで $J(\theta)$: ヤコビ行列

この式を用いることで、エンドエフェクタに必要な力 $F$ から関節に必要なトルク $\tau$ を計算することができる。

6.1. ヤコビ行列の定義

多変数ベクトル値関数において任意の変数について偏微分可能なとき 5. のヤコビ行列

$$J(\theta_1, \theta_2, \theta_3, \theta_4)$$

は以下で表せる。

$$J(\theta_1, \theta_2, \theta_3, \theta_4) = \begin{bmatrix} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} & \frac{\partial f_1}{\partial \theta_3} & \frac{\partial f_1}{\partial \theta_4} \\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} & \frac{\partial f_2}{\partial \theta_3} & \frac{\partial f_2}{\partial \theta_4} \\ \frac{\partial f_3}{\partial \theta_1} & \frac{\partial f_3}{\partial \theta_2} & \frac{\partial f_3}{\partial \theta_3} & \frac{\partial f_3}{\partial \theta_4} \end{bmatrix}$$

6.2. ヤコビ行列の計算

実際に 5. で求めた位置ベクトルとヤコビ行列を用いてトルクを算出する。

$$\begin{bmatrix} τ_1 \\ τ_2 \\ τ_3 \\ τ_4 \end{bmatrix} = J(\theta)^T \cdot F = \begin{bmatrix} J_{11} & J_{21} & J_{31} \\ J_{12} & J_{22} & J_{32} \\ J_{13} & J_{23} & J_{33} \\ J_{14} & J_{24} & J_{34} \end{bmatrix} \cdot \begin{bmatrix} F_1 \\ F_2 \\ F_3 \end{bmatrix}$$

展開すると

$$\begin{aligned} τ_1 &= J_{11}F_1 + J_{21}F_2 + J_{31}F_3 \\ τ_2 &= J_{12}F_1 + J_{22}F_2 + J_{32}F_3 \\ τ_3 &= J_{13}F_1 + J_{23}F_2 + J_{33}F_3 \\ τ_4 &= J_{14}F_1 + J_{24}F_2 + J_{34}F_3 \end{aligned}$$

ここで各ヤコビ行列の要素は

$$\begin{aligned} J_{11} &= \frac{\partial f_1}{\partial \theta_1} = (-\sin(\theta_1)\cos(\theta_2))(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \\ J_{12} &= \frac{\partial f_1}{\partial \theta_2} = (\cos(\theta_1)(-\sin(\theta_2)))(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \\ J_{21} &= \frac{\partial f_2}{\partial \theta_1} = (\cos(\theta_1)\cos(\theta_2))(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \\ J_{22} &= \frac{\partial f_2}{\partial \theta_2} = (\sin(\theta_1)(-\sin(\theta_2)))(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \\ J_{31} &= \frac{\partial f_3}{\partial \theta_1} = \sin(\theta_2)(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \\ J_{32} &= \frac{\partial f_3}{\partial \theta_2} = \cos(\theta_2)(a_4\cos(\theta_3)\cos(\theta_4) + a_3\cos(\theta_3) - a_4\sin(\theta_3)\sin(\theta_4)) + \dots \end{aligned}$$

と求まるので、これをコンピュータで計算すればトルクが求まる。（残りは文献参照してください。）

Appendix

A. 外積 (Cross product)

2つの3次元ベクトル $\mathbf{A} = \begin{bmatrix} a_x & a_y & a_z \end{bmatrix}$ と $\mathbf{B} = \begin{bmatrix} b_x & b_y & b_z \end{bmatrix}$ の外積 $\mathbf{A} \times \mathbf{B}$ は、3次の行列式の1行目($\hat{i}$, $\hat{j}$, $\hat{k}$)での余因子展開の式とみなすと，次のように行列式で表される：

$$\mathbf{A} \times \mathbf{B} = \begin{vmatrix} \hat{i} & \hat{j} & \hat{k} \\ a_x & a_y & a_z \\ b_x & b_y & b_z \end{vmatrix}$$

ここで、$\hat{i}$, $\hat{j}$, $\hat{k}$ はそれぞれ $x$軸、$y$軸、$z$軸方向の単位ベクトル。

行列式を展開すると、次のようになります：

$$\mathbf{A} \times \mathbf{B} = \hat{i} \begin{vmatrix} a_y & a_z \\ b_y & b_z \end{vmatrix} - \hat{j} \begin{vmatrix} a_x & a_z \\ b_x & b_z \end{vmatrix} + \hat{k} \begin{vmatrix} a_x & a_y \\ b_x & b_y \end{vmatrix}$$

各小行列の行列式を計算すると：

$$\begin{vmatrix} a_y & a_z \\ b_y & b_z \end{vmatrix} = a_y b_z - a_z b_y$$

$$\begin{vmatrix} a_x & a_z \\ b_x & b_z \end{vmatrix} = a_x b_z - a_z b_x$$

$$\begin{vmatrix} a_x & a_y \\ b_x & b_y \end{vmatrix} = a_x b_y - a_y b_x$$

これを代入すると：

$$\mathbf{A} \times \mathbf{B} = \hat{i}(a_y b_z - a_z b_y) - \hat{j}(a_x b_z - a_z b_x) + \hat{k}(a_x b_y - a_y b_x)$$

外積の各成分を整理すると：

$$\mathbf{A} \times \mathbf{B} = \begin{bmatrix} a_y b_z - a_z b_y \\ a_z b_x - a_x b_z \\ a_x b_y - a_y b_x \end{bmatrix}$$

B. 内積 (Dot Product)

• 1x3ベクトルの内積

ベクトル$\mathbf{a}=[a_1,a_2,a_3]^T$と$\mathbf{b}=[b_1,b_2,b_3]^T$が与えられたとき、内積$d=\mathbf{a}\cdot\mathbf{b}$は次のように計算される：

$$d=a_1b_1+a_2b_2+a_3b_3$$

• 3x3行列同士の内積

行列(A)と(B)を以下と表すと：

$$A = \begin{bmatrix} a_{x1} & a_{y1} & a_{z1} \\ a_{x2} & a_{y2} & a_{z2} \\ a_{x3} & a_{y3} & a_{z3} \end{bmatrix}, \quad B = \begin{bmatrix} b_{x1} & b_{y1} & b_{z1} \\ b_{x2} & b_{y2} & b_{z2} \\ b_{x3} & b_{y3} & b_{z3} \end{bmatrix}$$

行列(A)と(B)の積(C = A \cdot B)は、次のように計算される。

$$C = \begin{bmatrix} c_{11} & c_{12} & c_{13} \\ c_{21} & c_{22} & c_{23} \\ c_{31} & c_{32} & c_{33} \end{bmatrix}$$

各要素(c_{ij})は以下：

$$\begin{aligned} c_{11} &= a_{x1}b_{x1} + a_{y1}b_{x2} + a_{z1}b_{x3}, \\ c_{12} &= a_{x1}b_{y1} + a_{y1}b_{y2} + a_{z1}b_{y3}, \\ c_{13} &= a_{x1}b_{z1} + a_{y1}b_{z2} + a_{z1}b_{z3}, \\ c_{21} &= a_{x2}b_{x1} + a_{y2}b_{x2} + a_{z2}b_{x3}, \\ \vdots & \\ c_{33} &= a_{x3}b_{z1} + a_{y3}b_{z2} + a_{z3}b_{z3}. \end{aligned}$$

以上、行列計算のチップスでした。
Likeいただけると大変励みになりますので、よろしくお願いいたします。