四元数与旋转矩阵
复数
\[\begin{aligned}
a &= x+yi\\
ii&=-1
\end{aligned}
\]
四元数基础
定义
\[\begin{aligned}
ii&=jj=kk=-1\\
ij&=-ji=k\\
jk&=-kj=i\\
ki&=-ik=j\\
p&=p_w+p_xi+p_yj+p_zk\\
q&=q_w+q_xi+q_yj+q_zk\\
p\pm q:&=(p_w\pm q_w+(p_x\pm q_x)i+(p_y\pm q_y)j+(p_z\pm q_z)k\\
p*q:&=(p_w+p_xi+p_yj+p_zk)(q_w+q_xi+q_yj+q_zk)\\
&=p_wq_w - p_xq_x - p_yq_y - p_zq_z \\
&+(p_wq_x + q_wp_x + p_yq_z-p_zq_y)i\\
&+(p_wq_y + q_wp_y + p_zq_x-p_xq_z )j\\
&+(p_wq_z + q_wp_z + p_xq_y-p_yq_x )k\\
|p| :&= \sqrt{w^2+x^2+y^2+z^2}
\end{aligned}
\]
其中：
-
\(s,w,x,y,z\in R\)
-
单位四元数: \(|p| = \sqrt{w^2+x^2+y^2+z^2}=1\)
显然，四元数集在上述加法与数乘的定义下满足向量空间的公理，四元数集合是\(R^4\)向量空间。同时四元数也满足乘法定义，所以四元数也是\(R\)代数。
标量、向量型记法
令\(xi+yj+zk=\vec{v}, q = w + \vec{v}\), 其中\(w,x,y,z\in R\)
\[\begin{aligned}
p&=w_p+\vec{v_p}\\
q&=w_q+\vec{v_q}\\
p\pm q:&=w_p\pm w_q + (\vec{v_p}\pm \vec{v_q})\\
p* q:&=w_pw_q - \vec{v_p}\cdot \vec{v_q} + w_p\vec{v_q} + w_q\vec{v_p} + \vec{v_p}\times \vec{v_q}\\
\end{aligned}
\]
绕任意轴的旋转
以下的推导基于右手坐标系。
向量式
设任意轴单位向量为\(n(x,y,z),|n|=1,x,y,z\in R\)，逆时针旋转为\(\theta\)，则向量\(p(p_x,p_y,p_z)\)旋转后的向量\(p'\)为：
\[\begin{aligned}
p_{\parallel}&=(n\cdot p)n\\
p_{\perp}&=p-p_{\parallel}\\
|p_{\perp}|&=|p|sin(\alpha)\\
|n\times p|=|n||p|sin(\alpha)&=|p|sin(\alpha)=|p_{\perp}|\\
\end{aligned}
\]
\(\therefore p_{\perp},n\times p\)构成正交基，\(p'_⊥\)可由\(p_{\perp},n\times p\)线性表示:
\[\begin{aligned}
p'&=p'_{\perp}+p_{\parallel}\\
&=p_{\perp}cos\theta+(v\times p)sin\theta+p_{\parallel}\\
&=(p-p_{\parallel})cos\theta+(v\times p)sin\theta+p_{\parallel}\\
&=pcos\theta+p_{\parallel}(1-cos\theta)+(v\times p)sin\theta\\
&=pcos\theta+(n\cdot p)n(1-cos\theta)+(v\times p)sin\theta
\end{aligned}
\]
矩阵式
由
\[v\times p = \begin{pmatrix}0 & -z & y\\
z & 0 &-x\\
-y & x & 0
\end{pmatrix}p
\]
\[(n\cdot p)n= \begin{pmatrix}x^2 & xy & xz\\
xy & y^2 & yz\\
xz & yz & z^2
\end{pmatrix}p
\]
代入向量式可得：
\[\begin{aligned}
p'&=[cos\theta I_3+sin\theta\begin{pmatrix}0 & -z & y\\
z & 0 &-x\\
-y & x & 0
\end{pmatrix}+(1-cos\theta)\begin{pmatrix}
x^2 & xy & xz\\
xy & y^2 & yz\\
xz & yz & z^2
\end{pmatrix}]p \\
&=\begin{pmatrix}
cos\theta+x^2(1-cos\theta) & -zsin\theta+xy(1-cos\theta) & ysin\theta+xz(1-cos\theta)\\
zsin\theta+xy(1-cos\theta) & cos\theta+y^2(1-cos\theta) & -xsin\theta+yz(1-cos\theta)\\
-ysin\theta+xz(1-cos\theta) & xsin\theta+yz(1-cos\theta) & cos\theta+z^2(1-cos\theta)
\end{pmatrix}p
\end{aligned}
\]
从而得到绕\(\vec{n}\)轴旋转\(\theta\)的旋转矩阵
\[R_n(\theta)=\begin{pmatrix}
cos\theta+x^2(1-cos\theta) & -zsin\theta+xy(1-cos\theta) & ysin\theta+xz(1-cos\theta)\\
zsin\theta+xy(1-cos\theta) & cos\theta+y^2(1-cos\theta) & -xsin\theta+yz(1-cos\theta)\\
-ysin\theta+xz(1-cos\theta) & xsin\theta+yz(1-cos\theta) & cos\theta+z^2(1-cos\theta)
\end{pmatrix}
\]
令\(c= cos\theta, s= sin\theta, n(n_x,n_y,n_z)\)，代入上式得：
\[R_n(\theta)=\begin{pmatrix}
c+n_x^2(1-c) & -n_zs+n_xn_y(1-c) & n_ys+n_xn_z(1-c)\\
n_zs+n_xn_y(1-c) & c+n_y^2(1-c) & -n_xs+n_yn_z(1-c)\\
-n_ys+n_xn_z(1-c) & n_xs+n_yn_z(1-c) & c+n_z^2(1-c)
\end{pmatrix}
\]
四元数与旋转
设\(p=s+\vec{v}\)为单位四元数，则\(q^{-1}=s-\vec{v}\)，令向量\(p(p_x,p_y,p_z)\)旋转后的向量\(p'\)为：
\[\begin{aligned}
p'&=q\vec{p}q^{-1}\\
&= (s+\vec{v}) \vec{p}(s-\vec{v})\\
&= (s\vec{p}-\vec{v}\cdot \vec{p}+\vec{v}\times \vec{p})(s-\vec{v})\\
&= s^2\vec{p}-\underline{s\vec{v}\cdot \vec{p}}+s\vec{v}\times \vec{p}+\underline{s\vec{p}\cdot \vec{v}}-+\underbrace{s\vec{p}\times \vec{v}}_{-s\vec{v}\times \vec{p}}+(\vec{v}\cdot \vec{p})\vec{v}+\underbrace{(\vec{v}\times \vec{p})\cdot\vec{v}}_0-\vec{v}\times \vec{p}\times\vec{v}\\
&=s^2\vec{p}+2s(\vec{v}\times \vec{p})+(\vec{v}\cdot \vec{p})\vec{v}-\underbrace{\vec{v}\times \vec{p}\times\vec{v}}_{\vec{v}^2\vec{p}-(\vec{v}\cdot\vec{p})\vec{v}}\\
&=(s^2-\vec{v}^2)\vec{p}+2s(\vec{v}\times \vec{p})+2(\vec{v}\cdot \vec{p})\vec{v}
\end{aligned}
\]
用单位向量表示\(v\)，令\(\vec{v}=t\vec{n}\)，代入上式得
\[p'=(s^2-t^2)\vec{p}+2st(\vec{n}\times \vec{p})+2t^2(\vec{n}\cdot \vec{p})\vec{n}
\]
四元数表达式与向量式之间的关系
联立向量式\(p'=pcos\theta+(n\cdot p)n(1-cos\theta)+(v\times p)sin\theta\)和四元数表达式可得：
\[\begin{cases}
s^2-t^2=cos\theta \\
2st=sin\theta\\
2t^2 = 1-cos\theta
\end{cases}
\]
解得：\(t=sin(\theta/2), s=cos(\theta/2)\)，从而得到由任意轴单位向量为\(n(x,y,z),|n|=1,x,y,z\in R\)，逆时针旋转为\(\theta\)，的单位四元数：
\[p=(s,t\vec{n})=(cos(\theta/2),(n_xsin\theta, n_ysin\theta, n_zsin\theta ))
\]
或
\[\begin{cases}
s=cos(\theta/2)\\
v_x=n_xsin(\theta/2)\\
v_y=n_ysin(\theta/2)\\
v_z=n_zsin(\theta/2)
\end{cases}
\]
四元数表达式与矩阵式之间的关系
利用旋转四元数中的分量\(w^2+v_x^2+v_y^2+v_z^2=1\)对旋转矩阵进行化简：
\[\begin{aligned}
cos(\theta/2)&=w\\
n_xsin(\theta/2)&=v_x=x\\
n_ysin(\theta/2)&=v_y=y\\
n_zsin(\theta/2)&=v_z=z\\
c=cos\theta&=2cos^2(\theta/2)-1=2w^2-1\\
1-cos\theta&=2sin^2(\theta/2)\\
n_x^2(1-cos\theta)&=2n_x^2sin^2(\theta/2)=2v_x^2\\
n_y^2(1-cos\theta)&=2n_y^2\sim =2v_y^2\\
n_z^2(1-cos\theta)&=2n_z^2\sim =2v_z^2\\
sin\theta&=2sin(\theta/2)cos(\theta/2)=2sin(\theta/2)w\\
n_xsin(\theta)&=n_x2sin(\theta/2)w=2wv_x\\
n_ysin(\theta)&=n_y\sim=2wv_y\\
n_zsin(\theta)&=n_z\sim=2wv_z\\
n_xn_y(1-cos(\theta))&=2n_xn_ysin^2(\theta/2)=2v_xv_y\\
n_xn_z(1-cos(\theta))&=2n_xn_z\sim=2v_xv_z\\
n_yn_z(1-cos(\theta))&=2n_yn_z\sim=2v_yv_z\\
\end{aligned}
\]
\[\begin{aligned}
R_n(\theta)&=\begin{pmatrix}
c+n_x^2(1-c) & -n_zs+n_xn_y(1-c) & n_ys+n_xn_z(1-c)\\
n_zs+n_xn_y(1-c) & c+n_y^2(1-c) & -n_xs+n_yn_z(1-c)\\
-n_ys+n_xn_z(1-c) & n_xs+n_yn_z(1-c) & c+n_z^2(1-c)
\end{pmatrix}\\
&=\begin{pmatrix}
2w^2+2x^2-1 & 2xy-2wz & 2xz+2wy\\
2xy+2wz & 2w^2+2y^2-1 & 2yz-2wx\\
2xz-2wy & 2yz+2wx & 2w^2+2z^2-1
\end{pmatrix}\\
&=\begin{pmatrix}
1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy\\
2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx\\
2xz-2wy & 2yz+2wx & 1-2x^2-2y^2
\end{pmatrix}
\end{aligned}
\]
分开助记：
\[\begin{aligned}
R_n(\theta)&=[cos\theta I_3+sin\theta\begin{pmatrix}0 & -n_z & n_y\\
n_z & 0 &-n_x\\
-n_y & n_x & 0
\end{pmatrix}+(1-cos\theta)\begin{pmatrix}
n_x^2 & n_xn_y & n_xn_z\\
n_xn_y & n_y^2 & n_yn_z\\
n_xn_z & n_yn_z & n_z^2
\end{pmatrix}]\\
&=(2w^2-1)\begin{pmatrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1
\end{pmatrix}+2w\begin{pmatrix}
0 & -z & y\\
z & 0 &-x\\
-y & x & 0
\end{pmatrix}+\begin{pmatrix}
0 & 2xy & 2xz\\
2xy & 0 & 2yz\\
2xz & 2yz & 0
\end{pmatrix}\\
&=\begin{pmatrix}
2w^2-1 & 0 & 0\\
0 & 2w^2-1 & 0\\
0 & 0 & 2w^2-1
\end{pmatrix}+\begin{pmatrix}
0 & -2wz & 2wy\\
2wz & 0 & -2wx\\
-2wy & 2wx & 0
\end{pmatrix}+\begin{pmatrix}
0 & 2xy & 2xz\\
2xy & 0 & 2yz\\
2xz & 2yz & 0
\end{pmatrix}
\end{aligned}
\]
通过四元数，旋转矩阵用了竟然是（对角阵+反称矩阵+对称矩阵），神奇也优美！
