songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
keita
2026-09-10 16:16:05 +08:00
parent 3325465359
commit 538ae4c6c7
39 changed files with 2285 additions and 1444 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/视觉跟踪控制律技术路线.md
+85 -40
View File
@@ -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）。
---