三维空间刚体运动

三维空间刚体运动

三维空间中的刚体运动涉及到刚体在空间中的位置（translation）和方向（orientation）的变化。在机器视觉和机器人领域，尤其是在同时定位与地图构建（SLAM，Simultaneous Localization and Mapping）的应用中，精确地描述和计算这种运动是至关重要的。下面是对三维空间中刚体运动的详细介绍，特别是在SLAM的背景下。

位姿 (Pose)

位姿描述了一个刚体在三维空间中的位置和方向。通常，位姿由一个三维向量和一个旋转矩阵（或四元数）来表$t$（通常是$x,y,z$ 坐标）描述了刚体相对于参考坐标系的位置。

• 旋转矩阵 $R$ 或 四元数 表示刚体相对于参考坐标系的方向。

刚体变换 (Rigid-body Transformation)

刚体变换通常使用齐次变换矩阵来表示，该矩阵结合了旋转和平移：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]$$
其中，$R$ 是一个$3\times 3$ 旋转矩阵，$t$ 是一个三维平移向量。

旋转向量

旋转向量（也称为轴角表示）由一个向量表示，该向量的方向定义了旋转的轴，而其长度（模长）定义了绕该轴的旋转角度（以弧度为单位）。如果有一个单位向量 $u=(u_x,u_y,u_z)$ 和一个角度 $\theta$，则旋转向量 $r$ 可以表示为：
$$\mathbf{r}=\theta \mathbf{u}$$

从旋转向量到旋转矩阵

使用罗德里格斯公式（Rodrigues’ formula），可以将一个旋转向量转换成一个等价的旋转矩阵。罗德里格斯公式定义如下：
$$\mathbf{R}=\mathbf{I}+\sin (\theta )\mathbf{K}+(1-\cos (\theta )){\mathbf{K}}^2$$
其中：

• $I$ 是单位矩阵。
• $\theta$ 是旋转向量的模长，表示旋转角度。
• $K$ 是旋转轴 $u$ 的斜对称矩阵，定义为：
  $$\mathbf{K}=\left[\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right]$$

斜对称矩阵$K$

这个矩阵的每个元素都与向量 $u$ 的坐标有关，它们的意义可以从向量积的角度来解释。斜对称矩阵的作用可以看作是对任意向量 $\mathbf{v}=(v_x,v_y,v_z)$ 实施向量积操作，即 $\mathbf{u}\times \mathbf{v}$。具体来说：

• 第一行 $[0,-u_z,u^{}$