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

Question

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

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

我对四元数还很陌生，我试图了解一种通过 python 实现此目的的方法，但也在概念上理解它。

Answer 1

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

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

• 给定两个四元数(分别代表大腿和小腿的 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、theta、psi)的实际方向和符号与您期望的方向和符号。
• 在维基百科文章(在他们的 C 示例代码中)中，他们对用于计算 theta 的 asin 调用进行了一些修正。我不确定是否需要这样做。但如果 theta 确实是内收，我猜你无论如何都不需要担心超过 90 度的角度;-)