旋轉矩陣用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找到的一種擴充套件的複數。它既是緊湊的,也沒有奇異性。如果說缺點,四元數不夠直觀,其運算稍複雜些。
把四元數與複數類比可以幫助你更快地理解四元數。例如,當我們想要將複平面的向量旋轉
θ
\theta
θ角時,可以給這個復向量乘以
e
i
θ
e^{i\theta}
eiθ,這是極座標表示的複數,它也可以寫成普通的形式,只要用尤拉公式即可:
e
i
θ
=
c
o
s
θ
+
s
i
n
θ
.
e^{i\theta} = cos\theta + sin\theta.
eiθ=cosθ+sinθ.
尤拉公式將指數函數的定義域擴大到了複數域,建立和三角函數和指數函數的關係,被譽為「數學中的天橋」。尤拉公式的簡單推導如下,
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
θ
+
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 + sin\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θ+sinθ.當
θ
=
π
\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,這五個重要的數學元素全部被包含在內,在數學愛好者眼裡,彷彿一行詩道盡了數學的美好。尤拉公式的詳細說明可參見《尤拉公式之美》。
尤拉公式的右側正是一個單位長度的複數,所以在二維情況下,旋轉可以有單位複數來描述。類似的,可以看到,三維旋轉可以有單位四元數來描述。
四元數的定義和複數非常類似,唯一的區別就是四元數有三個虛部,而複數只有一個。所有的四元數
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∘後得到一個相反的東西,也就是要旋轉兩週才會和它原先的樣子相等。
下面我們看一下四元數之間的運演演算法則。
四元數和複數一樣,可以進行一系列的運算。常見的有四則運算、共軛、求逆、數乘等,下面分別介紹。
現有兩個四元數
q
a
,
q
b
q_{a},q_{b}
qa,qb,它們的向量表示為
[
s
a
,
v
a
]
T
[s_{a}, v_{a}]^{T}
[sa,va]T,
[
s
b
,
v
b
]
T
[s_{b}, v_{b}]^{T}
[sb,vb]T,其中
v
a
=
x
a
i
+
y
a
j
+
z
a
k
,
v
b
=
x
b
i
+
y
b
j
+
z
b
k
v_{a}=x_{a}i+y_{a}j+z_{a}k,v_{b}=x_{b}i+y_{b}j+z_{b}k
va=xai+yaj+zak,vb=xbi+ybj+zbk,或者原始四元數表示為:
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_{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.
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]
乘法
乘法是把
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
∥
=
s
a
2
+
x
a
2
+
y
a
2
+
z
a
2
\left \| q_{a} \right \|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}}
∥qa∥=sa2+xa2+ya2+za2可以驗證,兩個四元數乘積的模即模的乘積,這使得單位四元數相乘後仍是單位四元數:
∥
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
a
∗
=
s
a
−
x
a
i
−
y
a
j
−
z
a
k
=
[
s
a
,
−
v
a
]
.
q_{a}^{*}=s_{a}-x_{a}i-y_{a}j-z_{a}k=[s_{a}, -v_{a}].
qa∗=sa−xai−yaj−zak=[sa,−va].四元數共軛與其本身相乘,會得到一個實四元數,其實部為模長的平方:
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
−
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
數乘
又稱標量乘法,四元數可以與數相乘:
k
q
=
[
k
s
,
k
v
]
T
k\mathbf{q}=[ks,kv]^{T}
kq=[ks,kv]T
我們可以用四元數表達對一個點的旋轉。假設有一個空間三維點
p
=
[
x
,
y
,
z
]
∈
R
3
p=[x,y,z] \in \mathbb{R}^{3}
p=[x,y,z]∈R3,以及一個由單位四元數
q
q
q指定的旋轉。三維點
p
p
p經過
q
q
q的旋轉後變為
p
′
p^{'}
p′。如果使用矩陣描述,那麼有
p
′
=
R
p
p^{'}=Rp
p′=Rp。而如何用四元數描述旋轉呢?
首先,把三維空間點用一個虛四元數來描述:
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旋轉之間的關聯。如果只是簡單應用,不需要了解幾何證明過程,則本小節就足夠了。
回憶一下《旋轉向量與羅德里格斯公式》討論的內容:如果我們需要將一個向量
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∥的情況。
之前推導過,如果一個向量 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⊥,由於 w w ⊥ = [ − u ⋅ v ⊥ , u × v ⊥ ] = [ 0 , u × v ⊥ ] = u × v ⊥ \begin{aligned}ww_{\perp} &=[-u\cdot v_{\perp},u\times v_{\perp}] \\ &=[0,u\times v_{\perp}] \\ &= u\times v_{\perp}\end{aligned} ww⊥=[−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⊥′可以使用四元數乘法來獲得。令 w ⊥ = [ 0 , v ⊥ ] w_{\perp}=[0,v_{\perp}] w⊥=[0,v⊥], q = [ c o s ( θ ) , s i n ( θ ) u ] q=[cos(\theta), sin(\theta)u] q=[cos(θ),sin(θ)u],那麼: w ⊥ ′ = q w ⊥ w^{'}_{\perp }=qw_{\perp} w⊥′=qw⊥
接下來是平行於旋轉軸的 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∥
有了前面的結論,我們可以獲得一般情況下 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⊥在進一步化簡之前,引入三個引理:
關於引理的證明,讀者可以參考文獻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
]
,
q
=
[
c
o
s
(
1
2
θ
)
,
s
i
n
(
1
2
θ
)
u
]
w=[0,v],q=[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u]
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怎麼想到的,太牛了。
任意單位四元數描述了一個旋轉,該旋轉也可用旋轉向量、旋轉矩陣或尤拉角描述。現在來考察四元數與旋轉向量、旋轉矩陣或尤拉角的相互轉換關係。
本節介紹旋轉向量與四元數之間的轉換關係。
繞座標軸的多次旋轉可以等效為繞某一轉軸旋轉一定的角度。假設已知等效旋轉軸方向單位旋轉向量
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)
已知單位四元數
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)
那麼將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 穩定。
另外需要注意的就是奇異性問題,即萬向鎖,下面分析這種情況。當剛體繞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)。當俯仰角為-90°,即剛體豎直向下時的情況也與之類似,可以推匯出奇異姿態時的計算公式。
將四元數轉換為尤拉角可以參考附件。需要注意尤拉角有12種旋轉次序,而上面推導的公式是按照Z-Y-X順序進行的,所以有時會在網上看到不同的轉換公式(因為對應著不同的旋轉次序),在使用時一定要注意旋轉次序是什麼。比如ADAMS軟體裡就預設Body 3-1-3次序,即Z-X-Z尤拉角,而VREP中則按照X-Y-Z尤拉角旋轉。另外萬向鎖問題程式碼的Z-Y-X尤拉角程式碼見類CameraSpacePoint。
為更全面理解四元數和方便引入slerp插值,這一節補充四元數的其它性質:旋轉複合、雙倍覆蓋和指數形式。
旋轉的複合其實在我們之前證明
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_{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是兩個不同的四元數,但是由於旋轉矩陣中的每一項都包含了四元數兩個分量的乘積,它們的旋轉矩陣是完全相同的,即旋轉矩陣並不會出現雙倍覆蓋的問題。
在討論複數的時候,我們利用尤拉公式將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
=
[
c
o
s
(
θ
)
,
s
i
n
(
θ
)
u
]
q = [cos(θ), sin(θ)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等)》
現在,我們通過兩個小程式實際演練四元數的運算。
第一個小程式是演示四元數的常規運算,包括與旋轉矩陣和旋轉向量的轉換,以及用四元數旋轉一個向量,如下所示:
#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
請讀者注意,程式程式碼通常和數學表示有一些細微的差別。例如,通過運運算元過載,四元數和三維向量可以直接計算乘法,但在數學上則需要先把向量轉成虛四元數,再利用四元數乘法進行計算,同樣的情況也適用於變換矩陣乘三維向量的情況。總體而言,程式中的用法會比數學公式更靈活。
下面我們舉一個小例子來演示座標變換。
例子:設有小蘿蔔一號和小蘿蔔二號位於世界座標系中。記世界座標系為
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
四元數的第一部分講解到此,千萬別忘了一鍵三連哦(點贊收藏轉發)。
參考文獻: