具有飞行约束的无人机MPC MATLAB实现

一、无人机动力学模型与约束

1.1 无人机非线性动力学模型

matlab 复制代码
%% 无人机非线性动力学模型
% 状态变量: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
% 控制输入: [F_total, tau_phi, tau_theta, tau_psi]

function dx = quadrotor_dynamics(t, x, u, params)
    % 参数提取
    m = params.m;       % 质量 (kg)
    g = params.g;       % 重力加速度 (m/s^2)
    Ix = params.Ix;     % 转动惯量 (kg·m^2)
    Iy = params.Iy;
    Iz = params.Iz;
    
    % 状态变量
    pos = x(1:3);           % 位置 (m)
    vel = x(4:6);           % 速度 (m/s)
    euler = x(7:9);         % 欧拉角 (rad)
    omega = x(10:12);       % 角速度 (rad/s)
    
    % 控制输入
    F_total = u(1);         % 总推力 (N)
    tau_phi = u(2);         % 滚转力矩 (N·m)
    tau_theta = u(3);       % 俯仰力矩 (N·m)
    tau_psi = u(4);         % 偏航力矩 (N·m)
    
    % 提取欧拉角
    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;
    
    % 速度动力学
    F_inertial = R * [0; 0; F_total];  % 将推力转换到惯性系
    gravity = [0; 0; -m*g];
    dvel = (F_inertial + gravity) / m;
    
    % 欧拉角变化率
    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_phi + (Iy-Iz)*omega(2)*omega(3)) / Ix;
              (tau_theta + (Iz-Ix)*omega(3)*omega(1)) / Iy;
              (tau_psi + (Ix-Iy)*omega(1)*omega(2)) / Iz];
    
    dx = [dpos; dvel; deuler; domega];
end

1.2 飞行约束定义

matlab 复制代码
%% 飞行约束定义
classdef FlightConstraints
    properties
        % 位置约束
        pos_min = [-10; -10; 0.5];    % 最小位置 (m)
        pos_max = [10; 10; 20];       % 最大位置 (m)
        
        % 速度约束
        vel_max = [3; 3; 3];          % 最大速度 (m/s)
        vel_min = -[3; 3; 3];         % 最小速度 (m/s)
        
        % 欧拉角约束
        euler_max = [30; 30; 180] * pi/180;  % 最大欧拉角 (rad)
        euler_min = -[30; 30; 180] * pi/180; % 最小欧拉角 (rad)
        
        % 角速度约束
        omega_max = [45; 45; 45] * pi/180;   % 最大角速度 (rad/s)
        
        % 控制输入约束
        F_min = 0;                  % 最小推力 (N)
        F_max = 20;                 % 最大推力 (N) 约2g
        tau_max = [2; 2; 2];        % 最大力矩 (N·m)
        
        % 障碍物约束
        obstacles = struct('center', [], 'radius', []);
        
        % 避障安全距离
        safety_margin = 0.5;        % 安全裕度 (m)
        
        % 物理约束标志
        enforce_input_slew = true;  % 是否强制输入变化率约束
        input_slew_rate = [5; 5; 5; 5];  % 输入变化率限制 (N/s, N·m/s)
    end
    
    methods
        function obj = FlightConstraints(varargin)
            % 构造函数,可设置障碍物
            if nargin >= 2
                obj.obstacles.center = varargin{1};
                obj.obstacles.radius = varargin{2};
            end
        end
        
        function [A, b] = get_obstacle_constraints(obj, pos)
            % 获取障碍物约束
            n_obs = size(obj.obstacles.center, 2);
            n_horizon = size(pos, 2);
            
            A = [];
            b = [];
            
            for k = 1:n_horizon
                for i = 1:n_obs
                    % 球体障碍物的线性化约束
                    obs_center = obj.obstacles.center(:, i);
                    obs_radius = obj.obstacles.radius(i);
                    
                    % 计算障碍物到当前位置的方向
                    d = pos(:, k) - obs_center;
                    dist = norm(d);
                    
                    if dist < 1e-6
                        direction = [1; 0; 0];  % 避免除以零
                    else
                        direction = d / dist;
                    end
                    
                    % 线性化约束: direction' * (pos - center) >= radius + margin
                    A_k = -direction';
                    b_k = -obs_radius - obj.safety_margin + direction' * obs_center;
                    
                    A = [A; zeros(1, 3*(k-1)), A_k, zeros(1, 3*(n_horizon-k))];
                    b = [b; b_k];
                end
            end
        end
        
        function plot_constraints(obj, ax)
            % 绘制约束区域
            hold(ax, 'on');
            
            % 绘制位置约束框
            x_min = obj.pos_min(1); x_max = obj.pos_max(1);
            y_min = obj.pos_min(2); y_max = obj.pos_max(2);
            z_min = obj.pos_min(3); z_max = obj.pos_max(3);
            
            % 绘制立方体
            vertices = [x_min, y_min, z_min; x_max, y_min, z_min;
                        x_max, y_max, z_min; x_min, y_max, z_min;
                        x_min, y_min, z_max; x_max, y_min, z_max;
                        x_max, y_max, z_max; x_min, y_max, z_max];
            
            faces = [1,2,3,4; 5,6,7,8; 1,2,6,5;
                     2,3,7,6; 3,4,8,7; 4,1,5,8];
            
            patch(ax, 'Vertices', vertices, 'Faces', faces, ...
                  'FaceColor', 'none', 'EdgeColor', 'r', 'LineWidth', 1, ...
                  'FaceAlpha', 0.1, 'DisplayName', '飞行区域');
            
            % 绘制障碍物
            for i = 1:size(obj.obstacles.center, 2)
                center = obj.obstacles.center(:, i);
                radius = obj.obstacles.radius(i);
                
                [x_sphere, y_sphere, z_sphere] = sphere(20);
                x_sphere = x_sphere * radius + center(1);
                y_sphere = y_sphere * radius + center(2);
                z_sphere = z_sphere * radius + center(3);
                
                surf(ax, x_sphere, y_sphere, z_sphere, ...
                     'FaceColor', [0.8, 0.2, 0.2], 'FaceAlpha', 0.3, ...
                     'EdgeColor', 'none', 'DisplayName', '障碍物');
            end
            
            hold(ax, 'off');
        end
    end
end

二、非线性MPC控制器设计

2.1 主MPC控制器类

matlab 复制代码
%% 具有飞行约束的无人机非线性MPC控制器
classdef ConstrainedQuadrotorMPC < handle
    properties
        % MPC参数
        Ts = 0.1;           % 采样时间 (s)
        N = 10;             % 预测时域
        Nu = 5;             % 控制时域
        Q;                  % 状态权重矩阵
        R;                  % 控制权重矩阵
        P;                  % 终端权重矩阵
        
        % 无人机参数
        params = struct('m', 1.0, 'g', 9.81, ...
                        'Ix', 0.01, 'Iy', 0.01, 'Iz', 0.02);
        
        % 约束对象
        constraints;
        
        % 优化器选项
        solver_options = optimoptions('fmincon', ...
            'Algorithm', 'sqp', ...
            'Display', 'iter', ...
            'MaxIterations', 100, ...
            'MaxFunctionEvaluations', 10000, ...
            'OptimalityTolerance', 1e-4, ...
            'ConstraintTolerance', 1e-4, ...
            'StepTolerance', 1e-6);
        
        % 线性化模型
        A_discrete;         % 离散状态矩阵
        B_discrete;         % 离散输入矩阵
        linearization_point; % 线性化点
        
        % 性能记录
        solve_times = [];   % 求解时间历史
        cost_values = [];   % 代价函数值历史
    end
    
    methods
        function obj = ConstrainedQuadrotorMPC(Ts, N, constraints)
            % 构造函数
            obj.Ts = Ts;
            obj.N = N;
            obj.Nu = min(N, obj.Nu);
            obj.constraints = constraints;
            
            % 初始化权重矩阵
            obj.initialize_weights();
            
            % 初始线性化
            obj.linearize_model(zeros(12,1), zeros(4,1));
        end
        
        function initialize_weights(obj)
            % 初始化权重矩阵
            % 状态权重: [x,y,z, vx,vy,vz, phi,theta,psi, p,q,r]
            Q_diag = [10, 10, 10, ...      % 位置
                      5, 5, 5, ...         % 速度
                      2, 2, 0.5, ...       % 欧拉角
                      0.1, 0.1, 0.1];      % 角速度
            obj.Q = diag(Q_diag);
            
            % 控制权重: [F_total, tau_phi, tau_theta, tau_psi]
            R_diag = [0.1, 1, 1, 1];
            obj.R = diag(R_diag);
            
            % 终端权重 (通常设为Q的倍数)
            obj.P = 10 * obj.Q;
        end
        
        function linearize_model(obj, x0, u0)
            % 在给定点线性化模型
            n_states = 12;
            n_inputs = 4;
            
            % 使用数值微分计算雅可比矩阵
            eps = 1e-6;
            
            % 计算连续时间雅可比
            A_cont = zeros(n_states, n_states);
            B_cont = zeros(n_states, n_inputs);
            
            % 计算f(x0, u0)
            f0 = obj.continuous_dynamics(x0, u0);
            
            % 计算A = ∂f/∂x
            for i = 1:n_states
                x_pert = x0;
                x_pert(i) = x_pert(i) + eps;
                f_pert = obj.continuous_dynamics(x_pert, u0);
                A_cont(:, i) = (f_pert - f0) / eps;
            end
            
            % 计算B = ∂f/∂u
            for i = 1:n_inputs
                u_pert = u0;
                u_pert(i) = u_pert(i) + eps;
                f_pert = obj.continuous_dynamics(x0, u_pert);
                B_cont(:, i) = (f_pert - f0) / eps;
            end
            
            % 离散化 (零阶保持)
            sys_cont = ss(A_cont, B_cont, eye(n_states), zeros(n_states, n_inputs));
            sys_discrete = c2d(sys_cont, obj.Ts);
            
            obj.A_discrete = sys_discrete.A;
            obj.B_discrete = sys_discrete.B;
            obj.linearization_point.x = x0;
            obj.linearization_point.u = u0;
        end
        
        function f = continuous_dynamics(obj, x, u)
            % 连续时间非线性动力学
            f = quadrotor_dynamics(0, x, u, obj.params);
        end
        
        function [u_opt, info] = solve(obj, x_current, x_ref, u_prev)
            % 求解MPC优化问题
            t_start = tic;
            
            n_states = 12;
            n_inputs = 4;
            
            % 决策变量: [u0, u1, ..., u_{Nu-1}]
            n_vars = n_inputs * obj.Nu;
            
            % 初始猜测 (上一时刻的控制输入重复Nu次)
            u_init = repmat(u_prev, obj.Nu, 1);
            
            % 优化变量边界
            lb = -inf * ones(n_vars, 1);
            ub = inf * ones(n_vars, 1);
            
            % 输入约束
            for k = 1:obj.Nu
                idx = (k-1)*n_inputs + 1;
                lb(idx) = obj.constraints.F_min;
                ub(idx) = obj.constraints.F_max;
                lb(idx+1:idx+3) = -obj.constraints.tau_max;
                ub(idx+1:idx+3) = obj.constraints.tau_max;
            end
            
            % 输入变化率约束
            if obj.constraints.enforce_input_slew
                [A_slew, b_slew] = obj.get_input_slew_constraints(n_vars, u_prev);
            else
                A_slew = []; b_slew = [];
            end
            
            % 非线性约束函数
            nonlcon = @(u_vec) obj.nonlinear_constraints(u_vec, x_current, x_ref);
            
            % 目标函数
            cost_func = @(u_vec) obj.mpc_cost(u_vec, x_current, x_ref);
            
            % 求解优化问题
            [u_opt_vec, fval, exitflag, output] = fmincon(...
                cost_func, u_init, A_slew, b_slew, [], [], lb, ub, nonlcon, obj.solver_options);
            
            % 提取第一个控制输入
            u_opt = u_opt_vec(1:n_inputs);
            
            % 记录性能
            obj.solve_times(end+1) = toc(t_start);
            obj.cost_values(end+1) = fval;
            
            % 返回求解信息
            info = struct('exitflag', exitflag, 'output', output, ...
                          'fval', fval, 'solve_time', obj.solve_times(end));
            
            % 更新线性化点
            obj.linearize_model(x_current, u_opt);
        end
        
        function [c, ceq] = nonlinear_constraints(obj, u_vec, x_current, x_ref)
            % 非线性约束: 状态约束、障碍物约束
            n_states = 12;
            n_inputs = 4;
            
            % 提取控制序列
            U = reshape(u_vec, n_inputs, obj.Nu);
            
            % 预测状态轨迹
            X = obj.predict_trajectory(x_current, U);
            
            % 初始化约束
            c = [];
            ceq = [];
            
            % 状态约束
            for k = 1:obj.N
                x_k = X(:, k+1);  % 注意: X(:,1)是当前状态
                
                % 位置约束
                c_pos = [x_k(1:3) - obj.constraints.pos_max;
                         obj.constraints.pos_min - x_k(1:3)];
                
                % 速度约束
                c_vel = [x_k(4:6) - obj.constraints.vel_max;
                         obj.constraints.vel_min - x_k(4:6)];
                
                % 欧拉角约束
                c_euler = [x_k(7:9) - obj.constraints.euler_max;
                           obj.constraints.euler_min - x_k(7:9)];
                
                % 角速度约束
                c_omega = [x_k(10:12) - obj.constraints.omega_max;
                           -obj.constraints.omega_max - x_k(10:12)];
                
                c = [c; c_pos; c_vel; c_euler; c_omega];
            end
            
            % 障碍物约束
            if ~isempty(obj.constraints.obstacles.center)
                pos_trajectory = X(1:3, 2:end);  % 预测位置轨迹
                [A_obs, b_obs] = obj.constraints.get_obstacle_constraints(pos_trajectory);
                
                % 转换为c <= 0形式
                c_obs = A_obs * pos_trajectory(:) - b_obs;
                c = [c; c_obs];
            end
            
            % 等式约束 (动力学约束已经在预测中满足)
            ceq = [];
        end
        
        function [A, b] = get_input_slew_constraints(obj, n_vars, u_prev)
            % 获取输入变化率约束
            n_inputs = 4;
            A = [];
            b = [];
            
            slew_rate = obj.constraints.input_slew_rate;
            
            for k = 1:obj.Nu
                if k == 1
                    % 第一个输入相对于上一时刻的变化
                    for i = 1:n_inputs
                        A_row = zeros(1, n_vars);
                        A_row(i) = 1;
                        A_row(i+n_inputs) = -1;
                        b_row = slew_rate(i) * obj.Ts;
                        
                        A = [A; A_row; -A_row];
                        b = [b; b_row; b_row];
                    end
                else
                    % 后续输入之间的变化
                    idx1 = (k-2)*n_inputs + 1;
                    idx2 = (k-1)*n_inputs + 1;
                    
                    for i = 1:n_inputs
                        A_row = zeros(1, n_vars);
                        A_row(idx1 + i - 1) = 1;
                        A_row(idx2 + i - 1) = -1;
                        b_row = slew_rate(i) * obj.Ts;
                        
                        A = [A; A_row; -A_row];
                        b = [b; b_row; b_row];
                    end
                end
            end
        end
        
        function J = mpc_cost(obj, u_vec, x_current, x_ref)
            % MPC代价函数
            n_states = 12;
            n_inputs = 4;
            
            % 提取控制序列
            U = reshape(u_vec, n_inputs, obj.Nu);
            
            % 扩展控制序列到整个预测时域
            if obj.Nu < obj.N
                U_full = [U, repmat(U(:, end), 1, obj.N - obj.Nu)];
            else
                U_full = U(:, 1:obj.N);
            end
            
            % 预测状态轨迹
            X = obj.predict_trajectory(x_current, U_full);
            
            % 初始化代价
            J = 0;
            
            % 阶段代价
            for k = 1:obj.N
                x_k = X(:, k+1);
                u_k = U_full(:, k);
                
                % 状态跟踪误差
                if size(x_ref, 2) >= k
                    x_ref_k = x_ref(:, min(k, size(x_ref, 2)));
                else
                    x_ref_k = x_ref(:, end);
                end
                
                e_x = x_k - x_ref_k;
                J = J + e_x' * obj.Q * e_x;
                
                % 控制代价
                J = J + u_k' * obj.R * u_k;
            end
            
            % 终端代价
            x_N = X(:, end);
            if size(x_ref, 2) >= obj.N
                x_ref_N = x_ref(:, obj.N);
            else
                x_ref_N = x_ref(:, end);
            end
            e_N = x_N - x_ref_N;
            J = J + e_N' * obj.P * e_N;
        end
        
        function X = predict_trajectory(obj, x0, U)
            % 预测状态轨迹 (使用非线性模型)
            n_states = 12;
            N = size(U, 2);
            
            X = zeros(n_states, N+1);
            X(:, 1) = x0;
            
            % 数值积分预测
            for k = 1:N
                % 使用RK4积分
                x_k = X(:, k);
                u_k = U(:, k);
                
                k1 = obj.continuous_dynamics(x_k, u_k);
                k2 = obj.continuous_dynamics(x_k + 0.5*obj.Ts*k1, u_k);
                k3 = obj.continuous_dynamics(x_k + 0.5*obj.Ts*k2, u_k);
                k4 = obj.continuous_dynamics(x_k + obj.Ts*k3, u_k);
                
                x_next = x_k + (obj.Ts/6) * (k1 + 2*k2 + 2*k3 + k4);
                
                X(:, k+1) = x_next;
            end
        end
        
        function plot_results(obj, X_history, U_history, X_ref, t)
            % 绘制结果
            figure('Position', [100, 100, 1400, 800]);
            
            % 子图1: 3D轨迹
            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);
            
            if ~isempty(obj.constraints.obstacles.center)
                obj.constraints.plot_constraints(gca);
            end
            
            xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
            title('3D轨迹跟踪');
            legend('实际轨迹', '参考轨迹', 'Location', 'best');
            grid on; axis equal;
            view(45, 30);
            
            % 子图2: 位置跟踪
            subplot(2,3,2);
            plot(t, X_history(1:3,:)');
            hold on;
            plot(t, X_ref(1:3,1:length(t)), '--', '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, X_history(4:6,:)');
            xlabel('时间 (s)');
            ylabel('速度 (m/s)');
            title('速度');
            legend('v_x', 'v_y', 'v_z', 'Location', 'best');
            grid on;
            
            % 子图4: 欧拉角
            subplot(2,3,4);
            euler_deg = rad2deg(X_history(7:9,:));
            plot(t, euler_deg);
            xlabel('时间 (s)');
            ylabel('角度 (°)');
            title('姿态角');
            legend('\phi', '\theta', '\psi', 'Location', 'best');
            grid on;
            
            % 子图5: 控制输入
            subplot(2,3,5);
            stairs(t(1:end-1), U_history');
            xlabel('时间 (s)');
            ylabel('控制输入');
            title('控制输入');
            legend('F_{total}', '\tau_\phi', '\tau_\theta', '\tau_\psi', 'Location', 'best');
            grid on;
            
            % 子图6: 跟踪误差
            subplot(2,3,6);
            pos_error = vecnorm(X_history(1:3,:) - X_ref(1:3,1:length(t)), 2, 1);
            plot(t, pos_error, 'LineWidth', 2);
            xlabel('时间 (s)');
            ylabel('位置误差 (m)');
            title('位置跟踪误差');
            grid on;
        end
    end
end

2.2 参考轨迹生成

matlab 复制代码
%% 参考轨迹生成
classdef ReferenceTrajectory
    methods (Static)
        function [X_ref, U_ref] = generate_circle_trajectory(t, radius, height, freq)
            % 生成圆形轨迹
            n_points = length(t);
            
            X_ref = zeros(12, n_points);
            U_ref = zeros(4, n_points);
            
            % 圆形轨迹参数
            omega = 2*pi*freq;
            
            for i = 1:n_points
                % 位置
                X_ref(1,i) = radius * cos(omega*t(i));
                X_ref(2,i) = radius * sin(omega*t(i));
                X_ref(3,i) = height;
                
                % 速度
                X_ref(4,i) = -radius * omega * sin(omega*t(i));
                X_ref(5,i) = radius * omega * cos(omega*t(i));
                X_ref(6,i) = 0;
                
                % 姿态 (偏航角指向切线方向)
                X_ref(9,i) = atan2(X_ref(5,i), X_ref(4,i)) + pi/2;
                
                % 其他状态设为0
                X_ref(7:8,i) = 0;
                X_ref(10:12,i) = 0;
                
                % 控制输入 (悬停时推力等于重力)
                U_ref(:,i) = [9.81; 0; 0; 0];  % 假设质量为1kg
            end
        end
        
        function [X_ref, U_ref] = generate_point_to_point(start, goal, t, max_vel, max_acc)
            % 生成点到点轨迹 (梯形速度曲线)
            n_points = length(t);
            n_states = 12;
            
            X_ref = zeros(n_states, n_points);
            U_ref = zeros(4, n_points);
            
            % 计算梯形速度曲线
            delta_pos = goal(1:3) - start(1:3);
            dist = norm(delta_pos);
            
            if dist == 0
                X_ref(1:3,:) = repmat(start(1:3), 1, n_points);
                U_ref(1,:) = 9.81;  % 悬停推力
                return;
            end
            
            % 计算梯形速度参数
            t_acc = max_vel / max_acc;
            dist_acc = 0.5 * max_acc * t_acc^2;
            
            if 2*dist_acc > dist
                % 三角形速度曲线
                t_acc_actual = sqrt(dist / max_acc);
                max_vel_actual = max_acc * t_acc_actual;
                t_total = 2 * t_acc_actual;
            else
                % 梯形速度曲线
                t_const = (dist - 2*dist_acc) / max_vel;
                t_total = 2*t_acc + t_const;
                max_vel_actual = max_vel;
            end
            
            % 归一化时间
            t_normalized = min(t / t_total, 1);
            
            for i = 1:n_points
                tn = t_normalized(i);
                
                if tn <= t_acc_actual/t_total
                    % 加速段
                    s = 0.5 * max_acc * (t_total*tn)^2;
                    v = max_acc * (t_total*tn);
                elseif tn <= 1 - t_acc_actual/t_total
                    % 匀速段
                    s = dist_acc + max_vel_actual * (t_total*tn - t_acc_actual);
                    v = max_vel_actual;
                else
                    % 减速段
                    t_dec = t_total*tn - (t_total - t_acc_actual);
                    s = dist - 0.5 * max_acc * (t_acc_actual - t_dec)^2;
                    v = max_acc * (t_acc_actual - t_dec);
                end
                
                % 位置
                X_ref(1:3,i) = start(1:3) + (s/dist) * delta_pos;
                
                % 速度
                X_ref(4:6,i) = (v/dist) * delta_pos;
                
                % 姿态 (朝向目标)
                if v > 0.1
                    vel_dir = X_ref(4:6,i) / v;
                    X_ref(9,i) = atan2(vel_dir(2), vel_dir(1));
                end
                
                % 控制输入
                U_ref(1,i) = 9.81;  % 基本悬停推力
            end
        end
        
        function [X_ref, U_ref] = generate_lemniscate_trajectory(t, a, height, freq)
            % 生成8字形轨迹 (Lemniscate of Bernoulli)
            n_points = length(t);
            
            X_ref = zeros(12, n_points);
            U_ref = zeros(4, n_points);
            
            omega = 2*pi*freq;
            
            for i = 1:n_points
                % Lemniscate参数方程
                tau = omega * t(i);
                
                % 位置
                X_ref(1,i) = a * cos(tau) / (1 + sin(tau)^2);
                X_ref(2,i) = a * sin(tau) * cos(tau) / (1 + sin(tau)^2);
                X_ref(3,i) = height;
                
                % 速度 (通过数值微分计算)
                if i > 1
                    dt = t(i) - t(i-1);
                    X_ref(4:6,i) = (X_ref(1:3,i) - X_ref(1:3,i-1)) / dt;
                end
                
                % 姿态
                if i > 1 && norm(X_ref(4:6,i)) > 0.1
                    vel_dir = X_ref(4:6,i) / norm(X_ref(4:6,i));
                    X_ref(9,i) = atan2(vel_dir(2), vel_dir(1));
                end
                
                % 控制输入
                U_ref(1,i) = 9.81;
            end
        end
    end
end

三、主仿真程序

matlab 复制代码
%% 主仿真程序:具有飞行约束的无人机MPC控制
clear; clc; close all;

%% 1. 仿真参数设置
Ts = 0.1;           % 采样时间 (s)
T_total = 20;       % 总仿真时间 (s)
t = 0:Ts:T_total;   % 时间向量
n_steps = length(t);

%% 2. 初始化无人机状态
% 初始状态: [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
x0 = [0; 0; 1;      % 位置 (m)
      0; 0; 0;      % 速度 (m/s)
      0; 0; 0;      % 姿态 (rad)
      0; 0; 0];     % 角速度 (rad/s)

x_current = x0;

%% 3. 生成参考轨迹
% 选择轨迹类型
traj_type = 'circle';  % 'circle', 'point_to_point', 'lemniscate'

switch traj_type
    case 'circle'
        radius = 5;
        height = 3;
        freq = 0.1;
        [X_ref, U_ref] = ReferenceTrajectory.generate_circle_trajectory(t, radius, height, freq);
        
    case 'point_to_point'
        start_point = [0; 0; 2];
        end_point = [8; 6; 4];
        max_vel = 2;
        max_acc = 1;
        [X_ref, U_ref] = ReferenceTrajectory.generate_point_to_point(...
            start_point, end_point, t, max_vel, max_acc);
        
    case 'lemniscate'
        a = 6;
        height = 3;
        freq = 0.05;
        [X_ref, U_ref] = ReferenceTrajectory.generate_lemniscate_trajectory(t, a, height, freq);
end

%% 4. 定义飞行约束
% 定义障碍物
obstacle_centers = [3, 0, 2;    % 障碍物1中心
                    -2, 4, 3;    % 障碍物2中心
                    0, -3, 2]';  % 障碍物3中心
obstacle_radii = [1.5, 1.0, 1.2];  % 障碍物半径

% 创建约束对象
constraints = FlightConstraints(obstacle_centers, obstacle_radii);

% 调整其他约束
constraints.pos_max = [8; 8; 6];
constraints.pos_min = [-8; -8; 0.5];
constraints.vel_max = [3; 3; 2];
constraints.vel_min = -constraints.vel_max;
constraints.safety_margin = 0.3;

%% 5. 创建MPC控制器
mpc = ConstrainedQuadrotorMPC(Ts, 10, constraints);

% 调整MPC参数
mpc.Q = diag([20, 20, 20, 8, 8, 8, 3, 3, 1, 0.2, 0.2, 0.2]);
mpc.R = diag([0.05, 2, 2, 2]);

% 设置求解器选项
mpc.solver_options.Display = 'off';
mpc.solver_options.MaxIterations = 50;

%% 6. 初始化存储变量
X_history = zeros(12, n_steps);
U_history = zeros(4, n_steps-1);
solve_info = cell(1, n_steps-1);
X_history(:,1) = x_current;
u_prev = [9.81; 0; 0; 0];  % 初始控制 (悬停)

%% 7. 主仿真循环
disp('开始MPC仿真...');
for k = 1:n_steps-1
    fprintf('时间步: %d/%d\n', k, n_steps-1);
    
    % 获取当前参考状态
    x_ref_k = X_ref(:, min(k, size(X_ref,2)));
    
    % 求解MPC
    [u_opt, info] = mpc.solve(x_current, x_ref_k, u_prev);
    
    % 存储结果
    U_history(:,k) = u_opt;
    solve_info{k} = info;
    
    % 应用控制输入 (前向仿真)
    % 使用RK4积分
    k1 = quadrotor_dynamics(0, x_current, u_opt, mpc.params);
    k2 = quadrotor_dynamics(0, x_current + 0.5*Ts*k1, u_opt, mpc.params);
    k3 = quadrotor_dynamics(0, x_current + 0.5*Ts*k2, u_opt, mpc.params);
    k4 = quadrotor_dynamics(0, x_current + Ts*k3, u_opt, mpc.params);
    
    x_next = x_current + (Ts/6) * (k1 + 2*k2 + 2*k3 + k4);
    
    % 存储状态
    X_history(:,k+1) = x_next;
    
    % 更新状态
    x_current = x_next;
    u_prev = u_opt;
    
    % 检查约束违反
    if any(x_next(1:3) < constraints.pos_min) || any(x_next(1:3) > constraints.pos_max)
        warning('位置约束违反!');
    end
    
    % 实时可视化 (可选)
    if mod(k, 10) == 0
        plot_realtime(x_current, X_ref, constraints, k*Ts);
    end
end

%% 8. 分析结果
% 提取求解时间
solve_times = [mpc.solve_times];
avg_solve_time = mean(solve_times);
max_solve_time = max(solve_times);

fprintf('\n=== 性能统计 ===\n');
fprintf('平均求解时间: %.3f ms\n', avg_solve_time*1000);
fprintf('最大求解时间: %.3f ms\n', max_solve_time*1000);
fprintf('控制频率: %.1f Hz\n', 1/avg_solve_time);

% 计算跟踪误差
pos_error = vecnorm(X_history(1:3,1:length(t)) - X_ref(1:3,1:length(t)), 2, 1);
avg_pos_error = mean(pos_error);
max_pos_error = max(pos_error);

fprintf('平均位置误差: %.3f m\n', avg_pos_error);
fprintf('最大位置误差: %.3f m\n', max_pos_error);

% 检查约束满足情况
check_constraints(X_history, U_history, constraints, t);

%% 9. 绘制结果
mpc.plot_results(X_history, U_history, X_ref, t);

% 绘制求解时间
figure;
subplot(1,2,1);
plot(t(1:end-1), solve_times*1000, 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('求解时间 (ms)');
title('MPC求解时间');
grid on;

subplot(1,2,2);
histogram(solve_times*1000, 20);
xlabel('求解时间 (ms)');
ylabel('频数');
title('求解时间分布');
grid on;

%% 10. 约束检查函数
function check_constraints(X_history, U_history, constraints, t)
    % 检查各种约束的满足情况
    n_steps = size(X_history, 2);
    
    fprintf('\n=== 约束检查 ===\n');
    
    % 位置约束
    pos_violations = 0;
    for k = 1:n_steps
        if any(X_history(1:3,k) < constraints.pos_min) || ...
           any(X_history(1:3,k) > constraints.pos_max)
            pos_violations = pos_violations + 1;
        end
    end
    fprintf('位置约束违反: %d/%d (%.1f%%)\n', ...
        pos_violations, n_steps, pos_violations/n_steps*100);
    
    % 速度约束
    vel_violations = 0;
    for k = 1:n_steps
        if any(abs(X_history(4:6,k)) > constraints.vel_max)
            vel_violations = vel_violations + 1;
        end
    end
    fprintf('速度约束违反: %d/%d (%.1f%%)\n', ...
        vel_violations, n_steps, vel_violations/n_steps*100);
    
    % 输入约束
    input_violations = 0;
    for k = 1:size(U_history,2)
        if U_history(1,k) < constraints.F_min || U_history(1,k) > constraints.F_max
            input_violations = input_violations + 1;
        end
        if any(abs(U_history(2:4,k)) > constraints.tau_max)
            input_violations = input_violations + 1;
        end
    end
    fprintf('输入约束违反: %d/%d (%.1f%%)\n', ...
        input_violations, size(U_history,2), input_violations/size(U_history,2)*100);
end

%% 11. 实时可视化函数
function plot_realtime(x_current, X_ref, constraints, current_time)
    persistent fig ax1 ax2 ax3;
    
    if isempty(fig) || ~isvalid(fig)
        fig = figure('Position', [100, 100, 1200, 400]);
        
        % 3D轨迹
        ax1 = subplot(1,3,1);
        xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
        title('3D轨迹');
        grid on; hold on; axis equal;
        view(45, 30);
        
        % 绘制约束区域
        constraints.plot_constraints(ax1);
        
        % 位置跟踪
        ax2 = subplot(1,3,2);
        xlabel('时间 (s)'); ylabel('位置 (m)');
        title('位置跟踪');
        grid on; hold on;
        
        % 姿态
        ax3 = subplot(1,3,3);
        xlabel('时间 (s)'); ylabel('角度 (°)');
        title('姿态角');
        grid on; hold on;
    end
    
    % 更新3D轨迹
    plot3(ax1, x_current(1), x_current(2), x_current(3), 'b.', 'MarkerSize', 10);
    plot3(ax1, X_ref(1,:), X_ref(2,:), X_ref(3,:), 'r--', 'LineWidth', 0.5);
    
    % 更新位置跟踪
    plot(ax2, current_time, x_current(1), 'b.');
    plot(ax2, current_time, x_current(2), 'g.');
    plot(ax2, current_time, x_current(3), 'r.');
    
    % 更新姿态
    euler_deg = rad2deg(x_current(7:9));
    plot(ax3, current_time, euler_deg(1), 'b.');
    plot(ax3, current_time, euler_deg(2), 'g.');
    plot(ax3, current_time, euler_deg(3), 'r.');
    
    drawnow;
end

参考代码 具有飞行约束的无人机MPC www.youwenfan.com/contentcst/160704.html

四、高级特性扩展

4.1 自适应MPC权重调整

matlab 复制代码
%% 自适应MPC权重调整
classdef AdaptiveMPCWeights
    properties
        base_Q;
        base_R;
        adaptation_gain = 0.1;
        max_adaptation = 5.0;
        min_adaptation = 0.2;
    end
    
    methods
        function [Q_adapted, R_adapted] = adapt_weights(obj, tracking_error, control_effort, constraint_violation)
            % 根据性能调整权重
            
            % 计算适应因子
            alpha_tracking = 1 + obj.adaptation_gain * norm(tracking_error);
            alpha_control = 1 + obj.adaptation_gain * norm(control_effort);
            alpha_constraint = 1 + 2 * obj.adaptation_gain * constraint_violation;
            
            % 限制适应范围
            alpha_tracking = min(max(alpha_tracking, obj.min_adaptation), obj.max_adaptation);
            alpha_control = min(max(alpha_control, obj.min_adaptation), obj.max_adaptation);
            alpha_constraint = min(max(alpha_constraint, obj.min_adaptation), obj.max_adaptation);
            
            % 调整权重
            Q_adapted = obj.base_Q;
            R_adapted = obj.base_R;
            
            % 增加约束违反时的状态权重
            if constraint_violation > 0.1
                Q_adapted(1:3,1:3) = alpha_constraint * Q_adapted(1:3,1:3);
            end
            
            % 增加跟踪误差时的控制权重
            if norm(tracking_error) > 0.5
                R_adapted = alpha_control * R_adapted;
            end
        end
    end
end

4.2 事件触发MPC

matlab 复制代码
%% 事件触发MPC
classdef EventTriggeredMPC < ConstrainedQuadrotorMPC
    properties
        trigger_threshold_pos = 0.1;  % 位置触发阈值 (m)
        trigger_threshold_vel = 0.2;  % 速度触发阈值 (m/s)
        last_control;                 % 上次控制输入
        last_update_time = 0;         % 上次更新时间
        min_update_interval = 0.05;   % 最小更新间隔 (s)
        update_count = 0;             % 更新次数统计
    end
    
    methods
        function [u_opt, trigger, info] = solve_triggered(obj, x_current, x_ref, u_prev, current_time)
            % 事件触发的MPC求解
            
            % 检查触发条件
            trigger = false;
            
            % 1. 时间触发 (确保最小更新频率)
            if current_time - obj.last_update_time >= obj.min_update_interval
                trigger = true;
            end
            
            % 2. 状态误差触发
            pos_error = norm(x_current(1:3) - x_ref(1:3));
            vel_error = norm(x_current(4:6) - x_ref(4:6));
            
            if pos_error > obj.trigger_threshold_pos || ...
               vel_error > obj.trigger_threshold_vel
                trigger = true;
            end
            
            % 3. 约束接近度触发
            constraint_violation = obj.check_constraint_violation(x_current);
            if constraint_violation > 0.8 * obj.constraints.safety_margin
                trigger = true;
            end
            
            if trigger
                % 触发MPC求解
                [u_opt, info] = obj.solve(x_current, x_ref, u_prev);
                obj.last_control = u_opt;
                obj.last_update_time = current_time;
                obj.update_count = obj.update_count + 1;
            else
                % 使用上次控制输入
                u_opt = obj.last_control;
                info = struct('exitflag', 0, 'event_triggered', false);
            end
        end
        
        function violation = check_constraint_violation(obj, x)
            % 检查约束违反程度
            violation = 0;
            
            % 位置约束违反
            pos_violation = max(0, obj.constraints.pos_min - x(1:3));
            pos_violation = max(pos_violation, x(1:3) - obj.constraints.pos_max);
            violation = max(violation, norm(pos_violation));
            
            % 障碍物约束违反
            if ~isempty(obj.constraints.obstacles.center)
                for i = 1:size(obj.constraints.obstacles.center, 2)
                    obs_center = obj.constraints.obstacles.center(:, i);
                    obs_radius = obj.constraints.obstacles.radius(i);
                    
                    dist_to_obs = norm(x(1:3) - obs_center);
                    obs_violation = max(0, obs_radius + obj.constraints.safety_margin - dist_to_obs);
                    violation = max(violation, obs_violation);
                end
            end
        end
    end
end

五、代码使用指南

5.1 快速开始

  1. 基本仿真

    matlab 复制代码
    % 运行主仿真程序
    main_constrained_mpc;
  2. 自定义轨迹

    matlab 复制代码
    % 修改轨迹参数
    traj_type = 'lemniscate';
    radius = 8;
    height = 4;
  3. 调整约束

    matlab 复制代码
    % 修改障碍物
    obstacle_centers = [2, 1, 2; -3, 0, 3]';
    obstacle_radii = [1.5, 2.0];

5.2 性能调优

参数 调优建议 影响
预测时域 N 10-20 增大N提高鲁棒性但增加计算量
控制时域 Nu 3-5 通常为N的1/3-1/2
状态权重 Q 对角线元素 增大跟踪性能,减小稳定性
控制权重 R 对角线元素 增大控制平滑性,减小响应速度
采样时间 Ts 0.05-0.2s 影响控制精度和计算负荷

5.3 常见问题解决

问题 可能原因 解决方案
求解失败 约束过于严格 1. 放宽约束范围 2. 增加求解器迭代次数
计算时间过长 预测时域过大 1. 减小N和Nu 2. 使用事件触发MPC
轨迹振荡 权重设置不当 1. 增大控制权重R 2. 减小状态权重Q
避障失败 安全裕度过小 1. 增大safety_margin 2. 增加障碍物约束权重

六、总结

本实现提供了一个完整的具有飞行约束的无人机MPC控制系统,包含:

  1. 非线性动力学模型:完整的12状态四旋翼模型
  2. 多种飞行约束:位置、速度、姿态、障碍物避让
  3. 非线性MPC求解:使用fmincon求解约束优化问题
  4. 参考轨迹生成:圆形、点到点、8字形等轨迹
  5. 实时可视化:仿真过程可视化
  6. 性能分析:求解时间、跟踪误差、约束满足度
相关推荐
leaves falling2 小时前
C++ 继承详解:从入门到深入
开发语言·c++
草木红2 小时前
Python 中使用 Docker Compose
开发语言·python·docker·flask
南宫萧幕2 小时前
从零构建飞行汽车混合动力能量管理系统(含电池、增程器与EMS策略)
matlab·汽车·控制·pid
lsx2024062 小时前
PostgreSQL WITH 子句详解
开发语言
ID_180079054732 小时前
京东商品详情 API 数据分析业务场景 + JSON 返回参考
java·开发语言
周杰伦fans2 小时前
C# CAD二次开发:RotatedDimension 旋转标注完全指南
开发语言·c#
郝学胜-神的一滴2 小时前
Python魔法函数深度探索|从工具实操到核心应用,解锁语言底层的优雅密码
开发语言·数据库·人工智能·python·pycharm
她说彩礼65万2 小时前
C语言 函数指针
c语言·开发语言·算法
阿_旭2 小时前
基于YOLO26深度学习的【无人机视角DaMa检测】与语音提示系统【python源码+Pyqt5界面+数据集+训练代码】
python·深度学习·无人机