万向节死锁公式推导

欧拉角的万向节死锁

如果把刚体的旋转沿着三个旋转轴进行拆分，那么可以变成三个旋转角的叠加，这三个旋转角就是欧拉角，分别对应旋转矩阵，为了书写方便，记$S_{\theta }=\sin \theta ,C_{\theta }=\cos \theta$，则三个旋转矩阵为

$R_x(\theta )$	$R_y(\theta )$	$R_z(\theta )$
[ 1 0 0 0 C θ − S θ 0 S θ C θ ] \begin{bmatrix}1&0&0\\0&C_\theta&-S_\theta\\0&S_\theta&C_\theta\\\end{bmatrix} ​100​0Cθ​Sθ​​0−Sθ​Cθ​​ ​	[ C θ 0 S θ 0 1 0 − S θ 0 C θ ] \begin{bmatrix}C_\theta&0 &S_\theta\\0&1&0\\-S_\theta&0&C_\theta\\\end{bmatrix} ​Cθ​0−Sθ​​010​Sθ​0Cθ​​ ​	[ C θ S θ 0 − S θ C θ 0 0 0 1 ] \begin{bmatrix}C_\theta &S_\theta&0\\-S_\theta&C_\theta&0\\0&0&1\end{bmatrix} ​Cθ​−Sθ​0​Sθ​Cθ​0​001​ ​

在给定欧拉角$(\alpha ,\beta ,\gamma )$后，若物体的旋转次序为先$x\to y\to z$，则其旋转矩阵可以记作

$$R=R_z(\gamma )R_y(\beta )R_x(\alpha )$$

然而，当$\beta =90°$时， R y ( β ) = [ 0 0 1 0 1 0 − 1 0 0 ] R_y(\beta)=\begin{bmatrix}0&0 &1\\0&1&0\\-1&0&0\\\end{bmatrix} Ry​(β)= ​00−1​010​100​ ​，从而

R = [ C γ S γ 0 − S γ C γ 0 0 0 1 ] [ 0 0 1 0 1 0 − 1 0 0 ] [ 1 0 0 0 C α − S α 0 S α C α ] = [ C γ S γ 0 − S γ C γ 0 0 0 1 ] [ 0 − S α C α 0 C α S α − 1 0 0 ] = [ 0 − S α C γ + S γ C α C γ C α + S γ S α 0 S γ S α + C γ C α − S γ C α + C γ S α − 1 0 0 ] = [ 0 sin ⁡ ( γ − α ) cos ⁡ ( γ − α ) 0 cos ⁡ ( γ − α ) − sin ⁡ ( γ − α ) − 1 0 0 ] \begin{aligned} R&=\begin{bmatrix}C_\gamma &S_\gamma&0\\-S_\gamma&C_\gamma&0\\0&0&1\end{bmatrix} \begin{bmatrix}0&0 &1\\0&1&0\\-1&0&0\\\end{bmatrix} \begin{bmatrix}1&0&0\\0&C_\alpha&-S_\alpha\\0&S_\alpha&C_\alpha\\\end{bmatrix}\\ &=\begin{bmatrix}C_\gamma &S_\gamma&0\\-S_\gamma&C_\gamma&0\\0&0&1\end{bmatrix} \begin{bmatrix}0&-S_\alpha &C_\alpha\\0&C_\alpha&S_\alpha\\-1&0&0\\\end{bmatrix}\\ &=\begin{bmatrix} 0&-S_\alpha C_\gamma +S_\gamma C_\alpha&C_\gamma C_\alpha+S_\gamma S_\alpha\\ 0&S_\gamma S_\alpha+C_\gamma C_\alpha& -S_\gamma C_\alpha +C_\gamma S_\alpha\\ -1&0&0\end{bmatrix}\\ &=\begin{bmatrix} 0&\sin(\gamma-\alpha)&\cos(\gamma-\alpha)\\ 0&\cos(\gamma-\alpha)&-\sin(\gamma-\alpha)\\ -1&0&0\end{bmatrix} \end{aligned} R​= ​Cγ​−Sγ​0​Sγ​Cγ​0​001​ ​ ​00−1​010​100​ ​ ​100​0Cα​Sα​​0−Sα​Cα​​ ​= ​Cγ​−Sγ​0​Sγ​Cγ​0​001​ ​ ​00−1​−Sα​Cα​0​Cα​Sα​0​ ​= ​00−1​−Sα​Cγ​+Sγ​Cα​Sγ​Sα​+Cγ​Cα​0​Cγ​Cα​+Sγ​Sα​−Sγ​Cα​+Cγ​Sα​0​ ​= ​00−1​sin(γ−α)cos(γ−α)0​cos(γ−α)−sin(γ−α)0​ ​​

在此旋转矩阵中，$\alpha ,\gamma$被简并到了一起，相当于两个自由度变成了一个自由度。

旋转轴

记刚体的旋转轴$k=(k_1,k_2,k_3)$，则$k$是旋转矩阵的特征向量，其特征值为1，则其求解方程为$(R-I)k=0$，将其带入死锁后的旋转矩阵，记$\theta =\gamma -\alpha$，因其为齐次方程，则其系数矩阵可按如下过程变化

[ − 1 sin ⁡ θ cos ⁡ θ 0 cos ⁡ θ − 1 − sin ⁡ θ − 1 0 − 1 ] → [ 0 sin ⁡ θ cos ⁡ θ + 1 0 cos ⁡ θ − 1 − sin ⁡ θ − 1 0 − 1 ] → [ 0 sin ⁡ θ ( cos ⁡ θ − 1 ) cos ⁡ 2 θ − 1 0 sin ⁡ θ ( cos ⁡ θ − 1 ) − sin ⁡ 2 θ − 1 0 − 1 ] → [ 0 0 0 0 cos ⁡ θ − 1 − sin ⁡ θ − 1 0 − 1 ] \begin{aligned} &\begin{bmatrix}-1&\sin\theta&\cos\theta\\0&\cos\theta-1&-\sin\theta\\-1&0&-1\end{bmatrix}\to \begin{bmatrix}0&\sin\theta&\cos\theta+1\\0&\cos\theta-1&-\sin\theta\\-1&0&-1\end{bmatrix}\to \begin{bmatrix}0&\sin\theta(\cos\theta-1)&\cos^2\theta-1\\0&\sin\theta(\cos\theta-1)&-\sin^2\theta\\-1&0&-1\end{bmatrix}\\ \to& \begin{bmatrix}0&0&0\\0&\cos\theta-1&-\sin\theta\\-1&0&-1\end{bmatrix}\\ \end{aligned} →​ ​−10−1​sinθcosθ−10​cosθ−sinθ−1​ ​→ ​00−1​sinθcosθ−10​cosθ+1−sinθ−1​ ​→ ​00−1​sinθ(cosθ−1)sinθ(cosθ−1)0​cos2θ−1−sin2θ−1​ ​ ​00−1​0cosθ−10​0−sinθ−1​ ​​

解得

$$k_1=-k_3,k_2=\frac{\sin \theta }{1-\cos \theta }{k}_3$$

也就是说，此时的旋转轴将被限制在$x=-z$平面内。