keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
268 lines
17 KiB
Markdown
268 lines
17 KiB
Markdown
# L1 航向伺服落地方案（无距离，树莓派 / Python）
|
||
|
||
> 目标：**先把"只修正飞行方向、不需要距离"的部分落地**，速度-距离闭环（L2）滞后。
|
||
> 基于结论：本吊舱无激光 → 表4 永不出，且不允许二次开发（部署文档 §10.0 / §10）。
|
||
> 日期：2026-10-09 起，**2026-10-19 更新** 状态：
|
||
> **S0/S1/S2 + SITL + 真吊舱在环已验证**（全量 621 passed）。
|
||
> 后续变更：第二判据 `require_vision` → **LOS 运动学一致性**（§2.3/§6）；
|
||
> 距离源由「双目」改为规划中的「激光雷达」（§7）。
|
||
|
||
---
|
||
|
||
## 0. 结论先行：L1 不用新写控制律
|
||
|
||
`visual_track_control.TrackingControlLaw` **已经内建"无距离"路径**，第 ⑦ 步：
|
||
|
||
```
|
||
distance_available = snapshot.distance_valid and distance>0 and 未超龄
|
||
├─ True → 距离 PID → 前向速度（这是 L2，滞后做）
|
||
└─ False → 清积分，按 cfg.distance.no_distance_action 三选一：
|
||
HOLD → vx = 0 （只转向，不前近）← 首次上机用这个
|
||
SLOW_APPROACH → vx = 固定保守限速 ← 航向验证通过后用这个
|
||
EXIT → 持续无距离达 grace 才降级
|
||
```
|
||
|
||
所以 L1 = **`TrackingControlLaw` + `distance_valid=False` + 无距离动作配置**，
|
||
航向/垂直通道、限幅、超龄、Coast 清积分、NED 旋转全部复用既有实现
|
||
（该包有 81 个测试保障）。**要新写的只有"胶水层"**：把吊舱表2-2 的角度量喂进去。
|
||
|
||
---
|
||
|
||
## 1. 现有构件盘点（能复用什么）
|
||
|
||
| 构件 | 位置 | L1 怎么用 |
|
||
|---|---|---|
|
||
| `TrackingControlLaw` | `visual_track_control/tracking_control_law.py` | 主控制律，直接用 |
|
||
| `ControlSnapshot` / `TrackState` | `visual_track_control/types.py` | 输入结构，`distance_valid=False` |
|
||
| `PixelToAngle` | `line_of_sight.py` | ❌ **绕过**（见 §2.1，吊舱给的已是角度） |
|
||
| `PidController` / `SlewRateLimiter` | 同包 | 被控制律内部使用 |
|
||
| `PodBridge` 主循环 | `pod_bridge/bridge.py` | 改：把 GuidanceLaw 换成 TrackingControlLaw |
|
||
| `_pod_locked()` | `bridge.py:173` | 目标门控（⚠ 有坑，见 §2.3） |
|
||
| 表2-2 角度量 | `protocol.py:150-160` | `off_target_x/y_deg`、`frame_yaw/pitch_deg` |
|
||
|
||
---
|
||
|
||
## 2. 三个关键设计决策
|
||
|
||
### 2.1 🔴 航向信号要用「框架角 + 脱靶量」，不能只用脱靶量
|
||
|
||
这是本方案**最容易踩空**的地方：
|
||
|
||
> 吊舱锁定后**内部伺服会自己把目标保持在画面中心**，所以脱靶量长期 ≈ 0。
|
||
> 若只用脱靶量驱动航向，**稳态时根本没有信号**，飞机会"不知道该往哪转"。
|
||
|
||
正确的信号是**框架角**（`frame_yaw_deg`，云台相对机体的方位角）：目标居中时它
|
||
依然非零，它就是"目标相对机头的方位"。合成：
|
||
|
||
```
|
||
β = frame_yaw_deg + off_target_x_deg （水平，右正）
|
||
α = frame_pitch_deg + off_target_y_deg （垂直，下正）
|
||
```
|
||
|
||
- 目标居中时：`off_target ≈ 0`，β 由框架角提供（主信号）
|
||
- 云台滞后 / 大偏差时：脱靶量提供动态补充
|
||
|
||
控制律让 β → 0，等价于**把机体航向转向目标、让云台回中**（标准 gimbal-slaved
|
||
航向控制）。这正是"用吊舱修正飞行方向"的正确做法。
|
||
|
||
⚠ **必须在台架标定**：① 框架角是否确为"相对机体"（而非绝对方位）；
|
||
② β/α 的正方向是否与控制律约定（右正 / 下正）一致。**符号错了会反向转**。
|
||
|
||
**额外好处**：走角度输入就**与视场 FOV 无关**——吊舱会变焦，若走像素换算
|
||
（`PixelToAngle` 反推），变焦后 FOV 变化会直接失真。这是不用像素路径的硬理由。
|
||
|
||
> ⚠ **本版吊舱未装电机（决策 2，2026-10-09）**：云台刚性固连机体、朝前不动，
|
||
> `frame_yaw_deg` 恒 0（"框架角无用"），锁定后也**不会**自 servo 回中，故脱靶量
|
||
> 长期**不**≈0。此时 `off_target_x_deg` 恰好 = 目标相对机头的视线角 β，是**唯一**
|
||
> 有效信号。因此合成做成**配置双路**（`beta_from_frame` / `beta_from_off_target`）：
|
||
> - 现在（无电机）：`beta_from_frame=false`、`beta_from_off_target=true` → β = off_target_x
|
||
> - 将来（装电机）：两者皆 `true` → β = frame_yaw + off_target_x（即上文 §2.1）
|
||
>
|
||
> "装不装电机"只是翻配置，架构（姿态来自 PX4、角度来自吊舱）不变。**装电机后 §2.1
|
||
> 的"脱靶量长期≈0、必须靠框架角"警告才重新生效。**
|
||
>
|
||
> **S1 标定结论（2026-10-09）**：厂商确认本版电机无用；实测目标在**左下**时
|
||
> `off_target_x/y` **均为负** → 吊舱 X 轴右正（与控制律 β 同向）故 `beta_sign=+1`；
|
||
> Y 轴**上正**（与控制律 α"下正"相反）故 `alpha_sign=-1`。已回填 `pod_bridge.json`
|
||
> 并加标定回归测试（`test_deployed_config_has_calibrated_signs`）守护。
|
||
|
||
### 2.2 如何接入 TrackingControlLaw（三选一，需拍板）
|
||
|
||
控制律第 ④ 步现在是**硬编码从像素算角度**，没有"直接喂角度"的入口：
|
||
|
||
```python
|
||
x_offset = snapshot.track.center[0] - image_width * 0.5 # tracking_control_law.py:135
|
||
beta_deg = self._pixel_to_beta.convert(x_offset) * RAD_TO_DEG
|
||
```
|
||
|
||
| 路线 | 做法 | 评价 |
|
||
|---|---|---|
|
||
| **A1（推荐）** | `ControlSnapshot` 加可选字段 `los_beta_deg` / `los_alpha_deg`（默认 `None`）；第 ④ 步：**提供了就直接用，否则走原像素路径** | 改动约 10 行，**向后兼容**、现有 81 测试不受影响，⑤~⑧ 步（限幅/超龄/Coast/无距离/NED 旋转）全部复用 |
|
||
| A2 | 不动上游，在 `pod_bridge` 里写 `PodHeadingServo` 薄封装，复用 `PidController`/`SlewRateLimiter`/`is_stale` | 不碰稳定模块，但**要重寫 ⑤⑥⑦⑧ 的逻辑**，与既有实现双份维护，易走偏 |
|
||
| B | 把角度反推成"等价像素偏移"喂进现有字段 | ❌ 不推荐：需要 FOV，**吊舱变焦即失真**，且多绕一层徒增误差 |
|
||
|
||
**建议 A1**。代价是动一下 `visual_track_control`（需补 3~5 条测试守住"默认 None
|
||
仍走像素路径"的兼容性）。
|
||
|
||
### 2.3 ⚠ `tracked` 判据不能只信 bit2
|
||
|
||
已知事实（部署文档 §10）：**锁定位置位 bit2 在目标丢失后不复位**，且脱靶量
|
||
**没有无效标志位、丢失时表现为 0**。后果：
|
||
|
||
- 只信 bit2 → 目标早已丢失，仍认为"锁定中"继续转；
|
||
- 只看脱靶量 0 → 误判为"跟得很好"。
|
||
|
||
L1 的 `tracked` 判据按阶段递进：
|
||
|
||
| 阶段 | 判据 |
|
||
|---|---|
|
||
| 台架（S1~S2） | bit2 锁定 **且** 遥测未超龄（现有 `_pod_locked()`）+ **人工在旁观察**，随时按键 `u` 解锁 |
|
||
| ~~接入视觉后（S3+）~~ | ~~bit2 **且** 我方 `stereo_yolo`/`target_tracker` 有目标~~ |
|
||
| **现行（2026-10-19 起）** | bit2 **且** 遥测未超龄 **且 LOS 运动学一致性**（`pod_bridge/los_consistency.py`） |
|
||
|
||
> ⚠ **2026-10-19 架构变更**：后续决定**吊舱取代双目 + YOLO 作为唯一目标源**，
|
||
> 因此原「我方视觉第二判据」(`require_vision` / `vision_target_fn`) 已**删除** ——
|
||
> 它要么恒 False（链路永远进不了 TRACK），要么指向吊舱自身（自证，零信息量）。
|
||
>
|
||
> 替代判据：本吊舱无电机、相机刚性固连机体，机头转 Δψ 时真实目标的脱靶量必变
|
||
> ≈−Δψ。故用 `residual = Δβ + Δψ`（3s 窗口，激励门槛 20°、残差上限 8°）判断
|
||
> 「角度是活的还是冻住的」。超限 → `tracked=False` 且 `pod_locked()` 也返回 False
|
||
> （状态机会退出 TRACK 重新搜索，而不是只让控制律单方面刹车）。
|
||
>
|
||
> **局限**：它只能证明「角度是活的」，不能证明「是那个目标」。若吊舱锁错了物体
|
||
> 但角度正常变化，本判据无法识别 —— 那属于识别问题，需另接独立传感器。
|
||
|
||
---
|
||
|
||
## 3. 改动清单（文件级）
|
||
|
||
> 计划视图。**实际落地状态与逐文件勾选见 §9**（含计划外新增的 `config.py:load_from_dict`）。
|
||
|
||
| # | 文件 | 改动 |
|
||
|---|---|---|
|
||
| 1 | `utils/visual_track_control/types.py` | `ControlSnapshot` 加 `los_beta_deg` / `los_alpha_deg: Optional[float] = None`（路线 A1） |
|
||
| 2 | `utils/visual_track_control/tracking_control_law.py` | 第 ④ 步：角度直入优先，否则回退像素路径 |
|
||
| 3 | `utils/pod_bridge/bridge.py` | 新增 `_build_snapshot()`：表2-2 → `ControlSnapshot`（β/α 按 §2.1，符号由配置）；`step()` 改走 `TrackingControlLaw`；`tracked` 用 §2.3 判据 |
|
||
| 4 | `utils/pod_bridge/config.py` | 新增 L1 配置段（信号源、符号、门限、是否禁用垂直） |
|
||
| 5 | `utils/config/pod_bridge.json` | 落配置值（铁律：数值不写死在代码里） |
|
||
| 6 | `utils/tests/...` | 新增测试：角度直入路径、默认 None 兼容性、bridge L1 组装（FakePod） |
|
||
|
||
**暂不动**：`guidance.py`（L2 地理引导）保留，按配置在 L1 / L2 间切换，
|
||
等 L2 有标定过的距离源再启用。
|
||
|
||
---
|
||
|
||
## 4. 配置外置（要点，非最终字段名）
|
||
|
||
```jsonc
|
||
// utils/config/pod_bridge.json —— L1 段（已实现，字段名与 HeadingServoConfig 一致）
|
||
"heading_servo": {
|
||
"enabled": true, // true=走 L1(TrackingControlLaw)，false=L2(GuidanceLaw)
|
||
"beta_sign": 1, // 台架 S1 标定后填 ±1（符号错了会反向转）
|
||
"alpha_sign": 1,
|
||
"beta_from_frame": false, // 无电机 → 框架角无用；装电机后置 true
|
||
"beta_from_off_target": true,// 无电机 → 脱靶量即 β 主信号
|
||
"alpha_from_frame": false,
|
||
"alpha_from_off_target": true,
|
||
"vertical_enabled": true, // 决策3：无人机 L1 开垂直（vz 由 tracking.vertical 压低）
|
||
|
||
// 第二判据：LOS 运动学一致性（2026-10-19 取代 require_vision，见 §2.3）
|
||
"los_consistency_enabled": true, // false → 退回"只信 bit2"
|
||
"los_window_ms": 3000, // 比较窗口
|
||
"los_min_yaw_change_deg": 20.0, // 窗口内机头至少转这么多才判定（激励门槛）
|
||
"los_max_residual_deg": 8.0 // |Δβ+Δψ| 上限，超过判不一致
|
||
}
|
||
```
|
||
|
||
```jsonc
|
||
// tracking 段（内嵌整份 visual_track_control 配置，各分组须齐全；L1 首飞值）
|
||
"heading": { "mode": "rate", "gain": 1.0, "yaw_rate_limit_dps": 20.0, "yaw_slew_limit_dps2": 40.0 },
|
||
"distance": { "no_distance_action": "slow_approach", // 决策3：低速接近（非 hold）
|
||
"no_distance_approach_limit_mps": 0.2 }, // 极低限速起步
|
||
"vertical": { "vz_gain_mps_per_deg": 0.05, "vz_limit_mps": 0.5 } // 开垂直但压低
|
||
```
|
||
|
||
> 决策 3：首次上机 `no_distance_action = "slow_approach"`、限速 **0.2 m/s**（很低），
|
||
> 而非原推荐的 `hold`。安全不受影响——**S2 干跑不下发 offboard**，符号先验证过再通电。
|
||
> `beta_sign`/`alpha_sign`/增益均为占位，须 S1/S2 标定后回填，标定前不得实飞。
|
||
|
||
---
|
||
|
||
## 5. 台架落地步骤（增量，每步过了再下一步）
|
||
|
||
| 步骤 | 做什么 | 通过判据 |
|
||
|---|---|---|
|
||
| **S0** 纯软单测 | 新增测试 + 全量回归 | 全绿（当前 372 → 增加） |
|
||
| **S1** 标定 | `pod_console.py` 观察云台转动 / 移动目标时 `frame_yaw/frame_pitch`、`off_target_x/y` 的变化 | 确认框架角是"相对机体"；定出 `beta_sign` / `alpha_sign` |
|
||
| **S2** 干跑 | `bridge` 以 dry-run 跑 L1，**不下发 offboard**，只打印 yaw_rate | 目标偏左 → yaw_rate 为负（左转），符号正确；目标居中 → 趋于 0 |
|
||
| **S3** 不装桨接飞控 | 真实 offboard | 机体转向目标、云台逐步回中；人工可随时接管 |
|
||
| **S4** 切 slow_approach | vx = 0.3 m/s 起步 | 缓慢接近不振荡；异常立即 RC 切手动 |
|
||
| **S5** 装桨试飞 | — | 最后一步，先在空旷场地 |
|
||
|
||
⚠ S3 起必须：**不装桨 / 空旷场地 / RC 随时可切手动 / 有人值守**。
|
||
|
||
---
|
||
|
||
## 6. 风险与判据
|
||
|
||
| 风险 | 判据 / 对策 |
|
||
|---|---|
|
||
| **符号反了** → 反向转 | S1 标定 + S2 干跑必验；配置化而非硬编码 |
|
||
| bit2 粘滞 → 丢失后仍转 | **LOS 运动学一致性**（`pod_bridge/los_consistency.py`，2026-10-19 取代旧「我方视觉」方案）：机头转 Δψ 而脱靶量不动 → 判丢；台架期仍人工值守 |
|
||
| 框架角定义不符（绝对而非相对机体） | S1 实测确认，不符则改用脱靶量积分等替代 |
|
||
| 云台归中/搜索态框架角乱转 | 仅锁定态启用；非锁定 → `tracked=False` → BRAKE_HOVER |
|
||
| 垂直通道撞地 | L1 强制 `vz_gain=0`（本机实际按决策3 开垂直但压低限速） |
|
||
| 测距噪声（L2 才有） | L2 滞后，且**不用连续 PID**，改分段限速 |
|
||
|
||
---
|
||
|
||
## 7. 与 L2 的接口预留
|
||
|
||
L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构：
|
||
|
||
- `ControlSnapshot.distance_valid` / `target_distance_m` / `distance_time_ms` 字段
|
||
**现在就按 `False` 填全**；将来接**激光雷达**距离源后填入（原计划的
|
||
`stereo_ranger.forward_distance_m` 已随"吊舱取代双目"一并搁置）；
|
||
- `bridge.step()` 按配置在 `TrackingControlLaw`（L1）与 `GuidanceLaw`（L2）间切换；
|
||
- `guidance.py` 的输入接口保持不变，等有标定过的距离源再启用。
|
||
|
||
---
|
||
|
||
## 8. 已拍板（2026-10-09）
|
||
|
||
1. **接入路线 = A1**：`ControlSnapshot` 加可选 `los_beta_deg`/`los_alpha_deg`，第④步角度直入优先、否则回退像素路径。已实现（`types.py` / `tracking_control_law.py`，补 5 条兼容性测试）。
|
||
2. **目标类型 = 无人机（空中）**：影响 L2 几何反推法，L1 无关。
|
||
3. **首次上机 = `slow_approach` + 极低限速 0.2 m/s**（非 `hold`）；**L1 开垂直通道**，但 `vz` 限速压低（`vz_limit_mps=0.5`、`vz_gain=0.05`）防撞。
|
||
|
||
补充决策（BridgeConfig 结构）：**BridgeConfig 直接内嵌 `TrackingConfig` 字段 + `heading_servo` 段**；运行时按 `heading_servo.enabled` 在 L1(`TrackingControlLaw`)/L2(`GuidanceLaw`) 间切换，L2 接口天然保留（对齐 §7）。姿态来自 PX4、β/α 来自吊舱（决策 2 见 §2.1 无电机说明）。
|
||
|
||
---
|
||
|
||
## 9. 实现状态（S0 已完成）
|
||
|
||
| # | 文件 | 状态 |
|
||
|---|---|---|
|
||
| 1 | `visual_track_control/types.py` | ✅ `ControlSnapshot` 加 `los_beta_deg`/`los_alpha_deg: Optional[float]=None` |
|
||
| 2 | `visual_track_control/tracking_control_law.py` | ✅ 第④步角度直入优先，否则回退像素 |
|
||
| 3 | `visual_track_control/config.py` | ✅ 拆出 `load_from_dict()` 供 bridge 内嵌复用 |
|
||
| 4 | `pod_bridge/config.py` | ✅ 加 `HeadingServoConfig` + 内嵌 `TrackingConfig`，严格校验符号(±1) |
|
||
| 5 | `pod_bridge/bridge.py` | ✅ `_build_snapshot()`；`step()` 按 enabled 切 L1/L2；`_dispatch` 对 DEGRADED_HOLD 补发刹车保心跳；~~`vision_target_fn` 注入点~~ → **已删除，改为 LOS 一致性判据**（`los_consistency.py`，§2.3） |
|
||
| 6 | `config/pod_bridge.json` | ✅ 落 L1 首飞值（slow_approach/0.2、垂直压低、无电机 → 仅脱靶量） |
|
||
| 7 | `tests/` | ✅ 新增 24 条（vtc 5 + config 9 + bridge L1 10）；全量 **396 passed** |
|
||
| 8 | `pod_bridge/run_bridge.py` | ✅ 闭环 CLI 入口：`--dry-run`(S2 只接吊舱/不发 offboard/打印 β→yaw_rate) 与实飞(S3+，`PodBridge.connect`+信号优雅停机)；配套 `run_bridge_test.py` 5 条，全量 **401 passed** |
|
||
| 9 | `pod_bridge/bridge.py:run()` | ✅ 循环周期改按生效律取频率（L1=tracking.control，L2=guidance），修 L1 live 频率错配 |
|
||
| 10 | `pod_bridge/virtual_pod.py` | ✅ 模拟吊舱（SITL/台架工具）：按无电机语义合成表2-2（框架角恒0、无激光、不可见→bit2=0+脱靶量归0），符号与 S1 标定一致 |
|
||
| 11 | `l1_sitl_test.py`（根目录，原 `pod_sitl_test.py`） | ✅ L1 SITL 全流程：解锁→起飞→锚点→offboard→L1 闭环(VirtualPod→PodBridge→控制律)→丢目标→刹车→RTL→落地→上锁，轨迹落 CSV；飞行编排不塞进 run_bridge（保持其纯 offboard 闭环入口） |
|
||
| 12 | `tests/pod_bridge/virtual_pod*_test.py` | ✅ 15 条：S1 符号回归 + VirtualPod×PodBridge×控制律集成（右转/下降/慢速前近/丢目标刹车）；全量 **417 passed** |
|
||
| 13 | `pod_bridge/los_consistency.py`（2026-10-19） | ✅ 目标有效性第二判据：LOS 运动学一致性（Δβ+Δψ，3s 窗口/20° 激励/8° 残差）；`pod_locked()` 叠加该判据；配套 `los_consistency_test.py` 12 条 + 桥接层 3 条 |
|
||
|
||
**待办（非代码，台架）**：~~S1 标定~~ **已完成（2026-10-09）**：目标在左下时 `off_target_x/y`
|
||
均为负 → 吊舱 X 轴右正（与 β 同向）`beta_sign=+1`；Y 轴上正（与 α 下正相反）
|
||
`alpha_sign=-1`（已回填 `pod_bridge.json`，并加标定回归测试守住）。厂商确认本版
|
||
**电机无用** → `beta_from_frame/alpha_from_frame` 恒 false。
|
||
|
||
**已完成的后续验证（2026-10-19）**：S2 干跑（`run_bridge.py --dry-run`，真吊舱 COM5，20Hz
|
||
闭环无异常）、SITL 全流程（`l1_sitl_test.py` 与 `task_runner_sitl.py` 均实跑）、
|
||
真吊舱在环全流程（`task_runner_real.py --port COM5`，无目标时走"搜索超时→返航"安全路径）。
|
||
剩：**`heading.gain` 标定**、**吊舱真实目标识别（bit2 0→1）**、S3 不装桨接飞控。
|