3、系统建模基础：状态空间模型与传递函数，如何为四旋翼建立预测模型
各位同学，欢迎来到第三章。前面我们聊了PID的局限，也画了MPC的大饼。但有个核心问题绕不开——你得先有个“模型”。
说白了，MPC不是拍脑袋控制，它是“算”出来的。算之前，你得告诉它：你的四旋翼长什么样，飞起来会怎么动。这就是系统建模。
我个人习惯把建模分成两步：先搞懂物理规律，再写成数学形式。今天我们就从最基础的传递函数，一路干到状态空间模型，最后告诉你——怎么给四旋翼搭一个能用的预测模型。
3.1 传递函数 vs 状态空间：到底选哪个？
很多初学者会纠结这个问题。我当年也纠结过。其实没那么玄乎，我一句话给你讲明白：
- 传递函数：适合单输入单输出（SISO），只看输入输出关系，内部状态是黑箱。
- 状态空间：适合多输入多输出（MIMO），能看到系统内部所有状态的变化。
四旋翼是什么？4个电机输入，6个自由度输出（位置x,y,z + 姿态roll,pitch,yaw）。典型的MIMO系统。所以——状态空间模型是必选项。
核心结论：做四旋翼MPC，请直接上状态空间模型。传递函数可以作为理解基础，但别指望用它做预测控制。
3.2 从物理到数学：四旋翼的动力学方程
好，我们开始搭模型。先回忆一下四旋翼怎么飞的：
- 四个电机，产生升力F1~F4
- 总升力决定上下
- 差动力矩决定姿态
嗯，这里要注意：四旋翼是个强耦合、非线性的系统。但MPC用的预测模型，通常要线性化。为什么？因为非线性MPC计算量太大，飞控芯片扛不住。
我在项目中遇到过这种情况：一开始想上非线性MPC，结果STM32H7算到冒烟，帧率掉到10Hz。后来老老实实线性化，跑50Hz稳稳的。
所以，我们先用牛顿-欧拉法写出完整动力学：
// 位置动力学（惯性系下）
m * p_ddot = [0, 0, -mg] + R * [0, 0, T]
// 姿态动力学（机体系下）
J * omega_dot = -omega × (J * omega) + tau其中：
- p = [x, y, z] 位置
- R 是旋转矩阵（由欧拉角或四元数构成）
- T 是总推力
- J 是转动惯量矩阵
- tau = [tau_phi, tau_theta, tau_psi] 是控制力矩
你看，这里有个R矩阵，里面全是sin和cos。这就是非线性的来源。
3.3 线性化：小角度假设
怎么处理非线性？最常用的方法——小角度假设。
说白了，就是假设四旋翼在悬停或小幅度机动时，roll和pitch都很小（比如<15°）。这时候：
- sin(θ) ≈ θ
- cos(θ) ≈ 1
- 旋转矩阵R可以简化
我曾经吃过这个亏：有一次做特技飞行测试，roll角直接干到60°，模型瞬间失效，MPC输出乱跳，飞机差点翻掉。所以——小角度假设只适用于悬停和慢速飞行，做特技得换模型。
线性化之后，我们可以把系统写成标准的状态空间形式：
x_dot = A * x + B * u
y = C * x + D * u对于四旋翼，状态量通常选：
| 符号 | 含义 | 单位 |
|---|---|---|
| x, y, z | 位置 | m |
| u, v, w | 速度（机体系） | m/s |
| φ, θ, ψ | 欧拉角 | rad |
| p, q, r | 角速度 | rad/s |
控制量u通常是：
- u1 = 总推力 T
- u2 = roll力矩 tau_phi
- u3 = pitch力矩 tau_theta
- u4 = yaw力矩 tau_psi
3.4 离散化：让模型跑在芯片上
状态空间模型是连续的（x_dot）。但飞控是离散的（每隔dt算一次）。所以必须离散化。
最常用的方法是零阶保持法（ZOH）：
A_d = expm(A * dt)
B_d = A^(-1) * (A_d - I) * B嗯，这里要注意：expm是矩阵指数，不是逐元素exp。很多新手在这里写错。
我个人习惯用Python的scipy.signal.cont2discrete来做这一步，省得手算矩阵指数：
from scipy.signal import cont2discrete
# 连续系统
A = ... # 12x12矩阵
B = ... # 12x4矩阵
# 离散化，dt=0.02s (50Hz)
dt = 0.02
(A_d, B_d, C_d, D_d, _) = cont2discrete((A, B, C, D), dt, method='zoh')离散化之后，预测模型就变成了：
x(k+1) = A_d * x(k) + B_d * u(k)这就是MPC做预测的核心公式。给定当前状态x(k)和未来控制序列u(k), u(k+1), ...，你可以一步步算出未来状态。
3.5 画个图：四旋翼预测模型构建流程
说了这么多，我画个图帮你理清思路：
3.6 避坑指南：建模时最容易犯的错
做建模这么多年，我踩过的坑能写一本书。挑几个最要命的跟你说：
坑1：忽略执行器动态
我曾经直接把电机转速当控制量，结果模型预测飞得挺好，实际飞起来抖成筛子。为什么？因为电机有响应延迟（一阶惯性环节），模型里没考虑。
解决办法：在B矩阵后面串一个执行器模型，比如：
u_real = (1 / (tau_motor * s + 1)) * u_cmd坑2：离散化步长选太大
dt=0.05s（20Hz）？模型预测误差会大到离谱。四旋翼角速度响应很快，dt最好≤0.02s（50Hz）。
小技巧：模型建好后，先做开环仿真。给一个阶跃输入，看看响应曲线是否符合物理直觉。如果模型预测的上升时间跟实际差太多，赶紧回去检查参数。
3.7 总结：你需要记住的三件事
- 四旋翼MPC必须用状态空间模型，传递函数搞不定MIMO系统
- 线性化+离散化是工程标配，别想着上非线性MPC，除非你的芯片是i9
- 模型验证比建模本身更重要——我见过太多人花两周建模，却不愿意花两小时验证
好了，模型搭好了。下一章我们就要拿着这个模型，开始设计MPC控制器了。到时候你会看到，今天搭的每一个矩阵，都会变成优化问题里的约束条件。
公众号:蓝海资料掘金营，微信deep3321