加速度计数据中的重力补偿

Question

给定一个具有9个自由度(加速计、陀螺仪和磁强计)的加速度计，我希望消除/补偿加速度计读数中重力的影响(加速度计可以自由旋转)。该传感器以四元数表示相对于(磁性)北、西和上参考系的方向。

我找到了这个http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus，但不能理解给定等式的基础。

根据上述信息，我如何才能实现这一点？

Answer 1

我假设你的加速度计读数在传感器机身框架内。首先，我们需要表示相对于惯性系的加速度计数据，然后减去重力。如果直接使用欧拉角而不是四元数，则需要计算旋转矩阵

R = [
    ctheta*cpsi,
    -cphi*spsi + sphi*stheta*cpsi,
    sphi*spsi + cphi*stheta*cpsi;
    ctheta*spsi, cphi*cpsi + sphi*stheta*spsi,
    -sphi*cpsi + cphi*stheta*spsi;
    -stheta, sphi*ctheta, cphi*ctheta
]

(它由MATLAB表示法给出)。这里phi代表滚动角度，theta代表俯仰，psi代表偏航。这个R矩阵是从物体到惯性系的。我认为在飞行动力学中，它也被称为方向余弦矩阵的转置(DCM)。

当你应用矩阵乘法运算时，现在你需要从z方向减去重力，以消除静态加速度，即重力。