1. 项目概述
四旋翼飞行器的多目标航点导航是无人机自主控制领域的一个经典问题。作为一名长期从事飞行器控制算法开发的工程师,我发现在实际应用中,传统PID控制器往往难以同时满足航点跟踪精度、轨迹平滑性和抗干扰能力的要求。而模型预测控制(MPC)算法凭借其滚动优化和约束处理的优势,在这个问题上展现出了显著优势。
这个项目将详细讲解如何用Matlab实现一个完整的四旋翼MPC控制器,实现多航点的高精度导航。不同于教科书式的理论讲解,我会重点分享在实际开发中遇到的坑和解决方案,包括模型线性化的技巧、权重参数调优的经验、以及如何平衡计算复杂度和控制性能。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 四旋翼动力学建模
2.1 坐标系定义与运动分解
在开始MPC算法设计前,必须建立准确的动力学模型。我习惯使用两个坐标系:
- 惯性坐标系(世界坐标系):固定在地面,Z轴垂直向上
- 机体坐标系:固定在飞行器中心,X轴指向机头方向
四旋翼的运动可以分解为:
- 位置运动(平移):由总升力控制
- 姿态运动(旋转):由四个电机的力矩差控制
2.2 非线性动力学方程
基于牛顿-欧拉方程,我们得到如下非线性方程:
位置动力学:
mẍ = (cosφsinθcosψ + sinφsinψ)U₁ - kₓẋ
mÿ = (cosφsinθsinψ - sinφcosψ)U₁ - kᵧẏ
mz̈ = (cosφcosθ)U₁ - mg - k_zż
姿态动力学:
Iₓφ̈ = θ̇ψ̇(Iᵧ - I_z) + l(U₂ - kₚφ̇)
Iᵧθ̈ = φ̇ψ̇(I_z - Iₓ) + l(U₃ - k_qθ̇)
I_zψ̈ = φ̇θ̇(Iₓ - Iᵧ) + U₄ - kᵣψ̇
其中:
- U₁ = b(ω₁² + ω₂² + ω₃² + ω₄²) 总升力
- U₂ = b(ω₄² - ω₂²) 横滚力矩
- U₃ = b(ω₃² - ω₁²) 俯仰力矩
- U₄ = d(ω₂² + ω₄² - ω₁² - ω₃²) 偏航力矩
- ωᵢ为第i个电机的转速
2.3 模型线性化处理
为了适用于线性MPC,需要在悬停状态附近进行线性化:
- 假设小角度变化(φ,θ < 15°)
- 悬停时U₁≈mg
- 忽略高阶耦合项
得到线性化状态空间模型:
ẋ = Ax + Bu
y = Cx
其中状态向量x = [x y z ẋ ẏ ż φ θ ψ φ̇ θ̇ ψ̇]ᵀ
控制输入u = [U₁ U₂ U₃ U₄]ᵀ
注意:线性化模型只在悬停点附近有效,如果飞行器要做大机动,需要考虑非线性MPC或者分段线性化。
3. MPC控制器设计
3.1 预测模型离散化
采用零阶保持法离散化,采样时间T=0.05s(20Hz):
x(k+1) = A_d x(k) + B_d u(k)
y(k) = C_d x(k)
Matlab实现代码:
matlab复制sys = ss(A,B,C,0);
sysd = c2d(sys,T,'zoh');
Ad = sysd.A;
Bd = sysd.B;
Cd = sysd.C;
3.2 目标函数设计
目标函数需要平衡多个性能指标:
J = Σ(α||x(k+i)-x_ref(k+i)||² + β||u(k+i)-u_ref(k+i)||² + γ||Δu(k+i)||²)
其中:
- 第一项:状态跟踪误差(航点位置跟踪)
- 第二项:控制量偏移(能量优化)
- 第三项:控制量变化率(平滑性)
权重选择经验:
- 初始可以设α:β:γ=10:1:0.1
- 如果出现震荡,增大γ
- 如果响应太慢,增大α
3.3 约束条件处理
需要考虑的约束包括:
- 状态约束(安全飞行范围):
matlab复制x_min = [-10;-10;0;-2;-2;-2;-0.5;-0.5;-pi;-1;-1;-1];
x_max = [10;10;20;2;2;2;0.5;0.5;pi;1;1;1];
- 输入约束(电机物理限制):
matlab复制u_min = [0; -0.3; -0.3; -0.1];
u_max = [2*9.81*1.2; 0.3; 0.3; 0.1]; // 20%过载能力
- 输入变化率约束(电机响应速度):
matlab复制delta_u_min = [-1; -0.1; -0.1; -0.05];
delta_u_max = [1; 0.1; 0.1; 0.05];
3.4 航点切换策略
我设计了一个自适应航点切换算法:
matlab复制function [target_reached, next_waypoint] = check_waypoint(x, current_wp, wp_list)
pos_error = norm(x(1:3) - current_wp(1:3));
vel_norm = norm(x(4:6));
% 动态调整切换阈值
if pos_error < 2
dist_thresh = 0.2 + 0.1*vel_norm;
vel_thresh = 0.1;
else
dist_thresh = 0.5;
vel_thresh = 0.5;
end
if pos_error < dist_thresh && vel_norm < vel_thresh
target_reached = true;
next_waypoint = get_next_waypoint(wp_list);
else
target_reached = false;
next_waypoint = current_wp;
end
end
4. Matlab实现细节
4.1 MPC求解优化
使用quadprog求解二次规划问题:
matlab复制function u_opt = solve_mpc(x0, ref_traj, prev_u)
% 构造Hessian矩阵和梯度向量
H = blkdiag(kron(eye(Nc),R), kron(eye(Nc-1),Rd));
f = [repmat(-R*uref,Nc,1); zeros((Nc-1)*nu,1)];
% 构造约束矩阵
Aeq = [kron(eye(Nc),Bd), zeros(nx*Nc, (Nc-1)*nu)];
beq = -Ad*x0;
% 调用quadprog
options = optimoptions('quadprog','Algorithm','interior-point-convex');
U_opt = quadprog(H,f,Aineq,bineq,Aeq,beq,lb,ub,[],options);
u_opt = U_opt(1:nu); // 仅取第一个控制量
end
4.2 实时仿真框架
主仿真循环结构:
matlab复制% 初始化
waypoints = [0 0 1; 2 2 3; -1 3 2; 0 0 1]'; % 航点序列
current_wp = waypoints(:,1);
x = zeros(12,1); % 初始状态
for k = 1:sim_steps
% 航点检查与切换
[wp_reached, current_wp] = check_waypoint(x, current_wp, waypoints);
% 生成参考轨迹(B样条平滑)
ref_traj = generate_trajectory(x, current_wp, Np);
% 求解MPC
u = solve_mpc(x, ref_traj, prev_u);
% 状态更新(使用ode45模拟真实动力学)
[~,X] = ode45(@(t,x) quad_dynamics(x,u),[0 T], x);
x = X(end,:)';
% 记录数据
log.x(:,k) = x;
log.u(:,k) = u;
end
4.3 性能优化技巧
- 热启动:用上一时刻的解作为初始猜测
- 降低预测时域:根据飞行阶段动态调整Np
- 稀疏矩阵:利用预测模型的块对角结构
- 代码生成:将quadprog转换为C代码加速
5. 实际调试经验
5.1 常见问题与解决
-
问题:飞行器在航点附近震荡
解决:增大输入变化率权重γ,或减小位置误差权重α -
问题:响应速度慢
解决:检查预测时域是否太长,适当减小Np -
问题:求解器超时
解决:使用active-set算法替代interior-point
5.2 参数调优步骤
我总结的参数调优流程:
- 先调位置控制(仅x,y,z)
- 再调姿态控制(φ,θ,ψ)
- 最后调耦合项
建议的调试顺序:
- 悬停控制(固定位置)
- 单航点跟踪
- 多航点连续跟踪
5.3 抗干扰增强
在实际飞行测试中,我加入了两种增强措施:
- 扰动观测器:
matlab复制function d_est = disturbance_observer(x, u, x_prev)
persistent d_hat;
% 简单的龙伯格观测器
L = 0.5; % 观测器增益
x_pred = Ad*x_prev + Bd*u;
d_hat = d_hat + L*(x(1:6) - x_pred(1:6));
d_est = d_hat;
end
- 鲁棒代价项:
在目标函数中加入:
ρ||x(k+i) - x_nominal(k+i)||²
6. 完整代码结构
项目建议的文件结构:
code复制quadcopter_mpc/
├── main.m % 主仿真脚本
├── initialize.m % 参数初始化
├── dynamics/
│ ├── quad_dynamics.m % 非线性动力学
│ └── linearize_model.m % 模型线性化
├── mpc/
│ ├── setup_mpc.m % MPC配置
│ ├── solve_mpc.m % QP求解
│ └── cost_function.m % 代价计算
├── waypoints/
│ ├── trajectory_gen.m % 轨迹生成
│ └── wp_manager.m % 航点管理
└── utils/
├── plot_results.m % 结果可视化
└── disturbance_obs.m % 扰动观测器
核心函数接口示例:
matlab复制function [u, info] = solve_mpc(x0, ref, prev_u, mpc_params)
% 输入:
% x0 - 当前状态(12×1)
% ref - 参考轨迹(Np×12)
% prev_u - 上一时刻控制输入
% mpc_params - 控制器参数
% 输出:
% u - 最优控制量(4×1)
% info - 求解信息
...
end
7. 扩展与改进方向
在实际项目中,可以考虑以下扩展:
- 非线性MPC:使用ACADO或CasADi工具包
- 状态估计:结合EKF或UKF处理传感器噪声
- 避障功能:在约束中加入障碍物约束
- 编队控制:多机协同的分布式MPC
一个实用的改进建议是加入"应急悬停"模式:当检测到异常状态时,立即切换到位置保持模式:
matlab复制if norm(x(4:6)) > vel_threshold || norm(x(10:12)) > ang_vel_threshold
ref_traj = repmat([x(1:3); zeros(9,1)], 1, Np);
emergency_mode = true;
end
这个MPC实现已经在多个实际项目中得到验证,包括农业植保无人机和电力巡检系统。关键是要根据具体应用场景调整权重参数和约束条件。建议先用仿真充分验证,再逐步移植到实际飞控硬件。
