1 📖 四元数
1-1 🔖 四元数介绍
四元数 q 有一个实部和三个虚部。我们把实部写在前:
q
=
[
q
0
,
q
1
,
q
2
,
q
3
]
T
或
q
=
[
w
,
x
,
y
,
z
]
T
\mathbf{q}=\left[q_{0}, q_{1}, q_{2}, q_{3}\right]^{T} \text { 或 } \mathbf{q}=[w, x, y, z]^{T}
q=[q0,q1,q2,q3]T 或 q=[w,x,y,z]T
其中 q 0 q_0 q0 为实部, [ q 1 , q 2 , q 3 ] T \left[q_{1}, q_{2}, q_{3}\right]^{T} [q1,q2,q3]T 为虚部。
因为实部为标量,虚部为矢量,所以也可记为:
q = [ s , v ] T \mathbf{q}=[s, \mathbf{v}]^{T} q=[s,v]T
四元数和角轴的转换关系:设角轴为 ω 角度大小为θ ,那么它对应的四元数为:
q
=
[
cos
θ
2
,
ω
sin
θ
2
]
T
\mathbf{q}=\left[\cos \frac{\theta}{2}, \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}
q=[cos2θ,ωsin2θ]T
扩充
单位向量(x,y,z)旋转θ角度后的四元数
( cos θ 2 , x ∗ sin θ 2 , y ∗ sin θ 2 , z ∗ sin θ 2 ) \left(\cos \frac{\theta}{2}, x * \sin \frac{\theta}{2}, y * \sin \frac{\theta}{2}, z * \sin \frac{\theta}{2}\right) (cos2θ,x∗sin2θ,y∗sin2θ,z∗sin2θ)
1-3 🔖 四元数与旋转矩阵的相互转化
设四元数
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
\boldsymbol{q}=q_{0}+q_{1} i+q_{2} j+q_{3} k
q=q0+q1i+q2j+q3k,对应的旋转矩阵 R 为:
R
=
[
1
−
2
q
2
2
−
2
q
3
2
2
q
1
q
2
+
2
q
0
q
3
2
q
1
q
3
−
2
q
0
q
2
2
q
1
q
2
−
2
q
0
q
3
1
−
2
q
1
2
−
2
q
3
2
2
q
2
q
3
+
2
q
0
q
1
2
q
1
q
3
+
2
q
0
q
2
2
q
2
q
3
−
2
q
0
q
1
1
−
2
q
1
2
−
2
q
2
2
]
\boldsymbol{R}=\left[\begin{array}{ccc} 1-2 q_{2}^{2}-2 q_{3}^{2} & 2 q_{1} q_{2}+2 q_{0} q_{3} & 2 q_{1} q_{3}-2 q_{0} q_{2} \\ 2 q_{1} q_{2}-2 q_{0} q_{3} & 1-2 q_{1}^{2}-2 q_{3}^{2} & 2 q_{2} q_{3}+2 q_{0} q_{1} \\ 2 q_{1} q_{3}+2 q_{0} q_{2} & 2 q_{2} q_{3}-2 q_{0} q_{1} & 1-2 q_{1}^{2}-2 q_{2}^{2} \end{array}\right]
R=⎣⎡1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤
由旋转矩阵到四元数的转换如下:
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]
R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
则:
q
0
=
tr
(
R
)
+
1
2
,
q
1
=
r
23
−
r
32
4
q
0
,
q
2
=
r
31
−
r
13
4
q
0
,
q
3
=
r
12
−
r
21
4
q
0
q_{0}=\frac{\sqrt{\operatorname{tr}(R)+1}}{2}, q_{1}=\frac{r_{23}-r_{32}}{4 q_{0}}, q_{2}=\frac{r_{31}-r_{13}}{4 q_{0}}, q_{3}=\frac{r_{12}-r_{21}}{4 q_{0}}
q0=2tr(R)+1,q1=4q0r23−r32,q2=4q0r31−r13,q3=4q0r12−r21
由于 q 和 − q 表示同一个旋转,事实上一个 R 对应的四元数表示并不是惟一的。同时,除了上面给出的转换方式之外,还存在其他几种计算方法
1-2 🔖 四元数时间导数
• 世界坐标系 W ;
• IMU 坐标系 I ;
• 相机坐标系 C ;
四元数时间导数:
设初始旋转为 q = [s, v] ,然后,发生了角轴为 ω, θ 的旋转(右乘,对应四元数记作 ∆q ),那么 q 相对该旋转的导数为:(7)
lim
θ
→
0
q
⊗
Δ
q
−
q
θ
=
lim
θ
→
0
[
s
cos
θ
2
−
v
T
ω
sin
θ
2
,
s
ω
sin
θ
2
+
cos
θ
2
v
+
v
×
ω
sin
θ
2
]
T
−
q
θ
=
lim
θ
→
0
[
s
(
cos
θ
2
−
1
)
−
v
T
ω
sin
θ
2
,
s
ω
sin
θ
2
+
(
cos
θ
2
−
1
)
v
+
v
×
ω
sin
θ
2
]
T
θ
=
[
−
1
2
v
T
ω
,
1
2
s
ω
+
1
2
v
×
ω
]
T
=
q
⊗
[
0
,
1
2
ω
]
T
\begin{aligned} \lim _{\theta \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}-\mathbf{q}}{\theta} &=\lim _{\theta \rightarrow 0} \frac{\left[s \cos \frac{\theta}{2}-\mathbf{v}^{T} \omega \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\cos \frac{\theta}{2} \mathbf{v}+\mathbf{v} \times \omega \sin \frac{\theta}{2}\right]^{\mathrm{T}}-\mathbf{q}}{\theta} \\ &=\lim _{\theta \rightarrow 0} \frac{\left[s\left(\cos \frac{\theta}{2}-1\right)-\mathbf{v}^{T} \omega \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\left(\cos \frac{\theta}{2}-1\right) \mathbf{v}+\mathbf{v} \times \omega \sin \frac{\theta}{2}\right]^{\mathrm{T}}}{\theta} \\ &=\left[-\frac{1}{2} \mathbf{v}^{T} \boldsymbol{\omega}, \frac{1}{2} s \boldsymbol{\omega}+\frac{1}{2} \mathbf{v} \times \boldsymbol{\omega}\right]^{\mathrm{T}} \\ &=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}} \end{aligned}
θ→0limθq⊗Δq−q=θ→0limθ[scos2θ−vTωsin2θ,sωsin2θ+cos2θv+v×ωsin2θ]T−q=θ→0limθ[s(cos2θ−1)−vTωsin2θ,sωsin2θ+(cos2θ−1)v+v×ωsin2θ]T=[−21vTω,21sω+21v×ω]T=q⊗[0,21ω]T
因此,若角速度为 ω ,那么旋转的时间导数即为:(8)
q ˙ = q ⊗ [ 0 , 1 2 ω ] T \dot{\mathbf{q}}=\mathbf{q} \otimes\left[0, \frac{1}{2} \omega\right]^{\mathrm{T}} q˙=q⊗[0,21ω]T
2 📖 一些求导函数
2-1 🔖 扰动模型
注意: θ \theta θ表示旋转量, a a a表示旋转轴
ϕ = θ a \phi=\theta \boldsymbol{a} ϕ=θa
ln ( exp ( ϕ 1 ∧ ) exp ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 if ϕ 1 is small J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 if ϕ 2 is small. \ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll} \boldsymbol{J}_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { if } \phi_{1} \text { is small } \\ J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} & \text { if } \phi_{2} \text { is small. } \end{array}\right. ln(exp(ϕ1∧)exp(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2Jr(ϕ1)−1ϕ2+ϕ1 if ϕ1 is small if ϕ2 is small.
当对一个旋转矩阵 R 2 R_2 R2 (李代数为 ϕ 2 {\phi}_2 ϕ2 )左乘一个微小旋转矩阵 R 1 R_1 R1 (李代数为 ϕ 1 \phi_{1} ϕ1)时,可以近似地看作,在原有的李代数 ϕ 2 {\phi}_2 ϕ2 上,加上了一项 J l ( ϕ 2 ) − 1 ϕ 1 \boldsymbol{J}_{l}\left(\boldsymbol{\phi}_{2}\right)^{-1} \boldsymbol{\phi}_{1} Jl(ϕ2)−1ϕ1 。同理,第二个近似描述了右乘一个微小位移的情况。
其中左乘雅可比:
J
l
=
J
=
sin
θ
θ
I
+
(
1
−
sin
θ
θ
)
a
a
T
+
1
−
cos
θ
θ
a
∧
\boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}
Jl=J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧
逆为:
J
l
−
1
=
θ
2
cot
θ
2
I
+
(
1
−
θ
2
cot
θ
2
)
a
a
T
−
θ
2
a
∧
\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{T}-\frac{\theta}{2} \boldsymbol{a}^{\wedge}
Jl−1=2θcot2θI+(1−2θcot2θ)aaT−2θa∧
一般直接用下面形式的多:
J r ( ϕ ⃗ ) = I − 1 − cos ( ∥ ϕ ⃗ ∥ ) ∥ ϕ ⃗ ∥ 2 ϕ ⃗ ∧ + ∥ ϕ ⃗ ∥ − sin ( ∥ ϕ ⃗ ∥ ) ∥ ϕ ⃗ ∥ 3 ( ϕ ⃗ ∧ ) 2 \mathbf{J}_{r}(\vec{\phi})=\mathbf{I}-\frac{1-\cos (\|\vec{\phi}\|)}{\|\vec{\phi}\|^{2}} \vec{\phi}^{\wedge}+\frac{\|\vec{\phi}\|-\sin (\|\vec{\phi}\|)}{\|\vec{\phi}\|^{3}}\left(\vec{\phi}^{\wedge}\right)^{2} Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ∥3∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2J r − 1 ( ϕ ⃗ ) = I + 1 2 ϕ ⃗ ∧ + ( 1 ∥ ϕ ⃗ ∥ 2 − 1 + cos ( ∥ ϕ ⃗ ∥ ) 2 ⋅ ∥ ϕ ⃗ ∥ ⋅ sin ( ∥ ϕ ⃗ ∥ ) ) ( ϕ ⃗ ∧ ) 2 \mathbf{J}_{r}^{-1}(\vec{\phi})=\mathbf{I}+\frac{1}{2} \vec{\phi}^{\wedge}+\left(\frac{1}{\|\vec{\phi}\|^{2}}-\frac{1+\cos (\|\vec{\phi}\|)}{2 \cdot\|\vec{\phi}\| \cdot \sin (\|\vec{\phi}\|)}\right)\left(\vec{\phi}^{\wedge}\right)^{2} Jr−1(ϕ)=I+21ϕ∧+(∥ϕ∥21−2⋅∥ϕ∥⋅sin(∥ϕ∥)1+cos(∥ϕ∥))(ϕ∧)2
这种形式的写法,更贴近代码,容易对比
右乘雅可比:
J
r
(
ϕ
)
=
J
l
(
−
ϕ
)
\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi})
Jr(ϕ)=Jl(−ϕ)
2-1 🔖 旋转矩阵R的左右扰动求导
旋转点的左扰动雅可比:
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
exp
(
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
=
lim
φ
→
0
(
I
+
φ
∧
)
exp
(
ϕ
∧
)
p
−
exp
(
ϕ
∧
)
p
φ
=
lim
φ
→
0
φ
∧
R
p
φ
=
lim
φ
→
0
−
(
R
p
)
∧
φ
φ
=
−
(
R
p
)
∧
.
\begin{aligned} \frac{\partial(\mathbf{R} \mathbf{p})}{\partial \boldsymbol{\varphi}} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \mathbf{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \mathbf{p}}{\boldsymbol{\varphi}} \\ &=\lim _{\varphi \rightarrow 0} \frac{\left(\mathbf{I}+\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \mathbf{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \mathbf{p}}{\boldsymbol{\varphi}} \\ &=\lim _{\varphi \rightarrow 0} \frac{\boldsymbol{\varphi}^{\wedge} \mathbf{R} \mathbf{p}}{\boldsymbol{\varphi}}=\lim _{\varphi \rightarrow 0} \frac{-(\mathbf{R} \mathbf{p})^{\wedge} \boldsymbol{\varphi}}{\boldsymbol{\varphi}}=-(\mathbf{R} \mathbf{p})^{\wedge} . \end{aligned}
∂φ∂(Rp)=φ→0limφexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφ(I+φ∧)exp(ϕ∧)p−exp(ϕ∧)p=φ→0limφφ∧Rp=φ→0limφ−(Rp)∧φ=−(Rp)∧.
旋转点的右扰动雅可比:
∂
(
R
p
)
∂
φ
=
lim
φ
→
0
R
exp
(
φ
∧
)
p
−
R
p
φ
=
lim
φ
→
0
R
(
I
+
φ
∧
)
p
−
R
p
φ
=
lim
φ
→
0
R
φ
∧
p
φ
=
lim
φ
→
0
−
R
p
∧
φ
φ
=
−
R
p
∧
\begin{aligned} \frac{\partial(\mathbf{R} \mathbf{p})}{\partial \varphi} &=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R} \exp \left(\boldsymbol{\varphi}^{\wedge}\right) \mathbf{p}-\mathbf{R} \mathbf{p}}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R}\left(\mathbf{I}+\boldsymbol{\varphi}^{\wedge}\right) \mathbf{p}-\mathbf{R} \mathbf{p}}{\varphi} \\ &=\lim _{\varphi \rightarrow 0} \frac{\mathbf{R} \boldsymbol{\varphi}^{\wedge} \mathbf{p}}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-\mathbf{R} \mathbf{p}^{\wedge} \boldsymbol{\varphi}}{\varphi}=-\mathbf{R} \mathbf{p}^{\wedge} \end{aligned}
∂φ∂(Rp)=φ→0limφRexp(φ∧)p−Rp=φ→0limφR(I+φ∧)p−Rp=φ→0limφRφ∧p=φ→0limφ−Rp∧φ=−Rp∧
2-2 🔖 R 1 ∗ R 2 R_{1}*R_{2} R1∗R2对 R 2 R_{2} R2求导
下面用到了有扰动雅克比矩阵的近似等式
∂
R
1
R
2
∂
R
2
=
lim
ϕ
→
0
ln
(
R
1
R
2
exp
(
ϕ
∧
)
)
−
ln
(
R
1
R
2
)
ϕ
=
lim
ϕ
→
0
ln
(
R
1
R
2
)
+
J
r
−
1
(
ln
(
R
1
R
2
)
)
ϕ
−
ln
(
R
1
R
2
)
ϕ
=
J
r
−
1
(
ln
(
R
1
R
2
)
)
(
1
)
\begin{aligned} \frac{\partial \mathbf{R}_{1} \mathbf{R}_{2}}{\partial \mathbf{R}_{2}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(\mathbf{R}_{1} \mathbf{R}_{2} \exp \left(\phi^{\wedge}\right)\right)-\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)+\mathrm{J}_{r}^{-1}\left(\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)\right) \phi-\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)}{\phi} \\ &=\mathrm{J}_{r}^{-1}\left(\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)\right) \end{aligned} \space\space\space\space\space\space(1)
∂R2∂R1R2=ϕ→0limϕln(R1R2exp(ϕ∧))−ln(R1R2)=ϕ→0limϕln(R1R2)+Jr−1(ln(R1R2))ϕ−ln(R1R2)=Jr−1(ln(R1R2)) (1)
ln ( R 1 R 2 ) ∨ \ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)^{\vee} ln(R1R2)∨ :将矩阵 R 1 R 2 \bf R_{1}R_{2} R1R2变成向量形式(上面整个过程都是在向量形式下进行的求导)
J r − 1 ( ln ( R R 2 ) ∨ ) \mathbf{J}_{r}^{-1}\left(\ln \left(\mathbf{R} \mathbf{R}_{2}\right)^{\vee}\right) Jr−1(ln(RR2)∨): 以 ln ( R 1 R 2 ) ∨ \ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)^{\vee} ln(R1R2)∨ 为自变量的右雅可比(上面是求逆了)
2-3 🔖 R 1 ∗ R 2 R_{1}*R_{2} R1∗R2对 R 1 R_{1} R1求导
下面利用到了伴随矩阵的一个变式
exp ( ϕ ∧ ) R = R exp ( ( R T ϕ ) ∧ ) \exp \left(\phi^{\wedge}\right) \mathbf{R} = \mathbf{R} \exp \left(\left(\mathbf{R}^{T} \phi\right)^{\wedge}\right) exp(ϕ∧)R=Rexp((RTϕ)∧)
∂ R 1 R 2 ∂ R 1 = lim ϕ → 0 ln ( R 1 exp ( ϕ ∧ ) R 2 ) − ln ( R 1 R 2 ) ϕ = lim ϕ → 0 ln ( R 1 R 2 exp ( ( R 2 T ϕ ) ∧ ) ) − ln ( R 1 R 2 ) ϕ = J r − 1 ( ln ( R 1 R 2 ) ) R 2 T ( 2 ) \begin{aligned} \frac{\partial \mathbf{R}_{1} \mathbf{R}_{2}}{\partial \mathbf{R}_{1}} &=\lim _{\phi \rightarrow 0} \frac{\ln \left(\mathbf{R}_{1} \exp \left(\phi^{\wedge}\right) \mathbf{R}_{2}\right)-\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)}{\phi} \\ &=\lim _{\phi \rightarrow 0} \frac{\ln \left(\mathbf{R}_{1} \mathbf{R}_{2} \exp \left(\left(\mathbf{R}_{2}^{T} \phi\right)^{\wedge}\right)\right)-\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)}{\phi} \\ &=J_{r}^{-1}\left(\ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)\right) \mathbf{R}_{2}^{T} \end{aligned}\space\space\space\space\space\space(2) ∂R1∂R1R2=ϕ→0limϕln(R1exp(ϕ∧)R2)−ln(R1R2)=ϕ→0limϕln(R1R2exp((R2Tϕ)∧))−ln(R1R2)=Jr−1(ln(R1R2))R2T (2)
ln ( R 1 R 2 ) ∨ \ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)^{\vee} ln(R1R2)∨ :将矩阵 R 1 R 2 \bf R_{1}R_{2} R1R2变成向量形式(上面整个过程都是在向量形式下进行的求导)
J r − 1 ( ln ( R R 2 ) ∨ ) \mathbf{J}_{r}^{-1}\left(\ln \left(\mathbf{R} \mathbf{R}_{2}\right)^{\vee}\right) Jr−1(ln(RR2)∨): 以 ln ( R 1 R 2 ) ∨ \ln \left(\mathbf{R}_{1} \mathbf{R}_{2}\right)^{\vee} ln(R1R2)∨ 为自变量的右雅可比(上面是求逆了)
3 📖 vee( ∨ \vee ∨)和hat( ∧ \wedge ∧)
3-1 🔖 vee和hat介绍
hat运算符 ∧ \wedge ∧: (反对称算子)把 3 维实向量映射为 3×3 反对称阵;
vee 运算符 ∨ \vee ∨:把 3×3 反对称映射为 3 维实向量;
ω ∧ = [ ω 1 ω 2 ω 3 ] ∧ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] = W \boldsymbol{\omega}^{\wedge}=\left[\begin{array}{l} \omega_{1} \\ \omega_{2} \\ \omega_{3} \end{array}\right]^{\wedge}=\left[\begin{array}{ccc} 0 & -\omega_{3} & \omega_{2} \\ \omega_{3} & 0 & -\omega_{1} \\ -\omega_{2} & \omega_{1} & 0 \end{array}\right]=\mathbf{W} ω∧=⎣⎡ω1ω2ω3⎦⎤∧=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤=W
W ∨ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] ∨ = [ ω 1 ω 2 ω 3 ] = ω \mathbf{W}^{\vee}=\left[\begin{array}{ccc} 0 & -\omega_{3} & \omega_{2} \\ \omega_{3} & 0 & -\omega_{1} \\ -\omega_{2} & \omega_{1} & 0 \end{array}\right]^{\vee}=\left[\begin{array}{c} \omega_{1} \\ \omega_{2} \\ \omega_{3} \end{array}\right]=\boldsymbol{\omega} W∨=⎣⎡0ω3−ω2−ω30ω1ω2−ω10⎦⎤∨=⎣⎡ω1ω2ω3⎦⎤=ω
关于 hat 运算符的一个性质
a
∧
⋅
b
=
−
b
∧
⋅
a
,
∀
a
,
b
∈
R
3
\mathbf{a}^{\wedge} \cdot \mathbf{b}=-\mathbf{b}^{\wedge} \cdot \mathbf{a}, \quad \forall \mathbf{a}, \mathbf{b} \in R^{3}
a∧⋅b=−b∧⋅a,∀a,b∈R3
3-2 🔖 exp()和log()操作
指数映射
exp
(
∙
)
\exp (\bullet)
exp(∙)将
R
3
×
1
R^{3 \times 1}
R3×1映射到
R
3
×
3
R^{3 \times 3}
R3×3:
exp
(
ϕ
⃗
∧
)
=
I
+
sin
(
∥
ϕ
⃗
∥
)
∥
ϕ
⃗
∥
ϕ
⃗
∧
+
1
−
cos
(
∥
ϕ
⃗
∥
)
∥
ϕ
⃗
∥
2
(
ϕ
⃗
∧
)
2
\exp \left(\vec{\phi}^{\wedge}\right)=\mathbf{I}+\frac{\sin (\|\vec{\phi}\|)}{\|\vec{\phi}\|} \vec{\phi}^{\wedge}+\frac{1-\cos (\|\vec{\phi}\|)}{\|\vec{\phi}\|^{2}}\left(\vec{\phi}^{\wedge}\right)^{2}
exp(ϕ∧)=I+∥ϕ∥sin(∥ϕ∥)ϕ∧+∥ϕ∥21−cos(∥ϕ∥)(ϕ∧)2
当 ϕ ⃗ \vec{\phi} ϕ是小量时,有一阶近似如下:
exp ( ϕ ⃗ ∧ ) ≈ I + ϕ ⃗ ∧ \exp \left(\vec{\phi}^{\wedge}\right) \approx \mathbf{I}+\vec{\phi}^{\wedge} exp(ϕ∧)≈I+ϕ∧
对数映射
log
(
∙
)
\log (\bullet)
log(∙)将
R
3
×
3
R^{3 \times 3}
R3×3映射到
R
3
×
1
R^{3 \times 1}
R3×1
log
(
R
)
=
φ
⋅
(
R
−
R
T
)
2
sin
(
φ
)
\log (\mathbf{R})=\frac{\varphi \cdot\left(\mathbf{R}-\mathbf{R}^{T}\right)}{2 \sin (\varphi)}
log(R)=2sin(φ)φ⋅(R−RT)
其中 φ = cos − 1 ( tr ( R ) − 1 2 ) \varphi=\cos ^{-1}\left(\frac{\operatorname{tr}(R)-1}{2}\right) φ=cos−1(2tr(R)−1)
其他
log
(
R
)
∨
=
φ
⋅
a
\log (\mathbf{R})^{\vee}=\varphi \cdot \mathbf{a}
log(R)∨=φ⋅a
φ \varphi φ:旋转角
a \mathbf a a:旋转轴单位矢量,有 a = ( R − R T 2 sin ( φ ) ) ∨ \mathbf{a}=\left(\frac{\mathbf{R}-\mathbf{R}^{T}}{2 \sin (\varphi)}\right)^{\vee} a=(2sin(φ)R−RT)∨
小量下的近似表达
对于 3 维实向量
ϕ
⃗
\vec{\phi}
ϕ和一个小量
δ
ϕ
⃗
\delta \vec{\phi}
δϕ,有如下性质:
Exp
(
ϕ
⃗
+
δ
ϕ
⃗
)
≈
Exp
(
ϕ
⃗
)
⋅
Exp
(
J
r
(
ϕ
⃗
)
⋅
δ
ϕ
⃗
)
Log
(
Exp
(
ϕ
⃗
)
⋅
Exp
(
δ
ϕ
⃗
)
)
=
ϕ
⃗
+
J
r
−
1
(
ϕ
⃗
)
⋅
δ
ϕ
⃗
\operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_{r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) \\ \\ \operatorname{Log} (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))=\vec{\phi}+\mathbf{J}_{r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi}
Exp(ϕ+δϕ)≈Exp(ϕ)⋅Exp(Jr(ϕ)⋅δϕ)Log(Exp(ϕ)⋅Exp(δϕ))=ϕ+Jr−1(ϕ)⋅δϕ
注意:
Exp(ϕ) = exp ( ϕ ∧ ) \operatorname{Exp(\phi)}=\exp \left({\phi}^{\wedge}\right) Exp(ϕ)=exp(ϕ∧)
在等号后面的exp内部还需要取反对称运算,同理 Log(R) = log(R) \operatorname{Log(R)}=\operatorname{log(R)} Log(R)=log(R)
其中:
J
r
(
ϕ
⃗
)
=
I
−
1
−
cos
(
∥
ϕ
⃗
∥
)
∥
ϕ
⃗
∥
2
ϕ
⃗
∧
+
∥
ϕ
⃗
∥
−
sin
(
∥
ϕ
⃗
∥
)
∥
ϕ
⃗
∥
3
(
ϕ
⃗
∧
)
2
\mathbf{J}_{r}(\vec{\phi})=\mathbf{I}-\frac{1-\cos (\|\vec{\phi}\|)}{\|\vec{\phi}\|^{2}} \vec{\phi}^{\wedge}+\frac{\|\vec{\phi}\|-\sin (\|\vec{\phi}\|)}{\|\vec{\phi}\|^{3}}\left(\vec{\phi}^{\wedge}\right)^{2}
Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ∥3∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2
J r − 1 ( ϕ ⃗ ) = I + 1 2 ϕ ⃗ ∧ + ( 1 ∥ ϕ ⃗ ∥ 2 − 1 + cos ( ∥ ϕ ⃗ ∥ ) 2 ⋅ ∥ ϕ ⃗ ∥ ⋅ sin ( ∥ ϕ ⃗ ∥ ) ) ( ϕ ⃗ ∧ ) 2 \mathbf{J}_{r}^{-1}(\vec{\phi})=\mathbf{I}+\frac{1}{2} \vec{\phi}^{\wedge}+\left(\frac{1}{\|\vec{\phi}\|^{2}}-\frac{1+\cos (\|\vec{\phi}\|)}{2 \cdot\|\vec{\phi}\| \cdot \sin (\|\vec{\phi}\|)}\right)\left(\vec{\phi}^{\wedge}\right)^{2} Jr−1(ϕ)=I+21ϕ∧+(∥ϕ∥21−2⋅∥ϕ∥⋅sin(∥ϕ∥)1+cos(∥ϕ∥))(ϕ∧)2
3-3 🔖 一些关于旋转矩阵的性质(⭐️)
R对时间的导数
R
˙
=
R
ω
∧
\dot{\mathbf{R}}=\mathbf{R} \omega^{\wedge}
R˙=Rω∧
指数映射的 Adjoint 性质
R
⋅
Exp
(
ϕ
⃗
)
⋅
R
T
=
exp
(
R
ϕ
⃗
∧
R
T
)
=
Exp
(
R
ϕ
⃗
)
⇔
Exp
(
ϕ
⃗
)
⋅
R
=
R
⋅
Exp
(
R
T
ϕ
⃗
)
\begin{array}{l} \mathbf{R} \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}^{T}=\exp \left(\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}\right)=\operatorname{Exp}(\mathbf{R} \vec{\phi}) \\ \Leftrightarrow \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}=\mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right) \end{array}
R⋅Exp(ϕ)⋅RT=exp(Rϕ∧RT)=Exp(Rϕ)⇔Exp(ϕ)⋅R=R⋅Exp(RTϕ)
其他
(
R
ϕ
⃗
)
∧
=
R
ϕ
⃗
∧
R
T
(\mathbf{R} \vec{\phi})^{\wedge}=\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}
(Rϕ)∧=Rϕ∧RT
4 📖欧拉角与旋转矩阵的转换关系
4-1 🔖 基础概念
内在旋转(intrinsic rotations)和外在旋转(extrinsic rotations)
- 内在旋转每次旋转围绕的轴是上次旋转之后坐标系的某个轴。
- 外在旋转每次旋转的轴是固定坐标系中的轴。
内在旋转与外在旋转的转换关系:互换第一次和第三次旋转的位置则两者结果相同。例如
Z-Y-X
旋转的内部旋转和X-Y-Z
旋转的外部旋转的旋转矩阵相同下图是
z-x-z
的内在旋转和外在旋转示意图
旋转角度是左手系还是右手系
使用右手的大拇指指向旋转轴,其他4个手指在握拳过程中的指向便是正的角度
- 右手系是逆时针
- 左手系是顺时针
Roll(翻滚角)、Pitch(俯仰角)、Yaw(偏航角)
- 绕x轴旋转角度 ψ \psi ψ: Roll(翻滚角)
- 绕y轴旋转角度 θ \theta θ:Pitch(俯仰角)
- 绕z轴旋转角度 ϕ \phi ϕ:Yaw(偏航角)
4-2 🔖 不同轴的欧拉角转换成旋转矩阵
蓝色是起始坐标系,而红色的是旋转之后的坐标系
注意:上面的角度符号与下面完全没关系,不要搞混
右手系下,逆时针旋转的角度为正,绕不同轴的旋转结果:
R
x
(
α
)
=
[
1
0
0
0
cos
θ
−
sin
θ
0
sin
θ
cos
θ
]
R
y
(
θ
)
=
[
cos
θ
0
sin
θ
0
1
0
−
sin
θ
0
cos
θ
]
R
z
(
θ
)
=
[
cos
θ
−
sin
θ
0
sin
θ
cos
θ
0
0
0
1
]
\begin{aligned} &R_{x}(\alpha)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{array}\right] \\ &R_{y}(\theta)=\left[\begin{array}{ccc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{array}\right] \\ &R_{z}(\theta)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}
Rx(α)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤Ry(θ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤Rz(θ)=⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤
这里也写上左手系:
R x ( θ ) = [ 1 0 0 0 cos θ sin θ 0 − sin θ cos θ ] R y ( θ ) = [ cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ ] R z ( θ ) = [ cos θ sin θ 0 − sin θ cos θ 0 0 0 1 ] \begin{aligned} &R_{x}(\theta)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta \end{array}\right] \\ &R_{y}(\theta)=\left[\begin{array}{ccc} \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta \end{array}\right] \\ &R_{z}(\theta)=\left[\begin{array}{ccc} \cos \theta & \sin \theta & 0 \\ -\sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned} Rx(θ)=⎣⎡1000cosθ−sinθ0sinθcosθ⎦⎤Ry(θ)=⎣⎡cosθ0sinθ010−sinθ0cosθ⎦⎤Rz(θ)=⎣⎡cosθ−sinθ0sinθcosθ0001⎦⎤
即:所有的 s i n θ \bf sin{\theta} sinθ都变号
所以,容易得到,欧拉角转旋转矩阵如下:
R
=
R
z
(
γ
)
R
y
(
β
)
R
x
(
α
)
=
[
cos
β
cos
γ
sin
α
sin
β
cos
γ
−
cos
α
sin
γ
cos
α
sin
β
cos
γ
+
sin
α
sin
γ
cos
β
sin
γ
sin
α
sin
β
sin
γ
+
cos
α
cos
γ
cos
α
sin
β
sin
γ
−
sin
α
cos
γ
−
sin
β
sin
α
cos
β
cos
α
cos
β
]
\begin{aligned} R &=R_{z}(\gamma) R_{y}(\beta) R_{x}(\alpha) \\ &=\left[\begin{array}{ccc} \cos \beta \cos \gamma & \sin \alpha \sin \beta \cos \gamma-\cos \alpha \sin \gamma & \cos \alpha \sin \beta \cos \gamma+\sin \alpha \sin \gamma \\ \cos \beta \sin \gamma & \sin \alpha \sin \beta \sin \gamma+\cos \alpha \cos \gamma & \cos \alpha \sin \beta \sin \gamma-\sin \alpha \cos \gamma \\ -\sin \beta & \sin \alpha \cos \beta & \cos \alpha \cos \beta \end{array}\right] \end{aligned}
R=Rz(γ)Ry(β)Rx(α)=⎣⎡cosβcosγcosβsinγ−sinβsinαsinβcosγ−cosαsinγsinαsinβsinγ+cosαcosγsinαcosβcosαsinβcosγ+sinαsinγcosαsinβsinγ−sinαcosγcosαcosβ⎦⎤
即:先绕x轴旋转 α \alpha α,再绕y轴旋转 β \beta β,最后绕z轴旋转 γ \gamma γ:
线性近似
当
θ
≈
0
\theta \approx 0
θ≈0时有
sin
θ
≈
θ
,
cos
θ
≈
1
\sin \theta \approx \theta, \cos \theta \approx 1
sinθ≈θ,cosθ≈1因此当
α
,
β
,
γ
≈
0
\alpha, \beta, \gamma \approx 0
α,β,γ≈0时:
R
(
α
,
β
,
γ
)
≈
(
1
α
β
−
γ
α
γ
+
β
0
γ
α
β
γ
+
1
β
γ
−
α
0
−
β
α
1
0
0
0
0
1
)
≈
(
1
−
γ
β
0
γ
1
−
α
0
−
β
α
1
0
0
0
0
1
)
=
R
^
(
α
,
β
,
γ
)
.
\begin{aligned} \mathbf{R}(\alpha, \beta, \gamma) & \approx\left(\begin{array}{cccc} 1 & \alpha \beta-\gamma & \alpha \gamma+\beta & 0 \\ \gamma & \alpha \beta \gamma+1 & \beta \gamma-\alpha & 0 \\ -\beta & \alpha & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right) \\ & \approx\left(\begin{array}{cccc} 1 & -\gamma & \beta & 0 \\ \gamma & 1 & -\alpha & 0 \\ -\beta & \alpha & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right)=\hat{\mathbf{R}}(\alpha, \beta, \gamma) . \end{aligned}
R(α,β,γ)≈⎝⎜⎜⎛1γ−β0αβ−γαβγ+1α0αγ+ββγ−α100001⎠⎟⎟⎞≈⎝⎜⎜⎛1γ−β0−γ1α0β−α100001⎠⎟⎟⎞=R^(α,β,γ).
4-3 🔖 旋转矩阵求解欧拉角
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right] R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
对比上一节的旋转矩阵求解得到:
θ
x
=
atan
2
(
r
32
,
r
33
)
θ
y
=
atan
2
(
−
r
31
,
r
32
2
+
r
33
2
)
θ
z
=
atan
2
(
r
21
,
r
11
)
\begin{gathered} \theta_{x}=\operatorname{atan} 2\left(r_{32}, r_{33}\right) \\ \theta_{y}=\operatorname{atan} 2\left(-r_{31}, \sqrt{r_{32}^{2}+r_{33}^{2}}\right) \\ \theta_{z}=\operatorname{atan} 2\left(r_{21}, r_{11}\right) \end{gathered}
θx=atan2(r32,r33)θy=atan2(−r31,r322+r332)θz=atan2(r21,r11)