无人机常用坐标系、旋转矩阵、欧拉角、四元数以及相互转换关系

一、坐标系

1. 地心惯性坐标系$O{X}_i{Y}_i{Z}_i$$(i$系)

  该坐标系是惯性组件测量的基准。该系各轴确定方法如下 $:$ 地球的中心O为i系的原点，该系的基面为赤道面。$Xi$轴在地球赤道平面内并指向春分点，设地球自转轴为$Zi$轴，向北为正。由右手坐标系获得$Yi$，这三者组合$OXiYiZi$称为地心惯性坐标系。研究中通常假定它为一个理想坐标系，即认为它是时不变的，是相对惯性空间无任何运动的，且在该系经典力学可全部被应用；

2. 地球坐标系$O{X}_e{Y}_e{Z}_e$($e$系)

  地球的中心$O$为$e$系的原点，固联于地球，设地球自转轴为$Z_i$轴，向北为正。$X_e$由位于赤道平面内且通过零子午线。由右手坐标系获得$Y_i$这三者组合$O{X}_e{Y}_e{Z}_e$即为地球坐标系；

3. 地理坐标系$O{X}_g{Y}_g{Z}_g$($g$系)

  载体重心为其原点。$N$轴($X_g$)指向北；$E$轴($Y_g$)指向东；D轴($Z_g$)指向地表。此时的地理坐标系称为北东地坐标系(ONED)。在有些研究中，东北天(OENU)坐标系被采用，$Z_g$轴指向天顶；

4. 载体坐标系$O{X}_b{Y}_b{Z}_b$($b$系)

  该系为右手坐标系，其原点位于载体的重心，且固连在载体上，各轴配置如下：
  $X_b$轴沿载体纵轴指向前，$Y_b$轴沿载体横轴指向右；
  $Z_b$轴沿载体的竖轴指向下，并垂直于$O{X}_b{Y}_b$ 轴；

5. 平台坐标系$O{X}_p{Y}_p{Z}_p$($p$系)

  固连于物理(对平台式)或数学平台(对SINS)的右手直角坐标系；

6. 导航坐标系$O{X}_n{Y}_n{Z}_n$($n$系)

  该系常用于求解导航参数，根据平台不同有两种配置方式：
   1)平台式惯导系统
  导航坐标系常取理想的平台坐标系。
  2)SINS系统
  导航参数可在非载体坐标系上求解，例如可将加速度信号分解到特定的坐标系上以降低计算难度，而常用的导航坐标系之一是地理坐标系；

7. 计算坐标系$O{X}_c{Y}_c{Z}_c$($c$系)

  该系是一种虚拟坐标系，用于研究惯导系统，是以计算所得的经纬度为原点$O$建立起来的地理坐标系，它与载体实际位置$O$点上建立的地理坐标系$O{X}_g{Y}_g{Z}_g$不一致。两个坐标系之间的夹角为惯导系统的定位误差。为方便研究，定义重角：$O{X}_p{Y}_p{Z}_p$系相对于$O{X}_c{Y}_c{Z}_c$系的夹角；平台的姿态角：$O{X}_p{Y}_p{Z}_p$系相对于$O{X}_g{Y}_g{Z}_g$系的夹角。

二、无人机常用坐标系

1. 导航坐标系($n$系)

  常用的导航坐标系有北东地和东北天两种。
  两种坐标系的指向分别定义如下：

1.1 北东地坐标系

   X轴：指北;
   Y轴：指东;
   Z轴：指地。
 

1. 2 东北天坐标系

   X轴：指东;
   Y轴：指北;
   Z轴：指天。
 

2. 载体坐标系($b$系)

  与导航坐标系类似，常用的载体坐标系也有如下两种：

2.1 前右下坐标系—对应北东地导航坐标系

   X轴：指向载体前进方向;
   Y轴：指向载体右侧;
   Z轴：指向下。
 

2.2 右前上坐标系—对应东北天导航坐标系

   X轴：指向载体右侧;
   Y轴：指向载体前进方向;
   Z轴：指向上。

三、 姿态角/轴向/范围

  从导航坐标系经过3次旋转，得到载体坐标系，3次旋转的角度为姿态角。

1.姿态角

• 航向角: 也叫做方位角,偏航角, yaw, azimuth, heading. 希腊字母表示: $\psi$(读作psi)
• 俯仰角:也叫做pitch, elevation. 希腊字母表示: $\theta$(读作theta)
• 横滚角: 也叫做roll, 希腊字母表示 $\varphi$(读作phi)

姿态角	符号	读法
航向角	$\psi$	psi
俯仰角	$\theta$	theta
横滚角	$\varphi$	phi

2. 姿态角范围

   (1) 俯仰角(-90 ~ 90deg)；
   (2) 横滚角(-180 ~ 180deg)；
   (3) 航向角(-180 ~ 180deg，可转换为0~360deg)；
   满足右手定则

3. 姿态角对应轴向

   (1) 北东地坐标系
    X轴：横滚角；Y轴：俯仰角；Z轴：航向角。
   (2) 东北天坐标系
    X轴：俯仰角；Y轴：横滚角；Z轴：航向角。

四、姿态解算

1. 坐标变换

  以导航坐标系与载体坐标系之间的姿态旋转矩阵为例。所谓的姿态解算，就是求出当前的机体坐标系b（载体坐标系） 相对于地理坐标系n（导航坐标系） 的变化。由于n系和b 系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可理解成刚体的定点转动。而刚体定点转动这个变化可以通过一个变换矩阵来表示，这个矩阵包含了刚体的所有姿态信息。
右手系
$$\left[\begin{array}{cc}a& -c\\ c& a\end{array}\right]\text{\hspace{1em}(1)}$$

左手系
$$\left[\begin{array}{cc}a& c\\ -c& a\end{array}\right]\text{\hspace{1em}(2)}$$

1.1 旋转方式

  以z轴旋转为例：旋转方式由两种坐标系的旋转和向量的旋转

a. 坐标系的旋转

  如上图所示， $P$ 点不变，坐标系$O-XYZ$ 旋转$\alpha$,得到新的坐标系 $O-X^{{}^{\prime }}{Y}^{{}^{\prime }}{Z}^{{}^{\prime }}$ ，在新坐标系下，$P$点的坐标变为 $P^{{}^{\prime }}$，则有：

          $P=R_z(\alpha )P^{{}^{\prime }}$

b. 向量的旋转

  如上图所示，坐标系$O-XYZ$不变，$P^{{}^{\prime }}$点旋转$\alpha$ ,得到新的点 $P$ ，则有：

            $P=R_z(\alpha )P^{{}^{\prime }}$

2. 左乘右乘

  本身矩阵就是为了表示旋转而生的，三个轴的旋转也就是将三个旋转矩阵相乘即可。我们首先给出绕 $\mathrm{x},\mathrm{y}$, z轴各自旋转的旋转矩阵表示：

${\mathbf{R}}_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$

${\mathbf{R}}_y(\gamma )=\left[\begin{array}{ccc}\cos \gamma & 0& \sin \gamma \\ 0& 1& 0\\ -\sin \gamma & 0& \cos \gamma \end{array}\right]$

${\mathbf{R}}_z(\alpha )=\left[\begin{array}{ccc}\cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]$
其中，旋转角的正方向符合右手螺旋定则。

内旋和外旋

  每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。每次旋转是绕自身旋转之后的轴旋转，称为内旋。下图说明了内旋和外旋的区别。

  此处注意乘法规则, 我们前面得到的矩阵是外旋矩阵, 外旋是左乘，这里的左乘, 并不是算数中的左乘, 很多认错认为 $\mathrm{Y}-\mathrm{X}-\mathrm{Z}$ 的左乘，就应该是 $R_Y\times R_X\times R_Z$, 这是错误的, 根据外旋 的规则, 应该是 $R_Z\times R_X\times R_Y$, 不明白? 比如说我有一个向量 $\mathrm{P}(\mathrm{X},\mathrm{Y},\mathrm{Z}):$
1、先绕 $Y$ 旋转我们得到新坐标 $P(X1,Y1,Z1)$ 左乘指的是如下左乘：
$$\left[\begin{array}{l}{X}_1\\ Y_1\\ Z_1\end{array}\right]=R_Y\times \left[\begin{array}{l}X\\ Y\\ Z\end{array}\right]$$
2、先绕 $X$ 旋转我们得到新坐标 P(X2,Y2,Z2)
$$\left[\begin{array}{l}{X}_2\\ Y_2\\ Z_2\end{array}\right]=R_X\times \left[\begin{array}{l}{X}_1\\ Y_1\\ Z_1\end{array}\right]$$
3、先绕 $Z$ 旋转我们得到新坐标 P(X3,Y3,Z3)
$$\left[\begin{array}{l}{X}_3\\ Y_3\\ Z_3\end{array}\right]=R_Z\times \left[\begin{array}{l}{X}_2\\ Y_2\\ Z_2\end{array}\right]$$
4、把公式展开
$$\left[\begin{array}{l}{X}_3\\ Y_3\\ Z_3\end{array}\right]=R_Z\times \left(R_X\times \left(R_Y\times \left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\right)\right)=R_Z\times R_X\times R_Y\times \left[\begin{array}{l}X\\ Y\\ Z\end{array}\right]$$

姿态角	符号	读法	坐标轴
航向角	$\psi$	psi	z
俯仰角	$\theta$	theta	y
横滚角	$\varphi$	phi	x

  按照内旋方式，$Z-Y-X$旋转顺序 ( 指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X )，可得旋转矩阵 ( 内旋是右乘 )：
$$R1=R_{321}=R_z(\psi )\ast R_y(\theta )\ast R_x(\varphi )$$
  按照外旋方式， $X-Y-Z$旋转顺序 $($ 指先绕固定轴 $X$, 再绕固定轴Y，最后绕固定轴Z ) , 可得旋转矩阵 (外旋是左乘 ) :
$$R2=R_{123}=R_z(\psi )\ast R_y(\theta )\ast R_x(\varphi )$$
故R1=R2，具体不在此证明，记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。

五、方向余玄阵

  方向余弦表示一个向量的方向（姿态）我们可以用他在参考坐标系（地理坐标系）各个轴向的夹角的余弦来表示（及在各个轴的投影）。类似的 一个坐标系 可以看成是3个向量组成，所以三个向量分别在坐标轴上的投影可以用来表示一个坐标系与参考坐标系的关系。这总共9个方向余弦组成了一个三阶矩阵。
  设方向余弦阵为北东地坐标系到载体坐标系的转换矩阵$C_n^b$其
运动方程为：
$$\begin{array}{c}{\dot{C}}_n^b=-\omega C_n^b\end{array}$$
式中，$\omega =[pqr{]}^T$为载体相对北东地坐标系的旋转角速度。方向余弦阵是一个
正交矩阵，其逆等于其转置，即${C_n^b}^{-}$=${C_n^b}^T$

1. 优缺点

1.1 优点

1. 旋转轴可以是任意向量；

1.2 缺点

1. 旋转其实只需要知道一个向量+一个角度，一共4个值的信息，但矩阵法却使用了16个元素；
2. 做乘法操作时也会增加计算量，造成了空间和时间上的一些浪费；

六、欧拉角

1. 旋转顺序

  三维空间中刚体(或者坐标系) 定点转动需要三个自由度，最简单的就是欧拉角表示。但是由于旋转顺序问题，欧拉角表示不唯一。根据坐标系绕其轴的旋转顺序不同，存在12种定义方式。 常用的顺序有2种：国内一般用312旋转顺序(先转Z，然后X, 最后Y)；国外一般用321顺序(旋转Z，然后Y,最后X)。

常用坐标

• ENU-右前上（载体坐标系）
    右前上坐标系下，Z对应偏航，Y对应横滚，X对应俯仰。所以在常用的姿态旋转顺序（Yaw-Pitch-Roll）中，对应的旋转方式为312（Z-X-Y）。

举例说明

参考系（导航系）：(ENU)东北天
载体系（机体系）：(XYZ)右前上
旋转顺序：Z-X-Y(312)
姿态顺序：(Yaw-Pitch-Roll)偏航$\psi$-俯仰$\theta$-横滚$\varphi$
旋转轴：内旋

姿态角	符号	读法	坐标轴
航向角Yaw	$\psi$	psi	z
俯仰角Pitch	$\theta$	theta	x
横滚角Roll	$\varphi$	phi	y

$$\begin{array}{l}{R}_b^n=R_{ZXY}\\ =\left[\begin{array}{ccc}\text{ cosYaw* cosRoll }-\text{ sinYaw }\ast \text{ sinRoll }\ast \text{ sinPitch }& \text{ -cosPitch }\ast \text{ sinYaw }& \text{ cosYaw }\ast \text{ sinRoll }+\text{ cosRoll }\ast \text{ sinYaw }\ast \text{ sinPitch }\\ \text{ cosRoll }\ast \text{ sinYaw }+\text{ cosYaw }\ast \text{ sinRoll }\ast \text{ sinPitch }& \text{ cosYaw }\ast \text{ cosPitch }& \text{ sinYaw }\ast \text{ sinRoll }-\text{ cosYaw }\ast \text{ cosRoll }\ast \text{ sinPitch }\\ -\text{ cosPitch }\ast \text{ sinRoll }& \text{ sinPitch }& \text{ cosRoll }\ast \text{ cosPitch }\end{array}\right]\\ =\left[\begin{array}{ccc}\text{ cosψcosϕ }-\text{ sinψ }\text{ sinϕ }\text{ sin θ }& \text{ -cos θ }\text{ sinψ }& \text{ cosψ }\text{ sinϕ}+\text{ cosϕ }\text{ sinψ }\text{ sin θ }\\ \text{ cosϕ }\text{ sinψ }+\text{ cosψ }\text{ sinϕ }\text{ sin θ }& \text{ cosψ }\text{ cos θ }& \text{ sinψ }\text{ sinϕ }-\text{ cosψ }\text{ cosϕ }\text{ sin θ }\\ -\text{ cos θ }\text{ sinϕ }& \text{ sin θ }& \text{ cosϕ}\text{ cos θ }\end{array}\right]\end{array}$$

• NED-前右下（载体坐标系）
    前右下坐标系下，Z对应偏航，Y对应俯仰，X对应横滚。所以在常用的姿态旋转顺序（Yaw-Pitch-Roll）中，对应的旋转方式为321（Z-Y-X）。

按内旋方式 ( 绕固定轴）的旋转矩阵为：

$$R_{321}=R1=R_z(\gamma )\ast R_y(\beta )\ast R_x(\alpha )$$
按外旋方式 ( 绕固定轴）的旋转矩阵为：
$$R_{321}=R2=R_x(\gamma )\ast R_y(\beta )\ast R_z(\alpha )$$
姿态角方向都遵循右手定则。
注意：旋转矩阵和坐标系无关，所以NED下的ZYX和ENU下的ZYX是一样的，唯一不同的就是，姿态的命名和正负方向不一样了，绕X轴变成了横滚，绕Y轴变成了俯仰，Z轴还是偏航。

2. 优缺点

2.1 优点

1. 很容易理解，形象直观；

2. 表示更方便，只需要3个值（分别对应x、y、z轴的旋转角度）；但按我的理解，它还是转换到了3个3*3的矩阵做变换，效率不如四元数；

2.2 缺点

1. 旋转的次序无法确定
   之前提到过这种方法是要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同的结果；

2. 万向节锁
   会造成万向节锁（Gimbal Lock）的现象。这种现象的发生就是由于上述固定坐标轴旋转顺序造成的。理论上，欧拉旋转可以靠这种顺序让一个物体指到任何一个想要的方向，但如果在旋转中不幸让某些坐标轴重合了就会发生万向节锁，这时就会丢失一个方向上的旋转能力，也就是说在这种状态下我们无论怎么旋转（当然还是要原先的顺序）都不可能得到某些想要的旋转效果，除非我们打破原先的旋转顺序或者同时旋转3个坐标轴。

3. 不易在任意方向的旋转轴插值
   由于万向节锁的存在，欧拉旋转无法实现球面平滑插值；

七、四元数

1. 什么是四元数

  四元数是一个比较抽象的概念，他是站在四维空间的角度看待三维的旋转变化，比较难以理解。你可以简单的理解为，三维空间中的三轴旋转等价于绕某一个轴进行一个角度的旋转。
  顾名思义, 四元数就是包含四个元的一种数, 它可表示为
$$\mathbf{Q}=q_0+{\mathbf{q}}_v=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$$
其中, $q_0,q_1,q_2$ 和 $q_3$ 都是实数, $q_0$ 称为实部, ${\mathbf{q}}_v=q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$ 称为虚部。四元数可以看作是复数概念的扩充，有时也称其为超复数，当 $q_2=q_3=0$ 时四元数即退化为复数。四元数的虚数单位 $i,j,k$ 之间满足如下乘法运算规则:
$$\begin{array}{l}i\circ i=j\circ j=k\circ k=-1\\ i\circ j=k,j\circ k=i,k\circ i=j,j\circ i=-k,k\circ j=-i,i\circ k=-j\end{array}\}$$
即
$${\mathbf{i}}^2={\mathbf{j}}^2={\mathbf{k}}^2=ij\mathbf{k}=-1\text{ (哈密顿公式 })$$
其中，运算符“ $\circ$ ”表示四元数的乘法运算,在不引起歧义的情况下可写成“・”符号或直接省 略。上式中第一行运算规则与复数中虚数的运算规则完全相同; 第二行运算规则与三维空间中坐标轴单位矢量的叉乘运算规则相同。四元数可以看作是四维空间中的一种数,但 因其虚部单位矢量的叉乘运算特点, 也可将四元数的虚数部分 ${\mathbf{q}}_v=q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$ 看成是在三 维空间中的映象 (image), 反之,一个三维矢量可以看作是一个零标量四元数。

2. 旋转矩阵

由方向余弦阵式作恒等变形, 可得
$$\begin{array}{c}{\mathbf{C}}_b^i=\mathbf{I}+\sin \varphi (\mathbf{u}\times )+(1-\cos \varphi )(\mathbf{u}\times {)}^2=\mathbf{I}+2\sin \frac{\varphi }{2}\cos \frac{\varphi }{2}(\mathbf{u}\times )+2{\sin }^2\frac{\varphi }{2}(\mathbf{u}\times {)}^2=\\ \mathbf{I}+2\cos \frac{\varphi }{2}\left(\sin \frac{\varphi }{2}\mathbf{u}\times \right)+2{\left(\sin \frac{\varphi }{2}\mathbf{u}\times \right)}^2\end{array}$$
将上式的实部 $\cos \frac{\varphi }{2}=q_0$ 和虚部 $u\sin \frac{\varphi }{2}={\mathbf{q}}_v$ 代入 可得
$$\begin{array}{rl}& {\mathbf{C}}_b^i=\mathbf{I}+2q_0\left({\mathbf{q}}_v\times \right)+2{\left({\mathbf{q}}_v\times \right)}^2=\mathbf{I}+2q_0\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]+2{\left[\begin{array}{ccc}0& -q_3& q_2\\ q_3& 0& -q_1\\ -q_2& q_1& 0\end{array}\right]}^2=\\ & \left[\begin{array}{lll}1-2\left(q_2^2+q_3^2\right)& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& 1-2\left(q_1^2+q_3^2\right)& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& 1-2\left(q_1^2+q_2^2\right)\end{array}\right]=\\ & \left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\end{array}$$
上式建立了单位四元数与方向余弦阵之间的关系,并且表明了单位四元数三角表示法式 的几何意义。

3. 优缺点

3.1 优点

1. 可以避免万向节锁现象；
2. 只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高；
3. 可以提供平滑插值；

3.2 缺点

1. 比欧拉旋转稍微复杂了一点点，因为多了一个维度；
2. 理解更困难，不直观；

4. 四元数可视化

视频链接
四元数在线变换

八、四元数与欧拉角相互转化

1. 四元数转欧拉角

坐标系与旋转顺序为：NED_ZYX
根据矩阵对应得到 :
$$\left[\begin{array}{c}\text{ Roll }\\ \text{ Pitch }\\ \text{ Yaw }\end{array}\right]=\left[\begin{array}{c}\arctan \frac{C32}{C33}\\ -\arcsin C31\\ \arctan \frac{C21}{C11}\end{array}\right]=\left[\begin{array}{c}\arctan \frac{2\left(q_2{q}_3+q_0{q}_1\right)}{q_0^2-q_1^2-q_2^2+q_3^2}\\ -\arcsin \left(2\left(q_1{q}_3-q_0{q}_2\right)\right)\\ \arctan \frac{2\left(q_1{q}_2+q_0{q}_3\right)}{q_0^2+q_1^2-q_2^2-q_3^2}\end{array}\right]$$
坐标系与旋转顺序为：ENU_ZXY
$$\left[\begin{array}{c}\text{ Roll }\\ \text{ Pitch }\\ \text{ Yaw }\end{array}\right]=\left[\begin{array}{c}\arctan \frac{-C31}{C33}\\ \arcsin C32\\ \arctan \frac{-C12}{C22}\end{array}\right]=\left[\begin{array}{c}\arctan \frac{-2\left(q_1{q}_3-q_0{q}_2\right)}{q_0^2-q_1^2-q_2^2+q_3^2}\\ \arcsin \left(2\left(q_2{q}_3+q_0{q}_1\right)\right)\\ \arctan \frac{-2\left(q_1{q}_2-q_0{q}_3\right)}{q_0^2-q_1^2+q_2^2-q_3^2}\end{array}\right]$$

2. 欧拉角转四元数

坐标系与旋转顺序为：NED_ZYX
$$\begin{array}{c}{q}_x=\left[\cos \frac{\text{ Roll }}{2},\sin \frac{\text{ Roll }}{2},0,0\right]\\ q_y=\left[\cos \frac{\text{ Pitch }}{2},0,\sin \frac{\text{ Pitch }}{2},0\right]\\ q_z=\left[\cos \frac{Yaw}{2},0,0,\sin \frac{Yaw}{2}\right]\\ q=q_z\times q_y\times q_x\end{array}$$
注意 : 这里的乘法不是普通的乘法规则,需要遵守四元数乘法规则,不同旋转顺序得到的结果也不一 样,这一点和矩阵乘法相似,最后得到四元数方程为:
欧拉角转四元数:
$$q=\left[\begin{array}{l}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{l}\cos \left(\frac{\text{ Roll }}{2}\right)\cos \left(\frac{\text{ Pitch }}{2}\right)\cos \left(\frac{\text{ Yaw }}{2}\right)+\sin \left(\frac{\text{ Roll }}{2}\right)\sin \left(\frac{\text{ Pitch }}{2}\right)\sin \left(\frac{\text{ Yaw }}{2}\right)\\ \sin \left(\frac{\text{ Roll }}{2}\right)\cos \left(\frac{\text{ Pitch }}{2}\right)\cos \left(\frac{\text{ Yaw }}{2}\right)-\cos \left(\frac{\text{ Roll }}{2}\right)\sin \left(\frac{\text{ Pitch }}{2}\right)\sin \left(\frac{\text{ Yaw }}{2}\right)\\ \cos \left(\frac{\text{ Roll }}{2}\right)\sin \left(\frac{\text{ Pitch }}{2}\right)\cos \left(\frac{\text{ Yaw }}{2}\right)+\sin \left(\frac{\text{ Roll }}{2}\right)\cos \left(\frac{\text{ Pitch }}{2}\right)\sin \left(\frac{\text{ Yaw }}{2}\right)\\ \cos \left(\frac{\text{ Roll }}{2}\right)\cos \left(\frac{\text{ Pitch }}{2}\right)\sin \left(\frac{\text{ Yaw }}{2}\right)-\sin \left(\frac{\text{ Roll }}{2}\right)\sin \left(\frac{\text{ Pitch }}{2}\right)\cos \left(\frac{\text{ Yaw }}{2}\right)\end{array}\right]$$

坐标系与旋转顺序为：ENU_ZXY
$z-x-y$ 旋转:
$$\begin{array}{r}{q}_x=\left[\cos \frac{\text{ Pitch }}{2},\sin \frac{\text{ Pitch }}{2},0,0\right]\\ q_y=\left[\cos \frac{\text{ Roll }}{2},0,\sin \frac{\text{ Roll }}{2},0\right]\\ q_z=\left[\cos \frac{Yaw}{2},0,0,\sin \frac{Yaw}{2}\right]\\ q=q_z\times q_x\times q_y\end{array}$$
注意这里的乘法不是普通的乘法规则,需遵守四元数乘法规则,不同旋转顺序得到的结果也不 一样,这一点和矩阵乘法相似,最后得到四元数方程为:
$$q=\left[\begin{array}{l}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\left[\begin{array}{l}\cos \left(\frac{\text{ pitch }}{2}\right)\cos \left(\frac{\text{ roll }}{2}\right)\cos \left(\frac{\text{ yaw }}{2}\right)-\sin \left(\frac{\text{ pitch }}{2}\right)\sin \left(\frac{\text{ roll }}{2}\right)\sin \left(\frac{\text{ yaw }}{2}\right)\\ \cos \left(\frac{\text{ roll }}{2}\right)\cos \left(\frac{\text{ yaw }}{2}\right)\sin \left(\frac{\text{ pitch }}{2}\right)-\cos \left(\frac{\text{ pitch }}{2}\right)\sin \left(\frac{\text{ roll }}{2}\right)\sin \left(\frac{\text{ yaw }}{2}\right)\\ \cos \left(\frac{\text{ pitch }}{2}\right)\cos \left(\frac{\text{ yaw }}{2}\right)\sin \left(\frac{\text{ roll }}{2}\right)+\cos \left(\frac{\text{ roll }}{2}\right)\sin \left(\frac{\text{ pitch }}{2}\right)\sin \left(\frac{\text{ yaw }}{2}\right)\\ \cos \left(\frac{\text{ pitch }}{2}\right)\cos \left(\frac{\text{ roll }}{2}\right)\sin \left(\frac{\text{ yaw }}{2}\right)+\cos \left(\frac{\text{ yaw }}{2}\right)\sin \left(\frac{\text{ pitch }}{2}\right)\sin \left(\frac{roll}{2}\right\}\end{array}\right]$$