LQR与卡尔曼滤波在无人机状态估计中的应用

1. 项目背景与核心问题

四旋翼无人机作为现代控制理论研究的理想平台，其状态估计精度直接影响飞行控制的稳定性。在真实飞行环境中，传感器测量数据不可避免地会受到各种噪声干扰（如陀螺仪漂移、加速度计振动噪声等），导致原始观测值与实际状态存在偏差。本项目针对二维简化模型，通过LQR控制与卡尔曼滤波的协同工作，实现了在噪声环境下的高精度状态估计。

传统PID控制在四旋翼系统中面临两个主要挑战：一是系统存在强非线性耦合特性；二是传感器噪声会导致控制指令的高频抖动。我们采用线性二次调节器（LQR）作为基础控制器，其优势在于可以通过代价函数明确权衡状态误差与控制能耗的关系。而卡尔曼滤波则通过概率统计方法，将动力学模型预测与传感器观测进行最优融合。

2. 系统建模与LQR设计

2.1 二维四旋翼动力学模型

在二维垂直平面内，我们定义系统状态向量为：

code复制x = [φ θ v x ω y]^T

其中φ为滚转角，θ为俯仰角，v为x方向速度，x为水平位置，ω为角速度，y为垂直高度。对应的微分方程可表示为：

code复制φ' = (1/J)*T
θ' = ω
v' = (1/m)*(f1+f2)*sinθ
x' = v
ω' = (1/m)*((f1+f2)*cosθ - mg)
y' = ω

注意：这里进行了小角度假设（sinθ≈θ, cosθ≈1），将非线性系统在平衡点附近线性化，这是后续LQR设计的基础。

2.2 LQR控制器设计

LQR的核心是求解代数Riccati方程，得到最优状态反馈矩阵K。我们定义二次型代价函数：

code复制J = ∫(x^T Q x + u^T R u)dt

其中Q和R分别为状态和输入的权重矩阵。通过MATLAB的lqr()函数可以方便求解：

matlab复制[K, S, e] = lqr(A, B, Q, R);

在实际调参中，Q矩阵的对角元素取值反映了对不同状态变量的重视程度。例如增大高度误差的权重系数，可以使控制器更优先稳定垂直方向运动。经过多次实验验证，我们最终采用的参数为：

matlab复制Q = diag([10 5 1 0.1 2 8]); 
R = 0.1*eye(2);

3. 卡尔曼滤波实现

3.1 滤波器初始化

卡尔曼滤波包含两个主要阶段：预测和更新。首先需要定义过程噪声协方差Q_kf和观测噪声协方差R_kf（注意与LQR的Q/R区分）：

matlab复制Q_kf = diag([0.01 0.01 0.1 0.1 0.05 0.2]); % 过程噪声
R_kf = diag([0.5 0.5]); % 观测噪声
P = eye(6); % 初始估计误差协方差

3.2 实时滤波流程

在每个时间步长内执行以下操作：

1. 状态预测：

   matlab复制x_pred = A * x_est + B * u;
   P_pred = A * P * A' + Q_kf;
   

2. 卡尔曼增益计算：

   matlab复制K = P_pred * H' / (H * P_pred * H' + R_kf);
   

3. 状态更新：

   matlab复制x_est = x_pred + K * (z_meas - H * x_pred);
   P = (eye(6) - K * H) * P_pred;
   

关键技巧：当传感器数据异常时（如超出物理合理范围），可暂时增大R_kf对应元素值，降低该观测量的权重，避免滤波器发散。

4. 仿真结果与分析

4.1 状态跟踪性能

在加入高斯白噪声（SNR=20dB）的仿真条件下，卡尔曼滤波展现出优秀的去噪能力。以高度通道为例：

4.2 控制效果对比

对比纯LQR与LQR+KF的组合方案，在突加风扰条件下：

• 无KF时：控制输入出现高频抖动（标准差=2.3N）
• 加入KF后：控制指令平滑（标准差=0.7N），且稳态误差减小约60%

5. 工程实现中的挑战

5.1 计算效率优化

原始MATLAB实现每个周期需约5ms，难以满足实时性要求。通过以下改进将耗时降至0.8ms：

1. 预计算卡尔曼增益的稳态值
2. 将矩阵运算改为手工展开的标量运算
3. 使用定点数替代浮点运算

matlab复制% 优化后的标量运算示例
x_est(1) = x_pred(1) + K(1,1)*(z(1)-x_pred(1)) + K(1,2)*(z(2)-x_pred(2));

5.2 参数整定经验

1. Q_kf调整：增大对角元素会使滤波器更信任模型预测，适用于传感器噪声大的场景
2. R_kf调整：增大元素值会减弱对应观测量的影响，适合处理间歇性传感器故障
3. 收敛检测：监控P矩阵的迹（trace(P)），若持续增大表明滤波器发散

6. 扩展应用与改进方向

当前系统可进一步扩展：

1. 自适应噪声估计：根据新息（innovation）序列实时调整Q_kf和R_kf
2. 多传感器融合：加入视觉或UWB测量，扩展观测矩阵H
3. 非线性滤波：考虑采用EKF或UKF处理大角度机动

我在实际调试中发现，当无人机进行快速滚转时，线性模型误差会导致估计精度下降。此时临时增加过程噪声协方差Q_kf中对应角速度的元素值，可有效改善跟踪性能。