first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,338 @@
+"""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）、距离通道三种
+无距离降级动作、首拍变化率限幅（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.radar_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.radar_distance_m = 30.0  # Δd = +20 → kp 输出 10 → 限幅 3
+    snap.associated_to_target = True
+    snap.radar_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.radar_distance_m = 5.0  # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
+    snap.associated_to_target = True
+    snap.radar_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())  # 缺省 kHold
+    snap = make_snapshot(1000)
+    snap.associated_to_target = 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.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
+
+
+def test_no_distance_exit_degrades():
+    cfg = make_config()
+    cfg.distance.no_distance_action = NoDistanceAction.EXIT
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.associated_to_target = 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_radar_falls_back_to_no_distance():
+    law = TrackingControlLaw(make_config())  # kHold
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_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.radar_distance_m = 30.0  # 通道输出本应为 3.0
+    snap.associated_to_target = True
+    snap.radar_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.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_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.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_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.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    for t in range(1000, 3000, 50):
+        snap.track_time_ms = t
+        snap.radar_time_ms = t
+        snap.attitude_time_ms = t
+        law.update(snap, t)  # 若干拍后 vx 已爬升到限幅 3.0
+    law.reset()
+    snap.track_time_ms = 3000
+    snap.radar_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)