第一个 IMU（称为 S1）放置在肩部并用作参考；另一个（S2）放在手臂上。它们提供了相对于绝对参考（磁北和重力矢量）的旋转四元数。简单的想法是我需要显示这两者之间的偏航、俯仰和滚动差异（例如，理想的外展/内收运动应该只有俯仰贡献）。我开始使用四元数，通过计算两者之间的旋转 (conj(q1) * q2)，然后使用以下方法将其转换为 YPR 角度：
# Rotation quaternion q1 = (w,x,y,z)
# unit = sum of squared elements (sqx = x^2, etc.)
yaw = math.atan2(2 * q1[2] * q1[0] - 2 * q1[1] * q1[3], sqx - sqy - sqz + sqw)
pitch = math.asin(2 * (q1[1] * q1[2] + q1[3] * q1[0]) / sqx + sqy + sqz + sqw)
roll = math.atan2(2 * q1[1] * q1[0] - 2 * q1[2] * q1[3], -sqx - sqy + sqz + sqw)
但这在我的情况下不起作用，因为俯仰和滚动在不同的手臂位置上不一致。例如，如果相对偏航角为 90 度，则俯仰角和滚动角互换。例如，如果我对 S2 应用俯仰旋转，它似乎是滚动旋转（因为旋转在参考传感器 S1 的 y 轴上）。我怎样才能避免这种情况？我是否应该简单地将两个四元数转换为 YPR 角度，然后计算每对的差异（不使用四元数之间的差异）？也许“旋转”方法不正确，因为我不需要逆变换，而只需要每个轴的实际旋转？