6

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

我找到了这个http://www.varesano.net/blog/fabio/simple-gravity-compensation-9-dom-imus 但无法理解给定方程的基础。

给定上述信息，我怎么能做到这一点？

4

2 回答 2

14

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

在伪代码中，像这样

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

你说你可以q通过API。唯一重要的步骤是实现该rotate()功能。

要计算v旋转时矢量的图像，q应应用以下公式： v旋转= q v q -1。要使用浮点数计算它，您需要自己计算公式；它们可在Using quaternion rotations获得。

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

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

于 2013-08-15T13:47:46.740 回答
0

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

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方向中减去重力以消除静态加速度，即重力。

于 2016-11-13T00:36:32.710 回答