手把手教你学simulink（11.1）--Simulink 航空航天场景：基于Simulink的火箭发射和轨道插入仿真

 % 火箭动力学模型

    
 function dxdt = rocket_dynamics(t, x, u)
    
     % 状态向量 x = [x, y, z, vx, vy, vz, phi, theta, psi, mass]
    
     % 控制输入 u = [Fx, Fy, Fz, Mx, My, Mz, thrust]
    
  
    
     g = 9.81; % 重力加速度
    
     rho = 1.225; % 空气密度（海平面）
    
     S = 10; % 火箭参考面积
    
     Cd = 0.5; % 阻力系数
    
  
    
     % 解包状态向量
    
     x_pos = x(1);
    
     y_pos = x(2);
    
     z_pos = x(3);
    
     vx = x(4);
    
     vy = x(5);
    
     vz = x(6);
    
     phi = x(7);
    
     theta = x(8);
    
     psi = x(9);
    
     mass = x(10);
    
  
    
     % 解包控制输入
    
     Fx = u(1);
    
     Fy = u(2);
    
     Fz = u(3);
    
     Mx = u(4);
    
     My = u(5);
    
     Mz = u(6);
    
     thrust = u(7);
    
  
    
     % 计算空气阻力
    
     v = sqrt(vx^2 + vy^2 + vz^2);
    
     drag = 0.5 * rho * v^2 * S * Cd;
    
  
    
     % 计算位置和速度导数
    
     dxdt = zeros(10, 1);
    
     dxdt(1) = vx;
    
     dxdt(2) = vy;
    
     dxdt(3) = vz;
    
     dxdt(4) = (Fx - drag * vx / v) / mass;
    
     dxdt(5) = (Fy - drag * vy / v) / mass;
    
     dxdt(6) = (Fz - drag * vz / v - mass * g) / mass;
    
  
    
     % 计算姿态角导数
    
     p = (1/Ixx) * (Mx - (Iyy - Izz) * sin(phi) * cos(phi) * theta * psi);
    
     q = (1/Iyy) * (My - (Izz - Ixx) * sin(phi) * cos(phi) * phi * psi);
    
     r = (1/Izz) * (Mz - (Ixx - Iyy) * sin(phi) * cos(phi) * phi * theta);
    
  
    
     dxdt(7) = p + sin(phi) * tan(theta) * q + cos(phi) * tan(theta) * r;
    
     dxdt(8) = cos(phi) * q - sin(phi) * r;
    
     dxdt(9) = (sin(phi) / cos(theta)) * q + (cos(phi) / cos(theta)) * r;
    
  
    
     % 计算质量变化
    
     fuel_rate = thrust / Isp / g; % 燃料消耗率
    
     dxdt(10) = -fuel_rate;
    
 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 u = orbit_controller(error, gain)
    
     u = gain * error;
    
 end

 % 陀螺仪模型

    
 function gyro_output = gyro_model(omega, noise_std)
    
     gyro_output = omega + randn(size(omega)) * noise_std;
    
 end
    
  
    
 % 加速度计模型
    
 function acc_output = acc_model(acc, noise_std)
    
     acc_output = acc + randn(size(acc)) * noise_std;
    
 end

 % 创建Simulink模型

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

 % 优化PID控制器参数

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