手把手教你学simulink（27.8）--simulink鲁棒控制场景实例：无人机姿态控制

 % 生成随机的风速变化信号

    
 t = 0:0.01:10; % 时间向量 (秒)
    
 wind_speed = 5 + 2 * randn(size(t)); % 风速变化 (m/s)
    
  
    
 % 生成气流湍流信号
    
 turbulence = 1 * sin(2 * pi * 0.5 * t); % 气流湍流 (m/s)
    
  
    
 % 可视化外部扰动
    
 figure;
    
 subplot(2, 1, 1);
    
 plot(t, wind_speed);
    
 title('Random Wind Speed');
    
 xlabel('Time (s)');
    
 ylabel('Wind Speed (m/s)');
    
 grid on;
    
  
    
 subplot(2, 1, 2);
    
 plot(t, turbulence);
    
 title('Turbulence');
    
 xlabel('Time (s)');
    
 ylabel('Turbulence (m/s)');
    
 grid on;

 % 定义加权函数

    
 W1 = makeweight(0.1, 10, 10); % 姿态误差加权函数
    
 W2 = makeweight(0.01, 10, 10); % 控制输入加权函数
    
  
    
 % 构建广义对象
    
 A = [0 1 0 0 0 0; 0 0 0 0 0 0; 0 0 0 1 0 0; 0 0 0 0 0 0; 0 0 0 0 0 1; 0 0 0 0 0 0];
    
 B = [0; 1/Iy; 0; 1/Ix; 0; 1/Iz];
    
 C = [1 0 0 0 0 0; 0 0 1 0 0 0; 0 0 0 0 1 0];
    
 D = zeros(3, 3);
    
 G = ss(A, B, C, D); % 无人机姿态动力学模型
    
 P = augw(G, W1, [], W2); % 构建加权广义对象
    
  
    
 % 求解H∞ 控制器
    
 [K, CL, gamma] = hinfsyn(P);
    
  
    
 % 可视化H∞ 控制器的Bode图
    
 bode(K);
    
 title('H∞ Controller Bode Plot');
    
 grid on;

 % 定义参考模型

    
 A_ref = -1; % 参考模型的极点（负值表示稳定）
    
 B_ref = 1;  % 参考模型的输入增益
    
 ref_model = ss(A_ref, B_ref, 1, 0); % 创建状态空间模型

 % 定义自适应控制器参数

    
 theta = [1; 1]; % 初始控制器参数
    
 lambda = 0.1;   % 自适应增益

 % 初始化时间向量和仿真步长

    
 t = 0:0.01:10; % 时间向量 (秒)
    
 dt = t(2) - t(1); % 仿真步长
    
  
    
 % 初始化变量
    
 roll_angle = zeros(size(t)); % 滚转角
    
 pitch_angle = zeros(size(t)); % 俯仰角
    
 yaw_angle = zeros(size(t)); % 偏航角
    
 reference_signal = [10 * sin(0.5 * t); 5 * cos(0.5 * t); 0 * ones(size(t))]; % 参考姿态信号
    
 control_input = zeros(3, length(t)); % 控制输入（力矩）
    
  
    
 % 实现自适应控制律
    
 for k = 2:length(t)
    
     % 获取当前状态
    
     phi = roll_angle(k-1);
    
     theta = pitch_angle(k-1);
    
     psi = yaw_angle(k-1);
    
     
    
     % 计算参考模型的输出
    
     phi_ref = lsim(ref_model, reference_signal(1, :), t(k));
    
     theta_ref = lsim(ref_model, reference_signal(2, :), t(k));
    
     psi_ref = lsim(ref_model, reference_signal(3, :), t(k));
    
     
    
     % 计算误差
    
     e_phi = phi_ref - phi;
    
     e_theta = theta_ref - theta;
    
     e_psi = psi_ref - psi;
    
     
    
     % 更新控制器参数
    
     theta = theta + lambda * [e_phi; e_theta; e_psi] * control_input(:, k-1)';
    
     
    
     % 计算控制输入
    
     control_input(:, k) = -theta(1) * [phi; theta; psi] + theta(2) * [phi_ref; theta_ref; psi_ref];
    
     
    
     % 更新无人机姿态角（假设简单的动态模型）
    
     roll_angle(k) = roll_angle(k-1) + dt * (-0.5 * roll_angle(k-1) + control_input(1, k));
    
     pitch_angle(k) = pitch_angle(k-1) + dt * (-0.5 * pitch_angle(k-1) + control_input(2, k));
    
     yaw_angle(k) = yaw_angle(k-1) + dt * (-0.5 * yaw_angle(k-1) + control_input(3, k));
    
 end

 % 计算姿态误差

    
 roll_error = reference_signal(1, :) - roll_angle;
    
 pitch_error = reference_signal(2, :) - pitch_angle;
    
 yaw_error = reference_signal(3, :) - yaw_angle;
    
  
    
 % 可视化姿态误差
    
 figure;
    
 subplot(3, 1, 1);
    
 plot(t, roll_error);
    
 title('Roll Angle Error');
    
 xlabel('Time (s)');
    
 ylabel('Error (°)');
    
 grid on;
    
  
    
 subplot(3, 1, 2);
    
 plot(t, pitch_error);
    
 title('Pitch Angle Error');
    
 xlabel('Time (s)');
    
 ylabel('Error (°)');
    
 grid on;
    
  
    
 subplot(3, 1, 3);
    
 plot(t, yaw_error);
    
 title('Yaw Angle Error');
    
 xlabel('Time (s)');
    
 ylabel('Error (°)');
    
 grid on;
    
  
    
 % 计算控制输入幅度
    
 figure;
    
 plot(t, control_input');
    
 title('Control Input');
    
 xlabel('Time (s)');
    
 ylabel('Torque (N·m)');
    
 legend('Roll Torque', 'Pitch Torque', 'Yaw Torque');
    
 grid on;