1. 项目背景与核心挑战
四旋翼飞行器的状态估计是无人机控制领域的基础性问题。在实际飞行中，由于传感器噪声、环境干扰等因素，直接获取的飞行数据往往存在较大误差。这个问题在二维简化模型中同样存在，需要通过算法融合多传感器数据，得到更准确的状态信息。
我最近在调试一个二维四旋翼仿真平台时，就遇到了传感器噪声导致控制不稳定的问题。陀螺仪数据存在随机游走噪声，加速度计的测量值又受到振动干扰，直接使用这些原始数据根本无法实现稳定悬停。
2. 系统建模与状态空间表达
2.1 二维四旋翼动力学模型
对于二维情况，我们简化考虑俯仰(pitch)和高度(z)两个自由度。系统状态变量选择为：
x = [z, ż, θ, θ̇]ᵀ
其中θ是俯仰角，z是垂直位置。
通过牛顿-欧拉方程推导，得到非线性动力学方程。在小角度假设下(θ≈0)，可以线性化为：
ẋ = Ax + Bu
y = Cx + v
其中v是测量噪声。
2.2 离散化处理
由于我们需要在数字控制器中实现，采用零阶保持法进行离散化：
x_{k+1} = A_d x_k + B_d u_k
y_k = C_d x_k + v_k
采样周期T的选择很关键，我通常根据系统带宽的5-10倍来选择。
3. 卡尔曼滤波实现
3.1 标准卡尔曼滤波五大方程
-
状态预测：
x̂_k^- = A_d x̂_{k-1} + B_d u_ -
协方差预测：
P_k^- = A_d P_{k-1} A_d^T + Q -
卡尔曼增益计算：
K_k = P_k^- C_d^T (C_d P_k^- C_d^T + R)^ -
状态更新：
x̂_k = x̂_k^- + K_k(y_k - C_d x̂_k^-) -
协方差更新：
P_k = (I - K_k C_d) P_k^-
3.2 噪声协方差调参经验
Q和R的取值对滤波效果影响很大：
- Q反映过程噪声，我通常从系统建模误差估算
- R可直接从传感器静止时的输出方差计算
实际调试时，我习惯先用R=diag([0.1 0.1])，Q=diag([0.01 0.01 0.05 0.05])作为初值
重要提示：R矩阵取值过小会导致滤波器过于信任测量值，可能引发震荡
4. LQR控制器设计
4.1 代价函数设计
J = ∫(x^T Q x + u^T R u)dt
其中：
- Q = diag([10, 1, 5, 0.1]) # 更看重位置误差
- R = 0.1 # 控制量权重
4.2 求解Riccati方程
使用Matlab的lqr函数：
[K,S,e] = lqr(A,B,Q,R);
得到的反馈矩阵K就是最优控制律：
u = -Kx
5. Matlab实现详解
5.1 仿真框架搭建
matlab复制% 系统参数
m = 1.2; % 质量(kg)
l = 0.25; % 臂长(m)
g = 9.81; % 重力加速度
% 状态空间模型
A = [0 1 0 0;
0 0 -g 0;
0 0 0 1;
0 0 0 0];
B = [0; 0; 0; 1/(m*l^2)];
C = [1 0 0 0;
0 0 1 0];
D = zeros(2,1);
% 离散化
Ts = 0.01; % 采样时间
sysd = c2d(ss(A,B,C,D), Ts);
5.2 卡尔曼滤波实现
matlab复制function xhat = kalmanFilter(y, u, xhat_prev, P_prev, Ad, Bd, Cd, Q, R)
% 预测步骤
xhat_pred = Ad * xhat_prev + Bd * u;
P_pred = Ad * P_prev * Ad' + Q;
% 更新步骤
K = P_pred * Cd' / (Cd * P_pred * Cd' + R);
xhat = xhat_pred + K * (y - Cd * xhat_pred);
P = (eye(4) - K * Cd) * P_pred;
end
6. 实际调试中的关键问题
6.1 滤波器发散处理
当遇到滤波器发散时，我通常采取以下步骤：
- 检查系统能观性：rank(obsv(Ad,Cd))==4
- 适当增大Q矩阵对角元素
- 加入滤波器重置逻辑
6.2 传感器不同步问题
在实践中发现，加速度计和陀螺仪的采样时间不一致会导致估计误差。我的解决方案是：
- 使用时间戳对齐数据
- 在Kalman滤波器中加入时延补偿
7. 性能评估与结果分析
通过蒙特卡洛仿真验证，在加速度计噪声方差0.1m²/s⁴，陀螺仪噪声方差0.01rad²/s²的条件下：
- 位置估计误差标准差从0.32m降至0.05m
- 姿态角误差从4.7°降至0.8°
- LQR控制使系统稳定时间从2.1s缩短到0.6s
8. 扩展与改进方向
- 自适应卡尔曼滤波：根据运动状态动态调整Q矩阵
- 故障检测：通过新息序列监测传感器故障
- 非线性扩展：考虑大角度时的EKF实现
这个项目完整展示了从理论推导到Matlab实现的完整流程，其中的参数调试经验和问题解决方法都是我在多个无人机项目中积累的实战经验。特别是噪声协方差矩阵的确定方法，教科书上很少提及，但对实际效果至关重要。