求助 四元数姿态解算数据不稳定

求助 四元数姿态解算数据不稳定

在n系中，加速度计输出为，经过bCn（用四元数表示的转换矩阵）转换之后到b系中的值为；在b系中，加速度计的测量值为，现在和均表示在b系中的竖直向下的向量，由此，我们来做向量积（叉积），得到误差，利用这个误差来修正bCn矩阵，于是四元数就在这样一个过程中被修正了。但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
加速度计在静止时测量的是重力加速度，是有大小和方向的；同理，地磁计同样测量的是地球磁场的大小和方向，只不过这个方向不再是竖直向下，而是与x轴（或者y轴）呈一个角度，与z轴呈一个角度。记作，假设x轴对准北边，所以by=0，即。倘若知道bx和bz的精确值，那么就可以采用和加速度计一样的修正方法来修正。只不过在加速度计中，在n系中的参考向量是，变成了地磁计的。如果我们知道bx和bz的精确值，那么就可以摆脱掉加速度计的补偿，直接用地磁计和陀螺仪进行姿态解算，但是你看过谁只用陀螺仪和地磁计进行姿态解算吗？没有，因为没人会去测量当地的地磁场相对于东北天坐标的夹角，也就是bx和bz（插曲：关于这个bx和bz的理解：可以对比重力加速度的理解，就像vx vy vz似的，因为在每一处的归一化以后的重力加速度都是0 0 1然后旋转到机体坐标系，而地球每一处的地磁大小都不一样的，不能像重力加速度那样直接旋转得到了，只能用磁力计测量到的数据去强制拟合。）。那么现在怎么办？前面已经讲了，姿态解算就是求解旋转矩阵，这个矩阵的作用就是将b系和n正确的转化直到重合。现在我们假设nCb旋转矩阵是经过加速度计校正后的矩阵，当某个确定的向量（b系中）经过这个矩阵旋转之后（到n系），这两个坐标系在XOY平面上重合（参考DCM IMU:Theory的Drift cancellation部分），只是在z轴旋转上会存在一个偏航角的误差。下图表示的是经过nCb旋转之后的b系和n系的相对关系。可以明显发现加速度计可以把b系通过四元数法从任意角度拉到与n系水平的位置上，此时只剩下一个偏航角误差。这也是为什么加速度计无法修正偏航的原因。