6DoF运动跟踪实战：从MPU6050到四元数姿态解算

1. 从一颗陀螺仪说起：6DoF运动跟踪到底在解决什么问题

如果你拆开过任何一台VR头显、无人机飞控、或者最近很火的人形机器人，大概率会在主板中央看到一颗指甲盖大小的芯片，上面印着MPU6050或者ICM42688之类的丝印。这颗芯片就是整个运动跟踪系统的起点。但很多人第一次把MPU6050接到开发板上，兴冲冲地读出六个数据——三轴加速度、三轴角速度——然后发现：这数据根本没法直接用。加速度计在静止时能告诉你重力方向，但一振动就满屏噪声；陀螺仪积分出来的角度短时间内很准，几分钟后漂到姥姥家。这就是6DoF运动跟踪要解决的核心问题：如何把多个不完美的传感器数据，融合成一个稳定、准确、实时的姿态估计。

6DoF，即Six Degrees of Freedom，六个自由度，指的是刚体在三维空间中的运动描述——三个平移自由度（沿X、Y、Z轴的位移）和三个旋转自由度（绕X、Y、Z轴的旋转，通常用俯仰Pitch、横滚Roll、偏航Yaw表示）。注意，很多入门教程会把6DoF和9DoF搞混。9DoF是在加速度计和陀螺仪基础上加了磁力计，多了三轴磁场测量，能提供绝对的航向参考。但磁力计在室内、金属附近极易受干扰，所以实际工程中，6DoF方案反而更常见——用加速度计修正俯仰和横滚，用陀螺仪积分维持短时航向，航向的长期漂移则靠外部视觉或UWB等手段来校正。

这套技术栈的应用场景远比大多数人想象的广泛。VR/AR头显需要它来追踪头部转动，无人机飞控需要它来维持机身稳定，手机屏幕旋转需要它来判断横竖屏，扫地机器人需要它来构建房间地图，甚至你手里的游戏手柄、智能手表里的计步和睡眠监测，底层都在跑姿态解算算法。可以说，只要涉及“物体在三维空间中怎么动”这个问题，6DoF运动跟踪就是绕不开的基础设施。

这篇文章面向的是想真正搞懂6DoF运动跟踪底层原理并动手实现的开发者。不管你是刚接触MPU6050的嵌入式新手，还是已经在做无人机飞控但姿态环总是调不好的老手，我都会从传感器特性讲起，一步步拆到四元数姿态解算的代码实现，把传感器融合的每个关键决策背后的逻辑讲清楚。我不会只给你一堆公式，而是告诉你为什么选这个方案、参数怎么定、坑在哪里、怎么排查。读完你应该能自己搭出一套可用的6DoF姿态跟踪系统，并且知道每一行代码在干什么。

2. 传感器选型与数据特性：为什么不能只用一种传感器

2.1 加速度计、陀螺仪、磁力计各自的能力边界

要理解传感器融合的必要性，得先搞清楚每种传感器单独用会出什么问题。我拿最常见的MPU6050来举例，它内部集成了三轴MEMS加速度计和三轴MEMS陀螺仪，通过I2C接口输出原始数据。先看加速度计：它的原理是检测质量块在加速度作用下的位移，静止时输出的是重力加速度在三个轴上的分量。假设传感器水平放置，Z轴朝上，那么理论输出是(0, 0, 1g)。根据这个分量，你可以反推出俯仰角和横滚角：

• Pitch = arctan2(-ax, sqrt(ay² + az²))
• Roll = arctan2(ay, az)

这两个公式在静止或低速运动时非常准，而且没有累积误差——因为重力是绝对参考。但问题在于，一旦物体有运动加速度，比如无人机加速前飞，加速度计读到的就不只是重力了，姿态解算立刻被污染。更麻烦的是振动，电机转动带来的高频振动会让加速度计输出剧烈波动，直接解算出来的角度根本不能用。

再看陀螺仪：它测量的是角速度，单位通常是度每秒（dps）或弧度每秒（rad/s）。对角速度做一次积分就得到角度变化。陀螺仪的优点是动态响应极好，短时间内的角度变化非常精确，而且不受线性加速度影响。但它的致命伤是零偏（bias）——即使静止不动，输出也不是严格的零，而是有一个微小的直流偏置。这个偏置随温度变化，随时间缓慢漂移。积分会把零偏误差不断累加，假设零偏是0.1度每秒，积分60秒后角度误差就是6度，10分钟后误差60度，完全不可用。

磁力计的情况更复杂。它测量的是地磁场在三个轴上的分量，理论上可以解算出绝对的航向角（Yaw），而且没有累积误差。但实际环境中，电机、扬声器、金属结构、甚至手机壳里的磁铁都会产生干扰磁场，强度可能远超地磁场。室内环境下磁力计的数据基本不可信，所以很多6DoF方案干脆不用磁力计，航向靠陀螺仪积分，定期用其他手段校正。

2.2 传感器融合的核心逻辑：互补而非叠加

理解了三种传感器的特性，融合的思路就清晰了：加速度计和磁力计提供长期稳定的绝对参考，但动态响应差、噪声大；陀螺仪提供短期精确的动态响应，但存在累积漂移。融合的本质就是取长补短——用加速度计的低频信息去校正陀螺仪的高频积分漂移，用陀螺仪的高频信息去平滑加速度计的噪声和运动干扰。

这个思路在频域上看得最清楚。加速度计解算出的角度，低频段（比如0.1Hz以下）是可信的，因为重力方向不变；高频段（比如1Hz以上）被运动加速度和振动污染。陀螺仪积分出的角度，高频段可信，低频段因为零偏积分而漂移。所以理想的做法是设计一个互补滤波器：对加速度计角度做低通滤波，对陀螺仪积分角度做高通滤波，然后相加。这就是最经典的互补滤波姿态解算。

但互补滤波有个前提：你得先有加速度计解算出的角度。而加速度计解算角度需要用到反三角函数，计算量不小，而且在某些姿态下（比如俯仰角接近±90度时）会出现万向节死锁。所以更优雅的方案是直接在四元数空间做融合，用梯度下降或卡尔曼滤波来更新四元数。这就是Mahony滤波和Madgwick滤波的核心思想，后面会详细展开。

2.3 MPU6050的实操配置要点

在动手写融合算法之前，MPU6050的初始化和配置有几个关键点必须处理好，否则后面算法再牛也白搭。

首先是量程选择。MPU6050的陀螺仪可配置为±250、±500、±1000、±2000 dps，加速度计可配置为±2、±4、±8、±16 g。量程越小，分辨率越高，但容易饱和。对于大多数姿态跟踪场景，陀螺仪选±500 dps或±1000 dps，加速度计选±4g或±8g比较合适。如果你做的是无人机飞控，电机振动大，加速度计量程要选大一点，避免削顶。

其次是采样率。MPU6050内部有一个可配置的低通滤波器（DLPF），截止频率从5Hz到260Hz可选。采样率建议至少是信号带宽的2倍，实际工程中常用1kHz采样、42Hz或98Hz的DLPF带宽。采样率越高，积分越精确，但I2C读取压力也越大。如果你用STM32的硬件I2C，400kHz速率下读14字节（加速度6+陀螺仪6+温度2）大约需要0.3ms，1kHz采样完全没问题。

第三是零偏校准。这是最容易被忽视但影响最大的一步。MPU6050出厂时的零偏可能高达±20 dps，不校准的话积分几秒就飘了。校准方法很简单：把传感器静止水平放置，采集几百到几千个样本，求平均值作为零偏，后续读数减去这个零偏。注意校准时要确保传感器真正静止，桌面振动、风扇吹风都会影响校准精度。我一般会采集2000个样本，去掉前100个（等待传感器稳定），然后求均值。

注意：MPU6050的零偏随温度变化明显，从冷启动到热稳定，零偏可能漂移几度每秒。如果应用场景温度变化大，建议做温度补偿，或者定期在静止时重新校准零偏。

还有一个坑是I2C通信的稳定性。MPU6050在高速读取时偶尔会出现数据错位或NACK，尤其是在长排线或电机干扰环境下。我的经验是：I2C速率不要超过400kHz，SDA和SCL线上拉电阻用4.7kΩ，排线尽量短，必要时加屏蔽。如果数据偶尔跳变，可以在软件里加一个简单的异常值剔除——连续读两次，如果差异超过阈值就丢弃。

3. 姿态解算的数学基础：从欧拉角到四元数

3.1 欧拉角的直观与局限

描述三维旋转最直观的方式是欧拉角：绕X轴转Roll，绕Y轴转Pitch，绕Z轴转Yaw。人类很容易理解“先抬头30度，再左转45度”这样的描述。欧拉角在游戏UI、飞行仪表上很常见，但用在姿态解算里有三个致命问题。

第一是万向节死锁。当Pitch角接近±90度时，Roll轴和Yaw轴重合，失去一个自由度，解算会出现奇异点。你让一个物体先抬头90度，然后无论怎么转Roll和Yaw，效果是一样的，数学上无法区分。这在无人机做特技动作或VR头显用户仰头看天时都会遇到。

第二是旋转顺序依赖。欧拉角的三个角度值依赖于旋转顺序，是先转X还是先转Y，结果完全不同。不同厂商、不同软件可能用不同的顺序，导致数据交换时出现混乱。

第三是插值困难。两个欧拉角之间的平滑过渡需要处理角度环绕（比如从179度到-179度），插值算法复杂且容易出错。

所以姿态解算内部几乎不用欧拉角，只在最后输出给用户显示时才转成欧拉角。内部表示和运算用四元数或旋转矩阵。

3.2 四元数的几何直觉与运算规则

四元数听起来很玄，其实可以这样理解：一个四元数q = [w, x, y, z]表示绕单位轴(x, y, z)旋转角度θ，其中w = cos(θ/2)，x = sin(θ/2)·axis_x，y和z同理。为什么是θ/2而不是θ？这是四元数的一个数学特性，简单说是因为四元数旋转需要左右各乘一次，所以角度减半。

四元数的核心优势在于：表示旋转没有奇异点，运算效率高（比旋转矩阵少很多乘法），插值平滑（球面线性插值SLERP）。对于姿态解算，我们关心的是四元数的微分方程：

dq/dt = 0.5 * q ⊗ ω

其中ω是角速度四元数[0, ωx, ωy, ωz]，⊗是四元数乘法。这个方程告诉我们：如果知道当前姿态四元数和角速度，就能推下一时刻的姿态。离散化后就是：

q(t+Δt) = q(t) + 0.5 * q(t) ⊗ ω * Δt

然后归一化。这就是陀螺仪积分更新四元数的基本公式。但如前所述，纯积分会漂移，所以需要用加速度计数据来校正。

四元数乘法展开写出来是：

q ⊗ p = [ qw*pw - qx*px - qy*py - qz*pz, qw*px + qx*pw + qy*pz - qz*py, qw*py - qx*pz + qy*pw + qz*px, qw*pz + qx*py - qy*px + qz*pw ]

这个乘法在代码里就是一个函数，调用频率很高，要尽量优化。我一般用宏或者内联函数，避免函数调用开销。

3.3 从四元数到欧拉角的转换

解算出的四元数最终要转成欧拉角给用户看。转换公式如下（假设旋转顺序为Z-Y-X，即先Yaw再Pitch再Roll）：

• Roll = arctan2(2*(qwqx + qyqz), 1 - 2*(qx² + qy²))
• Pitch = arcsin(2*(qwqy - qzqx))
• Yaw = arctan2(2*(qwqz + qxqy), 1 - 2*(qy² + qz²))

注意Pitch用arcsin，因为它的范围是-90到+90度，正好对应sin函数的单调区间。Roll和Yaw用arctan2，范围-180到+180度。这些公式在代码里就是几行三角函数，但要注意浮点精度和除零保护。

实操心得：转换出来的欧拉角在Pitch接近±90度时，Roll和Yaw会剧烈跳动，这是正常的数学现象，不是算法bug。如果应用需要显示欧拉角，建议在UI层做平滑处理，或者直接显示四元数。

4. 传感器融合算法实战：互补滤波、Mahony与Madgwick

4.1 互补滤波：最简单可用的入门方案

互补滤波是理解传感器融合的最佳起点。它的核心思想用一句话概括：对加速度计解算的角度做低通滤波，对陀螺仪积分角度做高通滤波，然后相加。用公式表示：

angle = α * (angle + gyro * dt) + (1 - α) * accel_angle

其中α是滤波系数，通常取0.95到0.99之间。α越大，越信任陀螺仪，动态响应好但漂移校正慢；α越小，越信任加速度计，静态准但动态噪声大。这个系数需要根据你的采样率和应用场景调。采样率1kHz时，α=0.98对应的时间常数大约是1/(1-0.98)/1000 = 0.05秒，意味着加速度计大约50ms能校正一次陀螺仪漂移。

互补滤波的优点是计算量极小，一个浮点乘加就能更新，在8位单片机上都能跑。缺点是只对俯仰和横滚有效，航向角没有绝对参考（除非加磁力计），而且滤波系数固定，无法自适应调整。另外，它是在欧拉角空间做融合，仍然有万向节死锁的风险。

我早期做平衡车时用的就是互补滤波，代码不到20行，效果对于慢速倾斜场景完全够用。但如果你要做无人机特技飞行或者VR头显的快速转动，互补滤波的相位滞后会让你感觉“手感发虚”。

4.2 Mahony滤波：PI控制器校正的优雅实现

Mahony滤波的核心创新在于：它不是在欧拉角空间做融合，而是在四元数空间，用PI控制器来校正陀螺仪的漂移。具体来说，它把加速度计测量的重力方向与当前四元数估计的重力方向做叉积，得到误差向量，然后用这个误差去修正陀螺仪的角速度。

误差向量的计算：假设当前四元数估计的重力方向是v_est = [2*(qxqz - qwqy), 2*(qyqz + qwqx), qw² - qx² - qy² + qz²]，加速度计测量归一化后是v_meas = [ax, ay, az]，那么误差e = v_meas × v_est。这个叉积的物理意义是：如果估计姿态准确，两个向量应该重合，叉积为零；如果有偏差，叉积方向指示了需要旋转的方向，大小与偏差角度成正比。

然后把这个误差送入PI控制器：

• 比例项：Kp * e，直接修正角速度
• 积分项：Ki * ∫e dt，消除稳态误差

修正后的角速度ω_corrected = ω_gyro + Kpe + Ki∫e dt，再用这个角速度去更新四元数。

Kp和Ki的调参是关键。Kp决定收敛速度，太大容易振荡，太小收敛慢。Ki消除稳态误差，但太大会引入低频振荡。根据Madgwick的论文和我的实测经验，采样率1kHz时，Kp取0.5到2.0，Ki取0.001到0.01比较合适。如果加速度计噪声大，Kp要取小一点；如果陀螺仪零偏大，Ki要取大一点。

Mahony滤波的优点是计算量适中，收敛性好，而且PI控制器的物理意义明确，调参有方向感。缺点是仍然假设加速度计只测量重力，在有持续线性加速度的场景（比如无人机长时间加速）下会引入误差。

4.3 Madgwick滤波：梯度下降的工程化方案

Madgwick滤波是Sebastian Madgwick在2010年提出的，它的思路和Mahony不同：不是用PI控制器修正，而是把姿态估计问题转化成一个优化问题——找到一个四元数，使得它估计的重力方向与加速度计测量方向的误差最小。然后用梯度下降法迭代求解。

具体来说，定义目标函数f(q) = q* ⊗ g ⊗ q - a，其中g是重力向量[0,0,1]，a是归一化的加速度计测量值，q*是四元数共轭。这个函数表示的是：用当前四元数把重力向量旋转到机体坐标系，与加速度计测量值的差异。我们要找q使得f(q)最小。

梯度下降的更新公式：q(t+Δt) = q(t) + (dq_gyro - β * ∇f/|∇f|) * Δt，其中dq_gyro是陀螺仪积分项，∇f是目标函数的梯度，β是梯度下降的步长，控制加速度计校正的权重。

β的物理意义是：陀螺仪测量误差的估计值。如果陀螺仪很准，β取小一点；如果陀螺仪漂移大，β取大一点。典型值在0.01到0.1之间。Madgwick论文里推荐β = sqrt(3/4) * ω_β，其中ω_β是陀螺仪测量误差的均方根值。

Madgwick滤波的优点是收敛速度快，计算量比卡尔曼滤波小很多，而且有开源实现可以直接用。缺点是梯度下降的步长固定，在动态加速度大的场景下仍然会受影响。另外，它的推导涉及较多数学，第一次看容易懵。

4.4 三种算法的对比与选型建议

选型建议：如果你是第一次做姿态解算，从互补滤波开始，理解融合的基本逻辑。然后上Mahony或Madgwick，两者性能接近，Mahony的PI控制器更直观，Madgwick的梯度下降收敛更快。卡尔曼滤波除非你有很强的数学背景或者需要融合多种传感器（比如加GPS、视觉），否则不建议一上来就搞。

实操心得：我实测下来，MPU6050在1kHz采样、42Hz DLPF下，Mahony滤波Kp=1.0、Ki=0.005，静态角度误差小于0.5度，动态响应延迟约10ms，完全满足无人机和VR的基本需求。Madgwick滤波β=0.05时性能相当，但代码更简洁。

5. 完整实现流程：从I2C读取到姿态输出

5.1 硬件连接与初始化配置

先讲硬件。MPU6050的引脚不多：VCC接3.3V（注意不要接5V，会烧），GND接地，SCL和SDA接单片机的I2C引脚，另外INT引脚可以接中断用于数据就绪通知。如果你用的是模块，通常还带一个XDA和XCL，用于连接外部磁力计，6DoF场景不用管。

I2C地址：AD0接地时是0x68，接VCC时是0x69。大多数模块默认AD0接地，地址0x68。

初始化流程：

1. 复位设备：向PWR_MGMT_1寄存器（0x6B）写0x80，等待100ms。
2. 唤醒：向PWR_MGMT_1写0x00，退出睡眠模式。
3. 设置采样率：向SMPLRT_DIV寄存器（0x19）写分频值。采样率 = 陀螺仪输出率 / (1 + 分频值)。陀螺仪输出率默认1kHz，所以写0x00得到1kHz，写0x09得到100Hz。
4. 设置DLPF：向CONFIG寄存器（0x1A）写带宽值。0x03对应42Hz，0x04对应20Hz，0x05对应10Hz。带宽越低，噪声越小，但延迟越大。
5. 设置陀螺仪量程：向GYRO_CONFIG寄存器（0x1B）写0x08对应±500 dps，0x10对应±1000 dps。
6. 设置加速度计量程：向ACCEL_CONFIG寄存器（0x1C）写0x08对应±4g，0x10对应±8g。

初始化完成后，读取WHO_AM_I寄存器（0x75）应该返回0x68，确认通信正常。

5.2 原始数据读取与单位转换

MPU6050的数据寄存器从0x3B开始，连续14个字节：加速度X高、低，Y高、低，Z高、低，温度高、低，陀螺仪X高、低，Y高、低，Z高、低。一次I2C连续读14字节，比分开读效率高很多。

单位转换：

• 加速度：原始值 / 灵敏度。±4g时灵敏度是8192 LSB/g，所以加速度(g) = raw / 8192.0。
• 陀螺仪：原始值 / 灵敏度。±500 dps时灵敏度是65.5 LSB/dps，所以角速度(dps) = raw / 65.5。转弧度要乘以π/180。

温度：raw / 340.0 + 36.53，单位摄氏度。温度可以用来做零偏补偿，但MPU6050的温度传感器精度一般，只能粗略参考。

5.3 零偏校准的实操步骤

校准代码逻辑：

#define CALIB_SAMPLES 2000 float gyro_bias[3] = {0, 0, 0}; void calibrate_gyro() { float sum[3] = {0, 0, 0}; for (int i = 0; i < CALIB_SAMPLES; i++) { read_mpu6050(); if (i >= 100) { // 跳过前100个样本 sum[0] += gyro_raw[0]; sum[1] += gyro_raw[1]; sum[2] += gyro_raw[2]; } delay_ms(1); } gyro_bias[0] = sum[0] / (CALIB_SAMPLES - 100); gyro_bias[1] = sum[1] / (CALIB_SAMPLES - 100); gyro_bias[2] = sum[2] / (CALIB_SAMPLES - 100); }

校准完成后，每次读取陀螺仪数据都要减去零偏。注意零偏是原始值，不是转换后的物理量，所以要在转换前减。

注意：校准期间传感器必须完全静止。我试过在桌上校准，结果同事走过带来的振动让零偏偏了0.5 dps，积分几分钟后角度就飘了。最好放在水泥地面或者减震台上校准。

5.4 Mahony滤波的代码实现与参数调试

下面是一个精简的Mahony滤波实现，跑在STM32F103上，1kHz采样，CPU占用不到5%。

float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 四元数 float Kp = 1.0f, Ki = 0.005f; float exInt = 0, eyInt = 0, ezInt = 0; void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; // 归一化加速度计数据 norm = sqrt(ax*ax + ay*ay + az*az); if (norm == 0) return; ax /= norm; ay /= norm; az /= norm; // 估计重力方向 vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 误差叉积 ex = ay*vz - az*vy; ey = az*vx - ax*vz; ez = ax*vy - ay*vx; // PI控制器 exInt += ex * Ki * dt; eyInt += ey * Ki * dt; ezInt += ez * Ki * dt; // 修正角速度 gx += Kp*ex + exInt; gy += Kp*ey + eyInt; gz += Kp*ez + ezInt; // 四元数积分 float halfT = 0.5f * dt; float q0_last = q0, q1_last = q1, q2_last = q2, q3_last = q3; q0 += (-q1_last*gx - q2_last*gy - q3_last*gz) * halfT; q1 += ( q0_last*gx + q2_last*gz - q3_last*gy) * halfT; q2 += ( q0_last*gy - q1_last*gz + q3_last*gx) * halfT; q3 += ( q0_last*gz + q1_last*gy - q2_last*gx) * halfT; // 归一化四元数 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm; }

调参步骤：

1. 先把Ki设为0，只调Kp。从0.5开始，逐渐增大到2.0，观察静态时角度是否稳定、动态时是否跟得上。如果角度振荡，Kp太大；如果收敛慢，Kp太小。
2. Kp调好后，加Ki。从0.001开始，逐渐增大到0.01。Ki的作用是消除稳态误差，但如果太大，会在低频段引入振荡。
3. 用串口打印欧拉角，手动转动传感器，观察角度变化是否平滑、有无跳变。

我一般用Kp=1.0、Ki=0.005作为起点，大多数场景下微调即可。

5.5 欧拉角输出与数据可视化

解算出的四元数转欧拉角：

float roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2)) * 57.29578f; float pitch = asin(2*(q0*q2 - q3*q1)) * 57.29578f; float yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3)) * 57.29578f;

57.29578是180/π，把弧度转成度。

数据可视化推荐用匿名上位机或者VOFA+，通过串口发送欧拉角，实时看波形。调参时看波形比看数字直观得多。我习惯把Roll、Pitch、Yaw三条曲线放在同一个窗口，转动传感器时观察三条曲线的响应和耦合情况。

实操心得：如果发现Yaw角缓慢漂移，这是正常的——6DoF没有绝对航向参考，陀螺仪零偏残余会导致Yaw积分漂移。漂移速率取决于零偏校准精度，一般每小时几度到几十度。如果需要绝对航向，加磁力计做9DoF融合，或者用视觉/UWB做外部校正。

6. 常见问题与排查技巧实录

6.1 姿态解算问题速查表

6.2 三个我踩过的坑

第一个坑是I2C时钟拉伸。MPU6050在某些情况下会拉伸SCL时钟，如果单片机的I2C外设不支持时钟拉伸，通信会失败。我用的STM32F103硬件I2C就遇到过这个问题，后来换成软件I2C才稳定。如果你也遇到I2C随机NACK，先试试降低速率到100kHz，或者换软件I2C。

第二个坑是加速度计的量程选择。我做第一版无人机飞控时，加速度计量程选了±2g，结果电机一启动，加速度计就饱和了，姿态直接飞掉。后来换成±8g，问题解决。经验是：只要场景有振动或冲击，加速度计量程至少±8g，陀螺仪至少±1000 dps。

第三个坑是四元数归一化频率。我一开始每100次更新才归一化一次四元数，结果发现角度慢慢偏了。后来改成每次更新都归一化，问题消失。四元数归一化的计算量不大（一次sqrt和四次除法），1kHz下完全不是瓶颈，没必要省。

6.3 进阶优化方向

如果你已经跑通了基本的6DoF姿态解算，想进一步提升精度和鲁棒性，可以考虑这几个方向：

• 自适应滤波：根据加速度计的置信度动态调整Kp或β。当加速度计测量值接近1g时，认为可信，增大校正权重；当偏离1g时，认为有线性加速度，减小校正权重。
• 温度补偿：MPU6050内置温度传感器，可以建立零偏-温度查找表，实时补偿。
• 多传感器融合：加磁力计做9DoF，加气压计做高度，加GPS做位置，用扩展卡尔曼滤波统一融合。
• 视觉惯性融合：用摄像头做视觉里程计，与IMU数据融合，实现高精度的6DoF跟踪。这是目前VR/AR和机器人领域的主流方案。

我个人在实际操作中的体会是：6DoF姿态解算的难点不在算法本身，而在工程细节——传感器校准、数据同步、异常处理、参数调试。算法网上有大量开源实现，但每个硬件平台、每个应用场景的坑都不一样。最好的学习方式就是动手搭一套，从MPU6050开始，把原始数据读出来，把四元数跑起来，把欧拉角显示出来，然后一点点调、一点点优化。踩过的坑越多，理解越深。