无人机高度控制算法：PID与LQR的对比与优化-嵌云网-嵌入式AI开发资源站

无人机高度控制算法：PID与LQR的对比与优化

SeigRobotics

1. 无人机高度控制的核心挑战

在无人机飞行控制系统中，高度稳定性是最基础也是最重要的性能指标之一。想象一下，当你操控无人机进行航拍时，如果高度忽上忽下，拍出来的画面就会像坐过山车一样颠簸。而在更专业的应用场景，比如电力巡检或精准投送，高度波动可能导致任务完全失败。

风扰是影响高度稳定的主要外部因素。根据我的实测经验，即使是3-4级风（风速约5.5-7.9m/s）也会导致消费级无人机产生±0.5米的高度波动。这种扰动主要来自两方面：一是持续风造成的稳态偏移，二是阵风带来的瞬时冲击。传统的高度控制算法往往难以同时应对这两种扰动。

2. 控制算法选型思路

2.1 PID控制：经典但需调优

PID控制器由比例(P)、积分(I)、微分(D)三个环节组成，就像开车时的三个本能反应：

P环节相当于看到偏差立即打方向盘（快速响应）
I环节相当于发现持续偏离车道就多转一点（消除稳态误差）
D环节相当于预判弯道提前减速（抑制超调）

我在Matlab中实现的PID高度控制器核心代码如下：

matlab复制function u = pid_controller(e, prev_e, integral, Kp, Ki, Kd, dt)
    proportional = Kp * e;
    integral = integral + Ki * e * dt; 
    derivative = Kd * (e - prev_e) / dt;
    u = proportional + integral + derivative;
end

实际调参时，我总结出这样的经验法则：

先调Kp直到出现轻微震荡
然后调Kd直到震荡消失
最后调Ki直到稳态误差消除
在风扰环境下，通常需要将Kd设为正常值的1.2-1.5倍

2.2 LQR控制：系统化的最优方案

LQR(线性二次调节器)采用状态空间方法，通过最小化代价函数来设计控制器。其核心优势在于能够系统化地处理多变量耦合问题。在设计无人机高度控制器时，我通常考虑以下状态变量：

高度误差
高度变化率
加速度

代价函数一般形式为：
J = ∫(xᵀQx + uᵀRu)dt

其中Q矩阵决定状态变量的权重，R矩阵控制输入代价。经过多次实验，我发现这样的权重分配效果较好：

matlab复制Q = diag([10, 1, 0.1]);  % 高度误差权重最大
R = 0.01;  % 允许适度的控制量

3. 风扰建模与补偿策略

3.1 风场建模方法

在实际应用中，我将风扰分为三个分量建模：

稳态风：常数项
阵风：用正弦波叠加表示
随机扰动：白噪声

Matlab实现示例：

matlab复制function Fw = wind_model(t)
    steady = 2.0;  % 稳态风速
    gust = 3*sin(0.5*t);  % 阵风
    noise = randn()*0.5;  % 随机扰动
    Fw = steady + gust + noise;
end

3.2 抗扰增强方案

基于项目经验，我推荐两种有效的抗扰策略：

前馈补偿：

matlab复制u = u_nominal + Fw_estimate;  % 在控制量中加入风力估计

自适应PID：
根据风扰强度动态调整参数：

强风时增大Kd抑制震荡
弱风时恢复原参数保证响应速度

4. 仿真对比与结果分析

4.1 阶跃响应对比

在无风条件下测试：

PID：上升时间0.8s，超调量5%
LQR：上升时间1.2s，无超调

4.2 抗扰性测试

施加5m/s阶跃风扰：

PID：最大偏差0.6m，稳定时间4s
LQR：最大偏差0.3m，稳定时间2.5s

从频谱分析看，LQR在0.1-1Hz频段（典型风扰频率）的扰动抑制能力比PID强约40%。

5. 工程实现要点

5.1 离散化处理

实际飞控需要在离散时间运行，LQR的离散化实现关键步骤：

matlab复制[Kd, S, e] = dlqr(Ad, Bd, Q, R);  % 离散系统求解

5.2 计算效率优化

通过预先计算增益矩阵，实时运算仅需：

matlab复制u = -K*x;  % 向量乘法运算

在我的测试中，LQR的单个控制周期耗时约15μs（STM32F4平台），完全满足实时性要求。

6. 参数整定经验分享

6.1 PID参数整定

基于Ziegler-Nichols方法的改进流程：

先置Ki=Kd=0
增大Kp直到临界震荡
记录临界增益Ku和周期Tu
按修正公式设置：
Kp = 0.6Ku
Ki = 2Kp/Tu
Kd = KpTu/8

6.2 LQR权重调整

实用的权重调整策略：

先设Q=diag([1,0,0]), R=1
逐步增大Q(1,1)直到响应速度达标
调整Q(2,2)抑制速度波动
最后微调R限制控制量幅值

7. 实际部署注意事项

传感器滤波：建议使用二阶低通滤波，截止频率设为控制带宽的3-5倍

执行器饱和：必须加入抗饱和补偿，我常用的是：

matlab复制if u > umax
    u = umax;
    integral = integral - Ki*e*dt;  % 反计算补偿
end

采样时间选择：通常取系统响应时间的1/10~1/20

8. 扩展应用方向

这种控制架构经过适当修改，还可以应用于：

多旋翼姿态控制
自主起降控制
编队飞行高度同步

我在最近的一个项目中，将LQR扩展到三维位置控制，通过解耦实现了良好的控制效果。关键是将高度、水平位置分别设计控制器，再通过协调层实现耦合管理。