陀螺仪姿态积分:圆锥修正与符号连续
陀螺仪角增量没有丢失,姿态计算仍可能出现系统性错误。一类错误来自把不同轴的有限转动当成普通向量相加,另一类来自四元数符号的任意翻转,分别需要在旋转合成和表示连续性上约束。
绕两个不同轴先后转动,结果通常取决于先后顺序。把一个更新周期内各轴角增量直接相加,会保留总量,却丢掉这种顺序信息。在连续多轴摆动中,即使某些轴的角增量和接近零,实际合成姿态也未必返回原点。由旋转非交换性产生的高阶项,会在重复运动下积累成明显偏差。圆锥修正的任务,是利用周期内更细的角增量序列近似恢复这些项,而不是用滤波把角速率摆动全部压掉。
展开小转角的旋转乘积时,可以看到与前后角增量叉积相关的项,其方向取决于约定和运动顺序。实现中应根据传感器提供的是瞬时角速率、区间平均角速率还是区间角增量,选用相应的离散模型。不同插值假设、子样本数量和增量定义可能对应不同修正系数,不能只抄一个叉积系数便认为算法完整。若输入已经在器件内部完成了相关积分处理,主控再次修正还可能重复计算,必须先确认输出语义。
检验算法可以构造小幅、连续的多轴运动,并用更高精度的旋转矩阵连乘作为参考,比较不同更新步长下的姿态误差。还应改变运动顺序,检查误差方向是否随之符合预期。只做单轴匀速测试通常难以暴露这类问题,因为同轴旋转天然可交换。高频信息若在进入算法前已经被合并丢失,也无法由后续修正唯一重建;因此验收应覆盖输入增量的形成方式,而不能只审核最后的姿态更新公式。
完成正确旋转合成后,四元数还存在独立的表示问题。同一个三维姿态可由一组单位四元数表示,也可由它的整体相反数表示,两者对应的旋转矩阵完全相同。若上游模块在两个表示之间切换,物理姿态并未跳动,四个数值分量却可能同时突变。直接对这些分量做差、低通或插值,就可能制造很大的虚假变化,甚至把本来相邻的两个姿态平均到接近零范数,使后续归一化失去稳定依据。
对相邻姿态间真实转角小于半周、且需要采用最短插值路径的常见情况,可比较相邻四元数的点积。点积为负时,把其中一组整体取反,再进行连续插值或分量处理,从而让两组表示落在同一局部分支。这一操作只改变表示,不改变真实姿态,不能拿它掩盖实际的快速运动。若采样间可能发生超过半周的转动,仅凭两个姿态端点就无法唯一恢复过程,需要额外的运动信息来决定应选路径。
强制某一个分量永远为正,也未必能够保持整段轨迹连续;当该分量经过零附近时,仍可能触发整体翻转。应根据实际操作区分规范化存储、时间连续表示和插值路径,并固定四元数分量顺序、坐标变换方向与乘法约定。陀螺仪驱动、姿态传播和显示模块之间最好用已知旋转进行端到端核验。否则某一模块内部公式正确,跨模块组合后仍可能因为符号与约定不一致而给出相反或不连续的结果。
多轴积分应保留旋转顺序,姿态传递应维护适用的符号连续性。前者决定合成姿态是否正确,后者决定后续计算如何解释同一姿态,两项检查都不能被简单归一化替代。





