一、陀螺仪基础与姿态表示

各位同学好，我是老张。在惯性导航这个行当摸爬滚打了十几年，今天咱们来聊聊陀螺仪积分算法与漂移补偿。说实话，这个主题是惯导系统里最核心、也最让人头疼的部分。我刚开始做项目时，就被陀螺仪的漂移坑过好几次。

先问大家一个问题：你知道为什么无人机能悬停得那么稳？为什么手机横过来屏幕就自动旋转？背后都是陀螺仪在干活。但陀螺仪有个毛病——它会漂。嗯，这就是我们今天要解决的核心问题。

1.1 陀螺仪工作原理

陀螺仪的原理，说白了就是利用科里奥利力。你想想看，一个高速旋转的物体，当外界给它施加一个转动时，会产生一个垂直于转动方向的力。MEMS陀螺仪就是靠这个原理来测量角速度的。

我记得2015年做一款工业级IMU时，选型就折腾了两个月。不同厂家的陀螺仪，性能差异巨大。我给大家列个常见的参数对比：

参数	消费级（如MPU6050）	工业级（如ADIS16470）	战术级（如HG4930）
零偏稳定性	10-100 °/h	1-10 °/h	0.1-1 °/h
角随机游走	0.3-1.0 °/√h	0.1-0.3 °/√h	0.01-0.1 °/√h
量程	±2000 °/s	±500 °/s	±200 °/s
价格	几十元	几千元	几万元

1.2 角速度测量与积分

陀螺仪输出的是角速度 ω = (ωx, ωy, ωz)，单位是 °/s 或 rad/s。要得到角度，必须做积分：

// 简单的角度积分
angle += gyro * dt;

// 但实际项目中，我建议用这种
angle = angle + gyro * dt + 0.5 * gyro_dot * dt * dt;

为什么要有二阶项？因为角速度本身也在变化。你想想看，如果飞机在做快速滚转，只用一阶积分，误差会很大。

1.3 姿态表示方法

姿态表示，说白了就是描述一个物体在三维空间里是怎么转的。常用的有三种方法：欧拉角、旋转矩阵、四元数。我一个个说。

1.3.1 欧拉角

欧拉角最直观，就是绕三个轴转的角度：

• 横滚角（Roll）：绕X轴转动，范围 ±180°
• 俯仰角（Pitch）：绕Y轴转动，范围 ±90°
• 偏航角（Yaw）：绕Z轴转动，范围 ±180°

但欧拉角有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会耦合在一起，丢失一个自由度。我在做飞行器控制时就遇到过这个问题，飞机倒飞时姿态解算直接崩了。

1.3.2 旋转矩阵

旋转矩阵用 3×3 的矩阵表示旋转，没有万向锁问题。但计算量大，而且需要满足正交约束。

// 绕Z轴旋转θ角的旋转矩阵
Rz(θ) = [cosθ  -sinθ  0]
        [sinθ   cosθ  0]
        [0      0     1]

1.3.3 四元数

四元数是我最推荐的方法。它用四个数表示旋转，没有奇点，计算效率高。形式是：

q = [q0, q1, q2, q3] = [cos(θ/2), sin(θ/2)*nx, sin(θ/2)*ny, sin(θ/2)*nz]

其中 (nx, ny, nz) 是旋转轴，θ 是旋转角度。四元数的更新公式也很简洁：

// 四元数微分方程
dq/dt = 0.5 * q ⊗ ω

// 离散化更新
q_new = q_old + 0.5 * q_old ⊗ ω * dt

1.4 三种方法的对比

特性	欧拉角	旋转矩阵	四元数
参数数量	3	9	4
万向锁	有	无	无
计算量	小	大	中
插值	困难	困难	容易
直观性	好	差	中

1.5 知识体系结构图

下面这张图是我自己画的，把本章的知识脉络理清楚了：

1.6 避坑指南

最后，我分享几个实战中踩过的坑：

• 采样率不够：我曾经用100Hz的采样率做无人机姿态解算，结果高频振动导致积分发散。后来改到400Hz才稳定。
• 忘记归一化：四元数如果不做归一化，模长会慢慢偏离1，导致姿态失真。我建议每100次更新做一次归一化。
• 忽略地球自转：高精度导航时，地球自转的角速度（15°/h）不能忽略。我有个朋友做船载惯导，没考虑这个，航向误差每小时漂了15度。

好了，这一章就到这里。记住我说的：陀螺仪测角速度，积分得角度，四元数最靠谱。下一章我们聊聊具体的积分算法实现。

---

公众号:蓝海资料掘金营，微信deep3321