角速度推算姿态-19

太棒了！这正是从“数学理论”走向“工程实践”最关键的一步。

在实际的机器人或 SLAM 系统（比如扫地机器人、无人机、AR眼镜）中，我们通常使用 IMU（惯性测量单元） 中的陀螺仪（Gyroscope）来获取角速度，然后推算出当前的姿态。

这个过程在学术上叫 “航位推算（Dead Reckoning）” 或 “姿态积分（Attitude Integration）”。

下面我用图解 + 公式 + 代码的方式，带你走一遍这个流程。

---

1. 物理场景：我们有什么？

• 已知状态： 时刻的机器人旋转姿态 （或者四元数 ）。

• 传感器输入：IMU 陀螺仪测量的角速度 。

• 注意：陀螺仪测量的是物体自身坐标系（Body Frame）下的角速度（比如机头向左偏了多少）。

• 时间间隔：（通常 IMU 频率很高， 或更小）。

• 目标：求 时刻的姿态 。

---

2. 直观模型：在球面上行走

还是回到李群李代数的“地球仪”模型：

1. 当前位置：你站在地球表面的 点。
2. 传感器指令：陀螺仪告诉你：“沿着 这个方向走，速度是这么快”。
3. 计算步长：因为时间很短，我们假设在这 内速度不变。你的位移量（李代数增量）就是：

这是一个旋转向量（Rotation Vector）。
4. 迈出一步：你在切平面上走了 这么远。
5. 投影回球面：使用 指数映射 (exp) 把切平面上的这点贴回地球表面，你就到达了新的姿态 。

---

3. 数学公式：为什么是“右乘”？

因为陀螺仪测量的是Body Frame（自身坐标系）的角速度，根据刚体运动学，微分方程是：

如果你解这个微分方程（假设短时间内 恒定），得到的离散化更新公式是：

关键点：

• 右乘：因为 是相对于当前自身系定义的。如果是相对于世界系的旋转（这就很少见了），才是左乘。
• 保证正交：无论 是多少， 算出来的永远是一个合法的旋转矩阵。如果你用 这种线性加法，算几步矩阵就会歪掉（不正交了）。

---

4. 代码实战 (C++ / Eigen / Sophus)

假设我们有一个循环，不断接收 IMU 数据并更新姿态。

#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/so3.h"

using namespace std;
using namespace Eigen;

int main() {
    // 1. 初始化：假设机器人一开始是正对前方的（单位矩阵）
    Sophus::SO3d current_pose; // 内部存的是四元数，代表 R_k
    
    // 假设 IMU 频率 100Hz
    double dt = 0.01; 

    // 2. 模拟一段 IMU 数据：机器人绕 Z 轴匀速旋转
    // 假设角速度是 0.1 rad/s，绕 Z 轴转
    Vector3d omega_reading(0, 0, 0.1); 

    cout << "Start Pose (Quaternion): " << current_pose.unit_quaternion().coeffs().transpose() << endl;

    // 3. 循环积分（模拟 100 个时间步，即 1 秒）
    for (int i = 0; i < 100; ++i) {
        
        // --- 核心步骤开始 ---
        
        // Step A: 计算李代数增量 (角速度 * 时间)
        // 这就是我们在切空间走的“直线距离”
        Vector3d angle_axis_increment = omega_reading * dt;

        // Step B: 使用指数映射将增量转换为旋转增量 (Delta R)
        // Sophus::SO3d::exp 负责把向量变成旋转矩阵(或四元数)
        Sophus::SO3d delta_rotation = Sophus::SO3d::exp(angle_axis_increment);

        // Step C: 更新姿态 (右乘)
        // R_next = R_curr * Delta_R
        current_pose = current_pose * delta_rotation;
        
        // --- 核心步骤结束 ---
    }

    // 理论上 1秒后，应该转了 0.1 rad * 1s = 0.1 rad 
    // 0.1 rad 约等于 5.7 度
    cout << "End Pose (Quaternion): " << current_pose.unit_quaternion().coeffs().transpose() << endl;
    
    // 验证一下转了多少度
    Vector3d result_vec = current_pose.log();
    cout << "Total rotated angle (rad): " << result_vec.norm() << endl;

    return 0;
}

代码深度解析

1. omega_reading * dt: 这一步是在线性空间（李代数）里做的乘法。非常简单，没有复杂的三角函数。
2. Sophus::SO3d::exp(...): 这是最“贵”的一步操作。它内部会根据罗德里格斯公式计算 。
3. current_pose * delta_rotation: 这是在流形（李群）上做的乘法。Sophus 内部会自动处理四元数的乘法逻辑。

---

5. 进阶：现实中没这么简单 (中值积分)

上面的代码用了欧拉积分（一阶积分），假设这 0.01秒 内角速度是恒定的。
但在实际高精度 SLAM（如 VINS-Mono, ORB-SLAM3）中，为了减少误差，通常会使用中值积分（Mid-point Integration）。

假设 时刻测得 ， 时刻测得 ，我们认为这段时间的平均角速度是：

代码变成：

Vector3d avg_omega = 0.5 * (omega_curr + omega_next);
Vector3d increment = avg_omega * dt;
current_pose = current_pose * Sophus::SO3d::exp(increment);

这就叫“中值积分”，精度会比只用上一时刻数据高很多。

---

6. 总结

旋转积分的本质，就是把角速度（）切成无数个微小的片段，每个片段都转换成一个微小的旋转矩阵（），然后把这些微小的矩阵连乘起来。

• 物理公式：
• 数学工具：李代数乘时间 -> exp -> 李群乘法。
• 工程实现：pose = pose * SO3::exp(gyro * dt)。

你看，用了 Sophus 和李群李代数，原本复杂的微分方程求解，在代码里就变成了两行清爽的乘法！

接下来，如果你有兴趣，我们可以聊聊“如果陀螺仪有误差（Bias）怎么办？” 这就涉及到了 SLAM 中最精彩的IMU 预积分（Pre-integration）理论。想听听吗？