说明:转发请注明作者和出处;以下文章内容为精简版,详细版以pdf格式的幻灯片形式呈现,请见链接:视觉SLAM_04_四元数.pdf
0.目录
1.四元数的定义
旋转矩阵用9个量描述3个自由度的旋转,具有冗余性;欧拉角和旋转向量是紧凑的,但具有奇异性。事实上,找不到不带奇异性的三维向量描述方式。
四元数是Hamilton找到的一种扩展的复数,优点是紧凑且没有奇异性,缺点是不够直观、运算复杂。
一个四元数
q
\boldsymbol{q}
q拥有一个实部和三个虚部:
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
(1)
\boldsymbol{q} = q_0 + q_1 \boldsymbol{i} + q_2 \boldsymbol{j} + q_3 \boldsymbol{k} \tag{1}
q=q0+q1i+q2j+q3k(1)其中,
i
\boldsymbol{i}
i,
j
\boldsymbol{j}
j,
k
\boldsymbol{k}
k为四元数的三个虚部,并且满足如下关系:
{
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
(2)
\begin{cases} {} \boldsymbol{i}^2 = \boldsymbol{j}^2 = \boldsymbol{k}^2 = -1 & \\ \boldsymbol{i} \boldsymbol{j} = \boldsymbol{k}, \boldsymbol{j} \boldsymbol{i} = -\boldsymbol{k} & \\ \boldsymbol{j} \boldsymbol{k} = \boldsymbol{i}, \boldsymbol{k} \boldsymbol{j} = -\boldsymbol{i} & \\ \boldsymbol{k} \boldsymbol{i} = \boldsymbol{j}, \boldsymbol{i} \boldsymbol{k} = -\boldsymbol{j} \end{cases} \tag{2}
⎩⎪⎪⎪⎨⎪⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j(2)
也可以用一个标量和一个向量来表达四元数:
q
=
[
s
,
v
]
T
\boldsymbol{q} = [s, \boldsymbol{v}]^T
q=[s,v]T
其中,
s
=
q
0
∈
R
s = q_0 \in \mathbb{R}
s=q0∈R称为四元数的实部,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
\boldsymbol{v} = [q_1, q_2, q_3]^T \in \mathbb{R}^3
v=[q1,q2,q3]T∈R3称为四元数的虚部。如果一个四元数的虚部为0,则称为实四元数;如果一个四元数的实部为0,则称虚四元数。
2.四元数的运算
设有两个四元数:
q
a
=
s
a
+
x
a
i
+
y
a
j
+
z
a
k
=
[
s
a
,
v
a
]
T
,
q
b
=
s
b
+
x
b
i
+
y
b
j
+
z
b
k
=
[
s
b
,
v
b
]
T
\boldsymbol{q_a} = s_a + x_a \boldsymbol{i} + y_a \boldsymbol{j} + z_a \boldsymbol{k} = [s_a, \boldsymbol{v_a}]^T, \quad \boldsymbol{q_b} = s_b + x_b \boldsymbol{i} + y_b \boldsymbol{j} + z_b \boldsymbol{k} = [s_b, \boldsymbol{v_b}]^T
qa=sa+xai+yaj+zak=[sa,va]T,qb=sb+xbi+ybj+zbk=[sb,vb]T
1.加减法
q
a
±
q
b
=
[
s
a
±
s
b
,
v
a
±
v
b
]
T
(3)
\boldsymbol{q_a} \pm \boldsymbol{q_b} = [s_a \pm s_b, \boldsymbol{v_a} \pm \boldsymbol{v_b}]^T \tag{3}
qa±qb=[sa±sb,va±vb]T(3)
2.乘法
乘法是把
q
a
\boldsymbol{q_a}
qa的每一项与
q
b
\boldsymbol{q_b}
qb的每一项相乘,最后相加,虚部要按照(2)式进行。
q
a
q
b
=
s
a
s
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
+
(
s
a
x
b
+
x
a
s
b
+
y
a
z
b
−
z
a
y
b
)
i
+
(
s
a
y
b
−
x
a
z
b
+
y
a
s
b
+
z
a
x
b
)
j
+
(
s
a
z
b
+
x
a
y
b
−
y
a
x
b
+
z
a
s
b
)
k
(4)
\begin{aligned} \boldsymbol{q_a} \boldsymbol{q_b} = s_a s_b - x_a x_b - y_a y_b - z_a z_b \\ + (s_a x_b + x_a s_b + y_a z_b - z_a y_b)\boldsymbol{i} \\ + (s_a y_b - x_a z_b + y_a s_b + z_a x_b)\boldsymbol{j} \\ + (s_a z_b + x_a y_b - y_a x_b + z_a s_b)\boldsymbol{k} \end{aligned} \tag{4}
qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k(4)
如果写成向量形式并利用内外积运算,表达会更加简洁:
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
]
T
(5)
\boldsymbol{q_a} \boldsymbol{q_b} = [s_a s_b - \boldsymbol{v_a}^T \boldsymbol{v_b}, s_a \boldsymbol{v_b} + s_b \boldsymbol{v_a} + \boldsymbol{v_a} \times \boldsymbol{v_b}]^T \tag{5}
qaqb=[sasb−vaTvb,savb+sbva+va×vb]T(5)
3.模长
四元数的模长定义为:
∥
q
a
∥
=
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
(6)
\begin{Vmatrix} \boldsymbol{q_a} \end{Vmatrix} = \sqrt{s^2_a + x^2_a + y^2_a + z^2_a} \tag{6}
∥∥qa∥∥=sa2+xa2+ya2+za2(6)
可以验证,两个四元数乘积的模等于模的乘积:
∥
q
a
q
b
∥
=
∥
q
a
∥
∥
q
b
∥
(7)
\begin{Vmatrix} \boldsymbol{q_a} \boldsymbol{q_b} \end{Vmatrix} = \begin{Vmatrix} \boldsymbol{q_a} \end{Vmatrix} \begin{Vmatrix} \boldsymbol{q_b} \end{Vmatrix} \tag{7}
∥∥qaqb∥∥=∥∥qa∥∥∥∥qb∥∥(7)
4.共轭
四元数的共轭是把虚部取成相反数:
q
a
∗
=
s
a
−
x
a
i
−
y
a
j
−
z
a
k
=
[
s
a
,
−
v
a
]
T
(8)
\boldsymbol{q^*_a} = s_a - x_a \boldsymbol{i} - y_a \boldsymbol{j} - z_a \boldsymbol{k} = [s_a, -\boldsymbol{v_a}]^T \tag{8}
qa∗=sa−xai−yaj−zak=[sa,−va]T(8) 四元数共轭与其本身相乘,会得到一个实四元数,其实部为模长的平方:
q
∗
q
=
q
q
∗
=
[
s
a
2
+
v
T
v
,
0
]
(9)
\boldsymbol{q^*} \boldsymbol{q} = \boldsymbol{q} \boldsymbol{q^*} = [s^2_a + \boldsymbol{v}^T \boldsymbol{v}, \boldsymbol{0}] \tag{9}
q∗q=qq∗=[sa2+vTv,0](9)
5.逆
一个四元数的逆为:
q
−
1
=
q
∗
∥
q
a
∥
2
(10)
\boldsymbol{q^{-1}} = \frac{\boldsymbol{q^*}}{ \begin{Vmatrix} \boldsymbol{q_a} \end{Vmatrix}^2} \tag{10}
q−1=∥∥qa∥∥2q∗(10)
按此定义,四元数和自己的逆的乘积为实四元数
1
\boldsymbol 1
1:
q
q
−
1
=
q
−
1
q
=
1
(11)
\boldsymbol{q} \boldsymbol{q^{-1}} = \boldsymbol{q^{-1}} \boldsymbol{q} = \boldsymbol{1} \tag{11}
qq−1=q−1q=1(11)6.数乘
k
q
=
[
k
s
,
k
v
]
T
(12)
k \boldsymbol{q} = [ks, k \boldsymbol{v}]^T \tag{12}
kq=[ks,kv]T(12)
3.用四元数表示旋转
用四元数旋转点 p = [ x , y , z ] ∈ R 3 \boldsymbol p = [x, y, z] \in \mathbb{R}^3 p=[x,y,z]∈R3。
- 构造四元数:
p = [ 0 , x , y , z ] T = [ 0 , v ] T \boldsymbol p = [0, x, y, z]^T = [0, \boldsymbol v]^T p=[0,x,y,z]T=[0,v]T - 确定旋转四元数:
设单位长度旋转轴 n = ( n x , n y , n z ) \boldsymbol n = (n_x, n_y, n_z) n=(nx,ny,nz),和旋转角 θ \theta θ,则对应的旋转四元数为:
q = [ cos θ 2 , x sin θ 2 , y sin θ 2 , z sin θ 2 ] \boldsymbol{q} = [\cos \frac{\theta}{2}, x \sin \frac{\theta}{2}, y \sin \frac{\theta}{2}, z \sin \frac{\theta}{2}] q=[cos2θ,xsin2θ,ysin2θ,zsin2θ] - 代入四元数旋转公式:
q ′ = q p q − 1 (13) \boldsymbol q^{'} = \boldsymbol{q} \boldsymbol{p} \boldsymbol{q}^{-1} \tag{13} q′=qpq−1(13)这里的乘法均为四元数乘法,其中, p ′ \boldsymbol p^{'} p′为 p \boldsymbol p p经过旋转之后的四元数。取出 p ′ \boldsymbol p^{'} p′的虚部即为旋转之后的点坐标。
4.参考文献
- 高翔等. 视觉SLAM十四讲:从理论到实践第二版. 北京:电子工业出版社,2019.8.
- 赫恩. 计算机图形学第三版. 北京:电子工业出版社,2005.6.