first commit

utils/visual_track_control/pid_controller.py

@@ -0,0 +1,95 @@
+"""位置式 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