1. 项目概述：四旋翼飞行器的主动防御系统设计
这个项目构建了一套完整的四旋翼无人机主动防御系统，核心在于通过LQR（线性二次调节器）控制算法实现自主拦截功能。当系统检测到未经授权的无人机进入受保护空域时，防御无人机能够自动从巢穴起飞，精确追踪目标并完成物理捕获，最后返回起始位置。整个系统涉及飞行控制、目标追踪、路径规划和机械捕获四大技术模块，而LQR算法正是实现稳定飞行的关键所在。
在实际测试中，我们使用Matlab/Simulink进行控制算法仿真，验证了从起飞、追踪到捕获全流程的可行性。与传统的PID控制器相比，LQR控制表现出更好的抗干扰性和动态响应特性，特别适合这种需要快速机动且要求高稳定性的应用场景。
2. 系统架构与核心组件解析
2.1 硬件系统组成
防御系统的硬件平台主要由以下几个关键部件构成：
-
四旋翼飞行器平台：选用650轴距的碳纤维机架，搭载2212 920KV无刷电机和1045螺旋桨，提供足够的载重能力和机动性。飞控采用Pixhawk 4，作为底层飞行控制的核心处理器。
-
感知系统：
- 视觉识别：使用Intel RealSense D435i深度相机（分辨率1280×720，帧率30fps）进行目标检测与追踪
- 距离感知：配备TFmini Plus激光雷达（测距范围0.1-12m，精度±5cm）用于接近阶段的精确测距
- 定位模块：Ublox M8N GPS（定位精度2.5m CEP）配合光流传感器实现室内外定位
-
捕获机构：设计了一种轻量化机械网弹射装置，重量仅280g，可发射2×2m的凯夫拉纤维网，有效捕获距离3-5m。通过舵机控制发射角度，电磁铁触发释放。
2.2 软件控制架构
软件系统采用分层设计，各模块通过ROS（机器人操作系统）进行通信：
code复制上层决策层（运行在Jetson TX2）
├── 空域监控模块
├── 目标检测与追踪
└── 任务规划器
中层控制层（运行在Pixhawk）
├── LQR飞行控制器
├── 导航滤波器
└── 应急处理模块
底层驱动层
├── 电机控制
├── 传感器数据采集
└── 执行机构驱动
关键数据流：
- 视觉系统检测到入侵无人机后，输出相对位置信息（x,y,z坐标）
- 任务规划器生成拦截路径点序列
- LQR控制器解算各电机控制量
- 到达捕获距离后触发网弹装置
3. LQR控制器设计与实现
3.1 四旋翼动力学建模
建立准确的数学模型是LQR设计的基础。我们采用牛顿-欧拉方法建立四旋翼的6自由度非线性模型：
平移动力学：
$$
\begin{cases}
\ddot{x} = (\sin\psi\sin\phi + \cos\psi\sin\theta\cos\phi)\frac{U_1}{m} \
\ddot{y} = (-\cos\psi\sin\phi + \sin\psi\sin\theta\cos\phi)\frac{U_1}{m} \
\ddot{z} = -g + (\cos\theta\cos\phi)\frac{U_1}{m}
\end{cases}
$$
旋转动力学：
$$
\begin{cases}
\ddot{\phi} = \dot{\theta}\dot{\psi}(\frac{I_y-I_z}{I_x}) - \frac{J_r}{I_x}\dot{\theta}\Omega + \frac{U_2}{I_x} \
\ddot{\theta} = \dot{\phi}\dot{\psi}(\frac{I_z-I_x}{I_y}) + \frac{J_r}{I_y}\dot{\phi}\Omega + \frac{U_3}{I_y} \
\ddot{\psi} = \dot{\phi}\dot{\theta}(\frac{I_x-I_y}{I_z}) + \frac{U_4}{I_z}
\end{cases}
$$
其中：
- $U_1$：总升力（四个电机推力之和）
- $U_2, U_3, U_4$：绕x,y,z轴的力矩
- $I_x, I_y, I_z$：转动惯量
- $J_r$：转子惯量
- $\Omega$：转子总转速
3.2 线性化与状态空间表达
在悬停状态（ϕ=θ=ψ≈0）附近进行线性化处理，得到线性状态空间模型：
$$
\dot{x} = Ax + Bu
$$
状态向量选择为：
$$
x = [x\ y\ z\ \phi\ \theta\ \psi\ \dot{x}\ \dot{y}\ \dot{z}\ \dot{\phi}\ \dot{\theta}\ \dot{\psi}]^T
$$
控制输入为：
$$
u = [U_1\ U_2\ U_3\ U_4]^T
$$
通过雅可比矩阵计算得到的系统矩阵A和控制矩阵B将在不同工作点有所变化，这也是LQR优于固定参数PID的重要原因。
3.3 LQR优化问题求解
LQR的核心是求解Riccati方程，找到最优控制律$u = -Kx$。代价函数设计为：
$$
J = \int_0^\infty (x^TQx + u^TRu)dt
$$
其中Q和R是需要精心设计的权重矩阵。我们的实践经验表明：
- 位置状态权重（x,y,z）：通常设为1-10，过高会导致控制量过大
- 角度状态权重（ϕ,θ,ψ）：设为50-100，因为角度误差对稳定性影响更大
- 速度状态权重：设为0.1-1，避免过度抑制合理运动
- 控制权重矩阵R：对角线元素设为0.01-0.1，平衡控制效果与能耗
Matlab求解代码示例：
matlab复制[K,S,e] = lqr(A,B,Q,R);
3.4 控制器离散化与实现
由于实际系统采用数字控制，需要将连续LQR控制器离散化：
matlab复制sys_d = c2d(sys_c, Ts, 'zoh');
[Kd,Sd,ed] = dlqr(Ad,Bd,Q,R);
在我们的实现中，控制周期选择为50ms（20Hz），这是一个兼顾实时性和计算负载的折中选择。太高的频率会导致计算资源紧张，太低则会影响控制性能。
4. 拦截任务全流程控制
4.1 任务阶段划分
完整的拦截任务可分为六个阶段：
- 待命状态：无人机停放在巢穴，控制系统处于低功耗模式
- 起飞阶段：接收到拦截指令后垂直起飞至安全高度（通常5-10米）
- 接近阶段：根据目标位置信息规划路径，快速接近入侵无人机
- 追踪阶段：进入视觉锁定模式，保持相对距离并调整捕获姿态
- 捕获阶段：到达最佳位置后触发捕获装置
- 返回阶段：携带目标返回巢穴并降落
4.2 多模态控制器设计
针对不同阶段设计相应的控制策略：
| 阶段 | 控制模式 | 主要目标 | 关键参数 |
|---|---|---|---|
| 起飞 | 高度控制 | 快速稳定爬升 | 上升速度1.5m/s |
| 接近 | 位置控制 | 最小化抵达时间 | 最大倾斜角30° |
| 追踪 | 视觉伺服 | 保持相对位置 | 距离误差<0.5m |
| 捕获 | 精确悬停 | 稳定发射姿态 | 角度误差<3° |
| 返回 | 路径跟随 | 安全返航 | 速度2m/s |
LQR控制器在不同模式下通过调整Q矩阵参数实现控制重点的切换。例如在追踪阶段，我们会增大角度状态的权重，确保捕获时的姿态稳定。
4.3 路径规划与避障
采用改进的RRT*算法进行全局路径规划，主要考虑：
- 空域限制（如禁飞区、高度限制）
- 障碍物分布（通过先验地图或实时感知）
- 能量最优（尽量减少不必要的机动）
局部避障使用人工势场法，实时调整路径：
matlab复制function F = potential_field(q, q_goal, obstacles)
% 引力计算
F_att = k_att * (q_goal - q);
% 斥力计算
F_rep = zeros(3,1);
for i = 1:size(obstacles,2)
d = norm(q - obstacles(:,i));
if d < r_rep
F_rep = F_rep + k_rep*(1/d - 1/r_rep)*(1/d^2)*(q - obstacles(:,i))/d;
end
end
F = F_att + F_rep;
end
5. 系统集成与Matlab实现
5.1 Simulink模型架构
完整的控制系统在Simulink中实现，主要模块包括：
- 四旋翼动力学模型：基于6DOF方程搭建的受控对象
- LQR控制器：包含状态观测器和增益矩阵
- 任务管理器：状态机实现阶段转换
- 传感器仿真：模拟GPS、IMU和视觉数据
- 环境模型：包含障碍物和移动目标
5.2 核心代码解析
LQR控制器实现：
matlab复制function [u, x_hat] = lqr_controller(y, ref, dt)
persistent K x_hat_prev
if isempty(K)
% 初始化LQR增益
[A,B] = get_linear_model();
Q = diag([10 10 20 50 50 30 1 1 2 5 5 3]);
R = diag([0.1 0.5 0.5 0.5]);
[K,~,~] = lqr(A,B,Q,R);
x_hat_prev = zeros(12,1);
end
% 状态估计（简化版卡尔曼滤波）
x_hat = state_estimator(y, x_hat_prev, dt);
% 控制量计算
u = -K * (x_hat - ref);
% 限幅处理
u(1) = max(0, min(u(1), 15)); % 总推力限制
u(2:4) = max(-5, min(u(2:4), 5)); % 力矩限制
x_hat_prev = x_hat;
end
状态机实现：
matlab复制function next_state = mission_fsm(current_state, sensors)
switch current_state
case 'IDLE'
if sensors.intruder_detected
next_state = 'TAKEOFF';
else
next_state = 'IDLE';
end
case 'TAKEOFF'
if abs(sensors.z - 5) < 0.2
next_state = 'APPROACH';
else
next_state = 'TAKEOFF';
end
% 其他状态转换...
end
end
5.3 仿真结果分析
通过Matlab仿真验证了以下性能指标：
- 稳定悬停：在5级风扰下，位置保持误差<0.3m
- 机动性能：从0加速到5m/s耗时1.2s
- 捕获成功率：在目标速度<3m/s时成功率>90%
- 能耗表现：完整拦截任务平均耗电1500mAh
仿真中还发现几个关键现象：
- 权重矩阵Q中对角度误差的惩罚需要足够大，否则在捕获阶段会出现晃动
- 控制量限幅对防止执行器饱和至关重要
- 状态估计的准确性直接影响控制性能
6. 实际部署中的挑战与解决方案
6.1 风扰影响与补偿
在实际户外测试中，风扰是影响拦截精度的主要因素。我们采用两种补偿策略：
-
前馈补偿：通过机载气压计和IMU估计风场
matlab复制
wind_est = lowpass_filter(accel_measure - accel_cmd); u_ff = wind_est * wind_gain; -
自适应LQR：根据风场强度调整Q矩阵
- 大风时增加速度状态权重
- 减小角度权重以避免过度反应
6.2 目标丢失处理
视觉追踪可能因遮挡或光照变化中断，系统采用以下恢复策略：
- 记忆轨迹预测：基于最近5秒的运动数据预测目标位置
- 螺旋搜索模式：以最后已知位置为中心展开搜索
- 多重确认机制：需连续3帧检测到目标才确认重新捕获
6.3 能量管理与应急策略
为确保安全，设计了多级能量管理策略：
-
电量监测：实时计算剩余航时
matlab复制
flight_time_remaining = (battery_capacity - battery_used) / average_current; -
分级警报：
- 30%电量：缩短最大拦截距离
- 20%电量：中止任务立即返航
- 15%电量：寻找最近安全点迫降
-
应急降落：GPS失效时使用光流+气压计维持基本悬停
7. 关键参数调试经验
经过大量实地测试，总结出以下调试经验：
-
LQR权重调整黄金法则：
- 先调角度相关权重确保基本稳定
- 再调位置权重达到期望的跟踪性能
- 最后微调速度权重平滑运动
-
捕获阶段特殊处理：
matlab复制if strcmp(mission_phase, 'CAPTURE') Q(4:6,4:6) = Q(4:6,4:6) * 3; % 增大角度权重 R = R * 0.7; % 允许更大控制量 end -
滤波器参数选择：
- 状态估计：卡尔曼滤波过程噪声Q_kf=0.01，观测噪声R_kf=0.1
- 风速估计：低通滤波器截止频率1Hz
-
机械参数影响：
- 网弹发射时机：目标距离3±0.5m时成功率最高
- 最佳发射角度：俯角15-20度（考虑重力影响）
8. 扩展与优化方向
当前系统仍有多个可改进方向：
-
多机协同拦截：
- 主从式编队控制
- 分布式任务分配
-
智能识别升级：
- 基于深度学习的无人机类型识别
- 行为预测算法
-
控制算法增强：
matlab复制% 结合LQR与MPC的混合控制器 if norm(position_error) > 2 use_mpc(); % 大误差时用MPC else use_lqr(); % 小误差时用LQR end -
能源系统优化：
- 无线充电巢穴
- 太阳能辅助供电
-
抗干扰能力提升：
- GPS欺骗检测
- 通信加密增强