Java代码从四元数获取绕轴的旋转角度

Question

我真的很难找到从任意四元数获取绕单个轴的旋转角度的正确方法。换句话说，我想找到围绕指定轴(在我的例子中是坐标系的 Z 轴，但任意解决方案都很好)的角度表示的旋转部分。有人能指出要实现这一目标吗？理想情况下，一些 java 片段会很好。

我尝试了 1 中提出的解决方案对于态度，即:

asin(2*qx*qy + 2*qz*qw)

但是，这在某些情况下会失败，例如绕 Z 轴旋转一次，旋转角度超过 0.6 * PI。

Answer 1

四元数的角度和旋转轴

每个四元数 q 都可以分解为某种极分解

q = r * (c + s * e)

哪里

r = |q|, s = |imag(q/r)|, c = real(q/r) and e = imag(q/s/r)

x ↦ q * x * q^(-1) 的旋转轴为 e，角度为单位圆上点 (c,s)=(cos(α),sin(α)) 的角度 α 的 2 倍。

为了计算旋转角度，r 的缩放并不那么重要，所以

angle = 2*atan2( norm(imag(q)), real(q) )  

<小时/>

欧拉角理论

绕X轴的旋转由四元数ca+sa*i表示，绕Y轴的旋转由四元数cb+sb*j表示，Z轴由cc+sc*k表示，其中ca²+sa²=1表示旋转角a的一半的余弦-正弦对等。后面的2a、c2a和s2a等将表示双角及其余弦和正弦值。

按照应用到原点处的对象的 xyz 顺序相乘得到一个乘积

q=qw+qx*i+qy*j+qz*k
=(cc+sc*k)*(cb+sb*j)*(ca+sa*i)

现在，q*i*q^(-1) 和 q^(-1)*k*q 中发生了有趣的事情，因为内部项交换并取消，因此

q*i*q^(-1)*(-i) = (cc+sc*k)*(cb+sb*j)*(cb+sb*j)*(cc+sc*k)
= (cc+sc*k)*(c2b+s2b*j)*(cc+sc*k)
= (c2c+s2c*k)*c2b+s2b*j 

(-k)*q^(-1)*k*q = (ca+sa*i)*(cb+sb*j)*(cb+sb*j)*(ca+sa*i)
=(ca+sa*i)*(c2b+s2b*j)*(ca+sa*i)
=(c2a+s2a*i)*c2b+s2b*j

然后可以使用它来隔离角度 2a、2b 和 2c

q*i*q^(-1)*(-i) = (q*i)*(i*q)^(-1) 
   = (qw*i-qx-qy*k+qz*j)*(-qw*i-qx-qy*k+qz*j)
   =  (qw²+qx²-qy²-qz²)
    + 2*(qw*qy-qx*qz)*j
    + 2*(qw*qz+qx*qy)*k

(-k)*q^(-1)*k*q = (q*k)^(-1)*(k*q)
   = (-qw*k+qx*j-qy*i-qz)*(qw*k+qx*j-qy*i-qz)
   =   (qw²-qx²-qy²+qz²)
    + 2*(qw*qx+qy*qz)*i
    + 2*(qw*qy-qx*qz)*j

<小时/>

结果算法

识别表达式结果

s2b = 2*(qw*qy-qx*qz)
c2b*(c2a+s2a*i) = (qw²-qx²-qy²+qz²) + 2*(qw*qx+qy*qz)*i
c2b*(c2c+s2c*k) = (qw²+qx²-qy²-qz²) + 2*(qw*qz+qx*qy)*k

或

2a = atan2(2*(qw*qx+qy*qz), (qw²-qx²-qy²+qz²))
2b = asin(2*(qw*qy-qx*qz))
2c = atan2(2*(qw*qz+qx*qy), (qw²+qx²-qy²-qz²))

这以这样的方式构造角度

c2b=sqrt( (qw²+qx²+qy²+qz²)²+8*qw*qx*qy*qz )

为正数，因此 2b 介于 -pi/2 和 pi/2 之间。通过一些符号操作，也可以获得 c2b 为负的解。

<小时/>

回答关于asin公式的问题

显然，使用了不同类型的旋转顺序，其中 Z 旋转是中间旋转。准确地说，

q = (cb+sb*j)*(cc+sc*k)*(ca+sa*i)

哪里

2b = 标题 2a = 银行 2c = 态度

要处理大于 0.5*pi 的姿态旋转角度 2c，您需要计算全套欧拉角，因为它们将在 Z 旋转之前和之后包含围绕其他轴的两次翻转。

或者您需要检测这种情况，要么保持坡度余弦为正，要么检查是否存在过大的角度变化，并在 atan 公式中应用符号修改，通过 pi(+ 或 -)更改其结果角度，并将 Z 角度计算更改为 pi-asin(...)

或者，仅在计算后操纵角度，如果 (2a,2b,2c) 是计算的解，则

(2a-sign(2a)*pi, 2b-sign(2b)*pi, sign(2c)*pi-2c)

是另一个给出相同四元数和旋转的解决方案。选择最接近预期行为的一个。