无人机离散系统模型预测控制(MPC)MATLAB实现

一、系统概述

模型预测控制(MPC) 是一种先进控制策略,通过在线求解有限时域优化问题来生成控制输入。对于无人机系统,MPC能够有效处理:

  • 多变量耦合:位置、姿态、速度的强耦合

  • 状态/输入约束:电机转速限制、姿态角限制

  • 轨迹跟踪:精确跟踪期望轨迹

  • 抗干扰能力:应对风扰等外部干扰

离散系统优势:无人机控制器通常基于微处理器实现,离散MPC更适合数字实现,计算效率高。

二、无人机离散系统建模

2.1 四旋翼无人机连续时间模型

状态变量(12维):

x=[x,y,z,x˙,y˙,z˙,ϕ,θ,ψ,p,q,r]Tx=[x,y,z,\dot{x},\dot{y},\dot{z},ϕ,θ,ψ,p,q,r]^Tx=[x,y,z,x˙,y˙,z˙,ϕ,θ,ψ,p,q,r]T

  • 位置 (x,y,zx,y,zx,y,z),速度 (x˙,y˙,z˙\dot{x},\dot{y},\dot{z}x˙,y˙,z˙)

  • 欧拉角 (ϕ,θ,ψϕ,θ,ψϕ,θ,ψ):滚转、俯仰、偏航

  • 角速度 (p,q,rp,q,rp,q,r):机体坐标系下的角速度

控制输入(4维):

u=[ω12,ω22,ω32,ω42]Tu=[ω_1^2,ω_2^2,ω_3^2,ω_4^2]Tu=[ω12,ω22,ω32,ω42]T

  • 四个电机的转速平方(与升力成正比)

非线性动力学方程

matlab 复制代码
% 四旋翼非线性模型(连续时间)
function dx = quadrotor_nonlinear(t, x, u, params)
    % 参数
    m = params.m;      % 质量 (kg)
    g = params.g;      % 重力加速度 (m/s^2)
    Ixx = params.Ixx;  % 转动惯量
    Iyy = params.Iyy;
    Izz = params.Izz;
    kf = params.kf;    % 升力系数
    km = params.km;    % 力矩系数
    L = params.L;      % 臂长 (m)
    
    % 状态提取
    pos = x(1:3);      % 位置
    vel = x(4:6);      % 速度
    euler = x(7:9);    % 欧拉角
    omega = x(10:12);  % 角速度
    
    % 控制输入(电机转速平方)
    w_sq = u;
    
    % 总升力
    F_total = kf * sum(w_sq);
    
    % 机体坐标系下的力矩
    tau = [
        L*kf*(w_sq(2) - w_sq(4));      % 滚转力矩
        L*kf*(w_sq(3) - w_sq(1));      % 俯仰力矩
        km*(w_sq(1) - w_sq(2) + w_sq(3) - w_sq(4))  % 偏航力矩
    ];
    
    % 旋转矩阵(从机体到惯性系)
    phi = euler(1); theta = euler(2); psi = euler(3);
    R = [
        cos(theta)*cos(psi), sin(phi)*sin(theta)*cos(psi)-cos(phi)*sin(psi), cos(phi)*sin(theta)*cos(psi)+sin(phi)*sin(psi);
        cos(theta)*sin(psi), sin(phi)*sin(theta)*sin(psi)+cos(phi)*cos(psi), cos(phi)*sin(theta)*sin(psi)-sin(phi)*cos(psi);
        -sin(theta), sin(phi)*cos(theta), cos(phi)*cos(theta)
    ];
    
    % 动力学方程
    dpos = vel;
    dvel = [0; 0; -g] + (1/m) * R * [0; 0; F_total];
    
    % 欧拉角微分
    T = [
        1, sin(phi)*tan(theta), cos(phi)*tan(theta);
        0, cos(phi), -sin(phi);
        0, sin(phi)/cos(theta), cos(phi)/cos(theta)
    ];
    deuler = T * omega;
    
    % 角速度微分
    domega = [
        (tau(1) + (Iyy - Izz)*omega(2)*omega(3))/Ixx;
        (tau(2) + (Izz - Ixx)*omega(3)*omega(1))/Iyy;
        (tau(3) + (Ixx - Iyy)*omega(1)*omega(2))/Izz
    ];
    
    dx = [dpos; dvel; deuler; domega];
end

2.2 线性化与离散化

线性化点选择:悬停状态

线性化方法

matlab 复制代码
% 线性化无人机模型
function [A, B] = linearize_quadrotor(x0, u0, params, dt)
    % 使用数值微分计算雅可比矩阵
    n = length(x0);  % 状态维度
    m = length(u0);  % 输入维度
    
    % 计算f(x0, u0)
    f0 = quadrotor_nonlinear(0, x0, u0, params);
    
    % 计算A矩阵(对状态求导)
    A = zeros(n, n);
    epsilon = 1e-6;
    for i = 1:n
        x_pert = x0;
        x_pert(i) = x_pert(i) + epsilon;
        f_pert = quadrotor_nonlinear(0, x_pert, u0, params);
        A(:, i) = (f_pert - f0) / epsilon;
    end
    
    % 计算B矩阵(对输入求导)
    B = zeros(n, m);
    for i = 1:m
        u_pert = u0;
        u_pert(i) = u_pert(i) + epsilon;
        f_pert = quadrotor_nonlinear(0, x0, u_pert, params);
        B(:, i) = (f_pert - f0) / epsilon;
    end
    
    % 离散化(零阶保持)
    sysc = ss(A, B, eye(n), zeros(n, m));
    sysd = c2d(sysc, dt, 'zoh');
    A = sysd.A;
    B = sysd.B;
end

三、模型预测控制器设计

3.1 MPC问题 formulation

优化问题

约束

3.2 MPC求解器实现

matlab 复制代码
classdef QuadrotorMPC < handle
    % 无人机MPC控制器类
    
    properties
        % 系统参数
        A; B; C;      % 离散状态空间矩阵
        nx; nu; ny;   % 状态、输入、输出维度
        
        % MPC参数
        Np;           % 预测时域
        Nc;           % 控制时域
        Q; R; S;      % 权重矩阵
        
        % 约束
        x_min; x_max; % 状态约束
        u_min; u_max; % 输入约束
        du_min; du_max; % 输入变化率约束
        
        % 优化问题参数
        H; f;         % 二次规划参数
        Aineq; bineq; % 不等式约束
        Aeq; beq;     % 等式约束
        lb; ub;       % 边界约束
    end
    
    methods
        function obj = QuadrotorMPC(A, B, C, Np, Nc, Q, R, S)
            % 构造函数
            obj.A = A; obj.B = B; obj.C = C;
            obj.nx = size(A, 1);
            obj.nu = size(B, 2);
            obj.ny = size(C, 1);
            
            obj.Np = Np;
            obj.Nc = Nc;
            obj.Q = Q;
            obj.R = R;
            obj.S = S;
            
            % 初始化约束(默认无约束)
            obj.x_min = -inf(obj.nx, 1);
            obj.x_max = inf(obj.nx, 1);
            obj.u_min = zeros(obj.nu, 1);
            obj.u_max = 1000 * ones(obj.nu, 1);  % 电机最大转速平方
            obj.du_min = -100 * ones(obj.nu, 1);
            obj.du_max = 100 * ones(obj.nu, 1);
            
            % 构建优化问题
            obj.build_qp_matrices();
        end
        
        function build_qp_matrices(obj)
            % 构建二次规划矩阵
            
            % 扩展系统矩阵
            Psi = zeros(obj.ny*obj.Np, obj.nx);
            Gamma = zeros(obj.ny*obj.Np, obj.nu*obj.Nc);
            Theta = zeros(obj.ny*obj.Np, obj.nu*obj.Nc);
            
            % 构建Psi矩阵(状态预测)
            for i = 1:obj.Np
                Psi((i-1)*obj.ny+1:i*obj.ny, :) = obj.C * (obj.A^i);
            end
            
            % 构建Gamma和Theta矩阵(输入预测)
            for i = 1:obj.Np
                for j = 1:min(i, obj.Nc)
                    Gamma((i-1)*obj.ny+1:i*obj.ny, (j-1)*obj.nu+1:j*obj.nu) = ...
                        obj.C * (obj.A^(i-j)) * obj.B;
                end
            end
            
            % 构建Theta矩阵(输入变化率)
            Theta(1:obj.ny*obj.Nc, 1:obj.nu*obj.Nc) = kron(tril(ones(obj.Nc)), eye(obj.nu));
            
            % 构建Hessian矩阵
            obj.H = 2 * (Gamma' * kron(eye(obj.Np), obj.Q) * Gamma + ...
                        Theta' * kron(eye(obj.Nc), obj.R) * Theta + ...
                        kron(eye(obj.Nc), obj.S));
            
            % 构建梯度向量(将在每次迭代中更新)
            obj.f = [];
            
            % 构建约束矩阵
            obj.build_constraint_matrices();
        end
        
        function build_constraint_matrices(obj)
            % 构建约束矩阵
            
            % 输入约束
            A_u = [kron(eye(obj.Nc), eye(obj.nu)); 
                  -kron(eye(obj.Nc), eye(obj.nu))];
            b_u = [repmat(obj.u_max, obj.Nc, 1);
                  -repmat(obj.u_min, obj.Nc, 1)];
            
            % 输入变化率约束
            D = kron(diag(ones(obj.Nc,1)) + diag(-ones(obj.Nc-1,1), -1), eye(obj.nu));
            A_du = [D; -D];
            b_du = [repmat(obj.du_max, obj.Nc, 1);
                   -repmat(obj.du_min, obj.Nc, 1)];
            
            % 状态约束(简化处理)
            % 注意:完整的状态约束需要更复杂的处理
            
            % 合并约束
            obj.Aineq = [A_u; A_du];
            obj.bineq = [b_u; b_du];
            
            % 等式约束(无)
            obj.Aeq = [];
            obj.beq = [];
            
            % 边界约束
            obj.lb = repmat(obj.u_min, obj.Nc, 1);
            obj.ub = repmat(obj.u_max, obj.Nc, 1);
        end
        
        function [u_opt, info] = solve_mpc(obj, x0, x_ref, u_prev)
            % 求解MPC问题
            % 输入:
            %   x0: 当前状态
            %   x_ref: 参考状态
            %   u_prev: 上一时刻控制输入
            % 输出:
            %   u_opt: 最优控制序列(仅取第一个)
            %   info: 求解信息
            
            % 构建梯度向量
            Psi_x0 = zeros(obj.ny*obj.Np, 1);
            for i = 1:obj.Np
                Psi_x0((i-1)*obj.ny+1:i*obj.ny) = obj.C * (obj.A^i) * x0;
            end
            
            x_ref_vec = repmat(x_ref(1:obj.ny), obj.Np, 1);
            obj.f = 2 * (Psi_x0 - x_ref_vec)' * kron(eye(obj.Np), obj.Q);
            
            % 构建输入变化率约束的偏移量
            u_prev_vec = [u_prev; zeros((obj.Nc-1)*obj.nu, 1)];
            D = kron(diag(ones(obj.Nc,1)) + diag(-ones(obj.Nc-1,1), -1), eye(obj.nu));
            b_du_offset = D(1:obj.nu, :) * u_prev_vec;
            
            % 更新约束
            b_du = obj.bineq(end-obj.Nc*obj.nu+1:end);
            b_du(1:obj.nu) = b_du(1:obj.nu) - b_du_offset;
            b_du(obj.nu+1:2*obj.nu) = b_du(obj.nu+1:2*obj.nu) + b_du_offset;
            obj.bineq(end-obj.Nc*obj.nu+1:end) = b_du;
            
            % 求解二次规划
            options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex');
            [U_opt, ~, exitflag] = quadprog(obj.H, obj.f', obj.Aineq, obj.bineq, ...
                                           obj.Aeq, obj.beq, obj.lb, obj.ub, [], options);
            
            if exitflag > 0
                u_opt = U_opt(1:obj.nu);
                info.exitflag = exitflag;
                info.optimal = true;
            else
                warning('MPC求解失败,使用备用控制律');
                u_opt = u_prev;  % 保持上一时刻控制
                info.exitflag = exitflag;
                info.optimal = false;
            end
        end
    end
end

四、完整仿真示例

matlab 复制代码
%% 无人机MPC控制仿真主程序
clear; close all; clc;

%% 1. 无人机参数设置
params.m = 1.2;      % 质量 (kg)
params.g = 9.81;     % 重力加速度 (m/s^2)
params.Ixx = 0.023;  % 转动惯量 (kg·m^2)
params.Iyy = 0.023;
params.Izz = 0.046;
params.kf = 8.548e-6;  % 升力系数
params.km = 1.6e-2;    % 力矩系数
params.L = 0.225;      % 臂长 (m)

%% 2. 线性化与离散化
% 悬停状态
x0 = [0; 0; 1; 0; 0; 0; 0; 0; 0; 0; 0; 0];  % 高度1m悬停
u0 = params.m * params.g / (4 * params.kf) * ones(4, 1);  % 悬停所需升力

% 采样时间
dt = 0.02;  % 20ms采样周期

% 线性化并离散化
[A, B] = linearize_quadrotor(x0, u0, params, dt);
C = eye(12);  % 输出所有状态
D = zeros(12, 4);

%% 3. MPC控制器设计
% MPC参数
Np = 20;  % 预测时域
Nc = 10;  % 控制时域

% 权重矩阵
Q = diag([10, 10, 20, ...    % 位置权重
          1, 1, 2, ...       % 速度权重
          5, 5, 5, ...       % 姿态角权重
          0.1, 0.1, 0.1]);   % 角速度权重

R = 0.01 * eye(4);  % 控制输入权重
S = 0.1 * eye(4);   % 控制输入变化率权重

% 创建MPC控制器
mpc = QuadrotorMPC(A, B, C, Np, Nc, Q, R, S);

% 设置约束
mpc.u_min = zeros(4, 1);  % 最小转速(平方)
mpc.u_max = 20000 * ones(4, 1);  % 最大转速(平方)
mpc.du_min = -1000 * ones(4, 1);  % 最小变化率
mpc.du_max = 1000 * ones(4, 1);   % 最大变化率

%% 4. 轨迹跟踪仿真
% 仿真参数
T = 10;  % 总仿真时间 (s)
N = floor(T / dt);  % 仿真步数

% 初始化
x = x0;  % 初始状态
u = u0;  % 初始控制
x_history = zeros(12, N+1);
u_history = zeros(4, N);
t_history = zeros(1, N+1);

x_history(:, 1) = x;
t_history(1) = 0;

% 参考轨迹(圆形轨迹)
t_ref = linspace(0, T, N+1);
x_ref = zeros(12, N+1);
radius = 2;  % 圆形半径
omega = 1;   % 角速度

for k = 1:N+1
    t = t_ref(k);
    x_ref(1, k) = radius * sin(omega * t);  % x位置
    x_ref(2, k) = radius * (1 - cos(omega * t));  % y位置
    x_ref(3, k) = 1;  % 高度保持1m
    x_ref(9, k) = atan2(cos(omega * t), 1 - sin(omega * t));  % 偏航角跟踪
end

%% 5. 主仿真循环
fprintf('开始MPC仿真...\n');
for k = 1:N
    % 当前时间
    t = (k-1) * dt;
    
    % 获取当前参考状态
    x_ref_k = x_ref(:, min(k, size(x_ref, 2)));
    
    % 求解MPC问题
    [u_opt, info] = mpc.solve_mpc(x, x_ref_k, u);
    
    % 应用控制输入
    u = u_opt;
    
    % 记录
    u_history(:, k) = u;
    
    % 系统仿真(使用非线性模型)
    [~, x_sim] = ode45(@(t, x) quadrotor_nonlinear(t, x, u, params), ...
                       [0, dt], x);
    
    % 更新状态
    x = x_sim(end, :)';
    
    % 记录状态
    x_history(:, k+1) = x;
    t_history(k+1) = t + dt;
    
    % 显示进度
    if mod(k, 50) == 0
        fprintf('时间: %.2f s, 位置误差: %.3f m\n', ...
                t+dt, norm(x(1:3) - x_ref_k(1:3)));
    end
end
fprintf('仿真完成!\n');

%% 6. 结果可视化
figure('Position', [100, 100, 1200, 800]);

% 子图1:三维轨迹
subplot(2, 3, 1);
plot3(x_history(1, :), x_history(2, :), x_history(3, :), 'b-', 'LineWidth', 2);
hold on;
plot3(x_ref(1, :), x_ref(2, :), x_ref(3, :), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title('三维轨迹跟踪');
legend('实际轨迹', '参考轨迹', 'Location', 'best');
grid on; axis equal;

% 子图2:位置跟踪
subplot(2, 3, 2);
plot(t_history, x_history(1:3, :), 'LineWidth', 1.5);
hold on;
plot(t_ref, x_ref(1:3, :), '--', 'LineWidth', 1);
xlabel('时间 (s)'); ylabel('位置 (m)');
title('位置跟踪');
legend('x', 'y', 'z', 'x_{ref}', 'y_{ref}', 'z_{ref}', 'Location', 'best');
grid on;

% 子图3:姿态角
subplot(2, 3, 3);
plot(t_history, rad2deg(x_history(7:9, :)), 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('角度 (°)');
title('姿态角');
legend('\phi (滚转)', '\theta (俯仰)', '\psi (偏航)', 'Location', 'best');
grid on;

% 子图4:控制输入
subplot(2, 3, 4);
plot(t_history(1:end-1), u_history', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('控制输入 (ω²)');
title('电机转速平方');
legend('电机1', '电机2', '电机3', '电机4', 'Location', 'best');
grid on;

% 子图5:跟踪误差
subplot(2, 3, 5);
pos_error = vecnorm(x_history(1:3, :) - x_ref(:, 1:size(x_history, 2)), 2, 1);
plot(t_history, pos_error, 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('位置误差 (m)');
title('轨迹跟踪误差');
grid on;

% 子图6:相平面图
subplot(2, 3, 6);
plot(x_history(1, :), x_history(2, :), 'b-', 'LineWidth', 2);
hold on;
plot(x_ref(1, :), x_ref(2, :), 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)');
title('XY平面轨迹');
legend('实际', '参考', 'Location', 'best');
grid on; axis equal;

%% 7. 性能指标计算
% 均方根误差
rmse_pos = sqrt(mean(pos_error.^2));
fprintf('\n性能指标:\n');
fprintf('位置RMSE: %.4f m\n', rmse_pos);

% 控制能量
control_energy = sum(sum(u_history.^2)) * dt;
fprintf('控制能量: %.4f\n', control_energy);

% 最大跟踪误差
max_error = max(pos_error);
fprintf('最大跟踪误差: %.4f m\n', max_error);

%% 8. 保存结果
save('quadrotor_mpc_results.mat', 'x_history', 'u_history', 't_history', ...
     'x_ref', 't_ref', 'params', 'dt', 'Np', 'Nc');

参考代码 无人机离散系统模型预测控制 www.youwenfan.com/contentcst/160596.html

五、关键问题与解决方案

5.1 计算复杂度优化

matlab 复制代码
% 稀疏矩阵优化
function build_qp_matrices_sparse(obj)
    % 使用稀疏矩阵减少内存占用
    obj.H = sparse(obj.H);
    obj.Aineq = sparse(obj.Aineq);
    obj.Aeq = sparse(obj.Aeq);
end

% 热启动加速
function [u_opt, info] = solve_mpc_hotstart(obj, x0, x_ref, u_prev, U_guess)
    % 使用上一时刻解作为初始猜测
    options = optimoptions('quadprog', 'Display', 'off', ...
                          'Algorithm', 'interior-point-convex', ...
                          'InitBarrierParam', 0.01, ...
                          'InitTrustRegionRadius', 100);
    [U_opt, ~, exitflag] = quadprog(obj.H, obj.f', obj.Aineq, obj.bineq, ...
                                   obj.Aeq, obj.beq, obj.lb, obj.ub, ...
                                   U_guess, options);
    % ... 后续处理
end

5.2 实时性保障

matlab 复制代码
% 简化模型(降阶)
function [A_red, B_red] = reduce_model(A, B, n_red)
    % 使用平衡截断降阶
    sys = ss(A, B, eye(size(A)), zeros(size(B)), -1);  % 离散系统
    sys_red = balred(sys, n_red);
    A_red = sys_red.A;
    B_red = sys_red.B;
end

% 显式MPC(离线计算)
function mpc_explicit = design_explicit_mpc(A, B, Q, R, constraints)
    % 使用MPT工具箱生成显式MPC控制器
    model = LTISystem('A', A, 'B', B);
    model.x.min = constraints.x_min;
    model.x.max = constraints.x_max;
    model.u.min = constraints.u_min;
    model.u.max = constraints.u_max;
    
    model.x.penalty = QuadFunction(Q);
    model.u.penalty = QuadFunction(R);
    
    mpc_explicit = MPCController(model, Np);
    explicit_ctrl = mpc_explicit.toExplicit();
end

六、扩展功能

6.1 抗干扰增强

matlab 复制代码
% 扰动观测器
function [d_est] = disturbance_observer(x, u, x_prev, dt)
    % 简化的扰动观测器
    persistent d_hat;
    if isempty(d_hat)
        d_hat = zeros(size(x));
    end
    
    % 观测器增益
    L = 0.1 * eye(length(x));
    
    % 预测状态
    x_pred = A * x_prev + B * u;
    
    % 更新扰动估计
    d_hat = d_hat + L * (x - x_pred);
    
    d_est = d_hat;
end

6.2 自适应MPC

matlab 复制代码
classdef AdaptiveMPC < QuadrotorMPC
    % 自适应MPC,在线更新模型
    
    methods
        function update_model(obj, x_data, u_data)
            % 使用递归最小二乘法在线更新模型
            % x_data: 状态数据序列
            % u_data: 输入数据序列
            
            % 简化的模型更新(实际应用需更严谨)
            theta = (x_data(:, 2:end) - x_data(:, 1:end-1)) / dt;
            phi = [x_data(:, 1:end-1); u_data];
            
            % 最小二乘估计
            AB_est = theta * pinv(phi);
            
            % 更新模型(保持稳定性)
            obj.A = 0.9 * obj.A + 0.1 * AB_est(1:obj.nx, 1:obj.nx);
            obj.B = 0.9 * obj.B + 0.1 * AB_est(1:obj.nx, obj.nx+1:end);
            
            % 重新构建QP矩阵
            obj.build_qp_matrices();
        end
    end
end

七、总结

本实现提供了完整的无人机离散系统模型预测控制MATLAB解决方案,包含:

  1. 精确建模:非线性动力学模型 + 线性化离散化

  2. MPC核心:基于二次规划的优化求解,支持状态/输入约束

  3. 完整仿真:圆形轨迹跟踪示例,包含可视化与性能分析

  4. 工程优化:计算复杂度优化、实时性保障策略

  5. 扩展功能:抗干扰增强、自适应MPC等高级特性

关键优势

  • 直接处理多变量耦合与约束

  • 良好的轨迹跟踪性能

  • 适用于实时嵌入式实现

  • 可扩展至更复杂场景(避障、编队等)

应用建议

  1. 实际部署时考虑计算延迟,适当减小预测时域

  2. 添加积分项消除稳态误差

  3. 结合卡尔曼滤波进行状态估计

  4. 考虑输入饱和与执行器动力学

相关推荐
爱写代码的小朋友2 小时前
使用 Nuitka 打包 Python 应用:从入门到进阶
开发语言·python
yuan199972 小时前
C# 断点续传下载文件工具设计与实现
开发语言·c#
想唱rap2 小时前
线程之条件变量和生产消费模型
java·服务器·开发语言·数据库·mysql·ubuntu
Boop_wu2 小时前
[Java 算法] 栈
java·开发语言·算法
来自远方的老作者2 小时前
第7章 运算符-7.5 比较运算符
开发语言·数据结构·python·算法·代码规范·比较运算符
南境十里·墨染春水2 小时前
C++笔记 Lambda表达式
开发语言·c++·笔记
We་ct2 小时前
LeetCode 201. 数字范围按位与:位运算高效解题指南
开发语言·前端·javascript·算法·leetcode·typescript
程序员榴莲2 小时前
Java(十二)抽象类
java·开发语言
木子欢儿2 小时前
在 Fedora 上配置 Go 语言(Golang)开发环境
开发语言·后端·golang