航天器姿态控制仿真程序包括刚体动力学模型、四元数运动学、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²)Jdiag([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];% 指向惯性系% 控制器参数PDKpdiag([10,10,10]);% 比例增益Kddiag([20,20,20]);% 微分增益% 仿真时间t00;tf30;% 仿真时长 (s)dt0.01;% 步长 (s)tt0:dt:tf;nlength(t);%% 状态初始化% 状态向量: [q (4x1); omega (3x1)]state[q0;omega0];historyzeros(7,n);history(:,1)state;%% 主循环RK4积分fori1:n-1% 当前状态qstate(1:4);omegastate(5:7);% 计算控制力矩taupd_controller(q,omega,q_desired,Kp,Kd,J);% 系统导数f(s)spacecraft_dynamics(s,tau,J);% RK4积分k1f(state);k2f(state0.5*dt*k1);k3f(state0.5*dt*k2);k4f(statedt*k3);statestate(dt/6)*(k12*k22*k3k4);% 归一化四元数防止数值误差state(1:4)state(1:4)/norm(state(1:4));history(:,i1)state;end%% 提取结果q_historyhistory(1:4,:);omega_historyhistory(5:7,:);%% 计算姿态误差角绕某轴旋转的角度error_anglezeros(1,n);fori1:n q_errattitude_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.mfunctiondsspacecraft_dynamics(state,tau,J)% 航天器姿态动力学与运动学% 输入:% state - [q (4x1); omega (3x1)]% tau - 控制力矩 (3x1)% J - 惯量矩阵 (3x3)% 输出:% ds - 状态导数qstate(1:4);omegastate(5:7);% 四元数运动学 (标量在前)% dq/dt 0.5 * Omega(omega) * qOmega[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];dq0.5*Omega*q;% 刚体动力学 (欧拉方程)% J * domega/dt omega × (J*omega) taudomegaJ\(tau-cross(omega,J*omega));ds[dq;domega];end2.3 PD控制器pd_controller.mfunctiontaupd_controller(q,omega,q_des,Kp,Kd,J)% PD姿态控制器% 输入:% q - 当前四元数% omega - 当前角速度% q_des - 期望四元数% Kp, Kd - 增益矩阵% J - 惯量矩阵% 输出:% tau - 控制力矩% 计算误差四元数q_errattitude_error(q,q_des);% 提取误差的矢量部分 (用于比例控制)qe_vecq_err(2:4);% 期望角速度为零姿态稳定omega_des[0;0;0];omega_erromega-omega_des;% PD控制律tau-Kp*qe_vec-Kd*omega_err;% 可选的解耦项前馈% tau tau cross(omega, J*omega); % 若需要精确线性化end2.4 四元数误差计算attitude_error.mfunctionq_errattitude_error(q,q_des)% 计算从当前姿态到期望姿态的误差四元数% q_err q_des^{-1} ⊗ q% 注四元数乘法定义为 q1⊗q2q_des_conj[q_des(1);-q_des(2:4)];% 共轭q_errquaternion_multiply(q_des_conj,q);end2.5 四元数乘法quaternion_multiply.mfunctionqquaternion_multiply(q1,q2)% 四元数乘法 (标量在前)% q q1 ⊗ q2a1q1(1);b1q1(2);c1q1(3);d1q1(4);a2q2(1);b2q2(2);c2q2(3);d2q2(4);q[a1*a2-b1*b2-c1*c2-d1*d2;a1*b2b1*a2c1*d2-d1*c2;a1*c2-b1*d2c1*a2d1*b2;a1*d2b1*c2-c1*b2d1*a2];end2.6 结果绘图plot_results.mfunctionplot_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);% 绘制本体坐标系在惯性系中的指向step50;% 每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);fori1:step:n qq_history(:,i);% 四元数到方向余弦矩阵Cquat2dcm(q);% 需要Aerospace Toolbox若无则用自定义函数% 本体坐标轴在惯性系的表示x_bodyC*[1;0;0];y_bodyC*[0;1;0];z_bodyC*[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);endxlabel(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);Ek0.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_errsqrt(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 ToolboxfunctionCquat2dcm(q)% q [q0, q1, q2, q3] (标量在前)q0q(1);q1q(2);q2q(3);q3q(4);C[q0^2q1^2-q2^2-q3^2,2*(q1*q2q0*q3),2*(q1*q3-q0*q2);2*(q1*q2-q0*q3),q0^2-q1^2q2^2-q3^2,2*(q2*q3q0*q1);2*(q1*q3q0*q2),2*(q2*q3-q0*q1),q0^2-q1^2-q2^2q3^2];end三、运行说明将所有函数保存为独立的.m文件文件名与函数名一致。直接运行主程序spacecraft_attitude_control.m。若没有 Aerospace Toolbox程序中的quat2dcm已在plot_results.m末尾提供了自定义实现无需额外工具箱。仿真结果包括四元数时间历程角速度响应姿态误差角收敛三维姿态演化旋转动能变化四元数归一化误差验证数值稳定性参考代码 MATLAB航天姿态控制仿真程序www.youwenfan.com/contentcsv/81101.html四、参数调整建议参数作用典型值Kp比例增益决定姿态刚度5~50 (对角阵)Kd微分增益决定阻尼10~100J惯量矩阵根据实际航天器设定根据三轴主惯量q0初始姿态四元数根据需要设定omega0初始角速度通常较小五、扩展功能加入外部干扰力矩在spacecraft_dynamics.m中添加tau_disturbance。执行机构模型如反作用飞轮的饱和与摩擦力矩。姿态确定结合星敏感器、陀螺测量模型。非线性控制如滑模控制、自适应控制。