欧拉角表示旋转会出现的问题——万向锁(Gimbal Lock)

本文用来总结万向锁问题。尽量写得非常简单，方便自己复习和后人理解。

一、旋转的表示

• 本文中矩阵计算的结果是在世界坐标系（称之为North East Down Frame NED Frame)中的坐标；
• 参考文章中最后矩阵计算的出的坐标是在刚体坐标系(BODY)的坐标；
• 参考文章中左乘旋转矩阵的物理含义：每次的旋转都是绕固结在刚体上的坐标系的轴；
• 由于体坐标系的轴有三个（XYZ)所以旋转的表示跟绕轴旋转的顺序有关；
  顺序有：XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ 共12种；
• 本文选取 Z Y X 按照左乘矩阵的定义就是 先Z后Y再X;
• 从NED坐标系转换到体坐标的变换矩阵表示为简称为w2b(world to body)（参考文章中）：
  [ 1 0 0 0 cos ⁡ ( ϕ ) sin ⁡ ( ϕ ) 0 − sin ⁡ ( ϕ ) cos ⁡ ( ϕ ) ] [ cos ⁡ ( θ ) 0 − sin ⁡ ( θ ) 0 1 0 sin ⁡ ( θ ) 0 cos ⁡ ( θ ) ] [ cos ⁡ ( ψ ) sin ⁡ ( ψ ) 0 − sin ⁡ ( ψ ) cos ⁡ ( ψ ) 0 0 0 1 ] \begin{gathered} \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi)& \sin(\phi) \\ 0 & -\sin(\phi) & \cos(\phi) \end{bmatrix} \quad \begin{bmatrix} \cos(\theta) & 0 & -\sin(\theta) \\ 0 & 1 & 0 \\ \sin(\theta) & 0 & \cos(\theta) \end{bmatrix} \quad \begin{bmatrix} \cos(\psi) & \sin(\psi) & 0 \\ -\sin(\psi) & \cos(\psi)& 0 \\ 0 & 0 & 1 \end{bmatrix} \end{gathered} ⎣⎡​100​0cos(ϕ)−sin(ϕ)​0sin(ϕ)cos(ϕ)​⎦⎤​⎣⎡​cos(θ)0sin(θ)​010​−sin(θ)0cos(θ)​⎦⎤​⎣⎡​cos(ψ)−sin(ψ)0​sin(ψ)cos(ψ)0​001​⎦⎤​​
  注意：左乘为 矩阵在左侧，列向量在右侧的写法，左乘后的结果为列向量（基本的线代知识）；
  注意：最右边的为绕Z轴旋转，最左边为绕X轴旋转，计算时列向量写在右侧；
• 本文的计算结果是在NED坐标系中的所以应该是先乘以X的逆矩阵接着是Y的逆矩阵接着是Z的逆矩阵：
  [ cos ⁡ ( ψ ) − sin ⁡ ( ψ ) 0 sin ⁡ ( ψ ) cos ⁡ ( ψ ) 0 0 0 1 ] [ cos ⁡ ( θ ) 0 sin ⁡ ( θ ) 0 1 0 − sin ⁡ ( θ ) 0 cos ⁡ ( θ ) ] [ 1 0 0 0 cos ⁡ ( ϕ ) − sin ⁡ ( ϕ ) 0 sin ⁡ ( ϕ ) cos ⁡ ( ϕ ) ] \begin{gathered} \begin{bmatrix} \cos(\psi) & -\sin(\psi) & 0 \\ \sin(\psi) & \cos(\psi)& 0 \\ 0 & 0 & 1 \end{bmatrix} \quad \begin{bmatrix} \cos(\theta) & 0 & \sin(\theta) \\ 0 & 1 & 0 \\ -\sin(\theta) & 0 & \cos(\theta) \end{bmatrix} \quad \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\phi)& -\sin(\phi) \\ 0 & \sin(\phi) & \cos(\phi) \end{bmatrix} \end{gathered} ⎣⎡​cos(ψ)sin(ψ)0​−sin(ψ)cos(ψ)0​001​⎦⎤​⎣⎡​cos(θ)0−sin(θ)​010​sin(θ)0cos(θ)​⎦⎤​⎣⎡​100​0cos(ϕ)sin(ϕ)​0−sin(ϕ)cos(ϕ)​⎦⎤​​
  注意：航空中有定义 ψ \psi ψ航向角， θ \theta θ俯仰角， ϕ \phi ϕ滚转角；
  注意：Z轴为刚体质心指向下方向；Y轴正方向指向右侧；X轴正方向指向正前方；
  注意：旋转角度的正负遵循右手定则；
  注意：计算结果为向量在世界坐标系中的坐标，即为初始状态下和刚体坐标系重合的坐标；

二、万向锁的定义

下面是我个人的定义：

• 前提：用欧拉角表示刚体旋转，绕轴旋转顺序固定。
• 现象：当刚体绕某个轴旋转到特定角度时，会出现剩下两轴的绕轴旋转运动相互抵消的现象。
• 针对ZYX顺序：当刚体绕Y轴旋转到±90°时，绕X轴和绕Z轴的运动会相互抵消。即为：俯仰角90°，航向滚转效果等同；
  注意：这里的锁，指的是刚体绕某个轴的旋转效果失去了。不是说刚体只能固定在当前的旋转状态无法改变了。
  可以想象下，在ZYX顺序下，当绕Y轴旋转90°时，向量在XY平面上的投影是一个点，一个点此时谈何航向角可言。在Y轴90°，所有绕Z轴的旋转都可以用绕X轴旋转去补偿回来。
• 举个例子 （手头拿个物体自己定义XYZ坐标旋转下，比如定义手机屏幕竖直朝下为Z轴，Y轴平行于短边，X轴平行于短边）旋转过程如下：

1. 绕Z轴旋转90°（右旋90）
2. 绕Y轴旋转90度（抬头90）
3. 绕X轴旋转90度；
   物体最终呈现的空间状态 和 物体直接 绕Y轴旋转90度是一样的。

三、万向锁带来的问题

3D动画中的插帧问题

• 一般动画中有个关键帧的概念，关键帧之间用计算机自动插帧出物体的运动过程，设计人员只要指定两个关键帧的旋转状态就行了。
• 按照关键帧的定义插帧的帧与帧之间是按照物体运动的最短距离进行计算插值的。
• 当采用欧拉角进行插帧时，当物体最终状态处于万向锁的状态时候，会出现插值出来的运动不是按照最短路径运动的情况。
  下面举个例子(和上面例子相同）：
  初始状态：Z(0）Y(0) X(0)
  最终状态：Z(90) Y(90) X(90)
  如果按照线性插值的方法 中间帧的状态如下
  Z:[1 2 3… 88 89]
  Y:[1 2 3… 88 89]
  X:[1 2 3… 88 89]
  运动轨迹如下
  

当然上述的最终情况是用比较特殊的方式表示的，如果表示为0 90 0就不会出现为这个问题；
但是当设置为其他情况时候：
初始：Z(30) Y(10) X(15)
最终：Z(120) Y(90) X(45)

初始：Z(350) Y(0) X(0)
最终：Z(90) Y(90) X(0)

总结一下可以发现：只有初始状态是在XZ平面YZ平面内的向量在用欧拉角插值的时候才不会出现插值出异常的运动轨迹。其他的轨迹均是运动异常的。所以一般3D动画采用的是四元数插值方法。

高机动飞行器的姿态问题

这方面没有想通到底问题何在，普通飞控一般采用欧拉角进行控制，一般普通飞机的俯仰角不会达到90°，或许是类似于火箭/导弹一类飞行器，有垂直飞行转化为水平飞行的过程有涉及到万向锁的问题。若有懂哥请指教。

四、数学上的解释

另 θ = 90 \theta=90 θ=90
变换矩阵可化为
[ 0 sin ⁡ ( ϕ − ψ ) cos ⁡ ( ϕ − ψ ) 0 cos ⁡ ( ϕ − ψ ) − sin ⁡ ( ϕ − ψ ) 1 0 0 ] \begin{bmatrix} 0 & \sin(\phi-\psi)& \cos(\phi-\psi) \\ 0 & \cos(\phi-\psi) & -\sin(\phi-\psi) \\ 1 & 0 & 0 \end{bmatrix} ⎣⎡​001​sin(ϕ−ψ)cos(ϕ−ψ)0​cos(ϕ−ψ)−sin(ϕ−ψ)0​⎦⎤​
对于所有状态的初始向量 [ 1 0 0 ] \begin{bmatrix}1\\0\\0\end{bmatrix} ⎣⎡​100​⎦⎤​，无论滚转航向角度取何值，最终都是 [ 0 0 1 ] \begin{bmatrix}0\\0\\1\end{bmatrix} ⎣⎡​001​⎦⎤​；

五、总结

1. 首先是用坐标变换矩阵表示旋转，旋转总是从列向量 [ 1 0 0 ] \begin{bmatrix}1\\0\\0\end{bmatrix} ⎣⎡​100​⎦⎤​开始的；
2. 第三部分的例子的初始状态也是通过旋转矩阵从 [ 1 0 0 ] \begin{bmatrix}1\\0\\0\end{bmatrix} ⎣⎡​100​⎦⎤​旋转到例子中的初始状态下的（详见程序）；
3. 注意坐标变换时的输入和输出向量是在什么坐标系下的；
4. 万向锁是用欧拉角表示旋转过程中的一种状态；

参考

https://zhuanlan.zhihu.com/p/73406793
https://zhuanlan.zhihu.com/p/74040465
https://www.bilibili.com/video/BV1WK4y187A3?from=search&seid=14517252516529200541

附：matlab程序

clc;
clear;
axis on;
set(gca,'ZDir','reverse')%对Z方向反转
set(gca,'XDir','reverse')%对X方向反转
PI=3.1415926;
DEG2RAD=PI/180;
%初始
phi_init = 30*DEG2RAD;%X
tha_init = 10*DEG2RAD;%Y
psi_init = 15*DEG2RAD;%Z
%最终
phi_fin = 120*DEG2RAD;
tha_fin = 90*DEG2RAD;
psi_fin = 45*DEG2RAD;

inter_cnt=50;%插帧数量
PHI=[phi_init];
THA=[tha_init];
PSI=[psi_init];
%生成帧序列 各个中间状态
for i=1:inter_cnt
    PHI=[PHI PHI(i)+(phi_fin-phi_init)/inter_cnt];
    THA=[THA THA(i)+(tha_fin-tha_init)/inter_cnt];
    PSI=[PSI PSI(i)+(psi_fin-psi_init)/inter_cnt];
end

vecx=[];
vecy=[];
vecz=[];
hold on;
for i=1:inter_cnt
    phi=PHI(i);
    tha=THA(i);
    psi=PSI(i);
    RX=[1 0 0;
        0 cos(phi) -sin(phi);
        0 sin(phi) cos(phi)];
    RY=[cos(tha) 0 sin(tha);
        0 1 0;
        -sin(tha) 0 cos(tha)];
    RZ=[cos(psi) -sin(psi) 0;
        sin(psi) cos(psi) 0;
        0 0 1];
    
    vec=RZ*RY*RX*[1;0;0];%都是从[1;0;0]开始旋转来表示每帧状态，而非相对上一帧的增量
    vecx=[vecx vec(1)];
    vecy=[vecy vec(2)];
    vecz=[vecz vec(3)];
    line([0 vec(1)],[0 vec(2)],[0 vec(3)]);
    
end
%正方向指示
line([0 1],[0 0],[0 0],'color','r');%X
line([0 0],[0 1],[0 0],'color','g');%Y
line([0 0],[0 0],[0 1],'color','b');%Z
%划出的轨迹
plot3(vecx,vecy,vecz);

标签：phi,psi,Gimbal,欧拉角,Lock,cos,bmatrix,90,sin
来源： https://blog.csdn.net/seraph0321/article/details/111640415