万向节死锁公式推导
欧拉角的万向节死锁
如果把刚体的旋转沿着三个旋转轴进行拆分，那么可以变成三个旋转角的叠加，这三个旋转角就是欧拉角，分别对应旋转矩阵，为了书写方便，记 S θ = sin θ , C θ = cos θ S_\theta=\sin\theta, C_\theta=\cos\theta Sθ=sinθ,Cθ=cosθ，则三个旋转矩阵为
| R x ( θ ) R_x(\theta) Rx(θ) | R y ( θ ) R_y(\theta) Ry(θ) | R z ( θ ) R_z(\theta) Rz(θ) |
|---|---|---|
| [ 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} 1000Cθ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θ010Sθ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θ0SθCθ0001 |
在给定欧拉角 ( α , β , γ ) (\alpha, \beta, \gamma) (α,β,γ)后，若物体的旋转次序为先 x → y → z x\to y\to z x→y→z，则其旋转矩阵可以记作
R = R z ( γ ) R y ( β ) R x ( α ) R=R_z(\gamma)R_y(\beta)R_x(\alpha) R=Rz(γ)Ry(β)Rx(α)
然而，当 β = 90 ° \beta=90° β=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−1010100 ，从而
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γ0SγCγ0001 00−1010100 1000CαSα0−SαCα = Cγ−Sγ0SγCγ0001 00−1−SαCα0CαSα0 = 00−1−SαCγ+SγCαSγSα+CγCα0CγCα+SγSα−SγCα+CγSα0 = 00−1sin(γ−α)cos(γ−α)0cos(γ−α)−sin(γ−α)0
在此旋转矩阵中， α , γ \alpha, \gamma α,γ被简并到了一起，相当于两个自由度变成了一个自由度。
旋转轴
记刚体的旋转轴 k = ( k 1 , k 2 , k 3 ) k=(k_1, k_2, k_3) k=(k1,k2,k3)，则 k k k是旋转矩阵的特征向量，其特征值为1，则其求解方程为 ( R − I ) k = 0 (R-I)k=0 (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−1sinθcosθ−10cosθ−sinθ−1 → 00−1sinθcosθ−10cosθ+1−sinθ−1 → 00−1sinθ(cosθ−1)sinθ(cosθ−1)0cos2θ−1−sin2θ−1 00−10cosθ−100−sinθ−1
解得
k 1 = − k 3 , k 2 = sin θ 1 − cos θ k 3 k_1=-k_3, k_2=\frac{\sin\theta}{1-\cos\theta}k_3 k1=−k3,k2=1−cosθsinθk3
也就是说，此时的旋转轴将被限制在 x = − z x=-z x=−z平面内。
更多推荐
14
0- 0
已为社区贡献5条内容
所有评论(0)