本文是 Stevens《Aircraft Control and Simulation》读书笔记系列的第六篇（完结篇），覆盖教材第6-7章和附录中的数字实现部分：从连续域到离散域、数字PID、采样与零阶保持、实时仿真架构。本系列到此画上句号。

第六篇：数字实现与HITL仿真——从理论到代码

一、概念引入——飞控芯片上运行的是离散时间方程

1.1 问题

前五篇中我们用的都是连续时域的微分方程和传递函数 x˙=Ax+Bu\dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u}、Gc(s)G_c(s)。

但真实飞控在STM32上运行的是离散时间的差分方程——每隔固定的时间间隔（1ms、4ms）读取传感器、执行控制律、输出PWM。

从连续到离散的桥梁就是本讲的核心内容。

1.2 历史的5分钟

时间 人物 贡献
1947 香农 采样定理——解决了”模拟信号以多快频率采样才能无损重建”的问题
1950s Z变换 离散时间控制的数学基础（拉普拉斯变换的离散对应）
1970s 微处理器 飞控从模拟向数字的全面过渡
1980s HITL仿真 硬件在环——在真实飞控上运行，但接虚拟传感器

二、采样与Z变换

2.1 采样定理

香农采样定理：采样频率必须大于信号最高频率的两倍。

fs>2fmax⁡ f_s > 2 f_{\max}

在飞控中：

  • 陀螺仪带宽约 50-100 Hz → 采样率 > 200 Hz（Δt<5 ms\Delta t < 5\ \text{ms}）
  • PX4主循环通常运行在 250-500 Hz（Δt:2−4 ms\Delta t: 2-4\ \text{ms}）

2.2 Z变换

Z变换是离散时间版本的拉普拉斯变换：

F(z)=∑n=0∞f[n]z−n F(z) = \sum_{n=0}^\infty f[n] z^{-n}

其中 z=esTz = e^{sT}。

2.3 常见数字积分方法

方法 s域到z域的映射 传递函数替换 飞控中的用途
前向欧拉 s→(z−1)/Ts \to (z-1)/T 简单但可能不稳定 —
后向欧拉 s→(z−1)/(Tz)s \to (z-1)/(Tz) 总是稳定 积分器实现
双线性变换（Tustin） s→(2/T)(z−1)/(z+1)s \to (2/T)(z-1)/(z+1) 精度最高 滤波器设计

PX4中积分离散化的实现方式（以速率环的I项为例）：

1
2
3
4
// 后向欧拉法
integral += error * dt;  // 累加
output = Kp * error + Ki * integral + Kd * (error - last_error) / dt;
last_error = error;

三、数字PID的实现

3.1 位置式PID

直接按照PID公式离散化：

u[n]=Kpe[n]+KiT∑k=0ne[k]+KdT(e[n]−e[n−1]) u[n] = K_p e[n] + K_i T \sum_{k=0}^n e[k] + \frac{K_d}{T}(e[n] - e[n-1])

3.2 增量式PID

只计算PID输出的变化量（不累加积分项）：

Δu[n]=Kp(e[n]−e[n−1])+KiTe[n]+KdT(e[n]−2e[n−1]+e[n−2]) \Delta u[n] = K_p(e[n]-e[n-1]) + K_i T e[n] + \frac{K_d}{T}(e[n]-2e[n-1]+e[n-2]) u[n]=u[n−1]+Δu[n] u[n] = u[n-1] + \Delta u[n]

优点：输出限幅后，积分器不会饱和——抗积分饱和（anti-windup）自然实现。PX4中多采用这种形式。

3.3 抗积分饱和（Anti-Windup）

当PID输出达到物理极限（如电机PWM的100%）时，误差仍然存在，积分项会继续增长——这叫做积分饱和。

积分饱和导致的问题：当误差变号后，控制器需要先”退掉”积分的增长才能开始反向调节——产生很大的延迟和超调。

抗饱和方法：

  1. 积分钳制：当输出饱和时，停止积分累加
  2. 反向计算（back-calculation）：饱和时用 (输出−限幅值)/Ki(\text{输出} - \text{限幅值})/K_i 修正积分项
  3. 增量式PID（如上）：自然避免了积分饱和

四、线性化状态空间的离散化

对于离散时间控制器的设计，我们不能直接 u[n]=−Kx[n]\mathbf{u}[n] = -\mathbf{K}\mathbf{x}[n]——因为系统在采样间隔内演化。

离散化后的状态空间方程：

x[n+1]=Adx[n]+Bdu[n] \mathbf{x}[n+1] = \mathbf{A}_d \mathbf{x}[n] + \mathbf{B}_d \mathbf{u}[n]

其中：

Ad=eAT≈I+AT+12A2T2+⋯ \mathbf{A}_d = e^{\mathbf{A}T} \approx \mathbf{I} + \mathbf{A}T + \frac{1}{2}\mathbf{A}^2 T^2 + \cdots Bd=∫0TeAτ dτ⋅B \mathbf{B}_d = \int_0^T e^{\mathbf{A}\tau}\,d\tau \cdot \mathbf{B}

实际工程中，当 T≪min⁡(1/∣λi∣)T \ll \min(1/|\lambda_i|)（采样周期远小于最快模态时间常数）时，可以使用最简单的一阶近似：

Ad≈I+AT,Bd≈BT \mathbf{A}_d \approx \mathbf{I} + \mathbf{A}T, \quad \mathbf{B}_d \approx \mathbf{B}T

五、HITL仿真架构

5.1 硬件在环的三个层级

层级 真实硬件 仿真部分 每个真实时间步 Stevens章节
SIL 无 飞控代码+模型 仿真跑飞控和模型 —
HITL 飞控硬件 动力学和传感器模型 飞控按真实周期运行 第7章
PIL 飞控+传感器 环境、物理引擎 部分传感器实际激励 —

5.2 PX4 HITL 中的数据流

1
2
3
4
5
6
7
8
9
10
11
12
13
仿真环境 (Gazebo/JSBSim)                    飞控硬件 (Pixhawk/STM32)
    │                                              │
    ├── 动力学计算: ẋ=f(x,u)                       │
    ├── IMU仿真: p,q,r + 噪声                      │
    ├── GPS仿真: PN,PE,PD + 噪声                   │
    ├── 气压计仿真: h + 噪声                       │
    │                                              │
    └──→ MAVLink (UDP) ──→ ──→ UART/USB ──→ 传感器数据输入
                                                      │
                                                      ├── 状态估计 (EKF)
                                                      ├── 控制律 (PID)
                                                      └── PWM输出
    飞控PWM输出 ←── MAVLink ←──  ←── 飞控PWM通道输出

5.3 实时约束

HITL 的核心要求：仿真必须在一个采样周期内完成状态更新。

对于 PX4 HITL（主频 250-500 Hz）：

  • 采样周期 = 2-4 ms
  • 仿真必须在 2 ms 内完成：接收传感器 → 状态估计 → PID计算 → 生成PWM → 发送到仿真
  • 仿真的物理模型（6-DOF）必须在 1-2 ms 内完成一次积分

六、完整概念地图

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
连续域理论                          离散域实现
    │                                  │
    ├── 6-DOF ODE: ẋ=f(x,u)             ├── ODE离散化: x[n+1]=f(x[n],u[n])
    ├── PID: Gc(s)=Kp+Ki/s+Kd s         ├── 增量式PID: Δu[n]=...
    ├── 状态空间: ẋ=Ax+Bu               ├── Ad=e^(AT), Bd=∫e^(Aτ)dτ·B
    ├── 传递函数: G(s)=Y/U              ├── Z变换: G(z)
    └── 拉普拉斯变换                     └── 差分方程
              │
              ↓
        HITL实时仿真
    ┌──────────────────────────────┐
    │  真实飞控 + 仿真环境          │
    │  （传感器仿真→MAVLink→飞控）   │
    │  （飞控PWM→MAVLink→仿真）     │
    └──────────────────────────────┘

Stevens读书笔记系列（共6篇）全篇完结

七、核心公式速查卡

公式 含义
fs>2fmax⁡f_s > 2f_{\max} 采样定理（香农）
Ad=eAT\mathbf{A}_d = e^{\mathbf{A}T} 状态空间离散化
Bd=∫0TeAτdτ B\mathbf{B}_d = \int_0^T e^{\mathbf{A}\tau}d\tau \, \mathbf{B} 输入矩阵离散化
Δu[n]=KpΔe[n]+KiTe[n]+Kd/TΔ2e[n]\Delta u[n] = K_p\Delta e[n] + K_i T e[n] + K_d/T \Delta^2 e[n] 增量式PID
u[n]=u[n−1]+Δu[n]u[n] = u[n-1] + \Delta u[n] 增量式PID输出

参考文献

  1. Stevens, B. L., Lewis, F. L., & Johnson, E. N. (2015). Aircraft Control and Simulation (3rd ed., Chapter 6-7). Wiley.
  2. Åström, K. J., & Wittenmark, B. (1997). Computer-Controlled Systems (3rd ed.). Prentice Hall.
  3. Oppenheim, A. V., & Schafer, R. W. (2009). Discrete-Time Signal Processing (3rd ed.). Pearson.
  4. Franklin, G. F., Powell, J. D., & Workman, M. L. (1997). Digital Control of Dynamic Systems (3rd ed.). Addison-Wesley.

Stevens《飞行器控制与仿真》读书笔记系列（共6篇）全篇完结

从6-DOF建模到模态分析、PID控制、状态空间设计、制导律，最终落地到数字实现与HITL仿真。感谢阅读。