visual-c++ - 我想为 6 个自由度创建一个距离公式？

Question

我有一个具有 6 个自由度 3 个轴 x、y、z 和 3 个角度偏航、俯仰和滚动的传感器。3轴距离使用距离公式很简单，但如何计算3个角距离？这里有人可以帮我吗？？？

Answer 1

偏航、俯仰和滚动表示一组欧拉角，它们指定从一个坐标系 XYZ 到另一个坐标系 X'Y'Z' 的旋转。这种变换也可以表示为围绕任意轴的单次旋转。如果我正确理解你的问题，你想知道这个单一的旋转角度是什么。

可以将一组欧拉角转换为称为四元数的四元素结构。

q4 = cos(yaw/2) cos(pitch/2) cos(roll/2) + sin(yaw/2) sin(pitch/2) sin(roll/2)

q1 = cos(yaw/2) cos(pitch/2) sin(roll/2) - sin(yaw/2) sin(pitch/2) cos(roll/2)

q2 = cos(yaw/2) sin(pitch/2) cos(roll/2) + sin(yaw/2) cos(pitch/2) sin(roll/2)

q3 = sin(yaw/2) cos(pitch/2) cos(roll/2) - cos(yaw/2) sin(pitch/2) sin(roll/2)

（来源：http ://www.resonancepub.com/quaterni.htm ）

一旦你有了四元数，旋转轴和旋转角度很容易从四元数分量中计算出来。使用上述作者的符号，

q = [q4 q1 q2 q2]

q4 = cos(θ/2)

q1 = sin(theta/2)A

q2 = sin(theta/2)B

q3 = sin(theta/2)C

其中 [ABC] 是 XYZ 系统中指定旋转轴的向量，theta 是您要查找的旋转角度。

所以 theta = 2*acos(q4)