Question

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

我正在使用给定的四元数来获取大腿和小腿传感器之间的x，y和z（膝关节）角度。

我对四元数还很陌生，我试图了解一种通过python做到这一点的方法，但也从概念上理解它。

Answer 1

免责声明：我对Quaternions还是很陌生，但是在“附近”做了一些工作。以下是我的有限知识加上一些Google搜索的结果。看起来应该可以解决问题。

所以听起来您要解决的问题可以这样陈述：

给出两个四元数（分别代表大腿和小腿的3D方向）...
...计算两个四元数之间的3D角度差...
...并将该角度差表示为欧拉角

要获得3D角度差（它本身是一个四元数），只需将一个四元数乘以另一个四元数（reference）的共轭即可。

然后，您需要将四元数转换为欧拉角（绕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

注意事项：

我尚未手动验证此方法的正确性，但可以肯定它应该是正确的。
您当然要确保验证每个角度（phi，θ，psi）与期望值的实际方向和符号。
在Wikipedia文章中（在其C示例代码中），他们对用于计算theta的asin调用进行了一些更正。我不确定是否需要。但是，如果theta确实是内收，我想您还是不用担心90度以上的角度;-）

Answer 2

我认为您可以简单地将四元数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));

如何获得两个四元数之间的角度？

2 个答案: