手把手教你学simulink（11.5）--Simulink 航空航天场景：基于Simulink的飞机自动驾驶系统设计与仿真

 % 飞机动力学模型

    
 function dxdt = aircraft_dynamics(t, x, u)
    
     % 状态向量 x = [x, y, z, vx, vy, vz, phi, theta, psi, p, q, r]
    
     % 控制输入 u = [delta_e, delta_a, delta_r, thrust]
    
  
    
     g = 9.81; % 重力加速度
    
     m = 10000; % 飞机质量
    
     Ixx = 1000; % 惯性矩
    
     Iyy = 1000;
    
     Izz = 1000;
    
  
    
     % 解包状态向量
    
     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);
    
     p = x(10);
    
     q = x(11);
    
     r = x(12);
    
  
    
     % 解包控制输入
    
     delta_e = u(1); % 升降舵偏转角
    
     delta_a = u(2); % 副翼偏转角
    
     delta_r = u(3); % 方向舵偏转角
    
     thrust = u(4);  % 发动机推力
    
  
    
     % 计算空气动力和力矩
    
     V = sqrt(vx^2 + vy^2 + vz^2);
    
     alpha = atan2(vz, vx); % 迎角
    
     beta = atan2(vy, sqrt(vx^2 + vz^2)); % 侧滑角
    
     CL = 0.5 * (alpha + delta_e); % 升力系数
    
     CD = 0.02 + 0.05 * CL^2; % 阻力系数
    
     L = 0.5 * rho * V^2 * S * CL; % 升力
    
     D = 0.5 * rho * V^2 * S * CD; % 阻力
    
     M = 0.5 * rho * V^2 * S * c * (CL * alpha + Cm * delta_e); % 俯仰力矩
    
     N = 0.5 * rho * V^2 * S * b * (Cn * beta + Cn_delta_r * delta_r); % 偏航力矩
    
     L_roll = 0.5 * rho * V^2 * S * b * (Cl * beta + Cl_delta_a * delta_a); % 滚转力矩
    
  
    
     % 计算位置和速度导数
    
     dxdt = zeros(12, 1);
    
     dxdt(1) = vx;
    
     dxdt(2) = vy;
    
     dxdt(3) = vz;
    
     dxdt(4) = (thrust * cos(alpha) - D) / m;
    
     dxdt(5) = (thrust * sin(alpha) * sin(beta) - L * sin(beta)) / m;
    
     dxdt(6) = (thrust * sin(alpha) * cos(beta) - L * cos(beta) - m * g) / m;
    
  
    
     % 计算姿态角导数
    
     dxdt(7) = p + (q * sin(phi) + r * cos(phi)) * tan(theta);
    
     dxdt(8) = q * cos(phi) - r * sin(phi);
    
     dxdt(9) = (q * sin(phi) + r * cos(phi)) / cos(theta);
    
     dxdt(10) = (Iyy - Izz) / Ixx * q * r + L_roll / Ixx;
    
     dxdt(11) = (Izz - Ixx) / Iyy * p * r + M / Iyy;
    
     dxdt(12) = (Ixx - Iyy) / Izz * p * q + N / Izz;
    
 end
    
    
    
    

 % PID控制器

    
 function u = pid_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 = adaptive_controller(error, gain, learning_rate)
    
     gain = gain + learning_rate * error;
    
     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
    
  
    
 % GPS模型
    
 function gps_output = gps_model(position, noise_std)
    
     gps_output = position + randn(size(position)) * noise_std;
    
 end
    
    
    
    

 % 创建Simulink模型

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

 % 优化PID控制器参数

    
 Kp = 1.0;
    
 Ki = 0.1;
    
 Kd = 0.05;
    
  
    
 % 使用遗传算法优化参数
    
 options = optimoptions('ga', 'Display', 'iter', 'PopulationSize', 50, 'MaxGenerations', 100);
    
 [x, fval] = ga(@(x) -performance_metric(x(1), x(2), x(3)), 3, [], [], [], [], [0, 0, 0], [10, 10, 10], options);
    
  
    
 Kp = x(1);
    
 Ki = x(2);
    
 Kd = x(3);