旋转矩阵、欧拉角、万向锁详解

旋转矩阵、欧拉角、万向锁详解

一：物体的姿态描述
物体的姿态可由固定于此物体的坐标系描述，为了规定空间某刚体B的姿态，设置一直角坐标系{B}与此刚体固接。用坐标系{B}的三个单位主矢量{Xb，Yb，Zb}相对于参考坐标系{A}的方向余弦组成的3*3的矩阵来表示刚体B相对于坐标系{A}的方位，(_B^A)R称为旋转矩阵。

式中，上标A代表参考坐标系{A}，下表B代表被描述的坐标系{B}，(_B^A)R共9个元素，但只有三个式独立的。三个单位列矢量两两相互垂直，长度为1，因而它的9给元素满足正交条件：

旋转矩阵是正交的，并且满足条件：

例如：

如图所示有两个坐标系{A}与{B}，坐标系相对于参考坐标系{A}沿X轴旋转了θ角度，坐标系{B}的三个单位主矢量为Xb，Yb，Zb，
其中Xb相对与参考坐标系{A}的方向余弦为（1,0,0）
Yb相对与参考坐标系{A}的方向余弦为（0,cos⁡(θ), sin⁡(θ)）
Zb相对与参考坐标系{A}的方向余弦为（0，-sin⁡(θ),cos⁡(θ)）
故：

对于绕X,Y,Z作转角为θ的旋转变换，其旋转矩阵分别为（与右手系旋转方向相同的为旋转正方向）：

二：欧拉角
一个坐标系相对于另一个坐标系的姿态可以用一个旋转矩阵来表示，而且可以通过绕着坐标轴一定序列的旋转来完成坐标系之间的转换。欧拉角规定了一个旋转序列，如先绕z轴旋转ϕ角，再绕新的y轴旋转θ角，最后绕新的z轴旋转ψ角来描述任何可能的姿态。
则欧拉旋转矩阵：

三：欧拉角的旋转顺序
欧拉角的旋转顺序有多种，都可以用来描述从一个坐标系到另一个坐标系的变化，也就是姿态变化，一般分为两大类：
Proper Euler angles (z-x-z, x-y-x, y-z-y, z-y-z, x-z-x, y-x-y)
Tait–Bryan angles (x-y-z, y-z-x, z-x-y, x-z-y, z-y-x, y-x-z).
每个大类都使用了3个变量描述三次旋转过程中的旋转角度, 差别在于Proper Euler angles只涉及两个转轴.而Tait–Bryan angles涉及三个转轴。Tait–Bryan angles也被称为万向节角，航向角。在飞行器中应用最多的是yaw(偏航), pitch(俯仰), roll(横滚)三个航向角，三个变量一般对应飞行器的z,y,x三个坐标轴。

四：欧拉角的内旋外旋
内旋(intrinsic rotations) = 旋转轴(rotated axis)
外旋(extrinsic rotations) = 固定轴(static/fixed axis)
描述物体姿态是一共有两个坐标系，与机体固连的机体自身坐标系，还有一个是世界坐标系。那么每次旋转时的旋转轴也有两种，如果是绕着固定的世界坐标系的坐标轴进行旋转，那么称为外旋（绕着物体外部的旋转轴进行旋转），如果每次都是绕着新的机体自身的坐标系进行旋转就称为内旋（绕着物体自身坐标系旋转）。对于不同的旋转顺序其旋转矩阵的乘积顺序是不一致的，内旋从左向右乘，外旋从右向左乘。
例如，规定了旋转顺序为z-y-x：那么

内旋顺序z-y-x等价于外旋顺序x-y-z
故用欧拉角来表示物体姿态或者旋转时，需要定义以下三个元素：
1：旋转顺序；2：旋转角度；3旋转方向（内/外旋）

五：旋转矩阵
对于不同的旋转顺序，其欧拉角旋转矩阵可以参考以下这张表，c代表cos，s代表sin，1、2、3代表旋转角度（来源于维基百科）：

这张表描述了所有旋转的组合.以最后一行为例,我们可以把这里的公式认为是Z->X->Y内旋 or Y->X->Z外旋.从矩阵的角度就是ZXY.

六：旋转矩阵与欧拉角的转换
设三个轴x，y，z的欧拉角分别为θx,θy,θz，采用内旋的方式沿着z-y-x的顺序旋转，正弦值、余弦值分别为sx,cx,sy,cy,sz,cz那么旋转矩阵为：

由旋转矩阵求欧拉角：

解方程得：

从式中可以看出，θ=90度时，会产生除数为0的奇异点，这在物理空间上被称为万向节死锁问题，就是当三个旋转中间的这个角旋转达到90度时，第一次旋转和第三次旋转的旋转轴会产生重合，导致第三轴失去意义。原因是方向余弦矩阵无法完全描述SO3所代表的三维旋转群，在某些位置会出现奇点。

七：万向节死锁（Gimbal Lock）
首先理解几个基本的概念：
俯仰（pitch）：

偏航（yaw）：

滚转（roll）：

万向节：

yaw：
pitch：

roll：

观察下面这个gif你会发现当转轴1和转轴3位于同一平面后，中间的飞机会失去一个yaw方向的自由度，出现了万向锁！！！

再继续观察下面这个gif，代表y轴的gree和代表z轴的bule位于同一平面，两个轴重合，1个自由度消失。

此时，如果想要中间的箭头做浅灰色的运动，由于消失一个自由度、则需要多个轴一起旋转。


如此，该箭头的运动轨迹变成了一条曲线。

总结：欧拉角的“万向节死锁”问题，是由于欧拉旋转定义本身造成的。万向节死锁的结果，不是说不能旋转了，而是会导致不自然的旋转。

附加：
旋转坐标变换
设坐标系{B}与坐标系{A}有共同的坐标，但两者的方位不同，用旋转矩阵R表示{B}相对与{A}的方位，即从A系到B系的旋转矩阵。同一点p在两个坐标系{A}和{B}中描述的坐标有以下关系

注意：此公式的坐标为固定坐标系下的坐标，右边为动坐标系下的坐标， R为从固定坐标系到动坐标系的旋转矩阵，之所以左边为固定坐标系下的坐标，是因为这是为机器人专门设计的旋转矩阵，机器人的基座标系为固定坐标系，一般都是将末端执行器上即动坐标系上面一点相对与该动坐标系的坐标转换到基体坐标系下。
而对于无人机，其mpu求得的加速度等都是相对与世界坐标系的，但是无人机所需要的是相对于基体坐标系的角度，故需要将世界坐标系下求得的加速度转换到基体坐标系下才有意义，所以无人机的坐标变换矩阵下面这种方式更有意义。

参考：
《euler（gimbal）explained》
《机器人学基础》蔡自兴
https://blog.csdn.net/AndrewFan/article/details/60981437#
https://www.matongxue.com/madocs/442
https://www.zhihu.com/question/47736315
《多旋翼无人飞行器嵌入式飞控开发指南》