姿态解算这四个字，在做惯性测量、飞控、云台稳定、手柄追踪、机器人关节控制的人眼里，基本等同于每天都要碰的必修课。而只要你的解算链路里出现了四元数（很多资料和代码注释里写成“四元素”，其实是同一个东西的两种叫法，英文都是 quaternion），就意味着你已经不满足于让欧拉角在屏幕上转一转、看着好看就行——你需要的是一个不发散、不奇异、能连续跑几个小时甚至几天的姿态表示。四元数能干什么？说白了，它用四个数把三维空间里的任意旋转打包起来，既避开了欧拉角在俯仰角接近 90 度时的万向节死锁，又不像方向余弦矩阵那样要维护九个元素还得反复做正交化。它解决的问题很具体：让陀螺仪的角速度积分能一直积下去而不崩掉，让加速度计和磁力计能有一个稳定的融合目标，让姿态输出在跨越 ±180 度时不会突然跳变。这篇内容适合三类人看：刚上手六轴传感器、还在纠结欧拉角为什么转着转着就乱掉的新手；已经能把四元数乘法和归一化写出来、但一上真机就跑歪的中级选手；以及需要在嵌入式平台上把算力压到极限、还要保证长期稳定性的工程实践者。我会从“为什么最后都选它”讲起，一路讲到代码实现、参数整定和实测踩过的坑，中间该给公式给公式，该给代码给代码，能直接抄的部分我都写成可运行的片段。

1. 为什么姿态解算最后都绕不开四元数

1.1 从欧拉角的万向节死锁说起

三维空间的旋转有三个自由度，这件事决定了你至少需要三个参数来描述姿态。欧拉角就是最符合直觉的三参数方案：绕 Z 转一个偏航，绕 Y 转一个俯仰，绕 X 转一个横滚，人一看就懂。但它有个致命问题——当第二次旋转（俯仰）接近 ±90 度时，第一次旋转和第三次旋转的轴会重合，两个自由度塌缩成一个，这就是常说的万向节死锁。在数学上的表现是：从旋转矩阵反解欧拉角时，某个分母趋近于零，atan2 的两个参数同时趋近于零，解算出来的角度会剧烈抖动甚至直接跳变。

我用一个具体的数字说明这件事有多难受。假设你的设备是一个云台，俯仰角从 0 度慢慢抬到 85 度、88 度、89.5 度，你会看到横滚角和偏航角的数值开始疯狂往相反方向跑，两者的和倒是基本保持不变。人眼看着莫名其妙，但如果这是一台需要输出稳定画面的设备，画面就会在接近垂直俯视时突然“翻个身”。很多新手第一反应是“传感器坏了”，换传感器、换滤波、加延时，折腾一圈发现问题照旧，其实根源在姿态表示方法本身，跟你用什么芯片没关系。

四元数绕开这个坑的方式很巧妙：它不用三个角度去拼，而是用四个数描述“绕某根轴转某个角度”这件事的等价表示，四维空间里的单位球面到三维旋转群是一个双覆盖映射，中间没有奇异点。换句话说，四元数在整个旋转空间里是光滑的、处处可导的，你可以放心地对它做插值、做积分、做低通滤波，不用担心某一天积分积到一个“禁区”里去。这也是为什么所有正经的姿态融合算法，内部状态量几乎都用四元数，只在最终输出给人看的时候才转成欧拉角。

1.2 方向余弦矩阵的九个元素与正交化麻烦

另一个常见的选择是方向余弦矩阵，也就是常说的旋转矩阵 R，3×3 共九个元素。它的优点是直观：把一个向量左乘 R 就完成了坐标变换，跟线性代数的习惯完全一致，而且多个旋转连续作用就是矩阵连乘，写起来极其干净。在需要频繁做向量变换的场景（比如把机体系的速度转到导航系），矩阵形式确实最省事。

但矩阵的问题在于冗余。九个元素里真正独立的只有三个，剩下的六个是约束。你用角速度积分去更新矩阵时，数值误差会让这个矩阵慢慢偏离“正交且行列式为 1”的理想状态，表现为向量变换后长度发生变化——本来是纯旋转，结果带上了微小的缩放和剪切。跑几分钟可能看不出来，跑几个小时之后，姿态就会明显“塌陷”。解决办法是周期性做正交化，常见的有 Gram-Schmidt 方法或者用 SVD 找最近的旋转矩阵。这两种操作在嵌入式平台上都很贵：Gram-Schmidt 要做两次叉积加三次归一化，SVD 更不用说，基本上不会有人把它放在 200Hz 的中断里跑。

相比之下，四元数只有四个元素加一个约束（模长为 1），归一化就是除以自己的模长，四次乘法、一次开方、四次除法，算力开销小一个数量级。这是工程上极其关键的一点：约束越少、越容易维护，长期运行的稳定性就越好。我自己在做长时间静态测试时对比过，同样跑八小时，四元数方案每帧归一化后模长偏差在 1e-7 量级，而矩阵方案如果不做正交化，元素偏差已经到 1e-3 量级了，姿态误差肉眼可见。

1.3 四元数的四个数与“单位球面上的旋转”

四元数写成 q = w + xi + yj + zk，或者用向量形式记成 q = [w, x, y, z]，其中 w 是实部，xyz 是虚部。它有一套自己的代数规则，i² = j² = k² = ijk = -1，乘法不满足交换律。这套规则初看很抽象，但如果你把它理解成“四维空间里的一个点”，一切就顺了：单位四元数（模长为 1）构成四维空间里的一个单位球面，球面上的每一个点对应三维空间里的一个旋转，而且每个旋转对应球面上两个点（q 和 -q 表示同一个旋转），这就是所谓的双覆盖。

这个“双覆盖”性质在实际工程里有个非常实用的副作用：它天然保证了姿态插值的最短路径。当你需要在两个姿态之间做平滑过渡（比如云台从当前朝向转到目标朝向），用四元数做球面线性插值（SLERP），会自动走最短的那条弧，不会出现“转了一大圈才到位”的尴尬。用欧拉角做插值就没这个保证了，三个角度各自线性插值，组合出来的路径可能完全不是你想要的，甚至在接近死锁区时会剧烈甩动。

还有一点容易被忽略：四元数做增量旋转的复合特别方便。角速度积分本质上就是“每帧产生一个微小的增量旋转，然后把它乘到当前姿态上”。四元数的乘法正好表达“先做 A 旋转再做 B 旋转”，写成 q_new = q_old ⊗ Δq，一行代码搞定。如果用矩阵，你得做一次 3×3 乘 3×3 的矩阵乘法，二十七次乘加，再做正交化，代码量和算力都上去了。

2. 四元数的核心数学：把旋转塞进四个数里

2.1 定义、共轭、逆与模长，先把基本盘打牢

在写任何一行解算代码之前，我建议你把这几个基本运算亲手实现一遍，不要直接调库。原因很实在：姿态解算出问题时，你百分之百需要打断点看中间量的数值，如果连共轭和逆的区别都含糊，排查会非常痛苦。

四元数 q = [w, x, y, z] 的模长定义为 |q| = sqrt(w² + x² + y² + z²)。表示旋转的四元数必须是单位四元数，也就是模长为 1。共轭q* = [w, -x, -y, -z]，实部不变、虚部取反。逆q⁻¹ = q* / |q|²，当 q 是单位四元数时，逆就等于共轭，这个简化极其常用，也是为什么我们每帧都要坚持归一化——归一化之后，求逆就只是四次取反，省掉一次除法。

这里有个很常见的误解值得点出来：很多人以为“姿态四元数的逆就是反向旋转”，这话只在单位四元数成立的前提下才对。如果你的代码里因为某种原因漏了归一化，模长变成 0.98，那你用共轭当逆来用，得到的结果就带了一个 1/0.96 的缩放因子，姿态会慢慢缩水。我见过一个项目，跑十几分钟姿态就开始“变慢”，最后查出来就是归一化被放在了一个条件分支里，某些帧被跳过了。

typedef struct { float w, x, y, z; } quat_t; static inline float quat_norm(const quat_t *q) { return sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z); } static inline void quat_normalize(quat_t *q) { float n = quat_norm(q); if (n < 1e-6f) { /* 防零除，异常时重置为单位四元数 */ q->w = 1.0f; q->x = q->y = q->z = 0.0f; return; } float inv = 1.0f / n; q->w *= inv; q->x *= inv; q->y *= inv; q->z *= inv; } static inline quat_t quat_conj(const quat_t *q) { quat_t r = { q->w, -q->x, -q->y, -q->z }; return r; }

上面这段代码里那个n < 1e-6f的保护分支不是凑数的。在传感器上电瞬间或者数据异常（比如 I2C 读到了全零）时，四元数可能被算成全零，这时候归一化会直接产生 NaN，而 NaN 一旦进入状态量就再也洗不掉了，后面所有输出全是 NaN。加一个兜底重置，比事后调试三天要划算得多。

2.2 四元数乘法：手工推一遍，代码才不会写错

四元数乘法是整个解算的地基，写错了后面全错，而且错得很隐蔽——姿态看起来还在动，就是方向不对。它的定义是：

q ⊗ p = [w_q·w_p - x_q·x_p - y_q·y_p - z_q·z_p, w_q·x_p + x_q·w_p + y_q·z_p - z_q·y_p, w_q·y_p - x_q·z_p + y_q·w_p + z_q·x_p, w_q·z_p + x_q·y_p - y_q·x_p + z_q·w_p]

记忆方法有很多，我自己习惯这样记：实部是“实实减去虚虚点积”，虚部三项是“实乘虚 + 虚乘实 + 虚叉乘”。注意虚部最后那个叉乘项的符号，它正是乘法不满足交换律的来源，也是最容易写反的地方。写成代码：

static inline quat_t quat_mul(const quat_t *a, const quat_t *b) { quat_t r; r.w = a->w*b->w - a->x*b->x - a->y*b->y - a->z*b->z; r.x = a->w*b->x + a->x*b->w + a->y*b->z - a->z*b->y; r.y = a->w*b->y - a->x*b->z + a->y*b->w + a->z*b->x; r.z = a->w*b->z + a->x*b->y - a->y*b->x + a->z*b->w; return r; }

验证乘法写对没有，我推荐两个土办法，都很有效。第一个是单位元测试：用 [1,0,0,0] 去乘任意四元数，结果必须原样返回，这一条能筛掉大部分低级错误。第二个是模长测试：任意两个单位四元数相乘，结果的模长必须仍然是 1（允许 1e-6 的浮点误差）。如果模长跑到了 1.02 或者 0.97，说明你某个符号写反了，多半就是虚部叉乘那三项。这两条测试我写在单元测试里，每次改代码都跑一遍，省了很多事。

2.3 用四元数旋转一个向量，符号约定必须一次定死

把一个三维向量 v 用四元数 q 旋转，标准写法是 v' = q ⊗ [0, v] ⊗ q*。展开成纯向量运算，可以写成更高效的形式：

v' = v + 2w(u × v) + 2u × (u × v)，其中 u = [x, y, z] 是四元数的虚部

这个形式的计算量比完整做两次四元数乘法要小，而且中途不需要构造 [0, v] 这种临时结构，在嵌入式上很实用：

static inline void quat_rotate_vec(const quat_t *q, const float v[3], float out[3]) { float ux = q->x, uy = q->y, uz = q->z; /* t = 2 * (u × v) */ float tx = 2.0f * (uy*v[2] - uz*v[1]); float ty = 2.0f * (uz*v[0] - ux*v[2]); float tz = 2.0f * (ux*v[1] - uy*v[0]); /* out = v + w*t + u × t */ out[0] = v[0] + q->w*tx + (uy*tz - uz*ty); out[1] = v[1] + q->w*ty + (uz*tx - ux*tz); out[2] = v[2] + q->w*tz + (ux*ty - uy*tx); }

代码写出来了，但真正的坑在符号约定上。同一个四元数，在不同资料里有两种完全相反的解释：一种认为它表示“把机体系向量转到世界系”（body-to-world），另一种认为表示“世界系到机体系”（world-to-body）。这两种约定下，同一个姿态的四元数互为共轭，加速度计重力方向的计算公式也完全不同。如果你看的资料和参考代码用了不同约定，拼起来就会出现“静止时姿态是对的，一转动方向就反了”这种诡异现象。

我的建议是：在项目开始时就把约定写进注释，并且用一个可验证的物理场景做锚点。比如约定 q 为 body-to-world，那么静止水平放置时，机体系测到的重力方向应该是 [0, 0, +1]（如果传感器 Z 轴朝上），转到世界系后应该是 [0, 0, +1] 经过 q 变换的结果。把这个锚点写成单元测试，后面所有公式都拿它对，能省掉大量来回试的时间。

2.4 与欧拉角、旋转矩阵、轴角的互转，别在这里翻车

四元数最终还是要转成人类能看懂的角度。转欧拉角的标准公式（以 ZYX 顺序为例）是：

roll = atan2(2(wx + yz), 1 - 2(x² + y²)) pitch = asin(2(wy - zx)) yaw = atan2(2(wz + xy), 1 - 2(y² + z²))

用的时候要留意两件事。一是 asin 的参数在浮点误差下可能略微超出 [-1, 1]，直接喂进去会返回 NaN，所以必须做 clamp。二是 yaw 在 ±180 度处会跳变，如果你要做角度平滑或者做 PID 控制，必须先把跳变处理掉，否则控制器会在跨越那一点时输出一个巨大的反向修正，设备会猛地抽一下。处理办法是把角度差归一化到 [-180, 180]，或者干脆在四元数域里做误差计算，最后再转角度。

import math def quat_to_euler(w, x, y, z): sinr = 2.0 * (w * x + y * z) cosr = 1.0 - 2.0 * (x * x + y * y) roll = math.atan2(sinr, cosr) sinp = 2.0 * (w * y - z * x) sinp = max(-1.0, min(1.0, sinp)) # 关键：clamp 防 NaN pitch = math.asin(sinp) siny = 2.0 * (w * z + x * y) cosy = 1.0 - 2.0 * (y * y + z * z) yaw = math.atan2(siny, cosy) return math.degrees(roll), math.degrees(pitch), math.degrees(yaw)

转旋转矩阵的公式在解算里出现频率更高，因为计算重力方向、磁场方向都要用到它。body-to-world 的旋转矩阵用四元数表示是：

这个表我建议你直接抄进代码注释里。实际调试时，重力方向的估计值就是 R 的第三行乘上世界系的重力单位向量，写成展开形式是 [2(xz - wy), 2(yz + wx), 1 - 2(x² + y²)]，前提是世界系重力取 [0,0,1]。这个表达式会在互补滤波里反复用到，写错一个符号，滤波器就会往错误的方向修正，姿态会缓慢地朝某个固定方向倾斜——这个现象非常有辨识度，后面第 5 节会专门讲。

3. 姿态解算的三条主流路线与四元数的位置

3.1 纯角速度积分：最直接，也最容易漂

最朴素的做法就是拿陀螺仪的三轴角速度直接积分：q(k+1) = q(k) + 0.5 · q(k) ⊗ [0, ωx, ωy, ωz] · dt，然后归一化。这段代码短到可以写在中断服务函数里，延迟极低，动态响应极好，快速转动时几乎感觉不到滞后。

但它的缺陷同样明显：积分会累积误差。陀螺仪出厂就有零偏，哪怕只有 0.1 度每秒，一分钟就是 6 度，一小时就是 360 度，姿态直接转了一整圈。而且零偏随温度变化，冷机启动和跑热之后能差出一大截。我实测过一颗常见的六轴传感器，室温静止放置，刚上电时零偏约 0.05 度每秒，跑二十分钟后芯片温度上来了，零偏漂到 0.3 度每秒，纯积分方案在半小时内姿态就歪了十几度。

所以纯积分只适合两类场景：一是时间极短的测量，比如几秒钟的冲击记录；二是配合其他传感器做融合的内环。真要做长时间姿态输出，必须引入加速度计和磁力计做观测量来纠偏，这就是后面两种方案要干的事。

3.2 互补滤波：加速度计管长期，陀螺管短期

互补滤波的思想非常朴素，但极其有效：陀螺仪高频特性好、低频漂，加速度计低频（长期均值）准、高频噪声大，那就让它们各管一段。加速度计测的是重力方向，能告诉我们“哪边是下”，从而约束住横滚和俯仰两个自由度；陀螺仪负责快速变化的动态过程。

实现上最经典的形式是用叉积构造误差。先把加速度计读数归一化得到实测重力方向 a，再用当前四元数估算出机体系下的重力方向 v（也就是上面那个 R 第三行的表达式），两者的叉积 e = a × v 就代表了当前姿态估计偏离真实重力方向的程度。当估计准确时，a 和 v 平行，叉积为零；有偏差时，叉积的方向就是应该旋转的轴，模长正比于偏差角的正弦。

然后把这个误差反馈到角速度上：ω_corrected = ω_gyro + Kp · e，再用修正后的角速度去做积分。Kp 决定了“多信任加速度计”，本质上是给陀螺积分项加了一个一阶低通，截止频率大约与 Kp 成正比。如果再加一个积分项 Ki · ∫e，就能顺便把陀螺零偏估计出来，这就是常见的 PI 型互补滤波，工程上用得非常广。

/* 单帧姿态更新，200Hz 调用，dt = 0.005s */ void attitude_update(quat_t *q, const float gyro[3], const float acc[3], float dt, float Kp, float Ki, float *e_int) { float an = sqrtf(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]); if (an < 0.01f) return; /* 加速度计无效，跳过修正 */ float a[3] = { acc[0]/an, acc[1]/an, acc[2]/an }; /* 估计机体系下的重力方向（世界系重力取 [0,0,1]，body-to-world 约定） */ float v[3] = { 2.0f * (q->x*q->z - q->w*q->y), 2.0f * (q->y*q->z + q->w*q->x), 1.0f - 2.0f * (q->x*q->x + q->y*q->y) }; /* 叉积误差：a × v */ float ex = a[1]*v[2] - a[2]*v[1]; float ey = a[2]*v[0] - a[0]*v[2]; float ez = a[0]*v[1] - a[1]*v[0]; if (e_int) { /* 积分项估零偏 */ e_int[0] += ex * dt; e_int[1] += ey * dt; e_int[2] += ez * dt; } float gx = gyro[0] + Kp * ex + (e_int ? Ki * e_int[0] : 0.0f); float gy = gyro[1] + Kp * ey + (e_int ? Ki * e_int[1] : 0.0f); float gz = gyro[2] + Kp * ez + (e_int ? Ki * e_int[2] : 0.0f); /* 一阶积分：q += 0.5 * q ⊗ [0, ω] * dt */ quat_t dq = { 0.0f, gx, gy, gz }; quat_t qd = quat_mul(q, &dq); q->w += 0.5f * qd.w * dt; q->x += 0.5f * qd.x * dt; q->y += 0.5f * qd.y * dt; q->z += 0.5f * qd.z * dt; quat_normalize(q); }

这段代码里几个细节值得琢磨。an < 0.01f那个判断是必须的，设备做自由落体或者剧烈加减速时，加速度计测到的不是重力而是合加速度，这时候拿它当重力参考会把错误的修正灌进滤波器，典型表现是激烈运动后姿态突然歪掉。积分项用简单矩形积分就行，不必上梯形法，因为 Ki 本来就不大，这点精度差异可以忽略。还有 dt 一定要用实测的间隔，而不是名义值，中断被高优先级任务抢占导致 dt 抖动是很常见的，用固定 dt 会引入系统性误差。

3.3 滤波器方法：把四元数当作状态量

如果你对姿态精度的要求更高，或者需要同时融合磁力计做航向估计，那就要上扩展卡尔曼滤波的思路了。核心是把四元数的四个分量当作状态量，用陀螺角速度做状态预测，用加速度计和磁力计做量测更新。整个流程分两步：

预测步用角速度积分推进状态，同时推进协方差矩阵 P = F·P·Fᵀ + Q，其中 F 是状态转移矩阵，可以由 q(k+1) ≈ (I + 0.5·dt·Ω(ω))·q(k) 线性化得到，Ω(ω) 是那个 4×4 的反对称矩阵。更新步用量测残差 y = z - h(q) 去修正状态，K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹，四元数本身还要做归一化。

说实话，EKF 的调参门槛比互补滤波高一个档次，Q 和 R 两个噪声矩阵要花不少时间去试，而且一旦某个轴的量测失效（比如磁力计被干扰），不做失效检测的话滤波器会直接发散。我的经验是：如果互补滤波能达到精度要求的 1.5 倍以内，就先别上 EKF。互补滤波只有两三个参数，物理意义清楚，调起来直观；EKF 参数之间耦合强，改一个另一个也得跟着动，在小团队里维护成本很高。什么时候该上 EKF？当你的场景需要严格的协方差估计（比如做多传感器融合时想给每个观测源合理加权），或者需要在线估计零偏和其他状态量时，那 EKF 的优势才真正体现出来。

4. 工程实操：从零搭一条四元数解算链路

4.1 数据预处理：零偏标定比算法本身更影响精度

很多人一上来就纠结滤波器怎么调，其实零偏标定的收益远大于调参。标定方法很简单：设备静止水平放置，采集 N 帧陀螺数据（我一般用 2000 到 6000 帧，200Hz 下就是 10 到 30 秒），求每轴均值，这就是静态零偏。上电时从原始数据里减掉，姿态漂移量能立刻下降一个数量级。

def calibrate_gyro(samples): """samples: N x 3 的陀螺原始数据（度/秒）""" n = len(samples) bias = [sum(s[i] for s in samples) / n for i in range(3)] var = [sum((s[i] - bias[i]) ** 2 for s in samples) / n for i in range(3)] std = [v ** 0.5 for v in var] return bias, std

标定完别只看均值，顺便看一眼标准差。标准差能告诉你数据的噪声水平，这个数字在后面调 Kp 时很有用——噪声越大，加速度计那一侧的权重就该越低，Kp 也要相应调小。另外还可以看均值的稳定性：把采集数据分成前后两半分别求均值，如果两个均值差得比较多，说明这段时间设备没真正静止（可能桌面在振动，或者人碰到了），数据得重采。

温度补偿要不要做？取决于你的场景。如果你的设备工作几十分钟以上、并且发热明显，我强烈建议做一次简单的分段补偿：每隔 10 分钟记录一次零偏和温度，拟合一条零偏随温度变化的直线，运行时按当前温度查表修正。这个工作一小时就能做完，但能把长时间运行的漂移压下去一大截。不做补偿也行，那就得靠滤波器里的积分项去在线估零偏，收敛速度会慢一些。

4.2 更新频率与积分阶数：dt 选错了什么算法都救不回来

姿态更新频率的选择要在算力和精度之间权衡。经验上，200Hz 是消费级产品里最常见的折中：一方面陀螺带宽通常在 100Hz 以上，200Hz 采样不会明显丢信息；另一方面 dt = 5ms 时，一阶积分误差已经足够小。

具体算一下误差量级。一阶欧拉积分的单步误差是 O(dt²)，在角速度 ω 下，姿态角误差大约为 0.5·ω·dt²。设 ω = 500 度每秒（剧烈甩动），dt = 0.005s，单步误差约 0.5 × 500 × 2.5e-5 ≈ 0.006 度，看着很小。但如果每秒跑 200 次，一秒累积 1.25 度，甩个三五秒就很可观了。改用二阶龙格-库塔（中点法）能把误差降到 O(dt³)，同样条件下误差降到原来的千分之一量级。二阶的代价是多算一次四元数乘法和一个中间状态，在 Cortex-M 系列上大概多花十几个时钟周期，完全值得。

/* 二阶中点法积分，精度明显优于直接欧拉 */ void quat_integrate_rk2(quat_t *q, float gx, float gy, float gz, float dt) { quat_t k1 = quat_mul(q, &(quat_t){0.0f, gx, gy, gz}); quat_t qm = { q->w + 0.5f*dt*k1.w, q->x + 0.5f*dt*k1.x, q->y + 0.5f*dt*k1.y, q->z + 0.5f*dt*k1.z }; quat_normalize(&qm); quat_t k2 = quat_mul(&qm, &(quat_t){0.0f, gx, gy, gz}); q->w += dt * k2.w; q->x += dt * k2.x; q->y += dt * k2.y; q->z += dt * k2.z; quat_normalize(q); }

注意上面那个 0.5 的系数——有些资料写一阶积分是 q += 0.5·q⊗ω·dt，二阶中点法里中间的 qm 推进也带着 0.5。这些系数容易抄错，抄错的表现是姿态变化速度是真实的两倍或者一半，很容易发现，但也容易让人怀疑是传感器量程或单位问题，绕一圈才回来。建议把单位统一写死在变量名里，比如 gyro_dps、dt_s，从命名上杜绝“弧度还是角度”这类低级错误。

4.3 参数整定：用实验数据定，不要靠猜

互补滤波的 Kp 决定了加速度计修正的强度。理论上它对应一个一阶低通的截止频率，Kp 越大，姿态越“贴”加速度计，长期精度好，但对振动和线性加速度越敏感；Kp 越小，越平滑，但收敛慢，遇到大的初始姿态误差要好几十秒才能贴合。

我自己的整定流程是这样的：先把设备静止放置，初始化四元数故意设成一个偏 30 度的错误姿态，然后观察收敛过程。Kp 太小，十几秒都收不回来；Kp 太大，收敛很快但到了目标姿态附近会来回抖动，甚至有明显的过冲。我一般把 Kp 调到“约 1 到 2 秒内收敛到误差 2 度以内，且稳态时观察不到明显抖动”，在 200Hz 下这个值大概落在 0.5 到 2.0 之间。

然后是 Ki。Ki 用来消掉稳态误差和估计零偏，但积分项天生容易引入慢振荡。调试方法：加大 Ki 直到你看到姿态出现周期约十几秒的缓慢摆动，然后把它砍到原来的三分之一。这个“临界振荡再回退”的方法是老工程师传下来的，虽然土但非常有效。

还有一点：如果设备会振动（比如装在电机上），加速度计的原始数据必须做低通后再用，否则振动会让重力方向的估计一直在跳，Kp 大一点整个姿态就开始嗡嗡抖。低通用二阶 Butterworth 或者干脆用滑动平均都可以，截止频率根据振动频率选，一般在 20 到 50Hz 之间。低通本身会引入相位滞后，但加速度计只用来看长期趋势，这点滞后完全可以接受。

4.4 验证方法：没有真值对比就是在盲调

调参最怕什么？最怕你不知道自己调的到底对不对。手里只有一串姿态数值，看着平滑就当成了，实际上可能整体偏了 5 度而你根本不知道。所以一定要建立一套验证手段。

成本最低的做法是静态多点验证：把设备分别放在几个已知姿态上，比如水平、侧倾 90 度、倒置、俯仰 45 度，每个姿态静止采集 30 秒，看输出的欧拉角是否与几何关系吻合。用直角尺和水平仪就能搭出来，精度足够验证到 1 度以内。这个方法能立刻暴露系统性偏差——如果每个姿态都偏同一个方向 5 度，那大概率是坐标系定义或者安装误差，而不是滤波器的事。

进阶一点的做法是转台对比，没有转台就用一个匀速旋转的单轴平台（哪怕是个每分钟几转的小电机），让设备绕某个轴匀速转，看解算出来的角度是否随时间线性变化、转满一圈后是否回到原点。积分误差、尺度误差在这个测试下无所遁形。我做过一次这样的测试，发现转一圈误差 2.3 度，追查下去是陀螺标度因数的问题，用最小二乘拟合了一条标度修正曲线，误差降到 0.4 度。

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

5.1 姿态慢慢倾斜：九成是重力方向算错了

现象很有辨识度：设备静止放着，姿态不抖不跳，但横滚和俯仰角以肉眼可见的速度朝一个方向缓慢增加，几分钟后能歪十几度。这种“匀速漂”几乎一定是修正项的方向错了，而不是噪声问题。

排查顺序我一般这样走。第一步，把加速度计三轴原始数据和输出的欧拉角同时打出来，静止时看加速度计读数是否稳定在 [0, 0, ±1g] 附近，如果某个轴有 0.1g 以上的偏置，先查硬件安装和校准。第二步，把计算出的 v 向量（估计重力方向）和归一化后的 a 向量一起打印，静止且姿态正确时两者应该几乎相等。如果不等，或者差得很稳定，那说明四元数到重力方向的公式写错了——最常见的是 R 矩阵第三行的符号，尤其是 [2(xz - wy), 2(yz + wx), ...] 里这两个负号，写成正号的表现就是绕某个轴持续漂。第三步，如果 a 和 v 都对，那看叉积 e 的符号，修正方向反了的话把 Kp 取负号试一下就能确认（只是诊断用，别真这么交付）。

还有一种情况值得单独说：只有航向角在漂，横滚俯仰正常。这是符合预期的，因为加速度计只能观测重力方向，对绕重力轴的旋转完全不敏感，航向角本质上不可观测。没有磁力计的话，航向漂移是物理上无法避免的，只能靠陀螺零偏标定把漂移速度压到最低。很多人不理解这一点，反复调 Kp 想把航向调稳，白费功夫。

5.2 姿态突然跳变：先怀疑数据，再怀疑算法

突然跳变和缓慢漂移是两类完全不同的问题。跳变通常有以下几种来源，按出现频率排序：

第一种是加速度计受到非重力加速度干扰。设备被敲了一下、被快速平移、或者装在会振动的平台上，加速度计测到的是重力加运动加速度的合力，方向完全变了。如果这时滤波器还很信任加速度计（Kp 大），姿态就会瞬间被拽偏，等运动结束后再慢慢回来。解决办法是加有效性判断：比较加速度计测到的合加速度模长与 1g 的偏差，超过阈值（比如 0.3g）就临时把 Kp 降到很小甚至置零，只让陀螺积分。

第二种是通信或数据读取异常。I2C 总线被干扰时可能读到一帧全零或者全 0xFF 的数据，如果直接喂给滤波器，就是一次巨大的错误观测。所以读取之后必须做范围检查，超出量程或明显异常的数据直接丢弃。

第三种才是真正的算法问题，比如归一化条件分支被跳过、四元数变成 NaN 后又被后续逻辑“救”回来一个非法值。NaN 有个讨厌的特性：它参与任何运算结果都是 NaN，但如果你的代码里有 clamp 或者 max/min 操作，NaN 的比较行为在某些实现里会导致结果被替换成一个边界值，看起来“恢复正常”了，实际上状态已经废了。

提示：在调试阶段加一条断言，每帧检查四元数四个分量是否都是有限数、模长是否在 0.99 到 1.01 之间。这条检查的开销极小，但能在问题发生的瞬间定位到帧，比事后看波形猜要高效得多。

5.3 四元数不归一化：最隐蔽的一类误差

归一化这事听起来太简单了，简单到很多人觉得“我肯定写对了”。但我见过的真实案例里，归一化相关的 bug 占了相当比例。常见的几种写法错误：归一化放在了if (acc_valid)分支里，加速度计无效的帧就跳过了；归一化用了整型除法；归一化之后又做了一次加法没再归一化；以及在多线程环境里，归一化和读取之间被打断，读到了半更新的状态。

判断有没有归一化问题，最直接的办法是打印模长。正常情况下模长应该在 1.0 附近以 1e-7 的量级波动，如果你看到 1.0003 这种量级的偏差在累积，或者模长呈单调趋势变化，那归一化肯定有问题。还有一个间接指标：观察姿态输出的“尺度感”，如果设备实际转了 90 度，解算出来只有 88 度，而且这个比例随时间变差，那就是模长漂了导致旋转角度被缩放。

5.4 坐标系对不上：最容易浪费一整天的坑

这类问题的典型症状是“所有轴都反了”或者“两个轴对、一个轴反”。它和算法无关，纯粹是定义问题，但排查起来极其耗时，因为现象和算法 bug 长得很像。

我把排查要点整理成一张表，遇到问题时按顺序对照：

我的习惯是在项目最开始就画一张坐标系示意图，标清楚机体系三个轴指向、世界系怎么定义、正方向用右手定则，然后把这张图贴在代码文件头部。后面任何人改代码都对着图看，能避免绝大多数约定混乱。

6. 实测经验与几个容易踩的坑

6.1 浮点精度：float 够用，但要小心累积

嵌入式上默认用 float（32 位）。四元数四个分量在 [-1, 1] 范围内，float 的相对精度约 1e-7，单帧计算完全够用。但要注意累积效应：如果每帧都做归一化，误差不会累积；如果为了省算力改成每 10 帧归一化一次，中间那 9 帧的模长会缓慢偏离，虽然每次偏离很小，但在某些运算（比如连续做几百次旋转复合）里会被放大。

我的做法是：积分后立刻归一化，无条件、无分支。归一化本身开销不大，一次开方加四次乘法，在带硬件浮点的 MCU 上不到 20 个周期。为了省这点算力引入长期漂移风险，完全不划算。如果平台真的没有 FPU，那可以考虑用定点数实现四元数，但那套东西的精度分析和溢出处理要单独做，复杂度陡增，除非成本压到极限，否则我建议直接选带 FPU 的型号。

还有一个细节：转欧拉角时用 double 算 atan2 会更稳一点，尤其是角度接近 ±180 度、参数很小时。这不是必须的，但在做高精度对比测试时能减少一些由数值引起的困惑。

6.2 磁力计：能救航向，也能毁掉一切

加速度计搞不定航向，这是物理限制决定的，想稳住航向只能引入磁力计。但磁力计的使用门槛比加速度计高得多，因为它测的是环境磁场，而环境里到处都是干扰源：电机、扬声器、金属外壳、甚至桌上的螺丝刀。

接入磁力计的第一步不是写融合代码，而是做椭圆拟合标定。理想情况下，设备在空间中任意旋转，磁力计三轴读数应该落在一个球面上（球心是零偏，半径是地磁强度）。实际因为有硬磁干扰（比如外壳上有剩磁），球心会偏离原点，所以要先做硬磁标定，减去球心偏移；如果有软磁干扰（附近有铁磁材料导致各轴灵敏度不同），球会变成椭球，那就得做椭球拟合，乘一个 3×3 的修正矩阵。

import numpy as np def fit_hard_iron(samples): """最小二乘拟合球心，samples: N x 3""" A = np.hstack([2 * samples, np.ones((len(samples), 1))]) b = np.sum(samples ** 2, axis=1, keepdims=True) x, *_ = np.linalg.lstsq(A, b, rcond=None) center = x[:3].flatten() radius = np.sqrt(x[3, 0] + center @ center) return center, radius

标定完之后，判断数据好不好用还有一个实用指标：磁场模长的稳定性。标定良好的情况下，设备在空间里旋转，磁场模长应该稳定在地磁强度附近（国内大部分地区水平分量加垂直分量合起来在 45 到 55 微特斯拉之间），波动不超过几个微特斯拉。如果模长忽大忽小超过 20%，说明有软磁干扰或者附近有会动的铁磁物体，这时候宁可临时关闭磁力计的航向修正，也不要让它污染姿态。我做的一个手持设备就遇到过这个问题——用户手里握着它，手的位置一变磁场就变，最后干脆把磁力计降权到很低，航向靠陀螺短时维持，配合定期的小幅修正，虽然不完美但比被干扰带跑要稳得多。

6.3 初始化：前几秒的姿态往往是错的

上电后姿态需要一段时间收敛，这段时间的输出不可信，但很多产品没有把这个状态告诉用户，导致一开机就显示一个歪的姿态，用户以为坏了。

处理办法是加一个收敛状态机：上电后进入 INIT 状态，此时只做陀螺积分，同时把加速度计读数做低通取平均，估算初始姿态（水平放置时可以直接用加速度计反算横滚俯仰，航向设为零），等连续若干帧的修正量都小于阈值后，再切到 RUN 状态，这时候输出才对外可用。整个收敛过程一般几百毫秒到两秒。

还有一个容易被忽略的点：初始航向的歧义。如果只有加速度计，初始航向只能设为零，意味着每次上电的“前方向”都不一样。对某些应用（比如需要绝对朝向的手柄）这是不可接受的，必须用磁力计定初始航向，或者由外部提供一个参考（比如用户手动对准）。这个设计决策要在需求阶段就定下来，不要等做完了才发现。

6.4 长时间运行的看门狗与状态复位

姿态解算是个长期运行的循环，最怕的是某一帧出现异常数据把状态搞坏，然后一直坏下去。实践经验是加三层防护：第一层是输入数据校验，超量程、NaN、全零的数据直接丢弃并计数；第二层是状态合理性检查，每帧检查四元数模长和是否有限，异常则复位到由加速度计反算的初始姿态；第三层是长时间监控，如果连续 N 秒检测到异常或修正量持续饱和，触发一次彻底重初始化，同时记录一条日志。

这套机制听起来有点小题大做，但在真实设备上跑过几个月你就会明白，传感器偶尔抽风、电源纹波导致的总线误码、温度骤变引起的读数异常，都是必然会发生的。有一个能自愈的状态机，比事后远程排查要省太多事。

我个人在实际项目里的体会是，姿态解算这件事，算法的复杂度和最终效果并不是线性关系。把四元数的基本运算写对、做好零偏标定、保证每帧归一化、加一个带有效性判断的互补滤波，这套组合拳能覆盖 80% 的应用场景，而且代码量小、好调试、出了问题容易定位。真正需要上更复杂方案的时候，往往是因为场景里有强振动、大动态范围或者必须做绝对航向，这时候再针对性地补，而不是一开始就把所有复杂的东西堆上去——堆上去之后出了问题，你连是哪一部分引起的都说不清。