旋转与姿态表示体系 (Rotation & Pose Representations)
1. 刚体运动基础与参考系约定
在机器人学中，刚体的位姿 (Pose) 由位置 (Position) 与姿态 (Orientation) 共同描述。为统一数学表述，需建立严格的坐标系约定：
| 符号 | 含义 | 说明 |
|---|---|---|
| $\{s\}$ | 空间/固定参考系 (Space/Fixed Frame) | 通常作为世界坐标系或基座标系 |
| $\{b\}$ | 物体/载体参考系 (Body Frame) | 固连在刚体上，随刚体运动 |
| $\mathbf{v}$ | 自由向量 (Free Vector) | 与坐标系无关的物理量 |
| $\mathbf{v}^s, \mathbf{v}^b$ | 向量在特定系下的坐标表示 | 同一向量在不同基下的投影分量 |
| $\hat{v}$ | 单位向量 (Hat Notation) | 模长为 1 的方向向量 |
默认约定：所有参考系均为右手系 (Right-Handed) 且假设为惯性系。长度尺度已预先选定。
2. 旋转矩阵与特殊正交群 $SO(3)$
2.1 数学定义
设 $\{b\}$ 系的三个正交单位基向量在 $\{s\}$ 系下的坐标为 $\hat{x}_b^s, \hat{y}_b^s, \hat{z}_b^s$，则旋转矩阵定义为：
$$ R_{sb} = \begin{bmatrix} \hat{x}_b^s & \hat{y}_b^s & \hat{z}_b^s \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \in \mathbb{R}^{3 \times 3} $$2.2 约束条件与群定义
$R_{sb}$ 必须满足 6 个正交约束与 1 个行列式约束，统称为 特殊正交群 (Special Orthogonal Group)：
$$ SO(3) = \{ R \in \mathbb{R}^{3 \times 3} \mid R^T R = I, \ \det(R) = 1 \} $$- $R^T R = I$ 保证列向量两两正交且模长为 1（保持长度与夹角不变）。
- $\det(R) = 1$ 保证右手定则（排除镜像反射）。
2.3 核心性质
| 性质 | 数学表达 | 工程意义 |
|---|---|---|
| 逆等于转置 | $R^{-1} = R^T$ | 求反向姿态计算代价极低 |
| 保范数性 | $\|R\mathbf{v}\| = \|\mathbf{v}\|$ | 纯旋转不改变向量长度 |
| 非交换性 | $R_1 R_2 \neq R_2 R_1$ | 旋转顺序直接影响最终姿态（$SO(2)$ 除外） |
| 群公理 | 封闭、结合、单位元 $I$、可逆 | 构成连续李群，支撑微分与积分运算 |
3. 旋转矩阵的三大应用法则
旋转矩阵在工程中具有双重身份：表示 (Representation) 与 操作 (Operator)。
| 应用场景 | 数学形式 | 物理含义 | 关键规则 |
|---|---|---|---|
| 1. 表示姿态 | $R_{sb}$ | 描述 $\{b\}$ 相对于 $\{s\}$ 的静态朝向 | 列向量即 $\{b\}$ 轴在 $\{s\}$ 下的坐标 |
| 2. 改变参考系 | $R_{sc} = R_{sb} R_{bc}$ $\mathbf{p}_s = R_{sb} \mathbf{p}_b$ |
坐标系连乘或向量坐标切换 | 下标消去法：相邻相同下标抵消，首尾保留 |
| 3. 同系内旋转 | $R'_{sb} = R \cdot R_{sb}$ (左乘) $R'_{sb} = R_{sb} \cdot R$ (右乘) |
在固定系或动系内执行旋转操作 | 左定右动：左乘绕固定系 $\{s\}$ 转，右乘绕动系 $\{b\}$ 转 |
注意：仅当矩阵带下标（如 $R_{sb}$）时，才适用下标消去规则。无下标的 $R = \text{Rot}(\hat{z}, 90^\circ)$ 表示独立于坐标系的操作。
4. 角速度与旋转矩阵微分
4.1 角速度定义
角速度 $\boldsymbol{\omega}$ 是一个自由向量，由瞬时旋转轴 $\hat{\omega}$ 与转动速率 $\dot{\theta}$ 构成：
$$ \boldsymbol{\omega} = \hat{\omega} \dot{\theta} \in \mathbb{R}^3 $$4.2 旋转矩阵的变化率 $\dot{R}$
坐标系各轴的变化率满足叉乘关系 $\dot{\mathbf{r}}_i = \boldsymbol{\omega}_s \times \mathbf{r}_i$。引入 Hat 算子 (反对称矩阵) 将叉乘转为矩阵乘法：
$$ [\mathbf{x}] = \begin{bmatrix} 0 & -x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{bmatrix} \quad \Rightarrow \quad [\mathbf{x}]\mathbf{y} = \mathbf{x} \times \mathbf{y} $$得到旋转矩阵的微分方程：
$$ \dot{R}_{sb}(t) = [\boldsymbol{\omega}_s] R_{sb}(t) $$4.3 不同坐标系下的角速度变换
| 关系 | 数学表达 | 说明 |
|---|---|---|
| 向量坐标变换 | $\boldsymbol{\omega}_s = R_{sb} \boldsymbol{\omega}_b$ | 同一角速度在不同基下的投影 |
| 李代数伴随性质 | $[\boldsymbol{\omega}_s] = R_{sb} [\boldsymbol{\omega}_b] R_{sb}^T$ | 反对称矩阵的相似变换 |
| 从 $\dot{R}$ 提取 | $[\boldsymbol{\omega}_s] = \dot{R}_{sb} R_{sb}^T$ $[\boldsymbol{\omega}_b] = R_{sb}^T \dot{R}_{sb}$ |
外部观测 vs 本体传感器 (如 IMU) 测量值 |
所有 $3 \times 3$ 反对称矩阵构成的集合称为 李代数 $\mathfrak{so}(3)$，它是 $SO(3)$ 的切空间，描述姿态的瞬时变化率。
5. 指数坐标与矩阵对数 (Exponential Coordinates)
5.1 矩阵指数映射
若角速度恒定，对 $\dot{R} = [\boldsymbol{\omega}]R$ 积分并结合 $R(0)=I$，可得：
$$ R = e^{[\hat{\omega}]\theta} = I + \sin\theta \, [\hat{\omega}] + (1-\cos\theta) \, [\hat{\omega}]^2 $$此即 罗德里格斯公式 (Rodrigues’ Formula)。向量 $\hat{\omega}\theta \in \mathbb{R}^3$ 称为旋转的指数坐标。
5.2 矩阵对数映射 (逆问题)
给定 $R \in SO(3)$，求 $\hat{\omega}$ 与 $\theta$：
- $R = I$：$\theta = 0$，$\hat{\omega}$ 未定义（无旋转）。
- $\text{tr}(R) = -1$：$\theta = \pi$，$\hat{\omega}$ 可取对角线元素加 1 后归一化： $$ \hat{\omega} = \frac{1}{\sqrt{2(1+r_{ii})}} \begin{bmatrix} r_{1i} \\ r_{2i} \\ 1+r_{ii} \end{bmatrix} \quad (i=1,2,3 \text{ 任选}) $$
- 一般情况： $$ \theta = \cos^{-1}\left(\frac{\text{tr}(R)-1}{2}\right), \quad [\hat{\omega}] = \frac{R - R^T}{2\sin\theta} $$
指数映射 $\exp: \mathfrak{so}(3) \to SO(3)$ 与对数映射 $\log: SO(3) \to \mathfrak{so}(3)$ 构成了姿态连续演化的数学桥梁。
6. 欧拉角与 RPY 角
6.1 欧拉角 (Euler Angles)
- 定义：绕物体坐标系 (Body Frame) 连续旋转三次得到的姿态表示。
- 常用序列：$ZYZ$ (传统机械臂)、$ZYX$ (航空航天)。
- 数学表达：$R(\alpha,\beta,\gamma) = \text{Rot}(\hat{z},\alpha) \text{Rot}(\hat{y},\beta) \text{Rot}(\hat{z},\gamma)$（以 $ZYZ$ 为例）。
6.2 横滚-俯仰-偏航角 (Roll-Pitch-Yaw)
- 定义：绕固定参考系 (Space Frame) 的 $XYZ$ 轴连续旋转。
- 等价性：$R_{RPY}(\alpha,\beta,\gamma) = \text{Rot}(\hat{z},\alpha)\text{Rot}(\hat{y},\beta)\text{Rot}(\hat{x},\gamma)I$，其矩阵形式与 $ZYX$ 欧拉角完全一致，仅物理视角不同。
6.3 万向节死锁 (Gimbal Lock)
当中间旋转角 $\beta = \pm \frac{\pi}{2}$ 时，首尾旋转轴重合，系统丢失 1 个自由度，退化为二维旋转。
工程启示：欧拉角/RPY 存在奇异性，不适合作为底层状态估计的连续表示，仅适用于人机交互界面或特定构型控制。
7. 单位四元数 (Unit Quaternions)
7.1 定义与几何意义
四元数 $q = q_0 + q_1 i + q_2 j + q_3 k$ 是复数的四维扩展。单位四元数满足 $\|q\|^2 = q_0^2+q_1^2+q_2^2+q_3^2 = 1$，几何上对应 $\mathbb{R}^4$ 中的单位超球面 $S^3$。
与轴角映射关系：
$$ q = \begin{bmatrix} \cos\frac{\theta}{2} \\ \hat{\omega} \sin\frac{\theta}{2} \end{bmatrix} $$双覆盖性质 (Double Cover)：$q$ 与 $-q$ 表示完全相同的空间旋转。旋转 $2\pi$ 对应 $q \to -q$，需旋转 $4\pi$ 才回到 $q$。
7.2 核心运算
| 运算 | 公式 | 用途 |
|---|---|---|
| 哈密顿积 | $q_1 \otimes q_2$ (非交换) | 姿态连乘：$R(q_1)R(q_2) \Rightarrow q_1 \otimes q_2$ |
| 共轭/逆 | $q^* = [q_0, -\mathbf{q}_v]^T$，$\|q\|=1 \Rightarrow q^{-1}=q^*$ | 求反向旋转 |
| 向量旋转 | $\mathbf{v}' = q \otimes [0, \mathbf{v}]^T \otimes q^*$ | 在四元数域内旋转向量 |
7.3 球面线性插值 (Slerp)
为保证恒定角速度插值，使用测地线插值：
$$ \text{Slerp}(q_0, q_1; t) = \frac{\sin((1-t)\theta)}{\sin\theta} q_0 + \frac{\sin(t\theta)}{\sin\theta} q_1 $$其中 $\cos\theta = q_0 \cdot q_1$。若 $q_0 \cdot q_1 < 0$，先将 $q_1 \leftarrow -q_1$ 以保证走最短路径。
8. 姿态表示法拓扑对比与工程选型
| 表示法 | 参数数量 | 奇异性 | 拓扑空间 | 适用场景 |
|---|---|---|---|---|
| 旋转矩阵 $R$ | 9 (冗余 6) | 无 | $\mathbb{RP}^3$ (实投影空间) | 几何推导、坐标变换、可视化 |
| 指数坐标 / 轴角 | 3 / 4 | 有 ($\theta=0$ 时轴未定义) | $S^2 \times S^1$ | 运动学建模、螺旋理论 |
| 欧拉角 / RPY | 3 | 有 (万向节死锁) | $\mathbb{T}^3$ (三维环面) | 人机交互、特定关节控制 |
| 单位四元数 $q$ | 4 (约束 1) | 无 | $S^3$ (三维超球面) | 姿态积分、滤波优化、插值动画 |
建议
- 底层计算：优先使用 四元数 进行姿态递推、卡尔曼滤波与轨迹优化（无奇点、计算稳定）。
- 坐标变换：使用 旋转矩阵 进行向量投影与多坐标系链式传递（直观、易调试）。
- 用户接口：将四元数转换为 RPY/欧拉角 供操作人员读取或显示。
- 数值投影：实际系统中因浮点误差，姿态可能偏离流形。需定期执行：
- $R \leftarrow \text{svd}(R)$ 正交化
- $q \leftarrow q / \|q\|$ 归一化