【详谈四轴飞行器的姿态解算心得——机体坐标系，地面坐标系，四元数，四元数微分方程（运动方程），旋转矩阵，欧拉角，互补滤波】

前言（只说有用的干货，理清解算的思路）

在研究四轴飞行器的时候，一定会遇到姿态解算的问题，所谓的姿态解算，是从陀螺仪，加速度计等传感器中获得角速度，加速度等物理量，通过数学手段获得控制飞行器所需的三个欧拉角——分别是机头的上升与下降（俯仰角），机翼的左右偏转（横滚角），机头飞行的方向（偏航角）。
获得精确的角度是进行机体控制的前提，有关控制在下一篇文章。

1.机体坐标系与地面坐标系

机体坐标系一般情况下定义：

机头的方向为X轴正方向，机翼的右翼为Y轴正方向，机身原点竖直向下为Z轴正方向。

机体坐标系的作用有很多，但笔者认为主要有两条：

1.给陀螺仪，加速度计赋予真实的物理意义，陀螺仪和加速度计都是针对机体而测算的而不是地面。
2.机体坐标系与地面坐标系的偏差就是姿态角，后续的数学推导需要机体坐标系。

地面坐标系一般情况下定义：

正北为X轴正方向，正东为Y轴正方向，垂直与水平地面指向下为Z轴正方向，也就是重力的方向。

地面坐标系的作用主要是：

1.通过与机体坐标系对比，得出姿态角。是核心作用。
2.在更高级的带位置控制的无人机中，把无人机当成一个质点，地面坐标系可以给出位置信息。

机体坐标系与地面坐标系是理解后续内容的基础，要记住坐标系的方向。

2.四元数

为什么要用四元数？ 因为四元数的本质是描述旋转
定义一个三维空间旋转的方式大概有三种，给出个表格特点如下：

特性	四元数	旋转矩阵	欧拉角
变量数量	4	9	3
计算效率	高（适合实时系统）	低（矩阵乘法开销大）	中（简单三角函数）
奇点问题	无	无	有（万向节死锁）
数值稳定性	需归一化	需正交化	误差易累积
物理意义	抽象（需转换理解）	直观（坐标变换）	直观（绕轴旋转）
插值支持	优秀（SLERP）	复杂（需Slerp或分解）	差（非最优路径）
典型应用场景	IMU姿态估计、机器人控制	3D图形学、复杂变换	飞行仪表显示、用户交互

---

根据上面的特性，所以选择四元数。
这里引出四元数的定义：
在二维中，旋转可以用两个变量来表示：
$$q=\cos \theta +\sin \theta \ast i$$
则在三维旋转中，用四个变量来表示（由一个数学家发现，证明很复杂，不建议研究太深，作为工程师拿来用就好了）：
$$q=q1+q2\ast i+q3\ast j+q4\ast k$$
其中
$$\begin{gathered} q1=\cos \frac{\theta }{2} \\ q2=q3=q4=\sin \frac{\theta }{2} \end{gathered}$$
这里的$\theta$是在三维空间旋转的角度，$i,j,k$是单位向量。
划重点！便于后面的理解，这些定义要记住
$q1$是实部，也是标量部分。
$q2,q3,q4$是虚部系数。
$i,j,k$是虚数单位，并满足以下公式：
$$\begin{gathered} i^2=j^2=k^2=ijk=-1 \\ ij=k,jk=i,ki=j \\ ji=-k,kj=-i,ik=-j \end{gathered}$$
总结并很重要：按照$i,j,k$的顺序，顺序相乘为正，逆序相乘为负。

3.四元数微分方程

1. 数学定义
设：

• $\mathbf{q}=[q_0,q_1,q_2,q_3{]}^T$ 为单位四元数（$q_0^2+q_1^2+q_2^2+q_3^2=1$）
• $\mathbf{\omega }=[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ 为刚体的角速度向量（单位：rad/s）

则四元数的微分方程为：
$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathbf{\Omega }$$
其中：

• $\mathbf{\Omega }=\left[\begin{array}{cccc}0& -{\omega }_x& -{\omega }_y& -{\omega }_z\\ {\omega }_x& 0& {\omega }_z& -{\omega }_y\\ {\omega }_y& -{\omega }_z& 0& {\omega }_x\\ {\omega }_z& {\omega }_y& -{\omega }_x& 0\end{array}\right]$ 为角速度对应的斜对称矩阵
• $\otimes$ 表示四元数乘法

展开后得到分量形式：
$$\{\begin{array}{l}{\dot{q}}_0=-\frac{1}{2}({\omega }_x{q}_1+{\omega }_y{q}_2+{\omega }_z{q}_3)\\ {\dot{q}}_1=\frac{1}{2}({\omega }_x{q}_0-{\omega }_y{q}_3+{\omega }_z{q}_2)\\ {\dot{q}}_2=\frac{1}{2}({\omega }_y{q}_0-{\omega }_z{q}_1+{\omega }_x{q}_3)\\ {\dot{q}}_3=\frac{1}{2}({\omega }_z{q}_0-{\omega }_x{q}_2+{\omega }_y{q}_1)\end{array}$$

---

上述推导过程本质是向量$\mathbf{q}$和向量$\mathbf{\omega }$的叉乘运算，这里的$\mathbf{\Omega }$是为了方便运算直接给出的，但根本的运算逻辑是$\mathbf{q}$和$\mathbf{\omega }=[0,{\omega }_x,{\omega }_y,{\omega }_z{]}^T$的叉乘，计算过程很复杂，这里给出计算过程，不想看的可以跳过，直接记结论。


有了四元数，为什么要四元数微分方程？
因为四元数描述的是一个静态旋转量，通俗来讲就是三维向量围绕哪个轴旋转了多少度，告诉了我们此刻这个三维向量的旋转信息，而微分方程描述的是动态的旋转过程，动态的旋转在时间的作用下变成了静态的旋转量。

从物理的角度来讲，四元数的变量是角度，角度的微分是角速度。微分方程的变量是角速度，所以角速度的改变（来自陀螺仪的数据）通过微分方程的计算得到四元数的导数，在进行积分得到更新后的四元数。

4.旋转矩阵

有了四元数和微分方程，我们就能通过陀螺仪提供的角速度信息，代入微分方程计算出更新后的四元数，四元数里包含着旋转后的角度信息，但是如何从四元数里得到我们想要的欧拉角，答案藏在旋转矩阵里。

旋转矩阵的物理意义

旋转矩阵 R 是坐标系变换算子，其物理意义如下：

向量变换公式

${\mathbf{v}}_{world}=\mathbf{R}\cdot {\mathbf{v}}_{body}$

• 列向量：表示世界坐标系各轴在机体坐标系中的投影方向
• 行向量：表示机体坐标系各轴在世界坐标系中的投影方向

从四元数推导旋转矩阵

通过四元数旋转公式：
${\mathbf{v}}^{\prime }=\mathbf{q}\otimes \mathbf{v}\otimes {\mathbf{q}}^{\ast }$

展开后得到旋转矩阵 R 的具体形式：
$$\mathbf{R}=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& 1-2(q_1^2+q_2^2)\end{array}\right]$$

这里针对旋转矩阵 R 的推导过程给出部分手算过程，本质也是向量叉乘计算。

5.欧拉角

有了旋转矩阵，如何得到欧拉角？答案如下：
欧拉角是从旋转矩阵中通过数学反解推导得出的，其核心在于根据旋转顺序分解矩阵元素并应用反三角函数。以下是完整的原理和推导过程：

---

一、基本原理与关键约束

1. 旋转矩阵的数学本质
   旋转矩阵 $R$ 是正交矩阵（$R^{-1}=R^T$），其行列式为1。欧拉角通过三个连续绕轴旋转（如Z-Y-X）合成该矩阵：
   $$R=R_z(\psi )R_y(\theta )R_x(\varphi )$$
   其中 $\psi ,\theta ,\varphi$ 分别为偏航角（yaw）、俯仰角（pitch）、横滚角（roll）。

2. 旋转顺序的重要性
   不同旋转顺序对应不同的矩阵分解形式（如XYZ、ZYX、YXZ）。以最常见的 ZYX顺序（航空航天领域标准）为例：

   • 先绕Z轴旋转 $\psi$ → $R_z(\psi )$
   • 再绕Y轴旋转 $\theta$ → $R_y(\theta )$
   • 最后绕X轴旋转 $\varphi$ → $R_x(\varphi )$。

---

二、ZYX顺序的详细推导
设旋转矩阵为：
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

步骤1：展开合成矩阵
通过矩阵乘法展开 $R=R_z(\psi )R_y(\theta )R_x(\varphi )$，得到元素表达式：
$$\begin{array}{rl}{r}_{11}& =\cos \psi \cos \theta \\ r_{21}& =\sin \psi \cos \theta \\ r_{31}& =-\sin \theta \\ r_{32}& =\cos \theta \sin \varphi \\ r_{33}& =\cos \theta \cos \varphi \end{array}$$

步骤2：反解欧拉角

1. 俯仰角 $\theta$（绕Y轴）
   由 $r_{31}=-\sin \theta$ 直接得：
   $$\theta =\arcsin (-r_{31})$$

2. 偏航角 $\psi$（绕Z轴）
   由 $r_{11}=\cos \psi \cos \theta$ 和 $r_{21}=\sin \psi \cos \theta$ 得：
   $$\psi =\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(r_{21},r_{11})$$
   （$\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2$ 是四象限反正切函数，解决符号歧义）

3. 横滚角 $\varphi$（绕X轴）
   由 $r_{32}=\cos \theta \sin \varphi$ 和 $r_{33}=\cos \theta \cos \varphi$ 得：
   $$\varphi =\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(r_{32},r_{33})$$

---

展开合成矩阵这一步是省略的，本人给出完整计算过程！

这里的思路是绕不同轴的四元数不同，代入四元数到上一步的旋转矩阵里，可以得到单独绕不同轴的旋转矩阵 Rx,Ry,Rz。

数值计算示例（绕Z轴旋转90°）
给定旋转矩阵（ZYX顺序）：
$$R=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$$

1. 计算俯仰角 $\theta$：
   $\theta =\arcsin (-r_{31})=\arcsin (-0)=0^{\circ }$
2. 计算偏航角 $\psi$：
   $\psi =\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(r_{21},r_{11})=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(1,0)=90^{\circ }$
3. 计算横滚角 $\varphi$：
   $\varphi =\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(r_{32},r_{33})=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(0,1)=0^{\circ }$
   结果符合绕Z轴旋转90°的预期。

---

由此，我们通过旋转矩阵解算出了欧拉角。

6.互补滤波

写到此处，有一个疑问相信有一部分人存在，那就是既然我们已经知道了角速度，为什么不用角速度直接积分，就是角度呢。当时的我也存在这个疑问，直到自己亲自动手实验才发现，刚才的想法，理论上是正确的，但现实是骨感的。
陀螺仪存在零点漂移！，而且非常严重，随着时间的增加，这个漂移的角度几分钟就可以偏差几度，甚至十几度，属于是完全不能用的存在。有条件的同学可以实践一下，如下图是本人的实践效果：