はじめに
前編「3次元回転群」はリー群・リー代数による3次元の回転表現を議論したが、ロボティクス工学の世界では、回転運動だけではなく、並進運動も重要である。回転変換と並進変換の組み合わせによる変換は、物体の形状を変化せずに行うことができ、一般的には剛体変換と呼ばれる。
最新のSLAMやロボット運動学の論文では、頻繁的にリー群による剛体変換が用いられている。ロボット技術者として、最新の研究成果を追いかけるためには、これらの知識を理解する必要がある。しかし、Web資料や教科書などに情報が存在するものの、相当な数学力がないと理解しにくい。
本稿の目的は、ロボット技術者に必要な剛体変換群の知識をできるだけわかりやすく解説する。本稿の説明は、前編の内容を理解する必要があるので、もし「3次元回転群」がわからない方はまず前編を読んでください。
また、他のWeb資料や教科書ではリー群の指数写像と対数写像の式が記載されているものの、その導出過程が詳しく説明されていない場合が多い。本文では、詳細の導出過程も説明することで、理解を深めることができると思う。したがって、「なぜこんな式になるのか？」という疑問にも答える。
$$
\newcommand{\skew}[1]{[{#1}]_{\times}} %skew matrix
\newcommand{\so}[1]{ \mathfrak{so}{(#1)} } %lie algebra so3
\newcommand{\se}[1]{ \mathfrak{se}{(#1)} } %lie algebra se3
\newcommand{\norm}[1]{|{#1}|} %norm
$$
剛体変換
剛体変換(Rigid transformation)は、回転と並進の組み合わせであり、変換前後の物体の形状は変化しない。
剛体変換は下記のように4x4の行列で表現することができる。
$$
T = \left[\begin{matrix}
R & t \\
\mathbf{0}^T & 1 \
\end{matrix}\right]
\tag{1}
$$
- R：3x3回転行列式
- t：3次元の並進ベクトル
剛体変換を行列で表現する最大のメリットとして、下記のように行列の掛け算で2つの剛体変換の結合(2)や変換の適用(3)ができる。
$$
T_1 T_2 =
\left[\begin{matrix}
R_1 R_2 & R_1 t_2 + t_1 \\
0 & 1 \
\end{matrix}\right]
\tag{2}
$$
$$
T \left[\begin{matrix}
p \\
1 \
\end{matrix}\right] = \left[\begin{matrix}
Rp +t \\
1 \
\end{matrix}\right]
\tag{3}
$$
- p：3次元の点
デメリットは、ロボットの姿勢を最適化する際には、できるだけ最小限のパラメーター数で剛体変換を表現したいが、4x4の回転行列は16個の独立した要素を持っており冗長であること。また、16個の独立変数で最適化しても、結果は剛体変換であることが保証されないことである。
さらに、ロボットの姿勢の最適化推定では、姿勢の微小変化を解析する必要がある（ヤコビ行列の計算）が、4x4の行列式では難しい。
群論は、これら問題の解決するため有効な数学ツールである。以下の文章は、１つの事例から、モチベーションを解説し、リー群による剛体変換表現を導入する。
定速度で運動するロボット
以下の問題を考えよう：
あるロボットは6自由度の移動が可能であり、ロボットは定速度で運動しており(速度と角速度はそれぞれ$\rho$、$\omega$である)。1秒後の、ロボットの姿勢(回転と並進)を求める。
上の図は、3次元空間に定速度で運動するロボット(立方体)の一例を示す。角速度があるため、運動の軌跡はなめらかな曲線(赤い線)になる。
ロボットの回転
まず、回転から考えよう。
定速度であるため、ロボットは同じ微小な回転を少しづつ適用するので、以下のように回転を合成できる。
$$
R =\underbrace{R_\delta \times ... R_\delta}_\text{n factors}
\tag{4}
$$
前編にも議論したが、下式に$1/n$秒間の微小回転行列を示す。
$$
R_\delta = I + \dfrac{\skew{\omega}}{n}
\tag{5}
$$
この回転の合成は指数写像として計算できる。
$$
R
=\lim_{n \to \infty}(I+\frac{\skew{\omega}}{n})^n = \exp(\skew{\omega})
\tag{6}
$$
ロボットの並進
次の問題は並進がどうなるか？
直線運動なら、単純に速度×時間でよいね。
しかし、角速度がある場合、ロボットは進行の方向は少しづつ変化する。上の図に示すように、微小な並進運動の合成として考える必要がある。
式(3)を使うと、微小な並進運動の合成式は以下になる。
$$
\begin{aligned}
t &= R_\delta ... (R_\delta(R_\delta \rho_\delta + \rho_\delta)+\rho_\delta) ...+\rho_\delta \\
&=R_\delta^{n-1}\rho_\delta+R_\delta^{n-2}\rho_\delta ... R_\delta\rho_\delta+\rho_\delta \\
&=\rho_\delta \sum_{k=0}^{n-1} R_\delta^{k}
\end{aligned}
\tag{7}
$$
$\rho_\delta$は$1/n$秒間の微小並進ベクトルである。
$$
\rho_\delta = \dfrac{\rho}{n}
\tag{8}
$$
nが大きくなるほど、軌跡はよりなめらかになり、tの精度も高くなる。
ロボットの剛体変換
先ほど説明した微小回転(4)と微小並進の合成(7)は1つの式(9)にまとめられる。
$$
\begin{aligned}
T
&=\left[\begin{matrix}
R_\delta R_\delta \times...R_\delta & R_\delta ... (R_\delta(R_\delta \rho_\delta + \rho_\delta)+\rho_\delta) ...+\rho_\delta\\
\mathbf{0}^T & 1
\end{matrix}\right] \\
&= \lim_{n \to \infty}
\left[\begin{matrix}
R_\delta & \rho_\delta\\
\mathbf{0}^T & 1
\end{matrix}\right]^n\\
&= \lim_{n \to \infty} \left(I + \frac{1}{n}
\left[\begin{matrix}
{\skew{\omega}} & {\rho}\\
\mathbf{0}^T & 0
\end{matrix}\right] \right)^n \\
&= \exp{(\hat{\xi})}
\end{aligned}
\tag{9}
$$
式(9)から分かるように、ロボットの姿勢は、行列の指数関数より表現できる。
$\xi$は$\rho$と$\omega$から構成された6次元ベクトルである。
- $\xi=\left[\begin{matrix} \rho \\ \omega \end{matrix}\right]$
$\wedge$ は6次元のベクトルを4x4行列に変形する操作である。
$$
\hat{\xi} = \left[\begin{matrix}
\skew{\omega} & \rho \\
\mathbf{0}^T & 0
\end{matrix}\right]
\tag{10}
$$
3次元特殊ユークリッド群 SE(3)
上記の事例から、我々は指数写像を用いてロボットの姿勢（回転と並進）をベクトルとして表現できるようになった。リー群という言葉はまだ使ってないが、すでにリー群理論に肝心な指数写像を導出した。
3次元回転空間にある剛体変換 $T$ は、特殊ユークリッド群(special euclidean group) $SE(3)$と呼ばれる。式(11)のように、$SE(3)$は3次元の回転と3次元の並進ベクトルで構成される。
$$
SE(3) =
\left(
\left.
T = \left[ {\begin{matrix} R & t \\
{{\mathbf{0}^T}} & 1
\end{matrix}} \right]
\in \mathbb{R} ^{4 \times 4} \right|
R \in SO(3), t \in \mathbb{R}^3
\right)
\tag{11}
$$
$SE(3)$は6つの自由度（3自由度の回転と3自由度の並進）を持つため、対応するリー代数$\se3$は6次元のベクトルである。上の事例にすでに導出されたが、6次元の$\se3$は指数写像によって $SE(3)$に変換することができる。
$$
SE(3) =
\lim_{n \to \infty}(I+\frac{\hat{\xi}}{n})^n
=\exp{\hat{\xi}}
\tag{12}
$$
- $\xi$は$\se3$に属す。
可視化
$SE(3)$は高次元空間に存在するなめらかな6次元の超曲面なので、可視化は難しい。我々は$SE(3)$の自由度を2まで制約し、並進の分量だけ描いた図を作った。本当は高次元空間に存在する6次元の構造であるが、2次元の曲面や平面としてイメージ化する。
水色のまっすぐな平面は$\so3$の空間、緑のなめらかな曲面は$SO(3)$の空間である。指数写像のイメージは$\so3$の空間上にある直線を$SO(3)$の曲面に沿って巻き付ける。逆に対数写像のイメージは$SO(3)$の曲面にある線を真っ直ぐに伸ばす。そのため、指数写像と対数写像はwrap(巻き付け)とunwrap(伸ばす)と呼ばれることがある。
また、この図からわかるように、$\se3$を表す平面と$SE(3)$を表す曲面が原点で接する。したがって、$\se3$を表す空間は単位元における$SE(3)$の接空間(Tangent space)とも呼ばれる。
指数写像(Exponential map)
式(12)を展開すれば、$SE(3)$指数写像を計算できるが、数値計算が難しく、かつ有限項で近似しないといけない（精度が悪い）ため、以下で簡略化をしてみる。
$$
\begin{aligned}
\exp{\hat{\xi}}
&=\lim_{n \to \infty}
\left[\begin{matrix}
R_\delta & \rho_\delta\\
\mathbf{0}^T & 1
\end{matrix}\right]^n \\\\
&=
\left[\begin{matrix}
R_\delta R_\delta \times...R_\delta & R_\delta ... (R_\delta(R_\delta \rho_\delta + \rho_\delta)+\rho_\delta) ...+\rho_\delta\\
\mathbf{0}^T & 1
\end{matrix}\right]
\end{aligned}
\tag{13}
$$
前編に左上の回転部はすでにRodrigues' Formulaより、簡単に計算できることを説明したため、右上の並進部だけ注目する。
$$
\begin{aligned}
t
&= R_\delta ... (R_\delta(R_\delta \rho_\delta + \rho_\delta)+\rho_\delta) ...+\rho_\delta \\
&=\rho_\delta \sum_{k=0}^{n-1} R_\delta^{k}
\end{aligned}
\tag{14}
$$
上式に(5)と(8)を代入すると、以下のように書ける。
$$
\begin{aligned}
t
&=\lim_{n \to \infty} \dfrac{\rho}{n} \sum_{k=0}^{n-1} (I + \dfrac{\skew{\omega}}{n})^{k}
\end{aligned}
\tag{15}
$$
式(15)は、二項定理より展開して、$\rho$とある冪級数の積の形式として変形できる。
$$
\begin{aligned}
t &=\lim_{n \to \infty} \dfrac{\rho}{n} \sum_{k=0}^{n-1} (I + \dfrac{\skew{\omega}}{n})^{k} \\
&=\lim_{n \to \infty} \dfrac{\rho}{n} \sum_{k=0}^{n-1} (I + C_{k}^1 \frac{\skew{\omega}}{n} + C_{k}^2 \frac{\skew{\omega}^2}{n^2} + C_{k}^3 \frac{\skew{\omega}^3}{n^3}...) \\
&=\lim_{n \to \infty} \rho (\sum_{k=0}^{n-1} \frac{I}{n} + \sum_{k=0}^{n-1} C_{k}^1 \frac{\skew{\omega}}{n^2} + \sum_{k=0}^{n-1} C_{k}^2 \frac{\skew{\omega}^2}{n^3} + \sum_{k=0}^{n-1} C_{k}^3 \frac{\skew{\omega}^3}{n^4} ... ) \\
&=\lim_{n \to \infty} \rho (I + \frac{\sum_{k=0}^{n-1} k}{1!} \frac{\skew{\omega}}{n^2} + \frac{\sum_{k=0}^{n-1} k (k-1)}{2!} \frac{\skew{\omega}^2}{n^3} + \frac{\sum_{k=0}^{n-1} k (k-1) (k-2)}{3!} \frac{\skew{\omega}^3}{n^4}...) \\
&\approx \lim_{n \to \infty} \rho (I + \frac{n^2}{1! \times 2} \frac{\skew{\omega}}{n^2} + \frac{n^3 }{2! \times 3} \frac{\skew{\omega}^2}{n^3} + \frac{n^4 }{3! \times 4} \frac{\skew{\omega}^3}{n^4}...) \\
&= \lim_{n \to \infty} \rho (I + \frac{\skew{\omega}}{2!} + \frac{\skew{\omega}^2}{3!} + \frac{\skew{\omega}^3}{4!}...) \\
&= \rho \sum_{k=0}^\infty \frac{\skew{\omega}^{k}}{(k+1)!} \\
\end{aligned}
\tag{16}
$$
式(16)の5行目の計算は、冪乗和の公式(Faulhaber's formula)を利用した。また、$n \to \infty$のとき、式中の低次の冪乗項は省略できる。
式(16)はちょっとシンプルに見えたが、冪級数も無限項がある。以下はさらに簡略化をしてみる。
式(16)の冪級数部分は$J$という記号で書く。
- $\omega = \theta r$ を定義する。
- $r$ : $\omega$の単位ベクトル, $r =\dfrac{\omega}{\norm{\omega}}$
- $\theta$ : $\omega$のノルム, $\theta = \norm{\omega}$
$$
\begin{aligned}
J &= \sum\limits_{k = 0}^\infty
\frac{1}{(k+1)!} (\theta \skew{r} )^k \\
&= I +
\frac{1}{2!} \theta \skew{r} +
\frac{1}{3!} \theta^2 \skew{r}^2 +
\frac{1}{4!} \theta^3 \skew{r}^3 +
\frac{1}{5!} \theta^4 \skew{r}^4 + ... \\
&= I +
\frac{
\frac{1}{2!} \theta^2 \skew{r} +
\frac{1}{3!} \theta^3 \skew{r}^2 +
\frac{1}{4!} \theta^4 \skew{r}^3 +
\frac{1}{5!} \theta^5 \skew{r}^4 + ...}
{\theta} \\
&= I +
\frac{
(r-r + \frac{1}{2!} \theta^2 \skew{r} +
\frac{1}{4!} \theta^4 \skew{r}^3 + ...)
+
(\theta - \theta +\frac{1}{3!} \theta^3 \skew{r}^2 +
\frac{1}{5!} \theta^5 \skew{r}^4 + ...)
}
{\theta} \\
&= I + (\frac{1-\cos{\theta}}{\theta})\skew{r} +
(1-\frac{\sin{\theta}}{\theta})\skew{r}^2\\
\end{aligned}
\tag{17}
$$
(17)の計算では、以下の性質が使われている。
- $\skew{r}\skew{r}\skew{r} = -\skew{r}$
最後に、$SE(3)$の指数写像は、式(18)に変形できる。これで、我々は6次元のベクトル$\se3$を剛体変換群に変換できた。この式を利用すれば、6個のパラメーターを用いて、剛体変換を表現できる。
$$
\begin{aligned}
T
&= \exp{(\hat{\xi})}\\
&=
\left
[\begin{matrix}
\exp{(\skew{\omega})} & J \rho\\
\mathbf{0}^T & 1
\end{matrix}\right]
\end{aligned}
\tag{18}
$$
また、この指数写像の計算はとてもシンプルにでき、実際のアプリケーションでかなり高速的に計算できる。
対数写像 (Logarithm map)
対数写像は指数写像の逆計算であり、リー代数をリー群に対応させることができる。
$SE(3)$→$\so3$の対数写像は式(18)の逆計算であり、まず式(18)の逆計算を書く。
$$
\begin{aligned}
\xi
&= \log{(T)}^{\vee} \\
&=
\left
[\begin{matrix}
\exp^{-1}{(R)}^{\vee} & J^{-1} t
\end{matrix}\right]^T \\
&= \left
[\begin{matrix}
\log{(R)}^{\vee} & J^{-1} t
\end{matrix}\right]^T \\
\end{aligned}
\tag{19}
$$
- $\vee$は$\wedge$の逆であり、4x4の行列を6次元ベクトルに戻す操作である。
$\log{(R)}^{\vee}$はすでに前編で議論した$SO(3)$→$\so3$の対数写像である。次は$J^{-1}$のみ注目する。
まず、Jの逆行列は以下の形式であることを仮定する。
$$
J^{-1} = I + A \skew{r} + B \skew{r}^2
\tag{20}
$$
ここで$A$と$B$は未知の係数
$J$と$J^{-1}$を掛け算すると、式(21)になる。
$$
\begin{aligned}
J J^{-1}
&= I + ( \frac{A\sin{\theta}}{\theta} - \frac{B(1-\cos{\theta})}{\theta} + \frac{(1-\cos{\theta})}{\theta} )\skew{r} \\
&\quad +(\frac{A(1-\cos{\theta})}{\theta} + \frac{B\sin{\theta}}{\theta} + \frac{(\theta-\sin{\theta})}{\theta})\skew{r}^2
\end{aligned}
\tag{21}
$$
逆行列ともとの行列の積は単位行列であるため、式(21)$\skew{r}$と$\skew{r}^2$の係数は0でなければならない。したがって、以下の連立方程式が立てられる。
$$
\begin{cases}
\dfrac{A\sin{\theta}}{\theta} - \dfrac{B(1-\cos{\theta})}{\theta} + \dfrac{(1-\cos{\theta})}{\theta} = 0 \\
\dfrac{A(1-\cos{\theta})}{\theta} + \dfrac{B\sin{\theta}}{\theta} + \dfrac{(\theta-\sin{\theta})}{\theta} = 0
\end{cases}
\tag{22}
$$
この連立方程式は容易に解ける。解は式(23)に示す。
$$
\begin{cases}
A = -\dfrac{\theta}{2} \\
B = 1 - \dfrac{\theta \sin{\theta}}{2(1-\cos{\theta})}
=\dfrac{2 \sin{\theta - \theta (1+\cos{\theta})}}{2 \sin{\theta}}
\end{cases}
\tag{23}
$$
解を式(20)に代入すると、$J^{-1}$は以下のように求められる。
$$
J^{-1} = I - \frac{\theta \skew{r}}{2} + \frac{2 \sin{\theta - \theta (1+\cos{\theta})}}{2 \sin{\theta}} \skew{r}^2
\tag{24}
$$
さらに、$J^{-1}$を式(19)に代入すると、$SE(3)$の対数写像もシンプルな計算式で表現できた。
まとめ
今回は、リー群・リー代数を用いた剛体変換の解説を行した。一般的な剛体変換行列と比べ、群の知識を活用することで最小限のパラメーター数で剛体変換を表現でき、最適化も高速に行える。また、最適化結果を$SE(3)$の指数写像を用いた場合、変換結果が剛体変換であることも保証される。
今回は$SE(3)$の微分についてはまだ説明していないが、指数写像の本質が微小な変化の結合であることを説明したため、$SE(3)$が容易に微分できることが想像できる。
$SE(3)$の実際の応用においては、指数写像と対数写像の計算方法が非常に重要である。他の資料に載っていない（私が知る限りでは）、指数写像と対数写像の導出過程も説明することで、この謎めいた式をより深く理解できるだろう。
リー群・リー代数を導入する最終目的は、回転の最適化を行うことである。次回は実例を交えて、リー群とリー代数最適化について詳しく解説する。
また、今回の内容も含め、数学とロボティクスに関するおもしろいテーマを以下のGitHubに整理しているので、興味があれば見てくだい。
$SE(3)$の指数と対数写像のpython実装：
- utilities/math_tools.py