「无人机系列④」飞行控制算法入门——从PID到仿真验证
1. 控制系统的物理与数学基础
在研究飞控算法（Flight Control Algorithm）之前，理解姿态描述方式、闭环反馈思想及采样周期的作用至关重要。本章节主要介绍这些基础概念，为后续的建模与 PID 控制奠定直观认知。
1.1 坐标系与姿态描述
无人机的姿态描述通常基于两个参考系：惯性系（Inertial Frame）与机体系（Body Frame）。
- 惯性系：固定在地面或某个空间参考点上，其坐标轴多采用“东北下（NED）”方式定义，即以北（North）、东(East)、下(Down)三个方向作为基准，用来描述无人机相对于地球的运动状态。
- 机体系：固定在无人机机体上，原点通常位于质心，$x_b$ 轴沿机头方向，$y_b$ 轴指向右翼，$z_b$ 轴指向机体下方。
本文默认使用 ZYX（yaw–pitch–roll）的欧拉角表示顺序。实际工程中虽然也会使用欧拉角，但由于其在某些姿态下会出现万向锁，并且在数值积分时稳定性较差，因此飞控系统更常采用四元数来表示姿态。可以简单理解为：机体的角速度决定姿态变化的快慢，而欧拉角的旋转顺序会让各轴之间产生耦合，处理起来不如四元数稳定、可靠。
1.2 开环与闭环控制系统
飞控的核心是控制反馈。系统是否具有反馈，决定了它是开环还是闭环结构。
- 开环系统：控制量仅由输入指令决定，不考虑输出结果。例如，给定电机转速命令而不去监测实际转速。其响应简单但易受扰动。
- 闭环系统：实时测量输出（如姿态角），并与目标值比较，通过反馈误差调整控制量。闭环系统可在外界扰动下保持稳定和精确控制。
flowchart LR
R[输入 / 指令] --> C[控制器 / 指令发生器]
C --> A[执行器]
A --> P[被控对象]
P --> Y[输出]
%% 可选：外部扰动
D[扰动] -. 作用 .-> P
flowchart LR
R[参考输入] --> SUM((Σ))
Y --> S[传感器 / 测量模块]
S -->|反馈| SUM
SUM --> E[误差]
E --> C[控制器]
C --> A[执行器]
A --> P[被控对象]
P --> Y[输出]
%% 可选：外部扰动
D[扰动] -. 作用 .-> P
%% 连接方向说明（虚线注释）
classDef note fill:#f9f9f9,stroke:#bbb,color:#555,font-size:12px
闭环设计的关键在于误差修正。PID 控制器即是一种典型的闭环实现方式，通过比例（P）、积分（I）和微分（D）环节调节响应速度、消除稳态误差、抑制震荡。
1.3 一阶与二阶系统响应特性
为了更直观地理解闭环系统的动态特性，我们通常从低阶线性系统入手进行分析。它们虽然结构简单，却能揭示控制系统最核心的动态规律。
- 一阶系统：可以理解为“只有惯性、没有弹性”的系统
就像拧开水龙头后水流不会立刻达到目标大小，而是平滑地逐渐上升；这类系统的响应曲线呈指数趋近型，没有振荡或过冲。典型实例包括电机转速变化、温度调节等过程。它的主要性能指标是上升时间（反应速度）和稳态误差（最终能否准确到达目标值）。 - 二阶系统：则相当于“既有惯性又有弹性”的系统
类似汽车的悬挂或无人机的姿态调整，当系统受到扰动时，输出往往会出现一次或多次振荡，在超过目标值后再逐渐稳定下来。它能表现出过冲与振荡衰减等动态行为，是姿态角、机械振动等控制问题的常见模型。其关键性能指标包括上升时间、过冲量和稳态误差，用以评估系统在“反应快”和“过渡稳”之间的平衡。
左图为一阶系统响应，右图为二阶系统响应。虚线为连续系统理论曲线，彩色阶梯线为不同采样周期下的离散响应。可以观察到：
- 采样周期越短（如 0.02s），离散响应越平滑，几乎贴合连续曲线；
- 采样周期增大时，响应呈阶梯状，上升时间变长，稳态误差增大；
- 对二阶系统而言，采样过慢还会放大振荡甚至导致不稳定。
从图中可以看出足够高的采样频率是保证控制实时性和系统稳定性的前提。同时，从系统动态特性看，响应速度与过渡平稳性之间存在天然的权衡——控制器增益过高虽可使响应更快，但容易产生过冲；增益过低则响应迟缓但过程平稳。
因此，飞控算法的核心目标，就是在“响应快速”和“稳定平顺”之间取得合理的平衡，使无人机既能灵敏执行姿态调整，又保持平稳可控的飞行特性。
1.4 采样周期与离散控制
在实际飞控系统中，控制算法运行于数字处理器上，无法连续计算，因此需要离散化。系统以固定时间间隔采样传感器数据并计算控制量，这个间隔即为采样周期（Sampling Period）。
- 如果采样周期太长，反馈滞后，控制滞慢，系统可能震荡或不稳定。
- 如果采样周期过短，虽然响应更快，但计算负载增加，容易造成资源浪费。
控制频率 $f_c = 1/T_s$（其中 $T_s$ 为采样周期）通常设定在系统动态频率的 5–10 倍以上，以保证信号重建与反馈及时性。例如，姿态控制回路常在 200–500 Hz 范围运行。
小结
本章从姿态描述出发，逐步介绍了反馈控制的基本概念、系统动态特性与离散采样机制。理解这些内容，有助于在后续章节中更好地把握飞控算法的数学建模逻辑与 PID 调节原理。
2. 四旋翼动力学建模
飞控算法的设计必须以明确的控制对象模型为基础。模型不仅描述无人机的运动规律，更揭示了输入（电机转速）如何影响姿态与位置，是实现姿态稳定与轨迹跟踪控制的前提。本章从单个电机的推力方程出发，推导四旋翼系统的整体动力学关系，定义状态量与输入量，并说明线性化在控制器设计中的意义。
2.1 单电机推力与整机受力分析
四旋翼无人机通过四个电机/桨叶产生升力与力矩实现姿态与位置控制。单个电机推力与转速平方近似成正比：$$T_i = k_T ,\omega_i^2,\quad i=1,2,3,4$$推力方向指向上方、沿机体系 $-z_b$ 方向（NED 约定下 $+z_b$ 向下）。若定义沿 $+z_b$ 的合力为 $F_{z_b}$，则$$F_{z_b} = -\sum_{i=1}^4 T_i$$约定四旋翼“十字型（X）”布局，自上俯视，电机编号与旋向为：1↻、2↺、3↻、4↺，臂长为 $l$。对应滚转/俯仰/偏航力矩为：
$$\tau_\phi = l,(T_2 - T_4),\quad \tau_\theta = l,(T_3 - T_1),\quad\tau_\psi = k_M,(-T_1+T_2-T_3+T_4)$$
其中 $k_M$（亦记作 $k_Q$）为反扭矩系数。
2.2 状态变量与输入定义
为了描述系统的动态过程，需要定义合适的状态量与输入量。以六自由度刚体模型为基础，无人机的状态变量可选为：
| 符号 | 物理意义 | 单位 | 所属类别 | 备注 |
|---|---|---|---|---|
| $x, y, z$ | 无人机在惯性坐标系中的位置 | m | 位姿量（Position） | 描述质心的空间位置 |
| $\dot{x}, \dot{y}, \dot{z}$ | 无人机在惯性系下的线速度 | m/s | 速度量（Velocity） | 对应位置的一阶导数 |
| $\phi$ | 滚转角（Roll） | rad | 姿态角（Attitude） | 绕机体 x 轴的旋转 |
| $\theta$ | 俯仰角（Pitch） | rad | 姿态角（Attitude） | 绕机体 y 轴的旋转 |
| $\psi$ | 偏航角（Yaw） | rad | 姿态角（Attitude） | 绕机体 z 轴的旋转 |
| $p, q, r$ | 角速度（分别绕 x、y、z 轴） | rad/s | 角速度（Angular Rate） | 姿态角的一阶导数 |
| $\mathbf{u} = [\omega_1^2, \omega_2^2, \omega_3^2, \omega_4^2]^T$ | 电机输入（角速度平方） | rad²/s² | 控制输入（Input） | 对应各电机产生的推力 |
| $\mathbf{x} = [x, y, z, \phi, \theta, \psi, \dot{x}, \dot{y}, \dot{z}, p, q, r]^T$ | 状态向量 | — | 系统状态（State） | 综合描述无人机当前运动状态 |
| $\mathbf{y}$ | 输出量（姿态角、位置等） | — | 输出（Output） | 可测量或控制的系统输出 |
2.3 姿态动力学方程推导
无人机的姿态变化由力矩作用引起。根据刚体旋转动力学方程：
$$\mathbf{I} \dot{\boldsymbol{\omega}} = \boldsymbol{\tau} - \boldsymbol{\omega} \times (\mathbf{I}\boldsymbol{\omega})$$
其中 $\mathbf{I}$ 为转动惯量矩阵，$\boldsymbol{\omega} = [p, q, r]^T$ 为角速度向量，$\boldsymbol{\tau} = [\tau_\phi, \tau_\theta, \tau_\psi]^T$ 为控制力矩。在姿态角与角速度的耦合关系中，有：
该非线性方程揭示了角速度与姿态角变化的映射关系，也是飞控算法姿态解算的核心。
2.4 小角度线性化与 Pitch 轴简化模型
小角度近似下（$|\phi|,|\theta|\ll 1$），有 $\sin\theta\approx\theta,\ \cos\theta\approx1,\ \tan\theta\approx\theta$。Pitch 轴动力学关系可简化为：
$$\dot\theta = q,\qquad \dot q = \frac{\tau_\theta}{I_y}\ \Rightarrow\ \ddot\theta=\frac{\tau_\theta}{I_y}$$
若 $$\tau_\theta = l,k_T(\omega_3^2-\omega_1^2)$，则：$\ddot{\theta} = \frac{l,k_T}{I_y},(\omega_3^2-\omega_1^2)$$该式揭示了俯仰角加速度与电机推力差的线性关系。该模型揭示了俯仰角加速度与电机推力差的线性关系，是 PID 姿态控制中最常用的简化表达。
2.5 线性化在控制设计中的意义
无人机的真实动力学高度非线性、耦合性强。但在姿态控制等局部小范围运动中，线性化模型足以反映主要动态特征，这使得控制器设计与分析更加简洁。
以 PID 控制 为例，经过线性化处理后，无人机的姿态通道可以用一阶或二阶线性系统来近似描述。这种简化让控制规律更容易分析：系统的一阶或二阶模型能直观反映出 “调节快一点”会导致什么，“稳一点”又会牺牲什么——例如，比例增益变大时响应更快，但也更容易出现过冲；积分项能消除稳态误差，但响应会变慢；微分项则有助于抑制振荡。
有了线性模型后，还可以使用 根轨迹、波特图 等频域分析方法，观察参数变化对系统稳定性和动态性能的影响，从而有依据地选择 PID 参数。最后，在 Simulink 等仿真平台中，可以将模型搭建成模块化结构，通过阶跃响应、扰动测试等方式快速验证控制律效果，实现“设计–验证–调优”的闭环过程。
因此，线性化不仅是一种数学近似，更是将复杂飞行器控制问题转化为可实现工程解法的关键步骤。
2.6 Pitch 轴简化模型仿真
为了验证线性化后的俯仰轴模型是否能够反映实际动态特性，我们在 Python 环境中搭建了一个简化仿真系统，对比分析开环与闭环（PD 控制）情况下的响应过程。系统输入为电机推力差 $\Delta \omega^2$，经过力矩、角加速度、角速度等环节逐级积分得到俯仰角输出。
图中展示了线性化后的俯仰轴动力学模型在“推力差阶跃输入”下的响应过程。四个子图依次对应信号流向——*力矩 $\tau_\theta$ → 角加速度 $\dot{q}$ → 角速度 $q$ → 角度 $\theta$**。
- 开环（实线）：当电机推力差保持恒定时，系统输出呈抛物线增长，角速度持续积累，无法稳定于目标角度，表现出典型的非稳定特性。
- 闭环 PD 控制（虚线）：引入比例–微分反馈后，系统形成等效二阶环节。响应速度明显提升，能够快速收敛至期望角度，并具有可分析的上升时间、过冲量和稳态误差等指标。
从曲线可以看出，闭环控制有效抑制了发散趋势，使俯仰角在给定阶跃命令后迅速达到并保持稳定值。这验证了线性化模型对系统动态行为的合理刻画，也为后续基于 PID 的姿态控制参数整定提供了直观依据。
小结
本章从电机推力出发，逐步推导出四旋翼的姿态动力学关系，明确了输入、状态与输出之间的映射。通过小角度线性化，系统被简化为可用于控制器设计的线性模型，尤其在 Pitch 轴方向上，模型清晰揭示了推力差对姿态变化的直接作用。这为后续章节的 PID 参数设计与闭环控制仿真奠定了坚实的数学基础。
3. 控制结构设计：从开环到三层控制环
无人机的姿态稳定与位置控制并非由单一控制器完成，而是通过多层级、递进式的控制架构实现的。为了让读者建立整体飞控系统的层次化思维，下面说说常见的三环控制结构——位置环、速度环与姿态环，并分析它们之间的输入输出关系、频率分布与调试顺序。
3.1 三层控制结构原理
飞控系统的控制目标从外到内依次为位置 → 速度 → 姿态，形成典型的多层级闭环结构。
- 外环（位置环）：根据导航或用户输入的目标位置（或高度），计算期望速度。它反映了无人机的“运动趋势”，属于低频控制环节。
- 中环（速度环）：根据期望速度与实际速度的差值，输出期望姿态角（如俯仰角、滚转角）。它负责将线速度指令转换为角度指令，实现加减速与水平运动控制。
- 内环（姿态环）：以期望姿态角为输入，与传感器测得的姿态反馈比较后产生误差，通过 PID 或其他算法调节电机转速，实现高频、快速的姿态调整。
三者之间的层次关系可概括为：$\text{位置环输出} \Rightarrow \text{速度参考} \Rightarrow \text{速度环输出} \Rightarrow \text{姿态参考} \Rightarrow \text{姿态环输出} \Rightarrow \text{电机控制量}$
3.2 信号定义与层间关系
在多环控制结构中，每一层都作为上一层的执行对象、下一层的控制目标。以下为各层主要输入与输出信号定义：
| 控制层 | 输入信号 | 输出信号 | 控制对象 | 典型控制量 |
|---|---|---|---|---|
| 位置环 | 目标位置（x, y, z） | 期望速度（$vₓ₍ref₎, v_y₍ref₎, v_z₍ref₎$） | 无人机质心位置 | PID / 位置控制律 |
| 速度环 | 期望速度、实际速度 | 期望姿态角（$θ₍ref₎, φ₍ref₎$） | 无人机平移速度 | PID / 前馈补偿 |
| 姿态环 | 期望姿态角、角速度反馈 | 电机推力分配量 | 无人机姿态角与角速度 | PID / LQR / 模型补偿 |
可以看出，每层环节的输出信号都为下一级提供参考输入，而最内层的姿态环直接驱动电机 PWM 信号。
3.3 更新频率差异与分层设计思想
不同控制层级的更新频率差异显著，反映了控制对象的动态特性差异。
- 姿态环：响应最快，通常运行频率为 200–500 Hz，用于实时抑制扰动并稳定飞行姿态；
- 速度环：中等频率，约 100 Hz 左右，主要调整加减速及水平运动；
- 位置环：变化最慢，一般 20–50 Hz 即可，处理导航、路径规划等任务。
这种频率分层设计可有效避免计算冗余，同时确保系统稳定性。姿态环在高频下保持快速响应，相当于为外层提供“稳定的执行底座”；外层环节则在低频下平滑规划整体运动。
3.4 闭环信号流的逻辑
在整个控制系统中，闭环反馈机制贯穿始终。
- 外环根据位置误差输出期望速度，使无人机朝目标方向移动；
- 中环根据速度误差产生期望姿态角，以调整机体的倾斜方向和加速度；
- 内环根据姿态误差调节电机推力，实现机体姿态变化；
- 传感器（IMU、GPS、气压计）将姿态、速度、位置实时反馈至各控制层，形成完整的闭环系统。
通过这种自内向外、逐级调节的闭环结构，飞控系统能在多时间尺度上同时保持快速响应与平稳控制。
3.5 模块化设计与调试顺序
在工程实践中，三环控制往往采用模块化架构设计，便于独立调试与参数整定。一般遵循“由内向外”的调试顺序：
- 姿态环调试：确保机体能快速稳定姿态，抑制振荡；
- 速度环调试：在姿态环稳定的基础上，调整速度控制参数，使飞行加减速平滑；
- 位置环调试：最后调节导航控制，使无人机能精准跟踪目标点或轨迹。
这种从内到外的分层调试方式，不仅能提高系统鲁棒性，还可在出现异常时快速定位问题所在（例如姿态环未稳会直接导致外环指令失效）。
小结
三环控制结构是现代飞控系统的核心框架，其分层思想体现了“快环稳内态、慢环控全局”的设计原则。通过清晰定义各层输入输出信号并合理分配更新频率，飞控系统不仅能保证姿态稳定，还能实现平滑、精准的轨迹控制。掌握这种控制层级逻辑，是理解后续 PID 参数整定、外环跟踪算法与耦合控制策略的基础。
4. PID控制算法原理与参数调试
无人机姿态稳定控制的核心是 闭环反馈。在姿态环中，控制器必须持续计算当前姿态与目标姿态之间的误差，并实时调整电机推力，使系统快速响应而不过度振荡。最经典、通用且易于实现的控制器即为 PID 控制器（Proportional–Integral–Derivative）。
下面说说PID算法的作用机理，掌握离散实现方式及调参策略，也为后续的飞行姿态控制提供基础。
4.1 PID控制原理
PID控制器通过三种不同的作用方式对误差信号进行综合调节：
- 比例项（P）——快速响应
比例项直接按误差大小输出控制量，能加快系统响应速度。比例系数 $K_p$ 越大，系统反应越迅速，但过大容易引起振荡与超调。 - 积分项（I）——消除偏差
积分项累积误差随时间的面积，用以消除稳态误差，使系统最终能精确达到目标值。然而积分项过强会造成系统响应迟滞或“积分饱和”，导致过冲。 - 微分项（D）——抑制振荡
微分项反映误差变化的趋势，可在误差变化较快时产生提前的“阻尼作用”，有效抑制超调和振荡。若微分系数 $K_d$ 过大，则会放大噪声，造成输出不稳定。
flowchart LR %% ====== PID 控制结构框图 ====== %% 从左到右布局 R["输入信号 rₜ"] S["系统输出 yₜ"] SUM[["Σ 误差计算 eₜ = rₜ − yₜ"]] P["比例项 Kₚ·eₜ"] I["积分项 Kᵢ∫eₜ dt"] D["微分项 Kd·deₜ/dt"] SUM2[["Σ 控制量叠加"]] CTRL_OUT["执行器输出 uₜ"] PLANT["被控对象 姿态 / 电机系统"] %% 信号流向 R --> SUM S -. 反馈 .-> SUM SUM -->|eₜ| P SUM -->|eₜ| I SUM -->|eₜ| D P --> SUM2 I --> SUM2 D --> SUM2 SUM2 -->|uₜ| CTRL_OUT --> PLANT -->|yₜ| S %% 样式 classDef node fill:#fff,stroke:#666,stroke-width:1.3px,color:#111,font-size:13px; classDef sum fill:#f5f5f5,stroke:#999,stroke-dasharray:3 3,font-size:12px; class R,S,CTRL_OUT,PLANT node; class SUM,SUM2 sum;
4.2 离散PID的计算实现
飞控系统在嵌入式处理器中运行，所有控制量均以采样周期 $T_s$ 为时间步长离散化计算。连续形式的PID控制律为：
$$
u(t) = K_p e(t) + K_i \int e(t)dt + K_d \frac{de(t)}{dt}
$$
离散化后得到差分方程形式：
$$
u[k] = u[k-1] + K_p(e[k]-e[k-1]) + K_i T_s e[k] + \frac{K_d}{T_s}(e[k]-2e[k-1]+e[k-2])
$$
其中：
- $e[k]$ 为当前采样误差；
- $T_s$ 为控制周期（即采样时间）；
- $u[k]$ 为控制器输出（对应电机推力或力矩指令）。
控制周期越短，响应越平滑且系统稳定性越高；周期过长则可能导致采样延迟，使系统振荡甚至失稳。在飞控中，姿态环通常运行于 200–500 Hz，速度环约 100 Hz，位置环 20–50 Hz。
4.3 控制参数对系统响应的影响
PID的三个系数直接决定系统的动态特性：
- 增大 $K_p$：上升时间缩短，但可能产生较大过冲；
- 增大 $K_i$：能消除稳态误差，但易导致响应迟缓；
- 增大 $K_d$：可减小振荡、改善稳定性，但噪声敏感性增加。
图中展示了同一被控对象在不同参数组合下的阶跃响应曲线。蓝线为基准 PID，红线为增大比例增益 $K_p$，绿线为增大积分增益 $K_i$，紫线为增大微分增益 $K_d$。从曲线可观察到：
- 提高 $K_p$ 可显著加快上升速度，但同时引入更大的过冲与振荡；
- 提高 $K_i$ 能有效消除稳态误差，但系统响应变慢，容易出现超调；
- 提高 $K_d$ 则增强阻尼作用，能抑制振荡并改善稳定性，但过大时可能降低响应速度。
综合来看，PID 各项参数在系统动态性能中起到互补作用，实际调参需在 响应速度、稳定性与稳态精度 之间取得合理平衡。
从物理上看，比例项控制当前误差、积分项修正历史误差、微分项预测未来趋势。三者协同作用，使系统既能快速响应，又能稳态准确并抑制振荡。PID参数调整的核心即是：在响应速度、超调量和稳态误差之间寻找最优折中。
4.4 PID调参经验法（Ziegler–Nichols法）
在理论模型不完备的情况下，可采用 Ziegler–Nichols 经验法 进行快速调参：
- 关闭I、D项，仅保留P项；
缓慢增大 $K_p$，直至系统输出产生持续振荡，此时记录临界增益 $K_u$ 和振荡周期 $T_u$。 - 按经验公式计算初值：
| 控制类型 | $K_p$ | $K_i$ | $K_d$ |
|---|---|---|---|
| P | $0.5,K_u$ | — | — |
| PI | $0.45,K_u$ | $1.2,K_p/T_u$ | — |
| PID | $0.60,K_u$ | $2,K_p/T_u$ | $K_p,T_u/8$ |
- 在仿真中微调：
逐步调整 $K_p, K_i, K_d$ 以减少过冲、改善响应时间或消除稳态偏差。
| 参数组合 | $K_p$ | $K_i$ | $K_d$ | 上升时间 (s) | 超调量 (%) | 稳态误差 | 备注 |
|---|---|---|---|---|---|---|---|
| 基准 PID | 1.2 | 0.8 | 0.05 | 1.10 | 8.5 | 0.02 | 响应平稳，轻微过冲 |
| 仅增大 $K_p$ | 2.4 | 0.8 | 0.05 | 0.65 | 22.0 | 0.01 | 上升快但振荡明显 |
| 仅增大 $K_i$ | 1.2 | 1.6 | 0.05 | 1.35 | 15.5 | 0.00 | 消除稳态误差但过冲加大 |
| 仅增大 $K_d$ | 1.2 | 0.8 | 0.20 | 1.25 | 5.0 | 0.02 | 阻尼增强，响应更平稳 |
4.5 PID控制响应对比
图中展示了同一被控对象在不同 PID 参数组合下的动态响应过程。蓝线为较低比例增益（$K_p$）时的响应，系统上升较慢但稳定；橙线为较高 $K_p$ 时，响应速度显著提升但出现明显过冲与振荡；绿色曲线在同样的 $K_p$ 下加入微分项（$K_d$），系统阻尼增强，振荡得到抑制，输出更平稳。
由此可见，$K_p$ 主要影响响应速度与超调程度，而 $K_d$ 在改善系统稳定性、减小振荡方面起关键作用。调节 PID 参数时，需要在响应速度、过冲量与稳态精度之间取得平衡，以保证飞行姿态控制既快速又稳定。
小结
PID控制是无人机姿态稳定的基础算法，具有实现简单、物理含义清晰和鲁棒性强等优点。通过理解 P/I/D 各项的作用机制与采样周期影响，结合 Ziegler–Nichols 等经验调参法，读者可以在仿真或实际飞控系统中独立设计姿态环PID控制器，并根据响应曲线灵活优化控制参数，实现响应快、过冲小、稳态精度高的平衡控制效果。
5. 现代控制思想入门（LQR / MPC）
前面我们详细介绍了PID控制的原理与调参方法。PID控制以其结构简单、实现方便，在无人机姿态与速度控制中占据主导地位。然而，随着系统复杂度的提升（例如多轴耦合、动态约束、外界扰动等），传统PID逐渐显现出局限性。
为了在更复杂的环境下获得更优控制效果，现代控制理论应运而生。其中，LQR（线性二次型调节器，Linear Quadratic Regulator） 和MPC（模型预测控制，Model Predictive Control） 是两种最具代表性的方法。它们的共同目标是：在满足动态方程约束的前提下，使系统性能指标最优。
本章将从PID的局限性出发，理解LQR的最优反馈思想与MPC的预测优化机制，并对比三种控制策略的性能差异。
5.1 PID控制的局限性
虽然PID算法适用于大多数线性、单输入单输出（SISO）的系统，但对于无人机这样的多输入多输出（MIMO）系统，其局限性主要体现在以下几个方面：
- 多变量耦合问题
四旋翼的Roll、Pitch、Yaw三轴之间存在惯性耦合。当某一轴姿态变化时，会引起其他轴的动态响应。PID通常对每个轴独立设计，难以同时处理轴间耦合效应。 - 约束处理能力有限
无人机推力、角速度和姿态角均存在物理限制，而PID无法在计算中直接考虑这些约束。若外部干扰较大或目标变化剧烈，控制输出可能超过可执行范围，导致饱和甚至失稳。 - 对时延与模型变化敏感
PID依赖于误差的即时变化，在存在信号延迟或系统参数变化时容易出现滞后或振荡。
这些问题促使我们引入更系统化的控制方法，使控制器在设计阶段就能考虑耦合、约束和性能最优化。
5.2 LQR：最优状态反馈控制
LQR（Linear Quadratic Regulator）是一种基于数学优化的线性控制方法。与PID不同，LQR并不单独调节某个误差分量，而是通过最小化代价函数的方式，求得系统状态的最优反馈控制律。
设线性系统模型为：$\dot{\mathbf{x}} = A\mathbf{x} + B\mathbf{u}$。其中，$\mathbf{x}$为系统状态向量，$\mathbf{u}$为控制输入。LQR的目标是最小化以下性能指标：$$J = \int_0^{\infty} (\mathbf{x}^T Q \mathbf{x} + \mathbf{u}^T R \mathbf{u}),dt$$其中：
- $Q$：权重矩阵，用于惩罚状态误差（越大，系统越稳但响应越慢）；
- $R$：权重矩阵，用于惩罚控制能量（越大，控制动作越保守）。
求解该优化问题后，可得到最优控制律：$\mathbf{u} = -K\mathbf{x}$。其中增益矩阵 $K = R^{-1}B^TP$，而 P 来自代数黎卡提方程（ARE）：$A^TP + PA - PBR^{-1}B^TP + Q = 0$。通过适当选择 Q 和 R，LQR控制器能够在“稳态精度”和“能量效率”之间取得理想平衡。
5.3 Pitch轴LQR控制示例
以第二章线性化的Pitch轴模型为例：$\ddot{\theta} = \frac{1}{I_y}U_3$，定义状态变量为：$\mathbf{x} = \begin{bmatrix} \theta \ \dot{\theta} \end{bmatrix}, \quad \mathbf{u} = U_3$，则系统的状态空间模型可写为：$$\dot{\mathbf{x}} = \begin{bmatrix} 0 & 1\ 0 & 0 \end{bmatrix}\mathbf{x} + \begin{bmatrix} 0\ \frac{1}{I_y} \end{bmatrix}\mathbf{u}$$选取权重矩阵：
$$Q = \begin{bmatrix} 10 & 0\ 0 & 1 \end{bmatrix}, \quad R = [1]$$利用Python控制库（python-control）或Matlab的lqr()函数可求得最优增益矩阵：$K = [k_1, k_2]$。于是控制律为：$U_3 = -k_1\theta - k_2\dot{\theta}$，与PID不同，LQR的两个增益k_1, k_2 是通过数学优化自动确定的，而不是通过经验调参得到。
仿真结果表明：LQR在响应时间相近的情况下，能显著减少过冲和振荡，并在存在扰动时保持更好的稳定性。
5.4 MPC：基于预测的优化控制
模型预测控制（Model Predictive Control, MPC） 是现代控制中更进一步的思路。与LQR的静态反馈不同，MPC在每个时刻都通过预测未来的系统行为，动态求解一个有限时间的最优控制问题。其核心思想如下：
- 在当前时刻，根据模型预测未来 $N_p$ 步的状态变化；
- 构建代价函数，考虑状态误差和控制量变化；
- 通过优化求解未来控制序列，使代价函数最小；
- 仅执行第一个控制动作，下一时刻重复上述过程。
与LQR相比，MPC有两个显著优势：
- 显式约束处理：可直接在优化中考虑输入和状态限制（例如推力饱和、角度上限）；
- 动态可调性：可根据实时状态重新计算控制量，具备更强的鲁棒性。
5.5 三种控制方法对比
| 特性 | PID | LQR | MPC |
|---|---|---|---|
| 理论基础 | 经验法则 | 线性系统优化 | 模型预测优化 |
| 参数来源 | 人工调试 | 权重矩阵优化 | 在线优化求解 |
| 系统类型 | 单输入单输出 | 多变量线性系统 | 多变量非线性系统 |
| 约束处理 | 无 | 间接考虑 | 可直接处理 |
| 实现复杂度 | 低 | 中 | 高 |
| 实时性需求 | 低 | 中 | 高 |
| 典型应用 | 姿态环 | 姿态与速度控制 | 路径跟踪与高动态任务 |
如上表所示，PID依然是多数飞控系统的核心算法，但在高性能应用（如编队控制、路径跟踪或自主避障）中，LQR与MPC展现出更高的控制品质。
小结
本章介绍了现代控制理论中两种重要方法——LQR和MPC。LQR通过优化代价函数实现全局最优状态反馈，而MPC则利用模型预测与在线优化实现动态最优控制。
相较于PID，这两种方法提供了更系统、更智能的控制框架，尤其在多变量、受约束或高动态任务场景中表现出明显优势。
下面我们将以这些模型为基础，在仿真平台中验证不同控制策略在实际无人机系统中的性能表现，为进一步的实机测试打下基础。
6. 仿真验证与系统实现
飞控算法的最终目标，是让控制律不仅在数学模型中成立，更能在真实系统中稳定、准确地运行。因此，在算法设计完成后，需要通过仿真验证系统的动态性能、鲁棒性与实时性。本章将以姿态控制为例，介绍如何在 Simulink 与 Python 控制库 中进行仿真建模、噪声注入、延迟模拟及参数移植，为实际飞控系统实现提供依据。
6.1 仿真目的与意义
控制算法通常先在理论层面进行建模与验证，但真实飞行中，系统不可避免地存在传感器噪声、计算延迟、执行器饱和等非理想因素。仿真验证的目的在于：
- 检查控制律在这些非理想条件下是否仍能稳定运行；
- 评估控制周期（采样时间）对系统性能的影响；
- 为参数从仿真环境到 STM32 等嵌入式平台的移植提供数据参考。
通过仿真，可在不损坏真实硬件的前提下快速调整参数，使控制器在实机部署前已具备良好的动态特性。
6.2 Simulink 仿真模型构建
在 Simulink 环境中，控制系统可用模块化方式表示，如图 所示。
flowchart LR classDef blk fill:#ffffff,stroke:#666,stroke-width:1.3px,color:#111,font-size:13px; classDef soft fill:#f7f7f7,stroke:#999,stroke-width:1px,color:#333,font-size:12px; linkStyle default stroke:#444,stroke-width:1.3px; R["目标姿态角
θ_ref"]:::blk SUM["SUM
e = θ_ref − θ"]:::soft CTRL["控制器
PID / LQR"]:::blk SAT["执行器限幅
Saturation"]:::soft PLANT["被控对象
Pitch 轴模型"]:::blk SENSOR["传感器反馈
姿态/角速度"]:::blk NOISE["测量噪声
Band-Limited"]:::soft DELAY["系统延迟
Transport Delay"]:::soft R --> SUM --> CTRL --> SAT --> PLANT --> SENSOR SENSOR --> NOISE --> DELAY --> SUM
图中信号流从目标角度（参考输入）开始，经控制器计算输出控制量，再由执行器模型（含推力限幅）驱动姿态动力学模块。
反馈路径由传感器测量信号构成，可在其中插入以下模块以增强仿真真实性：
- Band-Limited White Noise：模拟 IMU 测量噪声；
- Transport Delay：模拟 MCU 计算与滤波延迟；
- Saturation：限制推力输出，避免非物理过驱。
这种“信号闭环 + 噪声扰动”结构，是飞控算法验证中最核心的模型形式。
6.3 实验步骤
为验证控制系统的稳定性与实时性，可按以下流程实施实验：
- 搭建基础闭环
- 新建 Simulink 模型；
- 用 Sum 模块计算误差 $e = θ_{ref} - θ$；
- 插入 PID Controller 模块或自定义 LQR 控制器；
- 通过 Transfer Function 模块实现 Pitch 轴动力学方程；
- 构建完整闭环结构。
- 引入噪声与延迟
- 在反馈路径中加入 Band-Limited White Noise，方差设为 $10^{-3}$～$10^{-4}$；
- 使用 Transport Delay 模拟计算与通讯延迟（约 2–5 ms）；
- 在控制输出端加入 Saturation 模块，推力限幅 ±1。
- 运行仿真与观测响应
- 设置仿真步长 $T_s = 0.002$ s（500 Hz 控制环）；
- 运行 5 s 仿真，记录姿态角响应曲线；
- 对比无噪声/无延迟与加入扰动条件下的响应差异。
- 调节参数与稳定性分析
- 调整 $K_p$、$K_i$、$K_d$，观察上升时间、过冲与稳态误差变化；
- 增加延迟与噪声强度，评估系统裕量；
- 对比 PID 与 LQR 控制性能。
6.4 Python 仿真实现
在 Python 中同样可基于 control 库进行系统级仿真，以验证控制律在延迟与扰动下的性能表现。以下代码展示了使用 Pitch 轴简化模型进行 PID 与 LQR 的对比实验：
1 | import numpy as np |
| 参数类型 | 仿真值 | 实机对应值 | 说明 |
|---|---|---|---|
| 控制周期 | 0.002 s | 500 Hz 姿态环 | Simulink 步长需与 MCU 控制周期一致 |
| 执行延迟 | 3 ms | 运算+通讯延迟 | 用 Transport Delay 模拟 |
| 噪声方差 | $10^{-3}$ | IMU 角速度噪声 | 通过 Band-Limited Noise 模块注入 |
| 推力限幅 | ±1 | 电机 PWM 饱和 | 对应执行器最大输出 |
在移植过程中，可直接使用仿真得到的 PID 参数初值，再通过实机试飞微调。若系统存在明显延迟或噪声，可适当减小 $K_p$、$K_i$，或采用前馈滤波、抗饱和补偿机制。
6.6 实验总结
通过上述仿真与实现验证，我们可以得出以下结论：
- 仿真平台能有效评估算法在非理想环境下的稳定性；
- 控制周期、延迟与噪声是决定控制系统性能的关键因素；
- 从仿真到实机的参数映射需考虑实时性与执行精度；
- 经验证的仿真模型可直接用于飞控系统代码生成与硬件在环（HIL）测试。
7. 工程反思与拓展方向
经过前几章的学习，我们已经完成了从控制理论、动力学建模、PID与LQR设计，到仿真验证与系统实现的完整流程。然而，在实际无人机飞行中，算法往往并不会如仿真中那样“理想”地运行。真实世界的飞控系统受到多种工程因素的影响——传感器噪声、控制延迟、执行器非线性、结构振动以及环境扰动，这些都可能导致系统性能下降甚至失稳。
本章从工程角度重新审视控制算法，理解理论模型与实机应用之间的差距，并探讨如何通过滤波、鲁棒与自适应控制等方法提升系统稳定性。这种“从实验到工程”的反思能力，是将控制理论真正落地的关键。
7.1 控制延迟与实时性问题
在飞控系统中，所有数据采集与控制输出都存在不可避免的延迟。例如：
- IMU采样存在约 1–2 ms 的延迟；
- 姿态解算与控制计算约 0.5–1 ms；
- 电调（ESC）响应信号再需 0.5–2 ms。
这些延迟叠加后，会在控制闭环中产生相位滞后，使系统对误差的响应滞后于实际变化，导致振荡或超调。常见应对策略包括：
- 提高控制频率：如姿态环500 Hz以上，以减小相对滞后；
- 优化任务调度：在RTOS中为姿态任务设置最高优先级；
- 前馈补偿或预测控制：在算法中提前补偿已知延迟；
- 算法与硬件协同设计：选用支持浮点运算和高速DMA的MCU。
7.2 IMU噪声与滤波耦合
IMU是姿态控制的核心传感器，但它的输出存在随机噪声与零偏漂移。加速度计噪声影响姿态修正的平滑性；陀螺仪漂移会在积分过程中累计误差，导致角度偏移。常用的滤波算法包括：
- 互补滤波（Complementary Filter）：融合陀螺仪与加速度计数据，兼顾短期稳定与长期准确；
- 卡尔曼滤波（Kalman Filter）：通过状态估计与协方差更新实现动态最优融合；
- 低通滤波（LPF）：抑制高频噪声，但若设置过低，会引入信号滞后。
滤波虽然能提高测量精度，但也会引起动态延迟，与控制环路相互耦合。如果滤波器的截止频率过低，控制器获得的姿态信息将滞后，从而影响快速响应。因此，滤波与控制应综合设计，在抗噪声与实时性之间取得平衡。
7.3 执行器非线性与结构振动
无人机的执行机构（电机与螺旋桨）并非理想线性器件，电机推力与PWM信号之间的关系往往呈非线性曲线，在高转速下，气流干扰会导致推力响应滞后，螺旋桨与机臂振动会在IMU中产生虚假加速度信号。这种“非理想性”会破坏控制器假设的线性模型，造成参数漂移和控制误差。常见改进措施包括：
- 推力曲线标定：通过实验获取PWM–推力曲线，并在控制算法中进行线性化补偿；
- 机械减振设计：在IMU安装座使用硅胶减震结构，降低高频振动传递；
- 增加前馈项：在控制律中引入预测补偿，减轻执行滞后影响。
7.4 外部扰动与鲁棒性分析
在户外飞行环境中，风扰动是影响稳定性的主要外部因素。风速变化会导致姿态角度偏移或高度波动，尤其是在悬停状态下更为显著。为了增强系统的抗扰性，可采用以下手段：
- 增稳控制（Stabilizing Control）：提高姿态环增益，提升刚性响应；
- 鲁棒控制（Robust Control）：在设计阶段考虑模型不确定性，使系统对参数变化不敏感；
- 自适应控制（Adaptive Control）：在线估计并更新系统参数，自动适应环境变化；
- 扰动观测器（Disturbance Observer, DOB）：实时估计外部扰动并补偿控制输入。
这些方法在理论上能显著改善系统性能，但实现复杂度与计算负担较高，需结合硬件算力与应用需求选择性使用。
7.5 常见误差来源与改进策略
下表总结了飞控系统中常见的误差来源、对控制性能的影响及对应改进措施。
| 误差来源 | 典型表现 | 控制影响 | 改进策略 |
|---|---|---|---|
| 控制延迟 | 响应滞后、振荡 | 相位裕度降低 | 提高控制频率，优化任务调度 |
| IMU噪声 | 姿态抖动 | 信号不稳定 | 使用互补或卡尔曼滤波 |
| 电机非线性 | 推力不均、响应慢 | 控制量失真 | 标定PWM–推力曲线 |
| 振动干扰 | 姿态漂移、误触发 | 信号失真 | 加装减振结构、软件滤波 |
| 风扰动 | 悬停不稳 | 系统偏离目标 | 增稳或自适应控制 |
| 参数漂移 | 长期误差积累 | 性能退化 | 定期重标定，在线自适应调参 |
通过系统地分析和优化这些环节，可以显著提升飞控系统在真实环境中的稳定性和可靠性。
7.6 拓展与未来方向
随着飞控硬件性能的提升和算法研究的深入，现代无人机控制正逐渐向更智能、更鲁棒的方向发展。主要趋势包括：
- 自适应与学习型控制：通过在线学习与参数估计，实现算法的动态自我优化；
- 模型预测控制（MPC）在多机协同中的应用：实现路径规划与碰撞规避的统一优化；
- 融合视觉与惯性导航（VIO）：结合图像信息提升姿态估计精度；
- 嵌入式鲁棒滤波与容错控制：提升系统在极端条件下的生存能力。
这些方向体现了飞控技术从“控制系统”向“智能系统”的演进。未来的飞控算法不再仅仅是维持稳定，而是具备自感知、自修正与协同决策的能力。