ぱたへね

はてなダイアリーはrustの色分けができないのでこっちに来た

回転角と回転速度

cv

回転は、3次元ベクトルr=[r1,r2,r3]^Tによって、表現することもできる。ベクトルの方向が回転軸を、ベクトルの大きさが回転量を表す。

3次元回転ベクトル

Mを3x3の行列とすると、次の式が成り立つ。

e^{M}=I+\frac{1}{1!}M+\frac{1}{2!}M^{2}+ ... +\frac{1}{n!}M^{n}

任意の3x1ベクトルrに対して、行列[r]xを以下のように定義する。

\left[r_{x} \right] = \begin{pmatrix} 0 & -r_{3} & r_{2} \\ r_{3} & 0 & -r_{1} \\ -r_{2} & r_{1} & 0 \end{pmatrix}
さらに
R=e^{\left[r_{x}\right]
を使用し、さらに
\overline{r}=\frac{r}{\theta}
を使用すると式はこのようになる。
R=e^{\left[r_{x}\right]} =I+ \frac{\sin \theta }{\theta }{\left[r_{x}\right]}+ \frac{1-\cos\theta }{\theta ^{2}}{\left[r_{x}\right]^{2}= I+ \sin \theta \left[\overline{r}_{x}\right]+(1-\cos \theta ){\left[\overline{r}_{x}\right]^{2}

θを求める

Rの回転トレースを求めると、そこからθが求まる。
\theta =\cos ^{-1}\frac{trace(R)-1}{2}

回転軸を求める

r×r=0から
R\overline{r}=\overline{r}
これは回転軸rに対して、回転Rをかけても元のままであることを意味している。また、この式は固有方程式にもなっている。この解として以下の式を得る。回転角の範囲を非負とすることで、一つだけの解を得る。
\overline{r}=\frac{a}{||a||}
ただし
a=\begin{pmatrix} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-r_{12} \end{pmatrix}