CV・CG・ロボティクスのためのリー群・リー代数入門: (5) 回転ベクトル
$3 \times 3$ 回転行列は 3 次特殊直交群 $SO(3)$ の元を最も素直に表現するものだと言えるが,本来 3 自由度しかない運動を 9 つのパラメータを用いて表さないといけない点で冗長であると言える.
3 次元空間での回転を最小の 3 パラメータで表す方法は古くからよく考えられていて,代表的なものにオイラー角表示と回転ベクトル表示がある.このエントリでは,両者を概観してから,後者が $SO(3)$ のリー代数 $\mathfrak{so}(3)$ の元をとある基底により表示したものであることを見る.
オイラー角
直交 3 軸まわりの回転を 3 回行うことで任意の回転を表すことができる.ここで重要なのは,その 3 回をどのような順序で行うかによって結果が全く違うということである.そのため,軸を選ぶ順番を定めることで初めて回転の表現として成立する.
何を「オイラー角」と呼ぶかは人によって微妙に違うのだが,一番広い定義は,上で述べたような直交軸まわりの回転を 3 回行う方法の総称だという捉え方であろう.
具体的な方法は,回す軸の選び方によっていろいろあり得る.よく使われるのは Z-Y-Z オイラー角で,まず z 軸まわりに回転し,次に新しい y 軸まわり,最後に新しい z 軸まわりに回転する.同様に Z-X-Z オイラー角もよく使われる.回し方の順番としては (同じ軸を続けて回しても意味はないので) $3 \times 2 \times 2 = 12$ 通りあるわけで,そのうちどれを使うかというのは本質的に慣習にすぎない.
今考えたように毎回の回転ごとに新しい座標軸を取るのではなく,常に固定された 3 軸まわりで回す方式も使われている.ややこしいのは用語が錯綜していることで,順次新しい座標軸を考えるのをオイラー角と呼び,固定して考えるのは固定角と呼ぶ流儀もあれば,その両方を総称してオイラー角と呼ぶ文献もある.
関連して,ロール・ピッチ・ヨー角という名称もよく用いられる.これは,飛行機とか船とか自動車のように「どちらが前でどちらが上か」がはっきりしている物体の場合に意味を持つ考え方で,飛行機でいう胴体軸まわりの回転をロール,機首を上げ下げする回転をピッチ,残りの軸まわりをヨーと呼ぶ.これら 3 軸について順に回転させることで 3 次元回転を表現する.ややこしいことに,これも順次新しい座標軸を考える場合もあれば,固定した座標軸を考える場合もある.
というわけで,単に「オイラー角」とか「ロール・ピッチ・ヨー角」とだけ言われた場合は,どの定義に従っているかよく確認しないとひどい目に合うことがあるので注意したい.確認したらしたで「そんなことも知らんのか」的な反応をされる場合もあってつらいのだが,以上の状況を踏まえると,そういう反応をする人の了見が狭いだけである (個人の感想です).
総称としてのオイラー角全般に共通する問題点としてジンバルロックと呼ばれる現象が挙げられる.これは,3 パラメータがある特定の値を取っているときに,そこからのパラメータ 3 つを独立して動かしても 2 自由度分しか動けなくなる状況を指す.例えば Z-Y-Z オイラー角で,2 番目の y 軸まわりの回転角が 0 の場合,最初の z 軸まわり回転と最後の z 軸まわり回転は全く同じものであり,1 自由度分が失われている.このような状況はどのような回し方を選んでもどこかで発生する.
回転表現の選び方について議論するとき,オイラー角ファミリーはとにかくこのジンバルロック問題を挙げて蛇蝎のごとく嫌われる傾向がある.しかし,実用上それが生じない範囲で使えることがわかっているのであれば (例えば船や自動車が垂直に立ち上がることはあり得ない,とか) 必ずしも支障とはならない.パラメータの意味が直観的にわかりやすいこともあって,実際広く用いられている.
オイラー角から回転行列,あるいは回転行列からオイラー角への変換は,Z-Y-Z オイラー角などのメジャーなものについては多くの教科書やウェブページ等で見つかる.よって省略.
メジャーじゃないものは自分で導出することになる.地道に計算するだけなのでまあ頑張れば導出できるのだが,やはり面倒なので,よく使われるものが少数のメジャーなものに収斂しているのはそういう事情もあるのだろう.どうしても全種類 (順次新たな軸を取る流儀と固定軸を取る流儀それぞれ 12 通りで合計 24 種類) の公式が欲しい人は,Craig (1989) の付録にあるので参照するとよいかも知れない.
回転ベクトル
任意の 3 次元回転は,回転軸方向を表すベクトルと回転角の組として表すことができる (これは証明が必要な事項である.このエントリの最後で言及する).安直に表すと,回転軸ベクトルが 3 成分なので,回転角とあわせて 4 つのパラメータによる表示となる.これを回転角-回転軸表現などと呼ぶ.コンピュータグラフィクス分野の人にとっては OpenGL の glRotatef()
や glm::rotate()
でおなじみではないかと思う.
この表現の嬉しくないところは,1 パラメータ分冗長だという点である.冗長な理由は回転軸ベクトルの長さをどのように取っても結果が同じだからである.この不定性を無くすために「回転軸ベクトルは単位ベクトルとする」などと制約することもよく行われるが,制約条件がつくと扱いが面倒なことも多い.また,「全く動かない回転」 (回転行列の場合の単位行列) のときに回転軸方向が不定になるという問題もある.
このような問題を回避する方法として,回転軸ベクトルの長さを回転角度と等しくし,そのベクトルの 3 要素だけを用いるのが常套手段である.つまり,$\v{w} = (w_1, w_2, w_3)\trans$ を回転軸方向の単位ベクトル,$\theta$ を回転角度として,$\theta \v{w}$ と表すこととする.こちらもやはり回転角-回転軸表現と呼ばれることがあるが,4 パラメータでの表示と紛らわしいこともあって,回転ベクトルと呼ばれることも多い.
有名どころでは,OpenCV でカメラキャリブレーションを行うときに,カメラ姿勢の表現として並進ベクトルとともに用いられるのが回転ベクトルなので,当該分野ではなじみの深い人も多いと思う.
3 パラメータという最もコンパクトな表現でありながら,オイラー角とは違ってジンバルロックの問題がない点は重要である.後述する四元数 (クォータニオン) が 4 パラメータによる表現なのと比べると大きな利点だと言えるだろう.
よく「ジンバルロックを避けるため四元数を用いる」などと書かれている文献を見かけるが,そういう意味では動機の説明として十分ではない.前述の OpenCV でも回転ベクトルは (単に関数に引き渡す表現としてだけではなく) 最適化の内部実装でも用いられており,もう長いこと広く使われていることも実用上問題ないことの証左と言えよう.
以降では,この回転ベクトル表現をリー群・リー代数によって基礎づける.
リー代数の元の座標表示
これまでさんざん見てきたように,3 次元空間の回転は 3 次特殊直交群 $SO(3)$ に属し,そのリー代数 $\mathfrak{so}(3)$ は,トレースが 0 の $3 \times 3$ 歪対称行列の全体だった.したがってその任意の元 $\m{X}$ は
$$
\begin{align}
\m{J}_1 = \bmat
0 & 0 & 0 \\
0 & 0 & -1 \\
0 & 1 & 0 \\
\emat, \,\,
\m{J}_2 = \bmat
0 & 0 & 1 \\
0 & 0 & 0 \\
-1 & 0 & 0 \\
\emat, \,\,
\m{J}_3 &= \bmat
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 0 \\
\emat
\end{align}
$$ を用いて
$$
\begin{align}
\m{X} &= x_1 \m{J}_1 + x_2 \m{J}_2 + x_3 \m{J}_3
\end{align}
$$ と一意に書けるのだった.
よって我々は $\m{X} \in \mathfrak{so}(3)$ に,$\m{J}_1, \m{J}_2, \m{J}_3$ を基底とした座標表示 $\v{x} = (x_1, x_2, x_3)\trans$ を与えることができる.
何のこっちゃと思うかもしれないが,別に驚くことではない.例えば我々は $\v{e}_1, \v{e}_2, \v{e}_3$ を基底とする普通のベクトル空間で,$3 \v{e}_1 + 4 \v{e}_2 -2 \v{e}_3$ なる幾何ベクトルを $(3, 4, -2)\trans$ という座標と当たり前のように同一視している.それと同じことを「行列がなすベクトル空間」に対して行おうというだけである.
同一視できるといっても,何しろ $\m{X}$ はもともと行列なので,表記として区別できるようにしておかないと混乱することも多い.だから,$\m{X}$ と $\v{x}$ の間の相互変換の記号を以下のように約束しておく.
念のため,$\mathfrak{so}(3)$ の場合を具体的に書き下すと,
$$
\begin{align}
\left[\bmat x_1 \\ x_2 \\ x_3 \emat \right]_\wed
&= \bmat
0 & -x_3 & x_2 \\
x_3 & 0 & -x_1 \\
-x_2 & x_1 & 0 \\
\emat
\end{align}
$$ である.$[\v{x}]_\wed$ のことを以降では,「座標 $\v{x}$ で表されるリー代数の元」などと呼ぶことにする.(必ずしも一般的な呼称ではない)
その逆のはたらきをする演算子も定めておこう.
このようにして得られる $\v{x}$ のことを「リー代数の元 $\m{X}$ の座標表示」などと呼ぶことにする.
これと同様の記法は主にロボティクス分野で見られる.$\m{X}^\vee$ 記号は Murray+ (1994) や Bullo+ (2010) と同じものを採用した.一方,本テキストの $[\v{x}]_\wed$ に対応するものは,これらの本では $\v{x}^\wed$ または $\hat{\v{x}}$ と書かかれている.この慣例に従わないのは,ベクトル化と行列化の両方を右上付きで書くと一見どっちがどっちだかわからなくなりやすいし (個人の感想です),$\hat{\v{x}}$ なんて最適推定値か何かにしか見えないので気持ち悪い (個人の感想です) という多分に個人的な理由からである.
コンピュータビジョン (CV) 分野の人は,$[\v{x}]_\wed$ の代わりに $[\v{x}]_\times$ のように書く方が,Hartley+ (2004) や Szeliski (2010) などでなじみ深いかもしれない.CV 分野で $[\v{x}]_\times$ と書くのが好まれるのは,$\mathfrak{so}(3)$ を扱うときにベクトル積が $\v{x} \times \v{y} = [\v{x}]_{\times} \v{y}$ のように表せるからだと思われる.実際,
$$
\begin{align}
[\v{x}]_\times \v{y} &= \bmat
0 & -x_3 & x_2 \\
x_3 & 0 & -x_1 \\
-x_2 & x_1 & 0 \\
\emat \bmat
y_1 \\
y_2 \\
y_3\\
\emat
= \bmat
x_2 y_3 - x_3 y_2 \\
x_3 y_1 - x_1 y_3 \\
x_1 y_2 - x_2 y_1 \\
\emat
= \v{x} \times \v{y}
\end{align}
$$ であり,これは確かに便利な記法である.
それにも関わらずこの慣例に従わずに $\wed$ を使うのは,逆変換である $\vee$ の上下をひっくり返したものを使いたいからである.何しろ $\times$ はひっくり返しても $\times$ だから困る.$\v{x} \wed \v{y}$ はいわゆる外積代数で外積を表す記号で,ベクトル積とはちょっと違うのだが,まあ許してもらうことにしよう.
なお,次回以降のエントリでは,これらの記号を $\mathfrak{so}(3)$ 以外のリー代数に対しても同様に (それぞれで採用する基底のもとで) 使用する.そうなってくると最早「ベクトル積に似ているから $\times$ を使う」などと言うことに意味がない.まあともかく,$\vee$ は v っぽいからベクトル化で,$\wed$ はその逆,ということで何とか慣れて頂きたい.
$\mathfrak{so}(3)$ から $SO(3)$ への指数写像
さて,ここで我々は回転ベクトルのことをいったん忘れて,$SO(3)$ に関する知識だけから回転ベクトルの概念にたどり着くことを試みる.
任意の $\m{X} \in \mathfrak{so}(3)$ に対して $\exp \m{X} \in SO(3)$,つまり $\exp \m{X}$ は 3 次元回転行列である.これが具体的にどんな回転を表すかを考える.
まず,ある $\m{X}$ を固定して考えたとき,$(\exp t\m{X}) \v{z}(0)$ が微分方程式
$$
\begin{align}
\frac{d}{dt}\v{z}(t) &= \m{X} \v{z}(t)
\end{align}
$$ の初期値 $\v{z}(0)$ のもとでの解であることを思い出しておく.
$\v{z}(t)$ が時間とともに動く 3 次元位置ベクトルだと考えて,それがどのように動くのか考える.$\v{x} = \m{X}^\vee$ とすると,$\dot{\v{z}} = \m{X} \v{z} = [\v{x}]_\wed \v{z} = \v{x} \times \v{z}$ である.ここで $\times$ は ベクトル積を表す.つまり位置ベクトル $\v{z}$ の速度は,常に $\v{z}$ と $\v{x}$ の両方に直交する方向を持っている.すなわち $\v{x}$ を軸とする回転をすることになり,軸からの距離 (回転半径) も,原点からの距離も,$\v{z}$ が軸となす角度 $\phi$ も常に一定である.
位置ベクトル $\v{z}$ の速さ $\| \dot{\v{z}} \|$ は
$$
\begin{align}
\|\v{x} \times \v{z}\| &= \|\v{x}\| \, \|\v{z}\| \, |\sin \phi|
\end{align}
$$ であり,回転半径が $\|\v{z}\| \, |\sin \phi|$ なので,回転角速度の大きさは $\|\v{x}\|$ になる.
つまり $\m{X} = [\v{x}]_\wed$ は,回転軸の方向が $\v{x}$,角速度が $\|\v{x}\|$ の瞬時回転を表す.これを左からかけることは,位置ベクトルを回転速度に変換する作用を持つ.
$(\exp \m{X})\v{z}(0)$ はこの微分方程式の解 $(\exp t\m{X})\v{z}(0)$ の $t = 1$ のときの値なので,この瞬時回転が $t = 0$ から $t = 1$ まで継続した後のベクトルの位置である.任意の初期値 $\v{z}(0)$ についてこれが言えるので,結局 $\exp \m{X}$ は,$\v{x}$ の方向を回転軸,$\|\v{x}\|$ を回転角とするような回転を表すと結論できる.
すなわち $\v{x} = (x_1, x_2, x_3)\trans$ は,我々が回転ベクトルと呼んでいるものにほかならない.
よって何らかの回転ベクトル $\v{x}$ が与えられたときに対応する回転行列 $\m{R}$ を作るには,指数写像により
$$
\begin{align}
\m{R} &= \exp [\v{x}]_{\wed}
\end{align}
$$ とすればよい.
今の議論で,$\v{x}$ の解釈が 2 通り出て来たことに注意しておきたい.一つは瞬時回転の方向と角速度をそれぞれ $\v{x}$ の方向と大きさによって表すという見方で,このとき $\v{x}$ を角速度ベクトルと呼ぶ.剛体の力学などで目にすることが多いのではないかと思う.リー代数がリー群の接ベクトルの集まりであり,接ベクトルとはリー群上を走る曲線の速度であるということを考えると,こちらの捉え方が第一義的なものだといえる.
二つめはこれから派生したもので,この回転軸と角速度をキープしたまま単位時間だけ経過したときの姿勢変化を表すという見方である.回転ベクトルはこちらの見方に基づいている.
ロドリーグの公式
指数写像は「無限」級数なので,$\m{R} = \exp \m{X}$ を定義どおり計算するのは厄介である.実はこれを有限の項数で計算するうまい方法がある.
まず $\theta = \| \m{X}^\vee \| = \sqrt{x_1^2 + x_2^2 + x_3^2}$ として,$\m{X} = \theta \m{W}$ と表し直す.$\theta$ は回転角で,$\v{w} = \m{W}^\vee$ は回転軸を表す単位ベクトルになる.$\m{W}$ の 3 乗を地道に計算すると $\m{W}^3 = -\m{W}$ という関係があることがわかり,これを用いると
$$
\begin{align}
\m{W}^{\,\,} &\\
\m{W}^2 &\\
\m{W}^3 &= -\m{W}\\
\m{W}^4 &= \m{W}^3 \cdot \m{W} = -\m{W} \cdot \m{W} = -\m{W}^2\\
\m{W}^5 &= \m{W}^4 \cdot \m{W} = -\m{W}^2 \cdot \m{W} = -\m{W}^3 = \m{W}\\
\m{W}^6 &= \m{W}^5 \cdot \m{W} = -\m{W}^3 \cdot \m{W} = -\m{W}^4 = \m{W}^2\\
&\vdots
\end{align}
$$ という風に,$\m{W}$ のべき乗は $\m{W}, \m{W}^2, -\m{W}, -\m{W}^2, \cdots$ を延々と繰り返すことがわかる.
よって指数写像は,$\cos$ と $\sin$ のテイラー展開を利用して
$$
\begin{align}
\exp \m{X} &= \exp \theta \m{W}\\
&= \m{I} + \theta \m{W} + \frac{1}{2!} (\theta \m{W})^2 + \frac{1}{3!} (\theta \m{W})^3
+ \cdots\\
&= \m{I} + \left(\theta - \frac{\theta^3}{3!} + \frac{\theta^5}{5!} -
\cdots \right) \m{W} + \left(\frac{\theta^2}{2!} - \frac{\theta^4}{4!} +
\frac{\theta^6}{6!} - \cdots \right) \m{W}^2\\
&= \m{I} + (\sin\theta) \m{W} + (1 - \cos\theta) \m{W}^2\\
&= \m{I} + (\sin\theta)[\m{w}]_\wed + (1 - \cos\theta) {[\m{w}]_\wed}^2
\end{align}
$$ と変形でき,$\m{W}^2$ までの項で計算できることがわかる.このように回転ベクトルから回転行列を求める式をロドリーグの公式と呼ぶ.
「なんだか俺の知ってるロドリーグの公式と違うんだけど?」という人のために,この公式のバリエーションをいくつか導いておく.上の式で得られた回転行列 $\exp \theta\m{W}$ でベクトル $\v{p}$ を回転した結果得られるベクトル $\v{p}'$ は,
$$
\begin{align}
\v{p}' &= \left\{ \m{I} + (\sin\theta) [\m{w}]_\wed + (1 - \cos\theta) {[\m{w}]_\wed}^2 \right\} \v{p}\\
&= \v{p} + (\sin\theta) (\v{w} \times \v{p}) + (1 - \cos\theta) (\v{w} \times (\v{w} \times \v{p}))
\end{align}
$$ と表せる.ここでベクトル 3 重積の公式 $\v{a} \times (\v{b} \times \v{c}) = (\v{a}\trans \v{c}) \v{b} - (\v{a}\trans \v{b}) \v{c}$ を使い,$\v{w}$ は単位ベクトルだから $\v{w}\trans \v{w} = 1$ であることに注意すると,
$$
\begin{align}
\v{p}' &= \v{p} + (1 - \cos\theta) ( (\v{w}\trans \v{p}) \v{w} - \v{p} ) + (\sin\theta) (\v{w} \times \v{p})\\
&= (\cos\theta)\v{p} + (1 - \cos\theta) (\v{w}\trans \v{p}) \v{w} + (\sin\theta) (\v{w} \times \v{p})
\end{align}
$$ と表せる.こちらの形式の方が目にすることが多いかもしれない.
さらに $(\v{w}\trans \v{p}) \v{w} = (\v{w} \v{w}\trans) \v{p}$ 使って
$$
\begin{align}
\v{p}' &= (\cos\theta) \v{p} + (1 - \cos\theta) (\v{w}\v{w}\trans) \v{p} + (\sin\theta) (\v{w} \times \v{p})
\end{align}
$$ と表したり,これをまた行列形式に戻して
$$
\begin{align}
\exp \theta\m{W} &= (\cos\theta) \m{I} + (1 - \cos\theta) \v{w}\v{w}\trans + (\sin\theta) [\v{w}]_\wedge
\end{align}
$$ と表すのもよく目にする (OpenCV の cv::Rodrigues()
のドキュメントに書かれているのがこれである).
いろいろな形式が出てきたので一覧にしておこう.いずれも等価である.
オイラーの剛体回転定理
回転ベクトル表現,あるいは回転角・回転軸表現の前提は,3 次元回転は常に何か適当な軸回りの適当な角度の回転として表せるということだった.これは経験的には自明なことのように思えるが,実は決して当たり前の事実ではない.というのも他の次元の $SO(n)$ では成り立たないからである.
その理由は $SO(n)$ の持つ自由度を考えればわかる.行列の要素は $n^2$ 個あり,そこに各列のノルムが 1 であるという制約が合計 $n$ 本,異なる列が直交しているという制約が合計 ${}_{n}C_2 = n(n-1)/2$ 本があるので,差し引き $n(n-1)/2$ 自由度が残る.この数は,$\mathfrak{so}(n)$ の次元を数えても得られるし,$n$ 本の座標軸のうちから「回転面」を張る 2 本の軸を選ぶ組み合わせの数と考えても同じものが得られる.
一方,$n$ 次元空間で回転軸方向を表す単位ベクトルは $n - 1$ 自由度であり,そのまわりの回転角度と合わせて $n$ パラメータの表示になる.$n = 3$ のときはたまたま $n(n-1)/2 = n$ が成立するが,それ以上の次元では明らかにパラメータ数が足りない.
というわけで 4 次元以上では回転角・回転軸では表現し切れないことがわかった.3 次元の場合はパラメータの数に関しては足りているようだが,実際に表現できるかどうかは検証を要する.
そもそも回転軸とは何かというと,その軸方向のベクトル $\v{v}$ に回転行列をかけても動かないようなものということである.つまり,任意の 3 次元回転行列 $\m{R}$ に対して
$$
\begin{align}
\m{R} \v{v} &= \v{v}
\end{align}
$$ となるようなベクトル $\v{v}$ が存在することを示せばよい.言い換えれば,$\m{R}$ が固有値 1 の固有ベクトルを持つことを示せばよい.これは線形代数ではおなじみの問題で,$\det{\m{R} - \m{I}} = 0$ であることを示せばよいのであった.なぜなら,もし $\m{R} - \m{I}$ が可逆なら,$(\m{R} - \m{I})\v{v} = \v{0}$ は $\v{v} = \v{0}$ 以外の解を持ち得ないからである.$\det{\m{R} - \m{I}}$ を変形していくと
$$
\begin{align}
\det{\m{\m{R}} - \m{\m{I}}} &= \det{(\m{I} - \m{R}\trans) \m{R}}\\
&= \det{\m{I} - \m{R}\trans} \, \det{\m{R}}\\
&= \det{\m{I} - \m{R}\trans} \cdot 1 \\
&= \det{\m{I}\trans - \m{R}} \,\,\,\text{(∵ 行列式は転置によって変わらない)} \\
&= \det{-(\m{R} - \m{I})}\\
&= (-1)^3 \det{\m{R} - \m{I}} \,\,\,\text{(∵ 行列式の多重線形性)}
\end{align}
$$ となる.つまり $\det{\m{\m{R}} - \m{\m{I}}} = - \det{\m{\m{R}} - \m{\m{I}}}$ だから,$\det{\m{\m{R}} - \m{\m{I}}} = 0$ である.したがって $\m{R}$ には必ず回転軸があり,それに直交する 2 次元平面が回転面になる.
これはオイラーの定理と呼ばれるが,オイラーの名を冠する定理は数多くあるので,他と区別したいときは剛体回転におけるオイラーの定理などと呼ばれる.
結論として,回転ベクトルはあらゆる 3 次元回転を表し尽くすことのできる表現として安心して使ってよい.
参考文献
このエントリから参照した文献を挙げておく.
- John J. Craig: Introduction to Robotics, Second Edition, Addison-Wesley, 1989. (三浦, 下山訳: ロボティクス―機構・力学・制御, 共立出版, 1991) ※原著最新版は第 4 版
- Richard M. Murray, Zexiang Li, S. Shankar Sastry: A Mathematical Introduction to Robotic Manipulation, First Edition, CRC Press, 1994. http://www.cds.caltech.edu/~murray/mlswiki
- Francesco Bullo and Andrew D. Lewis: Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems, Springer, 2010.
- Richard Hartley and Andrew Zisserman: Multiple View Geometry in Computer Vision, Cambridge University Press, 2004.
- Richard Szeliski: Computer Vision: Algorithms and Applications, Springer, 2010. http://szeliski.org/Book/ (玉木ほか訳: コンピュータビジョン ― アルゴリズムと応用, 共立出版, 2013)
… Richard 多過ぎませんかこの界隈.