390 lines
14 KiB
Python
390 lines
14 KiB
Python
"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。
|
||
|
||
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
|
||
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
|
||
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
|
||
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
|
||
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
|
||
限幅（S6）、机体系→NED 输出旋转（A4）。
|
||
"""
|
||
|
||
import math
|
||
|
||
import pytest
|
||
|
||
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
||
TrackingConfig)
|
||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||
DegradedReason, TrackState)
|
||
|
||
|
||
def make_config() -> TrackingConfig:
|
||
"""测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
|
||
画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
|
||
偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
|
||
"""
|
||
cfg = TrackingConfig()
|
||
cfg.control.frequency_hz = 20.0
|
||
cfg.control.visual_stale_ms = 200
|
||
cfg.control.distance_stale_ms = 500
|
||
cfg.control.attitude_stale_ms = 300
|
||
cfg.camera.image_width = 1000
|
||
cfg.camera.image_height = 1000
|
||
cfg.camera.fov_h_deg = 90.0
|
||
cfg.camera.fov_v_deg = 90.0
|
||
cfg.heading.mode = HeadingMode.RATE
|
||
cfg.heading.gain = 1.0
|
||
cfg.heading.yaw_rate_limit_dps = 30.0
|
||
cfg.heading.yaw_slew_limit_dps2 = 100000.0
|
||
cfg.vertical.vz_gain_mps_per_deg = 0.1
|
||
cfg.vertical.vz_limit_mps = 2.0
|
||
cfg.distance.d_exp_m = 10.0
|
||
cfg.distance.kp = 0.5
|
||
cfg.distance.ki = 0.0
|
||
cfg.distance.kd = 0.0
|
||
cfg.distance.integral_limit = 0.0
|
||
cfg.distance.derivative_filter_coef = 0.0
|
||
cfg.distance.approach_velocity_limit_mps = 3.0
|
||
cfg.distance.retreat_velocity_limit_mps = 1.0
|
||
cfg.distance.no_distance_action = NoDistanceAction.HOLD
|
||
cfg.distance.no_distance_approach_limit_mps = 0.5
|
||
cfg.accel_limit.ax_mps2 = 1000.0
|
||
cfg.accel_limit.ay_mps2 = 1000.0
|
||
cfg.accel_limit.az_mps2 = 1000.0
|
||
return cfg
|
||
|
||
|
||
def make_snapshot(now_ms: int) -> ControlSnapshot:
|
||
"""新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
|
||
return ControlSnapshot(
|
||
track=TrackState(tracked=True, is_predicted=False,
|
||
center=(500.0, 500.0)),
|
||
track_time_ms=now_ms,
|
||
attitude_present=True,
|
||
attitude_time_ms=now_ms,
|
||
)
|
||
|
||
|
||
# ---- 超龄降级（§7.1 / §9.2） ----
|
||
|
||
def test_attitude_missing_degrades():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.attitude_present = False
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||
assert not out.valid
|
||
|
||
|
||
def test_attitude_stale_degrades():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.attitude_time_ms = 600 # 400 ms 前 > 300 ms 阈值
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||
|
||
|
||
def test_vision_stale_degrades():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track_time_ms = 700 # 300 ms 前 > 200 ms 阈值
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.VISION_STALE
|
||
|
||
|
||
def test_never_sampled_track_degrades():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track_time_ms = 0 # 从未取样
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.VISION_STALE
|
||
|
||
|
||
def test_attitude_stale_beats_vision_stale():
|
||
# 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.attitude_present = False
|
||
snap.track_time_ms = 0
|
||
out = law.update(snap, 1000)
|
||
assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
|
||
|
||
|
||
# ---- 判丢刹车（§6.4） ----
|
||
|
||
def test_lost_target_brakes():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.tracked = False
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.BRAKE_HOVER
|
||
assert out.valid
|
||
assert out.vx_mps == 0.0
|
||
assert out.vy_mps == 0.0
|
||
assert out.vz_mps == 0.0
|
||
|
||
|
||
# ---- Coast（§6.4） ----
|
||
|
||
def test_coast_sets_flag_and_keeps_tracking():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.is_predicted = True
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||
assert out.coast_active
|
||
assert out.valid
|
||
|
||
|
||
# ---- 水平航向通道（§4.2，A2） ----
|
||
|
||
def test_rate_mode_maps_beta_to_yaw_rate():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (600.0, 500.0) # 目标偏右 100 像素
|
||
out = law.update(snap, 1000)
|
||
beta_deg = math.degrees(math.atan(0.2)) # ≈ 11.31°
|
||
assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6) # gain=1，右转正
|
||
assert out.yaw_deg == 0.0
|
||
|
||
|
||
def test_rate_mode_clamps_to_yaw_rate_limit():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (950.0, 500.0) # 大偏移：β≈43.5° > 30°/s 上限
|
||
out = law.update(snap, 1000)
|
||
assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9)
|
||
|
||
|
||
def test_position_mode_composes_absolute_heading():
|
||
# A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
|
||
cfg = make_config()
|
||
cfg.heading.mode = HeadingMode.POSITION
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (600.0, 500.0)
|
||
snap.yaw_rad = math.radians(45.0)
|
||
out = law.update(snap, 1000)
|
||
beta_deg = math.degrees(math.atan(0.2))
|
||
assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6)
|
||
assert out.yaw_rate_dps == 0.0
|
||
|
||
|
||
def test_position_mode_wraps_around_180():
|
||
cfg = make_config()
|
||
cfg.heading.mode = HeadingMode.POSITION
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (600.0, 500.0)
|
||
snap.yaw_rad = math.radians(175.0)
|
||
out = law.update(snap, 1000)
|
||
beta_deg = math.degrees(math.atan(0.2))
|
||
# 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
|
||
assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6)
|
||
assert -180.0 < out.yaw_deg < 180.0
|
||
|
||
|
||
# ---- 垂直统一速度支路（§5，A1） ----
|
||
|
||
def test_vertical_offset_drives_vz():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (500.0, 600.0) # 目标偏低 100 像素 → α>0 → 下降
|
||
out = law.update(snap, 1000)
|
||
alpha_deg = math.degrees(math.atan(0.2))
|
||
assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6)
|
||
assert out.mode is ControlMode.VELOCITY_HEADING # A1：不切姿态微调模式
|
||
|
||
|
||
def test_vertical_velocity_clamped():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.track.center = (500.0, 990.0) # α≈44.5° × 0.1 = 4.45 > 2 上限
|
||
out = law.update(snap, 1000)
|
||
assert out.vz_mps == pytest.approx(2.0, abs=1e-9)
|
||
|
||
|
||
# ---- 距离通道（§6.2 / §6.3） ----
|
||
|
||
def test_distance_approach_when_far():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.target_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
out = law.update(snap, 1000)
|
||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||
|
||
|
||
def test_distance_retreat_clamped_when_too_close():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.target_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
out = law.update(snap, 1000)
|
||
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
|
||
|
||
|
||
def test_no_distance_hold_zeros_vx():
|
||
law = TrackingControlLaw(make_config()) # 基准配置 HOLD
|
||
snap = make_snapshot(1000)
|
||
snap.distance_valid = False
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||
assert out.vx_mps == 0.0
|
||
|
||
|
||
def test_no_distance_slow_approach():
|
||
cfg = make_config()
|
||
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.distance_valid = False
|
||
out = law.update(snap, 1000)
|
||
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
|
||
|
||
|
||
def test_no_distance_exit_waits_grace_then_degrades():
|
||
# S1 拍板：EXIT 需持续无距离达门限才降级；grace 期间按 SLOW_APPROACH 行为
|
||
cfg = make_config()
|
||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||
cfg.distance.no_distance_exit_grace_ms = 200
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.distance_valid = False
|
||
out1 = law.update(snap, 1000)
|
||
assert out1.mode is ControlMode.VELOCITY_HEADING
|
||
assert out1.vx_mps == pytest.approx(0.5, abs=1e-9)
|
||
snap.track_time_ms = 1200 # 保持视觉/姿态新鲜，聚焦距离通道
|
||
snap.attitude_time_ms = 1200
|
||
out2 = law.update(snap, 1200) # 恰达门限 → 降级
|
||
assert out2.mode is ControlMode.DEGRADED_HOLD
|
||
assert out2.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||
assert not out2.valid
|
||
|
||
|
||
def test_no_distance_exit_grace_resets_on_valid_frame():
|
||
cfg = make_config()
|
||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||
cfg.distance.no_distance_exit_grace_ms = 200
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.distance_valid = False
|
||
assert law.update(snap, 1000).mode is ControlMode.VELOCITY_HEADING
|
||
|
||
snap.track_time_ms = 1100 # 有效距离帧：重置计时
|
||
snap.attitude_time_ms = 1100
|
||
snap.distance_valid = True
|
||
snap.target_distance_m = 30.0
|
||
snap.distance_time_ms = 1100
|
||
assert law.update(snap, 1100).mode is ControlMode.VELOCITY_HEADING
|
||
|
||
snap.track_time_ms = 1250 # 再次无效：重新计时
|
||
snap.attitude_time_ms = 1250
|
||
snap.distance_valid = False
|
||
snap.target_distance_m = 0.0
|
||
assert law.update(snap, 1250).mode is ControlMode.VELOCITY_HEADING # 0 ms < 200 ms
|
||
|
||
snap.track_time_ms = 1500
|
||
snap.attitude_time_ms = 1500
|
||
out = law.update(snap, 1500) # 250 ms ≥ 200 ms → 降级
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||
|
||
|
||
def test_no_distance_exit_zero_grace_degrades_immediately():
|
||
# grace = 0 保留旧"首个无效拍即退出"行为
|
||
cfg = make_config()
|
||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||
cfg.distance.no_distance_exit_grace_ms = 0
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.distance_valid = False
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||
assert not out.valid
|
||
|
||
|
||
def test_stale_distance_falls_back_to_no_distance():
|
||
law = TrackingControlLaw(make_config()) # HOLD
|
||
snap = make_snapshot(1000)
|
||
snap.target_distance_m = 30.0
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||
out = law.update(snap, 1000)
|
||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||
assert out.vx_mps == 0.0
|
||
|
||
|
||
# ---- 首拍/限幅（§8、S6） ----
|
||
|
||
def test_first_tick_vx_slews_from_zero():
|
||
# S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
|
||
cfg = make_config()
|
||
cfg.accel_limit.ax_mps2 = 2.0
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.target_distance_m = 30.0 # 通道输出本应为 3.0
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
|
||
assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1
|
||
|
||
|
||
# ---- 机体系 → NED 旋转（§2.3，A4） ----
|
||
|
||
def test_output_rotated_to_ned_by_yaw():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.yaw_rad = math.pi / 2.0 # 机头朝东
|
||
snap.target_distance_m = 30.0
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
out = law.update(snap, 1000)
|
||
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
|
||
assert out.vy_mps == pytest.approx(3.0, abs=1e-9)
|
||
|
||
|
||
def test_zero_yaw_keeps_forward_as_north():
|
||
law = TrackingControlLaw(make_config())
|
||
snap = make_snapshot(1000)
|
||
snap.yaw_rad = 0.0
|
||
snap.target_distance_m = 30.0
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
out = law.update(snap, 1000)
|
||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||
assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
|
||
|
||
|
||
# ---- Reset（§6.4） ----
|
||
|
||
def test_reset_restores_slew_from_zero():
|
||
cfg = make_config()
|
||
cfg.accel_limit.ax_mps2 = 2.0
|
||
law = TrackingControlLaw(cfg)
|
||
snap = make_snapshot(1000)
|
||
snap.target_distance_m = 30.0
|
||
snap.distance_valid = True
|
||
snap.distance_time_ms = 1000
|
||
for t in range(1000, 3000, 50):
|
||
snap.track_time_ms = t
|
||
snap.distance_time_ms = t
|
||
snap.attitude_time_ms = t
|
||
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
|
||
law.reset()
|
||
snap.track_time_ms = 3000
|
||
snap.distance_time_ms = 3000
|
||
snap.attitude_time_ms = 3000
|
||
out = law.update(snap, 3000)
|
||
# Reset 后限幅器归零：又从 0.1 开始渐变
|
||
assert out.vx_mps == pytest.approx(0.1, abs=1e-9)
|