MATLAB四旋翼无人机仿真轨迹跟踪滑模SMC控制 matlab 程序代码-优快云博客

四旋翼无人机的非线性动力学模型。
滑模控制器设计用于轨迹跟踪。
仿真与轨迹跟踪效果评估。

关键技术

非线性动力学建模。
滑模控制算法设计。
MATLAB/Simulink 仿真。

— 在这里插入图片描述

2. 代码实现

主程序文件 (`main.m`)

clc; clear; close all;

% 参数初始化
m = 1.0; % 质量 (kg)
g = 9.81; % 重力加速度 (m/s^2)
Ixx = 0.01; Iyy = 0.01; Izz = 0.02; % 惯性矩 (kg*m^2)
k = 0.5; % 控制增益

% 初始状态 [x, y, z, phi, theta, psi, u, v, w, p, q, r]
state = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0];

% 目标轨迹
t = linspace(0, 10, 1000); % 时间向量
x_ref = sin(t); % x方向参考轨迹
y_ref = cos(t); % y方向参考轨迹
z_ref = 1 + 0.1 * sin(2*t); % z方向参考轨迹

% 仿真参数
dt = 0.01; % 时间步长
N = length(t); % 仿真步数

% 存储变量
X = zeros(N, 3);
U = zeros(N, 4);

% 仿真循环
for i = 1:N
    % 当前状态
    x = state(1); y = state(2); z = state(3);
    phi = state(4); theta = state(5); psi = state(6);
    u = state(7); v = state(8); w = state(9);
    p = state(10); q = state(11); r = state(12);
    
    % 参考轨迹
    xd = x_ref(i); yd = y_ref(i); zd = z_ref(i);
    
    % 滑模控制器
    [u1, u2, u3, u4] = smc_controller(state, [xd, yd, zd], m, g, Ixx, Iyy, Izz, k);
    
    % 更新状态
    state = dynamics(state, [u1, u2, u3, u4], dt, m, g, Ixx, Iyy, Izz);
    
    % 存储数据
    X(i, :) = [x, y, z];
    U(i, :) = [u1, u2, u3, u4];
end

% 绘图
figure;
plot3(X(:, 1), X(:, 2), X(:, 3), 'b', 'LineWidth', 1.5);
hold on;
plot3(x_ref, y_ref, z_ref, 'r--', 'LineWidth', 1.5);
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
legend('Actual Trajectory', 'Reference Trajectory');
grid on; title('Trajectory Tracking');

滑模控制器函数 (`smc_controller.m`)

function [u1, u2, u3, u4] = smc_controller(state, ref, m, g, Ixx, Iyy, Izz, k)
    % 状态变量
    x = state(1); y = state(2); z = state(3);
    phi = state(4); theta = state(5); psi = state(6);
    u = state(7); v = state(8); w = state(9);
    p = state(10); q = state(11); r = state(12);
    
    % 参考轨迹
    xd = ref(1); yd = ref(2); zd = ref(3);
    
    % 滑模面
    s_x = u - xd;
    s_y = v - yd;
    s_z = w - zd;
    s_phi = p;
    s_theta = q;
    s_psi = r;
    
    % 控制律
    u1 = m * (g + k * sign(s_z)); % 垂直方向控制
    u2 = Ixx * (-k * sign(s_phi)); % 滚转角控制
    u3 = Iyy * (-k * sign(s_theta)); % 俯仰角控制
    u4 = Izz * (-k * sign(s_psi)); % 偏航角控制
end

动力学模型函数 (`dynamics.m`)

function state_dot = dynamics(state, u, dt, m, g, Ixx, Iyy, Izz)
    % 状态变量
    x = state(1); y = state(2); z = state(3);
    phi = state(4); theta = state(5); psi = state(6);
    u = state(7); v = state(8); w = state(9);
    p = state(10); q = state(11); r = state(12);
    
    % 控制输入
    u1 = u(1); u2 = u(2); u3 = u(3); u4 = u(4);
    
    % 动力学方程
    x_dot = u;
    y_dot = v;
    z_dot = w;
    
    phi_dot = p + q*sin(phi)*tan(theta) + r*cos(phi)*tan(theta);
    theta_dot = q*cos(phi) - r*sin(phi);
    psi_dot = q*sin(phi)/cos(theta) + r*cos(phi)/cos(theta);
    
    u_dot = -g*sin(theta) + u1/m;
    v_dot = g*sin(phi)*cos(theta) + u2/m;
    w_dot = g*cos(phi)*cos(theta) - u3/m;
    
    p_dot = (u2 / Ixx) - ((Iyy - Izz) * q * r) / Ixx;
    q_dot = (u3 / Iyy) - ((Izz - Ixx) * p * r) / Iyy;
    r_dot = (u4 / Izz) - ((Ixx - Iyy) * p * q) / Izz;
    
    % 状态导数
    state_dot = [x_dot; y_dot; z_dot; phi_dot; theta_dot; psi_dot; ...
                 u_dot; v_dot; w_dot; p_dot; q_dot; r_dot];
    
    % 更新状态
    state = state + state_dot * dt;
end