1. 项目概述
去年夏天调试农业植保无人机时，我遇到了一个棘手问题：在3-4级风况下，无人机喷洒农药时高度波动达到±1.5米，导致作物受药不均匀。这个经历让我深入研究了抗风扰的高度控制方案，最终形成了这套结合LQR和PID的混合控制策略。
这个方案的核心价值在于：通过LQR控制器处理系统级动态特性，同时用PID控制器补偿实时扰动，在Matlab仿真中实现了将高度波动控制在±0.2米内的效果。特别适合需要精确高度维持的场景，比如航测测绘、电力巡检等专业应用。
2. 控制策略设计原理
2.1 无人机高度动力学模型
建立准确的数学模型是控制器设计的基础。以一维高度控制为例，无人机的垂直动力学可以表示为：
code复制m·z'' = T - m·g - D
其中：
- m：无人机质量（kg）
- z：垂直位置（m）
- T：旋翼总推力（N）
- D：空气阻力（N）
在Matlab中，我们将其转化为状态空间形式：
matlab复制A = [0 1; 0 -d/m];
B = [0; 1/m];
C = [1 0];
D = 0;
sys = ss(A,B,C,D);
关键提示：模型中的阻尼系数d需要通过实际飞行数据辨识获得，实验室条件下可用风速仪辅助测量。
2.2 LQR控制器设计
线性二次调节器(LQR)的优势在于它能够全局优化系统性能。设计步骤包括：
- 定义代价函数矩阵：
matlab复制Q = diag([100, 10]); % 位置误差权重100，速度误差权重10
R = 0.01; % 控制量权重
- 求解Riccati方程：
matlab复制[K,S,e] = lqr(A,B,Q,R);
- 控制律实现：
matlab复制u = -K*x;
实测中发现，Q矩阵中位置误差权重取100-150，速度误差权重取5-15时，能在响应速度和超调量之间取得较好平衡。
2.3 PID控制器设计
PID控制器用于补偿模型误差和外部扰动。采用位置式PID算法：
matlab复制% 离散PID实现
error = ref - actual;
integral = integral + error*dt;
derivative = (error - prev_error)/dt;
output = Kp*error + Ki*integral + Kd*derivative;
prev_error = error;
参数整定经验：
- 先调Kp至系统开始振荡
- 然后调Kd抑制振荡
- 最后调Ki消除稳态误差
- 在5m/s风扰下，典型参数范围：
Kp∈[0.8,1.2], Ki∈[0.05,0.1], Kd∈[0.3,0.5]
3. 混合控制架构实现
3.1 控制结构框图
code复制[参考高度] → [LQR主控制器] → [无人机动力学]
↑ ↓
[风扰观测] → [PID补偿器] ← [高度反馈]
3.2 Matlab实现关键代码
matlab复制% 混合控制器主循环
for t = 0:dt:Tfinal
% 获取当前状态
[z, v] = getDroneState();
% LQR控制
u_lqr = -K*[z-ref; v];
% PID补偿
error = ref - z;
integral = integral + error*dt;
derivative = (error - prev_error)/dt;
u_pid = Kp*error + Ki*integral + Kd*derivative;
% 综合输出
u_total = u_lqr + u_pid;
% 执行控制
applyThrust(u_total);
% 记录数据
logData(t, z, u_total);
end
3.3 风扰建模方法
在仿真中加入Dryden湍流模型：
matlab复制% Dryden风谱实现
phi_u = (2*Lu/(pi))*(1/(1+(Lu*omega)^2));
u_wind = sqrt(phi_u)*randn;
典型参数设置：
- 风速均值：5m/s
- 湍流尺度Lu：50m
- 更新频率：100Hz
4. 仿真结果分析
4.1 阶跃响应对比
| 控制方式 | 上升时间(s) | 超调量(%) | 稳态误差(m) |
|---|---|---|---|
| 纯PID | 1.2 | 15.3 | 0.05 |
| 纯LQR | 0.8 | 5.2 | 0.02 |
| 混合控制 | 0.9 | 2.1 | 0.01 |
4.2 抗风扰性能
在5m/s随机风扰下：
- 纯PID控制：最大偏差0.8m
- 纯LQR控制：最大偏差0.5m
- 混合控制：最大偏差0.2m
实测技巧：在仿真中逐步增加风扰强度，可以观察到混合控制在6m/s风速时仍能保持0.3m以内的波动，而单一控制器此时已出现发散趋势。
5. 工程实现注意事项
-
传感器选择：
- 高度测量建议融合气压计(低频准确)和超声波(高频灵敏)
- 更新率不低于100Hz
-
执行器限制：
- 考虑电机响应延迟(典型值50-100ms)
- 加入推力变化率限制：
matlab复制du_max = 200; % N/s u = constrain(u_prev + du_max*dt, u_prev - du_max*dt);
-
实时性保障：
- 在嵌入式平台实现时，LQR矩阵运算需要预先计算
- PID运算时间应控制在1ms以内
-
安全机制：
- 设置高度容差带(如±0.5m)
- 超限时触发紧急悬停模式
6. 参数调试实战技巧
6.1 LQR权重调整
通过改变Q矩阵实现不同控制特性：
matlab复制% 追求快速响应
Q_fast = diag([200, 5]);
% 追求平稳
Q_smooth = diag([80, 20]);
调试步骤：
- 先固定R=0.01
- 调整Q(1,1)使上升时间达标
- 调整Q(2,2)抑制超调
- 最后微调R平衡控制量大小
6.2 PID参数整定
推荐使用衰减曲线法：
- 先置Ki=0, Kd=0
- 增大Kp至系统开始等幅振荡
- 记录此时Kp=Ku, 振荡周期=Tu
- 按Ziegler-Nichols规则设置：
- Kp = 0.6*Ku
- Ki = 1.2*Ku/Tu
- Kd = 0.075KuTu
7. 扩展应用方向
-
三维位置控制：
matlab复制% 扩展状态向量 x = [x; y; z; vx; vy; vz]; % 设计6x6权重矩阵 Q = blkdiag([100,100,150], [10,10,15]); -
自适应控制：
matlab复制% 根据风扰估计调整PID参数 if wind_estimate > 5 Kp = Kp_nominal * 1.2; Kd = Kd_nominal * 1.5; end -
硬件在环测试：
- 通过PX4硬件在环仿真验证
- 使用QGroundControl监控实时数据
这套方案在Gazebo仿真和实机测试中都表现良好，特别是在突风条件下，高度维持精度比单一控制器提高3-5倍。对于需要精确高度控制的工业级无人机应用，这种混合架构提供了可靠的解决方案。