89 lines
3.5 KiB
Python
89 lines
3.5 KiB
Python
class IncrementalPIDController:
|
|
"""
|
|
增量式 PID 控制器 (支持自动归一化缩放)
|
|
适用于发动机燃油控制等具有保持特性的执行机构
|
|
"""
|
|
def __init__(self, kp, ki, kd, dt, output_min, output_max, input_scale=1.0, output_scale=1.0):
|
|
"""
|
|
初始化 PID 控制器
|
|
|
|
如果提供了 scale 参数，则 kp/ki/kd 被视为归一化域的参数。
|
|
内部运算逻辑：
|
|
norm_error = (setpoint - measurement) / input_scale
|
|
norm_output += PID(norm_error, kp, ki, kd)
|
|
physical_output = norm_output * output_scale
|
|
|
|
:param kp: 比例系数 (建议使用归一化参数)
|
|
:param ki: 积分系数
|
|
:param kd: 微分系数
|
|
:param dt: 控制周期 (s)
|
|
:param output_min: 执行机构输出下限 (物理量)
|
|
:param output_max: 执行机构输出上限 (物理量)
|
|
:param input_scale: 输入测量值的量程基准 (例如额定功率 1000.0)
|
|
:param output_scale: 输出控制量的量程基准 (例如最大燃油 600.0)
|
|
"""
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.kd = kd
|
|
self.dt = dt
|
|
|
|
# 记录缩放因子
|
|
self.input_scale = input_scale
|
|
self.output_scale = output_scale
|
|
|
|
# 将物理限制转换为内部的归一化限制
|
|
self.out_min_norm = output_min / output_scale
|
|
self.out_max_norm = output_max / output_scale
|
|
|
|
# 历史误差状态 (归一化误差)
|
|
self.e_k1 = 0.0 # e(k-1)
|
|
self.e_k2 = 0.0 # e(k-2)
|
|
|
|
# 当前实际的控制输出 (归一化值 [0~1] 或 [-1~1])
|
|
self.current_output_norm = 0.0
|
|
|
|
def reset(self, initial_output=0.0):
|
|
"""
|
|
重置控制器状态，用于初始化或开闭环的无扰切换
|
|
:param initial_output: 当前执行机构的实际位置（物理量，如燃油流量）
|
|
"""
|
|
self.e_k1 = 0.0
|
|
self.e_k2 = 0.0
|
|
# 将物理初始值转换为归一化内部状态
|
|
self.current_output_norm = initial_output / self.output_scale
|
|
|
|
def compute(self, setpoint, measurement):
|
|
"""
|
|
计算下一拍的控制量
|
|
:param setpoint: 目标设定值 (物理量)
|
|
:param measurement: 当前测量值 (物理量)
|
|
:return: 经过限幅的绝对控制指令 (物理量)
|
|
"""
|
|
# 1. 计算归一化误差 e(k)
|
|
# 误差除以输入量程，使得误差在 -1~1 之间 (对于阶跃通常更小)
|
|
e_k = (setpoint - measurement) / self.input_scale
|
|
|
|
# 2. 计算归一化控制增量 delta_u
|
|
p_term = self.kp * (e_k - self.e_k1)
|
|
i_term = self.ki * e_k * self.dt
|
|
# 微分项除以dt可能会很大，归一化的时间常数有助于平滑
|
|
d_term = self.kd * (e_k - 2 * self.e_k1 + self.e_k2) / self.dt
|
|
|
|
delta_u_norm = p_term + i_term + d_term
|
|
|
|
# 3. 更新当前归一化输出量
|
|
self.current_output_norm += delta_u_norm
|
|
|
|
# 4. 绝对位置限幅 (在归一化域进行)
|
|
if self.current_output_norm > self.out_max_norm:
|
|
self.current_output_norm = self.out_max_norm
|
|
elif self.current_output_norm < self.out_min_norm:
|
|
self.current_output_norm = self.out_min_norm
|
|
|
|
# 5. 更新历史误差
|
|
self.e_k2 = self.e_k1
|
|
self.e_k1 = e_k
|
|
|
|
# 6. 返回物理量输出
|
|
return self.current_output_norm * self.output_scale
|