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

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代表侧倾角度，thetapitch，psi

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

Answer 2

你需要将加速度计的读数按四元数旋转到地球参照系中（如果你喜欢，可以旋转到房间的坐标系中），然后减去重力。剩余的加速度是地球参照系中传感器的加速度，通常称为线性加速度或用户加速度。

在伪代码中，像这样的东西

acceleration = [ax, ay, ay]  // accelerometer reading

q                            // quaternion corresponding to the orientation

gravity = [0, 0, -9.81]      // gravity on Earth in m/s^2

a_rotated = rotate(acceleration, q) // rotate the measured acceleration into
                                    // the Earth frame of reference

user_acceleration = a_rotated - gravity

你说你可以通过API得到q。唯一不平凡的步骤是实现 rotate（） 函数。

要计算向量 v 在旋转 q 时的图像，应应用以下公式：_旋转 v = qvq-1。要用浮点数计算它，你需要自己计算公式;它们在使用四元数旋转处可用。

据我所知，你提供的链接就是这样做的，你可以看到那里的扩展公式，现在你知道它们是从哪里来的了。此外，链接的内容似乎以g为单位测量重力，也就是说，重力是[0,0，-1]。

注意符号约定（无论您是否考虑重力[0,0，-1]或[0,0,1]）和坐标系的手性！