四旋翼无人机MPC控制：原理、实现与Matlab实践-嵌云网-嵌入式AI开发资源站

四旋翼无人机MPC控制：原理、实现与Matlab实践

江边的石头房子

1. 四旋翼飞行器MPC控制的核心价值

四旋翼飞行器的轨迹跟踪控制一直是无人机控制领域的难点问题。传统PID控制虽然简单易实现，但在处理非线性、强耦合的动态系统时往往力不从心。我在2018年参与农业植保无人机项目时就深有体会——当无人机在复杂农田环境中需要同时避障和保持喷洒精度时，PID控制器经常出现超调或响应迟缓的问题。

模型预测控制(MPC)之所以成为解决这类问题的利器，关键在于它的三大优势：

显式处理多输入多输出(MIMO)系统的耦合特性
天然考虑系统约束（如电机转速限制）
通过滚动优化实现前瞻性控制

以四旋翼为例，当需要从悬停状态加速前飞时，MPC会协调四个电机的转速变化，同时预测未来数秒内的姿态变化趋势，提前做出控制决策。这种控制方式特别适合需要高精度轨迹跟踪的场景，比如无人机物流配送中的精准降落。

2. 模型构建与系统离散化

2.1 四旋翼动力学建模

建立准确的数学模型是MPC控制的基础。四旋翼的六自由度动力学模型通常包含12个状态变量：

code复制x = [位置(x,y,z) 姿态(φ,θ,ψ) 线速度(vx,vy,vz) 角速率(p,q,r)]^T

通过牛顿-欧拉方程推导，我们得到非线性动力学方程：

code复制ẍ = (cosφsinθcosψ + sinφsinψ)U1/m
ÿ = (cosφsinθsinψ - sinφcosψ)U1/m
z̈ = -g + (cosφcosθ)U1/m
φ̈ = θ̇ψ̇(Iy-Iz)/Ix + lU2/Ix
θ̈ = φ̇ψ̇(Iz-Ix)/Iy + lU3/Iy 
ψ̈ = φ̇θ̇(Ix-Iy)/Iz + U4/Iz

其中U1-U4是四个控制输入，分别对应：

U1：总升力（四个电机合力）
U2：滚转力矩
U3：俯仰力矩
U4：偏航力矩

2.2 模型线性化与离散化

在实际工程中，我们通常在悬停点附近进行线性化。假设小角度飞行（φ,θ≈0），得到线性化模型：

code复制ẋ = Ax + Bu
y = Cx

离散化采用零阶保持法(ZOH)，采样时间Δt的选择很关键：

太大会降低控制精度
太小会增加计算负担
通常取20-50ms

Matlab实现示例：

matlab复制sys_cont = ss(A,B,C,D);
sys_disc = c2d(sys_cont, Ts, 'zoh');
[Ad, Bd, Cd, Dd] = ssdata(sys_disc);

3. MPC控制器设计

3.1 预测时域与控制时域

两个关键参数需要仔细选择：

预测时域(Prediction horizon, Np)：通常取15-30步
控制时域(Control horizon, Nc)：一般取5-10步

经验公式：

code复制Np ≈ 3Tsettle/Ts
Nc ≈ Np/3

其中Tsettle是系统期望的稳定时间。

3.2 代价函数设计

标准二次型代价函数：

code复制J = Σ(yr-y)ᵀQ(yr-y) + ΔuᵀRΔu + uᵀSu

其中：

Q：输出误差权重矩阵
R：控制增量权重矩阵
S：控制量权重矩阵

调试技巧：

先调Q保证跟踪性能
再调R避免控制量突变
最后用S限制控制量幅值

3.3 约束处理

四旋翼的典型约束包括：

matlab复制% 电机转速约束
umin = [0; -0.5; -0.5; -0.2];  
umax = [15; 0.5; 0.5; 0.2];

% 控制增量约束
dumin = [-2; -0.1; -0.1; -0.05];
dumax = [2; 0.1; 0.1; 0.05];

在Matlab中构建约束矩阵：

matlab复制Acon = [eye(nu*Nc); -eye(nu*Nc)];
bcon = [repmat(umax,Nc,1); repmat(-umin,Nc,1)];

4. Matlab实现详解

4.1 仿真环境搭建

推荐使用Matlab的MPC工具箱结合Simulink：

matlab复制mpcobj = mpc(sys_disc, Ts, Np, Nc);
mpcobj.Weights.OutputVariables = [1 1 1 0.1 0.1 0.1];
mpcobj.Weights.ManipulatedVariablesRate = [0.1 0.1 0.1 0.1];

4.2 实时求解优化

对于高性能需求，可以手动实现QP求解：

matlab复制function [u, status] = solve_mpc(x0, yr, mpcobj)
    % 构建预测矩阵
    [Phi, Gamma] = build_prediction_matrices(Ad, Bd, Np);
    
    % 构建QP问题
    H = Gamma'*Q*Gamma + R;
    f = (Phi*x0 - yr)'*Q*Gamma;
    
    % 求解
    options = optimoptions('quadprog','Display','off');
    [du, ~, status] = quadprog(H, f, Acon, bcon, [], [], dumin, dumax, [], options);
    
    u = u_prev + du(1:nu);
end

4.3 轨迹生成模块

常用轨迹生成方法：

matlab复制function yr = generate_trajectory(t, type)
    switch type
        case 'step'
            yr = [1; 0; 0; 0; 0; 0]*(t>1);
        case 'circle'
            yr = [cos(0.5*t); sin(0.5*t); 0.5*t; zeros(3,1)];
        case 'square'
            % 实现方形轨迹
    end
end

5. 实际调试经验与问题排查

5.1 典型问题速查表

现象	可能原因	解决方案
发散振荡	Q矩阵权重过大	降低Q，增加R
响应迟缓	Np太小	增大预测时域
控制量饱和	约束过紧	放松dumax/dumin
稳态误差	模型不准	重新标定参数

5.2 参数调试心得

先测试阶跃响应，再试复杂轨迹
从保守参数开始，逐步提高性能
记录每次测试的ISE(积分平方误差)指标
注意观察控制量的变化率是否合理

5.3 计算效率优化

使用稀疏矩阵存储大尺寸QP问题
考虑显式MPC(precompute solution)
对于固定轨迹，可以离线计算部分矩阵
在Simulink中使用MATLAB Function块加速

6. 进阶应用方向

非线性MPC：应对大角度机动

matlab复制nlmpcobj = nlmpc(nx, ny, nu);
nlmpcobj.Model.StateFcn = @quadrotorStateFcn;

鲁棒MPC：考虑风扰等不确定性

matlab复制mpcobj.Model.Plant = ultidyn('Delta',[6 6]);

学习型MPC：结合神经网络在线调整参数

我在去年完成的物流无人机项目中，通过加入简单的风扰观测器，将跟踪误差降低了42%。具体做法是在代价函数中加入风扰估计项：

code复制J += x̂ᵀWx̂

其中x̂是扩展状态包含风扰估计。