三维空间的旋转
三维空间中两个坐标系之间的旋转关系可以通过旋转矩阵,旋转向量(角轴),欧拉角,四元数表示。
1 旋转矩阵
设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1, e_2, e_3) (e1,e2,e3)经过一次旋转变成了 ( e 1 ′ , e 2 ′ , e 3 ′ ) (e_1^{'}, e_2^{'}, e_3^{'}) (e1′,e2′,e3′),那么对于同一个向量 a \boldsymbol{a} a(注意该向量并没有随着坐标系的旋转而发生运动),它在两个坐标系下的坐标为 ( a 1 , a 2 , a 3 ) T (a_1, a_2, a_3)^T (a1,a2,a3)T 和 ( a 1 ‘ , a 2 ‘ , a 3 ‘ ) T (a_1^{‘}, a_2^{‘}, a_3^{‘})^T (a1‘,a2‘,a3‘)T。根据坐标系的定义,:
[ e 1 , e 2 , e 3 ] [ a 1 a 2 a 3 ] = [ e 1 ′ , e 2 ′ , e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] ( 1 ) \begin{bmatrix} e_{1},e_{2},e_{3} \end{bmatrix}\begin{bmatrix} a_{1}\\ a_{2}\\ a_{3} \end{bmatrix}=\begin{bmatrix} e_{1}^{'} ,e_{2}^{'},e_{3}^{'} \end{bmatrix}\begin{bmatrix} a_{1}^{'}\\ a_{2}^{'}\\ a_{3}^{'} \end{bmatrix} (1) [e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤(1)
为描述两个坐标之间的关系,(1)左右两边同时乘以 ( e 1 T , e 2 T , e 3 T ) T (e_1^T, e_2^T, e_3^T)^T (e1T,e2T,e3T)T,则有:
[ a 1 a 2 a 3 ] = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 1 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] [ a 1 ′ a 2 ′ a 3 ′ ] = R a ′ ( 2 ) \left[ \begin{array}{c} a_1\\a_2\\a_3 \end{array} \right]= \left[ \begin{array}{ccc} e_1^{T}e_1{'} & e_1^Te_2^{'} & e_1^Te_3^{'}\\ e_2^{T}e_1^{'} & e_2^Te_2^{'} & e_1^Te_3^{'}\\ e_3^{T}e_1^{'} & e_3^Te_2^{'} & e_3^Te_3^{'} \end{array} \right] \left[ \begin{array}{c} a_1^{'}\\a_2^{'}\\a_3^{'} \end{array} \right] = R\boldsymbol{a^{'}}(2) ⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e1Te3′e3Te3′⎦⎤⎣⎡a1′a2′a3′⎦⎤=Ra′(2)
令: R = [ e 1 T e 1 ′ e 1 T e 2 ′ e 1 T e 3 ′ e 2 T e 1 ′ e 2 T e 2 ′ e 1 T e 3 ′ e 3 T e 1 ′ e 3 T e 2 ′ e 3 T e 3 ′ ] R= \left[\begin{array}{ccc} e_1^{T}e_1{'} & e_1^Te_2^{'} & e_1^Te_3^{'}\\ e_2^{T}e_1^{'} & e_2^Te_2^{'} & e_1^Te_3^{'}\\ e_3^{T}e_1^{'} & e_3^Te_2^{'} & e_3^Te_3^{'} \end{array}\right] R=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e1Te3′e3Te3′⎦⎤(3)
矩阵
R
R
R描述了坐标系之间的旋转,称为旋转矩阵。
旋转矩阵是行列式为1的正交矩阵,旋转矩阵的集合定义如下:
S
O
(
n
)
=
{
R
∈
R
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
I
}
(
4
)
SO(n) = \{R \in \mathbb{R}^{n\times n} | RR^T = I, det(R) = I \} (4)
SO(n)={R∈Rn×n∣RRT=I,det(R)=I}(4)
由于旋转矩阵是正交阵,它的逆(即转置)描述了一个相反的旋转,则有
a
‘
=
R
−
1
a
=
R
T
a
\boldsymbol{a^{‘}} = R^{-1}\boldsymbol{a}=R^T\boldsymbol{a}
a‘=R−1a=RTa。显然,
R
T
R^T
RT刻画了一个相反的旋转。
2 旋转向量(角轴,李代数)
w
w
w
坐标系的任意旋转都可以用一个旋转轴和旋转角来刻画。使用一个向量,其方向与旋转轴一致,而长度等于旋转角,这种向量称为旋转向量(角-轴)。
2.1旋转向量和旋转矩阵
假如一个旋转轴为
n
n
n,角度为
θ
\theta
θ的旋转,它对应的旋转向量为
θ
\theta
θ
n
n
n,旋转向量
w
=
n
θ
w=n\theta
w=nθ 到旋转矩阵的转换过程由罗德里格斯公式表明,转换公式如下:
R
=
cos
θ
I
+
(
1
−
cos
θ
)
n
n
T
+
sin
θ
[
n
]
X
(
5
)
R=\cos \theta I+(1-\cos \theta )nn^{T}+\sin \theta\begin{bmatrix}n\end{bmatrix}_{X} (5)
R=cosθI+(1−cosθ)nnT+sinθ[n]X(5)
[ ] X \begin{bmatrix} &\end{bmatrix}_{X} []X:表示反对称矩阵
[ W ] X = [ 0 − W 3 − W 2 W 3 0 − W 1 − W 2 W 1 0 ] ( 6 ) \begin{bmatrix} W\end{bmatrix}_{X}=\begin{bmatrix} 0&-W_{3}&-W_{2} \\ W_{3} &0& -W_{1}&\\-W_{2}& W_{1}&0 \end{bmatrix} (6) [W]X=⎣⎡0W3−W2−W30W1−W2−W10⎦⎤(6)
对于转角 θ \theta θ,有:
t r ( R ) = c o s θ t r ( I ) + ( 1 − c o s θ ) t r ( n n T ) + s i n θ t r ( [ n ] × ) = 3 c o s θ + ( 1 − c o s θ ) = 1 + 2 c o s θ ( 7 ) tr(R) = cos\theta\ tr(I) + (1-cos\theta)tr(\boldsymbol{n}\boldsymbol{n}^T) + sin\theta\ tr([\boldsymbol{n}]_{\times})\\ = 3cos\theta + (1-cos\theta) = 1 + 2cos\theta (7) tr(R)=cosθ tr(I)+(1−cosθ)tr(nnT)+sinθ tr([n]×)=3cosθ+(1−cosθ)=1+2cosθ(7)
因此 θ = a r c c o s t r ( R ) − 1 2 \theta = arc\ cos\frac{tr(R) - 1}{2} θ=arc cos2tr(R)−1 (8)
计算转轴
n
\boldsymbol{n}
n,由于旋转轴上的向量在旋转后不发生改变,有
R
n
=
n
R\boldsymbol{n} = \boldsymbol{n}
Rn=n。因此,转轴
n
\boldsymbol{n}
n 是矩阵
R
R
R 特征值为1对应的特征向量。求解次方程,再归一化,就得到了转轴。
2.2 旋转向量表示旋转
对于三维空间点
p
=
[
x
y
z
]
p=\begin{bmatrix}x & y&z \end{bmatrix}
p=[xyz]
罗德里格斯旋转公式:
p
r
o
t
=
p
c
o
s
θ
+
(
n
×
p
)
s
i
n
θ
+
n
(
n
⋅
p
)
(
1
−
c
o
s
θ
)
 
(
9
)
\boldsymbol{p}_{rot} = \boldsymbol{p}cos\theta + (\boldsymbol{n}\times \boldsymbol{p})sin\theta + \boldsymbol{n}(\boldsymbol{n} \cdot \boldsymbol{p})(1-cos\theta) \:(9)
prot=pcosθ+(n×p)sinθ+n(n⋅p)(1−cosθ)(9)
实际中,使用轴角表示法对三维点进行旋转会分两种情况:
∣
∣
w
∣
∣
||\boldsymbol{w}||
∣∣w∣∣ 相比0较大时。这个时候可以通过罗德里格斯旋转公式计算
∣
∣
w
∣
∣
||\boldsymbol{w}||
∣∣w∣∣ 相比0较小时。根据罗德里格斯公式,
R
=
c
o
s
∣
∣
w
∣
∣
I
+
[
w
]
×
∣
∣
w
∣
∣
s
i
n
(
∣
∣
w
∣
∣
)
+
[
w
]
×
2
∣
∣
w
∣
∣
2
(
1
−
c
o
s
(
∣
∣
w
∣
∣
)
)
R = cos||\boldsymbol{w}||I + \frac{[\boldsymbol{w}]_{\times}}{||\boldsymbol{w}||} sin(||\boldsymbol{w}||) + \frac{[\boldsymbol{w}]_{\times}^2}{||\boldsymbol{w}||^2}(1 - cos(||\boldsymbol{w}||))
R=cos∣∣w∣∣I+∣∣w∣∣[w]×sin(∣∣w∣∣)+∣∣w∣∣2[w]×2(1−cos(∣∣w∣∣))
当
w
\boldsymbol{w}
w较小时,
c
o
s
(
∣
∣
w
∣
∣
)
≈
1
,
s
i
n
(
∣
∣
w
∣
∣
)
≈
∣
∣
w
∣
∣
cos(||\boldsymbol{w}||) \approx 1, sin(||\boldsymbol{w}||) \approx ||\boldsymbol{w}||
cos(∣∣w∣∣)≈1,sin(∣∣w∣∣)≈∣∣w∣∣。因此,
R
≈
I
+
[
w
]
×
R \approx I + [\boldsymbol{w}]_{\times}
R≈I+[w]×。于是,
R
p
=
p
+
[
w
]
×
p
R\boldsymbol{p} = \boldsymbol{p} + [\boldsymbol{w}]_{\times} \boldsymbol{p}
Rp=p+[w]×p
注: 对于李代数
w
=
θ
n
\boldsymbol{w} = \theta \boldsymbol{n}
w=θn,其中
n
\boldsymbol{n}
n为单位转轴,因此
∣
∣
n
∣
∣
2
=
1
||\boldsymbol{n}||^2 = 1
∣∣n∣∣2=1,于是
∣
∣
w
∣
∣
2
=
θ
2
∣
∣
n
∣
∣
2
=
θ
2
||\boldsymbol{w}||^2 = \theta^2 ||\boldsymbol{n}||^2 = \theta^2
∣∣w∣∣2=θ2∣∣n∣∣2=θ2。因此,有
θ
=
∣
∣
w
∣
∣
\theta = ||\boldsymbol{w}||
θ=∣∣w∣∣
3 欧拉角
把旋转分解成3次绕不同轴旋转,每次旋转的角度构成欧拉角。旋转时,旋转轴有先后顺序,可以先绕X轴,再绕Y轴,最后绕Z轴,即XYZ轴的旋转,同理可以定义ZYX,ZYX等。欧拉角在摄影测量中应用比较广泛,常见的转角系统有rpy角度系统(ZYX),OPK角度系统(XYZ)。
欧拉角的一个重大缺点是会碰到万向锁问题。
3.1 欧拉角和旋转矩阵
opk角度系统
4 四元数
单位四元数可以表示三维空间的旋转。
四元数:
q
=
(
c
,
v
)
=
(
q
0
,
q
1
,
q
2
,
q
3
)
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
\boldsymbol{q} = (c, \boldsymbol{v}) = (q_0, q_1, q_2, q_3) = q_0 + q_1\boldsymbol{i} + q_2\boldsymbol{j} + q_3\boldsymbol{k}
q=(c,v)=(q0,q1,q2,q3)=q0+q1i+q2j+q3k
三个虚部满足以下关系式:
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
i^2 = j^2 = k^2 = -1\\ ij = k,\ ji = -k,\\ jk = i,\ kj = -i,\\ ki = j,\ ik = -j
i2=j2=k2=−1ij=k, ji=−k,jk=i, kj=−i,ki=j, ik=−j
4.1 四元数和旋转向量
假设旋转绕单位向量
n
=
(
n
x
,
x
y
,
n
z
)
T
\boldsymbol{n} = (n_x, x_y, n_z)^T
n=(nx,xy,nz)T 进行了角度为
θ
\theta
θ 的旋转,则该旋转的四元数定义为:
q = ( c o s θ 2 , n x s i n θ 2 , n y s i n θ 2 , n z s i n θ 2 ) T \boldsymbol{q} = (cos\frac{\theta}{2},\ n_xsin\frac{\theta}{2},\ n_ysin\frac{\theta}{2},\ n_zsin\frac{\theta}{2})^T q=(cos2θ, nxsin2θ, nysin2θ, nzsin2θ)T (9)
令 θ = θ + 2 π \theta = \theta + 2\pi θ=θ+2π,则 q = ( − c o s θ 2 , − n x sin θ 2 , − n y s i n θ 2 , − n z s i n θ 2 ) T = − q \boldsymbol{q} = (-cos\frac{\theta}{2}, -n_x\sin\frac{\theta}{2}, -n_ysin\frac{\theta}{2}, -n_zsin\frac{\theta}{2})^T = -\boldsymbol{q} q=(−cos2θ,−nxsin2θ,−nysin2θ,−nzsin2θ)T=−q。即 q \boldsymbol{q} q 和 − q -\boldsymbol{q} −q 表示同一个旋转。
t h e t a = 2 a r c c o s q 0 , ( n x , n y , n z ) T = 1 s i n θ 2 ( q 1 , q 2 , q 3 ) T = 1 ∣ ∣ n ∣ ∣ ( q 1 , q 2 , q 3 ) T ( 10 ) theta = 2arc cos\ q_0,\\ (n_x, n_y, n_z)^T = \frac{1}{sin\frac{\theta}{2}} (q_1, q_2, q_3)^T = \frac{1}{||\boldsymbol{n}||} (q_1, q_2, q_3)^T(10) theta=2arccos q0,(nx,ny,nz)T=sin2θ1(q1,q2,q3)T=∣∣n∣∣1(q1,q2,q3)T(10)
4.2 四元数和旋转矩阵
R = [ R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 ] ( 11 ) R = \left[ \begin{array}{ccc} R_{11} & R_{12} & R_{13}\\ R_{21} & R_{22} & R_{23}\\ R_{31} & R_{32} & R_{33} \end{array} \right] (11) R=⎣⎡R11R21R31R12R22R32R13R23R33⎦⎤(11)
则旋转矩阵到四元数的转换如下:
q 0 = t r ( 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{tr(R) + 1}}{2},\ q_1 = \frac{R_{23} - R_{32}}{4q_0},\ q_2 = \frac{R_{31} - R_{13}}{4q_0},\ q_3 = \frac{R_{12} - R{21}}{4q_0} q0=2tr(R)+1, q1=4q0R23−R32, q2=4q0R31−R13, q3=4q0R12−R21 (12)
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 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 ] ( 13 ) R=\begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} &2q_{1}q _{2}-2q_{0}q_{3} & 2q_{1}q_{3}+2q_{0}q_{2}\\ 2q_{1}q_{2}+2q_{0}q_{3}& 1-2q_{1}^{2} -2_{3}^{2}&2q_{2} q_{3}-2q_{0}q_{1}\\ 2q_{1}q_{3}-2q_{0}q_{2}&2q_{2}q_{3}+2q_{0}q_{1} & 1-2q_{1}^{2}-2q_{2}^{2} \end{bmatrix} (13) R=⎣⎡1−2q22−2q322q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q12−2322q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q12−2q22⎦⎤(13)
4.3 用四元数表示旋转
假设一个三维空间点
p
=
[
x
y
z
]
∩
∈
R
3
p=\begin{bmatrix}x & y&z \end{bmatrix}\cap \in \mathbb{R}^{3}
p=[xyz]∩∈R3,三维点
p
p
p经过旋转之后变成
p
′
p^{'}
p′
首先,把三维空间点用一个虚四元数来描述:
p
=
[
0
x
y
z
]
=
[
0
v
]
p=\begin{bmatrix} 0 &x &y &z \end{bmatrix}=\begin{bmatrix} 0 &v \end{bmatrix}
p=[0xyz]=[0v]
那么旋转后的点
p
′
p^{'}
p′表示为:
p
′
=
q
p
p
−
1
p^{'}=qpp^{-1}
p′=qpp−1 (14)
参考:
[1]: https://aibluefisher.github.io/2019/04/16/rotation/
[2]:视觉SLAM十四讲 从理论到实践