欧拉角表示旋转会出现的问题——万向锁(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)（参考文章中）：
  $$\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\varphi )& \sin (\varphi )\\ 0& -\sin (\varphi )& \cos (\varphi )\end{array}\right]\left[\begin{array}{ccc}\cos (\theta )& 0& -\sin (\theta )\\ 0& 1& 0\\ \sin (\theta )& 0& \cos (\theta )\end{array}\right]\left[\begin{array}{ccc}\cos (\psi )& \sin (\psi )& 0\\ -\sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]\end{array}$$
  ​ 注意：左乘为 矩阵在左侧，列向量在右侧的写法，左乘后的结果为列向量（基本的线代知识）；
  ​ 注意：最右边的为绕Z轴旋转，最左边为绕X轴旋转，计算时列向量写在右侧；

• 本文的计算结果是在NED坐标系中的所以应该是先乘以X的逆矩阵接着是Y的逆矩阵接着是Z的逆矩阵：
  $$\begin{array}{c}\left[\begin{array}{ccc}\cos (\psi )& -\sin (\psi )& 0\\ \sin (\psi )& \cos (\psi )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos (\theta )& 0& \sin (\theta )\\ 0& 1& 0\\ -\sin (\theta )& 0& \cos (\theta )\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (\varphi )& -\sin (\varphi )\\ 0& \sin (\varphi )& \cos (\varphi )\end{array}\right]\end{array}$$
  注意：航空中有定义$\psi$航向角，$\theta$俯仰角，$\varphi$滚转角；
  注意：Z轴为刚体质心指向下方向；Y轴正方向指向右侧；X轴正方向指向正前方；
  注意：旋转角度的正负遵循右手定则；
  注意：计算结果为向量在世界坐标系中的坐标，即为初始状态下和刚体坐标系重合的坐标；

二、万向锁的定义

下面是我个人的定义：

• 前提：用欧拉角表示刚体旋转，绕轴旋转顺序固定。

• 现象：当刚体绕某个轴旋转到特定角度时，会出现剩下两轴的绕轴旋转运动的导致的最终效果呈现同轴的现象。

• 针对ZYX顺序：当刚体绕Y轴旋转到±90°时，绕X轴和绕Z轴的运动会相互抵消。即为：俯仰角90°，航向滚转效果在NED中同轴；

注意：这里的锁，从NED中看指的是刚体绕某个轴的旋转效果失去了；

注意：在真实世界中的万向节中，从刚体坐标系来看，物体绕Z轴的自由度失去了（Z轴锁住了）；

• 可以想象下，在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度，此时就会出现万向锁现象。

试想这样一个场景，云台状态（$\varphi =0,\theta =45,\psi =0$）开始跟踪目标目标运动方位角是0度，跟踪至$\theta$接近90度，此时物体突然向方位角90的方向运动，这时候云台的方位角电机就要旋转90度以便追踪。在转向90度的过程中镜头移动的轨迹并非是最理想在YZ平面内的运动，而是一段在YZ平面外的弧形。若要在YZ平面内，则要求Z轴电机角速度为无穷大。

当相机处于短焦距宽视场状态时，物体可能不会处于摄像机的视野中心，但是仍会在视野中。当相机处于长焦距窄视场状态时，在方向角电机采用等角速率修正方位角时，物体可能从视野中丢失，导致跟踪失败，而丢失的时间和方位角电机的最大角速率${\omega }_{max}$相关，${\omega }_{max}$越大，丢失时间越短。

下图（红色X正方向，绿色Y正方向）是摄像机从($\varphi =0,\theta =90,\psi =0$)的状态旋转到($\varphi =0,\theta =45,\psi =\delta$)的状态，其中 δ = { 15 , 30 , 60 , 90 } \delta=\{15,30,60,90\} δ={15,30,60,90}，可以看到采用等角速度进行跟踪，在空间中所划出的轨迹并不是最短最理想的轨迹。

简单来说，当物体运动轨迹（这边单指直线）所在的平面不与万向轴Y轴垂直时，云台需要在方位角上进行补偿，否则会卡住无法跟踪。

不过在实际中由于云台的搭载平台会运动，使得上述情况出现的机率较小。

飞行器姿态控制

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

四、数学上的解释

另$\theta =90$
变换矩阵可化为
$$\begin{array}{c}\left[\begin{array}{ccc}0& \sin (\varphi -\psi )& \cos (\varphi -\psi )\\ 0& \cos (\varphi -\psi )& -\sin (\varphi -\psi )\\ 1& 0& 0\end{array}\right]\end{array}$$
对于所有状态的初始向量$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$，无论滚转航向角度取何值，最终都是$\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$；
注意：此结论针对的是绕刚体坐标系（动系）旋转，顺序为ZYX；其余情况可以按照相同方法推导；

五、总结

1. 首先是用坐标变换矩阵表示旋转，旋转总是从列向量$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$开始的；
2. 第三部分的例子的初始状态也是通过旋转矩阵从$\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$旋转到例子中的初始状态下的（详见程序）；
3. 注意坐标变换时的输入和输出向量是在什么坐标系下的；
4. 万向锁是用欧拉角表示旋转过程中的一种状态；
5. 欧拉角用来表示物体的旋转状态是没有问题的，万向锁的问题影响到的是连续运动的问题；
6. 四元数规划出的旋转轨迹则可能出现角速度上的突变（ZYX顺序，Z轴角速度趋向无穷大的问题）；

参考

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);