#1. 概要
同次変換行列(Homogeneous transformation matrix)は，行列計算によって座標変換や剛体変換を効率的に計算するためのテクニック(?)で，ロボット分野で度々用いられます．
座標変換や剛体変換は，座標系や次元の数が多くなってくると計算が複雑になり，実装も骨が折れます．
そこで，ロボットの姿勢や変位を行列で表すことで，多くの計算が行列の掛け算だけで表現でき，記述も実装も比較的楽になります．
こちらの記事では，ロボット分野の同次変換行列の基礎から使い方までを数式ベースで理解するまでを解説します．
なお，コンピュータビジョン分野で良く用いられている方の同次変換行列は，3次元空間座標と画像平面座標の間の変換を行うもので
\left(
\begin{array}{}
u \\
v \\
1
\end{array}
\right)
\propto
\left(
\begin{array}{}
f & sf & u_0 \\
0 & \alpha f & v_0 \\
0 & 0 & 1
\end{array}
\right)
\left(
\begin{array}{}
R_{\text{camera}} & \boldsymbol{t}_{\text{camera}}
\end{array}
\right)
\left(
\begin{array}{}
x \\
y \\
z \\
1
\end{array}
\right)
というように表されます．
この記事ではこちらの同次変換行列は扱わないため，ご注意下さい．
##目次
1. 概要
2. 定義
3. 掛け算で得られる結果
4. 掛け算の順序
5. 同次変換行列の計算の実装
#2. 定義
ロボットの姿勢・変位を回転行列$R$と並進ベクトル$\boldsymbol{t}$で表現するとき，同次変換行列$T$は
T =
\left(
\begin{array}{}
R & \boldsymbol{t} \\
\boldsymbol{0}^T & 1
\end{array}
\right)
となります．
※ここで，行列$X$の転置は$X^T$と表します．
もし扱う空間が2次元の場合であれば
T =
\left(
\begin{array}{}
\cos \theta & - \sin \theta & t_x \\
\sin \theta & \cos \theta & t_y \\
0 & 0 & 1
\end{array}
\right)
となり，3次元空間においては
T =
\left(
\begin{array}{}
R_{00} & R_{01} & R_{02} & t_x \\
R_{10} & R_{11} & R_{12} & t_y \\
R_{20} & R_{21} & R_{22} & t_z \\
0 & 0 & 0 & 1
\end{array}
\right)
と表現されます．
このように，扱う次元を$n$とおくと，同次変換行列のサイズは$(n+1 \times n+1)$となります．
- 位置と角度がなぜごちゃ混ぜなのか?
- 隙間を埋めている0や1は?
というように疑問点が浮かぶ方も多いとは思いますが，なんとか飲み込んで頂きたいと思います．
#3. 掛け算で得られる結果
同次変換行列を扱う上での大きな利点として，座標変換が簡単になるということがあります．
ワールド座標系におけるロボットAの姿勢を$T_A$とします．
ロボットの角度は$R_A$，位置は$\boldsymbol{t}_A$とします．
T_A =
\left(
\begin{array}{}
R_A & \boldsymbol{t}_A \\
\boldsymbol{0}^T & 1
\end{array}
\right)\\
続いて，このロボットにはカメラBが付いているものと考えます．
ロボットから見たカメラの姿勢を$T_B$とし，カメラの角度は$R_B$，位置は$\boldsymbol{t}_B$とします．
T_B =
\left(
\begin{array}{}
R_B & \boldsymbol{t}_B \\
\boldsymbol{0}^T & 1
\end{array}
\right)\\
##3.1 座標変換(通常)
ワールド座標系におけるカメラの姿勢$\left( R_{BW},\boldsymbol{t}_{BW} \right)$は，
$\left( R_B, \boldsymbol{t}_B \right)$を$\left( R_A, \boldsymbol{t}_A \right)$で座標変換することで
\begin{eqnarray}
R_{BW} &=& R_A R_B\\
\boldsymbol{t}_{BW} &=& R_A \boldsymbol{t}_B + \boldsymbol{t}_A
\end{eqnarray}
と求まります．
##3.2 座標変換(同次変換行列の掛け算)
$T_A,T_B$を掛け合わせてみると
\begin{eqnarray}
T_A T_B
&=&
\left(
\begin{array}{}
R_A & \boldsymbol{t}_A \\
\boldsymbol{0}^T & 1
\end{array}
\right)
\left(
\begin{array}{}
R_B & \boldsymbol{t}_B \\
\boldsymbol{0}^T & 1
\end{array}
\right) \\
&=&
\left(
\begin{array}{}
R_A R_B & R_A \boldsymbol{t}_B + \boldsymbol{t}_A \\
\boldsymbol{0}^T & 1
\end{array}
\right)
=
\left(
\begin{array}{}
R_{BW} & \boldsymbol{t}_{BW} \\
\boldsymbol{0}^T & 1
\end{array}
\right)
\end{eqnarray}
となります．
この掛け算により先ほどと同じ計算結果を得ることができます．
この方法の良い所としては
- 計算が単純でミスを防止できる
- 仮に相対座標系が増えた場合でも行列の掛け算1回で計算が済む
- 文字数が$R,\boldsymbol{t}$の2文字から$T$の1文字に減り，書くのが楽
などがあります．
そのため，姿勢・位置・変位などを同次変換行列の形で記述すると，多くの点でお得だと思います．
#4. 掛け算の順序
同次変換行列を扱う上では，掛け算の順序が重要な意味を持ちます．
##3.2 座標変換(同次変換行列の掛け算)
3.2節の内容を例に挙げると，$T_A T_B$は
- ①$T_A$をA座標系基準で$T_B$だけ変位させた値
- ②$T_B$をワールド座標系基準で$T_A$だけ変位させた値
という風に捉えることができます．
①に従って考える時は，ロボット姿勢は
T_A
で，その座標系基準に$T_B$だけ変位させる場合は右から掛け算して
T_A \color{red}{T_B}
となります．
②に従って考える時は，センサ姿勢が
T_B
で，そこからワールド座標系基準に$T_A$だけ変位させる場合は左から掛け算して
\color{red}{T_A} T_B
となります．
①と②の計算結果は同じですが，捉え方によっては数式の組み立て方に違いが表れます．
この考え方に慣れることで，座標変換がスムーズに行えるのではないかと思います．
##4.1 掛け算の順序の例
ロボット座標系基準のセンサ姿勢を$\color{red}{T_s}$とします．
ロボットが姿勢$T_{\text{robot}}$の姿勢の時，
オドメトリ変位によってロボット座標系基準に$\color{green}{T_{\text{odo}}}$，
ワールド座標系基準に$\color{blue}{\Delta T}$だけ動いた時の，
ワールド座標系基準のセンサ姿勢を求めてみます．
ロボット姿勢が
\color{blue}{\Delta T} T_{\text{robot}} \color{green}{T_{\text{odo}}}
\left( = T_{\text{robot}}' =
\left(
\begin{array}{}
R_{\text{robot}}' & \boldsymbol{t}_{\text{robot}}' \\
\boldsymbol{0}^T & 1
\end{array}
\right)
\right)
となるため，センサ姿勢は
\color{blue}{\Delta T} T_{\text{robot}} \color{green}{T_{\text{odo}}} \color{red}{T_s}
\left( = T_s' =
\left(
\begin{array}{}
R_s' & \boldsymbol{t}_s' \\
\boldsymbol{0}^T & 1
\end{array}
\right)
\right)
となります．
#5. 同次変換行列の計算の実装
こちらのサイトにて，C++の行列計算ライブラリ"Eigen"の使い方が分かりやすく解説されているので，実装の際は参考になると思われます．
姿勢や変位の変数から行列を生成し，それらの掛け算を行うことで計算を簡単にできると思います．
実際のプログラムに関しては，後日執筆するかもしれませんので，今しばらくお待ちください．
#まとめ
今回は，座標変換を助けるツールの"同次変換行列"を解説しました．
内容に関してお気付きの点があれば気軽にお問い合わせ下さい．