写这篇博客，已经是第三次了，花了一个周，一遍遍修改，只为了理解好姿态解算并表述出来。
之前写过一篇姿态解算的博客，
UAV021（四）：飞控传感器数据融合与姿态估计
，在小角度假设条件（俯仰角、滚转角都很小）下做了近似，并且使用欧拉角的方式来实现姿态解算，模型是很简单的。这一次想去除此假设，并且添加四元数来解算姿态，问题一下子变得复杂起来。
一、基本概念
1.1 坐标系
-
机体坐标系
机体坐标系（机体系）以机头方向为 x 轴，机体水平向右为 y 轴，垂直机体向下为 z 轴，即前右下，符合右手定则。
-
地球坐标系
地球坐标系（地球系） x 轴方向朝北，y 轴方向朝东，z 轴方向朝地，也即北东地（NED），同样符合右手定则。
-
航向坐标系
机体系 x、y 轴在水平面的投影为航向坐标系（航向系）的 x、y轴，竖直向下为 z 轴。
可见，航向系可由地球系绕 z 轴旋转一定角度得到。很多地方都没有航向系这个概念，但是匿名使用了，经过反复思考之后，添加这个坐标系是很利于后面姿态解算的，故说明。
1.2 姿态角
姿态解算就是通过融合传感器数据解算出姿态角。姿态角是俯仰角（pitch）、滚转角（roll）和偏航角（yaw）的合称，此文分别使用
α
,
β
,
γ
\alpha, \beta, \gamma
α
,
β
,
γ
表示。
-
俯仰角
俯仰角是无人机机体系 x 轴与水平面夹角，也即机体系与航向系 x 轴的夹角，机头上仰为正，范围
α∈
[
−
π
/
2
,
π
/
2
]
\alpha \in [-\pi/2, \pi/2]
α
∈
[
−
π
/
2
,
π
/
2
]
。 -
滚转角
滚转角是无人机机体系 y 轴与水平面夹角，也即机体系与航向系 y 轴的夹角，机身左升右降为正，范围
β∈
[
−
π
,
π
]
\beta \in [-\pi, \pi]
β
∈
[
−
π
,
π
]
。 -
偏航角
偏航角是无人机机体系 x 轴在水平面投影与地球系 x 轴（正北方）的夹角，也即航向系 x 轴与地球系 x 轴夹角。俯视机身，顺时针方向（往东）角度递增，逆时针方向角度递减，范围
γ∈
[
−
π
,
π
]
\gamma \in [-\pi, \pi]
γ
∈
[
−
π
,
π
]
。
可见，引入航向系之后可以更加方便地定义姿态角。注意与下面的欧拉角作对比，欧拉角和姿态角不是同样概念，这也是这里使用
α
,
β
,
γ
\alpha, \beta, \gamma
α
,
β
,
γ
而不是更常见的
θ
,
ϕ
,
ψ
\theta, \phi, \psi
θ
,
ϕ
,
ψ
的原因，后者用于表示欧拉角。
二、姿态解算
2.1 综述
2.1.1 姿态解算与数据融合
姿态解算是指使用传感器数据解算出姿态角，数据融合更强调各传感器之间数据的取长补短。在无人机中，传感器常用九轴传感器（三轴加速度、三轴陀螺仪、三轴磁力计）。加速度计与陀螺仪融合得到俯仰、滚转角，磁力计与陀螺仪融合得到偏航角。解算有姿态角，常用欧拉角、四元数与旋转矩阵三种方式；融合常用互补滤波与卡尔曼滤波。
比如使用欧拉角姿态解算互补滤波数据融合。如下图所示，其中公式1，公式2，角速度积分等都使用了欧拉角，将传感器数据不断变换到姿态角，最后一个互补滤波融合数据。当然，并不一定都是解算后融合，很多时候两者是交叉的，例如四元数姿态解算互补滤波数据融合的时候。
上图在后面会有具体说明，现在不怎么理解也没关系。
2.1.2 欧拉角、四元数、旋转矩阵的联系
无人机中，常提到三种姿态解算的方式：欧拉角、四元素和旋转矩阵。欧拉角最为直观简单，但是有死锁问题；四元数添加一个元素，将三维计算映射到四维又返回三维，比较抽象不易理解，但避免了死锁问题；旋转矩阵有九个元素，求解相对复杂，几乎都不被采用。
在实际运用中，如果无人机没有大机动（俯仰、滚转角小于60°就不算大机动），用欧拉角已经绰绰有余，所以欧拉角还是很实用的。但是想做个空翻的四轴，欧拉角就会有问题了（至少理论上有），需采用四元数。至于旋转矩阵，理论上会说明其概念及其重要性，但不会有最终的测试。
首先来看一个三者之间的关系图，有了整体概念之后再分别研究，不要在学习推导的过程中迷失方向。三者之间是可以相互转换的，后面的内容将一一说明。
2.2 欧拉角
2.2.1 欧拉角的定义
欧拉角描述了两个坐标系之间的转换关系，以机体系和地球系说明问题。机体系是运动的，地球系是固定的。假设初始时两坐标系重合，欧拉角分别是连续绕坐标轴三次旋转的三个角度。但是需要明确三个问题：
- 绕哪个坐标系的坐标轴
- 先后绕了哪些坐标轴旋转
- 分别绕坐标轴旋转了多少度
只有这三个问题都说清楚了，我们才能够确定机体系相对于地球系的状态。
对于问题一：可以绕机体系的坐标轴，称为内旋；也可以绕地球系的坐标轴，称为外旋。内外容易理解记住，对于无人机而言，自己的轴肯定是内部的，地球系的轴是外部的，内外容易区分。此文采用内旋定义欧拉角。值得注意的是，无论是使用内旋还是外旋定义欧拉角，姿态角和欧拉角都不是完全一致的，不要把姿态角与欧拉角混为一谈。
对于问题二：无人机中最常用的是 z-y-x 的顺序。据说是因为此种顺序万向节死锁的位置比较好（无人机很少会到达此角度），此文采用。
对于问题三：遵循习惯，绕 x 轴旋转角用
ϕ
\phi
ϕ
表示，绕 y 轴旋转角用
θ
\theta
θ
表示，绕 z 轴旋转角用
ψ
\psi
ψ
表示。绕坐标轴逆时针旋转为正，顺时针为负。注意判断顺逆时针时，把坐标系的箭头对准自己（比如 z 轴应该从下往上看），再来判断是顺时针还是逆时针。
综上，初始时机体系与地球系重合，无人机先绕机体系 z 轴旋转
ψ
\psi
ψ
，再绕机体系 y 轴旋转
θ
\theta
θ
，最后绕机体系 x 轴旋转
ϕ
\phi
ϕ
，无人机就有一个确定的姿态，我们把
[
θ
,
ϕ
,
ψ
]
[\theta, \phi, \psi]
[
θ
,
ϕ
,
ψ
]
合称为欧拉角。
2.2.2 从欧拉角与姿态角说起
无论是使用欧拉角，还是四元数、旋转矩阵，目标都是解算姿态角，因此，需要把姿态角与这些方式联系起来。欧拉角是绕机体系旋转轴旋转的角度，姿态角是机体系坐标轴与水平面的夹角（俯仰、滚转角）和与正北方的夹角（偏航角），两者存在什么关系呢？
不要感觉很复杂，先来简单想象一下：
- 最开始的状态，机体系与地球系重合，机头朝北；
-
然后无人机先绕 z 轴逆时针旋转（俯视无人机是顺时针旋转），比如
ψ=
90
°
\psi=90°
ψ
=
9
0
°
，此时无人机头朝东，欧拉角为
[0
,
0
,
90
°
]
[0, 0, 90°]
[
0
,
0
,
9
0
°
]
， 姿态角为
[0
,
0
,
90
°
]
[0, 0, 90°]
[
0
,
0
,
9
0
°
]
； -
无人机绕 y 轴逆时针旋转60°，由于是水平的，容易想象无人机有且仅有俯仰角增大，而且刚好为 60°。此时欧拉角为
[60
°
,
0
,
90
°
]
[60°, 0, 90°]
[
6
0
°
,
0
,
9
0
°
]
，姿态角也为
[60
°
,
0
,
90
°
]
[60°, 0, 90°]
[
6
0
°
,
0
,
9
0
°
]
。 -
无人机再绕 x 轴逆时针旋转45°，根据定义，欧拉角为
[60
°
,
45
°
,
90
°
]
[60°, 45°, 90°]
[
6
0
°
,
4
5
°
,
9
0
°
]
。但是姿态角也是
[60
°
,
45
°
,
90
°
]
[60°, 45°, 90°]
[
6
0
°
,
4
5
°
,
9
0
°
]
吗？如果不是，应该是多少呢？
为了解决上面的问题，我们不得不先学点东西。不过不要害怕，理论也挺有意思的。
2.2.3 姿态变化率与机体角速度的关系
上面需要求解姿态与欧拉角的关系，欧拉角也即机体转动角度。这需要对立体几何进行分析，过程略显复杂，没有推导出来。但是姿态变化率与机体角速度（欧拉角变化率）之间却有一个现成的公式——欧拉运动学方程：
从姿态变化率到机体角速度的关系：
[
ω
x
ω
y
ω
z
]
=
[
1
0
−
sin
α
0
cos
β
cos
α
sin
β
0
−
sin
β
cos
α
cos
β
]
[
α
′
β
′
γ
′
]
(2.1)
\left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end {array} \right]= \left[ \begin{array}{cc} 1 & 0 & -\sin \alpha \\ 0 & \cos \beta & \cos \alpha \sin \beta \\ 0 & -\sin \beta & \cos \alpha \cos \beta \end {array} \right] \left[ \begin{array}{c} \alpha’ \\ \beta’ \\ \gamma’ \end {array} \right] \tag{2.1}
⎣
⎡
ω
x
ω
y
ω
z
⎦
⎤
=
⎣
⎡
1
0
0
0
cos
β
−
sin
β
−
sin
α
cos
α
sin
β
cos
α
cos
β
⎦
⎤
⎣
⎡
α
′
β
′
γ
′
⎦
⎤
(
2
.
1
)
其中，
α
,
β
,
γ
\alpha, \beta, \gamma
α
,
β
,
γ
分别为俯仰角、滚转角和偏航角。
反过来，从机体角速度到姿态变化率的转换为式(2.1) 中矩阵的逆：
[
α
′
β
′
γ
′
]
=
[
1
tan
α
sin
β
tan
α
cos
β
0
cos
β
−
sin
β
0
sin
β
/
cos
α
cos
β
/
cos
α
]
[
ω
x
ω
y
ω
z
]
(2.2)
\left[ \begin{array}{c} \alpha’ \\ \beta’ \\ \gamma’ \end {array} \right]= \left[ \begin{array}{cc} 1 & \tan \alpha \sin \beta & \tan \alpha \cos \beta \\ 0 & \cos \beta & -\sin \beta \\ 0 & \sin \beta / \cos \alpha & \cos \beta / \cos \alpha \end {array} \right] \left[ \begin{array}{c} \omega_x \\ \omega_y \\ \omega_z \end {array} \right] \tag{2.2}
⎣
⎡
α
′
β
′
γ
′
⎦
⎤
=
⎣
⎡
1
0
0
tan
α
sin
β
cos
β
sin
β
/
cos
α
tan
α
cos
β
−
sin
β
cos
β
/
cos
α
⎦
⎤
⎣
⎡
ω
x
ω
y
ω
z
⎦
⎤
(
2
.
2
)
可见，
ω
x
,
ω
y
,
ω
z
\omega_x, \omega_y, \omega_z
ω
x
,
ω
y
,
ω
z
已知时，实际中可以使用陀螺仪测量。上式为关于
α
,
β
,
γ
\alpha, \beta, \gamma
α
,
β
,
γ
的微分方程组，三个独立方程三个未知数，方程可解。当然，工程中不去实际去解方程，更多的是使用数值计算方法。
为了求解之前的问题，我们模拟一下陀螺仪，把每次角度变化分为1000小份，并假设加机体角速度是均匀变化的。
那么，首先绕 z 轴逆时针旋转90°，假设 1s 完成，角速度为 90°/s，分成1000小份累加；再绕 y 轴逆时针旋转 60°与绕 x 轴逆时针旋转 45°，也分别使用 1s 完成，也都分为1000小份累加。
2.2.3 矩阵表示旋转的理论推导
先说明一下，在下面所有的描述中，涉及的坐标系的原点是重合的（即使不画在一起），因为我们研究的是转动关系，不是空间位置关系。坐标系的各个轴都是正交的，没有斜坐标系，默认此条件。
首先看二维平面内的旋转。坐标系
x
O
y
xOy
x
O
y
绕原点逆时针旋转
ψ
\psi
ψ
得到
x
′
O
y
′
x’Oy’
x
′
O
y
′
，点A在两坐标系下坐标分别为
(
x
1
,
y
1
)
(x_1, y_1)
(
x
1
,
y
1
)
和
(
x
2
,
y
2
)
(x_2, y_2)
(
x
2
,
y
2
)
，则
[
x
2
y
2
]
=
[
cos
ψ
sin
ψ
−
sin
ψ
cos
ψ
]
[
x
1
y
1
]
(2.1)
\left[ \begin{array}{c} x_2 \\ y_2 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \end {array} \right] \tag{2.1}
[
x
2
y
2
]
=
[
cos
ψ
−
sin
ψ
sin
ψ
cos
ψ
]
[
x
1
y
1
]
(
2
.
1
)
证明如下：
设A点与原点距离为
d
d
d
，则：
x
2
=
d
cos
(
α
−
ψ
)
=
d
(
cos
α
cos
ψ
+
sin
α
sin
ψ
)
=
(
d
cos
α
)
cos
ψ
+
(
d
sin
α
)
sin
ψ
=
x
1
cos
ψ
+
y
1
sin
ψ
\begin{aligned} x_2 &= d \cos(\alpha – \psi) \\ &=d(\cos \alpha \cos \psi + \sin \alpha \sin \psi) \\ &=(d \cos \alpha) \cos \psi + (d\sin \alpha) \sin \psi \\ &=x_1 \cos \psi + y_1 \sin \psi \\ \end {aligned}
x
2
=
d
cos
(
α
−
ψ
)
=
d
(
cos
α
cos
ψ
+
sin
α
sin
ψ
)
=
(
d
cos
α
)
cos
ψ
+
(
d
sin
α
)
sin
ψ
=
x
1
cos
ψ
+
y
1
sin
ψ
y
2
=
d
sin
(
α
−
ψ
)
=
d
(
sin
α
cos
ψ
−
cos
α
sin
ψ
)
=
(
d
sin
α
)
cos
ψ
−
(
d
cos
α
)
sin
ψ
=
y
1
cos
ψ
−
x
1
sin
ψ
\begin{aligned} y_2 &= d \sin(\alpha – \psi) \\ &=d(\sin \alpha \cos \psi – \cos \alpha \sin \psi) \\ &=(d \sin \alpha) \cos \psi – (d\cos \alpha) \sin \psi \\ &=y_1 \cos \psi – x_1 \sin \psi \\ \end {aligned}
y
2
=
d
sin
(
α
−
ψ
)
=
d
(
sin
α
cos
ψ
−
cos
α
sin
ψ
)
=
(
d
sin
α
)
cos
ψ
−
(
d
cos
α
)
sin
ψ
=
y
1
cos
ψ
−
x
1
sin
ψ
— 证毕 —
对于上述旋转矩阵，从两个方向推广。一是朝三维方向推广，上图中，可看作
O
x
y
z
Oxyz
O
x
y
z
坐标系绕 z 轴逆时针旋转
ψ
\psi
ψ
得到
O
x
′
y
′
z
′
Ox’y’z’
O
x
′
y
′
z
′
坐标系，点 A 在两个坐标系下的坐标分别为
(
x
1
,
y
1
,
z
1
)
(x_1, y_1, z_1)
(
x
1
,
y
1
,
z
1
)
，
(
x
2
,
y
2
,
z
2
)
(x_2, y_2, z_2)
(
x
2
,
y
2
,
z
2
)
。由于是绕 z 轴旋转，z 坐标是不变的，且z坐标不会影响 x、y轴上的变化，即
z
2
=
0
⋅
x
1
+
0
⋅
y
1
+
1
⋅
z
1
z_2 = 0\cdot x_1 + 0\cdot y_1 + 1\cdot z_1
z
2
=
0
⋅
x
1
+
0
⋅
y
1
+
1
⋅
z
1
，结合式（2.1），易得：
[
x
2
y
2
z
2
]
=
[
cos
ψ
sin
ψ
0
−
sin
ψ
cos
ψ
0
0
0
1
]
[
x
1
y
1
z
1
]
(2.2)
\left[ \begin{array}{c} x_2 \\ y_2 \\ z_2 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \\ z_1 \end {array} \right] \tag{2.2}
⎣
⎡
x
2
y
2
z
2
⎦
⎤
=
⎣
⎡
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
⎦
⎤
⎣
⎡
x
1
y
1
z
1
⎦
⎤
(
2
.
2
)
中间的矩阵便叫作旋转矩阵，此处用
R
z
R_z
R
z
表示，代表绕 z 轴转动的情况：
R
z
=
[
cos
ψ
sin
ψ
0
−
sin
ψ
cos
ψ
0
0
0
1
]
(2.3)
R_z = \left[ \begin{array}{cc} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end {array} \right] \tag{2.3}
R
z
=
⎣
⎡
cos
ψ
−
sin
ψ
0
sin
ψ
cos
ψ
0
0
0
1
⎦
⎤
(
2
.
3
)
同理，如果是绕 y 轴旋转
θ
\theta
θ
，那么在二维旋转矩阵的基础上变换，由于y 轴坐标在两个坐标系中大小不变，旋转矩阵第二行为
[
0
1
0
]
[0\quad1\quad0]
[
0
1
0
]
，容易直接写出：
R
y
=
[
cos
θ
0
sin
θ
0
1
0
−
sin
θ
0
cos
θ
]
(2.4)
R_y = \left[ \begin{array}{cc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end {array} \right] \tag{2.4}
R
y
=
⎣
⎡
cos
θ
0
−
sin
θ
0
1
0
sin
θ
0
cos
θ
⎦
⎤
(
2
.
4
)
绕 x 轴旋转
ϕ
\phi
ϕ
时，x 方向坐标在两个坐标系下相等，第一行为
[
1
0
0
]
[1\quad0\quad0]
[
1
0
0
]
，整个旋转矩阵为：
R
x
=
[
1
0
0
0
cos
ϕ
sin
ϕ
0
−
sin
ϕ
cos
ϕ
]
(2.5)
R_x = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi \end {array} \right] \tag{2.5}
R
x
=
⎣
⎡
1
0
0
0
cos
ϕ
−
sin
ϕ
0
sin
ϕ
cos
ϕ
⎦
⎤
(
2
.
5
)
另一个推广时连续旋转，如果坐标系
x
′
′
O
y
′
′
x”Oy”
x
′
′
O
y
′
′
又是坐标系
x
′
O
y
′
x’Oy’
x
′
O
y
′
逆时针旋转
ψ
2
\psi_2
ψ
2
，点A 在
x
′
′
O
y
′
′
x”Oy”
x
′
′
O
y
′
′
下的坐标为
(
x
3
,
y
3
)
(x_3, y_3)
(
x
3
,
y
3
)
，那么易得：
[
x
3
y
3
]
=
[
cos
ψ
2
sin
ψ
2
−
sin
ψ
2
cos
ψ
2
]
[
x
2
y
2
]
\left[ \begin{array}{c} x_3 \\ y_3 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi_2 & \sin \psi_2 \\ -\sin \psi_2 & \cos \psi_2 \end {array} \right] \left[ \begin{array}{c} x_2 \\ y_2 \end {array} \right]
[
x
3
y
3
]
=
[
cos
ψ
2
−
sin
ψ
2
sin
ψ
2
cos
ψ
2
]
[
x
2
y
2
]
也即：
[
x
3
y
3
]
=
[
cos
ψ
2
sin
ψ
2
−
sin
ψ
2
cos
ψ
2
]
[
cos
ψ
sin
ψ
−
sin
ψ
cos
ψ
]
[
x
1
y
1
]
\left[ \begin{array}{c} x_3 \\ y_3 \end {array} \right]= \left[ \begin{array}{cc} \cos \psi_2 & \sin \psi_2 \\ -\sin \psi_2 & \cos \psi_2 \end {array} \right] \left[ \begin{array}{cc} \cos \psi & \sin \psi \\ -\sin \psi & \cos \psi \end {array} \right] \left[ \begin{array}{c} x_1 \\ y_1 \end {array} \right]
[
x
3
y
3
]
=
[
cos
ψ
2
−
sin
ψ
2
sin
ψ
2
cos
ψ
2
]
[
cos
ψ
−
sin
ψ
sin
ψ
cos
ψ
]
[
x
1
y
1
]
根据矩阵运算的结合律，完全可以先计算左边两个旋转矩阵的连乘，得到一个新的旋转矩阵。这说明，连续旋转等效于旋转矩阵的连乘。这个结论完全可以推广到三维，依旧使用矩阵运算的结合律容易证明。所以，如果分别绕 z-y-x 旋转一次，那么可以直接使用这三个旋转矩阵的乘积（一个旋转矩阵）来代替。
R
=
R
z
R
y
R
x
R=R_zR_yR_x
R
=
R
z
R
y
R
x
将式（2.3）~（2.5）代入，并用
R
b
e
R_b^e
R
b
e
表示最终的旋转矩阵可得：
R
e
b
=
[
cos
θ
cos
ψ
cos
ψ
sin
θ
sin
ϕ
−
sin
ψ
cos
ϕ
cos
ψ
sin
θ
cos
ϕ
+
sin
ψ
sin
ϕ
cos
θ
sin
ψ
sin
ψ
sin
θ
sin
ϕ
+
cos
ψ
cos
ϕ
sin
ψ
sin
θ
cos
ϕ
−
cos
ψ
sin
ϕ
−
sin
θ
sin
ϕ
cos
θ
cos
ϕ
cos
θ
]
(2.6)
R_e^b={ \left[ \begin{array}{ccc} \cos\theta \cos\psi& \cos\psi \sin\theta \sin\phi-\sin\psi \cos \phi & \cos\psi \sin\theta \cos\phi +\sin\psi \sin\phi \\ \cos\theta \sin\psi & \sin\psi \sin\theta \sin\phi+\cos\psi \cos\phi & \sin\psi \sin\theta \cos\phi-\cos\psi \sin\phi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \end{array} \right ]} \tag{2.6}
R
e
b
=
⎣
⎡
cos
θ
cos
ψ
cos
θ
sin
ψ
−
sin
θ
cos
ψ
sin
θ
sin
ϕ
−
sin
ψ
cos
ϕ
sin
ψ
sin
θ
sin
ϕ
+
cos
ψ
cos
ϕ
sin
ϕ
cos
θ
cos
ψ
sin
θ
cos
ϕ
+
sin
ψ
sin
ϕ
sin
ψ
sin
θ
cos
ϕ
−
cos
ψ
sin
ϕ
cos
ϕ
cos
θ
⎦
⎤
(
2
.
6
)
这也即是欧拉角-四元数-旋转矩阵关系图里的第一个转换关系。注意这个矩阵是无人机分别绕机体系 z-y-x 坐标轴分别逆时针转动
ψ
,
ϕ
,
θ
\psi, \phi, \theta
ψ
,
ϕ
,
θ
得到的。再强调一下，是绕机体系，是 z-y-x 的旋转顺序，是分别逆时针旋转
ψ
,
ϕ
,
θ
\psi, \phi, \theta
ψ
,
ϕ
,
θ
角度。
推导完了，回到之前的问题，我们继续解决无人机绕 z-y-x 分别是
[
90
°
,
60
°
,
45
°
]
[90°, 60°, 45°]
[
9
0
°
,
6
0
°
,
4
5
°
]
时的姿态。注意不要先入为主，一下子又联系到熟悉的旋转矩阵
R
b
e
R_b^e
R
b
e
，那只不过是我们顺便提一下的结论而已，真正的“道”是对上面旋转的深入理解，然后具体问题具体分析。
2.2.4 欧拉角与姿态角的联系
首先，对于俯仰角与滚转角。从定义上看，俯仰角与滚转角分别是机体系 x、y 轴与航向系 x、y 轴的夹角，也即是旋转航向系使得和机体系重合的欧拉角。
无人机绕 z 轴逆时针旋转 90° 后，机体系与航向系依旧是重合的，绕 z 轴的旋转矩阵为：
R
z
=
[
1
0
0
0
1
0
0
0
1
]
R_z = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end {array} \right]
R
z
=
⎣
⎡
1
0
0
0
1
0
0
0
1
⎦
⎤
无人机绕 y 轴逆时针旋转 60° 后，可得旋转矩阵：
R
y
=
[
cos
60
°
0
sin
60
°
0
1
0
−
sin
60
°
0
cos
60
°
]
=
[
1
/
2
0
3
/
2
0
1
0
−
3
/
2
0
1
/
2
]
R_y = \left[ \begin{array}{cc} \cos 60° & 0 & \sin 60° \\ 0 & 1 & 0 \\ -\sin 60° & 0 & \cos 60° \end {array} \right] = \left[ \begin{array}{cc} 1/2 & 0 & \sqrt{3}/2 \\ 0 & 1 & 0 \\ -\sqrt{3}/2 & 0 & 1/2 \end {array} \right]
R
y
=
⎣
⎡
cos
6
0
°
0
−
sin
6
0
°
0
1
0
sin
6
0
°
0
cos
6
0
°
⎦
⎤
=
⎣
⎡
1
/
2
0
−
3
/
2
0
1
0
3
/
2
0
1
/
2
⎦
⎤
最后无人机绕 x 轴逆时针旋转 45°，
R
x
=
[
1
0
0
0
cos
45
°
sin
45
°
0
−
sin
45
°
cos
45
°
]
=
[
1
0
0
0
2
/
2
2
/
2
0
−
2
/
2
2
/
2
]
R_x = \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos 45° & \sin 45° \\ 0 & -\sin 45° & \cos 45° \end {array} \right]= \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \sqrt{2}/2 & \sqrt{2}/2 \\ 0 & -\sqrt{2}/2 & \sqrt{2}/2 \end {array} \right]
R
x
=
⎣
⎡
1
0
0
0
cos
4
5
°
−
sin
4
5
°
0
sin
4
5
°
cos
4
5
°
⎦
⎤
=
⎣
⎡
1
0
0
0
2
/
2
−
2
/
2
0
2
/
2
2
/
2
⎦
⎤
整个旋转矩阵最终为：
R
=
R
z
R
y
R
x
=
[
1
0
0
0
1
0
0
0
1
]
[
cos
60
°
0
sin
60
°
0
1
0
−
sin
60
°
0
cos
60
°
]
[
1
0
0
0
cos
45
°
sin
45
°
0
−
sin
45
°
cos
45
°
]
=
1
\begin{aligned} R &=R_z R_y R_x \\ &= \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end {array} \right] \left[ \begin{array}{cc} \cos 60° & 0 & \sin 60° \\ 0 & 1 & 0 \\ -\sin 60° & 0 & \cos 60° \end {array} \right] \left[ \begin{array}{cc} 1 & 0 & 0 \\ 0 & \cos 45° & \sin 45° \\ 0 & -\sin 45° & \cos 45° \end {array} \right] &= 1 \end{aligned}
R
=
R
z
R
y
R
x
=
⎣
⎡
1
0
0
0
1
0
0
0
1
⎦
⎤
⎣
⎡
cos
6
0
°
0
−
sin
6
0
°
0
1
0
sin
6
0
°
0
cos
6
0
°
⎦
⎤
⎣
⎡
1
0
0
0
cos
4
5
°
−
sin
4
5
°
0
sin
4
5
°
cos
4
5
°
⎦
⎤
=
1