基于MATLAB的三轴航天器姿态控制的仿真

基于MATLAB的三轴航天器姿态控制的仿真。包含了动力学模型、控制器设计和仿真分析。

1. MATLAB仿真

%% 三轴航天器姿态控制系统仿真
% 作者：基于MATLAB的航天器控制仿真
clear; close all; clc;

%% 航天器参数设置
J = [20, 1.2, 0.9;      % 惯性张量矩阵 [kg·m²]
     1.2, 17, 1.4;
     0.9, 1.4, 15];    

% 初始条件
q0 = [0.5; 0.5; 0.5; 0.5];  % 初始四元数 [q1 q2 q3 q0]
q0 = q0 / norm(q0);         % 四元数归一化
omega0 = [0.01; -0.01; 0.02]; % 初始角速度 [rad/s]

% 期望姿态
qd = [0; 0; 0; 1];          % 期望四元数 (零姿态)

% 仿真参数
dt = 0.1;                   % 采样时间 [s]
T_sim = 60;                 % 仿真时间 [s]
t = 0:dt:T_sim;             % 时间向量
N = length(t);

%% 控制器参数
% PD控制器增益
Kp = 5 * eye(3);           % 比例增益
Kd = 50 * eye(3);          % 微分增益

% 滑模控制器参数
Lambda = 0.5 * eye(3);     % 滑模面参数
K_smc = 2 * eye(3);        % 滑模控制增益
phi = 0.1;                 % 边界层厚度

%% 初始化状态变量
q_history = zeros(4, N);
omega_history = zeros(3, N);
control_torque = zeros(3, N);
error_quat = zeros(3, N);
error_angle = zeros(1, N);

% 设置初始状态
q_history(:, 1) = q0;
omega_history(:, 1) = omega0;

%% 外部干扰力矩
% 模拟太空环境干扰
disturbance = @(t) 0.01 * [sin(0.1*t); cos(0.15*t); 0.5*sin(0.2*t)];

%% 主仿真循环
for k = 1:N-1
    % 当前状态
    q_current = q_history(:, k);
    omega_current = omega_history(:, k);
    t_current = t(k);
    
    % 计算姿态误差四元数
    qe = quaternion_error(qd, q_current);
    error_quat(:, k) = qe(1:3);
    
    % 计算等效旋转误差角
    error_angle(k) = 2 * acos(abs(qe(4)));
    
    % 控制器选择（取消注释想要使用的控制器）
    
    % 1. PD控制器
    u = PD_controller(qe, omega_current, Kp, Kd);
    
    % 2. 滑模控制器（取消注释使用）
    % u = SMC_controller(qe, omega_current, J, K_smc, Lambda, phi);
    
    % 3. PID控制器（取消注释使用）
    % u = PID_controller(qe, omega_current, Kp, Kd, 0.1*eye(3), dt, k);
    
    % 添加干扰
    d = disturbance(t_current);
    u_total = u + d;
    
    % 存储控制力矩
    control_torque(:, k) = u;
    
    % 动力学和运动学更新
    [q_new, omega_new] = spacecraft_dynamics(q_current, omega_current, u_total, J, dt);
    
    % 存储新状态
    q_history(:, k+1) = q_new;
    omega_history(:, k+1) = omega_new;
end

% 计算最后的误差
qe_final = quaternion_error(qd, q_history(:, end));
error_angle(end) = 2 * acos(abs(qe_final(4)));

%% 绘制结果
plot_results(t, q_history, omega_history, control_torque, error_quat, error_angle);

%% 控制器性能分析
fprintf('控制器性能分析:\n');
fprintf('稳态姿态误差: %.4f rad\n', error_angle(end));
fprintf('最大控制力矩: %.4f N·m\n', max(max(abs(control_torque))));
fprintf('调节时间(2%%): %.2f s\n', find(error_angle < 0.02, 1) * dt);

%% 函数定义

% 四元数误差计算
function qe = quaternion_error(qd, q)
    % 计算期望姿态到当前姿态的误差四元数
    qe = quaternion_multiply(quaternion_conjugate(qd), q);
    if qe(4) < 0  % 保证标量部分非负
        qe = -qe;
    end
end

% 四元数乘法
function q = quaternion_multiply(p, r)
    q = [p(4)*r(1:3) + r(4)*p(1:3) + cross(p(1:3), r(1:3));
         p(4)*r(4) - dot(p(1:3), r(1:3))];
end

% 四元数共轭
function qc = quaternion_conjugate(q)
    qc = [-q(1:3); q(4)];
end

% PD控制器
function u = PD_controller(qe, omega, Kp, Kd)
    qe_vec = qe(1:3);  % 误差四元数的矢量部分
    u = -Kp * qe_vec - Kd * omega;
end

% 滑模控制器
function u = SMC_controller(qe, omega, J, K, Lambda, phi)
    qe_vec = qe(1:3);
    
    % 滑模面
    s = omega + Lambda * qe_vec;
    
    % 饱和函数代替符号函数（减少抖振）
    sat_s = saturation(s, phi);
    
    % 滑模控制律
    u = -K * sat_s - Kp * qe_vec - Kd * omega;
end

% 饱和函数
function y = saturation(x, phi)
    y = x / phi;
    for i = 1:length(y)
        if y(i) > 1
            y(i) = 1;
        elseif y(i) < -1
            y(i) = -1;
        end
    end
end

% PID控制器（带积分项）
function u = PID_controller(qe, omega, Kp, Kd, Ki, dt, k, persistent_vars)
    persistent integral_error;
    
    if k == 1 || isempty(integral_error)
        integral_error = zeros(3,1);
    end
    
    qe_vec = qe(1:3);
    
    % 积分项更新
    integral_error = integral_error + qe_vec * dt;
    
    % 抗饱和处理
    integral_error = max(min(integral_error, 0.1), -0.1);
    
    u = -Kp * qe_vec - Kd * omega - Ki * integral_error;
end

% 航天器动力学更新
function [q_new, omega_new] = spacecraft_dynamics(q, omega, u, J, dt)
    % 角速度动力学（欧拉方程）
    omega_dot = J \ (u - cross(omega, J * omega));
    
    % 角速度更新
    omega_new = omega + omega_dot * dt;
    
    % 四元数运动学更新
    Omega = [0, -omega_new(1), -omega_new(2), -omega_new(3);
             omega_new(1), 0, omega_new(3), -omega_new(2);
             omega_new(2), -omega_new(3), 0, omega_new(1);
             omega_new(3), omega_new(2), -omega_new(1), 0];
    
    q_dot = 0.5 * Omega * q;
    q_new = q + q_dot * dt;
    q_new = q_new / norm(q_new);  % 四元数归一化
end

% 结果绘图函数
function plot_results(t, q, omega, u, error_quat, error_angle)
    figure('Position', [100, 100, 1200, 800]);
    
    % 四元数时间响应
    subplot(3, 2, 1);
    plot(t, q);
    title('四元数时间响应');
    xlabel('时间 (s)');
    ylabel('四元数');
    legend('q_1', 'q_2', 'q_3', 'q_0');
    grid on;
    
    % 角速度时间响应
    subplot(3, 2, 2);
    plot(t, omega);
    title('角速度时间响应');
    xlabel('时间 (s)');
    ylabel('角速度 (rad/s)');
    legend('\omega_x', '\omega_y', '\omega_z');
    grid on;
    
    % 控制力矩
    subplot(3, 2, 3);
    plot(t(1:end-1), u(:, 1:end-1));
    title('控制力矩');
    xlabel('时间 (s)');
    ylabel('力矩 (N·m)');
    legend('u_x', 'u_y', 'u_z');
    grid on;
    
    % 姿态误差（四元数矢量部分）
    subplot(3, 2, 4);
    plot(t, error_quat);
    title('姿态误差（四元数矢量部分）');
    xlabel('时间 (s)');
    ylabel('误差');
    legend('e_1', 'e_2', 'e_3');
    grid on;
    
    % 等效旋转误差角
    subplot(3, 2, 5);
    plot(t, error_angle);
    title('等效旋转误差角');
    xlabel('时间 (s)');
    ylabel('误差角 (rad)');
    grid on;
    
    % 相平面图（误差角 vs 角速度）
    subplot(3, 2, 6);
    plot(error_quat(1, :), omega(1, :), 'b', ...
         error_quat(2, :), omega(2, :), 'r', ...
         error_quat(3, :), omega(3, :), 'g');
    title('相平面图');
    xlabel('姿态误差');
    ylabel('角速度 (rad/s)');
    legend('x轴', 'y轴', 'z轴');
    grid on;
end

2. 进阶版本 - 带执行机构特性的仿真

%% 带反作用轮和动量管理的完整仿真
function advanced_spacecraft_simulation()
    clear; close all; clc;
    
    % 航天器参数
    J = diag([20, 17, 15]); % 对角惯性矩阵
    
    % 反作用轮参数
    n_wheels = 4;           % 4个斜装反作用轮
    A = (1/sqrt(3)) * [1,  1, -1, -1;    % 安装矩阵
                       1, -1, -1,  1;
                       1, -1,  1, -1];
    
    Jw = 0.1;               % 每个轮的转动惯量 [kg·m²]
    hw_max = 10;            % 最大角动量 [N·m·s]
    tau_max = 0.5;          % 最大输出力矩 [N·m]
    
    % 初始化
    q0 = [0.3; -0.2; 0.4; sqrt(1-0.3^2-0.2^2-0.4^2)];
    omega0 = [0; 0; 0];
    hw0 = zeros(n_wheels, 1);  % 反作用轮初始角动量
    
    % 控制器参数
    Kp = 8 * eye(3);
    Kd = 60 * eye(3);
    
    % 动量管理控制器
    K_mmt = 0.1;
    
    % 仿真
    dt = 0.05;
    T_sim = 100;
    t = 0:dt:T_sim;
    
    % 主循环
    for k = 1:length(t)-1
        % 当前状态
        q = q_history(:, k);
        omega = omega_history(:, k);
        hw = hw_history(:, k);
        
        % 计算控制力矩（航天器本体）
        qe = quaternion_error(qd, q);
        u_body = -Kp * qe(1:3) - Kd * omega;
        
        % 动量管理
        u_mmt = -K_mmt * hw;
        
        % 总控制力矩
        u_total = u_body + u_mmt;
        
        % 分配到反作用轮
        tau_wheels = pinv(A) * u_total;
        
        % 力矩限幅
        tau_wheels = max(min(tau_wheels, tau_max), -tau_max);
        
        % 更新反作用轮角动量
        hw_new = hw + tau_wheels * dt;
        
        % 角动量限幅（动量卸载触发条件）
        if max(abs(hw_new)) > hw_max * 0.8
            % 这里可以添加磁力矩器卸载逻辑
            fprintf('动量接近饱和，需要卸载\n');
        end
        
        % 更新航天器状态
        % ...（动力学更新）
    end
end

3. 特性说明

主要功能：

1. 多种控制器：PD、滑模控制、PID
2. 完整动力学：欧拉动力学方程 + 四元数运动学
3. 干扰模拟：太空环境干扰力矩
4. 性能分析：稳态误差、调节时间等指标
5. 可视化：多维度结果展示

使用建议：

1. 参数整定：
   • 调整 Kp, Kd 来平衡响应速度和超调
   • 增大增益提高响应速度，但可能引起振荡
2. 控制器选择：
   • PD控制器：简单可靠，适合大多数情况
   • 滑模控制：抗干扰能力强，但可能有抖振
   • PID控制器：可消除稳态误差
3. 扩展方向：
   • 添加执行机构动力学（反作用轮/推力器）
   • 实现动量管理策略
   • 加入参数不确定性鲁棒性测试
   • 开发姿态机动轨迹规划

参考代码 基于三轴航天器的姿态控制 www.youwenfan.com/contentcni/64271.html

这个仿真框架为研究和学习三轴航天器姿态控制提供了完整的基础，您可以根据具体需求进行修改和扩展。