1. 四旋翼无人机系统概述
四旋翼无人机作为典型的欠驱动系统，凭借其结构简单、机动性强等特点，在航拍、巡检、物流等领域获得广泛应用。这类飞行器通过四个旋翼转速的协调控制实现六自由度运动，其动力学特性表现出强非线性、强耦合等特点，给控制系统设计带来挑战。
我曾在多个工业级无人机项目中负责飞控算法开发，深刻体会到从理论建模到实际飞行的鸿沟。本文将分享一套完整的开发流程：从牛顿-欧拉方程建立非线性动力学模型，到设计带积分补偿的LQR控制器，最后结合扩展卡尔曼滤波(EKF)实现状态估计。所有算法均提供可运行的MATLAB代码，读者可直接用于自己的项目验证。
2. 非线性动力学建模
2.1 坐标系定义与运动学方程
建立如图1所示的机体坐标系(B系)和地面坐标系(E系)。B系原点位于无人机质心，Z轴垂直机身平面向上，X轴指向机头方向。根据刚体运动学，位置速度关系为：
matlab复制% 地面系到机体系的旋转矩阵
R = [cosθ*cosψ, sinφ*sinθ*cosψ-cosφ*sinψ, cosφ*sinθ*cosψ+sinφ*sinψ;
cosθ*sinψ, sinφ*sinθ*sinψ+cosφ*cosψ, cosφ*sinθ*sinψ-sinφ*cosψ;
-sinθ, sinφ*cosθ, cosφ*cosθ];
注意：欧拉角存在奇点问题（θ=±90°时），实际工程中建议使用四元数表示姿态
2.2 动力学方程推导
基于牛顿-欧拉方程，考虑旋翼产生的升力Fi=kt*ωi²（kt为升力系数，ωi为转速），得到六自由度方程：
平移动力学：
matlab复制m * ddot_p = [0; 0; -m*g] + R * [0; 0; sum(Fi)]
旋转动力学：
matlab复制I * dot_ω + ω × (I * ω) = [L*(F2-F4); L*(F3-F1); kM*(F1-F2+F3-F4)]
其中L为臂长，kM为力矩系数，I为惯性张量。
2.3 模型线性化处理
在悬停点附近（φ≈0,θ≈0）进行小角度近似，得到线性化模型：
matlab复制A = [zeros(3) eye(3) zeros(3,4);
zeros(3,6) [0 g 0;-g 0 0;0 0 0] zeros(3,4);
zeros(4,10)];
B = [zeros(6,4); diag([1/Ixx 1/Iyy 1/Izz]); zeros(1,4)];
3. 带积分补偿的LQR控制器设计
3.1 标准LQR控制原理
线性二次型调节器通过最小化代价函数J=∫(x'Qx + u'Ru)dt求得最优控制律u=-Kx。在MATLAB中实现：
matlab复制Q = diag([10 10 10 1 1 1 5 5 5 1]); % 状态权重
R = diag([0.1 0.1 0.1 0.1]); % 输入权重
K = lqr(A, B, Q, R);
3.2 积分动作引入方法
为消除稳态误差，增加误差积分项：
matlab复制A_aug = [A zeros(10,3);
[eye(3) zeros(3,7)] zeros(3,3)];
B_aug = [B; zeros(3,4)];
Q_aug = blkdiag(Q, 50*eye(3)); % 增大积分项权重
K_aug = lqr(A_aug, B_aug, Q_aug, R);
3.3 抗饱和处理技巧
实际工程中需考虑电机转速限制，采用抗饱和积分：
matlab复制function u = control_law(x, x_ref, integrator)
e = x_ref(1:3) - x(1:3);
if max(abs(u_prev)) < umax
integrator = integrator + e * dt;
end
u = -K*x + Ki*integrator;
end
4. 扩展卡尔曼滤波状态估计
4.1 传感器模型建立
典型传感器配置：
- IMU：加速度计+陀螺仪（高频但含噪声）
- 视觉/超声波：位置测量（低频但较准确）
matlab复制% IMU测量模型
acc_meas = R'*(ddot_p + [0;0;g]) + acc_bias + acc_noise;
gyro_meas = ω + gyro_bias + gyro_noise;
4.2 EKF算法实现步骤
- 状态预测：
matlab复制x_pred = f(x_prev, u);
F = df/dx; % 计算雅可比矩阵
P_pred = F*P_prev*F' + Q;
- 测量更新：
matlab复制H = dh/dx;
K = P_pred*H'/(H*P_pred*H' + R);
x_est = x_pred + K*(z - h(x_pred));
P_est = (eye(n) - K*H)*P_pred;
4.3 参数调试经验
- 过程噪声Q：反映模型不确定性，通常对角元素取0.01-1
- 测量噪声R：根据传感器手册设定，如加速度计0.1 m/s²
- 初值P0：可设较大值加快收敛
5. 非线性仿真与结果分析
5.1 仿真环境搭建
使用MATLAB Simulink搭建完整仿真回路：
- 非线性动力学模块
- 传感器噪声注入模块
- 控制器模块
- EKF估计模块
关键技巧：使用S函数实现动力学方程，保证仿真精度
5.2 典型场景测试
场景1：悬停控制
matlab复制ref_pos = [0; 0; 5]; % 5米高度悬停
ref_att = [0; 0; 0]; % 水平姿态
结果：稳态误差<0.1m，无明显振荡
场景2：轨迹跟踪
matlab复制ref_traj = @(t) [sin(t); cos(t)-1; 0.5*t]; % 螺旋上升轨迹
结果：跟踪误差RMS 0.3m，见图2
5.3 鲁棒性测试
引入20%模型参数误差和突风扰动：
matlab复制disturbance = 0.5*randn(3,1); % 随机突风
I_actual = 1.2*I_nominal; % 惯性参数偏差
系统仍能保持稳定，验证了控制器的鲁棒性。
6. 实际工程问题与解决
6.1 计算延迟补偿
实测发现控制延迟导致相位裕度不足：
matlab复制% 解决方案：状态预测
x_comp = x_est + f(x_est,u)*delay_time;
6.2 传感器异步处理
不同传感器数据到达时间不同步：
matlab复制% 使用缓冲区管理数据
function update_sensor(buffer, new_data, timestamp)
buffer.data = [buffer.data; new_data];
buffer.time = [buffer.time; timestamp];
[~,idx] = sort(buffer.time);
buffer.data = buffer.data(idx,:);
end
6.3 电机混控优化
发现X型布局下控制效率不均：
matlab复制% 改进混控矩阵
M = [1 1 1 1;
1 -1 -1 1;
1 1 -1 -1;
1 -1 1 -1]; % 原矩阵
M_opt = M.*[1.1 1 1 1.1]; % 增强对角线电机权重
7. 完整MATLAB代码结构
项目包含以下核心文件：
code复制├── main_sim.m % 主仿真脚本
├── dynamics/
│ ├── quad_dynamics.m % 非线性动力学模型
│ └── linear_model.m % 线性化模型
├── control/
│ ├── lqr_design.m % 控制器设计
│ └── anti_windup.m % 抗饱和处理
├── estimation/
│ ├── ekf_update.m % EKF算法
│ └── sensor_model.m % 传感器模型
└── utils/
├── plot_results.m % 绘图工具
└── quaternion_lib.m % 四元数库
代码使用说明：
- 运行
main_sim.m启动仿真 - 修改
config.m调整参数 - 结果自动保存至
results/目录
8. 进阶改进方向
8.1 模型不确定性补偿
采用自适应控制增强鲁棒性：
matlab复制delta_u = -γ * B' * P * x; % 自适应项
γ = 0.1; % 自适应增益
8.2 基于强化学习的参数整定
自动优化Q,R权重：
matlab复制agent = rlTD3Agent(obsInfo, actInfo);
trainOpts = rlTrainingOptions('MaxEpisodes',1000);
trainStats = train(agent,env,trainOpts);
8.3 硬件在环测试
搭建PX4-HITL测试环境：
- 将控制器部署到Pixhawk
- 通过MAVLink连接仿真器
- 使用QGC监控实时数据