船舶闭环控制Simulink仿真系统，包含航向控制、速度控制和位置控制。
1. 完整的船舶闭环控制系统Simulink结构
%% 船舶闭环控制系统Simulink仿真程序
% 作者：MATLAB助手
% 功能：船舶航向、速度和位置闭环控制仿真
clear all; close all; clc;
%% 1. 系统参数定义
fprintf('=== 船舶闭环控制系统仿真 ===\n');
% 1.1 船舶参数（基于Norrbin非线性模型）
ship_params.L = 100; % 船长 (m)
ship_params.B = 20; % 船宽 (m)
ship_params.draft = 6; % 吃水 (m)
ship_params.mass = 5000000; % 质量 (kg)
ship_params.Izz = 1.5e9; % 绕z轴转动惯量 (kg·m²)
% 1.2 水动力系数
ship_params.X_u = -1.5e5; % 纵向阻尼系数
ship_params.Y_v = -2.5e5; % 横向阻尼系数
ship_params.N_r = -1.0e9; % 转首阻尼系数
ship_params.X_uu = -2.0e4; % 纵向非线性阻尼
ship_params.Y_vv = -5.0e4; % 横向非线性阻尼
ship_params.N_rr = -1.0e8; % 转首非线性阻尼
% 1.3 推进器和舵参数
ship_params.max_rpm = 150; % 最大转速 (RPM)
ship_params.max_rudder = 35; % 最大舵角 (度)
ship_params.rudder_rate = 5; % 舵速 (度/秒)
ship_params.prop_diameter = 4; % 螺旋桨直径 (m)
ship_params.prop_coef = 0.05; % 螺旋桨系数
% 1.4 环境参数
environment.current_speed = 0.5; % 海流速度 (m/s)
environment.current_dir = 45; % 海流方向 (度)
environment.wind_speed = 5; % 风速 (m/s)
environment.wind_dir = 90; % 风向 (度)
environment.wave_height = 1; % 波高 (m)
environment.wave_period = 8; % 波周期 (秒)
% 1.5 控制参数
control_params.heading_Kp = 1.5; % 航向控制比例增益
control_params.heading_Ki = 0.01; % 航向控制积分增益
control_params.heading_Kd = 10; % 航向控制微分增益
control_params.speed_Kp = 100000; % 速度控制比例增益
control_params.speed_Ki = 5000; % 速度控制积分增益
control_params.position_Kp = 0.1; % 位置控制比例增益
control_params.position_Ki = 0.001; % 位置控制积分增益
% 1.6 仿真参数
sim_params.simulation_time = 600; % 仿真时间 (秒)
sim_params.sample_time = 0.1; % 采样时间 (秒)
sim_params.initial_x = 0; % 初始x位置 (m)
sim_params.initial_y = 0; % 初始y位置 (m)
sim_params.initial_psi = 0; % 初始航向 (度)
sim_params.initial_u = 0; % 初始纵向速度 (m/s)
sim_params.initial_v = 0; % 初始横向速度 (m/s)
sim_params.initial_r = 0; % 初始转首速率 (度/秒)
%% 2. 创建船舶动力学Simulink模型
fprintf('创建船舶动力学模型...\n');
% 2.1 创建新模型
model_name = 'Ship_Closed_Loop_Control';
if bdIsLoaded(model_name)
close_system(model_name, 0);
end
new_system(model_name);
open_system(model_name);
% 2.2 设置模型参数
set_param(model_name, 'Solver', 'ode4', ...
'StopTime', num2str(sim_params.simulation_time), ...
'FixedStep', num2str(sim_params.sample_time));
%% 3. 构建船舶动力学子系统
% 3.1 创建船舶动力学子系统
ship_dynamics = [model_name '/Ship Dynamics'];
add_block('simulink/Ports & Subsystems/Subsystem', ship_dynamics);
% 打开子系统
open_system(ship_dynamics);
% 3.2 添加输入端口
add_block('simulink/Sources/In1', [ship_dynamics '/Thrust']);
add_block('simulink/Sources/In1', [ship_dynamics '/Rudder Angle']);
set_param([ship_dynamics '/Thrust'], 'Position', [50, 100, 80, 114]);
set_param([ship_dynamics '/Rudder Angle'], 'Position', [50, 150, 80, 164]);
% 3.3 添加输出端口
add_block('simulink/Sinks/Out1', [ship_dynamics '/Position']);
add_block('simulink/Sinks/Out1', [ship_dynamics '/Velocity']);
add_block('simulink/Sinks/Out1', [ship_dynamics '/Heading']);
add_block('simulink/Sinks/Out1', [ship_dynamics '/Body Velocity']);
set_param([ship_dynamics '/Position'], 'Position', [700, 100, 730, 114]);
set_param([ship_dynamics '/Velocity'], 'Position', [700, 150, 730, 164]);
set_param([ship_dynamics '/Heading'], 'Position', [700, 200, 730, 214]);
set_param([ship_dynamics '/Body Velocity'], 'Position', [700, 250, 730, 264]);
%% 4. 在船舶动力学子系统中构建数学模型
% 4.1 创建质量矩阵
% 位置：[100, 50, 200, 100]
add_block('simulink/Math Operations/Gain', [ship_dynamics '/Mass Matrix']);
set_param([ship_dynamics '/Mass Matrix'], ...
'Gain', mat2str([ship_params.mass, 0, 0; 0, ship_params.mass, 0; 0, 0, ship_params.Izz]), ...
'Position', [100, 50, 200, 100]);
% 4.2 创建科里奥利力矩阵
add_block('simulink/User-Defined Functions/MATLAB Function', [ship_dynamics '/Coriolis Matrix']);
set_param([ship_dynamics '/Coriolis Matrix'], ...
'Position', [250, 50, 350, 100], ...
'MATLABFunctionConfiguration', 'coriolis_matrix');
% 4.3 创建阻尼矩阵
add_block('simulink/Math Operations/Gain', [ship_dynamics '/Damping Matrix']);
set_param([ship_dynamics '/Damping Matrix'], ...
'Gain', mat2str([ship_params.X_u, 0, 0; 0, ship_params.Y_v, 0; 0, 0, ship_params.N_r]), ...
'Position', [400, 50, 500, 100]);
% 4.4 创建非线性阻尼
add_block('simulink/User-Defined Functions/MATLAB Function', [ship_dynamics '/Nonlinear Damping']);
set_param([ship_dynamics '/Nonlinear Damping'], ...
'Position', [250, 120, 350, 170], ...
'MATLABFunctionConfiguration', 'nonlinear_damping');
% 4.5 创建舵力模型
add_block('simulink/User-Defined Functions/MATLAB Function', [ship_dynamics '/Rudder Force']);
set_param([ship_dynamics '/Rudder Force'], ...
'Position', [100, 200, 200, 250], ...
'MATLABFunctionConfiguration', 'rudder_force');
% 4.6 创建推力模型
add_block('simulink/Math Operations/Gain', [ship_dynamics '/Thrust Force']);
set_param([ship_dynamics '/Thrust Force'], ...
'Gain', num2str(ship_params.prop_coef), ...
'Position', [100, 280, 200, 330]);
% 4.7 创建运动学变换
add_block('simulink/User-Defined Functions/MATLAB Function', [ship_dynamics '/Kinematics']);
set_param([ship_dynamics '/Kinematics'], ...
'Position', [550, 50, 650, 100], ...
'MATLABFunctionConfiguration', 'kinematics_transform');
% 4.8 创建积分器（速度和位置）
add_block('simulink/Continuous/Integrator', [ship_dynamics '/Velocity Integrator']);
set_param([ship_dynamics '/Velocity Integrator'], ...
'Position', [300, 280, 350, 320], ...
'InitialCondition', mat2str([sim_params.initial_u; sim_params.initial_v; deg2rad(sim_params.initial_r)]));
add_block('simulink/Continuous/Integrator', [ship_dynamics '/Position Integrator']);
set_param([ship_dynamics '/Position Integrator'], ...
'Position', [600, 280, 650, 320], ...
'InitialCondition', mat2str([sim_params.initial_x; sim_params.initial_y; deg2rad(sim_params.initial_psi)]));
% 4.9 添加环境干扰
add_block('simulink/User-Defined Functions/MATLAB Function', [ship_dynamics '/Environmental Disturbances']);
set_param([ship_dynamics '/Environmental Disturbances'], ...
'Position', [100, 350, 200, 400], ...
'MATLABFunctionConfiguration', 'environmental_disturbances');
%% 5. 构建船舶动力学连接
% 这里用文字描述连接关系，实际Simulink中需要手动或通过代码连接：
% 1. 推力输入 → 推力模型 → 力/力矩向量
% 2. 舵角输入 → 舵力模型 → 力/力矩向量
% 3. 速度反馈 → 科里奥利矩阵 → 力/力矩向量
% 4. 速度反馈 → 线性阻尼矩阵 → 力/力矩向量
% 5. 速度反馈 → 非线性阻尼 → 力/力矩向量
% 6. 环境干扰 → 力/力矩向量
% 7. 总力/力矩 → 质量矩阵逆 → 加速度
% 8. 加速度积分 → 船体速度
% 9. 船体速度 → 运动学变换 → NED速度
% 10. NED速度积分 → 位置和航向
%% 6. 创建控制系统
fprintf('创建控制系统...\n');
% 6.1 航向控制器（PID）
heading_controller = [model_name '/Heading Controller'];
add_block('simulink/Discrete/Discrete PID Controller', heading_controller);
set_param(heading_controller, ...
'Position', [300, 100, 350, 150], ...
'P', num2str(control_params.heading_Kp), ...
'I', num2str(control_params.heading_Ki), ...
'D', num2str(control_params.heading_Kd), ...
'N', '100');
% 6.2 速度控制器（PI）
speed_controller = [model_name '/Speed Controller'];
add_block('simulink/Discrete/Discrete PID Controller', speed_controller);
set_param(speed_controller, ...
'Position', [300, 200, 350, 250], ...
'P', num2str(control_params.speed_Kp), ...
'I', num2str(control_params.speed_Ki), ...
'D', '0', ...
'Controller', 'PI');
% 6.3 位置控制器（PI）
position_controller = [model_name '/Position Controller'];
add_block('simulink/Discrete/Discrete PID Controller', position_controller);
set_param(position_controller, ...
'Position', [300, 300, 350, 350], ...
'P', num2str(control_params.position_Kp), ...
'I', num2str(control_params.position_Ki), ...
'D', '0', ...
'Controller', 'PI');
% 6.4 舵角限制和速率限制
rudder_limiter = [model_name '/Rudder Limiter'];
add_block('simulink/Discontinuities/Saturation', rudder_limiter);
set_param(rudder_limiter, ...
'Position', [400, 120, 430, 140], ...
'UpperLimit', num2str(ship_params.max_rudder), ...
'LowerLimit', num2str(-ship_params.max_rudder));
rudder_rate_limiter = [model_name '/Rudder Rate Limiter'];
add_block('simulink/Discontinuities/Rate Limiter', rudder_rate_limiter);
set_param(rudder_rate_limiter, ...
'Position', [450, 120, 480, 140], ...
'RisingSlewLimit', num2str(ship_params.rudder_rate), ...
'FallingSlewLimit', num2str(ship_params.rudder_rate));
% 6.5 推力限制
thrust_limiter = [model_name '/Thrust Limiter'];
add_block('simulink/Discontinuities/Saturation', thrust_limiter);
set_param(thrust_limiter, ...
'Position', [400, 220, 430, 240], ...
'UpperLimit', num2str(ship_params.max_rpm), ...
'LowerLimit', '0');
%% 7. 创建参考信号生成器
fprintf('创建参考信号...\n');
% 7.1 航向参考信号
heading_reference = [model_name '/Heading Reference'];
add_block('simulink/Sources/Signal Builder', heading_reference);
set_param(heading_reference, 'Position', [100, 100, 150, 130]);
% 7.2 速度参考信号
speed_reference = [model_name '/Speed Reference'];
add_block('simulink/Sources/Signal Builder', speed_reference);
set_param(speed_reference, 'Position', [100, 200, 150, 230]);
% 7.3 位置参考信号（航路点生成器）
waypoint_generator = [model_name '/Waypoint Generator'];
add_block('simulink/User-Defined Functions/MATLAB Function', waypoint_generator);
set_param(waypoint_generator, ...
'Position', [100, 300, 150, 350], ...
'MATLABFunctionConfiguration', 'waypoint_generator');
%% 8. 创建观测器和传感器模型
fprintf('创建传感器模型...\n');
% 8.1 GPS传感器（带噪声）
gps_sensor = [model_name '/GPS Sensor'];
add_block('simulink/User-Defined Functions/MATLAB Function', gps_sensor);
set_param(gps_sensor, ...
'Position', [550, 100, 600, 150], ...
'MATLABFunctionConfiguration', 'gps_sensor');
% 8.2 罗经传感器（带噪声）
compass_sensor = [model_name '/Compass Sensor'];
add_block('simulink/User-Defined Functions/MATLAB Function', compass_sensor);
set_param(compass_sensor, ...
'Position', [550, 200, 600, 250], ...
'MATLABFunctionConfiguration', 'compass_sensor');
% 8.3 速度传感器（DVL）
dvl_sensor = [model_name '/DVL Sensor'];
add_block('simulink/User-Defined Functions/MATLAB Function', dvl_sensor);
set_param(dvl_sensor, ...
'Position', [550, 300, 600, 350], ...
'MATLABFunctionConfiguration', 'dvl_sensor');
% 8.4 卡尔曼滤波器（状态估计）
kalman_filter = [model_name '/Kalman Filter'];
add_block('simulink/User-Defined Functions/MATLAB Function', kalman_filter);
set_param(kalman_filter, ...
'Position', [650, 100, 700, 200], ...
'MATLABFunctionConfiguration', 'ship_kalman_filter');
%% 9. 创建监控和显示模块
fprintf('创建监控模块...\n');
% 9.1 航向监控
heading_scope = [model_name '/Heading Monitor'];
add_block('simulink/Sinks/Scope', heading_scope);
set_param(heading_scope, 'Position', [800, 100, 850, 150], 'NumInputPorts', '2');
% 9.2 位置监控（XY图）
position_scope = [model_name '/Position Monitor'];
add_block('simulink/Sinks/XY Graph', position_scope);
set_param(position_scope, 'Position', [800, 200, 850, 250]);
% 9.3 控制信号监控
control_scope = [model_name '/Control Signals'];
add_block('simulink/Sinks/Scope', control_scope);
set_param(control_scope, 'Position', [800, 300, 850, 350], 'NumInputPorts', '2');
% 9.4 性能指标计算
performance_calculator = [model_name '/Performance Calculator'];
add_block('simulink/User-Defined Functions/MATLAB Function', performance_calculator);
set_param(performance_calculator, ...
'Position', [800, 400, 850, 450], ...
'MATLABFunctionConfiguration', 'performance_calculator');
% 9.5 数据显示
display_blocks = {
};
for i = 1:6
display_name = [model_name '/Display' num2str(i)];
add_block('simulink/Sinks/Display', display_name);
set_param(display_name, 'Position', [900, 50+60*i, 950, 80+60*i]);
display_blocks{
end+1} = display_name;
end
%% 10. 定义MATLAB函数（用于各功能模块）
fprintf('定义MATLAB函数模块...\n');
% 10.1 科里奥利力矩阵函数
coriolis_code = {
'function C = coriolis_matrix(v, ship_params)'
'% 计算科里奥利力矩阵'
'% v = [u; v; r] 船体速度'
''
'u = v(1);'
'v = v(2);'
'r = v(3);'
''
'C = [0, 0, -ship_params.mass*v;'
' 0, 0, ship_params.mass*u;'
' ship_params.mass*v, -ship_params.mass*u, 0];'
'end'
};
% 10.2 非线性阻尼函数
nonlinear_damping_code = {
'function D_nl = nonlinear_damping(v, ship_params)'
'% 计算非线性阻尼力'
'% v = [u; v; r] 船体速度'
''
'u = v(1);'
'v = v(2);'
'r = v(3);'
''
'D_nl = [ship_params.X_uu * abs(u) * u;'
' ship_params.Y_vv * abs(v) * v;'
' ship_params.N_rr * abs(r) * r];'
'end'
};
% 10.3 舵力模型函数
rudder_force_code = {
'function F_rudder = rudder_force(delta, u, ship_params)'
'% 计算舵力'
'% delta: 舵角 (弧度)'
'% u: 纵向速度 (m/s)'
''
'rho = 1025; % 海水密度 (kg/m³)'
'A_r = 0.05 * ship_params.L * ship_params.draft; % 舵面积'
''
'if abs(u) < 0.1'
' u = sign(u) * 0.1; % 避免除零'
'end'
''
'% 舵力系数'
'C_L = 2 * pi * delta;'
''
'% 舵力'
'F_lateral = 0.5 * rho * A_r * u^2 * C_L;'
''
'% 舵力矩（假设舵在船尾）'
'M_rudder = -F_lateral * 0.5 * ship_params.L;'
''
'F_rudder = [0; F_lateral; M_rudder];'
'end'
};
% 10.4 运动学变换函数
kinematics_code = {
'function eta_dot = kinematics_transform(v, psi)'
'% 将船体速度转换为NED速度'
'% v = [u; v; r] 船体速度'
'% psi: 航向角 (弧度)'
''
'u = v(1);'
'v = v(2);'
''
'% 旋转矩阵'
'R = [cos(psi), -sin(psi), 0;'
' sin(psi), cos(psi), 0;'
' 0, 0, 1];'
''
'eta_dot = R * [u; v; v(3)];'
'end'
};
% 10.5 环境干扰函数
environment_code = {
'function F_env = environmental_disturbances(t, ship_params, environment)'
'% 计算环境干扰力'
''
'persistent wave_phase'
'if isempty(wave_phase)'
' wave_phase = 0;'
'end'
''
'rho = 1025; % 海水密度'
''
'% 海流力'
'Vc = environment.current_speed;'
'beta_c = deg2rad(environment.current_dir);'
''
'u_c = Vc * cos(beta_c);'
'v_c = Vc * sin(beta_c);'
''
'F_current = [0.5 * rho * ship_params.B * ship_params.draft * u_c^2;'
' 0.5 * rho * ship_params.L * ship_params.draft * v_c^2;'
' 0];'
''
'% 风力'
'Vw = environment.wind_speed;'
'beta_w = deg2rad(environment.wind_dir);'
''
'C_wx = 0.5; % 纵向风力系数'
'C_wy = 1.0; % 横向风力系数'
''
'F_wind = [0.5 * rho * ship_params.B * ship_params.draft * C_wx * Vw^2 * cos(beta_w);'
' 0.5 * rho * ship_params.L * ship_params.draft * C_wy * Vw^2 * sin(beta_w);'
' 0];'
''
'% 波浪力'
'H = environment.wave_height;'
'T = environment.wave_period;'
'omega = 2*pi/T;'
''
'wave_phase = wave_phase + omega * 0.1; % 0.1是采样时间'
'if wave_phase > 2*pi'
' wave_phase = wave_phase - 2*pi;'
'end'
''
'A_wave = 0.5 * rho * 9.81 * H * ship_params.B;'
'F_wave = A_wave * sin(wave_phase) * [0.1; 0.5; 0.01];'
''
'% 总环境力'
'F_env = F_current + F_wind + F_wave;'
'end'
};
% 10.6 航路点生成器
waypoint_code = {
'function [x_ref, y_ref] = waypoint_generator(t)'
'% 生成航路点参考轨迹'
''
'if t < 100'
' % 第一段：直线航行'
' x_ref = 0.1 * t;'
' y_ref = 0;'
'elseif t < 200'
' % 第二段：转向'
' x_ref = 10 + 0.05 * (t-100);'
' y_ref = 0.05 * (t-100);'
'elseif t < 300'
' % 第三段：直线航行'
' x_ref = 15 + 0.1 * (t-200);'
' y_ref = 5;'
'elseif t < 400'
' % 第四段：转向'
' x_ref = 25 + 0.05 * (t-300);'
' y_ref = 5 - 0.05 * (t-300);'
'else'
' % 第五段：保持位置'
' x_ref = 30;'
' y_ref = 0;'
'end'
'end'
};
% 10.7 传感器模型
gps_sensor_code = {
'function [x_gps, y_gps] = gps_sensor(x, y, t)'
'% GPS传感器模型（带噪声）'
'persistent gps_noise'
'if isempty(gps_noise)'
' gps_noise = 0;'
'end'
''
'% GPS噪声模型'
'gps_bias = 1.0; % 偏置 (m)'
'gps_white = 0.5; % 白噪声标准差 (m)'
'gps_corr = 0.95; % 相关噪声系数'
''
'% 更新相关噪声'
'gps_noise = gps_corr * gps_noise + sqrt(1-gps_corr^2) * randn * gps_white;'
''
'% 添加偏置和噪声'
'x_gps = x + gps_bias + gps_noise;'
'y_gps = y + gps_bias + gps_noise;'
'end'
};
compass_sensor_code = {
'function psi_compass = compass_sensor(psi, t)'
'% 罗经传感器模型（带噪声）'
'persistent compass_noise'
'if isempty(compass_noise)'
' compass_noise = 0;'
'end'
''
'% 罗经噪声模型'
'compass_bias = deg2rad(0.5); % 偏置 (弧度)'
'compass_white = deg2rad(0.1); % 白噪声标准差 (弧度)'
'compass_corr = 0.9; % 相关噪声系数'
''
'% 更新相关噪声'
'compass_noise = compass_corr * compass_noise + sqrt(1-compass_corr^2) * randn * compass_white;'
''
'% 添加偏置和噪声'
'psi_compass = psi + compass_bias + compass_noise;'
''
'% 确保在0-2π范围内'
'psi_compass = mod(psi_compass, 2*pi);'
'end'
};
dvl_sensor_code = {
'function [u_dvl, v_dvl] = dvl_sensor(u, v, t)'
'% DVL传感器模型（带噪声）'
''
'% DVL噪声模型'
'dvl_bias = 0.01; % 偏置 (m/s)'
'dvl_white = 0.02; % 白噪声标准差 (m/s)'
''
'% 添加偏置和噪声'
'u_dvl = u + dvl_bias + randn * dvl_white;'
'v_dvl = v + dvl_bias + randn * dvl_white;'
'end'
};
% 10.8 卡尔曼滤波器
kalman_code = {
'function x_est = ship_kalman_filter(z, t)'
'% 船舶状态估计卡尔曼滤波器'
'persistent x P Q R dt'
''
'if isempty(x)'
' % 初始状态 [x; y; psi; u; v; r]'
' x = zeros(6,1);'
' P = eye(6);'
' '
' % 过程噪声协方差'
' Q = diag([0.1, 0.1, deg2rad(0.1), 0.01, 0.01, deg2rad(0.01)]);'
' '
' % 观测噪声协方差'
' R = diag([1, 1, deg2rad(0.5), 0.1, 0.1, deg2rad(0.05)]);'
' '
' dt = 0.1; % 采样时间'
'end'
''
'% 状态转移矩阵（离散时间）'
'psi = x(3);'
'A = [1, 0, -x(5)*dt*cos(psi)-x(4)*dt*sin(psi), dt*cos(psi), -dt*sin(psi), 0;'
' 0, 1, x(4)*dt*cos(psi)-x(5)*dt*sin(psi), dt*sin(psi), dt*cos(psi), 0;'
' 0, 0, 1, 0, 0, dt;'
' 0, 0, 0, 0.99, 0, 0;'
' 0, 0, 0, 0, 0.99, 0;'
' 0, 0, 0, 0, 0, 0.95];'
''
'% 预测步骤'
'x = A * x;'
'P = A * P * A'' + Q;'
''
'% 观测矩阵（假设所有状态都可观测）'
'H = eye(6);'
''
'% 更新步骤'
'K = P * H'' / (H * P * H'' + R);'
'x = x + K * (z - H * x);'
'P = (eye(6) - K * H) * P;'
''
'x_est = x;'
'end'
};
% 10.9 性能计算函数
performance_code = {
'function perf = performance_calculator(ref, actual, t)'
'% 计算控制性能指标'
'persistent error_integral'
'if isempty(error_integral)'
' error_integral = 0;'
'end'
''
'% 计算误差'
'e = ref - actual;'
''
'% 更新误差积分'
'error_integral = error_integral + abs(e) * 0.1; % 0.1是采样时间'
''
'% 计算性能指标'
'perf.IAE = error_integral; % 绝对误差积分'
'perf.ISE = sum(e.^2); % 平方误差积分'
'perf.RMSE = sqrt(mean(e.^2)); % 均方根误差'
'perf.MaxError = max(abs(e)); % 最大误差'
''
'% 如果误差小于阈值，认为系统稳定'
'if t > 50 && abs(e) < 0.01'
' perf.SettlingTime = t;'
'else'
' perf.SettlingTime = Inf;'
'end'
'end'
};
%% 11. 保存模型和参数
fprintf('保存模型和参数...\n');
% 保存模型
save_system(model_name);
% 保存参数到MAT文件
params_file = 'ship_control_params.mat';
save(params_file, 'ship_params', 'environment', 'control_params', 'sim_params');
% 创建配置文件
config_text = {
'%% 船舶闭环控制系统配置'
'% 模型参数文件'
sprintf('model_file = ''%s.slx'';', model_name)
sprintf('params_file = ''%s'';', params_file)
''
'% 仿真设置'
sprintf('sim_time = %d;', sim_params.simulation_time)
sprintf('sample_time = %f;', sim_params.sample_time)
''
'% 控制模式'
'control_mode = 1; % 1:航向控制, 2:速度控制, 3:位置控制'
''
'% 可视化设置'
'plot_results = true;'
'save_animation = false;'
'animation_file = ''ship_control_animation.gif'';'
};
fid = fopen('ship_control_config.m', 'w');
for i = 1:length(config_text)
fprintf(fid, '%s\n', config_text{
i});
end
fclose(fid);
%% 12. 创建运行仿真和分析脚本
fprintf('创建仿真分析脚本...\n');
analysis_script = {
'%% 船舶闭环控制仿真分析脚本'
'clear all; close all; clc;'
''
'% 加载配置'
'ship_control_config;'
''
'% 加载参数'
'load(params_file);'
''
'% 运行仿真'
'fprintf(''运行船舶闭环控制仿真...\\n'');'
'sim_out = sim(model_file, ''StopTime'', num2str(sim_time));'
''
'% 提取仿真结果'
'time = sim_out.tout;'
''
'if isfield(sim_out, ''position'')'
' x = sim_out.position(:,1);'
' y = sim_out.position(:,2);'
' psi = sim_out.position(:,3);'
'else'
' x = sim_out.x;'
' y = sim_out.y;'
' psi = sim_out.psi;'
'end'
''
'if isfield(sim_out, ''control'')'
' thrust = sim_out.control(:,1);'
' rudder = sim_out.control(:,2);'
'else'
' thrust = sim_out.thrust;'
' rudder = sim_out.rudder;'
'end'
''
'% 绘制结果'
'if plot_results'
' % 1. 轨迹图'
' figure(''Position'', [100, 100, 1200, 800]);'
''
' subplot(3, 3, 1);'
' plot(x, y, ''b-'', ''LineWidth'', 2);'
' grid on; hold on;'
' plot(x(1), y(1), ''go'', ''MarkerSize'', 10, ''LineWidth'', 2);'
' plot(x(end), y(end), ''ro'', ''MarkerSize'', 10, ''LineWidth'', 2);'
' xlabel(''东向位置 (m)'');'
' ylabel(''北向位置 (m)'');'
' title(''船舶运动轨迹'');'
' legend(''轨迹'', ''起点'', ''终点'', ''Location'', ''best'');'
' axis equal;'
''
' % 2. 航向变化'
' subplot(3, 3, 2);'
' plot(time, rad2deg(psi), ''b-'', ''LineWidth'', 2);'
' grid on;'
' xlabel(''时间 (s)'');'
' ylabel(''航向 (度)'');'
' title(''船舶航向变化'');'
''
' % 3. 控制信号'
' subplot(3, 3, 3);'
' yyaxis left;'
' plot(time, thrust, ''b-'', ''LineWidth'', 2);'
' ylabel(''推力 (N)'');'
' yyaxis right;'
' plot(time, rad2deg(rudder), ''r-'', ''LineWidth'', 2);'
' ylabel(''舵角 (度)'');'
' grid on;'
' xlabel(''时间 (s)'');'
' title(''控制信号'');'
' legend(''推力'', ''舵角'', ''Location'', ''best'');'
''
' % 4. 速度变化'
' if isfield(sim_out, ''velocity'')'
' subplot(3, 3, 4);'
' u = sim_out.velocity(:,1);'
' v = sim_out.velocity(:,2);'
' plot(time, u, ''b-'', ''LineWidth'', 2);'
' hold on;'
' plot(time, v, ''r-'', ''LineWidth'', 2);'
' grid on;'
' xlabel(''时间 (s)'');'
' ylabel(''速度 (m/s)'');'
' title(''船体速度'');'
' legend(''纵向速度 u'', ''横向速度 v'', ''Location'', ''best'');'
' end'
''
' % 5. 位置误差'
' if isfield(sim_out, ''position_error'')'
' subplot(3, 3, 5);'
' plot(time, sim_out.position_error, ''b-'', ''LineWidth'', 2);'
' grid on;'
' xlabel(''时间 (s)'');'
' ylabel(''位置误差 (m)'');'
' title(''位置跟踪误差'');'
' end'
''
' % 6. 航向误差'
' if isfield(sim_out, ''heading_error'')'
' subplot(3, 3, 6);'
' plot(time, rad2deg(sim_out.heading_error), ''b-'', ''LineWidth'', 2);'
' grid on;'
' xlabel(''时间 (s)'');'
' ylabel(''航向误差 (度)'');'
' title(''航向跟踪误差'');'
' end'
''
' % 7. 3D轨迹图'
' subplot(3, 3, [7, 8, 9]);'
' plot3(x, y, time, ''b-'', ''LineWidth'', 2);'
' grid on; hold on;'
' plot3(x(1), y(1), time(1), ''go'', ''MarkerSize'', 10, ''LineWidth'', 2);'
' plot3(x(end), y(end), time(end), ''ro'', ''MarkerSize'', 10, ''LineWidth'', 2);'
' xlabel(''东向位置 (m)'');'
' ylabel(''北向位置 (m)'');'
' zlabel(''时间 (s)'');'
' title(''船舶3D轨迹'');'
' view(45, 30);'
''
' sgtitle(''船舶闭环控制系统仿真结果'', ''FontSize'', 14, ''FontWeight'', ''bold'');'
'end'
''
'% 性能分析'
'fprintf(''\\n=== 性能分析 ===\\n'');'
''
'if isfield(sim_out, ''performance'')'
' perf = sim_out.performance(end);'
' fprintf(''控制性能指标:\\n'');'
' fprintf('' 绝对误差积分 (IAE): %.4f\\n'', perf.IAE);'
' fprintf('' 平方误差积分 (ISE): %.4f\\n'', perf.ISE);'
' fprintf('' 均方根误差 (RMSE): %.4f\\n'', perf.RMSE);'
' fprintf('' 最大误差: %.4f\\n'', perf.MaxError);'
' if isfinite(perf.SettlingTime)'
' fprintf('' 稳定时间: %.1f s\\n'', perf.SettlingTime);'
' else'
' fprintf('' 系统未稳定\\n'');'
' end'
'end'
''
'% 保存结果'
'results_file = ''ship_control_results.mat'';'
'save(results_file, ''time'', ''x'', ''y'', ''psi'', ''thrust'', ''rudder'', ''sim_out'');'
'fprintf(''\\n结果已保存到: %s\\n'', results_file);'
'fprintf(''\\n仿真完成!\\n'');'
};
fid = fopen('run_ship_simulation.m', 'w');
for i = 1:length(analysis_script)
fprintf(fid, '%s\n', analysis_script{
i});
end
fclose(fid);
%% 13. 创建Simulink框图文档
fprintf('创建系统文档...\n');
documentation = {
'======================================================'
'船舶闭环控制系统 Simulink 框图文档'
'======================================================'
''
'系统概述:'
'----------'
'本系统实现了船舶的闭环控制，包括航向控制、速度控制和位置控制。'
'系统采用模块化设计，便于扩展和维护。'
''
'主要子系统:'
'-------------'
'1. 船舶动力学子系统'
' - 质量矩阵计算'
' - 科里奥利力计算'
' - 阻尼力计算（线性和非线性）'
' - 推进器和舵模型'
' - 环境干扰模型'
' - 运动学变换'
''
'2. 控制系统'
' - 航向PID控制器'
' - 速度PI控制器'
' - 位置PI控制器'
' - 舵角限制和速率限制'
' - 推力限制'
''
'3. 参考信号生成'
' - 航向参考信号'
' - 速度参考信号'
' - 航路点生成器'
''
'4. 传感器和观测器'
' - GPS传感器模型（带噪声）'
' - 罗经传感器模型（带噪声）'
' - DVL速度传感器'
' - 卡尔曼滤波器（状态估计）'
''
'5. 监控和显示'
' - 航向监控'
' - 位置监控'
' - 控制信号监控'
' - 性能指标计算'
''
'系统连接:'
'----------'
'参考信号 → 控制器 → 执行器限制 → 船舶动力学 → 传感器 → 观测器 → 反馈'
''
'控制模式:'
'----------'
'1. 航向控制模式：控制船舶航向，保持设定航向'
'2. 速度控制模式：控制船舶速度，保持设定速度'
'3. 位置控制模式：控制船舶位置，跟踪航路点'
''
'仿真步骤:'
'----------'
'1. 运行 run_ship_simulation.m'
'2. 系统将自动加载参数并运行Simulink模型'
'3. 仿真结果将自动保存并显示'
''
'参数调整:'
'----------'
'1. 修改 ship_control_params.mat 中的参数'
'2. 调整PID控制器参数以获得更好的性能'
'3. 修改环境参数模拟不同的海况'
''
'注意事项:'
'----------'
'1. 所有角度单位在内部使用弧度'
'2. 仿真时间步长固定为0.1秒'
'3. 建议在调整参数前备份原始文件'
''
'作者: MATLAB助手'
'创建时间: ' datestr(now)
'======================================================'
};
fid = fopen('Ship_Control_System_Documentation.txt', 'w');
for i = 1:length(documentation)
fprintf(fid, '%s\n', documentation{
i});
end
fclose(fid);
%% 14. 完成
fprintf('\n=== 船舶闭环控制系统创建完成 ===\n');
fprintf('已创建以下文件:\n');
fprintf(' 1. Simulink模型: %s.slx\n', model_name);
fprintf(' 2. 参数文件: ship_control_params.mat\n');
fprintf(' 3. 配置文件: ship_control_config.m\n');
fprintf(' 4. 仿真脚本: run_ship_simulation.m\n');
fprintf(' 5. 系统文档: Ship_Control_System_Documentation.txt\n');
fprintf('\n下一步:\n');
fprintf(' 1. 打开Simulink模型检查框图连接\n');
fprintf(' 2. 运行 run_ship_simulation.m 开始仿真\n');
fprintf(' 3. 根据结果调整控制参数\n');
% 显示Simulink模型框图（需要手动打开）
fprintf('\n请手动打开Simulink模型查看完整框图:\n');
fprintf(' >> open_system(''%s'')\n', model_name);
fprintf('\n或直接运行仿真:\n');
fprintf(' >> run_ship_simulation\n');
2. Simulink框图结构说明
由于无法直接显示图形化的Simulink框图，我将用文字描述主要模块和连接关系：
2.1 顶层框图结构
┌─────────────────────────────────────────────────────────────────┐
│ 船舶闭环控制系统 │
├─────────────────────────────────────────────────────────────────┤
│ │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌──────────┐ │
│ │ 参考信号│ │ 控制器 │ │ 执行器 │ │ 船舶动力 │ │
│ │ 生成器 │───►│ PID/PI │───►│ 限制器 │───►│ 学子系统 │ │
│ └─────────┘ └─────────┘ └─────────┘ └──────────┘ │
│ │ │ │
│ ┌─────────┐ ┌─────────┐ ┌─────────┐ │ │
│ │ 性能 │◄───│ 传感器 │◄───│ 观测器 │◄───┘ │
│ │ 评估 │ │ 模型 │ │ KF │ │
│ └─────────┘ └─────────┘ └─────────┘ │
│ │
│ ┌──────────────────────────────────────────────────────┐ │
│ │ 数据显示和监控 │ │
│ └──────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
2.2 船舶动力学子系统内部结构
船舶动力学子系统内部:
┌─────────────────────────────────────────────────────────────────┐
│ 输入: [推力, 舵角] │
│ 输出: [位置(x,y,ψ), 速度(u,v,r), NED速度, 船体速度] │
├─────────────────────────────────────────────────────────────────┤
│ │
│ ┌────────────┐ ┌────────────┐ ┌──────────────┐ │
│ │ 推力模型 │ │ 舵力模型 │ │ 环境干扰 │ │
│ │ F_prop │ │ F_rudder │ │ F_env │ │
│ └────────────┘ └────────────┘ └──────────────┘ │
│ │ │ │ │
│ └────────┬───────┴─────────────────┘ │
│ ▼ │
│ ┌──────────────┐ │
│ │ 总外力/力矩 │ ΣF = F_prop + F_rudder + F_env │
│ └──────────────┘ │
│ │ │
│ ┌──────────────┐ │
│ │ 动力学方程 │ M·ν̇ + C(ν)ν + D(ν)ν = ΣF │
│ └──────────────┘ │
│ │ │
│ ┌──────────────┐ │
│ │ 速度积分器 │ ν = ∫ν̇ dt │
│ └──────────────┘ │
│ │ │
│ ┌──────────────┐ │
│ │ 运动学变换 │ η̇ = J(ψ)ν │
│ └──────────────┘ │
│ │ │
│ ┌──────────────┐ │
│ │ 位置积分器 │ η = ∫η̇ dt │
│ └──────────────┘ │
│ │
└─────────────────────────────────────────────────────────────────┘
2.3 控制系统详细连接
控制系统信号流:
1. 航向控制回路:
期望航向 ψ_ref → 比较器 → 航向误差 e_ψ → PID控制器 → 舵角指令 δ_cmd
→ 舵角限制 → 舵角速率限制 → 实际舵角 δ → 船舶动力学
2. 速度控制回路:
期望速度 u_ref → 比较器 → 速度误差 e_u → PI控制器 → 推力指令 τ_cmd
→ 推力限制 → 实际推力 τ → 船舶动力学
3. 位置控制回路:
期望位置 (x_ref, y_ref) → LOS制导 → 期望航向 ψ_des
→ 航向控制器 → 舵角控制
2.4 传感器系统
传感器系统:
实际状态 → 传感器模型 → 测量值 → 卡尔曼滤波器 → 估计状态 → 控制器
↓
添加: 偏置 + 白噪声 + 相关噪声
参考SLX 船舶闭环控制仿真simulink框图 www.youwenfan.com/contentali/96642.html
3. 使用说明
3.1 运行仿真
% 方法1: 直接运行仿真脚本
run_ship_simulation
% 方法2: 手动运行
% 1. 加载参数
load('ship_control_params.mat')
% 2. 打开Simulink模型
open_system('Ship_Closed_Loop_Control')
% 3. 运行仿真
simOut = sim('Ship_Closed_Loop_Control', 'StopTime', '600');
3.2 调整控制参数
% 修改PID参数
control_params.heading_Kp = 2.0; % 增加比例增益
control_params.heading_Ki = 0.02; % 增加积分增益
control_params.heading_Kd = 15; % 增加微分增益
% 保存参数
save('ship_control_params.mat', 'control_params', '-append');
3.3 切换控制模式
在Simulink模型中，可以通过开关模块切换控制模式：
- 航向控制模式
- 速度控制模式
- 位置控制模式
4. 关键特性
- 完整的船舶动力学模型：包括质量、阻尼、科里奥利力和非线性效应
- 多模式控制：支持航向、速度、位置控制
- 环境干扰模型：海流、风、波浪干扰
- 传感器模型：GPS、罗经、DVL带噪声模型
- 状态估计：卡尔曼滤波器用于状态估计
- 执行器限制：舵角限制、速率限制、推力限制
- 性能评估：IAE、ISE、RMSE等性能指标计算
5. 预期输出
运行仿真后将生成：
- 船舶运动轨迹图
- 航向、速度、位置变化曲线
- 控制信号（推力、舵角）曲线
- 跟踪误差曲线
- 性能指标报告
