四旋翼无人机LQR控制与卡尔曼滤波状态估计实践-AI智能范式网

四旋翼无人机LQR控制与卡尔曼滤波状态估计实践

四旋翼控制LQR卡尔曼滤波

|难度：普通|阅读约 7 分钟

1. 项目背景与核心问题

在无人机控制领域，四旋翼系统因其结构简单但动力学特性复杂的特点，成为验证先进控制算法的理想平台。本项目聚焦于二维平面内的四旋翼状态估计问题，这是实现精准控制的前提条件。实际飞行中，传感器采集的原始数据往往包含多种噪声（如高斯白噪声、量化误差等），直接使用这些数据会导致控制性能显著下降。

传统PID控制器在面对噪声干扰时表现不佳，而单纯依靠动力学模型的开环估计又会因模型误差导致发散。为此，我们采用LQR（线性二次调节器）作为基础控制器，配合卡尔曼滤波构建状态估计器，形成完整的闭环解决方案。这种组合在保证系统稳定性的同时，能够有效抑制噪声影响。

需要模型API调用？免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。

2. 系统建模与LQR设计

2.1 二维四旋翼动力学模型

在二维简化模型中，我们考虑俯仰角θ和垂直位置y两个自由度。系统状态向量定义为：
x = [θ, ω, y, vy]^T
其中ω为角速度，vy为垂直速度。根据牛顿-欧拉方程，连续时间状态空间方程为：

code复制dx/dt = A*x + B*u
z = C*x + v

其中A矩阵包含质量、转动惯量等物理参数，B为控制矩阵，u=[u1,u2]^T为两个电机的推力输入。过程噪声w和观测噪声v假设为零均值高斯白噪声。

实际建模时需注意：当俯仰角较大时（如超过15°），线性化模型误差会显著增加。此时要么限制控制量保持小角度，要么考虑非线性观测器设计。

2.2 LQR控制器设计

LQR的核心是求解Riccati方程得到最优反馈矩阵K。代价函数设计为：
J = ∫(x^T Q x + u^T R u)dt

Q和R的选择直接影响控制性能：

Q对角元素对应状态量的权重，通常位置误差权重应大于角度
R反映控制代价，过大导致响应迟缓，过小可能引发执行器饱和
经验法则：初始值取Q=diag([10,1,100,10])，R=eye(2)*0.1

Matlab中求解代码如下：

matlab复制[K,S,e] = lqr(A,B,Q,R);
sys_cl = ss(A-B*K,B,C,0); % 闭环系统

3. 卡尔曼滤波实现

3.1 离散化处理

由于实际系统采用数字控制，需将连续模型离散化：

matlab复制sys_d = c2d(sys,Ts,'zoh'); 
[Ad,Bd,Cd,Dd] = ssdata(sys_d);

离散化时间Ts的选择需权衡计算负荷和估计精度，通常取控制周期的1/2～1/5。

3.2 滤波器参数调试

卡尔曼滤波包含两个关键协方差矩阵：

过程噪声协方差Q_kf：反映模型不确定性
观测噪声协方差R_kf：传感器噪声特性

调试技巧：

初始值可通过传感器手册获取噪声参数
实际运行中观察新息序列（z-Cx̂）应呈白噪声特性
若估计响应迟缓，适当增大Q_kf；若估计抖动明显，增大R_kf

典型实现代码框架：

matlab复制function x_est = kalman_filter(z,u)
    persistent x P
    if isempty(x)
        x = x0; P = P0; 
    end
    
    % 预测步骤
    x = Ad*x + Bd*u;
    P = Ad*P*Ad' + Q_kf;
    
    % 更新步骤
    K = P*Cd'/(Cd*P*Cd'+R_kf);
    x = x + K*(z-Cd*x);
    P = (eye(4)-K*Cd)*P;
    
    x_est = x;
end

4. 仿真实现与结果分析

4.1 噪声建模

为模拟真实环境，需要在理想信号上叠加噪声：

matlab复制% 角度传感器噪声（典型值：0.5°标准差）
theta_noise = 0.5*pi/180*randn; 

% 位置测量噪声（如超声波测距，5cm标准差）
y_noise = 0.05*randn;

4.2 性能评估指标

定量分析采用以下指标：

RMSE：估计状态与真实状态的均方根误差
ISE：积分平方误差 ∫e^2(t)dt
控制能量消耗：∫u^T(t)u(t)dt

4.3 典型问题排查

滤波器发散现象：
- 检查模型参数是否正确（特别是惯量、质量）
- 验证(A,C)是否可观测
- 尝试增大初始协方差P0
控制振荡：
- 检查LQR权重矩阵是否合理
- 确认执行器饱和限制
- 尝试增加速度状态权重
计算延迟：
- 优化代码结构，避免循环内冗余计算
- 考虑使用预先计算的增益矩阵

5. 进阶改进方向

自适应卡尔曼滤波：
根据新息序列实时调整Q_kf和R_kf，适应不同飞行阶段
故障检测：
通过监测残差特性识别传感器故障
硬件实现优化：
- 定点数运算加速
- 矩阵运算并行化
非线性扩展：
当角度较大时，可采用EKF或UKF替代线性卡尔曼滤波

实际部署时还需要考虑：

计算延迟补偿
传感器时间同步
执行器动态特性建模

我在实际测试中发现，电机响应延迟超过10ms就会显著影响控制性能。解决方法是在状态向量中增加电机动态项，或在前馈路径中加入延迟补偿。