96 lines
3.7 KiB
Python
96 lines
3.7 KiB
Python
"""位置式 PID 控制器（纯算法构件）。
|
|
|
|
对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp
|
|
（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。
|
|
|
|
anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和
|
|
时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"——
|
|
后者在误差反号后仍会冻结回卷，导致饱和残留。
|
|
"""
|
|
|
|
from dataclasses import dataclass
|
|
|
|
from .rate_limiter import clamp
|
|
|
|
|
|
@dataclass
|
|
class PidParams:
|
|
"""PID 参数（全部来自配置，代码不写死任何数值）。"""
|
|
|
|
kp: float = 0.0
|
|
ki: float = 0.0
|
|
kd: float = 0.0
|
|
integral_limit: float = 0.0 # 积分项限幅（<=0 表示不限幅）
|
|
derivative_filter_coef: float = 0.0 # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波
|
|
output_limit: float = 0.0 # 输出限幅（<=0 表示不限幅）
|
|
|
|
|
|
class PidController:
|
|
def __init__(self, params: PidParams) -> None:
|
|
self._params = params
|
|
self._integral = 0.0
|
|
self._prev_error = 0.0
|
|
self._filtered_derivative = 0.0
|
|
self._last_output = 0.0
|
|
self._has_prev_error = False
|
|
|
|
def update(self, error: float, dt_s: float) -> float:
|
|
"""单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
|
|
|
|
dt_s <= 0 时保持上次输出（异常节拍保护）。
|
|
"""
|
|
if dt_s <= 0.0:
|
|
return self._last_output
|
|
|
|
# 积分：候选值先钳位（积分限幅）
|
|
integral_candidate = self._integral + error * dt_s
|
|
if self._params.integral_limit > 0.0:
|
|
integral_candidate = clamp(integral_candidate,
|
|
-self._params.integral_limit,
|
|
self._params.integral_limit)
|
|
|
|
# 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波）
|
|
derivative = 0.0
|
|
if self._has_prev_error:
|
|
derivative = (error - self._prev_error) / dt_s
|
|
coef = self._params.derivative_filter_coef
|
|
derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative
|
|
self._filtered_derivative = derivative
|
|
self._prev_error = error
|
|
self._has_prev_error = True
|
|
|
|
# 位置式合成
|
|
integral_term = self._params.ki * integral_candidate
|
|
raw = self._params.kp * error + integral_term + self._params.kd * derivative
|
|
|
|
# anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结
|
|
saturated = (self._params.output_limit > 0.0
|
|
and abs(raw) > self._params.output_limit)
|
|
increment_pushes_deeper = (error * dt_s) * raw > 0.0
|
|
if not (saturated and increment_pushes_deeper):
|
|
self._integral = integral_candidate
|
|
|
|
# 输出限幅
|
|
if self._params.output_limit > 0.0:
|
|
self._last_output = clamp(raw, -self._params.output_limit,
|
|
self._params.output_limit)
|
|
else:
|
|
self._last_output = raw
|
|
return self._last_output
|
|
|
|
def reset_integral(self) -> None:
|
|
"""清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。"""
|
|
self._integral = 0.0
|
|
|
|
def reset(self) -> None:
|
|
"""清空全部内部状态（积分、微分历史、上次输出）。"""
|
|
self._integral = 0.0
|
|
self._prev_error = 0.0
|
|
self._filtered_derivative = 0.0
|
|
self._last_output = 0.0
|
|
self._has_prev_error = False
|
|
|
|
@property
|
|
def last_output(self) -> float:
|
|
return self._last_output
|