文章目录
3D矢量旋转
x
=
x
∥
+
x
⊥
x
∥
=
u
(
∥
x
∥
cos
α
)
=
u
u
T
x
x
⊥
=
x
−
x
∥
=
x
−
u
u
T
x
\begin{aligned} \bold{x}&=\bold{x}_{\parallel}+\bold{x}_{\perp}\\ \bold{x}_{\parallel}&=\bold{u}(\Vert \bold{x} \Vert\cos\alpha)=\bold{uu}^T\bold{x}\\ \bold{x}_{\perp}&=\bold{x}-\bold{x}_{\parallel}=\bold{x}-\bold{uu}^T\bold{x} \end{aligned}
xx∥x⊥=x∥+x⊥=u(∥x∥cosα)=uuTx=x−x∥=x−uuTx
平行部分不旋转:
x
∥
′
=
x
∥
\bold{x}'_{\parallel}=\bold{x}_{\parallel}
x∥′=x∥
垂直部分在垂直于
u
\bold{u}
u向量的平面内经历一个平面旋转。如果我们建立这个平面的一个正交基
{
e
1
,
e
2
}
\{\bold{e}_1,\bold{e}_2\}
{e1,e2}:
e
1
=
x
⊥
e
2
=
u
×
x
⊥
=
u
×
x
\bold{e}_1=\bold{x}_{\perp}\\ \bold{e}_2=\bold{u}\times\bold{x}_{\perp}=\bold{u}\times\bold{x}
e1=x⊥e2=u×x⊥=u×x
满足
∥
e
1
∥
=
∥
e
2
∥
\Vert\bold{e}_1\Vert=\Vert\bold{e}_2\Vert
∥e1∥=∥e2∥,
x
⊥
=
e
1
⋅
1
+
e
2
⋅
0
\bold{x}_{\perp}=\bold{e}_1\cdot 1+\bold{e}_2\cdot 0
x⊥=e1⋅1+e2⋅0 平面上的旋转角度
ϕ
\phi
ϕ
x
⊥
′
=
e
1
cos
ϕ
+
e
2
sin
ϕ
=
x
⊥
cos
ϕ
+
(
u
×
x
)
sin
ϕ
\begin{aligned} \bold{x}_{\perp}' &= \bold{e}_1 \cos\phi+\bold{e}_2\sin\phi\\ &= \bold{x}_{\perp}\cos\phi +(\bold{u}\times \bold{x})\sin\phi \end{aligned}
x⊥′=e1cosϕ+e2sinϕ=x⊥cosϕ+(u×x)sinϕ
则
x
′
=
x
∥
′
+
x
⊥
′
=
x
∥
+
x
⊥
cos
ϕ
+
(
u
×
x
)
sin
ϕ
\begin{aligned} \bold{x}'&=\bold{x}_{\parallel}'+\bold{x}_{\perp}'\\ &=\bold{x}_{\parallel}+\bold{x}_{\perp}\cos\phi+(\bold{u}\times\bold{x})\sin\phi \end{aligned}
x′=x∥′+x⊥′=x∥+x⊥cosϕ+(u×x)sinϕ
旋转向量
任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量称为旋转向量(或轴角/角轴,Axis-Angle),只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
考虑某个用旋转矩阵
R
\bold{R}
R表示的旋转。假设旋转轴为一个单位长度的向量
n
\bold{n}
n,角度为
θ
\theta
θ,那么向量
θ
n
\theta \bold{n}
θn也可以描述这个旋转。
从旋转向量到旋转矩阵的转换过程由罗德里格斯公式(Rodrigues’s Formula)表明,旋转向量其实就是特殊正交群的李代数,从李代数层面证明罗德里格斯公式。
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
\bold{R} =\cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}+\sin \theta \bold{n}^{\wedge }
R=cosθI+(1−cosθ)nnT+sinθn∧
t
r
(
R
)
=
cos
θ
t
r
(
I
)
+
(
1
−
cos
θ
)
t
r
(
n
n
T
)
+
sin
θ
t
r
(
n
∧
)
=
3
cos
θ
+
1
−
cos
θ
+
0
=
2
cos
θ
+
1
\begin{aligned} {\rm tr}\left( \bold{R}\right) &=\cos \theta{\rm tr}(\bold{I})+(1-\cos\theta){\rm tr} (\bold{n}\bold{n}^T)+\sin\theta{\rm tr}(\bold{n}^{\wedge}) \\&=3\cos \theta +1-\cos \theta +0=2\cos \theta +1 \end{aligned}
tr(R)=cosθtr(I)+(1−cosθ)tr(nnT)+sinθtr(n∧)=3cosθ+1−cosθ+0=2cosθ+1
因此
θ
=
arccos
t
r
(
R
)
−
1
2
\theta=\arccos \dfrac{\rm tr(\bold{R})-1}{2}
θ=arccos2tr(R)−1
旋转轴上的向量在旋转之后不发生改变
R
n
=
n
\bold{Rn}=\bold{n}
Rn=n
因此转轴
n
\bold{n}
n是旋转矩阵
R
\bold{R}
R特征值为1对应的特征向量。求解此方程,再归一化。
四元数
四元数定义
如果我们有两个复数
A
=
a
+
b
i
A=a+bi
A=a+bi和
C
=
c
+
d
i
C=c+di
C=c+di,接着构建
Q
=
A
+
C
j
Q=A+Cj
Q=A+Cj并定义
k
≜
i
j
k\triangleq ij
k≜ij则产生一个四元数空间
H
\mathbb{H}
H中的数:
Q
=
a
+
b
i
+
c
j
+
d
k
∈
H
Q=a+bi+cj+dk\in\mathbb{H}
Q=a+bi+cj+dk∈H
其中
{
a
,
b
,
c
,
d
}
∈
R
\{a,b,c,d\}\in\mathbb{R}
{a,b,c,d}∈R,
{
i
,
j
,
k
}
\{i,j,k\}
{i,j,k}是三个虚单位数:
i
2
=
j
2
=
k
2
=
i
j
k
=
−
1
i^2=j^2=k^2=ijk=-1
i2=j2=k2=ijk=−1
并且由上式可以导出
i
j
=
−
j
i
=
k
,
j
k
=
−
k
j
=
i
,
k
i
=
−
i
k
=
j
ij=-ji=k,\quad jk=-kj=i,\quad ki=-ik=j
ij=−ji=k,jk=−kj=i,ki=−ik=j
一个四元数也可以表示为标量+向量的形式:
Q
=
q
w
+
q
x
i
+
q
y
j
+
q
z
k
⟺
Q
=
q
w
+
q
v
Q=q_w+q_x i+q_y j+q_z k \iff Q=q_w+\bold{q}_v
Q=qw+qxi+qyj+qzk⟺Q=qw+qv
其中
q
w
q_w
qw代表实部,
q
v
=
q
x
i
+
q
y
j
+
q
z
k
=
(
q
x
,
q
y
,
q
z
)
\bold{q}_v=q_x i+q_y j+q_z k=(q_x,q_y,q_z)
qv=qxi+qyj+qzk=(qx,qy,qz)作为虚部或向量部分
我们更多会将一个四元数
Q
Q
Q作为4维向量
q
\bold{q}
q
q
≜
[
q
w
q
v
]
=
[
q
w
q
x
q
y
q
z
]
\bold{q}\triangleq\begin{bmatrix}q_w\\\bold{q}_v\end{bmatrix}=\begin{bmatrix}q_w\\q_x\\q_y\\q_z\end{bmatrix}
q≜[qwqv]=
qwqxqyqz
常规单位长度的复数
z
=
e
i
θ
\bold{z}=e^{i\theta}
z=eiθ可以编码2D平面上的旋转:使用复数乘法
x
′
=
z
⋅
x
\bold{x}'=\bold{z}\cdot\bold{x}
x′=z⋅x
单位长度的扩展复数(四元数)
q
=
e
(
q
x
i
+
q
y
j
+
q
z
k
)
θ
/
2
\bold{q}=e^{(q_x i+q_y j+q_z k)\theta/2}
q=e(qxi+qyj+qzk)θ/2可以编码3D空间中的旋转:使用双四元数乘法
x
=
q
⊗
x
⊗
q
∗
\bold{x}=\bold{q}\otimes\bold{x}\otimes\bold{q}^*
x=q⊗x⊗q∗
四元数的基本运算
相乘
q
a
⊗
q
b
=
(
s
a
+
v
a
)
⊗
(
s
b
+
v
b
)
=
s
a
s
b
+
s
a
v
b
+
s
b
v
a
+
v
a
⊗
v
b
\bold{q_a}\otimes\bold{q_b}=(s_a+\bold{v_a})\otimes(s_b+\bold{v_b})=s_as_b+s_a\bold{v_b}+s_b\bold{v_a}+\bold{v_a}\otimes\bold{v_b}
qa⊗qb=(sa+va)⊗(sb+vb)=sasb+savb+sbva+va⊗vb
最后一项两个向量相乘根据虚部满足的关系:
{
i
2
=
j
2
=
k
2
=
−
1
i
j
=
k
,
j
i
=
−
k
j
k
=
i
,
k
j
=
−
i
k
i
=
j
,
i
k
=
−
j
\begin{cases} \rm i^2=j^2=k^2=-1\\ \rm ij=k,ji=-k\\ \rm jk=i,kj=-i\\ \rm ki=j,ik=-j \end{cases}
⎩
⎨
⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j
第一个规则相同虚部相乘等价于向量点乘的相反数,后三条规则相异虚部相乘与与向量叉乘相同,则:
v
a
⊗
v
b
=
−
v
a
T
v
b
+
v
a
×
v
b
\bold{v}_a\otimes\bold{v}_b=-\bold{v}_a^T\bold{v}_b+\bold{v}_a\times\bold{v}_b
va⊗vb=−vaTvb+va×vb
q
a
⊗
q
b
=
(
s
a
s
b
−
v
a
T
v
b
)
+
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
=
[
s
a
s
b
−
v
a
T
v
b
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
]
=
[
s
a
−
v
a
T
v
a
s
a
I
+
v
a
∧
]
[
s
b
v
b
]
=
[
s
b
−
v
b
T
v
b
s
b
I
−
v
b
∧
]
[
s
a
v
a
]
\begin{aligned} \bold{q}_a\otimes\bold{q}_b &=(s_as_b-\bold{v}_a^T\bold{v}_b)+s_a\bold{v}_b+s_b\bold{v}_a+\bold{v}_a\times\bold{v}_b\\ &=\begin{bmatrix} s_{a}s_{b}-\bold{v}_{a}^{T}\bold{v}_{b} \\ s_{a}\bold{v}_{b}+s_{b}\bold{v}_{a}+\bold{v}_a\times \bold{v}_b \end{bmatrix}\\ &=\begin{bmatrix} s_{a} & -\bold{v}_{a}^{T} \\ \bold{v}_{a} & s_a\bold{I}+\bold{v}_{a}^{\wedge} \end{bmatrix}\begin{bmatrix} s_{b} \\ \bold{v}_{b} \end{bmatrix}\\ &=\begin{bmatrix} s_{b} & -\bold{v}_{b}^{T} \\ \bold{v}_{b} & s_{b}\bold{I}-\bold{v}_{b}^{\wedge} \end{bmatrix}\begin{bmatrix} s_a \\ \bold{v}_a \end{bmatrix} \end{aligned}
qa⊗qb=(sasb−vaTvb)+savb+sbva+va×vb=[sasb−vaTvbsavb+sbva+va×vb]=[sava−vaTsaI+va∧][sbvb]=[sbvb−vbTsbI−vb∧][sava]
四元数相乘满足结合律但不满足交换律:
q
a
⊗
q
b
≠
q
b
⊗
q
a
(
q
a
⊗
q
b
)
⊗
q
c
=
q
a
⊗
(
q
b
⊗
q
c
)
\begin{aligned} \bold{q}_a\otimes\bold{q}_b&\neq\bold{q}_b\otimes \bold{q}_a \\ (\bold{q}_a\otimes \bold{q}_b)\otimes\bold{q}_c&=\bold{q}_a\otimes(\bold{q}_b\otimes\bold{q}_c) \end{aligned}
qa⊗qb(qa⊗qb)⊗qc=qb⊗qa=qa⊗(qb⊗qc)
设
q
=
[
s
v
]
\bold{q}=\begin{bmatrix}s \\ \bold{v}\end{bmatrix}
q=[sv]定义如下的符号
[
⋅
]
L
[\cdot]_{L}
[⋅]L和
[
⋅
]
R
[\cdot]_{R}
[⋅]R为
[
q
]
L
=
[
s
−
v
T
v
s
I
+
v
∧
]
[
q
]
R
=
[
s
−
v
T
v
s
I
−
v
∧
]
\begin{aligned} \left[\bold{q}\right]_{L} &=\begin{bmatrix} s & -\bold{v}^{T} \\ \bold{v} & s\bold{I}+\bold{v}^{\wedge } \end{bmatrix} \\ \left[\bold{q}\right]_{R} &=\begin{bmatrix} s & -\bold{v}^{T} \\ \bold{v} & s\bold{I}-\bold{v}^{\wedge } \end{bmatrix} \end{aligned}
[q]L[q]R=[sv−vTsI+v∧]=[sv−vTsI−v∧]
则
q
a
⊗
q
b
=
[
q
a
]
L
q
b
=
[
q
b
]
R
q
a
\begin{aligned} \bold{q}_{a}\otimes\bold{q}_{b}=\left[\bold{q}_{a}\right]_{L}\bold{q}_{b}=\left[\bold{q}_{b}\right]_{R}\bold{q}_{a} \end{aligned}
qa⊗qb=[qa]Lqb=[qb]Rqa
共轭
四元数的共轭就是把虚部取成相反数:
q
a
∗
=
s
a
−
v
a
=
[
s
a
−
v
a
]
\begin{aligned} \bold{q}_a^*=s_a-\bold{v}_a=\begin{bmatrix}s_a\\-\bold{v}_a\end{bmatrix} \end{aligned}
qa∗=sa−va=[sa−va]
四元数共轭与其本身相乘,会得到一个实四元数
q
a
∗
⊗
q
a
=
q
a
⊗
q
a
∗
=
[
s
a
2
+
v
a
T
v
a
0
]
\begin{aligned} \bold{q}_a^*\otimes\bold{q}_a=\bold{q}_a\otimes\bold{q}_a^*=\begin{bmatrix}s_a^2+\bold{v}_a^T\bold{v}_a\\ \bold{0} \end{bmatrix} \end{aligned}
qa∗⊗qa=qa⊗qa∗=[sa2+vaTva0]
模长
∥
q
a
∥
=
q
a
⊗
q
a
∗
=
s
a
2
+
v
a
T
v
a
=
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
\begin{aligned} \lVert \bold{q}_a \rVert=\sqrt{\bold{q}_a\otimes\bold{q}_a^*}=\sqrt{s_a^2+\bold{v}_a^T\bold{v}_a}=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2} \end{aligned}
∥qa∥=qa⊗qa∗=sa2+vaTva=sa2+xa2+ya2+za2
可以验证,两个四元数乘积的模即模的乘积
∥
q
a
⊗
q
b
∥
=
∥
q
a
∥
∥
q
b
∥
\begin{aligned} \lVert \bold{q}_a\otimes\bold{q}_b \rVert = \lVert \bold{q}_a\rVert \lVert \bold{q}_b\rVert \end{aligned}
∥qa⊗qb∥=∥qa∥∥qb∥
证明:
∥
q
a
⊗
q
b
∥
2
=
(
s
a
s
b
−
v
a
T
v
b
)
2
+
(
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
)
T
(
s
a
v
b
+
s
b
v
a
+
v
a
×
v
b
)
=
s
a
2
s
b
2
−
2
s
a
s
b
v
a
T
v
b
+
(
v
a
T
v
b
)
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
(
v
a
×
v
b
)
T
(
v
a
×
v
b
)
+
s
a
s
b
v
b
T
v
a
+
s
a
s
b
v
a
T
v
b
+
s
a
v
b
T
(
v
a
×
v
b
)
⏟
0
+
s
b
v
a
T
(
v
a
×
v
b
)
⏟
0
+
s
a
(
v
a
×
v
b
)
T
v
b
⏟
0
+
s
b
(
v
a
×
v
b
)
T
v
a
⏟
0
=
s
a
2
s
b
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
(
v
a
T
v
b
)
2
+
(
v
a
×
v
b
)
T
=
s
a
2
s
b
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
∥
v
a
∥
2
∥
v
b
∥
2
cos
2
θ
+
∥
v
a
∥
2
∥
v
b
∥
2
sin
2
θ
=
s
a
2
s
b
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
∥
v
a
∥
2
∥
v
b
∥
2
\begin{aligned} \lVert \bold{q}_{a}\otimes\bold{q}_{b}\rVert ^{2} =& \left( s_{a}s_{b}-\bold{v}_{a}^{T}\bold{v}_{b}\right) ^{2}+\left( s_a\bold{v}_{b}+s_b\bold{v}_a+\bold{v}_{a}\times \bold{v}_{b}\right) ^{T}\left( s_{a}\bold{v}_{b}+s_{b}\bold{v}_ a+\bold{v}_{a}\times \bold{v}_{b}\right) \\ =&s_{a}^{2}s_{b}^{2}-2s_{a}s_{b}\bold{v}_{a}^{T}\bold{v}_{b}+\left( \bold{v}_{a}^{T}\bold{v}_{b}\right) ^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\left( \bold{v}_{a}\times \bold{v}_{b}\right) ^{T}\left( \bold{v}_{a}\times \bold{v}_{b}\right) +s_{a}s_{b}\bold{v}_{b}^{T}\bold{v}_{a}+s_{a}s_{b}\bold{v}_{a}^{T}\bold{v}_{b}\\ &+s_a\underbrace{\bold{v}_{b}^{T}\left( \bold{v}_{a}\times \bold{v}_b\right) }_{0}+s_b\underbrace{\bold{v}_{a}^{T}\left( \bold{v}_a\times \bold{v}_{b}\right) }_{0}+s_a\underbrace{\left( \bold{v}_{a}\times \bold{v}_{b}\right)^{T}\bold{v}_{b}}_{0}+s_b\underbrace{\left( \bold{v}_a\times \bold{v}_{b}\right) ^{T}\bold{v}_a}_{0} \\ =& s_{a}^{2}s_{b}^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\left( \bold{v}_{a}^{T}\bold{v}_{b}\right) ^{2}+\left( \bold{v}_{a}\times \bold{v}_b\right) ^{T} \\ =& s_{a}^{2}s_{b}^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\left\| \bold{v}_{a}\right\| ^{2}\left\| \bold{v}_{b}\right\| ^{2}\cos ^{2}\theta +\left\| \bold{v}_{a}\right\| ^{2}\left\| \bold{v}_{b}\right\| ^{2}\sin ^{2}\theta \\ =& s_{a}^{2}s_{b}^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\left\| \bold{v}_{a}\right\| ^{2}\left\| \bold{v}_{b}\right\| ^{2} \end{aligned}
∥qa⊗qb∥2=====(sasb−vaTvb)2+(savb+sbva+va×vb)T(savb+sbva+va×vb)sa2sb2−2sasbvaTvb+(vaTvb)2+sa2vbTvb+sb2vaTva+(va×vb)T(va×vb)+sasbvbTva+sasbvaTvb+sa0
vbT(va×vb)+sb0
vaT(va×vb)+sa0
(va×vb)Tvb+sb0
(va×vb)Tvasa2sb2+sa2vbTvb+sb2vaTva+(vaTvb)2+(va×vb)Tsa2sb2+sa2vbTvb+sb2vaTva+∥va∥2∥vb∥2cos2θ+∥va∥2∥vb∥2sin2θsa2sb2+sa2vbTvb+sb2vaTva+∥va∥2∥vb∥2
且
∥
q
a
∥
2
∥
q
b
∥
2
=
(
s
a
2
+
v
a
T
v
a
)
(
s
b
2
+
v
b
T
v
b
)
=
s
a
2
s
b
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
v
a
T
v
a
v
b
T
v
b
=
s
a
2
s
b
2
+
s
a
2
v
b
T
v
b
+
s
b
2
v
a
T
v
a
+
∥
v
a
∥
2
∥
v
b
∥
2
\begin{aligned} \left\| \bold{q}_{a}\right\| ^{2}\left\| \bold{q}_{b}\right\| ^{2} &= \left( s_{a}^{2}+\bold{v}_{a}^{T}\bold{v}_{a}\right) \left( s_{b}^{2}+\bold{v}_{b}^{T}\bold{v}_{b}\right) \\ &=s_{a}^{2}s_{b}^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\bold{v}_{a}^{T}\bold{v}_{a}\bold{v}_{b}^{T}\bold{v}_{b}\\ &= s_{a}^{2}s_{b}^{2}+s_{a}^{2}\bold{v}_{b}^{T}\bold{v}_{b}+s_{b}^{2}\bold{v}_{a}^{T}\bold{v}_{a}+\left\| \bold{v}_{a}\right\| ^{2}\left\| \bold{v}_{b}\right\| ^{2} \end{aligned}
∥qa∥2∥qb∥2=(sa2+vaTva)(sb2+vbTvb)=sa2sb2+sa2vbTvb+sb2vaTva+vaTvavbTvb=sa2sb2+sa2vbTvb+sb2vaTva+∥va∥2∥vb∥2
故
∥
q
a
⊗
q
b
∥
=
∥
q
a
∥
∥
q
b
∥
\begin{aligned} \lVert \bold{q}_{a}\otimes\bold{q}_{b}\rVert= \left\| \bold{q}_{a}\right\| \left\| \bold{q}_{b}\right\| \end{aligned}
∥qa⊗qb∥=∥qa∥∥qb∥
逆
四元数的逆为
q
−
1
=
q
∗
∥
q
∥
2
\begin{aligned} \bold{q}^{-1}=\dfrac{\bold{q}^*}{\lVert \bold{q} \rVert^2} \end{aligned}
q−1=∥q∥2q∗
按此定义,四元数和自己的逆的乘积为实四元数1:
q
⊗
q
−
1
=
q
−
1
⊗
q
=
1
\bold{q}\otimes\bold{q}^{-1}=\bold{q}^{-1}\otimes\bold{q}=1
q⊗q−1=q−1⊗q=1
乘积的逆具有与矩阵相似的性质:
(
q
a
⊗
q
b
)
−
1
=
q
b
−
1
⊗
q
a
−
1
\begin{aligned} (\bold{q}_a\otimes\bold{q}_b)^{-1}=\bold{q}^{-1}_b\otimes\bold{q}^{-1}_a \end{aligned}
(qa⊗qb)−1=qb−1⊗qa−1
单位四元数总是可以表示成以下形式:
q
=
[
cos
θ
u
sin
θ
]
\begin{aligned} \bold{q}=\begin{bmatrix} \cos\theta\\ \bold{u}\sin\theta \end{bmatrix} \end{aligned}
q=[cosθusinθ]
其中
u
\bold{u}
u是单位向量。
四元数括号
[
p
,
q
]
≜
p
⊗
q
−
q
⊗
p
=
2
p
v
×
q
v
\begin{aligned} [\bold{p},\bold{q}]&\triangleq \bold{p}\otimes\bold{q}-\bold{q}\otimes\bold{p}\\ &=2\bold{p}_v\times\bold{q}_v \end{aligned}
[p,q]≜p⊗q−q⊗p=2pv×qv
四元数的指数映射
v
a
⊗
v
b
=
[
−
v
a
T
v
b
v
a
×
v
b
]
v
a
⊗
v
a
=
−
v
a
T
v
a
=
−
∥
v
a
∥
2
\begin{aligned} \bold{v}_{a}\otimes\bold{v}_{b} &=\begin{bmatrix} -\bold{v}_{a}^{T}\bold{v}_{b} \\ \bold{v}_a\times \bold{v}_b \end{bmatrix} \\ \bold{v}_a\otimes\bold{v}_a&=-\bold{v}_a^T\bold{v}_a=-\Vert\bold{v}_a\Vert^2 \end{aligned}
va⊗vbva⊗va=[−vaTvbva×vb]=−vaTva=−∥va∥2
对于单位纯虚四元数
u
⊗
u
=
−
1
\begin{aligned} \bold{u}\otimes\bold{u}=-1 \end{aligned}
u⊗u=−1
纯虚四元数的幂:
令
v
=
θ
u
\bold{v}=\theta\bold{u}
v=θu,
θ
=
∥
v
∥
∈
R
\theta=\lVert \bold{v} \rVert\in\mathbb{R}
θ=∥v∥∈R,
u
\bold{u}
u是单位向量。
v
2
=
−
θ
2
,
v
3
=
−
u
θ
3
,
v
4
=
θ
4
,
⋯
\begin{aligned} \bold{v}^2=-\theta^2,\quad \bold{v}^3=-\bold{u}\theta^3,\quad \bold{v}^4=\theta^4,\quad \cdots \end{aligned}
v2=−θ2,v3=−uθ3,v4=θ4,⋯
四元数的指数映射
exp
(
q
)
=
∑
k
=
0
∞
1
k
!
q
k
\begin{aligned} \exp(\bold{q})=\sum_{k=0}^{\infty}\dfrac{1}{k!}\bold{q}^k \end{aligned}
exp(q)=k=0∑∞k!1qk
纯虚四元数
v
=
v
x
i
+
v
y
j
+
v
z
k
\bold{v}=v_x{\rm i}+v_y{\rm j}+v_z{\rm k}
v=vxi+vyj+vzk
exp
(
v
)
=
∑
k
=
0
∞
1
k
!
v
k
\begin{aligned} \exp{(\bold{v})}=\sum_{k=0}^{\infty}\dfrac{1}{k!}\bold{v}^k \end{aligned}
exp(v)=k=0∑∞k!1vk
令
v
=
θ
u
\bold{v}=\theta\bold{u}
v=θu,
θ
=
∥
v
∥
∈
R
\theta=\lVert \bold{v} \rVert\in\mathbb{R}
θ=∥v∥∈R,
u
\bold{u}
u是单位向量。
exp
(
θ
u
)
=
(
1
−
θ
2
2
!
+
θ
4
4
!
+
⋯
)
+
(
u
θ
−
u
θ
3
3
!
+
u
θ
5
5
!
+
⋯
)
=
[
cos
θ
u
sin
θ
]
∥
exp
(
u
θ
)
∥
=
cos
θ
2
+
sin
θ
2
=
1
\begin{aligned} \exp(\theta\bold{u})=\left(1-\dfrac{\theta^2}{2!}+\dfrac{\theta^4}{4!}+\cdots\right)&+\left(\bold{u}\theta-\dfrac{\bold{u}\theta^3}{3!}+\dfrac{\bold{u}\theta^5}{5!}+\cdots\right)=\begin{bmatrix}\cos\theta\\\bold{u}\sin\theta\end{bmatrix}\\ \Vert\exp(\bold{u}\theta)\Vert&=\cos\theta^2+\sin\theta^2=1 \end{aligned}
exp(θu)=(1−2!θ2+4!θ4+⋯)∥exp(uθ)∥+(uθ−3!uθ3+5!uθ5+⋯)=[cosθusinθ]=cosθ2+sinθ2=1
这是欧拉公式的拓展:
exp
(
i
θ
)
=
cos
θ
+
i
sin
θ
,
∥
exp
(
i
θ
)
∥
=
1
\exp(i\theta)=\cos\theta+i\sin\theta,\quad\Vert\exp(i\theta)\Vert=1
exp(iθ)=cosθ+isinθ,∥exp(iθ)∥=1
四元数指数函数的共轭:
exp
(
−
v
)
=
(
exp
(
v
)
)
∗
\exp(-\bold{v})=\left(\exp(\bold{v})\right)^*
exp(−v)=(exp(v))∗
普通四元数的指数映射:
exp
(
q
)
=
exp
(
s
+
v
)
=
exp
(
s
)
exp
(
v
)
=
exp
(
s
)
[
cos
∥
v
∥
v
∥
v
∥
sin
∥
v
∥
]
\exp(\bold{q})=\exp(s+\bold{v})=\exp(s)\exp(\bold{v})=\exp(s)\begin{bmatrix}\cos\Vert\bold{v}\Vert\\ \dfrac{\bold{v}}{\Vert\bold{v}\Vert}\sin\Vert\bold{v}\Vert\end{bmatrix}
exp(q)=exp(s+v)=exp(s)exp(v)=exp(s)[cos∥v∥∥v∥vsin∥v∥]
上式成立的原因是标量乘法可交换
s
v
=
v
s
s\bold{v}=\bold{v}s
sv=vs
四元数的对数映射
单位四元数的对数映射
log
q
=
log
(
cos
θ
+
u
sin
θ
)
=
log
(
exp
(
θ
u
)
)
=
θ
u
=
[
0
θ
u
]
\begin{aligned} \log\bold{q}=\log(\cos\theta+\bold{u}\sin\theta)=\log\left(\exp(\theta\bold{u})\right)=\theta\bold{u}=\begin{bmatrix}0\\\theta\bold{u}\end{bmatrix} \end{aligned}
logq=log(cosθ+usinθ)=log(exp(θu))=θu=[0θu]
其中
u
=
v
∥
v
∥
θ
=
arctan
(
∥
v
∥
,
s
)
\begin{aligned} \bold{u}&=\dfrac{\bold{v}}{\Vert\bold{v}\Vert} \\ \theta&=\arctan(\Vert\bold{v}\Vert,s) \end{aligned}
uθ=∥v∥v=arctan(∥v∥,s)
普通四元数的对数映射
log
q
=
log
(
∥
q
∥
q
∥
q
∥
)
=
log
(
∥
q
∥
)
+
log
(
q
∥
q
∥
)
=
log
(
∥
q
∥
)
+
θ
u
=
[
log
(
∥
q
∥
)
θ
u
]
\begin{aligned} \log\bold{q}=\log\left(\Vert\bold{q}\Vert\dfrac{\bold{q}}{\Vert\bold{q}\Vert}\right)=\log\left(\Vert\bold{q}\Vert\right)+\log\left(\dfrac{\bold{q}}{\Vert\bold{q}\Vert}\right)=\log\left(\Vert\bold{q}\Vert\right)+\theta\bold{u}=\begin{bmatrix}\log\left(\Vert\bold{q}\Vert\right)\\\theta\bold{u}\end{bmatrix} \end{aligned}
logq=log(∥q∥∥q∥q)=log(∥q∥)+log(∥q∥q)=log(∥q∥)+θu=[log(∥q∥)θu]
Exponential forms of the type of
q
t
\bold{q}^t
qt:
q
t
=
exp
(
log
(
q
t
)
)
=
exp
(
t
log
(
q
)
)
=
exp
(
t
⋅
θ
u
)
=
[
cos
t
θ
u
sin
t
θ
]
\bold{q}^t=\exp\left(\log\left(\bold{q}^t\right)\right)=\exp\left(t\log\left(\bold{q}\right)\right)=\exp\left(t\cdot\theta \bold{u}\right)=\begin{bmatrix}\cos t\theta\\\bold{u}\sin t\theta\end{bmatrix}
qt=exp(log(qt))=exp(tlog(q))=exp(t⋅θu)=[costθusintθ]
用四元数表示旋转
假设有一个空间三维点
p
=
[
x
,
y
,
z
]
∈
R
3
\bold{p}=[x,y,z]\in\mathbb{R}^3
p=[x,y,z]∈R3,以及一个由单位四元数
q
\bold{q}
q指定的旋转。
首先,把三维空间点用一个虚四元数来描述:
p
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
\bold{p}=[0,x,y,z]^T=[0,\bold{v}]^T
p=[0,x,y,z]T=[0,v]T
相当于把四元数的三个虚部与空间中的三个轴相对应。那么,旋转后的点
p
′
\bold{p}{'}
p′可以表示为这样的乘积:
r
(
p
)
=
p
′
=
q
⊗
p
⊗
q
−
1
\begin{aligned} r(\bold{p})=\bold{p}'=\bold{q}\otimes\bold{p}\otimes\bold{q}^{-1} \end{aligned}
r(p)=p′=q⊗p⊗q−1
可以验证结果的实部为0。最后把
p
′
\bold{p}{'}
p′的虚部取出即得旋转之后点的坐标。
由四元数括号可知:
r
(
v
)
×
r
(
w
)
=
(
q
⊗
v
⊗
q
∗
)
×
(
q
⊗
w
⊗
q
∗
)
=
1
2
(
(
q
⊗
v
⊗
q
∗
)
⊗
(
q
⊗
w
⊗
q
∗
)
−
(
q
⊗
w
⊗
q
∗
)
⊗
(
q
⊗
v
⊗
q
∗
)
)
=
1
2
(
q
⊗
v
⊗
w
⊗
q
∗
−
q
⊗
w
⊗
v
⊗
q
∗
)
=
1
2
(
q
⊗
(
v
⊗
w
−
w
⊗
v
)
⊗
q
∗
)
=
q
⊗
(
v
×
w
)
⊗
q
∗
=
r
(
v
×
w
)
\begin{aligned}r\left( \bold{v}\right) \times r\left( \bold{w}\right) &=\left( \bold{q}\otimes \bold{v}\otimes \bold{q}^{\ast }\right) \times \left( \bold{q}\otimes \bold{w}\otimes \bold{q}^{\ast }\right) \\ &=\dfrac{1}{2}\left( \left( \bold{q}\otimes \bold{v}\otimes \bold{q}^{\ast }\right) \otimes \left( \bold{q}\otimes \bold{w}\otimes \bold{q}^{\ast }\right) -\left( \bold{q}\otimes \bold{w}\otimes \bold{q}^{\ast }\right) \otimes \left( \bold{q}\otimes \bold{v}\otimes \bold{q}^{\ast }\right) \right) \\ &=\dfrac{1}{2}\left( \bold{q}\otimes \bold{v}\otimes \bold{w}\otimes \bold{q}^{\ast }-\bold{q}\otimes \bold{w}\otimes \bold{v}\otimes \bold{q}^{\ast }\right) \\ &=\dfrac{1}{2}\left( \bold{q}\otimes \left( \bold{v}\otimes \bold{w}-\bold{w}\otimes \bold{v}\right) \otimes \bold{q}^{\ast }\right) \\ &=\bold{q}\otimes \left( \bold{v}\times \bold{w}\right) \otimes \bold{q}^{\ast }\\ &=r\left( \bold{v}\times \bold{w}\right) \end{aligned}
r(v)×r(w)=(q⊗v⊗q∗)×(q⊗w⊗q∗)=21((q⊗v⊗q∗)⊗(q⊗w⊗q∗)−(q⊗w⊗q∗)⊗(q⊗v⊗q∗))=21(q⊗v⊗w⊗q∗−q⊗w⊗v⊗q∗)=21(q⊗(v⊗w−w⊗v)⊗q∗)=q⊗(v×w)⊗q∗=r(v×w)
四元数微分方程
一个单位四元数
q
∈
S
3
\bold{q}\in S^3
q∈S3,满足
q
∗
⊗
q
=
1
\bold{q}^{\ast}\otimes\bold{q}=1
q∗⊗q=1,像对旋转矩阵的处理一样,
R
T
R
=
I
\bold{R}^T\bold{R}=\bold{I}
RTR=I
d
(
q
∗
⊗
q
)
d
t
=
q
˙
∗
⊗
q
+
q
∗
⊗
q
˙
=
0
\dfrac{\rm d\left( \bold{q}^{\ast }\otimes \bold{q}\right) }{{\rm d}t}=\dot{\bold{q}}^{\ast }\otimes \bold{q}+\bold{q}^{\ast }\otimes \dot{\bold{q}}=0
dtd(q∗⊗q)=q˙∗⊗q+q∗⊗q˙=0
可以推得
q
∗
⊗
q
˙
=
−
(
q
˙
∗
⊗
q
)
=
−
(
q
∗
⊗
q
˙
)
∗
\bold{q}^{\ast }\otimes \dot{\bold{q}}=-\left( \dot{\bold{q}}^{\ast }\otimes \bold{q}\right) =-\left( \bold{q}^{\ast }\otimes \dot{\bold{q}}\right) ^{\ast }
q∗⊗q˙=−(q˙∗⊗q)=−(q∗⊗q˙)∗
这意味着
q
∗
⊗
q
˙
\bold{q}^{\ast }\otimes \dot{\bold{q}}
q∗⊗q˙是一个纯虚四元数。设一个纯虚四元数
Ω
∈
H
p
\bold{\Omega}\in \mathbb{H}_p
Ω∈Hp满足
q
∗
⊗
q
˙
=
Ω
=
[
0
Ω
]
\bold{q}^{\ast }\otimes \dot{\bold{q}}=\bold{\Omega}=\begin{bmatrix} 0 \\ \bold{\Omega} \end{bmatrix}
q∗⊗q˙=Ω=[0Ω]
等式两侧左乘一个
q
\bold{q}
q可得
q
˙
=
q
⊗
Ω
\dot{\bold{q}}=\bold{q}\otimes \bold{\Omega}
q˙=q⊗Ω
在原点附近,我们有
q
=
1
\bold{q}=1
q=1,上面的方程退化为
q
˙
=
Ω
∈
H
p
\dot{\bold{q}}=\bold{\Omega}\in \mathbb{H}_p
q˙=Ω∈Hp 因此,纯虚四元数空间
H
p
\mathbb{H}_p
Hp构成了四元数单位球
S
3
S^3
S3的切向量或李代数。然而,在四元数的情况下,这个空间不是直接的速度空间,而是半-速度空间。
如果
Ω
\bold{\Omega}
Ω为常数,微分方程可积分为
q
=
q
(
0
)
⊗
e
Ω
t
\bold{q}=\bold{q}(0)\otimes e^{\bold{\Omega}t}
q=q(0)⊗eΩt
其中
q
(
0
)
\bold{q}(0)
q(0)和
q
(
t
)
\bold{q}(t)
q(t)都是单位四元数,所以指数
e
Ω
t
e^{\bold{\Omega}t}
eΩt也是单位四元数,这在之前四元数的指数映射一节也已经推导过了。
定义
V
=
Ω
t
\bold{V}=\bold{\Omega}t
V=Ωt,我们有
q
=
e
V
\bold{q}=e^{\bold{V}}
q=eV
这又是一个指数映射:从纯四元数空间到由单位四元数表示的旋转空间:
exp
:
H
p
→
S
3
;
V
↦
exp
(
V
)
=
e
V
\exp: \mathbb{H}_{p}\to S^3;\bold{V}\mapsto\exp(\bold{V})=e^{\bold{V}}
exp:Hp→S3;V↦exp(V)=eV
四元数与旋转矩阵的转换
考虑使用单位四元数对空间点进行旋转的问题,有
p
′
=
q
⊗
p
⊗
q
∗
=
[
q
]
L
(
p
⊗
q
∗
)
=
[
q
]
L
[
q
∗
]
R
p
=
(
q
⊗
p
)
⊗
q
∗
=
[
q
]
L
p
⊗
q
∗
=
[
q
∗
]
R
[
q
]
L
p
\begin{aligned} \bold{p}'&=\bold{q}\otimes\bold{p}\otimes\bold{q}^{\ast}=\left[\bold{q}\right]_{L}\left( \bold{p}\otimes\bold{q}^{\ast}\right) =\left[\bold{q}\right]_{L}\left[ \bold{q}^{\ast}\right] _{R} \bold{p}\\ &=\left(\bold{q}\otimes \bold{p}\right)\otimes\bold{q}^{\ast}=\left[\bold{q}\right]_{L}\bold{p}\otimes\bold{q}^{\ast}=\left[ \bold{q}^{\ast}\right] _{R}\left[ \bold{q}\right] _{L}\bold{p} \end{aligned}
p′=q⊗p⊗q∗=[q]L(p⊗q∗)=[q]L[q∗]Rp=(q⊗p)⊗q∗=[q]Lp⊗q∗=[q∗]R[q]Lp
上式我们只利用了四元数乘法的结合律,可以得到对任意四元数都成立的如下关系式
[
q
1
]
R
[
q
2
]
L
=
[
q
2
]
L
[
q
1
]
R
\left[ \bold{q}_1\right] _{R}\left[ \bold{q}_2\right] _{L}=\left[ \bold{q}_2\right] _{L}\left[ \bold{q}_1\right] _{R}
[q1]R[q2]L=[q2]L[q1]R
代入两个符号对应的矩阵,得四元数乘法矩阵形式:
[
q
]
L
[
q
∗
]
R
=
[
s
−
v
T
v
s
I
+
v
∧
]
[
s
v
T
−
v
s
I
+
v
∧
]
=
[
s
2
+
v
T
v
s
v
T
−
s
v
T
−
v
T
v
∧
s
v
−
s
v
−
v
∧
v
v
v
T
+
(
s
I
+
v
∧
)
2
]
=
[
1
0
0
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
]
\begin{aligned} [\bold{q}]_{L}\left[ \bold{q}^{\ast}\right]_{R} &=\begin{bmatrix} s & -\bold{v}^{T} \\ \bold{v} & s\bold{I}+\bold{v}^{\wedge } \end{bmatrix}\begin{bmatrix} s & \bold{v}^{T} \\ -\bold{v} & s\bold{I}+\bold{v}^{\wedge } \end{bmatrix} \\ &=\begin{bmatrix} s^{2}+\bold{v}^{T}\bold{v} & s\bold{v}^{T}-s\bold{v}^{T}-\bold{v}^{T}\bold{v}^{\wedge} \\ s\bold{v}-s\bold{v}-\bold{v}^{\wedge }\bold{v} & \bold{v}\bold{v}^{T}+\left( s\bold{I}+\bold{v}^{\wedge}\right) ^{2} \end{bmatrix}\\ &=\begin{bmatrix} 1 & 0 \\ 0 & \bold{v}\bold{v}^{T}+s^{2}\bold{I}+2s\bold{v}^{\wedge }+\left( \bold{v}^{\wedge }\right) ^{2} \end{bmatrix} \end{aligned}
[q]L[q∗]R=[sv−vTsI+v∧][s−vvTsI+v∧]=[s2+vTvsv−sv−v∧vsvT−svT−vTv∧vvT+(sI+v∧)2]=[100vvT+s2I+2sv∧+(v∧)2]
因为
p
′
\bold{p}'
p′和
p
\bold{p}
p都是虚四元数,所以事实上该矩阵的右下角给出了从四元数到旋转矩阵的变换关系:
R
=
v
v
T
+
s
2
I
+
(
v
∧
)
2
+
2
s
v
∧
\begin{aligned} \bold{R}=\bold{v}\bold{v}^{T}+s^{2}\bold{I}+\left( \bold{v}^{\wedge}\right) ^{2}+2s\bold{v}^{\wedge } \end{aligned}
R=vvT+s2I+(v∧)2+2sv∧
观察到前三项均为对称矩阵,最后一项为反对称矩阵,利用这个性质可以得到:
R
−
R
T
=
4
s
v
∧
\begin{aligned} \bold{R}-\bold{R}^{T}=4s\bold{v}^{\wedge} \end{aligned}
R−RT=4sv∧
由下式求出
s
s
s进而求出
v
\bold{v}
v
t
r
(
R
)
=
t
r
(
v
v
T
)
+
t
r
(
s
2
I
)
+
t
r
(
(
v
∧
)
2
)
+
t
r
(
2
s
v
∧
)
=
t
r
(
v
T
v
)
+
3
s
2
−
2
(
v
1
2
+
v
2
2
+
v
3
2
)
+
0
=
v
1
2
+
v
2
2
+
v
3
2
+
3
s
2
−
2
(
v
1
2
+
v
2
2
+
v
3
2
)
=
3
s
2
−
(
v
1
2
+
v
2
2
+
v
3
2
)
=
3
s
2
−
(
1
−
s
2
)
=
4
s
2
−
1
\begin{aligned} {\rm tr}\left( \bold{R}\right) &={\rm tr}\left( \bold{v}\bold{v}^{T}\right) +{\rm tr}\left( s^{2}\bold{I}\right) +{\rm tr}\left( \left( \bold{v}^{\wedge}\right) ^{2}\right) +{\rm tr}\left( 2s\bold{v}^{\wedge}\right) \\ &={\rm tr}\left( \bold{v}^{T}\bold{v}\right) +3s^{2}-2\left( v_{1}^{2}+v_{2}^{2}+v_{3}^{2}\right) +0\\ &=v_{1}^{2}+v_{2}^{2}+v_{3}^{2}+3s^{2}-2\left( v_{1}^{2}+v_{2}^{2}+v_{3}^{2}\right) \\ &=3s^{2}-\left( v_{1}^{2}+v_{2}^{2}+v_3^{2}\right) \\ &=3s^{2}-\left( 1-s^{2}\right) \\ &=4s^{2}-1 \end{aligned}
tr(R)=tr(vvT)+tr(s2I)+tr((v∧)2)+tr(2sv∧)=tr(vTv)+3s2−2(v12+v22+v32)+0=v12+v22+v32+3s2−2(v12+v22+v32)=3s2−(v12+v22+v32)=3s2−(1−s2)=4s2−1
可以得出四元数转旋转矩阵有这样的性质:
R
{
[
1
,
0
,
0
,
0
]
T
}
=
I
R
{
−
q
}
=
R
{
q
}
R
{
q
∗
}
=
R
{
q
}
T
R
{
q
1
⊗
q
2
}
=
R
{
q
1
}
⋅
R
{
q
2
}
\begin{aligned}\bold{R}\left\{ \left[ 1,0,0,0\right] ^{T}\right\} &=\bold{I}\\ \bold{R}\left\{ -\bold{q}\right\} &=\bold{R}\left\{ \bold{q}\right\} \\ \bold{R}\left\{ \bold{q}^{\ast }\right\} &=\bold{R}\left\{ \bold{q}\right\} ^{T}\\ \bold{R} \left\{ \bold{q}_1\otimes\bold{q}_{2}\right\} &=\bold{R} \left\{ \bold{q}_{1}\right\} \cdot \bold{R} \left\{ \bold{q}_{2}\right\} \end{aligned}
R{[1,0,0,0]T}R{−q}R{q∗}R{q1⊗q2}=I=R{q}=R{q}T=R{q1}⋅R{q2}
四元数与旋转向量的转换
由旋转向量的旋转角与旋转矩阵的转换关系可得:
θ
=
arccos
(
t
r
(
R
)
−
1
2
)
=
arccos
(
2
s
2
−
1
)
cos
θ
=
2
s
2
−
1
=
2
cos
2
θ
2
−
1
⟹
θ
=
2
arccos
s
\begin{aligned} &\theta =\arccos \left( \dfrac{{\rm tr}\left( \bold{R}\right) -1}{2}\right) =\arccos \left( 2s^{2}-1\right) \\ &\cos \theta =2s^{2}-1=2\cos ^{2}\dfrac{\theta }{2}-1\\ &\implies \theta =2\arccos s \end{aligned}
θ=arccos(2tr(R)−1)=arccos(2s2−1)cosθ=2s2−1=2cos22θ−1⟹θ=2arccoss
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 ⟹ t r ( R ) = t r ( v T v ) + 3 s 2 + 0 − 2 v T v = 4 s 2 − 1 R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ ⟹ t r ( R ) = 3 cos θ + 1 − cos θ + 0 = 2 cos θ + 1 \begin{aligned} \bold{R}=\bold{v}\bold{v}^{T}+s^{2}I+2s\bold{v}^{\wedge }+\left( \bold{v}^{\wedge}\right) ^{2}&\implies {\rm tr}\left( \bold{R}\right) ={\rm tr}\left( \bold{v}^{T}\bold{v}\right) +3s^{2}+0-2\bold{v}^{T}\bold{v}=4s^{2}-1 \\ \bold{R}=\cos \theta I+\left( 1-\cos \theta \right) nn^{T}+\sin \theta n^{\wedge } &\implies {\rm tr}\left( \bold{R}\right) =3\cos \theta +1-\cos \theta +0=2\cos \theta +1 \end{aligned} R=vvT+s2I+2sv∧+(v∧)2R=cosθI+(1−cosθ)nnT+sinθn∧⟹tr(R)=tr(vTv)+3s2+0−2vTv=4s2−1⟹tr(R)=3cosθ+1−cosθ+0=2cosθ+1
cos
θ
=
2
s
2
−
1
=
2
cos
2
θ
2
−
1
⇒
s
2
=
cos
2
θ
2
\cos \theta =2s^{2}-1=2\cos ^{2}\dfrac{\theta }{2}-1\Rightarrow s^{2}=\cos ^{2}\dfrac{\theta }{2}
cosθ=2s2−1=2cos22θ−1⇒s2=cos22θ
至于旋转轴,将四元数乘法矩阵形式中的
p
\bold{p}
p用
q
\bold{q}
q的虚部代替,易知
q
\bold{q}
q的虚部组成的向量在旋转时是不变的,即构成旋转轴。
n
=
v
k
(
k
2
=
∣
v
∣
2
=
1
−
s
2
=
sin
2
θ
2
)
\begin{aligned} \bold{n}=\dfrac{\bold{v}}{k}\quad(k^2=|\bold{v}|^2=1-s^2=\sin^2\frac{\theta}{2}) \end{aligned}
n=kv(k2=∣v∣2=1−s2=sin22θ)
将上面两式代入旋转向量与旋转矩阵的转换式,得
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
=
(
2
s
2
−
1
)
I
+
2
(
1
−
s
2
)
v
v
T
1
−
s
2
+
sin
θ
n
∧
=
(
2
s
2
−
1
)
I
+
2
v
v
T
+
sin
θ
n
∧
=
v
v
T
+
s
2
I
+
[
v
v
T
−
(
1
−
s
2
)
I
]
+
sin
θ
n
∧
=
v
v
T
+
s
2
I
+
(
v
∧
)
2
+
sin
θ
n
∧
⟹
2
s
v
∧
=
sin
θ
n
∧
=
2
cos
θ
2
sin
θ
2
n
∧
⟹
s
cos
θ
2
v
∧
=
sin
θ
2
k
v
∧
\begin{aligned} \bold{R} =& \cos \theta \bold{I}+\left( 1-\cos \theta \right) \bold{n}\bold{n}^{T}+\sin \theta \bold{n}^{\wedge } \\ =& \left( 2s^{2}-1\right) \bold{I}+2\left( 1-s^{2}\right) \dfrac{\bold{v}\bold{v}^{T}}{1-s^{2}}+\sin \theta \bold{n}^{\wedge }\\ =& \left( 2s^{2}-1\right) \bold{I}+2\bold{v}\bold{v}^{T}+\sin \theta \bold{n}^{\wedge }\\ =& \bold{v}\bold{v}^{T}+s^{2}\bold{I}+\left[ \bold{v}\bold{v}^{T}-\left( 1-s^{2}\right) \bold{I}\right] +\sin \theta \bold{n}^{\wedge }\\ =& \bold{v}\bold{v}^{T}+s^{2}\bold{I}+\left( \bold{v}^{\wedge}\right) ^{2}+\sin \theta \bold{n}^{\wedge } \\ \implies & 2s\bold{v}^{\wedge }=\sin \theta \bold{n}^{\wedge }=2\cos \tfrac{\theta }{2}\sin \tfrac{\theta }{2}\bold{n}^{\wedge }\\ \implies& \dfrac{s}{\cos \frac{\theta }{2}}\bold{v}^{\wedge }=\dfrac{\sin \frac{\theta }{2}}{k}\bold{v}^{\wedge} \end{aligned}
R=====⟹⟹cosθI+(1−cosθ)nnT+sinθn∧(2s2−1)I+2(1−s2)1−s2vvT+sinθn∧(2s2−1)I+2vvT+sinθn∧vvT+s2I+[vvT−(1−s2)I]+sinθn∧vvT+s2I+(v∧)2+sinθn∧2sv∧=sinθn∧=2cos2θsin2θn∧cos2θsv∧=ksin2θv∧有两种对应关系:
s
=
cos
θ
2
k
=
sin
θ
2
o
r
s
=
−
cos
θ
2
k
=
−
sin
θ
2
\begin{aligned} s&=\cos\frac{\theta}{2}\\ k&=\sin\frac{\theta}{2} \end{aligned} \quad \rm or \quad \begin{aligned} s&=-\cos\frac{\theta}{2}\\ k&=-\sin\frac{\theta}{2} \end{aligned}
sk=cos2θ=sin2θorsk=−cos2θ=−sin2θ
通常取
s
=
cos
θ
2
k
=
sin
θ
2
\begin{aligned} s &=\cos\frac{\theta}{2}\\ k &=\sin\frac{\theta}{2} \end{aligned}
sk=cos2θ=sin2θ
则旋转向量转四元数:
s
=
cos
θ
2
v
=
n
sin
θ
2
\begin{aligned} s &=\cos\tfrac{\theta}{2}\\ \bold{v} &=\bold{n}\sin\tfrac{\theta}{2} \end{aligned}
sv=cos2θ=nsin2θ
四元数转旋转向量:
θ
=
2
arccos
s
n
=
v
sin
θ
2
\begin{aligned} \theta &=2\arccos s\\ \bold{n} &=\dfrac{\bold{v}}{\sin\tfrac{\theta}{2}} \end{aligned}
θn=2arccoss=sin2θv
大写指数映射
对于半-速度空间的一个通俗的解释是:旋转动作是由双积
x
′
=
q
⊗
x
⊗
q
∗
\bold{x}'=\bold{q}\otimes\bold{x}\otimes \bold{q}^{\ast}
x′=q⊗x⊗q∗完成的,向量
x
\bold{x}
x经历的旋转是由
q
\bold{q}
q中编码的旋转的两倍,或者等效的,四元数
q
\bold{q}
q编码了预期旋转的“一半”。
为了更好表达轴角和四元数的关系,
v
=
ϕ
u
∈
R
3
\bold{v}=\phi\bold{u}\in\mathbb{R}^3
v=ϕu∈R3,我们定义一个大写版本的指数映射:
E
x
p
:
R
3
→
S
3
;
v
↦
E
x
p
(
v
)
=
e
v
/
2
{\rm Exp}:\mathbb{R}^3\to S^3;\bold{v}\mapsto{\rm Exp(\bold{v})}=e^{\bold{v}/2}
Exp:R3→S3;v↦Exp(v)=ev/2
它和指数映射的关系:
E
x
p
≜
exp
(
v
/
2
)
{\rm Exp}\triangleq\exp(\bold{v}/2)
Exp≜exp(v/2)
引入角速度向量
ω
=
2
Ω
∈
R
3
\boldsymbol{\omega}=2\bold{\Omega}\in \mathbb{R}^3
ω=2Ω∈R3
q
˙
=
1
2
q
⊗
ω
q
=
e
ω
t
/
2
\dot{\bold{q}}=\dfrac{1}{2}\bold{q}\otimes\boldsymbol{\omega}\\ \bold{q}=e^{\boldsymbol{\omega}t/2}
q˙=21q⊗ωq=eωt/2
令
v
=
ϕ
u
\bold{v}=\phi \bold{u}
v=ϕu代表一个绕轴
u
\bold{u}
u旋转角度
ϕ
\phi
ϕ的旋转向量
q
≜
E
x
p
(
ϕ
u
)
=
e
ϕ
u
/
2
=
cos
ϕ
2
+
u
sin
ϕ
2
=
[
cos
ϕ
2
u
sin
ϕ
2
]
\bold{q}\triangleq {\rm Exp}(\phi \bold{u})=e^{\phi \bold{u}/2}=\cos\dfrac{\phi}{2}+\bold{u}\sin\frac{\phi}{2}= \begin{bmatrix}\cos\dfrac{\phi}{2} \\ \bold{u}\sin\dfrac{\phi}{2}\end{bmatrix}
q≜Exp(ϕu)=eϕu/2=cos2ϕ+usin2ϕ=
cos2ϕusin2ϕ
旋转向量到四元数的转换将被记作
q
=
q
{
v
}
≜
E
x
p
(
v
)
\bold{q}=\bold{q}\{\bold{v}\}\triangleq {\rm Exp}(\bold{v})
q=q{v}≜Exp(v)
大写对数映射
我们定义对数映射是指数映射的逆
L
o
g
:
S
3
→
R
3
;
q
↦
L
o
g
(
q
)
=
u
ϕ
\rm Log:S^3\to \mathbb{R}^3;\bold{q}\mapsto{\rm Log}(\bold{q})=\bold{u}\phi
Log:S3→R3;q↦Log(q)=uϕ
其中
ϕ
=
2
arctan
(
∥
q
v
∥
,
q
w
)
u
=
q
v
∥
q
v
∥
\begin{aligned} \phi=2\arctan(\Vert\bold{q}_v\Vert,q_w)\\ \bold{u}=\dfrac{\bold{q}_v}{\Vert\bold{q}_v\Vert} \end{aligned}
ϕ=2arctan(∥qv∥,qw)u=∥qv∥qv
大写对数映射和对数映射的关系:
L
o
g
(
q
)
≜
2
log
(
q
)
\rm Log(\bold{q})\triangleq 2\log(\bold{q})
Log(q)≜2log(q)
Spherical linear interpolation(SLEARP)
四元数可以非常方便的计算适当的方向插值。
给定由四元数
q
1
\bold{q}_1
q1和
q
1
\bold{q}_1
q1表示的两个方向,我们想找到一个四元数函数
q
(
t
)
,
t
∈
[
0
,
1
]
\bold{q}(t), t\in[0,1]
q(t),t∈[0,1],它从
q
(
0
)
=
q
0
\bold{q}(0) = \bold{q}_0
q(0)=q0到
q
(
1
)
=
q
1
\bold{q}(1) = \bold{q}_1
q(1)=q1进行线性插值。这种插值是这样的,当
t
t
t从0进化到1时,物体将沿着固定的轴以恒定的速度从方向
q
0
\bold{q}_0
q0不断旋转到方向
q
1
\bold{q}_1
q1。
方法1
第一种方法是使用四元数代数,首先计算从
q
0
\bold{q}_0
q0到
q
1
\bold{q}_1
q1的方向增量
Δ
q
\Delta\bold{q}
Δq满足
q
1
=
q
0
⊗
Δ
q
\bold{q}_1=\bold{q}_0\otimes \Delta \bold{q}
q1=q0⊗Δq
Δ
q
=
q
0
∗
⊗
q
1
\Delta \bold{q}=\bold{q}_0^{\ast}\otimes\bold{q}_1
Δq=q0∗⊗q1
接着使用对数映射获得对应的旋转向量
Δ
ϕ
=
u
Δ
ϕ
\Delta \boldsymbol{\phi}=\bold{u}\Delta\phi
Δϕ=uΔϕ
u
Δ
ϕ
=
L
o
g
(
Δ
q
)
\bold{u}\Delta\phi={\rm Log}(\Delta \bold{q})
uΔϕ=Log(Δq)
保持旋转轴
u
\bold{u}
u不变,对旋转角度进行插值
δ
ϕ
=
t
Δ
ϕ
\delta \phi = t\Delta \phi
δϕ=tΔϕ。利用指数映射将其映射回四元数
δ
q
=
E
x
p
(
u
δ
ϕ
)
\delta \bold{q}={\rm Exp}(\bold{u}\delta \phi)
δq=Exp(uδϕ),接着与原四元数组合得到最终的插值结果:
q
(
t
)
=
q
0
⊗
δ
q
=
q
0
⊗
E
x
p
(
t
u
Δ
ϕ
)
=
q
0
⊗
E
x
p
(
t
L
o
g
(
q
0
∗
⊗
q
1
)
=
q
0
⊗
(
q
0
∗
⊗
q
1
)
t
\begin{aligned} \bold{q}(t)&=\bold{q}_0\otimes\delta \bold{q}\\ &=\bold{q}_0\otimes{\rm Exp}(t\bold{u}\Delta \phi) \\&=\bold{q}_0\otimes{\rm Exp}(t{\rm Log}(\bold{q}_0^{\ast}\otimes\bold{q}_1) \\&=\bold{q}_0\otimes(\bold{q}_0^{\ast}\otimes\bold{q}_1)^t \end{aligned}
q(t)=q0⊗δq=q0⊗Exp(tuΔϕ)=q0⊗Exp(tLog(q0∗⊗q1)=q0⊗(q0∗⊗q1)t
上式经常被实现为
q
(
t
)
=
q
0
⊗
[
cos
(
t
Δ
ϕ
/
2
)
u
sin
(
t
Δ
ϕ
/
2
)
]
\bold{q}(t)=\bold{q}_0\otimes \begin{bmatrix} \cos(t\Delta \phi/2)\\ \bold{u}\sin(t\Delta\phi/2) \end{bmatrix}
q(t)=q0⊗[cos(tΔϕ/2)usin(tΔϕ/2)]
方法2
可以开发出与四元数代数内部无关的Slerp方法,甚至与嵌入圆弧的空间维数无关。
我们将四元数
q
0
\bold{q}_0
q0和
q
1
\bold{q}_1
q1作为单位球中的两个单位向量,在同一空间中进行插值,插值后的
q
(
t
)
\bold{q}(t)
q(t)是单位向量,它以恒定的角速度跟随连接
q
0
\bold{q}_0
q0和
q
1
\bold{q}_1
q1的最短球面路径。这条路径是有
q
0
,
q
1
\bold{q}_0 ,\bold{q}_1
q0,q1和原点定义的平面与单位球相交形成的平面弧。
第一种方法使用向量代数并遵循上面的思想。考虑
q
0
\bold{q}_0
q0和
q
1
\bold{q}_1
q1是两个单位向量;它们之间的夹角是由标量积得到的:
cos
(
Δ
θ
)
=
q
0
T
q
1
⟺
Δ
θ
=
arccos
(
q
0
T
q
1
)
\cos(\Delta \theta)=\bold{q}^T_0\bold{q}_1\iff\Delta\theta=\arccos(\bold{q}^T_0\bold{q}_1)
cos(Δθ)=q0Tq1⟺Δθ=arccos(q0Tq1)
我们命名旋转平面为
π
\pi
π,并构建该平面的一组单位正交基
{
q
0
,
q
⊥
}
\{\bold{q}_0,\bold{q}_{\perp}\}
{q0,q⊥},其中
q
⊥
\bold{q}_{\perp}
q⊥来自于
q
1
\bold{q}_1
q1对
q
0
\bold{q}_0
q0的单位正交化
q
⊥
=
q
1
−
(
q
0
T
q
1
)
q
0
∥
q
1
−
(
q
0
T
q
1
)
q
0
∥
\bold{q}_{\perp}=\dfrac{\bold{q}_1-(\bold{q}_0^T\bold{q}_1)\bold{q}_0}{\Vert\bold{q}_1-(\bold{q}_0^T\bold{q}_1)\bold{q}_0\Vert}
q⊥=∥q1−(q0Tq1)q0∥q1−(q0Tq1)q0
可以得到
q
1
=
q
0
cos
Δ
θ
+
q
⊥
sin
Δ
θ
(1)
\bold{q}_1=\bold{q}_0\cos\Delta\theta+\bold{q}_{\perp}\sin\Delta\theta\tag{1}
q1=q0cosΔθ+q⊥sinΔθ(1)
q
0
\bold{q}_0
q0在平面上旋转角度
t
Δ
θ
t\Delta \theta
tΔθ 就可以得到
q
(
t
)
=
q
0
cos
(
t
Δ
θ
)
+
q
⊥
sin
(
t
Δ
θ
)
(2)
\bold{q}(t)=\bold{q}_0\cos(t\Delta \theta)+\bold{q}_{\perp}\sin(t\Delta\theta)\tag{2}
q(t)=q0cos(tΔθ)+q⊥sin(tΔθ)(2)
另一种类似的方法利用 连接
q
0
\bold{q}_0
q0和
q
1
\bold{q}_1
q1 的大圆弧上的任意点都必须是其端点的线性组合(因为三个向量共面)。
由(1)式可得
q
⊥
=
q
1
−
q
0
cos
Δ
θ
sin
Δ
θ
\bold{q}_{\perp}=\dfrac{\bold{q}_{1}-\bold{q}_{0}\cos \Delta \theta }{\sin \Delta \theta }
q⊥=sinΔθq1−q0cosΔθ
代入(2)式
q
(
t
)
=
q
0
cos
(
t
Δ
θ
)
+
q
⊥
sin
(
t
Δ
θ
)
=
q
0
cos
(
t
Δ
θ
)
+
q
1
−
q
0
cos
0
θ
sin
Δ
θ
sin
(
t
Δ
θ
)
=
q
0
(
cos
(
t
Δ
θ
)
−
cos
Δ
θ
sin
(
t
Δ
θ
)
sin
Δ
θ
)
+
q
1
sin
(
t
Δ
θ
)
sin
Δ
θ
=
q
0
sin
Δ
θ
cos
(
t
Δ
θ
)
−
cos
Δ
θ
sin
(
t
Δ
θ
)
sin
Δ
θ
+
q
1
sin
(
t
Δ
θ
)
sin
Δ
θ
=
q
0
sin
(
1
−
t
)
Δ
θ
sin
Δ
θ
+
q
1
sin
(
t
Δ
θ
)
sin
Δ
θ
\begin{aligned} \bold{q}\left( t\right) &=\bold{q}_{0}\cos \left( t\Delta \theta \right) +\bold{q}_{\perp}\sin \left( t\Delta \theta \right) \\ &=\bold{q}_{0}\cos \left( t\Delta \theta \right) +\dfrac{\bold{q}_{1}-\bold{q}_{0}\cos _{0}\theta }{\sin \Delta \theta }\sin \left( t\Delta \theta \right) \\ &=\bold{q}_{0}\left( \cos \left( t\Delta \theta \right) -\dfrac{\cos \Delta \theta \sin \left( t\Delta \theta \right) }{\sin \Delta \theta }\right) +\bold{q}_1\dfrac{\sin \left( t\Delta \theta \right) }{\sin \Delta \theta }\\ &=\bold{q}_{0}\dfrac{\sin \Delta \theta \cos \left( t\Delta \theta \right) -\cos \Delta \theta \sin \left( t\Delta \theta \right) }{\sin \Delta \theta }+\bold{q}_1\dfrac{\sin \left( t\Delta \theta \right) }{\sin \Delta \theta }\\ &=\bold{q}_{0}\dfrac{\sin \left( 1-t\right) \Delta \theta }{\sin \Delta \theta }+\bold{q}_{1}\dfrac{\sin \left( t\Delta \theta \right) }{\sin \Delta \theta } \end{aligned}
q(t)=q0cos(tΔθ)+q⊥sin(tΔθ)=q0cos(tΔθ)+sinΔθq1−q0cos0θsin(tΔθ)=q0(cos(tΔθ)−sinΔθcosΔθsin(tΔθ))+q1sinΔθsin(tΔθ)=q0sinΔθsinΔθcos(tΔθ)−cosΔθsin(tΔθ)+q1sinΔθsin(tΔθ)=q0sinΔθsin(1−t)Δθ+q1sinΔθsin(tΔθ)
这个公式的好处是对称,定义反向插值系数
s
=
1
−
t
s=1-t
s=1−t
q
(
s
)
=
q
1
sin
(
1
−
s
)
Δ
θ
sin
Δ
θ
+
q
0
sin
(
s
Δ
θ
)
sin
Δ
θ
\bold{q}\left( s\right)=\bold{q}_{1}\dfrac{\sin \left( 1-s\right) \Delta \theta }{\sin \Delta \theta }+\bold{q}_{0}\dfrac{\sin \left( s\Delta \theta \right) }{\sin \Delta \theta }
q(s)=q1sinΔθsin(1−s)Δθ+q0sinΔθsin(sΔθ)
该式就是同样的公式将
q
0
\bold{q}_0
q0与
q
1
\bold{q}_1
q1互换。
所有这些基于四元数的SLERP方法都需要一定的注意:确保沿着最短路径进行正确的插值,即旋转角度 ϕ ⩽ π \phi\leqslant \pi ϕ⩽π。由于SO(3)的四元数双覆盖(见第2.4.6节),只有两个四元数之间的夹角是锐角 Δ θ ⩽ π / 2 \Delta \theta\leqslant \pi/2 Δθ⩽π/2,插值才是按照最短路径进行的。对这种情况的测试和求解很简单:如果 c o s ( Δ θ ) = q 0 T q 1 < 0 cos(\Delta \theta) = \bold{q}_0^T \bold{q}_1 < 0 cos(Δθ)=q0Tq1<0,则将 q 1 \bold{q}_1 q1替换为 − q 1 −\bold{q}_1 −q1,然后重新开始。
对于单位四元数
q
\bold{q}
q,其四元数乘法矩阵
[
q
]
L
\left[\bold{q}\right]_L
[q]L和
[
q
]
R
\left[\bold{q}\right]_R
[q]R满足如下性质(证明参考1):
[
q
]
[
q
]
T
=
I
det
(
[
q
]
)
=
+
1
\left[ \bold{q}\right] \left[ \bold{q}\right] ^{T}=\bold{I}\\ \det(\left[\bold{q}\right])=+1
[q][q]T=Idet([q])=+1
因此其为
S
O
(
4
)
SO(4)
SO(4)中的元素,即
R
4
\mathbb{R}^4
R4空间中的旋转矩阵。更具体地说,它们代表了一种特殊类型的旋转,称为isoclinic rotation等斜旋转,我们将在后面解释。因此,四元数旋转对应于中的两个串联的等斜旋转。
[ q ] L [ q ] L T = [ s − v T v s I + v ∧ ] [ s v T − v s I − v ∧ ] = [ s 2 + v T v s v T − s v T + v T v ∧ s v − s v − v ∧ v v v T + ( s I + v ∧ ) ( s I − v ∧ ) ] = [ s 2 + v T v 0 0 v v T + s 2 I − ( v ∧ ) 2 ] = I \begin{aligned} \left[ \bold{q}\right]_L \left[ \bold{q}\right]_L ^{T} &=\begin{bmatrix} s & -\bold{v}^{T} \\ \bold{v} & s\bold{I}+\bold{v}^{\wedge } \end{bmatrix}\begin{bmatrix} s & \bold{v}^{T} \\ -\bold{v} & s\bold{I}-\bold{v}^{\wedge } \end{bmatrix} \\ &=\begin{bmatrix} s^{2}+\bold{v}^{T}\bold{v} & s\bold{v}^{T}-s\bold{v}^{T}+\bold{v}^{T}\bold{v}^{\wedge } \\ s\bold{v}-s\bold{v}-\bold{v}^{\wedge }\bold{v} & \bold{vv}^{T}+\left( s\bold{I}+\bold{v}^{\wedge}\right) \left( s\bold{I}-\bold{v}^{\wedge}\right) \end{bmatrix} \\ &=\begin{bmatrix} s^{2}+\bold{v}^{T}\bold{v} & 0 \\ 0 & \bold{vv}^{T}+s^{2}I-\left( \bold{v}^{\wedge }\right) ^{2} \end{bmatrix}=\bold{I} \end{aligned} [q]L[q]LT=[sv−vTsI+v∧][s−vvTsI−v∧]=[s2+vTvsv−sv−v∧vsvT−svT+vTv∧vvT+(sI+v∧)(sI−v∧)]=[s2+vTv00vvT+s2I−(v∧)2]=I
由于 q \bold{q} q是单位四元数,所以
s 2 + v T v = 1 ( v ∧ ) 2 = v v T = ( 1 − s 2 ) I \begin{aligned} &s^{2}+\bold{v}^{T}\bold{v} =1 \\ &\left( \bold{v}^{\wedge }\right) ^{2}=\bold{vv}^{T}=\left( 1-s^{2}\right) \bold{I} \end{aligned} s2+vTv=1(v∧)2=vvT=(1−s2)I
同理可得
[ q ] R [ q ] R T = I \left[ \bold{q}\right]_R \left[ \bold{q}\right]_R ^{T}=\bold{I} [q]R[q]RT=I ↩︎