船舶机舱巡检机器人运动仿真：基于哈工程《工业过程控制》的摇摆环境自适应算法

“船舶在风浪中横摇±15°，机舱内设备振动剧烈，巡检机器人若按陆地算法行走，轻则定位漂移，重则侧翻坠舱。”

—— 基于哈尔滨工程大学《工业过程控制》第七章“自适应控制”与第九章“非线性系统控制”

一、实际应用场景描述

在远洋船舶、科考船、航母、潜艇等水面/水下航行器中，机舱是动力核心区域，也是巡检机器人替代人工的高价值场景：

┌──────────────────────────────────────────────────────┐

│ 船舶机舱巡检机器人仿真系统架构 │

│ │

│ [船舶运动环境模拟] │

│ • 横摇（Roll）: ±15°~±30°（风浪等级3~6级） │

│ • 纵摇（Pitch）: ±5°~±10° │

│ • 垂荡（Heave）: ±0.5m~±2m │

│ • 周期: 4s~12s（海况相关） │

│ • 随机扰动: 海浪谱（PM谱/JONSWAP谱） │

│ │ │

│ ▼ │

│ ┌──────────────────────────────────────────┐ │

│ │ 运动学/动力学解算模块 │ │

│ │ • 船舶-机器人坐标变换（旋转矩阵） │ │

│ │ • 重力分量实时计算 │ │

│ │ • 摩擦力动态修正 │ │

│ │ • 惯性力补偿（科氏力、离心力） │ │

│ └──────────────┬───────────────────────────┘ │

│ │ │

│ ▼ │

│ ┌──────────────────────────────────────────┐ │

│ │ 自适应运动控制器（核心） │ │

│ │ ┌────────────┐ ┌────────────┐ │ │

│ │ │ 姿态感知 │ │ 自适应PID │ │ │

│ │ │ • IMU数据 │ │ • 增益调度 │ │ │

│ │ │ • 倾角解算 │ │ • 抗积分饱和│ │ │

│ │ └────────────┘ └────────────┘ │ │

│ │ │ │ │ │

│ │ ▼ ▼ │ │

│ │ ┌────────────────────────────────────┐ │ │

│ │ │ 摇摆补偿算法 │ │ │

│ │ │ • 重力投影修正 │ │ │

│ │ │ • 速度前馈补偿 │ │ │

│ │ │ • 摩擦力自适应调节 │ │ │

│ │ └────────────────────────────────────┘ │ │

│ └──────────────┬───────────────────────────┘ │

│ │ │

│ ▼ │

│ ┌──────────────────────────────────────────┐ │

│ │ 机器人本体模型 │ │

│ │ • 差速驱动/全向轮底盘 │ │

│ │ • 6自由度IMU（姿态感知） │ │

│ │ • 编码器（里程计） │ │

│ │ • 避障传感器（激光/超声） │ │

│ └──────────────┬───────────────────────────┘ │

│ │ │

│ ▼ │

│ ┌──────────────────────────────────────────┐ │

│ │ 上位机监控系统 │ │

│ │ • 3D可视化（船舶+机器人） │ │

│ │ • 实时姿态曲线 │ │

│ │ • 控制参数在线调整 │ │

│ │ • 巡检路径规划与任务管理 │ │

│ └──────────────────────────────────────────┘ │

│ │

│ 典型巡检流程: │

│ 1. 船舶航行中，IMU实时采集姿态数据 │

│ 2. 解算船舶横摇/纵摇/垂荡角度 │

│ 3. 自适应控制器调整电机输出（补偿重力分量） │

│ 4. 机器人沿预定路径巡检（避障+路径跟踪） │

│ 5. 到达设备点，稳定停靠，执行检测任务 │

│ 6. 返回充电坞，自动对接充电 │

└──────────────────────────────────────────────────────┘

典型应用场景

场景 船舶类型 摇摆强度 控制难点

远洋货轮机舱 散货船/油轮 中（±10°~±15°） 长距离巡检，路径保持

科考船实验室 科考船 强（±20°~±30°） 精密仪器避碰，稳定停靠

航母机舱 航空母舰 中强（±15°~±25°） 高可靠性，抗冲击

潜艇舱室 潜艇 弱（±5°~±10°） 空间狭窄，避障复杂

海上风电运维船 运维船 极强（±25°~±35°） 短时大角度摇摆，紧急避障

二、引入痛点

2.1 现场的真实困境

场景 现场反馈 根因

“站不住” “机器人刚走到设备前，船一晃就滑出去半米” 重力分量变化，摩擦力不足

“走不直” “设定直线行走，结果走成了S形” 横摇导致左右轮受力不均

“翻跟头” “大角度横摇时，机器人直接侧翻” 重心偏移，稳定性不足

“停不稳” “到达目标点后，船晃一下就又动了” 积分饱和，抗干扰差

“能耗高” “为了对抗摇摆，电机一直满负荷运行” 控制策略未考虑重力补偿

“易损坏” “频繁急加速急停，减速机半年就坏了” 惯性冲击大，机械寿命短

2.2 核心矛盾

船舶机舱巡检机器人的核心矛盾是“动态海洋环境”与“稳定运动控制”之间的不匹配。

陆地机器人的运动控制假设地面静止，而船舶环境存在周期性摇摆和随机扰动，传统控制算法在摇摆环境下性能急剧下降。

2.3 三种解决思路对比

思路 核心思想 优势 劣势

被动适应 降低速度，增大摩擦 简单可靠，成本低 效率低，适应性差

主动补偿 基于IMU实时调整 响应快，适应性强 依赖传感器精度

自适应控制 参数在线调整 鲁棒性强，适应复杂海况 算法复杂，调参难

三、核心逻辑讲解

3.1 理论基础：自适应控制与船舶运动模型

本工具基于哈工程《工业过程控制》第七章“自适应控制”与第九章“非线性系统控制”：

① 船舶摇摆运动模型

船舶在规则波中的横摇运动可简化为二阶线性系统：

\ddot{\phi} + 2\zeta\omega_n\dot{\phi} + \omega_n^2\phi = K_\omega\omega^2\theta_0\cos(\omega t)

其中：

- \phi ：横摇角（Roll）

- \zeta ：阻尼比（通常0.05~0.1）

- \omega_n ：固有频率（与船型相关）

- \omega ：波浪频率

- \theta_0 ：波浪幅值

- K_\omega ：频率响应函数

离散化实现（后向差分）：

# 横摇角更新

phi[k] = (2*phi[k-1] - phi[k-2] +

(dt**2)*(Kw*omega**2*theta0*cos(omega*t[k]) -

2*zeta*omega_n*(phi[k-1]-phi[k-2])/dt -

omega_n**2*phi[k-1])) / (1 + 2*zeta*omega_n*dt + omega_n**2*dt**2)

② 重力分量补偿原理

在摇摆坐标系下，重力矢量分解为：

\vec{g}_{body} = R_{world}^{body} \cdot \vec{g}_{world}

其中旋转矩阵 R_{world}^{body} 由横摇角 \phi 、纵摇角 \theta 、艏摇角 \psi 组成：

R = R_z(\psi)R_y(\theta)R_x(\phi)

重力在机器人坐标系下的分量为：

g_x = g \sin\theta \quad (\text{前后方向})

g_y = -g \sin\phi \cos\theta \quad (\text{左右方向})

g_z = -g \cos\phi \cos\theta \quad (\text{垂直方向})

③ 自适应PID控制

基于MIT规则（模型参考自适应控制）：

\dot{K}_p = -\gamma_p e \frac{\partial e}{\partial u} u

\dot{K}_i = -\gamma_i e \frac{\partial e}{\partial u} \int e dt

\dot{K}_d = -\gamma_d e \frac{\partial e}{\partial u} \frac{de}{dt}

其中 \gamma_p, \gamma_i, \gamma_d 为自适应增益， \frac{\partial e}{\partial u} 为灵敏度导数。

工程简化形式（增益调度）：

K_p = K_{p0} + K_{p\phi}|\phi| + K_{p\theta}|\theta|

K_i = K_{i0} + K_{i\phi}|\phi| + K_{i\theta}|\theta|

K_d = K_{d0} + K_{d\phi}|\phi| + K_{d\theta}|\theta|

3.2 摇摆环境下的运动学修正

摇摆环境下差速机器人运动学修正:

1. 世界坐标系（船舶固定）:

• X轴: 船首方向

• Y轴: 右舷方向

• Z轴: 垂直向上

2. 机器人坐标系（随船摇摆）:

• x轴: 机器人前进方向

• y轴: 机器人右侧方向

• z轴: 机器人垂直向上

3. 坐标变换（旋转矩阵）:

R = Rz(ψ)·Ry(θ)·Rx(φ)

其中:

Rx(φ) = [[1, 0, 0],

[0, cφ, -sφ],

[0, sφ, cφ]] # 横摇

Ry(θ) = [[cθ, 0, sθ],

[0, 1, 0],

[-sθ, 0, cθ]] # 纵摇

Rz(ψ) = [[cψ, -sψ, 0],

[sψ, cψ, 0],

[0, 0, 1]] # 艏摇

4. 重力分量计算:

g_world = [0, 0, -9.81] # 世界坐标系重力

g_body = R^T · g_world # 机器人坐标系重力

g_x = g_body[0] # 前后方向重力分量

g_y = g_body[1] # 左右方向重力分量

g_z = g_body[2] # 垂直方向重力分量

5. 摩擦力动态修正:

• 法向力: F_n = m·|g_z| + 惯性力补偿

• 摩擦力: F_f = μ·F_n

• 有效驱动力: F_drive = min(F_motor, F_f)

6. 速度前馈补偿:

• 横摇速度: ω_roll = dφ/dt

• 补偿力矩: τ_comp = J·ω_roll² + b·ω_roll

• 其中J为转动惯量，b为阻尼系数

7. 控制量合成:

u_left = Kp·e_left + Ki·∫e_left + Kd·de_left/dt + u_ff_left

u_right = Kp·e_right + Ki·∫e_right + Kd·de_right/dt + u_ff_right

其中前馈项:

u_ff_left = Kv·v_ref - Kg·g_y + Ka·a_ref

u_ff_right = Kv·v_ref + Kg·g_y + Ka·a_ref

Kg为重力补偿增益，与机器人质量和重力分量相关

3.3 自适应控制算法流程

自适应摇摆补偿控制流程:

输入: 目标速度v_ref, 目标角速度ω_ref, IMU数据(φ,θ,ψ,ω_x,ω_y,ω_z)

Step 1: 船舶姿态解算

φ, θ, ψ = IMU.get_roll_pitch_yaw()

ω_roll, ω_pitch, ω_yaw = IMU.get_angular_velocity()

Step 2: 重力分量计算

g_body = R(φ,θ,ψ)^T · [0,0,-g]

g_x = g_body[0] # 前后分量

g_y = g_body[1] # 左右分量（关键！）

Step 3: 自适应增益计算（基于MIT规则简化）

Kp = Kp0 + Kp_φ·|φ| + Kp_θ·|θ|

Ki = Ki0 + Ki_φ·|φ| + Ki_θ·|θ|

Kd = Kd0 + Kd_φ·|φ| + Kd_θ·|θ|

Step 4: 速度前馈补偿

v_ff = Kv · v_ref # 速度前馈

a_ff = Ka · a_ref # 加速度前馈

g_ff_left = -Kg · g_y # 重力补偿（左轮）

g_ff_right = Kg · g_y # 重力补偿（右轮）

Step 5: PID反馈控制

e_left = v_ref - v_left

e_right = v_ref - v_right

u_fb_left = Kp·e_left + Ki·∫e_left + Kd·de_left/dt

u_fb_right = Kp·e_right + Ki·∫e_right + Kd·de_right/dt

Step 6: 控制量合成

u_left = u_fb_left + v_ff + a_ff + g_ff_left

u_right = u_fb_right + v_ff + a_ff + g_ff_right

Step 7: 输出限幅与抗饱和

u_left = sat(u_left, u_min, u_max)

u_right = sat(u_right, u_min, u_max)

Step 8: 摩擦力约束检查

F_available = μ · m · |g_z|

if |u_left| > F_available: u_left = sign(u_left)·F_available

if |u_right| > F_available: u_right = sign(u_right)·F_available

Step 9: 输出至电机

Motor_left.set_speed(u_left)

Motor_right.set_speed(u_right)

Step 10: 稳定性监测

if |φ| > φ_max or |θ| > θ_max:

触发紧急停止/稳定模式

关键参数:

• Kp0, Ki0, Kd0: 基础PID增益

• Kp_φ, Kp_θ: 横摇/纵摇对比例增益的影响系数

• Kv: 速度前馈增益（≈1/K_motor）

• Ka: 加速度前馈增益（≈T_motor/K_motor）

• Kg: 重力补偿增益（≈m·r_wheel，质量×轮半径）

• μ: 摩擦系数（橡胶轮在钢甲板上约0.6~0.8）

四、代码讲解（面向对象设计）

4.1 类结构总览

类名 职责 设计模式

"ShipMotionConfig" 船舶运动配置（dataclass） 值对象

"RobotConfig" 机器人配置（dataclass） 值对象

"ControlMode" 控制模式枚举 枚举类型

"MotionState" 运动状态（dataclass） 值对象

"ShipMotionSimulator" 船舶摇摆模拟器 单一职责

"IMUSensor" IMU传感器模拟 单一职责

"AdaptivePIDController" 自适应PID控制器 策略模式

"DifferentialDriveRobot" 差速机器人模型 单一职责

"SwingCompensationController" 摇摆补偿控制器（聚合根） 聚合根

"MarineInspectionSimulator" 机舱巡检仿真器 外观模式

4.2 核心代码（精简版，CSDN友好）

完整源码约 350 行，单文件，包含 9 个核心类 + 完整仿真逻辑。

以下为关键类摘录，完整代码见附件。

"""

船舶机舱巡检机器人运动仿真：摇摆环境自适应控制

参考哈尔滨工程大学《工业过程控制》第七章"自适应控制"

"""

from dataclasses import dataclass, field

from typing import Tuple, Optional, List, Dict, Any

from enum import Enum, auto

import numpy as np

import matplotlib.pyplot as plt

from collections import deque

import logging

import math

import time

# ============================================================

# 1. 基础数据结构（枚举与值对象）

# ============================================================

class ControlMode(Enum):

"""控制模式"""

FIXED_PID = "固定PID（陆地模式）"

ADAPTIVE_PID = "自适应PID（摇摆补偿）"

EMERGENCY_STOP = "紧急停止"

@dataclass

class ShipMotionConfig:

"""船舶运动配置（基于哈工程船舶原理）"""

# 横摇参数（规则波）

roll_amplitude: float = 15.0 # 横摇幅值（度）

roll_period: float = 8.0 # 横摇周期（秒）

roll_damping: float = 0.08 # 横摇阻尼比

roll_natural_freq: float = 0.8 # 横摇固有频率（rad/s）

# 纵摇参数

pitch_amplitude: float = 5.0 # 纵摇幅值（度）

pitch_period: float = 10.0 # 纵摇周期（秒）

# 垂荡参数

heave_amplitude: float = 0.5 # 垂荡幅值（米）

heave_period: float = 6.0 # 垂荡周期（秒）

# 随机扰动（海浪谱）

wave_spectrum_type: str = "PM" # Pierson-Moskowitz谱

significant_wave_height: float = 2.0 # 有义波高（米）

# 仿真参数

sample_time: float = 0.01 # 采样时间（秒）

gravity: float = 9.81 # 重力加速度（m/s²）

@dataclass

class RobotConfig:

"""巡检机器人配置"""

# 物理参数

mass: float = 25.0 # 机器人质量（kg）

wheel_radius: float = 0.1 # 轮半径（m）

wheel_base: float = 0.4 # 轮距（m）

center_of_mass_height: float = 0.3 # 重心高度（m）

# 摩擦参数

friction_coefficient: float = 0.7 # 橡胶轮-钢甲板摩擦系数

static_friction: float = 0.8 # 静摩擦系数

kinetic_friction: float = 0.6 # 动摩擦系数

# 电机参数

max_motor_torque: float = 5.0 # 最大电机转矩（Nm）

motor_constant: float = 0.5 # 电机转矩常数（Nm/A）

motor_time_constant: float = 0.1 # 电机时间常数（s）

# 控制参数

max_velocity: float = 1.0 # 最大速度（m/s）

max_acceleration: float = 2.0 # 最大加速度（m/s²）

@dataclass

class MotionState:

"""机器人运动状态"""

timestamp: float = 0.0

x: float = 0.0 # 世界坐标系X位置（船首方向）

y: float = 0.0 # 世界坐标系Y位置（右舷方向）

yaw: float = 0.0 # 艏向角（弧度）

v_x: float = 0.0 # X方向速度

v_y: float = 0.0 # Y方向速度

omega: float = 0.0 # 角速度

roll: float = 0.0 # 横摇角（弧度）

pitch: float = 0.0 # 纵摇角（弧度）

heave: float = 0.0 # 垂荡位移（米）

left_wheel_vel: float = 0.0 # 左轮速度（rad/s）

right_wheel_vel: float = 0.0 # 右轮速度（rad/s）

left_motor_cmd: float = 0.0 # 左电机指令

right_motor_cmd: float = 0.0 # 右电机指令

def to_dict(self) -> Dict:

"""转换为字典"""

return {

'timestamp': round(self.timestamp, 3),

'x': round(self.x, 3),

'y': round(self.y, 3),

'yaw_deg': round(math.degrees(self.yaw), 1),

'roll_deg': round(math.degrees(self.roll), 1),

'pitch_deg': round(math.degrees(self.pitch), 1),

'heave': round(self.heave, 3),

'v_x': round(self.v_x, 3),

'omega': round(self.omega, 3)

}

# ============================================================

# 2. 船舶摇摆模拟器（单一职责）

# ============================================================

class ShipMotionSimulator:

"""

船舶摇摆运动模拟器

基于哈工程《船舶原理》与《工业过程控制》第七章

模拟船舶在波浪中的六自由度运动

"""

def __init__(self, config: ShipMotionConfig):

self.config = config

# 状态变量（横摇二阶系统）

self.roll = 0.0 # 横摇角（rad）

self.roll_rate = 0.0 # 横摇角速度（rad/s）

self.roll_acc = 0.0 # 横摇角加速度（rad/s²）

# 纵摇状态

self.pitch = 0.0

self.pitch_rate = 0.0

# 垂荡状态

self.heave = 0.0

self.heave_velocity = 0.0

# 时间

self.time = 0.0

# 波浪参数

self.wave_frequency = 2 * math.pi / config.roll_period

self.natural_frequency = config.roll_natural_freq

self.damping_ratio = config.roll_damping

logging.info(f"[ShipSim] 船舶摇摆模拟器初始化:")

logging.info(f" 横摇幅值: ±{config.roll_amplitude}°")

logging.info(f" 横摇周期: {config.roll_period}s")

logging.info(f" 阻尼比: {config.roll_damping}")

def update(self, dt: float) -> Tuple[float, float, float]:

"""更新船舶姿态"""

self.time += dt

# 1. 横摇运动（二阶线性系统）

# 输入：波浪扰动力矩

wave_moment = self._calculate_wave_moment()

# 二阶系统离散化（后向差分）

# J*θ'' + b*θ' + k*θ = M_wave

J = 1.0 # 等效转动惯量（归一化）

b = 2 * self.damping_ratio * self.natural_frequency * J

k = self.natural_frequency**2 * J

# 离散更新

new_roll_acc = (wave_moment - b * self.roll_rate - k * self.roll) / J

new_roll_rate = self.roll_rate + new_roll_acc * dt

new_roll = self.roll + new_roll_rate * dt

self.roll = new_roll

self.roll_rate = new_roll_rate

self.roll_acc = new_roll_acc

# 2. 纵摇运动（简化正弦）

self.pitch = math.radians(self.config.pitch_amplitude) * \

math.sin(2 * math.pi * self.time / self.config.pitch_period)

self.pitch_rate = math.radians(self.config.pitch_amplitude) * \

(2 * math.pi / self.config.pitch_period) * \

math.cos(2 * math.pi * self.time / self.config.pitch_period)

# 3. 垂荡运动（简化正弦）

self.heave = self.config.heave_amplitude * \

math.sin(2 * math.pi * self.time / self.config.heave_period)

self.heave_velocity = self.config.heave_amplitude * \

(2 * math.pi / self.config.heave_period) * \

math.cos(2 * math.pi * self.time / self.config.heave_period)

return self.roll, self.pitch, self.heave

def _calculate_wave_moment(self) -> float:

"""计算波浪扰动力矩（简化模型）"""

# 规则波扰动力矩（正弦）

wave_moment = math.radians(self.config.roll_amplitude) * \

self.natural_frequency**2 * \

math.sin(self.wave_frequency * self.time)

# 添加随机扰动（模拟不规则波）

if self.config.wave_spectrum_type == "PM":

# Pierson-Moskowitz谱简化

random_component = 0.1 * np.random.normal(0, 1)

wave_moment += random_component

return wave_moment

def get_rotation_matrix(self) -> np.ndarray:

"""获取船舶旋转矩阵（世界→船体）"""

# 横摇（绕X轴）

cr, sr = math.cos(self.roll), math.sin(self.roll)

Rx = np.array([[1, 0, 0],

[0, cr, -sr],

[0, sr, cr]])

# 纵摇（绕Y轴）

cp, sp = math.cos(self.pitch), math.sin(self.pitch)

Ry = np.array([[cp, 0, sp],

[0, 1, 0],

[-sp, 0, cp]])

# 艏摇（绕Z轴，此处假设为0）

cy, sy = 1.0, 0.0

Rz = np.array([[cy, -sy, 0],

[sy, cy, 0],

[0, 0, 1]])

# 组合旋转矩阵（ZYX顺序）

R = Rz @ Ry @ Rx

return R

def get_gravity_vector_body(self) -> np.ndarray:

"""获取机器人坐标系下的重力矢量"""

# 世界坐标系重力

g_world = np.array([0, 0, -self.config.gravity])

# 旋转到船体坐标系

R = self.get_rotation_matrix()

g_body = R.T @ g_world # 注意：R是世界→船体，所以重力变换用R^T

return g_body

# ============================================================

# 3. IMU传感器模拟（单一职责）

# ============================================================

class IMUSensor:

"""

IMU传感器模拟

提供船舶姿态、角速度、加速度测量

"""

def __init__(self, noise_level: float = 0.01):

self.noise_level = noise_level

# 传感器参数

self.accel_bias = np.array([0.0, 0.0, 0.0])

self.gyro_bias = np.array([0.0, 0.0, 0.0])

self.accel_scale = np.array([1.0, 1.0, 1.0])

self.gyro_scale = np.array([1.0, 1.0, 1.0])

logging.info(f"[IMU] IMU传感器初始化，噪声水平: {noise_level}")

def measure(self, ship_sim: ShipMotionSimulator) -> Dict[str, np.ndarray]:

"""测量IMU数据"""

# 获取真实值

roll, pitch, heave = ship_sim.roll, ship_sim.pitch, ship_sim.heave

roll_rate, pitch_rate = ship_sim.roll_rate, ship_sim.pitch_rate

# 添加噪声和偏差

roll_noisy = roll + np.random.normal(0, self.noise_level)

pitch_noisy = pitch + np.random.normal(0, self.noise_level)

roll_rate_noisy = roll_rate + np.random.normal(0, self.noise_level * 10)

pitch_rate_noisy = pitch_rate + np.random.normal(0, self.noise_level * 10)

# 重力矢量（船体坐标系）

g_body = ship_sim.get_gravity_vector_body()

# 添加线性加速度（垂荡加速度）

linear_acc = np.array([0, 0, ship_sim.heave_velocity])

accel_measurement = g_body + linear_acc

# 添加噪声

accel_measurement += np.random.normal(0, self.noise_level * 5, 3)

return {

'roll': roll_noisy,

'pitch': pitch_noisy,

'roll_rate': roll_rate_noisy,

'pitch_rate': pitch_rate_noisy,

'acceleration': accel_measurement,

'gravity_body': g_body

}

# ============================================================

# 4. 自适应PID控制器（策略模式）

# ============================================================

class AdaptivePIDController:

"""

自适应PID控制器

基于哈工程《工业过程控制》第七章"自适应控制"

根据船舶摇摆程度在线调整PID参数

"""

def __init__(self, base_kp: float = 10.0, base_ki: float = 1.0,

base_kd: float = 0.5, sample_time: float = 0.01):

self.base_kp = base_kp

self.base_ki = base_ki

self.base_kd = base_kd

self.sample_time = sample_time

# 自适应增益系数（横摇/纵摇影响）

self.kp_roll_gain = 0.5 # 横摇对Kp的影响

self.kp_pitch_gain = 0.3 # 纵摇对Kp的影响

self.ki_roll_gain = 0.2 # 横摇对Ki的影响

self.ki_pitch_gain = 0.1 # 纵摇对Ki的影响

self.kd_roll_gain = 0.3 # 横摇对Kd的影响

self.kd_pitch_gain = 0.2 # 纵摇对Kd的影响

# 状态变量

self.integral = 0.0

self.prev_error = 0.0

self.prev_output = 0.0

# 当前自适应参数

self.kp = base_kp

self.ki = base_ki

self.kd = base_kd

logging.info(f"[AdaptivePID] 初始化: Kp={base_kp}, Ki={base_ki}, Kd={base_kd}")

def adapt_gains(self, roll: float, pitch: float):

"""根据船舶姿态自适应调整PID增益"""

# 计算摇摆角度绝对值（弧度）

roll_abs = abs(roll)

pitch_abs = abs(pitch)

# 自适应调整（基于MIT规则简化）

# 摇摆越大，增益越高（增强抗干扰能力）

self.kp = self.base_kp + self.kp_roll_gain * roll_abs + self.kp_pitch_gain * pitch_abs

self.ki = self.base_ki + self.ki_roll_gain * roll_abs + self.ki_pitch_gain * pitch_abs

self.kd = self.base_kd + self.kd_roll_gain * roll_abs + self.kd_pitch_gain * pitch_abs

# 限制增益范围

self.kp = max(0.1, min(self.kp, 50.0))

self.ki = max(0.01, min(self.ki, 20.0))

self.kd = max(0.01, min(self.kd, 10.0))

def compute(self, error: float, roll: float = 0.0, pitch: float = 0.0,

current_time: float = None) -> float:

"""计算自适应PID输出"""

if current_time is None:

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！