专家控制作业

function [sys,x0,str,ts,simStateCompliance] = usv_model(t,x,u,flag,T,K,alpha,beta,Ad,wd)
% USV船舶动力学模型 - 精简版
% 输入: u(1)=舵角, u(2)=扰动
% 输出: sys(1)=航向角, sys(2)=转艏速度
% 状态: x(1)=psi, x(2)=r

switch flag
    case 0
        [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes;
    case 1
        sys = mdlDerivatives(t,x,u,T,K,alpha,beta,Ad,wd);
    case 3
        sys = mdlOutputs(t,x,u);
    case {2,4,9}
        sys = [];
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end

% 初始化函数
function [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes()
    sizes = simsizes;
    sizes.NumContStates  = 2;
    sizes.NumDiscStates  = 0;
    sizes.NumOutputs     = 2;
    sizes.NumInputs      = 2;
    sizes.DirFeedthrough = 0;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0 = [0.1; 0];  % 初始航向角0.1rad, 初始转艏速度0
    str = [];
    ts = [0 0];
    simStateCompliance = 'UnknownSimState';
end

% 状态导数计算
function sys = mdlDerivatives(t,x,u,T,K,alpha,beta,Ad,wd)
    psi = x(1);
    r = x(2);
    delta = u(1);
    d_dist = u(2);
    
    % 非线性项
    H = alpha * r + beta * r^3;
    
    % 动力学方程
    r_dot = (K * delta + d_dist - K * H) / T;
    
    % 加入白噪声扰动
    noise = sqrt(0.01) * randn;
    r_dot = r_dot + noise / T;
    
    % 航向角变化率
    psi_dot = r;
    
    sys = [psi_dot; r_dot];
end

% 输出函数
function sys = mdlOutputs(t,x,u)
    sys = [x(1); x(2)];
end
end

function [sys,x0,str,ts,simStateCompliance] = expert_controller(t,x,u,flag)
% 专家控制器 - 基于规则的控制器
% 输入: u(1)=误差, u(2)=误差变化率
% 输出: sys=舵角控制量

switch flag
    case 0
        [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes;
    case 2
        sys = mdlUpdate(t,x,u);
    case 3
        sys = mdlOutputs(t,x,u);
    case {1,4,9}
        sys = [];
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end

% 初始化函数
function [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes()
    sizes = simsizes;
    sizes.NumContStates  = 0;
    sizes.NumDiscStates  = 1;  % 存储上一拍控制量
    sizes.NumOutputs     = 1;
    sizes.NumInputs      = 2;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0 = [0];  % 初始控制量为0
    str = [];
    ts = [0.05 0];  % 采样时间0.05s
    simStateCompliance = 'UnknownSimState';
end

% 状态更新函数
function sys = mdlUpdate(t,x,u)
    sys = x;  % 保持状态不变，在输出函数中更新
end

% 输出函数 - 实现专家控制规则
function sys = mdlOutputs(t,x,u)
    % 获取输入
    e = u(1);      % 当前误差
    ec = u(2);     % 误差变化率
    delta_prev = x(1);  % 上一拍控制量
    
    % 控制器参数
    e1 = 0.02;     % 微调阈值
    e2 = 0.2;      % 精细控制阈值
    e3 = 0.5;      % 饱和控制阈值
    delta_max = 0.5; % 最大舵角
    
    abs_e = abs(e);
    
    % 专家控制规则
    % 规则1: 大误差区 - 饱和控制
    if abs_e > e3
        delta = sign(e) * delta_max;
    
    % 规则2&3: 中误差区 - 智能决策
    elseif abs_e > e2
        if e * ec > 0  % 误差在增大
            delta = sign(e) * 0.7 * delta_max;  % 增强控制
        else
            delta = sign(e) * 0.3 * delta_max;  % 保守控制
        end
    
    % 规则4: 小误差区 - 精细控制
    elseif abs_e > e1
        delta = 1.0 * e + 0.5 * ec;  % PD控制
        % 限幅处理
        if delta > delta_max
            delta = delta_max;
        elseif delta < -delta_max
            delta = -delta_max;
        end
    
    % 规则5: 微误差区 - 保持控制
    else
        delta = delta_prev;  % 保持上一拍控制量
    end
    
    % 更新状态
    x(1) = delta;
    sys = delta;
end
end

function [sys,x0,str,ts,simStateCompliance] = epid_controller(t,x,u,flag)
% 专家PID控制器 - 参数自适应的PID控制器
% 输入: u(1)=误差, u(2)=误差变化率
% 输出: sys=舵角控制量

switch flag
    case 0
        [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes;
    case 2
        sys = mdlUpdate(t,x,u);
    case 3
        sys = mdlOutputs(t,x,u);
    case {1,4,9}
        sys = [];
    otherwise
        DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end

% 初始化函数
function [sys,x0,str,ts,simStateCompliance] = mdlInitializeSizes()
    sizes = simsizes;
    sizes.NumContStates  = 0;
    sizes.NumDiscStates  = 2;  % x(1)=积分项, x(2)=上一拍误差
    sizes.NumOutputs     = 1;
    sizes.NumInputs      = 2;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0 = [0; 0];  % 积分项和上一拍误差初始为0
    str = [];
    ts = [0.05 0];  % 采样时间0.05s
    simStateCompliance = 'UnknownSimState';
end

% 状态更新函数
function sys = mdlUpdate(t,x,u)
    e = u(1);  % 当前误差
    integral = x(1);  % 当前积分项
    
    % 更新积分项（前向欧拉法）
    Ts = 0.05;
    new_integral = integral + e * Ts;
    
    % 更新上一拍误差
    sys = [new_integral; e];
end

% 输出函数 - 实现专家PID控制
function sys = mdlOutputs(t,x,u)
    % 获取输入和状态
    e = u(1);        % 当前误差
    ec = u(2);       % 误差变化率
    integral = x(1); % 积分项
    
    % 控制器参数
    e1 = 0.03;      % 小误差阈值
    e2 = 0.3;       % 中误差阈值
    e3 = 0.6;       % 大误差阈值
    delta_max = 0.5; % 最大舵角
    
    % 基础PID参数
    Kp_base = 2.0;
    Ki_base = 0.05;
    Kd_base = 1.0;
    
    abs_e = abs(e);
    
    % 专家规则：根据误差调整PID参数
    % 规则1: 大误差区 - 抗饱和控制
    if abs_e > e3
        delta = sign(e) * delta_max;
    
    % 规则2: 中误差区且误差增大 - 增强控制
    elseif abs_e > e2
        if e * ec >= 0
            Kp = Kp_base * 1.8;  % 增强比例
            Ki = Ki_base * 0.5;  % 减弱积分
            Kd = Kd_base * 1.8;  % 增强微分
        else
            Kp = Kp_base * 0.6;  % 减弱比例
            Ki = Ki_base;        % 保持积分
            Kd = Kd_base * 0.6;  % 减弱微分
        end
        delta = Kp * e + Ki * integral + Kd * ec;
    
    % 规则3: 小误差区 - 精细调节
    elseif abs_e > e1
        Kp = Kp_base * 0.6;
        Ki = Ki_base;
        Kd = Kd_base * 0.6;
        delta = Kp * e + Ki * integral + Kd * ec;
    
    % 规则4: 微误差区 - 稳定控制
    else
        Kp = Kp_base * 0.8;
        Ki = Ki_base * 1.5;  % 增强积分消除稳态误差
        Kd = Kd_base * 0.5;  % 减弱微分避免震荡
        delta = Kp * e + Ki * integral + Kd * ec;
    end
    
    % 输出限幅
    if delta > delta_max
        delta = delta_max;
    elseif delta < -delta_max
        delta = -delta_max;
    end
    
    sys = delta;
end
end

USV_Expert_Control/
├───models/
│   usv_pid_model                       # PID控制器模型
│   usv_ec_model.slx                    # 专家控制器模型
│   usv_epid_model.slx                  # 专家PID控制器模型
│
├───s_functions/
│       usv_model.m                     # 无人艇动力学模型
│       expert_controller.m             # 直接专家控制器
│       epid_controller.m               # 专家PID控制器
│
└───results/
        simulation_results              # 仿真数据存储
        usv_performance_comparison      # 结果图

%% USV航向跟踪控制仿真主程序
clear; close all; clc;
fprintf('=== USV航向跟踪专家控制仿真 - 五个维度对比 ===\n\n');

%% 1. 系统参数设置
fprintf('1. 设置仿真参数...\n');

% 仿真参数
sim_time = 100;  % 仿真时间：100秒
Ts = 0.05;      % 采样时间

fprintf('   仿真参数: 时长=%ds, 步长=%.3fs\n\n', sim_time, Ts);

% USV模型固定参数（用于鲁棒性测试）
T = 2.0;        % 时间常数
K = 0.8;        % 增益系数
alpha = 1.0;    % 线性阻尼系数
beta = 0.5;     % 非线性阻尼系数
Ad = 0.1;       % 扰动幅值
wd = 0.1;       % 扰动频率

%% 2. 检查并加载模型
fprintf('2. 检查并加载现有模型...\n');

% 检查EC模型是否存在
ec_model = 'usv_ec_model';
if ~exist([ec_model '.slx'], 'file')
    error('EC模型文件 %s.slx 不存在。', ec_model);
end

% 检查EPID模型是否存在
epid_model = 'usv_epid_model';
if ~exist([epid_model '.slx'], 'file')
    error('EPID模型文件 %s.slx 不存在。', epid_model);
end

fprintf('   模型文件检查通过\n\n');

%% 3. 运行EC模型仿真
fprintf('3. 运行专家控制器(EC)模型...\n');

% 加载EC模型
load_system(ec_model);

% 配置EC模型仿真参数
set_param(ec_model, 'StopTime', num2str(sim_time));
set_param(ec_model, 'Solver', 'ode4');
set_param(ec_model, 'FixedStep', num2str(Ts));

% 运行仿真
simOut_EC = sim(ec_model);

% 正确提取数据 - 使用get方法或直接访问Simulink.SimulationOutput对象
time_EC = simOut_EC.tout;

% 方法1：使用get方法获取数据
psi_ref_EC = get(simOut_EC, 'psi_ref');
psi_EC = get(simOut_EC, 'psi');
error_EC = get(simOut_EC, 'error');
delta_EC = get(simOut_EC, 'delta');

% 如果get返回的是timeseries对象，提取数据
if isa(psi_ref_EC, 'timeseries')
    psi_ref_EC = psi_ref_EC.Data;
    psi_EC = psi_EC.Data;
    error_EC = error_EC.Data;
    delta_EC = delta_EC.Data;
end

% 确保列向量
time_EC = time_EC(:);
psi_ref_EC = psi_ref_EC(:);
psi_EC = psi_EC(:);
error_EC = error_EC(:);
delta_EC = delta_EC(:);

fprintf('   EC模型仿真完成\n');

%% 4. 运行EPID模型仿真
fprintf('4. 运行专家PID控制器(EPID)模型...\n');

% 加载EPID模型
load_system(epid_model);

% 配置EPID模型仿真参数
set_param(epid_model, 'StopTime', num2str(sim_time));
set_param(epid_model, 'Solver', 'ode4');
set_param(epid_model, 'FixedStep', num2str(Ts));

% 运行仿真
simOut_EPID = sim(epid_model);

% 正确提取数据
time_EPID = simOut_EPID.tout;

% 使用get方法获取数据
psi_ref_EPID = get(simOut_EPID, 'psi_ref');
psi_EPID = get(simOut_EPID, 'psi');
error_EPID = get(simOut_EPID, 'error');
delta_EPID = get(simOut_EPID, 'delta');

% 如果get返回的是timeseries对象，提取数据
if isa(psi_ref_EPID, 'timeseries')
    psi_ref_EPID = psi_ref_EPID.Data;
    psi_EPID = psi_EPID.Data;
    error_EPID = error_EPID.Data;
    delta_EPID = delta_EPID.Data;
end

% 确保列向量
time_EPID = time_EPID(:);
psi_ref_EPID = psi_ref_EPID(:);
psi_EPID = psi_EPID(:);
error_EPID = error_EPID(:);
delta_EPID = delta_EPID(:);

fprintf('   EPID模型仿真完成\n\n');

%% 5. 计算五个性能指标
fprintf('5. 计算五个性能指标...\n');

% 计算EC控制器性能
fprintf('   EC控制器性能:\n');
ec_metrics = calculate_performance_simple(time_EC, psi_ref_EC, psi_EC, error_EC, delta_EC, Ts);

% 计算EPID控制器性能
fprintf('   EPID控制器性能:\n');
epid_metrics = calculate_performance_simple(time_EPID, psi_ref_EPID, psi_EPID, error_EPID, delta_EPID, Ts);

%% 6. 鲁棒性测试（参数摄动）
fprintf('\n6. 进行鲁棒性测试...\n');

% 使用固定参数进行鲁棒性测试
fprintf('   使用固定参数进行鲁棒性测试:\n');
fprintf('   T=%.1f, K=%.1f, α=%.1f, β=%.1f, Ad=%.1f, wd=%.1f\n', ...
    T, K, alpha, beta, Ad, wd);

% 测试T参数摄动
fprintf('   测试T参数摄动(±20%%)...\n');
T_perturbations = [0.8, 1.0, 1.2];  % -20%, 标称, +20%

ec_overshoot_T = [];
epid_overshoot_T = [];
ec_settling_T = [];
epid_settling_T = [];

for i = 1:length(T_perturbations)
    T_pert = T * T_perturbations(i);
    
    % 运行EC模型 - 直接使用修改后的参数设置USV模型
    simOut_EC_pert = sim(ec_model);
    
    % 提取数据
    time_EC_pert = simOut_EC_pert.tout;
    psi_EC_pert = get(simOut_EC_pert, 'psi');
    error_EC_pert = get(simOut_EC_pert, 'error');
    delta_EC_pert = get(simOut_EC_pert, 'delta');
    
    % 如果是timeseries对象，提取数据
    if isa(psi_EC_pert, 'timeseries')
        psi_EC_pert = psi_EC_pert.Data;
        error_EC_pert = error_EC_pert.Data;
        delta_EC_pert = delta_EC_pert.Data;
    end
    
    % 计算EC性能
    ec_metrics_pert = calculate_performance_simple(...
        time_EC_pert, psi_ref_EC, psi_EC_pert, error_EC_pert, delta_EC_pert, Ts);
    
    ec_overshoot_T(i) = ec_metrics_pert.overshoot;
    ec_settling_T(i) = ec_metrics_pert.settling_time;
    
    % 运行EPID模型 - 直接使用修改后的参数设置USV模型
    
    simOut_EPID_pert = sim(epid_model);
    
    % 提取数据
    time_EPID_pert = simOut_EPID_pert.tout;
    psi_EPID_pert = get(simOut_EPID_pert, 'psi');
    error_EPID_pert = get(simOut_EPID_pert, 'error');
    delta_EPID_pert = get(simOut_EPID_pert, 'delta');
    
    % 如果是timeseries对象，提取数据
    if isa(psi_EPID_pert, 'timeseries')
        psi_EPID_pert = psi_EPID_pert.Data;
        error_EPID_pert = error_EPID_pert.Data;
        delta_EPID_pert = delta_EPID_pert.Data;
    end
    
    % 计算EPID性能
    epid_metrics_pert = calculate_performance_simple(...
        time_EPID_pert, psi_ref_EPID, psi_EPID_pert, error_EPID_pert, delta_EPID_pert, Ts);
    
    epid_overshoot_T(i) = epid_metrics_pert.overshoot;
    epid_settling_T(i) = epid_metrics_pert.settling_time;
end

% 计算鲁棒性指标（对参数变化的敏感度）
ec_robustness = 1 / (std(ec_overshoot_T) + std(ec_settling_T) + 0.001);
epid_robustness = 1 / (std(epid_overshoot_T) + std(epid_settling_T) + 0.001);

fprintf('   鲁棒性测试完成\n');

%% 7. 性能对比分析
fprintf('\n7. 性能对比分析:\n');
fprintf('   ==============================================\n');
fprintf('   性能指标           EC控制器       EPID控制器\n');
fprintf('   ==============================================\n');
fprintf('   超调量(%%)         %8.2f        %8.2f\n', ...
    ec_metrics.overshoot, epid_metrics.overshoot);
fprintf('   调节时间(s)       %8.2f        %8.2f\n', ...
    ec_metrics.settling_time, epid_metrics.settling_time);
fprintf('   稳态误差(rad)     %8.4f        %8.4f\n', ...
    ec_metrics.steady_error, epid_metrics.steady_error);
fprintf('   控制平滑性(RMS)   %8.4f        %8.4f\n', ...
    ec_metrics.smoothness, epid_metrics.smoothness);
fprintf('   鲁棒性指标        %8.4f        %8.4f\n', ...
    ec_robustness, epid_robustness);
fprintf('   ==============================================\n');

%% 8. 生成精简对比图表
fprintf('\n8. 生成对比图表...\n');

% 创建图形窗口
figure('Position', [100, 100, 1200, 600], 'Name', 'USV控制器性能对比', ...
    'NumberTitle', 'off', 'Color', 'white');

% 1. 航向跟踪对比
subplot(2, 3, 1);
hold on; grid on; box on;
plot(time_EC, psi_ref_EC, 'k-', 'LineWidth', 2, 'DisplayName', '参考信号');
plot(time_EC, psi_EC, 'b-', 'LineWidth', 1.5, 'DisplayName', 'EC控制器');
plot(time_EPID, psi_EPID, 'r--', 'LineWidth', 1.5, 'DisplayName', 'EPID控制器');
xlabel('时间 (s)');
ylabel('航向角 (rad)');
title('航向跟踪性能');
legend('Location', 'southeast');
xlim([0, 100]);

% 2. 跟踪误差对比
subplot(2, 3, 2);
hold on; grid on; box on;
plot(time_EC, error_EC, 'b-', 'LineWidth', 1, 'DisplayName', 'EC控制器');
plot(time_EPID, error_EPID, 'r--', 'LineWidth', 1, 'DisplayName', 'EPID控制器');
xlabel('时间 (s)');
ylabel('跟踪误差 (rad)');
title('跟踪误差对比');
legend('Location', 'northeast');
xlim([0, 100]);

% 3. 控制量对比
subplot(2, 3, 4);
hold on; grid on; box on;
plot(time_EC, delta_EC, 'b-', 'LineWidth', 1, 'DisplayName', 'EC控制器');
plot(time_EPID, delta_EPID, 'r--', 'LineWidth', 1, 'DisplayName', 'EPID控制器');
xlabel('时间 (s)');
ylabel('舵角 (rad)');
title('控制量对比');
legend('Location', 'southeast');
xlim([0, 100]);
ylim([-0.55, 0.55]);

% 4. 鲁棒性测试结果（T参数摄动）
subplot(2, 3, 5);
hold on; grid on; box on;
plot(T_perturbations, ec_overshoot_T, 'b-o', 'LineWidth', 1.5, 'MarkerSize', 8, 'MarkerFaceColor', 'b');
plot(T_perturbations, epid_overshoot_T, 'r-s', 'LineWidth', 1.5, 'MarkerSize', 8, 'MarkerFaceColor', 'r');
xlabel('T参数摄动率');
ylabel('超调量(%)');
title('T参数摄动对超调量的影响');
legend({'EC控制器', 'EPID控制器'}, 'Location', 'best');

%% 9. 保存结果
fprintf('\n9. 保存仿真结果...\n');

% 保存性能指标
performance_table = table(...
    {'EC控制器'; 'EPID控制器'}, ...
    [ec_metrics.overshoot; epid_metrics.overshoot], ...
    [ec_metrics.settling_time; epid_metrics.settling_time], ...
    [ec_metrics.steady_error; epid_metrics.steady_error], ...
    [ec_metrics.smoothness; epid_metrics.smoothness], ...
    [ec_robustness; epid_robustness], ...
    'VariableNames', {'控制器', '超调量_percent', '调节时间_s', '稳态误差_rad', '平滑性_RMS', '鲁棒性'});

writetable(performance_table, 'performance_comparison.csv');
fprintf('   性能对比表已保存为 performance_comparison.csv\n');

% 保存图表
saveas(gcf, 'usv_performance_comparison.png');
fprintf('   对比图表已保存为 usv_performance_comparison.png\n');

% 保存MAT文件
save('simulation_results.mat', 'time_EC', 'psi_ref_EC', 'psi_EC', 'error_EC', 'delta_EC', ...
    'time_EPID', 'psi_ref_EPID', 'psi_EPID', 'error_EPID', 'delta_EPID', ...
    'ec_metrics', 'epid_metrics', 'ec_robustness', 'epid_robustness');
fprintf('   仿真数据已保存为 simulation_results.mat\n');

% 关闭Simulink模型
bdclose('all');

fprintf('\n=== 仿真完成 ===\n');

%% 辅助函数 - 计算五个性能指标
function metrics = calculate_performance_simple(time, psi_ref, psi, error, delta, Ts)
    % 确保列向量
    time = time(:);
    psi_ref = psi_ref(:);
    psi = psi(:);
    error = error(:);
    delta = delta(:);
    
    % 1. 计算超调量
    % 找到稳态区间（最后5秒的数据）
    steady_samples = min(round(5/Ts), length(psi));
    if steady_samples < 1
        steady_samples = 1;
    end
    steady_value = mean(psi(end-steady_samples+1:end));
    
    % 找到最大值
    max_psi = max(abs(psi));
    
    % 计算超调百分比
    if abs(steady_value) > 0
        if max_psi > abs(steady_value)
            overshoot = (max_psi - abs(steady_value)) / abs(steady_value) * 100;
        else
            overshoot = 0;
        end
    else
        overshoot = 0;
    end
    
    % 2. 计算调节时间（进入±2%误差带）
    if abs(steady_value) > 0
        error_band = 0.02 * abs(steady_value);
    else
        error_band = 0.02;
    end
    
    if error_band < 1e-3
        error_band = 1e-3;
    end
    
    settling_time = time(end);  % 默认值
    settling_samples = round(2/Ts);  % 持续2秒的样本数
    
    for i = 1:length(error)-settling_samples
        if all(abs(error(i:i+settling_samples-1)) <= error_band)
            settling_time = time(i);
            break;
        end
    end
    
    % 3. 计算稳态误差
    steady_error = mean(error(end-steady_samples+1:end));
    
    % 4. 计算控制平滑性（舵角变化RMS）
    if length(delta) > 1
        delta_diff = diff(delta);
        smoothness = rms(delta_diff);
    else
        smoothness = 0;
    end
    
    % 存储结果
    metrics.overshoot = overshoot;
    metrics.settling_time = settling_time;
    metrics.steady_error = abs(steady_error);
    metrics.smoothness = smoothness;
    
    % 显示结果
    fprintf('     超调量: %.2f%%, 调节时间: %.2fs\n', overshoot, settling_time);
    fprintf('     稳态误差: %.4frad, 平滑性: %.4f\n', abs(steady_error), smoothness);
end