双目测距方案重构与其他小修改
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}\)。
|
||||
|
||||
---
|
||||
|
||||
*文档结束。*
|
||||
+85
-40
@@ -47,7 +47,7 @@
|
||||
1. **三路控制律计算**（每拍一次，固定周期）
|
||||
- **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令；
|
||||
- **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令；
|
||||
- **距离**：雷达目标距离与期望射程之差 Δd → PID → 前向速度指令。
|
||||
- **距离**：双目目标距离与期望射程之差 Δd → PID → 前向速度指令。
|
||||
2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许
|
||||
跳过限幅直达链路（含切换后的**首条**指令）。
|
||||
3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续
|
||||
@@ -63,8 +63,8 @@
|
||||
│ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│
|
||||
└─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘
|
||||
│
|
||||
┌─────────────┐ 前向距离 │ 控制意图
|
||||
│ 激光测距雷达│──────────────────────┐ ▼
|
||||
┌─────────────┐ 目标距离 │ 控制意图
|
||||
│ 双目立体测距│──────────────────────┐ ▼
|
||||
└─────────────┘ │ ┌──────────────────────┐
|
||||
┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │
|
||||
│ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │
|
||||
@@ -78,8 +78,15 @@
|
||||
```
|
||||
|
||||
**上游依赖**：追踪库提供主目标中心与 `tracked` / `is_predicted` 两个行为标志
|
||||
（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；雷达提供机头前向单点距离与
|
||||
**关联标志**；飞控链路提供自机姿态（至少航向角）。
|
||||
（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；双目模块提供目标距离
|
||||
（检测框自身的立体测距，前向分量 Z）与有效标志 `distance_valid`；飞控链路
|
||||
提供自机姿态（至少航向角）。
|
||||
|
||||
**方位源（接入层约定）**：双目模组两枚相机分列模组中线两侧（各偏 ±B/2），
|
||||
单目像素存在近场视差偏差（3m 处约 0.57°）。匹配成功时接入层应以**虚拟双目
|
||||
中心**像素 `x_mid = (xL + xR) / 2` 填充 `track.center`（推导见
|
||||
`docs/双目深度估计思路.md` §11.3）。虚拟中心相机与单目共用同一 FOV 配置，
|
||||
本模块"像素偏移 → 视线角"逻辑与配置**零改动**。
|
||||
|
||||
**下游依赖**：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。
|
||||
|
||||
@@ -100,8 +107,9 @@
|
||||
|
||||
### 1.4 明确不做的事（边界）
|
||||
|
||||
- **不做感知**：不跑检测/光流/雷达驱动，只消费调用方填入的快照；
|
||||
- **不做目标关联**：相机—雷达空间关联由感知融合层给出布尔标志，本模块只读取；
|
||||
- **不做感知**：不跑检测/光流/双目立体匹配，只消费调用方填入的快照；
|
||||
- **不做距离有效性判定**：`distance_valid` 由双目链路自身的一致性校验给出，
|
||||
track_id 一致性校验为调用方约定（§12.2），本模块只读取标志；
|
||||
- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略；
|
||||
- **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息；
|
||||
- **不做配置热更新**：启动期加载一次，调参重启生效；
|
||||
@@ -115,13 +123,13 @@
|
||||
|
||||
| 分组 | 字段 | 单位 / 类型 | 语义与约束 |
|
||||
|------|------|------------|-----------|
|
||||
| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正 |
|
||||
| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正；双目匹配成功时由接入层填虚拟双目中心 `x_mid`（§1.2） |
|
||||
| 视觉 | `track.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 |
|
||||
| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 |
|
||||
| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 |
|
||||
| 雷达 | `radar_distance_m` | m | 机头前向单点距离；`<= 0` 视为不可用 |
|
||||
| 雷达 | `associated_to_target` | 布尔 | 相机—雷达关联成立标志；**未关联不得当作目标距离** |
|
||||
| 雷达 | `radar_time_ms` | 单调时钟 ms | 雷达取样瞬间打戳 |
|
||||
| 距离 | `target_distance_m` | m | 双目目标距离（前向分量 Z）；`<= 0` 视为不可用 |
|
||||
| 距离 | `distance_valid` | 布尔 | 本帧测距可用标志：距离来自目标检测框自身的双目测距；**无效帧的距离不得当作目标距离** |
|
||||
| 距离 | `distance_time_ms` | 单调时钟 ms | 双目距离取样瞬间打戳 |
|
||||
| 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 |
|
||||
| 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 |
|
||||
| 姿态 | `yaw_rad` | rad (NED) | 自机航向；**参与计算**：位置式航向合成 + 输出端坐标旋转 |
|
||||
@@ -189,7 +197,7 @@
|
||||
|
||||
| 构件 | 职责 | 构造期预计算 | 运行期持久状态 | 复位入口 |
|
||||
|------|------|-------------|--------------|---------|
|
||||
| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode` | `Reset()` 清全部 |
|
||||
| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode`、无距离计时（`exit` 门限用，§7.5） | `Reset()` 清全部 |
|
||||
| 视线角转换器 ×2 | 像素偏移 → β / α | 针孔系数 `2·tan(θ/2)/size` | 无（纯函数） | 不需要 |
|
||||
| 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 |
|
||||
| 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` |
|
||||
@@ -320,6 +328,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
||||
|
||||
# ---- ② 判丢 → 零速刹车 ----
|
||||
若 非 快照.track.tracked:
|
||||
无距离计时.清除() # 刹车期间不计 kExit 时长
|
||||
若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态
|
||||
距离PID.全清(); 四个限幅器.复位(0)
|
||||
输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零
|
||||
@@ -354,12 +363,13 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
||||
输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps)
|
||||
|
||||
# ---- ⑦ 距离通道 ----
|
||||
距离可用 ← 快照.associated_to_target
|
||||
且 快照.radar_distance_m > 0
|
||||
且 非 已超龄(快照.radar_time_ms, now_ms, 配置.radar_stale_ms)
|
||||
距离可用 ← 快照.distance_valid
|
||||
且 快照.target_distance_m > 0
|
||||
且 非 已超龄(快照.distance_time_ms, now_ms, 配置.distance_stale_ms)
|
||||
|
||||
若 距离可用:
|
||||
Δd ← 快照.radar_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近
|
||||
无距离计时.清除() # 任一有效帧重置 kExit 计时
|
||||
Δd ← 快照.target_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近
|
||||
前向原始 ← 距离PID.更新(Δd, dt)
|
||||
限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅
|
||||
# 非对称限幅：接近可以快，后退必须慢
|
||||
@@ -370,9 +380,14 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
||||
按 配置.distance.no_distance_action 分支:
|
||||
保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0)
|
||||
限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||||
退出近距操作: 输出.mode ← kDegradedHold
|
||||
输出.degraded_reason ← kNoDistanceExit
|
||||
返回 收尾并记录模式切换(输出, "距离未关联，退出近距操作")
|
||||
退出近距操作:
|
||||
若 非 无距离计时.进行中: 无距离计时.启动(now_ms) # 首个无效拍起计时
|
||||
若 now_ms − 无距离计时.起始时刻 ≥ 配置.no_distance_exit_grace_ms:
|
||||
输出.mode ← kDegradedHold
|
||||
输出.degraded_reason ← kNoDistanceExit
|
||||
返回 收尾并记录模式切换(输出, "持续无距离达门限，退出近距操作")
|
||||
# grace 期内按"限速接近"执行（复用加速度限幅，vx 平滑无跳变）
|
||||
输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt)
|
||||
|
||||
# ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ----
|
||||
旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad)
|
||||
@@ -391,7 +406,7 @@ v_down = v_down # 绕下轴旋转不影响垂直分量
|
||||
| 姿态超龄降级 | 否 | 否 | 否 | `false` |
|
||||
| 视觉超龄降级 | 否 | 否 | 否 | `false` |
|
||||
| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` |
|
||||
| 无距离退出 | 是（仅积分） | 否 | 否 | `false` |
|
||||
| 无距离退出（grace 期满） | 是（仅积分） | 否 | 否 | `false` |
|
||||
| 正常/Coast | 视情况清积分 | 否 | **是** | `true` |
|
||||
|
||||
**降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的
|
||||
@@ -505,9 +520,10 @@ vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限
|
||||
anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效，
|
||||
保证 anti-windup 判据基于"真实饱和边界"。
|
||||
|
||||
**可用性三重门**（缺一不可）：关联标志成立 **且** 距离为正 **且** 未超龄。
|
||||
"未关联的雷达距离"只是机头前方某个障碍物的距离，把它当作目标距离会导致
|
||||
朝着错误目标调整射程。
|
||||
**可用性三重门**（缺一不可）：`distance_valid` 成立 **且** 距离为正 **且** 未超龄。
|
||||
双目测距取自目标检测框自身，不存在"回波打到别处"的跨模态关联问题；标志为
|
||||
`false` 表示本帧测距不可用（有效域外/未匹配/一致性校验失败），不得把任何数值
|
||||
当作目标距离。
|
||||
|
||||
### 6.5 PID 内部算法
|
||||
|
||||
@@ -601,12 +617,12 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
|------|-------------|------|
|
||||
| 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） |
|
||||
| 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 |
|
||||
| 雷达距离 | 500 ms | 雷达更新较慢，且距离不可用有独立降级动作 |
|
||||
| 双目距离 | 500 ms | 双目随推理帧更新，且距离不可用有独立降级动作 |
|
||||
|
||||
### 7.2 降级优先级（顺序即语义）
|
||||
|
||||
```text
|
||||
姿态不可用 > 视觉超龄 > 判丢 > 无距离（退出档）
|
||||
姿态不可用 > 视觉超龄 > 判丢 > 无距离（exit 档，持续达门限）
|
||||
```
|
||||
|
||||
| 优先级 | 检查 | 理由 |
|
||||
@@ -614,7 +630,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） |
|
||||
| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 |
|
||||
| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 |
|
||||
| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级 |
|
||||
| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级；`exit` 档还需持续无距离达门限（§7.5），默认档为 `slow_approach`（§10.8） |
|
||||
|
||||
### 7.3 判丢刹车与状态清理
|
||||
|
||||
@@ -642,19 +658,28 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间
|
||||
误差不反映真实世界，积分累加会把预测偏差固化成饱和量。
|
||||
|
||||
**Coast 期间距离通常不可用**：目标短暂消失往往伴随雷达关联失效，因此第 ⑦ 步会
|
||||
走无距离分支（默认"保持"→ 前向清零）。这是设计上的自然降级，不需要额外分支。
|
||||
**Coast 期间距离通常不可用**：目标短暂消失时双目失去匹配（`distance_valid ==
|
||||
false`），第 ⑦ 步走无距离分支；默认 `slow_approach` 下前向按低速定速推进，
|
||||
方位闭环继续（§7.5）。这是设计上的自然降级，不需要额外分支。
|
||||
|
||||
### 7.5 无距离三档动作
|
||||
|
||||
| 配置值 | 行为 | 适用场景 |
|
||||
|--------|------|---------|
|
||||
| `hold`（默认） | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 |
|
||||
| `slow_approach` | 以配置的低速限幅缓速接近（仍经加速度限幅） | 需要持续压迫、且低速可接受风险 |
|
||||
| `exit` | 整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞），无距离必须退出近距操作 |
|
||||
| `slow_approach`（默认） | 以配置的低速限幅缓速接近（仍经加速度限幅） | **默认档**：双目是机载唯一距离源，有效域外无距离是常态而非异常；即远场策略"方位闭环 + 定速接近"（D4） |
|
||||
| `hold` | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 |
|
||||
| `exit` | 持续无距离达 `no_distance_exit_grace_ms` 才整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞）时的可配置安全策略；持续无距离 ≈ 传感器级故障 |
|
||||
|
||||
三档中只有 `exit` 会中断整拍输出；`hold` 与 `slow_approach` 仍输出航向与垂直
|
||||
通道（继续把目标保持在画面中心）。
|
||||
三档中只有 `exit` 会中断整拍输出，且**延迟到 grace 期满**；其余情况（含 `exit`
|
||||
的 grace 期）仍输出航向与垂直通道（继续把目标保持在画面中心）。
|
||||
|
||||
**`exit` 门限语义**（替代旧版"首个无效拍即退出"）：
|
||||
|
||||
- `distance_available` 首次为 `false` 起计时，任一有效距离帧**重置**计时；
|
||||
- grace 期内前向按 `slow_approach` 低速推进，复用既有加速度限幅，`vx` 平滑
|
||||
过渡无跳变；
|
||||
- 判丢刹车期间不计时（每拍清计时状态），`Reset()` 同样清除；
|
||||
- `no_distance_exit_grace_ms = 0` 保留旧版行为（首个无效拍即退出）。
|
||||
|
||||
---
|
||||
|
||||
@@ -773,7 +798,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
|----|------|-------|------|
|
||||
| `frequency_hz` | Hz | 20.0 | 控制频率；同时作为 dt 兜底值 |
|
||||
| `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 |
|
||||
| `radar_stale_ms` | ms | 500 | 雷达距离超龄阈值 |
|
||||
| `distance_stale_ms` | ms | 500 | 双目距离超龄阈值 |
|
||||
| `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 |
|
||||
|
||||
### 9.2 camera 组：画面与视场
|
||||
@@ -816,8 +841,9 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
| `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） |
|
||||
| `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 |
|
||||
| `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 |
|
||||
| `no_distance_action` | 枚举 | `hold` | `hold` / `slow_approach` / `exit` |
|
||||
| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档的限速值 |
|
||||
| `no_distance_action` | 枚举 | `slow_approach` | `hold` / `slow_approach` / `exit`（默认档依据见 §10.8） |
|
||||
| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档（含 `exit` 的 grace 期）的限速值 |
|
||||
| `no_distance_exit_grace_ms` | ms | 3000 | `exit` 档持续无距离门限（0 = 首个无效拍即退出） |
|
||||
|
||||
### 9.6 accel_limit 组：加速度
|
||||
|
||||
@@ -889,6 +915,23 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断，
|
||||
恢复后必须从零渐变。两者语义不同，清理策略必须不同。
|
||||
|
||||
### 10.8 距离输入从雷达切换为双目：语义简化、默认动作与退出门限（2026-09-10）
|
||||
|
||||
**背景**：目标距离的机载唯一来源改为双目立体测距（决策 D2，替代激光雷达；
|
||||
见 `docs/双目深度估计思路.md`）。
|
||||
|
||||
1. **关联标志语义简化（S2）**：`associated_to_target`（相机—雷达空间关联）
|
||||
→ `distance_valid`（本帧测距可用）。双目距离来自目标检测框自身，"回波未
|
||||
打到目标"的跨模态关联问题消失，标志退化为纯粹的本帧可用性（域外/未匹配/
|
||||
一致性校验失败）；track_id 一致性校验转为调用方约定（§12.2）。
|
||||
2. **默认无距离动作 `hold` → `slow_approach`（D4）**：单雷达时代无距离多为
|
||||
瞬时丢关联，`hold` 占位安全；双目时代有效域外无距离是**常态**，`hold` 会让
|
||||
远场逼近停滞。`slow_approach` 即"方位闭环 + 定速接近"远场策略的库内承载。
|
||||
3. **`exit` 增加持续门限（S1）**：旧版首个无效拍即降级，会把单帧丢检误判为
|
||||
传感器级故障；改为持续无距离达 `no_distance_exit_grace_ms` 才退出，grace 期
|
||||
按 `slow_approach` 行为执行（复用加速度限幅，无跳变），任一有效帧重置。
|
||||
`exit` 语义收敛为"持续无距离 ≈ 传感器级故障"。
|
||||
|
||||
---
|
||||
|
||||
## 11. 实现与重写指引
|
||||
@@ -899,6 +942,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
|------|------|------|---------|
|
||||
| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 |
|
||||
| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） |
|
||||
| 无距离计时（进行中 + 起始时刻） | 主控律 | 未计时 | 首个无距离拍启动（`exit` 档）；有效距离帧 / 判丢刹车 / 复位() 清除 |
|
||||
| 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 |
|
||||
| 上拍误差 | PID | 0 | 每拍 |
|
||||
| 低通后微分 | PID | 0 | 每拍 |
|
||||
@@ -917,7 +961,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
|
||||
控制线程（固定周期，配置频率）:
|
||||
结果 ← 追踪库.取最新结果() # 线程安全拷贝
|
||||
快照 ← 采集(结果, 雷达, 姿态)
|
||||
快照 ← 采集(结果, 双目距离, 姿态)
|
||||
快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳
|
||||
意图 ← 控制律.更新(快照, 当前单调时钟())
|
||||
按 意图.mode 分发:
|
||||
@@ -926,7 +970,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
降级保持 → 不发送（记录降级原因/告警）
|
||||
|
||||
停止视觉闭环（状态机决定）:
|
||||
控制律.复位() # 清积分、清限幅器、清节拍与模式历史
|
||||
控制律.复位() # 清积分、清限幅器、清节拍与模式历史、清无距离计时
|
||||
```
|
||||
|
||||
**约束**：
|
||||
@@ -1003,7 +1047,7 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
| **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 |
|
||||
| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 |
|
||||
| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 |
|
||||
| **雷达关联判据** | 由感知融合层给出标志，本模块不实现 | 融合层设计 | 中：关联错误 = 距离通道朝错误目标调射程 |
|
||||
| **track_id 一致性校验** | 调用方约定：装配层校验双目距离与追踪主目标的 `track_id` 一致并告警，库内不实现（S3） | 主程序装配层落地时加一处校验 | 中：错配会把其他目标的距离当作本目标，距离通道朝错误目标调射程 |
|
||||
|
||||
### 12.3 红线
|
||||
|
||||
@@ -1014,7 +1058,8 @@ Coast 只清积分而不清微分：预测值仍在连续演化，微分历史
|
||||
3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。
|
||||
4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅，
|
||||
包括切换后的首条指令。
|
||||
5. **未关联的雷达距离不得当作目标距离**。
|
||||
5. **`distance_valid == false` 的距离不得当作目标距离**（v1.0 仅单目检出时
|
||||
不发布距离，方位退化为该目偏移，见 `docs/双目深度估计思路.md` §11.3）。
|
||||
|
||||
---
|
||||
|
||||
|
||||
Reference in New Issue
Block a user