三维空间刚体变换：旋转矩阵
通过对视觉SLAM十四讲第三章前半部分内容以及机器人学建模控制与视觉第三章位姿描述与齐次变换两本书的学习，整理了三维空间中旋转矩阵的表示方法。
点与坐标系
向量与坐标
空间中最基本的元素是点，点没有长度、体积。将两个点连接得到向量，向量表示由某点指向另一个点的有向线段。
对于一空间基底为
[
i
j
k
]
\begin{bmatrix}i&j&k\end{bmatrix}
[ijk]的坐标系，向量
a
a
a的坐标如下：
a
=
[
i
j
k
]
⋅
[
a
1
a
2
a
3
]
=
a
1
i
+
a
2
j
+
a
3
k
a=\begin{bmatrix}i&j&k\end{bmatrix}\cdot\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=a_1i+a_2j+a_3k
a=[ijk]⋅⎣⎡a1a2a3⎦⎤=a1i+a2j+a3k
称
[
a
1
a
2
a
3
]
T
\begin{bmatrix}a_1&a_2&a_3\end{bmatrix}^T
[a1a2a3]T为向量
a
a
a在此基底下的坐标，坐标的具体取值同向量本身以及坐标系的选取有关。
坐标系可根据由左手法制或右手法制得到而分左手系和右手系：
向量的内积用于描述向量间的投影关系：
a
⋅
b
=
a
T
b
=
∑
i
=
1
3
a
i
b
i
=
∣
a
∣
∣
b
∣
cos
⟨
a
,
b
⟩
a\cdot b=a^Tb=\sum^3_{i=1}a_ib_i=\vert a\vert\vert b\vert\cos\langle a,b\rangle
a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,b⟩
式中，
⟨
a
,
b
⟩
\langle a,b\rangle
⟨a,b⟩为向量
a
,
b
a,b
a,b间的夹角。
向量的外积为一个向量，其大小等于
∣
a
∣
∣
b
∣
sin
⟨
a
,
b
⟩
\vert a\vert\vert b\vert\sin\langle a,b\rangle
∣a∣∣b∣sin⟨a,b⟩，方向垂直于这两个向量：
a
×
b
=
∥
i
j
k
a
1
a
2
a
3
b
1
b
2
b
3
∥
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
=
a
∧
b
a\times b=\begin{Vmatrix}i&j&k\\a_1&a_2&a_3\\b_1&b_2&b_3\end{Vmatrix}=\begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}b=a^\wedge b
a×b=∥∥∥∥∥∥ia1b1ja2b2ka3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b
引入反对称矩阵，并用
a
∧
a^\wedge
a∧表示向量
a
a
a的反对称矩阵。任何向量仅有唯一一个反对称矩阵：
a
∧
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
a^\wedge=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}
a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤
坐标系与欧式变换
在机器人中，定义世界坐标系固定不动，而机器人坐标系跟随着机器人一起移动。同时对每个传感器（IMU、激光雷达、摄像头等）建立传感器坐标系。
设世界坐标系内某点坐标为
W
p
{}^Wp
Wp，上标
W
^W
W表示为参考坐标系为世界坐标系
{
W
}
\{W\}
{W}：
W
p
=
[
p
x
p
y
p
z
]
^Wp=\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}
Wp=⎣⎡pxpypz⎦⎤
由一个旋转加上一个平移得到的运动称为刚体运动。机器人相对于世界的移动就是一个刚体运动，两坐标系间相差一个欧式变换，可通过变换矩阵
T
T
T得到。
则向量 p p p相对于机器人坐标系的坐标可通过坐标 W p ^Wp Wp与变换矩阵 T T T得到。
旋转矩阵与旋转群
欧式变换由一个旋转和一个平移得到，首先仅考虑旋转运动。
旋转矩阵
设坐标系
{
A
}
\{A\}
{A}和坐标系
{
B
}
\{B\}
{B}，已知坐标系
{
B
}
\{B\}
{B}的基底为
[
x
C
y
C
z
C
]
T
\begin{bmatrix}x_C&y_C&z_C\end{bmatrix}^T
[xCyCzC]T，则可用其相对于
{
A
}
\{A\}
{A}的方向余弦组成的矩阵可表示坐标系
{
B
}
\{B\}
{B}相对
{
A
}
\{A\}
{A}的方位：
B
A
R
=
[
A
x
B
A
y
B
A
z
B
]
B
A
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
\begin{aligned} ^A_BR=&\begin{bmatrix}{}^Ax_B&{}^Ay_B&{}^Az_B\end{bmatrix}\\ ^A_BR=&\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\\\end{bmatrix} \end{aligned}
BAR=BAR=[AxBAyBAzB]⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
称
C
W
R
^W_CR
CWR为坐标系
{
B
}
\{B\}
{B}相对于坐标系
{
A
}
\{A\}
{A}的旋转矩阵。
旋转矩阵的九个元素中仅有三个为独立元素，这是因为基底的三分列向量相互垂直，且长度为1：
A
x
B
⋅
A
y
B
=
W
y
B
⋅
A
z
B
=
A
z
B
⋅
A
x
B
=
0
A
x
B
⋅
A
x
B
=
W
y
B
⋅
A
y
B
=
A
z
B
⋅
A
z
B
=
1
\begin{aligned} ^Ax_B\cdot{}^Ay_B=^Wy_B\cdot{}^Az_B=^Az_B\cdot{}^Ax_B=0\\ ^Ax_B\cdot{}^Ax_B=^Wy_B\cdot{}^Ay_B=^Az_B\cdot{}^Az_B=1 \end{aligned}
AxB⋅AyB=WyB⋅AzB=AzB⋅AxB=0AxB⋅AxB=WyB⋅AyB=AzB⋅AzB=1
由此可得旋转矩阵为正交矩阵 ，满足如下条件：
{
B
A
R
−
1
=
B
A
R
T
d
e
t
(
B
A
R
)
=
1
\begin{cases} {}^A_BR^{-1}={}^A_BR^T\\ det({}^A_BR)=1 \end{cases}
{BAR−1=BARTdet(BAR)=1
单位旋转
对于一个坐标系绕自身的x轴、y轴、z轴旋转
θ
\theta
θ角的旋转矩阵如下：
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)=&\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix}\\ R(y,\theta)=&\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix}\\ R(z,\theta)=&\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}\\ \end{aligned}
R(x,θ)=R(y,θ)=R(z,θ)=⎣⎡1000cosθsinθ0−sinθcosθ⎦⎤⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤
旋转群
旋转矩阵 R ∈ ℜ 3 × 3 R\in\Re^{3\times3} R∈ℜ3×3满足如下约束条件：
- 正交条件 ：旋转矩阵的逆等于它的转置， R − 1 = R T R^{-1}=R^T R−1=RT
- 特殊条件： 旋转矩阵的行列式为一， d e t ( R ) = 1 det(R)=1 det(R)=1
将满足上述条件的旋转矩阵
R
∈
ℜ
3
×
3
R\in\Re^{3\times3}
R∈ℜ3×3的集合定义为旋转群
S
O
(
3
)
SO(3)
SO(3)：
S
O
(
3
)
=
{
R
∈
ℜ
3
×
3
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(3)=\bigl\{R\in\Re^{3\times3}\big\vert RR^T=I,det(R)=1\bigr\}
SO(3)={R∈ℜ3×3∣∣RRT=I,det(R)=1}
旋转群又叫特殊正交群（Special Orthogonal Group），可推广至
ℜ
n
×
n
\Re^{n\times n}
ℜn×n空间中：
S
O
(
n
)
=
{
R
∈
ℜ
n
×
n
∣
R
R
T
=
I
,
d
e
t
(
R
)
=
1
}
SO(n)=\bigl\{R\in\Re^{n\times n}\big\vert RR^T=I,det(R)=1\bigr\}
SO(n)={R∈ℜn×n∣∣RRT=I,det(R)=1}
其维数为
n
(
n
−
1
)
2
\frac{n(n-1)}{2}
2n(n−1)的流形，当
n
=
2
n=2
n=2时用于表示平面运动；当
n
=
3
n=3
n=3时用于表示空间转动。旋转群是李群，满足李群条件。
CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！
更多推荐
2
0- 0
已为社区贡献1条内容
所有评论(0)