38 lines
1.5 KiB
Python
38 lines
1.5 KiB
Python
"""限幅与变化率限制（纯算法构件）。
|
|
|
|
对应 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
|