四旋翼无人机MPC控制算法实现与Matlab仿真

1. 四旋翼飞行器与MPC算法概述

四旋翼飞行器作为典型的欠驱动系统，其控制问题一直是无人机领域的研究热点。与传统PID控制相比，模型预测控制（MPC）算法能够显式处理系统约束，更适合处理多目标航点导航这类复杂任务。我在实际项目中发现，MPC算法对四旋翼的姿态和位置控制能实现更好的动态性能。

Matlab作为工程计算的标准工具，其Model Predictive Control Toolbox提供了完整的MPC设计环境。但要将理论算法转化为实际可运行的代码，需要解决几个关键问题：如何建立准确的动力学模型、如何处理实时性要求、如何设计合理的代价函数等。

2. 系统建模与问题描述

2.1 四旋翼动力学模型

四旋翼的六自由度动力学模型可表示为：

matlab复制% 状态方程示例
function dx = quadcopterDynamics(x, u)
    % x: [x,y,z, vx,vy,vz, phi,theta,psi, p,q,r]
    % u: [F1,F2,F3,F4] 四个电机的推力
    g = 9.81; m = 1.2; l = 0.25; 
    Ixx = 0.034; Iyy = 0.034; Izz = 0.06;
    
    % 位置动力学
    dx(1:3) = x(4:6);
    dx(4) = (sin(x(9))*sin(x(7)) + cos(x(9))*sin(x(8))*cos(x(7)))*sum(u)/m;
    dx(5) = (-cos(x(9))*sin(x(7)) + sin(x(9))*sin(x(8))*cos(x(7)))*sum(u)/m;
    dx(6) = -g + (cos(x(8))*cos(x(7)))*sum(u)/m;
    
    % 姿态动力学
    dx(7:9) = [1 sin(x(7))*tan(x(8)) cos(x(7))*tan(x(8)); 
               0 cos(x(7))            -sin(x(7));
               0 sin(x(7))/cos(x(8)) cos(x(7))/cos(x(8))] * x(10:12);
    
    tau_phi = l*(u(2)-u(4));
    tau_theta = l*(u(3)-u(1));
    tau_psi = 0.1*(u(1)-u(2)+u(3)-u(4));  % 空气阻力矩系数
    
    dx(10) = (tau_phi - (Izz-Iyy)*x(11)*x(12))/Ixx;
    dx(11) = (tau_theta - (Ixx-Izz)*x(10)*x(12))/Iyy;
    dx(12) = (tau_psi - (Iyy-Ixx)*x(10)*x(11))/Izz;
end

注意：实际实现时需要处理欧拉角奇异点问题，可考虑改用四元数表示

2.2 多目标航点导航问题

多目标航点导航的核心是设计一个能顺序通过预设航点的控制器。MPC算法的优势在于可以将未来多个航点信息纳入当前控制决策。代价函数通常设计为：

code复制J = Σ(||x(k)-xref||²_Q + ||u(k)||²_R) + Σ||x(ki)-wi||²_P

其中wi是第i个航点位置，ki是对应的时间节点。

3. MPC控制器设计与实现

3.1 Matlab MPC工具箱配置

matlab复制% 创建MPC控制器对象
Ts = 0.1;  % 采样时间
predictionHorizon = 20;
controlHorizon = 5;

mpcobj = mpc(linearizedModel, Ts, predictionHorizon, controlHorizon);

% 设置约束
mpcobj.MV(1).Min = 0;   % 电机最小推力
mpcobj.MV(1).Max = 15;  % 电机最大推力
mpcobj.MV(1).RateMin = -5; % 推力变化率限制
mpcobj.MV(1).RateMax = 5;

% 设置权重
mpcobj.Weights.OutputVariables = [10 10 10 1 1 1 5 5 5 1 1 1];
mpcobj.Weights.ManipulatedVariables = [0.1 0.1 0.1 0.1];
mpcobj.Weights.ManipulatedVariablesRate = [0.01 0.01 0.01 0.01];

3.2 航点切换策略

实现多航点导航的关键是动态更新MPC的参考信号。我推荐使用有限状态机管理航点切换：

matlab复制function [ref, currentWP] = updateWaypoint(x, waypoints, currentWP, threshold)
    % x: 当前状态
    % waypoints: 所有航点数组[Nx3]
    % currentWP: 当前目标航点索引
    % threshold: 切换距离阈值
    
    if norm(x(1:3) - waypoints(currentWP,:)) < threshold
        currentWP = mod(currentWP, size(waypoints,1)) + 1;
    end
    ref = waypoints(currentWP,:);
end

4. 仿真实现与结果分析

4.1 闭环仿真框架

matlab复制% 初始化
T = 20; % 总仿真时间
N = T/Ts; % 仿真步数
x = zeros(12,N); % 状态记录
u = zeros(4,N-1); % 控制输入记录
x(:,1) = x0; % 初始状态

% 航点设置
waypoints = [0 0 1; 
             2 0 1; 
             2 2 1.5; 
             0 2 1; 
             0 0 1];
currentWP = 1;

% 主循环
for k = 1:N-1
    % 更新参考信号
    [ref, currentWP] = updateWaypoint(x(:,k), waypoints, currentWP, 0.2);
    
    % MPC计算控制量
    u(:,k) = mpcmove(mpcobj, x(:,k), ref);
    
    % 状态更新(使用实际非线性模型)
    x(:,k+1) = integrateQuadcopter(x(:,k), u(:,k), Ts);
end

4.2 典型结果分析

通过调整MPC的预测时域和控制时域，可以观察到不同的控制效果：

实际应用中需要在性能和实时性之间权衡，通常选择PH=15-25，CH=3-8

5. 实现中的关键问题与解决方案

5.1 实时性优化

MPC的在线优化计算量较大，我通过以下方法提升实时性：

1. 使用线性时不变(LTI)近似模型代替非线性模型
2. 采用热启动(warm start)技术重用上一时刻的解
3. 使用Matlab Coder将关键代码转换为C/C++

matlab复制% 代码生成配置
cfg = coder.config('lib');
cfg.GenerateReport = true;
codegen('mpcQuadcopterStep', '-config', cfg, '-args', {coder.typeof(x0), coder.typeof(ref)});

5.2 模型失配补偿

实际系统与模型存在差异时，可采取：

1. 添加扰动观测器
2. 在线更新模型参数
3. 自适应MPC权重调整

matlab复制function [mpcobj] = adaptMPCWeights(mpcobj, trackingError)
    % 根据跟踪误差自适应调整权重
    if trackingError > 0.5
        mpcobj.Weights.OutputVariables(1:3) = 20; % 增加位置权重
    else
        mpcobj.Weights.OutputVariables(1:3) = 10;
    end
end

6. 扩展应用与进阶方向

基于这个基础框架，可以进一步实现：

1. 避障功能：在代价函数中添加障碍物排斥项
2. 编队控制：为每架无人机设计耦合的MPC代价函数
3. 视觉伺服：结合图像特征作为MPC的输出量

matlab复制% 避障代价函数示例
function J = obstacleCost(x, obstacles)
    J = 0;
    for i = 1:size(obstacles,1)
        dist = norm(x(1:3) - obstacles(i,:));
        J = J + 100*exp(-dist/0.5);  % 高斯型排斥场
    end
end

在实际飞行测试中，我发现MPC参数需要根据具体机型调整。例如小型四旋翼由于惯性小，需要更短的采样时间和更严格的输入变化率限制。建议先在仿真中充分验证，再逐步移植到真实平台。