双目测距方案重构与其他小修改
This commit is contained in:
keita
@@ -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）。
|
||||||
-432
@@ -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}\)。
|
|
||||||
|
|
||||||
---
|
|
||||||
|
|
||||||
*文档结束。*
|
|
||||||
+84
-39
@@ -47,7 +47,7 @@
|
|||||||
1. **三路控制律计算**（每拍一次，固定周期）
|
1. **三路控制律计算**（每拍一次，固定周期）
|
||||||
- **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令；
|
- **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令；
|
||||||
- **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令；
|
- **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令；
|
||||||
- **距离**：雷达目标距离与期望射程之差 Δd → PID → 前向速度指令。
|
- **距离**：双目目标距离与期望射程之差 Δd → PID → 前向速度指令。
|
||||||
2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许
|
2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许
|
||||||
跳过限幅直达链路（含切换后的**首条**指令）。
|
跳过限幅直达链路（含切换后的**首条**指令）。
|
||||||
3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续
|
3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续
|
||||||
@@ -63,8 +63,8 @@
|
|||||||
│ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│
|
│ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│
|
||||||
└─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘
|
└─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘
|
||||||
│
|
│
|
||||||
┌─────────────┐ 前向距离 │ 控制意图
|
┌─────────────┐ 目标距离 │ 控制意图
|
||||||
│ 激光测距雷达│──────────────────────┐ ▼
|
│ 双目立体测距│──────────────────────┐ ▼
|
||||||
└─────────────┘ │ ┌──────────────────────┐
|
└─────────────┘ │ ┌──────────────────────┐
|
||||||
┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │
|
┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │
|
||||||
│ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │
|
│ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │
|
||||||
@@ -78,8 +78,15 @@
|
|||||||
```
|
```
|
||||||
|
|
||||||
**上游依赖**：追踪库提供主目标中心与 `tracked` / `is_predicted` 两个行为标志
|
**上游依赖**：追踪库提供主目标中心与 `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 明确不做的事（边界）
|
### 1.4 明确不做的事（边界）
|
||||||
|
|
||||||
- **不做感知**：不跑检测/光流/雷达驱动，只消费调用方填入的快照；
|
- **不做感知**：不跑检测/光流/双目立体匹配，只消费调用方填入的快照；
|
||||||
- **不做目标关联**：相机—雷达空间关联由感知融合层给出布尔标志，本模块只读取；
|
- **不做距离有效性判定**：`distance_valid` 由双目链路自身的一致性校验给出，
|
||||||
|
track_id 一致性校验为调用方约定（§12.2），本模块只读取标志；
|
||||||
- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略；
|
- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略；
|
||||||
- **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息；
|
- **不做链路 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.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 |
|
||||||
| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 |
|
| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 |
|
||||||
| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 |
|
| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 |
|
||||||
| 雷达 | `radar_distance_m` | m | 机头前向单点距离；`<= 0` 视为不可用 |
|
| 距离 | `target_distance_m` | m | 双目目标距离（前向分量 Z）；`<= 0` 视为不可用 |
|
||||||
| 雷达 | `associated_to_target` | 布尔 | 相机—雷达关联成立标志；**未关联不得当作目标距离** |
|
| 距离 | `distance_valid` | 布尔 | 本帧测距可用标志：距离来自目标检测框自身的双目测距；**无效帧的距离不得当作目标距离** |
|
||||||
| 雷达 | `radar_time_ms` | 单调时钟 ms | 雷达取样瞬间打戳 |
|
| 距离 | `distance_time_ms` | 单调时钟 ms | 双目距离取样瞬间打戳 |
|
||||||
| 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 |
|
| 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 |
|
||||||
| 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 |
|
| 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 |
|
||||||
| 姿态 | `yaw_rad` | rad (NED) | 自机航向；**参与计算**：位置式航向合成 + 输出端坐标旋转 |
|
| 姿态 | `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` | 无（纯函数） | 不需要 |
|
| 视线角转换器 ×2 | 像素偏移 → β / α | 针孔系数 `2·tan(θ/2)/size` | 无（纯函数） | 不需要 |
|
||||||
| 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 |
|
| 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 |
|
||||||
| 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` |
|
| 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` |
|
||||||
@@ -320,6 +328,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
|||||||
|
|
||||||
# ---- ② 判丢 → 零速刹车 ----
|
# ---- ② 判丢 → 零速刹车 ----
|
||||||
若 非 快照.track.tracked:
|
若 非 快照.track.tracked:
|
||||||
|
无距离计时.清除() # 刹车期间不计 kExit 时长
|
||||||
若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态
|
若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态
|
||||||
距离PID.全清(); 四个限幅器.复位(0)
|
距离PID.全清(); 四个限幅器.复位(0)
|
||||||
输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零
|
输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零
|
||||||
@@ -354,12 +363,13 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
|||||||
输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps)
|
输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps)
|
||||||
|
|
||||||
# ---- ⑦ 距离通道 ----
|
# ---- ⑦ 距离通道 ----
|
||||||
距离可用 ← 快照.associated_to_target
|
距离可用 ← 快照.distance_valid
|
||||||
且 快照.radar_distance_m > 0
|
且 快照.target_distance_m > 0
|
||||||
且 非 已超龄(快照.radar_time_ms, now_ms, 配置.radar_stale_ms)
|
且 非 已超龄(快照.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)
|
前向原始 ← 距离PID.更新(Δd, dt)
|
||||||
限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅
|
限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅
|
||||||
# 非对称限幅：接近可以快，后退必须慢
|
# 非对称限幅：接近可以快，后退必须慢
|
||||||
@@ -370,9 +380,14 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
|||||||
按 配置.distance.no_distance_action 分支:
|
按 配置.distance.no_distance_action 分支:
|
||||||
保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0)
|
保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0)
|
||||||
限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||||||
退出近距操作: 输出.mode ← kDegradedHold
|
退出近距操作:
|
||||||
|
若 非 无距离计时.进行中: 无距离计时.启动(now_ms) # 首个无效拍起计时
|
||||||
|
若 now_ms − 无距离计时.起始时刻 ≥ 配置.no_distance_exit_grace_ms:
|
||||||
|
输出.mode ← kDegradedHold
|
||||||
输出.degraded_reason ← kNoDistanceExit
|
输出.degraded_reason ← kNoDistanceExit
|
||||||
返回 收尾并记录模式切换(输出, "距离未关联，退出近距操作")
|
返回 收尾并记录模式切换(输出, "持续无距离达门限，退出近距操作")
|
||||||
|
# grace 期内按"限速接近"执行（复用加速度限幅，vx 平滑无跳变）
|
||||||
|
输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||||||
|
|
||||||
# ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ----
|
# ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ----
|
||||||
旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad)
|
旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad)
|
||||||
@@ -391,7 +406,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
|||||||
| 姿态超龄降级 | 否 | 否 | 否 | `false` |
|
| 姿态超龄降级 | 否 | 否 | 否 | `false` |
|
||||||
| 视觉超龄降级 | 否 | 否 | 否 | `false` |
|
| 视觉超龄降级 | 否 | 否 | 否 | `false` |
|
||||||
| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` |
|
| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` |
|
||||||
| 无距离退出 | 是（仅积分） | 否 | 否 | `false` |
|
| 无距离退出（grace 期满） | 是（仅积分） | 否 | 否 | `false` |
|
||||||
| 正常/Coast | 视情况清积分 | 否 | **是** | `true` |
|
| 正常/Coast | 视情况清积分 | 否 | **是** | `true` |
|
||||||
|
|
||||||
**降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的
|
**降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的
|
||||||
@@ -505,9 +520,10 @@ vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限
|
|||||||
anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效，
|
anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效，
|
||||||
保证 anti-windup 判据基于"真实饱和边界"。
|
保证 anti-windup 判据基于"真实饱和边界"。
|
||||||
|
|
||||||
**可用性三重门**（缺一不可）：关联标志成立 **且** 距离为正 **且** 未超龄。
|
**可用性三重门**（缺一不可）：`distance_valid` 成立 **且** 距离为正 **且** 未超龄。
|
||||||
"未关联的雷达距离"只是机头前方某个障碍物的距离，把它当作目标距离会导致
|
双目测距取自目标检测框自身，不存在"回波打到别处"的跨模态关联问题；标志为
|
||||||
朝着错误目标调整射程。
|
`false` 表示本帧测距不可用（有效域外/未匹配/一致性校验失败），不得把任何数值
|
||||||
|
当作目标距离。
|
||||||
|
|
||||||
### 6.5 PID 内部算法
|
### 6.5 PID 内部算法
|
||||||
|
|
||||||
@@ -601,12 +617,12 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
|------|-------------|------|
|
|------|-------------|------|
|
||||||
| 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） |
|
| 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） |
|
||||||
| 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 |
|
| 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 |
|
||||||
| 雷达距离 | 500 ms | 雷达更新较慢，且距离不可用有独立降级动作 |
|
| 双目距离 | 500 ms | 双目随推理帧更新，且距离不可用有独立降级动作 |
|
||||||
|
|
||||||
### 7.2 降级优先级（顺序即语义）
|
### 7.2 降级优先级（顺序即语义）
|
||||||
|
|
||||||
```text
|
```text
|
||||||
姿态不可用 > 视觉超龄 > 判丢 > 无距离（退出档）
|
姿态不可用 > 视觉超龄 > 判丢 > 无距离（exit 档，持续达门限）
|
||||||
```
|
```
|
||||||
|
|
||||||
| 优先级 | 检查 | 理由 |
|
| 优先级 | 检查 | 理由 |
|
||||||
@@ -614,7 +630,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） |
|
| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） |
|
||||||
| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 |
|
| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 |
|
||||||
| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 |
|
| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 |
|
||||||
| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级 |
|
| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级；`exit` 档还需持续无距离达门限（§7.5），默认档为 `slow_approach`（§10.8） |
|
||||||
|
|
||||||
### 7.3 判丢刹车与状态清理
|
### 7.3 判丢刹车与状态清理
|
||||||
|
|
||||||
@@ -642,19 +658,28 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间
|
连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间
|
||||||
误差不反映真实世界，积分累加会把预测偏差固化成饱和量。
|
误差不反映真实世界，积分累加会把预测偏差固化成饱和量。
|
||||||
|
|
||||||
**Coast 期间距离通常不可用**：目标短暂消失往往伴随雷达关联失效，因此第 ⑦ 步会
|
**Coast 期间距离通常不可用**：目标短暂消失时双目失去匹配（`distance_valid ==
|
||||||
走无距离分支（默认"保持"→ 前向清零）。这是设计上的自然降级，不需要额外分支。
|
false`），第 ⑦ 步走无距离分支；默认 `slow_approach` 下前向按低速定速推进，
|
||||||
|
方位闭环继续（§7.5）。这是设计上的自然降级，不需要额外分支。
|
||||||
|
|
||||||
### 7.5 无距离三档动作
|
### 7.5 无距离三档动作
|
||||||
|
|
||||||
| 配置值 | 行为 | 适用场景 |
|
| 配置值 | 行为 | 适用场景 |
|
||||||
|--------|------|---------|
|
|--------|------|---------|
|
||||||
| `hold`（默认） | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 |
|
| `slow_approach`（默认） | 以配置的低速限幅缓速接近（仍经加速度限幅） | **默认档**：双目是机载唯一距离源，有效域外无距离是常态而非异常；即远场策略"方位闭环 + 定速接近"（D4） |
|
||||||
| `slow_approach` | 以配置的低速限幅缓速接近（仍经加速度限幅） | 需要持续压迫、且低速可接受风险 |
|
| `hold` | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 |
|
||||||
| `exit` | 整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞），无距离必须退出近距操作 |
|
| `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 兜底值 |
|
| `frequency_hz` | Hz | 20.0 | 控制频率；同时作为 dt 兜底值 |
|
||||||
| `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 |
|
| `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 |
|
||||||
| `radar_stale_ms` | ms | 500 | 雷达距离超龄阈值 |
|
| `distance_stale_ms` | ms | 500 | 双目距离超龄阈值 |
|
||||||
| `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 |
|
| `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 |
|
||||||
|
|
||||||
### 9.2 camera 组：画面与视场
|
### 9.2 camera 组：画面与视场
|
||||||
@@ -816,8 +841,9 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
| `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） |
|
| `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） |
|
||||||
| `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 |
|
| `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 |
|
||||||
| `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 |
|
| `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 |
|
||||||
| `no_distance_action` | 枚举 | `hold` | `hold` / `slow_approach` / `exit` |
|
| `no_distance_action` | 枚举 | `slow_approach` | `hold` / `slow_approach` / `exit`（默认档依据见 §10.8） |
|
||||||
| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档的限速值 |
|
| `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 组：加速度
|
### 9.6 accel_limit 组：加速度
|
||||||
|
|
||||||
@@ -889,6 +915,23 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断，
|
见 §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. 实现与重写指引
|
## 11. 实现与重写指引
|
||||||
@@ -899,6 +942,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
|------|------|------|---------|
|
|------|------|------|---------|
|
||||||
| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 |
|
| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 |
|
||||||
| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） |
|
| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） |
|
||||||
|
| 无距离计时（进行中 + 起始时刻） | 主控律 | 未计时 | 首个无距离拍启动（`exit` 档）；有效距离帧 / 判丢刹车 / 复位() 清除 |
|
||||||
| 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 |
|
| 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 |
|
||||||
| 上拍误差 | PID | 0 | 每拍 |
|
| 上拍误差 | PID | 0 | 每拍 |
|
||||||
| 低通后微分 | PID | 0 | 每拍 |
|
| 低通后微分 | PID | 0 | 每拍 |
|
||||||
@@ -917,7 +961,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
|
|
||||||
控制线程（固定周期，配置频率）:
|
控制线程（固定周期，配置频率）:
|
||||||
结果 ← 追踪库.取最新结果() # 线程安全拷贝
|
结果 ← 追踪库.取最新结果() # 线程安全拷贝
|
||||||
快照 ← 采集(结果, 雷达, 姿态)
|
快照 ← 采集(结果, 双目距离, 姿态)
|
||||||
快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳
|
快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳
|
||||||
意图 ← 控制律.更新(快照, 当前单调时钟())
|
意图 ← 控制律.更新(快照, 当前单调时钟())
|
||||||
按 意图.mode 分发:
|
按 意图.mode 分发:
|
||||||
@@ -926,7 +970,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
降级保持 → 不发送（记录降级原因/告警）
|
降级保持 → 不发送（记录降级原因/告警）
|
||||||
|
|
||||||
停止视觉闭环（状态机决定）:
|
停止视觉闭环（状态机决定）:
|
||||||
控制律.复位() # 清积分、清限幅器、清节拍与模式历史
|
控制律.复位() # 清积分、清限幅器、清节拍与模式历史、清无距离计时
|
||||||
```
|
```
|
||||||
|
|
||||||
**约束**：
|
**约束**：
|
||||||
@@ -1003,7 +1047,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
| **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 |
|
| **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 |
|
||||||
| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 |
|
| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 |
|
||||||
| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 |
|
| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 |
|
||||||
| **雷达关联判据** | 由感知融合层给出标志，本模块不实现 | 融合层设计 | 中：关联错误 = 距离通道朝错误目标调射程 |
|
| **track_id 一致性校验** | 调用方约定：装配层校验双目距离与追踪主目标的 `track_id` 一致并告警，库内不实现（S3） | 主程序装配层落地时加一处校验 | 中：错配会把其他目标的距离当作本目标，距离通道朝错误目标调射程 |
|
||||||
|
|
||||||
### 12.3 红线
|
### 12.3 红线
|
||||||
|
|
||||||
@@ -1014,7 +1058,8 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
|||||||
3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。
|
3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。
|
||||||
4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅，
|
4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅，
|
||||||
包括切换后的首条指令。
|
包括切换后的首条指令。
|
||||||
5. **未关联的雷达距离不得当作目标距离**。
|
5. **`distance_valid == false` 的距离不得当作目标距离**（v1.0 仅单目检出时
|
||||||
|
不发布距离，方位退化为该目偏移，见 `docs/双目深度估计思路.md` §11.3）。
|
||||||
|
|
||||||
---
|
---
|
||||||
|
|
||||||
|
|||||||
@@ -4,7 +4,7 @@
|
|||||||
"control": {
|
"control": {
|
||||||
"frequency_hz": 20.0,
|
"frequency_hz": 20.0,
|
||||||
"visual_stale_ms": 200,
|
"visual_stale_ms": 200,
|
||||||
"radar_stale_ms": 500,
|
"distance_stale_ms": 500,
|
||||||
"attitude_stale_ms": 300
|
"attitude_stale_ms": 300
|
||||||
},
|
},
|
||||||
|
|
||||||
@@ -36,8 +36,9 @@
|
|||||||
"derivative_filter_coef": 0.2,
|
"derivative_filter_coef": 0.2,
|
||||||
"approach_velocity_limit_mps": 3.0,
|
"approach_velocity_limit_mps": 3.0,
|
||||||
"retreat_velocity_limit_mps": 1.0,
|
"retreat_velocity_limit_mps": 1.0,
|
||||||
"no_distance_action": "hold",
|
"no_distance_action": "slow_approach",
|
||||||
"no_distance_approach_limit_mps": 0.5
|
"no_distance_approach_limit_mps": 0.5,
|
||||||
|
"no_distance_exit_grace_ms": 3000
|
||||||
},
|
},
|
||||||
|
|
||||||
"accel_limit": {
|
"accel_limit": {
|
||||||
|
|||||||
@@ -179,11 +179,11 @@ int main(int argc, char** argv) {
|
|||||||
snapshot.track = tracker.LastResult();
|
snapshot.track = tracker.LastResult();
|
||||||
snapshot.track_time_ms = now_ms;
|
snapshot.track_time_ms = now_ms;
|
||||||
|
|
||||||
// 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径）
|
// 双目距离：正常阶段有效；Coast/刹车阶段置无效（演示 §6.3 降级路径）
|
||||||
snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s;
|
snapshot.target_distance_m = 40.0 - 3.0 * elapsed_s;
|
||||||
snapshot.associated_to_target = (tick < normal_ticks) &&
|
snapshot.distance_valid = (tick < normal_ticks) &&
|
||||||
snapshot.radar_distance_m > 0.0;
|
snapshot.target_distance_m > 0.0;
|
||||||
snapshot.radar_time_ms = now_ms;
|
snapshot.distance_time_ms = now_ms;
|
||||||
|
|
||||||
// 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
|
// 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
|
||||||
snapshot.attitude_present = true;
|
snapshot.attitude_present = true;
|
||||||
@@ -209,11 +209,11 @@ int main(int argc, char** argv) {
|
|||||||
if (tick % 10 == 0) {
|
if (tick % 10 == 0) {
|
||||||
spdlog::info(
|
spdlog::info(
|
||||||
"[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} "
|
"[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),
|
elapsed_s, phase, ModeName(output.mode),
|
||||||
output.coast_active ? "(Coast)" : "", snapshot.track.tracked,
|
output.coast_active ? "(Coast)" : "", snapshot.track.tracked,
|
||||||
output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps,
|
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));
|
std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
|
||||||
|
|||||||
+6
-1
@@ -6,7 +6,7 @@
|
|||||||
// 对应规格：docs/物理追踪思路.md
|
// 对应规格：docs/物理追踪思路.md
|
||||||
// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
|
// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
|
||||||
// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
|
// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
|
||||||
// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3）
|
// §6 距离控制（PID(Δd) → 前向速度；距离无效按配置降级，§6.3）
|
||||||
// §6.4 Coast 清积分、判丢零速刹车
|
// §6.4 Coast 清积分、判丢零速刹车
|
||||||
// §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
|
// §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
|
||||||
// §8 统一限幅
|
// §8 统一限幅
|
||||||
@@ -58,6 +58,11 @@ private:
|
|||||||
// ---- 节拍与状态（仅打日志用，不参与数学） ----
|
// ---- 节拍与状态（仅打日志用，不参与数学） ----
|
||||||
std::int64_t last_now_ms_ = 0; // 上次计算时刻
|
std::int64_t last_now_ms_ = 0; // 上次计算时刻
|
||||||
ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志）
|
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
|
} // namespace drone::vtc
|
||||||
|
|||||||
@@ -33,18 +33,19 @@ enum class HeadingMode : std::uint8_t {
|
|||||||
kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
|
kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
|
||||||
};
|
};
|
||||||
|
|
||||||
/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定）
|
/// 目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）
|
||||||
enum class NoDistanceAction : std::uint8_t {
|
enum class NoDistanceAction : std::uint8_t {
|
||||||
kHold, // 保持：前向速度指令清零（安全占位语义）
|
kHold, // 保持：前向速度指令清零（安全占位语义）
|
||||||
kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
|
kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
|
||||||
kExit, // 退出近距操作：输出降级保持（kDegradedHold）
|
kExit, // 退出近距操作：持续无距离达 no_distance_exit_grace_ms 才
|
||||||
|
// 降级保持（kDegradedHold）；grace 期间按 kSlowApproach 行为
|
||||||
};
|
};
|
||||||
|
|
||||||
/// control 组：控制周期与各输入超龄阈值（§7）
|
/// control 组：控制周期与各输入超龄阈值（§7）
|
||||||
struct ControlGroupConfig {
|
struct ControlGroupConfig {
|
||||||
double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定）
|
double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定）
|
||||||
std::int64_t visual_stale_ms = 200; // 视觉追踪结果超龄阈值（毫秒）
|
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; // 自机姿态超龄阈值（毫秒）
|
std::int64_t attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒）
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -84,8 +85,12 @@ struct DistanceGroupConfig {
|
|||||||
double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波
|
double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波
|
||||||
double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8）
|
double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8）
|
||||||
double retreat_velocity_limit_mps = 1.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 限速值（米/秒）
|
double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒）
|
||||||
|
std::int64_t no_distance_exit_grace_ms = 3000; // kExit 门限：持续无距离达
|
||||||
|
// 此时长才退出（毫秒，占位）
|
||||||
};
|
};
|
||||||
|
|
||||||
/// accel_limit 组：各轴加速度限幅（§8，待定）
|
/// accel_limit 组：各轴加速度限幅（§8，待定）
|
||||||
|
|||||||
@@ -13,8 +13,8 @@
|
|||||||
// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
|
// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
|
||||||
// 控制律热路径零堆分配；
|
// 控制律热路径零堆分配；
|
||||||
// - 本库不依赖主工程 include/ 的任何类型，独立可编译；
|
// - 本库不依赖主工程 include/ 的任何类型，独立可编译；
|
||||||
// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作
|
// - 目标距离必须带 distance_valid 有效标志：无效值不得当作目标距离
|
||||||
// 目标距离（§6.3、需求分析.md FR-041）。
|
// （§6.3、需求分析.md FR-041）。
|
||||||
// =============================================================================
|
// =============================================================================
|
||||||
|
|
||||||
#include <cstdint>
|
#include <cstdint>
|
||||||
@@ -54,10 +54,11 @@ struct ControlSnapshot {
|
|||||||
drone::tracker::TrackResult track; // 追踪库结果（值拷贝）
|
drone::tracker::TrackResult track; // 追踪库结果（值拷贝）
|
||||||
std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样
|
std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样
|
||||||
|
|
||||||
// ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ----
|
// ---- 目标距离（双目立体测距，§6.3：distance_valid == false 时不得
|
||||||
double radar_distance_m = 0.0; // 机头前向单点距离（米）
|
// 当作目标距离） ----
|
||||||
bool associated_to_target = false; // 相机—雷达空间关联成立标志
|
double target_distance_m = 0.0; // 目标距离（米，双目前向分量 Z）
|
||||||
std::int64_t radar_time_ms = 0; // 取样单调时钟（毫秒）
|
bool distance_valid = false; // 距离有效标志（测距来自检测框自身，无跨模态关联）
|
||||||
|
std::int64_t distance_time_ms = 0; // 取样单调时钟（毫秒）
|
||||||
|
|
||||||
// ---- 自机姿态/速度 ----
|
// ---- 自机姿态/速度 ----
|
||||||
// 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转
|
// 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转
|
||||||
|
|||||||
@@ -9,7 +9,8 @@
|
|||||||
// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
|
// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
|
||||||
// vtc_types.h ControlMode 注释）
|
// vtc_types.h ControlMode 注释）
|
||||||
// §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）；
|
// §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）零速刹车
|
// §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车
|
||||||
// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
|
// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
|
||||||
// §8 所有输出统一限幅；首条指令经变化率限幅（S6）
|
// §8 所有输出统一限幅；首条指令经变化率限幅（S6）
|
||||||
@@ -123,6 +124,7 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
|
|||||||
|
|
||||||
// ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
|
// ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
|
||||||
if (!snapshot.track.tracked) {
|
if (!snapshot.track.tracked) {
|
||||||
|
no_distance_active_ = false; // 刹车期间不计 kExit 无距离时长
|
||||||
if (last_mode_ != ControlMode::kBrakeHover) {
|
if (last_mode_ != ControlMode::kBrakeHover) {
|
||||||
// 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
// 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
||||||
distance_pid_.Reset();
|
distance_pid_.Reset();
|
||||||
@@ -181,14 +183,16 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
// ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
||||||
// 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
|
// 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
|
||||||
|
// （§6.3、FR-041）
|
||||||
const bool distance_available =
|
const bool distance_available =
|
||||||
snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 &&
|
snapshot.distance_valid && snapshot.target_distance_m > 0.0 &&
|
||||||
!StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms,
|
!StalenessChecker::IsStale(snapshot.distance_time_ms, now_ms,
|
||||||
cfg_.control.radar_stale_ms);
|
cfg_.control.distance_stale_ms);
|
||||||
if (distance_available) {
|
if (distance_available) {
|
||||||
|
no_distance_active_ = false; // 任一有效距离帧重置 kExit 计时
|
||||||
// Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
|
// Δ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 vx_raw = distance_pid_.Update(delta_d, dt_s);
|
||||||
const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8）
|
const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8）
|
||||||
out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps,
|
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);
|
cfg_.distance.no_distance_approach_limit_mps, dt_s);
|
||||||
break;
|
break;
|
||||||
case NoDistanceAction::kExit:
|
case NoDistanceAction::kExit:
|
||||||
// 退出近距操作：整体降级保持
|
// 退出近距操作：双目有效域外无距离是常态（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.mode = ControlMode::kDegradedHold;
|
||||||
out.degraded_reason = DegradedReason::kNoDistanceExit;
|
out.degraded_reason = DegradedReason::kNoDistanceExit;
|
||||||
return FinishAndLog("距离未关联，退出近距操作");
|
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();
|
yaw_rate_limiter_.Reset();
|
||||||
vx_limiter_.Reset();
|
vx_limiter_.Reset();
|
||||||
vz_limiter_.Reset();
|
vz_limiter_.Reset();
|
||||||
|
no_distance_active_ = false;
|
||||||
last_now_ms_ = 0;
|
last_now_ms_ = 0;
|
||||||
last_mode_ = ControlMode::kDegradedHold;
|
last_mode_ = ControlMode::kDegradedHold;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -107,9 +107,9 @@ ControlGroupConfig LoadControl(const Json& root) {
|
|||||||
c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"),
|
c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"),
|
||||||
"control.frequency_hz");
|
"control.frequency_hz");
|
||||||
c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms");
|
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");
|
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 必须为正");
|
throw std::runtime_error("配置非法: control.*_stale_ms 必须为正");
|
||||||
}
|
}
|
||||||
return c;
|
return c;
|
||||||
@@ -193,6 +193,13 @@ DistanceGroupConfig LoadDistance(const Json& root) {
|
|||||||
d.no_distance_approach_limit_mps = RequireNonNegative(
|
d.no_distance_approach_limit_mps = RequireNonNegative(
|
||||||
RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
|
RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
|
||||||
"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;
|
return d;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -4,8 +4,9 @@
|
|||||||
// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
|
// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
|
||||||
// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
|
// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
|
||||||
// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
|
// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
|
||||||
// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
|
// A2）、垂直统一速度支路（A1）、距离通道无距离降级动作（含 kExit
|
||||||
// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
// 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、
|
||||||
|
// 机体系→NED 输出旋转（A4）。
|
||||||
// =============================================================================
|
// =============================================================================
|
||||||
|
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
@@ -31,7 +32,7 @@ TrackingConfig MakeConfig() {
|
|||||||
TrackingConfig cfg;
|
TrackingConfig cfg;
|
||||||
cfg.control.frequency_hz = 20.0;
|
cfg.control.frequency_hz = 20.0;
|
||||||
cfg.control.visual_stale_ms = 200;
|
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.control.attitude_stale_ms = 300;
|
||||||
cfg.camera.image_width = 1000;
|
cfg.camera.image_width = 1000;
|
||||||
cfg.camera.image_height = 1000;
|
cfg.camera.image_height = 1000;
|
||||||
@@ -59,7 +60,7 @@ TrackingConfig MakeConfig() {
|
|||||||
return cfg;
|
return cfg;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜
|
/// 新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜
|
||||||
ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
|
ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
|
||||||
ControlSnapshot s;
|
ControlSnapshot s;
|
||||||
s.track.tracked = true;
|
s.track.tracked = true;
|
||||||
@@ -220,9 +221,9 @@ TEST(TrackingControlLawTest, VerticalVelocityClamped) {
|
|||||||
TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
|
TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
|
||||||
TrackingControlLaw law(MakeConfig());
|
TrackingControlLaw law(MakeConfig());
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.radar_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3
|
snap.target_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
|
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
|
||||||
}
|
}
|
||||||
@@ -230,9 +231,9 @@ TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
|
|||||||
TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
|
TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
|
||||||
TrackingControlLaw law(MakeConfig());
|
TrackingControlLaw law(MakeConfig());
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.radar_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
snap.target_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
|
EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
|
||||||
}
|
}
|
||||||
@@ -240,7 +241,7 @@ TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
|
|||||||
TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
|
TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
|
||||||
TrackingControlLaw law(MakeConfig()); // 缺省 kHold
|
TrackingControlLaw law(MakeConfig()); // 缺省 kHold
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.associated_to_target = false;
|
snap.distance_valid = false;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
||||||
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
||||||
@@ -251,29 +252,80 @@ TEST(TrackingControlLawTest, NoDistanceSlowApproach) {
|
|||||||
cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
|
cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
|
||||||
TrackingControlLaw law(cfg);
|
TrackingControlLaw law(cfg);
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.associated_to_target = false;
|
snap.distance_valid = false;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
|
EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(TrackingControlLawTest, NoDistanceExitDegrades) {
|
TEST(TrackingControlLawTest, NoDistanceExitWaitsGraceThenDegrades) {
|
||||||
|
// S1 拍板：kExit 需持续无距离达门限才降级；grace 期间按 kSlowApproach 行为
|
||||||
auto cfg = MakeConfig();
|
auto cfg = MakeConfig();
|
||||||
cfg.distance.no_distance_action = NoDistanceAction::kExit;
|
cfg.distance.no_distance_action = NoDistanceAction::kExit;
|
||||||
|
cfg.distance.no_distance_exit_grace_ms = 200;
|
||||||
TrackingControlLaw law(cfg);
|
TrackingControlLaw law(cfg);
|
||||||
auto snap = MakeSnapshot(1000);
|
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);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
||||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
|
EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
|
||||||
EXPECT_FALSE(out.valid);
|
EXPECT_FALSE(out.valid);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) {
|
TEST(TrackingControlLawTest, StaleDistanceFallsBackToNoDistance) {
|
||||||
TrackingControlLaw law(MakeConfig()); // kHold
|
TrackingControlLaw law(MakeConfig()); // kHold
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.radar_distance_m = 30.0;
|
snap.target_distance_m = 30.0;
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用
|
snap.distance_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
||||||
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
||||||
@@ -287,9 +339,9 @@ TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) {
|
|||||||
cfg.accel_limit.ax_mps2 = 2.0;
|
cfg.accel_limit.ax_mps2 = 2.0;
|
||||||
TrackingControlLaw law(cfg);
|
TrackingControlLaw law(cfg);
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.radar_distance_m = 30.0; // 通道输出本应为 3.0
|
snap.target_distance_m = 30.0; // 通道输出本应为 3.0
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
const auto out = law.Update(snap, 1000); // 首拍：dt 按 1/20 Hz 兜底
|
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
|
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());
|
TrackingControlLaw law(MakeConfig());
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.yaw_rad = kPi / 2.0; // 机头朝东
|
snap.yaw_rad = kPi / 2.0; // 机头朝东
|
||||||
snap.radar_distance_m = 30.0;
|
snap.target_distance_m = 30.0;
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
// 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
// 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||||||
EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
|
EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
|
||||||
@@ -313,9 +365,9 @@ TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) {
|
|||||||
TrackingControlLaw law(MakeConfig());
|
TrackingControlLaw law(MakeConfig());
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.yaw_rad = 0.0;
|
snap.yaw_rad = 0.0;
|
||||||
snap.radar_distance_m = 30.0;
|
snap.target_distance_m = 30.0;
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
const auto out = law.Update(snap, 1000);
|
const auto out = law.Update(snap, 1000);
|
||||||
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
|
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
|
||||||
EXPECT_NEAR(out.vy_mps, 0.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;
|
cfg.accel_limit.ax_mps2 = 2.0;
|
||||||
TrackingControlLaw law(cfg);
|
TrackingControlLaw law(cfg);
|
||||||
auto snap = MakeSnapshot(1000);
|
auto snap = MakeSnapshot(1000);
|
||||||
snap.radar_distance_m = 30.0;
|
snap.target_distance_m = 30.0;
|
||||||
snap.associated_to_target = true;
|
snap.distance_valid = true;
|
||||||
snap.radar_time_ms = 1000;
|
snap.distance_time_ms = 1000;
|
||||||
for (std::int64_t t = 1000; t < 3000; t += 50) {
|
for (std::int64_t t = 1000; t < 3000; t += 50) {
|
||||||
snap.track_time_ms = t;
|
snap.track_time_ms = t;
|
||||||
snap.radar_time_ms = t;
|
snap.distance_time_ms = t;
|
||||||
snap.attitude_time_ms = t;
|
snap.attitude_time_ms = t;
|
||||||
law.Update(snap, t); // 若干拍后 vx 已爬升到限幅 3.0
|
law.Update(snap, t); // 若干拍后 vx 已爬升到限幅 3.0
|
||||||
}
|
}
|
||||||
law.Reset();
|
law.Reset();
|
||||||
snap.track_time_ms = 3000;
|
snap.track_time_ms = 3000;
|
||||||
snap.radar_time_ms = 3000;
|
snap.distance_time_ms = 3000;
|
||||||
snap.attitude_time_ms = 3000;
|
snap.attitude_time_ms = 3000;
|
||||||
const auto out = law.Update(snap, 3000);
|
const auto out = law.Update(snap, 3000);
|
||||||
// Reset 后限幅器归零：又从 0.1 开始渐变
|
// Reset 后限幅器归零：又从 0.1 开始渐变
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ const char* kFullJson = R"({
|
|||||||
"control": {
|
"control": {
|
||||||
"frequency_hz": 20.0,
|
"frequency_hz": 20.0,
|
||||||
"visual_stale_ms": 200,
|
"visual_stale_ms": 200,
|
||||||
"radar_stale_ms": 500,
|
"distance_stale_ms": 500,
|
||||||
"attitude_stale_ms": 300
|
"attitude_stale_ms": 300
|
||||||
},
|
},
|
||||||
"camera": {
|
"camera": {
|
||||||
@@ -59,7 +59,8 @@ const char* kFullJson = R"({
|
|||||||
"approach_velocity_limit_mps": 3.0,
|
"approach_velocity_limit_mps": 3.0,
|
||||||
"retreat_velocity_limit_mps": 1.0,
|
"retreat_velocity_limit_mps": 1.0,
|
||||||
"no_distance_action": "hold",
|
"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": {
|
"accel_limit": {
|
||||||
"ax_mps2": 2.0,
|
"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_gain_mps_per_deg, 0.1);
|
||||||
EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0);
|
EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0);
|
||||||
EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.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);
|
EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -221,4 +224,25 @@ TEST(VtcConfigTest, FovOutOfRangeThrows) {
|
|||||||
std::remove(path.c_str());
|
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
|
} // namespace
|
||||||
|
|||||||
@@ -34,8 +34,9 @@
|
|||||||
垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
|
垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
|
||||||
原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
|
原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
|
||||||
- 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
|
- 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
|
||||||
`vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
|
`vx_mps`；目标距离无效/超龄时按 `NoDistanceAction` 配置降级
|
||||||
（hold / slow_approach / exit）。
|
（hold / slow_approach / exit，exit 需持续无距离达
|
||||||
|
`no_distance_exit_grace_ms` 门限，见 §3.8）。
|
||||||
- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
|
- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
|
||||||
`SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
|
`SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
|
||||||
全部在机体系完成。
|
全部在机体系完成。
|
||||||
@@ -96,7 +97,8 @@
|
|||||||
（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
|
（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
|
||||||
|
|
||||||
`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
|
`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
|
||||||
`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
|
`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（持续无距离达门限且配置
|
||||||
|
退出近距操作）。
|
||||||
|
|
||||||
`ControlSnapshot`（输入，纯值类型）：
|
`ControlSnapshot`（输入，纯值类型）：
|
||||||
|
|
||||||
@@ -104,9 +106,9 @@
|
|||||||
|------|------|
|
|------|------|
|
||||||
| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
|
| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
|
||||||
| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
|
| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
|
||||||
| `radar_distance_m` | 机头前向单点距离（米） |
|
| `target_distance_m` | 目标距离（米，双目立体测距前向分量 Z） |
|
||||||
| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
|
| `distance_valid` | 距离有效标志；**无效不得当目标距离**（规格 §6.3、需求分析 FR-041）。距离来自目标检测框自身的双目测距，无跨模态关联问题 |
|
||||||
| `radar_time_ms` | 雷达取样单调时钟 |
|
| `distance_time_ms` | 距离取样单调时钟 |
|
||||||
| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
|
| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
|
||||||
| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
|
| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
|
||||||
| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
|
| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
|
||||||
@@ -124,15 +126,18 @@
|
|||||||
| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
|
| `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
|
||||||
|
|
||||||
`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
|
`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`、
|
`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
|
||||||
`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
|
`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
|
||||||
（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
|
（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
|
||||||
`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
|
`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
|
||||||
`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
|
`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 键名与字段一一对应，见
|
（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
|
||||||
`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
|
`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
|
||||||
|
`distance.no_distance_action` 默认 `slow_approach`（双目有效域外无距离
|
||||||
|
是常态，对齐 D4 远场定速接近策略，见《双目测距方案讨论纪要》S1）；
|
||||||
相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
|
相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
|
||||||
|
|
||||||
### 2.3 ICommandTransport 契约
|
### 2.3 ICommandTransport 契约
|
||||||
@@ -263,11 +268,11 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
|
|||||||
→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
|
→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
|
||||||
→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
|
→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
|
||||||
→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
|
→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
|
||||||
→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
|
→ ⑦ 距离通道（`distance_valid` 且未超龄才可用；否则清积分并按
|
||||||
`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
|
`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
|
||||||
随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
|
随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
|
||||||
|
|
||||||
两个设计决策记录：
|
三个设计决策记录：
|
||||||
|
|
||||||
- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
|
- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
|
||||||
→ ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
|
→ ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
|
||||||
@@ -278,6 +283,14 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
|
|||||||
航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
|
航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
|
||||||
位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
|
位置式分支输出 `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）
|
### 3.9 MAVLink 消息映射（规格 §2.2）
|
||||||
|
|
||||||
@@ -347,12 +360,12 @@ cmake --build build_vtc -j$(nproc)
|
|||||||
`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
|
`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
|
||||||
（对应规格 §6.4）：
|
（对应规格 §6.4）：
|
||||||
|
|
||||||
1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
|
1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，双目
|
||||||
关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
|
距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
|
||||||
角速度随 β 收敛而减小）；
|
角速度随 β 收敛而减小）；
|
||||||
2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
|
2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
|
||||||
继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
|
继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为
|
||||||
前向清零）；
|
`slow_approach`：按限速值缓速前向）；
|
||||||
3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
|
3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
|
||||||
零速刹车。
|
零速刹车。
|
||||||
|
|
||||||
@@ -372,8 +385,8 @@ cmake --build build_vtc -j$(nproc)
|
|||||||
| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
|
| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
|
||||||
| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
|
| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
|
||||||
| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
|
| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
|
||||||
| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
|
| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 kExit 门限、grace 有效帧重置、零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
|
||||||
| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
|
| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、默认降级动作为 slow_approach、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界/grace 为负均抛异常且消息携带字段名 |
|
||||||
| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
|
| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
|
||||||
|
|
||||||
运行方式（构建后）：
|
运行方式（构建后）：
|
||||||
@@ -430,8 +443,11 @@ CMake config 包）；未找到时自动跳过测试构建并打印状态，不
|
|||||||
由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
|
由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
|
||||||
3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
|
3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
|
||||||
（`ICommandTransport` 注入），打包与控制律零改动；
|
（`ICommandTransport` 注入），打包与控制律零改动；
|
||||||
4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
|
4. **距离有效标志来源**：`distance_valid` 由双目链路（StereoRanger）
|
||||||
规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
|
给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下
|
||||||
|
调用方填 `ControlSnapshot` 时应校验 `StereoTargetDistance.track_id`
|
||||||
|
与 `TrackResult.track_id` 一致（v1.0 单目标为调用方约定，纪要 S3，
|
||||||
|
库内不加代码）。
|
||||||
|
|
||||||
### 6.4 红线声明
|
### 6.4 红线声明
|
||||||
|
|
||||||
|
|||||||
@@ -9,7 +9,7 @@ PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python)
|
|||||||
适用于 PX4 1.17
|
适用于 PX4 1.17
|
||||||
|
|
||||||
YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
|
YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
|
||||||
检测框与激光测距（几何一致闭环），无人机会实际飞向该虚拟目标。
|
检测框与双目目标距离（几何一致闭环），无人机会实际飞向该虚拟目标。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import asyncio
|
import asyncio
|
||||||
@@ -116,7 +116,7 @@ TRACK_NORMAL_SECS = 20.0 # 虚拟目标保持可见时长（之后
|
|||||||
TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长
|
TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长
|
||||||
TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时
|
TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时
|
||||||
STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||||||
PLOT_TRAJECTORY = False # 跟踪结束后绘制无人机与虚拟目标轨迹
|
PLOT_TRAJECTORY = True # 跟踪结束后绘制无人机与虚拟目标轨迹
|
||||||
TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
|
TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
|
||||||
|
|
||||||
# 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
|
# 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
|
||||||
@@ -219,7 +219,7 @@ def plot_trajectories(
|
|||||||
|
|
||||||
|
|
||||||
class VirtualTarget:
|
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(
|
return [box], fwd, True
|
||||||
fwd * fwd
|
|
||||||
+ right * right
|
|
||||||
+ rel_d * rel_d
|
|
||||||
)
|
|
||||||
|
|
||||||
return [box], slant, True
|
|
||||||
|
|
||||||
|
|
||||||
class TelemetryHub:
|
class TelemetryHub:
|
||||||
@@ -616,7 +610,7 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float],
|
|||||||
target.set_visible(False)
|
target.set_visible(False)
|
||||||
print("-- 模拟目标消失，追踪器进入 Coast 预测")
|
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)
|
hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad)
|
||||||
|
|
||||||
# 记录本拍位置（目标位置在 observe 内已 update）
|
# 记录本拍位置（目标位置在 observe 内已 update）
|
||||||
@@ -628,9 +622,9 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float],
|
|||||||
snap = ControlSnapshot(
|
snap = ControlSnapshot(
|
||||||
track=track_result_to_state(result),
|
track=track_result_to_state(result),
|
||||||
track_time_ms=now, # 每拍取样即打戳（视觉始终新鲜）
|
track_time_ms=now, # 每拍取样即打戳（视觉始终新鲜）
|
||||||
radar_distance_m=radar_m,
|
target_distance_m=distance_m,
|
||||||
associated_to_target=associated,
|
distance_valid=distance_valid,
|
||||||
radar_time_ms=now if associated else 0,
|
distance_time_ms=now,
|
||||||
attitude_present=True,
|
attitude_present=True,
|
||||||
attitude_time_ms=hub.attitude_time_ms,
|
attitude_time_ms=hub.attitude_time_ms,
|
||||||
yaw_rad=hub.yaw_rad,
|
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] "
|
print(f"[跟踪 t={t_s:5.1f}s] "
|
||||||
f"检测={len(detections)} "
|
f"检测={len(detections)} "
|
||||||
f"追踪[trk={result.tracked} pred={result.is_predicted} "
|
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"[{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"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s "
|
||||||
f"coast={out.coast_active}")
|
f"coast={out.coast_active}")
|
||||||
|
|||||||
Binary file not shown.
|
Before Width: | Height: | Size: 113 KiB After Width: | Height: | Size: 109 KiB |
+27
-9
@@ -1,9 +1,12 @@
|
|||||||
# utils —— reference-c++ 两个子项目的 Python 移植
|
# utils —— reference-c++ 的 Python 移植 + 双目测距新实现
|
||||||
|
|
||||||
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
||||||
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
||||||
（唯一的数据转换点在胶水层 demo）。
|
（唯一的数据转换点在胶水层 demo）。
|
||||||
|
|
||||||
|
另有 `stereo_ranger`（双目稀疏目标测距）：**按规格新实现**（非 C++ 移植），
|
||||||
|
规格 `docs/双目测距技术路线.md`；同样与既有两包互不依赖。
|
||||||
|
|
||||||
## 目录结构
|
## 目录结构
|
||||||
|
|
||||||
```
|
```
|
||||||
@@ -12,19 +15,24 @@ utils/
|
|||||||
│ └── README.md # 接口清单 + C++ 差异
|
│ └── README.md # 接口清单 + C++ 差异
|
||||||
├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关
|
├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关
|
||||||
│ └── README.md # 接口清单 + C++ 差异
|
│ └── README.md # 接口清单 + C++ 差异
|
||||||
|
├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖）
|
||||||
|
│ └── README.md # 接口清单 + 与规格的偏差
|
||||||
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
||||||
|
├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件）
|
||||||
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
||||||
├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件）
|
├── config/stereo_ranger.json # 双目测距配置（占位值，待标定回填）
|
||||||
│ ├── target_tracker/ # 27 条
|
├── tests/ # pytest 测试
|
||||||
│ └── visual_track_control/ # 78 条
|
│ ├── target_tracker/ # 28 条（对应 C++ 测试文件）
|
||||||
├── conftest.py # 保证可从任意工作目录导入两包
|
│ ├── visual_track_control/ # 81 条（对应 C++ 测试文件）
|
||||||
|
│ └── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程）
|
||||||
|
├── conftest.py # 保证可从任意工作目录导入三包
|
||||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
|
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
|
||||||
```
|
```
|
||||||
|
|
||||||
## 环境
|
## 环境
|
||||||
|
|
||||||
conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
|
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
|
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||||
```
|
```
|
||||||
|
|
||||||
105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4
|
220 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
|
||||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。
|
回归、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）
|
# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
|
||||||
python utils/demo_track_control.py --transport mavsdk
|
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。
|
目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
|
||||||
|
|
||||||
|
双目演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
|
||||||
|
单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。
|
||||||
|
|
||||||
## 移植约定
|
## 移植约定
|
||||||
|
|
||||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
||||||
@@ -56,3 +71,6 @@ python utils/demo_track_control.py --transport mavsdk
|
|||||||
- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
|
- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
|
||||||
`EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
|
`EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
|
||||||
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
|
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
|
||||||
|
- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、
|
||||||
|
视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README；
|
||||||
|
⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
|
||||||
|
|||||||
@@ -4,7 +4,7 @@
|
|||||||
"control": {
|
"control": {
|
||||||
"frequency_hz": 20.0,
|
"frequency_hz": 20.0,
|
||||||
"visual_stale_ms": 200,
|
"visual_stale_ms": 200,
|
||||||
"radar_stale_ms": 500,
|
"distance_stale_ms": 500,
|
||||||
"attitude_stale_ms": 300
|
"attitude_stale_ms": 300
|
||||||
},
|
},
|
||||||
|
|
||||||
@@ -36,8 +36,9 @@
|
|||||||
"derivative_filter_coef": 0.2,
|
"derivative_filter_coef": 0.2,
|
||||||
"approach_velocity_limit_mps": 3.0,
|
"approach_velocity_limit_mps": 3.0,
|
||||||
"retreat_velocity_limit_mps": 1.0,
|
"retreat_velocity_limit_mps": 1.0,
|
||||||
"no_distance_action": "hold",
|
"no_distance_action": "slow_approach",
|
||||||
"no_distance_approach_limit_mps": 0.5
|
"no_distance_approach_limit_mps": 0.5,
|
||||||
|
"no_distance_exit_grace_ms": 3000
|
||||||
},
|
},
|
||||||
|
|
||||||
"accel_limit": {
|
"accel_limit": {
|
||||||
|
|||||||
@@ -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()
|
|
||||||
@@ -10,7 +10,7 @@
|
|||||||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||||||
|
|
||||||
场景时间线（20 Hz，--speedup 可加速）：
|
场景时间线（20 Hz，--speedup 可加速）：
|
||||||
0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近
|
0~ 6s 正常跟踪：目标匀速横移 + 双目距离 30→10 m 前向逼近
|
||||||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||||||
控制律自动切换零速刹车）
|
控制律自动切换零速刹车）
|
||||||
10~13s 持续判丢：维持零速刹车
|
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)]
|
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):
|
def simulate_distance(tick: int, normal_ticks: int, period_s: float):
|
||||||
"""场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。"""
|
"""场景双目距离源：正常阶段有效、距离 30→10 m；之后无效。"""
|
||||||
if tick >= normal_ticks:
|
if tick >= normal_ticks:
|
||||||
return 0.0, False
|
return 0.0, False
|
||||||
distance = max(10.0, 30.0 - tick * period_s * 3.0)
|
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):
|
for tick in range(total_ticks):
|
||||||
now_ms = 1000 + tick * period_ms
|
now_ms = 1000 + tick * period_ms
|
||||||
detections = simulate_detections(tick, normal_ticks, period_s)
|
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)
|
result = tracker.update(detections)
|
||||||
snap = ControlSnapshot(
|
snap = ControlSnapshot(
|
||||||
track=track_result_to_state(result),
|
track=track_result_to_state(result),
|
||||||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||||||
radar_distance_m=radar_m,
|
target_distance_m=distance_m,
|
||||||
associated_to_target=associated,
|
distance_valid=distance_valid,
|
||||||
radar_time_ms=now_ms if associated else 0,
|
distance_time_ms=now_ms,
|
||||||
attitude_present=True,
|
attitude_present=True,
|
||||||
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
||||||
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
||||||
|
|||||||
@@ -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，左上/右下角） |
|
| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 |
|
||||||
| `StereoConfig(...)` | 配置（默认值 = 规格示例值，见下） |
|
| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO |
|
||||||
| `load_from_json(path)` | 从 JSON 构建配置（缺字段/未知字段/非法值抛 `ValueError`） |
|
| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） |
|
||||||
| `StereoRanger(config=None)` | 测距器主类 |
|
| `.config` / `.matcher` | 只读访问（配置与匹配器） |
|
||||||
| `.update(left_dets, right_dets, frame_sequence=0, timestamp_s=None) -> list[RangeReport]` | 单帧主流程（返回记录数与左目检测数一致、顺序一致） |
|
| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||||
| `.reset()` / `.config` | 清空滤波状态 / 当前配置 |
|
| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） |
|
||||||
| `RangeReport` | 输出记录（字段见 `types.py`） |
|
| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） |
|
||||||
| `focal_from_hfov(w_px, hfov_deg)` / `range_error_abs(z, fx, B, δD=1)` / `range_error_rel(...)` | 工具函数（§5.2/§5.3） |
|
| `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、同缩放），由检测前端保证；
|
## 单帧流程（`update` 五步）
|
||||||
2. 传入本模块的框必须已映射回**原生分辨率**（视差在原生坐标计算），内参 `fx/fy/cx/cy` 亦对应原生分辨率；
|
|
||||||
3. 图像对须已完成立体校正（同一目标在两目 y 坐标一致）；校正残留由 `vertical_tau_px` 容错；
|
|
||||||
4. YOLO 推理及左右目并行执行由调用方负责，本模块只收检测结果。
|
|
||||||
|
|
||||||
## 单帧流程（对照规格）
|
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`：有效测距；
|
规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
|
||||||
- `matched=True, valid=False`：已配对但超出有效工作域（§5.4），距离/坐标仍发布供诊断，方位可用；
|
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
|
||||||
- `matched=False, valid=False`：左目检测本帧未配对（§6 失败处理），仅单目方位回退。
|
|
||||||
|
|
||||||
方位源 `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 | 校正后内参（原生分辨率） |
|
| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 |
|
||||||
| `baseline_m` | 0.06 | 基线 B |
|
| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 |
|
||||||
| `z_min_m / z_max_m` | 1.0 / 50.0 | 距离窗口（派生视差关联区间） |
|
| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` |
|
||||||
| `vertical_tau_px` / `height_ratio_tau` | 2.0 / 0.25 | 关联阈值 τy / τh |
|
| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） |
|
||||||
| `disparity_min_px` / `range_tol_m` | 3.0 / 2.0 | 有效性判据 D_min / Z_tol |
|
| 告警计数 ×异常类 | `WarnThrottle` | 0 | 每次异常 |
|
||||||
| `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 | 诊断日志节流间隔 |
|
|
||||||
|
|
||||||
## 运行
|
## 测试与演示
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
cd utils
|
cd utils
|
||||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全部测试（149 条）
|
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条
|
||||||
"D:/program file/conda/envs/pytorch/python.exe" demo_stereo_ranger.py # 合成场景 demo
|
"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/检测器及其预处理（由调用方按上文约定接入）；
|
- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对；
|
||||||
- 亚像素边缘精化、跨帧跟踪（`track_id` 由外部追踪器提供后，时间滤波即可用）、与主测距传感器融合（§5.5/§11）。
|
- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长；
|
||||||
|
- 有效性：三道门与边界归类、预算门通过/拦截两侧；
|
||||||
|
- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零；
|
||||||
|
- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、
|
||||||
|
空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。
|
||||||
|
|
||||||
|
demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布）
|
||||||
|
③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。
|
||||||
|
|||||||
@@ -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 .config import StereoConfig, load_from_json
|
||||||
from .stereo_ranger import (
|
from .consistency import ConsistencyMonitor, ConsistencyStatus
|
||||||
DISP_SOURCE_CENTER,
|
from .distance_filter import DistanceFilter
|
||||||
DISP_SOURCE_EDGE,
|
from .geometry import (
|
||||||
StereoRanger,
|
DELTA_D_PX,
|
||||||
focal_from_hfov,
|
RangeSolution,
|
||||||
range_error_abs,
|
ValidityDecision,
|
||||||
|
confidence_from_cost,
|
||||||
|
evaluate_validity,
|
||||||
|
forward_distance_m,
|
||||||
|
range_error_abs_m,
|
||||||
range_error_rel,
|
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__ = [
|
__all__ = [
|
||||||
"BBoxXYXY",
|
"BBoxXYXY",
|
||||||
"DISP_SOURCE_CENTER",
|
"DELTA_D_PX",
|
||||||
"DISP_SOURCE_EDGE",
|
"ConsistencyMonitor",
|
||||||
|
"ConsistencyStatus",
|
||||||
|
"Degradation",
|
||||||
"Detection",
|
"Detection",
|
||||||
"RangeReport",
|
"DetectionFrame",
|
||||||
|
"Disparity",
|
||||||
|
"DisparitySource",
|
||||||
|
"DistanceFilter",
|
||||||
|
"Eye",
|
||||||
|
"GreedyMatcher",
|
||||||
|
"MatchPair",
|
||||||
|
"Matcher",
|
||||||
|
"RangeRecord",
|
||||||
|
"RangeSolution",
|
||||||
|
"SamplePoint",
|
||||||
"StereoConfig",
|
"StereoConfig",
|
||||||
"StereoRanger",
|
"StereoRanger",
|
||||||
"focal_from_hfov",
|
"TargetStore",
|
||||||
|
"ValidityDecision",
|
||||||
|
"WarnThrottle",
|
||||||
|
"confidence_from_cost",
|
||||||
|
"evaluate_validity",
|
||||||
|
"forward_distance_m",
|
||||||
"load_from_json",
|
"load_from_json",
|
||||||
"range_error_abs",
|
"range_error_abs_m",
|
||||||
"range_error_rel",
|
"range_error_rel",
|
||||||
|
"sample_point",
|
||||||
|
"select_disparity",
|
||||||
|
"solve_range",
|
||||||
|
"virtual_center_px",
|
||||||
]
|
]
|
||||||
|
|||||||
@@ -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 json
|
||||||
|
import math
|
||||||
from dataclasses import dataclass, fields
|
from dataclasses import dataclass, fields
|
||||||
from pathlib import Path
|
from typing import Any, Dict
|
||||||
from typing import Any, Dict, Tuple
|
|
||||||
|
# JSON 加载时的整数型字段（其余按数值校验）
|
||||||
|
_INT_FIELDS = frozenset({"consistency_N_frames", "consistency_M_frames", "filter_reset_frames"})
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class StereoConfig:
|
class StereoConfig:
|
||||||
# ---- 相机参数（校正后、对应原生分辨率，§3.1 / §4.3）----
|
# ---- 几何组（来自标定 P1/P2，§9.1；⚠全部为占位值）----
|
||||||
fx_px: float = 960.0
|
fx_px: float = 960.0
|
||||||
fy_px: float = 960.0
|
fy_px: float = 960.0
|
||||||
cx_px: float = 960.0
|
cx_px: float = 960.0
|
||||||
cy_px: float = 540.0
|
cy_px: float = 540.0
|
||||||
baseline_m: float = 0.06 # 基线 B
|
baseline_m: float = 0.06
|
||||||
|
|
||||||
# ---- 目标距离窗口：关联硬约束（§6），派生视差区间 [FB/z_max, FB/z_min] ----
|
# ---- 匹配组（§9.2）----
|
||||||
z_min_m: float = 1.0
|
tau_y_px: float = 2.0
|
||||||
z_max_m: float = 50.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）----
|
# ---- 有效性组（§9.3）----
|
||||||
vertical_tau_px: float = 2.0 # 垂直残差 tau_y
|
max_delta_z_m: float = 2.0 # 误差预算 δZ(δD = 1px) 上限
|
||||||
height_ratio_tau: float = 0.25 # 相对高度差 tau_h
|
consistency_N_frames: int = 3 # 连续跳变降置信度的帧数
|
||||||
|
consistency_M_frames: int = 5 # 连续异常判 invalid 的帧数
|
||||||
|
consistency_jump_px: float = 3.0 # ⚠补充项：§9 未列，§6.7 的视差跳变门限
|
||||||
|
|
||||||
# ---- 视差取点与有效性（§5）----
|
# ---- 滤波组（§9.4）----
|
||||||
disparity_min_px: float = 3.0 # D_min：D 低于此值直接无效
|
smooth_alpha: float = 0.3
|
||||||
range_tol_m: float = 2.0 # Z_tol：δZ <= Z_tol 才算有效
|
jump_reset_m: float = 5.0
|
||||||
disp_noise_px: float = 1.0 # δD（误差模型中的边缘定位噪声）
|
filter_reset_frames: int = 5
|
||||||
use_center_fallback: bool = True # 左边缘视差越窗时回退中心（§5.1）
|
|
||||||
|
|
||||||
# ---- 可选时间滤波（§7，一阶低通 + 跳变重置）----
|
# ---- 目标隔离（§4：类别 + 框位置最近邻；⚠补充项）----
|
||||||
filter_enabled: bool = False
|
isolation_gate_px: float = 50.0
|
||||||
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
|
|
||||||
|
|
||||||
def __post_init__(self) -> None:
|
def __post_init__(self) -> None:
|
||||||
_require(self.fx_px > 0.0, "fx_px", "必须为正")
|
_require(self.fx_px > 0.0, "fx_px", "必须为正")
|
||||||
@@ -52,31 +53,71 @@ class StereoConfig:
|
|||||||
_require(self.cx_px >= 0.0, "cx_px", "必须非负")
|
_require(self.cx_px >= 0.0, "cx_px", "必须非负")
|
||||||
_require(self.cy_px >= 0.0, "cy_px", "必须非负")
|
_require(self.cy_px >= 0.0, "cy_px", "必须非负")
|
||||||
_require(self.baseline_m > 0.0, "baseline_m", "必须为正")
|
_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.tau_y_px > 0.0, "tau_y_px", "必须为正")
|
||||||
_require(self.vertical_tau_px > 0.0, "vertical_tau_px", "必须为正")
|
_require(self.tau_h_ratio > 0.0, "tau_h_ratio", "必须为正")
|
||||||
_require(self.height_ratio_tau > 0.0, "height_ratio_tau", "必须为正")
|
_require(
|
||||||
_require(self.disparity_min_px > 0.0, "disparity_min_px", "必须为正")
|
0.0 < self.disparity_min_px < self.disparity_max_px,
|
||||||
_require(self.range_tol_m > 0.0, "range_tol_m", "必须为正")
|
"disparity_min_px/disparity_max_px",
|
||||||
_require(self.disp_noise_px > 0.0, "disp_noise_px", "必须为正")
|
"须满足 0 < 下限 < 上限",
|
||||||
_require(0.0 < self.filter_alpha <= 1.0, "filter_alpha", "须落在 (0, 1]")
|
)
|
||||||
_require(self.filter_jump_ratio >= 0.0, "filter_jump_ratio", "必须非负")
|
_require(self.max_delta_z_m > 0.0, "max_delta_z_m", "必须为正")
|
||||||
_require(self.warn_interval_s >= 0.0, "warn_interval_s", "必须非负")
|
_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:
|
def load_from_json(path: str) -> StereoConfig:
|
||||||
"""从 JSON 文件构建配置。字段缺失或非法取值抛 ValueError（携带字段名）。"""
|
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||||
with open(path, encoding="utf-8") as f:
|
with open(path, encoding="utf-8") as handle:
|
||||||
data: Dict[str, Any] = json.load(f)
|
data: Dict[str, Any] = json.load(handle)
|
||||||
known = {field.name for field in fields(StereoConfig)}
|
if not isinstance(data, dict):
|
||||||
unknown = set(data) - known
|
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||||
|
known = {item.name for item in fields(StereoConfig)}
|
||||||
|
unknown = sorted(set(data) - known)
|
||||||
if unknown:
|
if unknown:
|
||||||
raise ValueError(f"未知配置字段: {sorted(unknown)}")
|
raise ValueError(f"未知配置字段: {unknown}")
|
||||||
missing = known - set(data)
|
missing = sorted(known - set(data))
|
||||||
if missing:
|
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)
|
return StereoConfig(**data)
|
||||||
|
|
||||||
|
|
||||||
def _require(ok: bool, field_name: str, message: str) -> None:
|
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||||
if not ok:
|
if not condition:
|
||||||
raise ValueError(f"配置非法: {field_name} {message}")
|
raise ValueError(f"配置非法: {field_name} {message}")
|
||||||
|
|||||||
@@ -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()
|
||||||
@@ -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()
|
||||||
@@ -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))
|
||||||
@@ -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
|
||||||
@@ -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
|
||||||
@@ -1,215 +1,227 @@
|
|||||||
"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。
|
"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。
|
||||||
|
|
||||||
输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做
|
单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 →
|
||||||
图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→
|
④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。
|
||||||
Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。
|
|
||||||
依赖仅为标准库。
|
输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像
|
||||||
|
采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例
|
||||||
|
不得并发调用 update()。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import logging
|
import logging
|
||||||
import math
|
from typing import List, Optional, Sequence
|
||||||
import time
|
|
||||||
from typing import Dict, List, Optional, Sequence, Tuple
|
|
||||||
|
|
||||||
from .config import StereoConfig
|
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")
|
logger = logging.getLogger("stereo_ranger")
|
||||||
|
|
||||||
DISP_SOURCE_EDGE = "left_edge"
|
_CONFIDENCE_DEGRADE_FACTOR = 0.5 # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4）
|
||||||
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)
|
|
||||||
|
|
||||||
|
|
||||||
class StereoRanger:
|
class StereoRanger:
|
||||||
"""双目稀疏目标测距器。
|
"""双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。"""
|
||||||
|
|
||||||
用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。
|
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()
|
||||||
def __init__(self, config: Optional[StereoConfig] = None):
|
self._store = TargetStore(self._config)
|
||||||
self._cfg = config if config is not None else StereoConfig()
|
self._warn = WarnThrottle()
|
||||||
self._z_state: Dict[Tuple[int, int], float] = {} # (cls_id, track_id) -> 滤波 Z
|
self._frame_index = 0
|
||||||
self._warn_at: Dict[str, float] = {}
|
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def config(self) -> StereoConfig:
|
def config(self) -> StereoConfig:
|
||||||
return self._cfg
|
return self._config
|
||||||
|
|
||||||
|
@property
|
||||||
|
def matcher(self) -> Matcher:
|
||||||
|
return self._matcher
|
||||||
|
|
||||||
def reset(self) -> None:
|
def reset(self) -> None:
|
||||||
self._z_state.clear()
|
"""清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。"""
|
||||||
self._warn_at.clear()
|
self._store.clear()
|
||||||
|
self._warn.clear()
|
||||||
|
self._frame_index = 0
|
||||||
|
|
||||||
def update(
|
def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]:
|
||||||
self,
|
"""单周期处理（§5）：返回本帧记录列表。
|
||||||
left: Sequence[Detection],
|
|
||||||
right: Sequence[Detection],
|
|
||||||
frame_sequence: int = 0,
|
|
||||||
timestamp_s: Optional[float] = None,
|
|
||||||
) -> List[RangeReport]:
|
|
||||||
"""单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。
|
|
||||||
|
|
||||||
配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的
|
顺序：先按左目框顺序给出各自记录（配对→完整记录 / 未配对→仅方位），
|
||||||
valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False，
|
再给出未配对的右目框的仅方位记录；左目存在时记录数与左目框数一致。
|
||||||
仅发布单目方位（§6 失败处理）。
|
|
||||||
"""
|
"""
|
||||||
pairs = self._match(left, right)
|
config = self._config
|
||||||
matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs}
|
if left.frame_id != right.frame_id:
|
||||||
reports: List[RangeReport] = []
|
self._warn_emit(
|
||||||
for l_idx, det_l in enumerate(left):
|
"frame_mismatch",
|
||||||
r_idx = matched_r.get(l_idx)
|
"左右帧标识不一致（L=%s, R=%s），拒收本帧",
|
||||||
if r_idx is None:
|
left.frame_id,
|
||||||
reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s))
|
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
|
continue
|
||||||
report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s)
|
records.append(
|
||||||
reports.append(report)
|
self._complete_record(
|
||||||
return reports
|
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]]:
|
def _complete_record(
|
||||||
"""硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。"""
|
self,
|
||||||
cfg = self._cfg
|
point_l: SamplePoint,
|
||||||
d_lo, d_hi = cfg.disparity_bounds_px()
|
point_r: SamplePoint,
|
||||||
candidates: List[Tuple[int, int, Detection, float]] = []
|
pair: MatchPair,
|
||||||
for i, det_l in enumerate(left):
|
frame_id: int,
|
||||||
for j, det_r in enumerate(right):
|
now_ms: int,
|
||||||
if det_l.cls_id != det_r.cls_id:
|
frame_index: int,
|
||||||
continue
|
) -> RangeRecord:
|
||||||
dy = abs(det_l.cy - det_r.cy)
|
"""配对成功的记录：测距 + 方位源 + 有效性（门①②）+ 一致性（门③）+ 滤波。"""
|
||||||
if dy > cfg.vertical_tau_px:
|
config = self._config
|
||||||
continue
|
box_l = point_l.box
|
||||||
h_max = max(det_l.height, det_r.height)
|
box_r = point_r.box
|
||||||
if h_max <= 0.0:
|
disparity = select_disparity(point_l, point_r, config)
|
||||||
continue
|
center_mid = virtual_center_px(box_l, box_r)
|
||||||
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 _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport:
|
if disparity.fallback_used:
|
||||||
cfg = self._cfg
|
self._warn_emit("center_fallback", "边缘视差越窗，回退框中心（框宽差可能混入视差）")
|
||||||
d_edge = det_l.x1 - det_r.x1
|
if not disparity.positive:
|
||||||
d_lo, d_hi = cfg.disparity_bounds_px()
|
self._warn_emit("disparity_violation", "视差违例 D=%.2f px（须为正），该目标判无效", disparity.d_px)
|
||||||
if d_lo <= d_edge <= d_hi:
|
elif not disparity.in_domain:
|
||||||
d = d_edge
|
self._warn_emit(
|
||||||
source = DISP_SOURCE_EDGE
|
"disparity_violation",
|
||||||
else:
|
"视差超界 D=%.2f px（域 [%.2f, %.2f]），该目标判无效",
|
||||||
d = det_l.cx - det_r.cx
|
disparity.d_px,
|
||||||
source = DISP_SOURCE_CENTER
|
config.disparity_min_px,
|
||||||
z = cfg.fx_px * cfg.baseline_m / d
|
config.disparity_max_px,
|
||||||
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),
|
|
||||||
)
|
)
|
||||||
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:
|
key = self._store.resolve(
|
||||||
report.valid = True
|
box_l.class_id,
|
||||||
z_out = self._filter_z(report.class_id, report.track_id, z)
|
_shared_track_id(box_l, box_r),
|
||||||
report.forward_distance_m = z_out
|
center_mid,
|
||||||
report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px)
|
frame_index,
|
||||||
|
)
|
||||||
|
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:
|
else:
|
||||||
report.error_est_m = err
|
self._store.distance_filter.invalidate(key)
|
||||||
self._fill_xyz(report, det_l.cx)
|
z_out = None
|
||||||
return report
|
|
||||||
|
|
||||||
def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None:
|
# 参考斜距与已发布的前向距离同基准：X/Y/Z 均 ∝ Z，按 z_out/z_raw 缩放即精确换基
|
||||||
"""由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。
|
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），
|
return RangeRecord(
|
||||||
而 X 定义在左目坐标系，二者相差 B/2，不得混用。
|
frame_id=frame_id,
|
||||||
"""
|
class_id=box_l.class_id,
|
||||||
cfg = self._cfg
|
degradation=Degradation.COMPLETE,
|
||||||
z = report.forward_distance_m
|
valid=valid,
|
||||||
x = (x_l_px - cfg.cx_px) * z / cfg.fx_px
|
track_id=box_l.track_id if box_l.track_id is not None else box_r.track_id,
|
||||||
y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px
|
forward_distance_m=z_out,
|
||||||
report.lateral_m = x
|
slant_range_m=slant_range_m,
|
||||||
report.vertical_m = y
|
disparity_px=disparity.d_px,
|
||||||
report.slant_range_m = math.sqrt(x * x + y * y + z * z)
|
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:
|
def _bearing_only_record(
|
||||||
"""可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。"""
|
self,
|
||||||
cfg = self._cfg
|
point: SamplePoint,
|
||||||
if not cfg.filter_enabled or track_id == 0:
|
frame_id: int,
|
||||||
return z
|
now_ms: int,
|
||||||
key = (cls_id, track_id)
|
eye: Eye,
|
||||||
prev = self._z_state.get(key)
|
) -> RangeRecord:
|
||||||
if prev is None:
|
"""单目退化记录（§5 第 ⑤ 步）：仅方位，valid = false，无距离。"""
|
||||||
self._z_state[key] = z
|
detection = point.box
|
||||||
return z
|
return RangeRecord(
|
||||||
if abs(z - prev) / prev > cfg.filter_jump_ratio:
|
frame_id=frame_id,
|
||||||
self._z_state[key] = z # 跳变：重置
|
class_id=detection.class_id,
|
||||||
return z
|
degradation=Degradation.BEARING_ONLY,
|
||||||
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,
|
|
||||||
valid=False,
|
valid=False,
|
||||||
confidence=det.conf,
|
track_id=detection.track_id,
|
||||||
centre_pixel_x_mid=det.cx,
|
center_mid_px=(detection.cx, detection.cy),
|
||||||
centre_pixel_y_mid=det.cy,
|
reference_box=detection.bbox,
|
||||||
ref_bbox=(det.x1, det.y1, det.x2, det.y2),
|
reference_eye=eye,
|
||||||
|
confidence=detection.confidence,
|
||||||
|
timestamp_ms=now_ms,
|
||||||
)
|
)
|
||||||
|
|
||||||
def _warn(self, category: str, message: str) -> None:
|
def _drop_invalid(self, points: Sequence[SamplePoint]) -> List[SamplePoint]:
|
||||||
"""节流诊断（§7：失败日志须节流）。"""
|
"""丢弃坐标非有限值的框（不可参与匹配，也不发布记录），告警节流。"""
|
||||||
now = time.monotonic()
|
kept: List[SamplePoint] = []
|
||||||
last = self._warn_at.get(category)
|
for point in points:
|
||||||
if last is not None and now - last < self._cfg.warn_interval_s:
|
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
|
return
|
||||||
self._warn_at[category] = now
|
logger.warning("[%s] " + message + "（累计 %d 次）", category, *args, self._warn.count(category))
|
||||||
logger.warning("[%s] %s", category, message)
|
|
||||||
|
|
||||||
|
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
|
||||||
|
|||||||
@@ -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
|
||||||
@@ -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 dataclasses import dataclass, field
|
||||||
from typing import Optional, Tuple
|
from enum import Enum, auto
|
||||||
|
from typing import Optional, Sequence, Tuple
|
||||||
|
|
||||||
BBoxXYXY = Tuple[float, float, float, float]
|
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
|
@dataclass
|
||||||
class Detection:
|
class Detection:
|
||||||
"""校正后原生坐标系下的一个检测框（单位 px）。
|
"""校正后图像坐标系下的一个检测框（px；x1/y1 左上角，x2/y2 右下角）。"""
|
||||||
|
|
||||||
调用方负责：左右目预处理几何完全一致，且框坐标已映射回原生分辨率
|
|
||||||
（规格 §4.3）。x1/y1 为左上角，x2/y2 为右下角（x1 即"左边缘"）。
|
|
||||||
"""
|
|
||||||
|
|
||||||
x1: float
|
x1: float
|
||||||
y1: float
|
y1: float
|
||||||
x2: float
|
x2: float
|
||||||
y2: float
|
y2: float
|
||||||
conf: float = 1.0
|
confidence: float = 1.0
|
||||||
cls_id: int = 0
|
class_id: int = 0
|
||||||
track_id: int = 0 # 未知为 0
|
track_id: Optional[int] = None # 检测侧轨道关联；None = 无（§2.2，v1.0 可无）
|
||||||
|
|
||||||
@property
|
@property
|
||||||
def width(self) -> float:
|
def width(self) -> float:
|
||||||
@@ -41,31 +62,46 @@ class Detection:
|
|||||||
def cy(self) -> float:
|
def cy(self) -> float:
|
||||||
return (self.y1 + self.y2) / 2.0
|
return (self.y1 + self.y2) / 2.0
|
||||||
|
|
||||||
|
@property
|
||||||
|
def bbox(self) -> BBoxXYXY:
|
||||||
|
return (self.x1, self.y1, self.x2, self.y2)
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class RangeReport:
|
class DetectionFrame:
|
||||||
"""单目标单帧测距结果（一个左目检测对应一条记录）。
|
"""单目一帧：帧标识 + 检测框列表。
|
||||||
|
|
||||||
matched=False 表示该左目检测本帧未配对：valid 恒为 False，距离字段为
|
同帧契约（§2.1）：左右两帧的 frame_id 必须一致，否则整帧拒收、输出空。
|
||||||
None，centre_pixel_x/y_mid 为单目自身中心（方位回退，规格 §11.3）。
|
|
||||||
matched=True 且 valid=False 表示已配对但距离超出有效工作域
|
|
||||||
（规格 §5.4）：距离字段仍给出估计值供诊断，方位照常发布。
|
|
||||||
"""
|
"""
|
||||||
|
|
||||||
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
|
class_id: int
|
||||||
track_id: int
|
degradation: Degradation
|
||||||
matched: bool
|
|
||||||
valid: bool
|
valid: bool
|
||||||
forward_distance_m: Optional[float] = None # Z，前向分量（主输出）
|
track_id: Optional[int] = None
|
||||||
lateral_m: Optional[float] = None # X，左目系横向偏移
|
forward_distance_m: Optional[float] = None
|
||||||
vertical_m: Optional[float] = None # Y，左目系纵向偏移
|
slant_range_m: Optional[float] = None
|
||||||
slant_range_m: Optional[float] = None # R = sqrt(X^2 + Y^2 + Z^2)
|
disparity_px: Optional[float] = None
|
||||||
disparity_px: Optional[float] = None # D = xL - xR（>0 为前方）
|
center_mid_px: Tuple[float, float] = (0.0, 0.0)
|
||||||
disparity_source: Optional[str] = None # "left_edge" / "center"
|
reference_box: Optional[BBoxXYXY] = None
|
||||||
|
reference_eye: Eye = Eye.LEFT
|
||||||
confidence: float = 0.0
|
confidence: float = 0.0
|
||||||
error_est_m: Optional[float] = None # δZ 估计（δD = 配置 disp_noise_px）
|
timestamp_ms: int = 0
|
||||||
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
|
|
||||||
|
|||||||
@@ -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()
|
||||||
@@ -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))
|
|
||||||
@@ -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)
|
|
||||||
@@ -3,8 +3,9 @@
|
|||||||
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
|
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
|
||||||
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
|
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
|
||||||
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
|
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
|
||||||
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
|
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
|
||||||
无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
|
||||||
|
限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||||
"""
|
"""
|
||||||
|
|
||||||
import math
|
import math
|
||||||
@@ -26,7 +27,7 @@ def make_config() -> TrackingConfig:
|
|||||||
cfg = TrackingConfig()
|
cfg = TrackingConfig()
|
||||||
cfg.control.frequency_hz = 20.0
|
cfg.control.frequency_hz = 20.0
|
||||||
cfg.control.visual_stale_ms = 200
|
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.control.attitude_stale_ms = 300
|
||||||
cfg.camera.image_width = 1000
|
cfg.camera.image_width = 1000
|
||||||
cfg.camera.image_height = 1000
|
cfg.camera.image_height = 1000
|
||||||
@@ -55,7 +56,7 @@ def make_config() -> TrackingConfig:
|
|||||||
|
|
||||||
|
|
||||||
def make_snapshot(now_ms: int) -> ControlSnapshot:
|
def make_snapshot(now_ms: int) -> ControlSnapshot:
|
||||||
"""新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
|
"""新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
|
||||||
return ControlSnapshot(
|
return ControlSnapshot(
|
||||||
track=TrackState(tracked=True, is_predicted=False,
|
track=TrackState(tracked=True, is_predicted=False,
|
||||||
center=(500.0, 500.0)),
|
center=(500.0, 500.0)),
|
||||||
@@ -213,9 +214,9 @@ def test_vertical_velocity_clamped():
|
|||||||
def test_distance_approach_when_far():
|
def test_distance_approach_when_far():
|
||||||
law = TrackingControlLaw(make_config())
|
law = TrackingControlLaw(make_config())
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
snap.target_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
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():
|
def test_distance_retreat_clamped_when_too_close():
|
||||||
law = TrackingControlLaw(make_config())
|
law = TrackingControlLaw(make_config())
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
snap.target_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
|
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
|
||||||
|
|
||||||
|
|
||||||
def test_no_distance_hold_zeros_vx():
|
def test_no_distance_hold_zeros_vx():
|
||||||
law = TrackingControlLaw(make_config()) # 缺省 kHold
|
law = TrackingControlLaw(make_config()) # 基准配置 HOLD
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.associated_to_target = False
|
snap.distance_valid = False
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||||
assert out.vx_mps == 0.0
|
assert out.vx_mps == 0.0
|
||||||
@@ -244,29 +245,79 @@ def test_no_distance_slow_approach():
|
|||||||
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
||||||
law = TrackingControlLaw(cfg)
|
law = TrackingControlLaw(cfg)
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.associated_to_target = False
|
snap.distance_valid = False
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
|
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 = make_config()
|
||||||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||||||
|
cfg.distance.no_distance_exit_grace_ms = 200
|
||||||
law = TrackingControlLaw(cfg)
|
law = TrackingControlLaw(cfg)
|
||||||
snap = make_snapshot(1000)
|
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)
|
out = law.update(snap, 1000)
|
||||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||||||
assert not out.valid
|
assert not out.valid
|
||||||
|
|
||||||
|
|
||||||
def test_stale_radar_falls_back_to_no_distance():
|
def test_stale_distance_falls_back_to_no_distance():
|
||||||
law = TrackingControlLaw(make_config()) # kHold
|
law = TrackingControlLaw(make_config()) # HOLD
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.radar_distance_m = 30.0
|
snap.target_distance_m = 30.0
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
snap.distance_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||||
assert out.vx_mps == 0.0
|
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
|
cfg.accel_limit.ax_mps2 = 2.0
|
||||||
law = TrackingControlLaw(cfg)
|
law = TrackingControlLaw(cfg)
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.radar_distance_m = 30.0 # 通道输出本应为 3.0
|
snap.target_distance_m = 30.0 # 通道输出本应为 3.0
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
|
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
|
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())
|
law = TrackingControlLaw(make_config())
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.yaw_rad = math.pi / 2.0 # 机头朝东
|
snap.yaw_rad = math.pi / 2.0 # 机头朝东
|
||||||
snap.radar_distance_m = 30.0
|
snap.target_distance_m = 30.0
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||||||
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
|
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())
|
law = TrackingControlLaw(make_config())
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.yaw_rad = 0.0
|
snap.yaw_rad = 0.0
|
||||||
snap.radar_distance_m = 30.0
|
snap.target_distance_m = 30.0
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
out = law.update(snap, 1000)
|
out = law.update(snap, 1000)
|
||||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||||||
assert out.vy_mps == pytest.approx(0.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
|
cfg.accel_limit.ax_mps2 = 2.0
|
||||||
law = TrackingControlLaw(cfg)
|
law = TrackingControlLaw(cfg)
|
||||||
snap = make_snapshot(1000)
|
snap = make_snapshot(1000)
|
||||||
snap.radar_distance_m = 30.0
|
snap.target_distance_m = 30.0
|
||||||
snap.associated_to_target = True
|
snap.distance_valid = True
|
||||||
snap.radar_time_ms = 1000
|
snap.distance_time_ms = 1000
|
||||||
for t in range(1000, 3000, 50):
|
for t in range(1000, 3000, 50):
|
||||||
snap.track_time_ms = t
|
snap.track_time_ms = t
|
||||||
snap.radar_time_ms = t
|
snap.distance_time_ms = t
|
||||||
snap.attitude_time_ms = t
|
snap.attitude_time_ms = t
|
||||||
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
|
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
|
||||||
law.reset()
|
law.reset()
|
||||||
snap.track_time_ms = 3000
|
snap.track_time_ms = 3000
|
||||||
snap.radar_time_ms = 3000
|
snap.distance_time_ms = 3000
|
||||||
snap.attitude_time_ms = 3000
|
snap.attitude_time_ms = 3000
|
||||||
out = law.update(snap, 3000)
|
out = law.update(snap, 3000)
|
||||||
# Reset 后限幅器归零：又从 0.1 开始渐变
|
# Reset 后限幅器归零：又从 0.1 开始渐变
|
||||||
|
|||||||
@@ -15,7 +15,7 @@ from pathlib import Path
|
|||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
||||||
load_from_json)
|
TrackingConfig, load_from_json)
|
||||||
|
|
||||||
_REPO_ROOT = Path(__file__).resolve().parents[3]
|
_REPO_ROOT = Path(__file__).resolve().parents[3]
|
||||||
REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
|
REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
|
||||||
@@ -27,7 +27,7 @@ def full_config() -> dict:
|
|||||||
"control": {
|
"control": {
|
||||||
"frequency_hz": 20.0,
|
"frequency_hz": 20.0,
|
||||||
"visual_stale_ms": 200,
|
"visual_stale_ms": 200,
|
||||||
"radar_stale_ms": 500,
|
"distance_stale_ms": 500,
|
||||||
"attitude_stale_ms": 300,
|
"attitude_stale_ms": 300,
|
||||||
},
|
},
|
||||||
"camera": {
|
"camera": {
|
||||||
@@ -57,6 +57,7 @@ def full_config() -> dict:
|
|||||||
"retreat_velocity_limit_mps": 1.0,
|
"retreat_velocity_limit_mps": 1.0,
|
||||||
"no_distance_action": "hold",
|
"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": {
|
"accel_limit": {
|
||||||
"ax_mps2": 2.0,
|
"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_gain_mps_per_deg == 0.1
|
||||||
assert cfg.vertical.vz_limit_mps == 2.0
|
assert cfg.vertical.vz_limit_mps == 2.0
|
||||||
assert cfg.distance.d_exp_m == 10.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
|
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:
|
with pytest.raises(ValueError) as exc_info:
|
||||||
load_from_json(path)
|
load_from_json(path)
|
||||||
assert "camera.fov_h_deg" in str(exc_info.value)
|
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)
|
||||||
|
|||||||
@@ -9,7 +9,7 @@
|
|||||||
| 名称 | 说明 |
|
| 名称 | 说明 |
|
||||||
| --- | --- |
|
| --- | --- |
|
||||||
| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
|
| `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)` | 主控律 |
|
| `TrackingControlLaw(cfg)` | 主控律 |
|
||||||
| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
|
| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
|
||||||
| `.reset()` | 停止闭环时清全部内部状态 |
|
| `.reset()` | 停止闭环时清全部内部状态 |
|
||||||
|
|||||||
@@ -23,18 +23,19 @@ class HeadingMode(Enum):
|
|||||||
|
|
||||||
|
|
||||||
class NoDistanceAction(Enum):
|
class NoDistanceAction(Enum):
|
||||||
"""雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
|
"""目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）。"""
|
||||||
|
|
||||||
HOLD = "hold" # 保持：前向速度指令清零
|
HOLD = "hold" # 保持：前向速度指令清零
|
||||||
SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速
|
SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速
|
||||||
EXIT = "exit" # 退出近距操作：输出降级保持
|
EXIT = "exit" # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
|
||||||
|
# 才降级保持；grace 期间按 SLOW_APPROACH 行为
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
class ControlGroupConfig:
|
class ControlGroupConfig:
|
||||||
frequency_hz: float = 20.0
|
frequency_hz: float = 20.0
|
||||||
visual_stale_ms: int = 200
|
visual_stale_ms: int = 200
|
||||||
radar_stale_ms: int = 500
|
distance_stale_ms: int = 500
|
||||||
attitude_stale_ms: int = 300
|
attitude_stale_ms: int = 300
|
||||||
|
|
||||||
|
|
||||||
@@ -70,8 +71,11 @@ class DistanceGroupConfig:
|
|||||||
derivative_filter_coef: float = 0.0
|
derivative_filter_coef: float = 0.0
|
||||||
approach_velocity_limit_mps: float = 3.0
|
approach_velocity_limit_mps: float = 3.0
|
||||||
retreat_velocity_limit_mps: float = 1.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_approach_limit_mps: float = 0.5
|
||||||
|
no_distance_exit_grace_ms: int = 3000 # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）
|
||||||
|
|
||||||
|
|
||||||
@dataclass
|
@dataclass
|
||||||
@@ -155,9 +159,9 @@ def _load_control(root: dict) -> ControlGroupConfig:
|
|||||||
c.frequency_hz = _require_positive(
|
c.frequency_hz = _require_positive(
|
||||||
_require_number(s, "control", "frequency_hz"), "control.frequency_hz")
|
_require_number(s, "control", "frequency_hz"), "control.frequency_hz")
|
||||||
c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
|
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")
|
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 必须为正")
|
raise ValueError("配置非法: control.*_stale_ms 必须为正")
|
||||||
return c
|
return c
|
||||||
|
|
||||||
@@ -240,6 +244,10 @@ def _load_distance(root: dict) -> DistanceGroupConfig:
|
|||||||
d.no_distance_approach_limit_mps = _require_non_negative(
|
d.no_distance_approach_limit_mps = _require_non_negative(
|
||||||
_require_number(s, "distance", "no_distance_approach_limit_mps"),
|
_require_number(s, "distance", "no_distance_approach_limit_mps"),
|
||||||
"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
|
return d
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -10,7 +10,8 @@
|
|||||||
④ 像素偏移 → 视线角 β/α（全程针孔模型）
|
④ 像素偏移 → 视线角 β/α（全程针孔模型）
|
||||||
⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
|
⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
|
||||||
⑥ 垂直统一速度支路（A1：不切姿态微调模式）
|
⑥ 垂直统一速度支路（A1：不切姿态微调模式）
|
||||||
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
|
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit，
|
||||||
|
exit 需持续无距离达 no_distance_exit_grace_ms 门限）
|
||||||
⑧ 机体系 → 局部 NED 旋转（A4）
|
⑧ 机体系 → 局部 NED 旋转（A4）
|
||||||
|
|
||||||
日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
|
日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
|
||||||
@@ -78,6 +79,10 @@ class TrackingControlLaw:
|
|||||||
self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
|
self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
|
||||||
self._last_now_ms = 0
|
self._last_now_ms = 0
|
||||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
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:
|
def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
|
||||||
# ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
|
# ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
|
||||||
@@ -107,6 +112,7 @@ class TrackingControlLaw:
|
|||||||
|
|
||||||
# ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
|
# ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
|
||||||
if not snapshot.track.tracked:
|
if not snapshot.track.tracked:
|
||||||
|
self._no_distance_active = False # 刹车期间不计 EXIT 无距离时长
|
||||||
if self._last_mode is not ControlMode.BRAKE_HOVER:
|
if self._last_mode is not ControlMode.BRAKE_HOVER:
|
||||||
# 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
# 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
||||||
self._distance_pid.reset()
|
self._distance_pid.reset()
|
||||||
@@ -156,14 +162,16 @@ class TrackingControlLaw:
|
|||||||
self._cfg.vertical.vz_limit_mps)
|
self._cfg.vertical.vz_limit_mps)
|
||||||
|
|
||||||
# ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
# ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
||||||
# 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
|
# 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
|
||||||
distance_available = (snapshot.associated_to_target
|
# （§6.3、FR-041）
|
||||||
and snapshot.radar_distance_m > 0.0
|
distance_available = (snapshot.distance_valid
|
||||||
and not is_stale(snapshot.radar_time_ms, now_ms,
|
and snapshot.target_distance_m > 0.0
|
||||||
self._cfg.control.radar_stale_ms))
|
and not is_stale(snapshot.distance_time_ms, now_ms,
|
||||||
|
self._cfg.control.distance_stale_ms))
|
||||||
if distance_available:
|
if distance_available:
|
||||||
|
self._no_distance_active = False # 任一有效距离帧重置 EXIT 计时
|
||||||
# Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
|
# Δ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)
|
vx_raw = self._distance_pid.update(delta_d, dt_s)
|
||||||
slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8）
|
slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8）
|
||||||
out.vx_mps = clamp(slewed,
|
out.vx_mps = clamp(slewed,
|
||||||
@@ -182,10 +190,20 @@ class TrackingControlLaw:
|
|||||||
out.vx_mps = self._vx_limiter.limit(
|
out.vx_mps = self._vx_limiter.limit(
|
||||||
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
|
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
|
||||||
else: # NoDistanceAction.EXIT
|
else: # NoDistanceAction.EXIT
|
||||||
# 退出近距操作：整体降级保持
|
# 退出近距操作：双目有效域外无距离是常态（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.mode = ControlMode.DEGRADED_HOLD
|
||||||
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
||||||
return self._finish_and_log(out, "距离未关联，退出近距操作")
|
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 修复） ----
|
# ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
|
||||||
# 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
|
# 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
|
||||||
@@ -207,6 +225,7 @@ class TrackingControlLaw:
|
|||||||
self._yaw_rate_limiter.reset()
|
self._yaw_rate_limiter.reset()
|
||||||
self._vx_limiter.reset()
|
self._vx_limiter.reset()
|
||||||
self._vz_limiter.reset()
|
self._vz_limiter.reset()
|
||||||
|
self._no_distance_active = False
|
||||||
self._last_now_ms = 0
|
self._last_now_ms = 0
|
||||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
self._last_mode = ControlMode.DEGRADED_HOLD
|
||||||
|
|
||||||
|
|||||||
@@ -55,9 +55,10 @@ class ControlSnapshot:
|
|||||||
track: TrackState = field(default_factory=TrackState)
|
track: TrackState = field(default_factory=TrackState)
|
||||||
track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样
|
track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样
|
||||||
|
|
||||||
radar_distance_m: float = 0.0 # 机头前向单点距离（米）
|
# 目标距离（双目立体测距，§6.3：distance_valid 为 False 时不得当作目标距离）
|
||||||
associated_to_target: bool = False # 相机—雷达空间关联成立标志（未关联不得当目标距离）
|
target_distance_m: float = 0.0 # 目标距离（米，双目前向分量 Z）
|
||||||
radar_time_ms: int = 0
|
distance_valid: bool = False # 距离有效标志（测距来自检测框自身，无跨模态关联）
|
||||||
|
distance_time_ms: int = 0 # 取样单调时钟（毫秒）
|
||||||
|
|
||||||
attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制）
|
attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制）
|
||||||
attitude_time_ms: int = 0
|
attitude_time_ms: int = 0
|
||||||
|
|||||||
Reference in New Issue
Block a user