三维空间的刚体运动【小白学视觉SLAM（一）】

1、点与坐标系

1.1 右手坐标系和左手坐标系

左手坐标系和右手坐标系是三维空间中定义坐标轴方向的两种约定，它们的核心区别在于第三个轴（通常是 Z 轴）的方向。

右手坐标系：伸出右手，四指从 X 轴方向弯向 Y 轴方向，此时大拇指指向的方向就是 Z 轴的正方向。在上图中，Z 轴指向屏幕外（朝向你）。

左手坐标系：同样的动作换成左手。四指从 X 轴弯向 Y 轴，大拇指指向的 Z 轴正方向变成了屏幕内（远离你）。

两者的 X 轴和 Y 轴方向完全相同，唯一的区别就是 Z 轴的朝向相反。这个差异看起来微小，但在实际应用中影响很大——它决定了叉积的方向、旋转的正方向（顺时针还是逆时针），以及法向量的朝向。

常见的使用场景：OpenGL、Vulkan、大多数物理和数学教材使用右手坐标系；DirectX 和 Unity 使用左手坐标系。在跨系统协作时，坐标系不一致是常见的 bug 来源，通常需要对 Z 轴取反来做转换。

1.2 外积和内积

1.2.1 外积

外积的定义和公式：

外积的几何意义：

1.2.2 内积

定义和公式：

内积的几何意义：

1.3 旋转矩阵

1.3.1 一次旋转

坐标系( e 1 , e 2 , e 3 ) (e_{1},e_{2},e_{3})(e1​,e2​,e3​)发生了旋转变成了( e 1 ′ , e 2 ′ , e 3 ′ ) (e^{\prime}_1,e^{\prime}_2,e^{\prime}_3)(e1′​,e2′​,e3′​)，向量a aa不动，那么它的坐标如何变化？因为( e 1 , e 2 , e 3 ) (e_{1},e_{2},e_{3})(e1​,e2​,e3​)是单位向量，等式两侧各乘以[ e 1 T , e 2 T , e 3 T ] [e^T_{1},e^T_{2},e^T_{3}][e1T​,e2T​,e3T​]，

R RR被称为旋转矩阵。
可以验证：
R RR是一个正交矩阵（正交矩阵满足Q T Q = Q Q T Q^TQ=QQ^TQTQ=QQT，即它的转置等于它的逆，Q − 1 = Q T Q^{-1}=Q^TQ−1=QT），R RR的行列式为+1。满足这两个性质的矩阵称为旋转矩阵。

满足这两个性质的矩阵也可以叫做Special Orthogonal Group（ 特殊正交群），

于是，1到2的旋转可以表达为：
a 1 = R 12 a 2 a_1=R_{12}a_2a1​=R12​a2​
反之，
a 2 = R 21 a 1 a_2=R_{21}a_1a2​=R21​a1​
矩阵关系：
R 21 = R 12 − 1 = R 12 T R_{21}=R^{-1}_{12}=R^{T}_{12}R21​=R12−1​=R12T​

1.3.2 旋转+平移

旋转+平移：
a ′ = R a + t a^\prime=Ra+ta′=Ra+t

两个坐标系间的运动可用R , t R,tR,t完全描述。

欧拉旋转定理（Euler’s rotation theorem）：刚体在三维空间里的一般运动，可分解为刚体上方某一点的平移，以及绕经过此点的旋转轴的转动。

1.3.3 齐次坐标与变换矩阵

旋转加平移在表达复合情况下有不便之处：

齐次形式（Homogeneous），加1之后由3维矩阵变为4维矩阵：

变换矩阵：

齐次坐标的性质：

变换矩阵的集合称为特殊欧氏群 SE(3) （Special Euclidean Group）。

逆形式：
T − 1 = [ R T − R T t 0 T 1 ] T^{-1}=\left[\begin{matrix} R^{T} & -R^{T}t \\ 0^T & 1 \end{matrix}\right]T−1=[RT0T​−RTt1​]

1.4 旋转向量和欧拉角

1.4.1 旋转向量

• 除了旋转矩阵/变换矩阵之外，还存在其他的表示方式。
• 旋转矩阵 R 有九个元素，但仅有三个自由度。
• 能否以更少的元素表达旋转？

旋转向量：

• 方向为旋转轴，长度为转过的角度，称为角轴/轴角（Angle Axis）或旋转向量（Rotation Vector）。

旋转向量与矩阵的不同：

• 仅有三个量
• 无约束
• 更直观
  它们可以是同一个东西的不同表达方式。

罗德里格斯公式（Rodrigues’s Formula），其中R RR是旋转矩阵，n nn是旋转向量的方向，θ \thetaθ是旋转向量的角度：
R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n ∧ R=cos{\theta}I+(1-cos\theta)nn^T+sin{\theta}n^\wedgeR=cosθI+(1−cosθ)nnT+sinθn∧

旋转矩阵转向量：

• 角度：
  θ = a r c c o s ( t r ( R ) − 1 2 ) \theta=arccos(\frac{tr(R)-1}{2})θ=arccos(2tr(R)−1​)
• 轴：
  R n = n Rn=nRn=n

1.4.2 欧拉角（Euler Angles）

• 将旋转分解为三个方向上的转动
• 例，按Z-Y-X顺序转动
• 轴可以是定轴或动轴，顺序亦可不同
• 常见的有：yaw-pitch-roll，东北天
• 不同领域的习惯有所不同

绕物体的Z轴旋转，得到偏航角yaw；
绕旋转之后的Y轴旋转，得到俯仰角pitch；
绕旋转之后的X轴旋转，得到滚转角roll。

1.4.3 欧拉角遇到的问题

万向锁（Gimbal Lock）：

• 欧拉角的奇异性问题。
• 在特定值时，旋转自由度减1。
• Yaw-pitch-roll顺序下，当pitch为90度时，存在奇异性。

正常情况：
下面的白色棍状物体为飞机。

奇异情况：

由于万向锁的存在，欧拉角不适合插值或迭代。

• 多用于人机交互中。
• 可以证明：仅用三个实数表达旋转时，不可避免地存在奇异性问题。
• SLAM中亦很少用欧拉角表达姿态。

1.5 四元数

1.5.1 概述

2D 情况下，可用单位复数表达旋转，
z = x + i y = ρ e i θ z=x+iy=\rho e^{i \theta}z=x+iy=ρeiθ
乘i ii即转90度，乘– i –i–i转-90度。

三维情况下，四元数可作为复数的扩充。
q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3kq=q0​+q1​i+q2​j+q3​k
四元数（Quaternion）

• 有三个虚部和一个实部
• 虚部之间满足关系：

{ 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 \begin{cases} i^2=j^2=k^2=-1 \\ ij=k,ji=-k \\ jk=i,kj=-i \\ki=j,ik=-j \end{cases}⎩⎨⎧​i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j​
自己和自己的运算像复数，自己和别人的运算像叉乘。

1.5.2 四元数与旋转

单位四元数可表达旋转，

为理解旋转的计算方式，先看四元数间如何运算。
四元数的运算：

1.5.3 角轴与四元数

四元数到角轴：

角轴到四元数：

1.5.4 如何用四元数旋转一个空间点

设点p pp经过一次以q qq表示的旋转后，得到了p ′ p^{\prime}p′，它们关系如何表示？
将p pp的坐标用四元数表示（虚四元数）：
p = [ 0 , x , y , z ] = [ 0 , v ] p=[0,x,y,z]=[0,v]p=[0,x,y,z]=[0,v]

旋转之后的关系为：
p ′ = q p q − 1 p^{\prime}=qpq^{-1}p′=qpq−1

四元数相比于角轴、欧拉角的优势：紧凑、无奇异性。