1. 项目概述：无人机LQR控制器的核心价值
去年调试四旋翼无人机时，我曾在PID控制器上耗费了两周时间调整参数。直到改用LQR（线性二次调节器）方案后，系统响应时间从1.2秒缩短到0.3秒，且抗风扰能力提升显著。这个基于6维简化线性模型的LQR控制器设计，正是解决无人机三维空间精准定位问题的经典方案。
LQR控制器的本质是通过状态反馈矩阵K，将系统动态特性与性能指标量化关联。相比传统PID，它能同时处理多变量耦合问题——这正是无人机在X/Y/Z三轴运动中面临的核心挑战。我们采用的6维模型包含位置(x,y,z)和姿态角(φ,θ,ψ)的线性化表达，既保留了足够的控制维度，又避免了全状态模型的计算负担。
关键提示：6维模型简化了无人机动力学中的高阶非线性项，但通过小角度假设和线性化处理，在悬停工况附近仍能保持90%以上的控制精度。
2. 模型构建与线性化处理
2.1 无人机动力学方程推导
四旋翼无人机的完整非线性动力学模型包含12个状态变量（位置、速度、角度、角速度）。通过牛顿-欧拉方程可建立如下关系：
code复制m·ẍ = (sinψ·sinφ + cosψ·sinθ·cosφ)·U₁
m·ÿ = (-cosψ·sinφ + sinψ·sinθ·cosφ)·U₁
m·z̈ = (cosθ·cosφ)·U₁ - mg
I·ω̇ + ω×(I·ω) = [U₂, U₃, U₄]ᵀ
其中U₁为总升力，U₂/U₃/U₄为三轴力矩。通过小角度假设（sinθ≈θ, cosθ≈1）和悬停点线性化，我们得到简化后的6维状态空间模型：
code复制ẋ = v_x
ẏ = v_y
ż = v_z
v̇_x = g·θ
v̇_y = -g·φ
v̇_z = U₁/m - g
2.2 状态空间表达
将系统表示为标准状态空间形式：
code复制Ẋ = A·X + B·U
Y = C·X
其中状态向量X=[x,y,z,v_x,v_y,v_z]ᵀ，控制输入U=[U₁,φ,θ]ᵀ。通过Matlab的ss()函数可快速构建系统矩阵：
matlab复制A = [zeros(3) eye(3);
zeros(3) zeros(3)];
B = [zeros(3);
0 0 0;
0 -g 0;
g 0 0];
C = eye(6);
sys = ss(A,B,C,0);
调试技巧：实际建模时需加入电机动力学延迟，可在B矩阵中添加一阶惯性环节(1/(τs+1))，典型时间常数τ≈0.05s。
3. LQR控制器设计与实现
3.1 代价函数参数整定
LQR的核心是求解Riccati方程得到最优反馈矩阵K，其性能由权重矩阵Q和R决定。经过多次飞行测试，推荐采用如下参数规则：
- 位置误差权重：Q(1:3) = diag([10 10 15])
（Z轴权重更高以对抗重力） - 速度误差权重：Q(4:6) = diag([5 5 8])
- 控制量权重：R = diag([0.1 1 1])
（升力U₁的代价低于姿态角）
Matlab求解代码：
matlab复制Q = diag([10 10 15 5 5 8]);
R = diag([0.1 1 1]);
[K,S,e] = lqr(A,B,Q,R);
3.2 状态反馈控制律
得到K矩阵后，控制量计算为：
code复制U = -K·(X - X_ref)
其中X_ref为期望状态。实际实现时需注意：
- 加入积分环节消除稳态误差：
matlab复制
error_integral = error_integral + (X - X_ref)*dt; U = -K*(X - X_ref) - Ki*error_integral; - 控制量限幅：
matlab复制U(1) = min(max(U(1), 0), U1_max); φ_cmd = min(max(U(2), -30*pi/180), 30*pi/180);
4. 三维轨迹跟踪实现
4.1 位置控制器结构
完整的控制架构包含内外环：
code复制[外环] LQR位置控制器 → [内环] PID姿态控制器 → 电机混控
在Simulink中的实现要点：
- 使用
MATLAB Function块实时计算K矩阵 - 通过
Rate Transition模块处理多速率系统（位置环100Hz，姿态环500Hz） - 添加
Transport Delay模拟传感器延迟
4.2 抗扰增强策略
为提高抗风性能，我们采用：
- 扰动观测器（DOB）：
matlab复制
d_hat = H(s)*(U - inv(B)*s·X) - 加速度前馈：
matlab复制
U_ff = m*[a_x_ref; a_y_ref; a_z_ref + g];
5. 典型问题排查指南
| 现象 | 可能原因 | 解决方案 |
|---|---|---|
| 高度持续振荡 | Z轴Q值过高或R值过小 | 增大R(1)或减小Q(3) |
| 水平方向漂移 | 未启用积分环节 | 添加Ki=0.1*diag([1 1 2]) |
| 响应迟缓 | 控制量权重R过大 | 按比例减小R矩阵元素 |
| 大角度机动时发散 | 超出线性模型有效范围 | 增加姿态角限幅或切换非线性控制器 |
实测数据对比（风速3m/s条件下）：
code复制 | 超调量 | 稳定时间(s) | 抗扰误差(m)
----------|--------|-------------|------------
PID控制 | 15% | 1.8 | ±0.5
LQR控制 | 3% | 0.4 | ±0.1
6. Matlab代码实现要点
完整工程应包含以下模块：
matlab复制% 主控制器脚本
function U = lqr_controller(X, X_ref, params)
persistent error_integral;
if isempty(error_integral)
error_integral = zeros(6,1);
end
error = X - X_ref;
error_integral = error_integral + error*params.dt;
U = -params.K*error - params.Ki*error_integral;
U(1) = min(max(U(1), 0), params.U1_max);
U(2:3) = min(max(U(2:3), -30*pi/180), 30*pi/180);
end
% 权重调整工具函数
function [K, Q, R] = tune_lqr(A, B, pos_weight, vel_weight)
Q = diag([pos_weight, vel_weight]);
R = diag([0.1, 1, 1]);
[K,~,~] = lqr(A,B,Q,R);
end
调试时建议先用
lsim()函数进行开环响应验证，再逐步闭环测试。遇到奇异矩阵错误时，检查B矩阵是否满秩。