MATLAB航天姿态控制仿真程序_simulink动力学_航天器控制_attitudekinematic_动力学程序_动力学si

close all;clear;clc; tic; %------------------清屏----------------- global J Kd Kp; global u1 u2 u3; J=[1000;1500;1800]; K=[3000.00000000000,0,0; 0,9000.00000000000,0; 0,0,7200; 3250.00000000000,0,0; 0,12000.0000000000,0; 0,0,5400.00000000000;]; Kd=-[K(1,1);K(2,2);K(3,3)]; Kp=-[K(4,1);K(5,2);K(6,3)]; step=0.01;total=1000; state0=[10; 20; -30; 10 ;20; -30]*pi/180; %------------------初始化数据--------------- for i=1:total; t=i*step; tout(i)=t; stateout(i,:)=state0; %-----------------龙格库塔求解过程------------ K1=step*Attitude_ode(t,state0); K2=step*Attitude_ode(t+step/2,state0+K1/2); K3=step*Attitude_ode(t+step/2,state0+K2/2); K4=step*Attitude_ode(t+step,state0+K3); state0=state0+(K1+2*K2+2*K3+K4)/6; %-------------------------------------------- control(i,:)=[u1 u2 u3]; end %------------------取出数据------------------ w1=stateout(:,1)*180/pi; w2=stateout(:,2)*180/pi; w3=stateout(:,3)*180/pi; theta1=stateout(:,4)*180/pi; theta2=stateout(:,5)*180/pi; theta3=stateout(:,6)*180/pi; Mx=control(:,1); My=control(:,2); Mz=control(:,3); %--------------------绘制图像-------------------- subplot(2,1,1) plot(tout,w1,tout,w2,tout,w3,'LineWidth',1); grid on; legend('ω1','ω2','ω3'); ylabel('角速度(deg/s)'); %--------------------角速度-------------------- subplot(2,1,2) plot(tout,theta1,tout,theta2,tout,theta3,'LineWidth',1); grid on; legend('θ','φ','ψ'); ylabel('姿态角(deg)'); xlabel('时间（s）'); %--------------------姿态角-------------------- figure; plot(tout,Mx,tout,My,tout,Mz,'LineWidth',1); grid on; legend('Mx','My','Mz'); ylabel('控制力矩(N.m)'); xlabel('时间（s）'); %--------------------控制力矩-------------------- %------------------------------------------------- toc;