手把手教你学simulink（11.6）--Simulink 航空航天场景：基于Simulink的航天器轨道转移仿真

 % 航天器轨道动力学模型

    
 function dxdt = orbit_dynamics(t, x, u)
    
     % 状态向量 x = [r_x, r_y, r_z, v_x, v_y, v_z]
    
     % 控制输入 u = [Fx, Fy, Fz]
    
  
    
     mu = 3.986e5; % 地球引力常数 (km^3/s^2)
    
  
    
     % 解包状态向量
    
     r_x = x(1);
    
     r_y = x(2);
    
     r_z = x(3);
    
     v_x = x(4);
    
     v_y = x(5);
    
     v_z = x(6);
    
  
    
     % 计算位置和速度导数
    
     r = norm([r_x, r_y, r_z]);
    
     a_gravity = -mu / r^3 * [r_x, r_y, r_z];
    
     a_control = u;
    
  
    
     dxdt = zeros(6, 1);
    
     dxdt(1) = v_x;
    
     dxdt(2) = v_y;
    
     dxdt(3) = v_z;
    
     dxdt(4) = a_gravity(1) + a_control(1);
    
     dxdt(5) = a_gravity(2) + a_control(2);
    
     dxdt(6) = a_gravity(3) + a_control(3);
    
 end
    
  
    
 % 航天器姿态动力学模型
    
 function dxdt = attitude_dynamics(t, x, u)
    
     % 状态向量 x = [phi, theta, psi, p, q, r]
    
     % 控制输入 u = [Mx, My, Mz]
    
  
    
     Ixx = 100; % 惯性矩 (kg*m^2)
    
     Iyy = 100;
    
     Izz = 100;
    
  
    
     % 解包状态向量
    
     phi = x(1);
    
     theta = x(2);
    
     psi = x(3);
    
     p = x(4);
    
     q = x(5);
    
     r = x(6);
    
  
    
     % 计算姿态角导数
    
     dxdt = zeros(6, 1);
    
     dxdt(1) = p + (q * sin(phi) + r * cos(phi)) * tan(theta);
    
     dxdt(2) = q * cos(phi) - r * sin(phi);
    
     dxdt(3) = (q * sin(phi) + r * cos(phi)) / cos(theta);
    
     dxdt(4) = (Iyy - Izz) / Ixx * q * r + u(1) / Ixx;
    
     dxdt(5) = (Izz - Ixx) / Iyy * p * r + u(2) / Iyy;
    
     dxdt(6) = (Ixx - Iyy) / Izz * p * q + u(3) / Izz;
    
 end
    
    
    
    

 % 轨道控制器

    
 function u = orbit_controller(error, Kp, Ki, Kd, dt, integral, prev_error)
    
     integral = integral + error * dt;
    
     derivative = (error - prev_error) / dt;
    
     u = Kp * error + Ki * integral + Kd * derivative;
    
 end
    
  
    
 % 姿态控制器
    
 function u = attitude_controller(error, Kp, Ki, Kd, dt, integral, prev_error)
    
     integral = integral + error * dt;
    
     derivative = (error - prev_error) / dt;
    
     u = Kp * error + Ki * integral + Kd * derivative;
    
 end
    
    
    
    

 % 陀螺仪模型

    
 function gyro_output = gyro_model(omega, noise_std)
    
     gyro_output = omega + randn(size(omega)) * noise_std;
    
 end
    
  
    
 % 星敏感器模型
    
 function star_sensor_output = star_sensor_model(attitude, noise_std)
    
     star_sensor_output = attitude + randn(size(attitude)) * noise_std;
    
 end
    
    
    
    

 % 创建Simulink模型

    
 model = 'orbit_transfer_simulation';
    
 open_system(model);
    
  
    
 % 添加子系统
    
 add_block('simulink/Sources/Step', [model '/Target Orbit']);
    
 add_block('simulink/Continuous/Integrator', [model '/Orbit State Integrator']);
    
 add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Orbit Dynamics']);
    
 add_block('simulink/Continuous/PID Controller', [model '/Orbit Controller']);
    
 add_block('simulink/Sinks/Scope', [model '/Orbit Output Scope']);
    
  
    
 add_block('simulink/Sources/Step', [model '/Target Attitude']);
    
 add_block('simulink/Continuous/Integrator', [model '/Attitude State Integrator']);
    
 add_block('simulink/User-Defined Functions/MATLAB Function', [model '/Attitude Dynamics']);
    
 add_block('simulink/Continuous/PID Controller', [model '/Attitude Controller']);
    
 add_block('simulink/Sinks/Scope', [model '/Attitude Output Scope']);
    
  
    
 % 连接模块
    
 connect_lines([model '/Target Orbit'], [model '/Orbit Controller']);
    
 connect_lines([model '/Orbit Controller'], [model '/Orbit Dynamics']);
    
 connect_lines([model '/Orbit Dynamics'], [model '/Orbit State Integrator']);
    
 connect_lines([model '/Orbit State Integrator'], [model '/Orbit Controller']);
    
 connect_lines([model '/Orbit Dynamics'], [model '/Orbit Output Scope']);
    
  
    
 connect_lines([model '/Target Attitude'], [model '/Attitude Controller']);
    
 connect_lines([model '/Attitude Controller'], [model '/Attitude Dynamics']);
    
 connect_lines([model '/Attitude Dynamics'], [model '/Attitude State Integrator']);
    
 connect_lines([model '/Attitude State Integrator'], [model '/Attitude Controller']);
    
 connect_lines([model '/Attitude Dynamics'], [model '/Attitude Output Scope']);
    
  
    
 % 设置参数
    
 set_param([model '/Orbit Controller'], 'P', '1.0');
    
 set_param([model '/Orbit Controller'], 'I', '0.1');
    
 set_param([model '/Orbit Controller'], 'D', '0.05');
    
  
    
 set_param([model '/Attitude Controller'], 'P', '1.0');
    
 set_param([model '/Attitude Controller'], 'I', '0.1');
    
 set_param([model '/Attitude Controller'], 'D', '0.05');
    
  
    
 % 运行仿真
    
 sim(model);
    
    
    
    

 % 优化轨道控制器参数

    
 Kp_orbit = 1.0;
    
 Ki_orbit = 0.1;
    
 Kd_orbit = 0.05;
    
  
    
 % 优化姿态控制器参数
    
 Kp_attitude = 1.0;
    
 Ki_attitude = 0.1;
    
 Kd_attitude = 0.05;
    
  
    
 % 使用遗传算法优化参数
    
 options = optimoptions('ga', 'Display', 'iter', 'PopulationSize', 50, 'MaxGenerations', 100);
    
 [x_orbit, fval_orbit] = ga(@(x) -orbit_performance_metric(x(1), x(2), x(3)), 3, [], [], [], [], [0, 0, 0], [10, 10, 10], options);
    
 [x_attitude, fval_attitude] = ga(@(x) -attitude_performance_metric(x(1), x(2), x(3)), 3, [], [], [], [], [0, 0, 0], [10, 10, 10], options);
    
  
    
 Kp_orbit = x_orbit(1);
    
 Ki_orbit = x_orbit(2);
    
 Kd_orbit = x_orbit(3);
    
  
    
 Kp_attitude = x_attitude(1);
    
 Ki_attitude = x_attitude(2);
    
 Kd_attitude = x_attitude(3);