104 lines
3.9 KiB
Python
104 lines
3.9 KiB
Python
"""pid_controller_test.py —— PidController 单元测试。
|
||
|
||
移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
|
||
覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
|
||
异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
|
||
"""
|
||
|
||
import pytest
|
||
|
||
from visual_track_control.pid_controller import PidController, PidParams
|
||
|
||
|
||
def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
|
||
return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
|
||
derivative_filter_coef=filter_coef, output_limit=output_limit)
|
||
|
||
|
||
def test_proportional_only():
|
||
pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
|
||
assert pid.update(3.0, 0.1) == 6.0
|
||
assert pid.update(-1.5, 0.1) == -3.0
|
||
|
||
|
||
def test_integral_accumulates():
|
||
pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
|
||
# 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
|
||
assert pid.update(2.0, 0.5) == 1.0
|
||
assert pid.update(2.0, 0.5) == 2.0
|
||
|
||
|
||
def test_integral_clamped_to_limit():
|
||
pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
|
||
for _ in range(5):
|
||
pid.update(10.0, 1.0)
|
||
# 积分候选值被钳位在 1.0 → 输出恒为 1.0
|
||
assert pid.last_output == 1.0
|
||
|
||
|
||
def test_output_clamped_to_limit():
|
||
pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
|
||
assert pid.update(1.0, 0.1) == 3.0
|
||
assert pid.update(-1.0, 0.1) == -3.0
|
||
|
||
|
||
def test_anti_windup_freeze_while_saturated_same_direction():
|
||
# kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
|
||
# 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
|
||
pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
|
||
for _ in range(10):
|
||
assert pid.update(10.0, 1.0) == 5.0
|
||
|
||
|
||
def test_anti_windup_unwinds_after_error_flips():
|
||
# C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
|
||
# 冻结回卷、输出一直钉在 +5）。
|
||
# kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
|
||
# error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
|
||
# 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
|
||
pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
|
||
for _ in range(3):
|
||
assert pid.update(10.0, 1.0) == 5.0
|
||
assert pid.update(-1.0, 1.0) == -2.0 # 首拍立即回卷
|
||
out = -2.0
|
||
for _ in range(11):
|
||
out = pid.update(-1.0, 1.0)
|
||
assert out == pytest.approx(-5.0, abs=1e-9) # 旧判据此处仍为 +5.0
|
||
|
||
|
||
def test_derivative_first_step_is_zero():
|
||
pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
|
||
# 首拍无微分历史：微分项为 0
|
||
assert pid.update(5.0, 1.0) == 0.0
|
||
|
||
|
||
def test_derivative_with_low_pass_filter():
|
||
pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
|
||
pid.update(0.0, 1.0) # 建立微分历史（导数 0）
|
||
# 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
|
||
assert pid.update(1.0, 1.0) == 0.5
|
||
|
||
|
||
def test_zero_or_negative_dt_holds_last_output():
|
||
pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
|
||
assert pid.update(2.0, 0.1) == 2.0
|
||
assert pid.update(99.0, 0.0) == 2.0
|
||
assert pid.update(99.0, -1.0) == 2.0
|
||
|
||
|
||
def test_reset_integral_clears_only_integral():
|
||
pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
|
||
pid.update(2.0, 1.0) # 积分 = 2
|
||
pid.reset_integral()
|
||
# 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
|
||
assert pid.update(0.0, 1.0) == -2.0
|
||
|
||
|
||
def test_reset_clears_everything():
|
||
pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
|
||
pid.update(5.0, 1.0)
|
||
pid.reset()
|
||
assert pid.last_output == 0.0
|
||
# 全清后等同全新控制器：首拍无微分历史
|
||
assert pid.update(3.0, 1.0) == 3.0 + 3.0 # kp·e + ki·∫e
|