diff --git a/docs/双目测距技术路线.md b/docs/双目测距技术路线.md new file mode 100644 index 0000000..831105d --- /dev/null +++ b/docs/双目测距技术路线.md @@ -0,0 +1,754 @@ +# 双目测距技术路线 + +> 本文档把双目测距（立体目标测距）抽象为**与编程语言、硬件平台无关**的技术路线： +> 只讲几何模型、数据结构、算法流程与质量语义，不依赖任何语言特性、加速器 +> 型号或操作系统。可作为任意语言（C++/Rust/Python 仿真/嵌入式 C）在任意 +> 算力平台上重新实现或移植的规格说明。 +> +> **文档分工**（各司其职，不重复抄录）： +> +> | 文档 | 定位 | 与本文的分工 | +> |------|------|--------------| +> | `docs/双目深度估计思路.md` | 平台工程路线 + 原稿对照 | **平台与工程细节权威**：采集/解码链路、加速器预算、线程与文件落点、原稿缺陷对照；本文一律不涉及 | +> | 本文 | **算法路线** | **算法与语义权威**：几何、取点、匹配、有效性、滤波、方位源、误差模型、职责与缺点 | +> | `docs/视觉跟踪控制律技术路线.md` | 距离/方位的消费者 | 其距离通道与方位源如何消费本文输出，见其 §1.2/§2/§6.4 | +> | `docs/双目测距方案讨论纪要.md` | 决策索引 | D2/D4/D5/S1~S3 等拍板结论的唯一索引，本文只落实、不重新讨论 | +> +> 三份技术路线若出现表述分歧：拍板结论以《双目测距方案讨论纪要》为准， +> 算法语义以本文为准，平台工程以《双目深度估计思路》为准。 +> +> **参考实现**：`utils/stereo_ranger/`（Python，构件划分与本文逐条对应）。 +> 实现时冻结的补充决策与发现的原文矛盾已回填本文（§4/§6.1/§6.2/§6.4/ +> §6.7/§9/§11），两文档以本文为准。 +> +> ⚠️ **全部数值均为占位值**：焦距/标定参数待厂商数据（P1/P2），各阈值未整定。 +> 标定与地面真值验证完成前，本模块输出**仅限静态标靶/回放/台架验证， +> 不得作为飞行控制决策依据**（见 §12.3 红线）。 + +--- + +## 目录 + +1. [概述与系统定位](#1-概述与系统定位) +2. [输入输出接口定义](#2-输入输出接口定义) +3. [坐标系与符号约定](#3-坐标系与符号约定) +4. [核心构件与状态清单](#4-核心构件与状态清单) +5. [单周期处理流程（主流程伪代码）](#5-单周期处理流程主流程伪代码) +6. [各环节算法细节](#6-各环节算法细节) +7. [误差模型与有效距离域](#7-误差模型与有效距离域) +8. [职责、边界与已知缺点](#8-职责边界与已知缺点) +9. [配置参数全量表](#9-配置参数全量表) +10. [设计取舍记录（为什么不是别的做法）](#10-设计取舍记录为什么不是别的做法) +11. [实现与重写指引](#11-实现与重写指引) +12. [验证路线与待定项](#12-验证路线与待定项) + +--- + +## 1. 概述与系统定位 + +### 1.1 模块职责 + +本模块是**目标距离与方位的机载唯一来源**（决策 D2：双目替代激光雷达， +与提交 b96111e 一致）。它对**同一时刻**左右两幅已立体校正图像上的目标 +检测框做立体几何解算，输出**少量目标的**距离与方位（不是稠密深度图）。 +职责四件事： + +1. **匹配**：把左右两目的检测框按"同一物理目标"配对（硬约束筛选 + + 贪心代价排序）； +2. **测距**：同侧边缘视差 D → `Z = F·B / D` 前向距离（斜距 R 仅参考输出）； +3. **方位源**：输出**虚拟双目中心**像素 `c_mid`（两目框中心等权平均， + 消除单目近场视差偏差，§6.6）； +4. **质量语义**：输出有效性标志与置信度（有效域内 + 匹配成功 + 一致性 + 正常），对距离做轻量时间滤波；域外/失配/异常时**明确输出无效**， + 不得强行估计。 + +### 1.2 在系统中的位置 + +```text +平台采集/解码/立体校正（平台相关，见《双目深度估计思路》第 3/8 章） + → 校正后左/右图像（同帧） + → 左/右目标检测（外部模块，输出原图坐标系的检测框） + → 本模块：匹配 + 测距 + 方位源 + 有效性 + 滤波 → 目标距离记录 + ├─ 距离 Z（valid）──▶ 控制律距离通道（Δd → PID → 前向速度，域内闭环） + ├─ 方位 c_mid ──────▶ 接入层填入控制律目标中心（虚拟中心相机像素） + └─ valid / 置信度 ──▶ 接入层降级语义： + 域外/失配 → "无距离"策略（方位闭环 + 定速接近，D4） + 持续无距离 → exit 档降级（S1 门限，消费侧实现） +``` + +**上游**：同帧校正图像对与左右检测框（检测、采集、解码、校正均为外部 +模块）；**下游**：跟踪控制律与接入层。"域外定速接近"的远场补偿策略 +属消费侧行为，本文只负责提供诚实的有效性标志与持续的方位。 + +### 1.3 设计特点 + +| 特点 | 含义 | 带来的约束 | +|------|------|-----------| +| **稀疏输出** | 只算检测到的目标，不做逐像素匹配 | 无检测即无输出；距离精度受框质量制约（§8） | +| **纯值输入/输出** | 每帧传入同帧框对 + 参数，输出纯值记录列表 | 无隐藏输入、可离线回放 | +| **纯函数构件** | 取点、测距、域判定均为无状态纯函数 | 可独立单元测试（合成框对即可，无需真实图像） | +| **有状态部分极小** | 仅滤波器、一致性监控与目标隔离状态，且按目标隔离 | 重写时状态清单见 §11.1，漏一个就变行为 | +| **参数全外置** | 几何、阈值、滤波参数全部来自配置 | 代码内不得写死任何相机参数或阈值 | +| **诚实降级** | 域外/失配输出 `valid = false` 而非勉强给值 | 消费侧的远场策略建立在"不撒谎"的有效性上 | + +### 1.4 明确不做的事（边界） + +- **不做采集/解码/立体校正**：消费校正后的图像与框；校正与自标定属 + 平台工程（v1.1 备选），不在本文范围； +- **不做稠密匹配**：不做 SGBM 类逐像素立体匹配，不输出深度图； +- **不做环境感知/避障**：环境深度由避障侧深度模型负责，与本模块并存 + 互不替代； +- **不做目标跟踪**：不维护轨道与运动滤波；上游 track_id 只用作目标 + 状态隔离键（§4），不参与匹配打分与连续性先验（后者留 v1.1）； +- **不做运动/IMU 补偿**：帧间运动引起的虚假视差靠帧同步契约与一致性 + 监控兜底，不做解算级补偿； +- **不做多传感器融合**：双目是目标距离机载唯一来源（D2），无交叉校验 + 基准；数据质量靠内部一致性监控自检（§6.7）； +- **不在域外强行估计**：超出有效距离域一律输出 `valid = false`。 + +--- + +## 2. 输入输出接口定义 + +### 2.1 输入（每帧一次，由平台层供给） + +| 分组 | 项 | 语义与约束 | +|------|----|-----------| +| 帧对 | 左帧、右帧 | 各含**帧标识**；两帧标识必须一致（同帧契约，见下） | +| 检测 | 检测框数组 ×2 | 每框含：类别、置信度、框（左上/右下角像素）；**已映射回校正图像坐标系** | +| 几何 | 基线 `B`、焦距 `F`、主点 `cx, cy` | 校正后内参（`fx = fy = F`）；来自标定配置，禁止写死 | +| 参数 | 匹配/有效性/滤波阈值 | 配置外置（§9），启动期加载并校验 | +| 时钟 | 当前单调时刻 | 用于输出打戳 | + +**契约要点**： + +- **同帧契约**：左右帧必须来自同一曝光时刻（同步与配对由平台层保证， + 形态差异见《双目深度估计思路》§3.2）。本文把"左右帧标识一致"作为 + 输入违约检查：不一致直接拒收本帧、输出空、告警节流——**不同帧的 + 框配对解算没有物理意义**； +- **校正契约**：检测框位于**已立体校正**的图像上（"两目 y 对齐"是校正 + 的结果而非假设，§3）；未校正图像上的框不得进入本模块； +- **坐标基准一致**：检测若在缩放/裁剪图上运行，框必须先映射回校正图像 + 基准，且几何参数 `F/cx/cy` 使用同一基准的内参——否则视差与几何失配， + 距离系统性错误； +- **时间戳纪律**：输出取样时刻以**平台接收时刻的单调时钟**为准， + 不伪造采集时刻。 + +### 2.2 输出：目标距离记录（每帧每目标一条，纯值） + +| 字段 | 单位/类型 | 语义 | +|------|-----------|------| +| 帧标识 | — | 继承自输入帧对 | +| 类别 | 枚举 | 目标类别 | +| 轨道关联 | 整数或"无" | 检测侧 track 关联（如有；v1.0 可无） | +| `Z` | m | 前向距离（沿光轴分量，**距离通道唯一消费量**） | +| `R` | m | 斜距（参考值，不进控制回路；与已发布的 `Z` **同基准**——按滤波后 Z 换算，保证 `R ≥ Z`） | +| `D` | px | 本次视差（调试与交叉校验用） | +| `c_mid` | 像素 (x, y) | **虚拟双目中心**框中心：方位控制源（§6.6） | +| 参考目框 | 像素框 | 左目（参考目）检测框，供交叉校验与调试 | +| `confidence` | [0,1] | 匹配质量与一致性综合置信度 | +| `valid` | 布尔 | 是否落在有效距离域且匹配成功且一致性正常（§6.4） | +| 退化标记 | 枚举 | `完整`（距离+方位）/ `仅方位`（单目检出，§5 第 ⑤ 步） | +| 取样时刻 | 单调时钟 | 平台接收时刻打戳（§2.1 时间戳纪律） | + +**记录两态**： + +- **完整记录**（匹配成功）：距离 + 方位 + 有效性； +- **仅方位记录**（仅一目检出，v1.0 允许）：发布该目框中心作方位， + `valid = false`，无距离；单目方位存在近场视差偏差（§6.6，v1.0 接受）。 + +### 2.3 输出语义与消费侧衔接 + +- `valid == true` 的 `Z` 才可作为控制律距离通道输入；消费侧另有自己的 + "有效性 + 正值 + 未超龄"三重门（见《视觉跟踪控制律技术路线》§6.4）； +- `valid == false`（域外/失配/一致性异常）→ 消费侧进入"无距离"策略 + （方位闭环 + 定速接近，默认档），持续无距离达门限才退出近距操作 + （消费侧 exit 门限，S1）； +- **`c_mid` 始终随检测发布**（域外仍有方位）——这是远场"方位闭环 + + 定速接近"策略得以成立的输入基础； +- 消费侧以 `track_id` 一致性校验本记录与追踪主目标对应（调用方约定 + S3，库内不实现）。 + +--- + +## 3. 坐标系与符号约定 + +### 3.1 符号表 + +| 符号 | 含义 | 单位 | +|------|------|------| +| `B` | 基线长度（左右光心间距） | m | +| `F` | 像素焦距（校正后 `fx = fy`） | px | +| `cx, cy` | x/y 方向主点 | px | +| `xL, yL` | 目标点在**左目（参考目）**图像中的像素坐标 | px | +| `xR` | 同一目标点在右目图像中的像素坐标 | px | +| `D` | 视差 = `xL − xR` | px | +| `Z` | 前向距离（沿光轴分量） | m | +| `X, Y` | 相机（左目）坐标系横向/纵向偏移 | m | +| `X_C` | 目标在**模组中心系**的横向偏移 | m | +| `R` | 斜距 | m | +| `cL, cR` | 同一目标在左/右目中的**检测框中心** | px | +| `c_mid` | 虚拟双目中心框中心（方位源） | px | + +**统一约定**：校正后**以左目为参考系**，视差 `D = xL − xR > 0` +（正视差是物理前提；`D ≤ 0` 视为违例，§6.3）。 + +### 3.2 公式（校正后坐标系） + +```text +D = xL − xR （同一物理特征点的 x 坐标差） +Z = F × B / D +X = (xL − cx) × Z / F （左目系） +Y = (yL − cy) × Z / F +R = sqrt(X² + Y² + Z²) +``` + +- **距离通道使用 Z（前向分量）而非 R**：与机头前向的期望射程语义对齐， + 也与地面真值仪（手持激光测距仪）的测量方向一致（决策 D8）； + 小角度下 `Z ≈ R`，大偏航角目标时二者差异增大，地面验证解读数据须注意； +- **X/Y 以左目系表达**；模组中心系下 `X_C = X − B/2`（左目光心在模组中线 + −B/2 侧，符号与 §6.6 一致；原文 `+B/2` 为笔误），`B/2` 对 `Z`、`R` 的 + 影响可忽略（本机型 3cm）。 + +--- + +## 4. 核心构件与状态清单 + +| 构件 | 职责 | 运行期持久状态 | 备注 | +|------|------|---------------|------| +| 流程编排 | 单周期五步流程（§5） | 无 | 每帧一次 | +| 视差取点器 | 框 → 取点 x（同侧边缘，回退中心） | 无（纯函数） | §6.1 | +| 匹配器 | 左右框配对 | 无（纯函数） | **接口注入**，默认贪心；可替换为匈牙利等而不动调用方 | +| 测距计算 | `(D, 框)` → `(Z, X, Y, R, c_mid)` | 无（纯函数） | §3.2/§6.6 | +| 有效性判定 | 域内 + 误差预算判定 | 无（纯函数） | §6.4 | +| 一致性监控 | 时序层面质量自检 | **每目标**：上次视差 + 连续异常计数 | §6.7 | +| 时间滤波器 | 距离低通 + 跳变重置 | **每目标**：上次滤波值、初始化标志、连续失效计数 | §6.5 | +| 目标状态容器 | 隔离键解析 + 帧末老化清理 | **每目标**：最近框中心、最近出现帧 | 隔离键见下；连续 `filter_reset_frames` 帧未出现则整体清理 | +| 告警节流器 | 异常日志限频 | 每类异常：首见标记 + 累计计数 | 第 1 次 + 每满 100 次 | + +**目标隔离键（v1.0 冻结）**：`track_id` 优先——左右目的 track_id 一致且 +为正时，用 `(轨道, 类别, id)` 作键（v1.1 语义提前启用，上游追踪库已在 +链路中）；否则回退**框位置最近邻**（同类别、门限 `isolation_gate_px` 内、 +本帧尚未被占用的最近状态，一一归属）。 + +重写/移植时最容易漏的是**每目标隔离**的一致性计数与滤波状态——按 +"全局单份"实现会在多目标下互相污染。 + +--- + +## 5. 单周期处理流程（主流程伪代码） + +```text +函数 单周期处理(左帧, 右帧, 参数, now_ms) -> 目标距离记录列表: + + # ---- ① 输入违约检查 ---- + 若 左帧.帧标识 ≠ 右帧.帧标识: + 告警节流("左右帧非同帧，拒收") + 返回 空列表 + + # ---- ② 取点（每目每框一次，纯函数） ---- + 左点表 ← 对 左帧.框列表 逐框取点(同侧边缘 x) # §6.1 + 右点表 ← 对 右帧.框列表 逐框取点(同侧边缘 x) + 丢弃 坐标非有限值 的框（不参与匹配，也不发布记录；告警节流） + + # ---- ③ 左右匹配（接口注入，默认贪心） ---- + 配对表 ← 匹配器.匹配(左点表, 右点表, 参数.匹配) # §6.2 + + # ---- ④ 配对成功的目标：测距 + 方位源 + 有效性 + 滤波 ---- + 对 配对表 中每个 (左框, 右框, 代价): + D ← 视差选择(左框, 右框) # 同侧边缘；越窗且中心在域内→回退中心（§6.1） + (Z, R) ← 测距(D, 参数.几何) # §3.2 + c_mid ← (左框.中心 + 右框.中心) / 2 # §6.6 + 域内 ← 有效性判定(D, Z, 代价, 参数.有效性) # §6.4 门①② + 一致性 ← 一致性监控(目标键).推进(D) # §6.7 + 置信度 ← 置信度映射(代价, 一致性) # §6.4 映射 + 有效 ← 域内 且 一致性 正常 + + 若 有效: + Z_out ← 滤波器(目标键).推进(Z, 参数.滤波) # §6.5 + 否则: + 滤波器(目标键).记失效() # §6.5 失效策略 + Z_out ← 无 + + 发布 记录(帧标识, 类别, Z_out, R, D, c_mid, 左框, + 置信度, 有效, 完整, 取样时刻 = now_ms) + + # ---- ⑤ 单目退化（仅一目检出） ---- + 对 未匹配的左框: + 发布 记录(仅方位 = 左框.中心, valid = false, 退化 = 单目) + 对 未匹配的右框: + 发布 记录(仅方位 = 右框.中心, valid = false, 退化 = 单目) + + # ---- ⑥ 空输入 ---- + 若 两目框列表均为空: + 发布 无目标记录（或空列表）；不告警（正常空帧） +``` + +**失败路径语义**： + +| 情形 | 行为 | 日志 | +|------|------|------| +| 左右帧标识不一致 | 拒收本帧，输出空 | WARN 节流 | +| 单目检测为空 | 另一目发布仅方位记录 | 不告警（正常退化） | +| 匹配全部失败 | 全部按仅方位发布 | 不告警 | +| 视差违例（`D ≤ 0` 或超界） | 该目标 `valid = false`，仍配对并发布 `D`/`c_mid` | WARN 节流 | +| 边缘视差越窗、中心回退 | 该目标用中心视差解算（框宽差可能混入，§6.1） | WARN 节流 | +| 框坐标非法（非有限值） | 丢弃该框，不参与匹配也不发布 | WARN 节流 | +| 两目均无检测 | 无目标记录 | 不告警 | + +高频热路径（每帧执行的数学）不打日志；所有异常日志按节流规范 +（第 1 次 + 每满 100 次，带累计计数）。 + +--- + +## 6. 各环节算法细节 + +### 6.1 视差取点：同侧边缘（v1.0 主方案） + +同一物理目标在两目中的检测框宽并不严格相等（检测抖动、外观差异）， +**框中心**视差会混入框宽差的一半，**同一侧边缘**视差可抵消框宽项： + +| 取点方式 | 误差来源 | 结论 | +|----------|----------|------| +| 框中心 | 两目框宽差直接进入视差 | 作对照/回退 | +| **同侧边缘（v1.0）** | 边缘定位噪声 | **v1.0 主方案** | +| 多边缘 + 亚像素精化（灰度重心/抛物线拟合） | 实现复杂度 | v1.1 | + +```text +取法: D = 左框.左上角x − 右框.左上角x （同取左侧边缘） +回退: 边缘视差越窗 且 中心视差在域内 → 回退框中心并告警节流 + 两者均越窗 → 保留边缘值（供诊断与违例告警），由 §6.4 门 ① 判无效 +``` + +> **不做"左右两边缘各算一次取中值"（v1.0 冻结）**：两边缘视差的中值恒 +> 等于二者均值，即 `(cL.x − cR.x)`——**框中心视差**，与本表"同侧边缘 +> 抵消框宽差"的理由自相矛盾（早期原文笔误），故不实现；亚像素精化留 +> v1.1（§8.3）。 + +### 6.2 左右匹配：硬约束 + 贪心代价 + +**硬约束**（任一不满足直接拒绝配对，不进代价排序）： + +- 类别一致； +- `|yL − yR| ≤ τ_y`（校正后 y 应对齐；这是校正质量的运行期体现）； +- 框高相近：`|hL − hR| / max(hL, hR) ≤ τ_h`。 + +**视差域不是配对硬约束（v1.0 冻结）**：越界目标仍应配对、仍发布 `D` 与 +`c_mid`（远场"方位闭环 + 定速接近"的输入基础），仅由 §6.4 门 ① 判 +`valid = false`。若把域并入硬约束，越界目标会退化为无方位的失配记录， +与 §2.3/§5 失败路径表的语义冲突。 + +**代价与贪心**： + +```text +cost = |Δy| / τ_y + |Δh| / (τ_h × max(hL, hR)) +按 cost 升序逐一配对，cost < 1 才接受，一一配对（一个左框至多配一个右框） +``` + +- 本场景目标数极少（1~2 个），O(n²) 贪心足够（§10.3）； +- 失败处理：任一目未匹配 → 该目标本帧无距离（仅方位发布）， + 不阻塞其他目标。 + +### 6.3 测距计算与违例处理 + +```text +Z = F × B / D （D ≤ 0：物理违例 → valid = false，告警节流） +R = sqrt(X² + Y² + Z²) （参考输出） +``` + +- `D ≤ 0` 在校正正确的正视差几何下不应出现；出现说明匹配错误或校正 + 残差过大，宁可无效不可给值； +- `D ≤ 0` 时**不出解算结果**（无 `Z`、无 `R`）；超界（`0 < D < D_min` + 或 `> D_max`）仍解算 `Z`/`R` 供诊断与告警，同样 `valid = false`； +- 边缘取点为像素级整数（取整/插值规则由实现自定，但**左右目必须一致**）； +- 所有除法分母（`D`）在使用前校验为正。 + +### 6.4 有效性判定：三道门（缺一不可） + +| 门 | 判据 | 拦截什么 | +|----|------|---------| +| ① 域内 | `D ∈ [D_min, D_max]`（含边界）；距离域由 `Z = F·B/D` 派生 | 远场小视差病态 + 近场大视差盲区 | +| ② 误差预算 | `δZ(δD=1px) = Z²/(F·B) ≤ δZ_max` | 有效域边界处的精度下限 | +| ③ 一致性正常 | §6.7 监控未判异常 | 持续视差跳变（标定漂移/同步故障/匹配错误） | + +```text +valid = 域内 且 误差预算内 且 匹配成功 且 一致性正常 +``` + +匹配成功由第 ③ 步保证（只有配对目标才进入本节判定）。 + +**门 ① 以视差域为唯一真源（v1.0 冻结）**：配置只存 +`disparity_min_px / disparity_max_px`，`Z_min / Z_max` 由 `Z = F·B/D` +派生为只读量——`D` 是直接观测量，`Z` 是导出量，用 `D` 判域可避免 +"距离阈值反推视差阈值"的舍入不一致；两处冗余存储正是该不一致的根源 +（原文 §9.2 占位值即不自洽：`D_min = 3px` ⇔ 19.2m ≠ 20m），故不再 +"互推并校验自洽"。 + +**门 ② 的 δD 基准固定为 1px（⚠占位，v1.0 冻结）**：δD 的真实构成 +（边缘定位噪声 + 校正残差 + 帧不同步 + 压缩伪影，§7.1）留待地面真值 +拟合（§12.2），当前不做估计、不随目标状态变化。 + +**置信度映射（⚠占位，v1.0 冻结）**：基值 = `clamp(1 − 匹配代价, 0, 1)`； +连续跳变 ≥ N 帧（§6.7）时乘降级系数（⚠0.5）；判 invalid（≥ M 帧）时置 0。 + +### 6.5 时间滤波与跳变重置（每目标独立） + +```text +推进(Z_new): + 若 未初始化: 状态 ← Z_new; 返回 Z_new + 若 |Z_new − 上次| > 跳变门限: + 状态 ← Z_new # 直接重置而非滤波，防止拖尾 + 否则: + 状态 ← 上次 + α × (Z_new − 上次) # 一阶低通，α ∈ (0,1] + 返回 状态 + +记失效(): # 无效帧与"目标本帧缺席"均记失效（§6.5 失效策略） + 连续失效计数 +1 + 若 连续失效 ≥ filter_reset_frames: 清空滤波状态（下次有效帧重新初始化） +``` + +- **单帧毛刺由跳变重置处理，持续性异常由 §6.7 一致性监控处理**， + 两者职责不同不得合并； +- `α = 1` 等效不滤波；滤波只在**有效帧**之间进行，无效帧不参与平滑； +- 清理门限 `filter_reset_frames` **独立配置**（不复用 §6.7 的 M，⚠占位 + 5 帧），防"陈旧状态平滑新测量"； +- 参考斜距 `R` 与**滤波后**的 `Z_out` 同基准（`X/Y/Z ∝ Z`，按 + `Z_out/Z_raw` 精确换基，保证 `R ≥ Z`，§2.2）； +- 滤波引入的群延迟约 `1/α` 帧（⚠占位 α=0.3 → 约 3 帧 ≈ 0.1s）， + 接近阶段的距离滞后由消费侧 PID 与限幅吸收。 + +### 6.6 方位源：虚拟双目中心 `c_mid` + +**问题**：双目模组两枚相机分列模组中线两侧（各偏 ±B/2），都不在机身 +中轴上。校正后严格成立的只有 **y 对齐**（垂直方向不受两目差异影响）； +水平方向上直接以某一目（如左目）的像素做方位控制，收敛后对准的是 +**该相机的光轴**，相对模组中线存在恒定视差偏差： + +```text +β_bias ≈ atan( (B/2) / Z ) +``` + +| 距离 Z | 角度偏差 | 像素等效（F≈960px，⚠占位） | +|-------:|-------:|-------:| +| 3 m | 0.57° | ≈10 px | +| 10 m | 0.17° | ≈2.9 px | + +远距离可忽略，**近场不可忽略**；偏差方向恒定、随距离减小。 + +**精确修正（零额外成本）**：校正后几何下，位于基线中点的**虚拟相机** +对任一物理点 P 的投影恰为该点在两目投影的**等权平均**： + +```text +xL = cx + F·(X_C + B/2)/Z （左目光心在模组中线 −B/2 侧） +xR = cx + F·(X_C − B/2)/Z （右目光心在模组中线 +B/2 侧） +(xL + xR) / 2 = cx + F·X_C/Z （= 虚拟相机投影，逐点精确，无近似） +``` + +检测框的每条边缘都是物理点、逐点满足上式，因此： + +```text +虚拟框 = 两目框逐边平均 ⇒ 虚拟框中心 c_mid = (cL + cR) / 2 +c_mid.y = (cL.y + cR.y) / 2 （校正后两目 y 天然相等，取平均兼得降噪） +``` + +即**等权平均等价于把相机搬到模组中线**，方位控制对标的是虚拟中心相机， +与机体中轴的安装偏移（如有）属安装外参，按安装标定处理。 + +**方位源与视差取点是两个不同的取点，不得混用**： + +| 用途 | 取点 | 原因 | +|------|------|------| +| 视差 D（测距） | **同侧边缘** | 抵消两目框宽差（§6.1） | +| 方位 c_mid（控制） | **框中心等权平均** | 瞄点语义 = 虚拟框中心；误用边缘点平均会把瞄点偏到目标边缘（约半个框宽） | + +**单目退化**（仅一目检出，v1.0）：退化为该目框中心，接受上表偏差； +v1.1 可用最近一次有效 Z 做定值修正 `Δx = F·(B/2)/Z` px。 + +### 6.7 内部一致性监控（替代原激光交叉校验） + +机载激光移除后无外部距离基准，飞行中只能靠自身时序一致性监控数据 +质量（**只能降级、不能纠偏**，绝对精度靠地面静态验证，§12.1）： + +```text +逐目标推进(D): + 视差跳变 = |D − 上次 D|（或等效 Z 跳变量；门限 consistency_jump_px） + 连续 N 帧跳变超门限 → confidence 降级（⚠×0.5） + 连续 M 帧异常 → valid 置 false 并 WARN（节流） + 任一正常帧 → 清零连续计数 +``` + +- 匹配层面质量（|Δy| 残差、框高一致性、代价门限）已由 §6.2 硬约束 + 承担；本节只管**时序层面**； +- 跳变门限 `consistency_jump_px`（⚠占位 3px）、N / M（⚠ 3 / 5 帧）均 + 为占位，实测后整定（§9.3）； +- **推进时机（v1.0 冻结）**：只在配对成功帧推进——仅方位帧没有 `D` + 观测量，不推进 D 序列、也不清零；目标缺席/无效帧的**滤波失效**由 + §6.5 记，两条状态机职责不混； +- 监控判 invalid 是"质量语义"而非"错误"：输出仍正常发布（仅方位）， + 由消费侧按无距离策略接管。 + +--- + +## 7. 误差模型与有效距离域 + +### 7.1 误差模型 + +由 `D = F·B/Z` 微分得： + +```text +δZ ≈ Z² · δD / (F·B) （绝对误差，随距离平方增长） +δZ / Z ≈ Z · δD / (F·B) （相对误差，随距离线性增长） +``` + +`δD` 的构成：检测框边缘定位噪声 + 校正残差（|Δy| 失配混入）+ 帧不同步 +引入的运动虚假视差 + 有损压缩伪影（平台相关，抽象计入边缘噪声）。 +基线与焦距误差（δB/B、δF/F）线性传递、量级小，出厂标定后可忽略。 + +**这是"双目只能作近场距离源"的定量依据**，也是第 8 章多数缺点的根源。 + +### 7.2 有效距离域判据 + +```text +valid 域 ⇔ D ∈ [D_min, D_max]（⚠占位 3 ~ 57.6px） + 且 δZ(δD = 1px) ≤ δZ_max（⚠占位 2m） +``` + +- 按占位焦距计算：宽角档（HFOV≈90°）有效域约 **≤15m**、窄角档 + （HFOV≈60°）约 **≤20m**（**⚠占位，待厂商实焦距回填后重算**； + 数值回填的权威位置在《双目深度估计思路》§5.3 误差表）； +- 近界：`D_max`（或 `Z_min`）限定最近可测距离——目标过近时视差超上限、 + 框占满画面、边缘取点失效（§8 第 3 条）； +- 敏感性：误差 ∝ 1/B。更换大基线机型误差按比例缩小、有效域等比扩大 + ——**换机型（P6）是扩大远场能力的唯一杠杆**。 + +### 7.3 远场空洞（如实声明） + +双目有效域之外（预计 >15~20m，待焦距回填），系统**不存在任何机载 +目标距离源**。补偿方式是消费侧的远场策略（方位闭环 + 定速接近，D4）， +而非强行使用不可靠的远场估计。本模块的职责边界是：把"测不了"诚实 +地表达为 `valid = false` + 持续发布方位。 + +--- + +## 8. 职责、边界与已知缺点 + +### 8.1 职责（一句话版） + +> 对同帧左右检测框做立体几何解算：**有效域内给出目标前向距离， +> 全程给出虚拟中心方位，测不了时诚实输出无效**——供控制律做 +> 近场距离闭环与远场方位闭环 + 定速接近。 + +边界（不做什么）见 §1.4；与避障深度模型的分工见《双目深度估计思路》§1.3。 + +### 8.2 已知缺点与局限（如实声明，不得因文档目的删减） + +| # | 缺点 | 根因 | 影响 | 现行对策 / 接受理由 | +|---|------|------|------|---------------------| +| 1 | **误差随距离平方增长** | `δZ ∝ Z²`（§7.1） | 远距离距离值不可用 | 有效域判据硬性拦截（§6.4）；域外交给定速接近 | +| 2 | **远场空洞** | 有效域 >15~20m 外无任何机载距离源 | 远场无法距离闭环 | 消费侧 D4 策略（方位闭环 + 定速接近）；扩域唯一杠杆是换机型（P6） | +| 3 | **近场盲区** | 视差超 `D_max`、框占满画面、边缘取点失效 | 反制逼近的最后阶段恰是最需要距离时失效 | 消费侧判丢刹车/定速接近兜底；`Z_min` 如实设界 | +| 4 | **检测框是单点依赖** | 全部几何量从框导出 | 无检测即无距离也无方位；框抖动直接进入 δD | 检测置信度门槛在检测侧；追踪库 Coast 补帧兜底 | +| 5 | **框级精度上限** | 取点在框边缘/中心，非特征级匹配 | 精度天花板高于特征级方案 | 接受（几何换算换实时性，§10.1）；v1.1 亚像素精化 | +| 6 | **小视差病态** | 远距 D → 1px，量化误差主导 | 远场 δD=1px 假设过于乐观 | 同上由有效域拦截 | +| 7 | **标定与校正依赖** | y 对齐、无畸变是解算前提 | 未标定则系统性偏差，甚至全盘不成立 | P2 出厂标定为 v1.0 首要前置；不达标则 v1.1 自标定提前 | +| 8 | **帧不同步敏感** | 非同瞬曝光 → 运动目标虚假视差 | 快速横移目标距离失真 | 同帧契约（§2.1）+ 13 章快速横移验证；同步形态待 P3 | +| 9 | **固定基线/焦距** | 单一几何配置 | 近盲远废的区间固定 | 参数化配置；换机型只改配置（红线 2） | +| 10 | **贪心匹配局限** | O(n²) 局部最优 | 目标多/邻近/遮挡时错配风险 | 目标数极少（1~2）可接受；匹配器接口注入保留替换空间 | +| 11 | **无飞行中绝对基准** | 激光移除后无外部真值（D2/D8） | 一致性监控只能降级、不能纠偏 | 绝对精度靠地面手持激光测距仪静态验证（§12.1） | +| 12 | **单目退化有瞄偏** | 单目光轴偏离模组中线（§6.6） | 仅方位记录时方位存在 0.5° 量级近场偏差 | v1.0 接受并如实标注退化；v1.1 定值修正 | + +**阅读提示**：1/2/3/6/9 是几何固有，只能拦截或换硬件，不能修好； +4/5/8/10/12 是工程折中，有 v1.1/v2.0 改进路径；7/11 是流程前提， +靠标定与地面验证闭环。 + +### 8.3 缺点对策索引 + +| 路径 | 覆盖的缺点 | +|------|-----------| +| v1.1（算法） | 亚像素边缘（5）、真值拟合误差修正项（1/6）、track_id 辅助匹配（10）、单目定值修正（12）、自标定（7，视 P2 结论提前） | +| v2.0（远期） | 实例分割替代框（4/5）、端到端立体评估（5）、更大基线/长焦机型（1/2/3/6/9，P6） | +| 消费侧兜底 | 定速接近 + 判丢刹车 + exit 门限（2/3 的安全网） | +| 地面验证 | 7/8/11 的唯一闭环手段 | + +--- + +## 9. 配置参数全量表 + +全部参数**配置外置、启动期一次加载并校验**：缺项、类型错误、非法取值 +一律拒绝进入运行期（fail-fast，语义要求，不绑定具体配置格式）。 +所有数值均为占位（⚠），整定后回填。 + +### 9.1 几何组（来自标定，P1/P2） + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `baseline_m` | m | 0.06 | 基线 B；更换机型只改此项 | +| `fx_px` / `fy_px` | px | 960.0（⚠占位，待 P1） | 像素焦距（校正后 `fx = fy = F`） | +| `cx_px` / `cy_px` | px | 960.0 / 540.0（⚠占位，待 P2） | 主点 | + +### 9.2 匹配组 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `tau_y_px` | px | 2.0 | \|Δy\| 硬约束（同时是校正质量的运行期体现） | +| `tau_h_ratio` | — | 0.25 | 框高相对差硬约束 | +| `disparity_min_px` | px | 3.0 | 最小可测视差（远界）；**域唯一真源**（§6.4） | +| `disparity_max_px` | px | 57.6 | 最大可测视差（近界）；⚠占位：按 F=960/B=0.06 反推 ⇔ 1.0 m | +| `distance_min_m` / `distance_max_m` | m | **不再配置** | v1.0 冻结：距离域由 `Z = F·B/D` 从视差域派生为只读量（§6.4） | + +### 9.3 有效性组 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `max_delta_z_m` | m | 2.0 | 误差预算 `δZ(δD=1px)` 上限（δD 基准固定 1px，§6.4） | +| `consistency_N_frames` | 帧 | 3 | 连续跳变降置信度的帧数 | +| `consistency_M_frames` | 帧 | 5 | 连续异常判 invalid 的帧数 | +| `consistency_jump_px` | px | 3.0 | 单帧视差跳变门限（§6.7）；**v1.0 补充项**（原文缺项） | + +### 9.4 滤波组 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `smooth_alpha` | — | 0.3 | 一阶低通系数（§6.5 定义，1 = 不滤波） | +| `jump_reset_m` | m | 5.0 | 单帧跳变直接重置的门限 | +| `filter_reset_frames` | 帧 | 5（⚠独立配置，不复用 M） | 连续失效多久后清空滤波状态（§6.5） | + +### 9.5 目标隔离组 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `isolation_gate_px` | px | 50.0 | 最近邻兜底的匹配门限（§4）；**v1.0 补充项**（原文缺项） | + +--- + +## 10. 设计取舍记录（为什么不是别的做法） + +### 10.1 两次目标检测代替稠密像素匹配 + +双目深度最难的是逐像素匹配：稠密立体匹配（SGBM 类）在该分辨率与 +嵌入式 CPU 预算下不可行，而系统需求只是**少量目标的距离**。用左右 +各一次轻量检测 + 框级几何换算绕开逐像素匹配，以"检测框天然给出 ROI" +换"精度天花板"（§8 第 5 条）。检测算力实测单核 20~30ms，NPU 预算 +成立（平台侧见《双目深度估计思路》第 9 章）。 + +### 10.2 不先上端到端深度网络 + +需采集标注数据集、可解释性差、误差行为不可解析；先以几何法建立 +**可验证的真值基线**，端到端立体/深度网络留 v2.0 评估。 + +### 10.3 贪心匹配而非匈牙利 + +目标数 1~2 个，贪心与全局最优几乎必然同解；O(n²) 足够。匹配器做 +**接口注入**，保留替换为匈牙利/带连续性先验算法的空间而不动调用方。 + +### 10.4 同侧边缘而非框中心取视差 + +框宽差直接污染中心视差；同侧边缘可抵消框宽项（§6.1，原稿缺陷 2 的修正）。 + +### 10.5 距离通道用 Z 不用 R + +与机头前向期望射程语义对齐，也与地面真值仪测量方向一致（决策 D8）； +R 仅作参考输出。 + +### 10.6 方位源取两目框中心等权平均（虚拟中心像素） + +两目均偏于模组中线 ±B/2，单目存在近场视差偏差（3m 处约 0.57°）； +等权平均是投影几何**精确解**（§6.6，原稿"offset 加权"构想的正确形式， +决策 D5）。注意与视差取点（同侧边缘）分工不同、不可混用。 + +### 10.7 双目为机载唯一距离源、域外定速接近 + +替代激光（b96111e / D2）；远场误差平方发散，强行估计不可靠——以 +"诚实 invalid + 持续方位"支撑消费侧的方位闭环 + 定速接近（D4）， +而不是在算法侧掩盖远场空洞。 + +--- + +## 11. 实现与重写指引 + +### 11.1 必须持久化的状态（漏一个就变行为） + +| 状态 | 所属 | 初值 | 何时改变 | +|------|------|------|---------| +| 上次视差 + 连续异常计数 ×目标 | 一致性监控 | 无 / 0 | 配对帧写入上次视差；异常 +1 / 正常清零 | +| 上次滤波值 ×目标 | 时间滤波器 | 无（未初始化） | 每有效帧写入 | +| 滤波初始化标志 ×目标 | 时间滤波器 | false | 首个有效帧置 true；清空时回 false | +| 连续失效计数 ×目标 | 时间滤波器 | 0 | 每无效/缺席帧 +1 / 有效帧清零 | +| 最近框中心、最近出现帧 ×目标 | 目标状态容器 | 首帧写入 | 每次解析隔离键时刷新；连续 `filter_reset_frames` 帧未出现则整体清理 | +| 帧序号 | 流程编排 | 0 | 每帧 +1（被拒收的违约帧不加） | +| 告警节流状态 ×异常类 | 节流器 | 首见未打印 + 计数 0 | 每次异常更新 | + +构造期预计算量：针孔系数与常量（如 `F·B`）一次算好，热路径只做乘除。 + +### 11.2 数值与精度注意 + +| 事项 | 处理方式 | 理由 | +|------|---------|------| +| 除零 | `D ≤ 0` 先判违例再除 | 正视差是物理前提（§6.3） | +| 视差取整 | 左右目规则必须一致 | 不一致会引入系统性 δD | +| 域判定基准 | 用 `D` 判域而非 `Z` | 直接观测量，避免往返舍入（§6.4） | +| 域配置单一来源 | 只配视差域，距离域派生 | 两处冗余存储是互推舍入不一致的根源（§6.4/§9.2） | +| 误差预算基准 | δD 固定 1px（⚠占位），不做在线估计 | 真实 δD 留待地面真值拟合（§12.2） | +| 浮点比较 | 阈值比较用 `≥/≤` 含边界语义 | 边界帧的归类必须确定 | + +### 11.3 并发与生命周期 + +- 本模块是**纯计算单元**：驱动方式（独立线程 / 订阅回调 / 同步调用） + 由平台层决定，本文只约定数据契约； +- 同一实例**不得并发调用**单周期处理（含滤波/一致性状态的无锁前提）； +- 输出记录为**值拷贝**语义：多消费者各自持有快照，不得共享可变状态； +- 单周期处理应当无阻塞、无 IO（日志走节流通道，不落在数学热路径上）。 + +### 11.4 可测试性设计（构件边界即测试边界） + +| 构件 | 是否纯函数 | 测试要点 | +|------|-----------|---------| +| 取点器 | 是 | 同侧边缘取值；框宽差不进入视差（构造两目等框不同宽断言 D 相同）；越窗回退中心；边界含等号 | +| 匹配器 | 是 | 硬约束拦截各一例；贪心排序；一一配对；cost ≥ 1 拒绝 | +| 测距计算 | 是 | 已知 Z 反推 D 构造框对，断言 `Z = F·B/D` 回收；`D ≤ 0` 违例；`c_mid` 等权平均精确性（§6.6 恒等式）；`R` 与发布 Z 同基准（`R ≥ Z`） | +| 有效性判定 | 是 | 三道门各一例；边界帧（恰在域界）归类确定；预算门通过/拦截两侧 | +| 时间滤波器 | 否 | 低通收敛；跳变重置不拖尾；失效清理 | +| 一致性监控 | 否 | N 帧降级、M 帧判 invalid、正常帧清零 | +| 目标隔离 | 否 | track_id 一致跨大位移保持状态；最近邻越门限新建；老化清理 | +| 全流程 | 否 | 合成帧对端到端：单目标/多目标/单目退化/非同帧拒收/记录顺序/非有限框/reset | + +**无需真实图像**：全部测试用合成框对驱动，几何是解析可断言的。参考实现 +的构件级用例已全部通过（§12.1）。 + +--- + +## 12. 验证路线与待定项 + +### 12.1 验证项 + +| 项 | 方法 | 通过标准 | +|----|------|----------| +| 构件级 | §11.4 合成框对单测 | 全部通过（无硬件依赖）；Python 参考实现 `utils/stereo_ranger/` 用例已跑通 | +| 校正质量 | 静态场景统计匹配框 \|Δy\| 分布 | P95 ≤ 1px（⚠占位）；不达标触发自标定提前 | +| 测距精度 | 5/10/15/20m 静态标靶，以**地面手持激光测距仪**为真值（非机载，D8），记录误差曲线 | 趋势与 §7 一致；有效域内 δZ ≤ 2m（⚠占位） | +| 帧同步 | 目标横向快速移动，观察视差时序抖动 | 无系统性虚假视差 | +| 控制联动 | 回放/SITL：距离闭环 ↔ 定速接近切换、invalid 持续降级路径 | 《视觉跟踪控制律技术路线》第 7 章场景全覆盖 | +| 日志纪律 | 检查异常路径日志 | 节流规范（第 1 次 + 每 100 次，带累计计数） | + +### 12.2 待定项 + +开放问题（P1 焦距规格、P2 出厂标定、P3 帧同步形态、P6 换机型、 +P7 性能实测、P8 避障激光去留）以《双目深度估计思路》第 15 章为权威 +清单，本文不另立编号；其中： + +- **P1/P2 直接决定本文 §7 结论与 §9 几何组能否从占位变实值**； +- **v1.0 实现补充的占位量**：`consistency_jump_px`（3px）、 + `isolation_gate_px`（50px）、置信度降级系数（0.5）、误差预算基准 δD + （固定 1px）——随 P1/P2 回填与实测一并整定（§9.3/§9.5）； +- **P6 是扩大有效域的唯一杠杆**（§7.2）； +- P8（避障单点激光去留）不影响本链路——目标测距职责已由本模块承担。 + +### 12.3 红线 + +1. **占位数值不得用于飞行决策**：§7/§9 中所有 ⚠ 数值未经厂商数据与 + 整定确认前，本模块输出仅限静态标靶/回放/台架验证； +2. **相机参数与阈值一律配置外置**，禁止写死（后续可能更换机型）； +3. **域外/失配必须输出 `valid = false`**，不得强行估计距离（远场策略 + 的正确性建立在有效性不撒谎之上）； +4. **标定与地面真值验证完成前不得接入实飞控制回路**（与消费侧控制律 + 红线一致，见《视觉跟踪控制律技术路线》§12.3）。 diff --git a/docs/双目测距方案.md b/docs/双目测距方案.md deleted file mode 100644 index 5ace1fb..0000000 --- a/docs/双目测距方案.md +++ /dev/null @@ -1,432 +0,0 @@ -# 双目目标测距技术规格（基于对偶检测） - -> **范围**：平台无关、语言无关的技术规格。通过立体相机对与目标检测器估计被检测目标的距离（及可选三维位置），不依赖稠密立体匹配或端到端深度网络。 -> -> **版本**：1.0（由项目专用路线图抽取并泛化而成）。 - ---- - -## 1. 目的与定位 - -### 1.1 动机 - -单目目标检测只能给出像素位置，无法提供度量距离。立体相机对提供几何约束，可将对应检测结果转换为距离。该方法适用于： - -- 只需关心少量离散目标（而非稠密环境地图）； -- 稠密立体匹配代价过高或不必要； -- 已有互补测距传感器（如激光雷达），双目估计可作为次要或近场来源。 - -### 1.2 一句话概述 - -左右图像各自独立运行轻量目标检测器；从对应检测框的边缘（或中心）提取视差；通过经典立体公式 \(Z = F \cdot B / D\) 将视差转换为度量距离。不生成稠密对应场或深度图。 - -### 1.3 能力边界 - -- 仅对被检测器报告的目标（如特定类别）测量距离； -- 有效工作距离由基线、焦距与可接受误差决定（见第 5 章）； -- 不输出稠密深度、实例掩码或目标以外的场景几何； -- 不替代主远程测距传感器，定位为近中距离的互补来源。 - ---- - -## 2. 硬件与输入假设 - -所有数值参数均为配置项，禁止硬编码。 - -| 项目 | 典型/示例值 | 说明 | -|------|-------------|------| -| 基线 \(B\) | 0.06 m（示例） | 可配置；误差与 \(1/B\) 成正比 | -| 图像分辨率 | 如 1920×1080 | 检测流水线支持的任意分辨率 | -| 帧率 | 如 30 fps | 流水线须满足所选帧率的周期 | -| 焦距 / 视场 / 传感器尺寸 | — | 须由标定或厂商数据获得 | -| 内参与外参 | — | 立体校正所必需（见第 3 章） | -| 同步方式 | 优先硬件同步 | 软件同步可行，但须量化残余误差 | -| 检测器输入尺寸 | 如 640×640 | 左右目的 letter-box / 缩放变换必须完全一致 | - ---- - -## 3. 前置条件：标定、立体校正与帧同步 - -### 3.1 标定与立体校正 - -本方法假设图像对已完成立体校正，使得： - -- 对应极线水平且对齐（同一物理点在两图中 \(y\) 坐标相同）； -- 视差纯为水平分量：\(D = x_L - x_R\)。 - -所需数据： - -1. 左右相机各自的内参（\(f_x, f_y, c_x, c_y\)）与畸变系数； -2. 两相机之间的相对旋转与平移； -3. 应用于每一帧的校正映射（或等效处理）。 - -**验证方法**：校正后收集匹配检测框，统计垂直残差 \(|\Delta y|\)。实用接受准则为 P95 \(\le 1\) px（实测后确定）。若残差过大，须进行离线自标定（如棋盘格）并在线重映射。 - -### 3.2 帧同步 - -若两路曝光时刻不一致，运动目标会产生虚假视差。 - -| 采集形态 | 同步质量 | 建议处理方式 | -|----------|----------|--------------| -| 同帧拼接单流（side-by-side） | 硬件保证同曝光 | 解码一次，裁切为左右图 | -| 双独立流 | 取决于硬件触发或软件时间戳 | 按序号 + 接收时间窗配对；监测残余时序抖动 | - -无硬件同步时，须在预期目标速度下量化距离误差，必要时限制仅用于低速场景。 - -### 3.3 时间戳约定 - -优先使用处理平台上的单调接收时间戳。不得伪造传输层未提供的采集时间戳。 - ---- - -## 4. 数学模型 - -### 4.1 符号约定 - -| 符号 | 含义 | 单位 | -|------|------|------| -| \(B\) | 基线（左右光心间距） | m | -| \(F\) | 像素焦距（校正后 \(f_x = f_y\)） | px | -| \(c_x, c_y\) | 主点 | px | -| \(x_L, y_L\) | 特征点在**左目（参考目）**图像中的像素坐标 | px | -| \(x_R\) | 同一特征点在右目图像中的像素坐标 | px | -| \(D\) | 视差 \(= x_L - x_R\) | px | -| \(Z\) | 前向距离（沿光轴分量） | m | -| \(X, Y\) | 左目坐标系下的横向 / 纵向偏移 | m | -| \(R\) | 斜距 \(\sqrt{X^2+Y^2+Z^2}\) | m | - -约定：校正后以左目为参考系；目标位于相机前方时视差为正。 - -### 4.2 核心公式（校正后坐标系） - -\[ -\begin{align*} -D &= x_L - x_R \\ -Z &= \frac{F \cdot B}{D} \\ -X &= \frac{(x_L - c_x) \cdot Z}{F} \\ -Y &= \frac{(y_L - c_y) \cdot Z}{F} \\ -R &= \sqrt{X^2 + Y^2 + Z^2} -\end{align*} -\] - -- 当主测距传感器同样报告前向距离时，优先使用 \(Z\)（前向分量）；仅在需要真实欧氏距离时使用 \(R\)。 -- 小角度下 \(Z \approx R\)；大偏航角时二者差异增大。 - -### 4.3 检测器预处理后的坐标映射 - -检测器通常接收固定尺寸的 letter-box 或缩放图像。必须遵守两条约束： - -1. **左右图像使用完全相同的几何变换**（同缩放、同填充）。否则垂直对齐性质被破坏。 -2. **视差在原始图像坐标系下计算**。将检测框映射回原生分辨率后再计算 \(D\)，可保留传感器的全部像素精度。 - -内参 \(F, c_x, c_y\) 同样须对应原生分辨率。 - -### 4.4 有损压缩的影响 - -帧内压缩（如 MJPEG）会在框边缘引入定位噪声。其对视差不确定度 \(\delta D\) 的贡献应计入第 5.2 节误差预算。若实测影响显著，可提高码流质量档位，或在带宽允许时改用无压缩 / 轻压缩格式。 - ---- - -## 5. 视差取点与误差分析 - -### 5.1 对应点选择 - -同一目标在两目中的检测框宽度很少严格相等（检测抖动、外观差异）。使用框中心会使宽度差的一半混入视差。 - -| 方法 | 主要误差来源 | 建议 | -|------|--------------|------| -| 框中心 | 宽度差直接进入视差 | 对照 / 回退方案 | -| **同侧边缘**（如两框左边缘） | 仅边缘定位噪声 | **主方案** | -| 多边缘 + 亚像素精化 | 实现成本更高 | 可选后续精化 | - -实用取法： - -\[ -D = x_L^{\text{左边缘}} - x_R^{\text{左边缘}} -\] - -（若检测器直接给出左上角坐标，该值可直接使用。）当边缘测量被判定不可靠时，回退到中心视差并输出诊断警告（需节流）。 - -### 5.2 误差传播 - -由 \(D = F B / Z\) 得： - -\[ -\begin{align*} -\delta Z &\approx \frac{Z^2 \cdot \delta D}{F B} && \text{（绝对误差）} \\ -\frac{\delta Z}{Z} &\approx \frac{Z \cdot \delta D}{F B} && \text{（相对误差）} -\end{align*} -\] - -\(\delta D\) 的构成： - -- 检测框边缘定位噪声； -- 校正后残余垂直失配； -- 运动目标的时序不同步； -- 压缩伪影。 - -基线与焦距的标定误差线性传递，完成出厂或自标定后通常可忽略。 - -### 5.3 示例误差表 - -下表仅为**示意**。假设 \(B = 0.06\,\text{m}\)、\(\delta D = 1\,\text{px}\)、原生宽度 1920 px。给出两种示例水平视场；真实焦距须由标定数据代入。 - -像素焦距近似：\(F = (W/2) / \tan(\text{HFOV}/2)\)。 - -| 距离 \(Z\) | \(D\) @ \(F\approx960\,\text{px}\)（HFOV \(\approx90^\circ\)） | \(D\) @ \(F\approx1663\,\text{px}\)（HFOV \(\approx60^\circ\)） | \(\delta Z\) @ 90° | \(\delta Z\) @ 60° | -|-----------:|----------------------------------------------------------:|------------------------------------------------------------:|-------------------:|-------------------:| -| 5 m | 11.5 px | 20.0 px | 0.43 m | 0.25 m | -| 10 m | 5.8 px | 10.0 px | 1.7 m | 1.0 m | -| 15 m | 3.8 px | 6.7 px | 3.9 m | 2.3 m | -| 20 m | 2.9 px | 5.0 px | 6.9 m | 4.0 m | -| 30 m | 1.9 px | 3.3 px | 15.6 m | 9.0 m | -| 50 m | 1.2 px | 2.0 px | 43 m | 25 m | -| 100 m| 0.6 px | 1.0 px | 174 m | 100 m | - -\(\delta D\) 加倍则表中所有误差加倍。取得真实焦距后必须重算本表。 - -### 5.4 有效工作距离 - -实用有效性判据（可调）： - -\[ -\text{valid} \;\Leftrightarrow\; D \ge D_{\min}\;\text{（如 3 px）} \;\land\; \delta Z(\delta D=1\,\text{px}) \le Z_{\text{tol}}\;\text{（如 2 m）}. -\] - -按上表示例，广角镜头有效域约 \(\le 15\,\text{m}\)，较窄视场约 \(\le 20\,\text{m}\)。域外估计标记为无效；方位信息仍可发布，距离由其他传感器承担。 - -敏感性：误差 \(\propto 1/B\)。基线加倍可使误差减半、有效域约扩大一倍——这是提升远距离能力最直接的杠杆。 - -### 5.5 远距离扩展（最小实现范围之外） - -- 亚像素边缘精化（灰度重心、抛物线拟合等）； -- 距离估计的时间滤波； -- 更大基线或更长焦距； -- 相对真值传感器拟合的经验修正项； -- 优雅降级为“仅方位 + 外部距离”。 - ---- - -## 6. 左右目关联 - -**输入**：属于同一逻辑帧的两路检测结果（见第 7 章）。 - -**硬约束**（任一不满足即拒绝配对）： - -- 类别标识一致； -- 垂直残差 \(|\Delta y| \le \tau_y\)（校正后，如 2 px）； -- 视差落在目标距离上下限对应的区间内； -- 相对高度相近：\(|h_L - h_R| / \max(h_L,h_R) \le \tau_h\)（如 0.25）。 - -**代价与贪心匹配**： - -\[ -\text{cost} = \frac{|\Delta y|}{\tau_y} + \frac{|\Delta h|}{\tau_h\cdot\max(h_L,h_R)}. -\] - -按代价升序排列候选对；仅当 \(\text{cost} < 1\) 时接受；强制一一配对。 - -多数跟踪场景目标数极少（一两个），\(O(n^2)\) 贪心算法足够。匹配器应通过接口注入，以便后续可替换为匈牙利算法、网络流等而不改动调用方。 - -**失败处理**：未匹配的检测本帧不产生距离，方位仍可发布；流水线其余部分继续运行。 - -**可选连续性线索**：若存在轨迹标识，优先匹配相同 track id 的框。 - ---- - -## 7. 单周期数据流水线 - -目标周期为所选帧率的倒数（如 30 fps 时 \(\le 33\,\text{ms}\)）。 - -``` -采集（优先硬件同步的立体对） - → 解码 / 去马赛克 / 色彩转换 - → 几何预处理（校正映射、左右完全一致的 letter-box / 缩放） - → 左右图像并行推理 - → 检测框映射回原生分辨率 - → 关联（第 6 章） - → 视差 → (Z, X, Y, R) - → 有效性判定（第 5.4 节） - → 可选时间滤波 - → 发布结果 -``` - -- **配对键**：共用帧序号（或序号不可用时采用紧接收时间窗）。 -- **时间滤波（轻量）**：对 \(Z\) 做一阶低通，配合跳变重置门限。跨帧关联可用类别 + 最近邻连续性；完整多目标跟踪为可选项。 -- **失败路径**：任一阶段失败（解码、推理、关联）导致丢帧或发布 `valid = false` 消息。失败日志须节流。 - ---- - -## 8. 图像通路考虑 - -具体编解码与加速硬件因平台而异，此处仅陈述逻辑要求。 - -- 优先从相机缓冲到校正、letter-box 直至检测器输入张量的零拷贝通路。 -- 校正与 letter-box 对左右两目必须位级一致。 -- 若采集格式为有损，须量化其对边缘噪声的贡献（见第 4.4 节）。 - ---- - -## 9. 性能预算 - -具体数值取决于所选检测器、加速器与图像尺寸。关键架构要求是： - -> 左右推理**必须并发执行**。串行执行通常使时延翻倍，可能无法满足帧周期预算。 - -代表性预算骨架（用实测值填充）： - -| 阶段 | 预算（示意） | -|------|--------------| -| 采集周期 | 1 / 帧率 | -| 解码 + 预处理 | 待实测 | -| 左目推理 | 单设备 / 单核上的检测器时延 | -| 右目推理 | 与左目并发 | -| 关联 + 测距 | 少量目标时 < 1 ms | -| 端到端时延 | 实测；须为控制或融合消费方留有余量 | - ---- - -## 10. 模块分解（语言无关） - -以下逻辑模块将算法、I/O 与配置清晰分离。 - -| 模块 | 职责 | -|------|------| -| StereoRanger | 纯算法：关联、视差提取、距离计算、有效性、可选滤波。接收检测结果 + 相机参数，输出距离消息。可完全单元测试。 | -| 检测前端 | 两个独立检测实例（或同一实例调用两次），输出原生坐标下的框。 | -| 图像采集与预处理 | 采集、解码、校正、左右完全一致的几何变换。 | -| 配置 | 基线、内参、视差上下限、关联阈值、滤波系数——全部外置。 | - -建议的结果记录（字段名仅为示意）： - -``` -header // 时间戳、序号、健康状态 -frame_sequence -class_id -track_id // 未知时为 0 -forward_distance_m // Z -slant_range_m // R（可选） -disparity_px -confidence // [0,1] -valid -centre_pixel_x_mid // 虚拟双目中心（见第 11.2 节） -bbox_* // 参考目框，供诊断 -``` - ---- - -## 11. 传感器融合与控制接口 - -### 11.1 距离源优先级（示例策略） - -``` -主传感器有效 → 使用主传感器 -主传感器无效 / 超龄， - 双目有效 → 使用双目 -双目超出有效距离域 → 标记无效 -两者均不可用 → “无距离”降级模式 -``` - -### 11.2 交叉校验 - -两者同时有效且 \(|Z_{\text{stereo}} - d_{\text{primary}}|\) 超过相对或绝对阈值时，记录节流警告并以主传感器为准。注意 \(Z\) 为前向分量；大偏航角时与纯径向测量存在差异。 - -### 11.3 方位：虚拟双目中心 - -每枚物理相机相对模组中线偏移 \(\pm B/2\)。直接使用单目像素偏移会引入恒定角度偏差 - -\[ -\beta_{\text{bias}} \approx \arctan\bigl((B/2)/Z\bigr) -\] - -近距离时不可忽略（如 \(B=60\,\text{mm}\) 时 3 m 处约 0.6°）。 - -校正后，位于基线中点的**虚拟相机**对同一目标的投影恰为两目像素的平均值： - -\[ -\begin{align*} -x_{\text{mid}} &= \frac{x_L + x_R}{2}, \\ -y_{\text{mid}} &= y_L = y_R. -\end{align*} -\] - -因此： - -- 存在立体匹配时，发布 \(x_{\text{mid}}\)（及 \(y_{\text{mid}}\)）作为方位源； -- 仅单目检出时，回退到该目原始坐标（接受偏差），或在存在最近有效距离时施加单侧修正 \(\Delta x = F\cdot(B/2)/Z\)。 - -模组中线与机体中轴之间的固定安装偏移属于外参标定问题，与双目几何无关。 - -### 11.4 互补工作域 - -双目测距通常在近中距离最准确，而窄波束主传感器在此区间可能丢靶；主传感器在远距离（双目视差坍缩处）保持优势。简单的优先级或融合规则即可实现全程覆盖。 - ---- - -## 12. 建议实施阶段 - -| 阶段 | 内容 | 出口条件 | -|------|------|----------| -| **v1.0** | 同步对采集、一致预处理、双路检测、同侧边缘视差、贪心关联、\(Z=F B/D\)、有效性标志、结果发布、可选与主测距传感器融合 | 标定数据可用；实测误差与第 5 章模型一致；基本时延预算满足 | -| **v1.1** | 出厂校正不足时的自标定 / 重映射、亚像素边缘精化、时间滤波、track-id 关联、相对真值的经验偏差修正 | 具备 v1.0 外场数据 | -| **v2.0**（可选） | 用实例分割掩码替代检测框、评估学习式立体或单目深度网络、更大基线或更长焦距硬件、框内局部稠密精化 | 单独可行性研究 | - ---- - -## 13. 验证清单 - -| 项目 | 方法 | 通过标准（示意） | -|------|------|------------------| -| 校正质量 | 静态场景，匹配框 \(|\Delta y|\) 直方图 | P95 \(\le 1\) px（实测后调整） | -| 测距精度 | 多个已知距离的静态标靶 + 真值传感器 | 误差趋势与第 5.3 节一致；有效域内 \(\delta Z\) 在容限内 | -| 同步性 | 目标快速横向运动 | 无系统性虚假视差 | -| 时延与吞吐 | 并发负载下的端到端计时 | 满足所选帧率预算 | -| 融合 / 降级 | 主传感器丢失、超龄、双目超距的回放或仿真 | 所有降级路径行为符合规格 | -| 日志纪律 | 注入失败 | 仅输出节流后的诊断信息 | - ---- - -## 14. 设计取舍记录 - -| 决策 | 理由 | -|------|------| -| 不做稠密立体匹配 | 稀疏目标无需；全分辨率对应计算代价高 | -| 几何公式而非学习深度 | 可解释、无需额外训练数据，并为后续网络评估提供干净真值基线 | -| 贪心关联 | 目标基数极小；接口保留后续升级空间 | -| 前向分量 \(Z\) 作为主距离输出 | 与多数单点测距传感器语义对齐 | -| 虚拟双目中心作为方位源 | 消除物理相机偏移引起的恒定近场角度偏差 | -| 同侧边缘视差 | 抵消两目框宽差 | -| 双目作为次要 / 互补源 | 绝对误差随 \(Z^2\) 增长；方法在主传感器最弱处价值最大 | - ---- - -## 15. 开放问题 / 配置检查清单 - -具体部署前须用真实数值填补以下项： - -1. 焦距（或完整内参）与畸变模型。 -2. 出厂立体标定是否可用及其格式；否则规划自标定。 -3. 采集形态（同帧拼接单流 vs 双流）与同步方式。 -4. 有损压缩对边缘噪声的实测贡献。 -5. 目标计算设备上所选检测器的实测时延（并确认左右推理可并发）。 -6. 可接受的绝对 / 相对距离误差及由此确定的有效工作包络。 -7. 未来硬件是否考虑更大基线或不同光学配置。 - ---- - -## 附录 A – 公式速查 - -\[ -\begin{align*} -D &= x_L - x_R \\ -Z &= \frac{F B}{D} \\ -\delta Z &\approx \frac{Z^2\,\delta D}{F B} \\ -x_{\text{mid}} &= \frac{x_L + x_R}{2} -\end{align*} -\] - -有效性（示例）：\(D \ge 3\,\text{px}\) 且 \(\delta Z(\delta D=1\,\text{px}) \le 2\,\text{m}\)。 - ---- - -*文档结束。* diff --git a/docs/视觉跟踪控制律技术路线.md b/docs/视觉跟踪控制律技术路线.md index 3307769..29af140 100644 --- a/docs/视觉跟踪控制律技术路线.md +++ b/docs/视觉跟踪控制律技术路线.md @@ -47,7 +47,7 @@ 1. **三路控制律计算**（每拍一次，固定周期） - **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令； - **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令； - - **距离**：雷达目标距离与期望射程之差 Δd → PID → 前向速度指令。 + - **距离**：双目目标距离与期望射程之差 Δd → PID → 前向速度指令。 2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许 跳过限幅直达链路（含切换后的**首条**指令）。 3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续 @@ -63,8 +63,8 @@ │ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│ └─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘ │ -┌─────────────┐ 前向距离 │ 控制意图 -│ 激光测距雷达│──────────────────────┐ ▼ +┌─────────────┐ 目标距离 │ 控制意图 +│ 双目立体测距│──────────────────────┐ ▼ └─────────────┘ │ ┌──────────────────────┐ ┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │ │ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │ @@ -78,8 +78,15 @@ ``` **上游依赖**：追踪库提供主目标中心与 `tracked` / `is_predicted` 两个行为标志 -（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；雷达提供机头前向单点距离与 -**关联标志**；飞控链路提供自机姿态（至少航向角）。 +（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；双目模块提供目标距离 +（检测框自身的立体测距，前向分量 Z）与有效标志 `distance_valid`；飞控链路 +提供自机姿态（至少航向角）。 + +**方位源（接入层约定）**：双目模组两枚相机分列模组中线两侧（各偏 ±B/2）， +单目像素存在近场视差偏差（3m 处约 0.57°）。匹配成功时接入层应以**虚拟双目 +中心**像素 `x_mid = (xL + xR) / 2` 填充 `track.center`（推导见 +`docs/双目深度估计思路.md` §11.3）。虚拟中心相机与单目共用同一 FOV 配置， +本模块"像素偏移 → 视线角"逻辑与配置**零改动**。 **下游依赖**：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。 @@ -100,8 +107,9 @@ ### 1.4 明确不做的事（边界） -- **不做感知**：不跑检测/光流/雷达驱动，只消费调用方填入的快照； -- **不做目标关联**：相机—雷达空间关联由感知融合层给出布尔标志，本模块只读取； +- **不做感知**：不跑检测/光流/双目立体匹配，只消费调用方填入的快照； +- **不做距离有效性判定**：`distance_valid` 由双目链路自身的一致性校验给出， + track_id 一致性校验为调用方约定（§12.2），本模块只读取标志； - **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略； - **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息； - **不做配置热更新**：启动期加载一次，调参重启生效； @@ -115,13 +123,13 @@ | 分组 | 字段 | 单位 / 类型 | 语义与约束 | |------|------|------------|-----------| -| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正 | +| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正；双目匹配成功时由接入层填虚拟双目中心 `x_mid`（§1.2） | | 视觉 | `track.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 | | 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 | | 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 | -| 雷达 | `radar_distance_m` | m | 机头前向单点距离；`<= 0` 视为不可用 | -| 雷达 | `associated_to_target` | 布尔 | 相机—雷达关联成立标志；**未关联不得当作目标距离** | -| 雷达 | `radar_time_ms` | 单调时钟 ms | 雷达取样瞬间打戳 | +| 距离 | `target_distance_m` | m | 双目目标距离（前向分量 Z）；`<= 0` 视为不可用 | +| 距离 | `distance_valid` | 布尔 | 本帧测距可用标志：距离来自目标检测框自身的双目测距；**无效帧的距离不得当作目标距离** | +| 距离 | `distance_time_ms` | 单调时钟 ms | 双目距离取样瞬间打戳 | | 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 | | 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 | | 姿态 | `yaw_rad` | rad (NED) | 自机航向；**参与计算**：位置式航向合成 + 输出端坐标旋转 | @@ -189,7 +197,7 @@ | 构件 | 职责 | 构造期预计算 | 运行期持久状态 | 复位入口 | |------|------|-------------|--------------|---------| -| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode` | `Reset()` 清全部 | +| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode`、无距离计时（`exit` 门限用，§7.5） | `Reset()` 清全部 | | 视线角转换器 ×2 | 像素偏移 → β / α | 针孔系数 `2·tan(θ/2)/size` | 无（纯函数） | 不需要 | | 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 | | 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` | @@ -320,6 +328,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量 # ---- ② 判丢 → 零速刹车 ---- 若 非 快照.track.tracked: + 无距离计时.清除() # 刹车期间不计 kExit 时长 若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态 距离PID.全清(); 四个限幅器.复位(0) 输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零 @@ -354,12 +363,13 @@ v_down = v_down # 绕下轴旋转不影响垂直分量 输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps) # ---- ⑦ 距离通道 ---- - 距离可用 ← 快照.associated_to_target - 且 快照.radar_distance_m > 0 - 且 非 已超龄(快照.radar_time_ms, now_ms, 配置.radar_stale_ms) + 距离可用 ← 快照.distance_valid + 且 快照.target_distance_m > 0 + 且 非 已超龄(快照.distance_time_ms, now_ms, 配置.distance_stale_ms) 若 距离可用: - Δd ← 快照.radar_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近 + 无距离计时.清除() # 任一有效帧重置 kExit 计时 + Δd ← 快照.target_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近 前向原始 ← 距离PID.更新(Δd, dt) 限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅 # 非对称限幅：接近可以快，后退必须慢 @@ -370,9 +380,14 @@ v_down = v_down # 绕下轴旋转不影响垂直分量 按 配置.distance.no_distance_action 分支: 保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0) 限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt) - 退出近距操作: 输出.mode ← kDegradedHold - 输出.degraded_reason ← kNoDistanceExit - 返回 收尾并记录模式切换(输出, "距离未关联，退出近距操作") + 退出近距操作: + 若 非 无距离计时.进行中: 无距离计时.启动(now_ms) # 首个无效拍起计时 + 若 now_ms − 无距离计时.起始时刻 ≥ 配置.no_distance_exit_grace_ms: + 输出.mode ← kDegradedHold + 输出.degraded_reason ← kNoDistanceExit + 返回 收尾并记录模式切换(输出, "持续无距离达门限，退出近距操作") + # grace 期内按"限速接近"执行（复用加速度限幅，vx 平滑无跳变） + 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt) # ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ---- 旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad) @@ -391,7 +406,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量 | 姿态超龄降级 | 否 | 否 | 否 | `false` | | 视觉超龄降级 | 否 | 否 | 否 | `false` | | 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` | -| 无距离退出 | 是（仅积分） | 否 | 否 | `false` | +| 无距离退出（grace 期满） | 是（仅积分） | 否 | 否 | `false` | | 正常/Coast | 视情况清积分 | 否 | **是** | `true` | **降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的 @@ -505,9 +520,10 @@ vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限 anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效， 保证 anti-windup 判据基于"真实饱和边界"。 -**可用性三重门**（缺一不可）：关联标志成立 **且** 距离为正 **且** 未超龄。 -"未关联的雷达距离"只是机头前方某个障碍物的距离，把它当作目标距离会导致 -朝着错误目标调整射程。 +**可用性三重门**（缺一不可）：`distance_valid` 成立 **且** 距离为正 **且** 未超龄。 +双目测距取自目标检测框自身，不存在"回波打到别处"的跨模态关联问题；标志为 +`false` 表示本帧测距不可用（有效域外/未匹配/一致性校验失败），不得把任何数值 +当作目标距离。 ### 6.5 PID 内部算法 @@ -601,12 +617,12 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 |------|-------------|------| | 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） | | 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 | -| 雷达距离 | 500 ms | 雷达更新较慢，且距离不可用有独立降级动作 | +| 双目距离 | 500 ms | 双目随推理帧更新，且距离不可用有独立降级动作 | ### 7.2 降级优先级（顺序即语义） ```text -姿态不可用 > 视觉超龄 > 判丢 > 无距离（退出档） +姿态不可用 > 视觉超龄 > 判丢 > 无距离（exit 档，持续达门限） ``` | 优先级 | 检查 | 理由 | @@ -614,7 +630,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 | 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） | | 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 | | 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 | -| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级 | +| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级；`exit` 档还需持续无距离达门限（§7.5），默认档为 `slow_approach`（§10.8） | ### 7.3 判丢刹车与状态清理 @@ -642,19 +658,28 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间 误差不反映真实世界，积分累加会把预测偏差固化成饱和量。 -**Coast 期间距离通常不可用**：目标短暂消失往往伴随雷达关联失效，因此第 ⑦ 步会 -走无距离分支（默认"保持"→ 前向清零）。这是设计上的自然降级，不需要额外分支。 +**Coast 期间距离通常不可用**：目标短暂消失时双目失去匹配（`distance_valid == +false`），第 ⑦ 步走无距离分支；默认 `slow_approach` 下前向按低速定速推进， +方位闭环继续（§7.5）。这是设计上的自然降级，不需要额外分支。 ### 7.5 无距离三档动作 | 配置值 | 行为 | 适用场景 | |--------|------|---------| -| `hold`（默认） | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 | -| `slow_approach` | 以配置的低速限幅缓速接近（仍经加速度限幅） | 需要持续压迫、且低速可接受风险 | -| `exit` | 整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞），无距离必须退出近距操作 | +| `slow_approach`（默认） | 以配置的低速限幅缓速接近（仍经加速度限幅） | **默认档**：双目是机载唯一距离源，有效域外无距离是常态而非异常；即远场策略"方位闭环 + 定速接近"（D4） | +| `hold` | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 | +| `exit` | 持续无距离达 `no_distance_exit_grace_ms` 才整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞）时的可配置安全策略；持续无距离 ≈ 传感器级故障 | -三档中只有 `exit` 会中断整拍输出；`hold` 与 `slow_approach` 仍输出航向与垂直 -通道（继续把目标保持在画面中心）。 +三档中只有 `exit` 会中断整拍输出，且**延迟到 grace 期满**；其余情况（含 `exit` +的 grace 期）仍输出航向与垂直通道（继续把目标保持在画面中心）。 + +**`exit` 门限语义**（替代旧版"首个无效拍即退出"）： + +- `distance_available` 首次为 `false` 起计时，任一有效距离帧**重置**计时； +- grace 期内前向按 `slow_approach` 低速推进，复用既有加速度限幅，`vx` 平滑 + 过渡无跳变； +- 判丢刹车期间不计时（每拍清计时状态），`Reset()` 同样清除； +- `no_distance_exit_grace_ms = 0` 保留旧版行为（首个无效拍即退出）。 --- @@ -773,7 +798,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 |----|------|-------|------| | `frequency_hz` | Hz | 20.0 | 控制频率；同时作为 dt 兜底值 | | `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 | -| `radar_stale_ms` | ms | 500 | 雷达距离超龄阈值 | +| `distance_stale_ms` | ms | 500 | 双目距离超龄阈值 | | `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 | ### 9.2 camera 组：画面与视场 @@ -816,8 +841,9 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 | `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） | | `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 | | `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 | -| `no_distance_action` | 枚举 | `hold` | `hold` / `slow_approach` / `exit` | -| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档的限速值 | +| `no_distance_action` | 枚举 | `slow_approach` | `hold` / `slow_approach` / `exit`（默认档依据见 §10.8） | +| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档（含 `exit` 的 grace 期）的限速值 | +| `no_distance_exit_grace_ms` | ms | 3000 | `exit` 档持续无距离门限（0 = 首个无效拍即退出） | ### 9.6 accel_limit 组：加速度 @@ -889,6 +915,23 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断， 恢复后必须从零渐变。两者语义不同，清理策略必须不同。 +### 10.8 距离输入从雷达切换为双目：语义简化、默认动作与退出门限（2026-09-10） + +**背景**：目标距离的机载唯一来源改为双目立体测距（决策 D2，替代激光雷达； +见 `docs/双目深度估计思路.md`）。 + +1. **关联标志语义简化（S2）**：`associated_to_target`（相机—雷达空间关联） + → `distance_valid`（本帧测距可用）。双目距离来自目标检测框自身，"回波未 + 打到目标"的跨模态关联问题消失，标志退化为纯粹的本帧可用性（域外/未匹配/ + 一致性校验失败）；track_id 一致性校验转为调用方约定（§12.2）。 +2. **默认无距离动作 `hold` → `slow_approach`（D4）**：单雷达时代无距离多为 + 瞬时丢关联，`hold` 占位安全；双目时代有效域外无距离是**常态**，`hold` 会让 + 远场逼近停滞。`slow_approach` 即"方位闭环 + 定速接近"远场策略的库内承载。 +3. **`exit` 增加持续门限（S1）**：旧版首个无效拍即降级，会把单帧丢检误判为 + 传感器级故障；改为持续无距离达 `no_distance_exit_grace_ms` 才退出，grace 期 + 按 `slow_approach` 行为执行（复用加速度限幅，无跳变），任一有效帧重置。 + `exit` 语义收敛为"持续无距离 ≈ 传感器级故障"。 + --- ## 11. 实现与重写指引 @@ -899,6 +942,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 |------|------|------|---------| | 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 | | 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） | +| 无距离计时（进行中 + 起始时刻） | 主控律 | 未计时 | 首个无距离拍启动（`exit` 档）；有效距离帧 / 判丢刹车 / 复位() 清除 | | 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 | | 上拍误差 | PID | 0 | 每拍 | | 低通后微分 | PID | 0 | 每拍 | @@ -917,7 +961,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 控制线程（固定周期，配置频率）: 结果 ← 追踪库.取最新结果() # 线程安全拷贝 - 快照 ← 采集(结果, 雷达, 姿态) + 快照 ← 采集(结果, 双目距离, 姿态) 快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳 意图 ← 控制律.更新(快照, 当前单调时钟()) 按 意图.mode 分发: @@ -926,7 +970,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 降级保持 → 不发送（记录降级原因/告警） 停止视觉闭环（状态机决定）: - 控制律.复位() # 清积分、清限幅器、清节拍与模式历史 + 控制律.复位() # 清积分、清限幅器、清节拍与模式历史、清无距离计时 ``` **约束**： @@ -1003,7 +1047,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 | **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 | | **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 | | **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 | -| **雷达关联判据** | 由感知融合层给出标志，本模块不实现 | 融合层设计 | 中：关联错误 = 距离通道朝错误目标调射程 | +| **track_id 一致性校验** | 调用方约定：装配层校验双目距离与追踪主目标的 `track_id` 一致并告警，库内不实现（S3） | 主程序装配层落地时加一处校验 | 中：错配会把其他目标的距离当作本目标，距离通道朝错误目标调射程 | ### 12.3 红线 @@ -1014,7 +1058,8 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史 3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。 4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅， 包括切换后的首条指令。 -5. **未关联的雷达距离不得当作目标距离**。 +5. **`distance_valid == false` 的距离不得当作目标距离**（v1.0 仅单目检出时 + 不发布距离，方位退化为该目偏移，见 `docs/双目深度估计思路.md` §11.3）。 --- diff --git a/reference-c++/visual_track_control/config/track_control.json b/reference-c++/visual_track_control/config/track_control.json index 7dcd26b..c9d9c8f 100644 --- a/reference-c++/visual_track_control/config/track_control.json +++ b/reference-c++/visual_track_control/config/track_control.json @@ -4,7 +4,7 @@ "control": { "frequency_hz": 20.0, "visual_stale_ms": 200, - "radar_stale_ms": 500, + "distance_stale_ms": 500, "attitude_stale_ms": 300 }, @@ -36,8 +36,9 @@ "derivative_filter_coef": 0.2, "approach_velocity_limit_mps": 3.0, "retreat_velocity_limit_mps": 1.0, - "no_distance_action": "hold", - "no_distance_approach_limit_mps": 0.5 + "no_distance_action": "slow_approach", + "no_distance_approach_limit_mps": 0.5, + "no_distance_exit_grace_ms": 3000 }, "accel_limit": { diff --git a/reference-c++/visual_track_control/examples/track_control_demo.cpp b/reference-c++/visual_track_control/examples/track_control_demo.cpp index 3e1a4b6..fd00eb3 100644 --- a/reference-c++/visual_track_control/examples/track_control_demo.cpp +++ b/reference-c++/visual_track_control/examples/track_control_demo.cpp @@ -179,11 +179,11 @@ int main(int argc, char** argv) { snapshot.track = tracker.LastResult(); snapshot.track_time_ms = now_ms; - // 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径） - snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s; - snapshot.associated_to_target = (tick < normal_ticks) && - snapshot.radar_distance_m > 0.0; - snapshot.radar_time_ms = now_ms; + // 双目距离：正常阶段有效；Coast/刹车阶段置无效（演示 §6.3 降级路径） + snapshot.target_distance_m = 40.0 - 3.0 * elapsed_s; + snapshot.distance_valid = (tick < normal_ticks) && + snapshot.target_distance_m > 0.0; + snapshot.distance_time_ms = now_ms; // 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot） snapshot.attitude_present = true; @@ -209,11 +209,11 @@ int main(int argc, char** argv) { if (tick % 10 == 0) { spdlog::info( "[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} " - "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}", + "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 有效={}", elapsed_s, phase, ModeName(output.mode), output.coast_active ? "(Coast)" : "", snapshot.track.tracked, output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps, - snapshot.radar_distance_m, snapshot.associated_to_target); + snapshot.target_distance_m, snapshot.distance_valid); } std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); diff --git a/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h index 1ccd940..894490d 100644 --- a/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h +++ b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h @@ -6,7 +6,7 @@ // 对应规格：docs/物理追踪思路.md // §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置） // §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并） -// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3） +// §6 距离控制（PID(Δd) → 前向速度；距离无效按配置降级，§6.3） // §6.4 Coast 清积分、判丢零速刹车 // §7.1 超龄判定（姿态/视觉超龄 → 降级保持） // §8 统一限幅 @@ -58,6 +58,11 @@ private: // ---- 节拍与状态（仅打日志用，不参与数学） ---- std::int64_t last_now_ms_ = 0; // 上次计算时刻 ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志） + + // ---- kExit 无距离计时（§6.3：持续无距离达 no_distance_exit_grace_ms + // 才退出，任一有效距离帧重置） ---- + bool no_distance_active_ = false; // 当前是否处于连续无距离状态 + std::int64_t no_distance_since_ms_ = 0; // 连续无距离起始时刻（单调时钟） }; } // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h index 820a24c..df67408 100644 --- a/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h @@ -33,18 +33,19 @@ enum class HeadingMode : std::uint8_t { kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg }; -/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定） +/// 目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3） enum class NoDistanceAction : std::uint8_t { kHold, // 保持：前向速度指令清零（安全占位语义） kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近 - kExit, // 退出近距操作：输出降级保持（kDegradedHold） + kExit, // 退出近距操作：持续无距离达 no_distance_exit_grace_ms 才 + // 降级保持（kDegradedHold）；grace 期间按 kSlowApproach 行为 }; /// control 组：控制周期与各输入超龄阈值（§7） struct ControlGroupConfig { double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定） std::int64_t visual_stale_ms = 200; // 视觉追踪结果超龄阈值（毫秒） - std::int64_t radar_stale_ms = 500; // 雷达距离超龄阈值（毫秒） + std::int64_t distance_stale_ms = 500; // 目标距离超龄阈值（毫秒） std::int64_t attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒） }; @@ -84,8 +85,12 @@ struct DistanceGroupConfig { double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波 double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8） double retreat_velocity_limit_mps = 1.0; // 后退速度限幅（米/秒，§8） - NoDistanceAction no_distance_action = NoDistanceAction::kHold; // 无距离降级（§6.3） + // 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给 + // 持续无距离（传感器级故障）场景 + NoDistanceAction no_distance_action = NoDistanceAction::kSlowApproach; // 无距离降级（§6.3） double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒） + std::int64_t no_distance_exit_grace_ms = 3000; // kExit 门限：持续无距离达 + // 此时长才退出（毫秒，占位） }; /// accel_limit 组：各轴加速度限幅（§8，待定） diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h index a156d75..790f8da 100644 --- a/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h @@ -13,8 +13,8 @@ // - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格）， // 控制律热路径零堆分配； // - 本库不依赖主工程 include/ 的任何类型，独立可编译； -// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作 -// 目标距离（§6.3、需求分析.md FR-041）。 +// - 目标距离必须带 distance_valid 有效标志：无效值不得当作目标距离 +// （§6.3、需求分析.md FR-041）。 // ============================================================================= #include @@ -54,10 +54,11 @@ struct ControlSnapshot { drone::tracker::TrackResult track; // 追踪库结果（值拷贝） std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样 - // ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ---- - double radar_distance_m = 0.0; // 机头前向单点距离（米） - bool associated_to_target = false; // 相机—雷达空间关联成立标志 - std::int64_t radar_time_ms = 0; // 取样单调时钟（毫秒） + // ---- 目标距离（双目立体测距，§6.3：distance_valid == false 时不得 + // 当作目标距离） ---- + double target_distance_m = 0.0; // 目标距离（米，双目前向分量 Z） + bool distance_valid = false; // 距离有效标志（测距来自检测框自身，无跨模态关联） + std::int64_t distance_time_ms = 0; // 取样单调时钟（毫秒） // ---- 自机姿态/速度 ---- // 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转 diff --git a/reference-c++/visual_track_control/src/tracking_control_law.cpp b/reference-c++/visual_track_control/src/tracking_control_law.cpp index c5890c9..fda6544 100644 --- a/reference-c++/visual_track_control/src/tracking_control_law.cpp +++ b/reference-c++/visual_track_control/src/tracking_control_law.cpp @@ -9,7 +9,8 @@ // 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见 // vtc_types.h ControlMode 注释） // §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）； -// 未关联/超龄 → 按配置降级动作（hold / slow_approach / exit） +// 无效/超龄 → 按配置降级动作（hold / slow_approach / exit， +// exit 需持续无距离达 no_distance_exit_grace_ms 门限） // §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车 // §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持 // §8 所有输出统一限幅；首条指令经变化率限幅（S6） @@ -123,6 +124,7 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot, // ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ---- if (!snapshot.track.tracked) { + no_distance_active_ = false; // 刹车期间不计 kExit 无距离时长 if (last_mode_ != ControlMode::kBrakeHover) { // 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅） distance_pid_.Reset(); @@ -181,14 +183,16 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot, } // ---- ⑦ 距离通道（§6.2 / §6.3） ---- - // 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041） + // 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用 + // （§6.3、FR-041） const bool distance_available = - snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 && - !StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms, - cfg_.control.radar_stale_ms); + snapshot.distance_valid && snapshot.target_distance_m > 0.0 && + !StalenessChecker::IsStale(snapshot.distance_time_ms, now_ms, + cfg_.control.distance_stale_ms); if (distance_available) { + no_distance_active_ = false; // 任一有效距离帧重置 kExit 计时 // Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2） - const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m; + const double delta_d = snapshot.target_distance_m - cfg_.distance.d_exp_m; const double vx_raw = distance_pid_.Update(delta_d, dt_s); const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8） out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps, @@ -208,10 +212,24 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot, cfg_.distance.no_distance_approach_limit_mps, dt_s); break; case NoDistanceAction::kExit: - // 退出近距操作：整体降级保持 - out.mode = ControlMode::kDegradedHold; - out.degraded_reason = DegradedReason::kNoDistanceExit; - return FinishAndLog("距离未关联，退出近距操作"); + // 退出近距操作：双目有效域外无距离是常态（D4 远场定速接近）， + // 首个无效拍不退出；持续无距离达 no_distance_exit_grace_ms + // （疑似传感器级故障）才降级。grace 期间按 kSlowApproach 行为 + // 执行（经加速度限幅，vx 平滑过渡）；grace=0 保留旧"立即退出" + // 行为 + if (!no_distance_active_) { + no_distance_active_ = true; + no_distance_since_ms_ = now_ms; + } + if (now_ms - no_distance_since_ms_ >= + cfg_.distance.no_distance_exit_grace_ms) { + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kNoDistanceExit; + return FinishAndLog("持续无距离达门限，退出近距操作"); + } + out.vx_mps = vx_limiter_.Limit( + cfg_.distance.no_distance_approach_limit_mps, dt_s); + break; } } @@ -232,6 +250,7 @@ void TrackingControlLaw::Reset() { yaw_rate_limiter_.Reset(); vx_limiter_.Reset(); vz_limiter_.Reset(); + no_distance_active_ = false; last_now_ms_ = 0; last_mode_ = ControlMode::kDegradedHold; } diff --git a/reference-c++/visual_track_control/src/vtc_config.cpp b/reference-c++/visual_track_control/src/vtc_config.cpp index 629bc64..960b2f7 100644 --- a/reference-c++/visual_track_control/src/vtc_config.cpp +++ b/reference-c++/visual_track_control/src/vtc_config.cpp @@ -107,9 +107,9 @@ ControlGroupConfig LoadControl(const Json& root) { c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"), "control.frequency_hz"); c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms"); - c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms"); + c.distance_stale_ms = RequireInteger(s, "control", "distance_stale_ms"); c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms"); - if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) { + if (c.visual_stale_ms <= 0 || c.distance_stale_ms <= 0 || c.attitude_stale_ms <= 0) { throw std::runtime_error("配置非法: control.*_stale_ms 必须为正"); } return c; @@ -193,6 +193,13 @@ DistanceGroupConfig LoadDistance(const Json& root) { d.no_distance_approach_limit_mps = RequireNonNegative( RequireNumber(s, "distance", "no_distance_approach_limit_mps"), "distance.no_distance_approach_limit_mps"); + // 0 容许：等效旧版"首个无效拍即退出"行为 + d.no_distance_exit_grace_ms = RequireInteger(s, "distance", + "no_distance_exit_grace_ms"); + if (d.no_distance_exit_grace_ms < 0) { + throw std::runtime_error( + "配置非法(不得为负): distance.no_distance_exit_grace_ms"); + } return d; } diff --git a/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp index 4ec1bce..b6a5d4a 100644 --- a/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp +++ b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp @@ -4,8 +4,9 @@ // 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）： // 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、 // Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕， -// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、 -// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。 +// A2）、垂直统一速度支路（A1）、距离通道无距离降级动作（含 kExit +// 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、 +// 机体系→NED 输出旋转（A4）。 // ============================================================================= #include @@ -31,7 +32,7 @@ TrackingConfig MakeConfig() { TrackingConfig cfg; cfg.control.frequency_hz = 20.0; cfg.control.visual_stale_ms = 200; - cfg.control.radar_stale_ms = 500; + cfg.control.distance_stale_ms = 500; cfg.control.attitude_stale_ms = 300; cfg.camera.image_width = 1000; cfg.camera.image_height = 1000; @@ -59,7 +60,7 @@ TrackingConfig MakeConfig() { return cfg; } -/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜 +/// 新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜 ControlSnapshot MakeSnapshot(std::int64_t now_ms) { ControlSnapshot s; s.track.tracked = true; @@ -220,9 +221,9 @@ TEST(TrackingControlLawTest, VerticalVelocityClamped) { TEST(TrackingControlLawTest, DistanceApproachWhenFar) { TrackingControlLaw law(MakeConfig()); auto snap = MakeSnapshot(1000); - snap.radar_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3 - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3 + snap.distance_valid = true; + snap.distance_time_ms = 1000; const auto out = law.Update(snap, 1000); EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); } @@ -230,9 +231,9 @@ TEST(TrackingControlLawTest, DistanceApproachWhenFar) { TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) { TrackingControlLaw law(MakeConfig()); auto snap = MakeSnapshot(1000); - snap.radar_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1 - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1 + snap.distance_valid = true; + snap.distance_time_ms = 1000; const auto out = law.Update(snap, 1000); EXPECT_NEAR(out.vx_mps, -1.0, 1e-9); } @@ -240,7 +241,7 @@ TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) { TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) { TrackingControlLaw law(MakeConfig()); // 缺省 kHold auto snap = MakeSnapshot(1000); - snap.associated_to_target = false; + snap.distance_valid = false; const auto out = law.Update(snap, 1000); EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); @@ -251,29 +252,80 @@ TEST(TrackingControlLawTest, NoDistanceSlowApproach) { cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach; TrackingControlLaw law(cfg); auto snap = MakeSnapshot(1000); - snap.associated_to_target = false; + snap.distance_valid = false; const auto out = law.Update(snap, 1000); EXPECT_NEAR(out.vx_mps, 0.5, 1e-9); } -TEST(TrackingControlLawTest, NoDistanceExitDegrades) { +TEST(TrackingControlLawTest, NoDistanceExitWaitsGraceThenDegrades) { + // S1 拍板：kExit 需持续无距离达门限才降级；grace 期间按 kSlowApproach 行为 auto cfg = MakeConfig(); cfg.distance.no_distance_action = NoDistanceAction::kExit; + cfg.distance.no_distance_exit_grace_ms = 200; TrackingControlLaw law(cfg); auto snap = MakeSnapshot(1000); - snap.associated_to_target = false; + snap.distance_valid = false; + const auto out1 = law.Update(snap, 1000); + EXPECT_EQ(out1.mode, ControlMode::kVelocityHeading); + EXPECT_NEAR(out1.vx_mps, 0.5, 1e-9); + snap.track_time_ms = 1200; // 保持视觉/姿态新鲜，聚焦距离通道 + snap.attitude_time_ms = 1200; + const auto out2 = law.Update(snap, 1200); // 恰达门限 → 降级 + EXPECT_EQ(out2.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out2.degraded_reason, DegradedReason::kNoDistanceExit); + EXPECT_FALSE(out2.valid); +} + +TEST(TrackingControlLawTest, NoDistanceExitGraceResetsOnValidFrame) { + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kExit; + cfg.distance.no_distance_exit_grace_ms = 200; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.distance_valid = false; + EXPECT_EQ(law.Update(snap, 1000).mode, ControlMode::kVelocityHeading); + + 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; + EXPECT_EQ(law.Update(snap, 1100).mode, ControlMode::kVelocityHeading); + + snap.track_time_ms = 1250; // 再次无效：重新计时 + snap.attitude_time_ms = 1250; + snap.distance_valid = false; + snap.target_distance_m = 0.0; + EXPECT_EQ(law.Update(snap, 1250).mode, + ControlMode::kVelocityHeading); // 0 ms < 200 ms 门限 + + snap.track_time_ms = 1500; + snap.attitude_time_ms = 1500; + const auto out = law.Update(snap, 1500); // 250 ms ≥ 200 ms → 降级 + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit); +} + +TEST(TrackingControlLawTest, NoDistanceExitZeroGraceDegradesImmediately) { + // grace = 0 保留旧"首个无效拍即退出"行为 + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kExit; + cfg.distance.no_distance_exit_grace_ms = 0; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.distance_valid = false; const auto out = law.Update(snap, 1000); EXPECT_EQ(out.mode, ControlMode::kDegradedHold); EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit); EXPECT_FALSE(out.valid); } -TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) { +TEST(TrackingControlLawTest, StaleDistanceFallsBackToNoDistance) { TrackingControlLaw law(MakeConfig()); // kHold auto snap = MakeSnapshot(1000); - snap.radar_distance_m = 30.0; - snap.associated_to_target = true; - snap.radar_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用 + snap.target_distance_m = 30.0; + snap.distance_valid = true; + snap.distance_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用 const auto out = law.Update(snap, 1000); EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); @@ -287,9 +339,9 @@ TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) { cfg.accel_limit.ax_mps2 = 2.0; TrackingControlLaw law(cfg); auto snap = MakeSnapshot(1000); - snap.radar_distance_m = 30.0; // 通道输出本应为 3.0 - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 30.0; // 通道输出本应为 3.0 + snap.distance_valid = true; + snap.distance_time_ms = 1000; const auto out = law.Update(snap, 1000); // 首拍：dt 按 1/20 Hz 兜底 EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9); // 0.1 } @@ -300,9 +352,9 @@ TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) { TrackingControlLaw law(MakeConfig()); auto snap = MakeSnapshot(1000); snap.yaw_rad = kPi / 2.0; // 机头朝东 - snap.radar_distance_m = 30.0; - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 30.0; + snap.distance_valid = true; + snap.distance_time_ms = 1000; const auto out = law.Update(snap, 1000); // 机体前向 3.0 → 东向 3.0，北向 ≈ 0 EXPECT_NEAR(out.vx_mps, 0.0, 1e-9); @@ -313,9 +365,9 @@ TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) { TrackingControlLaw law(MakeConfig()); auto snap = MakeSnapshot(1000); snap.yaw_rad = 0.0; - snap.radar_distance_m = 30.0; - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 30.0; + snap.distance_valid = true; + snap.distance_time_ms = 1000; const auto out = law.Update(snap, 1000); EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); EXPECT_NEAR(out.vy_mps, 0.0, 1e-9); @@ -328,18 +380,18 @@ TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) { cfg.accel_limit.ax_mps2 = 2.0; TrackingControlLaw law(cfg); auto snap = MakeSnapshot(1000); - snap.radar_distance_m = 30.0; - snap.associated_to_target = true; - snap.radar_time_ms = 1000; + snap.target_distance_m = 30.0; + snap.distance_valid = true; + snap.distance_time_ms = 1000; for (std::int64_t t = 1000; t < 3000; t += 50) { snap.track_time_ms = t; - snap.radar_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.radar_time_ms = 3000; + snap.distance_time_ms = 3000; snap.attitude_time_ms = 3000; const auto out = law.Update(snap, 3000); // Reset 后限幅器归零：又从 0.1 开始渐变 diff --git a/reference-c++/visual_track_control/tests/vtc_config_test.cpp b/reference-c++/visual_track_control/tests/vtc_config_test.cpp index 8a749c6..6307f9a 100644 --- a/reference-c++/visual_track_control/tests/vtc_config_test.cpp +++ b/reference-c++/visual_track_control/tests/vtc_config_test.cpp @@ -30,7 +30,7 @@ const char* kFullJson = R"({ "control": { "frequency_hz": 20.0, "visual_stale_ms": 200, - "radar_stale_ms": 500, + "distance_stale_ms": 500, "attitude_stale_ms": 300 }, "camera": { @@ -59,7 +59,8 @@ const char* kFullJson = R"({ "approach_velocity_limit_mps": 3.0, "retreat_velocity_limit_mps": 1.0, "no_distance_action": "hold", - "no_distance_approach_limit_mps": 0.5 + "no_distance_approach_limit_mps": 0.5, + "no_distance_exit_grace_ms": 3000 }, "accel_limit": { "ax_mps2": 2.0, @@ -96,7 +97,9 @@ TEST(VtcConfigTest, LoadRealConfigSucceeds) { EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1); EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0); EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0); - EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold); + EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kSlowApproach); + EXPECT_EQ(cfg.distance.no_distance_exit_grace_ms, 3000); + EXPECT_EQ(cfg.control.distance_stale_ms, 500); EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0); } @@ -221,4 +224,25 @@ TEST(VtcConfigTest, FovOutOfRangeThrows) { std::remove(path.c_str()); } +TEST(VtcConfigTest, DefaultNoDistanceActionIsSlowApproach) { + // S1 拍板：双目有效域外无距离是常态，默认定速接近（纪要 D4/S1） + const TrackingConfig cfg; + EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kSlowApproach); +} + +TEST(VtcConfigTest, NegativeExitGraceThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"no_distance_exit_grace_ms\": 3000", + "\"no_distance_exit_grace_ms\": -1"); + const std::string path = WriteTempJson(json, "vtc_cfg_badgrace.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("no_distance_exit_grace_ms"), + std::string::npos); + } + std::remove(path.c_str()); +} + } // namespace diff --git a/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md index a114a33..3164316 100644 --- a/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md +++ b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md @@ -34,8 +34,9 @@ 垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消， 原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）； - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度 - `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级 - （hold / slow_approach / exit）。 + `vx_mps`；目标距离无效/超龄时按 `NoDistanceAction` 配置降级 + （hold / slow_approach / exit，exit 需持续无距离达 + `no_distance_exit_grace_ms` 门限，见 §3.8）。 - **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 + `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度）， 全部在机体系完成。 @@ -96,7 +97,8 @@ （原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。） `enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/ -`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。 +`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（持续无距离达门限且配置 +退出近距操作）。 `ControlSnapshot`（输入，纯值类型）： @@ -104,9 +106,9 @@ |------|------| | `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） | | `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** | -| `radar_distance_m` | 机头前向单点距离（米） | -| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） | -| `radar_time_ms` | 雷达取样单调时钟 | +| `target_distance_m` | 目标距离（米，双目立体测距前向分量 Z） | +| `distance_valid` | 距离有效标志；**无效不得当目标距离**（规格 §6.3、需求分析 FR-041）。距离来自目标检测框自身的双目测距，无跨模态关联问题 | +| `distance_time_ms` | 距离取样单调时钟 | | `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） | | `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 | | `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） | @@ -124,15 +126,18 @@ | `coast_active` | 本周期处于 Coast（`is_predicted == true`） | `TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、 -`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、 +`distance_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、 `image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、 `yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical` （`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、 `kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、 `approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、 -`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit` +`no_distance_action`、`no_distance_approach_limit_mps`、 +`no_distance_exit_grace_ms`）、`accel_limit` （`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见 `config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）； +`distance.no_distance_action` 默认 `slow_approach`（双目有效域外无距离 +是常态，对齐 D4 远场定速接近策略，见《双目测距方案讨论纪要》S1）； 相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。 ### 2.3 ICommandTransport 契约 @@ -263,11 +268,11 @@ dt 上限 `kMaxDtS = 0.5` 秒）： → ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分 → ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支） → ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅） -→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按 +→ ⑦ 距离通道（`distance_valid` 且未超龄才可用；否则清积分并按 `no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`， 随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。 -两个设计决策记录： +三个设计决策记录： - **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim` → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态 @@ -278,6 +283,14 @@ dt 上限 `kMaxDtS = 0.5` 秒）： 航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。 位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式 （默认）输出角速度指令，天然相对量、无此问题。 +- **kExit 持续无距离门限（纪要 S1）**：换双目后有效域外无距离是常态 + （纪要 D4 远场定速接近），首个无效拍即退出会频繁降级。`kExit` 现需 + 连续无距离达 `no_distance_exit_grace_ms` 才降级（语义收敛为"疑似 + 传感器级故障"），grace 期间按 `kSlowApproach` 行为执行（经加速度 + 限幅，vx 平滑过渡），任一有效距离帧重置计时；`no_distance_exit_grace_ms + = 0` 保留旧"立即退出"行为。`no_distance_action` 默认值随之 + `kHold` → `kSlowApproach`。判丢刹车期间不计无距离时长，`Reset()` + 清计时状态。 ### 3.9 MAVLink 消息映射（规格 §2.2） @@ -347,12 +360,12 @@ cmake --build build_vtc -j$(nproc) `LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段 （对应规格 §6.4）： -1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达 - 关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限， +1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，双目 + 距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限， 角速度随 β 收敛而减小）； 2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、 - 继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold： - 前向清零）； + 继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为 + `slow_approach`：按限速值缓速前向）； 3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` → 零速刹车。 @@ -372,8 +385,8 @@ cmake --build build_vtc -j$(nproc) | `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 | | `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 | | `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` | -| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 | -| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 | +| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 kExit 门限、grace 有效帧重置、零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 | +| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、默认降级动作为 slow_approach、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界/grace 为负均抛异常且消息携带字段名 | | `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 | 运行方式（构建后）： @@ -430,8 +443,11 @@ CMake config 包）；未找到时自动跳过测试构建并打印状态，不 由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅； 3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现 （`ICommandTransport` 注入），打包与控制律零改动； -4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按 - 规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。 +4. **距离有效标志来源**：`distance_valid` 由双目链路（StereoRanger） + 给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下 + 调用方填 `ControlSnapshot` 时应校验 `StereoTargetDistance.track_id` + 与 `TrackResult.track_id` 一致（v1.0 单目标为调用方约定，纪要 S3， + 库内不加代码）。 ### 6.4 红线声明 diff --git a/test.py b/test.py index 0eef58f..a59e18b 100644 --- a/test.py +++ b/test.py @@ -9,7 +9,7 @@ PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python) 适用于 PX4 1.17 YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成 -检测框与激光测距（几何一致闭环），无人机会实际飞向该虚拟目标。 +检测框与双目目标距离（几何一致闭环），无人机会实际飞向该虚拟目标。 """ import asyncio @@ -116,7 +116,7 @@ TRACK_NORMAL_SECS = 20.0 # 虚拟目标保持可见时长（之后 TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长 TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时 STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态 -PLOT_TRAJECTORY = False # 跟踪结束后绘制无人机与虚拟目标轨迹 +PLOT_TRAJECTORY = True # 跟踪结束后绘制无人机与虚拟目标轨迹 TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png" # 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移 @@ -219,7 +219,7 @@ def plot_trajectories( class VirtualTarget: - """几何一致虚拟目标 —— YOLO 检测与激光雷达的替身。 + """几何一致虚拟目标 —— YOLO 检测与双目测距的替身。 目标初始位置： 以起飞后的悬停锚点为参考， @@ -387,7 +387,7 @@ class VirtualTarget: ): """返回： - (detections, radar_distance_m, associated) + (detections, target_distance_m, distance_valid) 不可见时： @@ -487,16 +487,10 @@ class VirtualTarget: ) # ============================================================ - # 9. 计算模拟激光雷达距离 + # 9. 模拟双目目标距离（前向分量 Z，与检测框几何一致） # ============================================================ - slant = math.sqrt( - fwd * fwd - + right * right - + rel_d * rel_d - ) - - return [box], slant, True + return [box], fwd, True class TelemetryHub: @@ -616,7 +610,7 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float], target.set_visible(False) print("-- 模拟目标消失，追踪器进入 Coast 预测") - detections, radar_m, associated = target.observe( + detections, distance_m, distance_valid = target.observe( hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad) # 记录本拍位置（目标位置在 observe 内已 update） @@ -628,9 +622,9 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float], snap = ControlSnapshot( track=track_result_to_state(result), track_time_ms=now, # 每拍取样即打戳（视觉始终新鲜） - radar_distance_m=radar_m, - associated_to_target=associated, - radar_time_ms=now if associated else 0, + target_distance_m=distance_m, + distance_valid=distance_valid, + distance_time_ms=now, attitude_present=True, attitude_time_ms=hub.attitude_time_ms, yaw_rad=hub.yaw_rad, @@ -644,7 +638,7 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float], print(f"[跟踪 t={t_s:5.1f}s] " f"检测={len(detections)} " f"追踪[trk={result.tracked} pred={result.is_predicted} " - f"lost={result.lost_frames}] 雷达={radar_m:5.1f}m | " + f"lost={result.lost_frames}] 距离={distance_m:5.1f}m | " f"[{out.mode.name}] vx_body={out.vx_body_mps:+.2f} " f"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s " f"coast={out.coast_active}") diff --git a/trajectory_plot.png b/trajectory_plot.png index d6af646..d26d650 100644 Binary files a/trajectory_plot.png and b/trajectory_plot.png differ diff --git a/utils/README.md b/utils/README.md index 074af5b..3829630 100644 --- a/utils/README.md +++ b/utils/README.md @@ -1,9 +1,12 @@ -# utils —— reference-c++ 两个子项目的 Python 移植 +# utils —— reference-c++ 的 Python 移植 + 双目测距新实现 将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与 `visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖** （唯一的数据转换点在胶水层 demo）。 +另有 `stereo_ranger`（双目稀疏目标测距）：**按规格新实现**（非 C++ 移植）， +规格 `docs/双目测距技术路线.md`；同样与既有两包互不依赖。 + ## 目录结构 ``` @@ -12,19 +15,24 @@ utils/ │ └── README.md # 接口清单 + C++ 差异 ├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关 │ └── README.md # 接口清单 + C++ 差异 +├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖） +│ └── README.md # 接口清单 + 与规格的偏差 ├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送） +├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件） ├── config/track_control.json # 控制律配置（拷贝自 reference-c++） -├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件） -│ ├── target_tracker/ # 27 条 -│ └── visual_track_control/ # 78 条 -├── conftest.py # 保证可从任意工作目录导入两包 +├── config/stereo_ranger.json # 双目测距配置（占位值，待标定回填） +├── tests/ # pytest 测试 +│ ├── target_tracker/ # 28 条（对应 C++ 测试文件） +│ ├── visual_track_control/ # 81 条（对应 C++ 测试文件） +│ └── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程） +├── conftest.py # 保证可从任意工作目录导入三包 └── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐） ``` ## 环境 conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12， -依赖：numpy、mavsdk、pytest。 +依赖：numpy、mavsdk、pytest（stereo_ranger 仅标准库）。 ## 运行测试 @@ -33,8 +41,9 @@ cd utils "D:/program file/conda/envs/pytorch/python.exe" -m pytest -q ``` -105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4 -回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。 +220 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4 +回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger +（合成框对驱动，无需真实图像/硬件）。 ## 运行演示 @@ -44,11 +53,17 @@ python utils/demo_track_control.py --speedup 10 # MAVSDK 实发（需飞控/SITL 在 udp://:14540） python utils/demo_track_control.py --transport mavsdk + +# 双目测距（合成框对，离线，无相机/飞控） +python utils/demo_stereo_ranger.py ``` -时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→ +控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→ 目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。 +双目演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→ +单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。 + ## 移植约定 - MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支 @@ -56,3 +71,6 @@ python utils/demo_track_control.py --transport mavsdk - 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`， `EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。 - 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。 +- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、 + 视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README； + ⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。 diff --git a/utils/config/track_control.json b/utils/config/track_control.json index 7dcd26b..c9d9c8f 100644 --- a/utils/config/track_control.json +++ b/utils/config/track_control.json @@ -4,7 +4,7 @@ "control": { "frequency_hz": 20.0, "visual_stale_ms": 200, - "radar_stale_ms": 500, + "distance_stale_ms": 500, "attitude_stale_ms": 300 }, @@ -36,8 +36,9 @@ "derivative_filter_coef": 0.2, "approach_velocity_limit_mps": 3.0, "retreat_velocity_limit_mps": 1.0, - "no_distance_action": "hold", - "no_distance_approach_limit_mps": 0.5 + "no_distance_action": "slow_approach", + "no_distance_approach_limit_mps": 0.5, + "no_distance_exit_grace_ms": 3000 }, "accel_limit": { diff --git a/utils/demo_stereo_ranger.py b/utils/demo_stereo_ranger.py deleted file mode 100644 index a7741e6..0000000 --- a/utils/demo_stereo_ranger.py +++ /dev/null @@ -1,96 +0,0 @@ -"""stereo_ranger demo：合成多目标场景演示单帧 update() 全流程。 - -本 demo 不使用任何检测器：按已知 (X, Y, Z) 目标解析投影生成左右目 -检测框（模拟"外部检测器输出，框已在校正后原生坐标系"），并叠加 ±1 px -亚像素抖动，随后用 StereoRanger 测距并对照真值。 - -运行（pytorch 环境）： - python utils/demo_stereo_ranger.py [--config path/to/stereo.json] - -依赖仅为标准库 + stereo_ranger 包（将 utils/ 加入 PYTHONPATH 或于 utils/ 下运行）。 -""" - -import argparse -import random -import sys -from pathlib import Path - -sys.path.insert(0, str(Path(__file__).resolve().parent)) - -from stereo_ranger import Detection, StereoConfig, StereoRanger, focal_from_hfov, load_from_json, range_error_abs - -# 模拟相机：1920x1080，HFOV 60°（文档 §5.3 示例之一） -IMAGE_W = 1920 -IMAGE_H = 1080 -HFOV_DEG = 60.0 -BASELINE_M = 0.06 - - -def project_to_boxes(x_m, y_m, z_m, fx, fy, cx, cy, b, rng, w_m=0.5, h_m=0.3): - """目标物理宽高 -> 左右目框；叠加亚像素抖动模拟检测噪声（种子固定）。""" - w_px = fx * w_m / z_m - h_px = fy * h_m / z_m - u_l = fx * x_m / z_m + cx - v_l = fy * y_m / z_m + cy - u_r = u_l - fx * b / z_m - jitter = lambda: rng.uniform(-1.0, 1.0) # ±1 px 框边缘抖动 - left = Detection(u_l - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(), - u_l + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(), - conf=0.95, cls_id=0, track_id=1) - right = Detection(u_r - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(), - u_r + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(), - conf=0.9, cls_id=0, track_id=1) - return left, right - - -def main() -> None: - parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--config", default=None, help="StereoConfig JSON 文件（可选）") - args = parser.parse_args() - - cfg = load_from_json(args.config) if args.config else StereoConfig( - fx_px=focal_from_hfov(IMAGE_W, HFOV_DEG), - fy_px=focal_from_hfov(IMAGE_W, HFOV_DEG), - cx_px=IMAGE_W / 2.0, cy_px=IMAGE_H / 2.0, - baseline_m=BASELINE_M, - range_tol_m=2.0, - ) - ranger = StereoRanger(cfg) - rng = random.Random(20260909) - - targets = [(0.0, 0.0, 5.0), (2.0, 0.5, 8.0), (-1.5, -0.4, 12.0)] - left_dets, right_dets = [], [] - for x_m, y_m, z_m in targets: - l, r = project_to_boxes(x_m, y_m, z_m, cfg.fx_px, cfg.fy_px, cfg.cx_px, cfg.cy_px, - cfg.baseline_m, rng) - left_dets.append(l) - right_dets.append(r) - # 一个只在左目检出的干扰框 -> 应输出 matched=False 的方位记录 - left_dets.append(Detection(100.0, 900.0, 160.0, 950.0, conf=0.6, cls_id=9)) - - reports = ranger.update(left_dets, right_dets, frame_sequence=1, timestamp_s=0.0) - - print(f"内参: F={cfg.fx_px:.1f}px cx={cfg.cx_px:.0f} B={cfg.baseline_m:.3f}m " - f"Z 窗口 [{cfg.z_min_m}, {cfg.z_max_m}]m") - print(f"{'真值 Z':>8} {'估计 Z':>8} {'真值 X':>7} {'估计 X':>7} {'视差':>6} {'δZ估计':>7} " - f"{'有效':>5} {'x_mid':>7} 备注") - for i, r in enumerate(reports): - if not r.matched: - print(f"{'-':>8} {'-':>8} {'-':>7} {'-':>7} {'-':>6} {'-':>7} {'-':>5} " - f"{r.centre_pixel_x_mid:7.1f} 左目未配对（方位直发）") - continue - truth = targets[i] - print(f"{truth[2]:8.2f} {r.forward_distance_m:8.2f} {truth[0]:7.2f} {r.lateral_m:7.2f} " - f"{r.disparity_px:6.1f} {r.error_est_m:7.2f} {str(r.valid):>5} " - f"{r.centre_pixel_x_mid:7.1f} src={r.disparity_source}") - print(f"\n共 {len(reports)} 条记录（{sum(1 for r in reports if r.valid)} 条有效）") - - print("\n误差模型示意（规格 §5.3，δD = 1 px）：") - print(f"{'Z (m)':>7} {'δZ (m)':>8}") - for z in [5, 10, 15, 20, 30]: - err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, 1.0) - print(f"{z:7d} {err:8.2f}") - - -if __name__ == "__main__": - main() diff --git a/utils/demo_track_control.py b/utils/demo_track_control.py index 213e0e6..7cd451f 100644 --- a/utils/demo_track_control.py +++ b/utils/demo_track_control.py @@ -10,7 +10,7 @@ → CommandSender（mock 记录 / MAVSDK offboard 实发） 场景时间线（20 Hz，--speedup 可加速）： - 0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近 + 0~ 6s 正常跟踪：目标匀速横移 + 双目距离 30→10 m 前向逼近 6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢， 控制律自动切换零速刹车） 10~13s 持续判丢：维持零速刹车 @@ -73,8 +73,8 @@ def simulate_detections(tick: int, normal_ticks: int, period_s: float): return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)] -def simulate_radar(tick: int, normal_ticks: int, period_s: float): - """场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。""" +def simulate_distance(tick: int, normal_ticks: int, period_s: float): + """场景双目距离源：正常阶段有效、距离 30→10 m；之后无效。""" if tick >= normal_ticks: return 0.0, False distance = max(10.0, 30.0 - tick * period_s * 3.0) @@ -155,15 +155,15 @@ async def run_demo(args) -> None: for tick in range(total_ticks): now_ms = 1000 + tick * period_ms detections = simulate_detections(tick, normal_ticks, period_s) - radar_m, associated = simulate_radar(tick, normal_ticks, period_s) + distance_m, distance_valid = simulate_distance(tick, normal_ticks, period_s) result = tracker.update(detections) snap = ControlSnapshot( track=track_result_to_state(result), track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜） - radar_distance_m=radar_m, - associated_to_target=associated, - radar_time_ms=now_ms if associated else 0, + target_distance_m=distance_m, + distance_valid=distance_valid, + distance_time_ms=now_ms, attitude_present=True, attitude_time_ms=now_ms, # 模拟姿态源持续可用 yaw_rad=0.0, # 机头朝北（NED 旋转恒等） diff --git a/utils/stereo_ranger/README.md b/utils/stereo_ranger/README.md index 9cd2c55..df5cd97 100644 --- a/utils/stereo_ranger/README.md +++ b/utils/stereo_ranger/README.md @@ -1,65 +1,112 @@ -# stereo_ranger（Python） +# stereo_ranger（Python 版） -双目稀疏目标测距（规格：`docs/双目测距方案.md`）。输入左右目各自、**校正后原生坐标系**下的检测框，输出目标的 \(Z, X, Y, R\) 与方位。不做目标识别（YOLO）、不做图像采集与立体校正——对应规格 §10 中可独立替换的检测前端与采集预处理模块。 +双目稀疏目标测距：对**同一时刻**左右两目已立体校正图像上的目标检测框做立体 +几何解算，输出少量目标的**前向距离 Z + 斜距 R（参考）+ 虚拟双目中心方位 +c_mid + 有效性/置信度**。不做目标识别、图像采集/立体校正、稠密深度。 -仅依赖标准库（`math`/`logging`），不 import 本仓库任何其他模块。 +规格：`docs/双目测距技术路线.md`（算法与语义权威）。 + +⚠ **全部参数为占位值**（焦距/标定待厂商数据）：标定与地面真值验证完成前， +本模块输出**仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据**（§12.3）。 ## 接口清单 | 名称 | 说明 | | --- | --- | -| `Detection(x1, y1, x2, y2, conf=1.0, cls_id=0, track_id=0)` | 输入检测框（px，左上/右下角） | -| `StereoConfig(...)` | 配置（默认值 = 规格示例值，见下） | -| `load_from_json(path)` | 从 JSON 构建配置（缺字段/未知字段/非法值抛 `ValueError`） | -| `StereoRanger(config=None)` | 测距器主类 | -| `.update(left_dets, right_dets, frame_sequence=0, timestamp_s=None) -> list[RangeReport]` | 单帧主流程（返回记录数与左目检测数一致、顺序一致） | -| `.reset()` / `.config` | 清空滤波状态 / 当前配置 | -| `RangeReport` | 输出记录（字段见 `types.py`） | -| `focal_from_hfov(w_px, hfov_deg)` / `range_error_abs(z, fx, B, δD=1)` / `range_error_rel(...)` | 工具函数（§5.2/§5.3） | +| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 | +| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO | +| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） | +| `.config` / `.matcher` | 只读访问（配置与匹配器） | +| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） | +| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） | +| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） | +| `DetectionFrame(frame_id, detections)` | 单目单帧 | +| `RangeRecord` | 输出记录：`frame_id / class_id / degradation / valid / track_id / forward_distance_m(Z) / slant_range_m(R) / disparity_px(D) / center_mid_px(c_mid) / reference_box / reference_eye / confidence / timestamp_ms` | +| `Degradation` / `Eye` / `DisparitySource` | 枚举：完整/仅方位；左/右目；同侧边缘/中心回退 | +| `sample_point` / `SamplePoint` / `select_disparity` / `Disparity` | 取点器（§6.1，纯函数） | +| `Matcher`（Protocol）/ `GreedyMatcher` / `MatchPair` | 匹配器（§6.2，接口注入，可换匈牙利等） | +| `solve_range` / `RangeSolution` / `virtual_center_px` / `forward_distance_m` | 测距与方位源（§3.2/§6.6，纯函数） | +| `evaluate_validity` / `ValidityDecision` / `range_error_abs_m` / `range_error_rel` / `confidence_from_cost` / `DELTA_D_PX` | 有效性三道门与误差模型（§6.4/§7.1，纯函数） | +| `ConsistencyMonitor` / `ConsistencyStatus` | 一致性监控（§6.7，每目标状态） | +| `DistanceFilter` | 距离低通 + 跳变重置（§6.5，每目标状态） | +| `TargetStore` | 目标隔离与老化清理（§4，track_id 优先 + 最近邻兜底） | +| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） | -## 与检测器的约定（规格 §4.3） +`logger` 名称为 `stereo_ranger`，走标准 `logging`。 -1. 左右图像必须使用**完全一致**的预处理几何（同 letter-box、同缩放），由检测前端保证； -2. 传入本模块的框必须已映射回**原生分辨率**（视差在原生坐标计算），内参 `fx/fy/cx/cy` 亦对应原生分辨率； -3. 图像对须已完成立体校正（同一目标在两目 y 坐标一致）；校正残留由 `vertical_tau_px` 容错； -4. YOLO 推理及左右目并行执行由调用方负责，本模块只收检测结果。 +## 单帧流程（`update` 五步） -## 单帧流程（对照规格） +1. **同帧契约**：`left.frame_id != right.frame_id` → 拒收整帧、返回空列表、WARN 节流（§2.1）； +2. **取点**：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）； +3. **匹配**：硬约束（类别一致、`|Δy| ≤ tau_y_px`、框高相对差 ≤ `tau_h_ratio`）+ 代价升序贪心一一配对，`cost < 1` 才接受； +4. **配对目标**：`D → Z = F·B/D`（`D ≤ 0` 判违例）→ `R` 参考 → `c_mid = (cL + cR)/2` → 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过 `DistanceFilter`，记 `Z_out`； +5. **未配对框**：发布仅方位记录（`BEARING_ONLY`，`valid = false`，右目未匹配同样发布）；帧末对未出现目标记失效、连续 `filter_reset_frames` 帧未出现整体清理。 -硬约束过滤（类别一致、`|Δy| ≤ τy`、高度比 `≤ τh`、视差落在 `FB/z_max ~ FB/z_min` 区间）→ 代价 `|Δy|/τy + |Δh|/(τh·max(hL,hR))` 升序贪心一一配对（严格 `cost < 1`；同 `track_id` 优先）→ 主取**同侧左边缘**视差，越出关联窗口时按配置回退中心视差（节流警告）→ `Z=FB/D`、`X=(xL−cx)Z/F`、`Y=(yL−cy)Z/F`、`R=√(X²+Y²+Z²)` → 有效性 `D ≥ D_min 且 δZ ≤ Z_tol` → 可选一阶低通滤波（按 (cls, track) 分键，跳变超门限重置；`track_id=0` 不过滤）。 +记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。 -## 输出语义 +## 实现决策（v1.0 冻结，已回填规格） -- `matched=True, valid=True`：有效测距； -- `matched=True, valid=False`：已配对但超出有效工作域（§5.4），距离/坐标仍发布供诊断，方位可用； -- `matched=False, valid=False`：左目检测本帧未配对（§6 失败处理），仅单目方位回退。 +规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/ +§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。 -方位源 `centre_pixel_x_mid` 为基线中点虚拟相机投影（§11.3）；注意 `X` 属**左目坐标系**（§4.2），二者相差 `B/2`，勿混用。 +1. **越界 = 有效性门，不是配对硬约束**：规格 §6.2 原把"视差在域内"列入 + 硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填—— + 越界目标仍配对、仍发布 `D` 与 `c_mid`（远场方位闭环的输入基础），仅 + `valid = false`。 +2. **视差域权威**：`disparity_min_px / disparity_max_px` 为真源，`z_min_m / z_max_m` + 为派生只读属性；原 §9.2 占位值不自洽（`D_min = 3 px ⇔ 19.2 m ≠ 20 m`）， + 距离域配置项已从规格删除（§6.4/§9.2）。 +3. **不实现 §6.1 的"左右边缘各算一次取中值"精化**：两支的 median 恒等于均值， + 即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留 + v1.1 亚像素精化。 +4. **补充配置项**（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）： + `consistency_jump_px`（§6.7 视差跳变门限）、`isolation_gate_px`（§4 最近邻 + 兜底的门限）。 +5. **δD 固定 1 px**：`DELTA_D_PX = 1.0`（§7.1 ⚠占位）；标定回填后可参数化。 +6. **R 与已发布 Z 同基准**：`R = R_raw · z_out / z_raw`（X/Y/Z 均 ∝ Z，精确换基， + 保证 `R ≥ Z`）；无效帧无 `Z_out` 时 `R` 取本帧原始值（诊断参考）。规格 + §2.2/§6.5 已注明。 +7. **置信度映射**（§6.4 ⚠占位）：`clamp(1 − cost, 0, 1)`；连续跳变 ≥ N 帧乘 0.5 + （`_CONFIDENCE_DEGRADE_FACTOR`）；判无效置 0。 +8. **一致性只在配对成功帧推进**：仅方位帧没有 D 观测量，不推进 D 序列；但目标 + 未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。 +9. **取点回退规则**：边缘视差越窗且中心视差在域内 → 回退中心 + （`CENTER_FALLBACK` + WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。 +10. **track_id 优先路径提前启用**（v1.1 语义，规格 §4 已按此冻结）：左右 + `track_id` 一致且 > 0 时用 `(track, 类别, id)` 作隔离键；最近邻只在无 + track_id 的状态间匹配，一一归属。 +11. **§3.2 勘误**：原文 `X_C = X + B/2` 与 §6.6 的符号相反（应为 `X_C = X − B/2`）， + 规格已修正；代码不输出 `X_C`，以 §6.6 恒等式为准。 -## StereoConfig 关键项 +## 运行期状态清单（§11.1，漏一个就变行为） -| 字段 | 默认 | 说明 | -| --- | --- | --- | -| `fx_px/fy_px/cx_px/cy_px` | 960/960/960/540 | 校正后内参（原生分辨率） | -| `baseline_m` | 0.06 | 基线 B | -| `z_min_m / z_max_m` | 1.0 / 50.0 | 距离窗口（派生视差关联区间） | -| `vertical_tau_px` / `height_ratio_tau` | 2.0 / 0.25 | 关联阈值 τy / τh | -| `disparity_min_px` / `range_tol_m` | 3.0 / 2.0 | 有效性判据 D_min / Z_tol | -| `disp_noise_px` | 1.0 | 误差模型 δD（用于 δZ 估计与有效性） | -| `use_center_fallback` | true | 边缘视差越窗时回退中心 | -| `filter_enabled` / `filter_alpha` / `filter_jump_ratio` | false / 0.3 / 0.2 | 时间滤波 | -| `warn_interval_s` | 5.0 | 诊断日志节流间隔 | +| 状态 | 所属 | 初值 | 改变时机 | +| --- | --- | --- | --- | +| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 | +| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 | +| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` | +| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） | +| 告警计数 ×异常类 | `WarnThrottle` | 0 | 每次异常 | -## 运行 +## 测试与演示 ```bash cd utils -"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全部测试（149 条） -"D:/program file/conda/envs/pytorch/python.exe" demo_stereo_ranger.py # 合成场景 demo +"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条 +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全量 220 条 +python utils/demo_stereo_ranger.py # 六场景合成框对演示 ``` -## 未覆盖边界（属其他模块/后续版本） +构件级测试逐条对应 §11.4（取点器/匹配器/测距/有效性/滤波器/一致性/全流程）， +**全部用合成框对驱动，无需真实图像**（几何是解析可断言的）： -- 图像采集、立体校正 remap、帧同步、side-by-side 裁切（规格 §3）； -- YOLO/检测器及其预处理（由调用方按上文约定接入）； -- 亚像素边缘精化、跨帧跟踪（`track_id` 由外部追踪器提供后，时间滤波即可用）、与主测距传感器融合（§5.5/§11）。 +- 取点：同侧边缘取值、框宽差不进入视差、中心对照、越窗回退、边界含等号、负视差； +- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对； +- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长； +- 有效性：三道门与边界归类、预算门通过/拦截两侧； +- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零； +- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、 + 空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。 + +demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布） +③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。 diff --git a/utils/stereo_ranger/__init__.py b/utils/stereo_ranger/__init__.py index 8a11b35..2dd4a9a 100644 --- a/utils/stereo_ranger/__init__.py +++ b/utils/stereo_ranger/__init__.py @@ -1,30 +1,72 @@ -"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距方案.md）。 +"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距技术路线.md）。 -纯算法包：输入校正后原生坐标系的左右目检测框，输出目标的 (Z, X, Y, R) -与方位。不做目标识别、图像采集与立体校正；仅依赖标准库。 +纯算法包：输入校正后坐标系的同帧左右目检测框，输出目标距离记录（Z/R/D、 +虚拟双目中心方位、valid/confidence）。不做目标识别、图像采集与立体校正； +仅依赖标准库。⚠ 全部数值参数为占位值，标定与地面验证完成前不得作为飞行 +控制决策依据（§12.3 红线）。 """ from .config import StereoConfig, load_from_json -from .stereo_ranger import ( - DISP_SOURCE_CENTER, - DISP_SOURCE_EDGE, - StereoRanger, - focal_from_hfov, - range_error_abs, +from .consistency import ConsistencyMonitor, ConsistencyStatus +from .distance_filter import DistanceFilter +from .geometry import ( + DELTA_D_PX, + RangeSolution, + ValidityDecision, + confidence_from_cost, + evaluate_validity, + forward_distance_m, + range_error_abs_m, range_error_rel, + solve_range, + virtual_center_px, ) -from .types import BBoxXYXY, Detection, RangeReport +from .matching import GreedyMatcher, MatchPair, Matcher +from .sampling import Disparity, SamplePoint, sample_point, select_disparity +from .stereo_ranger import StereoRanger +from .target_store import TargetStore +from .types import ( + BBoxXYXY, + Degradation, + Detection, + DetectionFrame, + DisparitySource, + Eye, + RangeRecord, +) +from .warn_throttle import WarnThrottle __all__ = [ "BBoxXYXY", - "DISP_SOURCE_CENTER", - "DISP_SOURCE_EDGE", + "DELTA_D_PX", + "ConsistencyMonitor", + "ConsistencyStatus", + "Degradation", "Detection", - "RangeReport", + "DetectionFrame", + "Disparity", + "DisparitySource", + "DistanceFilter", + "Eye", + "GreedyMatcher", + "MatchPair", + "Matcher", + "RangeRecord", + "RangeSolution", + "SamplePoint", "StereoConfig", "StereoRanger", - "focal_from_hfov", + "TargetStore", + "ValidityDecision", + "WarnThrottle", + "confidence_from_cost", + "evaluate_validity", + "forward_distance_m", "load_from_json", - "range_error_abs", + "range_error_abs_m", "range_error_rel", + "sample_point", + "select_disparity", + "solve_range", + "virtual_center_px", ] diff --git a/utils/stereo_ranger/config.py b/utils/stereo_ranger/config.py index ba8ef07..4307702 100644 --- a/utils/stereo_ranger/config.py +++ b/utils/stereo_ranger/config.py @@ -1,50 +1,51 @@ -"""StereoConfig 与 JSON 配置加载。 +"""StereoConfig 与 JSON 配置加载（§9 配置参数全量表）。 -规格 docs/双目测距方案.md：全部数值参数均为配置项（§2），默认值取文档 -示例值。配置校验在构造期一次性完成，热路径不读 JSON。 +全部参数配置外置、构造期一次加载并校验：缺项、类型错误、非法取值一律拒绝 +进入运行期（fail-fast）。所有默认值均为 ⚠占位值，未整定前不得用于飞行决策 +（§12.3 红线 1/2）。 + +域权威（实现决策）：视差域 disparity_min_px / disparity_max_px 为权威， +距离域 z_min_m / z_max_m 由 Z = F·B/D 派生为只读属性——域判定使用视差 +（§11.2 直接观测量），两套阈值不再冗余存储、避免互推舍入不一致（§6.4）。 """ import json +import math from dataclasses import dataclass, fields -from pathlib import Path -from typing import Any, Dict, Tuple +from typing import Any, Dict + +# JSON 加载时的整数型字段（其余按数值校验） +_INT_FIELDS = frozenset({"consistency_N_frames", "consistency_M_frames", "filter_reset_frames"}) @dataclass class StereoConfig: - # ---- 相机参数（校正后、对应原生分辨率，§3.1 / §4.3）---- + # ---- 几何组（来自标定 P1/P2，§9.1；⚠全部为占位值）---- fx_px: float = 960.0 fy_px: float = 960.0 cx_px: float = 960.0 cy_px: float = 540.0 - baseline_m: float = 0.06 # 基线 B + baseline_m: float = 0.06 - # ---- 目标距离窗口：关联硬约束（§6），派生视差区间 [FB/z_max, FB/z_min] ---- - z_min_m: float = 1.0 - z_max_m: float = 50.0 + # ---- 匹配组（§9.2）---- + tau_y_px: float = 2.0 + tau_h_ratio: float = 0.25 + disparity_min_px: float = 3.0 + disparity_max_px: float = 57.6 # ⚠占位：F=960/B=0.06 下 ⇔ 近界 1.0 m - # ---- 关联阈值（§6）---- - vertical_tau_px: float = 2.0 # 垂直残差 tau_y - height_ratio_tau: float = 0.25 # 相对高度差 tau_h + # ---- 有效性组（§9.3）---- + max_delta_z_m: float = 2.0 # 误差预算 δZ(δD = 1px) 上限 + consistency_N_frames: int = 3 # 连续跳变降置信度的帧数 + consistency_M_frames: int = 5 # 连续异常判 invalid 的帧数 + consistency_jump_px: float = 3.0 # ⚠补充项：§9 未列，§6.7 的视差跳变门限 - # ---- 视差取点与有效性（§5）---- - disparity_min_px: float = 3.0 # D_min：D 低于此值直接无效 - range_tol_m: float = 2.0 # Z_tol：δZ <= Z_tol 才算有效 - disp_noise_px: float = 1.0 # δD（误差模型中的边缘定位噪声） - use_center_fallback: bool = True # 左边缘视差越窗时回退中心（§5.1） + # ---- 滤波组（§9.4）---- + smooth_alpha: float = 0.3 + jump_reset_m: float = 5.0 + filter_reset_frames: int = 5 - # ---- 可选时间滤波（§7，一阶低通 + 跳变重置）---- - filter_enabled: bool = False - filter_alpha: float = 0.3 - filter_jump_ratio: float = 0.2 # 相对跳变超此门限则重置滤波器 - - # ---- 诊断日志节流 ---- - warn_interval_s: float = 5.0 - - def disparity_bounds_px(self) -> Tuple[float, float]: - """关联用的视差区间 (lo, hi)：由 z_min/z_max 派生（§6）。""" - fb = self.fx_px * self.baseline_m - return fb / self.z_max_m, fb / self.z_min_m + # ---- 目标隔离（§4：类别 + 框位置最近邻；⚠补充项）---- + isolation_gate_px: float = 50.0 def __post_init__(self) -> None: _require(self.fx_px > 0.0, "fx_px", "必须为正") @@ -52,31 +53,71 @@ class StereoConfig: _require(self.cx_px >= 0.0, "cx_px", "必须非负") _require(self.cy_px >= 0.0, "cy_px", "必须非负") _require(self.baseline_m > 0.0, "baseline_m", "必须为正") - _require(0.0 < self.z_min_m < self.z_max_m, "z_min_m/z_max_m", "须满足 0 < z_min_m < z_max_m") - _require(self.vertical_tau_px > 0.0, "vertical_tau_px", "必须为正") - _require(self.height_ratio_tau > 0.0, "height_ratio_tau", "必须为正") - _require(self.disparity_min_px > 0.0, "disparity_min_px", "必须为正") - _require(self.range_tol_m > 0.0, "range_tol_m", "必须为正") - _require(self.disp_noise_px > 0.0, "disp_noise_px", "必须为正") - _require(0.0 < self.filter_alpha <= 1.0, "filter_alpha", "须落在 (0, 1]") - _require(self.filter_jump_ratio >= 0.0, "filter_jump_ratio", "必须非负") - _require(self.warn_interval_s >= 0.0, "warn_interval_s", "必须非负") + _require(self.tau_y_px > 0.0, "tau_y_px", "必须为正") + _require(self.tau_h_ratio > 0.0, "tau_h_ratio", "必须为正") + _require( + 0.0 < self.disparity_min_px < self.disparity_max_px, + "disparity_min_px/disparity_max_px", + "须满足 0 < 下限 < 上限", + ) + _require(self.max_delta_z_m > 0.0, "max_delta_z_m", "必须为正") + _require(self.consistency_N_frames >= 1, "consistency_N_frames", "必须 ≥ 1") + _require( + self.consistency_M_frames >= self.consistency_N_frames, + "consistency_M_frames", + "必须 ≥ consistency_N_frames", + ) + _require(self.consistency_jump_px > 0.0, "consistency_jump_px", "必须为正") + _require(0.0 < self.smooth_alpha <= 1.0, "smooth_alpha", "须落在 (0, 1]") + _require(self.jump_reset_m > 0.0, "jump_reset_m", "必须为正") + _require(self.filter_reset_frames >= 1, "filter_reset_frames", "必须 ≥ 1") + _require(self.isolation_gate_px > 0.0, "isolation_gate_px", "必须为正") + + @property + def focal_baseline_px_m(self) -> float: + """F·B：构造期一次算好的常量，热路径只做乘除（§11.1）。""" + return self.fx_px * self.baseline_m + + @property + def z_min_m(self) -> float: + """近界（由 disparity_max_px 派生）。""" + return self.focal_baseline_px_m / self.disparity_max_px + + @property + def z_max_m(self) -> float: + """远界（由 disparity_min_px 派生）。""" + return self.focal_baseline_px_m / self.disparity_min_px + + @property + def z_error_budget_max_m(self) -> float: + """误差预算门限对应的最远距离 sqrt(δZ_max·F·B)（δD = 1px，§7.2）。""" + return math.sqrt(self.max_delta_z_m * self.focal_baseline_px_m) def load_from_json(path: str) -> StereoConfig: - """从 JSON 文件构建配置。字段缺失或非法取值抛 ValueError（携带字段名）。""" - with open(path, encoding="utf-8") as f: - data: Dict[str, Any] = json.load(f) - known = {field.name for field in fields(StereoConfig)} - unknown = set(data) - known + """从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。""" + with open(path, encoding="utf-8") as handle: + data: Dict[str, Any] = json.load(handle) + if not isinstance(data, dict): + raise ValueError("配置非法: 顶层须为 JSON 对象") + known = {item.name for item in fields(StereoConfig)} + unknown = sorted(set(data) - known) if unknown: - raise ValueError(f"未知配置字段: {sorted(unknown)}") - missing = known - set(data) + raise ValueError(f"未知配置字段: {unknown}") + missing = sorted(known - set(data)) if missing: - raise ValueError(f"缺少配置字段: {sorted(missing)}") + raise ValueError(f"缺少配置字段: {missing}") + for name, value in data.items(): + if isinstance(value, bool): + raise ValueError(f"配置类型非法: {name} 须为数值") + if name in _INT_FIELDS: + if not isinstance(value, int): + raise ValueError(f"配置类型非法: {name} 须为整数") + elif not isinstance(value, (int, float)): + raise ValueError(f"配置类型非法: {name} 须为数值") return StereoConfig(**data) -def _require(ok: bool, field_name: str, message: str) -> None: - if not ok: +def _require(condition: bool, field_name: str, message: str) -> None: + if not condition: raise ValueError(f"配置非法: {field_name} {message}") diff --git a/utils/stereo_ranger/consistency.py b/utils/stereo_ranger/consistency.py new file mode 100644 index 0000000..7ff9100 --- /dev/null +++ b/utils/stereo_ranger/consistency.py @@ -0,0 +1,74 @@ +"""一致性监控（§6.7）：时序层面质量自检，每目标独立状态。 + +机载激光移除后无外部距离基准，飞行中只能靠自身时序一致性监控数据质量： +**只能降级、不能纠偏**（绝对精度靠地面静态验证）。匹配层面的质量（|Δy| +残差、框高一致性、代价门限）由匹配硬约束承担，本节只管时序层面。 +""" + +from dataclasses import dataclass +from typing import Dict, Hashable, Optional + + +@dataclass +class ConsistencyStatus: + """单目标单帧一致性判定结果（§6.7）。""" + + jump_px: Optional[float] # |D − 上次 D|；首帧无参考为 None + abnormal: bool # 本帧是否跳变超门限 + streak: int # 连续异常帧数（含本帧，正常帧清零） + confidence_degraded: bool # streak ≥ N：置信度降级 + invalid: bool # streak ≥ M：质量语义判无效 + + +class ConsistencyMonitor: + """每目标：上次视差 + 连续异常计数（§11.1 必须持久化的状态）。""" + + def __init__(self, jump_px: float, degrade_frames: int, invalid_frames: int) -> None: + if jump_px <= 0.0: + raise ValueError("jump_px 必须为正") + if degrade_frames < 1: + raise ValueError("degrade_frames 必须 ≥ 1") + if invalid_frames < degrade_frames: + raise ValueError("invalid_frames 必须 ≥ degrade_frames") + self._jump_px = jump_px + self._degrade_frames = degrade_frames + self._invalid_frames = invalid_frames + self._last_d: Dict[Hashable, float] = {} + self._streak: Dict[Hashable, int] = {} + + def advance(self, key: Hashable, disparity_px: float) -> ConsistencyStatus: + """推进目标 key 的一致性判定（配对成功帧每帧一次）。""" + last = self._last_d.get(key) + if last is None: + jump: Optional[float] = None + abnormal = False + streak = 0 + else: + jump = abs(disparity_px - last) + abnormal = jump > self._jump_px + streak = self._streak.get(key, 0) + 1 if abnormal else 0 + self._last_d[key] = disparity_px + if streak: + self._streak[key] = streak + else: + self._streak.pop(key, None) + return ConsistencyStatus( + jump_px=jump, + abnormal=abnormal, + streak=streak, + confidence_degraded=streak >= self._degrade_frames, + invalid=streak >= self._invalid_frames, + ) + + def streak(self, key: Hashable) -> int: + """目标 key 当前连续异常计数。""" + return self._streak.get(key, 0) + + def drop(self, key: Hashable) -> None: + """清理目标 key 的状态（目标老化清理时调用）。""" + self._last_d.pop(key, None) + self._streak.pop(key, None) + + def clear(self) -> None: + self._last_d.clear() + self._streak.clear() diff --git a/utils/stereo_ranger/distance_filter.py b/utils/stereo_ranger/distance_filter.py new file mode 100644 index 0000000..c30ca50 --- /dev/null +++ b/utils/stereo_ranger/distance_filter.py @@ -0,0 +1,68 @@ +"""时间滤波器（§6.5）：一阶低通 + 单帧跳变重置，每目标独立状态。 + +- 单帧毛刺由跳变重置处理，持续性异常由一致性监控（§6.7）处理，两者职责 + 不同不得合并； +- 滤波只在**有效帧**之间进行，无效/缺席帧不参与平滑；α = 1 等效不滤波； +- 连续失效（无效或缺席）达 filter_reset_frames 帧清空状态，下次有效帧重新 + 初始化（防止用陈旧状态平滑新测量）。 +""" + +from typing import Dict, Hashable, Optional + + +class DistanceFilter: + """每目标：上次滤波值 + 连续失效计数（§11.1 必须持久化的状态）。""" + + def __init__(self, alpha: float, jump_reset_m: float, reset_frames: int) -> None: + if not 0.0 < alpha <= 1.0: + raise ValueError("alpha 须落在 (0, 1]") + if jump_reset_m <= 0.0: + raise ValueError("jump_reset_m 必须为正") + if reset_frames < 1: + raise ValueError("reset_frames 必须 ≥ 1") + self._alpha = alpha + self._jump_reset_m = jump_reset_m + self._reset_frames = reset_frames + self._values: Dict[Hashable, float] = {} + self._invalid_streak: Dict[Hashable, int] = {} + + def advance(self, key: Hashable, z_m: float) -> float: + """有效帧推进：未初始化→直接置值；跳变超门限→重置；否则一阶低通。""" + previous = self._values.get(key) + self._invalid_streak.pop(key, None) + if previous is None: + self._values[key] = z_m + return z_m + if abs(z_m - previous) > self._jump_reset_m: + self._values[key] = z_m # 直接重置而非滤波，防止拖尾 + return z_m + smoothed = previous + self._alpha * (z_m - previous) + self._values[key] = smoothed + return smoothed + + def invalidate(self, key: Hashable) -> None: + """无效/缺席帧：连续失效达门限后清空该目标滤波状态（§6.5 记失效）。""" + if key not in self._values: + return + streak = self._invalid_streak.get(key, 0) + 1 + if streak >= self._reset_frames: + self._values.pop(key, None) + self._invalid_streak.pop(key, None) + else: + self._invalid_streak[key] = streak + + def value(self, key: Hashable) -> Optional[float]: + """目标 key 当前滤波值；未初始化/已清空为 None。""" + return self._values.get(key) + + def invalid_streak(self, key: Hashable) -> int: + """目标 key 当前连续失效计数（内部状态，供测试/诊断）。""" + return self._invalid_streak.get(key, 0) + + def drop(self, key: Hashable) -> None: + self._values.pop(key, None) + self._invalid_streak.pop(key, None) + + def clear(self) -> None: + self._values.clear() + self._invalid_streak.clear() diff --git a/utils/stereo_ranger/geometry.py b/utils/stereo_ranger/geometry.py new file mode 100644 index 0000000..32156b3 --- /dev/null +++ b/utils/stereo_ranger/geometry.py @@ -0,0 +1,84 @@ +"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。 + +Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达； +R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目 +中心，§6.6，逐点精确、无近似）。 +""" + +import math +from dataclasses import dataclass +from typing import Optional, Tuple + +from .config import StereoConfig +from .types import Detection + +DELTA_D_PX = 1.0 # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)） + + +@dataclass(frozen=True) +class RangeSolution: + """测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。""" + + z_m: float + x_m: float + y_m: float + r_m: float # 斜距（参考值，不进控制回路） + + +@dataclass(frozen=True) +class ValidityDecision: + """有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。""" + + in_domain: bool # ① 域内：D ∈ [D_min, D_max] + budget_ok: bool # ② 误差预算：δZ(δD = 1px) ≤ δZ_max + valid: bool # ① 且 ② + + +def forward_distance_m(disparity_px: float, config: StereoConfig) -> float: + """Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。""" + if disparity_px <= 0.0: + raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}") + return config.focal_baseline_px_m / disparity_px + + +def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution: + """由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。""" + z_m = forward_distance_m(disparity_px, config) + x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px + y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px + r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m) + return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m) + + +def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]: + """虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。 + + 与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。 + """ + return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0) + + +def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float: + """绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。""" + return z_m * z_m * delta_d_px / config.focal_baseline_px_m + + +def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float: + """相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。""" + return z_m * delta_d_px / config.focal_baseline_px_m + + +def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision: + """有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。""" + in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px + budget_ok = ( + z_m is not None + and math.isfinite(z_m) + and range_error_abs_m(z_m, config) <= config.max_delta_z_m + ) + return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok) + + +def confidence_from_cost(cost: float) -> float: + """匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。""" + return min(1.0, max(0.0, 1.0 - cost)) diff --git a/utils/stereo_ranger/matching.py b/utils/stereo_ranger/matching.py new file mode 100644 index 0000000..74aafee --- /dev/null +++ b/utils/stereo_ranger/matching.py @@ -0,0 +1,82 @@ +"""左右匹配器（§6.2）：硬约束 + 代价升序贪心一一配对；接口注入。 + +默认 GreedyMatcher 为纯函数（§4 构件表）；调用方可通过 +StereoRanger(matcher=...) 注入匈牙利等替换实现而不动调用方（§10.3）。 + +硬约束（任一不满足直接拒绝配对，不进代价排序）：类别一致、|Δy| ≤ τ_y、 +框高相对差 ≤ τ_h。视差域 [D_min, D_max] 不参与配对：越界属有效性门 +（§6.2 已回填，与 §5 失败路径表/§6.3/§6.4/§2.3 一致）。 +""" + +from dataclasses import dataclass +from typing import List, Protocol, Sequence + +from .config import StereoConfig +from .sampling import SamplePoint + + +@dataclass(frozen=True) +class MatchPair: + """一一配对结果：左右下标 + 匹配代价（§6.2）。""" + + left_index: int + right_index: int + cost: float + + +class Matcher(Protocol): + """匹配器接口（§4：接口注入，默认贪心）。""" + + def match( + self, + left: Sequence[SamplePoint], + right: Sequence[SamplePoint], + config: StereoConfig, + ) -> List[MatchPair]: + ... + + +class GreedyMatcher: + """硬约束过滤 + 代价升序贪心：cost < 1 才接受，一一配对。""" + + def match( + self, + left: Sequence[SamplePoint], + right: Sequence[SamplePoint], + config: StereoConfig, + ) -> List[MatchPair]: + tau_y = config.tau_y_px + tau_h = config.tau_h_ratio + candidates = [] + for i, point_l in enumerate(left): + if not point_l.finite: + continue + for j, point_r in enumerate(right): + if not point_r.finite: + continue + if point_l.box.class_id != point_r.box.class_id: + continue + dy = abs(point_l.center_y - point_r.center_y) + if dy > tau_y: + continue + height_max = max(point_l.box.height, point_r.box.height) + if height_max <= 0.0: + continue + dh_ratio = abs(point_l.box.height - point_r.box.height) / height_max + if dh_ratio > tau_h: + continue + cost = dy / tau_y + dh_ratio / tau_h + if cost >= 1.0: + continue + candidates.append((cost, i, j)) + candidates.sort(key=lambda item: (item[0], item[1], item[2])) + used_left = set() + used_right = set() + pairs: List[MatchPair] = [] + for cost, i, j in candidates: + if i in used_left or j in used_right: + continue + pairs.append(MatchPair(left_index=i, right_index=j, cost=cost)) + used_left.add(i) + used_right.add(j) + return pairs diff --git a/utils/stereo_ranger/sampling.py b/utils/stereo_ranger/sampling.py new file mode 100644 index 0000000..c006e46 --- /dev/null +++ b/utils/stereo_ranger/sampling.py @@ -0,0 +1,73 @@ +"""视差取点器（§6.1）：同侧（左）边缘为主方案，边缘视差越窗时回退框中心。 + +纯函数、无状态（§4 构件表）。为什么用同侧边缘：同一物理目标在两目中的 +检测框宽并不严格相等，框中心视差会混入框宽差的一半，同侧边缘视差可抵消 +框宽项。 + +注：§6.1 的"左右两边缘各算一次视差取中值"精化在数学上恒等于框中心视差 +（两支的 median = 均值），与"中心视差混入框宽差"的理由矛盾，属原文笔误， +v1.0 不实现；规格 §6.1 已删除该项并注明（见包 README「实现决策」）。 +""" + +import math +from dataclasses import dataclass + +from .config import StereoConfig +from .types import Detection, DisparitySource + + +@dataclass(frozen=True) +class SamplePoint: + """单目单框取点结果：同侧边缘候选与框中心（§6.1/§6.6）。 + + finite=False 表示坐标非有限值：该框不可参与解算，由调用方丢弃并告警。 + """ + + box: Detection + edge_x: float # 同侧（左）边缘 x = box.x1 + center_x: float + center_y: float + finite: bool + + +@dataclass(frozen=True) +class Disparity: + """配对后的视差取点结果。""" + + d_px: float + source: DisparitySource + fallback_used: bool # 边缘视差越窗、回退中心（需告警节流，§6.1） + in_domain: bool # d ∈ [D_min, D_max]（含边界，§11.2） + positive: bool # d > 0（正视差是物理前提，§6.3） + + +def sample_point(detection: Detection) -> SamplePoint: + """逐框取点（§5 第 ② 步，每目每框一次）。""" + finite = all(math.isfinite(value) for value in (detection.x1, detection.y1, detection.x2, detection.y2)) + return SamplePoint( + box=detection, + edge_x=detection.x1, + center_x=detection.cx, + center_y=detection.cy, + finite=finite, + ) + + +def select_disparity(left: SamplePoint, right: SamplePoint, config: StereoConfig) -> Disparity: + """在同侧边缘视差与框中心视差之间选择（§6.1 主取点 + 越窗回退）。 + + 规则：边缘视差在域内取边缘；否则若中心视差在域内则回退中心（告警节流）； + 两者均越窗时保留边缘值（供诊断与违例告警），由有效性门判无效。 + """ + d_edge = left.edge_x - right.edge_x + if _in_domain(d_edge, config): + return Disparity(d_edge, DisparitySource.LEFT_EDGE, False, True, d_edge > 0.0) + d_center = left.center_x - right.center_x + if _in_domain(d_center, config): + return Disparity(d_center, DisparitySource.CENTER_FALLBACK, True, True, d_center > 0.0) + d = d_edge if math.isfinite(d_edge) else d_center + return Disparity(d, DisparitySource.LEFT_EDGE, False, False, d > 0.0) + + +def _in_domain(d_px: float, config: StereoConfig) -> bool: + return config.disparity_min_px <= d_px <= config.disparity_max_px diff --git a/utils/stereo_ranger/stereo_ranger.py b/utils/stereo_ranger/stereo_ranger.py index 12c6b0f..36b32c5 100644 --- a/utils/stereo_ranger/stereo_ranger.py +++ b/utils/stereo_ranger/stereo_ranger.py @@ -1,215 +1,227 @@ -"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。 +"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。 -输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做 -图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→ -Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。 -依赖仅为标准库。 +单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 → +④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。 + +输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像 +采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例 +不得并发调用 update()。 """ import logging -import math -import time -from typing import Dict, List, Optional, Sequence, Tuple +from typing import List, Optional, Sequence from .config import StereoConfig -from .types import BBoxXYXY, Detection, RangeReport +from .geometry import confidence_from_cost, evaluate_validity, solve_range, virtual_center_px +from .matching import GreedyMatcher, Matcher, MatchPair +from .sampling import SamplePoint, sample_point, select_disparity +from .target_store import TargetStore +from .types import Degradation, Detection, DetectionFrame, Eye, RangeRecord +from .warn_throttle import WarnThrottle logger = logging.getLogger("stereo_ranger") -DISP_SOURCE_EDGE = "left_edge" -DISP_SOURCE_CENTER = "center" - - -def focal_from_hfov(image_width_px: float, hfov_deg: float) -> float: - """由水平视场与图像宽近似像素焦距：F = (W/2) / tan(HFOV/2)（§5.3）。""" - return (image_width_px / 2.0) / math.tan(math.radians(hfov_deg) / 2.0) - - -def range_error_abs(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: - """绝对距离误差估计 δZ ≈ Z^2·δD/(F·B)（§5.2）。""" - return z_m * z_m * delta_d_px / (fx_px * baseline_m) - - -def range_error_rel(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: - """相对距离误差 δZ/Z ≈ Z·δD/(F·B)（§5.2）。""" - return z_m * delta_d_px / (fx_px * baseline_m) +_CONFIDENCE_DEGRADE_FACTOR = 0.5 # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4） class StereoRanger: - """双目稀疏目标测距器。 + """双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。""" - 用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。 - """ - - def __init__(self, config: Optional[StereoConfig] = None): - self._cfg = config if config is not None else StereoConfig() - self._z_state: Dict[Tuple[int, int], float] = {} # (cls_id, track_id) -> 滤波 Z - self._warn_at: Dict[str, float] = {} + def __init__(self, config: Optional[StereoConfig] = None, matcher: Optional[Matcher] = None) -> None: + self._config = config if config is not None else StereoConfig() + self._matcher = matcher if matcher is not None else GreedyMatcher() + self._store = TargetStore(self._config) + self._warn = WarnThrottle() + self._frame_index = 0 @property def config(self) -> StereoConfig: - return self._cfg + return self._config + + @property + def matcher(self) -> Matcher: + return self._matcher def reset(self) -> None: - self._z_state.clear() - self._warn_at.clear() + """清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。""" + self._store.clear() + self._warn.clear() + self._frame_index = 0 - def update( - self, - left: Sequence[Detection], - right: Sequence[Detection], - frame_sequence: int = 0, - timestamp_s: Optional[float] = None, - ) -> List[RangeReport]: - """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。 + def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]: + """单周期处理（§5）：返回本帧记录列表。 - 配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的 - valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False， - 仅发布单目方位（§6 失败处理）。 + 顺序：先按左目框顺序给出各自记录（配对→完整记录 / 未配对→仅方位）， + 再给出未配对的右目框的仅方位记录；左目存在时记录数与左目框数一致。 """ - pairs = self._match(left, right) - matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs} - reports: List[RangeReport] = [] - for l_idx, det_l in enumerate(left): - r_idx = matched_r.get(l_idx) - if r_idx is None: - reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s)) + config = self._config + if left.frame_id != right.frame_id: + self._warn_emit( + "frame_mismatch", + "左右帧标识不一致（L=%s, R=%s），拒收本帧", + left.frame_id, + right.frame_id, + ) + return [] + + self._frame_index += 1 + frame_index = self._frame_index + left_points = self._drop_invalid([sample_point(det) for det in left.detections]) + right_points = self._drop_invalid([sample_point(det) for det in right.detections]) + + pairs = self._matcher.match(left_points, right_points, config) + pair_of_left = {pair.left_index: pair for pair in pairs} + paired_right = {pair.right_index for pair in pairs} + + records: List[RangeRecord] = [] + for index, point_l in enumerate(left_points): + pair = pair_of_left.get(index) + if pair is None: + records.append(self._bearing_only_record(point_l, left.frame_id, now_ms, Eye.LEFT)) continue - report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s) - reports.append(report) - return reports + records.append( + self._complete_record( + point_l, + right_points[pair.right_index], + pair, + left.frame_id, + now_ms, + frame_index, + ) + ) + for index, point_r in enumerate(right_points): + if index in paired_right: + continue + records.append(self._bearing_only_record(point_r, left.frame_id, now_ms, Eye.RIGHT)) + + self._store.end_frame(frame_index) + return records # ---- 内部实现 ---- - def _match(self, left: Sequence[Detection], right: Sequence[Detection]) -> List[Tuple[int, int, Detection]]: - """硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。""" - cfg = self._cfg - d_lo, d_hi = cfg.disparity_bounds_px() - candidates: List[Tuple[int, int, Detection, float]] = [] - for i, det_l in enumerate(left): - for j, det_r in enumerate(right): - if det_l.cls_id != det_r.cls_id: - continue - dy = abs(det_l.cy - det_r.cy) - if dy > cfg.vertical_tau_px: - continue - h_max = max(det_l.height, det_r.height) - if h_max <= 0.0: - continue - dh_ratio = abs(det_l.height - det_r.height) / h_max - if dh_ratio > cfg.height_ratio_tau: - continue - cost = dy / cfg.vertical_tau_px + dh_ratio / cfg.height_ratio_tau - if cost >= 1.0: - continue - d_edge = det_l.x1 - det_r.x1 - if not (d_lo <= d_edge <= d_hi): - d_center = det_l.cx - det_r.cx - if cfg.use_center_fallback and (d_lo <= d_center <= d_hi): - self._warn("center_fallback", "左边缘视差越出窗口，回退中心视差（宽度差可能混入）") - else: - continue - same_track = det_l.track_id > 0 and det_l.track_id == det_r.track_id - candidates.append((0 if same_track else 1, cost, i, j, det_l)) - candidates.sort(key=lambda c: (c[0], c[1])) - matched_l: set = set() - used_r: set = set() - pairs: List[Tuple[int, int, Detection]] = [] - for _, _, i, j, det_l in candidates: - if i in matched_l or j in used_r: - continue - pairs.append((i, j, det_l)) - matched_l.add(i) - used_r.add(j) - return pairs + def _complete_record( + self, + point_l: SamplePoint, + point_r: SamplePoint, + pair: MatchPair, + frame_id: int, + now_ms: int, + frame_index: int, + ) -> RangeRecord: + """配对成功的记录：测距 + 方位源 + 有效性（门①②）+ 一致性（门③）+ 滤波。""" + config = self._config + box_l = point_l.box + box_r = point_r.box + disparity = select_disparity(point_l, point_r, config) + center_mid = virtual_center_px(box_l, box_r) - def _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport: - cfg = self._cfg - d_edge = det_l.x1 - det_r.x1 - d_lo, d_hi = cfg.disparity_bounds_px() - if d_lo <= d_edge <= d_hi: - d = d_edge - source = DISP_SOURCE_EDGE - else: - d = det_l.cx - det_r.cx - source = DISP_SOURCE_CENTER - z = cfg.fx_px * cfg.baseline_m / d - report = RangeReport( - frame_sequence=seq, - timestamp_s=ts, - class_id=det_l.cls_id, - track_id=det_l.track_id if det_l.track_id != 0 else det_r.track_id, - matched=True, - valid=False, # §5.4 判定见下 - forward_distance_m=z, - disparity_px=d, - disparity_source=source, - confidence=min(det_l.conf, det_r.conf), - centre_pixel_x_mid=(det_l.cx + det_r.cx) / 2.0, - centre_pixel_y_mid=det_l.cy, - ref_bbox=(det_l.x1, det_l.y1, det_l.x2, det_l.y2), + if disparity.fallback_used: + self._warn_emit("center_fallback", "边缘视差越窗，回退框中心（框宽差可能混入视差）") + if not disparity.positive: + self._warn_emit("disparity_violation", "视差违例 D=%.2f px（须为正），该目标判无效", disparity.d_px) + elif not disparity.in_domain: + self._warn_emit( + "disparity_violation", + "视差超界 D=%.2f px（域 [%.2f, %.2f]），该目标判无效", + disparity.d_px, + config.disparity_min_px, + config.disparity_max_px, + ) + + key = self._store.resolve( + box_l.class_id, + _shared_track_id(box_l, box_r), + center_mid, + frame_index, ) - err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) - if d >= cfg.disparity_min_px and err <= cfg.range_tol_m: - report.valid = True - z_out = self._filter_z(report.class_id, report.track_id, z) - report.forward_distance_m = z_out - report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) + status = self._store.consistency.advance(key, disparity.d_px) + if status.invalid: + self._warn_emit( + "consistency_invalid", + "连续 %d 帧视差跳变超门限（%.1f px），质量判无效", + status.streak, + config.consistency_jump_px, + ) + + z_raw = config.focal_baseline_px_m / disparity.d_px if disparity.positive else None + solution = solve_range(box_l, box_r, disparity.d_px, config) if z_raw is not None else None + decision = evaluate_validity(disparity.d_px, z_raw, config) + valid = decision.valid and not status.invalid + + confidence = confidence_from_cost(pair.cost) + if status.confidence_degraded: + confidence *= _CONFIDENCE_DEGRADE_FACTOR + if status.invalid: + confidence = 0.0 + + if valid: + z_out = self._store.distance_filter.advance(key, z_raw) else: - report.error_est_m = err - self._fill_xyz(report, det_l.cx) - return report + self._store.distance_filter.invalidate(key) + z_out = None - def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None: - """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。 + # 参考斜距与已发布的前向距离同基准：X/Y/Z 均 ∝ Z，按 z_out/z_raw 缩放即精确换基 + slant_range_m = None + if solution is not None: + slant_range_m = solution.r_m if z_out is None else solution.r_m * (z_out / z_raw) - 注意：方位源 centre_pixel_x_mid 属于基线中点虚拟相机（§11.3）， - 而 X 定义在左目坐标系，二者相差 B/2，不得混用。 - """ - cfg = self._cfg - z = report.forward_distance_m - x = (x_l_px - cfg.cx_px) * z / cfg.fx_px - y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px - report.lateral_m = x - report.vertical_m = y - report.slant_range_m = math.sqrt(x * x + y * y + z * z) + return RangeRecord( + frame_id=frame_id, + class_id=box_l.class_id, + degradation=Degradation.COMPLETE, + valid=valid, + track_id=box_l.track_id if box_l.track_id is not None else box_r.track_id, + forward_distance_m=z_out, + slant_range_m=slant_range_m, + disparity_px=disparity.d_px, + center_mid_px=center_mid, + reference_box=box_l.bbox, + reference_eye=Eye.LEFT, + confidence=confidence, + timestamp_ms=now_ms, + ) - def _filter_z(self, cls_id: int, track_id: int, z: float) -> float: - """可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。""" - cfg = self._cfg - if not cfg.filter_enabled or track_id == 0: - return z - key = (cls_id, track_id) - prev = self._z_state.get(key) - if prev is None: - self._z_state[key] = z - return z - if abs(z - prev) / prev > cfg.filter_jump_ratio: - self._z_state[key] = z # 跳变：重置 - return z - z_out = (1.0 - cfg.filter_alpha) * prev + cfg.filter_alpha * z - self._z_state[key] = z_out - return z_out - - def _unmatched_report(self, det: Detection, seq: int, ts: Optional[float]) -> RangeReport: - return RangeReport( - frame_sequence=seq, - timestamp_s=ts, - class_id=det.cls_id, - track_id=det.track_id, - matched=False, + def _bearing_only_record( + self, + point: SamplePoint, + frame_id: int, + now_ms: int, + eye: Eye, + ) -> RangeRecord: + """单目退化记录（§5 第 ⑤ 步）：仅方位，valid = false，无距离。""" + detection = point.box + return RangeRecord( + frame_id=frame_id, + class_id=detection.class_id, + degradation=Degradation.BEARING_ONLY, valid=False, - confidence=det.conf, - centre_pixel_x_mid=det.cx, - centre_pixel_y_mid=det.cy, - ref_bbox=(det.x1, det.y1, det.x2, det.y2), + track_id=detection.track_id, + center_mid_px=(detection.cx, detection.cy), + reference_box=detection.bbox, + reference_eye=eye, + confidence=detection.confidence, + timestamp_ms=now_ms, ) - def _warn(self, category: str, message: str) -> None: - """节流诊断（§7：失败日志须节流）。""" - now = time.monotonic() - last = self._warn_at.get(category) - if last is not None and now - last < self._cfg.warn_interval_s: + def _drop_invalid(self, points: Sequence[SamplePoint]) -> List[SamplePoint]: + """丢弃坐标非有限值的框（不可参与匹配，也不发布记录），告警节流。""" + kept: List[SamplePoint] = [] + for point in points: + if point.finite: + kept.append(point) + else: + self._warn_emit("invalid_box", "检测框坐标非法（非有限值），已丢弃该框") + return kept + + def _warn_emit(self, category: str, message: str, *args: object) -> None: + """异常日志（§5 节流规范：第 1 次 + 每满 100 次，带累计计数）。""" + if not self._warn.should_emit(category): return - self._warn_at[category] = now - logger.warning("[%s] %s", category, message) + logger.warning("[%s] " + message + "（累计 %d 次）", category, *args, self._warn.count(category)) + + +def _shared_track_id(box_l: Detection, box_r: Detection) -> Optional[int]: + """左右目轨道关联一致且有效时返回该 id（目标隔离键优先路径，§4）。""" + if box_l.track_id is not None and box_l.track_id > 0 and box_l.track_id == box_r.track_id: + return box_l.track_id + return None diff --git a/utils/stereo_ranger/target_store.py b/utils/stereo_ranger/target_store.py new file mode 100644 index 0000000..6ed3c8c --- /dev/null +++ b/utils/stereo_ranger/target_store.py @@ -0,0 +1,119 @@ +"""目标状态隔离（§4）：track_id 优先 + 框位置最近邻兜底。 + +目标隔离键：track_id 可用（左右一致且 > 0）时优先以 (track, 类别, track_id) +为键——上游 target_tracker 已在链路中，规格 §4 已按此冻结；否则回退类别 + +框位置最近邻（isolation_gate_px 门限，见包 README「实现决策」）。 + +连续 filter_reset_frames 帧未再出现的目标状态整体清理（位置 + 距离滤波 + +一致性），避免最近邻把新目标接到陈旧状态上。 +""" + +import math +from dataclasses import dataclass +from typing import Dict, Hashable, Optional, Tuple + +from .config import StereoConfig +from .consistency import ConsistencyMonitor +from .distance_filter import DistanceFilter + +_KIND_TRACK = "track" +_KIND_NEAREST = "nn" + + +@dataclass +class _TargetState: + class_id: int + track_id: Optional[int] + center_px: Tuple[float, float] + last_seen_frame: int + + +class TargetStore: + """每目标状态容器：隔离键解析 + 帧末老化清理（§11.1 状态清单）。""" + + def __init__(self, config: StereoConfig) -> None: + self._config = config + self._distance_filter = DistanceFilter( + config.smooth_alpha, config.jump_reset_m, config.filter_reset_frames + ) + self._consistency = ConsistencyMonitor( + config.consistency_jump_px, config.consistency_N_frames, config.consistency_M_frames + ) + self._states: Dict[Hashable, _TargetState] = {} + self._next_nearest_id = 0 + + @property + def distance_filter(self) -> DistanceFilter: + return self._distance_filter + + @property + def consistency(self) -> ConsistencyMonitor: + return self._consistency + + @property + def count(self) -> int: + """当前存活的目标状态数。""" + return len(self._states) + + def resolve( + self, + class_id: int, + track_id: Optional[int], + center_px: Tuple[float, float], + frame_index: int, + ) -> Hashable: + """解析（或新建）目标 key，并刷新该目标最近位置与出现帧。""" + if track_id is not None and track_id > 0: + key: Hashable = (_KIND_TRACK, class_id, track_id) + self._states[key] = _TargetState(class_id, track_id, center_px, frame_index) + return key + key = self._nearest_key(class_id, center_px, frame_index) + if key is None: + self._next_nearest_id += 1 + key = (_KIND_NEAREST, class_id, self._next_nearest_id) + self._states[key] = _TargetState(class_id, None, center_px, frame_index) + else: + state = self._states[key] + state.center_px = center_px + state.last_seen_frame = frame_index + return key + + def end_frame(self, frame_index: int) -> None: + """帧末收尾：未出现的目标记滤波失效，到期整体清理三类状态。""" + stale: List[Hashable] = [] + for key, state in self._states.items(): + if state.last_seen_frame == frame_index: + continue + self._distance_filter.invalidate(key) + if frame_index - state.last_seen_frame >= self._config.filter_reset_frames: + stale.append(key) + for key in stale: + del self._states[key] + self._distance_filter.drop(key) + self._consistency.drop(key) + + def clear(self) -> None: + self._states.clear() + self._distance_filter.clear() + self._consistency.clear() + self._next_nearest_id = 0 + + def _nearest_key( + self, + class_id: int, + center_px: Tuple[float, float], + frame_index: int, + ) -> Optional[Hashable]: + """同类别、isolation_gate_px 内、本帧尚未被占用的最近状态。""" + gate = self._config.isolation_gate_px + best_key: Optional[Hashable] = None + best_distance: Optional[float] = None + for key, state in self._states.items(): + if key[0] != _KIND_NEAREST or state.class_id != class_id: + continue + if state.last_seen_frame == frame_index: + continue # 本帧已被其他框接管，一一归属 + distance = math.hypot(center_px[0] - state.center_px[0], center_px[1] - state.center_px[1]) + if distance <= gate and (best_distance is None or distance < best_distance): + best_key, best_distance = key, distance + return best_key diff --git a/utils/stereo_ranger/types.py b/utils/stereo_ranger/types.py index fc964b2..e3b9b54 100644 --- a/utils/stereo_ranger/types.py +++ b/utils/stereo_ranger/types.py @@ -1,29 +1,50 @@ -"""stereo_ranger 输入输出数据结构：Detection / RangeReport。 +"""stereo_ranger 输入输出数据结构（纯值语义）。 -规格：docs/双目测距方案.md §10 结果记录。 +规格：docs/双目测距技术路线.md —— §2 输入输出接口、§3 符号约定、§6.6 方位源。 + +约定：所有像素坐标位于**已立体校正**的图像坐标系，检测框已映射回该基准 +（§2.1 校正契约 / 坐标基准一致）。本包仅依赖标准库。 """ -from dataclasses import dataclass -from typing import Optional, Tuple +from dataclasses import dataclass, field +from enum import Enum, auto +from typing import Optional, Sequence, Tuple BBoxXYXY = Tuple[float, float, float, float] +class Eye(Enum): + """物理目（参考目框来源，§2.2）。""" + + LEFT = auto() # 左目（解算参考系） + RIGHT = auto() # 右目 + + +class DisparitySource(Enum): + """视差取点来源（§6.1）。""" + + LEFT_EDGE = auto() # 同侧（左）边缘：v1.0 主方案 + CENTER_FALLBACK = auto() # 边缘视差越窗后回退框中心 + + +class Degradation(Enum): + """记录退化标记（§2.2）。描述记录的来源形态，质量由 valid 单独表达。""" + + COMPLETE = auto() # 完整：配对成功（距离 + 方位两路都在） + BEARING_ONLY = auto() # 仅方位：单目检出，无距离 + + @dataclass class Detection: - """校正后原生坐标系下的一个检测框（单位 px）。 - - 调用方负责：左右目预处理几何完全一致，且框坐标已映射回原生分辨率 - （规格 §4.3）。x1/y1 为左上角，x2/y2 为右下角（x1 即"左边缘"）。 - """ + """校正后图像坐标系下的一个检测框（px；x1/y1 左上角，x2/y2 右下角）。""" x1: float y1: float x2: float y2: float - conf: float = 1.0 - cls_id: int = 0 - track_id: int = 0 # 未知为 0 + confidence: float = 1.0 + class_id: int = 0 + track_id: Optional[int] = None # 检测侧轨道关联；None = 无（§2.2，v1.0 可无） @property def width(self) -> float: @@ -41,31 +62,46 @@ class Detection: def cy(self) -> float: return (self.y1 + self.y2) / 2.0 + @property + def bbox(self) -> BBoxXYXY: + return (self.x1, self.y1, self.x2, self.y2) + @dataclass -class RangeReport: - """单目标单帧测距结果（一个左目检测对应一条记录）。 +class DetectionFrame: + """单目一帧：帧标识 + 检测框列表。 - matched=False 表示该左目检测本帧未配对：valid 恒为 False，距离字段为 - None，centre_pixel_x/y_mid 为单目自身中心（方位回退，规格 §11.3）。 - matched=True 且 valid=False 表示已配对但距离超出有效工作域 - （规格 §5.4）：距离字段仍给出估计值供诊断，方位照常发布。 + 同帧契约（§2.1）：左右两帧的 frame_id 必须一致，否则整帧拒收、输出空。 """ - frame_sequence: int + frame_id: int + detections: Sequence[Detection] = field(default_factory=tuple) + + +@dataclass +class RangeRecord: + """目标距离记录（§2.2，字段与文档表格逐项对应）。 + + - forward_distance_m（Z）：唯一供距离通道消费的量；valid=False 或单目 + 退化时为 None（不得强行估计，§12.3 红线 3）； + - slant_range_m（R）：参考值，不进控制回路；无效但仍可解算时保留原始 + 测量值供诊断； + - disparity_px（D）：本次视差（调试与交叉校验用）； + - center_mid_px（c_mid）：方位源；配对记录为两目框中心等权平均（虚拟 + 双目中心，§6.6），单目退化为该目框中心； + - reference_box / reference_eye：参考目框（配对记录恒为左目）。 + """ + + frame_id: int class_id: int - track_id: int - matched: bool + degradation: Degradation valid: bool - forward_distance_m: Optional[float] = None # Z，前向分量（主输出） - lateral_m: Optional[float] = None # X，左目系横向偏移 - vertical_m: Optional[float] = None # Y，左目系纵向偏移 - slant_range_m: Optional[float] = None # R = sqrt(X^2 + Y^2 + Z^2) - disparity_px: Optional[float] = None # D = xL - xR（>0 为前方） - disparity_source: Optional[str] = None # "left_edge" / "center" + track_id: Optional[int] = None + forward_distance_m: Optional[float] = None + slant_range_m: Optional[float] = None + disparity_px: Optional[float] = None + center_mid_px: Tuple[float, float] = (0.0, 0.0) + reference_box: Optional[BBoxXYXY] = None + reference_eye: Eye = Eye.LEFT confidence: float = 0.0 - error_est_m: Optional[float] = None # δZ 估计（δD = 配置 disp_noise_px） - centre_pixel_x_mid: float = 0.0 # 虚拟双目中心 x（方位源，§11.3） - centre_pixel_y_mid: float = 0.0 # y_mid（校正后 yL == yR） - ref_bbox: Optional[BBoxXYXY] = None # 左目（参考目）框，供诊断 - timestamp_s: Optional[float] = None + timestamp_ms: int = 0 diff --git a/utils/stereo_ranger/warn_throttle.py b/utils/stereo_ranger/warn_throttle.py new file mode 100644 index 0000000..3845a79 --- /dev/null +++ b/utils/stereo_ranger/warn_throttle.py @@ -0,0 +1,30 @@ +"""告警节流器（§4/§5）：每类异常"第 1 次 + 每满 100 次"打印，带累计计数。 + +高频热路径（每帧数学）不打日志；所有异常日志走本通道，且不落在数学热路径 +的决策上（§11.3 单周期处理无阻塞、无 IO）。 +""" + +from typing import Dict + + +class WarnThrottle: + """按异常类独立计数与限频。""" + + def __init__(self, every: int = 100) -> None: + if every < 1: + raise ValueError("every 必须 ≥ 1") + self._every = every + self._counts: Dict[str, int] = {} + + def should_emit(self, category: str) -> bool: + """推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。""" + count = self._counts.get(category, 0) + 1 + self._counts[category] = count + return count == 1 or count % self._every == 0 + + def count(self, category: str) -> int: + """该类异常的累计次数。""" + return self._counts.get(category, 0) + + def clear(self) -> None: + self._counts.clear() diff --git a/utils/tests/stereo_ranger/config_test.py b/utils/tests/stereo_ranger/config_test.py deleted file mode 100644 index 612dd4a..0000000 --- a/utils/tests/stereo_ranger/config_test.py +++ /dev/null @@ -1,76 +0,0 @@ -"""StereoConfig 默认值、派生区间与 JSON 加载测试（规格 §2：参数全部外置）。""" - -import json - -import pytest - -from stereo_ranger.config import StereoConfig, load_from_json - -VALID = { - "fx_px": 1663.0, "fy_px": 1663.0, "cx_px": 960.0, "cy_px": 540.0, - "baseline_m": 0.12, "z_min_m": 2.0, "z_max_m": 40.0, - "vertical_tau_px": 3.0, "height_ratio_tau": 0.3, - "disparity_min_px": 4.0, "range_tol_m": 3.0, "disp_noise_px": 1.5, - "use_center_fallback": False, "filter_enabled": True, - "filter_alpha": 0.5, "filter_jump_ratio": 0.1, "warn_interval_s": 1.0, -} - - -def test_defaults_and_disparity_window(): - cfg = StereoConfig() - assert cfg.baseline_m == 0.06 and cfg.fx_px == 960.0 - # FB = 57.6，z 窗口 [1, 50] m -> D 窗口 [57.6/50, 57.6/1] - lo, hi = cfg.disparity_bounds_px() - assert lo == pytest.approx(1.152) - assert hi == pytest.approx(57.6) - - -@pytest.mark.parametrize("kwargs", [ - {"baseline_m": 0.0}, - {"fx_px": -1.0}, - {"fy_px": 0.0}, - {"cx_px": -1.0}, - {"z_min_m": 0.0}, - {"z_max_m": 10.0, "z_min_m": 20.0}, - {"vertical_tau_px": 0.0}, - {"height_ratio_tau": 0.0}, - {"disparity_min_px": 0.0}, - {"range_tol_m": 0.0}, - {"disp_noise_px": 0.0}, - {"filter_alpha": 0.0}, - {"filter_alpha": 1.5}, - {"filter_jump_ratio": -0.1}, - {"warn_interval_s": -1.0}, -]) -def test_invalid_config_raises(kwargs): - with pytest.raises(ValueError): - StereoConfig(**kwargs) - - -def test_load_from_json_roundtrip(tmp_path): - p = tmp_path / "stereo.json" - p.write_text(json.dumps(VALID), encoding="utf-8") - cfg = load_from_json(str(p)) - for field, value in VALID.items(): - assert getattr(cfg, field) == value - lo, hi = cfg.disparity_bounds_px() - assert hi == pytest.approx(1663.0 * 0.12 / 2.0) # FB / z_min - assert lo == pytest.approx(1663.0 * 0.12 / 40.0) # FB / z_max - - -def test_load_from_json_missing_field_reports_name(tmp_path): - p = tmp_path / "stereo.json" - data = dict(VALID) - del data["baseline_m"] - p.write_text(json.dumps(data), encoding="utf-8") - with pytest.raises(ValueError, match="baseline_m"): - load_from_json(str(p)) - - -def test_load_from_json_unknown_field_rejected(tmp_path): - p = tmp_path / "stereo.json" - data = dict(VALID) - data["magic_extra"] = 1 - p.write_text(json.dumps(data), encoding="utf-8") - with pytest.raises(ValueError, match="magic_extra"): - load_from_json(str(p)) diff --git a/utils/tests/stereo_ranger/stereo_ranger_test.py b/utils/tests/stereo_ranger/stereo_ranger_test.py deleted file mode 100644 index 6d69a97..0000000 --- a/utils/tests/stereo_ranger/stereo_ranger_test.py +++ /dev/null @@ -1,302 +0,0 @@ -"""stereo_ranger 测距核心单元测试。 - -规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、 -§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框， -再反推验证测距/方位输出。 -""" - -import math - -import pytest - -from stereo_ranger.config import StereoConfig -from stereo_ranger.stereo_ranger import ( - DISP_SOURCE_CENTER, - DISP_SOURCE_EDGE, - StereoRanger, - focal_from_hfov, - range_error_abs, - range_error_rel, -) -from stereo_ranger.types import Detection - -CFG = StereoConfig() -FB = CFG.fx_px * CFG.baseline_m # 57.6 px·m - - -def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0): - return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2, - conf, cls_id, track_id) - - -def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0): - """目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。""" - u_l = CFG.fx_px * x_m / z_m + CFG.cx_px - v_l = CFG.fy_px * y_m / z_m + CFG.cy_px - u_r = u_l - FB / z_m - return (box(u_l, v_l, w, h, cls_id, conf, track_id), - box(u_r, v_l, w, h, cls_id, conf, track_id)) - - -def one_report(ranger, left, right, **kw): - reports = ranger.update(left, right, **kw) - assert len(reports) == len(left) - return reports - - -# ---- §4.2 / §5.3 几何与误差公式 ---- - -def test_focal_from_hfov_matches_doc_examples(): - assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0) - assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769) - - -def test_range_error_matches_doc_table(): - # 文档 §5.3 示意表：B=0.06, F=960, δD=1 px - expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444, - 30: 15.625, 50: 43.4028, 100: 173.611} - for z, err in expected.items(): - assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3) - assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3) - - -def test_on_axis_target_z_and_bearing(): - left, right = target_pair(0.0, 0.0, 10.0) - r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0] - assert r.matched and r.valid - assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) - assert r.disparity_px == pytest.approx(FB / 10.0) - assert r.disparity_source == DISP_SOURCE_EDGE - assert r.lateral_m == pytest.approx(0.0, abs=1e-9) - assert r.vertical_m == pytest.approx(0.0, abs=1e-9) - assert r.slant_range_m == pytest.approx(10.0, rel=1e-9) - assert r.frame_sequence == 7 and r.timestamp_s == 1.5 - # 虚拟双目中心：两目中心平均，y 取左目（§11.3） - assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0) - assert r.centre_pixel_y_mid == left.cy - - -def test_off_axis_target_xyz_and_slant_range(): - left, right = target_pair(1.2, 0.3, 10.0) - r = one_report(StereoRanger(), [left], [right])[0] - assert r.valid - assert r.lateral_m == pytest.approx(1.2, rel=1e-9) - assert r.vertical_m == pytest.approx(0.3, rel=1e-9) - assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9) - assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px)) - - -def test_x_uses_left_camera_frame_not_virtual_center(): - # X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3） - left, right = target_pair(0.0, 0.0, 10.0) - r = one_report(StereoRanger(), [left], [right])[0] - assert r.lateral_m == pytest.approx(0.0, abs=1e-9) # 目标在左光轴上 - assert r.centre_pixel_x_mid < left.cx # 但虚拟中心相对左光轴偏左 B/2 - - -def test_width_diff_on_right_edge_only_does_not_taint_left_edge(): - # §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽 - # 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。 - left, right = target_pair(0.0, 0.0, 10.0) - right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2) - r = one_report(StereoRanger(), [left], [right_extended])[0] - assert r.matched and r.valid - assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) - assert r.disparity_source == DISP_SOURCE_EDGE - assert right_extended.width != left.width # 确有宽度差 - - -def test_center_fallback_when_edge_out_of_window(caplog): - # 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏 - left, right = target_pair(0.0, 0.0, 10.0) - left_wider = box(left.cx, left.cy, w=300.0, h=60.0) - ranger = StereoRanger() - r = one_report(ranger, [left_wider], [right])[0] - r2 = one_report(ranger, [left_wider], [right])[0] - assert r.matched and r.valid - assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) - assert r.disparity_source == DISP_SOURCE_CENTER - assert r2.disparity_source == DISP_SOURCE_CENTER - assert len(caplog.records) == 1 - - -def test_center_fallback_disabled_leaves_unmatched(): - left, right = target_pair(0.0, 0.0, 10.0) - left_wider = box(left.cx, left.cy, w=300.0, h=60.0) - cfg = StereoConfig(use_center_fallback=False) - r = one_report(StereoRanger(cfg), [left_wider], [right])[0] - assert not r.matched and not r.valid - assert r.forward_distance_m is None - - -# ---- §6 关联硬约束与贪心一一配对 ---- - -def test_class_mismatch_rejected(): - left, _ = target_pair(0.0, 0.0, 10.0) - _, right = target_pair(0.0, 0.0, 10.0, cls_id=1) - r = one_report(StereoRanger(), [left], [right])[0] - assert not r.matched - - -def test_vertical_residual_beyond_tau_rejected(): - left, _ = target_pair(0.0, 0.0, 10.0) - right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0) # |Δy| = 3 > τy = 2 - r = one_report(StereoRanger(), [left], [right])[0] - assert not r.matched - - -def test_height_ratio_beyond_tau_rejected(): - left, _ = target_pair(0.0, 0.0, 10.0) - _, right = target_pair(0.0, 0.0, 10.0, h=40.0) # |60-40|/60 > 0.25 - r = one_report(StereoRanger(), [left], [right])[0] - assert not r.matched - - -def test_cost_equal_one_rejected(): - # 文档 §6：仅当 cost < 1 时接受（严格小于） - # dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0 - left, _ = target_pair(0.0, 0.0, 10.0) - x_off = FB / 10.0 - right = Detection(left.x1 - x_off, left.y1 + 4.75, - left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5) - # 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125） - assert right.cy == pytest.approx(left.cy + 1.0) - r = one_report(StereoRanger(), [left], [right])[0] - assert not r.matched - - -def test_disparity_out_of_z_window_rejected(): - # 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6） - near_l, near_r = target_pair(0.0, 0.0, 0.5) - far_l, far_r = target_pair(0.0, 0.0, 60.0) - r = one_report(StereoRanger(), [near_l], [near_r])[0] - assert not r.matched - r = one_report(StereoRanger(), [far_l], [far_r])[0] - assert not r.matched - - -def test_greedy_one_to_one_crossing_indices(): - # 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。 - # 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。 - l_a, r_a = target_pair(0.0, 0.0, 10.0) - l_b, r_b = target_pair(0.3, 0.0, 6.0) - reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a]) - assert all(r.matched for r in reports) - assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9) - assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9) - assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2) - - -def test_track_id_continuity_preference(): - # §6：存在轨迹标识时优先匹配相同 track id - l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7) - l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8) - reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a]) - by_track = {r.track_id: r.forward_distance_m for r in reports} - assert by_track[7] == pytest.approx(10.0, rel=1e-9) - assert by_track[8] == pytest.approx(8.0, rel=1e-9) - - -def test_unmatched_left_reports_bearing_only(): - left, right = target_pair(0.0, 0.0, 10.0) - stray = box(500.0, 300.0, cls_id=3, conf=0.8) - reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5) - assert reports[0].matched and reports[0].valid - u = reports[1] - assert not u.matched and not u.valid - assert u.forward_distance_m is None - assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy - assert u.confidence == 0.8 and u.class_id == 3 - assert u.frame_sequence == 42 and u.timestamp_s == 1.5 - - -def test_empty_sides(): - ranger = StereoRanger() - assert ranger.update([], []) == [] - r = one_report(ranger, [box(100.0, 100.0)], [])[0] - assert not r.matched - - -def test_report_order_follows_left_detections(): - ranger = StereoRanger() - l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0) - l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2) - reports = ranger.update([l2, l1], [r1, r2]) # 输出顺序与输入左目一致 - assert [r.class_id for r in reports] == [2, 0] - assert reports[0].confidence == min(l2.conf, r2.conf) - - -# ---- §5.4 有效性判定 ---- - -def test_beyond_range_tolerance_invalid_but_distance_kept(): - left, right = target_pair(0.0, 0.0, 15.0) # δZ ≈ 3.9 m > 2 m - r = one_report(StereoRanger(), [left], [right])[0] - assert r.matched - assert not r.valid - assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9) - assert r.lateral_m is not None # 方位/坐标仍发布（§5.4 域外标记无效） - assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px)) - - -def test_within_range_tolerance_valid(): - left, right = target_pair(0.0, 0.0, 10.0) # δZ ≈ 1.74 m <= 2 m - r = one_report(StereoRanger(), [left], [right])[0] - assert r.valid - - -def test_invalid_when_disparity_below_dmin(): - # 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效 - cfg = StereoConfig(range_tol_m=50.0) - left, right = target_pair(0.0, 0.0, 25.0) - r = one_report(StereoRanger(cfg), [left], [right])[0] - assert r.matched and not r.valid - - -# ---- §7 时间滤波（可选） ---- - -def moving_pair(z_m, track_id=1, cls_id=0): - """返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。""" - left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id) - right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id) - return [left], [right] - - -def test_filter_smooths_and_resets_on_jump(): - cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2, - range_tol_m=50.0) # 放宽有效域，让 10~13 m 均有效以观察滤波 - ranger = StereoRanger(cfg) - z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0] - assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9) # 首帧置位 - z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] # 跳变 10% <= 20%：平滑 - assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9) - z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0] # 跳变 26% > 20%：重置 - assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9) - - -def test_filter_skipped_without_track_id(): - cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3) - ranger = StereoRanger(cfg) - one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1) - z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0] - assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9) - - -def test_reset_clears_filter_state(): - cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0) - ranger = StereoRanger(cfg) - one_report(ranger, *moving_pair(10.0), frame_sequence=1) # 状态 = 10.0 - ranger.reset() - # 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0 - z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] - assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9) - - -def test_invalid_measurements_do_not_pollute_filter_state(): - cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0) - ranger = StereoRanger(cfg) - one_report(ranger, *moving_pair(10.0), frame_sequence=1) - z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0] # D < D_min：无效 - assert not z2.valid - z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0] - # 无效帧未污染状态：仍按与首帧 10.0 平滑 - assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9) diff --git a/utils/tests/visual_track_control/tracking_control_law_test.py b/utils/tests/visual_track_control/tracking_control_law_test.py index 55704f2..a4b1531 100644 --- a/utils/tests/visual_track_control/tracking_control_law_test.py +++ b/utils/tests/visual_track_control/tracking_control_law_test.py @@ -3,8 +3,9 @@ 移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。 覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉 超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向 -通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种 -无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。 +通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离 +降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率 +限幅（S6）、机体系→NED 输出旋转（A4）。 """ import math @@ -26,7 +27,7 @@ def make_config() -> TrackingConfig: cfg = TrackingConfig() cfg.control.frequency_hz = 20.0 cfg.control.visual_stale_ms = 200 - cfg.control.radar_stale_ms = 500 + cfg.control.distance_stale_ms = 500 cfg.control.attitude_stale_ms = 300 cfg.camera.image_width = 1000 cfg.camera.image_height = 1000 @@ -55,7 +56,7 @@ def make_config() -> TrackingConfig: def make_snapshot(now_ms: int) -> ControlSnapshot: - """新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。""" + """新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。""" return ControlSnapshot( track=TrackState(tracked=True, is_predicted=False, center=(500.0, 500.0)), @@ -213,9 +214,9 @@ def test_vertical_velocity_clamped(): 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 + 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) @@ -223,17 +224,17 @@ def test_distance_approach_when_far(): 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 + 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()) # 缺省 kHold + law = TrackingControlLaw(make_config()) # 基准配置 HOLD snap = make_snapshot(1000) - snap.associated_to_target = False + snap.distance_valid = False out = law.update(snap, 1000) assert out.mode is ControlMode.VELOCITY_HEADING assert out.vx_mps == 0.0 @@ -244,29 +245,79 @@ def test_no_distance_slow_approach(): cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH law = TrackingControlLaw(cfg) snap = make_snapshot(1000) - snap.associated_to_target = False + 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_degrades(): +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.associated_to_target = False + 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_radar_falls_back_to_no_distance(): - law = TrackingControlLaw(make_config()) # kHold +def test_stale_distance_falls_back_to_no_distance(): + law = TrackingControlLaw(make_config()) # HOLD snap = make_snapshot(1000) - snap.radar_distance_m = 30.0 - snap.associated_to_target = True - snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用 + 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 @@ -280,9 +331,9 @@ def test_first_tick_vx_slews_from_zero(): 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 + 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 @@ -293,9 +344,9 @@ 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 + 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) @@ -306,9 +357,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 + 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) @@ -321,17 +372,17 @@ def test_reset_restores_slew_from_zero(): 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 + 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.radar_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.radar_time_ms = 3000 + snap.distance_time_ms = 3000 snap.attitude_time_ms = 3000 out = law.update(snap, 3000) # Reset 后限幅器归零：又从 0.1 开始渐变 diff --git a/utils/tests/visual_track_control/vtc_config_test.py b/utils/tests/visual_track_control/vtc_config_test.py index 4335252..2719a04 100644 --- a/utils/tests/visual_track_control/vtc_config_test.py +++ b/utils/tests/visual_track_control/vtc_config_test.py @@ -15,7 +15,7 @@ from pathlib import Path import pytest from visual_track_control.config import (HeadingMode, NoDistanceAction, - load_from_json) + TrackingConfig, load_from_json) _REPO_ROOT = Path(__file__).resolve().parents[3] REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json" @@ -27,7 +27,7 @@ def full_config() -> dict: "control": { "frequency_hz": 20.0, "visual_stale_ms": 200, - "radar_stale_ms": 500, + "distance_stale_ms": 500, "attitude_stale_ms": 300, }, "camera": { @@ -57,6 +57,7 @@ def full_config() -> dict: "retreat_velocity_limit_mps": 1.0, "no_distance_action": "hold", "no_distance_approach_limit_mps": 0.5, + "no_distance_exit_grace_ms": 3000, }, "accel_limit": { "ax_mps2": 2.0, @@ -84,7 +85,9 @@ def test_load_real_config_succeeds(): assert cfg.vertical.vz_gain_mps_per_deg == 0.1 assert cfg.vertical.vz_limit_mps == 2.0 assert cfg.distance.d_exp_m == 10.0 - assert cfg.distance.no_distance_action is NoDistanceAction.HOLD + assert cfg.distance.no_distance_action is NoDistanceAction.SLOW_APPROACH + assert cfg.distance.no_distance_exit_grace_ms == 3000 + assert cfg.control.distance_stale_ms == 500 assert cfg.accel_limit.ax_mps2 == 2.0 @@ -169,3 +172,18 @@ def test_fov_out_of_range_throws(tmp_path): with pytest.raises(ValueError) as exc_info: load_from_json(path) assert "camera.fov_h_deg" in str(exc_info.value) + + +def test_default_no_distance_action_is_slow_approach(): + # S1 拍板：双目有效域外无距离是常态，默认定速接近（纪要 D4/S1） + assert (TrackingConfig().distance.no_distance_action + is NoDistanceAction.SLOW_APPROACH) + + +def test_negative_exit_grace_throws(tmp_path): + config = full_config() + config["distance"]["no_distance_exit_grace_ms"] = -1 + path = write_temp_json(tmp_path, config, "vtc_cfg_badgrace.json") + with pytest.raises(ValueError) as exc_info: + load_from_json(path) + assert "no_distance_exit_grace_ms" in str(exc_info.value) diff --git a/utils/visual_track_control/README.md b/utils/visual_track_control/README.md index 25979a3..2b52d84 100644 --- a/utils/visual_track_control/README.md +++ b/utils/visual_track_control/README.md @@ -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()` | 停止闭环时清全部内部状态 | diff --git a/utils/visual_track_control/config.py b/utils/visual_track_control/config.py index 95117cc..ff7694a 100644 --- a/utils/visual_track_control/config.py +++ b/utils/visual_track_control/config.py @@ -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 diff --git a/utils/visual_track_control/tracking_control_law.py b/utils/visual_track_control/tracking_control_law.py index fc9dff7..cab5c48 100644 --- a/utils/visual_track_control/tracking_control_law.py +++ b/utils/visual_track_control/tracking_control_law.py @@ -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 diff --git a/utils/visual_track_control/types.py b/utils/visual_track_control/types.py index 01353f3..a6a7a0d 100644 --- a/utils/visual_track_control/types.py +++ b/utils/visual_track_control/types.py @@ -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