双目测距方案重构与其他小修改

2026-09-10 16:16:05 +08:00
parent 3325465359
commit 538ae4c6c7
39 changed files with 2285 additions and 1444 deletions
@@ -9,7 +9,7 @@
 | 名称 | 说明 |
 | --- | --- |
 | `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
-| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） |
+| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit，默认 slow_approach；exit 需持续无距离达 `no_distance_exit_grace_ms` 才降级） |
 | `TrackingControlLaw(cfg)` | 主控律 |
 | `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
 | `.reset()` | 停止闭环时清全部内部状态 |
@@ -23,18 +23,19 @@ class HeadingMode(Enum):


 class NoDistanceAction(Enum):
-    """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
+    """目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）。"""

    HOLD = "hold"                  # 保持：前向速度指令清零
    SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
-    EXIT = "exit"                  # 退出近距操作：输出降级保持
+    EXIT = "exit"                  # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
+                                   # 才降级保持；grace 期间按 SLOW_APPROACH 行为


@dataclass
 class ControlGroupConfig:
    frequency_hz: float = 20.0
    visual_stale_ms: int = 200
-    radar_stale_ms: int = 500
+    distance_stale_ms: int = 500
    attitude_stale_ms: int = 300


@@ -70,8 +71,11 @@ class DistanceGroupConfig:
    derivative_filter_coef: float = 0.0
    approach_velocity_limit_mps: float = 3.0
    retreat_velocity_limit_mps: float = 1.0
-    no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
+    # 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
+    # 持续无距离（传感器级故障）场景
+    no_distance_action: NoDistanceAction = NoDistanceAction.SLOW_APPROACH
    no_distance_approach_limit_mps: float = 0.5
+    no_distance_exit_grace_ms: int = 3000  # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）


@dataclass
@@ -155,9 +159,9 @@ def _load_control(root: dict) -> ControlGroupConfig:
    c.frequency_hz = _require_positive(
        _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
    c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
-    c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
+    c.distance_stale_ms = _require_integer(s, "control", "distance_stale_ms")
    c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
-    if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
+    if c.visual_stale_ms <= 0 or c.distance_stale_ms <= 0 or c.attitude_stale_ms <= 0:
        raise ValueError("配置非法: control.*_stale_ms 必须为正")
    return c

@@ -240,6 +244,10 @@ def _load_distance(root: dict) -> DistanceGroupConfig:
    d.no_distance_approach_limit_mps = _require_non_negative(
        _require_number(s, "distance", "no_distance_approach_limit_mps"),
        "distance.no_distance_approach_limit_mps")
+    # 0 容许：等效旧版"首个无效拍即退出"行为
+    d.no_distance_exit_grace_ms = _require_non_negative(
+        _require_integer(s, "distance", "no_distance_exit_grace_ms"),
+        "distance.no_distance_exit_grace_ms")
    return d


@@ -10,7 +10,8 @@
  ④ 像素偏移 → 视线角 β/α（全程针孔模型）
  ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
  ⑥ 垂直统一速度支路（A1：不切姿态微调模式）
-  ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
+  ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit，
+     exit 需持续无距离达 no_distance_exit_grace_ms 门限）
  ⑧ 机体系 → 局部 NED 旋转（A4）

 日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
@@ -78,6 +79,10 @@ class TrackingControlLaw:
        self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
        self._last_now_ms = 0
        self._last_mode = ControlMode.DEGRADED_HOLD
+        # EXIT 无距离计时（§6.3：持续无距离达 no_distance_exit_grace_ms 才退出，
+        # 任一有效距离帧重置）
+        self._no_distance_active = False
+        self._no_distance_since_ms = 0

    def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
        # ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
@@ -107,6 +112,7 @@ class TrackingControlLaw:

        # ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
        if not snapshot.track.tracked:
+            self._no_distance_active = False  # 刹车期间不计 EXIT 无距离时长
            if self._last_mode is not ControlMode.BRAKE_HOVER:
                # 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
                self._distance_pid.reset()
@@ -156,14 +162,16 @@ class TrackingControlLaw:
                           self._cfg.vertical.vz_limit_mps)

        # ---- ⑦ 距离通道（§6.2 / §6.3） ----
-        # 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
-        distance_available = (snapshot.associated_to_target
-                              and snapshot.radar_distance_m > 0.0
-                              and not is_stale(snapshot.radar_time_ms, now_ms,
-                                               self._cfg.control.radar_stale_ms))
+        # 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
+        # （§6.3、FR-041）
+        distance_available = (snapshot.distance_valid
+                              and snapshot.target_distance_m > 0.0
+                              and not is_stale(snapshot.distance_time_ms, now_ms,
+                                               self._cfg.control.distance_stale_ms))
        if distance_available:
+            self._no_distance_active = False  # 任一有效距离帧重置 EXIT 计时
            # Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
-            delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m
+            delta_d = snapshot.target_distance_m - self._cfg.distance.d_exp_m
            vx_raw = self._distance_pid.update(delta_d, dt_s)
            slewed = self._vx_limiter.limit(vx_raw, dt_s)  # 加速度限幅（§8）
            out.vx_mps = clamp(slewed,
@@ -182,10 +190,20 @@ class TrackingControlLaw:
                out.vx_mps = self._vx_limiter.limit(
                    self._cfg.distance.no_distance_approach_limit_mps, dt_s)
            else:  # NoDistanceAction.EXIT
-                # 退出近距操作：整体降级保持
-                out.mode = ControlMode.DEGRADED_HOLD
-                out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
-                return self._finish_and_log(out, "距离未关联，退出近距操作")
+                # 退出近距操作：双目有效域外无距离是常态（D4 远场定速接近），
+                # 首个无效拍不退出；持续无距离达 no_distance_exit_grace_ms
+                # （疑似传感器级故障）才降级。grace 期间按 SLOW_APPROACH 行为
+                # 执行（经加速度限幅，vx 平滑过渡）；grace=0 保留旧"立即退出"行为
+                if not self._no_distance_active:
+                    self._no_distance_active = True
+                    self._no_distance_since_ms = now_ms
+                if (now_ms - self._no_distance_since_ms
+                        >= self._cfg.distance.no_distance_exit_grace_ms):
+                    out.mode = ControlMode.DEGRADED_HOLD
+                    out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
+                    return self._finish_and_log(out, "持续无距离达门限，退出近距操作")
+                out.vx_mps = self._vx_limiter.limit(
+                    self._cfg.distance.no_distance_approach_limit_mps, dt_s)

        # ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
        # 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
@@ -207,6 +225,7 @@ class TrackingControlLaw:
        self._yaw_rate_limiter.reset()
        self._vx_limiter.reset()
        self._vz_limiter.reset()
+        self._no_distance_active = False
        self._last_now_ms = 0
        self._last_mode = ControlMode.DEGRADED_HOLD

@@ -55,9 +55,10 @@ class ControlSnapshot:
    track: TrackState = field(default_factory=TrackState)
    track_time_ms: int = 0            # 取样单调时钟（毫秒），<=0 视为从未取样

-    radar_distance_m: float = 0.0     # 机头前向单点距离（米）
-    associated_to_target: bool = False  # 相机—雷达空间关联成立标志（未关联不得当目标距离）
-    radar_time_ms: int = 0
+    # 目标距离（双目立体测距，§6.3：distance_valid 为 False 时不得当作目标距离）
+    target_distance_m: float = 0.0    # 目标距离（米，双目前向分量 Z）
+    distance_valid: bool = False      # 距离有效标志（测距来自检测框自身，无跨模态关联）
+    distance_time_ms: int = 0         # 取样单调时钟（毫秒）

    attitude_present: bool = False    # 姿态源当前是否可用（过期 → 禁止控制）
    attitude_time_ms: int = 0