文章目录
前言
研究旋转矩阵,旋转向量,单位四元数,都是为了表达机器人的姿态。欧拉角在SLAM的应用中不多,就不涉及了。
一、要点
1. 旋转矩阵
- 旋转矩阵是行列式为1的正交矩阵,由单位正交基组成。
正交矩阵的行列式为正负1
- 旋转矩阵是两个坐标系的过渡矩阵,其左乘坐标系1中的某向量的坐标,可以得到将此向量过渡到坐标系2后的坐标表示。
- n维空间的旋转矩阵构成特殊正交群。
- 优点:是计算非常方便,矩阵乘法非常简单。
- 缺点:1.表达方式冗余,旋转只有3个自由度,却要用9个量表示。2.带有正交,行列式为1的约束,不利于优化。
2. 旋转向量
- 旋转向量又称为轴角,其方向与旋转轴 n \mathbf n n一致,而长度等于旋转角 θ \mathit \theta θ。向量 θ n \mathit \theta \mathbf n θn即可表示旋转。
- 不严谨的说,这玩意就是李代数。
- 优点:表达紧凑;没有约束易于优化。
- 缺点:不能直接用来计算旋转一个向量,需要转换为旋转矩阵或四元数才能做这个计算;具有奇异性,模长大于
2
π
2 \pi
2π时具有周期性。
所谓奇异性,我理解的就是做不到一一对应,在这里是一个姿态可以对应多个旋转向量
3. 单位四元数
- 四元数的运算基本与复数的运算是类似的,唯一的区别是四元数的乘法不可交换(由于乘法公式的最后项有个叉乘)。
- 用四元数表示旋转的三个要点:
- 实部唯一表示了旋转角;
- 虚部所指方向即旋转轴的方向,(注意:虚部在 R 3 \mathbb R^3 R3里不需要是一个单位向量);
- 实部和虚部一起要服从是单位四元数的约束,即模为1。
- 单位四元数的逆等于其共轭。
- 优点:没有奇异性,将要旋转的向量变成纯虚数后可以直接用四元数乘法计算。
- 缺点:不够直观。
二、旋转向量—>旋转矩阵(罗德里格斯公式)
在上图中,向量
v
\mathbf v
v沿着旋转轴
n
\mathbf n
n旋转
θ
\mathit \theta
θ角,变为
v
′
\mathbf v'
v′。现在我们要将
v
′
\mathbf v'
v′分解成向量
v
\mathbf v
v,旋转轴
n
\mathbf n
n,旋转角
θ
\mathit \theta
θ组成的量
v
′
=
v
k
+
v
p
′
=
v
k
+
a
+
b
=
(
v
⋅
n
)
n
+
∣
v
i
∣
sin
θ
j
+
v
i
cos
θ
=
(
v
⋅
n
)
n
+
∣
(
n
×
v
)
×
n
∣
sin
θ
n
×
v
∣
n
×
v
∣
+
(
(
n
×
v
)
×
n
)
cos
θ
=
(
v
⋅
n
)
n
+
∣
n
∣
∣
v
∣
sin
α
∣
n
∣
sin
π
2
sin
θ
n
×
v
∣
n
∣
∣
v
∣
sin
α
+
(
(
n
×
v
)
×
n
)
cos
θ
=
(
v
⋅
n
)
n
+
(
n
×
v
)
sin
θ
+
(
v
−
(
v
⋅
n
)
n
)
cos
θ
=
(
1
−
cos
θ
)
(
v
⋅
n
)
n
+
n
∧
v
sin
θ
+
v
cos
θ
=
(
1
−
cos
θ
)
n
n
T
v
+
sin
θ
n
∧
v
+
cos
θ
v
=
[
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
]
v
\begin{split} \mathbf v' & = \mathbf v_k + \mathbf v'_p \\ & = \mathbf v_k + \mathbf a + \mathbf b \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + | \mathbf v_i|\sin\theta \mathbf j + \mathbf v_i \cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + |(\mathbf n \times \mathbf v) \times \mathbf n | \sin \theta \frac {\mathbf n \times \mathbf v}{|\mathbf n \times \mathbf v|} + ((\mathbf n \times \mathbf v) \times \mathbf n)\cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + |\mathbf n| |\mathbf v|\sin \alpha |\mathbf n| \sin \frac{\pi}{2} \sin\theta \frac{\mathbf n \times \mathbf v}{|\mathbf n| |\mathbf v| \sin \alpha} + ((\mathbf n \times \mathbf v) \times \mathbf n)\cos \theta \\ & = (\mathbf v \cdot \mathbf n) \mathbf n + (\mathbf n \times \mathbf v) \sin \theta + (\mathbf v - (\mathbf v \cdot \mathbf n ) \mathbf n) \cos \theta \\ & = (1 - \cos \theta) (\mathbf v \cdot \mathbf n) \mathbf n + \mathbf n^{\land} \mathbf v \sin \theta + \mathbf v \cos \theta \\ & = (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} \mathbf v + \sin \theta \mathbf n^{\land} \mathbf v + \cos \theta \mathbf v \\ & = [\cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land}] \mathbf v \end{split}
v′=vk+vp′=vk+a+b=(v⋅n)n+∣vi∣sinθj+vicosθ=(v⋅n)n+∣(n×v)×n∣sinθ∣n×v∣n×v+((n×v)×n)cosθ=(v⋅n)n+∣n∣∣v∣sinα∣n∣sin2πsinθ∣n∣∣v∣sinαn×v+((n×v)×n)cosθ=(v⋅n)n+(n×v)sinθ+(v−(v⋅n)n)cosθ=(1−cosθ)(v⋅n)n+n∧vsinθ+vcosθ=(1−cosθ)nnTv+sinθn∧v+cosθv=[cosθI+(1−cosθ)nnT+sinθn∧]v
要注意点乘得到的是标量;叉乘得到的是向量,且要用右手定则分析其方向。
( v ⋅ n ) n = n n T v (\mathbf v \cdot \mathbf n) \mathbf n = \mathbf n \mathbf n^{\mathrm T} \mathbf v (v⋅n)n=nnTv可以从微观层面去证明,很简单。
上面的计算有很多细节值得思考,比如第三行的 ∣ v i ∣ sin θ j | \mathbf v_i|\sin\theta \mathbf j ∣vi∣sinθj为什么要乘单位向量 j \mathbf j j? v i cos θ \mathbf v_i \cos \theta vicosθ为什么又不乘?
上式中中括号里的内容是一个矩阵,就是旋转矩阵
R
\mathbf R
R:
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
n
∧
\begin{equation} \mathbf R = \cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land} \end{equation}
R=cosθI+(1−cosθ)nnT+sinθn∧
三、旋转矩阵—>旋转向量
将(1)式两边取迹:
t
r
(
R
)
=
3
cos
θ
+
(
1
−
cos
θ
)
n
T
n
+
sin
θ
⋅
0
=
3
cos
θ
+
1
−
cos
θ
=
2
cos
θ
+
1
\begin{align} \mathrm tr(\mathbf R) & = 3 \cos \theta + (1 - \cos \theta) \mathbf n^{\mathrm T} \mathbf n + \sin \theta \cdot 0 \notag \\ & = 3 \cos \theta + 1 - \cos \theta \notag \\ & = 2 \cos \theta + 1 \end{align}
tr(R)=3cosθ+(1−cosθ)nTn+sinθ⋅0=3cosθ+1−cosθ=2cosθ+1
从旋转矩阵中得到旋转角:
θ
=
arccos
t
r
(
R
)
−
1
2
\theta = \arccos \frac {\mathrm tr(\mathbf R) - 1} {2}
θ=arccos2tr(R)−1
至于转轴
n
\mathbf n
n,把它看作普通的向量,用它对应的旋转矩阵
R
\mathbf R
R去左乘旋转它,它不会发生任何变化,于是有下式:
R
n
=
n
\mathbf R \mathbf n =\mathbf n
Rn=n
因此,转轴
n
\mathbf n
n是矩阵
R
\mathbf R
R的特征值1对应的特征向量,用求特征向量的方法去求即可,再归一化可以得到转轴
n
\mathbf n
n:
R
n
=
n
s.t.
n
T
n
=
1
\mathbf R \mathbf n = \mathbf n \qquad \textit{s.t.} \qquad \mathbf n^{\mathrm T} \mathbf n = 1
Rn=ns.t.nTn=1
四、单位四元数—>旋转矩阵
用四元数
q
\mathbf q
q对纯虚四元数数表示的空间点
p
=
[
0
,
v
p
T
]
T
\mathbf p= [0,\mathbf v_p^{\mathrm T}]^{\mathrm T}
p=[0,vpT]T进行旋转有:
p
′
=
q
p
q
−
1
=
q
+
p
+
q
−
1
=
q
+
(
q
−
1
)
⊕
p
=
[
s
−
v
T
v
s
I
+
v
∧
]
[
s
v
T
−
v
s
I
+
v
∧
]
[
0
v
p
]
=
[
s
2
+
v
T
v
s
v
T
−
s
v
T
−
v
T
v
∧
s
v
−
s
v
−
v
∧
v
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
]
[
0
v
p
]
=
[
1
(
v
∧
v
)
T
0
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
]
[
0
v
p
]
=
[
1
0
T
0
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
]
[
0
v
p
]
=
[
0
(
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
)
v
p
]
\begin{split} \mathbf p^{'} & = \mathbf q \mathbf p \mathbf q^{-1} \\ & = \mathbf q^{+} \mathbf p^{+} \mathbf q^{-1} \\ & = \mathbf q^{+} (\mathbf q^{-1})^{\oplus} \mathbf p \\ & = \begin{bmatrix} s & -\mathbf v^{\mathrm T} \\ \mathbf v & s \mathbf I + \mathbf v^{\land} \end{bmatrix} \begin{bmatrix} s & \mathbf v^{\mathrm T} \\ -\mathbf v & s \mathbf I + \mathbf v^{\land} \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} s^2 + \mathbf v^{\mathrm T} \mathbf v & s\mathbf v^{\mathrm T} -s\mathbf v^{\mathrm T} - \mathbf v^{\mathrm T} \mathbf v^{\land} \\ s\mathbf v - s\mathbf v - \mathbf v^{\land} \mathbf v& \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 1 & (\mathbf v^{\land} \mathbf v)^{\mathrm T} \\ \mathbf 0 & \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 1 & \mathbf 0^{\mathrm T} \\ \mathbf 0 & \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{bmatrix} \begin{bmatrix} 0 \\ \mathbf v_{p} \end{bmatrix} \\ & = \begin{bmatrix} 0 \\ (\mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2) \mathbf v_{p}\end{bmatrix} \end{split}
p′=qpq−1=q+p+q−1=q+(q−1)⊕p=[sv−vTsI+v∧][s−vvTsI+v∧][0vp]=[s2+vTvsv−sv−v∧vsvT−svT−vTv∧vvT+s2I+2sv∧+(v∧)2][0vp]=[10(v∧v)TvvT+s2I+2sv∧+(v∧)2][0vp]=[100TvvT+s2I+2sv∧+(v∧)2][0vp]=[0(vvT+s2I+2sv∧+(v∧)2)vp]
上面可以看出被单位四元数旋转后的坐标点(纯虚四元数)依然是一个纯虚四元数
由此可得单位四元数到旋转矩阵的变换:
R
=
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
\begin{equation} \mathbf R = \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 \end{equation}
R=vvT+s2I+2sv∧+(v∧)2
五、旋转矩阵—>单位四元数
将(3)式两边取迹:
t
r
(
R
)
=
v
T
v
+
3
s
2
+
0
+
t
r
(
(
v
∧
)
2
)
\mathrm tr(\mathbf R) = \mathbf v^{\mathrm T} \mathbf v + 3s^2 + 0 + \mathrm tr((\mathbf v^{\land})^2)
tr(R)=vTv+3s2+0+tr((v∧)2)
其中: ( v ∧ ) 2 = [ 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ] [ 0 − v 3 v 2 v 3 0 − v 1 − v 2 v 1 0 ] = [ − v 2 2 − v 3 2 ⋯ ⋯ ⋯ − v 1 2 − v 3 2 ⋯ ⋯ ⋯ − v 1 2 − v 2 2 ] \begin{split} (\mathbf v^{\land})^2 & = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \\ & = \begin{bmatrix} -v_2^2 - v_3^2 & \cdots & \cdots \\ \cdots & -v_1^2-v_3^2 & \cdots \\ \cdots & \cdots & -v_1^2-v_2^2 \end{bmatrix} \end{split} (v∧)2=⎣ ⎡0v3−v2−v30v1v2−v10⎦ ⎤⎣ ⎡0v3−v2−v30v1v2−v10⎦ ⎤=⎣ ⎡−v22−v32⋯⋯⋯−v12−v32⋯⋯⋯−v12−v22⎦ ⎤
t
r
(
R
)
=
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{align} \mathrm tr(\mathbf R) & = v_1^2 + v_2^2 + v_3^2 + 3s^2 - 2(v_1^2 + v_2^2 + v_3^2) \notag \\ & = 3s^2 - (v_1^2 + v_2^2 + v_3^2) \notag \\ & = 3s^2 - (1 - s^2) \notag \\ & = 4s^2 - 1 \end{align}
tr(R)=v12+v22+v32+3s2−2(v12+v22+v32)=3s2−(v12+v22+v32)=3s2−(1−s2)=4s2−1
稍作变换(4)式可以从旋转矩阵
R
\mathbf R
R中求得单位四元数的实部:
s
=
t
r
(
R
)
+
1
4
s = \sqrt {\frac {\mathrm tr(\mathbf R) +1}{4}}
s=4tr(R)+1
下面开始求虚部,将虚部当作待旋转的坐标点带入(3)式有:
R
v
=
(
v
v
T
+
s
2
I
+
2
s
v
∧
+
(
v
∧
)
2
)
v
=
v
v
T
v
+
s
2
v
+
0
+
0
=
(
1
−
s
2
)
v
+
s
2
v
=
v
\begin{split} \mathbf R \mathbf v & = (\mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2) \mathbf v \\ & = \mathbf v \mathbf v^{\mathrm T} \mathbf v + s^2 \mathbf v + 0 + 0 \\ & = (1 - s^2) \mathbf v + s^2 \mathbf v \\ & = \mathbf v \end{split}
Rv=(vvT+s2I+2sv∧+(v∧)2)v=vvTv+s2v+0+0=(1−s2)v+s2v=v
由此可见虚部也是矩阵
R
\mathbf R
R的特征值1对应的特征向量,即与旋转向量
θ
n
\theta \mathbf n
θn在
R
3
\mathbb R^3
R3中指向同一个方向,但是他们之间的模不一样,旋转向量的模是量转角
θ
\theta
θ,这里虚部的模由
s
2
+
v
T
v
=
1
s^2 + \mathbf v^{\mathrm T} \mathbf v = 1
s2+vTv=1确定。故由旋转矩阵求虚部为
R
v
=
v
s.t.
s
2
+
v
T
v
=
1
\mathbf R \mathbf v = \mathbf v \qquad \textit{s.t.} \qquad s^2 + \mathbf v^{\mathrm T} \mathbf v = 1
Rv=vs.t.s2+vTv=1
六、单位四元数<—>旋转向量
由式(2)与式(4)可得:
t
r
(
R
)
=
2
cos
θ
+
1
=
4
s
2
−
1
\mathrm tr(\mathbf R) = 2 \cos \theta +1 = 4s^2 -1
tr(R)=2cosθ+1=4s2−1
即
cos
θ
=
2
s
2
−
1
\cos \theta = 2s^2 -1
cosθ=2s2−1
又有三角函数关系
cos
θ
=
2
cos
2
θ
2
−
1
\cos \theta = 2\cos ^2 \frac{\theta}{2} -1
cosθ=2cos22θ−1
于是有实部与转角的相互转换:
s
=
cos
θ
2
或
θ
=
2
arccos
s
s = \cos \frac{\theta}{2} \quad \text{或} \quad\theta = 2 \arccos s
s=cos2θ或θ=2arccoss
刚刚已经分析,单位四元数的虚部
v
\mathbf v
v与旋转向量的转轴
n
\mathbf n
n在
R
3
\mathbb R^3
R3中指向同一个方向,区别是他们的模服从不同的约束。单位四元数转换为转轴
n
\mathbf n
n:
n
=
v
∣
v
∣
=
v
1
−
s
2
=
v
1
−
cos
2
θ
2
=
v
sin
θ
2
\mathbf n = \frac {\mathbf v}{|\mathbf v|} = \frac {\mathbf v}{\sqrt{1 - s^2}} = \frac {\mathbf v}{\sqrt{1 - \cos^2 \frac {\theta}{2}}} = \frac {\mathbf v}{\sin \frac {\theta}{2}}
n=∣v∣v=1−s2v=1−cos22θv=sin2θv
旋转向量转换为虚部
v
\mathbf v
v:
v
=
sin
θ
2
n
=
1
−
s
2
n
\mathbf v = \sin \frac {\theta}{2} \mathbf n = \sqrt{1 - s^2} \mathbf n
v=sin2θn=1−s2n
总结
旋转矩阵 R \mathbf R R | 旋转向量 θ n \theta \mathbf n θn | 单位四元数 q = [ s , v ] \mathbf q = [s, \mathbf v] q=[s,v] | |
---|---|---|---|
旋转矩阵 R \mathbf R R | R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ \mathbf R = \cos \theta \mathbf I + (1 - \cos \theta) \mathbf n \mathbf n^{\mathrm T} + \sin \theta \mathbf n^{\land} R=cosθI+(1−cosθ)nnT+sinθn∧ | R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 \mathbf R = \mathbf v \mathbf v^{\mathrm T} + s^2 \mathbf I + 2s \mathbf v^{\land} + (\mathbf v^{\land})^2 R=vvT+s2I+2sv∧+(v∧)2 | |
旋转向量 θ n \theta \mathbf n θn | θ = arccos t r ( R ) − 1 2 R n = n s.t. n T n = 1 \theta = \arccos \frac {\mathrm tr(\mathbf R) - 1} {2} \\ \mathbf R \mathbf n = \mathbf n \quad \textit{s.t.} \quad \mathbf n^{\mathrm T} \mathbf n = 1 θ=arccos2tr(R)−1Rn=ns.t.nTn=1 | θ = 2 arccos s n = v ∣ v ∣ = v 1 − s 2 = v sin θ 2 \quad\theta = 2 \arccos s \\ \mathbf n = \frac {\mathbf v}{\lvert \mathbf v \rvert} = \frac {\mathbf v}{\sqrt{1 - s^2}} = \frac {\mathbf v}{\sin \frac {\theta}{2}} θ=2arccossn=∣v∣v=1−s2v=sin2θv | |
单位四元数 q = [ s , v ] \mathbf q = [s, \mathbf v] q=[s,v] | s = t r ( R ) + 1 4 R v = v s.t. s 2 + v T v = 1 s = \sqrt {\frac {\mathrm tr(\mathbf R) +1}{4}} \\ \mathbf R \mathbf v = \mathbf v \quad \textit{s.t.} \quad s^2 + \mathbf v^{\mathrm T} \mathbf v = 1 s=4tr(R)+1Rv=vs.t.s2+vTv=1 | s = cos θ 2 v = sin θ 2 n = 1 − s 2 n s = \cos \frac{\theta}{2} \\ \mathbf v = \sin \frac {\theta}{2} \mathbf n = \sqrt{1 - s^2} \mathbf n s=cos2θv=sin2θn=1−s2n |