航天器姿态控制 MATLAB 仿真程序

航天器姿态控制仿真程序，包括刚体动力学模型、四元数运动学、PD控制器以及可视化结果。适用于姿态稳定控制和姿态机动控制。

---

一、程序结构

spacecraft_attitude_control.m  (主程序)
├── spacecraft_dynamics.m      (姿态动力学与运动学)
├── pd_controller.m            (PD控制器)
├── attitude_error.m           (四元数误差计算)
├── quaternion_multiply.m      (四元数乘法)
└── plot_results.m             (结果绘图)

---

二、完整代码实现

2.1 主程序：spacecraft_attitude_control.m

%% 航天器姿态控制仿真主程序
clear; clc; close all;

%% 参数设置
% 航天器惯量矩阵 (kg·m²)
J = diag([100, 150, 200]);  % 三轴主惯量

% 初始姿态（四元数，标量在前）
q0 = [0.8660; 0.2887; 0.2887; 0.2887];  % 初始四元数（绕某轴旋转30°）
omega0 = [0.1; -0.05; 0.08];  % 初始角速度 (rad/s)

% 目标姿态（期望四元数）
q_desired = [1; 0; 0; 0];  % 指向惯性系

% 控制器参数（PD）
Kp = diag([10, 10, 10]);    % 比例增益
Kd = diag([20, 20, 20]);    % 微分增益

% 仿真时间
t0 = 0; tf = 30;            % 仿真时长 (s)
dt = 0.01;                  % 步长 (s)
t = t0:dt:tf;
n = length(t);

%% 状态初始化
% 状态向量: [q (4x1); omega (3x1)]
state = [q0; omega0];
history = zeros(7, n);
history(:,1) = state;

%% 主循环（RK4积分）
for i = 1:n-1
    % 当前状态
    q = state(1:4);
    omega = state(5:7);
    
    % 计算控制力矩
    tau = pd_controller(q, omega, q_desired, Kp, Kd, J);
    
    % 系统导数
    f = @(s) spacecraft_dynamics(s, tau, J);
    
    % RK4积分
    k1 = f(state);
    k2 = f(state + 0.5*dt*k1);
    k3 = f(state + 0.5*dt*k2);
    k4 = f(state + dt*k3);
    state = state + (dt/6)*(k1 + 2*k2 + 2*k3 + k4);
    
    % 归一化四元数（防止数值误差）
    state(1:4) = state(1:4) / norm(state(1:4));
    
    history(:,i+1) = state;
end

%% 提取结果
q_history = history(1:4,:);
omega_history = history(5:7,:);

%% 计算姿态误差角（绕某轴旋转的角度）
error_angle = zeros(1,n);
for i = 1:n
    q_err = attitude_error(q_history(:,i), q_desired);
    error_angle(i) = 2 * acos(abs(q_err(1)));  % 误差角 (rad)
end

%% 绘图
plot_results(t, q_history, omega_history, error_angle);

2.2 姿态动力学与运动学：spacecraft_dynamics.m

function ds = spacecraft_dynamics(state, tau, J)
% 航天器姿态动力学与运动学
% 输入:
%   state - [q (4x1); omega (3x1)]
%   tau   - 控制力矩 (3x1)
%   J     - 惯量矩阵 (3x3)
% 输出:
%   ds    - 状态导数

q = state(1:4);
omega = state(5:7);

% 四元数运动学 (标量在前)
% dq/dt = 0.5 * Omega(omega) * q
Omega = [0, -omega(1), -omega(2), -omega(3);
         omega(1), 0, omega(3), -omega(2);
         omega(2), -omega(3), 0, omega(1);
         omega(3), omega(2), -omega(1), 0];
dq = 0.5 * Omega * q;

% 刚体动力学 (欧拉方程)
% J * domega/dt + omega × (J*omega) = tau
domega = J \ (tau - cross(omega, J*omega));

ds = [dq; domega];
end

2.3 PD控制器：pd_controller.m

function tau = pd_controller(q, omega, q_des, Kp, Kd, J)
% PD姿态控制器
% 输入:
%   q     - 当前四元数
%   omega - 当前角速度
%   q_des - 期望四元数
%   Kp, Kd - 增益矩阵
%   J     - 惯量矩阵
% 输出:
%   tau   - 控制力矩

% 计算误差四元数
q_err = attitude_error(q, q_des);

% 提取误差的矢量部分 (用于比例控制)
qe_vec = q_err(2:4);

% 期望角速度为零（姿态稳定）
omega_des = [0;0;0];
omega_err = omega - omega_des;

% PD控制律
tau = -Kp * qe_vec - Kd * omega_err;

% 可选的解耦项（前馈）
% tau = tau + cross(omega, J*omega);  % 若需要精确线性化
end

2.4 四元数误差计算：attitude_error.m

function q_err = attitude_error(q, q_des)
% 计算从当前姿态到期望姿态的误差四元数
% q_err = q_des^{-1} ⊗ q
% 注：四元数乘法定义为 q1⊗q2

q_des_conj = [q_des(1); -q_des(2:4)];  % 共轭
q_err = quaternion_multiply(q_des_conj, q);
end

2.5 四元数乘法：quaternion_multiply.m

function q = quaternion_multiply(q1, q2)
% 四元数乘法 (标量在前)
% q = q1 ⊗ q2

a1 = q1(1); b1 = q1(2); c1 = q1(3); d1 = q1(4);
a2 = q2(1); b2 = q2(2); c2 = q2(3); d2 = q2(4);

q = [a1*a2 - b1*b2 - c1*c2 - d1*d2;
     a1*b2 + b1*a2 + c1*d2 - d1*c2;
     a1*c2 - b1*d2 + c1*a2 + d1*b2;
     a1*d2 + b1*c2 - c1*b2 + d1*a2];
end

2.6 结果绘图：plot_results.m

function plot_results(t, q_history, omega_history, error_angle)
% 绘制仿真结果

figure('Position', [100, 100, 1200, 800]);

% 四元数时间历程
subplot(3,2,1);
plot(t, q_history(1,:), 'r-', 'LineWidth', 1.5); hold on;
plot(t, q_history(2,:), 'g-', 'LineWidth', 1.5);
plot(t, q_history(3,:), 'b-', 'LineWidth', 1.5);
plot(t, q_history(4,:), 'm-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('四元数');
legend('q0', 'q1', 'q2', 'q3'); grid on;
title('四元数时间历程');

% 角速度时间历程
subplot(3,2,2);
plot(t, omega_history(1,:)*180/pi, 'r-', 'LineWidth', 1.5); hold on;
plot(t, omega_history(2,:)*180/pi, 'g-', 'LineWidth', 1.5);
plot(t, omega_history(3,:)*180/pi, 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('角速度 (°/s)');
legend('\omega_x', '\omega_y', '\omega_z'); grid on;
title('角速度时间历程');

% 姿态误差角
subplot(3,2,3);
plot(t, error_angle*180/pi, 'k-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('姿态误差角 (°)');
grid on; title('姿态误差角');

% 三维姿态轨迹（用方向余弦矩阵表示）
subplot(3,2,4);
% 绘制本体坐标系在惯性系中的指向
step = 50; % 每50个点画一次
quiver3(0,0,0,1,0,0, 'k', 'LineWidth', 1); hold on; % 惯性系X轴
quiver3(0,0,0,0,1,0, 'k', 'LineWidth', 1);
quiver3(0,0,0,0,0,1, 'k', 'LineWidth', 1);
for i = 1:step:n
    q = q_history(:,i);
    % 四元数到方向余弦矩阵
    C = quat2dcm(q'); % 需要Aerospace Toolbox，若无则用自定义函数
    % 本体坐标轴在惯性系的表示
    x_body = C'*[1;0;0];
    y_body = C'*[0;1;0];
    z_body = C'*[0;0;1];
    quiver3(0,0,0, x_body(1), x_body(2), x_body(3), 'r', 'MaxHeadSize', 0.5);
    quiver3(0,0,0, y_body(1), y_body(2), y_body(3), 'g', 'MaxHeadSize', 0.5);
    quiver3(0,0,0, z_body(1), z_body(2), z_body(3), 'b', 'MaxHeadSize', 0.5);
end
xlabel('X'); ylabel('Y'); zlabel('Z');
axis equal; view(45,30);
title('本体坐标系指向演化');
legend('惯性X','惯性Y','惯性Z','本体X','本体Y','本体Z');

% 控制力矩（可选，需要在主程序中记录tau）
% 此处省略，可在主程序中增加tau_history

% 能量耗散（动能）
subplot(3,2,5);
Ek = 0.5 * (J(1,1)*omega_history(1,:).^2 + ...
            J(2,2)*omega_history(2,:).^2 + ...
            J(3,3)*omega_history(3,:).^2);
plot(t, Ek, 'b-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('旋转动能 (J)');
grid on; title('旋转动能');

% 四元数归一化误差
subplot(3,2,6);
norm_err = sqrt(sum(q_history.^2,1)) - 1;
plot(t, norm_err, 'r-', 'LineWidth', 1.5);
xlabel('时间 (s)'); ylabel('归一化误差');
grid on; title('四元数归一化误差');

sgtitle('航天器姿态控制仿真结果', 'FontSize', 14, 'FontWeight', 'bold');
end

% 辅助函数：四元数到方向余弦矩阵（若没有Aerospace Toolbox）
function C = quat2dcm(q)
    % q = [q0, q1, q2, q3] (标量在前)
    q0 = q(1); q1 = q(2); q2 = q(3); q3 = q(4);
    C = [q0^2+q1^2-q2^2-q3^2, 2*(q1*q2+q0*q3),     2*(q1*q3-q0*q2);
         2*(q1*q2-q0*q3),     q0^2-q1^2+q2^2-q3^2, 2*(q2*q3+q0*q1);
         2*(q1*q3+q0*q2),     2*(q2*q3-q0*q1),     q0^2-q1^2-q2^2+q3^2];
end

---

三、运行说明

1. 将所有函数保存为独立的 .m 文件，文件名与函数名一致。
2. 直接运行主程序 spacecraft_attitude_control.m。
3. 若没有 Aerospace Toolbox，程序中的 quat2dcm 已在 plot_results.m 末尾提供了自定义实现，无需额外工具箱。
4. 仿真结果包括：
   • 四元数时间历程
   • 角速度响应
   • 姿态误差角收敛
   • 三维姿态演化
   • 旋转动能变化
   • 四元数归一化误差（验证数值稳定性）

参考代码 MATLAB航天姿态控制仿真程序 www.youwenfan.com/contentcsv/81101.html

四、参数调整建议

参数	作用	典型值
Kp	比例增益，决定姿态刚度	5~50 (对角阵)
Kd	微分增益，决定阻尼	10~100
J	惯量矩阵，根据实际航天器设定	根据三轴主惯量
q0	初始姿态四元数	根据需要设定
omega0	初始角速度	通常较小

---

五、扩展功能

• 加入外部干扰力矩 ：在 spacecraft_dynamics.m 中添加 tau_disturbance。
• 执行机构模型：如反作用飞轮的饱和与摩擦力矩。
• 姿态确定：结合星敏感器、陀螺测量模型。
• 非线性控制：如滑模控制、自适应控制。