三维空间刚体运动
三维空间中的刚体运动涉及到刚体在空间中的位置(translation)和方向(orientation)的变化。在机器视觉和机器人领域,尤其是在同时定位与地图构建(SLAM,Simultaneous Localization and Mapping)的应用中,精确地描述和计算这种运动是至关重要的。下面是对三维空间中刚体运动的详细介绍,特别是在SLAM的背景下。
位姿 (Pose)
位姿描述了一个刚体在三维空间中的位置和方向。通常,位姿由一个三维向量和一个旋转矩阵(或四元数)来表 t {t} t(通常是 x , y , z x, y, z x,y,z 坐标)描述了刚体相对于参考坐标系的位置。
- 旋转矩阵 R {R} R 或 四元数 表示刚体相对于参考坐标系的方向。
刚体变换 (Rigid-body Transformation)
刚体变换通常使用齐次变换矩阵来表示,该矩阵结合了旋转和平移:
T = [ R t 0 1 ] \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{bmatrix} T=[R0t1]
其中, R {R} R 是一个 3 × 3 3 \times 3 3×3 旋转矩阵, t {t} t 是一个三维平移向量。
旋转向量
旋转向量(也称为轴角表示)由一个向量表示,该向量的方向定义了旋转的轴,而其长度(模长)定义了绕该轴的旋转角度(以弧度为单位)。如果有一个单位向量 u = ( u x , u y , u z ) {u} = (u_x, u_y, u_z) u=(ux,uy,uz) 和一个角度 θ \theta θ,则旋转向量 r {r} r 可以表示为:
r = θ u \mathbf{r} = \theta \mathbf{u} r=θu
从旋转向量到旋转矩阵
使用罗德里格斯公式(Rodrigues’ formula),可以将一个旋转向量转换成一个等价的旋转矩阵。罗德里格斯公式定义如下:
R = I + sin ( θ ) K + ( 1 − cos ( θ ) ) K 2 \mathbf{R} = \mathbf{I} + \sin(\theta) \mathbf{K} + (1 - \cos(\theta)) \mathbf{K}^2 R=I+sin(θ)K+(1−cos(θ))K2
其中:
- I {I} I 是单位矩阵。
- θ \theta θ 是旋转向量的模长,表示旋转角度。
- K {K} K 是旋转轴 u {u} u 的斜对称矩阵,定义为:
K = [ 0 − u z u y u z 0 − u x − u y u x 0 ] \mathbf{K} = \begin{bmatrix} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{bmatrix} K= 0uz−uy−uz0uxuy−ux0
斜对称矩阵 K {K} K
这个矩阵的每个元素都与向量 u {u} u 的坐标有关,它们的意义可以从向量积的角度来解释。斜对称矩阵的作用可以看作是对任意向量 v = ( v x , v y , v z ) \mathbf{v} = (v_x, v_y, v_z) v=(vx,vy,vz) 实施向量积操作,即 u × v \mathbf{u} \times \mathbf{v} u×v。具体来说:
- 第一行 [ 0 , − u z , u y ] [0, -u_z, u_y] [0,−uz,u