first commit

utils/visual_track_control/rate_limiter.py

@@ -0,0 +1,37 @@
+"""限幅与变化率限制（纯算法构件）。
+
+对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h
+（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。
+"""
+
+def clamp(v: float, lo: float, hi: float) -> float:
+    """静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。"""
+    return min(max(v, lo), hi)
+
+
+class SlewRateLimiter:
+    """变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。
+
+    初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后
+    首条指令从零按加速度限幅渐变。
+    """
+
+    def __init__(self, rate_limit: float, initial: float = 0.0) -> None:
+        self._rate_limit = rate_limit
+        self._output = initial
+
+    def limit(self, target: float, dt_s: float) -> float:
+        """朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。"""
+        if dt_s <= 0.0 or self._rate_limit <= 0.0:
+            return self._output
+        max_step = self._rate_limit * dt_s
+        self._output = clamp(target, self._output - max_step, self._output + max_step)
+        return self._output
+
+    def reset(self, value: float = 0.0) -> None:
+        """复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。"""
+        self._output = value
+
+    @property
+    def last_output(self) -> float:
+        return self._output