4. 基于陀螺仪的运动路径积分

任务说明

本案例实现运动相机EIS中的核心前置步骤：将陀螺仪三轴角速度积分，得到相机在世界坐标系下的旋转路径（姿态随时间的变化）。该旋转路径将用于后续帧间运动估计与防抖裁剪。输入为陀螺仪采样序列（时间戳+三轴角速度），输出为每个采样时刻对应的旋转矩阵（或四元数），并验证积分结果在静态与匀速旋转两种场景下的正确性。

实现方案

采用零阶保持积分（角速度在采样间隔内恒定），使用四元数表示姿态以避免万向锁问题。每个新姿态由前一姿态与增量旋转合成：q(t+Δt) = q(t) ⊗ Δq(ω·Δt)，其中增量四元数由旋转向量ω·Δt经指数映射得到。代码包含完整的四元数乘法、归一化、旋转向量转四元数函数，不依赖第三方库。

代码实现

# gyro_path_integration.py
import math

def quat_multiply(q1, q2):
    """四元数乘法 q1 ⊗ q2"""
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return (
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    )

def quat_normalize(q):
    """四元数归一化"""
    norm = math.sqrt(sum(v*v for v in q))
    return tuple(v / norm for v in q)

def rotation_vector_to_quat(axis_angle):
    """旋转向量(θ·轴) → 四元数"""
    theta = math.sqrt(sum(v*v for v in axis_angle))
    if theta < 1e-12:
        return (1.0, 0.0, 0.0, 0.0)
    axis = [v / theta for v in axis_angle]
    half = theta / 2.0
    s = math.sin(half)
    return (math.cos(half), axis[0]*s, axis[1]*s, axis[2]*s)

def integrate_gyro_path(timestamps, gyro_data, init_q=(1.0, 0.0, 0.0, 0.0)):
    """
    陀螺仪角速度积分得到旋转路径
    参数:
        timestamps: 采样时间戳列表 (s)
        gyro_data:  角速度列表 [(ωx, ωy, ωz)] (rad/s)
        init_q:     初始姿态四元数
    返回:
        姿态四元数列表，长度 = len(timestamps)
    """
    if len(timestamps) != len(gyro_data):
        raise ValueError("时间戳与角速度长度不一致")
    if len(timestamps) == 0:
        return []

    path = []
    q = quat_normalize(init_q)
    path.append(q)

    for i in range(1, len(timestamps)):
        dt = timestamps[i] - timestamps[i-1]
        if dt <= 0:
            raise ValueError("时间戳必须严格递增")

        # 零阶保持积分：角速度在Δt内恒定
        omega = gyro_data[i-1]  # 使用前一时刻角速度（或当前时刻，取决于传感器延迟）
        delta_angle = [omega[0]*dt, omega[1]*dt, omega[2]*dt]
        dq = rotation_vector_to_quat(delta_angle)
        q = quat_multiply(q, dq)
        q = quat_normalize(q)
        path.append(q)

    return path

# ========== 测试用例 ==========
if __name__ == "__main__":
    # 测试1：静态（零角速度）
    t_static = [0.0, 0.01, 0.02, 0.03]
    g_static = [(0.0, 0.0, 0.0)] * 4
    path_static = integrate_gyro_path(t_static, g_static)
    print("静态测试结果（应为全(1,0,0,0)）:")
    for q in path_static:
        print(f"  q = {q}")

    # 测试2：绕Z轴匀速旋转 1 rad/s，持续2s，采样率100Hz
    t_rot = [i * 0.01 for i in range(201)]  # 0~2.0s
    g_rot = [(0.0, 0.0, 1.0)] * len(t_rot)  # 绕Z轴 1 rad/s
    path_rot = integrate_gyro_path(t_rot, g_rot)

    # 验证：最终姿态应接近绕Z轴旋转2 rad
    q_final = path_rot[-1]
    w, x, y, z = q_final
    # 绕Z轴旋转θ的四元数为 (cos(θ/2), 0, 0, sin(θ/2))
    theta_expected = 2.0  # 1 rad/s * 2s
    w_exp = math.cos(theta_expected / 2)
    z_exp = math.sin(theta_expected / 2)
    print(f"\n匀速旋转测试（2s后）:")
    print(f"  最终四元数: q = {q_final}")
    print(f"  预期四元数: q = ({w_exp:.6f}, 0, 0, {z_exp:.6f})")
    print(f"  误差: Δw = {abs(w - w_exp):.2e}, Δz = {abs(z - z_exp):.2e}")

    # 测试3：四元数范数恒为1
    norms = [math.sqrt(sum(v*v for v in q)) for q in path_rot]
    max_norm_err = max(abs(n - 1.0) for n in norms)
    print(f"\n范数检查: 最大偏差 = {max_norm_err:.2e} (应接近0)")

运行与验证

运行方式：将代码保存为gyro_path_integration.py，在Python 3环境中执行python gyro_path_integration.py。代码无第三方依赖，仅使用标准库math。

验证判定标准：

• 静态测试：所有输出四元数均为(1.0, 0.0, 0.0, 0.0)，表示无旋转时姿态保持不变。
• 匀速旋转测试：最终四元数应接近(cos(1.0), 0, 0, sin(1.0))（即绕Z轴旋转2 rad），误差在1e-6量级（由离散采样引入的数值积分误差）。
• 范数检查：所有四元数范数与1的最大偏差应小于1e-12，证明归一化有效。

上述预期输出由代码逻辑直接推出，实际执行时数值误差取决于浮点运算，但量级应与上述分析一致。该旋转路径即为后续帧间运动估计的输入——通过比较相邻帧对应时刻的姿态差，可得到帧间旋转矩阵，进而指导防抖裁剪。