四旋翼飞行器的姿态控制离不开对空间方向的精确描述。本章将介绍飞行控制中常用的坐标系定义、欧拉角、旋转矩阵以及四元数等姿态表示方法，并讨论如何从传感器数据中计算姿态角。 飞行器上的加速度计、陀螺仪等传感器固定安装在机体上，因此传感器输出的数据通常是在机体坐标系中表示的；而飞行任务、目标位置、飞行方向以及重力方向通常是相对于地面描述的，因此需要建立导航坐标系。 由于同一个物理向量在不同坐标系中的分量不同，所以在进行姿态解算、重力补偿和导航计算时，必须先把相关物理量转换到同一个坐标系中，才能进行比较、相加或相减

12.1 导航坐标系与机体坐标系

在飞行控制中，我们需要两个基本坐标系来描述飞行器的运动状态：导航坐标系（NED坐标系）和机体坐标系（Body坐标系）。 飞行器的传感器安装在机体上，而飞行任务、重力方向和空间位置通常相对于地面描述，所以必须建立不同坐标系，并把同一个物理量转换到同一坐标系后才能比较和计算

12.1.1 导航坐标系（NED坐标系）

NED坐标系是固定在地面上的参考坐标系，也称地球坐标系或惯性坐标系。其定义如下：

X轴：指向正北方向（North）

Y轴：指向正东方向（East）

Z轴：指向正下方（Down），构成右手坐标系

该坐标系不随飞行器姿态变化而改变，是描述飞行器位置和速度的基准参考系。在本项目中，由于飞行器为室内短航时四旋翼，暂不涉及GPS导航，因此NED坐标系主要用于定义水平面和重力方向等参考基准。

12.1.2 机体坐标系（Body坐标系）

机体坐标系固定在飞行器本体上，随飞行器姿态变化而变化。对于X型四旋翼，定义如下：

X轴：指向飞行器前方（机头方向），即两前电机连线的中点方向

Y轴：指向飞行器右侧，与X轴垂直

Z轴：指向飞行器下方，与X、Y轴构成右手坐标系

机载IMU传感器（如MPU-6050）所测量的加速度和角速度均是在机体坐标系下表达的。姿态解算的核心任务之一，就是建立机体坐标系与导航坐标系之间的转换关系。

ℹ️**** 注意 在本项目的X型四旋翼布局中，电机1（前右）和电机2（前左）位于X轴正方向一侧，电机3（后右）和电机4（后左）位于X轴负方向一侧。机臂与X轴夹角约为45度。

12.2 欧拉角

欧拉角是最直观的姿态表示方法，用三个角度来描述刚体在三维空间中的旋转。在飞行器领域，这三个角度分别是：

滚转角（Roll，φ）：

绕机体X轴的旋转角度。正值表示右侧机翼下倾（向右翻滚），负值表示向左翻滚。

俯仰角（Pitch，θ）：

绕机体Y轴的旋转角度。正值表示机头向上抬起，负值表示机头向下俯冲。

偏航角（Yaw，ψ）：

绕机体Z轴的旋转角度。正值表示从上方看顺时针旋转（机头向右偏转），负值表示逆时针旋转。

12.2.1 旋转顺序 ZYX

欧拉角的旋转顺序有多种约定。在航空航天领域，通常采用ZYX旋转顺序（又称3-2-1欧拉角），即：

1. 首先绕Z轴旋转偏航角ψ

2. 然后绕新的Y轴旋转俯仰角θ

3. 最后绕最新的X轴旋转滚转角φ

这种旋转顺序的好处是：在飞行姿态角不大的情况下（|φ|<90度, |θ|<90度），各角度之间有较好的物理直观性和独立性。

12.2.2 万向节锁问题（Gimbal Lock）

欧拉角表示法存在一个固有缺陷——万向节锁。当俯仰角θ = ±90度时，滚转轴和偏航轴会重合，导致自由度减少一个，此时无法唯一确定滚转角和偏航角。

数学上表现为：当θ=90度时，旋转矩阵中只有(ψ-φ)的和是可确定的，ψ和φ各自的值无法唯一求解。

对于四旋翼飞行器而言，正常飞行时俯仰角很少超过±60度，因此万向节锁在实际应用中影响不大。但在大机动动作或翻滚特技中，需要特别注意这一问题。这也是引入四元数表示法的重要原因之一。

https://www.bilibili.com/video/BV1Nr4y1j7kn/?spm_id_from=333.1007.top_right_bar_window_history.content.click&vd_source=46a435879dbb2aa5171934787d22c71f

12.3 旋转矩阵

旋转矩阵是另一种描述三维空间旋转的数学工具。它是一个3x3的正交矩阵，满足R^T*R = I且det® = 1。

12.3.1 基本旋转矩阵

绕各坐标轴的基本旋转矩阵如下：

绕X轴旋转φ角：

Rx(φ) = [1, 0, 0; 0, cos(φ), -sin(φ); 0, sin(φ), cos(φ)]

绕Y轴旋转θ角：

Ry(θ) = [cos(θ), 0, sin(θ); 0, 1, 0; -sin(θ), 0, cos(θ)]

绕Z轴旋转ψ角：

Rz(ψ) = [cos(ψ), -sin(ψ), 0; sin(ψ), cos(ψ), 0; 0, 0, 1]

12.3.2 组合旋转矩阵

按照ZYX顺序，从机体坐标系到导航坐标系的完整旋转矩阵为：

R(φ,θ,ψ) = Rz(ψ) * Ry(θ) * Rx(φ)

展开后得到：

R = [ cos(θ)cos(ψ), sin(φ)sin(θ)cos(ψ)-cos(φ)sin(ψ), cos(φ)sin(θ)cos(ψ)+sin(φ)sin(ψ) ]

[ cos(θ)sin(ψ), sin(φ)sin(θ)sin(ψ)+cos(φ)cos(ψ), cos(φ)sin(θ)sin(ψ)-sin(φ)cos(ψ) ]

[ -sin(θ), sin(φ)cos(θ), cos(φ)cos(θ) ]

利用该矩阵，可以将机体坐标系下的向量转换到导航坐标系：v_ned = R * v_body。

旋转矩阵的优点是几何意义清晰，矩阵运算成熟。缺点是需要9个元素来描述3个自由度的旋转，存在冗余；且在迭代更新时容易偏离正交性，需要周期性的正交化处理。

12.4 四元数简介

四元数（Quaternion）是1843年由爱尔兰数学家哈密顿发明的超复数系统，在三维旋转描述中有着广泛应用。现代飞行控制器的姿态解算大多采用四元数作为核心表示方法。

12.4.1 四元数定义

一个四元数定义为：

q = w + xi + yj + z*k

其中w为实部（标量部分），(x, y, z)为虚部（向量部分），i、j、k为虚数单位，满足：

i^2 = j^2 = k^2 = ijk = -1

在程序实现中，四元数通常存储为一个四维数组 q = [q0, q1, q2, q3] = [w, x, y, z]。

12.4.2 四元数与旋转的关系

描述绕单位轴 n = (nx, ny, nz) 旋转α角度的四元数为：

q = [cos(α/2), nxsin(α/2), nysin(α/2), nz*sin(α/2)]

四元数表示旋转相比欧拉角有以下优势：

（1）无万向节锁问题

（2）只需4个元素（旋转矩阵需要9个）

（3）组合旋转通过四元数乘法实现，计算效率高

（4）容易进行归一化处理，保持数值稳定性

（5）适合插值运算（如SLERP球面线性插值）

利用四元数求出来的方向余弦矩阵，进而反解出欧拉角

12.4.3

求四元数的四个系数需要构建关于时间的微分方程并使用，一阶龙格库塔法求解这一微分方程

技巧 在嵌入式系统中，为减少除法运算开销，可用快速逆平方根算法（如Quake III中的0x5f3759df方法）来计算归一化系数。

12.5 互补滤波

由于角度是由角速度积分得到，而积分时，若从陀螺仪获得的角速度信息存有小的偏差，经过积分之后，就会使误差加大，从而使获得的角度存在偏差，所以我们需要用加速度计获得的角度信息去矫正陀螺仪获得的姿态信息，从而使算出来的角度误差消除。这就是互补滤波。

1.获取加速度计的值(为物体坐标系下对应的值)，对其归一化(归一化的原因是因为姿态变化矩阵中的四元数是规范四元数，利用陀螺仪去更新的四元数也要归一化，所以加速度计获得的值也需归一化才能是两者对应)。记从加速度计获得的值为ax、ay、az(分别对应x、y、z轴的值)，其归一化方法如下：

0. 获取陀螺仪算出的姿态矩阵中的重力分量(因为加速度计是测得的物体坐标系下的值，所以我们也要提取利用角速度算出的姿态矩阵中的物体坐标系下的重力分量)。重力分量记为Vx、Vy、Vz，具体计算方法如下：

通过四元数计算的从E系(地理坐标系)变换到b系(物体坐标系)姿态矩阵为：

4.对误差进行积分，从而消除误差，设accex、accey、 accez为x、y、z三轴对应的误差积分结果(对两个重力分量叉乘后的误差进行积分，结果得到角速度值)，ki为积分系数，dt为积分周期时间具体如下：

5.互补滤波，将误差输入Pid控制器与本次姿态更新中陀螺仪测得的角速度相加，得到一个修正的角速度值，获得的修正的角速度值去更新四元素，从而获得准确的姿态角信息。设gx、gy、gz为陀螺仪测得的三个轴的角速度及滤波后的角速度修正值，Kp为互补滤波系数，则修正角速度计算方法如下：

🧪**** 实验 将MPU-6050水平放置，通过串口观察加速度计计算的roll和pitch值。轻轻倾斜板子，观察数值变化是否与倾斜方向和角度一致。记录水平静止状态下各角度的噪声幅度。

---

学习资源

🎬 B站系列视频教程：手把手教你从零组装无人机

📦 百度网盘资料包（SDK、教材PDF、完整源码）：点击下载 提取码: JZS8

---

本系列持续更新，欢迎收藏关注。如有问题欢迎在评论区交流。