如何根据 3d 陀螺仪数据更新四元数？答案

【问题标题】：How to update quaternion based on 3d gyro data?如何根据 3d 陀螺仪数据更新四元数？
【发布时间】：2014-06-23 13:34:38
【问题描述】：

我有一个初始四元数 q0。我得到角速度测量值，我整合了速度，所以我在 50Hz 左右得到了 3 个角度。如何根据 3 个角度制作四元数？我不能只做 3 个四元数，可以吗？

所以说清楚。

Q.new=Q.new*Q.update(alfa,beta,gamma)

Q.new 以四元数表示我当前的方向，我想通过乘以 Q.update 四元数来更新它。如何使用角度进行 Q.update？

谢谢！

【问题讨论】：

不幸的消息是：它很多比这复杂。参见例如Orientation estimation using a quaternion-based indirect Kalman filter with adaptive estimation of external acceleration。对了，你打算怎么处理陀螺漂移？
这个问题似乎与帮助中心定义的范围内的编程无关。
对不起，伙计们。我两周前刚开始担任软件开发人员，这是我在这里的第一个问题（有趣的是我是一名机械工程师：D）。你是最棒的！关于漂移？我还不知道。

标签： orientation velocity quaternions gyroscope

【解决方案1】：

请原谅我的线程死灵，但答案似乎都很复杂，有些人，比如我，可能更喜欢这种更“方便”的方法：

假设 omega=(alpha,beta,gamma) 是测得的陀螺仪角速度矢量。然后我们旋转

theta = ||omega||*dt; //length of angular velocity vector

许多单位（度或弧度取决于陀螺仪）

v = omega / ( ||omega|| ); // normalized orientation of angular velocity vector

因此，我们可以将旋转四元数构造为：

Q.update = (cos(theta/2),v_x * sin(theta/2), v_y * sin(theta/2), v_z * sin(theta/2));

现在剩下的就是将我们当前的旋转旋转Q.update。这是微不足道的：

Q.new = multiply_quaternions(Q.update,Q.new); 
// note that Q.update * Q.new != Q.new * Q.update for quaternions

完成。四元数很漂亮，不是吗？

一些关于陀螺仪和四元数的幻灯片可能有用： http://stanford.edu/class/ee267/lectures/lecture10.pdf

【讨论】：

你为什么使用 Q.new = multiply_quaternions(Q.update,Q.new) 而不是 Q.new = multiply_quaternions(Q.new,Q.update)？在您链接的脚本的第 26 页上，它们乘以右侧的 Q.update。
这是一个很好的解决方案，但是如果欧米茄不及时改变怎么办？那么归一化将是无穷大。

【解决方案2】：

我认为您正在积分欧拉角，因为您喜欢让您的生活变得困难。首先，陀螺仪不会直接集成到您的欧拉角中。如果您问这个问题，我将假设您也不知道如何从陀螺测量中正确找到欧拉角的变化率。您需要一个转换矩阵才能使其工作。我强烈建议您阅读 Farrell 的“辅助导航”，第 57 页他解释了如何计算转换矩阵以将您的陀螺速率更改为欧拉速率。但是，当您可以直接从四元数和陀螺仪数据中获取四元数变化率时，何必费心呢：

rate of change quaternion  = qdot
quaternion = q
gyro quaternion = w = [0,gyrox,gyroy,gyroz]

所以

qdot = 0.5 *  q Ⓧ w

其中 Ⓧ 表示四元数积。小心你的框架。陀螺仪表示传感器框架相对于传感器框架中表示的惯性框架的角速率。这意味着您的四元数需要表示从传感器到惯性框架的类似旋转。在这种情况下，它应该代表从惯性系到陀螺仪系的旋转。如果我们忽略地球自转等因素，上述等式是有效的。

注意“辅助导航”。在我看来，他对四元数的处理非常混乱。

【讨论】：

roll、pitch、yaw 四元数乘法的顺序如何，因为在四元数乘法中roll x pitch 不等于pitch x roll
您能详细说明一下吗？为什么陀螺仪数据可以使用 0 作为旋转四元数，qdot = 0.5 * q Ⓧ w 中的 0.5 是什么意思？我们应该标量乘以四元数 q 吗？时间尺度去哪儿了？
关于 0 的使用...此答案中定义的所谓陀螺四元数表示 3d 空间中的向量，因此它的“标量”部分等于零。当使用四元数乘法时，这是允许旋转四元数（具有非零标量部分）对向量进行旋转的必要条件。这里的关键信息是四元数可以表示向量（具有三个基本分量）和旋转（这是围绕方向旋转的角度）。这是两个完全不同的东西。诚然，这很混乱。
关于roll x pitch 与pitch x roll 不同。这可以通过理解三个角度（gyroX * dt、gyroY *dt 和 gyroZ * dt）不是欧拉角来解决。这些是旋转矢量的组成部分。也就是说，它们是围绕 x、y 和 z 轴的旋转，与围绕旋转轴的转动同时发生。根据使用的约定，同一旋转有多个欧拉角表示。最好坚持与四元数密切相关的轴/角度表示
关于0.5的系数。我相信这与这样一个事实有关，即围绕具有分量 (x,y,z) 的轴的旋转角 g 对应于四元数：[cos g/2, sing g/2 x (x,y,z)]。注意半角。但是，这个答案没有正确构建四元数。请参阅下面的更好答案。这里也有描述quaternion rotation

【解决方案3】：

您可以将角速度积分以获得角位置（作为欧拉角），将欧拉角转换为四元数，然后将四元数相乘以累积方向。

假设您的输入由角速度的 3D 向量给出：omega = (alpha, beta, gamma em>)，以每秒度数给出。为了得到欧拉角，E，以度为单位，将 omega 乘以时间变化，我们可以称之为 dt。这导致：

Vector3D omega = new Vector3D(alpha, beta, gamma);
Vector3D E = omega * dt;

您可以通过将当前时间减去上一次更新的时间来获得 dt。从陀螺仪数据中得到 3D 欧拉角后，通过这个方程（来自Wikipedia）将其转换为四元数（w,x,y,z）：

float w = cos(E.x/2) * cos(E.y/2) * cos(E.z/2) + sin(E.x/2) * sin(E.y/2) * sin(E.z/2);
float x = sin(E.x/2) * cos(E.y/2) * cos(E.z/2) - cos(E.x/2) * sin(E.y/2) * sin(E.z/2);
float y = cos(E.x/2) * sin(E.y/2) * cos(E.z/2) + sin(E.x/2) * cos(E.y/2) * sin(E.z/2);
float z = cos(E.x/2) * cos(E.y/2) * sin(E.z/2) - sin(E.x/2) * sin(E.y/2) * cos(E.z/2);

Quaternion q = new Quaternion(w, x, y, z);

只需将上面的两个代码 sn-ps 复制粘贴到您的 Q.update() 方法中，然后返回四元数。如果您想知道方程式是如何工作的，只需查看 Wiki 链接并通读即可。

【讨论】：

我很确定这是错误的。第一部分很接近。角速度的积分为您提供角位置的变化，即从起始方向旋转。旋转向量具有分量 x、y 和 z，但值得注意的是，这些不是欧拉角。旋转围绕旋转轴发生，同时围绕三个轴发生。这里的另一个答案给出了 x,y,z 与四元数的相当密切的关系。欧拉角将旋转描述为一组三个旋转的顺序，并且有多种顺序的约定。
积分角速度不会给你欧拉角。陀螺仪数据给出了与欧拉角不同的身体参考系统的变化率。
这是错误的，因为E.x=pi/2 提供与Ex=pi/2+2*pi 相同的旋转

【解决方案4】：

您可以将这些角度转换为单个四元数，然后执行您描述的操作，或者您可以将它们中的每一个转换为轴角对，然后转换为四元数，然后将三个四元数相乘。请参阅http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles 和 http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 了解更多详情。

【讨论】：

-1 正如我在上面的评论中所说，它很多比这更复杂。请删除此答案。
我假设他已经有了相对于上一个方向的旋转（alpha、beta 和 gamma），只是想通过将两个四元数相乘来更新当前方向。他最初问以下问题：“如何根据 3 个角度制作四元数？我不能只制作 3 个四元数，可以吗？”。对于这个问题，我不明白为什么我的答案应该得到-1。但是，如果您将他的问题解释为：“我如何从陀螺仪中获得方向”，那么它当然要复杂得多，并且在集成任何东西之前都应该考虑其他因素，例如陀螺仪漂移。
是的，在我看来，他最终想要的是什么；看起来他正试图追踪方向。 From his comment 显然他不知道陀螺漂移以及如何处理它。告诉他如何进行更新无济于事，因为您帮助他在错误的轨道上走得更远。他越早意识到困难越好，这就是我认为的实际帮助。
同意。发生的事情是我在发表评论之前回答了他原来的问题。无论如何，如果他有 IMU（即陀螺仪和加速度计），我建议他研究一下 Madgwick 的方向过滤器。 x-io.co.uk中有很多资源
我知道漂移，我用加速度计使用互补滤波器对其进行校正：D 我想通了，我准备好后发布代码

【解决方案5】：

1) 更新本身很简单。只需从角速度的“轴角”表示创建四元数。

BWT，表示“角速度”的常用方法是 3 个标量的向量。矢量平行于局部对象框架中的瞬时旋转轴。所以你的方程看起来像：Q.new=Q.update(alfa,beta,gamma)*Q.new。 http://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Unit_quaternions

并考虑积分时间（最简单近似，欧拉积分） Q.new = Q.fromVector(angularVelocity * deltaTime)*Q.new

另外注意，angularVelocity * deltaTime 产生“指数映射”旋转，很容易转换成四元数。

为了精确积分，应该使用更复杂的近似值。但您必须知道测量的确切含义（测量时间、噪声等）。

2）您的问题不清楚功能更新。什么是 (alfa,beta,gamma) 参数？如果这是关于 3 个正交轴的瞬时旋转速度，那么您可以简单地由此建立角速度。请确保使用适当的单位（弧度/秒）。

3) 获得有用的加速度计数据集成的方法对于简短的回答来说太复杂了。每个硬件都应该使用自己的自定义属性来处理。它还应该融合来自线性加速度的数据以避免漂移。

【讨论】：

如我所说，信息太少。我猜它可能看起来像： update(alfa,beta,gamma) { Vector3 r = Vector3(alfa,beta,gamma) * deltaTimeSec;双 l = r.length();四元数 rQ = (0.5*rxsin(l)/l, 0.5*rysin(l)/l), 0.5*rz*sin(l)/l, 0.5*cos(l) ) ;返回 rQ； }