1. 四旋翼控制算法概述
四旋翼无人机作为典型的欠驱动系统，其控制问题一直是自动控制领域的经典案例。在众多控制策略中，PID和LQR代表了两种截然不同的设计哲学，它们在实际工程中各有优劣。
PID控制作为工业界应用最广泛的算法，其核心思想是通过比例(P)、积分(I)、微分(D)三个环节的线性组合来消除误差。对于四旋翼系统而言，PID控制器通常采用串级结构：外环控制位置/姿态角，内环控制角速度。这种分层设计能有效应对四旋翼的强耦合特性。
相比之下，LQR(线性二次调节器)属于现代控制理论范畴，它通过求解Riccati方程得到最优状态反馈矩阵。LQR控制器在设计阶段就考虑了系统各状态变量之间的耦合关系，理论上能提供更好的动态性能。但需要精确的系统模型作为设计基础，且计算复杂度较高。
实际工程中，90%以上的商用无人机采用PID或其变种算法，而LQR更多见于学术研究和高端应用场景。这种差异主要源于PID的易实现性和鲁棒性。
2. Simulink仿真环境搭建
2.1 四旋翼动力学建模
在Simulink中构建准确的动力学模型是仿真的基础。四旋翼的6自由度模型可以表示为：
matlab复制% 旋转动力学方程
I * omega_dot + cross(omega, I*omega) = tau;
% 平移动力学方程
m * acc = R * [0;0;T] - [0;0;m*g] - kd * vel;
其中I为惯性矩阵，ω为角速度，τ为力矩，R为旋转矩阵，T为总升力，kd为空气阻力系数。建议采用S-function或Simscape Multibody实现这些方程，确保模型既能反映主要动力学特性，又不过于复杂。
2.2 传感器与执行器建模
完整的仿真需要包含：
- IMU噪声模型(高斯白噪声+偏置漂移)
- 电机动态特性(一阶惯性环节+饱和限制)
- 螺旋桨气动力模型(推力/力矩系数)
这些模块可以通过Simulink的Band-Limited White Noise和Saturation等基础模块组合实现。关键参数如电机时间常数通常取50-100ms，与真实无刷电机响应特性相符。
3. PID控制器实现细节
3.1 串级PID结构设计
典型的位置-姿态双环控制结构如下：
code复制位置环(PID) → 姿态环(PID) → 角速度环(PD) → 电机混控
在Simulink中，每个PID控制器都应包含：
- 抗积分饱和机制(Clamping)
- 微分滤波器(一阶低通)
- 输出限幅保护
具体参数整定建议：
- 先调内环(角速度)，确保快速无超调
- 再调外环(姿态)，逐步提高响应速度
- 最后调位置环，注意避免振荡
3.2 参数自整定技巧
利用Simulink的PID Tuner工具可以快速获得基础参数，但需手动优化：
- 增大P直到出现轻微振荡，然后回调20%
- 积分时间设为振荡周期的0.5-1倍
- 微分时间取积分时间的1/4-1/8
对于高度耦合的横滚/俯仰通道，建议采用相同的PID参数以保持对称性。
4. LQR控制器设计与实现
4.1 状态空间建模
首先需要将四旋翼模型在工作点附近线性化，得到状态空间表达式：
code复制dx/dt = A x + B u
y = C x
其中状态变量x通常包含位置、速度、姿态角和角速度共12个分量。控制输入u为4个电机的PWM信号。通过泰勒展开或matlab的linmod函数可获得A,B矩阵。
4.2 权重矩阵选择
LQR性能取决于Q和R矩阵的选择：
matlab复制Q = diag([pos_w, vel_w, angle_w, omega_w]);
R = diag([motor_w]);
经验法则：
- 位置误差权重取1-10
- 角度误差权重取5-20
- 控制量权重根据电机最大推力调整
- 通常设Q对角元素比R大2-3个数量级
4.3 求解与实现
使用matlab的lqr函数求解：
matlab复制[K,S,e] = lqr(A,B,Q,R);
得到的反馈矩阵K需要与状态变量顺序严格对应。在Simulink中通过Gain模块实现状态反馈，注意处理传感器测量与状态估计的转换。
5. 仿真对比与分析
5.1 阶跃响应测试
设置相同的初始条件，对比：
- 上升时间(10%-90%)
- 超调量
- 稳态误差
- 控制能量消耗
典型结果：
| 指标 | PID | LQR |
|---|---|---|
| 上升时间(s) | 1.2 | 0.8 |
| 超调量(%) | 15 | 5 |
| 稳态误差(m) | 0.05 | 0.02 |
5.2 抗干扰测试
在稳态时施加：
- 脉冲风扰(0.5s持续)
- 持续侧风
- 质量突变(模拟负载投放)
LQR通常表现出更好的干扰抑制能力，尤其在多变量协调控制方面优势明显。
6. 工程实践建议
6.1 参数调试技巧
对于PID：
- 先调稳定再调快速
- 使用Ziegler-Nichols法则初设参数
- 记录每次测试的ISE(积分平方误差)
对于LQR：
- 从较大Q值开始逐步降低
- 观察闭环极点分布
- 确保主导极点阻尼比在0.7-1.0之间
6.2 实际部署问题
- 离散化：控制器需转换为离散时间形式
- 计算延迟：考虑实际处理时间影响
- 状态估计：使用卡尔曼滤波融合传感器数据
实测表明，在STM32F4平台上，PID计算耗时约50μs，而LQR需要200μs，这是许多实时系统选择PID的重要原因。
7. 学习资源推荐
7.1 基础理论
- 《自动控制原理》胡寿松版：掌握频域分析法
- 《现代控制工程》Ogata：理解状态空间方法
- MIT OpenCourseWare 16.30课程视频
7.2 实践工具
- MATLAB Onramp免费教程
- PX4开源飞控代码(研究其混控逻辑)
- Gazebo+ROS仿真环境(更逼真的物理引擎)
7.3 进阶方向
- 自适应PID(增益调度)
- 鲁棒控制(H∞方法)
- 模型预测控制(MPC)实现
建议从GitHub获取开源的Simulink模型作为起点，例如ETH Zurich的"Quadrotor-Simulink"项目提供了良好的基准实现。调试时务必保存每次仿真的参数和结果，建立自己的参数数据库。