一、无人机动力学模型与约束
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 快速开始
-
基本仿真:
matlab% 运行主仿真程序 main_constrained_mpc; -
自定义轨迹:
matlab% 修改轨迹参数 traj_type = 'lemniscate'; radius = 8; height = 4; -
调整约束:
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控制系统,包含:
- 非线性动力学模型:完整的12状态四旋翼模型
- 多种飞行约束:位置、速度、姿态、障碍物避让
- 非线性MPC求解:使用fmincon求解约束优化问题
- 参考轨迹生成:圆形、点到点、8字形等轨迹
- 实时可视化:仿真过程可视化
- 性能分析:求解时间、跟踪误差、约束满足度