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

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

所以听起来你要解决的问题可以表述如下：给出两个四元数(分别代表大腿和小腿的3D方向)。。。在

…计算两个四元数之间的三维角度差。。。在

。。。把角差表示为欧拉角

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

然后需要从四元数转换为Euler角度(围绕X、Y、Z旋转)。据我所知，你需要用老式的方法，使用Wikipedia中的公式。在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度以上的角度；-)