从旋转矩阵计算欧拉角
从旋转矩阵中找到所有可能的欧拉角的简单方法,在计算机图形学、视觉学、机器人学和运动学中,欧拉角的确定有时是必要的一步。然而,解决方案可能是明显的,也可能不是。
旋转矩阵
我们从三个主要轴的旋转的标准定义开始。
绕x轴的弧度旋转ψ被定义为:
Rx(ψ)=⎣⎡1000cosψsinψ0−sinψcosψ⎦⎤
类似地,绕y轴旋转的弧度定义为
Rx(ψ)=⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤
最后,定义了沿z轴旋转的弧度为
Rz(ϕ)=⎣⎡cosϕsinϕ0−sinϕcosϕ0001⎦⎤
这三个角ψ,θ,ϕ是欧拉角
广义旋转矩阵
一般的旋转矩阵可以有这样的形式
Rz(ϕ)=⎣⎡R11R21R31R12R22R32R13R23R33⎦⎤
这个矩阵可以被认为是一个三旋转的序列,每个主轴各一个。因为矩阵乘法不能交换,旋转的轴的顺序将影响结果。在这个分析中,我们先绕x轴旋转,然后绕y轴旋转,最后绕z轴旋转。这样的旋转序列可以用矩阵乘积表示。
R=Rz(ϕ)Ry(θ)Rx(ψ)=⎣⎡cosθcosϕcosθsinϕ−sinθsinψsinθcosϕ−cosψsinϕsinψsinθsinϕ+cosψcosϕsinψcosθcosψsinθcosϕ+sinψsinϕcosψsinθsinϕ−sinψcosϕcosψcosθ⎦⎤
给定一个旋转矩阵R,通过将R中的每个元素与矩阵乘Rz(ϕ),Ryθ,Rx(ψ)中的相应元素等价,可以计算出欧拉角ψ,θ,ϕ。九个方程,可以用来找到欧拉角。
求出两个可能的角度θ
从R31开始,我们发现
R31=−sinθ
这个方程可以倒过来表示
θ=−sin−1(R31)
然而,在解释这个等式时必须谨慎。由于$sin(\pi-\theta)=sin(\theta),实际上有两个不同的值(对于 R31=±1)满足方程,因此,这两个值
θ1=−sin−1(R31)θ2=π−θ1=π+sin−1(R31)
都是有效的解。我们将在后面处理 R31=±1的特殊情况,因此使用旋转矩阵的R_{31}元素,我们能够确定两个可能的值。
找到ψ对应的角度
想要找到ψ的值,我们观察这一点
R33R32=tan(ψ)
我们用这个方程来解出
ψ=atan2(R32,R33)
其中atan2(y,x)是两个变量x和y的arctan,类似于计算xy的arctan,只是用两个参数的符号来确定结果的象限,其范围为[−π,π],函数atan2在许多编程语言中都可用。
在解释方程2时必须小心,如果cos(θ)>0,那么ψ=atan2(R32,R33)。然而,当cos(θ)<0,ψ=atan2(−R32,−R33)。处理这个问题的一个简单方法是使用这个方程
ψ=atan2(cosθR32,cosθR33)
去计算ψ。
方程3对除cosθ=0之外的所有情况都有效。
ψ=atan2(cosθ1R32,cosθ1R33)ψ=atan2(cosθ2R32,cosθ2R33)
求出ϕ对应的角度
类似的分析也适用于寻找。我们观察到
R11R21=tanϕ
我们用这个方程解出了ϕ
ψ=atan2(cosθ1R21,cosθ1R11)ψ=atan2(cosθ2R21,cosθ2R11)
cosθ=0时的两个解
对于cosθ=0的情况,我们现在有两个三个一组的欧拉角再现了旋转矩阵,为
ψ1,θ1,ϕ1ψ2,θ2,ϕ2
这两个解都是有效的。
如果cosθ=0呢?
如果旋转矩阵的R31元素为1或−1,对应的θ=−2πor θ=2π,cosθ=0上述就不起作用。当我们尝试使用上述技术来解决可能的值ψ,ϕ问题发生了,因为R11,R21,R32和R33可能值为0,因此ψ,ϕ变为
ψ=atan2(00,00)ϕ=atan2(00,00)
这种情况下,R11、R21、R32和R33没有约束ψ,ϕ这些值。因此,我们必须利用旋转矩阵的不同元素来计算ψ,ϕ。
θ=2π:
R12=sinψcosϕ−cosψsinϕ=sin(ψ−ϕ)R13=cosψcosϕ+sinψsinϕ=cos(ψ−ϕ)R22=sinψsinϕ+cosψcosϕ=cos(ψ−ϕ)=R13R23=cosψsinϕ−sinψcosϕ=−sin(ψ−ϕ)=−R12
任意的ψ和ϕ都满足方程。我们会发现
(ψ−ϕ)=atan2(R12,R13)ψ=ϕ+atan2(R12,R13)
θ=−2π:
R12=−sinψcosϕ−cosψsinϕ=−sin(ψ+ϕ)R13=−cosψcosϕ+sinψsinϕ=−cos(ψ+ϕ)R22=−sinψsinϕ+cosψcosϕ=cos(ψ+ϕ)=−R13R23=−cosψsinϕ−sinψcosϕ=−sin(ψ+ϕ)=R12
同样
(ψ+ϕ)=atan2(−R12,−R13)ψ=−ϕ+atan2(−R12,−R13)
伪代码:
if (R31=±1)θ1=−asin(R31)θ2=π−θ1ψ1=atan2(cosθ1R32,cosθ1R33)ψ2=atan2(cosθ2R32,cosθ2R33)ϕ1=atan2(cosθ1R21,cosθ1R11)ϕ2=atan2(cosθ2R21,cosθ2R11) else ϕ= anything; can set to 0 if (R31=−1)θ=π/2ψ=ϕ+atan2(R12,R13) else θ=−π/2ψ=−ϕ+atan2(−R12,−R13) end if end if
实际例子
下面提供了一个例子,演示了从一个旋转矩阵中计算出ψ,θ,ϕ。
假设要求我们找出产生这个矩阵的欧拉角
Rz(ϕ)=⎣⎡.5.5−.7071−.1464.8536.5.8536−.1464.5⎦⎤
首先,求出可能θ的取值
θ1=−sin(−.7071)=4π
θ2=π−θ1=43π
然后,我们找到相应的数值
ψ1=atan2(cos(π/4).5,cos(π/4).5)=4πψ2=atan2(cos(3π/4).5,cos(3π/4).5)=−43π
ϕ1=atan2(cos(π/4).5,cos(π/4).5)=4πϕ2=atan2(cos(3π/4).5,cos(3π/4).5)=−43π
因此,解为
(4π,4π,4π),(−43π,43π,−43π)