角速度 (Angular Velocity) 与旋转矩阵变化率详解

July 2018

1. 引入与基本定义

假设一个坐标系 $\{b\}$ 绕经过原点的单位轴 $\hat{\omega}$ 连续旋转。实线表示 $t$ 时刻的姿态，虚线表示经过微小时间 $\Delta t$ 后 $t+\Delta t$ 时刻的姿态。

在此过程中，坐标系各轴的方向随时间连续变化。我们将方向的变化率与瞬时旋转轴结合，即可建立角速度与旋转矩阵的联系。

角速度标量（转动速率）： $$ \dot{\theta} = \lim_{\Delta t \to 0} \frac{\Delta \theta}{\Delta t} $$
瞬时旋转轴：单位向量 $\hat{\omega} = (\omega_x, \omega_y, \omega_z)^T$。
角速度向量： $$ \boldsymbol{\omega} = \hat{\omega} \dot{\theta} \in \mathbb{R}^3 $$

物理意义：$\boldsymbol{\omega}$ 是一个自由向量（Free Vector）。其方向由右手定则确定（四指弯曲方向为旋转方向，拇指指向为 $\boldsymbol{\omega}$ 方向），大小为单位时间转过的弧度（rad/s）。它独立于具体坐标系，但可用不同坐标系下的坐标分量表示。

2. 旋转矩阵的变化率 $\dot{R}$

设 $R(t) \in SO(3)$ 为物体坐标系 $\{b\}$ 在 $t$ 时刻相对于固定坐标系 $\{s\}$ 的姿态矩阵。其列向量 $\mathbf{r}_1, \mathbf{r}_2, \mathbf{r}_3$ 分别对应 $\{b\}$ 的 $\hat{x}, \hat{y}, \hat{z}$ 轴在 $\{s\}$ 下的坐标。

根据刚体运动学，各坐标轴的变化率满足叉乘关系：

$$ \dot{\mathbf{r}}_i = \boldsymbol{\omega}_s \times \mathbf{r}_i, \quad i=1,2,3 $$

将三列合并为矩阵形式，得到旋转矩阵的微分方程：

$$ \dot{R}(t) = [\boldsymbol{\omega}_s] R(t) = \boldsymbol{\omega}_s \times R $$

其中 $\boldsymbol{\omega}_s$ 是角速度在 $\{s\}$ 系下的坐标表示。该式用矩阵形式统一描述了动态旋转过程。

3. 反对称矩阵与李代数 $\mathfrak{so}(3)$

为了将向量叉乘转化为矩阵乘法，引入 Hat 算子（反对称矩阵）。

对于任意向量 $\mathbf{x} = [x_1, x_2, x_3]^T \in \mathbb{R}^3$，定义其反对称矩阵为：

$$ [\mathbf{x}] = \begin{bmatrix} 0 & -x_3 & x_2 \\ x_3 & 0 & -x_1 \\ -x_2 & x_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3} $$

3.1 核心性质

反对称性：$[\mathbf{x}]^T = -[\mathbf{x}]$
叉乘等价：$[\mathbf{x}]\mathbf{y} = \mathbf{x} \times \mathbf{y}$
李代数 $\mathfrak{so}(3)$：所有 $3 \times 3$ 反对称矩阵构成的集合。它精确描述了特殊正交群 $SO(3)$ 的切空间（即旋转的变化率）。

因此，旋转矩阵变化率可简洁写作：

$$ \dot{R} = [\boldsymbol{\omega}_s] R $$

4. 静态旋转 vs 动态旋转

4.1 为什么需要引入 $\dot{R}$？

视角	数学表达	物理含义	局限性
静态	$\mathbf{p}_s = R \mathbf{p}$ 或 $R = \text{Rot}(\hat{z}, 90^\circ)$	描述离散时刻的姿态或执行一次固定角度旋转	无法直接表达“过程中任意时刻”的状态，多次旋转需离散连乘
动态	$\dot{R} = [\boldsymbol{\omega}] R$	描述连续时间下的姿态演化率	需配合初始条件 $R(0)=I$ 积分使用

4.2 $\dot{R}$ 的直观物理意义

以绕 $\{s\}$ 的 $z$ 轴以角速度 $\dot{\theta}$ 旋转为例：

$$ [\boldsymbol{\omega}_s] = \begin{bmatrix} 0 & -\dot{\theta} & 0 \\ \dot{\theta} & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} $$

第一列：$\hat{x}_b$ 轴沿 $+y_s$ 方向运动，瞬时速率为 $\dot{\theta}$
第二列：$\hat{y}_b$ 轴沿 $-x_s$ 方向运动，瞬时速率为 $\dot{\theta}$
第三列：$\hat{z}_b$ 轴与旋转轴重合，瞬时速率为 $0$

优势：$\dot{R}$ 矩阵直接“可视化”了各轴在参考系中的瞬时速度方向与大小，比显式的 $R(t)=\text{Rot}(\hat{z}, \omega t)$ 更简洁，且为后续运动旋量 (Twist) 与螺旋理论奠定数学基础。

5. 角速度在不同坐标系下的变换

角速度是自由向量，但在不同坐标系下有不同的坐标表示。设 $\boldsymbol{\omega}_s$ 为在 $\{s\}$ 系下的表示，$\boldsymbol{\omega}_b$ 为在 $\{b\}$ 系下的表示。

关系	数学表达	物理/工程含义
坐标变换	$\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$	外部观测者看到的角速度
从 $\dot{R}$ 提取（物体系）	$[\boldsymbol{\omega}_b] = R_{sb}^T \dot{R}_{sb}$	自身传感器（如 IMU）直接测量的量

恒等式：$R [\boldsymbol{\omega}] R^T = [R \boldsymbol{\omega}]$。
证明：利用叉乘的坐标不变性 $R(\boldsymbol{\omega} \times \mathbf{v}) = (R\boldsymbol{\omega}) \times (R\mathbf{v})$，代入反对称定义即可得证。

6. 指数坐标与动态姿态演化

若角速度 $\boldsymbol{\omega}$ 恒定，对微分方程 $\dot{R} = [\boldsymbol{\omega}] R$ 积分，结合初始条件 $R(0)=I$，可得矩阵指数解（Rodrigues 公式的动态形式）：

$$ R(t) = e^{[\hat{\boldsymbol{\omega}}] \theta(t)} = I + \sin\theta \, [\hat{\boldsymbol{\omega}}] + (1-\cos\theta) \, [\hat{\boldsymbol{\omega}}]^2 $$

其中 $\theta(t) = \|\boldsymbol{\omega}\| t$ 为累计转角。这建立了李代数 $\mathfrak{so}(3)$ 到李群 $SO(3)$ 的指数映射（Exponential Map），是现代机器人运动学的核心工具。

7. C++ 动态旋转仿真示例

以下代码演示如何利用角速度动态生成旋转矩阵，并观察向量随时间的连续演化。对比“基于时间连续计算”与“离散角度步进”的等价性。

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
    // 设定角速度: 绕 Z 轴 10°/s ≈ 0.1745 rad/s
    double omega = M_PI / 18.0; 
    double dt = 0.2;           // 采样间隔
    double total_time = 9.0;   // 总时长 (9s 后应旋转约 90°)

    Vector3d p_init(1.0, 1.0, 0.0); // 初始向量

    cout << "--- 动态旋转仿真 (基于时间 t 的连续模型) ---\n";
    for (double t = 0.0; t <= total_time + 1e-6; t += dt) {
        // 直接根据时间计算旋转矩阵
        double theta = omega * t;
        Matrix3d R_t;
        R_t << cos(theta), -sin(theta), 0,
               sin(theta),  cos(theta), 0,
               0,           0,          1;

        Vector3d p_rotated = R_t * p_init;
        cout << "t=" << t << "s | p(t) = [" 
             << p_rotated.x() << ", " << p_rotated.y() << ", " << p_rotated.z() << "]\n";
    }

    cout << "\n--- 验证：等效的离散角度步进 (R_{k+1} = R_{step} * R_k) ---\n";
    // 构造微小时间步长对应的旋转矩阵
    Matrix3d R_step;
    R_step << cos(omega*dt), -sin(omega*dt), 0,
              sin(omega*dt),  cos(omega*dt), 0,
              0,              0,              1;
    
    Vector3d p_discrete = p_init;
    Matrix3d R_curr = Matrix3d::Identity(); // 初始姿态为单位阵
    
    for (double t = 0.0; t <= total_time + 1e-6; t += dt) {
        p_discrete = R_curr * p_init;
        cout << "t=" << t << "s | p_disc = [" 
             << p_discrete.x() << ", " << p_discrete.y() << ", " << p_discrete.z() << "]\n";
        R_curr = R_step * R_curr; // 离散姿态递推
    }
    return 0;
}

输出：两种方法在 $t=9.0\text{s}$ 时结果高度一致（约 $[-0.958, 1.040, 0.000]^T$），验证了 $\dot{R}=[\boldsymbol{\omega}]R$ 与 $R(t)=\text{Rot}(\hat{z},\omega t)$ 的数学等价性。

8. 核心总结

概念	数学形式	物理/工程意义
角速度向量	$\boldsymbol{\omega} = \hat{\boldsymbol{\omega}}\dot{\theta}$	描述旋转快慢与轴向的自由向量
Hat 算子	$[\boldsymbol{\omega}]$	将叉乘 $\boldsymbol{\omega} \times \mathbf{v}$ 转为矩阵乘法 $[\boldsymbol{\omega}]\mathbf{v}$
李代数 $\mathfrak{so}(3)$	所有 $3\times3$ 反对称矩阵	$SO(3)$ 的切空间，描述姿态变化率
旋转矩阵微分	$\dot{R} = [\boldsymbol{\omega}_s] R$	固定系视角下的姿态演化方程
物体系角速度	$[\boldsymbol{\omega}_b] = R^T \dot{R}$	自身传感器（如 IMU）直接测量的量
坐标变换	$\boldsymbol{\omega}_s = R_{sb} \boldsymbol{\omega}_b$	不同参考系下角速度坐标的转换

备注：

在实际机器人系统中，IMU 输出的角速度通常是 $\boldsymbol{\omega}_b$（物体坐标系下）。

结合 $\dot{R} = R [\boldsymbol{\omega}_b]$ 可实现姿态递推（姿态积分），常用于 SLAM 前端或滤波器预测步。

运动旋量 (Twist) 与 螺旋理论 (Screw Theory)，将角速度与线速度统一为 $6$ 维空间速度向量 $\mathcal{V} = [\boldsymbol{\omega}, \mathbf{v}]^T$。