1. 从四元数到旋转矩阵
推导过程利用以下矩阵(该矩阵的推导过程链接),它能计算绕任意轴的旋转:
R
(
n
,
θ
)
=
[
(
1
−
c
o
s
θ
)
n
x
2
+
c
o
s
θ
(
1
−
c
o
s
θ
)
n
x
n
y
−
n
z
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
z
+
n
y
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
y
+
n
z
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
2
+
c
o
s
θ
(
1
−
c
o
s
θ
)
n
y
n
z
−
n
x
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
z
−
n
y
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
n
z
+
n
x
s
i
n
θ
(
1
−
c
o
s
θ
)
n
z
2
+
c
o
s
θ
]
\textbf{R}(\textbf{n},\theta)=\begin{bmatrix} (1-cos\theta)n_x^2+cos\theta & (1-cos\theta)n_xn_y-n_zsin\theta & (1-cos\theta)n_xn_z+n_ysin\theta\\ (1-cos\theta)n_xn_y+n_zsin\theta & (1-cos\theta)n_y^2+cos\theta & (1-cos\theta)n_yn_z-n_xsin\theta\\ (1-cos\theta)n_xn_z-n_ysin\theta & (1-cos\theta)n_yn_z+n_xsin\theta & (1-cos\theta)n_z^2+cos\theta\\ \end{bmatrix}
R(n,θ)=⎣⎡(1−cosθ)nx2+cosθ(1−cosθ)nxny+nzsinθ(1−cosθ)nxnz−nysinθ(1−cosθ)nxny−nzsinθ(1−cosθ)ny2+cosθ(1−cosθ)nynz+nxsinθ(1−cosθ)nxnz+nysinθ(1−cosθ)nynz−nxsinθ(1−cosθ)nz2+cosθ⎦⎤
该矩阵用
n
\textbf{n}
n 和
θ
\theta
θ 表示,但其中四元数的分量是:
w
=
c
o
s
(
θ
/
2
)
x
=
n
x
s
i
n
(
θ
/
2
)
y
=
n
y
s
i
n
(
θ
/
2
)
z
=
n
z
s
i
n
(
θ
/
2
)
\begin{aligned} w&=cos(\theta/2)\\ x&=\textbf{n}_xsin(\theta/2)\\ y&=\textbf{n}_ysin(\theta/2)\\ z&=\textbf{n}_zsin(\theta/2)\\ \end{aligned}
wxyz=cos(θ/2)=nxsin(θ/2)=nysin(θ/2)=nzsin(θ/2)
将矩阵变形以带入
w
,
x
,
y
,
z
w,x,y,z
w,x,y,z。首先考虑矩阵对角线上的元素:
m
11
=
n
x
2
(
1
−
c
o
s
θ
)
+
c
o
s
θ
=
n
x
2
−
n
x
2
c
o
s
θ
+
c
o
s
θ
=
1
−
1
+
n
x
2
−
n
x
2
c
o
s
θ
+
c
o
s
θ
=
1
−
(
1
−
n
x
2
+
n
x
2
c
o
s
θ
−
c
o
s
θ
)
=
1
−
(
1
−
c
o
s
θ
−
n
x
2
+
n
x
2
c
o
s
θ
)
=
1
−
(
1
−
n
x
2
)
(
1
−
c
o
s
θ
)
\begin{aligned} m_{11}&= \textbf{n}_x^2(1-cos\theta)+cos\theta\\ &=\textbf{n}_x^2-\textbf{n}_x^2cos\theta+cos\theta\\ &=1-1+\textbf{n}_x^2-\textbf{n}_x^2cos\theta+cos\theta\\ &=1-(1-\textbf{n}_x^2+\textbf{n}_x^2cos\theta-cos\theta)\\ &=1-(1-cos\theta-\textbf{n}_x^2+\textbf{n}_x^2cos\theta)\\ &=1-(1-\textbf{n}_x^2)(1-cos\theta)\\ \end{aligned}
m11=nx2(1−cosθ)+cosθ=nx2−nx2cosθ+cosθ=1−1+nx2−nx2cosθ+cosθ=1−(1−nx2+nx2cosθ−cosθ)=1−(1−cosθ−nx2+nx2cosθ)=1−(1−nx2)(1−cosθ)
根据三角函数倍角公式有
c
o
s
θ
=
1
−
2
s
i
n
2
(
θ
/
2
)
cos\theta=1-2sin^2(\theta/2)
cosθ=1−2sin2(θ/2),带入上式:
m
11
=
1
−
(
1
−
n
x
2
)
(
1
−
c
o
s
θ
)
=
1
−
(
1
−
n
x
2
)
(
2
s
i
n
2
(
θ
/
2
)
)
=
1
−
2
s
i
n
2
(
θ
/
2
)
+
2
n
x
2
s
i
n
2
(
θ
/
2
)
=
1
−
2
(
1
−
c
o
s
2
(
θ
/
2
)
)
+
2
n
x
2
s
i
n
2
(
θ
/
2
)
=
1
−
2
+
2
c
o
s
2
(
θ
/
2
)
)
+
2
n
x
2
s
i
n
2
(
θ
/
2
)
=
−
1
+
2
c
o
s
2
(
θ
/
2
)
)
+
2
n
x
2
s
i
n
2
(
θ
/
2
)
\begin{aligned} m_{11}&=1-(1-\textbf{n}_x^2)(1-cos\theta)\\ &=1-(1-\textbf{n}_x^2)(2sin^2(\theta/2))\\ &=1-2sin^2(\theta/2)+2\textbf{n}_x^2sin^2(\theta/2)\\ &=1-2(1-cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ &=1-2+2cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ &=-1+2cos^2(\theta/2))+2\textbf{n}_x^2sin^2(\theta/2)\\ \end{aligned}
m11=1−(1−nx2)(1−cosθ)=1−(1−nx2)(2sin2(θ/2))=1−2sin2(θ/2)+2nx2sin2(θ/2)=1−2(1−cos2(θ/2))+2nx2sin2(θ/2)=1−2+2cos2(θ/2))+2nx2sin2(θ/2)=−1+2cos2(θ/2))+2nx2sin2(θ/2)
带入
w
,
x
w,x
w,x即可求得,
m
11
=
−
1
+
2
w
2
+
2
x
2
m_{11}=-1+2w^2+2x^2
m11=−1+2w2+2x2,等价于
m
11
=
1
−
2
y
2
−
2
z
2
m_{11}=1-2y^2-2z^2
m11=1−2y2−2z2
m
11
=
w
2
+
x
2
−
y
2
−
z
2
m_{11}=w^2+x^2-y^2-z^2
m11=w2+x2−y2−z2
对角元素
m
22
,
m
33
m_{22},m_{33}
m22,m33可以用同样的方式求得。
以
m
12
m_{12}
m12为例,使用三角形的倍角公式
s
i
n
θ
=
2
s
i
n
(
θ
/
2
)
c
o
s
(
θ
/
2
)
sin\theta=2sin(\theta/2)cos(\theta/2)
sinθ=2sin(θ/2)cos(θ/2) 推导非对角线元素,其他非对角线元素可用类似方式推导:
m
12
=
n
x
n
y
(
1
−
c
o
s
θ
)
−
n
z
s
i
n
θ
=
n
x
n
y
(
1
−
(
1
−
2
2
s
i
n
(
θ
/
2
)
)
)
−
n
z
2
s
i
n
(
θ
/
2
)
c
o
s
(
θ
/
2
)
=
n
x
n
y
(
2
s
i
n
2
(
θ
/
2
)
)
−
2
n
z
s
i
n
(
θ
/
2
)
c
o
s
(
θ
/
2
)
=
2
(
n
x
s
i
n
(
θ
/
2
)
n
y
s
i
n
(
θ
/
2
)
)
−
2
c
o
s
(
θ
/
2
)
(
n
z
s
i
n
(
θ
/
2
)
)
=
2
x
y
−
2
w
z
\begin{aligned} m_{12}&=\textbf{n}_x\textbf{n}_y(1-cos\theta)-\textbf{n}_zsin\theta\\ &=\textbf{n}_x\textbf{n}_y(1-(1-2^2sin(\theta/2)))-\textbf{n}_z2sin(\theta/2)cos(\theta/2)\\ &=\textbf{n}_x\textbf{n}_y(2sin^2(\theta/2))-2\textbf{n}_zsin(\theta/2)cos(\theta/2)\\ &=2(\textbf{n}_xsin(\theta/2)\textbf{n}_ysin(\theta/2))-2cos(\theta/2)(\textbf{n}_zsin(\theta/2))\\ &=2xy-2wz \end{aligned}
m12=nxny(1−cosθ)−nzsinθ=nxny(1−(1−22sin(θ/2)))−nz2sin(θ/2)cos(θ/2)=nxny(2sin2(θ/2))−2nzsin(θ/2)cos(θ/2)=2(nxsin(θ/2)nysin(θ/2))−2cos(θ/2)(nzsin(θ/2))=2xy−2wz
最终推导出的四元数构造的完整旋转矩阵为以下形式:
R
(
n
,
θ
)
=
[
1
−
2
y
2
−
2
z
2
2
x
y
−
2
w
z
2
x
z
+
2
w
y
2
x
y
+
2
w
z
1
−
2
x
2
−
2
z
2
2
y
z
−
2
w
x
2
x
z
−
2
w
y
2
y
z
+
2
w
x
1
−
2
x
2
−
2
y
2
]
(2)
R(n,\theta)= \begin{bmatrix} 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{bmatrix} \tag{2}
R(n,θ)=⎣⎡1−2y2−2z22xy+2wz2xz−2wy2xy−2wz1−2x2−2z22yz+2wx2xz+2wy2yz−2wx1−2x2−2y2⎦⎤(2)
2. 从旋转矩阵到四元数
利用公式(2),将对角线元素求和可以得到:
t
r
(
M
)
=
m
11
+
m
22
+
m
33
=
(
1
−
2
y
2
−
2
z
2
)
+
(
1
−
2
x
2
−
2
z
2
)
+
(
1
−
2
x
2
−
2
y
2
)
=
3
−
4
(
x
2
+
y
2
+
z
2
)
=
3
−
4
(
1
−
w
2
)
=
4
w
2
−
1
\begin{aligned} tr(\textbf{M})&=m11+m22+m33\\ &=(1-2y^2-2z^2)+(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ &=3-4(x^2+y^2+z^2)\\ &=3-4(1-w^2)\\ &=4w^2-1\\ \end{aligned}
tr(M)=m11+m22+m33=(1−2y2−2z2)+(1−2x2−2z2)+(1−2x2−2y2)=3−4(x2+y2+z2)=3−4(1−w2)=4w2−1
因此可以推导出:
w
=
m
11
+
m
22
+
m
33
+
1
2
w=\frac{\sqrt{m11+m22+m33+1}}{2}
w=2m11+m22+m33+1
通过使轨迹中三个元素中的两个为负,可以用类似的方法求得其他三个元素:
m
11
−
m
22
−
m
33
=
(
1
−
2
y
2
−
2
z
2
)
−
(
1
−
2
x
2
−
2
z
2
)
−
(
1
−
2
x
2
−
2
y
2
)
=
4
x
2
−
1
−
m
11
+
m
22
−
m
33
=
−
(
1
−
2
y
2
−
2
z
2
)
+
(
1
−
2
x
2
−
2
z
2
)
−
(
1
−
2
x
2
−
2
y
2
)
=
4
y
2
−
1
−
m
11
−
m
22
+
m
33
=
−
(
1
−
2
y
2
−
2
z
2
)
−
(
1
−
2
x
2
−
2
z
2
)
+
(
1
−
2
x
2
−
2
y
2
)
=
4
z
2
−
1
\begin{aligned} m11-m22-m33&=(1-2y^2-2z^2)-(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ &=4x^2-1\\ -m11+m22-m33&=-(1-2y^2-2z^2)+(1-2x^2-2z^2)-(1-2x^2-2y^2)\\ &=4y^2-1\\ -m11-m22+m33&=-(1-2y^2-2z^2)-(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ &=4z^2-1\\ \end{aligned}
m11−m22−m33−m11+m22−m33−m11−m22+m33=(1−2y2−2z2)−(1−2x2−2z2)−(1−2x2−2y2)=4x2−1=−(1−2y2−2z2)+(1−2x2−2z2)−(1−2x2−2y2)=4y2−1=−(1−2y2−2z2)−(1−2x2−2z2)+(1−2x2−2y2)=4z2−1
这种方法是存在一定问题的:平方根的结果总是正值(没有选择是负根还是正根的依据)。
q
q
q和
−
q
-q
−q代表相同的方位,因此我们可以任意选择用非负根作为4个分量中的一个并仍能得到正确的四元数,只是不能对四元数的所有4个数都用这种方法。
可以考虑计算相对于对角线的对称位置上元素的和与差:
m
21
+
m
12
=
(
2
x
y
+
2
w
z
)
+
(
2
x
y
−
2
w
z
)
=
4
x
y
m
21
−
m
12
=
(
2
x
y
+
2
w
z
)
−
(
2
x
y
−
2
w
z
)
=
4
w
z
m
13
+
m
31
=
(
2
x
z
+
2
w
y
)
+
(
2
x
z
−
2
w
y
)
=
4
x
z
m
13
−
m
31
=
(
2
x
z
+
2
w
y
)
−
(
2
x
z
−
2
w
y
)
=
4
w
y
m
32
+
m
23
=
(
2
y
z
+
2
w
x
)
+
(
2
y
z
−
2
w
x
)
=
4
y
z
m
32
−
m
23
=
(
2
y
z
+
2
w
x
)
−
(
2
y
z
−
2
w
x
)
=
4
w
x
\begin{aligned} m21+m12&=(2xy+2wz)+(2xy-2wz)=4xy\\ m21-m12&=(2xy+2wz)-(2xy-2wz)=4wz\\ m13+m31&=(2xz+2wy)+(2xz-2wy)=4xz\\ m13-m31&=(2xz+2wy)-(2xz-2wy)=4wy\\ m32+m23&=(2yz+2wx)+(2yz-2wx)=4yz\\ m32-m23&=(2yz+2wx)-(2yz-2wx)=4wx\\ \end{aligned}
m21+m12m21−m12m13+m31m13−m31m32+m23m32−m23=(2xy+2wz)+(2xy−2wz)=4xy=(2xy+2wz)−(2xy−2wz)=4wz=(2xz+2wy)+(2xz−2wy)=4xz=(2xz+2wy)−(2xz−2wy)=4wy=(2yz+2wx)+(2yz−2wx)=4yz=(2yz+2wx)−(2yz−2wx)=4wx
因此,一旦用对角线元素和/差的平方根解得了4个值中的一个,就能用以下方法计算其他三个:
w
=
m
11
+
m
22
+
m
33
+
1
2
⟹
x
=
m
32
−
m
23
4
w
y
=
m
13
−
m
31
4
w
z
=
m
21
−
m
12
4
w
x
=
m
11
−
m
22
−
m
33
+
1
2
⟹
w
=
m
32
−
m
23
4
x
y
=
m
21
+
m
12
4
x
z
=
m
13
+
m
31
4
x
y
=
−
m
11
+
m
22
−
m
33
+
1
2
⟹
w
=
m
13
−
m
31
4
y
x
=
m
12
+
m
21
4
y
z
=
m
23
+
m
32
4
y
z
=
−
m
11
−
m
22
+
m
33
+
1
2
⟹
w
=
m
21
−
m
12
4
z
x
=
m
13
+
m
31
4
z
y
=
m
23
+
m
32
4
z
\begin{aligned} w=\frac{\sqrt{m11+m22+m33+1}}{2}\implies&x=\frac{m32-m23}{4w} \quad\quad y=\frac{m13-m31}{4w} \quad\quad z=\frac{m21-m12}{4w} \\ x=\frac{\sqrt{m11-m22-m33+1}}{2}\implies&w=\frac{m32-m23}{4x} \quad\quad y=\frac{m21+m12}{4x} \quad\quad z=\frac{m13+m31}{4x} \\ y=\frac{\sqrt{-m11+m22-m33+1}}{2}\implies&w=\frac{m13-m31}{4y} \quad\quad x=\frac{m12+m21}{4y} \quad\quad z=\frac{m23+m32}{4y} \\ z=\frac{\sqrt{-m11-m22+m33+1}}{2}\implies&w=\frac{m21-m12}{4z} \quad\quad x=\frac{m13+m31}{4z} \quad\quad y=\frac{m23+m32}{4z} \\ \end{aligned}
w=2m11+m22+m33+1⟹x=2m11−m22−m33+1⟹y=2−m11+m22−m33+1⟹z=2−m11−m22+m33+1⟹x=4wm32−m23y=4wm13−m31z=4wm21−m12w=4xm32−m23y=4xm21+m12z=4xm13+m31w=4ym13−m31x=4ym12+m21z=4ym23+m32w=4zm21−m12x=4zm13+m31y=4zm23+m32
我们可以先计算四个分量中的其中一个,比如
w
w
w,然后在计算
x
,
y
,
z
x,y,z
x,y,z。这样也会出现问题,如果
w
=
0
w=0
w=0,除法就没有意义了,如果
w
w
w非常小,将会出现数值不稳定。所以我们可以采取的策略是,先判断
w
,
x
,
y
,
z
w,x,y,z
w,x,y,z中哪个元素最大,用矩阵对角线计算该元素,再计算其他三个。