三维空间刚体运动4-1:四元数表示变换(各形式相互转换加代码)
序:本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》,讲解三维空间刚体运动,为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解,其中四元数部分较多较复杂,又分为四部分。如果读者急于实践,可直接阅读第五部分的机器人运动轨迹,此部分详细讲解了安装准备工作。此系列总体目录如下:
- 旋转矩阵和变换矩阵;
- 旋转向量表示旋转;
- 欧拉角表示旋转;
- 四元数包括以下部分:
4-1. 四元数表示变换;
4-2. 四元数线性插值方法:LinEuler/LinMat/Lerp/Nlerp/Slerp;
4-3. 四元数多点插值方法:Squad;
4-4. 四元数多点连续解析解插值方法:Spicv;
4-5. 四元数多点离散数值解插值方法:Sping。 - 实践:SLAM中显示机器人运动轨迹及相机位姿。
1. 四元数的定义
1.1 为什么使用四元数
旋转矩阵用9个量描述3自由度的旋转,具有冗余性;欧拉角和旋转向量是紧凑的,但具有奇异性。事实上,我们找不到不带奇异性的三维向量描述方式。
回忆之前学习过的复数,我们用复数集
C
\mathbb{C}
C表示复平面上的向量,可以表示为
z
=
a
+
b
i
z=a+bi
z=a+bi的形式,其中
a
,
b
∈
R
a,b\in R
a,b∈R而且
i
2
=
−
1
i^{2}=-1
i2=−1,而复数的乘法则表示复平面上的旋转:例如,乘上复数
i
i
i相当于逆时针把一个复向量旋转
9
0
∘
90^{\circ }
90∘。类似的,在表达三维空间旋转时,也有一种类似于复数的代数:四元数(Quaternion)。四元数是Hamilton找到的一种扩展的复数。它既是紧凑的,也没有奇异性。如果说缺点,四元数不够直观,其运算稍复杂些。
1.2 复数与四元数
把四元数与复数类比可以帮助你更快地理解四元数。例如,当我们想要将复平面的向量旋转
θ
\theta
θ角时,可以给这个复向量乘以
e
i
θ
e^{i\theta}
eiθ,这是极坐标表示的复数,它也可以写成普通的形式,只要用欧拉公式即可:
e
i
θ
=
c
o
s
θ
+
i
s
i
n
θ
.
e^{i\theta} = cos\theta + isin\theta.
eiθ=cosθ+isinθ.
欧拉公式将指数函数的定义域扩大到了复数域,建立三角函数和指数函数的关系,被誉为“数学中的天桥”。欧拉公式的简单推导如下,
e
x
e^{x}
ex的泰勒展开式为:
e
x
=
1
+
x
+
1
2
!
x
2
+
1
3
!
x
3
+
⋅
⋅
⋅
e^{x} = 1+x+\frac{1}{2!}x^{2}+\frac{1}{3!}x^{3}+\cdot \cdot \cdot
ex=1+x+2!1x2+3!1x3+⋅⋅⋅将
x
x
x替换为
i
θ
i\theta
iθ:
e
i
θ
=
1
+
i
θ
+
1
2
!
(
i
θ
)
2
+
1
3
!
(
i
θ
)
3
+
1
4
!
(
i
θ
)
4
+
1
5
!
(
i
θ
)
5
+
1
6
!
(
i
θ
)
6
+
1
7
!
(
i
θ
)
7
+
1
8
!
(
i
θ
)
8
+
⋅
⋅
⋅
=
1
+
i
θ
−
1
2
!
θ
2
−
1
3
!
i
θ
3
+
1
4
!
θ
4
+
1
5
!
i
θ
5
−
1
6
!
θ
6
−
1
7
!
i
θ
7
+
1
8
!
θ
8
+
⋅
⋅
⋅
=
(
1
−
θ
2
2
!
+
θ
4
4
!
−
θ
6
6
!
+
θ
8
8
!
−
⋅
⋅
⋅
)
+
i
(
θ
−
θ
3
3
!
+
θ
5
5
!
−
θ
7
7
!
+
⋅
⋅
⋅
)
=
c
o
s
θ
+
i
s
i
n
θ
.
\begin{aligned} e^{i\theta} &= 1+i\theta+\frac{1}{2!}(i\theta)^{2}+\frac{1}{3!}(i\theta)^{3}+\frac{1}{4!}(i\theta)^{4}+\frac{1}{5!}(i\theta)^{5}+\frac{1}{6!}(i\theta)^{6}+\frac{1}{7!}(i\theta)^{7}+\frac{1}{8!}(i\theta)^{8}+\cdot \cdot \cdot \\ &= 1+i\theta-\frac{1}{2!}\theta^{2}-\frac{1}{3!}i\theta^{3}+\frac{1}{4!}\theta^{4}+\frac{1}{5!}i\theta^{5}-\frac{1}{6!}\theta^{6}-\frac{1}{7!}i\theta^{7}+\frac{1}{8!}\theta^{8}+\cdot \cdot \cdot \\ &= (1-\frac{\theta^{2}}{2!}+\frac{\theta^{4}}{4!}-\frac{\theta^{6}}{6!}+\frac{\theta^{8}}{8!}-\cdot \cdot \cdot)+i(\theta-\frac{\theta^{3}}{3!}+\frac{\theta^{5}}{5!}-\frac{\theta^{7}}{7!}+\cdot \cdot \cdot ) \\ &= cos\theta + isin\theta. \end{aligned}
eiθ=1+iθ+2!1(iθ)2+3!1(iθ)3+4!1(iθ)4+5!1(iθ)5+6!1(iθ)6+7!1(iθ)7+8!1(iθ)8+⋅⋅⋅=1+iθ−2!1θ2−3!1iθ3+4!1θ4+5!1iθ5−6!1θ6−7!1iθ7+8!1θ8+⋅⋅⋅=(1−2!θ2+4!θ4−6!θ6+8!θ8−⋅⋅⋅)+i(θ−3!θ3+5!θ5−7!θ7+⋅⋅⋅)=cosθ+isinθ.当
θ
=
π
\theta=\pi
θ=π时,带入欧拉公式得到:
e
i
π
=
c
o
s
π
+
i
s
i
n
π
=
−
1
⇒
e
i
π
+
1
=
0
e^{i\pi} = cos\pi + isin\pi = -1 \Rightarrow e^{i\pi} + 1 = 0
eiπ=cosπ+isinπ=−1⇒eiπ+1=0其中
e
i
π
+
1
=
0
e^{i\pi} + 1 = 0
eiπ+1=0就是欧拉恒等式,它被誉为上帝公式,因为
e
、
π
、
i
e 、 \pi 、 i
e、π、i、乘法单位元1、加法单位元0,这五个重要的数学元素全部被包含在内,在数学爱好者眼里,它仿佛一行诗道尽了数学的美好。欧拉公式的详细说明可参见《欧拉公式之美》。
欧拉公式的右侧正是一个单位长度的复数,所以在二维情况下,旋转可以有单位复数来描述。类似的,可以看到,三维旋转可以有单位四元数来描述。
1.3 四元数的形式
四元数的定义和复数非常类似,唯一的区别就是四元数有三个虚部,而复数只有一个。所有的四元数
q
∈
H
q \in \mathbb{H}
q∈H(
H
\mathbb{H}
H代表四元数的发现者William Rowan Hamilton)都可以写成如下形式:
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
.
\mathbf{q}=q_{0}+q_{1}i+q_{2}j+q_{3}k.
q=q0+q1i+q2j+q3k.其中
i
,
j
,
k
i,j,k
i,j,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
.
\left\{\begin{matrix} i^{2}=j^{2}=k^{2}=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j \end{matrix}\right..
⎩⎪⎪⎨⎪⎪⎧i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j.
如果把
i
,
j
,
k
i,j,k
i,j,k看成三个坐标轴,那么它们与自己的乘法和复数一样,相互之间的乘法和外积一样。有时,人们也用一个标量和一个向量来表达四元数:
q
=
[
s
,
v
]
T
,
s
=
q
0
∈
R
,
v
=
[
q
1
,
q
2
,
q
3
]
T
∈
R
3
.
\mathbf{q}= [s, v]^{T}, s=q_{0} \in \mathbb{R},v=[q_{1},q_{2},q_{3}]^{T} \in \mathbb{R}^{3}.
q=[s,v]T,s=q0∈R,v=[q1,q2,q3]T∈R3.这里,
s
s
s称为四元数的实部,而
v
v
v称为它的虚部。如果一个四元数的虚部为0,则称为实四元数。反之,若实部为0,则称为虚四元数或纯四元数。
如果
∥
q
∥
=
1
\left \| q \right \|=1
∥q∥=1,那么
q
q
q是单位四元数。可以用单位四元数表示三维空间中任意一个旋转,不过这种表达方式和复数有着微妙的不同。在复数中,乘以
i
i
i意味着旋转
9
0
∘
90^{\circ }
90∘,而在四元数中,乘以
i
i
i对应着旋转
18
0
∘
180^{\circ }
180∘,这样才能保证
i
j
=
k
ij=k
ij=k的性质。而
i
2
=
−
1
i^{2}=-1
i2=−1,意味着绕
i
i
i轴旋转
36
0
∘
360^{\circ }
360∘后得到一个相反的东西,也就是要旋转两周才会和它原先的样子相等。
下面我们看一下四元数之间的运算法则。
2. 四元数的运算
四元数和复数一样,可以进行一系列的运算。常见的有四则运算、数乘、内外积、模长、共轭、求逆等,高阶运算有指数、对数、求微分等,下面分别介绍。
为了方便,这里给出三个定义:1)不包含四元数{[0,(0,0,0)]}的集合记为
H
∘
\overset{\circ}{\mathbb{H}}
H∘;2)单位四元数集合记为
H
1
\mathbb{H}_{1}
H1,它是
H
∘
\overset{\circ}{\mathbb{H}}
H∘的子集;3)中元(neutral element)为:
I
=
[
1
,
0
]
∈
H
1
I=[1,0] \in \mathbb{H}_{1}
I=[1,0]∈H1,它使得:
q
I
=
I
q
=
[
1
s
,
1
v
]
=
[
s
,
v
]
=
q
qI=Iq=[1s,1v]=[s,v]=q
qI=Iq=[1s,1v]=[s,v]=q。
2.1 基础运算
现有四元数 q , q a , q b q,q_{a},q_{b} q,qa,qb,单元运算时使用 q q q,否则使用 q a , q b q_{a},q_{b} qa,qb。它们的向量表示为 [ s , v ] , [ s a , v a ] [s,v],[s_{a},v_{a}] [s,v],[sa,va], [ s b , v b ] [s_{b}, v_{b}] [sb,vb],其中 v = x i + y j + z k , v a = x a i + y a j + z a k , v b = x b i + y b j + z b k v=xi+yj+zk,v_{a}=x_{a}i+y_{a}j+z_{a}k,v_{b}=x_{b}i+y_{b}j+z_{b}k v=xi+yj+zk,va=xai+yaj+zak,vb=xbi+ybj+zbk,或者原始四元数表示为: q = s + x i + y j + z k , q a = s a + x a i + y a j + z a k , q b = s b + x b i + y b j + z b k . q=s+xi+yj+zk,q_{a}=s_{a}+x_{a}i+y_{a}j+z_{a}k,q_{b}=s_{b}+x_{b}i+y_{b}j+z_{b}k. q=s+xi+yj+zk,qa=sa+xai+yaj+zak,qb=sb+xbi+ybj+zbk.那么,其运算可表示如下:
- 加法和减法 q a ± q b = [ s a ± s b , v a ± v b ] q_{a}\pm q_{b}=[s_{a}\pm s_{b},v_{a}\pm v_{b}] qa±qb=[sa±sb,va±vb]
- 数乘
又称标量乘法,四元数可以与数 k ∈ R k \in \mathbb{R} k∈R相乘, k k k的四元数形式为 k = [ k , 0 ] k=[k, 0] k=[k,0],故数乘可以表示为: k q = [ k , 0 ] [ s , v ] = [ k s , k v ] kq=[k, 0][s,v]=[ks,kv] kq=[k,0][s,v]=[ks,kv] - 内积或点积
假设 q a q_{a} qa和 q b q_{b} qb之间的夹角为 α \alpha α,则内积或点积定义为: q a ⋅ q b = ∥ q a ∥ ∥ q b ∥ cos α = s a s b + x a x b + y a y b + z a z b \begin{aligned} q_{a} \cdot q_{b} &= \left \| q_{a} \right \| \left \| q_{b} \right \| \cos \alpha \\ &=s_{a}s_{b}+x_{a}x_{b}+y_{a}y_{b}+z_{a}z_{b}\end{aligned} qa⋅qb=∥qa∥∥qb∥cosα=sasb+xaxb+yayb+zazb - 外积或叉积
一般意义的四元数乘法指的是外积或叉积,它是把 q a q_{a} qa的每一项与 q b q_{b} qb的每一项相乘,最后相加,整理得: 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 . \begin{aligned} 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. \end{aligned} qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−yaxb+zasb)k.虽然稍微复杂,但形式上还是整齐有序的。如果写成向量形式并利用内外积运算,该表达会更加简洁: 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 ] q_{a}q_{b}=[s_{a}s_{b}-v_{a}^{T}v_{b},s_{a}v_{b}+s_{b}v_{a}+v_{a}\times v_{b}] qaqb=[sasb−vaTvb,savb+sbva+va×vb]这个结果也称为 G r a B m a n n GraBmann GraBmann积,它是四元数与旋转联系起来的关键。
另外,由于最后一项外积的存在,四元数乘法通常是不可交换的,除非 v a v_{a} va和 v b v_{b} vb在 R \mathbb{R} R中共线,此时外项积为零。
不过对于同一个四元数,具有相乘和幂的性质: q a q b = q a + b , ( q a ) b = q a b , a , b ∈ R q^{a}q^{b}=q^{a+b},(q^{a})^{b}=q^{ab},a,b\in\mathbb{R} qaqb=qa+b,(qa)b=qab,a,b∈R - 模长或范数
四元数的模长或范数定义为: ∥ q ∥ = q q ∗ = s 2 + x 2 + y 2 + z 2 \left \| q \right \|=\sqrt{qq^{*}}=\sqrt{s^{2}+x^{2}+y^{2}+z^{2}} ∥q∥=qq∗=s2+x2+y2+z2可以验证,两个四元数乘积的模即模的乘积,这使得单位四元数相乘后仍是单位四元数: ∥ q a q b ∥ = ∥ q a ∥ ∥ q b ∥ \left \| q_{a}q_{b} \right \|=\left \| q_{a} \right \|\left \| q_{b} \right \| ∥qaqb∥=∥qa∥∥qb∥ - 共轭
四元数的共轭是把虚部取成相反数: q ∗ = s − x i − y j − z k = [ s , − v ] . q^{*}=s-xi-yj-zk=[s, -v]. q∗=s−xi−yj−zk=[s,−v].四元数共轭与其本身相乘,会得到一个实四元数,其实部为模长的平方: q ∗ q = q q ∗ = [ s 2 + v T v , 0 ] T q^{*}q=qq^{*}=[s^{2}+v^{T}v,0]^{T} q∗q=qq∗=[s2+vTv,0]T此外,共轭的乘积满足以下性质: ( q a q b ) ∗ = q b ∗ q a ∗ (q_{a}q_{b})^{*}=q_{b}^{*}q_{a}^{*} (qaqb)∗=qb∗qa∗ - 逆
四元数 q ∈ H ∘ q \in \overset{\circ}{\mathbb{H}} q∈H∘的逆为: q − 1 = q ∗ / ∥ q ∥ q^{-1}=q^{*}/\left \| q \right\| q−1=q∗/∥q∥按此定义,四元数和自己的逆的乘积为实四元数 1 1 1: q q − 1 = q − 1 q = q q ∗ / ∥ q ∥ 2 = 1 qq^{-1}=q^{-1}q=qq^{*}/\left \| q \right\|^{2}=1 qq−1=q−1q=qq∗/∥q∥2=1如果 q \mathbf{q} q为单位四元数,其逆和共轭就是同一个量。同时,乘积的逆具有和矩阵相似的性质: ( q a q b ) − 1 = q b − 1 q a − 1 (q_{a}q_{b})^{-1}=q_{b}^{-1}q_{a}^{-1} (qaqb)−1=qb−1qa−1
2.2 高阶运算
高阶运算包括对数、指数和微分运算,对数和指数在5.3节旋转证明完后再次引出定义,这里作为性质总结先做一下介绍,感兴趣的可以阅读后边证明。介绍高阶运算前,引入以下定理:
四元数定理1:设
q
=
[
s
,
v
]
∈
H
1
q = [s,\mathbf{v}] \in \mathbb{H}_{1}
q=[s,v]∈H1,那么存在
v
′
∈
R
1
3
\mathbf{v}^{'} \in \mathbb{R}^{3}_{1}
v′∈R13和
θ
∈
[
−
π
,
π
]
\theta \in [-\pi, \pi]
θ∈[−π,π],使得
q
=
[
cos
θ
,
v
′
sin
θ
]
q=[\cos \theta,\mathbf{v}^{'}\sin \theta]
q=[cosθ,v′sinθ]。
证明:如果
q
=
[
1
,
0
]
q=[1,0]
q=[1,0],使
θ
=
0
\theta=0
θ=0,那么
v
′
\mathbf{v}^{'}
v′可以在
R
3
\mathbb{R}^{3}
R3中的单位向量自由选择。
如果
q
≠
[
1
,
0
]
q\neq[1,0]
q=[1,0],使
k
=
∣
v
∣
k=\left | \mathbf{v} \right |
k=∣v∣,那么
v
′
=
1
k
v
\mathbf{v}^{'}=\frac{1}{k}\mathbf{v}
v′=k1v,这时
v
=
k
v
′
\mathbf{v}=k\mathbf{v}^{'}
v=kv′并且
v
′
\mathbf{v}^{'}
v′也是
R
3
\mathbb{R}^{3}
R3中的单位向量,此时
q
=
[
s
,
k
v
′
]
q = [s,k\mathbf{v}^{'}]
q=[s,kv′]。由于
q
q
q是一个单位四元数,因此可以得到:
1
=
∥
q
∥
=
s
2
+
v
⋅
v
=
s
2
+
k
2
v
′
⋅
v
′
=
s
2
+
k
2
1=\left \| q \right \|=s^{2}+\mathbf{v}\cdot\mathbf{v}=s^{2}+k^{2}\mathbf{v}^{'}\cdot\mathbf{v}^{'}=s^{2}+k^{2}
1=∥q∥=s2+v⋅v=s2+k2v′⋅v′=s2+k2等式
s
2
+
k
2
=
1
s^{2}+k^{2}=1
s2+k2=1代表平面中的圆,由于圆也可以表示为
cos
2
θ
+
sin
2
θ
=
1
\cos^{2}\theta+\sin^{2}\theta=1
cos2θ+sin2θ=1,因此存在
θ
∈
[
−
π
,
π
]
\theta\in[-\pi,\pi]
θ∈[−π,π],使得
s
=
cos
θ
,
k
=
sin
θ
s=\cos\theta,k=\sin\theta
s=cosθ,k=sinθ,总之,我们会得到想要的结果:
q
=
[
s
,
v
]
=
[
s
,
k
v
′
]
=
[
cos
θ
,
v
′
sin
θ
]
q=[s,\mathbf{v}]=[s,k\mathbf{v}^{'}]=[\cos\theta,\mathbf{v}^{'}\sin\theta]
q=[s,v]=[s,kv′]=[cosθ,v′sinθ]
接下来我们具体介绍四元数的高阶运算:
- 对数
使 q ∈ H 1 q \in \mathbb{H}_{1} q∈H1, q q q表示为 [ cos θ , v sin θ ] , θ ∈ R , v ∈ R 3 , ∣ v ∣ = 1 [\cos \theta,\mathbf{v}\sin \theta], \theta \in \mathbb{R}, \mathbf{v} \in \mathbb{R}^{3}, |\mathbf{v}|=1 [cosθ,vsinθ],θ∈R,v∈R3,∣v∣=1,那么对数函数可以定义为: log q ≡ [ 0 , θ v ] \log q \equiv [0, \theta \mathbf{v}] logq≡[0,θv]注意,根据定义存在 log [ 1 , ( 0 , 0 , 0 ) ] = [ 0 , ( 0 , 0 , 0 ) ] \log [1,(0,0,0)]=[0,(0,0,0)] log[1,(0,0,0)]=[0,(0,0,0)],并且 log q \log q logq不是单位四元数。 - 指数
对于以下形式的四元数: q = [ 0 , θ v ] , θ ∈ R , v ∈ R 3 , ∣ v ∣ = 1 q = [0, \theta \mathbf{v}], \theta \in \mathbb{R}, \mathbf{v} \in \mathbb{R}^{3}, |\mathbf{v}|=1 q=[0,θv],θ∈R,v∈R3,∣v∣=1,指数函数被定义为: exp q ≡ [ cos θ , v sin θ ] \exp q \equiv [\cos \theta,\mathbf{v}\sin \theta] expq≡[cosθ,vsinθ]注意:指数函数和对数函数可以互相转换,并且指数可以对应到单位四元数。
从上面的定义可得到以下定义,对于 q t , q ∈ H 1 , t ∈ R q^{t},q \in \mathbb{H}_{1}, t \in \mathbb{R} qt,q∈H1,t∈R,它的指数方程形式: q t = exp ( t log q ) q^{t}=\exp (t\log q) qt=exp(tlogq)此外当 q ∈ H 1 , t , a , b ∈ R q \in \mathbb{H}_{1}, t,a,b \in \mathbb{R} q∈H1,t,a,b∈R还有其他性质,枚举如下(限于篇幅这里不再给出证明): log q t = t log q , q a q b = q a + b , ( q a ) b = q a b \log q^{t} = t \log q, q^{a}q^{b}=q^{a+b},(q^{a})^{b}=q^{ab} logqt=tlogq,qaqb=qa+b,(qa)b=qab - 微分
此处给出常用的微分性质和证明,并将用于下一篇的四元数插值曲线的推导。
(1)当指数为常数时的微分法则
对于 q t , q ∈ H 1 , t ∈ R q^{t},q \in \mathbb{H}_{1}, t \in \mathbb{R} qt,q∈H1,t∈R,有: d d t q t = q t log ( q ) \frac{d}{dt} q^{t}=q^{t} \log (q) dtdqt=qtlog(q)下边给出证明。
公式左侧: d d t q t = d d t exp ( t log ( q ) ) = d d t exp ( t [ 0 , θ v ] ) = d d t exp ( [ 0 , t θ v ] ) = d d t [ cos ( t θ ) , sin ( t θ ) v ] = θ [ − sin ( t θ ) , sin ( t θ ) v ] \begin{aligned} \frac{d}{dt} q^{t}&=\frac{d}{dt}\exp(t\log (q))=\frac{d}{dt} \exp(t[0,\theta \mathbf{v}])\\ &= \frac{d}{dt} \exp([0,t\theta \mathbf{v}])=\frac{d}{dt} [\cos(t\theta), \sin(t\theta )\mathbf{v}]\\ &=\theta[-\sin(t\theta),\sin(t\theta)\mathbf{v}] \end{aligned} dtdqt=dtdexp(tlog(q))=dtdexp(t[0,θv])=dtdexp([0,tθv])=dtd[cos(tθ),sin(tθ)v]=θ[−sin(tθ),sin(tθ)v]公式右侧: q t log ( q ) = exp ( t log ( q ) ) log ( q ) = [ cos ( t θ ) , sin ( t θ ) v ] [ 0 , θ v ] = [ − θ sin ( t θ ) ( v ⋅ v ) , θ cos ( t θ ) v + θ sin ( t θ ) ( v × v ) ] = [ − θ sin ( t θ ) , θ cos ( t θ ) v ] = θ [ − sin ( t θ ) , cos ( t θ ) v ] \begin{aligned} q^{t}\log(q) &= \exp(t\log(q))\log(q)=[\cos(t\theta),\sin(t\theta)\mathbf{v}][0,\theta \mathbf{v}] \\ &= [-\theta\sin(t\theta)(\mathbf{v}\cdot\mathbf{v}),\theta\cos(t\theta)\mathbf{v}+\theta\sin(t\theta)(\mathbf{v}\times\mathbf{v})] \\ &= [-\theta\sin(t\theta),\theta\cos(t\theta)\mathbf{v}]\\ &=\theta[-\sin(t\theta),\cos(t\theta)\mathbf{v}] \end{aligned} qtlog(q)=exp(tlog(q))log(q)=[cos(tθ),sin(tθ)v][0,θv]=[−θsin(tθ)(v⋅v),θcos(tθ)v+θsin(tθ)(v×v)]=[−θsin(tθ),θcos(tθ)v]=θ[−sin(tθ),cos(tθ)v]
(2)当指数为函数时的微分法则
在此之前,给出乘积微分法则和链式微分法则。
乘积微分法则:使 f , g ∈ C 1 ( R , H ) f,g\in C^{1}(\mathbb{R},\mathbb{H}) f,g∈C1(R,H),其中 C 1 ( R , H ) C^{1}(\mathbb{R},\mathbb{H}) C1(R,H)代表函数在定义域 R \mathbb{R} R,值域 H \mathbb{H} H范围内一阶连续可导,这时有: d d t ( f ( t ) g ( t ) ) = ( d d t f ( t ) ) g ( t ) + f ( t ) ( d d t g ( t ) ) \frac{d}{dt}(f(t)g(t))=(\frac{d}{dt}f(t))g(t)+f(t)(\frac{d}{dt}g(t)) dtd(f(t)g(t))=(dtdf(t))g(t)+f(t)(dtdg(t))
链式微分法则:使 f ∈ C 1 ( H , H ) , g ∈ C 1 ( R , H ) f\in C^{1}(\mathbb{H},\mathbb{H}),g\in C^{1}(\mathbb{R},\mathbb{H}) f∈C1(H,H),g∈C1(R,H),这时有: d d t ( f ( g ( t ) ) = f ′ ( g ( t ) ) g ′ ( t ) ) \frac{d}{dt}(f(g(t))=f^{'}(g(t))g^{'}(t)) dtd(f(g(t))=f′(g(t))g′(t))
当指数为函数时的微分法则:使 q ∈ C 1 ( R , H 1 ) , r ∈ C 1 ( R , R ) q\in C^{1}(\mathbb{R},\mathbb{H}_{1}),r\in C^{1}(\mathbb{R},\mathbb{R}) q∈C1(R,H1),r∈C1(R,R),由于 q q q为单位四元数,所以 q ( t ) q(t) q(t)可写为 [ cos θ ( t ) , v ( t ) sin θ ( t ) ] [\cos\theta(t),\mathbf{v}(t)\sin\theta(t)] [cosθ(t),v(t)sinθ(t)],这时有: d d t q ( t ) r ( t ) = d d t exp ( r ( t ) log ( q ( t ) ) ) = d d t exp ( r ( t ) [ 0 , v ( t ) θ ( t ) ] ) = d d t exp ( [ 0 , r ( t ) v ( t ) θ ( t ) ] ) = d d t [ cos ( r ( t ) θ ( t ) ) , sin ( r ( t ) θ ( t ) ) v ( t ) ] = [ − sin ( r ( t ) θ ( t ) ) ( r ′ ( t ) θ ( t ) + r ( t ) θ ′ ( t ) ) , cos ( r ( t ) θ ( t ) ) ( r ′ ( t ) θ ( t ) + r ( t ) θ ′ ( t ) ) v ( t ) + sin ( r ( t ) θ ( t ) ) v ′ ( t ) ] \begin{aligned} \frac{d}{dt}q(t)^{r(t)}&=\frac{d}{dt}\exp(r(t)\log(q(t)))\\ &=\frac{d}{dt}\exp(r(t)[0,\mathbf{v}(t)\theta(t)])\\&=\frac{d}{dt}\exp([0,r(t)\mathbf{v}(t)\theta(t)])\\&=\frac{d}{dt}[\cos(r(t)\theta(t)),\sin(r(t)\theta(t))\mathbf{v}(t)]\\&=[-\sin(r(t)\theta(t))(r^{'}(t)\theta(t)+r(t)\theta^{'}(t)),\\&\cos(r(t)\theta(t))(r^{'}(t)\theta(t)+r(t)\theta^{'}(t))\mathbf{v}(t)+\sin(r(t)\theta(t))\mathbf{v}^{'}(t)]\end{aligned} dtdq(t)r(t)=dtdexp(r(t)log(q(t)))=dtdexp(r(t)[0,v(t)θ(t)])=dtdexp([0,r(t)v(t)θ(t)])=dtd[cos(r(t)θ(t)),sin(r(t)θ(t))v(t)]=[−sin(r(t)θ(t))(r′(t)θ(t)+r(t)θ′(t)),cos(r(t)θ(t))(r′(t)θ(t)+r(t)θ′(t))v(t)+sin(r(t)θ(t))v′(t)]
3. 用四元数表示旋转
本节讲解四元数与旋转的关系,并给出几何证明。
3.1 四元数与旋转关系
我们可以用四元数表达对一个点的旋转。假设有一个空间三维点
v
=
[
x
,
y
,
z
]
∈
R
3
v=[x,y,z] \in \mathbb{R}^{3}
v=[x,y,z]∈R3,以及一个由单位四元数
q
q
q指定的旋转。三维点
v
v
v经过
q
q
q的旋转后变为
v
′
v^{'}
v′。如果使用矩阵描述,那么有
v
′
=
R
v
v^{'}=Rv
v′=Rv。而如何用四元数描述旋转呢?
首先,把三维空间点用一个虚四元数来描述:
w
=
[
0
,
x
,
y
,
z
]
T
=
[
0
,
v
]
T
w=[0,x,y,z]^{T}=[0,v]^{T}
w=[0,x,y,z]T=[0,v]T相当于把四元数的3个虚部和空间中的3个轴相对应。那么,旋转后的点
w
′
w^{'}
w′可表示为这样的乘积:
w
′
=
q
w
q
−
1
w^{'}=qwq^{-1}
w′=qwq−1这里的乘法均为四元数乘法,结果也是四元数。最后把
w
′
w^{'}
w′的虚部取出,即得旋转之后点的坐标。并且可以验证,计算结果的实部为0,即为虚四元数。
下面从几何的角度进一步讲解四元数与3D旋转之间的关联。如果只是简单应用,不需要了解几何证明过程,则本小节就足够了。
3.2 四元数与3D旋转几何证明
回忆一下《旋转向量与罗德里格斯公式》讨论的内容:如果我们需要将一个向量
v
v
v沿着一个用单位向量所定义的旋转轴
u
u
u旋转
θ
\theta
θ度,那么可以将其拆分为正交于旋转轴的
v
⊥
v_{\perp }
v⊥以及平行于旋转轴的
v
∥
v_{\parallel }
v∥,进行旋转后获得
v
⊥
′
v_{\perp }^{'}
v⊥′和
v
∥
′
v_{\parallel }^{'}
v∥′,相加后得到旋转后的结果
v
′
=
v
⊥
′
+
v
∥
′
v^{'}=v_{\perp }^{'}+v_{\parallel }^{'}
v′=v⊥′+v∥′。
将这些向量定义为纯四元数,下标
q
q
q代表三维空间变量对应的四元数:
w
=
[
0
,
v
]
w
′
=
[
0
,
v
′
]
w
⊥
=
[
0
,
v
⊥
]
w
⊥
′
=
[
0
,
v
⊥
′
]
w
∥
=
[
0
,
v
∥
]
w
∥
′
=
[
0
,
v
∥
′
]
u
q
=
[
0
,
u
]
\begin{aligned} & w=[0,v] && w^{'}=[0,v^{'}] \\ & w_{\perp }=[0,v_{\perp }] && w^{'}_{\perp }=[0,v^{'}_{\perp }] \\ & w_{\parallel }=[0,v_{\parallel }] && w^{'}_{\parallel }=[0,v^{'}_{\parallel }] \\ & u_{q}=[0,u]\end{aligned}
w=[0,v]w⊥=[0,v⊥]w∥=[0,v∥]uq=[0,u]w′=[0,v′]w⊥′=[0,v⊥′]w∥′=[0,v∥′]那么我们就能得到:
w
=
w
⊥
+
w
∥
w
′
=
w
⊥
′
+
w
∥
′
\begin{aligned} w=w_{\perp }+w_{\parallel } && w^{'}=w^{'}_{\perp }+w^{'}_{\parallel }\end{aligned}
w=w⊥+w∥w′=w⊥′+w∥′和之前一样,这里也分开讨论
w
⊥
w_{\perp }
w⊥和
w
∥
w_{\parallel }
w∥的情况。
3.2.1 w ⊥ w_{\perp} w⊥的旋转
之前推导过(见《旋转向量与罗德里格斯公式》第2.2.1节),如果一个向量 v ⊥ v_{\perp } v⊥正交于旋转轴 u u u,那么 v ⊥ ′ = c o s ( θ ) v ⊥ + s i n ( θ ) ( u × v ⊥ ) v^{'}_{\perp }=cos(\theta)v_{\perp }+sin(\theta)(u\times v_{\perp }) v⊥′=cos(θ)v⊥+sin(θ)(u×v⊥)将三维向量替换为对应的四元数, v ⊥ ′ v^{'}_{\perp } v⊥′和 v ⊥ v_{\perp } v⊥可以直接替换,而对于 u × v ⊥ u\times v_{\perp } u×v⊥,由于 u q w ⊥ = [ − u ⋅ v ⊥ , u × v ⊥ ] = [ 0 , u × v ⊥ ] = u × v ⊥ \begin{aligned}u_{q}w_{\perp} &=[-u\cdot v_{\perp},u\times v_{\perp}] \\ &=[0,u\times v_{\perp}] \\ &= u\times v_{\perp}\end{aligned} uqw⊥=[−u⋅v⊥,u×v⊥]=[0,u×v⊥]=u×v⊥将之前定义的四元数带入,就能得到 w ⊥ ′ = c o s ( θ ) w ⊥ + s i n ( θ ) ( u q w ⊥ ) w^{'}_{\perp }=cos(\theta)w_{\perp }+sin(\theta)(u_{q}w_{\perp}) w⊥′=cos(θ)w⊥+sin(θ)(uqw⊥)因为四元数遵守分配率,可以继续变换这个等式: w ⊥ ′ = ( c o s ( θ ) + s i n ( θ ) u q ) w ⊥ w^{'}_{\perp }=(cos(\theta)+sin(\theta)u_{q})w_{\perp} w⊥′=(cos(θ)+sin(θ)uq)w⊥此时,可以将 c o s ( θ ) + s i n ( θ ) u q cos(\theta)+sin(\theta)u_{q} cos(θ)+sin(θ)uq看作一个四元数,记为 q q q,且 ∥ q ∥ = 1 \left \| q \right \|=1 ∥q∥=1(读者可以自证,用到 ∥ u ∥ = 1 \left \| u \right \|=1 ∥u∥=1),这样我们就能将旋转写成四元数的乘积了。到此为止,我们已经将旋转与四元数的积联系起来了,由此得到 w ⊥ ′ = q w ⊥ w^{'}_{\perp }=qw_{\perp} w⊥′=qw⊥也就是说,如果旋转轴 u u u的坐标为 u = [ u x u y u z ] u=\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix} u=⎣⎡uxuyuz⎦⎤,旋转角为 θ \theta θ,那么完成这一旋转所需要的四元数 q q q可以构造为: q = c o s ( θ ) + s i n ( θ ) u q = [ c o s ( θ ) , 0 ] + [ 0 , s i n ( θ ) u ] = [ c o s ( θ ) , s i n ( θ ) u ] = c o s ( θ ) + s i n ( θ ) u x i + s i n ( θ ) u y j + s i n ( θ ) u z k \begin{aligned}q &= cos(\theta)+sin(\theta)u_{q} \\ &= [cos(\theta),0]+[0, sin(\theta)u] \\ &= [cos(\theta), sin(\theta)u] \\ &= cos(\theta)+sin(\theta)u_{x}i+sin(\theta)u_{y}j+sin(\theta)u_{z}k\end{aligned} q=cos(θ)+sin(θ)uq=[cos(θ),0]+[0,sin(θ)u]=[cos(θ),sin(θ)u]=cos(θ)+sin(θ)uxi+sin(θ)uyj+sin(θ)uzk总结一下,得到定理3D旋转公式(正交四元数型):当 v ⊥ v_{\perp} v⊥正交于旋转轴 u u u时,旋转 θ \theta θ角度之后的 v ⊥ ′ v^{'}_{\perp} v⊥′可以使用 v ⊥ v_{\perp} v⊥的对应四元数与四元数 q q q相乘来获得。令 w ⊥ = [ 0 , v ⊥ ] , w ⊥ ′ = [ 0 , v ⊥ ′ ] , q = [ c o s ( θ ) , s i n ( θ ) u ] w_{\perp}=[0,v_{\perp}],w^{'}_{\perp }=[0,v^{'}_{\perp }],q=[cos(\theta), sin(\theta)u] w⊥=[0,v⊥],w⊥′=[0,v⊥′],q=[cos(θ),sin(θ)u],那么: w ⊥ ′ = q w ⊥ w^{'}_{\perp }=qw_{\perp} w⊥′=qw⊥
3.2.2 w ∥ w_{\parallel} w∥的旋转
接下来是平行于旋转轴的 w ∥ w_{\parallel} w∥。之前已经讨论过,如果一个向量 v ∥ v_{\parallel} v∥平行于 u u u,那么旋转 θ \theta θ后对它不会做出任何变换,由此得到定理3D旋转公式(平行四元数型):当 v ∥ v_{\parallel} v∥平行于旋转轴 u u u时,旋转 θ \theta θ角度之后的 v ∥ ′ v^{'}_{\parallel} v∥′用四元数可以写为: w ∥ ′ = w ∥ w^{'}_{\parallel}=w_{\parallel} w∥′=w∥
3.2.3 w w w的旋转
有了前面的结论,我们可以获得一般情况下 w ′ w^{'} w′的结果了: w ′ = w ∥ ′ + w ⊥ ′ = w ∥ + q w ⊥ \begin{aligned} w^{'} &= w^{'}_{\parallel}+w^{'}_{\perp} \\ &= w_{\parallel}+qw_{\perp}\end{aligned} w′=w∥′+w⊥′=w∥+qw⊥在进一步化简之前,引入三个引理:
- 四元数引理1 :如果 q = [ c o s ( θ ) , s i n ( θ ) u ] q = [cos(\theta), sin(\theta)u] q=[cos(θ),sin(θ)u],而且 u u u为单位向量,那么 q 2 = q q = [ c o s ( 2 θ ) , s i n ( 2 θ ) u ] q^{2}=qq=[cos(2\theta),sin(2\theta)u] q2=qq=[cos(2θ),sin(2θ)u]。
- 四元数引理2 :假设 w ∥ = [ 0 , v ∥ ] w_{\parallel}=[0,v_{\parallel}] w∥=[0,v∥]是一个纯四元数,而 q = [ α , β u ] q=[\alpha,\beta u] q=[α,βu],其中 u u u是一个单位向量, α , β ∈ R \alpha, \beta \in \mathbb{R} α,β∈R。在这种条件下,如果 v ∥ v_{\parallel} v∥平行于 u u u,那么 q w ∥ = w ∥ q qw_{\parallel}=w_{\parallel}q qw∥=w∥q。
- 四元数引理3 :假设 w ⊥ = [ 0 , v ⊥ ] w_{\perp}=[0,v_{\perp}] w⊥=[0,v⊥]是一个纯四元数,而 q = [ α , β u ] q=[\alpha,\beta u] q=[α,βu],其中 u u u是一个单位向量, α , β ∈ R \alpha, \beta \in \mathbb{R} α,β∈R。在这种条件下,如果 v ⊥ v_{\perp} v⊥垂直于 u u u,那么 q w ⊥ = w ⊥ q ∗ qw_{\perp}=w_{\perp}q^{*} qw⊥=w⊥q∗。
关于引理的证明,读者可以参考文献2,这里不再扩展。
为方便证明,再引入一个新的四元数
p
=
[
c
o
s
(
1
2
θ
)
,
s
i
n
(
1
2
θ
)
u
]
p=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u]
p=[cos(21θ),sin(21θ)u],
q
=
p
2
q=p^{2}
q=p2,
∥
p
∥
=
1
\left \| p \right \|=1
∥p∥=1。现在,我们就能对原本的旋转公式进行变形了:
w
′
=
w
∥
+
q
w
⊥
=
p
p
−
1
w
∥
+
p
p
w
⊥
=
p
p
∗
w
∥
+
p
p
w
⊥
=
p
w
∥
p
∗
+
p
w
⊥
p
∗
=
p
(
w
∥
+
w
⊥
)
p
∗
=
p
w
p
∗
\begin{aligned} w^{'} &= w_{\parallel}+qw_{\perp} \\ &= pp^{-1}w_{\parallel}+ppw_{\perp} \\ &= pp^{*}w_{\parallel}+ppw_{\perp} \\ &= pw_{\parallel}p^{*}+pw_{\perp}p^{*} \\ &=p(w_{\parallel}+w_{\perp})p^{*} \\ &= pw_{}p^{*}\end{aligned}
w′=w∥+qw⊥=pp−1w∥+ppw⊥=pp∗w∥+ppw⊥=pw∥p∗+pw⊥p∗=p(w∥+w⊥)p∗=pwp∗至此,我们就解开了四元数与3D旋转之间的谜题。3D空间中任意一个旋转都能够用三个四元数相乘的形式表达出来。
综上,我们可以总结出一个定理3D旋转公式(一般情况四元数型):任意向量
v
v
v沿着以单位向量定义的旋转轴
u
u
u旋转
θ
\theta
θ度之后的
v
′
v^{'}
v′,可以使用四元数乘法来获得。令
w
=
[
0
,
v
]
,
w
′
=
[
0
,
v
′
]
q
=
[
c
o
s
(
1
2
θ
)
,
s
i
n
(
1
2
θ
)
u
]
w=[0,v],w^{'}=[0,v^{'}]q=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u]
w=[0,v],w′=[0,v′]q=[cos(21θ),sin(21θ)u],那么:
w
′
=
q
w
q
∗
=
q
w
q
−
1
w^{'}=qwq^{*}=qwq^{-1}
w′=qwq∗=qwq−1这里用的是
1
2
θ
\frac{1}{2}\theta
21θ而不是
θ
\theta
θ。这是因为
q
q
q做的就是一个
1
2
θ
\frac{1}{2}\theta
21θ的旋转,而
q
∗
q^{*}
q∗或
q
−
1
q^{-1}
q−1也做了一个
1
2
θ
\frac{1}{2}\theta
21θ的旋转。我们进行了两次旋转,而不是一次,这两次旋转的结果是一个旋转角为
θ
\theta
θ的旋转。下图或可解释旋转的过程:
注释:对
w
′
=
q
w
q
∗
=
q
w
q
−
1
w^{'}=qwq^{*}=qwq^{-1}
w′=qwq∗=qwq−1,可以这样理解:对向量
v
v
v转化成的纯四元数
w
w
w,经过
q
q
q的旋转
q
w
qw
qw后,得到实部不为0的普通四元数,这时没办法映射到三维超平面(总不能转着圈就转到了四维空间吧),结果与最初的点不在同一三维空间。为了解决这个问题,先对第四维(角度)旋转一半,再用逆或共轭反转回来,这时正好将产生的第四维变为0,重新回到初始的三维超平面空间。这样不仅解决了奇点问题,也不会产生冗余数据,不知道
H
a
m
i
l
t
o
n
Hamilton
Hamilton怎么想到的,总之很佩服。
4. 四元数到其它旋转表示的相互转换
任意单位四元数描述了一个旋转,该旋转也可用旋转向量、旋转矩阵或欧拉角描述。现在来考察四元数与旋转向量、旋转矩阵以及欧拉角的相互转换关系。
4.1 旋转向量
本节介绍旋转向量与四元数之间的转换关系。
绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设已知等效旋转轴方向单位旋转向量
u
u
u的坐标为
u
=
[
u
x
u
y
u
z
]
u=\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix}
u=⎣⎡uxuyuz⎦⎤,旋转角为
θ
\theta
θ。根据3.2.3推导的定理3D旋转公式(一般情况四元数型),设其等效的单位四元数为
q
=
[
q
0
,
q
1
,
q
2
,
q
3
]
q=[q_{0},q_{1},q_{2},q_{3}]
q=[q0,q1,q2,q3],则有:
{
q
0
=
c
o
s
(
1
2
θ
)
q
1
=
u
x
s
i
n
(
1
2
θ
)
q
2
=
u
y
s
i
n
(
1
2
θ
)
q
3
=
u
z
s
i
n
(
1
2
θ
)
(4.1)
\left\{\begin{matrix} q_{0}=cos(\frac{1}{2}\theta )\\ q_{1}=u_{x}sin(\frac{1}{2}\theta )\\ q_{2}=u_{y}sin(\frac{1}{2}\theta )\\ q_{3}=u_{z}sin(\frac{1}{2}\theta ) \end{matrix}\right.\tag{4.1}
⎩⎪⎪⎨⎪⎪⎧q0=cos(21θ)q1=uxsin(21θ)q2=uysin(21θ)q3=uzsin(21θ)(4.1)
同理,当已知单位四元数
q
=
[
q
0
,
q
1
,
q
2
,
q
3
]
q=[q_{0},q_{1},q_{2},q_{3}]
q=[q0,q1,q2,q3],其对应的旋转角
θ
\theta
θ和旋转向量
u
u
u为:
{
θ
=
2
arccos
q
0
[
u
x
,
u
y
,
u
z
]
T
=
[
q
1
,
q
2
,
q
3
]
T
/
sin
(
2
θ
)
(4.2)
\left\{\begin{matrix} \theta=2\arccos q_{0}\\ [u_{x},u_{y},u_{z}]^{T}=[q_{1},q_{2},q_{3}]^{T}/\sin(\frac{2}{\theta }) \end{matrix}\right.\tag{4.2}
{θ=2arccosq0[ux,uy,uz]T=[q1,q2,q3]T/sin(θ2)(4.2)
4.2 旋转矩阵
已知单位四元数
q
q
q,根据博文《三维空间刚体运动2:旋转向量与罗德里格斯公式》中的罗德里格斯公式展开式(2.3)及本文公式(4.2),将单位旋转向量
u
u
u及旋转角
θ
\theta
θ替换为单位四元数
q
q
q,省去计算过程,得到单位四元数到旋转矩阵的转换公式为:
R
=
[
1
−
2
q
2
2
−
2
q
3
2
2
(
q
1
q
2
−
q
0
q
3
)
2
(
q
1
q
3
+
q
0
q
2
)
2
(
q
1
q
2
+
q
0
q
3
)
1
−
2
q
1
2
−
2
q
3
2
2
(
q
2
q
3
−
q
0
q
1
)
2
(
q
1
q
3
−
q
0
q
2
)
2
(
q
2
q
3
+
q
0
q
1
)
1
−
2
q
1
2
−
2
q
2
2
]
(4.3)
R=\begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} & 2(q_{1}q_{2}-q_{0}q_{3}) & 2(q_{1}q_{3}+q_{0}q_{2})\\ 2(q_{1}q_{2}+q_{0}q_{3}) & 1-2q_{1}^{2}-2q_{3}^{2} & 2(q_{2}q_{3}-q_{0}q_{1})\\ 2(q_{1}q_{3}-q_{0}q_{2}) & 2(q_{2}q_{3}+q_{0}q_{1}) & 1-2q_{1}^{2}-2q_{2}^{2} \end{bmatrix}\tag{4.3}
R=⎣⎡1−2q22−2q322(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2q12−2q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2q12−2q22⎦⎤(4.3)
假设已知变换矩阵:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix}
R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤根据公式(4.3),推导对应的单位四元数为:
{
q
0
=
1
2
1
+
r
11
+
r
22
+
r
33
q
1
=
r
32
−
r
23
4
q
0
q
2
=
r
13
−
r
31
4
q
0
q
3
=
r
21
−
r
12
4
q
0
(4.4)
\left\{\begin{matrix} q_{0}=\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\\ q_{1}=\frac{r_{32}-r_{23}}{4q_{0}}\\ q_{2}=\frac{r_{13}-r_{31}}{4q_{0}}\\ q_{3}=\frac{r_{21}-r_{12}}{4q_{0}} \end{matrix}\right.\tag{4.4}
⎩⎪⎪⎨⎪⎪⎧q0=211+r11+r22+r33q1=4q0r32−r23q2=4q0r13−r31q3=4q0r21−r12(4.4)
4.3 欧拉角
4.3.1 转换关系
那么将Z-Y-X欧拉角(或RPY角:绕固定坐标系的X-Y-Z依次旋转α,β,γ角)转换为四元数:
q
=
[
cos
γ
2
0
0
sin
γ
2
]
[
cos
β
2
0
sin
β
2
0
]
[
cos
α
2
sin
α
2
0
0
]
=
[
cos
α
2
cos
β
2
cos
γ
2
+
sin
α
2
sin
β
2
sin
γ
2
sin
α
2
cos
β
2
cos
γ
2
−
cos
α
2
sin
β
2
sin
γ
2
cos
α
2
sin
β
2
cos
γ
2
+
sin
α
2
cos
β
2
sin
γ
2
cos
α
2
cos
β
2
sin
γ
2
−
sin
α
2
sin
β
2
cos
γ
2
]
q=\begin{bmatrix} \cos\frac{\gamma}{2}\\ 0\\ 0\\ \sin\frac{\gamma}{2} \end{bmatrix} \begin{bmatrix} \cos\frac{\beta}{2}\\ 0\\ \sin\frac{\beta}{2}\\ 0 \end{bmatrix} \begin{bmatrix} \cos\frac{\alpha}{2}\\ \sin\frac{\alpha}{2}\\ 0\\ 0 \end{bmatrix} =\begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}+\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix}
q=⎣⎢⎢⎡cos2γ00sin2γ⎦⎥⎥⎤⎣⎢⎢⎡cos2β0sin2β0⎦⎥⎥⎤⎣⎢⎢⎡cos2αsin2α00⎦⎥⎥⎤=⎣⎢⎢⎡cos2αcos2βcos2γ+sin2αsin2βsin2γsin2αcos2βcos2γ−cos2αsin2βsin2γcos2αsin2βcos2γ+sin2αcos2βsin2γcos2αcos2βsin2γ−sin2αsin2βcos2γ⎦⎥⎥⎤
根据上面的公式可以求出逆解,即由四元数
q
=
(
q
0
,
q
1
,
q
2
,
q
3
)
q=(q_{0},q_{1},q_{2},q_{3})
q=(q0,q1,q2,q3)到欧拉角的转换为:
[
α
β
γ
]
=
[
a
t
a
n
2
(
2
(
q
0
q
1
+
q
2
q
3
)
,
1
−
2
(
q
1
2
+
q
2
2
)
)
arcsin
(
2
(
q
0
q
2
−
q
1
q
3
)
a
t
a
n
2
(
2
(
q
0
q
3
+
q
1
q
2
)
,
1
−
2
(
q
2
2
+
q
3
2
)
)
]
\begin{bmatrix} \alpha\\ \beta\\ \gamma \end{bmatrix} =\begin{bmatrix} atan2(2(q_{0}q_{1}+q_{2}q_{3}),1-2(q^{2}_{1}+q^{2}_{2}))\\ \arcsin (2(q_{0}q_{2}-q_{1}q_{3})\\ atan2(2(q_{0}q_{3}+q_{1}q_{2}),1-2(q^{2}_{2}+q^{2}_{3})) \end{bmatrix}
⎣⎡αβγ⎦⎤=⎣⎡atan2(2(q0q1+q2q3),1−2(q12+q22))arcsin(2(q0q2−q1q3)atan2(2(q0q3+q1q2),1−2(q22+q32))⎦⎤这里使用了
a
t
a
n
2
(
y
,
x
)
atan2(y,x)
atan2(y,x)而不是
a
r
c
t
a
n
(
y
x
)
arctan(\frac{y}{x})
arctan(xy),因为
a
r
c
t
a
n
(
y
x
)
arctan(\frac{y}{x})
arctan(xy)的取值范围为
[
−
π
2
,
π
2
]
[-\frac{\pi}{2},\frac{\pi}{2}]
[−2π,2π],只有
180
°
180°
180°,而绕某个轴旋转时范围是
360
°
360°
360°,
a
t
a
n
2
(
y
,
x
)
atan2(y,x)
atan2(y,x)正好满足需求。
a
t
a
n
2
(
y
,
x
)
atan2(y,x)
atan2(y,x)是一个函数,在C语言里返回的是指方位角,函数原型为
d
o
u
b
l
e
a
t
a
n
2
(
d
o
u
b
l
e
y
,
d
o
u
b
l
e
x
)
double \space atan2(double y, double x)
double atan2(doubley,doublex) ,返回以弧度表示的
y
/
x
y/x
y/x 的反正切。y 和 x 的值的符号决定了正确的象限。也可以理解为计算复数
x
+
y
i
x+yi
x+yi 的辐角,计算时atan2 比 atan 稳定。
4.3.2 转换中的万象锁问题
另外需要注意的就是奇异性问题,即万向锁,下面分析这种情况。当刚体绕Y轴旋转了
90
°
90°
90°(俯仰角pitch=90)时,如何计算横滚角roll和偏航角yaw?这时会发生自由度丢失的情况,即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题:
arcsin
(
2
(
q
0
q
2
−
q
1
q
3
)
\arcsin (2(q_{0}q_{2}-q_{1}q_{3})
arcsin(2(q0q2−q1q3)的定义域为
[
−
1
,
1
]
[-1,1]
[−1,1],当
2
(
q
0
q
2
−
q
1
q
3
)
=
±
0.5
2(q_{0}q_{2}-q_{1}q_{3})=\pm 0.5
2(q0q2−q1q3)=±0.5时(在程序中浮点数不能直接进行等于判断,要使用合理的阈值),俯仰角
β
\beta
β为
±
90
°
\pm 90°
±90°,将其带入正向公式计算出四元数
(
q
0
,
q
1
,
q
2
,
q
3
)
(q_{0},q_{1},q_{2},q_{3})
(q0,q1,q2,q3),然后可以发现逆向公式中atan2函数中的参数全部为0,即出现了
0
0
\frac{0}{0}
00的情况!无法计算。
当
β
=
90
°
\beta=90°
β=90°时,
sin
β
2
=
cos
β
2
=
0.707
\sin \frac{\beta}{2}=\cos \frac{\beta}{2}=0.707
sin2β=cos2β=0.707,将其带入公式中有:
q
=
[
q
0
q
1
q
2
q
3
]
=
[
0.707
(
cos
α
2
cos
γ
2
+
sin
α
2
sin
γ
2
)
0.707
(
sin
α
2
cos
γ
2
−
cos
α
2
sin
γ
2
)
0.707
(
cos
α
2
cos
γ
2
+
sin
α
2
sin
γ
2
)
0.707
(
cos
α
2
sin
γ
2
−
sin
α
2
cos
γ
2
)
]
=
[
0.707
cos
α
−
γ
2
0.707
sin
α
−
γ
2
0.707
cos
α
−
γ
2
−
0.707
sin
α
−
γ
2
]
q=\begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix} =\begin{bmatrix} 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin \frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\sin\frac{\alpha}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}+\sin \frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\sin\frac{\gamma}{2}-\sin \frac{\alpha}{2}\cos\frac{\gamma}{2}) \end{bmatrix} =\begin{bmatrix} 0.707\cos\frac{\alpha - \gamma}{2}\\ 0.707\sin \frac{\alpha - \gamma}{2}\\ 0.707\cos \frac{\alpha - \gamma}{2}\\ -0.707\sin \frac{\alpha - \gamma}{2} \end{bmatrix}
q=⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤=⎣⎢⎢⎡0.707(cos2αcos2γ+sin2αsin2γ)0.707(sin2αcos2γ−cos2αsin2γ)0.707(cos2αcos2γ+sin2αsin2γ)0.707(cos2αsin2γ−sin2αcos2γ)⎦⎥⎥⎤=⎣⎢⎢⎡0.707cos2α−γ0.707sin2α−γ0.707cos2α−γ−0.707sin2α−γ⎦⎥⎥⎤则
q
1
q
0
=
−
q
3
q
2
=
tan
α
−
γ
2
\frac{q_{1}}{q_{0}}=-\frac{q_{3}}{q_{2}}=\tan\frac{\alpha - \gamma}{2}
q0q1=−q2q3=tan2α−γ,于是有:
α
−
γ
=
2
a
t
a
n
2
(
q
1
,
q
0
)
\alpha - \gamma = 2atan2(q_{1},q_{0})
α−γ=2atan2(q1,q0)通常令
α
=
0
\alpha=0
α=0,这时
γ
=
−
2
a
t
a
n
2
(
q
1
,
q
0
)
\gamma = -2atan2(q_{1},q_{0})
γ=−2atan2(q1,q0)。当俯仰角
β
\beta
β为-90°,即刚体竖直向下时的情况也与之类似,可以推导出奇异姿态时的计算公式。
将四元数转换为欧拉角的代码可以参考附件。需要注意欧拉角有12种旋转次序,而上面推导的公式是按照Z-Y-X顺序进行的,所以有时会在网上看到不同的转换公式(因为对应着不同的旋转次序),在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序,即Z-X-Z欧拉角,而VREP中则按照X-Y-Z欧拉角旋转。另外万向锁问题代码的Z-Y-X欧拉角代码见类CameraSpacePoint。
5. 四元数的其他性质
为更全面理解四元数和方便引入slerp插值,这一节补充四元数的其它性质:旋转复合、双倍覆盖和指数形式。
5.1 旋转的复合
旋转的复合其实在我们之前证明
q
2
=
q
q
=
[
c
o
s
(
2
θ
)
,
s
i
n
(
2
θ
)
u
]
q^{2}=qq=[cos(2\theta),sin(2\theta)u]
q2=qq=[cos(2θ),sin(2θ)u]的时候就已经涉及到一点了,但是这里我们考虑的是更一般的情况。
假设有两个表示沿着不同轴、不同角度旋转的四元数
q
1
、
q
2
q_{1}、q_{2}
q1、q2。首先,我们实施
q
1
q_{1}
q1的变换,变换之后的
v
′
v^{'}
v′为
v
′
=
q
1
v
q
1
∗
v^{'}=q_{1}vq^{*}_{1}
v′=q1vq1∗接下来,对
v
′
v^{'}
v′进行
q
2
q_{2}
q2的变换,得到
v
′
′
v^{''}
v′′:
v
′
′
=
q
2
v
′
q
2
∗
=
q
2
q
1
v
q
1
∗
q
2
∗
=
q
n
e
t
v
q
n
e
t
∗
v^{''}=q_{2}v^{'}q^{*}_{2}=q_{2}q_{1}vq^{*}_{1}q^{*}_{2}=q_{net}vq^{*}_{net}
v′′=q2v′q2∗=q2q1vq1∗q2∗=qnetvqnet∗其中
q
n
e
t
=
q
2
q
1
q_{net}=q_{2}q_{1}
qnet=q2q1,另外写成上面的形式,还用到性质
q
1
∗
q
2
∗
=
(
q
2
q
1
)
∗
q^{*}_{1}q^{*}_{2}=(q_{2}q_{1})^{*}
q1∗q2∗=(q2q1)∗。
注意四元数乘法的顺序,这和矩阵、复数的复合非常相似,都是从右向左叠加。另外要注意的是,
q
1
q_{1}
q1与
q
2
q_{2}
q2的等价旋转
q
n
e
t
q_{net}
qnet并不是沿着
q
1
q_{1}
q1与
q
2
q_{2}
q2的两个旋转轴进行的两次旋转,它是沿着一个全新的旋转轴进行的一个等价旋转,仅仅只有旋转的结果相同。
5.2 双倍覆盖
四元数与 3D 旋转的关系并不是一对一的,同一个 3D 旋转可以使用两个不同的四元数来表示.对任意的单位四元数
q
=
[
c
o
s
(
1
2
θ
)
,
s
i
n
(
1
2
θ
)
u
]
q=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u]
q=[cos(21θ),sin(21θ)u],
q
q
q与
−
q
−q
−q代表的是同一个旋转。如果
q
q
q表示的是沿着旋转轴
u
u
u旋转
θ
θ
θ度,那么
−
q
−q
−q代表的是沿着相反的旋转轴
−
u
−u
−u旋转
(
2
π
−
θ
)
(2π − θ)
(2π−θ),负负得正:
−
q
=
[
−
c
o
s
(
1
2
θ
)
,
−
s
i
n
(
1
2
θ
)
u
]
=
[
c
o
s
(
π
−
1
2
θ
)
,
s
i
n
(
π
−
1
2
θ
)
(
−
u
)
]
\begin{aligned} -q &= [-cos(\frac{1}{2}\theta),-sin(\frac{1}{2}\theta)u] \\ &= [cos(\pi - \frac{1}{2}\theta),sin(\pi - \frac{1}{2}\theta)(-u)]\end{aligned}
−q=[−cos(21θ),−sin(21θ)u]=[cos(π−21θ),sin(π−21θ)(−u)]所以,这个四元数旋转的角度为
2
(
π
−
1
2
θ
)
=
2
π
−
θ
2(\pi - \frac{1}{2}\theta)=2\pi-\theta
2(π−21θ)=2π−θ,从下面的图中我们可以看到,这两个旋转是完全等价的:
其实从四元数的旋转公式中也能推导出相同的结果:
(
−
q
)
v
(
−
q
)
∗
=
(
−
1
)
2
q
v
q
∗
=
q
v
q
∗
(-q)v(-q)^{*}=(-1)^{2}qvq^{*}=qvq^{*}
(−q)v(−q)∗=(−1)2qvq∗=qvq∗所以,我们经常说单位四元数与3D旋转有一个「2对1满射同态」(2-1 Surjective Homomorphism) 关系,或者说单位四元数双倍覆盖(Double Cover)了3D旋转。它的严格证明会用到一些李群的知识,这里不再拓展。
有一点需要注意的是,虽然
q
q
q与
−
q
−q
−q是两个不同的四元数,但是由于旋转矩阵中的每一项都包含了四元数两个分量的乘积,它们的旋转矩阵是完全相同的,即旋转矩阵并不会出现双倍覆盖的问题。
5.3 指数形式
在讨论复数的时候,我们利用欧拉公式将2D的旋转写成了
v
′
=
e
i
θ
v
v^{'}=e^{i\theta v}
v′=eiθv这样的指数形式。实际上,我们也可以利用四元数将 3D 旋转写成类似的形式。
类似于复数的欧拉公式,四元数也有一个类似的公式。如果
u
u
u是一个单位向量,那么对于单位四元数
u
q
=
[
0
,
u
]
u_{q}= [0, u]
uq=[0,u],有:
e
u
θ
=
c
o
s
(
θ
)
+
u
q
s
i
n
(
θ
)
=
c
o
s
(
θ
)
+
u
s
i
n
(
θ
)
e^{u\theta}=cos(\theta)+u_{q}sin(\theta)=cos(\theta)+usin(\theta)
euθ=cos(θ)+uqsin(θ)=cos(θ)+usin(θ)这也就是说,
q
=
[
cos
(
θ
)
,
sin
(
θ
)
u
]
q = [\cos(\theta), \sin(\theta)u]
q=[cos(θ),sin(θ)u]可以使用指数表示为
e
u
θ
e^{u\theta}
euθ。这个公式的证明与欧拉公式的证明非常类似,直接使用级数展开就可以了,这里不再扩展。
注意,因为
u
u
u是一个单位向量,
u
2
=
[
−
u
⋅
u
,
0
]
=
−
∥
u
∥
2
=
−
1
u^{2}= [−u · u, 0] = −∥u∥^{2}= −1
u2=[−u⋅u,0]=−∥u∥2=−1.这与欧拉公式中的
i
i
i是非常类似的。有了指数型的表示方式,我们就能够将之前四元数的旋转公式改写为指数形式了,由此可得到定义:
3D旋转公式(指数型):任意向量
v
v
v沿着以单位向量定义的旋转轴
u
u
u旋转
θ
θ
θ角度之后的
v
′
v^{'}
v′可以使用四元数的指数表示。令
w
=
[
0
,
v
]
,
u
q
=
[
0
,
u
]
w= [0, v], u_{q}= [0, u]
w=[0,v],uq=[0,u],那么:
w
′
=
e
u
q
θ
2
v
e
−
u
q
θ
2
w^{'}=e^{u_{q}\frac{\theta}{2}}ve^{-u_{q}\frac{\theta}{2}}
w′=euq2θve−uq2θ有了四元数的指数定义,我们就能够定义四元数的更多运算了。首先是自然对数
l
o
g
log
log,对任意单位四元数
q
=
[
c
o
s
(
θ
)
,
s
i
n
(
θ
)
u
]
=
e
u
q
θ
q = [cos(θ), sin(θ)u]=e^{u_{q}\theta}
q=[cos(θ),sin(θ)u]=euqθ:
l
o
g
(
q
)
=
l
o
g
(
e
u
q
θ
)
=
[
0
,
u
θ
]
log(q)=log(e^{u_{q}\theta})=[0,{u\theta}]
log(q)=log(euqθ)=[0,uθ]接下来是单位四元数的幂运算:
q
t
=
(
e
u
q
θ
)
t
=
e
u
q
(
t
θ
)
=
[
c
o
s
(
t
θ
)
,
s
i
n
(
t
θ
)
u
]
q^{t}=(e^{u_{q}\theta})^{t}=e^{u_{q}(t\theta)}=[cos(tθ), sin(tθ)u]
qt=(euqθ)t=euq(tθ)=[cos(tθ),sin(tθ)u]可以看到,一个单位四元数的
t
t
t次幂等同于将它的旋转角度缩放至
t
t
t倍,并且不会改变它的旋转轴(
u
u
u必须是单位向量,所以一般不能与
t
t
t结合)。这些运算会在之后讨论四元数插值时非常有用。限于篇幅,四元数插值的讲解见下一篇博客《三维空间刚体运动4-2:四元数插值(lerp,Nlerp,Slerp,Squad等)》
5.4 幂函数性质
上面给出四元数的指数形式后,可推导出一个重要的指函数性质,这一性质下一篇将用到。首先看推论:
四元数推论1 :设
q
∈
H
1
,
p
=
[
a
,
b
v
]
q\in \mathbb{H}_{1},p=[a,b\mathbf{v}]
q∈H1,p=[a,bv],其中
a
,
b
,
r
∈
R
,
v
∈
R
3
a,b,r\in \mathbb{R},\mathbf{v}\in\mathbb{R}^{3}
a,b,r∈R,v∈R3,如果
q
[
r
,
v
]
q
∗
=
[
r
,
v
′
]
q[r,\mathbf{v}]q^{*}=[r,\mathbf{v^{'}}]
q[r,v]q∗=[r,v′],那么
q
[
a
,
b
v
]
q
∗
=
[
a
,
b
v
′
]
q[a,b\mathbf{v}]q^{*}=[a,b\mathbf{v^{'}}]
q[a,bv]q∗=[a,bv′]。
推论证明:
q
p
q
∗
=
q
[
a
,
b
v
]
q
∗
=
q
b
[
a
b
,
v
]
q
∗
=
b
[
a
b
,
v
′
]
=
[
a
,
b
v
′
]
\begin{aligned}qpq^{*}&=q[a,b\mathbf{v}]q^{*}\\&=qb[\frac{a}{b},\mathbf{v}]q^{*}\\&=b[\frac{a}{b},\mathbf{v^{'}}]\\&=[a,b\mathbf{v^{'}}]\end{aligned}
qpq∗=q[a,bv]q∗=qb[ba,v]q∗=b[ba,v′]=[a,bv′]根据推论,得出以下定理:
四元数定理2 :设
q
,
p
∈
H
1
,
p
=
[
cos
θ
,
sin
θ
v
]
,
t
∈
R
q,p\in \mathbb{H}_{1},p=[\cos\theta,\sin\theta\mathbf{v}],t\in\mathbb{R}
q,p∈H1,p=[cosθ,sinθv],t∈R,那么
q
p
t
q
∗
=
(
q
p
q
∗
)
t
qp^{t}q^{*}=(qpq^{*})^{t}
qptq∗=(qpq∗)t。
定理证明:
根据推论,存在
v
′
∈
R
3
\mathbf{v^{'}}\in\mathbb{R}^{3}
v′∈R3,使得
q
[
cos
θ
,
sin
θ
v
]
q
∗
=
[
cos
θ
,
sin
θ
v
′
]
q[\cos\theta,\sin\theta\mathbf{v}]q^{*}=[\cos\theta,\sin\theta\mathbf{v^{'}}]
q[cosθ,sinθv]q∗=[cosθ,sinθv′],因此得到:
q
p
t
q
∗
=
q
(
exp
(
t
log
p
)
)
q
∗
=
q
(
exp
(
t
[
0
,
θ
v
]
)
)
q
∗
=
q
(
exp
[
0
,
t
θ
v
]
)
q
∗
=
q
(
[
cos
t
θ
,
sin
t
θ
v
]
)
q
∗
=
[
cos
t
θ
,
sin
t
θ
v
′
]
=
exp
(
t
[
0
,
θ
v
′
]
)
=
exp
(
t
log
[
cos
θ
,
sin
θ
v
′
]
)
=
exp
(
t
log
(
q
p
q
∗
)
)
=
(
q
p
q
∗
)
t
\begin{aligned} qp^{t}q^{*} &=q(\exp(t\log p))q^{*} \\&= q(\exp(t[0,\theta\mathbf{v}]))q^{*} \\&= q(\exp[0,t\theta\mathbf{v}])q^{*} \\&= q([\cos t\theta,\sin t\theta\mathbf{v}])q^{*} \\ &= [\cos t\theta,\sin t\theta\mathbf{v^{'}}] \\&= \exp(t[0,\theta\mathbf{v^{'}}]) \\&= \exp(t\log[\cos \theta,\sin \theta\mathbf{v^{'}}]) \\&= \exp(t\log(qpq^{*})) \\&= (qpq^{*})^{t}\end{aligned}
qptq∗=q(exp(tlogp))q∗=q(exp(t[0,θv]))q∗=q(exp[0,tθv])q∗=q([costθ,sintθv])q∗=[costθ,sintθv′]=exp(t[0,θv′])=exp(tlog[cosθ,sinθv′])=exp(tlog(qpq∗))=(qpq∗)t
至此,四元数表示旋转的理论部分讲解完了。
6. 实践
现在,我们通过两个小程序实际演练四元数的运算。其中四元数的基础运算和高阶运算程序实现放在下一讲Slerp中。
6.1 四元数旋转运算
第一个小程序是演示四元数的常规运算,包括与旋转矩阵、旋转向量的转换,以及用四元数旋转一个向量,如下所示:
#include<iostream>
#include<cmath>
using namespace std;
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
using namespace Eigen;
int main(int argc, char **argv){
//Eigen/Geometry模块提供了各种旋转和平移的表示,3D旋转矩阵直接使用Matrix3d或Matrix3f
Matrix3d rotation_matrix = Matrix3d::Identity();
//旋转向量使用AngleAxis,它底层不直接是Matrix,但运算可以当做矩阵,因为重载了运算符
AngleAxisd rotation_vector(M_PI/4, Vector3d(0, 0, 1));
//设置输出精度
cout.precision(3);
cout<<"rotation matrix =\n"<<rotation_matrix<<endl;
cout<<"rotation vector =\n"<<rotation_vector.matrix()<<endl;
//旋转向量转换的矩阵可以直接赋值给旋转矩阵
rotation_matrix = rotation_vector.toRotationMatrix();
cout<<"rotation vector to Matrix =\n"<<rotation_matrix<<endl;
//旋转向量
Vector3d v(1, 0, 0);
cout<<"v = "<<v.transpose()<<endl;
//四元数,可以直接把AngleAxis赋值给四元数,反之亦然
Quaterniond q = Quaterniond(rotation_vector);
//coeffs:多项式系数(coefficients),其顺序为(x,y,z,w),w为实部,xyz为虚部
cout<<"quaternion from rotation vector = "<<q.coeffs().transpose()<<endl;
//也可以直接把旋转矩阵赋给它
q = Quaterniond(rotation_matrix);
cout<<"quaternion from rotation matrix = "<<q.coeffs().transpose()<<endl;
//使用四元数旋转一个向量,使用重载的乘法即可
//注意q*v在数学上是qvq^{-1}
v_rotated = q*v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
//用常规向量乘法表示qvq^{-1},则计算如下:
Quaterniond q_rotate_v = q*Quaterniond(0,1,0,0)*q.inverse();
cout<<"should be equal to "<<q_rotate_v.coeffs().transpose()<<endl;
return 0;
输出结果如下:
rotation matrix =
1 0 0
0 1 0
0 0 1
rotation vector =
0.707 -0.707 0
0.707 0.707 0
0 0 1
rotation vector to Matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
v = 1 0 0
v transofrmed = 1.71 3.71 4
quaternion from rotation vector = 0 0 0.383 0.924
quaternion from rotation matrix = 0 0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707 0
should be equal to 0.707 0.707 0 0
请读者注意,程序代码通常和数学表示有一些细微的差别。例如,通过运算符重载,四元数和三维向量可以直接计算乘法,但在数学上则需要先把向量转成虚四元数,再利用四元数乘法进行计算,同样的情况也适用于变换矩阵乘三维向量的情况。总体而言,程序中的用法会比数学公式更灵活。
6.2 坐标变换
下面我们举一个小例子来演示坐标变换。
例子:设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为
W
W
W,小萝卜们的坐标系分别为
R
1
R_{1}
R1和
R
2
R_{2}
R2。小萝卜一号的位姿为
q
1
=
[
0.35
,
0.2
,
0.3
,
0.1
]
T
q_{1}=[0.35,0.2,0.3,0.1]^{T}
q1=[0.35,0.2,0.3,0.1]T,
t
1
=
[
0.3
,
0.1
,
0.1
]
T
t_{1}=[0.3,0.1,0.1]^{T}
t1=[0.3,0.1,0.1]T。小萝卜二号的位姿为
q
2
=
[
−
0.5
,
0.4
,
−
0.1
,
0.2
]
T
q_{2}=[-0.5,0.4,-0.1,0.2]^{T}
q2=[−0.5,0.4,−0.1,0.2]T,
t
2
=
[
−
0.1
,
0.5
,
0.3
]
T
t_{2}=[-0.1,0.5,0.3]^{T}
t2=[−0.1,0.5,0.3]T。这里的
q
q
q和
t
t
t表达的是
T
R
k
,
W
,
k
=
1
,
2
T_{R_{k},W},k=1,2
TRk,W,k=1,2,也就是世界坐标系到相机坐标系的变换关系。现在,小萝卜一号看到某个点在自身的坐标系下坐标为
p
R
1
=
[
0.5
,
0
,
0.2
]
T
p_{R_{1}}=[0.5,0,0.2]^{T}
pR1=[0.5,0,0.2]T,求该向量在小萝卜二号坐标系下的坐标。
这是一个非常简单,但又具有代表性的例子。在实际场景中你经常需要在同一个机器人的不同部分,或者不同机器人之间转换坐标。计算过程也很简单,只需计算:
p
R
2
=
T
R
2
,
W
T
W
,
R
1
p
R
1
p_{R_{2}}=T_{R_{2},W}T_{W,R_{1}}p_{R_{1}}
pR2=TR2,WTW,R1pR1下面请看程序:
#include<iostream>
#include<vector>
#include<algorithm>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc, char** argv){
//位姿四元数q1,q2
Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
cout<<"q1.coeffs=\n"<<q1.coeffs()<<endl;
cout<<"q2.coeffs=\n"<<q2.coeffs()<<endl;
//四元数转换为旋转矩阵
cout<<"q1.matrix=\n"<<q1.matrix()<<endl;
cout<<"q2.matrix=\n"<<q2.matrix()<<endl;
//归一化,转换为单位四元数
q1.normalize();
q2.normalize();
cout<<"q1.normalize="<<q1.matrix()<<endl;
cout<<"q2.normalize="<<q2.matrix()<<endl;
//位移向量t1,t2,小萝卜一号下的坐标pr1
Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
Vector3d pr1(0.5, 0, 0.2);
//Eigen::Isometry3d:欧式变换矩阵4*4
Isometry3d Twr1(q1), Tr2w(q2);
cout<<"Twr1.matrix="<<Twr1.matrix()<<endl;
cout<<"Tr2w.matrix="<<Tr2w.matrix()<<endl;
//坐标转换,计算T=Ra+t
Twr1.pretranslate(t1);
Tr2w.pretranslate(t2);
//先将pr1转换为世界坐标系,然后转换为小萝卜二号下的坐标pr2
Vector3d pr2 = Tr2w * Twr1.inverse()*pr1;
cout<<"pr2.transpose="<<pr2.transpose()<<endl;
return 0;
}
输出结果如下:
q1.coeffs=
0.2
0.3
0.1
0.35
q2.coeffs=
0.4
-0.1
0.2
-0.5
q1.matrix=
0.8 0.05 0.25
0.19 0.9 -0.08
-0.17 0.2 0.74
q2.matrix=
0.9 0.12 0.26
-0.28 0.6 0.36
0.06 -0.44 0.66
q1.normalize=
0.238095 0.190476 0.952381
0.72381 0.619048 -0.304762
-0.647619 0.761905 0.00952381
q2.normalize=
0.782609 0.26087 0.565217
-0.608696 0.130435 0.782609
0.130435 -0.956522 0.26087
Twr1.matrix=
0.238095 0.190476 0.952381 0
0.72381 0.619048 -0.304762 0
-0.647619 0.761905 0.00952381 0
0 0 0 1
Tr2w.matrix=
0.782609 0.26087 0.565217 0
-0.608696 0.130435 0.782609 0
0.130435 -0.956522 0.26087 0
0 0 0 1
pr2.transpose=
-0.0309731 0.73499 0.296108
四元数的第一部分讲解到此。
本文基于《视觉SLAM十四讲:从理论到实践》和《Quaternions, Interpolation and Animation》编写,但相对于原文会适当精简,同时为便于全面理解,会收集其他网络好文,根据作者理解,加入一些注解和扩展知识点,如果您觉得还不错,请一键四连(点赞关注收藏评论),让更多的人看到。
参考文献:
- 《视觉SLAM十四讲:从理论到实践》,高翔、张涛等著,中国工信出版社
- Quaternions, Interpolation and Animation
- 四元数与三维旋转
- 四元数与欧拉角(RPY角)的相互转换
- 如何形象地理解四元数?