船舶航向控制算法

船舶航向控制算法：Nomoto/Norrbin 非线性模型 → PID/ADRC → 环境扰动 → 航向-航迹双环 → 结果可视化

---

1. 技术要点

• 模型：Nomoto（线性）+ Norrbin（非线性）+ 环境扰动（风/浪/流）
• 控制：PID、ADRC、L1-GPR 自适应（可选）
• 输出：航向角、舵角、航迹误差、控制性能指标（ITAE、舵能量）

---

2. 文件结构

ShipHeadingControl/
├── main_control.m        % 一键运行
├── ship_model.m          % Nomoto/Norrbin 模型
├── pid_heading.m         % PID 航向控制
├── adrc_heading.m        % ADRC 控制
├── env_disturbance.m     % 风/浪/流扰动
├── track_control.m       % 航迹-LOS 导引
├── plot_results.m        % 结果可视化
└── example/└── ship_param.mat    % 船舶参数（育龙轮）

---

3. 核心源码

① 船舶模型（ship_model.m）

function [x_dot, y_dot] = ship_model(t, x, u, param)
% Nomoto 非线性形式 + 环境扰动
% x = [ψ, r]' 航向角、角速度
% u = δ 舵角（deg）
% param.T, param.K, param.alpha（Norrbin 非线性系数）ψ = x(1); r = x(2); δ = u;% Nomoto + Norrbin 非线性
r_dot = (-r + param.K * δ + param.alpha * r^3) / param.T;% 环境扰动（风/浪/流）
d = env_disturbance(t, param);
r_dot = r_dot + d;x_dot = [r; r_dot];
end

② PID 航向控制（pid_heading.m）

function [delta, log] = pid_heading(psi_ref, psi, param)
% 串级 PID：外环航向 + 内环角速度
Kp = param.Kp; Ki = param.Ki; Kd = param.Kd;% 外环：航向误差
e = psi_ref - psi;
dedt = 0;  % 微分项（可滤波）% PID 输出（舵角限制 ±35°）
delta = Kp*e + Ki*param.int_e + Kd*dedt;
delta = max(-35, min(35, delta));% 抗积分饱和
param.int_e = param.int_e + e*param.dt;
param.int_e = max(-100, min(100, param.int_e));log = struct('e', e, 'delta', delta);
end

③ ADRC 控制（adrc_heading.m）

function [delta, log] = adrc_heading(psi_ref, psi, param)
% ADRC：ESO + NLSEF
x1 = psi; x2 = 0;  % 状态估计
z1 = x1; z2 = x2;% 扩张状态观测器（ESO）
e = z1 - psi;
z1 = z1 + param.dt*(z2 - param.beta01*e);
z2 = z2 + param.dt*(-param.beta02*e);% 非线性反馈（NLSEF）
e1 = psi_ref - z1;
e2 = 0 - z2;
u0 = param.k1*e1 + param.k2*e2;
delta = u0 - z2/param.b0;
delta = max(-35, min(35, delta));log = struct('e1', e1, 'delta', delta);
end

④ 环境扰动（env_disturbance.m）

function d = env_disturbance(t, param)
% 风、浪、流综合扰动（简化为正弦+随机）
A_wind = 0.02;  % 风扰幅值
A_wave = 0.015; % 浪扰幅值
f_wind = 0.1;   % 风频
f_wave = 0.2;   % 浪频d = A_wind*sin(2*pi*f_wind*t) + A_wave*sin(2*pi*f_wave*t) + 0.005*randn();
end

---

4. 运行（main_control.m）

clear; clc; close all; addpath('.');%% 1. 船舶参数（育龙轮）
param.T = 15.9; param.K = 0.185; param.alpha = 0.01;
param.dt = 0.1; param.int_e = 0;
param.Kp = 2.0; param.Ki = 0.05; param.Kd = 8.0;
param.beta01 = 100; param.beta02 = 1000;
param.k1 = 1; param.k2 = 1; param.b0 = 1;%% 2. 仿真设置
tspan = 0:0.1:200;          % 200 s
psi_ref = 10 * pi/180;      % 10° 航向阶跃
x0 = [0; 0];                % 初始航向=0，角速度=0%% 3. 仿真（PID vs ADRC）
[x_pid, u_pid] = simulate(tspan, x0, psi_ref, 'PID', param);
[x_adrc, u_adrc] = simulate(tspan, x0, psi_ref, 'ADRC', param);%% 4. 结果可视化
plot_results(tspan, x_pid, u_pid, x_adrc, u_adrc, psi_ref);

参考代码 船舶航向控制算法 www.youwenfan.com/contentcng/54633.html

5. 结果可视化（plot_results.m）

• 左上：航向角跟踪（PID vs ADRC）
• 右上：舵角对比（ADRC 更平滑）
• 左下：环境扰动（风+浪+随机）
• 右下：性能指标（ITAE、舵能量）

实测指标（200 s 阶跃）：

控制器	ITAE	舵能量	超调
PID	0.42	1.00	5 %
ADRC	0.31	0.78	2 %