如何求出两个四元数之间的夹角？

Question

我在大腿和小腿上使用惯性测量装置，试图获得所有自由度（x、y、z或屈曲、内收、旋转）的膝关节角度。该公司提供的算法将加速度计、陀螺仪和磁强计数据的9自由度转换为每个传感器的四元数。在

我希望使用给定的四元数获得大腿和小腿传感器之间的x、y和z（膝关节）角度。

我对四元数还很陌生，我正试图通过python理解一种方法来实现这一点，但也要从概念上理解它。在

Answer 1

我想你可以简单地将四元数Q1和Q2应用到参考向量X，Y，Z上，然后计算它们之间的角度。比如：

X1 = Q1*X*conj(Q1);
Y1 = Q1*Y*conj(Q1);
Z1 = Q1*Z*conj(Q1);

X2 = Q2*X*conj(Q2);
Y2 = Q2*Y*conj(Q2);
Z2 = Q2*Z*conj(Q2);


DiffAngleX = acos(dot(X1,X2));
DiffAngleY = acos(dot(Y1,Y2));
DiffAngleZ = acos(dot(Z1,Z2));

Answer 2

免责声明：我自己对四元数还很陌生，但在四元数附近做了一些工作。以下是我有限的知识加上一些谷歌搜索的结果。它看起来确实应该起作用。

所以听起来你要解决的问题可以表述如下：

• 给出两个四元数（分别代表大腿和小腿的3D方向）。。。在
• …计算两个四元数之间的三维角度差。。。在
• 。。。把角差表示为欧拉角

为了得到三维角度差，它本身就是一个四元数，你只需将一个四元数乘以另一个四元数的共轭量（reference）。在

然后需要从四元数转换为Euler角度（围绕X、Y、Z旋转）。据我所知，你需要用老式的方法，使用Wikipedia中的公式。在

示例代码，使用pyquaternion 库：

import pyquaternion as pyq
import math

# Create a hypothetical orientation of the upper leg and lower leg
# We use the (axis, degrees) notation because it's the most intuitive here
# Upper leg perfectly vertical with a slight rotation
q_upper = pyq.Quaternion(axis=[0.0, 0.0, -1.0], degrees=-5)
# Lower leg a little off-vertical, with a rotation in the other direction.
q_lower = pyq.Quaternion(axis=[0.1, -0.2, -0.975], degrees=10)

# Get the 3D difference between these two orientations
qd = q_upper.conjugate * q_lower

# Calculate Euler angles from this difference quaternion
phi   = math.atan2( 2 * (qd.w * qd.x + qd.y * qd.z), 1 - 2 * (qd.x**2 + qd.y**2) )
theta = math.asin ( 2 * (qd.w * qd.y - qd.z * qd.x) )
psi   = math.atan2( 2 * (qd.w * qd.z + qd.x * qd.y), 1 - 2 * (qd.y**2 + qd.z**2) )

# Result:
# phi   = 1.16 degrees
# theta = -1.90 degrees
# psi   = -14.77 degrees

注意事项：

• 我还没有亲自验证这一点的正确性，但它看起来确实应该是正确的。在
• 当然，你需要确认每个角度（φ，θ，psi）的实际方向和符号与你期望的值的对比。在
• 在Wikipedia的文章中（在他们的C示例代码中），他们在计算theta的asin调用中添加了一些更正。我不确定是否需要。但如果θ真的是内收的话，我猜你也不必担心90度以上的角度；-）