IMU位姿推算学习指南

使用三轴加速度计和三轴陀螺仪进行惯性测量与位姿推算

---

概述

惯性测量单元（IMU）由三轴加速度计和三轴陀螺仪组成，是现代导航系统的核心传感器。本指南系统讲解IMU位姿推算的原理、公式与实现方法。

核心问题

传感器	测量量	推算目标
三轴陀螺仪	角速度 $(\omega_x, \omega_y, \omega_z)$	姿态（方位）
三轴加速度计	线加速度 $(a_x, a_y, a_z)$	位置（位移）

---

一、坐标系定义

1.1 常用坐标系

坐标系	符号	说明
载体坐标系	$b$	固连于IMU，随载体运动
导航坐标系	$n$	当地水平坐标系（北-东-地）
惯性坐标系	$i$	不旋转的参考坐标系
地球坐标系	$e$	固连于地球

1.2 姿态表示方法

表示方法	参数数量	优点	缺点
欧拉角	3	直观易懂	万向节死锁
方向余弦矩阵(DCM)	9	无奇异性	冗余、需正交化
四元数	4	无奇异性、计算高效	不直观

推荐：实际工程中普遍采用四元数进行姿态更新。

---

二、姿态推算

2.1 流程框图

┌─────────────────────────────────────────────────────────────┐
│                      姿态推算流程                            │
├─────────────────────────────────────────────────────────────┤
│                                                             │
│  陀螺仪输出 ──→ 误差补偿 ──→ 姿态更新 ──→ 姿态矩阵/四元数     │
│  (ω_x,ω_y,ω_z)     │            │            │              │
│                    ↓            ↓            ↓              │
│               零偏/标定    四元数微分方程   欧拉角/DCM         │
│                    │            │            │              │
│                    └────────────┴────────────┘              │
│                              │                              │
│                              ↓                              │
│                    加速度计辅助修正(可选)                     │
│                    (互补滤波/卡尔曼滤波)                      │
│                                                             │
└─────────────────────────────────────────────────────────────┘

2.2 核心公式

四元数微分方程

$$\dot{q} = \frac{1}{2} q \otimes \omega$$

展开形式：

$$\begin{bmatrix} \dot{q}_0 \ \dot{q}_1 \ \dot{q}_2 \ \dot{q}_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \ \omega_x & 0 & \omega_z & -\omega_y \ \omega_y & -\omega_z & 0 & \omega_x \ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \ q_1 \ q_2 \ q_3 \end{bmatrix}$$

离散化更新（一阶龙格库塔）

$$q_{k+1} = q_k + \dot{q}_k \cdot \Delta t$$

四元数归一化

$$q = \frac{q}{|q|} = \frac{q}{\sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2}}$$

2.3 公式解释

符号	含义
$q = [q_0, q_1, q_2, q_3]^T$	单位四元数，$q_0$为标量部分
$\omega = [0, \omega_x, \omega_y, \omega_z]^T$	角速度四元数形式
$\otimes$	四元数乘法运算
$\Delta t$	采样周期

为什么需要归一化？ 数值积分会破坏单位四元数约束 $|q|=1$，需周期性归一化以保持姿态表示的有效性。

---

三、位置推算

3.1 流程框图

┌─────────────────────────────────────────────────────────────────┐
│                        位置推算流程                              │
├─────────────────────────────────────────────────────────────────┤
│                                                                 │
│  加速度计输出 ──→ 去除重力 ──→ 坐标转换 ──→ 二次积分 ──→ 位置    │
│  (a_x,a_y,a_z)      │            │            │          │      │
│                     ↓            ↓            ↓          ↓      │
│                a_measured     C_n^b · a_b    速度更新    位置    │
│                  - g          (导航系)      位置更新    输出    │
│                                                                 │
│                     │            ↑                              │
│                     └────────────┴── 需要姿态矩阵               │
│                                                                 │
└─────────────────────────────────────────────────────────────────┘

3.2 核心公式

Step 1: 去除重力（载体坐标系）

$$a_{body} = a_{measured} - C_n^b \cdot g_{nav}$$

其中 $g_{nav} = [0, 0, g]^T$，$g \approx 9.8 , m/s^2$

Step 2: 坐标转换（载体系→导航系）

$$a_{nav} = C_b^n \cdot a_{body}$$

方向余弦矩阵由四元数计算：

$$C_b^n = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1 q_2 - q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \ 2(q_1 q_2 + q_0 q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2 q_3 - q_0 q_1) \ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$$

Step 3: 速度更新

$$v_{k+1} = v_k + a_{nav} \cdot \Delta t$$

Step 4: 位置更新

$$p_{k+1} = p_k + v_k \cdot \Delta t + \frac{1}{2} a_{nav} \cdot \Delta t^2$$

3.3 公式解释

步骤	说明
重力补偿	加速度计测量值包含重力分量，必须去除
坐标转换	加速度计测量的是载体坐标系下的值，需转换到导航坐标系
二次积分	加速度→速度→位置，需要两次积分
误差累积	位置推算误差随时间平方增长，这是IMU航位推算的根本缺陷

---

四、完整系统框图

┌───────────────────────────────────────────────────────────────────────┐
│                    IMU位姿推算完整流程                                  │
├───────────────────────────────────────────────────────────────────────┤
│                                                                       │
│   ┌─────────────┐         ┌─────────────┐                            │
│   │  陀螺仪     │──ω─────→│  姿态推算   │───→ q, C_b^n               │
│   └─────────────┘         └─────────────┘         │                  │
│                                                   │                  │
│                                                   ↓                  │
│   ┌─────────────┐         ┌─────────────┐   ┌─────────────┐          │
│   │ 加速度计    │──a─────→│ 重力补偿    │──→│ 坐标转换    │          │
│   └─────────────┘         └─────────────┘   └─────────────┘          │
│                                                    │                  │
│                                                    ↓                  │
│                                           ┌─────────────┐            │
│                                           │ 位置推算   │──→ p, v    │
│                                           └─────────────┘            │
│                                                                       │
│   输出: 姿态(四元数q/欧拉角) + 位置p + 速度v                           │
│                                                                       │
└───────────────────────────────────────────────────────────────────────┘

---

五、关键问题与改进方法

5.1 主要误差源

误差类型	产生原因	影响
陀螺零偏	传感器固有偏差	姿态线性漂移
加速度零偏	传感器固有偏差	速度线性漂移、位置二次漂移
随机游走	传感器噪声	误差随时间累积
标定误差	安装偏差、比例因子误差	系统性偏差

5.2 改进方法

方法	原理	适用场景
零偏估计与补偿	静态时估计零偏	所有应用
零速修正(ZUPT)	静止时速度归零	行人导航
互补滤波	融合加速度计姿态修正	短时导航
卡尔曼滤波	最优估计传感器误差	高精度应用
多传感器融合	融合GPS/视觉/磁力计	长时导航

---

六、代码示例

6.1 四元数更新（Python）

import numpy as np

def quaternion_update(q, omega, dt):
    """
    四元数姿态更新
    
    参数:
        q: 当前四元数 [q0, q1, q2, q3]
        omega: 角速度 [wx, wy, wz] (rad/s)
        dt: 采样周期 (s)
    
    返回:
        更新后的四元数
    """
    wx, wy, wz = omega
    
    # 四元数微分方程的系数矩阵
    Omega = np.array([
        [0, -wx, -wy, -wz],
        [wx, 0, wz, -wy],
        [wy, -wz, 0, wx],
        [wz, wy, -wx, 0]
    ])
    
    # 四元数导数
    q_dot = 0.5 * Omega @ q
    
    # 一阶龙格库塔更新
    q_new = q + q_dot * dt
    
    # 归一化
    q_new = q_new / np.linalg.norm(q_new)
    
    return q_new

6.2 方向余弦矩阵计算

def quaternion_to_dcm(q):
    """
    四元数转方向余弦矩阵
    
    参数:
        q: 四元数 [q0, q1, q2, q3]
    
    返回:
        3x3方向余弦矩阵 C_b^n
    """
    q0, q1, q2, q3 = q
    
    C = np.array([
        [q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)],
        [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)],
        [2*(q1*q3 - q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]
    ])
    
    return C

6.3 位置推算

def position_update(p, v, a_nav, dt):
    """
    位置和速度更新
    
    参数:
        p: 当前位置 [x, y, z]
        v: 当前速度 [vx, vy, vz]
        a_nav: 导航系加速度 [ax, ay, az]
        dt: 采样周期
    
    返回:
        新位置, 新速度
    """
    # 速度更新
    v_new = v + a_nav * dt
    
    # 位置更新
    p_new = p + v * dt + 0.5 * a_nav * dt**2
    
    return p_new, v_new

---

七、参考文献

1. Titterton D, Weston J. Strapdown Inertial Navigation Technology. AIAA, 2004.
2. Savage P G. Strapdown Analytics. Strapdown Associates, 2007.
3. 秦永元. 惯性导航. 科学出版社, 2014.
4. Groves P D. Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House, 2013.

---

附录：欧拉角与四元数转换

欧拉角转四元数

$$q = \begin{bmatrix} \cos\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} + \sin\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \ \sin\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} - \cos\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \ \cos\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} + \sin\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} \ \cos\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} - \sin\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} \end{bmatrix}$$

其中 $\phi$（横滚）、$\theta$（俯仰）、$\psi$（偏航）为欧拉角。

四元数转欧拉角

$$\begin{cases} \phi = \arctan2(2(q_2 q_3 + q_0 q_1), q_0^2 - q_1^2 - q_2^2 + q_3^2) \ \theta = \arcsin(2(q_0 q_2 - q_1 q_3)) \ \psi = \arctan2(2(q_1 q_2 + q_0 q_3), q_0^2 + q_1^2 - q_2^2 - q_3^2) \end{cases}$$

---

本指南基于经典惯性导航理论整理，适用于机器人、无人机、自动驾驶等领域的IMU应用开发。