行列による座標変換
目的
座標変換の基本的な考え方を整理し，ベクトルに対して行列を作用させることで異なる座標系間での位置関係を記述・変換できることを明らかにする．特に，同次変換行列や直交行列の性質を踏まえ，座標系間の関係性やベクトルの見え方がどのように変化するかについて考察する．
行列の基本的な性質
ベクトルに行列を作用させる
行列をベクトルに作用させるとは，ある空間に属するベクトルを別の空間に対応付ける操作を指す．例えば，以下の式 (1.1) に示すように，ベクトル $ \bm{x} \in \mathbb{R}^n に行列 $ A \in \mathbb{R}^{m \times n} を作用させることで，新たなベクトル $ \bm{y} \in \mathbb{R}^m が得られる．
$ \bm{y}=A\bm{x}\tag{1.1}
このとき，n次元のベクトル空間からm次元のベクトル空間への線形写像 ($ f(r\bm{x}+s\bm{y})=rf(\bm{x})+sf(\bm{y})) が行われており，入力空間と出力空間が異なる場合は次元の異なる空間への写像として理解される．一方入出力空間の次元が一致する場合は，入力と出力の空間が一致するために空間自体の変形，すなわち回転，拡大縮小，反射，せん断などによる幾何的変換と解釈できる．このように，行列をベクトルに作用させる操作はベクトルを別の空間へ写すか，同じ空間内で形を変えるかという空間的な意味を持つ線形操作である．
正則行列と直交行列
行と列の数が等しい行列を正方行列と呼び，逆行列を持つ正方行列は正則行列という．この正則行列は以下のような性質をもつ．
1. $ |A|\neq 0
2. $ \mathrm{rank}A = n
3. $ A\bm{x}=\bm{0} の解は自明な解 $ \bm{0} のみ
式 (1.1) のように行列を用いて座標変換を行う場合，変換行列が正則でないと変換後の空間が次元的に押し潰される (写像の像が低次元になる) ことがある．その結果として逆変換が存在せず，元の座標へ一意に復元することができなくなる．
$ \bm{u},\bm{v} を実内積空間 $ V の正規直交基底とするとき， $ \bm{v} から $ \bm{u} への基底の変換正方行列を直交行列という．簡単にいえば，直交行列とは互いに直交する基底を直交したまま保った状態で変換する行列である．すなわち，長さや内積といった幾何的な関係を保ったまま回転や反射といった変換を行うことができる．この直交行列は以下の式 (1.2) で示す性質をもつ．ただし， $ E は単位行列である．
$ A^TA=AA^T=E\tag{1.2}
つまり，転置行列が逆行列に一致するという特徴をもつ．
代表的な直交行列として以下の式 (1.3) で示す回転行列 $ R が挙げられる．回転行列を任意のベクトルに作用させることで，そのベクトルを任意の角度だけ回転させることができる．また，これは角度 $ \theta だけ回転してずれた 2 つの空間の間で相互に変換を行う操作とも解釈できる．
$ R = \begin{bmatrix} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} & \cos{\theta} \end{bmatrix}\tag{1.3}
固有値と固有ベクトル
以下の式 (1.4) のように，特定のベクトル $ \bm{v} に対し行列 $ A を作用させたものが $ \bm{v} をスカラー $ \lambda 倍したものと等しいとき，$ \lambda を $ A の固有値， $ \bm{v} を$ \lambda に属する固有ベクトルという．
$ A\bm{v}=\lambda\bm{v}\tag{1.4}
このとき，$ \bm{v} が$ \bm{0} でないとすると，正則行列の性質から式 (1.5) がいえる．
$ \det{(\lambda E - A)}=0\tag{1.5}
式 (1.5) の左辺を $ A の固有多項式，式 (1.5) を $ A 固有方程式という．
この固有値には次の性質がある．
1. n次元正方行列の固有値は複素数であり，重複度を込めてn個存在する
2. 転置行列 $ A^T の固有値は $ A の固有値と等しい
3. n次元正方行列 $ A の固有値を $ \lambda_1,\lambda_2,...,\lambda_n とすると， $ \det A = \lambda_1\lambda_2...\lambda_n
4. $ A^n\bm{v}=A(\lambda\bm{v})=\lambda(A\bm{v})=\lambda^n\bm{v}
5. $ (E-A)\bm{v}=(\bm{v}-\lambda\bm{v})=(1-\lambda)\bm{v}
特に性質3と正則行列の性質から，「固有値に0が含まれているとき正則でない」といえる．
例として式 (1.3) で示した回転行列の固有値を求める．式 (1.5) より以下の式 (1.6) を得る．
$ (\cos{\theta}-\lambda)^2+\sin{\theta}=0\tag{1.6}
これを解くと固有値を式 (1.7) のように得る．ここでオイラーの公式を用いた．
$ \lambda = \cos{\theta}\pm j\sin{\theta}=\exp^{\pm j\theta}\tag{1.7}
順運動学
座標変換
他座標系における対象への位置情報と現在座標系と他座標系の位置関係をもとに，現在座標系から対象への位置関係を文献 (1) を参考にして求める．現在座標を $ \Sigma_A ，他座標系を $ \Sigma_B とし，対象を $ P とする． $ \Sigma_A から見た $ P の位置を $ {}^A\bm{P}=\lbrack x,y,z\rbrack^T ， $ \Sigma_B から見た $ P の位置を $ {}^B\bm{P}=\lbrack u,v,w\rbrack^T ， $ \Sigma_A から見た $ {}^B\bm{P} を $ {}^A\bm{P}_B ， $ \Sigma_A から見た $ \Sigma_B の位置を $ {}^A\bm{q} とする．これら変数は基準座標記号を左上に対象座標記号を右下に示す．ここで，ベクトルの加算より以下の式 (2.1) が成立する．
$ {}^A\bm{P}={}^A\bm{q}_B+{}^A\bm{P}_B\tag{2.1}
また， $ \Sigma_A から見た $ \Sigma_B の単位ベクトルを $ {}^A\bm{l},{}^A\bm{m},{}^A\bm{n} とすると以下の式 (2.2) が成立する．
$ \begin{align} {}^A\bm{P}_B&=u{}^A\bm{l}+v{}^A\bm{m}+w{}^A\bm{n} \\ &=\begin{bmatrix} {}^A\bm{l}&{}^A\bm{m}&{}^A\bm{n}\end{bmatrix} \begin{bmatrix} \bm{u} \\ \bm{v} \\ \bm{w} \end{bmatrix} \\ &= {}^AR_B{}^B\bm{P} \end{align}\tag{2.2}
ここで， $ {}^AR_B は $ \begin{bmatrix} {}^A\bm{l}&{}^A\bm{m}&{}^A\bm{n}\end{bmatrix} \in \mathbb{R}^{3 \times 3} と表現される回転行列である．以上式 (2.2) より式 (2.1) は以下の式 (2.3) のように並進と回転の式として表現できる．
$ {}^A\bm{P}={}^A\bm{q}_B+{}^AR_B{}^B\bm{P}\tag{2.3}
逆変換は以下の式 (2.4) で表現できる．
$ \begin{align} {}^B\bm{P}&={}^AR_B^{-1}({}^A\bm{P}-{}^A\bm{q}_B) \\ &={}^AR_B^T({}^A\bm{P}-{}^A\bm{q}_B) \end{align}\tag{2.4}
上記式 (2.4) では，直交行列の1つである回転行列の持つ式 (1.2) で示した性質を用いた．
同次変換行列 (Homogeneous Transformation Matrix) を用いて上記式 (2.3) を1回の演算で行うようにする． $ \Sigma_B から見たベクトルを $ \Sigma_A から見たベクトルに変換する同次変換行列を $ {}^AT_B とすると以下の式 (2.5) で表現できる．
$ {}^AT_B=\begin{bmatrix} {}^AR_B & {}^A\bm{q}_B \\ \bm{0} & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \tag{2.5}
変換対象のベクトルを1次元拡張し，末尾に1を加えたベクトルに対して同次変換行列を作用させると以下の式 (2.6) のように表される．
$ \begin{bmatrix} {}^A\bm{P} \\ 1 \end{bmatrix} = {}^AT_B\begin{bmatrix} {}^B\bm{P} \\ 1 \end{bmatrix} \tag{2.6}
上記式 (2.4) の同次変換行列を求めると以下の式 (2.7) で表現できる．
$ \begin{align} {}^BT_A&=\begin{bmatrix} {}^AR_B^T & -{}^AR_B^T{}^A\bm{q}_B \\ \bm{0} & 1 \end{bmatrix} \\ &={}^AT_B^{-1} \end{align} \tag{2.7}
ここで，同次変換行列は正方行列であっても直交行列ではないため逆行列と転置行列は一致しないことに注意したい．
次に，これまで扱ってきた座標系に加え，新たに座標系 $ \Sigma_C を導入して考える．この場合， $ \Sigma_C から見たベクトルを $ \Sigma_A から見たベクトルに変換する同次変換行列を $ {}^AT_C とすると以下の式 (2.8) で表現できる．
$ {}^AT_C={}^AT_B{}^BT_C\tag{2.8}
以下の式 (2.9) を確認すると上記式が得られることは明らかである．
$ \begin{bmatrix} {}^A\bm{P} \\ 1 \end{bmatrix} = {}^AT_B \begin{bmatrix} {}^B\bm{P} \\ 1 \end{bmatrix} \tag{2.9a}
$ \begin{bmatrix} {}^B\bm{P} \\ 1 \end{bmatrix} = {}^BT_C \begin{bmatrix} {}^C\bm{P} \\ 1 \end{bmatrix} \tag{2.9b}
$ \begin{align} \begin{bmatrix} {}^A\bm{P} \\ 1 \end{bmatrix} &= {}^AT_B{}^BT_C \begin{bmatrix} {}^C\bm{P} \\ 1 \end{bmatrix} \\ &= {}^AT_C \begin{bmatrix} {}^C\bm{P} \\ 1 \end{bmatrix} \end{align} \tag{2.9c}
式 (2.9c) は式 (2.9a) に式 (2.9b) を代入することで得ている．
修正Denavit - Hartenberg記法
修正DH記法とはある関節を挟んだ2つの座標系間の同次変換行列を4つのパラメータ (DH Parameters) で表現する手法である． $ z_i 軸を関節 $ i の軸， $ x_i 軸を $ z_i 軸と $ z_{i+1} 軸の共通垂線，$ y 軸を $ x_i 軸と $ z_i 軸に対して右手座標系になるように設定する．このとき，DHパラメータとして $ x_{i-1} 軸に沿った並進を $ a_i ，$ x_{i-1} 軸回転を $ \alpha_i ， $ z_i 軸に沿った並進を $ d_i ， $ z_i 軸に沿った回転を $ \theta_i とする．それぞれの移動法に対応する同次変換行列は文献 (2) および文献 (3) によると，次の式 (2.10) から式 (2.13) のように示される．
$ T_1=\begin{bmatrix}1&0&0&a_i\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}\tag{2.10}
$ T_2=\begin{bmatrix}1&0&0&0\\0&\cos{\alpha_i}&-\sin{\alpha_i}&0\\0&\sin{\alpha_i}&\cos{\alpha_i}&0\\0&0&0&1\end{bmatrix}\tag{2.11}
$ T_3=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&d_i\\0&0&0&1\end{bmatrix}\tag{2.12}
$ T_4=\begin{bmatrix}\cos{\theta_i}&-\sin{\theta_i}&0&0\\\sin{\theta_i}&\cos{\theta_i}&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}\tag{2.13}
座標 $ \Sigma_{i} から見たベクトルを座標 $ \Sigma_{i - 1} から見たベクトルに変換する同次変換行列 $ {}^{i-1}T_i は上記式 (2.10) から式 (2.13) を用いて以下の式 (2.14) で与えられる．
$ \begin{align} {}^{i-1}T_i &= T_1T_2T_3T_4 \\ &= \begin{bmatrix} \cos{\theta_i}&-\sin{\theta_i}&0&a_i\\\cos{\alpha_i}\sin{\theta_i}&\cos{\alpha_i}\cos{\theta_i}&-\sin{\alpha_i}&-d_i\sin{\alpha_i}\\\sin{\alpha_i}\sin{\theta_i}&\sin{\alpha_i}\cos{\theta_i}&\cos{\alpha_i}&d_i\cos{\alpha_i}\\0&0&0&1 \end{bmatrix} \end{align} \tag{2.14}
上記式 (2.14) で示した同次変換行列の1から3列目は $ \Sigma_{i-1} から見た $ \Sigma_i の姿勢を意味し，4列目は$ \Sigma_{i-1} から見た $ \Sigma_i の位置を意味する．また，1から3列目のそれぞれの列は$ \Sigma_{i-1} から見た $ \Sigma_i の各軸に沿った単位ベクトルであることがわかる．
座標変換の特殊例 (こんなのもあるんだな程度)
ここでは移動前後が式 (2.7) で表現されるとき，個々の中間座標への変換行列 $ T^\frac{1}{n} を考える．つまり，以下の式 (2.15) を考える．
$ T=\begin{bmatrix} R & \bm{q} \\ \bm{0} & 1 \end{bmatrix} = (T^\frac{1}{n})^n \tag{2.15}
ここで，以下の式 (2.16) が成立する．
$ T^n = \begin{bmatrix} R^n & (E+R+R^2+...+R^{n-1})\bm{q} \\ \bm{0} & 1 \end{bmatrix}\tag{2.16}
したがって， $ S=(E+R^\frac{1}{n}+...+(R^\frac{1}{n})^{n-1}) としたとき， $ \bm{q} = S\bm{t} を満たすベクトルを $ \bm{t} とすれば，同次変換行列 $ T^\frac{1}{n} は以下の式 (2.17) で表現される．
$ T^\frac{1}{n}=\begin{bmatrix} R^\frac{1}{n} & \bm{t} \\ \bm{0} & 1 \end{bmatrix} \tag{2.17}
行列 $ S を求める． $ S は次の式 (2.18) で表現される．
$ S=(E+R^\frac{1}{n}+...+(R^\frac{1}{n})^{n-1})\tag{2.18}
この式 (2.18) の両辺に行列 $ R^\frac{1}{n} を右からかけた後に $ S-SR^\frac{1}{n}を取ると以下の式 (2.19) を得る．
$ S-SR^\frac{1}{n} = S(E-R^\frac{1}{n}) = E-R\tag{2.19}
ここで，$ (E-R^\frac{1}{n}) が正則ならば逆行列が存在するため，両辺に$ (E-R^\frac{1}{n})^{-1} を右からかけることで，Sは以下の式 (2.20) で表現される．
$ S = (E-R)(E-R^\frac{1}{n})^{-1}\tag{2.20}
ここで，$ (E-R^\frac{1}{n}) が正則である条件を確認する．回転行列の固有値と固有値の性質より，以下の式 (2.21) が成り立つ．
$ \lambda = 1-\exp^{\pm j\frac{\theta}{n}}\tag{2.21}
よって，固有値の性質から以下の式 (2.22) が成立する．
$ 0=det(E-R^\frac{1}{n})=(1-\exp^{j\frac{\theta}{n}})(1-\exp^{-j\frac{\theta}{n}})\tag{2.22}
以上より，角度 $ \frac{\theta}{n} \neq 2\pi m (m=0,1,2...) のとき $ (E-R^\frac{1}{n}) は正則となり，式 (2.20) が成立する．
ベクトル $ \bm{t} は $ S^{-1}\bm{q} で表現されるため， $ S が正則である必要がある．つまり，式 (2.20) より $ (E-R) が正則である必要がある．これは式 (2.22) の $ n=1 のときであるから，同様に角度 $ \theta \neq 2\pi m (m=0,1,2...) のとき $ S は正則である．
このことから，並進のみのときは別の変換行列を使用し，それ以外の場合では式 (2.17) を用いればよい．
参考文献
(1) 宮崎文夫 他. "ロボティクス入門". 共立出版株式会社. 2000. pp16 - 19
(2) 前掲. pp28 - 31
(3) 一般社団法人 日本機械学会. "機構学". 丸善出版株式会社. 2023. pp176 - 179