剛体変換をねじパラメータで表す

はじめに

「ねじパラメータ」とは、英語で「Screw Parameter」のことです。この訳が一般的かは知りません。
英語のまま使うのが嫌だったので、無理やり日本語にしています。

直線をプリュッカー座標で表す

直線は方向ベクトル ${\bf l}\in{\mathbb R}^3$ と直線上の点の位置ベクトル ${\bf p}\in{\mathbb R}^3$ で表されることが多いと思います。
プリュッカー座標では、直線上の点の位置ベクトルの代わりに、 ${\bf m}\in{\mathbb R}^3,\ {\bf m}={\bf p}\times{\bf l}$ を使います。
プリュッカー座標で直線を表すと、 $({\bf l},\ {\bf m})$ となります。
${\bf p}$ と異なる直線上の点の位置ベクトル ${\bf p}'\in{\mathbb R}^3$ を用いても ${\bf m}$ は同じベクトルです。

$\begin{eqnarray} {\bf p}'\times{\bf l}&=&({\bf p}+({\bf p}'-{\bf p}))\times{\bf l}\\ &=&({\bf m}+\lambda{\bf l}\times{\bf l}\ \ \ (for\ some\ \lambda)\\ &=&{\bf m}\tag{1} \end{eqnarray}$

${\bf l}$ のノルムは $1$ である、すなわち $\|{\bf l}\|=1$ であることが多いようです。
以降本記事では、直線の方向ベクトルのノルムは $1$ とします。

原点から直線への直交投影した点の位置ベクトルを ${\bf p}_\perp\in{\mathbb R}^3$ とすると、以下が成り立ちます。

$\begin{eqnarray} {\bf p}={\bf p}_\perp+({\bf l}^\top{\bf p}){\bf l}\tag{2} \end{eqnarray}$

式 $(2)$ は ${\bf p}$ を ${\bf l}$ に垂直な成分と垂直な成分に分けているだけです。
式 $(2)$ を ${\bf p}_\perp$ について解きます。

$\begin{eqnarray} {\bf p}_\perp&=&{\bf p}-({\bf l}^\top{\bf p}){\bf l}\\ &=&({\bf l}^\top{\bf l}){\bf p}-({\bf l}^\top{\bf p}){\bf l}\\ &=&{\bf l}\times({\bf p}\times{\bf l})\\ &=&{\bf l}\times{\bf m}\tag{3} \end{eqnarray}$

式 $(3)$ からわかるように ${\bf p}_\perp$ は ${\bf l}$ と ${\bf m}$ の外積で求まります。

Chaslesの定理

任意の座標変換（同次変換）は、ある回転軸周りの回転とその回転軸に平行な並進移動によって表すことができる

この定理については認めることにします。

剛体変換を表す行列を ${\bf M}\in{\mathbb R}^{4\times4}$ とします。
この剛体変換の回転を表す行列を ${\bf R}\in{\mathbb R}^{3\times3}$ 、平行移動を表すベクトルを ${\bf t}\in{\mathbb R}^3$ とします。

$\begin{eqnarray} {\bf M}= \left( \begin{array}{ccc|c} & & & \\ & \large{\bf R} & & \large{\bf t} \\ & & & \\ \hline & {\bf 0}^\top & & 1 \end{array} \right)\tag{4} \end{eqnarray}$

回転行列から原点を通る回転軸を表す単位ベクトル ${\bf l}\in{\mathbb R}^3$ 、回転角 $\theta\in{\mathbb R}$ を求めておきます。
原点を通る回転軸と回転角の求め方については、回転行列から単位四元数への変換と単位四元数を回転軸と回転角に変換するを参照してください。

ねじパラメータは、回転軸 $({\bf l},{\bf m})$ と回転角 $\theta$ と回転軸に沿った平行移動の符号付距離 $d\in{\mathbb R}$ です。
${\bf l},\theta$ はすでに求まっているので、 ${\bf m},d$ を求める必要があります。

まず、 $d$ を求めます。 $d$ は ${\bf t}$ を ${\bf l}$ に投影すればよいです。

$\begin{eqnarray} d={\bf t}^\top{\bf l}\tag{5} \end{eqnarray}$

次に ${\bf m}$ を求めるのですが、先に原点を回転軸に直交投影した点の位置ベクトルを ${\bf c}\in{\mathbb R}^3$ を求めます。
原点を変換することを考えます。(原点の位置ベクトルは ${\bf 0}\in{\mathbb R}^3$ で表せます。)
まず、回転後平行移動すると考えると、以下のように変位します。

$\begin{eqnarray} {\bf R}{\bf 0}+{\bf t}={\bf t}\tag{6} \end{eqnarray}$

次に、ねじの回転軸よって回転した後、ねじの回転軸に沿って平行移動したと考えると、以下のようになります。

$\begin{eqnarray} {\bf R}({\bf 0}-{\bf c})+{\bf c}+d{\bf l}=d{\bf l}+({\bf I}-{\bf R}){\bf c}\tag{7} \end{eqnarray}$

式 $(6)$ と式 $(7)$ は等しいので、等式で結びます。

$\begin{eqnarray} {\bf t}=d{\bf l}+({\bf I}-{\bf R}){\bf c}\tag{8} \end{eqnarray}$

ロドリゲスの公式より、 ${\bf R}{\bf c}$ は、以下のように書けます。

$\begin{eqnarray} {\bf R}{\bf c}&=&{\bf c}+\sin\theta{\bf l}\times{\bf c}+(1-\cos\theta){\bf l}\times({\bf l}\times{\bf c})\\ &=&{\bf c}+\sin\theta{\bf l}\times{\bf c}+(1-\cos\theta)({\bf l}^\top{\bf c}){\bf l}-({\bf l}^\top{\bf l}){\bf c})\\ &=&{\bf c}+\sin\theta{\bf l}\times{\bf c}+(-1+\cos\theta){\bf c}\\ &=&\sin\theta{\bf l}\times{\bf c}+\cos\theta{\bf c}\tag{9} \end{eqnarray}$

式 $(9)$ を式 $(8)$ に代入して ${\bf c}$ について解きます。

$\begin{eqnarray} &&{\bf t}=d{\bf l}+{\bf c}-\sin\theta{\bf l}\times{\bf c}-\cos\theta{\bf c}\\ &&\Leftrightarrow (1-\cos\theta){\bf c}-\sin\theta{\bf l}\times{\bf c}={\bf t}-({\bf t}^\top{\bf l}){\bf l}\\ &&\Leftrightarrow 2\sin^2\frac{\theta}{2}{\bf c} -\sin\theta \underbrace{\begin{pmatrix} 0 & -l_z & -l_y\\ l_z & 0 & -l_x\\ -l_y & l_x & 0 \end{pmatrix}}_{=:{\bf L}_c} {\bf c}={\bf t}-({\bf t}^\top{\bf l}){\bf l}\\ &&\Leftrightarrow 2\sin^2\frac{\theta}{2}{\bf c} -\sin\theta {\bf L}_c {\bf c}={\bf t}-({\bf t}^\top{\bf l}){\bf l}\\ &&\Leftrightarrow \left(2\sin^2\frac{\theta}{2}{\bf I}-2\sin\frac{\theta}{2}\cos\frac{\theta}{2}{\bf L}_c\right){\bf c}={\bf t}-({\bf t}^\top{\bf l}){\bf l}\\ &&\Leftrightarrow \sin^2\frac{\theta}{2}\left({\bf I}-\cot\frac{\theta}{2}{\bf L}_c\right){\bf c}=\dfrac{1}{2}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &&\Leftrightarrow \bigg({\bf I}-\underbrace{\cot\frac{\theta}{2}}_{=:h}{\bf L}_c\bigg){\bf c}=\dfrac{1}{2}\left(\sin^2\frac{\theta}{2}\right)^{-1}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &&\Leftrightarrow \bigg({\bf I}-h{\bf L}_c\bigg){\bf c}=\dfrac{1}{2}\left(\sin^2\frac{\theta}{2}\right)^{-1}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &&\Leftrightarrow h\left(h^{-1}{\bf I}-{\bf L}_c\right){\bf c}=\dfrac{1}{2}\left(\sin^2\frac{\theta}{2}\right)^{-1}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &&\Leftrightarrow \begin{pmatrix} h^{-1} & l_z & -l_y\\ -l_z & h^{-1} & l_x\\ l_y & -l_x & h^{-1} \end{pmatrix}{\bf c}=\dfrac{1}{2}\left(h\sin^2\frac{\theta}{2}\right)^{-1}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &&\Leftrightarrow {\bf c}=\dfrac{1}{2}\left(h\sin^2\frac{\theta}{2}\right)^{-1} \begin{pmatrix} h^{-1} & l_z & -l_y\\ -l_z & h^{-1} & l_x\\ l_y & -l_x & h^{-1} \end{pmatrix}^{-1} \left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\tag{10} \end{eqnarray}$

${\bf c}$ について解くことができたので、後は式 $(10)$ の右辺を計算して簡単にします。

$\begin{eqnarray} {\bf c}&=&\dfrac{1}{2}\left(h\sin^2\frac{\theta}{2}\right)^{-1} \frac{h}{h^2+1} \begin{pmatrix} h^2l_x^2 + 1 & h^2l_xl_y-hl_z & h^2l_xl_z+hl_y\\ h^2l_xl_y+hl_z & h^2l_y^2+1 & h^2l_yl_z-hl_x\\ h^2l_xl_z-hl_y & h^2l_yl_z+hl_x & h^2l_z^2+1 \end{pmatrix} \left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &=&\dfrac{1}{2} \Bigg( \begin{pmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{pmatrix} +h^2 \underbrace{ \begin{pmatrix} l_x^2 & l_xl_y & l_xl_z\\ l_xl_y & l_y^2 & l_yl_z\\ l_xl_z & l_yl_z & l_z^2 \end{pmatrix}}_{=:{\bf L}_p} +h \underbrace{ \begin{pmatrix} 0 & -l_z & l_y\\ l_z & 0 & -l_x\\ -l_y & l_x & 0 \end{pmatrix}}_{=:{\bf L}_c} \Bigg) \left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &=&\dfrac{1}{2} \left({\bf I} +h^2{\bf L}_p+h{\bf L}_c\right) \left({\bf t}-({\bf t}^\top{\bf l}){\bf l}\right)\\ &=&\dfrac{1}{2} \big( {\bf t}-({\bf t}^\top{\bf l}){\bf l} +h^2\underbrace{{\bf L}_p{\bf t}}_{=({\bf t}^\top{\bf l}){\bf l}} -h^2({\bf t}^\top{\bf l})\underbrace{{\bf L}_p{\bf l}}_{=({\bf l}^\top{\bf l}){\bf l}} +h\underbrace{{\bf L}_c{\bf t}}_{={\bf l}\times{\bf t}} -h({\bf t}^\top{\bf l})\underbrace{{\bf L}_c{\bf l}}_{={\bf l}\times{\bf l}} \big)\\ &=& \dfrac{1}{2} \left( {\bf t}-({\bf t}^\top{\bf l}){\bf l} +h^2({\bf t}^\top{\bf l}){\bf l} -h^2({\bf t}^\top{\bf l})({\bf l}^\top{\bf l}){\bf l} +h{\bf l}\times{\bf t} -h({\bf t}^\top{\bf l}){\bf l}\times{\bf l} \right)\\ &=& \dfrac{1}{2} \left( {\bf t}-({\bf t}^\top{\bf l}){\bf l} +h^2({\bf t}^\top{\bf l}){\bf l} -h^2({\bf t}^\top{\bf l}){\bf l} +h{\bf l}\times{\bf t} \right)\\ &=&\dfrac{1}{2}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}+h{\bf l}\times{\bf t}\right)\\ &=&\dfrac{1}{2}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}+\cot\frac{\theta}{2}{\bf l}\times{\bf t}\right)\tag{11} \end{eqnarray}$

式 $(11)$ より、 ${\bf c}$ が求まったので、 ${\bf m}$ が計算できます。

$\begin{eqnarray} {\bf m}&=&{\bf c}\times{\bf l}\\ &=&\dfrac{1}{2}\left({\bf t}-({\bf t}^\top{\bf l}){\bf l}+\cot\frac{\theta}{2}{\bf l}\times{\bf t}\right)\times{\bf l}\\ &=&\dfrac{1}{2}\left({\bf t}\times{\bf l}-({\bf t}^\top{\bf l}){\bf l}\times{\bf l}+\left({\bf l}\times{\bf t}\right)\times{\bf l}\cot\frac{\theta}{2}\right)\\ &=&\dfrac{1}{2}\left({\bf t}\times{\bf l}+{\bf l}\times\left({\bf t}\times{\bf l}\right)\cot\frac{\theta}{2}\right)\\ &=&\dfrac{1}{2}\left({\bf t}\times{\bf l}+\left(({\bf l}^\top{\bf l}){\bf t}-({\bf l}^\top{\bf t}){\bf l}\right)\cot\frac{\theta}{2}\right)\\ &=&\dfrac{1}{2}\left({\bf t}\times{\bf l}+\left({\bf t}-d{\bf l}\right)\cot\frac{\theta}{2}\right)\tag{12} \end{eqnarray}$

以上より、ねじパラメータ ${\bf l},{\bf m},\theta,d$ が求まりました。

ねじパラメータから剛体変換を求める

ねじパラメータ ${\bf l},{\bf m},\theta,d$ から剛体変換の回転成分を表す行列 ${\bf R}\in{\mathbb R}^{3\times3}$ と平行移動を表すベクトル ${\bf t}\in{\mathbb R}^3$ を求めてみます。
${\bf R}$ はロドリゲスの公式を用いれば、 ${\bf l},\theta$ から求めることができます。
${\bf c}$ は原点を直交射影した点の位置ベクトルなので、 ${\bf c}={\bf l}\times{\bf m}$ と求まります。
後は式 $(8)$ に求めた ${\bf R},{\bf c}$ を代入すれば、 ${\bf t}$ が求まります。

最後に

式 $(11)$ で求めた ${\bf c}$ ですが、参考文献には「 $\theta=0^\circ\ or\ 180^\circ$ のときは、 ${\bf c}$ は定義されない」とあるんですが、
「 $\theta/2=0^\circ\ or\ 180^\circ$ の時に定義できない」の間違いではないかと思っています。(もし間違っていたらコメントください。)

参考文献

デュアルクォータニオンによる3次元位置姿勢の補間とハンドアイキャリブレーション
 Dual Quaternion
Hand-Eye Calibration Using Dual Quaternions